summaryrefslogtreecommitdiff
path: root/Host/nxt-gdb-server.py
blob: c1ca25adc863c1670020a42b80e840a686ec1aad (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
#!/usr/bin/env python
#
# Copyright (C) 2011 the NxOS developers
#
# Module Developed by: Nicolas Schodet
#                      TC Wan
#
# See AUTHORS for a full list of the developers.
#
# See COPYING for redistribution license
#
# Exchange GDB messages with the NXT brick.
#
# Every message is encapsulated with the debug command and message length.
# This can be used by the firmware to make the distinction between debug
# messages and regular messages.
#
import nxt.locator
import socket
import optparse
import select
#import usb
import pyfantom
import struct
import sys

CTRLC = chr(3)
NAKCHAR = '-'
ACKCHAR = '+'
STATUS_QUERY = "$?#3F"
DEFAULT_PORT = 2828
SELECT_TIMEOUT = 0.1
DEBUG = False
DEBUG2 = False
NXT_RECV_ERR = -1

# Libusb 0.12.x blocks on USB reads
LIBUSB_RECEIVE_BLOCKING = True

class NXTGDBServer:

    # Socket read size.
    recv_size = 1024

    # Maximum message size.
    pack_size = 61
    
    # Debug command header, no reply.
    debug_command = 0x8d

    def __init__ (self, port, nowait):
        """Initialise server."""
        self.nowait = nowait
        self.port = port
        self.in_buf = ''
        self.brick = None

    def pack (self, data, segment_no):
        """Return packed data to send to NXT."""
        # Insert command and length.
        assert len (data) <= self.pack_size
        return struct.pack ('BBB', self.debug_command, segment_no, len (data)) + data

    def unpack (self, data):
        """Return unpacked data from NXT."""
        # May be improved, for now, check command and announced length.
        if len (data) == 0:
            return '', 0  # No message, exit
        if len (data) < 3:
            return '', NXT_RECV_ERR
        header, body = data[0:3], data[3:]
        command, segment_no, length = struct.unpack ('BBB', header)
        if command != self.debug_command or length != len (body):
            return '', NXT_RECV_ERR
        return body, segment_no

    def segment (self, data):
        """Split messages in GDB commands and make segments with each command."""
        segs = [ ]
        self.in_buf += data

        # Find ACK '+' 
        end = self.in_buf.find (ACKCHAR)
        while end == 0:
            self.in_buf = self.in_buf[end+1:]   # Strip out any leading ACKCHAR
            if DEBUG2:
                print "stripped ACK, remain: ", self.in_buf
            end = self.in_buf.find (ACKCHAR)

        # Find NAK '-' 
        end = self.in_buf.find (NAKCHAR)
        if end == 0:
            msg, self.in_buf = self.in_buf[0:end+1], self.in_buf[end+1:]
            segs.append (self.pack (msg, 0))
            end = self.in_buf.find (NAKCHAR)

        # Find Ctrl-C (assumed to be by itself and not following a normal command)
        end = self.in_buf.find (CTRLC)
        if end >= 0:
            msg, self.in_buf = self.in_buf[0:end+1], self.in_buf[end+1:]
            assert len (msg) <= self.pack_size, "Ctrl-C Command Packet too long!"
            segs.append (self.pack (msg, 0))
            end = self.in_buf.find (CTRLC)
        
        end = self.in_buf.find ('#')
        # Is # found and enough place for the checkum?
        while end >= 0 and end < len (self.in_buf) - 2:
            msg, self.in_buf = self.in_buf[0:end + 3], self.in_buf[end + 3:]
            i = 0
            gdbprefix = msg[i]
            while gdbprefix in [ACKCHAR]:
                # Ignore any '+'
                i += 1
                gdbprefix = msg[i]
                if DEBUG2:
                    print "Checking '", gdbprefix, "'"
            assert gdbprefix == '$', "not a GDB command"
            # Make segments.
            seg_no = 0
            while msg:
                seg, msg = msg[0:self.pack_size], msg[self.pack_size:]
                seg_no += 1
                if not msg: # Last segment.
                    seg_no = 0
                segs.append (self.pack (seg, seg_no))
            # Look for next one.
            end = self.in_buf.find ('#')
        return segs
        
    def reassemble (self, sock):
        msg = ''
        prev_segno = 0
        segno = NXT_RECV_ERR                    # force initial pass through while loop
        while segno != 0:
            try:
                s, segno = self.unpack (sock.recv ())
                if len (s) == 0:
                    if segno == 0 and prev_segno == 0:
                        return ''               # No message pending
                    else:
                        segno = NXT_RECV_ERR    # Keep waiting for segments
                # Ignore error packets
                if segno >= 0:
                    # Check segno, if non-zero it must be monotonically increasing from 1, otherwise 0
                    if segno > 0:
                       assert segno == prev_segno + 1, "segno = %s, prev_segno = %s" % (segno, prev_segno)
                    prev_segno = segno
                    msg += s               
            except usb.USBError as e:
                # Some pyusb are buggy, ignore some "errors".
                if e.args != ('No error', ):
                    raise e
        return msg
        
    def run (self):
        """Endless run loop."""
        # Create the listening socket.
        s = socket.socket (socket.AF_INET, socket.SOCK_STREAM)
        s.setsockopt (socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        s.bind (('', self.port))
        s.listen (1)
        while True:
            # We should open the NXT connection first, otherwise Python startup delay
            # may cause GDB to misbehave
            if not self.nowait:
                dummy = raw_input('Waiting...Press <ENTER> when NXT GDB Stub is ready. ')
            # Open connection to the NXT brick.
            self.brick = nxt.locator.find_one_brick ()
            self.brick.sock.debug = DEBUG
            # Wait for a connection.
            print "Waiting for GDB connection on port %s..." % self.port
            client, addr = s.accept ()
            print "Client from", addr
            # Work loop, wait for a message from client socket or NXT brick.
            while client is not None:
                data = ''
                # Wait for a message from client or timeout.
                rlist, wlist, xlist = select.select ([ client ], [ ], [ ],
                        SELECT_TIMEOUT)
                for c in rlist:
                    assert c is client
                    # Data from client, read it and forward it to NXT brick.
                    data = client.recv (self.recv_size)
                    data = data.strip()
                    if len (data) > 0:
                        #if len (data) == 1 and data.find(CTRLC) >= 0:
                        #   print "CTRL-C Received!"
                        #   data = STATUS_QUERY
                        if DEBUG:
                            if data[0] == CTRLC:
                                print "[GDB->NXT] <CTRL-C>"
                            else:
                                print "[GDB->NXT] %s" % data
                        segments = self.segment (data)
                        data = ''
                        for seg in segments:
                            try:
                                self.brick.sock.send (seg)
                            except usb.USBError as e:
                                # Some pyusb are buggy, ignore some "errors".
                                if e.args != ('No error', ):
                                    raise e
                        if segments != [] and LIBUSB_RECEIVE_BLOCKING:
                            if DEBUG2:
                                print "Accessing Blocking sock.recv()"
                            data = self.reassemble (self.brick.sock)
                    else:
                        client.close ()
                        client = None
                if not LIBUSB_RECEIVE_BLOCKING:
                    if DEBUG2:
                         print "Accessing Non-Blocking sock.recv()"
                    data = self.reassemble (self.brick.sock)
                    
                # Is there something from NXT brick?
                if data:
                    if DEBUG:
                        print "[NXT->GDB] %s" % data
                    if client:
                        client.send (data)
                    data = ''
            self.brick.sock.close()
            print "Connection closed."

if __name__ == '__main__':
    # Read options from command line.
    parser = optparse.OptionParser (description = """
    Gateway between the GNU debugger and a NXT brick.
    """)
    parser.add_option ('-p', '--port', type = 'int', default = DEFAULT_PORT,
            help = "server listening port (default: %default)", metavar = "PORT")
    parser.add_option ('-v', '--verbose', action='store_true', dest='verbose', default = False,
            help = "verbose mode (default: %default)")
    parser.add_option ('-n', '--nowait', action='store_true', dest='nowait', default = False,
            help = "Don't wait for NXT GDB Stub Setup before connecting (default: %default)")
    (options, args) = parser.parse_args ()
    if args:
        parser.error ("Too many arguments")
    # Run.
    try:
        DEBUG = options.verbose
        if DEBUG:
            print "Debug Mode Enabled!"
        server = NXTGDBServer (options.port, options.nowait)
        server.run ()
    except KeyboardInterrupt:
        print "\n\nException caught. Bye!"
        if server.brick is not None:
            server.brick.sock.close()
        sys.exit()