From e17b398483df6b9fcfc209b02426c5c4f5a371da Mon Sep 17 00:00:00 2001
From: mwojcik <mw@m-labs.hk>
Date: Wed, 4 Aug 2021 12:55:03 +0200
Subject: [PATCH] added siphaser driver code for drtio satellites

---
 src/libboard_artiq/src/lib.rs    |  8 +++-
 src/libboard_artiq/src/si5324.rs | 72 ++++++++++++++++++++++++++++++++
 2 files changed, 79 insertions(+), 1 deletion(-)

diff --git a/src/libboard_artiq/src/lib.rs b/src/libboard_artiq/src/lib.rs
index 3f87ef6..8376b1d 100644
--- a/src/libboard_artiq/src/lib.rs
+++ b/src/libboard_artiq/src/lib.rs
@@ -13,9 +13,15 @@ pub mod pl;
 #[cfg(has_drtio)]
 pub mod drtioaux;
 pub mod drtio_routing;
-pub mod si5324;
+
 pub mod logger;
 
+#[cfg(has_si5324)]
+pub mod si5324;
+
+#[cfg(has_siphaser)]
+pub mod siphaser;
+
 use core::{cmp, str};
 
 pub fn identifier_read(buf: &mut [u8]) -> &str {
diff --git a/src/libboard_artiq/src/si5324.rs b/src/libboard_artiq/src/si5324.rs
index c8a8a9a..e54ae2e 100644
--- a/src/libboard_artiq/src/si5324.rs
+++ b/src/libboard_artiq/src/si5324.rs
@@ -271,4 +271,76 @@ pub fn select_input(i2c: &mut I2c, input: Input, timer: GlobalTimer) -> Result<(
     }
     monitor_lock(i2c, timer)?;
     Ok(())
+}
+
+#[cfg(has_siphaser)]
+pub mod siphaser {
+    use super::*;
+    use pl::csr;
+
+    pub fn select_recovered_clock(i2c: &mut I2c, rc: bool, timer: GlobalTimer) -> Result<()> {
+        write(i2c, 3,   (read(3)? & 0xdf) | (1 << 5))?;  // DHOLD=1
+        unsafe {
+            csr::siphaser::switch_clocks_write(if rc { 1 } else { 0 });
+        }
+        write(i2c, 3,   (read(3)? & 0xdf) | (0 << 5))?;  // DHOLD=0
+        monitor_lock(timer)?;
+        Ok(())
+    }
+
+    fn phase_shift(direction: u8, timer: GlobalTimer) {
+        unsafe {
+            csr::siphaser::phase_shift_write(direction);
+            while csr::siphaser::phase_shift_done_read() == 0 {}
+        }
+        // wait for the Si5324 loop to stabilize
+        timer.delay_us(500);
+    }
+
+    fn has_error(timer: GlobalTimer) -> bool {
+        unsafe {
+            csr::siphaser::error_write(1);
+        }
+        timer.delay_us(5_000);
+        unsafe {
+            csr::siphaser::error_read() != 0
+        }
+    }
+
+    fn find_edge(target: bool, timer: GlobalTimer) -> Result<u32> {
+        let mut nshifts = 0;
+
+        let mut previous = has_error(timer);
+        loop {
+            phase_shift(1, timer);
+            nshifts += 1;
+            let current = has_error(timer);
+            if previous != target && current == target {
+                return Ok(nshifts);
+            }
+            if nshifts > 5000 {
+                return Err("failed to find timing error edge");
+            }
+            previous = current;
+        }
+    }
+
+    pub fn calibrate_skew(timer: GlobalTimer) -> Result<()> {
+        let jitter_margin = 32;
+        let lead = find_edge(false, timer)?;
+        for _ in 0..jitter_margin {
+            phase_shift(1, timer);
+        }
+        let width = find_edge(true, timer)? + jitter_margin;
+        // width is 360 degrees (one full rotation of the phase between s/h limits) minus jitter
+        info!("calibration successful, lead: {}, width: {} ({}deg)", lead, width, width*360/(56*8));
+
+        // Apply reverse phase shift for half the width to get into the
+        // middle of the working region.
+        for _ in 0..width/2 {
+            phase_shift(0, timer);
+        }
+
+        Ok(())
+    }
 }
\ No newline at end of file