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

6 files changed, 280 insertions, 196 deletions

diff --git a/hid/pico/src/CMakeLists.txt b/hid/pico/src/CMakeLists.txt
index 0426110c..e2897198 100644
--- a/hid/pico/src/CMakeLists.txt
+++ b/hid/pico/src/CMakeLists.txt
@@ -8,19 +8,19 @@ target_sources(${target_name} PRIVATE
 	ph_usb_kbd.c
 	ph_usb_mouse.c
 	ph_ps2.c
+	ph_ps2_phy.c
 	ph_cmds.c
 	ph_com.c
 	ph_com_bridge.c
 	ph_com_spi.c
 	ph_com_uart.c
 	ph_debug.c
-	# TODO: PS2: ${PS2_PATH}/foo.c
 )
 target_link_options(${target_name} PRIVATE -Xlinker --print-memory-usage)
 target_compile_options(${target_name} PRIVATE -Wall -Wextra)
 target_include_directories(${target_name} PRIVATE ${CMAKE_CURRENT_LIST_DIR})
 
-pico_generate_pio_header(${target_name} ${CMAKE_CURRENT_LIST_DIR}/ph_ps2.pio)
+pico_generate_pio_header(${target_name} ${CMAKE_CURRENT_LIST_DIR}/ph_ps2_phy.pio)
 
 target_link_libraries(${target_name} PRIVATE
 	pico_stdlib
@@ -29,6 +29,5 @@ target_link_libraries(${target_name} PRIVATE
 	hardware_spi
 	hardware_watchdog
 	tinyusb_device
-	# TODO: PS2: ... or make a library
 )
 pico_add_extra_outputs(${target_name})
diff --git a/hid/pico/src/ph_ps2.c b/hid/pico/src/ph_ps2.c
index caddc174..51c0a21b 100644
--- a/hid/pico/src/ph_ps2.c
+++ b/hid/pico/src/ph_ps2.c
@@ -24,15 +24,17 @@
 
 #include "ph_types.h"
 #include "ph_outputs.h"
-#include "ph_ps2.pio.h"
+#include "ph_ps2_phy.h"
 #include "hardware/gpio.h"
 
 u8 ph_g_ps2_kbd_leds = 0;
 bool ph_g_ps2_kbd_online = 0;
 bool ph_g_ps2_mouse_online = 0;
+ph_ps2_phy ph_ps2_kbd;
+ph_ps2_phy ph_ps2_mouse;
 
-uint8_t const mod2ps2[] = { 0x14, 0x12, 0x11, 0x1f, 0x14, 0x59, 0x11, 0x27 };
-uint8_t const hid2ps2[] = {
+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,
 	0x3a, 0x31, 0x44, 0x4d, 0x15, 0x2d, 0x1b, 0x2c, 0x3c, 0x2a, 0x1d, 0x22, 0x35, 0x1a, 0x16, 0x1e,
 	0x26, 0x25, 0x2e, 0x36, 0x3d, 0x3e, 0x46, 0x45, 0x5a, 0x76, 0x66, 0x0d, 0x29, 0x4e, 0x55, 0x54,
@@ -42,119 +44,106 @@ uint8_t const hid2ps2[] = {
 	0x75, 0x7d, 0x70, 0x71, 0x61, 0x2f, 0x37, 0x0f, 0x08, 0x10, 0x18, 0x20, 0x28, 0x30, 0x38, 0x40,
 	0x48, 0x50, 0x57, 0x5f
 };
-uint8_t const maparray = sizeof(hid2ps2) / sizeof(uint8_t);
+u8 const ph_ps2_maparray = sizeof(ph_ps2_hid2ps2);
 
-PIO pio = pio0;
-uint sm;
-uint offset;
-
-uint16_t ph_ps2_frame(uint8_t data) {
-	uint8_t parity = 1;
-	for (uint8_t i = 0; i < 8; i++) {
-		parity = parity ^ (data >> i & 1);
-	}
-	
-	return ((1 << 10) | (parity << 9) | (data << 1)) ^ 0x7ff;
-}
-
-void ph_ps2_kbd_send(uint8_t data) {
-	pio_sm_put(pio, sm, ph_ps2_frame(data));
+void ph_ps2_kbd_send(u8 byte) {
+	queue_try_add(&ph_ps2_kbd.qbytes, &byte);
 }
 
-void ph_ps2_kbd_maybe_send_e0(uint8_t data) {
-	if (data == 0x46 ||
-		 (data >= 0x49 && data <= 0x52) ||
-			data == 0x54 || data == 0x58 ||
-			data == 0x65 || data == 0x66 ||
-			data >= 0x81) {
+void ph_ps2_kbd_maybe_send_e0(u8 byte) {
+	if (byte == 0x46 ||
+		 (byte >= 0x49 && byte <= 0x52) ||
+			byte == 0x54 || byte == 0x58 ||
+			byte == 0x65 || byte == 0x66 ||
+			byte >= 0x81) {
 		ph_ps2_kbd_send(0xe0);
 	}
 }
 
-void ph_ps2_init(void) {
-	if (PH_O_IS_KBD_PS2 || PH_O_IS_MOUSE_PS2) {
-		gpio_init(13);
-		gpio_set_dir(13, GPIO_OUT);
-		gpio_put(13, 1); // LV pull-up voltage
-		
-		sm = pio_claim_unused_sm(pio, true);
-		offset = pio_add_program(pio, &ps2device_program);
-		ps2device_program_init(pio, sm, offset, 14);
-	}
-}
-
-void ph_ps2_task(void) {
-	if (PH_O_IS_KBD_PS2 || PH_O_IS_MOUSE_PS2) {
-		
-		if (!pio_sm_is_rx_fifo_empty(pio, sm)) {
-			uint32_t fifo = pio_sm_get(pio, sm);
-			fifo = fifo >> 23;
+void ph_ps2_kbd_receive(u8 byte) {
+	switch(byte) {
+		case 0xed: // CMD: Set LEDs
 			
-			uint8_t parity = 1;
-			for(uint8_t i = 0; i < 8; i++) {
-				parity = parity ^ (fifo >> i & 1);
-			}
+		break;
+		
+		case 0xf3: // CMD: Set typematic rate and delay
 			
-			if(parity != fifo >> 8) {
-				ph_ps2_kbd_send(0xfe);
+		break;
+		
+		default:
+			switch(byte) {
+				case 0xff: // CMD: Reset
+					//pio_sm_clear_fifos(pio, sm);
+					//pio_sm_drain_tx_fifo(pio, sm);
+					ph_ps2_kbd_send(0xfa);
+					ph_ps2_kbd_send(0xaa);
 				return;
-			}
-			
-			uint8_t data = fifo;
-			
-			/*switch() {
+				
+				case 0xfe: // CMD: Resend
+					
+				return;
+				
+				case 0xee: // CMD: Echo
+					ph_ps2_kbd_send(0xee);
+				return;
+				
+				case 0xf2: // CMD: Identify keyboard
+					ph_ps2_kbd_send(0xfa);
+					ph_ps2_kbd_send(0xab);
+					ph_ps2_kbd_send(0x83);
+				return;
+				
+				case 0xf3: // CMD: Set typematic rate and delay
 				case 0xed: // CMD: Set LEDs
 					
 				break;
 				
-				case 0xf3: // CMD: Set typematic rate and delay
+				case 0xf4: // CMD: Enable scanning
 					
 				break;
 				
-				default:*/
-					switch(data) {
-						case 0xff: // CMD: Reset
-							pio_sm_clear_fifos(pio, sm);
-							pio_sm_drain_tx_fifo(pio, sm);
-							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;
-						
-						case 0xf2: // CMD: Identify keyboard
-							ph_ps2_kbd_send(0xfa);
-							ph_ps2_kbd_send(0xab);
-							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
-							
-						break;
-						
-						case 0xf5: // CMD: Disable scanning, restore default parameters
-						case 0xf6: // CMD: Set default parameters
-							
-						break;
-					}
-			/*	break;
-			}*/
-			
-			ph_ps2_kbd_send(0xfa);
-		}
+				case 0xf5: // CMD: Disable scanning, restore default parameters
+				case 0xf6: // CMD: Set default parameters
+					
+				break;
+			}
+	  break;
+	}
+	
+	ph_ps2_kbd_send(0xfa);
+}
+
+void ph_ps2_mouse_receive(u8 byte) {
+	switch(byte) {
 		
 	}
+}
+
+void ph_ps2_init(void) {
+	if (PH_O_IS_KBD_PS2 || PH_O_IS_MOUSE_PS2) {
+		gpio_init(13);
+		gpio_set_dir(13, GPIO_OUT);
+		gpio_put(13, 1); // GPIO13=LV pull-up voltage
+	}
+	
+	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);
+	}
+	
+	if (PH_O_IS_MOUSE_PS2) {
+		ph_ps2_phy_init(&ph_ps2_mouse, pio1, 14, &ph_ps2_mouse_receive); // mouse: GPIO14=data, GPIO15=clock
+	}
+}
+
+void ph_ps2_task(void) {
+	if (PH_O_IS_KBD_PS2) {
+		ph_ps2_phy_task(&ph_ps2_kbd);
+	}
+	
+	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?)
@@ -167,25 +156,39 @@ void ph_ps2_kbd_send_key(u8 key, bool state) {
 	if (PH_O_IS_KBD_PS2) {
 		if (key >= 0xe0 && key <= 0xe7) {
 			key -= 0xe0;
-			if(key > 2 && key != 5) {
+			
+			if (key > 2 && key != 5) {
 				ph_ps2_kbd_send(0xe0);
 			}
 			
-			if(!state) {
+			if (!state) {
 				ph_ps2_kbd_send(0xf0);
 			}
 			
-			ph_ps2_kbd_send(mod2ps2[key]);
+			ph_ps2_kbd_send(ph_ps2_mod2ps2[key]);
 			
-		} else if (key < maparray) {
-			ph_ps2_kbd_maybe_send_e0(key);
+		} else if (key < ph_ps2_maparray) {
 			
-			if(!state) {
-				ph_ps2_kbd_send(0xf0);
+			if (key == 0x48) {
+				if (state) {
+					
+					if (false) { // TODO: is shift
+						ph_ps2_kbd_send(0xe0); ph_ps2_kbd_send(0x7e); ph_ps2_kbd_send(0xe0); ph_ps2_kbd_send(0xf0); ph_ps2_kbd_send(0x7e);
+					} else {
+						ph_ps2_kbd_send(0xe1); ph_ps2_kbd_send(0x14); ph_ps2_kbd_send(0x77); ph_ps2_kbd_send(0xe1);
+						ph_ps2_kbd_send(0xf0); ph_ps2_kbd_send(0x14); ph_ps2_kbd_send(0xf0); ph_ps2_kbd_send(0x77);
+					}
+					
+				}
+			} else {
+				ph_ps2_kbd_maybe_send_e0(key);
+				
+				if (!state) {
+					ph_ps2_kbd_send(0xf0);
+				}
+				
+				ph_ps2_kbd_send(ph_ps2_hid2ps2[key]);
 			}
-			
-			ph_ps2_kbd_send(hid2ps2[key]);
-			
 		}
 	}
 }
diff --git a/hid/pico/src/ph_ps2.pio b/hid/pico/src/ph_ps2.pio
deleted file mode 100644
index 2313419d..00000000
--- a/hid/pico/src/ph_ps2.pio
+++ /dev/null
@@ -1,86 +0,0 @@
-;
-; Copyright (c) 2022 No0ne (https://github.com/No0ne)
-;           (c) 2023 Dustin Hoffman
-;
-; SPDX-License-Identifier: MIT
-;
-
-.program ps2device
-.side_set 1 opt pindirs
-
-  //mov y ! null
-start:
-.wrap_target
-  set pindirs 0 side 0 // Clock and data to input mode.
-  jmp pin sendcheck // If clock is high, see if we have data to send.
-  // Clock is being pulled low.
-  wait 1 pin 1 // Wait for clock to be pulled high.
-
-  // 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.
-  jmp start // Not receiving, restart.
-
-receive:
-  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.
-  set pindirs, 1 [6]// Clock low
-  jmp x-- receiveloop [4]// Iterate
-  set pindirs, 0 [6] // Clock high
-  nop side 1 [6]// Data low
-  set pindirs, 1 [7] // Clock low
-  //in null 1
-.wrap
-
-sendcheck:
-  jmp !osre wait_to_write // See if we have data to send.
-  jmp start // No data to send, restart.
-
-wait_to_write:
-  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.
-  // Pin was low, host wants to send data.
-  // Notify of failure to send data..
-  //in null 8
-  //in y 2
-  jmp start
-sendcontinue:
-  out pindirs, 1 [6] // Write out data.
-  set pindirs, 1 [6] // Set clock low.
-  jmp x-- sendloop [6]
-  //in y 10
-  jmp start
-
-% c-sdk {
-
-  void ps2device_program_init(PIO pio, uint sm, uint offset, uint dat) {
-    pio_sm_config c = ps2device_program_get_default_config(offset);
-    
-    uint clk = dat + 1;
-    pio_gpio_init(pio, clk);
-    pio_gpio_init(pio, dat);
-    
-    // Use a frequency high enough to effectivly sample clock and data.
-    sm_config_set_clkdiv(&c, 427); // 2560 is 20 us,  640 is 5 us,  427 is 3.3333 us
-    sm_config_set_jmp_pin(&c, clk);
-    sm_config_set_set_pins(&c, clk, 1);
-    sm_config_set_sideset_pins(&c, dat);
-    sm_config_set_out_pins(&c, dat, 1);
-    sm_config_set_out_shift(&c, true, true, 11);
-    sm_config_set_in_pins(&c, dat);
-    sm_config_set_in_shift(&c, true, true, 9);
-    
-    pio_sm_init(pio, sm, offset, &c);
-    pio_sm_set_enabled(pio, sm, true);
-  }
-
-%}
\ No newline at end of file
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);
+  }
+}
diff --git a/hid/pico/src/ph_ps2_phy.h b/hid/pico/src/ph_ps2_phy.h
new file mode 100644
index 00000000..17dd9e0d
--- /dev/null
+++ b/hid/pico/src/ph_ps2_phy.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include "ph_types.h"
+#include "hardware/pio.h"
+#include "pico/util/queue.h"
+
+typedef void (*rx_callback)(u8 byte);
+
+typedef struct {
+  PIO pio;
+  uint sm;
+  queue_t qbytes;
+  queue_t qpacks;
+  u8 sent;
+  rx_callback rx;
+} ph_ps2_phy;
+
+void ph_ps2_phy_init(ph_ps2_phy* this, PIO pio, u8 data_pin, rx_callback rx);
+void ph_ps2_phy_task(ph_ps2_phy* this);
\ No newline at end of file
diff --git a/hid/pico/src/ph_ps2_phy.pio b/hid/pico/src/ph_ps2_phy.pio
new file mode 100644
index 00000000..055cc6d7
--- /dev/null
+++ b/hid/pico/src/ph_ps2_phy.pio
@@ -0,0 +1,81 @@
+;
+; Copyright (c) 2022 No0ne (https://github.com/No0ne)
+;           (c) 2023 Dustin Hoffman
+;
+; SPDX-License-Identifier: MIT
+;
+
+.program ps2phy
+.side_set 1 opt pindirs
+
+restart:
+  irq clear 0 rel
+  set pindirs 0 side 0 // clock and data to input mode
+  
+receivecheck:
+  jmp pin sendcheck // if clock is high, see if we have data to send
+  irq set 0 rel     // clock is being pulled low, set busy flag
+  wait 1 pin 1      // wait for clock to be pulled high
+
+  // 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.
+  jmp restart    // not receiving
+
+receive:
+  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.
+  set pindirs, 1      [6] // clock low
+  jmp x-- receiveloop [4] // iterate
+  set pindirs, 0      [6] // clock high
+  nop side 1          [6] // data low
+  set pindirs, 1      [7] // clock low
+  jmp restart
+  
+sendcheck:
+  jmp !osre wait_to_write // see if we have data to send
+  jmp receivecheck        // no data to send, restart
+
+wait_to_write:
+  irq set 0 rel
+  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
+sendcontinue:
+  out pindirs, 1   [6] // write out data
+  set pindirs, 1   [6] // set clock low
+  jmp x-- sendloop [6]
+
+% c-sdk {
+  void ps2phy_program_init(PIO pio, uint sm, uint offset, uint dat) {
+    pio_sm_config c = ps2phy_program_get_default_config(offset);
+    
+    u8 clk = dat + 1;
+    pio_gpio_init(pio, clk);
+    pio_gpio_init(pio, dat);
+    
+    // Use a frequency high enough to effectivly sample clock and data.
+    sm_config_set_clkdiv(&c, 427); // 2560 is 20 µs,  640 is 5 µs,  427 is 3.3333 µs
+    sm_config_set_jmp_pin(&c, clk);
+    sm_config_set_set_pins(&c, clk, 1);
+    sm_config_set_sideset_pins(&c, dat);
+    sm_config_set_out_pins(&c, dat, 1);
+    sm_config_set_out_shift(&c, true, true, 11);
+    sm_config_set_in_pins(&c, dat);
+    sm_config_set_in_shift(&c, true, true, 9);
+    
+    pio_sm_init(pio, sm, offset, &c);
+    pio_sm_set_enabled(pio, sm, true);
+  }
+%}
\ No newline at end of file