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