Major multicore fixes Newlib and FreeRTOS (#640)

Instead of wrapping the memory functions in the link stage, rebuild
Newlib and enable retargetable locks.  Override the weak definitions
in the libc.a with our own, SDK based ones.

The wrapping utilized before catches app-level memory allocations
but misses allocations inside Newlib libc (like printf/etc.).

Each core needs its own _impure_ptr or else crashes like the one seen
in parallel printf_floats can happen.  Enable it in the toolchain
build and implement a simple swapper here.

FreeRTOS SMP doesn't support Newlib's dynamic reent which is needed
to allow save MT support.  Minor patch to FreeRTOS and update the
FreeRTOS variant.cpp and setup to support it.

Author: earlephilhower

cores/rp2040/RP2040USB.cpp

@@ -130,7 +130,7 @@ static uint8_t *GetDescHIDReport(int *len) {
     return __hid_report;
 }
 
-static void __SetupDescHIDReport() {
+void __SetupDescHIDReport() {
     if (__USBInstallKeyboard && __USBInstallMouse) {
         uint8_t desc_hid_report[] = {
             TUD_HID_REPORT_DESC_KEYBOARD(HID_REPORT_ID(1)),
@@ -179,7 +179,7 @@ const uint8_t *tud_descriptor_configuration_cb(uint8_t index) {
     return usbd_desc_cfg;
 }
 
-static void __SetupUSBDescriptor() {
+void __SetupUSBDescriptor() {
     if (!usbd_desc_cfg) {
         bool hasHID = __USBInstallKeyboard || __USBInstallMouse;
 

cores/rp2040/lock.cpp

@@ -0,0 +1,89 @@
+/*
+    Newlib retargetable lock class using RP2040 SDK
+
+    Overrides weak functions in Newlib for locking to safely support
+    multi-core operation.  Does not need any --wrap for memory allocators.
+
+    Copyright (c) 2022 Earle F. Philhower, III <[email protected]>
+
+    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 <pico/mutex.h>
+#include <sys/lock.h>
+
+recursive_mutex_t __lock___sinit_recursive_mutex;
+recursive_mutex_t __lock___sfp_recursive_mutex;
+recursive_mutex_t __lock___atexit_recursive_mutex;
+mutex_t __lock___at_quick_exit_mutex;
+recursive_mutex_t __lock___malloc_recursive_mutex;
+recursive_mutex_t __lock___env_recursive_mutex;
+mutex_t __lock___tz_mutex;
+mutex_t __lock___dd_hash_mutex;
+mutex_t __lock___arc4random_mutex;
+
+
+__attribute__((constructor)) void __init_all_newlib_mutexes() {
+    recursive_mutex_init(&__lock___sinit_recursive_mutex);
+    recursive_mutex_init(&__lock___sfp_recursive_mutex);
+    recursive_mutex_init(&__lock___atexit_recursive_mutex);
+    mutex_init(&__lock___at_quick_exit_mutex);
+    recursive_mutex_init(&__lock___malloc_recursive_mutex);
+    recursive_mutex_init(&__lock___env_recursive_mutex);
+    mutex_init(&__lock___tz_mutex);
+    mutex_init(&__lock___dd_hash_mutex);
+    mutex_init(&__lock___arc4random_mutex);
+}
+
+void __retarget_lock_init(_LOCK_T *lock) {
+    mutex_init((mutex_t*) lock);
+}
+
+void __retarget_lock_init_recursive(_LOCK_T *lock) {
+    recursive_mutex_init((recursive_mutex_t*) lock);
+}
+
+void __retarget_lock_close(_LOCK_T lock) {
+    (void) lock;
+}
+
+void __retarget_lock_close_recursive(_LOCK_T lock) {
+    (void) lock;
+}
+
+void __retarget_lock_acquire(_LOCK_T lock) {
+    mutex_enter_blocking((mutex_t*)lock);
+}
+
+void __retarget_lock_acquire_recursive(_LOCK_T lock) {
+    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 __retarget_lock_try_acquire_recursive(_LOCK_T lock) {
+    return recursive_mutex_try_enter((recursive_mutex_t*)lock, NULL);
+}
+
+void __retarget_lock_release(_LOCK_T lock) {
+    mutex_exit((mutex_t*)lock);
+}
+
+void __retarget_lock_release_recursive(_LOCK_T lock) {
+    recursive_mutex_exit((recursive_mutex_t*)lock);
+}

cores/rp2040/main.cpp

@@ -22,6 +22,7 @@
 #include "RP2040USB.h"
 #include <pico/stdlib.h>
 #include <pico/multicore.h>
+#include <reent.h>
 
 RP2040 rp2040;
 extern "C" {
@@ -72,6 +73,7 @@ extern void __loop() {
         arduino::serialEvent2Run();
     }
 }
+static struct _reent *_impure_ptr1 = nullptr;
 
 extern "C" int main() {
 #if F_CPU != 125000000
@@ -81,6 +83,12 @@ extern "C" int main() {
     // Let rest of core know if we're using FreeRTOS
     __isFreeRTOS = initFreeRTOS ? true : false;
 
+    // Allocate impure_ptr (newlib temps) if there is a 2nd core running
+    if (!__isFreeRTOS && (setup1 || loop1)) {
+        _impure_ptr1 = (struct _reent*)calloc(sizeof(struct _reent), 1);
+        _REENT_INIT_PTR(_impure_ptr1);
+    }
+
     mutex_init(&_pioMutex);
 
     rp2040.begin();
@@ -142,3 +150,21 @@ extern "C" int main() {
 extern "C" unsigned long ulMainGetRunTimeCounterValue() {
     return rp2040.getCycleCount64();
 }
+
+extern "C" void __register_impure_ptr(struct _reent *p) {
+    if (get_core_num() == 0) {
+        _impure_ptr = p;
+    } else {
+        _impure_ptr1 = p;
+    }
+}
+
+
+// TODO:  FreeRTOS should implement this based on thread ID (and each thread should have its own struct _reent
+extern "C" struct _reent *__wrap___getreent() {
+    if (get_core_num() == 0) {
+        return _impure_ptr;
+    } else {
+        return _impure_ptr1;
+    }
+}

lib/platform_wrap.txt

@@ -71,7 +71,6 @@
 -Wl,--wrap=atanf
 -Wl,--wrap=atanh
 -Wl,--wrap=atanhf
--Wl,--wrap=calloc
 -Wl,--wrap=cbrt
 -Wl,--wrap=cbrtf
 -Wl,--wrap=ceil
@@ -105,7 +104,6 @@
 -Wl,--wrap=fmaf
 -Wl,--wrap=fmod
 -Wl,--wrap=fmodf
--Wl,--wrap=free
 -Wl,--wrap=hypot
 -Wl,--wrap=hypotf
 -Wl,--wrap=ldexp
@@ -118,7 +116,6 @@
 -Wl,--wrap=log2
 -Wl,--wrap=log2f
 -Wl,--wrap=logf
--Wl,--wrap=malloc
 -Wl,--wrap=memcpy
 -Wl,--wrap=memset
 -Wl,--wrap=__popcountdi2
@@ -127,7 +124,6 @@
 -Wl,--wrap=powf
 -Wl,--wrap=powint
 -Wl,--wrap=powintf
--Wl,--wrap=realloc
 -Wl,--wrap=remainder
 -Wl,--wrap=remainderf
 -Wl,--wrap=remquo
@@ -148,3 +144,4 @@
 -Wl,--wrap=tanhf
 -Wl,--wrap=trunc
 -Wl,--wrap=truncf
+-Wl,--wrap=__getreent

libraries/FreeRTOS/lib/FreeRTOS-Kernel

@@ -27,6 +27,11 @@
 #define configSUPPORT_STATIC_ALLOCATION 0
 #define configSTACK_DEPTH_TYPE			uint32_t
 
+#define configUSE_NEWLIB_REENTRANT 1
+#include <reent.h>
+extern void __register_impure_ptr(struct _reent *p);
+#define portSET_IMPURE_PTR(x) __register_impure_ptr(x)
+
 /* Run time stats related definitions. */
 void vMainConfigureTimerForRunTimeStats( void );
 unsigned long ulMainGetRunTimeCounterValue( void );

libraries/FreeRTOS/src/FreeRTOSConfig.h

@@ -409,10 +409,16 @@ static void __usb(void *param)
     }
 }
 
+extern void __SetupDescHIDReport();
+extern void __SetupUSBDescriptor();
 
 void __USBStart()
 {
     mutex_init(&__usb_mutex);
+
+    __SetupDescHIDReport();
+    __SetupUSBDescriptor();
+
     // Make highest prio and locked to core 0
     xTaskCreate(__usb, "USB", 256, 0, configMAX_PRIORITIES - 1, &__usbTask);
     vTaskCoreAffinitySet( __usbTask, 1 << 0 );