diff --git a/controller.sv b/controller.sv
index acdb7a3b..7f6622b7 100644
--- a/controller.sv
+++ b/controller.sv
@@ -73,6 +73,7 @@ module riscv_controller
   input  logic        exc_req_i,
   output logic        exc_ack_o,
 
+  output logic        exc_save_if_o,
   output logic        exc_save_id_o,
   output logic        exc_restore_id_o,
 
@@ -167,6 +168,7 @@ module riscv_controller
     instr_req_o      = 1'b1;
 
     exc_ack_o        = 1'b0;
+    exc_save_if_o    = 1'b0;
     exc_save_id_o    = 1'b0;
     exc_restore_id_o = 1'b0;
 
@@ -252,7 +254,7 @@ module riscv_controller
 
           // TODO: This assumes that the pipeline is always flushed before
           //       going to sleep.
-          exc_save_id_o = 1'b1;
+          exc_save_if_o = 1'b1;
         end
       end
 
diff --git a/cs_registers.sv b/cs_registers.sv
index 527c9363..2f39f424 100644
--- a/cs_registers.sv
+++ b/cs_registers.sv
@@ -57,10 +57,12 @@ module riscv_cs_registers
   output logic        irq_enable_o,
   output logic [31:0] mepc_o,
 
+  input  logic [31:0] pc_if_i,
   input  logic [31:0] pc_id_i,
   input  logic [31:0] pc_ex_i,
   input  logic        data_load_event_ex_i,
-  input  logic        exc_save_i,
+  input  logic        exc_save_if_i,
+  input  logic        exc_save_id_i,
   input  logic        exc_restore_i,
 
   input  logic [5:0]  exc_cause_i,
@@ -205,10 +207,18 @@ module riscv_cs_registers
     endcase
 
     // exception controller gets priority over other writes
-    if (exc_save_i) begin
-      mepc_n     = data_load_event_ex_i ? pc_ex_i : pc_id_i; // save EX PC if special event load
+    if (exc_save_if_i || exc_save_id_i) begin
       mestatus_n = mstatus_q;
       mstatus_n  = 1'b0;
+
+      if (data_load_event_ex_i) begin
+        mepc_n = pc_ex_i;
+      end else begin
+        if (exc_save_if_i)
+          mepc_n = pc_if_i;
+        else
+          mepc_n = pc_id_i;
+      end
     end
 
     if (save_exc_cause_i)
diff --git a/id_stage.sv b/id_stage.sv
index 70a1186e..5f4c1e47 100644
--- a/id_stage.sv
+++ b/id_stage.sv
@@ -151,6 +151,7 @@ module riscv_id_stage
     output logic [5:0]  exc_cause_o,
     output logic        save_exc_cause_o,
 
+    output logic        exc_save_if_o,
     output logic        exc_save_id_o,
     output logic        exc_restore_id_o,
 
@@ -851,6 +852,7 @@ module riscv_id_stage
     .exc_req_i                      ( exc_req                ),
     .exc_ack_o                      ( exc_ack                ),
 
+    .exc_save_if_o                  ( exc_save_if_o          ),
     .exc_save_id_o                  ( exc_save_id_o          ),
     .exc_restore_id_o               ( exc_restore_id_o       ),
 
diff --git a/riscv_core.sv b/riscv_core.sv
index 6860ded8..8d4d17ba 100644
--- a/riscv_core.sv
+++ b/riscv_core.sv
@@ -208,6 +208,7 @@ module riscv_core
 
   logic [5:0]  exc_cause;
   logic        save_exc_cause;
+  logic        exc_save_if;
   logic        exc_save_id;
   logic        exc_restore_id;
 
@@ -493,6 +494,7 @@ module riscv_core
     .irq_enable_i                 ( irq_enable           ), // global interrupt enable
     .exc_cause_o                  ( exc_cause            ),
     .save_exc_cause_o             ( save_exc_cause       ),
+    .exc_save_if_o                ( exc_save_if          ), // control signal to save pc
     .exc_save_id_o                ( exc_save_id          ), // control signal to save pc
     .exc_restore_id_o             ( exc_restore_id       ), // control signal to restore pc
     .lsu_load_err_i               ( lsu_load_err         ),
@@ -699,10 +701,12 @@ module riscv_core
     .irq_enable_o            ( irq_enable         ),
     .mepc_o                  ( mepc               ),
 
+    .pc_if_i                 ( pc_if              ),
     .pc_id_i                 ( pc_id              ), // from IF stage
     .pc_ex_i                 ( pc_ex              ), // from ID/EX pipeline
     .data_load_event_ex_i    ( data_load_event_ex ), // from ID/EX pipeline
-    .exc_save_i              ( exc_save_id        ),
+    .exc_save_if_i           ( exc_save_if        ),
+    .exc_save_id_i           ( exc_save_id        ),
     .exc_restore_i           ( exc_restore_id     ),
 
     .exc_cause_i             ( exc_cause          ),