2 files changed, 35 insertions, 19 deletions

diff --git a/embassy-rp/src/spi.rs b/embassy-rp/src/spi.rs
index a8f4e72c7..c48b5c54f 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> {
@@ -697,15 +721,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.