diff --git a/pipelined/src/privileged/csr.sv b/pipelined/src/privileged/csr.sv
index 6d71b08d..b6b76c93 100644
--- a/pipelined/src/privileged/csr.sv
+++ b/pipelined/src/privileged/csr.sv
@@ -135,7 +135,7 @@ module csr #(parameter
 
   if(`VECTORED_INTERRUPTS_SUPPORTED) begin:vec
       always_comb
-        if (PrivilegedTrapVector[1:0] == 2'b01 & CauseM[`XLEN-1] == 1)
+        if (PrivilegedTrapVector[1:0] == 2'b01 & InterruptM)
           PrivilegedVectoredTrapVector = {PrivilegedTrapVector[`XLEN-1:2] + CauseM[`XLEN-3:0], 2'b00};
         else
           PrivilegedVectoredTrapVector = {PrivilegedTrapVector[`XLEN-1:2], 2'b00};
@@ -178,7 +178,7 @@ module csr #(parameter
   assign CSRAdrM = InstrM[31:20];
   assign UnalignedNextEPCM = TrapM ? ((wfiM & InterruptM) ? 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 NextCauseM = TrapM ? {InterruptM, CauseM[`XLEN-2:0]}: CSRWriteValM;
   assign NextMtvalM = TrapM ? NextFaultMtvalM : CSRWriteValM;
   assign CSRMWriteM = CSRWriteM & (PrivilegeModeW == `M_MODE);
   assign CSRSWriteM = CSRWriteM & (|PrivilegeModeW);