From 9b76896eddf6794e0e1268d523e2d064d70fccee Mon Sep 17 00:00:00 2001 From: Egor Savkin Date: Thu, 12 Oct 2023 16:44:36 +0800 Subject: [PATCH] Add grabber module (cherry picked from commit b768d5648c9f3dce440953d776a9e21351de7e59) Signed-off-by: Egor Savkin --- src/gateware/kasli_soc.py | 12 ++- src/libboard_artiq/src/grabber.rs | 163 ++++++++++++++++++++++++++++++ src/libboard_artiq/src/lib.rs | 2 + src/runtime/src/main.rs | 18 ++++ src/satman/src/main.rs | 4 + 5 files changed, 195 insertions(+), 4 deletions(-) create mode 100644 src/libboard_artiq/src/grabber.rs diff --git a/src/gateware/kasli_soc.py b/src/gateware/kasli_soc.py index cd1239a..ddd3eef 100755 --- a/src/gateware/kasli_soc.py +++ b/src/gateware/kasli_soc.py @@ -77,7 +77,7 @@ class RTIOCRG(Module, AutoCSR): MultiReg(pll_locked, self.pll_locked.status) ] - + eem_iostandard_dict = { 0: "LVDS_25", 1: "LVDS_25", @@ -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) @@ -330,7 +333,7 @@ class GenericMaster(SoCCore): class GenericSatellite(SoCCore): def __init__(self, description, acpki=False): - sys_clk_freq = 125e6 + sys_clk_freq = 125e6 rtio_clk_freq = description["rtio_frequency"] self.acpki = acpki @@ -353,7 +356,7 @@ class GenericSatellite(SoCCore): self.submodules.rtio_crg = RTIOClockMultiplier(rtio_clk_freq) self.csr_devices.append("rtio_crg") fix_serdes_timing_path(platform) - + data_pads = [platform.request("sfp", i) for i in range(4)] self.submodules.drtio_transceiver = gtx_7series.GTX( @@ -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"), @@ -485,7 +489,7 @@ class GenericSatellite(SoCCore): self.config["HAS_GRABBER"] = None self.add_csr_group("grabber", self.grabber_csr_group) # no RTIO CRG here - + self.submodules.virtual_leds = virtual_leds.VirtualLeds() self.csr_devices.append("virtual_leds") diff --git a/src/libboard_artiq/src/grabber.rs b/src/libboard_artiq/src/grabber.rs new file mode 100644 index 0000000..1c406d5 --- /dev/null +++ b/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; + } + } +} diff --git a/src/libboard_artiq/src/lib.rs b/src/libboard_artiq/src/lib.rs index 8abbb67..b5ff893 100644 --- a/src/libboard_artiq/src/lib.rs +++ b/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; diff --git a/src/runtime/src/main.rs b/src/runtime/src/main.rs index 3c3fa92..4d07248 100644 --- a/src/runtime/src/main.rs +++ b/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), diff --git a/src/satman/src/main.rs b/src/satman/src/main.rs index 87b6ba9..ceae24d 100644 --- a/src/satman/src/main.rs +++ b/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(); } }