diff --git a/src/decode.c b/src/decode.c
index 4dbce5bd..248db479 100644
--- a/src/decode.c
+++ b/src/decode.c
@@ -907,9 +907,8 @@ static inline bool op_system(rv_insn_t *ir, const uint32_t insn)
     default: /* illegal instruction */
         return false;
     }
-    if (!csr_is_writable(ir->imm) && ir->rs1 != rv_reg_zero)
-        return false;
-    return true;
+
+    return csr_is_writable(ir->imm) || (ir->rs1 == rv_reg_zero);
 }
 
 /* MISC-MEM: I-type
diff --git a/src/emulate.c b/src/emulate.c
index 52e1f037..03aae0d0 100644
--- a/src/emulate.c
+++ b/src/emulate.c
@@ -41,18 +41,21 @@ extern struct target_ops gdbstub_ops;
 #define IF_rs2(i, r) (i->rs2 == rv_reg_##r)
 #define IF_imm(i, v) (i->imm == v)
 
-/* RISC-V exception code list */
+/* RISC-V trap code list */
 /* clang-format off */
-#define RV_TRAP_LIST                                               \
-    IIF(RV32_HAS(EXT_C))(,                                         \
-        _(insn_misaligned, 0) /* Instruction address misaligned */ \
-    )                                                              \
-    _(illegal_insn, 2)     /* Illegal instruction */               \
-    _(breakpoint, 3)       /* Breakpoint */                        \
-    _(load_misaligned, 4)  /* Load address misaligned */           \
-    _(store_misaligned, 6) /* Store/AMO address misaligned */      \
-    IIF(RV32_HAS(SYSTEM))(,                                        \
-        _(ecall_M, 11)     /* Environment call from M-mode */      \
+#define RV_TRAP_LIST                                                                  \
+    IIF(RV32_HAS(EXT_C))(,                                                            \
+        _(insn_misaligned, 0)              /* Instruction address misaligned */       \
+    )                                                                                 \
+    _(illegal_insn, 2)                     /* Illegal instruction */                  \
+    _(breakpoint, 3)                       /* Breakpoint */                           \
+    _(load_misaligned, 4)                  /* Load address misaligned */              \
+    _(store_misaligned, 6)                 /* Store/AMO address misaligned */         \
+    IIF(RV32_HAS(SYSTEM))(                                                            \
+        _(pagefault_insn, 12)              /* Instruction page fault */               \
+        _(pagefault_load, 13)              /* Load page fault */                      \
+        _(pagefault_store, 15),            /* Store page fault */                     \
+        _(ecall_M, 11)                     /* Environment call from M-mode */         \
     )
 /* clang-format on */
 
@@ -75,9 +78,7 @@ static void rv_trap_default_handler(riscv_t *rv)
  * the registered trap handler, PC by PC. Once the trap is handled,
  * resume the previous execution flow where cause the trap.
  *
- * Since the system emulation has not yet included in rv32emu, the page
- * fault is not practical in current test suite. Instead, we try to
- * emulate the misaligned handling in the test suite.
+ * Now, rv32emu supports misaligned access and page fault handling.
  */
 #if RV32_HAS(SYSTEM)
 static void trap_handler(riscv_t *rv);
@@ -532,7 +533,7 @@ static bool do_fuse3(riscv_t *rv, rv_insn_t *ir, uint64_t cycle, uint32_t PC)
     for (int i = 0; i < ir->imm2; i++) {
         uint32_t addr = rv->X[fuse[i].rs1] + fuse[i].imm;
         RV_EXC_MISALIGN_HANDLER(3, store, false, 1);
-        rv->io.mem_write_w(addr, rv->X[fuse[i].rs2]);
+        rv->io.mem_write_w(rv, addr, rv->X[fuse[i].rs2]);
     }
     PC += ir->imm2 * 4;
     if (unlikely(RVOP_NO_NEXT(ir))) {
@@ -556,7 +557,7 @@ static bool do_fuse4(riscv_t *rv, rv_insn_t *ir, uint64_t cycle, uint32_t PC)
     for (int i = 0; i < ir->imm2; i++) {
         uint32_t addr = rv->X[fuse[i].rs1] + fuse[i].imm;
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        rv->X[fuse[i].rd] = rv->io.mem_read_w(addr);
+        rv->X[fuse[i].rd] = rv->io.mem_read_w(rv, addr);
     }
     PC += ir->imm2 * 4;
     if (unlikely(RVOP_NO_NEXT(ir))) {
@@ -666,7 +667,7 @@ static void block_translate(riscv_t *rv, block_t *block)
             prev_ir->next = ir;
 
         /* fetch the next instruction */
-        const uint32_t insn = rv->io.mem_ifetch(block->pc_end);
+        const uint32_t insn = rv->io.mem_ifetch(rv, block->pc_end);
 
         /* decode the instruction */
         if (!rv_decode(ir, insn)) {
@@ -1122,24 +1123,8 @@ void rv_step(void *arg)
 }
 
 #if RV32_HAS(SYSTEM)
-static void trap_handler(riscv_t *rv)
-{
-    rv_insn_t *ir = mpool_alloc(rv->block_ir_mp);
-    assert(ir);
-
-    /* set to false by sret/mret implementation */
-    uint32_t insn;
-    while (rv->is_trapped && !rv_has_halted(rv)) {
-        insn = rv->io.mem_ifetch(rv->PC);
-        assert(insn);
-
-        rv_decode(ir, insn);
-        ir->impl = dispatch_table[ir->opcode];
-        rv->compressed = is_compressed(insn);
-        ir->impl(rv, ir, rv->csr_cycle, rv->PC);
-    }
-}
-#endif
+#include "system.c"
+#endif /* SYSTEM */
 
 void ebreak_handler(riscv_t *rv)
 {
diff --git a/src/feature.h b/src/feature.h
index ee27936a..93eb6160 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -63,5 +63,10 @@
 #define RV32_FEATURE_T2C 0
 #endif
 
+/* System */
+#ifndef RV32_FEATURE_SYSTEM
+#define RV32_FEATURE_SYSTEM 0
+#endif
+
 /* Feature test macro */
 #define RV32_HAS(x) RV32_FEATURE_##x
diff --git a/src/gdbstub.c b/src/gdbstub.c
index 663458d6..d9d635b7 100644
--- a/src/gdbstub.c
+++ b/src/gdbstub.c
@@ -55,7 +55,7 @@ static int rv_read_mem(void *args, size_t addr, size_t len, void *val)
          * an invalid address. We may have to do error handling in the
          * mem_read_* function directly.
          */
-        *((uint8_t *) val + i) = rv->io.mem_read_b(addr + i);
+        *((uint8_t *) val + i) = rv->io.mem_read_b(rv, addr + i);
     }
 
     return err;
@@ -66,7 +66,7 @@ static int rv_write_mem(void *args, size_t addr, size_t len, void *val)
     riscv_t *rv = (riscv_t *) args;
 
     for (size_t i = 0; i < len; i++)
-        rv->io.mem_write_b(addr + i, *((uint8_t *) val + i));
+        rv->io.mem_write_b(rv, addr + i, *((uint8_t *) val + i));
 
     return 0;
 }
diff --git a/src/main.c b/src/main.c
index 58480e07..d9b261e6 100644
--- a/src/main.c
+++ b/src/main.c
@@ -217,12 +217,21 @@ int main(int argc, char **args)
         .log_level = 0,
         .run_flag = run_flag,
         .profile_output_file = prof_out_file,
+#if RV32_HAS(SYSTEM)
+        .data.system = malloc(sizeof(vm_system_t)),
+#else
         .data.user = malloc(sizeof(vm_user_t)),
+#endif
         .cycle_per_step = CYCLE_PER_STEP,
         .allow_misalign = opt_misaligned,
     };
+#if RV32_HAS(SYSTEM)
+    assert(attr.data.system);
+    attr.data.system->elf_program = opt_prog_name;
+#else
     assert(attr.data.user);
     attr.data.user->elf_program = opt_prog_name;
+#endif
 
     /* create the RISC-V runtime */
     rv = rv_create(&attr);
diff --git a/src/riscv.c b/src/riscv.c
index ce77a5a8..20a488d3 100644
--- a/src/riscv.c
+++ b/src/riscv.c
@@ -165,8 +165,10 @@ void rv_remap_stdstream(riscv_t *rv, fd_stream_pair_t *fsp, uint32_t fsp_size)
 #define MEMIO(op) on_mem_##op
 #define IO_HANDLER_IMPL(type, op, RW)                                     \
     static IIF(RW)(                                                       \
-        /* W */ void MEMIO(op)(riscv_word_t addr, riscv_##type##_t data), \
-        /* R */ riscv_##type##_t MEMIO(op)(riscv_word_t addr))            \
+        /* W */ void MEMIO(op)(UNUSED riscv_t * rv, riscv_word_t addr,    \
+                               riscv_##type##_t data),                    \
+        /* R */ riscv_##type##_t MEMIO(op)(UNUSED riscv_t * rv,           \
+                                           riscv_word_t addr))            \
     {                                                                     \
         IIF(RW)                                                           \
         (memory_##op(addr, (uint8_t *) &data), return memory_##op(addr)); \
@@ -260,9 +262,37 @@ riscv_t *rv_create(riscv_user_t rv_attr)
             .on_memset = memset_handler,
         };
         memcpy(&rv->io, &io, sizeof(riscv_io_t));
-    } else {
-        /* TODO: system emulator */
     }
+#if RV32_HAS(SYSTEM)
+    else {
+        /*
+         * TODO: system emulator
+         * e.g., kernel image, dtb, rootfs
+         *
+         * The test suite is compiled into a single ELF file, so load it as
+         * an ELF executable, just like a userspace ELF.
+         *
+         */
+        elf_t *elf = elf_new();
+        assert(elf && elf_open(elf, (attr->data.system)->elf_program));
+
+        const struct Elf32_Sym *end;
+        if ((end = elf_get_symbol(elf, "_end")))
+            attr->break_addr = end->st_value;
+
+        assert(elf_load(elf, attr->mem));
+
+        /* set the entry pc */
+        const struct Elf32_Ehdr *hdr = get_elf_header(elf);
+        assert(rv_set_pc(rv, hdr->e_entry));
+
+        elf_delete(elf);
+
+        /* this variable has external linkage to mmu_io defined in system.c */
+        extern riscv_io_t mmu_io;
+        memcpy(&rv->io, &mmu_io, sizeof(riscv_io_t));
+    }
+#endif /* SYSTEM */
 
     /* default standard stream.
      * rv_remap_stdstream can be called to overwrite them
@@ -303,20 +333,6 @@ riscv_t *rv_create(riscv_user_t rv_attr)
 #endif
 #endif
 
-#if RV32_HAS(SYSTEM)
-    /*
-     * System simulation defaults to S-mode as
-     * it does not rely on M-mode software like OpenSBI.
-     */
-    rv->priv_mode = RV_PRIV_S_MODE;
-
-    /* not being trapped */
-    rv->is_trapped = false;
-#else
-    /* ISA simulation defaults to M-mode */
-    rv->priv_mode = RV_PRIV_M_MODE;
-#endif
-
     return rv;
 }
 
@@ -356,7 +372,11 @@ void rv_run(riscv_t *rv)
     assert(rv);
 
     vm_attr_t *attr = PRIV(rv);
+#if RV32_HAS(SYSTEM)
+    assert(attr && attr->data.system && attr->data.system->elf_program);
+#else
     assert(attr && attr->data.user && attr->data.user->elf_program);
+#endif
 
     if (attr->run_flag & RV_RUN_TRACE)
         rv_run_and_trace(rv);
@@ -516,6 +536,21 @@ void rv_reset(riscv_t *rv, riscv_word_t pc)
     /* reset sp pointing to argc */
     rv->X[rv_reg_sp] = stack_top;
 
+    /* reset privilege mode */
+#if RV32_HAS(SYSTEM)
+    /*
+     * System simulation defaults to S-mode as
+     * it does not rely on M-mode software like OpenSBI.
+     */
+    rv->priv_mode = RV_PRIV_S_MODE;
+
+    /* not being trapped */
+    rv->is_trapped = false;
+#else
+    /* ISA simulation defaults to M-mode */
+    rv->priv_mode = RV_PRIV_M_MODE;
+#endif
+
     /* reset the csrs */
     rv->csr_mtvec = 0;
     rv->csr_cycle = 0;
@@ -541,7 +576,6 @@ void rv_reset(riscv_t *rv, riscv_word_t pc)
     rv->csr_misa |= MISA_M;
 #endif
 
-
     rv->halt = false;
 }
 
diff --git a/src/riscv.h b/src/riscv.h
index 30d6369b..95f0f808 100644
--- a/src/riscv.h
+++ b/src/riscv.h
@@ -91,10 +91,117 @@ enum {
 #define MISA_A (1 << ('A' - 'A'))
 #define MISA_F (1 << ('F' - 'A'))
 #define MISA_C (1 << ('C' - 'A'))
+
+/* The mstatus register keeps track of and controls the hart’s current operating
+ * state */
+#define MSTATUS_SIE_SHIFT 1
+#define MSTATUS_MIE_SHIFT 3
+#define MSTATUS_SPIE_SHIFT 5
+#define MSTATUS_UBE_SHIFT 6
 #define MSTATUS_MPIE_SHIFT 7
+#define MSTATUS_SPP_SHIFT 8
 #define MSTATUS_MPP_SHIFT 11
+#define MSTATUS_MPRV_SHIFT 17
+#define MSTATUS_SUM_SHIFT 18
+#define MSTATUS_MXR_SHIFT 18
+#define MSTATUS_TVM_SHIFT 20
+#define MSTATUS_TW_SHIFT 21
+#define MSTATUS_TSR_SHIFT 22
+#define MSTATUS_SIE (1 << MSTATUS_SIE_SHIFT)
+#define MSTATUS_MIE (1 << MSTATUS_MIE_SHIFT)
+#define MSTATUS_SPIE (1 << MSTATUS_SPIE_SHIFT)
+#define MSTATUS_UBE (1 << MSTATUS_UBE_SHIFT)
 #define MSTATUS_MPIE (1 << MSTATUS_MPIE_SHIFT)
+#define MSTATUS_SPP (1 << MSTATUS_SPP_SHIFT)
 #define MSTATUS_MPP (3 << MSTATUS_MPP_SHIFT)
+#define MSTATUS_MPRV (1 << MSTATUS_MPRV_SHIFT)
+#define MSTATUS_SUM (1 << MSTATUS_SUM_SHIFT)
+#define MSTATUS_MXR (1 << MSTATUS_MXR_SHIFT)
+#define MSTATUS_TVM (1 << MSTATUS_TVM_SHIFT)
+#define MSTATUS_TW (1 << MSTATUS_TW_SHIFT)
+#define MSTATUS_TSR (1 << MSTATUS_TSR_SHIFT)
+
+/* A restricted view of mstatus */
+#define SSTATUS_SIE_SHIFT 1
+#define SSTATUS_SPIE_SHIFT 5
+#define SSTATUS_UBE_SHIFT 6
+#define SSTATUS_SPP_SHIFT 8
+#define SSTATUS_SUM_SHIFT 18
+#define SSTATUS_MXR_SHIFT 19
+#define SSTATUS_SIE (1 << SSTATUS_SIE_SHIFT)
+#define SSTATUS_SPIE (1 << SSTATUS_SPIE_SHIFT)
+#define SSTATUS_UBE (1 << SSTATUS_UBE_SHIFT)
+#define SSTATUS_SPP (1 << SSTATUS_SPP_SHIFT)
+#define SSTATUS_SUM (1 << SSTATUS_SUM_SHIFT)
+#define SSTATUS_MXR (1 << SSTATUS_MXR_SHIFT)
+
+#define SIP_SSIP_SHIFT 1
+#define SIP_STIP_SHIFT 5
+#define SIP_SEIP_SHIFT 9
+#define SIP_SSIP (1 << SIP_SSIP_SHIFT)
+#define SIP_STIP (1 << SIP_STIP_SHIFT)
+#define SIP_SEIP (1 << SIP_SEIP_SHIFT)
+
+#define RV_PG_SHIFT 12
+#define RV_PG_SIZE (1 << RV_PG_SHIFT)
+
+#define RV_PRIV_U_MODE 0
+#define RV_PRIV_S_MODE 1
+#define RV_PRIV_M_MODE 3
+
+#define PTE_V (1U)
+#define PTE_R (1U << 1)
+#define PTE_W (1U << 2)
+#define PTE_X (1U << 3)
+#define PTE_U (1U << 4)
+#define PTE_G (1U << 5)
+#define PTE_A (1U << 6)
+#define PTE_D (1U << 7)
+
+/*
+ * SBI functions must return a pair of values:
+ *
+ * struct sbiret {
+ *     long error;
+ *     long value;
+ * };
+ *
+ * The error and value field will be set to register a0 and a1 respectively
+ * after the SBI function return. The error field indicate whether the
+ * SBI call is success or not. SBI_SUCCESS indicates success and
+ * SBI_ERR_NOT_SUPPORTED indicates not supported failure. The value field is
+ * the information based on the extension ID(EID) and SBI function ID(FID).
+ *
+ * SBI reference: https://github.com/riscv-non-isa/riscv-sbi-doc
+ *
+ */
+#define SBI_SUCCESS 0
+#define SBI_ERR_NOT_SUPPORTED -2
+
+/*
+ * All of the functions in the base extension must be supported by
+ * all SBI implementations.
+ */
+#define SBI_EID_BASE 0x10
+#define SBI_BASE_GET_SBI_SPEC_VERSION 0
+#define SBI_BASE_GET_SBI_IMPL_ID 1
+#define SBI_BASE_GET_SBI_IMPL_VERSION 2
+#define SBI_BASE_PROBE_EXTENSION 3
+#define SBI_BASE_GET_MVENDORID 4
+#define SBI_BASE_GET_MARCHID 5
+#define SBI_BASE_GET_MIMPID 6
+
+/* Make supervisor to schedule the clock for next timer event. */
+#define SBI_EID_TIMER 0x54494D45
+#define SBI_TIMER_SET_TIMER 0
+
+/* Allows the supervisor to request system-level reboot or shutdown. */
+#define SBI_EID_RST 0x53525354
+#define SBI_RST_SYSTEM_RESET 0
+
+#define RV_MVENDORID 0x12345678
+#define RV_MARCHID ((1ULL << 31) | 1)
+#define RV_MIMPID 1
 
 /* The mstatus register keeps track of and controls the hart’s current operating
  * state */
@@ -211,15 +318,21 @@ typedef softfloat_float32_t riscv_float_t;
 #endif
 
 /* memory read handlers */
-typedef riscv_word_t (*riscv_mem_ifetch)(riscv_word_t addr);
-typedef riscv_word_t (*riscv_mem_read_w)(riscv_word_t addr);
-typedef riscv_half_t (*riscv_mem_read_s)(riscv_word_t addr);
-typedef riscv_byte_t (*riscv_mem_read_b)(riscv_word_t addr);
+typedef riscv_word_t (*riscv_mem_ifetch)(riscv_t *rv, riscv_word_t addr);
+typedef riscv_word_t (*riscv_mem_read_w)(riscv_t *rv, riscv_word_t addr);
+typedef riscv_half_t (*riscv_mem_read_s)(riscv_t *rv, riscv_word_t addr);
+typedef riscv_byte_t (*riscv_mem_read_b)(riscv_t *rv, riscv_word_t addr);
 
 /* memory write handlers */
-typedef void (*riscv_mem_write_w)(riscv_word_t addr, riscv_word_t data);
-typedef void (*riscv_mem_write_s)(riscv_word_t addr, riscv_half_t data);
-typedef void (*riscv_mem_write_b)(riscv_word_t addr, riscv_byte_t data);
+typedef void (*riscv_mem_write_w)(riscv_t *rv,
+                                  riscv_word_t addr,
+                                  riscv_word_t data);
+typedef void (*riscv_mem_write_s)(riscv_t *rv,
+                                  riscv_word_t addr,
+                                  riscv_half_t data);
+typedef void (*riscv_mem_write_b)(riscv_t *rv,
+                                  riscv_word_t addr,
+                                  riscv_byte_t data);
 
 /* system instruction handlers */
 typedef void (*riscv_on_ecall)(riscv_t *rv);
@@ -328,9 +441,20 @@ typedef struct {
     char *elf_program;
 } vm_user_t;
 
+/* FIXME: replace with kernel image, dtb, etc */
+#if RV32_HAS(SYSTEM)
+typedef struct {
+    char *elf_program;
+} vm_system_t;
+#endif /* SYSTEM */
+
 typedef struct {
     vm_user_t *user;
-    /* TODO: system emulator stuff */
+
+#if RV32_HAS(SYSTEM)
+    vm_system_t *system;
+#endif /* SYSTEM */
+
 } vm_data_t;
 
 typedef struct {
diff --git a/src/rv32_template.c b/src/rv32_template.c
index e8f9ee99..8c395de9 100644
--- a/src/rv32_template.c
+++ b/src/rv32_template.c
@@ -155,8 +155,10 @@ RVOP(
     jal,
     {
         const uint32_t pc = PC;
+
         /* Jump */
         PC += ir->imm;
+
         /* link with return address */
         if (ir->rd)
             rv->X[ir->rd] = pc + 4;
@@ -259,11 +261,14 @@ RVOP(
     jalr,
     {
         const uint32_t pc = PC;
+
         /* jump */
         PC = (rv->X[ir->rs1] + ir->imm) & ~1U;
+
         /* link */
         if (ir->rd)
             rv->X[ir->rd] = pc + 4;
+
         /* check instruction misaligned */
 #if !RV32_HAS(EXT_C)
         RV_EXC_MISALIGN_HANDLER(pc, insn, false, 0);
@@ -513,7 +518,7 @@ RVOP(
     lb,
     {
         rv->X[ir->rd] =
-            sign_extend_b(rv->io.mem_read_b(rv->X[ir->rs1] + ir->imm));
+            sign_extend_b(rv->io.mem_read_b(rv, rv->X[ir->rs1] + ir->imm));
     },
     GEN({
         mem;
@@ -530,7 +535,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1] + ir->imm;
         RV_EXC_MISALIGN_HANDLER(1, load, false, 1);
-        rv->X[ir->rd] = sign_extend_h(rv->io.mem_read_s(addr));
+        rv->X[ir->rd] = sign_extend_h(rv->io.mem_read_s(rv, addr));
     },
     GEN({
         mem;
@@ -547,7 +552,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1] + ir->imm;
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        rv->X[ir->rd] = rv->io.mem_read_w(addr);
+        rv->X[ir->rd] = rv->io.mem_read_w(rv, addr);
     },
     GEN({
         mem;
@@ -561,7 +566,7 @@ RVOP(
 /* LBU: Load Byte Unsigned */
 RVOP(
     lbu,
-    { rv->X[ir->rd] = rv->io.mem_read_b(rv->X[ir->rs1] + ir->imm); },
+    { rv->X[ir->rd] = rv->io.mem_read_b(rv, rv->X[ir->rs1] + ir->imm); },
     GEN({
         mem;
         rald, VR0, rs1;
@@ -577,7 +582,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1] + ir->imm;
         RV_EXC_MISALIGN_HANDLER(1, load, false, 1);
-        rv->X[ir->rd] = rv->io.mem_read_s(addr);
+        rv->X[ir->rd] = rv->io.mem_read_s(rv, addr);
     },
     GEN({
         mem;
@@ -597,7 +602,7 @@ RVOP(
 /* SB: Store Byte */
 RVOP(
     sb,
-    { rv->io.mem_write_b(rv->X[ir->rs1] + ir->imm, rv->X[ir->rs2]); },
+    { rv->io.mem_write_b(rv, rv->X[ir->rs1] + ir->imm, rv->X[ir->rs2]); },
     GEN({
         mem;
         rald, VR0, rs1;
@@ -613,7 +618,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1] + ir->imm;
         RV_EXC_MISALIGN_HANDLER(1, store, false, 1);
-        rv->io.mem_write_s(addr, rv->X[ir->rs2]);
+        rv->io.mem_write_s(rv, addr, rv->X[ir->rs2]);
     },
     GEN({
         mem;
@@ -630,7 +635,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1] + ir->imm;
         RV_EXC_MISALIGN_HANDLER(3, store, false, 1);
-        rv->io.mem_write_w(addr, rv->X[ir->rs2]);
+        rv->io.mem_write_w(rv, addr, rv->X[ir->rs2]);
     },
     GEN({
         mem;
@@ -1041,7 +1046,7 @@ RVOP(
         assert; /* FIXME: Implement */
     }))
 
-/* MRET: return from traps in U-mode */
+/* MRET: return from traps in M-mode */
 RVOP(
     mret,
     {
@@ -1373,7 +1378,7 @@ RVOP(
         const uint32_t addr = rv->X[ir->rs1];
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
         if (ir->rd)
-            rv->X[ir->rd] = rv->io.mem_read_w(addr);
+            rv->X[ir->rd] = rv->io.mem_read_w(rv, addr);
         /* skip registration of the 'reservation set'
          * FIXME: unimplemented
          */
@@ -1391,7 +1396,7 @@ RVOP(
          */
         const uint32_t addr = rv->X[ir->rs1];
         RV_EXC_MISALIGN_HANDLER(3, store, false, 1);
-        rv->io.mem_write_w(addr, rv->X[ir->rs2]);
+        rv->io.mem_write_w(rv, addr, rv->X[ir->rs2]);
         rv->X[ir->rd] = 0;
     },
     GEN({
@@ -1404,11 +1409,11 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1];
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        const uint32_t value1 = rv->io.mem_read_w(addr);
+        const uint32_t value1 = rv->io.mem_read_w(rv, addr);
         const uint32_t value2 = rv->X[ir->rs2];
         if (ir->rd)
             rv->X[ir->rd] = value1;
-        rv->io.mem_write_w(addr, value2);
+        rv->io.mem_write_w(rv, addr, value2);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -1420,12 +1425,12 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1];
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        const uint32_t value1 = rv->io.mem_read_w(addr);
+        const uint32_t value1 = rv->io.mem_read_w(rv, addr);
         const uint32_t value2 = rv->X[ir->rs2];
         if (ir->rd)
             rv->X[ir->rd] = value1;
         const uint32_t res = value1 + value2;
-        rv->io.mem_write_w(addr, res);
+        rv->io.mem_write_w(rv, addr, res);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -1437,12 +1442,12 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1];
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        const uint32_t value1 = rv->io.mem_read_w(addr);
+        const uint32_t value1 = rv->io.mem_read_w(rv, addr);
         const uint32_t value2 = rv->X[ir->rs2];
         if (ir->rd)
             rv->X[ir->rd] = value1;
         const uint32_t res = value1 ^ value2;
-        rv->io.mem_write_w(addr, res);
+        rv->io.mem_write_w(rv, addr, res);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -1454,12 +1459,12 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1];
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        const uint32_t value1 = rv->io.mem_read_w(addr);
+        const uint32_t value1 = rv->io.mem_read_w(rv, addr);
         const uint32_t value2 = rv->X[ir->rs2];
         if (ir->rd)
             rv->X[ir->rd] = value1;
         const uint32_t res = value1 & value2;
-        rv->io.mem_write_w(addr, res);
+        rv->io.mem_write_w(rv, addr, res);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -1471,12 +1476,12 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1];
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        const uint32_t value1 = rv->io.mem_read_w(addr);
+        const uint32_t value1 = rv->io.mem_read_w(rv, addr);
         const uint32_t value2 = rv->X[ir->rs2];
         if (ir->rd)
             rv->X[ir->rd] = value1;
         const uint32_t res = value1 | value2;
-        rv->io.mem_write_w(addr, res);
+        rv->io.mem_write_w(rv, addr, res);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -1488,14 +1493,14 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1];
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        const uint32_t value1 = rv->io.mem_read_w(addr);
+        const uint32_t value1 = rv->io.mem_read_w(rv, addr);
         const uint32_t value2 = rv->X[ir->rs2];
         if (ir->rd)
             rv->X[ir->rd] = value1;
         const int32_t a = value1;
         const int32_t b = value2;
         const uint32_t res = a < b ? value1 : value2;
-        rv->io.mem_write_w(addr, res);
+        rv->io.mem_write_w(rv, addr, res);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -1507,14 +1512,14 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1];
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        const uint32_t value1 = rv->io.mem_read_w(addr);
+        const uint32_t value1 = rv->io.mem_read_w(rv, addr);
         const uint32_t value2 = rv->X[ir->rs2];
         if (ir->rd)
             rv->X[ir->rd] = value1;
         const int32_t a = value1;
         const int32_t b = value2;
         const uint32_t res = a > b ? value1 : value2;
-        rv->io.mem_write_w(addr, res);
+        rv->io.mem_write_w(rv, addr, res);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -1526,12 +1531,12 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1];
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        const uint32_t value1 = rv->io.mem_read_w(addr);
+        const uint32_t value1 = rv->io.mem_read_w(rv, addr);
         const uint32_t value2 = rv->X[ir->rs2];
         if (ir->rd)
             rv->X[ir->rd] = value1;
         const uint32_t ures = value1 < value2 ? value1 : value2;
-        rv->io.mem_write_w(addr, ures);
+        rv->io.mem_write_w(rv, addr, ures);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -1543,12 +1548,12 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1];
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        const uint32_t value1 = rv->io.mem_read_w(addr);
+        const uint32_t value1 = rv->io.mem_read_w(rv, addr);
         const uint32_t value2 = rv->X[ir->rs2];
         if (ir->rd)
             rv->X[ir->rd] = value1;
         const uint32_t ures = value1 > value2 ? value1 : value2;
-        rv->io.mem_write_w(addr, ures);
+        rv->io.mem_write_w(rv, addr, ures);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -1565,7 +1570,7 @@ RVOP(
         /* copy into the float register */
         const uint32_t addr = rv->X[ir->rs1] + ir->imm;
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        rv->F[ir->rd].v = rv->io.mem_read_w(addr);
+        rv->F[ir->rd].v = rv->io.mem_read_w(rv, addr);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -1578,7 +1583,7 @@ RVOP(
         /* copy from float registers */
         const uint32_t addr = rv->X[ir->rs1] + ir->imm;
         RV_EXC_MISALIGN_HANDLER(3, store, false, 1);
-        rv->io.mem_write_w(addr, rv->F[ir->rs2].v);
+        rv->io.mem_write_w(rv, addr, rv->F[ir->rs2].v);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -1939,7 +1944,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1] + (uint32_t) ir->imm;
         RV_EXC_MISALIGN_HANDLER(3, load, true, 1);
-        rv->X[ir->rd] = rv->io.mem_read_w(addr);
+        rv->X[ir->rd] = rv->io.mem_read_w(rv, addr);
     },
     GEN({
         mem;
@@ -1960,7 +1965,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1] + (uint32_t) ir->imm;
         RV_EXC_MISALIGN_HANDLER(3, store, true, 1);
-        rv->io.mem_write_w(addr, rv->X[ir->rs2]);
+        rv->io.mem_write_w(rv, addr, rv->X[ir->rs2]);
     },
     GEN({
         mem;
@@ -2313,7 +2318,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[rv_reg_sp] + ir->imm;
         RV_EXC_MISALIGN_HANDLER(3, load, true, 1);
-        rv->X[ir->rd] = rv->io.mem_read_w(addr);
+        rv->X[ir->rd] = rv->io.mem_read_w(rv, addr);
     },
     GEN({
         mem;
@@ -2422,7 +2427,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[rv_reg_sp] + ir->imm;
         RV_EXC_MISALIGN_HANDLER(3, store, true, 1);
-        rv->io.mem_write_w(addr, rv->X[ir->rs2]);
+        rv->io.mem_write_w(rv, addr, rv->X[ir->rs2]);
     },
     GEN({
         mem;
@@ -2441,7 +2446,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[rv_reg_sp] + ir->imm;
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        rv->F[ir->rd].v = rv->io.mem_read_w(addr);
+        rv->F[ir->rd].v = rv->io.mem_read_w(rv, addr);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -2453,7 +2458,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[rv_reg_sp] + ir->imm;
         RV_EXC_MISALIGN_HANDLER(3, store, false, 1);
-        rv->io.mem_write_w(addr, rv->F[ir->rs2].v);
+        rv->io.mem_write_w(rv, addr, rv->F[ir->rs2].v);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -2465,7 +2470,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1] + (uint32_t) ir->imm;
         RV_EXC_MISALIGN_HANDLER(3, load, false, 1);
-        rv->F[ir->rd].v = rv->io.mem_read_w(addr);
+        rv->F[ir->rd].v = rv->io.mem_read_w(rv, addr);
     },
     GEN({
         assert; /* FIXME: Implement */
@@ -2477,7 +2482,7 @@ RVOP(
     {
         const uint32_t addr = rv->X[ir->rs1] + (uint32_t) ir->imm;
         RV_EXC_MISALIGN_HANDLER(3, store, false, 1);
-        rv->io.mem_write_w(addr, rv->F[ir->rs2].v);
+        rv->io.mem_write_w(rv, addr, rv->F[ir->rs2].v);
     },
     GEN({
         assert; /* FIXME: Implement */
diff --git a/src/syscall.c b/src/syscall.c
index 1c25408a..01021218 100644
--- a/src/syscall.c
+++ b/src/syscall.c
@@ -34,9 +34,11 @@
     _(brk,                  214)           \
     _(clock_gettime,        403)           \
     _(open,                 1024)          \
-    _(sbi_base,             0x10)          \
-    _(sbi_timer,            0x54494D45)    \
-    _(sbi_rst,              0x53525354)    \
+    IIF(RV32_HAS(SYSTEM))(                 \
+        _(sbi_base,         0x10)          \
+        _(sbi_timer,        0x54494D45)    \
+        _(sbi_rst,          0x53525354)    \
+    )                                      \
     IIF(RV32_HAS(SDL))(                    \
         _(draw_frame,       0xBEEF)        \
         _(setup_queue,      0xC0DE)        \
@@ -372,6 +374,8 @@ extern void syscall_setup_audio(riscv_t *rv);
 extern void syscall_control_audio(riscv_t *rv);
 #endif
 
+#if RV32_HAS(SYSTEM)
+
 /* SBI related system calls */
 static void syscall_sbi_timer(riscv_t *rv)
 {
@@ -459,6 +463,7 @@ static void syscall_sbi_rst(riscv_t *rv)
         break;
     }
 }
+#endif /* SYSTEM */
 
 void syscall_handler(riscv_t *rv)
 {
diff --git a/tests/system/pgfault/Makefile b/tests/system/pgfault/Makefile
new file mode 100644
index 00000000..259f3eef
--- /dev/null
+++ b/tests/system/pgfault/Makefile
@@ -0,0 +1,36 @@
+PREFIX ?= riscv-none-elf-
+ARCH = -march=rv32izicsr
+LINKER_SCRIPT = linker.ld
+
+DEBUG_CFLAGS = -g
+CFLAGS = -c -march=rv32i_zicsr
+LDFLAGS = -T
+EXEC = vm.elf
+
+CC = $(PREFIX)gcc
+AS = $(PREFIX)as
+LD = $(PREFIX)ld
+OBJDUMP = $(PREFIX)objdump
+
+# Locate libgcc.a dynamically, as the itoa function uses division and modulo operators,
+# which rely on __udivsi3 and __umodsi3, both provided by libgcc
+LIBGCC = $(shell $(CC) -print-libgcc-file-name)
+LIBGCC_PATH = $(shell dirname $(LIBGCC))
+
+deps = main.o setup.o vm_setup.o
+
+all:
+	$(CC) $(DEBUG_CLAGS) $(CFLAGS) main.c
+	$(CC) $(DEBUG_CLAGS) $(CFLAGS) vm_setup.c
+	$(AS) $(DEBUG_CLAGS) $(ARCH) setup.S -o setup.o
+	$(LD) $(LDFLAGS) $(LINKER_SCRIPT) -o $(EXEC) $(deps) -L$(LIBGCC_PATH) -lgcc
+
+dump:
+	$(OBJDUMP) -DS $(EXEC) | less
+
+# __udivsi3 and __umodsi3 can be found
+nm:
+	$(NM) $(LIBGCC) | less
+
+clean:
+	rm $(EXEC) $(deps)
diff --git a/tests/system/pgfault/linker.ld b/tests/system/pgfault/linker.ld
new file mode 100644
index 00000000..74546df2
--- /dev/null
+++ b/tests/system/pgfault/linker.ld
@@ -0,0 +1,31 @@
+OUTPUT_ARCH( "riscv" )
+
+ENTRY(_start)
+
+SECTIONS
+{
+  . = 0x00000000;
+  .text.main : { *(.text.main) }
+  . = ALIGN(0x1000);
+  .mystring : { *(.mystring) }
+  .data.main : { *(.data.main) }
+  .bss.main : { *(.bss.main) }
+
+  .text : {
+    . = 0x7fffeffc;
+    *(.text.setup)
+    *(.text.vm_setup)
+  }
+  . = ALIGN(0x1000);
+  .data : {
+    *(.data.setup)
+    *(.data.vm_setup)
+  }
+  .bss : {
+    *(.bss.setup)
+    *(.bss.vm_setup)
+  }
+
+  . = ALIGN(0x1000);
+  _end = .;
+}
diff --git a/tests/system/pgfault/main.c b/tests/system/pgfault/main.c
new file mode 100644
index 00000000..7036a3aa
--- /dev/null
+++ b/tests/system/pgfault/main.c
@@ -0,0 +1,102 @@
+#define SECTION_TEXT_MAIN __attribute__((section(".text.main")))
+#define SECTION_DATA_MAIN __attribute__((section(".data.main")))
+#define SECTION_BSS_MAIN __attribute__((section(".bss.main")))
+
+#define printstr(ptr, length)                   \
+    do {                                        \
+        asm volatile(                           \
+            "add a7, x0, 0x40;"                 \
+            "add a0, x0, 0x1;" /* stdout */     \
+            "add a1, x0, %0;"                   \
+            "mv a2, %1;" /* length character */ \
+            "ecall;"                            \
+            :                                   \
+            : "r"(ptr), "r"(length)             \
+            : "a0", "a1", "a2", "a7");          \
+    } while (0)
+
+#define TEST_OUTPUT(msg, length) printstr(msg, length)
+
+#define TEST_LOGGER(msg)                     \
+    {                                        \
+        char _msg[] = msg;                   \
+        TEST_OUTPUT(_msg, sizeof(_msg) - 1); \
+    }
+
+#define SUCCESS 0
+#define FAIL 1
+
+__attribute__((section(".mystring"))) const char pagefault_load_str[] =
+    "rv32emu";
+
+extern void _exit(int status);
+
+int SECTION_TEXT_MAIN main()
+{
+    /* instruction fetch page fault test */
+    int x = 100; /* trigger instruction page fault */
+    int y = 200;
+    int z = x + y;
+    TEST_LOGGER("INSTRUCTION FETCH PAGE FAULT TEST PASSED!\n");
+
+    char buf[8];
+    /* data load page fault test */
+    /* Clear buffer */
+    for (int i = 0; i < 8; i++) {
+        buf[i] = 0;
+    }
+
+    char *qtr = (char *) 0x1000; /* first data page */
+    /* should trigger load page fault and load pagefault_load_str */
+    for (int i = 0; i < 8; i++) {
+        qtr = (char *) 0x1000; /* FIXME: weird result without this */
+        buf[i] = *(qtr + i);
+    }
+
+    for (int i = 0; i < 8; i++) { /* should not trigger load page fault */
+        if (buf[i] != *(qtr + i)) {
+            TEST_LOGGER("[LOAD PAGE FAULT TEST] rv32emu string not match\n")
+            _exit(FAIL);
+        }
+    }
+    TEST_LOGGER("LOAD PAGE FAULT TEST PASSED!\n");
+
+    /* data store page fault test */
+    /* Clear buffer */
+    for (int i = 0; i < 8; i++) {
+        buf[i] = 0;
+    }
+
+    char *ptr = (char *) 0x2000; /* second data page */
+    *ptr = 'r';                  /* trigger store page fault */
+    ptr = (char *) 0x2000;       /* FIXME: weird result without this */
+    *(ptr + 1) = 'v';            /* should not trigger store page fault */
+    ptr = (char *) 0x2000;       /* FIXME: weird result without this */
+    *(ptr + 2) = '3';            /* should not trigger store page fault */
+    ptr = (char *) 0x2000;       /* FIXME: weird result without this */
+    *(ptr + 3) = '2';            /* should not trigger store page fault */
+    ptr = (char *) 0x2000;       /* FIXME: weird result without this */
+    *(ptr + 4) = 'e';            /* should not trigger store page fault */
+    ptr = (char *) 0x2000;       /* FIXME: weird result without this */
+    *(ptr + 5) = 'm';            /* should not trigger store page fault */
+    ptr = (char *) 0x2000;       /* FIXME: weird result without this */
+    *(ptr + 6) = 'u';            /* should not trigger store page fault */
+    ptr = (char *) 0x2000;       /* FIXME: weird result without this */
+    *(ptr + 7) = '\0';           /* should not trigger store page fault */
+
+    /* should not trigger load page fault */
+    for (int i = 0; i < 8; i++) {
+        buf[i] = *(ptr + i);
+    }
+
+    /* should not trigger load page fault */
+    for (int i = 0; i < 8; i++) {
+        if (buf[i] != *(ptr + i)) {
+            TEST_LOGGER("[STORE PAGE FAULT TEST] rv32emu string not match\n")
+            _exit(FAIL);
+        }
+    }
+    TEST_LOGGER("STORE PAGE FAULT TEST PASSED!\n");
+
+    _exit(SUCCESS);
+}
diff --git a/tests/system/pgfault/setup.S b/tests/system/pgfault/setup.S
new file mode 100644
index 00000000..177cdfb2
--- /dev/null
+++ b/tests/system/pgfault/setup.S
@@ -0,0 +1,162 @@
+.section .text.setup
+TRAPFRAME_SIZE = 35 * 4
+PG_SIZE = 4096
+STACK_TOP = _end + PG_SIZE
+
+# FIXME: implement proper machine trap vector
+# Since I assume that all interrupts and exceptions are
+# handled by S-mode software, so the machine trap
+# vector is not that much important in here
+machine_trap_vector:
+    j _exit;
+
+.globl _start
+_start:
+    # init regs
+    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
+
+    la sp, STACK_TOP - TRAPFRAME_SIZE
+    csrw mscratch, sp;
+
+    #la t0, machine_trap_vector
+    #csrw mtvec, t0
+    #csrr t0, mtvec # for debugging
+
+    # init virtual memory
+    j vm_boot
+
+.globl _exit
+_exit:
+    li a7, 93
+    ecall
+
+.globl user_entry
+user_entry:
+    la sp, STACK_TOP - TRAPFRAME_SIZE
+    jalr x0, 0x4  # jump to user space main
+
+.globl supervisor_trap_entry
+supervisor_trap_entry:
+    # get trapframe pointer (save a0 into scratch)
+    csrrw a0, sscratch, a0;
+
+    # push regs into trapframe
+    sw x1, 0*4(a0);
+    sw x2, 1*4(a0);
+    sw x3, 2*4(a0);
+    sw x4, 3*4(a0);
+    sw x5, 4*4(a0);
+    sw x6, 5*4(a0);
+    sw x7, 6*4(a0);
+    sw x8, 7*4(a0);
+    sw x9, 8*4(a0);
+    sw x11, 10*4(a0);
+    sw x12, 11*4(a0);
+    sw x13, 12*4(a0);
+    sw x14, 13*4(a0);
+    sw x15, 14*4(a0);
+    sw x16, 15*4(a0);
+    sw x17, 16*4(a0);
+    sw x18, 17*4(a0);
+    sw x19, 18*4(a0);
+    sw x20, 19*4(a0);
+    sw x21, 20*4(a0);
+    sw x22, 21*4(a0);
+    sw x23, 22*4(a0);
+    sw x24, 23*4(a0);
+    sw x25, 24*4(a0);
+    sw x26, 25*4(a0);
+    sw x27, 26*4(a0);
+    sw x28, 27*4(a0);
+    sw x29, 28*4(a0);
+    sw x20, 29*4(a0);
+    sw x31, 30*4(a0);
+
+    # load stack pointer and save trapframe pointer into scratch
+    csrrw t0, sscratch, a0;
+    sw t0, 9*4(a0);
+
+    # push status, epc, badaddr, cause
+    csrr t0, sstatus;
+    sw t0, 31*4(a0);
+    csrr t0, sepc;
+    sw t0, 32*4(a0);
+    csrr t0, stval;
+    sw t0, 33*4(a0);
+    csrr t0, scause;
+    sw t0, 34*4(a0);
+
+    csrr sp, sscratch;
+    j handle_trap;
+
+.globl pop_tf
+pop_tf:
+    # a0 need to save trapframe pointer
+    # pop epc and regs from trapframe
+    lw t0, 32*4(a0)
+    csrw sepc, t0
+    lw x1, 0*4(a0)
+    lw x2, 1*4(a0)
+    lw x3, 2*4(a0)
+    lw x4, 3*4(a0)
+    lw x5, 4*4(a0)
+    lw x6, 5*4(a0)
+    lw x7, 6*4(a0)
+    lw x8, 7*4(a0)
+    lw x9, 8*4(a0)
+    lw x11, 10*4(a0)
+    lw x12, 11*4(a0)
+    lw x13, 12*4(a0)
+    lw x14, 13*4(a0)
+    lw x15, 14*4(a0)
+    lw x16, 15*4(a0)
+    lw x17, 16*4(a0)
+    lw x18, 17*4(a0)
+    lw x19, 18*4(a0)
+    lw x20, 19*4(a0)
+    lw x21, 20*4(a0)
+    lw x22, 21*4(a0)
+    lw x23, 22*4(a0)
+    lw x24, 23*4(a0)
+    lw x25, 24*4(a0)
+    lw x26, 25*4(a0)
+    lw x27, 26*4(a0)
+    lw x28, 27*4(a0)
+    lw x29, 28*4(a0)
+    lw x20, 29*4(a0)
+    lw x31, 30*4(a0)
+
+    # save trapframe pointer to sscratch
+    csrrw a0, sscratch, a0;
+
+    sret
diff --git a/tests/system/pgfault/vm_setup.c b/tests/system/pgfault/vm_setup.c
new file mode 100644
index 00000000..015c270d
--- /dev/null
+++ b/tests/system/pgfault/vm_setup.c
@@ -0,0 +1,420 @@
+/*
+ * Reference:
+ * 1. https://github.com/sifive/example-vm-test
+ * 2. https://github.com/yutongshen/RISC-V-Simulator
+ */
+
+#include <stdarg.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#if !defined(__GNUC__) || !defined(__riscv) || 32 != __riscv_xlen
+#error "GNU Toolchain for 32-bit RISC-V is required"
+#endif
+
+#define SECTION_TEXT_VMSETUP __attribute__((section(".text.vm_setup")))
+#define SECTION_DATA_VMSETUP __attribute__((section(".data.vm_setup")))
+#define SECTION_BSS_VMSETUP __attribute__((section(".bss.vm_setup")))
+
+#define assert(x)                                                     \
+    do {                                                              \
+        if (!x) {                                                     \
+            printf("Assertion failed '%s' at line %d of '%s'\n", #x); \
+            _exit(1);                                                 \
+        }                                                             \
+    } while (0)
+
+#define read_csr(reg)                                 \
+    ({                                                \
+        uint32_t __tmp;                               \
+        asm volatile("csrr %0, " #reg : "=r"(__tmp)); \
+        __tmp;                                        \
+    })
+
+#define write_csr(reg, val) ({ asm volatile("csrw " #reg ", %0" ::"rK"(val)); })
+
+#define swap_csr(reg, val)                                                \
+    ({                                                                    \
+        uint32_t __tmp;                                                   \
+        asm volatile("csrrw %0, " #reg ", %1" : "=r"(__tmp) : "rK"(val)); \
+        __tmp;                                                            \
+    })
+
+#define set_csr(reg, bit)                                                 \
+    ({                                                                    \
+        uint32_t __tmp;                                                   \
+        asm volatile("csrrs %0, " #reg ", %1" : "=r"(__tmp) : "rK"(bit)); \
+        __tmp;                                                            \
+    })
+
+#define clear_csr(reg, bit)                                               \
+    ({                                                                    \
+        uint32_t __tmp;                                                   \
+        asm volatile("csrrc %0, " #reg ", %1" : "=r"(__tmp) : "rK"(bit)); \
+        __tmp;                                                            \
+    })
+
+typedef struct {
+    uint32_t ra;
+    uint32_t sp;
+    uint32_t gp;
+    uint32_t tp;
+    uint32_t t0;
+    uint32_t t1;
+    uint32_t t2;
+    uint32_t s0;
+    uint32_t s1;
+    uint32_t a0;
+    uint32_t a1;
+    uint32_t a2;
+    uint32_t a3;
+    uint32_t a4;
+    uint32_t a5;
+    uint32_t a6;
+    uint32_t a7;
+    uint32_t s2;
+    uint32_t s3;
+    uint32_t s4;
+    uint32_t s5;
+    uint32_t s6;
+    uint32_t s7;
+    uint32_t s8;
+    uint32_t s9;
+    uint32_t s10;
+    uint32_t s11;
+    uint32_t t3;
+    uint32_t t4;
+    uint32_t t5;
+    uint32_t t6;
+    uint32_t status;
+    uint32_t epc;
+    uint32_t badvaddr;
+    uint32_t cause;
+} trapframe_t;
+
+#define SV32_MODE 0x80000000
+#define BARE_MODE 0x00000000
+#define PG_SHIFT 12
+#define PG_SIZE (1U << 12)
+#define FREE_FRAME_BASE 0x400000
+#define MAX_TEST_PG 32          /* FREE_FRAME_BASE - 0x41ffff */
+#define MEGA_PG (PG_SIZE << 10) /* 4 MiB */
+
+#define CAUSE_USER_ECALL (1U << 8)
+#define CAUSE_SUPERVISOR_ECALL (1U << 9)
+#define CAUSE_FETCH_PAGE_FAULT (1U << 12)
+#define CAUSE_LOAD_PAGE_FAULT (1U << 13)
+#define CAUSE_STORE_PAGE_FAULT (1U << 15)
+
+#define SSTATUS_SUM (1U << 18)
+
+#define PTE_V (1U)
+#define PTE_R (1U << 1)
+#define PTE_W (1U << 2)
+#define PTE_X (1U << 3)
+#define PTE_U (1U << 4)
+#define PTE_G (1U << 5)
+#define PTE_A (1U << 6)
+#define PTE_D (1U << 7)
+
+extern void main();
+extern void _start();
+extern int user_entry();
+extern void supervisor_trap_entry();
+extern void pop_tf(trapframe_t *);
+extern void _exit(int status);
+
+typedef uint32_t pte_t;
+typedef struct {
+    pte_t addr;
+    void *next;
+} freelist_t;
+
+freelist_t freelist_nodes[MAX_TEST_PG] SECTION_BSS_VMSETUP;
+freelist_t *SECTION_BSS_VMSETUP freelist_head;
+freelist_t *SECTION_BSS_VMSETUP freelist_tail;
+
+#define l1pt pt[0]
+#define user_l1pt pt[1]
+#define NPT 2
+#define PTES_PER_PT (1U << 10)
+#define PTE_PPN_SHIFT 10
+pte_t pt[NPT][PTES_PER_PT] SECTION_BSS_VMSETUP
+    __attribute__((aligned(PG_SIZE)));
+
+#define MASK(n) (~((~0U << (n))))
+#define pa2kva(x) (((pte_t) (x)) - ((pte_t) (_start)) - MEGA_PG)
+#define uva2kva(x) (((pte_t) (x)) - MEGA_PG)
+
+void *SECTION_TEXT_VMSETUP memcpy(void *ptr, void *src, size_t len)
+{
+    uint32_t *word_ptr = ptr;
+    uint32_t *word_src = src;
+
+    while (len >= 4) {
+        *word_ptr++ = *word_src++;
+        len -= 4;
+    }
+
+    char *byte_ptr = (char *) word_ptr;
+    char *byte_src = (char *) word_src;
+    while (len--) {
+        *byte_ptr++ = *byte_src++;
+    }
+
+    return ptr;
+}
+
+void *SECTION_TEXT_VMSETUP memset(void *ptr, int val, size_t len)
+{
+    uint32_t *word_ptr = ptr;
+    uint32_t write_word = (char) val;
+    write_word |= write_word << 8;
+    write_word |= write_word << 16;
+
+    while (len >= 4) {
+        *word_ptr++ = write_word;
+        len -= 4;
+    }
+
+    char *byte_ptr = (char *) word_ptr;
+    while (len--) {
+        *byte_ptr++ = val;
+    }
+
+    return ptr;
+}
+
+#define SYSCALL_WRITE 64
+int SECTION_TEXT_VMSETUP putchar(int ch)
+{
+    int syscall_nr = SYSCALL_WRITE;
+    asm volatile(
+        "mv a7, %0;"
+        "li a0, 0x1;" /* stdout */
+        "add a1, x0, %1;"
+        "li a2, 0x1;" /* one character */
+        "ecall;"
+        :
+        : "r"(syscall_nr), "r"(&ch)
+        : "a0", "a1", "a2", "a7");
+
+    return 0;
+}
+
+int SECTION_TEXT_VMSETUP _puts(const char *s)
+{
+    int res = 0;
+    while (*s) {
+        putchar(*(s++));
+        ++res;
+    }
+
+    return res;
+}
+
+int SECTION_TEXT_VMSETUP puts(const char *s)
+{
+    int res = 1;
+    res += _puts(s);
+    putchar('\n');
+
+    return res;
+}
+
+char *SECTION_TEXT_VMSETUP itoa(uint32_t value,
+                                int base,
+                                int min_len,
+                                char fill_char)
+{
+    static char digitals[16] = {'0', '1', '2', '3', '4', '5', '6', '7',
+                                '8', '9', 'a', 'b', 'c', 'd', 'e', 'f'};
+    static char str[64];
+    char *ptr = str + 63;
+    int tmp;
+    *ptr = 0;
+    do {
+        if (!value)
+            *--ptr = fill_char;
+        else
+            *--ptr = digitals[value % base];
+        value /= base;
+    } while (--min_len > 0 || value);
+
+    return ptr;
+}
+
+int SECTION_TEXT_VMSETUP printf(const char *format, ...)
+{
+    const char *ptr = format;
+    int res = 0;
+
+    va_list va;
+    va_start(va, format);
+
+    while (*ptr) {
+        if (*ptr == '%') {
+            int min_len = 0;
+            char fill_char = 0;
+        loop1:
+            switch (*(++ptr)) {
+            case 'c':
+                assert(!(min_len || fill_char));
+                ++res;
+                putchar(va_arg(va, int));
+                break;
+            case 's':
+                assert(!(min_len || fill_char));
+                res += _puts(va_arg(va, const char *));
+                break;
+            case 'x': {
+                assert(!(!fill_char ^ !min_len));
+                uint32_t n = va_arg(va, uint32_t);
+                res += _puts(itoa(n, 16, min_len, min_len ? fill_char : '0'));
+            } break;
+            case 'd': {
+                assert(!(!fill_char ^ !min_len));
+                int64_t n = va_arg(va, int64_t);
+                if (n < 0) {
+                    ++res;
+                    putchar('-');
+                    n = -n;
+                }
+                res += _puts(itoa(n, 10, min_len, min_len ? fill_char : '0'));
+            } break;
+            case '%':
+                ++res;
+                putchar('%');
+                break;
+            case '1':
+                assert(fill_char);
+                min_len *= 10;
+                min_len += 1;
+                goto loop1;
+            case '2':
+                assert(fill_char);
+                min_len *= 10;
+                min_len += 2;
+                goto loop1;
+            case '3':
+                assert(fill_char);
+                min_len *= 10;
+                min_len += 3;
+                goto loop1;
+            case '4':
+                assert(fill_char);
+                min_len *= 10;
+                min_len += 4;
+                goto loop1;
+            case '5':
+                assert(fill_char);
+                min_len *= 10;
+                min_len += 5;
+                goto loop1;
+            case '6':
+                assert(fill_char);
+                min_len *= 10;
+                min_len += 6;
+                goto loop1;
+            case '7':
+                assert(fill_char);
+                min_len *= 10;
+                min_len += 7;
+                goto loop1;
+            case '8':
+                assert(fill_char);
+                min_len *= 10;
+                min_len += 8;
+                goto loop1;
+            case '9':
+                assert(fill_char);
+                min_len *= 10;
+                min_len += 9;
+                goto loop1;
+            default:
+                assert(!min_len);
+                fill_char = *ptr;
+                goto loop1;
+            }
+        } else {
+            ++res;
+            putchar(*ptr);
+        }
+        ++ptr;
+    }
+    return res;
+}
+
+void SECTION_TEXT_VMSETUP handle_fault(uint32_t addr, uint32_t cause)
+{
+    addr = addr >> PG_SHIFT << PG_SHIFT; /* round down page */
+    pte_t *pte = user_l1pt + ((addr >> PG_SHIFT) & MASK(10));
+
+    /* create a new pte */
+    assert(freelist_head);
+    pte_t pa = freelist_head->addr;
+    freelist_head = freelist_head->next;
+    *pte = (pa >> PG_SHIFT << PTE_PPN_SHIFT) | PTE_A | PTE_U | PTE_R | PTE_W |
+           PTE_X | PTE_V;
+    if ((1U << cause) == CAUSE_STORE_PAGE_FAULT)
+        *pte |= PTE_D;
+
+    /* temporarily allow kernel to access user memory to copy data */
+    set_csr(sstatus, SSTATUS_SUM);
+    /* page table is updated, so main should not cause trap here */
+    memcpy((uint32_t *) addr, (uint32_t *) uva2kva(addr), PG_SIZE);
+    /* disallow kernel to access user memory */
+    clear_csr(sstatus, SSTATUS_SUM);
+}
+
+void SECTION_TEXT_VMSETUP handle_trap(trapframe_t *tf)
+{
+    if ((1U << tf->cause) == CAUSE_FETCH_PAGE_FAULT ||
+        (1U << tf->cause) == CAUSE_LOAD_PAGE_FAULT ||
+        (1U << tf->cause) == CAUSE_STORE_PAGE_FAULT) {
+        handle_fault(tf->badvaddr, tf->cause);
+    } else
+        assert(!"Unknown exception");
+
+    pop_tf(tf);
+}
+
+void SECTION_TEXT_VMSETUP vm_boot()
+{
+    /* map first page table entry to a next level page table for user */
+    l1pt[0] = ((pte_t) user_l1pt >> PG_SHIFT << PTE_PPN_SHIFT) | PTE_V;
+    /* map last page table leaf entry for kernel which direct maps for user
+     * virtual memory, note that this is a trick of 2's complement, e.g., 0 -
+     * MEGA_PG = 0b1111111111xxx...x when page fault occurs after entering main
+     */
+    l1pt[PTES_PER_PT - 1] = ((pte_t) main >> PG_SHIFT << PTE_PPN_SHIFT) |
+                            PTE_V | PTE_R | PTE_W | PTE_X | PTE_A | PTE_D;
+
+    /* direct map kernel virtual memory */
+    l1pt[PTES_PER_PT >> 1] = ((pte_t) _start >> PG_SHIFT << PTE_PPN_SHIFT) |
+                             PTE_V | PTE_R | PTE_W | PTE_X | PTE_A | PTE_D;
+
+    /* Enable paging */
+    uintptr_t satp_val = ((pte_t) &l1pt >> PG_SHIFT) | SV32_MODE;
+    write_csr(satp, satp_val);
+
+    /* set up supervisor trap handler */
+    write_csr(stvec, supervisor_trap_entry);
+    write_csr(sscratch, read_csr(mscratch));
+    /* No delegation */
+    /*write_csr(medeleg, CAUSE_FETCH_PAGE_FAULT | CAUSE_LOAD_PAGE_FAULT |
+                           CAUSE_STORE_PAGE_FAULT);*/
+
+    freelist_head = (void *) &freelist_nodes[0];
+    freelist_tail = &freelist_nodes[MAX_TEST_PG - 1];
+    for (uint32_t i = 0; i < MAX_TEST_PG; i++) {
+        freelist_nodes[i].addr = FREE_FRAME_BASE + i * PG_SIZE;
+        freelist_nodes[i].next = &freelist_nodes[i + 1];
+    }
+    freelist_nodes[MAX_TEST_PG - 1].next = 0;
+
+    trapframe_t tf;
+    memset(&tf, 0, sizeof(tf));
+    tf.epc = (uint32_t) &user_entry;
+    pop_tf(&tf);
+}