4 files changed, 35 insertions, 15 deletions

diff --git a/src/gpio.c b/src/gpio.c
index 29ed763..27bfdd3 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -20,23 +20,31 @@
 
 #include <gpio.h>
 
+static const GPIO_TypeDef *gpio_ports[8] = {
+	GPIOA, GPIOB, GPIOC, GPIOD,
+	GPIOE, GPIOF, GPIOG, GPIOH
+};
+
 void gpio_svc(uint32_t *args)
 {
+	GPIO_TypeDef *port = gpio_ports[args[1] / 16];
+	uint32_t pin = args[1] & 0xF;
+
 	switch (args[0]) {
 	case 0:
-		gpio_mode((GPIO_TypeDef *)args[1], args[2], args[3]);
+		gpio_mode(port, pin, args[2]);
 		break;
 	case 1:
-		gpio_type((GPIO_TypeDef *)args[1], args[2], args[3]);
+		gpio_type(port, pin, args[2]);
 		break;
 	case 2:
-		gpio_pupd((GPIO_TypeDef *)args[1], args[2], args[3]);
+		gpio_pupd(port, pin, args[2]);
 		break;
 	case 3:
-		gpio_speed((GPIO_TypeDef *)args[1], args[2], args[3]);
+		gpio_speed(port, pin, args[2]);
 		break;
 	case 4:
-		gpio_dout((GPIO_TypeDef *)args[1], args[2], args[3]);
+		gpio_dout(port, pin, args[2]);
 		break;
 	}
 }
diff --git a/src/main.c b/src/main.c
index c929d16..bbcd09b 100644
--- a/src/main.c
+++ b/src/main.c
@@ -54,12 +54,11 @@ int main(void)
 void task2(void);

 void kmain(void)

 {

-	gpio(GPIO_MODE, (uint32_t)GPIOA, 5, OUTPUT);

-	//gpio_mode(GPIOA, 5, OUTPUT);

+	gpio(GPIO_MODE, 5, OUTPUT);

 	task_start(task2, 512);

 

 	for (int i = 0; i < 8; i++) {

-		gpio_dout(GPIOA, 5, !(i & 1));

+		gpio(GPIO_OUT, 5, !(i & 1));

 		delay(200);

 	}

 }

@@ -76,7 +75,7 @@ void task3(void)
 	int state = 0;

 	delay(2500);

 	while (1) {

-		gpio_dout(GPIOA, 5, state ^= 1);

+		gpio(GPIO_OUT, 5, state ^= 1);

 		delay(500);

 	}

 }

diff --git a/src/svc.c b/src/svc.c
index 4342a95..f132571 100644
--- a/src/svc.c
+++ b/src/svc.c
@@ -22,14 +22,19 @@
 #include <stdint.h>
 #include <gpio.h>
 #include <clock.h>
+#include <task.h>
 
 extern void gpio_svc(uint32_t *);
 
 void svc_handler(uint32_t *args)
 {
-	uint32_t svc_number = ((char *)args[6])[-2];
+	/*uint32_t*/int svc_number = ((char *)args[6])[-2];
 
 	switch (svc_number) {
+	case -1:
+	case 0:
+		_exit(args[0]);
+		break;
 	case 1:
 		gpio_svc(args);
 		break;
diff --git a/src/task.c b/src/task.c
index d87f48d..4da2be1 100644
--- a/src/task.c
+++ b/src/task.c
@@ -48,7 +48,6 @@ void _exit(int code)
 	free(current);
 
 	SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
-	while (1);
 }
 
 /**
@@ -56,11 +55,18 @@ void _exit(int code)
  * Calls the task's main code, setting _exit() as the return point.
  */
 __attribute__ ((naked))
+void task_doexit(void)
+{
+	asm("eor r0, r0; svc 0");
+	while (1);
+}
+
+__attribute__ ((naked))
 void task_crt0(void)
 {
 	asm("\
 		mov r4, lr; \
-		ldr lr, =_exit; \
+		ldr lr, =task_doexit; \
 		bx r4; \
 	");
 }
@@ -95,8 +101,11 @@ task_t *task_create(void (*code)(void), uint32_t stackSize)
 void task_init(void (*init)(void))
 {
 	current = (task_t *)malloc(sizeof(task_t));
+	current->stack = 0;
+
 	task_t *init_task = task_create(init, 4096);
 
+	prev = init_task;
 	current->next = init_task;
 	init_task->next = init_task;
 
@@ -108,13 +117,12 @@ void task_init(void (*init)(void))
 		mov %0, r0; \
 		msr psp, r0; \
 		mrs r0, control; \
-		orr r0, r0, #2; \
+		orr r0, r0, #3; \
 		cpsie i; \
 		msr control, r0; \
 	" : "=r" (current->sp));
 
-	SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
-	while (1);
+	task_doexit();
 }
 
 void task_start(void (*task)(void), uint16_t stackSize)