diff --git a/rtl/serv_rf_if.v b/rtl/serv_rf_if.v
index 61b1f23..b26e75f 100644
--- a/rtl/serv_rf_if.v
+++ b/rtl/serv_rf_if.v
@@ -1,14 +1,15 @@
 `default_nettype none
 module serv_rf_if
+  #(parameter WITH_CSR = 1)
   (//RF Interface
-   output wire [5:0] o_wreg0,
-   output wire [5:0] o_wreg1,
+   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 [5:0] o_rreg0,
-   output wire [5:0] o_rreg1,
+   output wire [4+WITH_CSR:0] o_rreg0,
+   output wire [4+WITH_CSR:0] o_rreg1,
    input wire 	     i_rdata0,
    input wire 	     i_rdata1,
 
@@ -42,7 +43,6 @@ module serv_rf_if
    input wire [4:0]  i_rs2_raddr,
    output wire 	     o_rs2);
 
-   parameter WITH_CSR = 1;
 
 `include "serv_params.vh"
 
@@ -102,7 +102,8 @@ module serv_rf_if
       assign 	     o_wdata0 = rd;
       assign	     o_wdata1 = 1'b0;
 
-      assign o_wreg0 = {1'b0,i_rd_waddr};
+      assign o_wreg0 = i_rd_waddr;
+      assign o_wreg1 = 5'd0;
 
       assign       o_wen0 =i_rd_wen;
       assign       o_wen1 = 1'b0;
@@ -111,8 +112,8 @@ module serv_rf_if
     ********** Read side ***********
     */
 
-      assign o_rreg0 = {1'b0, i_rs1_raddr};
-      assign o_rreg1 = {1'b0, i_rs2_raddr};
+      assign o_rreg0 = i_rs1_raddr;
+      assign o_rreg1 = i_rs2_raddr;
 
       assign o_rs1 = i_rdata0;
       assign o_rs2 = i_rdata1;
diff --git a/rtl/serv_rf_ram_if.v b/rtl/serv_rf_ram_if.v
index ec94fdb..a0744ec 100644
--- a/rtl/serv_rf_ram_if.v
+++ b/rtl/serv_rf_ram_if.v
@@ -5,27 +5,27 @@ module serv_rf_ram_if
     parameter depth=32*(32+csr_regs)/width)
   (
    //SERV side
-   input wire 			   i_clk,
-   input wire 			   i_rst,
-   input wire 			   i_wreq,
-   input wire 			   i_rreq,
-   output wire 			   o_ready,
-   input wire [5:0] 		   i_wreg0,
-   input wire [5:0] 		   i_wreg1,
-   input wire 			   i_wen0,
-   input wire 			   i_wen1,
-   input wire 			   i_wdata0,
-   input wire 			   i_wdata1,
-   input wire [5:0] 		   i_rreg0,
-   input wire [5:0] 		   i_rreg1,
-   output wire 			   o_rdata0,
-   output wire 			   o_rdata1,
+   input wire 				i_clk,
+   input wire 				i_rst,
+   input wire 				i_wreq,
+   input wire 				i_rreq,
+   output wire 				o_ready,
+   input wire [$clog2(32+csr_regs)-1:0] i_wreg0,
+   input wire [$clog2(32+csr_regs)-1:0] i_wreg1,
+   input wire 				i_wen0,
+   input wire 				i_wen1,
+   input wire 				i_wdata0,
+   input wire 				i_wdata1,
+   input wire [$clog2(32+csr_regs)-1:0] i_rreg0,
+   input wire [$clog2(32+csr_regs)-1:0] i_rreg1,
+   output wire 				o_rdata0,
+   output wire 				o_rdata1,
    //RAM side
-   output wire [$clog2(depth)-1:0] o_waddr,
-   output wire [width-1:0] 	   o_wdata,
-   output wire 			   o_wen,
-   output wire [$clog2(depth)-1:0] o_raddr,
-   input wire [width-1:0] 	   i_rdata);
+   output wire [$clog2(depth)-1:0] 	o_waddr,
+   output wire [width-1:0] 		o_wdata,
+   output wire 				o_wen,
+   output wire [$clog2(depth)-1:0] 	o_raddr,
+   input wire [width-1:0] 		i_rdata);
 
    localparam l2w = $clog2(width);
 
@@ -63,7 +63,7 @@ module serv_rf_ram_if
 			       wdata1_r :
 			       {i_wdata0, wdata0_r};
 
-   wire [5:0] wreg  = wtrig1 ? i_wreg1 : i_wreg0;
+   wire [$clog2(32+csr_regs)-1:0] wreg  = wtrig1 ? i_wreg1 : i_wreg0;
    generate if (width == 32)
      assign o_waddr = wreg;
    else
@@ -111,7 +111,7 @@ module serv_rf_ram_if
    wire 	  rtrig0;
    reg 		  rtrig1;
 
-   wire [5:0] 	  rreg = rtrig0 ? i_rreg1 : i_rreg0;
+   wire [$clog2(32+csr_regs)-1:0] rreg = rtrig0 ? i_rreg1 : i_rreg0;
    generate if (width == 32)
      assign o_raddr = rreg;
    else
diff --git a/rtl/serv_rf_top.v b/rtl/serv_rf_top.v
index b688012..66aec5d 100644
--- a/rtl/serv_rf_top.v
+++ b/rtl/serv_rf_top.v
@@ -1,6 +1,9 @@
 `default_nettype none
 
 module serv_rf_top
+  #(parameter RESET_PC = 32'd0,
+    parameter WITH_CSR = 1,
+    parameter RF_WIDTH = 2)
   (
    input wire 	      clk,
    input wire 	      i_rst,
@@ -40,34 +43,32 @@ module serv_rf_top
    input wire [31:0]  i_dbus_rdt,
    input wire 	      i_dbus_ack);
 
-   parameter RESET_PC = 32'd0;
-   parameter WITH_CSR = 1;
-   parameter RF_WIDTH = 2;
-   localparam RF_L2W = $clog2(RF_WIDTH);
+   localparam CSR_REGS = WITH_CSR*4;
+   localparam RF_L2D = $clog2((32+CSR_REGS)*32/RF_WIDTH);
 
    wire 	      rf_wreq;
    wire 	      rf_rreq;
-   wire [5:0]	      wreg0;
-   wire [5:0]	      wreg1;
+   wire [4+WITH_CSR:0] wreg0;
+   wire [4+WITH_CSR:0] wreg1;
    wire 	      wen0;
    wire 	      wen1;
    wire 	      wdata0;
    wire 	      wdata1;
-   wire [5:0]	      rreg0;
-   wire [5:0] 	      rreg1;
+   wire [4+WITH_CSR:0] rreg0;
+   wire [4+WITH_CSR:0] rreg1;
    wire 	      rf_ready;
    wire 	      rdata0;
    wire 	      rdata1;
 
-   wire [10-RF_L2W:0] waddr;
+   wire [RF_L2D-1:0]   waddr;
    wire [RF_WIDTH-1:0] wdata;
-   wire 	      wen;
-   wire [10-RF_L2W:0] raddr;
+   wire 	       wen;
+   wire [RF_L2D-1:0]   raddr;
    wire [RF_WIDTH-1:0] rdata;
 
    serv_rf_ram_if
      #(.width    (RF_WIDTH),
-       .csr_regs (WITH_CSR*4))
+       .csr_regs (CSR_REGS))
    rf_ram_if
      (.i_clk    (clk),
       .i_rst    (i_rst),
@@ -92,7 +93,7 @@ module serv_rf_top
 
    serv_rf_ram
      #(.width (RF_WIDTH),
-       .csr_regs (WITH_CSR*4))
+       .csr_regs (CSR_REGS))
    rf_ram
      (.i_clk    (clk),
       .i_waddr (waddr),
diff --git a/rtl/serv_state.v b/rtl/serv_state.v
index 6938c25..b4703a1 100644
--- a/rtl/serv_state.v
+++ b/rtl/serv_state.v
@@ -138,6 +138,9 @@ module serv_state
       if (i_ibus_ack)
 	misalign_trap_sync <= 1'b0;
    end // always @ (posedge i_clk)
-      end // if (WITH_CSR)
+      end else begin
+	 always @(*)
+	   o_pending_irq = 1'b0;
+      end
    endgenerate
 endmodule
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index 503f69a..1131199 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -1,61 +1,61 @@
 `default_nettype none
 
 module serv_top
-  (
-   input wire 	      clk,
-   input wire 	      i_rst,
-   input wire 	      i_timer_irq,
+  #(parameter WITH_CSR = 1,
+    parameter RESET_PC = 32'd0)
+   (
+   input wire 		      clk,
+   input wire 		      i_rst,
+   input wire 		      i_timer_irq,
 `ifdef RISCV_FORMAL
-   output reg 	      rvfi_valid = 1'b0,
-   output reg [63:0]  rvfi_order = 64'd0,
-   output reg [31:0]  rvfi_insn = 32'd0,
-   output reg 	      rvfi_trap = 1'b0,
-   output reg 	      rvfi_halt = 1'b0,
-   output reg 	      rvfi_intr = 1'b0,
-   output reg [1:0]   rvfi_mode = 2'b11,
-   output reg [1:0]   rvfi_ixl = 2'b01,
-   output reg [4:0]   rvfi_rs1_addr,
-   output reg [4:0]   rvfi_rs2_addr,
-   output reg [31:0]  rvfi_rs1_rdata,
-   output reg [31:0]  rvfi_rs2_rdata,
-   output reg [4:0]   rvfi_rd_addr,
-   output reg [31:0]  rvfi_rd_wdata,
-   output reg [31:0]  rvfi_pc_rdata,
-   output reg [31:0]  rvfi_pc_wdata,
-   output reg [31:0]  rvfi_mem_addr,
-   output reg [3:0]   rvfi_mem_rmask,
-   output reg [3:0]   rvfi_mem_wmask,
-   output reg [31:0]  rvfi_mem_rdata,
-   output reg [31:0]  rvfi_mem_wdata,
+   output reg 		      rvfi_valid = 1'b0,
+   output reg [63:0] 	      rvfi_order = 64'd0,
+   output reg [31:0] 	      rvfi_insn = 32'd0,
+   output reg 		      rvfi_trap = 1'b0,
+   output reg 		      rvfi_halt = 1'b0,
+   output reg 		      rvfi_intr = 1'b0,
+   output reg [1:0] 	      rvfi_mode = 2'b11,
+   output reg [1:0] 	      rvfi_ixl = 2'b01,
+   output reg [4:0] 	      rvfi_rs1_addr,
+   output reg [4:0] 	      rvfi_rs2_addr,
+   output reg [31:0] 	      rvfi_rs1_rdata,
+   output reg [31:0] 	      rvfi_rs2_rdata,
+   output reg [4:0] 	      rvfi_rd_addr,
+   output reg [31:0] 	      rvfi_rd_wdata,
+   output reg [31:0] 	      rvfi_pc_rdata,
+   output reg [31:0] 	      rvfi_pc_wdata,
+   output reg [31:0] 	      rvfi_mem_addr,
+   output reg [3:0] 	      rvfi_mem_rmask,
+   output reg [3:0] 	      rvfi_mem_wmask,
+   output reg [31:0] 	      rvfi_mem_rdata,
+   output reg [31:0] 	      rvfi_mem_wdata,
 `endif
    //RF Interface
-   output wire 	     o_rf_rreq,
-   output wire 	     o_rf_wreq,
-   input wire 	     i_rf_ready,
-   output wire [5:0] o_wreg0,
-   output wire [5:0] o_wreg1,
-   output wire 	     o_wen0,
-   output wire 	     o_wen1,
-   output wire 	     o_wdata0,
-   output wire 	     o_wdata1,
-   output wire [5:0] o_rreg0,
-   output wire [5:0] o_rreg1,
-   input wire 	     i_rdata0,
-   input wire 	     i_rdata1,
+   output wire 		      o_rf_rreq,
+   output wire 		      o_rf_wreq,
+   input wire 		      i_rf_ready,
+   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 [4+WITH_CSR:0] o_rreg0,
+   output wire [4+WITH_CSR:0] o_rreg1,
+   input wire 		      i_rdata0,
+   input wire 		      i_rdata1,
 
-   output wire [31:0] o_ibus_adr,
-   output wire 	      o_ibus_cyc,
-   input wire [31:0]  i_ibus_rdt,
-   input wire 	      i_ibus_ack,
-   output wire [31:0] o_dbus_adr,
-   output wire [31:0] o_dbus_dat,
-   output wire [3:0]  o_dbus_sel,
-   output wire 	      o_dbus_we ,
-   output wire 	      o_dbus_cyc,
-   input wire [31:0]  i_dbus_rdt,
-   input wire 	      i_dbus_ack);
-
-   parameter WITH_CSR = 1;
+   output wire [31:0] 	      o_ibus_adr,
+   output wire 		      o_ibus_cyc,
+   input wire [31:0] 	      i_ibus_rdt,
+   input wire 		      i_ibus_ack,
+   output wire [31:0] 	      o_dbus_adr,
+   output wire [31:0] 	      o_dbus_dat,
+   output wire [3:0] 	      o_dbus_sel,
+   output wire 		      o_dbus_we ,
+   output wire 		      o_dbus_cyc,
+   input wire [31:0] 	      i_dbus_rdt,
+   input wire 		      i_dbus_ack);
 
    wire [4:0]    rd_addr;
    wire [4:0]    rs1_addr;
@@ -136,7 +136,6 @@ module serv_top
    wire [1:0] 	 csr_addr;
    wire 	 csr_pc;
 
-   parameter RESET_PC = 32'd0;
 
    wire 	 new_irq;
    wire 	 trap_taken;