From 6e16922aaeb8bcc354b035096bc5b4bc13fe876a Mon Sep 17 00:00:00 2001
From: David Harris <david_harris@hmc.edu>
Date: Thu, 14 Apr 2022 17:05:22 +0000
Subject: [PATCH] WFI should set EPC to PC+4

---
 pipelined/src/privileged/csr.sv        | 4 ++--
 pipelined/src/privileged/csrsr.sv      | 2 +-
 pipelined/src/privileged/privileged.sv | 2 +-
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/pipelined/src/privileged/csr.sv b/pipelined/src/privileged/csr.sv
index 2628726d..cd363967 100644
--- a/pipelined/src/privileged/csr.sv
+++ b/pipelined/src/privileged/csr.sv
@@ -41,7 +41,7 @@ module csr #(parameter
   input  logic             StallE, StallM, StallW,
   input  logic [31:0]      InstrM, 
   input  logic [`XLEN-1:0] PCM, SrcAM,
-  input  logic             CSRReadM, CSRWriteM, TrapM, MTrapM, STrapM, UTrapM, mretM, sretM, 
+  input  logic             CSRReadM, CSRWriteM, TrapM, MTrapM, STrapM, UTrapM, mretM, sretM, wfiM,
   input  logic             TimerIntM, MExtIntM, SExtIntM, SwIntM,
   input  logic [63:0]      MTIME_CLINT, 
   input  logic             InstrValidM, FRegWriteM, LoadStallD,
@@ -123,7 +123,7 @@ module csr #(parameter
 
   // write CSRs
   assign CSRAdrM = InstrM[31:20];
-  assign UnalignedNextEPCM = TrapM ? PCM : CSRWriteValM;
+  assign UnalignedNextEPCM = TrapM ? (wfiM ? PCM+4 : PCM) : CSRWriteValM;
   assign NextEPCM = `C_SUPPORTED ? {UnalignedNextEPCM[`XLEN-1:1], 1'b0} : {UnalignedNextEPCM[`XLEN-1:2], 2'b00}; // 3.1.15 alignment
   assign NextCauseM = TrapM ? CauseM : CSRWriteValM;
   assign NextMtvalM = TrapM ? NextFaultMtvalM : CSRWriteValM;
diff --git a/pipelined/src/privileged/csrsr.sv b/pipelined/src/privileged/csrsr.sv
index d7636a5e..d08851bf 100644
--- a/pipelined/src/privileged/csrsr.sv
+++ b/pipelined/src/privileged/csrsr.sv
@@ -140,7 +140,7 @@ module csrsr (
         STATUS_MIE <= #1 STATUS_MPIE;
         STATUS_MPIE <= #1 1;
         STATUS_MPP <= #1 `U_SUPPORTED ? `U_MODE : `M_MODE; // per spec, not sure why
-        STATUS_MPRV_INT <= #1 0; // per 20210108 draft spec
+        STATUS_MPRV_INT <= #1 (STATUS_MPP == `M_MODE & STATUS_MPRV_INT); //0; // per 20210108 draft spec
       end else if (sretM) begin
         STATUS_SIE <= #1 STATUS_SPIE;
         STATUS_SPIE <= #1 `S_SUPPORTED;
diff --git a/pipelined/src/privileged/privileged.sv b/pipelined/src/privileged/privileged.sv
index 0336a3b0..003a84b7 100644
--- a/pipelined/src/privileged/privileged.sv
+++ b/pipelined/src/privileged/privileged.sv
@@ -167,7 +167,7 @@ module privileged (
           .FlushE, .FlushM, .FlushW,
           .StallE, .StallM, .StallW,
           .InstrM, .PCM, .SrcAM,
-          .CSRReadM, .CSRWriteM, .TrapM, .MTrapM, .STrapM, .UTrapM, .mretM, .sretM, 
+          .CSRReadM, .CSRWriteM, .TrapM, .MTrapM, .STrapM, .UTrapM, .mretM, .sretM, .wfiM,
           .TimerIntM, .MExtIntM, .SExtIntM, .SwIntM,
           .MTIME_CLINT, 
           .InstrValidM, .FRegWriteM, .LoadStallD,