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