Firmware: Satman WRPLL
satman: drive CLK_SEL to true when si549 is used satman & si549: add main & helper si549 setup satman & si549: add WRPLL select_recovered_clock si549: add tag collector to process gtx & main tags si549: add tag offset to meet setup/hold constraints for CDR 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
parent
917579f005
commit
82e9fbc6d2
|
|
@ -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" }
|
||||
|
|
|
|||
|
|
@ -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 {}
|
||||
});
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -328,3 +328,217 @@ fn set_adpll(dcxo: i2c::DCXO, adpll: i32) -> Result<(), &'static str> {
|
|||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
#[cfg(has_wrpll)]
|
||||
pub mod wrpll {
|
||||
|
||||
use libcortex_a9::mutex::Mutex;
|
||||
|
||||
use super::*;
|
||||
|
||||
const BEATING_PERIOD: i32 = 0x8000;
|
||||
const BEATING_HALFPERIOD: i32 = 0x4000;
|
||||
const TIMER_WIDTH: u32 = 24;
|
||||
const COUNTER_DIV: u32 = 2;
|
||||
|
||||
const KP: i32 = 6;
|
||||
const KI: i32 = 2;
|
||||
|
||||
static BASE_ADPLL: Mutex<i32> = Mutex::new(0);
|
||||
static H_INTEGRATOR: Mutex<i32> = Mutex::new(0);
|
||||
static M_INTEGRATOR: Mutex<i32> = Mutex::new(0);
|
||||
|
||||
#[derive(Clone, Copy)]
|
||||
pub enum ISR {
|
||||
RefTag,
|
||||
MainTag,
|
||||
}
|
||||
|
||||
mod tag_collector {
|
||||
use super::*;
|
||||
|
||||
static REF_TAG: Mutex<u32> = Mutex::new(0);
|
||||
static REF_TAG_READY: Mutex<bool> = Mutex::new(false);
|
||||
static MAIN_TAG: Mutex<u32> = Mutex::new(0);
|
||||
static MAIN_TAG_READY: Mutex<bool> = Mutex::new(false);
|
||||
|
||||
pub fn reset() {
|
||||
clear_phase_diff_ready();
|
||||
*REF_TAG.lock() = 0;
|
||||
*MAIN_TAG.lock() = 0;
|
||||
}
|
||||
|
||||
pub fn clear_phase_diff_ready() {
|
||||
*REF_TAG_READY.lock() = false;
|
||||
*MAIN_TAG_READY.lock() = false;
|
||||
}
|
||||
|
||||
pub fn collect_tags(interrupt: ISR) {
|
||||
match interrupt {
|
||||
ISR::RefTag => {
|
||||
*REF_TAG.lock() = unsafe { csr::wrpll::ref_tag_read() };
|
||||
*REF_TAG_READY.lock() = true;
|
||||
}
|
||||
ISR::MainTag => {
|
||||
*MAIN_TAG.lock() = unsafe { csr::wrpll::main_tag_read() };
|
||||
*MAIN_TAG_READY.lock() = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn phase_diff_ready() -> bool {
|
||||
*REF_TAG_READY.lock() && *MAIN_TAG_READY.lock()
|
||||
}
|
||||
|
||||
pub fn get_period_error() -> i32 {
|
||||
// n * BEATING_PERIOD - REF_TAG(n) mod BEATING_PERIOD
|
||||
let mut period_error = (*REF_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) - REF_TAG(n) mod BEATING_PERIOD
|
||||
let mut phase_error = (*MAIN_TAG.lock())
|
||||
.overflowing_sub(*REF_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
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
/// 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 (ref_count, main_count) = get_freq_counts(timer);
|
||||
let mut base_adpll_lock = BASE_ADPLL.lock();
|
||||
*base_adpll_lock = count2adpll(ref_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);
|
||||
#[cfg(wrpll_ref_clk = "GTX_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() -> 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: 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();
|
||||
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_isr(false);
|
||||
|
||||
if rc {
|
||||
tag_collector::reset();
|
||||
reset_plls().expect("failed to reset main and helper PLL");
|
||||
|
||||
info!("warming up refclk...");
|
||||
// refclk 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(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");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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"))]
|
||||
pub 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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue