diff --git a/src/device/kbc_at_dev.c b/src/device/kbc_at_dev.c
index cd55f91fa..b20e38860 100644
--- a/src/device/kbc_at_dev.c
+++ b/src/device/kbc_at_dev.c
@@ -95,10 +95,6 @@ kbc_at_dev_queue_add(atkbc_dev_t *dev, uint8_t val, uint8_t main)
         dev->cmd_queue[dev->cmd_queue_end] = val;
         dev->cmd_queue_end                 = (dev->cmd_queue_end + 1) & 0xf;
     }
-
-    /* TODO: This should be done on actual send to host. */
-    if (val != 0xfe)
-        dev->last_scan_code = val;
 }
 
 static void
@@ -123,6 +119,8 @@ kbc_at_dev_poll(void *priv)
                 (dev->queue_start != dev->queue_end)) {
                 kbc_at_dev_log("%s: %02X (DATA) on channel 1\n", dev->name, dev->queue[dev->queue_start]);
                 dev->port->out_new   = dev->queue[dev->queue_start];
+                if (dev->port->out_new != 0xfe)
+                    dev->last_scan_code = dev->port->out_new;
                 dev->queue_start     = (dev->queue_start + 1) & dev->fifo_mask;
             }
             if (dev->ignore || !(*dev->scan) || dev->port->wantcmd)
@@ -143,6 +141,8 @@ kbc_at_dev_poll(void *priv)
             if ((dev->port->out_new == -1) && (dev->cmd_queue_start != dev->cmd_queue_end)) {
                 kbc_at_dev_log("%s: %02X (CMD ) on channel 1\n", dev->name, dev->cmd_queue[dev->cmd_queue_start]);
                 dev->port->out_new   = dev->cmd_queue[dev->cmd_queue_start];
+                if (dev->port->out_new != 0xfe)
+                    dev->last_scan_code = dev->port->out_new;
                 dev->cmd_queue_start = (dev->cmd_queue_start + 1) & 0xf;
             }
             if (dev->cmd_queue_start == dev->cmd_queue_end)
@@ -166,6 +166,8 @@ kbc_at_dev_poll(void *priv)
             if ((dev->port->out_new == -1) && (dev->cmd_queue_start != dev->cmd_queue_end)) {
                 kbc_at_dev_log("%s: %02X (CMD ) on channel 1\n", dev->name, dev->cmd_queue[dev->cmd_queue_start]);
                 dev->port->out_new   = dev->cmd_queue[dev->cmd_queue_start];
+                if (dev->port->out_new != 0xfe)
+                    dev->last_scan_code = dev->port->out_new;
                 dev->cmd_queue_start = (dev->cmd_queue_start + 1) & 0xf;
             }
             if (dev->cmd_queue_start == dev->cmd_queue_end)
diff --git a/src/device/keyboard_at.c b/src/device/keyboard_at.c
index ee5036f74..7fa2b9706 100644
--- a/src/device/keyboard_at.c
+++ b/src/device/keyboard_at.c
@@ -3494,7 +3494,6 @@ keyboard_at_invalid_cmd(atkbc_dev_t *dev)
     kbc_at_dev_queue_add(dev, inv_cmd_response, 0);
 }
 
-
 static void
 keyboard_at_write(void *priv)
 {
@@ -3719,7 +3718,6 @@ keyboard_at_write(void *priv)
             /* TODO: This is supposed to resend multiple bytes after some commands. */
             case 0xfe: /* resend last scan code */
                 keyboard_at_log("%s: resend last scan code\n", dev->name);
-                kbc_at_dev_queue_add(dev, 0xfa, 0);
                 kbc_at_dev_queue_add(dev, dev->last_scan_code, 0);
                 break;