From 72c1cc33f595cbe6d189a7250f81bc8b6572213d Mon Sep 17 00:00:00 2001
From: David Harris <david_harris@hmc.edu>
Date: Wed, 15 Sep 2021 13:14:00 -0400
Subject: [PATCH] Added Zfencei support in instruction decoder and
 configurations.  Also added riscv-arch-test 32-bit tests to regression.

---
 .../config/buildroot/wally-config.vh          |  5 +-
 .../config/busybear/wally-config.vh           |  5 +-
 .../config/coremark/wally-config.vh           |  5 +-
 .../config/coremark_bare/wally-config.vh      |  5 +-
 wally-pipelined/config/rv32ic/wally-config.vh |  5 +-
 .../config/rv32icfd/wally-config.vh           |  5 +-
 wally-pipelined/config/rv64BP/wally-config.vh |  5 +-
 wally-pipelined/config/rv64ic/wally-config.vh |  5 +-
 .../config/rv64icfd/wally-config.vh           |  5 +-
 .../config/rv64imc/wally-config.vh            |  5 +-
 .../regression/regression-wally.py            |  5 +
 wally-pipelined/src/hazard/hazard.sv          |  9 +-
 wally-pipelined/src/ieu/controller.sv         | 97 +++++++++++--------
 wally-pipelined/src/ieu/ieu.sv                |  1 +
 wally-pipelined/src/ifu/ifu.sv                |  1 +
 wally-pipelined/src/lsu/lsu.sv                |  1 +
 wally-pipelined/src/privileged/csr.sv         |  2 +-
 wally-pipelined/src/privileged/csrc.sv        |  4 +-
 .../src/wally/wallypipelinedhart.sv           |  3 +-
 19 files changed, 104 insertions(+), 69 deletions(-)

diff --git a/wally-pipelined/config/buildroot/wally-config.vh b/wally-pipelined/config/buildroot/wally-config.vh
index f6373e1a4..1b6e030ff 100644
--- a/wally-pipelined/config/buildroot/wally-config.vh
+++ b/wally-pipelined/config/buildroot/wally-config.vh
@@ -37,8 +37,9 @@
 `define XLEN 64
 
 `define MISA (32'h0014112D)
-`define ZCSR_SUPPORTED 1
-`define ZCOUNTERS_SUPPORTED 1
+`define ZICSR_SUPPORTED 1
+`define ZIFENCEI_SUPPORTED 1
+`define ZICOUNTERS_SUPPORTED 1
 `define COUNTERS 32
 
 // Microarchitectural Features
diff --git a/wally-pipelined/config/busybear/wally-config.vh b/wally-pipelined/config/busybear/wally-config.vh
index 25394589c..86385bd88 100644
--- a/wally-pipelined/config/busybear/wally-config.vh
+++ b/wally-pipelined/config/busybear/wally-config.vh
@@ -37,8 +37,9 @@
 `define XLEN 64
 
 `define MISA (32'h0014112D)
-`define ZCSR_SUPPORTED 1
-`define ZCOUNTERS_SUPPORTED 1
+`define ZICSR_SUPPORTED 1
+`define ZIFENCEI_SUPPORTED 1
+`define ZICOUNTERS_SUPPORTED 1
 `define COUNTERS 32
 
 // Microarchitectural Features
diff --git a/wally-pipelined/config/coremark/wally-config.vh b/wally-pipelined/config/coremark/wally-config.vh
index 1706d345f..32006c690 100644
--- a/wally-pipelined/config/coremark/wally-config.vh
+++ b/wally-pipelined/config/coremark/wally-config.vh
@@ -36,9 +36,10 @@
 
 //`define MISA (32'h00000104)
 `define MISA (32'h00000104 | 1<<5 | 1<<18 | 1 << 20 | 1 << 12)
-`define ZCSR_SUPPORTED 1
+`define ZICSR_SUPPORTED 1
+`define ZIFENCEI_SUPPORTED 1
 `define COUNTERS 32
-`define ZCOUNTERS_SUPPORTED 1
+`define ZICOUNTERS_SUPPORTED 1
 
 // Microarchitectural Features
 `define UARCH_PIPELINED 1
diff --git a/wally-pipelined/config/coremark_bare/wally-config.vh b/wally-pipelined/config/coremark_bare/wally-config.vh
index e877db395..8f79212b5 100644
--- a/wally-pipelined/config/coremark_bare/wally-config.vh
+++ b/wally-pipelined/config/coremark_bare/wally-config.vh
@@ -37,9 +37,10 @@
 //`define MISA (32'h00000104)
 //`define MISA (32'h00001104 | 1<<5 | 1<<18 | 1 << 20 | 1 << 12 | 1 << 0)
 `define MISA (32'h00000104 | 0 << 5 | 0 << 3 | 1 << 18 | 1 << 20 | 1 << 12 | 1 << 0)
-`define ZCSR_SUPPORTED 1
+`define ZICSR_SUPPORTED 1
+`define ZIFENCEI_SUPPORTED 1
 `define COUNTERS 32
-`define ZCOUNTERS_SUPPORTED 1
+`define ZICOUNTERS_SUPPORTED 1
 
 // Microarchitectural Features
 `define UARCH_PIPELINED 1
diff --git a/wally-pipelined/config/rv32ic/wally-config.vh b/wally-pipelined/config/rv32ic/wally-config.vh
index 6325a2363..dfe1c61e0 100644
--- a/wally-pipelined/config/rv32ic/wally-config.vh
+++ b/wally-pipelined/config/rv32ic/wally-config.vh
@@ -35,9 +35,10 @@
 `define XLEN 32
 
 `define MISA (32'h00000104 | 1 << 20 | 1 << 18 | 1 << 12 | 1 << 0)
-`define ZCSR_SUPPORTED 1
+`define ZICSR_SUPPORTED 1
+`define ZIFENCEI_SUPPORTED 1
 `define COUNTERS 32
-`define ZCOUNTERS_SUPPORTED 1
+`define ZICOUNTERS_SUPPORTED 1
 
 // Microarchitectural Features
 `define UARCH_PIPELINED 1
diff --git a/wally-pipelined/config/rv32icfd/wally-config.vh b/wally-pipelined/config/rv32icfd/wally-config.vh
index fbfd12559..2f0bc378e 100644
--- a/wally-pipelined/config/rv32icfd/wally-config.vh
+++ b/wally-pipelined/config/rv32icfd/wally-config.vh
@@ -35,9 +35,10 @@
 `define XLEN 32
 
 `define MISA (32'h00000104 | 1 << 5 | 1 << 20 | 1 << 18 | 1 << 12)
-`define ZCSR_SUPPORTED 1
+`define ZICSR_SUPPORTED 1
+`define ZIFENCEI_SUPPORTED 1
 `define COUNTERS 32
-`define ZCOUNTERS_SUPPORTED 1
+`define ZICOUNTERS_SUPPORTED 1
 
 // Microarchitectural Features
 `define UARCH_PIPELINED 1
diff --git a/wally-pipelined/config/rv64BP/wally-config.vh b/wally-pipelined/config/rv64BP/wally-config.vh
index 0085c77bf..c189cb0fe 100644
--- a/wally-pipelined/config/rv64BP/wally-config.vh
+++ b/wally-pipelined/config/rv64BP/wally-config.vh
@@ -37,9 +37,10 @@
 
 //`define MISA (32'h00000105)
 `define MISA (32'h00000104 | 1 << 5 | 1 << 3 | 1 << 18 | 1 << 20 | 1 << 12 | 1 << 0)
-`define ZCSR_SUPPORTED 1
+`define ZICSR_SUPPORTED 1
+`define ZIFENCEI_SUPPORTED 1
 `define COUNTERS 32
-`define ZCOUNTERS_SUPPORTED 1
+`define ZICOUNTERS_SUPPORTED 1
 
 // Microarchitectural Features
 `define UARCH_PIPELINED 1
diff --git a/wally-pipelined/config/rv64ic/wally-config.vh b/wally-pipelined/config/rv64ic/wally-config.vh
index 5b3eddb60..ef935ae2c 100644
--- a/wally-pipelined/config/rv64ic/wally-config.vh
+++ b/wally-pipelined/config/rv64ic/wally-config.vh
@@ -36,9 +36,10 @@
 
 // MISA RISC-V configuration per specification
 `define MISA (32'h00000104 | 0 << 5 | 0 << 3 | 1 << 18 | 1 << 20 | 1 << 12 | 1 << 0)
-`define ZCSR_SUPPORTED 1
+`define ZICSR_SUPPORTED 1
+`define ZIFENCEI_SUPPORTED 1
 `define COUNTERS 32
-`define ZCOUNTERS_SUPPORTED 1
+`define ZICOUNTERS_SUPPORTED 1
 
 // Microarchitectural Features
 `define UARCH_PIPELINED 1
diff --git a/wally-pipelined/config/rv64icfd/wally-config.vh b/wally-pipelined/config/rv64icfd/wally-config.vh
index 83a1070f1..a91531dd0 100644
--- a/wally-pipelined/config/rv64icfd/wally-config.vh
+++ b/wally-pipelined/config/rv64icfd/wally-config.vh
@@ -37,9 +37,10 @@
 
 // MISA RISC-V configuration per specification
 `define MISA (32'h00000104 | 1 << 5 | 1 << 3 | 1 << 18 | 1 << 20 | 1 << 12 | 1 << 0)
-`define ZCSR_SUPPORTED 1
+`define ZICSR_SUPPORTED 1
+`define ZIFENCEI_SUPPORTED 1
 `define COUNTERS 32
-`define ZCOUNTERS_SUPPORTED 1
+`define ZICOUNTERS_SUPPORTED 1
 
 // Microarchitectural Features
 `define UARCH_PIPELINED 1
diff --git a/wally-pipelined/config/rv64imc/wally-config.vh b/wally-pipelined/config/rv64imc/wally-config.vh
index 1e8b24376..0a874a72d 100644
--- a/wally-pipelined/config/rv64imc/wally-config.vh
+++ b/wally-pipelined/config/rv64imc/wally-config.vh
@@ -35,9 +35,10 @@
 
 // MISA RISC-V configuration per specification
 `define MISA (32'h00000104 | 0 << 5 | 0 << 3 | 1 << 18 | 1 << 20 | 1 << 12 | 1 << 0)
-`define ZCSR_SUPPORTED 1
+`define ZICSR_SUPPORTED 1
+`define ZIFENCEI_SUPPORTED 1
 `define COUNTERS 32
-`define ZCOUNTERS_SUPPORTED 1
+`define ZICOUNTERS_SUPPORTED 1
 
 // Microarchitectural Features
 `define UARCH_PIPELINED 1
diff --git a/wally-pipelined/regression/regression-wally.py b/wally-pipelined/regression/regression-wally.py
index 4d7baf8e5..e7790f9fc 100755
--- a/wally-pipelined/regression/regression-wally.py
+++ b/wally-pipelined/regression/regression-wally.py
@@ -38,6 +38,11 @@ configs = [
         cmd="vsim > {} -c <<!\ndo wally-arch.do ../config/rv64ic rv64ic\n!",
         grepstr="All tests ran without failures"
     ),
+    TestCase(
+        name="arch32",
+        cmd="vsim > {} -c <<!\ndo wally-arch.do ../config/rv32ic rv32ic\n!",
+        grepstr="All tests ran without failures"
+    ),
     TestCase(
         name="rv32ic",
         cmd="vsim > {} -c <<!\ndo wally-pipelined-batch.do ../config/rv32ic rv32ic\n!",
diff --git a/wally-pipelined/src/hazard/hazard.sv b/wally-pipelined/src/hazard/hazard.sv
index 6b81ebbca..c5b078eda 100644
--- a/wally-pipelined/src/hazard/hazard.sv
+++ b/wally-pipelined/src/hazard/hazard.sv
@@ -35,6 +35,7 @@ module hazard(
               input logic  FPUStallD, FStallD,
 	      input logic  DivBusyE,FDivBusyE,
 	      input logic  EcallFaultM, BreakpointFaultM,
+        input logic  InvalidateICacheM,
   // Stall & flush outputs
 	      output logic StallF, StallD, StallE, StallM, StallW,
 	      output logic FlushF, FlushD, FlushE, FlushM, FlushW
@@ -68,17 +69,15 @@ module hazard(
   assign StallM = StallMCause | StallW;
   assign StallW = StallWCause;
 
-  //assign FirstUnstalledD = (~StallD & StallF & ~MulDivStallD);
-  //assign FirstUnstalledE = (~StallE & StallD & ~MulDivStallD);
   assign FirstUnstalledD = (~StallD && StallF);
   assign FirstUnstalledE = (~StallE && StallD);
   assign FirstUnstalledM = (~StallM && StallE);
   assign FirstUnstalledW = (~StallW && StallM);
   
   // Each stage flushes if the previous stage is the last one stalled (for cause) or the system has reason to flush
-  assign FlushF = BPPredWrongE;
-  assign FlushD = FirstUnstalledD | TrapM | RetM | BPPredWrongE;
-  assign FlushE = FirstUnstalledE | TrapM | RetM | BPPredWrongE;
+  assign FlushF = BPPredWrongE | InvalidateICacheM;
+  assign FlushD = FirstUnstalledD | TrapM | RetM | BPPredWrongE | InvalidateICacheM;
+  assign FlushE = FirstUnstalledE | TrapM | RetM | BPPredWrongE | InvalidateICacheM;
   assign FlushM = FirstUnstalledM | TrapM | RetM;
   // on Trap the memory stage should be flushed going into the W stage,
   // except if the instruction causing the Trap is an ecall or ebreak.
diff --git a/wally-pipelined/src/ieu/controller.sv b/wally-pipelined/src/ieu/controller.sv
index ab503483c..ed5f02404 100644
--- a/wally-pipelined/src/ieu/controller.sv
+++ b/wally-pipelined/src/ieu/controller.sv
@@ -56,6 +56,7 @@ module controller(
   output logic [1:0] AtomicM,
   output logic [2:0] Funct3M,
   output logic       RegWriteM,     // for Hazard Unit
+  output logic       InvalidateICacheM, FlushDCacheM,
   output logic       InstrValidM, InstrValidW,
   // Writeback stage control signals
   input  logic       StallW, FlushW,
@@ -71,7 +72,7 @@ module controller(
   logic [6:0] Funct7D;
   logic [4:0] Rs1D;
 
-  `define CTRLW 23
+  `define CTRLW 24
 
   // pipelined control signals
   logic 	    RegWriteE;
@@ -86,9 +87,12 @@ module controller(
   logic       CSRZeroSrcD;
   logic       CSRReadD;
   logic [1:0] AtomicD;
+  logic       FenceD;
+  logic       InvalidateICacheD, FlushDCacheD;
   logic       CSRWriteD, CSRWriteE;
   logic       InstrValidD, InstrValidE;
   logic       PrivilegedD, PrivilegedE;
+  logic       InvalidateICacheE, FlushDCacheE;
   logic [`CTRLW-1:0] ControlsD;
   logic        aluc3D;
   logic        subD, sraD, sltD, sltuD;
@@ -103,55 +107,53 @@ module controller(
   assign Rs1D = InstrD[19:15];
 
   // Main Instruction Decoder
-  // *** perhaps decoding of non-IEU instructions should also go here, and should be gated by MISA bits in a generate so
-  // they don't get generated if that mode is disabled
   generate
     always_comb
       case(OpD)
-      // RegWrite_ImmSrc_ALUSrc_MemRW_ResultSrc_Branch_ALUOp_Jump_TargetSrc_W64_CSRRead_Privileged_MulDiv_Atomic_Illegal
-        7'b0000000:   ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_00_1; // illegal instruction
-        7'b0000011:   ControlsD = `CTRLW'b1_000_01_10_001_0_00_0_0_0_0_0_0_00_0; // lw
-        7'b0000111:   ControlsD = `CTRLW'b0_000_01_10_001_0_00_0_0_0_0_0_0_00_0; // flw
-        7'b0001111:   ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_00_0; // fence = nop
-        7'b0010011:   ControlsD = `CTRLW'b1_000_01_00_000_0_10_0_0_0_0_0_0_00_0; // I-type ALU
-        7'b0010111:   ControlsD = `CTRLW'b1_100_11_00_000_0_00_0_0_0_0_0_0_00_0; // auipc
+      // RegWrite_ImmSrc_ALUSrc_MemRW_ResultSrc_Branch_ALUOp_Jump_TargetSrc_W64_CSRRead_Privileged_Fence_MulDiv_Atomic_Illegal
+        7'b0000000:   ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_0_00_1; // illegal instruction
+        7'b0000011:   ControlsD = `CTRLW'b1_000_01_10_001_0_00_0_0_0_0_0_0_0_00_0; // lw
+        7'b0000111:   ControlsD = `CTRLW'b0_000_01_10_001_0_00_0_0_0_0_0_0_0_00_0; // flw
+        7'b0001111:   ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_1_0_00_0; // fence
+        7'b0010011:   ControlsD = `CTRLW'b1_000_01_00_000_0_10_0_0_0_0_0_0_0_00_0; // I-type ALU
+        7'b0010111:   ControlsD = `CTRLW'b1_100_11_00_000_0_00_0_0_0_0_0_0_0_00_0; // auipc
         7'b0011011: if (`XLEN == 64)
-                      ControlsD = `CTRLW'b1_000_01_00_000_0_10_0_0_1_0_0_0_00_0; // IW-type ALU for RV64i
+                      ControlsD = `CTRLW'b1_000_01_00_000_0_10_0_0_1_0_0_0_0_00_0; // IW-type ALU for RV64i
                     else
-                      ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_00_1; // non-implemented instruction
-        7'b0100011:   ControlsD = `CTRLW'b0_001_01_01_000_0_00_0_0_0_0_0_0_00_0; // sw
-        7'b0100111:   ControlsD = `CTRLW'b0_001_01_01_000_0_00_0_0_0_0_0_0_00_0; // fsw
+                      ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_0_00_1; // non-implemented instruction
+        7'b0100011:   ControlsD = `CTRLW'b0_001_01_01_000_0_00_0_0_0_0_0_0_0_00_0; // sw
+        7'b0100111:   ControlsD = `CTRLW'b0_001_01_01_000_0_00_0_0_0_0_0_0_0_00_0; // fsw
         7'b0101111: if (`A_SUPPORTED) begin
                       if (InstrD[31:27] == 5'b00010)
-                        ControlsD = `CTRLW'b1_000_00_10_001_0_00_0_0_0_0_0_0_01_0; // lr
+                        ControlsD = `CTRLW'b1_000_00_10_001_0_00_0_0_0_0_0_0_0_01_0; // lr
                       else if (InstrD[31:27] == 5'b00011)
-                        ControlsD = `CTRLW'b1_101_01_01_100_0_00_0_0_0_0_0_0_01_0; // sc
+                        ControlsD = `CTRLW'b1_101_01_01_100_0_00_0_0_0_0_0_0_0_01_0; // sc
                       else 
-                        ControlsD = `CTRLW'b1_101_01_11_001_0_00_0_0_0_0_0_0_10_0;; // amo
+                        ControlsD = `CTRLW'b1_101_01_11_001_0_00_0_0_0_0_0_0_0_10_0;; // amo
                     end else
-                      ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_00_1; // non-implemented instruction
+                      ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_0_00_1; // non-implemented instruction
         7'b0110011: if (Funct7D == 7'b0000000 || Funct7D == 7'b0100000)
-                      ControlsD = `CTRLW'b1_000_00_00_000_0_10_0_0_0_0_0_0_00_0; // R-type 
+                      ControlsD = `CTRLW'b1_000_00_00_000_0_10_0_0_0_0_0_0_0_00_0; // R-type 
                     else if (Funct7D == 7'b0000001 && `M_SUPPORTED)
-                      ControlsD = `CTRLW'b1_000_00_00_011_0_00_0_0_0_0_0_1_00_0; // Multiply/Divide
+                      ControlsD = `CTRLW'b1_000_00_00_011_0_00_0_0_0_0_0_0_1_00_0; // Multiply/Divide
                     else
-                      ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_00_1; // non-implemented instruction
-        7'b0110111:   ControlsD = `CTRLW'b1_100_01_00_000_0_11_0_0_0_0_0_0_00_0; // lui
+                      ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_0_00_1; // non-implemented instruction
+        7'b0110111:   ControlsD = `CTRLW'b1_100_01_00_000_0_11_0_0_0_0_0_0_0_00_0; // lui
         7'b0111011: if ((Funct7D == 7'b0000000 || Funct7D == 7'b0100000) && `XLEN == 64)
-                      ControlsD = `CTRLW'b1_000_00_00_000_0_10_0_0_1_0_0_0_00_0; // R-type W instructions for RV64i
+                      ControlsD = `CTRLW'b1_000_00_00_000_0_10_0_0_1_0_0_0_0_00_0; // R-type W instructions for RV64i
                     else if (Funct7D == 7'b0000001 && `M_SUPPORTED && `XLEN == 64)
-                      ControlsD = `CTRLW'b1_000_00_00_011_0_00_0_0_1_0_0_1_00_0; // W-type Multiply/Divide
+                      ControlsD = `CTRLW'b1_000_00_00_011_0_00_0_0_1_0_0_0_1_00_0; // W-type Multiply/Divide
                     else
-                      ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_00_1; // non-implemented instruction
-        //7'b1010011:   ControlsD = `CTRLW'b0_000_00_00_101_0_00_0_0_0_0_0_0_00_1; // FP
-        7'b1100011:   ControlsD = `CTRLW'b0_010_00_00_000_1_01_0_0_0_0_0_0_00_0; // beq
-        7'b1100111:   ControlsD = `CTRLW'b1_000_00_00_000_0_00_1_1_0_0_0_0_00_0; // jalr
-        7'b1101111:   ControlsD = `CTRLW'b1_011_00_00_000_0_00_1_0_0_0_0_0_00_0; // jal
+                      ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_0_00_1; // non-implemented instruction
+        //7'b1010011:   ControlsD = `CTRLW'b0_000_00_00_101_0_00_0_0_0_0_0_0_0_00_1; // FP
+        7'b1100011:   ControlsD = `CTRLW'b0_010_00_00_000_1_01_0_0_0_0_0_0_0_00_0; // beq
+        7'b1100111:   ControlsD = `CTRLW'b1_000_00_00_000_0_00_1_1_0_0_0_0_0_00_0; // jalr
+        7'b1101111:   ControlsD = `CTRLW'b1_011_00_00_000_0_00_1_0_0_0_0_0_0_00_0; // jal
         7'b1110011: if (Funct3D == 3'b000)
-                      ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_1_0_00_0; // privileged; decoded further in priveleged modules
+                      ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_1_0_0_00_0; // privileged; decoded further in priveleged modules
                     else
-                      ControlsD = `CTRLW'b1_000_00_00_010_0_00_0_0_0_1_0_0_00_0; // csrs
-        default:      ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_00_1; // non-implemented instruction
+                      ControlsD = `CTRLW'b1_000_00_00_010_0_00_0_0_0_1_0_0_0_00_0; // csrs
+        default:      ControlsD = `CTRLW'b0_000_00_00_000_0_00_0_0_0_0_0_0_0_00_1; // non-implemented instruction
       endcase
   endgenerate
 
@@ -160,7 +162,7 @@ module controller(
   assign IllegalBaseInstrFaultD = ControlsD[0];
   assign {RegWriteD, ImmSrcD, ALUSrcAD, ALUSrcBD, MemRWD,
           ResultSrcD, BranchD, ALUOpD, JumpD, TargetSrcD, W64D, CSRReadD, 
-          PrivilegedD, MulDivD, AtomicD, unused} = IllegalIEUInstrFaultD ? `CTRLW'b0 : ControlsD;
+          PrivilegedD, FenceD, MulDivD, AtomicD, unused} = IllegalIEUInstrFaultD ? `CTRLW'b0 : ControlsD;
           // *** move Privileged, CSRwrite??  Or move controller out of IEU into datapath and handle all instructions
 
   assign CSRZeroSrcD = InstrD[14] ? (InstrD[19:15] == 0) : (Rs1D == 0); // Is a CSR instruction using zero as the source?
@@ -181,14 +183,29 @@ module controller(
       2'b11:                ALUControlD = 5'b01110;  // pass B through for lui
       default:              ALUControlD = {W64D, aluc3D, Funct3D}; // R-type instructions
     endcase
-  
+
+  // Fences
+  // Ordinary fence is presently a nop
+  // FENCE.I flushes the D$ and invalidates the I$ if Zifencei is supported and I$ is implemented
+  generate
+    if (`ZIFENCEI_SUPPORTED & `MEM_ICACHE) begin
+      logic FenceID;
+      assign FenceID = FenceD & (Funct3D == 3'b001); // is it a FENCE.I instruction?
+      assign InvalidateICacheD = FenceID;
+      assign FlushDCacheD = FenceID;
+    end else begin
+      assign InvalidateICacheD = 0;
+      assign FlushDCacheD = 0;
+    end
+  endgenerate
+ 
   // Decocde stage pipeline control register
   flopenrc #(1)  controlregD(clk, reset, FlushD, ~StallD, 1'b1, InstrValidD);
 
   // Execute stage pipeline control register and logic
-  flopenrc #(27) controlregE(clk, reset, FlushE, ~StallE,
-                           {RegWriteD, ResultSrcD, MemRWD, JumpD, BranchD, ALUControlD, ALUSrcAD, ALUSrcBD, TargetSrcD, CSRReadD, CSRWriteD, PrivilegedD, Funct3D, W64D, MulDivD, AtomicD, InstrValidD},
-                           {RegWriteE, ResultSrcE, MemRWE, JumpE, BranchE, ALUControlE, ALUSrcAE, ALUSrcBE, TargetSrcE, CSRReadE, CSRWriteE, PrivilegedE, Funct3E, W64E, MulDivE, AtomicE, InstrValidE});
+  flopenrc #(29) controlregE(clk, reset, FlushE, ~StallE,
+                           {RegWriteD, ResultSrcD, MemRWD, JumpD, BranchD, ALUControlD, ALUSrcAD, ALUSrcBD, TargetSrcD, CSRReadD, CSRWriteD, PrivilegedD, Funct3D, W64D, MulDivD, AtomicD, InvalidateICacheD, FlushDCacheD, InstrValidD},
+                           {RegWriteE, ResultSrcE, MemRWE, JumpE, BranchE, ALUControlE, ALUSrcAE, ALUSrcBE, TargetSrcE, CSRReadE, CSRWriteE, PrivilegedE, Funct3E, W64E, MulDivE, AtomicE, InvalidateICacheE, FlushDCacheE, InstrValidE});
 
   // Branch Logic
   assign {zeroE, ltE, ltuE} = FlagsE;
@@ -210,9 +227,9 @@ module controller(
   assign SCE = (ResultSrcE == 3'b100);
   
   // Memory stage pipeline control register
-  flopenrc #(15) controlregM(clk, reset, FlushM, ~StallM,
-                         {RegWriteE, ResultSrcE, MemRWE, CSRReadE, CSRWriteE, PrivilegedE, Funct3E, AtomicE, InstrValidE},
-                         {RegWriteM, ResultSrcM, MemRWM, CSRReadM, CSRWriteM, PrivilegedM, Funct3M, AtomicM, InstrValidM});
+  flopenrc #(17) controlregM(clk, reset, FlushM, ~StallM,
+                         {RegWriteE, ResultSrcE, MemRWE, CSRReadE, CSRWriteE, PrivilegedE, Funct3E, AtomicE, InvalidateICacheE, FlushDCacheE, InstrValidE},
+                         {RegWriteM, ResultSrcM, MemRWM, CSRReadM, CSRWriteM, PrivilegedM, Funct3M, AtomicM, InvalidateICacheM, FlushDCacheM, InstrValidM});
   
   // Writeback stage pipeline control register
   flopenrc #(5) controlregW(clk, reset, FlushW, ~StallW,
diff --git a/wally-pipelined/src/ieu/ieu.sv b/wally-pipelined/src/ieu/ieu.sv
index b23f61b64..f2984d7ff 100644
--- a/wally-pipelined/src/ieu/ieu.sv
+++ b/wally-pipelined/src/ieu/ieu.sv
@@ -59,6 +59,7 @@ module ieu (
   output logic [4:0]    RdM,
   input logic 		   DataAccessFaultM,
   input logic [`XLEN-1:0]  FIntResM, 
+  output logic       InvalidateICacheM, FlushDCacheM,
 
   // Writeback stage
   input logic [`XLEN-1:0]  CSRReadValW, ReadDataM, MulDivResultW,
diff --git a/wally-pipelined/src/ifu/ifu.sv b/wally-pipelined/src/ifu/ifu.sv
index 7215c382f..b8d35d370 100644
--- a/wally-pipelined/src/ifu/ifu.sv
+++ b/wally-pipelined/src/ifu/ifu.sv
@@ -48,6 +48,7 @@ module ifu (
   // Mem
   input logic 		      RetM, TrapM, 
   input logic [`XLEN-1:0]     PrivilegedNextPCM, 
+  input logic                 InvalidateICacheM,
   output logic [31:0] 	      InstrD, InstrE, InstrM, InstrW,
   output logic [`XLEN-1:0]    PCM, 
   output logic [4:0] 	      InstrClassM,
diff --git a/wally-pipelined/src/lsu/lsu.sv b/wally-pipelined/src/lsu/lsu.sv
index ed13ebebe..fc9b7c588 100644
--- a/wally-pipelined/src/lsu/lsu.sv
+++ b/wally-pipelined/src/lsu/lsu.sv
@@ -42,6 +42,7 @@ module lsu
    input logic [1:0] 	       AtomicM,
    input logic 		       ExceptionM,
    input logic 		       PendingInterruptM,
+   input logic           FlushDCacheM,
    output logic 	       CommittedM, 
    output logic 	       SquashSCW,
    output logic 	       DataMisalignedM,
diff --git a/wally-pipelined/src/privileged/csr.sv b/wally-pipelined/src/privileged/csr.sv
index eb5322537..b995a3bbf 100644
--- a/wally-pipelined/src/privileged/csr.sv
+++ b/wally-pipelined/src/privileged/csr.sv
@@ -90,7 +90,7 @@ module csr #(parameter
   logic IllegalCSRMWriteReadonlyM;
   
   generate
-    if (`ZCSR_SUPPORTED) begin
+    if (`ZICSR_SUPPORTED) begin
       // modify CSRs
       always_comb begin
         // Choose either rs1 or uimm[4:0] as source
diff --git a/wally-pipelined/src/privileged/csrc.sv b/wally-pipelined/src/privileged/csrc.sv
index 4149e0cb5..3b1e544d7 100644
--- a/wally-pipelined/src/privileged/csrc.sv
+++ b/wally-pipelined/src/privileged/csrc.sv
@@ -91,7 +91,7 @@ module csrc #(parameter
   );
 
   generate
-    if (`ZCOUNTERS_SUPPORTED) begin
+    if (`ZICOUNTERS_SUPPORTED) begin
       //  logic [63:0] TIME_REGW, TIMECMP_REGW;
       logic [63:0] CYCLE_REGW, INSTRET_REGW;
       logic [63:0] HPMCOUNTER3_REGW, HPMCOUNTER4_REGW; // add more performance counters here if desired
@@ -413,7 +413,7 @@ module csrc #(parameter
 
     genvar j;       
     generate
-        if (`ZCOUNTERS_SUPPORTED) begin
+        if (`ZICOUNTERS_SUPPORTED) begin
             logic [`COUNTERS:0][63:0] HPMCOUNTER_REGW;
             logic [`COUNTERS:0][63:0] HPMCOUNTERPlusM;
             logic [`COUNTERS:0][`XLEN-1:0] NextHPMCOUNTERM;
diff --git a/wally-pipelined/src/wally/wallypipelinedhart.sv b/wally-pipelined/src/wally/wallypipelinedhart.sv
index 0b1457b90..eb5169eb2 100644
--- a/wally-pipelined/src/wally/wallypipelinedhart.sv
+++ b/wally-pipelined/src/wally/wallypipelinedhart.sv
@@ -85,7 +85,7 @@ module wallypipelinedhart
   logic 		    LoadMisalignedFaultM, LoadAccessFaultM;
   logic 		    StoreMisalignedFaultM, StoreAccessFaultM;
   logic [`XLEN-1:0] 	    InstrMisalignedAdrM;
-
+  logic       InvalidateICacheM, FlushDCacheM;
   logic 		    PCSrcE;
   logic 		    CSRWritePendingDEM;
   logic 		    DivDoneE;
@@ -196,6 +196,7 @@ module wallypipelinedhart
 	  .MemAdrM(MemAdrM),      
 	  .WriteDataM(WriteDataM),
 	  .ReadDataM(ReadDataM),
+    .FlushDCacheM,
 
 	  // connected to ahb (all stay the same)
 	  .DCtoAHBPAdrM(DCtoAHBPAdrM),