pico hid: ps/2 mouse support (#148)

5 files changed, 160 insertions, 198 deletions

diff --git a/hid/pico/src/ph_ps2_kbd.c b/hid/pico/src/ph_ps2_kbd.c
index f9e0caa3..e2fd7faf 100644
--- a/hid/pico/src/ph_ps2_kbd.c
+++ b/hid/pico/src/ph_ps2_kbd.c
@@ -213,10 +213,10 @@ void ph_ps2_kbd_receive(u8 byte, u8 prev_byte) {
 
 void ph_ps2_kbd_task(void) {
   ph_ps2_phy_task(&ph_ps2_kbd);
-  ph_g_ps2_kbd_online = ph_ps2_kbd_scanning && ph_ps2_kbd.idle;
+  ph_g_ps2_kbd_online = ph_ps2_kbd_scanning && !ph_ps2_kbd.busy;
 }
 
 void ph_ps2_kbd_init(u8 gpio) {
   ph_ps2_phy_init(&ph_ps2_kbd, pio0, gpio, &ph_ps2_kbd_receive); 
   ph_ps2_kbd_reset();
-}
\ No newline at end of file
+}
diff --git a/hid/pico/src/ph_ps2_mouse.c b/hid/pico/src/ph_ps2_mouse.c
index 279a06d6..c825ca03 100644
--- a/hid/pico/src/ph_ps2_mouse.c
+++ b/hid/pico/src/ph_ps2_mouse.c
@@ -4,179 +4,134 @@
 extern bool ph_g_ps2_mouse_online;
 
 ph_ps2_phy ph_ps2_mouse;
-u8 ms_type = 0;
-u8 ms_mode = 0;
-u8 ms_input_mode = 0;
-u8 ms_rate = 100;
-u32 ms_magic_seq = 0x00;
-
-u8 buttons = 0;
-
-#define MS_TYPE_STANDARD  0x00
-#define MS_TYPE_WHEEL_3   0x03
-#define MS_TYPE_WHEEL_5   0x04
-
-#define MS_MODE_IDLE      0
-#define MS_MODE_STREAMING 1
-
-#define MS_INPUT_CMD      0
-#define MS_INPUT_SET_RATE 1
+bool ph_ps2_mouse_streaming = false;
+u32 ph_ps2_mouse_magic_seq = 0;
+u8 ph_ps2_mouse_type = 0;
+u8 ph_ps2_mouse_buttons = 0;
 
 void ph_ps2_mouse_send(u8 byte) {
   queue_try_add(&ph_ps2_mouse.qbytes, &byte);
 }
 
-void ph_ps2_mouse_packet(u8 button, u8 x1, u8 y1) {
-  if(ms_mode == MS_MODE_STREAMING) {
-    u8 s = (button & 7) + 8;
-    u8 x = x1 & 0x7f;
-    u8 y = y1 & 0x7f;
-    u8 z = 0;
-    
-    if(x1 >> 7) {
-      s += 0x10;
-      x += 0x80;
-    }
+void ph_ps2_mouse_pack(s8 x, s8 y, s8 h, s8 v) {
+  if (ph_ps2_mouse_streaming) {
+    u8 byte1 = 0x8 | (ph_ps2_mouse_buttons & 0x7);
+    s8 byte2 = x;
+    s8 byte3 = 0x100 - y;
+    s8 byte4 = 0; // = 0x100 - z;
     
-    if(y1 >> 7) {
-      y = 0x80 - y;
-    } else if(y) {
-      s += 0x20;
-      y = 0x100 - y;
-    }
+    if (byte2 < 0) byte1 |= 0x10;
+    if (byte3 < 0) byte1 |= 0x20;
     
-    ph_ps2_mouse_send(s);
-    ph_ps2_mouse_send(x);
-    ph_ps2_mouse_send(y);
+    ph_ps2_mouse_send(byte1);
+    ph_ps2_mouse_send(byte2);
+    ph_ps2_mouse_send(byte3);
     
-    if (ms_type == MS_TYPE_WHEEL_3 || ms_type == MS_TYPE_WHEEL_5) {
-      /*if(report[3] >> 7) {
-        z = 0x8 - z;
-      } else if(z) {
-        z = 0x10 - z;
+    if (ph_ps2_mouse_type == 3 || ph_ps2_mouse_type == 4) {
+      //if (byte4 < -8) byte4 = -8;
+      //if (byte4 > 7) byte4 = 7;
+      //if (byte4 < 0) byte4 |= 0xf8;
+      
+      if (v < 0) byte4 = 0x01;
+      if (v > 0) byte4 = 0xff;
+      if (h < 0) byte4 = 0x02;
+      if (h > 0) byte4 = 0xfe;
+      
+      if (ph_ps2_mouse_type == 4) {
+        byte4 &= 0xf;
+        byte4 |= (ph_ps2_mouse_buttons << 1) & 0x30;
       }
-    
-      if (ms_type == MS_TYPE_WHEEL_5) {
-        if (report[0] & 0x8) {
-          z += 0x10;
-        }
-    
-        if (report[0] & 0x10) {
-          z += 0x20;
-        }
-      }*/
-    
-      ph_ps2_mouse_send(z);
+      
+      ph_ps2_mouse_send(byte4);
     }
   }
 }
 
 void ph_ps2_mouse_send_button(u8 button, bool state) {
-  // TODO: PS2: Send mouse button
-  //   @button - USB button code
-  //   @state - true if pressed, false if released
-  // The function should take care not to send duplicate events (if needed for PS/2)
-  // If the PS2 keyboard is not used (PH_O_IS_MOUSE_PS2 is false), the function should do nothing.
-  (void)button; // Remove this
-  (void)state; // Remove this
-  
-  u8 bitval = 1;
-  
-  button--;
-  
-  if(state) {
-    buttons = buttons | (bitval << button);
-  } else {
-    buttons = buttons & ~(bitval << button);
+  if (PH_O_IS_MOUSE_PS2) {
+    button--;
+    
+    if (state) {
+      ph_ps2_mouse_buttons = ph_ps2_mouse_buttons | (1 << button);
+    } else {
+      ph_ps2_mouse_buttons = ph_ps2_mouse_buttons & ~(1 << button);
+    }
+    
+    ph_ps2_mouse_pack(0, 0, 0, 0);
   }
-  
-  ph_ps2_mouse_packet(buttons, 0, 0);
 }
 
-void ph_ps2_mouse_send_rel(s8 x1, s8 y1) {
-  // TODO: PS2: Send relative move event
-  // If the PS2 keyboard is not used (PH_O_IS_MOUSE_PS2 is false), the function should do nothing.
-  ph_ps2_mouse_packet(buttons, x1, y1);
+void ph_ps2_mouse_send_rel(s8 x, s8 y) {
+  if (PH_O_IS_MOUSE_PS2) {
+    ph_ps2_mouse_pack(x, y, 0, 0);
+  }
 }
 
 void ph_ps2_mouse_send_wheel(s8 h, s8 v) {
-  (void)h;
-  // TODO: PS2: Send wheel. As I understand, PS/2 has no horizontal scrolling, so @h just can be ignored.
-  //   @v - vertical scrolling like on USB
-  // If the PS2 keyboard is not used (PH_O_IS_MOUSE_PS2 is false), the function should do nothing.
-  (void)v; // Remove this
+  if (PH_O_IS_MOUSE_PS2) {
+    ph_ps2_mouse_pack(0, 0, h, v);
+  }
 }
 
 void ph_ps2_mouse_receive(u8 byte, u8 prev_byte) {
-  
-  if(ms_input_mode == MS_INPUT_SET_RATE) {
-    ms_rate = byte;
-    ms_input_mode = MS_INPUT_CMD;
-    ph_ps2_mouse_send(0xfa);
-  
-    ms_magic_seq = (ms_magic_seq << 8) | byte;
-    if(ms_type == MS_TYPE_STANDARD && ms_magic_seq == 0xc86450) {
-      ms_type = MS_TYPE_WHEEL_3;
-    } else if (ms_type == MS_TYPE_WHEEL_3 && ms_magic_seq == 0xc8c850) {
-      ms_type = MS_TYPE_WHEEL_5;
-    }
-    return;
-  }
-  
-  if(byte != 0xf3) {
-    ms_magic_seq = 0x00;
-  }
-  
-  switch(byte) {
-    case 0xff: // Reset
-      ms_type = MS_TYPE_STANDARD;
-      ms_mode = MS_MODE_IDLE;
-      ms_rate = 100;
-      
-      ph_ps2_mouse_send(0xfa);
-      ph_ps2_mouse_send(0xaa);
-      ph_ps2_mouse_send(ms_type);
-    return;
-  
-    case 0xf6: // Set Defaults
-      ms_type = MS_TYPE_STANDARD;
-      ms_rate = 100;
-    case 0xf5: // Disable Data Reporting
-    case 0xea: // Set Stream Mode
-      ms_mode = MS_MODE_IDLE;
-      ph_ps2_mouse_send(0xfa);
-    return;
-  
-    case 0xf4: // Enable Data Reporting
-      ms_mode = MS_MODE_STREAMING;
-      ph_ps2_mouse_send(0xfa);
-    return;
-  
+  switch (prev_byte) {
     case 0xf3: // Set Sample Rate
-      ms_input_mode = MS_INPUT_SET_RATE;
-      ph_ps2_mouse_send(0xfa);
-    return;
-  
-    case 0xf2: // Get Device ID
-      ph_ps2_mouse_send(0xfa);
-      ph_ps2_mouse_send(ms_type);
-    return;
-  
-    case 0xe9: // Status Request
-      ph_ps2_mouse_send(0xfa);
-      ph_ps2_mouse_send(0x00); // Bit6: Mode, Bit 5: Enable, Bit 4: Scaling, Bits[2,1,0] = Buttons[L,M,R]
-      ph_ps2_mouse_send(0x02); // Resolution
-      ph_ps2_mouse_send(ms_rate); // Sample Rate
-    return;
-  
-  // TODO: Implement (more of) these?
-  //    case 0xf0: // Set Remote Mode
-  //    case 0xee: // Set Wrap Mode
-  //    case 0xec: // Reset Wrap Mode
-  //    case 0xeb: // Read Data
-  //    case 0xe8: // Set Resolution
-  //    case 0xe7: // Set Scaling 2:1
-  //    case 0xe6: // Set Scaling 1:1
+      ph_ps2_mouse_magic_seq = ((ph_ps2_mouse_magic_seq << 8) | byte) & 0xffffff;
+      
+      if (ph_ps2_mouse_type == 0 && ph_ps2_mouse_magic_seq == 0xc86450) {
+        ph_ps2_mouse_type = 3;
+      } else if (ph_ps2_mouse_type == 3 && ph_ps2_mouse_magic_seq == 0xc8c850) {
+        ph_ps2_mouse_type = 4;
+      }
+    break;
+    
+    default:
+      switch (byte) {
+        case 0xff: // Reset
+          ph_ps2_mouse_streaming = false;
+          ph_ps2_mouse_type = 0;
+          
+          ph_ps2_mouse_send(0xfa);
+          ph_ps2_mouse_send(0xaa);
+          ph_ps2_mouse_send(ph_ps2_mouse_type);
+        return;
+        
+        case 0xf6: // Set Defaults
+          ph_ps2_mouse_streaming = false;
+          ph_ps2_mouse_type = 0;
+        break;
+        
+        case 0xf5: // Disable Data Reporting
+        case 0xea: // Set Stream Mode
+          ph_ps2_mouse_streaming = false;
+        break;
+        
+        case 0xf4: // Enable Data Reporting
+          ph_ps2_mouse_streaming = true;
+        break;
+        
+        case 0xf2: // Get Device ID
+          ph_ps2_mouse_send(0xfa);
+          ph_ps2_mouse_send(ph_ps2_mouse_type);
+        return;
+        
+        case 0xe9: // Status Request
+          ph_ps2_mouse_send(0xfa);
+          ph_ps2_mouse_send(0x00); // Bit6: Mode, Bit 5: Enable, Bit 4: Scaling, Bits[2,1,0] = Buttons[L,M,R]
+          ph_ps2_mouse_send(0x02); // Resolution
+          ph_ps2_mouse_send(100);  // Sample Rate
+        return;
+        
+        // TODO: Implement (more of) these?
+        // case 0xf0: // Set Remote Mode
+        // case 0xee: // Set Wrap Mode
+        // case 0xec: // Reset Wrap Mode
+        // case 0xeb: // Read Data
+        // case 0xe8: // Set Resolution
+        // case 0xe7: // Set Scaling 2:1
+        // case 0xe6: // Set Scaling 1:1
+      }
+    break;
   }
   
   ph_ps2_mouse_send(0xfa);
@@ -184,6 +139,7 @@ void ph_ps2_mouse_receive(u8 byte, u8 prev_byte) {
 
 void ph_ps2_mouse_task(void) {
   ph_ps2_phy_task(&ph_ps2_mouse);
+  ph_g_ps2_mouse_online = ph_ps2_mouse_streaming && !ph_ps2_mouse.busy;
 }
 
 void ph_ps2_mouse_init(u8 gpio) {
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;
diff --git a/hid/pico/src/ph_ps2_phy.h b/hid/pico/src/ph_ps2_phy.h
index 3ed7dcfb..a6a0fc13 100644
--- a/hid/pico/src/ph_ps2_phy.h
+++ b/hid/pico/src/ph_ps2_phy.h
@@ -11,11 +11,11 @@ typedef struct {
   uint sm;
   queue_t qbytes;
   queue_t qpacks;
-  u8 sent;
   rx_callback rx;
   u8 last_rx;
   u8 last_tx;
-  bool idle;
+  u8 sent;
+  u8 busy;
 } 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 f2d7578c..19786168 100644
--- a/hid/pico/src/ph_ps2_phy.pio
+++ b/hid/pico/src/ph_ps2_phy.pio
@@ -10,52 +10,54 @@
 .side_set 1 opt pindirs
 
 restart:
-  set pindirs 0 [4]      // set clock to input mode
-  irq clear 0 rel side 0 // clear busy flag, set data to input mode
+  set    pindirs, 0                 // 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
-  irq set 0 rel     // clock is being pulled low, set busy flag
-  wait 1 pin 1      // wait for clock to be pulled high
+  jmp    pin, sendcheck             // if clock is high, see if we have data to send
+  irq    nowait 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
+  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
+  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
+  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                [7]
   
 sendcheck:
-  jmp !osre wait_to_write // see if we have data to send
-  jmp receivecheck        // no data to send, restart
+  jmp    !osre, send                // see if we have data to send
+  jmp    receivecheck               // no data to send, restart
 
-wait_to_write:
-  irq set 0 rel // set busy flag
-  set x 10      // number of bits to write out
+send:
+  irq    nowait 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 wait 4 rel       // clock was low, host wants to send data, notify of failure to send data
-  jmp restart          // and wait for restart
+  set    pindirs, 0             [6] // clock set to input (high)
+  jmp    pin, sendcontinue          // if clock is high, host is still receiving data
+  out    null, 32                   // clear OSR
+  irq    wait 4 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
-  jmp x-- sendloop [6]
+  out    pindirs, 1             [6] // write out data
+  set    pindirs, 1             [6] // set clock low
+  jmp    x--, sendloop          [6]
 
 % c-sdk {
   void ph_ps2_phy_program_init(PIO pio, uint sm, uint offset, uint dat) {