From 076ada4c0233d2f89c89cda4c01910a86add90ac Mon Sep 17 00:00:00 2001
From: Dario Nieuwenhuis <dirbaio@dirbaio.net>
Date: Sun, 25 Dec 2022 22:50:27 +0100
Subject: [PATCH] Add feature to display console logs from the wifi firmware.

---
 Cargo.toml                     |  3 ++
 examples/rpi-pico-w/Cargo.toml |  2 +-
 src/lib.rs                     | 81 ++++++++++++++++++++++++++++++++++
 src/structs.rs                 | 26 +++++++++++
 4 files changed, 111 insertions(+), 1 deletion(-)

diff --git a/Cargo.toml b/Cargo.toml
index cc19c938..dadfb5c5 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -7,6 +7,9 @@ edition = "2021"
 defmt = ["dep:defmt"]
 log = ["dep:log"]
 
+# Fetch console logs from the WiFi firmware and forward them to `log` or `defmt`.
+firmware-logs = []
+
 [dependencies]
 embassy-time = { version = "0.1.0" }
 embassy-sync = { version = "0.1.0" }
diff --git a/examples/rpi-pico-w/Cargo.toml b/examples/rpi-pico-w/Cargo.toml
index bb44667d..b817289e 100644
--- a/examples/rpi-pico-w/Cargo.toml
+++ b/examples/rpi-pico-w/Cargo.toml
@@ -5,7 +5,7 @@ edition = "2021"
 
 
 [dependencies]
-cyw43 = { path = "../../", features = ["defmt"]}
+cyw43 = { path = "../../", features = ["defmt", "firmware-logs"]}
 embassy-executor = { version = "0.1.0",  features = ["defmt", "integrated-timers"] }
 embassy-time = { version = "0.1.0",  features = ["defmt", "defmt-timestamp-uptime"] }
 embassy-rp = { version = "0.1.0",  features = ["defmt", "unstable-traits", "nightly", "unstable-pac", "time-driver"] }
diff --git a/src/lib.rs b/src/lib.rs
index 43082175..883e669d 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -575,6 +575,17 @@ pub struct Runner<'a, PWR, SPI> {
     backplane_window: u32,
 
     sdpcm_seq_max: u8,
+
+    #[cfg(feature = "firmware-logs")]
+    log: LogState,
+}
+
+#[cfg(feature = "firmware-logs")]
+struct LogState {
+    addr: u32,
+    last_idx: usize,
+    buf: [u8; 256],
+    buf_count: usize,
 }
 
 pub async fn new<'a, PWR, SPI>(
@@ -598,6 +609,14 @@ where
         backplane_window: 0xAAAA_AAAA,
 
         sdpcm_seq_max: 1,
+
+        #[cfg(feature = "firmware-logs")]
+        log: LogState {
+            addr: 0,
+            last_idx: 0,
+            buf: [0; 256],
+            buf_count: 0,
+        },
     };
 
     runner.init(firmware).await;
@@ -715,12 +734,74 @@ where
         //while self.read8(FUNC_BACKPLANE, REG_BACKPLANE_CHIP_CLOCK_CSR).await & 0x80 == 0 {}
         //info!("clock ok");
 
+        #[cfg(feature = "firmware-logs")]
+        self.log_init().await;
+
         info!("init done ");
     }
 
+    #[cfg(feature = "firmware-logs")]
+    async fn log_init(&mut self) {
+        // Initialize shared memory for logging.
+
+        let shared_addr = self
+            .bp_read32(CHIP.atcm_ram_base_address + CHIP.chip_ram_size - 4 - CHIP.socram_srmem_size)
+            .await;
+        info!("shared_addr {:08x}", shared_addr);
+
+        let mut shared = [0; SharedMemData::SIZE];
+        self.bp_read(shared_addr, &mut shared).await;
+        let shared = SharedMemData::from_bytes(&shared);
+        info!("shared: {:08x}", shared);
+
+        self.log.addr = shared.console_addr + 8;
+    }
+
+    #[cfg(feature = "firmware-logs")]
+    async fn log_read(&mut self) {
+        // Read log struct
+        let mut log = [0; SharedMemLog::SIZE];
+        self.bp_read(self.log.addr, &mut log).await;
+        let log = SharedMemLog::from_bytes(&log);
+
+        let idx = log.idx as usize;
+
+        // If pointer hasn't moved, no need to do anything.
+        if idx == self.log.last_idx {
+            return;
+        }
+
+        // Read entire buf for now. We could read only what we need, but then we
+        // run into annoying alignment issues in `bp_read`.
+        let mut buf = [0; 0x400];
+        self.bp_read(log.buf, &mut buf).await;
+
+        while self.log.last_idx != idx as usize {
+            let b = buf[self.log.last_idx];
+            if b == b'\r' || b == b'\n' {
+                if self.log.buf_count != 0 {
+                    let s = unsafe { core::str::from_utf8_unchecked(&self.log.buf[..self.log.buf_count]) };
+                    debug!("LOGS: {}", s);
+                    self.log.buf_count = 0;
+                }
+            } else if self.log.buf_count < self.log.buf.len() {
+                self.log.buf[self.log.buf_count] = b;
+                self.log.buf_count += 1;
+            }
+
+            self.log.last_idx += 1;
+            if self.log.last_idx == 0x400 {
+                self.log.last_idx = 0;
+            }
+        }
+    }
+
     pub async fn run(mut self) -> ! {
         let mut buf = [0; 512];
         loop {
+            #[cfg(feature = "firmware-logs")]
+            self.log_read().await;
+
             // Send stuff
             // TODO flow control not yet complete
             if !self.has_credit() {
diff --git a/src/structs.rs b/src/structs.rs
index 6d4525a4..41a34066 100644
--- a/src/structs.rs
+++ b/src/structs.rs
@@ -18,6 +18,32 @@ macro_rules! impl_bytes {
     };
 }
 
+#[derive(Clone, Copy)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[repr(C)]
+pub struct SharedMemData {
+    pub flags: u32,
+    pub trap_addr: u32,
+    pub assert_exp_addr: u32,
+    pub assert_file_addr: u32,
+    pub assert_line: u32,
+    pub console_addr: u32,
+    pub msgtrace_addr: u32,
+    pub fwid: u32,
+}
+impl_bytes!(SharedMemData);
+
+#[derive(Clone, Copy)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[repr(C)]
+pub struct SharedMemLog {
+    pub buf: u32,
+    pub buf_size: u32,
+    pub idx: u32,
+    pub out_idx: u32,
+}
+impl_bytes!(SharedMemLog);
+
 #[derive(Clone, Copy)]
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
 #[repr(C)]