diff --git a/Makefile b/Makefile
index 6d6311090..0f5e40644 100644
--- a/Makefile
+++ b/Makefile
@@ -89,7 +89,7 @@ $(INITRAMFS_CPIO): $(INITRAMFS_FILES)
 	cd initramfs; find . | cpio -o -H newc > ../$(INITRAMFS_CPIO)
 
 qemu: all $(INITRAMFS_CPIO) $(RPI3_DTB)
-	qemu-system-aarch64 -M raspi3 -kernel $(BOOTLOADER_IMG) -display none \
+	qemu-system-aarch64 -M raspi3 -kernel $(BOOTLOADER_IMG) \
 						-initrd $(INITRAMFS_CPIO) \
 						-dtb $(RPI3_DTB) \
 						-chardev pty,id=pty0,logfile=pty.log,signal=off \
diff --git a/README.md b/README.md
index e3a8ef038..73dfe5019 100644
--- a/README.md
+++ b/README.md
@@ -35,6 +35,18 @@ make image
 make qemu
 ```
 
+* You should either attach gdb to qemu, or just remove the arugments `-s -S` passed to qemu in the Makefile
+
+* Qemu will emulate the bootloader and you can then attach to the bootloader shell with:
+```
+sudo screen /dev/pts/<pts_idx>
+```
+
+* In the bootloader shell, you can use the command `load` to ask the bootloader to load the kernel image. After entering the `load` command, you can send the kernel image to the bootloader with the following command:
+```
+sudo ./tools/loadkernel.py /dev/pts/<pts_id> build/kernel8.img
+```
+
 ## How to burn it into pi3
 
 ```
diff --git a/include/kernel/cpio.h b/include/kernel/cpio.h
index cc4af40b3..48f70943e 100644
--- a/include/kernel/cpio.h
+++ b/include/kernel/cpio.h
@@ -11,12 +11,12 @@ void cpio_cat(char *cpio, char *filename);
 
 /*
  * Allocate a memory chunk and load the @filename program onto it. Then return
- * the address of the chunk. This memory chunk needs to be passed to kfree() 
- * manually.
+ * the address of the chunk by @output_data. @output_data needs to be passed 
+ * to kfree() manually.
  *
- * Return NULL if failed.
+ * Return output_data length, return 0 if no such file.
  */
-char *cpio_load_prog(char *cpio, char *filename);
+uint32 cpio_load_prog(char *cpio, const char *filename, char **output_data);
 
 void initramfs_init(void);
 
diff --git a/include/kernel/current.h b/include/kernel/current.h
new file mode 100644
index 000000000..c98baf989
--- /dev/null
+++ b/include/kernel/current.h
@@ -0,0 +1,20 @@
+#ifndef _CURRENT_H
+#define _CURRENT_H
+
+#include <utils.h>
+
+struct _task_struct;
+
+static inline struct _task_struct *get_current(void)
+{
+    return (struct _task_struct *)read_sysreg(tpidr_el1);
+}
+
+static inline void set_current(struct _task_struct *task)
+{
+    write_sysreg(tpidr_el1, task);
+}
+
+#define current get_current()
+
+#endif /* _CURRENT_H */
\ No newline at end of file
diff --git a/include/kernel/exec.h b/include/kernel/exec.h
index e95cbf78a..c3f48b435 100644
--- a/include/kernel/exec.h
+++ b/include/kernel/exec.h
@@ -1,8 +1,11 @@
 #ifndef _EXEC_H
 #define _EXEC_H
 
-// Change current EL to EL0 and execute the user program at @mem
-// Set user stack to @user_sp
-void exec_user_prog(char *mem, char *user_sp);
+// TODO: Add argv & envp
+void sched_new_user_prog(char *filename);
+
+void exit_user_prog(void);
+
+void exec_user_prog(void *entry, char *user_sp, char *kernel_sp);
 
 #endif /* _EXEC_H */
\ No newline at end of file
diff --git a/include/kernel/irq.h b/include/kernel/irq.h
index 08e2065b4..3fd4b75e9 100644
--- a/include/kernel/irq.h
+++ b/include/kernel/irq.h
@@ -8,7 +8,10 @@ void irq_init();
 /*
  * On success, 0 is returned
  */
-int irq_add_tasks(void (*task)(void *), void *args, uint32 prio);
+int irq_run_task(void (*task)(void *),
+                 void *args,
+                 void (*fini)(void),
+                 uint32 prio);
 
 void irq_handler();
 void exception_default_handler(uint32 n);
diff --git a/include/kernel/kthread.h b/include/kernel/kthread.h
new file mode 100644
index 000000000..129ba07c2
--- /dev/null
+++ b/include/kernel/kthread.h
@@ -0,0 +1,15 @@
+#ifndef _KTHREAD_H
+#define _KTHREAD_H
+
+#include <task.h>
+
+void kthread_init(void);
+
+void kthread_create(void (*start)(void));
+void kthread_fini(void);
+
+void kthread_add_wait_queue(task_struct *task);
+
+void kthread_kill_zombies(void);
+
+#endif /* _KTHREAD_H */
\ No newline at end of file
diff --git a/include/kernel/mini_uart.h b/include/kernel/mini_uart.h
index 8e643b8be..ef83dedf6 100644
--- a/include/kernel/mini_uart.h
+++ b/include/kernel/mini_uart.h
@@ -6,15 +6,15 @@
 uint32 uart_recvline(char *buff, int maxlen);
 void uart_init(void);
 char uart_recv(void);
+void uart_recvn(char *buff, int n);
 uint32 uart_recv_uint(void);
 void uart_send(char c);
-void uart_sendn(char *str, int n);
-void uart_printf(char *fmt, ...);
+void uart_sendn(const char *str, int n);
+void uart_printf(const char *fmt, ...);
 
-void uart_sync_printf(char *fmt, ...);
+void uart_sync_printf(const char *fmt, ...);
 
-void uart_irq_check(void);
-void uart_irq_handler(void);
+int uart_irq_check(void);
 
 /* Switch asynchronous/synchronous mode for uart RW */
 int uart_switch_mode(void);
diff --git a/include/kernel/mode_switch.h b/include/kernel/mode_switch.h
new file mode 100644
index 000000000..54edbf70c
--- /dev/null
+++ b/include/kernel/mode_switch.h
@@ -0,0 +1,8 @@
+#ifndef _MODE_SWITCH_H
+#define _MODE_SWITCH_H
+
+#include <trapframe.h>
+
+void exit_to_user_mode(trapframe regs);
+
+#endif /* _MODE_SWITCH_H */
\ No newline at end of file
diff --git a/include/kernel/preempt.h b/include/kernel/preempt.h
new file mode 100644
index 000000000..51f3d1920
--- /dev/null
+++ b/include/kernel/preempt.h
@@ -0,0 +1,7 @@
+#ifndef _PREEMPT_H
+#define _PREEMPT_H
+
+void preempt_disable(void);
+void preempt_enable(void);
+
+#endif /* _PREEMPT_H */
\ No newline at end of file
diff --git a/include/kernel/sched.h b/include/kernel/sched.h
new file mode 100644
index 000000000..2cce0fb5d
--- /dev/null
+++ b/include/kernel/sched.h
@@ -0,0 +1,18 @@
+#ifndef _SCHED_H
+#define _SCHED_H
+
+#include <task.h>
+
+void switch_to(task_struct *from, task_struct *to);
+
+void scheduler_init(void);
+
+void schedule(void);
+
+void schedule_tick(void);
+
+void sched_add_task(task_struct *task);
+
+void sched_del_task(task_struct *task);
+
+#endif /* _SCHED_H */
\ No newline at end of file
diff --git a/include/kernel/signal.h b/include/kernel/signal.h
new file mode 100644
index 000000000..4f6bdc108
--- /dev/null
+++ b/include/kernel/signal.h
@@ -0,0 +1,89 @@
+#ifndef _SIGNAL_H
+#define _SIGNAL_H
+
+#include <types.h>
+#include <list.h>
+#include <trapframe.h>
+
+// https://man7.org/linux/man-pages/man7/signal.7.html
+// 0 means no signal
+#define SIGHUP      1
+#define SIGINT      2
+#define SIGQUIT     3
+#define SIGILL      4
+#define SIGTRAP     5
+#define SIGABRT     6
+#define SIGIOT      6
+#define SIGBUS      7
+#define SIGFPE      8
+#define SIGKILL     9
+#define SIGUSR1     10
+#define SIGSEGV     11
+#define SIGUSR2     12
+#define SIGPIPE     13
+#define SIGALRM     14
+#define SIGTERM     15
+#define SIGSTKFLT   16
+#define SIGCHLD     17
+#define SIGCONT     18
+#define SIGSTOP     19
+#define SIGTSTP     20
+#define SIGTTIN     21
+#define SIGTTOU     22
+#define SIGURG      23
+#define SIGXCPU     24
+#define SIGXFSZ     25
+#define SIGVTALRM   26
+#define SIGPROF     27
+#define SIGWINCH    28
+#define SIGIO       29
+#define SIGPOLL     29
+#define SIGPWR      30
+#define SIGSYS      31
+#define SIGUNUSED   31
+
+#define NSIG		32
+
+struct signal_head_t {
+    /* Link signal_t */
+    struct list_head list;
+};
+
+struct signal_t {
+    struct list_head list;
+    uint32 signum;
+};
+
+typedef void (*sighandler_t)(int);
+
+struct sigaction_t {
+    sighandler_t sighand;
+
+    /* flags */
+    uint32 kernel_hand;
+};
+
+struct sighand_t {
+    // 0-th sigaction_t is not used
+    struct sigaction_t sigactions[NSIG];
+};
+
+struct signal_head_t *signal_head_create(void);
+void signal_head_free(struct signal_head_t *head);
+void signal_head_reset(struct signal_head_t *head);
+
+void handle_signal(trapframe *_);
+
+struct sighand_t *sighand_create(void);
+void sighand_free(struct sighand_t *sighand);
+void sighand_reset(struct sighand_t *sighand);
+
+/* Copy current signal handler to @sighand */
+void sighand_copy(struct sighand_t *sighand, void *addrbase);
+
+/* syscalls */
+void syscall_signal(trapframe *_, uint32 signal, void (*handler)(int));
+void syscall_kill(trapframe *_, int pid, int signal);
+void syscall_sigreturn(trapframe *_);
+
+#endif /* _SIGNAL_H */
\ No newline at end of file
diff --git a/include/kernel/syscall.h b/include/kernel/syscall.h
index 0a0649411..f1a7593ac 100644
--- a/include/kernel/syscall.h
+++ b/include/kernel/syscall.h
@@ -2,9 +2,10 @@
 #define _SYSCALL_H
 
 #include <types.h>
+#include <trapframe.h>
 
 // @syn: syndrome information for an synchronous exception
 // One should pass esr_el1 register value to @syn
-void syscall_handler(uint32 syn);
+void syscall_handler(trapframe regs, uint32 syn);
 
 #endif /* _SYSCALL_H */
\ No newline at end of file
diff --git a/include/kernel/task.h b/include/kernel/task.h
new file mode 100644
index 000000000..1720f1455
--- /dev/null
+++ b/include/kernel/task.h
@@ -0,0 +1,75 @@
+#ifndef _TASK_H
+#define _TASK_H
+
+#include <types.h>
+#include <list.h>
+
+#define STACK_SIZE (2 * PAGE_SIZE)
+
+/* Task status */
+#define TASK_NEW        0
+#define TASK_RUNNING    1
+#define TASK_DEAD       2
+
+/* Define in include/kernel/signal.h */
+struct signal_head_t;
+struct sighand_t;
+
+struct pt_regs {
+    void *x19;
+    void *x20;
+    void *x21;
+    void *x22;
+    void *x23;
+    void *x24;
+    void *x25;
+    void *x26;
+    void *x27;
+    void *x28;
+    void *fp;
+    void *lr;
+    void *sp;
+};
+
+typedef struct _task_struct {
+    /* This must be the first element */
+    struct pt_regs regs;
+    void *kernel_stack;
+    void *user_stack;
+    /* TODO: Update to address_space */
+    void *data;
+    uint32 datalen;
+    /* @list is used by run_queue / wait_queue */
+    struct list_head list;
+    /* @task_list links all tasks */
+    struct list_head task_list;
+    uint16 status;
+    uint16 need_resched:1;
+    uint32 tid;
+    uint32 preempt;
+    /* Signal */
+    struct signal_head_t *signal;
+    struct sighand_t *sighand;
+} task_struct;
+
+#define SAVE_REGS(task) \
+    asm volatile (                          \
+        "stp x19, x20, [%x0, 16 * 0]\n"     \
+        "stp x21, x22, [%x0, 16 * 1]\n"     \
+        "stp x23, x24, [%x0, 16 * 2]\n"     \
+        "stp x25, x26, [%x0, 16 * 3]\n"     \
+        "stp x27, x28, [%x0, 16 * 4]\n"     \
+        "stp fp, lr, [%x0, 16 * 5]\n"       \
+        "mov x9, sp\n"                      \
+        "str x9, [%x0, 16 * 6]\n"           \
+        : : "r" (&task->regs)               \
+    );
+
+void task_init(void);
+
+task_struct *task_create(void);
+void task_free(task_struct *task);
+
+task_struct *task_get_by_tid(uint32 tid);
+
+#endif /* _TASK_H */
\ No newline at end of file
diff --git a/include/kernel/timer.h b/include/kernel/timer.h
index 64d13ae89..8253f6ad0 100644
--- a/include/kernel/timer.h
+++ b/include/kernel/timer.h
@@ -4,11 +4,13 @@
 #include <types.h>
 
 void timer_init();
-void timer_irq_check();
-void timer_irq_handler();
+int timer_irq_check();
 void timer_switch_info();
 
 /* Call @proc(@args) after @after seconds. */
-void timer_add_proc(void (*proc)(void *), void *args, uint32 after);
+void timer_add_proc_after(void (*proc)(void *), void *args, uint32 after);
+
+/* Call @proc(@args) after 1/@freq second. */
+void timer_add_proc_freq(void (*proc)(void *), void *args, uint32 freq);
 
 #endif /* _TIMER_H */
\ No newline at end of file
diff --git a/include/kernel/trapframe.h b/include/kernel/trapframe.h
new file mode 100644
index 000000000..4a9cb37b5
--- /dev/null
+++ b/include/kernel/trapframe.h
@@ -0,0 +1,43 @@
+#ifndef _TRAPFRAME_H
+#define _TRAPFRAME_H
+
+#include <types.h>
+
+typedef struct {
+    uint64 x0;
+    uint64 x1;
+    uint64 x2;
+    uint64 x3;
+    uint64 x4;
+    uint64 x5;
+    uint64 x6;
+    uint64 x7;
+    uint64 x8;
+    uint64 x9;
+    uint64 x10;
+    uint64 x11;
+    uint64 x12;
+    uint64 x13;
+    uint64 x14;
+    uint64 x15;
+    uint64 x16;
+    uint64 x17;
+    uint64 x18;
+    uint64 x19;
+    uint64 x20;
+    uint64 x21;
+    uint64 x22;
+    uint64 x23;
+    uint64 x24;
+    uint64 x25;
+    uint64 x26;
+    uint64 x27;
+    uint64 x28;
+    uint64 x29;
+    uint64 x30;
+    void *sp_el0;
+    void *elr_el1;
+    void *spsr_el1;
+} trapframe;
+
+#endif /* _TRAPFRAME_H */
\ No newline at end of file
diff --git a/include/kernel/waitqueue.h b/include/kernel/waitqueue.h
new file mode 100644
index 000000000..8c31735b3
--- /dev/null
+++ b/include/kernel/waitqueue.h
@@ -0,0 +1,19 @@
+#ifndef _WAITQUEUE_H
+#define _WAITQUEUE_H
+
+#include <task.h>
+
+typedef struct {
+    struct list_head list;
+} wait_queue_head;
+
+wait_queue_head *wq_create(void);
+
+int wq_empty(wait_queue_head *head);
+
+void wq_add_task(task_struct *task, wait_queue_head *head);
+void wq_del_task(task_struct *task);
+
+task_struct *wq_get_first_task(wait_queue_head *head);
+
+#endif /* _WAITQUEUE_H */
\ No newline at end of file
diff --git a/include/lib/rpi3.h b/include/lib/rpi3.h
index 1822b096d..90d9ab894 100644
--- a/include/lib/rpi3.h
+++ b/include/lib/rpi3.h
@@ -6,6 +6,7 @@ struct arm_memory_info {
     unsigned int size;
 };
 
+void mailbox_call(unsigned char channel, unsigned int *mb);
 unsigned int get_board_revision(void);
 void get_arm_memory(struct arm_memory_info *);
 
diff --git a/include/lib/string.h b/include/lib/string.h
index 4307927d8..a333c2562 100644
--- a/include/lib/string.h
+++ b/include/lib/string.h
@@ -1,10 +1,10 @@
 #ifndef _STRING_H
 #define _STRING_H
 
-int strcmp(char *str1, char *str2);
-int strncmp(char *str1, char *str2, int n);
-int strlen(char *str);
+int strcmp(const char *str1, const char *str2);
+int strncmp(const char *str1, const char *str2, int n);
+int strlen(const char *str);
 
-int atoi(char *str);
+int atoi(const char *str);
 
 #endif  /* _STRING_H */
\ No newline at end of file
diff --git a/include/lib/utils.h b/include/lib/utils.h
index 6c9547582..3c86f40a3 100644
--- a/include/lib/utils.h
+++ b/include/lib/utils.h
@@ -36,6 +36,21 @@ void memncpy(char *dst, char *src, unsigned long n);
     asm volatile("msr DAIFSet, 0xf"); \
 } while (0)
 
+static inline uint32 save_and_disable_interrupt(void)
+{
+    uint32 daif;
+
+    daif = read_sysreg(DAIF);
+    disable_interrupt();
+
+    return daif;
+}
+
+static inline void restore_interrupt(uint32 daif)
+{
+    write_sysreg(DAIF, daif);
+}
+
 #define get_elem_idx(elem, array) \
     (((char *)elem - (char *)array) / sizeof(array[0]))
 
diff --git a/initramfs/Makefile b/initramfs/Makefile
index 3a1f56b3b..43e87bd49 100644
--- a/initramfs/Makefile
+++ b/initramfs/Makefile
@@ -2,6 +2,10 @@ CROSS_COMPILE ?= aarch64-linux-gnu-
 CC := $(CROSS_COMPILE)gcc
 LD := $(CROSS_COMPILE)ld
 
+CFLAGS := -Wall -nostdlib -nostartfiles -ffreestanding -mgeneral-regs-only
+
+all: userprog1 userprog2
+
 userprog1: userprog1.elf
 	$(CROSS_COMPILE)objcopy -O binary $^ $@
 
@@ -11,6 +15,19 @@ userprog1.elf: linker.ld userprog1.o
 userprog1.o: userprog1.s
 	$(CC) $(CFLAGS) -c $< -o $@
 
+userprog2: userprog2.elf
+	$(CROSS_COMPILE)objcopy -O binary $^ $@
+
+userprog2.elf: linker.ld userprog2.o
+	$(LD) $(LDFLAGS) -T $< -o $@ userprog2.o
+
+userprog2.o: userprog2.c
+	$(CC) $(CFLAGS) -c $< -o $@
+
 .PHONY: clean
 clean:
-	rm -f *.o *.elf
\ No newline at end of file
+	rm -f *.o *.elf
+
+.PHONY: clean-all
+clean-all: clean
+	rm -f userprog1 userprog2
\ No newline at end of file
diff --git a/initramfs/linker.ld b/initramfs/linker.ld
index b525f56a6..ab050c29d 100644
--- a/initramfs/linker.ld
+++ b/initramfs/linker.ld
@@ -1,5 +1,9 @@
 SECTIONS
 {
+  .start : {
+    *(.start)
+  }
+
   .text : {
     *(.text)
   }
diff --git a/initramfs/userprog1 b/initramfs/userprog1
index 409eb763b..237765563 100755
Binary files a/initramfs/userprog1 and b/initramfs/userprog1 differ
diff --git a/initramfs/userprog1.s b/initramfs/userprog1.s
index bd5024d54..1e0ab6b7e 100644
--- a/initramfs/userprog1.s
+++ b/initramfs/userprog1.s
@@ -4,11 +4,16 @@ _start:
     mov x0, 0
 1:
     add x0, x0, 1
-    // syscall_test
+    
+    // syscall_show_info
+    mov x8, 10
     svc 0
+
     cmp x0, 5
     blt 1b
 1:
     // syscall_exit
-    svc 1
-    b 1b
\ No newline at end of file
+    mov x8, 5
+    svc 0
+
+    b 1b
diff --git a/initramfs/userprog2 b/initramfs/userprog2
new file mode 100755
index 000000000..6f524be75
Binary files /dev/null and b/initramfs/userprog2 differ
diff --git a/initramfs/userprog2.c b/initramfs/userprog2.c
new file mode 100644
index 000000000..80529d420
--- /dev/null
+++ b/initramfs/userprog2.c
@@ -0,0 +1,278 @@
+#include <stddef.h>
+#include <stdarg.h>
+
+#define SIGN        1
+
+typedef unsigned long long int uint64;
+typedef unsigned int uint32;
+typedef unsigned short uint16;
+typedef unsigned char uint8;
+
+typedef long long int int64;
+typedef int int32;
+typedef short int16;
+typedef char int8;
+
+#define SYS_GETPID      0
+#define SYS_UART_RECV   1
+#define SYS_UART_WRITE  2
+#define SYS_EXEC        3
+#define SYS_FORK        4
+#define SYS_EXIT        5
+#define SYS_MBOX_CALL   6
+#define SYS_KILL_PID    7
+
+int start(void) __attribute__((section(".start")));
+
+uint64 syscall(uint64 syscall_num,
+               void *x0,
+               void *x1,
+               void *x2,
+               void *x3,
+               void *x4,
+               void *x5)
+{
+    uint64 result;
+
+    asm volatile (
+        "ldr x8, %0\n"
+        "ldr x0, %1\n"
+        "ldr x1, %2\n"
+        "ldr x2, %3\n"
+        "ldr x3, %4\n"
+        "ldr x4, %5\n"
+        "ldr x5, %6\n"
+        "svc 0\n"
+        :: "m" (syscall_num), "m" (x0), "m" (x1), 
+           "m" (x2), "m" (x3), "m" (x4), "m" (x5)
+    );
+
+    asm volatile (
+        "str x0, %0\n"
+        : "=m" (result)
+    );
+
+    return result;
+}
+
+int getpid()
+{
+    return (int)syscall(SYS_GETPID, 0, 0, 0, 0, 0, 0);
+}
+
+void uart_recv(const char buf[], size_t size)
+{
+    syscall(SYS_UART_RECV, (void *)buf, (void *)size, 0, 0, 0, 0);
+}
+
+void uart_write(const char buf[], size_t size)
+{
+    syscall(SYS_UART_WRITE, (void *)buf, (void *)size, 0, 0, 0, 0);
+}
+
+static void uart_send_char(char c)
+{
+    uart_write(&c, 1);
+}
+
+static void uart_send_string(const char *str)
+{
+    for (int i = 0; str[i] != '\0'; i++) {
+        uart_send_char(str[i]);
+    }
+}
+
+static void uart_send_num(int64 num, int base, int type)
+{
+    static const char digits[16] = "0123456789ABCDEF";
+    char tmp[66];
+    int i;
+
+    if (type | SIGN) {
+        if (num < 0) {
+            uart_send_char('-');
+        }
+    }
+
+    i = 0;
+
+    if (num == 0) {
+        tmp[i++] = '0';
+    } else {
+        while (num != 0) {
+            uint8 r = (uint32)num % base;
+            num = (uint32)num / base;
+            tmp[i++] = digits[r];
+        }
+    }
+
+    while (--i >= 0) {
+        uart_send_char(tmp[i]);
+    }
+}
+
+static void _uart_printf(const char *fmt, va_list args)
+{
+    const char *s;
+    char c;
+    uint64 num;
+    char width;
+
+    for (; *fmt; ++fmt) {
+        if (*fmt != '%') {
+            uart_send_char(*fmt);
+            continue;
+        }
+
+        ++fmt;
+
+        // Get width
+        width = 0;
+        if (fmt[0] == 'l' && fmt[1] == 'l') {
+            width = 1;
+            fmt += 2;
+        }
+
+        switch (*fmt) {
+        case 'c':
+            c = va_arg(args, uint32) & 0xff;
+            uart_send_char(c);
+            continue;
+        case 'd':
+            if (width) {
+                num = va_arg(args, int64);
+            } else {
+                num = va_arg(args, int32);
+            }
+            uart_send_num(num, 10, SIGN);
+            continue;
+        case 's':
+            s = va_arg(args, char *);
+            uart_send_string(s);
+            continue;
+        case 'x':
+            if (width) {
+                num = va_arg(args, uint64);
+            } else {
+                num = va_arg(args, uint32);
+            }
+            uart_send_num(num, 16, 0);
+            continue;
+        }
+    }
+}
+
+void uart_printf(const char *fmt, ...)
+{
+    va_list args;
+    va_start(args, fmt);
+
+    _uart_printf(fmt, args);
+
+    va_end(args);
+}
+
+void exec(const char *name, char *const argv[])
+{
+    syscall(SYS_EXEC, (void *)name, (void *)argv, 0, 0, 0, 0);
+}
+
+int fork(void)
+{
+    return (int)syscall(SYS_FORK, 0, 0, 0, 0, 0, 0);
+}
+
+void exit(void)
+{
+    syscall(SYS_EXIT, 0, 0, 0, 0, 0, 0);
+}
+
+void mbox_call(unsigned char ch, unsigned int *mbox)
+{
+    syscall(SYS_MBOX_CALL, (void *)(uint64)ch, mbox, 0, 0, 0, 0);
+}
+
+void kill_pid(int pid)
+{
+    syscall(SYS_KILL_PID, (void *)(uint64)pid, 0, 0, 0, 0, 0);
+}
+
+/* Channels */
+#define MAILBOX_CH_PROP    8
+
+/* Tags */
+#define TAG_REQUEST_CODE    0x00000000
+#define END_TAG             0x00000000
+
+/* Tag identifier */
+#define GET_BOARD_REVISION  0x00010002
+
+/* Others */
+#define REQUEST_CODE        0x00000000
+
+static unsigned int __attribute__((aligned(0x10))) mailbox[16];
+
+// It should be 0xa020d3 for rpi3 b+
+unsigned int get_board_revision(void)
+{
+    mailbox[0] = 7 * 4;              // Buffer size in bytes
+    mailbox[1] = REQUEST_CODE;
+    // Tags begin
+    mailbox[2] = GET_BOARD_REVISION; // Tag identifier
+    mailbox[3] = 4;                  // Value buffer size in bytes
+    mailbox[4] = TAG_REQUEST_CODE;   // Request/response codes
+    mailbox[5] = 0;                  // Optional value buffer
+    // Tags end
+    mailbox[6] = END_TAG;
+
+    mbox_call(MAILBOX_CH_PROP, mailbox); // Message passing procedure call
+
+    return mailbox[5];
+}
+
+void show_stack(void)
+{
+    uint64 sp;
+
+    asm volatile (
+        "mov x9, sp\n"
+        "str x9, %0\n"
+        : "=m" (sp)
+    );
+
+    uart_printf("[User] Stack: %llx\r\n", sp);
+}
+
+int start(void)
+{
+    char buf1[0x10] = { 0 };
+    int pid, revision;
+    
+    pid = getpid();
+    uart_printf("[User] pid: %d\r\n", pid);
+
+    uart_printf("[User] kill_pid(2)\r\n");
+    kill_pid(2);
+
+    uart_printf("[User] Input:\r\n");
+    uart_recv(buf1, 8);
+    uart_printf("[User] Output: %s\r\n", buf1);
+
+    revision = get_board_revision();
+    uart_printf("[User] Revision: %x\r\n", revision);
+
+    pid = fork();
+
+    if (pid == 0) {
+        uart_printf("[User] Child: exec userprog1\r\n");
+        show_stack();
+        exec("userprog1", NULL);
+    } else {
+        uart_printf("[User] Parent: child pid: %d\r\n", pid);
+        show_stack();
+    }
+
+    uart_printf("[User] Exit\r\n");
+    exit();
+    return 0;
+}
\ No newline at end of file
diff --git a/src/kernel/cpio.c b/src/kernel/cpio.c
index 83af0992c..d9a3db95d 100644
--- a/src/kernel/cpio.c
+++ b/src/kernel/cpio.c
@@ -132,7 +132,7 @@ void cpio_cat(char *cpio, char *filename)
     }
 }
 
-char *cpio_load_prog(char *cpio, char *filename)
+uint32 cpio_load_prog(char *cpio, const char *filename, char **output_data)
 {
     char *cur = cpio;
 
@@ -144,7 +144,7 @@ char *cpio_load_prog(char *cpio, char *filename)
         if (*(uint32 *)&pheader->c_magic[0] != 0x37303730 &&
             *(uint16 *)&pheader->c_magic[4] != 0x3130) {
             uart_printf("[*] Only support new ASCII format of cpio.\r\n");
-            return NULL;
+            return 0;
         }
 
         uint32 namesize = _cpio_read_8hex(pheader->c_namesize);
@@ -164,15 +164,15 @@ char *cpio_load_prog(char *cpio, char *filename)
 
         if (!strcmp(filename, curfilename)) {
             // Found it!
-            char *mem = (char *)kmalloc(filesize);
-            memncpy(mem, curcontent, filesize);
-            return mem;
+            *output_data = (char *)kmalloc(filesize);
+            memncpy(*output_data, curcontent, filesize);
+            return filesize;
         }
 
         // TRAILER!!!
         if (namesize == 0xb && !strcmp(curfilename, "TRAILER!!!")) {
             uart_printf("[*] File not found.\r\n");
-            return NULL;
+            return 0;
         }
     } 
 }
diff --git a/src/kernel/exec.S b/src/kernel/exec.S
index f2e03a550..64fb98abe 100644
--- a/src/kernel/exec.S
+++ b/src/kernel/exec.S
@@ -1,4 +1,22 @@
-// See include/kernel/exec.h for function declaration
+// See include/kernel/exec.h and src/kernel/exec.c for function declaration
+
+.globl enter_el0_run_user_prog
+enter_el0_run_user_prog:
+    // Set exception return address
+    msr elr_el1, x0
+
+    // Set user stack
+    msr sp_el0, x1
+
+    // Enable interrupt ({D, A, I, F} = 0 (unmasked))
+    // EL0 ({M[3:0]} = 0)
+    mov x0, 0
+    msr spsr_el1, x0
+
+    // TODO: Clear all general registers
+    
+    // return to EL0
+    eret
 
 .globl exec_user_prog
 exec_user_prog:
@@ -12,6 +30,11 @@ exec_user_prog:
     // EL0 ({M[3:0]} = 0)
     mov x0, 0
     msr spsr_el1, x0
+
+    // Set kernel stack
+    mov sp, x2
+
+    // TODO: Clear all general registers
     
     // return to EL0
     eret
\ No newline at end of file
diff --git a/src/kernel/exec.c b/src/kernel/exec.c
new file mode 100644
index 000000000..fb8b11b6a
--- /dev/null
+++ b/src/kernel/exec.c
@@ -0,0 +1,74 @@
+#include <exec.h>
+#include <task.h>
+#include <cpio.h>
+#include <current.h>
+#include <sched.h>
+#include <kthread.h>
+#include <mm/mm.h>
+
+// Change current EL to EL0 and execute the user program at @entry
+// Set user stack to @user_sp
+void enter_el0_run_user_prog(void *entry, char *user_sp);
+
+static void user_prog_start(void)
+{
+    char *user_sp;
+
+    user_sp = (char *)current->user_stack + STACK_SIZE - 0x10;
+
+    enter_el0_run_user_prog(current->data, user_sp);
+
+    // User program should call exit() to terminate
+}
+
+static inline void pt_regs_init(struct pt_regs *regs)
+{
+    regs->x19 = 0;
+    regs->x20 = 0;
+    regs->x21 = 0;
+    regs->x22 = 0;
+    regs->x23 = 0;
+    regs->x24 = 0;
+    regs->x25 = 0;
+    regs->x26 = 0;
+    regs->x27 = 0;
+    regs->x28 = 0;
+    regs->fp = 0;
+    regs->lr = user_prog_start;
+}
+
+// TODO: Add argv & envp
+void sched_new_user_prog(char *filename)
+{
+    void *data;
+    uint32 datalen;
+    task_struct *task;
+    
+    datalen = cpio_load_prog(initramfs_base, filename, (char **)&data);
+
+    if (datalen == 0) {
+        goto EXEC_USER_PROG_END;
+    }
+
+    task = task_create();
+
+    task->kernel_stack = kmalloc(STACK_SIZE);
+    task->user_stack = kmalloc(STACK_SIZE);
+    task->data = data;
+    task->datalen = datalen;
+
+    task->regs.sp = (char *)task->kernel_stack + STACK_SIZE - 0x10;
+    pt_regs_init(&task->regs);
+
+    sched_add_task(task);
+
+EXEC_USER_PROG_END:
+    return;
+}
+
+void exit_user_prog(void)
+{
+    kthread_fini();
+
+    // Never reach
+}
\ No newline at end of file
diff --git a/src/kernel/head.S b/src/kernel/head.S
index 92fb6fc90..85e704204 100644
--- a/src/kernel/head.S
+++ b/src/kernel/head.S
@@ -105,6 +105,68 @@ set_exception_vector_table:
   add sp, sp, 2 * 8
 .endm
 
+.macro kernel_entry el
+  sub sp, sp, 17 * 16
+  stp x0, x1, [sp ,16 * 0]
+  stp x2, x3, [sp ,16 * 1]
+  stp x4, x5, [sp ,16 * 2]
+  stp x6, x7, [sp ,16 * 3]
+  stp x8, x9, [sp ,16 * 4]
+  stp x10, x11, [sp ,16 * 5]
+  stp x12, x13, [sp ,16 * 6]
+  stp x14, x15, [sp ,16 * 7]
+  stp x16, x17, [sp ,16 * 8]
+  stp x18, x19, [sp ,16 * 9]
+  stp x20, x21, [sp ,16 * 10]
+  stp x22, x23, [sp ,16 * 11]
+  stp x24, x25, [sp ,16 * 12]
+  stp x26, x27, [sp ,16 * 13]
+  stp x28, x29, [sp ,16 * 14]
+
+  .if \el == 0
+  mrs x0, sp_el\el
+  stp x30, x0, [sp, 16 * 15]
+  .else
+  str x30, [sp, 16 * 15]
+  .endif
+
+  mrs x0, elr_el1
+  mrs x1, spsr_el1
+  stp x0, x1, [sp, 16 * 16]
+.endm
+
+.macro kernel_exit el
+  ldp x0, x1, [sp, 16 * 16]
+  msr elr_el1, x0
+  msr spsr_el1, x1
+
+  .if \el ==0
+  ldp x30, x0, [sp, 16 * 15]
+  msr sp_el\el, x0
+  .else
+  ldr x30, [sp, 16 * 15]
+  .endif
+
+  ldp x28, x29, [sp ,16 * 14]
+  ldp x26, x27, [sp ,16 * 13]
+  ldp x24, x25, [sp ,16 * 12]
+  ldp x22, x23, [sp ,16 * 11]
+  ldp x20, x21, [sp ,16 * 10]
+  ldp x18, x19, [sp ,16 * 9]
+  ldp x16, x17, [sp ,16 * 8]
+  ldp x14, x15, [sp ,16 * 7]
+  ldp x12, x13, [sp ,16 * 6]
+  ldp x10, x11, [sp ,16 * 5]
+  ldp x8, x9, [sp ,16 * 4]
+  ldp x6, x7, [sp ,16 * 3]
+  ldp x4, x5, [sp ,16 * 2]
+  ldp x2, x3, [sp ,16 * 1]
+  ldp x0, x1, [sp ,16 * 0]
+  add sp, sp, 17 * 16
+
+  eret
+.endm
+
 exception_handler:
   // Do nothing
   save_all
@@ -116,33 +178,26 @@ exception_handler:
   eret
 
 l64_syn_eh:
-  save_all
-  save_exception_reg
-
-  // Get exception class ({EC[31:26]})
-  mrs x0, esr_el1
-  asr x1, x0, 26
-
-  // SVC instruction execution
-  cmp x1, 0x15
-  bne l64_syn_eh_end
+  kernel_entry 0
 
+  mov x0, sp
+  mrs x1, esr_el1
   bl syscall_handler
 
-l64_syn_eh_end:
-  load_exception_reg
-  load_all
-  eret
+  mov x0, sp
+  bl exit_to_user_mode
+
+  kernel_exit 0
 
 l64_irq_eh:
-  save_all
-  save_exception_reg
+  kernel_entry 0
 
   bl irq_handler
 
-  load_exception_reg
-  load_all
-  eret
+  mov x0, sp
+  bl exit_to_user_mode
+
+  kernel_exit 0
 
 curr_syn_eh:
   save_all
@@ -154,14 +209,11 @@ curr_syn_eh:
   eret
 
 curr_irq_eh:
-  save_all
-  save_exception_reg
+  kernel_entry 1
 
   bl irq_handler
 
-  load_exception_reg
-  load_all
-  eret
+  kernel_exit 1
 
 curr_fiq_eh:
   save_all
diff --git a/src/kernel/irq.c b/src/kernel/irq.c
index 82c349bac..24a04a41c 100644
--- a/src/kernel/irq.c
+++ b/src/kernel/irq.c
@@ -5,6 +5,8 @@
 #include <BCM2837.h>
 #include <list.h>
 #include <bitops.h>
+#include <sched.h>
+#include <current.h>
 
 #define IRQ_TASK_NUM 32
 
@@ -42,6 +44,8 @@ struct list_head irq_tasks_meta;
  */
 uint32 irq_tasks_status;
 
+uint32 irq_nested_layer;
+
 /*
  * Interrupts must be disabled before calling this function.
  */
@@ -175,7 +179,10 @@ void irq_init()
 /*
  * Interrupts must be disabled before calling this function.
  */
-int irq_add_tasks(void (*task)(void *), void *args, uint32 prio)
+int irq_run_task(void (*task)(void *),
+                 void *args,
+                 void (*fini)(void),
+                 uint32 prio)
 {
     irq_task *it;
     int preempt;
@@ -203,6 +210,8 @@ int irq_add_tasks(void (*task)(void *), void *args, uint32 prio)
         
         disable_interrupt();
 
+        (fini)();
+
         it_remove(it);
     }
 
@@ -213,9 +222,26 @@ int irq_add_tasks(void (*task)(void *), void *args, uint32 prio)
 
 void irq_handler()
 {
-    // These check functions may add irq_task.
-    timer_irq_check();
-    uart_irq_check();
+    irq_nested_layer++;
+
+    // These check functions may add irq_task and run it.
+    if (!timer_irq_check()) {}
+    else if (!uart_irq_check()) {}
+
+    irq_nested_layer--;
+
+    // IRQ handling completed
+
+    // Reschedule
+    if (irq_nested_layer || !current->need_resched || current->preempt) {
+        return;
+    }
+
+    enable_interrupt();
+
+    schedule();
+
+    disable_interrupt();
 }
 
 void exception_default_handler(uint32 n)
diff --git a/src/kernel/kthread.c b/src/kernel/kthread.c
new file mode 100644
index 000000000..ebb31b2e9
--- /dev/null
+++ b/src/kernel/kthread.c
@@ -0,0 +1,112 @@
+#include <kthread.h>
+#include <current.h>
+#include <sched.h>
+#include <mm/mm.h>
+#include <waitqueue.h>
+#include <preempt.h>
+
+static wait_queue_head *wait_queue;
+
+static void kthread_start(void)
+{
+    void (*main)(void);
+
+    asm volatile("mov %0, x19\n"
+                 "mov x19, xzr"
+                 : "=r" (main));
+
+    main();
+
+    kthread_fini();
+}
+
+void kthread_add_wait_queue(task_struct *task)
+{
+    wq_add_task(task, wait_queue);
+}
+
+/*
+ * Add to wait_queue to wait some process to recycle this kthread
+ */
+void kthread_fini(void)
+{    
+    preempt_disable();
+
+    current->status = TASK_DEAD;
+
+    sched_del_task(current);
+
+    wq_add_task(current, wait_queue);
+
+    preempt_enable();
+
+    schedule();
+
+    // Never reach
+}
+
+void kthread_init(void)
+{
+    task_struct *task;
+    
+    // Initialze current task
+    task = task_create();
+
+    // Must set current first
+    set_current(task);
+
+    sched_add_task(task);
+
+    // Create wait_queue
+    wait_queue = wq_create();
+}
+
+static inline void pt_regs_init(struct pt_regs *regs, void *main)
+{
+    regs->x19 = main;
+    regs->x20 = 0;
+    regs->x21 = 0;
+    regs->x22 = 0;
+    regs->x23 = 0;
+    regs->x24 = 0;
+    regs->x25 = 0;
+    regs->x26 = 0;
+    regs->x27 = 0;
+    regs->x28 = 0;
+    regs->fp = 0;
+    regs->lr = kthread_start;
+}
+
+void kthread_create(void (*start)(void))
+{
+    task_struct *task;
+    
+    task = task_create();
+
+    task->kernel_stack = kmalloc(STACK_SIZE);
+    task->regs.sp = (char *)task->kernel_stack + STACK_SIZE - 0x10;
+    pt_regs_init(&task->regs, start);
+
+    sched_add_task(task);
+}
+
+void kthread_kill_zombies(void)
+{
+    while (1) {
+        task_struct *task;
+
+        if (wq_empty(wait_queue)) {
+            return;
+        }
+
+        preempt_disable();
+        
+        task = wq_get_first_task(wait_queue);
+
+        wq_del_task(task);
+
+        preempt_enable();
+
+        task_free(task);
+    }
+}
\ No newline at end of file
diff --git a/src/kernel/main.c b/src/kernel/main.c
index 175903749..f309adcc3 100644
--- a/src/kernel/main.c
+++ b/src/kernel/main.c
@@ -9,6 +9,9 @@
 #include <timer.h>
 #include <irq.h>
 #include <mm/mm.h>
+#include <sched.h>
+#include <kthread.h>
+#include <current.h>
 
 #define BUFSIZE 0x100
 
@@ -26,6 +29,15 @@ static void timeout_print(char *str)
     kfree(str);
 }
 
+static void foo(void)
+{
+    for (int i = 0; i < 10; ++i) {
+        uart_sync_printf("Thread id: %d %d\r\n", current->tid, i);
+        delay(1000000);
+        schedule();
+    }
+}
+
 static void cmd_alloc(char *ssize)
 {
     int size, idx;
@@ -89,6 +101,7 @@ static void cmd_help(void)
                     "print @msg after @sec seconds" "\r\n"
                 "sw_timer\t: " "turn on/off timer debug info" "\r\n"
                 "sw_uart_mode\t: " "use sync/async UART" "\r\n"
+                "thread_test\t: " "test kthread" "\r\n"
             );
 }
 
@@ -132,7 +145,7 @@ static void cmd_setTimeout(char *msg, char *ssec)
     memncpy(m, msg, len);
 
     uart_printf("[*] time: %d\r\n", atoi(ssec));
-    timer_add_proc((void (*)(void *))timeout_print, m, atoi(ssec));
+    timer_add_proc_after((void (*)(void *))timeout_print, m, atoi(ssec));
 }
 
 static void cmd_sw_timer(void)
@@ -163,25 +176,8 @@ static void cmd_cat(char *filename)
 
 static void cmd_exec(char *filename)
 {
-    char *mem;
-    char *user_sp;
-
-    mem = cpio_load_prog(initramfs_base, filename);
-
-    if (mem == NULL) {
-        return;
-    }
-
-    user_sp = kmalloc(PAGE_SIZE);
-
-    if (user_sp == NULL) {
-        kfree(mem);
-        return;
-    }
-
-    exec_user_prog(mem, user_sp);
-
-    // TODO: Free user_sp and mem
+    // TODO: Add argv & envp
+    sched_new_user_prog(filename);
 }
 
 static void cmd_parsedtb(void)
@@ -189,6 +185,13 @@ static void cmd_parsedtb(void)
     fdt_traversal(fdt_base);
 }
 
+static void cmd_thread_test(void)
+{
+    for (int i = 0; i < 3; ++i) {
+        kthread_create(foo);
+    }
+}
+
 static int shell_read_cmd(void)
 {
     return uart_recvline(shell_buf, BUFSIZE);
@@ -261,6 +264,8 @@ static void shell(void)
             cmd_ls();
         } else if (!strcmp("parsedtb", shell_buf)) {
             cmd_parsedtb();
+        } else if (!strcmp("thread_test", shell_buf)) {
+            cmd_thread_test();
         } else if (!strncmp("cat", shell_buf, 3)) {
             if (cmd_len >= 5) {
                 cmd_cat(&shell_buf[4]);
@@ -276,26 +281,41 @@ static void shell(void)
     }
 }
 
+static void idle(void)
+{
+    while (1) {
+        kthread_kill_zombies();
+        schedule();
+    }
+}
+
 void start_kernel(char *fdt)
 {
     fdt_base = fdt;
 
     irq_init();
-
     uart_init();
-    uart_printf("[*] fdt base: %x\r\n", fdt_base);
-    uart_printf("[*] Kernel\r\n");
-    
     initramfs_init();
-
     mm_init();
-
     timer_init();
+    task_init();
+    scheduler_init();
+    kthread_init();
+
+    uart_printf("[*] fdt base: %x\r\n", fdt_base);
+    uart_printf("[*] Kernel start!\r\n");
+
+    // TODO: Remove shell?
+    // kthread_create(shell);
+
+    // TODO: Add argv & envp
+    // First user program
+    sched_new_user_prog("syscall.img");
 
     // Enable interrupt from Auxiliary peripherals
     irq1_enable(29);
 
     enable_interrupt();
 
-    shell();
+    idle();
 }
\ No newline at end of file
diff --git a/src/kernel/mini_uart.c b/src/kernel/mini_uart.c
index 2d00a1e13..9e220a84c 100644
--- a/src/kernel/mini_uart.c
+++ b/src/kernel/mini_uart.c
@@ -9,6 +9,9 @@
 
 #define BUFSIZE 0x100
 
+static void uart_irq_handler(void *);
+static void uart_irq_fini(void);
+
 // UART asynchronous/synchronous mode
 // 0: Synchronous mode
 // 1: Asynchronous mode
@@ -85,6 +88,13 @@ void uart_send(char c)
     (uart_send_fp)(c);
 }
 
+void uart_recvn(char *buff, int n)
+{
+    while (n--) {
+        *buff++ = (uart_recv_fp)();
+    }
+}
+
 uint32 uart_recv_uint(void)
 {
     char buf[4];
@@ -123,7 +133,7 @@ uint32 uart_recvline(char *buff, int maxlen)
     return cnt;
 }
 
-void uart_sendn(char *str, int n)
+void uart_sendn(const char *str, int n)
 {
     while (n--) {
         (uart_send_fp)(*str++);
@@ -172,7 +182,7 @@ static void uart_send_num(sendfp _send_fp, int64 num, int base, int type)
 }
 
 // Ref: https://elixir.bootlin.com/linux/v3.5/source/arch/x86/boot/printf.c#L115
-static void _uart_printf(sendfp _send_fp, char *fmt, va_list args)
+static void _uart_printf(sendfp _send_fp, const char *fmt, va_list args)
 {
     const char *s;
     char c;
@@ -223,7 +233,7 @@ static void _uart_printf(sendfp _send_fp, char *fmt, va_list args)
     }
 }
 
-void uart_printf(char *fmt, ...)
+void uart_printf(const char *fmt, ...)
 {
     va_list args;
     va_start(args, fmt);
@@ -233,7 +243,7 @@ void uart_printf(char *fmt, ...)
     va_end(args);
 }
 
-void uart_sync_printf(char *fmt, ...)
+void uart_sync_printf(const char *fmt, ...)
 {
     va_list args;
     va_start(args, fmt);
@@ -275,27 +285,27 @@ void uart_init(void)
     uart_send_fp = uart_sync_send;
 }
 
-void uart_irq_check(void)
+int uart_irq_check(void)
 {
     uint32 iir = get32(AUX_MU_IIR_REG);
 
     if (iir & 0x01) {
         // No interrupt
-        return;
+        return 0;
     }
 
     // Disable RW interrupt
     put32(AUX_MU_IER_REG, 0);
-    if (irq_add_tasks((void (*)(void *))uart_irq_handler, NULL, 1)) {
+    if (irq_run_task(uart_irq_handler, NULL, uart_irq_fini, 1)) {
         put32(AUX_MU_IER_REG, 0x03);
     }
+
+    return 1;
 }
 
-void uart_irq_handler(void)
+static void uart_irq_handler(void *_)
 {
     uint32 iir = get32(AUX_MU_IIR_REG);
-    uint32 ier = get32(AUX_MU_IER_REG);
-    ier = ier & ~(0x03);
 
     if (iir & 0x02) {
         // Transmit holding register empty
@@ -310,6 +320,12 @@ void uart_irq_handler(void)
             r_tail = (r_tail + 1) % BUFSIZE;
         }
     }
+}
+
+static void uart_irq_fini(void)
+{
+    uint32 ier = get32(AUX_MU_IER_REG);
+    ier = ier & ~(0x03);
 
     // Set RW interrupt
     if (r_head != (r_tail + 1) % BUFSIZE) {
diff --git a/src/kernel/mm/mm.c b/src/kernel/mm/mm.c
index 9fef7bbe3..a8388bea3 100644
--- a/src/kernel/mm/mm.c
+++ b/src/kernel/mm/mm.c
@@ -96,11 +96,10 @@ void mm_init(void)
 
 void *kmalloc(int size)
 {
-    uint64 daif;
+    uint32 daif;
     void *ret;
 
-    daif = read_sysreg(DAIF);
-    disable_interrupt();
+    daif = save_and_disable_interrupt();
 
     if (size <= PAGE_SIZE) {
         // Use the Small Chunk allocator
@@ -112,17 +111,16 @@ void *kmalloc(int size)
         ret = alloc_pages(page_cnt);
     }
 
-    write_sysreg(DAIF, daif);
+    restore_interrupt(daif);
 
     return ret;
 }
 
 void kfree(void *ptr)
 {
-    uint64 daif;
+    uint32 daif;
 
-    daif = read_sysreg(DAIF);
-    disable_interrupt();
+    daif = save_and_disable_interrupt();
 
     if (!sc_free(ptr)) {
         /*
@@ -136,5 +134,5 @@ void kfree(void *ptr)
 
 _KFREE_END:
 
-    write_sysreg(DAIF, daif);
+    restore_interrupt(daif);
 }
\ No newline at end of file
diff --git a/src/kernel/mode_switch.c b/src/kernel/mode_switch.c
new file mode 100644
index 000000000..eb516daa4
--- /dev/null
+++ b/src/kernel/mode_switch.c
@@ -0,0 +1,12 @@
+#include <mode_switch.h>
+#include <signal.h>
+#include <utils.h>
+
+void exit_to_user_mode(trapframe regs)
+{
+    enable_interrupt();
+
+    handle_signal(&regs);
+
+    disable_interrupt();
+}
\ No newline at end of file
diff --git a/src/kernel/preempt.c b/src/kernel/preempt.c
new file mode 100644
index 000000000..5132698f5
--- /dev/null
+++ b/src/kernel/preempt.c
@@ -0,0 +1,25 @@
+#include <preempt.h>
+#include <task.h>
+#include <current.h>
+
+void preempt_disable(void)
+{
+    uint32 daif;
+
+    daif = save_and_disable_interrupt();
+
+    current->preempt += 1;
+
+    restore_interrupt(daif);
+}
+
+void preempt_enable(void)
+{
+    uint32 daif;
+
+    daif = save_and_disable_interrupt();
+
+    current->preempt -= 1;
+
+    restore_interrupt(daif);
+}
\ No newline at end of file
diff --git a/src/kernel/sched.S b/src/kernel/sched.S
new file mode 100644
index 000000000..0fd3da883
--- /dev/null
+++ b/src/kernel/sched.S
@@ -0,0 +1,24 @@
+.globl switch_to
+switch_to:
+    stp x19, x20, [x0, 16 * 0]
+    stp x21, x22, [x0, 16 * 1]
+    stp x23, x24, [x0, 16 * 2]
+    stp x25, x26, [x0, 16 * 3]
+    stp x27, x28, [x0, 16 * 4]
+    stp fp, lr, [x0, 16 * 5]
+    mov x9, sp
+    str x9, [x0, 16 * 6]
+
+    ldp x19, x20, [x1, 16 * 0]
+    ldp x21, x22, [x1, 16 * 1]
+    ldp x23, x24, [x1, 16 * 2]
+    ldp x25, x26, [x1, 16 * 3]
+    ldp x27, x28, [x1, 16 * 4]
+    ldp fp, lr, [x1, 16 * 5]
+    ldr x9, [x1, 16 * 6]
+    mov sp,  x9
+
+    // set_current
+    msr tpidr_el1, x1
+
+    ret
diff --git a/src/kernel/sched.c b/src/kernel/sched.c
new file mode 100644
index 000000000..bf3817374
--- /dev/null
+++ b/src/kernel/sched.c
@@ -0,0 +1,78 @@
+#include <types.h>
+#include <sched.h>
+#include <timer.h>
+#include <current.h>
+#include <list.h>
+#include <preempt.h>
+
+#define SCHEDULER_TIMER_HZ 32
+#define SCHEDULER_WATERMARK 1
+
+static struct list_head run_queue;
+
+static uint32 schedule_ticks;
+
+static void timer_schdule_tick(void *_)
+{
+    schedule_tick();
+
+    timer_add_proc_freq(timer_schdule_tick, NULL, SCHEDULER_TIMER_HZ);
+}
+
+void scheduler_init(void)
+{
+    INIT_LIST_HEAD(&run_queue);
+
+    timer_add_proc_freq(timer_schdule_tick, NULL, SCHEDULER_TIMER_HZ);
+}
+
+void schedule(void)
+{
+    uint64 daif;
+    task_struct *task;
+
+    daif = save_and_disable_interrupt();
+
+    task = list_first_entry(&run_queue, task_struct, list);
+
+    list_del(&task->list);
+    list_add_tail(&task->list, &run_queue);
+
+    current->need_resched = 0;
+
+    restore_interrupt(daif);
+
+    // Set registers. Set current to task
+    switch_to(current, task);
+}
+
+void schedule_tick(void)
+{
+    schedule_ticks += 1;
+
+    if (schedule_ticks >= SCHEDULER_WATERMARK) {
+        schedule_ticks = 0;
+
+        current->need_resched = 1;
+    }
+}
+
+void sched_add_task(task_struct *task)
+{
+    preempt_disable();
+
+    task->status = TASK_RUNNING;
+
+    list_add_tail(&task->list, &run_queue);
+
+    preempt_enable();
+}
+
+void sched_del_task(task_struct *task)
+{
+    preempt_disable();
+
+    list_del(&task->list);
+
+    preempt_enable();
+}
\ No newline at end of file
diff --git a/src/kernel/signal.c b/src/kernel/signal.c
new file mode 100644
index 000000000..35205bc17
--- /dev/null
+++ b/src/kernel/signal.c
@@ -0,0 +1,272 @@
+#include <signal.h>
+#include <task.h>
+#include <exec.h>
+#include <current.h>
+#include <preempt.h>
+#include <mm/mm.h>
+
+// TODO: implement SIGSTOP & SIGCONT kernel handler
+
+#define DATA_OFFSET(x) ((uint64)current->data - (uint64)x)
+
+/* Kernel defined sighandler_t */
+#define SIG_DFL     (sighandler_t)0
+#define SIG_IGN     (sighandler_t)1
+
+// SIG_DFL
+static void sig_termiante(int);
+
+// SIG_IGN
+static void sig_ignore(int);
+
+static void sig_termiante(int _)
+{
+    exit_user_prog();
+
+    // Never reach
+}
+
+static void sig_ignore(int _)
+{
+    return;
+}
+
+/*
+ * TODO: This function runs in user mode, make user can execute this function
+ *       when MMU is enable.
+ */
+void sigreturn(void)
+{
+    // sigreturn: syscall 11
+    asm volatile(
+        "mov x8, 11\n"
+        "svc 0\n"
+    );
+}
+
+/* 
+ * Try to get the pending signal of current task.
+ */
+static struct signal_t *signal_try_get(void)
+{
+    if (list_empty(&current->signal->list)) {
+       return NULL; 
+    }
+
+    return list_first_entry(&current->signal->list, struct signal_t, list);
+}
+
+static void signal_add(uint32 signum, struct signal_head_t *head)
+{
+    struct signal_t *signal;
+
+    signal = kmalloc(sizeof(struct signal_t));
+
+    signal->signum = signum;
+
+    list_add(&signal->list, &head->list);
+}
+
+static void signal_del(struct signal_t *signal)
+{
+    list_del(&signal->list);
+    kfree(signal);
+}
+
+static void save_context(void *user_sp, trapframe *frame)
+{
+    memncpy(user_sp, (char *)frame, sizeof(trapframe));
+}
+
+struct signal_head_t *signal_head_create(void)
+{
+    struct signal_head_t *head;
+
+    head = kmalloc(sizeof(struct signal_head_t));
+
+    INIT_LIST_HEAD(&head->list);
+
+    return head;
+}
+
+void signal_head_free(struct signal_head_t *head)
+{
+    struct signal_t *signal, *safe;
+
+    list_for_each_entry_safe(signal, safe, &head->list, list) {
+        signal_del(signal);
+    }
+
+    kfree(head);
+}
+
+void signal_head_reset(struct signal_head_t *head)
+{
+    struct signal_t *signal, *safe;
+
+    list_for_each_entry_safe(signal, safe, &head->list, list) {
+        signal_del(signal);
+    }
+}
+
+void handle_signal(trapframe *frame)
+{
+    struct signal_t *signal;
+    struct sigaction_t *sigaction;
+
+    preempt_disable();
+
+    signal = signal_try_get();
+
+    preempt_enable();
+
+    if (signal == NULL) {
+        return;
+    }
+
+    sigaction = &current->sighand->sigactions[signal->signum];
+
+    if (sigaction->kernel_hand) {
+        sigaction->sighand(signal->signum);
+    } else {
+        char *user_sp;
+        uint32 reserve_size;
+
+        // Reserve space on user stack
+        reserve_size = sizeof(trapframe);
+        user_sp = frame->sp_el0 - ALIGN(reserve_size, 0x10);
+
+        // Save cpu context onto user stack
+        save_context(user_sp, frame);
+
+        // Set user sp
+        frame->sp_el0 = user_sp;
+
+        // Set parameter of handler
+        frame->x0 = signal->signum;
+
+        // Set user pc to handler
+        frame->elr_el1 = sigaction->sighand;
+
+        // Set user lr to sigreturn
+        frame->x30 = (uint64)sigreturn;
+    }
+
+    signal_del(signal);
+}
+
+struct sighand_t *sighand_create(void)
+{
+    struct sighand_t *sighand;
+
+    sighand = kmalloc(sizeof(struct sighand_t));
+
+    for (int i = 1; i < NSIG; ++i) {
+        sighand->sigactions[i].kernel_hand = 1;
+        sighand->sigactions[i].sighand = sig_ignore;
+    }
+
+    sighand->sigactions[SIGKILL].sighand = sig_termiante;
+
+    return sighand;
+}
+
+void sighand_free(struct sighand_t *sighand)
+{
+    kfree(sighand);
+}
+
+void sighand_reset(struct sighand_t *sighand)
+{
+    for (int i = 1; i < NSIG; ++i) {
+        sighand->sigactions[i].kernel_hand = 1;
+        sighand->sigactions[i].sighand = sig_ignore;
+    }
+
+    sighand->sigactions[SIGKILL].sighand = sig_termiante;
+}
+
+static inline void kernel_sighand_copy(struct sigaction_t *from,
+                                       struct sigaction_t *to)
+{
+    to->kernel_hand = 1;
+    to->sighand = from->sighand;
+}
+
+static inline void user_sighand_copy(struct sigaction_t *from,
+                                     struct sigaction_t *to,
+                                     uint64 offset)
+{
+    to->kernel_hand = 0;
+    to->sighand = (sighandler_t)((char *)from->sighand - offset);
+}
+
+/* Copy current signal handler to @sighand */
+void sighand_copy(struct sighand_t *sighand, void *addrbase)
+{
+    struct sighand_t *currhand;
+
+    currhand = current->sighand;
+
+    for (int i = 1; i < NSIG; ++i) {
+        if (currhand->sigactions[i].kernel_hand) {
+            kernel_sighand_copy(&currhand->sigactions[i],
+                                &sighand->sigactions[i]);
+        } else {
+            user_sighand_copy(&currhand->sigactions[i],
+                              &sighand->sigactions[i],
+                              DATA_OFFSET(addrbase));
+        }
+    }
+}
+
+void syscall_signal(trapframe *_, uint32 signal, void (*handler)(int))
+{
+    struct sigaction_t *sigaction;
+
+    // Check if signal is valid
+    if (signal >= NSIG) {
+        return;
+    }
+
+    sigaction = &current->sighand->sigactions[signal];
+
+    if (handler == SIG_DFL) {
+        sigaction->kernel_hand = 1;
+        sigaction->sighand = sig_termiante;
+    } else if (handler == SIG_IGN) {
+        sigaction->kernel_hand = 1;
+        sigaction->sighand = sig_ignore;
+    } else {
+        sigaction->kernel_hand = 0;
+        sigaction->sighand = handler;
+    }
+}
+
+void syscall_kill(trapframe *_, int pid, int signal)
+{
+    task_struct *task;
+    
+    preempt_disable();
+
+    task = task_get_by_tid(pid);
+
+    if (!task || task->status != TASK_RUNNING) {
+        goto SYSCALL_KILL_END;
+    }
+
+    signal_add((uint32)signal, task->signal);
+
+SYSCALL_KILL_END:
+    preempt_enable();
+}
+
+// restore context
+void syscall_sigreturn(trapframe *frame)
+{
+    trapframe *user_sp;
+
+    user_sp = frame->sp_el0;
+
+    memncpy((char *)frame, (char *)user_sp, sizeof(trapframe));
+}
\ No newline at end of file
diff --git a/src/kernel/syscall.c b/src/kernel/syscall.c
index 0d83a052c..05cefe65c 100644
--- a/src/kernel/syscall.c
+++ b/src/kernel/syscall.c
@@ -1,15 +1,58 @@
 #include <mini_uart.h>
 #include <syscall.h>
 #include <utils.h>
+#include <exec.h>
+#include <current.h>
+#include <task.h>
+#include <mini_uart.h>
+#include <rpi3.h>
+#include <preempt.h>
+#include <kthread.h>
+#include <cpio.h>
+#include <sched.h>
+#include <signal.h>
+#include <mm/mm.h>
+
+#define KSTACK_VARIABLE(x)                      \
+    (void *)((uint64)x -                        \
+             (uint64)current->kernel_stack +    \
+             (uint64)child->kernel_stack)
+
+#define USTACK_VARIABLE(x)                      \
+    (void *)((uint64)x -                        \
+             (uint64)current->user_stack +      \
+             (uint64)child->user_stack)
 
-typedef void *(*funcp)();
+#define DATA_VARIABLE(x)                        \
+    (void *)((uint64)x -                        \
+             (uint64)current->data +            \
+             (uint64)child->data)
 
-void *syscall_test();
-void *syscall_exit();
+typedef void (*syscall_funcp)();
 
-funcp syscall_table[] = {
-    syscall_test, // 0
-    syscall_exit, // 1
+void syscall_getpid(trapframe *_);
+void syscall_uart_read(trapframe *_, char buf[], size_t size);
+void syscall_uart_write(trapframe *_, const char buf[], size_t size);
+void syscall_exec(trapframe *_, const char* name, char *const argv[]);
+void syscall_fork(trapframe *_);
+void syscall_exit(trapframe *_);
+void syscall_mbox_call(trapframe *_, unsigned char ch, unsigned int *mbox);
+void syscall_kill_pid(trapframe *_, int pid);
+void syscall_show_info(trapframe *_);
+
+syscall_funcp syscall_table[] = {
+    (syscall_funcp) syscall_getpid,     // 0
+    (syscall_funcp) syscall_uart_read,
+    (syscall_funcp) syscall_uart_write,
+    (syscall_funcp) syscall_exec,
+    (syscall_funcp) syscall_fork,       // 4
+    (syscall_funcp) syscall_exit,
+    (syscall_funcp) syscall_mbox_call,
+    (syscall_funcp) syscall_kill_pid,
+    (syscall_funcp) syscall_signal,     // 8
+    (syscall_funcp) syscall_kill,
+    (syscall_funcp) syscall_show_info,
+    (syscall_funcp) syscall_sigreturn,
 };
 
 typedef struct {
@@ -18,25 +61,184 @@ typedef struct {
                  ec:6;   // Exception class
 } esr_el1;
 
-void syscall_handler(uint32 syn)
+void syscall_handler(trapframe regs, uint32 syn)
 {
-    esr_el1 *esr = (esr_el1 *)&syn;
+    esr_el1 *esr;
+    uint64 syscall_num;
+      
+    esr = (esr_el1 *)&syn;
+
+    // SVC instruction execution
+    if (esr->ec != 0x15) {
+        return;
+    }
 
-    int syscall_num = esr->iss & 0xffff;
+    syscall_num = regs.x8;
 
-    if (syscall_num >= ARRAY_SIZE(syscall_table)) {
+    if (syscall_num > ARRAY_SIZE(syscall_table)) {
         // Invalid syscall
         return;
     }
 
-    // TODO: bring the arguments to syscall
     enable_interrupt();
-    (syscall_table[syscall_num])();
+
+    (syscall_table[syscall_num])(&regs,
+        regs.x0, regs.x1, regs.x2, regs.x3, regs.x4, regs.x5);
+
     disable_interrupt();
 }
 
+void syscall_getpid(trapframe *frame)
+{
+    frame->x0 = current->tid;
+}
+
+void syscall_uart_read(trapframe *_, char buf[], size_t size)
+{
+    uart_recvn(buf, size);
+}
+
+void syscall_uart_write(trapframe *_, const char buf[], size_t size)
+{
+    uart_sendn(buf, size);
+}
+
+// TODO: Passing argv
+void syscall_exec(trapframe *_, const char* name, char *const argv[])
+{
+    void *data;
+    char *kernel_sp;
+    char *user_sp;
+    uint32 datalen;
+    
+    datalen = cpio_load_prog(initramfs_base, name, (char **)&data);
+
+    if (datalen == 0) {
+        return;
+    }
+
+    // Use origin kernel stack
+
+    // TODO: Clear user stack
+
+    kfree(current->data);
+    current->data = data;
+    current->datalen = datalen;
+
+    kernel_sp = (char *)current->kernel_stack + STACK_SIZE - 0x10;
+    user_sp = (char *)current->user_stack + STACK_SIZE - 0x10;
+
+    // Reset signal
+    signal_head_reset(current->signal);
+    sighand_reset(current->sighand);
+
+    exec_user_prog(current->data, user_sp, kernel_sp);
+}
+
+static inline void copy_regs(struct pt_regs *regs)
+{
+    regs->x19 = current->regs.x19;
+    regs->x20 = current->regs.x20;
+    regs->x21 = current->regs.x21;
+    regs->x22 = current->regs.x22;
+    regs->x23 = current->regs.x23;
+    regs->x24 = current->regs.x24;
+    regs->x25 = current->regs.x25;
+    regs->x26 = current->regs.x26;
+    regs->x27 = current->regs.x27;
+    regs->x28 = current->regs.x28;
+}
+
+void syscall_fork(trapframe *frame)
+{
+    task_struct *child;
+    trapframe *child_frame;
+
+    child = task_create();
+
+    child->kernel_stack = kmalloc(STACK_SIZE);
+    child->user_stack = kmalloc(STACK_SIZE);    
+    child->data = kmalloc(current->datalen);
+    child->datalen = current->datalen;
+
+    memncpy(child->kernel_stack, current->kernel_stack, STACK_SIZE);
+    memncpy(child->user_stack, current->user_stack, STACK_SIZE);
+    memncpy(child->data, current->data, current->datalen);
+
+    // Copy signal handler
+    sighand_copy(child->sighand, child->data);
+
+    // Save regs
+    SAVE_REGS(current);
+
+    // Copy register
+    copy_regs(&child->regs);
+
+    // Copy stack realted registers
+    child->regs.fp = KSTACK_VARIABLE(current->regs.fp);
+    child->regs.sp = KSTACK_VARIABLE(current->regs.sp);
+
+    // https://gcc.gnu.org/onlinedocs/gcc/Labels-as-Values.html
+    child->regs.lr = &&SYSCALL_FORK_END;
+
+    // Adjust child trapframe
+    child_frame = KSTACK_VARIABLE(frame);
+
+    child_frame->x0 = 0;
+    child_frame->x30 = (uint64)DATA_VARIABLE(frame->x30);
+    child_frame->sp_el0 = USTACK_VARIABLE(frame->sp_el0);
+    child_frame->elr_el1 = DATA_VARIABLE(frame->elr_el1);
+
+    sched_add_task(child);
+
+    // Set return value
+    frame->x0 = child->tid;
+
+SYSCALL_FORK_END:
+
+    asm volatile("nop");
+}
+
+void syscall_exit(trapframe *_)
+{
+    exit_user_prog();
+
+    // Never reach
+}
+
+void syscall_mbox_call(trapframe *_, unsigned char ch, unsigned int *mbox)
+{
+    mailbox_call(ch, mbox);
+}
+
+void syscall_kill_pid(trapframe *_, int pid)
+{
+    task_struct *task;
+
+    if (current->tid == pid) {
+        exit_user_prog();
+
+        // Never reach
+        return;
+    }
+
+    preempt_disable();
+
+    task = task_get_by_tid(pid);
+
+    if (!task || task->status != TASK_RUNNING) {
+        goto SYSCALL_KILL_PID_END;
+    }
+
+    list_del(&task->list);
+    kthread_add_wait_queue(task);
+
+SYSCALL_KILL_PID_END:
+    preempt_enable();
+}
+
 // Print the content of spsr_el1, elr_el1 and esr_el1
-void *syscall_test()
+void syscall_show_info(trapframe *_)
 {
     uint64 spsr_el1;
     uint64 elr_el1;
@@ -46,13 +248,6 @@ void *syscall_test()
     elr_el1 = read_sysreg(elr_el1);
     esr_el1 = read_sysreg(esr_el1);
 
-    uart_printf("[TEST] spsr_el1: %llx; elr_el1: %llx; esr_el1: %llx\r\n", 
+    uart_printf("[TEST] spsr_el1: %llx; elr_el1: %llx; esr_el1: %llx\r\n",
         spsr_el1, elr_el1, esr_el1);
-
-    return NULL;
-}
-
-void *syscall_exit()
-{
-    return NULL;
-}
+}
\ No newline at end of file
diff --git a/src/kernel/task.c b/src/kernel/task.c
new file mode 100644
index 000000000..49b1aaae7
--- /dev/null
+++ b/src/kernel/task.c
@@ -0,0 +1,84 @@
+#include <task.h>
+#include <signal.h>
+#include <mm/mm.h>
+
+// TODO: Use rbtree to manage tasks
+static struct list_head task_queue;
+
+// TODO: recycle usable tid
+uint32 max_tid;
+
+static uint32 alloc_tid(void)
+{
+    uint32 tid;
+
+    tid = max_tid;
+    max_tid += 1;
+
+    return tid;
+}
+
+void task_init(void)
+{
+    INIT_LIST_HEAD(&task_queue);
+}
+
+task_struct *task_create(void)
+{
+    task_struct *task;
+    struct signal_head_t *signal;
+    struct sighand_t *sighand;
+    
+    task = kmalloc(sizeof(task_struct));
+    signal = signal_head_create();
+    sighand = sighand_create();
+
+    task->kernel_stack = NULL;
+    task->user_stack = NULL;
+    task->data = NULL;
+    INIT_LIST_HEAD(&task->list);
+    list_add_tail(&task->task_list, &task_queue);
+    task->status = TASK_NEW;
+    task->need_resched = 0;
+    task->tid = alloc_tid();
+    task->preempt = 0;
+
+    task->signal = signal;
+    task->sighand = sighand;
+
+    return task;
+}
+
+void task_free(task_struct *task)
+{
+    if (task->kernel_stack)
+        kfree(task->kernel_stack);
+
+    if (task->user_stack)
+        kfree(task->user_stack);
+
+    if (task->data)
+        kfree(task->data);
+
+    list_del(&task->task_list);
+
+    signal_head_free(task->signal);
+    sighand_free(task->sighand);
+
+    // TODO: release tid
+    
+    kfree(task);
+}
+
+task_struct *task_get_by_tid(uint32 tid)
+{
+    task_struct *task;
+
+    list_for_each_entry(task, &task_queue, task_list) {
+        if (task->tid == tid) {
+            return task;
+        }
+    }
+
+    return NULL;
+}
\ No newline at end of file
diff --git a/src/kernel/timer.c b/src/kernel/timer.c
index 609f6cb47..0ccd4a6d6 100644
--- a/src/kernel/timer.c
+++ b/src/kernel/timer.c
@@ -10,6 +10,9 @@
 
 #define TIMER_PROC_NUM 32
 
+static void timer_irq_handler(void *);
+static void timer_irq_fini(void);
+
 typedef struct {
     // cb(args)
     void (*cb)(void *);
@@ -59,23 +62,22 @@ static void timer_disable()
 
 static timer_proc *tp_alloc()
 {
-    uint64 daif;
+    uint32 daif;
     uint32 idx;
 
-    daif = read_sysreg(DAIF);
-    disable_interrupt();
+    daif = save_and_disable_interrupt();
 
     idx = ffs(t_status);
 
     if (idx == 0) {
-        write_sysreg(DAIF, daif);
+        restore_interrupt(daif);
 
         return NULL;
     }
 
     t_status &= ~(1 << (idx - 1));
 
-    write_sysreg(DAIF, daif);
+    restore_interrupt(daif);
 
     return &t_procs[idx - 1];
 }
@@ -97,7 +99,7 @@ static void tp_release(timer_proc *tp)
 static void timer_update_remain_time()
 {
     timer_proc *iter;
-    uint32 cntp_tval_el0;
+    int32 cntp_tval_el0;
     uint32 diff;
 
     if (!t_meta.size) {
@@ -109,7 +111,11 @@ static void timer_update_remain_time()
     t_interval = cntp_tval_el0;
 
     list_for_each_entry(iter, &t_meta.head, list) {
-        iter->remain_time -= diff;
+        if (diff > iter->remain_time) {
+            iter->remain_time = 0;
+        } else {
+            iter->remain_time -= diff;
+        }
     }
 }
 
@@ -118,12 +124,11 @@ static void timer_update_remain_time()
  */
 static int tp_insert(timer_proc *tp)
 {
-    uint64 daif;
+    uint32 daif;
     timer_proc *iter;
     int first;
 
-    daif = read_sysreg(DAIF);
-    disable_interrupt();
+    daif = save_and_disable_interrupt();
 
     // Update remain_time
     timer_update_remain_time();
@@ -141,7 +146,7 @@ static int tp_insert(timer_proc *tp)
 
     t_meta.size += 1;
 
-    write_sysreg(DAIF, daif);
+    restore_interrupt(daif);
 
     return first;
 }
@@ -151,14 +156,14 @@ static int tp_insert(timer_proc *tp)
  */
 static void timer_set()
 {
-    uint64 daif;
+    uint32 daif;
     timer_proc *tp;
 
-    daif = read_sysreg(DAIF);
-    disable_interrupt();
+    daif = save_and_disable_interrupt();
 
     if (!t_meta.size) {
-        write_sysreg(DAIF, daif);
+        restore_interrupt(daif);
+
         return;
     }
     
@@ -168,7 +173,7 @@ static void timer_set()
     t_interval = tp->remain_time;
     write_sysreg(cntp_tval_el0, t_interval);
 
-    write_sysreg(DAIF, daif);
+    restore_interrupt(daif);
 }
 
 static void timer_set_boot_cnt()
@@ -185,11 +190,13 @@ static void timer_show_boot_time(void *_)
                     (cntpct_el0 - timer_boot_cnt) / cntfrq_el0);
     }
 
-    timer_add_proc(timer_show_boot_time, NULL, 2);
+    timer_add_proc_after(timer_show_boot_time, NULL, 2);
 }
 
 void timer_init()
 {
+    uint64 cntkctl_el1;
+
     timer_set_boot_cnt();
 
     INIT_LIST_HEAD(&t_meta.head);
@@ -198,22 +205,31 @@ void timer_init()
     t_interval = 0;
     t_status = 0xffffffff;
 
-    timer_add_proc(timer_show_boot_time, NULL, 2);
+    // Allow EL0 to access timer
+    cntkctl_el1 = read_sysreg(CNTKCTL_EL1);
+    cntkctl_el1 |= 1;
+    write_sysreg(CNTKCTL_EL1, cntkctl_el1);
+
+    timer_add_proc_after(timer_show_boot_time, NULL, 2);
 }
 
-void timer_irq_check()
+int timer_irq_check()
 {
     uint32 core0_irq_src = get32(CORE0_IRQ_SOURCE);
 
-    if (core0_irq_src & 0x02) {
-        timer_disable();
-        if (irq_add_tasks(timer_irq_handler, NULL, 0)) {
-            timer_enable();
-        }
+    if (!(core0_irq_src & 0x02)) {
+        return 0;
     }
+
+    timer_disable();
+    if (irq_run_task(timer_irq_handler, NULL, timer_irq_fini, 0)) {
+        timer_enable();
+    }
+
+    return 1;
 }
 
-void timer_irq_handler()
+static void timer_irq_handler(void *_)
 {
     timer_proc *tp;
 
@@ -233,7 +249,10 @@ void timer_irq_handler()
     // Execute the callback function
     (tp->cb)(tp->args);
     tp_release(tp);
+}
 
+static void timer_irq_fini(void)
+{
     timer_enable();
 }
 
@@ -242,11 +261,22 @@ void timer_switch_info()
     timer_show_enable = !timer_show_enable;
 }
 
-void timer_add_proc(void (*proc)(void *), void *args, uint32 after)
+static void timer_add_proc(timer_proc *tp)
+{
+    int need_update;
+
+    need_update = tp_insert(tp);
+
+    if (need_update) {
+        timer_set();
+        timer_enable();
+    }
+}
+
+void timer_add_proc_after(void (*proc)(void *), void *args, uint32 after)
 {
     timer_proc *tp;
     uint32 cntfrq_el0;
-    int need_update;
 
     tp = tp_alloc();
 
@@ -259,10 +289,26 @@ void timer_add_proc(void (*proc)(void *), void *args, uint32 after)
     tp->cb = proc;
     tp->args = args;
     tp->remain_time = after * cntfrq_el0;
-    need_update = tp_insert(tp);
-    
-    if (need_update) {
-        timer_set();
-        timer_enable();
+
+    timer_add_proc(tp);
+}
+
+void timer_add_proc_freq(void (*proc)(void *), void *args, uint32 freq)
+{
+    timer_proc *tp;
+    uint32 cntfrq_el0;
+
+    tp = tp_alloc();
+
+    if (!tp) {
+        return;
     }
+
+    cntfrq_el0 = read_sysreg(cntfrq_el0);
+
+    tp->cb = proc;
+    tp->args = args;
+    tp->remain_time = cntfrq_el0 / freq;
+   
+    timer_add_proc(tp);
 }
\ No newline at end of file
diff --git a/src/kernel/waitqueue.c b/src/kernel/waitqueue.c
new file mode 100644
index 000000000..9e2a69484
--- /dev/null
+++ b/src/kernel/waitqueue.c
@@ -0,0 +1,50 @@
+#include <waitqueue.h>
+#include <preempt.h>
+#include <mm/mm.h>
+
+wait_queue_head *wq_create(void)
+{
+    wait_queue_head *head;
+
+    head = kmalloc(sizeof(wait_queue_head));
+
+    INIT_LIST_HEAD(&head->list);
+
+    return head;
+}
+
+int wq_empty(wait_queue_head *head)
+{
+    return list_empty(&head->list);
+}
+
+void wq_add_task(task_struct *task, wait_queue_head *head)
+{
+    preempt_disable();
+
+    list_add_tail(&task->list, &head->list);
+
+    preempt_enable();
+}
+
+void wq_del_task(task_struct *task)
+{
+    preempt_disable();
+
+    list_del(&task->list);
+
+    preempt_enable();
+}
+
+task_struct *wq_get_first_task(wait_queue_head *head)
+{
+    task_struct *task;
+
+    preempt_disable();
+
+    task = list_first_entry(&head->list, task_struct, list);
+
+    preempt_enable();
+
+    return task;
+}
\ No newline at end of file
diff --git a/src/lib/rpi3.c b/src/lib/rpi3.c
index 6778e9e1e..471e2a308 100644
--- a/src/lib/rpi3.c
+++ b/src/lib/rpi3.c
@@ -40,11 +40,11 @@
 // Aligned buffer
 static unsigned int __attribute__((aligned(0x10))) mailbox[16];
 
-static void mailbox_call(void)
+void mailbox_call(unsigned char channel, unsigned int *mb)
 {
     // Write the data (shifted into the upper 28 bits) combined with 
     // the channel (in the lower four bits) to the write register.
-    unsigned int r = (((unsigned long)mailbox) & ~0xf) | MAILBOX_CH_PROP;
+    unsigned int r = (((unsigned long)mb) & ~0xf) | channel;
 
     // Check if Mailbox 0 status register’s full flag is set.
     while ((get32(MAILBOX_STATUS) & MAILBOX_FULL)) {};
@@ -76,7 +76,7 @@ unsigned int get_board_revision(void)
     // Tags end
     mailbox[6] = END_TAG;
 
-    mailbox_call(); // Message passing procedure call
+    mailbox_call(MAILBOX_CH_PROP, mailbox); // Message passing procedure call
 
     return mailbox[5]; 
 }
@@ -94,7 +94,7 @@ void get_arm_memory(struct arm_memory_info *info)
     // Tags end
     mailbox[7] = END_TAG;
 
-    mailbox_call(); // Message passing procedure call
+    mailbox_call(MAILBOX_CH_PROP, mailbox); // Message passing procedure call
 
     info->baseaddr = mailbox[5];
     info->size     = mailbox[6];
diff --git a/src/lib/string.c b/src/lib/string.c
index 7c4bf0274..69170a319 100644
--- a/src/lib/string.c
+++ b/src/lib/string.c
@@ -1,6 +1,6 @@
 #include <string.h>
 
-int strcmp(char *str1, char *str2)
+int strcmp(const char *str1, const char *str2)
 {
     char c1, c2;
 
@@ -9,7 +9,7 @@ int strcmp(char *str1, char *str2)
     return c1 - c2;
 }
 
-int strncmp(char *str1, char *str2, int n)
+int strncmp(const char *str1, const char *str2, int n)
 {
     char c1, c2;
 
@@ -21,7 +21,7 @@ int strncmp(char *str1, char *str2, int n)
     return n ? c1 - c2 : 0;
 }
 
-int strlen(char *str)
+int strlen(const char *str)
 {
     int ret = 0;
     
@@ -32,7 +32,7 @@ int strlen(char *str)
     return ret;
 }
 
-int atoi(char *str)
+int atoi(const char *str)
 {
     int i = 0, tmp = 0;