3 files changed, 20 insertions, 16 deletions
diff --git a/os/src/init.c b/os/src/init.c
index 070fac1..4b518ab 100644
--- a/os/src/init.c
+++ b/os/src/init.c
@@ -20,7 +20,7 @@ void _exit(int return_code) {
 void _start() {
     BoardEarlyInit();
     BoardConsoleInit();
-    BoardConsolePuts("uC-sdk - booting.");    
+    BoardConsolePuts("uC-sdk - booting.");
 //    __sinit(_impure_ptr);
     __libc_init_array();
     BoardLateInit();
diff --git a/os/src/malloc.c b/os/src/malloc.c
index 0906cc0..a24a87f 100644
--- a/os/src/malloc.c
+++ b/os/src/malloc.c
@@ -2,7 +2,18 @@
 #include <malloc.h>
 #include <osdebug.h>
 
+static xSemaphoreHandle malloc_sem = NULL;
+
+__attribute__((constructor)) static void malloc_init() {
+    malloc_sem = xSemaphoreCreateMutex();
+}
+
 void * malloc(size_t size) {
-    DBGOUT("malloc(%u)\r\n", size);
-    return _malloc_r(_impure_ptr, size);
+    void * ptr;
+    
+    if (malloc_sem)
+        xSemaphoreTake(malloc_sem, portMAX_DELAY);
+    ptr =_malloc_r(_impure_ptr, size);
+    if (malloc_sem)
+        xSemaphoreGive(malloc_sem);
 }
diff --git a/os/src/sbrk.c b/os/src/sbrk.c
index df196f0..ac8649c 100644
--- a/os/src/sbrk.c
+++ b/os/src/sbrk.c
@@ -17,25 +17,19 @@ extern uintptr_t __stack_start;
 /* Low-level bulk RAM allocator -- used by Newlib's Malloc */
 static void *heap_end = (void *) &__heap_start;
 
-#define USE_SBRK_MUTEX 0
-
-#if USE_SBRK_MUTEX
-static xSemaphoreHandle sbrk_sem;
+static xSemaphoreHandle sbrk_sem = NULL;
 
 __attribute__((constructor)) static void sbrk_init() {
-    DBGOUT("Creating Mutex...");
     sbrk_sem = xSemaphoreCreateMutex();
 }
-#endif
 
 void *_sbrk_r(struct _reent *ptr, ptrdiff_t incr)
 {
     void *prev_heap_end, *next_heap_end, *ret;
     void *stack_min = (void *) &__stack_start;
-
-#if USE_SBRK_MUTEX
-    xSemaphoreTake(sbrk_sem, portMAX_DELAY);
-#endif
+    
+    if (sbrk_sem)
+        xSemaphoreTake(sbrk_sem, portMAX_DELAY);
 
     prev_heap_end = heap_end;
 
@@ -51,9 +45,8 @@ void *_sbrk_r(struct _reent *ptr, ptrdiff_t incr)
         ret = (void *)prev_heap_end;
     }
 
-#if USE_SBRK_MUTEX
-    xSemaphoreGive(sbrk_sem);
-#endif
+    if (sbrk_sem)
+        xSemaphoreGive(sbrk_sem);
 
     return ret;
 }