Support for Keil compiler RAW32 CPU specific task switching for Cortex M0,1,3,4,7

4 files changed, 106 insertions, 70 deletions

diff --git a/src/gos/gos_x_threads.c b/src/gos/gos_x_threads.c
index 6f4a9ee7..7afd224a 100644
--- a/src/gos/gos_x_threads.c
+++ b/src/gos/gos_x_threads.c
@@ -186,7 +186,7 @@ typedef struct threadQ {
 
 static threadQ		readyQ;					// The list of ready threads
 static threadQ		deadQ;					// Where we put threads waiting to be deallocated
-static thread *		current;				// The current running thread
+thread *			_gfxCurrentThread;		// The current running thread - unfortunately this has to be non-static for the keil compiler
 static thread		mainthread;				// The main thread context
 
 #undef GFX_THREADS_DONE
@@ -399,7 +399,7 @@ static thread		mainthread;				// The main thread context
 				//	as we are on a different stack.
 
 				// Run the users function.
-				gfxThreadExit(current->fn(current->param));
+				gfxThreadExit(_gfxCurrentThread->fn(_gfxCurrentThread->param));
 
 				// We never get here as gfxThreadExit() never returns
 			}
@@ -451,11 +451,11 @@ void _gosThreadsInit(void) {
 
 	_gfxThreadsInit();
 
-	current = &mainthread;
+	_gfxCurrentThread = &mainthread;
 }
 
 gfxThreadHandle gfxThreadMe(void) {
-	return (gfxThreadHandle)current;
+	return (gfxThreadHandle)_gfxCurrentThread;
 }
 
 // Check if there are dead processes to deallocate
@@ -476,9 +476,9 @@ void gfxYield(void) {
 	if (!readyQ.head)
 		return;
 
-	Qadd(&readyQ, me = current);
-	current = Qpop(&readyQ);
-	_gfxTaskSwitch(me, current);
+	Qadd(&readyQ, me = _gfxCurrentThread);
+	_gfxCurrentThread = Qpop(&readyQ);
+	_gfxTaskSwitch(me, _gfxCurrentThread);
 }
 
 // This routine is not currently public - but it could be.
@@ -486,7 +486,7 @@ void gfxThreadExit(threadreturn_t ret) {
 	thread	*me;
 
 	// Save the results in case someone is waiting
-	me = current;
+	me = _gfxCurrentThread;
 	me->param = (void *)ret;
 	me->flags |= FLG_THD_DEAD;
 
@@ -496,11 +496,11 @@ void gfxThreadExit(threadreturn_t ret) {
 		Qadd(&deadQ, me);
 
 	// Set the next thread. Exit if it was the last thread
-	if (!(current = Qpop(&readyQ)))
+	if (!(_gfxCurrentThread = Qpop(&readyQ)))
 		gfxExit();
 
 	// Switch to the new thread
-	_gfxTaskSwitch(me, current);
+	_gfxTaskSwitch(me, _gfxCurrentThread);
 
 	// We never get back here as we didn't re-queue ourselves
 }
@@ -530,9 +530,9 @@ gfxThreadHandle gfxThreadCreate(void *stackarea, size_t stacksz, threadpriority_
 	t->param = param;
 
 	// Add the current thread to the queue because we are starting a new thread.
-	me = current;
+	me = _gfxCurrentThread;
 	Qadd(&readyQ, me);
-	current = t;
+	_gfxCurrentThread = t;
 
 	_gfxStartThread(me, t);
 
@@ -544,7 +544,7 @@ threadreturn_t gfxThreadWait(gfxThreadHandle th) {
 	thread *		t;
 
 	t = th;
-	if (t == current)
+	if (t == _gfxCurrentThread)
 		return -1;
 
 	// Mark that we are waiting
diff --git a/src/gos/gos_x_threads_cortexm01.h b/src/gos/gos_x_threads_cortexm01.h
index df2d1f78..8dfe1683 100644
--- a/src/gos/gos_x_threads_cortexm01.h
+++ b/src/gos/gos_x_threads_cortexm01.h
@@ -60,42 +60,54 @@
 
 #elif GFX_COMPILER == GFX_COMPILER_KEIL || GFX_COMPILER == GFX_COMPILER_ARMCC
 
-	static /*__arm*/ void _gfxTaskSwitch(thread *oldt, thread *newt) {
-/*		push	{r4, r5, r6, r7, lr}
+	static __asm void _gfxTaskSwitch(thread *oldt, thread *newt) {
+		// Save the old context
+		push	{r4, r5, r6, r7, lr}
 		mov     r4, r8
 		mov     r5, r9
 		mov     r6, r10
 		mov     r7, r11
 		push    {r4, r5, r6, r7}
-		str	sp, %[oldtcxt]
-		ldr	sp, %[newtcxt]
+		str	sp, [r0,#__cpp(offsetof(thread,cxt))]		// oldt->cxt
+		
+		// Load the new context
+		ldr	sp, [r1,#__cpp(offsetof(thread,cxt))]		// newt->cxt
 		pop     {r4, r5, r6, r7}
 		mov     r8, r4
 		mov     r9, r5
 		mov     r10, r6
 		mov     r11, r7
 		pop     {r4, r5, r6, r7, pc}
-		: [newtcxt] "=m" (newt->cxt)
-		: [oldtcxt] "m" (oldt->cxt)
-		: "memory");
-*/	}
+	}
 
-	static /* __arm */ void _gfxStartThread(thread *oldt, thread *newt) {
-		newt->cxt = (char *)newt + newt->size;
-/*		push    {r4, r5, r6, r7, lr}
+	static __asm void _gfxStartThread(thread *oldt, thread *newt) {
+		// Calculate where to generate the new context
+		//		newt->cxt = (char *)newt + newt->size;
+        ldr      r2,[r1,#__cpp(offsetof(thread,size))]
+        add      r2,r2,r1
+        str      r2,[r1,#__cpp(offsetof(thread,cxt))]
+		
+		// Save the old context
+		push	{r4, r5, r6, r7, lr}
 		mov     r4, r8
 		mov     r5, r9
 		mov     r6, r10
 		mov     r7, r11
 		push    {r4, r5, r6, r7}
-		str	sp, %[oldtcxt]
-		ldr	sp, %[newtcxt]
-		: [newtcxt] "=m" (newt->cxt)
-		: [oldtcxt] "m" (oldt->cxt)
-		: "memory");
-*/
-		// Run the users function
-		gfxThreadExit(current->fn(current->param));
+		str		sp, [r0,#__cpp(offsetof(thread,cxt))]	// oldt->cxt
+		
+		// Load the new (imcomplete) context
+		ldr		sp, [r1,#__cpp(offsetof(thread,cxt))]	// newt->cxt
+		
+		// Run the users function - we save some code because gfxThreadExit() never returns
+		//		gfxThreadExit(current->fn(current->param));
+        LDR      r2,__cpp(&_gfxCurrentThread)
+        LDR      r2,[r2,#0]
+        LDR      r0,[r2,#__cpp(offsetof(thread,param))]
+        LDR      r1,[r2,#__cpp(offsetof(thread,fn))]
+        BLX      r1
+        MOV      r4,r0
+        BL       gfxThreadExit
 	}
 
 #else
diff --git a/src/gos/gos_x_threads_cortexm347.h b/src/gos/gos_x_threads_cortexm347.h
index 0392e674..522fe9b8 100644
--- a/src/gos/gos_x_threads_cortexm347.h
+++ b/src/gos/gos_x_threads_cortexm347.h
@@ -46,27 +46,39 @@
 
 #elif GFX_COMPILER == GFX_COMPILER_KEIL || GFX_COMPILER == GFX_COMPILER_ARMCC
 
-	static /*__arm*/ void _gfxTaskSwitch(thread *oldt, thread *newt) {
-/*		push	{r4, r5, r6, r7, r8, r9, r10, r11, lr}
-		str	sp, %[oldtcxt]
-		ldr	sp, %[newtcxt]
+	static __asm void _gfxTaskSwitch(thread *oldt, thread *newt) {
+		// Save the old context
+		push	{r4, r5, r6, r7, r8, r9, r10, r11, lr}
+		str	sp, [r0,#__cpp(offsetof(thread,cxt))]		// oldt->cxt
+		
+		// Load the new context
+		ldr	sp, [r1,#__cpp(offsetof(thread,cxt))]		// newt->cxt
 		pop	{r4, r5, r6, r7, r8, r9, r10, r11, pc}
-		: [newtcxt] "=m" (newt->cxt)
-		: [oldtcxt] "m" (oldt->cxt)
-		: "memory");
-*/	}
+	}
 
-	static /* __arm */ void _gfxStartThread(thread *oldt, thread *newt) {
-		newt->cxt = (char *)newt + newt->size;
-/*		push	{r4, r5, r6, r7, r8, r9, r10, r11, lr}
-		str	sp, %[oldtcxt]
-		ldr	sp, %[newtcxt]
-		: [newtcxt] "=m" (newt->cxt)
-		: [oldtcxt] "m" (oldt->cxt)
-		: "memory");
-*/
-		// Run the users function
-		gfxThreadExit(current->fn(current->param));
+	static __asm void _gfxStartThread(thread *oldt, thread *newt) {
+		// Calculate where to generate the new context
+		//		newt->cxt = (char *)newt + newt->size;
+        ldr      r2,[r1,#__cpp(offsetof(thread,size))]
+        add      r2,r2,r1
+        str      r2,[r1,#__cpp(offsetof(thread,cxt))]
+		
+		// Save the old context
+		push	{r4, r5, r6, r7, r8, r9, r10, r11, lr}
+		str		sp, [r0,#__cpp(offsetof(thread,cxt))]	// oldt->cxt
+		
+		// Load the new (imcomplete) context
+		ldr		sp, [r1,#__cpp(offsetof(thread,cxt))]	// newt->cxt
+		
+		// Run the users function - we save some code because gfxThreadExit() never returns
+		//		gfxThreadExit(current->fn(current->param));
+        LDR      r2,__cpp(&_gfxCurrentThread)
+        LDR      r2,[r2,#0]
+        LDR      r0,[r2,#__cpp(offsetof(thread,param))]
+        LDR      r1,[r2,#__cpp(offsetof(thread,fn))]
+        BLX      r1
+        MOV      r4,r0
+        BL       gfxThreadExit
 	}
 
 #else
diff --git a/src/gos/gos_x_threads_cortexm47fp.h b/src/gos/gos_x_threads_cortexm47fp.h
index 0d4d0d89..2fc15d3c 100644
--- a/src/gos/gos_x_threads_cortexm47fp.h
+++ b/src/gos/gos_x_threads_cortexm47fp.h
@@ -48,30 +48,42 @@
 
 #elif GFX_COMPILER == GFX_COMPILER_KEIL || GFX_COMPILER == GFX_COMPILER_ARMCC
 
-	static /*__arm*/ void _gfxTaskSwitch(thread *oldt, thread *newt) {
-/*		push	{r4, r5, r6, r7, r8, r9, r10, r11, lr}
+	static __asm void _gfxTaskSwitch(thread *oldt, thread *newt) {
+		// Save the old context
+		push	{r4, r5, r6, r7, r8, r9, r10, r11, lr}
 		vpush	{s16-s31}
-		str	sp, %[oldtcxt]
-		ldr	sp, %[newtcxt]
+		str	sp, [r0,#__cpp(offsetof(thread,cxt))]		// oldt->cxt
+		
+		// Load the new context
+		ldr	sp, [r1,#__cpp(offsetof(thread,cxt))]		// newt->cxt
 		vpop	{s16-s31}
 		pop	{r4, r5, r6, r7, r8, r9, r10, r11, pc}
-		: [newtcxt] "=m" (newt->cxt)
-		: [oldtcxt] "m" (oldt->cxt)
-		: "memory");
-*/	}
+	}
 
-	static /* __arm */ void _gfxStartThread(thread *oldt, thread *newt) {
-		newt->cxt = (char *)newt + newt->size;
-/*		push	{r4, r5, r6, r7, r8, r9, r10, r11, lr}
+	static __asm void _gfxStartThread(thread *oldt, thread *newt) {
+		// Calculate where to generate the new context
+		//		newt->cxt = (char *)newt + newt->size;
+        ldr      r2,[r1,#__cpp(offsetof(thread,size))]
+        add      r2,r2,r1
+        str      r2,[r1,#__cpp(offsetof(thread,cxt))]
+		
+		// Save the old context
+		push	{r4, r5, r6, r7, r8, r9, r10, r11, lr}
 		vpush	{s16-s31}
-		str	sp, %[oldtcxt]
-		ldr	sp, %[newtcxt]
-		: [newtcxt] "=m" (newt->cxt)
-		: [oldtcxt] "m" (oldt->cxt)
-		: "memory");
-*/
-		// Run the users function
-		gfxThreadExit(current->fn(current->param));
+		str		sp, [r0,#__cpp(offsetof(thread,cxt))]	// oldt->cxt
+		
+		// Load the new (imcomplete) context
+		ldr		sp, [r1,#__cpp(offsetof(thread,cxt))]	// newt->cxt
+		
+		// Run the users function - we save some code because gfxThreadExit() never returns
+		//		gfxThreadExit(current->fn(current->param));
+        LDR      r2,__cpp(&_gfxCurrentThread)
+        LDR      r2,[r2,#0]
+        LDR      r0,[r2,#__cpp(offsetof(thread,param))]
+        LDR      r1,[r2,#__cpp(offsetof(thread,fn))]
+        BLX      r1
+        MOV      r4,r0
+        BL       gfxThreadExit
 	}
 
 #else