m3dev

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

debugger-commands.c (22338B)


      1 /* debugger-commands.);
      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 <unistd.h>
     21 #include <string.h>
     22 #include <ctype.h>
     23 #include <stdarg.h>
     24 
     25 #include <fcntl.h>
     26 #include <sys/time.h>
     27 
     28 #include <fw/types.h>
     29 #include <protocol/rswdp.h>
     30 #include "rswdp.h"
     31 #include "arm-v7m.h"
     32 
     33 #include "debugger.h"
     34 #include "lkdebug.h"
     35 
     36 #define _AGENT_HOST_ 1
     37 #include <agent/flash.h>
     38 
     39 extern struct debugger_command debugger_commands[];
     40 
     41 long long now() {
     42 	struct timeval tv;
     43 	gettimeofday(&tv, 0);
     44 	return ((long long) tv.tv_usec) + ((long long) tv.tv_sec) * 1000000LL;
     45 }
     46 
     47 extern int disassemble_thumb2(u32 addr, u16 op0, u16 op1,
     48 		char *text, int len) __attribute__ ((weak));
     49 
     50 int disassemble(u32 addr) {
     51 	char text[128];
     52 	int r;
     53 	union {
     54 		u32 w[2];
     55 		u16 h[4];
     56 	} mem;
     57 
     58 #if WITH_THUMB2_DISASSEMBLE
     59 	if (!disassemble_thumb2)
     60 		return -1;
     61 
     62 	if (addr & 2) {
     63 		if (swdp_ahb_read32(addr & (~3), mem.w, 2))
     64 			return -1;
     65 		r = disassemble_thumb2(addr, mem.h[1], mem.h[2], text, 128);
     66 	} else {
     67 		if (swdp_ahb_read32(addr & (~3), mem.w, 1))
     68 			return -1;
     69 		r = disassemble_thumb2(addr, mem.h[0], mem.h[1], text, 128);
     70 	}
     71 	if (r > 0)
     72 		xprintf(XDATA, "%s\n", text);
     73 	return r;
     74 #else
     75 	return -1;
     76 #endif
     77 }
     78 
     79 int do_exit(int argc, param *argv) {
     80 	exit(0);
     81 	return 0;
     82 }
     83 
     84 int do_attach(int argc, param *argv) {
     85 	return swdp_reset();
     86 }
     87 
     88 static u32 lastregs[19];
     89 
     90 int do_regs(int argc, param *argv) {
     91 	if (swdp_core_read_all(lastregs))
     92 		return -1;
     93 
     94 	xprintf(XDATA, "r0 %08x r4 %08x r8 %08x ip %08x psr %08x\n",
     95 		lastregs[0], lastregs[4], lastregs[8],
     96 		lastregs[12], lastregs[16]);
     97 	xprintf(XDATA, "r1 %08x r5 %08x r9 %08x sp %08x msp %08x\n",
     98 		lastregs[1], lastregs[5], lastregs[9],
     99 		lastregs[13], lastregs[17]);
    100 	xprintf(XDATA, "r2 %08x r6 %08x 10 %08x lr %08x psp %08x\n",
    101 		lastregs[2], lastregs[6], lastregs[10],
    102 		lastregs[14], lastregs[18]);
    103 	xprintf(XDATA, "r3 %08x r7 %08x 11 %08x pc %08x\n",
    104 		lastregs[3], lastregs[7], lastregs[11],
    105 		lastregs[15]);
    106 	disassemble(lastregs[15]);
    107 	return 0;
    108 }
    109 
    110 int do_stop(int argc, param *argv) {
    111 	swdp_core_halt();
    112 	do_regs(0, 0);
    113 	return 0;
    114 }
    115 
    116 int do_resume(int argc, param *argv) {
    117 	swdp_core_resume();
    118 	return 0;
    119 }
    120 
    121 int do_step(int argc, param *argv) {
    122 	if (argc > 0) {
    123 		u32 pc;
    124 		do {
    125 			swdp_core_step();
    126 			swdp_core_wait_for_halt();
    127 			if (swdp_core_read(15, &pc)) {
    128 				xprintf(XCORE, "step: error\n");
    129 				return -1;
    130 			}
    131 			fflush(stdout);
    132 		} while (pc != argv[0].n);
    133 	} else {
    134 		swdp_core_step();
    135 		swdp_core_wait_for_halt();
    136 	}
    137 	do_regs(0, 0);
    138 	return 0;
    139 }
    140 
    141 struct {
    142 	const char *name;
    143 	unsigned n;
    144 } core_regmap[] = {
    145 	{ "r0", 0 },
    146 	{ "r1", 1 },
    147 	{ "r2", 2 },
    148 	{ "r3", 3 },
    149 	{ "r4", 4 },
    150 	{ "r5", 5 },
    151 	{ "r6", 6 },
    152 	{ "r7", 7 },
    153 	{ "r8", 8 },
    154 	{ "r9", 9 },
    155 	{ "r10", 10 },
    156 	{ "r11", 11 },
    157 	{ "r12", 12 },
    158 	{ "r13", 13 }, { "sp", 13 },
    159 	{ "r14", 14 }, { "lr", 14 },
    160 	{ "r15", 15 }, { "pc", 15 },
    161 	{ "psr", 16 },
    162 	{ "msp", 17 },
    163 	{ "psp", 18 },
    164 };
    165 
    166 int read_memory_word(u32 addr, u32 *value) {
    167 	return swdp_ahb_read(addr, value);
    168 }
    169 
    170 int read_register(const char *name, u32 *value) {
    171 	int n;
    172 	for (n = 0; n < (sizeof(core_regmap) / sizeof(core_regmap[0])); n++) {
    173 		if (!strcasecmp(name, core_regmap[n].name)) {
    174 			if (swdp_core_read(core_regmap[n].n, value))
    175 				return -1;
    176 			return 0;
    177 		}
    178 	}
    179 	return ERROR_UNKNOWN;
    180 }
    181 
    182 int do_dr(int argc, param *argv) {
    183 	unsigned n;
    184 	u32 x = 0xeeeeeeee;
    185 	if (argc < 1)
    186 		return -1;
    187 	for (n = 0; n < (sizeof(core_regmap) / sizeof(core_regmap[0])); n++) {
    188 		if (!strcasecmp(argv[0].s, core_regmap[n].name)) {
    189 			swdp_core_read(core_regmap[n].n, &x);
    190 			xprintf(XDATA, "%s: %08x\n", argv[0].s, x);
    191 			return 0;
    192 		}
    193 	}
    194 	swdp_ahb_read(argv[0].n, &x);
    195 	xprintf(XDATA, "%08x: %08x\n", argv[0].n, x);
    196 	return 0;
    197 }
    198 
    199 int do_wr(int argc, param *argv) {
    200 	unsigned n;
    201 	if (argc < 2)
    202 		return -1;
    203 	for (n = 0; n < (sizeof(core_regmap) / sizeof(core_regmap[0])); n++) {
    204 		if (!strcasecmp(argv[0].s, core_regmap[n].name)) {
    205 			swdp_core_write(core_regmap[n].n, argv[1].n);
    206 			xprintf(XDATA, "%s<<%08x\n", argv[0].s, argv[1].n);
    207 			return 0;
    208 		}
    209 	}
    210 	swdp_ahb_write(argv[0].n, argv[1].n);
    211 	xprintf(XDATA, "%08x<<%08x\n", argv[0].n, argv[1].n);
    212 	return 0;
    213 }
    214 
    215 int do_text(int argc, param *argv) {
    216 	u8 data[1024], *x;
    217 	u32 addr;
    218 
    219 	if (argc < 1)
    220 		return -1;
    221 	addr = argv[0].n;
    222 	memset(data, 0, sizeof(data));
    223 
    224 	if (swdp_ahb_read32(addr, (void*) data, sizeof(data)/4))
    225 		return -1;
    226 
    227 	data[sizeof(data)-1] = 0;
    228 	for (x = data; *x; x++) {
    229 		if ((*x >= ' ') && (*x < 128))
    230 			continue;
    231 		if (*x == '\n')
    232 			continue;
    233 		*x = '.';
    234 	}
    235 	xprintf(XDATA, "%08x: %s\n", addr, (char*) data);
    236 	return 0;
    237 }
    238 
    239 static u32 lastaddr = 0x20000000;
    240 static u32 lastcount = 0x40;
    241 
    242 int do_dw(int argc, param *argv) {
    243 	u32 data[4096];
    244 	u32 addr = lastaddr;
    245 	u32 count = lastcount;
    246 	unsigned n;
    247 
    248 	if (argc > 0) addr = argv[0].n;
    249 	if (argc > 1) count = argv[1].n;
    250 
    251 	memset(data, 0xee, 256 *4);
    252 
    253 	/* word align */
    254 	addr = (addr + 3) & ~3;
    255 	count = (count + 3) & ~3;
    256 	if (count > sizeof(data))
    257 		count = sizeof(data);
    258 
    259 	lastaddr = addr + count;
    260 	lastcount = count;
    261 
    262 	count /= 4;
    263 	if (swdp_ahb_read32(addr, data, count))
    264 		return -1;
    265 
    266 	for (n = 0; count > 0; n += 4, addr += 16) {
    267 		switch (count) {
    268 		case 1:
    269 			count = 0;
    270 			xprintf(XDATA, "%08x: %08x\n", addr, data[n]);
    271 			break;
    272 		case 2:
    273 			count = 0;
    274 			xprintf(XDATA, "%08x: %08x %08x\n",
    275 				addr, data[n], data[n+1]);
    276 			break;
    277 		case 3:
    278 			count = 0;
    279 			xprintf(XDATA, "%08x: %08x %08x %08x\n",
    280 				addr, data[n], data[n+1], data[n+2]);
    281 			break;
    282 		default:
    283 			count -= 4;
    284 			xprintf(XDATA, "%08x: %08x %08x %08x %08x\n",
    285 				addr, data[n], data[n+1], data[n+2], data[n+3]);
    286 			break;
    287 		}
    288 	}
    289 	return 0;
    290 }
    291 
    292 
    293 int do_db(int argc, param *argv) {
    294 	u32 addr, count;
    295 	u8 data[1024];
    296 	char line[256];
    297 	unsigned n, m, xfer;
    298 
    299 	if (argc < 2)
    300 		return -1;
    301 
    302 	addr = argv[0].n;
    303 	count = argv[1].n;
    304 
    305 	if (count > 1024)
    306 		count = 1024;
    307 
    308 	memset(data, 0xee, 1024);
    309 	// todo: fix this
    310 	swdp_ahb_write(AHB_CSW, AHB_CSW_MDEBUG | AHB_CSW_PRIV |
    311 		AHB_CSW_DBG_EN | AHB_CSW_8BIT);
    312 	for (n = 0; n < count; n++) {
    313 		u32 tmp;
    314 		if (swdp_ahb_read(addr + n, &tmp)) {
    315 			swdp_reset();
    316 			break;
    317 		}
    318 		data[n] = tmp >> (8 * (n & 3));
    319 	}
    320 	swdp_ahb_write(AHB_CSW, AHB_CSW_MDEBUG | AHB_CSW_PRIV |
    321 		AHB_CSW_DBG_EN | AHB_CSW_32BIT);
    322 
    323 	for (n = 0; count > 0; count -= xfer) {
    324 		xfer = (count > 16) ? 16 : count;
    325 		char *p = line + sprintf(line, "%08x:", addr + n);
    326 		for (m = 0; m < xfer; m++) {
    327 			p += sprintf(p, " %02x", data[n++]);
    328 		}
    329 		xprintf(XDATA, "%s\n", line);
    330 	}
    331 	return 0;
    332 }
    333 
    334 // vector catch flags to apply
    335 u32 vcflags = DEMCR_VC_HARDERR | DEMCR_VC_BUSERR | DEMCR_VC_STATERR | DEMCR_VC_CHKERR;
    336 
    337 int do_reset(int argc, param *argv) {
    338 	swdp_core_halt();
    339 	swdp_ahb_write(DEMCR, DEMCR_TRCENA | vcflags);
    340 	/* core reset and sys reset */
    341 	swdp_ahb_write(0xe000ed0c, 0x05fa0005);
    342 	swdp_ahb_write(DEMCR, DEMCR_TRCENA | vcflags);
    343 	return 0;
    344 }
    345 
    346 int do_reset_hw(int argc, param *argv) {
    347 	swdp_target_reset(1);
    348 	usleep(10000);
    349 	swdp_target_reset(0);
    350 	usleep(10000);
    351 	return 0;
    352 }
    353 
    354 int do_reset_stop(int argc, param *argv) {
    355 	swdp_core_halt();
    356 	// enable vector-trap on reset, enable DWT/FPB
    357 	swdp_ahb_write(DEMCR, DEMCR_VC_CORERESET | DEMCR_TRCENA | vcflags);
    358 	// core reset and sys reset
    359 	swdp_ahb_write(0xe000ed0c, 0x05fa0005);
    360 	//swdp_core_wait_for_halt();
    361 	do_stop(0,0);
    362 	swdp_ahb_write(DEMCR, DEMCR_TRCENA | vcflags);
    363 	return 0;
    364 }
    365 
    366 int do_watch_pc(int argc, param *argv) {
    367 	if (argc < 1)
    368 		return -1;
    369 	return swdp_watchpoint_pc(0, argv[0].n);
    370 }
    371 
    372 int do_watch_rw(int argc, param *argv) {
    373 	if (argc < 1)
    374 		return -1;
    375 	return swdp_watchpoint_rw(0, argv[0].n);
    376 }
    377 
    378 int do_watch_off(int argc, param *argv) {
    379 	return swdp_watchpoint_disable(0);
    380 }
    381 
    382 int do_print(int argc, param *argv) {
    383 	while (argc-- > 0)
    384 		xprintf(XCORE, "%08x\n", argv++[0].n);
    385 	return 0;
    386 }
    387 
    388 int do_echo(int argc, param *argv) {
    389 	while (argc-- > 0) {
    390 		unsigned int argn = argv[0].n;
    391 		const char *arg = argv++[0].s;
    392 
    393 		if (arg[0] == '$') {
    394 			xprintf(XCORE, "%08x\n", argn);
    395 		} else {
    396 			xprintf(XCORE, "%s\n", arg);
    397 		}
    398 	}
    399 	return 0;
    400 }
    401 
    402 int do_bootloader(int argc, param *argv) {
    403 	return swdp_bootloader();
    404 }
    405 
    406 int do_setclock(int argc, param *argv) {
    407 	if (argc < 1)
    408 		return -1;
    409 	return swdp_set_clock(argv[0].n);
    410 }
    411 
    412 int do_swoclock(int argc, param *argv) {
    413 	if (argc < 1)
    414 		return -1;
    415 	return swo_set_clock(argv[0].n);
    416 }
    417 
    418 int do_help(int argc, param *argv) {
    419 	struct debugger_command *cmd;
    420 	for (cmd = debugger_commands; cmd->func != NULL; cmd++) {
    421 		xprintf(XCORE, "%-16s: %s\n", cmd->name, cmd->help);
    422 	}
    423 
    424 	return 0;
    425 }
    426 
    427 void *get_builtin_file(const char *fn, size_t *sz);
    428 
    429 void *load_file(const char *fn, size_t *_sz) {
    430 	int fd;
    431 	off_t sz;
    432 	void *data = NULL;
    433 	fd = open(fn, O_RDONLY);
    434 	if (fd < 0) goto fail;
    435 	sz = lseek(fd, 0, SEEK_END);
    436 	if (sz < 0) goto fail;
    437 	if (lseek(fd, 0, SEEK_SET)) goto fail;
    438 	if ((data = malloc(sz + 4)) == NULL) goto fail;
    439 	if (read(fd, data, sz) != sz) goto fail;
    440 	*_sz = sz;
    441 	return data;
    442 fail:
    443 	if (data) free(data);
    444 	if (fd >= 0) close(fd);
    445 	return NULL;
    446 }
    447 
    448 int do_download(int argc, param *argv) {
    449 	u32 addr;
    450 	void *data;
    451 	size_t sz;
    452 	long long t0, t1;
    453 
    454 	if (argc != 2) {
    455 		xprintf(XCORE, "error: usage: download <file> <addr>\n");
    456 		return -1;
    457 	}
    458 
    459 	if ((data = load_file(argv[0].s, &sz)) == NULL) {
    460 		xprintf(XCORE, "error: cannot read '%s'\n", argv[0].s);
    461 		return -1;
    462 	}
    463 	sz = (sz + 3) & ~3;
    464 	addr = argv[1].n;
    465 
    466 	xprintf(XCORE, "sending %d bytes...\n", sz);
    467 	t0 = now();
    468 	if (swdp_ahb_write32(addr, (void*) data, sz / 4)) {
    469 		xprintf(XCORE, "error: failed to write data\n");
    470 		free(data);
    471 		return -1;
    472 	}
    473 	t1 = now();
    474 	xprintf(XCORE, "%lld uS -> %lld B/s\n", (t1 - t0), 
    475 		(((long long)sz) * 1000000LL) / (t1 - t0));
    476 	free(data);
    477 	return 0;
    478 }
    479 
    480 int do_run(int argc, param *argv) {
    481 	u32 addr;
    482 	void *data;
    483 	size_t sz;
    484 	u32 sp, pc;
    485 	if (argc != 2) {
    486 		xprintf(XCORE, "error: usage: run <file> <addr>\n");
    487 		return -1;
    488 	}
    489 	if ((data = load_file(argv[0].s, &sz)) == NULL) {
    490 		xprintf(XCORE, "error: cannot read '%s'\n", argv[0].s);
    491 		return -1;
    492 	}
    493 	swdp_core_halt();
    494 	sz = (sz + 3) & ~3;
    495 	addr = argv[1].n;
    496 	if (swdp_ahb_write32(addr, (void*) data, sz / 4)) {
    497 		xprintf(XCORE, "error: failed to write data\n");
    498 		free(data);
    499 		return -1;
    500 	}
    501 	memcpy(&sp, data, 4);
    502 	memcpy(&pc, ((char*) data) + 4, 4);
    503 	swdp_core_write(13, sp);
    504 	swdp_core_write(15, pc);
    505 	swdp_ahb_write(0xe000ed0c, 0x05fa0002);
    506 	swdp_core_write(16, 0x01000000);
    507 	swdp_core_resume();
    508 	free(data);
    509 	return 0;
    510 }
    511 
    512 
    513 void *load_agent(const char *arch, size_t *_sz) {
    514 	void *data;
    515 	size_t sz;
    516 	char name[256];
    517 	if (arch == NULL) return NULL;
    518 	snprintf(name, 256, "agent-%s.bin", arch);
    519 	if ((data = get_builtin_file(name, &sz))) {
    520 		void *copy = malloc(sz + 4);
    521 		if (copy == NULL) return NULL;
    522 		memcpy(copy, data, sz);
    523 		*_sz = sz;
    524 		return copy;
    525 	}
    526 	snprintf(name, sizeof(name), "out/agent-%s.bin", arch);
    527 	return load_file(name, _sz);
    528 }
    529 
    530 static char *agent_arch = NULL;
    531 
    532 int do_setarch(int argc, param *argv) {
    533 	char *x;
    534 	if (argc != 1) return -1;
    535 	if((x = strdup(argv[0].s))) {
    536 		free(agent_arch);
    537 		agent_arch = x;
    538 	}
    539 	return 0;
    540 }
    541 
    542 int invoke(u32 agent, u32 func, u32 r0, u32 r1, u32 r2, u32 r3) {
    543 	swdp_core_write(0, r0);
    544 	swdp_core_write(1, r1);
    545 	swdp_core_write(2, r2);
    546 	swdp_core_write(3, r3);
    547 	swdp_core_write(13, agent - 4);
    548 	swdp_core_write(14, agent | 1); // include T bit
    549 	swdp_core_write(15, func | 1); // include T bit
    550 
    551 	// if the target has bogus data at 0, the processor may be in
    552 	// pending-exception state after reset-stop, so we will clear
    553 	// any exceptions and then set the PSR to something reasonable
    554 
    555 	// Write VECTCLRACTIVE to AIRCR
    556 	swdp_ahb_write(0xe000ed0c, 0x05fa0002);
    557 	swdp_core_write(16, 0x01000000);
    558 
    559 	// todo: readback and verify?
    560 
    561 	xprintf(XCORE, "invoke <func@%08x>(0x%x,0x%x,0x%x,0x%x)\n", func, r0, r1, r2, r3);
    562 
    563 	swdp_core_resume();
    564 	if (swdp_core_wait_for_halt() == 0) {
    565 		// todo: timeout after a few seconds?
    566 		u32 pc = 0xffffffff, res = 0xffffffff;
    567 		swdp_core_read(0, &res);
    568 		swdp_core_read(15, &pc);
    569 		if (pc != agent) {
    570 			xprintf(XCORE, "error: pc (%08x) is not at %08x\n", pc, agent);
    571 			return -1;
    572 		}
    573 		if (res) xprintf(XCORE, "failure code %08x\n", res);
    574 		return res;
    575 	}
    576 	xprintf(XCORE, "interrupted\n");
    577 	return -1;
    578 }
    579 
    580 int run_flash_agent(u32 flashaddr, void *data, size_t data_sz) {
    581 	flash_agent *agent = NULL;
    582 	size_t agent_sz;
    583 
    584 	if ((agent = load_agent(agent_arch, &agent_sz)) == NULL) {
    585 		xprintf(XCORE, "error: cannot load flash agent for architecture '%s'\n",
    586 			agent_arch ? agent_arch : "unknown");
    587 		xprintf(XCORE, "error: set architecture with: arch <name>\n");
    588 		goto fail;
    589 	}
    590 	// sanity check
    591 	if ((agent_sz < sizeof(flash_agent)) ||
    592 		(agent->magic != AGENT_MAGIC) ||
    593 		(agent->version != AGENT_VERSION)) {
    594 		xprintf(XCORE, "error: invalid agent image\n");
    595 		goto fail;
    596 	}
    597 	// replace magic with bkpt instructions
    598 	agent->magic = 0xbe00be00;
    599 
    600 	if (do_attach(0,0)) {
    601 		xprintf(XCORE, "error: failed to attach\n");
    602 		goto fail;
    603 	}
    604 	do_reset_stop(0,0);
    605 
    606 	if (agent->flags & FLAG_BOOT_ROM_HACK) {
    607 		xprintf(XCORE, "executing boot rom\n");
    608 		if (swdp_watchpoint_rw(0, 0)) {
    609 			goto fail;
    610 		}
    611 		swdp_core_resume();
    612 		swdp_core_wait_for_halt();
    613 		swdp_watchpoint_disable(0);
    614 		// todo: timeout?
    615 		// todo: confirm halted
    616 	}
    617 
    618 	if (swdp_ahb_write32(agent->load_addr, (void*) agent, agent_sz / 4)) {
    619 		xprintf(XCORE, "error: failed to download agent\n");
    620 		goto fail;
    621 	}
    622 	if (invoke(agent->load_addr, agent->setup, 0, 0, 0, 0)) {
    623 		goto fail;
    624 	}
    625 	if (swdp_ahb_read32(agent->load_addr + 16, (void*) &agent->data_addr, 4)) {
    626 		goto fail;
    627 	}
    628 	xprintf(XCORE, "agent %d @%08x, buffer %dK @%08x, flash %dK @%08x\n",
    629 		agent_sz, agent->load_addr,
    630 		agent->data_size / 1024, agent->data_addr,
    631 		agent->flash_size / 1024, agent->flash_addr);
    632 
    633 	if ((flashaddr == 0) && (data == NULL) && (data_sz == 0xFFFFFFFF)) {
    634 		// erase all
    635 		flashaddr = agent->flash_addr;
    636 		data_sz = agent->flash_size;
    637 	}
    638 
    639 	if ((flashaddr < agent->flash_addr) ||
    640 		(data_sz > agent->flash_size) ||
    641 		((flashaddr + data_sz) > (agent->flash_addr + agent->flash_size))) {
    642 		xprintf(XCORE, "invalid flash address %08x\n", flashaddr);
    643 		goto fail;
    644 	}
    645 
    646 	if (data == NULL) {
    647 		// erase
    648 		if (invoke(agent->load_addr, agent->erase, flashaddr, data_sz, 0, 0)) {
    649 			xprintf(XCORE, "failed to erase %d bytes at %08x\n", data_sz, flashaddr);
    650 			goto fail;
    651 		}
    652 	} else {
    653 		// write
    654 		u8 *ptr = (void*) data;
    655 		u32 xfer;
    656 		xprintf(XCORE, "flashing %d bytes at %08x...\n", data_sz, flashaddr);
    657 		if (invoke(agent->load_addr, agent->erase, flashaddr, data_sz, 0, 0)) {
    658 			xprintf(XCORE, "failed to erase %d bytes at %08x\n", data_sz, flashaddr);
    659 			goto fail;
    660 		}
    661 		while (data_sz > 0) {
    662 			if (data_sz > agent->data_size) {
    663 				xfer = agent->data_size;
    664 			} else {
    665 				xfer = data_sz;
    666 			}
    667 			if (swdp_ahb_write32(agent->data_addr, (void*) ptr, xfer / 4)) {
    668 				xprintf(XCORE, "download to %08x failed\n", agent->data_addr);
    669 				goto fail;
    670 			}
    671 			if (invoke(agent->load_addr, agent->write,
    672 				flashaddr, agent->data_addr, xfer, 0)) {
    673 				xprintf(XCORE, "failed to flash %d bytes to %08x\n", xfer, flashaddr);
    674 				goto fail;
    675 			}
    676 			ptr += xfer;
    677 			data_sz -= xfer;
    678 			flashaddr += xfer;
    679 		}
    680 	}
    681 
    682 	free(agent);
    683 	if (data) free(data);
    684 	return 0;
    685 fail:
    686 	if (agent) free(agent);
    687 	if (data) free(data);
    688 	return -1;
    689 }
    690 
    691 int do_flash(int argc, param *argv) {
    692 	void *data = NULL;
    693 	size_t data_sz;
    694 	if (argc != 2) {
    695 		xprintf(XCORE, "error: usage: flash <file> <addr>\n");
    696 		return -1;
    697 	}
    698 	if ((data = load_file(argv[0].s, &data_sz)) == NULL) {
    699 		xprintf(XCORE, "error: cannot load '%s'\n", argv[0].s);
    700 		return -1;
    701 	}
    702 	// word align
    703 	data_sz = (data_sz + 3) & ~3;
    704 	return run_flash_agent(argv[1].n, data, data_sz);
    705 }
    706 
    707 int do_erase(int argc, param *argv) {
    708 	if ((argc == 1) && !strcmp(argv[0].s, "all")) {
    709 		return run_flash_agent(0, NULL, 0xFFFFFFFF);
    710 	}
    711 	if (argc != 2) {
    712 		xprintf(XCORE, "error: usage: erase <addr> <length> | erase all\n");
    713 		return -1;
    714 	}
    715 	return run_flash_agent(argv[0].n, NULL, argv[1].n);
    716 }
    717 
    718 int do_log(int argc, param *argv) {
    719 	unsigned flags = 0;
    720 	while (argc > 0) {
    721 		if (!strcmp(argv[0].s, "gdb")) {
    722 			flags |= LF_GDB;
    723 		} else if (!strcmp(argv[0].s, "swd")) {
    724 			flags |= LF_SWD;
    725 		} else {
    726 			xprintf(XCORE, "error: allowed flags: gdb swd\n");
    727 			return -1;
    728 		}
    729 		argc--;
    730 		argv++;
    731 	}
    732 	log_flags = flags;
    733 	return 0;
    734 }
    735 
    736 int do_finfo(int argc, param *argv) {
    737 	u32 cfsr = 0, hfsr = 0, dfsr = 0, mmfar = 0, bfar = 0;
    738 	swdp_ahb_read(CFSR, &cfsr);
    739 	swdp_ahb_read(HFSR, &hfsr);
    740 	swdp_ahb_read(DFSR, &dfsr);
    741 	swdp_ahb_read(MMFAR, &mmfar);
    742 	swdp_ahb_read(BFAR, &bfar);
    743 
    744 	xprintf(XDATA, "CFSR %08x  MMFAR %08x\n", cfsr, mmfar);
    745 	xprintf(XDATA, "HFSR %08x   BFAR %08x\n", hfsr, bfar);
    746 	xprintf(XDATA, "DFSR %08x\n", dfsr);
    747 
    748 	if (cfsr & CFSR_IACCVIOL)	xprintf(XDATA, ">MM: Inst Access Violation\n");
    749 	if (cfsr & CFSR_DACCVIOL)	xprintf(XDATA, ">MM: Data Access Violation\n");
    750 	if (cfsr & CFSR_MUNSTKERR)	xprintf(XDATA, ">MM: Derived MM Fault on Exception Return\n");
    751 	if (cfsr & CFSR_MSTKERR)	xprintf(XDATA, ">MM: Derived MM Fault on Exception Entry\n");
    752 	if (cfsr & CFSR_MLSPERR)	xprintf(XDATA, ">MM: MM Fault During Lazy FP Save\n");
    753 	if (cfsr & CFSR_MMARVALID)	xprintf(XDATA, ">MM: MMFAR has valid contents\n");
    754 
    755 	if (cfsr & CFSR_IBUSERR)	xprintf(XDATA, ">BF: Bus Fault on Instruction Prefetch\n");
    756 	if (cfsr & CFSR_PRECISERR)	xprintf(XDATA, ">BF: Precise Data Access Error, Addr in BFAR\n");
    757 	if (cfsr & CFSR_IMPRECISERR)	xprintf(XDATA, ">BF: Imprecise Data Access Error\n");
    758 	if (cfsr & CFSR_UNSTKERR)	xprintf(XDATA, ">BF: Derived Bus Fault on Exception Return\n");
    759 	if (cfsr & CFSR_STKERR)		xprintf(XDATA, ">BF: Derived Bus Fault on Exception Entry\n");
    760 	if (cfsr & CFSR_LSPERR)		xprintf(XDATA, ">BF: Bus Fault During Lazy FP Save\n");
    761 	if (cfsr & CFSR_BFARVALID)	xprintf(XDATA, ">BF: BFAR has valid contents\n");
    762 
    763 	if (cfsr & CFSR_UNDEFINSTR)	xprintf(XDATA, ">UF: Undefined Instruction Usage Fault\n");
    764 	if (cfsr & CFSR_INVSTATE)	xprintf(XDATA, ">UF: EPSR.T or ESPR.IT invalid\n");
    765 	if (cfsr & CFSR_INVPC)		xprintf(XDATA, ">UF: Integrity Check Error on EXC_RETURN\n");
    766 	if (cfsr & CFSR_NOCP)		xprintf(XDATA, ">UF: Coprocessor Error\n");
    767 	if (cfsr & CFSR_UNALIGNED)	xprintf(XDATA, ">UF: Unaligned Access Error\n");
    768 	if (cfsr & CFSR_DIVBYZERO)	xprintf(XDATA, ">UF: Divide by Zero\n");
    769 
    770 	if (hfsr & HFSR_VECTTBL)	xprintf(XDATA, ">HF: Vector Table Read Fault\n");
    771 	if (hfsr & HFSR_FORCED)		xprintf(XDATA, ">HF: Exception Escalated to Hard Fault\n");
    772 	if (hfsr & HFSR_DEBUGEVT)	xprintf(XDATA, ">HF: Debug Event\n");
    773 
    774 	// clear sticky fault bits
    775 	swdp_ahb_write(CFSR, CFSR_ALL);
    776 	swdp_ahb_write(HFSR, HFSR_ALL);
    777 	return 0;
    778 }
    779 
    780 extern int swdp_step_no_ints;
    781 
    782 int do_maskints(int argc, param *argv) {
    783 	if (argc != 1) {
    784 		xprintf(XCORE, "usage: maskints [on|off|always]\n");
    785 		return -1;
    786 	}
    787 	if (!strcmp(argv[0].s, "on")) {
    788 		swdp_step_no_ints = 1;
    789 		xprintf(XCORE, "maskints: while stepping\n");
    790 	} else if (!strcmp(argv[0].s, "always")) {
    791 		swdp_step_no_ints = 2;
    792 		xprintf(XCORE, "maskints: always\n");
    793 	} else {
    794 		swdp_step_no_ints = 0;
    795 		xprintf(XCORE, "maskints: never\n");
    796 	}
    797 	return 0;
    798 }
    799 
    800 int do_threads(int argc, param *argv) {
    801 	if (argc == 1) {
    802 		if (strcmp(argv[0].s, "clear")) {
    803 			xprintf(XCORE, "usage: threads [clear]\n");
    804 			return -1;
    805 		}
    806 		swdp_core_halt();
    807 		clear_lk_threads();
    808 		return 0;
    809 	}
    810 	lkthread_t *t;
    811 	swdp_core_halt();
    812 	t = find_lk_threads(1);
    813 	dump_lk_threads(t);
    814 	free_lk_threads(t);
    815 	return 0;
    816 }
    817 
    818 int remote_msg(u32 cmd) {
    819 	unsigned timeout = 250;
    820 	u32 n = 0;
    821 	if (swdp_ahb_write(DCRDR, cmd)) goto fail;
    822 	if (swdp_ahb_read(DEMCR, &n)) goto fail;
    823 	if (swdp_ahb_write(DEMCR, n | DEMCR_MON_PEND)) goto fail;
    824 	while (timeout > 0) {
    825 		if (swdp_ahb_read(DCRDR, &n)) goto fail;
    826 		if (!(n & 0x80000000)) return 0;
    827 		timeout--;
    828 	}
    829 	xprintf(XCORE, "console write timeout\n");
    830 	return -1;
    831 fail:
    832 	xprintf(XCORE, "console write io error\n");
    833 	return -1;
    834 }
    835 
    836 int do_wconsole(int argc, param *argv) {
    837 	if (argc != 1) return -1;
    838 	const char *line = argv[0].s;
    839 	while (*line) {
    840 		if (remote_msg(0x80000000 | *line)) return -1;
    841 		line++;
    842 	}
    843 	if (remote_msg(0x80000000 | '\r')) return -1;
    844 	return 0;
    845 }
    846 
    847 
    848 struct debugger_command debugger_commands[] = {
    849 	{ "exit",	"", do_exit,		"" },
    850 	{ "attach",	"", do_attach,		"attach/reattach to sw-dp" },
    851 	{ "regs",	"", do_regs,		"show cpu registers" },
    852 	{ "finfo",	"", do_finfo,		"Fault Information" },
    853 	{ "stop",	"", do_stop,		"halt cpu" },
    854 	{ "step",	"", do_step,		"single-step cpu" },
    855 	{ "go",		"", do_resume,		"resume cpu" },
    856 	{ "dw",		"", do_dw,		"dump words" },
    857 	{ "db",		"", do_db,		"dump bytes" },
    858 	{ "dr",		"", do_dr,		"dump register" },
    859 	{ "wr",		"", do_wr,		"write register" },
    860 	{ "download",	"", do_download,	"download file to device" },
    861 	{ "run",	"", do_run,		"download file and execute it" },
    862 	{ "flash",	"", do_flash,		"write file to device flash" },
    863 	{ "erase",	"", do_erase,		"erase flash" },
    864 	{ "reset",	"", do_reset,		"reset target" },
    865 	{ "reset-stop",	"", do_reset_stop,	"reset target and halt cpu" },
    866 	{ "reset-hw",	"", do_reset_hw,	"strobe /RESET pin" },
    867 	{ "watch-pc",	"", do_watch_pc,	"set watchpoint at addr" },
    868 	{ "watch-rw",	"", do_watch_rw,	"set watchpoint at addr" },
    869 	{ "watch-off",	"", do_watch_off,	"disable watchpoint" },
    870 	{ "log",	"", do_log,		"enable/disable logging" },
    871 	{ "maskints",	"", do_maskints,	"enable/disable IRQ mask during step" },
    872 	{ "print",	"", do_print,		"print numeric arguments" },
    873 	{ "echo",	"", do_echo,		"echo command line" },
    874 	{ "bootloader", "", do_bootloader,	"reboot into bootloader" },
    875 	{ "setclock",	"", do_setclock,	"set SWD clock rate (khz)" },
    876 	{ "swoclock",	"", do_swoclock,	"set SWO clock rate (khz)" },
    877 	{ "arch",	"", do_setarch,		"set architecture for flash agent" },
    878 	{ "threads",	"", do_threads,		"thread dump" },
    879 	{ "text",	"", do_text,		"dump text" },
    880 	{ "wconsole",	"", do_wconsole,	"write to remote console" },
    881 	{ "help",	"", do_help,		"help" },
    882 	{ 0, 0, 0, 0 },
    883 };
    884