Merge pull request #5061 from ericseppanen/pio_dividerHEADmain

embassy_rp: improve calculate_pio_clock_divider

1 files changed, 55 insertions, 6 deletions

diff --git a/embassy-rp/src/pio_programs/clock_divider.rs b/embassy-rp/src/pio_programs/clock_divider.rs
index 02e353f53..87ba93008 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,12 +13,62 @@ 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_value(clk_sys_freq(), target_hz)
+}
+
+/// 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");
+    core::assert!(target_hz > 0);
+
+    // 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.
+    core::assert!(result <= 0xffff_ff);
+    // The clock divider can't be used to go faster than the system clock.
+    core::assert!(result >= 0x0001_00);
+
+    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_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_value(2_000_000_000, 40_000);
+        let expected: fixed::FixedU32<U8> = 50000.to_fixed();
+        assert_eq!(divider, expected);
 
-    // Calculate the divider
-    let divider = (clk_sys_freq() + target_hz / 2) / target_hz;
-    divider.to_fixed()
+        // A divider that requires all 8 fractional bits.
+        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);
+    }
 }