2 files changed, 37 insertions, 2 deletions

diff --git a/embassy-stm32/src/rcc/g4.rs b/embassy-stm32/src/rcc/g4.rs
index 7d061192b..6b1206c6a 100644
--- a/embassy-stm32/src/rcc/g4.rs
+++ b/embassy-stm32/src/rcc/g4.rs
@@ -1,4 +1,6 @@
+use stm32_metapac::flash::vals::Latency;
 use stm32_metapac::rcc::vals::{Hpre, Pllsrc, Ppre, Sw};
+use stm32_metapac::FLASH;
 
 use crate::pac::{PWR, RCC};
 use crate::rcc::{set_freqs, Clocks};
@@ -283,6 +285,39 @@ pub(crate) unsafe fn init(config: Config) {
             let freq = src_freq / prediv.to_div() * mul.to_mul() / div.to_div();
             assert!(freq <= 170_000_000);
 
+            if freq >= 150_000_000 {
+                // Enable Core Boost mode ([RM0440] p234)
+                PWR.cr5()
+                    .modify(|w: &mut stm32_metapac::pwr::regs::Cr5| w.set_r1mode(false));
+                // Set flash wait state in boost mode based on frequency ([RM0440] p191)
+                if freq <= 36_000_000 {
+                    FLASH.acr().modify(|w| w.set_latency(Latency::WS0));
+                } else if freq <= 68_000_000 {
+                    FLASH.acr().modify(|w| w.set_latency(Latency::WS1));
+                } else if freq <= 102_000_000 {
+                    FLASH.acr().modify(|w| w.set_latency(Latency::WS2));
+                } else if freq <= 136_000_000 {
+                    FLASH.acr().modify(|w| w.set_latency(Latency::WS3));
+                } else {
+                    FLASH.acr().modify(|w| w.set_latency(Latency::WS4));
+                }
+            } else {
+                PWR.cr5()
+                    .modify(|w: &mut stm32_metapac::pwr::regs::Cr5| w.set_r1mode(true));
+                // Set flash wait state in normal mode based on frequency ([RM0440] p191)
+                if freq <= 30_000_000 {
+                    FLASH.acr().modify(|w| w.set_latency(Latency::WS0));
+                } else if freq <= 60_000_000 {
+                    FLASH.acr().modify(|w| w.set_latency(Latency::WS1));
+                } else if freq <= 80_000_000 {
+                    FLASH.acr().modify(|w| w.set_latency(Latency::WS2));
+                } else if freq <= 120_000_000 {
+                    FLASH.acr().modify(|w| w.set_latency(Latency::WS3));
+                } else {
+                    FLASH.acr().modify(|w| w.set_latency(Latency::WS4));
+                }
+            }
+
             RCC.pllcfgr().write(move |w| {
                 w.set_plln(mul.into());
                 w.set_pllm(prediv.into());
diff --git a/examples/stm32g4/src/bin/pll.rs b/examples/stm32g4/src/bin/pll.rs
index 580afe03d..bde30c284 100644
--- a/examples/stm32g4/src/bin/pll.rs
+++ b/examples/stm32g4/src/bin/pll.rs
@@ -13,8 +13,8 @@ use {defmt_rtt as _, panic_probe as _};
 async fn main(_spawner: Spawner) {
     let mut config = Config::default();
 
-    // Configure PLL to 128Mhz frequency
-    config.rcc.mux = ClockSrc::PLL(PllSrc::HSI16, PllM::Div4, PllN::Mul64, PllClkDiv::Div2);
+    // Configure PLL to max frequency of 170 MHz
+    config.rcc.mux = ClockSrc::PLL(PllSrc::HSI16, PllM::Div4, PllN::Mul85, PllClkDiv::Div2);
 
     let _p = embassy_stm32::init(config);
     info!("Hello World!");