1 files changed, 17 insertions, 13 deletions

diff --git a/hid/pico/src/ph_ps2_phy.c b/hid/pico/src/ph_ps2_phy.c
index 05046b2b..edaa165c 100644
--- a/hid/pico/src/ph_ps2_phy.c
+++ b/hid/pico/src/ph_ps2_phy.c
@@ -4,7 +4,7 @@
 #include "ph_ps2_phy.h"
 #include "ph_ps2_phy.pio.h"
 
-uint prog = 0;
+s8 prog = -1;
 
 u32 ph_ps2_phy_frame(u8 byte) {
   bool parity = 1;
@@ -15,7 +15,7 @@ u32 ph_ps2_phy_frame(u8 byte) {
 }
 
 void ph_ps2_phy_init(ph_ps2_phy* this, PIO pio, u8 data_pin, rx_callback rx) {
-  if(!prog) {
+  if (prog == -1) {
     prog = pio_add_program(pio, &ph_ps2_phy_program);
   }
   
@@ -30,7 +30,7 @@ void ph_ps2_phy_init(ph_ps2_phy* this, PIO pio, u8 data_pin, rx_callback rx) {
   this->rx = rx;
   this->last_rx = 0;
   this->last_tx = 0;
-  this->idle = true;
+  this->busy = 0;
 }
 
 void ph_ps2_phy_task(ph_ps2_phy* this) {
@@ -48,9 +48,18 @@ void ph_ps2_phy_task(ph_ps2_phy* this) {
     queue_try_add(&this->qpacks, &pack);
   }
   
-  this->idle = !pio_interrupt_get(this->pio, this->sm);
+  if (pio_interrupt_get(this->pio, this->sm)) {
+    this->busy = 1;
+  } else {
+    this->busy &= 2;
+  }
+  
+  if (pio_interrupt_get(this->pio, this->sm + 4)) {
+    this->sent--;
+    pio_interrupt_clear(this->pio, this->sm + 4);
+  }
   
-  if (!queue_is_empty(&this->qpacks) && pio_sm_is_tx_fifo_empty(this->pio, this->sm) && this->idle) {
+  if (!queue_is_empty(&this->qpacks) && pio_sm_is_tx_fifo_empty(this->pio, this->sm) && !this->busy) {
     if (queue_try_peek(&this->qpacks, &pack)) {
       if (this->sent == pack[0]) {
         this->sent = 0;
@@ -58,17 +67,12 @@ void ph_ps2_phy_task(ph_ps2_phy* this) {
       } else {
         this->sent++;
         this->last_tx = pack[this->sent];
+        this->busy |= 2;
         pio_sm_put(this->pio, this->sm, ph_ps2_phy_frame(this->last_tx));
       }
     }
   }
   
-  if (pio_interrupt_get(this->pio, this->sm + 4)) {
-    this->sent = 0;
-    pio_sm_drain_tx_fifo(this->pio, this->sm);
-    pio_interrupt_clear(this->pio, this->sm + 4);
-  }
-  
   if (!pio_sm_is_rx_fifo_empty(this->pio, this->sm)) {
     u32 fifo = pio_sm_get(this->pio, this->sm) >> 23;
     
@@ -87,8 +91,8 @@ void ph_ps2_phy_task(ph_ps2_phy* this) {
       return;
     }
     
-    while(queue_try_remove(&this->qbytes, &byte));
-    while(queue_try_remove(&this->qpacks, &pack));
+    while (queue_try_remove(&this->qbytes, &byte));
+    while (queue_try_remove(&this->qpacks, &pack));
     
     (*this->rx)(fifo, this->last_rx);
     this->last_rx = fifo;