From 291777f764539385a15a0cb258b8e34b642288a2 Mon Sep 17 00:00:00 2001 From: morgan Date: Tue, 9 Apr 2024 16:32:56 +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 | 216 ++++++++++++++++++++++++++++++ src/satman/src/main.rs | 46 +++++++ 5 files changed, 288 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 5677984c..9c1ad24e 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 00000000..dc9c0133 --- /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 4ee5a3c6..e9b94603 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 ca11cccd..1723cd33 100644 --- a/src/libboard_artiq/src/si549.rs +++ b/src/libboard_artiq/src/si549.rs @@ -324,3 +324,219 @@ fn set_adpll(dcxo: i2c::DCXO, adpll: i32) -> Result<(), &'static str> { Ok(()) } + +#[cfg(has_wrpll)] +pub mod wrpll { + + use super::*; + + const BEATING_PERIOD: i32 = 0x8000; + const BEATING_HALFPERIOD: i32 = 0x4000; + const COUNTER_WIDTH: u32 = 24; + const DIV_WIDTH: u32 = 2; + + const KP: i32 = 6; + const KI: i32 = 2; + // 4 ppm capture range + const ADPLL_LIM: i32 = (4.0 / 0.0001164) as i32; + + static mut BASE_ADPLL: i32 = 0; + static mut H_LAST_ADPLL: i32 = 0; + static mut LAST_PERIOD_ERR: i32 = 0; + static mut M_LAST_ADPLL: i32 = 0; + static mut LAST_PHASE_ERR: i32 = 0; + + #[derive(Clone, Copy)] + pub enum ISR { + RefTag, + MainTag, + } + + mod tag_collector { + use super::*; + + static mut REF_TAG: u32 = 0; + static mut REF_TAG_READY: bool = false; + static mut MAIN_TAG: u32 = 0; + static mut MAIN_TAG_READY: bool = false; + + pub fn reset() { + clear_phase_diff_ready(); + unsafe { + REF_TAG = 0; + MAIN_TAG = 0; + } + } + + pub fn clear_phase_diff_ready() { + unsafe { + REF_TAG_READY = false; + MAIN_TAG_READY = false; + } + } + + pub fn collect_tags(interrupt: ISR) { + match interrupt { + ISR::RefTag => unsafe { + REF_TAG = csr::wrpll::ref_tag_read(); + REF_TAG_READY = true; + }, + ISR::MainTag => unsafe { + MAIN_TAG = csr::wrpll::main_tag_read(); + MAIN_TAG_READY = true; + }, + } + } + + pub fn phase_diff_ready() -> bool { + unsafe { REF_TAG_READY && MAIN_TAG_READY } + } + + pub fn get_period_error() -> i32 { + // n * BEATING_PERIOD - REF_TAG(n) mod BEATING_PERIOD + let mut period_error = unsafe { REF_TAG.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 = + unsafe { MAIN_TAG.overflowing_sub(REF_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 + } + } + + 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); + } + } + + fn set_base_adpll() -> Result<(), &'static str> { + let count2adpll = + |error: i32| ((error as f64 * 1e6) / (0.0001164 * (1 << (COUNTER_WIDTH - DIV_WIDTH)) as f64)) as i32; + + let (ref_count, main_count) = get_freq_counts(); + unsafe { + BASE_ADPLL = count2adpll(ref_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() -> (u32, u32) { + unsafe { + csr::wrpll::frequency_counter_update_write(1); + while csr::wrpll::frequency_counter_busy_read() == 1 {} + #[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(timer: &mut GlobalTimer) -> Result<(), &'static str> { + unsafe { + H_LAST_ADPLL = 0; + LAST_PERIOD_ERR = 0; + M_LAST_ADPLL = 0; + LAST_PHASE_ERR = 0; + } + set_adpll(i2c::DCXO::Main, 0)?; + set_adpll(i2c::DCXO::Helper, 0)?; + // wait for adpll to transfer and DCXO to settle + timer.delay_us(200); + 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(); + unsafe { + // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw?view#Integral-wind-up-and-output-limiting + let adpll = (H_LAST_ADPLL + (KP + KI) * period_err - KP * LAST_PERIOD_ERR).clamp(-ADPLL_LIM, ADPLL_LIM); + set_adpll(i2c::DCXO::Helper, BASE_ADPLL + adpll)?; + H_LAST_ADPLL = adpll; + LAST_PERIOD_ERR = period_err; + }; + Ok(()) + } + + fn main_pll() -> Result<(), &'static str> { + let phase_err = tag_collector::get_phase_error(); + unsafe { + // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw?view#Integral-wind-up-and-output-limiting + let adpll = (M_LAST_ADPLL + (KP + KI) * phase_err - KP * LAST_PHASE_ERR).clamp(-ADPLL_LIM, ADPLL_LIM); + set_adpll(i2c::DCXO::Main, BASE_ADPLL + adpll)?; + M_LAST_ADPLL = adpll; + LAST_PHASE_ERR = phase_err; + }; + Ok(()) + } + + pub fn select_recovered_clock(rc: bool, timer: &mut GlobalTimer) { + set_isr(false); + + if rc { + tag_collector::reset(); + reset_plls(timer).expect("failed to reset main and helper PLL"); + + // get within capture range + set_base_adpll().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 1ddea7bf..2a588b9c 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"))] +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); } }