pico-mdebug

mdebug firmware for pico / rp2040
git clone http://frotz.net/git/pico-mdebug.git
Log | Files | Refs | README

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