From fbf973efd02f9017690f108477e5577917df2ba4 Mon Sep 17 00:00:00 2001 From: morgan Date: Mon, 26 Feb 2024 16:30:21 +0800 Subject: [PATCH] Firmware: Satman WRPLL satman: drive CLK_SEL to true when si549 is used satman : add main & helper si549 setup satman : add WRPLL select_recovered_clock si549: add tag collector to process gtx & main tags si549: add frequency counter to set BASE_ADPLL si549: add set_adpll for main & helper PLL si549: add main & helper PLL FIQ & si549: replace dummy with a custom handler for gtx & main tags ISR --- src/libboard_artiq/Cargo.toml.tpl | 2 +- src/libboard_artiq/src/fiq.rs | 22 +++ src/libboard_artiq/src/lib.rs | 3 + src/libboard_artiq/src/si549.rs | 214 ++++++++++++++++++++++++++++++ src/satman/src/main.rs | 46 +++++++ 5 files changed, 286 insertions(+), 1 deletion(-) create mode 100644 src/libboard_artiq/src/fiq.rs diff --git a/src/libboard_artiq/Cargo.toml.tpl b/src/libboard_artiq/Cargo.toml.tpl index 5677984..9c1ad24 100644 --- a/src/libboard_artiq/Cargo.toml.tpl +++ b/src/libboard_artiq/Cargo.toml.tpl @@ -25,7 +25,7 @@ void = { version = "1", default-features = false } io = { path = "../libio", features = ["byteorder"] } libboard_zynq = { path = "@@ZYNQ_RS@@/libboard_zynq" } -libsupport_zynq = { path = "@@ZYNQ_RS@@/libsupport_zynq", default-features = false, features = ["alloc_core", "dummy_fiq_handler"] } +libsupport_zynq = { path = "@@ZYNQ_RS@@/libsupport_zynq", default-features = false, features = ["alloc_core"] } libregister = { path = "@@ZYNQ_RS@@/libregister" } libconfig = { path = "@@ZYNQ_RS@@/libconfig", features = ["fat_lfn"] } libcortex_a9 = { path = "@@ZYNQ_RS@@/libcortex_a9" } diff --git a/src/libboard_artiq/src/fiq.rs b/src/libboard_artiq/src/fiq.rs new file mode 100644 index 0000000..dc9c013 --- /dev/null +++ b/src/libboard_artiq/src/fiq.rs @@ -0,0 +1,22 @@ +use libboard_zynq::{println, stdio}; +use libcortex_a9::{interrupt_handler, regs::MPIDR}; +use libregister::RegisterR; + +#[cfg(has_si549)] +use crate::si549; + +interrupt_handler!(FIQ, fiq, __irq_stack0_start, __irq_stack1_start, { + match MPIDR.read().cpu_id() { + 0 => { + // nFIQ is driven directly and bypass GIC + #[cfg(has_si549)] + si549::wrpll::interrupt_handler(); + return; + } + _ => {} + }; + + stdio::drop_uart(); + println!("FIQ"); + loop {} +}); diff --git a/src/libboard_artiq/src/lib.rs b/src/libboard_artiq/src/lib.rs index 4ee5a3c..e9b9460 100644 --- a/src/libboard_artiq/src/lib.rs +++ b/src/libboard_artiq/src/lib.rs @@ -1,5 +1,7 @@ #![no_std] #![feature(never_type)] +#![feature(naked_functions)] +#![feature(asm)] extern crate core_io; extern crate crc; @@ -19,6 +21,7 @@ pub mod drtioaux; #[cfg(has_drtio)] pub mod drtioaux_async; pub mod drtioaux_proto; +pub mod fiq; #[cfg(all(feature = "target_kasli_soc", has_drtio))] pub mod io_expander; pub mod logger; diff --git a/src/libboard_artiq/src/si549.rs b/src/libboard_artiq/src/si549.rs index e10bbe3..0c98045 100644 --- a/src/libboard_artiq/src/si549.rs +++ b/src/libboard_artiq/src/si549.rs @@ -328,3 +328,217 @@ fn set_adpll(dcxo: i2c::DCXO, adpll: i32) -> Result<(), &'static str> { Ok(()) } + +#[cfg(has_wrpll)] +pub mod wrpll { + + use libcortex_a9::mutex::Mutex; + + use super::*; + + const BEATING_PERIOD: i32 = 0x8000; + const BEATING_HALFPERIOD: i32 = 0x4000; + const TIMER_WIDTH: u32 = 24; + const COUNTER_DIV: u32 = 2; + + const KP: i32 = 6; + const KI: i32 = 2; + + static BASE_ADPLL: Mutex = Mutex::new(0); + static H_INTEGRATOR: Mutex = Mutex::new(0); + static M_INTEGRATOR: Mutex = Mutex::new(0); + + #[derive(Clone, Copy)] + pub enum ISR { + RefTag, + MainTag, + } + + mod tag_collector { + use super::*; + + static REF_TAG: Mutex = Mutex::new(0); + static REF_TAG_READY: Mutex = Mutex::new(false); + static MAIN_TAG: Mutex = Mutex::new(0); + static MAIN_TAG_READY: Mutex = Mutex::new(false); + + pub fn reset() { + clear_phase_diff_ready(); + *REF_TAG.lock() = 0; + *MAIN_TAG.lock() = 0; + } + + pub fn clear_phase_diff_ready() { + *REF_TAG_READY.lock() = false; + *MAIN_TAG_READY.lock() = false; + } + + pub fn collect_tags(interrupt: ISR) { + match interrupt { + ISR::RefTag => { + *REF_TAG.lock() = unsafe { csr::wrpll::ref_tag_read() }; + *REF_TAG_READY.lock() = true; + } + ISR::MainTag => { + *MAIN_TAG.lock() = unsafe { csr::wrpll::main_tag_read() }; + *MAIN_TAG_READY.lock() = true; + } + } + } + + pub fn phase_diff_ready() -> bool { + *REF_TAG_READY.lock() && *MAIN_TAG_READY.lock() + } + + pub fn get_period_error() -> i32 { + // n * BEATING_PERIOD - REF_TAG(n) mod BEATING_PERIOD + let mut period_error = (*REF_TAG.lock()).overflowing_neg().0.rem_euclid(BEATING_PERIOD as u32) as i32; + + // mapping tags from [0, 2π] -> [-π, π] + if period_error > BEATING_HALFPERIOD { + period_error -= BEATING_PERIOD + } + period_error + } + + pub fn get_phase_error() -> i32 { + // MAIN_TAG(n) - REF_TAG(n) mod BEATING_PERIOD + let mut phase_error = (*MAIN_TAG.lock()) + .overflowing_sub(*REF_TAG.lock()) + .0 + .rem_euclid(BEATING_PERIOD as u32) as i32; + + // mapping tags from [0, 2π] -> [-π, π] + if phase_error > BEATING_HALFPERIOD { + phase_error -= BEATING_PERIOD + } + phase_error + } + } + + fn set_isr(en: bool) { + let val = if en { 1 } else { 0 }; + unsafe { + csr::wrpll::ref_tag_ev_enable_write(val); + csr::wrpll::main_tag_ev_enable_write(val); + } + } + + /// To get within capture range + fn set_base_adpll(timer: &mut GlobalTimer) -> Result<(), &'static str> { + let count2adpll = + |error: i32| (((error) as f64 * 1e6) / (0.0001164 * (1 << (TIMER_WIDTH - COUNTER_DIV)) as f64)) as i32; + + let (ref_count, main_count) = get_freq_counts(timer); + let mut base_adpll_lock = BASE_ADPLL.lock(); + *base_adpll_lock = count2adpll(ref_count as i32 - main_count as i32); + set_adpll(i2c::DCXO::Main, *base_adpll_lock)?; + set_adpll(i2c::DCXO::Helper, *base_adpll_lock)?; + + Ok(()) + } + + fn get_freq_counts(timer: &mut GlobalTimer) -> (u32, u32) { + unsafe { + csr::wrpll::frequency_counter_update_en_write(1); + timer.delay_us(150_000); // 8ns << TIMER_WIDTH + csr::wrpll::frequency_counter_update_en_write(0); + #[cfg(wrpll_ref_clk = "GT_CDR")] + let ref_count = csr::wrpll::frequency_counter_counter_rtio_rx0_read(); + #[cfg(wrpll_ref_clk = "SMA_CLKIN")] + let ref_count = csr::wrpll::frequency_counter_counter_ref_read(); + let main_count = csr::wrpll::frequency_counter_counter_sys_read(); + + (ref_count, main_count) + } + } + + fn reset_plls() -> Result<(), &'static str> { + *H_INTEGRATOR.lock() = 0; + *M_INTEGRATOR.lock() = 0; + set_adpll(i2c::DCXO::Main, 0)?; + set_adpll(i2c::DCXO::Helper, 0)?; + Ok(()) + } + + fn clear_pending(interrupt: ISR) { + match interrupt { + ISR::RefTag => unsafe { csr::wrpll::ref_tag_ev_pending_write(1) }, + ISR::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_write(1) }, + }; + } + + fn is_pending(interrupt: ISR) -> bool { + match interrupt { + ISR::RefTag => unsafe { csr::wrpll::ref_tag_ev_pending_read() == 1 }, + ISR::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_read() == 1 }, + } + } + + pub fn interrupt_handler() { + if is_pending(ISR::RefTag) { + tag_collector::collect_tags(ISR::RefTag); + clear_pending(ISR::RefTag); + + helper_pll().expect("failed to run helper DCXO PLL"); + } + + if is_pending(ISR::MainTag) { + tag_collector::collect_tags(ISR::MainTag); + clear_pending(ISR::MainTag); + } + + if tag_collector::phase_diff_ready() { + main_pll().expect("failed to run main DCXO PLL"); + tag_collector::clear_phase_diff_ready(); + } + } + + fn helper_pll() -> Result<(), &'static str> { + let period_err = tag_collector::get_period_error(); + let mut integrator_lock = H_INTEGRATOR.lock(); + + *integrator_lock += period_err * KI; + let mut h_adpll = *BASE_ADPLL.lock() + period_err * KP + *integrator_lock; + + h_adpll = h_adpll.clamp(-ADPLL_MAX, ADPLL_MAX); + set_adpll(i2c::DCXO::Helper, h_adpll)?; + + Ok(()) + } + + fn main_pll() -> Result<(), &'static str> { + let phase_err = tag_collector::get_phase_error(); + let mut integrator_lock = M_INTEGRATOR.lock(); + + *integrator_lock += phase_err * KI; + let mut m_adpll = *BASE_ADPLL.lock() + phase_err * KP + *integrator_lock; + + m_adpll = m_adpll.clamp(-ADPLL_MAX, ADPLL_MAX); + set_adpll(i2c::DCXO::Main, m_adpll)?; + + Ok(()) + } + + pub fn select_recovered_clock(rc: bool, timer: &mut GlobalTimer) { + set_isr(false); + + if rc { + tag_collector::reset(); + reset_plls().expect("failed to reset main and helper PLL"); + + info!("warming up refclk..."); + // refclk need a couple seconds for freq counter to read it properly + timer.delay_us(20_000_000); + set_base_adpll(timer).expect("failed to set base adpll"); + + // clear gateware pending flag + clear_pending(ISR::RefTag); + clear_pending(ISR::MainTag); + + // use nFIQ to avoid IRQ being disabled by mutex lock and mess up PLL + set_isr(true); + info!("WRPLL interrupt enabled"); + } + } +} diff --git a/src/satman/src/main.rs b/src/satman/src/main.rs index 1ddea7b..35be2cb 100644 --- a/src/satman/src/main.rs +++ b/src/satman/src/main.rs @@ -29,6 +29,8 @@ use libboard_artiq::grabber; use libboard_artiq::io_expander; #[cfg(has_si5324)] use libboard_artiq::si5324; +#[cfg(has_si549)] +use libboard_artiq::si549; use libboard_artiq::{drtio_routing, drtioaux, drtioaux_proto::{MASTER_PAYLOAD_MAX_SIZE, SAT_PAYLOAD_MAX_SIZE}, identifier_read, logger, @@ -828,6 +830,36 @@ const SI5324_SETTINGS: si5324::FrequencySettings = si5324::FrequencySettings { crystal_as_ckin2: true, }; +#[cfg(all(has_si549, rtio_frequency = "125.0"))] +const SI549_SETTINGS: si549::FrequencySetting = si549::FrequencySetting { + main: si549::DividerConfig { + hsdiv: 0x058, + lsdiv: 0, + fbdiv: 0x04815791F25, + }, + helper: si549::DividerConfig { + // 125Mhz*32767/32768 + hsdiv: 0x058, + lsdiv: 0, + fbdiv: 0x04814E8F442, + }, +}; + +#[cfg(all(has_si549, rtio_frequency = "100.0"))] +pub const SI549_SETTINGS: si549::FrequencySetting = si549::FrequencySetting { + main: si549::DividerConfig { + hsdiv: 0x06C, + lsdiv: 0, + fbdiv: 0x046C5F49797, + }, + helper: si549::DividerConfig { + // 100Mhz*32767/32768 + hsdiv: 0x06C, + lsdiv: 0, + fbdiv: 0x046C5670BBD, + }, +}; + static mut LOG_BUFFER: [u8; 1 << 17] = [0; 1 << 17]; #[no_mangle] @@ -864,6 +896,11 @@ pub extern "C" fn main_core0() -> i32 { io_expander1 .init(&mut i2c) .expect("I2C I/O expander #1 initialization failed"); + + // Drive CLK_SEL to true + #[cfg(has_si549)] + io_expander0.set(1, 7, true); + // Drive TX_DISABLE to false on SFP0..3 io_expander0.set(0, 1, false); io_expander1.set(0, 1, false); @@ -875,6 +912,8 @@ pub extern "C" fn main_core0() -> i32 { #[cfg(has_si5324)] si5324::setup(&mut i2c, &SI5324_SETTINGS, si5324::Input::Ckin1, &mut timer).expect("cannot initialize Si5324"); + #[cfg(has_si549)] + si549::main_setup(&mut timer, &SI549_SETTINGS).expect("cannot initialize main Si549"); timer.delay_us(100_000); info!("Switching SYS clocks..."); @@ -892,6 +931,8 @@ pub extern "C" fn main_core0() -> i32 { unsafe { csr::gt_drtio::txenable_write(0xffffffffu32 as _); } + #[cfg(has_si549)] + si549::helper_setup(&mut timer, &SI549_SETTINGS).expect("cannot initialize helper Si549"); #[cfg(has_drtio_routing)] let mut repeaters = [repeater::Repeater::default(); csr::DRTIOREP.len()]; @@ -937,6 +978,9 @@ pub extern "C" fn main_core0() -> i32 { si5324::siphaser::calibrate_skew(&mut timer).expect("failed to calibrate skew"); } + #[cfg(has_wrpll)] + si549::wrpll::select_recovered_clock(true, &mut timer); + // Various managers created here, so when link is dropped, all DMA traces // are cleared out for a clean slate on subsequent connections, // without a manual intervention. @@ -1034,6 +1078,8 @@ pub extern "C" fn main_core0() -> i32 { info!("uplink is down, switching to local oscillator clock"); #[cfg(has_siphaser)] si5324::siphaser::select_recovered_clock(&mut i2c, false, &mut timer).expect("failed to switch clocks"); + #[cfg(has_wrpll)] + si549::wrpll::select_recovered_clock(false, &mut timer); } }