From b38d496d519f87d7f455e6c6b32f5d6379af44a5 Mon Sep 17 00:00:00 2001
From: pennae <github@quasiparticle.net>
Date: Sat, 6 May 2023 20:53:06 +0200
Subject: [PATCH] rp/pio: allow wrap-around program loading

execution wraps around after the end of instruction memory and wrapping
works with this, so we may as well allow program loading across this
boundary. could be useful for reusing chunks of instruction memory.
---
 embassy-rp/src/pio.rs              | 11 ++++++-----
 embassy-rp/src/relocate.rs         | 10 +++-------
 examples/rp/src/bin/pio_hd44780.rs |  3 ++-
 3 files changed, 11 insertions(+), 13 deletions(-)

diff --git a/embassy-rp/src/pio.rs b/embassy-rp/src/pio.rs
index 5fed4f1fa..fe66d9aef 100644
--- a/embassy-rp/src/pio.rs
+++ b/embassy-rp/src/pio.rs
@@ -855,13 +855,14 @@ impl<'d, PIO: Instance> Common<'d, PIO> {
     {
         let mut used_mask = 0;
         for (i, instr) in instrs.enumerate() {
-            let addr = (i + start) as u8;
-            let mask = 1 << (addr as usize);
-            if self.instructions_used & mask != 0 {
-                return Err(addr as usize);
+            // wrapping around the end of program memory is valid, let's make use of that.
+            let addr = (i + start) % 32;
+            let mask = 1 << addr;
+            if (self.instructions_used | used_mask) & mask != 0 {
+                return Err(addr);
             }
             unsafe {
-                PIO::PIO.instr_mem(addr as usize).write(|w| {
+                PIO::PIO.instr_mem(addr).write(|w| {
                     w.set_instr_mem(instr);
                 });
             }
diff --git a/embassy-rp/src/relocate.rs b/embassy-rp/src/relocate.rs
index f36170e03..9cb279ccd 100644
--- a/embassy-rp/src/relocate.rs
+++ b/embassy-rp/src/relocate.rs
@@ -26,11 +26,7 @@ where
             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"
-                );
+                let address = address.wrapping_add(self.offset) % 32;
                 instr & (!0b11111) | address as u16
             } else {
                 instr
@@ -62,8 +58,8 @@ impl<'a, const PROGRAM_SIZE: usize> RelocatedProgram<'a, PROGRAM_SIZE> {
         let wrap = self.program.wrap;
         let origin = self.origin;
         Wrap {
-            source: wrap.source + origin,
-            target: wrap.target + origin,
+            source: wrap.source.wrapping_add(origin) % 32,
+            target: wrap.target.wrapping_add(origin) % 32,
         }
     }
 
diff --git a/examples/rp/src/bin/pio_hd44780.rs b/examples/rp/src/bin/pio_hd44780.rs
index 61c5565d3..088fd5649 100644
--- a/examples/rp/src/bin/pio_hd44780.rs
+++ b/examples/rp/src/bin/pio_hd44780.rs
@@ -95,6 +95,7 @@ impl<'l> HD44780<'l> {
         let prg = pio_proc::pio_asm!(
             r#"
                 .side_set 1 opt
+                .origin 20
 
                 loop:
                     out x,     24
@@ -148,7 +149,7 @@ impl<'l> HD44780<'l> {
         // many side sets are only there to free up a delay bit!
         let prg = pio_proc::pio_asm!(
             r#"
-                .origin 7
+                .origin 27
                 .side_set 1
 
                 .wrap_target