Unify ps2 port data processing.

Unify the code for mouse and keyboard irq handling.
Support case where mouse irq fires but keyboard data present and
    vice-versa.
Support data in i8042 queue while ps2 command being processed.

1 files changed, 32 insertions, 26 deletions

diff --git a/src/ps2port.c b/src/ps2port.c
index d03c049b..150529bc 100644
--- a/src/ps2port.c
+++ b/src/ps2port.c
@@ -136,6 +136,21 @@ i8042_aux_write(u8 c)
 #define PS2_RET_ACK             0xfa
 #define PS2_RET_NAK             0xfe
 
+static void
+process_ps2byte(u8 status, u8 data)
+{
+    if (!MODE16) {
+        // Don't pull in all of keyboard/mouse code into 32bit code -
+        // just discard the data.
+        dprintf(1, "Discarding ps2 data %x (status=%x)\n", data, status);
+        return;
+    }
+    if (status & I8042_STR_AUXDATA)
+        process_mouse(data);
+    else
+        process_key(data);
+}
+
 static int
 ps2_recvbyte(int aux, int needack, int timeout)
 {
@@ -157,8 +172,8 @@ ps2_recvbyte(int aux, int needack, int timeout)
                 }
             }
 
-            // This data not for us - XXX - just discard it for now.
-            dprintf(1, "Discarding ps2 data %x (status=%x)\n", data, status);
+            // Data not part of this command.
+            process_ps2byte(status, data);
         }
 
         if (check_time(end)) {
@@ -288,6 +303,19 @@ aux_command(int command, u8 *param)
  * IRQ handlers
  ****************************************************************/
 
+static void
+process_ps2irq()
+{
+    u8 status = inb(PORT_PS2_STATUS);
+    if (!(status & I8042_STR_OBF)) {
+        dprintf(1, "ps2 irq but no data.\n");
+        return;
+    }
+    u8 data = inb(PORT_PS2_DATA);
+
+    process_ps2byte(status, data);
+}
+
 // INT74h : PS/2 mouse hardware interrupt
 void VISIBLE16
 handle_74()
@@ -296,18 +324,7 @@ handle_74()
         return;
 
     debug_isr(DEBUG_ISR_74);
-
-    u8 v = inb(PORT_PS2_STATUS);
-    if ((v & (I8042_STR_OBF|I8042_STR_AUXDATA))
-        != (I8042_STR_OBF|I8042_STR_AUXDATA)) {
-        dprintf(1, "mouse irq but no mouse data.\n");
-        goto done;
-    }
-    v = inb(PORT_PS2_DATA);
-
-    process_mouse(v);
-
-done:
+    process_ps2irq();
     eoi_pic2();
 }
 
@@ -319,18 +336,7 @@ handle_09()
         return;
 
     debug_isr(DEBUG_ISR_09);
-
-    // read key from keyboard controller
-    u8 v = inb(PORT_PS2_STATUS);
-    if ((v & (I8042_STR_OBF|I8042_STR_AUXDATA)) != I8042_STR_OBF) {
-        dprintf(1, "keyboard irq but no keyboard data.\n");
-        goto done;
-    }
-    v = inb(PORT_PS2_DATA);
-
-    process_key(v);
-
-done:
+    process_ps2irq();
     eoi_pic1();
 }