diff --git a/Makefile b/Makefile
index b7dadfe6..e869980c 100644
--- a/Makefile
+++ b/Makefile
@@ -1,27 +1,24 @@
 .POSIX:
-export ROOT=${abspath .}
+.SECONDARY:
 
-
-PROJECTS:=projects/hello projects/trapped projects/ping-pong
-PLATFORM?=qemu_virt
-
-include tools.mk
-include common/plat/${PLATFORM}.mk
+PROJECTS:=projects/hello \
+	  projects/trapped \
+	  projects/ping-pong \
+	  projects/demonstrator
 
 all: ${PROJECTS}
 
+${PROJECTS}: common
+
 common ${PROJECTS}:
-	make -C $@ all PLATFORM=${PLATFORM}
+	@${MAKE} -C $@ all
 
 clean:
-	for i in ${PROJECTS}; do \
-		make -C $$i clean PLATFORM=${PLATFORM}; \
+	@for i in common ${PROJECTS}; do \
+		${MAKE} -C $$i clean; \
 	done
 
-docs:
-	doxygen
-
 format:
-	clang-format -i $(shell find -name '*.[hc]' -not -path '*/.*')
+	clang-format -i $$(find * -type f -name '*.[hc]')
 
-.PHONY: all docs clean common ${PROJECTS}
+.PHONY: all clean format common ${PROJECTS}
diff --git a/README.md b/README.md
index 7f7cd508..da8b16e6 100644
--- a/README.md
+++ b/README.md
@@ -1,23 +1,31 @@
-s3k - Simple Secure Separation Kernel
+S3K - Simple Secure Separation Kernel
 =====================================
 
-s3k is a separation kernel targetting embedded RISC-V systems.
+S3K is a capability-based separation kernel targetting embedded RISC-V systems.
 
-- [Documentation Index](https://github.com/kth-step/s3k/wiki)
-- [S3K Design](https://github.com/kth-step/s3k/wiki/S3K-Design)
-- [S3K API](https://github.com/kth-step/s3k/wiki/S3K-API)
+Documentation
+-------------
+
+| Page | Description |
+| --- | --- |
+| [Home](https://github.com/kth-step/s3k/wiki)                  | Documentation Index |
+| [S3K Design](https://github.com/kth-step/s3k/wiki/S3K-Design) | High-level design of S3K |
+| [S3K Implementation](https://github.com/kth-step/s3k/wiki/S3K-Implementation) | Description of S3K implementation |
+| [S3K API](https://github.com/kth-step/s3k/wiki/S3K-API)       | User-level Kernel API |
+
+More documenation will be added.
 
 Configuration
 -------------
 
 Set your compiler toolchain in `tools.mk`. By default we have the following:
 ```
-CC=riscv64-unknown-elf-gcc
-AR=riscv64-unknown-elf-ar
-LD=riscv64-unknown-elf-ld
-SIZE=riscv64-unknown-elf-size
-OBJDUMP=riscv64-unknown-elf-objdump
-OBJCOPY=riscv64-unknown-elf-objcopy
+CC      =riscv64-unknown-elf-gcc
+AR      =riscv64-unknown-elf-ar
+LD      =riscv64-unknown-elf-ld
+SIZE    =riscv64-unknown-elf-size
+OBJDUMP =riscv64-unknown-elf-objdump
+OBJCOPY =riscv64-unknown-elf-objcopy
 ```
 
 Build and Run
@@ -73,7 +81,5 @@ Repository structure
   - hello - Hello, world example with two processes
   - ping-ping - IPC example
   - trapped - Trap handling example
-  - wcet - deprecated project (for now)
-- API.md - Kernel API
 - LICENSE - MIT License file
 - tools.mk - Set the compiler tools here
diff --git a/common/Makefile b/common/Makefile
index 3ebbd658..0e514154 100644
--- a/common/Makefile
+++ b/common/Makefile
@@ -1,57 +1,15 @@
 .POSIX:
+.SECONDARY:
 
-include plat/${PLATFORM}.mk
+PLATFORM?=${patsubst plat/%.mk, %, ${wildcard plat/*.mk}}
 
-include ${ROOT}/tools.mk
+all: ${addsuffix .all, ${PLATFORM}}
+clean: ${addsuffix .clean, ${PLATFORM}}
 
-# CFLAGS
-CFLAGS:=-march=${ARCH} -mabi=${ABI} -mcmodel=${CMODEL} \
-	-DPLATFORM_${PLATFORM} \
-	-Os -flto -ffat-lto-objects \
-	-nostdlib -Iinc \
+%.all: 
+	${MAKE} -f build.mk PLATFORM=${@:.all=} all
 
-BUILD :=build/${PLATFORM}
-SRCS2OBJS=${patsubst src/%.S, ${BUILD}/%.o, ${filter %.S, ${1}}} \
-	  ${patsubst src/%.c, ${BUILD}/%.o, ${filter %.c, ${1}}}
-
-ALTC_SRCS :=${wildcard src/altc/*.[cS]}
-S3K_SRCS  :=${wildcard src/s3k/*.[cS]}
-START_SRCS:=${wildcard src/start/*.S}
-
-PLAT_OBJS :=${call SRCS2OBJS, ${PLAT_SRCS}}
-ALTC_OBJS :=${call SRCS2OBJS, ${ALTC_SRCS}}
-S3K_OBJS  :=${call SRCS2OBJS, ${S3K_SRCS}}
-START_OBJS:=${call SRCS2OBJS, ${START_SRCS}}
-
-TARGETS:=${BUILD}/libplat.a \
-	 ${BUILD}/libaltc.a \
-	 ${BUILD}/libs3k.a \
-	 ${START_OBJS}
-
-
-all: ${TARGETS}
-
-clean:
-	rm -rf ${TARGETS}
-
-${BUILD}/libplat.a: ${PLAT_OBJS}
-	@mkdir -p ${@D}
-	${AR} cr $@ $^
-
-${BUILD}/libaltc.a: ${ALTC_OBJS}
-	@mkdir -p ${@D}
-	${AR} cr $@ $^
-
-${BUILD}/libs3k.a: ${S3K_OBJS}
-	@mkdir -p ${@D}
-	${AR} cr $@ $^
-
-${BUILD}/%.o: src/%.c
-	@mkdir -p ${@D}
-	${CC} -o $@ $< -c ${CFLAGS}
-
-${BUILD}/%.o: src/%.S
-	@mkdir -p ${@D}
-	${CC} -o $@ $< -c ${CFLAGS}
+%.clean:
+	${MAKE} -f build.mk PLATFORM=${@:.clean=} clean
 
 .PHONY: all clean
diff --git a/common/build.mk b/common/build.mk
new file mode 100644
index 00000000..5e4bc7ab
--- /dev/null
+++ b/common/build.mk
@@ -0,0 +1,64 @@
+.POSIX:
+
+ROOT:=${abspath ..}
+
+include plat/${PLATFORM}.mk
+
+include ${ROOT}/tools.mk
+
+# CFLAGS
+CFLAGS:=-march=${ARCH} -mabi=${ABI} -mcmodel=${CMODEL} \
+	-DPLATFORM_${PLATFORM} \
+	-Os -flto -ffat-lto-objects \
+	-nostdlib -Iinc \
+
+BUILD :=build/${PLATFORM}
+SRCS2OBJS=${patsubst src/%.S, ${BUILD}/%.o, ${filter %.S, ${1}}} \
+	  ${patsubst src/%.c, ${BUILD}/%.o, ${filter %.c, ${1}}}
+
+ALTC_SRCS :=${wildcard src/altc/*.[cS]}
+S3K_SRCS  :=${wildcard src/s3k/*.[cS]}
+START_SRCS:=${wildcard src/start/*.S}
+
+PLAT_OBJS :=${call SRCS2OBJS, ${PLAT_SRCS}}
+ALTC_OBJS :=${call SRCS2OBJS, ${ALTC_SRCS}}
+S3K_OBJS  :=${call SRCS2OBJS, ${S3K_SRCS}}
+START_OBJS:=${call SRCS2OBJS, ${START_SRCS}}
+
+TARGETS:=${BUILD}/libplat.a \
+	 ${BUILD}/libaltc.a \
+	 ${BUILD}/libs3k.a \
+	 ${START_OBJS}
+
+
+all: ${TARGETS}
+
+clean:
+	rm -rf ${BUILD}
+
+${BUILD}/libplat.a: ${PLAT_OBJS}
+	@mkdir -p ${@D}
+	@${AR} cr $@ $^
+	@printf "AR\t$@\n"
+
+${BUILD}/libaltc.a: ${ALTC_OBJS}
+	@mkdir -p ${@D}
+	@${AR} cr $@ $^
+	@printf "AR\t$@\n"
+
+${BUILD}/libs3k.a: ${S3K_OBJS}
+	@mkdir -p ${@D}
+	@${AR} cr $@ $^
+	@printf "AR\t$@\n"
+
+${BUILD}/%.o: src/%.c
+	@mkdir -p ${@D}
+	@${CC} -o $@ $< -c ${CFLAGS}
+	@printf "CC\t$@\n"
+
+${BUILD}/%.o: src/%.S
+	@mkdir -p ${@D}
+	@${CC} -o $@ $< -c ${CFLAGS}
+	@printf "CC\t$@\n"
+
+.PHONY: all clean
diff --git a/common/inc/altc/altio.h b/common/inc/altc/altio.h
index 6725c6a9..64a4d3d7 100644
--- a/common/inc/altc/altio.h
+++ b/common/inc/altc/altio.h
@@ -1,4 +1,6 @@
 #pragma once
+#include <stdarg.h>
+#include <stddef.h>
 
 int alt_getchar(void);
 int alt_putchar(char c);
@@ -6,3 +8,6 @@ int alt_putstr(const char *str);
 int alt_puts(const char *str);
 int alt_gets(char *str);
 int alt_printf(const char *fmt, ...);
+int alt_snprintf(char *restrict s, size_t n, const char *restrict fmt, ...);
+int alt_vsnprintf(char *restrict s, size_t n, const char *restrict fmt,
+		  va_list ap);
diff --git a/common/inc/altc/memchr.S b/common/inc/altc/memchr.S
deleted file mode 100644
index 39ffd470..00000000
--- a/common/inc/altc/memchr.S
+++ /dev/null
@@ -1,17 +0,0 @@
-.globl memchr
-.type memchr,@function
-
-.section .text.libaltc
-
-// void *memchr(const void *s, int c, size_t n);
-memchr:
-	beqz	a2,2f	  	// if (n == 0) goto exit1;
-	add	a2,a2,a0  	// char *a2 = s + n;
-	andi	a1,a1,0xFF	// c = c & 0xFF;
-				// do {
-1:	lb	t0,(a0)		// 	t0 = *s;
-	addi	a0,a0,1   	// 	++s;
-	beq	t0,a1,3f  	// 	if (t0 == c) goto exit2;
-	bne	a0,a1,1b  	// } while (s != a2);
-2:	li	a0,0		// exit1: return NULL;
-3:	ret			// exit2: return s;
diff --git a/common/inc/altc/memcmp.S b/common/inc/altc/memcmp.S
deleted file mode 100644
index 59249131..00000000
--- a/common/inc/altc/memcmp.S
+++ /dev/null
@@ -1,20 +0,0 @@
-.globl memcmp
-.type memcmp,@function
-
-.section .text.libaltc
-
-// int memcmp(const void *s1, const void *s2, size_t n);
-memcmp:
-	beqz	a2,2f		// if (n == 0) goto eq;
-	add	a2,a2,a0	// char *a2 = s + n;
-				// do {
-1:	lb	t0,(a0)		// 	char t0 = *s1;
-	lb	t1,(a1)		// 	char t1 = *s2;
-	addi	a0,a0,1		// 	++s1;
-	addi	a1,a1,1		// 	++s2;
-	bne	t0,t1,3f	// 	if (t0 != t1) goto neq;
-	bne	a0,a2,1b	// } while (s1 != a2);
-2:	li	a0,1		// eq: return 1;
-	ret
-3:	li	a0,0		// neq: return 0;
-	ret
diff --git a/common/inc/altc/memcpy.S b/common/inc/altc/memcpy.S
deleted file mode 100644
index 5f4a70e7..00000000
--- a/common/inc/altc/memcpy.S
+++ /dev/null
@@ -1,17 +0,0 @@
-.globl memcpy
-.type memcpy,@function
-
-.section .text.libaltc
-
-// void *memcpy(void *dst, const void *src, size_t n);
-memcpy:
-	beqz	a2,2f		// if (n == 0) goto exit;
-	mv	t0,a0		// t0 = dst;
-	add	a2,a2,a0	// a2 = dst + n;
-				// do {
-1:	lb	t1,(a1)		// 	t1 = *src;
-	addi	a1,a1,1		// 	++src;
-	addi	t0,t0,1		// 	++t0;
-	sb	t1,-1(t0)	// 	*(t0-1) = t1;
-	bne	t0,a2,1b	// } while (t0 != a2);
-2:	ret			// exit: return dst;
diff --git a/common/inc/altc/memmove.S b/common/inc/altc/memmove.S
deleted file mode 100644
index 13444d33..00000000
--- a/common/inc/altc/memmove.S
+++ /dev/null
@@ -1,19 +0,0 @@
-.globl memmove
-.type memmove,@function
-
-.section .text.libaltc
-
-// void *memmove(void *dst, const void *src, size_t n);
-memmove:
-	beq	a0,a1,2f 	// if (dst == src) goto exit;
-	beqz	a2,2f		// if (n == 0) goto exit;
-	bltu	a0,a1,memcpy	// if (dst < src) memcpy(dst, src, n);
-	add	a2,a2,a0	// a2 = dst + n;
-	add	a1,a1,a0	// src = src + n;
-				// do {
-1:	lb	t1,(a1)		//	t1 = *src;
-	addi	a1,a1,-1	//	--src;
-	addi	a2,a2,-1	//	--a2;
-	sb	t1,1(a2)	//	*(a2+1) = t1;
-	bne	a2,a0,1b	// } while (a2 != dst);
-2:	ret			// exit: return dst
diff --git a/common/inc/altc/memset.S b/common/inc/altc/memset.S
deleted file mode 100644
index 6dfcb8b4..00000000
--- a/common/inc/altc/memset.S
+++ /dev/null
@@ -1,15 +0,0 @@
-.globl memset
-.type memset,@function
-
-.section .text.libaltc
-
-// void *memset(void *s, int c, size_t n);
-memset:
-	beqz	a2,2f		// if (n == 0) goto exit;
-	mv	t0,a0		// char *t0 = s;
-	add	a2,a2,a0	// char *a2 = s + n;
-				// do {
-1:	sb	a1,(t0)		// 	*t0 = c;
-	addi	t0,t0,1		// 	++t0;
-	bne	t0,a2,1b	// } while (t0 != a2);
-2:	ret			// exit: return s;
diff --git a/common/inc/plat/config.h b/common/inc/plat/config.h
index 95756917..de488f0b 100644
--- a/common/inc/plat/config.h
+++ b/common/inc/plat/config.h
@@ -2,8 +2,16 @@
 
 #if defined(PLATFORM_qemu_virt)
 #include "plat/qemu_virt.h"
+#elif defined(PLATFORM_qemu_virt4)
+#include "plat/qemu_virt4.h"
 #elif defined(PLATFORM_sifive_unleashed)
 #include "plat/sifive_unleashed.h"
+#elif defined(PLATFORM_sifive_unleashed4)
+#include "plat/sifive_unleashed4.h"
 #else
 #error "Unsupported platform or platform not found"
 #endif
+
+#if S3K_HART_CNT > 1
+#define SMP
+#endif
diff --git a/common/inc/plat/qemu_virt.h b/common/inc/plat/qemu_virt.h
index 97e13bd5..ecf38508 100644
--- a/common/inc/plat/qemu_virt.h
+++ b/common/inc/plat/qemu_virt.h
@@ -11,7 +11,7 @@
 
 // Min and max usable hart ID.
 #define S3K_MIN_HART 0
-#define S3K_MAX_HART 3
+#define S3K_MAX_HART 0
 
 // Total number of usable harts.
 #define S3K_HART_CNT (S3K_MAX_HART - S3K_MIN_HART + 1ul)
@@ -28,13 +28,10 @@
 #define INIT_CAPS                                                     \
 	{                                                             \
 		[0] = cap_mk_pmp(0x20005fff, MEM_RWX),                \
-		[1] = cap_mk_memory(0x80020000, 0x80100000, MEM_RWX), \
+		[1] = cap_mk_memory(0x80020000, 0x88000000, MEM_RWX), \
 		[2] = cap_mk_memory(0x10000000, 0x10001000, MEM_RW),  \
 		[3] = cap_mk_memory(0x200b000, 0x200c000, MEM_R),     \
 		[4] = cap_mk_time(0, 0, S3K_SLOT_CNT),                \
-		[5] = cap_mk_time(1, 0, S3K_SLOT_CNT),                \
-		[6] = cap_mk_time(2, 0, S3K_SLOT_CNT),                \
-		[7] = cap_mk_time(3, 0, S3K_SLOT_CNT),                \
 		[8] = cap_mk_monitor(0, S3K_PROC_CNT),                \
 		[9] = cap_mk_channel(0, S3K_CHAN_CNT),                \
 	}
diff --git a/common/inc/plat/qemu_virt4.h b/common/inc/plat/qemu_virt4.h
new file mode 100644
index 00000000..c2e3e510
--- /dev/null
+++ b/common/inc/plat/qemu_virt4.h
@@ -0,0 +1,40 @@
+/**
+ * Platform configuration for QEMU virt
+ */
+#pragma once
+
+#define UART_NS16550A
+#define UART0_BASE_ADDR (0x10000000ull)
+
+#define MTIME_BASE_ADDR 0x200bff8ull
+#define MTIMECMP_BASE_ADDR 0x2004000ull
+
+// Min and max usable hart ID.
+#define S3K_MIN_HART 0
+#define S3K_MAX_HART 3
+
+// Total number of usable harts.
+#define S3K_HART_CNT (S3K_MAX_HART - S3K_MIN_HART + 1ul)
+
+// Number of PMP slots.
+#define S3K_PMP_CNT 8
+
+// RTC ticks per second
+#define S3K_RTC_HZ 1000000ull
+
+/// Stack size of 1024 KiB
+#define S3K_LOG_STACK_SIZE 10
+
+#define INIT_CAPS                                                     \
+	{                                                             \
+		[0] = cap_mk_pmp(0x20005fff, MEM_RWX),                \
+		[1] = cap_mk_memory(0x80020000, 0x88000000, MEM_RWX), \
+		[2] = cap_mk_memory(0x10000000, 0x10001000, MEM_RW),  \
+		[3] = cap_mk_memory(0x200b000, 0x200c000, MEM_R),     \
+		[4] = cap_mk_time(0, 0, S3K_SLOT_CNT),                \
+		[5] = cap_mk_time(1, 0, S3K_SLOT_CNT),                \
+		[6] = cap_mk_time(2, 0, S3K_SLOT_CNT),                \
+		[7] = cap_mk_time(3, 0, S3K_SLOT_CNT),                \
+		[8] = cap_mk_monitor(0, S3K_PROC_CNT),                \
+		[9] = cap_mk_channel(0, S3K_CHAN_CNT),                \
+	}
diff --git a/common/inc/plat/sifive_unleashed.h b/common/inc/plat/sifive_unleashed.h
index 02a53e43..38978405 100644
--- a/common/inc/plat/sifive_unleashed.h
+++ b/common/inc/plat/sifive_unleashed.h
@@ -8,7 +8,7 @@
 
 // Min and max usable hart ID.
 #define S3K_MIN_HART 1
-#define S3K_MAX_HART 4
+#define S3K_MAX_HART 1
 
 // Total number of usable harts.
 #define S3K_HART_CNT (S3K_MAX_HART - S3K_MIN_HART + 1ul)
@@ -27,13 +27,10 @@
 #define INIT_CAPS                                                     \
 	{                                                             \
 		[0] = cap_mk_pmp(0x20005fff, MEM_RWX),                \
-		[1] = cap_mk_memory(0x80020000, 0x80100000, MEM_RWX), \
+		[1] = cap_mk_memory(0x80020000, 0x88000000, MEM_RWX), \
 		[2] = cap_mk_memory(0x10010000, 0x10011000, MEM_RW),  \
 		[3] = cap_mk_memory(0x200b000, 0x200c000, MEM_R),     \
 		[4] = cap_mk_time(1, 0, S3K_SLOT_CNT),                \
-		[5] = cap_mk_time(2, 0, S3K_SLOT_CNT),                \
-		[6] = cap_mk_time(3, 0, S3K_SLOT_CNT),                \
-		[7] = cap_mk_time(4, 0, S3K_SLOT_CNT),                \
 		[8] = cap_mk_monitor(0, S3K_PROC_CNT),                \
 		[9] = cap_mk_channel(0, S3K_CHAN_CNT),                \
 	}
diff --git a/common/inc/plat/sifive_unleashed4.h b/common/inc/plat/sifive_unleashed4.h
new file mode 100644
index 00000000..a6dc7e11
--- /dev/null
+++ b/common/inc/plat/sifive_unleashed4.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#define UART_SIFIVE
+#define UART0_BASE_ADDR (0x10010000ull)
+
+#define MTIME_BASE_ADDR 0x200bff8ull
+#define MTIMECMP_BASE_ADDR 0x2004000ull
+
+// Min and max usable hart ID.
+#define S3K_MIN_HART 1
+#define S3K_MAX_HART 4
+
+// Total number of usable harts.
+#define S3K_HART_CNT (S3K_MAX_HART - S3K_MIN_HART + 1ul)
+
+// Number of PMP slots.
+#define S3K_PMP_CNT 8
+
+// RTC ticks per second
+#define S3K_RTC_HZ 1000000ull
+
+/// Stack size of 1024 KiB
+#define S3K_LOG_STACK_SIZE 10
+
+#define UART_SIFIVE
+
+#define INIT_CAPS                                                     \
+	{                                                             \
+		[0] = cap_mk_pmp(0x20005fff, MEM_RWX),                \
+		[1] = cap_mk_memory(0x80020000, 0x88000000, MEM_RWX), \
+		[2] = cap_mk_memory(0x10010000, 0x10011000, MEM_RW),  \
+		[3] = cap_mk_memory(0x200b000, 0x200c000, MEM_R),     \
+		[4] = cap_mk_time(1, 0, S3K_SLOT_CNT),                \
+		[5] = cap_mk_time(2, 0, S3K_SLOT_CNT),                \
+		[6] = cap_mk_time(3, 0, S3K_SLOT_CNT),                \
+		[7] = cap_mk_time(4, 0, S3K_SLOT_CNT),                \
+		[8] = cap_mk_monitor(0, S3K_PROC_CNT),                \
+		[9] = cap_mk_channel(0, S3K_CHAN_CNT),                \
+	}
diff --git a/common/inc/s3k/syscall.h b/common/inc/s3k/syscall.h
index 32037ed7..69f2e358 100644
--- a/common/inc/s3k/syscall.h
+++ b/common/inc/s3k/syscall.h
@@ -1,50 +1,14 @@
 #pragma once
 #include "s3k/types.h"
 
-typedef enum {
-	// Basic Info & Registers
-	S3K_SYS_GET_INFO,  // Retrieve system information
-	S3K_SYS_REG_READ,  // Read from a register
-	S3K_SYS_REG_WRITE, // Write to a register
-	S3K_SYS_SYNC,	   // Synchronize memory and time.
-
-	// Capability Management
-	S3K_SYS_CAP_READ,   // Read the properties of a capability.
-	S3K_SYS_CAP_MOVE,   // Move a capability to a different slot.
-	S3K_SYS_CAP_DELETE, // Delete a capability from the system.
-	S3K_SYS_CAP_REVOKE, // Deletes derived capabilities.
-	S3K_SYS_CAP_DERIVE, // Creates a new capability.
-
-	// PMP calls
-	S3K_SYS_PMP_LOAD,
-	S3K_SYS_PMP_UNLOAD,
-
-	// Monitor calls
-	S3K_SYS_MON_SUSPEND,
-	S3K_SYS_MON_RESUME,
-	S3K_SYS_MON_STATE_GET,
-	S3K_SYS_MON_YIELD,
-	S3K_SYS_MON_REG_READ,
-	S3K_SYS_MON_REG_WRITE,
-	S3K_SYS_MON_CAP_READ,
-	S3K_SYS_MON_CAP_MOVE,
-	S3K_SYS_MON_PMP_LOAD,
-	S3K_SYS_MON_PMP_UNLOAD,
-
-	// Socket calls
-	S3K_SYS_SOCK_SEND,
-	S3K_SYS_SOCK_RECV,
-	S3K_SYS_SOCK_SENDRECV,
-} s3k_syscall_t;
-
 uint64_t s3k_get_pid(void);
 uint64_t s3k_get_time(void);
 uint64_t s3k_get_timeout(void);
-uint64_t s3k_get_wcet(bool reset);
 uint64_t s3k_reg_read(s3k_reg_t reg);
 uint64_t s3k_reg_write(s3k_reg_t reg, uint64_t val);
 void s3k_sync();
 void s3k_sync_mem();
+void s3k_sleep(uint64_t time);
 s3k_err_t s3k_cap_read(s3k_cidx_t idx, s3k_cap_t *cap);
 s3k_err_t s3k_cap_move(s3k_cidx_t src, s3k_cidx_t dst);
 s3k_err_t s3k_cap_delete(s3k_cidx_t idx);
diff --git a/common/inc/s3k/types.h b/common/inc/s3k/types.h
index 26a86adb..f7bbd894 100644
--- a/common/inc/s3k/types.h
+++ b/common/inc/s3k/types.h
@@ -37,6 +37,7 @@ typedef enum {
 	S3K_ERR_INVALID_SOCKET,
 	S3K_ERR_INVALID_SYSCALL,
 	S3K_ERR_INVALID_REGISTER,
+	S3K_ERR_INVALID_CAPABILITY,
 	S3K_ERR_NO_RECEIVER,
 	S3K_ERR_PREEMPTED,
 	S3K_ERR_TIMEOUT,
diff --git a/common/inc/s3k/util.h b/common/inc/s3k/util.h
index a39a3ce2..65190928 100644
--- a/common/inc/s3k/util.h
+++ b/common/inc/s3k/util.h
@@ -14,8 +14,8 @@ bool s3k_is_valid(s3k_cap_t a);
 bool s3k_is_parent(s3k_cap_t a, s3k_cap_t b);
 bool s3k_is_derivable(s3k_cap_t a, s3k_cap_t b);
 
-void s3k_napot_decode(s3k_napot_t napot_addr, s3k_addr_t *begin,
-		      s3k_addr_t *end);
+void s3k_napot_decode(s3k_napot_t napot_addr, s3k_addr_t *base,
+		      s3k_addr_t *size);
 s3k_napot_t s3k_napot_encode(s3k_addr_t base, s3k_addr_t size);
 
 static inline bool s3k_is_ready(s3k_state_t state)
diff --git a/common/plat/qemu_virt.mk b/common/plat/qemu_virt.mk
index 8325ff1f..72d7e6f8 100644
--- a/common/plat/qemu_virt.mk
+++ b/common/plat/qemu_virt.mk
@@ -1,8 +1,6 @@
-export ARCH=rv64imac_zicsr
+export ARCH=rv64imac_zicsr_zifencei
 export ABI=lp64
 export CMODEL=medany
-export QEMU_MACHINE=virt
-export QEMU_SMP=4
 export COMMON_INC:=${ROOT}/common/inc
 export COMMON_LIB:=${ROOT}/common/build/${PLATFORM}
 export STARTFILES:=${ROOT}/common/build/${PLATFORM}/start
diff --git a/common/plat/qemu_virt4.mk b/common/plat/qemu_virt4.mk
new file mode 100644
index 00000000..72d7e6f8
--- /dev/null
+++ b/common/plat/qemu_virt4.mk
@@ -0,0 +1,7 @@
+export ARCH=rv64imac_zicsr_zifencei
+export ABI=lp64
+export CMODEL=medany
+export COMMON_INC:=${ROOT}/common/inc
+export COMMON_LIB:=${ROOT}/common/build/${PLATFORM}
+export STARTFILES:=${ROOT}/common/build/${PLATFORM}/start
+PLAT_SRCS=src/drivers/uart/ns16550a.c src/drivers/time.c
diff --git a/common/plat/sifive_unleashed.mk b/common/plat/sifive_unleashed.mk
index b61c3aa9..11160f16 100644
--- a/common/plat/sifive_unleashed.mk
+++ b/common/plat/sifive_unleashed.mk
@@ -1,8 +1,6 @@
-export ARCH=rv64imac_zicsr
+export ARCH=rv64imac_zicsr_zifencei
 export ABI=lp64
 export CMODEL=medany
-export QEMU_MACHINE=sifive_u
-export QEMU_SMP=5
 export COMMON_INC:=${ROOT}/common/inc
 export COMMON_LIB:=${ROOT}/common/build/${PLATFORM}
 export STARTFILES:=${ROOT}/common/build/${PLATFORM}/start
diff --git a/common/plat/sifive_unleashed4.mk b/common/plat/sifive_unleashed4.mk
new file mode 100644
index 00000000..11160f16
--- /dev/null
+++ b/common/plat/sifive_unleashed4.mk
@@ -0,0 +1,7 @@
+export ARCH=rv64imac_zicsr_zifencei
+export ABI=lp64
+export CMODEL=medany
+export COMMON_INC:=${ROOT}/common/inc
+export COMMON_LIB:=${ROOT}/common/build/${PLATFORM}
+export STARTFILES:=${ROOT}/common/build/${PLATFORM}/start
+PLAT_SRCS=src/drivers/uart/sifive.c src/drivers/time.c
diff --git a/common/src/altc/printf.c b/common/src/altc/printf.c
index c9c2b94f..25e3a978 100644
--- a/common/src/altc/printf.c
+++ b/common/src/altc/printf.c
@@ -1,67 +1,118 @@
 #include "altc/altio.h"
 
 #include <stdarg.h>
+#include <stddef.h>
 
-int alt_printf(const char *fmt, ...)
+#define ALT_PRINTF_BUF_SIZE 128
+
+static char *write_dec(char *restrict dst, const char *end,
+		       unsigned long long val)
+{
+	if (!val && dst != end) {
+		*(dst++) = '0';
+	} else if (dst != end) {
+		int i = 0;
+		char buf[24];
+		while (val) {
+			int tmp = val % 10;
+			buf[i++] = '0' + tmp;
+			val /= 10;
+		}
+		while (i > 0 && dst != end)
+			*(dst++) = buf[--i];
+	}
+	return dst;
+}
+
+static char *write_hex(char *restrict dst, const char *end,
+		       unsigned long long val)
+{
+	if (!val && dst != end) {
+		*(dst++) = '0';
+	} else if (dst != end) {
+		int i = 0;
+		char buf[16];
+		while (val) {
+			int tmp = val & 0xF;
+			buf[i++] = tmp < 10 ? ('0' + tmp) : 'A' + (tmp - 10);
+			val >>= 4;
+		}
+		while (i > 0 && dst != end)
+			*(dst++) = buf[--i];
+	}
+	return dst;
+}
+
+static char *write_str(char *restrict dst, const char *end, char *restrict src)
 {
-	static const char hex[16] = {'0', '1', '2', '3', '4', '5', '6', '7',
-				     '8', '9', 'a', 'b', 'c', 'd', 'e', 'f'};
-	unsigned long long x;
-	int len;
-	va_list args;
-	va_start(args, fmt);
-	len = 0;
+	while (dst != end && *src != '\0')
+		*(dst++) = *(src++);
+	return dst;
+}
 
-	while (*fmt != '\0') {
-		if (*fmt++ != '%') {
-			alt_putchar(*(fmt - 1));
-			len++;
+static char *write_char(char *restrict dst, const char *end, char c)
+{
+	if (dst != end)
+		*(dst++) = c;
+	return dst;
+}
+
+int alt_vsnprintf(char *restrict str, size_t size, const char *restrict fmt,
+		  va_list ap)
+{
+	char *s = str;
+	const char *end = str + size - 1;
+	while (*fmt != '\0' && s != end) {
+		if (*(fmt++) != '%') {
+			s = write_char(s, end, *(fmt - 1));
 			continue;
 		}
-		switch (*fmt++) {
+		switch (*(fmt++)) {
+		case '%':
+			s = write_char(s, end, '%');
+			break;
 		case 'c':
-			alt_putchar((char)va_arg(args, int));
-			len++;
+			s = write_char(s, end, va_arg(ap, int));
 			break;
 		case 's':
-			len += alt_putstr(va_arg(args, char *));
+			s = write_str(s, end, va_arg(ap, char *));
 			break;
 		case 'x':
-			x = va_arg(args, unsigned int);
-			if (!x) {
-				alt_putchar('0');
-				len++;
-				break;
-			}
-			for (int i = 28; i >= 0; i -= 4) {
-				if (x >> i) {
-					alt_putchar(hex[(x >> i) & 0xF]);
-					len++;
-				}
-			}
+			s = write_hex(s, end, va_arg(ap, unsigned int));
 			break;
 		case 'X':
-			x = va_arg(args, unsigned long long);
-			if (!x) {
-				alt_putchar('0');
-				len++;
-				break;
-			}
-			for (int i = 60; i >= 0; i -= 4) {
-				if (x >> i) {
-					alt_putchar(hex[(x >> i) & 0xF]);
-					len++;
-				}
-			}
+			s = write_hex(s, end, va_arg(ap, unsigned long));
 			break;
-		case '%':
-			alt_putchar('%');
-			len++;
+		case 'd':
+			s = write_dec(s, end, va_arg(ap, unsigned int));
+			break;
+		case 'D':
+			s = write_dec(s, end, va_arg(ap, unsigned long));
 			break;
 		case '\0':
 			break;
 		}
 	}
-	va_end(args);
+	*s = '\0';
+	return s - str;
+}
+
+int alt_snprintf(char *restrict str, size_t size, const char *restrict fmt, ...)
+{
+	va_list ap;
+	va_start(ap, fmt);
+	int len = alt_vsnprintf(str, size, fmt, ap);
+	va_end(ap);
+	return len;
+}
+
+int alt_printf(const char *restrict fmt, ...)
+{
+	char buf[ALT_PRINTF_BUF_SIZE];
+	va_list ap;
+	va_start(ap, fmt);
+	int len = alt_vsnprintf(buf, ALT_PRINTF_BUF_SIZE, fmt, ap);
+	va_end(ap);
+	alt_putstr(buf);
 	return len;
 }
diff --git a/common/src/s3k/s3k.c b/common/src/s3k/s3k.c
index 69c666c1..37440820 100644
--- a/common/src/s3k/s3k.c
+++ b/common/src/s3k/s3k.c
@@ -339,470 +339,3 @@ bool s3k_cap_is_derivable(s3k_cap_t p, s3k_cap_t c)
 		return false;
 	}
 }
-
-static inline s3k_ret_t do_ecall(s3k_syscall_t call, sys_args_t args)
-{
-	register uint64_t t0 __asm__("t0") = call;
-	register uint64_t a0 __asm__("a0") = args.a0;
-	register uint64_t a1 __asm__("a1") = args.a1;
-	register uint64_t a2 __asm__("a2") = args.a2;
-	register uint64_t a3 __asm__("a3") = args.a3;
-	register uint64_t a4 __asm__("a4") = args.a4;
-	register uint64_t a5 __asm__("a5") = args.a5;
-	register uint64_t a6 __asm__("a6") = args.a6;
-	register uint64_t a7 __asm__("a7") = args.a7;
-	__asm__ volatile("ecall"
-			 : "+r"(t0), "+r"(a0)
-			 : "r"(a1), "r"(a2), "r"(a3), "r"(a4), "r"(a5), "r"(a6),
-			   "r"(a7));
-	return (s3k_ret_t){.err = t0, .val = a0};
-}
-
-uint64_t s3k_get_pid(void)
-{
-	sys_args_t args = {.get_info = {0}};
-	return do_ecall(S3K_SYS_GET_INFO, args).val;
-}
-
-uint64_t s3k_get_time(void)
-{
-	sys_args_t args = {.get_info = {1}};
-	return do_ecall(S3K_SYS_GET_INFO, args).val;
-}
-
-uint64_t s3k_get_timeout(void)
-{
-	sys_args_t args = {.get_info = {2}};
-	return do_ecall(S3K_SYS_GET_INFO, args).val;
-}
-
-uint64_t s3k_get_wcet(bool reset)
-{
-	sys_args_t args = {.get_info = {reset ? 4 : 3}};
-	return do_ecall(S3K_SYS_GET_INFO, args).val;
-}
-
-uint64_t s3k_reg_read(s3k_reg_t reg)
-{
-	sys_args_t args = {.reg = {reg}};
-	return do_ecall(S3K_SYS_REG_READ, args).val;
-}
-
-uint64_t s3k_reg_write(s3k_reg_t reg, uint64_t val)
-{
-	sys_args_t args = {
-	    .reg = {reg, val}
-	     };
-	return do_ecall(S3K_SYS_REG_WRITE, args).val;
-}
-
-void s3k_sync(void)
-{
-	sys_args_t args = {.sync = {true}};
-	do_ecall(S3K_SYS_SYNC, args);
-}
-
-void s3k_sync_mem(void)
-{
-	sys_args_t args = {.sync = {false}};
-	do_ecall(S3K_SYS_SYNC, args);
-}
-
-s3k_err_t s3k_cap_read(s3k_cidx_t idx, s3k_cap_t *cap)
-{
-	sys_args_t args = {.cap = {idx}};
-	s3k_ret_t ret = do_ecall(S3K_SYS_CAP_READ, args);
-	if (!ret.err)
-		cap->raw = ret.val;
-	return ret.err;
-}
-
-s3k_err_t s3k_cap_move(s3k_cidx_t src, s3k_cidx_t dst)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_cap_move(src, dst);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_cap_delete(s3k_cidx_t idx)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_cap_delete(idx);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_cap_revoke(s3k_cidx_t idx)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_cap_revoke(idx);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_cap_derive(s3k_cidx_t src, s3k_cidx_t dst, s3k_cap_t ncap)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_cap_derive(src, dst, ncap);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_pmp_load(s3k_cidx_t idx, s3k_pmp_slot_t slot)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_pmp_load(idx, slot);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_pmp_unload(s3k_cidx_t idx)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_pmp_unload(idx);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_mon_suspend(s3k_cidx_t mon_idx, s3k_pid_t pid)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_mon_suspend(mon_idx, pid);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_mon_resume(s3k_cidx_t mon_idx, s3k_pid_t pid)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_mon_resume(mon_idx, pid);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_mon_state_get(s3k_cidx_t mon_idx, s3k_pid_t pid,
-			    s3k_state_t *state)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_mon_state_get(mon_idx, pid, state);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_mon_yield(s3k_cidx_t mon_idx, s3k_pid_t pid)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_mon_yield(mon_idx, pid);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_mon_reg_read(s3k_cidx_t mon_idx, s3k_pid_t pid, s3k_reg_t reg,
-			   uint64_t *val)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_mon_reg_read(mon_idx, pid, reg, val);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_mon_reg_write(s3k_cidx_t mon_idx, s3k_pid_t pid, s3k_reg_t reg,
-			    uint64_t val)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_mon_reg_write(mon_idx, pid, reg, val);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_mon_cap_read(s3k_cidx_t mon_idx, s3k_pid_t pid, s3k_cidx_t idx,
-			   s3k_cap_t *cap)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_mon_cap_read(mon_idx, pid, idx, cap);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_mon_cap_move(s3k_cidx_t mon_idx, s3k_pid_t src_pid,
-			   s3k_cidx_t src_idx, s3k_pid_t dst_pid,
-			   s3k_cidx_t dst_idx)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_mon_cap_move(mon_idx, src_pid, src_idx, dst_pid,
-					   dst_idx);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_mon_pmp_load(s3k_cidx_t mon_idx, s3k_pid_t pid,
-			   s3k_cidx_t pmp_idx, s3k_pmp_slot_t pmp_slot)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_mon_pmp_load(mon_idx, pid, pmp_idx, pmp_slot);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_mon_pmp_unload(s3k_cidx_t mon_idx, s3k_pid_t pid,
-			     s3k_cidx_t pmp_idx)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_mon_pmp_unload(mon_idx, pid, pmp_idx);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_err_t s3k_sock_send(s3k_cidx_t sock_idx, const s3k_msg_t *msg)
-{
-	s3k_err_t err;
-	do {
-		err = s3k_try_sock_send(sock_idx, msg);
-	} while (err == S3K_ERR_PREEMPTED);
-	return err;
-}
-
-s3k_reply_t s3k_sock_recv(s3k_cidx_t sock_idx, s3k_cidx_t cap_idx)
-{
-	s3k_reply_t reply;
-	do {
-		reply = s3k_try_sock_recv(sock_idx, cap_idx);
-	} while (reply.err == S3K_ERR_PREEMPTED);
-	return reply;
-}
-
-s3k_reply_t s3k_sock_sendrecv(s3k_cidx_t sock_idx, const s3k_msg_t *msg)
-{
-	s3k_reply_t reply;
-	do {
-		reply = s3k_try_sock_sendrecv(sock_idx, msg);
-	} while (reply.err == S3K_ERR_PREEMPTED);
-	return reply;
-}
-
-s3k_err_t s3k_try_cap_move(s3k_cidx_t src, s3k_cidx_t dst)
-{
-	sys_args_t args = {
-	    .cap = {src, dst}
-	     };
-	return do_ecall(S3K_SYS_CAP_MOVE, args).err;
-}
-
-s3k_err_t s3k_try_cap_delete(s3k_cidx_t idx)
-{
-	sys_args_t args = {.cap = {idx}};
-	return do_ecall(S3K_SYS_CAP_DELETE, args).err;
-}
-
-s3k_err_t s3k_try_cap_revoke(s3k_cidx_t idx)
-{
-	sys_args_t args = {.cap = {idx}};
-	return do_ecall(S3K_SYS_CAP_REVOKE, args).err;
-}
-
-s3k_err_t s3k_try_cap_derive(s3k_cidx_t src, s3k_cidx_t dst, s3k_cap_t ncap)
-{
-	sys_args_t args = {
-	    .cap = {src, dst, ncap}
-	   };
-	return do_ecall(S3K_SYS_CAP_DERIVE, args).err;
-}
-
-s3k_err_t s3k_try_pmp_load(s3k_cidx_t idx, s3k_pmp_slot_t slot)
-{
-	sys_args_t args = {
-	    .pmp = {idx, slot}
-	      };
-	return do_ecall(S3K_SYS_PMP_LOAD, args).err;
-}
-
-s3k_err_t s3k_try_pmp_unload(s3k_cidx_t idx)
-{
-	sys_args_t args = {.pmp = {idx}};
-	return do_ecall(S3K_SYS_PMP_UNLOAD, args).err;
-}
-
-s3k_err_t s3k_try_mon_suspend(s3k_cidx_t mon, s3k_pid_t pid)
-{
-	sys_args_t args = {
-	    .mon_state = {mon, pid}
-	   };
-	return do_ecall(S3K_SYS_MON_SUSPEND, args).err;
-}
-
-s3k_err_t s3k_try_mon_resume(s3k_cidx_t mon, s3k_pid_t pid)
-{
-	sys_args_t args = {
-	    .mon_state = {mon, pid}
-	   };
-	return do_ecall(S3K_SYS_MON_RESUME, args).err;
-}
-
-s3k_err_t s3k_try_mon_state_get(s3k_cidx_t mon, s3k_pid_t pid,
-				s3k_state_t *state)
-{
-	sys_args_t args = {
-	    .mon_state = {mon, pid}
-	   };
-	s3k_ret_t ret = do_ecall(S3K_SYS_MON_STATE_GET, args);
-	*state = ret.val;
-	return ret.err;
-}
-
-s3k_err_t s3k_try_mon_yield(s3k_cidx_t mon, s3k_pid_t pid)
-{
-	sys_args_t args = {
-	    .mon_state = {mon, pid}
-	   };
-	return do_ecall(S3K_SYS_MON_YIELD, args).err;
-}
-
-s3k_err_t s3k_try_mon_reg_read(s3k_cidx_t mon, s3k_pid_t pid, s3k_reg_t reg,
-			       uint64_t *val)
-{
-	sys_args_t args = {
-	    .mon_reg = {mon, pid, reg}
-	      };
-	s3k_ret_t ret = do_ecall(S3K_SYS_MON_REG_READ, args);
-	*val = ret.val;
-	return ret.err;
-}
-
-s3k_err_t s3k_try_mon_reg_write(s3k_cidx_t mon, s3k_pid_t pid, s3k_reg_t reg,
-				uint64_t val)
-{
-	sys_args_t args = {
-	    .mon_reg = {mon, pid, reg, val}
-	   };
-	s3k_ret_t ret = do_ecall(S3K_SYS_MON_REG_WRITE, args);
-	return ret.err;
-}
-
-s3k_err_t s3k_try_mon_cap_read(s3k_cidx_t mon_idx, s3k_pid_t pid,
-			       s3k_cidx_t idx, s3k_cap_t *cap)
-{
-	sys_args_t args = {
-	    .mon_cap = {mon_idx, pid, idx}
-	  };
-	s3k_ret_t ret = do_ecall(S3K_SYS_MON_CAP_READ, args);
-	if (!ret.err)
-		cap->raw = ret.val;
-	return ret.err;
-}
-
-s3k_err_t s3k_try_mon_cap_move(s3k_cidx_t mon_idx, s3k_pid_t src_pid,
-			       s3k_cidx_t src_idx, s3k_pid_t dst_pid,
-			       s3k_cidx_t dst_idx)
-{
-	sys_args_t args = {
-	    .mon_cap = {mon_idx, src_pid, src_idx, dst_pid, dst_idx}
-	    };
-	return do_ecall(S3K_SYS_MON_CAP_MOVE, args).err;
-}
-
-s3k_err_t s3k_try_mon_pmp_load(s3k_cidx_t mon_idx, s3k_pid_t pid,
-			       s3k_cidx_t pmp_idx, s3k_pmp_slot_t pmp_slot)
-{
-	sys_args_t args = {
-	    .mon_pmp = {mon_idx, pid, pmp_idx, pmp_slot}
-	};
-	return do_ecall(S3K_SYS_MON_PMP_LOAD, args).err;
-}
-
-s3k_err_t s3k_try_mon_pmp_unload(s3k_cidx_t mon_idx, s3k_pid_t pid,
-				 s3k_cidx_t pmp_idx)
-{
-	sys_args_t args = {
-	    .mon_pmp = {mon_idx, pid, pmp_idx}
-	      };
-	return do_ecall(S3K_SYS_MON_PMP_UNLOAD, args).err;
-}
-
-s3k_err_t s3k_try_sock_send(s3k_cidx_t sock_idx, const s3k_msg_t *msg)
-{
-	sys_args_t args = {
-	    .sock = {.sock_idx = sock_idx,
-		     .cap_idx = msg->cap_idx,
-		     .send_cap = msg->send_cap,
-		     {msg->data[0], msg->data[1], msg->data[2], msg->data[3]}}
-	      };
-	return do_ecall(S3K_SYS_SOCK_SEND, args).err;
-}
-
-s3k_reply_t s3k_try_sock_recv(s3k_cidx_t sock_idx, s3k_cidx_t cap_idx)
-{
-	sys_args_t args = {
-	    .sock = {.sock_idx = sock_idx, .cap_idx = cap_idx}
-	      };
-	register uint64_t t0 __asm__("t0") = S3K_SYS_SOCK_SENDRECV;
-	register uint64_t a0 __asm__("a0") = args.a0;
-	register uint64_t a1 __asm__("a1") = args.a1;
-	register uint64_t a2 __asm__("a2") = args.a2;
-	register uint64_t a3 __asm__("a3") = args.a3;
-	register uint64_t a4 __asm__("a4") = args.a4;
-	register uint64_t a5 __asm__("a5") = args.a5;
-	register uint64_t a6 __asm__("a6") = args.a6;
-	register uint64_t a7 __asm__("a7") = args.a7;
-	__asm__ volatile("ecall"
-			 : "+r"(t0), "+r"(a0), "+r"(a1), "+r"(a2), "+r"(a3),
-			   "+r"(a4), "+r"(a5)
-			 : "r"(a6), "r"(a7));
-	s3k_reply_t reply;
-	reply.err = t0;
-	reply.tag = a0;
-	reply.cap.raw = a1;
-	reply.data[0] = a2;
-	reply.data[1] = a3;
-	reply.data[2] = a4;
-	reply.data[3] = a5;
-	return reply;
-}
-
-s3k_reply_t s3k_try_sock_sendrecv(s3k_cidx_t sock_idx, const s3k_msg_t *msg)
-{
-	sys_args_t args = {
-	    .sock = {.sock_idx = sock_idx,
-		     .cap_idx = msg->cap_idx,
-		     .send_cap = msg->send_cap,
-		     {msg->data[0], msg->data[1], msg->data[2], msg->data[3]}}
-	      };
-	register uint64_t t0 __asm__("t0") = S3K_SYS_SOCK_SENDRECV;
-	register uint64_t a0 __asm__("a0") = args.a0;
-	register uint64_t a1 __asm__("a1") = args.a1;
-	register uint64_t a2 __asm__("a2") = args.a2;
-	register uint64_t a3 __asm__("a3") = args.a3;
-	register uint64_t a4 __asm__("a4") = args.a4;
-	register uint64_t a5 __asm__("a5") = args.a5;
-	register uint64_t a6 __asm__("a6") = args.a6;
-	register uint64_t a7 __asm__("a7") = args.a7;
-	__asm__ volatile("ecall"
-			 : "+r"(t0), "+r"(a0), "+r"(a1), "+r"(a2), "+r"(a3),
-			   "+r"(a4), "+r"(a5)
-			 : "r"(a6), "r"(a7));
-	s3k_reply_t reply;
-	reply.err = t0;
-	reply.tag = a0;
-	reply.cap.raw = a1;
-	reply.data[0] = a2;
-	reply.data[1] = a3;
-	reply.data[2] = a4;
-	reply.data[3] = a5;
-	return reply;
-}
diff --git a/common/src/s3k/syscall.c b/common/src/s3k/syscall.c
new file mode 100644
index 00000000..28f71f03
--- /dev/null
+++ b/common/src/s3k/syscall.c
@@ -0,0 +1,673 @@
+#include "s3k/s3k.h"
+
+typedef enum {
+	// Basic Info & Registers
+	S3K_SYS_GET_INFO,  // Retrieve system information
+	S3K_SYS_REG_READ,  // Read from a register
+	S3K_SYS_REG_WRITE, // Write to a register
+	S3K_SYS_SYNC,	   // Synchronize memory and time.
+	S3K_SYS_SLEEP,
+
+	// Capability Management
+	S3K_SYS_CAP_READ,   // Read the properties of a capability.
+	S3K_SYS_CAP_MOVE,   // Move a capability to a different slot.
+	S3K_SYS_CAP_DELETE, // Delete a capability from the system.
+	S3K_SYS_CAP_REVOKE, // Deletes derived capabilities.
+	S3K_SYS_CAP_DERIVE, // Creates a new capability.
+
+	// PMP calls
+	S3K_SYS_PMP_LOAD,
+	S3K_SYS_PMP_UNLOAD,
+
+	// Monitor calls
+	S3K_SYS_MON_SUSPEND,
+	S3K_SYS_MON_RESUME,
+	S3K_SYS_MON_STATE_GET,
+	S3K_SYS_MON_YIELD,
+	S3K_SYS_MON_REG_READ,
+	S3K_SYS_MON_REG_WRITE,
+	S3K_SYS_MON_CAP_READ,
+	S3K_SYS_MON_CAP_MOVE,
+	S3K_SYS_MON_PMP_LOAD,
+	S3K_SYS_MON_PMP_UNLOAD,
+
+	// Socket calls
+	S3K_SYS_SOCK_SEND,
+	S3K_SYS_SOCK_RECV,
+	S3K_SYS_SOCK_SENDRECV,
+} s3k_syscall_t;
+
+typedef union {
+	struct {
+		uint64_t a0, a1, a2, a3, a4, a5, a6, a7;
+	};
+
+	struct {
+		uint64_t info;
+	} get_info;
+
+	struct {
+		uint64_t reg;
+	} reg_read;
+
+	struct {
+		uint64_t reg;
+		uint64_t val;
+	} reg_write;
+
+	struct {
+		uint64_t full;
+	} sync;
+
+	struct {
+		uint64_t time;
+	} sleep;
+
+	struct {
+		uint64_t idx;
+	} cap_read;
+
+	struct {
+		uint64_t src_idx;
+		uint64_t dst_idx;
+	} cap_move;
+
+	struct {
+		uint64_t idx;
+	} cap_delete;
+
+	struct {
+		uint64_t idx;
+	} cap_revoke;
+
+	struct {
+		uint64_t src_idx;
+		uint64_t dst_idx;
+		uint64_t cap_raw;
+	} cap_derive;
+
+	struct {
+		uint64_t idx;
+		uint64_t slot;
+	} pmp_load;
+
+	struct {
+		uint64_t idx;
+	} pmp_unload;
+
+	struct {
+		uint64_t mon_idx;
+		uint64_t pid;
+	} mon_state;
+
+	struct {
+		uint64_t mon_idx;
+		uint64_t pid;
+		uint64_t reg;
+	} mon_reg_read;
+
+	struct {
+		uint64_t mon_idx;
+		uint64_t pid;
+		uint64_t reg;
+		uint64_t val;
+	} mon_reg_write;
+
+	struct {
+		uint64_t mon_idx;
+		uint64_t pid;
+		uint64_t idx;
+	} mon_cap_read;
+
+	struct {
+		uint64_t mon_idx;
+		uint64_t src_pid;
+		uint64_t src_idx;
+		uint64_t dst_pid;
+		uint64_t dst_idx;
+	} mon_cap_move;
+
+	struct {
+		uint64_t mon_idx;
+		uint64_t pid;
+		uint64_t idx;
+		uint64_t slot;
+	} mon_pmp_load;
+
+	struct {
+		uint64_t mon_idx;
+		uint64_t pid;
+		uint64_t idx;
+	} mon_pmp_unload;
+
+	struct {
+		uint64_t sock_idx;
+		uint64_t cap_idx;
+		uint64_t send_cap;
+		uint64_t data[4];
+	} sock;
+} sys_args_t;
+
+typedef struct {
+	s3k_err_t err;
+	uint64_t val;
+} s3k_ret_t;
+
+_Static_assert(sizeof(sys_args_t) == 64, "sys_args_t has the wrong size");
+
+#define DO_ECALL(call, args, width)                                            \
+	({                                                                     \
+		register uint64_t t0 __asm__("t0") = call;                     \
+		register uint64_t a0 __asm__("a0") = args.a0;                  \
+		register uint64_t a1 __asm__("a1") = args.a1;                  \
+		register uint64_t a2 __asm__("a2") = args.a2;                  \
+		register uint64_t a3 __asm__("a3") = args.a3;                  \
+		register uint64_t a4 __asm__("a4") = args.a4;                  \
+		register uint64_t a5 __asm__("a5") = args.a5;                  \
+		register uint64_t a6 __asm__("a6") = args.a6;                  \
+		register uint64_t a7 __asm__("a7") = args.a7;                  \
+		switch ((width + 7) / 8) {                                     \
+		case 0:                                                        \
+			__asm__ volatile("ecall" : "+r"(t0));                  \
+			break;                                                 \
+		case 1:                                                        \
+			__asm__ volatile("ecall" : "+r"(t0), "+r"(a0));        \
+			break;                                                 \
+		case 2:                                                        \
+			__asm__ volatile("ecall"                               \
+					 : "+r"(t0), "+r"(a0)                  \
+					 : "r"(a1));                           \
+			break;                                                 \
+		case 3:                                                        \
+			__asm__ volatile("ecall"                               \
+					 : "+r"(t0), "+r"(a0)                  \
+					 : "r"(a1), "r"(a2));                  \
+			break;                                                 \
+		case 4:                                                        \
+			__asm__ volatile("ecall"                               \
+					 : "+r"(t0), "+r"(a0)                  \
+					 : "r"(a1), "r"(a2), "r"(a3));         \
+			break;                                                 \
+		case 5:                                                        \
+			__asm__ volatile("ecall"                               \
+					 : "+r"(t0), "+r"(a0)                  \
+					 : "r"(a1), "r"(a2), "r"(a3),          \
+					   "r"(a4));                           \
+			break;                                                 \
+		case 6:                                                        \
+			__asm__ volatile("ecall"                               \
+					 : "+r"(t0), "+r"(a0)                  \
+					 : "r"(a1), "r"(a2), "r"(a3), "r"(a4), \
+					   "r"(a5));                           \
+			break;                                                 \
+		case 7:                                                        \
+			__asm__ volatile("ecall"                               \
+					 : "+r"(t0), "+r"(a0)                  \
+					 : "r"(a1), "r"(a2), "r"(a3), "r"(a4), \
+					   "r"(a5), "r"(a6));                  \
+			break;                                                 \
+		case 8:                                                        \
+			__asm__ volatile("ecall"                               \
+					 : "=r"(t0), "+r"(a0)                  \
+					 : "r"(a1), "r"(a2), "r"(a3), "r"(a4), \
+					   "r"(a5), "r"(a6), "r"(a7));         \
+			break;                                                 \
+		}                                                              \
+		(s3k_ret_t){.err = t0, .val = a0};                             \
+	})
+
+uint64_t s3k_get_pid(void)
+{
+	sys_args_t args = {.get_info = {0}};
+	return DO_ECALL(S3K_SYS_GET_INFO, args, sizeof(args.get_info)).val;
+}
+
+uint64_t s3k_get_time(void)
+{
+	sys_args_t args = {.get_info = {1}};
+	return DO_ECALL(S3K_SYS_GET_INFO, args, sizeof(args.get_info)).val;
+}
+
+uint64_t s3k_get_timeout(void)
+{
+	sys_args_t args = {.get_info = {2}};
+	return DO_ECALL(S3K_SYS_GET_INFO, args, sizeof(args.get_info)).val;
+}
+
+uint64_t s3k_reg_read(s3k_reg_t reg)
+{
+	sys_args_t args = {.reg_read = {reg}};
+	return DO_ECALL(S3K_SYS_REG_READ, args, sizeof(args.reg_read)).val;
+}
+
+uint64_t s3k_reg_write(s3k_reg_t reg, uint64_t val)
+{
+	sys_args_t args = {
+	    .reg_write = {reg, val}
+	   };
+	return DO_ECALL(S3K_SYS_REG_WRITE, args, sizeof(args.reg_write)).val;
+}
+
+void s3k_sync(void)
+{
+	sys_args_t args = {.sync = {true}};
+	DO_ECALL(S3K_SYS_SYNC, args, sizeof(args.sync));
+}
+
+void s3k_sync_mem(void)
+{
+	sys_args_t args = {.sync = {false}};
+	DO_ECALL(S3K_SYS_SYNC, args, sizeof(args.sync));
+}
+
+void s3k_sleep(uint64_t time)
+{
+	sys_args_t args = {.sleep = {time}};
+	DO_ECALL(S3K_SYS_SLEEP, args, sizeof(args.sleep));
+}
+
+s3k_err_t s3k_cap_read(s3k_cidx_t idx, s3k_cap_t *cap)
+{
+	sys_args_t args = {.cap_read = {idx}};
+	s3k_ret_t ret = DO_ECALL(S3K_SYS_CAP_READ, args, sizeof(args.cap_read));
+	cap->raw = ret.val;
+	return ret.err;
+}
+
+s3k_err_t s3k_cap_move(s3k_cidx_t src, s3k_cidx_t dst)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_cap_move(src, dst);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_cap_delete(s3k_cidx_t idx)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_cap_delete(idx);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_cap_revoke(s3k_cidx_t idx)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_cap_revoke(idx);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_cap_derive(s3k_cidx_t src, s3k_cidx_t dst, s3k_cap_t ncap)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_cap_derive(src, dst, ncap);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_pmp_load(s3k_cidx_t idx, s3k_pmp_slot_t slot)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_pmp_load(idx, slot);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_pmp_unload(s3k_cidx_t idx)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_pmp_unload(idx);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_mon_suspend(s3k_cidx_t mon_idx, s3k_pid_t pid)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_mon_suspend(mon_idx, pid);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_mon_resume(s3k_cidx_t mon_idx, s3k_pid_t pid)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_mon_resume(mon_idx, pid);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_mon_state_get(s3k_cidx_t mon_idx, s3k_pid_t pid,
+			    s3k_state_t *state)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_mon_state_get(mon_idx, pid, state);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_mon_yield(s3k_cidx_t mon_idx, s3k_pid_t pid)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_mon_yield(mon_idx, pid);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_mon_reg_read(s3k_cidx_t mon_idx, s3k_pid_t pid, s3k_reg_t reg,
+			   uint64_t *val)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_mon_reg_read(mon_idx, pid, reg, val);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_mon_reg_write(s3k_cidx_t mon_idx, s3k_pid_t pid, s3k_reg_t reg,
+			    uint64_t val)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_mon_reg_write(mon_idx, pid, reg, val);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_mon_cap_read(s3k_cidx_t mon_idx, s3k_pid_t pid, s3k_cidx_t idx,
+			   s3k_cap_t *cap)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_mon_cap_read(mon_idx, pid, idx, cap);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_mon_cap_move(s3k_cidx_t mon_idx, s3k_pid_t src_pid,
+			   s3k_cidx_t src_idx, s3k_pid_t dst_pid,
+			   s3k_cidx_t dst_idx)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_mon_cap_move(mon_idx, src_pid, src_idx, dst_pid,
+					   dst_idx);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_mon_pmp_load(s3k_cidx_t mon_idx, s3k_pid_t pid,
+			   s3k_cidx_t pmp_idx, s3k_pmp_slot_t pmp_slot)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_mon_pmp_load(mon_idx, pid, pmp_idx, pmp_slot);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_mon_pmp_unload(s3k_cidx_t mon_idx, s3k_pid_t pid,
+			     s3k_cidx_t pmp_idx)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_mon_pmp_unload(mon_idx, pid, pmp_idx);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_err_t s3k_sock_send(s3k_cidx_t sock_idx, const s3k_msg_t *msg)
+{
+	s3k_err_t err;
+	do {
+		err = s3k_try_sock_send(sock_idx, msg);
+	} while (err == S3K_ERR_PREEMPTED);
+	return err;
+}
+
+s3k_reply_t s3k_sock_recv(s3k_cidx_t sock_idx, s3k_cidx_t cap_idx)
+{
+	s3k_reply_t reply;
+	do {
+		reply = s3k_try_sock_recv(sock_idx, cap_idx);
+	} while (reply.err == S3K_ERR_PREEMPTED);
+	return reply;
+}
+
+s3k_reply_t s3k_sock_sendrecv(s3k_cidx_t sock_idx, const s3k_msg_t *msg)
+{
+	s3k_reply_t reply;
+	do {
+		reply = s3k_try_sock_sendrecv(sock_idx, msg);
+	} while (reply.err == S3K_ERR_PREEMPTED);
+	return reply;
+}
+
+s3k_err_t s3k_try_cap_move(s3k_cidx_t src, s3k_cidx_t dst)
+{
+	sys_args_t args = {
+	    .cap_move = {src, dst}
+	  };
+	return DO_ECALL(S3K_SYS_CAP_MOVE, args, sizeof(args.cap_move)).err;
+}
+
+s3k_err_t s3k_try_cap_delete(s3k_cidx_t idx)
+{
+	const sys_args_t args = {.cap_delete = {idx}};
+	return DO_ECALL(S3K_SYS_CAP_DELETE, args, sizeof(args.cap_delete)).err;
+}
+
+s3k_err_t s3k_try_cap_revoke(s3k_cidx_t idx)
+{
+	sys_args_t args = {.cap_revoke = {idx}};
+	return DO_ECALL(S3K_SYS_CAP_REVOKE, args, sizeof(args.cap_revoke)).err;
+}
+
+s3k_err_t s3k_try_cap_derive(s3k_cidx_t src, s3k_cidx_t dst, s3k_cap_t ncap)
+{
+	sys_args_t args = {
+	    .cap_derive = {src, dst, ncap.raw}
+	      };
+	return DO_ECALL(S3K_SYS_CAP_DERIVE, args, sizeof(args.cap_derive)).err;
+}
+
+s3k_err_t s3k_try_pmp_load(s3k_cidx_t idx, s3k_pmp_slot_t slot)
+{
+	sys_args_t args = {
+	    .pmp_load = {idx, slot}
+	   };
+	return DO_ECALL(S3K_SYS_PMP_LOAD, args, sizeof(args.pmp_load)).err;
+}
+
+s3k_err_t s3k_try_pmp_unload(s3k_cidx_t idx)
+{
+	sys_args_t args = {.pmp_unload = {idx}};
+	return DO_ECALL(S3K_SYS_PMP_UNLOAD, args, sizeof(args.pmp_unload)).err;
+}
+
+s3k_err_t s3k_try_mon_suspend(s3k_cidx_t mon, s3k_pid_t pid)
+{
+	sys_args_t args = {
+	    .mon_state = {mon, pid}
+	   };
+	return DO_ECALL(S3K_SYS_MON_SUSPEND, args, sizeof(args.mon_state)).err;
+}
+
+s3k_err_t s3k_try_mon_resume(s3k_cidx_t mon, s3k_pid_t pid)
+{
+	sys_args_t args = {
+	    .mon_state = {mon, pid}
+	   };
+	return DO_ECALL(S3K_SYS_MON_RESUME, args, sizeof(args.mon_state)).err;
+}
+
+s3k_err_t s3k_try_mon_state_get(s3k_cidx_t mon, s3k_pid_t pid,
+				s3k_state_t *state)
+{
+	sys_args_t args = {
+	    .mon_state = {mon, pid}
+	   };
+	s3k_ret_t ret
+	    = DO_ECALL(S3K_SYS_MON_STATE_GET, args, sizeof(args.mon_state));
+	*state = ret.val;
+	return ret.err;
+}
+
+s3k_err_t s3k_try_mon_yield(s3k_cidx_t mon, s3k_pid_t pid)
+{
+	sys_args_t args = {
+	    .mon_state = {mon, pid}
+	   };
+	return DO_ECALL(S3K_SYS_MON_YIELD, args, sizeof(args.mon_state)).err;
+}
+
+s3k_err_t s3k_try_mon_reg_read(s3k_cidx_t mon, s3k_pid_t pid, s3k_reg_t reg,
+			       uint64_t *val)
+{
+	sys_args_t args = {
+	    .mon_reg_read = {mon, pid, reg}
+	   };
+	s3k_ret_t ret
+	    = DO_ECALL(S3K_SYS_MON_REG_READ, args, sizeof(args.mon_reg_read));
+	*val = ret.val;
+	return ret.err;
+}
+
+s3k_err_t s3k_try_mon_reg_write(s3k_cidx_t mon, s3k_pid_t pid, s3k_reg_t reg,
+				uint64_t val)
+{
+	sys_args_t args = {
+	    .mon_reg_write = {mon, pid, reg, val}
+	 };
+	s3k_ret_t ret
+	    = DO_ECALL(S3K_SYS_MON_REG_WRITE, args, sizeof(args.mon_reg_write));
+	return ret.err;
+}
+
+s3k_err_t s3k_try_mon_cap_read(s3k_cidx_t mon_idx, s3k_pid_t pid,
+			       s3k_cidx_t idx, s3k_cap_t *cap)
+{
+	sys_args_t args = {
+	    .mon_cap_read = {mon_idx, pid, idx}
+	       };
+	s3k_ret_t ret
+	    = DO_ECALL(S3K_SYS_MON_CAP_READ, args, sizeof(args.mon_cap_read));
+	if (!ret.err)
+		cap->raw = ret.val;
+	return ret.err;
+}
+
+s3k_err_t s3k_try_mon_cap_move(s3k_cidx_t mon_idx, s3k_pid_t src_pid,
+			       s3k_cidx_t src_idx, s3k_pid_t dst_pid,
+			       s3k_cidx_t dst_idx)
+{
+	sys_args_t args = {
+	    .mon_cap_move = {mon_idx, src_pid, src_idx, dst_pid, dst_idx}
+	 };
+	return DO_ECALL(S3K_SYS_MON_CAP_MOVE, args, sizeof(args.mon_cap_move))
+	    .err;
+}
+
+s3k_err_t s3k_try_mon_pmp_load(s3k_cidx_t mon_idx, s3k_pid_t pid,
+			       s3k_cidx_t idx, s3k_pmp_slot_t slot)
+{
+	sys_args_t args = {
+	    .mon_pmp_load = {mon_idx, pid, idx, slot}
+	     };
+	return DO_ECALL(S3K_SYS_MON_PMP_LOAD, args, sizeof(args.mon_pmp_load))
+	    .err;
+}
+
+s3k_err_t s3k_try_mon_pmp_unload(s3k_cidx_t mon_idx, s3k_pid_t pid,
+				 s3k_cidx_t idx)
+{
+	sys_args_t args = {
+	    .mon_pmp_unload = {mon_idx, pid, idx}
+	 };
+	return DO_ECALL(S3K_SYS_MON_PMP_UNLOAD, args,
+			sizeof(args.mon_pmp_unload))
+	    .err;
+}
+
+s3k_err_t s3k_try_sock_send(s3k_cidx_t sock_idx, const s3k_msg_t *msg)
+{
+	sys_args_t args = {
+	    .sock = {.sock_idx = sock_idx,
+		     .cap_idx = msg->cap_idx,
+		     .send_cap = msg->send_cap,
+		     {msg->data[0], msg->data[1], msg->data[2], msg->data[3]}}
+	      };
+	return DO_ECALL(S3K_SYS_SOCK_SEND, args, sizeof(args.sock)).err;
+}
+
+s3k_reply_t s3k_try_sock_recv(s3k_cidx_t sock_idx, s3k_cidx_t cap_idx)
+{
+	sys_args_t args = {
+	    .sock = {.sock_idx = sock_idx, .cap_idx = cap_idx}
+	      };
+	register uint64_t t0 __asm__("t0") = S3K_SYS_SOCK_RECV;
+	register uint64_t a0 __asm__("a0") = args.a0;
+	register uint64_t a1 __asm__("a1") = args.a1;
+	register uint64_t a2 __asm__("a2") = args.a2;
+	register uint64_t a3 __asm__("a3") = args.a3;
+	register uint64_t a4 __asm__("a4") = args.a4;
+	register uint64_t a5 __asm__("a5") = args.a5;
+	register uint64_t a6 __asm__("a6") = args.a6;
+	register uint64_t a7 __asm__("a7") = args.a7;
+	__asm__ volatile("ecall"
+			 : "+r"(t0), "+r"(a0), "+r"(a1), "+r"(a2), "+r"(a3),
+			   "+r"(a4), "+r"(a5)
+			 : "r"(a6), "r"(a7));
+	s3k_reply_t reply;
+	reply.err = t0;
+	reply.tag = a0;
+	reply.cap.raw = a1;
+	reply.data[0] = a2;
+	reply.data[1] = a3;
+	reply.data[2] = a4;
+	reply.data[3] = a5;
+	return reply;
+}
+
+s3k_reply_t s3k_try_sock_sendrecv(s3k_cidx_t sock_idx, const s3k_msg_t *msg)
+{
+	sys_args_t args = {
+	    .sock = {.sock_idx = sock_idx,
+		     .cap_idx = msg->cap_idx,
+		     .send_cap = msg->send_cap,
+		     {msg->data[0], msg->data[1], msg->data[2], msg->data[3]}}
+	      };
+	register uint64_t t0 __asm__("t0") = S3K_SYS_SOCK_SENDRECV;
+	register uint64_t a0 __asm__("a0") = args.a0;
+	register uint64_t a1 __asm__("a1") = args.a1;
+	register uint64_t a2 __asm__("a2") = args.a2;
+	register uint64_t a3 __asm__("a3") = args.a3;
+	register uint64_t a4 __asm__("a4") = args.a4;
+	register uint64_t a5 __asm__("a5") = args.a5;
+	register uint64_t a6 __asm__("a6") = args.a6;
+	register uint64_t a7 __asm__("a7") = args.a7;
+	__asm__ volatile("ecall"
+			 : "+r"(t0), "+r"(a0), "+r"(a1), "+r"(a2), "+r"(a3),
+			   "+r"(a4), "+r"(a5)
+			 : "r"(a6), "r"(a7));
+	s3k_reply_t reply;
+	reply.err = t0;
+	reply.tag = a0;
+	reply.cap.raw = a1;
+	reply.data[0] = a2;
+	reply.data[1] = a3;
+	reply.data[2] = a4;
+	reply.data[3] = a5;
+	return reply;
+}
diff --git a/kernel/Makefile b/kernel/Makefile
index bfcb41cd..1864f7bc 100644
--- a/kernel/Makefile
+++ b/kernel/Makefile
@@ -1,5 +1,6 @@
 # See LICENSE file for copyright and license details.
 .POSIX:
+.SECONDARY:
 
 # Kernel basename
 ROOT=..
@@ -12,15 +13,15 @@ include ${ROOT}/common/plat/${PLATFORM}.mk
 CFLAGS:=-march=${ARCH} -mabi=${ABI} -mcmodel=${CMODEL} \
 	-DPLATFORM_${PLATFORM} \
 	-std=c11 \
-	-Os -g3 \
+	-O2 -g3 \
 	-Wall -Wextra -Werror \
 	-Wno-unused-parameter \
 	-Wshadow -fno-common \
 	-Wno-builtin-declaration-mismatch \
 	-fno-stack-protector \
-	-flto \
+	-flto -fwhole-program \
 	-include ${S3K_CONF_H} \
-	-Iinc -I${COMMON_INC}
+	-Iinc -I${COMMON_INC} \
 
 # LD flags
 LDFLAGS:=-march=${ARCH} -mabi=${ABI} -mcmodel=${CMODEL} \
@@ -51,32 +52,38 @@ BIN:=${ELF:.elf=.bin}
 HEX:=${ELF:.elf=.hex}
 DA :=${ELF:.elf=.da}
 
-all: ${ELF} ${BIN} ${HEX}
+all: ${ELF} ${BIN} ${HEX} ${DA}
 
 clean:
 	rm -f ${ELF} ${OBJS} ${DEPS}
 
 ${BUILD}/${PROGRAM}/%.o: src/%.S
 	@mkdir -p ${@D}
-	${CC} -o $@ $< -c -MMD ${CFLAGS} 
+	@${CC} -o $@ $< -c -MMD ${CFLAGS}
+	@printf "CC\t$@\n"
 
 ${BUILD}/${PROGRAM}/%.o: src/%.c
 	@mkdir -p ${@D}
-	${CC} -o $@ $< -c -MMD ${CFLAGS}
+	@${CC} -o $@ $< -c -MMD ${CFLAGS}
+	@printf "CC\t$@\n"
 
 %.elf: ${OBJS}
 	@mkdir -p ${@D}
-	${CC} -o $@ ${OBJS} ${LDFLAGS}
+	@${CC} -o $@ ${OBJS} ${LDFLAGS}
+	@printf "CC\t$@\n"
 
 %.bin: %.elf
-	${OBJCOPY} -O binary $< $@
+	@${OBJCOPY} -O binary $< $@
+	@printf "OBJCOPY\t$@\n"
 
 %.hex: %.elf
-	${OBJCOPY} -O ihex $< $@
+	@${OBJCOPY} -O ihex $< $@
+	@printf "OBJCOPY\t$@\n"
 
 %.da: %.elf
-	${OBJDUMP} -D $< > $@
+	@${OBJDUMP} -D $< > $@
+	@printf "OBJDUMP\t$@\n"
 
-.PHONY: all common da format clean
+.PHONY: all da format clean
 
 -include ${DEPS}
diff --git a/kernel/inc/cap_ipc.h b/kernel/inc/cap_ipc.h
index 483edf5b..1db9f44a 100644
--- a/kernel/inc/cap_ipc.h
+++ b/kernel/inc/cap_ipc.h
@@ -7,12 +7,12 @@
 #include <stdint.h>
 
 typedef struct ipc_msg {
-	cte_t src_buf;
+	cte_t cap_buf;
 	bool send_cap;
 	uint64_t data[4];
 } ipc_msg_t;
 
 err_t cap_sock_send(cte_t sock, const ipc_msg_t *msg, proc_t **next);
-err_t cap_sock_recv(cte_t sock);
+err_t cap_sock_recv(cte_t sock, const cte_t cap_buf, proc_t **next);
 err_t cap_sock_sendrecv(cte_t sock, const ipc_msg_t *msg, proc_t **next);
-void cap_sock_clear(cap_t cap, proc_t *p);
+void cap_sock_clear(cap_t cap, proc_t *next);
diff --git a/kernel/inc/cap_lock.h b/kernel/inc/cap_lock.h
new file mode 100644
index 00000000..c8606ca9
--- /dev/null
+++ b/kernel/inc/cap_lock.h
@@ -0,0 +1,5 @@
+#include <stdbool.h>
+
+void cap_lock_init(void);
+bool cap_lock_acquire(void);
+void cap_lock_release(void);
diff --git a/kernel/inc/cap_ops.h b/kernel/inc/cap_ops.h
index f4b0464c..b1d15a2c 100644
--- a/kernel/inc/cap_ops.h
+++ b/kernel/inc/cap_ops.h
@@ -4,8 +4,8 @@
 #include "error.h"
 
 err_t cap_read(cte_t cte, cap_t *cap);
-err_t cap_move(cte_t src, cte_t dst, cap_t *cap);
+err_t cap_move(cte_t src, cte_t dst);
+err_t cap_ipc_move(cte_t src, cte_t dst);
 err_t cap_delete(cte_t cte);
-void cap_reclaim(cte_t parent, cap_t parent_cap, cte_t child, cap_t child_cap);
-err_t cap_reset(cte_t cte);
+err_t cap_revoke(cte_t parent);
 err_t cap_derive(cte_t src, cte_t dst, cap_t new_cap);
diff --git a/kernel/inc/cap_table.h b/kernel/inc/cap_table.h
index 72a76a1f..df066393 100644
--- a/kernel/inc/cap_table.h
+++ b/kernel/inc/cap_table.h
@@ -19,6 +19,6 @@ cte_t cte_next(cte_t c);
 cte_t cte_prev(cte_t c);
 cap_t cte_cap(cte_t c);
 uint64_t cte_pid(cte_t c);
-void cte_move(cte_t src, cte_t dst, cap_t *cap);
+void cte_move(cte_t src, cte_t dst);
 cap_t cte_delete(cte_t c);
 void cte_insert(cte_t c, cap_t cap, cte_t prev);
diff --git a/kernel/inc/cap_types.h b/kernel/inc/cap_types.h
index b212343c..b4d4095c 100644
--- a/kernel/inc/cap_types.h
+++ b/kernel/inc/cap_types.h
@@ -56,6 +56,7 @@ typedef enum capty {
 	CAPTY_MONITOR = 4, ///< Monitor capability.
 	CAPTY_CHANNEL = 5, ///< IPC Channel capability.
 	CAPTY_SOCKET = 6,  ///< IPC Socket capability.
+	CAPTY_COUNT	   ///< Number of capability types
 } capty_t;
 
 /// Capability description
diff --git a/kernel/inc/cap_util.h b/kernel/inc/cap_util.h
index 474f523f..0b377244 100644
--- a/kernel/inc/cap_util.h
+++ b/kernel/inc/cap_util.h
@@ -1,6 +1,8 @@
 #pragma once
 #include "cap_types.h"
 
+#include <stddef.h>
+
 cap_t cap_mk_time(hart_t hart, time_slot_t bgn, time_slot_t end);
 cap_t cap_mk_memory(addr_t bgn, addr_t end, rwx_t rwx);
 cap_t cap_mk_pmp(napot_t addr, rwx_t rwx);
@@ -9,6 +11,23 @@ cap_t cap_mk_channel(chan_t bgn, chan_t end);
 cap_t cap_mk_socket(chan_t chan, ipc_mode_t mode, ipc_perm_t perm,
 		    uint32_t tag);
 
-bool cap_is_valid(cap_t cap);
-bool cap_is_revokable(cap_t parent, cap_t child);
-bool cap_is_derivable(cap_t parent, cap_t child);
+static inline addr_t tag_block_to_addr(tag_t tag, block_t block)
+{
+	return ((uint64_t)tag << MAX_BLOCK_SIZE)
+	       + ((uint64_t)block << MIN_BLOCK_SIZE);
+}
+
+static inline void pmp_napot_decode(uint64_t addr, uint64_t *base,
+				    uint64_t *size)
+{
+	*base = ((addr + 1) & addr) << 2;
+	*size = (((addr + 1) ^ addr) + 1) << 2;
+}
+
+static inline uint64_t pmp_napot_encode(uint64_t base, uint64_t size)
+{
+	return (base | (size / 2 - 1)) >> 2;
+}
+
+bool cap_is_valid(const cap_t cap);
+void cap_snprint(char *restrict buf, size_t size, cap_t cap);
diff --git a/kernel/inc/csr.h b/kernel/inc/csr.h
index 0b95af9a..7afa69af 100644
--- a/kernel/inc/csr.h
+++ b/kernel/inc/csr.h
@@ -15,32 +15,24 @@
 #define MSTATUS_MIE 0x8
 
 #ifndef __ASSEMBLER__
-#include <stdint.h>
-uint64_t csrr_mhartid(void);
-uint64_t csrr_mip(void);
-uint64_t csrr_mcycle(void);
-uint64_t csrr_mhpmcounter3(void);
-void csrw_mcycle(uint64_t val);
-void csrw_mhpmcounter3(uint64_t val);
-void csrw_mstatus(uint64_t val);
-void csrs_mstatus(uint64_t val);
-void csrc_mstatus(uint64_t val);
-uint64_t csrr_pmpcfg0(void);
-uint64_t csrr_pmpaddr0(void);
-uint64_t csrr_pmpaddr1(void);
-uint64_t csrr_pmpaddr2(void);
-uint64_t csrr_pmpaddr3(void);
-uint64_t csrr_pmpaddr4(void);
-uint64_t csrr_pmpaddr5(void);
-uint64_t csrr_pmpaddr6(void);
-uint64_t csrr_pmpaddr7(void);
-void csrw_pmpcfg0(uint64_t val);
-void csrw_pmpaddr0(uint64_t val);
-void csrw_pmpaddr1(uint64_t val);
-void csrw_pmpaddr2(uint64_t val);
-void csrw_pmpaddr3(uint64_t val);
-void csrw_pmpaddr4(uint64_t val);
-void csrw_pmpaddr5(uint64_t val);
-void csrw_pmpaddr6(uint64_t val);
-void csrw_pmpaddr7(uint64_t val);
+#define csrr(__reg)                                                \
+	({                                                         \
+		unsigned long __ret;                               \
+		__asm__ volatile("csrr %0," #__reg : "=r"(__ret)); \
+		__ret;                                             \
+	})
+#define csrw(__reg, __val) \
+	({ __asm__ volatile("csrw " #__reg ", %0" ::"r"(__val)); })
+#define csrrw(__reg, __val)                               \
+	({                                                \
+		unsigned long __ret;                      \
+		__asm__ volatile("csrrw %0," #__reg ",%1" \
+				 : "=r"(__ret)            \
+				 : "r"(__val));           \
+		__ret;                                    \
+	})
+#define csrs(__reg, __val) \
+	({ __asm__ volatile("csrs " #__reg ", %0" ::"r"(__val)); })
+#define csrc(__reg, __val) \
+	({ __asm__ volatile("csrc " #__reg ", %0" ::"r"(__val)); })
 #endif
diff --git a/kernel/inc/error.h b/kernel/inc/error.h
index 29e12197..e6daa28f 100644
--- a/kernel/inc/error.h
+++ b/kernel/inc/error.h
@@ -1,7 +1,6 @@
 #pragma once
 
 typedef enum {
-	YIELD = -1,
 	SUCCESS = 0,
 	ERR_EMPTY,
 	ERR_SRC_EMPTY,
@@ -16,6 +15,7 @@ typedef enum {
 	ERR_INVALID_SOCKET,
 	ERR_INVALID_SYSCALL,
 	ERR_INVALID_REGISTER,
+	ERR_INVALID_CAPABILITY,
 	ERR_NO_RECEIVER,
 	ERR_PREEMPTED,
 	ERR_TIMEOUT,
diff --git a/kernel/inc/exception.h b/kernel/inc/exception.h
index 74c7099a..c77747a2 100644
--- a/kernel/inc/exception.h
+++ b/kernel/inc/exception.h
@@ -10,19 +10,5 @@
 
 #include <stdint.h>
 
-/**
- * @brief Handle an exception
- *
- * This function handles exceptions by checking the mcause register for
- * specific types of exceptions. If the exception is an illegal instruction
- * and the mtval register contains a valid return instruction, it calls the
- * handle_ret() function. Otherwise, it calls the handle_default() function
- * to handle the exception.
- *
- * @param p  Process having the exception
- * @param mcause  The value of the mcause register
- * @param mepc  The value of the mepc register
- * @param mtval  The value of the mtval register
- */
-void handle_exception(proc_t *p, uint64_t mcause, uint64_t mepc, uint64_t mtval)
-    __attribute__((noreturn));
+proc_t *handle_exception(void);
+proc_t *handle_illegal_instruction(void);
diff --git a/kernel/inc/kassert.h b/kernel/inc/kassert.h
index b21d5012..369bbb2f 100644
--- a/kernel/inc/kassert.h
+++ b/kernel/inc/kassert.h
@@ -1,5 +1,5 @@
 #pragma once
-#include "altc/altio.h"
+#include "kprintf.h"
 
 /**
  * @file kassert.h
@@ -17,28 +17,24 @@
  */
 #ifndef NDEBUG
 
-#define _X_(x) #x
 #define KASSERT_FAILURE(FILE, LINE) \
-	alt_puts("Kernel assertion failed at " FILE ":" _X_(LINE) ".");
+	kprintf(0, "Kernel assertion failed at %s:%d.\n", FILE, LINE);
 
-#define KASSERT(EXPR)                                        \
-	do {                                                 \
-		if (!(EXPR)) {                               \
-			KASSERT_FAILURE(__FILE__, __LINE__); \
-			while (1)                            \
-				;                            \
-		}                                            \
+#define KASSERT(expr)                                \
+	do {                                         \
+		if (expr)                            \
+			break;                       \
+		KASSERT_FAILURE(__FILE__, __LINE__); \
+		while (1)                            \
+			;                            \
 	} while (false)
 
 #else /* NDEBUG */
 
 #define KASSERT(expr)                            \
 	do {                                     \
-		if (!(expr)) {                   \
+		if (!(expr))                     \
 			__builtin_unreachable(); \
-			while (1)                \
-				;                \
-		}                                \
 	} while (false)
 
 #endif /* NDEBUG */
diff --git a/kernel/inc/kernel.h b/kernel/inc/kernel.h
index 6e9ec678..c1ae85c6 100644
--- a/kernel/inc/kernel.h
+++ b/kernel/inc/kernel.h
@@ -7,10 +7,8 @@
 
 void kernel_init(void);
 uint64_t kernel_wcet(void);
-void kernel_wcet_reset(void);
+uint64_t kernel_wcet_reset(void);
 
-bool kernel_lock(proc_t *p);
-void kernel_unlock(proc_t *p);
-
-void kernel_hook_sys_entry(proc_t *p);
-void kernel_hook_sys_exit(proc_t *p);
+void kernel_syscall_entry(void);
+void kernel_syscall_exit(void);
+bool kernel_preempt(void);
diff --git a/kernel/inc/kprintf.h b/kernel/inc/kprintf.h
new file mode 100644
index 00000000..a1b7ede8
--- /dev/null
+++ b/kernel/inc/kprintf.h
@@ -0,0 +1,4 @@
+#pragma once
+#include <stdbool.h>
+
+void kprintf(int verb, const char *restrict fmt, ...);
diff --git a/kernel/inc/macro.inc b/kernel/inc/macro.inc
index b0e94c28..fe0cf1f4 100644
--- a/kernel/inc/macro.inc
+++ b/kernel/inc/macro.inc
@@ -17,12 +17,3 @@
 	la	gp, __global_pointer$
 .option pop
 .endm
-
-.macro	instr_init
-#ifdef INSTRUMENT
-	csrw	mcounteren,0xF
-	csrw	scounteren,0xF
-	csrw	mcountinhibit,0x8
-	csrw	mhpmcounter3,0
-#endif
-.endm
diff --git a/kernel/inc/offsets.h b/kernel/inc/offsets.h
index 9c1bd63f..87e25563 100644
--- a/kernel/inc/offsets.h
+++ b/kernel/inc/offsets.h
@@ -2,52 +2,54 @@
 
 #define _OFFSET(x) (x * 8)
 
+#define PROC_STATE _OFFSET(0)
+
 /* Register offsets */
-#define PROC_PC _OFFSET(0)
-#define PROC_RA _OFFSET(1)
-#define PROC_SP _OFFSET(2)
-#define PROC_GP _OFFSET(3)
-#define PROC_TP _OFFSET(4)
-#define PROC_T0 _OFFSET(5)
-#define PROC_T1 _OFFSET(6)
-#define PROC_T2 _OFFSET(7)
-#define PROC_S0 _OFFSET(8)
-#define PROC_S1 _OFFSET(9)
-#define PROC_A0 _OFFSET(10)
-#define PROC_A1 _OFFSET(11)
-#define PROC_A2 _OFFSET(12)
-#define PROC_A3 _OFFSET(13)
-#define PROC_A4 _OFFSET(14)
-#define PROC_A5 _OFFSET(15)
-#define PROC_A6 _OFFSET(16)
-#define PROC_A7 _OFFSET(17)
-#define PROC_S2 _OFFSET(18)
-#define PROC_S3 _OFFSET(19)
-#define PROC_S4 _OFFSET(20)
-#define PROC_S5 _OFFSET(21)
-#define PROC_S6 _OFFSET(22)
-#define PROC_S7 _OFFSET(23)
-#define PROC_S8 _OFFSET(24)
-#define PROC_S9 _OFFSET(25)
-#define PROC_S10 _OFFSET(26)
-#define PROC_S11 _OFFSET(27)
-#define PROC_T3 _OFFSET(28)
-#define PROC_T4 _OFFSET(29)
-#define PROC_T5 _OFFSET(30)
-#define PROC_T6 _OFFSET(31)
-#define PROC_TPC _OFFSET(32)
-#define PROC_TSP _OFFSET(33)
-#define PROC_EPC _OFFSET(34)
-#define PROC_ESP _OFFSET(35)
-#define PROC_ECAUSE _OFFSET(36)
-#define PROC_EVAL _OFFSET(37)
-#define PROC_SERVTIME _OFFSET(38)
-#define PROC_PMPCFG0 _OFFSET(39)
-#define PROC_PMPADDR0 _OFFSET(40)
-#define PROC_PMPADDR1 _OFFSET(41)
-#define PROC_PMPADDR2 _OFFSET(42)
-#define PROC_PMPADDR3 _OFFSET(43)
-#define PROC_PMPADDR4 _OFFSET(44)
-#define PROC_PMPADDR5 _OFFSET(45)
-#define PROC_PMPADDR6 _OFFSET(46)
-#define PROC_PMPADDR7 _OFFSET(47)
+#define PROC_PC _OFFSET(1)
+#define PROC_RA _OFFSET(2)
+#define PROC_SP _OFFSET(3)
+#define PROC_GP _OFFSET(4)
+#define PROC_TP _OFFSET(5)
+#define PROC_T0 _OFFSET(6)
+#define PROC_T1 _OFFSET(7)
+#define PROC_T2 _OFFSET(8)
+#define PROC_S0 _OFFSET(9)
+#define PROC_S1 _OFFSET(10)
+#define PROC_A0 _OFFSET(11)
+#define PROC_A1 _OFFSET(12)
+#define PROC_A2 _OFFSET(13)
+#define PROC_A3 _OFFSET(14)
+#define PROC_A4 _OFFSET(15)
+#define PROC_A5 _OFFSET(16)
+#define PROC_A6 _OFFSET(17)
+#define PROC_A7 _OFFSET(18)
+#define PROC_S2 _OFFSET(19)
+#define PROC_S3 _OFFSET(20)
+#define PROC_S4 _OFFSET(21)
+#define PROC_S5 _OFFSET(22)
+#define PROC_S6 _OFFSET(23)
+#define PROC_S7 _OFFSET(24)
+#define PROC_S8 _OFFSET(25)
+#define PROC_S9 _OFFSET(26)
+#define PROC_S10 _OFFSET(27)
+#define PROC_S11 _OFFSET(28)
+#define PROC_T3 _OFFSET(29)
+#define PROC_T4 _OFFSET(30)
+#define PROC_T5 _OFFSET(31)
+#define PROC_T6 _OFFSET(32)
+#define PROC_TPC _OFFSET(33)
+#define PROC_TSP _OFFSET(34)
+#define PROC_EPC _OFFSET(35)
+#define PROC_ESP _OFFSET(36)
+#define PROC_ECAUSE _OFFSET(37)
+#define PROC_EVAL _OFFSET(38)
+#define PROC_SERVTIME _OFFSET(39)
+#define PROC_PMPCFG0 _OFFSET(40)
+#define PROC_PMPADDR0 _OFFSET(41)
+#define PROC_PMPADDR1 _OFFSET(42)
+#define PROC_PMPADDR2 _OFFSET(43)
+#define PROC_PMPADDR3 _OFFSET(44)
+#define PROC_PMPADDR4 _OFFSET(45)
+#define PROC_PMPADDR5 _OFFSET(46)
+#define PROC_PMPADDR6 _OFFSET(47)
+#define PROC_PMPADDR7 _OFFSET(48)
diff --git a/kernel/inc/pmp.h b/kernel/inc/pmp.h
deleted file mode 100644
index 3b1af0db..00000000
--- a/kernel/inc/pmp.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#pragma once
-
-#include "proc.h"
-
-#include <stdint.h>
-
-static inline void pmp_napot_decode(uint64_t addr, uint64_t *base,
-				    uint64_t *size)
-{
-	*base = ((addr + 1) & addr) << 2;
-	*size = (((addr + 1) ^ addr) + 1) << 2;
-}
-
-static inline uint64_t pmp_napot_encode(uint64_t base, uint64_t size)
-{
-	return (base | (size / 2 - 1)) >> 2;
-}
diff --git a/kernel/inc/preempt.h b/kernel/inc/preempt.h
deleted file mode 100644
index 072fbf21..00000000
--- a/kernel/inc/preempt.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#pragma once
-#include "csr.h"
-
-#include <stdbool.h>
-
-static inline bool preempt(void)
-{
-	return csrr_mip() & MIP_MTIP;
-}
-
-static inline void preempt_enable(void)
-{
-	csrs_mstatus(MSTATUS_MIE);
-}
-
-static inline void preempt_disable(void)
-{
-	csrc_mstatus(MSTATUS_MIE);
-}
diff --git a/kernel/inc/proc.h b/kernel/inc/proc.h
index 1fc6fcd3..b5e5ba2b 100644
--- a/kernel/inc/proc.h
+++ b/kernel/inc/proc.h
@@ -17,11 +17,6 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-/** Process state flags
- * PSF_BUSY: Process has been acquired.
- * PSF_BLOCKED: Waiting for IPC.
- * PSF_SUSPENDED: Waiting for monitor
- */
 typedef uint64_t proc_state_t;
 
 typedef enum {
@@ -80,6 +75,8 @@ typedef enum {
  * Contains all information needed manage a process except the capabilities.
  */
 typedef struct {
+	/** Process state. */
+	proc_state_t state;
 	/** The registers of the process (RISC-V registers and virtual
 	 * registers). */
 	uint64_t regs[REG_CNT];
@@ -89,17 +86,8 @@ typedef struct {
 	/** Instrumentation registers */
 	/** Process ID. */
 	pid_t pid;
-	/** Process state. */
-	proc_state_t state;
-	qnode_t qnode;
 
 	/** Scheduling information */
-
-	/***** IPC related things *****/
-	/**
-	 * Timeout of IPC yielding send.
-	 * Time before sender gives up.
-	 */
 	uint64_t timeout;
 	/**
 	 * Minimum remaining time required for receiving messages.
@@ -107,17 +95,13 @@ typedef struct {
 	 * it is not allowed to send the message.
 	 */
 	uint64_t serv_time;
-	/**
-	 * Source and destination pointer for transmitting capabilities.
-	 */
-	cte_t cap_buf;
 } proc_t;
 
+register proc_t *current __asm__("tp");
+
 /**
  * Initializes all processes in the system.
  *
- * @param payload A pointer to the boot loader's code.
- *
  * @note This function should be called only once during system startup.
  */
 void proc_init(void);
@@ -130,48 +114,14 @@ void proc_init(void);
  */
 proc_t *proc_get(pid_t pid);
 
-/**
- * @brief Attempt to acquire the lock for a process.
- *
- * The process's lock is embedded in its state. This function attempts to
- * acquire the lock by atomically setting the LSB of the state to 1 if it
- * currently has the value 'expected'. If the lock is already held by another
- * process, this function will return false.
- *
- * @param proc Pointer to the process to acquire the lock for.
- * @param expected The expected value of the process's state.
- * @return True if the lock was successfully acquired, false otherwise.
- */
-bool proc_acquire(proc_t *proc);
+proc_state_t proc_get_state(proc_t *proc);
 
-/**
- * @brief Release the lock on a process.
- *
- * The process's lock is embedded in its state. This function sets the LSB of
- * the state to 0 to unlock the process.
- *
- * @param proc Pointer to the process to release the lock for.
- */
+bool proc_acquire(proc_t *proc);
 void proc_release(proc_t *proc);
-
-/**
- * Set the process to a suspended state without locking it. The process may
- * still be running, but it will not resume after its timeslice has ended.
- *
- * @param proc Pointer to process to suspend.
- */
 void proc_suspend(proc_t *proc);
-
-/**
- * Resumes a process from its suspend state without locking it.
- *
- * @param proc Pointer to process to be resumed.
- */
 void proc_resume(proc_t *proc);
-
-void proc_ipc_wait(proc_t *proc, chan_t channel);
-bool proc_ipc_acquire(proc_t *proc, chan_t channel);
-
+void proc_ipc_wait(proc_t *proc, chan_t chan);
+bool proc_ipc_acquire(proc_t *proc, chan_t chan);
 bool proc_is_suspended(proc_t *proc);
 
 bool proc_pmp_avail(proc_t *proc, pmp_slot_t slot);
diff --git a/kernel/inc/sched.h b/kernel/inc/sched.h
index 5272915d..482883c7 100644
--- a/kernel/inc/sched.h
+++ b/kernel/inc/sched.h
@@ -29,7 +29,7 @@ void sched_init(void);
  * This function finds the next process to schedule based on the current
  * state of the system.
  */
-void sched(proc_t *) NORETURN;
+proc_t *sched(void);
 
 /// Let pid run on hartid, begin-end.
 void sched_update(uint64_t pid, uint64_t end, uint64_t hartid, uint64_t from,
diff --git a/kernel/inc/syscall.h b/kernel/inc/syscall.h
index 7cf7414b..84fefe91 100644
--- a/kernel/inc/syscall.h
+++ b/kernel/inc/syscall.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "cap_types.h"
+#include "macro.h"
 #include "proc.h"
 
 #include <stdint.h>
@@ -11,6 +12,7 @@ typedef enum {
 	SYS_REG_READ,  // Set the value of a specific register
 	SYS_REG_WRITE, // Get the value of a specific register
 	SYS_SYNC,      // Synchronize with capabilities/scheduling
+	SYS_SLEEP,
 
 	// Capability Management
 	SYS_CAP_READ,	// Read the properties of a capability
@@ -47,64 +49,111 @@ typedef union {
 	};
 
 	struct {
-		int info;
+		uint64_t info;
 	} get_info;
 
 	struct {
-		regnr_t reg;
+		uint64_t reg;
+	} reg_read;
+
+	struct {
+		uint64_t reg;
 		uint64_t val;
-	} reg;
+	} reg_write;
 
 	struct {
-		bool full;
+		uint64_t full;
 	} sync;
 
 	struct {
-		cidx_t idx;
-		cidx_t dst_idx;
-		cap_t cap;
-	} cap;
+		uint64_t time;
+	} sleep;
+
+	struct {
+		uint64_t idx;
+	} cap_read;
+
+	struct {
+		uint64_t src_idx;
+		uint64_t dst_idx;
+	} cap_move;
+
+	struct {
+		uint64_t idx;
+	} cap_delete;
+
+	struct {
+		uint64_t idx;
+	} cap_revoke;
 
 	struct {
-		cidx_t pmp_idx;
-		pmp_slot_t pmp_slot;
-	} pmp;
+		uint64_t src_idx;
+		uint64_t dst_idx;
+		uint64_t cap_raw;
+	} cap_derive;
 
 	struct {
-		cidx_t mon_idx;
-		pid_t pid;
+		uint64_t idx;
+		uint64_t slot;
+	} pmp_load;
+
+	struct {
+		uint64_t idx;
+	} pmp_unload;
+
+	struct {
+		uint64_t mon_idx;
+		uint64_t pid;
 	} mon_state;
 
 	struct {
-		cidx_t mon_idx;
-		pid_t pid;
-		regnr_t reg;
+		uint64_t mon_idx;
+		uint64_t pid;
+		uint64_t reg;
+	} mon_reg_read;
+
+	struct {
+		uint64_t mon_idx;
+		uint64_t pid;
+		uint64_t reg;
 		uint64_t val;
-	} mon_reg;
+	} mon_reg_write;
+
+	struct {
+		uint64_t mon_idx;
+		uint64_t pid;
+		uint64_t idx;
+	} mon_cap_read;
+
+	struct {
+		uint64_t mon_idx;
+		uint64_t src_pid;
+		uint64_t src_idx;
+		uint64_t dst_pid;
+		uint64_t dst_idx;
+	} mon_cap_move;
 
 	struct {
-		cidx_t mon_idx;
-		pid_t pid;
-		cidx_t idx;
-		pid_t dst_pid;
-		cidx_t dst_idx;
-	} mon_cap;
+		uint64_t mon_idx;
+		uint64_t pid;
+		uint64_t idx;
+		uint64_t slot;
+	} mon_pmp_load;
 
 	struct {
-		cidx_t mon_idx;
-		pid_t pid;
-		cidx_t pmp_idx;
-		pmp_slot_t pmp_slot;
-	} mon_pmp;
+		uint64_t mon_idx;
+		uint64_t pid;
+		uint64_t idx;
+	} mon_pmp_unload;
 
 	struct {
-		cidx_t sock_idx;
-		cidx_t cap_idx;
-		bool send_cap;
+		uint64_t sock_idx;
+		uint64_t cap_idx;
+		uint64_t send_cap;
 		uint64_t data[4];
 	} sock;
 } sys_args_t;
 
 _Static_assert(sizeof(sys_args_t) == 64, "sys_args_t has the wrong size");
 
-void handle_syscall(proc_t *p) __attribute__((noreturn));
+proc_t *handle_syscall(void);
diff --git a/kernel/inc/trap.h b/kernel/inc/trap.h
index 9a2166a4..20cbdae9 100644
--- a/kernel/inc/trap.h
+++ b/kernel/inc/trap.h
@@ -5,8 +5,9 @@
  * @copyright MIT License
  * @author Henrik Karlsson (henrik10@kth.se)
  */
+#include "macro.h"
+
 #include <stdint.h>
 
-void trap_entry(void) __attribute__((noreturn));
-void trap_exit(proc_t *) __attribute__((noreturn));
-void trap_resume(proc_t *) __attribute__((noreturn));
+void trap_entry(void) NORETURN;
+void trap_exit(void) NORETURN;
diff --git a/kernel/src/cap_ipc.c b/kernel/src/cap_ipc.c
index 17a0e447..5466b57d 100644
--- a/kernel/src/cap_ipc.c
+++ b/kernel/src/cap_ipc.c
@@ -1,11 +1,14 @@
 #include "cap_ipc.h"
 
+#include "altc/string.h"
 #include "cap_ops.h"
 #include "cap_table.h"
 #include "csr.h"
 #include "drivers/time.h"
 #include "error.h"
 #include "kassert.h"
+#include "kernel.h"
+#include "macro.h"
 #include "proc.h"
 
 #include <stdint.h>
@@ -13,181 +16,224 @@
 #define SERVER 0
 #define CLIENT 1
 
-static proc_t *clients[S3K_CHAN_CNT];
-static proc_t *servers[S3K_CHAN_CNT];
+struct {
+	proc_t *server;
+	proc_t *client;
+	cte_t cap_buf;
+} channels[S3K_CHAN_CNT];
 
-void set_client(uint64_t chan, proc_t *proc, ipc_mode_t mode)
+static err_t do_send(cap_t cap, const ipc_msg_t *msg, proc_t **next)
 {
-	// If no yielding mode, we have no timeout.
-	if (mode == IPC_NOYIELD)
-		proc->timeout = UINT64_MAX;
-	else
-		proc->timeout = timeout_get(csrr_mhartid());
-	proc->serv_time = 0;
-	clients[chan] = proc;
-	proc_ipc_wait(proc, chan);
+	bool is_server = (cap.sock.tag == 0);
+	cte_t cap_buf = channels[cap.sock.chan].cap_buf;
+
+	uint64_t curr_time = time_get();
+	uint64_t timeout = timeout_get(csrr(mhartid));
+
+	if (curr_time >= timeout) {
+		return ERR_PREEMPTED;
+	}
+
+	proc_t *recv;
+
+	if (is_server) {
+		recv = channels[cap.sock.chan].client;
+		if (!recv)
+			return ERR_NO_RECEIVER;
+		channels[cap.sock.chan].client = NULL;
+	} else {
+		recv = channels[cap.sock.chan].server;
+		if (!recv)
+			return ERR_NO_RECEIVER;
+		if (cap.sock.mode == IPC_YIELD
+		    && curr_time + recv->regs[REG_SERVTIME] >= timeout)
+			return ERR_NO_RECEIVER;
+		channels[cap.sock.chan].server = NULL;
+	}
+
+	if (proc_ipc_acquire(recv, cap.sock.chan)) {
+		recv->regs[REG_T0] = SUCCESS;
+		recv->regs[REG_A0] = cap.sock.tag;
+		recv->regs[REG_A2] = msg->data[0];
+		recv->regs[REG_A3] = msg->data[1];
+		recv->regs[REG_A4] = msg->data[2];
+		recv->regs[REG_A5] = msg->data[3];
+
+		if (msg->send_cap) {
+			recv->regs[REG_A1] = cte_cap(msg->cap_buf).raw;
+			cap_move(msg->cap_buf, cap_buf);
+		} else {
+			recv->regs[REG_A1] = 0;
+		}
+
+		if (cap.sock.mode == IPC_YIELD) {
+			recv->timeout = (*next)->timeout;
+			*next = recv;
+		} else if (cap.sock.mode == IPC_NOYIELD) {
+			recv->timeout = 0;
+			proc_release(recv);
+		} else {
+			KASSERT(0);
+		}
+		return SUCCESS;
+	}
+	return ERR_NO_RECEIVER;
 }
 
-void set_server(uint64_t chan, proc_t *proc, ipc_mode_t mode)
+static void do_recv(cap_t cap, cte_t cap_buf, proc_t *recv)
 {
-	if (mode == IPC_NOYIELD)
-		proc->serv_time = 0;
-	else
-		proc->serv_time = proc->regs[REG_SERVTIME];
-	proc->timeout = UINT64_MAX;
-	servers[chan] = proc;
-	proc_ipc_wait(proc, chan);
+	bool is_server = (cap.sock.tag == 0);
+	if (is_server) {
+		channels[cap.sock.chan].server = recv;
+		channels[cap.sock.chan].cap_buf = cap_buf;
+		recv->timeout = UINT64_MAX;
+		proc_ipc_wait(recv, cap.sock.chan);
+	} else {
+		channels[cap.sock.chan].client = recv;
+		channels[cap.sock.chan].cap_buf = cap_buf;
+		if (cap.sock.mode == IPC_NOYIELD)
+			recv->timeout = UINT64_MAX;
+		proc_ipc_wait(recv, cap.sock.chan);
+	}
 }
 
-err_t valid_sock(cap_t sock_cap, bool send_cap)
+static err_t reply(cte_t sock, cap_t cap, const ipc_msg_t *msg, proc_t **next)
 {
-	if (!sock_cap.type)
-		return ERR_EMPTY;
-	if (sock_cap.type != CAPTY_SOCKET)
+	if (msg->send_cap && !(cap.sock.perm & IPC_SCAP))
 		return ERR_INVALID_SOCKET;
+	return do_send(cap, msg, next);
+}
 
-	// If send_cap == true, then can_send_cap must be true.
-	bool is_server = (sock_cap.sock.tag == 0);
-	ipc_perm_t perm = sock_cap.sock.perm;
-	bool can_send_cap = (perm & (is_server ? IPC_SCAP : IPC_CCAP));
-	if (!can_send_cap && send_cap)
+static err_t send(cte_t sock, cap_t cap, const ipc_msg_t *msg, proc_t **next)
+{
+	if (msg->send_cap && !(cap.sock.perm & IPC_CCAP))
 		return ERR_INVALID_SOCKET;
-	return SUCCESS;
+	return do_send(cap, msg, next);
 }
 
-err_t do_sock_send(cap_t sock_cap, const ipc_msg_t *msg, proc_t **next)
+static err_t recv(cte_t sock, cap_t cap, cte_t cap_buf, proc_t **next)
 {
-	uint64_t chan = sock_cap.sock.chan;
-	uint64_t tag = sock_cap.sock.tag;
-	uint64_t mode = sock_cap.sock.mode;
-	uint64_t perm = sock_cap.sock.perm;
-	bool is_server = (tag == 0);
-
-	proc_t *recv = is_server ? clients[chan] : servers[chan];
-
-	bool send_data = (perm & (is_server ? IPC_SDATA : IPC_CDATA));
+	do_recv(cap, cap_buf, *next);
+	*next = NULL;
+	return ERR_TIMEOUT;
+}
 
-	if (!recv || !proc_ipc_acquire(recv, chan))
-		return ERR_NO_RECEIVER;
+static err_t replyrecv(cte_t sock, cap_t cap, const ipc_msg_t *msg,
+		       proc_t **next)
+{
+	cte_t cap_buf = msg->cap_buf;
+	proc_t *server = *next;
+	KASSERT(server->state == PSF_BUSY);
 
-	if (is_server)
-		clients[chan] = NULL;
-	else
-		servers[chan] = NULL;
+	// Can send capability?
+	if (msg->send_cap && !(cap.sock.perm & IPC_SCAP))
+		return ERR_INVALID_SOCKET;
 
-	recv->regs[REG_T0] = SUCCESS;
-	recv->regs[REG_A0] = tag;
-	recv->regs[REG_A1] = 0;
-	if (send_data) {
-		recv->regs[REG_A2] = msg->data[0];
-		recv->regs[REG_A3] = msg->data[0];
-		recv->regs[REG_A4] = msg->data[0];
-		recv->regs[REG_A5] = msg->data[0];
-	}
-	if (msg->send_cap) {
-		cap_move(msg->src_buf, recv->cap_buf,
-			 (cap_t *)&recv->regs[REG_A1]);
-	}
+	// Can receive capability?
+	if ((cap.sock.perm & IPC_CCAP) && !cte_is_empty(cap_buf)
+	    && !msg->send_cap)
+		return ERR_DST_OCCUPIED;
 
-	if (mode == IPC_YIELD) {
-		// Yield to receiver
-		*next = recv;
-		return YIELD;
-	} else {
-		// No yield, just success
-		proc_release(recv);
-		return SUCCESS;
+	err_t err = do_send(cap, msg, next);
+	if (err == ERR_PREEMPTED) {
+		*next = NULL;
+		return err;
+	} else if (err == ERR_NO_RECEIVER && msg->send_cap) {
+		cap_delete(msg->cap_buf);
 	}
+	do_recv(cap, cap_buf, server);
+	if (*next == server)
+		*next = NULL;
+	return ERR_TIMEOUT;
 }
 
-err_t do_sock_recv(proc_t *recv, cap_t sock_cap)
+static err_t call(cte_t sock, cap_t cap, const ipc_msg_t *msg, proc_t **next)
 {
-	chan_t chan = sock_cap.sock.chan;
-	ipc_mode_t mode = sock_cap.sock.mode;
-	ipc_perm_t perm = sock_cap.sock.perm;
-	uint32_t tag = sock_cap.sock.tag;
-	bool is_server = (tag == 0);
-
-	// If the other party can send a capability.
-	bool recv_cap = perm & (is_server ? IPC_CCAP : IPC_SCAP);
-
-	// if we can receive a capability, free the slot.
-	if (recv_cap)
-		cap_delete(recv->cap_buf);
-
-	if (is_server)
-		set_server(chan, recv, mode);
-	else
-		set_client(chan, recv, mode);
-	return YIELD;
-}
+	cte_t cap_buf = msg->cap_buf;
+	proc_t *client = *next;
 
-err_t cap_sock_send(cte_t sock, const ipc_msg_t *msg, proc_t **next)
-{
-	cap_t sock_cap = cte_cap(sock);
-	proc_t *proc = proc_get(cte_pid(sock));
+	// Can send capability?
+	if (msg->send_cap && !(cap.sock.perm & IPC_CCAP))
+		return ERR_INVALID_SOCKET;
+
+	// Can receive capability?
+	if ((cap.sock.perm & IPC_SCAP) && !cte_is_empty(cap_buf)
+	    && !msg->send_cap)
+		return ERR_DST_OCCUPIED;
 
-	// Check that we have a valid socket capability.
-	err_t err = valid_sock(sock_cap, msg->send_cap);
+	err_t err = do_send(cap, msg, next);
 	if (err)
 		return err;
-
-	// If suspend flag is set, suspend.
-	if (proc->state & PSF_SUSPENDED)
-		return ERR_SUSPENDED;
-	return do_sock_send(sock_cap, msg, next);
+	do_recv(cap, cap_buf, client);
+	if (*next == client)
+		*next = NULL;
+	return ERR_TIMEOUT;
 }
 
-err_t cap_sock_recv(cte_t sock)
+/* Entry points */
+err_t cap_sock_send(cte_t sock, const ipc_msg_t *msg, proc_t **next)
 {
-	cap_t sock_cap = cte_cap(sock);
-	proc_t *proc = proc_get(cte_pid(sock));
-
-	err_t err = valid_sock(sock_cap, false);
-	if (err)
-		return err;
-	if (sock_cap.sock.tag != 0)
+	cap_t cap = cte_cap(sock);
+	if (cap.type == CAPTY_NONE)
+		return ERR_EMPTY;
+	if (cap.type != CAPTY_SOCKET)
 		return ERR_INVALID_SOCKET;
+	if ((*next)->state & PSF_SUSPENDED) {
+		*next = NULL;
+		return ERR_PREEMPTED;
+	}
+	if (cap.sock.tag == 0) {
+		return reply(sock, cap, msg, next);
+	} else {
+		return send(sock, cap, msg, next);
+	}
+}
 
-	// If suspend flag is set, suspend.
-	if (proc->state & PSF_SUSPENDED)
-		return ERR_SUSPENDED;
-	return do_sock_recv(proc, sock_cap);
+err_t cap_sock_recv(cte_t sock, cte_t cap_buf, proc_t **next)
+{
+	cap_t cap = cte_cap(sock);
+	if (cap.type == CAPTY_NONE)
+		return ERR_EMPTY;
+	if (cap.type != CAPTY_SOCKET)
+		return ERR_INVALID_CAPABILITY;
+	if (cap.sock.tag != 0)
+		return ERR_INVALID_SOCKET;
+	if ((cap.sock.perm & IPC_CCAP) && !cte_is_empty(cap_buf))
+		return ERR_DST_OCCUPIED;
+	if ((*next)->state & PSF_SUSPENDED) {
+		*next = NULL;
+		return ERR_PREEMPTED;
+	}
+	return recv(sock, cap, cap_buf, next);
 }
 
 err_t cap_sock_sendrecv(cte_t sock, const ipc_msg_t *msg, proc_t **next)
 {
-	cap_t sock_cap = cte_cap(sock);
-	proc_t *proc = proc_get(cte_pid(sock));
-
-	err_t err = valid_sock(sock_cap, msg->send_cap);
-	if (err)
-		return err;
-
-	// If suspend flag is set, suspend.
-	if (proc->state & PSF_SUSPENDED)
-		return ERR_SUSPENDED;
-
-	// Try send capability
-	err = do_sock_send(sock_cap, msg, next);
-
-	bool is_server = (sock_cap.sock.tag == 0);
+	cap_t cap = cte_cap(sock);
+	if (cap.type == CAPTY_NONE)
+		return ERR_EMPTY;
+	if (cap.type != CAPTY_SOCKET)
+		return ERR_INVALID_CAPABILITY;
+	if ((*next)->state & PSF_SUSPENDED) {
+		*next = NULL;
+		return ERR_PREEMPTED;
+	}
+	KASSERT((*next)->state == PSF_BUSY);
 
-	if (!is_server && err == ERR_NO_RECEIVER) {
-		// Clients fail if we get an error, servers do not care.
-		return ERR_NO_RECEIVER;
-	} else if (is_server && msg->send_cap) {
-		// Servers scrap capabilities that were not sent.
-		cap_delete(proc->cap_buf);
+	if (cap.sock.tag == 0) {
+		return replyrecv(sock, cap, msg, next);
+	} else {
+		return call(sock, cap, msg, next);
 	}
-	return do_sock_recv(proc, sock_cap);
 }
 
 void cap_sock_clear(cap_t cap, proc_t *p)
 {
-	if (!cap.sock.tag) {
-		servers[cap.sock.chan] = NULL;
-	} else if (clients[cap.sock.chan] == p) {
-		clients[cap.sock.chan] = NULL;
+	if (cap.sock.tag == 0) {
+		channels[cap.sock.chan].server = NULL;
+		channels[cap.sock.chan].client = NULL;
+		channels[cap.sock.chan].cap_buf = NULL;
+	} else if (channels[cap.sock.chan].client == p) {
+		channels[cap.sock.chan].client = NULL;
 	}
 }
diff --git a/kernel/src/cap_lock.c b/kernel/src/cap_lock.c
new file mode 100644
index 00000000..0db9f290
--- /dev/null
+++ b/kernel/src/cap_lock.c
@@ -0,0 +1,29 @@
+#include "cap_lock.h"
+
+#include "csr.h"
+#include "mcslock.h"
+
+#ifdef SMP
+static mcslock_t lock;
+static qnode_t nodes[S3K_MAX_HART + 1];
+
+void cap_lock_init(void)
+{
+	mcslock_init(&lock);
+}
+
+bool cap_lock_acquire(void)
+{
+#ifndef NOPREEMPT
+	return mcslock_try_acquire(&lock, &nodes[csrr(mhartid)]);
+#else
+	mcslock_acquire(&lock, &nodes[csrr(mhartid)]);
+	return true;
+#endif
+}
+
+void cap_lock_release(void)
+{
+	mcslock_release(&lock, &nodes[csrr(mhartid)]);
+}
+#endif
diff --git a/kernel/src/cap_monitor.c b/kernel/src/cap_monitor.c
index 1a3a0720..17de2fc9 100644
--- a/kernel/src/cap_monitor.c
+++ b/kernel/src/cap_monitor.c
@@ -58,10 +58,12 @@ err_t cap_monitor_yield(cte_t mon, pid_t pid, proc_t **next)
 {
 	err_t err = check_monitor(mon, pid, false);
 	if (!err) {
+		proc_t *monitor = *next;
 		proc_t *proc = proc_get(pid);
 		if (proc_acquire(proc)) {
 			*next = proc;
-			return YIELD;
+			proc_release(monitor);
+			return SUCCESS;
 		}
 		return ERR_INVALID_STATE;
 	}
@@ -95,9 +97,8 @@ err_t cap_monitor_cap_read(cte_t mon, cte_t src, cap_t *cap)
 err_t cap_monitor_cap_move(cte_t mon, cte_t src, cte_t dst)
 {
 	err_t err = check_monitor_move(mon, src, dst);
-	cap_t cap;
 	if (!err)
-		err = cap_move(src, dst, &cap);
+		err = cap_move(src, dst);
 	return err;
 }
 
diff --git a/kernel/src/cap_ops.c b/kernel/src/cap_ops.c
index 9803a28e..a5aa0955 100644
--- a/kernel/src/cap_ops.c
+++ b/kernel/src/cap_ops.c
@@ -2,213 +2,425 @@
 
 #include "cap_ipc.h"
 #include "cap_util.h"
-#include "pmp.h"
+#include "kernel.h"
 #include "sched.h"
 
+typedef err_t (*ipc_move_handler)(cte_t, cap_t, cte_t);
+typedef err_t (*delete_handler)(cte_t, cap_t);
+typedef err_t (*revoke_handler)(cte_t, cap_t);
+typedef err_t (*derive_handler)(cte_t, cap_t, cte_t, cap_t);
+
+static err_t cap_delete_time(cte_t src, cap_t cap);
+static err_t cap_delete_memory(cte_t src, cap_t cap);
+static err_t cap_delete_pmp(cte_t src, cap_t cap);
+static err_t cap_delete_monitor(cte_t src, cap_t cap);
+static err_t cap_delete_channel(cte_t src, cap_t cap);
+static err_t cap_delete_socket(cte_t src, cap_t cap);
+
+static err_t cap_revoke_time(cte_t src, cap_t cap);
+static err_t cap_revoke_memory(cte_t src, cap_t cap);
+static err_t cap_revoke_pmp(cte_t src, cap_t cap);
+static err_t cap_revoke_monitor(cte_t src, cap_t cap);
+static err_t cap_revoke_channel(cte_t src, cap_t cap);
+static err_t cap_revoke_socket(cte_t src, cap_t cap);
+
+static err_t cap_derive_time(cte_t src, cap_t cap, cte_t dst, cap_t new_cap);
+static err_t cap_derive_memory(cte_t src, cap_t cap, cte_t dst, cap_t new_cap);
+static err_t cap_derive_pmp(cte_t src, cap_t cap, cte_t dst, cap_t new_cap);
+static err_t cap_derive_monitor(cte_t src, cap_t cap, cte_t dst, cap_t new_cap);
+static err_t cap_derive_channel(cte_t src, cap_t cap, cte_t dst, cap_t new_cap);
+static err_t cap_derive_socket(cte_t src, cap_t cap, cte_t dst, cap_t new_cap);
+
+static const delete_handler delete_handlers[CAPTY_COUNT] = {
+    NULL,
+    cap_delete_time,
+    cap_delete_memory,
+    cap_delete_pmp,
+    cap_delete_monitor,
+    cap_delete_channel,
+    cap_delete_socket,
+};
+static const revoke_handler revoke_handlers[CAPTY_COUNT] = {
+    NULL,
+    cap_revoke_time,
+    cap_revoke_memory,
+    cap_revoke_pmp,
+    cap_revoke_monitor,
+    cap_revoke_channel,
+    cap_revoke_socket,
+};
+static const derive_handler derive_handlers[CAPTY_COUNT] = {
+    NULL,
+    cap_derive_time,
+    cap_derive_memory,
+    cap_derive_pmp,
+    cap_derive_monitor,
+    cap_derive_channel,
+    cap_derive_socket,
+};
+
 err_t cap_read(cte_t c, cap_t *cap)
 {
 	*cap = cte_cap(c);
 	return cap->raw ? SUCCESS : ERR_EMPTY;
 }
 
-static void ipc_move_hook(cte_t src, cte_t dst)
+static void cap_ipc_move_hook(cte_t src, cte_t dst)
 {
 	cap_t cap = cte_cap(src);
 	switch (cap.type) {
 	case CAPTY_TIME: {
-		uint64_t pid = cte_pid(dst);
-		uint64_t end = cap.time.end;
-		uint64_t hartid = cap.time.hart;
-		uint64_t from = cap.time.mrk;
-		uint64_t to = cap.time.end;
-		sched_update(pid, end, hartid, from, to);
+		sched_update(cte_pid(dst), cap.time.end, cap.time.hart,
+			     cap.time.mrk, cap.time.end);
 	} break;
-	case CAPTY_PMP:
+	case CAPTY_PMP: {
 		if (cap.pmp.used) {
 			proc_pmp_unload(proc_get(cte_pid(src)), cap.pmp.slot);
 			cap.pmp.used = 0;
 			cap.pmp.slot = 0;
 			cte_set_cap(src, cap);
 		}
-		break;
-	case CAPTY_SOCKET:
-		cap_sock_clear(cap, proc_get(cte_pid(src)));
-		break;
+	} break;
 	default:
 		break;
 	}
 }
 
-err_t cap_move(cte_t src, cte_t dst, cap_t *cap)
+err_t cap_move(cte_t src, cte_t dst)
 {
-	if (!cte_cap(src).type)
+	if (cte_is_empty(src))
 		return ERR_SRC_EMPTY;
-
-	if (cte_cap(dst).type)
+	if (!cte_is_empty(dst))
 		return ERR_DST_OCCUPIED;
-
 	if (cte_pid(src) != cte_pid(dst))
-		ipc_move_hook(src, dst);
-	cte_move(src, dst, cap);
+		cap_ipc_move_hook(src, dst);
+	cte_move(src, dst);
 	return SUCCESS;
 }
 
-static void delete_hook(cte_t c, cap_t cap)
+err_t cap_delete(cte_t c)
 {
-	// Clean-up resources
-	switch (cap.type) {
-	case CAPTY_TIME: {
-		uint64_t hartid = cap.time.hart;
-		uint64_t from = cap.time.mrk;
-		uint64_t end = cap.time.end;
-		sched_delete(hartid, from, end);
-	} break;
-	case CAPTY_PMP:
-		if (cap.pmp.used)
-			proc_pmp_unload(proc_get(cte_pid(c)), cap.pmp.slot);
-		break;
-	case CAPTY_SOCKET:
-		cap_sock_clear(cap, proc_get(cte_pid(c)));
-		break;
-	default:
-		break;
-	}
+	if (cte_is_empty(c))
+		return ERR_EMPTY;
+	cap_t cap = cte_cap(c);
+	return delete_handlers[cap.type](c, cap);
 }
 
-err_t cap_delete(cte_t c)
+err_t cap_revoke(cte_t parent)
 {
-	if (!cte_cap(c).type)
+	cap_t pcap = cte_cap(parent);
+	if (pcap.type == CAPTY_NONE)
 		return ERR_EMPTY;
-	delete_hook(c, cte_delete(c));
-	return SUCCESS;
+	int err;
+	do {
+		err = revoke_handlers[pcap.type](parent, cte_cap(parent));
+	} while (err < 0 && !kernel_preempt());
+	return err < 0 ? ERR_PREEMPTED : SUCCESS;
 }
 
-void cap_reclaim(cte_t p, cap_t pcap, cte_t c, cap_t ccap)
+err_t cap_derive(cte_t src, cte_t dst, cap_t ncap)
 {
-	if ((cte_prev(c) != p) || cte_cap(c).raw != ccap.raw)
-		return;
+	if (cte_is_empty(src))
+		return ERR_SRC_EMPTY;
+
+	if (!cte_is_empty(dst))
+		return ERR_DST_OCCUPIED;
+
+	cap_t scap = cte_cap(src);
+	return derive_handlers[scap.type](src, scap, dst, ncap);
+}
 
+/********** HANDLERS ***********/
+
+err_t cap_delete_time(cte_t c, cap_t cap)
+{
+	sched_delete(cap.time.hart, cap.time.mrk, cap.time.end);
 	cte_delete(c);
+	return SUCCESS;
+}
 
-	switch (ccap.type) {
-	case CAPTY_TIME: {
-		pcap.time.mrk = ccap.time.mrk;
-		uint64_t pid = cte_pid(p);
+err_t cap_revoke_time(cte_t parent, cap_t pcap)
+{
+	cte_t child = cte_next(parent);
+	cap_t ccap = cte_cap(child);
+	if (ccap.type == CAPTY_TIME && pcap.time.hart == ccap.time.hart
+	    && pcap.time.bgn <= ccap.time.bgn
+	    && ccap.time.end <= pcap.time.end) {
+		// delete the child
+		cte_delete(child);
+
+		// Update schedule.
+		uint64_t pid = cte_pid(parent);
 		uint64_t end = pcap.time.end;
 		uint64_t hartid = pcap.time.hart;
-		uint64_t from = pcap.time.mrk;
-		uint64_t to = pcap.time.end;
+		uint64_t from = ccap.time.mrk;
+		uint64_t to = pcap.time.mrk;
 		sched_update(pid, end, hartid, from, to);
-	} break;
-	case CAPTY_MEMORY:
+
+		// Update parent.
+		pcap.time.mrk = ccap.time.mrk;
+		cte_set_cap(parent, pcap);
+		return pcap.time.mrk == pcap.time.bgn ? SUCCESS : -1;
+	}
+
+	// Update schedule.
+	uint64_t pid = cte_pid(parent);
+	uint64_t end = pcap.time.end;
+	uint64_t hartid = pcap.time.hart;
+	uint64_t from = pcap.time.bgn;
+	uint64_t to = pcap.time.mrk;
+	sched_update(pid, end, hartid, from, to);
+
+	// Update parent.
+	pcap.time.mrk = pcap.time.bgn;
+	cte_set_cap(parent, pcap);
+	return SUCCESS;
+}
+
+err_t cap_derive_time(cte_t src, cap_t cap, cte_t dst, cap_t new_cap)
+{
+	if (new_cap.type == CAPTY_TIME && new_cap.time.hart == cap.time.hart
+	    && new_cap.time.bgn == cap.time.mrk
+	    && new_cap.time.end <= cap.time.end) {
+		sched_update(cte_pid(dst), new_cap.time.end, new_cap.time.hart,
+			     new_cap.time.bgn, new_cap.time.end);
+		cap.time.mrk = new_cap.time.end;
+		cte_set_cap(src, cap);
+		cte_insert(dst, new_cap, src);
+		return SUCCESS;
+	}
+	return ERR_INVALID_DERIVATION;
+}
+
+err_t cap_delete_memory(cte_t c, cap_t cap)
+{
+	cte_delete(c);
+	return SUCCESS;
+}
+
+err_t cap_revoke_memory(cte_t parent, cap_t pcap)
+{
+	cte_t child = cte_next(parent);
+	cap_t ccap = cte_cap(child);
+	if (ccap.type == CAPTY_MEMORY && pcap.mem.tag == ccap.mem.tag
+	    && pcap.mem.bgn <= ccap.mem.bgn) {
+		// delete the child
+		cte_delete(child);
+
+		// Update parent.
 		pcap.mem.mrk = ccap.mem.mrk;
 		pcap.mem.lck = ccap.mem.lck;
-		break;
-	case CAPTY_PMP:
+		cte_set_cap(parent, pcap);
+
+		return (pcap.mem.mrk == pcap.mem.bgn && !pcap.mem.lck) ?
+			   SUCCESS :
+			   -1;
+	}
+
+	uint64_t base, size;
+	pmp_napot_decode(ccap.pmp.addr, &base, &size);
+
+	if (ccap.type == CAPTY_PMP
+	    && tag_block_to_addr(pcap.mem.tag, pcap.mem.bgn) <= base) {
+		// delete the child
+		cte_delete(child);
+
+		// Clear PMP config
 		if (ccap.pmp.used) {
-			proc_pmp_unload(proc_get(cte_pid(c)), ccap.pmp.slot);
+			proc_pmp_unload(proc_get(cte_pid(child)),
+					ccap.pmp.slot);
 		}
-		return;
-	case CAPTY_MONITOR:
-		pcap.mon.mrk = ccap.mon.mrk;
-		break;
-	case CAPTY_CHANNEL:
-		pcap.chan.mrk = ccap.chan.mrk;
-		break;
-	case CAPTY_SOCKET:
-		cap_sock_clear(ccap, proc_get(cte_pid(c)));
-		return;
-	default:
-		KASSERT(0);
+
+		return -1;
 	}
 
-	cte_set_cap(p, pcap);
+	pcap.mem.mrk = pcap.mem.bgn;
+	pcap.mem.lck = 0;
+	cte_set_cap(parent, pcap);
 
-	return;
+	return SUCCESS;
 }
 
-err_t cap_reset(cte_t c)
+err_t cap_derive_memory(cte_t src, cap_t cap, cte_t dst, cap_t new_cap)
 {
-	if (!cte_cap(c).type)
-		return ERR_EMPTY;
+	if (new_cap.type == CAPTY_MEMORY && cap.mem.tag == new_cap.mem.tag
+	    && cap.mem.tag == new_cap.mem.tag && cap.mem.mrk <= new_cap.mem.bgn
+	    && new_cap.mem.end <= cap.mem.end
+	    && (new_cap.mem.rwx & cap.mem.rwx) == new_cap.mem.rwx
+	    && !cap.mem.lck) {
+		cap.mem.mrk = new_cap.mem.end;
+		cte_set_cap(src, cap);
+		cte_insert(dst, new_cap, src);
+		return SUCCESS;
+	}
 
-	cap_t cap = cte_cap(c);
-	switch (cap.type) {
-	case CAPTY_TIME: {
-		uint64_t pid = cte_pid(c);
-		uint64_t end = cap.time.end;
-		uint64_t hartid = cap.time.hart;
-		uint64_t from = cap.time.bgn;
-		uint64_t to = cap.time.mrk;
-		sched_update(pid, end, hartid, from, to);
-		cap.time.mrk = cap.time.bgn;
-	} break;
-	case CAPTY_MEMORY:
-		cap.mem.mrk = cap.mem.bgn;
-		cap.mem.lck = false;
-		break;
-	case CAPTY_MONITOR:
-		cap.mon.mrk = cap.mon.bgn;
-		break;
-	case CAPTY_CHANNEL:
-		cap.chan.mrk = cap.chan.bgn;
-		break;
-	default:
+	uint64_t pmp_begin, pmp_end;
+	uint64_t mem_mrk, mem_end;
+	pmp_napot_decode(new_cap.pmp.addr, &pmp_begin, &pmp_end);
+	pmp_end += pmp_begin;
+	mem_mrk = tag_block_to_addr(cap.mem.tag, cap.mem.mrk);
+	mem_end = tag_block_to_addr(cap.mem.tag, cap.mem.end);
+
+	if (new_cap.type == CAPTY_PMP && mem_mrk <= pmp_begin
+	    && pmp_end <= mem_end
+	    && (new_cap.pmp.rwx & cap.mem.rwx) == new_cap.pmp.rwx) {
+		cap.mem.lck = true;
+		cte_set_cap(src, cap);
+		cte_insert(dst, new_cap, src);
 		return SUCCESS;
 	}
+	return ERR_INVALID_DERIVATION;
+}
 
-	cte_set_cap(c, cap);
+err_t cap_delete_pmp(cte_t c, cap_t cap)
+{
+	proc_t *proc = proc_get(cte_pid(c));
+	if (cap.pmp.used)
+		proc_pmp_unload(proc, cap.pmp.slot);
+	cte_delete(c);
+	return SUCCESS;
+}
 
+err_t cap_revoke_pmp(cte_t parent, cap_t pcap)
+{
 	return SUCCESS;
 }
 
-static void derive(cte_t src, cap_t scap, cte_t dst, cap_t ncap)
+err_t cap_derive_pmp(cte_t src, cap_t cap, cte_t dst, cap_t new_cap)
 {
-	// Update the original capability
-	switch (ncap.type) {
-	case CAPTY_TIME: {
-		uint64_t pid = cte_pid(dst);
-		uint64_t end = ncap.time.end;
-		uint64_t hartid = ncap.time.hart;
-		uint64_t from = ncap.time.mrk;
-		uint64_t to = ncap.time.end;
-		sched_update(pid, end, hartid, from, to);
-		scap.time.mrk = ncap.time.end;
-	} break;
-	case CAPTY_MEMORY:
-		scap.mem.mrk = ncap.mem.end;
-		break;
-	case CAPTY_PMP:
-		scap.mem.lck = true;
-		break;
-	case CAPTY_MONITOR:
-		scap.mon.mrk = ncap.mon.end;
-		break;
-	case CAPTY_CHANNEL:
-		scap.chan.mrk = ncap.chan.end;
-		break;
-	case CAPTY_SOCKET:
-		if (ncap.sock.tag == 0)
-			scap.chan.mrk = ncap.sock.chan + 1;
-		break;
-	case CAPTY_NONE:
-		KASSERT(0);
+	return ERR_INVALID_DERIVATION;
+}
+
+err_t cap_delete_monitor(cte_t c, cap_t cap)
+{
+	cte_delete(c);
+	return SUCCESS;
+}
+
+err_t cap_revoke_monitor(cte_t parent, cap_t pcap)
+{
+	cte_t child = cte_next(parent);
+	cap_t ccap = cte_cap(child);
+	if (ccap.type == CAPTY_MONITOR && pcap.mon.bgn <= ccap.mon.bgn) {
+		// delete the child
+		cte_delete(child);
+
+		// Update parent.
+		pcap.mon.mrk = ccap.mon.mrk;
+		cte_set_cap(parent, pcap);
+
+		return (pcap.mon.mrk == pcap.mon.bgn) ? SUCCESS : -1;
 	}
-	cte_insert(dst, ncap, src);
-	cte_set_cap(src, scap);
+
+	pcap.mon.mrk = pcap.mon.bgn;
+	cte_set_cap(parent, pcap);
+
+	return SUCCESS;
 }
 
-err_t cap_derive(cte_t src, cte_t dst, cap_t ncap)
+err_t cap_derive_monitor(cte_t src, cap_t cap, cte_t dst, cap_t new_cap)
 {
-	if (!cte_cap(src).type)
-		return ERR_SRC_EMPTY;
+	if (new_cap.type == CAPTY_MONITOR && cap.mon.mrk <= new_cap.mon.bgn
+	    && new_cap.mon.end <= cap.mon.end) {
+		cap.mon.mrk = new_cap.mon.end;
+		cte_set_cap(src, cap);
+		cte_insert(dst, new_cap, src);
+		return SUCCESS;
+	}
+	return ERR_INVALID_DERIVATION;
+}
 
-	if (cte_cap(dst).type)
-		return ERR_DST_OCCUPIED;
+err_t cap_delete_channel(cte_t c, cap_t cap)
+{
+	cte_delete(c);
+	return SUCCESS;
+}
+
+err_t cap_revoke_channel(cte_t parent, cap_t pcap)
+{
+	cte_t child = cte_next(parent);
+	cap_t ccap = cte_cap(child);
+	if (ccap.type == CAPTY_CHANNEL && pcap.chan.bgn <= ccap.chan.bgn) {
+		// delete the child
+		cte_delete(child);
+
+		// Update parent.
+		pcap.chan.mrk = ccap.chan.mrk;
+		cte_set_cap(parent, pcap);
+
+		return (pcap.chan.mrk == pcap.chan.bgn) ? SUCCESS : -1;
+	}
+
+	if (ccap.type == CAPTY_SOCKET && pcap.chan.bgn <= ccap.sock.chan) {
+		// delete the child
+		cte_delete(child);
+
+		// Clear socket
+		cap_sock_clear(ccap, proc_get(cte_pid(child)));
+
+		return -1;
+	}
+
+	pcap.chan.mrk = pcap.chan.bgn;
+	cte_set_cap(parent, pcap);
+
+	return SUCCESS;
+}
+
+err_t cap_derive_channel(cte_t src, cap_t cap, cte_t dst, cap_t new_cap)
+{
+	if (new_cap.type == CAPTY_CHANNEL && cap.chan.mrk <= new_cap.chan.bgn
+	    && new_cap.chan.end <= cap.chan.end) {
+		cap.chan.mrk = new_cap.chan.end;
+		cte_set_cap(src, cap);
+		cte_insert(dst, new_cap, src);
+		return SUCCESS;
+	}
+
+	if (new_cap.type == CAPTY_SOCKET && cap.chan.mrk <= new_cap.sock.chan
+	    && new_cap.sock.chan < cap.chan.end) {
+		cap.chan.mrk = new_cap.sock.chan + 1;
+		cte_set_cap(src, cap);
+		cte_insert(dst, new_cap, src);
+		return SUCCESS;
+	}
+	return ERR_INVALID_DERIVATION;
+}
+
+err_t cap_delete_socket(cte_t c, cap_t cap)
+{
+	proc_t *proc = proc_get(cte_pid(c));
+	cap_sock_clear(cap, proc);
+	cte_delete(c);
+	return SUCCESS;
+}
+
+err_t cap_revoke_socket(cte_t parent, cap_t pcap)
+{
+	cte_t child = cte_next(parent);
+	cap_t ccap = cte_cap(child);
+	if (ccap.type == CAPTY_SOCKET && pcap.sock.chan == ccap.sock.chan
+	    && pcap.sock.tag == 0) {
+		// delete the child
+		cte_delete(child);
+
+		// Clear socket
+		cap_sock_clear(ccap, proc_get(cte_pid(child)));
+
+		return -1;
+	}
 
-	cap_t scap = cte_cap(src);
-	if (!cap_is_derivable(scap, ncap))
-		return ERR_INVALID_DERIVATION;
-	derive(src, scap, dst, ncap);
 	return SUCCESS;
 }
+
+err_t cap_derive_socket(cte_t src, cap_t cap, cte_t dst, cap_t new_cap)
+{
+	if (new_cap.type == CAPTY_SOCKET && new_cap.sock.chan == cap.sock.chan
+	    && new_cap.sock.perm == cap.sock.perm
+	    && new_cap.sock.mode == cap.sock.mode && cap.sock.tag == 0
+	    && new_cap.sock.tag != 0) {
+		cte_insert(dst, new_cap, src);
+		return SUCCESS;
+	}
+	return ERR_INVALID_DERIVATION;
+}
diff --git a/kernel/src/cap_pmp.c b/kernel/src/cap_pmp.c
index 001c305d..3b1d07aa 100644
--- a/kernel/src/cap_pmp.c
+++ b/kernel/src/cap_pmp.c
@@ -2,38 +2,38 @@
 #include "cap_types.h"
 #include "error.h"
 #include "kernel.h"
-#include "pmp.h"
 
 err_t cap_pmp_load(cte_t pmp, pmp_slot_t slot)
 {
 	proc_t *proc = proc_get(cte_pid(pmp));
-	cap_t pmp_cap = cte_cap(pmp);
-
-	if (!pmp_cap.type)
+	cap_t cap = cte_cap(pmp);
+	if (cap.type == CAPTY_NONE)
 		return ERR_EMPTY;
-	if (pmp_cap.type != CAPTY_PMP || pmp_cap.pmp.used)
+	if (cap.type != CAPTY_PMP || cap.pmp.used)
 		return ERR_INVALID_PMP;
 	if (!proc_pmp_avail(proc, slot))
 		return ERR_DST_OCCUPIED;
-	proc_pmp_load(proc, slot, pmp_cap.pmp.rwx, pmp_cap.pmp.addr);
-	pmp_cap.pmp.slot = slot;
-	pmp_cap.pmp.used = 1;
-	cte_set_cap(pmp, pmp_cap);
+
+	proc_pmp_load(proc, slot, cap.pmp.rwx, cap.pmp.addr);
+	cap.pmp.slot = slot;
+	cap.pmp.used = 1;
+	cte_set_cap(pmp, cap);
 	return SUCCESS;
 }
 
 err_t cap_pmp_unload(cte_t pmp)
 {
 	proc_t *proc = proc_get(cte_pid(pmp));
-	cap_t pmp_cap = cte_cap(pmp);
+	cap_t cap = cte_cap(pmp);
 
-	if (!pmp_cap.type)
+	if (cap.type == CAPTY_NONE)
 		return ERR_EMPTY;
-	if (pmp_cap.type != CAPTY_PMP || !pmp_cap.pmp.used)
+	if (cap.type != CAPTY_PMP || !cap.pmp.used)
 		return ERR_INVALID_PMP;
-	proc_pmp_unload(proc, pmp_cap.pmp.slot);
-	pmp_cap.pmp.slot = 0;
-	pmp_cap.pmp.used = 0;
-	cte_set_cap(pmp, pmp_cap);
+
+	proc_pmp_unload(proc, cap.pmp.slot);
+	cap.pmp.slot = 0;
+	cap.pmp.used = 0;
+	cte_set_cap(pmp, cap);
 	return SUCCESS;
 }
diff --git a/kernel/src/cap_table.c b/kernel/src/cap_table.c
index 4147a023..8662a4b8 100644
--- a/kernel/src/cap_table.c
+++ b/kernel/src/cap_table.c
@@ -21,8 +21,16 @@ void ctable_init(void)
 {
 	const cap_t init_caps[] = INIT_CAPS;
 	cte_t prev = ctable;
-	for (unsigned int i = 0; i < ARRAY_SIZE(init_caps); ++i)
+	kprintf(0, "# Initial capabilities:\n");
+	for (unsigned int i = 0; i < ARRAY_SIZE(init_caps); ++i) {
+		if (init_caps[i].type == CAPTY_NONE)
+			continue;
 		cte_insert(&ctable[i], init_caps[i], prev);
+
+		char buf[128];
+		cap_snprint(buf, 128, init_caps[i]);
+		kprintf(0, "#\t%d: %s\n", i, buf);
+	}
 }
 
 cte_t ctable_get(uint64_t pid, uint64_t index)
@@ -72,17 +80,16 @@ uint64_t cte_pid(cte_t c)
 	return offset(c) / S3K_CAP_CNT;
 }
 
-void cte_move(cte_t src, cte_t dst, cap_t *cap)
+void cte_move(cte_t src, cte_t dst)
 {
-	*cap = src->cap;
 	if (src == dst)
 		return;
+	cte_set_cap(dst, cte_cap(src));
 	cte_set_cap(src, (cap_t){0});
 	cte_set_prev(dst, cte_prev(src));
 	cte_set_next(dst, cte_next(src));
 	cte_prev(dst)->next = offset(dst);
 	cte_next(dst)->prev = offset(dst);
-	cte_set_cap(dst, *cap);
 }
 
 cap_t cte_delete(cte_t c)
diff --git a/kernel/src/cap_util.c b/kernel/src/cap_util.c
index 1e952cc6..dcaf0158 100644
--- a/kernel/src/cap_util.c
+++ b/kernel/src/cap_util.c
@@ -1,9 +1,16 @@
 #include "cap_util.h"
 
-#include "pmp.h"
+#include "altc/altio.h"
+#include "kassert.h"
 
 cap_t cap_mk_time(hart_t hart, time_slot_t bgn, time_slot_t end)
 {
+	KASSERT(bgn < end);
+#if S3K_MIN_HART > 0
+	KASSERT(hart >= S3K_MIN_HART);
+#endif
+	KASSERT(hart <= S3K_MAX_HART);
+	KASSERT(end <= S3K_SLOT_CNT);
 	cap_t cap;
 	cap.type = CAPTY_TIME;
 	cap.time.hart = hart;
@@ -16,12 +23,14 @@ cap_t cap_mk_time(hart_t hart, time_slot_t bgn, time_slot_t end)
 cap_t cap_mk_memory(addr_t bgn, addr_t end, rwx_t rwx)
 {
 	uint64_t tag = bgn >> MAX_BLOCK_SIZE;
+	KASSERT(bgn < end);
+	KASSERT(end <= (tag + 1) << MAX_BLOCK_SIZE);
 	cap_t cap;
 	cap.mem.type = CAPTY_MEMORY;
 	cap.mem.tag = tag;
 	cap.mem.bgn = (bgn - (tag << MAX_BLOCK_SIZE)) >> MIN_BLOCK_SIZE;
 	cap.mem.end = (end - (tag << MAX_BLOCK_SIZE)) >> MIN_BLOCK_SIZE;
-	cap.mem.mrk = bgn;
+	cap.mem.mrk = cap.mem.bgn;
 	cap.mem.rwx = rwx;
 	cap.mem.lck = false;
 	return cap;
@@ -40,6 +49,8 @@ cap_t cap_mk_pmp(napot_t addr, rwx_t rwx)
 
 cap_t cap_mk_monitor(pid_t bgn, pid_t end)
 {
+	KASSERT(bgn < end);
+	KASSERT(end <= S3K_PROC_CNT);
 	cap_t cap;
 	cap.mon.type = CAPTY_MONITOR;
 	cap.mon.bgn = bgn;
@@ -50,6 +61,8 @@ cap_t cap_mk_monitor(pid_t bgn, pid_t end)
 
 cap_t cap_mk_channel(chan_t bgn, chan_t end)
 {
+	KASSERT(bgn < end);
+	KASSERT(end <= S3K_CHAN_CNT);
 	cap_t cap;
 	cap.chan.type = CAPTY_CHANNEL;
 	cap.chan.bgn = bgn;
@@ -69,172 +82,70 @@ cap_t cap_mk_socket(chan_t chan, ipc_mode_t mode, ipc_perm_t perm, uint32_t tag)
 	return cap;
 }
 
-static inline bool is_range_subset(uint64_t a_bgn, uint64_t a_end,
-				   uint64_t b_bgn, uint64_t b_end)
+void cap_snprint(char *restrict buf, size_t size, cap_t cap)
 {
-	return a_bgn <= b_bgn && b_end <= a_end;
-}
-
-static inline bool is_range_prefix(uint64_t a_bgn, uint64_t a_end,
-				   uint64_t b_bgn, uint64_t b_end)
-{
-	return a_bgn == b_bgn && b_end <= a_end;
-}
-
-static inline bool is_bit_subset(uint64_t a, uint64_t b)
-{
-	return (a & b) == a;
-}
-
-static inline addr_t tag_block_to_addr(tag_t tag, block_t block)
-{
-	return ((uint64_t)tag << MAX_BLOCK_SIZE)
-	       + ((uint64_t)block << MIN_BLOCK_SIZE);
-}
-
-static bool cap_time_revokable(cap_t p, cap_t c)
-{
-	return (c.type == CAPTY_TIME) && (p.time.hart == c.time.hart)
-	       && is_range_subset(p.time.bgn, p.time.end, c.time.bgn,
-				  c.time.end);
-}
-
-static bool cap_mem_revokable(cap_t p, cap_t c)
-{
-	if (c.type == CAPTY_PMP) {
-		uint64_t p_bgn, p_end, c_base, c_size;
-		p_bgn = tag_block_to_addr(p.mem.tag, p.mem.bgn);
-		p_end = tag_block_to_addr(p.mem.tag, p.mem.end);
-		pmp_napot_decode(c.pmp.addr, &c_base, &c_size);
-		return is_range_subset(p_bgn, p_end, c_base, c_base + c_size);
-	}
-	return (c.type == CAPTY_MEMORY) && (p.mem.tag == c.mem.tag)
-	       && is_range_subset(p.mem.bgn, p.mem.end, c.mem.bgn, c.mem.end);
-}
-
-static bool cap_mon_revokable(cap_t p, cap_t c)
-{
-	return (c.type == CAPTY_MONITOR)
-	       && is_range_subset(p.mon.bgn, p.mon.end, c.mon.bgn, c.mon.end);
-}
-
-static bool cap_chan_revokable(cap_t p, cap_t c)
-{
-	if (c.type == CAPTY_SOCKET) {
-		return is_range_subset(p.chan.bgn, p.chan.end, c.sock.chan,
-				       c.sock.chan + 1);
-	}
-	return (c.type == CAPTY_CHANNEL)
-	       && is_range_subset(p.chan.bgn, p.chan.end, c.chan.bgn,
-				  c.chan.end);
-}
-
-static bool cap_sock_revokable(cap_t p, cap_t c)
-{
-	return (p.sock.tag == 0) && (c.sock.tag != 0)
-	       && (p.sock.chan == c.sock.chan);
-}
-
-bool cap_is_revokable(cap_t p, cap_t c)
-{
-	switch (p.type) {
+	switch (cap.type) {
+	case CAPTY_NONE:
+		alt_snprintf(buf, size, "NONE{}");
+		break;
 	case CAPTY_TIME:
-		return cap_time_revokable(p, c);
-	case CAPTY_MEMORY:
-		return cap_mem_revokable(p, c);
+		alt_snprintf(buf, size, "TIME{hart=%d,bgn=%d,end=%d,mrk=%d}",
+			     cap.time.hart, cap.time.bgn, cap.time.end,
+			     cap.time.mrk);
+		break;
+	case CAPTY_MEMORY: {
+		uint64_t bgn = tag_block_to_addr(cap.mem.tag, cap.mem.bgn);
+		uint64_t end = tag_block_to_addr(cap.mem.tag, cap.mem.end);
+		uint64_t mrk = tag_block_to_addr(cap.mem.tag, cap.mem.mrk);
+		alt_snprintf(buf, size,
+			     "MEMORY{bgn=0x%X,end=0x%X,mrk=0x%X,rwx=%d,lck=%x}",
+			     bgn, end, mrk, cap.mem.rwx, cap.mem.lck);
+	} break;
+	case CAPTY_PMP: {
+		uint64_t base, _size;
+		pmp_napot_decode(cap.pmp.addr, &base, &_size);
+		alt_snprintf(buf, size,
+			     "PMP{bgn=0x%X,end=0x%X,rwx=%d,used=%d,slot=%d}",
+			     base, base + _size, cap.pmp.rwx, cap.pmp.used,
+			     cap.pmp.slot);
+	} break;
 	case CAPTY_MONITOR:
-		return cap_mon_revokable(p, c);
+		alt_snprintf(buf, size, "MONITOR{bgn=%d,end=%d,mrk=%d}",
+			     cap.mon.bgn, cap.mon.end, cap.mon.mrk);
+		break;
 	case CAPTY_CHANNEL:
-		return cap_chan_revokable(p, c);
+		alt_snprintf(buf, size, "CHANNEL{bgn=%d,end=%d,mrk=%d}",
+			     cap.chan.bgn, cap.chan.end, cap.chan.mrk);
+		break;
 	case CAPTY_SOCKET:
-		return cap_sock_revokable(p, c);
+		alt_snprintf(buf, size,
+			     "SOCKET{chan=%d,tag=%d,perm=%d,mode=%d}",
+			     cap.sock.chan, cap.sock.tag, cap.sock.perm,
+			     cap.sock.mode);
+		break;
 	default:
-		return false;
+		alt_snprintf(buf, size, "UNKNOWN{raw=0x%X}", cap.raw);
 	}
 }
 
-bool cap_is_valid(cap_t c)
+bool cap_is_valid(const cap_t cap)
 {
-	switch (c.type) {
+	switch (cap.type) {
 	case CAPTY_TIME:
-		return (c.time.bgn == c.time.mrk) && (c.time.bgn < c.time.end);
+		return cap.time.bgn < cap.time.end
+		       && cap.time.bgn == cap.time.mrk;
 	case CAPTY_MEMORY:
-		return (c.mem.bgn == c.mem.mrk) && (c.mem.bgn < c.mem.end);
+		return cap.mem.lck == 0 && cap.mem.bgn < cap.mem.end
+		       && cap.mem.mrk == cap.mem.bgn;
 	case CAPTY_PMP:
-		return (c.pmp.used == 0) && (c.pmp.slot == 0);
-	case CAPTY_MONITOR:
-		return (c.mon.bgn == c.mon.mrk) && (c.mon.bgn < c.mon.end);
-	case CAPTY_CHANNEL:
-		return (c.chan.bgn == c.chan.mrk) && (c.chan.bgn < c.chan.end);
-	case CAPTY_SOCKET:
-		return is_bit_subset(c.sock.perm, IPC_SDATA | IPC_CDATA
-						      | IPC_SCAP | IPC_CCAP)
-		       && is_bit_subset(c.sock.mode, IPC_YIELD | IPC_NOYIELD);
-	default:
-		return false;
-	}
-}
-
-static bool cap_time_derivable(cap_t p, cap_t c)
-{
-	return (c.type == CAPTY_TIME) && (p.time.hart == c.time.hart)
-	       && is_range_prefix(p.time.mrk, p.time.end, c.time.bgn,
-				  c.time.end);
-}
-
-static bool cap_mem_derivable(cap_t p, cap_t c)
-{
-	if (c.type == CAPTY_PMP) {
-		uint64_t p_mrk, p_end, c_base, c_size;
-		p_mrk = tag_block_to_addr(p.mem.tag, p.mem.mrk);
-		p_end = tag_block_to_addr(p.mem.tag, p.mem.end);
-		pmp_napot_decode(c.pmp.addr, &c_base, &c_size);
-		return is_range_subset(p_mrk, p_end, c_base, c_base + c_size)
-		       && is_bit_subset(c.pmp.rwx, p.mem.rwx);
-	}
-	return (c.type == CAPTY_MEMORY) && (p.mem.tag == c.mem.tag)
-	       && is_range_subset(p.mem.mrk, p.mem.end, c.mem.bgn, c.mem.end)
-	       && is_bit_subset(c.mem.rwx, p.mem.rwx);
-}
-
-static bool cap_mon_derivable(cap_t p, cap_t c)
-{
-	return (c.type == CAPTY_MONITOR)
-	       && is_range_subset(p.mon.mrk, p.mon.end, c.mon.bgn, c.mon.end);
-}
-
-static bool cap_chan_derivable(cap_t p, cap_t c)
-{
-	if (c.type == CAPTY_SOCKET) {
-		return (c.sock.tag == 0)
-		       && is_range_subset(p.chan.mrk, p.chan.end, c.sock.chan,
-					  c.sock.chan + 1);
-	}
-	return (c.type == CAPTY_CHANNEL)
-	       && is_range_subset(p.chan.mrk, p.chan.end, c.chan.bgn,
-				  c.chan.end);
-}
-
-static bool cap_sock_derivable(cap_t p, cap_t c)
-{
-	return (c.type == CAPTY_SOCKET) && (p.sock.chan == c.sock.chan)
-	       && (p.sock.tag == 0) && (c.sock.tag != 0)
-	       && (p.sock.mode == c.sock.mode) && (p.sock.perm == c.sock.perm);
-}
-
-bool cap_is_derivable(cap_t p, cap_t c)
-{
-	switch (p.type) {
-	case CAPTY_TIME:
-		return cap_time_derivable(p, c);
-	case CAPTY_MEMORY:
-		return cap_mem_derivable(p, c);
+		return cap.pmp.used == 0 && cap.pmp.slot == 0;
 	case CAPTY_MONITOR:
-		return cap_mon_derivable(p, c);
+		return cap.mon.bgn < cap.mon.end && cap.mon.bgn == cap.mon.mrk;
 	case CAPTY_CHANNEL:
-		return cap_chan_derivable(p, c);
+		return cap.mem.bgn < cap.mem.end && cap.mem.bgn == cap.mem.mrk;
 	case CAPTY_SOCKET:
-		return cap_sock_derivable(p, c);
+		return (cap.sock.mode == IPC_YIELD)
+		       || (cap.sock.mode == IPC_NOYIELD);
 	default:
 		return false;
 	}
diff --git a/kernel/src/csr.c b/kernel/src/csr.c
deleted file mode 100644
index c7fd970f..00000000
--- a/kernel/src/csr.c
+++ /dev/null
@@ -1,162 +0,0 @@
-#include "csr.h"
-
-uint64_t csrr_mhartid(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,mhartid" : "=r"(val));
-	return val;
-}
-
-uint64_t csrr_mip(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,mip" : "=r"(val));
-	return val;
-}
-
-uint64_t csrr_mcycle(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,mcycle" : "=r"(val));
-	return val;
-}
-
-void csrw_mcycle(uint64_t val)
-{
-	__asm__ volatile("csrw mcycle,%0" ::"r"(val));
-}
-
-uint64_t csrr_mhpmcounter3(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,mhpmcounter3" : "=r"(val));
-	return val;
-}
-
-void csrw_mhpmcounter3(uint64_t val)
-{
-	__asm__ volatile("csrw mhpmcounter3,%0" ::"r"(val));
-}
-
-void csrw_mstatus(uint64_t val)
-{
-	__asm__ volatile("csrw mstatus,%0" ::"r"(val));
-}
-
-void csrs_mstatus(uint64_t val)
-{
-	__asm__ volatile("csrs mstatus,%0" ::"r"(val));
-}
-
-void csrc_mstatus(uint64_t val)
-{
-	__asm__ volatile("csrc mstatus,%0" ::"r"(val));
-}
-
-uint64_t csrr_pmpcfg0(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,pmpcfg0" : "=r"(val));
-	return val;
-}
-
-void csrw_pmpcfg0(uint64_t val)
-{
-	__asm__ volatile("csrw pmpcfg0,%0" ::"r"(val));
-}
-
-uint64_t csrr_pmpaddr0(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,pmpaddr0" : "=r"(val));
-	return val;
-}
-
-uint64_t csrr_pmpaddr1(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,pmpaddr1" : "=r"(val));
-	return val;
-}
-
-uint64_t csrr_pmpaddr2(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,pmpaddr2" : "=r"(val));
-	return val;
-}
-
-uint64_t csrr_pmpaddr3(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,pmpaddr3" : "=r"(val));
-	return val;
-}
-
-uint64_t csrr_pmpaddr4(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,pmpaddr4" : "=r"(val));
-	return val;
-}
-
-uint64_t csrr_pmpaddr5(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,pmpaddr5" : "=r"(val));
-	return val;
-}
-
-uint64_t csrr_pmpaddr6(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,pmpaddr6" : "=r"(val));
-	return val;
-}
-
-uint64_t csrr_pmpaddr7(void)
-{
-	uint64_t val;
-	__asm__ volatile("csrr %0,pmpaddr7" : "=r"(val));
-	return val;
-}
-
-void csrw_pmpaddr0(uint64_t val)
-{
-	__asm__ volatile("csrw pmpaddr0,%0" ::"r"(val));
-}
-
-void csrw_pmpaddr1(uint64_t val)
-{
-	__asm__ volatile("csrw pmpaddr1,%0" ::"r"(val));
-}
-
-void csrw_pmpaddr2(uint64_t val)
-{
-	__asm__ volatile("csrw pmpaddr2,%0" ::"r"(val));
-}
-
-void csrw_pmpaddr3(uint64_t val)
-{
-	__asm__ volatile("csrw pmpaddr3,%0" ::"r"(val));
-}
-
-void csrw_pmpaddr4(uint64_t val)
-{
-	__asm__ volatile("csrw pmpaddr4,%0" ::"r"(val));
-}
-
-void csrw_pmpaddr5(uint64_t val)
-{
-	__asm__ volatile("csrw pmpaddr5,%0" ::"r"(val));
-}
-
-void csrw_pmpaddr6(uint64_t val)
-{
-	__asm__ volatile("csrw pmpaddr6,%0" ::"r"(val));
-}
-
-void csrw_pmpaddr7(uint64_t val)
-{
-	__asm__ volatile("csrw pmpaddr7,%0" ::"r"(val));
-}
diff --git a/kernel/src/exception.c b/kernel/src/exception.c
index 82a1c8c2..2319504c 100644
--- a/kernel/src/exception.c
+++ b/kernel/src/exception.c
@@ -1,6 +1,7 @@
 /* See LICENSE file for copyright and license details. */
 #include "exception.h"
 
+#include "csr.h"
 #include "kernel.h"
 #include "proc.h"
 #include "trap.h"
@@ -11,51 +12,44 @@
 #define SRET 0x10200073
 #define URET 0x00200073
 
-static void handle_ret(proc_t *p) __attribute__((noreturn));
-static void handle_default(proc_t *p, uint64_t mcause, uint64_t mepc,
-			   uint64_t mtval) __attribute__((noreturn));
-
-void handle_exception(proc_t *p, uint64_t mcause, uint64_t mepc, uint64_t mtval)
+proc_t *handle_exception(void)
 {
+	kprintf(1, "> handle_exception(pid=%X,mcause=%X,mtval=%X,mepc=%X)\n",
+		current->pid, csrr(mcause), csrr(mtval), csrr(mepc));
 	/* Check if it is a return from exception */
-	if (mcause == ILLEGAL_INSTRUCTION
-	    && (mtval == MRET || mtval == SRET || mtval == URET))
-		// Handle return from exception
-		handle_ret(p);
-	// Handle default exception
-	handle_default(p, mcause, mepc, mtval);
+	current->regs[REG_ECAUSE] = csrr(mcause);
+	current->regs[REG_EVAL] = csrr(mtval);
+	current->regs[REG_EPC] = current->regs[REG_PC];
+	current->regs[REG_ESP] = current->regs[REG_SP];
+	current->regs[REG_PC] = current->regs[REG_TPC];
+	current->regs[REG_SP] = current->regs[REG_TSP];
+	if (!current->regs[REG_PC]) {
+		proc_suspend(current);
+		return NULL;
+	}
+	return current;
 }
 
-/**
- * This function restores the program counter and stack pointer to their values
- * prior to the exception, and clears the exception cause and exception value
- * registers.
- */
-void handle_ret(proc_t *p)
+static proc_t *handle_trap_return(void)
 {
-	p->regs[REG_PC] = p->regs[REG_EPC];
-	p->regs[REG_SP] = p->regs[REG_ESP];
-	p->regs[REG_ECAUSE] = 0;
-	p->regs[REG_EVAL] = 0;
-	p->regs[REG_EPC] = 0;
-	p->regs[REG_ESP] = 0;
-	trap_resume(p);
+	kprintf(1, "> handle_trap_return(pid=%X)\n", current->pid);
+	current->regs[REG_PC] = current->regs[REG_EPC];
+	current->regs[REG_SP] = current->regs[REG_ESP];
+	current->regs[REG_ECAUSE] = 0;
+	current->regs[REG_EVAL] = 0;
+	current->regs[REG_EPC] = 0;
+	current->regs[REG_ESP] = 0;
+	return current;
 }
 
-/*
- * This function is called when an exception occurs that doesn't fall under the
- * category of an illegal instruction return, such as a page fault or a timer
- * interrupt. It updates the exception cause, value, program counter, and stack
- * pointer in the process's registers, and switches to the trap handler program
- * counter and stack pointer.
- */
-void handle_default(proc_t *p, uint64_t mcause, uint64_t mepc, uint64_t mtval)
+proc_t *handle_illegal_instruction(void)
 {
-	p->regs[REG_ECAUSE] = mcause;
-	p->regs[REG_EVAL] = mtval;
-	p->regs[REG_EPC] = p->regs[REG_PC];
-	p->regs[REG_ESP] = p->regs[REG_SP];
-	p->regs[REG_PC] = p->regs[REG_TPC];
-	p->regs[REG_SP] = p->regs[REG_TSP];
-	trap_resume(p);
+	switch (csrr(mtval)) {
+	case MRET:
+	case SRET:
+	case URET:
+		return handle_trap_return();
+	default:
+		return handle_exception();
+	}
 }
diff --git a/kernel/src/head.S b/kernel/src/head.S
index 5dbe3e69..4938b234 100644
--- a/kernel/src/head.S
+++ b/kernel/src/head.S
@@ -7,55 +7,26 @@
 .extern sched
 .extern trap_entry
 .extern trap_exit
+.extern trap_resume
 
 .section .text.init,"ax",@progbits
 
 .globl _start
 .type _start, @function
 _start:
-	li	x1,0
-	li	x2,0
-	li	x3,0
-	li	x4,0
-	li	x5,0
-	li	x6,0
-	li	x7,0
-	li	x8,0
-	li	x9,0
-	li	x10,0
-	li	x11,0
-	li	x12,0
-	li	x13,0
-	li	x14,0
-	li	x15,0
-	li	x16,0
-	li	x17,0
-	li	x18,0
-	li	x19,0
-	li	x20,0
-	li	x21,0
-	li	x22,0
-	li	x23,0
-	li	x24,0
-	li	x25,0
-	li	x26,0
-	li	x27,0
-	li	x28,0
-	li	x29,0
-	li	x30,0
-	li	x31,0
-	/* Load global pointer */
+head_entry:
+	// Load global pointer.
 	ld_gp
 	ld_sp	t1
 
-	/* If hartid != MIN_HARTID, then jump to wait. */
+	// If hartid != MIN_HARTID, then jump to wait.
 	csrr	t0,mhartid
 	li	t1,S3K_MIN_HART
 	li	t2,S3K_MAX_HART
 	bltu	t0,t1,__hang
 	bgtu	t0,t2,__hang
 
-	/* Set some CSRs to 0 */
+	// Set some CSRs to 0.
 	csrw	mstatus,0
 	csrw	medeleg,0
 	csrw	mideleg,0
@@ -63,15 +34,16 @@ _start:
 	csrw	mie,0
 	csrw	satp,0
 
-	/* Set trap entry. */
-	la	t0,trap_entry
+	// Set trap entry.
+	la	t0,__hang
 	csrw	mtvec,t0
 
 	csrr	t0,mhartid
 	li	t1,S3K_MIN_HART
 	bne	t0,t1,wait
 
-zero_bss: /* write zeros to the bss section */
+zero_bss:
+	// Initialize the bss section.
 	la	t0,_bss
 	la 	t1,_end
 	j	2f
@@ -85,21 +57,26 @@ zero_bss: /* write zeros to the bss section */
 	li	t1,1
 	sw	t1,(t0)
 
-wait:   /* Wait for initilization to finish. */
+wait:   // Wait for initilization to finish.
 	la	t0,kernel_ready
 	lw	t0,(t0)
 	beqz	t0,wait
 
 head_exit:
-	/* Enable timer interrupts */
+	// Enable timer interrupts.
 	li	t0,MIE_MTIE
 	csrw	mie,t0
 
-	/* Start user processes. */
-	li	a0,0
+	// Start user processes.
+	la	t0,trap_entry
+	csrw	mtvec,t0
+	csrw	mscratch,0
+	la	ra,trap_exit
 	tail	sched
 
 __hang:
+	csrw	mie,0
+	wfi
 	j	__hang
 
 .section .data
diff --git a/kernel/src/kernel.c b/kernel/src/kernel.c
index 1781e9dd..8271df56 100644
--- a/kernel/src/kernel.c
+++ b/kernel/src/kernel.c
@@ -2,60 +2,36 @@
 
 #include "altc/altio.h"
 #include "altc/init.h"
+#include "cap_lock.h"
 #include "cap_table.h"
 #include "csr.h"
 #include "kassert.h"
-#include "mcslock.h"
 #include "proc.h"
 #include "sched.h"
 
-static mcslock_t lock;
-static uint64_t wcet;
-
 void kernel_init(void)
 {
 	alt_init();
-	mcslock_init(&lock);
+	kprintf(0, "# uart initialized\n");
+#ifdef SMP
+	cap_lock_init();
+	kprintf(0, "# capability lock initialized\n");
+#endif
 	ctable_init();
+	kprintf(0, "# ctable initialized\n");
 	sched_init();
+	kprintf(0, "# scheduler initialized\n");
 	proc_init();
-	alt_puts("kernel initialized");
-}
-
-uint64_t kernel_wcet(void)
-{
-	return wcet;
+	kprintf(0, "# processes initialized\n");
+	kprintf(0, "# kernel initialization complete\n");
+	kprintf(0, "# starting boot process\n");
 }
 
-void kernel_wcet_reset(void)
+bool kernel_preempt(void)
 {
-	wcet = 0;
-}
-
-bool kernel_lock(proc_t *p)
-{
-	kernel_hook_sys_exit(p);
-	bool res = mcslock_try_acquire(&lock, &p->qnode);
-	kernel_hook_sys_entry(p);
-	return res;
-}
-
-void kernel_unlock(proc_t *p)
-{
-	mcslock_release(&lock, &p->qnode);
-}
-
-void kernel_hook_sys_entry(proc_t *p)
-{
-#ifdef INSTRUMENT
-	csrw_mcycle(0);
-#endif
-}
-
-void kernel_hook_sys_exit(proc_t *p)
-{
-#ifdef INSTRUMENT
-	uint64_t cycles = csrr_mcycle();
-	__asm__ volatile("amomax.d x0,%0,(%1)" ::"r"(cycles), "r"(&wcet));
-#endif
+#ifndef NPREEMPT
+	return csrr(mip) & 0x80;
+#else  /* NPREEMPT */
+	return false;
+#endif /* NPREEMPT */
 }
diff --git a/kernel/src/kprintf.c b/kernel/src/kprintf.c
new file mode 100644
index 00000000..b68ba466
--- /dev/null
+++ b/kernel/src/kprintf.c
@@ -0,0 +1,35 @@
+#include "kprintf.h"
+
+#include <altc/altio.h>
+#include <stdarg.h>
+
+#define BUF_SIZE 128
+
+#if defined(NDEBUG) || !defined(VERBOSITY)
+#undef VERBOSITY
+#define VERBOSITY 0
+#endif
+
+#ifdef SMP
+static int lock = 0;
+#endif
+
+void kprintf(int verb, const char *restrict fmt, ...)
+{
+	if (verb > VERBOSITY)
+		return;
+	char buf[BUF_SIZE];
+	va_list ap;
+	va_start(ap, fmt);
+	alt_vsnprintf(buf, BUF_SIZE, fmt, ap);
+	va_end(ap);
+
+#ifdef SMP
+	while (__atomic_fetch_or(&lock, 1, __ATOMIC_ACQUIRE))
+		;
+	alt_putstr(buf);
+	__atomic_store_n(&lock, 0, __ATOMIC_RELEASE);
+#else
+	alt_putstr(buf);
+#endif
+}
diff --git a/kernel/src/mcslock.c b/kernel/src/mcslock.c
index f503b789..21efc529 100644
--- a/kernel/src/mcslock.c
+++ b/kernel/src/mcslock.c
@@ -2,7 +2,7 @@
 #include "mcslock.h"
 
 #include "kassert.h"
-#include "preempt.h"
+#include "kernel.h"
 
 #include <stddef.h>
 
@@ -22,8 +22,6 @@ static void _release(qnode_t *const node)
 
 static bool _acquire(mcslock_t *lock, qnode_t *const node, bool preemptive)
 {
-	if (preemptive && preempt())
-		return false;
 	node->next = &lock->tail;
 	node->prev
 	    = __atomic_exchange_n(&lock->tail.prev, node, __ATOMIC_ACQUIRE);
@@ -31,7 +29,7 @@ static bool _acquire(mcslock_t *lock, qnode_t *const node, bool preemptive)
 		return true;
 	node->prev->next = node;
 	while (__atomic_load_n(&node->prev, __ATOMIC_ACQUIRE)) {
-		if (preemptive && preempt()) {
+		if (preemptive && kernel_preempt()) {
 			_release(node);
 			return false;
 		}
diff --git a/kernel/src/proc.c b/kernel/src/proc.c
index 010a8547..a37db62d 100644
--- a/kernel/src/proc.c
+++ b/kernel/src/proc.c
@@ -6,108 +6,105 @@
 #include "drivers/time.h"
 #include "kassert.h"
 
-static proc_t _processes[S3K_PROC_CNT];
+static proc_t procs[S3K_PROC_CNT];
 extern unsigned char _payload[];
 
 void proc_init(void)
 {
 	for (uint64_t i = 0; i < S3K_PROC_CNT; i++) {
-		_processes[i].pid = i;
-		_processes[i].state = PSF_SUSPENDED;
+		procs[i].pid = i;
+		procs[i].state = PSF_SUSPENDED;
 	}
-	_processes[0].state = 0;
-	_processes[0].regs[REG_PC] = (uint64_t)_payload;
+	procs[0].state = 0;
+	procs[0].regs[REG_PC] = (uint64_t)_payload;
 	KASSERT(cap_pmp_load(ctable_get(0, 0), 0) == SUCCESS);
 }
 
 proc_t *proc_get(pid_t pid)
 {
 	KASSERT(pid < S3K_PROC_CNT);
-	KASSERT(_processes[pid].pid == pid);
-	return &_processes[pid];
+	KASSERT(procs[pid].pid == pid);
+	return &procs[pid];
+}
+
+proc_state_t proc_get_state(proc_t *proc)
+{
+	proc_state_t state = proc->state;
+	if ((state == PSF_BLOCKED) && time_get() >= proc->timeout)
+		return 0;
+	return state;
 }
 
 bool proc_acquire(proc_t *proc)
 {
-	// Set the busy flag if expected state
-	uint64_t expected = proc->state;
-	uint64_t desired = PSF_BUSY;
-	uint64_t curr_time = time_get();
-
-	// If state == 0, then process is ready.
-	bool is_ready = (expected == 0);
-	// If state is blocked, then process logically ready on timeout.
-	bool is_timeout
-	    = ((expected & PSF_BLOCKED) && curr_time >= proc->timeout);
-
-	if (!is_ready && !is_timeout)
+	proc_state_t expected = proc->state;
+	proc_state_t desired = PSF_BUSY;
+
+	if (expected & (PSF_BUSY | PSF_SUSPENDED))
 		return false;
 
-	bool succ = __atomic_compare_exchange(&proc->state, &expected, &desired,
-					      false /* not weak */,
-					      __ATOMIC_ACQUIRE /* succ */,
-					      __ATOMIC_RELAXED /* fail */);
-	if (is_timeout && succ)
-		proc->regs[REG_T0] = ERR_TIMEOUT;
-	return succ;
+	if (time_get() < proc->timeout)
+		return false;
+#ifdef SMP
+	return __atomic_compare_exchange(&proc->state, &expected, &desired,
+					 false, __ATOMIC_ACQUIRE,
+					 __ATOMIC_RELAXED);
+#else
+	proc->state = desired;
+	return true;
+#endif
 }
 
 void proc_release(proc_t *proc)
 {
-	// Unset the busy flag.
 	KASSERT(proc->state & PSF_BUSY);
-	__atomic_fetch_and(&proc->state, (uint64_t)~PSF_BUSY, __ATOMIC_RELEASE);
+#ifdef SMP
+	__atomic_fetch_xor(&proc->state, PSF_BUSY, __ATOMIC_RELEASE);
+#else
+	proc->state = 0;
+#endif
 }
 
 void proc_suspend(proc_t *proc)
 {
-	// Set the suspend flag
-	uint64_t prev_state
-	    = __atomic_fetch_or(&proc->state, PSF_SUSPENDED, __ATOMIC_ACQUIRE);
-
-	// If the process was waiting, we also unset the waiting flag.
-	if ((prev_state & 0xFF) == PSF_BLOCKED) {
-		proc->regs[REG_T0] = ERR_SUSPENDED;
+	proc_state_t prev
+	    = __atomic_fetch_or(&proc->state, PSF_SUSPENDED, __ATOMIC_RELAXED);
+	if (prev & PSF_BLOCKED) {
 		proc->state = PSF_SUSPENDED;
-		__atomic_thread_fence(__ATOMIC_ACQUIRE);
+		proc->regs[REG_T0] = ERR_SUSPENDED;
 	}
 }
 
 void proc_resume(proc_t *proc)
 {
-	// Unset the suspend flag
-	__atomic_fetch_and(&proc->state, (uint64_t)~PSF_SUSPENDED,
-			   __ATOMIC_RELEASE);
+	if (proc->state == PSF_SUSPENDED)
+		proc->timeout = 0;
+	__atomic_fetch_and(&proc->state, ~PSF_SUSPENDED, __ATOMIC_RELAXED);
 }
 
-void proc_ipc_wait(proc_t *proc, chan_t channel)
+void proc_ipc_wait(proc_t *proc, chan_t chan)
 {
 	KASSERT(proc->state == PSF_BUSY);
-	proc->state = PSF_BLOCKED | PSF_BUSY | ((uint64_t)channel << 32);
+	proc->state = PSF_BLOCKED | ((uint64_t)chan << 48) | PSF_BUSY;
 }
 
-bool proc_ipc_acquire(proc_t *proc, chan_t channel)
+bool proc_ipc_acquire(proc_t *proc, chan_t chan)
 {
-	uint64_t curr_time = time_get();
-	uint64_t timeout = timeout_get(csrr_mhartid());
-
-	if (proc->serv_time > 0) {
-		// proc is a server for a YIELDING channel with minimum server
-		// time.
-		if (proc->serv_time + curr_time >= timeout)
-			return false; // not enough time
-	}
+	proc_state_t expected = PSF_BLOCKED | ((uint64_t)chan << 48);
+	proc_state_t desired = PSF_BUSY;
 
-	// Check if the process has timed out
-	if (curr_time >= proc->timeout)
+	if (proc->state != expected)
 		return false;
-
-	// Try to acquire the process
-	uint64_t expected = PSF_BLOCKED | ((uint64_t)channel << 32);
-	uint64_t desired = PSF_BUSY;
-	return __atomic_compare_exchange(&proc->state, &expected, &desired,
-					 false, __ATOMIC_ACQUIRE,
-					 __ATOMIC_RELAXED);
+	if (time_get() >= proc->timeout)
+		return false;
+#ifdef SMP
+	return __atomic_compare_exchange_n(&proc->state, &expected, desired,
+					   false, __ATOMIC_ACQUIRE,
+					   __ATOMIC_RELAXED);
+#else
+	proc->state = desired;
+	return true;
+#endif
 }
 
 bool proc_is_suspended(proc_t *proc)
@@ -130,3 +127,16 @@ void proc_pmp_unload(proc_t *proc, pmp_slot_t slot)
 {
 	proc->pmpcfg[slot] = 0;
 }
+
+void proc_pmp_sync(proc_t *proc)
+{
+	csrw(pmpaddr0, proc->pmpaddr[0]);
+	csrw(pmpaddr1, proc->pmpaddr[1]);
+	csrw(pmpaddr2, proc->pmpaddr[2]);
+	csrw(pmpaddr3, proc->pmpaddr[3]);
+	csrw(pmpaddr4, proc->pmpaddr[4]);
+	csrw(pmpaddr5, proc->pmpaddr[5]);
+	csrw(pmpaddr6, proc->pmpaddr[6]);
+	csrw(pmpaddr7, proc->pmpaddr[7]);
+	csrw(pmpcfg0, *(uint64_t *)proc->pmpcfg);
+}
diff --git a/kernel/src/sched.c b/kernel/src/sched.c
index 423bce96..571cb48c 100644
--- a/kernel/src/sched.c
+++ b/kernel/src/sched.c
@@ -6,6 +6,7 @@
 #include "drivers/time.h"
 #include "kassert.h"
 #include "kernel.h"
+#include "kprintf.h"
 #include "proc.h"
 #include "semaphore.h"
 #include "trap.h"
@@ -13,13 +14,21 @@
 
 typedef struct slot_info {
 	// Owner of time slot.
-	uint32_t pid;
+	uint8_t pid;
 	// Remaining length of corresponding slice.
-	uint32_t length;
+	uint8_t length;
 } slot_info_t;
 
-static slot_info_t slots[S3K_HART_CNT][S3K_SLOT_CNT];
+struct sched_decision {
+	proc_t *proc;
+	uint64_t end_time;
+};
+
+static uint64_t slots[S3K_SLOT_CNT];
+
+#ifdef SMP
 static semaphore_t sched_semaphore;
+#endif
 
 void sched_init(void)
 {
@@ -28,60 +37,76 @@ void sched_init(void)
 	uint64_t from = 0;
 	uint64_t to = S3K_SLOT_CNT;
 
+#ifdef SMP
 	semaphore_init(&sched_semaphore, S3K_HART_CNT);
+#endif
 
 	for (uint64_t hartid = S3K_MIN_HART; hartid <= S3K_MAX_HART; hartid++)
 		sched_update(pid, end, hartid, from, to);
 }
 
-void sched_update(uint64_t pid, uint64_t end, uint64_t hartid, uint64_t from,
+void sched_update(uint64_t pid, uint64_t end, uint64_t hart, uint64_t from,
 		  uint64_t to)
 {
-	// Acquire all resources, blocking everyone else.
+	kprintf(1, "# sched_update(pid=%D,end=%D,hart=%D,from=%D,to=%D)\n", pid,
+		end, hart, from, to);
+#ifdef SMP
 	semaphore_acquire_n(&sched_semaphore, S3K_HART_CNT);
+#endif
+	hart -= S3K_MIN_HART;
+	int offset = hart * 16;
+	uint64_t mask = 0xFFFFull << offset;
 	for (uint64_t i = from; i < to; i++) {
-		slots[hartid - S3K_MIN_HART][i].pid = pid & 0xFF;
-		slots[hartid - S3K_MIN_HART][i].length = (end - i) & 0xFF;
+		slots[i] &= ~mask;
+		slots[i] |= ((pid << 8) | (end - i)) << offset;
 	}
-	// Release the resources.
+#ifdef SMP
 	semaphore_release_n(&sched_semaphore, S3K_HART_CNT);
+#endif
 }
 
-void sched_delete(uint64_t hartid, uint64_t from, uint64_t to)
+void sched_delete(uint64_t hart, uint64_t from, uint64_t to)
 {
+	kprintf(1, "# sched_delete(hart=%D,from=%D,to=%D)\n", hart, from, to);
+#ifdef SMP
 	semaphore_acquire_n(&sched_semaphore, S3K_HART_CNT);
-	for (uint64_t i = from; i < to; ++i) {
-		slots[hartid - S3K_MIN_HART][i].pid = 0;
-		slots[hartid - S3K_MIN_HART][i].length = 0;
-	}
-	// Release the resources.
+#endif
+	hart -= S3K_MIN_HART;
+	int offset = hart * 16;
+	uint64_t mask = 0xFFFFull << offset;
+	for (uint64_t i = from; i < to; ++i)
+		slots[i] &= ~mask;
+#ifdef SMP
 	semaphore_release_n(&sched_semaphore, S3K_HART_CNT);
+#endif
 }
 
-slot_info_t slot_info_get(uint64_t hartid, uint64_t slot)
+static slot_info_t slot_info_get(uint64_t hart, uint64_t slot)
 {
-	return slots[hartid - S3K_MIN_HART][slot % S3K_SLOT_CNT];
+	uint64_t entry = slots[slot % S3K_SLOT_CNT]
+			 >> (hart - S3K_MIN_HART) * 16;
+	uint64_t pid = (entry >> 8) & 0xFF;
+	uint64_t length = entry & 0xFF;
+	return (slot_info_t){.pid = pid, .length = length};
 }
 
-static proc_t *sched_fetch(uint64_t hartid, uint64_t *start_time,
-			   uint64_t *end_time)
+static proc_t *sched_fetch(uint64_t hart, uint64_t slot)
 {
+	proc_t *proc = NULL;
+#ifdef SMP
 	semaphore_acquire(&sched_semaphore);
-
-	// Get time slot (in global sense)
-	uint64_t slot = time_get() / S3K_SLOT_LEN;
+#endif
 	// Get time slot information
-	slot_info_t si = slot_info_get(hartid, slot);
-	// Check if time slot is first in time slice.
-	bool first = (slot_info_get(hartid, slot - 1).length == 0);
+	slot_info_t si = slot_info_get(hart, slot);
 
 	// If length = 0, then slice is deleted.
 	if (si.length == 0)
 		goto fail;
 
+#ifdef SMP
 	// Have priority over harts with lower ID when scheduling length is
 	// longer.
-	for (uint64_t i = S3K_MIN_HART; i < hartid; i++) {
+	for (uint64_t i = S3K_MIN_HART; i < hart; i++) {
 		slot_info_t other_si = slot_info_get(i, slot);
 		if (si.pid == other_si.pid && si.length <= other_si.length)
 			goto fail;
@@ -89,43 +114,49 @@ static proc_t *sched_fetch(uint64_t hartid, uint64_t *start_time,
 
 	// Have priority over harts with higher ID when scheduling length is
 	// equal or longer.
-	for (uint64_t i = hartid + 1; i < S3K_MAX_HART; i++) {
+	for (uint64_t i = hart + 1; i < S3K_MAX_HART; i++) {
 		slot_info_t other_si = slot_info_get(i, slot);
 		if (si.pid == other_si.pid && si.length < other_si.length)
 			goto fail;
 	}
+#endif
 
-	// Get the process.
-	proc_t *p = proc_get(si.pid);
+	proc = proc_get(si.pid);
 
 	// Try to acquire the process.
-	if (!proc_acquire(p))
+	if (!proc_acquire(proc)) {
+		proc = NULL;
 		goto fail;
-	semaphore_release(&sched_semaphore);
-	*start_time = slot * S3K_SLOT_LEN + (first ? S3K_SCHED_TIME : 0);
-	*end_time = (slot + si.length) * S3K_SLOT_LEN;
-	p->timeout = *end_time;
-	return p;
+	}
 
+	// Get the process.
+	kprintf(2, "# sched(hart=%d,pid=%d,slot=%D)\n", hart, si.pid,
+		slot % S3K_SLOT_CNT);
+	proc->timeout = (slot + si.length) * S3K_SLOT_LEN - S3K_SCHED_TIME;
 fail:
+#ifdef SMP
 	semaphore_release(&sched_semaphore);
-	return NULL;
+#endif
+	return proc;
 }
 
-void sched(proc_t *p)
+proc_t *sched(void)
 {
-	uint64_t hartid = csrr_mhartid();
-	uint64_t start_time, end_time;
-	if (p)
-		proc_release(p);
+	// Hart ID
+	uint64_t hart = csrr(mhartid);
+	// Time slot
+	uint64_t slot;
+	// Process to schedule
+	proc_t *proc;
+	timeout_set(hart, (uint64_t)-1);
 
 	do {
-		p = sched_fetch(hartid, &start_time, &end_time);
-	} while (!p);
-
-	timeout_set(hartid, end_time);
-	while (time_get() < start_time)
-		;
-
-	trap_exit(p);
+		slot = (time_get() + S3K_SCHED_TIME) / S3K_SLOT_LEN;
+		while (time_get() < slot * S3K_SLOT_LEN)
+			;
+		// Try schedule process
+		proc = sched_fetch(hart, slot);
+	} while (!proc);
+	timeout_set(hart, proc->timeout);
+	return proc;
 }
diff --git a/kernel/src/stack.S b/kernel/src/stack.S
index 7713995b..79fac71a 100644
--- a/kernel/src/stack.S
+++ b/kernel/src/stack.S
@@ -1,5 +1,6 @@
 .globl _sp
 
 .section .bss.stack
+.balign 8
 .skip S3K_HART_CNT * (1 << S3K_LOG_STACK_SIZE)
 _sp:
diff --git a/kernel/src/syscall.c b/kernel/src/syscall.c
index dac72303..eef34c08 100644
--- a/kernel/src/syscall.c
+++ b/kernel/src/syscall.c
@@ -2,6 +2,7 @@
 #include "syscall.h"
 
 #include "cap_ipc.h"
+#include "cap_lock.h"
 #include "cap_monitor.h"
 #include "cap_ops.h"
 #include "cap_pmp.h"
@@ -12,7 +13,6 @@
 #include "drivers/time.h"
 #include "error.h"
 #include "kernel.h"
-#include "preempt.h"
 #include "sched.h"
 #include "trap.h"
 
@@ -20,117 +20,133 @@
 
 #define ARGS 8
 
-/** True if process p should ignore ERR_PREEMPTED for system call */
-static err_t validate_arguments(uint64_t call, const sys_args_t *args);
-static err_t sys_get_info(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_reg_read(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_reg_write(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_sync(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_cap_read(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_cap_move(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_cap_delete(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_cap_revoke(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_cap_derive(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_pmp_load(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_pmp_unload(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_mon_suspend(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_mon_resume(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_mon_state_get(proc_t *p, const sys_args_t *args,
-			       uint64_t *ret);
-static err_t sys_mon_yield(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_mon_reg_read(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_mon_reg_write(proc_t *p, const sys_args_t *args,
-			       uint64_t *ret);
-static err_t sys_mon_cap_read(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_mon_cap_move(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_mon_pmp_load(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_mon_pmp_unload(proc_t *p, const sys_args_t *args,
-				uint64_t *ret);
-static err_t sys_sock_send(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_sock_recv(proc_t *p, const sys_args_t *args, uint64_t *ret);
-static err_t sys_sock_sendrecv(proc_t *p, const sys_args_t *args,
-			       uint64_t *ret);
-
-typedef err_t (*sys_handler_t)(proc_t *, const sys_args_t *, uint64_t *);
-
-sys_handler_t handlers[] = {
-    sys_get_info,	sys_reg_read,	   sys_reg_write,    sys_sync,
-    sys_cap_read,	sys_cap_move,	   sys_cap_delete,   sys_cap_revoke,
-    sys_cap_derive,	sys_pmp_load,	   sys_pmp_unload,   sys_mon_suspend,
-    sys_mon_resume,	sys_mon_state_get, sys_mon_yield,    sys_mon_reg_read,
-    sys_mon_reg_write,	sys_mon_cap_read,  sys_mon_cap_move, sys_mon_pmp_load,
-    sys_mon_pmp_unload, sys_sock_send,	   sys_sock_recv,    sys_sock_sendrecv,
+static inline err_t validate_get_info(const sys_args_t *);
+static inline err_t validate_reg_read(const sys_args_t *);
+static inline err_t validate_reg_write(const sys_args_t *);
+static inline err_t validate_sync(const sys_args_t *);
+static inline err_t validate_sleep(const sys_args_t *);
+static inline err_t validate_cap_read(const sys_args_t *);
+static inline err_t validate_cap_move(const sys_args_t *);
+static inline err_t validate_cap_delete(const sys_args_t *);
+static inline err_t validate_cap_revoke(const sys_args_t *);
+static inline err_t validate_cap_derive(const sys_args_t *);
+static inline err_t validate_pmp_load(const sys_args_t *);
+static inline err_t validate_pmp_unload(const sys_args_t *);
+static inline err_t validate_mon_suspend(const sys_args_t *);
+static inline err_t validate_mon_resume(const sys_args_t *);
+static inline err_t validate_mon_state_get(const sys_args_t *);
+static inline err_t validate_mon_yield(const sys_args_t *);
+static inline err_t validate_mon_reg_read(const sys_args_t *);
+static inline err_t validate_mon_reg_write(const sys_args_t *);
+static inline err_t validate_mon_cap_read(const sys_args_t *);
+static inline err_t validate_mon_cap_move(const sys_args_t *);
+static inline err_t validate_mon_pmp_load(const sys_args_t *);
+static inline err_t validate_mon_pmp_unload(const sys_args_t *);
+static inline err_t validate_sock_send(const sys_args_t *);
+static inline err_t validate_sock_recv(const sys_args_t *);
+static inline err_t validate_sock_sendrecv(const sys_args_t *);
+
+static proc_t *handle_get_info(proc_t *const, const sys_args_t *);
+static proc_t *handle_reg_read(proc_t *const, const sys_args_t *);
+static proc_t *handle_reg_write(proc_t *const, const sys_args_t *);
+static proc_t *handle_sync(proc_t *const, const sys_args_t *);
+static proc_t *handle_sleep(proc_t *const, const sys_args_t *);
+static proc_t *handle_cap_read(proc_t *const, const sys_args_t *);
+static proc_t *handle_cap_move(proc_t *const, const sys_args_t *);
+static proc_t *handle_cap_delete(proc_t *const, const sys_args_t *);
+static proc_t *handle_cap_revoke(proc_t *const, const sys_args_t *);
+static proc_t *handle_cap_derive(proc_t *const, const sys_args_t *);
+static proc_t *handle_pmp_load(proc_t *const, const sys_args_t *);
+static proc_t *handle_pmp_unload(proc_t *const, const sys_args_t *);
+static proc_t *handle_mon_suspend(proc_t *const, const sys_args_t *);
+static proc_t *handle_mon_resume(proc_t *const, const sys_args_t *);
+static proc_t *handle_mon_state_get(proc_t *const, const sys_args_t *);
+static proc_t *handle_mon_yield(proc_t *const, const sys_args_t *);
+static proc_t *handle_mon_reg_read(proc_t *const, const sys_args_t *);
+static proc_t *handle_mon_reg_write(proc_t *const, const sys_args_t *);
+static proc_t *handle_mon_cap_read(proc_t *const, const sys_args_t *);
+static proc_t *handle_mon_cap_move(proc_t *const, const sys_args_t *);
+static proc_t *handle_mon_pmp_load(proc_t *const, const sys_args_t *);
+static proc_t *handle_mon_pmp_unload(proc_t *const, const sys_args_t *);
+static proc_t *handle_sock_send(proc_t *const, const sys_args_t *);
+static proc_t *handle_sock_recv(proc_t *const, const sys_args_t *);
+static proc_t *handle_sock_sendrecv(proc_t *const, const sys_args_t *);
+
+typedef proc_t *(*handler_t)(proc_t *const, const sys_args_t *);
+typedef err_t (*validator_t)(const sys_args_t *);
+
+handler_t handlers[] = {
+    handle_get_info,	   handle_reg_read,	handle_reg_write,
+    handle_sync,	   handle_sleep,	handle_cap_read,
+    handle_cap_move,	   handle_cap_delete,	handle_cap_revoke,
+    handle_cap_derive,	   handle_pmp_load,	handle_pmp_unload,
+    handle_mon_suspend,	   handle_mon_resume,	handle_mon_state_get,
+    handle_mon_yield,	   handle_mon_reg_read, handle_mon_reg_write,
+    handle_mon_cap_read,   handle_mon_cap_move, handle_mon_pmp_load,
+    handle_mon_pmp_unload, handle_sock_send,	handle_sock_recv,
+    handle_sock_sendrecv,
 };
 
-void handle_syscall(proc_t *p)
+validator_t validators[] = {
+    validate_get_info,	     validate_reg_read,	    validate_reg_write,
+    validate_sync,	     validate_sleep,	    validate_cap_read,
+    validate_cap_move,	     validate_cap_delete,   validate_cap_revoke,
+    validate_cap_derive,     validate_pmp_load,	    validate_pmp_unload,
+    validate_mon_suspend,    validate_mon_resume,   validate_mon_state_get,
+    validate_mon_yield,	     validate_mon_reg_read, validate_mon_reg_write,
+    validate_mon_cap_read,   validate_mon_cap_move, validate_mon_pmp_load,
+    validate_mon_pmp_unload, validate_sock_send,    validate_sock_recv,
+    validate_sock_sendrecv,
+};
+
+proc_t *handle_syscall(void)
 {
 	// System call arguments.
-	const sys_args_t *args = (sys_args_t *)&p->regs[REG_A0];
-	// System call number.
-	uint64_t call = p->regs[REG_T0];
-	// Return value.
-	uint64_t ret = 0;
-
-	// Check that the arguments of the system calls are valid.
-	err_t err = validate_arguments(call, args);
-	if (err)
-		goto fail_lbl;
-
-	if (preempt())
-		sched(p);
-	kernel_hook_sys_entry(p);
-
-	switch (call) {
-		/* System calls without initial lock */
-	case SYS_GET_INFO:
-	case SYS_REG_READ:
-	case SYS_REG_WRITE:
-	case SYS_SYNC:
-	case SYS_CAP_READ:
-	case SYS_CAP_REVOKE:
-		err = handlers[call](p, args, &ret);
-		break;
-	default:
-		/* System calls using an initial lock */
-		if (!kernel_lock(p)) {
-			/* Kernel lock fails on preemption. */
-			err = ERR_PREEMPTED;
-			break;
-		}
-		err = handlers[call](p, args, &ret);
-		kernel_unlock(p);
-		break;
-	}
-
-	/* Exit hook for instrumentation */
-	kernel_hook_sys_exit(p);
-
-	switch (err) {
-	case YIELD: { // Yield to another process.
-		p->regs[REG_PC] += 4;
-		p->regs[REG_T0] = SUCCESS;
-		proc_t *next = (proc_t *)ret;
-		if (next == NULL)
-			sched(p);
-		if (next != p)
-			proc_release(p);
-		trap_exit(next);
-		UNREACHABLE();
+	proc_t *proc = current;
+	const sys_args_t *args = (sys_args_t *)&proc->regs[REG_A0];
+	uint64_t call = proc->regs[REG_T0];
+
+	// Validate system call arguments.
+	err_t err = ERR_INVALID_SYSCALL;
+	if (call < ARRAY_SIZE(validators))
+		err = validators[call](args);
+
+#ifndef SMP /* Single core */
+	if (err) {
+		// Increment PC
+		proc->regs[REG_PC] += 4;
+		proc->regs[REG_T0] = err;
+	} else if (kernel_preempt()) {
+		proc = NULL;
+	} else {
+		proc->regs[REG_PC] += 4;
+		proc = handlers[call](proc, args);
 	}
-	case ERR_SUSPENDED:
-	case ERR_PREEMPTED:
-		p->regs[REG_PC] += 4;
-		p->regs[REG_T0] = err;
-		sched(p);
-		UNREACHABLE();
-	default:
-	fail_lbl:
-		p->regs[REG_PC] += 4;
-		p->regs[REG_T0] = err;
-		p->regs[REG_A0] = ret;
-		trap_resume(p);
-		UNREACHABLE();
+#else /* Multicore */
+	if (err) {
+		// Invalid parameters
+		proc->regs[REG_PC] += 4;
+		proc->regs[REG_T0] = err;
+	} else if (kernel_preempt()) {
+		// Kernel preemption
+		proc = NULL;
+	} else if (call < SYS_CAP_MOVE) {
+		// These system calls do not require a lock
+		proc->regs[REG_PC] += 4;
+		proc = handlers[call](proc, args);
+	} else if (cap_lock_acquire()) {
+		// These system calls requires a lock
+		proc->regs[REG_PC] += 4;
+		proc = handlers[call](proc, args);
+		cap_lock_release();
+	} else {
+		// Lock acquire failed due to preemption
+		proc->regs[REG_PC] += 4;
+		proc->regs[REG_T0] = ERR_PREEMPTED;
+		proc = NULL;
 	}
+#endif
+	return proc;
 }
 
 static bool valid_idx(cidx_t idx)
@@ -153,341 +169,453 @@ static bool valid_reg(reg_t reg)
 	return reg < REG_CNT;
 }
 
-err_t validate_arguments(uint64_t call, const sys_args_t *args)
-{
-	// Check the argument of the system call, if they are
-	// within bounds.
-	// Checks start from the first argument.
-	switch (call) {
-	case SYS_GET_INFO:
-	case SYS_SYNC:
-		return SUCCESS;
-
-	case SYS_REG_READ:
-	case SYS_REG_WRITE:
-		if (!valid_reg(args->reg.reg))
-			return ERR_INVALID_REGISTER;
-		return SUCCESS;
-
-	case SYS_CAP_READ:
-	case SYS_CAP_DELETE:
-	case SYS_CAP_REVOKE:
-		if (!valid_idx(args->cap.idx))
-			return ERR_INVALID_INDEX;
-		return SUCCESS;
-
-	case SYS_CAP_MOVE:
-		if (!valid_idx(args->cap.idx))
-			return ERR_INVALID_INDEX;
-		if (!valid_idx(args->cap.dst_idx))
-			return ERR_INVALID_INDEX;
-		return SUCCESS;
-
-	case SYS_CAP_DERIVE:
-		if (!valid_idx(args->cap.idx))
-			return ERR_INVALID_INDEX;
-		if (!valid_idx(args->cap.dst_idx))
-			return ERR_INVALID_INDEX;
-		if (!cap_is_valid(args->cap.cap))
-			return ERR_INVALID_DERIVATION;
-		return SUCCESS;
-
-	case SYS_PMP_LOAD:
-		if (!valid_idx(args->pmp.pmp_idx))
-			return ERR_INVALID_INDEX;
-		if (!valid_slot(args->pmp.pmp_slot))
-			return ERR_INVALID_SLOT;
-		return SUCCESS;
-
-	case SYS_PMP_UNLOAD:
-		if (!valid_idx(args->pmp.pmp_idx))
-			return ERR_INVALID_INDEX;
-		return SUCCESS;
-
-	case SYS_MON_SUSPEND:
-	case SYS_MON_RESUME:
-	case SYS_MON_STATE_GET:
-	case SYS_MON_YIELD:
-		if (!valid_idx(args->mon_state.mon_idx))
-			return ERR_INVALID_INDEX;
-		if (!valid_pid(args->mon_state.pid))
-			return ERR_INVALID_PID;
-		return SUCCESS;
-
-	case SYS_MON_REG_READ:
-	case SYS_MON_REG_WRITE:
-		if (!valid_idx(args->mon_reg.mon_idx))
-			return ERR_INVALID_INDEX;
-		if (!valid_pid(args->mon_reg.pid))
-			return ERR_INVALID_PID;
-		if (!valid_reg(args->mon_reg.reg))
-			return ERR_INVALID_REGISTER;
-		return SUCCESS;
-
-	case SYS_MON_CAP_READ:
-		if (!valid_idx(args->mon_cap.mon_idx))
-			return ERR_INVALID_INDEX;
-		if (!valid_pid(args->mon_cap.pid))
-			return ERR_INVALID_PID;
-		if (!valid_idx(args->mon_cap.idx))
-			return ERR_INVALID_INDEX;
-		return SUCCESS;
-
-	case SYS_MON_CAP_MOVE:
-		if (!valid_idx(args->mon_cap.mon_idx))
-			return ERR_INVALID_INDEX;
-		if (!valid_pid(args->mon_cap.pid))
-			return ERR_INVALID_PID;
-		if (!valid_idx(args->mon_cap.idx))
-			return ERR_INVALID_INDEX;
-		if (!valid_pid(args->mon_cap.dst_pid))
-			return ERR_INVALID_PID;
-		if (!valid_idx(args->mon_cap.dst_idx))
-			return ERR_INVALID_INDEX;
-		return SUCCESS;
-
-	case SYS_MON_PMP_LOAD:
-		if (!valid_idx(args->mon_pmp.mon_idx))
-			return ERR_INVALID_INDEX;
-		if (!valid_pid(args->mon_pmp.pid))
-			return ERR_INVALID_PID;
-		if (!valid_idx(args->mon_pmp.pmp_idx))
-			return ERR_INVALID_INDEX;
-		if (!valid_slot(args->mon_pmp.pmp_slot))
-			return ERR_INVALID_SLOT;
-		return SUCCESS;
-
-	case SYS_MON_PMP_UNLOAD:
-		if (!valid_idx(args->mon_pmp.mon_idx))
-			return ERR_INVALID_INDEX;
-		if (!valid_pid(args->mon_pmp.pid))
-			return ERR_INVALID_PID;
-		if (!valid_idx(args->mon_pmp.pmp_idx))
-			return ERR_INVALID_INDEX;
-		return SUCCESS;
-
-	case SYS_SOCK_SEND:
-	case SYS_SOCK_SENDRECV:
-		if (!valid_idx(args->sock.sock_idx))
-			return ERR_INVALID_INDEX;
-		if (!valid_idx(args->sock.cap_idx))
-			return ERR_INVALID_INDEX;
-		return SUCCESS;
-	default:
-		return ERR_INVALID_SYSCALL;
-	}
+err_t validate_get_info(const sys_args_t *args)
+{
+	return SUCCESS;
 }
 
-err_t sys_get_info(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_get_info(proc_t *const p, const sys_args_t *args)
 {
 	switch (args->get_info.info) {
 	case 0:
-		*ret = p->pid;
+		p->regs[REG_A0] = p->pid;
 		break;
 	case 1:
-		*ret = time_get();
+		p->regs[REG_A0] = time_get();
 		break;
 	case 2:
-		*ret = timeout_get(csrr_mhartid());
-		break;
-	case 3:
-		*ret = kernel_wcet();
-		break;
-	case 4:
-		*ret = kernel_wcet();
-		kernel_wcet_reset();
+		p->regs[REG_A0] = timeout_get(csrr(mhartid));
 		break;
 	default:
-		*ret = 0;
+		p->regs[REG_A0] = 0;
 	}
+	p->regs[REG_T0] = SUCCESS;
+	return p;
+}
+
+err_t validate_reg_read(const sys_args_t *args)
+{
+	if (!valid_reg(args->reg_read.reg))
+		return ERR_INVALID_REGISTER;
 	return SUCCESS;
 }
 
-err_t sys_reg_read(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_reg_read(proc_t *const p, const sys_args_t *args)
 {
-	*ret = p->regs[args->reg.reg];
+	p->regs[REG_T0] = SUCCESS;
+	p->regs[REG_A0] = p->regs[args->reg_read.reg];
+	return p;
+}
+
+err_t validate_reg_write(const sys_args_t *args)
+{
+	if (!valid_reg(args->reg_write.reg))
+		return ERR_INVALID_REGISTER;
 	return SUCCESS;
 }
 
-err_t sys_reg_write(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_reg_write(proc_t *const p, const sys_args_t *args)
+{
+	p->regs[REG_T0] = SUCCESS;
+	p->regs[args->reg_write.reg] = args->reg_write.val;
+	return p;
+}
+
+err_t validate_sync(const sys_args_t *args)
 {
-	p->regs[args->reg.reg] = args->reg.val;
 	return SUCCESS;
 }
 
-err_t sys_sync(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_sync(proc_t *const p, const sys_args_t *args)
 {
 	// Full sync invokes scheduler,
 	// otherwise only update memory.
-	*ret = args->sync.full ? 0 : ((uint64_t)p);
-	return YIELD;
+	if (args->sync.full) {
+		proc_release(p);
+		return NULL;
+	}
+	proc_pmp_sync(p);
+	return p;
 }
 
-err_t sys_cap_read(proc_t *p, const sys_args_t *args, uint64_t *ret)
+err_t validate_sleep(const sys_args_t *args)
 {
-	cte_t c = ctable_get(p->pid, args->cap.idx);
-	return cap_read(c, (cap_t *)ret);
+	return SUCCESS;
 }
 
-err_t sys_cap_move(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_sleep(proc_t *const p, const sys_args_t *args)
 {
-	cte_t src = ctable_get(p->pid, args->cap.idx);
-	cte_t dst = ctable_get(p->pid, args->cap.dst_idx);
-	return cap_move(src, dst, (cap_t *)ret);
+	p->regs[REG_T0] = SUCCESS;
+	if (args->sleep.time)
+		p->timeout = args->sleep.time;
+	return NULL;
 }
 
-err_t sys_cap_delete(proc_t *p, const sys_args_t *args, uint64_t *ret)
+err_t validate_cap_read(const sys_args_t *args)
 {
-	cte_t c = ctable_get(p->pid, args->cap.idx);
-	return cap_delete(c);
+	if (!valid_idx(args->cap_read.idx))
+		return ERR_INVALID_INDEX;
+	return SUCCESS;
 }
 
-err_t sys_cap_revoke(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_cap_read(proc_t *const p, const sys_args_t *args)
 {
-	cte_t c = ctable_get(p->pid, args->cap.idx);
-	while (1) {
-		cap_t cap = cte_cap(c);
-		cte_t next = cte_next(c);
-		cap_t ncap = cte_cap(next);
-		if (!cap.type)
-			return ERR_EMPTY;
-		// If ncap can not be revoked, we have no more children.
-		if (!cap_is_revokable(cap, ncap))
-			break;
-		if (!kernel_lock(p))
-			return ERR_PREEMPTED;
-		// Delete (next, ncap), take its resource, and update (c, cap)
-		// The delete may fail due to interference.
-		// Checks cte_next(c) == next && cte_cap == ncap before
-		// deleting.
-		cap_reclaim(c, cap, next, ncap);
-		kernel_unlock(p);
-	}
+	cte_t c = ctable_get(p->pid, args->cap_read.idx);
+	p->regs[REG_T0] = cap_read(c, (cap_t *)&p->regs[REG_A0]);
+	return p;
+}
+
+err_t validate_cap_move(const sys_args_t *args)
+{
+	if (!valid_idx(args->cap_move.src_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_idx(args->cap_move.dst_idx))
+		return ERR_INVALID_INDEX;
+	return SUCCESS;
+}
+
+proc_t *handle_cap_move(proc_t *const p, const sys_args_t *args)
+{
+	cte_t src = ctable_get(p->pid, args->cap_move.src_idx);
+	cte_t dst = ctable_get(p->pid, args->cap_move.dst_idx);
+	p->regs[REG_T0] = cap_move(src, dst);
+	return p;
+}
+
+err_t validate_cap_delete(const sys_args_t *args)
+{
+	if (!valid_idx(args->cap_delete.idx))
+		return ERR_INVALID_INDEX;
+	return SUCCESS;
+}
+
+proc_t *handle_cap_delete(proc_t *const p, const sys_args_t *args)
+{
+	cte_t c = ctable_get(p->pid, args->cap_delete.idx);
+	p->regs[REG_T0] = cap_delete(c);
+	return p;
+}
+
+err_t validate_cap_revoke(const sys_args_t *args)
+{
+	if (!valid_idx(args->cap_revoke.idx))
+		return ERR_INVALID_INDEX;
+	return SUCCESS;
+}
+
+proc_t *handle_cap_revoke(proc_t *const p, const sys_args_t *args)
+{
+	cte_t c = ctable_get(p->pid, args->cap_revoke.idx);
+
+	p->regs[REG_T0] = cap_revoke(c);
+	return p->regs[REG_T0] == ERR_PREEMPTED ? NULL : p;
+}
+
+err_t validate_cap_derive(const sys_args_t *args)
+{
+	if (!valid_idx(args->cap_derive.src_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_idx(args->cap_derive.dst_idx))
+		return ERR_INVALID_INDEX;
+	cap_t cap = {.raw = args->cap_derive.cap_raw};
+	if (!cap_is_valid(cap))
+		return ERR_INVALID_DERIVATION;
+	return SUCCESS;
+}
+
+proc_t *handle_cap_derive(proc_t *const p, const sys_args_t *args)
+{
+	cte_t src = ctable_get(p->pid, args->cap_derive.src_idx);
+	cte_t dst = ctable_get(p->pid, args->cap_derive.dst_idx);
+	cap_t cap = {.raw = args->cap_derive.cap_raw};
+	p->regs[REG_T0] = cap_derive(src, dst, cap);
+	return p;
+}
 
-	// We should reach here if we have no more children.
-	if (!kernel_lock(p))
-		return ERR_PREEMPTED;
-	// Reset the capability at c.
-	err_t err = cap_reset(c);
-	kernel_unlock(p);
-	return err;
+err_t validate_pmp_load(const sys_args_t *args)
+{
+	if (!valid_idx(args->pmp_load.idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_slot(args->pmp_load.slot))
+		return ERR_INVALID_SLOT;
+	return SUCCESS;
 }
 
-err_t sys_cap_derive(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_pmp_load(proc_t *const p, const sys_args_t *args)
 {
-	cte_t src = ctable_get(p->pid, args->cap.idx);
-	cte_t dst = ctable_get(p->pid, args->cap.dst_idx);
-	return cap_derive(src, dst, args->cap.cap);
+	cte_t pmp = ctable_get(p->pid, args->pmp_load.idx);
+	p->regs[REG_T0] = cap_pmp_load(pmp, args->pmp_load.slot);
+	return p;
+}
+
+err_t validate_pmp_unload(const sys_args_t *args)
+{
+	if (!valid_idx(args->pmp_unload.idx))
+		return ERR_INVALID_INDEX;
+	return SUCCESS;
 }
 
-err_t sys_pmp_load(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_pmp_unload(proc_t *const p, const sys_args_t *args)
 {
-	cte_t pmp = ctable_get(p->pid, args->pmp.pmp_idx);
-	return cap_pmp_load(pmp, args->pmp.pmp_slot);
+	cte_t pmp = ctable_get(p->pid, args->pmp_unload.idx);
+	p->regs[REG_T0] = cap_pmp_unload(pmp);
+	return p;
 }
 
-err_t sys_pmp_unload(proc_t *p, const sys_args_t *args, uint64_t *ret)
+err_t validate_mon_suspend(const sys_args_t *args)
 {
-	cte_t pmp = ctable_get(p->pid, args->pmp.pmp_idx);
-	return cap_pmp_unload(pmp);
+	if (!valid_idx(args->mon_state.mon_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_pid(args->mon_state.pid))
+		return ERR_INVALID_PID;
+	return SUCCESS;
 }
 
-err_t sys_mon_suspend(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_mon_suspend(proc_t *const p, const sys_args_t *args)
 {
 	cte_t mon = ctable_get(p->pid, args->mon_state.mon_idx);
-	return cap_monitor_suspend(mon, args->mon_state.pid);
+	p->regs[REG_T0] = cap_monitor_suspend(mon, args->mon_state.pid);
+	return p;
 }
 
-err_t sys_mon_resume(proc_t *p, const sys_args_t *args, uint64_t *ret)
+err_t validate_mon_resume(const sys_args_t *args)
+{
+	if (!valid_idx(args->mon_state.mon_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_pid(args->mon_state.pid))
+		return ERR_INVALID_PID;
+	return SUCCESS;
+}
+
+proc_t *handle_mon_resume(proc_t *const p, const sys_args_t *args)
 {
 	cte_t mon = ctable_get(p->pid, args->mon_state.mon_idx);
-	return cap_monitor_resume(mon, args->mon_state.pid);
+	p->regs[REG_T0] = cap_monitor_resume(mon, args->mon_state.pid);
+	return p;
+}
+
+err_t validate_mon_state_get(const sys_args_t *args)
+{
+	if (!valid_idx(args->mon_state.mon_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_pid(args->mon_state.pid))
+		return ERR_INVALID_PID;
+	return SUCCESS;
 }
 
-err_t sys_mon_state_get(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_mon_state_get(proc_t *const p, const sys_args_t *args)
 {
 	cte_t mon = ctable_get(p->pid, args->mon_state.mon_idx);
-	return cap_monitor_state_get(mon, args->mon_state.pid,
-				     (proc_state_t *)ret);
+	p->regs[REG_T0] = cap_monitor_state_get(
+	    mon, args->mon_state.pid, (proc_state_t *)&p->regs[REG_A0]);
+	return p;
 }
 
-err_t sys_mon_yield(proc_t *p, const sys_args_t *args, uint64_t *ret)
+err_t validate_mon_yield(const sys_args_t *args)
+{
+	if (!valid_idx(args->mon_state.mon_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_pid(args->mon_state.pid))
+		return ERR_INVALID_PID;
+	return SUCCESS;
+}
+
+proc_t *handle_mon_yield(proc_t *const p, const sys_args_t *args)
 {
 	cte_t mon = ctable_get(p->pid, args->mon_state.mon_idx);
-	return cap_monitor_yield(mon, args->mon_state.pid, (proc_t **)ret);
+	proc_t *next = p;
+	p->regs[REG_T0] = cap_monitor_yield(mon, args->mon_state.pid, &next);
+	return next;
+}
+
+err_t validate_mon_reg_read(const sys_args_t *args)
+{
+	if (!valid_idx(args->mon_reg_read.mon_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_pid(args->mon_reg_read.pid))
+		return ERR_INVALID_PID;
+	if (!valid_reg(args->mon_reg_read.reg))
+		return ERR_INVALID_REGISTER;
+	return SUCCESS;
+}
+
+proc_t *handle_mon_reg_read(proc_t *const p, const sys_args_t *args)
+{
+	cte_t mon = ctable_get(p->pid, args->mon_reg_read.mon_idx);
+	p->regs[REG_T0] = cap_monitor_reg_read(mon, args->mon_reg_read.pid,
+					       args->mon_reg_read.reg,
+					       &p->regs[REG_A0]);
+	return p;
+}
+
+err_t validate_mon_reg_write(const sys_args_t *args)
+{
+	if (!valid_idx(args->mon_reg_write.mon_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_pid(args->mon_reg_write.pid))
+		return ERR_INVALID_PID;
+	if (!valid_reg(args->mon_reg_write.reg))
+		return ERR_INVALID_REGISTER;
+	return SUCCESS;
+}
+
+proc_t *handle_mon_reg_write(proc_t *const p, const sys_args_t *args)
+{
+	cte_t mon = ctable_get(p->pid, args->mon_reg_write.mon_idx);
+	p->regs[REG_T0] = cap_monitor_reg_write(mon, args->mon_reg_write.pid,
+						args->mon_reg_write.reg,
+						args->mon_reg_write.val);
+	return p;
+}
+
+err_t validate_mon_cap_read(const sys_args_t *args)
+{
+	if (!valid_idx(args->mon_cap_read.mon_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_pid(args->mon_cap_read.pid))
+		return ERR_INVALID_PID;
+	if (!valid_idx(args->mon_cap_read.idx))
+		return ERR_INVALID_INDEX;
+	return SUCCESS;
+}
+
+proc_t *handle_mon_cap_read(proc_t *const p, const sys_args_t *args)
+{
+	cte_t mon = ctable_get(p->pid, args->mon_cap_read.mon_idx);
+	cte_t src = ctable_get(args->mon_cap_read.pid, args->mon_cap_read.idx);
+	p->regs[REG_T0]
+	    = cap_monitor_cap_read(mon, src, (cap_t *)&p->regs[REG_A0]);
+	return p;
+}
+
+err_t validate_mon_cap_move(const sys_args_t *args)
+{
+	if (!valid_idx(args->mon_cap_move.mon_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_pid(args->mon_cap_move.src_pid))
+		return ERR_INVALID_PID;
+	if (!valid_idx(args->mon_cap_move.src_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_pid(args->mon_cap_move.dst_pid))
+		return ERR_INVALID_PID;
+	if (!valid_idx(args->mon_cap_move.dst_idx))
+		return ERR_INVALID_INDEX;
+	return SUCCESS;
 }
 
-err_t sys_mon_reg_read(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_mon_cap_move(proc_t *const p, const sys_args_t *args)
 {
-	cte_t mon = ctable_get(p->pid, args->mon_reg.mon_idx);
-	return cap_monitor_reg_read(mon, args->mon_reg.pid, args->mon_reg.reg,
-				    ret);
+	cte_t mon = ctable_get(p->pid, args->mon_cap_move.mon_idx);
+	cte_t src = ctable_get(args->mon_cap_move.src_pid,
+			       args->mon_cap_move.src_idx);
+	cte_t dst = ctable_get(args->mon_cap_move.dst_pid,
+			       args->mon_cap_move.dst_idx);
+	p->regs[REG_T0] = cap_monitor_cap_move(mon, src, dst);
+	return p;
 }
 
-err_t sys_mon_reg_write(proc_t *p, const sys_args_t *args, uint64_t *ret)
+err_t validate_mon_pmp_load(const sys_args_t *args)
 {
-	cte_t mon = ctable_get(p->pid, args->mon_reg.mon_idx);
-	return cap_monitor_reg_write(mon, args->mon_reg.pid, args->mon_reg.reg,
-				     args->mon_reg.val);
+	if (!valid_idx(args->mon_pmp_load.mon_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_pid(args->mon_pmp_load.pid))
+		return ERR_INVALID_PID;
+	if (!valid_idx(args->mon_pmp_load.idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_pid(args->mon_pmp_load.slot))
+		return ERR_INVALID_PID;
+	return SUCCESS;
 }
 
-err_t sys_mon_cap_read(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_mon_pmp_load(proc_t *const p, const sys_args_t *args)
 {
-	cte_t mon = ctable_get(p->pid, args->mon_cap.mon_idx);
-	cte_t src = ctable_get(args->mon_cap.pid, args->mon_cap.idx);
-	return cap_monitor_cap_read(mon, src, (cap_t *)ret);
+	cte_t mon = ctable_get(p->pid, args->mon_pmp_load.mon_idx);
+	cte_t pmp = ctable_get(args->mon_pmp_load.pid, args->mon_pmp_load.idx);
+	p->regs[REG_T0]
+	    = cap_monitor_pmp_load(mon, pmp, args->mon_pmp_load.slot);
+	return p;
 }
 
-err_t sys_mon_cap_move(proc_t *p, const sys_args_t *args, uint64_t *ret)
+err_t validate_mon_pmp_unload(const sys_args_t *args)
 {
-	cte_t mon = ctable_get(p->pid, args->mon_cap.mon_idx);
-	cte_t src = ctable_get(args->mon_cap.pid, args->mon_cap.idx);
-	cte_t dst = ctable_get(args->mon_cap.dst_pid, args->mon_cap.dst_idx);
-	return cap_monitor_cap_move(mon, src, dst);
+	if (!valid_idx(args->mon_pmp_unload.mon_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_pid(args->mon_pmp_unload.pid))
+		return ERR_INVALID_PID;
+	if (!valid_idx(args->mon_pmp_unload.idx))
+		return ERR_INVALID_INDEX;
+	return SUCCESS;
 }
 
-err_t sys_mon_pmp_load(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_mon_pmp_unload(proc_t *const p, const sys_args_t *args)
 {
-	cte_t mon = ctable_get(p->pid, args->mon_pmp.mon_idx);
-	cte_t pmp = ctable_get(args->mon_pmp.pid, args->mon_pmp.pmp_idx);
-	return cap_monitor_pmp_load(mon, pmp, args->mon_pmp.pmp_slot);
+	cte_t mon = ctable_get(p->pid, args->mon_pmp_unload.mon_idx);
+	cte_t pmp
+	    = ctable_get(args->mon_pmp_unload.pid, args->mon_pmp_unload.idx);
+	p->regs[REG_T0] = cap_monitor_pmp_unload(mon, pmp);
+	return p;
 }
 
-err_t sys_mon_pmp_unload(proc_t *p, const sys_args_t *args, uint64_t *ret)
+err_t validate_sock_send(const sys_args_t *args)
 {
-	cte_t mon = ctable_get(p->pid, args->mon_pmp.mon_idx);
-	cte_t pmp = ctable_get(args->mon_pmp.pid, args->mon_pmp.pmp_idx);
-	return cap_monitor_pmp_unload(mon, pmp);
+	if (!valid_idx(args->sock.sock_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_idx(args->sock.cap_idx))
+		return ERR_INVALID_INDEX;
+	return SUCCESS;
 }
 
-err_t sys_sock_send(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_sock_send(proc_t *const p, const sys_args_t *args)
 {
 	cte_t sock = ctable_get(p->pid, args->sock.sock_idx);
 	const ipc_msg_t msg = {
-	    .src_buf = ctable_get(p->pid, args->sock.cap_idx),
+	    .cap_buf = ctable_get(p->pid, args->sock.cap_idx),
 	    .send_cap = args->sock.send_cap,
-	    .data = {args->sock.data[0], args->sock.data[1], args->sock.data[1],
+	    .data = {args->sock.data[0], args->sock.data[1], args->sock.data[2],
 		     args->sock.data[3]},
 	};
-	return cap_sock_send(sock, &msg, (proc_t **)ret);
+	proc_t *next = p;
+	p->regs[REG_T0] = cap_sock_send(sock, &msg, &next);
+	return next;
+}
+
+err_t validate_sock_recv(const sys_args_t *args)
+{
+	if (!valid_idx(args->sock.sock_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_idx(args->sock.cap_idx))
+		return ERR_INVALID_INDEX;
+	return SUCCESS;
 }
 
-err_t sys_sock_recv(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_sock_recv(proc_t *const p, const sys_args_t *args)
 {
 	cte_t sock = ctable_get(p->pid, args->sock.sock_idx);
-	p->cap_buf = ctable_get(p->pid, args->sock.cap_idx);
-	return cap_sock_recv(sock);
+	cte_t cap_buf = ctable_get(p->pid, args->sock.cap_idx);
+	proc_t *next = p;
+	p->regs[REG_T0] = cap_sock_recv(sock, cap_buf, &next);
+	return next;
+}
+
+err_t validate_sock_sendrecv(const sys_args_t *args)
+{
+	if (!valid_idx(args->sock.sock_idx))
+		return ERR_INVALID_INDEX;
+	if (!valid_idx(args->sock.cap_idx))
+		return ERR_INVALID_INDEX;
+	return SUCCESS;
 }
 
-err_t sys_sock_sendrecv(proc_t *p, const sys_args_t *args, uint64_t *ret)
+proc_t *handle_sock_sendrecv(proc_t *const p, const sys_args_t *args)
 {
 	cte_t sock = ctable_get(p->pid, args->sock.sock_idx);
-	p->cap_buf = ctable_get(p->pid, args->sock.cap_idx);
 	const ipc_msg_t msg = {
-	    .src_buf = ctable_get(p->pid, args->sock.cap_idx),
+	    .cap_buf = ctable_get(p->pid, args->sock.cap_idx),
 	    .send_cap = args->sock.send_cap,
-	    .data = {args->sock.data[0], args->sock.data[1], args->sock.data[1],
+	    .data = {args->sock.data[0], args->sock.data[1], args->sock.data[2],
 		     args->sock.data[3]},
 	};
-	return cap_sock_sendrecv(sock, &msg, (proc_t **)ret);
+	proc_t *next = p;
+	p->regs[REG_T0] = cap_sock_sendrecv(sock, &msg, &next);
+	return next;
 }
diff --git a/kernel/src/trap.S b/kernel/src/trap.S
index a20cbcac..38169f49 100644
--- a/kernel/src/trap.S
+++ b/kernel/src/trap.S
@@ -4,104 +4,145 @@
 #include "csr.h"
 
 .globl trap_entry
+.globl trap_exit_pmp
 .globl trap_exit
-.globl trap_resume
+.globl trap_sched
 
 .type trap_entry, @function
 .type trap_exit, @function
 .type trap_resume, @function
+.type trap_sched, @function
 
 .section .text.trap
 .balign 16
 trap_entry:
-	/* Save user tp to scratch, load PCB pointer */
-	csrrw	a0,mscratch,a0
-	beqz	a0,_machine_yield
-
-	sd	ra,PROC_RA(a0)
-	sd	sp,PROC_SP(a0)
-	sd	gp,PROC_GP(a0)
-	sd	tp,PROC_TP(a0)
-	sd	t0,PROC_T0(a0)
-	sd	t1,PROC_T1(a0)
-	sd	t2,PROC_T2(a0)
-	sd	s0,PROC_S0(a0)
-	sd	s1,PROC_S1(a0)
-	/*sd	a0,PROC_A0(a0)*/
-	sd	a1,PROC_A1(a0)
-	sd	a2,PROC_A2(a0)
-	sd	a3,PROC_A3(a0)
-	sd	a4,PROC_A4(a0)
-	sd	a5,PROC_A5(a0)
-	sd	a6,PROC_A6(a0)
-	sd	a7,PROC_A7(a0)
-	sd	s2,PROC_S2(a0)
-	sd	s3,PROC_S3(a0)
-	sd	s4,PROC_S4(a0)
-	sd	s5,PROC_S5(a0)
-	sd	s6,PROC_S6(a0)
-	sd	s7,PROC_S7(a0)
-	sd	s8,PROC_S8(a0)
-	sd	s9,PROC_S9(a0)
-	sd	s10,PROC_S10(a0)
-	sd	s11,PROC_S11(a0)
-	sd	t3,PROC_T3(a0)
-	sd	t4,PROC_T4(a0)
-	sd	t5,PROC_T5(a0)
-	sd	t6,PROC_T6(a0)
+	// Save user a0 to scratch, load PCB pointer
+	csrrw	tp,mscratch,tp
+	beqz	tp,_machine_yield
+
+	sd	ra,PROC_RA(tp)
+	sd	sp,PROC_SP(tp)
+	sd	gp,PROC_GP(tp)
+	// sd	tp,PROC_TP(tp)
+	sd	t0,PROC_T0(tp)
+	sd	t1,PROC_T1(tp)
+	sd	t2,PROC_T2(tp)
+	sd	s0,PROC_S0(tp)
+	sd	s1,PROC_S1(tp)
+	sd	a0,PROC_A0(tp)
+	sd	a1,PROC_A1(tp)
+	sd	a2,PROC_A2(tp)
+	sd	a3,PROC_A3(tp)
+	sd	a4,PROC_A4(tp)
+	sd	a5,PROC_A5(tp)
+	sd	a6,PROC_A6(tp)
+	sd	a7,PROC_A7(tp)
+	sd	s2,PROC_S2(tp)
+	sd	s3,PROC_S3(tp)
+	sd	s4,PROC_S4(tp)
+	sd	s5,PROC_S5(tp)
+	sd	s6,PROC_S6(tp)
+	sd	s7,PROC_S7(tp)
+	sd	s8,PROC_S8(tp)
+	sd	s9,PROC_S9(tp)
+	sd	s10,PROC_S10(tp)
+	sd	s11,PROC_S11(tp)
+	sd	t3,PROC_T3(tp)
+	sd	t4,PROC_T4(tp)
+	sd	t5,PROC_T5(tp)
+	sd	t6,PROC_T6(tp)
 	csrr	t1,mepc
-	sd	t1,PROC_PC(a0)
+	sd	t1,PROC_PC(tp)
 	csrrw	t2,mscratch,zero
-	sd	t2,PROC_A0(a0)
+	sd	t2,PROC_TP(tp)
 
 
-	/* Load the global and stack pointer of the kernel. */
+	// Load the global and stack pointer of the kernel.
 	ld_gp
 	ld_sp	t0
 
 	csrr	t0,mcause
-	bltz	t0,_yield
-
-	csrw	mstatus,MSTATUS_MIE
-
-	li	t1,MCAUSE_USER_ECALL
-	beq	t0,t1,_syscall
-
-_exception:
-	/* Otherwise, it is exception. */
-	csrr	a1,mcause
-	csrr	a2,mepc
-	csrr	a3,mtval
-	csrw	mstatus,x0
-	tail	handle_exception
-
-_syscall:
-	csrw	mstatus,x0
-	tail	handle_syscall
+	bltz	t0,trap_sched
+
+#ifndef NPREEMPT
+	csrs	mstatus,MSTATUS_MIE
+#endif
+
+trap_exception:
+	// Load handler address.
+1:	auipc	ra,%pcrel_hi(_exception_table)
+	slli	t0,t0,2
+	add	ra,ra,t0
+#ifndef NPREEMPT
+	// Disable preemption.
+	csrc	mstatus,MSTATUS_MIE	
+#endif
+
+	// Call handler.
+	jalr	ra,%pcrel_lo(1b)(ra)	
+
+	beq	a0,tp,trap_resume
+
+	// Release old process.
+	li	t1,~1
+	amoand.d.aqrl	x0,t1,(tp)
+	beqz	a0,_sched
+	j	trap_exit
+
+_exception_table:
+.option push
+.option norvc
+	j	handle_exception
+	j	handle_exception
+	j	handle_illegal_instruction
+	j	handle_exception
+	j	handle_exception
+	j	handle_exception
+	j	handle_exception
+	j	handle_exception
+	j	handle_syscall
+	j	handle_exception
+	j	handle_exception
+	j	handle_exception
+	j	handle_exception
+	j	handle_exception
+	j	handle_exception
+	j	handle_exception
+.option pop
 
 _machine_yield:
-	csrrw	a0,mscratch,a0
-	ld_sp	t0
+	csrrw	tp,mscratch,tp
+#ifndef NDEBUG
 	csrr	t0,mcause
-
-_yield:
 	li	t1,0x8000000000000007
 	bne	t0,t1,__hang
-	/* Call scheduler */
-	tail	sched
+#endif
+	csrw	mstatus,0
+
+trap_sched:
+	// Release old process.
+	li	t1,~1
+	amoand.d.aqrl	x0,t1,(tp)
+
+	// Call scheduler.
+_sched:
+	call	sched
 
 trap_exit:
-	csrw	mstatus,MSTATUS_MIE
-	/* Load PMP registers */
-	ld	s0,PROC_PMPADDR0(a0)
-	ld	s1,PROC_PMPADDR1(a0)
-	ld	s2,PROC_PMPADDR2(a0)
-	ld	s3,PROC_PMPADDR3(a0)
-	ld	s4,PROC_PMPADDR4(a0)
-	ld	s5,PROC_PMPADDR5(a0)
-	ld	s6,PROC_PMPADDR6(a0)
-	ld	s7,PROC_PMPADDR7(a0)
-	ld	s8,PROC_PMPCFG0(a0)
+	mv	tp,a0
+#ifndef PREEMPT
+	// Enable preemption
+	csrs	mstatus,MSTATUS_MIE
+#endif
+	ld	s0,PROC_PMPADDR0(tp)
+	ld	s1,PROC_PMPADDR1(tp)
+	ld	s2,PROC_PMPADDR2(tp)
+	ld	s3,PROC_PMPADDR3(tp)
+	ld	s4,PROC_PMPADDR4(tp)
+	ld	s5,PROC_PMPADDR5(tp)
+	ld	s6,PROC_PMPADDR6(tp)
+	ld	s7,PROC_PMPADDR7(tp)
+	ld	s8,PROC_PMPCFG0(tp)
 	csrw	pmpaddr0,s0
 	csrw	pmpaddr1,s1
 	csrw	pmpaddr2,s2
@@ -111,57 +152,59 @@ trap_exit:
 	csrw	pmpaddr6,s6
 	csrw	pmpaddr7,s7
 	csrw	pmpcfg0,s8
-	beqz	s8,trap_exit
-trap_resume:
-	csrw	mstatus,MSTATUS_MIE
 
-	/* Load call-used registers, ra, sp, gp, tp, pc */
-	ld	t0,PROC_PC(a0)
+trap_resume:
+#ifndef PREEMPT
+	csrs	mstatus,MSTATUS_MIE
+#endif
+	ld	t0,PROC_PC(tp)
 	csrw	mepc,t0
 
-	ld	ra,PROC_RA(a0)
-	ld	sp,PROC_SP(a0)
-	ld	gp,PROC_GP(a0)
-	ld	tp,PROC_TP(a0)
-	ld	t0,PROC_T0(a0)
-	ld	t1,PROC_T1(a0)
-	ld	t2,PROC_T2(a0)
-	ld	s0,PROC_S0(a0)
-	ld	s1,PROC_S1(a0)
-	/*ld	a0,PROC_A0(a0) */
-	ld	a1,PROC_A1(a0)
-	ld	a2,PROC_A2(a0)
-	ld	a3,PROC_A3(a0)
-	ld	a4,PROC_A4(a0)
-	ld	a5,PROC_A5(a0)
-	ld	a6,PROC_A6(a0)
-	ld	a7,PROC_A7(a0)
-	ld	s2,PROC_S2(a0)
-	ld	s3,PROC_S3(a0)
-	ld	s4,PROC_S4(a0)
-	ld	s5,PROC_S5(a0)
-	ld	s6,PROC_S6(a0)
-	ld	s7,PROC_S7(a0)
-	ld	s8,PROC_S8(a0)
-	ld	s9,PROC_S9(a0)
-	ld	s10,PROC_S10(a0)
-	ld	s11,PROC_S11(a0)
-	ld	t3,PROC_T3(a0)
-	ld	t4,PROC_T4(a0)
-	ld	t5,PROC_T5(a0)
-	ld	t6,PROC_T6(a0)
-
-	/* disable interrupt */
-	csrw	mstatus,0
-
-	/* Save PCB pointer */
-	csrw	mscratch,a0
-
-	/* Load user tp */
-	ld	a0,PROC_A0(a0)
+	ld	ra,PROC_RA(tp)
+	// ld	sp,PROC_SP(tp)
+	// ld	gp,PROC_GP(tp)
+	// ld	tp,PROC_TP(tp)
+	ld	t0,PROC_T0(tp)
+	ld	t1,PROC_T1(tp)
+	ld	t2,PROC_T2(tp)
+	ld	s0,PROC_S0(tp)
+	ld	s1,PROC_S1(tp)
+	ld	a0,PROC_A0(tp)
+	ld	a1,PROC_A1(tp)
+	ld	a2,PROC_A2(tp)
+	ld	a3,PROC_A3(tp)
+	ld	a4,PROC_A4(tp)
+	ld	a5,PROC_A5(tp)
+	ld	a6,PROC_A6(tp)
+	ld	a7,PROC_A7(tp)
+	ld	s2,PROC_S2(tp)
+	ld	s3,PROC_S3(tp)
+	ld	s4,PROC_S4(tp)
+	ld	s5,PROC_S5(tp)
+	ld	s6,PROC_S6(tp)
+	ld	s7,PROC_S7(tp)
+	ld	s8,PROC_S8(tp)
+	ld	s9,PROC_S9(tp)
+	ld	s10,PROC_S10(tp)
+	ld	s11,PROC_S11(tp)
+	ld	t3,PROC_T3(tp)
+	ld	t4,PROC_T4(tp)
+	ld	t5,PROC_T5(tp)
+	ld	t6,PROC_T6(tp)
+
+	// Disable interrupts.
+	csrc	mstatus,MSTATUS_MIE
+
+	// Save PCB pointer.
+	csrw	mscratch,tp
+
+	// Load user sp, gp and tp.
+	ld	sp,PROC_SP(tp)
+	ld	gp,PROC_GP(tp)
+	ld	tp,PROC_TP(tp)
 
 	mret
 
 __hang:
-	ebreak
+	nop
 	j __hang
diff --git a/projects/trapped/build.mk b/projects/build.mk
similarity index 74%
rename from projects/trapped/build.mk
rename to projects/build.mk
index 7cb8d249..4a752703 100644
--- a/projects/trapped/build.mk
+++ b/projects/build.mk
@@ -1,4 +1,5 @@
 .POSIX:
+.SECONDARY:
 
 BUILD   ?=build
 PROGRAM ?=a
@@ -38,24 +39,30 @@ clean:
 
 ${BUILD}/${PROGRAM}/%.o: ${PROGRAM}/%.S
 	@mkdir -p ${@D}
-	${CC} -o $@ $< ${CFLAGS} ${INC} -MMD -c
+	@${CC} -o $@ $< ${CFLAGS} ${INC} -MMD -c
+	@printf "CC\t$@\n"
 
 ${BUILD}/${PROGRAM}/%.o: ${PROGRAM}/%.c
 	@mkdir -p ${@D}
-	${CC} -o $@ $< ${CFLAGS} ${INC} -MMD -c
+	@${CC} -o $@ $< ${CFLAGS} ${INC} -MMD -c
+	@printf "CC\t$@\n"
 
 %.elf: ${OBJS}
 	@mkdir -p ${@D}
-	${CC} -o $@ ${OBJS} ${LDFLAGS} ${INC}
+	@${CC} -o $@ ${OBJS} ${LDFLAGS} ${INC}
+	@printf "CC\t$@\n"
 
 %.bin: %.elf
-	${OBJCOPY} -O binary $< $@
+	@${OBJCOPY} -O binary $< $@
+	@printf "OBJCOPY\t$@\n"
 
 %.hex: %.elf
-	${OBJCOPY} -O ihex $< $@
+	@${OBJCOPY} -O ihex $< $@
+	@printf "OBJCOPY\t$@\n"
 
 %.da: %.elf
-	${OBJDUMP} -D $< > $@
+	@${OBJDUMP} -D $< > $@
+	@printf "OBJDUMP\t$@\n"
 
 .PHONY: all clean
 
diff --git a/projects/demonstrator/Makefile b/projects/demonstrator/Makefile
index 650df38f..6504ee56 100644
--- a/projects/demonstrator/Makefile
+++ b/projects/demonstrator/Makefile
@@ -1,10 +1,12 @@
 .POSIX:
+.SECONDARY:
 
 export PLATFORM   ?=qemu_virt
 export ROOT       :=${abspath ../..}
 export BUILD      :=${abspath build/${PLATFORM}}
 export S3K_CONF_H :=${abspath s3k_conf.h}
 
+include ${ROOT}/tools.mk
 include ${ROOT}/common/plat/${PLATFORM}.mk
 
 APPS=boot app0 app1 monitor crypto uartppp
@@ -14,11 +16,7 @@ ELFS:=${patsubst %,${BUILD}/%.elf,kernel boot}
 all: kernel ${APPS}
 
 clean:
-	@${MAKE} -C ${ROOT}/common clean
-	@${MAKE} -C ${ROOT}/kernel clean
-	@for prog in ${APPS}; do \
-		${MAKE} -f build.mk PROGRAM=$$prog clean; \
-		done
+	rm -r ${BUILD}
 
 common:
 	@${MAKE} -C ${ROOT}/common
@@ -28,7 +26,7 @@ kernel: common
 
 boot: monitor crypto uartppp
 ${APPS}:
-	@${MAKE} -f build.mk PROGRAM=$@
+	@${MAKE} -f ../build.mk PROGRAM=$@
 
 qemu: kernel ${APPS}
 	@ELFS="${ELFS}" ${ROOT}/scripts/qemu.sh -serial tcp:localhost:8888,server,nowait
diff --git a/projects/demonstrator/boot/main.c b/projects/demonstrator/boot/main.c
index d17b53da..c59cc2fb 100644
--- a/projects/demonstrator/boot/main.c
+++ b/projects/demonstrator/boot/main.c
@@ -1,7 +1,7 @@
 #include "../config.h"
 #include "altc/altio.h"
+#include "altc/string.h"
 #include "s3k/s3k.h"
-#include "string.h"
 
 #include <stddef.h>
 
@@ -164,18 +164,18 @@ void start_proc(void)
 	s3k_mon_resume(MONITOR_CIDX, CRYPTO_PID);
 	while (s3k_mon_yield(MONITOR_CIDX, CRYPTO_PID))
 		;
-	alt_puts("crypto started");
+	alt_puts("{crypto started}");
 
 	s3k_mon_resume(MONITOR_CIDX, MONITOR_PID);
 	while (s3k_mon_yield(MONITOR_CIDX, MONITOR_PID))
 		;
-	alt_puts("monitor started");
+	alt_puts("{monitor started}");
 
 	s3k_mon_resume(MONITOR_CIDX, UART_PID);
 	while (s3k_mon_yield(MONITOR_CIDX, UART_PID))
 		;
 	// UART outputs "uart started"
-	alt_puts("error");
+	alt_puts("{error}");
 	while (1)
 		;
 }
@@ -197,7 +197,7 @@ void main(void)
 	s3k_pmp_load(5, 1);
 	s3k_sync_mem();
 
-	alt_puts("setting up memory ...");
+	alt_puts("{setting up memory ...}");
 	/* Copy binary of monitor process, setup PMP and program counter. */
 	setup_bin(MONITOR_PID, MONITOR_MEM, MONITOR_MEM_LEN, monitor_bin,
 		  monitor_bin_len);
@@ -213,15 +213,15 @@ void main(void)
 	/* Give monitor access to application memory */
 	setup_app_mem();
 
-	alt_puts("setting up ipc");
+	alt_puts("{setting up ipc}");
 	setup_uart_ipc(0, S3K_IPC_NOYIELD, S3K_IPC_CDATA | S3K_IPC_SDATA);
 	setup_crypto_ipc(1, S3K_IPC_NOYIELD, S3K_IPC_CDATA | S3K_IPC_SDATA);
 	setup_app_channels();
 	setup_app_monitoring();
 
-	alt_puts("setting up time");
+	alt_puts("{setting up time}");
 	setup_time();
 
-	alt_puts("starting processes");
+	alt_puts("{starting processes}");
 	start_proc();
 }
diff --git a/projects/demonstrator/boot/payload.S b/projects/demonstrator/boot/payload.S
index f2357298..ebb36435 100644
--- a/projects/demonstrator/boot/payload.S
+++ b/projects/demonstrator/boot/payload.S
@@ -1,15 +1,24 @@
+
+
 .globl monitor_bin, monitor_bin_len
 .globl crypto_bin, crypto_bin_len
 .globl uart_bin, uart_bin_len
 
 .section .data
 
+#define STR(x) #x
+#if defined(PLATFORM_qemu_virt)
+#define PATH(x) STR(build/qemu_virt/x.bin)
+#elif defined(PLATFORM_sifive_unleashed)
+#define PATH(x) STR(build/sifive_unleashed/x.bin)
+#endif
+
 monitor_bin:
-.incbin "build/monitor.bin"
+.incbin PATH(monitor)
 crypto_bin:
-.incbin "build/crypto.bin"
+.incbin PATH(crypto)
 uart_bin:
-.incbin "build/uartppp.bin"
+.incbin PATH(uartppp)
 end_bin:
 
 monitor_bin_len:
diff --git a/projects/demonstrator/build.mk b/projects/demonstrator/build.mk
deleted file mode 100644
index 7cb8d249..00000000
--- a/projects/demonstrator/build.mk
+++ /dev/null
@@ -1,62 +0,0 @@
-.POSIX:
-
-BUILD   ?=build
-PROGRAM ?=a
-
-include ${ROOT}/tools.mk
-include ${ROOT}/common/plat/${PLATFORM}.mk
-
-C_SRCS:=${wildcard ${PROGRAM}/*.c}
-S_SRCS:=${wildcard ${PROGRAM}/*.S}
-OBJS  :=${patsubst %.c,${BUILD}/%.o,${C_SRCS}} \
-	${patsubst %.S,${BUILD}/%.o,${S_SRCS}} \
-	${STARTFILES}/start.o
-DEPS  :=${OBJS:.o=.d}
-
-CFLAGS:=-march=${ARCH} -mabi=${ABI} -mcmodel=${CMODEL} \
-	-DPLATFORM_${PLATFORM} \
-	-nostdlib \
-	-Os -g3 -flto \
-	-I${COMMON_INC} -include ${S3K_CONF_H}
-
-LDFLAGS:=-march=${ARCH} -mabi=${ABI} -mcmodel=${CMODEL} \
-	 -nostdlib \
-	 -flto \
-	 -T${PROGRAM}.ld -Tdefault.ld \
-	 -Wl,--no-warn-rwx-segments \
-	 -L${COMMON_LIB} -ls3k -laltc -lplat \
-
-ELF:=${BUILD}/${PROGRAM}.elf
-BIN:=${ELF:.elf=.bin}
-HEX:=${ELF:.elf=.hex}
-DA :=${ELF:.elf=.da}
-
-all: ${ELF} ${BIN} ${HEX} ${DA}
-
-clean:
-	rm -f ${ELF} ${OBJS} ${DEPS}
-
-${BUILD}/${PROGRAM}/%.o: ${PROGRAM}/%.S
-	@mkdir -p ${@D}
-	${CC} -o $@ $< ${CFLAGS} ${INC} -MMD -c
-
-${BUILD}/${PROGRAM}/%.o: ${PROGRAM}/%.c
-	@mkdir -p ${@D}
-	${CC} -o $@ $< ${CFLAGS} ${INC} -MMD -c
-
-%.elf: ${OBJS}
-	@mkdir -p ${@D}
-	${CC} -o $@ ${OBJS} ${LDFLAGS} ${INC}
-
-%.bin: %.elf
-	${OBJCOPY} -O binary $< $@
-
-%.hex: %.elf
-	${OBJCOPY} -O ihex $< $@
-
-%.da: %.elf
-	${OBJDUMP} -D $< > $@
-
-.PHONY: all clean
-
--include ${DEPS}
diff --git a/projects/demonstrator/default.ld b/projects/demonstrator/default.ld
index 19f95cbe..d169cad6 100644
--- a/projects/demonstrator/default.ld
+++ b/projects/demonstrator/default.ld
@@ -26,7 +26,7 @@ SECTIONS {
 		*(.bss .bss.*)
 	} > RAM
 
-	.stach : ALIGN(8) {
+	.stack : ALIGN(8) {
 		. += __stack_size;
 		__stack_pointer = .;
 		_end = .;
diff --git a/projects/demonstrator/scripts/client.py b/projects/demonstrator/scripts/client.py
index 8cb83aa9..3d43d018 100755
--- a/projects/demonstrator/scripts/client.py
+++ b/projects/demonstrator/scripts/client.py
@@ -21,7 +21,8 @@
 CMD_RESET = 0x5
 
 # Initializing serial connection
-ser = serial.Serial("/dev/ttyUSB1", 115200)
+#ser = serial.Serial("/dev/ttyUSB1", 115200)
+ser = serial.serial_for_url("socket://localhost:8888")
 
 # Function for sending data with PPP encoding
 def ppp_send(data, file=sys.stdout.buffer):
@@ -31,16 +32,20 @@ def ppp_send(data, file=sys.stdout.buffer):
     :param data: The data to be sent, as bytes.
     :param file: The file-like object where data will be written. Defaults to sys.stdout.buffer.
     """
+    i = 2
     file.write(PPP_BGN)
     for c in data:
         byte = c.to_bytes(1, byteorder="little")
         if byte in [PPP_ESC, PPP_BGN, PPP_END]:
             file.write(PPP_ESC)
             file.write((c ^ 0x20).to_bytes(1, byteorder="little"))
+            i += 2
         else:
             file.write(byte)
+            i += 1
     file.write(PPP_END)
     file.flush()
+    print(f"Sent {i} bytes")
 
 # Function for receiving data with PPP encoding
 def ppp_recv(file=sys.stdin.buffer):
diff --git a/projects/demonstrator/uartppp/main.c b/projects/demonstrator/uartppp/main.c
index 4f9b4fb4..c3e45ec7 100644
--- a/projects/demonstrator/uartppp/main.c
+++ b/projects/demonstrator/uartppp/main.c
@@ -12,7 +12,7 @@ size_t ppp_recv(char *buf);
 void main(void)
 {
 	s3k_cap_revoke(0x2);
-	alt_puts("uart started");
+	alt_puts("{uart started}");
 	s3k_msg_t msg;
 	s3k_reply_t reply;
 
@@ -20,6 +20,7 @@ void main(void)
 
 	while (1) {
 		size_t len = ppp_recv(buf);
+		alt_puts("{received message}");
 		msg.data[0] = len;
 		reply = s3k_sock_sendrecv(0x5, &msg);
 		if (reply.err != S3K_SUCCESS) {
@@ -31,7 +32,6 @@ void main(void)
 			ppp_send(s, 6);
 			continue;
 		}
-		alt_printf("{wcet:0x%X}", s3k_get_wcet(true));
 		ppp_send(buf, reply.data[0]);
 	}
 }
diff --git a/projects/hello/Makefile b/projects/hello/Makefile
index 8c0aabe9..040e15ba 100644
--- a/projects/hello/Makefile
+++ b/projects/hello/Makefile
@@ -1,24 +1,18 @@
 .POSIX:
 
-export PLATFORM   ?=qemu_virt
+export PLATFORM   ?=qemu_virt4
 export ROOT       :=${abspath ../..}
 export BUILD      :=${abspath build/${PLATFORM}}
 export S3K_CONF_H :=${abspath s3k_conf.h}
 
-include ${ROOT}/common/plat/${PLATFORM}.mk
-
 APPS=app0 app1
 
-ELFS=${patsubst %, ${BUILD}/%.elf, kernel ${APPS}}
+ELFS:=${patsubst %, ${BUILD}/%.elf, kernel ${APPS}}
 
 all: kernel ${APPS}
 
 clean:
-	@${MAKE} -C ${ROOT}/common clean
-	@${MAKE} -C ${ROOT}/kernel clean
-	@for prog in ${APPS}; do \
-		${MAKE} -f build.mk PROGRAM=$$prog clean; \
-		done
+	rm -rf ${BUILD}
 
 common:
 	@${MAKE} -C ${ROOT}/common
@@ -26,6 +20,9 @@ common:
 kernel: common
 	@${MAKE} -C ${ROOT}/kernel
 
+${APPS}: common
+	@${MAKE} -f ../build.mk PROGRAM=$@
+
 qemu: kernel ${APPS}
 	@ELFS="${ELFS}" ${ROOT}/scripts/qemu.sh
 
@@ -38,7 +35,4 @@ gdb: kernel ${APPS}
 gdb-openocd: kernel ${APPS}
 	@ELFS="${ELFS}" ${ROOT}/scripts/gdb-openocd.sh
 
-${APPS}: common
-	@${MAKE} -f build.mk PROGRAM=$@
-
 .PHONY: all clean qemu qemu-gdb gdb kernel common ${APPS}
diff --git a/projects/hello/app0/main.c b/projects/hello/app0/main.c
index c5daaada..0ecdf1ad 100644
--- a/projects/hello/app0/main.c
+++ b/projects/hello/app0/main.c
@@ -61,8 +61,11 @@ int main(void)
 	// Setup app1 capabilities and PC
 	setup_app1(11);
 
+	char buf[12];
+	alt_snprintf(buf, 64, "hello, world from app%x", 0);
+
 	// Write hello world.
-	alt_puts("hello, world from app0");
+	alt_puts(buf);
 
 	// BYE!
 	alt_puts("bye from app0");
diff --git a/projects/hello/build.mk b/projects/hello/build.mk
deleted file mode 100644
index 7cb8d249..00000000
--- a/projects/hello/build.mk
+++ /dev/null
@@ -1,62 +0,0 @@
-.POSIX:
-
-BUILD   ?=build
-PROGRAM ?=a
-
-include ${ROOT}/tools.mk
-include ${ROOT}/common/plat/${PLATFORM}.mk
-
-C_SRCS:=${wildcard ${PROGRAM}/*.c}
-S_SRCS:=${wildcard ${PROGRAM}/*.S}
-OBJS  :=${patsubst %.c,${BUILD}/%.o,${C_SRCS}} \
-	${patsubst %.S,${BUILD}/%.o,${S_SRCS}} \
-	${STARTFILES}/start.o
-DEPS  :=${OBJS:.o=.d}
-
-CFLAGS:=-march=${ARCH} -mabi=${ABI} -mcmodel=${CMODEL} \
-	-DPLATFORM_${PLATFORM} \
-	-nostdlib \
-	-Os -g3 -flto \
-	-I${COMMON_INC} -include ${S3K_CONF_H}
-
-LDFLAGS:=-march=${ARCH} -mabi=${ABI} -mcmodel=${CMODEL} \
-	 -nostdlib \
-	 -flto \
-	 -T${PROGRAM}.ld -Tdefault.ld \
-	 -Wl,--no-warn-rwx-segments \
-	 -L${COMMON_LIB} -ls3k -laltc -lplat \
-
-ELF:=${BUILD}/${PROGRAM}.elf
-BIN:=${ELF:.elf=.bin}
-HEX:=${ELF:.elf=.hex}
-DA :=${ELF:.elf=.da}
-
-all: ${ELF} ${BIN} ${HEX} ${DA}
-
-clean:
-	rm -f ${ELF} ${OBJS} ${DEPS}
-
-${BUILD}/${PROGRAM}/%.o: ${PROGRAM}/%.S
-	@mkdir -p ${@D}
-	${CC} -o $@ $< ${CFLAGS} ${INC} -MMD -c
-
-${BUILD}/${PROGRAM}/%.o: ${PROGRAM}/%.c
-	@mkdir -p ${@D}
-	${CC} -o $@ $< ${CFLAGS} ${INC} -MMD -c
-
-%.elf: ${OBJS}
-	@mkdir -p ${@D}
-	${CC} -o $@ ${OBJS} ${LDFLAGS} ${INC}
-
-%.bin: %.elf
-	${OBJCOPY} -O binary $< $@
-
-%.hex: %.elf
-	${OBJCOPY} -O ihex $< $@
-
-%.da: %.elf
-	${OBJDUMP} -D $< > $@
-
-.PHONY: all clean
-
--include ${DEPS}
diff --git a/projects/hello/default.ld b/projects/hello/default.ld
index 5ddcfeeb..e32de601 100644
--- a/projects/hello/default.ld
+++ b/projects/hello/default.ld
@@ -27,7 +27,7 @@ SECTIONS {
 		_end = .;
 	} > RAM
 
-	.stach : ALIGN(8) {
+	.stack : ALIGN(8) {
 		. += __stack_size;
 		__stack_pointer = .;
 		_end = .;
diff --git a/projects/hello/s3k_conf.h b/projects/hello/s3k_conf.h
index 1a0d6add..f9893f28 100644
--- a/projects/hello/s3k_conf.h
+++ b/projects/hello/s3k_conf.h
@@ -16,11 +16,11 @@
 #define S3K_SLOT_CNT 32ull
 
 // Length of slots in ticks.
-#define S3K_SLOT_LEN (S3K_RTC_HZ / S3K_SLOT_CNT / 100ull)
+#define S3K_SLOT_LEN (S3K_RTC_HZ / S3K_SLOT_CNT)
 
 // Scheduler time
 #define S3K_SCHED_TIME (S3K_SLOT_LEN / 10)
 
 // If debugging, comment
-#define NDEBUG
-#define INSTRUMENT
+//#define NDEBUG
+#define VERBOSITY 0
diff --git a/projects/ping-pong/Makefile b/projects/ping-pong/Makefile
index 7312cb73..136871ca 100644
--- a/projects/ping-pong/Makefile
+++ b/projects/ping-pong/Makefile
@@ -5,20 +5,14 @@ export ROOT       :=${abspath ../..}
 export BUILD      :=${abspath build/${PLATFORM}}
 export S3K_CONF_H :=${abspath s3k_conf.h}
 
-include ${ROOT}/common/plat/${PLATFORM}.mk
-
 APPS=app0 app1
 
-ELFS:=${patsubst %,${BUILD}/%.elf,kernel ${APPS}}
+ELFS:=${patsubst %, ${BUILD}/%.elf, kernel ${APPS}}
 
 all: kernel ${APPS}
 
 clean:
-	@${MAKE} -C ${ROOT}/common clean
-	@${MAKE} -C ${ROOT}/kernel clean
-	@for prog in ${APPS}; do \
-		${MAKE} -f build.mk PROGRAM=$$prog clean; \
-		done
+	rm -rf ${BUILD}
 
 common:
 	@${MAKE} -C ${ROOT}/common
@@ -26,6 +20,9 @@ common:
 kernel: common
 	@${MAKE} -C ${ROOT}/kernel
 
+${APPS}: common
+	@${MAKE} -f ../build.mk PROGRAM=$@
+
 qemu: kernel ${APPS}
 	@ELFS="${ELFS}" ${ROOT}/scripts/qemu.sh
 
@@ -38,7 +35,4 @@ gdb: kernel ${APPS}
 gdb-openocd: kernel ${APPS}
 	@ELFS="${ELFS}" ${ROOT}/scripts/gdb-openocd.sh
 
-${APPS}: common
-	@${MAKE} -f build.mk PROGRAM=$@
-
 .PHONY: all clean qemu qemu-gdb gdb kernel common ${APPS}
diff --git a/projects/ping-pong/app0/main.c b/projects/ping-pong/app0/main.c
index 469f05e8..dbeb4821 100644
--- a/projects/ping-pong/app0/main.c
+++ b/projects/ping-pong/app0/main.c
@@ -1,6 +1,6 @@
 #include "altc/altio.h"
+#include "altc/string.h"
 #include "s3k/s3k.h"
-#include "string.h"
 
 #define APP0_PID 0
 #define APP1_PID 1
@@ -30,7 +30,7 @@ void setup_uart(uint64_t uart_idx)
 
 void setup_app1(uint64_t tmp)
 {
-	uint64_t uart_addr = s3k_napot_encode(0x10000000, 0x8);
+	uint64_t uart_addr = s3k_napot_encode(UART0_BASE_ADDR, 0x8);
 	uint64_t app1_addr = s3k_napot_encode(0x80020000, 0x10000);
 
 	// Derive a PMP capability for app1 main memory
@@ -44,9 +44,9 @@ void setup_app1(uint64_t tmp)
 	s3k_mon_pmp_load(MONITOR, APP1_PID, 1, 1);
 
 	// derive a time slice capability
-	// s3k_cap_derive(HART0_TIME, tmp, s3k_mk_time(S3K_MIN_HART, 0,
-	// S3K_SLOT_CNT / 2));
-	s3k_mon_cap_move(MONITOR, APP0_PID, HART1_TIME, APP1_PID, 2);
+	s3k_cap_derive(HART0_TIME, tmp,
+		       s3k_mk_time(S3K_MIN_HART, 0, S3K_SLOT_CNT / 2));
+	s3k_mon_cap_move(MONITOR, APP0_PID, tmp, APP1_PID, 2);
 
 	// Write start PC of app1 to PC
 	s3k_mon_reg_write(MONITOR, APP1_PID, S3K_REG_PC, 0x80020000);
@@ -65,6 +65,9 @@ void setup_socket(uint64_t socket, uint64_t tmp)
 
 int main(void)
 {
+	s3k_cap_delete(HART1_TIME);
+	s3k_cap_delete(HART2_TIME);
+	s3k_cap_delete(HART3_TIME);
 	// Setup UART access
 	setup_uart(10);
 
@@ -86,7 +89,7 @@ int main(void)
 	s3k_msg_t msg;
 	s3k_reply_t reply;
 	memcpy(msg.data, "pong", 5);
-	s3k_reg_write(S3K_REG_SERVTIME, 1500);
+	s3k_reg_write(S3K_REG_SERVTIME, 100);
 	while (1) {
 		do {
 			reply = s3k_sock_sendrecv(11, &msg);
diff --git a/projects/ping-pong/app1/main.c b/projects/ping-pong/app1/main.c
index ddd3b5de..b9812f86 100644
--- a/projects/ping-pong/app1/main.c
+++ b/projects/ping-pong/app1/main.c
@@ -1,18 +1,12 @@
 #include "altc/altio.h"
+#include "altc/string.h"
 #include "s3k/s3k.h"
 
 #include <stdint.h>
 
-void *memcpy(void *dest, const void *src, size_t n)
-{
-	for (int i = 0; i < n; ++i) {
-		((char *)dest)[i] = ((char *)src)[i];
-	}
-	return dest;
-}
-
 int main(void)
 {
+	alt_puts("starting app1");
 	s3k_msg_t msg;
 	s3k_reply_t reply;
 	memcpy(msg.data, "ping", 5);
diff --git a/projects/ping-pong/build.mk b/projects/ping-pong/build.mk
deleted file mode 100644
index 7cb8d249..00000000
--- a/projects/ping-pong/build.mk
+++ /dev/null
@@ -1,62 +0,0 @@
-.POSIX:
-
-BUILD   ?=build
-PROGRAM ?=a
-
-include ${ROOT}/tools.mk
-include ${ROOT}/common/plat/${PLATFORM}.mk
-
-C_SRCS:=${wildcard ${PROGRAM}/*.c}
-S_SRCS:=${wildcard ${PROGRAM}/*.S}
-OBJS  :=${patsubst %.c,${BUILD}/%.o,${C_SRCS}} \
-	${patsubst %.S,${BUILD}/%.o,${S_SRCS}} \
-	${STARTFILES}/start.o
-DEPS  :=${OBJS:.o=.d}
-
-CFLAGS:=-march=${ARCH} -mabi=${ABI} -mcmodel=${CMODEL} \
-	-DPLATFORM_${PLATFORM} \
-	-nostdlib \
-	-Os -g3 -flto \
-	-I${COMMON_INC} -include ${S3K_CONF_H}
-
-LDFLAGS:=-march=${ARCH} -mabi=${ABI} -mcmodel=${CMODEL} \
-	 -nostdlib \
-	 -flto \
-	 -T${PROGRAM}.ld -Tdefault.ld \
-	 -Wl,--no-warn-rwx-segments \
-	 -L${COMMON_LIB} -ls3k -laltc -lplat \
-
-ELF:=${BUILD}/${PROGRAM}.elf
-BIN:=${ELF:.elf=.bin}
-HEX:=${ELF:.elf=.hex}
-DA :=${ELF:.elf=.da}
-
-all: ${ELF} ${BIN} ${HEX} ${DA}
-
-clean:
-	rm -f ${ELF} ${OBJS} ${DEPS}
-
-${BUILD}/${PROGRAM}/%.o: ${PROGRAM}/%.S
-	@mkdir -p ${@D}
-	${CC} -o $@ $< ${CFLAGS} ${INC} -MMD -c
-
-${BUILD}/${PROGRAM}/%.o: ${PROGRAM}/%.c
-	@mkdir -p ${@D}
-	${CC} -o $@ $< ${CFLAGS} ${INC} -MMD -c
-
-%.elf: ${OBJS}
-	@mkdir -p ${@D}
-	${CC} -o $@ ${OBJS} ${LDFLAGS} ${INC}
-
-%.bin: %.elf
-	${OBJCOPY} -O binary $< $@
-
-%.hex: %.elf
-	${OBJCOPY} -O ihex $< $@
-
-%.da: %.elf
-	${OBJDUMP} -D $< > $@
-
-.PHONY: all clean
-
--include ${DEPS}
diff --git a/projects/ping-pong/default.ld b/projects/ping-pong/default.ld
index 5ddcfeeb..e32de601 100644
--- a/projects/ping-pong/default.ld
+++ b/projects/ping-pong/default.ld
@@ -27,7 +27,7 @@ SECTIONS {
 		_end = .;
 	} > RAM
 
-	.stach : ALIGN(8) {
+	.stack : ALIGN(8) {
 		. += __stack_size;
 		__stack_pointer = .;
 		_end = .;
diff --git a/projects/ping-pong/s3k_conf.h b/projects/ping-pong/s3k_conf.h
index e931475f..e94b5441 100644
--- a/projects/ping-pong/s3k_conf.h
+++ b/projects/ping-pong/s3k_conf.h
@@ -13,7 +13,7 @@
 #define S3K_CHAN_CNT 2
 
 // Number of slots per period
-#define S3K_SLOT_CNT 64ull
+#define S3K_SLOT_CNT 32ull
 
 // Length of slots in ticks.
 #define S3K_SLOT_LEN (S3K_RTC_HZ / S3K_SLOT_CNT / 100ull)
@@ -22,4 +22,4 @@
 #define S3K_SCHED_TIME (S3K_SLOT_LEN / 10)
 
 // If debugging, comment
-#define NDEBUG
+// #define NDEBUG
diff --git a/projects/ping-pong/scripts/gdb.sh b/projects/ping-pong/scripts/gdb.sh
deleted file mode 100755
index 11faf60b..00000000
--- a/projects/ping-pong/scripts/gdb.sh
+++ /dev/null
@@ -1,31 +0,0 @@
-#!/bin/bash
-
-KERNEL=build/s3k
-APP0=build/app0
-APP1=build/app1
-
-if [[ -z "$TMUX" ]]; then
-	echo "No tmux session"
-	exit 0
-fi
-
-tmux split-window -d "./scripts/qemu.sh -s -S"
-sleep 0.5
-
-riscv64-unknown-elf-gdb				\
-	-ex "set confirm off"			\
-	-ex "set pagination off"		\
-	-ex "set output-radix 16"		\
-	-ex "symbol-file $KERNEL.elf"		\
-	-ex "add-symbol-file $APP0.elf"		\
-	-ex "add-symbol-file $APP1.elf"		\
-	-ex "j 0x80000000"			\
-	-ex "b _hang"				\
-	-ex "b *0x80010000"			\
-	-ex "b *0x80020000"			\
-	-ex "b handle_exception"		\
-	-ex "target remote localhost:1234"	\
-	-ex "layout split"			\
-	-ex "fs cmd"
-
-kill -SIGTERM $(pgrep qemu-system-*)
diff --git a/projects/ping-pong/scripts/qemu.sh b/projects/ping-pong/scripts/qemu.sh
deleted file mode 100755
index 38c08122..00000000
--- a/projects/ping-pong/scripts/qemu.sh
+++ /dev/null
@@ -1,11 +0,0 @@
-#!/bin/bash
-
-KERNEL=build/s3k
-APP0=build/app0
-APP1=build/app1
-
-qemu-system-riscv64 -M virt -smp 4 -m 128M		\
-	-nographic -bios none -kernel $KERNEL.elf	\
-	-device loader,file=$APP0.bin,addr=0x80010000	\
-	-device loader,file=$APP1.bin,addr=0x80020000	\
-	$@
diff --git a/projects/trapped/Makefile b/projects/trapped/Makefile
index 1ed23edd..a96cf6de 100644
--- a/projects/trapped/Makefile
+++ b/projects/trapped/Makefile
@@ -5,20 +5,14 @@ export ROOT       :=${abspath ../..}
 export BUILD      :=${abspath build/${PLATFORM}}
 export S3K_CONF_H :=${abspath s3k_conf.h}
 
-include ${ROOT}/common/plat/${PLATFORM}.mk
-
 APPS=app0
 
-ELFS:=${patsubst %,${BUILD}/%.elf,kernel ${APPS}}
+ELFS:=${patsubst %, ${BUILD}/%.elf, kernel ${APPS}}
 
 all: kernel ${APPS}
 
 clean:
-	@${MAKE} -C ${ROOT}/common clean
-	@${MAKE} -C ${ROOT}/kernel clean
-	@for prog in ${APPS}; do \
-		${MAKE} -f build.mk PROGRAM=$$prog clean; \
-		done
+	rm -rf ${BUILD}
 
 common:
 	@${MAKE} -C ${ROOT}/common
@@ -26,6 +20,9 @@ common:
 kernel: common
 	@${MAKE} -C ${ROOT}/kernel
 
+${APPS}: common
+	@${MAKE} -f ../build.mk PROGRAM=$@
+
 qemu: kernel ${APPS}
 	@ELFS="${ELFS}" ${ROOT}/scripts/qemu.sh
 
@@ -38,7 +35,4 @@ gdb: kernel ${APPS}
 gdb-openocd: kernel ${APPS}
 	@ELFS="${ELFS}" ${ROOT}/scripts/gdb-openocd.sh
 
-${APPS}: common
-	@${MAKE} -f build.mk PROGRAM=$@
-
 .PHONY: all clean qemu qemu-gdb gdb kernel common ${APPS}
diff --git a/projects/trapped/app0/main.c b/projects/trapped/app0/main.c
index 5e6de264..608c91f2 100644
--- a/projects/trapped/app0/main.c
+++ b/projects/trapped/app0/main.c
@@ -63,13 +63,12 @@ void setup_trap(void)
 
 void setup_uart(s3k_cidx_t uart_idx)
 {
-	uint64_t uart_addr = s3k_napot_encode(0x10000000, 0x8);
+	uint64_t uart_addr = s3k_napot_encode(UART0_BASE_ADDR, 0x8);
 	// Derive a PMP capability for accessing UART
 	s3k_cap_derive(UART_MEM, uart_idx, s3k_mk_pmp(uart_addr, S3K_MEM_RW));
 	// Load the derive PMP capability to PMP configuration
 	s3k_pmp_load(uart_idx, 1);
 	// Synchronize PMP unit (hardware) with PMP configuration
-	// false => not full synchronization.
 	s3k_sync_mem();
 }
 
diff --git a/projects/trapped/default.ld b/projects/trapped/default.ld
index 5ddcfeeb..e32de601 100644
--- a/projects/trapped/default.ld
+++ b/projects/trapped/default.ld
@@ -27,7 +27,7 @@ SECTIONS {
 		_end = .;
 	} > RAM
 
-	.stach : ALIGN(8) {
+	.stack : ALIGN(8) {
 		. += __stack_size;
 		__stack_pointer = .;
 		_end = .;
diff --git a/projects/trapped/s3k_conf.h b/projects/trapped/s3k_conf.h
index 1a0d6add..407317df 100644
--- a/projects/trapped/s3k_conf.h
+++ b/projects/trapped/s3k_conf.h
@@ -16,11 +16,11 @@
 #define S3K_SLOT_CNT 32ull
 
 // Length of slots in ticks.
-#define S3K_SLOT_LEN (S3K_RTC_HZ / S3K_SLOT_CNT / 100ull)
+#define S3K_SLOT_LEN (S3K_RTC_HZ / S3K_SLOT_CNT / 1ull)
 
 // Scheduler time
 #define S3K_SCHED_TIME (S3K_SLOT_LEN / 10)
 
 // If debugging, comment
-#define NDEBUG
-#define INSTRUMENT
+//#define NDEBUG
+#define VERBOSITY 1
diff --git a/scripts/qemu.sh b/scripts/qemu.sh
index dcb7961b..040de5fb 100755
--- a/scripts/qemu.sh
+++ b/scripts/qemu.sh
@@ -1,18 +1,63 @@
 #!/usr/bin/env bash
+RED='\033[0;31m'
+YELLOW='\033[0;33m'
+NOCOLOR='\033[0m'
 
+warning () {
+	printf ${RED}"Warning: $1${NOCOLOR}\n"
+}
+
+info () {
+	printf ${YELLOW}"$1${NOCOLOR}\n"
+}
+
+case ${PLATFORM} in
+	qemu_virt)
+		QEMU_MACHINE=virt
+		QEMU_SMP=1
+		QEMU_OPTIONS="-icount 3 $@"
+		warning "Platform qemu_virt uses hart 0 only. If you want multiple cores, use qemu_virt4."
+		;;
+	qemu_virt4)
+		QEMU_MACHINE=virt
+		QEMU_SMP=4
+		QEMU_OPTIONS="$@"
+		;;
+	sifive_unleashed)
+		QEMU_MACHINE=sifive_u
+		QEMU_SMP=5
+		QEMU_OPTIONS="$@"
+		warning "Platform sifive_unleashed uses harts 1, 2, 3, and 4. Hart 0 is present but not unused."
+		;;
+	sifive_unleashed4)
+		QEMU_MACHINE=sifive_u
+		QEMU_SMP=5
+		QEMU_OPTIONS="$@"
+		warning "Platform sifive_unleashed4 uses harts 1.  Harts 0, 2, 3, 4 are present but not unused."
+		;;
+	*)
+		warning "Unknown platform ${PLATFORM}!"
+		exit 1
+esac
+
+
+info "platform: ${PLATFORM}"
+info "elfs: ${ELFS}"
+info "machine: ${QEMU_MACHINE}"
+info "smp: ${QEMU_SMP}"
+info "extra options: ${QEMU_OPTIONS}"
+info "Exit qemu with C-a+x"
+info "Starting qemu ...\n"
 
 LOADER=""
 for elf in $ELFS; do
 	LOADER+="-device loader,file=$elf "
 done
 
-if [[ -z "$QEMU_MACHINE" ]]; then
-	echo "QEMU_MACHINE not set"
-fi
-
-if [[ -z "$QEMU_SMP" ]]; then
-	echo "QEMU_SMP not set"
-fi
-
-qemu-system-riscv64 -M $QEMU_MACHINE -smp $QEMU_SMP -m 128M -nographic -bios none \
-	$LOADER $@
+qemu-system-riscv64 \
+	-M $QEMU_MACHINE \
+	-smp $QEMU_SMP \
+	-m 128M -nographic \
+	-bios none \
+	$LOADER \
+	$QEMU_OPTIONS