From 8737a0620258ac20fed2d0b60532fd85d6ff49eb Mon Sep 17 00:00:00 2001
From: Jonathan Bell <jonathan@raspberrypi.com>
Date: Wed, 13 Jul 2022 17:01:36 +0100
Subject: [PATCH] probe: resolve a bug in init/deinit handling

OpenOCD is fond of calling PORT_OFF and PORT_SWD_SETUP frequently, which
played havoc with caching both adapter khz and the DAP delay.

Revert to caching just the DAP delay and reset it when PORT_OFF is called.

Signed-off-by: Jonathan Bell <jonathan@raspberrypi.com>
---
 include/DAP_config.h |  5 ++++-
 src/main.c           |  1 +
 src/probe.c          | 10 ++++------
 src/probe.h          |  4 +++-
 src/sw_dp_pio.c      |  2 +-
 5 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/include/DAP_config.h b/include/DAP_config.h
index e1d91c0..e05e27b 100755
--- a/include/DAP_config.h
+++ b/include/DAP_config.h
@@ -315,8 +315,11 @@ Configures the DAP Hardware I/O pins for Serial Wire Debug (SWD) mode:
  - SWCLK, SWDIO, nRESET to output mode and set to default high level.
  - TDI, nTRST to HighZ mode (pins are unused in SWD mode).
 */
+// hack - zap our "stop doing divides everywhere" cache
+extern volatile uint32_t cached_delay;
 __STATIC_INLINE void PORT_SWD_SETUP (void) {
   probe_init();
+  cached_delay = 0;
 }
 
 /** Disable JTAG/SWD I/O Pins.
@@ -542,7 +545,7 @@ Status LEDs. In detail the operation of Hardware I/O and LED pins are enabled an
  - LED output pins are enabled and LEDs are turned off.
 */
 __STATIC_INLINE void DAP_SETUP (void) {
-  ;
+  probe_gpio_init();
 }
 
 /** Reset Target Device with custom specific I/O pin or command sequence.
diff --git a/src/main.c b/src/main.c
index d3ee842..baee8e1 100644
--- a/src/main.c
+++ b/src/main.c
@@ -87,6 +87,7 @@ int main(void) {
     cdc_uart_init();
     tusb_init();
 #if (PICOPROBE_DEBUG_PROTOCOL == PROTO_OPENOCD_CUSTOM)
+    probe_gpio_init();
     probe_init();
 #else
     DAP_Setup();
diff --git a/src/probe.c b/src/probe.c
index 6bc6d0b..719468f 100644
--- a/src/probe.c
+++ b/src/probe.c
@@ -63,8 +63,6 @@ struct _probe {
 
     // PIO offset
     uint offset;
-
-    uint cached_freq_khz;
 };
 
 static struct _probe probe;
@@ -89,14 +87,11 @@ struct __attribute__((__packed__)) probe_pkt_hdr {
 };
 
 void probe_set_swclk_freq(uint freq_khz) {
-    if (freq_khz != probe.cached_freq_khz) {
         uint clk_sys_freq_khz = clock_get_hz(clk_sys) / 1000;
         picoprobe_info("Set swclk freq %dKHz sysclk %dkHz\n", freq_khz, clk_sys_freq_khz);
         // Worked out with saleae
         uint32_t divider = clk_sys_freq_khz / freq_khz / 2;
         pio_sm_set_clkdiv_int_frac(pio0, PROBE_SM, divider, 0);
-        probe.cached_freq_khz = freq_khz;
-    }
 }
 
 void probe_assert_reset(bool state)
@@ -141,13 +136,16 @@ void probe_write_mode(void) {
     while(!(pio0->dbg_padoe & (1 << PROBE_PIN_SWDIO)));
 }
 
-void probe_init() {
+void probe_gpio_init()
+{
     // Funcsel pins
     pio_gpio_init(pio0, PROBE_PIN_SWCLK);
     pio_gpio_init(pio0, PROBE_PIN_SWDIO);
     // Make sure SWDIO has a pullup on it. Idle state is high
     gpio_pull_up(PROBE_PIN_SWDIO);
+}
 
+void probe_init() {
     // Target reset pin: pull up, input to emulate open drain pin
     gpio_pull_up(PROBE_PIN_RESET);
     // gpio_init will leave the pin cleared and set as input
diff --git a/src/probe.h b/src/probe.h
index 6818f1f..0ad8c76 100644
--- a/src/probe.h
+++ b/src/probe.h
@@ -37,6 +37,8 @@ void probe_handle_read(uint total_bits);
 void probe_handle_write(uint8_t *data, uint total_bits);
 
 void probe_task(void);
+void probe_gpio_init(void);
 void probe_init(void);
 void probe_deinit(void);
-#endif
\ No newline at end of file
+
+#endif
diff --git a/src/sw_dp_pio.c b/src/sw_dp_pio.c
index 799d898..951b47f 100755
--- a/src/sw_dp_pio.c
+++ b/src/sw_dp_pio.c
@@ -32,7 +32,7 @@
 /* Slight hack - we're not bitbashing so we need to set baudrate off the DAP's delay cycles.
  * Ideally we don't want calls to udiv everywhere... */
 #define MAKE_KHZ(x) (CPU_CLOCK / (2000 * ((x) + 1)))
-static uint32_t cached_delay;
+volatile uint32_t cached_delay = 0;
 
 // Generate SWJ Sequence
 //   count:  sequence bit count