diff --git a/run.sh b/run.sh
index a1c344c..8253d05 100755
--- a/run.sh
+++ b/run.sh
@@ -3,7 +3,7 @@
 # Starts openocd and connects gdb to the target, for programming/debugging
 #
 
-openocd -f /usr/share/openocd/scripts/board/st_nucleo_l476rg.cfg &
+openocd -f /usr/local/share/openocd/scripts/board/st_nucleo_l4.cfg &
 sleep 1 # Need to wait some time for openocd to connect
 gdb-multiarch -ex "target remote localhost:3333" main.elf
 pkill openocd # Ensure openocd exits when we're done
diff --git a/src/initrd/Makefile b/src/initrd/Makefile
index 82785f4..3385008 100644
--- a/src/initrd/Makefile
+++ b/src/initrd/Makefile
@@ -16,6 +16,7 @@ all: $(OFILES)
 files/%: %.c
 	@echo "  CC     " $<
 	@$(CROSS)$(CC) $(CFLAGS) $< ../libgpio/libgpio.a -o $@
+	@cp -v $@ init.debug
 	@$(CROSS)strip $@
 
 clean:
diff --git a/src/initrd/init.c b/src/initrd/init.c
index dc88ff8..ad3be41 100644
--- a/src/initrd/init.c
+++ b/src/initrd/init.c
@@ -10,8 +10,7 @@
 int main(void)
 {
 	gpioMode(5, OUTPUT);
-
-	printf("Hello, world!\n");
+	printf("Hello, world!\n\r");
 
 	while (1) {
 		gpioWrite(5, 1);
diff --git a/src/kernel/clock.c b/src/kernel/clock.c
index a49b4c1..41a3199 100644
--- a/src/kernel/clock.c
+++ b/src/kernel/clock.c
@@ -27,14 +27,14 @@ volatile uint32_t clock_ticks = 0;
 
 volatile uint8_t tim2_finished = 1;
 
-void clock_svc(uint32_t *args)
+void clock_svc(uint32_t n, uint32_t *ret, uint32_t *args)
 {
-	if (args[0] == 0)
-		task_sleep(args[1]);
-	else if (args[0] == 1)
-		clock_udelay(args[1]);
-	else if (args[0] == 2)
-		*((unsigned int *)args[1]) = clock_millis();
+	if (n == 0)
+		task_sleep(args[0]);
+	else if (n == 1)
+		clock_udelay(args[0]);
+	else if (n == 2)
+		*((unsigned int *)ret) = clock_millis();
 }
 
 void clock_init(void)
diff --git a/src/kernel/gpio.c b/src/kernel/gpio.c
index 33423b9..3bd67cb 100644
--- a/src/kernel/gpio.c
+++ b/src/kernel/gpio.c
@@ -25,29 +25,29 @@ static GPIO_TypeDef *gpio_ports[8] = {
 	GPIOE, GPIOF, GPIOG, GPIOH
 };
 
-void gpio_svc(uint32_t *args)
+void gpio_svc(uint32_t n, uint32_t *ret, uint32_t *args)
 {
-	GPIO_TypeDef *port = gpio_ports[args[1] >> 4];
-	uint32_t pin = args[1] & 0xF;
+	GPIO_TypeDef *port = gpio_ports[args[0] >> 4];
+	uint32_t pin = args[0] & 0xF;
 
-	switch (args[0]) {
+	switch (n) {
 	case 0:
-		gpio_mode(port, pin, args[2]);
+		gpio_mode(port, pin, args[1]);
 		break;
 	case 1:
-		gpio_type(port, pin, args[2]);
+		gpio_type(port, pin, args[1]);
 		break;
 	case 2:
-		gpio_pupd(port, pin, args[2]);
+		gpio_pupd(port, pin, args[1]);
 		break;
 	case 3:
-		gpio_speed(port, pin, args[2]);
+		gpio_speed(port, pin, args[1]);
 		break;
 	case 4:
-		gpio_dout(port, pin, args[2]);
+		gpio_dout(port, pin, args[1]);
 		break;
 	case 5:
-		*((int *)args[2]) = gpio_din(port, pin);
+		*((int *)ret) = gpio_din(port, pin);
 		break;
 	}
 }
diff --git a/src/kernel/svc.c b/src/kernel/svc.c
index 9044141..7c81e5f 100644
--- a/src/kernel/svc.c
+++ b/src/kernel/svc.c
@@ -23,22 +23,25 @@
 #include "clock.h"
 #include "task.h"
 
-extern void gpio_svc(uint32_t *);
-extern void clock_svc(uint32_t *);
-extern void task_svc(uint32_t *);
-extern void vfs_svc(uint32_t *args);
+extern void gpio_svc(uint32_t, uint32_t *, uint32_t *);
+extern void clock_svc(uint32_t, uint32_t *, uint32_t *);
+extern void task_svc(uint32_t, uint32_t *, uint32_t *);
+extern void vfs_svc(uint32_t, uint32_t *, uint32_t *args);
 
 void SVC_Handler(void) {
-	uint32_t *args;
+	uint32_t *stack;
 
 	asm("\
 		tst lr, #4; \
 		ite eq; \
 		mrseq %0, msp; \
 		mrsne %0, psp; \
-	" : "=r" (args));
+	" : "=r" (stack));
 
-	int svc_number = ((char *)args[6])[-2];
+	int svc_number = ((char *)stack[6])[-2];
+	uint32_t min_number = stack[0];
+	uint32_t *ret = (uint32_t *)stack[1];
+	uint32_t *args = (uint32_t *)stack[2];
 
 	switch (svc_number) {
 	case -1:
@@ -50,7 +53,7 @@ void SVC_Handler(void) {
 		 * 4 - sbrk (TODO bad)
 		 * 5 - execve
 		 */
-		task_svc(args);
+		task_svc(min_number, ret, args);
 		break;
 
 	case 1: /* GPIO-related calls
@@ -61,7 +64,7 @@ void SVC_Handler(void) {
 		 * 4 - gpio_dout
 		 * 5 - gpio_din
 		 */
-		gpio_svc(args);
+		gpio_svc(min_number, ret, args);
 		break;
 
 	case 2: /* Clock-related calls
@@ -69,7 +72,7 @@ void SVC_Handler(void) {
 		 * 1 - udelay
 		 * 2 - ticks
 		 */
-		clock_svc(args);
+		clock_svc(min_number, ret, args);
 		break;
 
 	case 3: /* Filesystem-related calls
@@ -79,7 +82,7 @@ void SVC_Handler(void) {
 		 * 3 - read
 		 * 4 - write
 		 */
-		vfs_svc(args);
+		vfs_svc(min_number, ret, args);
 		break;
 	default:
 		break;
diff --git a/src/kernel/task.c b/src/kernel/task.c
index c5d09d4..304e608 100644
--- a/src/kernel/task.c
+++ b/src/kernel/task.c
@@ -31,28 +31,28 @@ static task_t *task_queue;
 static uint8_t task_disable = 0;
 static pid_t task_next_pid = 0;
 
-void task_svc(uint32_t *args)
+void task_svc(uint32_t n, uint32_t *ret, uint32_t *args)
 {
-	switch (args[0]) {
+	switch (n) {
 	case 0:
-		task_exit(args[1]);
+		task_exit(args[0]);
 		break;
 	case 1:
-		*((int *)args[1]) = task_fork();
+		*((int *)ret) = task_fork();
 		break;
 	case 2:
-		*((int *)args[1]) = task_getpid();
+		*((int *)ret) = task_getpid();
 		break;
 	case 3:
-		*((int *)args[4]) = task_waitpid(args[1], (int *)args[2],
-			args[3]);
+		*((int *)ret) = task_waitpid(args[0], (int *)args[1],
+			args[2]);
 		break;
 	case 4:
-		*((void **)args[2]) = task_sbrk(args[1]);
+		*((void **)ret) = task_sbrk(args[0]);
 		break;
 	case 5:
-		*((uint32_t *)args[4]) = elf_execve((const char *)args[1],
-			(char * const *)args[2], (char * const *)args[3]);
+		*((uint32_t *)ret) = elf_execve((const char *)args[0],
+			(char * const *)args[1], (char * const *)args[2]);
 		break;
 	default:
 		break;
@@ -325,7 +325,7 @@ void PendSV_Handler(void)
 			task_current = task_queue;
 
 		if (task_current->status.state == TASK_SLEEPING &&
-			task_current->status.value <= ticks)
+			task_current->status.value < ticks)
 			task_current->status.state = TASK_RUNNING;
 	} while (task_current->status.state != TASK_RUNNING);
 
diff --git a/src/kernel/vfs.c b/src/kernel/vfs.c
index b9be9bc..8d4b975 100644
--- a/src/kernel/vfs.c
+++ b/src/kernel/vfs.c
@@ -35,24 +35,24 @@ static vfs_file vfs_files[VFS_MAX_FILES];
 	if (pid != fpid && fpid != 0) \
 		return 0; }
 
-void vfs_svc(uint32_t *args)
+void vfs_svc(uint32_t n, uint32_t *ret, uint32_t *args)
 {
-	switch (args[0]) {
+	switch (n) {
 	case 0:
-		*((int *)args[3]) = vfs_mount((vfs_volume_funcs *)args[1],
-			args[2]);
+		*((int *)ret) = vfs_mount((vfs_volume_funcs *)args[0],
+			args[1]);
 		break;
 	case 1:
-		*((int *)args[3]) = vfs_open((const char *)args[1], args[2]);
+		*((int *)ret) = vfs_open((const char *)args[0], args[1]);
 		break;
 	case 2:
-		*((int *)args[2]) = vfs_close(args[1]);
+		*((int *)ret) = vfs_close(args[0]);
 		break;
 	case 3:
-		*((int *)args[4]) = vfs_read(args[1], args[2], (uint8_t *)args[3]);
+		*((int *)ret) = vfs_read(args[0], args[1], (uint8_t *)args[2]);
 		break;
 	case 4:
-		*((int *)args[4]) = vfs_write(args[1], args[2], (const uint8_t *)args[3]);
+		*((int *)ret) = vfs_write(args[0], args[1], (const uint8_t *)args[2]);
 		break;
 	default:
 		break;
diff --git a/src/libgpio/gpio.c b/src/libgpio/gpio.c
index fdc877c..522817f 100644
--- a/src/libgpio/gpio.c
+++ b/src/libgpio/gpio.c
@@ -2,77 +2,79 @@
 
 #include <stdint.h>
 
+#define NOOPTIMIZE __attribute__((optimize(0)))
+
+NOOPTIMIZE
 void gpioMode(gpio_pin_t pin, int mode)
 {
-	register uint32_t r1 __asm("r1") = pin;
-	register uint32_t r2 __asm("r2") = mode;
-	__asm("\
+	uint32_t args[2] = { (uint32_t)pin, mode };
+	__asm volatile("\
 		mov r0, 0; \
-		mov r1, %0; \
-		mov r2, %1; \
+		mov r1, 0; \
+		mov r2, %0; \
 		svc 1; \
-	" :: "r" (r1), "r" (r2));
+	" :: "r" (args));
 }
 
+NOOPTIMIZE
 void gpioType(gpio_pin_t pin, int type)
 {
-	register uint32_t r1 __asm("r1") = pin;
-	register uint32_t r2 __asm("r2") = type;
-	__asm("\
+	uint32_t args[2] = { (uint32_t)pin, type };
+	__asm volatile("\
 		mov r0, 1; \
-		mov r1, %0; \
-		mov r2, %1; \
+		mov r1, 0; \
+		mov r2, %0; \
 		svc 1; \
-	" :: "r" (r1), "r" (r2));
+	" :: "r" (args));
 }
 
+NOOPTIMIZE
 void gpioPuPd(gpio_pin_t pin, int pupd)
 {
-	register uint32_t r1 __asm("r1") = pin;
-	register uint32_t r2 __asm("r2") = pupd;
-	__asm("\
+	uint32_t args[2] = { (uint32_t)pin, pupd };
+	__asm volatile("\
 		mov r0, 2; \
-		mov r1, %0; \
-		mov r2, %1; \
+		mov r1, 0; \
+		mov r2, %0; \
 		svc 1; \
-	" :: "r" (r1), "r" (r2));
+	" :: "r" (args));
 }
 
+NOOPTIMIZE
 void gpioSpeed(gpio_pin_t pin, int speed)
 {
-	register uint32_t r1 __asm("r1") = pin;
-	register uint32_t r2 __asm("r2") = speed;
-	__asm("\
+	uint32_t args[2] = { (uint32_t)pin, speed };
+	__asm volatile("\
 		mov r0, 3; \
-		mov r1, %0; \
-		mov r2, %1; \
+		mov r1, 0; \
+		mov r2, %0; \
 		svc 1; \
-	" :: "r" (r1), "r" (r2));
+	" :: "r" (args));
 }
 
+NOOPTIMIZE
 void gpioWrite(gpio_pin_t pin, int value)
 {
-	register uint32_t r1 __asm("r1") = pin;
-	register uint32_t r2 __asm("r2") = value;
-	__asm("\
+	uint32_t args[2] = { (uint32_t)pin, value };
+	__asm volatile("\
 		mov r0, 4; \
-		mov r1, %0; \
-		mov r2, %1; \
+		mov r1, 0; \
+		mov r2, %0; \
 		svc 1; \
-	" :: "r" (r1), "r" (r2));
+	" :: "r" (args));
 }
 
+NOOPTIMIZE
 int gpioRead(gpio_pin_t pin)
 {
-	int ret = 0;
-	register uint32_t r1 __asm("r1") = pin;
-	register uint32_t r2 __asm("r2") = (uint32_t)&ret;
-	__asm("\
+	volatile int ret = 0;
+	uint32_t args[1] = { (uint32_t)pin };
+	__asm volatile("\
 		mov r0, 5; \
 		mov r1, %0; \
 		mov r2, %1; \
 		svc 1; \
-	" :: "r" (r1), "r" (r2));
+	" :: "r" (&ret), "r" (args));
 	return ret;
 }
 
diff --git a/src/libgpio/libgpio.a b/src/libgpio/libgpio.a
index b8c781d..36cb0e8 100644
Binary files a/src/libgpio/libgpio.a and b/src/libgpio/libgpio.a differ
diff --git a/src/pdclib/platform/stmos/functions/os/syscalls.c b/src/pdclib/platform/stmos/functions/os/syscalls.c
index 912b988..823da23 100644
--- a/src/pdclib/platform/stmos/functions/os/syscalls.c
+++ b/src/pdclib/platform/stmos/functions/os/syscalls.c
@@ -1,72 +1,73 @@
 #include "syscalls.h"
 
+#define NOOPTIMIZE __attribute__((optimize(0)))
+
 //
 // Task-related calls
 
+NOOPTIMIZE
 void _exit(int code)
 {
-	register uint32_t r1 __asm("r1") = code;
-	__asm("\
+	uint32_t args[1] = { code };
+	__asm volatile("\
 		mov r0, 0; \
-		mov r1, %0; \
+		mov r1, 0; \
+		mov r2, %0; \
 		svc 0; \
-	" :: "r" (r1));
+	" :: "r" (args));
 }
 
+NOOPTIMIZE
 int fork(void)
 {
-	int ret = 0;
-	register uint32_t r1 __asm("r1") = (uint32_t)&ret;
-	__asm("\
+	volatile uint32_t ret = 0;
+	__asm volatile("\
 		mov r0, 1; \
 		mov r1, %0; \
+		mov r2, 0; \
 		svc 0; \
-	" :: "r" (r1));
+	" :: "r" (&ret));
 	return ret;
 }
 
+NOOPTIMIZE
 int getpid(void)
 {
-	int ret = 0;
-	register uint32_t r1 __asm("r1") = (uint32_t)&ret;
-	__asm("\
+	volatile uint32_t ret = 0;
+	__asm volatile("\
 		mov r0, 2; \
 		mov r1, %0; \
+		mov r2, 0; \
 		svc 0; \
-	" :: "r" (r1));
+	" :: "r" (&ret));
 	return ret;
 }
 
+NOOPTIMIZE
 void *sbrk(unsigned int bytes)
 {
-	uint32_t ret = 0;
-	register uint32_t r1 __asm("r1") = bytes;
-	register uint32_t r2 __asm("r2") = (uint32_t)&ret;
+	volatile uint32_t ret = 0;
+	uint32_t args[1] = { bytes };
 	__asm("\
 		mov r0, 4; \
 		mov r1, %0; \
 		mov r2, %1; \
 		svc 0; \
-	" :: "r" (r1), "r" (r2));
-	__asm("mov %0, r2" : "=r" (ret));
-	return *((void **)ret);
+	" :: "r" (&ret), "r" (args));
+	return (void *)ret;
 }
 
+NOOPTIMIZE
 int execve(const char *file, char * const argv[], char * const envp[])
 {
 	volatile uint32_t ret = 0;
-	register uint32_t r1 __asm("r1") = (uint32_t)file;
-	register uint32_t r2 __asm("r2") = (uint32_t)argv;
-	register uint32_t r3 __asm("r3") = (uint32_t)envp;
-	register uint32_t r12 __asm("r12") = (uint32_t)&ret;
+	uint32_t args[3] = { (uint32_t)file, (uint32_t)argv, (uint32_t)envp };
 	__asm("\
 		mov r0, 5; \
 		mov r1, %0; \
 		mov r2, %1; \
-		mov r3, %2; \
-		mov r12, %3; \
 		svc 0; \
-	" :: "r" (r1), "r" (r2), "r" (r3), "r" (r12));
+	" :: "r" (&ret), "r" (args));
 
 	if (ret == (uint32_t)-1)
 		return ret;
@@ -77,110 +78,101 @@ int execve(const char *file, char * const argv[], char * const envp[])
 //
 // Clock-related calls
 
+NOOPTIMIZE
 void delay(unsigned int ms)
 {
-	register uint32_t r1 __asm("r1") = ms;
+	uint32_t args[1] = { ms };
 	__asm("\
 		mov r0, 0; \
-		mov r1, %0; \
+		mov r1, 0; \
+		mov r2, %0; \
 		svc 2; \
-	" :: "r" (r1));
+	" :: "r" (args));
 }
 
+NOOPTIMIZE
 unsigned int ticks(void)
 {
-	unsigned int ret = 0;
-	register uint32_t r1 __asm("r1") = (uint32_t)&ret;
+	volatile unsigned int ret = 0;
 	__asm("\
 		mov r0, 2; \
 		mov r1, %0; \
+		mov r2, 0; \
 		svc 2; \
-	" :: "r" (r1));
+	" :: "r" (&ret));
 	return ret;
 }
 
 //
 // File-related calls
 
+NOOPTIMIZE
 int mount(vfs_volume_funcs *funcs, uint32_t flags)
 {
-	int ret = 0;
-	register uint32_t r1 __asm("r1") = (uint32_t)funcs;
-	register uint32_t r2 __asm("r2") = flags;
-	register uint32_t r3 __asm("r3") = (uint32_t)&ret;
+	volatile int ret = 0;
+	uint32_t args[2] = { (uint32_t)funcs, flags };
 	__asm("\
 		mov r0, 0; \
 		mov r1, %0; \
 		mov r2, %1; \
-		mov r3, %2; \
 		svc 3; \
-	" :: "r" (r1), "r" (r2), "r" (r3));
+	" :: "r" (&ret), "r" (args));
 	return ret;
 }
 
+NOOPTIMIZE
 int open(const char *path, uint32_t flags)
 {
-	int ret = 0;
-	register uint32_t r1 __asm("r1") = (uint32_t)path;
-	register uint32_t r2 __asm("r2") = flags;
-	register uint32_t r3 __asm("r3") = (uint32_t)&ret;
+	volatile int ret = 0;
+	uint32_t args[2] = { (uint32_t)path, flags };
 	__asm("\
 		mov r0, 1; \
 		mov r1, %0; \
 		mov r2, %1; \
-		mov r3, %2; \
 		svc 3; \
-	" :: "r" (r1), "r" (r2), "r" (r3));
+	" :: "r" (&ret), "r" (args));
 	return ret;
 }
 
+NOOPTIMIZE
 int close(int fd)
 {
-	int ret = 0;
-	register uint32_t r1 __asm("r1") = fd;
-	register uint32_t r2 __asm("r2") = (uint32_t)&ret;
+	volatile int ret = 0;
+	uint32_t args[1] = { fd };
 	__asm("\
 		mov r0, 2; \
 		mov r1, %0; \
 		mov r2, %1; \
 		svc 3; \
-	" :: "r" (r1), "r" (r2));
+	" :: "r" (&ret), "r" (args));
 	return ret;
 }
 
+NOOPTIMIZE
 int read(int fd, uint32_t count, uint8_t *buffer)
 {
-	int ret = 0;
-	register uint32_t r1 __asm("r1") = fd;
-	register uint32_t r2 __asm("r2") = count;
-	register uint32_t r3 __asm("r3") = (uint32_t)buffer;
-	register uint32_t r12 __asm("r12") = (uint32_t)&ret;
+	volatile int ret = 0;
+	uint32_t args[3] = { fd, count, (uint32_t)buffer };
 	__asm("\
 		mov r0, 3; \
 		mov r1, %0; \
 		mov r2, %1; \
-		mov r3, %2; \
-		mov r12, %3; \
 		svc 3; \
-	" :: "r" (r1), "r" (r2), "r" (r3), "r" (r12));
+	" :: "r" (&ret), "r" (args));
 	return ret;
 }
 
+NOOPTIMIZE
 int write(int fd, uint32_t count, const uint8_t *buffer)
 {
-	int ret = 0;
-	register uint32_t r1 __asm("r1") = fd;
-	register uint32_t r2 __asm("r2") = count;
-	register uint32_t r3 __asm("r3") = (uint32_t)buffer;
-	register uint32_t r12 __asm("r12") = (uint32_t)&ret;
+	volatile int ret = 0;
+	uint32_t args[3] = { fd, count, (uint32_t)buffer };
 	__asm("\
 		mov r0, 4; \
 		mov r1, %0; \
 		mov r2, %1; \
-		mov r3, %2; \
-		mov r12, %3; \
 		svc 3; \
-	" :: "r" (r1), "r" (r2), "r" (r3), "r" (r12));
+	" :: "r" (&ret), "r" (args));
 	return ret;
 }
 
diff --git a/src/pdclib/stmos_install_headers.sh b/src/pdclib/stmos_install_headers.sh
new file mode 100755
index 0000000..02f46ab
--- /dev/null
+++ b/src/pdclib/stmos_install_headers.sh
@@ -0,0 +1,6 @@
+#!/bin/bash
+# Installs header files to arm-stmos directory (~/arm-stmos)
+
+cp -Rv include/* $HOME/arm-stmos/usr/include
+cp -Rv platform/stmos/include/* $HOME/arm-stmos/usr/include
+