rp2/rp2_pio: Fix support for pin wrapping and RP2350B upper-bank pins.

On RP2350B where there are more than 32 pins, using
`pio_sm_set_pins_with_mask()` and `pio_sm_set_pindirs_with_mask()` is not
correct because their arguments are `uint32_t` and higher bits get lost
when `pio.gpio_base(16)` is used.

This commit fixes the issue by using the 64-bit API functions on RP2350B.
It also makes sure pin wrapping is supported, i.e. using [30, 31, 0, 1] or
[46, 47, 16, 17] as contiguous pin ranges for a PIO program.

Fixes issue #16199.

Signed-off-by: Anson Mansfield <amansfield@mantaro.com>

1 files changed, 25 insertions, 5 deletions

diff --git a/ports/rp2/rp2_pio.c b/ports/rp2/rp2_pio.c
index 611e74a15..81351431c 100644
--- a/ports/rp2/rp2_pio.c
+++ b/ports/rp2/rp2_pio.c
@@ -271,12 +271,32 @@ static void asm_pio_get_pins(PIO pio, const char *type, mp_obj_t prog_pins, mp_o
     }
 }
 
+static inline uint32_t rotl32a(const uint32_t x, const int k) {
+    return (x << k) | (x >> (32 - k));
+}
+
 static void asm_pio_init_gpio(PIO pio, uint32_t sm, asm_pio_config_t *config) {
-    uint32_t pinmask = ((1 << config->count) - 1) << (config->base - pio_get_gpio_base(pio));
-    pio_sm_set_pins_with_mask(pio, sm, config->pinvals << (config->base - pio_get_gpio_base(pio)), pinmask);
-    pio_sm_set_pindirs_with_mask(pio, sm, config->pindirs << (config->base - pio_get_gpio_base(pio)), pinmask);
-    for (size_t i = 0; i < config->count; ++i) {
-        gpio_set_function(config->base + i, GPIO_FUNC_PIO0 + pio_get_index(pio));
+    uint gpio_base = pio_get_gpio_base(pio);
+    uint32_t pinmask = rotl32a(~(-1 << config->count), config->base - gpio_base);
+    uint32_t pinvals = rotl32a(config->pinvals, config->base - gpio_base);
+    uint32_t pindirs = rotl32a(config->pindirs, config->base - gpio_base);
+
+    #if !PICO_PIO_USE_GPIO_BASE
+    // optimization: avoid 64-bit arithmetic on RP2040 and RP2350A
+    pio_sm_set_pins_with_mask(pio, sm, pinvals, pinmask);
+    pio_sm_set_pindirs_with_mask(pio, sm, pindirs, pinmask);
+    #else
+    uint64_t pinmask64 = (uint64_t)pinmask << gpio_base;
+    uint64_t pinvals64 = (uint64_t)pinvals << gpio_base;
+    uint64_t pindirs64 = (uint64_t)pindirs << gpio_base;
+    pio_sm_set_pins_with_mask64(pio, sm, pinvals64, pinmask64);
+    pio_sm_set_pindirs_with_mask64(pio, sm, pindirs64, pinmask64);
+    #endif
+
+    for (size_t i = 0; i < 32; ++i) {
+        if (pinmask & (1 << i)) {
+            gpio_set_function(gpio_base + i, GPIO_FUNC_PIO0 + pio_get_index(pio));
+        }
     }
 }