From f1c077ed2e420b1b4f8c1835fe05a1279a742267 Mon Sep 17 00:00:00 2001
From: Christian Enderle <mail@chrenderle.de>
Date: Tue, 2 Jan 2024 23:05:47 +0100
Subject: [PATCH] low-power: add stop support for stm32l5

---
 embassy-stm32/src/lib.rs       |  2 +-
 embassy-stm32/src/low_power.rs | 20 ++++++++++++++++++--
 2 files changed, 19 insertions(+), 3 deletions(-)

diff --git a/embassy-stm32/src/lib.rs b/embassy-stm32/src/lib.rs
index 2508c4fca..45a2ab63e 100644
--- a/embassy-stm32/src/lib.rs
+++ b/embassy-stm32/src/lib.rs
@@ -157,7 +157,7 @@ pub struct Config {
     /// RCC config.
     pub rcc: rcc::Config,
 
-    /// Enable debug during sleep.
+    /// Enable debug during sleep and stop.
     ///
     /// May incrase power consumption. Defaults to true.
     #[cfg(dbgmcu)]
diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs
index 4fab8dae4..c5c5dd96f 100644
--- a/embassy-stm32/src/low_power.rs
+++ b/embassy-stm32/src/low_power.rs
@@ -37,6 +37,8 @@
 //! async fn async_main(spawner: Spawner) {
 //!     // initialize the platform...
 //!     let mut config = embassy_stm32::Config::default();
+//!     // when enabled the power-consumption is much higher during stop, but debugging and RTT is working
+//!     config.enable_debug_during_sleep = false;
 //!     let p = embassy_stm32::init(config);
 //!
 //!     // give the RTC to the executor...
@@ -107,6 +109,19 @@ pub enum StopMode {
     Stop2,
 }
 
+#[cfg(stm32l5)]
+use stm32_metapac::pwr::vals::Lpms;
+
+#[cfg(stm32l5)]
+impl Into<Lpms> for StopMode {
+    fn into(self) -> Lpms {
+        match self {
+            StopMode::Stop1 => Lpms::STOP1,
+            StopMode::Stop2 => Lpms::STOP2,
+        }
+    }
+}
+
 /// Thread mode executor, using WFE/SEV.
 ///
 /// This is the simplest and most common kind of executor. It runs on
@@ -164,8 +179,9 @@ impl Executor {
         }
     }
 
-    fn configure_stop(&mut self, _stop_mode: StopMode) {
-        // TODO: configure chip-specific settings for stop
+    fn configure_stop(&mut self, stop_mode: StopMode) {
+        #[cfg(stm32l5)]
+        crate::pac::PWR.cr1().modify(|m| m.set_lpms(stop_mode.into()));
     }
 
     fn configure_pwr(&mut self) {