From fbe66684d54cfd87ef154a77f34f8b9d13873535 Mon Sep 17 00:00:00 2001
From: Simon Renblad <srenblad@m-labs.hk>
Date: Thu, 26 Sep 2024 17:38:12 +0800
Subject: [PATCH] wip monitor loss of lock

---
 src/libboard_artiq/src/si5324.rs | 13 ++++++++++---
 src/runtime/src/main.rs          |  2 ++
 src/runtime/src/rtio_clocking.rs | 20 ++++++++++++++++++--
 3 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/src/libboard_artiq/src/si5324.rs b/src/libboard_artiq/src/si5324.rs
index b53174d..8619100 100644
--- a/src/libboard_artiq/src/si5324.rs
+++ b/src/libboard_artiq/src/si5324.rs
@@ -142,7 +142,9 @@ fn read(i2c: &mut I2c, reg: u8) -> Result<u8> {
 }
 
 fn rmw<F>(i2c: &mut I2c, reg: u8, f: F) -> Result<()>
-where F: Fn(u8) -> u8 {
+where
+    F: Fn(u8) -> u8,
+{
     let value = read(i2c, reg)?;
     write(i2c, reg, f(value))?;
     Ok(())
@@ -171,7 +173,7 @@ fn has_ckin(i2c: &mut I2c, input: Input) -> Result<bool> {
     }
 }
 
-fn locked(i2c: &mut I2c) -> Result<bool> {
+pub fn locked(i2c: &mut I2c) -> Result<bool> {
     Ok((read(i2c, 130)? & 0x01) == 0) // LOL_INT=0
 }
 
@@ -225,7 +227,12 @@ pub fn bypass(i2c: &mut I2c, input: Input, timer: &mut GlobalTimer) -> Result<()
     Ok(())
 }
 
-pub fn setup(i2c: &mut I2c, settings: &FrequencySettings, input: Input, timer: &mut GlobalTimer) -> Result<()> {
+pub fn setup(
+    i2c: &mut I2c,
+    settings: &FrequencySettings,
+    input: Input,
+    timer: &mut GlobalTimer,
+) -> Result<()> {
     let s = map_frequency_settings(settings)?;
     let cksel_reg = match input {
         Input::Ckin1 => 0b00,
diff --git a/src/runtime/src/main.rs b/src/runtime/src/main.rs
index dd24e25..8a605f4 100644
--- a/src/runtime/src/main.rs
+++ b/src/runtime/src/main.rs
@@ -51,6 +51,8 @@ async fn io_expanders_service(
 ) {
     loop {
         task::r#yield().await;
+        #[cfg(has_si5324)]
+        rtio_clocking::monitor_loss_lock(&mut i2c_bus.borrow_mut())
         io_expander0
             .borrow_mut()
             .service(&mut i2c_bus.borrow_mut())
diff --git a/src/runtime/src/rtio_clocking.rs b/src/runtime/src/rtio_clocking.rs
index 06918b2..da54f0a 100644
--- a/src/runtime/src/rtio_clocking.rs
+++ b/src/runtime/src/rtio_clocking.rs
@@ -26,6 +26,20 @@ pub enum RtioClock {
     Ext0_Synth0_125to125,
 }
 
+#[cfg(has_si5324)]
+pub fn monitor_loss_lock(i2c_bus: &mut libboard_zynq::i2c::I2c) {
+    match si5324::locked(i2c_bus) {
+        Ok(val) => {
+            if !val {
+                warn!("si5324 detected loss of lock")
+            }
+        }
+        Err(s) => {
+            panic!(s)
+        }
+    }
+}
+
 #[allow(unreachable_code)]
 fn get_rtio_clock_cfg(cfg: &Config) -> RtioClock {
     let mut res = RtioClock::Default;
@@ -259,7 +273,8 @@ fn setup_si5324(i2c: &mut I2c, timer: &mut GlobalTimer, clk: RtioClock) {
             )
         }
     };
-    si5324::setup(i2c, &si5324_settings, si5324_ref_input, timer).expect("cannot initialize Si5324");
+    si5324::setup(i2c, &si5324_settings, si5324_ref_input, timer)
+        .expect("cannot initialize Si5324");
 }
 
 #[cfg(all(has_si549, has_wrpll))]
@@ -338,7 +353,8 @@ fn wrpll_setup(timer: &mut GlobalTimer, clk: RtioClock, si549_settings: &si549::
     };
 
     si549::helper_setup(timer, &si549_settings).expect("cannot initialize helper Si549");
-    si549::wrpll_refclk::setup(timer, mmcm_setting, mmcm_bypass).expect("cannot initialize ref clk for wrpll");
+    si549::wrpll_refclk::setup(timer, mmcm_setting, mmcm_bypass)
+        .expect("cannot initialize ref clk for wrpll");
     si549::wrpll::select_recovered_clock(true, timer);
 }