From cd59046e6c179809ee19fb2592e65cf5a0a07980 Mon Sep 17 00:00:00 2001 From: Simon Berg Date: Tue, 6 Dec 2022 21:42:30 +0100 Subject: [PATCH] Added RelocateProgram class for adjusting PIO-programs for different origins. --- embassy-rp/src/lib.rs | 2 + embassy-rp/src/pio.rs | 19 +++++--- embassy-rp/src/relocate.rs | 77 ++++++++++++++++++++++++++++++++ examples/rp/src/bin/pio_async.rs | 31 ++++++++----- examples/rp/src/bin/pio_dma.rs | 10 +++-- 5 files changed, 118 insertions(+), 21 deletions(-) create mode 100644 embassy-rp/src/relocate.rs diff --git a/embassy-rp/src/lib.rs b/embassy-rp/src/lib.rs index 67eb29d5..55139272 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 f049a16e..1a067e54 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(&mut self, start: usize, instrs: I) + where + I: Iterator, + { let _ = self; write_instr( Self::Pio::PIO_NO, @@ -1098,8 +1101,11 @@ impl PioCommon for PioCommonInstance { 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(pio_no: u8, start: usize, instrs: I, mem_user: u32) +where + I: Iterator, +{ + 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(&mut self, start: usize, instrs: I) + where + I: Iterator, + { 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 00000000..f36170e0 --- /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, +{ + iter: I, + offset: u8, +} + +impl<'a, I: Iterator> 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, +{ + type Item = u16; + fn next(&mut self) -> Option { + 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, + origin: u8, +} + +impl<'a, const PROGRAM_SIZE: usize> RelocatedProgram<'a, PROGRAM_SIZE> { + pub fn new(program: &Program) -> RelocatedProgram { + let origin = program.origin.unwrap_or(0); + RelocatedProgram { program, origin } + } + + pub fn new_with_origin(program: &Program, origin: u8) -> RelocatedProgram { + 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 e9211db3..45a8c73f 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, 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) { // 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) { "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()); - 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); + let pio::Wrap { source, target } = relocated.wrap(); + sm.set_wrap(source, target); + + 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 bdcdf200..b19ef408 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);