1 files changed, 74 insertions, 12 deletions

diff --git a/src/arch/riscv/faults.cc b/src/arch/riscv/faults.cc
index ce4cb3846..efab6c444 100644
--- a/src/arch/riscv/faults.cc
+++ b/src/arch/riscv/faults.cc
@@ -32,6 +32,8 @@
  */
 #include "arch/riscv/faults.hh"
 
+#include "arch/riscv/isa.hh"
+#include "arch/riscv/registers.hh"
 #include "arch/riscv/system.hh"
 #include "arch/riscv/utility.hh"
 #include "cpu/base.hh"
@@ -39,10 +41,11 @@
 #include "sim/debug.hh"
 #include "sim/full_system.hh"
 
-using namespace RiscvISA;
+namespace RiscvISA
+{
 
 void
-RiscvFault::invoke_se(ThreadContext *tc, const StaticInstPtr &inst)
+RiscvFault::invokeSE(ThreadContext *tc, const StaticInstPtr &inst)
 {
     panic("Fault %s encountered at pc 0x%016llx.", name(), tc->pcState().pc());
 }
@@ -50,14 +53,71 @@ RiscvFault::invoke_se(ThreadContext *tc, const StaticInstPtr &inst)
 void
 RiscvFault::invoke(ThreadContext *tc, const StaticInstPtr &inst)
 {
+    PCState pcState = tc->pcState();
+
     if (FullSystem) {
-        panic("Full system mode not supported for RISC-V.");
+        PrivilegeMode pp = (PrivilegeMode)tc->readMiscReg(MISCREG_PRV);
+        PrivilegeMode prv = PRV_M;
+        STATUS status = tc->readMiscReg(MISCREG_STATUS);
+
+        // Set fault handler privilege mode
+        if (pp != PRV_M &&
+            bits(tc->readMiscReg(MISCREG_MEDELEG), _code) != 0) {
+            prv = PRV_S;
+        }
+        if (pp == PRV_U &&
+            bits(tc->readMiscReg(MISCREG_SEDELEG), _code) != 0) {
+            prv = PRV_U;
+        }
+
+        // Set fault registers and status
+        MiscRegIndex cause, epc, tvec;
+        switch (prv) {
+          case PRV_U:
+            cause = MISCREG_UCAUSE;
+            epc = MISCREG_UEPC;
+            tvec = MISCREG_UTVEC;
+
+            status.upie = status.uie;
+            status.uie = 0;
+            break;
+          case PRV_S:
+            cause = MISCREG_SCAUSE;
+            epc = MISCREG_SEPC;
+            tvec = MISCREG_STVEC;
+
+            status.spp = pp;
+            status.spie = status.sie;
+            status.sie = 0;
+            break;
+          case PRV_M:
+            cause = MISCREG_MCAUSE;
+            epc = MISCREG_MEPC;
+            tvec = MISCREG_MTVEC;
+
+            status.mpp = pp;
+            status.mpie = status.sie;
+            status.mie = 0;
+            break;
+          default:
+            panic("Unknown privilege mode %d.", prv);
+            break;
+        }
+
+        // Set fault cause, privilege, and return PC
+        tc->setMiscReg(cause,
+                       (isInterrupt() << (sizeof(MiscReg) * 4 - 1)) | _code);
+        tc->setMiscReg(epc, tc->instAddr());
+        tc->setMiscReg(MISCREG_PRV, prv);
+        tc->setMiscReg(MISCREG_STATUS, status);
+
+        // Set PC to fault handler address
+        pcState.set(tc->readMiscReg(tvec) >> 2);
     } else {
-        invoke_se(tc, inst);
-        PCState pcState = tc->pcState();
+        invokeSE(tc, inst);
         advancePC(pcState, inst);
-        tc->pcState(pcState);
     }
+    tc->pcState(pcState);
 }
 
 void Reset::invoke(ThreadContext *tc, const StaticInstPtr &inst)
@@ -73,21 +133,21 @@ void Reset::invoke(ThreadContext *tc, const StaticInstPtr &inst)
 }
 
 void
-UnknownInstFault::invoke_se(ThreadContext *tc, const StaticInstPtr &inst)
+UnknownInstFault::invokeSE(ThreadContext *tc, const StaticInstPtr &inst)
 {
     panic("Unknown instruction 0x%08x at pc 0x%016llx", inst->machInst,
         tc->pcState().pc());
 }
 
 void
-IllegalInstFault::invoke_se(ThreadContext *tc, const StaticInstPtr &inst)
+IllegalInstFault::invokeSE(ThreadContext *tc, const StaticInstPtr &inst)
 {
     panic("Illegal instruction 0x%08x at pc 0x%016llx: %s", inst->machInst,
         tc->pcState().pc(), reason.c_str());
 }
 
 void
-UnimplementedFault::invoke_se(ThreadContext *tc,
+UnimplementedFault::invokeSE(ThreadContext *tc,
         const StaticInstPtr &inst)
 {
     panic("Unimplemented instruction %s at pc 0x%016llx", instName,
@@ -95,21 +155,23 @@ UnimplementedFault::invoke_se(ThreadContext *tc,
 }
 
 void
-IllegalFrmFault::invoke_se(ThreadContext *tc, const StaticInstPtr &inst)
+IllegalFrmFault::invokeSE(ThreadContext *tc, const StaticInstPtr &inst)
 {
     panic("Illegal floating-point rounding mode 0x%x at pc 0x%016llx.",
             frm, tc->pcState().pc());
 }
 
 void
-BreakpointFault::invoke_se(ThreadContext *tc, const StaticInstPtr &inst)
+BreakpointFault::invokeSE(ThreadContext *tc, const StaticInstPtr &inst)
 {
     schedRelBreak(0);
 }
 
 void
-SyscallFault::invoke_se(ThreadContext *tc, const StaticInstPtr &inst)
+SyscallFault::invokeSE(ThreadContext *tc, const StaticInstPtr &inst)
 {
     Fault *fault = NoFault;
     tc->syscall(tc->readIntReg(SyscallNumReg), fault);
 }
+
+} // namespace RiscvISA
\ No newline at end of file