mode 3 (jtag etc pinout scan): firmware side of things

CMakeLists.txt

@@ -74,12 +74,15 @@ target_sources(${PROJECT} PUBLIC
   ${CMAKE_CURRENT_SOURCE_DIR}/src/m_default/cdc_serprog.c
   ${CMAKE_CURRENT_SOURCE_DIR}/src/m_default/tempsensor.c
   ${CMAKE_CURRENT_SOURCE_DIR}/src/m_default/vnd_i2ctinyusb.c
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/m_jscan/_jscan.c
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/m_jscan/jscan.c
   ${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/i2c_tinyusb.c
   ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/spi_serprog.c
   ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/tempsensor.c
+  ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_jscan/jscan_hw.c
   ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_sump/sump_hw.c
 )
 if(USE_USBCDC_FOR_STDIO)

bsp/rp2040/m_jscan/bsp-feature.h

@@ -0,0 +1,29 @@
+
+#ifndef BSP_FEATURE_M_JSCAN_H_
+#define BSP_FEATURE_M_JSCAN_H_
+
+#define DBOARD_HAS_JSCAN
+
+#include "bsp-info.h"
+
+// not all that much here
+
+enum {
+    HID_N__NITF = 0
+};
+enum {
+#ifdef USE_USBCDC_FOR_STDIO
+    CDC_N_STDIO,
+#endif
+
+    CDC_N__NITF
+};
+enum {
+#if CFG_TUD_VENDOR > 0
+    VND_N_CFG = 0,
+#endif
+
+    VND_N__NITF
+};
+
+#endif

bsp/rp2040/m_jscan/jscan_hw.c

@@ -0,0 +1,18 @@
+
+#include <hardware/gpio.h>
+
+#include "m_jscan/jscan.h"
+#include "m_jscan/jscan_hw.h"
+
+void jscan_pin_disable(void) {
+    uint32_t mask = (1 << JSCAN_PIN_MAX) - 1;
+    if (JSCAN_PIN_MIN)
+        mask ^= (1 << (JSCAN_PIN_MIN - 1)) - 1;
+
+    for (uint8_t i = JSCAN_PIN_MIN; i <= JSCAN_PIN_MAX; ++i)
+        gpio_disable_pulls(i);
+
+    gpio_set_dir_masked(mask, 0); // all inputs
+}
+
+

bsp/rp2040/m_jscan/jscan_hw.h

@@ -0,0 +1,24 @@
+
+#ifndef BSP_RP2040_JSCAN_HW_H_
+#define BSP_RP2040_JSCAN_HW_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <hardware/gpio.h>
+#include <pico/time.h>
+
+// inclusive
+#define JSCAN_PIN_MIN 2
+#define JSCAN_PIN_MAX 22
+
+inline static void jscan_delay_half_clk(void) { sleep_us(25); }
+
+inline static void jscan_pin_mode(uint8_t pin, int mode) {
+    gpio_set_dir(pin, mode == 1);
+    if (mode == 0) gpio_pull_up(pin);
+}
+inline static bool jscan_pin_get(uint8_t pin) { return gpio_get(pin); }
+inline static void jscan_pin_set(uint8_t pin, bool v) { gpio_put(pin, v); }
+
+#endif

host/dmctl/protocol.py

@@ -12,6 +12,7 @@ STAT_ILLCMD     = 0x01
 STAT_BADMODE    = 0x02
 STAT_NOSUCHMODE = 0x03
 STAT_BADARG     = 0x04
+STAT_ILLSTATE   = 0x05
 
 
 def check_statpl(stat, pl, defmsg, minl=None, maxl=None):
@@ -21,6 +22,7 @@ def check_statpl(stat, pl, defmsg, minl=None, maxl=None):
         STAT_BADMODE: "Bad mode for this command",
         STAT_NOSUCHMODE: "No such mode exists or is available",
         STAT_BADARG: "Bad argument"
+        STAT_ILLSTATE: "Wrong state for command"
     }
 
     if stat != STAT_OK:

host/modules/dmj.h

@@ -9,6 +9,7 @@
 #define DMJ_RESP_STAT_BADMODE     0x02
 #define DMJ_RESP_STAT_NOSUCHMODE  0x03
 #define DMJ_RESP_STAT_BADARG      0x04
+#define DMJ_RESP_STAT_ILLSTATE    0x05
 
 #define DMJ_CMD_CFG_GET_VERSION   0x00
 #define DMJ_CMD_CFG_GET_MODES     0x01
@@ -41,6 +42,7 @@ inline static const char *dmj_get_protoerr(int err)
 	case DMJ_RESP_STAT_BADMODE: return "bad mode";
 	case DMJ_RESP_STAT_NOSUCHMODE: return "no such mode available";
 	case DMJ_RESP_STAT_BADARG: return "illegal argument";
+	case DMJ_RESP_STAT_ILLSTATE: return "wrong state for command";
 	default: return "???";
 	}
 }

src/m_jscan/_jscan.c

@@ -0,0 +1,202 @@
+// vim: set et:
+
+#include <tusb.h>
+
+#include "mode.h"
+#include "thread.h"
+#include "vnd_cfg.h"
+
+#include "m_jscan/bsp-feature.h"
+
+#include "m_jscan/jscan.h"
+#include "m_jscan/jscan_hw.h"
+
+enum m_jscan_cmds {
+    mjscan_cmd_getstat = mode_cmd__specific,
+    mjscan_cmd_getres,
+    mjscan_cmd_start,
+    mjscan_cmd_getpins,
+    mjscan_cmd_stop
+};
+//typedef enum jscan_types m_jscan_features;
+
+#ifdef DBOARD_HAS_JSCAN
+static cothread_t jscanthread;
+static uint8_t    jscanstack[THREAD_STACK_SIZE];
+
+static void jscan_thread_fn(void) {
+    jscan_init();
+    thread_yield();
+    while (1) {
+        jscan_task();
+        thread_yield();
+    }
+}
+#endif
+
+static void enter_cb(void) {
+#ifdef USE_USBCDC_FOR_STDIO
+    stdio_usb_set_itf_num(CDC_N_STDIO);
+#endif
+    vnd_cfg_set_itf_num(VND_N_CFG);
+
+#ifdef DBOARD_HAS_JSCAN
+    jscanthread = co_derive(jscanstack, sizeof jscanstack, jscan_thread_fn);
+    thread_enter(jscanthread);
+#endif
+}
+static void leave_cb(void) {
+#ifdef DBOARD_HAS_JSCAN
+    jscan_stop_force();
+    jscan_deinit();
+    jscan_stop_force();
+#endif
+}
+
+static void task_cb(void) {
+#ifdef DBOARD_HAS_JSCAN
+    tud_task();
+    thread_enter(jscanthread);
+#endif
+}
+
+static void handle_cmd_cb(uint8_t cmd) {
+    uint8_t resp = 0;
+    static uint8_t resb[JSCAN_MAX_RESULT_BYTES];
+
+    switch (cmd) {
+    case mode_cmd_get_features:
+#ifdef DBOARD_HAS_JSCAN
+        resp |= JSCAN_TYPES_SUPPORTED;
+#endif
+        vnd_cfg_write_resp(cfg_resp_ok, 1, &resp);
+        break;
+    case mjscan_cmd_getstat:
+        resp = jscan_get_status();
+        vnd_cfg_write_resp(cfg_resp_ok, 1, &resp);
+        break;
+    case mjscan_cmd_getres:
+        if (jscan_get_status() & jscan_mode_done_f) {
+            size_t rv = jscan_get_result_size();
+            jscan_copy_result(resb);
+            vnd_cfg_write_resp(cfg_resp_ok, rv, resb);
+        } else {
+            vnd_cfg_write_str(cfg_resp_illstate, "Cannot get scan result when scan has not finished");
+        }
+        break;
+    case mjscan_cmd_start: {
+            uint8_t type = vnd_cfg_read_byte();
+            uint8_t start = vnd_cfg_read_byte();
+            uint8_t end = vnd_cfg_read_byte();
+
+            if (start > end || start < JSCAN_PIN_MIN || end > JSCAN_PIN_MAX) {
+                vnd_cfg_write_str(cfg_resp_badarg, "Start and end pins out of range");
+            } else if (((1 << type) & JSCAN_TYPES_SUPPORTED) == 0) {
+                vnd_cfg_write_strf(cfg_resp_badarg, "Type '%hhu' not supported", type);
+            } else if (jscan_get_status() < jscan_mode_idle) {
+                vnd_cfg_write_str(cfg_resp_illstate, "Scan already ongoing, cannot start a new one now");
+            } else {
+                jscan_start(type, start, end);
+                vnd_cfg_write_resp(cfg_resp_ok, 0, NULL);
+            }
+        } break;
+    case mjscan_cmd_getpins: {
+        uint8_t ret[2];
+        ret[0] = JSCAN_PIN_MIN;
+        ret[1] = JSCAN_PIN_MAX;
+        vnd_cfg_write_resp(cfg_resp_ok, 2, ret);
+        } break;
+    case mjscan_cmd_stop:
+        jscan_stop_force();
+        vnd_cfg_write_resp(cfg_resp_ok, 0, NULL);
+        break;
+    default:
+        vnd_cfg_write_strf(cfg_resp_illcmd, "unknown mode3 command %02x", cmd);
+        break;
+    }
+}
+
+enum {
+    STRID_LANGID = 0,
+    STRID_MANUFACTURER,
+    STRID_PRODUCT,
+    STRID_SERIAL,
+
+    STRID_CONFIG,
+
+    STRID_IF_VND_CFG,
+    STRID_IF_CDC_SUMP,
+    STRID_IF_CDC_STDIO,
+};
+enum {
+#if CFG_TUD_VENDOR > 0
+    ITF_NUM_VND_CFG,
+#endif
+#ifdef USE_USBCDC_FOR_STDIO
+    ITF_NUM_CDC_STDIO_COM,
+    ITF_NUM_CDC_STDIO_DATA,
+#endif
+
+    ITF_NUM__TOTAL
+};
+enum {
+    CONFIG_TOTAL_LEN
+        = TUD_CONFIG_DESC_LEN
+#if CFG_TUD_VENDOR > 0
+        + TUD_VENDOR_DESC_LEN
+#endif
+#ifdef USE_USBCDC_FOR_STDIO
+        + TUD_CDC_DESC_LEN
+#endif
+};
+
+#define EPNUM_VND_CFG_OUT       0x01
+#define EPNUM_VND_CFG_IN        0x81
+#define EPNUM_CDC_STDIO_OUT     0x02
+#define EPNUM_CDC_STDIO_IN      0x82
+#define EPNUM_CDC_STDIO_NOTIF   0x83
+
+// clang-format off
+static const uint8_t desc_configuration[] = {
+    TUD_CONFIG_DESCRIPTOR(1, ITF_NUM__TOTAL, STRID_CONFIG, CONFIG_TOTAL_LEN,
+        TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP, 100),
+
+#if CFG_TUD_VENDOR > 0
+    TUD_VENDOR_DESCRIPTOR_EX(ITF_NUM_VND_CFG, STRID_IF_VND_CFG, EPNUM_VND_CFG_OUT,
+        EPNUM_VND_CFG_IN, CFG_TUD_VENDOR_RX_BUFSIZE, VND_CFG_SUBCLASS, VND_CFG_PROTOCOL),
+#endif
+
+#ifdef USE_USBCDC_FOR_STDIO
+    TUD_CDC_DESCRIPTOR(ITF_NUM_CDC_STDIO_COM, STRID_IF_CDC_STDIO, EPNUM_CDC_STDIO_NOTIF,
+        CFG_TUD_CDC_RX_BUFSIZE, EPNUM_CDC_STDIO_OUT, EPNUM_CDC_STDIO_IN, CFG_TUD_CDC_RX_BUFSIZE),
+#endif
+};
+static const char* string_desc_arr[] = {
+    NULL,
+
+    [STRID_CONFIG]            = "Configuration descriptor",
+    // max string length check:  |||||||||||||||||||||||||||||||
+    [STRID_IF_VND_CFG  ]      = "Device cfg/ctl interface",
+#ifdef USE_USBCDC_FOR_STDIO
+    [STRID_IF_CDC_STDIO]      = "stdio CDC interface (debug)",
+#endif
+};
+// clang-format on
+
+extern struct mode m_03_jscan;
+// clang-format off
+struct mode m_03_jscan = {
+    .name = "JTAG (etc) pinout scanner",
+    .version = 0x0010,
+    .n_string_desc = sizeof(string_desc_arr)/sizeof(string_desc_arr[0]),
+
+    .usb_desc = desc_configuration,
+    .string_desc = string_desc_arr,
+
+    .enter = enter_cb,
+    .leave = leave_cb,
+    .task  = task_cb,
+    .handle_cmd = handle_cmd_cb,
+};
+// clang-format on
+

src/m_jscan/jscan.c

@@ -0,0 +1,368 @@
+// vim: set et:
+
+#include <string.h>
+
+#include "thread.h"
+
+#include "m_jscan/jscan.h"
+#include "m_jscan/jscan_hw.h"
+
+struct match_rec_jtag {
+    uint8_t tck, tms, tdi, tdo, ntrst;
+    uint8_t irlen, ntoggle, short_warn;
+};
+struct match_rec_swd {
+    uint8_t swclk, swdio;
+    uint16_t manuf, part;
+};
+
+#define N_MATCHES_JTAG (JSCAN_MAX_RESULT_BYTES / (sizeof(struct match_rec_jtag)))
+#define N_MATCHES_SWD  (JSCAN_MAX_RESULT_BYTES / (sizeof(struct match_rec_swd )))
+static union {
+    struct match_rec_jtag j[N_MATCHES_JTAG];
+    struct match_rec_swd  s[N_MATCHES_SWD ];
+} matches;
+static size_t nmatches = 0;
+static uint8_t status = jscan_mode_idle;
+static uint8_t startpin = 0xff, endpin = 0xff;
+static enum jscan_types type = 0xff;
+
+uint8_t jscan_get_status(void) { return status; }
+size_t jscan_get_result_size(void) {
+    switch (type) {
+    case jscan_type_jtag:
+        return sizeof(struct match_rec_jtag) * nmatches;
+    case jscan_type_swd:
+        return sizeof(struct match_rec_swd ) * nmatches;
+    default: return 0;
+    }
+}
+void jscan_copy_result(uint8_t* dest) {
+    memcpy(dest, &matches, jscan_get_result_size());
+}
+
+void jscan_init(void) {
+    status = jscan_mode_idle;
+    startpin = endpin = 0xff;
+    type = 0xff;
+    nmatches = 0;
+
+    memset(&matches, 0, sizeof matches);
+}
+void jscan_deinit(void) {
+    jscan_stop_force();
+
+    //status = jscan_mode_idle;
+    startpin = endpin = 0xff;
+    type = 0xff;
+    nmatches = 0;
+
+    memset(&matches, 0, sizeof matches);
+}
+void jscan_stop_force(void) {
+    status = jscan_mode_idle;
+    jscan_pin_disable();
+}
+
+void jscan_start(uint8_t type_, uint8_t startpin_, uint8_t endpin_) {
+    status = type = type_;
+    startpin = startpin_;
+    endpin = endpin_;
+    nmatches = 0;
+
+    memset(&matches, 0, sizeof matches);
+}
+
+
+// the above was all boilerplate. time for the fun stuff. /////////////////////
+
+static void scan_jtag(void);
+static void scan_swd(void);
+
+void jscan_task(void) {
+    switch (status) { // should we start something?
+    case jscan_type_jtag:
+        scan_jtag();
+        if (type != 0xff) // -1 means force-stopped
+            status = jscan_mode_done_f | nmatches;
+        jscan_pin_disable();
+        break;
+    case jscan_type_swd:
+        scan_swd();
+        if (type != 0xff) // -1 means force-stopped
+            status = jscan_mode_done_f | nmatches;
+        jscan_pin_disable();
+        break;
+    }
+}
+
+#define YIELD_AND_CHECK_IF_STOPPED() \
+    do { \
+        thread_yield(); \
+        if (status == 0xff) return; \
+    } while (0) \
+
+
+/// JTAG TIME /////////////////////////////////////////////////////////////////
+
+// TODO: generate randomly? also use long ints instead of strings
+#define PATTERN_MATCH_LEN 64
+#define PATTERN_CMP_LEN 34
+static const char PATTERN[PATTERN_MATCH_LEN] = "0110011101001101101000010111001001";
+//static const uint64_t PATTERN = 0xf6c27bd50cec641eULL; // 0b1111011011000010011110111101010100001100111011000110010000011110
+/*#define PATTERN_MATCH_LEN 64
+#define PATTERN_CMP_LEN 32
+static const uint32_t PATTERN = 0x7bd50cec; // 01111011110101010000110011101100*/
+
+#define TAP_SHIFTIR 0x3ec
+#define TAP_SHIFTIR_LEN 10
+
+static void init_pins(uint8_t tck, uint8_t tms, uint8_t tdi, uint8_t ntrst) {
+    for (int i = startpin; i <= endpin; ++i) {
+        jscan_pin_mode(i, 0);
+    }
+
+    if (tck != 0xff) jscan_pin_mode(tck, 1);
+    if (tms != 0xff) jscan_pin_mode(tms, 1);
+    if (tdi != 0xff) jscan_pin_mode(tdi, 1);
+    if (ntrst != 0xff) {
+        jscan_pin_mode(ntrst, 1);
+        jscan_pin_set(ntrst, 1);
+    }
+}
+
+static void tap_state(uint32_t state, size_t tslen, uint8_t tck, uint8_t tms) {
+    for (size_t i = 0; i < tslen; ++i) {
+        jscan_delay_half_clk();
+        jscan_pin_set(tck, 0);
+        jscan_pin_set(tms, (state >> i) & 1);
+        jscan_delay_half_clk();
+        jscan_pin_set(tck, 1);
+    }
+}
+
+static void pulse_tdi(int tck, int tdi, int s_tdi) {
+    if (tck != 0xff) {
+        jscan_delay_half_clk();
+        jscan_pin_set(tck, 0);
+    }
+    jscan_pin_set(tdi, s_tdi);
+    if (tck != 0xff) {
+        jscan_delay_half_clk();
+        jscan_pin_set(tck, 1);
+    }
+}
+
+static size_t check_data(/*uint64_t*/const char* pattern, size_t iterations, uint8_t tck, uint8_t tdi, uint8_t tdo, size_t* reg_len) {
+    size_t w = 0;
+    //size_t plen = PATTERN_CMP_LEN;//strlen(pattern);
+    char tdo_read, tdo_prev;
+    size_t nr_toggle = 0;
+    //uint64_t rcv = 0;
+    char rcv[PATTERN_MATCH_LEN];
+
+    tdo_prev = '0' + (jscan_pin_get(tdo) ? 1 : 0);
+
+    for (size_t i = 0; i < iterations; ++i) {
+        pulse_tdi(tck, tdi, /*(pattern >> w) & 1*/pattern[w] - '0');
+
+        ++w;
+        if (w == PATTERN_CMP_LEN) w = 0;
+
+        tdo_read = '0' + (jscan_pin_get(tdo) ? 1 : 0);
+
+        if (tdo_read != tdo_prev) ++nr_toggle;
+        tdo_prev = tdo_read;
+
+        if (i < PATTERN_CMP_LEN) rcv[i] = tdo_read;
+        else {
+            memmove(rcv, rcv + 1, PATTERN_CMP_LEN - 1);
+            rcv[PATTERN_CMP_LEN - 1] = tdo_read;
+        }
+        //rcv = (rcv >> 1) | ((uint64_t)tdo_read << (PATTERN_MATCH_LEN-1))//(rcv << 1) | tdo_read;
+
+        if (i >= PATTERN_CMP_LEN - 1) {
+            //if (pattern == ((rcv >> (PATTERN_MATCH_LEN - PATTERN_CMP_LEN)) & (1uLL << PATTERN_CMP_LEN) - 1))
+            if (!memcmp(pattern, rcv, PATTERN_CMP_LEN))
+            {
+                *reg_len = i + 1 - PATTERN_CMP_LEN;
+                return 1;
+            }
+        }
+    }
+
+    *reg_len = 0;
+    return (nr_toggle > 1) ? nr_toggle : 0;
+}
+
+static void scan_jtag(void) {
+    for (uint8_t ntrst = startpin; ntrst <= endpin; ++ntrst) {
+        for (uint8_t tck = startpin; tck <= endpin; ++tck) {
+            if (tck == ntrst) continue;
+
+            for (uint8_t tms = startpin; tms <= endpin; ++tms) {
+                if (tms == ntrst) continue;
+                if (tms == tck) continue;
+
+                for (uint8_t tdo = startpin; tdo <= endpin; ++tdo) {
+                    if (tdo == ntrst) continue;
+                    if (tdo == tck) continue;
+                    if (tdo == tms) continue;
+
+                    for (uint8_t tdi = startpin; tdi <= endpin; ++tdi) {
+                        if (tdi == ntrst) continue;
+                        if (tdi == tck) continue;
+                        if (tdi == tms) continue;
+                        if (tdi == tdo) continue;
+
+                        init_pins(tck, tms, tdi, ntrst);
+                        tap_state(TAP_SHIFTIR, TAP_SHIFTIR_LEN, tck, tms);
+                        size_t reg_len;
+                        size_t ret = check_data(PATTERN, 2*PATTERN_MATCH_LEN, tck, tdi, tdo, &reg_len);
+                        if (ret == 0) {
+                            YIELD_AND_CHECK_IF_STOPPED();
+                            continue;
+                        }
+
+                        // do loopback check to filter out shorts
+                        init_pins(0xff, 0xff, tdi, 0xff);
+                        size_t reg_len2;
+                        size_t ret2 = check_data(PATTERN, 2*PATTERN_MATCH_LEN, 0xff, tdi, tdo, &reg_len2);
+                        (void)ret2;
+
+                        if (nmatches < N_MATCHES_JTAG) {
+                            //memset(&matches.j[nmatches], 0, sizeof(struct match_rec_jtag));
+                            matches.j[nmatches].tck = tck;
+                            matches.j[nmatches].tms = tms;
+                            matches.j[nmatches].tdo = tdo;
+                            matches.j[nmatches].tdi = tdi;
+                            matches.j[nmatches].ntrst = ntrst;
+                            matches.j[nmatches].short_warn = reg_len2; // should be zero when not clocking
+
+                            if (ret == 1) {
+                                matches.j[nmatches].irlen = reg_len;
+                            } else if (ret > 1) {
+                                matches.j[nmatches].ntoggle = ret;
+                            }
+
+                            ++nmatches;
+                        }
+
+                        YIELD_AND_CHECK_IF_STOPPED();
+                    }
+                }
+            }
+
+            YIELD_AND_CHECK_IF_STOPPED();
+        }
+    }
+}
+
+/// SWD TIME //////////////////////////////////////////////////////////////////
+
+#define RESET_SEQUENCE_LENGTH 64
+#define JTAG_TO_SWD 0xe79e
+
+static void pulse_clk(uint8_t swclk) {
+    jscan_pin_set(swclk, 0);
+    jscan_delay_half_clk();
+    jscan_pin_set(swclk, 1);
+    jscan_delay_half_clk();
+}
+
+static void reset_line(uint8_t swclk, uint8_t swdio) {
+    jscan_pin_set(swdio, 1);
+    for (int i = 0; i < RESET_SEQUENCE_LENGTH; ++i) {
+        pulse_clk(swclk);
+    }
+}
+
+static void write_bits(uint8_t swclk, uint8_t swdio, uint64_t val, int len) {
+    for (int i = 0; i < len; ++i) {
+        jscan_pin_set(swdio, (val >> i) & 1);
+        pulse_clk(swclk);
+    }
+}
+
+static void read_bits(uint8_t swclk, uint8_t swdio, uint64_t* val, int len) {
+    jscan_pin_mode(swdio, 0); //setup_m_read();
+
+    for (int i = 0; i < len; ++i) {
+        int bit = jscan_pin_get(swdio) ? 1 : 0;
+        *val = (*val & ~(1 << i)) | (bit << i);
+        pulse_clk(swclk);
+    }
+
+    jscan_pin_mode(swdio, 1); //setup_m_write();
+}
+
+inline static uint32_t get_ack(uint32_t val) { return val & 0x7; }
+inline static uint32_t get_manuf(uint32_t val) { return (val >> 4) & 0x7ff; }
+inline static uint32_t get_partno(uint32_t val) { return (val >> 15) & 0xffff; }
+
+static void turn_around(uint8_t swclk, uint8_t swdio) {
+    jscan_pin_mode(swdio, 0); //setup_m_read();
+    pulse_clk(swclk);
+}
+
+static void switch_jtag_to_swd(uint8_t swclk, uint8_t swdio) {
+    reset_line(swclk, swdio);
+    write_bits(swclk, swdio, JTAG_TO_SWD, 16);
+    reset_line(swclk, swdio);
+    write_bits(swclk, swdio, 0x00, 4);
+}
+
+static void read_id_code(uint8_t swclk, uint8_t swdio, uint64_t* buf) {
+    write_bits(swclk, swdio, 0xa5, 8);
+    turn_around(swclk, swdio);
+    read_bits(swclk, swdio, buf, 36);
+    turn_around(swclk, swdio);
+    jscan_pin_mode(swdio, 1); //setup_m_write();
+    write_bits(swclk, swdio, 0x00, 8);
+}
+
+static bool test_swd_lines(uint8_t swclk, uint8_t swdio, uint16_t* manuf, uint16_t* part) {
+    uint64_t readbuf = 0;
+    //set_pins(swclk, swdio); // saves pins to globals
+    init_pins(swclk, swdio, 0xff, 0xff);
+    switch_jtag_to_swd(swclk, swdio);
+    read_id_code(swclk, swdio, &readbuf);
+    bool result = get_ack(readbuf) == 1;
+    init_pins(0xff, 0xff, 0xff, 0xff);
+    if (result) {
+        *manuf = get_manuf(readbuf);
+        *part  = get_partno(readbuf);
+    } else { *manuf = 0; *part = 0; }
+    return result;
+}
+
+static void scan_swd(void) {
+    init_pins(0xff, 0xff, 0xff, 0xff);
+
+    for (uint8_t swclk = startpin; swclk <= endpin; ++swclk) {
+        if (!jscan_pin_get(swclk)) continue;
+        for (uint8_t swdio = startpin; swdio <= endpin; ++swdio) {
+            if (swdio == swclk) continue;
+
+            if (!jscan_pin_get(swdio)) continue;
+
+            uint16_t manuf = 0, part = 0;
+            if (test_swd_lines(swclk, swdio, &manuf, &part)) {
+                if (nmatches < N_MATCHES_SWD) {
+                    //memset(&matches.s[nmatches], 0, sizeof(struct match_rec));
+                    matches.s[nmatches].swclk = swclk;
+                    matches.s[nmatches].swdio = swdio;
+                    matches.s[nmatches].manuf = manuf;
+                    matches.s[nmatches].part  = part ;
+
+                    ++nmatches;
+                }
+
+                YIELD_AND_CHECK_IF_STOPPED();
+            }
+        }
+
+        YIELD_AND_CHECK_IF_STOPPED();
+    }
+}
+

src/m_jscan/jscan.h

@@ -0,0 +1,54 @@
+
+#ifndef JSCAN_H_
+#define JSCAN_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#define JSCAN_MAX_RESULT_BYTES 512
+
+#define JSCAN_FREQ 20000 /* 20 kHz */
+
+enum jscan_types {
+    jscan_type_jtag = 0,
+    jscan_type_swd  = 1,
+    jscan_type_sbw  = 2,
+    // others?
+
+    jscan_type__count
+};
+
+enum jscan_mode {
+    jscan_mode_busy   = 0,
+    jscan_mode_idle   = 0x7f,
+    jscan_mode_done_f = 0x80
+};
+
+#define JSCAN_TYPES_SUPPORTED ((1 << jscan_type_jtag) | (1 << jscan_type_swd))
+
+uint8_t jscan_get_status(void);
+size_t jscan_get_result_size(void);
+void jscan_copy_result(uint8_t* dest);
+
+void jscan_start(uint8_t type, uint8_t startpin, uint8_t endpin);
+void jscan_stop_force(void);
+
+void jscan_init(void);
+void jscan_deinit(void);
+void jscan_task(void);
+
+// hardware functions
+
+// mode: 0: input, pullup. 1: output
+/*void jscan_pin_mode(uint8_t pin, int mode);
+bool jscan_pin_get(uint8_t pin);
+void jscan_pin_set(uint8_t pin, bool v);*/
+// implement these inline in jscan_hw.h
+void jscan_pin_disable(void);
+
+// sleep for 25 microseconds (half a clock cycle of a 20 kHz clock)
+//void jscan_delay_half_clk(void);
+// implement this inline in jscan_hw.h
+
+#endif
+

src/modeset.c

@@ -5,14 +5,14 @@
 #include "alloc.h"
 #include "mode.h"
 
-extern struct mode m_01_default, m_04_sump;
+extern struct mode m_01_default, m_03_jscan, m_04_sump;
 
 // clang-format off
 const struct mode* const mode_list[16] = {
     NULL, // dummy 0 entry
     &m_01_default, // entry 1 CANNOT be NULL!
-    NULL, // mode 2 (jtag scan) not implemented yet
+    NULL, // mode 2 (hw chip programming stuff) not implemented yet
-    NULL, // mode 3 (hw chip programming stuff) not implemented yet
+    &m_03_jscan,
     &m_04_sump,
     NULL, // terminating entry
 };

src/vnd_cfg.h

@@ -48,7 +48,8 @@ enum cfg_resp {
     cfg_resp_illcmd     = 0x01,
     cfg_resp_badmode    = 0x02,
     cfg_resp_nosuchmode = 0x03,
-    cfg_resp_badarg     = 0x04
+    cfg_resp_badarg     = 0x04,
+    cfg_resp_illstate   = 0x05
 };
 
 void    vnd_cfg_set_itf_num(int itf);