From d547ee05487632d7ddcaa9dd8a64ce5992893264 Mon Sep 17 00:00:00 2001
From: lucavanstraaten <lucavanstraaten@local>
Date: Tue, 28 Apr 2026 20:56:52 +0200
Subject: [PATCH] Improve jitter: direct GPIO access and noInterrupts instead
 of digitalRead/Write

---
 src/main.cpp | 65 +++++++++++++++++++++++++++++++++-------------------
 1 file changed, 41 insertions(+), 24 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index a439d58..bf90961 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,5 +1,7 @@
 #include <Arduino.h>
 #include <Adafruit_NeoPixel.h>
+#include <hardware/sync.h>
+#include <hardware/structs/sio.h>
 
 // CLOCK module (TX)
 const uint8_t TX_DI = 0;
@@ -15,39 +17,54 @@ const uint8_t RX_RO = 29;
 const uint8_t LED_PIN = 16;
 Adafruit_NeoPixel led(1, LED_PIN, NEO_GRB + NEO_KHZ800);
 
+// Pre-computed bit masks for the SIO registers.
+// Writing to gpio_set sets a pin HIGH atomically in one cycle.
+// Writing to gpio_clr clears it LOW. Reading gpio_in gives all GPIO states.
+const uint32_t TX_DI_MASK = 1u << TX_DI;
+const uint32_t RX_RO_MASK = 1u << RX_RO;
+
 void setStatus(uint8_t r, uint8_t g, uint8_t b) {
   led.setPixelColor(0, led.Color(r, g, b));
   led.show();
 }
 
-// Read N bits from SSI encoder.
-// SSI convention: encoder latches on first falling edge,
-// shifts new bit on rising edge, master samples on falling edge.
+// SSI read with direct register I/O and interrupts disabled.
+// Single-cycle GPIO ops (~8 ns each) instead of ~1 us digitalWrite().
+// Interrupts off so USB/timer IRQs can't insert delays mid-frame.
 uint64_t ssi_read(uint8_t bits, uint16_t half_us) {
   uint64_t value = 0;
 
-  // Frame start: clock idles HIGH, drop to LOW = latch trigger
-  digitalWrite(TX_DI, LOW);
-  delayMicroseconds(half_us);
+  // Save interrupt state and disable. save_and_disable_interrupts() returns
+  // the previous state so we can restore it exactly (don't just blindly
+  // re-enable - a caller higher up the stack may have wanted them off).
+  uint32_t irq_state = save_and_disable_interrupts();
+
+  // Frame start: clock idles HIGH, drop to LOW = encoder latches position
+  sio_hw->gpio_clr = TX_DI_MASK;
+  busy_wait_us_32(half_us);
 
   for (uint8_t i = 0; i < bits; i++) {
-    digitalWrite(TX_DI, HIGH);                     // encoder shifts next bit out
-    delayMicroseconds(half_us);
-    digitalWrite(TX_DI, LOW);                      // sample on falling edge
-    value = (value << 1) | (digitalRead(RX_RO) ? 1 : 0);
-    delayMicroseconds(half_us);
+    sio_hw->gpio_set = TX_DI_MASK;                 // rising edge: encoder shifts bit
+    busy_wait_us_32(half_us);
+    sio_hw->gpio_clr = TX_DI_MASK;                 // falling edge: sample now
+    uint32_t bit = (sio_hw->gpio_in & RX_RO_MASK) ? 1 : 0;
+    value = (value << 1) | bit;
+    busy_wait_us_32(half_us);
   }
 
-  // Return to idle and wait monoflop time
-  digitalWrite(TX_DI, HIGH);
-  delayMicroseconds(30);
+  // Return clock to idle HIGH
+  sio_hw->gpio_set = TX_DI_MASK;
+
+  // Re-enable interrupts before the (long) monoflop wait - no need to
+  // hold them off any longer, the timing-critical part is done.
+  restore_interrupts(irq_state);
+
+  busy_wait_us_32(30);                             // monoflop reset time
 
   return value;
 }
 
 void handleCommand(const String& cmd) {
-  // Format: "READ <bits> <half_us>"
-  // Example: "READ 25 5"
   if (!cmd.startsWith("READ ")) {
     Serial.println("ERR unknown command. Use: READ <bits> <half_us>");
     return;
@@ -73,13 +90,13 @@ void handleCommand(const String& cmd) {
   }
 
   setStatus(0, 0, 16);                             // blue = reading
+  uint32_t t0 = micros();
   uint64_t raw = ssi_read(bits, halfUs);
+  uint32_t elapsed = micros() - t0;
   setStatus(0, 16, 0);                             // green = idle
 
-  // Print as hex and decimal
-  // (Pico printf supports %llX for 64-bit)
-  Serial.printf("OK bits=%d half_us=%d hex=0x%llX dec=%llu\n",
-                bits, halfUs, raw, raw);
+  Serial.printf("OK bits=%d half_us=%d hex=0x%llX dec=%llu took=%luus\n",
+                bits, halfUs, raw, raw, elapsed);
 }
 
 void setup() {
@@ -93,16 +110,16 @@ void setup() {
   pinMode(RX_DI, OUTPUT);
   pinMode(RX_RO, INPUT);
 
-  digitalWrite(TX_DE, HIGH);
+  digitalWrite(TX_DE, HIGH);                       // CLK module = transmit
   digitalWrite(TX_RE, HIGH);
-  digitalWrite(RX_DE, LOW);
+  digitalWrite(RX_DE, LOW);                        // DATA module = receive
   digitalWrite(RX_RE, LOW);
   digitalWrite(RX_DI, LOW);
-  digitalWrite(TX_DI, HIGH);                       // SSI idle HIGH
+  digitalWrite(TX_DI, HIGH);                       // SSI clock idles HIGH
 
   delay(200);
   while (!Serial && millis() < 3000) { delay(10); }
-  Serial.println("\nSSI bridge ready");
+  Serial.println("\nSSI bridge ready (B+C: direct registers, IRQs off during read)");
   Serial.println("Send: READ <bits> <half_us>");
   Serial.println("Example: READ 25 5");
   setStatus(0, 16, 0);