From 880b71a1e8246011109d64794ec98477722bde0e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Henrik=20Als=C3=A9r?= <henrik@mindbite.se>
Date: Sat, 9 Jul 2022 02:28:05 +0200
Subject: [PATCH] impl SetConfig for stm32 i2c and SPI

---
 embassy-stm32/Cargo.toml     |  1 +
 embassy-stm32/src/i2c/v1.rs  | 21 +++++++++++++++++++++
 embassy-stm32/src/i2c/v2.rs  | 17 +++++++++++++++++
 embassy-stm32/src/spi/mod.rs |  8 ++++++++
 4 files changed, 47 insertions(+)

diff --git a/embassy-stm32/Cargo.toml b/embassy-stm32/Cargo.toml
index a21384be..547aad02 100644
--- a/embassy-stm32/Cargo.toml
+++ b/embassy-stm32/Cargo.toml
@@ -35,6 +35,7 @@ embassy = { version = "0.1.0", path = "../embassy" }
 embassy-cortex-m = { version = "0.1.0", path = "../embassy-cortex-m", features = ["prio-bits-4"]}
 embassy-macros = { version = "0.1.0", path = "../embassy-macros", features = ["stm32"] }
 embassy-hal-common = {version = "0.1.0", path = "../embassy-hal-common" }
+embassy-embedded-hal = {version = "0.1.0", path = "../embassy-embedded-hal" }
 embassy-net = { version = "0.1.0", path = "../embassy-net", optional = true }
 embassy-usb = {version = "0.1.0", path = "../embassy-usb", optional = true }
 
diff --git a/embassy-stm32/src/i2c/v1.rs b/embassy-stm32/src/i2c/v1.rs
index 87a4f969..4bff90af 100644
--- a/embassy-stm32/src/i2c/v1.rs
+++ b/embassy-stm32/src/i2c/v1.rs
@@ -1,5 +1,6 @@
 use core::marker::PhantomData;
 
+use embassy_embedded_hal::SetConfig;
 use embassy_hal_common::unborrow;
 
 use crate::gpio::sealed::AFType;
@@ -381,3 +382,23 @@ impl Timings {
         }
     }
 }
+
+impl<'d, T: Instance> SetConfig for I2c<'d, T> {
+    type Config = Hertz;
+    fn set_config(&mut self, config: &Self::Config) {
+        let timings = Timings::new(T::frequency(), *config);
+        unsafe {
+            T::regs().cr2().modify(|reg| {
+                reg.set_freq(timings.freq);
+            });
+            T::regs().ccr().modify(|reg| {
+                reg.set_f_s(timings.mode.f_s());
+                reg.set_duty(timings.duty.duty());
+                reg.set_ccr(timings.ccr);
+            });
+            T::regs().trise().modify(|reg| {
+                reg.set_trise(timings.trise);
+            });
+        }
+    }
+}
diff --git a/embassy-stm32/src/i2c/v2.rs b/embassy-stm32/src/i2c/v2.rs
index 1a085e78..8778da91 100644
--- a/embassy-stm32/src/i2c/v2.rs
+++ b/embassy-stm32/src/i2c/v2.rs
@@ -4,6 +4,7 @@ use core::task::Poll;
 
 use atomic_polyfill::{AtomicUsize, Ordering};
 use embassy::waitqueue::AtomicWaker;
+use embassy_embedded_hal::SetConfig;
 use embassy_hal_common::drop::OnDrop;
 use embassy_hal_common::unborrow;
 use futures::future::poll_fn;
@@ -899,3 +900,19 @@ cfg_if::cfg_if! {
         }
     }
 }
+
+impl<'d, T: Instance> SetConfig for I2c<'d, T> {
+    type Config = Hertz;
+    fn set_config(&mut self, config: &Self::Config) {
+        let timings = Timings::new(T::frequency(), *config);
+        unsafe {
+            T::regs().timingr().write(|reg| {
+                reg.set_presc(timings.prescale);
+                reg.set_scll(timings.scll);
+                reg.set_sclh(timings.sclh);
+                reg.set_sdadel(timings.sdadel);
+                reg.set_scldel(timings.scldel);
+            });
+        }
+    }
+}
diff --git a/embassy-stm32/src/spi/mod.rs b/embassy-stm32/src/spi/mod.rs
index 7c142e50..a839bb41 100644
--- a/embassy-stm32/src/spi/mod.rs
+++ b/embassy-stm32/src/spi/mod.rs
@@ -3,6 +3,7 @@
 use core::marker::PhantomData;
 use core::ptr;
 
+use embassy_embedded_hal::SetConfig;
 use embassy_hal_common::unborrow;
 pub use embedded_hal_02::spi::{Mode, Phase, Polarity, MODE_0, MODE_1, MODE_2, MODE_3};
 use futures::future::join;
@@ -1022,3 +1023,10 @@ foreach_peripheral!(
         impl Instance for peripherals::$inst {}
     };
 );
+
+impl<'d, T: Instance, Tx, Rx> SetConfig for Spi<'d, T, Tx, Rx> {
+    type Config = Config;
+    fn set_config(&mut self, config: &Self::Config) {
+        self.reconfigure(*config);
+    }
+}