Fix GPIO interrupt on freeRTOS (#959)

Author: imwoo90

cores/rp2040/CoreMutex.cpp

@@ -24,17 +24,22 @@
 #include "Arduino.h"
 #include "CoreMutex.h"
 
-CoreMutex::CoreMutex(mutex_t *mutex, bool debugEnable) {
+CoreMutex::CoreMutex(mutex_t *mutex, uint8_t option) {
     _mutex = mutex;
     _acquired = false;
+    _option = option;
     if (__isFreeRTOS) {
         auto m = __get_freertos_mutex_for_ptr(mutex);
-        __freertos_mutex_take(m);
+        if (_option & FromISR) {
+            __freertos_mutex_take_from_isr(m);
+        } else {
+            __freertos_mutex_take(m);
+        }
     } else {
         uint32_t owner;
         if (!mutex_try_enter(_mutex, &owner)) {
             if (owner == get_core_num()) { // Deadlock!
-                if (debugEnable) {
+                if (_option & DebugEnable) {
                     DEBUGCORE("CoreMutex - Deadlock detected!\n");
                 }
                 return;
@@ -49,7 +54,11 @@ CoreMutex::~CoreMutex() {
     if (_acquired) {
         if (__isFreeRTOS) {
             auto m = __get_freertos_mutex_for_ptr(_mutex);
-            __freertos_mutex_give(m);
+            if (_option & FromISR) {
+                __freertos_mutex_give_from_isr(m);
+            } else {
+                __freertos_mutex_give(m);
+            }
         } else {
             mutex_exit(_mutex);
         }

cores/rp2040/CoreMutex.h

@@ -26,9 +26,14 @@
 #include "pico/mutex.h"
 #include "_freertos.h"
 
+enum {
+    DebugEnable = 1,
+    FromISR = 1 << 1,
+};
+
 class CoreMutex {
 public:
-    CoreMutex(mutex_t *mutex, bool debugEnable = true);
+    CoreMutex(mutex_t *mutex, uint8_t option = DebugEnable);
     ~CoreMutex();
 
     operator bool() {
@@ -38,4 +43,5 @@ class CoreMutex {
 private:
     mutex_t *_mutex;
     bool _acquired;
+    u_int8_t _option;
 };

cores/rp2040/_freertos.h

@@ -41,8 +41,10 @@ extern "C" {
     extern SemaphoreHandle_t _freertos_recursive_mutex_create() __attribute__((weak));
 
     extern void __freertos_mutex_take(SemaphoreHandle_t mtx) __attribute__((weak));
+    extern void __freertos_mutex_take_from_isr(SemaphoreHandle_t mtx) __attribute__((weak));
     extern int __freertos_mutex_try_take(SemaphoreHandle_t mtx) __attribute__((weak));
     extern void __freertos_mutex_give(SemaphoreHandle_t mtx) __attribute__((weak));
+    extern void __freertos_mutex_give_from_isr(SemaphoreHandle_t mtx) __attribute__((weak));
 
     extern void __freertos_recursive_mutex_take(SemaphoreHandle_t mtx) __attribute__((weak));
     extern int __freertos_recursive_mutex_try_take(SemaphoreHandle_t mtx) __attribute__((weak));

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);
+    CoreMutex m(&_irqMutex, (FromISR | DebugEnable));
     if (m) {
         auto irq = _map.find(gpio);
         if (irq != _map.end()) {

libraries/FreeRTOS/src/variantHooks.cpp

@@ -414,6 +414,10 @@ extern "C" {
         xSemaphoreTake(mtx, portMAX_DELAY);
     }
 
+    void __freertos_mutex_take_from_isr(SemaphoreHandle_t mtx) {
+        xSemaphoreTakeFromISR(mtx, NULL);
+    }
+
     int __freertos_mutex_try_take(SemaphoreHandle_t mtx) {
         return xSemaphoreTake(mtx, 0);
     }
@@ -422,6 +426,10 @@ extern "C" {
         xSemaphoreGive(mtx);
     }
 
+    void __freertos_mutex_give_from_isr(SemaphoreHandle_t mtx) {
+        xSemaphoreGiveFromISR(mtx, NULL);
+    }
+
     void __freertos_recursive_mutex_take(SemaphoreHandle_t mtx) {
         xSemaphoreTakeRecursive(mtx, portMAX_DELAY);
     }