include proper pll divs/divt initialization

1 files changed, 33 insertions, 1 deletions

diff --git a/embassy-stm32/src/rcc/h.rs b/embassy-stm32/src/rcc/h.rs
index c31b1bbd1..837210b6a 100644
--- a/embassy-stm32/src/rcc/h.rs
+++ b/embassy-stm32/src/rcc/h.rs
@@ -758,6 +758,12 @@ struct PllOutput {
     q: Option<Hertz>,
     #[allow(dead_code)]
     r: Option<Hertz>,
+    #[cfg(stm32h7rs)]
+    #[allow(dead_code)]
+    s: Option<Hertz>,
+    #[cfg(stm32h7rs)]
+    #[allow(dead_code)]
+    t: Option<Hertz>,
 }
 
 fn init_pll(num: usize, config: Option<Pll>, input: &PllInput) -> PllOutput {
@@ -776,6 +782,10 @@ fn init_pll(num: usize, config: Option<Pll>, input: &PllInput) -> PllOutput {
             p: None,
             q: None,
             r: None,
+            #[cfg(stm32h7rs)]
+            s: None,
+            #[cfg(stm32h7rs)]
+            t: None,
         };
     };
 
@@ -823,6 +833,10 @@ fn init_pll(num: usize, config: Option<Pll>, input: &PllInput) -> PllOutput {
     });
     let q = config.divq.map(|div| vco_clk / div);
     let r = config.divr.map(|div| vco_clk / div);
+    #[cfg(stm32h7rs)]
+    let s = config.divs.map(|div| vco_clk / div);
+    #[cfg(stm32h7rs)]
+    let t = config.divt.map(|div| vco_clk / div);
 
     #[cfg(stm32h5)]
     RCC.pllcfgr(num).write(|w| {
@@ -849,6 +863,10 @@ fn init_pll(num: usize, config: Option<Pll>, input: &PllInput) -> PllOutput {
             w.set_divpen(num, p.is_some());
             w.set_divqen(num, q.is_some());
             w.set_divren(num, r.is_some());
+            #[cfg(stm32h7rs)]
+            w.set_divsen(num, s.is_some());
+            #[cfg(stm32h7rs)]
+            w.set_divten(num, t.is_some());
         });
     }
 
@@ -859,10 +877,24 @@ fn init_pll(num: usize, config: Option<Pll>, input: &PllInput) -> PllOutput {
         w.set_pllr(config.divr.unwrap_or(PllDiv::DIV2));
     });
 
+    #[cfg(stm32h7rs)]
+    RCC.plldivr2(num).write(|w| {
+        w.set_plls(config.divs.unwrap_or(Plldivst::DIV2));
+        w.set_pllt(config.divt.unwrap_or(Plldivst::DIV2));
+    });
+
     RCC.cr().modify(|w| w.set_pllon(num, true));
     while !RCC.cr().read().pllrdy(num) {}
 
-    PllOutput { p, q, r }
+    PllOutput {
+        p,
+        q,
+        r,
+        #[cfg(stm32h7rs)]
+        s,
+        #[cfg(stm32h7rs)]
+        t,
+    }
 }
 
 fn flash_setup(clk: Hertz, vos: VoltageScale) {