Merge pull request #4150 from 1-rafael-1/rp2040-overclocking

RP: rp2040 overclocking

6 files changed, 839 insertions, 44 deletions

diff --git a/embassy-rp/src/clocks.rs b/embassy-rp/src/clocks.rs
index 67aa5e540..6694aab66 100644
--- a/embassy-rp/src/clocks.rs
+++ b/embassy-rp/src/clocks.rs
@@ -1,4 +1,67 @@
-//! Clock configuration for the RP2040
+//! # Clock configuration for the RP2040 and RP235x microcontrollers.
+//!
+//! # Clock Configuration API
+//!
+//! This module provides both high-level convenience functions and low-level manual
+//! configuration options for the RP2040 clock system.
+//!
+//! ## High-Level Convenience Functions
+//!
+//! For most users, these functions provide an easy way to configure clocks:
+//!
+//! - `ClockConfig::system_freq(125_000_000)` - Set system clock to a specific frequency with automatic voltage scaling
+//! - `ClockConfig::crystal(12_000_000)` - Default configuration with 12MHz crystal giving 125MHz system clock
+//!
+//! ## Manual Configuration
+//!
+//! For advanced users who need precise control:
+//!
+//! ```rust,ignore
+//! // Start with default configuration and customize it
+//! let mut config = ClockConfig::default();
+//!
+//! // Set custom PLL parameters
+//! config.xosc = Some(XoscConfig {
+//!     hz: 12_000_000,
+//!     sys_pll: Some(PllConfig {
+//!         refdiv: 1,
+//!         fbdiv: 200,
+//!         post_div1: 6,
+//!         post_div2: 2,
+//!     }),
+//!     // ... other fields
+//! });
+//!
+//! // Set voltage for overclocking
+//! config.core_voltage = CoreVoltage::V1_15;
+//! ```
+//!
+//! ## Examples
+//!
+//! ### Standard 125MHz configuration
+//! ```rust,ignore
+//! let config = ClockConfig::crystal(12_000_000);
+//! ```
+//!
+//! Or using the default configuration:
+//! ```rust,ignore
+//! let config = ClockConfig::default();
+//! ```
+//!
+//! ### Overclock to 200MHz
+//! ```rust,ignore
+//! let config = ClockConfig::system_freq(200_000_000);
+//! ```
+//!
+//! ### Manual configuration for advanced scenarios
+//! ```rust,ignore
+//! use embassy_rp::clocks::{ClockConfig, XoscConfig, PllConfig, CoreVoltage};
+//!
+//! // Start with defaults and customize
+//! let mut config = ClockConfig::default();
+//! config.core_voltage = CoreVoltage::V1_15;
+//! // Set other parameters as needed...
+//! ```
 
 #[cfg(feature = "rp2040")]
 use core::arch::asm;
@@ -18,6 +81,7 @@ use crate::{pac, reset, Peri};
 // gpin is not usually safe to use during the boot init() call, so it won't
 // be very useful until we have runtime clock reconfiguration. once this
 // happens we can resurrect the commented-out gpin bits.
+
 struct Clocks {
     xosc: AtomicU32,
     sys: AtomicU32,
@@ -26,6 +90,7 @@ struct Clocks {
     pll_usb: AtomicU32,
     usb: AtomicU32,
     adc: AtomicU32,
+    // See above re gpin handling being commented out
     // gpin0: AtomicU32,
     // gpin1: AtomicU32,
     rosc: AtomicU32,
@@ -42,6 +107,7 @@ static CLOCKS: Clocks = Clocks {
     pll_usb: AtomicU32::new(0),
     usb: AtomicU32::new(0),
     adc: AtomicU32::new(0),
+    // See above re gpin handling being commented out
     // gpin0: AtomicU32::new(0),
     // gpin1: AtomicU32::new(0),
     rosc: AtomicU32::new(0),
@@ -65,10 +131,64 @@ pub enum PeriClkSrc {
     Rosc = ClkPeriCtrlAuxsrc::ROSC_CLKSRC_PH as _,
     /// XOSC.
     Xosc = ClkPeriCtrlAuxsrc::XOSC_CLKSRC as _,
+    // See above re gpin handling being commented out
     // Gpin0 = ClkPeriCtrlAuxsrc::CLKSRC_GPIN0 as _ ,
     // Gpin1 = ClkPeriCtrlAuxsrc::CLKSRC_GPIN1 as _ ,
 }
 
+/// Core voltage regulator settings for RP2040.
+///
+/// The RP2040 voltage regulator can be configured for different output voltages.
+/// Higher voltages allow for higher clock frequencies but increase power consumption and heat.
+#[cfg(feature = "rp2040")]
+#[derive(Clone, Copy, Debug, PartialEq, Eq)]
+#[repr(u8)]
+pub enum CoreVoltage {
+    /// 0.80V - Suitable for lower frequencies
+    V0_80 = 0b0000,
+    /// 0.85V
+    V0_85 = 0b0110,
+    /// 0.90V
+    V0_90 = 0b0111,
+    /// 0.95V
+    V0_95 = 0b1000,
+    /// 1.00V
+    V1_00 = 0b1001,
+    /// 1.05V
+    V1_05 = 0b1010,
+    /// 1.10V - Default voltage level
+    V1_10 = 0b1011,
+    /// 1.15V - Required for overclocking to 133-200MHz
+    V1_15 = 0b1100,
+    /// 1.20V
+    V1_20 = 0b1101,
+    /// 1.25V
+    V1_25 = 0b1110,
+    /// 1.30V
+    V1_30 = 0b1111,
+}
+
+#[cfg(feature = "rp2040")]
+impl CoreVoltage {
+    /// Get the recommended Brown-Out Detection (BOD) setting for this voltage.
+    /// Sets the BOD threshold to approximately 80% of the core voltage.
+    fn recommended_bod(self) -> u8 {
+        match self {
+            CoreVoltage::V0_80 => 0b0100, // 0.645V (~81% of 0.80V)
+            CoreVoltage::V0_85 => 0b0101, // 0.688V (~81% of 0.85V)
+            CoreVoltage::V0_90 => 0b0110, // 0.731V (~81% of 0.90V)
+            CoreVoltage::V0_95 => 0b0111, // 0.774V (~81% of 0.95V)
+            CoreVoltage::V1_00 => 0b1000, // 0.817V (~82% of 1.00V)
+            CoreVoltage::V1_05 => 0b1000, // 0.817V (~78% of 1.05V)
+            CoreVoltage::V1_10 => 0b1001, // 0.860V (~78% of 1.10V)
+            CoreVoltage::V1_15 => 0b1010, // 0.903V (~79% of 1.15V)
+            CoreVoltage::V1_20 => 0b1011, // 0.946V (~79% of 1.20V)
+            CoreVoltage::V1_25 => 0b1100, // 0.989V (~79% of 1.25V)
+            CoreVoltage::V1_30 => 0b1101, // 1.032V (~79% of 1.30V)
+        }
+    }
+}
+
 /// CLock configuration.
 #[non_exhaustive]
 pub struct ClockConfig {
@@ -89,12 +209,63 @@ pub struct ClockConfig {
     /// RTC clock configuration.
     #[cfg(feature = "rp2040")]
     pub rtc_clk: Option<RtcClkConfig>,
+    /// Core voltage scaling (RP2040 only). Defaults to 1.10V.
+    #[cfg(feature = "rp2040")]
+    pub core_voltage: CoreVoltage,
+    /// Voltage stabilization delay in microseconds.
+    /// If not set, defaults will be used based on voltage level.
+    #[cfg(feature = "rp2040")]
+    pub voltage_stabilization_delay_us: Option<u32>,
+    // See above re gpin handling being commented out
     // gpin0: Option<(u32, Gpin<'static, AnyPin>)>,
     // gpin1: Option<(u32, Gpin<'static, AnyPin>)>,
 }
 
+impl Default for ClockConfig {
+    /// Creates a minimal default configuration with safe values.
+    ///
+    /// This configuration uses the ring oscillator (ROSC) as the clock source
+    /// and sets minimal defaults that guarantee a working system. It's intended
+    /// as a starting point for manual configuration.
+    ///
+    /// Most users should use one of the more specific configuration functions:
+    /// - `ClockConfig::crystal()` - Standard configuration with external crystal
+    /// - `ClockConfig::rosc()` - Configuration using only the internal oscillator
+    /// - `ClockConfig::system_freq()` - Configuration for a specific system frequency
+    fn default() -> Self {
+        Self {
+            rosc: None,
+            xosc: None,
+            ref_clk: RefClkConfig {
+                src: RefClkSrc::Rosc,
+                div: 1,
+            },
+            sys_clk: SysClkConfig {
+                src: SysClkSrc::Rosc,
+                div_int: 1,
+                div_frac: 0,
+            },
+            peri_clk_src: None,
+            usb_clk: None,
+            adc_clk: None,
+            #[cfg(feature = "rp2040")]
+            rtc_clk: None,
+            #[cfg(feature = "rp2040")]
+            core_voltage: CoreVoltage::V1_10,
+            #[cfg(feature = "rp2040")]
+            voltage_stabilization_delay_us: None,
+            // See above re gpin handling being commented out
+            // gpin0: None,
+            // gpin1: None,
+        }
+    }
+}
+
 impl ClockConfig {
     /// Clock configuration derived from external crystal.
+    ///
+    /// This uses default settings for most parameters, suitable for typical use cases.
+    /// For manual control of PLL parameters, use `new_manual()` or modify the struct fields directly.
     pub fn crystal(crystal_hz: u32) -> Self {
         Self {
             rosc: Some(RoscConfig {
@@ -152,6 +323,11 @@ impl ClockConfig {
                 div_frac: 0,
                 phase: 0,
             }),
+            #[cfg(feature = "rp2040")]
+            core_voltage: CoreVoltage::V1_10, // Use hardware default (1.10V)
+            #[cfg(feature = "rp2040")]
+            voltage_stabilization_delay_us: None,
+            // See above re gpin handling being commented out
             // gpin0: None,
             // gpin1: None,
         }
@@ -192,20 +368,157 @@ impl ClockConfig {
                 div_frac: 171,
                 phase: 0,
             }),
+            #[cfg(feature = "rp2040")]
+            core_voltage: CoreVoltage::V1_10, // Use hardware default (1.10V)
+            #[cfg(feature = "rp2040")]
+            voltage_stabilization_delay_us: None,
+            // See above re gpin handling being commented out
             // gpin0: None,
             // gpin1: None,
         }
     }
 
-    // pub fn bind_gpin<P: GpinPin>(&mut self, gpin: Gpin<'static, P>, hz: u32) {
-    //     match P::NR {
-    //         0 => self.gpin0 = Some((hz, gpin.into())),
-    //         1 => self.gpin1 = Some((hz, gpin.into())),
-    //         _ => unreachable!(),
-    //     }
-    //     // pin is now provisionally bound. if the config is applied it must be forgotten,
-    //     // or Gpin::drop will deconfigure the clock input.
-    // }
+    /// Configure clocks derived from an external crystal with specific system frequency.
+    ///
+    /// This function calculates optimal PLL parameters to achieve the requested system
+    /// frequency. This only works for the usual 12MHz crystal. In case a different crystal is used,
+    /// You will have to set the PLL parameters manually.
+    ///
+    /// # Arguments
+    ///
+    /// * `sys_freq_hz` - The desired system clock frequency in Hz
+    ///
+    /// # Returns
+    ///
+    /// A ClockConfig configured to achieve the requested system frequency using the
+    /// the usual 12Mhz crystal, or panic if no valid parameters can be found.
+    ///
+    /// # Note on core voltage:
+    /// To date the only officially documented core voltages (see Datasheet section 2.15.3.1. Instances) are:
+    /// - Up to 133MHz: V1_10 (default)
+    /// - Above 133MHz: V1_15, but in the context of the datasheet covering reaching up to 200Mhz
+    /// That way all other frequencies below 133MHz or above 200MHz are not explicitly documented and not covered here.
+    /// In case You want to go below 133MHz or above 200MHz and want a different voltage, You will have to set that manually and with caution.
+    #[cfg(feature = "rp2040")]
+    pub fn system_freq(hz: u32) -> Self {
+        // Start with the standard configuration from crystal()
+        const DEFAULT_CRYSTAL_HZ: u32 = 12_000_000;
+        let mut config = Self::crystal(DEFAULT_CRYSTAL_HZ);
+
+        // No need to modify anything if target frequency is already 125MHz
+        // (which is what crystal() configures by default)
+        if hz == 125_000_000 {
+            return config;
+        }
+
+        // Find optimal PLL parameters for the requested frequency
+        let sys_pll_params = find_pll_params(DEFAULT_CRYSTAL_HZ, hz)
+            .unwrap_or_else(|| panic!("Could not find valid PLL parameters for system clock"));
+
+        // Replace the sys_pll configuration with our custom parameters
+        if let Some(xosc) = &mut config.xosc {
+            xosc.sys_pll = Some(sys_pll_params);
+        }
+
+        // Set the voltage scale based on the target frequency
+        // Higher frequencies require higher voltage
+        #[cfg(feature = "rp2040")]
+        {
+            config.core_voltage = match hz {
+                freq if freq > 133_000_000 => CoreVoltage::V1_15,
+                _ => CoreVoltage::V1_10, // Use default voltage (V1_10)
+            };
+        }
+
+        config
+    }
+
+    /// Configure with manual PLL settings for full control over system clock
+    ///
+    /// This method provides a simple way to configure the system with custom PLL parameters
+    /// without needing to understand the full nested configuration structure.
+    ///
+    /// # Arguments
+    ///
+    /// * `xosc_hz` - The frequency of the external crystal in Hz
+    /// * `pll_config` - The PLL configuration parameters to achieve desired frequency
+    /// * `core_voltage` - Voltage scaling for overclocking (required for >133MHz)
+    ///
+    /// # Returns
+    ///
+    /// A ClockConfig configured with the specified PLL parameters
+    ///
+    /// # Example
+    ///
+    /// ```rust,ignore
+    /// // Configure for 200MHz operation
+    /// let config = Config::default();
+    /// config.clocks = ClockConfig::manual_pll(
+    ///     12_000_000,
+    ///     PllConfig {
+    ///         refdiv: 1,    // Reference divider (12 MHz / 1 = 12 MHz)
+    ///         fbdiv: 100,   // Feedback divider (12 MHz * 100 = 1200 MHz VCO)
+    ///         post_div1: 3, // First post divider (1200 MHz / 3 = 400 MHz)
+    ///         post_div2: 2, // Second post divider (400 MHz / 2 = 200 MHz)
+    ///     },
+    ///     CoreVoltage::V1_15
+    /// );
+    /// ```
+    #[cfg(feature = "rp2040")]
+    pub fn manual_pll(xosc_hz: u32, pll_config: PllConfig, core_voltage: CoreVoltage) -> Self {
+        // Validate PLL parameters
+        assert!(pll_config.is_valid(xosc_hz), "Invalid PLL parameters");
+
+        let mut config = Self::default();
+
+        config.xosc = Some(XoscConfig {
+            hz: xosc_hz,
+            sys_pll: Some(pll_config),
+            usb_pll: Some(PllConfig {
+                refdiv: 1,
+                fbdiv: 120,
+                post_div1: 6,
+                post_div2: 5,
+            }),
+            delay_multiplier: 128,
+        });
+
+        config.ref_clk = RefClkConfig {
+            src: RefClkSrc::Xosc,
+            div: 1,
+        };
+
+        config.sys_clk = SysClkConfig {
+            src: SysClkSrc::PllSys,
+            div_int: 1,
+            div_frac: 0,
+        };
+
+        config.core_voltage = core_voltage;
+        config.peri_clk_src = Some(PeriClkSrc::Sys);
+
+        // Set reasonable defaults for other clocks
+        config.usb_clk = Some(UsbClkConfig {
+            src: UsbClkSrc::PllUsb,
+            div: 1,
+            phase: 0,
+        });
+
+        config.adc_clk = Some(AdcClkConfig {
+            src: AdcClkSrc::PllUsb,
+            div: 1,
+            phase: 0,
+        });
+
+        config.rtc_clk = Some(RtcClkConfig {
+            src: RtcClkSrc::PllUsb,
+            div_int: 1024,
+            div_frac: 0,
+            phase: 0,
+        });
+
+        config
+    }
 }
 
 /// ROSC freq range.
@@ -251,6 +564,7 @@ pub struct XoscConfig {
 }
 
 /// PLL configuration.
+#[derive(Clone, Copy, Debug)]
 pub struct PllConfig {
     /// Reference divisor.
     pub refdiv: u8,
@@ -262,6 +576,50 @@ pub struct PllConfig {
     pub post_div2: u8,
 }
 
+impl PllConfig {
+    /// Calculate the output frequency for this PLL configuration
+    /// given an input frequency.
+    pub fn output_frequency(&self, input_hz: u32) -> u32 {
+        let ref_freq = input_hz / self.refdiv as u32;
+        let vco_freq = ref_freq * self.fbdiv as u32;
+        vco_freq / ((self.post_div1 * self.post_div2) as u32)
+    }
+
+    /// Check if this PLL configuration is valid for the given input frequency.
+    pub fn is_valid(&self, input_hz: u32) -> bool {
+        // Check divisor constraints
+        if self.refdiv < 1 || self.refdiv > 63 {
+            return false;
+        }
+        if self.fbdiv < 16 || self.fbdiv > 320 {
+            return false;
+        }
+        if self.post_div1 < 1 || self.post_div1 > 7 {
+            return false;
+        }
+        if self.post_div2 < 1 || self.post_div2 > 7 {
+            return false;
+        }
+        if self.post_div2 > self.post_div1 {
+            return false;
+        }
+
+        // Calculate reference frequency
+        let ref_freq = input_hz / self.refdiv as u32;
+
+        // Check reference frequency range
+        if ref_freq < 5_000_000 || ref_freq > 800_000_000 {
+            return false;
+        }
+
+        // Calculate VCO frequency
+        let vco_freq = ref_freq * self.fbdiv as u32;
+
+        // Check VCO frequency range
+        vco_freq >= 750_000_000 && vco_freq <= 1_800_000_000
+    }
+}
+
 /// Reference clock config.
 pub struct RefClkConfig {
     /// Reference clock source.
@@ -280,6 +638,7 @@ pub enum RefClkSrc {
     Rosc,
     /// PLL USB.
     PllUsb,
+    // See above re gpin handling being commented out
     // Gpin0,
     // Gpin1,
 }
@@ -298,6 +657,7 @@ pub enum SysClkSrc {
     Rosc,
     /// XOSC.
     Xosc,
+    // See above re gpin handling being commented out
     // Gpin0,
     // Gpin1,
 }
@@ -333,6 +693,7 @@ pub enum UsbClkSrc {
     Rosc = ClkUsbCtrlAuxsrc::ROSC_CLKSRC_PH as _,
     /// XOSC.
     Xosc = ClkUsbCtrlAuxsrc::XOSC_CLKSRC as _,
+    // See above re gpin handling being commented out
     // Gpin0 = ClkUsbCtrlAuxsrc::CLKSRC_GPIN0 as _ ,
     // Gpin1 = ClkUsbCtrlAuxsrc::CLKSRC_GPIN1 as _ ,
 }
@@ -360,6 +721,7 @@ pub enum AdcClkSrc {
     Rosc = ClkAdcCtrlAuxsrc::ROSC_CLKSRC_PH as _,
     /// XOSC.
     Xosc = ClkAdcCtrlAuxsrc::XOSC_CLKSRC as _,
+    // See above re gpin handling being commented out
     // Gpin0 = ClkAdcCtrlAuxsrc::CLKSRC_GPIN0 as _ ,
     // Gpin1 = ClkAdcCtrlAuxsrc::CLKSRC_GPIN1 as _ ,
 }
@@ -388,6 +750,7 @@ pub enum RtcClkSrc {
     Rosc = ClkRtcCtrlAuxsrc::ROSC_CLKSRC_PH as _,
     /// XOSC.
     Xosc = ClkRtcCtrlAuxsrc::XOSC_CLKSRC as _,
+    // See above re gpin handling being commented out
     // Gpin0 = ClkRtcCtrlAuxsrc::CLKSRC_GPIN0 as _ ,
     // Gpin1 = ClkRtcCtrlAuxsrc::CLKSRC_GPIN1 as _ ,
 }
@@ -405,6 +768,103 @@ pub struct RtcClkConfig {
     pub phase: u8,
 }
 
+/// Find valid PLL parameters (refdiv, fbdiv, post_div1, post_div2) for a target output frequency
+/// based on the input frequency.
+///
+/// This function searches for the best PLL configuration to achieve the requested target frequency
+/// while staying within the VCO frequency range of 750MHz to 1800MHz. It prioritizes stability
+/// over exact frequency matching by using larger divisors where possible.
+///
+/// # Parameters
+///
+/// * `input_hz`: The input frequency in Hz (typically the crystal frequency, e.g. 12MHz for th most common one used on rp2040 boards)
+/// * `target_hz`: The desired output frequency in Hz (e.g. 125MHz for standard RP2040 operation)
+///
+/// # Returns
+///
+/// * `Some(PllConfig)` if valid parameters were found
+/// * `None` if no valid parameters could be found for the requested combination
+///
+/// # Example
+///
+/// ```rust,ignore
+/// // Find parameters for 133MHz system clock from 12MHz crystal
+/// let pll_params = find_pll_params(12_000_000, 133_000_000).unwrap();
+/// ```
+#[cfg(feature = "rp2040")]
+fn find_pll_params(input_hz: u32, target_hz: u32) -> Option<PllConfig> {
+    // Fixed reference divider for system PLL
+    const PLL_SYS_REFDIV: u8 = 1;
+
+    // Calculate reference frequency
+    let reference_freq = input_hz as u64 / PLL_SYS_REFDIV as u64;
+
+    // Start from highest fbdiv for better stability (like SDK does)
+    for fbdiv in (16..=320).rev() {
+        let vco_freq = reference_freq * fbdiv;
+
+        // Check VCO frequency is within valid range
+        if vco_freq < 750_000_000 || vco_freq > 1_800_000_000 {
+            continue;
+        }
+
+        // Try all possible postdiv combinations starting from larger values
+        // (more conservative/stable approach)
+        for post_div1 in (1..=7).rev() {
+            for post_div2 in (1..=post_div1).rev() {
+                let out_freq = vco_freq / (post_div1 * post_div2);
+
+                // Check if we get the exact target frequency without remainder
+                if out_freq == target_hz as u64 && (vco_freq % (post_div1 * post_div2) == 0) {
+                    return Some(PllConfig {
+                        refdiv: PLL_SYS_REFDIV,
+                        fbdiv: fbdiv as u16,
+                        post_div1: post_div1 as u8,
+                        post_div2: post_div2 as u8,
+                    });
+                }
+            }
+        }
+    }
+
+    // If we couldn't find an exact match, find the closest match
+    let mut best_config = None;
+    let mut min_diff = u32::MAX;
+
+    for fbdiv in (16..=320).rev() {
+        let vco_freq = reference_freq * fbdiv;
+
+        if vco_freq < 750_000_000 || vco_freq > 1_800_000_000 {
+            continue;
+        }
+
+        for post_div1 in (1..=7).rev() {
+            for post_div2 in (1..=post_div1).rev() {
+                let out_freq = (vco_freq / (post_div1 * post_div2) as u64) as u32;
+                let diff = if out_freq > target_hz {
+                    out_freq - target_hz
+                } else {
+                    target_hz - out_freq
+                };
+
+                // If this is closer to the target, save it
+                if diff < min_diff {
+                    min_diff = diff;
+                    best_config = Some(PllConfig {
+                        refdiv: PLL_SYS_REFDIV,
+                        fbdiv: fbdiv as u16,
+                        post_div1: post_div1 as u8,
+                        post_div2: post_div2 as u8,
+                    });
+                }
+            }
+        }
+    }
+
+    // Return the closest match if we found one
+    best_config
+}
+
 /// safety: must be called exactly once at bootup
 pub(crate) unsafe fn init(config: ClockConfig) {
     // Reset everything except:
@@ -447,6 +907,7 @@ pub(crate) unsafe fn init(config: ClockConfig) {
     reset::reset(peris);
     reset::unreset_wait(peris);
 
+    // See above re gpin handling being commented out
     // let gpin0_freq = config.gpin0.map_or(0, |p| {
     //     core::mem::forget(p.1);
     //     p.0
@@ -464,19 +925,60 @@ pub(crate) unsafe fn init(config: ClockConfig) {
     };
     CLOCKS.rosc.store(rosc_freq, Ordering::Relaxed);
 
+    // Set Core Voltage (RP2040 only), if we have config for it and we're not using the default
+    #[cfg(feature = "rp2040")]
+    {
+        let voltage = config.core_voltage;
+        let vreg = pac::VREG_AND_CHIP_RESET;
+        let current_vsel = vreg.vreg().read().vsel();
+        let target_vsel = voltage as u8;
+
+        // If the target voltage is different from the current one, we need to change it
+        if target_vsel != current_vsel {
+            // Use modify() to preserve the HIZ and EN bits - otherwise we will disable the regulator when changing voltage
+            vreg.vreg().modify(|w| w.set_vsel(target_vsel));
+
+            // Wait for the voltage to stabilize. Use the provided delay or default based on voltage
+            let settling_time_us = config.voltage_stabilization_delay_us.unwrap_or_else(|| {
+                match voltage {
+                    CoreVoltage::V1_15 => 1000,                                           // 1ms for 1.15V
+                    CoreVoltage::V1_20 | CoreVoltage::V1_25 | CoreVoltage::V1_30 => 2000, // 2ms for higher voltages
+                    _ => 0,                                                               // no delay for all others
+                }
+            });
+
+            if settling_time_us != 0 {
+                // Delay in microseconds, using the ROSC frequency to calculate cycles
+                let cycles_per_us = rosc_freq / 1_000_000;
+                let delay_cycles = settling_time_us * cycles_per_us;
+                cortex_m::asm::delay(delay_cycles);
+            }
+
+            // Only now set the BOD level. At this point the voltage is considered stable.
+            vreg.bod().write(|w| {
+                w.set_vsel(voltage.recommended_bod());
+                w.set_en(true); // Enable brownout detection
+            });
+        }
+    }
+
     let (xosc_freq, pll_sys_freq, pll_usb_freq) = match config.xosc {
         Some(config) => {
             // start XOSC
-            // datasheet mentions support for clock inputs into XIN, but doesn't go into
-            // how this is achieved. pico-sdk doesn't support this at all.
             start_xosc(config.hz, config.delay_multiplier);
 
             let pll_sys_freq = match config.sys_pll {
-                Some(sys_pll_config) => configure_pll(pac::PLL_SYS, config.hz, sys_pll_config),
+                Some(sys_pll_config) => match configure_pll(pac::PLL_SYS, config.hz, sys_pll_config) {
+                    Ok(freq) => freq,
+                    Err(e) => panic!("Failed to configure PLL_SYS: {}", e),
+                },
                 None => 0,
             };
             let pll_usb_freq = match config.usb_pll {
-                Some(usb_pll_config) => configure_pll(pac::PLL_USB, config.hz, usb_pll_config),
+                Some(usb_pll_config) => match configure_pll(pac::PLL_USB, config.hz, usb_pll_config) {
+                    Ok(freq) => freq,
+                    Err(e) => panic!("Failed to configure PLL_USB: {}", e),
+                },
                 None => 0,
             };
 
@@ -484,6 +986,7 @@ pub(crate) unsafe fn init(config: ClockConfig) {
         }
         None => (0, 0, 0),
     };
+
     CLOCKS.xosc.store(xosc_freq, Ordering::Relaxed);
     CLOCKS.pll_sys.store(pll_sys_freq, Ordering::Relaxed);
     CLOCKS.pll_usb.store(pll_usb_freq, Ordering::Relaxed);
@@ -496,6 +999,7 @@ pub(crate) unsafe fn init(config: ClockConfig) {
             RefClkSrc::Xosc => (Src::XOSC_CLKSRC, Aux::CLKSRC_PLL_USB, xosc_freq / div),
             RefClkSrc::Rosc => (Src::ROSC_CLKSRC_PH, Aux::CLKSRC_PLL_USB, rosc_freq / div),
             RefClkSrc::PllUsb => (Src::CLKSRC_CLK_REF_AUX, Aux::CLKSRC_PLL_USB, pll_usb_freq / div),
+            // See above re gpin handling being commented out
             // RefClkSrc::Gpin0 => (Src::CLKSRC_CLK_REF_AUX, Aux::CLKSRC_GPIN0, gpin0_freq / div),
             // RefClkSrc::Gpin1 => (Src::CLKSRC_CLK_REF_AUX, Aux::CLKSRC_GPIN1, gpin1_freq / div),
         }
@@ -540,6 +1044,7 @@ pub(crate) unsafe fn init(config: ClockConfig) {
             SysClkSrc::PllUsb => (Src::CLKSRC_CLK_SYS_AUX, Aux::CLKSRC_PLL_USB, pll_usb_freq),
             SysClkSrc::Rosc => (Src::CLKSRC_CLK_SYS_AUX, Aux::ROSC_CLKSRC, rosc_freq),
             SysClkSrc::Xosc => (Src::CLKSRC_CLK_SYS_AUX, Aux::XOSC_CLKSRC, xosc_freq),
+            // See above re gpin handling being commented out
             // SysClkSrc::Gpin0 => (Src::CLKSRC_CLK_SYS_AUX, Aux::CLKSRC_GPIN0, gpin0_freq),
             // SysClkSrc::Gpin1 => (Src::CLKSRC_CLK_SYS_AUX, Aux::CLKSRC_GPIN1, gpin1_freq),
         };
@@ -583,6 +1088,7 @@ pub(crate) unsafe fn init(config: ClockConfig) {
             PeriClkSrc::PllUsb => pll_usb_freq,
             PeriClkSrc::Rosc => rosc_freq,
             PeriClkSrc::Xosc => xosc_freq,
+            // See above re gpin handling being commented out
             // PeriClkSrc::Gpin0 => gpin0_freq,
             // PeriClkSrc::Gpin1 => gpin1_freq,
         };
@@ -608,6 +1114,7 @@ pub(crate) unsafe fn init(config: ClockConfig) {
             UsbClkSrc::PllSys => pll_sys_freq,
             UsbClkSrc::Rosc => rosc_freq,
             UsbClkSrc::Xosc => xosc_freq,
+            // See above re gpin handling being commented out
             // UsbClkSrc::Gpin0 => gpin0_freq,
             // UsbClkSrc::Gpin1 => gpin1_freq,
         };
@@ -631,6 +1138,7 @@ pub(crate) unsafe fn init(config: ClockConfig) {
             AdcClkSrc::PllSys => pll_sys_freq,
             AdcClkSrc::Rosc => rosc_freq,
             AdcClkSrc::Xosc => xosc_freq,
+            // See above re gpin handling being commented out
             // AdcClkSrc::Gpin0 => gpin0_freq,
             // AdcClkSrc::Gpin1 => gpin1_freq,
         };
@@ -659,6 +1167,7 @@ pub(crate) unsafe fn init(config: ClockConfig) {
             RtcClkSrc::PllSys => pll_sys_freq,
             RtcClkSrc::Rosc => rosc_freq,
             RtcClkSrc::Xosc => xosc_freq,
+            // See above re gpin handling being commented out
             // RtcClkSrc::Gpin0 => gpin0_freq,
             // RtcClkSrc::Gpin1 => gpin1_freq,
         };
@@ -725,6 +1234,7 @@ pub fn xosc_freq() -> u32 {
     CLOCKS.xosc.load(Ordering::Relaxed)
 }
 
+// See above re gpin handling being commented out
 // pub fn gpin0_freq() -> u32 {
 //     CLOCKS.gpin0.load(Ordering::Relaxed)
 // }
@@ -783,46 +1293,100 @@ fn start_xosc(crystal_hz: u32, delay_multiplier: u32) {
     while !pac::XOSC.status().read().stable() {}
 }
 
+/// PLL (Phase-Locked Loop) configuration
 #[inline(always)]
-fn configure_pll(p: pac::pll::Pll, input_freq: u32, config: PllConfig) -> u32 {
+fn configure_pll(p: pac::pll::Pll, input_freq: u32, config: PllConfig) -> Result<u32, &'static str> {
+    // Calculate reference frequency
     let ref_freq = input_freq / config.refdiv as u32;
+
+    // Validate PLL parameters
+    // Feedback divider (FBDIV) must be between 16 and 320
     assert!(config.fbdiv >= 16 && config.fbdiv <= 320);
+
+    // Post divider 1 (POSTDIV1) must be between 1 and 7
     assert!(config.post_div1 >= 1 && config.post_div1 <= 7);
+
+    // Post divider 2 (POSTDIV2) must be between 1 and 7
     assert!(config.post_div2 >= 1 && config.post_div2 <= 7);
+
+    // Post divider 2 (POSTDIV2) must be less than or equal to post divider 1 (POSTDIV1)
+    assert!(config.post_div2 <= config.post_div1);
+
+    // Reference divider (REFDIV) must be between 1 and 63
     assert!(config.refdiv >= 1 && config.refdiv <= 63);
+
+    // Reference frequency (REF_FREQ) must be between 5MHz and 800MHz
     assert!(ref_freq >= 5_000_000 && ref_freq <= 800_000_000);
+
+    // Calculate VCO frequency
     let vco_freq = ref_freq.saturating_mul(config.fbdiv as u32);
+
+    // VCO (Voltage Controlled Oscillator) frequency must be between 750MHz and 1800MHz
     assert!(vco_freq >= 750_000_000 && vco_freq <= 1_800_000_000);
 
-    // Load VCO-related dividers before starting VCO
+    // We follow the SDK's approach to PLL configuration which is:
+    // 1. Power down PLL
+    // 2. Configure the reference divider
+    // 3. Configure the feedback divider
+    // 4. Power up PLL and VCO
+    // 5. Wait for PLL to lock
+    // 6. Configure post-dividers
+    // 7. Enable post-divider output
+
+    // 1. Power down PLL before configuration
+    p.pwr().write(|w| {
+        w.set_pd(true); // Power down the PLL
+        w.set_vcopd(true); // Power down the VCO
+        w.set_postdivpd(true); // Power down the post divider
+        w.set_dsmpd(true); // Disable fractional mode
+        *w
+    });
+
+    // Short delay after powering down
+    cortex_m::asm::delay(10);
+
+    // 2. Configure reference divider first
     p.cs().write(|w| w.set_refdiv(config.refdiv as _));
+
+    // 3. Configure feedback divider
     p.fbdiv_int().write(|w| w.set_fbdiv_int(config.fbdiv));
 
-    // Turn on PLL
-    let pwr = p.pwr().write(|w| {
-        w.set_dsmpd(true); // "nothing is achieved by setting this low"
-        w.set_pd(false);
-        w.set_vcopd(false);
-        w.set_postdivpd(true);
+    // 4. Power up PLL and VCO, but keep post divider powered down during initial lock
+    p.pwr().write(|w| {
+        w.set_pd(false); // Power up the PLL
+        w.set_vcopd(false); // Power up the VCO
+        w.set_postdivpd(true); // Keep post divider powered down during initial lock
+        w.set_dsmpd(true); // Disable fractional mode (simpler configuration)
         *w
     });
 
-    // Wait for PLL to lock
-    while !p.cs().read().lock() {}
+    // 5. Wait for PLL to lock with a timeout
+    let mut timeout = 1_000_000;
+    while !p.cs().read().lock() {
+        timeout -= 1;
+        if timeout == 0 {
+            // PLL failed to lock, return 0 to indicate failure
+            return Err("PLL failed to lock");
+        }
+    }
 
-    // Set post-dividers
+    // 6. Configure post dividers after PLL is locked
     p.prim().write(|w| {
         w.set_postdiv1(config.post_div1);
         w.set_postdiv2(config.post_div2);
     });
 
-    // Turn on post divider
-    p.pwr().write(|w| {
-        *w = pwr;
-        w.set_postdivpd(false);
+    // 7. Enable the post divider output
+    p.pwr().modify(|w| {
+        w.set_postdivpd(false); // Power up post divider
+        *w
     });
 
-    vco_freq / ((config.post_div1 * config.post_div2) as u32)
+    // Final delay to ensure everything is stable
+    cortex_m::asm::delay(100);
+
+    // Calculate and return actual output frequency
+    Ok(vco_freq / ((config.post_div1 * config.post_div2) as u32))
 }
 
 /// General purpose input clock pin.
@@ -906,6 +1470,7 @@ impl_gpoutpin!(PIN_25, 3);
 pub enum GpoutSrc {
     /// Sys PLL.
     PllSys = ClkGpoutCtrlAuxsrc::CLKSRC_PLL_SYS as _,
+    // See above re gpin handling being commented out
     // Gpin0 = ClkGpoutCtrlAuxsrc::CLKSRC_GPIN0 as _ ,
     // Gpin1 = ClkGpoutCtrlAuxsrc::CLKSRC_GPIN1 as _ ,
     /// USB PLL.
@@ -1001,6 +1566,7 @@ impl<'d, T: GpoutPin> Gpout<'d, T> {
 
         let base = match src {
             ClkGpoutCtrlAuxsrc::CLKSRC_PLL_SYS => pll_sys_freq(),
+            // See above re gpin handling being commented out
             // ClkGpoutCtrlAuxsrc::CLKSRC_GPIN0 => gpin0_freq(),
             // ClkGpoutCtrlAuxsrc::CLKSRC_GPIN1 => gpin1_freq(),
             ClkGpoutCtrlAuxsrc::CLKSRC_PLL_USB => pll_usb_freq(),
@@ -1009,7 +1575,6 @@ impl<'d, T: GpoutPin> Gpout<'d, T> {
             ClkGpoutCtrlAuxsrc::CLK_SYS => clk_sys_freq(),
             ClkGpoutCtrlAuxsrc::CLK_USB => clk_usb_freq(),
             ClkGpoutCtrlAuxsrc::CLK_ADC => clk_adc_freq(),
-            //ClkGpoutCtrlAuxsrc::CLK_RTC => clk_rtc_freq() as _,
             ClkGpoutCtrlAuxsrc::CLK_REF => clk_ref_freq(),
             _ => unreachable!(),
         };
@@ -1069,6 +1634,7 @@ impl rand_core::RngCore for RoscRng {
         dest.fill_with(Self::next_u8)
     }
 }
+
 /// Enter the `DORMANT` sleep state. This will stop *all* internal clocks
 /// and can only be exited through resets, dormant-wake GPIO interrupts,
 /// and RTC interrupts. If RTC is clocked from an internal clock source
@@ -1197,3 +1763,196 @@ pub fn dormant_sleep() {
         }
     }
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[cfg(feature = "rp2040")]
+    #[test]
+    fn test_find_pll_params() {
+        #[cfg(feature = "rp2040")]
+        {
+            // Test standard 125 MHz configuration with 12 MHz crystal
+            let params = find_pll_params(12_000_000, 125_000_000).unwrap();
+            assert_eq!(params.refdiv, 1);
+            assert_eq!(params.fbdiv, 125);
+
+            // Test USB PLL configuration for 48MHz
+            // The algorithm may find different valid parameters than the SDK defaults
+            // We'll check that it generates a valid configuration that produces 48MHz
+            let params = find_pll_params(12_000_000, 48_000_000).unwrap();
+            assert_eq!(params.refdiv, 1);
+
+            // Calculate the actual output frequency
+            let ref_freq = 12_000_000 / params.refdiv as u32;
+            let vco_freq = ref_freq as u64 * params.fbdiv as u64;
+            let output_freq = (vco_freq / ((params.post_div1 * params.post_div2) as u64)) as u32;
+
+            // Verify the output frequency is correct
+            assert_eq!(output_freq, 48_000_000);
+
+            // Verify VCO frequency is in valid range
+            assert!(vco_freq >= 750_000_000 && vco_freq <= 1_800_000_000);
+
+            // Test overclocked configuration for 200 MHz
+            let params = find_pll_params(12_000_000, 200_000_000).unwrap();
+            assert_eq!(params.refdiv, 1);
+            let vco_freq = 12_000_000 as u64 * params.fbdiv as u64;
+            let output_freq = (vco_freq / ((params.post_div1 * params.post_div2) as u64)) as u32;
+            assert_eq!(output_freq, 200_000_000);
+            assert!(vco_freq >= 750_000_000 && vco_freq <= 1_800_000_000); // VCO in valid range
+
+            // Test non-standard crystal with 16 MHz
+            let params = find_pll_params(16_000_000, 125_000_000).unwrap();
+            let vco_freq = (16_000_000 / params.refdiv as u32) as u64 * params.fbdiv as u64;
+            let output_freq = (vco_freq / ((params.post_div1 * params.post_div2) as u64)) as u32;
+
+            // Test non-standard crystal with 15 MHz
+            let params = find_pll_params(15_000_000, 125_000_000).unwrap();
+            let vco_freq = (15_000_000 / params.refdiv as u32) as u64 * params.fbdiv as u64;
+            let output_freq = (vco_freq / ((params.post_div1 * params.post_div2) as u64)) as u32;
+
+            // With a 15 MHz crystal, we might not get exactly 125 MHz
+            // Check that it's close enough (within 0.2% margin)
+            let freq_diff = if output_freq > 125_000_000 {
+                output_freq - 125_000_000
+            } else {
+                125_000_000 - output_freq
+            };
+            let error_percentage = (freq_diff as f64 / 125_000_000.0) * 100.0;
+            assert!(
+                error_percentage < 0.2,
+                "Output frequency {} is not close enough to target 125 MHz. Error: {:.2}%",
+                output_freq,
+                error_percentage
+            );
+
+            assert!(vco_freq >= 750_000_000 && vco_freq <= 1_800_000_000);
+        }
+    }
+
+    #[cfg(feature = "rp2040")]
+    #[test]
+    fn test_pll_config_validation() {
+        // Test PLL configuration validation logic
+        let valid_config = PllConfig {
+            refdiv: 1,
+            fbdiv: 125,
+            post_div1: 6,
+            post_div2: 2,
+        };
+
+        // Valid configuration should pass validation
+        assert!(valid_config.is_valid(12_000_000));
+
+        // Test fbdiv constraints
+        let mut invalid_config = valid_config;
+        invalid_config.fbdiv = 15; // Below minimum of 16
+        assert!(!invalid_config.is_valid(12_000_000));
+
+        invalid_config.fbdiv = 321; // Above maximum of 320
+        assert!(!invalid_config.is_valid(12_000_000));
+
+        // Test post_div constraints
+        invalid_config = valid_config;
+        invalid_config.post_div1 = 0; // Below minimum of 1
+        assert!(!invalid_config.is_valid(12_000_000));
+
+        invalid_config = valid_config;
+        invalid_config.post_div1 = 8; // Above maximum of 7
+        assert!(!invalid_config.is_valid(12_000_000));
+
+        // Test post_div2 must be <= post_div1
+        invalid_config = valid_config;
+        invalid_config.post_div2 = 7;
+        invalid_config.post_div1 = 3;
+        assert!(!invalid_config.is_valid(12_000_000));
+
+        // Test reference frequency constraints
+        invalid_config = valid_config;
+        assert!(!invalid_config.is_valid(4_000_000)); // Below minimum of 5 MHz
+        assert!(!invalid_config.is_valid(900_000_000)); // Above maximum of 800 MHz
+
+        // Test VCO frequency constraints
+        invalid_config = valid_config;
+        invalid_config.fbdiv = 16;
+        assert!(!invalid_config.is_valid(12_000_000)); // VCO too low: 12MHz * 16 = 192MHz
+
+        // Test VCO frequency constraints - too high
+        invalid_config = valid_config;
+        invalid_config.fbdiv = 200;
+        invalid_config.refdiv = 1;
+        // This should be INVALID: 12MHz * 200 = 2400MHz exceeds max VCO of 1800MHz
+        assert!(!invalid_config.is_valid(12_000_000));
+
+        // Test a valid high VCO configuration
+        invalid_config.fbdiv = 150; // 12MHz * 150 = 1800MHz, exactly at the limit
+        assert!(invalid_config.is_valid(12_000_000));
+    }
+
+    #[cfg(feature = "rp2040")]
+    #[test]
+    fn test_manual_pll_helper() {
+        {
+            // Test the new manual_pll helper method
+            let config = ClockConfig::manual_pll(
+                12_000_000,
+                PllConfig {
+                    refdiv: 1,
+                    fbdiv: 100,
+                    post_div1: 3,
+                    post_div2: 2,
+                },
+                CoreVoltage::V1_15,
+            );
+
+            // Check voltage scale was set correctly
+            assert_eq!(config.core_voltage, CoreVoltage::V1_15);
+
+            // Check PLL config was set correctly
+            assert_eq!(config.xosc.as_ref().unwrap().sys_pll.as_ref().unwrap().refdiv, 1);
+            assert_eq!(config.xosc.as_ref().unwrap().sys_pll.as_ref().unwrap().fbdiv, 100);
+            assert_eq!(config.xosc.as_ref().unwrap().sys_pll.as_ref().unwrap().post_div1, 3);
+            assert_eq!(config.xosc.as_ref().unwrap().sys_pll.as_ref().unwrap().post_div2, 2);
+
+            // Check we get the expected frequency
+            assert_eq!(
+                config
+                    .xosc
+                    .as_ref()
+                    .unwrap()
+                    .sys_pll
+                    .as_ref()
+                    .unwrap()
+                    .output_frequency(12_000_000),
+                200_000_000
+            );
+        }
+    }
+
+    #[cfg(feature = "rp2040")]
+    #[test]
+    fn test_auto_voltage_scaling() {
+        {
+            // Test automatic voltage scaling based on frequency
+            // Under 133 MHz should use default voltage (V1_10)
+            let config = ClockConfig::system_freq(125_000_000);
+            assert_eq!(config.core_voltage, CoreVoltage::V1_10);
+
+            // 133-200 MHz should use V1_15
+            let config = ClockConfig::system_freq(150_000_000);
+            assert_eq!(config.core_voltage, CoreVoltage::V1_15);
+            let config = ClockConfig::system_freq(200_000_000);
+            assert_eq!(config.core_voltage, CoreVoltage::V1_15);
+
+            // Above 200 MHz should use V1_25
+            let config = ClockConfig::system_freq(250_000_000);
+            assert_eq!(config.core_voltage, CoreVoltage::V1_15);
+
+            // Below 125 MHz should use V1_10
+            let config = ClockConfig::system_freq(100_000_000);
+            assert_eq!(config.core_voltage, CoreVoltage::V1_10);
+        }
+    }
+}
diff --git a/embassy-rp/src/pio_programs/clock_divider.rs b/embassy-rp/src/pio_programs/clock_divider.rs
new file mode 100644
index 000000000..02e353f53
--- /dev/null
+++ b/embassy-rp/src/pio_programs/clock_divider.rs
@@ -0,0 +1,25 @@
+//! Helper functions for calculating PIO clock dividers
+
+use fixed::traits::ToFixed;
+use fixed::types::extra::U8;
+
+use crate::clocks::clk_sys_freq;
+
+/// Calculate a PIO clock divider value based on the desired target frequency.
+///
+/// # Arguments
+///
+/// * `target_hz` - The desired PIO clock frequency in Hz
+///
+/// # Returns
+///
+/// A fixed-point divider value suitable for use in a PIO state machine configuration
+#[inline]
+pub fn calculate_pio_clock_divider(target_hz: u32) -> fixed::FixedU32<U8> {
+    // Requires a non-zero frequency
+    assert!(target_hz > 0, "PIO clock frequency cannot be zero");
+
+    // Calculate the divider
+    let divider = (clk_sys_freq() + target_hz / 2) / target_hz;
+    divider.to_fixed()
+}
diff --git a/embassy-rp/src/pio_programs/hd44780.rs b/embassy-rp/src/pio_programs/hd44780.rs
index 5846a8027..546c85a89 100644
--- a/embassy-rp/src/pio_programs/hd44780.rs
+++ b/embassy-rp/src/pio_programs/hd44780.rs
@@ -5,6 +5,7 @@ use crate::pio::{
     Common, Config, Direction, FifoJoin, Instance, Irq, LoadedProgram, PioPin, ShiftConfig, ShiftDirection,
     StateMachine,
 };
+use crate::pio_programs::clock_divider::calculate_pio_clock_divider;
 use crate::Peri;
 
 /// This struct represents a HD44780 program that takes command words (<wait:24> <command:4> <0:4>)
@@ -134,7 +135,10 @@ impl<'l, P: Instance, const S: usize> PioHD44780<'l, P, S> {
 
         let mut cfg = Config::default();
         cfg.use_program(&word_prg.prg, &[&e]);
-        cfg.clock_divider = 125u8.into();
+
+        // Target 1 MHz PIO clock (each cycle is 1µs)
+        cfg.clock_divider = calculate_pio_clock_divider(1_000_000);
+
         cfg.set_out_pins(&[&db4, &db5, &db6, &db7]);
         cfg.shift_out = ShiftConfig {
             auto_fill: true,
@@ -160,7 +164,10 @@ impl<'l, P: Instance, const S: usize> PioHD44780<'l, P, S> {
 
         let mut cfg = Config::default();
         cfg.use_program(&seq_prg.prg, &[&e]);
-        cfg.clock_divider = 8u8.into(); // ~64ns/insn
+
+        // Target ~15.6 MHz PIO clock (~64ns/insn)
+        cfg.clock_divider = calculate_pio_clock_divider(15_600_000);
+
         cfg.set_jmp_pin(&db7);
         cfg.set_set_pins(&[&rs, &rw]);
         cfg.set_out_pins(&[&db4, &db5, &db6, &db7]);
diff --git a/embassy-rp/src/pio_programs/mod.rs b/embassy-rp/src/pio_programs/mod.rs
index 74537825b..8eac328b3 100644
--- a/embassy-rp/src/pio_programs/mod.rs
+++ b/embassy-rp/src/pio_programs/mod.rs
@@ -1,5 +1,6 @@
 //! Pre-built pio programs for common interfaces
 
+pub mod clock_divider;
 pub mod hd44780;
 pub mod i2s;
 pub mod onewire;
diff --git a/embassy-rp/src/pio_programs/rotary_encoder.rs b/embassy-rp/src/pio_programs/rotary_encoder.rs
index e520da8a3..70b3795e9 100644
--- a/embassy-rp/src/pio_programs/rotary_encoder.rs
+++ b/embassy-rp/src/pio_programs/rotary_encoder.rs
@@ -1,11 +1,10 @@
 //! PIO backed quadrature encoder
 
-use fixed::traits::ToFixed;
-
 use crate::gpio::Pull;
 use crate::pio::{
     Common, Config, Direction as PioDirection, FifoJoin, Instance, LoadedProgram, PioPin, ShiftDirection, StateMachine,
 };
+use crate::pio_programs::clock_divider::calculate_pio_clock_divider;
 use crate::Peri;
 
 /// This struct represents an Encoder program loaded into pio instruction memory.
@@ -48,7 +47,10 @@ impl<'d, T: Instance, const SM: usize> PioEncoder<'d, T, SM> {
         cfg.set_in_pins(&[&pin_a, &pin_b]);
         cfg.fifo_join = FifoJoin::RxOnly;
         cfg.shift_in.direction = ShiftDirection::Left;
-        cfg.clock_divider = 10_000.to_fixed();
+
+        // Target 12.5 KHz PIO clock
+        cfg.clock_divider = calculate_pio_clock_divider(12_500);
+
         cfg.use_program(&program.prg, &[]);
         sm.set_config(&cfg);
         sm.set_enable(true);
diff --git a/embassy-rp/src/pio_programs/stepper.rs b/embassy-rp/src/pio_programs/stepper.rs
index 495191659..0e9a8daf9 100644
--- a/embassy-rp/src/pio_programs/stepper.rs
+++ b/embassy-rp/src/pio_programs/stepper.rs
@@ -2,11 +2,8 @@
 
 use core::mem::{self, MaybeUninit};
 
-use fixed::traits::ToFixed;
-use fixed::types::extra::U8;
-use fixed::FixedU32;
-
 use crate::pio::{Common, Config, Direction, Instance, Irq, LoadedProgram, PioPin, StateMachine};
+use crate::pio_programs::clock_divider::calculate_pio_clock_divider;
 use crate::Peri;
 
 /// This struct represents a Stepper driver program loaded into pio instruction memory.
@@ -64,7 +61,9 @@ impl<'d, T: Instance, const SM: usize> PioStepper<'d, T, SM> {
         sm.set_pin_dirs(Direction::Out, &[&pin0, &pin1, &pin2, &pin3]);
         let mut cfg = Config::default();
         cfg.set_out_pins(&[&pin0, &pin1, &pin2, &pin3]);
-        cfg.clock_divider = (125_000_000 / (100 * 136)).to_fixed();
+
+        cfg.clock_divider = calculate_pio_clock_divider(100 * 136);
+
         cfg.use_program(&program.prg, &[]);
         sm.set_config(&cfg);
         sm.set_enable(true);
@@ -73,9 +72,11 @@ impl<'d, T: Instance, const SM: usize> PioStepper<'d, T, SM> {
 
     /// Set pulse frequency
     pub fn set_frequency(&mut self, freq: u32) {
-        let clock_divider: FixedU32<U8> = (125_000_000 / (freq * 136)).to_fixed();
-        assert!(clock_divider <= 65536, "clkdiv must be <= 65536");
-        assert!(clock_divider >= 1, "clkdiv must be >= 1");
+        let clock_divider = calculate_pio_clock_divider(freq * 136);
+        let divider_f32 = clock_divider.to_num::<f32>();
+        assert!(divider_f32 <= 65536.0, "clkdiv must be <= 65536");
+        assert!(divider_f32 >= 1.0, "clkdiv must be >= 1");
+
         self.sm.set_clock_divider(clock_divider);
         self.sm.clkdiv_restart();
     }