Enable proper reuse of PIO programs (#1526)

* Enable proper reuse of PIO programs

Rewrite the PIOProgram helper class to properly re-use loaded programs
and to try to re-use loaded instructions before allocating a new PIO
program.

Supersedes #1524

* Less copy-pasta

Author: earlephilhower

cores/rp2040/PIOProgram.cpp

@@ -0,0 +1,82 @@
+/*
+    RP2040 PIO utility class
+
+    Copyright (c) 2023 Earle F. Philhower, III <[email protected]>
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+    This library is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include <Arduino.h>
+#include "PIOProgram.h"
+#include <map>
+
+static std::map<const pio_program_t *, int> __pioMap[2];
+auto_init_mutex(_pioMutex);
+
+
+PIOProgram::PIOProgram(const pio_program_t *pgm) {
+    _pgm = pgm;
+    _pio = nullptr;
+    _sm = -1;
+}
+
+// We leave the INSN loaded in INSN RAM
+PIOProgram::~PIOProgram() {
+    if (_pio) {
+        pio_sm_unclaim(_pio, _sm);
+    }
+}
+
+// Possibly load into a PIO and allocate a SM
+bool PIOProgram::prepare(PIO *pio, int *sm, int *offset) {
+    CoreMutex m(&_pioMutex);
+    PIO pi[2] = { pio0, pio1 };
+
+    // If it's already loaded into PIO IRAM, try and allocate in that specific PIO
+    for (int o = 0; o < 2; o++) {
+        auto p = __pioMap[o].find(_pgm);
+        if (p != __pioMap[o].end()) {
+            int idx = pio_claim_unused_sm(pi[o], false);
+            if (idx >= 0) {
+                _pio = pi[o];
+                _sm = idx;
+                *pio = pi[o];
+                *sm = idx;
+                *offset = p->second;
+                return true;
+            }
+        }
+    }
+
+    // Not in any PIO IRAM, so try and add
+    for (int o = 0; o < 2; o++) {
+        if (pio_can_add_program(pi[o], _pgm)) {
+            int idx = pio_claim_unused_sm(pi[o], false);
+            if (idx >= 0) {
+                int off = pio_add_program(pi[o], _pgm);
+                __pioMap[o].insert({_pgm, off});
+                _pio = pi[o];
+                _sm = idx;
+                *pio = pi[o];
+                *sm = idx;
+                *offset = off;
+                return true;
+            }
+        }
+    }
+
+    // Nope, no room either for SMs or INSNs
+    return false;
+}

cores/rp2040/PIOProgram.h

@@ -0,0 +1,37 @@
+/*
+    RP2040 PIO utility class
+
+    Copyright (c) 2023 Earle F. Philhower, III <[email protected]>
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+    This library is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#pragma once
+
+#include <hardware/pio.h>
+
+// Wrapper class for PIO programs, abstracting common operations out
+class PIOProgram {
+public:
+    PIOProgram(const pio_program_t *pgm);
+    ~PIOProgram();
+    // Possibly load into a PIO and allocate a SM
+    bool prepare(PIO *pio, int *sm, int *offset);
+
+private:
+    const pio_program_t *_pgm;
+    PIO _pio;
+    int _sm;
+};

cores/rp2040/RP2040Support.h

@@ -31,6 +31,7 @@
 #include <pico/util/queue.h>
 #include <pico/bootrom.h>
 #include "CoreMutex.h"
+#include "PIOProgram.h"
 #include "ccount.pio.h"
 #include <malloc.h>
 
@@ -154,65 +155,9 @@ class _MFIFO {
 class RP2040;
 extern RP2040 rp2040;
 extern "C" void main1();
-class PIOProgram;
-
 extern "C" char __StackLimit;
 extern "C" char __bss_end__;
 
-// Wrapper class for PIO programs, abstracting common operations out
-// TODO - Add unload/destructor
-class PIOProgram {
-public:
-    PIOProgram(const pio_program_t *pgm) {
-        _pgm = pgm;
-    }
-
-    // Possibly load into a PIO and allocate a SM
-    bool prepare(PIO *pio, int *sm, int *offset) {
-        extern mutex_t _pioMutex;
-        CoreMutex m(&_pioMutex);
-        // Is there an open slot to run in, first?
-        if (!_findFreeSM(pio, sm)) {
-            return false;
-        }
-        // Is it loaded on that PIO?
-        if (_offset[pio_get_index(*pio)] < 0) {
-            // Nope, need to load it
-            if (!pio_can_add_program(*pio, _pgm)) {
-                return false;
-            }
-            _offset[pio_get_index(*pio)] = pio_add_program(*pio, _pgm);
-        }
-        // Here it's guaranteed loaded, return values
-        // PIO and SM already set
-        *offset = _offset[pio_get_index(*pio)];
-        return true;
-    }
-
-private:
-    // Find an unused PIO state machine to grab, returns false when none available
-    static bool _findFreeSM(PIO *pio, int *sm) {
-        int idx = pio_claim_unused_sm(pio0, false);
-        if (idx >= 0) {
-            *pio = pio0;
-            *sm = idx;
-            return true;
-        }
-        idx = pio_claim_unused_sm(pio1, false);
-        if (idx >= 0) {
-            *pio = pio1;
-            *sm = idx;
-            return true;
-        }
-        return false;
-    }
-
-
-private:
-    int _offset[2] = { -1, -1 };
-    const pio_program_t *_pgm;
-};
-
 class RP2040 {
 public:
     RP2040()  { /* noop */ }

cores/rp2040/main.cpp

@@ -29,8 +29,6 @@ extern "C" {
     volatile bool __otherCoreIdled = false;
 };
 
-mutex_t _pioMutex;
-
 extern void setup();
 extern void loop();
 
@@ -91,8 +89,6 @@ extern "C" int main() {
         _REENT_INIT_PTR(_impure_ptr1);
     }
 
-    mutex_init(&_pioMutex);
-
     rp2040.begin();
 
     initVariant();