diff --git a/rtl/serv_rf_ram_if.v b/rtl/serv_rf_ram_if.v
index 7ac1a35..c8c809d 100644
--- a/rtl/serv_rf_ram_if.v
+++ b/rtl/serv_rf_ram_if.v
@@ -8,7 +8,7 @@ module serv_rf_ram_if
    input wire 			   i_rst,
    input wire 			   i_wreq,
    input wire 			   i_rreq,
-   output reg 			   o_rgnt,
+   output wire 			   o_ready,
    input wire [5:0] 		   i_wreg0,
    input wire [5:0] 		   i_wreg1,
    input wire 			   i_wen0,
@@ -28,6 +28,9 @@ module serv_rf_ram_if
 
    localparam l2w = $clog2(width);
 
+   reg 				   rgnt;
+   assign o_ready = rgnt | i_wreq;
+
    /*
     ********** Write side ***********
     */
@@ -80,7 +83,7 @@ module serv_rf_ram_if
    always @(posedge i_clk) begin
       wen0_r    <= i_wen0;
       wen1_r    <= i_wen1;
-      wreq_r    <= i_wreq;
+      wreq_r    <= i_wreq | rgnt;
       wreq_edge <= i_wreq & !wreq_r;
 
       wdata1_r  <= {i_wdata1,wdata1_r[width-1:1]};
@@ -143,14 +146,14 @@ module serv_rf_ram_if
 	 rcnt <= 5'd0;
 
       rreq_r <= i_rreq;
-      o_rgnt <= rreq_r;
+      rgnt <= rreq_r;
 
       rdata0 <= {1'b0,rdata0[width-1:1]};
       if (rtrig0)
 	rdata0 <= i_rdata;
 
       if (i_rst) begin
-	 o_rgnt <= 1'b0;
+	 rgnt <= 1'b0;
 	 rreq_r <= 1'b0;
       end
    end
diff --git a/rtl/serv_rf_top.v b/rtl/serv_rf_top.v
index 47c4898..5a9a761 100644
--- a/rtl/serv_rf_top.v
+++ b/rtl/serv_rf_top.v
@@ -69,7 +69,7 @@ module serv_rf_top
       .i_rst    (i_rst),
       .i_wreq   (rf_wreq),
       .i_rreq   (rf_rreq),
-      .o_rgnt   (rf_ready),
+      .o_ready  (rf_ready),
       .i_wreg0  (wreg0),
       .i_wreg1  (wreg1),
       .i_wen0   (wen0),
diff --git a/rtl/serv_state.v b/rtl/serv_state.v
index 93489f0..61e5078 100644
--- a/rtl/serv_state.v
+++ b/rtl/serv_state.v
@@ -83,7 +83,7 @@ module serv_state
    assign o_rf_rreq = i_ibus_ack | (stage_two_req & trap_pending);
 
    //Prepare RF for writes when everything is ready to enter stage two
-   assign o_rf_wreq = ((i_shift_op & i_alu_sh_done & stage_two_pending) | (i_mem_op & i_dbus_ack) | (stage_two_req & (i_slt_op | i_branch_op))) & !trap_pending | i_rf_ready;
+   assign o_rf_wreq = ((i_shift_op & i_alu_sh_done & stage_two_pending) | (i_mem_op & i_dbus_ack) | (stage_two_req & (i_slt_op | i_branch_op))) & !trap_pending;
 
    //Shift operations require bufreg to hold for one cycle between INIT and RUN before shifting
    assign o_bufreg_hold = !cnt_en & (stage_two_req | ~i_shift_op);
@@ -117,31 +117,14 @@ module serv_state
       if (i_ibus_ack)
 	misalign_trap_sync <= 1'b0;
 
-      case (state)
-        IDLE : begin
-	   if (stage_two_pending) begin
-	      if (o_rf_wreq)
-		state <= RUN;
-	      if (trap_pending & i_rf_ready)
-		state <= TRAP;
-	   end else begin
-	      if (i_rf_ready)
-		if (i_e_op | o_pending_irq)
-		  state <= TRAP;
-		else if (two_stage_op)
-		  state <= INIT;
-		else
-		  state <= RUN;
-           end
-	end
-        INIT : begin
-        end
-        RUN : begin
-        end
-	TRAP : begin
-	end
-        default : state <= IDLE;
-      endcase
+      if (i_rf_ready && !cnt_en)
+	if (i_e_op | o_pending_irq | (stage_two_pending & trap_pending))
+	  state <= TRAP;
+	else if (two_stage_op & !stage_two_pending)
+	  state <= INIT;
+	else
+	  state <= RUN;
+
       if (cnt_done)
         state <= IDLE;