From b768d5648c9f3dce440953d776a9e21351de7e59 Mon Sep 17 00:00:00 2001 From: Egor Savkin Date: Thu, 12 Oct 2023 16:44:36 +0800 Subject: [PATCH] Add grabber module Signed-off-by: Egor Savkin --- src/gateware/kasli_soc.py | 6 +- src/libboard_artiq/src/grabber.rs | 163 ++++++++++++++++++++++++++++++ src/libboard_artiq/src/lib.rs | 2 + src/runtime/src/main.rs | 17 ++++ src/satman/src/main.rs | 4 + 5 files changed, 191 insertions(+), 1 deletion(-) create mode 100644 src/libboard_artiq/src/grabber.rs diff --git a/src/gateware/kasli_soc.py b/src/gateware/kasli_soc.py index 6541ccbc..4bcb8ed5 100755 --- a/src/gateware/kasli_soc.py +++ b/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"), diff --git a/src/libboard_artiq/src/grabber.rs b/src/libboard_artiq/src/grabber.rs new file mode 100644 index 00000000..1c406d54 --- /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 c9f80d15..bee0d74d 100644 --- a/src/libboard_artiq/src/lib.rs +++ b/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; diff --git a/src/runtime/src/main.rs b/src/runtime/src/main.rs index 665e8e96..ec5a84c5 100644 --- a/src/runtime/src/main.rs +++ b/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")] diff --git a/src/satman/src/main.rs b/src/satman/src/main.rs index 99683bfe..b501cbaa 100644 --- a/src/satman/src/main.rs +++ b/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(); } }