From a4d3360a70869e5dc985d6a0255d1c8e075e1ca2 Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Fri, 25 Oct 2019 20:38:10 +0200
Subject: [PATCH] zynq::slcr: implement Display for PllStatus

---
 src/zynq/clocks.rs  | 22 +++++++++++++++++++++-
 src/zynq/ddr/mod.rs |  5 +++++
 src/zynq/slcr.rs    | 23 ++++++++++++++++++++++-
 3 files changed, 48 insertions(+), 2 deletions(-)

diff --git a/src/zynq/clocks.rs b/src/zynq/clocks.rs
index 48da26ae..daa041f1 100644
--- a/src/zynq/clocks.rs
+++ b/src/zynq/clocks.rs
@@ -1,4 +1,4 @@
-use crate::regs::RegisterR;
+use crate::regs::{RegisterR, RegisterRW};
 use super::slcr;
 
 #[cfg(feature = "target_zc706")]
@@ -89,4 +89,24 @@ impl CpuClocks {
         };
         pll / u32::from(uart_clk_ctrl.divisor())
     }
+
+    pub fn enable_ddr(target_clock: u32) {
+        let regs = slcr::RegisterBlock::new();
+        regs.ddr_pll_ctrl.modify(|_, w| w
+            .pll_pwrdwn(false)
+            .pll_reset(true)
+            .pll_bypass_force(true)
+        );
+        let fdiv = (target_clock / PS_CLK).max(127) as u16;
+        regs.ddr_pll_ctrl.modify(|_, w| w
+            .pll_pwrdwn(false)
+            .pll_reset(false)
+            .pll_fdiv(fdiv)
+        );
+        while ! regs.pll_status.read().ddr_pll_lock() {}
+        regs.ddr_pll_ctrl.modify(|_, w| w
+            .pll_bypass_force(false)
+            .pll_bypass_qual(false)
+        );
+    }
 }
diff --git a/src/zynq/ddr/mod.rs b/src/zynq/ddr/mod.rs
index 57237fae..fbbe1db3 100644
--- a/src/zynq/ddr/mod.rs
+++ b/src/zynq/ddr/mod.rs
@@ -28,6 +28,8 @@ impl DdrRam {
     /// Zynq-7000 AP SoC Technical Reference Manual:
     /// 10.6.1 DDR Clock Initialization
     fn clock_setup(clocks: &CpuClocks) {
+        CpuClocks::enable_ddr(1_066_000_000);
+
         let ddr3x_clk_divisor = ((clocks.ddr - 1) / DDR_FREQ + 1).min(255) as u8;
         let ddr2x_clk_divisor = 3 * ddr3x_clk_divisor / 2;
 
@@ -99,6 +101,7 @@ impl DdrRam {
                 .output_en(slcr::DdriobOutputEn::Obuf);
             slcr.ddriob_addr0.write(addr_config.clone());
             slcr.ddriob_addr1.write(addr_config);
+
             let data_config = slcr::DdriobConfig::zeroed()
                 .inp_type(slcr::DdriobInputType::VrefDifferential)
                 .term_en(true)
@@ -106,6 +109,7 @@ impl DdrRam {
                 .output_en(slcr::DdriobOutputEn::Obuf);
             slcr.ddriob_data0.write(data_config.clone());
             slcr.ddriob_data1.write(data_config);
+
             let diff_config = slcr::DdriobConfig::zeroed()
                 .inp_type(slcr::DdriobInputType::Differential)
                 .term_en(true)
@@ -113,6 +117,7 @@ impl DdrRam {
                 .output_en(slcr::DdriobOutputEn::Obuf);
             slcr.ddriob_diff0.write(diff_config.clone());
             slcr.ddriob_diff1.write(diff_config);
+
             slcr.ddriob_clock.write(
                 slcr::DdriobConfig::zeroed()
                     .output_en(slcr::DdriobOutputEn::Obuf)
diff --git a/src/zynq/slcr.rs b/src/zynq/slcr.rs
index edbe6704..2579df37 100644
--- a/src/zynq/slcr.rs
+++ b/src/zynq/slcr.rs
@@ -68,7 +68,7 @@ pub struct RegisterBlock {
     pub arm_pll_ctrl: PllCtrl,
     pub ddr_pll_ctrl: PllCtrl,
     pub io_pll_ctrl: PllCtrl,
-    pub pll_status: RO<u32>,
+    pub pll_status: PllStatus,
     pub arm_pll_cfg: PllCfg,
     pub ddr_pll_cfg: PllCfg,
     pub io_pll_cfg: PllCfg,
@@ -293,6 +293,27 @@ register_bit!(pll_ctrl, pll_bypass_qual, 3);
 register_bit!(pll_ctrl, pll_pwrdwn, 1);
 register_bit!(pll_ctrl, pll_reset, 0);
 
+register!(pll_status, PllStatus, RO, u32);
+register_bit!(pll_status, arm_pll_lock, 0);
+register_bit!(pll_status, ddr_pll_lock, 1);
+register_bit!(pll_status, io_pll_lock, 2);
+register_bit!(pll_status, arm_pll_stable, 3);
+register_bit!(pll_status, ddr_pll_stable, 4);
+register_bit!(pll_status, io_pll_stable, 5);
+
+impl core::fmt::Display for pll_status::Read {
+    fn fmt(&self, fmt: &mut core::fmt::Formatter) -> Result<(), core::fmt::Error> {
+        write!(fmt, "ARM: {}/{} DDR: {}/{} IO: {}/{}",
+               if self.arm_pll_lock() { "locked" } else { "NOT locked" },
+               if self.arm_pll_stable() { "stable" } else { "UNSTABLE" },
+               if self.ddr_pll_lock() { "locked" } else { "NOT locked" },
+               if self.ddr_pll_stable() { "stable" } else { "UNSTABLE" },
+               if self.io_pll_lock() { "locked" } else { "NOT locked" },
+               if self.io_pll_stable() { "stable" } else { "UNSTABLE" },
+        )
+    }
+}
+
 register!(pll_cfg, PllCfg, RW, u32);
 register_bits!(pll_cfg, pll_res, u8, 4, 7);
 register_bits!(pll_cfg, pll_cp, u8, 8, 11);