1 files changed, 39 insertions, 23 deletions

diff --git a/src/task.c b/src/task.c
index d0cb3f1..6a755de 100644
--- a/src/task.c
+++ b/src/task.c
@@ -2,13 +2,7 @@
 #include <heap.h>
 #include <stm32l476xx.h>
 
-typedef struct {
-	void *next;
-	uint32_t *stack;
-	uint32_t *sp;
-} task_t;
-
-static task_t *current;
+task_t *current;
 static uint8_t task_disable = 0;
 
 void task_hold(uint8_t hold)
@@ -32,10 +26,20 @@ task_t *task_create(void (*code)(void), uint32_t stackSize)
 	task_t *t = (task_t *)malloc(sizeof(task_t));
 	t->next = 0;
 	t->stack = (uint32_t *)malloc(stackSize);
-	t->sp = (void *)t->stack + stackSize - 64;//16;
-	t->sp[13] = (uint32_t)task_exit;
-	t->sp[14] = (uint32_t)code;
-	t->sp[15] = 0x01000000;
+	void *sp = (uint8_t *)t->stack + stackSize - 68; // excep. stack + regs
+	t->sp = (uint32_t *)sp;
+	for (uint8_t i = 0; i < 14; i++)
+		t->sp[i] = 0;
+	t->sp[8] = 0xFFFFFFFD;
+	t->sp[14] = 0xFFFFFFFD;
+	t->sp[15] = (uint32_t)code;
+	t->sp[16] = 0x01000000;
+
+	//void *sp = (uint8_t *)t->stack + stackSize - 64; // 16 words
+	//t->sp = (uint32_t *)sp;
+	//t->sp[13] = (uint32_t)task_exit;
+	//t->sp[14] = (uint32_t)code;
+	//t->sp[15] = 0x01000000;
 	return t;
 }
 
@@ -43,6 +47,8 @@ void task_init(void (*init)(void))
 {
 	current = task_create(init, 4096);
 	current->next = current;
+	task_disable = 0;
+
 	// bit 0 - priv, bit 1 - psp/msp
 	asm("\
 		msr psp, %0; \
@@ -50,14 +56,8 @@ void task_init(void (*init)(void))
 		orr r0, r0, #2; \
 		msr control, r0; \
 		isb; \
-	" :: "r" (current->sp));
-
-	task_disable = 0;
-	init();
-	/*asm("\
-		cpsie i; \
-		mov pc, %0; \
-	" :: "r" (init + 4));*/
+		bx %1; \
+	" :: "r" (current->sp), "r" (init));
 }
 
 void task_start(void (*task)(void), uint16_t stackSize)
@@ -75,13 +75,29 @@ void PendSV_Handler(void)
 	if (task_disable != 0)
 		asm("bx lr");
 
-	// save state
+	asm("\
+		mrs r0, psp; \
+		isb; \
+		ldr r1, =current; \
+		ldr r2, [r1]; \
+		stmdb r0!, {r4-r11, r14}; \
+		str r0, [r2, #8]; \
+		ldr r0, [r2, #0]; \
+		str r0, [r1]; \
+		ldr r2, [r1]; \
+		ldr r0, [r2, #8]; \
+		ldmia r0!, {r4-r11, r14}; \
+		msr psp, r0; \
+		bx lr; \
+	");
+
+	/*// save state
 	asm("\
 		cpsid i; \
 		isb; \
 		dsb; \
 		mrs r0, psp; \
-		stmdb r0!, {r4-r11}; \
+		stmdb r0!, {r4-r11, lr}; \
 		mov %0, r0; \
 	" : "=r" (current->sp));
 
@@ -90,12 +106,12 @@ void PendSV_Handler(void)
 	// restore
 	asm("\
 		mov r0, %0; \
-		ldmia r0!, {r4-r11}; \
+		ldmia r0!, {r4-r11, lr}; \
 		msr psp, r0; \
 		isb; \
 		dsb; \
 		cpsie i; \
 		bx lr; \
-	" :: "r" (current->sp));
+	" :: "r" (current->sp));*/
 }