Fixed WFI to commit when an interrupt occurs

src/hazard/hazard.sv

@@ -36,7 +36,7 @@ module hazard (
   input logic  FCvtIntStallD, FPUStallD,
   input logic  DivBusyE, FDivBusyE,
   input logic  EcallFaultM, BreakpointFaultM,
-  input logic  WFIStallM,
+  input logic  wfiM, IntPendingM,
   // Stall & flush outputs
   output logic StallF, StallD, StallE, StallM, StallW,
   output logic FlushD, FlushE, FlushM, FlushW
@@ -45,6 +45,12 @@ module hazard (
   logic                                       StallFCause, StallDCause, StallECause, StallMCause, StallWCause;
   logic                                       LatestUnstalledD, LatestUnstalledE, LatestUnstalledM, LatestUnstalledW;
   logic                                       FlushDCause, FlushECause, FlushMCause, FlushWCause;
+
+  logic WFIStallM, WFIInterruptedM;
+
+  // WFI logic
+  assign WFIStallM = wfiM & ~IntPendingM;         // WFI waiting for an interrupt or timeout
+  assign WFIInterruptedM = wfiM & IntPendingM;    // WFI detects a pending interrupt.  Retire WFI; trap if interrupt is enabled.
   
   // stalls and flushes
   // loads: stall for one cycle if the subsequent instruction depends on the load
@@ -68,7 +74,7 @@ module hazard (
   assign FlushDCause = TrapM | RetM | CSRWriteFenceM | BPWrongE;
   assign FlushECause = TrapM | RetM | CSRWriteFenceM |(BPWrongE & ~(DivBusyE | FDivBusyE));
   assign FlushMCause = TrapM | RetM | CSRWriteFenceM;
-  assign FlushWCause = TrapM;
+  assign FlushWCause = TrapM & ~WFIInterruptedM;
 
   // Stall causes
   //  Most data depenency stalls are identified in the decode stage

src/privileged/privileged.sv

@@ -93,7 +93,7 @@ module privileged (
   output logic             BigEndianM,                                     // Use big endian in current privilege mode
   // Fault outputs                                                         
   output logic             BreakpointFaultM, EcallFaultM,                  // breakpoint and Ecall traps should retire
-  output logic             WFIStallM                                       // Stall in Memory stage for WFI until interrupt or timeout
+  output logic             wfiM, IntPendingM                               // Stall in Memory stage for WFI until interrupt pending or timeout
 );                                                                         
                                                                            
   logic [3:0]              CauseM;                                         // trap cause
@@ -110,8 +110,6 @@ module privileged (
   logic [11:0]             MIP_REGW, MIE_REGW;                             // interrupt pending and enable bits
   logic [1:0]              NextPrivilegeModeM;                             // next privilege mode based on trap or return
   logic                    DelegateM;                                      // trap should be delegated
-  logic                    wfiM;                                           // wait for interrupt instruction
-  logic                    IntPendingM;                                    // interrupt is pending, even if not enabled.  ends wfi
   logic                    InterruptM;                                     // interrupt occuring
   logic                    ExceptionM;                                     // Memory stage instruction caused a fault
  
@@ -156,7 +154,7 @@ module privileged (
     .mretM, .sretM, .PrivilegeModeW, 
     .MIP_REGW, .MIE_REGW, .MIDELEG_REGW, .MEDELEG_REGW, .STATUS_MIE, .STATUS_SIE,
     .InstrValidM, .CommittedM, .CommittedF,
-    .TrapM, .RetM, .wfiM, .InterruptM, .ExceptionM, .IntPendingM, .DelegateM, .WFIStallM, .CauseM);
+    .TrapM, .RetM, .wfiM, .InterruptM, .ExceptionM, .IntPendingM, .DelegateM, .CauseM);
 endmodule
 
 

src/privileged/trap.sv

@@ -48,7 +48,6 @@ module trap (
   output logic                 ExceptionM,                                      // exception is occurring
   output logic                 IntPendingM,                                     // Interrupt is pending, might occur if enabled
   output logic                 DelegateM,                                       // Delegate trap to supervisor handler
-  output logic                 WFIStallM,                                       // Stall due to WFI instruction
   output logic [3:0]           CauseM                                           // trap cause
 );
 
@@ -74,7 +73,6 @@ module trap (
   assign InterruptM = (|ValidIntsM) & InstrValidM; // suppress interrupt if the memory system has partially processed a request.
   assign DelegateM = `S_SUPPORTED & (InterruptM ? MIDELEG_REGW[CauseM] : MEDELEG_REGW[CauseM]) & 
                      (PrivilegeModeW == `U_MODE | PrivilegeModeW == `S_MODE);
-  assign WFIStallM = wfiM & ~IntPendingM;
 
   ///////////////////////////////////////////
   // Trigger Traps and RET

src/wally/wallypipelinedcore.sv

@@ -106,7 +106,7 @@ module wallypipelinedcore (
   logic [1:0]                    PrivilegeModeW;
   logic [`XLEN-1:0]              PTE;
   logic [1:0]                    PageType;
-  logic                          sfencevmaM, WFIStallM;
+  logic                          sfencevmaM;
   logic                          SelHPTW;
 
   // PMA checker signals
@@ -162,7 +162,8 @@ module wallypipelinedcore (
   logic                          CommittedF;
   logic                          BranchD, BranchE, JumpD, JumpE;
   logic                          DCacheStallM, ICacheStallF;
-  
+  logic                          wfiM, IntPendingM;
+
   // instruction fetch unit: PC, branch prediction, instruction cache
   ifu ifu(.clk, .reset,
     .StallF, .StallD, .StallE, .StallM, .StallW, .FlushD, .FlushE, .FlushM, .FlushW,
@@ -265,7 +266,7 @@ module wallypipelinedcore (
     .FCvtIntStallD, .FPUStallD,
     .DivBusyE, .FDivBusyE,
     .EcallFaultM, .BreakpointFaultM,
-    .WFIStallM,
+    .wfiM, .IntPendingM,
     // Stall & flush outputs
     .StallF, .StallD, .StallE, .StallM, .StallW,
     .FlushD, .FlushE, .FlushM, .FlushW);    
@@ -292,13 +293,14 @@ module wallypipelinedcore (
       .PrivilegeModeW, .SATP_REGW,
       .STATUS_MXR, .STATUS_SUM, .STATUS_MPRV, .STATUS_MPP, .STATUS_FS,
       .PMPCFG_ARRAY_REGW, .PMPADDR_ARRAY_REGW, 
-      .FRM_REGW,.BreakpointFaultM, .EcallFaultM, .WFIStallM, .BigEndianM);
+      .FRM_REGW,.BreakpointFaultM, .EcallFaultM, .wfiM, .IntPendingM, .BigEndianM);
   end else begin
     assign CSRReadValW = 0;
     assign UnalignedPCNextF = PC2NextF;
     assign RetM = 0;
     assign TrapM = 0;
-    assign WFIStallM = 0;
+    assign wfiM = 0;
+    assign IntPendingM = 0;
     assign sfencevmaM = 0;
     assign BigEndianM = 0;
   end