From 652f9f9eda0d77efeafebc7c1ff5cd45defc71bf Mon Sep 17 00:00:00 2001
From: "Earle F. Philhower, III" <earlephilhower@yahoo.com>
Date: Tue, 23 May 2023 10:12:52 -0700
Subject: [PATCH] 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
---
 cores/rp2040/CoreMutex.cpp              | 4 ++--
 cores/rp2040/CoreMutex.h                | 3 +--
 cores/rp2040/_freertos.h                | 2 ++
 cores/rp2040/wiring_private.cpp         | 2 +-
 libraries/FreeRTOS/src/variantHooks.cpp | 6 ++++--
 5 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/cores/rp2040/CoreMutex.cpp b/cores/rp2040/CoreMutex.cpp
index 2331b10..25a23bd 100644
--- 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
index 1f44266..099dc2a 100644
--- 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
index 04db74c..8116f36 100644
--- 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
index 06c6bc8..cb1ac78 100644
--- 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
index 8414f60..0105b7f 100644
--- 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);
 }
-
-