diff --git a/rtl/serv_ctrl.v b/rtl/serv_ctrl.v
index ab906ee..826f0b2 100644
--- a/rtl/serv_ctrl.v
+++ b/rtl/serv_ctrl.v
@@ -9,6 +9,7 @@ module serv_ctrl
    //State
    input wire 	      i_pc_en,
    input wire 	      i_cnt12to31,
+   input wire 	      i_cnt0,
    input wire 	      i_cnt2,
    input wire 	      i_cnt_done,
    //Control
@@ -54,7 +55,7 @@ module serv_ctrl
 
    generate
       if (WITH_CSR)
-	assign new_pc = i_trap ? (i_csr_pc & en_pc_r) : i_jump ? pc_plus_offset_aligned : pc_plus_4;
+	assign new_pc = i_trap ? (i_csr_pc & !i_cnt0) : i_jump ? pc_plus_offset_aligned : pc_plus_4;
       else
 	assign new_pc = i_jump ? pc_plus_offset_aligned : pc_plus_4;
    endgenerate
@@ -64,7 +65,7 @@ module serv_ctrl
    assign offset_b = i_utype ? (i_imm & i_cnt12to31): i_buf;
    assign {pc_plus_offset_cy,pc_plus_offset} = offset_a+offset_b+pc_plus_offset_cy_r;
 
-   assign pc_plus_offset_aligned = pc_plus_offset & en_pc_r;
+   assign pc_plus_offset_aligned = pc_plus_offset & !i_cnt0;
 
    assign o_ibus_cyc = en_pc_r & !i_pc_en;
 
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index 9f2e5d5..0bb0f4e 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -306,6 +306,7 @@ module serv_top
       //State
       .i_pc_en    (ctrl_pc_en),
       .i_cnt12to31 (cnt12to31),
+      .i_cnt0     (cnt0),
       .i_cnt2     (cnt2),
       .i_cnt_done (cnt_done),
       //Control