Move pio programs into embassy-rp

1 files changed, 8 insertions, 127 deletions

diff --git a/examples/rp/src/bin/pio_stepper.rs b/examples/rp/src/bin/pio_stepper.rs
index 6ee45a414..3862c248b 100644
--- a/examples/rp/src/bin/pio_stepper.rs
+++ b/examples/rp/src/bin/pio_stepper.rs
@@ -3,143 +3,20 @@
 
 #![no_std]
 #![no_main]
-use core::mem::{self, MaybeUninit};
 
 use defmt::info;
 use embassy_executor::Spawner;
 use embassy_rp::bind_interrupts;
 use embassy_rp::peripherals::PIO0;
-use embassy_rp::pio::{Common, Config, Direction, Instance, InterruptHandler, Irq, Pio, PioPin, StateMachine};
+use embassy_rp::pio::{InterruptHandler, Pio};
+use embassy_rp::pio_programs::stepper::{PioStepper, PioStepperProgram};
 use embassy_time::{with_timeout, Duration, Timer};
-use fixed::traits::ToFixed;
-use fixed::types::extra::U8;
-use fixed::FixedU32;
 use {defmt_rtt as _, panic_probe as _};
 
 bind_interrupts!(struct Irqs {
     PIO0_IRQ_0 => InterruptHandler<PIO0>;
 });
 
-pub struct PioStepper<'d, T: Instance, const SM: usize> {
-    irq: Irq<'d, T, SM>,
-    sm: StateMachine<'d, T, SM>,
-}
-
-impl<'d, T: Instance, const SM: usize> PioStepper<'d, T, SM> {
-    pub fn new(
-        pio: &mut Common<'d, T>,
-        mut sm: StateMachine<'d, T, SM>,
-        irq: Irq<'d, T, SM>,
-        pin0: impl PioPin,
-        pin1: impl PioPin,
-        pin2: impl PioPin,
-        pin3: impl PioPin,
-    ) -> Self {
-        let prg = pio_proc::pio_asm!(
-            "pull block",
-            "mov x, osr",
-            "pull block",
-            "mov y, osr",
-            "jmp !x end",
-            "loop:",
-            "jmp !osre step",
-            "mov osr, y",
-            "step:",
-            "out pins, 4 [31]"
-            "jmp x-- loop",
-            "end:",
-            "irq 0 rel"
-        );
-        let pin0 = pio.make_pio_pin(pin0);
-        let pin1 = pio.make_pio_pin(pin1);
-        let pin2 = pio.make_pio_pin(pin2);
-        let pin3 = pio.make_pio_pin(pin3);
-        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.use_program(&pio.load_program(&prg.program), &[]);
-        sm.set_config(&cfg);
-        sm.set_enable(true);
-        Self { irq, 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");
-        self.sm.set_clock_divider(clock_divider);
-        self.sm.clkdiv_restart();
-    }
-
-    // Full step, one phase
-    pub async fn step(&mut self, steps: i32) {
-        if steps > 0 {
-            self.run(steps, 0b1000_0100_0010_0001_1000_0100_0010_0001).await
-        } else {
-            self.run(-steps, 0b0001_0010_0100_1000_0001_0010_0100_1000).await
-        }
-    }
-
-    // Full step, two phase
-    pub async fn step2(&mut self, steps: i32) {
-        if steps > 0 {
-            self.run(steps, 0b1001_1100_0110_0011_1001_1100_0110_0011).await
-        } else {
-            self.run(-steps, 0b0011_0110_1100_1001_0011_0110_1100_1001).await
-        }
-    }
-
-    // Half step
-    pub async fn step_half(&mut self, steps: i32) {
-        if steps > 0 {
-            self.run(steps, 0b1001_1000_1100_0100_0110_0010_0011_0001).await
-        } else {
-            self.run(-steps, 0b0001_0011_0010_0110_0100_1100_1000_1001).await
-        }
-    }
-
-    async fn run(&mut self, steps: i32, pattern: u32) {
-        self.sm.tx().wait_push(steps as u32).await;
-        self.sm.tx().wait_push(pattern).await;
-        let drop = OnDrop::new(|| {
-            self.sm.clear_fifos();
-            unsafe {
-                self.sm.exec_instr(
-                    pio::InstructionOperands::JMP {
-                        address: 0,
-                        condition: pio::JmpCondition::Always,
-                    }
-                    .encode(),
-                );
-            }
-        });
-        self.irq.wait().await;
-        drop.defuse();
-    }
-}
-
-struct OnDrop<F: FnOnce()> {
-    f: MaybeUninit<F>,
-}
-
-impl<F: FnOnce()> OnDrop<F> {
-    pub fn new(f: F) -> Self {
-        Self { f: MaybeUninit::new(f) }
-    }
-
-    pub fn defuse(self) {
-        mem::forget(self)
-    }
-}
-
-impl<F: FnOnce()> Drop for OnDrop<F> {
-    fn drop(&mut self) {
-        unsafe { self.f.as_ptr().read()() }
-    }
-}
-
 #[embassy_executor::main]
 async fn main(_spawner: Spawner) {
     let p = embassy_rp::init(Default::default());
@@ -147,14 +24,18 @@ async fn main(_spawner: Spawner) {
         mut common, irq0, sm0, ..
     } = Pio::new(p.PIO0, Irqs);
 
-    let mut stepper = PioStepper::new(&mut common, sm0, irq0, p.PIN_4, p.PIN_5, p.PIN_6, p.PIN_7);
+    let prg = PioStepperProgram::new(&mut common);
+    let mut stepper = PioStepper::new(&mut common, sm0, irq0, p.PIN_4, p.PIN_5, p.PIN_6, p.PIN_7, &prg);
     stepper.set_frequency(120);
     loop {
         info!("CW full steps");
         stepper.step(1000).await;
 
         info!("CCW full steps, drop after 1 sec");
-        if let Err(_) = with_timeout(Duration::from_secs(1), stepper.step(-i32::MAX)).await {
+        if with_timeout(Duration::from_secs(1), stepper.step(-i32::MAX))
+            .await
+            .is_err()
+        {
             info!("Time's up!");
             Timer::after(Duration::from_secs(1)).await;
         }