Make sure ps2 port command reads are from the desired device.

Discard reads from real-time events or from a different device.
Also, improve the kbd/mouse diagnostic messages.

3 files changed, 49 insertions, 24 deletions

diff --git a/src/kbd.c b/src/kbd.c
index f171cf3..a88f1ea 100644
--- a/src/kbd.c
+++ b/src/kbd.c
@@ -617,8 +617,8 @@ handle_09()
 
     // read key from keyboard controller
     u8 v = inb(PORT_PS2_STATUS);
-    if ((v & 0x21) != 0x01) {
-        dprintf(1, "int09 but no keyboard data.\n");
+    if ((v & (I8042_STR_OBF|I8042_STR_AUXDATA)) != I8042_STR_OBF) {
+        dprintf(1, "keyboard irq but no keyboard data.\n");
         goto done;
     }
     u8 key = inb(PORT_PS2_DATA);
diff --git a/src/mouse.c b/src/mouse.c
index 65baa20..b57a7e7 100644
--- a/src/mouse.c
+++ b/src/mouse.c
@@ -111,7 +111,7 @@ mouse_15c201(struct bregs *regs)
 static void
 mouse_15c202(struct bregs *regs)
 {
-    static u8 sample_rates[7] = {10, 20, 40, 60, 80, 100, 200};
+    static u8 VAR16 sample_rates[7] = {10, 20, 40, 60, 80, 100, 200};
     if (regs->bh >= ARRAY_SIZE(sample_rates)) {
         set_code_fail(regs, RET_EINVINPUT);
         return;
@@ -285,15 +285,8 @@ handle_15c2(struct bregs *regs)
 }
 
 static void
-int74_function()
+process_mouse(u8 data)
 {
-    u8 v = inb(PORT_PS2_STATUS);
-    if ((v & 0x21) != 0x21) {
-        dprintf(1, "int74 but no mouse data.\n");
-        return;
-    }
-    v = inb(PORT_PS2_DATA);
-
     u16 ebda_seg = get_ebda_seg();
     u8 mouse_flags_1 = GET_EBDA2(ebda_seg, mouse_flag1);
     u8 mouse_flags_2 = GET_EBDA2(ebda_seg, mouse_flag2);
@@ -304,7 +297,7 @@ int74_function()
 
     u8 package_count = mouse_flags_2 & 0x07;
     u8 index = mouse_flags_1 & 0x07;
-    SET_EBDA2(ebda_seg, mouse_data[index], v);
+    SET_EBDA2(ebda_seg, mouse_data[index], data);
 
     if ((index+1) < package_count) {
         mouse_flags_1++;
@@ -345,7 +338,15 @@ handle_74()
     if (! CONFIG_PS2_MOUSE)
         goto done;
 
-    int74_function();
+    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:
     eoi_pic2();
diff --git a/src/ps2port.c b/src/ps2port.c
index c9c3046..ed401bf 100644
--- a/src/ps2port.c
+++ b/src/ps2port.c
@@ -64,7 +64,8 @@ i8042_flush(void)
             return 0;
         }
         udelay(50);
-        inb(PORT_PS2_DATA);
+        u8 data = inb(PORT_PS2_DATA);
+        dprintf(7, "i8042 flushed %x\n", data);
     }
 
     irq_restore(flags);
@@ -99,6 +100,7 @@ __i8042_command(int command, u8 *param)
         if (ret)
             return ret;
         param[i] = inb(PORT_PS2_DATA);
+        dprintf(7, "i8042 param=%x\n", param[i]);
     }
 
     return 0;
@@ -146,6 +148,33 @@ i8042_aux_write(u8 c)
 #define PS2_RET_NAK             0xfe
 
 static int
+ps2_recvbyte(int aux, int needack, int timeout)
+{
+    u64 end = calc_future_tsc(timeout);
+    for (;;) {
+        if (rdtscll() >= end) {
+            dprintf(1, "ps2_recvbyte timeout\n");
+            return -1;
+        }
+
+        u8 status = inb(PORT_PS2_STATUS);
+        if (! (status & I8042_STR_OBF))
+            continue;
+        u8 data = inb(PORT_PS2_DATA);
+        dprintf(7, "ps2 read %x\n", data);
+
+        if ((!!(status & I8042_STR_AUXDATA) != aux)
+            || (needack && data != PS2_RET_ACK)) {
+            // This data not for us - XXX - just discard it for now.
+            dprintf(1, "Discarding ps2 data %x\n", data);
+            continue;
+        }
+
+        return data;
+    }
+}
+
+static int
 ps2_sendbyte(int aux, u8 command)
 {
     dprintf(7, "ps2_sendbyte aux=%d cmd=%x\n", aux, command);
@@ -158,14 +187,9 @@ ps2_sendbyte(int aux, u8 command)
         return ret;
 
     // Read ack.
-    ret = i8042_wait_read();
-    if (ret)
+    ret = ps2_recvbyte(aux, 1, 200);
+    if (ret < 0)
         return ret;
-    u8 ack = inb(PORT_PS2_DATA);
-    if (ack != PS2_RET_ACK) {
-        dprintf(1, "Missing ack (got %x not %x)\n", ack, PS2_RET_ACK);
-        return -1;
-    }
 
     return 0;
 }
@@ -205,14 +229,14 @@ ps2_command(int aux, int command, u8 *param)
 
     // Receive parameters (if any).
     for (i = 0; i < receive; i++) {
-        ret = i8042_wait_read();
-        if (ret) {
+        u8 data = ps2_recvbyte(aux, 0, 200);
+        if (data < 0) {
             // On a receive timeout, return the item number that the
             // transfer failed on.
             ret = i + 1;
             goto fail;
         }
-        param[i] = inb(PORT_PS2_DATA);
+        param[i] = data;
     }
 
 fail: