From f96243eb804a674122a5010eeaa47cf1e2bff2c5 Mon Sep 17 00:00:00 2001
From: sys64738 <sys64738@hellsite.site>
Date: Mon, 27 Sep 2021 16:29:45 +0200
Subject: [PATCH] preinit (SBW enable handshake)

---
 src/main.c    |  3 +++
 src/pio_sbw.c | 30 ++++++++++++++++++++++++++++++
 src/pio_sbw.h |  3 +++
 3 files changed, 36 insertions(+)

diff --git a/src/main.c b/src/main.c
index 670254c..c57622e 100644
--- a/src/main.c
+++ b/src/main.c
@@ -24,6 +24,9 @@ int main() {
 
     stdio_init_all();
 
+    sbw_preinit();
+    printf("%s\n", "preinited");
+
     bool s = sbw_init();
     printf("%s\n", s ? "inited" : "failure");
 
diff --git a/src/pio_sbw.c b/src/pio_sbw.c
index b731b1e..608e855 100644
--- a/src/pio_sbw.c
+++ b/src/pio_sbw.c
@@ -9,11 +9,41 @@
 #include <hardware/gpio.h>
 #include <hardware/pio.h>
 #include <hardware/pio_instructions.h>
+#include <hardware/timer.h>
 
 #include "pio_sbw.h"
 
 #include "sbw.pio.h"
 
+void sbw_preinit(void) {
+    //SLAU320AJ 2.3.1.1 ; SLAS722G p36:
+    // TEST/SBWTCK low for >100 us: reset debug state
+    // set nRST/NMI/SBWTDIO low: avoid sending an NMI when the debugger detaches
+    // TEST/SBWTCK high for >1 us : signal that we want a debugger
+    // nRST/NMI/SBWTDIO high: we want SBW
+    // TEST low for >0.025us <7us: latch on "we want SBW" signal
+    // TEST high again for >1 us: ready to SBW
+
+    gpio_put(PINOUT_SBW_TCK , false);
+    gpio_put(PINOUT_SBW_TDIO, false);
+    gpio_set_dir(PINOUT_SBW_TCK , true);
+    gpio_set_dir(PINOUT_SBW_TDIO, true);
+    gpio_set_function(PINOUT_SBW_TCK , GPIO_FUNC_SIO);
+    gpio_set_function(PINOUT_SBW_TDIO, GPIO_FUNC_SIO);
+
+    // TCK, TDIO now low
+    busy_wait_us_32(150); // reset debug state while keeping CPU in reset
+
+    gpio_put(PINOUT_SBW_TCK , true ); // we want a debugger
+    busy_wait_us_32(1);
+    gpio_put(PINOUT_SBW_TDIO, true ); // we want SBW
+    busy_wait_us_32(1);
+    gpio_put(PINOUT_SBW_TCK , false); // latch "we want SBW"
+    busy_wait_us_32(3);
+    gpio_put(PINOUT_SBW_TCK , true ); // start SBW stuff
+    busy_wait_us_32(100); // wait a bit more
+}
+
 int sbw_piosm = -1, sbw_offset = -1;
 
 static uint last_tclk = 1;
diff --git a/src/pio_sbw.h b/src/pio_sbw.h
index 5d2ac4c..050bd83 100644
--- a/src/pio_sbw.h
+++ b/src/pio_sbw.h
@@ -16,6 +16,9 @@
 
 extern int sbw_piosm, sbw_offset;
 
+// does the debug handshake/SBW setup thingy, call before sbw_init()
+void sbw_preinit(void);
+
 bool sbw_init(void);
 void sbw_deinit(void);