From bd01e90bfa4895b45e41ad538cb24a959b0b58ab Mon Sep 17 00:00:00 2001
From: chemicstry <chemicstry@gmail.com>
Date: Sun, 10 Jul 2022 20:38:30 +0300
Subject: [PATCH] Implement IWDG timeout calculation

---
 embassy-stm32/src/wdg/mod.rs    | 50 ++++++++++++++++++++++++++++++---
 examples/stm32f4/src/bin/wdt.rs | 44 +++++++++++++++++++++++++++++
 2 files changed, 90 insertions(+), 4 deletions(-)
 create mode 100644 examples/stm32f4/src/bin/wdt.rs

diff --git a/embassy-stm32/src/wdg/mod.rs b/embassy-stm32/src/wdg/mod.rs
index da25692ab..b4d59ff2d 100644
--- a/embassy-stm32/src/wdg/mod.rs
+++ b/embassy-stm32/src/wdg/mod.rs
@@ -1,21 +1,63 @@
 use core::marker::PhantomData;
 
+use embassy::time::Duration;
 use embassy_hal_common::{unborrow, Unborrow};
-use stm32_metapac::iwdg::vals::Key;
-pub use stm32_metapac::iwdg::vals::Pr as Prescaler;
+use stm32_metapac::iwdg::vals::{Key, Pr};
+
+use crate::time::Hertz;
 
 pub struct IndependentWatchdog<'d, T: Instance> {
     wdg: PhantomData<&'d mut T>,
 }
 
+// Some STM32 families have 40 kHz LSI clock instead of 32 kHz
+cfg_if::cfg_if! {
+    if #[cfg(any(stm32f0, stm32f1))] {
+        pub const IWDG_FREQ: Hertz = Hertz(40_000);
+    } else {
+        pub const IWDG_FREQ: Hertz = Hertz(32_000);
+    }
+}
+
+// 12-bit counter
+const MAX_RL: u16 = 0xFFF;
+
+/// Calculates maximum watchdog timeout (RL = 0xFFF) for a given prescaler
+const fn max_timeout(prescaler: u8) -> Duration {
+    Duration::from_micros(1_000_000 / (IWDG_FREQ.0 / prescaler as u32) as u64 * MAX_RL as u64)
+}
+
+/// Calculates watchdog reload value for the given prescaler and desired timeout
+const fn reload_value(prescaler: u8, timeout: Duration) -> u16 {
+    ((IWDG_FREQ.0 / prescaler as u32) as u64 * timeout.as_micros() / 1_000_000) as u16
+}
+
 impl<'d, T: Instance> IndependentWatchdog<'d, T> {
-    pub fn new(_instance: impl Unborrow<Target = T> + 'd, presc: Prescaler) -> Self {
+    pub fn new(_instance: impl Unborrow<Target = T> + 'd, timeout: Duration) -> Self {
         unborrow!(_instance);
 
+        // Find lowest prescaler value, which makes watchdog period longer or equal to timeout.
+        // This iterates from 4 (2^2) to 256 (2^8).
+        let psc_power = unwrap!((2..=8).find(|psc_power| {
+            let psc = 2u8.pow(*psc_power);
+            timeout <= max_timeout(psc)
+        }));
+
+        // Prescaler value
+        let psc = 2u8.pow(psc_power);
+
+        // Convert prescaler power to PR register value
+        let pr = psc_power as u8 - 2;
+        assert!(pr <= 0b110);
+
+        // Reload value
+        let rl = reload_value(psc, timeout);
+
         let wdg = T::regs();
         unsafe {
             wdg.kr().write(|w| w.set_key(Key::ENABLE));
-            wdg.pr().write(|w| w.set_pr(presc));
+            wdg.pr().write(|w| w.set_pr(Pr(pr)));
+            wdg.rlr().write(|w| w.set_rl(rl));
         }
 
         IndependentWatchdog {
diff --git a/examples/stm32f4/src/bin/wdt.rs b/examples/stm32f4/src/bin/wdt.rs
new file mode 100644
index 000000000..41e1f4c7b
--- /dev/null
+++ b/examples/stm32f4/src/bin/wdt.rs
@@ -0,0 +1,44 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+use defmt::*;
+use embassy::executor::Spawner;
+use embassy::time::{Duration, Timer};
+use embassy_stm32::gpio::{Level, Output, Speed};
+use embassy_stm32::wdg::IndependentWatchdog;
+use embassy_stm32::Peripherals;
+use {defmt_rtt as _, panic_probe as _};
+
+#[embassy::main]
+async fn main(_spawner: Spawner, p: Peripherals) {
+    info!("Hello World!");
+
+    let mut led = Output::new(p.PB7, Level::High, Speed::Low);
+
+    let mut wdt = IndependentWatchdog::new(p.IWDG, Duration::from_secs(1));
+    unsafe {
+        wdt.unleash();
+    }
+
+    let mut i = 0;
+
+    loop {
+        info!("high");
+        led.set_high();
+        Timer::after(Duration::from_millis(300)).await;
+
+        info!("low");
+        led.set_low();
+        Timer::after(Duration::from_millis(300)).await;
+
+        if i < 5 {
+            info!("Petting watchdog");
+            unsafe {
+                wdt.pet();
+            }
+        }
+
+        i += 1;
+    }
+}