From 177c7c06276bd3e8f351d12fef8d99937a574485 Mon Sep 17 00:00:00 2001
From: Jeremy Soller <jeremy@system76.com>
Date: Mon, 17 Feb 2020 12:36:37 -0700
Subject: [PATCH 1/4] Sync SMFI changes to other boards

---
 src/board/system76/darp5/board.mk             |  4 +
 src/board/system76/darp5/include/board/smfi.h |  7 ++
 src/board/system76/darp5/main.c               |  6 +-
 src/board/system76/darp5/pnp.c                |  6 ++
 src/board/system76/darp5/smfi.c               | 98 +++++++++++++++++++
 src/board/system76/galp3-c/board.mk           |  4 +
 .../system76/galp3-c/include/board/smfi.h     |  7 ++
 src/board/system76/galp3-c/main.c             |  6 +-
 src/board/system76/galp3-c/pnp.c              |  6 ++
 src/board/system76/galp3-c/smfi.c             | 98 +++++++++++++++++++
 src/board/system76/lemp9/board.mk             |  4 +
 11 files changed, 244 insertions(+), 2 deletions(-)
 create mode 100644 src/board/system76/darp5/include/board/smfi.h
 create mode 100644 src/board/system76/darp5/smfi.c
 create mode 100644 src/board/system76/galp3-c/include/board/smfi.h
 create mode 100644 src/board/system76/galp3-c/smfi.c

diff --git a/src/board/system76/darp5/board.mk b/src/board/system76/darp5/board.mk
index d1dfc77..83dced9 100644
--- a/src/board/system76/darp5/board.mk
+++ b/src/board/system76/darp5/board.mk
@@ -42,3 +42,7 @@ flash: $(BUILD)/ec.rom
 isp: $(BUILD)/ec.rom
 	cargo build --manifest-path ecflash/Cargo.toml --example isp --release
 	sudo ecflash/target/release/examples/isp $<
+
+version:
+	cargo build --manifest-path ecflash/Cargo.toml --example smfi --release
+	sudo ecflash/target/release/examples/smfi
diff --git a/src/board/system76/darp5/include/board/smfi.h b/src/board/system76/darp5/include/board/smfi.h
new file mode 100644
index 0000000..532ef23
--- /dev/null
+++ b/src/board/system76/darp5/include/board/smfi.h
@@ -0,0 +1,7 @@
+#ifndef _BOARD_SMFI_H
+#define _BOARD_SMFI_H
+
+void smfi_init(void);
+void smfi_event(void);
+
+#endif // _BOARD_SMFI_H
diff --git a/src/board/system76/darp5/main.c b/src/board/system76/darp5/main.c
index 8d4643e..91f94eb 100644
--- a/src/board/system76/darp5/main.c
+++ b/src/board/system76/darp5/main.c
@@ -17,6 +17,7 @@
 #include <board/ps2.h>
 #include <board/pwm.h>
 #include <board/smbus.h>
+#include <board/smfi.h>
 #include <common/debug.h>
 #include <common/macro.h>
 #include <common/version.h>
@@ -45,6 +46,7 @@ void init(void) {
     pmc_init();
     pwm_init();
     smbus_init();
+    smfi_init();
 
     //TODO: INTC
 }
@@ -113,7 +115,9 @@ void main(void) {
         }
         // Handles ACPI communication
         pmc_event(&PMC_1);
+        // AP/EC communication over SMFI
+        smfi_event();
         // Idle until next timer interrupt
-        PCON |= 1;
+        //Disabled until interrupts used: PCON |= 1;
     }
 }
diff --git a/src/board/system76/darp5/pnp.c b/src/board/system76/darp5/pnp.c
index 1146a87..6f9ab5b 100644
--- a/src/board/system76/darp5/pnp.c
+++ b/src/board/system76/darp5/pnp.c
@@ -41,6 +41,12 @@ void pnp_enable() {
     pnp_write(0x07, 0x05);
     pnp_write(0x30, 0x01);
 
+    // Enable SMFI
+    pnp_write(0x07, 0x0F);
+    pnp_write(0xF5, 0x00);
+    pnp_write(0xF6, 0x01);
+    pnp_write(0x30, 0x01);
+
     // Enable SWUC
     pnp_write(0x07, 0x04);
     pnp_write(0x30, 0x01);
diff --git a/src/board/system76/darp5/smfi.c b/src/board/system76/darp5/smfi.c
new file mode 100644
index 0000000..ac1590b
--- /dev/null
+++ b/src/board/system76/darp5/smfi.c
@@ -0,0 +1,98 @@
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#include <common/macro.h>
+#include <common/version.h>
+
+// Shared memory host semaphore
+volatile uint8_t __xdata __at(0x1022) SMHSR;
+// Host RAM window control
+volatile uint8_t __xdata __at(0x105A) HRAMWC;
+// Host RAM window 0 base address
+volatile uint8_t __xdata __at(0x105B) HRAMW0BA;
+// Host RAM window 1 base address
+volatile uint8_t __xdata __at(0x105C) HRAMW1BA;
+// Host RAM window 0 access allow size
+volatile uint8_t __xdata __at(0x105D) HRAMW0AAS;
+// Host RAM window 1 access allow size
+volatile uint8_t __xdata __at(0x105E) HRAMW1AAS;
+
+static volatile uint8_t __xdata __at(0xC00) smfi_cmd[256];
+static volatile uint8_t __xdata __at(0xD00) smfi_dbg[256];
+
+enum SmfiCmd {
+    SMFI_CMD_NONE = 0,
+    SMFI_CMD_PROBE = 1,
+    SMFI_CMD_BOARD = 2,
+    SMFI_CMD_VERSION = 3,
+    //TODO
+};
+
+enum SmfiRes {
+    SMFI_RES_OK = 0,
+    SMFI_RES_ERR = 1,
+    //TODO
+};
+
+void smfi_init(void) {
+    int i;
+
+    // Clear command region
+    for (i = 0; i < ARRAY_SIZE(smfi_cmd); i++) {
+        smfi_cmd[i] = 0x00;
+    }
+
+    // Clear debug region
+    for (i = 0; i < ARRAY_SIZE(smfi_dbg); i++) {
+        smfi_dbg[i] = 0x00;
+    }
+
+
+    // H2RAM window 0 address 0xC00 - 0xCFF, read/write
+    HRAMW0BA = 0xC0;
+    HRAMW0AAS = 0x04;
+
+    // H2RAM window 1 address 0xD00 - 0xDFF, read/write
+    HRAMW1BA = 0xD0;
+    HRAMW1AAS = 0x04;
+
+    // Enable H2RAM window 0 and 1 using LPC I/O
+    HRAMWC |= 0x13;
+}
+
+void smfi_event(void) {
+    if (smfi_cmd[0]) {
+        // Default to success
+        smfi_cmd[1] = SMFI_RES_OK;
+
+        switch (smfi_cmd[0]) {
+            case SMFI_CMD_PROBE:
+                // Signature
+                smfi_cmd[2] = 0x76;
+                smfi_cmd[3] = 0xEC;
+                // Version
+                smfi_cmd[4] = 0x01;
+                break;
+            case SMFI_CMD_BOARD:
+                strncpy(&smfi_cmd[2], board(), ARRAY_SIZE(smfi_cmd) - 2);
+                break;
+            case SMFI_CMD_VERSION:
+                strncpy(&smfi_cmd[2], version(), ARRAY_SIZE(smfi_cmd) - 2);
+                break;
+        }
+
+        // Mark command as finished
+        smfi_cmd[0] = SMFI_CMD_NONE;
+    }
+
+    if (smfi_dbg[0]) {
+        int i;
+        for(i = 1; (i <= (int)smfi_dbg[0]) && (i < ARRAY_SIZE(smfi_dbg)); i++) {
+            putchar(smfi_dbg[i]);
+        }
+
+        // Mark debug transaction as complete
+        smfi_dbg[0] = 0;
+    }
+}
diff --git a/src/board/system76/galp3-c/board.mk b/src/board/system76/galp3-c/board.mk
index d1dfc77..83dced9 100644
--- a/src/board/system76/galp3-c/board.mk
+++ b/src/board/system76/galp3-c/board.mk
@@ -42,3 +42,7 @@ flash: $(BUILD)/ec.rom
 isp: $(BUILD)/ec.rom
 	cargo build --manifest-path ecflash/Cargo.toml --example isp --release
 	sudo ecflash/target/release/examples/isp $<
+
+version:
+	cargo build --manifest-path ecflash/Cargo.toml --example smfi --release
+	sudo ecflash/target/release/examples/smfi
diff --git a/src/board/system76/galp3-c/include/board/smfi.h b/src/board/system76/galp3-c/include/board/smfi.h
new file mode 100644
index 0000000..532ef23
--- /dev/null
+++ b/src/board/system76/galp3-c/include/board/smfi.h
@@ -0,0 +1,7 @@
+#ifndef _BOARD_SMFI_H
+#define _BOARD_SMFI_H
+
+void smfi_init(void);
+void smfi_event(void);
+
+#endif // _BOARD_SMFI_H
diff --git a/src/board/system76/galp3-c/main.c b/src/board/system76/galp3-c/main.c
index 3809667..b3cab6b 100644
--- a/src/board/system76/galp3-c/main.c
+++ b/src/board/system76/galp3-c/main.c
@@ -18,6 +18,7 @@
 #include <board/ps2.h>
 #include <board/pwm.h>
 #include <board/smbus.h>
+#include <board/smfi.h>
 #include <common/debug.h>
 #include <common/macro.h>
 #include <common/version.h>
@@ -47,6 +48,7 @@ void init(void) {
     pmc_init();
     pwm_init();
     smbus_init();
+    smfi_init();
 
     //TODO: INTC
 }
@@ -115,7 +117,9 @@ void main(void) {
         }
         // Handles ACPI communication
         pmc_event(&PMC_1);
+        // AP/EC communication over SMFI
+        smfi_event();
         // Idle until next timer interrupt
-        PCON |= 1;
+        //Disabled until interrupts used: PCON |= 1;
     }
 }
diff --git a/src/board/system76/galp3-c/pnp.c b/src/board/system76/galp3-c/pnp.c
index 1146a87..6f9ab5b 100644
--- a/src/board/system76/galp3-c/pnp.c
+++ b/src/board/system76/galp3-c/pnp.c
@@ -41,6 +41,12 @@ void pnp_enable() {
     pnp_write(0x07, 0x05);
     pnp_write(0x30, 0x01);
 
+    // Enable SMFI
+    pnp_write(0x07, 0x0F);
+    pnp_write(0xF5, 0x00);
+    pnp_write(0xF6, 0x01);
+    pnp_write(0x30, 0x01);
+
     // Enable SWUC
     pnp_write(0x07, 0x04);
     pnp_write(0x30, 0x01);
diff --git a/src/board/system76/galp3-c/smfi.c b/src/board/system76/galp3-c/smfi.c
new file mode 100644
index 0000000..ac1590b
--- /dev/null
+++ b/src/board/system76/galp3-c/smfi.c
@@ -0,0 +1,98 @@
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#include <common/macro.h>
+#include <common/version.h>
+
+// Shared memory host semaphore
+volatile uint8_t __xdata __at(0x1022) SMHSR;
+// Host RAM window control
+volatile uint8_t __xdata __at(0x105A) HRAMWC;
+// Host RAM window 0 base address
+volatile uint8_t __xdata __at(0x105B) HRAMW0BA;
+// Host RAM window 1 base address
+volatile uint8_t __xdata __at(0x105C) HRAMW1BA;
+// Host RAM window 0 access allow size
+volatile uint8_t __xdata __at(0x105D) HRAMW0AAS;
+// Host RAM window 1 access allow size
+volatile uint8_t __xdata __at(0x105E) HRAMW1AAS;
+
+static volatile uint8_t __xdata __at(0xC00) smfi_cmd[256];
+static volatile uint8_t __xdata __at(0xD00) smfi_dbg[256];
+
+enum SmfiCmd {
+    SMFI_CMD_NONE = 0,
+    SMFI_CMD_PROBE = 1,
+    SMFI_CMD_BOARD = 2,
+    SMFI_CMD_VERSION = 3,
+    //TODO
+};
+
+enum SmfiRes {
+    SMFI_RES_OK = 0,
+    SMFI_RES_ERR = 1,
+    //TODO
+};
+
+void smfi_init(void) {
+    int i;
+
+    // Clear command region
+    for (i = 0; i < ARRAY_SIZE(smfi_cmd); i++) {
+        smfi_cmd[i] = 0x00;
+    }
+
+    // Clear debug region
+    for (i = 0; i < ARRAY_SIZE(smfi_dbg); i++) {
+        smfi_dbg[i] = 0x00;
+    }
+
+
+    // H2RAM window 0 address 0xC00 - 0xCFF, read/write
+    HRAMW0BA = 0xC0;
+    HRAMW0AAS = 0x04;
+
+    // H2RAM window 1 address 0xD00 - 0xDFF, read/write
+    HRAMW1BA = 0xD0;
+    HRAMW1AAS = 0x04;
+
+    // Enable H2RAM window 0 and 1 using LPC I/O
+    HRAMWC |= 0x13;
+}
+
+void smfi_event(void) {
+    if (smfi_cmd[0]) {
+        // Default to success
+        smfi_cmd[1] = SMFI_RES_OK;
+
+        switch (smfi_cmd[0]) {
+            case SMFI_CMD_PROBE:
+                // Signature
+                smfi_cmd[2] = 0x76;
+                smfi_cmd[3] = 0xEC;
+                // Version
+                smfi_cmd[4] = 0x01;
+                break;
+            case SMFI_CMD_BOARD:
+                strncpy(&smfi_cmd[2], board(), ARRAY_SIZE(smfi_cmd) - 2);
+                break;
+            case SMFI_CMD_VERSION:
+                strncpy(&smfi_cmd[2], version(), ARRAY_SIZE(smfi_cmd) - 2);
+                break;
+        }
+
+        // Mark command as finished
+        smfi_cmd[0] = SMFI_CMD_NONE;
+    }
+
+    if (smfi_dbg[0]) {
+        int i;
+        for(i = 1; (i <= (int)smfi_dbg[0]) && (i < ARRAY_SIZE(smfi_dbg)); i++) {
+            putchar(smfi_dbg[i]);
+        }
+
+        // Mark debug transaction as complete
+        smfi_dbg[0] = 0;
+    }
+}
diff --git a/src/board/system76/lemp9/board.mk b/src/board/system76/lemp9/board.mk
index ea1be51..5ff2249 100644
--- a/src/board/system76/lemp9/board.mk
+++ b/src/board/system76/lemp9/board.mk
@@ -45,3 +45,7 @@ flash: $(BUILD)/ec.rom
 isp: $(BUILD)/ec.rom
 	cargo build --manifest-path ecflash/Cargo.toml --example isp --release
 	sudo ecflash/target/release/examples/isp $<
+
+version:
+	cargo build --manifest-path ecflash/Cargo.toml --example smfi --release
+	sudo ecflash/target/release/examples/smfi

From 7c1e0508a935af9808b765543d0b14ffc7ecd189 Mon Sep 17 00:00:00 2001
From: Jeremy Soller <jeremy@system76.com>
Date: Mon, 17 Feb 2020 12:56:28 -0700
Subject: [PATCH 2/4] Include smfi header

---
 src/board/system76/lemp9/smfi.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/board/system76/lemp9/smfi.c b/src/board/system76/lemp9/smfi.c
index ac1590b..e550adf 100644
--- a/src/board/system76/lemp9/smfi.c
+++ b/src/board/system76/lemp9/smfi.c
@@ -2,6 +2,7 @@
 #include <stdio.h>
 #include <string.h>
 
+#include <board/smfi.h>
 #include <common/macro.h>
 #include <common/version.h>
 

From 35462cb7a0f45b75793d2a4c1b5cbf620f505faf Mon Sep 17 00:00:00 2001
From: Jeremy Soller <jeremy@system76.com>
Date: Mon, 17 Feb 2020 13:35:06 -0700
Subject: [PATCH 3/4] Add SMFI console

---
 ecflash                                       |  2 +-
 src/board/system76/darp5/board.mk             | 12 +++----
 src/board/system76/darp5/include/board/smfi.h |  1 +
 src/board/system76/darp5/smfi.c               | 33 +++++++++++++------
 src/board/system76/darp5/stdio.c              |  3 ++
 src/board/system76/galp3-c/board.mk           | 12 +++----
 .../system76/galp3-c/include/board/smfi.h     |  1 +
 src/board/system76/galp3-c/smfi.c             | 33 +++++++++++++------
 src/board/system76/galp3-c/stdio.c            |  3 ++
 src/board/system76/lemp9/board.mk             | 12 +++----
 src/board/system76/lemp9/include/board/smfi.h |  1 +
 src/board/system76/lemp9/smfi.c               | 32 ++++++++++++------
 src/board/system76/lemp9/stdio.c              |  3 ++
 13 files changed, 99 insertions(+), 49 deletions(-)

diff --git a/ecflash b/ecflash
index 5a9300d..945ec8d 160000
--- a/ecflash
+++ b/ecflash
@@ -1 +1 @@
-Subproject commit 5a9300d804df6b658196f690a10ba8fc7e821a78
+Subproject commit 945ec8d276edd78f26cd744624344e67549824ad
diff --git a/src/board/system76/darp5/board.mk b/src/board/system76/darp5/board.mk
index 83dced9..4d1d49b 100644
--- a/src/board/system76/darp5/board.mk
+++ b/src/board/system76/darp5/board.mk
@@ -11,10 +11,10 @@ SRC+=$(BOARD_DIR)/keymap/$(KEYMAP).c
 # 3 - INFO
 # 4 - DEBUG
 # 5 - TRACE
-CFLAGS+=-DLEVEL=2
+CFLAGS+=-DLEVEL=4
 
 # Enable I2C debug on 0x76
-CFLAGS+=-DI2C_DEBUGGER=0x76
+#CFLAGS+=-DI2C_DEBUGGER=0x76
 
 # Set battery I2C bus
 CFLAGS+=-DI2C_SMBUS=I2C_0
@@ -35,6 +35,10 @@ include $(SCRATCH_DIR)/scratch.mk
 CFLAGS+=-I$(BUILD)/include
 INCLUDE+=$(BUILD)/include/scratch.h
 
+console:
+	cargo build --manifest-path ecflash/Cargo.toml --example smfi --release
+	sudo ecflash/target/release/examples/smfi
+
 flash: $(BUILD)/ec.rom
 	cargo build --manifest-path ecflash/Cargo.toml --example isp --release
 	sudo ecflash/target/release/examples/isp --internal $<
@@ -42,7 +46,3 @@ flash: $(BUILD)/ec.rom
 isp: $(BUILD)/ec.rom
 	cargo build --manifest-path ecflash/Cargo.toml --example isp --release
 	sudo ecflash/target/release/examples/isp $<
-
-version:
-	cargo build --manifest-path ecflash/Cargo.toml --example smfi --release
-	sudo ecflash/target/release/examples/smfi
diff --git a/src/board/system76/darp5/include/board/smfi.h b/src/board/system76/darp5/include/board/smfi.h
index 532ef23..4ee0398 100644
--- a/src/board/system76/darp5/include/board/smfi.h
+++ b/src/board/system76/darp5/include/board/smfi.h
@@ -3,5 +3,6 @@
 
 void smfi_init(void);
 void smfi_event(void);
+void smfi_debug(unsigned char byte);
 
 #endif // _BOARD_SMFI_H
diff --git a/src/board/system76/darp5/smfi.c b/src/board/system76/darp5/smfi.c
index ac1590b..ff95768 100644
--- a/src/board/system76/darp5/smfi.c
+++ b/src/board/system76/darp5/smfi.c
@@ -2,6 +2,7 @@
 #include <stdio.h>
 #include <string.h>
 
+#include <board/smfi.h>
 #include <common/macro.h>
 #include <common/version.h>
 
@@ -26,6 +27,7 @@ enum SmfiCmd {
     SMFI_CMD_PROBE = 1,
     SMFI_CMD_BOARD = 2,
     SMFI_CMD_VERSION = 3,
+    SMFI_CMD_DEBUG = 4,
     //TODO
 };
 
@@ -62,6 +64,7 @@ void smfi_init(void) {
 }
 
 void smfi_event(void) {
+    int i;
     if (smfi_cmd[0]) {
         // Default to success
         smfi_cmd[1] = SMFI_RES_OK;
@@ -80,19 +83,29 @@ void smfi_event(void) {
             case SMFI_CMD_VERSION:
                 strncpy(&smfi_cmd[2], version(), ARRAY_SIZE(smfi_cmd) - 2);
                 break;
+            case SMFI_CMD_DEBUG:
+                for (i = 2; i < ARRAY_SIZE(smfi_cmd) - 2; i++) {
+                    uint8_t b = smfi_cmd[i];
+                    if (b == 0) break;
+                    putchar(b);
+                }
+            default:
+                // Command not found
+                smfi_cmd[1] = SMFI_RES_ERR;
+                break;
         }
 
         // Mark command as finished
         smfi_cmd[0] = SMFI_CMD_NONE;
     }
-
-    if (smfi_dbg[0]) {
-        int i;
-        for(i = 1; (i <= (int)smfi_dbg[0]) && (i < ARRAY_SIZE(smfi_dbg)); i++) {
-            putchar(smfi_dbg[i]);
-        }
-
-        // Mark debug transaction as complete
-        smfi_dbg[0] = 0;
-    }
+}
+
+void smfi_debug(unsigned char byte) {
+    int tail = (int)smfi_dbg[0];
+    tail++;
+    if (tail >= ARRAY_SIZE(smfi_dbg)) {
+        tail = 1;
+    }
+    smfi_dbg[tail] = byte;
+    smfi_dbg[0] = (uint8_t)tail;
 }
diff --git a/src/board/system76/darp5/stdio.c b/src/board/system76/darp5/stdio.c
index 9bdb177..6cb8951 100644
--- a/src/board/system76/darp5/stdio.c
+++ b/src/board/system76/darp5/stdio.c
@@ -1,5 +1,7 @@
 #include <stdio.h>
 
+#include <board/smfi.h>
+
 #ifdef SERIAL_DEBUGGER
     #include <mcs51/8051.h>
 #endif
@@ -10,6 +12,7 @@
 
 int putchar(int c) {
     unsigned char byte = (unsigned char)c;
+    smfi_debug(byte);
 #ifdef SERIAL_DEBUGGER
     SBUF = byte;
 #endif
diff --git a/src/board/system76/galp3-c/board.mk b/src/board/system76/galp3-c/board.mk
index 83dced9..4d1d49b 100644
--- a/src/board/system76/galp3-c/board.mk
+++ b/src/board/system76/galp3-c/board.mk
@@ -11,10 +11,10 @@ SRC+=$(BOARD_DIR)/keymap/$(KEYMAP).c
 # 3 - INFO
 # 4 - DEBUG
 # 5 - TRACE
-CFLAGS+=-DLEVEL=2
+CFLAGS+=-DLEVEL=4
 
 # Enable I2C debug on 0x76
-CFLAGS+=-DI2C_DEBUGGER=0x76
+#CFLAGS+=-DI2C_DEBUGGER=0x76
 
 # Set battery I2C bus
 CFLAGS+=-DI2C_SMBUS=I2C_0
@@ -35,6 +35,10 @@ include $(SCRATCH_DIR)/scratch.mk
 CFLAGS+=-I$(BUILD)/include
 INCLUDE+=$(BUILD)/include/scratch.h
 
+console:
+	cargo build --manifest-path ecflash/Cargo.toml --example smfi --release
+	sudo ecflash/target/release/examples/smfi
+
 flash: $(BUILD)/ec.rom
 	cargo build --manifest-path ecflash/Cargo.toml --example isp --release
 	sudo ecflash/target/release/examples/isp --internal $<
@@ -42,7 +46,3 @@ flash: $(BUILD)/ec.rom
 isp: $(BUILD)/ec.rom
 	cargo build --manifest-path ecflash/Cargo.toml --example isp --release
 	sudo ecflash/target/release/examples/isp $<
-
-version:
-	cargo build --manifest-path ecflash/Cargo.toml --example smfi --release
-	sudo ecflash/target/release/examples/smfi
diff --git a/src/board/system76/galp3-c/include/board/smfi.h b/src/board/system76/galp3-c/include/board/smfi.h
index 532ef23..4ee0398 100644
--- a/src/board/system76/galp3-c/include/board/smfi.h
+++ b/src/board/system76/galp3-c/include/board/smfi.h
@@ -3,5 +3,6 @@
 
 void smfi_init(void);
 void smfi_event(void);
+void smfi_debug(unsigned char byte);
 
 #endif // _BOARD_SMFI_H
diff --git a/src/board/system76/galp3-c/smfi.c b/src/board/system76/galp3-c/smfi.c
index ac1590b..ff95768 100644
--- a/src/board/system76/galp3-c/smfi.c
+++ b/src/board/system76/galp3-c/smfi.c
@@ -2,6 +2,7 @@
 #include <stdio.h>
 #include <string.h>
 
+#include <board/smfi.h>
 #include <common/macro.h>
 #include <common/version.h>
 
@@ -26,6 +27,7 @@ enum SmfiCmd {
     SMFI_CMD_PROBE = 1,
     SMFI_CMD_BOARD = 2,
     SMFI_CMD_VERSION = 3,
+    SMFI_CMD_DEBUG = 4,
     //TODO
 };
 
@@ -62,6 +64,7 @@ void smfi_init(void) {
 }
 
 void smfi_event(void) {
+    int i;
     if (smfi_cmd[0]) {
         // Default to success
         smfi_cmd[1] = SMFI_RES_OK;
@@ -80,19 +83,29 @@ void smfi_event(void) {
             case SMFI_CMD_VERSION:
                 strncpy(&smfi_cmd[2], version(), ARRAY_SIZE(smfi_cmd) - 2);
                 break;
+            case SMFI_CMD_DEBUG:
+                for (i = 2; i < ARRAY_SIZE(smfi_cmd) - 2; i++) {
+                    uint8_t b = smfi_cmd[i];
+                    if (b == 0) break;
+                    putchar(b);
+                }
+            default:
+                // Command not found
+                smfi_cmd[1] = SMFI_RES_ERR;
+                break;
         }
 
         // Mark command as finished
         smfi_cmd[0] = SMFI_CMD_NONE;
     }
-
-    if (smfi_dbg[0]) {
-        int i;
-        for(i = 1; (i <= (int)smfi_dbg[0]) && (i < ARRAY_SIZE(smfi_dbg)); i++) {
-            putchar(smfi_dbg[i]);
-        }
-
-        // Mark debug transaction as complete
-        smfi_dbg[0] = 0;
-    }
+}
+
+void smfi_debug(unsigned char byte) {
+    int tail = (int)smfi_dbg[0];
+    tail++;
+    if (tail >= ARRAY_SIZE(smfi_dbg)) {
+        tail = 1;
+    }
+    smfi_dbg[tail] = byte;
+    smfi_dbg[0] = (uint8_t)tail;
 }
diff --git a/src/board/system76/galp3-c/stdio.c b/src/board/system76/galp3-c/stdio.c
index 9bdb177..6cb8951 100644
--- a/src/board/system76/galp3-c/stdio.c
+++ b/src/board/system76/galp3-c/stdio.c
@@ -1,5 +1,7 @@
 #include <stdio.h>
 
+#include <board/smfi.h>
+
 #ifdef SERIAL_DEBUGGER
     #include <mcs51/8051.h>
 #endif
@@ -10,6 +12,7 @@
 
 int putchar(int c) {
     unsigned char byte = (unsigned char)c;
+    smfi_debug(byte);
 #ifdef SERIAL_DEBUGGER
     SBUF = byte;
 #endif
diff --git a/src/board/system76/lemp9/board.mk b/src/board/system76/lemp9/board.mk
index 5ff2249..4e78ea3 100644
--- a/src/board/system76/lemp9/board.mk
+++ b/src/board/system76/lemp9/board.mk
@@ -11,10 +11,10 @@ SRC+=$(BOARD_DIR)/keymap/$(KEYMAP).c
 # 3 - INFO
 # 4 - DEBUG
 # 5 - TRACE
-CFLAGS+=-DLEVEL=2
+CFLAGS+=-DLEVEL=4
 
 # Enable I2C debug on 0x76
-CFLAGS+=-DI2C_DEBUGGER=0x76
+#CFLAGS+=-DI2C_DEBUGGER=0x76
 
 # Set battery I2C bus
 CFLAGS+=-DI2C_SMBUS=I2C_4
@@ -38,6 +38,10 @@ include $(SCRATCH_DIR)/scratch.mk
 CFLAGS+=-I$(BUILD)/include
 INCLUDE+=$(BUILD)/include/scratch.h
 
+console:
+	cargo build --manifest-path ecflash/Cargo.toml --example smfi --release
+	sudo ecflash/target/release/examples/smfi
+
 flash: $(BUILD)/ec.rom
 	cargo build --manifest-path ecflash/Cargo.toml --example isp --release
 	sudo ecflash/target/release/examples/isp --internal $<
@@ -45,7 +49,3 @@ flash: $(BUILD)/ec.rom
 isp: $(BUILD)/ec.rom
 	cargo build --manifest-path ecflash/Cargo.toml --example isp --release
 	sudo ecflash/target/release/examples/isp $<
-
-version:
-	cargo build --manifest-path ecflash/Cargo.toml --example smfi --release
-	sudo ecflash/target/release/examples/smfi
diff --git a/src/board/system76/lemp9/include/board/smfi.h b/src/board/system76/lemp9/include/board/smfi.h
index 532ef23..4ee0398 100644
--- a/src/board/system76/lemp9/include/board/smfi.h
+++ b/src/board/system76/lemp9/include/board/smfi.h
@@ -3,5 +3,6 @@
 
 void smfi_init(void);
 void smfi_event(void);
+void smfi_debug(unsigned char byte);
 
 #endif // _BOARD_SMFI_H
diff --git a/src/board/system76/lemp9/smfi.c b/src/board/system76/lemp9/smfi.c
index e550adf..ff95768 100644
--- a/src/board/system76/lemp9/smfi.c
+++ b/src/board/system76/lemp9/smfi.c
@@ -27,6 +27,7 @@ enum SmfiCmd {
     SMFI_CMD_PROBE = 1,
     SMFI_CMD_BOARD = 2,
     SMFI_CMD_VERSION = 3,
+    SMFI_CMD_DEBUG = 4,
     //TODO
 };
 
@@ -63,6 +64,7 @@ void smfi_init(void) {
 }
 
 void smfi_event(void) {
+    int i;
     if (smfi_cmd[0]) {
         // Default to success
         smfi_cmd[1] = SMFI_RES_OK;
@@ -81,19 +83,29 @@ void smfi_event(void) {
             case SMFI_CMD_VERSION:
                 strncpy(&smfi_cmd[2], version(), ARRAY_SIZE(smfi_cmd) - 2);
                 break;
+            case SMFI_CMD_DEBUG:
+                for (i = 2; i < ARRAY_SIZE(smfi_cmd) - 2; i++) {
+                    uint8_t b = smfi_cmd[i];
+                    if (b == 0) break;
+                    putchar(b);
+                }
+            default:
+                // Command not found
+                smfi_cmd[1] = SMFI_RES_ERR;
+                break;
         }
 
         // Mark command as finished
         smfi_cmd[0] = SMFI_CMD_NONE;
     }
-
-    if (smfi_dbg[0]) {
-        int i;
-        for(i = 1; (i <= (int)smfi_dbg[0]) && (i < ARRAY_SIZE(smfi_dbg)); i++) {
-            putchar(smfi_dbg[i]);
-        }
-
-        // Mark debug transaction as complete
-        smfi_dbg[0] = 0;
-    }
+}
+
+void smfi_debug(unsigned char byte) {
+    int tail = (int)smfi_dbg[0];
+    tail++;
+    if (tail >= ARRAY_SIZE(smfi_dbg)) {
+        tail = 1;
+    }
+    smfi_dbg[tail] = byte;
+    smfi_dbg[0] = (uint8_t)tail;
 }
diff --git a/src/board/system76/lemp9/stdio.c b/src/board/system76/lemp9/stdio.c
index 9bdb177..6cb8951 100644
--- a/src/board/system76/lemp9/stdio.c
+++ b/src/board/system76/lemp9/stdio.c
@@ -1,5 +1,7 @@
 #include <stdio.h>
 
+#include <board/smfi.h>
+
 #ifdef SERIAL_DEBUGGER
     #include <mcs51/8051.h>
 #endif
@@ -10,6 +12,7 @@
 
 int putchar(int c) {
     unsigned char byte = (unsigned char)c;
+    smfi_debug(byte);
 #ifdef SERIAL_DEBUGGER
     SBUF = byte;
 #endif

From 1e203038515de1aee920a56683c14c55245e9908 Mon Sep 17 00:00:00 2001
From: Jeremy Soller <jeremy@system76.com>
Date: Mon, 17 Feb 2020 13:46:39 -0700
Subject: [PATCH 4/4] Fix battery indicator when charged

---
 src/board/system76/darp5/acpi.c   | 10 ++++++++--
 src/board/system76/galp3-c/acpi.c | 10 ++++++++--
 src/board/system76/lemp9/acpi.c   | 10 ++++++++--
 3 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/src/board/system76/darp5/acpi.c b/src/board/system76/darp5/acpi.c
index 4063b27..c48b5c4 100644
--- a/src/board/system76/darp5/acpi.c
+++ b/src/board/system76/darp5/acpi.c
@@ -59,9 +59,15 @@ uint8_t acpi_read(uint8_t addr) {
         ACPI_16(0x1A, battery_full_capacity);
         ACPI_16(0x22, battery_design_voltage);
 
-        // Bypass status test in ACPI - TODO
         case 0x26:
-            data |= 1 << 1;
+            // If AC adapter connected
+            if (!gpio_get(&ACIN_N)) {
+                // And battery is not fully charged
+                if (!(battery_status & 0x0020)) {
+                    // Battery is charging
+                    data |= 1 << 1;
+                }
+            }
             break;
 
         ACPI_16(0x2A, battery_current);
diff --git a/src/board/system76/galp3-c/acpi.c b/src/board/system76/galp3-c/acpi.c
index 2ffb5c9..5df0af5 100644
--- a/src/board/system76/galp3-c/acpi.c
+++ b/src/board/system76/galp3-c/acpi.c
@@ -74,9 +74,15 @@ uint8_t acpi_read(uint8_t addr) {
         ACPI_16(0x1A, battery_full_capacity);
         ACPI_16(0x22, battery_design_voltage);
 
-        // Bypass status test in ACPI - TODO
         case 0x26:
-            data |= 1 << 1;
+            // If AC adapter connected
+            if (!gpio_get(&ACIN_N)) {
+                // And battery is not fully charged
+                if (!(battery_status & 0x0020)) {
+                    // Battery is charging
+                    data |= 1 << 1;
+                }
+            }
             break;
 
         ACPI_16(0x2A, battery_current);
diff --git a/src/board/system76/lemp9/acpi.c b/src/board/system76/lemp9/acpi.c
index bd9e345..ec0b79c 100644
--- a/src/board/system76/lemp9/acpi.c
+++ b/src/board/system76/lemp9/acpi.c
@@ -74,9 +74,15 @@ uint8_t acpi_read(uint8_t addr) {
         ACPI_16(0x1A, battery_full_capacity);
         ACPI_16(0x22, battery_design_voltage);
 
-        // Bypass status test in ACPI - TODO
         case 0x26:
-            data |= 1 << 1;
+            // If AC adapter connected
+            if (!gpio_get(&ACIN_N)) {
+                // And battery is not fully charged
+                if (!(battery_status & 0x0020)) {
+                    // Battery is charging
+                    data |= 1 << 1;
+                }
+            }
             break;
 
         ACPI_16(0x2A, battery_current);