Added RelocateProgram class for adjusting PIO-programs for different origins.

5 files changed, 118 insertions, 21 deletions

diff --git a/embassy-rp/src/lib.rs b/embassy-rp/src/lib.rs
index 67eb29d5e..551392725 100644
--- a/embassy-rp/src/lib.rs
+++ b/embassy-rp/src/lib.rs
@@ -17,6 +17,8 @@ pub mod interrupt;
 pub mod pio;
 #[cfg(feature = "pio")]
 pub mod pio_instr_util;
+#[cfg(feature = "pio")]
+pub mod relocate;
 
 pub mod rom_data;
 pub mod rtc;
diff --git a/embassy-rp/src/pio.rs b/embassy-rp/src/pio.rs
index f049a16e7..1a067e54f 100644
--- a/embassy-rp/src/pio.rs
+++ b/embassy-rp/src/pio.rs
@@ -941,7 +941,10 @@ pub trait PioStateMachine: Sized + Unpin {
         }
     }
 
-    fn write_instr(&mut self, start: usize, instrs: &[u16]) {
+    fn write_instr<I>(&mut self, start: usize, instrs: I)
+    where
+        I: Iterator<Item = u16>,
+    {
         let _ = self;
         write_instr(
             Self::Pio::PIO_NO,
@@ -1098,8 +1101,11 @@ impl<PIO: PioInstance> PioCommon for PioCommonInstance<PIO> {
     type Pio = PIO;
 }
 
-fn write_instr(pio_no: u8, start: usize, instrs: &[u16], mem_user: u32) {
-    for (i, instr) in instrs.iter().enumerate() {
+fn write_instr<I>(pio_no: u8, start: usize, instrs: I, mem_user: u32)
+where
+    I: Iterator<Item = u16>,
+{
+    for (i, instr) in instrs.enumerate() {
         let addr = (i + start) as u8;
         assert!(
             instr_mem_is_free(pio_no, addr),
@@ -1108,7 +1114,7 @@ fn write_instr(pio_no: u8, start: usize, instrs: &[u16], mem_user: u32) {
         );
         unsafe {
             PIOS[pio_no as usize].instr_mem(addr as usize).write(|w| {
-                w.set_instr_mem(*instr);
+                w.set_instr_mem(instr);
             });
             instr_mem_set_status(pio_no, addr, mem_user);
         }
@@ -1118,7 +1124,10 @@ fn write_instr(pio_no: u8, start: usize, instrs: &[u16], mem_user: u32) {
 pub trait PioCommon: Sized {
     type Pio: PioInstance;
 
-    fn write_instr(&mut self, start: usize, instrs: &[u16]) {
+    fn write_instr<I>(&mut self, start: usize, instrs: I)
+    where
+        I: Iterator<Item = u16>,
+    {
         let _ = self;
         write_instr(Self::Pio::PIO_NO, start, instrs, MEM_USED_BY_COMMON);
     }
diff --git a/embassy-rp/src/relocate.rs b/embassy-rp/src/relocate.rs
new file mode 100644
index 000000000..f36170e03
--- /dev/null
+++ b/embassy-rp/src/relocate.rs
@@ -0,0 +1,77 @@
+use core::iter::Iterator;
+
+use pio::{Program, SideSet, Wrap};
+
+pub struct CodeIterator<'a, I>
+where
+    I: Iterator<Item = &'a u16>,
+{
+    iter: I,
+    offset: u8,
+}
+
+impl<'a, I: Iterator<Item = &'a u16>> CodeIterator<'a, I> {
+    pub fn new(iter: I, offset: u8) -> CodeIterator<'a, I> {
+        CodeIterator { iter, offset }
+    }
+}
+
+impl<'a, I> Iterator for CodeIterator<'a, I>
+where
+    I: Iterator<Item = &'a u16>,
+{
+    type Item = u16;
+    fn next(&mut self) -> Option<Self::Item> {
+        self.iter.next().and_then(|&instr| {
+            Some(if instr & 0b1110_0000_0000_0000 == 0 {
+                // this is a JMP instruction -> add offset to address
+                let address = (instr & 0b1_1111) as u8;
+                let address = address + self.offset;
+                assert!(
+                    address < pio::RP2040_MAX_PROGRAM_SIZE as u8,
+                    "Invalid JMP out of the program after offset addition"
+                );
+                instr & (!0b11111) | address as u16
+            } else {
+                instr
+            })
+        })
+    }
+}
+
+pub struct RelocatedProgram<'a, const PROGRAM_SIZE: usize> {
+    program: &'a Program<PROGRAM_SIZE>,
+    origin: u8,
+}
+
+impl<'a, const PROGRAM_SIZE: usize> RelocatedProgram<'a, PROGRAM_SIZE> {
+    pub fn new(program: &Program<PROGRAM_SIZE>) -> RelocatedProgram<PROGRAM_SIZE> {
+        let origin = program.origin.unwrap_or(0);
+        RelocatedProgram { program, origin }
+    }
+
+    pub fn new_with_origin(program: &Program<PROGRAM_SIZE>, origin: u8) -> RelocatedProgram<PROGRAM_SIZE> {
+        RelocatedProgram { program, origin }
+    }
+
+    pub fn code(&'a self) -> CodeIterator<'a, core::slice::Iter<'a, u16>> {
+        CodeIterator::new(self.program.code.iter(), self.origin)
+    }
+
+    pub fn wrap(&self) -> Wrap {
+        let wrap = self.program.wrap;
+        let origin = self.origin;
+        Wrap {
+            source: wrap.source + origin,
+            target: wrap.target + origin,
+        }
+    }
+
+    pub fn side_set(&self) -> SideSet {
+        self.program.side_set
+    }
+
+    pub fn origin(&self) -> u8 {
+        self.origin
+    }
+}
diff --git a/examples/rp/src/bin/pio_async.rs b/examples/rp/src/bin/pio_async.rs
index e9211db3b..45a8c73f7 100644
--- a/examples/rp/src/bin/pio_async.rs
+++ b/examples/rp/src/bin/pio_async.rs
@@ -6,6 +6,7 @@ use embassy_executor::Spawner;
 use embassy_rp::gpio::{AnyPin, Pin};
 use embassy_rp::pio::{Pio0, PioPeripherial, PioStateMachine, PioStateMachineInstance, ShiftDirection, Sm0, Sm1, Sm2};
 use embassy_rp::pio_instr_util;
+use embassy_rp::relocate::RelocatedProgram;
 use {defmt_rtt as _, panic_probe as _};
 
 #[embassy_executor::task]
@@ -21,15 +22,17 @@ async fn pio_task_sm0(mut sm: PioStateMachineInstance<Pio0, Sm0>, pin: AnyPin) {
         ".wrap",
     );
 
-    let origin = prg.program.origin.unwrap_or(0);
+    let relocated = RelocatedProgram::new(&prg.program);
     let out_pin = sm.make_pio_pin(pin);
     let pio_pins = [&out_pin];
     sm.set_out_pins(&pio_pins);
-    sm.write_instr(origin as usize, &prg.program.code);
-    pio_instr_util::exec_jmp(&mut sm, origin);
+    sm.write_instr(relocated.origin() as usize, relocated.code());
+    pio_instr_util::exec_jmp(&mut sm, relocated.origin());
     sm.set_clkdiv((125e6 / 20.0 / 2e2 * 256.0) as u32);
     sm.set_set_range(0, 1);
-    sm.set_wrap(prg.program.wrap.source + origin, prg.program.wrap.target + origin);
+    let pio::Wrap { source, target } = relocated.wrap();
+    sm.set_wrap(source, target);
+
     sm.set_autopull(true);
     sm.set_out_shift_dir(ShiftDirection::Left);
 
@@ -50,12 +53,14 @@ async fn pio_task_sm1(mut sm: PioStateMachineInstance<Pio0, Sm1>) {
     // Read 0b10101 repeatedly until ISR is full
     let prg = pio_proc::pio_asm!(".origin 8", "set x, 0x15", ".wrap_target", "in x, 5 [31]", ".wrap",);
 
-    let origin = prg.program.origin.unwrap_or(0);
-    sm.write_instr(origin as usize, &prg.program.code);
-    pio_instr_util::exec_jmp(&mut sm, origin);
+    let relocated = RelocatedProgram::new(&prg.program);
+    sm.write_instr(relocated.origin() as usize, relocated.code());
+    pio_instr_util::exec_jmp(&mut sm, relocated.origin());
     sm.set_clkdiv((125e6 / 2e3 * 256.0) as u32);
     sm.set_set_range(0, 0);
-    sm.set_wrap(prg.program.wrap.source + origin, prg.program.wrap.target + origin);
+    let pio::Wrap { source, target } = relocated.wrap();
+    sm.set_wrap(source, target);
+
     sm.set_autopush(true);
     sm.set_in_shift_dir(ShiftDirection::Right);
     sm.set_enable(true);
@@ -79,11 +84,13 @@ async fn pio_task_sm2(mut sm: PioStateMachineInstance<Pio0, Sm2>) {
         "irq 3 [15]",
         ".wrap",
     );
-    let origin = prg.program.origin.unwrap_or(0);
+    let relocated = RelocatedProgram::new(&prg.program);
+    sm.write_instr(relocated.origin() as usize, relocated.code());
+
+    let pio::Wrap { source, target } = relocated.wrap();
+    sm.set_wrap(source, target);
 
-    sm.write_instr(origin as usize, &prg.program.code);
-    sm.set_wrap(prg.program.wrap.source + origin, prg.program.wrap.target + origin);
-    pio_instr_util::exec_jmp(&mut sm, origin);
+    pio_instr_util::exec_jmp(&mut sm, relocated.origin());
     sm.set_clkdiv((125e6 / 2e3 * 256.0) as u32);
     sm.set_enable(true);
     loop {
diff --git a/examples/rp/src/bin/pio_dma.rs b/examples/rp/src/bin/pio_dma.rs
index bdcdf200c..b19ef4083 100644
--- a/examples/rp/src/bin/pio_dma.rs
+++ b/examples/rp/src/bin/pio_dma.rs
@@ -5,6 +5,7 @@ use defmt::info;
 use embassy_executor::Spawner;
 use embassy_futures::join::join;
 use embassy_rp::pio::{PioPeripherial, PioStateMachine, ShiftDirection};
+use embassy_rp::relocate::RelocatedProgram;
 use embassy_rp::{pio_instr_util, Peripheral};
 use {defmt_rtt as _, panic_probe as _};
 
@@ -32,11 +33,12 @@ async fn main(_spawner: Spawner) {
         ".wrap",
     );
 
-    let origin = prg.program.origin.unwrap_or(0);
-    sm.write_instr(origin as usize, &prg.program.code);
-    pio_instr_util::exec_jmp(&mut sm, origin);
+    let relocated = RelocatedProgram::new(&prg.program);
+    sm.write_instr(relocated.origin() as usize, relocated.code());
+    pio_instr_util::exec_jmp(&mut sm, relocated.origin());
     sm.set_clkdiv((125e6 / 10e3 * 256.0) as u32);
-    sm.set_wrap(prg.program.wrap.source + origin, prg.program.wrap.target + origin);
+    let pio::Wrap { source, target } = relocated.wrap();
+    sm.set_wrap(source, target);
     sm.set_autopull(true);
     sm.set_autopush(true);
     sm.set_pull_threshold(32);