From 3ec537a5daa8ab88ff811f2f067db6d34afcdd7c Mon Sep 17 00:00:00 2001 From: morgan Date: Fri, 2 Feb 2024 17:00:30 +0800 Subject: [PATCH] WRPLL firmware satman main & si549: add WRPLL select_recovered_clock satman main & si549: add helper si549 setup 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: add handler for gtx & main tags FIQ --- src/libboard_artiq/src/si549.rs | 285 ++++++++++++++++++++++++++++++++ src/libksupport/src/fiq.rs | 21 +++ src/libksupport/src/lib.rs | 1 + src/satman/src/main.rs | 7 + 4 files changed, 314 insertions(+) create mode 100644 src/libksupport/src/fiq.rs diff --git a/src/libboard_artiq/src/si549.rs b/src/libboard_artiq/src/si549.rs index 49faddf..f08c8e0 100644 --- a/src/libboard_artiq/src/si549.rs +++ b/src/libboard_artiq/src/si549.rs @@ -261,3 +261,288 @@ pub fn main_setup(timer: &mut GlobalTimer) -> Result<(), &'static str> { info!("Main Si549 started"); Ok(()) } + +#[cfg(has_wrpll)] +pub mod wrpll { + + use libcortex_a9::mutex::Mutex; + + use super::*; + + const TIMER_WIDTH: u32 = 24; + const COUNTER_DIV: u32 = 2; + + const ADPLL_MAX: i32 = (950.0 / 0.0001164) as i32; + + 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 FIQ { + GTXTag, + MainTag, + } + + mod tag_collector { + use super::*; + + const BEATING_PERIOD: i32 = 0x8000; + const BEATING_HALFPERIOD: i32 = 0x4000; + + // for main PLL + static GTX_TAG: Mutex = Mutex::new(0); + static GTX_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(); + *GTX_TAG.lock() = 0; + *MAIN_TAG.lock() = 0; + } + + pub fn clear_phase_diff_ready() { + *GTX_TAG_READY.lock() = false; + *MAIN_TAG_READY.lock() = false; + } + + pub fn collect_tags(interrupt: FIQ) { + match interrupt { + FIQ::GTXTag => { + *GTX_TAG.lock() = unsafe { csr::wrpll::gtx_tag_read() }; + *GTX_TAG_READY.lock() = true; + } + FIQ::MainTag => { + *MAIN_TAG.lock() = unsafe { csr::wrpll::main_tag_read() }; + *MAIN_TAG_READY.lock() = true; + } + } + } + + pub fn phase_diff_ready() -> bool { + *GTX_TAG_READY.lock() && *MAIN_TAG_READY.lock() + } + + pub fn get_period_error() -> i32 { + // n * BEATING_PERIOD - GTX_TAG(n) mod BEATING_PERIOD + let mut period_error = (*GTX_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) - GTX_TAG(n) mod BEATING_PERIOD + let mut phase_error = (*MAIN_TAG.lock()) + .overflowing_sub(*GTX_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 + } + } + + pub fn helper_setup(timer: &mut GlobalTimer) -> Result<(), &'static str> { + unsafe { + csr::wrpll::helper_reset_write(1); + csr::helper_dcxo::bitbang_enable_write(1); + csr::helper_dcxo::i2c_address_write(ADDRESS); + } + + #[cfg(rtio_frequency = "125.0")] + let (h_hsdiv, h_lsdiv, h_fbdiv) = (0x058, 0, 0x04814E8F442); // 125Mhz*32767/32768 + + setup(i2c::DCXO::Helper, h_hsdiv, h_lsdiv, h_fbdiv, timer)?; + + // Si549 maximum settling time for large frequency change. + timer.delay_us(40_000); + + unsafe { + csr::wrpll::helper_reset_write(0); + csr::helper_dcxo::bitbang_enable_write(0); + } + info!("Helper Si549 started"); + Ok(()) + } + + fn set_fiq(en: bool) { + let val = if en { 1 } else { 0 }; + unsafe { + csr::wrpll::gtx_tag_ev_enable_write(val); + csr::wrpll::main_tag_ev_enable_write(val); + } + } + + /// set adpll using gateware i2c + /// Note: disable main/helper i2c bitbang before using this function + fn set_adpll(dcxo: i2c::DCXO, adpll: i32) -> Result<(), &'static str> { + if adpll.abs() > ADPLL_MAX { + return Err("adpll is too large"); + } + + match dcxo { + i2c::DCXO::Main => unsafe { + if csr::main_dcxo::bitbang_enable_read() == 1 { + return Err("Main si549 bitbang mode is active when using gateware i2c"); + } + + while csr::main_dcxo::adpll_busy_read() == 1 {} + csr::main_dcxo::i2c_address_write(ADDRESS); + csr::main_dcxo::adpll_write(adpll as u32); + + csr::main_dcxo::adpll_stb_write(1); + csr::main_dcxo::adpll_stb_write(0); + + if csr::main_dcxo::nack_read() == 1 { + return Err("Main si549 failed to ack adpll write"); + } + }, + #[cfg(has_wrpll)] + i2c::DCXO::Helper => unsafe { + if csr::helper_dcxo::bitbang_enable_read() == 1 { + return Err("Helper si549 bitbang mode is active when using gateware i2c"); + } + + while csr::helper_dcxo::adpll_busy_read() == 1 {} + csr::helper_dcxo::i2c_address_write(ADDRESS); + csr::helper_dcxo::adpll_write(adpll as u32); + + csr::helper_dcxo::adpll_stb_write(1); + csr::helper_dcxo::adpll_stb_write(0); + + if csr::helper_dcxo::nack_read() == 1 { + return Err("Helper si549 failed to ack adpll write"); + } + }, + }; + + Ok(()) + } + + /// 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 (gtx_count, main_count, _helper_count) = get_freq_counts(timer); + let mut base_adpll_lock = BASE_ADPLL.lock(); + *base_adpll_lock = count2adpll(gtx_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, 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); + let gtx = csr::wrpll::frequency_counter_counter_gtx0_rtio_rx_read(); + let main = csr::wrpll::frequency_counter_counter_sys_read(); + let helper = csr::wrpll::frequency_counter_counter_helper_read(); + + (gtx, main, helper) + } + } + + 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: FIQ) { + match interrupt { + FIQ::GTXTag => unsafe { csr::wrpll::gtx_tag_ev_pending_write(1) }, + FIQ::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_write(1) }, + }; + } + + fn is_pending(interrupt: FIQ) -> bool { + match interrupt { + FIQ::GTXTag => unsafe { csr::wrpll::gtx_tag_ev_pending_read() == 1 }, + FIQ::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_read() == 1 }, + } + } + + pub fn interrupt_handler() { + if is_pending(FIQ::GTXTag) { + tag_collector::collect_tags(FIQ::GTXTag); + clear_pending(FIQ::GTXTag); + + helper_pll().expect("failed to run helper DCXO PLL"); + } + + if is_pending(FIQ::MainTag) { + tag_collector::collect_tags(FIQ::MainTag); + clear_pending(FIQ::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_fiq(false); + + if rc { + tag_collector::reset(); + reset_plls().expect("failed to reset main and helper PLL"); + + info!("warming up GTX CDR..."); + // gtx 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(FIQ::GTXTag); + clear_pending(FIQ::MainTag); + + // use nFIQ to avoid IRQ being disabled by mutex lock and mess up PLL + set_fiq(true); + info!("WRPLL interrupt enabled"); + } + } +} diff --git a/src/libksupport/src/fiq.rs b/src/libksupport/src/fiq.rs new file mode 100644 index 0000000..5dc1277 --- /dev/null +++ b/src/libksupport/src/fiq.rs @@ -0,0 +1,21 @@ +#[cfg(has_wrpll)] +use libboard_artiq::si549; +use libboard_zynq::{println, stdio}; +use libcortex_a9::{interrupt_handler, regs::MPIDR}; +use libregister::RegisterR; + +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_wrpll)] + si549::wrpll::interrupt_handler(); + return; + } + _ => {} + }; + + stdio::drop_uart(); + println!("FIQ"); + loop {} +}); diff --git a/src/libksupport/src/lib.rs b/src/libksupport/src/lib.rs index 630f19e..29c4945 100644 --- a/src/libksupport/src/lib.rs +++ b/src/libksupport/src/lib.rs @@ -21,6 +21,7 @@ pub use pl::csr::rtio_core; use void::Void; pub mod eh_artiq; +pub mod fiq; pub mod i2c; pub mod irq; pub mod kernel; diff --git a/src/satman/src/main.rs b/src/satman/src/main.rs index 0f83d38..676bf4e 100644 --- a/src/satman/src/main.rs +++ b/src/satman/src/main.rs @@ -799,6 +799,8 @@ pub extern "C" fn main_core0() -> i32 { unsafe { csr::gt_drtio::txenable_write(0xffffffffu32 as _); } + #[cfg(has_wrpll)] + si549::wrpll::helper_setup(&mut timer).expect("cannot initialize helper Si549"); #[cfg(has_drtio_routing)] let mut repeaters = [repeater::Repeater::default(); csr::DRTIOREP.len()]; @@ -841,6 +843,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. @@ -898,6 +903,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); } }