CoreMutex add portYieldFromISR for FreeRTOS (#1484)

earlephilhower · Jun 7, 2023 · 3f475ac · 3f475ac
1 parent 5204dab
commit 3f475ac
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 8 deletions.
diff --git a/cores/rp2040/CoreMutex.cpp b/cores/rp2040/CoreMutex.cpp
@@ -28,10 +28,12 @@ CoreMutex::CoreMutex(mutex_t *mutex, uint8_t option) {
     _mutex = mutex;
     _acquired = false;
     _option = option;
+    _pxHigherPriorityTaskWoken = 0; // pdFALSE
     if (__isFreeRTOS) {
         auto m = __get_freertos_mutex_for_ptr(mutex);
+
         if (__freertos_check_if_in_isr()) {
-            if (!__freertos_mutex_take_from_isr(m)) {
+            if (!__freertos_mutex_take_from_isr(m, &_pxHigherPriorityTaskWoken)) {
                 return;
             }
             // At this point we have the mutex in ISR
@@ -59,7 +61,7 @@ CoreMutex::~CoreMutex() {
         if (__isFreeRTOS) {
             auto m = __get_freertos_mutex_for_ptr(_mutex);
             if (__freertos_check_if_in_isr()) {
-                __freertos_mutex_give_from_isr(m);
+                __freertos_mutex_give_from_isr(m, &_pxHigherPriorityTaskWoken);
             } else {
                 __freertos_mutex_give(m);
             }

diff --git a/cores/rp2040/CoreMutex.h b/cores/rp2040/CoreMutex.h
@@ -43,4 +43,5 @@ class CoreMutex {
     mutex_t *_mutex;
     bool _acquired;
     uint8_t _option;
+    BaseType_t _pxHigherPriorityTaskWoken;
 };
diff --git a/cores/rp2040/_freertos.h b/cores/rp2040/_freertos.h
@@ -35,6 +35,7 @@ extern "C" {
     struct QueueDefinition; /* Using old naming convention so as not to break kernel aware debuggers. */
     typedef struct QueueDefinition   * QueueHandle_t;
     typedef QueueHandle_t SemaphoreHandle_t;
+    typedef int32_t BaseType_t;
 #endif
 
     extern bool __freertos_check_if_in_isr() __attribute__((weak));
@@ -43,10 +44,11 @@ extern "C" {
     extern SemaphoreHandle_t _freertos_recursive_mutex_create() __attribute__((weak));
 
     extern void __freertos_mutex_take(SemaphoreHandle_t mtx) __attribute__((weak));
-    extern int __freertos_mutex_take_from_isr(SemaphoreHandle_t mtx) __attribute__((weak));
+
+    extern int __freertos_mutex_take_from_isr(SemaphoreHandle_t mtx, BaseType_t* pxHigherPriorityTaskWoken) __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_mutex_give_from_isr(SemaphoreHandle_t mtx, BaseType_t* pxHigherPriorityTaskWoken) __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));

diff --git a/libraries/FreeRTOS/src/variantHooks.cpp b/libraries/FreeRTOS/src/variantHooks.cpp
@@ -58,8 +58,8 @@ extern "C" {
         xSemaphoreTake(mtx, portMAX_DELAY);
     }
 
-    int __freertos_mutex_take_from_isr(SemaphoreHandle_t mtx) {
-        return xSemaphoreTakeFromISR(mtx, NULL);
+    int __freertos_mutex_take_from_isr(SemaphoreHandle_t mtx, BaseType_t* pxHigherPriorityTaskWoken) {
+        return xSemaphoreTakeFromISR(mtx, pxHigherPriorityTaskWoken);
     }
 
     int __freertos_mutex_try_take(SemaphoreHandle_t mtx) {
@@ -70,8 +70,10 @@ extern "C" {
         xSemaphoreGive(mtx);
     }
 
-    void __freertos_mutex_give_from_isr(SemaphoreHandle_t mtx) {
-        xSemaphoreGiveFromISR(mtx, NULL);
+    void __freertos_mutex_give_from_isr(SemaphoreHandle_t mtx, BaseType_t* pxHigherPriorityTaskWoken) {
+        BaseType_t hiPrio = pxHigherPriorityTaskWoken ? *pxHigherPriorityTaskWoken : pdFALSE;
+        xSemaphoreGiveFromISR(mtx, &hiPrio);
+        portYIELD_FROM_ISR(hiPrio);
     }
 
     void __freertos_recursive_mutex_take(SemaphoreHandle_t mtx) {