From e86923b51d0bf895d1a33d7f89f7f4e3d60f66f0 Mon Sep 17 00:00:00 2001
From: mwojcik <mw@m-labs.hk>
Date: Tue, 14 Feb 2023 11:24:10 +0800
Subject: [PATCH] rtio_clocking: verify clock switch

---
 src/runtime/src/main.rs          |  2 +-
 src/runtime/src/rtio_clocking.rs | 21 +++++++++++++++++++--
 2 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/src/runtime/src/main.rs b/src/runtime/src/main.rs
index d6c0fb1..3421ee1 100644
--- a/src/runtime/src/main.rs
+++ b/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();
diff --git a/src/runtime/src/rtio_clocking.rs b/src/runtime/src/rtio_clocking.rs
index d9b37d3..00e51c9 100644
--- a/src/runtime/src/rtio_clocking.rs
+++ b/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 _);