Fix FreeRTOS CoreMutex shim to handle ISRs (#1442)

* Fix FreeRTOS CoreMutex shim to handle ISRs Automatically check, when in FreeRTOS, if we're in an ISR and if so call the correct mutex grab. Thanks to @caveman99 for finding and proposing a solution! Fixes #1441 * Fix the CoreMutex destructor, too
earlephilhower · May 23, 2023 · 652f9f9 · 652f9f9
1 parent cac9eb0
commit 652f9f9
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 7 deletions.
diff --git a/cores/rp2040/CoreMutex.cpp b/cores/rp2040/CoreMutex.cpp
@@ -30,7 +30,7 @@ CoreMutex::CoreMutex(mutex_t *mutex, uint8_t option) {
     _option = option;
     if (__isFreeRTOS) {
         auto m = __get_freertos_mutex_for_ptr(mutex);
-        if (_option & FromISR) {
+        if (__freertos_check_if_in_isr()) {
             __freertos_mutex_take_from_isr(m);
         } else {
             if (!__freertos_mutex_try_take(m)) {
@@ -56,7 +56,7 @@ CoreMutex::~CoreMutex() {
     if (_acquired) {
         if (__isFreeRTOS) {
             auto m = __get_freertos_mutex_for_ptr(_mutex);
-            if (_option & FromISR) {
+            if (__freertos_check_if_in_isr()) {
                 __freertos_mutex_give_from_isr(m);
             } else {
                 __freertos_mutex_give(m);

diff --git a/cores/rp2040/CoreMutex.h b/cores/rp2040/CoreMutex.h
@@ -27,8 +27,7 @@
 #include "_freertos.h"
 
 enum {
-    DebugEnable = 1,
-    FromISR = 1 << 1,
+    DebugEnable = 1
 };
 
 class CoreMutex {

diff --git a/cores/rp2040/_freertos.h b/cores/rp2040/_freertos.h
@@ -37,6 +37,8 @@ extern "C" {
     typedef QueueHandle_t SemaphoreHandle_t;
 #endif
 
+    extern bool __freertos_check_if_in_isr() __attribute__((weak));
+
     extern SemaphoreHandle_t __freertos_mutex_create() __attribute__((weak));
     extern SemaphoreHandle_t _freertos_recursive_mutex_create() __attribute__((weak));
 

diff --git a/cores/rp2040/wiring_private.cpp b/cores/rp2040/wiring_private.cpp
@@ -77,7 +77,7 @@ static std::map<pin_size_t, CBInfo> _map;
 void _gpioInterruptDispatcher(uint gpio, uint32_t events) {
     (void) events;
     // Only need to lock around the std::map check, not the whole IRQ callback
-    CoreMutex m(&_irqMutex, (FromISR | DebugEnable));
+    CoreMutex m(&_irqMutex);
     if (m) {
         auto irq = _map.find(gpio);
         if (irq != _map.end()) {

diff --git a/libraries/FreeRTOS/src/variantHooks.cpp b/libraries/FreeRTOS/src/variantHooks.cpp
@@ -85,6 +85,10 @@ extern "C" {
     void __freertos_recursive_mutex_give(SemaphoreHandle_t mtx) {
         xSemaphoreGiveRecursive(mtx);
     }
+
+    bool __freertos_check_if_in_isr() {
+        return portCHECK_IF_IN_ISR();
+    }
 }
 
 
@@ -488,5 +492,3 @@ void __USBStart() {
     xTaskCreate(__usb, "USB", 256, 0, configMAX_PRIORITIES - 2, &__usbTask);
     vTaskCoreAffinitySet(__usbTask, 1 << 0);
 }
-
-