Protect core/Newlib mutexes from task preemption under FreeRTOS (#798)

earlephilhower · web-flow · commit bde4da2b7e12 · 2022-08-29T08:56:59.000-07:00
Fixes #795 Replace all CoreMutex and Newlib mutex accesses with FreeRTOS calls when in FreeRTOS mode. Avoid issues with hange/etc. due to priority inversion. No changes to normal operating mode. Add a FreeRTOS stress test that caught the issue fixed here.
diff --git a/cores/rp2040/CoreMutex.h b/cores/rp2040/CoreMutex.h
@@ -24,28 +24,39 @@
 #pragma once
 
 #include "pico/mutex.h"
+#include "_freertos.h"
 
 class CoreMutex {
 public:
     CoreMutex(mutex_t *mutex, bool debugEnable = true) {
-        uint32_t owner;
         _mutex = mutex;
         _acquired = false;
-        if (!mutex_try_enter(_mutex, &owner)) {
-            if (owner == get_core_num()) { // Deadlock!
-                if (debugEnable) {
-                    DEBUGCORE("CoreMutex - Deadlock detected!\n");
+        if (__isFreeRTOS) {
+            auto m = __get_freertos_mutex_for_ptr(mutex);
+            __freertos_mutex_take(m);
+        } else {
+            uint32_t owner;
+            if (!mutex_try_enter(_mutex, &owner)) {
+                if (owner == get_core_num()) { // Deadlock!
+                    if (debugEnable) {
+                        DEBUGCORE("CoreMutex - Deadlock detected!\n");
+                    }
+                    return;
                 }
-                return;
+                mutex_enter_blocking(_mutex);
             }
-            mutex_enter_blocking(_mutex);
         }
         _acquired = true;
     }
 
     ~CoreMutex() {
         if (_acquired) {
-            mutex_exit(_mutex);
+            if (__isFreeRTOS) {
+                auto m = __get_freertos_mutex_for_ptr(_mutex);
+                __freertos_mutex_give(m);
+            } else {
+                mutex_exit(_mutex);
+            }
         }
     }
 
diff --git a/cores/rp2040/RP2040Support.h b/cores/rp2040/RP2040Support.h
@@ -32,19 +32,9 @@
 #include "ccount.pio.h"
 #include <malloc.h>
 
-extern "C" volatile bool __otherCoreIdled;
+#include "_freertos.h"
 
-// Halt the FreeRTOS PendSV task switching magic
-extern "C" int __holdUpPendSV;
-
-// FreeRTOS weak functions, to be overridden when we really are running FreeRTOS
-extern "C" {
-    extern void vTaskSuspendAll() __attribute__((weak));
-    extern int32_t xTaskResumeAll() __attribute__((weak));
-    typedef struct tskTaskControlBlock * TaskHandle_t;
-    extern void vTaskPreemptionDisable(TaskHandle_t p) __attribute__((weak));
-    extern void vTaskPreemptionEnable(TaskHandle_t p) __attribute__((weak));
-}
+extern "C" volatile bool __otherCoreIdled;
 
 class _MFIFO {
 public:
diff --git a/cores/rp2040/_freertos.cpp b/cores/rp2040/_freertos.cpp
@@ -0,0 +1,51 @@
+/*
+    _freertos.cpp - Internal core definitions for FreeRTOS
+
+    Copyright (c) 2022 Earle F. Philhower, III <earlephilhower@yahoo.com>
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+    This library is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include "_freertos.h"
+#include "pico/mutex.h"
+#include <stdlib.h>
+
+typedef struct {
+    mutex_t *src;
+    SemaphoreHandle_t dst;
+} FMMap;
+
+static FMMap *_map = nullptr;
+extern "C" SemaphoreHandle_t __get_freertos_mutex_for_ptr(mutex_t *m) {
+    if (!_map) {
+        _map = (FMMap *)calloc(sizeof(FMMap), 16);
+    }
+    // Pre-existing map
+    for (int i = 0; i < 16; i++) {
+        if (m == _map[i].src) {
+            return _map[i].dst;
+        }
+    }
+    // Make a new mutex
+    auto fm = __freertos_mutex_create();
+    for (int i = 0; i < 16; i++) {
+        if (_map[i].src == nullptr) {
+            _map[i].src = m;
+            _map[i].dst = fm;
+            return fm;
+        }
+    }
+    return nullptr; // Need to make space for more mutex maps!
+}
diff --git a/cores/rp2040/_freertos.h b/cores/rp2040/_freertos.h
@@ -0,0 +1,64 @@
+/*
+    _freertos.h - Internal core definitions for FreeRTOS
+
+    Copyright (c) 2022 Earle F. Philhower, III <earlephilhower@yahoo.com>
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+    This library is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#pragma once
+#include "pico/mutex.h"
+
+// Cannot include refs to FreeRTOS's actual semaphore calls because they
+// are implemented as macros, so we have a wrapper in our variant hook
+// to handle it.
+
+extern bool __isFreeRTOS;
+
+// FreeRTOS has been set up
+extern volatile bool __freeRTOSinitted;
+
+extern "C" {
+#ifndef INC_FREERTOS_H
+    struct QueueDefinition; /* Using old naming convention so as not to break kernel aware debuggers. */
+    typedef struct QueueDefinition   * QueueHandle_t;
+    typedef QueueHandle_t SemaphoreHandle_t;
+#endif
+
+    extern SemaphoreHandle_t __freertos_mutex_create() __attribute__((weak));
+    extern SemaphoreHandle_t _freertos_recursive_mutex_create() __attribute__((weak));
+
+    extern void __freertos_mutex_take(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_recursive_mutex_take(SemaphoreHandle_t mtx) __attribute__((weak));
+    extern int __freertos_recursive_mutex_try_take(SemaphoreHandle_t mtx) __attribute__((weak));
+    extern void __freertos_recursive_mutex_give(SemaphoreHandle_t mtx) __attribute__((weak));
+
+#ifndef INC_FREERTOS_H
+    extern void vTaskSuspendAll() __attribute__((weak));
+    extern int32_t xTaskResumeAll() __attribute__((weak));
+
+    typedef struct tskTaskControlBlock * TaskHandle_t;
+    extern void vTaskPreemptionDisable(TaskHandle_t p) __attribute__((weak));
+    extern void vTaskPreemptionEnable(TaskHandle_t p) __attribute__((weak));
+#endif
+
+    extern SemaphoreHandle_t __get_freertos_mutex_for_ptr(mutex_t *m);
+}
+
+// Halt the FreeRTOS PendSV task switching magic
+extern "C" int __holdUpPendSV;
diff --git a/cores/rp2040/lock.cpp b/cores/rp2040/lock.cpp
@@ -25,6 +25,8 @@
 #include <pico/mutex.h>
 #include <sys/lock.h>
 
+#include "_freertos.h"
+
 // HACK ALERT
 // Pico-SDK defines mutex which can be at global scope, but when the auto_init_
 // macros are used they are defined as static.  Newlib needs global access to
@@ -42,12 +44,75 @@ auto_init_mutex(__lock___dd_hash_mutex);
 auto_init_mutex(__lock___arc4random_mutex);
 #undef static
 
+// FreeRTOS hack - Allow Newlib to use FreeRTOS mutexes which preserve TASKID which
+// is needed to support multithread
+
+SemaphoreHandle_t __lock___sinit_recursive_mutex_freertos;
+SemaphoreHandle_t __lock___sfp_recursive_mutex_freertos;
+SemaphoreHandle_t __lock___atexit_recursive_mutex_freertos;
+SemaphoreHandle_t __lock___at_quick_exit_mutex_freertos;
+SemaphoreHandle_t __lock___malloc_recursive_mutex_freertos;
+SemaphoreHandle_t __lock___env_recursive_mutex_freertos;
+SemaphoreHandle_t __lock___tz_mutex_freertos;
+SemaphoreHandle_t __lock___dd_hash_mutex_freertos;
+SemaphoreHandle_t __lock___arc4random_mutex_freertos;
+
+void __initFreeRTOSMutexes() {
+    __lock___sinit_recursive_mutex_freertos = _freertos_recursive_mutex_create();
+    __lock___sfp_recursive_mutex_freertos = _freertos_recursive_mutex_create();
+    __lock___atexit_recursive_mutex_freertos = _freertos_recursive_mutex_create();
+    __lock___at_quick_exit_mutex_freertos = __freertos_mutex_create();
+    __lock___malloc_recursive_mutex_freertos = _freertos_recursive_mutex_create();
+    __lock___env_recursive_mutex_freertos = _freertos_recursive_mutex_create();
+    __lock___tz_mutex_freertos = __freertos_mutex_create();
+    __lock___dd_hash_mutex_freertos = __freertos_mutex_create();
+    __lock___arc4random_mutex_freertos = __freertos_mutex_create();
+}
+
+static SemaphoreHandle_t __getFreeRTOSMutex(_LOCK_T lock) {
+    mutex_t *l = (mutex_t *)lock;
+    if (l == &__lock___at_quick_exit_mutex) {
+        return __lock___at_quick_exit_mutex_freertos;
+    } else if (l == &__lock___tz_mutex) {
+        return __lock___tz_mutex_freertos;
+    } else if (l == &__lock___dd_hash_mutex) {
+        return __lock___dd_hash_mutex_freertos;
+    } else if (l == &__lock___arc4random_mutex) {
+        return __lock___arc4random_mutex_freertos;
+    }
+    return nullptr;
+}
+
+static SemaphoreHandle_t __getFreeRTOSRecursiveMutex(_LOCK_T lock) {
+    recursive_mutex_t *l = (recursive_mutex_t *)lock;
+    if (l  == &__lock___sinit_recursive_mutex) {
+        return __lock___sinit_recursive_mutex_freertos;
+    } else if (l == &__lock___sfp_recursive_mutex) {
+        return __lock___sfp_recursive_mutex_freertos;
+    } else if (l == &__lock___atexit_recursive_mutex) {
+        return __lock___atexit_recursive_mutex_freertos;
+    } else if (l == &__lock___malloc_recursive_mutex) {
+        return __lock___malloc_recursive_mutex_freertos;
+    } else if (l == &__lock___env_recursive_mutex) {
+        return __lock___env_recursive_mutex_freertos;
+    }
+    return nullptr;
+}
+
 void __retarget_lock_init(_LOCK_T *lock) {
-    mutex_init((mutex_t*) lock);
+    if (__freeRTOSinitted) {
+        /* Already done in initFreeRTOSMutexes() */
+    } else {
+        mutex_init((mutex_t*) lock);
+    }
 }
 
 void __retarget_lock_init_recursive(_LOCK_T *lock) {
-    recursive_mutex_init((recursive_mutex_t*) lock);
+    if (__freeRTOSinitted) {
+        /* Already done in initFreeRTOSMutexes() */
+    } else {
+        recursive_mutex_init((recursive_mutex_t*) lock);
+    }
 }
 
 void __retarget_lock_close(_LOCK_T lock) {
@@ -59,25 +124,59 @@ void __retarget_lock_close_recursive(_LOCK_T lock) {
 }
 
 void __retarget_lock_acquire(_LOCK_T lock) {
-    mutex_enter_blocking((mutex_t*)lock);
+    if (__freeRTOSinitted) {
+        auto mtx = __getFreeRTOSMutex(lock);
+        __freertos_mutex_take(mtx);
+    } else {
+        mutex_enter_blocking((mutex_t*)lock);
+    }
 }
 
 void __retarget_lock_acquire_recursive(_LOCK_T lock) {
-    recursive_mutex_enter_blocking((recursive_mutex_t*)lock);
+    if (__freeRTOSinitted) {
+        auto mtx = __getFreeRTOSRecursiveMutex(lock);
+        __freertos_recursive_mutex_take(mtx);
+    } else {
+        recursive_mutex_enter_blocking((recursive_mutex_t*)lock);
+    }
 }
 
 int __retarget_lock_try_acquire(_LOCK_T lock) {
-    return mutex_try_enter((mutex_t *)lock, NULL);
+    int ret;
+    if (__freeRTOSinitted) {
+        auto mtx = __getFreeRTOSMutex(lock);
+        ret = __freertos_mutex_try_take(mtx);
+    } else {
+        ret = mutex_try_enter((mutex_t *)lock, NULL);
+    }
+    return ret;
 }
 
 int __retarget_lock_try_acquire_recursive(_LOCK_T lock) {
-    return recursive_mutex_try_enter((recursive_mutex_t*)lock, NULL);
+    int ret;
+    if (__freeRTOSinitted) {
+        auto mtx = __getFreeRTOSRecursiveMutex(lock);
+        ret = __freertos_recursive_mutex_try_take(mtx);
+    } else {
+        ret = recursive_mutex_try_enter((recursive_mutex_t*)lock, NULL);
+    }
+    return ret;
 }
 
 void __retarget_lock_release(_LOCK_T lock) {
-    mutex_exit((mutex_t*)lock);
+    if (__freeRTOSinitted) {
+        auto mtx = __getFreeRTOSMutex(lock);
+        __freertos_mutex_give(mtx);
+    } else {
+        mutex_exit((mutex_t*)lock);
+    }
 }
 
 void __retarget_lock_release_recursive(_LOCK_T lock) {
-    recursive_mutex_exit((recursive_mutex_t*)lock);
+    if (__freeRTOSinitted) {
+        auto mtx = __getFreeRTOSRecursiveMutex(lock);
+        __freertos_recursive_mutex_give(mtx);
+    } else {
+        recursive_mutex_exit((recursive_mutex_t*)lock);
+    }
 }
diff --git a/cores/rp2040/main.cpp b/cores/rp2040/main.cpp
@@ -42,6 +42,8 @@ extern void loop();
 extern void initFreeRTOS() __attribute__((weak));
 extern void startFreeRTOS() __attribute__((weak));
 bool __isFreeRTOS;
+volatile bool __freeRTOSinitted;
+
 
 // Weak empty variant initialization. May be redefined by variant files.
 void initVariant() __attribute__((weak));
diff --git a/libraries/FreeRTOS/examples/Stress.ino b/libraries/FreeRTOS/examples/Stress.ino
diff --git a/libraries/FreeRTOS/src/variantHooks.cpp b/libraries/FreeRTOS/src/variantHooks.cpp
