5 files changed, 43 insertions, 25 deletions

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 3aa54495f..546c85a89 100644
--- a/embassy-rp/src/pio_programs/hd44780.rs
+++ b/embassy-rp/src/pio_programs/hd44780.rs
@@ -1,11 +1,11 @@
 //! [HD44780 display driver](https://www.sparkfun.com/datasheets/LCD/HD44780.pdf)
 
-use crate::clocks::clk_sys_freq;
 use crate::dma::{AnyChannel, Channel};
 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>)
@@ -136,10 +136,8 @@ impl<'l, P: Instance, const S: usize> PioHD44780<'l, P, S> {
         let mut cfg = Config::default();
         cfg.use_program(&word_prg.prg, &[&e]);
 
-        // Scale the divider based on system clock frequency
-        // Original: 125 at 125 MHz (1 MHz PIO clock)
-        let word_divider = (clk_sys_freq() / 1_000_000) as u8; // Target 1 MHz PIO clock
-        cfg.clock_divider = word_divider.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 {
@@ -167,10 +165,8 @@ impl<'l, P: Instance, const S: usize> PioHD44780<'l, P, S> {
         let mut cfg = Config::default();
         cfg.use_program(&seq_prg.prg, &[&e]);
 
-        // Original: 8 at 125 MHz (~15.6 MHz PIO clock)
-        // Comment says ~64ns/insn which is 1/(15.6 MHz) = ~64ns
-        let seq_divider = (clk_sys_freq() / 15_600_000) as u8; // Target ~15.6 MHz PIO clock (~64ns/insn)
-        cfg.clock_divider = seq_divider.into();
+        // 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]);
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 71567a602..70b3795e9 100644
--- a/embassy-rp/src/pio_programs/rotary_encoder.rs
+++ b/embassy-rp/src/pio_programs/rotary_encoder.rs
@@ -1,12 +1,10 @@
 //! PIO backed quadrature encoder
 
-use fixed::traits::ToFixed;
-
-use crate::clocks::clk_sys_freq;
 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.
@@ -50,10 +48,8 @@ impl<'d, T: Instance, const SM: usize> PioEncoder<'d, T, SM> {
         cfg.fifo_join = FifoJoin::RxOnly;
         cfg.shift_in.direction = ShiftDirection::Left;
 
-        // Original: 10_000 at 125 MHz (12.5 KHz PIO clock)
-        // Scale divider to maintain same PIO clock frequency at different system clocks
-        let divider = (clk_sys_freq() as f32 / 12_500.0).to_fixed();
-        cfg.clock_divider = divider;
+        // Target 12.5 KHz PIO clock
+        cfg.clock_divider = calculate_pio_clock_divider(12_500);
 
         cfg.use_program(&program.prg, &[]);
         sm.set_config(&cfg);
diff --git a/embassy-rp/src/pio_programs/stepper.rs b/embassy-rp/src/pio_programs/stepper.rs
index 6878c32f5..0e9a8daf9 100644
--- a/embassy-rp/src/pio_programs/stepper.rs
+++ b/embassy-rp/src/pio_programs/stepper.rs
@@ -2,12 +2,8 @@
 
 use core::mem::{self, MaybeUninit};
 
-use fixed::traits::ToFixed;
-use fixed::types::extra::U8;
-use fixed::FixedU32;
-
-use crate::clocks::clk_sys_freq;
 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.
@@ -65,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 = (clk_sys_freq() / (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);
@@ -74,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> = (clk_sys_freq() / (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();
     }