continue with FSBL

zynq7000-hal/src/clocks/mod.rs

@@ -1,5 +1,5 @@
 //! Clock module.
-use arbitrary_int::Number;
+use arbitrary_int::{Number, u6};
 
 pub mod pll;
 
@@ -47,21 +47,67 @@ impl ArmClocks {
 
 #[derive(Debug)]
 pub struct DdrClocks {
+    /// DDR reference clock generated by the DDR PLL.
     ref_clk: Hertz,
     ddr_3x_clk: Hertz,
     ddr_2x_clk: Hertz,
 }
 
 impl DdrClocks {
+    /// Update the DDR 3x and 2x clocks in the SLCR.
+    ///
+    /// Usually, the DDR PLL output clock will be set to an even multiple of the DDR operating
+    /// frequency. In that case, the multiplicator should be used as the DDR 3x clock divisor.
+    /// The DDR 2x clock divisor should be set so that the resulting clock is 2/3 of the DDR
+    /// operating frequency.
+    ///
+    /// # Safety
+    ///
+    /// This should only be called once during start-up. It accesses the SLCR register.
+    pub unsafe fn configure_2x_3x_clk(ddr_3x_div: u6, ddr_2x_div: u6) {
+        // Safety: The DDR clock structure is a singleton.
+        unsafe {
+            crate::slcr::Slcr::with(|slcr| {
+                slcr.clk_ctrl().modify_ddr_clk_ctrl(|mut val| {
+                    val.set_div_3x_clk(ddr_3x_div);
+                    val.set_div_2x_clk(ddr_2x_div);
+                    val
+                });
+            });
+        }
+    }
+
+    /// Update the DDR 3x and 2x clocks in the SLCR and creates a DDR clock information structure.
+    ///
+    /// Usually, the DDR PLL output clock will be set to an even multiple of the DDR operating
+    /// frequency. In that case, the multiplicator should be used as the DDR 3x clock divisor.
+    /// The DDR 2x clock divisor should be set so that the resulting clock is 2/3 of the DDR
+    /// operating frequency.
+    ///
+    /// # Safety
+    ///
+    /// This should only be called once during start-up. It accesses the SLCR register.
+    pub unsafe fn new_with_2x_3x_init(ref_clk: Hertz, ddr_3x_div: u6, ddr_2x_div: u6) -> Self {
+        unsafe { Self::configure_2x_3x_clk(ddr_3x_div, ddr_2x_div) };
+        Self {
+            ref_clk,
+            ddr_3x_clk: ref_clk / ddr_3x_div.as_u32(),
+            ddr_2x_clk: ref_clk / ddr_2x_div.as_u32(),
+        }
+    }
+
     /// Reference clock provided by DDR PLL which is used to calculate all other clock frequencies.
     pub const fn ref_clk(&self) -> Hertz {
         self.ref_clk
     }
 
+    /// DDR 3x clock which is used by the DRAM and must be set to the operating frequency.
     pub fn ddr_3x_clk(&self) -> Hertz {
         self.ddr_3x_clk
     }
 
+    /// DDR 2x clock is used by the interconnect and is typically set to 2/3 of the operating
+    /// frequency.
     pub fn ddr_2x_clk(&self) -> Hertz {
         self.ddr_2x_clk
     }

zynq7000-hal/src/ddr.rs

@@ -0,0 +1,41 @@
+use arbitrary_int::u6;
+use zynq7000::slcr::clocks::DciClkCtrl;
+
+use crate::{clocks::DdrClocks, time::Hertz};
+
+const DCI_MAX_FREQ: Hertz = Hertz::from_raw(10_000_000);
+
+pub fn calculate_dci_divisors(ddr_clk: Hertz) -> (u6, u6) {
+    let target_div = ddr_clk.raw().div_ceil(DCI_MAX_FREQ.raw());
+
+    let mut best = None;
+    let mut best_error = 0;
+    for divisor0 in 1..63 {
+        for divisor1 in 1..63 {
+            let current_div = (divisor0 as u32) * (divisor1 as u32);
+            let error = current_div.abs_diff(target_div);
+            if best.is_none() || best_error > error {
+                best = Some((divisor0, divisor1));
+                best_error = error;
+            }
+        }
+    }
+    best.map(|(div0, div1)| (u6::new(div0), u6::new(div1)))
+        .unwrap()
+}
+
+pub fn configure_dci(ddr_clk: &DdrClocks) {
+    let (divisor0, divisor1) = calculate_dci_divisors(ddr_clk.ref_clk());
+    // Safety: Only done once here during start-up.
+    unsafe {
+        crate::Slcr::with(|slcr| {
+            slcr.clk_ctrl().write_dci_clk_ctrl(
+                DciClkCtrl::builder()
+                    .with_divisor_1(divisor1)
+                    .with_divisor_0(divisor0)
+                    .with_clk_act(true)
+                    .build(),
+            );
+        });
+    }
+}

zynq7000-hal/src/lib.rs

@@ -14,6 +14,7 @@ use zynq7000::slcr::{BootModeRegister, BootPllConfig, LevelShifterReg};
 
 pub mod cache;
 pub mod clocks;
+pub mod ddr;
 pub mod eth;
 pub mod gic;
 pub mod gpio;