diff --git a/src/isa/riscv64/csr.h b/src/isa/riscv64/csr.h
index f00550902d536cc2835f0f3bcc759d7ef9b21b58..d2613402555d4b1eaaca04d99bd38fdc19d6d5c4 100644
--- a/src/isa/riscv64/csr.h
+++ b/src/isa/riscv64/csr.h
@@ -10,7 +10,7 @@
   f(mhartid    , 0xf14) \
   f(sstatus    , 0x100) \
   f(sie        , 0x104) f(stvec      , 0x105) \
-  f(sscratch   , 0x140) f(sepc       , 0x141) \
+  f(sscratch   , 0x140) f(sepc       , 0x141) f(scause     , 0x142)\
   f(satp       , 0x180)
 
 #define CSR_STRUCT_START(name) \
@@ -40,6 +40,8 @@ CSR_STRUCT_START(mtvec)
 CSR_STRUCT_END(mtvec)
 
 CSR_STRUCT_START(mcause)
+  uint64_t code:63;
+  uint64_t intr: 1;
 CSR_STRUCT_END(mcause)
 
 CSR_STRUCT_START(mepc)
@@ -104,6 +106,11 @@ CSR_STRUCT_START(satp)
   uint64_t mode: 4;
 CSR_STRUCT_END(satp)
 
+CSR_STRUCT_START(scause)
+  uint64_t code:63;
+  uint64_t intr: 1;
+CSR_STRUCT_END(scause)
+
 CSR_STRUCT_START(sepc)
 CSR_STRUCT_END(sepc)
 
@@ -115,4 +122,7 @@ MAP(CSRS, CSRS_DECL)
 
 word_t* csr_decode(uint32_t addr);
 
+enum { MODE_U = 0, MODE_S, MODE_H, MODE_M };
+void change_mode(uint8_t m);
+
 #endif
diff --git a/src/isa/riscv64/exec/system.c b/src/isa/riscv64/exec/system.c
index 91baffcdb9b4a20e075ad8bd2de80c2df55ab9e4..2d8900ddcfc59ce5b96f5487922974f6639c8e53 100644
--- a/src/isa/riscv64/exec/system.c
+++ b/src/isa/riscv64/exec/system.c
@@ -40,12 +40,14 @@ make_EHelper(priv) {
   uint32_t type = decinfo.isa.instr.csr;
   switch (type) {
     case 0:
-      raise_intr(11, cpu.pc);
+      raise_intr(8 + cpu.mode, cpu.pc);
       print_asm("ecall");
       break;
     case 0x102:
-      sstatus->sie = sstatus->spie;
-      sstatus->spie = 1;
+      mstatus->sie = mstatus->spie;
+      mstatus->spie = 1;
+      change_mode(mstatus->spp);
+      mstatus->mpp = MODE_U;
       rtl_li(&s0, sepc->val);
       rtl_jr(&s0);
       print_asm("sret");
@@ -56,6 +58,8 @@ make_EHelper(priv) {
     case 0x302:
       mstatus->mie = mstatus->mpie;
       mstatus->mpie = 1;
+      change_mode(mstatus->mpp);
+      mstatus->mpp = MODE_U;
       rtl_li(&s0, mepc->val);
       rtl_jr(&s0);
       print_asm("mret");
diff --git a/src/isa/riscv64/include/isa/reg.h b/src/isa/riscv64/include/isa/reg.h
index 04ec34e0b47b2145586924b3204025899f5efb9e..929ac76863c08f0067057f9ea91baed335292b53 100644
--- a/src/isa/riscv64/include/isa/reg.h
+++ b/src/isa/riscv64/include/isa/reg.h
@@ -15,6 +15,7 @@ typedef struct {
   } gpr[32];
 
   vaddr_t pc;
+  uint8_t mode;
 
   bool INTR;
 } CPU_state;
diff --git a/src/isa/riscv64/init.c b/src/isa/riscv64/init.c
index d276f23a1faaea0a1b410e9f05a59f3cf79ca8d9..49ddecd62ac7c0454270f7d88d029baf255b47b0 100644
--- a/src/isa/riscv64/init.c
+++ b/src/isa/riscv64/init.c
@@ -16,6 +16,7 @@ void init_clint(void);
 void init_isa(void) {
   cpu.gpr[0]._64 = 0;
   cpu.pc = PC_START;
+  cpu.mode = MODE_M;
   //mstatus->val = 0x000c0100;
 
   register_pmem(0x80000000u);
diff --git a/src/isa/riscv64/intr.c b/src/isa/riscv64/intr.c
index b6feec80d0279a691eb61e76f7c0d7cd7d2c489f..ca67342e92e4e8f4e0923502c92ff21cbfd9da7d 100644
--- a/src/isa/riscv64/intr.c
+++ b/src/isa/riscv64/intr.c
@@ -4,15 +4,29 @@
 #define INTR_BIT (1ULL << 63)
 
 void raise_intr(word_t NO, vaddr_t epc) {
-  /* TODO: Trigger an interrupt/exception with ``NO''.
-   * That is, use ``NO'' to index the IDT.
-   */
+  // TODO: Trigger an interrupt/exception with ``NO''
+
+  word_t deleg = (NO & INTR_BIT ? mideleg->val : medeleg->val);
+  bool delegS = ((deleg & (1 << (NO & 0xf))) != 0) && (cpu.mode < MODE_M);
+
+  if (delegS) {
+    scause->val = NO;
+    sepc->val = epc;
+    mstatus->spp = cpu.mode;
+    mstatus->spie = mstatus->sie;
+    mstatus->sie = 0;
+    cpu.mode = MODE_S;
+    rtl_li(&s0, stvec->val);
+  } else {
+    mcause->val = NO;
+    mepc->val = epc;
+    mstatus->mpp = cpu.mode;
+    mstatus->mpie = mstatus->mie;
+    mstatus->mie = 0;
+    cpu.mode = MODE_M;
+    rtl_li(&s0, mtvec->val);
+  }
 
-  mcause->val = NO;
-  mepc->val = epc;
-  mstatus->mpie = mstatus->mie;
-  mstatus->mie = 0;
-  rtl_li(&s0, mtvec->val);
   rtl_jr(&s0);
 }
 
diff --git a/src/isa/riscv64/reg.c b/src/isa/riscv64/reg.c
index 8826527b7222b50a6150a4b5c7e63b1aad7e803b..b94c25ade0d921214561c7eb0f14551dcfa228fb 100644
--- a/src/isa/riscv64/reg.c
+++ b/src/isa/riscv64/reg.c
@@ -52,3 +52,8 @@ word_t* csr_decode(uint32_t addr) {
   Assert(csr_exist[addr], "unimplemented CSR 0x%x at pc = " FMT_WORD, addr, cpu.pc);
   return &csr_array[addr];
 }
+
+void change_mode(uint8_t m) {
+  assert(m < 4 && m != MODE_H);
+  cpu.mode = m;
+}