forked from M-Labs/nac3
1
0
Fork 0

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
This commit is contained in:
morgan 2024-04-09 16:32:56 +08:00
parent a1d80fb93b
commit 291777f764
5 changed files with 288 additions and 1 deletions

View File

@ -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" }

View File

@ -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 {}
});

View File

@ -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;

View File

@ -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");
}
}
}

View File

@ -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);
}
}