1 files changed, 32 insertions, 11 deletions

diff --git a/embassy-rp/src/pio_programs/clock_divider.rs b/embassy-rp/src/pio_programs/clock_divider.rs
index 687fd53ba..28c931ed2 100644
--- a/embassy-rp/src/pio_programs/clock_divider.rs
+++ b/embassy-rp/src/pio_programs/clock_divider.rs
@@ -1,6 +1,5 @@
 //! Helper functions for calculating PIO clock dividers
 
-use fixed::traits::ToFixed;
 use fixed::types::extra::U8;
 
 use crate::clocks::clk_sys_freq;
@@ -14,39 +13,61 @@ use crate::clocks::clk_sys_freq;
 /// # 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> {
-    calculate_pio_clock_divider_inner(clk_sys_freq(), target_hz)
+    calculate_pio_clock_divider_value(clk_sys_freq(), target_hz)
 }
 
-#[inline]
-fn calculate_pio_clock_divider_inner(sys_freq: u32, target_hz: u32) -> fixed::FixedU32<U8> {
+/// Calculate a PIO clock divider value based on the desired target frequency.
+///
+/// # Arguments
+///
+/// * `sys_hz` - The system clock frequency in Hz
+/// * `target_hz` - The desired PIO clock frequency in Hz
+///
+/// # Returns
+///
+/// A fixed-point divider value suitable for use in a PIO state machine configuration
+pub const fn calculate_pio_clock_divider_value(sys_hz: u32, 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 = (sys_freq + target_hz / 2) / target_hz;
-    divider.to_fixed()
+    // Compute the integer and fractional part of the divider.
+    // Doing it this way allows us to avoid u64 division while
+    // maintaining precision.
+    let integer = sys_hz / target_hz;
+    let remainder = sys_hz % target_hz;
+    let frac = (remainder << 8) / target_hz;
+
+    let result = integer << 8 | frac;
+
+    // Ensure the result will fit in 16+8 bits.
+    assert!(result <= 0xffff_ff, "pio clock divider too big");
+    // The clock divider can't be used to go faster than the system clock.
+    assert!(result >= 0x0001_00, "pio clock divider cannot be less than 1");
+
+    fixed::FixedU32::from_bits(result)
 }
 
 #[cfg(test)]
 mod tests {
+    use fixed::traits::ToFixed;
+
     use super::*;
 
     #[test]
     fn clock_divider_math() {
         // A simple divider that must have a fractional part.
-        let divider = calculate_pio_clock_divider_inner(125_000_000, 40_000_000);
+        let divider = calculate_pio_clock_divider_value(125_000_000, 40_000_000);
         let expected: fixed::FixedU32<U8> = 3.125.to_fixed();
         assert_eq!(divider, expected);
 
         // A system clk so high it would overflow a u32 if shifted left.
-        let divider = calculate_pio_clock_divider_inner(2_000_000_000, 40_000);
+        let divider = calculate_pio_clock_divider_value(2_000_000_000, 40_000);
         let expected: fixed::FixedU32<U8> = 50000.to_fixed();
         assert_eq!(divider, expected);
 
         // A divider that requires all 8 fractional bits.
-        let divider = calculate_pio_clock_divider_inner(134_283_264, 16_777_216);
+        let divider = calculate_pio_clock_divider_value(134_283_264, 16_777_216);
         let expected: fixed::FixedU32<U8> = 8.00390625.to_fixed();
         assert_eq!(divider, expected);
     }