Пример #1
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """
    def __init__(self, board, port_urlWSS):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.packet_size = 2048
        self.flashData = list()
        self.conn = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        self.quit = False
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)
        self.setDaemon(True)
        self.start()

    def restart(self):
        if self.isAlive():
            self.detach_event.set()

    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            while self.isAlive():
                pass
            logging.info("GDB server thread killed")
        self.board.uninit()

    def setBoard(self, board, stop=True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return

    def run(self):
        while True:
            new_command = False
            data = ""
            logging.info('GDB server started at port:%d', self.port)

            self.shutdown_event.clear()
            self.detach_event.clear()

            while not self.shutdown_event.isSet(
            ) and not self.detach_event.isSet():
                connected = self.abstract_socket.connect()
                if connected != None:
                    break

            if self.shutdown_event.isSet():
                return

            if self.detach_event.isSet():
                continue

            logging.info("One client connected!")

            while True:

                if self.shutdown_event.isSet():
                    return

                if self.detach_event.isSet():
                    continue

                # read command
                while True:
                    if (new_command == True):
                        new_command = False
                        break
                    try:
                        if self.shutdown_event.isSet(
                        ) or self.detach_event.isSet():
                            break
                        self.abstract_socket.setBlocking(0)
                        data += self.abstract_socket.read()
                        if data.index("$") >= 0 and data.index("#") >= 0:
                            break
                    except (ValueError, socket.error):
                        pass

                if self.shutdown_event.isSet():
                    return

                if self.detach_event.isSet():
                    continue

                self.abstract_socket.setBlocking(1)

                data = data[data.index("$"):]

                self.lock.acquire()

                if len(data) != 0:
                    # decode and prepare resp
                    [resp, ack, detach] = self.handleMsg(data)

                    if resp is not None:
                        # ack
                        if ack:
                            resp = "+" + resp
                        # send resp
                        self.abstract_socket.write(resp)
                        # wait a '+' from the client
                        try:
                            data = self.abstract_socket.read()
                            if data[0] != '+':
                                logging.debug('gdb client has not ack!')
                            else:
                                logging.debug('gdb client has ack!')
                            if data.index("$") >= 0 and data.index("#") >= 0:
                                new_command = True
                        except:
                            pass

                    if detach:
                        self.abstract_socket.close()
                        self.lock.release()
                        break

                self.lock.release()

    def handleMsg(self, msg):

        if msg[0] != '$':
            logging.debug('msg ignored: first char != $')
            return None, 0, 0

        #logging.debug('-->>>>>>>>>>>> GDB rsp packet: %s', msg)

        # query command
        if msg[1] == 'q':
            return self.handleQuery(msg[2:]), 1, 0

        elif msg[1] == 'H':
            return self.createRSPPacket(''), 1, 0

        elif msg[1] == '?':
            return self.lastSignal(), 1, 0

        elif msg[1] == 'g':
            return self.getRegister(), 1, 0

        elif msg[1] == 'p':
            return self.readRegister(msg[2:]), 1, 0

        elif msg[1] == 'P':
            return self.writeRegister(msg[2:]), 1, 0

        elif msg[1] == 'm':
            return self.getMemory(msg[2:]), 1, 0

        elif msg[1] == 'X':
            return self.writeMemory(msg[2:]), 1, 0

        elif msg[1] == 'v':
            return self.flashOp(msg[2:]), 1, 0

        # we don't send immediately the response for C and S commands
        elif msg[1] == 'C' or msg[1] == 'c':
            return self.resume()

        elif msg[1] == 'S' or msg[1] == 's':
            return self.step()

        elif msg[1] == 'Z' or msg[1] == 'z':
            return self.breakpoint(msg[1:]), 1, 0

        elif msg[1] == 'D':
            return self.detach(msg[1:]), 1, 1

        elif msg[1] == 'k':
            return self.kill(), 1, 1

        else:
            logging.error("Unknown RSP packet: %s", msg)
            return self.createRSPPacket(""), 1, 0

    def detach(self, data):
        resp = "OK"
        return self.createRSPPacket(resp)

    def kill(self):
        return self.createRSPPacket("")

    def breakpoint(self, data):
        # handle Z1/z1 commands
        addr = int(data.split(',')[1], 16)
        if data[1] == '1':
            if data[0] == 'Z':
                if self.target.setBreakpoint(addr) == False:
                    resp = "ENN"
                    return self.createRSPPacket(resp)
            else:
                self.target.removeBreakpoint(addr)
            resp = "OK"
            return self.createRSPPacket(resp)

        return None

    def resume(self):
        self.ack()
        self.target.resume()
        self.abstract_socket.setBlocking(0)

        # Try to set break point at hardfault handler to avoid
        # halting target constantly
        if (self.target.availableBreakpoint() >= 1):
            bpSet = True
            hardfault_handler = self.target.readMemory(4 * 3)
            self.target.setBreakpoint(hardfault_handler)
        else:
            bpSet = False
            logging.info(
                "No breakpoint available. Interfere target constantly.")

        val = ''

        while True:
            sleep(0.01)
            if self.shutdown_event.isSet():
                return self.createRSPPacket(val), 0, 0

            try:
                data = self.abstract_socket.read()
                if (data[0] == '\x03'):
                    self.target.halt()
                    val = 'S05'
                    logging.debug("receive CTRL-C")
                    break
            except:
                pass

            if self.target.getState() == TARGET_HALTED:
                logging.debug("state halted")
                xpsr = self.target.readCoreRegister('xpsr')
                # Get IPSR value from XPSR
                if (xpsr & 0x1f) == 3:
                    val = "S" + FAULT[3]
                else:
                    val = 'S05'
                break

            if not bpSet:
                # Only do this when no bp available as it slows resume operation
                self.target.halt()
                xpsr = self.target.readCoreRegister('xpsr')
                logging.debug("GDB resume xpsr: 0x%X", xpsr)
                # Get IPSR value from XPSR
                if (xpsr & 0x1f) == 3:
                    val = "S" + FAULT[3]
                    break
                self.target.resume()

        if bpSet:
            self.target.removeBreakpoint(hardfault_handler)

        self.abstract_socket.setBlocking(1)
        return self.createRSPPacket(val), 0, 0

    def step(self):
        self.ack()
        self.target.step()
        return self.createRSPPacket("S05"), 0, 0

    def halt(self):
        self.ack()
        self.target.halt()
        return self.createRSPPacket("S05"), 0, 0

    def flashOp(self, data):
        ops = data.split(':')[0]
        logging.debug("flash op: %s", ops)

        if ops == 'FlashErase':
            self.flash.init()
            self.flash.eraseAll()
            return self.createRSPPacket("OK")

        elif ops == 'FlashWrite':
            write_addr = int(data.split(':')[1], 16)
            logging.debug("flash write addr: 0x%s", write_addr)
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ':':
                    second_colon += 1
                idx_begin += 1

            # if there's gap between sections, fill it
            flash_watermark = len(self.flashData)
            pad_size = write_addr - flash_watermark
            if pad_size > 0:
                self.flashData += [0xFF] * pad_size

            # append the new data if it doesn't overlap existing data
            if write_addr >= flash_watermark:
                self.flashData += self.unescape(data[idx_begin:len(data) - 3])
            else:
                logging.error(
                    "Invalid FlashWrite address %d overlaps current data of size %d",
                    write_addr, flash_watermark)

            return self.createRSPPacket("OK")

        # we need to flash everything
        elif 'FlashDone' in ops:
            flashPtr = 0
            bytes_to_be_written = len(self.flashData)
            """
            bin = open(os.path.join(parentdir, 'res', 'bad_bin.txt'), "w+")
            
            i = 0
            while (i < bytes_to_be_written):
                bin.write(str(self.flashData[i:i+16]) + "\n")
                i += 16
            """

            logging.info("flashing %d bytes", bytes_to_be_written)

            while len(self.flashData) > 0:
                size_to_write = min(self.flash.page_size, len(self.flashData))
                #if 0 is returned from programPage, security check failed
                if (self.flash.programPage(
                        flashPtr, self.flashData[:size_to_write]) == 0):
                    logging.error(
                        "Protection bits error, flashing has stopped")
                    return None
                flashPtr += size_to_write

                self.flashData = self.flashData[size_to_write:]

                # print progress bar
                sys.stdout.write('\r')
                i = int((float(flashPtr) / float(bytes_to_be_written)) * 20.0)
                # the exact output you're looking for:
                sys.stdout.write("[%-20s] %d%%" % ('=' * i, 5 * i))
                sys.stdout.flush()

            sys.stdout.write("\n\r")

            self.flashData = []
            """
            bin.close()
            """

            # reset and stop on reset handler
            self.target.resetStopOnReset()

            return self.createRSPPacket("OK")

        elif 'Cont' in ops:
            if 'Cont?' in ops:
                return self.createRSPPacket("vCont;c;s;t")

        return None

    def unescape(self, data):
        data_idx = 0

        # unpack the data into binary array
        str_unpack = str(len(data)) + 'B'
        data = unpack(str_unpack, data)
        data = list(data)

        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7d:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1

        return data

    def getMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = split[1]
        length = int(length[:len(length) - 3], 16)

        val = ''

        mem = self.target.readBlockMemoryUnaligned8(addr, length)
        for x in mem:
            if x >= 0x10:
                val += hex(x)[2:4]
            else:
                val += '0' + hex(x)[2:3]

        return self.createRSPPacket(val)

    def writeMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = int(split[1].split(':')[0], 16)

        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ':':
                idx_begin += 1
                break
            idx_begin += 1

        data = data[idx_begin:len(data) - 3]
        data = self.unescape(data)

        if length > 0:
            self.target.writeBlockMemoryUnaligned8(addr, data)

        return self.createRSPPacket("OK")

    def readRegister(self, data):
        num = int(data.split('#')[0], 16)
        reg = self.target.readCoreRegister(num)
        logging.debug("GDB: read reg %d: 0x%X", num, reg)
        val = self.intToHexGDB(reg)
        return self.createRSPPacket(val)

    def writeRegister(self, data):
        num = int(data.split('=')[0], 16)
        val = data.split('=')[1].split('#')[0]
        val = val[6:8] + val[4:6] + val[2:4] + val[0:2]
        logging.debug("GDB: write reg %d: 0x%X", num, int(val, 16))
        self.target.writeCoreRegister(num, int(val, 16))
        return self.createRSPPacket("OK")

    def intToHexGDB(self, val):
        val = hex(int(val))[2:]
        size = len(val)
        r = ''
        for i in range(8 - size):
            r += '0'
        r += str(val)

        resp = ''
        for i in range(4):
            resp += r[8 - 2 * i - 2:8 - 2 * i]

        return resp

    def getRegister(self):
        resp = ''
        # only core registers are printed
        for i in sorted(CORE_REGISTER.values())[4:20]:
            reg = self.target.readCoreRegister(i)
            resp += self.intToHexGDB(reg)
            logging.debug("GDB reg: %s = 0x%X", self.target.getRegisterName(i),
                          reg)
        return self.createRSPPacket(resp)

    def lastSignal(self):
        fault = self.target.readCoreRegister('xpsr') & 0xff
        try:
            fault = FAULT[fault]
        except:
            # Values above 16 are for interrupts
            fault = "17"  # SIGSTOP
            pass
        logging.debug("GDB lastSignal: %s", fault)
        return self.createRSPPacket('S' + fault)

    def handleQuery(self, msg):
        query = msg.split(':')
        logging.debug('GDB received query: %s', query)

        if query is None:
            logging.error('GDB received query packet malformed')
            return None

        if query[0] == 'Supported':
            resp = "qXfer:memory-map:read+;qXfer:features:read+;PacketSize="
            resp += hex(self.packet_size)[2:]
            return self.createRSPPacket(resp)

        elif query[0] == 'Xfer':

            if query[1] == 'features' and query[2] == 'read' and \
               query[3] == 'target.xml':
                data = query[4].split(',')
                resp = self.handleQueryXML('read_feature', int(data[0], 16),
                                           int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == 'memory-map' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('memory_map', int(data[0], 16),
                                           int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            else:
                return None

        elif query[0] == 'C#b4':
            return self.createRSPPacket("")

        elif query[0].find('Attached') != -1:
            return self.createRSPPacket("1")

        elif query[0].find('TStatus') != -1:
            return self.createRSPPacket("")

        elif query[0].find('Tf') != -1:
            return self.createRSPPacket("")

        elif 'Offsets' in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)

        elif 'Symbol' in query[0]:
            resp = "OK"
            return self.createRSPPacket(resp)

        elif query[0].startswith('Rcmd,'):
            cmd = self.hexDecode(query[0][5:].split('#')[0])
            logging.debug('Remote command: %s', cmd)

            safecmd = {
                'reset': ['Reset target', 0x1],
                'halt': ['Halt target', 0x2],
                'help': ['Display this help', 0x4],
            }
            resultMask = 0x00
            if cmd == 'help':
                resp = ''
                for k, v in safecmd.items():
                    resp += '%s\t%s\n' % (k, v)
                resp = self.hexEncode(resp)
            else:
                cmdList = cmd.split(' ')
                #check whether all the cmds is valid cmd for monitor
                for cmd_sub in cmdList:
                    if not cmd_sub in safecmd:
                        #error cmd for monitor, just return directly
                        resp = ''
                        return self.createRSPPacket(resp)
                    else:
                        resultMask = resultMask | safecmd[cmd_sub][1]
                #if it's a single cmd, just launch it!
                if len(cmdList) == 1:
                    tmp = eval('self.target.%s()' % cmd_sub)
                    logging.debug(tmp)
                    resp = "OK"
                else:
                    #101 for help reset, so output reset cmd help information
                    if resultMask == 0x5:
                        resp = 'Reset the target\n'
                        resp = self.hexEncode(resp)
                    #110 for help halt, so output halt cmd help information
                    elif resultMask == 0x6:
                        resp = 'Halt the target\n'
                        resp = self.hexEncode(resp)
                    #011 for reset halt cmd, so launch self.target.resetStopOnReset()
                    elif resultMask == 0x3:
                        resp = "OK"
                        self.target.resetStopOnReset()
                    else:
                        resp = ''
            return self.createRSPPacket(resp)

        else:
            return self.createRSPPacket("")

    def handleQueryXML(self, query, offset, size):
        logging.debug('GDB query %s: offset: %s, size: %s', query, offset,
                      size)
        xml = ''
        if query == 'memory_map':
            xml = self.target.memoryMapXML
        elif query == 'read_feature':
            xml = self.target.targetXML

        size_xml = len(xml)

        prefix = 'm'

        if offset > size_xml:
            logging.error('GDB: offset target.xml > size!')
            return

        if size > (self.packet_size - 4):
            size = self.packet_size - 4

        nbBytesAvailable = size_xml - offset

        if size > nbBytesAvailable:
            prefix = 'l'
            size = nbBytesAvailable

        resp = prefix + xml[offset:offset + size]

        return resp

    def createRSPPacket(self, data):
        resp = '$' + data + '#'

        c = 0
        checksum = 0
        for c in data:
            checksum += ord(c)
        checksum = checksum % 256
        checksum = hex(checksum)

        if int(checksum[2:], 16) < 0x10:
            resp += '0'
        resp += checksum[2:]

        #logging.debug('--<<<<<<<<<<<< GDB rsp packet: %s', resp)
        return resp

    def ack(self):
        self.abstract_socket.write("+")

    def hexDecode(self, cmd):
        return ''.join(
            [chr(int(cmd[i:i + 2], 16)) for i in range(0, len(cmd), 2)])

    def hexEncode(self, string):
        return ''.join(['%02x' % ord(i) for i in string])
Пример #2
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """
    def __init__(self, board, port_urlWSS, options = {}):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.break_at_hardfault = bool(options.get('break_at_hardfault', True))
        self.packet_size = 2048
        self.flashData = list()
        self.flashOffset = None
        self.conn = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        self.quit = False
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)
        self.setDaemon(True)
        self.start()
    
    def restart(self):
        if self.isAlive():
            self.detach_event.set()
    
    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            while self.isAlive():
                pass
            logging.info("GDB server thread killed")
        self.board.uninit()
        
    def setBoard(self, board, stop = True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return
        
    def run(self):
        while True:
            new_command = False
            data = ""
            logging.info('GDB server started at port:%d',self.port)
            
            self.shutdown_event.clear()
            self.detach_event.clear()
                
            while not self.shutdown_event.isSet() and not self.detach_event.isSet():
                connected = self.abstract_socket.connect()
                if connected != None:
                    break
            
            if self.shutdown_event.isSet():
                return
            
            if self.detach_event.isSet():
                continue
            
            logging.info("One client connected!")
            
            while True:
                
                if self.shutdown_event.isSet():
                    return
            
                if self.detach_event.isSet():
                    continue
                
                # read command
                while True:
                    if (new_command == True):
                        new_command = False
                        break
                    try:
                        if self.shutdown_event.isSet() or self.detach_event.isSet():
                            break
                        self.abstract_socket.setBlocking(0)
                        data += self.abstract_socket.read()
                        if data.index("$") >= 0 and data.index("#") >= 0:
                            break
                    except (ValueError, socket.error):
                        pass
                
                if self.shutdown_event.isSet():
                    return
            
                if self.detach_event.isSet():
                    continue
                
                self.abstract_socket.setBlocking(1)
                    
                data = data[data.index("$"):]
                
                self.lock.acquire()
            
                if len(data) != 0:
                    # decode and prepare resp
                    [resp, ack, detach] = self.handleMsg(data)
            
                    if resp is not None:
                        # ack
                        if ack:
                            resp = "+" + resp
                        # send resp
                        self.abstract_socket.write(resp)
                        # wait a '+' from the client
                        try:
                            data = self.abstract_socket.read()
                            if data[0] != '+':
                                logging.debug('gdb client has not ack!')
                            else:
                                logging.debug('gdb client has ack!')
                            if data.index("$") >= 0 and data.index("#") >= 0:
                                new_command = True
                        except:
                            pass
                        
                    if detach:
                        self.abstract_socket.close()
                        self.lock.release()
                        break
                    
                self.lock.release()
        
        
    def handleMsg(self, msg):
        
        if msg[0] != '$':
            logging.debug('msg ignored: first char != $')
            return None, 0, 0
        
        #logging.debug('-->>>>>>>>>>>> GDB rsp packet: %s', msg)
        
        # query command
        if msg[1] == 'q':
            return self.handleQuery(msg[2:]), 1, 0
            
        elif msg[1] == 'H':
            return self.createRSPPacket(''), 1, 0
        
        elif msg[1] == '?':
            return self.lastSignal(), 1, 0
        
        elif msg[1] == 'g':
            return self.getRegister(), 1, 0
        
        elif msg[1] == 'p':
            return self.readRegister(msg[2:]), 1, 0
        
        elif msg[1] == 'P':
            return self.writeRegister(msg[2:]), 1, 0
        
        elif msg[1] == 'm':
            return self.getMemory(msg[2:]), 1, 0
        
        elif msg[1] == 'X':
            return self.writeMemory(msg[2:]), 1, 0
        
        elif msg[1] == 'v':
            return self.flashOp(msg[2:]), 1, 0
        
        # we don't send immediately the response for C and S commands
        elif msg[1] == 'C' or msg[1] == 'c':
            return self.resume()
        
        elif msg[1] == 'S' or msg[1] == 's':
            return self.step()
        
        elif msg[1] == 'Z' or msg[1] == 'z':
            return self.breakpoint(msg[1:]), 1, 0
        
        elif msg[1] == 'D':
            return self.detach(msg[1:]), 1, 1
        
        elif msg[1] == 'k':
            return self.kill(), 1, 1
        
        else:
            logging.error("Unknown RSP packet: %s", msg)
            return self.createRSPPacket(""), 1, 0
        
    def detach(self, data):
        resp = "OK"
        return self.createRSPPacket(resp)
    
    def kill(self):
        return self.createRSPPacket("")
        
    def breakpoint(self, data):
        # handle Z1/z1 commands
        addr = int(data.split(',')[1], 16)
        if data[1] == '1':
            if data[0] == 'Z':
                if self.target.setBreakpoint(addr) == False:
                    resp = "ENN"
                    return self.createRSPPacket(resp)
            else:
                self.target.removeBreakpoint(addr)
            resp = "OK"
            return self.createRSPPacket(resp)
        
        return None
            
    def resume(self):
        self.ack()
        self.abstract_socket.setBlocking(0)
        
        # Try to set break point at hardfault handler to avoid
        # halting target constantly
        if not self.break_at_hardfault:
            bpSet=False
        elif (self.target.availableBreakpoint() >= 1):
            bpSet=True
            hardfault_handler = self.target.readMemory(4*3)
            self.target.setBreakpoint(hardfault_handler)
        else:
            bpSet=False
            logging.info("No breakpoint available. Interfere target constantly.")

        self.target.resume()
        
        val = ''
        
        while True:
            sleep(0.01)
            if self.shutdown_event.isSet():
                return self.createRSPPacket(val), 0, 0
            
            try:
                data = self.abstract_socket.read()
                if (data[0] == '\x03'):
                    self.target.halt()
                    val = 'S05'
                    logging.debug("receive CTRL-C")
                    break
            except:
                pass
            
            try:
                if self.target.getState() == TARGET_HALTED:
                    logging.debug("state halted")
                    xpsr = self.target.readCoreRegister('xpsr')
                    # Get IPSR value from XPSR
                    if (xpsr & 0x1f) == 3:
                        val = "S" + FAULT[3]
                    else:
                        val = 'S05'
                    break
            except:
                logging.debug('Target is unavailable temporary.')

            if (not bpSet) and self.break_at_hardfault:
                # Only do this when no bp available as it slows resume operation
                self.target.halt()
                xpsr = self.target.readCoreRegister('xpsr')
                logging.debug("GDB resume xpsr: 0x%X", xpsr)
                # Get IPSR value from XPSR
                if (xpsr & 0x1f) == 3:
                    val = "S" + FAULT[3]
                    break
                self.target.resume()
        
        if bpSet and self.break_at_hardfault:
            self.target.removeBreakpoint(hardfault_handler)

        self.abstract_socket.setBlocking(1)
        return self.createRSPPacket(val), 0, 0
    
    def step(self):
        self.ack()
        self.target.step()
        return self.createRSPPacket("S05"), 0, 0
    
    def halt(self):
        self.ack()
        self.target.halt()
        return self.createRSPPacket("S05"), 0, 0
        
    def flashOp(self, data):
        ops = data.split(':')[0]
        logging.debug("flash op: %s", ops)
        
        if ops == 'FlashErase':
            return self.createRSPPacket("OK")
        
        elif ops == 'FlashWrite':
            write_addr = int(data.split(':')[1], 16)
            logging.debug("flash write addr: 0x%x", write_addr)
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ':':
                    second_colon += 1
                idx_begin += 1

            # determine the address to start flashing
            if self.flashOffset == None:
                # flash offset must be a multiple of the page size
                self.flashOffset = write_addr - ( write_addr % self.flash.page_size )

            # if there's gap between sections, fill it
            flash_watermark = len(self.flashData) + self.flashOffset
            pad_size = write_addr - flash_watermark
            if pad_size > 0:
                self.flashData += [0xFF] * pad_size
            
            # append the new data if it doesn't overlap existing data
            if write_addr >= flash_watermark:
                self.flashData += self.unescape(data[idx_begin:len(data) - 3])
            else:
                logging.error("Invalid FlashWrite address %d overlaps current data of size %d", write_addr, flash_watermark)
                
            return self.createRSPPacket("OK")
        
        # we need to flash everything
        elif 'FlashDone' in ops :
            bytes_to_be_written = len(self.flashData)
            flashPtr = self.flashOffset
            
            self.flash.init()

            # use mass erase if the address starts at 0
            mass_erase = flashPtr == 0
            if mass_erase:
                logging.debug("Erasing entire flash")
                self.flash.eraseAll()

            while len(self.flashData) > 0:
                size_to_write = min(self.flash.page_size, len(self.flashData))
                
                #Erase Page if flash has not been erased
                if not mass_erase:
                    logging.debug("Erasing page 0x%x", flashPtr)
                    self.flash.erasePage(flashPtr)

                #ProgramPage
                self.flash.programPage(flashPtr, self.flashData[:size_to_write])
                flashPtr += size_to_write

                self.flashData = self.flashData[size_to_write:]

                # print progress bar
                sys.stdout.write('\r')
                i = int((float(flashPtr - self.flashOffset)/float(bytes_to_be_written))*20.0)
                # the exact output you're looking for:
                sys.stdout.write("[%-20s] %d%%" % ('='*i, 5*i))
                sys.stdout.flush()
                
            sys.stdout.write("\n\r")
            
            self.flashData = []
            self.flashOffset = None

            # reset and stop on reset handler
            self.target.resetStopOnReset()
            
            return self.createRSPPacket("OK")
        
        elif 'Cont' in ops:
            if 'Cont?' in ops:
                return self.createRSPPacket("vCont;c;s;t")
                
        return None
    
    def unescape(self, data):
        data_idx = 0
    
        # unpack the data into binary array
        str_unpack = str(len(data)) + 'B'
        data = unpack(str_unpack, data)
        data = list(data)
    
        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7d:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1
            
        return data
            
        
    def getMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = split[1]
        length = int(length[:len(length)-3],16)
        
        val = ''
        
        mem = self.target.readBlockMemoryUnaligned8(addr, length)
        for x in mem:
            if x >= 0x10:
                val += hex(x)[2:4]
            else:
                val += '0' + hex(x)[2:3]
            
        return self.createRSPPacket(val)
    
    def writeMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = int(split[1].split(':')[0], 16)
        
        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ':':
                idx_begin += 1
                break
            idx_begin += 1
        
        data = data[idx_begin:len(data) - 3]
        data = self.unescape(data)
        
        if length > 0:
            self.target.writeBlockMemoryUnaligned8(addr, data)
        
        return self.createRSPPacket("OK")
        
    def readRegister(self, data):
        num = int(data.split('#')[0], 16)
        reg = self.target.readCoreRegister(num)
        logging.debug("GDB: read reg %d: 0x%X", num, reg)
        val = self.intToHexGDB(reg)
        return self.createRSPPacket(val)
    
    def writeRegister(self, data):
        num = int(data.split('=')[0], 16)
        val = data.split('=')[1].split('#')[0]
        val = val[6:8] + val[4:6] + val[2:4] + val[0:2]
        logging.debug("GDB: write reg %d: 0x%X", num, int(val, 16))
        self.target.writeCoreRegister(num, int(val, 16))
        return self.createRSPPacket("OK")
    
    def intToHexGDB(self, val):
        val = hex(int(val))[2:]
        size = len(val)
        r = ''
        for i in range(8-size):
            r += '0'
        r += str(val)
        
        resp = ''
        for i in range(4):
            resp += r[8 - 2*i - 2: 8 - 2*i]
        
        return resp
            
    def getRegister(self):
        resp = ''
        # only core registers are printed
        for i in sorted(CORE_REGISTER.values())[4:20]:
            reg = self.target.readCoreRegister(i)
            resp += self.intToHexGDB(reg)
            logging.debug("GDB reg: %s = 0x%X", self.target.getRegisterName(i), reg)
        return self.createRSPPacket(resp)
        
    def lastSignal(self):
        fault = self.target.readCoreRegister('xpsr') & 0xff
        try:
            fault = FAULT[fault]
        except:
            # Values above 16 are for interrupts
            fault = "17"    # SIGSTOP
            pass
        logging.debug("GDB lastSignal: %s", fault)
        return self.createRSPPacket('S' + fault)
            
    def handleQuery(self, msg):
        query = msg.split(':')
        logging.debug('GDB received query: %s', query)
        
        if query is None:
            logging.error('GDB received query packet malformed')
            return None
        
        if query[0] == 'Supported':
            resp = "qXfer:memory-map:read+;qXfer:features:read+;PacketSize="
            resp += hex(self.packet_size)[2:]
            return self.createRSPPacket(resp)
            
        elif query[0] == 'Xfer':
            
            if query[1] == 'features' and query[2] == 'read' and \
               query[3] == 'target.xml':
                data = query[4].split(',')
                resp = self.handleQueryXML('read_feature', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)
            
            elif query[1] == 'memory-map' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('memory_map', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)
                
            else:
                return None
            
        elif query[0] == 'C#b4':
            return self.createRSPPacket("")
        
        elif query[0].find('Attached') != -1:
            return self.createRSPPacket("1")
        
        elif query[0].find('TStatus') != -1:
            return self.createRSPPacket("")
        
        elif query[0].find('Tf') != -1:
            return self.createRSPPacket("")
        
        elif 'Offsets' in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)
        
        elif 'Symbol' in query[0]:
            resp = "OK"
            return self.createRSPPacket(resp)

        elif query[0].startswith('Rcmd,'):
            cmd = self.hexDecode(query[0][5:].split('#')[0])
            logging.debug('Remote command: %s', cmd)

            safecmd = {
                'reset' : ['Reset target', 0x1],
                'halt'  : ['Halt target', 0x2],
                'resume': ['Resume target', 0x4],
                'help'  : ['Display this help', 0x80],
            }
            resultMask = 0x00
            if cmd == 'help':
                resp = ''
                for k,v in safecmd.items():
                    resp += '%s\t%s\n' % (k,v)
                resp = self.hexEncode(resp)
            else:
                cmdList = cmd.split(' ')
                #check whether all the cmds is valid cmd for monitor
                for cmd_sub in cmdList:
                    if not cmd_sub in safecmd:
                        #error cmd for monitor, just return directly
                        resp = ''
                        return self.createRSPPacket(resp)
                    else:
                        resultMask = resultMask | safecmd[cmd_sub][1]
                #if it's a single cmd, just launch it!
                if len(cmdList) == 1:
                    tmp = eval ('self.target.%s()' % cmd_sub)
                    logging.debug(tmp)
                    resp = "OK"
                else:
                    #10000001 for help reset, so output reset cmd help information
                    if resultMask == 0x81:
                        resp = 'Reset the target\n'
                        resp = self.hexEncode(resp)
                    #10000010 for help halt, so output halt cmd help information
                    elif resultMask == 0x82:
                        resp = 'Halt the target\n'
                        resp = self.hexEncode(resp)
                    #10000100 for help resume, so output resume cmd help information
                    elif resultMask == 0x84:
                        resp = 'Resume the target\n'
                        resp = self.hexEncode(resp)
                    #11 for reset halt cmd, so launch self.target.resetStopOnReset()
                    elif resultMask == 0x3:
                        resp = "OK"
                        self.target.resetStopOnReset()
                    #111 for reset halt resume cmd, so launch self.target.resetStopOnReset() and self.target.resume()
                    elif resultMask == 0x7:
                        resp = "OK"
                        self.target.resetStopOnReset()
                        self.target.resume()
                    else:
                        resp = ''
            return self.createRSPPacket(resp)

        else:
            return self.createRSPPacket("")
            
    def handleQueryXML(self, query, offset, size):
        logging.debug('GDB query %s: offset: %s, size: %s', query, offset, size)
        xml = ''
        if query == 'memory_map':
            xml = self.target.memoryMapXML
        elif query == 'read_feature':
            xml = self.target.targetXML

        size_xml = len(xml)
        
        prefix = 'm'
        
        if offset > size_xml:
            logging.error('GDB: offset target.xml > size!')
            return
        
        if size > (self.packet_size - 4):
            size = self.packet_size - 4
        
        nbBytesAvailable = size_xml - offset
        
        if size > nbBytesAvailable:
            prefix = 'l'
            size = nbBytesAvailable
        
        resp = prefix + xml[offset:offset + size]
        
        return resp
            
            
    def createRSPPacket(self, data):
        resp = '$' + data + '#'
        
        c = 0
        checksum = 0
        for c in data:
            checksum += ord(c)
        checksum = checksum % 256
        checksum = hex(checksum)

        if int(checksum[2:], 16) < 0x10:
            resp += '0'
        resp += checksum[2:]
        
        #logging.debug('--<<<<<<<<<<<< GDB rsp packet: %s', resp)
        return resp
    
    def ack(self):
        self.abstract_socket.write("+")

    def hexDecode(self, cmd):
        return ''.join([ chr(int(cmd[i:i+2], 16)) for i in range(0, len(cmd), 2)])

    def hexEncode(self, string):
        return ''.join(['%02x' % ord(i) for i in string])
Пример #3
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """
    def __init__(self, board, port_urlWSS):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.packet_size = 2048
        self.flashData = list()
        self.flash_watermark = 0
        self.conn = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        self.quit = False
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)
        self.start()
    
    def restart(self):
        if self.isAlive():
            self.detach_event.set()
    
    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            while self.isAlive():
                pass
            logging.info("GDB server thread killed")
        self.board.uninit()
        
    def setBoard(self, board, stop = True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return
        
    def run(self):
        while True:
            new_command = False
            data = ""
            logging.info('GDB server started')
            
            self.shutdown_event.clear()
            self.detach_event.clear()
                
            while not self.shutdown_event.isSet() and not self.detach_event.isSet():
                connected = self.abstract_socket.connect()
                if connected != None:
                    break
            
            if self.shutdown_event.isSet():
                return
            
            if self.detach_event.isSet():
                continue
            
            logging.info("One client connected!")
            
            while True:
                
                if self.shutdown_event.isSet():
                    return
            
                if self.detach_event.isSet():
                    continue
                
                # read command
                while True:
                    if (new_command == True):
                        new_command = False
                        break
                    try:
                        if self.shutdown_event.isSet() or self.detach_event.isSet():
                            break
                        self.abstract_socket.setBlocking(0)
                        data += self.abstract_socket.read()
                        if data.index("$") >= 0 and data.index("#") >= 0:
                            break
                    except (ValueError, socket.error):
                        pass
                
                if self.shutdown_event.isSet():
                    return
            
                if self.detach_event.isSet():
                    continue
                
                self.abstract_socket.setBlocking(1)
                    
                data = data[data.index("$"):]
                
                self.lock.acquire()
            
                if len(data) != 0:
                    # decode and prepare resp
                    [resp, ack, detach] = self.handleMsg(data)
            
                    if resp is not None:
                        # ack
                        if ack:
                            resp = "+" + resp
                        # send resp
                        self.abstract_socket.write(resp)
                        # wait a '+' from the client
                        try:
                            data = self.abstract_socket.read()
                            if data[0] != '+':
                                logging.debug('gdb client has not ack!')
                            else:
                                logging.debug('gdb client has ack!')
                            if data.index("$") >= 0 and data.index("#") >= 0:
                                new_command = True
                        except:
                            pass
                        
                    if detach:
                        self.abstract_socket.close()
                        self.lock.release()
                        break
                    
                self.lock.release()
        
        
    def handleMsg(self, msg):
        
        if msg[0] != '$':
            logging.debug('msg ignored: first char != $')
            return None, 0, 0
        
        #logging.debug('-->>>>>>>>>>>> GDB rsp packet: %s', msg)
        
        # query command
        if msg[1] == 'q':
            return self.handleQuery(msg[2:]), 1, 0
            
        elif msg[1] == 'H':
            return self.createRSPPacket(''), 1, 0
        
        elif msg[1] == '?':
            return self.lastSignal(), 1, 0
        
        elif msg[1] == 'g':
            return self.getRegister(), 1, 0
        
        elif msg[1] == 'p':
            return self.readRegister(msg[2:]), 1, 0
        
        elif msg[1] == 'P':
            return self.writeRegister(msg[2:]), 1, 0
        
        elif msg[1] == 'm':
            return self.getMemory(msg[2:]), 1, 0
        
        elif msg[1] == 'X':
            return self.writeMemory(msg[2:]), 1, 0
        
        elif msg[1] == 'v':
            return self.flashOp(msg[2:]), 1, 0
        
        # we don't send immediately the response for C and S commands
        elif msg[1] == 'C' or msg[1] == 'c':
            return self.resume()
        
        elif msg[1] == 'S' or msg[1] == 's':
            return self.step()
        
        elif msg[1] == 'Z' or msg[1] == 'z':
            return self.breakpoint(msg[1:]), 1, 0
        
        elif msg[1] == 'D':
            return self.detach(msg[1:]), 1, 1
        
        elif msg[1] == 'k':
            return self.kill(), 1, 1
        
        else:
            logging.error("Unknown RSP packet: %s", msg)
            return None
        
    def detach(self, data):
        resp = "OK"
        return self.createRSPPacket(resp)
    
    def kill(self):
        return self.createRSPPacket("")
        
    def breakpoint(self, data):
        # handle Z1/z1 commands
        addr = int(data.split(',')[1], 16)
        if data[1] == '1':
            if data[0] == 'Z':
                if self.target.setBreakpoint(addr) == False:
                    resp = "ENN"
                    return self.createRSPPacket(resp)
            else:
                self.target.removeBreakpoint(addr)
            resp = "OK"
            return self.createRSPPacket(resp)
        
        return None
            
    def resume(self):
        self.ack()
        self.target.resume()
        self.abstract_socket.setBlocking(0)
        
        # Try to set break point at hardfault handler to avoid
        # halting target constantly
        if (self.target.availableBreakpoint() >= 1):
            bpSet=True
            hardfault_handler = self.target.readMemory(4*3)
            self.target.setBreakpoint(hardfault_handler)
        else:
            bpSet=False
            logging.info("No breakpoint available. Interfere target constantly.")
        
        val = ''
        
        while True:
            sleep(0.01)
            
            try:
                data = self.abstract_socket.read()
                if (data[0] == '\x03'):
                    self.target.halt()
                    val = 'S05'
                    logging.debug("receive CTRL-C")
                    break
            except:
                pass
            
            if self.target.getState() == TARGET_HALTED:
                logging.debug("state halted")
                ipsr = self.target.readCoreRegister('xpsr')
                if (ipsr & 0x1f) == 3:
                    val = "S" + FAULT[3]
                else:
                    val = 'S05'
                break
            
            if not bpSet:
                # Only do this when no bp available as it slows resume operation
                self.target.halt()
                ipsr = self.target.readCoreRegister('xpsr')
                logging.debug("GDB resume xpsr: 0x%X", ipsr)
                if (ipsr & 0x1f) == 3:
                    val = "S" + FAULT[3]
                    break
                self.target.resume()
        
        if bpSet:
            self.target.removeBreakpoint(hardfault_handler)

        self.abstract_socket.setBlocking(1)
        return self.createRSPPacket(val), 0, 0
    
    def step(self):
        self.ack()
        self.target.step()
        return self.createRSPPacket("S05"), 0, 0
    
    def halt(self):
        self.ack()
        self.target.halt()
        return self.createRSPPacket("S05"), 0, 0
        
    def flashOp(self, data):
        ops = data.split(':')[0]
        #logging.debug("flash op: %s", ops)
        
        if ops == 'FlashErase':
            self.flash.init()
            self.flash.eraseAll()
            return self.createRSPPacket("OK")
        
        elif ops == 'FlashWrite':
            logging.debug("flash write addr: 0x%s", data.split(':')[1])
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ':':
                    second_colon += 1
                idx_begin += 1

            #if there's gap between sections, fill it with zeros
            count = int(data.split(':')[1], 16)
            if (count != 0 and self.flash_watermark != count):
                count -= self.flash_watermark
                while (count):
                    self.flashData += [0]
                    count -= 1

            data_to_unescape = data[idx_begin:len(data) - 3]

            unescaped_data = self.unescape(data_to_unescape)
            self.flashData += unescaped_data
            #flash_watermark contains the end of the flash data
            self.flash_watermark = len(self.flashData)

            return self.createRSPPacket("OK")
        
        # we need to flash everything
        elif 'FlashDone' in ops :
            flashPtr = 0
            bytes_to_be_written = len(self.flashData)

            """
            bin = open(os.path.join(parentdir, 'res', 'bad_bin.txt'), "w+")
            
            i = 0
            while (i < bytes_to_be_written):
                bin.write(str(self.flashData[i:i+16]) + "\n")
                i += 16
            """

            logging.info("flashing %d bytes", bytes_to_be_written)

            while len(self.flashData) > 0:
                size_to_write = min(self.flash.page_size, len(self.flashData))
                #if 0 is returned from programPage, security check failed
                if (self.flash.programPage(flashPtr, self.flashData[:size_to_write]) == 0):
                    logging.error("Protection bits error, flashing has stopped")
                    return None
                flashPtr += size_to_write

                self.flashData = self.flashData[size_to_write:]

                # print progress bar
                sys.stdout.write('\r')
                i = int((float(flashPtr)/float(bytes_to_be_written))*20.0)
                # the exact output you're looking for:
                sys.stdout.write("[%-20s] %d%%" % ('='*i, 5*i))
                sys.stdout.flush()
                
            sys.stdout.write("\n\r")
            
            self.flashData = []
            
            """
            bin.close()
            """
            
            # reset and stop on reset handler
            self.target.resetStopOnReset()
            
            return self.createRSPPacket("OK")
        
        elif 'Cont' in ops:
            if 'Cont?' in ops:
                return self.createRSPPacket("vCont;c;s;t")
                
        return None
    
    def unescape(self, data):
        data_idx = 0
    
        # unpack the data into binary array
        str_unpack = str(len(data)) + 'B'
        data = unpack(str_unpack, data)
        data = list(data)
    
        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7d:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1
            
        return data
            
        
    def getMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = split[1]
        length = int(length[:len(length)-3],16)
        
        val = ''
        
        mem = self.target.readBlockMemoryUnaligned8(addr, length)
        for x in mem:
            if x >= 0x10:
                val += hex(x)[2:4]
            else:
                val += '0' + hex(x)[2:3]
            
        return self.createRSPPacket(val)
    
    def writeMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = int(split[1].split(':')[0], 16)
        
        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ':':
                idx_begin += 1
                break
            idx_begin += 1
        
        data = data[idx_begin:len(data) - 3]
        data = self.unescape(data)
        
        if length > 0:
            self.target.writeBlockMemoryUnaligned8(addr, data)
        
        return self.createRSPPacket("OK")
        
    def readRegister(self, data):
        num = int(data.split('#')[0], 16)
        reg = self.target.readCoreRegister(num)
        logging.debug("GDB: read reg %d: 0x%X", num, reg)
        val = self.intToHexGDB(reg)
        return self.createRSPPacket(val)
    
    def writeRegister(self, data):
        num = int(data.split('=')[0], 16)
        val = data.split('=')[1].split('#')[0]
        val = val[6:8] + val[4:6] + val[2:4] + val[0:2]
        logging.debug("GDB: write reg %d: 0x%X", num, int(val, 16))
        self.target.writeCoreRegister(num, int(val, 16))
        return self.createRSPPacket("OK")
    
    def intToHexGDB(self, val):
        val = hex(int(val))[2:]
        size = len(val)
        r = ''
        for i in range(8-size):
            r += '0'
        r += str(val)
        
        resp = ''
        for i in range(4):
            resp += r[8 - 2*i - 2: 8 - 2*i]
        
        return resp
            
    def getRegister(self):
        resp = ''
        # only core registers are printed
        for i in sorted(CORE_REGISTER.values())[4:20]:
            reg = self.target.readCoreRegister(i)
            resp += self.intToHexGDB(reg)
            logging.debug("GDB reg: %s = 0x%X", self.target.getRegisterName(i), reg)
        return self.createRSPPacket(resp)
        
    def lastSignal(self):
        fault = self.target.readCoreRegister('xpsr') & 0xff
        fault = FAULT[fault]
        logging.debug("GDB lastSignal: %s", fault)
        return self.createRSPPacket('S' + fault)
            
    def handleQuery(self, msg):
        query = msg.split(':')
        logging.debug('GDB received query: %s', query)
        
        if query is None:
            logging.error('GDB received query packet malformed')
            return None
        
        if query[0] == 'Supported':
            resp = "qXfer:memory-map:read+;qXfer:features:read+;PacketSize="
            resp += hex(self.packet_size)[2:]
            return self.createRSPPacket(resp)
            
        elif query[0] == 'Xfer':
            
            if query[1] == 'features' and query[2] == 'read' and \
               query[3] == 'target.xml':
                data = query[4].split(',')
                resp = self.handleQueryXML('read_feature', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)
            
            elif query[1] == 'memory-map' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('memory_map', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)
                
            else:
                return None
            
        elif query[0] == 'C#b4':
            return self.createRSPPacket("")
        
        elif query[0].find('Attached') != -1:
            return self.createRSPPacket("1")
        
        elif query[0].find('TStatus') != -1:
            return self.createRSPPacket("")
        
        elif query[0].find('Tf') != -1:
            return self.createRSPPacket("")
        
        elif 'Offsets' in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)
        
        elif 'Symbol' in query[0]:
            resp = "OK"
            return self.createRSPPacket(resp)
        
        else:
            return None
            
    def handleQueryXML(self, query, offset, size):
        logging.debug('GDB query %s: offset: %s, size: %s', query, offset, size)
        xml = ''
        if query == 'memory_map':
            xml = self.target.memoryMapXML
        elif query == 'read_feature':
            xml = self.target.targetXML

        size_xml = len(xml)
        
        prefix = 'm'
        
        if offset > size_xml:
            logging.error('GDB: offset target.xml > size!')
            return
        
        if size > (self.packet_size - 4):
            size = self.packet_size - 4
        
        nbBytesAvailable = size_xml - offset
        
        if size > nbBytesAvailable:
            prefix = 'l'
            size = nbBytesAvailable
        
        resp = prefix + xml[offset:offset + size]
        
        return resp
            
            
    def createRSPPacket(self, data):
        resp = '$' + data + '#'
        
        c = 0
        checksum = 0
        for c in data:
            checksum += ord(c)
        checksum = checksum % 256
        checksum = hex(checksum)

        if int(checksum[2:], 16) < 0x10:
            resp += '0'
        resp += checksum[2:]
        
        #logging.debug('--<<<<<<<<<<<< GDB rsp packet: %s', resp)
        return resp
    
    def ack(self):
        self.abstract_socket.write("+")
Пример #4
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """
    def __init__(self, board, port_urlWSS, options={}):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.break_at_hardfault = bool(options.get('break_at_hardfault', True))
        self.board.target.setVectorCatchFault(self.break_at_hardfault)
        self.break_on_reset = options.get('break_on_reset', False)
        self.board.target.setVectorCatchReset(self.break_on_reset)
        self.step_into_interrupt = options.get('step_into_interrupt', False)
        self.persist = options.get('persist', False)
        self.soft_bkpt_as_hard = options.get('soft_bkpt_as_hard', False)
        self.chip_erase = options.get('chip_erase', None)
        self.hide_programming_progress = options.get('hide_programming_progress', False)
        self.fast_program = options.get('fast_program', False)
        self.enable_semihosting = options.get('enable_semihosting', False)
        self.telnet_port = options.get('telnet_port', 4444)
        self.semihost_use_syscalls = options.get('semihost_use_syscalls', False)
        self.server_listening_callback = options.get('server_listening_callback', None)
        self.serve_local_only = options.get('serve_local_only', True)
        self.packet_size = 2048
        self.packet_io = None
        self.gdb_features = []
        self.non_stop = False
        self.is_target_running = (self.target.getState() == Target.TARGET_RUNNING)
        self.flashBuilder = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
            if self.serve_local_only:
                self.abstract_socket.host = 'localhost'
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)

        # Init semihosting and telnet console.
        if self.semihost_use_syscalls:
            semihost_io_handler = GDBSyscallIOHandler(self)
        else:
            # Use internal IO handler.
            semihost_io_handler = semihost.InternalSemihostIOHandler()
        self.telnet_console = semihost.TelnetSemihostIOHandler(self.telnet_port, self.serve_local_only)
        self.semihost = semihost.SemihostAgent(self.target, io_handler=semihost_io_handler, console=self.telnet_console)

        self.setDaemon(True)
        self.start()

    def restart(self):
        if self.isAlive():
            self.detach_event.set()

    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            while self.isAlive():
                pass
            logging.info("GDB server thread killed")
        self.board.uninit()

    def setBoard(self, board, stop=True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return

    def _cleanup(self):
        logging.debug("GDB server cleaning up")
        if self.packet_io:
            self.packet_io.stop()
            self.packet_io = None
        if self.semihost:
            self.semihost.cleanup()
            self.semihost = None
        if self.telnet_console:
            self.telnet_console.stop()
            self.telnet_console = None

    def run(self):
        logging.info('GDB server started at port:%d', self.port)

        while True:
            try:
                self.detach_event.clear()

                # Inform callback that the server is running.
                if self.server_listening_callback:
                    self.server_listening_callback(self)

                while not self.shutdown_event.isSet() and not self.detach_event.isSet():
                    connected = self.abstract_socket.connect()
                    if connected != None:
                        self.packet_io = GDBServerPacketIOThread(self.abstract_socket)
                        break

                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    continue

                logging.info("One client connected!")
                self._run_connection()

            except Exception as e:
                logging.error("Unexpected exception: %s", e)
                traceback.print_exc()

    def _run_connection(self):
        while True:
            try:
                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    break

                if self.packet_io.interrupt_event.isSet():
                    if self.non_stop:
                        self.target.halt()
                        self.is_target_running = False
                        self.sendStopNotification()
                    else:
                        logging.debug("Got unexpected ctrl-c, ignoring")
                    self.packet_io.interrupt_event.clear()

                if self.non_stop and self.is_target_running:
                    try:
                        if self.target.getState() == Target.TARGET_HALTED:
                            logging.debug("state halted")
                            self.is_target_running = False
                            self.sendStopNotification()
                    except Exception as e:
                        logging.error("Unexpected exception: %s", e)
                        traceback.print_exc()

                # read command
                try:
                    packet = self.packet_io.receive(block=not self.non_stop)
                except ConnectionClosedException:
                    break

                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    break

                if self.non_stop and packet is None:
                    sleep(0.1)
                    continue

                self.lock.acquire()

                if len(packet) != 0:
                    # decode and prepare resp
                    resp, detach = self.handleMsg(packet)

                    if resp is not None:
                        # send resp
                        self.packet_io.send(resp)

                    if detach:
                        self.abstract_socket.close()
                        self.packet_io.stop()
                        self.packet_io = None
                        self.lock.release()
                        if self.persist:
                            break
                        else:
                            self.shutdown_event.set()
                            return

                self.lock.release()

            except Exception as e:
                logging.error("Unexpected exception: %s", e)
                traceback.print_exc()

    def handleMsg(self, msg):
        try:
            if msg[0] != '$':
                logging.debug('msg ignored: first char != $')
                return None, 0

            # query command
            if msg[1] == '?':
                return self.stopReasonQuery(), 0

            # we don't send immediately the response for C and S commands
            elif msg[1] == 'C' or msg[1] == 'c':
                return self.resume(msg[1:]), 0

            elif msg[1] == 'D':
                return self.detach(msg[1:]), 1

            elif msg[1] == 'g':
                return self.getRegisters(), 0

            elif msg[1] == 'G':
                return self.setRegisters(msg[2:]), 0

            elif msg[1] == 'H':
                return self.createRSPPacket('OK'), 0

            elif msg[1] == 'k':
                return self.kill(), 1

            elif msg[1] == 'm':
                return self.getMemory(msg[2:]), 0

            elif msg[1] == 'M': # write memory with hex data
                return self.writeMemoryHex(msg[2:]), 0

            elif msg[1] == 'p':
                return self.readRegister(msg[2:]), 0

            elif msg[1] == 'P':
                return self.writeRegister(msg[2:]), 0

            elif msg[1] == 'q':
                return self.handleQuery(msg[2:]), 0

            elif msg[1] == 'Q':
                return self.handleGeneralSet(msg[2:]), 0

            elif msg[1] == 'S' or msg[1] == 's':
                return self.step(msg[1:]), 0

            elif msg[1] == 'T': # check if thread is alive
                return self.createRSPPacket('OK'), 0

            elif msg[1] == 'v':
                return self.vCommand(msg[2:]), 0

            elif msg[1] == 'X': # write memory with binary data
                return self.writeMemory(msg[2:]), 0

            elif msg[1] == 'Z' or msg[1] == 'z':
                return self.breakpoint(msg[1:]), 0

            else:
                logging.error("Unknown RSP packet: %s", msg)
                return self.createRSPPacket(""), 0

        except Exception as e:
            logging.error("Unhandled exception in handleMsg: %s", e)
            traceback.print_exc()
            return self.createRSPPacket("E01"), 0

    def detach(self, data):
        logging.info("Client detached")
        resp = "OK"
        return self.createRSPPacket(resp)

    def kill(self):
        logging.debug("GDB kill")
        # Keep target halted and leave vector catches if in persistent mode.
        if not self.persist:
            self.board.target.setVectorCatchFault(False)
            self.board.target.setVectorCatchReset(False)
            self.board.target.resume()
        return self.createRSPPacket("")

    def breakpoint(self, data):
        # handle breakpoint/watchpoint commands
        split = data.split('#')[0].split(',')
        addr = int(split[1], 16)
        logging.debug("GDB breakpoint %s%d @ %x" % (data[0], int(data[1]), addr))

        # handle software breakpoint Z0/z0
        if data[1] == '0' and not self.soft_bkpt_as_hard:
            if data[0] == 'Z':
                if not self.target.setBreakpoint(addr, Target.BREAKPOINT_SW):
                    return self.createRSPPacket('E01') #EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware breakpoint Z1/z1
        if data[1] == '1' or (self.soft_bkpt_as_hard and data[1] == '0'):
            if data[0] == 'Z':
                if self.target.setBreakpoint(addr, Target.BREAKPOINT_HW) == False:
                    return self.createRSPPacket('E01') #EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware watchpoint Z2/z2/Z3/z3/Z4/z4
        if data[1] == '2':
            # Write-only watch
            watchpoint_type = Target.WATCHPOINT_WRITE
        elif data[1] == '3':
            # Read-only watch
            watchpoint_type = Target.WATCHPOINT_READ
        elif data[1] == '4':
            # Read-Write watch
            watchpoint_type = Target.WATCHPOINT_READ_WRITE
        else:
            return self.createRSPPacket('E01') #EPERM

        size = int(split[2], 16)
        if data[0] == 'Z':
            if self.target.setWatchpoint(addr, size, watchpoint_type) == False:
                return self.createRSPPacket('E01') #EPERM
        else:
            self.target.removeWatchpoint(addr, size, watchpoint_type)
        return self.createRSPPacket("OK")

    def stopReasonQuery(self):
        # In non-stop mode, if no threads are stopped we need to reply with OK.
        if self.non_stop and self.is_target_running:
            return self.createRSPPacket("OK")

        return self.createRSPPacket(self.target.getTResponse())

    def _get_resume_step_addr(self, data):
        if data is None:
            return None
        data = data.split('#')[0]
        if ';' not in data:
            return None
        # c[;addr]
        if data[0] in ('c', 's'):
            addr = int(data[2:], base=16)
        # Csig[;addr]
        elif data[0] in ('C', 'S'):
            addr = int(data[1:].split(';')[1], base=16)
        return addr

    def resume(self, data):
        addr = self._get_resume_step_addr(data)
        self.target.resume()
        logging.debug("target resumed")

        val = ''

        while True:
            if self.shutdown_event.isSet():
                self.packet_io.interrupt_event.clear()
                return self.createRSPPacket(val)

            # Wait for a ctrl-c to be received.
            if self.packet_io.interrupt_event.wait(0.01):
                logging.debug("receive CTRL-C")
                self.packet_io.interrupt_event.clear()
                self.target.halt()
                val = self.target.getTResponse(forceSignal=signals.SIGINT)
                break

            try:
                if self.target.getState() == Target.TARGET_HALTED:
                    # Handle semihosting
                    if self.enable_semihosting:
                        was_semihost = self.semihost.check_and_handle_semihost_request()

                        if was_semihost:
                            self.target.resume()
                            continue

                    logging.debug("state halted")
                    val = self.target.getTResponse()
                    break
            except Exception as e:
                try:
                    self.target.halt()
                except:
                    pass
                traceback.print_exc()
                logging.debug('Target is unavailable temporarily.')
                val = 'S%02x' % self.target.getSignalValue()
                break

        return self.createRSPPacket(val)

    def step(self, data):
        addr = self._get_resume_step_addr(data)
        logging.debug("GDB step: %s", data)
        self.target.step(not self.step_into_interrupt)
        return self.createRSPPacket(self.target.getTResponse())

    def halt(self):
        self.target.halt()
        return self.createRSPPacket(self.target.getTResponse())

    def sendStopNotification(self, forceSignal=None):
        data = self.target.getTResponse(forceSignal=forceSignal)
        packet = '%Stop:' + data + '#' + checksum(data)
        self.packet_io.send(packet)

    def vCommand(self, data):
        cmd = data.split('#')[0]
        logging.debug("GDB vCommand: %s", cmd)

        # Flash command.
        if cmd.startswith('Flash'):
            return self.flashOp(data)

        # vCont capabilities query.
        elif 'Cont?' == cmd:
            return self.createRSPPacket("vCont;c;C;s;S;t")

        # vCont, thread action command.
        elif cmd.startswith('Cont'):
            return self.vCont(cmd)

        # vStopped, part of thread stop state notification sequence.
        elif 'Stopped' in cmd:
            # Because we only support one thread for now, we can just reply OK to vStopped.
            return self.createRSPPacket("OK")

        return self.createRSPPacket("")

    # Example: $vCont;s:1;c#c1
    def vCont(self, cmd):
        ops = cmd.split(';')[1:] # split and remove 'Cont' from list
        if not ops:
            return self.createRSPPacket("OK")

        thread_actions = { 1 : None } # our only thread
        default_action = None
        for op in ops:
            args = op.split(':')
            action = args[0]
            if len(args) > 1:
                thread_id = args[1]
                if thread_id == '-1' or thread_id == '0':
                    thread_id = '1'
                thread_id = int(thread_id, base=16)
                thread_actions[thread_id] = action
            else:
                default_action = action

        logging.debug("thread_actions=%s; default_action=%s", repr(thread_actions), default_action)

        # Only thread 1 is supported at the moment.
        if thread_actions[1] is None:
            if default_action is None:
                return self.createRSPPacket('E01')
            thread_actions[1] = default_action

        if thread_actions[1] in ('c', 'C'):
            if self.non_stop:
                self.target.resume()
                self.is_target_running = True
                return self.createRSPPacket("OK")
            else:
                return self.resume(None)
        elif thread_actions[1] in ('s', 'S'):
            if self.non_stop:
                self.target.step(not self.step_into_interrupt)
                self.packet_io.send(self.createRSPPacket("OK"))
                self.sendStopNotification()
                return None
            else:
                return self.step(None)
        elif thread_actions[1] == 't':
            # Must ignore t command in all-stop mode.
            if not self.non_stop:
                return self.createRSPPacket("")
            self.packet_io.send(self.createRSPPacket("OK"))
            self.target.halt()
            self.is_target_running = False
            self.sendStopNotification(forceSignal=0)

    def flashOp(self, data):
        ops = data.split(':')[0]
        logging.debug("flash op: %s", ops)

        if ops == 'FlashErase':
            return self.createRSPPacket("OK")

        elif ops == 'FlashWrite':
            write_addr = int(data.split(':')[1], 16)
            logging.debug("flash write addr: 0x%x", write_addr)
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ':':
                    second_colon += 1
                idx_begin += 1

            # Get flash builder if there isn't one already
            if self.flashBuilder == None:
                self.flashBuilder = self.flash.getFlashBuilder()

            # Add data to flash builder
            self.flashBuilder.addData(write_addr, self.unescape(data[idx_begin:len(data) - 3]))


            return self.createRSPPacket("OK")

        # we need to flash everything
        elif 'FlashDone' in ops :

            def print_progress(progress):
                # Reset state on 0.0
                if progress == 0.0:
                    print_progress.done = False

                # print progress bar
                if not print_progress.done:
                    sys.stdout.write('\r')
                    i = int(progress * 20.0)
                    sys.stdout.write("[%-20s] %3d%%" % ('=' * i, round(progress * 100)))
                    sys.stdout.flush()

                # Finish on 1.0
                if progress >= 1.0:
                    if not print_progress.done:
                        print_progress.done = True
                        sys.stdout.write("\r\n")

            if self.hide_programming_progress:
                progress_cb = None
            else:
                 progress_cb = print_progress

            self.flashBuilder.program(chip_erase=self.chip_erase, progress_cb=progress_cb, fast_verify=self.fast_program)

            # Set flash builder to None so that on the next flash command a new
            # object is used.
            self.flashBuilder = None

            return self.createRSPPacket("OK")

        return None

    def unescape(self, data):
        data_idx = 0

        # unpack the data into binary array
        str_unpack = str(len(data)) + 'B'
        data = unpack(str_unpack, data)
        data = list(data)

        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7d:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1

        return data


    def getMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = split[1].split('#')[0]
        length = int(length, 16)

        if LOG_MEM:
            logging.debug("GDB getMem: addr=%x len=%x", addr, length)

        try:
            val = ''
            mem = self.target.readBlockMemoryUnaligned8(addr, length)
            # Flush so an exception is thrown now if invalid memory was accesses
            self.target.flush()
            for x in mem:
                if x >= 0x10:
                    val += hex(x)[2:4]
                else:
                    val += '0' + hex(x)[2:3]
        except DAPAccess.TransferError:
            logging.debug("getMemory failed at 0x%x" % addr)
            val = 'E01' #EPERM
        return self.createRSPPacket(val)

    def writeMemoryHex(self, data):
        split = data.split(',')
        addr = int(split[0], 16)

        split = split[1].split(':')
        length = int(split[0], 16)

        split = split[1].split('#')
        data = hexToByteList(split[0])

        if LOG_MEM:
            logging.debug("GDB writeMemHex: addr=%x len=%x", addr, length)

        try:
            if length > 0:
                self.target.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target.flush()
            resp = "OK"
        except DAPAccess.TransferError:
            logging.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01' #EPERM

        return self.createRSPPacket(resp)

    def writeMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = int(split[1].split(':')[0], 16)

        if LOG_MEM:
            logging.debug("GDB writeMem: addr=%x len=%x", addr, length)

        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ':':
                idx_begin += 1
                break
            idx_begin += 1

        data = data[idx_begin:len(data) - 3]
        data = self.unescape(data)

        try:
            if length > 0:
                self.target.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target.flush()
            resp = "OK"
        except DAPAccess.TransferError:
            logging.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01' #EPERM

        return self.createRSPPacket(resp)

    def readRegister(self, which):
        return self.createRSPPacket(self.target.gdbGetRegister(which))

    def writeRegister(self, data):
        reg = int(data.split('=')[0], 16)
        val = data.split('=')[1].split('#')[0]
        self.target.setRegister(reg, val)
        return self.createRSPPacket("OK")

    def getRegisters(self):
        return self.createRSPPacket(self.target.getRegisterContext())

    def setRegisters(self, data):
        self.target.setRegisterContext(data)
        return self.createRSPPacket("OK")

    def handleQuery(self, msg):
        query = msg.split(':')
        logging.debug('GDB received query: %s', query)

        if query is None:
            logging.error('GDB received query packet malformed')
            return None

        if query[0] == 'Supported':
            # Save features sent by gdb.
            self.gdb_features = query[1].split(';')

            # Build our list of features.
            features = ['qXfer:features:read+', 'QStartNoAckMode+', 'qXfer:threads:read+', 'QNonStop+']
            features.append('PacketSize=' + hex(self.packet_size)[2:])
            if self.target.getMemoryMapXML() is not None:
                features.append('qXfer:memory-map:read+')
            resp = ';'.join(features)
            return self.createRSPPacket(resp)

        elif query[0] == 'Xfer':

            if query[1] == 'features' and query[2] == 'read' and \
               query[3] == 'target.xml':
                data = query[4].split(',')
                resp = self.handleQueryXML('read_feature', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == 'memory-map' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('memory_map', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == 'threads' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('threads', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            else:
                logging.debug("Unsupported qXfer request: %s:%s:%s:%s", query[1], query[2], query[3], query[4])
                return None

        elif query[0].startswith('C'):
            return self.createRSPPacket("QC1")

        elif query[0].find('Attached') != -1:
            return self.createRSPPacket("1")

        elif query[0].find('TStatus') != -1:
            return self.createRSPPacket("")

        elif query[0].find('Tf') != -1:
            return self.createRSPPacket("")

        elif 'Offsets' in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)

        elif 'Symbol' in query[0]:
            resp = "OK"
            return self.createRSPPacket(resp)

        elif query[0].startswith('Rcmd,'):
            cmd = hexDecode(query[0][5:].split('#')[0])
            return self.handleRemoteCommand(cmd)

        else:
            return self.createRSPPacket("")

    # TODO rewrite the remote command handler
    def handleRemoteCommand(self, cmd):
        logging.debug('Remote command: %s', cmd)

        safecmd = {
            'reset' : ['Reset target', 0x1],
            'halt'  : ['Halt target', 0x2],
            'resume': ['Resume target', 0x4],
            'help'  : ['Display this help', 0x80],
            'reg'   : ['Show registers', 0],
            'init'  : ['Init reset sequence', 0]
        }
        resultMask = 0x00
        resp = 'OK'
        if cmd == 'help':
            resp = ''
            for k, v in safecmd.items():
                resp += '%s\t%s\n' % (k, v[0])
            resp = hexEncode(resp)
        elif cmd.startswith('arm semihosting'):
            self.enable_semihosting = 'enable' in cmd
            logging.info("Semihosting %s", ('enabled' if self.enable_semihosting else 'disabled'))
        else:
            cmdList = cmd.split(' ')
            #check whether all the cmds is valid cmd for monitor
            for cmd_sub in cmdList:
                if not cmd_sub in safecmd:
                    #error cmd for monitor
                    logging.warning("Invalid mon command '%s'", cmd)
                    resp = 'Invalid Command: "%s"\n' % cmd
                    resp = hexEncode(resp)
                    return self.createRSPPacket(resp)
                else:
                    resultMask = resultMask | safecmd[cmd_sub][1]
            #10000001 for help reset, so output reset cmd help information
            if resultMask == 0x81:
                resp = 'Reset the target\n'
                resp = hexEncode(resp)
            #10000010 for help halt, so output halt cmd help information
            elif resultMask == 0x82:
                resp = 'Halt the target\n'
                resp = hexEncode(resp)
            #10000100 for help resume, so output resume cmd help information
            elif resultMask == 0x84:
                resp = 'Resume the target\n'
                resp = hexEncode(resp)
            #11 for reset halt cmd, so launch self.target.resetStopOnReset()
            elif resultMask == 0x3:
                self.target.resetStopOnReset()
            #111 for reset halt resume cmd, so launch self.target.resetStopOnReset() and self.target.resume()
            elif resultMask == 0x7:
                self.target.resetStopOnReset()
                self.target.resume()
            elif resultMask == 0x1:
                self.target.reset()
            elif resultMask == 0x2:
                self.target.halt()

            if self.target.getState() != Target.TARGET_HALTED:
                logging.error("Remote command left target running!")
                logging.error("Forcing target to halt")
                self.target.halt()

        return self.createRSPPacket(resp)

    def handleGeneralSet(self, msg):
        feature = msg.split('#')[0]
        logging.debug("GDB general set: %s", feature)

        if feature == 'StartNoAckMode':
            # Disable acks after the reply and ack.
            self.packet_io.set_send_acks(False)
            return self.createRSPPacket("OK")

        elif feature.startswith('NonStop'):
            enable = feature.split(':')[1]
            self.non_stop = (enable == '1')
            return self.createRSPPacket("OK")

        else:
            return self.createRSPPacket("")

    def handleQueryXML(self, query, offset, size):
        logging.debug('GDB query %s: offset: %s, size: %s', query, offset, size)
        xml = ''
        if query == 'memory_map':
            xml = self.target.getMemoryMapXML()
        elif query == 'read_feature':
            xml = self.target.getTargetXML()
        elif query == 'threads':
            xml = self.target.getThreadsXML()
        else:
            raise RuntimeError("Invalid XML query (%s)" % query)

        size_xml = len(xml)

        prefix = 'm'

        if offset > size_xml:
            logging.error('GDB: xml offset > size for %s!', query)
            return

        if size > (self.packet_size - 4):
            size = self.packet_size - 4

        nbBytesAvailable = size_xml - offset

        if size > nbBytesAvailable:
            prefix = 'l'
            size = nbBytesAvailable

        resp = prefix + xml[offset:offset + size]

        return resp


    def createRSPPacket(self, data):
        resp = '$' + data + '#' + checksum(data)
        return resp

    def syscall(self, op):
        logging.debug("GDB server syscall: %s", op)
        request = self.createRSPPacket('F' + op)
        self.packet_io.send(request)

        while not self.packet_io.interrupt_event.is_set():
            # Read a packet.
            packet = self.packet_io.receive(False)
            if packet is None:
                sleep(0.1)
                continue

            # Check for file I/O response.
            if packet[0] == '$' and packet[1] == 'F':
                logging.debug("Syscall: got syscall response " + packet)
                args = packet[2:packet.index('#')].split(',')
                result = int(args[0], base=16)
                errno = int(args[1], base=16) if len(args) > 1 else 0
                ctrl_c = args[2] if len(args) > 2 else ''
                if ctrl_c == 'C':
                    self.packet_io.interrupt_event.set()
                    self.packet_io.drop_reply = True
                return result, errno

            # decode and prepare resp
            resp, detach = self.handleMsg(packet)

            if resp is not None:
                # send resp
                self.packet_io.send(resp)

            if detach:
                self.detach_event.set()
                logging.warning("GDB server received detach request while waiting for file I/O completion")
                break

        return -1, 0
Пример #5
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """
    def __init__(self, board, port_urlWSS, options={}):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.log = logging.getLogger('gdbserver')
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.vector_catch = options.get('vector_catch', Target.CATCH_HARD_FAULT)
        self.board.target.setVectorCatch(self.vector_catch)
        self.step_into_interrupt = options.get('step_into_interrupt', False)
        self.persist = options.get('persist', False)
        self.soft_bkpt_as_hard = options.get('soft_bkpt_as_hard', False)
        self.chip_erase = options.get('chip_erase', None)
        self.hide_programming_progress = options.get('hide_programming_progress', False)
        self.fast_program = options.get('fast_program', False)
        self.enable_semihosting = options.get('enable_semihosting', False)
        self.telnet_port = options.get('telnet_port', 4444)
        self.semihost_use_syscalls = options.get('semihost_use_syscalls', False)
        self.server_listening_callback = options.get('server_listening_callback', None)
        self.serve_local_only = options.get('serve_local_only', True)
        self.packet_size = 2048
        self.packet_io = None
        self.gdb_features = []
        self.non_stop = False
        self.is_target_running = (self.target.getState() == Target.TARGET_RUNNING)
        self.flashBuilder = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        self.target_context = self.target.getTargetContext()
        self.target_facade = GDBDebugContextFacade(self.target_context)
        self.thread_provider = None
        self.did_init_thread_providers = False
        self.current_thread_id = 0
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
            if self.serve_local_only:
                self.abstract_socket.host = 'localhost'
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)

        # Init semihosting and telnet console.
        if self.semihost_use_syscalls:
            semihost_io_handler = GDBSyscallIOHandler(self)
        else:
            # Use internal IO handler.
            semihost_io_handler = semihost.InternalSemihostIOHandler()
        self.telnet_console = semihost.TelnetSemihostIOHandler(self.telnet_port, self.serve_local_only)
        self.semihost = semihost.SemihostAgent(self.target_context, io_handler=semihost_io_handler, console=self.telnet_console)

        # Command handler table.
        #
        # The dict keys are the first character of the incoming command from gdb. Values are a
        # bi-tuple. The first element is the handler method, and the second element is the start
        # offset of the command string passed to the handler.
        #
        # Start offset values:
        #  0 - Special case: handler method does not take any parameters.
        #  1 - Strip off leading "$" from command.
        #  2 - Strip leading "$" plus character matched through this table.
        #  3+ - Supported, but not very useful.
        #
        self.COMMANDS = {
        #       CMD    HANDLER                  START    DESCRIPTION
                '?' : (self.stopReasonQuery,    0   ), # Stop reason query.
                'C' : (self.resume,             1   ), # Continue (at addr)
                'c' : (self.resume,             1   ), # Continue with signal.
                'D' : (self.detach,             1   ), # Detach.
                'g' : (self.getRegisters,       0   ), # Read general registers.
                'G' : (self.setRegisters,       2   ), # Write general registers.
                'H' : (self.setThread,          2   ), # Set thread for subsequent operations.
                'k' : (self.kill,               0   ), # Kill.
                'm' : (self.getMemory,          2   ), # Read memory.
                'M' : (self.writeMemoryHex,     2   ), # Write memory (hex).
                'p' : (self.readRegister,       2   ), # Read register.
                'P' : (self.writeRegister,      2   ), # Write register.
                'q' : (self.handleQuery,        2   ), # General query.
                'Q' : (self.handleGeneralSet,   2   ), # General set.
                's' : (self.step,               1   ), # Single step.
                'S' : (self.step,               1   ), # Step with signal.
                'T' : (self.isThreadAlive,      1   ), # Thread liveness query.
                'v' : (self.vCommand,           2   ), # v command.
                'X' : (self.writeMemory,        2   ), # Write memory (binary).
                'z' : (self.breakpoint,         1   ), # Insert breakpoint/watchpoint.
                'Z' : (self.breakpoint,         1   ), # Remove breakpoint/watchpoint.
            }

        # Commands that kill the connection to gdb.
        self.DETACH_COMMANDS = ('D', 'k')

        self.setDaemon(True)
        self.start()

    def restart(self):
        if self.isAlive():
            self.detach_event.set()

    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            while self.isAlive():
                pass
            self.log.info("GDB server thread killed")
        self.board.uninit()

    def setBoard(self, board, stop=True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return

    def _cleanup(self):
        self.log.debug("GDB server cleaning up")
        if self.packet_io:
            self.packet_io.stop()
            self.packet_io = None
        if self.semihost:
            self.semihost.cleanup()
            self.semihost = None
        if self.telnet_console:
            self.telnet_console.stop()
            self.telnet_console = None

    def _cleanup_for_next_connection(self):
        self.non_stop = False
        self.thread_provider = None
        self.did_init_thread_providers = False
        self.current_thread_id = 0

    def run(self):
        self.log.info('GDB server started at port:%d', self.port)

        while True:
            try:
                self.detach_event.clear()

                # Inform callback that the server is running.
                if self.server_listening_callback:
                    self.server_listening_callback(self)

                while not self.shutdown_event.isSet() and not self.detach_event.isSet():
                    connected = self.abstract_socket.connect()
                    if connected != None:
                        self.packet_io = GDBServerPacketIOThread(self.abstract_socket)
                        break

                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    continue

                self.log.info("One client connected!")
                self._run_connection()

            except Exception as e:
                self.log.error("Unexpected exception: %s", e)
                traceback.print_exc()

    def _run_connection(self):
        while True:
            try:
                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    break

                if self.packet_io.interrupt_event.isSet():
                    if self.non_stop:
                        self.target.halt()
                        self.is_target_running = False
                        self.sendStopNotification()
                    else:
                        self.log.error("Got unexpected ctrl-c, ignoring")
                    self.packet_io.interrupt_event.clear()

                if self.non_stop and self.is_target_running:
                    try:
                        if self.target.getState() == Target.TARGET_HALTED:
                            self.log.debug("state halted")
                            self.is_target_running = False
                            self.sendStopNotification()
                    except Exception as e:
                        self.log.error("Unexpected exception: %s", e)
                        traceback.print_exc()

                # read command
                try:
                    packet = self.packet_io.receive(block=not self.non_stop)
                except ConnectionClosedException:
                    break

                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    break

                if self.non_stop and packet is None:
                    sleep(0.1)
                    continue

                self.lock.acquire()

                if len(packet) != 0:
                    # decode and prepare resp
                    resp, detach = self.handleMsg(packet)

                    if resp is not None:
                        # send resp
                        self.packet_io.send(resp)

                    if detach:
                        self.abstract_socket.close()
                        self.packet_io.stop()
                        self.packet_io = None
                        self.lock.release()
                        if self.persist:
                            self._cleanup_for_next_connection()
                            break
                        else:
                            self.shutdown_event.set()
                            return

                self.lock.release()

            except Exception as e:
                self.log.error("Unexpected exception: %s", e)
                traceback.print_exc()

    def handleMsg(self, msg):
        try:
            assert msg[0] == '$', "invalid first char of message (!= $"

            try:
                handler, msgStart = self.COMMANDS[msg[1]]
                if msgStart == 0:
                    reply = handler()
                else:
                    reply = handler(msg[msgStart:])
                detach = 1 if msg[1] in self.DETACH_COMMANDS else 0
                return reply, detach
            except (KeyError, IndexError):
                self.log.error("Unknown RSP packet: %s", msg)
                return self.createRSPPacket(""), 0

        except Exception as e:
            self.log.error("Unhandled exception in handleMsg: %s", e)
            traceback.print_exc()
            return self.createRSPPacket("E01"), 0

    def detach(self, data):
        self.log.info("Client detached")
        resp = "OK"
        return self.createRSPPacket(resp)

    def kill(self):
        self.log.debug("GDB kill")
        # Keep target halted and leave vector catches if in persistent mode.
        if not self.persist:
            self.board.target.setVectorCatch(Target.CATCH_NONE)
            self.board.target.resume()
        return self.createRSPPacket("")

    def breakpoint(self, data):
        # handle breakpoint/watchpoint commands
        split = data.split('#')[0].split(',')
        addr = int(split[1], 16)
        self.log.debug("GDB breakpoint %s%d @ %x" % (data[0], int(data[1]), addr))

        # handle software breakpoint Z0/z0
        if data[1] == '0' and not self.soft_bkpt_as_hard:
            if data[0] == 'Z':
                if not self.target.setBreakpoint(addr, Target.BREAKPOINT_SW):
                    return self.createRSPPacket('E01') #EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware breakpoint Z1/z1
        if data[1] == '1' or (self.soft_bkpt_as_hard and data[1] == '0'):
            if data[0] == 'Z':
                if self.target.setBreakpoint(addr, Target.BREAKPOINT_HW) == False:
                    return self.createRSPPacket('E01') #EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware watchpoint Z2/z2/Z3/z3/Z4/z4
        if data[1] == '2':
            # Write-only watch
            watchpoint_type = Target.WATCHPOINT_WRITE
        elif data[1] == '3':
            # Read-only watch
            watchpoint_type = Target.WATCHPOINT_READ
        elif data[1] == '4':
            # Read-Write watch
            watchpoint_type = Target.WATCHPOINT_READ_WRITE
        else:
            return self.createRSPPacket('E01') #EPERM

        size = int(split[2], 16)
        if data[0] == 'Z':
            if self.target.setWatchpoint(addr, size, watchpoint_type) == False:
                return self.createRSPPacket('E01') #EPERM
        else:
            self.target.removeWatchpoint(addr, size, watchpoint_type)
        return self.createRSPPacket("OK")

    def setThread(self, data):
        if not self.is_threading_enabled():
            return self.createRSPPacket('OK')

        self.log.debug("setThread:%s", data)
        op = data[0]
        thread_id = int(data[1:-3], 16)
        if not (thread_id in (0, -1) or self.thread_provider.is_valid_thread_id(thread_id)):
            return self.createRSPPacket('E01')

        if op == 'c':
            pass
        elif op == 'g':
            if thread_id == -1:
                self.target_facade.set_context(self.target_context)
            else:
                if thread_id == 0:
                    thread = self.thread_provider.current_thread
                    thread_id = thread.unique_id
                else:
                    thread = self.thread_provider.get_thread(thread_id)
                self.target_facade.set_context(thread.context)
        else:
            return self.createRSPPacket('E01')

        self.current_thread_id = thread_id
        return self.createRSPPacket('OK')

    def isThreadAlive(self, data):
        threadId = int(data[1:-3], 16)

        if self.is_threading_enabled():
            isAlive = self.thread_provider.is_valid_thread_id(threadId)
        else:
            isAlive = (threadId == 1)

        if isAlive:
            return self.createRSPPacket('OK')
        else:
            self.validateDebugContext()
            return self.createRSPPacket('E00')

    def validateDebugContext(self):
        if self.is_threading_enabled():
            currentThread = self.thread_provider.current_thread
            if self.current_thread_id != currentThread.unique_id:
                self.target_facade.set_context(currentThread.context)
                self.current_thread_id = currentThread.unique_id
        else:
            if self.current_thread_id != 1:
                self.log.debug("Current thread %x is no longer valid, switching context to target", self.current_thread_id)
                self.target_facade.set_context(self.target_context)
                self.current_thread_id = 1

    def stopReasonQuery(self):
        # In non-stop mode, if no threads are stopped we need to reply with OK.
        if self.non_stop and self.is_target_running:
            return self.createRSPPacket("OK")

        return self.createRSPPacket(self.getTResponse())

    def _get_resume_step_addr(self, data):
        if data is None:
            return None
        data = data.split('#')[0]
        if ';' not in data:
            return None
        # c[;addr]
        if data[0] in ('c', 's'):
            addr = int(data[2:], base=16)
        # Csig[;addr]
        elif data[0] in ('C', 'S'):
            addr = int(data[1:].split(';')[1], base=16)
        return addr

    def resume(self, data):
        addr = self._get_resume_step_addr(data)
        self.target.resume()
        self.log.debug("target resumed")

        val = ''

        while True:
            if self.shutdown_event.isSet():
                self.packet_io.interrupt_event.clear()
                return self.createRSPPacket(val)

            # Wait for a ctrl-c to be received.
            if self.packet_io.interrupt_event.wait(0.01):
                self.log.debug("receive CTRL-C")
                self.packet_io.interrupt_event.clear()
                self.target.halt()
                val = self.getTResponse(forceSignal=signals.SIGINT)
                break

            try:
                if self.target.getState() == Target.TARGET_HALTED:
                    # Handle semihosting
                    if self.enable_semihosting:
                        was_semihost = self.semihost.check_and_handle_semihost_request()

                        if was_semihost:
                            self.target.resume()
                            continue

                    pc = self.target_context.readCoreRegister('pc')
                    self.log.debug("state halted; pc=0x%08x", pc)
                    val = self.getTResponse()
                    break
            except Exception as e:
                try:
                    self.target.halt()
                except:
                    pass
                traceback.print_exc()
                self.log.debug('Target is unavailable temporarily.')
                val = 'S%02x' % self.target_facade.getSignalValue()
                break

        return self.createRSPPacket(val)

    def step(self, data):
        addr = self._get_resume_step_addr(data)
        self.log.debug("GDB step: %s", data)
        self.target.step(not self.step_into_interrupt)
        return self.createRSPPacket(self.getTResponse())

    def halt(self):
        self.target.halt()
        return self.createRSPPacket(self.getTResponse())

    def sendStopNotification(self, forceSignal=None):
        data = self.getTResponse(forceSignal=forceSignal)
        packet = '%Stop:' + data + '#' + checksum(data)
        self.packet_io.send(packet)

    def vCommand(self, data):
        cmd = data.split('#')[0]

        # Flash command.
        if cmd.startswith('Flash'):
            return self.flashOp(data)

        # vCont capabilities query.
        elif 'Cont?' == cmd:
            return self.createRSPPacket("vCont;c;C;s;S;t")

        # vCont, thread action command.
        elif cmd.startswith('Cont'):
            return self.vCont(cmd)

        # vStopped, part of thread stop state notification sequence.
        elif 'Stopped' in cmd:
            # Because we only support one thread for now, we can just reply OK to vStopped.
            return self.createRSPPacket("OK")

        return self.createRSPPacket("")

    # Example: $vCont;s:1;c#c1
    def vCont(self, cmd):
        ops = cmd.split(';')[1:] # split and remove 'Cont' from list
        if not ops:
            return self.createRSPPacket("OK")

        if self.is_threading_enabled():
            thread_actions = {}
            threads = self.thread_provider.get_threads()
            for k in threads:
                thread_actions[k.unique_id] = None
            currentThread = self.thread_provider.get_current_thread_id()
        else:
            thread_actions = { 1 : None } # our only thread
            currentThread = 1
        default_action = None

        for op in ops:
            args = op.split(':')
            action = args[0]
            if len(args) > 1:
                thread_id = int(args[1], 16)
                if thread_id == -1 or thread_id == 0:
                    thread_id = currentThread
                thread_actions[thread_id] = action
            else:
                default_action = action

        self.log.debug("thread_actions=%s; default_action=%s", repr(thread_actions), default_action)

        # Only the current thread is supported at the moment.
        if thread_actions[currentThread] is None:
            if default_action is None:
                return self.createRSPPacket('E01')
            thread_actions[currentThread] = default_action

        if thread_actions[currentThread][0] in ('c', 'C'):
            if self.non_stop:
                self.target.resume()
                self.is_target_running = True
                return self.createRSPPacket("OK")
            else:
                return self.resume(None)
        elif thread_actions[currentThread][0] in ('s', 'S'):
            if self.non_stop:
                self.target.step(not self.step_into_interrupt)
                self.packet_io.send(self.createRSPPacket("OK"))
                self.sendStopNotification()
                return None
            else:
                return self.step(None)
        elif thread_actions[currentThread] == 't':
            # Must ignore t command in all-stop mode.
            if not self.non_stop:
                return self.createRSPPacket("")
            self.packet_io.send(self.createRSPPacket("OK"))
            self.target.halt()
            self.is_target_running = False
            self.sendStopNotification(forceSignal=0)
        else:
            self.log.error("Unsupported vCont action '%s'" % thread_actions[1])

    def flashOp(self, data):
        ops = data.split(':')[0]
        self.log.debug("flash op: %s", ops)

        if ops == 'FlashErase':
            return self.createRSPPacket("OK")

        elif ops == 'FlashWrite':
            write_addr = int(data.split(':')[1], 16)
            self.log.debug("flash write addr: 0x%x", write_addr)
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ':':
                    second_colon += 1
                idx_begin += 1

            # Get flash builder if there isn't one already
            if self.flashBuilder == None:
                self.flashBuilder = self.flash.getFlashBuilder()

            # Add data to flash builder
            self.flashBuilder.addData(write_addr, self.unescape(data[idx_begin:len(data) - 3]))


            return self.createRSPPacket("OK")

        # we need to flash everything
        elif 'FlashDone' in ops :

            def print_progress(progress):
                # Reset state on 0.0
                if progress == 0.0:
                    print_progress.done = False

                # print progress bar
                if not print_progress.done:
                    sys.stdout.write('\r')
                    i = int(progress * 20.0)
                    sys.stdout.write("[%-20s] %3d%%" % ('=' * i, round(progress * 100)))
                    sys.stdout.flush()

                # Finish on 1.0
                if progress >= 1.0:
                    if not print_progress.done:
                        print_progress.done = True
                        sys.stdout.write("\r\n")
                        sys.stdout.flush()

            if self.hide_programming_progress:
                progress_cb = None
            else:
                 progress_cb = print_progress

            self.flashBuilder.program(chip_erase=self.chip_erase, progress_cb=progress_cb, fast_verify=self.fast_program)

            # Set flash builder to None so that on the next flash command a new
            # object is used.
            self.flashBuilder = None

            return self.createRSPPacket("OK")

        return None

    def unescape(self, data):
        data_idx = 0

        # unpack the data into binary array
        str_unpack = str(len(data)) + 'B'
        data = unpack(str_unpack, data)
        data = list(data)

        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7d:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1

        return data

    def escape(self, data):
        result = ''
        for c in data:
            if c in '#$}*':
                result += '}' + chr(ord(c) ^ 0x20)
            else:
                result += c
        return result

    def getMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = split[1].split('#')[0]
        length = int(length, 16)

        if LOG_MEM:
            self.log.debug("GDB getMem: addr=%x len=%x", addr, length)

        try:
            val = ''
            mem = self.target_context.readBlockMemoryUnaligned8(addr, length)
            # Flush so an exception is thrown now if invalid memory was accesses
            self.target_context.flush()
            for x in mem:
                if x >= 0x10:
                    val += hex(x)[2:4]
                else:
                    val += '0' + hex(x)[2:3]
        except DAPAccess.TransferError:
            self.log.debug("getMemory failed at 0x%x" % addr)
            val = 'E01' #EPERM
        except MemoryAccessError as e:
            logging.debug("getMemory failed at 0x%x: %s", addr, str(e))
            val = 'E01' #EPERM
        return self.createRSPPacket(val)

    def writeMemoryHex(self, data):
        split = data.split(',')
        addr = int(split[0], 16)

        split = split[1].split(':')
        length = int(split[0], 16)

        split = split[1].split('#')
        data = hexToByteList(split[0])

        if LOG_MEM:
            self.log.debug("GDB writeMemHex: addr=%x len=%x", addr, length)

        try:
            if length > 0:
                self.target_context.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target_context.flush()
            resp = "OK"
        except DAPAccess.TransferError:
            self.log.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01' #EPERM
        except MemoryAccessError as e:
            logging.debug("getMemory failed at 0x%x: %s", addr, str(e))
            val = 'E01' #EPERM

        return self.createRSPPacket(resp)

    def writeMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = int(split[1].split(':')[0], 16)

        if LOG_MEM:
            self.log.debug("GDB writeMem: addr=%x len=%x", addr, length)

        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ':':
                idx_begin += 1
                break
            idx_begin += 1

        data = data[idx_begin:len(data) - 3]
        data = self.unescape(data)

        try:
            if length > 0:
                self.target_context.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target_context.flush()
            resp = "OK"
        except DAPAccess.TransferError:
            self.log.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01' #EPERM
        except MemoryAccessError as e:
            logging.debug("getMemory failed at 0x%x: %s", addr, str(e))
            val = 'E01' #EPERM

        return self.createRSPPacket(resp)

    def readRegister(self, which):
        return self.createRSPPacket(self.target_facade.gdbGetRegister(which))

    def writeRegister(self, data):
        reg = int(data.split('=')[0], 16)
        val = data.split('=')[1].split('#')[0]
        self.target_facade.setRegister(reg, val)
        return self.createRSPPacket("OK")

    def getRegisters(self):
        return self.createRSPPacket(self.target_facade.getRegisterContext())

    def setRegisters(self, data):
        self.target_facade.setRegisterContext(data)
        return self.createRSPPacket("OK")

    def handleQuery(self, msg):
        query = msg.split(':')
        self.log.debug('GDB received query: %s', query)

        if query is None:
            self.log.error('GDB received query packet malformed')
            return None

        if query[0] == 'Supported':
            # Save features sent by gdb.
            self.gdb_features = query[1].split(';')

            # Build our list of features.
            features = ['qXfer:features:read+', 'QStartNoAckMode+', 'qXfer:threads:read+', 'QNonStop+']
            features.append('PacketSize=' + hex(self.packet_size)[2:])
            if self.target_facade.getMemoryMapXML() is not None:
                features.append('qXfer:memory-map:read+')
            resp = ';'.join(features)
            return self.createRSPPacket(resp)

        elif query[0] == 'Xfer':

            if query[1] == 'features' and query[2] == 'read' and \
               query[3] == 'target.xml':
                data = query[4].split(',')
                resp = self.handleQueryXML('read_feature', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == 'memory-map' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('memory_map', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == 'threads' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('threads', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            else:
                self.log.debug("Unsupported qXfer request: %s:%s:%s:%s", query[1], query[2], query[3], query[4])
                return None

        elif query[0].startswith('C'):
            if not self.is_threading_enabled():
                return self.createRSPPacket("QC1")
            else:
                self.validateDebugContext()
                return self.createRSPPacket("QC%x" % self.current_thread_id)

        elif query[0].find('Attached') != -1:
            return self.createRSPPacket("1")

        elif query[0].find('TStatus') != -1:
            return self.createRSPPacket("")

        elif query[0].find('Tf') != -1:
            return self.createRSPPacket("")

        elif 'Offsets' in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)

        elif 'Symbol' in query[0]:
            if self.did_init_thread_providers:
                return self.createRSPPacket("OK")
            return self.initThreadProviders()

        elif query[0].startswith('Rcmd,'):
            cmd = hexDecode(query[0][5:].split('#')[0])
            return self.handleRemoteCommand(cmd)

        else:
            return self.createRSPPacket("")

    def initThreadProviders(self):
        symbol_provider = GDBSymbolProvider(self)

        for rtosName, rtosClass in RTOS.iteritems():
            try:
                self.log.info("Attempting to load %s", rtosName)
                rtos = rtosClass(self.target)
                if rtos.init(symbol_provider):
                    self.log.info("%s loaded successfully", rtosName)
                    self.thread_provider = rtos
                    break
            except RuntimeError as e:
                self.log.error("Error during symbol lookup: " + str(e))
                traceback.print_exc()

        self.did_init_thread_providers = True

        # Done with symbol processing.
        return self.createRSPPacket("OK")

    def getSymbol(self, name):
        # Send the symbol request.
        request = self.createRSPPacket('qSymbol:' + hexEncode(name))
        self.packet_io.send(request)

        # Read a packet.
        packet = self.packet_io.receive()

        # Parse symbol value reply packet.
        packet = packet[1:-3]
        if not packet.startswith('qSymbol:'):
            raise RuntimeError("Got unexpected response from gdb when asking for symbol value")
        packet = packet[8:]
        symValue, symName = packet.split(':')

        symName = hexDecode(symName)
        if symName != name:
            raise RuntimeError("Symbol value reply from gdb has unexpected symbol name")
        if symValue:
            symValue = hex8leToU32le(symValue)
        else:
            return None
        return symValue

    # TODO rewrite the remote command handler
    def handleRemoteCommand(self, cmd):
        self.log.debug('Remote command: %s', cmd)

        safecmd = {
            'init'  : ['Init reset sequence', 0x1],
            'reset' : ['Reset and halt the target', 0x2],
            'halt'  : ['Halt target', 0x4],
            # 'resume': ['Resume target', 0x8],
            'help'  : ['Display this help', 0x80],
        }

        cmdList = cmd.split()
        resp = 'OK'
        if cmd == 'help':
            resp = ''.join(['%s\t%s\n' % (k, v[0]) for k, v in safecmd.items()])
            resp = hexEncode(resp)
        elif cmd.startswith('arm semihosting'):
            self.enable_semihosting = 'enable' in cmd
            self.log.info("Semihosting %s", ('enabled' if self.enable_semihosting else 'disabled'))
        elif cmdList[0] == 'set':
            if len(cmdList) < 3:
                resp = hexEncode("Error: invalid set command")
            elif cmdList[1] == 'vector-catch':
                try:
                    self.board.target.setVectorCatch(convert_vector_catch(cmdList[2]))
                except ValueError as e:
                    resp = hexEncode("Error: " + str(e))
            elif cmdList[1] == 'step-into-interrupt':
                self.step_into_interrupt = (cmdList[2].lower() in ("true", "on", "yes", "1"))
            else:
                resp = hexEncode("Error: invalid set option")
        else:
            resultMask = 0x00
            if cmdList[0] == 'help':
                # a 'help' is only valid as the first cmd, and only
                # gives info on the second cmd if it is valid
                resultMask |= 0x80
                del cmdList[0]

            for cmd_sub in cmdList:
                if cmd_sub not in safecmd:
                    self.log.warning("Invalid mon command '%s'", cmd_sub)
                    resp = 'Invalid Command: "%s"\n' % cmd_sub
                    resp = hexEncode(resp)
                    return self.createRSPPacket(resp)
                elif resultMask == 0x80:
                    # if the first command was a 'help', we only need
                    # to return info about the first cmd after it
                    resp = hexEncode(safecmd[cmd_sub][0]+'\n')
                    return self.createRSPPacket(resp)
                resultMask |= safecmd[cmd_sub][1]

            # Run cmds in proper order
            if resultMask & 0x1:
                pass
            if (resultMask & 0x6) == 0x6:
                self.target.resetStopOnReset()
            elif resultMask & 0x2:
                # on 'reset' still do a reset halt
                self.target.resetStopOnReset()
                # self.target.reset()
            elif resultMask & 0x4:
                self.target.halt()
            # if resultMask & 0x8:
            #     self.target.resume()

        return self.createRSPPacket(resp)

    def handleGeneralSet(self, msg):
        feature = msg.split('#')[0]
        self.log.debug("GDB general set: %s", feature)

        if feature == 'StartNoAckMode':
            # Disable acks after the reply and ack.
            self.packet_io.set_send_acks(False)
            return self.createRSPPacket("OK")

        elif feature.startswith('NonStop'):
            enable = feature.split(':')[1]
            self.non_stop = (enable == '1')
            return self.createRSPPacket("OK")

        else:
            return self.createRSPPacket("")

    def handleQueryXML(self, query, offset, size):
        self.log.debug('GDB query %s: offset: %s, size: %s', query, offset, size)
        xml = ''
        if query == 'memory_map':
            xml = self.target_facade.getMemoryMapXML()
        elif query == 'read_feature':
            xml = self.target.getTargetXML()
        elif query == 'threads':
            xml = self.getThreadsXML()
        else:
            raise RuntimeError("Invalid XML query (%s)" % query)

        size_xml = len(xml)

        prefix = 'm'

        if offset > size_xml:
            self.log.error('GDB: xml offset > size for %s!', query)
            return

        if size > (self.packet_size - 4):
            size = self.packet_size - 4

        nbBytesAvailable = size_xml - offset

        if size > nbBytesAvailable:
            prefix = 'l'
            size = nbBytesAvailable

        resp = prefix + self.escape(xml[offset:offset + size])

        return resp


    def createRSPPacket(self, data):
        resp = '$' + data + '#' + checksum(data)
        return resp

    def syscall(self, op):
        self.log.debug("GDB server syscall: %s", op)
        request = self.createRSPPacket('F' + op)
        self.packet_io.send(request)

        while not self.packet_io.interrupt_event.is_set():
            # Read a packet.
            packet = self.packet_io.receive(False)
            if packet is None:
                sleep(0.1)
                continue

            # Check for file I/O response.
            if packet[0] == '$' and packet[1] == 'F':
                self.log.debug("Syscall: got syscall response " + packet)
                args = packet[2:packet.index('#')].split(',')
                result = int(args[0], base=16)
                errno = int(args[1], base=16) if len(args) > 1 else 0
                ctrl_c = args[2] if len(args) > 2 else ''
                if ctrl_c == 'C':
                    self.packet_io.interrupt_event.set()
                    self.packet_io.drop_reply = True
                return result, errno

            # decode and prepare resp
            resp, detach = self.handleMsg(packet)

            if resp is not None:
                # send resp
                self.packet_io.send(resp)

            if detach:
                self.detach_event.set()
                self.log.warning("GDB server received detach request while waiting for file I/O completion")
                break

        return -1, 0

    def getTResponse(self, forceSignal=None):
        self.validateDebugContext()
        response = self.target_facade.getTResponse(forceSignal)

        # Append thread and core
        if not self.is_threading_enabled():
            response += "thread:1;core:0;"
        else:
            if self.current_thread_id in (-1, 0, 1):
                response += "thread:%x;core:0;" % self.thread_provider.current_thread.unique_id
            else:
                response += "thread:%x;core:0;" % self.current_thread_id
        self.log.debug("Tresponse=%s", response)
        return response

    def getThreadsXML(self):
        root = Element('threads')

        if not self.is_threading_enabled():
            t = SubElement(root, 'thread', id="1", core="0")
            t.text = "Thread mode"
        else:
            threads = self.thread_provider.get_threads()
            for thread in threads:
                hexId = "%x" % thread.unique_id
                t = SubElement(root, 'thread', id=hexId, core="0")

                desc = thread.description
                if desc:
                    desc = thread.name + "; " + desc
                else:
                    desc = thread.name
                t.text = desc

        return '<?xml version="1.0"?><!DOCTYPE feature SYSTEM "threads.dtd">' + tostring(root)

    def is_threading_enabled(self):
        return (self.thread_provider is not None) and self.thread_provider.is_enabled \
            and (self.thread_provider.current_thread is not None)
Пример #6
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """

    def __init__(self, board, port_urlWSS, options={}):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.break_at_hardfault = bool(options.get("break_at_hardfault", True))
        self.board.target.setVectorCatchFault(self.break_at_hardfault)
        self.break_on_reset = options.get("break_on_reset", False)
        self.board.target.setVectorCatchReset(self.break_on_reset)
        self.step_into_interrupt = options.get("step_into_interrupt", False)
        self.persist = options.get("persist", False)
        self.soft_bkpt_as_hard = options.get("soft_bkpt_as_hard", False)
        self.chip_erase = options.get("chip_erase", None)
        self.hide_programming_progress = options.get("hide_programming_progress", False)
        self.fast_program = options.get("fast_program", False)
        self.enable_semihosting = options.get("enable_semihosting", False)
        self.telnet_port = options.get("telnet_port", 4444)
        self.semihost_use_syscalls = options.get("semihost_use_syscalls", False)
        self.server_listening_callback = options.get("server_listening_callback", None)
        self.serve_local_only = options.get("serve_local_only", True)
        self.packet_size = 2048
        self.packet_io = None
        self.gdb_features = []
        self.non_stop = False
        self.is_target_running = self.target.getState() == Target.TARGET_RUNNING
        self.flashBuilder = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
            if self.serve_local_only:
                self.abstract_socket.host = "localhost"
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)

        # Init semihosting and telnet console.
        if self.semihost_use_syscalls:
            semihost_io_handler = GDBSyscallIOHandler(self)
        else:
            # Use internal IO handler.
            semihost_io_handler = semihost.InternalSemihostIOHandler()
        self.telnet_console = semihost.TelnetSemihostIOHandler(self.telnet_port, self.serve_local_only)
        self.semihost = semihost.SemihostAgent(self.target, io_handler=semihost_io_handler, console=self.telnet_console)

        self.setDaemon(True)
        self.start()

    def restart(self):
        if self.isAlive():
            self.detach_event.set()

    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            while self.isAlive():
                pass
            logging.info("GDB server thread killed")
        self.board.uninit()

    def setBoard(self, board, stop=True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return

    def _cleanup(self):
        logging.debug("GDB server cleaning up")
        if self.packet_io:
            self.packet_io.stop()
            self.packet_io = None
        if self.semihost:
            self.semihost.cleanup()
            self.semihost = None
        if self.telnet_console:
            self.telnet_console.stop()
            self.telnet_console = None

    def run(self):
        logging.info("GDB server started at port:%d", self.port)

        while True:
            try:
                self.detach_event.clear()

                # Inform callback that the server is running.
                if self.server_listening_callback:
                    self.server_listening_callback(self)

                while not self.shutdown_event.isSet() and not self.detach_event.isSet():
                    connected = self.abstract_socket.connect()
                    if connected != None:
                        self.packet_io = GDBServerPacketIOThread(self.abstract_socket)
                        break

                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    continue

                logging.info("One client connected!")
                self._run_connection()

            except Exception as e:
                logging.error("Unexpected exception: %s", e)
                traceback.print_exc()

    def _run_connection(self):
        while True:
            try:
                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    break

                if self.packet_io.interrupt_event.isSet():
                    if self.non_stop:
                        self.target.halt()
                        self.is_target_running = False
                        self.sendStopNotification()
                    else:
                        logging.debug("Got unexpected ctrl-c, ignoring")
                    self.packet_io.interrupt_event.clear()

                if self.non_stop and self.is_target_running:
                    try:
                        if self.target.getState() == Target.TARGET_HALTED:
                            logging.debug("state halted")
                            self.is_target_running = False
                            self.sendStopNotification()
                    except Exception as e:
                        logging.error("Unexpected exception: %s", e)
                        traceback.print_exc()

                # read command
                try:
                    packet = self.packet_io.receive(block=not self.non_stop)
                except ConnectionClosedException:
                    break

                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    break

                if self.non_stop and packet is None:
                    sleep(0.1)
                    continue

                self.lock.acquire()

                if len(packet) != 0:
                    # decode and prepare resp
                    resp, detach = self.handleMsg(packet)

                    if resp is not None:
                        # send resp
                        self.packet_io.send(resp)

                    if detach:
                        self.abstract_socket.close()
                        self.packet_io.stop()
                        self.packet_io = None
                        self.lock.release()
                        if self.persist:
                            break
                        else:
                            self.shutdown_event.set()
                            return

                self.lock.release()

            except Exception as e:
                logging.error("Unexpected exception: %s", e)
                traceback.print_exc()

    def handleMsg(self, msg):
        try:
            if msg[0] != "$":
                logging.debug("msg ignored: first char != $")
                return None, 0

            # query command
            if msg[1] == "?":
                return self.stopReasonQuery(), 0

            # we don't send immediately the response for C and S commands
            elif msg[1] == "C" or msg[1] == "c":
                return self.resume(msg[1:]), 0

            elif msg[1] == "D":
                return self.detach(msg[1:]), 1

            elif msg[1] == "g":
                return self.getRegisters(), 0

            elif msg[1] == "G":
                return self.setRegisters(msg[2:]), 0

            elif msg[1] == "H":
                return self.createRSPPacket("OK"), 0

            elif msg[1] == "k":
                return self.kill(), 1

            elif msg[1] == "m":
                return self.getMemory(msg[2:]), 0

            elif msg[1] == "M":  # write memory with hex data
                return self.writeMemoryHex(msg[2:]), 0

            elif msg[1] == "p":
                return self.readRegister(msg[2:]), 0

            elif msg[1] == "P":
                return self.writeRegister(msg[2:]), 0

            elif msg[1] == "q":
                return self.handleQuery(msg[2:]), 0

            elif msg[1] == "Q":
                return self.handleGeneralSet(msg[2:]), 0

            elif msg[1] == "S" or msg[1] == "s":
                return self.step(msg[1:]), 0

            elif msg[1] == "T":  # check if thread is alive
                return self.createRSPPacket("OK"), 0

            elif msg[1] == "v":
                return self.vCommand(msg[2:]), 0

            elif msg[1] == "X":  # write memory with binary data
                return self.writeMemory(msg[2:]), 0

            elif msg[1] == "Z" or msg[1] == "z":
                return self.breakpoint(msg[1:]), 0

            else:
                logging.error("Unknown RSP packet: %s", msg)
                return self.createRSPPacket(""), 0

        except Exception as e:
            logging.error("Unhandled exception in handleMsg: %s", e)
            traceback.print_exc()
            return self.createRSPPacket("E01"), 0

    def detach(self, data):
        logging.info("Client detached")
        resp = "OK"
        return self.createRSPPacket(resp)

    def kill(self):
        logging.debug("GDB kill")
        # Keep target halted and leave vector catches if in persistent mode.
        if not self.persist:
            self.board.target.setVectorCatchFault(False)
            self.board.target.setVectorCatchReset(False)
            self.board.target.resume()
        return self.createRSPPacket("")

    def breakpoint(self, data):
        # handle breakpoint/watchpoint commands
        split = data.split("#")[0].split(",")
        addr = int(split[1], 16)
        logging.debug("GDB breakpoint %s%d @ %x" % (data[0], int(data[1]), addr))

        # handle software breakpoint Z0/z0
        if data[1] == "0" and not self.soft_bkpt_as_hard:
            if data[0] == "Z":
                if not self.target.setBreakpoint(addr, Target.BREAKPOINT_SW):
                    return self.createRSPPacket("E01")  # EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware breakpoint Z1/z1
        if data[1] == "1" or (self.soft_bkpt_as_hard and data[1] == "0"):
            if data[0] == "Z":
                if self.target.setBreakpoint(addr, Target.BREAKPOINT_HW) == False:
                    return self.createRSPPacket("E01")  # EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware watchpoint Z2/z2/Z3/z3/Z4/z4
        if data[1] == "2":
            # Write-only watch
            watchpoint_type = Target.WATCHPOINT_WRITE
        elif data[1] == "3":
            # Read-only watch
            watchpoint_type = Target.WATCHPOINT_READ
        elif data[1] == "4":
            # Read-Write watch
            watchpoint_type = Target.WATCHPOINT_READ_WRITE
        else:
            return self.createRSPPacket("E01")  # EPERM

        size = int(split[2], 16)
        if data[0] == "Z":
            if self.target.setWatchpoint(addr, size, watchpoint_type) == False:
                return self.createRSPPacket("E01")  # EPERM
        else:
            self.target.removeWatchpoint(addr, size, watchpoint_type)
        return self.createRSPPacket("OK")

    def stopReasonQuery(self):
        # In non-stop mode, if no threads are stopped we need to reply with OK.
        if self.non_stop and self.is_target_running:
            return self.createRSPPacket("OK")

        return self.createRSPPacket(self.target.getTResponse())

    def _get_resume_step_addr(self, data):
        if data is None:
            return None
        data = data.split("#")[0]
        if ";" not in data:
            return None
        # c[;addr]
        if data[0] in ("c", "s"):
            addr = int(data[2:], base=16)
        # Csig[;addr]
        elif data[0] in ("C", "S"):
            addr = int(data[1:].split(";")[1], base=16)
        return addr

    def resume(self, data):
        addr = self._get_resume_step_addr(data)
        self.target.resume()
        logging.debug("target resumed")

        val = ""

        while True:
            if self.shutdown_event.isSet():
                self.packet_io.interrupt_event.clear()
                return self.createRSPPacket(val)

            # Wait for a ctrl-c to be received.
            if self.packet_io.interrupt_event.wait(0.01):
                logging.debug("receive CTRL-C")
                self.packet_io.interrupt_event.clear()
                self.target.halt()
                val = self.target.getTResponse(forceSignal=signals.SIGINT)
                break

            try:
                if self.target.getState() == Target.TARGET_HALTED:
                    # Handle semihosting
                    if self.enable_semihosting:
                        was_semihost = self.semihost.check_and_handle_semihost_request()

                        if was_semihost:
                            self.target.resume()
                            continue

                    logging.debug("state halted")
                    val = self.target.getTResponse()
                    break
            except Exception as e:
                try:
                    self.target.halt()
                except:
                    pass
                traceback.print_exc()
                logging.debug("Target is unavailable temporarily.")
                val = "S%02x" % self.target.getSignalValue()
                break

        return self.createRSPPacket(val)

    def step(self, data):
        addr = self._get_resume_step_addr(data)
        logging.debug("GDB step: %s", data)
        self.target.step(not self.step_into_interrupt)
        return self.createRSPPacket(self.target.getTResponse())

    def halt(self):
        self.target.halt()
        return self.createRSPPacket(self.target.getTResponse())

    def sendStopNotification(self, forceSignal=None):
        data = self.target.getTResponse(forceSignal=forceSignal)
        packet = "%Stop:" + data + "#" + checksum(data)
        self.packet_io.send(packet)

    def vCommand(self, data):
        cmd = data.split("#")[0]
        logging.debug("GDB vCommand: %s", cmd)

        # Flash command.
        if cmd.startswith("Flash"):
            return self.flashOp(data)

        # vCont capabilities query.
        elif "Cont?" == cmd:
            return self.createRSPPacket("vCont;c;C;s;S;t")

        # vCont, thread action command.
        elif cmd.startswith("Cont"):
            return self.vCont(cmd)

        # vStopped, part of thread stop state notification sequence.
        elif "Stopped" in cmd:
            # Because we only support one thread for now, we can just reply OK to vStopped.
            return self.createRSPPacket("OK")

        return self.createRSPPacket("")

    # Example: $vCont;s:1;c#c1
    def vCont(self, cmd):
        ops = cmd.split(";")[1:]  # split and remove 'Cont' from list
        if not ops:
            return self.createRSPPacket("OK")

        thread_actions = {1: None}  # our only thread
        default_action = None
        for op in ops:
            args = op.split(":")
            action = args[0]
            if len(args) > 1:
                thread_id = args[1]
                if thread_id == "-1" or thread_id == "0":
                    thread_id = "1"
                thread_id = int(thread_id, base=16)
                thread_actions[thread_id] = action
            else:
                default_action = action

        logging.debug("thread_actions=%s; default_action=%s", repr(thread_actions), default_action)

        # Only thread 1 is supported at the moment.
        if thread_actions[1] is None:
            if default_action is None:
                return self.createRSPPacket("E01")
            thread_actions[1] = default_action

        if thread_actions[1] in ("c", "C"):
            if self.non_stop:
                self.target.resume()
                self.is_target_running = True
                return self.createRSPPacket("OK")
            else:
                return self.resume(None)
        elif thread_actions[1] in ("s", "S"):
            if self.non_stop:
                self.target.step(not self.step_into_interrupt)
                self.packet_io.send(self.createRSPPacket("OK"))
                self.sendStopNotification()
                return None
            else:
                return self.step(None)
        elif thread_actions[1] == "t":
            # Must ignore t command in all-stop mode.
            if not self.non_stop:
                return self.createRSPPacket("")
            self.packet_io.send(self.createRSPPacket("OK"))
            self.target.halt()
            self.is_target_running = False
            self.sendStopNotification(forceSignal=0)

    def flashOp(self, data):
        ops = data.split(":")[0]
        logging.debug("flash op: %s", ops)

        if ops == "FlashErase":
            return self.createRSPPacket("OK")

        elif ops == "FlashWrite":
            write_addr = int(data.split(":")[1], 16)
            logging.debug("flash write addr: 0x%x", write_addr)
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ":":
                    second_colon += 1
                idx_begin += 1

            # Get flash builder if there isn't one already
            if self.flashBuilder == None:
                self.flashBuilder = self.flash.getFlashBuilder()

            # Add data to flash builder
            self.flashBuilder.addData(write_addr, self.unescape(data[idx_begin : len(data) - 3]))

            return self.createRSPPacket("OK")

        # we need to flash everything
        elif "FlashDone" in ops:

            def print_progress(progress):
                # Reset state on 0.0
                if progress == 0.0:
                    print_progress.done = False

                # print progress bar
                if not print_progress.done:
                    sys.stdout.write("\r")
                    i = int(progress * 20.0)
                    sys.stdout.write("[%-20s] %3d%%" % ("=" * i, round(progress * 100)))
                    sys.stdout.flush()

                # Finish on 1.0
                if progress >= 1.0:
                    if not print_progress.done:
                        print_progress.done = True
                        sys.stdout.write("\r\n")

            if self.hide_programming_progress:
                progress_cb = None
            else:
                progress_cb = print_progress

            self.flashBuilder.program(
                chip_erase=self.chip_erase, progress_cb=progress_cb, fast_verify=self.fast_program
            )

            # Set flash builder to None so that on the next flash command a new
            # object is used.
            self.flashBuilder = None

            return self.createRSPPacket("OK")

        return None

    def unescape(self, data):
        data_idx = 0

        # unpack the data into binary array
        str_unpack = str(len(data)) + "B"
        data = unpack(str_unpack, data)
        data = list(data)

        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7D:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1

        return data

    def getMemory(self, data):
        split = data.split(",")
        addr = int(split[0], 16)
        length = split[1].split("#")[0]
        length = int(length, 16)

        if LOG_MEM:
            logging.debug("GDB getMem: addr=%x len=%x", addr, length)

        try:
            val = ""
            mem = self.target.readBlockMemoryUnaligned8(addr, length)
            # Flush so an exception is thrown now if invalid memory was accesses
            self.target.flush()
            for x in mem:
                if x >= 0x10:
                    val += hex(x)[2:4]
                else:
                    val += "0" + hex(x)[2:3]
        except DAPAccess.TransferError:
            logging.debug("getMemory failed at 0x%x" % addr)
            val = "E01"  # EPERM
        return self.createRSPPacket(val)

    def writeMemoryHex(self, data):
        split = data.split(",")
        addr = int(split[0], 16)

        split = split[1].split(":")
        length = int(split[0], 16)

        split = split[1].split("#")
        data = hexToByteList(split[0])

        if LOG_MEM:
            logging.debug("GDB writeMemHex: addr=%x len=%x", addr, length)

        try:
            if length > 0:
                self.target.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target.flush()
            resp = "OK"
        except DAPAccess.TransferError:
            logging.debug("writeMemory failed at 0x%x" % addr)
            resp = "E01"  # EPERM

        return self.createRSPPacket(resp)

    def writeMemory(self, data):
        split = data.split(",")
        addr = int(split[0], 16)
        length = int(split[1].split(":")[0], 16)

        if LOG_MEM:
            logging.debug("GDB writeMem: addr=%x len=%x", addr, length)

        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ":":
                idx_begin += 1
                break
            idx_begin += 1

        data = data[idx_begin : len(data) - 3]
        data = self.unescape(data)

        try:
            if length > 0:
                self.target.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target.flush()
            resp = "OK"
        except DAPAccess.TransferError:
            logging.debug("writeMemory failed at 0x%x" % addr)
            resp = "E01"  # EPERM

        return self.createRSPPacket(resp)

    def readRegister(self, which):
        return self.createRSPPacket(self.target.gdbGetRegister(which))

    def writeRegister(self, data):
        reg = int(data.split("=")[0], 16)
        val = data.split("=")[1].split("#")[0]
        self.target.setRegister(reg, val)
        return self.createRSPPacket("OK")

    def getRegisters(self):
        return self.createRSPPacket(self.target.getRegisterContext())

    def setRegisters(self, data):
        self.target.setRegisterContext(data)
        return self.createRSPPacket("OK")

    def handleQuery(self, msg):
        query = msg.split(":")
        logging.debug("GDB received query: %s", query)

        if query is None:
            logging.error("GDB received query packet malformed")
            return None

        if query[0] == "Supported":
            # Save features sent by gdb.
            self.gdb_features = query[1].split(";")

            # Build our list of features.
            features = ["qXfer:features:read+", "QStartNoAckMode+", "qXfer:threads:read+", "QNonStop+"]
            features.append("PacketSize=" + hex(self.packet_size)[2:])
            if self.target.getMemoryMapXML() is not None:
                features.append("qXfer:memory-map:read+")
            resp = ";".join(features)
            return self.createRSPPacket(resp)

        elif query[0] == "Xfer":

            if query[1] == "features" and query[2] == "read" and query[3] == "target.xml":
                data = query[4].split(",")
                resp = self.handleQueryXML("read_feature", int(data[0], 16), int(data[1].split("#")[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == "memory-map" and query[2] == "read":
                data = query[4].split(",")
                resp = self.handleQueryXML("memory_map", int(data[0], 16), int(data[1].split("#")[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == "threads" and query[2] == "read":
                data = query[4].split(",")
                resp = self.handleQueryXML("threads", int(data[0], 16), int(data[1].split("#")[0], 16))
                return self.createRSPPacket(resp)

            else:
                logging.debug("Unsupported qXfer request: %s:%s:%s:%s", query[1], query[2], query[3], query[4])
                return None

        elif query[0].startswith("C"):
            return self.createRSPPacket("QC1")

        elif query[0].find("Attached") != -1:
            return self.createRSPPacket("1")

        elif query[0].find("TStatus") != -1:
            return self.createRSPPacket("")

        elif query[0].find("Tf") != -1:
            return self.createRSPPacket("")

        elif "Offsets" in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)

        elif "Symbol" in query[0]:
            resp = "OK"
            return self.createRSPPacket(resp)

        elif query[0].startswith("Rcmd,"):
            cmd = hexDecode(query[0][5:].split("#")[0])
            return self.handleRemoteCommand(cmd)

        else:
            return self.createRSPPacket("")

    # TODO rewrite the remote command handler
    def handleRemoteCommand(self, cmd):
        logging.debug("Remote command: %s", cmd)

        safecmd = {
            "init": ["Init reset sequence", 0x1],
            "reset": ["Reset and halt the target", 0x2],
            "halt": ["Halt target", 0x4],
            # 'resume': ['Resume target', 0x8],
            "help": ["Display this help", 0x80],
        }

        resp = "OK"
        if cmd == "help":
            resp = "".join(["%s\t%s\n" % (k, v[0]) for k, v in safecmd.items()])
            resp = hexEncode(resp)
        elif cmd.startswith("arm semihosting"):
            self.enable_semihosting = "enable" in cmd
            logging.info("Semihosting %s", ("enabled" if self.enable_semihosting else "disabled"))
        else:
            resultMask = 0x00
            cmdList = cmd.split()
            if cmdList[0] == "help":
                # a 'help' is only valid as the first cmd, and only
                # gives info on the second cmd if it is valid
                resultMask |= 0x80
                del cmdList[0]

            for cmd_sub in cmdList:
                if cmd_sub not in safecmd:
                    logging.warning("Invalid mon command '%s'", cmd_sub)
                    resp = 'Invalid Command: "%s"\n' % cmd_sub
                    resp = hexEncode(resp)
                    return self.createRSPPacket(resp)
                elif resultMask == 0x80:
                    # if the first command was a 'help', we only need
                    # to return info about the first cmd after it
                    resp = hexEncode(safecmd[cmd_sub][0] + "\n")
                    return self.createRSPPacket(resp)
                resultMask |= safecmd[cmd_sub][1]

            # Run cmds in proper order
            if resultMask & 0x1:
                self.target.init()
            if (resultMask & 0x6) == 0x6:
                self.target.resetStopOnReset()
            elif resultMask & 0x2:
                # on 'reset' still do a reset halt
                self.target.resetStopOnReset()
                # self.target.reset()
            elif resultMask & 0x4:
                self.target.halt()
            # if resultMask & 0x8:
            #     self.target.resume()

        return self.createRSPPacket(resp)

    def handleGeneralSet(self, msg):
        feature = msg.split("#")[0]
        logging.debug("GDB general set: %s", feature)

        if feature == "StartNoAckMode":
            # Disable acks after the reply and ack.
            self.packet_io.set_send_acks(False)
            return self.createRSPPacket("OK")

        elif feature.startswith("NonStop"):
            enable = feature.split(":")[1]
            self.non_stop = enable == "1"
            return self.createRSPPacket("OK")

        else:
            return self.createRSPPacket("")

    def handleQueryXML(self, query, offset, size):
        logging.debug("GDB query %s: offset: %s, size: %s", query, offset, size)
        xml = ""
        if query == "memory_map":
            xml = self.target.getMemoryMapXML()
        elif query == "read_feature":
            xml = self.target.getTargetXML()
        elif query == "threads":
            xml = self.target.getThreadsXML()
        else:
            raise RuntimeError("Invalid XML query (%s)" % query)

        size_xml = len(xml)

        prefix = "m"

        if offset > size_xml:
            logging.error("GDB: xml offset > size for %s!", query)
            return

        if size > (self.packet_size - 4):
            size = self.packet_size - 4

        nbBytesAvailable = size_xml - offset

        if size > nbBytesAvailable:
            prefix = "l"
            size = nbBytesAvailable

        resp = prefix + xml[offset : offset + size]

        return resp

    def createRSPPacket(self, data):
        resp = "$" + data + "#" + checksum(data)
        return resp

    def syscall(self, op):
        logging.debug("GDB server syscall: %s", op)
        request = self.createRSPPacket("F" + op)
        self.packet_io.send(request)

        while not self.packet_io.interrupt_event.is_set():
            # Read a packet.
            packet = self.packet_io.receive(False)
            if packet is None:
                sleep(0.1)
                continue

            # Check for file I/O response.
            if packet[0] == "$" and packet[1] == "F":
                logging.debug("Syscall: got syscall response " + packet)
                args = packet[2 : packet.index("#")].split(",")
                result = int(args[0], base=16)
                errno = int(args[1], base=16) if len(args) > 1 else 0
                ctrl_c = args[2] if len(args) > 2 else ""
                if ctrl_c == "C":
                    self.packet_io.interrupt_event.set()
                    self.packet_io.drop_reply = True
                return result, errno

            # decode and prepare resp
            resp, detach = self.handleMsg(packet)

            if resp is not None:
                # send resp
                self.packet_io.send(resp)

            if detach:
                self.detach_event.set()
                logging.warning("GDB server received detach request while waiting for file I/O completion")
                break

        return -1, 0
Пример #7
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """
    def __init__(self, board, port_urlWSS, options = {}):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.break_at_hardfault = bool(options.get('break_at_hardfault', True))
        self.board.target.setVectorCatchFault(self.break_at_hardfault)
        self.break_on_reset = options.get('break_on_reset', False)
        self.board.target.setVectorCatchReset(self.break_on_reset)
        self.step_into_interrupt = options.get('step_into_interrupt', False)
        self.persist = options.get('persist', False)
        self.packet_size = 2048
        self.flashData = list()
        self.flashOffset = None
        self.conn = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        self.quit = False
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)
        self.setDaemon(True)
        self.start()
    
    def restart(self):
        if self.isAlive():
            self.detach_event.set()
    
    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            while self.isAlive():
                pass
            logging.info("GDB server thread killed")
        self.board.uninit()
        
    def setBoard(self, board, stop = True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return
        
    def run(self):
        self.timeOfLastPacket = time()
        while True:
            new_command = False
            data = ""
            logging.info('GDB server started at port:%d',self.port)
            
            self.shutdown_event.clear()
            self.detach_event.clear()
                
            while not self.shutdown_event.isSet() and not self.detach_event.isSet():
                connected = self.abstract_socket.connect()
                if connected != None:
                    break
            
            if self.shutdown_event.isSet():
                return
            
            if self.detach_event.isSet():
                continue
            
            logging.info("One client connected!")
            
            while True:
                
                if self.shutdown_event.isSet():
                    return
            
                if self.detach_event.isSet():
                    continue
                
                # read command
                while True:
                    if (new_command == True):
                        new_command = False
                        break

                    # Reduce CPU usage by sleep()ing once we know that the
                    # debugger doesn't have a queue of commands that we should
                    # execute as quickly as possible.
                    if time() - self.timeOfLastPacket > 0.5:
                        sleep(0.1)
                    try:
                        if self.shutdown_event.isSet() or self.detach_event.isSet():
                            break
                        self.abstract_socket.setBlocking(0)
                        data += self.abstract_socket.read().decode()
                        if data.index("$") >= 0 and data.index("#") >= 0:
                            break
                    except (ValueError, socket.error):
                        pass
                
                if self.shutdown_event.isSet():
                    return
            
                if self.detach_event.isSet():
                    continue
                
                self.abstract_socket.setBlocking(1)
                    
                data = data[data.index("$"):]
                
                self.lock.acquire()
            
                if len(data) != 0:
                    # decode and prepare resp
                    [resp, ack, detach] = self.handleMsg(data)
            
                    if resp is not None:
                        # ack
                        if ack:
                            resp = "+" + resp
                        # send resp
                        self.abstract_socket.write(resp.encode())
                        # wait a '+' from the client
                        try:
                            data = self.abstract_socket.read().decode()
                            if data[0] != '+':
                                logging.debug('gdb client has not ack!')
                            else:
                                logging.debug('gdb client has ack!')
                            if data.index("$") >= 0 and data.index("#") >= 0:
                                new_command = True
                        except:
                            pass
                        
                    if detach:
                        self.abstract_socket.close()
                        self.lock.release()
                        if self.persist:
                            break
                        else:
                            return

                    self.timeOfLastPacket = time()

                self.lock.release()
        
        
    def handleMsg(self, msg):
        
        if msg[0] != '$':
            logging.debug('msg ignored: first char != $')
            return None, 0, 0
        
        #logging.debug('-->>>>>>>>>>>> GDB rsp packet: %s', msg)
        
        # query command
        if msg[1] == 'q':
            return self.handleQuery(msg[2:]), 1, 0
            
        elif msg[1] == 'H':
            return self.createRSPPacket(''), 1, 0
        
        elif msg[1] == '?':
            return self.createRSPPacket(self.target.getTResponse()), 1, 0

        elif msg[1] == 'g':
            return self.getRegisters(), 1, 0

        elif msg[1] == 'G':
            return self.setRegisters(msg[2:]), 1, 0

        elif msg[1] == 'P':
            return self.writeRegister(msg[2:]), 1, 0

        elif msg[1] == 'm':
            return self.getMemory(msg[2:]), 1, 0
        
        elif msg[1] == 'X':
            return self.writeMemory(msg[2:]), 1, 0
        
        elif msg[1] == 'v':
            return self.flashOp(msg[2:]), 1, 0
        
        # we don't send immediately the response for C and S commands
        elif msg[1] == 'C' or msg[1] == 'c':
            return self.resume()
        
        elif msg[1] == 'S' or msg[1] == 's':
            return self.step()
        
        elif msg[1] == 'Z' or msg[1] == 'z':
            return self.breakpoint(msg[1:]), 1, 0
        
        elif msg[1] == 'D':
            return self.detach(msg[1:]), 1, 1
        
        elif msg[1] == 'k':
            return self.kill(), 1, 1
        
        else:
            logging.error("Unknown RSP packet: %s", msg)
            return self.createRSPPacket(""), 1, 0
        
    def detach(self, data):
        resp = "OK"
        return self.createRSPPacket(resp)
    
    def kill(self):
        # Keep target halted and leave vector catches if in persistent mode.
        if not self.persist:
            self.board.target.setVectorCatchFault(False)
            self.board.target.setVectorCatchReset(False)
            self.board.target.resume()
        return self.createRSPPacket("")
        
    def breakpoint(self, data):
        # handle breakpoint/watchpoint commands
        split = data.split('#')[0].split(',')
        addr = int(split[1], 16)

        # handle hardware breakpoint Z1/z1
        # and software breakpoint Z0/z0
        if data[1] == '1' or data[1] == '0':
            #TODO - add support for real software breakpoints
            if data[0] == 'Z':
                if self.target.setBreakpoint(addr) == False:
                    return self.createRSPPacket('E01') #EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware watchpoint Z2/z2/Z3/z3/Z4/z4
        if data[1] == '2':
            # Write-only watch
            watchpoint_type = WATCHPOINT_WRITE
        elif data[1] == '3':
            # Read-only watch
            watchpoint_type = WATCHPOINT_READ
        elif data[1] == '4':
            # Read-Write watch
            watchpoint_type = WATCHPOINT_READ_WRITE
        else:
            return self.createRSPPacket('E01') #EPERM
        size = int(split[2], 16)
        if data[0] == 'Z':
            if self.target.setWatchpoint(addr, size, watchpoint_type) == False:
                return self.createRSPPacket('E01') #EPERM
        else:
            self.target.removeWatchpoint(addr, size, watchpoint_type)
        return self.createRSPPacket("OK")

    def resume(self):
        self.ack()
        self.abstract_socket.setBlocking(0)

        self.target.resume()
        
        val = ''

        self.timeOfLastPacket = time()
        while True:
            if self.shutdown_event.isSet():
                return self.createRSPPacket(val), 0, 0

            # Introduce a delay between non-blocking socket reads once we know
            # that the CPU isn't going to halt quickly.
            if time() - self.timeOfLastPacket > 0.5:
                sleep(0.1)
            try:
                data = self.abstract_socket.read().decode()
                if (data[0] == '\x03'):
                    self.target.halt()
                    val = self.target.getTResponse(True)
                    logging.debug("receive CTRL-C")
                    break
            except:
                pass

            try:
                if self.target.getState() == TARGET_HALTED:
                    logging.debug("state halted")
                    val = self.target.getTResponse()
                    break
            except:
                logging.debug('Target is unavailable temporary.')

        self.abstract_socket.setBlocking(1)
        return self.createRSPPacket(val), 0, 0

    def step(self):
        self.ack()
        self.target.step(not self.step_into_interrupt)
        return self.createRSPPacket(self.target.getTResponse()), 0, 0

    def halt(self):
        self.ack()
        self.target.halt()
        return self.createRSPPacket(self.target.getTResponse()), 0, 0

    def flashOp(self, data):
        ops = data.split(':')[0]
        logging.debug("flash op: %s", ops)
        
        if ops == 'FlashErase':
            return self.createRSPPacket("OK")
        
        elif ops == 'FlashWrite':
            write_addr = int(data.split(':')[1], 16)
            logging.debug("flash write addr: 0x%x", write_addr)
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ':':
                    second_colon += 1
                idx_begin += 1

            # determine the address to start flashing
            if self.flashOffset == None:
                # flash offset must be a multiple of the page size
                self.flashOffset = write_addr - ( write_addr % self.flash.page_size )

            # if there's gap between sections, fill it
            flash_watermark = len(self.flashData) + self.flashOffset
            pad_size = write_addr - flash_watermark
            if pad_size > 0:
                self.flashData += [0xFF] * pad_size
            
            # append the new data if it doesn't overlap existing data
            if write_addr >= flash_watermark:
                self.flashData += self.unescape(data[idx_begin:len(data) - 3])
            else:
                logging.error("Invalid FlashWrite address %d overlaps current data of size %d", write_addr, flash_watermark)
                
            return self.createRSPPacket("OK")
        
        # we need to flash everything
        elif 'FlashDone' in ops :
            bytes_to_be_written = len(self.flashData)
            flashPtr = self.flashOffset
            
            self.flash.init()

            # use mass erase if the address starts at 0
            mass_erase = flashPtr == 0
            if mass_erase:
                logging.debug("Erasing entire flash")
                self.flash.eraseAll()

            while len(self.flashData) > 0:
                size_to_write = min(self.flash.page_size, len(self.flashData))
                
                #Erase Page if flash has not been erased
                if not mass_erase:
                    logging.debug("Erasing page 0x%x", flashPtr)
                    self.flash.erasePage(flashPtr)

                #ProgramPage
                self.flash.programPage(flashPtr, self.flashData[:size_to_write])
                flashPtr += size_to_write

                self.flashData = self.flashData[size_to_write:]

                # print progress bar
                sys.stdout.write('\r')
                i = int((float(flashPtr - self.flashOffset)/float(bytes_to_be_written))*20.0)
                # the exact output you're looking for:
                sys.stdout.write("[%-20s] %d%%" % ('='*i, 5*i))
                sys.stdout.flush()
                
            sys.stdout.write("\n\r")
            
            self.flashData = []
            self.flashOffset = None

            # reset and stop on reset handler
            self.target.resetStopOnReset()
            
            return self.createRSPPacket("OK")
        
        elif 'Cont' in ops:
            if 'Cont?' in ops:
                return self.createRSPPacket("vCont;c;s;t")
                
        return None
    
    def unescape(self, data):
        data_idx = 0
    
        # unpack the data into binary array
        str_unpack = str(len(data)) + 'B'
        data = unpack(str_unpack, data)
        data = list(data)
    
        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7d:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1
            
        return data
            
        
    def getMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = split[1]
        length = int(length[:len(length)-3],16)
        
        try:
            val = ''
            mem = self.target.readBlockMemoryUnaligned8(addr, length)
            for x in mem:
                if x >= 0x10:
                    val += hex(x)[2:4]
                else:
                    val += '0' + hex(x)[2:3]
        except TransferError:
            logging.debug("getMemory failed at 0x%x" % addr)
            val = 'E01' #EPERM
        return self.createRSPPacket(val)
    
    def writeMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = int(split[1].split(':')[0], 16)
        
        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ':':
                idx_begin += 1
                break
            idx_begin += 1
        
        data = data[idx_begin:len(data) - 3]
        data = self.unescape(data)
        
        try:
            if length > 0:
                self.target.writeBlockMemoryUnaligned8(addr, data)
            resp = "OK"
        except TransferError:
            logging.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01' #EPERM
        
        return self.createRSPPacket(resp)

    def writeRegister(self, data):
        reg = int(data.split('=')[0], 16)
        val = data.split('=')[1].split('#')[0]
        self.target.setRegister(reg, val)
        return self.createRSPPacket("OK")

    def getRegisters(self):
        return self.createRSPPacket(self.target.getRegisterContext())

    def setRegisters(self, data):
        self.target.setRegisterContext(data)
        return self.createRSPPacket("OK")

    def handleQuery(self, msg):
        query = msg.split(':')
        logging.debug('GDB received query: %s', query)

        if query is None:
            logging.error('GDB received query packet malformed')
            return None
        
        if query[0] == 'Supported':
            resp = "qXfer:memory-map:read+;qXfer:features:read+;PacketSize="
            resp += hex(self.packet_size)[2:]
            return self.createRSPPacket(resp)
            
        elif query[0] == 'Xfer':
            
            if query[1] == 'features' and query[2] == 'read' and \
               query[3] == 'target.xml':
                data = query[4].split(',')
                resp = self.handleQueryXML('read_feature', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)
            
            elif query[1] == 'memory-map' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('memory_map', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)
                
            else:
                return None
            
        elif query[0] == 'C#b4':
            return self.createRSPPacket("")
        
        elif query[0].find('Attached') != -1:
            return self.createRSPPacket("1")
        
        elif query[0].find('TStatus') != -1:
            return self.createRSPPacket("")
        
        elif query[0].find('Tf') != -1:
            return self.createRSPPacket("")
        
        elif 'Offsets' in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)
        
        elif 'Symbol' in query[0]:
            resp = "OK"
            return self.createRSPPacket(resp)

        elif query[0].startswith('Rcmd,'):
            cmd = self.hexDecode(query[0][5:].split('#')[0])
            logging.debug('Remote command: %s', cmd)

            safecmd = {
                'reset' : ['Reset target', 0x1],
                'halt'  : ['Halt target', 0x2],
                'resume': ['Resume target', 0x4],
                'help'  : ['Display this help', 0x80],
            }
            resultMask = 0x00
            if cmd == 'help':
                resp = ''
                for k,v in safecmd.items():
                    resp += '%s\t%s\n' % (k,v[0])
                resp = self.hexEncode(resp)
            else:
                cmdList = cmd.split(' ')
                #check whether all the cmds is valid cmd for monitor
                for cmd_sub in cmdList:
                    if not cmd_sub in safecmd:
                        #error cmd for monitor
                        logging.warning("Invalid mon command '%s'", cmd)
                        resp = 'Invalid Command: "%s"\n' % cmd
                        resp = self.hexEncode(resp)
                        return self.createRSPPacket(resp)
                    else:
                        resultMask = resultMask | safecmd[cmd_sub][1]
                #if it's a single cmd, just launch it!
                if len(cmdList) == 1:
                    tmp = eval ('self.target.%s()' % cmd_sub)
                    logging.debug(tmp)
                    resp = "OK"
                else:
                    #10000001 for help reset, so output reset cmd help information
                    if resultMask == 0x81:
                        resp = 'Reset the target\n'
                        resp = self.hexEncode(resp)
                    #10000010 for help halt, so output halt cmd help information
                    elif resultMask == 0x82:
                        resp = 'Halt the target\n'
                        resp = self.hexEncode(resp)
                    #10000100 for help resume, so output resume cmd help information
                    elif resultMask == 0x84:
                        resp = 'Resume the target\n'
                        resp = self.hexEncode(resp)
                    #11 for reset halt cmd, so launch self.target.resetStopOnReset()
                    elif resultMask == 0x3:
                        resp = "OK"
                        self.target.resetStopOnReset()
                    #111 for reset halt resume cmd, so launch self.target.resetStopOnReset() and self.target.resume()
                    elif resultMask == 0x7:
                        resp = "OK"
                        self.target.resetStopOnReset()
                        self.target.resume()
                    else:
                        logging.warning("Invalid mon command '%s'", cmd)
                        resp = 'Invalid Command: "%s"\n' % cmd
                        resp = self.hexEncode(resp)
            return self.createRSPPacket(resp)

        else:
            return self.createRSPPacket("")
            
    def handleQueryXML(self, query, offset, size):
        logging.debug('GDB query %s: offset: %s, size: %s', query, offset, size)
        xml = ''
        if query == 'memory_map':
            xml = self.target.memoryMapXML
        elif query == 'read_feature':
            xml = self.target.getTargetXML()

        size_xml = len(xml)
        
        prefix = 'm'
        
        if offset > size_xml:
            logging.error('GDB: offset target.xml > size!')
            return
        
        if size > (self.packet_size - 4):
            size = self.packet_size - 4
        
        nbBytesAvailable = size_xml - offset
        
        if size > nbBytesAvailable:
            prefix = 'l'
            size = nbBytesAvailable
        
        resp = prefix + xml[offset:offset + size]
        
        return resp
            
            
    def createRSPPacket(self, data):
        resp = '$' + data + '#'
        
        c = 0
        checksum = 0
        for c in data:
            checksum += ord(c)
        checksum = checksum % 256
        checksum = hex(checksum)

        if int(checksum[2:], 16) < 0x10:
            resp += '0'
        resp += checksum[2:]
        
        #logging.debug('--<<<<<<<<<<<< GDB rsp packet: %s', resp)
        return resp
    
    def ack(self):
        self.abstract_socket.write(b"+")

    def hexDecode(self, cmd):
        return ''.join([ chr(int(cmd[i:i+2], 16)) for i in range(0, len(cmd), 2)])

    def hexEncode(self, string):
        return ''.join(['%02x' % ord(i) for i in string])
Пример #8
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """
    def __init__(self, board, port_urlWSS, options = {}):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.break_at_hardfault = bool(options.get('break_at_hardfault', True))
        self.board.target.setVectorCatchFault(self.break_at_hardfault)
        self.break_on_reset = options.get('break_on_reset', False)
        self.board.target.setVectorCatchReset(self.break_on_reset)
        self.step_into_interrupt = options.get('step_into_interrupt', False)
        self.persist = options.get('persist', False)
        self.soft_bkpt_as_hard = options.get('soft_bkpt_as_hard', False)
        self.chip_erase = options.get('chip_erase', None)
        self.hide_programming_progress = options.get('hide_programming_progress', False)
        self.fast_program = options.get('fast_program', False)
        self.packet_size = 2048
        self.send_acks = True
        self.clear_send_acks = False
        self.gdb_features = []
        self.flashBuilder = None
        self.conn = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        self.quit = False
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)
        self.setDaemon(True)
        self.start()
    
    def restart(self):
        if self.isAlive():
            self.detach_event.set()
    
    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            while self.isAlive():
                pass
            logging.info("GDB server thread killed")
        self.board.uninit()
        
    def setBoard(self, board, stop = True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return
        
    def run(self):
        self.timeOfLastPacket = time()
        while True:
            new_command = False
            data = ""
            logging.info('GDB server started at port:%d',self.port)
            
            self.shutdown_event.clear()
            self.detach_event.clear()
                
            while not self.shutdown_event.isSet() and not self.detach_event.isSet():
                connected = self.abstract_socket.connect()
                if connected != None:
                    break
            
            if self.shutdown_event.isSet():
                return
            
            if self.detach_event.isSet():
                continue
            
            logging.info("One client connected!")
            
            while True:
                
                if self.shutdown_event.isSet():
                    return
            
                if self.detach_event.isSet():
                    continue
                
                # read command
                while True:
                    if (new_command == True):
                        new_command = False
                        break

                    # Reduce CPU usage by sleep()ing once we know that the
                    # debugger doesn't have a queue of commands that we should
                    # execute as quickly as possible.
                    if time() - self.timeOfLastPacket > 0.5:
                        sleep(0.1)
                    try:
                        if self.shutdown_event.isSet() or self.detach_event.isSet():
                            break
                        self.abstract_socket.setBlocking(0)
                        data += self.abstract_socket.read()
                        if data.index("$") >= 0 and data.index("#") >= 0:
                            break
                    except (ValueError, socket.error):
                        pass
                
                if self.shutdown_event.isSet():
                    return
            
                if self.detach_event.isSet():
                    continue
                
                self.abstract_socket.setBlocking(1)
                    
                data = data[data.index("$"):]
                
                self.lock.acquire()
            
                if len(data) != 0:
                    # decode and prepare resp
                    [resp, ack, detach] = self.handleMsg(data)

                    # Clear out data
                    data = ""
            
                    if resp is not None:
                        # ack
                        if ack and self.send_acks:
                            resp = "+" + resp
                        # send resp
                        self.abstract_socket.write(resp)
                        if self.send_acks:
                            # wait a '+' from the client
                            try:
                                data = self.abstract_socket.read()
                                if LOG_ACK:
                                    if data[0] != '+':
                                        logging.debug('gdb client has not ack!')
                                    else:
                                        logging.debug('gdb client has ack!')
                                if self.clear_send_acks:
                                    self.send_acks = False
                                if data.index("$") >= 0 and data.index("#") >= 0:
                                    new_command = True
                            except:
                                pass

                    if detach:
                        self.abstract_socket.close()
                        self.lock.release()
                        if self.persist:
                            break
                        else:
                            return

                    self.timeOfLastPacket = time()

                self.lock.release()
        
        
    def handleMsg(self, msg):
        
        if msg[0] != '$':
            logging.debug('msg ignored: first char != $')
            return None, 0, 0
        
        #logging.debug('-->>>>>>>>>>>> GDB rsp packet: %s', msg)
        
        # query command
        if msg[1] == '?':
            return self.createRSPPacket(self.target.getTResponse()), 1, 0

        # we don't send immediately the response for C and S commands
        elif msg[1] == 'C' or msg[1] == 'c':
            return self.resume()

        elif msg[1] == 'D':
            return self.detach(msg[1:]), 1, 1

        elif msg[1] == 'g':
            return self.getRegisters(), 1, 0

        elif msg[1] == 'G':
            return self.setRegisters(msg[2:]), 1, 0

        elif msg[1] == 'H':
            return self.createRSPPacket(''), 1, 0

        elif msg[1] == 'k':
            return self.kill(), 1, 1

        elif msg[1] == 'm':
            return self.getMemory(msg[2:]), 1, 0

        elif msg[1] == 'M': # write memory with hex data
            return self.writeMemoryHex(msg[2:]), 1, 0

        elif msg[1] == 'p':
            return self.readRegister(msg[2:]), 1, 0

        elif msg[1] == 'P':
            return self.writeRegister(msg[2:]), 1, 0

        elif msg[1] == 'q':
            return self.handleQuery(msg[2:]), 1, 0

        elif msg[1] == 'Q':
            return self.handleGeneralSet(msg[2:]), 1, 0

        elif msg[1] == 'S' or msg[1] == 's':
            return self.step()

        elif msg[1] == 'v':
            return self.flashOp(msg[2:]), 1, 0

        elif msg[1] == 'X': # write memory with binary data
            return self.writeMemory(msg[2:]), 1, 0

        elif msg[1] == 'Z' or msg[1] == 'z':
            return self.breakpoint(msg[1:]), 1, 0

        else:
            logging.error("Unknown RSP packet: %s", msg)
            return self.createRSPPacket(""), 1, 0
        
    def detach(self, data):
        logging.info("Client detached")
        resp = "OK"
        return self.createRSPPacket(resp)
    
    def kill(self):
        logging.debug("GDB kill")
        # Keep target halted and leave vector catches if in persistent mode.
        if not self.persist:
            self.board.target.setVectorCatchFault(False)
            self.board.target.setVectorCatchReset(False)
            self.board.target.resume()
        return self.createRSPPacket("")
        
    def breakpoint(self, data):
        # handle breakpoint/watchpoint commands
        split = data.split('#')[0].split(',')
        addr = int(split[1], 16)
        logging.debug("GDB breakpoint %d @ %x" % (int(data[1]), addr))

        if data[1] == '0' and not self.soft_bkpt_as_hard:   
            # Empty response indicating no support for software breakpoints
            return self.createRSPPacket("")

        # handle hardware breakpoint Z1/z1
        # and software breakpoint Z0/z0
        if data[1] == '1' or (self.soft_bkpt_as_hard and data[1] == '0'):
            if data[0] == 'Z':
                if self.target.setBreakpoint(addr) == False:
                    return self.createRSPPacket('E01') #EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware watchpoint Z2/z2/Z3/z3/Z4/z4
        if data[1] == '2':
            # Write-only watch
            watchpoint_type = WATCHPOINT_WRITE
        elif data[1] == '3':
            # Read-only watch
            watchpoint_type = WATCHPOINT_READ
        elif data[1] == '4':
            # Read-Write watch
            watchpoint_type = WATCHPOINT_READ_WRITE
        else:
            return self.createRSPPacket('E01') #EPERM
        size = int(split[2], 16)
        if data[0] == 'Z':
            if self.target.setWatchpoint(addr, size, watchpoint_type) == False:
                return self.createRSPPacket('E01') #EPERM
        else:
            self.target.removeWatchpoint(addr, size, watchpoint_type)
        return self.createRSPPacket("OK")

    def resume(self):
        self.ack()
        self.abstract_socket.setBlocking(0)

        self.target.resume()
        logging.debug("target resumed")

        val = ''

        self.timeOfLastPacket = time()
        while True:
            if self.shutdown_event.isSet():
                return self.createRSPPacket(val), 0, 0

            # Introduce a delay between non-blocking socket reads once we know
            # that the CPU isn't going to halt quickly.
            if time() - self.timeOfLastPacket > 0.5:
                sleep(0.1)
            try:
                data = self.abstract_socket.read()
                if (data[0] == '\x03'):
                    self.target.halt()
                    val = self.target.getTResponse(True)
                    logging.debug("receive CTRL-C")
                    break
            except:
                pass

            try:
                if self.target.getState() == TARGET_HALTED:
                    logging.debug("state halted")
                    val = self.target.getTResponse()
                    break
            except:
                logging.debug('Target is unavailable temporary.')

        self.abstract_socket.setBlocking(1)
        return self.createRSPPacket(val), 0, 0

    def step(self):
        self.ack()
        logging.debug("GDB step")
        self.target.step(not self.step_into_interrupt)
        return self.createRSPPacket(self.target.getTResponse()), 0, 0

    def halt(self):
        self.ack()
        self.target.halt()
        return self.createRSPPacket(self.target.getTResponse()), 0, 0

    def flashOp(self, data):
        ops = data.split(':')[0]
        logging.debug("flash op: %s", ops)
        
        if ops == 'FlashErase':
            return self.createRSPPacket("OK")
        
        elif ops == 'FlashWrite':
            write_addr = int(data.split(':')[1], 16)
            logging.debug("flash write addr: 0x%x", write_addr)
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ':':
                    second_colon += 1
                idx_begin += 1

            # Get flash builder if there isn't one already
            if self.flashBuilder == None:
                self.flashBuilder = self.flash.getFlashBuilder()

            # Add data to flash builder
            self.flashBuilder.addData(write_addr, self.unescape(data[idx_begin:len(data) - 3]))
            
                
            return self.createRSPPacket("OK")
        
        # we need to flash everything
        elif 'FlashDone' in ops :

            def print_progress(progress):
                # Reset state on 0.0
                if progress == 0.0:
                    print_progress.done = False
                
                # print progress bar
                if not print_progress.done:
                    sys.stdout.write('\r')
                    i = int(progress*20.0)
                    sys.stdout.write("[%-20s] %3d%%" % ('='*i, round(progress * 100)))
                    sys.stdout.flush()

                # Finish on 1.0
                if progress >= 1.0:
                    if not print_progress.done:
                        print_progress.done = True
                        sys.stdout.write("\r\n")

            if self.hide_programming_progress:
                progress_cb = None
            else:
                 progress_cb = print_progress

            self.flashBuilder.program(chip_erase = self.chip_erase, progress_cb=progress_cb, fast_verify=self.fast_program)

            # Set flash builder to None so that on the next flash command a new
            # object is used.
            self.flashBuilder = None

            return self.createRSPPacket("OK")
        
        elif 'Cont' in ops:
            if 'Cont?' in ops:
                return self.createRSPPacket("vCont;c;s;t")
                
        return None
    
    def unescape(self, data):
        data_idx = 0
    
        # unpack the data into binary array
        str_unpack = str(len(data)) + 'B'
        data = unpack(str_unpack, data)
        data = list(data)
    
        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7d:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1
            
        return data
            
        
    def getMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = split[1].split('#')[0]
        length = int(length,16)

        if LOG_MEM:
            logging.debug("GDB getMem: addr=%x len=%x", addr, length)

        try:
            val = ''
            mem = self.target.readBlockMemoryUnaligned8(addr, length)
            # Flush so an exception is thrown now if invalid memory was accesses
            self.target.flush()
            for x in mem:
                if x >= 0x10:
                    val += hex(x)[2:4]
                else:
                    val += '0' + hex(x)[2:3]
        except TransferError:
            logging.debug("getMemory failed at 0x%x" % addr)
            val = 'E01' #EPERM
        return self.createRSPPacket(val)

    def writeMemoryHex(self, data):
        split = data.split(',')
        addr = int(split[0], 16)

        split = split[1].split(':')
        length = int(split[0], 16)

        split = split[1].split('#')
        data = hexStringToIntList(split[0])

        if LOG_MEM:
            logging.debug("GDB writeMemHex: addr=%x len=%x", addr, length)

        try:
            if length > 0:
                self.target.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target.flush()
            resp = "OK"
        except TransferError:
            logging.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01' #EPERM

        return self.createRSPPacket(resp)

    def writeMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = int(split[1].split(':')[0], 16)

        if LOG_MEM:
            logging.debug("GDB writeMem: addr=%x len=%x", addr, length)

        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ':':
                idx_begin += 1
                break
            idx_begin += 1
        
        data = data[idx_begin:len(data) - 3]
        data = self.unescape(data)
        
        try:
            if length > 0:
                self.target.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target.flush()
            resp = "OK"
        except TransferError:
            logging.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01' #EPERM
        
        return self.createRSPPacket(resp)

    def readRegister(self, which):
        return self.createRSPPacket(self.target.gdbGetRegister(which))

    def writeRegister(self, data):
        reg = int(data.split('=')[0], 16)
        val = data.split('=')[1].split('#')[0]
        self.target.setRegister(reg, val)
        return self.createRSPPacket("OK")

    def getRegisters(self):
        return self.createRSPPacket(self.target.getRegisterContext())

    def setRegisters(self, data):
        self.target.setRegisterContext(data)
        return self.createRSPPacket("OK")

    def handleQuery(self, msg):
        query = msg.split(':')
        logging.debug('GDB received query: %s', query)

        if query is None:
            logging.error('GDB received query packet malformed')
            return None
        
        if query[0] == 'Supported':
            # Save features sent by gdb.
            self.gdb_features = query[1].split(';')

            # Build our list of features.
            features = ['qXfer:features:read+', 'QStartNoAckMode+']
            features.append('PacketSize=' + hex(self.packet_size)[2:])
            if hasattr(self.target, 'memoryMapXML'):
                features.append('qXfer:memory-map:read+')
            resp = ';'.join(features)
            return self.createRSPPacket(resp)
            
        elif query[0] == 'Xfer':
            
            if query[1] == 'features' and query[2] == 'read' and \
               query[3] == 'target.xml':
                data = query[4].split(',')
                resp = self.handleQueryXML('read_feature', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)
            
            elif query[1] == 'memory-map' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('memory_map', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)
                
            else:
                return None
            
        elif query[0] == 'C#b4':
            return self.createRSPPacket("")
        
        elif query[0].find('Attached') != -1:
            return self.createRSPPacket("1")
        
        elif query[0].find('TStatus') != -1:
            return self.createRSPPacket("")
        
        elif query[0].find('Tf') != -1:
            return self.createRSPPacket("")
        
        elif 'Offsets' in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)
        
        elif 'Symbol' in query[0]:
            resp = "OK"
            return self.createRSPPacket(resp)

        elif query[0].startswith('Rcmd,'):
            cmd = hexDecode(query[0][5:].split('#')[0])
            logging.debug('Remote command: %s', cmd)

            safecmd = {
                'reset' : ['Reset target', 0x1],
                'halt'  : ['Halt target', 0x2],
                'resume': ['Resume target', 0x4],
                'help'  : ['Display this help', 0x80],
            }
            resultMask = 0x00
            if cmd == 'help':
                resp = ''
                for k,v in safecmd.items():
                    resp += '%s\t%s\n' % (k,v[0])
                resp = hexEncode(resp)
            else:
                cmdList = cmd.split(' ')
                #check whether all the cmds is valid cmd for monitor
                for cmd_sub in cmdList:
                    if not cmd_sub in safecmd:
                        #error cmd for monitor
                        logging.warning("Invalid mon command '%s'", cmd)
                        resp = 'Invalid Command: "%s"\n' % cmd
                        resp = hexEncode(resp)
                        return self.createRSPPacket(resp)
                    else:
                        resultMask = resultMask | safecmd[cmd_sub][1]
                #if it's a single cmd, just launch it!
                if len(cmdList) == 1:
                    tmp = eval ('self.target.%s()' % cmd_sub)
                    logging.debug(tmp)
                    resp = "OK"
                else:
                    #10000001 for help reset, so output reset cmd help information
                    if resultMask == 0x81:
                        resp = 'Reset the target\n'
                        resp = hexEncode(resp)
                    #10000010 for help halt, so output halt cmd help information
                    elif resultMask == 0x82:
                        resp = 'Halt the target\n'
                        resp = hexEncode(resp)
                    #10000100 for help resume, so output resume cmd help information
                    elif resultMask == 0x84:
                        resp = 'Resume the target\n'
                        resp = hexEncode(resp)
                    #11 for reset halt cmd, so launch self.target.resetStopOnReset()
                    elif resultMask == 0x3:
                        resp = "OK"
                        self.target.resetStopOnReset()
                    #111 for reset halt resume cmd, so launch self.target.resetStopOnReset() and self.target.resume()
                    elif resultMask == 0x7:
                        resp = "OK"
                        self.target.resetStopOnReset()
                        self.target.resume()
                    else:
                        logging.warning("Invalid mon command '%s'", cmd)
                        resp = 'Invalid Command: "%s"\n' % cmd
                        resp = hexEncode(resp)

                if self.target.getState() != TARGET_HALTED:
                    logging.error("Remote command left target running!")
                    logging.error("Forcing target to halt")
                    self.target.halt()

            return self.createRSPPacket(resp)

        else:
            return self.createRSPPacket("")

    def handleGeneralSet(self, msg):
        logging.debug("GDB general set: %s", msg)
        feature = msg.split('#')[0]

        if feature == 'StartNoAckMode':
            # Disable acks after the reply and ack.
            self.clear_send_acks = True
            return self.createRSPPacket("OK")
        else:
            return self.createRSPPacket("")

    def handleQueryXML(self, query, offset, size):
        logging.debug('GDB query %s: offset: %s, size: %s', query, offset, size)
        xml = ''
        if query == 'memory_map':
            xml = self.target.memoryMapXML
        elif query == 'read_feature':
            xml = self.target.getTargetXML()

        size_xml = len(xml)
        
        prefix = 'm'
        
        if offset > size_xml:
            logging.error('GDB: offset target.xml > size!')
            return
        
        if size > (self.packet_size - 4):
            size = self.packet_size - 4
        
        nbBytesAvailable = size_xml - offset
        
        if size > nbBytesAvailable:
            prefix = 'l'
            size = nbBytesAvailable
        
        resp = prefix + xml[offset:offset + size]
        
        return resp
            
            
    def createRSPPacket(self, data):
        resp = '$' + data + '#'
        
        c = 0
        checksum = 0
        for c in data:
            checksum += ord(c)
        checksum = checksum % 256
        checksum = hex(checksum)

        if int(checksum[2:], 16) < 0x10:
            resp += '0'
        resp += checksum[2:]
        
        #logging.debug('--<<<<<<<<<<<< GDB rsp packet: %s', resp)
        return resp
    
    def ack(self):
        if self.send_acks:
            self.abstract_socket.write("+")
Пример #9
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """
    def __init__(self, board, port_urlWSS, options = {}):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.break_at_hardfault = bool(options.get('break_at_hardfault', True))
        self.board.target.setVectorCatchFault(self.break_at_hardfault)
        self.break_on_reset = options.get('break_on_reset', False)
        self.board.target.setVectorCatchReset(self.break_on_reset)
        self.step_into_interrupt = options.get('step_into_interrupt', False)
        self.persist = options.get('persist', False)
        self.soft_bkpt_as_hard = options.get('soft_bkpt_as_hard', False)
        self.chip_erase = options.get('chip_erase', None)
        self.hide_programming_progress = options.get('hide_programming_progress', False)
        self.fast_program = options.get('fast_program', False)
        self.packet_size = 2048
        self.packet_io = None
        self.gdb_features = []
        self.flashBuilder = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)
        self.setDaemon(True)
        self.start()

    def restart(self):
        if self.isAlive():
            self.detach_event.set()

    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            if self.packet_io:
                self.packet_io.stop()
            while self.isAlive():
                pass
            logging.info("GDB server thread killed")
        self.board.uninit()

    def setBoard(self, board, stop = True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return

    def run(self):
        while True:
            logging.info('GDB server started at port:%d', self.port)

            self.shutdown_event.clear()
            self.detach_event.clear()

            while not self.shutdown_event.isSet() and not self.detach_event.isSet():
                connected = self.abstract_socket.connect()
                if connected != None:
                    self.packet_io = GDBServerPacketIOThread(self.abstract_socket)
                    break

            if self.shutdown_event.isSet():
                return

            if self.detach_event.isSet():
                continue

            logging.info("One client connected!")

            while True:

                if self.shutdown_event.isSet():
                    return

                if self.detach_event.isSet():
                    break

                if self.packet_io.interrupt_event.isSet():
                    logging.debug("Got unexpected ctrl-c, ignoring")
                    self.packet_io.interrupt_event.clear()

                # read command
                try:
                    packet = self.packet_io.receive()
                except ConnectionClosedException:
                    break

                if self.shutdown_event.isSet():
                    return

                if self.detach_event.isSet():
                    break

                self.lock.acquire()

                if len(packet) != 0:
                    # decode and prepare resp
                    resp, detach = self.handleMsg(packet)

                    if resp is not None:
                        # send resp
                        self.packet_io.send(resp)

                    if detach:
                        self.abstract_socket.close()
                        self.packet_io.stop()
                        self.packet_io = None
                        self.lock.release()
                        if self.persist:
                            break
                        else:
                            return

                self.lock.release()

        if self.packet_io is not None:
            self.packet_io.stop()
            self.packet_io = None

    def handleMsg(self, msg):
        if msg[0] != '$':
            logging.debug('msg ignored: first char != $')
            return None, 0

        # query command
        if msg[1] == '?':
            return self.createRSPPacket(self.target.getTResponse()), 0

        # we don't send immediately the response for C and S commands
        elif msg[1] == 'C' or msg[1] == 'c':
            return self.resume()

        elif msg[1] == 'D':
            return self.detach(msg[1:]), 1

        elif msg[1] == 'g':
            return self.getRegisters(), 0

        elif msg[1] == 'G':
            return self.setRegisters(msg[2:]), 0

        elif msg[1] == 'H':
            return self.createRSPPacket(''), 0

        elif msg[1] == 'k':
            return self.kill(), 1

        elif msg[1] == 'm':
            return self.getMemory(msg[2:]), 0

        elif msg[1] == 'M': # write memory with hex data
            return self.writeMemoryHex(msg[2:]), 0

        elif msg[1] == 'p':
            return self.readRegister(msg[2:]), 0

        elif msg[1] == 'P':
            return self.writeRegister(msg[2:]), 0

        elif msg[1] == 'q':
            return self.handleQuery(msg[2:]), 0

        elif msg[1] == 'Q':
            return self.handleGeneralSet(msg[2:]), 0

        elif msg[1] == 'S' or msg[1] == 's':
            return self.step()

        elif msg[1] == 'v':
            return self.flashOp(msg[2:]), 0

        elif msg[1] == 'X': # write memory with binary data
            return self.writeMemory(msg[2:]), 0

        elif msg[1] == 'Z' or msg[1] == 'z':
            return self.breakpoint(msg[1:]), 0

        else:
            logging.error("Unknown RSP packet: %s", msg)
            return self.createRSPPacket(""), 0

    def detach(self, data):
        logging.info("Client detached")
        resp = "OK"
        return self.createRSPPacket(resp)

    def kill(self):
        logging.debug("GDB kill")
        # Keep target halted and leave vector catches if in persistent mode.
        if not self.persist:
            self.board.target.setVectorCatchFault(False)
            self.board.target.setVectorCatchReset(False)
            self.board.target.resume()
        return self.createRSPPacket("")

    def breakpoint(self, data):
        # handle breakpoint/watchpoint commands
        split = data.split('#')[0].split(',')
        addr = int(split[1], 16)
        logging.debug("GDB breakpoint %d @ %x" % (int(data[1]), addr))

        if data[1] == '0' and not self.soft_bkpt_as_hard:
            # Empty response indicating no support for software breakpoints
            return self.createRSPPacket("")

        # handle hardware breakpoint Z1/z1
        # and software breakpoint Z0/z0
        if data[1] == '1' or (self.soft_bkpt_as_hard and data[1] == '0'):
            if data[0] == 'Z':
                if self.target.setBreakpoint(addr) == False:
                    return self.createRSPPacket('E01') #EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware watchpoint Z2/z2/Z3/z3/Z4/z4
        if data[1] == '2':
            # Write-only watch
            watchpoint_type = WATCHPOINT_WRITE
        elif data[1] == '3':
            # Read-only watch
            watchpoint_type = WATCHPOINT_READ
        elif data[1] == '4':
            # Read-Write watch
            watchpoint_type = WATCHPOINT_READ_WRITE
        else:
            return self.createRSPPacket('E01') #EPERM
        size = int(split[2], 16)
        if data[0] == 'Z':
            if self.target.setWatchpoint(addr, size, watchpoint_type) == False:
                return self.createRSPPacket('E01') #EPERM
        else:
            self.target.removeWatchpoint(addr, size, watchpoint_type)
        return self.createRSPPacket("OK")

    def resume(self):
        self.target.resume()
        logging.debug("target resumed")

        val = ''

        while True:
            if self.shutdown_event.isSet():
                self.packet_io.interrupt_event.clear()
                return self.createRSPPacket(val), 0

            # Wait for a ctrl-c to be received.
            if self.packet_io.interrupt_event.wait(0.01):
                logging.debug("receive CTRL-C")
                self.packet_io.interrupt_event.clear()
                self.target.halt()
                val = self.target.getTResponse(True)
                break

            try:
                if self.target.getState() == TARGET_HALTED:
                    logging.debug("state halted")
                    val = self.target.getTResponse()
                    break
            except:
                logging.debug('Target is unavailable temporary.')

        return self.createRSPPacket(val), 0

    def step(self):
        logging.debug("GDB step")
        self.target.step(not self.step_into_interrupt)
        return self.createRSPPacket(self.target.getTResponse()), 0

    def halt(self):
        self.target.halt()
        return self.createRSPPacket(self.target.getTResponse()), 0

    def flashOp(self, data):
        ops = data.split(':')[0]
        logging.debug("flash op: %s", ops)

        if ops == 'FlashErase':
            return self.createRSPPacket("OK")

        elif ops == 'FlashWrite':
            write_addr = int(data.split(':')[1], 16)
            logging.debug("flash write addr: 0x%x", write_addr)
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ':':
                    second_colon += 1
                idx_begin += 1

            # Get flash builder if there isn't one already
            if self.flashBuilder == None:
                self.flashBuilder = self.flash.getFlashBuilder()

            # Add data to flash builder
            self.flashBuilder.addData(write_addr, self.unescape(data[idx_begin:len(data) - 3]))


            return self.createRSPPacket("OK")

        # we need to flash everything
        elif 'FlashDone' in ops :

            def print_progress(progress):
                # Reset state on 0.0
                if progress == 0.0:
                    print_progress.done = False

                # print progress bar
                if not print_progress.done:
                    sys.stdout.write('\r')
                    i = int(progress*20.0)
                    sys.stdout.write("[%-20s] %3d%%" % ('='*i, round(progress * 100)))
                    sys.stdout.flush()

                # Finish on 1.0
                if progress >= 1.0:
                    if not print_progress.done:
                        print_progress.done = True
                        sys.stdout.write("\r\n")

            if self.hide_programming_progress:
                progress_cb = None
            else:
                 progress_cb = print_progress

            self.flashBuilder.program(chip_erase = self.chip_erase, progress_cb=progress_cb, fast_verify=self.fast_program)

            # Set flash builder to None so that on the next flash command a new
            # object is used.
            self.flashBuilder = None

            return self.createRSPPacket("OK")

        elif 'Cont' in ops:
            if 'Cont?' in ops:
                return self.createRSPPacket("vCont;c;s;t")

        return None

    def unescape(self, data):
        data_idx = 0

        # unpack the data into binary array
        str_unpack = str(len(data)) + 'B'
        data = unpack(str_unpack, data)
        data = list(data)

        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7d:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1

        return data


    def getMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = split[1].split('#')[0]
        length = int(length,16)

        if LOG_MEM:
            logging.debug("GDB getMem: addr=%x len=%x", addr, length)

        try:
            val = ''
            mem = self.target.readBlockMemoryUnaligned8(addr, length)
            # Flush so an exception is thrown now if invalid memory was accesses
            self.target.flush()
            for x in mem:
                if x >= 0x10:
                    val += hex(x)[2:4]
                else:
                    val += '0' + hex(x)[2:3]
        except TransferError:
            logging.debug("getMemory failed at 0x%x" % addr)
            val = 'E01' #EPERM
        return self.createRSPPacket(val)

    def writeMemoryHex(self, data):
        split = data.split(',')
        addr = int(split[0], 16)

        split = split[1].split(':')
        length = int(split[0], 16)

        split = split[1].split('#')
        data = hexToByteList(split[0])

        if LOG_MEM:
            logging.debug("GDB writeMemHex: addr=%x len=%x", addr, length)

        try:
            if length > 0:
                self.target.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target.flush()
            resp = "OK"
        except TransferError:
            logging.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01' #EPERM

        return self.createRSPPacket(resp)

    def writeMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = int(split[1].split(':')[0], 16)

        if LOG_MEM:
            logging.debug("GDB writeMem: addr=%x len=%x", addr, length)

        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ':':
                idx_begin += 1
                break
            idx_begin += 1

        data = data[idx_begin:len(data) - 3]
        data = self.unescape(data)

        try:
            if length > 0:
                self.target.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target.flush()
            resp = "OK"
        except TransferError:
            logging.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01' #EPERM

        return self.createRSPPacket(resp)

    def readRegister(self, which):
        return self.createRSPPacket(self.target.gdbGetRegister(which))

    def writeRegister(self, data):
        reg = int(data.split('=')[0], 16)
        val = data.split('=')[1].split('#')[0]
        self.target.setRegister(reg, val)
        return self.createRSPPacket("OK")

    def getRegisters(self):
        return self.createRSPPacket(self.target.getRegisterContext())

    def setRegisters(self, data):
        self.target.setRegisterContext(data)
        return self.createRSPPacket("OK")

    def handleQuery(self, msg):
        query = msg.split(':')
        logging.debug('GDB received query: %s', query)

        if query is None:
            logging.error('GDB received query packet malformed')
            return None

        if query[0] == 'Supported':
            # Save features sent by gdb.
            self.gdb_features = query[1].split(';')

            # Build our list of features.
            features = ['qXfer:features:read+', 'QStartNoAckMode+']
            features.append('PacketSize=' + hex(self.packet_size)[2:])
            if self.target.getMemoryMapXML() is not None:
                features.append('qXfer:memory-map:read+')
            resp = ';'.join(features)
            return self.createRSPPacket(resp)

        elif query[0] == 'Xfer':

            if query[1] == 'features' and query[2] == 'read' and \
               query[3] == 'target.xml':
                data = query[4].split(',')
                resp = self.handleQueryXML('read_feature', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == 'memory-map' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('memory_map', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            else:
                return None

        elif query[0] == 'C#b4':
            return self.createRSPPacket("")

        elif query[0].find('Attached') != -1:
            return self.createRSPPacket("1")

        elif query[0].find('TStatus') != -1:
            return self.createRSPPacket("")

        elif query[0].find('Tf') != -1:
            return self.createRSPPacket("")

        elif 'Offsets' in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)

        elif 'Symbol' in query[0]:
            resp = "OK"
            return self.createRSPPacket(resp)

        elif query[0].startswith('Rcmd,'):
            cmd = hexDecode(query[0][5:].split('#')[0])
            logging.debug('Remote command: %s', cmd)

            safecmd = {
                'reset' : ['Reset target', 0x1],
                'halt'  : ['Halt target', 0x2],
                'resume': ['Resume target', 0x4],
                'help'  : ['Display this help', 0x80],
            }
            resultMask = 0x00
            if cmd == 'help':
                resp = ''
                for k,v in safecmd.items():
                    resp += '%s\t%s\n' % (k,v[0])
                resp = hexEncode(resp)
            else:
                cmdList = cmd.split(' ')
                #check whether all the cmds is valid cmd for monitor
                for cmd_sub in cmdList:
                    if not cmd_sub in safecmd:
                        #error cmd for monitor
                        logging.warning("Invalid mon command '%s'", cmd)
                        resp = 'Invalid Command: "%s"\n' % cmd
                        resp = hexEncode(resp)
                        return self.createRSPPacket(resp)
                    else:
                        resultMask = resultMask | safecmd[cmd_sub][1]
                #if it's a single cmd, just launch it!
                if len(cmdList) == 1:
                    tmp = eval ('self.target.%s()' % cmd_sub)
                    logging.debug(tmp)
                    resp = "OK"
                else:
                    #10000001 for help reset, so output reset cmd help information
                    if resultMask == 0x81:
                        resp = 'Reset the target\n'
                        resp = hexEncode(resp)
                    #10000010 for help halt, so output halt cmd help information
                    elif resultMask == 0x82:
                        resp = 'Halt the target\n'
                        resp = hexEncode(resp)
                    #10000100 for help resume, so output resume cmd help information
                    elif resultMask == 0x84:
                        resp = 'Resume the target\n'
                        resp = hexEncode(resp)
                    #11 for reset halt cmd, so launch self.target.resetStopOnReset()
                    elif resultMask == 0x3:
                        resp = "OK"
                        self.target.resetStopOnReset()
                    #111 for reset halt resume cmd, so launch self.target.resetStopOnReset() and self.target.resume()
                    elif resultMask == 0x7:
                        resp = "OK"
                        self.target.resetStopOnReset()
                        self.target.resume()
                    else:
                        logging.warning("Invalid mon command '%s'", cmd)
                        resp = 'Invalid Command: "%s"\n' % cmd
                        resp = hexEncode(resp)

                if self.target.getState() != TARGET_HALTED:
                    logging.error("Remote command left target running!")
                    logging.error("Forcing target to halt")
                    self.target.halt()

            return self.createRSPPacket(resp)

        else:
            return self.createRSPPacket("")

    def handleGeneralSet(self, msg):
        logging.debug("GDB general set: %s", msg)
        feature = msg.split('#')[0]

        if feature == 'StartNoAckMode':
            # Disable acks after the reply and ack.
            self.packet_io.set_send_acks(False)
            return self.createRSPPacket("OK")
        else:
            return self.createRSPPacket("")

    def handleQueryXML(self, query, offset, size):
        logging.debug('GDB query %s: offset: %s, size: %s', query, offset, size)
        xml = ''
        if query == 'memory_map':
            xml = self.target.getMemoryMapXML()
        elif query == 'read_feature':
            xml = self.target.getTargetXML()

        size_xml = len(xml)

        prefix = 'm'

        if offset > size_xml:
            logging.error('GDB: offset target.xml > size!')
            return

        if size > (self.packet_size - 4):
            size = self.packet_size - 4

        nbBytesAvailable = size_xml - offset

        if size > nbBytesAvailable:
            prefix = 'l'
            size = nbBytesAvailable

        resp = prefix + xml[offset:offset + size]

        return resp


    def createRSPPacket(self, data):
        resp = '$' + data + '#' + checksum(data)
        return resp
Пример #10
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """
    def __init__(self, board, port_urlWSS, options={}):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.break_at_hardfault = bool(options.get('break_at_hardfault', True))
        self.board.target.setVectorCatchFault(self.break_at_hardfault)
        self.break_on_reset = options.get('break_on_reset', False)
        self.board.target.setVectorCatchReset(self.break_on_reset)
        self.step_into_interrupt = options.get('step_into_interrupt', False)
        self.persist = options.get('persist', False)
        self.soft_bkpt_as_hard = options.get('soft_bkpt_as_hard', False)
        self.chip_erase = options.get('chip_erase', None)
        self.hide_programming_progress = options.get(
            'hide_programming_progress', False)
        self.fast_program = options.get('fast_program', False)
        self.enable_semihosting = options.get('enable_semihosting', False)
        self.telnet_port = options.get('telnet_port', 4444)
        self.semihost_use_syscalls = options.get('semihost_use_syscalls',
                                                 False)
        self.server_listening_callback = options.get(
            'server_listening_callback', None)
        self.serve_local_only = options.get('serve_local_only', True)
        self.packet_size = 2048
        self.packet_io = None
        self.gdb_features = []
        self.non_stop = False
        self.is_target_running = (
            self.target.getState() == Target.TARGET_RUNNING)
        self.flashBuilder = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
            if self.serve_local_only:
                self.abstract_socket.host = 'localhost'
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)

        # Init semihosting and telnet console.
        if self.semihost_use_syscalls:
            semihost_io_handler = GDBSyscallIOHandler(self)
        else:
            # Use internal IO handler.
            semihost_io_handler = semihost.InternalSemihostIOHandler()
        self.telnet_console = semihost.TelnetSemihostIOHandler(
            self.telnet_port, self.serve_local_only)
        self.semihost = semihost.SemihostAgent(self.target,
                                               io_handler=semihost_io_handler,
                                               console=self.telnet_console)

        self.setDaemon(True)
        self.start()

    def restart(self):
        if self.isAlive():
            self.detach_event.set()

    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            while self.isAlive():
                pass
            logging.info("GDB server thread killed")
        self.board.uninit()

    def setBoard(self, board, stop=True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return

    def _cleanup(self):
        logging.debug("GDB server cleaning up")
        if self.packet_io:
            self.packet_io.stop()
            self.packet_io = None
        if self.semihost:
            self.semihost.cleanup()
            self.semihost = None
        if self.telnet_console:
            self.telnet_console.stop()
            self.telnet_console = None

    def run(self):
        logging.info('GDB server started at port:%d', self.port)

        while True:
            try:
                self.detach_event.clear()

                # Inform callback that the server is running.
                if self.server_listening_callback:
                    self.server_listening_callback(self)

                while not self.shutdown_event.isSet(
                ) and not self.detach_event.isSet():
                    connected = self.abstract_socket.connect()
                    if connected != None:
                        self.packet_io = GDBServerPacketIOThread(
                            self.abstract_socket)
                        break

                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    continue

                logging.info("One client connected!")
                self._run_connection()

            except Exception as e:
                logging.error("Unexpected exception: %s", e)
                traceback.print_exc()

    def _run_connection(self):
        while True:
            try:
                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    break

                if self.packet_io.interrupt_event.isSet():
                    if self.non_stop:
                        self.target.halt()
                        self.is_target_running = False
                        self.sendStopNotification()
                    else:
                        logging.error("Got unexpected ctrl-c, ignoring")
                    self.packet_io.interrupt_event.clear()

                if self.non_stop and self.is_target_running:
                    try:
                        if self.target.getState() == Target.TARGET_HALTED:
                            logging.debug("state halted")
                            self.is_target_running = False
                            self.sendStopNotification()
                    except Exception as e:
                        logging.error("Unexpected exception: %s", e)
                        traceback.print_exc()

                # read command
                try:
                    packet = self.packet_io.receive(block=not self.non_stop)
                except ConnectionClosedException:
                    break

                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    break

                if self.non_stop and packet is None:
                    sleep(0.1)
                    continue

                self.lock.acquire()

                if len(packet) != 0:
                    # decode and prepare resp
                    resp, detach = self.handleMsg(packet)

                    if resp is not None:
                        # send resp
                        self.packet_io.send(resp)

                    if detach:
                        self.abstract_socket.close()
                        self.packet_io.stop()
                        self.packet_io = None
                        self.lock.release()
                        if self.persist:
                            break
                        else:
                            self.shutdown_event.set()
                            return

                self.lock.release()

            except Exception as e:
                logging.error("Unexpected exception: %s", e)
                traceback.print_exc()

    def handleMsg(self, msg):
        try:
            if msg[0] != '$':
                logging.debug('msg ignored: first char != $')
                return None, 0

            # query command
            if msg[1] == '?':
                return self.stopReasonQuery(), 0

            # we don't send immediately the response for C and S commands
            elif msg[1] == 'C' or msg[1] == 'c':
                return self.resume(msg[1:]), 0

            elif msg[1] == 'D':
                return self.detach(msg[1:]), 1

            elif msg[1] == 'g':
                return self.getRegisters(), 0

            elif msg[1] == 'G':
                return self.setRegisters(msg[2:]), 0

            elif msg[1] == 'H':
                return self.createRSPPacket('OK'), 0

            elif msg[1] == 'k':
                return self.kill(), 1

            elif msg[1] == 'm':
                return self.getMemory(msg[2:]), 0

            elif msg[1] == 'M':  # write memory with hex data
                return self.writeMemoryHex(msg[2:]), 0

            elif msg[1] == 'p':
                return self.readRegister(msg[2:]), 0

            elif msg[1] == 'P':
                return self.writeRegister(msg[2:]), 0

            elif msg[1] == 'q':
                return self.handleQuery(msg[2:]), 0

            elif msg[1] == 'Q':
                return self.handleGeneralSet(msg[2:]), 0

            elif msg[1] == 'S' or msg[1] == 's':
                return self.step(msg[1:]), 0

            elif msg[1] == 'T':  # check if thread is alive
                return self.createRSPPacket('OK'), 0

            elif msg[1] == 'v':
                return self.vCommand(msg[2:]), 0

            elif msg[1] == 'X':  # write memory with binary data
                return self.writeMemory(msg[2:]), 0

            elif msg[1] == 'Z' or msg[1] == 'z':
                return self.breakpoint(msg[1:]), 0

            else:
                logging.error("Unknown RSP packet: %s", msg)
                return self.createRSPPacket(""), 0

        except Exception as e:
            logging.error("Unhandled exception in handleMsg: %s", e)
            traceback.print_exc()
            return self.createRSPPacket("E01"), 0

    def detach(self, data):
        logging.info("Client detached")
        resp = "OK"
        return self.createRSPPacket(resp)

    def kill(self):
        logging.debug("GDB kill")
        # Keep target halted and leave vector catches if in persistent mode.
        if not self.persist:
            self.board.target.setVectorCatchFault(False)
            self.board.target.setVectorCatchReset(False)
            self.board.target.resume()
        return self.createRSPPacket("")

    def breakpoint(self, data):
        # handle breakpoint/watchpoint commands
        split = data.split('#')[0].split(',')
        addr = int(split[1], 16)
        logging.debug("GDB breakpoint %s%d @ %x" %
                      (data[0], int(data[1]), addr))

        # handle software breakpoint Z0/z0
        if data[1] == '0' and not self.soft_bkpt_as_hard:
            if data[0] == 'Z':
                if not self.target.setBreakpoint(addr, Target.BREAKPOINT_SW):
                    return self.createRSPPacket('E01')  #EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware breakpoint Z1/z1
        if data[1] == '1' or (self.soft_bkpt_as_hard and data[1] == '0'):
            if data[0] == 'Z':
                if self.target.setBreakpoint(addr,
                                             Target.BREAKPOINT_HW) == False:
                    return self.createRSPPacket('E01')  #EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware watchpoint Z2/z2/Z3/z3/Z4/z4
        if data[1] == '2':
            # Write-only watch
            watchpoint_type = Target.WATCHPOINT_WRITE
        elif data[1] == '3':
            # Read-only watch
            watchpoint_type = Target.WATCHPOINT_READ
        elif data[1] == '4':
            # Read-Write watch
            watchpoint_type = Target.WATCHPOINT_READ_WRITE
        else:
            return self.createRSPPacket('E01')  #EPERM

        size = int(split[2], 16)
        if data[0] == 'Z':
            if self.target.setWatchpoint(addr, size, watchpoint_type) == False:
                return self.createRSPPacket('E01')  #EPERM
        else:
            self.target.removeWatchpoint(addr, size, watchpoint_type)
        return self.createRSPPacket("OK")

    def stopReasonQuery(self):
        # In non-stop mode, if no threads are stopped we need to reply with OK.
        if self.non_stop and self.is_target_running:
            return self.createRSPPacket("OK")

        return self.createRSPPacket(self.target.getTResponse())

    def _get_resume_step_addr(self, data):
        if data is None:
            return None
        data = data.split('#')[0]
        if ';' not in data:
            return None
        # c[;addr]
        if data[0] in ('c', 's'):
            addr = int(data[2:], base=16)
        # Csig[;addr]
        elif data[0] in ('C', 'S'):
            addr = int(data[1:].split(';')[1], base=16)
        return addr

    def resume(self, data):
        addr = self._get_resume_step_addr(data)
        self.target.resume()
        logging.debug("target resumed")

        val = ''

        while True:
            if self.shutdown_event.isSet():
                self.packet_io.interrupt_event.clear()
                return self.createRSPPacket(val)

            # Wait for a ctrl-c to be received.
            if self.packet_io.interrupt_event.wait(0.01):
                logging.debug("receive CTRL-C")
                self.packet_io.interrupt_event.clear()
                self.target.halt()
                val = self.target.getTResponse(forceSignal=signals.SIGINT)
                break

            try:
                if self.target.getState() == Target.TARGET_HALTED:
                    # Handle semihosting
                    if self.enable_semihosting:
                        was_semihost = self.semihost.check_and_handle_semihost_request(
                        )

                        if was_semihost:
                            self.target.resume()
                            continue

                    logging.debug("state halted")
                    val = self.target.getTResponse()
                    break
            except Exception as e:
                try:
                    self.target.halt()
                except:
                    pass
                traceback.print_exc()
                logging.debug('Target is unavailable temporarily.')
                val = 'S%02x' % self.target.getSignalValue()
                break

        return self.createRSPPacket(val)

    def step(self, data):
        addr = self._get_resume_step_addr(data)
        logging.debug("GDB step: %s", data)
        self.target.step(not self.step_into_interrupt)
        return self.createRSPPacket(self.target.getTResponse())

    def halt(self):
        self.target.halt()
        return self.createRSPPacket(self.target.getTResponse())

    def sendStopNotification(self, forceSignal=None):
        data = self.target.getTResponse(forceSignal=forceSignal)
        packet = '%Stop:' + data + '#' + checksum(data)
        self.packet_io.send(packet)

    def vCommand(self, data):
        cmd = data.split('#')[0]
        logging.debug("GDB vCommand: %s", cmd)

        # Flash command.
        if cmd.startswith('Flash'):
            return self.flashOp(data)

        # vCont capabilities query.
        elif 'Cont?' == cmd:
            return self.createRSPPacket("vCont;c;C;s;S;t")

        # vCont, thread action command.
        elif cmd.startswith('Cont'):
            return self.vCont(cmd)

        # vStopped, part of thread stop state notification sequence.
        elif 'Stopped' in cmd:
            # Because we only support one thread for now, we can just reply OK to vStopped.
            return self.createRSPPacket("OK")

        return self.createRSPPacket("")

    # Example: $vCont;s:1;c#c1
    def vCont(self, cmd):
        ops = cmd.split(';')[1:]  # split and remove 'Cont' from list
        if not ops:
            return self.createRSPPacket("OK")

        thread_actions = {1: None}  # our only thread
        default_action = None
        for op in ops:
            args = op.split(':')
            action = args[0]
            if len(args) > 1:
                thread_id = args[1]
                if thread_id == '-1' or thread_id == '0':
                    thread_id = '1'
                thread_id = int(thread_id, base=16)
                thread_actions[thread_id] = action
            else:
                default_action = action

        logging.debug("thread_actions=%s; default_action=%s",
                      repr(thread_actions), default_action)

        # Only thread 1 is supported at the moment.
        if thread_actions[1] is None:
            if default_action is None:
                return self.createRSPPacket('E01')
            thread_actions[1] = default_action

        if thread_actions[1][0] in ('c', 'C'):
            if self.non_stop:
                self.target.resume()
                self.is_target_running = True
                return self.createRSPPacket("OK")
            else:
                return self.resume(None)
        elif thread_actions[1][0] in ('s', 'S'):
            if self.non_stop:
                self.target.step(not self.step_into_interrupt)
                self.packet_io.send(self.createRSPPacket("OK"))
                self.sendStopNotification()
                return None
            else:
                return self.step(None)
        elif thread_actions[1] == 't':
            # Must ignore t command in all-stop mode.
            if not self.non_stop:
                return self.createRSPPacket("")
            self.packet_io.send(self.createRSPPacket("OK"))
            self.target.halt()
            self.is_target_running = False
            self.sendStopNotification(forceSignal=0)
        else:
            logging.error("Unsupported vCont action '%s'" % thread_actions[1])

    def flashOp(self, data):
        ops = data.split(':')[0]
        logging.debug("flash op: %s", ops)

        if ops == 'FlashErase':
            return self.createRSPPacket("OK")

        elif ops == 'FlashWrite':
            write_addr = int(data.split(':')[1], 16)
            logging.debug("flash write addr: 0x%x", write_addr)
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ':':
                    second_colon += 1
                idx_begin += 1

            # Get flash builder if there isn't one already
            if self.flashBuilder == None:
                self.flashBuilder = self.flash.getFlashBuilder()

            # Add data to flash builder
            self.flashBuilder.addData(
                write_addr, self.unescape(data[idx_begin:len(data) - 3]))

            return self.createRSPPacket("OK")

        # we need to flash everything
        elif 'FlashDone' in ops:

            def print_progress(progress):
                # Reset state on 0.0
                if progress == 0.0:
                    print_progress.done = False

                # print progress bar
                if not print_progress.done:
                    sys.stdout.write('\r')
                    i = int(progress * 20.0)
                    sys.stdout.write("[%-20s] %3d%%" %
                                     ('=' * i, round(progress * 100)))
                    sys.stdout.flush()

                # Finish on 1.0
                if progress >= 1.0:
                    if not print_progress.done:
                        print_progress.done = True
                        sys.stdout.write("\r\n")

            if self.hide_programming_progress:
                progress_cb = None
            else:
                progress_cb = print_progress

            self.flashBuilder.program(chip_erase=self.chip_erase,
                                      progress_cb=progress_cb,
                                      fast_verify=self.fast_program)

            # Set flash builder to None so that on the next flash command a new
            # object is used.
            self.flashBuilder = None

            return self.createRSPPacket("OK")

        return None

    def unescape(self, data):
        data_idx = 0

        # unpack the data into binary array
        str_unpack = str(len(data)) + 'B'
        data = unpack(str_unpack, data)
        data = list(data)

        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7d:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1

        return data

    def getMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = split[1].split('#')[0]
        length = int(length, 16)

        if LOG_MEM:
            logging.debug("GDB getMem: addr=%x len=%x", addr, length)

        try:
            val = ''
            mem = self.target.readBlockMemoryUnaligned8(addr, length)
            # Flush so an exception is thrown now if invalid memory was accesses
            self.target.flush()
            for x in mem:
                if x >= 0x10:
                    val += hex(x)[2:4]
                else:
                    val += '0' + hex(x)[2:3]
        except DAPAccess.TransferError:
            logging.debug("getMemory failed at 0x%x" % addr)
            val = 'E01'  #EPERM
        return self.createRSPPacket(val)

    def writeMemoryHex(self, data):
        split = data.split(',')
        addr = int(split[0], 16)

        split = split[1].split(':')
        length = int(split[0], 16)

        split = split[1].split('#')
        data = hexToByteList(split[0])

        if LOG_MEM:
            logging.debug("GDB writeMemHex: addr=%x len=%x", addr, length)

        try:
            if length > 0:
                self.target.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target.flush()
            resp = "OK"
        except DAPAccess.TransferError:
            logging.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01'  #EPERM

        return self.createRSPPacket(resp)

    def writeMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = int(split[1].split(':')[0], 16)

        if LOG_MEM:
            logging.debug("GDB writeMem: addr=%x len=%x", addr, length)

        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ':':
                idx_begin += 1
                break
            idx_begin += 1

        data = data[idx_begin:len(data) - 3]
        data = self.unescape(data)

        try:
            if length > 0:
                self.target.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target.flush()
            resp = "OK"
        except DAPAccess.TransferError:
            logging.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01'  #EPERM

        return self.createRSPPacket(resp)

    def readRegister(self, which):
        return self.createRSPPacket(self.target.gdbGetRegister(which))

    def writeRegister(self, data):
        reg = int(data.split('=')[0], 16)
        val = data.split('=')[1].split('#')[0]
        self.target.setRegister(reg, val)
        return self.createRSPPacket("OK")

    def getRegisters(self):
        return self.createRSPPacket(self.target.getRegisterContext())

    def setRegisters(self, data):
        self.target.setRegisterContext(data)
        return self.createRSPPacket("OK")

    def handleQuery(self, msg):
        query = msg.split(':')
        logging.debug('GDB received query: %s', query)

        if query is None:
            logging.error('GDB received query packet malformed')
            return None

        if query[0] == 'Supported':
            # Save features sent by gdb.
            self.gdb_features = query[1].split(';')

            # Build our list of features.
            features = [
                'qXfer:features:read+', 'QStartNoAckMode+',
                'qXfer:threads:read+', 'QNonStop+'
            ]
            features.append('PacketSize=' + hex(self.packet_size)[2:])
            if self.target.getMemoryMapXML() is not None:
                features.append('qXfer:memory-map:read+')
            resp = ';'.join(features)
            return self.createRSPPacket(resp)

        elif query[0] == 'Xfer':

            if query[1] == 'features' and query[2] == 'read' and \
               query[3] == 'target.xml':
                data = query[4].split(',')
                resp = self.handleQueryXML('read_feature', int(data[0], 16),
                                           int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == 'memory-map' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('memory_map', int(data[0], 16),
                                           int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == 'threads' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('threads', int(data[0], 16),
                                           int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            else:
                logging.debug("Unsupported qXfer request: %s:%s:%s:%s",
                              query[1], query[2], query[3], query[4])
                return None

        elif query[0].startswith('C'):
            return self.createRSPPacket("QC1")

        elif query[0].find('Attached') != -1:
            return self.createRSPPacket("1")

        elif query[0].find('TStatus') != -1:
            return self.createRSPPacket("")

        elif query[0].find('Tf') != -1:
            return self.createRSPPacket("")

        elif 'Offsets' in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)

        elif 'Symbol' in query[0]:
            resp = "OK"
            return self.createRSPPacket(resp)

        elif query[0].startswith('Rcmd,'):
            cmd = hexDecode(query[0][5:].split('#')[0])
            return self.handleRemoteCommand(cmd)

        else:
            return self.createRSPPacket("")

    # TODO rewrite the remote command handler
    def handleRemoteCommand(self, cmd):
        logging.debug('Remote command: %s', cmd)

        safecmd = {
            'init': ['Init reset sequence', 0x1],
            'reset': ['Reset and halt the target', 0x2],
            'halt': ['Halt target', 0x4],
            # 'resume': ['Resume target', 0x8],
            'help': ['Display this help', 0x80],
        }

        resp = 'OK'
        if cmd == 'help':
            resp = ''.join(
                ['%s\t%s\n' % (k, v[0]) for k, v in safecmd.items()])
            resp = hexEncode(resp)
        elif cmd.startswith('arm semihosting'):
            self.enable_semihosting = 'enable' in cmd
            logging.info(
                "Semihosting %s",
                ('enabled' if self.enable_semihosting else 'disabled'))
        else:
            resultMask = 0x00
            cmdList = cmd.split()
            if cmdList[0] == 'help':
                # a 'help' is only valid as the first cmd, and only
                # gives info on the second cmd if it is valid
                resultMask |= 0x80
                del cmdList[0]

            for cmd_sub in cmdList:
                if cmd_sub not in safecmd:
                    logging.warning("Invalid mon command '%s'", cmd_sub)
                    resp = 'Invalid Command: "%s"\n' % cmd_sub
                    resp = hexEncode(resp)
                    return self.createRSPPacket(resp)
                elif resultMask == 0x80:
                    # if the first command was a 'help', we only need
                    # to return info about the first cmd after it
                    resp = hexEncode(safecmd[cmd_sub][0] + '\n')
                    return self.createRSPPacket(resp)
                resultMask |= safecmd[cmd_sub][1]

            # Run cmds in proper order
            if resultMask & 0x1:
                self.target.init()
            if (resultMask & 0x6) == 0x6:
                self.target.resetStopOnReset()
            elif resultMask & 0x2:
                # on 'reset' still do a reset halt
                self.target.resetStopOnReset()
                # self.target.reset()
            elif resultMask & 0x4:
                self.target.halt()
            # if resultMask & 0x8:
            #     self.target.resume()

        return self.createRSPPacket(resp)

    def handleGeneralSet(self, msg):
        feature = msg.split('#')[0]
        logging.debug("GDB general set: %s", feature)

        if feature == 'StartNoAckMode':
            # Disable acks after the reply and ack.
            self.packet_io.set_send_acks(False)
            return self.createRSPPacket("OK")

        elif feature.startswith('NonStop'):
            enable = feature.split(':')[1]
            self.non_stop = (enable == '1')
            return self.createRSPPacket("OK")

        else:
            return self.createRSPPacket("")

    def handleQueryXML(self, query, offset, size):
        logging.debug('GDB query %s: offset: %s, size: %s', query, offset,
                      size)
        xml = ''
        if query == 'memory_map':
            xml = self.target.getMemoryMapXML()
        elif query == 'read_feature':
            xml = self.target.getTargetXML()
        elif query == 'threads':
            xml = self.target.getThreadsXML()
        else:
            raise RuntimeError("Invalid XML query (%s)" % query)

        size_xml = len(xml)

        prefix = 'm'

        if offset > size_xml:
            logging.error('GDB: xml offset > size for %s!', query)
            return

        if size > (self.packet_size - 4):
            size = self.packet_size - 4

        nbBytesAvailable = size_xml - offset

        if size > nbBytesAvailable:
            prefix = 'l'
            size = nbBytesAvailable

        resp = prefix + xml[offset:offset + size]

        return resp

    def createRSPPacket(self, data):
        resp = '$' + data + '#' + checksum(data)
        return resp

    def syscall(self, op):
        logging.debug("GDB server syscall: %s", op)
        request = self.createRSPPacket('F' + op)
        self.packet_io.send(request)

        while not self.packet_io.interrupt_event.is_set():
            # Read a packet.
            packet = self.packet_io.receive(False)
            if packet is None:
                sleep(0.1)
                continue

            # Check for file I/O response.
            if packet[0] == '$' and packet[1] == 'F':
                logging.debug("Syscall: got syscall response " + packet)
                args = packet[2:packet.index('#')].split(',')
                result = int(args[0], base=16)
                errno = int(args[1], base=16) if len(args) > 1 else 0
                ctrl_c = args[2] if len(args) > 2 else ''
                if ctrl_c == 'C':
                    self.packet_io.interrupt_event.set()
                    self.packet_io.drop_reply = True
                return result, errno

            # decode and prepare resp
            resp, detach = self.handleMsg(packet)

            if resp is not None:
                # send resp
                self.packet_io.send(resp)

            if detach:
                self.detach_event.set()
                logging.warning(
                    "GDB server received detach request while waiting for file I/O completion"
                )
                break

        return -1, 0
Пример #11
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """
    def __init__(self, board, port_urlWSS, options={}):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.log = logging.getLogger('gdbserver')
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.vector_catch = options.get('vector_catch', Target.CATCH_HARD_FAULT)
        self.board.target.setVectorCatch(self.vector_catch)
        self.step_into_interrupt = options.get('step_into_interrupt', False)
        self.persist = options.get('persist', False)
        self.soft_bkpt_as_hard = options.get('soft_bkpt_as_hard', False)
        self.chip_erase = options.get('chip_erase', None)
        self.hide_programming_progress = options.get('hide_programming_progress', False)
        self.fast_program = options.get('fast_program', False)
        self.enable_semihosting = options.get('enable_semihosting', False)
        self.telnet_port = options.get('telnet_port', 4444)
        self.semihost_use_syscalls = options.get('semihost_use_syscalls', False)
        self.server_listening_callback = options.get('server_listening_callback', None)
        self.serve_local_only = options.get('serve_local_only', True)
        self.packet_size = 2048
        self.packet_io = None
        self.gdb_features = []
        self.non_stop = False
        self.is_target_running = (self.target.getState() == Target.TARGET_RUNNING)
        self.flashBuilder = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        self.target_context = self.target.getTargetContext()
        self.target_facade = GDBDebugContextFacade(self.target_context)
        self.thread_provider = None
        self.did_init_thread_providers = False
        self.current_thread_id = 0
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
            if self.serve_local_only:
                self.abstract_socket.host = 'localhost'
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)

        # Init semihosting and telnet console.
        if self.semihost_use_syscalls:
            semihost_io_handler = GDBSyscallIOHandler(self)
        else:
            # Use internal IO handler.
            semihost_io_handler = semihost.InternalSemihostIOHandler()
        self.telnet_console = semihost.TelnetSemihostIOHandler(self.telnet_port, self.serve_local_only)
        self.semihost = semihost.SemihostAgent(self.target_context, io_handler=semihost_io_handler, console=self.telnet_console)

        # Command handler table.
        #
        # The dict keys are the first character of the incoming command from gdb. Values are a
        # bi-tuple. The first element is the handler method, and the second element is the start
        # offset of the command string passed to the handler.
        #
        # Start offset values:
        #  0 - Special case: handler method does not take any parameters.
        #  1 - Strip off leading "$" from command.
        #  2 - Strip leading "$" plus character matched through this table.
        #  3+ - Supported, but not very useful.
        #
        self.COMMANDS = {
        #       CMD    HANDLER                  START    DESCRIPTION
                '?' : (self.stopReasonQuery,    0   ), # Stop reason query.
                'C' : (self.resume,             1   ), # Continue (at addr)
                'c' : (self.resume,             1   ), # Continue with signal.
                'D' : (self.detach,             1   ), # Detach.
                'g' : (self.getRegisters,       0   ), # Read general registers.
                'G' : (self.setRegisters,       2   ), # Write general registers.
                'H' : (self.setThread,          2   ), # Set thread for subsequent operations.
                'k' : (self.kill,               0   ), # Kill.
                'm' : (self.getMemory,          2   ), # Read memory.
                'M' : (self.writeMemoryHex,     2   ), # Write memory (hex).
                'p' : (self.readRegister,       2   ), # Read register.
                'P' : (self.writeRegister,      2   ), # Write register.
                'q' : (self.handleQuery,        2   ), # General query.
                'Q' : (self.handleGeneralSet,   2   ), # General set.
                's' : (self.step,               1   ), # Single step.
                'S' : (self.step,               1   ), # Step with signal.
                'T' : (self.isThreadAlive,      1   ), # Thread liveness query.
                'v' : (self.vCommand,           2   ), # v command.
                'X' : (self.writeMemory,        2   ), # Write memory (binary).
                'z' : (self.breakpoint,         1   ), # Insert breakpoint/watchpoint.
                'Z' : (self.breakpoint,         1   ), # Remove breakpoint/watchpoint.
            }

        # Commands that kill the connection to gdb.
        self.DETACH_COMMANDS = ('D', 'k')

        self.setDaemon(True)
        self.start()

    def restart(self):
        if self.isAlive():
            self.detach_event.set()

    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            while self.isAlive():
                pass
            self.log.info("GDB server thread killed")
        self.board.uninit()

    def setBoard(self, board, stop=True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return

    def _cleanup(self):
        self.log.debug("GDB server cleaning up")
        if self.packet_io:
            self.packet_io.stop()
            self.packet_io = None
        if self.semihost:
            self.semihost.cleanup()
            self.semihost = None
        if self.telnet_console:
            self.telnet_console.stop()
            self.telnet_console = None

    def _cleanup_for_next_connection(self):
        self.non_stop = False
        self.thread_provider = None
        self.did_init_thread_providers = False
        self.current_thread_id = 0

    def run(self):
        self.log.info('GDB server started at port:%d', self.port)

        while True:
            try:
                self.detach_event.clear()

                # Inform callback that the server is running.
                if self.server_listening_callback:
                    self.server_listening_callback(self)

                while not self.shutdown_event.isSet() and not self.detach_event.isSet():
                    connected = self.abstract_socket.connect()
                    if connected != None:
                        self.packet_io = GDBServerPacketIOThread(self.abstract_socket)
                        break

                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    continue

                self.log.info("One client connected!")
                self._run_connection()

            except Exception as e:
                self.log.error("Unexpected exception: %s", e)
                traceback.print_exc()

    def _run_connection(self):
        while True:
            try:
                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    break

                if self.packet_io.interrupt_event.isSet():
                    if self.non_stop:
                        self.target.halt()
                        self.is_target_running = False
                        self.sendStopNotification()
                    else:
                        self.log.error("Got unexpected ctrl-c, ignoring")
                    self.packet_io.interrupt_event.clear()

                if self.non_stop and self.is_target_running:
                    try:
                        if self.target.getState() == Target.TARGET_HALTED:
                            self.log.debug("state halted")
                            self.is_target_running = False
                            self.sendStopNotification()
                    except Exception as e:
                        self.log.error("Unexpected exception: %s", e)
                        traceback.print_exc()

                # read command
                try:
                    packet = self.packet_io.receive(block=not self.non_stop)
                except ConnectionClosedException:
                    break

                if self.shutdown_event.isSet():
                    self._cleanup()
                    return

                if self.detach_event.isSet():
                    break

                if self.non_stop and packet is None:
                    sleep(0.1)
                    continue

                self.lock.acquire()

                if len(packet) != 0:
                    # decode and prepare resp
                    resp, detach = self.handleMsg(packet)

                    if resp is not None:
                        # send resp
                        self.packet_io.send(resp)

                    if detach:
                        self.abstract_socket.close()
                        self.packet_io.stop()
                        self.packet_io = None
                        self.lock.release()
                        if self.persist:
                            self._cleanup_for_next_connection()
                            break
                        else:
                            self.shutdown_event.set()
                            return

                self.lock.release()

            except Exception as e:
                self.log.error("Unexpected exception: %s", e)
                traceback.print_exc()

    def handleMsg(self, msg):
        try:
            assert msg[0] == '$', "invalid first char of message (!= $"

            try:
                handler, msgStart = self.COMMANDS[msg[1]]
                if msgStart == 0:
                    reply = handler()
                else:
                    reply = handler(msg[msgStart:])
                detach = 1 if msg[1] in self.DETACH_COMMANDS else 0
                return reply, detach
            except (KeyError, IndexError):
                self.log.error("Unknown RSP packet: %s", msg)
                return self.createRSPPacket(""), 0

        except Exception as e:
            self.log.error("Unhandled exception in handleMsg: %s", e)
            traceback.print_exc()
            return self.createRSPPacket("E01"), 0

    def detach(self, data):
        self.log.info("Client detached")
        resp = "OK"
        return self.createRSPPacket(resp)

    def kill(self):
        self.log.debug("GDB kill")
        # Keep target halted and leave vector catches if in persistent mode.
        if not self.persist:
            self.board.target.setVectorCatch(Target.CATCH_NONE)
            self.board.target.resume()
        return self.createRSPPacket("")

    def breakpoint(self, data):
        # handle breakpoint/watchpoint commands
        split = data.split('#')[0].split(',')
        addr = int(split[1], 16)
        self.log.debug("GDB breakpoint %s%d @ %x" % (data[0], int(data[1]), addr))

        # handle software breakpoint Z0/z0
        if data[1] == '0' and not self.soft_bkpt_as_hard:
            if data[0] == 'Z':
                if not self.target.setBreakpoint(addr, Target.BREAKPOINT_SW):
                    return self.createRSPPacket('E01') #EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware breakpoint Z1/z1
        if data[1] == '1' or (self.soft_bkpt_as_hard and data[1] == '0'):
            if data[0] == 'Z':
                if self.target.setBreakpoint(addr, Target.BREAKPOINT_HW) == False:
                    return self.createRSPPacket('E01') #EPERM
            else:
                self.target.removeBreakpoint(addr)
            return self.createRSPPacket("OK")

        # handle hardware watchpoint Z2/z2/Z3/z3/Z4/z4
        if data[1] == '2':
            # Write-only watch
            watchpoint_type = Target.WATCHPOINT_WRITE
        elif data[1] == '3':
            # Read-only watch
            watchpoint_type = Target.WATCHPOINT_READ
        elif data[1] == '4':
            # Read-Write watch
            watchpoint_type = Target.WATCHPOINT_READ_WRITE
        else:
            return self.createRSPPacket('E01') #EPERM

        size = int(split[2], 16)
        if data[0] == 'Z':
            if self.target.setWatchpoint(addr, size, watchpoint_type) == False:
                return self.createRSPPacket('E01') #EPERM
        else:
            self.target.removeWatchpoint(addr, size, watchpoint_type)
        return self.createRSPPacket("OK")

    def setThread(self, data):
        if not self.is_threading_enabled():
            return self.createRSPPacket('OK')

        self.log.debug("setThread:%s", data)
        op = data[0]
        thread_id = int(data[1:-3], 16)
        if not (thread_id in (0, -1) or self.thread_provider.is_valid_thread_id(thread_id)):
            return self.createRSPPacket('E01')

        if op == 'c':
            pass
        elif op == 'g':
            if thread_id == -1:
                self.target_facade.set_context(self.target_context)
            else:
                if thread_id == 0:
                    thread = self.thread_provider.current_thread
                    thread_id = thread.unique_id
                else:
                    thread = self.thread_provider.get_thread(thread_id)
                self.target_facade.set_context(thread.context)
        else:
            return self.createRSPPacket('E01')

        self.current_thread_id = thread_id
        return self.createRSPPacket('OK')

    def isThreadAlive(self, data):
        threadId = int(data[1:-3], 16)

        if self.is_threading_enabled():
            isAlive = self.thread_provider.is_valid_thread_id(threadId)
        else:
            isAlive = (threadId == 1)

        if isAlive:
            return self.createRSPPacket('OK')
        else:
            self.validateDebugContext()
            return self.createRSPPacket('E00')

    def validateDebugContext(self):
        if self.is_threading_enabled():
            currentThread = self.thread_provider.current_thread
            if self.current_thread_id != currentThread.unique_id:
                self.target_facade.set_context(currentThread.context)
                self.current_thread_id = currentThread.unique_id
        else:
            if self.current_thread_id != 1:
                self.log.debug("Current thread %x is no longer valid, switching context to target", self.current_thread_id)
                self.target_facade.set_context(self.target_context)
                self.current_thread_id = 1

    def stopReasonQuery(self):
        # In non-stop mode, if no threads are stopped we need to reply with OK.
        if self.non_stop and self.is_target_running:
            return self.createRSPPacket("OK")

        return self.createRSPPacket(self.getTResponse())

    def _get_resume_step_addr(self, data):
        if data is None:
            return None
        data = data.split('#')[0]
        if ';' not in data:
            return None
        # c[;addr]
        if data[0] in ('c', 's'):
            addr = int(data[2:], base=16)
        # Csig[;addr]
        elif data[0] in ('C', 'S'):
            addr = int(data[1:].split(';')[1], base=16)
        return addr

    def resume(self, data):
        addr = self._get_resume_step_addr(data)
        self.target.resume()
        self.log.debug("target resumed")

        val = ''

        while True:
            if self.shutdown_event.isSet():
                self.packet_io.interrupt_event.clear()
                return self.createRSPPacket(val)

            # Wait for a ctrl-c to be received.
            if self.packet_io.interrupt_event.wait(0.01):
                self.log.debug("receive CTRL-C")
                self.packet_io.interrupt_event.clear()
                self.target.halt()
                val = self.getTResponse(forceSignal=signals.SIGINT)
                break

            try:
                if self.target.getState() == Target.TARGET_HALTED:
                    # Handle semihosting
                    if self.enable_semihosting:
                        was_semihost = self.semihost.check_and_handle_semihost_request()

                        if was_semihost:
                            self.target.resume()
                            continue

                    pc = self.target_context.readCoreRegister('pc')
                    self.log.debug("state halted; pc=0x%08x", pc)
                    val = self.getTResponse()
                    break
            except Exception as e:
                try:
                    self.target.halt()
                except:
                    pass
                traceback.print_exc()
                self.log.debug('Target is unavailable temporarily.')
                val = 'S%02x' % self.target_facade.getSignalValue()
                break

        return self.createRSPPacket(val)

    def step(self, data):
        addr = self._get_resume_step_addr(data)
        self.log.debug("GDB step: %s", data)
        self.target.step(not self.step_into_interrupt)
        return self.createRSPPacket(self.getTResponse())

    def halt(self):
        self.target.halt()
        return self.createRSPPacket(self.getTResponse())

    def sendStopNotification(self, forceSignal=None):
        data = self.getTResponse(forceSignal=forceSignal)
        packet = '%Stop:' + data + '#' + checksum(data)
        self.packet_io.send(packet)

    def vCommand(self, data):
        cmd = data.split('#')[0]

        # Flash command.
        if cmd.startswith('Flash'):
            return self.flashOp(data)

        # vCont capabilities query.
        elif 'Cont?' == cmd:
            return self.createRSPPacket("vCont;c;C;s;S;t")

        # vCont, thread action command.
        elif cmd.startswith('Cont'):
            return self.vCont(cmd)

        # vStopped, part of thread stop state notification sequence.
        elif 'Stopped' in cmd:
            # Because we only support one thread for now, we can just reply OK to vStopped.
            return self.createRSPPacket("OK")

        return self.createRSPPacket("")

    # Example: $vCont;s:1;c#c1
    def vCont(self, cmd):
        ops = cmd.split(';')[1:] # split and remove 'Cont' from list
        if not ops:
            return self.createRSPPacket("OK")

        if self.is_threading_enabled():
            thread_actions = {}
            threads = self.thread_provider.get_threads()
            for k in threads:
                thread_actions[k.unique_id] = None
            currentThread = self.thread_provider.get_current_thread_id()
        else:
            thread_actions = { 1 : None } # our only thread
            currentThread = 1
        default_action = None

        for op in ops:
            args = op.split(':')
            action = args[0]
            if len(args) > 1:
                thread_id = int(args[1], 16)
                if thread_id == -1 or thread_id == 0:
                    thread_id = currentThread
                thread_actions[thread_id] = action
            else:
                default_action = action

        self.log.debug("thread_actions=%s; default_action=%s", repr(thread_actions), default_action)

        # Only the current thread is supported at the moment.
        if thread_actions[currentThread] is None:
            if default_action is None:
                return self.createRSPPacket('E01')
            thread_actions[currentThread] = default_action

        if thread_actions[currentThread][0] in ('c', 'C'):
            if self.non_stop:
                self.target.resume()
                self.is_target_running = True
                return self.createRSPPacket("OK")
            else:
                return self.resume(None)
        elif thread_actions[currentThread][0] in ('s', 'S'):
            if self.non_stop:
                self.target.step(not self.step_into_interrupt)
                self.packet_io.send(self.createRSPPacket("OK"))
                self.sendStopNotification()
                return None
            else:
                return self.step(None)
        elif thread_actions[currentThread] == 't':
            # Must ignore t command in all-stop mode.
            if not self.non_stop:
                return self.createRSPPacket("")
            self.packet_io.send(self.createRSPPacket("OK"))
            self.target.halt()
            self.is_target_running = False
            self.sendStopNotification(forceSignal=0)
        else:
            self.log.error("Unsupported vCont action '%s'" % thread_actions[1])

    def flashOp(self, data):
        ops = data.split(':')[0]
        self.log.debug("flash op: %s", ops)

        if ops == 'FlashErase':
            return self.createRSPPacket("OK")

        elif ops == 'FlashWrite':
            write_addr = int(data.split(':')[1], 16)
            self.log.debug("flash write addr: 0x%x", write_addr)
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ':':
                    second_colon += 1
                idx_begin += 1

            # Get flash builder if there isn't one already
            if self.flashBuilder == None:
                self.flashBuilder = self.flash.getFlashBuilder()

            # Add data to flash builder
            self.flashBuilder.addData(write_addr, self.unescape(data[idx_begin:len(data) - 3]))


            return self.createRSPPacket("OK")

        # we need to flash everything
        elif 'FlashDone' in ops :

            def print_progress(progress):
                # Reset state on 0.0
                if progress == 0.0:
                    print_progress.done = False

                # print progress bar
                if not print_progress.done:
                    sys.stdout.write('\r')
                    i = int(progress * 20.0)
                    sys.stdout.write("[%-20s] %3d%%" % ('=' * i, round(progress * 100)))
                    sys.stdout.flush()

                # Finish on 1.0
                if progress >= 1.0:
                    if not print_progress.done:
                        print_progress.done = True
                        sys.stdout.write("\r\n")
                        sys.stdout.flush()

            if self.hide_programming_progress:
                progress_cb = None
            else:
                 progress_cb = print_progress

            self.flashBuilder.program(chip_erase=self.chip_erase, progress_cb=progress_cb, fast_verify=self.fast_program)

            # Set flash builder to None so that on the next flash command a new
            # object is used.
            self.flashBuilder = None

            return self.createRSPPacket("OK")

        return None

    def unescape(self, data):
        data_idx = 0

        # unpack the data into binary array
        str_unpack = str(len(data)) + 'B'
        data = unpack(str_unpack, data)
        data = list(data)

        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7d:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1

        return data

    def escape(self, data):
        result = ''
        for c in data:
            if c in '#$}*':
                result += '}' + chr(ord(c) ^ 0x20)
            else:
                result += c
        return result

    def getMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = split[1].split('#')[0]
        length = int(length, 16)

        if LOG_MEM:
            self.log.debug("GDB getMem: addr=%x len=%x", addr, length)

        try:
            val = ''
            mem = self.target_context.readBlockMemoryUnaligned8(addr, length)
            # Flush so an exception is thrown now if invalid memory was accesses
            self.target_context.flush()
            for x in mem:
                if x >= 0x10:
                    val += hex(x)[2:4]
                else:
                    val += '0' + hex(x)[2:3]
        except DAPAccess.TransferError:
            self.log.debug("getMemory failed at 0x%x" % addr)
            val = 'E01' #EPERM
        return self.createRSPPacket(val)

    def writeMemoryHex(self, data):
        split = data.split(',')
        addr = int(split[0], 16)

        split = split[1].split(':')
        length = int(split[0], 16)

        split = split[1].split('#')
        data = hexToByteList(split[0])

        if LOG_MEM:
            self.log.debug("GDB writeMemHex: addr=%x len=%x", addr, length)

        try:
            if length > 0:
                self.target_context.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target_context.flush()
            resp = "OK"
        except DAPAccess.TransferError:
            self.log.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01' #EPERM

        return self.createRSPPacket(resp)

    def writeMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = int(split[1].split(':')[0], 16)

        if LOG_MEM:
            self.log.debug("GDB writeMem: addr=%x len=%x", addr, length)

        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ':':
                idx_begin += 1
                break
            idx_begin += 1

        data = data[idx_begin:len(data) - 3]
        data = self.unescape(data)

        try:
            if length > 0:
                self.target_context.writeBlockMemoryUnaligned8(addr, data)
                # Flush so an exception is thrown now if invalid memory was accessed
                self.target_context.flush()
            resp = "OK"
        except DAPAccess.TransferError:
            self.log.debug("writeMemory failed at 0x%x" % addr)
            resp = 'E01' #EPERM

        return self.createRSPPacket(resp)

    def readRegister(self, which):
        return self.createRSPPacket(self.target_facade.gdbGetRegister(which))

    def writeRegister(self, data):
        reg = int(data.split('=')[0], 16)
        val = data.split('=')[1].split('#')[0]
        self.target_facade.setRegister(reg, val)
        return self.createRSPPacket("OK")

    def getRegisters(self):
        return self.createRSPPacket(self.target_facade.getRegisterContext())

    def setRegisters(self, data):
        self.target_facade.setRegisterContext(data)
        return self.createRSPPacket("OK")

    def handleQuery(self, msg):
        query = msg.split(':')
        self.log.debug('GDB received query: %s', query)

        if query is None:
            self.log.error('GDB received query packet malformed')
            return None

        if query[0] == 'Supported':
            # Save features sent by gdb.
            self.gdb_features = query[1].split(';')

            # Build our list of features.
            features = ['qXfer:features:read+', 'QStartNoAckMode+', 'qXfer:threads:read+', 'QNonStop+']
            features.append('PacketSize=' + hex(self.packet_size)[2:])
            if self.target_facade.getMemoryMapXML() is not None:
                features.append('qXfer:memory-map:read+')
            resp = ';'.join(features)
            return self.createRSPPacket(resp)

        elif query[0] == 'Xfer':

            if query[1] == 'features' and query[2] == 'read' and \
               query[3] == 'target.xml':
                data = query[4].split(',')
                resp = self.handleQueryXML('read_feature', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == 'memory-map' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('memory_map', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == 'threads' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('threads', int(data[0], 16), int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            else:
                self.log.debug("Unsupported qXfer request: %s:%s:%s:%s", query[1], query[2], query[3], query[4])
                return None

        elif query[0].startswith('C'):
            if not self.is_threading_enabled():
                return self.createRSPPacket("QC1")
            else:
                self.validateDebugContext()
                return self.createRSPPacket("QC%x" % self.current_thread_id)

        elif query[0].find('Attached') != -1:
            return self.createRSPPacket("1")

        elif query[0].find('TStatus') != -1:
            return self.createRSPPacket("")

        elif query[0].find('Tf') != -1:
            return self.createRSPPacket("")

        elif 'Offsets' in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)

        elif 'Symbol' in query[0]:
            if self.did_init_thread_providers:
                return self.createRSPPacket("OK")
            return self.initThreadProviders()

        elif query[0].startswith('Rcmd,'):
            cmd = hexDecode(query[0][5:].split('#')[0])
            return self.handleRemoteCommand(cmd)

        else:
            return self.createRSPPacket("")

    def initThreadProviders(self):
        symbol_provider = GDBSymbolProvider(self)

        for rtosName, rtosClass in RTOS.iteritems():
            try:
                self.log.info("Attempting to load %s", rtosName)
                rtos = rtosClass(self.target)
                if rtos.init(symbol_provider):
                    self.log.info("%s loaded successfully", rtosName)
                    self.thread_provider = rtos
                    break
            except RuntimeError as e:
                self.log.error("Error during symbol lookup: " + str(e))
                traceback.print_exc()

        self.did_init_thread_providers = True

        # Done with symbol processing.
        return self.createRSPPacket("OK")

    def getSymbol(self, name):
        # Send the symbol request.
        request = self.createRSPPacket('qSymbol:' + hexEncode(name))
        self.packet_io.send(request)

        # Read a packet.
        packet = self.packet_io.receive()

        # Parse symbol value reply packet.
        packet = packet[1:-3]
        if not packet.startswith('qSymbol:'):
            raise RuntimeError("Got unexpected response from gdb when asking for symbol value")
        packet = packet[8:]
        symValue, symName = packet.split(':')

        symName = hexDecode(symName)
        if symName != name:
            raise RuntimeError("Symbol value reply from gdb has unexpected symbol name")
        if symValue:
            symValue = hex8leToU32le(symValue)
        else:
            return None
        return symValue

    # TODO rewrite the remote command handler
    def handleRemoteCommand(self, cmd):
        self.log.debug('Remote command: %s', cmd)

        safecmd = {
            'init'  : ['Init reset sequence', 0x1],
            'reset' : ['Reset and halt the target', 0x2],
            'halt'  : ['Halt target', 0x4],
            # 'resume': ['Resume target', 0x8],
            'help'  : ['Display this help', 0x80],
        }

        cmdList = cmd.split()
        resp = 'OK'
        if cmd == 'help':
            resp = ''.join(['%s\t%s\n' % (k, v[0]) for k, v in safecmd.items()])
            resp = hexEncode(resp)
        elif cmd.startswith('arm semihosting'):
            self.enable_semihosting = 'enable' in cmd
            self.log.info("Semihosting %s", ('enabled' if self.enable_semihosting else 'disabled'))
        elif cmdList[0] == 'set':
            if len(cmdList) < 3:
                resp = hexEncode("Error: invalid set command")
            elif cmdList[1] == 'vector-catch':
                try:
                    self.board.target.setVectorCatch(convert_vector_catch(cmdList[2]))
                except ValueError as e:
                    resp = hexEncode("Error: " + str(e))
            elif cmdList[1] == 'step-into-interrupt':
                self.step_into_interrupt = (cmdList[2].lower() in ("true", "on", "yes", "1"))
            else:
                resp = hexEncode("Error: invalid set option")
        else:
            resultMask = 0x00
            if cmdList[0] == 'help':
                # a 'help' is only valid as the first cmd, and only
                # gives info on the second cmd if it is valid
                resultMask |= 0x80
                del cmdList[0]

            for cmd_sub in cmdList:
                if cmd_sub not in safecmd:
                    self.log.warning("Invalid mon command '%s'", cmd_sub)
                    resp = 'Invalid Command: "%s"\n' % cmd_sub
                    resp = hexEncode(resp)
                    return self.createRSPPacket(resp)
                elif resultMask == 0x80:
                    # if the first command was a 'help', we only need
                    # to return info about the first cmd after it
                    resp = hexEncode(safecmd[cmd_sub][0]+'\n')
                    return self.createRSPPacket(resp)
                resultMask |= safecmd[cmd_sub][1]

            # Run cmds in proper order
            if resultMask & 0x1:
                pass
            if (resultMask & 0x6) == 0x6:
                self.target.resetStopOnReset()
            elif resultMask & 0x2:
                # on 'reset' still do a reset halt
                self.target.resetStopOnReset()
                # self.target.reset()
            elif resultMask & 0x4:
                self.target.halt()
            # if resultMask & 0x8:
            #     self.target.resume()

        return self.createRSPPacket(resp)

    def handleGeneralSet(self, msg):
        feature = msg.split('#')[0]
        self.log.debug("GDB general set: %s", feature)

        if feature == 'StartNoAckMode':
            # Disable acks after the reply and ack.
            self.packet_io.set_send_acks(False)
            return self.createRSPPacket("OK")

        elif feature.startswith('NonStop'):
            enable = feature.split(':')[1]
            self.non_stop = (enable == '1')
            return self.createRSPPacket("OK")

        else:
            return self.createRSPPacket("")

    def handleQueryXML(self, query, offset, size):
        self.log.debug('GDB query %s: offset: %s, size: %s', query, offset, size)
        xml = ''
        if query == 'memory_map':
            xml = self.target_facade.getMemoryMapXML()
        elif query == 'read_feature':
            xml = self.target.getTargetXML()
        elif query == 'threads':
            xml = self.getThreadsXML()
        else:
            raise RuntimeError("Invalid XML query (%s)" % query)

        size_xml = len(xml)

        prefix = 'm'

        if offset > size_xml:
            self.log.error('GDB: xml offset > size for %s!', query)
            return

        if size > (self.packet_size - 4):
            size = self.packet_size - 4

        nbBytesAvailable = size_xml - offset

        if size > nbBytesAvailable:
            prefix = 'l'
            size = nbBytesAvailable

        resp = prefix + self.escape(xml[offset:offset + size])

        return resp


    def createRSPPacket(self, data):
        resp = '$' + data + '#' + checksum(data)
        return resp

    def syscall(self, op):
        self.log.debug("GDB server syscall: %s", op)
        request = self.createRSPPacket('F' + op)
        self.packet_io.send(request)

        while not self.packet_io.interrupt_event.is_set():
            # Read a packet.
            packet = self.packet_io.receive(False)
            if packet is None:
                sleep(0.1)
                continue

            # Check for file I/O response.
            if packet[0] == '$' and packet[1] == 'F':
                self.log.debug("Syscall: got syscall response " + packet)
                args = packet[2:packet.index('#')].split(',')
                result = int(args[0], base=16)
                errno = int(args[1], base=16) if len(args) > 1 else 0
                ctrl_c = args[2] if len(args) > 2 else ''
                if ctrl_c == 'C':
                    self.packet_io.interrupt_event.set()
                    self.packet_io.drop_reply = True
                return result, errno

            # decode and prepare resp
            resp, detach = self.handleMsg(packet)

            if resp is not None:
                # send resp
                self.packet_io.send(resp)

            if detach:
                self.detach_event.set()
                self.log.warning("GDB server received detach request while waiting for file I/O completion")
                break

        return -1, 0

    def getTResponse(self, forceSignal=None):
        self.validateDebugContext()
        response = self.target_facade.getTResponse(forceSignal)

        # Append thread and core
        if not self.is_threading_enabled():
            response += "thread:1;core:0;"
        else:
            if self.current_thread_id in (-1, 0, 1):
                response += "thread:%x;core:0;" % self.thread_provider.current_thread.unique_id
            else:
                response += "thread:%x;core:0;" % self.current_thread_id
        self.log.debug("Tresponse=%s", response)
        return response

    def getThreadsXML(self):
        root = Element('threads')

        if not self.is_threading_enabled():
            t = SubElement(root, 'thread', id="1", core="0")
            t.text = "Thread mode"
        else:
            threads = self.thread_provider.get_threads()
            for thread in threads:
                hexId = "%x" % thread.unique_id
                t = SubElement(root, 'thread', id=hexId, core="0")

                desc = thread.description
                if desc:
                    desc = thread.name + "; " + desc
                else:
                    desc = thread.name
                t.text = desc

        return '<?xml version="1.0"?><!DOCTYPE feature SYSTEM "threads.dtd">' + tostring(root)

    def is_threading_enabled(self):
        return (self.thread_provider is not None) and self.thread_provider.is_enabled \
            and (self.thread_provider.current_thread is not None)
Пример #12
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """

    def __init__(self, board, port_urlWSS):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.packet_size = 2048
        self.flashData = list()
        self.conn = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        self.quit = False
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)
        self.setDaemon(True)
        self.start()

    def restart(self):
        if self.isAlive():
            self.detach_event.set()

    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            while self.isAlive():
                pass
            logging.info("GDB server thread killed")
        self.board.uninit()

    def setBoard(self, board, stop=True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return

    def run(self):
        while True:
            new_command = False
            data = ""
            logging.info("GDB server started")

            self.shutdown_event.clear()
            self.detach_event.clear()

            while not self.shutdown_event.isSet() and not self.detach_event.isSet():
                connected = self.abstract_socket.connect()
                if connected != None:
                    break

            if self.shutdown_event.isSet():
                return

            if self.detach_event.isSet():
                continue

            logging.info("One client connected!")

            while True:

                if self.shutdown_event.isSet():
                    return

                if self.detach_event.isSet():
                    continue

                # read command
                while True:
                    if new_command == True:
                        new_command = False
                        break
                    try:
                        if self.shutdown_event.isSet() or self.detach_event.isSet():
                            break
                        self.abstract_socket.setBlocking(0)
                        data += self.abstract_socket.read()
                        if data.index("$") >= 0 and data.index("#") >= 0:
                            break
                    except (ValueError, socket.error):
                        pass

                if self.shutdown_event.isSet():
                    return

                if self.detach_event.isSet():
                    continue

                self.abstract_socket.setBlocking(1)

                data = data[data.index("$") :]

                self.lock.acquire()

                if len(data) != 0:
                    # decode and prepare resp
                    [resp, ack, detach] = self.handleMsg(data)

                    if resp is not None:
                        # ack
                        if ack:
                            resp = "+" + resp
                        # send resp
                        self.abstract_socket.write(resp)
                        # wait a '+' from the client
                        try:
                            data = self.abstract_socket.read()
                            if data[0] != "+":
                                logging.debug("gdb client has not ack!")
                            else:
                                logging.debug("gdb client has ack!")
                            if data.index("$") >= 0 and data.index("#") >= 0:
                                new_command = True
                        except:
                            pass

                    if detach:
                        self.abstract_socket.close()
                        self.lock.release()
                        break

                self.lock.release()

    def handleMsg(self, msg):

        if msg[0] != "$":
            logging.debug("msg ignored: first char != $")
            return None, 0, 0

        # logging.debug('-->>>>>>>>>>>> GDB rsp packet: %s', msg)

        # query command
        if msg[1] == "q":
            return self.handleQuery(msg[2:]), 1, 0

        elif msg[1] == "H":
            return self.createRSPPacket(""), 1, 0

        elif msg[1] == "?":
            return self.lastSignal(), 1, 0

        elif msg[1] == "g":
            return self.getRegister(), 1, 0

        elif msg[1] == "p":
            return self.readRegister(msg[2:]), 1, 0

        elif msg[1] == "P":
            return self.writeRegister(msg[2:]), 1, 0

        elif msg[1] == "m":
            return self.getMemory(msg[2:]), 1, 0

        elif msg[1] == "X":
            return self.writeMemory(msg[2:]), 1, 0

        elif msg[1] == "v":
            return self.flashOp(msg[2:]), 1, 0

        # we don't send immediately the response for C and S commands
        elif msg[1] == "C" or msg[1] == "c":
            return self.resume()

        elif msg[1] == "S" or msg[1] == "s":
            return self.step()

        elif msg[1] == "Z" or msg[1] == "z":
            return self.breakpoint(msg[1:]), 1, 0

        elif msg[1] == "D":
            return self.detach(msg[1:]), 1, 1

        elif msg[1] == "k":
            return self.kill(), 1, 1

        else:
            logging.error("Unknown RSP packet: %s", msg)
            return self.createRSPPacket("")

    def detach(self, data):
        resp = "OK"
        return self.createRSPPacket(resp)

    def kill(self):
        return self.createRSPPacket("")

    def breakpoint(self, data):
        # handle Z1/z1 commands
        addr = int(data.split(",")[1], 16)
        if data[1] == "1":
            if data[0] == "Z":
                if self.target.setBreakpoint(addr) == False:
                    resp = "ENN"
                    return self.createRSPPacket(resp)
            else:
                self.target.removeBreakpoint(addr)
            resp = "OK"
            return self.createRSPPacket(resp)

        return None

    def resume(self):
        self.ack()
        self.target.resume()
        self.abstract_socket.setBlocking(0)

        # Try to set break point at hardfault handler to avoid
        # halting target constantly
        if self.target.availableBreakpoint() >= 1:
            bpSet = True
            hardfault_handler = self.target.readMemory(4 * 3)
            self.target.setBreakpoint(hardfault_handler)
        else:
            bpSet = False
            logging.info("No breakpoint available. Interfere target constantly.")

        val = ""

        while True:
            sleep(0.01)

            try:
                data = self.abstract_socket.read()
                if data[0] == "\x03":
                    self.target.halt()
                    val = "S05"
                    logging.debug("receive CTRL-C")
                    break
            except:
                pass

            if self.target.getState() == TARGET_HALTED:
                logging.debug("state halted")
                xpsr = self.target.readCoreRegister("xpsr")
                # Get IPSR value from XPSR
                if (xpsr & 0x1F) == 3:
                    val = "S" + FAULT[3]
                else:
                    val = "S05"
                break

            if not bpSet:
                # Only do this when no bp available as it slows resume operation
                self.target.halt()
                xpsr = self.target.readCoreRegister("xpsr")
                logging.debug("GDB resume xpsr: 0x%X", xpsr)
                # Get IPSR value from XPSR
                if (xpsr & 0x1F) == 3:
                    val = "S" + FAULT[3]
                    break
                self.target.resume()

        if bpSet:
            self.target.removeBreakpoint(hardfault_handler)

        self.abstract_socket.setBlocking(1)
        return self.createRSPPacket(val), 0, 0

    def step(self):
        self.ack()
        self.target.step()
        return self.createRSPPacket("S05"), 0, 0

    def halt(self):
        self.ack()
        self.target.halt()
        return self.createRSPPacket("S05"), 0, 0

    def flashOp(self, data):
        ops = data.split(":")[0]
        # logging.debug("flash op: %s", ops)

        if ops == "FlashErase":
            self.flash.init()
            self.flash.eraseAll()
            return self.createRSPPacket("OK")

        elif ops == "FlashWrite":
            write_addr = int(data.split(":")[1], 16)
            logging.debug("flash write addr: 0x%s", write_addr)
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ":":
                    second_colon += 1
                idx_begin += 1

            # if there's gap between sections, fill it
            flash_watermark = len(self.flashData)
            pad_size = write_addr - flash_watermark
            if pad_size > 0:
                self.flashData += [0xFF] * pad_size

            # append the new data if it doesn't overlap existing data
            if write_addr >= flash_watermark:
                self.flashData += self.unescape(data[idx_begin : len(data) - 3])
            else:
                logging.error(
                    "Invalid FlashWrite address %d overlaps current data of size %d", write_addr, flash_watermark
                )

            return self.createRSPPacket("OK")

        # we need to flash everything
        elif "FlashDone" in ops:
            flashPtr = 0
            bytes_to_be_written = len(self.flashData)

            """
            bin = open(os.path.join(parentdir, 'res', 'bad_bin.txt'), "w+")
            
            i = 0
            while (i < bytes_to_be_written):
                bin.write(str(self.flashData[i:i+16]) + "\n")
                i += 16
            """

            logging.info("flashing %d bytes", bytes_to_be_written)

            while len(self.flashData) > 0:
                size_to_write = min(self.flash.page_size, len(self.flashData))
                # if 0 is returned from programPage, security check failed
                if self.flash.programPage(flashPtr, self.flashData[:size_to_write]) == 0:
                    logging.error("Protection bits error, flashing has stopped")
                    return None
                flashPtr += size_to_write

                self.flashData = self.flashData[size_to_write:]

                # print progress bar
                sys.stdout.write("\r")
                i = int((float(flashPtr) / float(bytes_to_be_written)) * 20.0)
                # the exact output you're looking for:
                sys.stdout.write("[%-20s] %d%%" % ("=" * i, 5 * i))
                sys.stdout.flush()

            sys.stdout.write("\n\r")

            self.flashData = []

            """
            bin.close()
            """

            # reset and stop on reset handler
            self.target.resetStopOnReset()

            return self.createRSPPacket("OK")

        elif "Cont" in ops:
            if "Cont?" in ops:
                return self.createRSPPacket("vCont;c;s;t")

        return None

    def unescape(self, data):
        data_idx = 0

        # unpack the data into binary array
        str_unpack = str(len(data)) + "B"
        data = unpack(str_unpack, data)
        data = list(data)

        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7D:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1

        return data

    def getMemory(self, data):
        split = data.split(",")
        addr = int(split[0], 16)
        length = split[1]
        length = int(length[: len(length) - 3], 16)

        val = ""

        mem = self.target.readBlockMemoryUnaligned8(addr, length)
        for x in mem:
            if x >= 0x10:
                val += hex(x)[2:4]
            else:
                val += "0" + hex(x)[2:3]

        return self.createRSPPacket(val)

    def writeMemory(self, data):
        split = data.split(",")
        addr = int(split[0], 16)
        length = int(split[1].split(":")[0], 16)

        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ":":
                idx_begin += 1
                break
            idx_begin += 1

        data = data[idx_begin : len(data) - 3]
        data = self.unescape(data)

        if length > 0:
            self.target.writeBlockMemoryUnaligned8(addr, data)

        return self.createRSPPacket("OK")

    def readRegister(self, data):
        num = int(data.split("#")[0], 16)
        reg = self.target.readCoreRegister(num)
        logging.debug("GDB: read reg %d: 0x%X", num, reg)
        val = self.intToHexGDB(reg)
        return self.createRSPPacket(val)

    def writeRegister(self, data):
        num = int(data.split("=")[0], 16)
        val = data.split("=")[1].split("#")[0]
        val = val[6:8] + val[4:6] + val[2:4] + val[0:2]
        logging.debug("GDB: write reg %d: 0x%X", num, int(val, 16))
        self.target.writeCoreRegister(num, int(val, 16))
        return self.createRSPPacket("OK")

    def intToHexGDB(self, val):
        val = hex(int(val))[2:]
        size = len(val)
        r = ""
        for i in range(8 - size):
            r += "0"
        r += str(val)

        resp = ""
        for i in range(4):
            resp += r[8 - 2 * i - 2 : 8 - 2 * i]

        return resp

    def getRegister(self):
        resp = ""
        # only core registers are printed
        for i in sorted(CORE_REGISTER.values())[4:20]:
            reg = self.target.readCoreRegister(i)
            resp += self.intToHexGDB(reg)
            logging.debug("GDB reg: %s = 0x%X", self.target.getRegisterName(i), reg)
        return self.createRSPPacket(resp)

    def lastSignal(self):
        fault = self.target.readCoreRegister("xpsr") & 0xFF
        try:
            fault = FAULT[fault]
        except:
            # Values above 16 are for interrupts
            fault = "17"  # SIGSTOP
            pass
        logging.debug("GDB lastSignal: %s", fault)
        return self.createRSPPacket("S" + fault)

    def handleQuery(self, msg):
        query = msg.split(":")
        logging.debug("GDB received query: %s", query)

        if query is None:
            logging.error("GDB received query packet malformed")
            return None

        if query[0] == "Supported":
            resp = "qXfer:memory-map:read+;qXfer:features:read+;PacketSize="
            resp += hex(self.packet_size)[2:]
            return self.createRSPPacket(resp)

        elif query[0] == "Xfer":

            if query[1] == "features" and query[2] == "read" and query[3] == "target.xml":
                data = query[4].split(",")
                resp = self.handleQueryXML("read_feature", int(data[0], 16), int(data[1].split("#")[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == "memory-map" and query[2] == "read":
                data = query[4].split(",")
                resp = self.handleQueryXML("memory_map", int(data[0], 16), int(data[1].split("#")[0], 16))
                return self.createRSPPacket(resp)

            else:
                return None

        elif query[0] == "C#b4":
            return self.createRSPPacket("")

        elif query[0].find("Attached") != -1:
            return self.createRSPPacket("1")

        elif query[0].find("TStatus") != -1:
            return self.createRSPPacket("")

        elif query[0].find("Tf") != -1:
            return self.createRSPPacket("")

        elif "Offsets" in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)

        elif "Symbol" in query[0]:
            resp = "OK"
            return self.createRSPPacket(resp)

        else:
            return self.createRSPPacket("")

    def handleQueryXML(self, query, offset, size):
        logging.debug("GDB query %s: offset: %s, size: %s", query, offset, size)
        xml = ""
        if query == "memory_map":
            xml = self.target.memoryMapXML
        elif query == "read_feature":
            xml = self.target.targetXML

        size_xml = len(xml)

        prefix = "m"

        if offset > size_xml:
            logging.error("GDB: offset target.xml > size!")
            return

        if size > (self.packet_size - 4):
            size = self.packet_size - 4

        nbBytesAvailable = size_xml - offset

        if size > nbBytesAvailable:
            prefix = "l"
            size = nbBytesAvailable

        resp = prefix + xml[offset : offset + size]

        return resp

    def createRSPPacket(self, data):
        resp = "$" + data + "#"

        c = 0
        checksum = 0
        for c in data:
            checksum += ord(c)
        checksum = checksum % 256
        checksum = hex(checksum)

        if int(checksum[2:], 16) < 0x10:
            resp += "0"
        resp += checksum[2:]

        # logging.debug('--<<<<<<<<<<<< GDB rsp packet: %s', resp)
        return resp

    def ack(self):
        self.abstract_socket.write("+")
Пример #13
0
class GDBServer(threading.Thread):
    """
    This class start a GDB server listening a gdb connection on a specific port.
    It implements the RSP (Remote Serial Protocol).
    """
    def __init__(self, board, port_urlWSS):
        threading.Thread.__init__(self)
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.abstract_socket = None
        self.wss_server = None
        self.port = 0
        if isinstance(port_urlWSS, str) == True:
            self.wss_server = port_urlWSS
        else:
            self.port = port_urlWSS
        self.packet_size = 2048
        self.flashData = ""
        self.conn = None
        self.lock = threading.Lock()
        self.shutdown_event = threading.Event()
        self.detach_event = threading.Event()
        self.quit = False
        if self.wss_server == None:
            self.abstract_socket = GDBSocket(self.port, self.packet_size)
        else:
            self.abstract_socket = GDBWebSocket(self.wss_server)
        self.start()

    def restart(self):
        if self.isAlive():
            self.detach_event.set()

    def stop(self):
        if self.isAlive():
            self.shutdown_event.set()
            while self.isAlive():
                pass
            logging.info("GDB server thread killed")
        self.board.uninit()

    def setBoard(self, board, stop=True):
        self.lock.acquire()
        if stop:
            self.restart()
        self.board = board
        self.target = board.target
        self.flash = board.flash
        self.lock.release()
        return

    def run(self):
        while True:
            new_command = False
            data = []
            logging.info('GDB server started')

            self.shutdown_event.clear()
            self.detach_event.clear()

            while not self.shutdown_event.isSet(
            ) and not self.detach_event.isSet():
                connected = self.abstract_socket.connect()
                if connected != None:
                    break

            if self.shutdown_event.isSet():
                return

            if self.detach_event.isSet():
                continue

            logging.info("One client connected!")

            while True:

                if self.shutdown_event.isSet():
                    return

                if self.detach_event.isSet():
                    continue

                # read command
                while True:
                    if (new_command == True):
                        new_command = False
                        break
                    try:
                        if self.shutdown_event.isSet(
                        ) or self.detach_event.isSet():
                            break
                        self.abstract_socket.setBlocking(0)
                        data = self.abstract_socket.read()
                        if data.index("$") >= 0 and data.index("#") >= 0:
                            break
                    except (ValueError, socket.error):
                        pass

                if self.shutdown_event.isSet():
                    return

                if self.detach_event.isSet():
                    continue

                self.abstract_socket.setBlocking(1)

                data = data[data.index("$"):]

                self.lock.acquire()

                if len(data) != 0:
                    # decode and prepare resp
                    [resp, ack, detach] = self.handleMsg(data)

                    if resp is not None:
                        # ack
                        if ack:
                            resp = "+" + resp
                        # send resp
                        self.abstract_socket.write(resp)
                        # wait a '+' from the client
                        try:
                            data = self.abstract_socket.read()
                            if data[0] != '+':
                                logging.debug('gdb client has not ack!')
                            else:
                                logging.debug('gdb client has ack!')
                            if data.index("$") >= 0 and data.index("#") >= 0:
                                new_command = True
                        except:
                            pass

                    if detach:
                        self.abstract_socket.close()
                        self.lock.release()
                        break

                self.lock.release()

    def handleMsg(self, msg):

        if msg[0] != '$':
            logging.debug('msg ignored: first char != $')
            return None, 0, 0

        #logging.debug('-->>>>>>>>>>>> GDB rsp packet: %s', msg)

        # query command
        if msg[1] == 'q':
            return self.handleQuery(msg[2:]), 1, 0

        elif msg[1] == 'H':
            return self.createRSPPacket(''), 1, 0

        elif msg[1] == '?':
            return self.lastSignal(), 1, 0

        elif msg[1] == 'g':
            return self.getRegister(), 1, 0

        elif msg[1] == 'p':
            return self.readRegister(msg[2:]), 1, 0

        elif msg[1] == 'P':
            return self.writeRegister(msg[2:]), 1, 0

        elif msg[1] == 'm':
            return self.getMemory(msg[2:]), 1, 0

        elif msg[1] == 'X':
            return self.writeMemory(msg[2:]), 1, 0

        elif msg[1] == 'v':
            return self.flashOp(msg[2:]), 1, 0

        # we don't send immediately the response for C and S commands
        elif msg[1] == 'C' or msg[1] == 'c':
            return self.resume()

        elif msg[1] == 'S' or msg[1] == 's':
            return self.step()

        elif msg[1] == 'Z' or msg[1] == 'z':
            return self.breakpoint(msg[1:]), 1, 0

        elif msg[1] == 'D':
            return self.detach(msg[1:]), 1, 1

        elif msg[1] == 'k':
            return self.kill(), 1, 1

        else:
            logging.error("Unknown RSP packet: %s", msg)
            return None

    def detach(self, data):
        resp = "OK"
        return self.createRSPPacket(resp)

    def kill(self):
        return self.createRSPPacket("")

    def breakpoint(self, data):
        # handle Z1/z1 commands
        addr = int(data.split(',')[1], 16)
        if data[1] == '1':
            if data[0] == 'Z':
                if self.target.setBreakpoint(addr) == False:
                    resp = "ENN"
                    return self.createRSPPacket(resp)
            else:
                self.target.removeBreakpoint(addr)
            resp = "OK"
            return self.createRSPPacket(resp)

        return None

    def resume(self):
        self.ack()
        self.target.resume()
        self.abstract_socket.setBlocking(0)

        val = ''

        while True:
            sleep(0.01)

            try:
                data = self.abstract_socket.read()
                if (data[0] == '\x03'):
                    self.target.halt()
                    val = 'S05'
                    logging.debug("receive CTRL-C")
                    break
            except:
                pass

            if self.target.getState() == TARGET_HALTED:
                logging.debug("state halted")
                val = 'S05'
                break

            self.target.halt()
            ipsr = self.target.readCoreRegister('xpsr')
            logging.debug("GDB resume xpsr: 0x%X", ipsr)
            if (ipsr & 0x1f) == 3:
                val = "S" + FAULT[3]
                break
            self.target.resume()

        self.abstract_socket.setBlocking(1)
        return self.createRSPPacket(val), 0, 0

    def step(self):
        self.ack()
        self.target.step()
        return self.createRSPPacket("S05"), 0, 0

    def halt(self):
        self.ack()
        self.target.halt()
        return self.createRSPPacket("S05"), 0, 0

    def flashOp(self, data):
        ops = data.split(':')[0]
        #logging.debug("flash op: %s", ops)

        if ops == 'FlashErase':
            self.flash.init()
            self.flash.eraseAll()
            return self.createRSPPacket("OK")

        elif ops == 'FlashWrite':
            logging.debug("flash write addr: 0x%s", data.split(':')[1])
            # search for second ':' (beginning of data encoded in the message)
            second_colon = 0
            idx_begin = 0
            while second_colon != 2:
                if data[idx_begin] == ':':
                    second_colon += 1
                idx_begin += 1

            self.flashData += data[idx_begin:len(data) - 3]
            return self.createRSPPacket("OK")

        # we need to flash everything
        elif 'FlashDone' in ops:
            flashPtr = 0

            unescaped_data = self.unescape(self.flashData)

            bytes_to_be_written = len(unescaped_data)
            """
            bin = open(os.path.join(parentdir, 'res', 'bad_bin.txt'), "w+")
            
            i = 0
            while (i < bytes_to_be_written):
                bin.write(str(unescaped_data[i:i+16]) + "\n")
                i += 16
            """

            logging.info("flashing %d bytes", bytes_to_be_written)

            while len(unescaped_data) > 0:
                size_to_write = min(self.flash.page_size, len(unescaped_data))
                self.flash.programPage(flashPtr,
                                       unescaped_data[:size_to_write])
                flashPtr += size_to_write

                unescaped_data = unescaped_data[size_to_write:]

                # print progress bar
                sys.stdout.write('\r')
                i = int((float(flashPtr) / float(bytes_to_be_written)) * 20.0)
                # the exact output you're looking for:
                sys.stdout.write("[%-20s] %d%%" % ('=' * i, 5 * i))
                sys.stdout.flush()

            sys.stdout.write("\n\r")

            self.flashData = ""
            """
            bin.close()
            """

            # reset and stop on reset handler
            self.target.resetStopOnReset()

            return self.createRSPPacket("OK")

        elif 'Cont' in ops:
            if 'Cont?' in ops:
                return self.createRSPPacket("vCont;c;s;t")

        return None

    def unescape(self, data):
        data_idx = 0

        # unpack the data into binary array
        str_unpack = str(len(data)) + 'B'
        data = unpack(str_unpack, data)
        data = list(data)

        # check for escaped characters
        while data_idx < len(data):
            if data[data_idx] == 0x7d:
                data.pop(data_idx)
                data[data_idx] = data[data_idx] ^ 0x20
            data_idx += 1

        return data

    def getMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = split[1]
        length = int(length[:len(length) - 3], 16)

        val = ''

        mem = self.target.readBlockMemoryUnaligned8(addr, length)
        for x in mem:
            if x >= 0x10:
                val += hex(x)[2:4]
            else:
                val += '0' + hex(x)[2:3]

        return self.createRSPPacket(val)

    def writeMemory(self, data):
        split = data.split(',')
        addr = int(split[0], 16)
        length = int(split[1].split(':')[0], 16)

        idx_begin = 0
        for i in range(len(data)):
            if data[i] == ':':
                idx_begin += 1
                break
            idx_begin += 1

        data = data[idx_begin:len(data) - 3]
        data = self.unescape(data)

        if length > 0:
            self.target.writeBlockMemoryUnaligned8(addr, data)

        return self.createRSPPacket("OK")

    def readRegister(self, data):
        num = int(data.split('#')[0], 16)
        reg = self.target.readCoreRegister(num)
        logging.debug("GDB: read reg %d: 0x%X", num, reg)
        val = self.intToHexGDB(reg)
        return self.createRSPPacket(val)

    def writeRegister(self, data):
        num = int(data.split('=')[0], 16)
        val = data.split('=')[1].split('#')[0]
        val = val[6:8] + val[4:6] + val[2:4] + val[0:2]
        logging.debug("GDB: write reg %d: 0x%X", num, int(val, 16))
        self.target.writeCoreRegister(num, int(val, 16))
        return self.createRSPPacket("OK")

    def intToHexGDB(self, val):
        val = hex(int(val))[2:]
        size = len(val)
        r = ''
        for i in range(8 - size):
            r += '0'
        r += str(val)

        resp = ''
        for i in range(4):
            resp += r[8 - 2 * i - 2:8 - 2 * i]

        return resp

    def getRegister(self):
        resp = ''
        for i in range(len(CORE_REGISTER)):
            reg = self.target.readCoreRegister(i)
            resp += self.intToHexGDB(reg)
            logging.debug("GDB reg: %s = 0x%X", i, reg)
        return self.createRSPPacket(resp)

    def lastSignal(self):
        fault = self.target.readCoreRegister('xpsr') & 0xff
        fault = FAULT[fault]
        logging.debug("GDB lastSignal: %s", fault)
        return self.createRSPPacket('S' + fault)

    def handleQuery(self, msg):
        query = msg.split(':')
        logging.debug('GDB received query: %s', query)

        if query is None:
            logging.error('GDB received query packet malformed')
            return None

        if query[0] == 'Supported':
            resp = "qXfer:memory-map:read+;qXfer:features:read+;PacketSize="
            resp += hex(self.packet_size)[2:]
            return self.createRSPPacket(resp)

        elif query[0] == 'Xfer':

            if query[1] == 'features' and query[2] == 'read' and \
               query[3] == 'target.xml':
                data = query[4].split(',')
                resp = self.handleQueryXML('read_feature', int(data[0], 16),
                                           int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            elif query[1] == 'memory-map' and query[2] == 'read':
                data = query[4].split(',')
                resp = self.handleQueryXML('momery_map', int(data[0], 16),
                                           int(data[1].split('#')[0], 16))
                return self.createRSPPacket(resp)

            else:
                return None

        elif query[0] == 'C#b4':
            return self.createRSPPacket("")

        elif query[0].find('Attached') != -1:
            return self.createRSPPacket("1")

        elif query[0].find('TStatus') != -1:
            return self.createRSPPacket("")

        elif query[0].find('Tf') != -1:
            return self.createRSPPacket("")

        elif 'Offsets' in query[0]:
            resp = "Text=0;Data=0;Bss=0"
            return self.createRSPPacket(resp)

        elif 'Symbol' in query[0]:
            resp = "OK"
            return self.createRSPPacket(resp)

        else:
            return None

    def handleQueryXML(self, query, offset, size):
        logging.debug('GDB query %s: offset: %s, size: %s', query, offset,
                      size)
        xml = ''
        if query == 'momery_map':
            xml = self.flash.memoryMapXML
        elif query == 'read_feature':
            xml = self.target.targetXML

        size_xml = len(xml)

        prefix = 'm'

        if offset > size_xml:
            logging.error('GDB: offset target.xml > size!')
            return

        if size > (self.packet_size - 4):
            size = self.packet_size - 4

        nbBytesAvailable = size_xml - offset

        if size > nbBytesAvailable:
            prefix = 'l'
            size = nbBytesAvailable

        resp = prefix + xml[offset:offset + size]

        return resp

    def createRSPPacket(self, data):
        resp = '$' + data + '#'

        c = 0
        checksum = 0
        for c in data:
            checksum += ord(c)
        checksum = checksum % 256
        checksum = hex(checksum)

        if int(checksum[2:], 16) < 0x10:
            resp += '0'
        resp += checksum[2:]

        #logging.debug('--<<<<<<<<<<<< GDB rsp packet: %s', resp)
        return resp

    def ack(self):
        self.abstract_socket.write("+")