pico hid: ps/2 fix retransmission (#145)

* pico hid: ps/2 keyboard support getting started (#139)

* pico hid: ps/2 bytes and packets queue

* pico hid: ps/2 bytes and packets queue (#142)

* pico hid: ps/2 fix retransmission

4 files changed, 44 insertions, 43 deletions

diff --git a/hid/pico/src/ph_ps2.c b/hid/pico/src/ph_ps2.c
index 51c0a21b..4aca54ee 100644
--- a/hid/pico/src/ph_ps2.c
+++ b/hid/pico/src/ph_ps2.c
@@ -33,6 +33,7 @@ bool ph_g_ps2_mouse_online = 0;
 ph_ps2_phy ph_ps2_kbd;
 ph_ps2_phy ph_ps2_mouse;
 
+u8 const ph_ps2_led2ps2[] = { 0, 4, 1, 5, 2, 6, 3, 7 };
 u8 const ph_ps2_mod2ps2[] = { 0x14, 0x12, 0x11, 0x1f, 0x14, 0x59, 0x11, 0x27 };
 u8 const ph_ps2_hid2ps2[] = {
 	0x00, 0x00, 0xfc, 0x00, 0x1c, 0x32, 0x21, 0x23, 0x24, 0x2b, 0x34, 0x33, 0x43, 0x3b, 0x42, 0x4b,
@@ -60,10 +61,10 @@ void ph_ps2_kbd_maybe_send_e0(u8 byte) {
 	}
 }
 
-void ph_ps2_kbd_receive(u8 byte) {
-	switch(byte) {
+void ph_ps2_kbd_receive(u8 byte, u8 prev_byte) {
+	switch(prev_byte) {
 		case 0xed: // CMD: Set LEDs
-			
+			ph_g_ps2_kbd_leds = ph_ps2_led2ps2[byte];
 		break;
 		
 		case 0xf3: // CMD: Set typematic rate and delay
@@ -73,16 +74,12 @@ void ph_ps2_kbd_receive(u8 byte) {
 		default:
 			switch(byte) {
 				case 0xff: // CMD: Reset
-					//pio_sm_clear_fifos(pio, sm);
-					//pio_sm_drain_tx_fifo(pio, sm);
+					ph_g_ps2_kbd_online = true;
+					ph_g_ps2_kbd_leds = 0;
 					ph_ps2_kbd_send(0xfa);
 					ph_ps2_kbd_send(0xaa);
 				return;
 				
-				case 0xfe: // CMD: Resend
-					
-				return;
-				
 				case 0xee: // CMD: Echo
 					ph_ps2_kbd_send(0xee);
 				return;
@@ -93,18 +90,14 @@ void ph_ps2_kbd_receive(u8 byte) {
 					ph_ps2_kbd_send(0x83);
 				return;
 				
-				case 0xf3: // CMD: Set typematic rate and delay
-				case 0xed: // CMD: Set LEDs
-					
-				break;
-				
 				case 0xf4: // CMD: Enable scanning
-					
+					ph_g_ps2_kbd_online = true;
 				break;
 				
 				case 0xf5: // CMD: Disable scanning, restore default parameters
 				case 0xf6: // CMD: Set default parameters
-					
+					ph_g_ps2_kbd_online = byte == 0xf6;
+					ph_g_ps2_kbd_leds = 0;
 				break;
 			}
 	  break;
@@ -129,6 +122,7 @@ void ph_ps2_init(void) {
 	if (PH_O_IS_KBD_PS2) {
 		ph_ps2_phy_init(&ph_ps2_kbd, pio0, 11, &ph_ps2_kbd_receive); // keyboard: GPIO11=data, GPIO12=clock
 		ph_ps2_kbd_send(0xaa);
+		ph_g_ps2_kbd_online = true;
 	}
 	
 	if (PH_O_IS_MOUSE_PS2) {
@@ -144,16 +138,10 @@ void ph_ps2_task(void) {
 	if (PH_O_IS_MOUSE_PS2) {
 		ph_ps2_phy_task(&ph_ps2_mouse);
 	}
-	// Here you should update some values:
-	//   - ph_g_ps2_kbd_leds - keyboard LEDs mask like on USB
-	//   - ph_g_ps2_kbd_online - if keyboard online (by clock?)
-	//   - ph_g_ps2_mouse_online if mouse online (by clock?)
-	// It is important not to have ANY sleep() call inside it.
-	// There should also be no freezes if the keyboard or mouse is not available.
 }
 
 void ph_ps2_kbd_send_key(u8 key, bool state) {
-	if (PH_O_IS_KBD_PS2) {
+	if (PH_O_IS_KBD_PS2 && ph_g_ps2_kbd_online) {
 		if (key >= 0xe0 && key <= 0xe7) {
 			key -= 0xe0;
 			
diff --git a/hid/pico/src/ph_ps2_phy.c b/hid/pico/src/ph_ps2_phy.c
index b5ffba75..6f19a7ca 100644
--- a/hid/pico/src/ph_ps2_phy.c
+++ b/hid/pico/src/ph_ps2_phy.c
@@ -11,9 +11,11 @@ void ph_ps2_phy_init(ph_ps2_phy* this, PIO pio, u8 data_pin, rx_callback rx) {
   
   this->sent = 0;
   this->rx = rx;
+  this->last_rx = 0;
+  this->last_tx = 0;
 }
 
-u16 ph_ps2_frame(u8 byte) {
+u32 ph_ps2_frame(u8 byte) {
   u8 parity = 1;
   for (u8 i = 0; i < 8; i++) {
     parity = parity ^ (byte >> i & 1);
@@ -23,11 +25,10 @@ u16 ph_ps2_frame(u8 byte) {
 
 void ph_ps2_phy_task(ph_ps2_phy* this) {
   u8 i = 0;
+  u8 byte;
   u8 pack[9];
   
   if (!queue_is_empty(&this->qbytes)) {
-    u8 byte;
-    
     while (i < 9 && queue_try_remove(&this->qbytes, &byte)) {
       i++;
       pack[i] = byte;
@@ -37,18 +38,25 @@ void ph_ps2_phy_task(ph_ps2_phy* this) {
     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_is_empty(&this->qpacks) && pio_sm_is_tx_fifo_empty(this->pio, this->sm) && !pio_interrupt_get(this->pio, this->sm * 2 + 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]));
+        this->last_tx = pack[this->sent];
+        pio_sm_put(this->pio, this->sm, ph_ps2_frame(this->last_tx));
       }
     }
   }
   
+  if (pio_interrupt_get(this->pio, this->sm * 2 + 1)) {
+    this->sent = 0;
+    pio_sm_drain_tx_fifo(this->pio, this->sm);
+    pio_interrupt_clear(this->pio, this->sm * 2 + 1);
+  }
+  
   if (!pio_sm_is_rx_fifo_empty(this->pio, this->sm)) {
     u32 fifo = pio_sm_get(this->pio, this->sm);
     fifo = fifo >> 23;
@@ -59,10 +67,14 @@ void ph_ps2_phy_task(ph_ps2_phy* this) {
     }
     
     if (parity != fifo >> 8) {
-      //ph_ps2_kbd_send(0xfe);
+      pio_sm_put(this->pio, this->sm, ph_ps2_frame(0xfe));
       return;
     }
     
-    (*this->rx)(fifo);
+    while(queue_try_remove(&this->qbytes, &byte));
+    while(queue_try_remove(&this->qpacks, &pack));
+    
+    (*this->rx)(fifo, this->last_rx);
+    this->last_rx = fifo;
   }
 }
diff --git a/hid/pico/src/ph_ps2_phy.h b/hid/pico/src/ph_ps2_phy.h
index 17dd9e0d..19008c99 100644
--- a/hid/pico/src/ph_ps2_phy.h
+++ b/hid/pico/src/ph_ps2_phy.h
@@ -4,7 +4,7 @@
 #include "hardware/pio.h"
 #include "pico/util/queue.h"
 
-typedef void (*rx_callback)(u8 byte);
+typedef void (*rx_callback)(u8 byte, u8 prev_byte);
 
 typedef struct {
   PIO pio;
@@ -13,6 +13,8 @@ typedef struct {
   queue_t qpacks;
   u8 sent;
   rx_callback rx;
+  u8 last_rx;
+  u8 last_tx;
 } ph_ps2_phy;
 
 void ph_ps2_phy_init(ph_ps2_phy* this, PIO pio, u8 data_pin, rx_callback rx);
diff --git a/hid/pico/src/ph_ps2_phy.pio b/hid/pico/src/ph_ps2_phy.pio
index 055cc6d7..a2e4c81f 100644
--- a/hid/pico/src/ph_ps2_phy.pio
+++ b/hid/pico/src/ph_ps2_phy.pio
@@ -9,8 +9,8 @@
 .side_set 1 opt pindirs
 
 restart:
-  irq clear 0 rel
-  set pindirs 0 side 0 // clock and data to input mode
+  set pindirs 0 [4]      // set clock to input mode
+  irq clear 0 rel side 0 // clear busy flag, set data to input mode
   
 receivecheck:
   jmp pin sendcheck // if clock is high, see if we have data to send
@@ -20,17 +20,17 @@ receivecheck:
   // we are not sending, look for a start bit (clock high, data low)
   in pins 1      // read in from data
   mov x isr      // move what we read to x
-  mov isr null   // clear ISR.
-  jmp !x receive // if x is low, start the receive process.
+  mov isr null   // clear ISR
+  jmp !x receive // if x is low, start the receive process
   jmp restart    // not receiving
 
 receive:
-  set pindirs, 1 [6] // clock low.
-  set x, 8           // set loop counter.
+  set pindirs, 1 [6] // clock low
+  set x, 8           // set loop counter
 
 receiveloop:
   set pindirs, 0      [6] // clock high
-  in pins, 1          [4] // read a bit into ISR.
+  in pins, 1          [4] // read a bit into ISR
   set pindirs, 1      [6] // clock low
   jmp x-- receiveloop [4] // iterate
   set pindirs, 0      [6] // clock high
@@ -43,15 +43,14 @@ sendcheck:
   jmp receivecheck        // no data to send, restart
 
 wait_to_write:
-  irq set 0 rel
-  set x 10 // number of bits to write out
+  irq set 0 rel // set busy flag
+  set x 10      // number of bits to write out
 
 sendloop:
   set pindirs, 0   [6] // clock set to input (high)
   jmp pin sendcontinue // if clock is high, host is still receiving data
-  irq set 1 rel        // clock was low, host wants to send data, notify of failure to send data
-  mov osr null         // clear OSR
-  jmp restart
+  irq wait 1 rel       // clock was low, host wants to send data, notify of failure to send data
+  jmp restart          // and wait for restart
 sendcontinue:
   out pindirs, 1   [6] // write out data
   set pindirs, 1   [6] // set clock low