From 137e47f98db224815231460766386d9a88b3d88a Mon Sep 17 00:00:00 2001
From: Dion Dokter <diondokter@gmail.com>
Date: Mon, 2 Oct 2023 21:14:44 +0200
Subject: [PATCH] Do affect the frequency

---
 embassy-stm32/src/timer/complementary_pwm.rs |  9 ++++-
 embassy-stm32/src/timer/mod.rs               | 41 ++++++++++++++++++--
 embassy-stm32/src/timer/simple_pwm.rs        |  9 ++++-
 3 files changed, 52 insertions(+), 7 deletions(-)

diff --git a/embassy-stm32/src/timer/complementary_pwm.rs b/embassy-stm32/src/timer/complementary_pwm.rs
index bfe5137e..72ff84b6 100644
--- a/embassy-stm32/src/timer/complementary_pwm.rs
+++ b/embassy-stm32/src/timer/complementary_pwm.rs
@@ -70,8 +70,8 @@ impl<'d, T: ComplementaryCaptureCompare16bitInstance> ComplementaryPwm<'d, T> {
 
         let mut this = Self { inner: tim };
 
-        this.inner.set_frequency(freq);
         this.inner.set_counting_mode(counting_mode);
+        this.set_freq(freq);
         this.inner.start();
 
         this.inner.enable_outputs(true);
@@ -98,7 +98,12 @@ impl<'d, T: ComplementaryCaptureCompare16bitInstance> ComplementaryPwm<'d, T> {
     }
 
     pub fn set_freq(&mut self, freq: Hertz) {
-        self.inner.set_frequency(freq);
+        let multiplier = if self.inner.get_counting_mode().is_center_aligned() {
+            2u8
+        } else {
+            1u8
+        };
+        self.inner.set_frequency(freq * multiplier);
     }
 
     pub fn get_max_duty(&self) -> u16 {
diff --git a/embassy-stm32/src/timer/mod.rs b/embassy-stm32/src/timer/mod.rs
index 52d1dc11..b8e63288 100644
--- a/embassy-stm32/src/timer/mod.rs
+++ b/embassy-stm32/src/timer/mod.rs
@@ -76,7 +76,7 @@ pub(crate) mod sealed {
         fn regs_gp16() -> crate::pac::timer::TimGp16;
 
         fn set_counting_mode(&mut self, mode: CountingMode) {
-            let (cms, dir) = mode.values();
+            let (cms, dir) = mode.into();
 
             let timer_enabled = Self::regs().cr1().read().cen();
             // Changing from edge aligned to center aligned (and vice versa) is not allowed while the timer is running.
@@ -87,6 +87,11 @@ pub(crate) mod sealed {
             Self::regs_gp16().cr1().modify(|r| r.set_cms(cms))
         }
 
+        fn get_counting_mode(&self) -> CountingMode {
+            let cr1 = Self::regs_gp16().cr1().read();
+            (cr1.cms(), cr1.dir()).into()
+        }
+
         fn set_clock_division(&mut self, ckd: vals::Ckd) {
             Self::regs_gp16().cr1().modify(|r| r.set_ckd(ckd));
         }
@@ -273,6 +278,7 @@ impl From<InputTISelection> for stm32_metapac::timer::vals::CcmrInputCcs {
     }
 }
 
+#[repr(u8)]
 #[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
 pub enum CountingMode {
     #[default]
@@ -298,9 +304,26 @@ pub enum CountingMode {
 }
 
 impl CountingMode {
-    /// Get the register values to set the timer mode to the current variant
-    pub fn values(&self) -> (vals::Cms, vals::Dir) {
+    pub fn is_edge_aligned(&self) -> bool {
         match self {
+            CountingMode::EdgeAlignedUp | CountingMode::EdgeAlignedDown => true,
+            _ => false,
+        }
+    }
+
+    pub fn is_center_aligned(&self) -> bool {
+        match self {
+            CountingMode::CenterAlignedDownInterrupts
+            | CountingMode::CenterAlignedUpInterrupts
+            | CountingMode::CenterAlignedBothInterrupts => true,
+            _ => false,
+        }
+    }
+}
+
+impl From<CountingMode> for (vals::Cms, vals::Dir) {
+    fn from(value: CountingMode) -> Self {
+        match value {
             CountingMode::EdgeAlignedUp => (vals::Cms::EDGEALIGNED, vals::Dir::UP),
             CountingMode::EdgeAlignedDown => (vals::Cms::EDGEALIGNED, vals::Dir::DOWN),
             CountingMode::CenterAlignedDownInterrupts => (vals::Cms::CENTERALIGNED1, vals::Dir::UP),
@@ -310,6 +333,18 @@ impl CountingMode {
     }
 }
 
+impl From<(vals::Cms, vals::Dir)> for CountingMode {
+    fn from(value: (vals::Cms, vals::Dir)) -> Self {
+        match value {
+            (vals::Cms::EDGEALIGNED, vals::Dir::UP) => CountingMode::EdgeAlignedUp,
+            (vals::Cms::EDGEALIGNED, vals::Dir::DOWN) => CountingMode::EdgeAlignedDown,
+            (vals::Cms::CENTERALIGNED1, _) => CountingMode::CenterAlignedDownInterrupts,
+            (vals::Cms::CENTERALIGNED2, _) => CountingMode::CenterAlignedUpInterrupts,
+            (vals::Cms::CENTERALIGNED3, _) => CountingMode::CenterAlignedBothInterrupts,
+        }
+    }
+}
+
 #[derive(Clone, Copy)]
 pub enum OutputCompareMode {
     Frozen,
diff --git a/embassy-stm32/src/timer/simple_pwm.rs b/embassy-stm32/src/timer/simple_pwm.rs
index cdf12189..b54f8cb0 100644
--- a/embassy-stm32/src/timer/simple_pwm.rs
+++ b/embassy-stm32/src/timer/simple_pwm.rs
@@ -69,8 +69,8 @@ impl<'d, T: CaptureCompare16bitInstance> SimplePwm<'d, T> {
 
         let mut this = Self { inner: tim };
 
-        this.inner.set_frequency(freq);
         this.inner.set_counting_mode(counting_mode);
+        this.set_freq(freq);
         this.inner.start();
 
         this.inner.enable_outputs(true);
@@ -95,7 +95,12 @@ impl<'d, T: CaptureCompare16bitInstance> SimplePwm<'d, T> {
     }
 
     pub fn set_freq(&mut self, freq: Hertz) {
-        self.inner.set_frequency(freq);
+        let multiplier = if self.inner.get_counting_mode().is_center_aligned() {
+            2u8
+        } else {
+            1u8
+        };
+        self.inner.set_frequency(freq * multiplier);
     }
 
     pub fn get_max_duty(&self) -> u16 {