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
2024-04-09 16:32:56 +08:00 · 2024-04-09 16:32:56 +08:00 · 291777f764
commit 291777f764
parent a1d80fb93b
5 changed files with 288 additions and 1 deletions
--- 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" }
--- a/src/libboard_artiq/src/fiq.rs
+++ 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 {}
+});
--- 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;
--- 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");
+        }
+    }
+}
--- 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);
    }
 }