5 files changed, 49 insertions, 13 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 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();
     }