diff --git a/pipelined/src/hazard/hazard.sv b/pipelined/src/hazard/hazard.sv
index 368d4e52d..70e8e5f51 100644
--- a/pipelined/src/hazard/hazard.sv
+++ b/pipelined/src/hazard/hazard.sv
@@ -38,7 +38,7 @@ module hazard(
 (* mark_debug = "true" *)              input logic  FPUStallD, FStallD,
 (* mark_debug = "true" *)	      input logic  DivBusyE,FDivBusyE,
 (* mark_debug = "true" *)	      input logic  EcallFaultM, BreakpointFaultM,
-(* mark_debug = "true" *)        input logic  InvalidateICacheM,
+(* mark_debug = "true" *)        input logic  InvalidateICacheM, wfiM,
   // Stall & flush outputs
 (* mark_debug = "true" *)	      output logic StallF, StallD, StallE, StallM, StallW,
 (* mark_debug = "true" *)	      output logic FlushF, FlushD, FlushE, FlushM, FlushW
@@ -63,7 +63,7 @@ module hazard(
   assign StallFCause = CSRWritePendingDEM & ~(TrapM | RetM | BPPredWrongE);
   assign StallDCause = (LoadStallD | StoreStallD | MDUStallD | CSRRdStallD | FPUStallD | FStallD) & ~(TrapM | RetM | BPPredWrongE);    // stall in decode if instruction is a load/mul/csr dependent on previous
   assign StallECause = (DivBusyE | FDivBusyE) & ~(TrapM);
-  assign StallMCause = 0; 
+  assign StallMCause = wfiM & ~TrapM; // 0; // *** dh for wfi
   assign StallWCause = LSUStallM | IFUStallF;
 
   assign StallF = StallFCause | StallD;
diff --git a/pipelined/src/ifu/ifu.sv b/pipelined/src/ifu/ifu.sv
index f7b23ccf1..e8460def9 100644
--- a/pipelined/src/ifu/ifu.sv
+++ b/pipelined/src/ifu/ifu.sv
@@ -114,8 +114,6 @@ module ifu (
 (* mark_debug = "true" *)  logic [31:0] 				   PostSpillInstrRawF;
   // branch predictor signal
   logic [`XLEN-1:0]            PCNext1F, PCNext2F, PCNext0F;
-  logic [31:0] InstrNextF;
-  logic         wfiD;
 
   assign PCFExt = {2'b00, PCFSpill};
 
@@ -135,13 +133,6 @@ module ifu (
     assign {SelNextSpillF, CompressedF} = 0;
   end
 
-  /////////////////////////////////////////////////////////////////////////////////////////////
-  // WFI
-  /////////////////////////////////////////////////////////////////////////////////////////////
-
-  assign wfiD = (InstrD[6:0] == 7'b1110011 && InstrD[31:20] == 12'b000100000101); // WFI in decode stage
-  assign InstrNextF = wfiD ? InstrD : PostSpillInstrRawF; // on WFI, keep replaying WFI
-
   ////////////////////////////////////////////////////////////////////////////////////////////////
   // Memory management
   ////////////////////////////////////////////////////////////////////////////////////////////////
@@ -247,7 +238,7 @@ module ifu (
   assign IFUStallF = IFUCacheBusStallF | SelNextSpillF;
   assign CPUBusy = StallF & ~SelNextSpillF;
   
-  flopenl #(32) AlignedInstrRawDFlop(clk, reset, ~StallD, FlushD ? nop : InstrNextF /*PostSpillInstrRawF*/, nop, InstrRawD);
+  flopenl #(32) AlignedInstrRawDFlop(clk, reset, ~StallD, FlushD ? nop : PostSpillInstrRawF, nop, InstrRawD);
 
   ////////////////////////////////////////////////////////////////////////////////////////////////
   // PCNextF logic
diff --git a/pipelined/src/privileged/privileged.sv b/pipelined/src/privileged/privileged.sv
index b0da7bc4e..4e794806a 100644
--- a/pipelined/src/privileged/privileged.sv
+++ b/pipelined/src/privileged/privileged.sv
@@ -78,8 +78,7 @@ module privileged (
   output var logic [7:0]   PMPCFG_ARRAY_REGW[`PMP_ENTRIES-1:0],
   output var logic [`XLEN-1:0] PMPADDR_ARRAY_REGW [`PMP_ENTRIES-1:0], 
   output logic [2:0]       FRM_REGW,
-  output logic             BreakpointFaultM, EcallFaultM
-
+  output logic             BreakpointFaultM, EcallFaultM, wfiM
 );
 
   logic [1:0] NextPrivilegeModeM;
@@ -89,7 +88,7 @@ module privileged (
   logic [`XLEN-1:0] MEDELEG_REGW;
   logic [11:0]      MIDELEG_REGW;
 
-  logic sretM, mretM, ecallM, ebreakM, wfiM, sfencevmaM;
+  logic sretM, mretM, ecallM, ebreakM, sfencevmaM;
   logic IllegalCSRAccessM;
   logic IllegalIEUInstrFaultE, IllegalIEUInstrFaultM;
   logic IllegalFPUInstrM;
diff --git a/pipelined/src/wally/wallypipelinedcore.sv b/pipelined/src/wally/wallypipelinedcore.sv
index a136608ef..0aa6220c5 100644
--- a/pipelined/src/wally/wallypipelinedcore.sv
+++ b/pipelined/src/wally/wallypipelinedcore.sv
@@ -112,6 +112,7 @@ module wallypipelinedcore (
   logic [1:0]             PrivilegeModeW;
   logic [`XLEN-1:0]     PTE;
   logic [1:0]             PageType;
+  logic              wfiM;
 
   // PMA checker signals
   var logic [`XLEN-1:0] PMPADDR_ARRAY_REGW [`PMP_ENTRIES-1:0];
@@ -305,7 +306,7 @@ module wallypipelinedcore (
      .FPUStallD, .FStallD,
     .DivBusyE, .FDivBusyE,
     .EcallFaultM, .BreakpointFaultM,
-     .InvalidateICacheM,
+     .InvalidateICacheM, .wfiM,
      // Stall & flush outputs
     .StallF, .StallD, .StallE, .StallM, .StallW,
     .FlushF, .FlushD, .FlushE, .FlushM, .FlushW
@@ -340,7 +341,7 @@ 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
+         .FRM_REGW,.BreakpointFaultM, .EcallFaultM, .wfiM
       );
    end else begin
       assign CSRReadValW = 0;
diff --git a/pipelined/testbench/tests.vh b/pipelined/testbench/tests.vh
index 46b5d068a..ebf91c86c 100644
--- a/pipelined/testbench/tests.vh
+++ b/pipelined/testbench/tests.vh
@@ -1453,6 +1453,7 @@ string imperas32f[] = '{
 
  string wally64priv[] = '{
     `WALLYTEST,
+    "rv64i_m/privilege/WALLY-status-tw-01", "0050a0",
     "rv64i_m/privilege/WALLY-csr-permission-s-01", "0060a0",
     "rv64i_m/privilege/WALLY-csr-permission-u-01", "0060a0",
     "rv64i_m/privilege/WALLY-minfo-01", "0050a0",
@@ -1471,7 +1472,7 @@ string imperas32f[] = '{
     "rv64i_m/privilege/WALLY-status-mie-01", "0050a0",
     "rv64i_m/privilege/WALLY-status-sie-01", "0050a0",
     "rv64i_m/privilege/WALLY-trap-sret-01", "0050a0",
-    // "rv64i_m/privilege/WALLY-status-tw-01", "0050a0",
+    "rv64i_m/privilege/WALLY-status-tw-01", "0050a0",
     "rv64i_m/privilege/WALLY-wfi-01", "0050a0"
  };