From 7bb74f83ee18704efaeccfd6b1aa1854005cf274 Mon Sep 17 00:00:00 2001
From: newell <newell.jensen@gmail.com>
Date: Fri, 1 Nov 2024 17:22:05 -0700
Subject: [PATCH] Set fclk0 for ebaz4205

---
 src/runtime/src/rtio_clocking.rs | 62 ++++++++++++++++++++++++++++++++
 1 file changed, 62 insertions(+)

diff --git a/src/runtime/src/rtio_clocking.rs b/src/runtime/src/rtio_clocking.rs
index 9cb42b7..db3b3fa 100644
--- a/src/runtime/src/rtio_clocking.rs
+++ b/src/runtime/src/rtio_clocking.rs
@@ -15,6 +15,8 @@ use libconfig::Config;
 #[cfg(not(feature = "target_ebaz4205"))]
 use log::info;
 use log::warn;
+#[cfg(feature = "target_ebaz4205")]
+use {libboard_zynq::slcr, libregister::RegisterRW};
 
 #[derive(Debug, PartialEq, Copy, Clone)]
 #[allow(non_camel_case_types)]
@@ -410,6 +412,55 @@ 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;
+
+    match clk {
+        RtioClock::Int_100 => {
+            target_freq = 100_000_000;
+        }
+        RtioClock::Int_125 => {
+            target_freq = 125_000_000;
+        }
+        RtioClock::Int_150 => {
+            target_freq = 150_000_000;
+        }
+        _ => {}
+    }
+    let mut divisor0 = 1u8; // Start with divisor0 at 1
+    let mut divisor1 = 1u8; // Default value for divisor1
+
+    // Calculate the smallest valid divisor0 and divisor1 pair
+    while divisor0 < 64 && io_pll_freq / u32::from(divisor0) > target_freq {
+        divisor0 += 1;
+    }
+
+    // If divisor0 alone isn't enough, adjust divisor1 as well
+    if io_pll_freq / (u32::from(divisor0) * u32::from(divisor1)) > target_freq {
+        divisor1 += 1;
+        divisor0 = 1;
+
+        while divisor1 < 64 && io_pll_freq / (u32::from(divisor0) * u32::from(divisor1)) > target_freq {
+            divisor1 += 1;
+        }
+    }
+
+    // Ensure the calculated divisors achieve the target frequency
+    if io_pll_freq / (u32::from(divisor0) * u32::from(divisor1)) == target_freq {
+        slcr::RegisterBlock::unlocked(|slcr| {
+            slcr.fpga0_clk_ctrl
+                .modify(|_, w| w.divisor0(divisor0).divisor1(divisor1));
+        });
+    } else {
+        warn!(
+            "Could not set fclk0 for target frequency of '{:?}' with available divisors",
+            target_freq
+        );
+    }
+}
+
 pub fn init(timer: &mut GlobalTimer, cfg: &Config) {
     let clk = get_rtio_clock_cfg(cfg);
     #[cfg(has_si5324)]
@@ -436,6 +487,17 @@ pub fn init(timer: &mut GlobalTimer, cfg: &Config) {
     #[cfg(not(any(has_drtio, feature = "target_ebaz4205")))]
     init_rtio(timer);
 
+    #[cfg(feature = "target_ebaz4205")]
+    {
+        // Set FPGA0_FCLK
+        match clk {
+            RtioClock::Int_100 | RtioClock::Int_125 | RtioClock::Int_150 => {
+                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