Report correct scause when faulting while fetching emulated instruction

2 files changed, 26 insertions, 7 deletions

diff --git a/machine/mtrap.c b/machine/mtrap.c
index e48d3d5..42006b8 100644
--- a/machine/mtrap.c
+++ b/machine/mtrap.c
@@ -194,14 +194,30 @@ void pmp_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
   redirect_trap(mepc, read_csr(mstatus), read_csr(mbadaddr));
 }
 
-static void machine_page_fault(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
+static void machine_page_fault(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
 {
   // MPRV=1 iff this trap occurred while emulating an instruction on behalf
   // of a lower privilege level. In that case, a2=epc and a3=mstatus.
+  // a1 holds MPRV if emulating a load or store, or MPRV | MXR if loading
+  // an instruction from memory.  In the latter case, we should report an
+  // instruction fault instead of a load fault.
   if (read_csr(mstatus) & MSTATUS_MPRV) {
+    if (regs[11] == (MSTATUS_MPRV | MSTATUS_MXR)) {
+      if (mcause == CAUSE_LOAD_PAGE_FAULT)
+        write_csr(mcause, CAUSE_FETCH_PAGE_FAULT);
+      else if (mcause == CAUSE_LOAD_ACCESS)
+        write_csr(mcause, CAUSE_FETCH_ACCESS);
+      else
+        goto fail;
+    } else if (regs[11] != MSTATUS_MPRV) {
+      goto fail;
+    }
+
     return redirect_trap(regs[12], regs[13], read_csr(mbadaddr));
   }
-  bad_trap(regs, dummy, mepc);
+
+fail:
+  bad_trap(regs, mcause, mepc);
 }
 
 void trap_from_machine_mode(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
@@ -215,7 +231,7 @@ void trap_from_machine_mode(uintptr_t* regs, uintptr_t dummy, uintptr_t mepc)
     case CAUSE_FETCH_ACCESS:
     case CAUSE_LOAD_ACCESS:
     case CAUSE_STORE_ACCESS:
-      return machine_page_fault(regs, dummy, mepc);
+      return machine_page_fault(regs, mcause, mepc);
     default:
       bad_trap(regs, dummy, mepc);
   }
diff --git a/machine/unprivileged_memory.h b/machine/unprivileged_memory.h
index a904fd6..fb70baa 100644
--- a/machine/unprivileged_memory.h
+++ b/machine/unprivileged_memory.h
@@ -10,6 +10,7 @@
 #define DECLARE_UNPRIVILEGED_LOAD_FUNCTION(type, insn)              \
   static inline type load_##type(const type* addr, uintptr_t mepc)  \
   {                                                                 \
+    register uintptr_t __mstatus_adjust asm ("a1") = MSTATUS_MPRV;  \
     register uintptr_t __mepc asm ("a2") = mepc;                    \
     register uintptr_t __mstatus asm ("a3");                        \
     type val;                                                       \
@@ -17,20 +18,21 @@
          #insn " %1, %2\n"                                          \
          "csrw mstatus, %0"                                         \
          : "+&r" (__mstatus), "=&r" (val)                           \
-         : "m" (*addr), "r" (MSTATUS_MPRV), "r" (__mepc));          \
+         : "m" (*addr), "r" (__mstatus_adjust), "r" (__mepc));      \
     return val;                                                     \
   }
 
 #define DECLARE_UNPRIVILEGED_STORE_FUNCTION(type, insn)                 \
   static inline void store_##type(type* addr, type val, uintptr_t mepc) \
   {                                                                     \
+    register uintptr_t __mstatus_adjust asm ("a1") = MSTATUS_MPRV;      \
     register uintptr_t __mepc asm ("a2") = mepc;                        \
     register uintptr_t __mstatus asm ("a3");                            \
     asm volatile ("csrrs %0, mstatus, %3\n"                             \
                   #insn " %1, %2\n"                                     \
                   "csrw mstatus, %0"                                    \
                   : "+&r" (__mstatus)                                   \
-                  : "r" (val), "m" (*addr), "r" (MSTATUS_MPRV),         \
+                  : "r" (val), "m" (*addr), "r" (__mstatus_adjust),     \
                     "r" (__mepc));                                      \
   }
 
@@ -66,6 +68,7 @@ static inline void store_uint64_t(uint64_t* addr, uint64_t val, uintptr_t mepc)
 
 static uintptr_t __attribute__((always_inline)) get_insn(uintptr_t mepc, uintptr_t* mstatus)
 {
+  register uintptr_t __mstatus_adjust asm ("a1") = MSTATUS_MPRV | MSTATUS_MXR;
   register uintptr_t __mepc asm ("a2") = mepc;
   register uintptr_t __mstatus asm ("a3");
   uintptr_t val;
@@ -74,7 +77,7 @@ static uintptr_t __attribute__((always_inline)) get_insn(uintptr_t mepc, uintptr
        STR(LWU) " %[insn], (%[addr])\n"
        "csrw mstatus, %[mstatus]"
        : [mstatus] "+&r" (__mstatus), [insn] "=&r" (val)
-       : [mprv] "r" (MSTATUS_MPRV | MSTATUS_MXR), [addr] "r" (__mepc));
+       : [mprv] "r" (__mstatus_adjust), [addr] "r" (__mepc));
 #else
   uintptr_t rvc_mask = 3, tmp;
   asm ("csrrs %[mstatus], mstatus, %[mprv]\n"
@@ -95,7 +98,7 @@ static uintptr_t __attribute__((always_inline)) get_insn(uintptr_t mepc, uintptr
        "add %[insn], %[insn], %[tmp]\n"
        "2: csrw mstatus, %[mstatus]"
        : [mstatus] "+&r" (__mstatus), [insn] "=&r" (val), [tmp] "=&r" (tmp)
-       : [mprv] "r" (MSTATUS_MPRV | MSTATUS_MXR), [addr] "r" (__mepc),
+       : [mprv] "r" (__mstatus_adjust), [addr] "r" (__mepc),
          [rvc_mask] "r" (rvc_mask), [xlen_minus_16] "i" (__riscv_xlen - 16));
 #endif
   *mstatus = __mstatus;