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) } fn sda_i(dcxo: DCXO) -> bool { match dcxo { DCXO::Main => unsafe { csr::main_dcxo::sda_in_read() != 0 }, #[cfg(has_wrpll)] DCXO::Helper => unsafe { csr::helper_dcxo::sda_in_read() != 0 }, } } fn sda_oe(dcxo: DCXO, oe: bool) { let val = if oe { 1 } else { 0 }; match dcxo { DCXO::Main => unsafe { csr::main_dcxo::sda_oe_write(val) }, #[cfg(has_wrpll)] DCXO::Helper => unsafe { csr::helper_dcxo::sda_oe_write(val) }, }; } fn sda_o(dcxo: DCXO, o: bool) { let val = if o { 1 } else { 0 }; match dcxo { DCXO::Main => unsafe { csr::main_dcxo::sda_out_write(val) }, #[cfg(has_wrpll)] DCXO::Helper => unsafe { csr::helper_dcxo::sda_out_write(val) }, }; } fn scl_oe(dcxo: DCXO, oe: bool) { let val = if oe { 1 } else { 0 }; match dcxo { DCXO::Main => unsafe { csr::main_dcxo::scl_oe_write(val) }, #[cfg(has_wrpll)] DCXO::Helper => unsafe { csr::helper_dcxo::scl_oe_write(val) }, }; } fn scl_o(dcxo: DCXO, o: bool) { let val = if o { 1 } else { 0 }; match dcxo { DCXO::Main => unsafe { csr::main_dcxo::scl_out_write(val) }, #[cfg(has_wrpll)] DCXO::Helper => unsafe { csr::helper_dcxo::scl_out_write(val) }, }; } 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 { 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 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 = Mutex::new(0); static H_INTEGRATOR: Mutex = Mutex::new(0); static M_INTEGRATOR: Mutex = 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 = Mutex::new(0); static GTX_TAG_READY: Mutex = Mutex::new(false); static MAIN_TAG: Mutex = Mutex::new(0); static MAIN_TAG_READY: Mutex = 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"); } } }