diff --git a/rtl/serv_bufreg.v b/rtl/serv_bufreg.v
new file mode 100644
index 0000000..cddb12b
--- /dev/null
+++ b/rtl/serv_bufreg.v
@@ -0,0 +1,40 @@
+module serv_bufreg
+  (
+   input wire 	      i_clk,
+   input wire 	      i_rst,
+   input wire [4:0]   i_cnt,
+   input wire [3:0]   i_cnt_r,
+   input wire 	      i_en,
+   input wire 	      i_clr,
+   input wire 	      i_rs1,
+   input wire 	      i_rs1_en,
+   input wire 	      i_imm,
+   input wire 	      i_imm_en,
+   output reg [1:0]   o_lsb,
+   output wire [31:0] o_reg,
+   output wire 	      o_q);
+
+   wire 	      c, q;
+   reg 		      c_r;
+   reg [31:0] 	      data;
+
+   assign {c,q} = (i_rs1 & i_rs1_en) + (i_imm & i_imm_en) + c_r;
+
+   always @(posedge i_clk) begin
+      c_r <= c & !i_clr;
+
+      if (i_rst)
+	data <= 32'd0;
+      else if (i_en)
+	data <= {q, data[31:1]};
+
+      if ((i_cnt[4:2] == 3'd0) & i_cnt_r[0] & i_en)
+	o_lsb[0] <= q;
+      if ((i_cnt[4:2] == 3'd0) & i_cnt_r[1] & i_en)
+	o_lsb[1] <= q;
+   end
+
+   assign o_q = data[0];
+   assign o_reg = data;
+
+endmodule
diff --git a/rtl/serv_mem_if.v b/rtl/serv_mem_if.v
index 8dd9b83..9245186 100644
--- a/rtl/serv_mem_if.v
+++ b/rtl/serv_mem_if.v
@@ -8,15 +8,12 @@ module serv_mem_if
    input wire 	      i_cmd,
    input wire [1:0]   i_bytecnt,
    input wire [2:0]   i_funct3,
-   input wire 	      i_rs1,
    input wire 	      i_rs2,
-   input wire 	      i_imm,
    output wire 	      o_rd,
+   input wire [1:0]   i_lsb,
    output reg 	      o_misalign,
    input wire 	      i_trap,
-   output wire 	      o_adr,
    //External interface
-   output wire [31:0] o_wb_adr,
    output wire [31:0] o_wb_dat,
    output wire [3:0]  o_wb_sel,
    output wire 	      o_wb_we ,
@@ -26,8 +23,7 @@ module serv_mem_if
 
    wire          wb_en = o_wb_cyc & i_wb_ack;
    reg           init_r;
-   reg 		 init_2r;
-   wire          adr;
+
    reg           signbit = 1'b0;
 
    reg [7:0] 	 dat0;
@@ -39,30 +35,6 @@ module serv_mem_if
    wire 	 dat2_en;
    wire 	 dat3_en;
 
-   assign o_adr = adr;
-
-   ser_add ser_add_rs1_plus_imm
-     (
-      .clk (i_clk),
-      .rst (i_rst),
-      .a   (i_rs1),
-      .b   (i_imm),
-      .clr (!i_en),
-      .q   (adr),
-      .o_v ());
-
-   assign o_wb_adr[1:0] = 2'b00;
-
-   shift_reg #(30) shift_reg_adr
-     (
-      .clk   (i_clk),
-      .i_rst (i_rst),
-      .i_en  (i_init | i_trap),
-      .i_d   (adr),
-      .o_q   (o_wb_adr[2]),
-      .o_par (o_wb_adr[31:3])
-      );
-
    wire 	 dat_cur = (dat_sel == 3) ? dat3[0] :
 		 (dat_sel == 2) ? dat2[0] :
 		 (dat_sel == 1) ? dat1[0] : dat0[0];
@@ -76,8 +48,6 @@ module serv_mem_if
    wire is_half = i_funct3[0];
    wire is_byte = !(|i_funct3[1:0]);
 
-
-
    wire       upper_half = bytepos[1];
 /*
    assign o_wb_sel = (is_word ? 4'b1111 :
@@ -90,7 +60,7 @@ module serv_mem_if
    assign o_wb_sel[0] = (bytepos == 2'b00);
 
    assign o_wb_we = i_cmd;
-   reg [1:0]  bytepos;
+   wire [1:0]  bytepos;
 
 
    wire       wbyte0 = (i_bytecnt == 2'b00);
@@ -107,6 +77,8 @@ module serv_mem_if
 
    wire [1:0] dat_sel = i_bytecnt[1] ? i_bytecnt : (i_bytecnt | bytepos);
 
+   assign bytepos = i_lsb;
+
    always @(posedge i_clk) begin
       if (dat0_en)
 	dat0 <= {i_rs2, dat0[7:1]};
@@ -120,17 +92,11 @@ module serv_mem_if
       if (wb_en)
 	{dat3,dat2,dat1,dat0} <= i_wb_rdt;
 
-      if (i_init & !init_r)
-        bytepos[0] <= adr;
-      if (init_r & !init_2r)
-        bytepos[1] <= adr;
-
       o_misalign <= i_en & ((bytepos[0] & !is_byte) | (bytepos[1] & is_word));
       if (dat_valid)
         signbit <= dat_cur;
 
       init_r <= i_init;
-      init_2r <= init_r;
       if (wb_en)
         o_wb_cyc <= 1'b0;
       else if (init_r & !i_init & !i_trap) begin //Optimize?
@@ -139,7 +105,6 @@ module serv_mem_if
       if (i_rst) begin
 	 o_wb_cyc <= 1'b0;
 	 init_r <= 1'b0;
-	 init_2r <= 1'b0;
       end
    end
 endmodule
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index ae1a87f..9ed8524 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -189,6 +189,26 @@ module serv_top
       .o_rd_alu_en    (rd_alu_en),
       .o_rd_mem_en    (rd_mem_en));
 
+   wire [1:0] 	 lsb;
+   wire [31:0] 	 bufreg_out;
+   assign o_dbus_adr = {bufreg_out[31:2], 2'b00};
+
+   serv_bufreg bufreg
+     (
+      .i_clk    (clk),
+      .i_rst    (i_rst),
+      .i_cnt    (cnt),
+      .i_cnt_r  (cnt_r),
+      .i_en     (alu_en),
+      .i_clr    (!mem_en),
+      .i_rs1    (rs1),
+      .i_rs1_en (1'b1),
+      .i_imm    (imm),
+      .i_imm_en (1'b1),
+      .o_lsb    (lsb),
+      .o_reg    (bufreg_out),
+      .o_q      (bad_adr));
+
    serv_ctrl
      #(.RESET_PC (RESET_PC))
    ctrl
@@ -267,15 +287,12 @@ module serv_top
       .i_cmd    (mem_cmd),
       .i_bytecnt (mem_bytecnt),
       .i_funct3 (funct3),
-      .i_rs1    (rs1),
       .i_rs2    (rs2),
-      .i_imm    (imm),
       .o_rd     (mem_rd),
+      .i_lsb      (lsb),
       .o_misalign (mem_misalign),
       .i_trap   (trap),
-      .o_adr    (bad_adr),
       //External interface
-      .o_wb_adr   (o_dbus_adr),
       .o_wb_dat   (o_dbus_dat),
       .o_wb_sel   (o_dbus_sel),
       .o_wb_we    (o_dbus_we ),
diff --git a/serv.core b/serv.core
index f5bb7dd..7b93e3f 100644
--- a/serv.core
+++ b/serv.core
@@ -11,6 +11,7 @@ filesets:
       - rtl/ser_eq.v
       - rtl/ser_lt.v
       - rtl/ser_shift.v
+      - rtl/serv_bufreg.v
       - rtl/serv_alu.v
       - rtl/serv_csr.v
       - rtl/serv_ctrl.v