diff --git a/rtl/serv_bufreg2.v b/rtl/serv_bufreg2.v
index 3445700..ee25576 100644
--- a/rtl/serv_bufreg2.v
+++ b/rtl/serv_bufreg2.v
@@ -5,6 +5,8 @@ module serv_bufreg2
    input wire 	      i_en,
    input wire 	      i_init,
    input wire 	      i_cnt7,
+   input wire 	      i_cnt_done,
+   input wire 	      i_sh_right,
    input wire [1:0]   i_lsb,
    input wire [1:0]   i_bytecnt,
    output wire 	      o_sh_done,
@@ -42,7 +44,7 @@ module serv_bufreg2
 
    wire 	 shift_en = i_shift_op ? (i_en & i_init & (i_bytecnt == 2'b00)) : (i_en & byte_valid);
 
-   wire		 cnt_en = (i_shift_op & !i_init);
+   wire		 cnt_en = (i_shift_op & (!i_init | (i_cnt_done & i_sh_right)));
 
    /* The dat register has three different use cases for store, load and
     shift operations.
diff --git a/rtl/serv_state.v b/rtl/serv_state.v
index 487bd82..bd73b3c 100644
--- a/rtl/serv_state.v
+++ b/rtl/serv_state.v
@@ -59,7 +59,6 @@ module serv_state
    input wire 	     i_rf_ready,
    output wire 	     o_rf_rd_en);
 
-   reg 	stage_two_req;
    reg 	init_done;
    wire misalign_trap_sync;
 
@@ -101,7 +100,7 @@ module serv_state
    //Right shift. o_sh_done
    //Mem ops. i_dbus_ack
    //MDU ops. i_mdu_ready
-   assign o_rf_wreq = (i_shift_op & (i_sh_right ? (i_sh_done & !o_cnt_en & init_done) : last_init)) |
+   assign o_rf_wreq = (i_shift_op & (i_sh_right ? (i_sh_done & (last_init | !o_cnt_en & init_done)) : last_init)) |
 	   	       i_dbus_ack | (MDU & i_mdu_ready) |
 	   	      (i_branch_op & (last_init & !trap_pending)) |
 	   	      (i_rd_alu_en & i_alu_rd_sel1 & last_init);
@@ -126,7 +125,7 @@ module serv_state
             for the first cycle after init). Shift out during phase 2
     */
    
-   assign o_bufreg_en = (o_cnt_en & (o_init | ((o_ctrl_trap | i_branch_op) & i_two_stage_op))) | (i_shift_op & init_done & (i_sh_right ? !stage_two_req : i_sh_done));
+   assign o_bufreg_en = (o_cnt_en & (o_init | ((o_ctrl_trap | i_branch_op) & i_two_stage_op))) | (i_shift_op & init_done & (i_sh_right | i_sh_done));
 
    assign o_ibus_cyc = ibus_cyc & !i_rst;
 
@@ -152,14 +151,10 @@ module serv_state
 	 o_ctrl_jump <= o_init & take_branch;
       end
 
-      //Need a strobe for the first cycle in the IDLE state after INIT
-      stage_two_req <= o_cnt_done & o_init;
-
       if (i_rst) begin
 	 if (RESET_STRATEGY != "NONE") begin
 	    init_done <= 1'b0;
 	    o_ctrl_jump <= 1'b0;
-	    stage_two_req <= 1'b0;
 	 end
       end
    end
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index 3a9fc76..425f325 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -400,6 +400,8 @@ module serv_top
       .i_en         (cnt_en),
       .i_init       (init),
       .i_cnt7       (cnt7),
+      .i_cnt_done   (cnt_done),
+      .i_sh_right   (sh_right),
       .i_lsb        (lsb),
       .i_bytecnt    (mem_bytecnt),
       .o_sh_done    (sh_done),