diff --git a/rtl/serv_rf_if.v b/rtl/serv_rf_if.v
index 431bfbb..f3f7842 100644
--- a/rtl/serv_rf_if.v
+++ b/rtl/serv_rf_if.v
@@ -2,46 +2,47 @@
 module serv_rf_if
   #(parameter WITH_CSR = 1)
   (//RF Interface
+   input wire 		      i_cnt_en,
    output wire [4+WITH_CSR:0] o_wreg0,
    output wire [4+WITH_CSR:0] o_wreg1,
-   output wire 	     o_wen0,
-   output wire 	     o_wen1,
-   output wire 	     o_wdata0,
-   output wire 	     o_wdata1,
+   output wire 		      o_wen0,
+   output wire 		      o_wen1,
+   output wire 		      o_wdata0,
+   output wire 		      o_wdata1,
    output wire [4+WITH_CSR:0] o_rreg0,
    output wire [4+WITH_CSR:0] o_rreg1,
-   input wire 	     i_rdata0,
-   input wire 	     i_rdata1,
+   input wire 		      i_rdata0,
+   input wire 		      i_rdata1,
 
    //Trap interface
-   input wire 	     i_trap,
-   input wire 	     i_mret,
-   input wire 	     i_mepc,
-   input wire 	     i_mem_op,
-   input wire 	     i_bufreg_q,
-   input wire 	     i_bad_pc,
-   output wire 	     o_csr_pc,
+   input wire 		      i_trap,
+   input wire 		      i_mret,
+   input wire 		      i_mepc,
+   input wire 		      i_mem_op,
+   input wire 		      i_bufreg_q,
+   input wire 		      i_bad_pc,
+   output wire 		      o_csr_pc,
    //CSR interface
-   input wire 	     i_csr_en,
-   input wire [1:0]  i_csr_addr,
-   input wire 	     i_csr,
-   output wire 	     o_csr,
+   input wire 		      i_csr_en,
+   input wire [1:0] 	      i_csr_addr,
+   input wire 		      i_csr,
+   output wire 		      o_csr,
    //RD write port
-   input wire 	     i_rd_wen,
-   input wire [4:0]  i_rd_waddr,
-   input wire 	     i_ctrl_rd,
-   input wire 	     i_alu_rd,
-   input wire 	     i_rd_alu_en,
-   input wire 	     i_csr_rd,
-   input wire 	     i_rd_csr_en,
-   input wire 	     i_mem_rd,
+   input wire 		      i_rd_wen,
+   input wire [4:0] 	      i_rd_waddr,
+   input wire 		      i_ctrl_rd,
+   input wire 		      i_alu_rd,
+   input wire 		      i_rd_alu_en,
+   input wire 		      i_csr_rd,
+   input wire 		      i_rd_csr_en,
+   input wire 		      i_mem_rd,
 
    //RS1 read port
-   input wire [4:0]  i_rs1_raddr,
-   output wire 	     o_rs1,
+   input wire [4:0] 	      i_rs1_raddr,
+   output wire 		      o_rs1,
    //RS2 read port
-   input wire [4:0]  i_rs2_raddr,
-   output wire 	     o_rs2);
+   input wire [4:0] 	      i_rs2_raddr,
+   output wire 		      o_rs2);
 
 
 `include "serv_params.vh"
@@ -73,8 +74,8 @@ module serv_rf_if
    assign o_wreg0 = i_trap ? {4'b1000,CSR_MTVAL} : {1'b0,i_rd_waddr};
    assign o_wreg1 = i_trap ? {4'b1000,CSR_MEPC}  : {4'b1000,i_csr_addr};
 
-   assign       o_wen0 = i_trap | rd_wen;
-   assign       o_wen1 = i_trap | i_csr_en;
+   assign       o_wen0 = i_cnt_en & (i_trap | rd_wen);
+   assign       o_wen1 = i_cnt_en & (i_trap | i_csr_en);
 
    /*
     ********** Read side ***********
diff --git a/rtl/serv_rf_ram_if.v b/rtl/serv_rf_ram_if.v
index f7b47da..4de1169 100644
--- a/rtl/serv_rf_ram_if.v
+++ b/rtl/serv_rf_ram_if.v
@@ -31,12 +31,13 @@ module serv_rf_ram_if
 
    reg 				   rgnt;
    assign o_ready = rgnt | i_wreq;
+   reg [4:0] 	  rcnt;
 
    /*
     ********** Write side ***********
     */
 
-   reg [4:0] 	     wcnt;
+   wire [4:0] 	     wcnt;
    reg 		     wgo;
 
 
@@ -70,9 +71,7 @@ module serv_rf_ram_if
      assign o_waddr = {wreg, wcnt[4:l2w]};
    endgenerate
 
-   assign o_wen = wgo & ((wtrig0 & wen0_r) | (wtrig1 & wen1_r));
-
-   reg 	      wreq_r;
+   assign o_wen = (wtrig0 & wen0_r) | (wtrig1 & wen1_r);
 
    generate if (width > 2)
      always @(posedge i_clk) wdata0_r  <= {i_wdata0, wdata0_r[width-2:1]};
@@ -80,34 +79,20 @@ module serv_rf_ram_if
      always @(posedge i_clk) wdata0_r  <= i_wdata0;
    endgenerate
 
+   assign wcnt = rcnt-3;
+
    always @(posedge i_clk) begin
       wen0_r    <= i_wen0;
       wen1_r    <= i_wen1;
-      wreq_r    <= i_wreq | rgnt;
 
       wdata1_r  <= {i_wdata1,wdata1_r[width-1:1]};
 
-      if (wgo)
-	wcnt <= wcnt+5'd1;
-
-      if (wreq_r) begin
-	 wgo <= 1'b1;
-      end
-
-      if (wcnt == 5'b11111)
-	wgo <= 1'b0;
-
-      if (i_rst) begin
-	 if (reset_strategy != "NONE")
-	   wcnt <= 5'd0;
-      end
    end
 
    /*
     ********** Read side ***********
     */
 
-   reg [4:0] 	  rcnt;
 
    wire 	  rtrig0;
    reg 		  rtrig1;
@@ -144,6 +129,8 @@ module serv_rf_ram_if
       rcnt <= rcnt+5'd1;
       if (i_rreq)
 	 rcnt <= 5'd0;
+      if (i_wreq)
+	 rcnt <= 5'd2;
 
       rreq_r <= i_rreq;
       rgnt <= rreq_r;
diff --git a/rtl/serv_state.v b/rtl/serv_state.v
index 547cd75..f78c108 100644
--- a/rtl/serv_state.v
+++ b/rtl/serv_state.v
@@ -89,11 +89,11 @@ module serv_state
    //Prepare RF for writes when everything is ready to enter stage two
    // and the first stage didn't cause a misalign exception
    assign o_rf_wreq = !misalign_trap_sync &
-		      ((i_shift_op & (i_sh_done | !i_sh_right) & init_done) |
+		      ((i_shift_op & (i_sh_done | !i_sh_right) & !o_cnt_en & init_done) |
 		       (i_mem_op & i_dbus_ack) |
 		       (stage_two_req & (i_slt_op | i_branch_op)));
 
-   assign o_rf_rd_en = i_rd_op & o_cnt_en & !o_init;
+   assign o_rf_rd_en = i_rd_op & !o_init;
 
    /*
     bufreg is used during mem. branch and shift operations
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index 7fbd0d9..9355d9f 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -347,6 +347,7 @@ module serv_top
      #(.WITH_CSR (WITH_CSR))
    rf_if
      (//RF interface
+      .i_cnt_en    (cnt_en),
       .o_wreg0     (o_wreg0),
       .o_wreg1     (o_wreg1),
       .o_wen0      (o_wen0),