From 95953555779dfe2715d67b33bc252dcbdb09f365 Mon Sep 17 00:00:00 2001
From: MorganTL <chengtz414@gmail.com>
Date: Tue, 1 Aug 2023 16:20:56 +0800
Subject: [PATCH] fix SPF LED stay on after removal

---
 src/runtime/src/main.rs | 33 ++++++++++++++-------------------
 src/satman/src/main.rs  | 19 +++++++++++++------
 2 files changed, 27 insertions(+), 25 deletions(-)

diff --git a/src/runtime/src/main.rs b/src/runtime/src/main.rs
index 3001b87..207bd10 100644
--- a/src/runtime/src/main.rs
+++ b/src/runtime/src/main.rs
@@ -103,19 +103,13 @@ async fn report_async_rtio_errors() {
 }
 
 #[cfg(all(feature = "target_kasli_soc", has_drtio))]
-static mut SEEN_RTIO_LED: u8 = 0;
-#[cfg(all(feature = "target_kasli_soc", has_drtio))]
-static mut READ_RTIO_LED: u8 = 0;
+static mut LAST_VIRTUAL_LED_STATUS: u8 = 0;
 
 #[cfg(all(feature = "target_kasli_soc", has_drtio))]
-fn wait_for_rtio_led_change() -> nb::Result<(), Void> {
+fn wait_for_virtual_leds_change() -> nb::Result<(), Void> {
     unsafe {
-        let len: usize = pl::csr::DRTIO.len();
-        READ_RTIO_LED = 0;
-        for linkno in 0..len {
-            READ_RTIO_LED |= (pl::csr::DRTIO[linkno].rx_up_read)() << linkno;
-        }
-        if READ_RTIO_LED != SEEN_RTIO_LED {
+        if pl::csr::virtual_leds::status_read() != LAST_VIRTUAL_LED_STATUS {
+            LAST_VIRTUAL_LED_STATUS = pl::csr::virtual_leds::status_read(); 
             Ok(())
         } else {
             Err(nb::Error::WouldBlock)
@@ -124,15 +118,16 @@ fn wait_for_rtio_led_change() -> nb::Result<(), Void> {
 }
 #[cfg(all(feature = "target_kasli_soc", has_drtio))]
 async fn async_rtio_led() {
-    let _ = block_async!(wait_for_rtio_led_change()).await;
-    unsafe {
-        let i2c = (&mut i2c::I2C_BUS).as_mut().unwrap();
-        for expander_i in 0..=1 {
-            let mut io_expander = io_expander::IoExpander::new(i2c, expander_i).unwrap();
-            io_expander.service().expect("I2C I/O expander service failed");
-        }
-        SEEN_RTIO_LED = READ_RTIO_LED;
-    };
+    let i2c0 = unsafe { (&mut i2c::I2C_BUS).as_mut().unwrap() };
+    let i2c1 = unsafe { (&mut i2c::I2C_BUS).as_mut().unwrap() };
+    let mut io_expander0 = io_expander::IoExpander::new(i2c0, 0).unwrap();
+    let mut io_expander1 = io_expander::IoExpander::new(i2c1, 1).unwrap();
+    loop{
+        let _ = block_async!(wait_for_virtual_leds_change()).await;
+        info!("switching");
+        io_expander0.service().expect("I2C I/O expander #0 service failed");
+        io_expander1.service().expect("I2C I/O expander #1 service failed");
+    }
 }
 
 static mut LOG_BUFFER: [u8; 1 << 17] = [0; 1 << 17];
diff --git a/src/satman/src/main.rs b/src/satman/src/main.rs
index a819b01..a044472 100644
--- a/src/satman/src/main.rs
+++ b/src/satman/src/main.rs
@@ -655,6 +655,13 @@ pub extern "C" fn main_core0() -> i32 {
 
     let mut hardware_tick_ts = 0;
 
+    let mut i2c0 = I2c::i2c0();
+    let mut i2c1 = I2c::i2c0();
+    i2c0.init().expect("I2C0 initialization failed");
+    i2c1.init().expect("I2C1 initialization failed");    
+    let mut io_expander0 = io_expander::IoExpander::new(&mut i2c0, 0).unwrap();
+    let mut io_expander1 = io_expander::IoExpander::new(&mut i2c1, 1).unwrap();
+
     loop {
         while !drtiosat_link_rx_up() {
             drtiosat_process_errors();
@@ -663,9 +670,9 @@ pub extern "C" fn main_core0() -> i32 {
                 rep.service(&routing_table, rank, &mut timer);
             }
             #[cfg(all(feature = "target_kasli_soc", has_drtio))]
-            for expander_i in 0..=1 {
-                let mut io_expander = io_expander::IoExpander::new(&mut i2c, expander_i).unwrap();
-                io_expander.service().expect("I2C I/O expander service failed")
+            {
+                io_expander0.service().expect("I2C I/O expander #0 service failed");
+                io_expander1.service().expect("I2C I/O expander #1 service failed");
             }
 
             hardware_tick(&mut hardware_tick_ts, &mut timer);
@@ -705,9 +712,9 @@ pub extern "C" fn main_core0() -> i32 {
                 rep.service(&routing_table, rank, &mut timer);
             }
             #[cfg(all(feature = "target_kasli_soc", has_drtio))]
-            for expander_i in 0..=1 {
-                let mut io_expander = io_expander::IoExpander::new(&mut i2c, expander_i).unwrap();
-                io_expander.service().expect("I2C I/O expander service failed")
+            {
+                io_expander0.service().expect("I2C I/O expander #0 service failed");
+                io_expander1.service().expect("I2C I/O expander #1 service failed");
             }
             hardware_tick(&mut hardware_tick_ts, &mut timer);
             if drtiosat_tsc_loaded() {