From 302fd9a811c289daf682edc03baa0cc31f1b0c82 Mon Sep 17 00:00:00 2001
From: Graham Sanderson <graham.sanderson@raspberrypi.com>
Date: Fri, 14 Feb 2025 09:26:22 -0600
Subject: [PATCH] Fix `WAIT GPIO n` for n > 32 and when using gpio_base = 16
 (#2265)

- fix bug: pioasm was spilling bit 5 of arg2 into arg1
- make pio_add_program flip bit 5 of a WAIT GPIO instruction if gpio_base is 16

Note: pioasm will encode only the bits 4-0 of arg2 (the GPIO number), however it does indicate whether
0-15 means 0-15 or 32-47. This can only be fixed up when loading the program, and indeed once the GPIO
base is known. pio_can_add_program and friends already detected the cases where this was impossible
---
 .../hardware_pio/include/hardware/pio_instructions.h     | 4 ++++
 src/rp2_common/hardware_pio/pio.c                        | 9 +++++++++
 tools/pioasm/pio_assembler.cpp                           | 2 +-
 3 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/rp2_common/hardware_pio/include/hardware/pio_instructions.h b/src/rp2_common/hardware_pio/include/hardware/pio_instructions.h
index c27a4c17..67114560 100644
--- a/src/rp2_common/hardware_pio/include/hardware/pio_instructions.h
+++ b/src/rp2_common/hardware_pio/include/hardware/pio_instructions.h
@@ -79,6 +79,10 @@ static inline uint _pio_major_instr_bits(uint instr) {
     return instr & 0xe000u;
 }
 
+static inline uint _pio_arg1(uint instr) {
+    return (instr >> 5) & 0x7u;
+}
+
 static inline uint _pio_encode_instr_and_args(enum pio_instr_bits instr_bits, uint arg1, uint arg2) {
     valid_params_if(PIO_INSTRUCTIONS, arg1 <= 0x7);
 #if PARAM_ASSERTIONS_ENABLED(PIO_INSTRUCTIONS)
diff --git a/src/rp2_common/hardware_pio/pio.c b/src/rp2_common/hardware_pio/pio.c
index ed34cd81..33b03945 100644
--- a/src/rp2_common/hardware_pio/pio.c
+++ b/src/rp2_common/hardware_pio/pio.c
@@ -162,6 +162,15 @@ static int add_program_at_offset(PIO pio, const pio_program_t *program, uint off
     if (rc != 0) return rc;
     for (uint i = 0; i < program->length; ++i) {
         uint16_t instr = program->instructions[i];
+#if PICO_PIO_USE_GPIO_BASE
+        if (pio_instr_bits_wait == _pio_major_instr_bits(instr) && !((_pio_arg1(instr) & 3u))) {
+            // wait GIO will include only the 5 lower bits of the GPIO number, so if the GPIO
+            // base is 16 we need to flip bit 4 (which is equivalent to subtracting 16 from
+            // the original number 16-47 stored as 16-31 and 0-15)
+            static_assert(PIO_GPIOBASE_BITS == 16, ""); // only works for gpio base being 0 or 16
+            instr ^= pio_get_gpio_base(pio);
+        }
+#endif
         pio->instr_mem[offset + i] = pio_instr_bits_jmp != _pio_major_instr_bits(instr) ? instr : instr + offset;
     }
     uint32_t program_mask = (1u << program->length) - 1;
diff --git a/tools/pioasm/pio_assembler.cpp b/tools/pioasm/pio_assembler.cpp
index 3a8a3009..a36854b2 100644
--- a/tools/pioasm/pio_assembler.cpp
+++ b/tools/pioasm/pio_assembler.cpp
@@ -292,7 +292,7 @@ uint instruction::encode(program &program) {
         }
     }
     // note we store the 6th bit of arg2 above the 16 bits of instruction
-    return (((uint) raw.type) << 13u) | (((uint) _delay | (uint) _sideset) << 8u) | (raw.arg1 << 5u) | raw.arg2 | ((raw.arg2 >> 5) << 16);
+    return (((uint) raw.type) << 13u) | (((uint) _delay | (uint) _sideset) << 8u) | (raw.arg1 << 5u) | (raw.arg2 & 0x1fu) | ((raw.arg2 >> 5) << 16);
 }
 
 raw_encoding instruction::raw_encode(program& program) {