2 files changed, 27 insertions, 3 deletions

diff --git a/embassy-stm32/Cargo.toml b/embassy-stm32/Cargo.toml
index 660ab0532..08b9b3a38 100644
--- a/embassy-stm32/Cargo.toml
+++ b/embassy-stm32/Cargo.toml
@@ -59,7 +59,7 @@ sdio-host = "0.5.0"
 embedded-sdmmc = { git = "https://github.com/embassy-rs/embedded-sdmmc-rs", rev = "a4f293d3a6f72158385f79c98634cb8a14d0d2fc", optional = true }
 critical-section = "1.1"
 atomic-polyfill = "1.0.1"
-stm32-metapac = { git = "https://github.com/embassy-rs/stm32-data-generated", tag = "stm32-data-1551a1c01a993bb5ffc603311f80097c14e03f85" }
+stm32-metapac = { git = "https://github.com/embassy-rs/stm32-data-generated", tag = "stm32-data-bdbf126746919e1c07730d80f9345b1a494c72a6" }
 vcell = "0.1.3"
 bxcan = "0.7.0"
 nb = "1.0.0"
@@ -78,7 +78,7 @@ critical-section = { version = "1.1", features = ["std"] }
 [build-dependencies]
 proc-macro2 = "1.0.36"
 quote = "1.0.15"
-stm32-metapac = { git = "https://github.com/embassy-rs/stm32-data-generated", tag = "stm32-data-1551a1c01a993bb5ffc603311f80097c14e03f85", default-features = false, features = ["metadata"]}
+stm32-metapac = { git = "https://github.com/embassy-rs/stm32-data-generated", tag = "stm32-data-bdbf126746919e1c07730d80f9345b1a494c72a6", default-features = false, features = ["metadata"]}
 
 [features]
 default = ["rt"]
diff --git a/embassy-stm32/src/rcc/l0.rs b/embassy-stm32/src/rcc/l0.rs
index 67355afbe..7358be31b 100644
--- a/embassy-stm32/src/rcc/l0.rs
+++ b/embassy-stm32/src/rcc/l0.rs
@@ -1,10 +1,11 @@
 use super::bd::BackupDomain;
 pub use super::bus::{AHBPrescaler, APBPrescaler};
 use super::RtcClockSource;
+pub use crate::pac::pwr::vals::Vos as VoltageScale;
 use crate::pac::rcc::vals::{Hpre, Msirange, Plldiv, Pllmul, Pllsrc, Ppre, Sw};
-use crate::pac::RCC;
 #[cfg(crs)]
 use crate::pac::{crs, CRS, SYSCFG};
+use crate::pac::{FLASH, PWR, RCC};
 use crate::rcc::{set_freqs, Clocks};
 use crate::time::Hertz;
 
@@ -140,6 +141,7 @@ pub struct Config {
     pub rtc: Option<RtcClockSource>,
     pub lse: Option<Hertz>,
     pub lsi: bool,
+    pub voltage_scale: VoltageScale,
 }
 
 impl Default for Config {
@@ -155,11 +157,17 @@ impl Default for Config {
             rtc: None,
             lse: None,
             lsi: false,
+            voltage_scale: VoltageScale::RANGE1,
         }
     }
 }
 
 pub(crate) unsafe fn init(config: Config) {
+    // Set voltage scale
+    while PWR.csr().read().vosf() {}
+    PWR.cr().write(|w| w.set_vos(config.voltage_scale));
+    while PWR.csr().read().vosf() {}
+
     let (sys_clk, sw) = match config.mux {
         ClockSrc::MSI(range) => {
             // Set MSI range
@@ -245,6 +253,22 @@ pub(crate) unsafe fn init(config: Config) {
         config.lse.map(|_| Default::default()),
     );
 
+    let wait_states = match config.voltage_scale {
+        VoltageScale::RANGE1 => match sys_clk {
+            ..=16_000_000 => 0,
+            _ => 1,
+        },
+        VoltageScale::RANGE2 => match sys_clk {
+            ..=8_000_000 => 0,
+            _ => 1,
+        },
+        VoltageScale::RANGE3 => 0,
+        _ => unreachable!(),
+    };
+    FLASH.acr().modify(|w| {
+        w.set_latency(wait_states != 0);
+    });
+
     RCC.cfgr().modify(|w| {
         w.set_sw(sw);
         w.set_hpre(config.ahb_pre.into());