CMSIS-DAP protocol fixes and other stuff

CMSIS-DAP/Firmware/Source/DAP.c

@@ -1642,10 +1642,12 @@ uint32_t DAP_ProcessCommand(const uint8_t *request, uint8_t *response) {
 
   *response++ = *request;
 
+  //printf("dap cmd=%02hhx\n", *request);
   switch (*request++) {
     case ID_DAP_Info:
       num = DAP_Info(*request, response+1);
       *response = (uint8_t)num;
+      //printf("cmd info\n");
       return ((2U << 16) + 2U + num);
 
     case ID_DAP_HostStatus:
@@ -1752,9 +1754,11 @@ uint32_t DAP_ProcessCommand(const uint8_t *request, uint8_t *response) {
 
     default:
       *(response-1) = ID_DAP_Invalid;
+      //printf("cmd invalid\n");
       return ((1U << 16) | 1U);
   }
 
+  //printf("cmd end\n");
   return ((1U << 16) + 1U + num);
 }
 

bsp/rp2040/bsp-info.h

@@ -4,10 +4,14 @@
 #define BSP_INFO_H_
 
 /*#define USB_VID 0x2e8a*/ /* Raspberry Pi */
-#define USB_VID 0xcafe     /* TinyUSB */
+/*#define USB_VID 0xcafe*/     /* TinyUSB */
 /*#define USB_VID 0x1209*/ /* Generic */
 /*#define USB_VID 0x1d50*/ /* OpenMoko */
-#define USB_PID 0x1312
+/*#define USB_PID 0x1312*/
+
+/* openFPGAloader silliness */
+#define USB_VID 0x0d28 /* NXP */
+#define USB_PID 0x0204 /* ARM mbed */
 
 // TODO: other RP2040 boards
 #define INFO_BOARDNAME "RP2040 Pico"

bsp/rp2040/m_default/dap_jtag.c

@@ -17,20 +17,24 @@
 int jtagsm = -1, jtagoffset = -1;
 
 void PORT_OFF(void) {
-    if (jtagsm) {
+    //printf("disable\n");
+    if (jtagsm >= 0) {
         pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, jtagsm, false);
         pio_sm_unclaim(PINOUT_JTAG_PIO_DEV, jtagsm);
     }
-    if (jtagoffset)
+    if (jtagoffset >= 0) {
         pio_remove_program(PINOUT_JTAG_PIO_DEV, &dap_jtag_program, jtagoffset);
+    }
     jtagoffset = jtagsm = -1;
 
-    if (swdsm) {
+    if (swdsm >= 0) {
         pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, swdsm, false);
         pio_sm_unclaim(PINOUT_JTAG_PIO_DEV, swdsm);
     }
-    if (swdoffset)
+    if (swdoffset >= 0) {
         pio_remove_program(PINOUT_JTAG_PIO_DEV, &dap_swd_program, swdoffset);
+    }
+    swdoffset = swdsm = -1;
 
     sio_hw->gpio_oe_clr = PINOUT_SWCLK_MASK | PINOUT_SWDIO_MASK |
                           PINOUT_TDI_MASK  //| PINOUT_TDO_MASK
@@ -85,16 +89,16 @@ void PORT_JTAG_SETUP(void) {
                         | PINOUT_nTRST_MASK | PINOUT_nRESET_MASK;
 }*/
 
-void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
+/*void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
     uint32_t n = info & JTAG_SEQUENCE_TCK;
     if (n == 0) n = 64;
 
-    printf("seq hi 0x%lx\n", info);
+//    printf("seq hi 0x%lx\n", info);
-
+//
-    printf("%s", "tdi: ");
+//    printf("%s", "tdi: ");
-    for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
+//    for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
-        printf("0x%x ", ((const uint8_t*)tdi)[j]);
+//        printf("0x%x ", ((const uint8_t*)tdi)[j]);
-    }
+//    }
 
     if (info & JTAG_SEQUENCE_TMS) PIN_SWDIO_TMS_SET();
     else PIN_SWDIO_TMS_CLR();
@@ -117,17 +121,32 @@ void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
     n = info & JTAG_SEQUENCE_TCK;
     if (n == 0) n = 64;
 
-    if (info & JTAG_SEQUENCE_TDO) {
+//    if (info & JTAG_SEQUENCE_TDO) {
-        printf("%s", "\ntdo: ");
+//        printf("%s", "\ntdo: ");
-        for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
+//        for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
-            printf("0x%x ", ((const uint8_t*)tdo)[j]);
+//            printf("0x%x ", ((const uint8_t*)tdo)[j]);
-        }
+//        }
-        printf("%c", '\n');
+//        printf("%c", '\n');
-    } else printf("%s", "\nno tdo\n");
+//    } else printf("%s", "\nno tdo\n");
+}*/
+
+void jtag_tms_seq(uint32_t count, const uint8_t* data) {
+    for (size_t i = 0; i < count; ++i) {
+        uint8_t byte = data[i >> 3],
+                bit  = (byte >> (i & 7)) & 1;
+
+        if (bit) PIN_SWDIO_TMS_SET();
+        else PIN_SWDIO_TMS_CLR();
+        PIN_SWCLK_TCK_CLR();
+        PIN_DELAY_SLOW(DAP_Data.clock_delay);
+        PIN_SWCLK_TCK_SET();
+        PIN_DELAY_SLOW(DAP_Data.clock_delay);
+    }
 }
 #else
 
 void PORT_JTAG_SETUP(void) {
+    //printf("jtag setup\n");
     resets_hw->reset &= ~(RESETS_RESET_IO_BANK0_BITS | RESETS_RESET_PADS_BANK0_BITS);
 
     /* set to default high level */
@@ -167,14 +186,19 @@ void PORT_JTAG_SETUP(void) {
     iobank0_hw->io[PINOUT_JTAG_nTRST].ctrl  = GPIO_FUNC_SIO << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
     iobank0_hw->io[PINOUT_JTAG_nRESET].ctrl = GPIO_FUNC_SIO << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
 
-    if (jtagsm == -1) jtagsm = pio_claim_unused_sm(PINOUT_JTAG_PIO_DEV, true);
+    if (jtagsm == -1) jtagsm = pio_claim_unused_sm(PINOUT_JTAG_PIO_DEV, false);
     if (jtagoffset == -1)
         jtagoffset = pio_add_program(PINOUT_JTAG_PIO_DEV, &dap_jtag_program);
     dap_jtag_program_init(PINOUT_JTAG_PIO_DEV, jtagsm, jtagoffset,
              50*1000, PINOUT_JTAG_TCK, PINOUT_JTAG_TDI, PINOUT_JTAG_TDO);
 }
 
+#define JTAG_SEQUENCE_NO_TMS 0x80000u /* should be large enough */
+
 void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
+    //printf("jtag seq\n");
+    //pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, jtagsm, true);
+
     float div = (float)clock_get_hz(clk_sys) / (4 * DAP_Data.clock_freq);
     if (div < 2) div = 2;
     else if (div > 65536) div = 65536;
@@ -185,8 +209,10 @@ void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
     uint32_t n = info & JTAG_SEQUENCE_TCK;
     if (n == 0) n = 64;
 
-    if (info & JTAG_SEQUENCE_TMS) PIN_SWDIO_TMS_SET();
+    //if (!(n & JTAG_SEQUENCE_NO_TMS)) {
-    else PIN_SWDIO_TMS_CLR();
+        if (info & JTAG_SEQUENCE_TMS) PIN_SWDIO_TMS_SET();
+        else PIN_SWDIO_TMS_CLR();
+    //}
 
     io_wo_8* tx = (io_wo_8*)&PINOUT_JTAG_PIO_DEV->txf[jtagsm];
     io_ro_8* rx = (io_ro_8*)&PINOUT_JTAG_PIO_DEV->rxf[jtagsm];
@@ -196,9 +222,9 @@ void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
     //printf("n=%lu bytelen=%lu last_shift=%lu\n", n, bytelen, last_shift);
     uint32_t txremain = bytelen,
              rxremain = last_shift ? bytelen : (bytelen + 1);
-    /*printf("txremain=%lu rxremain=%lu\n", txremain, rxremain);
+    //printf("txremain=%lu rxremain=%lu\n", txremain, rxremain);
 
-    printf("%s", "tdi: ");
+    /*printf("%s", "tdi: ");
     for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
         printf("0x%x ", ((const uint8_t*)tdi)[j]);
     }
@@ -238,6 +264,54 @@ void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
         }
         printf("%c", '\n');
     } else printf("%s", "no tdo\n");*/
+
+    //pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, jtagsm, false);
+}
+
+void jtag_tms_seq(uint32_t count, const uint8_t* data) {
+    //printf("jtag tms seq\n");
+    // work around openFPGAloader bug (how did this even get here?)
+    if (DAP_Data.clock_delay == 0) {
+        DAP_Data.clock_delay = 8;
+    }
+
+    /*pio_sm_set_out_pins(PINOUT_JTAG_PIO_DEV, jtagsm, PINOUT_JTAG_TMS, 1);
+    pio_sm_set_set_pins(PINOUT_JTAG_PIO_DEV, jtagsm, PINOUT_JTAG_TMS, 1);
+    pio_sm_set_pins(PINOUT_JTAG_PIO_DEV, jtagsm, gpio_get(PINOUT_JTAG_TMS)?1:0);
+    gpio_set_function(PINOUT_JTAG_TMS, GPIO_FUNC_PIO0 + ((PINOUT_JTAG_PIO_DEV == pio0) ? 0 : 1));
+
+    for (uint32_t i = 0, n; i < count; i += n) {
+        n = count - i;
+        if (n == 0) break;
+        if (n > 64) n = 64;
+        n &= JTAG_SEQUENCE_TCK;
+        JTAG_Sequence(n | JTAG_SEQUENCE_NO_TMS, data, NULL);
+    }
+
+    gpio_put(PINOUT_JTAG_TMS, data[(count >> 3)] & (1 << (count & 7)));
+    gpio_set_function(PINOUT_JTAG_TMS, GPIO_FUNC_SIO);
+    pio_sm_set_out_pins(PINOUT_JTAG_PIO_DEV, jtagsm, PINOUT_JTAG_TDI, 1);
+    pio_sm_set_set_pins(PINOUT_JTAG_PIO_DEV, jtagsm, PINOUT_JTAG_TDI, 1);*/
+
+    // FIXME: above doesn't seem to work somehow -- so fall back to bit-banging
+
+    const uint8_t tdibit = 0xff;
+    PIN_SWCLK_TCK_SET();
+    gpio_set_function(PINOUT_JTAG_TMS, GPIO_FUNC_SIO);
+    gpio_set_function(PINOUT_JTAG_TCK, GPIO_FUNC_SIO);
+    for (size_t i = 0; i < count; ++i) {
+        uint8_t byte = data[i >> 3],
+                bit  = byte & (1 << (i & 7));//(byte >> (i & 7)) & 1;
+
+        //JTAG_Sequence(1 | (bit ? JTAG_SEQUENCE_TMS : 0), &tdibit, NULL);
+        if (bit) PIN_SWDIO_TMS_SET();
+        else PIN_SWDIO_TMS_CLR();
+        PIN_SWCLK_TCK_CLR();
+        PIN_DELAY_SLOW(DAP_Data.clock_delay);
+        PIN_SWCLK_TCK_SET();
+        PIN_DELAY_SLOW(DAP_Data.clock_delay);
+    }
+    gpio_set_function(PINOUT_JTAG_TCK, GPIO_FUNC_PIO0);
 }
 #endif
 
@@ -270,6 +344,7 @@ static void jtag_seq(uint32_t num, int tms, const void* tdi, void* tdo) {
 }
 
 uint32_t JTAG_ReadIDCode(void) {
+    //printf("jtag readID\n");
     // tdi=NULL: ~~0xff!~~ repeat last-seen bit, ignore otherwise
     // tdo=NULL: ignore
     jtag_seq(1, JTAG_SEQUENCE_TMS, NULL, NULL);
@@ -310,6 +385,7 @@ uint32_t JTAG_ReadIDCode(void) {
 }
 
 void JTAG_IR(uint32_t ir) {
+    //printf("jtag IR\n");
     jtag_seq(2,JTAG_SEQUENCE_TMS, NULL, NULL);
     jtag_seq(2,0, NULL, NULL);
     uint64_t v = ~(uint64_t)0;
@@ -411,7 +487,7 @@ static uint8_t xfer_base(uint32_t request, uint32_t* data, bool check_ack) {
 exit:
     jtag_seq(1,JTAG_SEQUENCE_TMS, NULL, NULL);
     jtag_seq(1,0, NULL, NULL);
-    PIN_TDI_OUT(1); // TODO: TDI HI (no clk)
+    PIN_TDI_OUT(1); // TDI HI (no clk)
     if (request & DAP_TRANSFER_TIMESTAMP) DAP_Data.timestamp = TIMESTAMP_GET();
     if (check_ack) jtag_seq(DAP_Data.transfer.idle_cycles, 0, NULL, NULL);
     return (uint8_t)ack;
@@ -525,10 +601,12 @@ exit:
 }
 
 void JTAG_WriteAbort(uint32_t data) {
+    //printf("jtag wrabort\n");
     xfer_base(0 /* write,A2=0,A3=0 */, &data, false);
 }
 
 uint8_t JTAG_Transfer(uint32_t request, uint32_t* data) {
+    //printf("jtag xfer\n");
     return xfer_base(request, data, true);
 }
 //#endif

bsp/rp2040/m_default/dap_jtag.pio

@@ -33,6 +33,7 @@ static inline void dap_jtag_program_init(PIO pio, uint sm, uint offset,
         uint16_t freq, uint pin_tck, uint pin_tdi, uint pin_tdo) {
     pio_sm_config c = dap_jtag_program_get_default_config(offset);
     sm_config_set_out_pins(&c, pin_tdi, 1);
+    //sm_config_set_set_pins(&c, pin_tdi, 1);
     sm_config_set_in_pins(&c, pin_tdo);
     sm_config_set_sideset_pins(&c, pin_tck);
     // (shift to left, autopush/pull, threshold=nbits)

bsp/rp2040/m_default/dap_swd.c

@@ -101,6 +101,7 @@ void SWD_Sequence (uint32_t info, const uint8_t *swdo, uint8_t *swdi) {
 #else
 
 void PORT_SWD_SETUP(void) {
+    //printf("swd setup\n");
     resets_hw->reset &= ~(RESETS_RESET_IO_BANK0_BITS | RESETS_RESET_PADS_BANK0_BITS);
 
     /* set to default high level */
@@ -114,7 +115,7 @@ void PORT_SWD_SETUP(void) {
     iobank0_hw->io[PINOUT_SWCLK].ctrl = GPIO_FUNC_SIO << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
     iobank0_hw->io[PINOUT_SWDIO].ctrl = GPIO_FUNC_SIO << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
 
-    if (swdsm == -1) swdsm = pio_claim_unused_sm(PINOUT_JTAG_PIO_DEV, true);
+    if (swdsm == -1) swdsm = pio_claim_unused_sm(PINOUT_JTAG_PIO_DEV, false);
     if (swdoffset == -1)
         swdoffset = pio_add_program(PINOUT_JTAG_PIO_DEV, &dap_swd_program);
     dap_swd_program_init(PINOUT_JTAG_PIO_DEV, swdsm, swdoffset,
@@ -138,6 +139,7 @@ inline static void PIN_SWDIO_SET_PIO(void) {
 }
 
 void SWD_Sequence(uint32_t info, const uint8_t* swdo, uint8_t* swdi) {
+    //printf("swd sequence\n");
     pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, swdsm, true);
     //busy_wait_us_32(0);
 
@@ -209,7 +211,8 @@ void SWD_Sequence(uint32_t info, const uint8_t* swdo, uint8_t* swdi) {
 }
 #endif
 
-void swd_seq(uint32_t count, uint32_t flags, const uint8_t* swdo, uint8_t* swdi) {
+static void swd_seq(uint32_t count, uint32_t flags, const uint8_t* swdo, uint8_t* swdi) {
+    //printf("swd seqbase count=%lu\n", count);
     static uint64_t last_bit = ~(uint64_t)0;
     uint64_t devnull = 0;
 
@@ -245,24 +248,38 @@ void swd_seq(uint32_t count, uint32_t flags, const uint8_t* swdo, uint8_t* swdi)
         uint8_t lastbyte = swdo[((count + 7) >> 3) - 1];
         last_bit = (lastbyte & (1 << (count & 7))) ? ~(uint64_t)0 : (uint64_t)0;
     }
+
+    //printf("swd seqbase end\n");
 }
 
+void jtag_tms_seq(uint32_t count, const uint8_t* data);
+
 void SWJ_Sequence(uint32_t count, const uint8_t* data) {
-    swd_seq(count, 0, data, NULL);
+    //printf("swj sequence\n");
 
-    /*for (uint32_t i = 0, k = 0; i < count; ++i) {
+    // we can't just do a swd_seq() call here, as the debugger might be in JTAG
-        if ((i & 7) == 0) {
+    // instead of SWD mode.
-            val = data[k];
-            ++k;
-        }
 
-        swdio = (val >> (i & 7)) & 1;
+    if ((swdsm == -1 || swdoffset == -1) && jtagsm >= 0 && jtagoffset >= 0) {
-        // SET SWDIO
+        jtag_tms_seq(count, data); // JTAG mode -- handle in JTAG code
-        // SWCLK LO; DELAY; SWCLK HI; DELAY
+    } else if (swdsm >= 0 && swdoffset >= 0) {
+        swd_seq(count, 0, data, NULL); // SWD mode - we can do just this
+    } else {
+        //printf("E: SWJ_Sequence while not in JTAG or SBW mode\n");
+        // welp - can't really report an error to the upper CMSIS-DAP layers
+
+        jtag_tms_seq(count, data); // uses only SIO for now so ¯\_(ツ)_/¯
+    }
+
+    // hackier but stabler
+    /*jtag_tms_seq(count, data);
+    if (swdsm != -1 && swdoffset != -1 && jtagsm == -1 && jtagoffset == -1) {
+        gpio_set_function(PINOUT_JTAG_TMS, GPIO_FUNC_PIO0);
     }*/
 }
 
 uint8_t SWD_Transfer(uint32_t request, uint32_t* data) {
+    //printf("swd xfer request=%08lx\n", request);
     uint32_t parity;
     uint8_t swdo;
 
@@ -275,11 +292,13 @@ uint8_t SWD_Transfer(uint32_t request, uint32_t* data) {
 
     uint8_t ack = 0;
     swd_seq(3, SWD_SEQUENCE_DIN, NULL, &ack);
+    //printf("  ack=%hhu\n", ack);
 
     uint32_t num;
     switch (ack) {
     case DAP_TRANSFER_OK:
         if (request & DAP_TRANSFER_RnW) {
+            //printf("  xfer ok, r\n");
             uint64_t val = 0;
             parity = 0;
             // FIXME: this is little-endian-only!
@@ -295,6 +314,7 @@ uint8_t SWD_Transfer(uint32_t request, uint32_t* data) {
 
             //PIN_SWDIO_OUT_ENABLE();
         } else { // write
+            //printf("  xfer ok, w\n");
             swd_seq(DAP_Data.swd_conf.turnaround, SWD_SEQUENCE_DIN, NULL, NULL);
 
             //PIN_SWDIO_OUT_ENABLE();
@@ -309,36 +329,45 @@ uint8_t SWD_Transfer(uint32_t request, uint32_t* data) {
             swd_seq(33, 0, (const uint8_t*)&out, NULL);
         }
 
+        //printf("  set ts\n");
         if (request & DAP_TRANSFER_TIMESTAMP) DAP_Data.timestamp = TIMESTAMP_GET();
 
         num = DAP_Data.transfer.idle_cycles;
-        for (uint32_t i = 0; i < num; num += 64) {
+        //printf("  idlecyc=%lu\n", num);
+        for (uint32_t i = 0; i < num; i += 64) {
             uint64_t swdio = 0;
 
             uint32_t cyc = num - i;
             if (cyc > 64) cyc = 64;
 
+            //printf("  sequence of %lu\n", cyc);
             SWD_Sequence((cyc & SWD_SEQUENCE_CLK), (const uint8_t*)&swdio, NULL);
         }
+        //printf("  idlecyc end\n");
         break;
     case DAP_TRANSFER_WAIT: case DAP_TRANSFER_FAULT:
         num = DAP_Data.swd_conf.turnaround;
         if (DAP_Data.swd_conf.data_phase &&  (request & DAP_TRANSFER_RnW)) {
             num += 33; // 32 bits + parity
         }
+        //printf("  wait/fault: %lu\n", num);
 
         swd_seq(num, SWD_SEQUENCE_DIN, NULL, NULL);
 
         if (DAP_Data.swd_conf.data_phase && !(request & DAP_TRANSFER_RnW)) {
+            //printf("  w/f dataphase\n");
+
             uint64_t swdio = 0;
             swd_seq(33, 0, (const uint8_t*)&swdio, NULL); // 32 data bits + parity
         }
         break;
     default: // protocol error
+        //printf("  proto error\n");
         swd_seq(DAP_Data.swd_conf.turnaround + 33, SWD_SEQUENCE_DIN, NULL, NULL);
         break;
     }
 
+    //printf("  finished\n");
     PIN_SWDIO_OUT_ENABLE();
     PIN_SWDIO_SET_PIO();
     return ack;

bsp/rp2040/m_isp/sbw_hw.c

@@ -36,7 +36,6 @@ void sbw_preinit(bool nrst) {
     gpio_put(PINOUT_SBW_TCK , true ); // start SBW stuff
     busy_wait_ms(5);//busy_wait_us_32(100); // wait a bit more*/
 
-    // TODO: test #if 0 & switch over if it works
 #if 1
     (void)nrst; // always assumed nrst=false here :/
     // from slau320 sources
@@ -78,6 +77,7 @@ void sbw_preinit(bool nrst) {
 #else
     // from MSP430.DLL 'BIOS' (FETUIF?) sources
     // can handle SBW/JTAG selection and nRST stuff
+    // ... but it doesn't seem to work
 
     // TEST = TCK
     // nRESET = TDIO = NMI