From 146f2d2b4a2736e9f01ff67601126f440642460d Mon Sep 17 00:00:00 2001
From: Jeremy Soller <jeremy@system76.com>
Date: Mon, 19 Jul 2021 12:22:57 -0600
Subject: [PATCH] Implement scancode buffer

---
 src/board/system76/common/include/board/kbc.h |  2 +-
 src/board/system76/common/kbc.c               | 50 ++++++++++++++++---
 src/board/system76/common/kbscan.c            | 18 +++----
 3 files changed, 52 insertions(+), 18 deletions(-)

diff --git a/src/board/system76/common/include/board/kbc.h b/src/board/system76/common/include/board/kbc.h
index c74bbcf..07961c6 100644
--- a/src/board/system76/common/include/board/kbc.h
+++ b/src/board/system76/common/include/board/kbc.h
@@ -10,7 +10,7 @@
 extern uint8_t kbc_leds;
 
 void kbc_init(void);
-bool kbc_scancode(struct Kbc * kbc, uint16_t key, bool pressed);
+bool kbc_scancode(uint16_t key, bool pressed);
 void kbc_event(struct Kbc * kbc);
 
 #endif // _BOARD_KBC_H
diff --git a/src/board/system76/common/kbc.c b/src/board/system76/common/kbc.c
index 2a35009..ee3ec63 100644
--- a/src/board/system76/common/kbc.c
+++ b/src/board/system76/common/kbc.c
@@ -72,16 +72,46 @@ static const uint16_t kbc_typematic_period[32] = {
     500,    //  2.0 cps = 500ms
 };
 
-bool kbc_scancode(struct Kbc * kbc, uint16_t key, bool pressed) {
+static uint8_t kbc_buffer[16] = { 0 };
+static uint8_t kbc_buffer_head = 0;
+static uint8_t kbc_buffer_tail = 0;
+
+static bool kbc_buffer_pop(uint8_t * scancode) {
+    if (kbc_buffer_head == kbc_buffer_tail) {
+        return false;
+    }
+    *scancode = kbc_buffer[kbc_buffer_head];
+    kbc_buffer_head = (kbc_buffer_head + 1U) % ARRAY_SIZE(kbc_buffer);
+    return true;
+}
+
+static bool kbc_buffer_push(uint8_t * scancodes, uint8_t len) {
+    //TODO: make this test more efficient
+    for (uint8_t i = 0; i < len; i++) {
+        if ((kbc_buffer_tail + i + 1U) % ARRAY_SIZE(kbc_buffer) == kbc_buffer_head) {
+            return false;
+        }
+    }
+
+    for (uint8_t i = 0; i < len; i++) {
+        kbc_buffer[kbc_buffer_tail] = scancodes[i];
+        kbc_buffer_tail = (kbc_buffer_tail + 1U) % ARRAY_SIZE(kbc_buffer);
+    }
+    return true;
+}
+
+bool kbc_scancode(uint16_t key, bool pressed) {
     if (!kbc_first) return true;
     if (kbc_translate) {
         key = keymap_translate(key);
     }
     if (!key) return true;
+
+    uint8_t scancodes[3] = {0, 0, 0};
+    uint8_t scancodes_len = 0;
     switch (key & 0xFF00) {
         case KF_E0:
-            TRACE("  E0\n");
-            if (!kbc_keyboard(kbc, 0xE0, KBC_TIMEOUT)) return false;
+            scancodes[scancodes_len++] = 0xE0;
             key &= 0xFF;
             // Fall through
         case 0x00:
@@ -89,15 +119,14 @@ bool kbc_scancode(struct Kbc * kbc, uint16_t key, bool pressed) {
                 if (kbc_translate) {
                     key |= 0x80;
                 } else {
-                    TRACE("  F0\n");
-                    if (!kbc_keyboard(kbc, 0xF0, KBC_TIMEOUT)) return false;
+                    scancodes[scancodes_len++] = 0xF0;
                 }
             }
-            TRACE("  %02X\n", key);
-            if (!kbc_keyboard(kbc, (uint8_t)key, KBC_TIMEOUT)) return false;
+            scancodes[scancodes_len++] = (uint8_t)key;
             break;
     }
-    return true;
+
+    return kbc_buffer_push(scancodes, scancodes_len);
 }
 
 enum KbcState {
@@ -416,6 +445,11 @@ static void kbc_on_output_empty(struct Kbc * kbc) {
 void kbc_event(struct Kbc * kbc) {
     uint8_t sts;
 
+    // Read from scancode buffer when possible
+    if (state == KBC_STATE_NORMAL && kbc_buffer_pop(&state_data)) {
+        state = KBC_STATE_KEYBOARD;
+    }
+
     // Read from touchpad when possible
     if (kbc_second) {
         if (kbc_second_wait > 0) {
diff --git a/src/board/system76/common/kbscan.c b/src/board/system76/common/kbscan.c
index 66401b1..7d3b3bf 100644
--- a/src/board/system76/common/kbscan.c
+++ b/src/board/system76/common/kbscan.c
@@ -203,7 +203,7 @@ bool kbscan_press(uint16_t key, bool pressed, uint8_t * layer) {
     switch (key & KT_MASK) {
         case (KT_NORMAL):
             if (kbscan_enabled) {
-                kbc_scancode(&KBC, key, pressed);
+                kbc_scancode(key, pressed);
             }
             break;
         case (KT_FN):
@@ -220,22 +220,22 @@ bool kbscan_press(uint16_t key, bool pressed, uint8_t * layer) {
                 case COMBO_DISPLAY_MODE:
                     if (kbscan_enabled) {
                         if (pressed) {
-                            kbc_scancode(&KBC, K_LEFT_SUPER, true);
-                            kbc_scancode(&KBC, K_P, true);
-                            kbc_scancode(&KBC, K_P, false);
+                            kbc_scancode(K_LEFT_SUPER, true);
+                            kbc_scancode(K_P, true);
+                            kbc_scancode(K_P, false);
                         } else {
-                            kbc_scancode(&KBC, K_LEFT_SUPER, false);
+                            kbc_scancode(K_LEFT_SUPER, false);
                         }
                     }
                     break;
                 case COMBO_PRINT_SCREEN:
                     if (kbscan_enabled) {
                         if (pressed) {
-                            kbc_scancode(&KBC, KF_E0 | 0x12, true);
-                            kbc_scancode(&KBC, KF_E0 | 0x7C, true);
+                            kbc_scancode(KF_E0 | 0x12, true);
+                            kbc_scancode(KF_E0 | 0x7C, true);
                         } else {
-                            kbc_scancode(&KBC, KF_E0 | 0x7C, false);
-                            kbc_scancode(&KBC, KF_E0 | 0x12, false);
+                            kbc_scancode(KF_E0 | 0x7C, false);
+                            kbc_scancode(KF_E0 | 0x12, false);
                         }
                     }
                     break;