mdebug

cortex m series debugger
git clone http://frotz.net/git/mdebug.git
Log | Files | Refs | README | LICENSE

rswdp.c (22434B)


      1 /* rswdp.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 <unistd.h>
     21 #include <string.h>
     22 
     23 #include <pthread.h>
     24 
     25 #include "usb.h"
     26 
     27 #include <fw/types.h>
     28 #include <protocol/rswdp.h>
     29 #include "rswdp.h"
     30 #include "arm-v7m.h"
     31 
     32 #include "debugger.h"
     33 
     34 int swd_verbose = 0;
     35 
     36 static pthread_mutex_t swd_lock = PTHREAD_MUTEX_INITIALIZER;
     37 static pthread_cond_t swd_event = PTHREAD_COND_INITIALIZER;
     38 static pthread_t swd_thread;
     39 
     40 // these are all protected by swd_lock
     41 static u16 sequence = 1;
     42 static int swd_online = 0;
     43 static usb_handle *usb = NULL;
     44 
     45 static unsigned swd_txn_id = 0;
     46 static void *swd_txn_data = NULL;
     47 static int swd_txn_status = 0;
     48 
     49 #define TXN_STATUS_WAIT		-2
     50 #define TXN_STATUS_FAIL		-1
     51 
     52 // swd_thread is responsible for setting swd_online to 1 once
     53 // the USB connection is active.
     54 //
     55 // In the event of a usb connection error, swd_thread sets
     56 // swd_online to -1, and the next swd io attempt must acknowledge
     57 // this by zeroing the usb handle and setting swd_online to 0
     58 // at which point the swd_thread may attempt to reconnect.
     59 
     60 #define MAXWORDS (8192/4)
     61 static unsigned swd_maxwords = 512;
     62 static unsigned swd_version = 0x0001;
     63 
     64 static int swd_error = 0;
     65 
     66 static int _swdp_error(void) {
     67 	return swd_error;
     68 }
     69 
     70 struct txn {
     71 	/* words to transmit */
     72 	u32 tx[MAXWORDS];
     73 	/* pointers to words for reply data */
     74 	u32 *rx[MAXWORDS];
     75 
     76 	/* words to send and receive */
     77 	unsigned txc;
     78 	unsigned rxc;
     79 
     80 	/* cached state */
     81 	u32 cache_apaddr;
     82 	u32 cache_ahbtar;
     83 
     84 	unsigned magic;
     85 };
     86 
     87 void process_swo_data(void *data, unsigned count);
     88 void transmit_swo_data(void *data, unsigned count);
     89 
     90 static void process_async(u32 *data, unsigned count) {
     91 	unsigned msg, n;
     92 	u32 tmp;
     93 	while (count-- > 0) {
     94 		msg = *data++;
     95 		switch (RSWD_MSG_CMD(msg)) {
     96 		case CMD_NULL:
     97 			continue;
     98 		case CMD_DEBUG_PRINT:
     99 			n = RSWD_MSG_ARG(msg);
    100 			if (n > count)
    101 				return;
    102 			tmp = data[n];
    103 			data[n] = 0;
    104 			xprintf(XSWD, "%s",(char*) data);
    105 			data[n] = tmp;
    106 			data += n;
    107 			count -= n;
    108 			break;
    109 		case CMD_SWO_DATA:
    110 			n = RSWD_MSG_ARG(msg);
    111 			if (swd_version < RSWD_VERSION_1_1) {
    112 				// arg is wordcount
    113 				tmp = n;
    114 				n *= 4;
    115 			} else {
    116 				// arg is bytecount
    117 				tmp = (n + 3) / 4;
    118 			}
    119 			if (tmp > count)
    120 				return;
    121 			process_swo_data(data, n);
    122 			transmit_swo_data(data, n);
    123 			data += tmp;
    124 			count -= tmp;
    125 		default:
    126 			return;
    127 		}
    128 	}
    129 }
    130 
    131 static void process_query(u32 *data, unsigned count) {
    132 	unsigned n;
    133 	const char *board = "unknown";
    134 	const char *build = "unknown";
    135 	unsigned version = 0x0005;
    136 	unsigned maxdata = 2048;
    137 
    138 	while (count-- > 0) {
    139 		unsigned msg = *data++;
    140 		switch (RSWD_MSG_CMD(msg)) {
    141 		case CMD_NULL:
    142 			break;
    143 		case CMD_BUILD_STR:
    144 			n = RSWD_MSG_ARG(msg);
    145 			if (n > count) goto done;
    146 			build = (void*) data;
    147 			data += n;
    148 			break;
    149 		case CMD_BOARD_STR:
    150 			n = RSWD_MSG_ARG(msg);
    151 			if (n > count) goto done;
    152 			board = (void*) data;
    153 			data += n;
    154 			break;
    155 		case CMD_VERSION:
    156 			version = RSWD_MSG_ARG(msg);
    157 			break;
    158 		case CMD_RX_MAXDATA:
    159 			maxdata = RSWD_MSG_ARG(msg);
    160 			break;
    161 		default:
    162 			goto done;
    163 		}
    164 	}
    165 done:
    166 	if (maxdata > (MAXWORDS * 4)) {
    167 		maxdata = MAXWORDS * 4;
    168 	}
    169 	xprintf(XSWD, "usb: board id: %s\n", board);
    170 	xprintf(XSWD, "usb: build id: %s\n", build);
    171 	xprintf(XSWD, "usb: protocol: %d.%d\n", version >> 8, version & 0xff);
    172 	xprintf(XSWD, "usb: max data: %d byte rx buffer\n", maxdata);
    173 
    174 	if (version < 0x0103) {
    175 		xprintf(XSWD, "usb: WARNING, FIRMWARE OUT OF DATE\n");
    176 	}
    177 	swd_version = version;
    178 	swd_maxwords = maxdata / 4;
    179 }
    180 
    181 const char *swd_err_str(unsigned op) {
    182 	switch (op) {
    183 	case ERR_NONE: return "NONE";
    184 	case ERR_INTERNAL: return "INTERNAL";
    185 	case ERR_TIMEOUT: return "TIMEOUT";
    186 	case ERR_IO: return "IO";
    187 	case ERR_PARITY: return "PARITY";
    188 	default: return "UNKNOWN";
    189 	}
    190 }
    191 
    192 static int process_reply(struct txn *t, u32 *data, int count) {
    193 	unsigned msg, op, n, rxp, rxc;
    194 
    195 	rxc = t->rxc;
    196 	rxp = 0;
    197 
    198 	while (count-- > 0) {
    199 		msg = *data++;
    200 		op = RSWD_MSG_OP(msg);
    201 		n = RSWD_MSG_ARG(msg);
    202 
    203 		//fprintf(stderr,"[ %02x %02x %04x ]\n",RSWD_MSG_CMD(msg), op, n);
    204 		switch (RSWD_MSG_CMD(msg)) {
    205 		case CMD_NULL:
    206 			continue;
    207 		case CMD_SWD_DATA:
    208 			if (n > rxc) {
    209 				xprintf(XSWD, "reply overrun (%d > %d)\n", n, rxc);
    210 				return -1;
    211 			}
    212 			while (n-- > 0) {
    213 				//printf("data %08x -> %p\n", data[0], t->rx[rxp]);
    214 				*(t->rx[rxp++]) = *data++;
    215 				rxc--;
    216 			}
    217 			continue;
    218 		case CMD_JTAG_DATA:
    219 			//xprintf(XSWD, "JTAG DATA %d bits\n", n);
    220 			if (((n+31)/32) > rxc) {
    221 				xprintf(XSWD, "reply overrun (%d bits > %d words)\n", n, rxc);
    222 				return -1;
    223 			}
    224 			n = (n + 31) / 32;
    225 			while (n > 0) {
    226 				//xprintf(XSWD, "JTAG %08x\n", *data);
    227 				*(t->rx[rxp++]) = *data++;
    228 				rxc--;
    229 				n--;
    230 			}
    231 			continue;
    232 		case CMD_STATUS:
    233 			if (op) {
    234 				if (swd_verbose) {
    235 					xprintf(XSWD, "SWD ERROR: %s\n", swd_err_str(op));
    236 				}
    237 				swd_error = -op;
    238 				return -op;
    239 			} else {
    240 				return 0;
    241 			}
    242 		case CMD_CLOCK_KHZ:
    243 			xprintf(XSWD,"mdebug: %s clock: %d KHz\n", op ? "SWO" : "SWD", n);
    244 			continue;
    245 		default:
    246 			xprintf(XSWD,"unknown command 0x%02x\n", RSWD_MSG_CMD(msg));
    247 			return -1;
    248 		}
    249 	}
    250 	return 0;
    251 }
    252 
    253 #define TRACE_RSWD_XMIT 0
    254 #if TRACE_RSWD_XMIT
    255 static const char* opstr(unsigned op) {
    256 	switch (op) {
    257 	case OP_RD|OP_DP|OP_X0: return "RD DP x0 (DPIDR)";
    258 	case OP_RD|OP_DP|OP_X4: return "RD DP x4 (DPCTRL)";
    259 	case OP_RD|OP_DP|OP_X8: return "RD DP x8 (RESEND)";
    260 	case OP_RD|OP_DP|OP_XC: return "RD DP xC";
    261 	case OP_WR|OP_DP|OP_X0: return "WR DP x0 (ABORT)";
    262 	case OP_WR|OP_DP|OP_X4: return "WR DP x4 (DPCTRL)";
    263 	case OP_WR|OP_DP|OP_X8: return "WR DP x8 (SELECT)";
    264 	case OP_WR|OP_DP|OP_XC: return "WR DP xC (TARGETSEL)";
    265 	case OP_RD|OP_AP|OP_X0: return "RD AP x0";
    266 	case OP_RD|OP_AP|OP_X4: return "RD AP x4";
    267 	case OP_RD|OP_AP|OP_X8: return "RD AP x8";
    268 	case OP_RD|OP_AP|OP_XC: return "RD AP xC";
    269 	case OP_WR|OP_AP|OP_X0: return "WR AP x0";
    270 	case OP_WR|OP_AP|OP_X4: return "WR AP x4";
    271 	case OP_WR|OP_AP|OP_X8: return "WR AP x8";
    272 	case OP_WR|OP_AP|OP_XC: return "WR AP xC";
    273 	default: return "???";
    274 	}
    275 }
    276 
    277 static void q_dump(u32* tx, unsigned count) {
    278 	while (count-- > 0) {
    279 		unsigned cmd = RSWD_MSG_CMD(*tx);
    280 		unsigned op = RSWD_MSG_OP(*tx);
    281 		unsigned arg = RSWD_MSG_ARG(*tx);
    282 		switch (cmd) {
    283 		case CMD_SWD_WRITE:
    284 			xprintf(XSWD, "CMD_SWD_WRITE %s 0x%08x n=%u\n", opstr(op), tx[1], arg);
    285 			while (arg > 0) { arg--; tx++; count--; }
    286 			break;
    287 		case CMD_SWD_READ:
    288 			xprintf(XSWD, "CMD_SWD_READ %s n=%u\n", opstr(op), arg);
    289 			break;
    290 		case CMD_SWD_DISCARD:
    291 			xprintf(XSWD, "CMD_SWD_DISCARD %s n=%u\n", opstr(op), arg);
    292 			break;
    293 		case CMD_ATTACH:
    294 			xprintf(XSWD, "CMD_ATTACH\n");
    295 			break;
    296 		case CMD_RESET:
    297 			xprintf(XSWD, "CMD_RESET n=%u\n", arg);
    298 			break;
    299 		default:
    300 			xprintf(XSWD, "CMD_%02x\n", cmd);
    301 		}
    302 		tx++;
    303 	}
    304 	xprintf(XSWD, "---\n");
    305 }
    306 #endif
    307 
    308 static int q_exec(struct txn *t) {
    309 	unsigned data[MAXWORDS];
    310 	unsigned seq;
    311 	int r;
    312 	u32 id;
    313 
    314 	if (t->magic != 0x12345678) {
    315 		fprintf(stderr,"FATAL: bogus txn magic\n");
    316 		exit(1);
    317 	}
    318 	t->magic = 0;
    319 
    320 	/* If we are a multiple of 64, and not exactly 4K,
    321 	 * add padding to ensure the target can detect the end of txn
    322 	 */
    323 	if (((t->txc % 16) == 0) && (t->txc != swd_maxwords))
    324 		t->tx[t->txc++] = RSWD_MSG(CMD_NULL, 0, 0);
    325 
    326 #if TRACE_RSWD_XMIT
    327 	q_dump(t->tx + 1, t->txc - 1);
    328 #endif
    329 
    330 	pthread_mutex_lock(&swd_lock);
    331  	seq = sequence++;
    332 	id = RSWD_TXN_START(seq);
    333 	t->tx[0] = id;
    334 
    335 	if (swd_online != 1) {
    336 		if (swd_online == -1) {
    337 			// ack disconnect
    338 			usb_close(usb);
    339 			usb = NULL;
    340 			swd_online = 0;
    341 			pthread_cond_broadcast(&swd_event);
    342 		}
    343 		r = -1;
    344 	} else {
    345 		r = usb_write(usb, t->tx, t->txc * sizeof(u32));
    346 		if (r == (t->txc * sizeof(u32))) {
    347 			swd_txn_id = id;
    348 			swd_txn_data = data;
    349 			swd_txn_status = TXN_STATUS_WAIT;
    350 			do {
    351 				pthread_cond_wait(&swd_event, &swd_lock);
    352 			} while (swd_txn_status == TXN_STATUS_WAIT);
    353 			if (swd_txn_status == TXN_STATUS_FAIL) {
    354 				r = -1;
    355 			} else {
    356 				r = swd_txn_status;
    357 			}
    358 			swd_txn_data = NULL;
    359 		} else {
    360 			r = -1;
    361 		}
    362 	}
    363 	pthread_mutex_unlock(&swd_lock);
    364 	if (r > 0) {
    365 		return process_reply(t, data + 1, (r / 4) - 1);
    366 	} else {
    367 		return -1;
    368 	}
    369 }
    370 
    371 static void *swd_reader(void *arg) {
    372 	uint32_t data[MAXWORDS];
    373 	unsigned query_id;
    374 	int r;
    375 	int once = 1;
    376 restart:
    377 	for (;;) {
    378 		if ((usb = usb_open(0x1209, 0x5038, 0))) break;
    379 		if ((usb = usb_open(0x18d1, 0xdb03, 0))) break;
    380 		if ((usb = usb_open(0x18d1, 0xdb04, 0))) break;
    381 		if (once) {
    382 			xprintf(XSWD, "usb: waiting for debugger device\n");
    383 			once = 0;
    384 		}
    385 		usleep(250000);
    386 	}
    387 	once = 0;
    388 	xprintf(XSWD, "usb: debugger connected\n");
    389 
    390 	pthread_mutex_lock(&swd_lock);
    391 
    392 	// send a version query to find out about the firmware
    393 	// old m3debug fw will just report failure
    394  	query_id = sequence++;
    395 	query_id = RSWD_TXN_START(query_id);
    396 	data[0] = query_id;
    397 	data[1] = RSWD_MSG(CMD_VERSION, 0, RSWD_VERSION);
    398 	usb_write(usb, data, 8);
    399 	for (;;) {
    400 		pthread_mutex_unlock(&swd_lock);
    401 		r = usb_read_forever(usb, data, MAXWORDS * 4);
    402 		pthread_mutex_lock(&swd_lock);
    403 		if (r < 0) {
    404 			xprintf(XSWD, "usb: debugger disconnected\n");
    405 			swd_online = -1;
    406 			swd_txn_status = TXN_STATUS_FAIL;
    407 			pthread_cond_broadcast(&swd_event);
    408 			break;
    409 		}
    410 		if ((r < 4) || (r & 3)) {
    411 			xprintf(XSWD, "usb: discard packet (%d)\n", r);
    412 			continue;
    413 		}
    414 		if (query_id && (data[0] == query_id)) {
    415 			query_id = 0;
    416 			process_query(data + 1, (r / 4) - 1);
    417 			swd_online = 1;
    418 		} else if (data[0] == RSWD_TXN_ASYNC) {
    419 			pthread_mutex_unlock(&swd_lock);
    420 			process_async(data + 1, (r / 4) - 1);
    421 			pthread_mutex_lock(&swd_lock);
    422 		} else if ((swd_txn_status == TXN_STATUS_WAIT) &&
    423 			(data[0] == swd_txn_id)) {
    424 			swd_txn_status = r;
    425 			memcpy(swd_txn_data, data, r);
    426 			pthread_cond_broadcast(&swd_event);
    427 		} else {
    428 			xprintf(XSWD, "usb: rx: unexpected txn %08x (%d)\n", data[0], r);
    429 		}
    430 	}
    431 	// wait for a reader to ack the shutdown (and close usb)
    432 	while (swd_online == -1) {
    433 		pthread_cond_wait(&swd_event, &swd_lock);
    434 	}
    435 	pthread_mutex_unlock(&swd_lock);
    436 	usleep(250000);
    437 	goto restart;
    438 	return NULL;
    439 }
    440 
    441 static void q_check(struct txn *t, int n) {
    442 	if ((t->txc + n) >= MAXWORDS) {
    443 		fprintf(stderr,"FATAL: txn buffer overflow\n");
    444 		exit(1);
    445 	}
    446 }
    447 
    448 static void q_init(struct txn *t) {
    449 	t->magic = 0x12345678;
    450 	t->txc = 1;
    451 	t->rxc = 0;
    452 	t->cache_apaddr = 0xffffffff;
    453 	t->cache_ahbtar = 0xffffffff;
    454 }
    455 
    456 #define SWD_WR(a,n) RSWD_MSG(CMD_SWD_WRITE, OP_WR | (a), (n))
    457 #define SWD_RD(a,n) RSWD_MSG(CMD_SWD_READ, OP_RD | (a), (n))
    458 #define SWD_RX(a,n) RSWD_MSG(CMD_SWD_DISCARD, OP_RD | (a), (n))
    459 
    460 static void q_ap_select(struct txn *t, u32 addr) {
    461 	addr &= 0xF0;
    462 	if (t->cache_apaddr != addr) {
    463 		t->tx[t->txc++] = SWD_WR(DP_SELECT, 1);
    464 		t->tx[t->txc++] = addr;
    465 		t->cache_apaddr = addr;
    466 	}
    467 }
    468 
    469 static void q_ap_write(struct txn *t, u32 addr, u32 value) {
    470 	q_check(t, 3);
    471 	q_ap_select(t, addr);
    472 	t->tx[t->txc++] = SWD_WR(OP_AP | (addr & 0xC), 1);
    473 	t->tx[t->txc++] = value;
    474 }
    475 
    476 static void q_ap_read(struct txn *t, u32 addr, u32 *value) {
    477 	q_check(t, 4);
    478 	q_ap_select(t, addr);
    479 	t->tx[t->txc++] = SWD_RX(OP_AP | (addr & 0xC), 1);
    480 	t->tx[t->txc++] = SWD_RD(DP_BUFFER, 1);
    481 	t->rx[t->rxc++] = value;
    482 }
    483 
    484 static void q_ahb_write(struct txn *t, u32 addr, u32 value) {
    485 //	xprintf(XSWD, "WR %08x -> %08x\n", value, addr);
    486 	if (t->cache_ahbtar != addr) {
    487 		q_ap_write(t, AHB_TAR, addr);
    488 		t->cache_ahbtar = addr;
    489 	}
    490 	q_ap_write(t, AHB_DRW, value);
    491 }
    492 
    493 static void q_ahb_read(struct txn *t, u32 addr, u32 *value) {
    494 	if (t->cache_ahbtar != addr) {
    495 		q_ap_write(t, AHB_TAR, addr);
    496 		t->cache_ahbtar = addr;
    497 	}
    498 	q_ap_read(t, AHB_DRW, value);
    499 }
    500 
    501 int swdp_ap_write(u32 addr, u32 value) {
    502 	struct txn t;
    503 	q_init(&t);
    504 	q_ap_write(&t, addr, value);
    505 	return q_exec(&t);
    506 }
    507 
    508 int swdp_ap_read(u32 addr, u32 *value) {
    509 	struct txn t;
    510 	q_init(&t);
    511 	q_ap_read(&t, addr, value);
    512 	return q_exec(&t);
    513 }
    514 
    515 static int _swdp_ahb_read(u32 addr, u32 *value) {
    516 	struct txn t;
    517 	q_init(&t);
    518 	q_ahb_read(&t, addr, value);
    519 	return q_exec(&t);
    520 }
    521 
    522 static int _swdp_ahb_write(u32 addr, u32 value) {
    523 	struct txn t;
    524 	q_init(&t);
    525 	q_ahb_write(&t, addr, value);
    526 	return q_exec(&t);
    527 }
    528 
    529 #if 0
    530 /* simpler but far less optimal. keeping against needing to debug */
    531 int _swdp_ahb_read32(u32 addr, u32 *out, int count) {
    532 	struct txn t;
    533 	while (count > 0) {
    534 		int xfer = (count > 128) ? 128: count;
    535 		count -= xfer;
    536 		q_init(&t);
    537 		while (xfer-- > 0) {
    538 			q_ahb_read(&t, addr, out++);
    539 			addr += 4;
    540 		}
    541 		if (q_exec(&t))
    542 			return -1;
    543 	}
    544 	return 0;
    545 }
    546 
    547 int _swdp_ahb_write32(u32 addr, u32 *in, int count) {
    548 	struct txn t;
    549 	while (count > 0) {
    550 		int xfer = (count > 128) ? 128: count;
    551 		count -= xfer;
    552 		q_init(&t);
    553 		while (xfer-- > 0) {
    554 			q_ahb_write(&t, addr, *in++);
    555 			addr += 4;
    556 		}
    557 		if (q_exec(&t))
    558 			return -1;
    559 	}
    560 	return 0;
    561 }
    562 #else
    563 
    564 // some implementations support >10 bits, but 10 is the minimum required
    565 // by spec (and some targets like rp2040 are limited to this)
    566 // TODO: detect this support 0x1000 or higher on targets that can handle it
    567 #define WRAPSIZE 0x400
    568 #define WRAPMASK (WRAPSIZE - 1)
    569 
    570 #define MAXDATAWORDS (swd_maxwords - 16)
    571 /* 10 txns overhead per 128 read txns - 126KB/s on 72MHz STM32F
    572  * 8 txns overhead per 128 write txns - 99KB/s on 72MHz STM32F
    573  */
    574 static int _swdp_ahb_read32(u32 addr, u32 *out, int count) {
    575 	struct txn t;
    576 
    577 	while (count > 0) {
    578 		int xfer;
    579 
    580 		// limit transfer so we won't cross a wrap boundary
    581 		xfer = (WRAPSIZE - (addr & WRAPMASK)) / 4;
    582 		if (xfer > count)
    583 			xfer = count;
    584 		if (xfer > MAXDATAWORDS)
    585 			xfer = MAXDATAWORDS;
    586 
    587 		count -= xfer;
    588 		q_init(&t);
    589 
    590 		/* setup before initial txn */
    591 		q_ap_write(&t, AHB_CSW,
    592 			AHB_CSW_MDEBUG | AHB_CSW_PRIV | AHB_CSW_INC_SINGLE |
    593 			AHB_CSW_DBG_EN | AHB_CSW_32BIT);
    594 
    595 		/* initial address */
    596 		q_ap_write(&t, AHB_TAR, addr);
    597 		addr += xfer * 4;
    598 
    599 		/* kick off first read, ignore result, as the
    600 		 * real result will show up during the *next* read
    601 		 */
    602 		t.tx[t.txc++] = SWD_RX(OP_AP | (AHB_DRW & 0xC), 1);
    603 		t.tx[t.txc++] = SWD_RD(OP_AP | (AHB_DRW & 0xC), xfer -1);
    604 		while (xfer-- > 1)
    605 			t.rx[t.rxc++] = out++;
    606 		t.tx[t.txc++] = SWD_RD(DP_BUFFER, 1);
    607 		t.rx[t.rxc++] = out++;
    608 
    609 		/* restore state after last batch */
    610 		if (count == 0)
    611 			q_ap_write(&t, AHB_CSW,
    612 				AHB_CSW_MDEBUG | AHB_CSW_PRIV |
    613 				AHB_CSW_DBG_EN | AHB_CSW_32BIT);
    614 
    615 		if (q_exec(&t))
    616 			return -1;
    617 	}
    618 	return 0;
    619 }
    620 
    621 static int _swdp_ahb_write32(u32 addr, u32 *in, int count) {
    622 	struct txn t;
    623 
    624 	while (count > 0) {
    625 		int xfer;
    626 
    627 		// limit transfer so we won't cross a wrap boundary
    628 		xfer = (WRAPSIZE - (addr & WRAPMASK)) / 4;
    629 		if (xfer > count)
    630 			xfer = count;
    631 		if (xfer > MAXDATAWORDS)
    632 			xfer = MAXDATAWORDS;
    633 
    634 		count -= xfer;
    635 		q_init(&t);
    636 
    637 		/* setup before initial txn */
    638 		q_ap_write(&t, AHB_CSW,
    639 			AHB_CSW_MDEBUG | AHB_CSW_PRIV | AHB_CSW_INC_SINGLE |
    640 			AHB_CSW_DBG_EN | AHB_CSW_32BIT);
    641 
    642 		/* initial address */
    643 		q_ap_write(&t, AHB_TAR, addr);
    644 
    645 		t.tx[t.txc++] = SWD_WR(OP_AP | (AHB_DRW & 0xC), xfer);
    646 		addr += xfer * 4;
    647 		while (xfer-- > 0) 
    648 			t.tx[t.txc++] = *in++;
    649 
    650 		/* restore state after last batch */
    651 		if (count == 0)
    652 			q_ap_write(&t, AHB_CSW,
    653 				AHB_CSW_MDEBUG | AHB_CSW_PRIV |
    654 				AHB_CSW_DBG_EN | AHB_CSW_32BIT);
    655 
    656 		if (q_exec(&t))
    657 			return -1;
    658 	}
    659 	return 0;
    660 }
    661 #endif
    662 
    663 #if 0
    664 int swdp_core_write(u32 n, u32 v) {
    665 	struct txn t;
    666 	q_init(&t);
    667 	q_ahb_write(&t, CDBG_REG_DATA, v);
    668 	q_ahb_write(&t, CDBG_REG_ADDR, (n & 0x1F) | 0x10000);
    669 	return q_exec(&t);
    670 }
    671 
    672 int swdp_core_read(u32 n, u32 *v) {
    673 	struct txn t;
    674 	q_init(&t);
    675 	q_ahb_write(&t, CDBG_REG_ADDR, n & 0x1F);
    676 	q_ahb_read(&t, CDBG_REG_DATA, v);
    677 	return q_exec(&t);
    678 }
    679 
    680 int swdp_core_read_all(u32 *v) {
    681 	struct txn t;
    682 	unsigned n;
    683 	q_init(&t);
    684 	for (n = 0; n < 19; n++) {
    685 		q_ahb_write(&t, CDBG_REG_ADDR, n & 0x1F);
    686 		q_ahb_read(&t, CDBG_REG_DATA, v++);
    687 	}
    688 	return q_exec(&t);
    689 }
    690 
    691 int swdp_step_no_ints = 0;
    692 
    693 int swdp_core_halt(void) {
    694 	u32 x;
    695 	if (swdp_ahb_read(CDBG_CSR, &x)) return -1;
    696 	x &= (CDBG_C_HALT | CDBG_C_DEBUGEN | CDBG_C_MASKINTS);
    697 	x |= CDBG_CSR_KEY | CDBG_C_DEBUGEN | CDBG_C_HALT;
    698 	return swdp_ahb_write(CDBG_CSR, x);
    699 }
    700 
    701 int swdp_core_step(void) {
    702 	u32 x;
    703 	if (swdp_ahb_read(CDBG_CSR, &x)) return -1;
    704 	x &= (CDBG_C_HALT | CDBG_C_DEBUGEN | CDBG_C_MASKINTS);
    705 	x |= CDBG_CSR_KEY;
    706 
    707 	if (!(x & CDBG_C_HALT)) {
    708 		// HALT if we're not already HALTED
    709 		x |= CDBG_C_HALT | CDBG_C_DEBUGEN;
    710 		swdp_ahb_write(CDBG_CSR, x);
    711 	}
    712 	if (swdp_step_no_ints) {
    713 		// set MASKINTS if not already set
    714 		if (!(x & CDBG_C_MASKINTS)) {
    715 			x |= CDBG_C_MASKINTS;
    716 			swdp_ahb_write(CDBG_CSR, x);
    717 		}
    718 	} else {
    719 		// clear MASKINTs if not already clear
    720 		if (x & CDBG_C_MASKINTS) {
    721 			x &= (~CDBG_C_MASKINTS);
    722 			swdp_ahb_write(CDBG_CSR, x);
    723 		}
    724 	}
    725 	// STEP
    726 	x &= (~CDBG_C_HALT);
    727 	return swdp_ahb_write(CDBG_CSR, x | CDBG_C_STEP);
    728 }
    729 
    730 int swdp_core_resume(void) {
    731 	u32 x;
    732 	if (swdp_ahb_read(CDBG_CSR, &x)) return -1;
    733 	x &= (CDBG_C_HALT | CDBG_C_DEBUGEN | CDBG_C_MASKINTS);
    734 	x |= CDBG_CSR_KEY | CDBG_C_DEBUGEN;
    735 
    736 	if (swdp_step_no_ints > 1) {
    737 		// not just on during step, but always
    738 		if (!(x & CDBG_C_MASKINTS)) {
    739 			x |= CDBG_C_MASKINTS;
    740 			swdp_ahb_write(CDBG_CSR, x);
    741 		}
    742 	} else {
    743 		if (x & CDBG_C_MASKINTS) {
    744 			x &= (~CDBG_C_MASKINTS);
    745 			swdp_ahb_write(CDBG_CSR, x);
    746 		}
    747 	}
    748 
    749 	x &= ~(CDBG_C_HALT | CDBG_C_STEP);
    750 	return swdp_ahb_write(CDBG_CSR, x);
    751 }
    752 
    753 int swdp_core_wait_for_halt(void) {
    754 	int last = ATTN;
    755 	u32 csr;
    756 	for (;;) {
    757 		if (swdp_ahb_read(CDBG_CSR, &csr))
    758 			return -1;
    759 		if (csr & CDBG_S_HALT)
    760 			return 0;
    761 		if (ATTN != last)
    762 			return -2;
    763 	}
    764 }
    765 
    766 int swdp_ahb_wait_for_change(u32 addr, u32 oldval) {
    767 	int last = ATTN;
    768 	u32 val;
    769 	do {
    770 		if (swdp_ahb_read(addr, &val))
    771 			return -1;
    772 		if (ATTN != last)
    773 			return -2;
    774 	} while (val == oldval);
    775 	return 0;
    776 }
    777 
    778 int swdp_watchpoint(unsigned n, u32 addr, u32 func) {
    779 	struct txn t;
    780 
    781 	if (n > 3)
    782 		return -1;
    783 
    784 	q_init(&t);
    785 	/* enable DWT, enable all exception traps */
    786 	q_ahb_write(&t, DEMCR, DEMCR_TRCENA | DEMCR_VC_CORERESET);
    787 	q_ahb_write(&t, DWT_FUNC(n), DWT_FN_DISABLED);
    788 	if (func != DWT_FN_DISABLED) {
    789 		q_ahb_write(&t, DWT_COMP(n), addr);
    790 		q_ahb_write(&t, DWT_MASK(n), 0);
    791 		q_ahb_write(&t, DWT_FUNC(n), func);
    792 	}
    793 	return q_exec(&t);
    794 }
    795 
    796 int swdp_watchpoint_pc(unsigned n, u32 addr) {
    797 	return swdp_watchpoint(n, addr, DWT_FN_WATCH_PC);
    798 }
    799 
    800 int swdp_watchpoint_rd(unsigned n, u32 addr) {
    801 	return swdp_watchpoint(n, addr, DWT_FN_WATCH_RD);
    802 }
    803 
    804 int swdp_watchpoint_wr(unsigned n, u32 addr) {
    805 	return swdp_watchpoint(n, addr, DWT_FN_WATCH_WR);
    806 }
    807 
    808 int swdp_watchpoint_rw(unsigned n, u32 addr) {
    809 	return swdp_watchpoint(n, addr, DWT_FN_WATCH_RW);
    810 }
    811 
    812 int swdp_watchpoint_disable(unsigned n) {
    813 	return swdp_watchpoint(n, 0, DWT_FN_DISABLED);
    814 }
    815 #endif
    816 
    817 int swdp_bootloader(void) {
    818 	struct txn t;
    819 	q_init(&t);
    820 	t.tx[t.txc++] = RSWD_MSG(CMD_BOOTLOADER, 0, 0);
    821 	return q_exec(&t);
    822 }
    823 
    824 static uint32_t targetsel_val = 0;
    825 static unsigned targetsel_on = 0;
    826 
    827 void swdp_targetsel(uint32_t val, unsigned on) {
    828 	targetsel_val = val;
    829 	targetsel_on = on;
    830 }
    831 
    832 static int _swdp_reset(void) {
    833 	struct txn t;
    834 	u32 n, idcode;
    835 
    836 	swd_error = 0;
    837 	q_init(&t);
    838 	t.tx[t.txc++] = RSWD_MSG(CMD_ATTACH, ATTACH_JTAG_TO_SWD, 0);
    839 	t.tx[t.txc++] = RSWD_MSG(CMD_ATTACH, ATTACH_DORMANT_TO_SWD, 0);
    840 	t.tx[t.txc++] = RSWD_MSG(CMD_ATTACH, ATTACH_SWD_RESET, 0);
    841 
    842 	if (targetsel_on) {
    843 		t.tx[t.txc++] = SWD_WR(DP_BUFFER, 1);
    844 		t.tx[t.txc++] = targetsel_val;
    845 	}
    846 	
    847 	t.tx[t.txc++] = SWD_RD(DP_IDCODE, 1);
    848 	t.rx[t.rxc++] = &idcode;
    849 	if (q_exec(&t)) {
    850 		if (swd_verbose) {
    851 			xprintf(XSWD, "attach: IDCODE: ????????\n");
    852 		}
    853 	} else {
    854 		if (swd_verbose) {
    855 			xprintf(XSWD, "attach: IDCODE: %08x\n", idcode);
    856 		}
    857 	}
    858 
    859 	swd_error = 0;
    860 	q_init(&t);
    861 
    862  	/* clear any stale errors */
    863 	t.tx[t.txc++] = SWD_WR(DP_ABORT, 1);
    864 	t.tx[t.txc++] = 0x1E;
    865 
    866 	if (targetsel_on && (targetsel_val == 0xf1002927)) {
    867 		// for pico recovery dap, only valid action is clear
    868 		// debug power bits to put the chip in to recovery mode
    869 		t.tx[t.txc++] = SWD_WR(DP_DPCTRL, 1);
    870 		t.tx[t.txc++] = 0;
    871 		t.tx[t.txc++] = SWD_RD(DP_DPCTRL, 1);
    872 		t.rx[t.rxc++] = &n;
    873 	} else{
    874 		/* power up */
    875 		t.tx[t.txc++] = SWD_WR(DP_DPCTRL, 1);
    876 		t.tx[t.txc++] = (1 << 28) | (1 << 30);
    877 		t.tx[t.txc++] = SWD_RD(DP_DPCTRL, 1);
    878 		t.rx[t.rxc++] = &n;
    879 
    880 		/* configure for 32bit IO */
    881 		q_ap_write(&t, AHB_CSW,
    882 			AHB_CSW_MDEBUG | AHB_CSW_PRIV |
    883 			AHB_CSW_PRIV | AHB_CSW_DBG_EN | AHB_CSW_32BIT);
    884 	}
    885 
    886 	//u32 base;
    887 	//q_ap_read(&t, 0xF8, &base);
    888 	if (q_exec(&t))
    889 		return -1;
    890 
    891 	if (swd_verbose) {
    892 		xprintf(XSWD, "attach: DPCTRL: %08x\n", n);
    893 	}
    894 	//xprintf(XSWD, "attach: BASE: %08x\n", base);
    895 	return 0;
    896 }
    897 
    898 static int _swdp_clear_error(void) {
    899 	if (swd_error == 0) {
    900 		return 0;
    901 	} else {
    902 		struct txn t;
    903 		swd_error = 0;
    904 
    905 		q_init(&t);
    906 		t.tx[t.txc++] = SWD_WR(DP_ABORT, 1);
    907 		t.tx[t.txc++] = 0x1E;
    908 		q_exec(&t);
    909 
    910 		return swd_error;
    911 	}
    912 }
    913 
    914 void swdp_enable_tracing(int yes) {
    915 	struct txn t;
    916 	q_init(&t);
    917 	t.tx[t.txc++] = RSWD_MSG(CMD_TRACE, yes, 0);
    918 	q_exec(&t);
    919 }
    920 
    921 void swdp_target_reset(int enable) {
    922 	struct txn t;
    923 	q_init(&t);
    924 	t.tx[t.txc++] = RSWD_MSG(CMD_RESET, 0, enable);
    925 	q_exec(&t);
    926 }
    927 
    928 int swdp_set_clock(unsigned khz) {
    929 	struct txn t;
    930 	if (khz > 0xFFFF)
    931 		return -1;
    932 	if (khz < 1000)
    933 		khz = 1000;
    934 	q_init(&t);
    935 	t.tx[t.txc++] = RSWD_MSG(CMD_SET_CLOCK, 0, khz);
    936 	return q_exec(&t);
    937 }
    938 
    939 int swo_set_clock(unsigned khz) {
    940 	struct txn t;
    941 	if (khz > 0xFFFF)
    942 		return -1;
    943 	if (khz < 1000)
    944 		khz = 1000;
    945 	q_init(&t);
    946 	t.tx[t.txc++] = RSWD_MSG(CMD_SWO_CLOCK, 0, khz);
    947 	return q_exec(&t);
    948 }
    949 
    950 int swdp_open(void) {
    951 	pthread_create(&swd_thread, NULL, swd_reader, NULL);
    952 	return 0;
    953 }
    954 
    955 int jtag_io(unsigned count, u32 *tms, u32 *tdi, u32 *tdo) {
    956 	struct txn t;
    957 	q_init(&t);
    958 	if (count > 32768)
    959 		return -1;
    960 	t.tx[t.txc++] = RSWD_MSG(CMD_JTAG_IO, 0, count);
    961 	count = (count + 31) / 32;
    962 	while (count > 0) {
    963 		t.tx[t.txc++] = *tms++;
    964 		t.tx[t.txc++] = *tdi++;
    965 		t.rx[t.rxc++] = tdo++;
    966 		count--;
    967 	}
    968 	return q_exec(&t);
    969 }
    970 
    971 debug_transport SWDP_TRANSPORT = {
    972 	.attach = _swdp_reset,
    973 	.error = _swdp_error,
    974 	.clear_error = _swdp_clear_error,
    975 	.mem_rd_32 = _swdp_ahb_read,
    976 	.mem_wr_32 = _swdp_ahb_write,
    977 	.mem_rd_32_c = _swdp_ahb_read32,
    978 	.mem_wr_32_c = _swdp_ahb_write32,
    979 };
    980