jtag pio workingish but actually not at all

CMakeLists.txt

@@ -45,11 +45,12 @@ if(FAMILY STREQUAL "rp2040")
   # need uart stdio, usb is busy doing other stuff
   if(USE_USBCDC_FOR_STDIO)
     # we're going to manually implement this case
-    pico_enable_stdio_uart(${PROJECT} 0)
+	#pico_enable_stdio_uart(${PROJECT} 0)
     target_compile_definitions(${PROJECT} PUBLIC USE_USBCDC_FOR_STDIO=1 PICO_STDIO_USB=1)
   else()
-    pico_enable_stdio_uart(${PROJECT} 1)
+	  #pico_enable_stdio_uart(${PROJECT} 1)
   endif()
+  pico_enable_stdio_uart(${PROJECT} 1)
   pico_enable_stdio_usb(${PROJECT} 0)
 else()
   message(FATAL_ERROR "Invalid FAMILY '${FAMILY}' specified")
@@ -81,6 +82,8 @@ target_sources(${PROJECT} PUBLIC
   ${CMAKE_CURRENT_SOURCE_DIR}/src/m_sump/_sump.c
   ${CMAKE_CURRENT_SOURCE_DIR}/src/m_sump/cdc_sump.c
   ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/cdc_uart.c
+  ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/dap_jtag.c
+  ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/dap_swd.c
   ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/dap_uart.c
   ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/dap_swo.c
   ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/i2c_tinyusb.c
@@ -133,6 +136,7 @@ if(FAMILY STREQUAL "rp2040")
     target_link_libraries(${PROJECT} pico_stdio)
   endif()
 
+  pico_generate_pio_header(${PROJECT} ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/dap_jtag.pio)
   pico_generate_pio_header(${PROJECT} ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/swo_uart_rx.pio)
   pico_generate_pio_header(${PROJECT} ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/swo_manchester_encoding.pio)
 

README.md

@@ -120,6 +120,7 @@ projects. These respective licenses can be found in
 - [ ] PIC programming
   - https://github.com/travisgoodspeed/goodfet/tree/master/firmware/apps/pic
 - iCE40 programming??
+  - https://github.com/adamgreig/ffp
 - Renesas E7-{0,1,2} programming thing????
   - Renesas tell us how this works pls
   - https://github.com/szymonh/rl78check is something at least

bsp/rp2040/m_default/dap_jtag.c

@@ -1,9 +1,18 @@
 // vim: set et:
 
+#include <hardware/clocks.h>
+#include <hardware/gpio.h>
+#include <hardware/pio.h>
+
 #include "DAP_config.h"
 #include <DAP.h>
 
-/*#define JTAG_PIO*/
+#include "tusb_config.h"
+#include <tusb.h>
+
+#include "dap_jtag.pio.h"
+
+#define JTAG_PIO
 
 #ifndef JTAG_PIO
 void PORT_JTAG_SETUP(void) {
@@ -52,19 +61,204 @@ void PORT_OFF(void) {
                           PINOUT_TDI_MASK  //| PINOUT_TDO_MASK
                         | PINOUT_nTRST_MASK | PINOUT_nRESET_MASK;
 }
-#else
-void PORT_JTAG_SETUP(void) {
-    // TODO...
-}
-
-void PORT_OFF(void) {
-
-}
 
 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("%s", "tdi: ");
+    for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
+        printf("0x%x ", ((const uint8_t*)tdi)[j]);
+    }
+
+    if (info & JTAG_SEQUENCE_TMS) PIN_SWDIO_TMS_SET();
+    else PIN_SWDIO_TMS_CLR();
+
+    for (size_t i = 0; n != 0; ++i) {
+        uint8_t iv = tdi[i], ov = 0;
+
+        for (size_t k = 0; k < 8 && n != 0; ++k, --n) {
+            PIN_TDI_OUT((iv >> k) & 1);
+            PIN_SWCLK_TCK_CLR();
+            PIN_DELAY_SLOW(DAP_Data.clock_delay);
+            ov |= PIN_TDO_IN() << k;
+            PIN_SWCLK_TCK_SET();
+            PIN_DELAY_SLOW(DAP_Data.clock_delay);
+        }
+
+        if (info & JTAG_SEQUENCE_TDO) tdo[i] = ov;
+    }
+
+    n = info & JTAG_SEQUENCE_TCK;
+    if (n == 0) n = 64;
+
+    if (info & JTAG_SEQUENCE_TDO) {
+        printf("%s", "\ntdo: ");
+        for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
+            printf("0x%x ", ((const uint8_t*)tdo)[j]);
+        }
+        printf("%c", '\n');
+    } else printf("%s", "\nno tdo\n");
+}
+#else
+static int jtagsm = -1, offset = -1;
+
+void PORT_JTAG_SETUP(void) {
+    resets_hw->reset &= ~(RESETS_RESET_IO_BANK0_BITS | RESETS_RESET_PADS_BANK0_BITS);
+
+    /* set to default high level */
+    sio_hw->gpio_oe_set = PINOUT_TCK_MASK | PINOUT_TMS_MASK | PINOUT_TDI_MASK | PINOUT_nTRST_MASK |
+                          PINOUT_nRESET_MASK;
+    sio_hw->gpio_set = PINOUT_TCK_MASK | PINOUT_TMS_MASK | PINOUT_TDI_MASK | PINOUT_nTRST_MASK |
+                       PINOUT_nRESET_MASK;
+    /* TDO needs to be an input */
+    sio_hw->gpio_oe_clr = PINOUT_TDO_MASK;
+
+    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TCK],
+        PADS_BANK0_GPIO0_IE_BITS,  // bits to set: input enable
+        PADS_BANK0_GPIO0_IE_BITS |
+            PADS_BANK0_GPIO0_OD_BITS);  // bits to mask out: input enable, output disable
+    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TMS], PADS_BANK0_GPIO0_IE_BITS,
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
+    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TDI], PADS_BANK0_GPIO0_IE_BITS,
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
+    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TDO],
+        PADS_BANK0_GPIO0_IE_BITS |
+            PADS_BANK0_GPIO0_OD_BITS,  // TDO needs to have its output disabled
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
+    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_nTRST], PADS_BANK0_GPIO0_IE_BITS,
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
+    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_nRESET], PADS_BANK0_GPIO0_IE_BITS,
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
+
+    // NOTE: hiZ: ctrl = (ctrl & ~(CTRL_OEOVER_BITS)) | (GPIO_OVERRIDE_LOW << CTRL_OEOVER_LSB);
+    // normal == 0, low == 2
+
+    // set pin modes to general IO (SIO)
+    iobank0_hw->io[PINOUT_JTAG_TCK].ctrl = GPIO_FUNC_PIO0 << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
+    iobank0_hw->io[PINOUT_JTAG_TMS].ctrl = GPIO_FUNC_SIO << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
+    iobank0_hw->io[PINOUT_JTAG_TDI].ctrl = GPIO_FUNC_PIO0 << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
+    iobank0_hw->io[PINOUT_JTAG_TDO].ctrl = (GPIO_FUNC_PIO0 << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB)
+        /*| (GPIO_OVERRIDE_LOW << IO_BANK0_GPIO0_CTRL_OEOVER_LSB)*/;
+    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;
+
+    jtagsm = pio_claim_unused_sm(PINOUT_JTAG_PIO_DEV, true);
+    offset = pio_add_program(PINOUT_JTAG_PIO_DEV, &dap_jtag_program);
+    dap_jtag_program_init(PINOUT_JTAG_PIO_DEV, jtagsm, offset,
+             50*1000, PINOUT_JTAG_TCK, PINOUT_JTAG_TDI, PINOUT_JTAG_TDO);
+}
+
+void PORT_OFF(void) {
+    pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, jtagsm, false);
+    pio_sm_unclaim(PINOUT_JTAG_PIO_DEV, jtagsm);
+    pio_remove_program(PINOUT_JTAG_PIO_DEV, &dap_jtag_program, offset);
+    offset = jtagsm = -1;
+
+    sio_hw->gpio_oe_clr = PINOUT_SWCLK_MASK | PINOUT_SWDIO_MASK |
+                          PINOUT_TDI_MASK  //| PINOUT_TDO_MASK
+                        | PINOUT_nTRST_MASK | PINOUT_nRESET_MASK;
+}
+
+static uint8_t bitswap(uint8_t in) {
+    static const uint8_t lut[16] = {
+        0x0, 0x8, 0x4, 0xc, 0x2, 0xa, 0x6, 0xe,
+        0x1, 0x9, 0x5, 0xd, 0x3, 0xb, 0x7, 0xf
+    };
+
+    return (lut[in&0xf] << 4) | lut[in>>4];
+}
+
+void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
+    dap_jtag_program_init(PINOUT_JTAG_PIO_DEV, jtagsm, offset,
+             50*1000, PINOUT_JTAG_TCK, PINOUT_JTAG_TDI, PINOUT_JTAG_TDO);
+
+    uint32_t n = info & JTAG_SEQUENCE_TCK;
+    if (n == 0) n = 64;
+
+    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];
+
+    uint32_t bytelen = (n + 7) >> 3;
+    uint32_t last_shift = (8 - n) & 7;//(bytelen << 3) - n;
+    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("%s", "tdi: ");
+    for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
+        printf("0x%x ", ((const uint8_t*)tdi)[j]);
+    }
+
+    //printf("init : pc=%x insn=%04lx\n", pio_sm_get_pc(PINOUT_JTAG_PIO_DEV, jtagsm), pio0_hw->sm[jtagsm].instr);
+    *tx = (uint8_t)(n - 1);
+    //printf("txlen: pc=%x insn=%04lx\n", pio_sm_get_pc(PINOUT_JTAG_PIO_DEV, jtagsm), pio0_hw->sm[jtagsm].instr);
+    //return;
+
+    bool done_extrabit = false;
+    size_t oi = 0, ii = 0;
+    while (txremain || rxremain) {
+        if (txremain && !pio_sm_is_tx_fifo_full(PINOUT_JTAG_PIO_DEV, jtagsm)) {
+            //printf("tdi%2u: pc=%x insn=%04lx\n", ii, pio_sm_get_pc(PINOUT_JTAG_PIO_DEV, jtagsm), pio0_hw->sm[jtagsm].instr);
+            *tx = bitswap(tdi[ii]);
+            --txremain;
+            printf("tx %02x rem %lu smpc=%x\n", tdi[ii], txremain, pio_sm_get_pc(PINOUT_JTAG_PIO_DEV, jtagsm));
+            ++ii;
+        }
+
+        if (rxremain && !pio_sm_is_rx_fifo_empty(PINOUT_JTAG_PIO_DEV, jtagsm)) {
+            //printf("tdo%2u: pc=%x insn=%04lx\n", oi, pio_sm_get_pc(PINOUT_JTAG_PIO_DEV, jtagsm), pio0_hw->sm[jtagsm].instr);
+            uint8_t ov = *rx;
+            --rxremain;
+            printf("rx %02x rem %lu smpc=%x\n", ov, rxremain, pio_sm_get_pc(PINOUT_JTAG_PIO_DEV, jtagsm));
+            if ((info & JTAG_SEQUENCE_TDO) /*&& !(!last_shift && rxremain == 1)*/) {
+                if (last_shift && oi == bytelen - 1) {
+                    tdo[oi] = bitswap(ov << last_shift);
+                } else {
+                    tdo[oi] = bitswap(ov);
+                }
+                ++oi;
+            }
+        }
+
+        /*if (rxremain == 1 && txremain == 0 && last_shift == 0) {
+            printf("smpc=%x smin=%04lx\n",
+                    pio_sm_get_pc(PINOUT_JTAG_PIO_DEV, jtagsm), pio0_hw->sm[jtagsm].instr);
+
+            if (!pio_sm_is_tx_fifo_full(PINOUT_JTAG_PIO_DEV, jtagsm) && !done_extrabit) {
+                done_extrabit = true;
+                uint8_t last_byte = tdi[ii-1];
+                uint8_t last_bit = last_byte & 0x80;
+                printf("extrabit %x!\n", last_bit);
+                *tx = last_bit ? 0xff : 0x00;
+            }
+        }*/
+    }
+
+    /*if (last_shift) {
+        uint8_t* last = &tdo[bytelen-1];
+        *last = bitswap(*last);
+        *last <<= last_shift;
+        *last = bitswap(*last);
+    }*/
+
+    if (info & JTAG_SEQUENCE_TDO) {
+        printf("%s", "\ntdo: ");
+        for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
+            printf("0x%x ", ((const uint8_t*)tdo)[j]);
+        }
+        printf("%c", '\n');
+    } else printf("%s", "\nno tdo\n");
+
+    /*uint32_t n = info & JTAG_SEQUENCE_TCK;
+    if (n == 0) n = 64;
+
     bool tms = info & JTAG_SEQUENCE_TMS;
     // SET TMS TO ^
 
@@ -80,24 +274,48 @@ void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
             ov |= tdo << k;
         }
 
-        tdo[i] = ov;
+        if (info & JTAG_SEQUENCE_TDO) tdo[i] = ov;
-    }
+    }*/
 }
+#endif
 
 // TODO: the following ones can all be implemented in terms of JTAG_Sequence
 
+static void jtag_seq(uint32_t num, int tms, const void* tdi, void* tdo) {
+    static uint64_t last_bit = ~(uint64_t)0;
+    uint64_t devnull = 0;
+
+    bool notdi, notdo;
+
+    if ((notdi = (tdi == NULL))) tdi = &last_bit;
+    if ((notdo = (tdo == NULL))) tdo = &devnull;
+    else tms |= JTAG_SEQUENCE_TDO;
+
+    uint32_t nreal;
+    for (uint32_t i = 0; i < num; i += nreal) {
+        uint32_t nmod = (num - i) & 63;
+        nreal = nmod ? nmod : 64;
+
+        JTAG_Sequence(nmod | tms, (const uint8_t*)tdi + (i >> 3), (uint8_t*)tdo + (i >> 3));
+    }
+
+    uint8_t lastbyte = *((const uint8_t*)tdi + (num >> 3));
+    last_bit = (lastbyte & (1 << (num & 7))) ? ~(uint64_t)0 : (uint64_t)0;
+}
+
 uint32_t JTAG_ReadIDCode(void) {
     // tdi=NULL: ~~0xff!~~ repeat last-seen bit, ignore otherwise
     // tdo=NULL: ignore
-    JTAG_Sequence(1|JTAG_SEQUENCE_TMS, NULL, NULL);
+    jtag_seq(1, JTAG_SEQUENCE_TMS, NULL, NULL);
-    JTAG_Sequence((2+DAP_Data.jtag_dev.index)|0, NULL, NULL);
+    jtag_seq(2+DAP_Data.jtag_dev.index, 0, NULL, NULL);
     uint32_t v=0, v2=0;
-    JTAG_Sequence(31|0, NULL, &v);
+    jtag_seq(31, 0, NULL, &v);
-    JTAG_Sequence(2|JTAG_SEQUENCE_TMS, NULL, &v2);
+    jtag_seq(2, JTAG_SEQUENCE_TMS, NULL, &v2);
-    v |= (v2 << 31 & 1);
+    v |= (v2 << 31);
-    JTAG_Sequence(1|0, NULL, NULL);
+    jtag_seq(1, 0, NULL, NULL);
+    return v;
 
-    // TMS HI
+    /*// TMS HI
     // TCK LOW; DELAY; TCK HI; DELAY (DRscan)
     // TMS LOW
     // TCK LOW; DELAY; TCK HI; DELAY (capture)
@@ -122,26 +340,30 @@ uint32_t JTAG_ReadIDCode(void) {
     // TMS LO
     // TCK LOW; DELAY; TCK HI; DELAY
 
-    return v;
+    return v;*/
 }
 
 void JTAG_IR(uint32_t ir) {
-    JTAG_Sequence(2|JTAG_SEQUENCE_TMS, NULL, NULL);
+    jtag_seq(2,JTAG_SEQUENCE_TMS, NULL, NULL);
-    JTAG_Sequence(2|0, NULL, NULL);
+    jtag_seq(2,0, NULL, NULL);
     uint64_t v = ~(uint64_t)0;
-    JTAG_Sequence(DAP_Data.jtag_dev.ir_before[DAP_Data.jtag_dev.index]|0, &v, NULL);
+    jtag_seq(DAP_Data.jtag_dev.ir_before[DAP_Data.jtag_dev.index],0, &v, NULL);
-    JTAG_Sequence((DAP_Data.jtag_dev.ir_length[DAP_Data.jtag_dev.index]-1)|0, &ir, NULL);
+    jtag_seq((DAP_Data.jtag_dev.ir_length[DAP_Data.jtag_dev.index]-1),0, &ir, NULL);
+    uint32_t ir2 = ir >> (DAP_Data.jtag_dev.ir_length[DAP_Data.jtag_dev.index]-1);
     uint32_t n = DAP_Data.jtag_dev.ir_after[DAP_Data.jtag_dev.index];
     if (n) {
-        JTAG_Sequence(1|0, &(ir>>TODO_N), NULL);
+        jtag_seq(1,0, &ir2, NULL);
-        ir = -1;
+        ir = ~(uint32_t)0;
-        JTAG_Sequence((n-1)|0, &ir, NULL);
+        jtag_seq((n-1),0, &ir, NULL);
-        JTAG_Sequence(1|JTAG_SEQUENCE_TMS, &ir, NULL);
+        jtag_seq(1,JTAG_SEQUENCE_TMS, &ir, NULL);
     } else {
-        JTAG_Sequence(1|JTAG_SEQUENCE_TMS, &(ir>>TODO_N), NULL);
+        jtag_seq(1,JTAG_SEQUENCE_TMS, &ir2, NULL);
     }
+    jtag_seq(1,JTAG_SEQUENCE_TMS, NULL, NULL);
+    jtag_seq(1,0, NULL, NULL);
+    PIN_TDI_OUT(1); // TODO: TDI HI
 
-    // TMS HI
+    /*// TMS HI
     // TCK LOW; DELAY; TCK HI; DELAY (DRscan)
     // TCK LOW; DELAY; TCK HI; DELAY (IRscan)
     // TMS LO
@@ -180,50 +402,56 @@ void JTAG_IR(uint32_t ir) {
     // TCK LOW; DELAY; TCK HI; DELAY
     // TMS LO
     // TCK LOW; DELAY; TCK HI; DELAY
-    // TDI HI
+    // TDI HI*/
 }
 
-static uint8_t xfer_base(uint32_t request, uint32_t* data; bool check_ack) {
+static uint8_t xfer_base(uint32_t request, uint32_t* data, bool check_ack) {
-    JTAG_Sequence(1|JTAG_SEQUENCE_TMS, NULL, NULL);
+    jtag_seq(1,JTAG_SEQUENCE_TMS, NULL, NULL);
-    JTAG_Sequence((2+DAP_Data.jtag_dev.index)|0, NULL, NULL);
+    jtag_seq((2+DAP_Data.jtag_dev.index),0, NULL, NULL);
     uint32_t ack=0;
-    JTAG_Sequence(3|0, &(request>>1), &ack);
+    uint32_t reqsh1 = request>>1;
+    jtag_seq(3,0, &reqsh1, &ack);
+    // ack bits are 1,0,2, not 0,1,2 => swap bottom 2 bits
+    ack = ((ack & 2) >> 1) | ((ack & 1) << 2) | (ack & ~(uint32_t)3);
     if (ack!=DAP_TRANSFER_OK && check_ack) {
-        JTAG_Sequence(1|JTAG_SEQUENCE_TMS, NULL, NULL);
+        jtag_seq(1,JTAG_SEQUENCE_TMS, NULL, NULL);
         goto exit;
-    }
+    } else ack=DAP_TRANSFER_OK;
     if (request & DAP_TRANSFER_RnW) { // read
         uint32_t val = 0;
-        JTAG_Sequence(31|0, NULL, &val);
+        jtag_seq(31,0, NULL, &val);
         uint32_t n = DAP_Data.jtag_dev.count - DAP_Data.jtag_dev.index - 1;
+        uint32_t valb = 0;
         if (n) {
-            JTAG_Sequence(1|0, NULL, &(val>>31));
+            jtag_seq(1,0, NULL, &valb);
-            JTAG_Sequence((n-1)|0, NULL, NULL);
+            jtag_seq((n-1),0, NULL, NULL);
-            JTAG_Sequence(1|JTAG_SEQUENCE_TMS, NULL, NULL);
+            jtag_seq(1,JTAG_SEQUENCE_TMS, NULL, NULL);
         } else {
-            JTAG_Sequence(1|JTAG_SEQUENCE_TMS, NULL, &(val>>31));
+            jtag_seq(1,JTAG_SEQUENCE_TMS, NULL, &valb);
         }
+        *data = val | (valb << 31);
     } else {
-        uint32_t val = *data;
+        uint32_t val = *data, valb=val>>31;
-        JTAG_Sequence(31|0, &val, NULL);
+        jtag_seq(31,0, &val, NULL);
         uint32_t n = DAP_Data.jtag_dev.count - DAP_Data.jtag_dev.index - 1;
         if (n) {
-            JTAG_Sequence(1|0, &(val>>31), NULL);
+            jtag_seq(1,0, &valb, NULL);
-            JTAG_Sequence((n-1)|0, NULL, NULL);
+            jtag_seq((n-1),0, NULL, NULL);
-            JTAG_Sequence(1|JTAG_SEQUENCE_TMS, NULL, NULL);
+            jtag_seq(1,JTAG_SEQUENCE_TMS, NULL, NULL);
         } else {
-            JTAG_Sequence(1|JTAG_SEQUENCE_TMS, &(val>>31), NULL);
+            jtag_seq(1,JTAG_SEQUENCE_TMS, &valb, NULL);
         }
     }
 exit:
-    JTAG_Sequence(1|JTAG_SEQUENCE_TMS, NULL, NULL);
+    jtag_seq(1,JTAG_SEQUENCE_TMS, NULL, NULL);
-    JTAG_Sequence(1|0, NULL, NULL);
+    jtag_seq(1,0, NULL, NULL);
-    // TODO: TDI HI (no clk)
+    PIN_TDI_OUT(1); // TODO: TDI HI (no clk)
-    if (request & DAP_REQUEST_TIMESTAMP) DAP_Data.timestamp = TIMESTAMP_GET();
+    if (request & DAP_TRANSFER_TIMESTAMP) DAP_Data.timestamp = TIMESTAMP_GET();
-    if (check_ack) JTAG_Sequence(DAP_Data.idle_cycles, NULL, NULL);
+    if (check_ack) jtag_seq(DAP_Data.transfer.idle_cycles, 0, NULL, NULL);
+    return (uint8_t)ack;
 
 
-    // TMS HI
+    /*// TMS HI
     // TCK LOW; DELAY; TCK HI; DELAY
     // TMS LO
     // TCK LOW; DELAY; TCK HI; DELAY
@@ -327,7 +555,7 @@ exit:
         // TCK LOW; DELAY; TCK HI; DELAY
     }
 
-    return (uint8_t)ack;
+    return (uint8_t)ack;*/
 }
 
 void JTAG_WriteAbort(uint32_t data) {
@@ -337,5 +565,5 @@ void JTAG_WriteAbort(uint32_t data) {
 uint8_t JTAG_Transfer(uint32_t request, uint32_t* data) {
     return xfer_base(request, data, true);
 }
-#endif
+//#endif
 

bsp/rp2040/m_default/dap_jtag.pio

@@ -1,28 +1,55 @@
 ; vim: set et:
 
 .program dap_jtag
+.side_set 1 opt
+
+; Pin assignments:
+; - TCK is side-set pin 0
+; - TDI is OUT pin 0
+; - TDO is IN pin 0
+;
+; Autopush and autopull must be enabled, and the serial frame size is set by
+; configuring the push/pull threshold (32 bits). Shift should be left
+;
+; data is captured on the leading edge of each TCK pulse, and
+; transitions on the trailing edge, or some time before the first leading edge.
 
 start:
-    jmp start
+    pull                        ; get length-1 and disregard previous OSR state
+    out x, 32       side 0      ; this moves the first 32 bits into X
+loop:
+    out pins, 1     side 0      ; Stall here on empty (sideset proceeds even if instruction stalls, so we stall with SCK low
+    in pins, 1      side 1 [1]
+    jmp x-- loop    side 0
+end:
+    push            side 0      ; Force the last ISR bits to be pushed to the tx fifo
 
 
 % c-sdk {
-static inline void dap_jtag_program_init(PIO pio, uint sm, uint offset, uint pin, uint baud) {
+static inline void dap_jtag_program_init(PIO pio, uint sm, uint offset,
-    pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, false);
+        uint16_t freq, uint pin_tck, uint pin_tdi, uint pin_tdo) {
-    pio_gpio_init(pio, pin);
-    gpio_pull_up(pin);
-
     pio_sm_config c = dap_jtag_program_get_default_config(offset);
-    sm_config_set_in_pins(&c, pin); // for WAIT, IN
+    sm_config_set_out_pins(&c, pin_tdi, 1);
-    sm_config_set_jmp_pin(&c, pin); // for JMP
+    sm_config_set_in_pins(&c, pin_tdo);
-    // Shift to right, autopull disabled
+    sm_config_set_sideset_pins(&c, pin_tck);
-    sm_config_set_in_shift(&c, true, false, 32);
+    // (shift to left, autopush/pull, threshold=nbits)
-    // SM transmits 1 bit per 8 execution cycles.
+    sm_config_set_out_shift(&c, false, true, 8); // shift left feature is broken???
-    float div = (float)clock_get_hz(clk_sys) / (8 * baud);
+    sm_config_set_in_shift(&c, false, true, 8);
-    sm_config_set_clkdiv(&c, div);
+    sm_config_set_clkdiv(&c, (float)clock_get_hz(clk_sys) / (4 * freq));
 
+    // TDI, TCK output are low, TDO is input
+    pio_sm_set_pins_with_mask(pio, sm, 0, (1u << pin_tck) | (1u << pin_tdi));
+    pio_sm_set_pindirs_with_mask(pio, sm, (1u << pin_tck) | (1u << pin_tdi),
+            (1u << pin_tck) | (1u << pin_tdi) | (1u << pin_tdo));
+    pio_gpio_init(pio, pin_tdi);
+    //pio_gpio_init(pio, pin_tdo);
+    pio_gpio_init(pio, pin_tck);
+
+    // jtag is synchronous, so bypass input synchroniser to reduce input delay.
+    hw_set_bits(&pio->input_sync_bypass, 1u << pin_tdo);
+    gpio_set_pulls(pin_tdo, false, true); // TDO is pulled down
     pio_sm_init(pio, sm, offset, &c);
-    //pio_sm_set_enabled(pio, sm, true);
+    pio_sm_set_enabled(pio, sm, true);
 }
 
 %}

bsp/rp2040/m_default/dap_swd.c

@@ -25,16 +25,200 @@ void PORT_SWD_SETUP(void) {
     // TODO...
 }
 
-void SWJ_Sequence(uint32_t count, const uint8_t* data) {
+// TODO: also hijack PIN_SWDIO_OUT_{DIS,EN}ABLE ! (used in DAP_SWD_Sequence)
+// TODO: also hijack DAP_SWJ_PINS(?: should data pins be controlled like that? only rst stuff tbh)
 
+void SWJ_Sequence(uint32_t count, const uint8_t* data) {
+    for (uint32_t i = 0, k = 0; i < count; ++i) {
+        if ((i & 7) == 0) {
+            val = data[k];
+            ++k;
+        }
+
+        swdio = (val >> (i & 7)) & 1;
+        // SET SWDIO
+        // SWCLK LO; DELAY; SWCLK HI; DELAY
+    }
 }
 
 void SWD_Sequence(uint32_t info, const uint8_t* swdo, uint8_t* swdi) {
+    uint32_t n = info & SWD_SEQUENCE_CLK;
+    if (n == 0) n = 64;
 
+    if (info & SWD_SEQUENCE_DIN) {
+        for (uint32_t i = 0; i < n; ) {
+            uint32_t v = 0;
+            for (uint32_t k = 0; k < 8; ++k, ++i) {
+                // SWCLK LO; DELAY
+                // GET SWDIO; SWCLK HI; DELAY
+                val |= swdio << k;
+            }
+            swdi[i >> 3] = v;
+        }
+    } else {
+        for (uint32_t i = 0; i < n; ) {
+            uint32_t val = swdo[i >> 3];
+            for (uint32_t k = 0; k < 8; ++i, ++k) {
+                swdio = (val >> k) & 1;
+                // SET SWDIO
+                // SWCLK LO; DELAY; SWCLK HI; DELAY
+            }
+        }
+    }
 }
 
 void SWD_Transfer(uint32_t request, uint32_t* data) {
+    // TODO: to SWD_Sequence stuff(?)
 
+    uint32_t parity = 0;
+
+    swdio = 1;
+    parity += swdio;
+    // SET SWDIO
+    // SWCLK LO; DELAY; SWCLK HI; DELAY
+    swdio = (request >> 0) & 1;
+    parity += swdio;
+    // SET SWDIO
+    // SWCLK LO; DELAY; SWCLK HI; DELAY
+    swdio = (request >> 1) & 1;
+    parity += swdio;
+    // SET SWDIO
+    // SWCLK LO; DELAY; SWCLK HI; DELAY
+    swdio = (request >> 2) & 1;
+    parity += swdio;
+    // SET SWDIO
+    // SWCLK LO; DELAY; SWCLK HI; DELAY
+    swdio = (request >> 3) & 1;
+    parity += swdio;
+    // SET SWDIO
+    // SWCLK LO; DELAY; SWCLK HI; DELAY
+
+    swdio = parity & 1;
+    // SET SWDIO
+    // SWCLK LO; DELAY; SWCLK HI; DELAY
+
+    swdio = 0;
+    // SET SWDIO
+    // SWCLK LO; DELAY; SWCLK HI; DELAY
+
+    swdio = 1;
+    // SET SWDIO
+    // SWCLK LO; DELAY; SWCLK HI; DELAY
+
+    // TODO: SWDIO is now input
+
+    for (size_t i = 0; i < DAP_Data.swd_conf.turnaround; ++i) {
+        // SWCLK LO; DELAY; SWCLK HI; DELAY
+    }
+
+    uint32_t ack = 0;
+
+    // SWCLK LO; DELAY
+    // GET SWDIO; SWCLK HI; DELAY
+    ack |= swdio << 0;
+    // SWCLK LO; DELAY
+    // GET SWDIO; SWCLK HI; DELAY
+    ack |= swdio << 1;
+    // SWCLK LO; DELAY
+    // GET SWDIO; SWCLK HI; DELAY
+    ack |= swdio << 2;
+
+    switch (ack) {
+    case DAP_TRANSFER_OK:
+        if (request & DAP_TRANSFER_RnW) { // read
+            uint32_t val = 0;
+            parity = 0;
+
+            for (size_t i = 0; i < 32; ++i) {
+                // SWCLK LO; DELAY
+                // GET SWDIO; SWCLK HI; DELAY
+                parity += swdio;
+                val |= swdio << i;
+            }
+
+            // SWCLK LO; DELAY
+            // GET SWDIO; SWCLK HI; DELAY
+            if ((parity & 1) != (swdio & 1)) {
+                ack = DAP_TRANSFER_ERROR;
+            }
+            if (data) *data = val;
+
+            for (size_t i = 0; i < DAP_Data.swd_conf.turnaround; ++i) {
+                // SWCLK LO; DELAY; SWCLK HI; DELAY
+            }
+
+            // TODO: swdio is now output!
+        } else { // write
+            for (size_t i = 0; i < DAP_Data.swd_conf.turnaround; ++i) {
+                // SWCLK LO; DELAY; SWCLK HI; DELAY
+            }
+
+            // TODO: SWDIO is now output!
+
+            uint32_t val = *data;
+            parity = 0;
+
+            for (size_t i = 0; i < 32; ++i) {
+                swdio = (val >> i) & 1;
+                parity += swdio;
+                // SET SWDIO
+                // SWCLK LO; DELAY; SWCLK HI; DELAY
+            }
+            swdio = parity;
+            // SET SWDIO
+            // SWCLK LO; DELAY; SWCLK HI; DELAY
+        }
+
+        if (request & DAP_TRANSFER_TIMESTAMP) DAP_Data.timestamp = TIMESTAMP_GET();
+
+        uint32_t n = DAP_Data.transfer.idle_cycles;
+        if (n) {
+            swdio = 0;
+            // SET SWDIO
+            for (size_t i = 0; i < n; ++i) {
+                // SWCLK LO; DELAY; SWCLK HI; DELAY
+            }
+        }
+
+        swdio = 1;
+        // SET SWDIO (no clk!)
+
+        return (uint8_t)ack;
+
+    case DAP_TRANSFER_WAIT: case DAP_TRANSFER_FAULT:
+        if (DAP_Data.swd_conf.data_phase && ((request & DAP_TRANSFER_RnW) != 0)) {
+            for (size_t i = 0; i < 33; ++i) { // 32data + parity
+                // SWCLK LO; DELAY; SWCLK HI; DELAY
+            }
+        }
+
+        for (size_t i = 0; i < DAP_Data.swd_conf.turnaround; ++i) {
+            // SWCLK LO; DELAY; SWCLK HI; DELAY
+        }
+
+        // TODO: SWDIO to output!
+        if (DAP_Data.swd_conf.data_phase && ((request & DAP_TRANSFER_RnW) != 0)) {
+            swdio = 0;
+            // SET SWDIO
+            for (size_t i = 0; i < 33; ++i) { // 32data + parity
+                // SWCLK LO; DELAY; SWCLK HI; DELAY
+            }
+        }
+
+        swdio = 1;
+        // SET SWDIO (no clk!)
+        return (uint8_t)ack;
+
+    default: // protocol error
+        for (uint32_t i = 0; i < DAP_Data.swd_conf.turnaround + 33; ++i) {
+            // SWCLK LO; DELAY; SWCLK HI; DELAY
+        }
+
+        // TODO: SWDIO to output!
+        swdio = 1;
+        // SET SWDIO (no clk!)
+        return (uint8_t)ack;
+    }
 }
 #endif
 

bsp/rp2040/m_default/dap_swo.c

@@ -17,7 +17,7 @@ static uint32_t
 static int swo_sm = -1, swo_dmach = -1;
 static bool mode_enabled = false;
 
-#define SWO_PIO pio1
+#define SWO_PIO PINOUT_JTAG_SWO_DEV
 
 // Enable or disable SWO Mode (UART)
 //   enable: enable flag

bsp/rp2040/m_default/pinout.h

@@ -18,6 +18,8 @@
 #define PINOUT_JTAG_TDO    5
 #define PINOUT_JTAG_nTRST  6
 #define PINOUT_JTAG_nRESET 7
+#define PINOUT_JTAG_PIO_DEV pio0
+#define PINOUT_JTAG_SWO_DEV pio1
 
 // SPI config
 #define PINOUT_SPI_DEV  spi1
@@ -60,6 +62,10 @@
  *   SWO-MC	1
  *
  * PIO:
+ *   PIO0: (max. 4)
+ *     JTAG	1
+ *   PIO1: (max. 4)
+ *     SWO	2
  *
  * UART0: stdio
  *