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])
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])
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("+")
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("+")
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])
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("+")
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("+")
