rswdp.c (8047B)
1 // Copyright 2011-2021, Brian Swetland <swetland@frotz.net> 2 // Licensed under the Apache License, Version 2.0. 3 4 #include <string.h> 5 #include <stdlib.h> 6 #include <stdio.h> 7 8 #include "swd.h" 9 #include "rswdp.h" 10 11 void usb_xmit(void *data, unsigned len); 12 int usb_recv(void *data, unsigned len); 13 14 unsigned swdp_trace = 0; 15 16 // indicates host knows about v1.0 protocol features 17 unsigned host_version = 0; 18 19 static uint8_t optable[16] = { 20 [OP_RD | OP_DP | OP_X0] = RD_IDCODE, 21 [OP_RD | OP_DP | OP_X4] = RD_DPCTRL, 22 [OP_RD | OP_DP | OP_X8] = RD_RESEND, 23 [OP_RD | OP_DP | OP_XC] = RD_BUFFER, 24 [OP_WR | OP_DP | OP_X0] = WR_ABORT, 25 [OP_WR | OP_DP | OP_X4] = WR_DPCTRL, 26 [OP_WR | OP_DP | OP_X8] = WR_SELECT, 27 [OP_WR | OP_DP | OP_XC] = WR_BUFFER, 28 [OP_RD | OP_AP | OP_X0] = RD_AP0, 29 [OP_RD | OP_AP | OP_X4] = RD_AP1, 30 [OP_RD | OP_AP | OP_X8] = RD_AP2, 31 [OP_RD | OP_AP | OP_XC] = RD_AP3, 32 [OP_WR | OP_AP | OP_X0] = WR_AP0, 33 [OP_WR | OP_AP | OP_X4] = WR_AP1, 34 [OP_WR | OP_AP | OP_X8] = WR_AP2, 35 [OP_WR | OP_AP | OP_XC] = WR_AP3, 36 }; 37 38 static const char *board_str = "rpi pico"; 39 static const char *build_str = "fw v1.00 (" __DATE__ ", " __TIME__ ")"; 40 41 #define MODE_SWD 0 42 #define MODE_JTAG 1 43 static unsigned mode = MODE_SWD; 44 45 /* TODO bounds checking -- we trust the host far too much */ 46 unsigned process_txn(uint32_t txnid, uint32_t *rx, int rxc, uint32_t *tx) { 47 unsigned msg, op, n; 48 unsigned txc = 1; 49 unsigned count = 0; 50 unsigned status = 0; 51 void (*func)(void) = 0; 52 53 tx[0] = txnid; 54 55 while (rxc-- > 0) { 56 count++; 57 msg = *rx++; 58 op = RSWD_MSG_OP(msg); 59 n = RSWD_MSG_ARG(msg); 60 #if CONFIG_MDEBUG_TRACE 61 printf("> %02x %02x %04x <\n", RSWD_MSG_CMD(msg), op, n); 62 #endif 63 switch (RSWD_MSG_CMD(msg)) { 64 case CMD_NULL: 65 continue; 66 case CMD_SWD_WRITE: 67 while (n-- > 0) { 68 rxc--; 69 status = swd_write(optable[op], *rx++); 70 if (status) { 71 goto done; 72 } 73 } 74 continue; 75 case CMD_SWD_READ: 76 tx[txc++] = RSWD_MSG(CMD_SWD_DATA, 0, n); 77 while (n-- > 0) { 78 status = swd_read(optable[op], tx + txc); 79 if (status) { 80 txc++; 81 while (n-- > 0) 82 tx[txc++] = 0xfefefefe; 83 goto done; 84 } 85 txc++; 86 } 87 continue; 88 case CMD_SWD_DISCARD: 89 while (n-- > 0) { 90 uint32_t tmp; 91 status = swd_read(optable[op], &tmp); 92 if (status) { 93 goto done; 94 } 95 } 96 continue; 97 case CMD_ATTACH: 98 if (mode != MODE_SWD) { 99 mode = MODE_SWD; 100 swd_init(); 101 } 102 swd_reset(op); 103 continue; 104 #if 0 105 case CMD_JTAG_IO: 106 if (mode != MODE_JTAG) { 107 mode = MODE_JTAG; 108 jtag_init(); 109 } 110 tx[txc++] = RSWD_MSG(CMD_JTAG_DATA, 0, n); 111 while (n > 0) { 112 unsigned xfer = (n > 32) ? 32 : n; 113 jtag_io(xfer, rx[0], rx[1], tx + txc); 114 rx += 2; 115 rxc -= 2; 116 txc += 1; 117 n -= xfer; 118 } 119 continue; 120 case CMD_JTAG_VRFY: 121 if (mode != MODE_JTAG) { 122 mode = MODE_JTAG; 123 jtag_init(); 124 } 125 // (n/32) x 4 words: TMS, TDI, DATA, MASK 126 while (n > 0) { 127 unsigned xfer = (n > 32) ? 32 : n; 128 jtag_io(xfer, rx[0], rx[1], tx + txc); 129 if ((tx[txc] & rx[3]) != rx[2]) { 130 status = ERR_BAD_MATCH; 131 goto done; 132 } 133 rx += 4; 134 rxc -= 4; 135 n -= xfer; 136 } 137 continue; 138 case CMD_JTAG_TX: { 139 unsigned tms = (op & 1) ? 0xFFFFFFFF : 0; 140 if (mode != MODE_JTAG) { 141 mode = MODE_JTAG; 142 jtag_init(); 143 } 144 while (n > 0) { 145 unsigned xfer = (n > 32) ? 32 : n; 146 jtag_io(xfer, tms, rx[0], rx); 147 rx++; 148 rxc--; 149 n -= xfer; 150 } 151 continue; 152 } 153 case CMD_JTAG_RX: { 154 unsigned tms = (op & 1) ? 0xFFFFFFFF : 0; 155 unsigned tdi = (op & 2) ? 0xFFFFFFFF : 0; 156 if (mode != MODE_JTAG) { 157 mode = MODE_JTAG; 158 jtag_init(); 159 } 160 tx[txc++] = RSWD_MSG(CMD_JTAG_DATA, 0, n); 161 while (n > 0) { 162 unsigned xfer = (n > 32) ? 32 : n; 163 jtag_io(xfer, tms, tdi, tx + txc); 164 txc++; 165 n -= xfer; 166 } 167 continue; 168 } 169 #endif 170 case CMD_RESET: 171 swd_hw_reset(n); 172 continue; 173 case CMD_TRACE: 174 swdp_trace = op; 175 continue; 176 #if 0 177 case CMD_BOOTLOADER: 178 func = _reboot; 179 continue; 180 #endif 181 case CMD_SET_CLOCK: 182 n = swd_set_clock(n); 183 if (host_version >= RSWD_VERSION_1_0) { 184 tx[txc++] = RSWD_MSG(CMD_CLOCK_KHZ, 0, n); 185 } 186 continue; 187 case CMD_SWO_CLOCK: 188 n = swo_set_clock(n); 189 if (host_version >= RSWD_VERSION_1_0) { 190 tx[txc++] = RSWD_MSG(CMD_CLOCK_KHZ, 1, n); 191 } 192 continue; 193 case CMD_VERSION: 194 host_version = n; 195 tx[txc++] = RSWD_MSG(CMD_VERSION, 0, RSWD_VERSION); 196 197 n = strlen(board_str); 198 memcpy(tx + txc + 1, board_str, n + 1); 199 n = (n + 4) / 4; 200 tx[txc++] = RSWD_MSG(CMD_BOARD_STR, 0, n); 201 txc += n; 202 203 n = strlen(build_str); 204 memcpy(tx + txc + 1, build_str, n + 1); 205 n = (n + 4) / 4; 206 tx[txc++] = RSWD_MSG(CMD_BUILD_STR, 0, n); 207 txc += n; 208 209 tx[txc++] = RSWD_MSG(CMD_RX_MAXDATA, 0, 8192); 210 txc += n; 211 continue; 212 default: 213 printf("unknown command %02x\n", RSWD_MSG_CMD(msg)); 214 status = 1; 215 goto done; 216 } 217 } 218 219 done: 220 tx[txc++] = RSWD_MSG(CMD_STATUS, status, count); 221 222 /* if we're about to send an even multiple of the packet size 223 * (64), add a NULL op on the end to create a short packet at 224 * the end. 225 */ 226 if ((txc & 0xf) == 0) 227 tx[txc++] = RSWD_MSG(CMD_NULL, 0, 0); 228 229 #if CONFIG_MDEBUG_TRACE 230 printf("[ send %d words ]\n", txc); 231 for (n = 0; n < txc; n+=4) { 232 printx("%08x %08x %08x %08x\n", 233 tx[n], tx[n+1], tx[n+2], tx[n+3]); 234 } 235 #endif 236 237 return txc; 238 } 239 240 void rswd_init(void) { 241 printf("[ rswdp agent v0.9 ]\n"); 242 printf("[ built " __DATE__ " " __TIME__ " ]\n"); 243 244 swd_init(); 245 } 246 247 uint32_t rswd_txn(uint32_t* rx, uint32_t rxc, uint32_t* tx) { 248 #if CONFIG_MDEBUG_TRACE 249 printf("[ recv %d words ]\n", rxc); 250 for (unsigned n = 0; n < rxc; n += 4) { 251 printf("%08x %08x %08x %08x\n", 252 rx[n], rx[n+1], rx[n+2], rx[n+3]); 253 } 254 #endif 255 256 if ((rx[0] & 0xFFFF0000) != 0xAA770000) { 257 printf("invalid frame %x\n", rx[0]); 258 return 0; 259 } 260 261 return process_txn(rx[0], rx + 1, rxc - 1, tx); 262 } 263