From c1b3c26299c424064754eb5617a83d51ab6326ee Mon Sep 17 00:00:00 2001 From: sys64738 Date: Mon, 11 Oct 2021 22:38:14 +0200 Subject: [PATCH] CMSIS-DAP protocol fixes and other stuff --- CMSIS-DAP/Firmware/Source/DAP.c | 4 + bsp/rp2040/bsp-info.h | 8 +- bsp/rp2040/m_default/dap_jtag.c | 126 ++++++++++++++++++++++++------ bsp/rp2040/m_default/dap_jtag.pio | 1 + bsp/rp2040/m_default/dap_swd.c | 53 ++++++++++--- bsp/rp2040/m_isp/sbw_hw.c | 2 +- 6 files changed, 155 insertions(+), 39 deletions(-) diff --git a/CMSIS-DAP/Firmware/Source/DAP.c b/CMSIS-DAP/Firmware/Source/DAP.c index 111a075..896d145 100644 --- a/CMSIS-DAP/Firmware/Source/DAP.c +++ b/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); } diff --git a/bsp/rp2040/bsp-info.h b/bsp/rp2040/bsp-info.h index c54a56d..cde02e2 100644 --- a/bsp/rp2040/bsp-info.h +++ b/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" diff --git a/bsp/rp2040/m_default/dap_jtag.c b/bsp/rp2040/m_default/dap_jtag.c index a20a7c4..7133842 100644 --- a/bsp/rp2040/m_default/dap_jtag.c +++ b/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("%s", "tdi: "); - for (size_t j = 0; j < ((n + 7) >> 3); ++j) { - printf("0x%x ", ((const uint8_t*)tdi)[j]); - } +// 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(); @@ -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) { - 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"); +// 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"); +}*/ + +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(); - else PIN_SWDIO_TMS_CLR(); + //if (!(n & JTAG_SEQUENCE_NO_TMS)) { + 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 diff --git a/bsp/rp2040/m_default/dap_jtag.pio b/bsp/rp2040/m_default/dap_jtag.pio index 1234359..c348e79 100644 --- a/bsp/rp2040/m_default/dap_jtag.pio +++ b/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) diff --git a/bsp/rp2040/m_default/dap_swd.c b/bsp/rp2040/m_default/dap_swd.c index f03584f..520b736 100644 --- a/bsp/rp2040/m_default/dap_swd.c +++ b/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) { - if ((i & 7) == 0) { - val = data[k]; - ++k; - } + // we can't just do a swd_seq() call here, as the debugger might be in JTAG + // instead of SWD mode. - swdio = (val >> (i & 7)) & 1; - // SET SWDIO - // SWCLK LO; DELAY; SWCLK HI; DELAY + if ((swdsm == -1 || swdoffset == -1) && jtagsm >= 0 && jtagoffset >= 0) { + jtag_tms_seq(count, data); // JTAG mode -- handle in JTAG code + } 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; diff --git a/bsp/rp2040/m_isp/sbw_hw.c b/bsp/rp2040/m_isp/sbw_hw.c index 8b3597c..3503a3c 100644 --- a/bsp/rp2040/m_isp/sbw_hw.c +++ b/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