libboard_zynq::clocks: setup clock sources and cpu clock

experiments/src/main.rs

@@ -3,7 +3,7 @@
 
 use core::mem::transmute;
 use libcortex_a9::mutex::Mutex;
-use libboard_zynq::{print, println, self as zynq};
+use libboard_zynq::{print, println, self as zynq, clocks::Clocks, clocks::source::{ClockSource, ArmPll, IoPll}};
 use libboard_zc706::{
     ram, alloc::{vec, vec::Vec},
     boot,
@@ -27,6 +27,20 @@ pub fn main_core0() {
         println!("Boot mode: {:?}", zynq::slcr::RegisterBlock::new().boot_mode.read().boot_mode_pins());
     }
 
+    #[cfg(feature = "target_zc706")]
+    const CPU_FREQ: u32 = 800_000_000;
+    #[cfg(feature = "target_cora_z7_10")]
+    const CPU_FREQ: u32 = 650_000_000;
+
+    println!("Setup clock sources...");
+    ArmPll::setup(2 * CPU_FREQ);
+    Clocks::set_cpu_freq(CPU_FREQ);
+    IoPll::setup(700_000_000);
+    libboard_zynq::stdio::drop_uart();
+    println!("PLLs set up");
+    let clocks = zynq::clocks::Clocks::get();
+    println!("CPU Clocks: {}/{}/{}/{}", clocks.cpu_6x4x(), clocks.cpu_3x2x(), clocks.cpu_2x(), clocks.cpu_1x());
+
     let mut flash = zynq::flash::Flash::new(200_000_000).linear_addressing_mode();
     let flash_ram: &[u8] = unsafe { core::slice::from_raw_parts(flash.ptr(), flash.size()) };
     for i in 0..=1 {

libboard_zynq/src/clocks.rs

@@ -1,175 +0,0 @@
-use libregister::{RegisterR, RegisterW, RegisterRW};
-use super::slcr;
-
-#[cfg(feature = "target_zc706")]
-const PS_CLK: u32 = 33_333_333;
-#[cfg(feature = "target_cora_z7_10")]
-const PS_CLK: u32 = 50_000_000;
-
-enum CpuClockMode {
-    /// Clocks run in 4:2:2:1 mode
-    C421,
-    /// Clocks run in 6:3:2:1 mode
-    C621,
-}
-
-impl CpuClockMode {
-    pub fn get() -> Self {
-        let regs = slcr::RegisterBlock::new();
-        if regs.clk_621_true.read().clk_621_true() {
-            CpuClockMode::C621
-        } else {
-            CpuClockMode::C421
-        }
-    }    
-}
-
-#[derive(Debug, Clone)]
-pub struct CpuClocks {
-    /// ARM PLL: Recommended clock source for the CPUs and the interconnect
-    pub arm: u32,
-    /// DDR PLL: Recommended clock for the DDR DRAM controller and AXI_HP interfaces
-    pub ddr: u32,
-    /// I/O PLL: Recommended clock for I/O peripherals
-    pub io: u32,
-}
-
-impl CpuClocks {
-    pub fn get() -> Self {
-        let regs = slcr::RegisterBlock::new();
-        let arm = u32::from(regs.arm_pll_ctrl.read().pll_fdiv()) * PS_CLK;
-        let ddr = u32::from(regs.ddr_pll_ctrl.read().pll_fdiv()) * PS_CLK;
-        let io = u32::from(regs.io_pll_ctrl.read().pll_fdiv()) * PS_CLK;
-        CpuClocks { arm, ddr, io }
-    }
-
-    pub fn cpu_6x4x(&self) -> u32 {
-        let regs = slcr::RegisterBlock::new();
-        let arm_clk_ctrl = regs.arm_clk_ctrl.read();
-        let pll = match arm_clk_ctrl.srcsel() {
-            slcr::ArmPllSource::ArmPll => self.arm,
-            slcr::ArmPllSource::DdrPll => self.ddr,
-            slcr::ArmPllSource::IoPll => self.io,
-        };
-        pll / u32::from(arm_clk_ctrl.divisor())
-    }
-
-    pub fn cpu_3x2x(&self) -> u32 {
-        self.cpu_6x4x() / 2
-    }
-
-    pub fn cpu_2x(&self) -> u32 {
-        match CpuClockMode::get() {
-            CpuClockMode::C421 =>
-                self.cpu_6x4x() / 2,
-            CpuClockMode::C621 =>
-                self.cpu_6x4x() / 3,
-        }
-    }
-
-    pub fn cpu_1x(&self) -> u32 {
-        match CpuClockMode::get() {
-            CpuClockMode::C421 =>
-                self.cpu_6x4x() / 4,
-            CpuClockMode::C621 =>
-                self.cpu_6x4x() / 6,
-        }
-    }
-
-    pub fn uart_ref_clk(&self) -> u32 {
-        let regs = slcr::RegisterBlock::new();
-        let uart_clk_ctrl = regs.uart_clk_ctrl.read();
-        let pll = match uart_clk_ctrl.srcsel() {
-            slcr::PllSource::ArmPll =>
-                self.arm,
-            slcr::PllSource::DdrPll =>
-                self.ddr,
-            slcr::PllSource::IoPll =>
-                self.io,
-        };
-        pll / u32::from(uart_clk_ctrl.divisor())
-    }
-
-    /// Zynq-7000 AP SoC Technical Reference Manual:
-    /// 25.10.4 PLLs
-    pub fn enable_io(target_clock: u32) {
-        let fdiv = (target_clock / PS_CLK).min(66) as u16;
-        slcr::RegisterBlock::unlocked(|slcr| {
-            slcr.io_pll_ctrl.modify(|_, w| w
-                                    .pll_pwrdwn(false)
-                                    .pll_bypass_force(true)
-                                    .pll_fdiv(fdiv)
-            );
-            slcr.io_pll_ctrl.modify(|_, w| w
-                                    .pll_reset(true)
-            );
-            slcr.io_pll_ctrl.modify(|_, w| w
-                                    .pll_reset(false)
-            );
-            while ! slcr.pll_status.read().io_pll_lock() {}
-            slcr.io_pll_ctrl.modify(|_, w| w
-                                    .pll_bypass_force(false)
-                                    .pll_bypass_qual(false)
-            );
-        });
-    }
-
-    /// Zynq-7000 AP SoC Technical Reference Manual:
-    /// 25.10.4 PLLs
-    pub fn enable_ddr(target_clock: u32) {
-        let fdiv = (target_clock / PS_CLK).min(66) as u16;
-        let (pll_res, pll_cp, lock_cnt) = PLL_FDIV_LOCK_PARAM.iter()
-            .filter(|(fdiv_max, _)| fdiv <= *fdiv_max)
-            .nth(0)
-            .expect("PLL_FDIV_LOCK_PARAM")
-            .1.clone();
-        slcr::RegisterBlock::unlocked(|regs| {
-            regs.ddr_pll_ctrl.modify(|_, w| w
-                                     .pll_pwrdwn(false)
-                                     .pll_bypass_force(true)
-                                     .pll_fdiv(fdiv)
-            );
-            regs.ddr_pll_cfg.write(
-                slcr::PllCfg::zeroed()
-                    .pll_res(pll_res)
-                    .pll_cp(pll_cp)
-                    .lock_cnt(lock_cnt)
-            );
-            regs.ddr_pll_ctrl.modify(|_, w| w
-                                     .pll_reset(true)
-            );
-            regs.ddr_pll_ctrl.modify(|_, w| w
-                                     .pll_reset(false)
-            );
-            while ! regs.pll_status.read().ddr_pll_lock() {}
-            regs.ddr_pll_ctrl.modify(|_, w| w
-                                     .pll_bypass_force(false)
-                                     .pll_bypass_qual(false)
-            );
-        });
-    }
-}
-
-/// (pll_fdiv_max, (pll_cp, pll_res, lock_cnt))
-const PLL_FDIV_LOCK_PARAM: &[(u16, (u8, u8, u16))] = &[
-    (13, (2, 6, 750)),
-    (14, (2, 6, 700)),
-    (15, (2, 6, 650)),
-    (16, (2, 10, 625)),
-    (17, (2, 10, 575)),
-    (18, (2, 10, 550)),
-    (19, (2, 10, 525)),
-    (20, (2, 12, 500)),
-    (21, (2, 12, 475)),
-    (22, (2, 12, 450)),
-    (23, (2, 12, 425)),
-    (25, (2, 12, 400)),
-    (26, (2, 12, 375)),
-    (28, (2, 12, 350)),
-    (30, (2, 12, 325)),
-    (33, (2, 2, 300)),
-    (36, (2, 2, 275)),
-    (40, (2, 2, 250)),
-    (47, (3, 12, 250)),
-    (66, (2, 4, 250)),
-];

libboard_zynq/src/clocks/mod.rs

@@ -0,0 +1,107 @@
+use libregister::{RegisterR, RegisterW, RegisterRW};
+use super::slcr;
+pub use slcr::ArmPllSource;
+
+pub mod source;
+use source::*;
+
+enum CpuClockMode {
+    /// Clocks run in 4:2:2:1 mode
+    C421,
+    /// Clocks run in 6:3:2:1 mode
+    C621,
+}
+
+impl CpuClockMode {
+    pub fn get() -> Self {
+        let regs = slcr::RegisterBlock::new();
+        if regs.clk_621_true.read().clk_621_true() {
+            CpuClockMode::C621
+        } else {
+            CpuClockMode::C421
+        }
+    }
+}
+
+#[derive(Debug, Clone)]
+pub struct Clocks {
+    /// ARM PLL: Recommended clock source for the CPUs and the interconnect
+    pub arm: u32,
+    /// DDR PLL: Recommended clock for the DDR DRAM controller and AXI_HP interfaces
+    pub ddr: u32,
+    /// I/O PLL: Recommended clock for I/O peripherals
+    pub io: u32,
+}
+
+impl Clocks {
+    pub fn get() -> Self {
+        Clocks {
+            arm: ArmPll::freq(),
+            ddr: DdrPll::freq(),
+            io: IoPll::freq(),
+        }
+    }
+
+    pub fn set_cpu_freq(target_freq: u32) {
+        let arm_pll = ArmPll::freq();
+        // 1 and 3 cannot be used
+        let mut div = 2u8;
+        while div == 3 || (div < 63 && arm_pll / u32::from(div) > target_freq) {
+            div += 1;
+        }
+
+        slcr::RegisterBlock::unlocked(|slcr| {
+            slcr.arm_clk_ctrl.modify(|_, w| w
+                .srcsel(ArmPllSource::ArmPll)
+                .divisor(div)
+            );
+        })
+    }
+
+    pub fn cpu_6x4x(&self) -> u32 {
+        let slcr = slcr::RegisterBlock::new();
+        let arm_clk_ctrl = slcr.arm_clk_ctrl.read();
+        let pll = match arm_clk_ctrl.srcsel() {
+            ArmPllSource::ArmPll => self.arm,
+            ArmPllSource::DdrPll => self.ddr,
+            ArmPllSource::IoPll => self.io,
+        };
+        pll / u32::from(arm_clk_ctrl.divisor())
+    }
+
+    pub fn cpu_3x2x(&self) -> u32 {
+        self.cpu_6x4x() / 2
+    }
+
+    pub fn cpu_2x(&self) -> u32 {
+        match CpuClockMode::get() {
+            CpuClockMode::C421 =>
+                self.cpu_6x4x() / 2,
+            CpuClockMode::C621 =>
+                self.cpu_6x4x() / 3,
+        }
+    }
+
+    pub fn cpu_1x(&self) -> u32 {
+        match CpuClockMode::get() {
+            CpuClockMode::C421 =>
+                self.cpu_6x4x() / 4,
+            CpuClockMode::C621 =>
+                self.cpu_6x4x() / 6,
+        }
+    }
+
+    pub fn uart_ref_clk(&self) -> u32 {
+        let regs = slcr::RegisterBlock::new();
+        let uart_clk_ctrl = regs.uart_clk_ctrl.read();
+        let pll = match uart_clk_ctrl.srcsel() {
+            slcr::PllSource::ArmPll =>
+                self.arm,
+            slcr::PllSource::DdrPll =>
+                self.ddr,
+            slcr::PllSource::IoPll =>
+                self.io,
+        };
+        pll / u32::from(uart_clk_ctrl.divisor())
+    }
+}

libboard_zynq/src/clocks/source.rs

@@ -0,0 +1,156 @@
+use libregister::{RegisterR, RegisterW, RegisterRW};
+use super::slcr;
+
+#[cfg(feature = "target_zc706")]
+pub const PS_CLK: u32 = 33_333_333;
+#[cfg(feature = "target_cora_z7_10")]
+pub const PS_CLK: u32 = 50_000_000;
+
+/// (pll_fdiv_max, (pll_cp, pll_res, lock_cnt))
+const PLL_FDIV_LOCK_PARAM: &[(u16, (u8, u8, u16))] = &[
+    (13, (2, 6, 750)),
+    (14, (2, 6, 700)),
+    (15, (2, 6, 650)),
+    (16, (2, 10, 625)),
+    (17, (2, 10, 575)),
+    (18, (2, 10, 550)),
+    (19, (2, 10, 525)),
+    (20, (2, 12, 500)),
+    (21, (2, 12, 475)),
+    (22, (2, 12, 450)),
+    (23, (2, 12, 425)),
+    (25, (2, 12, 400)),
+    (26, (2, 12, 375)),
+    (28, (2, 12, 350)),
+    (30, (2, 12, 325)),
+    (33, (2, 2, 300)),
+    (36, (2, 2, 275)),
+    (40, (2, 2, 250)),
+    (47, (3, 12, 250)),
+    (66, (2, 4, 250)),
+];
+
+pub trait ClockSource {
+    /// picks this ClockSource's registers from the SLCR block
+    fn pll_regs(slcr: &mut crate::slcr::RegisterBlock)
+                -> (&mut crate::slcr::PllCtrl,
+                    &mut crate::slcr::PllCfg,
+                    &mut crate::slcr::PllStatus
+                   );
+
+    /// query PLL lock status
+    fn pll_locked(pll_status: &mut crate::slcr::PllStatus) -> bool;
+
+    /// get configured frequency
+    fn freq() -> u32 {
+        let mut slcr = slcr::RegisterBlock::new();
+        let (pll_ctrl, _, _) = Self::pll_regs(&mut slcr);
+        u32::from(pll_ctrl.read().pll_fdiv()) * PS_CLK
+    }
+
+    /// Zynq-7000 AP SoC Technical Reference Manual:
+    /// 25.10.4 PLLs
+    fn setup(target_freq: u32) {
+        let fdiv = (target_freq / PS_CLK).min(66) as u16;
+        let (pll_res, pll_cp, lock_cnt) = PLL_FDIV_LOCK_PARAM.iter()
+            .filter(|(fdiv_max, _)| fdiv <= *fdiv_max)
+            .nth(0)
+            .expect("PLL_FDIV_LOCK_PARAM")
+            .1.clone();
+
+        slcr::RegisterBlock::unlocked(|slcr| {
+            let (pll_ctrl, pll_cfg, pll_status) = Self::pll_regs(slcr);
+
+            // Bypass
+            pll_ctrl.modify(|_, w| w
+                            .pll_pwrdwn(false)
+                            .pll_bypass_force(true)
+                            .pll_fdiv(fdiv)
+            );
+            // Configure
+            pll_cfg.write(
+                slcr::PllCfg::zeroed()
+                    .pll_res(pll_res)
+                    .pll_cp(pll_cp)
+                    .lock_cnt(lock_cnt)
+            );
+            // Reset
+            pll_ctrl.modify(|_, w| w.pll_reset(true));
+            pll_ctrl.modify(|_, w| w.pll_reset(false));
+            // Wait for PLL lock
+            while ! Self::pll_locked(pll_status) {}
+            // Remove bypass
+            pll_ctrl.modify(|_, w| w
+                            .pll_bypass_force(false)
+                            .pll_bypass_qual(false)
+            );
+        });
+    }
+}
+
+/// ARM PLL: Recommended clock source for the CPUs and the interconnect
+pub struct ArmPll;
+
+impl ClockSource for ArmPll {
+    #[inline]
+    fn pll_regs(slcr: &mut crate::slcr::RegisterBlock)
+                -> (&mut crate::slcr::PllCtrl,
+                    &mut crate::slcr::PllCfg,
+                    &mut crate::slcr::PllStatus
+    ) {
+        (&mut slcr.arm_pll_ctrl,
+         &mut slcr.arm_pll_cfg,
+         &mut slcr.pll_status
+        )
+    }
+
+    #[inline]
+    fn pll_locked(pll_status: &mut crate::slcr::PllStatus) -> bool {
+        pll_status.read().arm_pll_lock()
+    }
+}
+
+/// DDR PLL: Recommended clock for the DDR DRAM controller and AXI_HP interfaces
+pub struct DdrPll;
+
+impl ClockSource for DdrPll {
+    #[inline]
+    fn pll_regs(slcr: &mut crate::slcr::RegisterBlock)
+                -> (&mut crate::slcr::PllCtrl,
+                    &mut crate::slcr::PllCfg,
+                    &mut crate::slcr::PllStatus
+    ) {
+        (&mut slcr.ddr_pll_ctrl,
+         &mut slcr.ddr_pll_cfg,
+         &mut slcr.pll_status
+        )
+    }
+
+    #[inline]
+    fn pll_locked(pll_status: &mut crate::slcr::PllStatus) -> bool {
+        pll_status.read().ddr_pll_lock()
+    }
+}
+
+/// I/O PLL: Recommended clock for I/O peripherals
+pub struct IoPll;
+
+
+impl ClockSource for IoPll {
+    #[inline]
+    fn pll_regs(slcr: &mut crate::slcr::RegisterBlock)
+                -> (&mut crate::slcr::PllCtrl,
+                    &mut crate::slcr::PllCfg,
+                    &mut crate::slcr::PllStatus
+    ) {
+        (&mut slcr.io_pll_ctrl,
+         &mut slcr.io_pll_cfg,
+         &mut slcr.pll_status
+        )
+    }
+
+    #[inline]
+    fn pll_locked(pll_status: &mut crate::slcr::PllStatus) -> bool {
+        pll_status.read().io_pll_lock()
+    }
+}

libboard_zynq/src/ddr/mod.rs

@@ -1,7 +1,7 @@
 use libregister::{RegisterR, RegisterW, RegisterRW};
 use crate::{print, println};
 use super::slcr;
-use super::clocks::CpuClocks;
+use super::clocks::{Clocks, source::{DdrPll, ClockSource}};
 
 mod regs;
 
@@ -11,7 +11,7 @@ const DDR_FREQ: u32 = 666_666_666;
 
 #[cfg(feature = "target_cora_z7_10")]
 /// Micron MT41K256M16HA-125: 800 MHz DDR3L, max supported 533 MHz
-const DDR_FREQ: u32 = 533_333_333;
+const DDR_FREQ: u32 = 525_000_000;
 
 /// MT41K256M16HA-125
 const DCI_FREQ: u32 = 10_000_000;
@@ -34,16 +34,14 @@ impl DdrRam {
 
     /// Zynq-7000 AP SoC Technical Reference Manual:
     /// 10.6.1 DDR Clock Initialization
-    fn clock_setup() -> CpuClocks {
+    fn clock_setup() -> Clocks {
-        let clocks = CpuClocks::get();
+        DdrPll::setup(2 * DDR_FREQ);
-        if clocks.ddr == 0 {
+
-            CpuClocks::enable_ddr(clocks.arm);
+        let clocks = Clocks::get();
-        }
-        let clocks = CpuClocks::get();
         println!("Clocks: {:?}", clocks);
 
-        let ddr3x_clk_divisor = ((DDR_FREQ - 1 + clocks.ddr) / DDR_FREQ).min(255) as u8;
+        let ddr3x_clk_divisor = 2;
-        let ddr2x_clk_divisor = 3 * ddr3x_clk_divisor / 2;
+        let ddr2x_clk_divisor = 3;
         println!("DDR 3x/2x clocks: {}/{}", clocks.ddr / u32::from(ddr3x_clk_divisor), clocks.ddr / u32::from(ddr2x_clk_divisor));
 
         slcr::RegisterBlock::unlocked(|slcr| {
@@ -60,7 +58,7 @@ impl DdrRam {
 
     /// Zynq-7000 AP SoC Technical Reference Manual:
     /// 10.6.2 DDR IOB Impedance Calibration
-    fn calibrate_iob_impedance(clocks: &CpuClocks) {
+    fn calibrate_iob_impedance(clocks: &Clocks) {
         let divisor0 = ((DCI_FREQ - 1 + clocks.ddr) / DCI_FREQ)
             .max(1).min(63) as u8;
         let divisor1 = (clocks.ddr / DCI_FREQ / u32::from(divisor0))

libboard_zynq/src/eth/mod.rs

@@ -1,7 +1,7 @@
 use libregister::*;
 use crate::println;
 use super::slcr;
-use super::clocks::CpuClocks;
+use super::clocks::Clocks;
 
 pub mod phy;
 use phy::{Phy, PhyAccess};
@@ -194,7 +194,7 @@ impl<'r> Eth<'r, (), ()> {
 
 impl<'r, RX, TX> Eth<'r, RX, TX> {
     pub fn setup_gem0_clock(tx_clock: u32) {
-        let io_pll = CpuClocks::get().io;
+        let io_pll = Clocks::get().io;
         let d0 = ((tx_clock - 1 + io_pll) / tx_clock).max(1).min(63);
         let d1 = (io_pll / tx_clock / d0).max(1).min(63);
 
@@ -218,7 +218,7 @@ impl<'r, RX, TX> Eth<'r, RX, TX> {
     }
 
     pub fn setup_gem1_clock(tx_clock: u32) {
-        let io_pll = CpuClocks::get().io;
+        let io_pll = Clocks::get().io;
         let d0 = ((tx_clock - 1 + io_pll) / tx_clock).max(1).min(63);
         let d1 = (io_pll / tx_clock / d0).max(1).min(63);
 
@@ -452,7 +452,7 @@ impl<'r> EthInner<'r> {
     }
 
     fn configure(&mut self, macaddr: [u8; 6]) {
-        let clocks = CpuClocks::get();
+        let clocks = Clocks::get();
         let mdc_clk_div = (clocks.cpu_1x() / MAX_MDC) + 1;
 
         self.regs.net_cfg.write(

libboard_zynq/src/flash/mod.rs

@@ -4,7 +4,7 @@ use crate::{print, println};
 use core::marker::PhantomData;
 use libregister::{RegisterR, RegisterW, RegisterRW};
 use super::slcr;
-use super::clocks::CpuClocks;
+use super::clocks::source::{IoPll, ClockSource};
 
 mod regs;
 mod bytes;
@@ -137,8 +137,9 @@ impl Flash<()> {
         flash
     }
 
+    /// typical: `200_000_000` Hz
     fn enable_clocks(clock: u32) {
-        let io_pll = CpuClocks::get().io;
+        let io_pll = IoPll::freq();
         let divisor = ((clock - 1 + io_pll) / clock)
             .max(1).min(63) as u8;
 

libboard_zynq/src/uart/mod.rs

@@ -2,7 +2,7 @@ use core::fmt;
 
 use libregister::*;
 use super::slcr;
-use super::clocks::CpuClocks;
+use super::clocks::Clocks;
 
 mod regs;
 mod baud_rate_gen;
@@ -110,7 +110,7 @@ impl Uart {
         self.disable_rx();
         self.disable_tx();
 
-        let clocks = CpuClocks::get();
+        let clocks = Clocks::get();
         baud_rate_gen::configure(self.regs, clocks.uart_ref_clk(), baudrate);
 
         // Enable controller