WRPLL firmware
satman main & si549: add WRPLL select_recovered_clock satman main & si549: add helper si549 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 FIQ & si549: add handler for gtx & main tags FIQ
This commit is contained in:
parent
142e01a98c
commit
3ec537a5da
|
@ -261,3 +261,288 @@ pub fn main_setup(timer: &mut GlobalTimer) -> Result<(), &'static str> {
|
||||||
info!("Main Si549 started");
|
info!("Main Si549 started");
|
||||||
Ok(())
|
Ok(())
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#[cfg(has_wrpll)]
|
||||||
|
pub mod wrpll {
|
||||||
|
|
||||||
|
use libcortex_a9::mutex::Mutex;
|
||||||
|
|
||||||
|
use super::*;
|
||||||
|
|
||||||
|
const TIMER_WIDTH: u32 = 24;
|
||||||
|
const COUNTER_DIV: u32 = 2;
|
||||||
|
|
||||||
|
const ADPLL_MAX: i32 = (950.0 / 0.0001164) as i32;
|
||||||
|
|
||||||
|
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 FIQ {
|
||||||
|
GTXTag,
|
||||||
|
MainTag,
|
||||||
|
}
|
||||||
|
|
||||||
|
mod tag_collector {
|
||||||
|
use super::*;
|
||||||
|
|
||||||
|
const BEATING_PERIOD: i32 = 0x8000;
|
||||||
|
const BEATING_HALFPERIOD: i32 = 0x4000;
|
||||||
|
|
||||||
|
// for main PLL
|
||||||
|
static GTX_TAG: Mutex<u32> = Mutex::new(0);
|
||||||
|
static GTX_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();
|
||||||
|
*GTX_TAG.lock() = 0;
|
||||||
|
*MAIN_TAG.lock() = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn clear_phase_diff_ready() {
|
||||||
|
*GTX_TAG_READY.lock() = false;
|
||||||
|
*MAIN_TAG_READY.lock() = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn collect_tags(interrupt: FIQ) {
|
||||||
|
match interrupt {
|
||||||
|
FIQ::GTXTag => {
|
||||||
|
*GTX_TAG.lock() = unsafe { csr::wrpll::gtx_tag_read() };
|
||||||
|
*GTX_TAG_READY.lock() = true;
|
||||||
|
}
|
||||||
|
FIQ::MainTag => {
|
||||||
|
*MAIN_TAG.lock() = unsafe { csr::wrpll::main_tag_read() };
|
||||||
|
*MAIN_TAG_READY.lock() = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn phase_diff_ready() -> bool {
|
||||||
|
*GTX_TAG_READY.lock() && *MAIN_TAG_READY.lock()
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn get_period_error() -> i32 {
|
||||||
|
// n * BEATING_PERIOD - GTX_TAG(n) mod BEATING_PERIOD
|
||||||
|
let mut period_error = (*GTX_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) - GTX_TAG(n) mod BEATING_PERIOD
|
||||||
|
let mut phase_error = (*MAIN_TAG.lock())
|
||||||
|
.overflowing_sub(*GTX_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
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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, 0x04814E8F442); // 125Mhz*32767/32768
|
||||||
|
|
||||||
|
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(())
|
||||||
|
}
|
||||||
|
|
||||||
|
fn set_fiq(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(())
|
||||||
|
}
|
||||||
|
|
||||||
|
/// 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 (gtx_count, main_count, _helper_count) = get_freq_counts(timer);
|
||||||
|
let mut base_adpll_lock = BASE_ADPLL.lock();
|
||||||
|
*base_adpll_lock = count2adpll(gtx_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, 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> {
|
||||||
|
*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: FIQ) {
|
||||||
|
match interrupt {
|
||||||
|
FIQ::GTXTag => unsafe { csr::wrpll::gtx_tag_ev_pending_write(1) },
|
||||||
|
FIQ::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_write(1) },
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
fn is_pending(interrupt: FIQ) -> bool {
|
||||||
|
match interrupt {
|
||||||
|
FIQ::GTXTag => unsafe { csr::wrpll::gtx_tag_ev_pending_read() == 1 },
|
||||||
|
FIQ::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_read() == 1 },
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn interrupt_handler() {
|
||||||
|
if is_pending(FIQ::GTXTag) {
|
||||||
|
tag_collector::collect_tags(FIQ::GTXTag);
|
||||||
|
clear_pending(FIQ::GTXTag);
|
||||||
|
|
||||||
|
helper_pll().expect("failed to run helper DCXO PLL");
|
||||||
|
}
|
||||||
|
|
||||||
|
if is_pending(FIQ::MainTag) {
|
||||||
|
tag_collector::collect_tags(FIQ::MainTag);
|
||||||
|
clear_pending(FIQ::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_fiq(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(FIQ::GTXTag);
|
||||||
|
clear_pending(FIQ::MainTag);
|
||||||
|
|
||||||
|
// use nFIQ to avoid IRQ being disabled by mutex lock and mess up PLL
|
||||||
|
set_fiq(true);
|
||||||
|
info!("WRPLL interrupt enabled");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -0,0 +1,21 @@
|
||||||
|
#[cfg(has_wrpll)]
|
||||||
|
use libboard_artiq::si549;
|
||||||
|
use libboard_zynq::{println, stdio};
|
||||||
|
use libcortex_a9::{interrupt_handler, regs::MPIDR};
|
||||||
|
use libregister::RegisterR;
|
||||||
|
|
||||||
|
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_wrpll)]
|
||||||
|
si549::wrpll::interrupt_handler();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
_ => {}
|
||||||
|
};
|
||||||
|
|
||||||
|
stdio::drop_uart();
|
||||||
|
println!("FIQ");
|
||||||
|
loop {}
|
||||||
|
});
|
|
@ -21,6 +21,7 @@ pub use pl::csr::rtio_core;
|
||||||
use void::Void;
|
use void::Void;
|
||||||
|
|
||||||
pub mod eh_artiq;
|
pub mod eh_artiq;
|
||||||
|
pub mod fiq;
|
||||||
pub mod i2c;
|
pub mod i2c;
|
||||||
pub mod irq;
|
pub mod irq;
|
||||||
pub mod kernel;
|
pub mod kernel;
|
||||||
|
|
|
@ -799,6 +799,8 @@ pub extern "C" fn main_core0() -> i32 {
|
||||||
unsafe {
|
unsafe {
|
||||||
csr::gt_drtio::txenable_write(0xffffffffu32 as _);
|
csr::gt_drtio::txenable_write(0xffffffffu32 as _);
|
||||||
}
|
}
|
||||||
|
#[cfg(has_wrpll)]
|
||||||
|
si549::wrpll::helper_setup(&mut timer).expect("cannot initialize helper Si549");
|
||||||
|
|
||||||
#[cfg(has_drtio_routing)]
|
#[cfg(has_drtio_routing)]
|
||||||
let mut repeaters = [repeater::Repeater::default(); csr::DRTIOREP.len()];
|
let mut repeaters = [repeater::Repeater::default(); csr::DRTIOREP.len()];
|
||||||
|
@ -841,6 +843,9 @@ pub extern "C" fn main_core0() -> i32 {
|
||||||
si5324::siphaser::calibrate_skew(&mut timer).expect("failed to calibrate skew");
|
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
|
// Various managers created here, so when link is dropped, all DMA traces
|
||||||
// are cleared out for a clean slate on subsequent connections,
|
// are cleared out for a clean slate on subsequent connections,
|
||||||
// without a manual intervention.
|
// without a manual intervention.
|
||||||
|
@ -898,6 +903,8 @@ pub extern "C" fn main_core0() -> i32 {
|
||||||
info!("uplink is down, switching to local oscillator clock");
|
info!("uplink is down, switching to local oscillator clock");
|
||||||
#[cfg(has_siphaser)]
|
#[cfg(has_siphaser)]
|
||||||
si5324::siphaser::select_recovered_clock(&mut i2c, false, &mut timer).expect("failed to switch clocks");
|
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