CoreMutex freeRTOS Mutex acquire properly (#1481)

Since FreeRTOS has real tasks and mutexes with support for priority bumping, actually always try and take a `CoreMutex` instead of seeing if someone else already has it and aborting immediately.

This fix helps ensure things like Serial output in a multi-task system won't get lost.

cores/rp2040/CoreMutex.cpp

@@ -30,13 +30,10 @@ CoreMutex::CoreMutex(mutex_t *mutex, uint8_t option) {
     _option = option;
     if (__isFreeRTOS) {
         auto m = __get_freertos_mutex_for_ptr(mutex);
-        if (__freertos_check_if_in_isr()) {
-            __freertos_mutex_take_from_isr(m);
-        } else {
-            if (!__freertos_mutex_try_take(m)) {
-                return;
-            }
+        if (__freertos_check_if_in_isr() && !__freertos_mutex_take_from_isr(m)) {
+            return;
         }
+        __freertos_mutex_take(m);
     } else {
         uint32_t owner;
         if (!mutex_try_enter(_mutex, &owner)) {

cores/rp2040/_freertos.h

@@ -43,7 +43,7 @@ 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_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));

libraries/FreeRTOS/src/variantHooks.cpp

@@ -58,8 +58,8 @@ extern "C" {
         xSemaphoreTake(mtx, portMAX_DELAY);
     }
 
-    void __freertos_mutex_take_from_isr(SemaphoreHandle_t mtx) {
-        xSemaphoreTakeFromISR(mtx, NULL);
+    int __freertos_mutex_take_from_isr(SemaphoreHandle_t mtx) {
+        return xSemaphoreTakeFromISR(mtx, NULL);
     }
 
     int __freertos_mutex_try_take(SemaphoreHandle_t mtx) {