Add grabber module

(cherry picked from commit b768d5648c)
Signed-off-by: Egor Savkin <es@m-labs.hk>

src/gateware/kasli_soc.py

@@ -114,6 +114,7 @@ class SMAClkinForward(Module):
 class GenericStandalone(SoCCore):
     def __init__(self, description, acpki=False):
         self.acpki = acpki
+        sys_clk_freq = 125e6
 
         platform = kasli_soc.Platform()
         platform.toolchain.bitstream_commands.extend([
@@ -142,6 +143,7 @@ class GenericStandalone(SoCCore):
             self.ps7.cd_sys.clk,
             self.rtio_crg.cd_rtio.clk)
         fix_serdes_timing_path(platform)
+        self.config["CLOCK_FREQUENCY"] = int(sys_clk_freq)
 
         self.rtio_channels = []
         has_grabber = any(peripheral["type"] == "grabber" for peripheral in description["peripherals"])
@@ -226,6 +228,7 @@ class GenericMaster(SoCCore):
             pads=data_pads,
             sys_clk_freq=sys_clk_freq)
         self.csr_devices.append("drtio_transceiver")
+        self.config["CLOCK_FREQUENCY"] = int(sys_clk_freq)
 
         self.crg = self.ps7 # HACK for eem_7series to find the clock
         self.submodules.rtio_crg = RTIOClockMultiplier(rtio_clk_freq)
@@ -458,6 +461,7 @@ class GenericSatellite(SoCCore):
 
         rtio_clk_period = 1e9/rtio_clk_freq
         self.config["RTIO_FREQUENCY"] = str(rtio_clk_freq/1e6)
+        self.config["CLOCK_FREQUENCY"] = int(sys_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

@@ -29,6 +29,8 @@ pub mod drtioaux_async;
 pub mod mem;
 #[cfg(feature = "target_kasli_soc")]
 pub mod io_expander;
+#[cfg(has_grabber)]
+pub mod grabber;
 
 use core::{cmp, str};
 use libboard_zynq::slcr;

src/runtime/src/main.rs

@@ -110,6 +110,21 @@ async fn io_expanders_service(
             .expect("I2C I/O expander #1 service failed");
     }
 }
+#[cfg(has_grabber)]
+mod grabber {
+    use libasync::delay;
+    use libboard_artiq::grabber;
+    use libboard_zynq::time::Milliseconds;
+    use crate::GlobalTimer;
+    pub async fn grabber_thread(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]
@@ -168,6 +183,9 @@ pub fn main_core0() {
 
     task::spawn(report_async_rtio_errors());
 
+    #[cfg(has_grabber)]
+    task::spawn(grabber::grabber_thread(timer));
+
     #[cfg(feature = "target_kasli_soc")]
     task::spawn(io_expanders_service(
         RefCell::new(i2c_bus),

src/satman/src/main.rs

@@ -20,6 +20,8 @@ extern crate alloc;
 
 use libboard_zynq::{i2c::I2c, timer::GlobalTimer, time::Milliseconds, print, println, mpcore, gic, stdio};
 use libsupport_zynq::ram;
+#[cfg(has_grabber)]
+use libboard_artiq::grabber;
 #[cfg(has_si5324)]
 use libboard_artiq::si5324;
 #[cfg(feature = "target_kasli_soc")]
@@ -417,6 +419,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();
     }
 }