More multicore safety plumbing, IRQs, analogXXX (#107)

Keep a per-core IRQ stack for noInterrupts since each core has its own enable/disable. Make analogRead/analogWrite multicore safe
earlephilhower · Apr 16, 2021 · 011ecdb · 011ecdb
1 parent b793ad5
commit 011ecdb
Show file tree

Hide file tree

Showing 4 changed files with 50 additions and 25 deletions.
diff --git a/cores/rp2040/CoreMutex.h b/cores/rp2040/CoreMutex.h
@@ -33,6 +33,7 @@ class CoreMutex {
         _acquired = false;
         if (!mutex_try_enter(_mutex, &owner)) {
             if (owner == get_core_num()) { // Deadlock!
+                DEBUGCORE("CoreMutex - Deadlock detected!\n");
                 return;
             }
             mutex_enter_blocking(_mutex);

diff --git a/cores/rp2040/Tone.cpp b/cores/rp2040/Tone.cpp
@@ -31,20 +31,14 @@ typedef struct {
 } Tone;
 
 // Keep std::map safe for multicore use
-static bool _mutexInitted = false;
-static mutex_t _mutex;
+auto_init_mutex(_toneMutex);
 
 
 #include "tone.pio.h"
 static PIOProgram _tonePgm(&tone_program);
-
 static std::map<pin_size_t, Tone *> _toneMap;
 
 void tone(uint8_t pin, unsigned int frequency, unsigned long duration) {
-    if (!_mutexInitted) {
-        mutex_init(&_mutex);
-        _mutexInitted = true;
-    }
     if ((pin < 0) || (pin > 29)) {
         DEBUGCORE("ERROR: Illegal pin in tone (%d)\n", pin);
         return;
@@ -55,7 +49,7 @@ void tone(uint8_t pin, unsigned int frequency, unsigned long duration) {
     }
 
     // Ensure only 1 core can start or stop at a time
-    CoreMutex m(&_mutex);
+    CoreMutex m(&_toneMutex);
     if (!m) return; // Weird deadlock case
 
     int us = 1000000 / frequency / 2;
@@ -91,12 +85,7 @@ void tone(uint8_t pin, unsigned int frequency, unsigned long duration) {
 }
 
 void noTone(uint8_t pin) {
-    if (!_mutexInitted) {
-        mutex_init(&_mutex);
-        _mutexInitted = true;
-    }
-
-    CoreMutex m(&_mutex);
+    CoreMutex m(&_toneMutex);
 
     if ((pin < 0) || (pin > 29) || !m) {
         DEBUGCORE("ERROR: Illegal pin in tone (%d)\n", pin);

diff --git a/cores/rp2040/wiring_analog.cpp b/cores/rp2040/wiring_analog.cpp
@@ -19,6 +19,7 @@
  */
 
 #include <Arduino.h>
+#include <CoreMutex.h>
 #include <hardware/gpio.h>
 #include <hardware/pwm.h>
 #include <hardware/clocks.h>
@@ -31,6 +32,8 @@ static uint16_t analogFreq = 1000;
 static bool pwmInitted = false;
 static bool adcInitted = false;
 
+auto_init_mutex(_dacMutex);
+
 extern "C" void analogWriteFreq(uint32_t freq) {
     if (freq == analogFreq) {
         return;
@@ -68,7 +71,9 @@ extern "C" void analogWriteResolution(int res) {
 }
 
 extern "C" void analogWrite(pin_size_t pin, int val) {
-    if ((pin < 0) || (pin > 29)) {
+    CoreMutex m(&_dacMutex);
+
+    if ((pin < 0) || (pin > 29) || !m) {
         DEBUGCORE("ERROR: Illegal analogWrite pin (%d)\n", pin);
         return;
     }
@@ -92,8 +97,12 @@ extern "C" void analogWrite(pin_size_t pin, int val) {
     pwm_set_gpio_level(pin, val);
 }
 
+auto_init_mutex(_adcMutex);
+
 extern "C" int analogRead(pin_size_t pin) {
-    if ((pin < A0) || (pin > A3)) {
+    CoreMutex m(&_adcMutex);
+
+    if ((pin < A0) || (pin > A3) || !m) {
         DEBUGCORE("ERROR: Illegal analogRead pin (%d)\n", pin);
         return 0;
     }
@@ -106,6 +115,11 @@ extern "C" int analogRead(pin_size_t pin) {
 }
 
 extern "C" float analogReadTemp() {
+    CoreMutex m(&_adcMutex);
+
+    if (!m) {
+        return 0.0f; // Deadlock
+    }
     if (!adcInitted) {
         adc_init();
     }

diff --git a/cores/rp2040/wiring_private.cpp b/cores/rp2040/wiring_private.cpp
@@ -19,42 +19,58 @@
  */
 
 #include <Arduino.h>
+#include <CoreMutex.h>
 #include <hardware/gpio.h>
 #include <hardware/sync.h>
 #include <stack>
 #include <map>
 
 // Support nested IRQ disable/re-enable
-static std::stack<uint32_t> _irqStack;
+static std::stack<uint32_t> _irqStack[2];
 
 extern "C" void interrupts() {
-    if (_irqStack.empty()) {
+    if (_irqStack[get_core_num()].empty()) {
         // ERROR
         return;
     }
-    restore_interrupts(_irqStack.top());
-    _irqStack.pop();
+    restore_interrupts(_irqStack[get_core_num()].top());
+    _irqStack[get_core_num()].pop();
 }
 
 extern "C" void noInterrupts() {
-    _irqStack.push(save_and_disable_interrupts());
+    _irqStack[get_core_num()].push(save_and_disable_interrupts());
 }
 
 // Only 1 GPIO IRQ callback for all pins, so we need to look at the pin it's for and
 // dispatch to the real callback manually
+auto_init_mutex(_irqMutex);
 static std::map<pin_size_t, voidFuncPtr> _map;
 
 void _gpioInterruptDispatcher(uint gpio, uint32_t events) {
-    auto irq = _map.find(gpio);
-    if (irq != _map.end()) {
-        // Ignore events, only one event per pin supported by Arduino
-        irq->second(); // Do the callback
+    // Only need to lock around the std::map check, not the whole IRQ callback
+    voidFuncPtr cb = nullptr;
+    {
+	CoreMutex m(&_irqMutex);
+        if (m) {
+            auto irq = _map.find(gpio);
+            if (irq != _map.end()) {
+                cb = irq->second;
+            }
+        }
+    }
+    if (cb) {
+        cb(); // Do the callback
     } else {
         // ERROR, but we're in an IRQ so do nothing
     }
 }
 
 extern "C" void attachInterrupt(pin_size_t pin, voidFuncPtr callback, PinStatus mode) {
+    CoreMutex m(&_irqMutex);
+    if (!m) {
+        return;
+    }
+
     uint32_t events;
     switch (mode) {
         case LOW:     events = 1; break;
@@ -72,6 +88,11 @@ extern "C" void attachInterrupt(pin_size_t pin, voidFuncPtr callback, PinStatus
 }
 
 extern "C" void detachInterrupt(pin_size_t pin) {
+    CoreMutex m(&_irqMutex);
+    if (!m) {
+        return;
+    }
+
     noInterrupts();
     auto irq = _map.find(pin);
     if (irq != _map.end()) {