From 02b31998d5be751bfe2a55633b5c00f3c8cfc933 Mon Sep 17 00:00:00 2001
From: Jonathan Bell <jonathan@raspberrypi.com>
Date: Mon, 4 Jul 2022 15:04:55 +0100
Subject: [PATCH] probe: fix SWDIO line idle state and export low-level
 functions

Signed-off-by: Jonathan Bell <jonathan@raspberrypi.com>
---
 src/probe.c   | 32 ++++++++++++++++++++++----------
 src/probe.h   | 12 +++++++++++-
 src/probe.pio |  2 +-
 3 files changed, 34 insertions(+), 12 deletions(-)

diff --git a/src/probe.c b/src/probe.c
index 3396796..2fe9672 100644
--- a/src/probe.c
+++ b/src/probe.c
@@ -63,6 +63,8 @@ struct _probe {
 
     // PIO offset
     uint offset;
+
+    uint cached_freq_khz;
 };
 
 static struct _probe probe;
@@ -87,20 +89,23 @@ struct __attribute__((__packed__)) probe_pkt_hdr {
 };
 
 void probe_set_swclk_freq(uint freq_khz) {
-    picoprobe_info("Set swclk freq %dKHz\n", 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, PROBE_SM, divider, 0);
+    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;
+    }
 }
 
-static inline void probe_assert_reset(bool state)
+void probe_assert_reset(bool state)
 {
     /* Change the direction to out to drive pin to 0 or to in to emulate open drain */
     gpio_set_dir(PROBE_PIN_RESET, state);
 }
 
-static inline void probe_write_bits(uint bit_count, uint8_t data_byte) {
+void probe_write_bits(uint bit_count, uint8_t data_byte) {
     DEBUG_PINS_SET(probe_timing, DBG_PIN_WRITE);
     pio_sm_put_blocking(pio0, PROBE_SM, bit_count - 1);
     pio_sm_put_blocking(pio0, PROBE_SM, data_byte);
@@ -112,7 +117,7 @@ static inline void probe_write_bits(uint bit_count, uint8_t data_byte) {
     DEBUG_PINS_CLR(probe_timing, DBG_PIN_WRITE);
 }
 
-static inline uint8_t probe_read_bits(uint bit_count) {
+uint8_t probe_read_bits(uint bit_count) {
     DEBUG_PINS_SET(probe_timing, DBG_PIN_READ);
     pio_sm_put_blocking(pio0, PROBE_SM, bit_count - 1);
     uint32_t data = pio_sm_get_blocking(pio0, PROBE_SM);
@@ -127,12 +132,12 @@ static inline uint8_t probe_read_bits(uint bit_count) {
     return data_shifted;
 }
 
-static void probe_read_mode(void) {
+void probe_read_mode(void) {
     pio_sm_exec(pio0, PROBE_SM, pio_encode_jmp(probe.offset + probe_offset_in_posedge));
     while(pio0->dbg_padoe & (1 << PROBE_PIN_SWDIO));
 }
 
-static void probe_write_mode(void) {
+void probe_write_mode(void) {
     pio_sm_exec(pio0, PROBE_SM, pio_encode_jmp(probe.offset + probe_offset_out_negedge));
     while(!(pio0->dbg_padoe & (1 << PROBE_PIN_SWDIO)));
 }
@@ -183,6 +188,13 @@ void probe_init() {
     probe_write_mode();
 }
 
+void probe_deinit(void)
+{
+  probe_read_mode();
+  pio_sm_set_enabled(pio0, PROBE_SM, 0);
+  pio_remove_program(pio0, &probe_program, probe.offset);
+}
+
 void probe_handle_read(uint total_bits) {
     picoprobe_debug("Read %d bits\n", total_bits);
     probe_read_mode();
diff --git a/src/probe.h b/src/probe.h
index 2474a38..69810db 100644
--- a/src/probe.h
+++ b/src/probe.h
@@ -26,7 +26,17 @@
 #ifndef PROBE_H_
 #define PROBE_H_
 
+void probe_set_swclk_freq(uint freq_khz);
+void probe_write_bits(uint bit_count, uint8_t data_byte);
+uint8_t probe_read_bits(uint bit_count);
+
+void probe_read_mode(void);
+void probe_write_mode(void);
+
+void probe_handle_read(uint total_bits);
+void probe_handle_write(uint8_t *data, uint total_bits);
+
 void probe_task(void);
 void probe_init(void);
-
+void probe_deinit(void);
 #endif
\ No newline at end of file
diff --git a/src/probe.pio b/src/probe.pio
index 920a3a5..4a242ba 100644
--- a/src/probe.pio
+++ b/src/probe.pio
@@ -34,7 +34,7 @@ public out_negedge:
 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
+    set pins, 1                  side 0x0 ; Drive data high (idle bus state)
     push                                 ; Push to rx fifo just so processor knows when done
     jmp out_negedge                      ; Wait for next transaction