diff --git a/pipelined/src/privileged/privileged.sv b/pipelined/src/privileged/privileged.sv
index 4f056662b..0336a3b00 100644
--- a/pipelined/src/privileged/privileged.sv
+++ b/pipelined/src/privileged/privileged.sv
@@ -114,6 +114,7 @@ module privileged (
   assign md = CauseM[`XLEN-1] ? MIDELEG_REGW[CauseM[3:0]] : MEDELEG_REGW[CauseM[`LOG_XLEN-1:0]];
   
   // PrivilegeMode FSM
+/* -----\/----- EXCLUDED -----\/-----
   always_comb begin
     TrappedSRETM = 0;
     if (mretM)      NextPrivilegeModeM = STATUS_MPP;
@@ -129,6 +130,23 @@ module privileged (
     end else        NextPrivilegeModeM = PrivilegeModeW;
   end
 
+ -----/\----- EXCLUDED -----/\----- */
+  
+  always_comb begin
+    if (TrapM) begin // Change privilege based on DELEG registers (see 3.1.8)
+      if (`S_SUPPORTED & md & (PrivilegeModeW == `U_MODE | PrivilegeModeW == `S_MODE))
+                    NextPrivilegeModeM = `S_MODE;
+      else          NextPrivilegeModeM = `M_MODE;
+    end else if (mretM) NextPrivilegeModeM = STATUS_MPP;
+    else if (sretM) begin
+      if (STATUS_TSR & PrivilegeModeW == `S_MODE) begin
+        NextPrivilegeModeM = PrivilegeModeW;
+      end else      NextPrivilegeModeM = {1'b0, STATUS_SPP};
+    end else        NextPrivilegeModeM = PrivilegeModeW;
+  end
+
+  assign TrappedSRETM = sretM & STATUS_TSR & PrivilegeModeW == `S_MODE;
+
   flopenl #(2) privmodereg(clk, reset, ~StallW, NextPrivilegeModeM, `M_MODE, PrivilegeModeW);
 
   // *** WFI could be implemented here and depends on TW
diff --git a/pipelined/src/privileged/trap.sv b/pipelined/src/privileged/trap.sv
index 25a246a5a..a2cc6ef35 100644
--- a/pipelined/src/privileged/trap.sv
+++ b/pipelined/src/privileged/trap.sv
@@ -111,9 +111,9 @@ module trap (
   end
 
   always_comb 
-    if      (mretM)                         PrivilegedNextPCM = MEPC_REGW;
-    else if (sretM)                         PrivilegedNextPCM = SEPC_REGW;
-    else                                    PrivilegedNextPCM = PrivilegedVectoredTrapVector;
+    if      (TrapM)                         PrivilegedNextPCM = PrivilegedVectoredTrapVector;
+    else if (mretM)                         PrivilegedNextPCM = MEPC_REGW;
+    else                                    PrivilegedNextPCM = SEPC_REGW;
 
   // Cause priority defined in table 3.7 of 20190608 privileged spec
   // Exceptions are of lower priority than all interrupts (3.1.9)