From 2c633409b8b0f55f2771480f4457e88dc5ab843e Mon Sep 17 00:00:00 2001
From: newell <newell.jensen@gmail.com>
Date: Sun, 17 Nov 2024 10:08:43 +0800
Subject: [PATCH] Set FCLK0 for EBAZ4205

EBAZ4205 uses FCLK0 as the RTIO clock.

If the user modifies the gateware to use an external clock, FCLK0 is not used.
Co-authored-by: newell <newell.jensen@gmail.com>
Co-committed-by: newell <newell.jensen@gmail.com>
---
 src/runtime/src/rtio_clocking.rs | 48 ++++++++++++++++++++++++++++++--
 1 file changed, 45 insertions(+), 3 deletions(-)

diff --git a/src/runtime/src/rtio_clocking.rs b/src/runtime/src/rtio_clocking.rs
index 9cb42b7..abe78c0 100644
--- a/src/runtime/src/rtio_clocking.rs
+++ b/src/runtime/src/rtio_clocking.rs
@@ -12,9 +12,9 @@ use libboard_artiq::si549;
 use libboard_zynq::i2c::I2c;
 use libboard_zynq::timer::GlobalTimer;
 use libconfig::Config;
-#[cfg(not(feature = "target_ebaz4205"))]
-use log::info;
-use log::warn;
+use log::{info, warn};
+#[cfg(feature = "target_ebaz4205")]
+use {libboard_zynq::slcr, libregister::RegisterRW};
 
 #[derive(Debug, PartialEq, Copy, Clone)]
 #[allow(non_camel_case_types)]
@@ -410,6 +410,38 @@ fn get_si549_setting(clk: RtioClock) -> si549::FrequencySetting {
     }
 }
 
+#[cfg(feature = "target_ebaz4205")]
+fn set_fclk0_freq(clk: RtioClock, cfg: &Config) {
+    let io_pll_freq: u32 = 1_000_000_000; // Hardcoded in zynq-rs
+    let mut target_freq = 0;
+    let mut divisor0 = 1u8;
+
+    match clk {
+        RtioClock::Int_100 => {
+            target_freq = 100_000_000;
+            divisor0 = 10;
+        }
+        RtioClock::Int_125 => {
+            target_freq = 125_000_000;
+            divisor0 = 8;
+        }
+        _ => {
+            warn!("Unsupported RTIO Clock: '{:?}'", clk);
+            return;
+        }
+    }
+
+    slcr::RegisterBlock::unlocked(|slcr| {
+        slcr.fpga0_clk_ctrl.modify(|_, w| w.divisor0(divisor0));
+    });
+
+    info!(
+        "Set FCLK0 to {:.2} MHz (target: {} MHz).",
+        io_pll_freq as f64 / divisor0 as f64,
+        target_freq / 1_000_000
+    );
+}
+
 pub fn init(timer: &mut GlobalTimer, cfg: &Config) {
     let clk = get_rtio_clock_cfg(cfg);
     #[cfg(has_si5324)]
@@ -436,6 +468,16 @@ pub fn init(timer: &mut GlobalTimer, cfg: &Config) {
     #[cfg(not(any(has_drtio, feature = "target_ebaz4205")))]
     init_rtio(timer);
 
+    #[cfg(feature = "target_ebaz4205")]
+    {
+        match clk {
+            RtioClock::Int_100 | RtioClock::Int_125 => {
+                set_fclk0_freq(clk, cfg);
+            }
+            _ => {} // Not set for external clocks
+        }
+    }
+
     #[cfg(all(has_si549, has_wrpll))]
     {
         // SYS CLK switch will reset CSRs that are used by WRPLL