From 61c0999d57a8dca08298c60d8d2fe07b9b040b6b Mon Sep 17 00:00:00 2001
From: Cacodemon345 <wahil1976@outlook.com>
Date: Sat, 29 Apr 2023 22:43:29 +0600
Subject: [PATCH 1/4] OHCI: Replace magic numbers with named enums

---
 src/usb.c | 216 +++++++++++++++++++++++++++++-------------------------
 1 file changed, 118 insertions(+), 98 deletions(-)

diff --git a/src/usb.c b/src/usb.c
index 75e60d438..eecb2902c 100644
--- a/src/usb.c
+++ b/src/usb.c
@@ -131,6 +131,35 @@ uhci_update_io_mapping(usb_t *dev, uint8_t base_l, uint8_t base_h, int enable)
         io_sethandler(dev->uhci_io_base, 0x20, uhci_reg_read, NULL, NULL, uhci_reg_write, uhci_reg_writew, NULL, dev);
 }
 
+/* OHCI registers */
+enum
+{
+    OHCI_HcRevision = 0x00,
+    OHCI_HcControl = 0x04,
+    OHCI_HcCommandStatus = 0x08,
+    OHCI_HcInterruptStatus = 0x0C,
+    OHCI_HcInterruptEnable = 0x10,
+    OHCI_HcInterruptDisable = 0x14,
+    OHCI_HcHCCA = 0x18,
+    OHCI_HcPeriodCurrentED = 0x1C,
+    OHCI_HcControlHeadED = 0x20,
+    OHCI_HcControlCurrentED = 0x24,
+    OHCI_HcBulkHeadED = 0x28,
+    OHCI_HcBulkCurrentED = 0x2C,
+    OHCI_HcDoneHead = 0x30,
+    OHCI_HcFMInterval = 0x34,
+    OHCI_HcFmRemaining = 0x38,
+    OHCI_HcFmNumber = 0x3C,
+    OHCI_HcPeriodicStart = 0x40,
+    OHCI_HcLSThreshold = 0x44,
+    OHCI_HcRhDescriptorA = 0x48,
+    OHCI_HcRhDescriptorB = 0x4C,
+    OHCI_HcRhStatus = 0x50,
+    OHCI_HcRhPortStatus1 = 0x54,
+    OHCI_HcRhPortStatus2 = 0x58,
+    OHCI_HcRhPortStatus3 = 0x5C
+};
+
 static uint8_t
 ohci_mmio_read(uint32_t addr, void *p)
 {
@@ -156,132 +185,132 @@ ohci_mmio_write(uint32_t addr, uint8_t val, void *p)
     addr &= 0x00000fff;
 
     switch (addr) {
-        case 0x04:
+        case OHCI_HcControl:
             if ((val & 0xc0) == 0x00) {
                 /* UsbReset */
-                dev->ohci_mmio[0x56] = dev->ohci_mmio[0x5a] = 0x16;
+                dev->ohci_mmio[OHCI_HcRhPortStatus1 + 2] = dev->ohci_mmio[OHCI_HcRhPortStatus2 + 2] = 0x16;
             }
             break;
-        case 0x08: /* HCCOMMANDSTATUS */
+        case OHCI_HcCommandStatus:
             /* bit OwnershipChangeRequest triggers an ownership change (SMM <-> OS) */
             if (val & 0x08) {
-                dev->ohci_mmio[0x0f] = 0x40;
-                if ((dev->ohci_mmio[0x13] & 0xc0) == 0xc0)
+                dev->ohci_mmio[OHCI_HcInterruptStatus + 3] = 0x40;
+                if ((dev->ohci_mmio[OHCI_HcInterruptEnable + 3] & 0xc0) == 0xc0)
                     smi_raise();
             }
 
             /* bit HostControllerReset must be cleared for the controller to be seen as initialized */
             if (val & 0x01) {
                 memset(dev->ohci_mmio, 0x00, 4096);
-                dev->ohci_mmio[0x00] = 0x10;
-                dev->ohci_mmio[0x01] = 0x01;
-                dev->ohci_mmio[0x48] = 0x02;
+                dev->ohci_mmio[OHCI_HcRevision] = 0x10;
+                dev->ohci_mmio[OHCI_HcRevision + 1] = 0x01;
+                dev->ohci_mmio[OHCI_HcRhDescriptorA] = 0x02;
                 val &= ~0x01;
             }
             break;
-        case 0x0c:
+        case OHCI_HcInterruptStatus:
             dev->ohci_mmio[addr] &= ~(val & 0x7f);
             return;
-        case 0x0d:
-        case 0x0e:
+        case OHCI_HcInterruptStatus + 1:
+        case OHCI_HcInterruptStatus + 2:
             return;
-        case 0x0f:
+        case OHCI_HcInterruptStatus + 3:
             dev->ohci_mmio[addr] &= ~(val & 0x40);
             return;
-        case 0x3b:
+        case OHCI_HcFmRemaining + 3:
             dev->ohci_mmio[addr] = (val & 0x80);
             return;
-        case 0x39:
-        case 0x41:
+        case OHCI_HcFmRemaining + 1:
+        case OHCI_HcPeriodicStart + 1:
             dev->ohci_mmio[addr] = (val & 0x3f);
             return;
-        case 0x45:
+        case OHCI_HcLSThreshold + 1:
             dev->ohci_mmio[addr] = (val & 0x0f);
             return;
-        case 0x3a:
-        case 0x3e:
-        case 0x3f:
-        case 0x42:
-        case 0x43:
-        case 0x46:
-        case 0x47:
-        case 0x48:
-        case 0x4a:
+        case OHCI_HcFmRemaining + 2:
+        case OHCI_HcFmNumber + 2:
+        case OHCI_HcFmNumber + 3:
+        case OHCI_HcPeriodicStart + 2:
+        case OHCI_HcPeriodicStart + 3:
+        case OHCI_HcLSThreshold + 2:
+        case OHCI_HcLSThreshold + 3:
+        case OHCI_HcRhDescriptorA:
+        case OHCI_HcRhDescriptorA + 2:
             return;
-        case 0x49:
+        case OHCI_HcRhDescriptorA + 1:
             dev->ohci_mmio[addr] = (val & 0x1b);
             if (val & 0x02) {
-                dev->ohci_mmio[0x55] |= 0x01;
-                dev->ohci_mmio[0x59] |= 0x01;
+                dev->ohci_mmio[OHCI_HcRhPortStatus1 + 1] |= 0x01;
+                dev->ohci_mmio[OHCI_HcRhPortStatus2 + 1] |= 0x01;
             }
             return;
-        case 0x4b:
+        case OHCI_HcRhDescriptorA + 3:
             dev->ohci_mmio[addr] = (val & 0x03);
             return;
-        case 0x4c:
-        case 0x4e:
+        case OHCI_HcRhDescriptorB:
+        case OHCI_HcRhDescriptorB + 2:
             dev->ohci_mmio[addr] = (val & 0x06);
-            if ((addr == 0x4c) && !(val & 0x04)) {
-                if (!(dev->ohci_mmio[0x58] & 0x01))
-                    dev->ohci_mmio[0x5a] |= 0x01;
-                dev->ohci_mmio[0x58] |= 0x01;
+            if ((addr == OHCI_HcRhDescriptorB) && !(val & 0x04)) {
+                if (!(dev->ohci_mmio[OHCI_HcRhPortStatus2] & 0x01))
+                    dev->ohci_mmio[OHCI_HcRhPortStatus2 + 2] |= 0x01;
+                dev->ohci_mmio[OHCI_HcRhPortStatus2] |= 0x01;
             }
-            if ((addr == 0x4c) && !(val & 0x02)) {
-                if (!(dev->ohci_mmio[0x54] & 0x01))
-                    dev->ohci_mmio[0x56] |= 0x01;
-                dev->ohci_mmio[0x54] |= 0x01;
+            if ((addr == OHCI_HcRhDescriptorB) && !(val & 0x02)) {
+                if (!(dev->ohci_mmio[OHCI_HcRhPortStatus1] & 0x01))
+                    dev->ohci_mmio[OHCI_HcRhPortStatus1 + 2] |= 0x01;
+                dev->ohci_mmio[OHCI_HcRhPortStatus1] |= 0x01;
             }
             return;
-        case 0x4d:
-        case 0x4f:
+        case OHCI_HcRhDescriptorB + 1:
+        case OHCI_HcRhDescriptorB + 3:
             return;
-        case 0x50:
+        case OHCI_HcRhStatus:
             if (val & 0x01) {
-                if ((dev->ohci_mmio[0x49] & 0x03) == 0x00) {
-                    dev->ohci_mmio[0x55] &= ~0x01;
-                    dev->ohci_mmio[0x54] &= ~0x17;
-                    dev->ohci_mmio[0x56] &= ~0x17;
-                    dev->ohci_mmio[0x59] &= ~0x01;
-                    dev->ohci_mmio[0x58] &= ~0x17;
-                    dev->ohci_mmio[0x5a] &= ~0x17;
-                } else if ((dev->ohci_mmio[0x49] & 0x03) == 0x01) {
-                    if (!(dev->ohci_mmio[0x4e] & 0x02)) {
-                        dev->ohci_mmio[0x55] &= ~0x01;
-                        dev->ohci_mmio[0x54] &= ~0x17;
-                        dev->ohci_mmio[0x56] &= ~0x17;
+                if ((dev->ohci_mmio[OHCI_HcRhDescriptorA + 1] & 0x03) == 0x00) {
+                    dev->ohci_mmio[OHCI_HcRhPortStatus1 + 1] &= ~0x01;
+                    dev->ohci_mmio[OHCI_HcRhPortStatus1] &= ~0x17;
+                    dev->ohci_mmio[OHCI_HcRhPortStatus1 + 2] &= ~0x17;
+                    dev->ohci_mmio[OHCI_HcRhPortStatus2 + 1] &= ~0x01;
+                    dev->ohci_mmio[OHCI_HcRhPortStatus2] &= ~0x17;
+                    dev->ohci_mmio[OHCI_HcRhPortStatus2 + 2] &= ~0x17;
+                } else if ((dev->ohci_mmio[OHCI_HcRhDescriptorA + 1] & 0x03) == 0x01) {
+                    if (!(dev->ohci_mmio[OHCI_HcRhDescriptorB + 2] & 0x02)) {
+                        dev->ohci_mmio[OHCI_HcRhPortStatus1 + 1] &= ~0x01;
+                        dev->ohci_mmio[OHCI_HcRhPortStatus1] &= ~0x17;
+                        dev->ohci_mmio[OHCI_HcRhPortStatus1 + 2] &= ~0x17;
                     }
-                    if (!(dev->ohci_mmio[0x4e] & 0x04)) {
-                        dev->ohci_mmio[0x59] &= ~0x01;
-                        dev->ohci_mmio[0x58] &= ~0x17;
-                        dev->ohci_mmio[0x5a] &= ~0x17;
+                    if (!(dev->ohci_mmio[OHCI_HcRhDescriptorB + 2] & 0x04)) {
+                        dev->ohci_mmio[OHCI_HcRhPortStatus2 + 1] &= ~0x01;
+                        dev->ohci_mmio[OHCI_HcRhPortStatus2] &= ~0x17;
+                        dev->ohci_mmio[OHCI_HcRhPortStatus2 + 2] &= ~0x17;
                     }
                 }
             }
             return;
-        case 0x51:
+        case OHCI_HcRhStatus + 1:
             if (val & 0x80)
                 dev->ohci_mmio[addr] |= 0x80;
             return;
-        case 0x52:
+        case OHCI_HcRhStatus + 2:
             dev->ohci_mmio[addr] &= ~(val & 0x02);
             if (val & 0x01) {
-                if ((dev->ohci_mmio[0x49] & 0x03) == 0x00) {
-                    dev->ohci_mmio[0x55] |= 0x01;
-                    dev->ohci_mmio[0x59] |= 0x01;
-                } else if ((dev->ohci_mmio[0x49] & 0x03) == 0x01) {
-                    if (!(dev->ohci_mmio[0x4e] & 0x02))
-                        dev->ohci_mmio[0x55] |= 0x01;
-                    if (!(dev->ohci_mmio[0x4e] & 0x04))
-                        dev->ohci_mmio[0x59] |= 0x01;
+                if ((dev->ohci_mmio[OHCI_HcRhDescriptorA + 1] & 0x03) == 0x00) {
+                    dev->ohci_mmio[OHCI_HcRhPortStatus1 + 1] |= 0x01;
+                    dev->ohci_mmio[OHCI_HcRhPortStatus2 + 1] |= 0x01;
+                } else if ((dev->ohci_mmio[OHCI_HcRhDescriptorA + 1] & 0x03) == 0x01) {
+                    if (!(dev->ohci_mmio[OHCI_HcRhDescriptorB + 2] & 0x02))
+                        dev->ohci_mmio[OHCI_HcRhPortStatus1 + 1] |= 0x01;
+                    if (!(dev->ohci_mmio[OHCI_HcRhDescriptorB + 2] & 0x04))
+                        dev->ohci_mmio[OHCI_HcRhPortStatus2 + 1] |= 0x01;
                 }
             }
             return;
-        case 0x53:
+        case OHCI_HcRhStatus + 3:
             if (val & 0x80)
-                dev->ohci_mmio[0x51] &= ~0x80;
+                dev->ohci_mmio[OHCI_HcRhStatus + 1] &= ~0x80;
             return;
-        case 0x54:
-        case 0x58:
+        case OHCI_HcRhPortStatus1:
+        case OHCI_HcRhPortStatus2:
             old = dev->ohci_mmio[addr];
 
             if (val & 0x10) {
@@ -315,30 +344,30 @@ ohci_mmio_write(uint32_t addr, uint8_t val, void *p)
             /* if (!(dev->ohci_mmio[addr] & 0x02))
                     dev->ohci_mmio[addr + 2] |= 0x02; */
             return;
-        case 0x55:
-            if ((val & 0x02) && ((dev->ohci_mmio[0x49] & 0x03) == 0x00) && (dev->ohci_mmio[0x4e] & 0x02)) {
+        case OHCI_HcRhPortStatus1 + 1:
+            if ((val & 0x02) && ((dev->ohci_mmio[OHCI_HcRhDescriptorA + 1] & 0x03) == 0x00) && (dev->ohci_mmio[OHCI_HcRhDescriptorB + 2] & 0x02)) {
                 dev->ohci_mmio[addr] &= ~0x01;
-                dev->ohci_mmio[0x54] &= ~0x17;
-                dev->ohci_mmio[0x56] &= ~0x17;
+                dev->ohci_mmio[OHCI_HcRhPortStatus1] &= ~0x17;
+                dev->ohci_mmio[OHCI_HcRhPortStatus1 + 2] &= ~0x17;
             }
-            if ((val & 0x01) && ((dev->ohci_mmio[0x49] & 0x03) == 0x00) && (dev->ohci_mmio[0x4e] & 0x02)) {
+            if ((val & 0x01) && ((dev->ohci_mmio[OHCI_HcRhDescriptorA + 1] & 0x03) == 0x00) && (dev->ohci_mmio[OHCI_HcRhDescriptorB + 2] & 0x02)) {
                 dev->ohci_mmio[addr] |= 0x01;
-                dev->ohci_mmio[0x58] &= ~0x17;
-                dev->ohci_mmio[0x5a] &= ~0x17;
+                dev->ohci_mmio[OHCI_HcRhPortStatus2] &= ~0x17;
+                dev->ohci_mmio[OHCI_HcRhPortStatus2 + 2] &= ~0x17;
             }
             return;
-        case 0x59:
-            if ((val & 0x02) && ((dev->ohci_mmio[0x49] & 0x03) == 0x00) && (dev->ohci_mmio[0x4e] & 0x04))
+        case OHCI_HcRhPortStatus2 + 1:
+            if ((val & 0x02) && ((dev->ohci_mmio[OHCI_HcRhDescriptorA + 1] & 0x03) == 0x00) && (dev->ohci_mmio[OHCI_HcRhDescriptorB + 2] & 0x04))
                 dev->ohci_mmio[addr] &= ~0x01;
-            if ((val & 0x01) && ((dev->ohci_mmio[0x49] & 0x03) == 0x00) && (dev->ohci_mmio[0x4e] & 0x04))
+            if ((val & 0x01) && ((dev->ohci_mmio[OHCI_HcRhDescriptorA + 1] & 0x03) == 0x00) && (dev->ohci_mmio[OHCI_HcRhDescriptorB + 2] & 0x04))
                 dev->ohci_mmio[addr] |= 0x01;
             return;
-        case 0x56:
-        case 0x5a:
+        case OHCI_HcRhPortStatus1 + 2:
+        case OHCI_HcRhPortStatus2 + 2:
             dev->ohci_mmio[addr] &= ~(val & 0x1f);
             return;
-        case 0x57:
-        case 0x5b:
+        case OHCI_HcRhPortStatus1 + 3:
+        case OHCI_HcRhPortStatus2 + 3:
             return;
     }
 
@@ -368,9 +397,9 @@ usb_reset(void *priv)
     dev->uhci_io[0x10] = dev->uhci_io[0x12] = 0x80;
 
     memset(dev->ohci_mmio, 0x00, 4096);
-    dev->ohci_mmio[0x00] = 0x10;
-    dev->ohci_mmio[0x01] = 0x01;
-    dev->ohci_mmio[0x48] = 0x02;
+    dev->ohci_mmio[OHCI_HcRevision] = 0x10;
+    dev->ohci_mmio[OHCI_HcRevision + 1] = 0x01;
+    dev->ohci_mmio[OHCI_HcRhDescriptorA] = 0x02;
 
     io_removehandler(dev->uhci_io_base, 0x20, uhci_reg_read, NULL, NULL, uhci_reg_write, uhci_reg_writew, NULL, dev);
     dev->uhci_enable = 0;
@@ -397,15 +426,6 @@ usb_init(const device_t *info)
         return (NULL);
     memset(dev, 0x00, sizeof(usb_t));
 
-    memset(dev->uhci_io, 0x00, 128);
-    dev->uhci_io[0x0c] = 0x40;
-    dev->uhci_io[0x10] = dev->uhci_io[0x12] = 0x80;
-
-    memset(dev->ohci_mmio, 0x00, 4096);
-    dev->ohci_mmio[0x00] = 0x10;
-    dev->ohci_mmio[0x01] = 0x01;
-    dev->ohci_mmio[0x48] = 0x02;
-
     mem_mapping_add(&dev->ohci_mmio_mapping, 0, 0,
                     ohci_mmio_read, NULL, NULL,
                     ohci_mmio_write, NULL, NULL,

From 7e90e9215aebd3f87ed7d10c30179f21c96d2eef Mon Sep 17 00:00:00 2001
From: Cacodemon345 <wahil1976@outlook.com>
Date: Sun, 30 Apr 2023 00:59:11 +0600
Subject: [PATCH 2/4] OHCI: HcHCCA's lower 8 bits are always zero

Start of work on USB endpoint device infrastructure
---
 src/include/86box/usb.h | 8 ++++++++
 src/usb.c               | 2 ++
 2 files changed, 10 insertions(+)

diff --git a/src/include/86box/usb.h b/src/include/86box/usb.h
index d0b169b6c..ab0549844 100644
--- a/src/include/86box/usb.h
+++ b/src/include/86box/usb.h
@@ -22,6 +22,7 @@
 extern "C" {
 #endif
 
+/* USB Host Controller device struct */
 typedef struct
 {
     uint8_t       uhci_io[32], ohci_mmio[4096];
@@ -31,6 +32,13 @@ typedef struct
     mem_mapping_t ohci_mmio_mapping;
 } usb_t;
 
+/* USB endpoint device struct. Incomplete and unused. */
+typedef struct
+{
+    uint16_t vendor_id;
+    uint16_t device_id;
+} usb_device_t;
+
 /* Global variables. */
 extern const device_t usb_device;
 
diff --git a/src/usb.c b/src/usb.c
index eecb2902c..e9702afa7 100644
--- a/src/usb.c
+++ b/src/usb.c
@@ -208,6 +208,8 @@ ohci_mmio_write(uint32_t addr, uint8_t val, void *p)
                 val &= ~0x01;
             }
             break;
+        case OHCI_HcHCCA:
+            return;
         case OHCI_HcInterruptStatus:
             dev->ohci_mmio[addr] &= ~(val & 0x7f);
             return;

From 3ea8a9607fdae391a81ba9da5848c728093a3420 Mon Sep 17 00:00:00 2001
From: OBattler <oubattler@gmail.com>
Date: Sun, 30 Apr 2023 00:32:50 +0200
Subject: [PATCH 3/4] Replaced my implmenetation of FXTRACT with TC1995's.

---
 src/cpu/x87_ops_misc.h | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/src/cpu/x87_ops_misc.h b/src/cpu/x87_ops_misc.h
index 3a528f847..21fd4c084 100644
--- a/src/cpu/x87_ops_misc.h
+++ b/src/cpu/x87_ops_misc.h
@@ -36,12 +36,19 @@ opFNOP(uint32_t fetchdat)
 static int
 opFXTRACT(uint32_t fetchdat)
 {
-    double_decompose_t temp = (double_decompose_t) ST(0);
+    x87_conv_t test;
+    int64_t exp80, exp80final;
+    double mant;
 
     FP_ENTER();
     cpu_state.pc++;
-    ST(0) = (double) temp.exponent;
-    x87_push((double) temp.mantissa);
+    test.eind.d = ST(0);
+    exp80 = test.eind.ll & (0x7ff0000000000000ll);
+    exp80final = (exp80 >> 52) - BIAS64;
+    mant = test.eind.d / (pow(2.0, (double)exp80final));
+    ST(0) = (double)exp80final;
+    FP_TAG_VALID;
+    x87_push(mant);
     CLOCK_CYCLES_FPU((fpu_type >= FPU_487SX) ? (x87_timings.fxtract) : (x87_timings.fxtract * cpu_multi));
     CONCURRENCY_CYCLES((fpu_type >= FPU_487SX) ? (x87_concurrency.fxtract) : (x87_concurrency.fxtract * cpu_multi));
     return 0;

From 1eaa3830d341861321c8b2b286a74c4bc4cb4aa2 Mon Sep 17 00:00:00 2001
From: OBattler <oubattler@gmail.com>
Date: Sun, 30 Apr 2023 17:13:03 +0200
Subject: [PATCH 4/4] KBC command output is now correctly two-phased like in
 the M&KB branch, fixes keyboard input on the Intel Advanced/EV with no PS/2
 mouse.

---
 src/device/kbc_at.c | 189 ++++++++++++++++++++++++++++----------------
 1 file changed, 120 insertions(+), 69 deletions(-)

diff --git a/src/device/kbc_at.c b/src/device/kbc_at.c
index 24121e699..b3b5c5784 100644
--- a/src/device/kbc_at.c
+++ b/src/device/kbc_at.c
@@ -88,24 +88,27 @@
 #define FLAG_PCI           0x08
 
 enum {
-    STATE_RESET = 0,
-    STATE_MAIN_IBF,
-    STATE_MAIN_KBD,
-    STATE_MAIN_AUX,
-    STATE_MAIN_BOTH,
-    STATE_KBC_OUT,
-    STATE_KBC_PARAM,
-    STATE_SEND_KBD,
-    STATE_SCAN_KBD,
-    STATE_SEND_AUX,
-    STATE_SCAN_AUX
+    STATE_RESET = 0,       /* KBC reset state, only accepts command AA. */
+    STATE_KBC_DELAY_OUT,   /* KBC is sending one single byte. */
+    STATE_KBC_AMI_OUT,     /* KBC waiting for OBF - needed for AMIKey commands that require clearing of the output byte. */
+    STATE_MAIN_IBF,        /* KBC checking if the input buffer is full. */
+    STATE_MAIN_KBD,        /* KBC checking if the keyboard has anything to send. */
+    STATE_MAIN_AUX,        /* KBC checking if the auxiliary has anything to send. */
+    STATE_MAIN_BOTH,       /* KBC checking if either device has anything to send. */
+    STATE_KBC_OUT,         /* KBC is sending multiple bytes. */
+    STATE_KBC_PARAM,       /* KBC wants a parameter. */
+    STATE_SEND_KBD,        /* KBC is sending command to the keyboard. */
+    STATE_SCAN_KBD,        /* KBC is waiting for the keyboard command response. */
+    STATE_SEND_AUX,        /* KBC is sending command to the auxiliary device. */
+    STATE_SCAN_AUX         /* KBC is waiting for the auxiliary command response. */
 };
 
 typedef struct {
     uint8_t state, command, command_phase, status,
             wantdata, ib, ob, sc_or,
             mem_addr, p1, p2, old_p2,
-            misc_flags, ami_flags, key_ctrl_queue_start, key_ctrl_queue_end;
+            misc_flags, ami_flags, key_ctrl_queue_start, key_ctrl_queue_end,
+            val, channel, stat_hi, pending;
 
     uint8_t mem[0x100];
 
@@ -202,14 +205,15 @@ kbc_at_queue_add(atkbc_t *dev, uint8_t val)
     kbc_at_log("ATkbc: dev->key_ctrl_queue[%02X] = %02X;\n", dev->key_ctrl_queue_end, val);
     dev->key_ctrl_queue[dev->key_ctrl_queue_end] = val;
     dev->key_ctrl_queue_end                 = (dev->key_ctrl_queue_end + 1) & 0x3f;
+    dev->state = STATE_KBC_OUT;
 }
 
 static int
 kbc_translate(atkbc_t *dev, uint8_t val)
 {
-    /* TODO: Does the IBM AT keyboard controller firmware apply translation in XT mode or not? */
     int      xt_mode   = (dev->mem[0x20] & 0x20) && !(dev->misc_flags & FLAG_PS2);
-    int      translate = (dev->mem[0x20] & 0x40) || xt_mode || ((dev->flags & KBC_TYPE_MASK) == KBC_TYPE_PS2_2);
+    /* The IBM AT keyboard controller firmware does not apply translation in XT mode. */
+    int      translate = !xt_mode && ((dev->mem[0x20] & 0x40) || ((dev->flags & KBC_TYPE_MASK) == KBC_TYPE_PS2_2));
     uint8_t  kbc_ven   = dev->flags & KBC_VEN_MASK;
     int      ret       = - 1;
 
@@ -298,7 +302,7 @@ kbc_translate(atkbc_t *dev, uint8_t val)
 }
 
 static void
-add_to_kbc_queue_front(atkbc_t *dev, uint8_t val, uint8_t channel, uint8_t stat_hi)
+kbc_send_to_ob(atkbc_t *dev, uint8_t val, uint8_t channel, uint8_t stat_hi)
 {
     uint8_t kbc_ven = dev->flags & KBC_VEN_MASK;
     int temp = (channel == 1) ? kbc_translate(dev, val) : val;
@@ -312,7 +316,7 @@ add_to_kbc_queue_front(atkbc_t *dev, uint8_t val, uint8_t channel, uint8_t stat_
     else
         stat_hi |= 0x10;
 
-    kbc_at_log("ATkbc: Adding %02X to front on channel %i...\n", temp, channel);
+    kbc_at_log("ATkbc: Sending %02X to the output buffer on channel %i...\n", temp, channel);
     dev->status = (dev->status & ~0xf0) | STAT_OFULL | stat_hi;
 
     /* WARNING: On PS/2, all IRQ's are level-triggered, but the IBM PS/2 KBC firmware is explicitly
@@ -335,6 +339,16 @@ add_to_kbc_queue_front(atkbc_t *dev, uint8_t val, uint8_t channel, uint8_t stat_
     dev->ob = temp;
 }
 
+static void
+kbc_delay_to_ob(atkbc_t *dev, uint8_t val, uint8_t channel, uint8_t stat_hi)
+{
+    dev->val = val;
+    dev->channel = channel;
+    dev->stat_hi = stat_hi;
+    dev->pending = 1;
+    dev->state = STATE_KBC_DELAY_OUT;
+}
+
 static void kbc_at_process_cmd(void *priv);
 
 static void
@@ -366,7 +380,7 @@ kbc_ibf_process(atkbc_t *dev)
             dev->ports[0]->dat = dev->ib;
             dev->state         = STATE_SEND_KBD;
         } else
-            add_to_kbc_queue_front(dev, 0xfe, 1, 0x40);
+            kbc_delay_to_ob(dev, 0xfe, 1, 0x40);
     }
 }
 
@@ -378,7 +392,7 @@ kbc_scan_kbd_at(atkbc_t *dev)
         /* XT mode. */
         if (dev->mem[0x20] & 0x20) {
             if ((dev->ports[0] != NULL) && (dev->ports[0]->out_new != -1)) {
-                add_to_kbc_queue_front(dev, dev->ports[0]->out_new, 1, 0x00);
+                kbc_send_to_ob(dev, dev->ports[0]->out_new, 1, 0x00);
                 dev->ports[0]->out_new = -1;
                 dev->state             = STATE_MAIN_IBF;
             } else if (dev->status & STAT_IFULL)
@@ -397,10 +411,10 @@ kbc_scan_kbd_at(atkbc_t *dev)
                 /* Read data from the keyboard. */
                 if (dev->mem[0x20] & 0x40) {
                     if ((dev->mem[0x20] & 0x08) || (dev->p1 & 0x80))
-                        add_to_kbc_queue_front(dev, dev->ports[0]->out_new, 1, 0x00);
+                        kbc_send_to_ob(dev, dev->ports[0]->out_new, 1, 0x00);
                     dev->mem[0x2d] = (dev->ports[0]->out_new == 0xf0) ? 0x80 : 0x00;
                 } else
-                    add_to_kbc_queue_front(dev, dev->ports[0]->out_new, 1, 0x00);
+                    kbc_send_to_ob(dev, dev->ports[0]->out_new, 1, 0x00);
                 dev->ports[0]->out_new = -1;
                 dev->state             = STATE_MAIN_IBF;
             }
@@ -421,8 +435,13 @@ kbc_at_poll_at(atkbc_t *dev)
                     kbc_at_process_cmd(dev);
             }
             break;
+        case STATE_KBC_AMI_OUT:
+            if (dev->status & STAT_OFULL)
+                break;
+            /* FALLTHROUGH */
         case STATE_MAIN_IBF:
         default:
+at_main_ibf:
            if (dev->status & STAT_OFULL) {
                 /* OBF set, wait until it is cleared but still process commands. */
                 if ((dev->status & STAT_IFULL) && (dev->status & STAT_CD)) {
@@ -443,6 +462,15 @@ kbc_at_poll_at(atkbc_t *dev)
                 dev->state = STATE_MAIN_IBF;
             }
             break;
+        case STATE_KBC_DELAY_OUT:
+            /* Keyboard controller command want to output a single byte. */
+            kbc_at_log("ATkbc: %02X coming from channel %i with high status %02X\n", dev->val, dev->channel, dev->stat_hi);
+            kbc_send_to_ob(dev, dev->val, dev->channel, dev->stat_hi);
+            // dev->state = (dev->pending == 2) ? STATE_KBC_AMI_OUT : STATE_MAIN_IBF;
+            dev->state = STATE_MAIN_IBF;
+            dev->pending = 0;
+            goto at_main_ibf;
+            break;
         case STATE_KBC_OUT:
             /* Keyboard controller command want to output multiple bytes. */
             if (dev->status & STAT_IFULL) {
@@ -453,7 +481,7 @@ kbc_at_poll_at(atkbc_t *dev)
             /* Do not continue dumping until OBF is clear. */
             if (!(dev->status & STAT_OFULL)) {
                 kbc_at_log("ATkbc: %02X coming from channel 0\n", dev->key_ctrl_queue[dev->key_ctrl_queue_start]);
-                add_to_kbc_queue_front(dev, dev->key_ctrl_queue[dev->key_ctrl_queue_start], 0, 0x00);
+                kbc_send_to_ob(dev, dev->key_ctrl_queue[dev->key_ctrl_queue_start], 0, 0x00);
                 dev->key_ctrl_queue_start = (dev->key_ctrl_queue_start + 1) & 0x3f;
                 if (dev->key_ctrl_queue_start == dev->key_ctrl_queue_end)
                     dev->state = STATE_MAIN_IBF;
@@ -494,7 +522,7 @@ kbc_scan_kbd_ps2(atkbc_t *dev)
 {
     if ((dev->ports[0] != NULL) && (dev->ports[0]->out_new != -1)) {
         kbc_at_log("ATkbc: %02X coming from channel 1\n", dev->ports[0]->out_new & 0xff);
-        add_to_kbc_queue_front(dev, dev->ports[0]->out_new, 1, 0x00);
+        kbc_send_to_ob(dev, dev->ports[0]->out_new, 1, 0x00);
         dev->ports[0]->out_new = -1;
         dev->state             = STATE_MAIN_IBF;
         return 1;
@@ -508,7 +536,7 @@ kbc_scan_aux_ps2(atkbc_t *dev)
 {
     if ((dev->ports[1] != NULL) && (dev->ports[1]->out_new != -1)) {
         kbc_at_log("ATkbc: %02X coming from channel 2\n", dev->ports[1]->out_new & 0xff);
-        add_to_kbc_queue_front(dev, dev->ports[1]->out_new, 2, 0x00);
+        kbc_send_to_ob(dev, dev->ports[1]->out_new, 2, 0x00);
         dev->ports[1]->out_new = -1;
         dev->state             = STATE_MAIN_IBF;
         return 1;
@@ -528,8 +556,13 @@ kbc_at_poll_ps2(atkbc_t *dev)
                     kbc_at_process_cmd(dev);
             }
             break;
+        case STATE_KBC_AMI_OUT:
+            if (dev->status & STAT_OFULL)
+                break;
+            /* FALLTHROUGH */
         case STATE_MAIN_IBF:
         default:
+ps2_main_ibf:
             if (dev->status & STAT_IFULL)
                 kbc_ibf_process(dev);
             else if (!(dev->status & STAT_OFULL)) {
@@ -571,6 +604,15 @@ kbc_at_poll_ps2(atkbc_t *dev)
             else
                 dev->state = STATE_MAIN_AUX;
             break;
+        case STATE_KBC_DELAY_OUT:
+            /* Keyboard controller command want to output a single byte. */
+            kbc_at_log("ATkbc: %02X coming from channel %i with high status %02X\n", dev->val, dev->channel, dev->stat_hi);
+            kbc_send_to_ob(dev, dev->val, dev->channel, dev->stat_hi);
+            // dev->state = (dev->pending == 2) ? STATE_KBC_AMI_OUT : STATE_MAIN_IBF;
+            dev->state = STATE_MAIN_IBF;
+            dev->pending = 0;
+            goto ps2_main_ibf;
+            break;
         case STATE_KBC_OUT:
             /* Keyboard controller command want to output multiple bytes. */
             if (dev->status & STAT_IFULL) {
@@ -581,7 +623,7 @@ kbc_at_poll_ps2(atkbc_t *dev)
             /* Do not continue dumping until OBF is clear. */
             if (!(dev->status & STAT_OFULL)) {
                 kbc_at_log("ATkbc: %02X coming from channel 0\n", dev->key_ctrl_queue[dev->key_ctrl_queue_start] & 0xff);
-                add_to_kbc_queue_front(dev, dev->key_ctrl_queue[dev->key_ctrl_queue_start], 0, 0x00);
+                kbc_send_to_ob(dev, dev->key_ctrl_queue[dev->key_ctrl_queue_start], 0, 0x00);
                 dev->key_ctrl_queue_start = (dev->key_ctrl_queue_start + 1) & 0x3f;
                 if (dev->key_ctrl_queue_start == dev->key_ctrl_queue_end)
                     dev->state = STATE_MAIN_IBF;
@@ -761,7 +803,7 @@ write64_generic(void *priv, uint8_t val)
         case 0xa4: /* check if password installed */
             if (dev->misc_flags & FLAG_PS2) {
                 kbc_at_log("ATkbc: check if password installed\n");
-                add_to_kbc_queue_front(dev, 0xf1, 0, 0x00);
+                kbc_delay_to_ob(dev, 0xf1, 0, 0x00);
                 return 0;
             }
             break;
@@ -785,14 +827,14 @@ write64_generic(void *priv, uint8_t val)
         case 0xa9: /* Test auxiliary port */
             kbc_at_log("ATkbc: test auxiliary port\n");
             if (dev->misc_flags & FLAG_PS2) {
-                add_to_kbc_queue_front(dev, 0x00, 0, 0x00); /* no error, this is testing the channel 2 interface */
+                kbc_delay_to_ob(dev, 0x00, 0, 0x00); /* no error, this is testing the channel 2 interface */
                 return 0;
             }
             break;
 
         case 0xaf: /* read keyboard version */
             kbc_at_log("ATkbc: read keyboard version\n");
-            add_to_kbc_queue_front(dev, kbc_award_revision, 0, 0x00);
+            kbc_delay_to_ob(dev, kbc_award_revision, 0, 0x00);
             return 0;
 
         /*
@@ -869,8 +911,8 @@ write64_generic(void *priv, uint8_t val)
             if (kbc_ven == KBC_VEN_IBM_PS1) {
                 current_drive = fdc_get_current_drive();
                 /* (B0 or F0) | (fdd_is_525(current_drive) on bit 6) */
-                add_to_kbc_queue_front(dev, dev->p1 | fixed_bits | (fdd_is_525(current_drive) ? 0x40 : 0x00),
-                                       0, 0x00);
+                kbc_delay_to_ob(dev, dev->p1 | fixed_bits | (fdd_is_525(current_drive) ? 0x40 : 0x00),
+                                0, 0x00);
             } else if (kbc_ven == KBC_VEN_NCR) {
                 /* switch settings
                  * bit 7: keyboard disable
@@ -883,8 +925,8 @@ write64_generic(void *priv, uint8_t val)
                  * bit 0: dma mode
                  */
                 /* (B0 or F0) | 0x04 | (display on bit 6) | (fpu on bit 3) */
-                add_to_kbc_queue_front(dev, (dev->p1 | fixed_bits | (video_is_mda() ? 0x40 : 0x00) | (hasfpu ? 0x08 : 0x00)) & 0xdf,
-                                       0, 0x00);
+                kbc_delay_to_ob(dev, (dev->p1 | fixed_bits | (video_is_mda() ? 0x40 : 0x00) | (hasfpu ? 0x08 : 0x00)) & 0xdf,
+                                0, 0x00);
             } else if (kbc_ven == KBC_VEN_TRIGEM_AMI) {
                 /* Bit 3, 2:
                        1, 1: TriGem logo;
@@ -894,13 +936,13 @@ write64_generic(void *priv, uint8_t val)
                 if (dev->misc_flags & FLAG_PCI)
                     fixed_bits |= 8;
                 /* (B0 or F0) | (0x04 or 0x0c) */
-                add_to_kbc_queue_front(dev, dev->p1 | fixed_bits, 0, 0x00);
+                kbc_delay_to_ob(dev, dev->p1 | fixed_bits, 0, 0x00);
             } else if (((dev->flags & KBC_TYPE_MASK) >= KBC_TYPE_PS2_1) && ((dev->flags & KBC_TYPE_MASK) < KBC_TYPE_GREEN))
                 /* (B0 or F0) | (0x08 or 0x0c) */
-                add_to_kbc_queue_front(dev, ((dev->p1 | fixed_bits) & 0xf0) | (((dev->flags & KBC_VEN_MASK) == KBC_VEN_ACER) ? 0x08 : 0x0c), 0, 0x00);
+                kbc_delay_to_ob(dev, ((dev->p1 | fixed_bits) & 0xf0) | (((dev->flags & KBC_VEN_MASK) == KBC_VEN_ACER) ? 0x08 : 0x0c), 0, 0x00);
             else
                 /* (B0 or F0) | (0x04 or 0x44) */
-                add_to_kbc_queue_front(dev, dev->p1 | fixed_bits, 0, 0x00);
+                kbc_delay_to_ob(dev, dev->p1 | fixed_bits, 0, 0x00);
             dev->p1 = ((dev->p1 + 1) & 3) | (dev->p1 & 0xfc);
             return 0;
 
@@ -963,7 +1005,7 @@ write60_ami(void *priv, uint8_t val)
 
         case 0xa5: /* get extended controller RAM */
             kbc_at_log("ATkbc: AMI - get extended controller RAM\n");
-            add_to_kbc_queue_front(dev, dev->mem[val], 0, 0x00);
+            kbc_delay_to_ob(dev, dev->mem[val], 0, 0x00);
             return 0;
 
         case 0xaf: /* set extended controller RAM */
@@ -1011,7 +1053,7 @@ write64_ami(void *priv, uint8_t val)
     switch (val) {
         case 0x00 ... 0x1f:
             kbc_at_log("ATkbc: AMI - alias read from %08X\n", val);
-            add_to_kbc_queue_front(dev, dev->mem[val + 0x20], 0, 0x00);
+            kbc_delay_to_ob(dev, dev->mem[val + 0x20], 0, 0x00);
             return 0;
 
         case 0x40 ... 0x5f:
@@ -1023,19 +1065,18 @@ write64_ami(void *priv, uint8_t val)
         case 0xa0: /* copyright message */
             kbc_at_queue_add(dev, 0x28);
             kbc_at_queue_add(dev, 0x00);
-            dev->state = STATE_KBC_OUT;
             return 0;
 
         case 0xa1: /* get controller version */
             kbc_at_log("ATkbc: AMI - get controller version\n");
-            add_to_kbc_queue_front(dev, kbc_ami_revision, 0, 0x00);
+            kbc_delay_to_ob(dev, kbc_ami_revision, 0, 0x00);
             return 0;
 
         case 0xa2: /* clear keyboard controller lines P22/P23 */
             if (!(dev->misc_flags & FLAG_PS2)) {
                 kbc_at_log("ATkbc: AMI - clear KBC lines P22 and P23\n");
                 write_p2(dev, dev->p2 & 0xf3);
-                add_to_kbc_queue_front(dev, 0x00, 0, 0x00);
+                kbc_delay_to_ob(dev, 0x00, 0, 0x00);
                 return 0;
             }
             break;
@@ -1044,7 +1085,7 @@ write64_ami(void *priv, uint8_t val)
             if (!(dev->misc_flags & FLAG_PS2)) {
                 kbc_at_log("ATkbc: AMI - set KBC lines P22 and P23\n");
                 write_p2(dev, dev->p2 | 0x0c);
-                add_to_kbc_queue_front(dev, 0x00, 0, 0x00);
+                kbc_delay_to_ob(dev, 0x00, 0, 0x00);
                 return 0;
             }
             break;
@@ -1071,7 +1112,7 @@ write64_ami(void *priv, uint8_t val)
         case 0xa6: /* read clock */
             if (!(dev->misc_flags & FLAG_PS2)) {
                 kbc_at_log("ATkbc: AMI - read clock\n");
-                add_to_kbc_queue_front(dev, (dev->misc_flags & FLAG_CLOCK) ? 0xff : 0x00, 0, 0x00);
+                kbc_delay_to_ob(dev, (dev->misc_flags & FLAG_CLOCK) ? 0xff : 0x00, 0, 0x00);
                 return 0;
             }
             break;
@@ -1095,7 +1136,7 @@ write64_ami(void *priv, uint8_t val)
         case 0xa9: /* read cache */
             if (!(dev->misc_flags & FLAG_PS2)) {
                 kbc_at_log("ATkbc: AMI - read cache\n");
-                add_to_kbc_queue_front(dev, (dev->misc_flags & FLAG_CACHE) ? 0xff : 0x00, 0, 0x00);
+                kbc_delay_to_ob(dev, (dev->misc_flags & FLAG_CACHE) ? 0xff : 0x00, 0, 0x00);
                 return 0;
             }
             break;
@@ -1115,7 +1156,8 @@ write64_ami(void *priv, uint8_t val)
             kbc_at_log("ATkbc: set KBC lines P10-P13 (P1 bits 0-3) low\n");
             if (!(dev->flags & DEVICE_PCI) || (val > 0xb1))
                 dev->p1 &= ~(1 << (val & 0x03));
-            add_to_kbc_queue_front(dev, 0x00, 0, 0x00);
+            kbc_delay_to_ob(dev, dev->ob, 0, 0x00);
+            dev->pending++;
             return 0;
 
         /* TODO: The ICS SB486PV sends command B4 but expects to read *TWO* bytes. */
@@ -1124,7 +1166,8 @@ write64_ami(void *priv, uint8_t val)
             kbc_at_log("ATkbc: set KBC lines P22-P23 (P2 bits 2-3) low\n");
             if (!(dev->flags & DEVICE_PCI))
                 write_p2(dev, dev->p2 & ~(4 << (val & 0x01)));
-            add_to_kbc_queue_front(dev, 0x00, 0, 0x00);
+            kbc_delay_to_ob(dev, dev->ob, 0, 0x00);
+            dev->pending++;
             return 0;
 
         case 0xb8 ... 0xbb:
@@ -1132,7 +1175,8 @@ write64_ami(void *priv, uint8_t val)
             kbc_at_log("ATkbc: set KBC lines P10-P13 (P1 bits 0-3) high\n");
             if (!(dev->flags & DEVICE_PCI) || (val > 0xb9)) {
                 dev->p1 |= (1 << (val & 0x03));
-                add_to_kbc_queue_front(dev, 0x00, 0, 0x00);
+                kbc_delay_to_ob(dev, dev->ob, 0, 0x00);
+                dev->pending++;
             }
             return 0;
 
@@ -1141,7 +1185,8 @@ write64_ami(void *priv, uint8_t val)
             kbc_at_log("ATkbc: set KBC lines P22-P23 (P2 bits 2-3) high\n");
             if (!(dev->flags & DEVICE_PCI))
                 write_p2(dev, dev->p2 | (4 << (val & 0x01)));
-            add_to_kbc_queue_front(dev, 0x00, 0, 0x00);
+            kbc_delay_to_ob(dev, dev->ob, 0, 0x00);
+            dev->pending++;
             return 0;
 
         case 0xc1: /* write P1 */
@@ -1154,13 +1199,15 @@ write64_ami(void *priv, uint8_t val)
             /* set KBC line P14 low */
             kbc_at_log("ATkbc: set KBC line P14 (P1 bit 4) low\n");
             dev->p1 &= 0xef;
-            add_to_kbc_queue_front(dev, 0x00, 0, 0x00);
+            kbc_delay_to_ob(dev, dev->ob, 0, 0x00);
+            dev->pending++;
             return 0;
         case 0xc5:
             /* set KBC line P15 low */
             kbc_at_log("ATkbc: set KBC line P15 (P1 bit 5) low\n");
             dev->p1 &= 0xdf;
-            add_to_kbc_queue_front(dev, 0x00, 0, 0x00);
+            kbc_delay_to_ob(dev, dev->ob, 0, 0x00);
+            dev->pending++;
             return 0;
 
         case 0xc8:
@@ -1185,13 +1232,15 @@ write64_ami(void *priv, uint8_t val)
             /* set KBC line P14 high */
             kbc_at_log("ATkbc: set KBC line P14 (P1 bit 4) high\n");
             dev->p1 |= 0x10;
-            add_to_kbc_queue_front(dev, 0x00, 0, 0x00);
+            kbc_delay_to_ob(dev, dev->ob, 0, 0x00);
+            dev->pending++;
             return 0;
         case 0xcd:
             /* set KBC line P15 high */
             kbc_at_log("ATkbc: set KBC line P15 (P1 bit 5) high\n");
             dev->p1 |= 0x20;
-            add_to_kbc_queue_front(dev, 0x00, 0, 0x00);
+            kbc_delay_to_ob(dev, dev->ob, 0, 0x00);
+            dev->pending++;
             return 0;
 
         case 0xef: /* ??? - sent by AMI486 */
@@ -1230,7 +1279,7 @@ write64_olivetti(void *priv, uint8_t val)
              * bit 2: keyboard fuse present
              * bits 0-1: ???
              */
-            add_to_kbc_queue_front(dev, (0x0c | ((is386) ? 0x00 : 0x80)) & 0xdf, 0, 0x00);
+            kbc_delay_to_ob(dev, (0x0c | ((is386) ? 0x00 : 0x80)) & 0xdf, 0, 0x00);
             dev->p1 = ((dev->p1 + 1) & 3) | (dev->p1 & 0xfc);
             return 0;
     }
@@ -1305,12 +1354,12 @@ write64_toshiba(void *priv, uint8_t val)
 
         case 0xb4: /* T3100e: Get configuration / status */
             kbc_at_log("ATkbc: T3100e: Get configuration / status\n");
-            add_to_kbc_queue_front(dev, t3100e_config_get(), 0, 0x00);
+            kbc_delay_to_ob(dev, t3100e_config_get(), 0, 0x00);
             return 0;
 
         case 0xb5: /* T3100e: Get colour / mono byte */
             kbc_at_log("ATkbc: T3100e: Get colour / mono byte\n");
-            add_to_kbc_queue_front(dev, t3100e_mono_get(), 0, 0x00);
+            kbc_delay_to_ob(dev, t3100e_mono_get(), 0, 0x00);
             return 0;
 
         case 0xb6: /* T3100e: Set colour / mono byte */
@@ -1340,9 +1389,9 @@ write64_toshiba(void *priv, uint8_t val)
             kbc_at_log("ATkbc: T3100e: Read 'Fn' key\n");
             if (keyboard_recv(0xb8) || /* Right Alt */
                 keyboard_recv(0x9d))   /* Right Ctrl */
-                add_to_kbc_queue_front(dev, 0x04, 0, 0x00);
+                kbc_delay_to_ob(dev, 0x04, 0, 0x00);
             else
-                add_to_kbc_queue_front(dev, 0x00, 0, 0x00);
+                kbc_delay_to_ob(dev, 0x00, 0, 0x00);
             return 0;
 
         case 0xbc: /* T3100e: Reset Fn+Key notification */
@@ -1356,7 +1405,7 @@ write64_toshiba(void *priv, uint8_t val)
             /* The T3100e returns all bits set except bit 6 which
              * is set by t3100e_mono_set() */
             dev->p1 = (t3100e_mono_get() & 1) ? 0xff : 0xbf;
-            add_to_kbc_queue_front(dev, dev->p1, 0, 0x00);
+            kbc_delay_to_ob(dev, dev->p1, 0, 0x00);
             return 0;
     }
 
@@ -1382,7 +1431,9 @@ kbc_at_process_cmd(void *priv)
         switch (dev->ib) {
             /* Read data from KBC memory. */
             case 0x20 ... 0x3f:
-                add_to_kbc_queue_front(dev, dev->mem[dev->ib], 0, 0x00);
+                kbc_delay_to_ob(dev, dev->mem[dev->ib], 0, 0x00);
+                if (dev->ib == 0x20)
+                    dev->pending++;
                 break;
 
             /* Write data to KBC memory. */
@@ -1442,16 +1493,12 @@ kbc_at_process_cmd(void *priv)
                     dev->ports[1]->out_new = -1;
                 kbc_at_queue_reset(dev);
 
-                // dev->state = STATE_MAIN_IBF;
-                dev->state = STATE_KBC_OUT;
-
-                // add_to_kbc_queue_front(dev, 0x55, 0, 0x00);
                 kbc_at_queue_add(dev, 0x55);
                 break;
 
             case 0xab: /* interface test */
                 kbc_at_log("ATkbc: interface test\n");
-                add_to_kbc_queue_front(dev, 0x00, 0, 0x00); /*no error*/
+                kbc_delay_to_ob(dev, 0x00, 0, 0x00); /*no error*/
                 break;
 
             case 0xac: /* diagnostic dump */
@@ -1467,7 +1514,6 @@ kbc_at_process_cmd(void *priv)
                         kbc_at_queue_add(dev, cmd_ac_conv[dev->mem[i + 0x20] & 0x0f]);
                         kbc_at_queue_add(dev, 0x39);
                     }
-                    dev->state = STATE_KBC_OUT;
                 }
                 break;
 
@@ -1489,7 +1535,7 @@ kbc_at_process_cmd(void *priv)
 
             case 0xca: /* read keyboard mode */
                 kbc_at_log("ATkbc: AMI - read keyboard mode\n");
-                add_to_kbc_queue_front(dev, dev->ami_flags, 0, 0x00);
+                kbc_delay_to_ob(dev, dev->ami_flags, 0, 0x00);
                 break;
 
             case 0xcb: /* set keyboard mode */
@@ -1503,7 +1549,7 @@ kbc_at_process_cmd(void *priv)
                 mask = 0xff;
                 if ((kbc_ven != KBC_VEN_OLIVETTI) && !(dev->misc_flags & FLAG_PS2) && (dev->mem[0x20] & 0x10))
                     mask &= 0xbf;
-                add_to_kbc_queue_front(dev, (((dev->p2 & 0xfd) | mem_a20_key) & mask), 0, 0x00);
+                kbc_delay_to_ob(dev, ((dev->p2 & 0xfd) | mem_a20_key) & mask, 0, 0x00);
                 break;
 
             case 0xd1: /* write P2 */
@@ -1526,7 +1572,7 @@ kbc_at_process_cmd(void *priv)
 
             case 0xe0: /* read test inputs */
                 kbc_at_log("ATkbc: read test inputs\n");
-                add_to_kbc_queue_front(dev, 0x00, 0, 0x00);
+                kbc_delay_to_ob(dev, 0x00, 0, 0x00);
                 break;
 
             default:
@@ -1578,12 +1624,12 @@ kbc_at_process_cmd(void *priv)
 
             case 0xd2: /* write to keyboard output buffer */
                 kbc_at_log("ATkbc: write to keyboard output buffer\n");
-                add_to_kbc_queue_front(dev, dev->ib, 0, 0x00);
+                kbc_delay_to_ob(dev, dev->ib, 0, 0x00);
                 break;
 
             case 0xd3: /* write to auxiliary output buffer */
                 kbc_at_log("ATkbc: write to auxiliary output buffer\n");
-                add_to_kbc_queue_front(dev, dev->ib, 2, 0x00);
+                kbc_delay_to_ob(dev, dev->ib, 2, 0x00);
                 break;
 
             case 0xd4: /* write to auxiliary port */
@@ -1599,7 +1645,7 @@ kbc_at_process_cmd(void *priv)
                         dev->ports[1]->dat = dev->ib;
                         dev->state         = STATE_SEND_AUX;
                     } else
-                        add_to_kbc_queue_front(dev, 0xfe, 2, 0x40);
+                        kbc_delay_to_ob(dev, 0xfe, 2, 0x40);
                 }
                 break;
 
@@ -1633,6 +1679,7 @@ kbc_at_write(uint16_t port, uint8_t val, void *priv)
             if (dev->wantdata && (dev->command == 0xd1)) {
                 kbc_at_log("ATkbc: write P2\n");
 
+#if 0
                 /* Fast A20 - ignore all other bits. */
                 val = (val & 0x02) | (dev->p2 & 0xfd);
 
@@ -1646,6 +1693,10 @@ kbc_at_write(uint16_t port, uint8_t val, void *priv)
                 }
 
                 write_p2_fast_a20(dev, val | 0x01);
+#else
+                /* Fast A20 - ignore all other bits. */
+                write_p2_fast_a20(dev, (dev->p2 & 0xfd) | (val & 0x02));
+#endif
 
                 dev->wantdata  = 0;                
                 dev->state     = STATE_MAIN_IBF;