From fc39885d3b961d7cf91f703169825d98e16477d9 Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Mon, 28 Oct 2019 00:43:09 +0100
Subject: [PATCH] zynq::ddr: fix clock setup

---
 src/main.rs         | 10 ----------
 src/zynq/ddr/mod.rs | 18 ++++++++++--------
 2 files changed, 10 insertions(+), 18 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index 29914a2c..27d351fd 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -87,16 +87,6 @@ const HWADDR: [u8; 6] = [0, 0x23, 0xde, 0xea, 0xbe, 0xef];
 fn main() {
     println!("Main.");
 
-    zynq::clocks::CpuClocks::enable_ddr(1_066_666_666);
-    let pll_status = zynq::slcr::RegisterBlock::new().pll_status.read();
-    println!("PLLs: {}", pll_status);
-    let clocks = zynq::clocks::CpuClocks::get();
-    println!("Clocks: {:?}", clocks);
-    println!("CPU speeds: {}/{}/{}/{} MHz",
-             clocks.cpu_6x4x() / 1_000_000,
-             clocks.cpu_3x2x() / 1_000_000,
-             clocks.cpu_2x() / 1_000_000,
-             clocks.cpu_1x() / 1_000_000);
     let mut ddr = zynq::ddr::DdrRam::new();
     println!("DDR: {:?}", ddr.status());
     ddr.memtest();
diff --git a/src/zynq/ddr/mod.rs b/src/zynq/ddr/mod.rs
index fc5e117e..e9504dfe 100644
--- a/src/zynq/ddr/mod.rs
+++ b/src/zynq/ddr/mod.rs
@@ -10,8 +10,8 @@ mod regs;
 const DDR_FREQ: u32 = 666_666_666;
 
 #[cfg(feature = "target_cora_z7_10")]
-/// Micron MT41K256M16HA-125: 800 MHz DDR3L
-const DDR_FREQ: u32 = 800_000_000;
+/// Micron MT41K256M16HA-125: 800 MHz DDR3L, max supported 533 MHz
+const DDR_FREQ: u32 = 533_333_333;
 
 /// MT41K256M16HA-125
 const DCI_FREQ: u32 = 10_000_000;
@@ -22,8 +22,7 @@ pub struct DdrRam {
 
 impl DdrRam {
     pub fn new() -> Self {
-        let clocks = CpuClocks::get();
-        Self::clock_setup(&clocks);
+        let clocks = Self::clock_setup();
         Self::calibrate_iob_impedance(&clocks);
         Self::configure_iob();
 
@@ -35,8 +34,10 @@ 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_666_666);
+    fn clock_setup() -> CpuClocks {
+        let clocks = CpuClocks::get();
+        CpuClocks::enable_ddr(clocks.cpu);
+        let clocks = CpuClocks::get();
 
         let ddr3x_clk_divisor = ((clocks.ddr - 1) / DDR_FREQ + 1).min(255) as u8;
         let ddr2x_clk_divisor = 3 * ddr3x_clk_divisor / 2;
@@ -50,6 +51,7 @@ impl DdrRam {
                     .ddr_3xclk_divisor(ddr3x_clk_divisor)
             );
         });
+        clocks
     }
 
     /// Zynq-7000 AP SoC Technical Reference Manual:
@@ -57,7 +59,7 @@ impl DdrRam {
     fn calibrate_iob_impedance(clocks: &CpuClocks) {
         let divisor0 = (clocks.ddr / DCI_FREQ)
             .max(1).min(63) as u8;
-        let divisor1 = (clocks.ddr / DCI_FREQ / u32::from(divisor0))
+        let divisor1 = 1 + (clocks.ddr / DCI_FREQ / u32::from(divisor0))
             .max(1).min(63) as u8;
 
         slcr::RegisterBlock::unlocked(|slcr| {
@@ -76,7 +78,7 @@ impl DdrRam {
             slcr.ddriob_dci_ctrl.modify(|_, w|
                 w.reset(true)
             );
-            // Step 3.b. for DDR3
+            // Step 3.b. for DDR3/DDR3L
             slcr.ddriob_dci_ctrl.modify(|_, w|
                 w.nref_opt1(0)
                  .nref_opt2(0)