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

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.

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);
+            }
         }
     }
 

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:

cores/rp2040/_freertos.cpp

+/*
+    _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!
+}

cores/rp2040/_freertos.h

+/*
+    _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;

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);
+    }
 }

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));

libraries/FreeRTOS/examples/Stress.ino

+// FreeRTOS system call/mutex stress test
+
+#include <Arduino.h>
+#include <FreeRTOS.h>
+#include <task.h>
+#include <semphr.h>
+#define DELAY 1
+#define SERIAL_DEBUG Serial1
+#define STACK_SIZE 512
+#define CORE_0 (1 << 0)
+#define CORE_1 (1 << 1)
+
+void semphrTakeConditional(bool useMutexOn, SemaphoreHandle_t xSemaphore);
+void semphrGiveConditional(bool useMutexOn, SemaphoreHandle_t xSemaphore);
+
+/*
+  I want to keep the possibility of using different and independent
+  mutexes for each of the functions that operate on the heap.
+
+  If you want to enable the use of these mutexes remove the defines
+  on lines 30 and 31 and enable the their initialization in setup()
+*/
+SemaphoreHandle_t xSemaphoreMalloc = NULL;
+// SemaphoreHandle_t xSemaphoreRealloc = NULL;
+// SemaphoreHandle_t xSemaphoreFree = NULL;
+
+/*
+  A lazy way to use the same mutex for malloc, realloc and free
+  in order to bring us back to the same situation as the MCVE
+  posted here: https://github.com/earlephilhower/arduino-pico/issues/795#issuecomment-1227122082
+*/
+#define xSemaphoreRealloc xSemaphoreMalloc
+#define xSemaphoreFree xSemaphoreMalloc
+
+const bool useMutexOnMalloc = false;
+const bool useMutexOnRealloc = false;
+const bool useMutexOnFree = false;
+
+/*
+  Enabling this, a realloc will be performed and the string "_realloc"
+  will be concateneted to *tmp
+*/
+const bool tryRealloc = true;
+
+TaskHandle_t loop2Handle = NULL;
+TaskHandle_t loop3Handle = NULL;
+TaskHandle_t loop4Handle = NULL;
+TaskHandle_t loop5Handle = NULL;
+TaskHandle_t loop6Handle = NULL;
+TaskHandle_t loop7Handle = NULL;
+
+void loop2(void *pvPramaters);
+void loop3(void *pvPramaters);
+void loop4(void *pvPramaters);
+void loop5(void *pvPramaters);
+void loop6(void *pvPramaters);
+void loop7(void *pvPramaters);
+
+void setup()
+{
+  pinMode(LED_BUILTIN, OUTPUT);
+
+  xSemaphoreMalloc = xSemaphoreCreateMutex();
+  // xSemaphoreRealloc = xSemaphoreCreateMutex();
+  // xSemaphoreFree = xSemaphoreCreateMutex();
+
+  xTaskCreate(loop2, "loop2", STACK_SIZE, NULL, 1, &loop2Handle);
+  vTaskCoreAffinitySet(loop2Handle, CORE_0);
+  xTaskCreate(loop3, "loop3", STACK_SIZE, NULL, 1, &loop3Handle);
+  vTaskCoreAffinitySet(loop3Handle, CORE_1);
+  xTaskCreate(loop4, "loop4", STACK_SIZE, NULL, 1, &loop4Handle);
+  vTaskCoreAffinitySet(loop4Handle, CORE_0);
+  xTaskCreate(loop5, "loop5", STACK_SIZE, NULL, 1, &loop5Handle);
+  vTaskCoreAffinitySet(loop5Handle, CORE_1);
+  // xTaskCreate(loop6, "loop6", STACK_SIZE, NULL, 1, &loop6Handle);
+  // vTaskCoreAffinitySet(loop6Handle, CORE_0);
+  // xTaskCreate(loop7, "loop7", STACK_SIZE, NULL, 1, &loop7Handle);
+  // vTaskCoreAffinitySet(loop7Handle, CORE_1);
+}
+static int _loop[8];
+
+void loop()
+{
+  while (1)
+  {
+    _loop[0]++;
+    digitalWrite(LED_BUILTIN, HIGH);
+    delay(500);
+    digitalWrite(LED_BUILTIN, LOW);
+    delay(500);
+    for (int i=0; i<8; i++) Serial.printf("%d ", _loop[i]);
+    Serial.println("");
+  }
+}
+
+void loop1()
+{
+  while (1)
+  {
+    _loop[1]++;
+    char *tmp;
+
+    semphrTakeConditional(useMutexOnMalloc, xSemaphoreMalloc);
+    tmp = (char *)malloc(10 * sizeof(char));
+    semphrGiveConditional(useMutexOnMalloc, xSemaphoreMalloc);
+
+    strcpy(tmp, "foo");
+
+    if (tryRealloc)
+    {
+      semphrTakeConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      tmp = (char *)realloc(tmp, 20 * sizeof(char));
+      semphrGiveConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      strcat(tmp, "_realloc");
+    }
+
+    semphrTakeConditional(useMutexOnFree, xSemaphoreFree);
+    free(tmp);
+    semphrGiveConditional(useMutexOnFree, xSemaphoreFree);
+
+    delay(DELAY);
+  }
+}
+
+void loop2(void *pvPramaters)
+{
+  (void) pvPramaters;
+  while (1)
+  {
+    _loop[2]++;
+    char *tmp;
+
+    semphrTakeConditional(useMutexOnMalloc, xSemaphoreMalloc);
+    tmp = (char *)malloc(10 * sizeof(char));
+    semphrGiveConditional(useMutexOnMalloc, xSemaphoreMalloc);
+
+    strcpy(tmp, "bar");
+
+    if (tryRealloc)
+    {
+      semphrTakeConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      tmp = (char *)realloc(tmp, 20 * sizeof(char));
+      semphrGiveConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      strcat(tmp, "_realloc");
+    }
+
+    semphrTakeConditional(useMutexOnFree, xSemaphoreFree);
+    free(tmp);
+    semphrGiveConditional(useMutexOnFree, xSemaphoreFree);
+
+    delay(DELAY);
+  }
+}
+
+void loop3(void *pvPramaters)
+{
+  (void) pvPramaters;
+  while (1)
+  {
+    _loop[3]++;
+    char *tmp;
+
+    semphrTakeConditional(useMutexOnMalloc, xSemaphoreMalloc);
+    tmp = (char *)malloc(10 * sizeof(char));
+    semphrGiveConditional(useMutexOnMalloc, xSemaphoreMalloc);
+
+    strcpy(tmp, "yeah");
+
+    if (tryRealloc)
+    {
+      semphrTakeConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      tmp = (char *)realloc(tmp, 20 * sizeof(char));
+      semphrGiveConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      strcat(tmp, "_realloc");
+    }
+
+    semphrTakeConditional(useMutexOnFree, xSemaphoreFree);
+    free(tmp);
+    semphrGiveConditional(useMutexOnFree, xSemaphoreFree);
+
+    delay(DELAY);
+  }
+}
+
+void loop4(void *pvPramaters)
+{
+  (void) pvPramaters;
+  while (1)
+  {
+    _loop[4]++;
+    char *tmp;
+
+    semphrTakeConditional(useMutexOnMalloc, xSemaphoreMalloc);
+    tmp = (char *)malloc(10 * sizeof(char));
+    semphrGiveConditional(useMutexOnMalloc, xSemaphoreMalloc);
+
+    strcpy(tmp, "baz");
+
+    if (tryRealloc)
+    {
+      semphrTakeConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      tmp = (char *)realloc(tmp, 20 * sizeof(char));
+      semphrGiveConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      strcat(tmp, "_realloc");
+    }
+
+    semphrTakeConditional(useMutexOnFree, xSemaphoreFree);
+    free(tmp);
+    semphrGiveConditional(useMutexOnFree, xSemaphoreFree);
+
+    delay(DELAY);
+  }
+}
+
+void loop5(void *pvPramaters)
+{
+  (void) pvPramaters;
+  while (1)
+  {
+    _loop[5]++;
+    char *tmp;
+
+    semphrTakeConditional(useMutexOnMalloc, xSemaphoreMalloc);
+    tmp = (char *)malloc(10 * sizeof(char));
+    semphrGiveConditional(useMutexOnMalloc, xSemaphoreMalloc);
+
+    strcpy(tmp, "asd");
+
+    if (tryRealloc)
+    {
+      semphrTakeConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      tmp = (char *)realloc(tmp, 20 * sizeof(char));
+      semphrGiveConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      strcat(tmp, "_realloc");
+    }
+
+    semphrTakeConditional(useMutexOnFree, xSemaphoreFree);
+    free(tmp);
+    semphrGiveConditional(useMutexOnFree, xSemaphoreFree);
+
+    delay(DELAY);
+  }
+}
+
+void loop6(void *pvPramaters)
+{
+  (void) pvPramaters;
+  while (1)
+  {
+    _loop[6]++;
+    char *tmp;
+
+    semphrTakeConditional(useMutexOnMalloc, xSemaphoreMalloc);
+    tmp = (char *)malloc(10 * sizeof(char));
+    semphrGiveConditional(useMutexOnMalloc, xSemaphoreMalloc);
+
+    strcpy(tmp, "lol");
+
+    if (tryRealloc)
+    {
+      semphrTakeConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      tmp = (char *)realloc(tmp, 20 * sizeof(char));
+      semphrGiveConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      strcat(tmp, "_realloc");
+    }
+
+    semphrTakeConditional(useMutexOnFree, xSemaphoreFree);
+    free(tmp);
+    semphrGiveConditional(useMutexOnFree, xSemaphoreFree);
+
+    delay(DELAY);
+  }
+}
+
+void loop7(void *pvPramaters)
+{
+  (void) pvPramaters;
+  while (1)
+  {
+    _loop[7]++;
+    char *tmp;
+
+    semphrTakeConditional(useMutexOnMalloc, xSemaphoreMalloc);
+    tmp = (char *)malloc(10 * sizeof(char));
+    semphrGiveConditional(useMutexOnMalloc, xSemaphoreMalloc);
+
+    strcpy(tmp, "yay");
+
+    if (tryRealloc)
+    {
+      semphrTakeConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      tmp = (char *)realloc(tmp, 20 * sizeof(char));
+      semphrGiveConditional(useMutexOnRealloc, xSemaphoreRealloc);
+      strcat(tmp, "_realloc");
+    }
+
+    semphrTakeConditional(useMutexOnFree, xSemaphoreFree);
+    free(tmp);
+    semphrGiveConditional(useMutexOnFree, xSemaphoreFree);
+
+    delay(DELAY);
+  }
+}
+
+void semphrTakeConditional(bool useMutexOn, SemaphoreHandle_t xSemaphore)
+{
+  if (useMutexOn)
+  {
+    xSemaphoreTake(xSemaphore, TickType_t(portMAX_DELAY));
+  }
+}
+
+void semphrGiveConditional(bool useMutexOn, SemaphoreHandle_t xSemaphore)
+{
+  if (useMutexOn)
+  {
+    xSemaphoreGive(xSemaphore);
+  }
+}

libraries/FreeRTOS/src/variantHooks.cpp

@@ -25,24 +25,28 @@
 */
 #include <stdlib.h>
 
+/* FreeRTOS includes. */
+#include "FreeRTOS.h"
+#include "task.h"
+#include "timers.h"
+#include "semphr.h"
+
 /* Arduino Core includes */
 #include <Arduino.h>
 #include <RP2040USB.h>
 #include "tusb.h"
 
-
 /* Raspberry PI Pico includes */
 #include <pico.h>
 #include <pico/time.h>
 
-/* FreeRTOS includes. */
-#include "FreeRTOS.h"
-#include "task.h"
-#include "timers.h"
+#include "_freertos.h"
 
 /*-----------------------------------------------------------*/
 
+extern void __initFreeRTOSMutexes();
 void initFreeRTOS(void) {
+    __initFreeRTOSMutexes();
 }
 
 extern void setup() __attribute__((weak));
@@ -107,7 +111,7 @@ extern "C" void yield() {
 extern mutex_t __usb_mutex;
 static TaskHandle_t __usbTask;
 static void __usb(void *param);
-
+extern volatile bool __freeRTOSinitted;
 void startFreeRTOS(void) {
 
     TaskHandle_t c0;
@@ -121,6 +125,7 @@ void startFreeRTOS(void) {
     }
 
     // Initialise and run the freeRTOS scheduler. Execution should never return here.
+    __freeRTOSinitted = true;
     vTaskStartScheduler();
 
     while (true) {
@@ -392,3 +397,42 @@ void __USBStart() {
     xTaskCreate(__usb, "USB", 256, 0, configMAX_PRIORITIES - 1, &__usbTask);
     vTaskCoreAffinitySet(__usbTask, 1 << 0);
 }
+
+
+// Interfaces for the main core to use FreeRTOS mutexes
+extern "C" {
+
+    SemaphoreHandle_t __freertos_mutex_create() {
+        return xSemaphoreCreateMutex();
+    }
+
+    SemaphoreHandle_t _freertos_recursive_mutex_create() {
+        return xSemaphoreCreateRecursiveMutex();
+    }
+
+    void __freertos_mutex_take(SemaphoreHandle_t mtx) {
+        xSemaphoreTake(mtx, portMAX_DELAY);
+    }
+
+    int __freertos_mutex_try_take(SemaphoreHandle_t mtx) {
+        return xSemaphoreTake(mtx, 0);
+    }
+
+    void __freertos_mutex_give(SemaphoreHandle_t mtx) {
+        xSemaphoreGive(mtx);
+    }
+
+    void __freertos_recursive_mutex_take(SemaphoreHandle_t mtx) {
+        xSemaphoreTakeRecursive(mtx, portMAX_DELAY);
+    }
+
+    int __freertos_recursive_mutex_try_take(SemaphoreHandle_t mtx) {
+        return xSemaphoreTakeRecursive(mtx, 0);
+    }
+
+    void __freertos_recursive_mutex_give(SemaphoreHandle_t mtx) {
+        xSemaphoreGiveRecursive(mtx);
+    }
+
+}
+