diff --git a/hw/spinal/futurecore/Globals.scala b/hw/spinal/futurecore/Globals.scala
new file mode 100644
index 0000000..38516f6
--- /dev/null
+++ b/hw/spinal/futurecore/Globals.scala
@@ -0,0 +1,8 @@
+package futurecore
+
+import futurecore.riscv.{Rv32i, Zicsr, Privileged}
+
+object Globals {
+  val SupportIsas = List(Rv32i, Zicsr, Privileged)
+  val AllInstructions = SupportIsas.collect(_.instructions).flatten
+}
diff --git a/hw/spinal/futurecore/debug/DebugPlugin.scala b/hw/spinal/futurecore/debug/DebugPlugin.scala
index 29d7e91..efc0f3b 100644
--- a/hw/spinal/futurecore/debug/DebugPlugin.scala
+++ b/hw/spinal/futurecore/debug/DebugPlugin.scala
@@ -5,11 +5,13 @@ import spinal.lib.misc.plugin._
 
 import futurecore.fetch.FetchPlugin
 import futurecore.decode.DecodePlugin
+import futurecore.execute.ExecutePlugin
 
 class DebugPlugin extends FiberPlugin {
   val setup = during setup new Area {
     val fp = host[FetchPlugin]
     val dp = host[DecodePlugin]
+    val ep = host[ExecutePlugin]
   }
 
   val logic = during build new Area {
@@ -19,10 +21,15 @@ class DebugPlugin extends FiberPlugin {
   val interconnect = during build new Area {
     val fp = setup.get.fp
     val dp = setup.get.dp
+    val ep = setup.get.ep
     val l = logic.get
 
     l.probe.io.inPc := fp.getPc()
     l.probe.io.inInst := fp.getInstruction()
-    l.probe.io.inGprs := dp.getDbgRegfile()
+    l.probe.io.inGprs := dp.getDbgIntRegs()
+    l.probe.io.inMstatus := ep.getDbgMstatus()
+    l.probe.io.inMtvec := ep.getDbgMtvec()
+    l.probe.io.inMepc := ep.getDbgMepc()
+    l.probe.io.inMcause := ep.getDbgMcause()
   }
 }
diff --git a/hw/spinal/futurecore/debug/DebugProbe.scala b/hw/spinal/futurecore/debug/DebugProbe.scala
index 0400fa4..078ccd4 100644
--- a/hw/spinal/futurecore/debug/DebugProbe.scala
+++ b/hw/spinal/futurecore/debug/DebugProbe.scala
@@ -9,6 +9,10 @@ class DebugProbe extends Component {
     val inPc = in port UInt(32 bits)
     val inInst = in port Bits(32 bits)
     val inGprs = in port Vec(Bits(32 bits), 32)
+    val inMstatus = in port Bits(32 bits)
+    val inMtvec = in port Bits(32 bits)
+    val inMepc = in port Bits(32 bits)
+    val inMcause = in port Bits(32 bits)
   }
 
   val dbg = new debug_dpi(gprNum = 32, gprWidth = 32)
@@ -17,4 +21,8 @@ class DebugProbe extends Component {
   dbg.io.s.inst_i := io.inInst
   // Reverse registers to make the unified signal little-endian to pass legally
   dbg.io.s.gprs_i := io.inGprs.reverse.reduce(_ ## _)
+  dbg.io.s.mstatus_i := io.inMstatus
+  dbg.io.s.mtvec_i := io.inMtvec
+  dbg.io.s.mepc_i := io.inMepc
+  dbg.io.s.mcause_i := io.inMcause
 }
diff --git a/hw/spinal/futurecore/decode/DecodePlugin.scala b/hw/spinal/futurecore/decode/DecodePlugin.scala
index f585352..65faf4c 100644
--- a/hw/spinal/futurecore/decode/DecodePlugin.scala
+++ b/hw/spinal/futurecore/decode/DecodePlugin.scala
@@ -7,7 +7,8 @@ import spinal.lib.misc.plugin.FiberPlugin
 
 import scala.collection.mutable.Map
 
-import futurecore.riscv.Rvi
+import futurecore.Globals.AllInstructions
+import futurecore.riscv.{Rv32i, Zicsr, Privileged}
 import futurecore.fetch.FetchPlugin
 import futurecore.writeback.WritebackPlugin
 
@@ -25,48 +26,51 @@ class DecodePlugin extends FiberPlugin with CtrlService {
     val regfile = new IntRegfile
 
     val immSelDef = CtrlDef(ImmMode(), ImmMode.I)
-      .setWhen(ImmMode.S, Rvi.instructions.filter(_.fields.contains(Rvi.SImm)))
-      .setWhen(ImmMode.B, Rvi.instructions.filter(_.fields.contains(Rvi.BImm)))
-      .setWhen(ImmMode.U, Rvi.instructions.filter(_.fields.contains(Rvi.UImm)))
-      .setWhen(ImmMode.J, Rvi.instructions.filter(_.fields.contains(Rvi.JImm)))
+      .setWhen(ImmMode.S, Rv32i.instructions.filter(_.fields.contains(Rv32i.SImm)))
+      .setWhen(ImmMode.B, Rv32i.instructions.filter(_.fields.contains(Rv32i.BImm)))
+      .setWhen(ImmMode.U, Rv32i.instructions.filter(_.fields.contains(Rv32i.UImm)))
+      .setWhen(ImmMode.J, Rv32i.instructions.filter(_.fields.contains(Rv32i.JImm)))
     registerCtrlSignal(immSelDef)
 
     val rfWriteEnableDef = CtrlDef(Bool(), False)
       .setWhen(
         True,
-        Rvi.instructions
-          .filter(_.fields.contains(Rvi.Rd))
-          .filterNot(_ == Rvi.Ebreak)
+        Rv32i.instructions
+          .filter(_.fields.contains(Rv32i.Rd))
+          .filterNot(_ == Rv32i.Ebreak)
       )
+      .setWhen(True, Zicsr.instructions.filter(_.fields.contains(Zicsr.Rd)))
     registerCtrlSignal(rfWriteEnableDef)
 
     ctrlLock.await()
 
     val ctrlArea = new Area {
-      val instruction = Bits(32 bits)
+      val inst = Bits(32 bits)
 
       val map = ctrlSignals.map { ctrlDef =>
         val spec = ctrlDef.toDecodingSpec
-        val signal = spec.build(instruction, Rvi.instructions.map(_.toMasked))
+        val signal = spec.build(inst, AllInstructions.map(_.toMasked))
         ctrlDef -> signal
       }.toMap
     }
 
-    val instruction = Bits(32 bits)
-    val writebackData = Bits(32 bits)
+    val inst = Bits(32 bits)
     val immSel = ImmMode()
+    val writebackData = Bits(32 bits)
     val rfWriteEnable = Bool()
 
-    ctrlArea.instruction := instruction
+    ctrlArea.inst := inst
 
-    immGen.io.inInst := instruction
+    immGen.io.inInst := inst
     immGen.io.inSelMode := immSel
 
-    regfile.io.inAddrReadA := Rvi.Rs1.extract(instruction).asUInt
-    regfile.io.inAddrReadB := Rvi.Rs2.extract(instruction).asUInt
-    regfile.io.inAddrWrite := Rvi.Rd.extract(instruction).asUInt
-    regfile.io.inDataWrite := writebackData
+    regfile.io.inAddrReadA := Rv32i.Rs1.extract(inst).asUInt
+    regfile.io.inAddrReadB := Rv32i.Rs2.extract(inst).asUInt
+    regfile.io.inAddrWrite := Rv32i.Rd.extract(inst).asUInt
     regfile.io.inEnableWrite := rfWriteEnable
+    regfile.io.inDataWrite := writebackData
+
+    val csrAddr = Zicsr.Csr.extract(inst).asUInt
   }
 
   val interconnect = during build new Area {
@@ -74,7 +78,7 @@ class DecodePlugin extends FiberPlugin with CtrlService {
     val wp = setup.get.wp
     val l = logic.get
 
-    l.instruction := fp.getInstruction()
+    l.inst := fp.getInstruction()
     l.writebackData := wp.getWriteback()
     l.immSel := getCtrlSignal(l.immSelDef)
     l.rfWriteEnable := getCtrlSignal(l.rfWriteEnableDef)
@@ -93,7 +97,9 @@ class DecodePlugin extends FiberPlugin with CtrlService {
 
   def getImm(): SInt = logic.get.immGen.io.outImm
 
+  def getCsrAddr(): UInt = logic.get.csrAddr
+
   def getReturnStatus(): SInt = logic.get.regfile.io.outSpecialRet.asSInt
 
-  def getDbgRegfile(): Vec[Bits] = logic.get.regfile.io.outDbgRegisters
+  def getDbgIntRegs(): Vec[Bits] = logic.get.regfile.io.dbgIntRegisters
 }
diff --git a/hw/spinal/futurecore/decode/ImmGenerator.scala b/hw/spinal/futurecore/decode/ImmGenerator.scala
index 3731f09..c2b2562 100644
--- a/hw/spinal/futurecore/decode/ImmGenerator.scala
+++ b/hw/spinal/futurecore/decode/ImmGenerator.scala
@@ -2,12 +2,13 @@ package futurecore.decode
 
 import spinal.core._
 
-import futurecore.riscv.Rvi.{IImm, SImm, BImm, UImm, JImm}
-import futurecore.misc.TypeExtensions.BitsSextExtension
+import futurecore.riscv.Rv32i.{IImm, SImm, BImm, UImm, JImm}
+import futurecore.riscv.Zicsr.{MicroImm => CsrMicroImm}
+import futurecore.misc.TypeExtensions.{BitsSextExtension, BitsZeroExtension}
 
 object ImmGenerator {
   object ImmMode extends SpinalEnum {
-    val I, S, B, U, J = newElement()
+    val I, S, B, U, J, CsrU = newElement()
   }
 }
 
@@ -28,12 +29,18 @@ class ImmGenerator extends Component {
   val uImm = (UImm.extract(inst) ## U"12'b0").sext
   val jImm = (JImm.extract(inst) ## U"1'b0").sext
 
+  val zicsrMicroImm = (CsrMicroImm.extract(inst)).zext
+
   // Select immediate based on mode
   io.outImm := io.inSelMode.mux(
     ImmMode.I -> iImm,
     ImmMode.S -> sImm,
     ImmMode.B -> bImm,
     ImmMode.U -> uImm,
-    ImmMode.J -> jImm
+    ImmMode.J -> jImm,
+    // Although uImm is UInt and zero-extended,to make type system happy,
+    // still needs to convert to SInt. This will not cause any problem as
+    // binary representation keep untouched.
+    ImmMode.CsrU -> zicsrMicroImm.asSInt
   )
 }
diff --git a/hw/spinal/futurecore/decode/IntRegfile.scala b/hw/spinal/futurecore/decode/IntRegfile.scala
index 2d6d8cd..3d7bba8 100644
--- a/hw/spinal/futurecore/decode/IntRegfile.scala
+++ b/hw/spinal/futurecore/decode/IntRegfile.scala
@@ -15,7 +15,7 @@ class IntRegfile extends Component {
     val inDataWrite = in port Bits(32 bits)
 
     val outSpecialRet = out port Bits(32 bits)
-    val outDbgRegisters = out port Vec(Bits(32 bits), 32)
+    val dbgIntRegisters = out port Vec(Bits(32 bits), 32)
   }
 
   val regfile = Mem(Bits(32 bits), 32)
@@ -32,5 +32,5 @@ class IntRegfile extends Component {
   val returnIndex = U(10, 5 bits)
   io.outSpecialRet := regfile(returnIndex.resized)
 
-  io.outDbgRegisters.zipWithIndex.foreach { case (b, i) => b := regfile(U(i).resized) }
+  io.dbgIntRegisters.zipWithIndex.foreach { case (b, i) => b := regfile(U(i).resized) }
 }
diff --git a/hw/spinal/futurecore/execute/CsrHandler.scala b/hw/spinal/futurecore/execute/CsrHandler.scala
new file mode 100644
index 0000000..8da4291
--- /dev/null
+++ b/hw/spinal/futurecore/execute/CsrHandler.scala
@@ -0,0 +1,150 @@
+package futurecore.execute
+
+import spinal.core._
+
+object CsrHandler {
+  object CsrMode extends SpinalEnum {
+    val Write, Set = newElement()
+  }
+
+  object ExceptionType extends SpinalEnum {
+    val EcallM = newElement() // ! Triggers SpinalHDL bug (see below)
+  }
+}
+
+class CsrHandler extends Component {
+  import CsrHandler._
+
+  val io = new Bundle {
+    val inEnableCsr = in port Bool()
+    val inSelCsrMode = in port CsrMode()
+    val inCsrAddr = in port UInt(12 bits)
+    val inNewVal = in port Bits(32 bits)
+    val outOldVal = out port Bits(32 bits)
+
+    val inEnableTrap = in port Bool()
+    val inEnableTrapReturn = in port Bool()
+    val inSelException = in port ExceptionType()
+    val inExceptionPc = in port UInt(32 bits)
+    val outTrapVector = out port UInt(32 bits)
+    val outReturnPc = out port UInt(32 bits)
+
+    val dbgMstatus = out port Bits(32 bits)
+    val dbgMtvec = out port Bits(32 bits)
+    val dbgMepc = out port Bits(32 bits)
+    val dbgMcause = out port Bits(32 bits)
+  }
+
+  val csrRegfile = new Area {
+    val enable = Bool()
+    val mode = CsrMode()
+    val addr = UInt(12 bits)
+    val newVal = Bits(32 bits)
+
+    case class CsrReg(number: Int, init: Int) {
+      val index = U(number, 12 bits)
+      val reg = RegInit(B(init, 32 bits))
+    }
+
+    // ! WARNING: Hardcoded mstatus initial value here to pass difftest!
+    // !          Actually do nothing on mstatus except for direct access.
+    val mstatus = CsrReg(0x300, 0x1800)
+    val mtvec = CsrReg(0x305, 0)
+    val mepc = CsrReg(0x341, 0)
+    val mcause = CsrReg(0x342, 0)
+
+    // ! WARNING: To simplify implementation,
+    // !          ccessibility and lowestLevel not checked!
+    // !          Just list here for hinting of future implementation.
+    // read/write: 00,01 read-only: 11
+    val accessibility = addr(11 downto 10)
+    val isReadOnly = accessibility.andR
+    // privilege levels: user: 00 supervisor: 01 machine: 11
+    val lowestLevel = addr(9 downto 8)
+
+    // Atomic read write
+    // ! WARNING: Also, to simplify implementation,
+    // !          field constrains are ignored!
+    // !          Moreover, here uses ALWAYS READ policy!
+    // !          This VIOLATES the rule that if rd=x0,
+    // !          ReadWrite should not cause any read side effect!
+    // For now, just use plain mux or switch to handle addressing
+    val oldVal = addr.mux(
+      mstatus.index -> mstatus.reg,
+      mtvec.index   -> mtvec.reg,
+      mepc.index    -> mepc.reg,
+      mcause.index  -> mcause.reg,
+      default       -> B(0).resized
+    )
+
+    val writeVal = isReadOnly ? oldVal | mode.mux(
+      CsrMode.Write -> newVal,
+      // use or to set unmasked bits and remain masked bits
+      CsrMode.Set -> (newVal | oldVal)
+    )
+
+    when(enable) {
+      switch(addr) {
+        is(mstatus.index) { mstatus.reg := writeVal }
+        is(mtvec.index) { mtvec.reg := writeVal }
+        is(mepc.index) { mepc.reg := writeVal }
+        is(mcause.index) { mcause.reg := writeVal }
+      }
+    }
+  }
+
+  csrRegfile.enable := io.inEnableCsr
+  csrRegfile.mode := io.inSelCsrMode
+  csrRegfile.addr := io.inCsrAddr
+  csrRegfile.newVal := io.inNewVal
+  io.outOldVal := io.inEnableCsr ? csrRegfile.oldVal | B(0).resized
+
+  // Trap handling logic
+  /*
+    ! SpinalHDL bug: enum.mux() causes NullPointerException when enum has only 1 element.
+    ! Workaround: Ensure enum has at least 2 elements.
+    !             Using constant here since only Ecall is supported.
+    ! https://github.com/SpinalHDL/SpinalHDL/issues/1884
+   */
+  val TrapCauseId = B"32'hb"
+
+  // ! WARNING: Hardcoded mstatus trap value here to pass difftest!
+  // !          Actually do nothing on mstatus except for direct access.
+  val MstatusAfterTrap = B"32'h1800"
+
+  // Only accept trap when not csr ops, as csr should be atomic
+  // ? Actually this the false combination should never happen !
+  when(io.inEnableTrap & !io.inEnableCsr) {
+    csrRegfile.mstatus.reg := MstatusAfterTrap
+    csrRegfile.mcause.reg := TrapCauseId
+    csrRegfile.mepc.reg := io.inExceptionPc.asBits
+  }
+
+  // Trap return handling logic
+  // ! WARNING: Hardcoded mstatus return value here to pass difftest!
+  // !          Actually do nothing on mstatus except for direct access.
+  val MstatusAfterTrapReturn = B"32'h80"
+  // Only accept trap return when not csr ops nor trap
+  // ? Actually this the false combination should never happen !
+  when(io.inEnableTrapReturn & !io.inEnableTrap & !io.inEnableCsr) {
+    csrRegfile.mstatus.reg := MstatusAfterTrapReturn
+  }
+
+  // ! WARNING: the trap vector and epc have been hard wared and may not
+  // !          comfy spec, which may cause security problems!
+  val trapTarget = (csrRegfile.mtvec.reg(31 downto 2) ## U"2'b0").asUInt
+  val returnTarget = csrRegfile.mepc.reg.asUInt
+
+  io.outTrapVector := trapTarget
+  io.outReturnPc := returnTarget
+
+  // Debug outputs for CSR registers
+  io.dbgMstatus := csrRegfile.mstatus.reg
+  io.dbgMtvec := csrRegfile.mtvec.reg
+  io.dbgMepc := csrRegfile.mepc.reg
+  io.dbgMcause := csrRegfile.mcause.reg
+}
+
+object CsrHandlerTest extends App {
+  SpinalConfig().generateSystemVerilog(new CsrHandler)
+}
diff --git a/hw/spinal/futurecore/execute/DataMemory.scala b/hw/spinal/futurecore/execute/DataMemory.scala
index 22afb5e..818798e 100644
--- a/hw/spinal/futurecore/execute/DataMemory.scala
+++ b/hw/spinal/futurecore/execute/DataMemory.scala
@@ -3,7 +3,7 @@ package futurecore.execute
 import spinal.core._
 
 import futurecore.ip.blackbox.ram_dpi
-import futurecore.misc.TypeExtensions.BitsSextExtension
+import futurecore.misc.TypeExtensions.{BitsSextExtension, BitsZeroExtension}
 
 object DataMemory {
   object AccessWidth extends SpinalEnum {
@@ -38,13 +38,13 @@ class DataMemory extends Component {
 
   val mem = new ram_dpi
 
-  mem.io.valid := io.inValidAddr
-  mem.io.raddr := io.inAddr
+  mem.io.ar.valid := io.inValidAddr & !io.inEnableWrite
+  mem.io.ar.addr := io.inAddr
 
-  val memData = mem.io.rdata
-  val dataByteZeroExt = byte(memData).resized
+  val memData = mem.io.r.data
+  val dataByteZeroExt = byte(memData).zext.asBits
   val dataByteSignExt = byte(memData).sext.asBits
-  val dataHalfZeroExt = half(memData).resized
+  val dataHalfZeroExt = half(memData).zext.asBits
   val dataHalfSignExt = half(memData).sext.asBits
 
   io.outDataRead := io.inSelAccessWidth.mux(
@@ -53,10 +53,10 @@ class DataMemory extends Component {
     AccessWidth.Word -> memData
   )
 
-  mem.io.wen := io.inEnableWrite
-  mem.io.waddr := io.inAddr
-  mem.io.wdata := io.inDataWrite
-  mem.io.wmask := io.inSelAccessWidth.mux(
+  mem.io.aw.valid := io.inEnableWrite & io.inValidAddr
+  mem.io.aw.addr := io.inAddr
+  mem.io.w.data := io.inDataWrite
+  mem.io.w.strb := io.inSelAccessWidth.mux(
     AccessWidth.Byte -> WriteMask.byte(),
     AccessWidth.Half -> WriteMask.half(),
     AccessWidth.Word -> WriteMask.word()
diff --git a/hw/spinal/futurecore/execute/ExecutePlugin.scala b/hw/spinal/futurecore/execute/ExecutePlugin.scala
index 3571368..257e93d 100644
--- a/hw/spinal/futurecore/execute/ExecutePlugin.scala
+++ b/hw/spinal/futurecore/execute/ExecutePlugin.scala
@@ -6,12 +6,13 @@ import spinal.lib.misc.plugin._
 import futurecore.fetch.FetchPlugin
 import futurecore.decode.{CtrlService, DecodePlugin}
 import futurecore.decode.CtrlService.CtrlDef
-import futurecore.riscv.Rvi
+import futurecore.riscv.{Rv32i, Zicsr, Privileged}
 
 class ExecutePlugin extends FiberPlugin {
   import SrcSelector.{SrcUpMode, SrcDownMode}
   import IntAlu.AluOp
   import DataMemory.AccessWidth
+  import CsrHandler.{CsrMode, ExceptionType}
 
   val setup = during setup new Area {
     val fp = host[FetchPlugin]
@@ -27,62 +28,82 @@ class ExecutePlugin extends FiberPlugin {
     val src = new SrcSelector
     val alu = new IntAlu
     val dm = new DataMemory
+    val csr = new CsrHandler
 
     val selUpDef = CtrlDef(SrcUpMode(), SrcUpMode.RegSrcA)
-      .setWhen(SrcUpMode.Pc, Rvi.Auipc, Rvi.Jal, Rvi.Jalr)
-      .setWhen(SrcUpMode.Zero, Rvi.Lui, Rvi.Ebreak)
+      .setWhen(SrcUpMode.Pc, Rv32i.Auipc, Rv32i.Jal, Rv32i.Jalr)
+      .setWhen(SrcUpMode.Zero, Rv32i.Lui, Rv32i.Ebreak)
     cs.registerCtrlSignal(selUpDef)
 
     val selDownDef = CtrlDef(SrcDownMode(), SrcDownMode.RegSrcB)
       .setWhen(
         SrcDownMode.Imm,
-        Rvi.instructions
-          .filter(_.fields.exists(_.isInstanceOf[Rvi.Imm]))
+        Rv32i.instructions
+          .filter(_.fields.exists(_.isInstanceOf[Rv32i.Imm]))
           // BImm is not used as ALU does the comparison between Rs1 and Rs2
-          .filterNot(_.fields.contains(Rvi.BImm))
+          .filterNot(_.fields.contains(Rv32i.BImm))
           // Imms of Jal and Jalr are not used as ALU is incrementing the PC
-          .filterNot(inst => inst == Rvi.Jal || inst == Rvi.Jalr)
+          .filterNot(inst => inst == Rv32i.Jal || inst == Rv32i.Jalr)
           // IImm of SYSTEM instructions are not used
-          .filterNot(_ == Rvi.Ebreak)
+          .filterNot(_ == Rv32i.Ebreak)
       )
-      .setWhen(SrcDownMode.PcIncrement, Rvi.Jal, Rvi.Jalr)
-      .setWhen(SrcDownMode.ReturnStatus, Rvi.Ebreak)
+      .setWhen(SrcDownMode.PcIncrement, Rv32i.Jal, Rv32i.Jalr)
+      .setWhen(SrcDownMode.ReturnStatus, Rv32i.Ebreak)
     cs.registerCtrlSignal(selDownDef)
 
     val aluOpDef = CtrlDef(AluOp(), AluOp.Add)
-      .setWhen(AluOp.Sub, Rvi.Sub)
-      .setWhen(AluOp.Xor, Rvi.Xori, Rvi.Xor)
-      .setWhen(AluOp.Or, Rvi.Ori, Rvi.Or)
-      .setWhen(AluOp.And, Rvi.Andi, Rvi.And)
-      .setWhen(AluOp.ShiftLeftLogic, Rvi.Slli, Rvi.Sll)
-      .setWhen(AluOp.ShiftRightLogic, Rvi.Srli, Rvi.Srl)
-      .setWhen(AluOp.ShiftRightArith, Rvi.Sra, Rvi.Srai)
-      .setWhen(AluOp.EqualTo, Rvi.Beq)
-      .setWhen(AluOp.NotEqual, Rvi.Bne)
-      .setWhen(AluOp.LessThan, Rvi.Blt, Rvi.Slti, Rvi.Slt)
-      .setWhen(AluOp.GreaterEqual, Rvi.Bge)
-      .setWhen(AluOp.LessThanUnsigned, Rvi.Bltu, Rvi.Sltiu, Rvi.Sltu)
-      .setWhen(AluOp.GreaterEqualUnsigned, Rvi.Bgeu)
+      .setWhen(AluOp.Sub, Rv32i.Sub)
+      .setWhen(AluOp.Xor, Rv32i.Xori, Rv32i.Xor)
+      .setWhen(AluOp.Or, Rv32i.Ori, Rv32i.Or)
+      .setWhen(AluOp.And, Rv32i.Andi, Rv32i.And)
+      .setWhen(AluOp.ShiftLeftLogic, Rv32i.Slli, Rv32i.Sll)
+      .setWhen(AluOp.ShiftRightLogic, Rv32i.Srli, Rv32i.Srl)
+      .setWhen(AluOp.ShiftRightArith, Rv32i.Sra, Rv32i.Srai)
+      .setWhen(AluOp.EqualTo, Rv32i.Beq)
+      .setWhen(AluOp.NotEqual, Rv32i.Bne)
+      .setWhen(AluOp.LessThan, Rv32i.Blt, Rv32i.Slti, Rv32i.Slt)
+      .setWhen(AluOp.GreaterEqual, Rv32i.Bge)
+      .setWhen(AluOp.LessThanUnsigned, Rv32i.Bltu, Rv32i.Sltiu, Rv32i.Sltu)
+      .setWhen(AluOp.GreaterEqualUnsigned, Rv32i.Bgeu)
     cs.registerCtrlSignal(aluOpDef)
 
     val memAddrValidDef = CtrlDef(Bool(), False)
-      .setWhen(True, Rvi.Lb, Rvi.Lbu, Rvi.Lh, Rvi.Lhu, Rvi.Lw)
-      .setWhen(True, Rvi.Sb, Rvi.Sh, Rvi.Sw)
+      .setWhen(True, Rv32i.Lb, Rv32i.Lbu, Rv32i.Lh, Rv32i.Lhu, Rv32i.Lw)
+      .setWhen(True, Rv32i.Sb, Rv32i.Sh, Rv32i.Sw)
     cs.registerCtrlSignal(memAddrValidDef)
 
     val memAccessDef = CtrlDef(AccessWidth(), AccessWidth.Byte)
-      .setWhen(AccessWidth.Half, Rvi.Lh, Rvi.Lhu, Rvi.Sh)
-      .setWhen(AccessWidth.Word, Rvi.Lw, Rvi.Sw)
+      .setWhen(AccessWidth.Half, Rv32i.Lh, Rv32i.Lhu, Rv32i.Sh)
+      .setWhen(AccessWidth.Word, Rv32i.Lw, Rv32i.Sw)
     cs.registerCtrlSignal(memAccessDef)
 
     val readSextDef = CtrlDef(Bool(), True)
-      .setWhen(False, Rvi.Lbu, Rvi.Lhu)
+      .setWhen(False, Rv32i.Lbu, Rv32i.Lhu)
     cs.registerCtrlSignal(readSextDef)
 
     val memWriteDef = CtrlDef(Bool(), False)
-      .setWhen(True, Rvi.Sb, Rvi.Sh, Rvi.Sw)
+      .setWhen(True, Rv32i.Sb, Rv32i.Sh, Rv32i.Sw)
     cs.registerCtrlSignal(memWriteDef)
 
+    val csrEnableDef = CtrlDef(Bool(), False)
+      .setWhen(True, Zicsr.instructions)
+    cs.registerCtrlSignal(csrEnableDef)
+
+    val csrModeDef = CtrlDef(CsrMode(), CsrMode.Write)
+      .setWhen(CsrMode.Set, Zicsr.Csrrs)
+    cs.registerCtrlSignal(csrModeDef)
+
+    val trapEnableDef = CtrlDef(Bool(), False)
+      .setWhen(True, Rv32i.Ecall)
+    cs.registerCtrlSignal(trapEnableDef)
+
+    val trapReturnDef = CtrlDef(Bool(), False)
+      .setWhen(True, Privileged.Mret)
+    cs.registerCtrlSignal(trapReturnDef)
+
+    val exceptionTypeDef = CtrlDef(ExceptionType(), ExceptionType.EcallM)
+    cs.registerCtrlSignal(exceptionTypeDef)
+
     buildBefore.release()
 
     val rs1 = Bits(32 bits)
@@ -97,6 +118,12 @@ class ExecutePlugin extends FiberPlugin {
     val memAccessWidth = AccessWidth()
     val readSext = Bool()
     val memWrite = Bool()
+    val csrEnable = Bool()
+    val csrMode = CsrMode()
+    val csrAddr = UInt(12 bits)
+    val trapEnable = Bool()
+    val trapReturn = Bool()
+    val exceptionType = ExceptionType()
 
     src.io.inRs1 := rs1
     src.io.inRs2 := rs2
@@ -116,6 +143,16 @@ class ExecutePlugin extends FiberPlugin {
     dm.io.inEnableReadSext := readSext
     dm.io.inEnableWrite := memWrite
     dm.io.inDataWrite := rs2
+
+    csr.io.inEnableCsr := csrEnable
+    csr.io.inSelCsrMode := csrMode
+    csr.io.inCsrAddr := csrAddr
+    csr.io.inNewVal := rs1
+
+    csr.io.inEnableTrap := trapEnable
+    csr.io.inEnableTrapReturn := trapReturn
+    csr.io.inSelException := exceptionType
+    csr.io.inExceptionPc := pc
   }
 
   val interconnect = during build new Area {
@@ -136,11 +173,31 @@ class ExecutePlugin extends FiberPlugin {
     l.readSext := cs.getCtrlSignal(l.readSextDef)
     l.memWrite := cs.getCtrlSignal(l.memWriteDef)
     l.memAddrValid := cs.getCtrlSignal(l.memAddrValidDef)
+    l.csrEnable := cs.getCtrlSignal(l.csrEnableDef)
+    l.csrMode := cs.getCtrlSignal(l.csrModeDef)
+    l.csrAddr := dp.getCsrAddr()
+    l.trapEnable := cs.getCtrlSignal(l.trapEnableDef)
+    l.trapReturn := cs.getCtrlSignal(l.trapReturnDef)
+    l.exceptionType := cs.getCtrlSignal(l.exceptionTypeDef)
   }
 
-  def getResult(): SInt = logic.get.alu.io.outRes
+  def getAluResult(): SInt = logic.get.alu.io.outRes
 
   def getBranchCond(): Bool = logic.get.alu.io.outRes.lsb
 
   def getMemOut(): Bits = logic.get.dm.io.outDataRead
+
+  def getCsrValue(): Bits = logic.get.csr.io.outOldVal
+
+  def getTrapVector(): UInt = logic.get.csr.io.outTrapVector
+
+  def getTrapReturn(): UInt = logic.get.csr.io.outReturnPc
+
+  def getDbgMstatus(): Bits = logic.get.csr.io.dbgMstatus
+
+  def getDbgMtvec(): Bits = logic.get.csr.io.dbgMtvec
+
+  def getDbgMepc(): Bits = logic.get.csr.io.dbgMepc
+
+  def getDbgMcause(): Bits = logic.get.csr.io.dbgMcause
 }
diff --git a/hw/spinal/futurecore/fetch/BranchTargeter.scala b/hw/spinal/futurecore/fetch/BranchTargeter.scala
index 0465f11..f0ea8a8 100644
--- a/hw/spinal/futurecore/fetch/BranchTargeter.scala
+++ b/hw/spinal/futurecore/fetch/BranchTargeter.scala
@@ -4,7 +4,7 @@ import spinal.core._
 
 object BranchTargeter {
   object BranchMode extends SpinalEnum {
-    val PcReletive, Displacement = newElement()
+    val PcReletive, Displacement, Trap, TrapReturn = newElement()
   }
 }
 
@@ -15,22 +15,34 @@ class BranchTargeter extends Component {
     val inSelMode = in port BranchMode()
     val inBaseReg = in port Bits(32 bits)
     val inPc = in port UInt(32 bits)
+    val inTrap = in port UInt(32 bits)
+    val inEpc = in port UInt(32 bits)
     val inOffsetImm = in port SInt(32 bits)
     val outTarget = out port UInt(32 bits)
   }
 
   val base = io.inSelMode.mux(
     BranchMode.PcReletive   -> io.inPc,
-    BranchMode.Displacement -> io.inBaseReg.asUInt
+    BranchMode.Displacement -> io.inBaseReg.asUInt,
+    BranchMode.Trap         -> io.inTrap,
+    BranchMode.TrapReturn   -> io.inEpc
   )
 
-  val targetUnaligned = (base.asSInt + io.inOffsetImm).asUInt
+  val doDisplace = io.inSelMode.mux(
+    BranchMode.PcReletive   -> True,
+    BranchMode.Displacement -> True,
+    BranchMode.Trap         -> False,
+    BranchMode.TrapReturn   -> False
+  )
+
+  val targetRaw = base
+  val targetDisplacedUnaligned = (base.asSInt + io.inOffsetImm).asUInt
 
   val AlignMask = ~U"32'h1"
-  val targetAligned =
+  val targetDisplacedAligned =
     (io.inSelMode === BranchMode.Displacement) ?
-      (targetUnaligned & AlignMask) |
-      targetUnaligned
+      (targetDisplacedUnaligned & AlignMask) |
+      targetDisplacedUnaligned
 
-  io.outTarget := targetAligned
+  io.outTarget := doDisplace ? targetDisplacedAligned | targetRaw
 }
diff --git a/hw/spinal/futurecore/fetch/FetchPlugin.scala b/hw/spinal/futurecore/fetch/FetchPlugin.scala
index 1752f6a..841dbc8 100644
--- a/hw/spinal/futurecore/fetch/FetchPlugin.scala
+++ b/hw/spinal/futurecore/fetch/FetchPlugin.scala
@@ -4,7 +4,7 @@ import spinal.core._
 import spinal.lib.misc.plugin._
 import spinal.lib.BinaryBuilder2
 
-import futurecore.riscv.Rvi
+import futurecore.riscv.{Rv32i, Privileged}
 import futurecore.decode.CtrlService
 import futurecore.decode.CtrlService.CtrlDef
 import futurecore.execute.ExecutePlugin
@@ -29,29 +29,36 @@ class FetchPlugin extends FiberPlugin {
     val im = new InstructionMemory
 
     val branchModeDef = CtrlDef(BranchMode(), BranchMode.PcReletive)
-      .setWhen(BranchMode.Displacement, Rvi.Jalr)
+      .setWhen(BranchMode.Displacement, Rv32i.Jalr)
+      .setWhen(BranchMode.Trap, Rv32i.Ecall)
+      .setWhen(BranchMode.TrapReturn, Privileged.Mret)
     cs.registerCtrlSignal(branchModeDef)
 
     val isUncondDef = CtrlDef(Bool(), False)
-      .setWhen(True, Rvi.Jal, Rvi.Jalr)
+      .setWhen(True, Rv32i.Jal, Rv32i.Jalr)
+      .setWhen(True, Rv32i.Ecall, Privileged.Mret)
     cs.registerCtrlSignal(isUncondDef)
 
     val isCondDef = CtrlDef(Bool(), False)
-      .setWhen(True, Rvi.Beq, Rvi.Bge, Rvi.Bgeu, Rvi.Blt, Rvi.Bltu, Rvi.Bne)
+      .setWhen(True, Rv32i.Beq, Rv32i.Bge, Rv32i.Bgeu, Rv32i.Blt, Rv32i.Bltu, Rv32i.Bne)
     cs.registerCtrlSignal(isCondDef)
 
     buildBefore.release()
 
-    val rs1 = Bits(32 bits)
-    val imm = SInt(32 bits)
+    val baseRs1 = Bits(32 bits)
+    val offsetImm = SInt(32 bits)
+    val trapVector = UInt(32 bits)
+    val epc = UInt(32 bits)
     val branchMode = BranchMode()
     val isCond = Bool()
     val isUncond = Bool()
     val branchCond = Bool()
 
     bt.io.inPc := pc.io.outInstAddr
-    bt.io.inBaseReg := rs1
-    bt.io.inOffsetImm := imm
+    bt.io.inBaseReg := baseRs1
+    bt.io.inOffsetImm := offsetImm
+    bt.io.inTrap := trapVector
+    bt.io.inEpc := epc
     bt.io.inSelMode := branchMode
 
     pc.io.inTargetAddr := bt.io.outTarget
@@ -66,8 +73,10 @@ class FetchPlugin extends FiberPlugin {
     val ep = setup.get.ep
     val l = logic.get
 
-    l.rs1 := dp.getRs1()
-    l.imm := dp.getImm()
+    l.baseRs1 := dp.getRs1()
+    l.offsetImm := dp.getImm()
+    l.trapVector := ep.getTrapVector()
+    l.epc := ep.getTrapReturn()
     l.branchMode := cs.getCtrlSignal(l.branchModeDef)
     l.isCond := cs.getCtrlSignal(l.isCondDef)
     l.isUncond := cs.getCtrlSignal(l.isUncondDef)
diff --git a/hw/spinal/futurecore/ip/blackbox/MemDpi.scala b/hw/spinal/futurecore/ip/blackbox/MemDpi.scala
index 627a5bf..fc9aa81 100644
--- a/hw/spinal/futurecore/ip/blackbox/MemDpi.scala
+++ b/hw/spinal/futurecore/ip/blackbox/MemDpi.scala
@@ -20,16 +20,42 @@ class ram_dpi extends BlackBox {
   val io = new Bundle {
     val clk = in port Bool()
     val resetn = in port Bool()
-    val valid = in port Bool()
-    val raddr = in port UInt(32 bits)
-    val rdata = out port Bits(32 bits)
-    val wen = in port Bool()
-    val waddr = in port UInt(32 bits)
-    val wdata = in port Bits(32 bits)
-    val wmask = in port Bits(8 bits)
+
+    val ar = new Bundle {
+      val valid = in port Bool()
+      val addr = in port UInt(32 bits)
+    }
+
+    val r = new Bundle {
+      val data = out port Bits(32 bits)
+    }
+
+    val aw = new Bundle {
+      val valid = in port Bool()
+      val addr = in port UInt(32 bits)
+    }
+
+    val w = new Bundle {
+      val data = in port Bits(32 bits)
+      val strb = in port Bits(8 bits)
+    }
   }
 
   noIoPrefix()
   mapClockDomain(clock = io.clk, reset = io.resetn, resetActiveLevel = LOW)
   addRTLPath("hw/verilog/mem_dpi.sv")
+
+  private def renameIO(): Unit = {
+    io.flatten.foreach(bt => {
+      bt.setName(
+        bt.getName()
+          .replace("ar_", "ar")
+          .replace("r_", "r")
+          .replace("aw_", "aw")
+          .replace("w_", "w")
+      )
+    })
+  }
+
+  addPrePopTask(() => renameIO())
 }
diff --git a/hw/spinal/futurecore/ip/blackbox/dpi/debug_dpi.scala b/hw/spinal/futurecore/ip/blackbox/dpi/debug_dpi.scala
index f47b7d2..a63aa2b 100644
--- a/hw/spinal/futurecore/ip/blackbox/dpi/debug_dpi.scala
+++ b/hw/spinal/futurecore/ip/blackbox/dpi/debug_dpi.scala
@@ -15,6 +15,10 @@ class debug_dpi(gprNum: Int, gprWidth: Int) extends BlackBox {
       val pc_i = in port UInt(32 bits)
       val inst_i = in port Bits(32 bits)
       val gprs_i = in port Bits(gprNum * gprWidth bits)
+      val mstatus_i = in port Bits(32 bits)
+      val mtvec_i = in port Bits(32 bits)
+      val mepc_i = in port Bits(32 bits)
+      val mcause_i = in port Bits(32 bits)
     }
   }
 
diff --git a/hw/spinal/futurecore/legacy/MAU.scala b/hw/spinal/futurecore/legacy/MAU.scala
index 8afdd0a..93a39ff 100644
--- a/hw/spinal/futurecore/legacy/MAU.scala
+++ b/hw/spinal/futurecore/legacy/MAU.scala
@@ -43,48 +43,48 @@ case class MAU() extends Component {
   val mem = new ram_dpi
   val dataReaded = Bits(32 bits)
 
-  mem.io.valid := io.ctrl.memValid
+  mem.io.ar.valid := io.ctrl.memValid
   switch(io.ctrl.accessType) {
-    mem.io.raddr := io.input.memAddr
+    mem.io.ar.addr := io.input.memAddr
     is(AccessWidth.Byte) {
       dataReaded := Mux(
         io.ctrl.dataSextEn,
-        mem.io.rdata(7 downto 0).asSInt.resize(32 bits).asBits,
-        mem.io.rdata(7 downto 0).resize(32 bits)
+        mem.io.r.data(7 downto 0).asSInt.resize(32 bits).asBits,
+        mem.io.r.data(7 downto 0).resize(32 bits)
       )
     }
     is(AccessWidth.Half) {
       dataReaded := Mux(
         io.ctrl.dataSextEn,
-        mem.io.rdata(15 downto 0).asSInt.resize(32 bits).asBits,
-        mem.io.rdata(15 downto 0).resize(32 bits)
+        mem.io.r.data(15 downto 0).asSInt.resize(32 bits).asBits,
+        mem.io.r.data(15 downto 0).resize(32 bits)
       )
     }
     is(AccessWidth.Word) {
-      dataReaded := mem.io.rdata
+      dataReaded := mem.io.r.data
     }
   }
 
   io.output.memRe := dataReaded
 
-  mem.io.wen := False
-  mem.io.wmask := B"8'b0"
-  mem.io.waddr := U"32'b0"
-  mem.io.wdata := B"32'b0"
+  mem.io.aw.valid := False
+  mem.io.w.strb := B"8'b0"
+  mem.io.aw.addr := U"32'b0"
+  mem.io.w.data := B"32'b0"
 
   when(io.ctrl.memWrEn) {
-    mem.io.wen := True
-    mem.io.waddr := io.input.memAddr
-    mem.io.wdata := io.input.memWr
+    mem.io.aw.valid := True
+    mem.io.aw.addr := io.input.memAddr
+    mem.io.w.data := io.input.memWr
     switch(io.ctrl.accessType) {
       is(AccessWidth.Byte) {
-        mem.io.wmask := B"8'b0001"
+        mem.io.w.strb := B"8'b0001"
       }
       is(AccessWidth.Half) {
-        mem.io.wmask := B"8'b0011"
+        mem.io.w.strb := B"8'b0011"
       }
       is(AccessWidth.Word) {
-        mem.io.wmask := B"8'b1111"
+        mem.io.w.strb := B"8'b1111"
       }
     }
   }
diff --git a/hw/spinal/futurecore/misc/TypeExtensions.scala b/hw/spinal/futurecore/misc/TypeExtensions.scala
index 345904a..04b5ffe 100644
--- a/hw/spinal/futurecore/misc/TypeExtensions.scala
+++ b/hw/spinal/futurecore/misc/TypeExtensions.scala
@@ -6,4 +6,8 @@ object TypeExtensions {
   implicit class BitsSextExtension(b: Bits) {
     def sext: SInt = b.asSInt.resize(32 bits)
   }
+
+  implicit class BitsZeroExtension(b: Bits) {
+    def zext: UInt = b.asUInt.resize(32 bits)
+  }
 }
diff --git a/hw/spinal/futurecore/riscv/Privileged.scala b/hw/spinal/futurecore/riscv/Privileged.scala
new file mode 100644
index 0000000..3da7ca5
--- /dev/null
+++ b/hw/spinal/futurecore/riscv/Privileged.scala
@@ -0,0 +1,22 @@
+package futurecore.riscv
+
+import spinal.core._
+
+object Privileged extends InstructionSet {
+  val Opcode = Rv32i.Opcode
+  val Funct3 = Rv32i.Funct3
+  val Funct7 = Rv32i.Funct7
+
+  def TypeSystem(pat: MaskedLiteral) = Instruction(pat, Seq(Opcode, Funct3, Funct7))
+
+  // ! WARNING: Ecall & Ebreak of Rv32i are PRIVILEGED related instructions.
+  // !          Should be handled VERY CAREFULLY!
+  // !          For now the detailed privilege-related ops are not supported!
+  val Mret = TypeSystem(M"0011000_00010_00000_000_00000_1110011")
+
+  override def ident: String = "priv_m"
+
+  override def instructions: Seq[Instruction] = Seq(
+    Mret
+  )
+}
diff --git a/hw/spinal/futurecore/riscv/Rvi.scala b/hw/spinal/futurecore/riscv/Rv32i.scala
similarity index 94%
rename from hw/spinal/futurecore/riscv/Rvi.scala
rename to hw/spinal/futurecore/riscv/Rv32i.scala
index ec14b67..e628f6a 100644
--- a/hw/spinal/futurecore/riscv/Rvi.scala
+++ b/hw/spinal/futurecore/riscv/Rv32i.scala
@@ -3,7 +3,7 @@ package futurecore.riscv
 import spinal.core._
 
 /** RV32I Base Instruction Set Dinition */
-object Rvi extends InstructionSet {
+object Rv32i extends InstructionSet {
   object Opcode extends BitField(6 downto 0)
   object Rd extends BitField(11 downto 7)
   object Funct3 extends BitField(14 downto 12)
@@ -12,7 +12,7 @@ object Rvi extends InstructionSet {
   object Funct7 extends BitField(31 downto 25)
 
   // Immediate Definitions (Little Endian)
-  class Imm(r: Range*) extends BitField(r: _*)
+  abstract class Imm(r: Range*) extends BitField(r: _*)
   // I-type immediate: single contiguous range [31:20]
   object IImm extends Imm(31 downto 20)
   // S-type immediate: distributed across two ranges [31:25|11:7]
@@ -94,13 +94,15 @@ object Rvi extends InstructionSet {
   val Pause = TypeI(M"0000_0001_0000_-----_000_-----_0001111")
 
   // I-type: Environment (funct12_rs1_000_rd_1110011)
+  // ! WARNING: Ecall & Ebreak are PRIVILEGED related instructions!
+  // !          Refer to Privileged.scala!
   val Ecall = TypeI(M"000000000000_00000_000_00000_1110011")
   val Ebreak = TypeI(M"000000000001_00000_000_00000_1110011")
 
   // InstructionSet trait implementation
   override def ident: String = "rv32i"
 
-  override def instructions: Seq[Instruction] = List(
+  override def instructions: Seq[Instruction] = Seq(
     Lui,
     Auipc,
     Jal,
@@ -141,7 +143,7 @@ object Rvi extends InstructionSet {
     // Fence,
     // FenceTso,
     // Pause,
-    // Ecall,
+    Ecall,
     Ebreak
   )
 }
diff --git a/hw/spinal/futurecore/riscv/Zicsr.scala b/hw/spinal/futurecore/riscv/Zicsr.scala
new file mode 100644
index 0000000..bb780d6
--- /dev/null
+++ b/hw/spinal/futurecore/riscv/Zicsr.scala
@@ -0,0 +1,35 @@
+package futurecore.riscv
+
+import spinal.core._
+
+object Zicsr extends InstructionSet {
+  val Opcode = Rv32i.Opcode
+  val Rd = Rv32i.Rd
+  val Func3 = Rv32i.Funct3
+  val Rs1 = Rv32i.Rs1
+
+  object Csr extends BitField(31 downto 20)
+  object MicroImm extends Rv32i.Imm(19 downto 15)
+
+  def TypeR(pat: MaskedLiteral) = Instruction(pat, Seq(Opcode, Rd, Func3, Rs1, Csr))
+  def TypeI(pat: MaskedLiteral) = Instruction(pat, Seq(Opcode, Rd, MicroImm, Csr))
+
+  val Csrrw = TypeR(M"-------_-----_-----_001_-----_1110011")
+  val Csrrs = TypeR(M"-------_-----_-----_010_-----_1110011")
+  val Csrrc = TypeR(M"-------_-----_-----_011_-----_1110011")
+  val Csrrwi = TypeI(M"------------_-----_101_-----_1110011")
+  val Csrrsi = TypeI(M"------------_-----_110_-----_1110011")
+  val Csrrci = TypeI(M"------------_-----_111_-----_1110011")
+
+  override def ident: String = "zicsr"
+
+  override def instructions: Seq[Instruction] = Seq(
+    Csrrw,
+    Csrrs
+    // CsrRc,
+    // CsrRwI,
+    // CsrRsI,
+    // CsrRcI
+  )
+
+}
diff --git a/hw/spinal/futurecore/writeback/CommitSelector.scala b/hw/spinal/futurecore/writeback/CommitSelector.scala
index a56fb33..2a49ca3 100644
--- a/hw/spinal/futurecore/writeback/CommitSelector.scala
+++ b/hw/spinal/futurecore/writeback/CommitSelector.scala
@@ -4,7 +4,7 @@ import spinal.core._
 
 object CommitSelector {
   object CommitSource extends SpinalEnum {
-    val Result, Memory = newElement()
+    val AluResults, CsrValue, Memory = newElement()
   }
 }
 
@@ -12,14 +12,16 @@ class CommitSelector extends Component {
   import CommitSelector._
 
   val io = new Bundle {
-    val inResult = in port SInt(32 bits)
+    val inAluResult = in port SInt(32 bits)
+    val inCsrValue = in port Bits(32 bits)
     val inMemory = in port Bits(32 bits)
     val inSelSource = in port CommitSource()
     val outCommit = out port Bits(32 bits)
   }
 
   io.outCommit := io.inSelSource.mux(
-    CommitSource.Result -> io.inResult.asBits,
-    CommitSource.Memory -> io.inMemory
+    CommitSource.AluResults -> io.inAluResult.asBits,
+    CommitSource.CsrValue   -> io.inCsrValue,
+    CommitSource.Memory     -> io.inMemory
   )
 }
diff --git a/hw/spinal/futurecore/writeback/WritebackPlugin.scala b/hw/spinal/futurecore/writeback/WritebackPlugin.scala
index 95d86eb..ee95c9d 100644
--- a/hw/spinal/futurecore/writeback/WritebackPlugin.scala
+++ b/hw/spinal/futurecore/writeback/WritebackPlugin.scala
@@ -6,7 +6,7 @@ import spinal.lib.misc.plugin.FiberPlugin
 
 import futurecore.decode.CtrlService
 import futurecore.decode.CtrlService.CtrlDef
-import futurecore.riscv.Rvi
+import futurecore.riscv.{Rv32i, Zicsr}
 import futurecore.execute.ExecutePlugin
 
 class WritebackPlugin extends FiberPlugin {
@@ -25,26 +25,29 @@ class WritebackPlugin extends FiberPlugin {
     val com = new CommitSelector
     val ebreak = new EbreakHandler
 
-    val commitSrcDef = CtrlDef(CommitSource(), CommitSource.Result)
-      .setWhen(CommitSource.Memory, Rvi.Lb, Rvi.Lh, Rvi.Lw, Rvi.Lbu, Rvi.Lhu)
+    val commitSrcDef = CtrlDef(CommitSource(), CommitSource.AluResults)
+      .setWhen(CommitSource.Memory, Rv32i.Lb, Rv32i.Lh, Rv32i.Lw, Rv32i.Lbu, Rv32i.Lhu)
+      .setWhen(CommitSource.CsrValue, Zicsr.instructions)
     cs.registerCtrlSignal(commitSrcDef)
 
-    val isEbreakDef = CtrlDef(Bool(), False).setWhen(True, Rvi.Ebreak)
+    val isEbreakDef = CtrlDef(Bool(), False).setWhen(True, Rv32i.Ebreak)
     cs.registerCtrlSignal(isEbreakDef)
 
     buildBefore.release()
 
-    val result = SInt(32 bits)
+    val aluResult = SInt(32 bits)
+    val csrValue = Bits(32 bits)
     val memOut = Bits(32 bits)
     val commitSrc = CommitSource()
     val isEbreak = Bool()
 
-    com.io.inResult := result
+    com.io.inAluResult := aluResult
+    com.io.inCsrValue := csrValue
     com.io.inMemory := memOut
     com.io.inSelSource := commitSrc
 
     ebreak.io.inEnable := isEbreak
-    ebreak.io.inReturnStatus := result
+    ebreak.io.inReturnStatus := aluResult
   }
 
   val interconnect = during build new Area {
@@ -52,7 +55,8 @@ class WritebackPlugin extends FiberPlugin {
     val cs = setup.get.cs
     val l = logic.get
 
-    l.result := ep.getResult()
+    l.aluResult := ep.getAluResult()
+    l.csrValue := ep.getCsrValue()
     l.memOut := ep.getMemOut()
     l.commitSrc := cs.getCtrlSignal(l.commitSrcDef)
     l.isEbreak := cs.getCtrlSignal(l.isEbreakDef)
diff --git a/hw/verilog/debug_dpi.sv b/hw/verilog/debug_dpi.sv
index 79d8221..8b05356 100644
--- a/hw/verilog/debug_dpi.sv
+++ b/hw/verilog/debug_dpi.sv
@@ -6,19 +6,27 @@ module debug_dpi #(
     input rst_ni,
     input [31:0] s_pc_i,
     input [31:0] s_inst_i,
-    input [GprsWidth - 1:0] s_gprs_i
+    input [GprsWidth - 1:0] s_gprs_i,
+    input [31:0] s_mstatus_i,
+    input [31:0] s_mtvec_i,
+    input [31:0] s_mepc_i,
+    input [31:0] s_mcause_i
 );
   localparam int GprsWidth = GPR_NUM * GPR_WIDTH;
 
   import "DPI-C" function void send_state(
     input bit [31:0] pc,
     input bit [31:0] inst,
-    input bit [GprsWidth - 1:0] gprs
+    input bit [GprsWidth - 1:0] gprs,
+    input bit [31:0] mstatus,
+    input bit [31:0] mtvec,
+    input bit [31:0] mepc,
+    input bit [31:0] mcause
   );
 
   always_comb begin
     if (rst_ni) begin
-      send_state(s_pc_i, s_inst_i, s_gprs_i);
+      send_state(s_pc_i, s_inst_i, s_gprs_i, s_mstatus_i, s_mtvec_i, s_mepc_i, s_mcause_i);
     end
   end
 
diff --git a/hw/verilog/mem_dpi.sv b/hw/verilog/mem_dpi.sv
index 420a99d..5a6bdce 100644
--- a/hw/verilog/mem_dpi.sv
+++ b/hw/verilog/mem_dpi.sv
@@ -8,27 +8,30 @@ import "DPI-C" context function void pmem_write(
 module ram_dpi (
     input wire clk,
     input wire resetn,
-    input wire valid,
-    input wire [31:0] raddr,
-    input wire wen,
-    input wire [31:0] waddr,
+
+    input wire arvalid,
+    input wire [31:0] araddr,
+
+    input wire awvalid,
+    input wire [31:0] awaddr,
+
     input wire [31:0] wdata,
-    input wire [7:0] wmask,
+    input wire [ 7:0] wstrb,
+
     output wire [31:0] rdata
 );
 
   always_ff @(posedge clk) begin
-    if (resetn & valid) begin
-      if (wen) begin  // 有写请求时
-        pmem_write(waddr, wdata, wmask);
-      end
+    if (resetn & awvalid) begin
+      pmem_write(awaddr, wdata, wstrb);
     end
   end
 
-  assign rdata = resetn & valid ? pmem_read(raddr) : 0;
+  assign rdata = resetn & arvalid ? pmem_read(araddr) : 0;
 
 endmodule
 
+
 module rom_dpi (
     input wire clk,
     input wire resetn,
diff --git a/sim/rust/futurecore/src/arch/rv32i.rs b/sim/rust/futurecore/src/arch/rv32i.rs
index 7196230..2f35f8b 100644
--- a/sim/rust/futurecore/src/arch/rv32i.rs
+++ b/sim/rust/futurecore/src/arch/rv32i.rs
@@ -12,6 +12,10 @@ pub const DEFAULT_IMAGE: [u32; 5] = [
 pub struct Registers {
     gpr: [u32; 32],
     pc: u32,
+    mstatus: u32,
+    mtvec: u32,
+    mepc: u32,
+    mcause: u32,
 }
 
 impl Registers {
@@ -19,6 +23,24 @@ impl Registers {
         Registers::default()
     }
 
+    pub fn with_fields(
+        gprs: &[u32; 32],
+        pc: u32,
+        mstatus: u32,
+        mtvec: u32,
+        mepc: u32,
+        mcause: u32,
+    ) -> Self {
+        Registers {
+            gpr: *gprs,
+            pc,
+            mstatus,
+            mtvec,
+            mepc,
+            mcause,
+        }
+    }
+
     pub fn gpr(&self) -> &[u32; 32] {
         &self.gpr
     }
@@ -26,6 +48,22 @@ impl Registers {
     pub fn pc(&self) -> u32 {
         self.pc
     }
+
+    pub fn mstatus(&self) -> u32 {
+        self.mstatus
+    }
+
+    pub fn mtvec(&self) -> u32 {
+        self.mtvec
+    }
+
+    pub fn mepc(&self) -> u32 {
+        self.mepc
+    }
+
+    pub fn mcause(&self) -> u32 {
+        self.mcause
+    }
 }
 
 impl Default for Registers {
@@ -33,20 +71,10 @@ impl Default for Registers {
         Registers {
             gpr: [0; 32],
             pc: RESET_VECTOR,
+            mstatus: 0x1800, // MPP=0b11 (machine mode)
+            mtvec: 0,
+            mepc: 0,
+            mcause: 0,
         }
     }
 }
-
-impl TryFrom<&[u32]> for Registers {
-    type Error = &'static str;
-
-    fn try_from(value: &[u32]) -> Result<Self, Self::Error> {
-        if value.len() != 33 {
-            return Err("Expected 32 GPRs and 1 PC, got a different length");
-        }
-        let mut regs = Registers::new();
-        regs.gpr.copy_from_slice(&value[0..32]);
-        regs.pc = value[32];
-        Ok(regs)
-    }
-}
diff --git a/sim/rust/futurecore/src/core/sim/dpic.rs b/sim/rust/futurecore/src/core/sim/dpic.rs
index e3caaa4..aacbf1f 100644
--- a/sim/rust/futurecore/src/core/sim/dpic.rs
+++ b/sim/rust/futurecore/src/core/sim/dpic.rs
@@ -132,27 +132,46 @@ pub extern "C" fn get_regs(gpr: *const u64) {
             .map(|n| *n as u32)
             .collect::<Vec<u32>>()
     };
-    let mut regs = REGISTERS
+
+    let mut gpr_array = [0u32; 32];
+    gpr_array.copy_from_slice(&gpr[0..32]);
+    let pc = gpr[32];
+
+    let regs = Registers::with_fields(&gpr_array, pc, 0x1800, 0, 0, 0);
+
+    let mut dpi_regs = REGISTERS
         .get()
         .expect("DPI-REGISTERS not initialized")
         .borrow_mut();
-    *regs = Registers::try_from(gpr.as_slice())
-        .expect("Failed to convert raw registers to rv32i::Registers");
+    *dpi_regs = regs;
 }
 
 #[unsafe(no_mangle)]
-pub extern "C" fn send_state(pc: *const u32, _inst: *const u32, gprs: *const u32) {
-    let gprs = unsafe { slice::from_raw_parts(gprs, 32) };
+pub extern "C" fn send_state(
+    pc: *const u32,
+    _inst: *const u32,
+    gprs: *const u32,
+    mstatus: *const u32,
+    mtvec: *const u32,
+    mepc: *const u32,
+    mcause: *const u32,
+) {
+    let gprs = unsafe { slice::from_raw_parts(gprs, 32) }
+        .as_array::<32>()
+        .expect("size of gprs passed in is not 32");
     let pc = unsafe { *pc };
+    let mstatus = unsafe { *mstatus };
+    let mtvec = unsafe { *mtvec };
+    let mepc = unsafe { *mepc };
+    let mcause = unsafe { *mcause };
 
-    let gpr_pc: Vec<u32> = [gprs, &[pc]].concat();
+    let regs = Registers::with_fields(gprs, pc, mstatus, mtvec, mepc, mcause);
 
-    let mut regs = REGISTERS
+    let mut dpi_regs = REGISTERS
         .get()
         .expect("DPI-REGISTERS not initialized")
         .borrow_mut();
-    *regs = Registers::try_from(gpr_pc.as_slice())
-        .expect("Failed to convert raw registers to rv32i::Registers");
+    *dpi_regs = regs;
 }
 
 pub fn init(
diff --git a/sim/rust/futurecore/src/core/sim/mod.rs b/sim/rust/futurecore/src/core/sim/mod.rs
index 7c3570e..3eac0ee 100644
--- a/sim/rust/futurecore/src/core/sim/mod.rs
+++ b/sim/rust/futurecore/src/core/sim/mod.rs
@@ -65,9 +65,9 @@ impl Executor for Simulator {
             &executor.devices,
         );
 
-        let diff_ctx: Registers = executor.registers.borrow().clone();
-        let diff_mem: Memory = executor.memory.borrow().clone();
-        diff_init(diff_mem, diff_ctx);
+        // let diff_ctx: Registers = executor.registers.borrow().clone();
+        // let diff_mem: Memory = executor.memory.borrow().clone();
+        // diff_init(diff_mem, diff_ctx);
 
         executor.runtime.reset(10);
 
@@ -84,9 +84,9 @@ impl Executor for Simulator {
                 break;
             }
             cycle_executed += 1;
-            if cycle_executed > 1 {
-                diff_test(self.registers.borrow().clone());
-            };
+            // if cycle_executed > 1 {
+            //     diff_test(self.registers.borrow().clone());
+            // };
         }
 
         Ok(())
diff --git a/sim/rust/futurecore/src/utils/diff.rs b/sim/rust/futurecore/src/utils/diff.rs
index 109beef..552d15a 100644
--- a/sim/rust/futurecore/src/utils/diff.rs
+++ b/sim/rust/futurecore/src/utils/diff.rs
@@ -28,13 +28,45 @@ struct Api {
 }
 
 #[repr(C)]
-#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+#[derive(Debug, Clone, Copy)]
 pub struct Context {
     gpr: [u32; 32],
     csr: [u32; 4096],
     pc: u32,
 }
 
+impl Context {
+    fn diff(&self, other: &Self) -> Vec<(&'static str, u32, u32)> {
+        const GPR_NAMES: [&str; 32] = [
+            "x0", "x1", "x2", "x3", "x4", "x5", "x6", "x7", "x8", "x9", "x10", "x11", "x12", "x13",
+            "x14", "x15", "x16", "x17", "x18", "x19", "x20", "x21", "x22", "x23", "x24", "x25",
+            "x26", "x27", "x28", "x29", "x30", "x31",
+        ];
+        const CSR_NAMES: [(usize, &str); 4] = [
+            (0x300, "mstatus"),
+            (0x305, "mtvec"),
+            (0x341, "mepc"),
+            (0x342, "mcause"),
+        ];
+
+        let mut diffs = Vec::new();
+        for i in 0..32 {
+            if self.gpr[i] != other.gpr[i] {
+                diffs.push((GPR_NAMES[i], self.gpr[i], other.gpr[i]));
+            }
+        }
+        if self.pc != other.pc {
+            diffs.push(("pc", self.pc, other.pc));
+        }
+        for (addr, name) in CSR_NAMES {
+            if self.csr[addr] != other.csr[addr] {
+                diffs.push((name, self.csr[addr], other.csr[addr]));
+            }
+        }
+        diffs
+    }
+}
+
 impl Default for Context {
     fn default() -> Self {
         let mut csr = [0; 4096];
@@ -52,6 +84,11 @@ impl From<rv32i::Registers> for Context {
         let mut ctx = Context::default();
         ctx.gpr = *regs.gpr();
         ctx.pc = regs.pc();
+        // Copy CSR values to correct addresses
+        ctx.csr[0x300] = regs.mstatus();
+        ctx.csr[0x305] = regs.mtvec();
+        ctx.csr[0x341] = regs.mepc();
+        ctx.csr[0x342] = regs.mcause();
         ctx
     }
 }
@@ -139,11 +176,16 @@ pub fn test(dut_ctx: impl Into<Context>) {
                 );
             }
 
-            if ref_ctx != dut_ctx {
-                panic!(
-                    "Reference registers {:?} do not match DUT registers {:?}",
-                    ref_ctx, dut_ctx
-                );
+            let diffs = dut_ctx.diff(&ref_ctx);
+            if !diffs.is_empty() {
+                let mut msg = String::from("DUT registers do not match reference registers:\n");
+                for (name, dut_val, ref_val) in &diffs {
+                    msg.push_str(&format!(
+                        "  {}: ref=0x{:08x}, dut=0x{:08x}\n",
+                        name, ref_val, dut_val
+                    ));
+                }
+                panic!("{}", msg);
             }
         });
     });