From 84f9aad7d8323119238515cb7d614cf8992ac738 Mon Sep 17 00:00:00 2001
From: Jeremy Soller <jackpot51@gmail.com>
Date: Thu, 10 Feb 2022 09:30:13 -0700
Subject: [PATCH] Add force console mode to arduino firmware

---
 src/board/arduino/mega2560/parallel.c | 64 +++++++++++++++++++++++++++
 src/board/system76/common/common.mk   |  5 +++
 2 files changed, 69 insertions(+)

diff --git a/src/board/arduino/mega2560/parallel.c b/src/board/arduino/mega2560/parallel.c
index eeb4d51..96ca038 100644
--- a/src/board/arduino/mega2560/parallel.c
+++ b/src/board/arduino/mega2560/parallel.c
@@ -327,11 +327,35 @@ bool parallel_peripheral_cycle(struct Parallel * port, uint8_t * data, bool * re
 
 static uint8_t ADDRESS_INDAR1 = 0x05;
 static uint8_t ADDRESS_INDDR = 0x08;
+static uint8_t ADDRESS_ECMSADDR0 = 0x2E;
+static uint8_t ADDRESS_ECMSADDR1 = 0x2F;
+static uint8_t ADDRESS_ECMSDATA = 0x30;
 
 static uint8_t ZERO = 0x00;
 static uint8_t SPI_ENABLE = 0xFE;
 static uint8_t SPI_DATA = 0xFD;
 
+int16_t parallel_ecms_read(struct Parallel *port, uint16_t addr, uint8_t * data, int16_t length) {
+    int16_t res;
+
+    res = parallel_set_address(port, &ADDRESS_ECMSADDR1, 1);
+    if (res < 0) return res;
+
+    res = parallel_write(port, ((uint8_t *)&addr) + 1, 1);
+    if (res < 0) return res;
+
+    res = parallel_set_address(port, &ADDRESS_ECMSADDR0, 1);
+    if (res < 0) return res;
+
+    res = parallel_write(port, (uint8_t *)&addr, 1);
+    if (res < 0) return res;
+
+    res = parallel_set_address(port, &ADDRESS_ECMSDATA, 1);
+    if (res < 0) return res;
+
+    return parallel_read(port, data, length);
+}
+
 // Disable chip
 int16_t parallel_spi_reset(struct Parallel *port) {
     int16_t res;
@@ -525,6 +549,46 @@ int parallel_main(void) {
 
                 break;
 
+            // Forced debug console (can be used on all firmware but may miss bytes)
+            case 'F':
+                serial_write(console_msg, sizeof(console_msg));
+
+                // We must be in host mode
+                parallel_state(port, PARALLEL_STATE_HOST);
+
+                uint16_t head = 0;
+                for (;;) {
+                    // Read current position
+                    res = parallel_ecms_read(port, 0xF00, data, 1);
+                    if (res < 0) goto err;
+
+                    uint16_t tail = (uint16_t)data[0];
+                    if (tail == 0 || head == tail) {
+                        // No new data
+                        continue;
+                    }
+
+                    if (head == 0) {
+                        // Set head if necessary
+                        head = tail;
+                        continue;
+                    }
+
+                    while (head != tail) {
+                        head += 1;
+                        if (head >= 256) { head = 1; }
+
+                        // Read byte at head
+                        res = parallel_ecms_read(port, 0xF00 + head, data, 1);
+                        if (res < 0) goto err;
+
+                        // Print read byte
+                        serial_write(data, 1);
+                    }
+                }
+
+                break;
+
             // Read data
             case 'R':
                 parallel_state(port, PARALLEL_STATE_HOST);
diff --git a/src/board/system76/common/common.mk b/src/board/system76/common/common.mk
index 6a12de8..4a62c67 100644
--- a/src/board/system76/common/common.mk
+++ b/src/board/system76/common/common.mk
@@ -62,6 +62,11 @@ console_external:
 	sleep 1 && echo C | sudo tee "$(PROGRAMMER)" &
 	sudo tio -b 1000000 -m INLCRNL -t "$(PROGRAMMER)"
 
+console_external_forced:
+	sudo test -c "$(PROGRAMMER)"
+	sleep 1 && echo F | sudo tee "$(PROGRAMMER)" &
+	sudo tio -b 1000000 -m INLCRNL -t "$(PROGRAMMER)"
+
 flash_internal: $(BUILD)/ec.rom
 	cargo build --manifest-path tool/Cargo.toml --release
 	sudo tool/target/release/system76_ectool flash $<