rtio_clocking: verify clock switch

src/runtime/src/main.rs

@@ -110,7 +110,7 @@ pub fn main_core0() {
     info!("NAR3/Zynq7000 starting...");
 
     init_gateware();
-    timer.delay_us(500); // wait for FCLK to switch and MMCM to lock
+    timer.delay_us(500); // wait for FCLK to switch and PLL to lock
 
     ram::init_alloc_core0();
     gic::InterruptController::gic(mpcore::RegisterBlock::mpcore()).enable_interrupts();

src/runtime/src/rtio_clocking.rs

@@ -1,4 +1,4 @@
-use log::{info, warn};
+use log::{info, warn, error};
 use libboard_zynq::timer::GlobalTimer;
 use embedded_hal::blocking::delay::DelayMs;
 use libconfig::Config;
@@ -73,6 +73,15 @@ fn init_rtio(timer: &mut GlobalTimer) {
         pl::csr::sys_crg::clock_switch_write(1);
     }
     // if it's not locked, it will hang at the CSR.
+
+    timer.delay_ms(20); // wait for CPLL/QPLL/SYS PLL lock
+    let clk = unsafe { pl::csr::sys_crg::current_clock_read() };
+    if clk == 1 {
+        info!("SYS CLK switched successfully");
+    }
+    else {
+        error!("SYS CLK did not switch");
+    }
     unsafe {
         pl::csr::rtio_core::reset_phy_write(1);
     }
@@ -83,11 +92,19 @@ fn init_rtio(timer: &mut GlobalTimer) {
 #[cfg(has_drtio)]
 fn init_drtio(timer: &mut GlobalTimer)
 {
+    timer.delay_ms(1000); // wait for si output to really stabilize
     unsafe {
         pl::csr::drtio_transceiver::stable_clkin_write(1);
     }
     
-    timer.delay_ms(20); // wait for CPLL/QPLL/MMCM lock
+    timer.delay_ms(20); // wait for CPLL/QPLL/SYS PLL lock
+    let clk = unsafe { pl::csr::sys_crg::current_clock_read() };
+    if clk == 1 {
+        info!("SYS CLK switched successfully");
+    }
+    else {
+        error!("SYS CLK did not switch");
+    }
     unsafe {
         pl::csr::rtio_core::reset_phy_write(1);
         pl::csr::drtio_transceiver::txenable_write(0xffffffffu32 as _);