594 lines
19 KiB
Rust
594 lines
19 KiB
Rust
use core::result::Result::Ok;
|
|
|
|
use embedded_hal::prelude::_embedded_hal_blocking_delay_DelayUs;
|
|
use libboard_zynq::timer::GlobalTimer;
|
|
use log::info;
|
|
|
|
use crate::pl::csr;
|
|
|
|
#[cfg(feature = "target_kasli_soc")]
|
|
const ADDRESS: u8 = 0x67;
|
|
|
|
mod i2c {
|
|
use super::*;
|
|
|
|
#[derive(Clone, Copy)]
|
|
pub enum DCXO {
|
|
Main,
|
|
#[cfg(has_wrpll)]
|
|
Helper,
|
|
}
|
|
|
|
fn half_period(timer: &mut GlobalTimer) {
|
|
timer.delay_us(1)
|
|
}
|
|
const SDA_MASK: u8 = 2;
|
|
const SCL_MASK: u8 = 1;
|
|
|
|
fn sda_i(dcxo: DCXO) -> bool {
|
|
let reg = match dcxo {
|
|
DCXO::Main => unsafe { csr::main_dcxo::gpio_in_read() },
|
|
#[cfg(has_wrpll)]
|
|
DCXO::Helper => unsafe { csr::helper_dcxo::gpio_in_read() },
|
|
};
|
|
reg & SDA_MASK != 0
|
|
}
|
|
|
|
fn sda_oe(dcxo: DCXO, oe: bool) {
|
|
let reg = match dcxo {
|
|
DCXO::Main => unsafe { csr::main_dcxo::gpio_oe_read() },
|
|
#[cfg(has_wrpll)]
|
|
DCXO::Helper => unsafe { csr::helper_dcxo::gpio_oe_read() },
|
|
};
|
|
let reg = if oe { reg | SDA_MASK } else { reg & !SDA_MASK };
|
|
match dcxo {
|
|
DCXO::Main => unsafe { csr::main_dcxo::gpio_oe_write(reg) },
|
|
#[cfg(has_wrpll)]
|
|
DCXO::Helper => unsafe { csr::helper_dcxo::gpio_oe_write(reg) },
|
|
}
|
|
}
|
|
|
|
fn sda_o(dcxo: DCXO, o: bool) {
|
|
let reg = match dcxo {
|
|
DCXO::Main => unsafe { csr::main_dcxo::gpio_out_read() },
|
|
#[cfg(has_wrpll)]
|
|
DCXO::Helper => unsafe { csr::helper_dcxo::gpio_out_read() },
|
|
};
|
|
let reg = if o { reg | SDA_MASK } else { reg & !SDA_MASK };
|
|
match dcxo {
|
|
DCXO::Main => unsafe { csr::main_dcxo::gpio_out_write(reg) },
|
|
#[cfg(has_wrpll)]
|
|
DCXO::Helper => unsafe { csr::helper_dcxo::gpio_out_write(reg) },
|
|
}
|
|
}
|
|
|
|
fn scl_oe(dcxo: DCXO, oe: bool) {
|
|
let reg = match dcxo {
|
|
DCXO::Main => unsafe { csr::main_dcxo::gpio_oe_read() },
|
|
#[cfg(has_wrpll)]
|
|
DCXO::Helper => unsafe { csr::helper_dcxo::gpio_oe_read() },
|
|
};
|
|
let reg = if oe { reg | SCL_MASK } else { reg & !SCL_MASK };
|
|
match dcxo {
|
|
DCXO::Main => unsafe { csr::main_dcxo::gpio_oe_write(reg) },
|
|
#[cfg(has_wrpll)]
|
|
DCXO::Helper => unsafe { csr::helper_dcxo::gpio_oe_write(reg) },
|
|
}
|
|
}
|
|
|
|
fn scl_o(dcxo: DCXO, o: bool) {
|
|
let reg = match dcxo {
|
|
DCXO::Main => unsafe { csr::main_dcxo::gpio_out_read() },
|
|
#[cfg(has_wrpll)]
|
|
DCXO::Helper => unsafe { csr::helper_dcxo::gpio_out_read() },
|
|
};
|
|
let reg = if o { reg | SCL_MASK } else { reg & !SCL_MASK };
|
|
match dcxo {
|
|
DCXO::Main => unsafe { csr::main_dcxo::gpio_out_write(reg) },
|
|
#[cfg(has_wrpll)]
|
|
DCXO::Helper => unsafe { csr::helper_dcxo::gpio_out_write(reg) },
|
|
}
|
|
}
|
|
|
|
pub fn init(dcxo: DCXO, timer: &mut GlobalTimer) -> Result<(), &'static str> {
|
|
// Set SCL as output, and high level
|
|
scl_o(dcxo, true);
|
|
scl_oe(dcxo, true);
|
|
// Prepare a zero level on SDA so that sda_oe pulls it down
|
|
sda_o(dcxo, false);
|
|
// Release SDA
|
|
sda_oe(dcxo, false);
|
|
|
|
// Check the I2C bus is ready
|
|
half_period(timer);
|
|
half_period(timer);
|
|
if !sda_i(dcxo) {
|
|
// Try toggling SCL a few times
|
|
for _bit in 0..8 {
|
|
scl_o(dcxo, false);
|
|
half_period(timer);
|
|
scl_o(dcxo, true);
|
|
half_period(timer);
|
|
}
|
|
}
|
|
|
|
if !sda_i(dcxo) {
|
|
return Err("SDA is stuck low and doesn't get unstuck");
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
pub fn start(dcxo: DCXO, timer: &mut GlobalTimer) {
|
|
// Set SCL high then SDA low
|
|
scl_o(dcxo, true);
|
|
half_period(timer);
|
|
sda_oe(dcxo, true);
|
|
half_period(timer);
|
|
}
|
|
|
|
pub fn stop(dcxo: DCXO, timer: &mut GlobalTimer) {
|
|
// First, make sure SCL is low, so that the target releases the SDA line
|
|
scl_o(dcxo, false);
|
|
half_period(timer);
|
|
// Set SCL high then SDA high
|
|
sda_oe(dcxo, true);
|
|
scl_o(dcxo, true);
|
|
half_period(timer);
|
|
sda_oe(dcxo, false);
|
|
half_period(timer);
|
|
}
|
|
|
|
pub fn write(dcxo: DCXO, data: u8, timer: &mut GlobalTimer) -> bool {
|
|
// MSB first
|
|
for bit in (0..8).rev() {
|
|
// Set SCL low and set our bit on SDA
|
|
scl_o(dcxo, false);
|
|
sda_oe(dcxo, data & (1 << bit) == 0);
|
|
half_period(timer);
|
|
// Set SCL high ; data is shifted on the rising edge of SCL
|
|
scl_o(dcxo, true);
|
|
half_period(timer);
|
|
}
|
|
// Check ack
|
|
// Set SCL low, then release SDA so that the I2C target can respond
|
|
scl_o(dcxo, false);
|
|
half_period(timer);
|
|
sda_oe(dcxo, false);
|
|
// Set SCL high and check for ack
|
|
scl_o(dcxo, true);
|
|
half_period(timer);
|
|
// returns true if acked (I2C target pulled SDA low)
|
|
!sda_i(dcxo)
|
|
}
|
|
|
|
pub fn read(dcxo: DCXO, ack: bool, timer: &mut GlobalTimer) -> u8 {
|
|
// Set SCL low first, otherwise setting SDA as input may cause a transition
|
|
// on SDA with SCL high which will be interpreted as START/STOP condition.
|
|
scl_o(dcxo, false);
|
|
half_period(timer); // make sure SCL has settled low
|
|
sda_oe(dcxo, false);
|
|
|
|
let mut data: u8 = 0;
|
|
|
|
// MSB first
|
|
for bit in (0..8).rev() {
|
|
scl_o(dcxo, false);
|
|
half_period(timer);
|
|
// Set SCL high and shift data
|
|
scl_o(dcxo, true);
|
|
half_period(timer);
|
|
if sda_i(dcxo) {
|
|
data |= 1 << bit
|
|
}
|
|
}
|
|
// Send ack
|
|
// Set SCL low and pull SDA low when acking
|
|
scl_o(dcxo, false);
|
|
if ack {
|
|
sda_oe(dcxo, true)
|
|
}
|
|
half_period(timer);
|
|
// then set SCL high
|
|
scl_o(dcxo, true);
|
|
half_period(timer);
|
|
|
|
data
|
|
}
|
|
}
|
|
|
|
fn write(dcxo: i2c::DCXO, reg: u8, val: u8, timer: &mut GlobalTimer) -> Result<(), &'static str> {
|
|
i2c::start(dcxo, timer);
|
|
if !i2c::write(dcxo, ADDRESS << 1, timer) {
|
|
return Err("Si549 failed to ack write address");
|
|
}
|
|
if !i2c::write(dcxo, reg, timer) {
|
|
return Err("Si549 failed to ack register");
|
|
}
|
|
if !i2c::write(dcxo, val, timer) {
|
|
return Err("Si549 failed to ack value");
|
|
}
|
|
i2c::stop(dcxo, timer);
|
|
Ok(())
|
|
}
|
|
|
|
fn read(dcxo: i2c::DCXO, reg: u8, timer: &mut GlobalTimer) -> Result<u8, &'static str> {
|
|
i2c::start(dcxo, timer);
|
|
if !i2c::write(dcxo, ADDRESS << 1, timer) {
|
|
return Err("Si549 failed to ack write address");
|
|
}
|
|
if !i2c::write(dcxo, reg, timer) {
|
|
return Err("Si549 failed to ack register");
|
|
}
|
|
i2c::stop(dcxo, timer);
|
|
|
|
i2c::start(dcxo, timer);
|
|
if !i2c::write(dcxo, (ADDRESS << 1) | 1, timer) {
|
|
return Err("Si549 failed to ack read address");
|
|
}
|
|
let val = i2c::read(dcxo, false, timer);
|
|
i2c::stop(dcxo, timer);
|
|
Ok(val)
|
|
}
|
|
|
|
fn setup(dcxo: i2c::DCXO, hsdiv: u16, lsdiv: u8, fbdiv: u64, timer: &mut GlobalTimer) -> Result<(), &'static str> {
|
|
i2c::init(dcxo, timer)?;
|
|
|
|
write(dcxo, 255, 0x00, timer)?; // PAGE
|
|
write(dcxo, 69, 0x00, timer)?; // Disable FCAL override.
|
|
write(dcxo, 17, 0x00, timer)?; // Synchronously disable output
|
|
|
|
// The Si549 has no ID register, so we check that it responds correctly
|
|
// by writing values to a RAM-like register and reading them back.
|
|
for test_value in 0..255 {
|
|
write(dcxo, 23, test_value, timer)?;
|
|
let readback = read(dcxo, 23, timer)?;
|
|
if readback != test_value {
|
|
return Err("Si549 detection failed");
|
|
}
|
|
}
|
|
|
|
write(dcxo, 23, hsdiv as u8, timer)?;
|
|
write(dcxo, 24, (hsdiv >> 8) as u8 | (lsdiv << 4), timer)?;
|
|
write(dcxo, 26, fbdiv as u8, timer)?;
|
|
write(dcxo, 27, (fbdiv >> 8) as u8, timer)?;
|
|
write(dcxo, 28, (fbdiv >> 16) as u8, timer)?;
|
|
write(dcxo, 29, (fbdiv >> 24) as u8, timer)?;
|
|
write(dcxo, 30, (fbdiv >> 32) as u8, timer)?;
|
|
write(dcxo, 31, (fbdiv >> 40) as u8, timer)?;
|
|
|
|
write(dcxo, 7, 0x08, timer)?; // Start FCAL
|
|
timer.delay_us(30_000); // Internal FCAL VCO calibration
|
|
write(dcxo, 17, 0x01, timer)?; // Synchronously enable output
|
|
|
|
Ok(())
|
|
}
|
|
|
|
pub fn main_setup(timer: &mut GlobalTimer) -> Result<(), &'static str> {
|
|
unsafe {
|
|
csr::main_dcxo::bitbang_enable_write(1);
|
|
csr::main_dcxo::i2c_address_write(ADDRESS);
|
|
}
|
|
|
|
#[cfg(rtio_frequency = "125.0")]
|
|
let (m_hsdiv, m_lsdiv, m_fbdiv) = (0x058, 0, 0x04815791F25);
|
|
|
|
setup(i2c::DCXO::Main, m_hsdiv, m_lsdiv, m_fbdiv, timer)?;
|
|
|
|
// Si549 maximum settling time for large frequency change.
|
|
timer.delay_us(40_000);
|
|
|
|
unsafe {
|
|
csr::main_dcxo::bitbang_enable_write(0);
|
|
}
|
|
|
|
info!("Main Si549 started");
|
|
Ok(())
|
|
}
|
|
|
|
#[cfg(has_wrpll)]
|
|
pub mod wrpll {
|
|
|
|
use libboard_zynq::gic;
|
|
use libcortex_a9::mutex::Mutex;
|
|
|
|
use super::*;
|
|
|
|
const IRQ_ID: [u8; 2] = [61, 62];
|
|
|
|
const TIMER_WIDTH: u32 = 24;
|
|
const COUNTER_DIV: u32 = 2;
|
|
|
|
const ADPLL_MAX: i32 = (950.0 / 0.0001164) as i32;
|
|
|
|
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 IRQ {
|
|
GTXTag,
|
|
MainTag,
|
|
}
|
|
|
|
mod tag_collector {
|
|
use super::*;
|
|
|
|
const BEATING_PERIOD: i32 = 0x4000;
|
|
const BEATING_HALFPERIOD: i32 = 0x2000;
|
|
|
|
// for helper PLL
|
|
static LAST_GTX_TAG: Mutex<u32> = Mutex::new(0);
|
|
static FIRST_GTX_INTERRUPT: Mutex<bool> = Mutex::new(false);
|
|
static PERIOD_DET_READY: Mutex<bool> = Mutex::new(false);
|
|
|
|
// 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_period_det_ready();
|
|
clear_phase_det_ready();
|
|
*LAST_GTX_TAG.lock() = 0;
|
|
*FIRST_GTX_INTERRUPT.lock() = true;
|
|
*GTX_TAG.lock() = 0;
|
|
*MAIN_TAG.lock() = 0;
|
|
}
|
|
|
|
pub fn clear_period_det_ready() {
|
|
*PERIOD_DET_READY.lock() = false;
|
|
}
|
|
|
|
pub fn clear_phase_det_ready() {
|
|
*GTX_TAG_READY.lock() = false;
|
|
*MAIN_TAG_READY.lock() = false;
|
|
}
|
|
|
|
pub fn collect_tags(interrupt: IRQ) {
|
|
match interrupt {
|
|
IRQ::GTXTag => {
|
|
let mut gtx_tag_lock = GTX_TAG.lock();
|
|
|
|
if !(*FIRST_GTX_INTERRUPT.lock()) {
|
|
*LAST_GTX_TAG.lock() = *gtx_tag_lock;
|
|
}
|
|
|
|
*gtx_tag_lock = unsafe { csr::wrpll::gtx_tag_read() };
|
|
*FIRST_GTX_INTERRUPT.lock() = false;
|
|
*GTX_TAG_READY.lock() = true;
|
|
}
|
|
IRQ::MainTag => {
|
|
*MAIN_TAG.lock() = unsafe { csr::wrpll::main_tag_read() };
|
|
*MAIN_TAG_READY.lock() = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn period_det_ready() -> bool {
|
|
*PERIOD_DET_READY.lock()
|
|
}
|
|
|
|
pub fn phase_det_ready() -> bool {
|
|
*GTX_TAG_READY.lock() && *MAIN_TAG_READY.lock()
|
|
}
|
|
|
|
pub fn get_period_error() -> i32 {
|
|
// overflowing_sub can calculate the correct beating period when GTX_TAG roll over
|
|
BEATING_PERIOD - ((*GTX_TAG.lock()).overflowing_sub(*LAST_GTX_TAG.lock()).0) as i32
|
|
}
|
|
|
|
pub fn get_phase_error() -> i32 {
|
|
// unwrap tag difference
|
|
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, 0x0481458C94D); // 125Mhz*16383/16384
|
|
|
|
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(())
|
|
}
|
|
|
|
pub fn interrupt_setup(interrupt_controller: &mut gic::InterruptController) {
|
|
for id in IRQ_ID.iter() {
|
|
// setup shared peripheral interrupts (SPI)
|
|
interrupt_controller.enable(
|
|
gic::InterruptId(*id),
|
|
gic::CPUCore::Core0,
|
|
gic::InterruptSensitivity::Edge,
|
|
0,
|
|
);
|
|
}
|
|
}
|
|
|
|
fn set_irq(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(())
|
|
}
|
|
|
|
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: IRQ) {
|
|
match interrupt {
|
|
IRQ::GTXTag => unsafe { csr::wrpll::gtx_tag_ev_pending_write(1) },
|
|
IRQ::MainTag => unsafe { csr::wrpll::main_tag_ev_pending_write(1) },
|
|
}
|
|
}
|
|
|
|
pub fn interrupt_handler(interrupt: IRQ) {
|
|
tag_collector::collect_tags(interrupt);
|
|
|
|
if tag_collector::period_det_ready() {
|
|
helper_pll().expect("failed to run helper DCXO PLL");
|
|
tag_collector::clear_period_det_ready();
|
|
}
|
|
|
|
if tag_collector::phase_det_ready() {
|
|
main_pll().expect("failed to run main DCXO PLL");
|
|
tag_collector::clear_phase_det_ready();
|
|
}
|
|
|
|
clear_pending(interrupt);
|
|
}
|
|
|
|
pub fn helper_pll() -> Result<(), &'static str> {
|
|
const H_KP: i32 = 2;
|
|
const H_KI: f32 = 0.5;
|
|
|
|
let period_err = tag_collector::get_period_error();
|
|
let mut integrator_lock = H_INTEGRATOR.lock();
|
|
|
|
*integrator_lock += (period_err as f32 * H_KI) as i32;
|
|
let mut h_adpll = *BASE_ADPLL.lock() + period_err * H_KP + *integrator_lock;
|
|
|
|
h_adpll = h_adpll.clamp(-ADPLL_MAX, ADPLL_MAX);
|
|
set_adpll(i2c::DCXO::Helper, h_adpll)?;
|
|
|
|
Ok(())
|
|
}
|
|
|
|
pub fn main_pll() -> Result<(), &'static str> {
|
|
const M_KP: i32 = 12;
|
|
const M_KI: i32 = 2;
|
|
|
|
let phase_err = tag_collector::get_phase_error();
|
|
let mut integrator_lock = M_INTEGRATOR.lock();
|
|
|
|
*integrator_lock += phase_err * M_KI;
|
|
let mut m_adpll = *BASE_ADPLL.lock() + phase_err * M_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_irq(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(IRQ::GTXTag);
|
|
clear_pending(IRQ::MainTag);
|
|
set_irq(true);
|
|
info!("WRPLL interrupt enabled");
|
|
}
|
|
}
|
|
}
|