m3dev

cortex m3 debug tools -- superceded by mdebug
git clone http://frotz.net/git/m3dev.git
Log | Files | Refs | README | LICENSE

gdb-bridge.c (18854B)


      1 /* gdb-bridge.c
      2  *
      3  * Copyright 2011 Brian Swetland <swetland@frotz.net>
      4  *
      5  * Licensed under the Apache License, Version 2.0 (the "License");
      6  * you may not use this file except in compliance with the License.
      7  * You may obtain a copy of the License at
      8  *
      9  *     http://www.apache.org/licenses/LICENSE-2.0
     10  *
     11  * Unless required by applicable law or agreed to in writing, software
     12  * distributed under the License is distributed on an "AS IS" BASIS,
     13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     14  * See the License for the specific language governing permissions and
     15  * limitations under the License.
     16  */
     17 
     18 #include <stdio.h>
     19 #include <stdlib.h>
     20 #include <stdarg.h>
     21 #include <unistd.h>
     22 #include <string.h>
     23 #include <errno.h>
     24 #include <signal.h>
     25 #include <poll.h>
     26 
     27 #include <fw/types.h>
     28 #include "rswdp.h"
     29 #include <protocol/rswdp.h>
     30 #include "debugger.h"
     31 #include "lkdebug.h"
     32 
     33 // useful gdb stuff
     34 // set debug remote 1               protocol tracing
     35 // set debug arch 1                 architecture tracing
     36 // maint print registers-remote     check on register map
     37 
     38 #define MAXPKT		8192
     39 
     40 #define S_IDLE		0
     41 #define S_RECV		1
     42 #define S_CHK1		2
     43 #define S_CHK2		3
     44 
     45 #define F_ACK		0x01
     46 #define F_RUNNING	0x04
     47 #define F_CONSOLE	0x08
     48 #define F_LK_THREADS	0x10
     49 
     50 struct gdbcnxn {
     51 	int fd;
     52 	unsigned state;
     53 	unsigned txsum;
     54 	unsigned rxsum;
     55 	unsigned flags;
     56 	unsigned char *txptr;
     57 	unsigned char *rxptr;
     58 	unsigned char rxbuf[MAXPKT];
     59 	unsigned char txbuf[MAXPKT];
     60 	char chk[4];
     61 	lkthread_t *threadlist;
     62 	lkthread_t *gselected;
     63 	lkthread_t *cselected;
     64 };
     65 
     66 void gdb_init(struct gdbcnxn *gc, int fd) {
     67 	gc->fd = fd;
     68 	gc->state = S_IDLE;
     69 	gc->txsum = 0;
     70 	gc->rxsum = 0;
     71 	gc->flags = F_ACK;
     72 	gc->txptr = gc->txbuf;
     73 	gc->rxptr = gc->rxbuf;
     74 	gc->chk[2] = 0;
     75 	gc->threadlist = NULL;
     76 }
     77 
     78 static inline int rx_full(struct gdbcnxn *gc) {
     79 	return (gc->rxptr - gc->rxbuf) == (MAXPKT - 1);
     80 }
     81 
     82 static inline int tx_full(struct gdbcnxn *gc) {
     83 	return (gc->txptr - gc->txbuf) == (MAXPKT - 1);
     84 }
     85 
     86 static inline void gdb_putc(struct gdbcnxn *gc, unsigned n) {
     87 	unsigned char c = n;
     88 	if (!tx_full(gc)) {
     89 		gc->txsum += c;
     90 		*(gc->txptr++) = c;
     91 	}
     92 }
     93 
     94 void gdb_puts(struct gdbcnxn *gc, const char *s) {
     95 	while (*s) {
     96 		gdb_putc(gc, *s++);
     97 	}
     98 }
     99 
    100 void gdb_prologue(struct gdbcnxn *gc) {
    101 	gc->txptr = gc->txbuf;
    102 	*(gc->txptr++) = '$';
    103 	gc->txsum = 0;
    104 }
    105 
    106 void gdb_epilogue(struct gdbcnxn *gc) {
    107 	unsigned char *ptr;
    108 	int len, r;
    109 	char tmp[4];
    110 	sprintf(tmp,"#%02x", gc->txsum & 0xff);
    111 	gdb_puts(gc, tmp);
    112 
    113 	if (tx_full(gc)) {
    114 		xprintf(XGDB, "gdb: TX Packet Too Large\n");
    115 		return;
    116 	}
    117 
    118 	ptr = gc->txbuf;
    119 	len = gc->txptr - gc->txbuf;
    120 	while (len > 0) {
    121 		r = write(gc->fd, ptr, len);
    122 		if (r <= 0) {
    123 			if (errno == EINTR) continue;
    124 			xprintf(XGDB, "gdb: TX Write Error\n");
    125 			return;
    126 		}
    127 		ptr += r;
    128 		len -= r;
    129 	}
    130 }
    131 
    132 static char HEX[16] = "0123456789abcdef";
    133 void gdb_puthex(struct gdbcnxn *gc, const void *ptr, unsigned len) {
    134 	const unsigned char *data = ptr;
    135 	while (len-- > 0) {
    136 		unsigned c = *data++;
    137 		gdb_putc(gc, HEX[c >> 4]);
    138 		gdb_putc(gc, HEX[c & 15]);
    139 	}
    140 }
    141 
    142 void handle_command(struct gdbcnxn *gc, unsigned char *cmd);
    143 
    144 void gdb_recv_cmd(struct gdbcnxn *gc) {
    145 	if (log_flags & LF_GDB) {
    146 		xprintf(XGDB, "gdb: pkt: %s\n", gc->rxbuf);
    147 	}
    148 	debugger_lock();
    149 	handle_command(gc, gc->rxbuf);
    150 	debugger_unlock();
    151 }
    152 
    153 int gdb_recv(struct gdbcnxn *gc, unsigned char *ptr, int len) {
    154 	unsigned char *start = ptr;
    155 	unsigned char c;
    156 
    157 	while (len > 0) {
    158 		c = *ptr++;
    159 		len--;
    160 		switch (gc->state) {
    161 		case S_IDLE:
    162 			if (c == 3) {
    163 				gc->rxbuf[0] = '$';
    164 				gc->rxbuf[1] = 0;
    165 				gdb_recv_cmd(gc);
    166 			} else if (c == '$') {
    167 				gc->state = S_RECV;
    168 				gc->rxsum = 0;
    169 				gc->rxptr = gc->rxbuf;
    170 			}
    171 			break;
    172 		case S_RECV:
    173 			if (c == '#') {
    174 				gc->state = S_CHK1;
    175 			} else {
    176 				if (rx_full(gc)) {
    177 					xprintf(XGDB, "gdb: pkt: Too Large, Discarding.");
    178 					gc->rxptr = gc->rxbuf;
    179 					gc->state = S_IDLE;
    180 				} else {
    181 					*(gc->rxptr++) = c;
    182 					gc->rxsum += c;
    183 				}
    184 			}
    185 			break;
    186 		case S_CHK1:
    187 			gc->chk[0] = c;
    188 			gc->state = S_CHK2;
    189 			break;
    190 		case S_CHK2:
    191 			gc->chk[1] = c;
    192 			gc->state = S_IDLE;
    193 			*(gc->rxptr++) = 0;
    194 			if (strtoul(gc->chk, NULL, 16) == (gc->rxsum & 0xFF)) {
    195 				if (gc->flags & F_ACK) {
    196 					if (write(gc->fd, "+", 1)) ;
    197 				}
    198 				gdb_recv_cmd(gc);
    199 			} else {
    200 				if (gc->flags & F_ACK) {
    201 					if (write(gc->fd, "-", 1)) ;
    202 				}
    203 			}
    204 			break;
    205 		}
    206 	}
    207 	return ptr - start;
    208 }
    209 
    210 unsigned unhex(char *x) {
    211 	char tmp[3];
    212 	unsigned n;
    213 	tmp[0] = x[0];
    214 	tmp[1] = x[1];
    215 	tmp[2] = 0;
    216 	n = strtoul(tmp, 0, 16);
    217  	return n;
    218 }
    219 
    220 int hextobin(void *_dst, char *src, int max) {
    221 	unsigned char *dst = _dst;
    222 	while (max > 0) {
    223 		if (src[0] && src[1]) {
    224 			*dst++ = unhex(src);
    225 			src += 2;
    226 			max -= 2;
    227 		} else {
    228 			break;
    229 		}
    230 	}
    231 	return dst - ((unsigned char*) _dst);
    232 }
    233 
    234 static const char *target_xml =
    235 "<?xml version=\"1.0\"?>"
    236 "<target>"
    237 "<architecture>arm</architecture>"
    238 "<feature name=\"org.gnu.gdb.arm.m-profile\">"
    239 "<reg name=\"r0\" bitsize=\"32\"/>"
    240 "<reg name=\"r1\" bitsize=\"32\"/>"
    241 "<reg name=\"r2\" bitsize=\"32\"/>"
    242 "<reg name=\"r3\" bitsize=\"32\"/>"
    243 "<reg name=\"r4\" bitsize=\"32\"/>"
    244 "<reg name=\"r5\" bitsize=\"32\"/>"
    245 "<reg name=\"r6\" bitsize=\"32\"/>"
    246 "<reg name=\"r7\" bitsize=\"32\"/>"
    247 "<reg name=\"r8\" bitsize=\"32\"/>"
    248 "<reg name=\"r9\" bitsize=\"32\"/>"
    249 "<reg name=\"r10\" bitsize=\"32\"/>"
    250 "<reg name=\"r11\" bitsize=\"32\"/>"
    251 "<reg name=\"r12\" bitsize=\"32\"/>"
    252 "<reg name=\"sp\" bitsize=\"32\" type=\"data_ptr\"/>"
    253 "<reg name=\"lr\" bitsize=\"32\"/>"
    254 "<reg name=\"pc\" bitsize=\"32\" type=\"code_ptr\"/>"
    255 "<reg name=\"xpsr\" bitsize=\"32\"/>"
    256 "<reg name=\"msp\" bitsize=\"32\" type=\"data_ptr\"/>"
    257 "<reg name=\"psp\" bitsize=\"32\" type=\"data_ptr\"/>"
    258 "</feature>"
    259 "</target>";
    260 
    261 static void handle_query(struct gdbcnxn *gc, char *cmd, char *args) {
    262 	if (!strcmp(cmd, "fThreadInfo")) {
    263 		if (gc->threadlist) {
    264 			char tmp[32];
    265 			lkthread_t *t = gc->threadlist;
    266 			sprintf(tmp, "m%x", t->threadptr);
    267 			gdb_puts(gc, tmp);
    268 			for (t = t->next; t != NULL; t = t->next) {
    269 				sprintf(tmp, ",%x",t->threadptr);
    270 				gdb_puts(gc, tmp);
    271 			}
    272 		} else {
    273 			/* report just one thread id, #1, for now */
    274 			gdb_puts(gc, "m1");
    275 		}
    276 	} else if(!strcmp(cmd, "sThreadInfo")) {
    277 		/* no additional thread ids */
    278 		gdb_puts(gc, "l");
    279 	} else if(!strcmp(cmd, "ThreadExtraInfo")) {
    280 		u32 n = strtoul(args, NULL, 16);
    281 		lkthread_t *t = find_lk_thread(gc->threadlist, n);
    282 		/* gdb manual suggest 'Runnable', 'Blocked on Mutex', etc */
    283 		/* informational text shown in gdb's "info threads" listing */
    284 		if (t) {
    285 			char tmp[128];
    286 			get_lk_thread_name(t, tmp, sizeof(tmp));
    287 			gdb_puthex(gc, tmp, strlen(tmp));
    288 		} else {
    289 			gdb_puthex(gc, "Native", 6);
    290 		}
    291 	} else if(!strcmp(cmd, "C")) {
    292 		/* current thread ID */
    293 		if (gc->cselected) {
    294 			char tmp[32];
    295 			sprintf(tmp, "QC%x", gc->cselected->threadptr);
    296 			gdb_puts(gc, tmp);
    297 		} else {
    298 			gdb_puts(gc, "QC1");
    299 		}
    300 	} else if (!strcmp(cmd, "Rcmd")) {
    301 		char *p = args;
    302 		cmd = p;
    303 		while (p[0] && p[1]) {
    304 			*cmd++ = unhex(p);
    305 			p+=2;
    306 		}
    307 		*cmd = 0;
    308 		xprintf(XGDB, "gdb: %s\n", args);
    309 		gc->flags |= F_CONSOLE;
    310 		debugger_unlock();
    311 		debugger_command(args);
    312 		debugger_lock();
    313 		gc->flags &= ~F_CONSOLE;
    314 		gdb_prologue(gc);
    315 		gdb_puts(gc, "OK");
    316 	} else if(!strcmp(cmd, "Supported")) {
    317 		gdb_puts(gc,
    318 			"qXfer:features:read+"
    319 			";QStartNoAckMode+"
    320 			";PacketSize=2004" /* size includes "$" and "#xx" */
    321 			);
    322 	} else if(!strcmp(cmd, "Xfer")) {
    323 		if (!strncmp(args, "features:read:target.xml:", 25)) {
    324 			gdb_puts(gc,"l");
    325 			// todo: support binary format w/ escaping
    326 			gdb_puts(gc, target_xml);
    327 		}
    328 	} else if(!strcmp(cmd, "TStatus")) {
    329 		/* tracepoints unsupported. ignore. */
    330 	} else if(!strcmp(cmd, "Attached")) {
    331 		/* no process management. ignore */
    332 	} else {
    333 		xprintf(XGDB, "gdb: unsupported: q%s:%s\n", cmd, args);
    334 	}
    335 }
    336 
    337 static void handle_set(struct gdbcnxn *gc, char *cmd, char *args) {
    338 	if(!strcmp(cmd, "StartNoAckMode")) {
    339 		gc->flags &= ~F_ACK;
    340 		gdb_puts(gc, "OK");
    341 	} else {
    342 		xprintf(XGDB, "gdb: unsupported: Q%s:%s\n", cmd, args);
    343 	}
    344 }
    345 
    346 // Since we're only guaranteed 32bit reads and writes by the MEM-AP
    347 // convert non-aligned writes into a read-modify-write followed by
    348 // an aligned multiword write, followed by a read-modify-write.
    349 // (as necessary)
    350 void write_memory(u32 addr, unsigned char *data, int len) {
    351 	u32 x;
    352 
    353 	if (len < 1) {
    354 		return;
    355 	}
    356 
    357 	if (addr & 3) {
    358 		int offset = (addr & 3);
    359 		int count = 4 - offset;
    360 		if (count > len) {
    361 			count = len;
    362 		}
    363 		swdp_ahb_read(addr & (~3), &x);
    364 		memcpy(((char*) &x) + offset, data, count);
    365 		swdp_ahb_write(addr & (~3), x);
    366 		len -= count;
    367 		data += count;
    368 		addr += count;
    369 	}
    370 
    371 	if (len > 3) {
    372 		int count = (4 * (len / 4));
    373 		swdp_ahb_write32(addr, (void*) data, len / 4);
    374 		len -= count;
    375 		data += count;
    376 		addr += count;
    377 	}
    378 
    379 	if (len > 0) {
    380 		swdp_ahb_read(addr, &x);
    381 		memcpy(&x, data, len);
    382 		swdp_ahb_write(addr, x);
    383 	}
    384 }
    385 
    386 // bit 1 in each romtable entry indicates peripheral is present
    387 #define ROMTAB_DWT	0xE00FF004
    388 #define ROMTAB_FPB	0xE00FF008
    389 #define ROMTAB_ITM	0xE00FF00C
    390 #define ROMTAB_TPIU	0xE00FF010
    391 #define ROMTAB_ETM	0xE00FF014
    392 
    393 #define DWT_CTRL	0xE0001000
    394 
    395 #define FP_CTRL		0xE0002000
    396 #define FP_REMAP	0xE0002004
    397 #define FP_COMP(n)	(0xE0002008 + ((n) * 4))
    398 
    399 #define MAXFP 16
    400 static u32 maxfp = 0;
    401 static u32 fp_addr[MAXFP] = { 0, };
    402 static u32 fp_state[MAXFP] = { 0, };
    403 
    404 #define MAXBP 16
    405 static u32 maxbp;
    406 static u32 bp_addr[MAXBP] = { 0, };
    407 static u32 bp_state[MAXBP] = { 0, };
    408 
    409 int handle_flashpatch(int add, u32 addr, u32 kind) {
    410 	u32 x;
    411 	int n;
    412 	if (swdp_ahb_read(ROMTAB_FPB, &x)) {
    413 		xprintf(XGDB, "gdb: cannot read romtable\n");
    414 		return -1;
    415 	}
    416 	if (x & 1) {
    417 		if (swdp_ahb_read(FP_CTRL, &x)) {
    418 			xprintf(XGDB, "gdb: cannot read flashpatch ctrl\n");
    419 			return -1;
    420 		}
    421 		n = ((x & 0xF0) >> 4) | ((x & 0x7000) >> 4);
    422 	} else {
    423 		n = 0;
    424 	}
    425 	if (n != maxfp) {
    426 		xprintf(XGDB, "gdb: %d flashpatch breakpoint registers\n", n);
    427 		if (n > 16) n = 16;
    428 		if (maxfp != 0) {
    429 			xprintf(XGDB, "gdb: previously %d registers...\n", maxfp);
    430 		}
    431 		maxfp = n;
    432 	}
    433 	for (n = 0; n < maxfp; n++) {
    434 		if (fp_state[n] && (fp_addr[n] == addr)) {
    435 			if (add) {
    436 				goto add0;
    437 			} else {
    438 				fp_state[n] = 0;
    439 				swdp_ahb_write(FP_COMP(n), 0);
    440 				xprintf(XGDB, "gdb: - FP BP @ %08x\n", addr);
    441 				return 0;
    442 			}
    443 		}
    444 	}
    445 	if (!add) {
    446 		// no breakpoint to remove
    447 		return -1;
    448 	}
    449 	for (n = 0; n < maxfp; n++) {
    450 		if (fp_state[n] == 0) {
    451 			goto add1;
    452 		}
    453 	}
    454 	return -1;
    455 add1:
    456 	swdp_ahb_write(FP_CTRL,3);
    457 	if (addr & 2) {
    458 		// breakpoint on low half-word, enable
    459 		swdp_ahb_write(FP_COMP(n), 0x80000001 | (addr & 0x1FFFFFFC));
    460 	} else {
    461 		// breakpoint on high half-word, enable
    462 		swdp_ahb_write(FP_COMP(n), 0x40000001 | (addr & 0x1FFFFFFC));
    463 	}
    464 	fp_state[n] = 1;
    465 	fp_addr[n] = addr;
    466 add0:
    467 	xprintf(XGDB, "gdb: + FP BP @ %08x\n", addr);
    468 	return 0;
    469 }
    470 
    471 int handle_breakpoint(int add, u32 addr, u32 kind) {
    472 	u32 x;
    473 	int n;
    474 
    475 	if ((addr < 0x20000000) && (!handle_flashpatch(add,addr,kind))) {
    476 		return 0;
    477 	}
    478 	if (swdp_ahb_read(ROMTAB_DWT, &x)) {
    479 		xprintf(XGDB, "gdb: cannot read romtable\n");
    480 		return -1;
    481 	}
    482 	if (x & 1) {
    483 		if (swdp_ahb_read(DWT_CTRL, &x)) {
    484 			xprintf(XGDB, "gdb: cannot read dwt ctrl\n");
    485 			return -1;
    486 		}
    487 		n = x >> 28;
    488 	} else {
    489 		n = 0;
    490 	}
    491 	if (n != maxbp) {
    492 		xprintf(XGDB, "gdb: %d dwt breakpoint registers\n", n);
    493 		if (maxbp != 0) {
    494 			xprintf(XGDB, "gdb: previously %d registers...\n", maxbp);
    495 		}
    496 		maxbp = n;
    497 	}
    498 
    499 	if (add) {
    500 		for (n = 0; n < maxbp; n++) {
    501 			if (bp_state[n] && (bp_addr[n] == addr)) {
    502 				// already setup
    503 				return 0;
    504 			}
    505 		}
    506 		for (n = 0; n < maxbp; n++) {
    507 			if (bp_state[n] == 0) {
    508 				bp_addr[n] = addr;
    509 				bp_state[n] = 1;
    510 				swdp_watchpoint_pc(n, addr);
    511 				xprintf(XGDB, "gdb: + HW BP @ %08x\n", addr);
    512 				return 0;
    513 			}
    514 		}
    515 		if (n == maxbp) {
    516 			xprintf(XGDB, "gdb: Out of hardware breakpoints.\n");
    517 			return 1;
    518 		}
    519 		return 0;
    520 	} else {
    521 		for (n = 0; n < maxbp; n++) {
    522 			if (bp_state[n] && (bp_addr[n] == addr)) {
    523 				bp_state[n] = 0;
    524 				swdp_watchpoint_disable(n);
    525 				break;
    526 			}
    527 		}
    528 		xprintf(XGDB, "gdb: - HW BP @ %08x\n", addr);
    529 		return 0;
    530 	}
    531 }
    532 
    533 void gdb_update_threads(struct gdbcnxn *gc) {
    534 	xprintf(XGDB, "gdb: sync threadlist\n");
    535 	free_lk_threads(gc->threadlist);
    536 	if (gc->flags & F_LK_THREADS) {
    537 		if ((gc->threadlist = find_lk_threads(0)) == NULL) {
    538 			xprintf(XGDB, "gdb: problem syncing threadlist\n");
    539 		}
    540 		gc->cselected = gc->threadlist;
    541 		gc->gselected = gc->threadlist;
    542 	} else {
    543 		gc->threadlist = NULL;
    544 		gc->cselected = NULL;
    545 		gc->gselected = NULL;
    546 	}
    547 }
    548 
    549 void handle_command(struct gdbcnxn *gc, unsigned char *cmd) {
    550 	union {
    551 		u32 w[256+2];
    552 		u16 h[512+4];
    553 		u8 b[1024+8];
    554 	} tmp;
    555 	unsigned n,x;
    556 
    557 	/* silent (no-response) commands */
    558 	switch (cmd[0]) {
    559 	case 'c':
    560 		if (cmd[1]) {
    561 			x = strtoul((char*) cmd + 1, NULL, 16) | 1;
    562 			swdp_core_write(15, x);
    563 		}
    564 		swdp_core_resume();
    565 		gc->flags |= F_RUNNING;
    566 		return;
    567 	// single step
    568 	case 's':
    569 		if (cmd[1]) {
    570 			x = strtoul((char*) cmd + 1, NULL, 16) | 1;
    571 			swdp_core_write(15, x);
    572 		}
    573 		swdp_core_step();
    574 		gc->flags |= F_RUNNING;
    575 		return;
    576 	}
    577 
    578 	gdb_prologue(gc);
    579 	switch (cmd[0]) {
    580 	case '?':
    581 		gdb_puts(gc, "S05");
    582 		gc->flags &= (~F_RUNNING);
    583 		swdp_core_halt();
    584 		gdb_update_threads(gc);
    585 		break;
    586 	case 'H': {
    587 		// select thread
    588 		lkthread_t *t = NULL;
    589 		if ((cmd[2] == '-') && (cmd[3] == '1')) {
    590 			t = gc->threadlist;
    591 		} else {
    592 			n = strtoul((char*) cmd + 2, NULL, 16);
    593 			if ((t = find_lk_thread(gc->threadlist, n)) == NULL) {
    594 				t = gc->threadlist;
    595 			}
    596 		}
    597 		if (cmd[1] == 'g') {
    598 			gc->gselected = t;
    599 		} else if (cmd[1] == 'c') {
    600 			gc->cselected = t;
    601 		} else {
    602 			xprintf(XGDB, "gdb: selecting '%c' thread?!\n", cmd[1]);
    603 		}
    604 		gdb_puts(gc, "OK");
    605 		break;
    606 	}
    607 	// is thread alive?
    608 	case 'T':
    609 		n = strtoul((char*) cmd + 1, NULL, 16);
    610 		if (find_lk_thread(gc->threadlist, n)) {
    611 			gdb_puts(gc, "OK");
    612 		} else {
    613 			gdb_puts(gc, "E00");
    614 		}
    615 		break;
    616 	// m hexaddr , hexcount
    617 	// read from memory
    618 	case 'm':
    619 		if (sscanf((char*) cmd + 1, "%x,%x", &x, &n) != 2) {
    620 			break;
    621 		}
    622 		if (n > 1024) {
    623 			n = 1024;
    624 		}
    625 		swdp_ahb_read32(x & (~3), tmp.w, ((n + 3) & (~3)) / 4);
    626 		gdb_puthex(gc, tmp.b + (x & 3), n);
    627 		break;
    628 	// M hexaddr , hexcount : hexbytes
    629 	// write to memory
    630 	case 'M': {
    631 		char *data = strchr((char*) cmd + 1, ':');
    632 		int len;
    633 		if (!data) {
    634 			break;
    635 		}
    636 		*data++ = 0;
    637 		if (sscanf((char*) cmd + 1, "%x,%x", &x, &n) != 2) {
    638 			break;
    639 		}
    640 		len = hextobin(gc->rxbuf, data, MAXPKT);
    641 		write_memory(x, gc->rxbuf, len);
    642 		gdb_puts(gc, "OK");
    643 		break;
    644 		}
    645 	// g
    646 	// read registers 0...
    647 	case 'g':  {
    648 		u32 regs[19];
    649 		if (gc->gselected && !gc->gselected->active) {
    650 			memset(regs, 0, sizeof(regs));
    651 			memcpy(regs, gc->gselected->regs, sizeof(gc->gselected->regs)); 
    652 		} else {
    653 			swdp_core_read_all(regs);
    654 		}
    655 		gdb_puthex(gc, regs, sizeof(regs));
    656 		break;
    657 	}
    658 	// G hexbytes
    659 	// write registers 0...
    660 	case 'G': {
    661 		if (gc->gselected && !gc->gselected->active) {
    662 			xprintf(XGDB, "gdb: attempting to write to inactive registers\n");
    663 			break;
    664 		}
    665 		int len = hextobin(gc->rxbuf, (char*) cmd + 1, MAXPKT);
    666 		for (n = 0; n < len / 4; n++) {
    667 			memcpy(&x, gc->rxbuf + (n * 4), sizeof(x));
    668 			swdp_core_write(n, x);
    669 		}
    670 		gdb_puts(gc, "OK");
    671 		break;
    672 	}
    673 	// p reghex
    674 	// read from register
    675 	case 'p': {
    676 		u32 v;
    677 		u32 n = strtoul((char*) cmd + 1, NULL, 16);
    678 		if (gc->gselected && !gc->gselected->active) {
    679 			if (n > 16) {
    680 				v = 0xeeeeeeee;
    681 			} else {
    682 				v = gc->gselected->regs[n];
    683 			}
    684 		} else {
    685 			swdp_core_read(n, &v);
    686 		}
    687 		gdb_puthex(gc, &v, sizeof(v));
    688 		break;
    689 	}
    690 	// P reghex = hexbytes
    691 	// write to register
    692 	case 'P': {
    693 		int len;
    694 		char *data = strchr((char*) cmd + 1, '=');
    695 		if (gc->gselected && !gc->gselected->active) {
    696 			xprintf(XGDB, "gdb: attempting to write to inactive registers\n");
    697 			break;
    698 		}
    699 		if (data) {
    700 			*data++ = 0;
    701 			n = strtoul((char*) cmd + 1, NULL, 16);
    702 			len = hextobin(gc->rxbuf, data, MAXPKT);
    703 			if (len != 4) break;
    704 			memcpy(&x, gc->rxbuf, sizeof(x));
    705 			swdp_core_write(n, x);
    706 			gdb_puts(gc, "OK");
    707 		}
    708 		break;
    709 	}
    710 	// halt (^c)
    711 	case '$':
    712 		swdp_core_halt();
    713 		gdb_update_threads(gc);
    714 		gc->flags &= (~F_RUNNING);
    715 		gdb_puts(gc, "S05");
    716 		break;
    717 	// extended query and set commands
    718 	case 'q': 
    719 	case 'Q': {
    720 		char *args = (char*) (cmd + 1);
    721 		while (*args) {
    722 			if ((*args == ':') || (*args == ',')) {
    723 				*args++ = 0;
    724 				break;
    725 			}
    726 			args++;
    727 		}
    728 		if (cmd[0] == 'q') {
    729 			handle_query(gc, (char*) (cmd + 1), args);
    730 		} else {
    731 			handle_set(gc, (char*) (cmd + 1), args);
    732 		}
    733 		break;
    734 	}
    735 	case 'z':
    736 	case 'Z': {
    737 		u32 type, addr, kind;
    738 		if (sscanf((char*) cmd + 1, "%x,%x,%x", &type, &addr, &kind) != 3) {
    739 			break;
    740 		}
    741 		if (type != 1) {
    742 			// only support hw breakpoints
    743 		}
    744 		if (handle_breakpoint(cmd[0] == 'Z', addr, kind)) {
    745 			gdb_puts(gc, "E1");
    746 		} else {
    747 			gdb_puts(gc, "OK");
    748 		}
    749 		break;
    750 	}
    751 	default:
    752 		xprintf(XGDB, "gdb: unknown command: %c\n", cmd[0]);
    753 	}
    754 	gdb_epilogue(gc);
    755 }
    756 
    757 static int pipefds[2] = { -1, -1 };
    758 
    759 void signal_gdb_server(void) {
    760 	if (pipefds[1] >= 0) {
    761 		char x = 0;
    762 		if (write(pipefds[1], &x, 1) < 0) ;
    763 	}
    764 }
    765 
    766 static struct gdbcnxn *active_gc = NULL;
    767 
    768 void gdb_console_puts(const char *msg) {
    769 	if (active_gc == NULL) return;
    770 	if (!(active_gc->flags & F_CONSOLE)) return;
    771 	gdb_prologue(active_gc);
    772 	gdb_putc(active_gc, 'O');
    773 	gdb_puthex(active_gc, msg, strlen(msg));
    774 	gdb_epilogue(active_gc);
    775 }
    776 
    777 void gdb_server(int fd) {
    778 	struct pollfd fds[2];
    779 	struct gdbcnxn gc;
    780 	unsigned char iobuf[32768];
    781 	unsigned char *ptr;
    782 	int r, len;
    783 
    784 	gdb_init(&gc, fd);
    785 
    786 	debugger_lock();
    787 	active_gc = &gc;
    788 	if (pipefds[0] == -1) {
    789 		if (pipe(pipefds)) ;
    790 	}
    791 	xprintf(XGDB,"[ gdb connected ]\n");
    792 	debugger_unlock();
    793 
    794 	gc.flags |= F_LK_THREADS;
    795 
    796 	for (;;) {
    797 
    798 		fds[0].fd = fd;
    799 		fds[0].events = POLLIN;
    800 		fds[0].revents = 0;
    801 		fds[1].fd = pipefds[0];
    802 		fds[1].events = POLLIN;
    803 		fds[1].revents = 0;
    804 
    805 		// check ahead of the poll, since we may have
    806 		// just resume'd a single-step that will have
    807 		// halted again by now
    808 		debugger_lock();
    809 		if (gc.flags & F_RUNNING) {
    810 			u32 csr;
    811 			if (swdp_ahb_read(CDBG_CSR, &csr) == 0) {
    812 				if (csr & CDBG_S_HALT) {
    813 					gc.flags &= (~F_RUNNING);
    814 					// todo: indicate specific halt reason
    815 					gdb_prologue(&gc);
    816 					gdb_puts(&gc, "S05");
    817 					gdb_epilogue(&gc);
    818 					gdb_update_threads(&gc);
    819 				}
    820 			}
    821 		}
    822 		debugger_unlock();
    823 
    824 		r = poll(fds, 2, (gc.flags & F_RUNNING) ? 10 : -1);
    825 		if (r < 0) {
    826 			if (errno == EINTR) continue;
    827 			break;
    828 		}
    829 		if (r == 0) {
    830 			continue;
    831 		}
    832 		if (fds[0].revents & POLLIN) {
    833 			r = read(fd, iobuf, sizeof(iobuf));
    834 			if (r <= 0) {
    835 				if (errno == EINTR) continue;
    836 				break;
    837 			}
    838 			len = r;
    839 			ptr = iobuf;
    840 			while (len > 0) {
    841 				r = gdb_recv(&gc, ptr, len);
    842 				ptr += r;
    843 				len -= r;
    844 			}
    845 		}
    846 		if (fds[1].revents & POLLIN) {
    847 			char x;
    848 			if (read(fds[1].fd, &x, 1) < 0) ;
    849 		}
    850 	}
    851 
    852 	debugger_lock();
    853 	active_gc = NULL;
    854 	xprintf(XGDB, "[ gdb connected ]\n");
    855 	debugger_unlock();
    856 }