From 74d1df3ff0541a91ec0af296d8ab70b4085f2381 Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Wed, 7 Mar 2018 10:56:12 +0800
Subject: [PATCH] firmware: implement si5324 skew calibration

---
 artiq/firmware/libboard_artiq/si5324.rs | 75 ++++++++++++++++++++++++-
 artiq/firmware/satman/main.rs           |  2 +-
 2 files changed, 74 insertions(+), 3 deletions(-)

diff --git a/artiq/firmware/libboard_artiq/si5324.rs b/artiq/firmware/libboard_artiq/si5324.rs
index 2738bec71..9b93eae96 100644
--- a/artiq/firmware/libboard_artiq/si5324.rs
+++ b/artiq/firmware/libboard_artiq/si5324.rs
@@ -278,7 +278,78 @@ pub fn select_recovered_clock(rc: bool) -> Result<()> {
 }
 
 #[cfg(has_si_phaser)]
-pub fn calibrate_skew() -> Result<()> {
-    // TODO: implement
+fn phase_shift(direction: u8) {
+    unsafe {
+        csr::si_phaser::phase_shift_write(direction);
+        while csr::si_phaser::phase_shift_done_read() == 0 {}
+    }
+    // wait for the Si5324 loop to stabilize
+    clock::spin_us(500);
+}
+
+#[cfg(has_si_phaser)]
+fn get_phaser_sample() -> bool {
+    let mut sample = true;
+    for _ in 0..32 {
+        if unsafe { csr::si_phaser::sample_result_read() } == 0 {
+            sample = false;
+        }
+    }
+    sample
+}
+
+#[cfg(has_si_phaser)]
+const PS_MARGIN: u32 = 28;
+
+#[cfg(has_si_phaser)]
+fn get_stable_phaser_sample() -> (bool, u32) {
+    let mut nshifts: u32 = 0;
+    loop {
+        let s1 = get_phaser_sample();
+        for _ in 0..PS_MARGIN {
+            phase_shift(1);
+        }
+        let s2 = get_phaser_sample();
+        for _ in 0..PS_MARGIN {
+            phase_shift(1);
+        }
+        let s3 = get_phaser_sample();
+        nshifts += 2*PS_MARGIN;
+        if s1 == s2 && s2 == s3 {
+            for _ in 0..PS_MARGIN {
+                phase_shift(0);
+            }
+            nshifts -= PS_MARGIN;
+            return (s2, nshifts);
+        }
+    }
+}
+
+#[cfg(has_si_phaser)]
+pub fn calibrate_skew(skew: u16) -> Result<()> {
+    // Get into a 0 region
+    let (s1, mut nshifts) = get_stable_phaser_sample();
+    if s1 {
+        while get_phaser_sample() {
+            phase_shift(1);
+            nshifts += 1;
+        }
+        for _ in 0..PS_MARGIN {
+            phase_shift(1);
+        }
+        nshifts += PS_MARGIN;
+    }
+
+    // Get to the 0->1 transition
+    while !get_phaser_sample() {
+        phase_shift(1);
+        nshifts += 1;
+    }
+    info!("nshifts to 0->1 siphaser transition: {} ({}deg)", nshifts, nshifts*360/(56*8));
+
+    // Apply specified skew referenced to that transition
+    for _ in 0..skew {
+        phase_shift(1);
+    }
     Ok(())
 }
diff --git a/artiq/firmware/satman/main.rs b/artiq/firmware/satman/main.rs
index e1b60179d..19f0d2517 100644
--- a/artiq/firmware/satman/main.rs
+++ b/artiq/firmware/satman/main.rs
@@ -236,7 +236,7 @@ fn startup() {
         }
         info!("link is up, switching to recovered clock");
         si5324::select_recovered_clock(true).expect("failed to switch clocks");
-        si5324::calibrate_skew().expect("failed to calibrate skew");
+        si5324::calibrate_skew(32).expect("failed to calibrate skew");
         drtio_reset(false);
         drtio_reset_phy(false);
         while drtio_link_rx_up() {