Add grabber module

Signed-off-by: Egor Savkin <es@m-labs.hk>

src/gateware/kasli_soc.py

@@ -107,6 +107,7 @@ class GTPBootstrapClock(Module):
 class GenericStandalone(SoCCore):
     def __init__(self, description, acpki=False):
         self.acpki = acpki
+        clk_freq = description["rtio_frequency"]
 
         platform = kasli_soc.Platform()
         platform.toolchain.bitstream_commands.extend([
@@ -138,7 +139,8 @@ class GenericStandalone(SoCCore):
             Instance("BUFG", i_I=clk_synth_se, o_O=clk_synth_se_buf),
         ]
         fix_serdes_timing_path(platform)
-        self.submodules.bootstrap = GTPBootstrapClock(self.platform, description["rtio_frequency"])
+        self.submodules.bootstrap = GTPBootstrapClock(self.platform, clk_freq)
+        self.config["CLOCK_FREQUENCY"] = int(clk_freq)
 
         self.submodules.sys_crg = zynq_clocking.SYSCRG(self.platform, self.ps7, clk_synth_se_buf)
         platform.add_false_path_constraints(
@@ -229,6 +231,7 @@ class GenericMaster(SoCCore):
             pads=data_pads,
             clk_freq=clk_freq)
         self.csr_devices.append("gt_drtio")
+        self.config["CLOCK_FREQUENCY"] = int(clk_freq)
 
         txout_buf = Signal()
         gtx0 = self.gt_drtio.gtxs[0]
@@ -532,6 +535,7 @@ class GenericSatellite(SoCCore):
 
         rtio_clk_period = 1e9/clk_freq
         self.config["RTIO_FREQUENCY"] = str(clk_freq/1e6)
+        self.config["CLOCK_FREQUENCY"] = int(clk_freq)
 
         self.submodules.siphaser = SiPhaser7Series(
             si5324_clkin=platform.request("cdr_clk"),

src/libboard_artiq/src/grabber.rs

@@ -0,0 +1,163 @@
+use log::info;
+
+use crate::pl::csr;
+
+#[derive(PartialEq, Clone, Copy)]
+enum State {
+    Reset,
+    ExitReset,
+    Lock,
+    Align,
+    Watch,
+}
+
+#[derive(Clone, Copy)]
+struct Info {
+    state: State,
+    frame_size: (u16, u16),
+}
+
+static mut INFO: [Info; csr::GRABBER_LEN] = [Info {
+    state: State::Reset,
+    frame_size: (0, 0),
+}; csr::GRABBER_LEN];
+
+fn get_pll_reset(g: usize) -> bool {
+    unsafe { (csr::GRABBER[g].pll_reset_read)() != 0 }
+}
+
+fn set_pll_reset(g: usize, reset: bool) {
+    let val = if reset { 1 } else { 0 };
+    unsafe { (csr::GRABBER[g].pll_reset_write)(val) }
+}
+
+fn pll_locked(g: usize) -> bool {
+    unsafe { (csr::GRABBER[g].pll_locked_read)() != 0 }
+}
+
+fn clock_pattern_ok(g: usize) -> bool {
+    unsafe { (csr::GRABBER[g].clk_sampled_read)() == 0b1100011 }
+}
+
+fn clock_pattern_ok_filter(g: usize) -> bool {
+    for _ in 0..128 {
+        if !clock_pattern_ok(g) {
+            return false;
+        }
+    }
+    true
+}
+
+fn phase_shift(g: usize, direction: u8) {
+    unsafe {
+        (csr::GRABBER[g].phase_shift_write)(direction);
+        while (csr::GRABBER[g].phase_shift_done_read)() == 0 {}
+    }
+}
+
+fn clock_align(g: usize) -> bool {
+    while clock_pattern_ok_filter(g) {
+        phase_shift(g, 1);
+    }
+    phase_shift(g, 1);
+
+    let mut count = 0;
+    while !clock_pattern_ok_filter(g) {
+        phase_shift(g, 1);
+        count += 1;
+        if count > 1024 {
+            return false;
+        }
+    }
+
+    let mut window = 1;
+    phase_shift(g, 1);
+    while clock_pattern_ok_filter(g) {
+        phase_shift(g, 1);
+        window += 1;
+    }
+
+    for _ in 0..window / 2 {
+        phase_shift(g, 0);
+    }
+
+    true
+}
+
+fn get_last_pixels(g: usize) -> (u16, u16) {
+    unsafe { ((csr::GRABBER[g].last_x_read)(), (csr::GRABBER[g].last_y_read)()) }
+}
+
+fn get_video_clock(g: usize) -> u32 {
+    let freq_count = unsafe { (csr::GRABBER[g].freq_count_read)() } as u32;
+    2 * freq_count * (csr::CONFIG_CLOCK_FREQUENCY / 1000) / (511 * 1000)
+}
+
+pub fn tick() {
+    for g in 0..csr::GRABBER.len() {
+        let next = match unsafe { INFO[g].state } {
+            State::Reset => {
+                set_pll_reset(g, true);
+                unsafe {
+                    INFO[g].frame_size = (0, 0);
+                }
+                State::ExitReset
+            }
+            State::ExitReset => {
+                if get_pll_reset(g) {
+                    set_pll_reset(g, false);
+                    State::Lock
+                } else {
+                    State::ExitReset
+                }
+            }
+            State::Lock => {
+                if pll_locked(g) {
+                    info!("grabber{} locked: {}MHz", g, get_video_clock(g));
+                    State::Align
+                } else {
+                    State::Lock
+                }
+            }
+            State::Align => {
+                if pll_locked(g) {
+                    if clock_align(g) {
+                        info!("grabber{} alignment success", g);
+                        State::Watch
+                    } else {
+                        info!("grabber{} alignment failure", g);
+                        State::Reset
+                    }
+                } else {
+                    info!("grabber{} lock lost", g);
+                    State::Reset
+                }
+            }
+            State::Watch => {
+                if pll_locked(g) {
+                    if clock_pattern_ok(g) {
+                        let last_xy = get_last_pixels(g);
+                        if last_xy != unsafe { INFO[g].frame_size } {
+                            // x capture is on ~LVAL which is after
+                            // the last increment on DVAL
+                            // y capture is on ~FVAL which coincides with the
+                            // last increment on ~LVAL
+                            info!("grabber{} frame size: {}x{}", g, last_xy.0, last_xy.1 + 1);
+                            unsafe { INFO[g].frame_size = last_xy }
+                        }
+                        State::Watch
+                    } else {
+                        info!("grabber{} alignment lost", g);
+                        State::Reset
+                    }
+                } else {
+                    info!("grabber{} lock lost", g);
+                    State::Reset
+                }
+            }
+        };
+        unsafe {
+            INFO[g].state = next;
+        }
+    }
+}

src/libboard_artiq/src/lib.rs

@@ -31,6 +31,8 @@ pub mod mem;
 pub mod pl;
 #[cfg(has_drtio_eem)]
 pub mod drtio_eem;
+#[cfg(has_grabber)]
+pub mod grabber;
 #[cfg(has_si5324)]
 pub mod si5324;
 

src/runtime/src/main.rs

@@ -57,6 +57,20 @@ async fn io_expanders_service(
     }
 }
 
+#[cfg(has_grabber)]
+mod grabber {
+    use libasync::delay;
+    use libboard_artiq::grabber;
+    use libboard_zynq::time::Milliseconds;
+    pub async fn grabber_thread(mut timer: GlobalTimer) {
+        let mut countdown = timer.countdown();
+        loop {
+            grabber::tick();
+            delay(&mut countdown, Milliseconds(200)).await;
+        }
+    }
+}
+
 static mut LOG_BUFFER: [u8; 1 << 17] = [0; 1 << 17];
 
 #[no_mangle]
@@ -114,6 +128,9 @@ pub fn main_core0() {
     #[cfg(has_drtio_eem)]
     drtio_eem::init(&mut timer, &cfg);
 
+    #[cfg(has_grabber)]
+    task::spawn(grabber::grabber_thread(timer));
+
     task::spawn(ksupport::report_async_rtio_errors());
 
     #[cfg(feature = "target_kasli_soc")]

src/satman/src/main.rs

@@ -23,6 +23,8 @@ extern crate alloc;
 use analyzer::Analyzer;
 use dma::Manager as DmaManager;
 use embedded_hal::blocking::delay::DelayUs;
+#[cfg(has_grabber)]
+use libboard_artiq::grabber;
 #[cfg(feature = "target_kasli_soc")]
 use libboard_artiq::io_expander;
 #[cfg(has_si5324)]
@@ -698,6 +700,8 @@ fn hardware_tick(ts: &mut u64, timer: &mut GlobalTimer) {
     if now > ts_ms {
         ts_ms = now + Milliseconds(200);
         *ts = ts_ms.0;
+        #[cfg(has_grabber)]
+        grabber::tick();
     }
 }