1 files changed, 68 insertions, 0 deletions
diff --git a/hid/pico/src/ph_ps2_phy.c b/hid/pico/src/ph_ps2_phy.c
new file mode 100644
index 00000000..b5ffba75
--- /dev/null
+++ b/hid/pico/src/ph_ps2_phy.c
@@ -0,0 +1,68 @@
+#include "ph_ps2_phy.h"
+#include "ph_ps2_phy.pio.h"
+
+void ph_ps2_phy_init(ph_ps2_phy* this, PIO pio, u8 data_pin, rx_callback rx) {
+  queue_init(&this->qbytes, sizeof(u8), 9);
+  queue_init(&this->qpacks, sizeof(u8) * 9, 16);
+  
+  this->pio = pio;
+  this->sm = pio_claim_unused_sm(this->pio, true);
+  ps2phy_program_init(this->pio, this->sm, pio_add_program(this->pio, &ps2phy_program), data_pin);
+  
+  this->sent = 0;
+  this->rx = rx;
+}
+
+u16 ph_ps2_frame(u8 byte) {
+  u8 parity = 1;
+  for (u8 i = 0; i < 8; i++) {
+    parity = parity ^ (byte >> i & 1);
+  }
+  return ((1 << 10) | (parity << 9) | (byte << 1)) ^ 0x7ff;
+}
+
+void ph_ps2_phy_task(ph_ps2_phy* this) {
+  u8 i = 0;
+  u8 pack[9];
+  
+  if (!queue_is_empty(&this->qbytes)) {
+    u8 byte;
+    
+    while (i < 9 && queue_try_remove(&this->qbytes, &byte)) {
+      i++;
+      pack[i] = byte;
+    }
+    
+    pack[0] = i;
+    queue_try_add(&this->qpacks, &pack);
+  }
+  
+  if (!queue_is_empty(&this->qpacks) && pio_sm_is_tx_fifo_empty(this->pio, this->sm) && !pio_interrupt_get(this->pio, 0)) {
+    if (queue_try_peek(&this->qpacks, &pack)) {
+      if (this->sent == pack[0]) {
+        this->sent = 0;
+        queue_try_remove(&this->qpacks, &pack);
+      } else {
+        this->sent++;
+        pio_sm_put(this->pio, this->sm, ph_ps2_frame(pack[this->sent]));
+      }
+    }
+  }
+  
+  if (!pio_sm_is_rx_fifo_empty(this->pio, this->sm)) {
+    u32 fifo = pio_sm_get(this->pio, this->sm);
+    fifo = fifo >> 23;
+    
+    u8 parity = 1;
+    for (i = 0; i < 8; i++) {
+      parity = parity ^ (fifo >> i & 1);
+    }
+    
+    if (parity != fifo >> 8) {
+      //ph_ps2_kbd_send(0xfe);
+      return;
+    }
+    
+    (*this->rx)(fifo);
+  }
+}