From 2f57ccf6172616187d6b9e80f83f10f4f7ec112c Mon Sep 17 00:00:00 2001 From: morgan Date: Thu, 4 Jan 2024 15:46:59 +0800 Subject: [PATCH] WRPLL firmware satman main & si549: add WRPLL select_recovered_clock satman main & si549: add helper si549 & interrupt 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 IRQ & si549: add handler for gtx & main tags irq IRQ: add docs for IRQ id --- src/libboard_artiq/src/si549.rs | 320 ++++++++++++++++++++++++++++++++ src/libksupport/src/irq.rs | 41 +++- src/satman/src/main.rs | 15 ++ 3 files changed, 367 insertions(+), 9 deletions(-) diff --git a/src/libboard_artiq/src/si549.rs b/src/libboard_artiq/src/si549.rs index fb79c16..fb88396 100644 --- a/src/libboard_artiq/src/si549.rs +++ b/src/libboard_artiq/src/si549.rs @@ -284,3 +284,323 @@ pub fn main_setup(timer: &mut GlobalTimer) -> Result<(), &'static str> { info!("Main Si549 started"); Ok(()) } + +#[cfg(has_wrpll)] +pub mod wrpll { + + use libboard_zynq::gic; + + use super::*; + + const IRQ_ID: [u8; 2] = [61, 62]; + + const TIMER_WIDTH: u32 = 24; + const COUNTER_DIV: u32 = 2; + + const ADPLL_MAX: i32 = (950.0 / 0.0001164) as i32; + + static mut BASE_ADPLL: i32 = 0; + static mut H_INTEGRATOR: i32 = 0; + static mut M_INTEGRATOR: i32 = 0; + + #[derive(Clone, Copy)] + pub enum IRQ { + GTXTag, + MainTag, + } + + mod tag_collector { + use super::IRQ; + use crate::pl::csr; + + const BEATING_PERIOD: i32 = 0x4000; + const BEATING_HALFPERIOD: i32 = 0x2000; + + // for helper PLL + static mut LAST_GTX_TAG: u32 = 0; + static mut FIRST_GTX_INTERRUPT: bool = true; + static mut PERIOD_DET_READY: bool = false; + + // for main PLL + static mut GTX_TAG: u32 = 0; + static mut GTX_TAG_READY: bool = false; + static mut MAIN_TAG: u32 = 0; + static mut MAIN_TAG_READY: bool = false; + + pub fn reset() { + clear_period_det_ready(); + clear_phase_det_ready(); + unsafe { + LAST_GTX_TAG = 0; + FIRST_GTX_INTERRUPT = true; + GTX_TAG = 0; + MAIN_TAG = 0; + } + } + + pub fn clear_period_det_ready() { + unsafe { + PERIOD_DET_READY = false; + } + } + + pub fn clear_phase_det_ready() { + unsafe { + GTX_TAG_READY = false; + MAIN_TAG_READY = false; + } + } + + pub fn collect_tags(interrupt: IRQ) { + match interrupt { + IRQ::GTXTag => unsafe { + if !FIRST_GTX_INTERRUPT { + LAST_GTX_TAG = GTX_TAG; + PERIOD_DET_READY = true; + } + + GTX_TAG = csr::wrpll::gtx_tag_read(); + FIRST_GTX_INTERRUPT = false; + GTX_TAG_READY = true; + }, + IRQ::MainTag => unsafe { + MAIN_TAG = csr::wrpll::main_tag_read(); + MAIN_TAG_READY = true; + }, + } + } + + pub fn period_det_ready() -> bool { + unsafe { PERIOD_DET_READY } + } + + pub fn phase_det_ready() -> bool { + unsafe { GTX_TAG_READY && MAIN_TAG_READY } + } + + pub fn get_period_error() -> i32 { + // when gtx tag roll over, overflowing_sub prevent the difference to overflow and still get the correct period + unsafe { BEATING_PERIOD - (GTX_TAG.overflowing_sub(LAST_GTX_TAG).0) as i32 } + } + + pub fn get_phase_error() -> i32 { + let mut phase_error: i32; + + unsafe { + // unwrap tag difference + phase_error = MAIN_TAG.overflowing_sub(GTX_TAG).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, 0x0481458C94D); // 125Mhz*16383/16384 + + 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(()) + } + + pub fn interrupt_setup(interrupt_controller: &mut gic::InterruptController) { + for id in IRQ_ID.iter() { + // setup shared peripheral interrupts (SPI) + interrupt_controller.enable( + gic::InterruptId(*id), + gic::CPUCore::Core0, + gic::InterruptSensitivity::Edge, + 0, + ); + } + } + + fn set_irq(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(()) + } + + 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); + unsafe { + BASE_ADPLL = count2adpll(gtx_count as i32 - main_count as i32); + set_adpll(i2c::DCXO::Main, BASE_ADPLL)?; + set_adpll(i2c::DCXO::Helper, BASE_ADPLL)?; + } + + 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> { + unsafe { + H_INTEGRATOR = 0; + M_INTEGRATOR = 0; + } + set_adpll(i2c::DCXO::Main, 0)?; + set_adpll(i2c::DCXO::Helper, 0)?; + Ok(()) + } + + fn clear_pending(interrupt: IRQ) { + match interrupt { + IRQ::GTXTag => unsafe { csr::wrpll::gtx_tag_ev_pending_write(1) }, + IRQ::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_write(1) }, + } + } + + pub fn interrupt_handler(interrupt: IRQ) { + tag_collector::collect_tags(interrupt); + + if tag_collector::period_det_ready() { + helper_pll().expect("failed to run helper DCXO PLL"); + tag_collector::clear_period_det_ready(); + } + + if tag_collector::phase_det_ready() { + main_pll().expect("failed to run main DCXO PLL"); + tag_collector::clear_phase_det_ready(); + } + + clear_pending(interrupt); + } + + pub fn helper_pll() -> Result<(), &'static str> { + let period_err = tag_collector::get_period_error(); + + const H_KP: i32 = 2; + const H_KI: f32 = 0.5; + + let mut h_adpll: i32; + unsafe { + H_INTEGRATOR += (period_err as f32 * H_KI) as i32; + h_adpll = BASE_ADPLL + period_err * H_KP + H_INTEGRATOR; + } + + h_adpll = h_adpll.clamp(-ADPLL_MAX, ADPLL_MAX); + set_adpll(i2c::DCXO::Helper, h_adpll)?; + + Ok(()) + } + + pub fn main_pll() -> Result<(), &'static str> { + let phase_err = tag_collector::get_phase_error(); + + const M_KP: i32 = 12; + const M_KI: i32 = 2; + + let mut m_adpll: i32; + unsafe { + M_INTEGRATOR += phase_err * M_KI; + m_adpll = BASE_ADPLL + phase_err * M_KP + M_INTEGRATOR; + } + + 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_irq(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(IRQ::GTXTag); + clear_pending(IRQ::MainTag); + set_irq(true); + info!("WRPLL interrupt enabled"); + } + } +} diff --git a/src/libksupport/src/irq.rs b/src/libksupport/src/irq.rs index a18feb9..4e9819f 100644 --- a/src/libksupport/src/irq.rs +++ b/src/libksupport/src/irq.rs @@ -1,5 +1,7 @@ use core::sync::atomic::{AtomicBool, Ordering}; +#[cfg(has_wrpll)] +use libboard_artiq::si549; use libboard_zynq::{gic, mpcore, println, stdio}; use libcortex_a9::{asm, interrupt_handler, notify_spin_lock, regs::MPIDR, spin_lock_yield}; use libregister::RegisterR; @@ -12,16 +14,37 @@ extern "C" { static CORE1_RESTART: AtomicBool = AtomicBool::new(false); interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, { - if MPIDR.read().cpu_id() == 1 { - let mpcore = mpcore::RegisterBlock::mpcore(); - let mut gic = gic::InterruptController::gic(mpcore); - let id = gic.get_interrupt_id(); - if id.0 == 0 { - gic.end_interrupt(id); - asm::exit_irq(); - asm!("b core1_restart"); + let mpcore = mpcore::RegisterBlock::mpcore(); + let mut gic = gic::InterruptController::gic(mpcore); + let id = gic.get_interrupt_id(); + // IRQ ID information at https://docs.xilinx.com/r/en-US/ug585-zynq-7000-SoC-TRM/Software-Generated-Interrupts-SGI?tocId=D777grsNOem_mnEV7glhfg + + match MPIDR.read().cpu_id() { + 0 => match id.0 { + 61 => { + #[cfg(has_wrpll)] + si549::wrpll::interrupt_handler(si549::wrpll::IRQ::GTXTag); + gic.end_interrupt(id); + return; + } + 62 => { + #[cfg(has_wrpll)] + si549::wrpll::interrupt_handler(si549::wrpll::IRQ::MainTag); + gic.end_interrupt(id); + return; + } + _ => {} + }, + 1 => { + if id.0 == 0 { + gic.end_interrupt(id); + asm::exit_irq(); + asm!("b core1_restart"); + } } - } + _ => {} + }; + stdio::drop_uart(); println!("IRQ"); loop {} diff --git a/src/satman/src/main.rs b/src/satman/src/main.rs index 0f83d38..15d1eb7 100644 --- a/src/satman/src/main.rs +++ b/src/satman/src/main.rs @@ -37,6 +37,8 @@ use libboard_artiq::{drtio_routing, drtioaux, pl::csr}; #[cfg(feature = "target_kasli_soc")] use libboard_zynq::error_led::ErrorLED; +#[cfg(has_wrpll)] +use libboard_zynq::{gic::InterruptController, mpcore::RegisterBlock}; use libboard_zynq::{i2c::I2c, print, println, time::Milliseconds, timer::GlobalTimer}; use libcortex_a9::{l2c::enable_l2_cache, regs::MPIDR}; use libregister::RegisterR; @@ -739,6 +741,9 @@ pub extern "C" fn main_core0() -> i32 { let mut timer = GlobalTimer::start(); + #[cfg(has_wrpll)] + let mut interrupt_controller = InterruptController::gic(RegisterBlock::mpcore()); + let buffer_logger = unsafe { logger::BufferLogger::new(&mut LOG_BUFFER[..]) }; buffer_logger.set_uart_log_level(log::LevelFilter::Info); buffer_logger.register(); @@ -799,6 +804,11 @@ 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"); + si549::wrpll::interrupt_setup(&mut interrupt_controller); + } #[cfg(has_drtio_routing)] let mut repeaters = [repeater::Repeater::default(); csr::DRTIOREP.len()]; @@ -841,6 +851,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 +911,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); } }