diff --git a/src/libboard_artiq/src/si549.rs b/src/libboard_artiq/src/si549.rs index cf263d0..974fcb6 100644 --- a/src/libboard_artiq/src/si549.rs +++ b/src/libboard_artiq/src/si549.rs @@ -270,3 +270,282 @@ pub fn main_setup(timer: &mut GlobalTimer, settings: FrequencySetting) -> Result 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; + + 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, settings: FrequencySetting) -> Result<(), &'static str> { + unsafe { + csr::wrpll::helper_reset_write(1); + csr::helper_dcxo::bitbang_enable_write(1); + csr::helper_dcxo::i2c_address_write(ADDRESS); + } + + setup(i2c::DCXO::Helper, settings.helper, 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"); + } + }, + 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) = 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) { + 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(); + + (gtx, main) + } + } + + 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 07d0ec5..915eb05 100644 --- a/src/satman/src/main.rs +++ b/src/satman/src/main.rs @@ -830,6 +830,38 @@ 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, + }, + #[cfg(has_wrpll)] + 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, + }, + #[cfg(has_wrpll)] + helper: si549::DividerConfig { + // 100Mhz*32767/32768 + hsdiv: 0x06C, + lsdiv: 0, + fbdiv: 0x046C5670BBD, + }, +}; + static mut LOG_BUFFER: [u8; 1 << 17] = [0; 1 << 17]; #[no_mangle] @@ -883,7 +915,7 @@ 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).expect("cannot initialize main Si549"); + si549::main_setup(&mut timer, SI549_SETTINGS).expect("cannot initialize main Si549"); timer.delay_us(100_000); info!("Switching SYS clocks..."); @@ -901,6 +933,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, SI549_SETTINGS).expect("cannot initialize helper Si549"); #[cfg(has_drtio_routing)] let mut repeaters = [repeater::Repeater::default(); csr::DRTIOREP.len()]; @@ -946,6 +980,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. @@ -1043,6 +1080,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); } }