diff --git a/src/device/kbc_at.c b/src/device/kbc_at.c
index 9ba84888c..d38a9bb72 100644
--- a/src/device/kbc_at.c
+++ b/src/device/kbc_at.c
@@ -1201,6 +1201,9 @@ kbc_at_set_ps2(void *priv, const uint8_t ps2)
         kbc_at_do_poll = kbc_at_poll_ps2;
     } else
         kbc_at_do_poll = kbc_at_poll_at;
+
+    write_cmd(dev, ~dev->mem[0x20]);
+    write_cmd(dev, dev->mem[0x20]);
 }
 
 static uint8_t
@@ -2230,7 +2233,11 @@ static void
 kbc_at_close(void *priv)
 {
     atkbc_t *dev = (atkbc_t *) priv;
+#ifdef OLD_CODE
     int max_ports = ((dev->flags & KBC_TYPE_MASK) >= KBC_TYPE_PS2_1) ? 2 : 1;
+#else
+    int max_ports = 2;
+#endif
 
     /* Stop timers. */
     timer_disable(&dev->kbc_dev_poll_timer);
@@ -2372,7 +2379,11 @@ kbc_at_init(const device_t *info)
             break;
     }
 
+#ifdef OLD_CODE
     max_ports = ((dev->flags & KBC_TYPE_MASK) >= KBC_TYPE_PS2_1) ? 2 : 1;
+#else
+    max_ports = 2;
+#endif
 
     for (int i = 0; i < max_ports; i++) {
         kbc_at_ports[i] = (kbc_at_port_t *) malloc(sizeof(kbc_at_port_t));