[WIP] save trap info and return from traps

src/arch/metal.rs

@@ -49,6 +49,16 @@ impl Architecture for Metal {
         return MCause::new(mcause);
     }
 
+    fn read_mcause_raw() -> usize {
+        let mcause: usize;
+        unsafe {
+            asm!(
+                "csrr {x}, mcause",
+                x = out(reg) mcause);
+        }
+        return mcause;
+    }
+
     fn read_mepc() -> usize {
         let mepc: usize;
         unsafe {
@@ -69,6 +79,16 @@ impl Architecture for Metal {
         return mtval;
     }
 
+    fn read_mtinst() -> usize {
+        let mtinst: usize;
+        unsafe {
+            asm!(
+                "csrr {x}, mtinst",
+                x = out(reg) mtinst);
+        }
+        return mtinst;
+    }
+
     unsafe fn set_mpp(mode: Mode) {
         const MPP_MASK: usize = 0b11_usize << 11;
         let value = mode.to_bits() << 11;
@@ -83,6 +103,27 @@ impl Architecture for Metal {
         )
     }
 
+    unsafe fn write_mcause(mcause: usize) {
+        asm!(
+            "csrw mcause, {x}",
+            x = in(reg) mcause
+        )
+    }
+
+    unsafe fn write_mepc(mepc: usize) {
+        asm!(
+            "csrw mepc, {x}",
+            x = in(reg) mepc
+        )
+    }
+
+    unsafe fn write_mtval(mtval: usize) {
+        asm!(
+            "csrw mtval, {x}",
+            x = in(reg) mtval
+        )
+    }
+
     unsafe fn write_mstatus(mstatus: usize) {
         asm!(
             "csrw mstatus, {x}",

src/arch/mod.rs

@@ -23,11 +23,16 @@ pub trait Architecture {
     fn read_misa() -> usize;
     fn read_mstatus() -> usize;
     fn read_mcause() -> MCause;
+    fn read_mcause_raw() -> usize;
     fn read_mepc() -> usize;
     fn read_mtval() -> usize;
+    fn read_mtinst() -> usize;
     unsafe fn set_mpp(mode: Mode);
     unsafe fn write_misa(misa: usize);
     unsafe fn write_mstatus(mstatus: usize);
+    unsafe fn write_mcause(mcause: usize);
+    unsafe fn write_mepc(mepc: usize);
+    unsafe fn write_mtval(mtval: usize);
     unsafe fn write_pmpcfg(idx: usize, pmpcfg: usize);
     unsafe fn write_pmpaddr(idx: usize, pmpaddr: usize);
     unsafe fn mret() -> !;

src/decoder.rs

@@ -95,6 +95,7 @@ fn decode_system(raw: usize) -> Instr {
             0b000000000000 => Instr::Ecall,
             0b000000000001 => Instr::Ebreak,
             0b000100000101 => Instr::Wfi,
+            0b001100000010 => Instr::Mret,
             _ => Instr::Unknown,
         };
     }

src/main.rs

@@ -95,37 +95,18 @@ fn handle_trap(ctx: &mut VirtContext, max_exit: Option<usize>) {
             // Skip to next instruction
             ctx.pc += 4;
         }
-        cause@_ => {
+        cause @ _ => {
             // Save csr registers
-            ctx.csr.mcause = 0;
-            ctx.csr.mepc = 0;
-            ctx.csr.mtval = 0;
-            ctx.csr.mstatus = 0;
-
-            //Set new csr registers for the jump to OpenSbi
-       //     Arch::set_mcause();
-       //     Arch::set_mepc();
-       //     Arch::set_mtval();
-       //     Arch::set_mstatus();
-
-            ctx.pc = ctx.csr.mtvec //Go to OpenSbi trap handler    
+            ctx.csr.mcause = Arch::read_mcause_raw();
+            ctx.csr.mepc = Arch::read_mepc();
+            ctx.csr.mtval = Arch::read_mtval();
+            ctx.csr.mstatus = Arch::read_mstatus();
+            ctx.csr.mtinst = Arch::read_mtinst();
+
+
+            ctx.pc = ctx.csr.mtvec //Go to OpenSbi trap handler
         }
     }
-
-    /*
-       A trap should
-           - Jump to this trap handler
-           - Save values of registers into Virtual Context
-               - mepc
-               - mcause
-               - mtval
-               - mstatus
-           - Set new register values
-           - Jump to trap handler of OpenSbi
-           - mret will jump to mirage
-           - Set old register values
-           - Jump to mepc : pc = 0xFFFF
-    */
 }
 
 fn emulate_instr(ctx: &mut VirtContext, instr: &Instr) {
@@ -183,20 +164,29 @@ fn emulate_instr(ctx: &mut VirtContext, instr: &Instr) {
             ctx.set(rd, tmp);
         }
         Instr::Mret => {
-            // Return the values to the registers
-       //     Arch::set_mcause(ctx.csr.mcause);
-       //     Arch::set_mepc(ctx.csr.mepc);
-       //     Arch::set_mtval(ctx.csr.mtval);
-       //     Arch::set_mstatus(ctx.csr.mstatus);
+            //MPV = 0, MPP = 0, MIE= MPIE, MPIE = 1
+            let mpie = 0b1 & (ctx.csr.mstatus >> 7);
 
-            //Jump back to payload
-            ctx.pc = ctx.csr.mepc-4;
+            ctx.csr.mstatus = ctx.csr.mstatus | 0b1 << 7;
+
+            ctx.csr.mstatus = ctx.csr.mstatus & !(0b1 << 3);
+            ctx.csr.mstatus = ctx.csr.mstatus | mpie << 3;
 
+            ctx.csr.mstatus = ctx.csr.mstatus & !(0b1 << 39);
+            ctx.csr.mstatus = ctx.csr.mstatus & !(0b11 << 11);
+
+
+            //Jump back to payload
+            ctx.pc = ctx.csr.mepc;
         }
         _ => todo!("Instruction not yet implemented: {:?}", instr),
     }
 }
 
+fn not_implemented_csr() {
+
+}
+
 #[panic_handler]
 fn panic(info: &PanicInfo) -> ! {
     log::error!("Panicked at {:#?} ", info);

src/virt.rs

@@ -67,6 +67,7 @@ pub struct VirtCsr {
     pub mepc: usize,
     pub mtval: usize,
     pub mstatus: usize,
+    pub mtinst: usize,
 }
 
 impl Default for VirtCsr {
@@ -94,6 +95,7 @@ impl Default for VirtCsr {
             mepc: 0,
             mtval: 0,
             mstatus: 0,
+            mtinst: 0,
         }
     }
 }