From 96362b33e883781c6b11ac9c2c33fcae27c933ad Mon Sep 17 00:00:00 2001
From: dstric-aqueduct <96876452+dstric-aqueduct@users.noreply.github.com>
Date: Tue, 15 Oct 2024 11:48:54 -0400
Subject: [PATCH] Add `set_config` method to RP SPI driver

Add a `set_config` method to `Spi` to allow reconfiguring SPI mode after creation.

The existing implementation of the `embassy-embedded-hal` trait `SetConfig` is changed to use the new method.

Existing uses of `SetConfig`  trait may need to explicitly call the trait method to maintain current return type.
---
 embassy-rp/src/spi.rs              | 52 +++++++++++++++++++-----------
 examples/rp23/src/bin/spi_sdmmc.rs |  2 +-
 2 files changed, 35 insertions(+), 19 deletions(-)

diff --git a/embassy-rp/src/spi.rs b/embassy-rp/src/spi.rs
index b89df74a2..8271082f9 100644
--- a/embassy-rp/src/spi.rs
+++ b/embassy-rp/src/spi.rs
@@ -84,16 +84,9 @@ impl<'d, T: Instance, M: Mode> Spi<'d, T, M> {
     ) -> Self {
         into_ref!(inner);
 
-        let p = inner.regs();
-        let (presc, postdiv) = calc_prescs(config.frequency);
+        Self::apply_config(&inner, &config);
 
-        p.cpsr().write(|w| w.set_cpsdvsr(presc));
-        p.cr0().write(|w| {
-            w.set_dss(0b0111); // 8bit
-            w.set_spo(config.polarity == Polarity::IdleHigh);
-            w.set_sph(config.phase == Phase::CaptureOnSecondTransition);
-            w.set_scr(postdiv);
-        });
+        let p = inner.regs();
 
         // Always enable DREQ signals -- harmless if DMA is not listening
         p.dmacr().write(|reg| {
@@ -164,6 +157,23 @@ impl<'d, T: Instance, M: Mode> Spi<'d, T, M> {
         }
     }
 
+    /// Private function to apply SPI configuration (phase, polarity, frequency) settings.
+    ///
+    /// Driver should be disabled before making changes and reenabled after the modifications
+    /// are applied.
+    fn apply_config(inner: &PeripheralRef<'d, T>, config: &Config) {
+        let p = inner.regs();
+        let (presc, postdiv) = calc_prescs(config.frequency);
+
+        p.cpsr().write(|w| w.set_cpsdvsr(presc));
+        p.cr0().write(|w| {
+            w.set_dss(0b0111); // 8bit
+            w.set_spo(config.polarity == Polarity::IdleHigh);
+            w.set_sph(config.phase == Phase::CaptureOnSecondTransition);
+            w.set_scr(postdiv);
+        });
+    }
+
     /// Write data to SPI blocking execution until done.
     pub fn blocking_write(&mut self, data: &[u8]) -> Result<(), Error> {
         let p = self.inner.regs();
@@ -244,6 +254,20 @@ impl<'d, T: Instance, M: Mode> Spi<'d, T, M> {
         // enable
         p.cr1().write(|w| w.set_sse(true));
     }
+
+    /// Set SPI config.
+    pub fn set_config(&mut self, config: &Config) {
+        let p = self.inner.regs();
+
+        // disable
+        p.cr1().write(|w| w.set_sse(false));
+
+        // change stuff
+        Self::apply_config(&self.inner, config);
+
+        // enable
+        p.cr1().write(|w| w.set_sse(true));
+    }
 }
 
 impl<'d, T: Instance> Spi<'d, T, Blocking> {
@@ -696,15 +720,7 @@ impl<'d, T: Instance, M: Mode> SetConfig for Spi<'d, T, M> {
     type Config = Config;
     type ConfigError = ();
     fn set_config(&mut self, config: &Self::Config) -> Result<(), ()> {
-        let p = self.inner.regs();
-        let (presc, postdiv) = calc_prescs(config.frequency);
-        p.cpsr().write(|w| w.set_cpsdvsr(presc));
-        p.cr0().write(|w| {
-            w.set_dss(0b0111); // 8bit
-            w.set_spo(config.polarity == Polarity::IdleHigh);
-            w.set_sph(config.phase == Phase::CaptureOnSecondTransition);
-            w.set_scr(postdiv);
-        });
+        self.set_config(config);
 
         Ok(())
     }
diff --git a/examples/rp23/src/bin/spi_sdmmc.rs b/examples/rp23/src/bin/spi_sdmmc.rs
index aa6b44ffa..cfc38dfd9 100644
--- a/examples/rp23/src/bin/spi_sdmmc.rs
+++ b/examples/rp23/src/bin/spi_sdmmc.rs
@@ -56,7 +56,7 @@ async fn main(_spawner: Spawner) {
     // Now that the card is initialized, the SPI clock can go faster
     let mut config = spi::Config::default();
     config.frequency = 16_000_000;
-    sdcard.spi(|dev| dev.bus_mut().set_config(&config)).ok();
+    sdcard.spi(|dev| SetConfig::set_config(dev.bus_mut(), &config)).ok();
 
     // Now let's look for volumes (also known as partitions) on our block device.
     // To do this we need a Volume Manager. It will take ownership of the block device.