sys64738
/
DragonProbe
1
0
Fork 0
Code Issues 1 Pull Requests 1 Releases Wiki Activity
DragonProbe/bsp/rp2040/m_default/dap_swd.c

355 lines
11 KiB
C
Raw Blame History

// vim: set et:
#include <hardware/clocks.h>
#include <hardware/gpio.h>
#include <hardware/pio.h>
#include <hardware/timer.h>
#include "DAP_config.h"
#include <DAP.h>
#include "dap_swd.pio.h"
#define SWD_PIO
int swdsm = -1, swdoffset = -1;
#ifndef SWD_PIO
void PORT_SWD_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_SWCLK_MASK | PINOUT_SWDIO_MASK;
sio_hw->gpio_set = PINOUT_SWCLK_MASK | PINOUT_SWDIO_MASK;
hw_write_masked(&padsbank0_hw->io[PINOUT_SWCLK], PADS_BANK0_GPIO0_IE_BITS,
PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
hw_write_masked(&padsbank0_hw->io[PINOUT_SWDIO], PADS_BANK0_GPIO0_IE_BITS,
PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
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;
}
void PIN_SWDIO_OUT_ENABLE(void) {
sio_hw->gpio_oe_set = PINOUT_SWDIO_MASK;
}
void PIN_SWDIO_OUT_DISABLE(void) {
sio_hw->gpio_oe_clr = PINOUT_SWDIO_MASK;
}
inline static void PIN_SWDIO_SET_PIO(void) { PIN_SWDIO_TMS_SET(); }
/*#define PIN_SWCLK_SET PIN_SWCLK_TCK_SET
#define PIN_SWCLK_CLR PIN_SWCLK_TCK_CLR
#define SW_WRITE_BIT(bit) \
PIN_SWDIO_OUT(bit); \
PIN_SWCLK_CLR(); \
PIN_DELAY(); \
PIN_SWCLK_SET(); \
PIN_DELAY()
#define SW_READ_BIT(bit) \
PIN_SWCLK_CLR(); \
PIN_DELAY(); \
bit = PIN_SWDIO_IN(); \
PIN_SWCLK_SET(); \
PIN_DELAY()
#define PIN_DELAY() PIN_DELAY_SLOW(DAP_Data.clock_delay)
void SWD_Sequence (uint32_t info, const uint8_t *swdo, uint8_t *swdi) {
printf("hi seq\n");
uint32_t val;
uint32_t bit;
uint32_t n, k;
n = info & SWD_SEQUENCE_CLK;
if (n == 0U) {
n = 64U;
}
if (info & SWD_SEQUENCE_DIN) {
printf("seq n=%lu din\n", n);
while (n) {
val = 0U;
for (k = 8U; k && n; k--, n--) {
SW_READ_BIT(bit);
val >>= 1;
val |= bit << 7;
}
val >>= k;
*swdi++ = (uint8_t)val;
printf("rx %02lx\n", val);
}
} else {
printf("seq n=%lu dout\n", n);
while (n) {
val = *swdo++;
printf("tx %02lx\n", val);
for (k = 8U; k && n; k--, n--) {
SW_WRITE_BIT(val);
val >>= 1;
}
}
}
}*/
#else
void PORT_SWD_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_SWCLK_MASK | PINOUT_SWDIO_MASK;
sio_hw->gpio_set = PINOUT_SWCLK_MASK | PINOUT_SWDIO_MASK;
hw_write_masked(&padsbank0_hw->io[PINOUT_SWCLK], PADS_BANK0_GPIO0_IE_BITS,
PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
hw_write_masked(&padsbank0_hw->io[PINOUT_SWDIO], PADS_BANK0_GPIO0_IE_BITS,
PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
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 (swdoffset == -1)
swdoffset = pio_add_program(PINOUT_JTAG_PIO_DEV, &dap_swd_program);
dap_swd_program_init(PINOUT_JTAG_PIO_DEV, swdsm, swdoffset,
50*1000, PINOUT_SWCLK, PINOUT_SWDIO);
}
// TODO: also hijack DAP_SWJ_PINS(?: should data pins be controlled like that? only rst stuff tbh)
void PIN_SWDIO_OUT_ENABLE(void) {
pio_sm_set_pindirs_with_mask(PINOUT_JTAG_PIO_DEV, swdsm,
(1u << PINOUT_SWDIO), (1u << PINOUT_SWDIO));
}
void PIN_SWDIO_OUT_DISABLE(void) {
pio_sm_set_pindirs_with_mask(PINOUT_JTAG_PIO_DEV, swdsm,
(0u << PINOUT_SWDIO), (1u << PINOUT_SWDIO));
}
inline static void PIN_SWDIO_SET_PIO(void) {
pio_sm_set_pins_with_mask(PINOUT_JTAG_PIO_DEV, swdsm,
(1u << PINOUT_SWDIO), (1u << PINOUT_SWDIO));
}
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 SWD_Sequence(uint32_t info, const uint8_t* swdo, uint8_t* swdi) {
pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, swdsm, true);
//busy_wait_us_32(0);
float div = (float)clock_get_hz(clk_sys) / (2 * DAP_Data.clock_freq);
if (div < 2) div = 2;
else if (div > 65536) div = 65536;
pio_sm_set_clkdiv(PINOUT_JTAG_PIO_DEV, swdsm, div);
uint32_t n = info & SWD_SEQUENCE_CLK;
if (n == 0) n = 64;
io_wo_8* tx = (io_wo_8*)&PINOUT_JTAG_PIO_DEV->txf[swdsm];
io_ro_8* rx = (io_ro_8*)&PINOUT_JTAG_PIO_DEV->rxf[swdsm];
uint32_t bytelen = (n + 7) >> 3;
uint32_t last_shift = (8 - n) & 7;
uint32_t txremain = bytelen,
rxremain = last_shift ? bytelen : (bytelen + 1);
/*printf("seq n=%lu bytelen=%lu lsh=%lu txr=%lu rxr=%lu\n",
n, bytelen, last_shift, txremain, rxremain);*/
pio_sm_put_blocking(PINOUT_JTAG_PIO_DEV, swdsm,
(uint8_t)(n - 1) | ((info & SWD_SEQUENCE_DIN) ? 0x80000000u : 0));
if (info & SWD_SEQUENCE_DIN) {
for (size_t i = 0; rxremain; tight_loop_contents()) {
if (!pio_sm_is_rx_fifo_empty(PINOUT_JTAG_PIO_DEV, swdsm)) {
uint8_t v = *rx;
--rxremain;
//printf("seq din rx %02x rxr=%lu\n", bitswap(v), rxremain);
if (i < bytelen) {
if (last_shift && i == bytelen - 1) {
swdi[i] = bitswap(v) >> last_shift;
} else {
swdi[i] = bitswap(v);
}
++i;
}
}
}
} else {
for (size_t i = 0; txremain; tight_loop_contents()) {
if (!pio_sm_is_tx_fifo_full(PINOUT_JTAG_PIO_DEV, swdsm)) {
*tx = bitswap(swdo[i]);
--txremain;
//printf("seq dout tx %02x txr=%lu\n", swdo[i], txremain);
++i;
}
}
// wait until FIFO empty, so that all bytes have been xmitted
while (!pio_sm_is_tx_fifo_empty(PINOUT_JTAG_PIO_DEV, swdsm)) tight_loop_contents();
// ^ isn't enough, because of the side-set, so we need the loop below
// however, we still need the above one because otherwise we might
// somehow miss sending some bits in practice...
// wait until last bit xmitted, and back at the starting insn
while (pio_sm_get_pc(PINOUT_JTAG_PIO_DEV, swdsm) != swdoffset) tight_loop_contents();
}
// we need to disable (and reenable at the start of this routine) the SM
// because we also use it to set the SWDIO pin and direction elsewhere,
// which shouldn't happen when the SM is enabled
//busy_wait_us_32(0);
pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, swdsm, false);
//printf("seq done, disabled\n");
}
#endif
void swd_seq(uint32_t count, uint32_t flags, const uint8_t* swdo, uint8_t* swdi) {
static uint64_t last_bit = ~(uint64_t)0;
uint64_t devnull = 0;
if ( (flags & SWD_SEQUENCE_DIN) && !swdi) swdi = (uint8_t*)&devnull;
if (!(flags & SWD_SEQUENCE_DIN) && !swdo) swdo = (const uint8_t*)&last_bit;
if (flags & SWD_SEQUENCE_DIN) {
PIN_SWDIO_OUT_DISABLE();
} else {
PIN_SWDIO_OUT_ENABLE();
}
const uint8_t* sdo = NULL;
uint8_t* sdi = NULL;
uint32_t nreal;
for (uint32_t i = 0; i < count; i += nreal) {
uint32_t nmod = (count - i) & 63;
nreal = nmod ? nmod : 64;
if (flags & SWD_SEQUENCE_DIN) {
sdi = swdi ? &swdi[i >> 3] : ( uint8_t*)&devnull ;
sdo = NULL;
} else {
sdo = swdo ? &swdo[i >> 3] : (const uint8_t*)&last_bit;
sdi = NULL;
}
SWD_Sequence(nmod | flags, sdo, sdi);
}
if (!(flags & SWD_SEQUENCE_DIN) && swdo) {
uint8_t lastbyte = swdo[((count + 7) >> 3) - 1];
last_bit = (lastbyte & (1 << (count & 7))) ? ~(uint64_t)0 : (uint64_t)0;
}
}
void SWJ_Sequence(uint32_t count, const uint8_t* data) {
swd_seq(count, 0, data, NULL);
/*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
}*/
}
uint8_t SWD_Transfer(uint32_t request, uint32_t* data) {
uint32_t parity;
uint8_t swdo;
parity = ((request >> 0) & 1) + ((request >> 1) & 1)
+ ((request >> 2) & 1) + ((request >> 3) & 1);
swdo = 1 | ((request & 0xf) << 1) | ((parity & 1) << 5) | (0<<6) | (1<<7);
swd_seq(8, 0, &swdo, NULL);
swd_seq(DAP_Data.swd_conf.turnaround, SWD_SEQUENCE_DIN, NULL, NULL);
uint8_t ack = 0;
swd_seq(3, SWD_SEQUENCE_DIN, NULL, &ack);
uint32_t num;
switch (ack) {
case DAP_TRANSFER_OK:
if (request & DAP_TRANSFER_RnW) {
uint64_t val = 0;
parity = 0;
// FIXME: this is little-endian-only!
swd_seq(33, SWD_SEQUENCE_DIN, NULL, (uint8_t*)&val);
for (size_t i = 0; i < 32; ++i) parity += ((uint32_t)val >> i) & 1;
if ((parity & 1) != ((uint32_t)(val >> 32) & 1)) {
ack = DAP_TRANSFER_ERROR;
}
if (data) *data = (uint32_t)val;
swd_seq(DAP_Data.swd_conf.turnaround, SWD_SEQUENCE_DIN, NULL, NULL);
//PIN_SWDIO_OUT_ENABLE();
} else { // write
swd_seq(DAP_Data.swd_conf.turnaround, SWD_SEQUENCE_DIN, NULL, NULL);
//PIN_SWDIO_OUT_ENABLE();
uint32_t val = *data;
parity = 0;
for (size_t i = 0; i < 32; ++i) parity += (val >> i) & 1;
uint64_t out = val | ((uint64_t)(parity & 1) << 32);
// FIXME: this is little-endian-only!
swd_seq(33, 0, (const uint8_t*)&out, NULL);
}
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) {
uint64_t swdio = 0;
uint32_t cyc = num - i;
if (cyc > 64) cyc = 64;
SWD_Sequence((cyc & SWD_SEQUENCE_CLK), (const uint8_t*)&swdio, NULL);
}
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
}
swd_seq(num, SWD_SEQUENCE_DIN, NULL, NULL);
if (DAP_Data.swd_conf.data_phase && !(request & DAP_TRANSFER_RnW)) {
uint64_t swdio = 0;
swd_seq(33, 0, (const uint8_t*)&swdio, NULL); // 32 data bits + parity
}
break;
default: // protocol error
swd_seq(DAP_Data.swd_conf.turnaround + 33, SWD_SEQUENCE_DIN, NULL, NULL);
break;
}
PIN_SWDIO_OUT_ENABLE();
PIN_SWDIO_SET_PIO();
return ack;
}
//#endif
Reference in New Issue View Git Blame Copy Permalink