[freertos] Make malloc/free thread-safe

ext/aws/modm_port.cpp

@@ -1,12 +1,66 @@
-
+/*
+ * Copyright (c) 2019-2020 Niklas Hauser
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+// ----------------------------------------------------------------------------
 
 #include <freertos/FreeRTOS.h>
 #include <freertos/task.h>
 #include <modm/architecture/interface/assert.hpp>
+#include <cstdlib>
 
 extern "C" void vApplicationStackOverflowHook(TaskHandle_t, char *);
-
 void vApplicationStackOverflowHook(TaskHandle_t /*pxTask*/, char *pcTaskName)
 {
 	modm_assert(false, "freertos.stack", "FreeRTOS detected a stack overflow!", pcTaskName);
 }
+
+// ----------------------------------------------------------------------------
+// Make the Newlib heap thread-safe with FreeRTOS
+
+extern "C" void __malloc_lock(struct _reent *);
+void __malloc_lock(struct _reent *)
+{
+	vTaskSuspendAll();
+}
+
+extern "C" void __malloc_unlock(struct _reent *);
+void __malloc_unlock(struct _reent *)
+{
+	xTaskResumeAll();
+}
+
+// ----------------------------------------------------------------------------
+// Make the FreeRTOS heap use Newlib heap
+
+extern "C" void *pvPortMalloc(size_t xWantedSize);
+void *pvPortMalloc(size_t xWantedSize)
+{
+	void *pvReturn = malloc(xWantedSize);
+	traceMALLOC(pvReturn, xWantedSize);
+
+#if configUSE_MALLOC_FAILED_HOOK == 1
+	if(pvReturn == nullptr)
+	{
+		extern "C" void vApplicationMallocFailedHook(void);
+		vApplicationMallocFailedHook();
+	}
+#endif
+
+	return pvReturn;
+}
+
+extern "C" void vPortFree(void *pv);
+void vPortFree(void *pv)
+{
+	if(pv)
+	{
+		free(pv);
+		traceFREE(pv, 0);
+	}
+}

ext/aws/module.lb

@@ -104,8 +104,6 @@ def build(env):
     env.copy("{}/port.c".format(path), "freertos/port.c")
     # Copy the portmacro.h file
     env.copy("{}/portmacro.h".format(path), "freertos/inc/freertos/portmacro.h")
-    # Copy the head_3.c file that just wraps the libc malloc
-    env.copy("freertos/FreeRTOS/Source/portable/MemMang/heap_3.c", "freertos/heap_3.c")
 
     # Generate the FreeRTOSConfig.h file
     env.template("FreeRTOSConfig.h.in", "freertos/inc/freertos/FreeRTOSConfig.h")

src/modm/platform/heap/cortex/heap_block.cpp

@@ -52,9 +52,14 @@ void __modm_initialize_memory(void)
 	allocator.initialize((void*)heap_start, (void*)heap_end);
 }
 
-void* __wrap__malloc_r(struct _reent *, size_t size)
+extern void __malloc_lock(struct _reent *);
+extern void __malloc_unlock(struct _reent *);
+
+void* __wrap__malloc_r(struct _reent *r, size_t size)
 {
+	__malloc_lock(r);
 	void *ptr = allocator.allocate(size);
+	__malloc_unlock(r);
 	modm_assert_continue_fail_debug(ptr, "malloc",
 			"No memory left in Block heap!", size);
 	return ptr;
@@ -74,9 +79,11 @@ void* __wrap__realloc_r(struct _reent *, void *, size_t size)
 	return NULL;
 }
 
-void __wrap__free_r(struct _reent *, void *p)
+void __wrap__free_r(struct _reent *r, void *p)
 {
+	__malloc_lock(r);
 	allocator.free(p);
+	__malloc_unlock(r);
 }
 
 } // extern "C"

src/modm/platform/heap/cortex/heap_tlsf.cpp

@@ -88,6 +88,9 @@ get_tlsf_for_ptr(void *p)
 	return NULL;
 }
 
+extern void __malloc_lock(struct _reent *);
+extern void __malloc_unlock(struct _reent *);
+
 void * malloc_traits(size_t size, uint32_t traits)
 {
 try_again:
@@ -97,7 +100,9 @@ void * malloc_traits(size_t size, uint32_t traits)
 	{
 		if ((pool->traits & traits) == traits)
 		{
+			__malloc_lock(_REENT);
 			void *p = tlsf_malloc(pool->tlsf, size);
+			__malloc_unlock(_REENT);
 			if (p) return p;
 		}
 	}
@@ -124,7 +129,8 @@ void * malloc_traits(size_t size, uint32_t traits)
 void *__wrap__malloc_r(struct _reent *, size_t size)
 {
 	// default is accessible by S-Bus and DMA-able
-	return malloc_traits(size, 0x8 | 0x1);
+	return malloc_traits(size, uint32_t(modm::MemoryTrait::AccessSBus) |
+							   uint32_t(modm::MemoryTrait::AccessDMA));
 }
 
 void *__wrap__calloc_r(struct _reent *r, size_t size)
@@ -134,26 +140,29 @@ void *__wrap__calloc_r(struct _reent *r, size_t size)
 	return ptr;
 }
 
-void *__wrap__realloc_r(struct _reent *, void *p, size_t size)
+void *__wrap__realloc_r(struct _reent *r, void *p, size_t size)
 {
-	tlsf_t pool = get_tlsf_for_ptr(p);
-	// if pointer belongs to no pool, exit.
-	if (!pool) return NULL;
+	void *ptr = NULL;
+
+	__malloc_lock(r);
+	const tlsf_t pool = get_tlsf_for_ptr(p);
+	if (pool) ptr = tlsf_realloc(pool, p, size);
+	__malloc_unlock(r);
 
-	void *ptr = tlsf_realloc(pool, p, size);
-	modm_assert_continue_fail_debug(0, "realloc",
+	modm_assert_continue_fail_debug(ptr, "realloc",
 			"Unable to realloc in TLSF pool!", size);
 	return ptr;
 }
 
-void __wrap__free_r(struct _reent *, void *p)
+void __wrap__free_r(struct _reent *r, void *p)
 {
 	// do nothing if NULL pointer
 	if (!p) return;
-	tlsf_t pool = get_tlsf_for_ptr(p);
-	// if pointer belongs to no pool, exit.
-	if (!pool) return;
-	tlsf_free(pool, p);
+	__malloc_lock(r);
+	const tlsf_t pool = get_tlsf_for_ptr(p);
+	// free if pointer belongs to a pool.
+	if (pool) tlsf_free(pool, p);
+	__malloc_unlock(r);
 }
 
 } // extern "C"