diff --git a/src/kernel/clock.c b/src/kernel/clock.c
index 5bfc93a..5b80203 100644
--- a/src/kernel/clock.c
+++ b/src/kernel/clock.c
@@ -31,12 +31,10 @@ extern task_t *current;
 
 void clock_svc(uint32_t *args)
 {
-	if (args[0] == 0) {
-		current->sleep = ticks + args[1];
-		SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
-	} else if (args[0] == 1) {
+	if (args[0] == 0)
+		task_sleep(args[1]);
+	else if (args[0] == 1)
 		udelay(args[1]);
-	}
 }
 
 void clock_init(void)
diff --git a/src/kernel/svc.c b/src/kernel/svc.c
index edf2b68..4aa156e 100644
--- a/src/kernel/svc.c
+++ b/src/kernel/svc.c
@@ -1,7 +1,6 @@
 /**
  * @file svc.c
  * An unused handler for SVC calls
- * TODO: use SVC calls, possibly allowing for switch to unprivileged mode?
  *
  * Copyright (C) 2018 Clyne Sullivan
  *
@@ -28,9 +27,17 @@ extern void gpio_svc(uint32_t *);
 extern void clock_svc(uint32_t *);
 extern void task_svc(uint32_t *);
 
-void svc_handler(uint32_t *args)
-{
-	/*uint32_t*/int svc_number = ((char *)args[6])[-2];
+void SVC_Handler(void) {
+	uint32_t *args;
+
+	asm("\
+		tst lr, #4; \
+		ite eq; \
+		mrseq %0, msp; \
+		mrsne %0, psp; \
+	" : "=r" (args));
+
+	int svc_number = ((char *)args[6])[-2];
 
 	switch (svc_number) {
 	case -1:
@@ -44,24 +51,16 @@ void svc_handler(uint32_t *args)
 		clock_svc(args);
 		break;
 	case 3:
+		asm("\
+			mrs r0, psp; \
+			stmdb r0!, {r4-r11, r14}; \
+			mov %0, r0; \
+		" : "=r" (args[0]));
 		task_svc(args);
-		asm("mov r0, %0" :: "r" (args[0]));
+		asm("mov r0, %0" :: "r" (args[0])); // TODO doesn't work, r0 overwritten on exc. return
 		break;
 	default:
 		break;
 	}
 }
 
-void SVC_Handler(void) {
-	uint32_t *args;
-
-	asm("\
-		tst lr, #4; \
-		ite eq; \
-		mrseq %0, msp; \
-		mrsne %0, psp; \
-	" : "=r" (args));
-
-	svc_handler(args);
-}
-
diff --git a/src/kernel/task.c b/src/kernel/task.c
index 96051a9..4f61e1d 100644
--- a/src/kernel/task.c
+++ b/src/kernel/task.c
@@ -23,8 +23,10 @@
 #include "task.h"
 #include <arch/stm/stm32l476xx.h>
 
-task_t *current, *prev;
+static task_t *task_current;
+static task_t *task_queue;
 static uint8_t task_disable = 0;
+static uint32_t task_next_pid = 0;
 
 int task_fork(uint32_t sp);
 void task_svc(uint32_t *args)
@@ -41,21 +43,41 @@ void task_hold(uint8_t hold)
 		task_disable--;
 }
 
+void task_sleep(uint32_t ms)
+{
+	task_current->sleep = millis() + ms;
+	SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+}
+
+uint32_t task_getpid(void)
+{
+	return task_current->pid;
+}
+
 void _exit(int code)
 {
 	(void)code;
 
-	while (prev == 0);
+	if (task_queue == task_current) {
+		task_queue = task_queue->next;
+	} else {
+		task_t *prev = task_queue;
+		while (prev->next != 0 && prev->next != task_current)
+			prev = prev->next;
 
-	prev->next = current->next;
+		if (prev->next != 0)
+			prev->next = task_current->next;
+	}
 
 	// Free this thread's stack, and task data.
 	// Since we're single core, no one else can claim this memory until
 	// a task switch, after which we're done with this memory anyway.
-	free(current->stack);
-	free(current);
+	free(task_current->stack);
+	free(task_current);
 
 	SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+	// TODO if child proc. set return code in parent task handle?
+	// or something like that
 }
 
 
@@ -87,6 +109,9 @@ task_t *task_create(void (*code)(void), uint16_t stackSize)
 {
 	task_t *t = (task_t *)malloc(sizeof(task_t));
 	t->next = 0;
+	t->sleep = 0;
+	t->pid = task_next_pid++;
+
 	t->stack = (uint32_t *)malloc(stackSize);
 	void *sp = (uint8_t *)t->stack + stackSize - 68; // excep. stack + regs
 	t->sp = sp;
@@ -107,20 +132,18 @@ task_t *task_create(void (*code)(void), uint16_t stackSize)
 	t->sp[14] = (uint32_t)code;
 	t->sp[15] = (uint32_t)task_crt0;
 	t->sp[16] = 0x01000000;
-	t->sleep = 0;
 	return t;
 }
 
 void task_init(void (*init)(void), uint16_t stackSize)
 {
-	current = (task_t *)malloc(sizeof(task_t));
-	current->stack = 0; // free() is called on this
-
-	task_t *init_task = task_create(init, stackSize);
+	task_current = (task_t *)malloc(sizeof(task_t));
+	task_current->next = 0;
+	task_current->stack = 0; // free() is called on this
+	task_current->sp = 0;
+	task_current->sleep = 1000;
 
-	prev = init_task;
-	current->next = init_task;
-	init_task->next = init_task;
+	task_queue = task_create(init, stackSize);
 
 	task_disable = 0;
 
@@ -143,8 +166,8 @@ void task_start(void (*task)(void), uint16_t stackSize)
 {
 	task_hold(1);
 	task_t *t = task_create(task, stackSize);
-	t->next = current->next;
-	current->next = t;
+	t->next = task_queue;
+	task_queue = t;
 	task_hold(0);
 }
 
@@ -156,32 +179,37 @@ int task_fork_ret(void)
 // Return 0 for child, non-zero for parent
 int task_fork(uint32_t sp)
 {
-	task_hold(1);
+	asm("cpsid i");
 
-	// 1. Get a PC for the child
-	void (*pc)(void) = (void (*)(void))((uint32_t)task_fork_ret);
-
-	// 2. Prepare child task
-	alloc_t *stackInfo = (alloc_t *)(((uint8_t *)current->stack)
+	//// 1. Prepare child task
+	// Get parent task's stack info
+	alloc_t *stackInfo = (alloc_t *)(((uint8_t *)task_current->stack)
 		- sizeof(alloc_t));
-	task_t *childTask = task_create(pc, stackInfo->size - 8);
-	for (uint32_t i = 0; i < (stackInfo->size - 8); i++)
-		childTask->stack[i] = current->stack[i];
 
-	//uint32_t *sp;
-	//asm("mov %0, sp" : "=r" (sp));
+	// Create child task data
+	task_t *childTask = (task_t *)malloc(sizeof(task_t));
+	childTask->stack = (uint32_t *)malloc(stackInfo->size - sizeof(alloc_t));
+	childTask->sleep = 0;
+	childTask->pid = task_next_pid++;
+
+	// Copy parent's stack
+	for (uint32_t i = 0; i < (stackInfo->size - sizeof(alloc_t)); i++)
+		childTask->stack[i] = task_current->stack[i];
+
 	childTask->sp = (uint32_t *)((uint32_t)childTask->stack + (sp
-		- (uint32_t)current->stack));
+		- (uint32_t)task_current->stack));
+	childTask->sp[15] = (uint32_t)task_fork_ret;
+	//childTask->sp[16] = 0x01000000;
 
-	// 3. Insert child into task chain
-	childTask->next = current->next;
-	current->next = childTask;
+	//// 2. Insert child into task chain
+	childTask->next = task_queue;
+	task_queue = childTask;
 
-	// 4. Re-enable scheduler, make change happen
-	task_hold(0);
+	//// 3. Re-enable scheduler, make change happen
+	asm("cpsie i");
 
 	SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
-	return 1;
+	return childTask->pid;
 }
 
 __attribute__ ((naked))
@@ -198,15 +226,21 @@ void PendSV_Handler(void)
 		isb; \
 		stmdb r0!, {r4-r11, r14}; \
 		mov %0, r0; \
-	" : "=r" (current->sp));
+	" : "=r" (task_current->sp));
 
 	// Load next task
-	prev = current;
 	uint32_t ticks = millis();
 	do {
-		current = current->next;
-	} while (current->sleep > ticks);
-	current->sleep = 0;
+		task_current = task_current->next;
+		if (task_current == 0)
+			task_current = task_queue;
+	} while (task_current->sleep > ticks);
+	task_current->sleep = 0;
+
+	/*task_current = task_current->next;
+	if (task_current == 0)
+		task_current = task_queue;*/
+
 
 	// Load stack pointer, return
 	asm("\
@@ -214,6 +248,6 @@ void PendSV_Handler(void)
 		ldmia r0!, {r4-r11, r14}; \
 		msr psp, r0; \
 		bx lr; \
-	" :: "r" (current->sp));
+	" :: "r" (task_current->sp));
 }
 
diff --git a/src/kernel/task.h b/src/kernel/task.h
index 472e909..0e2e07a 100644
--- a/src/kernel/task.h
+++ b/src/kernel/task.h
@@ -26,11 +26,12 @@
 /**
  * A structure to contain task data.
  */
-typedef struct {
-	void *next;      /**< pointer to the next task_t instance */
-	uint32_t *stack; /**< pointer to the task's stack */
-	uint32_t *sp;    /**< pointer to the task's last sp register value */
-	uint32_t sleep;  /**< number of milliseconds task is sleeping for */
+typedef struct task_t {
+	struct task_t *next; /**< pointer to the next task_t instance */
+	uint32_t *sp;        /**< pointer to the task's last sp register value */
+	uint32_t *stack;     /**< pointer to the task's stack */
+	uint32_t sleep;      /**< number of milliseconds task is sleeping for */
+	uint32_t pid;
 } task_t;
 
 /**
@@ -61,4 +62,8 @@ void task_hold(uint8_t hold);
  */
 void _exit(int code);
 
+void task_sleep(uint32_t ms);
+
+uint32_t task_getpid(void);
+
 #endif // TASK_H_
diff --git a/src/user/user.c b/src/user/user.c
index 7a55678..87f4b49 100644
--- a/src/user/user.c
+++ b/src/user/user.c
@@ -17,7 +17,6 @@ int fork(void)
 {
 	int result;
 	asm("\
-		mov r0, sp; \
 		svc 3; \
 		mov %0, r0; \
 	" : "=r" (result));
@@ -31,14 +30,14 @@ void user_main(void)
 	if (fork() == 0) {
 		while (1) {
 			gpio(GPIO_OUT, 5, 1);
-			user_delay(1000);
+			user_delay(2000);
 		}
 	} else {
 		while (1) {
+			user_delay(1000);
 			gpio(GPIO_OUT, 5, 0);
-			user_delay(500);
+			user_delay(1000);
 		}
-	
 	}
 }