5 files changed, 41 insertions, 18 deletions

diff --git a/FreeRTOS/Source/queue.c b/FreeRTOS/Source/queue.c
index a62d179..b34e102 100644
--- a/FreeRTOS/Source/queue.c
+++ b/FreeRTOS/Source/queue.c
@@ -53,6 +53,7 @@
 

 #include <stdlib.h>

 #include <string.h>

+#include <osdebug.h>
 

 /* Defining MPU_WRAPPERS_INCLUDED_FROM_API_FILE prevents task.h from redefining

 all the API functions to use the MPU wrappers.  That should only be done when

@@ -1092,11 +1093,20 @@ static void prvCopyDataFromQueue( xQUEUE * const pxQueue, const void *pvBuffer )
 {

 	if( pxQueue->uxQueueType != queueQUEUE_IS_MUTEX )

 	{

+	        DBGOUT("ret: %p\r\n", __builtin_return_address(0));
+		DBGOUT("pxQueue: %p\r\n", pxQueue);
+		DBGOUT("pxQueue->uxQueueType: %d\r\n", pxQueue->uxQueueType);
+		DBGOUT("pxQueue->pcReadFrom:  %p\r\n", pxQueue->pcReadFrom);
+		DBGOUT("pxQueue->uxItemSize:  %d\r\n", pxQueue->uxItemSize);
 		pxQueue->pcReadFrom += pxQueue->uxItemSize;

+		DBGOUT("pxQueue->pcReadFrom:  %p\r\n", pxQueue->pcReadFrom);
 		if( pxQueue->pcReadFrom >= pxQueue->pcTail )

 		{

 			pxQueue->pcReadFrom = pxQueue->pcHead;

 		}

+		DBGOUT("pxQueue->pcHead:      %p\r\n", pxQueue->pcHead);
+		DBGOUT("pxQueue->pcReadFrom:  %p\r\n", pxQueue->pcReadFrom);
+		DBGOUT("pvBuffer: %p\r\n", pvBuffer);
 		memcpy( ( void * ) pvBuffer, ( void * ) pxQueue->pcReadFrom, ( unsigned ) pxQueue->uxItemSize );

 	}

 }

diff --git a/demo.c b/demo.c
index 41eaacb..59b41ca 100644
--- a/demo.c
+++ b/demo.c
@@ -1,5 +1,6 @@
 #include <FreeRTOS.h>
 #include <task.h>
+#include <semphr.h>
 #include <lpc17xx_gpio.h>
 #include <BoardConsole.h>
 #include <osdebug.h>
@@ -31,16 +32,22 @@ static void litLED(int led, int value) {
     }
 }
 
+xSemaphoreHandle handle;
+
 static void simpleTask1(void *p) {
     while (1) {
+        xSemaphoreTake(handle, portMAX_DELAY);
         BoardConsolePuts("Task 1");
+        xSemaphoreGive(handle);
         vTaskDelay(1234);
     }
 }
 
 static void simpleTask2(void *p) {
     while (1) {
+        xSemaphoreTake(handle, portMAX_DELAY);
         BoardConsolePuts("Task 2");
+        xSemaphoreGive(handle);
         vTaskDelay(1357);
     }
 }
@@ -52,14 +59,16 @@ static void badTask(void *x) {
 }
 
 int main() {
+    handle = xSemaphoreCreateMutex();
+
     setupLEDs();
     litLED(1, 0);
     litLED(2, 0);
     litLED(3, 0);
     litLED(4, 0);
     BoardConsolePuts("Creating simple tasks.");
-    xTaskCreate(simpleTask1, (signed char *) "st1", configMINIMAL_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY | portPRIVILEGE_BIT, NULL);
-    xTaskCreate(simpleTask2, (signed char *) "st2", configMINIMAL_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY | portPRIVILEGE_BIT, NULL);
+    xTaskCreate(simpleTask1, (signed char *) "st1", configMINIMAL_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY, NULL);
+    xTaskCreate(simpleTask2, (signed char *) "st2", configMINIMAL_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY, NULL);
     xTaskCreate(badTask, (signed char *) "bad", configMINIMAL_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY, NULL);
     BoardConsolePuts("Scheduler starting.");
     vTaskStartScheduler();
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;
 }