diff --git a/include/priv_gpio.h b/include/priv_gpio.h
index eee5e18..a97e294 100644
--- a/include/priv_gpio.h
+++ b/include/priv_gpio.h
@@ -7,20 +7,18 @@
 #define GPIO_SPEED 3
 #define GPIO_OUT 4
 
-void gpio(uint32_t call, uint32_t port, uint32_t pin, uint32_t value) 
+void gpio(uint32_t call, uint32_t pin, uint32_t value) 
 {
 	register uint32_t r0 asm("r0") = call;
-	register uint32_t r1 asm("r1") = port;
-	register uint32_t r2 asm("r2") = pin;
-	register uint32_t r3 asm("r3") = value;
+	register uint32_t r1 asm("r1") = pin;
+	register uint32_t r2 asm("r2") = value;
 
 	asm("\
 		mov r0, %0; \
 		mov r1, %1; \
 		mov r2, %2; \
-		mov r3, %3; \
 		svc 1; \
-	" :: "r" (r0), "r" (r1), "r" (r2), "r" (r3));
+	" :: "r" (r0), "r" (r1), "r" (r2));
 }
 
 #endif // PRIV_GPIO_H_
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,19 +48,25 @@ void _exit(int code)
 	free(current);
 
 	SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
-	while (1);
 }
 
 /**
  * 'Prepares' task for running.
  * 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)