From 788fa5ee52e894faee9bd18cb0cb3edf22e172ee Mon Sep 17 00:00:00 2001
From: Jeremy Soller <jeremy@system76.com>
Date: Mon, 18 Nov 2019 19:19:55 -0700
Subject: [PATCH] Improve keyboard scanning and add function key

---
 .../system76/galp3-c/include/board/keymap.h   |  4 +++
 src/board/system76/galp3-c/kbscan.c           | 36 +++++++++----------
 src/board/system76/galp3-c/keymap.c           |  2 +-
 3 files changed, 23 insertions(+), 19 deletions(-)

diff --git a/src/board/system76/galp3-c/include/board/keymap.h b/src/board/system76/galp3-c/include/board/keymap.h
index fdc5e25..b91857a 100644
--- a/src/board/system76/galp3-c/include/board/keymap.h
+++ b/src/board/system76/galp3-c/include/board/keymap.h
@@ -19,6 +19,10 @@ uint16_t keymap_translate(uint16_t key);
 // Should send 0xE0 before scancode bytes
 #define K_E0 0x0100
 
+// Layer selection
+
+#define K_FN (0x8000)
+
 // Function keys
 
 #define K_F1 (0x05)
diff --git a/src/board/system76/galp3-c/kbscan.c b/src/board/system76/galp3-c/kbscan.c
index 474e998..0086ba4 100644
--- a/src/board/system76/galp3-c/kbscan.c
+++ b/src/board/system76/galp3-c/kbscan.c
@@ -10,14 +10,13 @@ void kbscan_init(void) {
     KSOCTRL = 0x05;
     KSICTRLR = 0x04;
 
-    // Set all outputs to GPIO mode and low
+    // Set all outputs to GPIO mode, low, and inputs
     KSOL = 0;
     KSOH1 = 0;
-    KSOH2 = 0;
     KSOLGCTRL = 0xFF;
-    KSOLGOEN = 0xFF;
+    KSOLGOEN = 0;
     KSOHGCTRL = 0xFF;
-    KSOHGOEN = 0xFF;
+    KSOHGOEN = 0;
 }
 
 void kbscan_event(void) {
@@ -26,19 +25,17 @@ void kbscan_event(void) {
 
     int i;
     for (i = 0; i < KM_OUT; i++) {
-        KSOL = 0xFF;
-        KSOH1 = 0xFF;
-        KSOH2 = 0xFF;
+        // Set current line as output
         if (i < 8) {
-            KSOL = ~(1 << i);
+            KSOLGOEN = 1 << i;
+            KSOHGOEN = 0;
         } else if (i < 16) {
-            KSOH1 = ~(1 << (i - 8));
-        } else if (i < 24) {
-            KSOH2 = ~(1 << (i - 16));
+            KSOLGOEN = 0;
+            KSOHGOEN = 1 << (i - 8);
         }
 
         // TODO: figure out optimal delay
-        delay_ticks(1);
+        delay_ticks(10);
 
         uint8_t new = ~KSI;
         uint8_t last = kbscan_last[i];
@@ -49,8 +46,11 @@ void kbscan_event(void) {
                 bool last_b = last & (1 << j);
                 if (new_b != last_b) {
                     uint16_t key = keymap(i, j, kbscan_layer);
-                    TRACE("KB %d, %d, %d = 0x%04X, %d\n", i, j, kbscan_layer, key, new_b);
-                    if (kbscan_enabled && key) {
+                    DEBUG("KB %d, %d, %d = 0x%04X, %d\n", i, j, kbscan_layer, key, new_b);
+                    if (key == K_FN) {
+                        if (new_b) kbscan_layer = 1;
+                        else kbscan_layer = 0;
+                    } else if (kbscan_enabled && key) {
                         kbc_scancode(&KBC, key, new_b);
                     }
                 }
@@ -60,10 +60,10 @@ void kbscan_event(void) {
         }
     }
 
-    KSOL = 0;
-    KSOH1 = 0;
-    KSOH2 = 0;
+    // Reset all lines to inputs
+    KSOLGOEN = 0;
+    KSOHGOEN = 0;
 
     // TODO: figure out optimal delay
-    delay_ticks(1);
+    delay_ticks(10);
 }
diff --git a/src/board/system76/galp3-c/keymap.c b/src/board/system76/galp3-c/keymap.c
index 430d6d4..69e334f 100644
--- a/src/board/system76/galp3-c/keymap.c
+++ b/src/board/system76/galp3-c/keymap.c
@@ -63,7 +63,7 @@ uint16_t __code KEYMAP[KM_OUT][KM_IN][KM_LAY] = {
         {K_E, 0}, // 7
     },
     {   //  6
-        {0, 0}, // 0
+        {K_FN, K_FN}, // 0
         {K_B, 0}, // 1
         {K_F, 0}, // 2
         {K_G, 0}, // 3