commit 5464b14264032eebb4049f6399db238d1d3dadc9
Author: Brian Swetland <swetland@frotz.net>
Date: Thu, 11 Feb 2021 19:43:15 -0800
initial commit
- A quick port of the lpclink2 mdebug firmware to pico.
- Uses pio but not the second m0
- pio interface based on the implementation in picoprobe
Diffstat:
A | .gitignore | | | 1 | + |
A | CMakeLists.txt | | | 26 | ++++++++++++++++++++++++++ |
A | README | | | 33 | +++++++++++++++++++++++++++++++++ |
A | boards/pico-mdebug.h | | | 15 | +++++++++++++++ |
A | main.c | | | 111 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
A | rswdp.c | | | 263 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
A | rswdp.h | | | 124 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
A | swd-io.pio | | | 24 | ++++++++++++++++++++++++ |
A | swd-pio.c | | | 156 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
A | swd-pio.h | | | 100 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
A | swd.h | | | 44 | ++++++++++++++++++++++++++++++++++++++++++++ |
A | tusb_config.h | | | 21 | +++++++++++++++++++++ |
12 files changed, 918 insertions(+), 0 deletions(-)
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1 @@
+build/
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.13)
+
+#include(pico_sdk_import.cmake)
+include(../pico-sdk/pico_sdk_init.cmake)
+
+project(pico-mdebug)
+
+set(PICO_BOARD pico-mdebug)
+set(PICO_BOARD_HEADER_DIRS ${CMAKE_CURRENT_LIST_DIR}/boards)
+
+pico_sdk_init()
+
+add_executable(
+ pico-mdebug
+ main.c
+ swd-pio.c
+ rswdp.c
+)
+
+pico_generate_pio_header(pico-mdebug ${CMAKE_CURRENT_LIST_DIR}/swd-io.pio)
+
+target_include_directories(pico-mdebug PRIVATE .)
+
+target_link_libraries(pico-mdebug PRIVATE hardware_pio pico_stdlib tinyusb_device tinyusb_board)
+
+pico_add_extra_outputs(pico-mdebug)
diff --git a/README b/README
@@ -0,0 +1,33 @@
+mdebug firmware for rpi pico / rp2040
+-------------------------------------
+
+WARNING: WORK IN PROGRESS. DEFAULT PINOUT MAY CHANGE.
+
+ +---USB---+
+ debug-uart-tx |o :
+ debug-uart-rx |o
+ gnd |o
+swdclk-to-target |o
+ swdio-to-target |o
+ |o
+ |o
+ gnd |o
+ :
+
+building:
+ 1. checkout along-side pico-sdk
+ 2. mkdir build && cd build && make ..
+ 3. cd build && make
+
+third party code:
+ swd-io.pio and swd-pio.h based on picoprobe
+ https://github.com/raspberrypi/picoprobe
+
+mdebug cortex-m debugger available here:
+ https://github.com/swetland/mdebug/
+
+todo:
+- investigate performance issues vis lpclink2 implementation
+- support level shifters (needs custom/addon board)
+- add swo/uart support
+
diff --git a/boards/pico-mdebug.h b/boards/pico-mdebug.h
@@ -0,0 +1,15 @@
+#define PICO_DEFAULT_UART 0
+#define PICO_DEFAULT_UART_TX_PIN 0
+#define PICO_DEFAULT_UART_RX_PIN 1
+#define PICO_DEFAULT_UART_BAUD_RATE 1000000
+
+#define PICO_DEFAULT_LED_PIN 25
+
+#define PICO_FLASH_SPI_CLKDIV 2
+#define PICO_FLASH_SIZE_BYTES (2 * 1024 * 1024)
+
+#define PICO_SMPS_MODE_PIN 23
+
+#define PICO_FLOAT_SUPPORT_ROM_V1 1
+#define PICO_DOUBLE_SUPPORT_ROM_V1 1
+
diff --git a/main.c b/main.c
@@ -0,0 +1,111 @@
+// Copyright 2021, Brian Swetland <swetland@frotz.net>
+// Licensed under the Apache License, Version 2.0.
+
+#include <stdio.h>
+#include <stdint.h>
+
+#include <pico/stdlib.h>
+#include <tusb.h>
+
+void rswd_init();
+uint32_t rswd_txn(uint32_t* rx, uint32_t rxc, uint32_t* tx);
+
+uint32_t rxbuf[8192 / 4];
+uint32_t txbuf[8192 / 4];
+
+uint32_t rxcount = 0;
+
+void mdebug_task(void) {
+ if (!tud_vendor_available()) {
+ return;
+ }
+
+ unsigned count = tud_vendor_read(rxbuf + rxcount, 64);
+ if (count == 0) {
+ return;
+ }
+ //printf("USB RX %u (%u)\n", count, rxcount);
+
+ if (count & 3) {
+ // bogus packet
+ rxcount = 0;
+ return;
+ }
+
+ rxcount += (count / 4);
+
+ if (count < 64) {
+ // short packet: end of txn
+ count = rswd_txn(rxbuf, rxcount, txbuf);
+ if (count) {
+ tud_vendor_write(txbuf, count * 4);
+ }
+ rxcount = 0;
+ return;
+ }
+
+ if (rxcount == (8192 / 4)) {
+ // overflow
+ rxcount = 0;
+ return;
+ }
+}
+
+int main() {
+ board_init();
+ setup_default_uart();
+ printf("Hello, world!\n");
+ rswd_init();
+ tusb_init();
+
+ for (;;) {
+ tud_task();
+ mdebug_task();
+ }
+
+ return 0;
+}
+
+
+static const tusb_desc_device_t dev_desc = {
+ .bLength = sizeof(tusb_desc_device_t),
+ .bDescriptorType = TUSB_DESC_DEVICE,
+ .bcdUSB = 0x0110,
+ .bDeviceClass = 0x00,
+ .bDeviceSubClass = 0x00,
+ .bDeviceProtocol = 0x00,
+ .bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE,
+ .idVendor = 0x1209,
+ .idProduct = 0x5038,
+ .bcdDevice = 0x0100,
+ .iManufacturer = 0,
+ .iProduct = 0,
+ .iSerialNumber = 0,
+ .bNumConfigurations = 1,
+};
+
+const uint8_t* tud_descriptor_device_cb(void) {
+ return (const uint8_t*) &dev_desc;
+}
+
+#define IFC_MDEBUG 0
+#define IFC_TOTAL 1
+
+#define EP_MDEBUG_IN 0x81
+#define EP_MDEBUG_OUT 0x01
+
+#define CFG_LEN (TUD_CONFIG_DESC_LEN + TUD_VENDOR_DESC_LEN)
+
+static const uint8_t cfg_desc[] = {
+ TUD_CONFIG_DESCRIPTOR(1, IFC_TOTAL, 0, CFG_LEN,
+ TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP, 100),
+ TUD_VENDOR_DESCRIPTOR(IFC_MDEBUG, 0, EP_MDEBUG_OUT, EP_MDEBUG_IN, 64),
+};
+
+const uint8_t* tud_descriptor_configuration_cb(uint8_t index) {
+ return cfg_desc;
+}
+
+const uint16_t* tud_descriptor_string_cb(uint8_t index, uint16_t langid) {
+ return NULL;
+}
diff --git a/rswdp.c b/rswdp.c
@@ -0,0 +1,263 @@
+// Copyright 2011-2021, Brian Swetland <swetland@frotz.net>
+// Licensed under the Apache License, Version 2.0.
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+#include "swd.h"
+#include "rswdp.h"
+
+void usb_xmit(void *data, unsigned len);
+int usb_recv(void *data, unsigned len);
+
+unsigned swdp_trace = 0;
+
+// indicates host knows about v1.0 protocol features
+unsigned host_version = 0;
+
+static uint8_t optable[16] = {
+ [OP_RD | OP_DP | OP_X0] = RD_IDCODE,
+ [OP_RD | OP_DP | OP_X4] = RD_DPCTRL,
+ [OP_RD | OP_DP | OP_X8] = RD_RESEND,
+ [OP_RD | OP_DP | OP_XC] = RD_BUFFER,
+ [OP_WR | OP_DP | OP_X0] = WR_ABORT,
+ [OP_WR | OP_DP | OP_X4] = WR_DPCTRL,
+ [OP_WR | OP_DP | OP_X8] = WR_SELECT,
+ [OP_WR | OP_DP | OP_XC] = WR_BUFFER,
+ [OP_RD | OP_AP | OP_X0] = RD_AP0,
+ [OP_RD | OP_AP | OP_X4] = RD_AP1,
+ [OP_RD | OP_AP | OP_X8] = RD_AP2,
+ [OP_RD | OP_AP | OP_XC] = RD_AP3,
+ [OP_WR | OP_AP | OP_X0] = WR_AP0,
+ [OP_WR | OP_AP | OP_X4] = WR_AP1,
+ [OP_WR | OP_AP | OP_X8] = WR_AP2,
+ [OP_WR | OP_AP | OP_XC] = WR_AP3,
+};
+
+static const char *board_str = "rpi pico";
+static const char *build_str = "fw v1.00 (" __DATE__ ", " __TIME__ ")";
+
+#define MODE_SWD 0
+#define MODE_JTAG 1
+static unsigned mode = MODE_SWD;
+
+/* TODO bounds checking -- we trust the host far too much */
+unsigned process_txn(uint32_t txnid, uint32_t *rx, int rxc, uint32_t *tx) {
+ unsigned msg, op, n;
+ unsigned txc = 1;
+ unsigned count = 0;
+ unsigned status = 0;
+ void (*func)(void) = 0;
+
+ tx[0] = txnid;
+
+ while (rxc-- > 0) {
+ count++;
+ msg = *rx++;
+ op = RSWD_MSG_OP(msg);
+ n = RSWD_MSG_ARG(msg);
+#if CONFIG_MDEBUG_TRACE
+ printf("> %02x %02x %04x <\n", RSWD_MSG_CMD(msg), op, n);
+#endif
+ switch (RSWD_MSG_CMD(msg)) {
+ case CMD_NULL:
+ continue;
+ case CMD_SWD_WRITE:
+ while (n-- > 0) {
+ rxc--;
+ status = swd_write(optable[op], *rx++);
+ if (status) {
+ goto done;
+ }
+ }
+ continue;
+ case CMD_SWD_READ:
+ tx[txc++] = RSWD_MSG(CMD_SWD_DATA, 0, n);
+ while (n-- > 0) {
+ status = swd_read(optable[op], tx + txc);
+ if (status) {
+ txc++;
+ while (n-- > 0)
+ tx[txc++] = 0xfefefefe;
+ goto done;
+ }
+ txc++;
+ }
+ continue;
+ case CMD_SWD_DISCARD:
+ while (n-- > 0) {
+ uint32_t tmp;
+ status = swd_read(optable[op], &tmp);
+ if (status) {
+ goto done;
+ }
+ }
+ continue;
+ case CMD_ATTACH:
+ if (mode != MODE_SWD) {
+ mode = MODE_SWD;
+ swd_init();
+ }
+ swd_reset(op);
+ continue;
+#if 0
+ case CMD_JTAG_IO:
+ if (mode != MODE_JTAG) {
+ mode = MODE_JTAG;
+ jtag_init();
+ }
+ tx[txc++] = RSWD_MSG(CMD_JTAG_DATA, 0, n);
+ while (n > 0) {
+ unsigned xfer = (n > 32) ? 32 : n;
+ jtag_io(xfer, rx[0], rx[1], tx + txc);
+ rx += 2;
+ rxc -= 2;
+ txc += 1;
+ n -= xfer;
+ }
+ continue;
+ case CMD_JTAG_VRFY:
+ if (mode != MODE_JTAG) {
+ mode = MODE_JTAG;
+ jtag_init();
+ }
+ // (n/32) x 4 words: TMS, TDI, DATA, MASK
+ while (n > 0) {
+ unsigned xfer = (n > 32) ? 32 : n;
+ jtag_io(xfer, rx[0], rx[1], tx + txc);
+ if ((tx[txc] & rx[3]) != rx[2]) {
+ status = ERR_BAD_MATCH;
+ goto done;
+ }
+ rx += 4;
+ rxc -= 4;
+ n -= xfer;
+ }
+ continue;
+ case CMD_JTAG_TX: {
+ unsigned tms = (op & 1) ? 0xFFFFFFFF : 0;
+ if (mode != MODE_JTAG) {
+ mode = MODE_JTAG;
+ jtag_init();
+ }
+ while (n > 0) {
+ unsigned xfer = (n > 32) ? 32 : n;
+ jtag_io(xfer, tms, rx[0], rx);
+ rx++;
+ rxc--;
+ n -= xfer;
+ }
+ continue;
+ }
+ case CMD_JTAG_RX: {
+ unsigned tms = (op & 1) ? 0xFFFFFFFF : 0;
+ unsigned tdi = (op & 2) ? 0xFFFFFFFF : 0;
+ if (mode != MODE_JTAG) {
+ mode = MODE_JTAG;
+ jtag_init();
+ }
+ tx[txc++] = RSWD_MSG(CMD_JTAG_DATA, 0, n);
+ while (n > 0) {
+ unsigned xfer = (n > 32) ? 32 : n;
+ jtag_io(xfer, tms, tdi, tx + txc);
+ txc++;
+ n -= xfer;
+ }
+ continue;
+ }
+#endif
+ case CMD_RESET:
+ swd_hw_reset(n);
+ continue;
+ case CMD_TRACE:
+ swdp_trace = op;
+ continue;
+#if 0
+ case CMD_BOOTLOADER:
+ func = _reboot;
+ continue;
+#endif
+ case CMD_SET_CLOCK:
+ n = swd_set_clock(n);
+ if (host_version >= RSWD_VERSION_1_0) {
+ tx[txc++] = RSWD_MSG(CMD_CLOCK_KHZ, 0, n);
+ }
+ continue;
+ case CMD_SWO_CLOCK:
+ n = swo_set_clock(n);
+ if (host_version >= RSWD_VERSION_1_0) {
+ tx[txc++] = RSWD_MSG(CMD_CLOCK_KHZ, 1, n);
+ }
+ continue;
+ case CMD_VERSION:
+ host_version = n;
+ tx[txc++] = RSWD_MSG(CMD_VERSION, 0, RSWD_VERSION);
+
+ n = strlen(board_str);
+ memcpy(tx + txc + 1, board_str, n + 1);
+ n = (n + 4) / 4;
+ tx[txc++] = RSWD_MSG(CMD_BOARD_STR, 0, n);
+ txc += n;
+
+ n = strlen(build_str);
+ memcpy(tx + txc + 1, build_str, n + 1);
+ n = (n + 4) / 4;
+ tx[txc++] = RSWD_MSG(CMD_BUILD_STR, 0, n);
+ txc += n;
+
+ tx[txc++] = RSWD_MSG(CMD_RX_MAXDATA, 0, 8192);
+ txc += n;
+ continue;
+ default:
+ printf("unknown command %02x\n", RSWD_MSG_CMD(msg));
+ status = 1;
+ goto done;
+ }
+ }
+
+done:
+ tx[txc++] = RSWD_MSG(CMD_STATUS, status, count);
+
+ /* if we're about to send an even multiple of the packet size
+ * (64), add a NULL op on the end to create a short packet at
+ * the end.
+ */
+ if ((txc & 0xf) == 0)
+ tx[txc++] = RSWD_MSG(CMD_NULL, 0, 0);
+
+#if CONFIG_MDEBUG_TRACE
+ printf("[ send %d words ]\n", txc);
+ for (n = 0; n < txc; n+=4) {
+ printx("%08x %08x %08x %08x\n",
+ tx[n], tx[n+1], tx[n+2], tx[n+3]);
+ }
+#endif
+
+ return txc;
+}
+
+void rswd_init(void) {
+ printf("[ rswdp agent v0.9 ]\n");
+ printf("[ built " __DATE__ " " __TIME__ " ]\n");
+
+ swd_init();
+}
+
+uint32_t rswd_txn(uint32_t* rx, uint32_t rxc, uint32_t* tx) {
+#if CONFIG_MDEBUG_TRACE
+ printf("[ recv %d words ]\n", rxc);
+ for (unsigned n = 0; n < rxc; n += 4) {
+ printf("%08x %08x %08x %08x\n",
+ rx[n], rx[n+1], rx[n+2], rx[n+3]);
+ }
+#endif
+
+ if ((rx[0] & 0xFFFF0000) != 0xAA770000) {
+ printf("invalid frame %x\n", rx[0]);
+ return 0;
+ }
+
+ return process_txn(rx[0], rx + 1, rxc - 1, tx);
+}
+
diff --git a/rswdp.h b/rswdp.h
@@ -0,0 +1,124 @@
+// Copyright 2011-2021, Brian Swetland <swetland@frotz.net>
+// Licensed under the Apache License, Version 2.0.
+
+/* Remote Serial Wire Debug Protocol */
+
+#ifndef _RSWDP_PROTOCOL_H_
+#define _RSWDP_PROTOCOL_H_
+
+/* Basic framing:
+ * - host and device exchange "transactions" consisting of
+ * some number of "messages".
+ * - each "message" has a 32bit header and may have 0 or more
+ * 32bit words of payload
+ * - a transaction may not exceed 4K (1024 words)
+ * - a transaction is sent in a series of USB BULK packets
+ * - the final packet must be a short packet unless the
+ * transaction is exactly 4K in length
+ * - packets must be a multiple of 4 bytes
+ * - the first message in a transaction must be
+ * CMD_TXN_START or CMD_TXN_ASYNC
+ */
+
+#define RSWD_MSG(cmd,op,n) ((((cmd)&0xFF) << 24) | (((op) & 0xFF)<<16) | ((n) & 0xFFFF))
+#define RSWD_MSG_CMD(n) (((n) >> 24) & 0xFF)
+#define RSWD_MSG_OP(n) (((n) >> 16) & 0xFF)
+#define RSWD_MSG_ARG(n) ((n) & 0xFFFF)
+
+#define RSWD_TXN_START(seq) (0xAA770000 | ((seq) & 0xFFFF))
+#define RSWD_TXN_ASYNC (0xAA001111)
+
+/* valid: either */
+#define CMD_NULL 0x00 /* used for padding */
+
+/* valid: host to target */
+#define CMD_SWD_WRITE 0x01 /* op=addr arg=count payload: data x count */
+#define CMD_SWD_READ 0x02 /* op=addr arg=count payload: data x count */
+#define CMD_SWD_DISCARD 0x03 /* op=addr arg=count payload: none (discards) */
+#define CMD_ATTACH 0x04 /* do swdp reset/connect handshake */
+#define CMD_RESET 0x05 /* arg=1 -> assert RESETn, otherwise deassert */
+#define CMD_DOWNLOAD 0x06 /* arg=wordcount, payload: addr x 1, data x n */
+#define CMD_EXECUTE 0x07 /* payload: addr x 1 */
+#define CMD_TRACE 0x08 /* op=tracebits n=0 */
+#define CMD_BOOTLOADER 0x09 /* return to bootloader for reflashing */
+#define CMD_SET_CLOCK 0x0A /* set SWCLK rate to n khz */
+#define CMD_SWO_CLOCK 0x0B /* set SWOCLK rate to n khz, 0 = disable SWO */
+#define CMD_JTAG_IO 0x0C /* op=0, arg=bitcount, data x (count/32) * 2 */
+/* tms, tdi word pairs per 32bits */
+#define CMD_JTAG_TX 0x0D /* tms=op.0, arg=bitcount, data x (count/32) words of tdi */
+#define CMD_JTAG_RX 0x0E /* tms=op.0, tdi=op.1, arg=bitcount, return (count/32) words */
+#define CMD_JTAG_VRFY 0x0F /* arg=bitcount, tms/tdi data/mask, error if tdo&mask != data */
+
+/* ATTACH ops */
+#define ATTACH_SWD_RESET 0
+#define ATTACH_JTAG_TO_SWD 1
+#define ATTACH_DORMANT_TO_SWD 2
+#define ATTACH_SWD_TO_DORMANT 3
+
+/* valid: target to host */
+#define CMD_STATUS 0x10 /* op=errorcode, arg=commands since last TXN_START */
+#define CMD_SWD_DATA 0x11 /* op=0 arg=count, payload: data x count */
+#define CMD_SWO_DATA 0x12 /* op=0 arg=count, payload: ((count+3)/4) words */
+#define CMD_JTAG_DATA 0x14 /* op=0 arg=bitcount, payload: (arg/32) words */
+
+/* valid: target to host async */
+#define CMD_DEBUG_PRINT 0x20 /* arg*4 bytes of ascii debug output */
+
+/* valid: bidirectional query/config messages */
+#define CMD_VERSION 0x30 /* arg=bcdversion (0x0100 etc) */
+#define CMD_BUILD_STR 0x31 /* arg=wordcount, payload = asciiz */
+#define CMD_BOARD_STR 0x32 /* arg=wordcount, payload = asciiz */
+#define CMD_RX_MAXDATA 0x33 /* arg=bytes, declares senders rx buffer size */
+#define CMD_CLOCK_KHZ 0x34 /* arg=khz, reports active clock rate */
+
+/* CMD_STATUS error codes */
+#define ERR_NONE 0
+#define ERR_INTERNAL 1
+#define ERR_TIMEOUT 2
+#define ERR_IO 3
+#define ERR_PARITY 4
+#define ERR_BAD_MATCH 5
+
+
+#define RSWD_VERSION_1_0 0x0100
+#define RSWD_VERSION_1_1 0x0101
+#define RSWD_VERSION_1_2 0x0102
+#define RSWD_VERSION_1_3 0x0103
+
+#define RSWD_VERSION RSWD_VERSION_1_3
+
+// Pre-1.0
+// - max packet size fixed at 2048 bytes
+//
+// Version 1.0
+// - CMD_VERSION, CMD_BUILD_STR, CMD_BOARD_STR, CMD_RX_MAXDATA,
+// CMD_CLOCK_KHZ added
+//
+// Version 1.1
+// - CMD_SWO_DATA arg is now byte count, not word count
+//
+// Version 1.2
+// - CMD_JTAG_IO, CMD_JTAG_RX, CMD_JTAG_TX, CMD_JTAG_VRFY, CMD_JTAG_DATA added
+//
+// Version 1.3
+// - CMD_ATTACH ops (ATTACH_xyz), CMD_CLOCK_KHZ op indicates 0=SWCLK, 1=SWO
+
+/* CMD_SWD_OP operations - combine for direct AP/DP io */
+#define OP_RD 0x00
+#define OP_WR 0x01
+#define OP_DP 0x00
+#define OP_AP 0x02
+#define OP_X0 0x00
+#define OP_X4 0x04
+#define OP_X8 0x08
+#define OP_XC 0x0C
+
+/* DP registers */
+#define DP_IDCODE (OP_DP|OP_X0)
+#define DP_ABORT (OP_DP|OP_X0)
+#define DP_DPCTRL (OP_DP|OP_X4)
+#define DP_RESEND (OP_DP|OP_X8)
+#define DP_SELECT (OP_DP|OP_X8)
+#define DP_BUFFER (OP_DP|OP_XC)
+
+#endif
diff --git a/swd-io.pio b/swd-io.pio
@@ -0,0 +1,24 @@
+.program swdio
+.side_set 1 opt
+
+public out_negedge:
+ set pindirs, 1 side 0x0 ; Init OE clock 0
+ pull ; Pull number of bits to shift -1 from tx fifo and put into output shift register
+ mov x, osr ; mov bits to shift -1 from output shift register into x
+ pull ; Pull data to shift out
+out_negedge_bitloop:
+ out pins, 1 side 0x0 ; clock data out on falling edge
+ jmp x-- out_negedge_bitloop side 0x1 ; data is present for posedge
+ set pins, 0 side 0x0 ; Drive data low
+ push ; Push to rx fifo just so processor knows when done
+ jmp out_negedge ; Wait for next transaction
+
+public in_posedge:
+ set pindirs, 0 side 0x0 ; INIT IE clock 0
+ pull ; Pull number of bits to shift -1 from tx fifo and put into output shift register
+ mov x, osr ; mov bits to shift -1 from output shift register into x into x
+in_posedge_bitloop:
+ in pins, 1 side 0x1 ; Generate posedge and read data
+ jmp x-- in_posedge_bitloop side 0x0 ;
+ push ; Push to rx fifo when done
+ jmp in_posedge ; Jump back to start
diff --git a/swd-pio.c b/swd-pio.c
@@ -0,0 +1,156 @@
+// Copyright 2021, Brian Swetland <swetland@frotz.net>
+// Licensed under the Apache License, Version 2.0.
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+#include "swd.h"
+#include "rswdp.h"
+
+#include "swd-pio.h"
+
+/* returns 1 if the number of bits set in n is odd */
+static unsigned parity(unsigned n) {
+ n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1);
+ n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2);
+ n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4);
+ n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8);
+ n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16);
+ return n & 1;
+}
+
+void swd_init(void) {
+ swdio_init();
+}
+
+int swd_write(uint32_t hdr, uint32_t data) {
+ unsigned timeout = 64;
+ unsigned n;
+ unsigned p = parity(data);
+
+ if (hdr == WR_BUFFER) {
+ // writing to TARGETSEL is done blind
+ swdio_write_mode();
+ swdio_write_bits(hdr << 8, 16);
+
+ swdio_read_mode();
+ swdio_read_bits(5);
+
+ swdio_write_mode();
+ swdio_write_bits(data, 32);
+ swdio_write_bits(p, 1);
+ return 0;
+ }
+
+ for (;;) {
+ swdio_write_mode();
+ swdio_write_bits(hdr << 8, 16);
+
+ swdio_read_mode();
+ n = swdio_read_bits(5);
+
+ swdio_write_mode();
+
+ // check the ack bits, ignoring the surrounding turnaround bits
+ switch ((n >> 1) & 7) {
+ case 1: // OK
+ swdio_write_bits(data, 32);
+ swdio_write_bits(p, 1);
+ return 0;
+ case 2: // WAIT
+ if (--timeout == 0) {
+ return ERR_TIMEOUT;
+ }
+ continue;
+ default: // ERROR
+ return ERR_IO;
+ }
+ }
+}
+
+int swd_read(uint32_t hdr, uint32_t *val) {
+ unsigned timeout = 64;
+ unsigned n, data, p;
+
+ for (;;) {
+ swdio_write_mode();
+ swdio_write_bits(hdr << 8, 16);
+
+ swdio_read_mode();
+ n = swdio_read_bits(4);
+
+ switch (n >> 1) {
+ case 1: // OK
+ data = swdio_read_bits(32);
+ // read party + turnaround
+ p = swdio_read_bits(2) & 1;
+ if (p != parity(data)) {
+ return ERR_PARITY;
+ }
+ *val = data;
+ return 0;
+ case 2: // WAIT
+ swdio_read_bits(1);
+ if (--timeout == 0) {
+ return ERR_TIMEOUT;
+ }
+ continue;
+ default: // ERROR
+ swdio_read_bits(1);
+ return ERR_IO;
+ }
+ }
+}
+
+void swd_reset(uint32_t kind) {
+ swdio_write_mode();
+
+ switch (kind) {
+ case ATTACH_SWD_RESET:
+ // 50+ HI (60 here), 2+ LO (4 here)
+ swdio_write_bits(0xffffffff, 32);
+ swdio_write_bits(0x0fffffff, 32);
+ break;
+ case ATTACH_JTAG_TO_SWD:
+ swdio_write_bits(0xffffffff, 32);
+ swdio_write_bits(0xffffffff, 32);
+ swdio_write_bits(0b1110011110011110, 16);
+ break;
+ case ATTACH_DORMANT_TO_SWD:
+ swdio_write_bits(0xffff, 16);
+ swdio_write_bits(0x6209F392, 32);
+ swdio_write_bits(0x86852D95, 32);
+ swdio_write_bits(0xE3DDAFE9, 32);
+ swdio_write_bits(0x19BC0EA2, 32);
+ swdio_write_bits(0xF1A0, 16);
+ break;
+ case ATTACH_SWD_TO_DORMANT:
+ swdio_write_bits(0xffffffff, 32);
+ swdio_write_bits(0xffffffff, 32);
+ swdio_write_bits(0xE3BC, 16);
+ break;
+ default:
+ break;
+ }
+}
+
+unsigned swd_set_clock(unsigned khz) {
+ return swdio_set_freq(khz);
+}
+
+unsigned swo_set_clock(unsigned khz) {
+ return khz;
+}
+
+void swd_hw_reset(int assert) {
+#if 0
+ if (assert) {
+ gpio_set(GPIO_RESET, 0);
+ gpio_set(GPIO_RESET_TXEN, 1);
+ } else {
+ gpio_set(GPIO_RESET, 1);
+ gpio_set(GPIO_RESET_TXEN, 0);
+ }
+#endif
+}
diff --git a/swd-pio.h b/swd-pio.h
@@ -0,0 +1,100 @@
+/**
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+// based on picoprobe's probe.c
+
+//#include <pico/stdlib.h>
+//#include <stdio.h>
+//#include <string.h>
+
+#include <hardware/clocks.h>
+#include <hardware/gpio.h>
+
+#include "swd-io.pio.h"
+
+#define DIV_ROUND_UP(m, n) (((m) + (n) - 1) / (n))
+
+#define SWDIO_SM 0
+#define PIN_OFFSET 2
+#define PIN_SWCLK (PIN_OFFSET + 0)
+#define PIN_SWDIO (PIN_OFFSET + 1)
+
+static uint32_t swdio_offset;
+
+unsigned swdio_set_freq(unsigned freq_khz) {
+ uint clk_sys_freq_khz = clock_get_hz(clk_sys) / 1000;
+ // Worked out with saleae
+ uint32_t divider = clk_sys_freq_khz / freq_khz / 2;
+ pio_sm_set_clkdiv_int_frac(pio0, SWDIO_SM, divider, 0);
+ return clk_sys_freq_khz / divider / 2;
+}
+
+static inline void swdio_write_bits(uint32_t data, unsigned bit_count) {
+ pio_sm_put_blocking(pio0, SWDIO_SM, bit_count - 1);
+ pio_sm_put_blocking(pio0, SWDIO_SM, data);
+ // Wait for pio to push garbage to rx fifo so we know it has finished sending
+ pio_sm_get_blocking(pio0, SWDIO_SM);
+}
+
+static inline uint32_t swdio_read_bits(unsigned bit_count) {
+ pio_sm_put_blocking(pio0, SWDIO_SM, bit_count - 1);
+ uint32_t data = pio_sm_get_blocking(pio0, SWDIO_SM);
+ if (bit_count < 32) {
+ data >>= (32 - bit_count);
+ }
+ return data;
+}
+
+static void swdio_read_mode(void) {
+ pio_sm_exec(pio0, SWDIO_SM, pio_encode_jmp(swdio_offset + swdio_offset_in_posedge));
+ while(pio0->dbg_padoe & (1 << PIN_SWDIO));
+}
+
+static void swdio_write_mode(void) {
+ pio_sm_exec(pio0, SWDIO_SM, pio_encode_jmp(swdio_offset + swdio_offset_out_negedge));
+ while(!(pio0->dbg_padoe & (1 << PIN_SWDIO)));
+}
+
+void swdio_init() {
+ // Funcsel pins
+ pio_gpio_init(pio0, PIN_SWCLK);
+ pio_gpio_init(pio0, PIN_SWDIO);
+
+ // Make sure SWDIO has a pullup on it. Idle state is high
+ gpio_pull_up(PIN_SWDIO);
+
+ swdio_offset = pio_add_program(pio0, &swdio_program);
+
+ pio_sm_config sm_config = swdio_program_get_default_config(swdio_offset);
+
+ // Set SWCLK as a sideset pin
+ sm_config_set_sideset_pins(&sm_config, PIN_SWCLK);
+
+ // Set SWDIO offset
+ sm_config_set_out_pins(&sm_config, PIN_SWDIO, 1);
+ sm_config_set_set_pins(&sm_config, PIN_SWDIO, 1);
+ sm_config_set_in_pins(&sm_config, PIN_SWDIO);
+
+ // Set SWD and SWDIO pins as output to start. This will be set in the sm
+ pio_sm_set_consecutive_pindirs(pio0, SWDIO_SM, PIN_OFFSET, 2, true);
+
+ // shift output right, autopull off, autopull threshold
+ sm_config_set_out_shift(&sm_config, true, false, 0);
+ // shift input right as swd data is lsb first, autopush off
+ sm_config_set_in_shift(&sm_config, true, false, 0);
+
+ // Init SM with config
+ pio_sm_init(pio0, SWDIO_SM, swdio_offset, &sm_config);
+
+ // Set up divisor
+ swdio_set_freq(4000);
+
+ // Enable SM
+ pio_sm_set_enabled(pio0, SWDIO_SM, 1);
+
+ // Jump to write program
+ swdio_write_mode();
+}
diff --git a/swd.h b/swd.h
@@ -0,0 +1,44 @@
+// Copyright 2011-2021, Brian Swetland <swetland@frotz.net>
+// Licensed under the Apache License, Version 2.0.
+
+#ifndef _SWDP_H_
+#define _SWDP_H_
+
+void swd_init(void);
+void swd_reset(uint32_t kind);
+int swd_write(uint32_t reg, uint32_t val);
+int swd_read(uint32_t reg, uint32_t *val);
+
+unsigned swd_set_clock(unsigned khz);
+unsigned swo_set_clock(unsigned khz);
+void swd_hw_reset(int assert);
+
+void jtag_init(void);
+int jtag_io(unsigned count, unsigned tms, unsigned tdi, unsigned *tdo);
+
+// swdp_read/write() register codes
+
+// Park Stop Parity Addr3 Addr2 RnW APnDP Start
+
+#define RD_IDCODE 0b10100101
+#define RD_DPCTRL 0b10001101
+#define RD_RESEND 0b10010101
+#define RD_BUFFER 0b10111101
+
+#define WR_ABORT 0b10000001
+#define WR_DPCTRL 0b10101001
+#define WR_SELECT 0b10110001
+#define WR_BUFFER 0b10011001
+
+#define RD_AP0 0b10000111
+#define RD_AP1 0b10101111
+#define RD_AP2 0b10110111
+#define RD_AP3 0b10011111
+
+#define WR_AP0 0b10100011
+#define WR_AP1 0b10001011
+#define WR_AP2 0b10010011
+#define WR_AP3 0b10111011
+
+#endif
+
diff --git a/tusb_config.h b/tusb_config.h
@@ -0,0 +1,21 @@
+
+#ifndef _TUSB_CONFIG_H_
+#define _TUSB_CONFIG_H_
+
+#define CFG_TUSB_RHPORT0_MODE OPT_MODE_DEVICE
+#define CFG_TUSB_OS OPT_OS_PICO
+#define CFG_TUSB_MEM_SECTION
+#define CFG_TUSB_MEM_ALIGN __attribute__((aligned(4)))
+
+#define CFG_TUD_ENDPOINT0_SIZE64
+
+#define CFG_TUD_HID 0
+#define CFG_TUD_CDC 0
+#define CFG_TUD_MSC 0
+#define CFG_TUD_MIDI 0
+#define CFG_TUD_VENDOR 1
+
+#define CFG_TUD_VENDOR_RX_BUFSIZE 8192
+#define CFG_TUD_VENDOR_TX_BUFSIZE 8192
+
+#endif