diff --git a/arch/x86/entry.S b/arch/x86/entry.S
index be4de372..7e7636a7 100644
--- a/arch/x86/entry.S
+++ b/arch/x86/entry.S
@@ -29,6 +29,8 @@
 #include <segment.h>
 #include <page.h>
 #include <traps.h>
+#include <usermode.h>
+#include <errno.h>
 
 .macro _handle_usermode cr3
     testb $0x3, cpu_exc_cs(%_ASM_SP)
@@ -46,6 +48,18 @@
     _handle_usermode user_cr3
 .endm
 
+.macro syscall_from_usermode
+    SET_CR3 cr3
+    swapgs
+    SWITCH_STACK
+.endm
+
+.macro syscall_to_usermode
+    SWITCH_STACK
+    swapgs
+    SET_CR3 user_cr3
+.endm
+
 .macro exception_handler sym vec has_error_code
 ENTRY(entry_\sym)
     enter_from_usermode
@@ -54,7 +68,12 @@ ENTRY(entry_\sym)
         push $0
     .endif
 
+#if defined(__x86_64__)
     movl $\vec, cpu_exc_vector(%_ASM_SP)
+#else
+    push $\vec
+#endif
+
     jmp handle_exception
 END_FUNC(entry_\sym)
 .endm
@@ -105,11 +124,7 @@ ENTRY(handle_exception)
 
     RESTORE_ALL_REGS
 
-#if defined(__x86_64__)
     add $8, %_ASM_SP
-#else
-    add $4, %_ASM_SP
-#endif
 
     enter_to_usermode
     IRET
@@ -152,6 +167,49 @@ ENTRY(enter_usermode)
     IRET
 END_FUNC(enter_usermode)
 
+ENTRY(syscall_exit)
+    POPF
+
+    /* Save exit code to return value register (AX) */
+    mov %_ASM_SI, cpu_regs_ax(%_ASM_SP)
+    RESTORE_ALL_REGS
+
+    ret
+END_FUNC(syscall_exit)
+
+ENTRY(terminate_user_task)
+    SWITCH_STACK
+    POPF
+
+    movl $-EFAULT, cpu_regs_ax(%_ASM_SP)
+    RESTORE_ALL_REGS
+
+    ret
+END_FUNC(terminate_user_task)
+
+.align PAGE_SIZE
+ENTRY(syscall_handler_entry)
+    SAVE_CALLEE_SAVED_REGS
+    syscall_from_usermode
+
+    cmp $SYSCALL_EXIT, %_ASM_AX
+    jz syscall_exit
+
+    push %_ASM_CX
+    push %r11
+
+    mov %_ASM_DI, %_ASM_CX
+    mov %_ASM_AX, %_ASM_DI
+    call syscall_handler
+
+    pop %r11
+    pop %_ASM_CX
+
+    syscall_to_usermode
+    RESTORE_CALLEE_SAVED_REGS
+    SYSRET
+END_FUNC(syscall_handler_entry)
+
 SECTION(.text.user, "ax", 16)
 ENTRY(usermode_stub)
     /* DI: User function to be called
@@ -163,7 +221,7 @@ ENTRY(usermode_stub)
 
     /* sys_exit */
     mov %_ASM_AX, %_ASM_DI
-    xor %_ASM_AX, %_ASM_AX
+    mov $SYSCALL_EXIT, %_ASM_AX
     syscall
 END_FUNC(usermode_stub)
 
diff --git a/arch/x86/traps.c b/arch/x86/traps.c
index 0a1adf4e..7f8bd970 100644
--- a/arch/x86/traps.c
+++ b/arch/x86/traps.c
@@ -43,6 +43,8 @@ extern void asm_interrupt_handler_uart(void);
 extern void asm_interrupt_handler_keyboard(void);
 extern void asm_interrupt_handler_timer(void);
 
+extern void terminate_user_task(void);
+
 static void init_tss(percpu_t *percpu) {
 #if defined(__i386__)
     percpu->tss_df.iopb = sizeof(percpu->tss_df);
@@ -316,7 +318,7 @@ void do_exception(struct cpu_regs *regs) {
     /* Handle user tasks' exceptions */
     if (enter_from_usermode(regs->exc.cs)) {
         printk("Task exception: %s\n", panic_str);
-        goto_syscall_exit(-EFAULT);
+        terminate_user_task();
     }
 
     panic(panic_str);
diff --git a/common/console.c b/common/console.c
index f820103b..8c67ed33 100644
--- a/common/console.c
+++ b/common/console.c
@@ -41,7 +41,7 @@
 static console_callback_entry_t console_callbacks[2];
 static unsigned int num_console_callbacks;
 
-static void vprintk(const char *fmt, va_list args) {
+void vprintk(const char *fmt, va_list args) {
     static char buf[VPRINTK_BUF_SIZE];
     static spinlock_t lock = SPINLOCK_INIT;
     unsigned int i;
diff --git a/common/usermode.c b/common/usermode.c
index c510688b..a214d64a 100644
--- a/common/usermode.c
+++ b/common/usermode.c
@@ -28,98 +28,20 @@
 #include <processor.h>
 #include <usermode.h>
 
-static inline void syscall_save(void) {
-    /* clang-format off */
-    asm volatile(
-        "push %%" STR(_ASM_CX) "\n"
-        "push %%r11"
-        ::: "memory"
-    );
-    /* clang-format on */
-}
-
-static inline void syscall_restore(void) {
-    /* clang-format off */
-    asm volatile(
-        "pop %%r11\n"
-        "pop %%" STR(_ASM_CX)
-        ::: "memory"
-    );
-    /* clang-format on */
-}
-
-static inline long syscall_return(long return_code) {
-    asm volatile("" ::"a"(return_code));
-    return return_code;
-}
-
-static inline void stack_switch(void) {
-    /* clang-format off */
-    asm volatile(
-        "xchg %%gs:%[private], %%" STR(_ASM_SP) "\n"
-        ::[private] "m"(ACCESS_ONCE(PERCPU_VAR(usermode_private)))
-    );
-    /* clang-format on */
-}
-
-static inline void switch_address_space(const cr3_t *cr3) {
-    /* clang-format off */
-    asm volatile(
-        "push %%" STR(_ASM_AX) "\n"
-        "mov %[cr3], %%" STR(_ASM_AX) "\n"
-        "mov %%" STR(_ASM_AX) ", %%cr3\n"
-        "pop %%" STR(_ASM_AX) "\n"
-        :: [cr3] "m" (*cr3)
-        : STR(_ASM_AX)
-    );
-    /* clang-format on */
-}
-
-static inline void _sys_exit(void) {
-    /* clang-format off */
-    asm volatile (
-        "mov %%" STR(_ASM_DI)", %%gs:%[private]\n"
-        POPF()
-        RESTORE_ALL_REGS()
-        "mov %%gs:%[private], %%" STR(_ASM_AX) "\n"
-        "ret\n"
-        :: [ private ] "m"(ACCESS_ONCE(PERCPU_VAR(usermode_private)))
-        : STR(_ASM_AX)
-    );
-    /* clang-format on */
-}
-
-void __naked syscall_handler(void) {
-    register unsigned long syscall_nr asm(STR(_ASM_AX));
-    register unsigned long param1 asm(STR(_ASM_DI));
-    (void) param1;
-    register unsigned long param2 asm(STR(_ASM_SI));
-    (void) param2;
-    register unsigned long param3 asm(STR(_ASM_BX));
-    (void) param3;
-    register unsigned long param4 asm(STR(_ASM_DX));
-    (void) param4;
-
-    SAVE_CLOBBERED_REGS();
-    switch_address_space(&cr3);
-    swapgs();
-    stack_switch();
-    syscall_save();
-
+long syscall_handler(long syscall_nr, long arg1, long arg2, long arg3, long arg4,
+                     long arg5) {
     switch (syscall_nr) {
     case SYSCALL_EXIT:
-        syscall_restore();
-        _sys_exit();
+        /* SYSCALL_EXIT is handled by asm routine syscall_exit() */
         UNREACHABLE();
 
     case SYSCALL_PRINTF:
-        printk(_ptr(param1), param2, param3, param4);
-        syscall_return(0);
-        break;
+        vprintk(_ptr(arg1), _ptr(arg2));
+        return 0;
 
     case SYSCALL_MMAP: {
-        void *va = _ptr(param1);
-        unsigned int order = _u(param2);
+        void *va = _ptr(arg1);
+        unsigned int order = _u(arg2);
         frame_t *frame;
 
         frame = get_free_frames(order);
@@ -128,41 +50,32 @@ void __naked syscall_handler(void) {
 
         va = vmap_user(va, frame->mfn, order, L4_PROT_USER, L3_PROT_USER, L2_PROT_USER,
                        L1_PROT_USER);
-        syscall_return(_ul(va));
-    } break;
+        return _ul(va);
+    }
 
     case SYSCALL_MUNMAP: {
-        void *va = _ptr(param1);
-        unsigned int order = _u(param2);
+        void *va = _ptr(arg1);
+        unsigned int order = _u(arg2);
 
         vunmap_user(va, order);
-    } break;
+        return 0;
+    }
 
     default:
         printk("Unknown syscall: %lu\n", syscall_nr);
-        syscall_return(-1L);
-        break;
+        return -1;
     }
-
-    syscall_restore();
-    stack_switch();
-    swapgs();
-    switch_address_space(&user_cr3);
-
-    RESTORE_CLOBBERED_REGS();
-
-    sysret();
 }
 
 static void init_syscall(void) {
     msr_star_t star;
 
-    star.eip = _u(_ul(&syscall_handler));
+    star.eip = _u(_ul(&syscall_handler_entry));
     star.kern_cs = __KERN_CS64;
     star.user_cs = __USER_CS64;
 
     wrmsr(MSR_STAR, star.reg);
-    wrmsr(MSR_LSTAR, _ul(&syscall_handler));
+    wrmsr(MSR_LSTAR, _ul(&syscall_handler_entry));
     /* FIXME: Add compat support */
     wrmsr(MSR_CSTAR, _ul(NULL));
 
@@ -177,67 +90,61 @@ static void init_syscall(void) {
 void init_usermode(percpu_t *percpu) {
     vmap_user_4k(&cr3, virt_to_mfn(&cr3), L1_PROT);
     vmap_user_4k(&enter_usermode, virt_to_mfn(&enter_usermode), L1_PROT);
-    vmap_user_4k(&syscall_handler, virt_to_mfn(&syscall_handler), L1_PROT);
-
+    vmap_user_4k(&syscall_handler_entry, virt_to_mfn(&syscall_handler_entry), L1_PROT);
     init_syscall();
 }
 
-static inline void __user_text sys_exit(unsigned long exit_code) {
-    asm volatile("syscall" ::"A"(SYSCALL_EXIT), "D"(exit_code) : STR(_ASM_CX), "r11");
-}
-
-static inline long __user_text sys_printf(const char *fmt, unsigned long arg1,
-                                          unsigned long arg2, unsigned long arg3) {
-    register unsigned long rax asm(STR(_ASM_AX));
+static inline long __user_text syscall(long syscall_nr, long arg1, long arg2, long arg3,
+                                       long arg4, long arg5) {
+    register long return_code asm(STR(_ASM_AX));
+    register long _arg4 asm("r8") = arg4;
+    register long _arg5 asm("r9") = arg5;
 
     /* clang-format off */
     asm volatile(
-        "syscall"
-        : "=A"(rax)
-        : "0"(SYSCALL_PRINTF), "D"(fmt), "S"(arg1), "b"(arg2), "d"(arg3)
+        "syscall\n"
+        : "=a"(return_code)
+        : "0"(syscall_nr), "S"(arg1), "d"(arg2), "D" (arg3), "r"(_arg4), "r"(_arg5)
         : STR(_ASM_CX), "r11"
     );
     /* clang-format on */
 
-    return rax;
+    return return_code;
 }
 
-static inline long __user_text sys_mmap(void *va, unsigned long order) {
-    register unsigned long rax asm(STR(_ASM_AX));
+#define syscall0(nr)                     syscall((nr), 0, 0, 0, 0, 0)
+#define syscall1(nr, a1)                 syscall((nr), (a1), 0, 0, 0, 0)
+#define syscall2(nr, a1, a2)             syscall((nr), (a1), (a2), 0, 0, 0)
+#define syscall3(nr, a1, a2, a3)         syscall((nr), (a1), (a2), (a3), 0, 0)
+#define syscall4(nr, a1, a2, a3, a4)     syscall((nr), (a1), (a2), (a3), (a4), 0)
+#define syscall5(nr, a1, a2, a3, a4, a5) syscall((nr), (a1), (a2), (a3), (a4), (a5))
 
-    /* clang-format off */
-    asm volatile(
-        "syscall"
-        : "=A"(rax)
-        : "0"(SYSCALL_MMAP), "D"(va), "S"(order)
-        : STR(_ASM_CX), "r11"
-    );
-    /* clang-format on */
-
-    return rax;
+static inline void __user_text sys_exit(unsigned long exit_code) {
+    syscall1(SYSCALL_EXIT, exit_code);
 }
 
-static inline long __user_text sys_munmap(void *va, unsigned long order) {
-    register unsigned long rax asm(STR(_ASM_AX));
+static inline long __user_text sys_printf(const char *fmt, va_list args) {
+    return syscall2(SYSCALL_PRINTF, _ul(fmt), _ul(args));
+}
 
-    /* clang-format off */
-    asm volatile(
-        "syscall"
-        ::"A"(SYSCALL_MUNMAP), "D"(va), "S"(order)
-        : STR(_ASM_CX), "r11"
-    );
-    /* clang-format on */
+static inline long __user_text sys_mmap(void *va, unsigned long order) {
+    return syscall2(SYSCALL_MMAP, _ul(va), order);
+}
 
-    return rax;
+static inline long __user_text sys_munmap(void *va, unsigned long order) {
+    return syscall2(SYSCALL_MUNMAP, _ul(va), order);
 }
 
 void __user_text exit(unsigned long exit_code) {
     sys_exit(exit_code);
 }
 
-void __user_text printf(const char *fmt, unsigned long arg1, unsigned long arg2,
-                        unsigned long arg3) {
-    sys_printf(fmt, arg1, arg2, arg3);
+void __user_text printf(const char *fmt, ...) {
+    va_list args;
+
+    va_start(args, fmt);
+    sys_printf(fmt, args);
+    va_end(args);
 }
 
 void *__user_text mmap(void *va, unsigned long order) {
diff --git a/include/arch/x86/asm-macros.h b/include/arch/x86/asm-macros.h
index e1f7f2ef..17a2e415 100644
--- a/include/arch/x86/asm-macros.h
+++ b/include/arch/x86/asm-macros.h
@@ -162,6 +162,14 @@
 #endif
 .endm
 
+.macro SYSRET
+#if defined(__x86_64__)
+    sysretq
+#else
+    sysret
+#endif
+.endm
+
 .macro SET_CR3 val
     push %_ASM_AX
     mov (\val), %_ASM_AX
@@ -169,6 +177,10 @@
     pop %_ASM_AX
 .endm
 
+.macro SWITCH_STACK
+    xchg %_ASM_SP, %gs:usermode_private
+.endm
+
 #define GLOBAL(name) \
     .global name;    \
 name:
@@ -216,8 +228,8 @@ name ## _end:
 /* clang-format off */
 #if defined(__x86_64__)
 #define SAVE_ALL_REGS64() \
-    "push %%r8\n" \
-    "push %%r9\n" \
+    "push %%r8\n"  \
+    "push %%r9\n"  \
     "push %%r10\n" \
     "push %%r11\n" \
     "push %%r12\n" \
@@ -226,14 +238,14 @@ name ## _end:
     "push %%r15\n"
 
 #define RESTORE_ALL_REGS64() \
-    "pop %%" STR(r15) "\n" \
-    "pop %%" STR(r14) "\n" \
-    "pop %%" STR(r13) "\n" \
-    "pop %%" STR(r12) "\n" \
-    "pop %%" STR(r11) "\n" \
-    "pop %%" STR(r10) "\n" \
-    "pop %%" STR(r9) "\n" \
-    "pop %%" STR(r8) "\n"
+    "pop %%r15\n" \
+    "pop %%r14\n" \
+    "pop %%r13\n" \
+    "pop %%r12\n" \
+    "pop %%r11\n" \
+    "pop %%r10\n" \
+    "pop %%r9\n"  \
+    "pop %%r8\n"
 #else
 #define SAVE_ALL_REGS64()
 #define RESTORE_ALL_REGS64()
@@ -260,55 +272,31 @@ name ## _end:
     "pop %%" STR(_ASM_AX) "\n"
 
 #if defined(__x86_64__)
-#define SAVE_CLOBBERED_REGS64() \
-    asm volatile (              \
-        "push %%r8\n"           \
-        "push %%r9\n"           \
-        "push %%r10\n"          \
-        "push %%r11\n"          \
-        "push %%r12\n"          \
-        "push %%r13\n"          \
-        "push %%r14\n"          \
-        "push %%r15\n"          \
-    ::: "memory")
-
-#define RESTORE_CLOBBERED_REGS64() \
-    asm volatile (                 \
-        "pop %%" STR(r15) "\n"     \
-        "pop %%" STR(r14) "\n"     \
-        "pop %%" STR(r13) "\n"     \
-        "pop %%" STR(r12) "\n"     \
-        "pop %%" STR(r11) "\n"     \
-        "pop %%" STR(r10) "\n"     \
-        "pop %%" STR(r9) "\n"      \
-        "pop %%" STR(r8) "\n"      \
-    ::: "r8", "r9", "r10", "r11", "r12", "r13", "r14", "r15")
+#define SAVE_CALLEE_SAVED_REGS64() \
+    "push %%r12\n" \
+    "push %%r13\n" \
+    "push %%r14\n" \
+    "push %%r15\n"
+
+#define RESTORE_CALLEE_SAVED_REGS64() \
+    "pop %%r15\n" \
+    "pop %%r14\n" \
+    "pop %%r13\n" \
+    "pop %%r12\n"
 #else
-#define SAVE_CLOBBERED_REGS64()
-#define RESTORE_CLOBBERED_REGS64()
+#define SAVE_CALLEE_SAVED_REGS64()
+#define RESTORE_CALLEE_SAVED_REGS64()
 #endif
 
-#define SAVE_CLOBBERED_REGS()       \
-    asm volatile (                  \
-        "push %%" STR(_ASM_CX) "\n" \
-        "push %%" STR(_ASM_DX) "\n" \
-        "push %%" STR(_ASM_SI) "\n" \
-        "push %%" STR(_ASM_DI) "\n" \
-        "push %%" STR(_ASM_BP) "\n" \
-    ::: "memory");                  \
-    SAVE_CLOBBERED_REGS64()
-
-#define RESTORE_CLOBBERED_REGS()    \
-    RESTORE_CLOBBERED_REGS64();     \
-    asm volatile (                  \
-        "pop %%" STR(_ASM_BP) "\n"  \
-        "pop %%" STR(_ASM_DI) "\n"  \
-        "pop %%" STR(_ASM_SI) "\n"  \
-        "pop %%" STR(_ASM_DX) "\n"  \
-        "pop %%" STR(_ASM_CX) "\n"  \
-    ::: STR(_ASM_BP), STR(_ASM_DI), \
-    STR(_ASM_SI), STR(_ASM_DX), STR(_ASM_CX))
-/* clang-format on */
+#define SAVE_CALLEE_SAVED_REGS() \
+    "push %%" STR(_ASM_BX) "\n"  \
+    "push %%" STR(_ASM_BP) "\n"  \
+    SAVE_CALLEE_SAVED_REGS64()
+
+#define RESTORE_CALLEE_SAVED_REGS() \
+    RESTORE_CALLEE_SAVED_REGS64()   \
+    "pop %%" STR(_ASM_BP) "\n"      \
+    "pop %%" STR(_ASM_BX) "\n"
 
 #if defined(__x86_64__)
 #define POPF() "popfq\n"
diff --git a/include/compiler.h b/include/compiler.h
index f77ae2a0..c3957ef1 100644
--- a/include/compiler.h
+++ b/include/compiler.h
@@ -148,4 +148,10 @@ typedef uint64_t off_t;
 
 #define __always_inline __inline __attribute__((always_inline))
 
+#define USTR(str)                                                                        \
+    ({                                                                                   \
+        static char _str[] __user_data = str;                                            \
+        _str;                                                                            \
+    })
+
 #endif /* KTF_COMPILER_H */
diff --git a/include/console.h b/include/console.h
index c9b07f47..5181bd17 100644
--- a/include/console.h
+++ b/include/console.h
@@ -35,6 +35,7 @@ struct console_callback_entry {
 };
 typedef struct console_callback_entry console_callback_entry_t;
 
+extern void vprintk(const char *fmt, va_list args);
 extern void printk(const char *fmt, ...);
 
 #define dprintk(fmt, ...)                                                                \
diff --git a/include/lib.h b/include/lib.h
index 78568583..a4f08ecd 100644
--- a/include/lib.h
+++ b/include/lib.h
@@ -332,10 +332,6 @@ static inline void swapgs(void) {
     asm volatile("swapgs");
 }
 
-static inline void syscall(void) {
-    asm volatile("syscall");
-}
-
 static inline void sysret(void) {
 #if defined(__i386__)
     asm volatile("sysret");
diff --git a/include/usermode.h b/include/usermode.h
index f82e7275..cf0148c2 100644
--- a/include/usermode.h
+++ b/include/usermode.h
@@ -40,21 +40,15 @@ static inline bool enter_from_usermode(uint16_t cs) {
     return (cs & 0x3) == 0x3;
 }
 
-static inline void goto_syscall_exit(long exit_code) {
-    swapgs();
-    asm volatile("jmp syscall_handler" ::"A"(SYSCALL_EXIT), "D"(exit_code));
-}
-
 /* External declarations */
 
 extern unsigned long enter_usermode(task_func_t fn, void *fn_arg, void *user_stack);
-extern void __naked syscall_handler(void);
+extern void syscall_handler_entry(void);
 
 extern void init_usermode(percpu_t *percpu);
 
 extern void __user_text exit(unsigned long exit_code);
-extern void __user_text printf(const char *fmt, unsigned long arg1, unsigned long arg2,
-                               unsigned long arg3);
+extern void __user_text printf(const char *fmt, ...);
 extern void *__user_text mmap(void *va, unsigned long order);
 extern void __user_text munmap(void *va, unsigned long order);
 
diff --git a/tests/unittests.c b/tests/unittests.c
index 0b40974f..954511f1 100644
--- a/tests/unittests.c
+++ b/tests/unittests.c
@@ -72,21 +72,19 @@ static unsigned long test_kernel_task_func(void *arg) {
 }
 
 static unsigned long __user_text test_user_task_func1(void *arg) {
-    static char *fmt_printf __user_data = "printf: %u %x %d\n";
 
-    printf(fmt_printf, 1234, 0x41414141, 9);
-    printf(fmt_printf, 1235, 0x42424242, -9);
+    printf(USTR("printf: %u %x %d\n"), 1234, 0x41414141, 9);
+    printf(USTR("printf: %u %x %d\n"), 1235, 0x42424242, -9);
 
     exit(9);
     return 0;
 }
 
 static unsigned long __user_text test_user_task_func2(void *arg) {
-    static char *fmt_mmap __user_data = "mmap: %lx\n";
     void *va;
 
     va = mmap(_ptr(0xfff80000), PAGE_ORDER_4K);
-    printf(fmt_mmap, _ul(va), 0, 0);
+    printf(USTR("mmap: %lx\n"), _ul(va));
 
     memset(va, 0xcc, 0x1000);
     ((void (*)(void)) va)();