From 1a4c13cf262869d88a38575eb14abd09900e8e01 Mon Sep 17 00:00:00 2001
From: Dario Nieuwenhuis <dirbaio@dirbaio.net>
Date: Mon, 12 Jul 2021 02:45:59 +0200
Subject: [PATCH] rp/spi: add set_frequency

---
 embassy-rp/src/spi.rs | 75 +++++++++++++++++++++++--------------------
 1 file changed, 41 insertions(+), 34 deletions(-)

diff --git a/embassy-rp/src/spi.rs b/embassy-rp/src/spi.rs
index 6c3aabfb..ec2437e9 100644
--- a/embassy-rp/src/spi.rs
+++ b/embassy-rp/src/spi.rs
@@ -33,6 +33,33 @@ pub struct Spi<'d, T: Instance> {
     phantom: PhantomData<&'d mut T>,
 }
 
+fn div_roundup(a: u32, b: u32) -> u32 {
+    (a + b - 1) / b
+}
+
+fn calc_prescs(freq: u32) -> (u8, u8) {
+    let clk_peri = crate::clocks::clk_peri_freq();
+
+    // final SPI frequency: spi_freq = clk_peri / presc / postdiv
+    // presc must be in 2..=254, and must be even
+    // postdiv must be in 1..=256
+
+    // divide extra by 2, so we get rid of the "presc must be even" requirement
+    let ratio = div_roundup(clk_peri, freq * 2);
+    if ratio > 127 * 256 {
+        panic!("Requested too low SPI frequency");
+    }
+
+    let presc = div_roundup(ratio, 256);
+    let postdiv = if presc == 1 {
+        ratio
+    } else {
+        div_roundup(ratio, presc)
+    };
+
+    ((presc * 2) as u8, (postdiv - 1) as u8)
+}
+
 impl<'d, T: Instance> Spi<'d, T> {
     pub fn new(
         inner: impl Unborrow<Target = T>,
@@ -46,45 +73,14 @@ impl<'d, T: Instance> Spi<'d, T> {
 
         unsafe {
             let p = inner.regs();
+            let (presc, postdiv) = calc_prescs(config.frequency);
 
-            let clk_peri = crate::clocks::clk_peri_freq();
-            assert!(config.frequency <= clk_peri);
-
-            // final SPI frequency: spi_freq = clk_peri / presc / postdiv
-            // presc must be in 2..=254, and must be even
-            // postdiv must be in 1..=256
-
-            fn div_roundup(a: u32, b: u32) -> u32 {
-                (a + b - 1) / b
-            }
-
-            // divide extra by 2, so we get rid of the "presc must be even" requirement
-            let ratio = div_roundup(clk_peri, config.frequency * 2);
-            if ratio > 127 * 256 {
-                panic!("Requested too low SPI frequency");
-            }
-
-            let presc = div_roundup(ratio, 256);
-            let postdiv = if presc == 1 {
-                ratio
-            } else {
-                div_roundup(ratio, presc)
-            };
-
-            debug!(
-                "SPI: requested freq {=u32}, actual freq {=u32}, presc {=u32}, postdiv {=u32}",
-                config.frequency,
-                clk_peri / (presc * 2 * postdiv),
-                presc * 2,
-                postdiv
-            );
-
-            p.cpsr().write(|w| w.set_cpsdvsr((presc * 2) as _));
+            p.cpsr().write(|w| w.set_cpsdvsr(presc));
             p.cr0().write(|w| {
                 w.set_dss(0b0111); // 8bit
                 w.set_spo(config.polarity == ehnb::Polarity::IdleHigh);
                 w.set_sph(config.phase == ehnb::Phase::CaptureOnSecondTransition);
-                w.set_scr((postdiv - 1) as u8);
+                w.set_scr(postdiv);
             });
             p.cr1().write(|w| {
                 w.set_sse(true); // enable
@@ -139,6 +135,17 @@ impl<'d, T: Instance> Spi<'d, T> {
             while p.sr().read().bsy() {}
         }
     }
+
+    pub fn set_frequency(&mut self, freq: u32) {
+        let (presc, postdiv) = calc_prescs(freq);
+        let p = self.inner.regs();
+        unsafe {
+            p.cpsr().write(|w| w.set_cpsdvsr(presc));
+            p.cr0().modify(|w| {
+                w.set_scr(postdiv);
+            });
+        }
+    }
 }
 
 impl<'d, T: Instance> eh::Write<u8> for Spi<'d, T> {