rejoice! tasks suck no more

3 files changed, 88 insertions, 58 deletions

diff --git a/src/heap.c b/src/heap.c
index 2157aad..5d32a14 100644
--- a/src/heap.c
+++ b/src/heap.c
@@ -1,4 +1,5 @@
-#include "heap.h"
+#include <heap.h>
+#include <task.h>
 
 #define HEAP_ALIGN 16
 
@@ -20,6 +21,9 @@ void heap_init(void *buf)
 
 void *malloc(uint32_t size)
 {
+	task_hold(1);
+	if (size < 16)
+		size = 16;
 	alloc_t *node = &root;
 	while (node->next & 1 || node->size < size) {
 		if ((node->next & ~(1)) == 0) {
@@ -34,6 +38,7 @@ void *malloc(uint32_t size)
 	}
 
 	node->next |= 1;
+	task_hold(0);
 
 	return (void *)((uint32_t)node + sizeof(alloc_t));
 }
@@ -53,4 +58,3 @@ void free(void *buf)
 	alloc_t *alloc = (alloc_t *)((uint32_t)buf - sizeof(alloc_t));
 	alloc->next &= ~(1);
 }
-
diff --git a/src/main.c b/src/main.c
index c70d24b..0245801 100644
--- a/src/main.c
+++ b/src/main.c
@@ -9,6 +9,7 @@
 #include <initrd.h>

 #include <serial.h>

 #include <parser.h>

+#include <builtins.h>

 #include <stack.h>

 #include <string.h>

 

@@ -100,6 +101,32 @@ int script_ppos(interpreter *it)
 	return 0;

 }

 

+int script_color(interpreter *it)

+{

+	uint16_t c = dsp_color(igetarg_integer(it, 0), igetarg_integer(it, 1),

+		igetarg_integer(it, 2));

+	variable v;

+	v.valtype = INTEGER;

+	v.value = c;

+	v.svalue = 0;

+	isetstr(&v);

+	iret(it, &v);

+	return 0;

+}

+

+int script_rand(interpreter *it)

+{

+	static uint32_t next = 1;

+	next = (next * 182 + 1829) % igetarg_integer(it, 0);

+	variable v;

+	v.valtype = INTEGER;

+	v.value = next;

+	v.svalue = 0;

+	isetstr(&v);

+	iret(it, &v);

+	return 0;

+}

+

 void task_interpreter(void)

 {

 	interpreter it;

@@ -110,6 +137,8 @@ void task_interpreter(void)
 	inew_cfunc(&it, "rect", script_rect);

 	inew_cfunc(&it, "ppos", script_ppos);

 	inew_cfunc(&it, "line", script_line);

+	inew_cfunc(&it, "color", script_color);

+	inew_cfunc(&it, "rand", script_rand);

 

 	/*int ret = 0;

 	char *linebuf = malloc(100), c[2] = {0, 0};

@@ -134,8 +163,10 @@ void task_interpreter(void)
 	}*/

 

 	char *s = initrd_getfile("init");

-	if (s == 0)

+	if (s == 0) {

+		dsp_puts("init not found");

 		goto end;

+	}

 

 	char *linebuf = (char *)malloc(120);

 	uint32_t i = 0, prev = 0, lc;

@@ -150,7 +181,9 @@ void task_interpreter(void)
 		}

 		strncpy(linebuf, s + prev, lc + 1);

 		linebuf[lc] = '\0';

+		//task_hold(1);

 		ret = idoline(&it, linebuf);

+		//task_hold(0);

 		if (ret < 0)

 			break;

 		prev = ++i;

@@ -171,7 +204,6 @@ end:
 void kmain(void)

 {

 	asm("cpsie i");

-

 	dsp_init();

 	dsp_rect(0, 0, LCD_WIDTH, LCD_HEIGHT, dsp_color(0, 0, 0));

 	dsp_cursoron();

diff --git a/src/task.c b/src/task.c
index b290e3c..13e4e75 100644
--- a/src/task.c
+++ b/src/task.c
@@ -3,77 +3,78 @@
 #include <stm32l476xx.h>
 
 typedef struct {
-	uint32_t *sp;
-	uint8_t use;
+	void *next;
 	uint32_t *stack;
-	void (*code)(void);
+	uint32_t *sp;
 } task_t;
 
-#define MAX_TASKS	6
+static task_t *current;
+static uint8_t task_disable = 0;
 
-static task_t tasks[MAX_TASKS];
-static volatile int next_idx = 0;
+void task_hold(uint8_t hold)
+{
+	if (hold != 0)
+		task_disable++;
+	else if (task_disable > 0)
+		task_disable--;
+}
 
-static uint8_t task_enable = 0;
+void task_exit(void)
+{
+	// TODO free stack?
+	// TODO remove from chain
+	// hopefully current is preserved..?
+	while (1); // bye
+}
 
-void task_hold(uint8_t hold)
+task_t *task_create(void (*code)(void), uint32_t stackSize)
 {
-	task_enable = !hold;
+	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;
+	return t;
 }
 
 void task_init(void (*init)(void))
 {
-	for (int i = 0; i < MAX_TASKS; i++)
-		tasks[i].use = 0;
-
-	task_start(init, 4096);
+	current = task_create(init, 4096);
+	current->next = current;
+	// bit 0 - priv, bit 1 - psp/msp
 	asm("\
 		msr psp, %0; \
 		mrs r0, control; \
-		orr r0, r0, #3; \
+		orr r0, r0, #2; \
 		msr control, r0; \
 		isb; \
-	" :: "r" (tasks[0].sp));
+	" :: "r" (current->sp));
 
-	task_enable = 1;
+	task_disable = 0;
 	init();
-}
-
-extern void _exit(int);
-void task_exit(void)
-{
-	// TODO free stack?
-	asm("cpsid i"); // hope to catch next_idx
-	tasks[next_idx].use = 0;
-	asm("cpsie i");
-	while (1); // bye
+	// you dirty dirty dog
+	/*asm("\
+		cpsie i; \
+		mov pc, %0; \
+	" :: "r" (init + 4));*/
 }
 
 void task_start(void (*task)(void), uint16_t stackSize)
 {
-	asm("cpsid i"); // just to be safe
-
-	for (int i = 0; i < MAX_TASKS; i++) {
-		if (tasks[i].use == 0) {
-			tasks[i].stack = malloc(stackSize);
-			tasks[i].sp = tasks[i].stack + stackSize - 16;
-			tasks[i].sp[13] = (uint32_t)task_exit;
-			tasks[i].sp[14] = (uint32_t)task;
-			tasks[i].sp[15] = 0x01000000;
-			tasks[i].use = 1;
-			tasks[i].code = task;
-			asm("cpsie i");
-			return;
-		}
-	}
-
-	// TODO handle error
+	task_hold(1);
+	task_t *t = task_create(task, stackSize);
+	task_t *next = (task_t *)current->next;
+	current->next = t;
+	t->next = next;
+	task_hold(0);
 }
 
 __attribute__ ((naked))
 void PendSV_Handler(void) 
 {
-	if (task_enable == 0)
+	if (task_disable != 0)
 		asm("bx lr");
 
 	// save state
@@ -84,15 +85,9 @@ void PendSV_Handler(void)
 		mrs r0, psp; \
 		stmdb r0!, {r4-r11}; \
 		mov %0, r0; \
-	" : "=r" (tasks[next_idx].sp));
+	" : "=r" (current->sp));
 
-	// find next task (round-robin style)
-	do {
-		if (++next_idx == MAX_TASKS) {
-			next_idx = 0;
-			break; // task 0 better exist
-		}
-	} while (tasks[next_idx].use == 0);
+	current = current->next;
 
 	// restore
 	asm("\
@@ -101,11 +96,10 @@ void PendSV_Handler(void)
 		msr psp, r0; \
 		isb; \
 		dsb; \
-	" :: "r" (tasks[next_idx].sp));
+	" :: "r" (current->sp));
 
 	// end
 	asm("\
-		mov r0, #0xFFFFFFFD; \
 		cpsie i; \
 		bx lr; \
 	");