From 4ce90862405a3b59d0e3ecffd0a30df1ea064779 Mon Sep 17 00:00:00 2001
From: Mateusz Faderewski <sc@mateuszfaderewski.pl>
Date: Sat, 4 Mar 2023 11:31:56 +0100
Subject: [PATCH] added debug info

---
 sw/deployer/src/main.rs       |  2 +
 sw/deployer/src/sc64/mod.rs   | 31 +++++++++++++--
 sw/deployer/src/sc64/types.rs | 74 +++++++++++++++++++++++++++++++++++
 3 files changed, 103 insertions(+), 4 deletions(-)

diff --git a/sw/deployer/src/main.rs b/sw/deployer/src/main.rs
index bff2f65..496de97 100644
--- a/sw/deployer/src/main.rs
+++ b/sw/deployer/src/main.rs
@@ -406,6 +406,8 @@ fn handle_info_command(sn: Option<String>) -> Result<(), sc64::Error> {
     println!(" Button mode:         {}", state.button_mode);
     println!(" Button state:        {}", state.button_state);
     println!(" LED blink:           {}", state.led_enable);
+    println!(" FPGA debug data:     {}", state.fpga_debug_data);
+    println!(" MCU stack usage:     {}", state.mcu_stack_usage);
 
     Ok(())
 }
diff --git a/sw/deployer/src/sc64/mod.rs b/sw/deployer/src/sc64/mod.rs
index 7058666..fe43eb3 100644
--- a/sw/deployer/src/sc64/mod.rs
+++ b/sw/deployer/src/sc64/mod.rs
@@ -9,8 +9,8 @@ pub use self::{
     error::Error,
     link::list_serial_devices,
     types::{
-        BootMode, DataPacket, DdDiskState, DdDriveType, DdMode, DebugPacket, DiskPacket, SaveType,
-        TvType,
+        BootMode, ButtonMode, ButtonState, CicSeed, DataPacket, DdDiskState, DdDriveType, DdMode,
+        DebugPacket, DiskPacket, FpgaDebugData, McuStackUsage, SaveType, Switch, TvType,
     },
 };
 
@@ -18,8 +18,7 @@ use self::{
     cic::{calculate_ipl3_checksum, guess_ipl3_seed, IPL3_LENGTH, IPL3_OFFSET},
     link::{Command, Link},
     types::{
-        get_config, get_setting, ButtonMode, ButtonState, CicSeed, Config, ConfigId,
-        FirmwareStatus, Setting, SettingId, Switch, UpdateStatus,
+        get_config, get_setting, Config, ConfigId, FirmwareStatus, Setting, SettingId, UpdateStatus,
     },
     utils::{args_from_vec, datetime_from_vec, u32_from_vec, vec_from_datetime},
 };
@@ -52,6 +51,8 @@ pub struct DeviceState {
     pub rom_extended_enable: Switch,
     pub led_enable: Switch,
     pub datetime: DateTime<Local>,
+    pub fpga_debug_data: FpgaDebugData,
+    pub mcu_stack_usage: McuStackUsage,
 }
 
 const SC64_V2_IDENTIFIER: &[u8; 4] = b"SCv2";
@@ -292,6 +293,26 @@ impl SC64 {
         )?;
         Ok(FirmwareStatus::try_from(utils::u32_from_vec(&data[0..4])?)?)
     }
+
+    fn command_debug_get(&mut self) -> Result<FpgaDebugData, Error> {
+        self.link
+            .execute_command(&mut Command {
+                id: b'?',
+                args: [0, 0],
+                data: vec![],
+            })?
+            .try_into()
+    }
+
+    fn command_stack_usage_get(&mut self) -> Result<McuStackUsage, Error> {
+        self.link
+            .execute_command(&mut Command {
+                id: b'%',
+                args: [0, 0],
+                data: vec![],
+            })?
+            .try_into()
+    }
 }
 
 impl SC64 {
@@ -452,6 +473,8 @@ impl SC64 {
             rom_extended_enable: get_config!(self, RomExtendedEnable)?,
             led_enable: get_setting!(self, LedEnable)?,
             datetime: self.get_datetime()?,
+            fpga_debug_data: self.command_debug_get()?,
+            mcu_stack_usage: self.command_stack_usage_get()?,
         })
     }
 
diff --git a/sw/deployer/src/sc64/types.rs b/sw/deployer/src/sc64/types.rs
index d533143..0b75199 100644
--- a/sw/deployer/src/sc64/types.rs
+++ b/sw/deployer/src/sc64/types.rs
@@ -733,6 +733,80 @@ impl TryFrom<u32> for UpdateStatus {
     }
 }
 
+pub struct FpgaDebugData {
+    pub last_pi_address: u32,
+    pub read_fifo_wait: bool,
+    pub read_fifo_failure: bool,
+    pub write_fifo_wait: bool,
+    pub write_fifo_failure: bool,
+}
+
+impl TryFrom<Vec<u8>> for FpgaDebugData {
+    type Error = Error;
+    fn try_from(value: Vec<u8>) -> Result<Self, Self::Error> {
+        if value.len() < 8 {
+            return Err(Error::new("Invalid data length for FPGA debug data"));
+        }
+        Ok(FpgaDebugData {
+            last_pi_address: u32_from_vec(&value[0..4])?,
+            read_fifo_wait: (value[7] & (1 << 0)) != 0,
+            read_fifo_failure: (value[7] & (1 << 1)) != 0,
+            write_fifo_wait: (value[7] & (1 << 2)) != 0,
+            write_fifo_failure: (value[7] & (1 << 3)) != 0,
+        })
+    }
+}
+
+impl Display for FpgaDebugData {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        f.write_fmt(format_args!("PI address: 0x{:08X}", self.last_pi_address))?;
+        if self.read_fifo_wait {
+            f.write_str(" RW")?;
+        }
+        if self.read_fifo_failure {
+            f.write_str(" RF")?;
+        }
+        if self.write_fifo_wait {
+            f.write_str(" WW")?;
+        }
+        if self.write_fifo_failure {
+            f.write_str(" WF")?;
+        }
+        Ok(())
+    }
+}
+
+pub struct McuStackUsage {
+    pub cic: u32,
+    pub rtc: u32,
+    pub led: u32,
+    pub gvr: u32,
+}
+
+impl TryFrom<Vec<u8>> for McuStackUsage {
+    type Error = Error;
+    fn try_from(value: Vec<u8>) -> Result<Self, Self::Error> {
+        if value.len() < 16 {
+            return Err(Error::new("Invalid data length for MCU stack usage"));
+        }
+        Ok(McuStackUsage {
+            cic: u32_from_vec(&value[0..4])?,
+            rtc: u32_from_vec(&value[4..8])?,
+            led: u32_from_vec(&value[8..12])?,
+            gvr: u32_from_vec(&value[12..16])?,
+        })
+    }
+}
+
+impl Display for McuStackUsage {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        f.write_fmt(format_args!(
+            "CIC: {}, RTC: {}, LED: {}, GVR: {}",
+            self.cic, self.rtc, self.led, self.gvr
+        ))
+    }
+}
+
 macro_rules! get_config {
     ($sc64:ident, $config:ident) => {{
         if let Config::$config(value) = $sc64.command_config_get(ConfigId::$config)? {