diff --git a/doc/index.rst b/doc/index.rst
index cf9fefc..c8cd221 100644
--- a/doc/index.rst
+++ b/doc/index.rst
@@ -38,12 +38,10 @@ serv_alu
 
 .. image:: serv_alu.png
 
-serv_alu handles alu and shift operations. The first input operand (A) comes from i_rs1 and the second operand (B) comes from i_rs2 or i_imm depending on the type of operation. The data passes through the add/sub or bool logic unit and finally ends up in o_rd to be written to the destination register. The output o_cmp is used for conditional branches to decide whether or not to take the branch.
+serv_alu handles alu operations. The first input operand (A) comes from i_rs1 and the second operand (B) comes from i_rs2 or i_imm depending on the type of operation. The data passes through the add/sub or bool logic unit and finally ends up in o_rd to be written to the destination register. The output o_cmp is used for conditional branches to decide whether or not to take the branch.
 
 The add/sub unit can do additions A+B or subtractions A-B by converting it to A+B̅+1. Subtraction mode (i_sub = 1) is also used for the comparisions in the slt* and conditional branch instructions. The +1 used in subtraction mode is done by preloading the carry input with 1. Less-than comparisons are handled by converting the expression A<B to A-B<0 and checking the MSB, which will be set when the result is less than 0. This however requires sign-extending the operands to 33-bit inputs. For signed operands (when i_cmp_sig is set), the extra bit is the same as the MSB. For unsigned, the extra bit is always 0. Because the ALU is only active for 32 cycles, the 33rd bit must be calculated in parallel to the ordinary addition. The result from this operations is available in result_lt. For equality checks, result_eq checks that all bits are 0 from the subtraction.
 
-For shift operations, the data to be shifted resides in bufreg. The shift control unit in the ALU keeps track of how many steps to shift the bufreg.
-
 .. image:: serv_alu_int.png
 
 serv_bufreg
@@ -111,6 +109,8 @@ During load operations, the data from the bus is latched into the shift register
 
 When SERV is built with `WITH_CSR`, there is also logic to detect misaligned accesses which asserts the o_misalign flag to the core.
 
+The shift register used for stores and loads are also used to keep track of the number of steps to shift for left/right shift operations. In this mode, the six LSB of the register is loaded with the shift amount during the init stage and the used as a down counter which raises o_sh_done and o_sh_done_r when the number of shift steps have been completed.
+
 .. image:: serv_mem_if_int.png
 
 .. _`Data bus byte mask`:
diff --git a/doc/serv_alu.png b/doc/serv_alu.png
index 7edd15b..33f0828 100644
Binary files a/doc/serv_alu.png and b/doc/serv_alu.png differ
diff --git a/doc/serv_alu_int.png b/doc/serv_alu_int.png
index 1d8c04b..a58ca45 100644
Binary files a/doc/serv_alu_int.png and b/doc/serv_alu_int.png differ
diff --git a/doc/serv_dataflow.png b/doc/serv_dataflow.png
index 1d70155..6bf1237 100644
Binary files a/doc/serv_dataflow.png and b/doc/serv_dataflow.png differ
diff --git a/doc/serv_mem_if.png b/doc/serv_mem_if.png
index babcf55..0ffc48b 100644
Binary files a/doc/serv_mem_if.png and b/doc/serv_mem_if.png differ
diff --git a/doc/serv_mem_if_int.png b/doc/serv_mem_if_int.png
index d23ec6d..ba7c43d 100644
Binary files a/doc/serv_mem_if_int.png and b/doc/serv_mem_if_int.png differ
diff --git a/rtl/serv_alu.v b/rtl/serv_alu.v
index d8bd1e9..400c181 100644
--- a/rtl/serv_alu.v
+++ b/rtl/serv_alu.v
@@ -3,15 +3,10 @@ module serv_alu
   (
    input wire 	    clk,
    //State
-   input wire 	    i_init,
    input wire 	    i_en,
    input wire 	    i_cnt0,
-   input wire 	    i_shamt_en,
    output wire 	    o_cmp,
-   output wire 	    o_sh_done,
-   output wire 	    o_sh_done_r,
    //Control
-   input wire 	    i_op_b_rs2,
    input wire 	    i_sub,
    input wire [1:0] i_bool_op,
    input wire 	    i_cmp_eq,
@@ -19,8 +14,7 @@ module serv_alu
    input wire [3:0] i_rd_sel,
    //Data
    input wire 	    i_rs1,
-   input wire 	    i_rs2,
-   input wire 	    i_imm,
+   input wire 	    i_op_b,
    input wire 	    i_buf,
    output wire 	    o_rd);
 
@@ -28,18 +22,14 @@ module serv_alu
 
    reg 	       cmp_r;
 
-   reg [5:0]   shamt_r;
-
    wire        add_cy;
    reg 	       add_cy_r;
 
-   wire op_b = i_op_b_rs2 ? i_rs2 : i_imm;
-
    //Sign-extended operands
    wire rs1_sx  = i_rs1 & i_cmp_sig;
-   wire op_b_sx = op_b  & i_cmp_sig;
+   wire op_b_sx = i_op_b  & i_cmp_sig;
 
-   wire add_b = op_b^i_sub;
+   wire add_b = i_op_b^i_sub;
 
    assign {add_cy,result_add}   = i_rs1+add_b+add_cy_r;
 
@@ -50,26 +40,18 @@ module serv_alu
    assign o_cmp = i_cmp_eq ? result_eq : result_lt;
 
    localparam [15:0] BOOL_LUT = 16'h8E96;//And, Or, =, xor
-   wire result_bool = BOOL_LUT[{i_bool_op, i_rs1, op_b}];
+   wire result_bool = BOOL_LUT[{i_bool_op, i_rs1, i_op_b}];
 
    assign o_rd = (i_rd_sel[0] & result_add) |
                  (i_rd_sel[1] & i_buf) |
                  (i_rd_sel[2] & cmp_r & i_cnt0) |
                  (i_rd_sel[3] & result_bool);
 
-
-   wire [5:0] shamt = i_init ? {1'b0,op_b,shamt_r[4:1]} : shamt_r-1;
-   assign o_sh_done = shamt[5];
-   assign o_sh_done_r = shamt_r[5];
-
    always @(posedge clk) begin
       add_cy_r <= i_en ? add_cy : i_sub;
 
       if (i_en)
 	cmp_r <= o_cmp;
-
-      if (i_shamt_en)
-	 shamt_r <= shamt;
    end
 
 endmodule
diff --git a/rtl/serv_decode.v b/rtl/serv_decode.v
index 2daee16..8bc1dc4 100644
--- a/rtl/serv_decode.v
+++ b/rtl/serv_decode.v
@@ -6,6 +6,7 @@ module serv_decode
    input wire [31:2] i_wb_rdt,
    input wire 	     i_wb_en,
    //To state
+   output wire 	     o_sh_right,
    output wire 	     o_bne_or_bge,
    output wire 	     o_cond_branch,
    output wire 	     o_e_op,
@@ -19,6 +20,7 @@ module serv_decode
    output wire 	     o_bufreg_rs1_en,
    output wire 	     o_bufreg_imm_en,
    output wire 	     o_bufreg_clr_lsb,
+   output wire 	     o_bufreg_sh_signed,
    //To ctrl
    output wire 	     o_ctrl_jal_or_jalr,
    output wire 	     o_ctrl_utype,
@@ -29,8 +31,6 @@ module serv_decode
    output wire [1:0] o_alu_bool_op,
    output wire 	     o_alu_cmp_eq,
    output wire 	     o_alu_cmp_sig,
-   output wire 	     o_alu_sh_signed,
-   output wire 	     o_alu_sh_right,
    output wire [3:0] o_alu_rd_sel,
    //To mem IF
    output wire 	     o_mem_signed,
@@ -106,6 +106,7 @@ module serv_decode
    //funct3
    //
 
+   assign o_sh_right   = funct3[2];
    assign o_bne_or_bge = funct3[0];
    
    //
@@ -132,6 +133,8 @@ module serv_decode
 
    //opcode & funct3 & imm30
 
+   assign o_bufreg_sh_signed = imm30;
+
    /*
     True for sub, b*, slt*
     False for add*
@@ -184,8 +187,6 @@ module serv_decode
    assign o_alu_cmp_eq = funct3[2:1] == 2'b00;
 
    assign o_alu_cmp_sig = ~((funct3[0] & funct3[1]) | (funct3[1] & funct3[2]));
-   assign o_alu_sh_signed = imm30;
-   assign o_alu_sh_right = funct3[2];
 
    assign o_mem_cmd  = opcode[3];
    assign o_mem_signed = ~funct3[2];
diff --git a/rtl/serv_mem_if.v b/rtl/serv_mem_if.v
index 85a5ac8..1130b8c 100644
--- a/rtl/serv_mem_if.v
+++ b/rtl/serv_mem_if.v
@@ -5,16 +5,21 @@ module serv_mem_if
    input wire 	      i_clk,
    //State
    input wire 	      i_en,
+   input wire 	      i_init,
+   input wire 	      i_cnt_done,
    input wire [1:0]   i_bytecnt,
    input wire [1:0]   i_lsb,
    output wire 	      o_misalign,
+   output wire 	      o_sh_done,
+   output wire 	      o_sh_done_r,
    //Control
    input wire 	      i_mem_op,
+   input wire 	      i_shift_op,
    input wire 	      i_signed,
    input wire 	      i_word,
    input wire 	      i_half,
    //Data
-   input wire 	      i_rs2,
+   input wire 	      i_op_b,
    output wire 	      o_rd,
    //External interface
    output wire [31:0] o_wb_dat,
@@ -27,7 +32,7 @@ module serv_mem_if
 
    wire [2:0] 	 tmp = {1'b0,i_bytecnt}+{1'b0,i_lsb};
 
-   wire 	 dat_en = i_en & !tmp[2];
+   wire 	 dat_en = i_shift_op | (i_en & !tmp[2]);
 
    wire 	 dat_cur =
 		 ((i_lsb == 2'd3) & dat[24]) |
@@ -49,12 +54,30 @@ module serv_mem_if
 
    assign o_wb_dat = dat;
 
-   always @(posedge i_clk) begin
-      if (dat_en)
-	dat <= {i_rs2, dat[31:1]};
+   /* The dat register has three different use cases for store, load and
+    shift operations.
+    store : Data to be written is shifted to the correct position in dat during
+            init by dat_en and is presented on the data bus as o_wb_dat
+    load  : Data from the bus gets latched into dat during i_wb_ack and is then
+            shifted out at the appropriate time to end up in the correct
+            position in rd
+    shift : Data is shifted in during init. After that, the six LSB are used as
+            a downcounter (with bit 5 initially set to 0) that triggers
+            o_sh_done and o_sh_done_r when they wrap around to indicate that
+            the requested number of shifts have been performed
+    */
+   wire [5:0] dat_shamt = (i_shift_op & !i_init) ?
+	      //Down counter mode
+	      dat[5:0]-1 :
+	      //Shift reg mode with optional clearing of bit 5
+	      {dat[6] & !(i_shift_op & i_cnt_done),dat[5:1]};
 
-      if (i_wb_ack)
-	dat <= i_wb_rdt;
+   assign o_sh_done = dat_shamt[5];
+   assign o_sh_done_r = dat[5];
+
+   always @(posedge i_clk) begin
+      if (dat_en | i_wb_ack)
+	dat <= i_wb_ack ? i_wb_rdt : {i_op_b, dat[31:7], dat_shamt};
 
       if (dat_valid)
         signbit <= dat_cur;
diff --git a/rtl/serv_state.v b/rtl/serv_state.v
index ef2302c..547cd75 100644
--- a/rtl/serv_state.v
+++ b/rtl/serv_state.v
@@ -35,9 +35,8 @@ module serv_state
    output reg 	     o_ctrl_jump,
    output wire 	     o_ctrl_trap,
    input wire 	     i_ctrl_misalign,
-   output wire 	     o_alu_shamt_en,
-   input wire 	     i_alu_sh_done,
-   input wire 	     i_alu_sh_done_r,
+   input wire 	     i_sh_done,
+   input wire 	     i_sh_done_r,
    output wire 	     o_dbus_cyc,
    output wire [1:0] o_mem_bytecnt,
    input wire 	     i_mem_misalign,
@@ -70,9 +69,6 @@ module serv_state
    assign cnt4   = (o_cnt[4:2] == 3'd1) & o_cnt_r[0];
    assign o_cnt7 = (o_cnt[4:2] == 3'd1) & o_cnt_r[3];
 
-
-   assign o_alu_shamt_en = o_cnt0to3 | cnt4 | !o_init;
-
    //Take branch for jump or branch instructions (opcode == 1x0xx) if
    //a) It's an unconditional branch (opcode[0] == 1)
    //b) It's a conditional branch (opcode[0] == 0) of type beq,blt,bltu (funct3[0] == 0) and ALU compare is true
@@ -93,7 +89,7 @@ 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_alu_sh_done | !i_sh_right) & init_done) |
+		      ((i_shift_op & (i_sh_done | !i_sh_right) & init_done) |
 		       (i_mem_op & i_dbus_ack) |
 		       (stage_two_req & (i_slt_op | i_branch_op)));
 
@@ -110,7 +106,7 @@ module serv_state
     shift : Shift in during phase 1. Continue shifting between phases (except
             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_shift_op & !stage_two_req & (i_sh_right | i_alu_sh_done_r));
+   assign o_bufreg_en = (o_cnt_en & (o_init | o_ctrl_trap | i_branch_op)) | (i_shift_op & !stage_two_req & (i_sh_right | i_sh_done_r));
 
    assign o_ibus_cyc = ibus_cyc & !i_rst;
 
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index 3113073..7fbd0d9 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -64,6 +64,7 @@ module serv_top
 
    wire [3:0] 	 immdec_ctrl;
 
+   wire          sh_right;
    wire 	 bne_or_bge;
    wire 	 cond_branch;
    wire 	 e_op;
@@ -103,6 +104,7 @@ module serv_top
    wire 	 cnt_done;
 
    wire 	 bufreg_en;
+   wire          bufreg_sh_signed;
    wire 	 bufreg_rs1_en;
    wire 	 bufreg_imm_en;
    wire 	 bufreg_clr_lsb;
@@ -113,11 +115,6 @@ module serv_top
    wire          alu_cmp_eq;
    wire          alu_cmp_sig;
    wire          alu_cmp;
-   wire          alu_shamt_en;
-   wire          alu_sh_signed;
-   wire          alu_sh_right;
-   wire 	 alu_sh_done;
-   wire 	 alu_sh_done_r;
    wire [3:0]    alu_rd_sel;
 
    wire          rs1;
@@ -130,6 +127,8 @@ module serv_top
    wire          mem_word;
    wire          mem_half;
    wire [1:0] 	 mem_bytecnt;
+   wire 	 mem_sh_done;
+   wire 	 mem_sh_done_r;
 
    wire 	 mem_misalign;
 
@@ -152,6 +151,8 @@ module serv_top
 
    wire [1:0]   lsb;
 
+   wire 	op_b = op_b_source ? rs2 : imm;
+
    serv_state
      #(.RESET_STRATEGY (RESET_STRATEGY),
        .WITH_CSR (WITH_CSR))
@@ -177,9 +178,8 @@ module serv_top
       .o_ctrl_jump    (jump),
       .o_ctrl_trap    (trap),
       .i_ctrl_misalign(lsb[1]),
-      .o_alu_shamt_en (alu_shamt_en),
-      .i_alu_sh_done  (alu_sh_done),
-      .i_alu_sh_done_r (alu_sh_done_r),
+      .i_sh_done      (mem_sh_done),
+      .i_sh_done_r    (mem_sh_done_r),
       .o_mem_bytecnt  (mem_bytecnt),
       .i_mem_misalign (mem_misalign),
       //Control
@@ -188,7 +188,7 @@ module serv_top
       .i_branch_op    (branch_op),
       .i_mem_op       (mem_op),
       .i_shift_op     (shift_op),
-      .i_sh_right (alu_sh_right),
+      .i_sh_right     (sh_right),
       .i_slt_op       (slt_op),
       .i_e_op         (e_op),
       .i_rd_op        (rd_op),
@@ -219,10 +219,12 @@ module serv_top
       .o_shift_op         (shift_op),
       .o_slt_op           (slt_op),
       .o_rd_op            (rd_op),
+      .o_sh_right         (sh_right),
       //To bufreg
       .o_bufreg_rs1_en    (bufreg_rs1_en),
       .o_bufreg_imm_en    (bufreg_imm_en),
       .o_bufreg_clr_lsb   (bufreg_clr_lsb),
+      .o_bufreg_sh_signed (bufreg_sh_signed),
       //To ctrl
       .o_ctrl_jal_or_jalr (jal_or_jalr),
       .o_ctrl_utype       (utype),
@@ -234,8 +236,6 @@ module serv_top
       .o_alu_bool_op      (alu_bool_op),
       .o_alu_cmp_eq       (alu_cmp_eq),
       .o_alu_cmp_sig      (alu_cmp_sig),
-      .o_alu_sh_signed    (alu_sh_signed),
-      .o_alu_sh_right     (alu_sh_right),
       .o_alu_rd_sel       (alu_rd_sel),
       //To mem IF
       .o_mem_cmd          (o_dbus_we),
@@ -285,10 +285,10 @@ module serv_top
       .i_init   (init),
       .o_lsb    (lsb),
       //Control
-      .i_sh_signed (alu_sh_signed),
-      .i_rs1_en (bufreg_rs1_en),
-      .i_imm_en (bufreg_imm_en),
-      .i_clr_lsb (bufreg_clr_lsb),
+      .i_sh_signed (bufreg_sh_signed),
+      .i_rs1_en    (bufreg_rs1_en),
+      .i_imm_en    (bufreg_imm_en),
+      .i_clr_lsb   (bufreg_clr_lsb),
       //Data
       .i_rs1    (rs1),
       .i_imm    (imm),
@@ -329,14 +329,9 @@ module serv_top
       .clk        (clk),
       //State
       .i_en       (cnt_en),
-      .i_init       (init),
       .i_cnt0     (cnt0),
-      .i_shamt_en (alu_shamt_en),
       .o_cmp      (alu_cmp),
-      .o_sh_done  (alu_sh_done),
-      .o_sh_done_r (alu_sh_done_r),
       //Control
-      .i_op_b_rs2 (op_b_source),
       .i_sub      (alu_sub),
       .i_bool_op  (alu_bool_op),
       .i_cmp_eq   (alu_cmp_eq),
@@ -344,8 +339,7 @@ module serv_top
       .i_rd_sel   (alu_rd_sel),
       //Data
       .i_rs1      (rs1),
-      .i_rs2      (rs2),
-      .i_imm      (imm),
+      .i_op_b     (op_b),
       .i_buf      (bufreg_q),
       .o_rd       (alu_rd));
 
@@ -403,17 +397,22 @@ module serv_top
       .i_clk      (clk),
       //State
       .i_en       (cnt_en),
+      .i_init     (init),
+      .i_cnt_done (cnt_done),
       .i_bytecnt  (mem_bytecnt),
       .i_lsb      (lsb),
       .o_misalign (mem_misalign),
+      .o_sh_done   (mem_sh_done),
+      .o_sh_done_r (mem_sh_done_r),
       //Control
       .i_mem_op   (mem_op),
+      .i_shift_op (shift_op),
       .i_signed   (mem_signed),
       .i_word     (mem_word),
       .i_half     (mem_half),
       //Data
-      .i_rs2    (rs2),
-      .o_rd     (mem_rd),
+      .i_op_b     (op_b),
+      .o_rd       (mem_rd),
       //External interface
       .o_wb_dat   (o_dbus_dat),
       .o_wb_sel   (o_dbus_sel),