int tftp_hex(unsigned int addr, unsigned char * buf, int size)
{
unsigned char * s;
unsigned char * cp;
int n;
int i;
/* Read data at buffer's last half... */
n = target_mem_read(addr, &buf[256], size);
if (n < 0) {
DCC_LOG(LOG_WARNING, "target memory read error.");
n = 0;
}
s = &buf[256];
cp = buf;
/* convert to hexadecimal (in place) */
for (i = 0; i < n; i++) {
*cp++ = hextab[(s[i] >> 4) & 0xf];
*cp++ = hextab[s[i] & 0xf];
}
return n * 2;
}
void __attribute__((noreturn)) tftp_daemon_task(struct debugger * dbg)
{
uint8_t buf[MAX_TFTP_MSG];
struct tftphdr * hdr = (struct tftphdr *)buf;
char * msg = (char *)buf;
struct sockaddr_in sin;
struct udp_pcb * udp;
struct tftp_req req;
int state = TFTPD_IDLE;
unsigned int addr_start = 0;
unsigned int addr_end = 0;
int block = 0;
int opc;
int len;
int blksize = TFTP_SEGSIZE;
DCC_LOG1(LOG_TRACE, "thread: %d", __os_thread_self());
if ((udp = udp_alloc()) == NULL) {
DCC_LOG(LOG_WARNING, "udp_alloc() fail!");
abort();
}
if (udp_bind(udp, INADDR_ANY, htons(IPPORT_TFTP)) < 0) {
DCC_LOG(LOG_WARNING, "udp_bind() fail!");
abort();
}
for (;;) {
if ((len = udp_recv(udp, buf, MAX_TFTP_MSG, &sin)) < 0) {
if (len == -ECONNREFUSED) {
DCC_LOG(LOG_WARNING, "udp_rcv ICMP error: ECONNREFUSED");
}
if (len == -EFAULT) {
DCC_LOG(LOG_WARNING, "udp_rcv error: EFAULT");
}
if (len == -ENOTCONN) {
DCC_LOG(LOG_WARNING, "udp_rcv error: ENOTCONN");
}
continue;
}
opc = htons(hdr->th_opcode);
if ((opc != TFTP_RRQ) && (opc != TFTP_WRQ)) {
DCC_LOG1(LOG_WARNING, "invalid opc: %d", opc);
WARN("TFTP: invalid opc: %d", opc);
continue;
}
if (udp_connect(udp, sin.sin_addr.s_addr, sin.sin_port) < 0) {
DCC_LOG(LOG_WARNING, "udp_connect() error");
WARN("TFTP: UDP connect failed!");
continue;
}
DCC_LOG2(LOG_TRACE, "Connected to: %I.%d", sin.sin_addr.s_addr,
ntohs(sin.sin_port));
// INF("Connected to: %08x.%d", sin.sin_addr.s_addr,
// ntohs(sin.sin_port));
for (;;) {
DCC_LOG3(LOG_INFO, "%I.%d %d",
sin.sin_addr.s_addr, ntohs(sin.sin_port), len);
DCC_LOG2(LOG_INFO, "len=%d, opc=%s", len, tftp_opc[opc]);
switch (opc) {
case TFTP_RRQ:
DCC_LOG(LOG_TRACE, "read request: ...");
tftp_req_parse((char *)&(hdr->th_stuff), &req);
blksize = req.blksize;
INF("TFTP: RRQ '%s' '%s' blksize=%d", req.fname,
tftp_mode[req.mode], req.blksize);
if (tftp_decode_fname(dbg, req.fname) < 0) {
ERR("TFTP: bad address.");
tftp_error(udp, &sin, TFTP_ENOTFOUND, "BAD ADDR.");
break;
}
/* set the transfer info */
addr_start = dbg->transf.base;
addr_end = addr_start + dbg->transf.size;
block = 0;
DCC_LOG2(LOG_TRACE, "start=0x%08x end=0x%08x",
addr_start, addr_end);
DBG("TFTP: start=0x%08x end=0x%08x",
addr_start, addr_end);
if (req.mode == TFTP_NETASCII) {
state = TFTPD_SEND_NETASCII;
} else if (req.mode == TFTP_OCTET) {
state = TFTPD_SEND_OCTET;
} else {
tftp_error(udp, &sin, TFTP_EUNDEF, NULL);
break;
}
if (req.opt_len) {
tftp_oack(udp, &sin, req.opt, req.opt_len);
break;
}
if (req.mode == TFTP_NETASCII)
goto send_netascii;
if (req.mode == TFTP_OCTET)
goto send_octet;
break;
case TFTP_WRQ:
/* Write Request */
DCC_LOG(LOG_TRACE, "write request...");
tftp_req_parse((char *)&(hdr->th_stuff), &req);
blksize = req.blksize;
INF("WRQ '%s' '%s' blksize=%d", req.fname,
tftp_mode[req.mode], req.blksize);
if (tftp_decode_fname(dbg, req.fname) < 0) {
tftp_error(udp, &sin, TFTP_ENOTFOUND, "BAD ADDR.");
break;
}
/* set the transfer info */
addr_start = dbg->transf.base;
addr_end = addr_start + dbg->transf.size;
block = 0;
DCC_LOG2(LOG_TRACE, "start=0x%08x end=0x%08x",
addr_start, addr_end);
DBG("TFTP: start=0x%08x end=0x%08x",
addr_start, addr_end);
if ((req.mode == TFTP_NETASCII) || (req.mode == TFTP_OCTET)) {
state = (req.mode == TFTP_NETASCII) ?
TFTPD_RECV_NETASCII : TFTPD_RECV_OCTET;
if (req.opt_len)
tftp_oack(udp, &sin, req.opt, req.opt_len);
else
tftp_ack(udp, block, &sin);
break;
}
tftp_error(udp, &sin, TFTP_EUNDEF, NULL);
break;
case TFTP_ACK:
block = htons(hdr->th_block);
DCC_LOG1(LOG_TRACE, "ACK: %d.", block);
if (state == TFTPD_SEND_NETASCII) {
unsigned int addr;
int rem;
int n;
send_netascii:
addr = addr_start + (block * 256);
rem = addr_end - addr;
if (rem < 0) {
state = TFTPD_IDLE;
DCC_LOG1(LOG_TRACE, "eot: %d bytes sent.",
addr_end - addr_start);
break;
}
n = (rem < 256) ? rem : 256;
DCC_LOG2(LOG_TRACE, "send netascii: addr=0x%08x n=%d",
addr, n);
/* build the packet */
len = tftp_hex(addr, hdr->th_data, n);
goto send_data;
}
if (state == TFTPD_SEND_OCTET) {
unsigned int addr;
int rem;
int n;
send_octet:
addr = addr_start + (block * blksize);
rem = addr_end - addr;
if (rem < 0) {
state = TFTPD_IDLE;
DCC_LOG1(LOG_TRACE, "eot: %d bytes sent.",
addr_end - addr_start);
break;
}
n = (rem < blksize) ? rem : blksize;
DCC_LOG2(LOG_TRACE, "send octet: addr=0x%08x n=%d",
addr, n);
/* build the packet */
len = target_mem_read(addr, hdr->th_data, n);
if (len < 0) {
DCC_LOG(LOG_WARNING, "target memory read error.");
len = 0;
}
send_data:
hdr->th_opcode = htons(TFTP_DATA);
hdr->th_block = htons(block + 1);
DCC_LOG2(LOG_TRACE, "block %d: %d bytes.",
block + 1, len);
if (udp_sendto(udp, hdr,
sizeof(struct tftphdr) + len, &sin) < 0) {
DCC_LOG(LOG_WARNING, "udp_sendto() fail");
state = TFTPD_IDLE;
break;
}
break;
}
DCC_LOG(LOG_WARNING, "state invalid!");
break;
case TFTP_DATA:
/* skip the header */
len -= 4;
DCC_LOG2(LOG_TRACE, "block=%d len=%d",
htons(hdr->th_block), len);
DBG("TFTP: DATA block=%d len=%d", htons(hdr->th_block), len);
if (htons(hdr->th_block) != (block + 1)) {
/* retransmission, just ack */
DCC_LOG2(LOG_WARNING, "retransmission, block=%d len=%d",
block, len);
tftp_ack(udp, block, &sin);
break;
}
if (state == TFTPD_RECV_OCTET) {
unsigned int addr;
int n;
addr = addr_start + (block * blksize);
block++;
if (len != blksize) {
DCC_LOG(LOG_TRACE, "last packet...");
state = TFTPD_IDLE;
if (len == 0) {
tftp_ack(udp, block, &sin);
break;
}
} else {
DCC_LOG2(LOG_TRACE, "rcvd octet: addr=0x%08x n=%d",
addr, len);
/* ACK the packet before writing to
speed up the transfer, errors are postponed... */
tftp_ack(udp, block, &sin);
}
n = target_mem_write(addr, hdr->th_data, len);
if (n < len) {
if (n < 0) {
DCC_LOG(LOG_ERROR, "target_mem_write()!");
sprintf(msg, "TARGET WRITE FAIL: %08x", addr);
WARN("memory write failed at "
"addr=0x%08x, code %d", addr, n);
} else {
DCC_LOG2(LOG_WARNING, "short writ: "
"ret(%d) < len(%d)!", n, len);
sprintf(msg, "TARGET SHORT WRITE: %08x",
addr + n);
}
tftp_error(udp, &sin, TFTP_EUNDEF, msg);
state = TFTPD_RECV_ERROR;
} else {
if (n > len) {
DCC_LOG2(LOG_WARNING, "long write: "
"ret(%d) < len(%d)!", n, len);
}
if (state == TFTPD_IDLE) {
/* ack the last packet ... */
tftp_ack(udp, block, &sin);
}
}
break;
}
if (state == TFTPD_RECV_ERROR) {
// tftp_error(udp, &sin, TFTP_EUNDEF, "TARGET WRITE FAIL.");
state = TFTPD_IDLE;
break;
}
if (state == TFTPD_RECV_NETASCII) {
block++;
if (len != blksize) {
state = TFTPD_IDLE;
if (len == 0) {
tftp_ack(udp, block, &sin);
break;
}
}
DCC_LOG1(LOG_TRACE, "ASCII recv %d...", len);
tftp_recv_netascii(udp, &sin, block,
(char *)hdr->th_data, len);
break;
}
tftp_error(udp, &sin, TFTP_EUNDEF, NULL);
break;
case TFTP_ERROR:
DCC_LOG2(LOG_TRACE, "error: %d: %s.",
htons(hdr->th_code), hdr->th_data);
break;
}
if (state == TFTPD_IDLE) {
DCC_LOG(LOG_TRACE, "[IDLE]");
break;
}
DBG("TFTP: UDP receive ...");
if ((len = udp_recv_tmo(udp, buf, MAX_TFTP_MSG, &sin, 5000)) < 0) {
if (len == -ETIMEDOUT) {
DCC_LOG(LOG_WARNING, "udp_recv_tmo() timeout!");
WARN("TFTP: UDP receive timeout!");
} else {
if (len == -ECONNREFUSED) {
DCC_LOG(LOG_WARNING, "udp_recv_tmo() lost peer!");
WARN("TFTP: UDP peer lost!");
} else {
DCC_LOG(LOG_WARNING, "udp_recv_tmo() failed!");
WARN("TFTP: UDP receive failed!");
}
}
/* break the inner loop */
break;
}
opc = htons(hdr->th_opcode);
}
/* disconnect */
DCC_LOG(LOG_TRACE, "disconnecting.");
udp_connect(udp, INADDR_ANY, 0);
}
}
void
gdb_main(void)
{
int size;
bool single_step = false;
char last_activity = 0;
DEBUG("Entring GDB protocol main loop\n");
/* GDB protocol main loop */
while(1) {
SET_IDLE_STATE(1);
size = gdb_getpacket(pbuf, BUF_SIZE);
SET_IDLE_STATE(0);
continue_activity:
switch(pbuf[0]) {
/* Implementation of these is mandatory! */
case 'g': { /* 'g': Read general registers */
ERROR_IF_NO_TARGET();
uint8_t arm_regs[target_regs_size(cur_target)];
target_regs_read(cur_target, arm_regs);
gdb_putpacket(hexify(pbuf, arm_regs, sizeof(arm_regs)),
sizeof(arm_regs) * 2);
break;
}
case 'm': { /* 'm addr,len': Read len bytes from addr */
uint32_t addr, len;
ERROR_IF_NO_TARGET();
sscanf(pbuf, "m%" SCNx32 ",%" SCNx32, &addr, &len);
DEBUG("m packet: addr = %" PRIx32 ", len = %" PRIx32 "\n", addr, len);
uint8_t mem[len];
target_mem_read(cur_target, mem, addr, len);
if(target_check_error(cur_target))
gdb_putpacketz("E01");
else
gdb_putpacket(hexify(pbuf, mem, len), len*2);
break;
}
case 'G': { /* 'G XX': Write general registers */
ERROR_IF_NO_TARGET();
uint8_t arm_regs[target_regs_size(cur_target)];
unhexify(arm_regs, &pbuf[1], sizeof(arm_regs));
target_regs_write(cur_target, arm_regs);
gdb_putpacketz("OK");
break;
}
case 'M': { /* 'M addr,len:XX': Write len bytes to addr */
uint32_t addr, len;
int hex;
ERROR_IF_NO_TARGET();
sscanf(pbuf, "M%" SCNx32 ",%" SCNx32 ":%n", &addr, &len, &hex);
DEBUG("M packet: addr = %" PRIx32 ", len = %" PRIx32 "\n", addr, len);
uint8_t mem[len];
unhexify(mem, pbuf + hex, len);
target_mem_write(cur_target, addr, mem, len);
if(target_check_error(cur_target))
gdb_putpacketz("E01");
else
gdb_putpacketz("OK");
break;
}
case 's': /* 's [addr]': Single step [start at addr] */
single_step = true;
// Fall through to resume target
case 'c': /* 'c [addr]': Continue [at addr] */
if(!cur_target) {
gdb_putpacketz("X1D");
break;
}
target_halt_resume(cur_target, single_step);
SET_RUN_STATE(1);
single_step = false;
// Fall through to wait for target halt
case '?': { /* '?': Request reason for target halt */
/* This packet isn't documented as being mandatory,
* but GDB doesn't work without it. */
uint32_t watch_addr;
int sig;
if(!cur_target) {
/* Report "target exited" if no target */
gdb_putpacketz("W00");
break;
}
last_activity = pbuf[0];
/* Wait for target halt */
while(!(sig = target_halt_wait(cur_target))) {
unsigned char c = gdb_if_getchar_to(0);
if((c == '\x03') || (c == '\x04')) {
target_halt_request(cur_target);
last_activity = 's';
}
}
SET_RUN_STATE(0);
/* Negative signal indicates we're in a syscall */
if (sig < 0)
break;
/* Target disappeared */
if (cur_target == NULL) {
gdb_putpacket_f("X%02X", sig);
break;
}
/* Report reason for halt */
if(target_check_hw_wp(cur_target, &watch_addr)) {
/* Watchpoint hit */
gdb_putpacket_f("T%02Xwatch:%08X;", sig, watch_addr);
} else {
gdb_putpacket_f("T%02X", sig);
}
break;
}
case 'F': { /* Semihosting call finished */
int retcode, errcode, items;
char c, *p;
if (pbuf[1] == '-')
p = &pbuf[2];
else
p = &pbuf[1];
items = sscanf(p, "%x,%x,%c", &retcode, &errcode, &c);
if (pbuf[1] == '-')
retcode = -retcode;
target_hostio_reply(cur_target, retcode, errcode);
/* if break is requested */
if (items == 3 && c == 'C') {
gdb_putpacketz("T02");
break;
}
pbuf[0] = last_activity;
goto continue_activity;
}
/* Optional GDB packet support */
case '!': /* Enable Extended GDB Protocol. */
/* This doesn't do anything, we support the extended
* protocol anyway, but GDB will never send us a 'R'
* packet unless we answer 'OK' here.
*/
gdb_putpacketz("OK");
break;
case 0x04:
case 'D': /* GDB 'detach' command. */
if(cur_target)
target_detach(cur_target);
last_target = cur_target;
cur_target = NULL;
gdb_putpacketz("OK");
break;
case 'k': /* Kill the target */
if(cur_target) {
target_reset(cur_target);
target_detach(cur_target);
last_target = cur_target;
cur_target = NULL;
}
break;
case 'r': /* Reset the target system */
case 'R': /* Restart the target program */
if(cur_target)
target_reset(cur_target);
else if(last_target) {
cur_target = target_attach(last_target,
gdb_target_destroy_callback);
target_reset(cur_target);
}
break;
case 'X': { /* 'X addr,len:XX': Write binary data to addr */
uint32_t addr, len;
int bin;
ERROR_IF_NO_TARGET();
sscanf(pbuf, "X%" SCNx32 ",%" SCNx32 ":%n", &addr, &len, &bin);
DEBUG("X packet: addr = %" PRIx32 ", len = %" PRIx32 "\n", addr, len);
target_mem_write(cur_target, addr, pbuf+bin, len);
if(target_check_error(cur_target))
gdb_putpacketz("E01");
else
gdb_putpacketz("OK");
break;
}
case 'q': /* General query packet */
handle_q_packet(pbuf, size);
break;
case 'v': /* General query packet */
handle_v_packet(pbuf, size);
break;
/* These packet implement hardware break-/watchpoints */
case 'Z': /* Z type,addr,len: Set breakpoint packet */
case 'z': /* z type,addr,len: Clear breakpoint packet */
ERROR_IF_NO_TARGET();
handle_z_packet(pbuf, size);
break;
default: /* Packet not implemented */
DEBUG("*** Unsupported packet: %s\n", pbuf);
gdb_putpacketz("");
}
}
}
int gdb_main_loop(struct target_controller *tc, bool in_syscall)
{
int size;
bool single_step = false;
/* GDB protocol main loop */
while(1) {
SET_IDLE_STATE(1);
size = gdb_getpacket(pbuf, BUF_SIZE);
SET_IDLE_STATE(0);
switch(pbuf[0]) {
/* Implementation of these is mandatory! */
case 'g': { /* 'g': Read general registers */
ERROR_IF_NO_TARGET();
uint8_t arm_regs[target_regs_size(cur_target)];
target_regs_read(cur_target, arm_regs);
gdb_putpacket(hexify(pbuf, arm_regs, sizeof(arm_regs)),
sizeof(arm_regs) * 2);
break;
}
case 'm': { /* 'm addr,len': Read len bytes from addr */
uint32_t addr, len;
ERROR_IF_NO_TARGET();
sscanf(pbuf, "m%" SCNx32 ",%" SCNx32, &addr, &len);
if (len > sizeof(pbuf) / 2) {
gdb_putpacketz("E02");
break;
}
DEBUG("m packet: addr = %" PRIx32 ", len = %" PRIx32 "\n", addr, len);
uint8_t mem[len];
if (target_mem_read(cur_target, mem, addr, len))
gdb_putpacketz("E01");
else
gdb_putpacket(hexify(pbuf, mem, len), len*2);
break;
}
case 'G': { /* 'G XX': Write general registers */
ERROR_IF_NO_TARGET();
uint8_t arm_regs[target_regs_size(cur_target)];
unhexify(arm_regs, &pbuf[1], sizeof(arm_regs));
target_regs_write(cur_target, arm_regs);
gdb_putpacketz("OK");
break;
}
case 'M': { /* 'M addr,len:XX': Write len bytes to addr */
uint32_t addr, len;
int hex;
ERROR_IF_NO_TARGET();
sscanf(pbuf, "M%" SCNx32 ",%" SCNx32 ":%n", &addr, &len, &hex);
if (len > (unsigned)(size - hex) / 2) {
gdb_putpacketz("E02");
break;
}
DEBUG("M packet: addr = %" PRIx32 ", len = %" PRIx32 "\n", addr, len);
uint8_t mem[len];
unhexify(mem, pbuf + hex, len);
if (target_mem_write(cur_target, addr, mem, len))
gdb_putpacketz("E01");
else
gdb_putpacketz("OK");
break;
}
case 's': /* 's [addr]': Single step [start at addr] */
single_step = true;
/* fall through */
case 'c': /* 'c [addr]': Continue [at addr] */
if(!cur_target) {
gdb_putpacketz("X1D");
break;
}
target_halt_resume(cur_target, single_step);
SET_RUN_STATE(1);
single_step = false;
/* fall through */
case '?': { /* '?': Request reason for target halt */
/* This packet isn't documented as being mandatory,
* but GDB doesn't work without it. */
target_addr watch;
enum target_halt_reason reason;
if(!cur_target) {
/* Report "target exited" if no target */
gdb_putpacketz("W00");
break;
}
/* Wait for target halt */
while(!(reason = target_halt_poll(cur_target, &watch))) {
unsigned char c = gdb_if_getchar_to(0);
if((c == '\x03') || (c == '\x04')) {
target_halt_request(cur_target);
}
}
SET_RUN_STATE(0);
/* Translate reason to GDB signal */
switch (reason) {
case TARGET_HALT_ERROR:
gdb_putpacket_f("X%02X", GDB_SIGLOST);
morse("TARGET LOST.", true);
break;
case TARGET_HALT_REQUEST:
gdb_putpacket_f("T%02X", GDB_SIGINT);
break;
case TARGET_HALT_WATCHPOINT:
gdb_putpacket_f("T%02Xwatch:%08X;", GDB_SIGTRAP, watch);
break;
case TARGET_HALT_FAULT:
gdb_putpacket_f("T%02X", GDB_SIGSEGV);
break;
default:
gdb_putpacket_f("T%02X", GDB_SIGTRAP);
}
break;
}
case 'F': /* Semihosting call finished */
if (in_syscall) {
return hostio_reply(tc, pbuf, size);
} else {
DEBUG("*** F packet when not in syscall! '%s'\n", pbuf);
gdb_putpacketz("");
}
break;
/* Optional GDB packet support */
case '!': /* Enable Extended GDB Protocol. */
/* This doesn't do anything, we support the extended
* protocol anyway, but GDB will never send us a 'R'
* packet unless we answer 'OK' here.
*/
gdb_putpacketz("OK");
break;
case 0x04:
case 'D': /* GDB 'detach' command. */
if(cur_target)
target_detach(cur_target);
last_target = cur_target;
cur_target = NULL;
gdb_putpacketz("OK");
break;
case 'k': /* Kill the target */
if(cur_target) {
target_reset(cur_target);
target_detach(cur_target);
last_target = cur_target;
cur_target = NULL;
}
break;
case 'r': /* Reset the target system */
case 'R': /* Restart the target program */
if(cur_target)
target_reset(cur_target);
else if(last_target) {
cur_target = target_attach(last_target,
&gdb_controller);
target_reset(cur_target);
}
break;
case 'X': { /* 'X addr,len:XX': Write binary data to addr */
uint32_t addr, len;
int bin;
ERROR_IF_NO_TARGET();
sscanf(pbuf, "X%" SCNx32 ",%" SCNx32 ":%n", &addr, &len, &bin);
if (len > (unsigned)(size - bin)) {
gdb_putpacketz("E02");
break;
}
DEBUG("X packet: addr = %" PRIx32 ", len = %" PRIx32 "\n", addr, len);
if (target_mem_write(cur_target, addr, pbuf+bin, len))
gdb_putpacketz("E01");
else
gdb_putpacketz("OK");
break;
}
case 'q': /* General query packet */
handle_q_packet(pbuf, size);
break;
case 'v': /* General query packet */
handle_v_packet(pbuf, size);
break;
/* These packet implement hardware break-/watchpoints */
case 'Z': /* Z type,addr,len: Set breakpoint packet */
case 'z': /* z type,addr,len: Clear breakpoint packet */
ERROR_IF_NO_TARGET();
handle_z_packet(pbuf, size);
break;
default: /* Packet not implemented */
DEBUG("*** Unsupported packet: %s\n", pbuf);
gdb_putpacketz("");
}
}
}
