From afd96bd887b5ca3972b9713f9d72a02ddd5a34dc Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Wed, 6 Nov 2019 23:21:22 +0100
Subject: [PATCH] zynq::clocks: unlock slcr in enable_io()

---
 src/zynq/clocks.rs | 24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/src/zynq/clocks.rs b/src/zynq/clocks.rs
index 45fda27..42cc895 100644
--- a/src/zynq/clocks.rs
+++ b/src/zynq/clocks.rs
@@ -90,6 +90,30 @@ impl CpuClocks {
         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) {