diff --git a/rtl/serv_alu.v b/rtl/serv_alu.v
index 98c7216..3af4e2c 100644
--- a/rtl/serv_alu.v
+++ b/rtl/serv_alu.v
@@ -24,6 +24,8 @@ module serv_alu
    
    wire [4:0] shamt;
    
+   reg        en_r;
+
    shift_reg #(.LEN (5)) shamt_reg
      (.clk (clk),
       .i_en (i_shamt_en),
@@ -39,12 +41,26 @@ module serv_alu
       .i_sr (/*FIXME*/),
       .i_d (i_rs1),
       .o_q (result_sh));
-     
+
+   wire       plus_1 = i_en & !en_r;
+   wire       b_inv_plus_1;
+
+   ser_add ser_add_inv_plus_1
+     (
+      .clk (clk),
+      .a   (~i_op_b),
+      .b   (plus_1),
+      .clr (!i_en),
+      .q   (b_inv_plus_1));
+
+   wire       add_b = sub ? b_inv_plus_1 : i_op_b;
+   wire       sub = i_op[1];
+   
    ser_add ser_add
      (
       .clk (clk),
       .a   (i_rs1),
-      .b   (i_op_b),
+      .b   (add_b),
       .clr (!i_en),
       .q   (result_add));
 
@@ -60,8 +76,12 @@ module serv_alu
                   (i_funct3 == BNE) ? ~result_eq : 1'bx;
 
    assign o_rd = (i_op == ALU_OP_ADD) ? result_add :
+                 (i_op == ALU_OP_SUB) ? result_add :
                  (i_op == ALU_OP_SR)  ? result_sh :
                  1'bx;
+
+   always @(posedge clk)
+     en_r <= i_en;
    
 endmodule
    
diff --git a/rtl/serv_ctrl.v b/rtl/serv_ctrl.v
index 5aafde1..fcd5b92 100644
--- a/rtl/serv_ctrl.v
+++ b/rtl/serv_ctrl.v
@@ -5,6 +5,7 @@ module serv_ctrl
    input         i_en,
    input         i_jump,
    input         i_offset,
+   input         i_auipc,
    output        o_rd,
    output [31:0] o_i_ca_adr,
    output reg    o_i_ca_vld = 1'b0,
@@ -47,7 +48,7 @@ module serv_ctrl
       );
 
    assign new_pc = i_jump ? pc_plus_offset : pc_plus_4;
-   assign o_rd = pc_plus_4;
+   assign o_rd  = i_auipc ? pc_plus_offset : pc_plus_4;
    
    ser_add ser_add_pc_plus_offset
      (
diff --git a/rtl/serv_decode.v b/rtl/serv_decode.v
index 84c036d..fff8359 100644
--- a/rtl/serv_decode.v
+++ b/rtl/serv_decode.v
@@ -6,6 +6,7 @@ module serv_decode
    output reg   o_i_rd_rdy = 1'b1,
    output       o_ctrl_en,
    output       o_ctrl_jump,
+   output       o_ctrl_auipc, 
    output       o_rf_rd_en,
    output [4:0] o_rf_rd_addr,
    output       o_rf_rs_en,
@@ -41,6 +42,8 @@ module serv_decode
      OP_LOAD   = 5'b00000,
      OP_STORE  = 5'b01000,
      OP_OPIMM  = 5'b00100,
+     OP_AUIPC  = 5'b00101,
+     OP_OP     = 5'b01100,
      OP_LUI    = 5'b01101,
      OP_BRANCH = 5'b11000,
      OP_JAL    = 5'b11011;
@@ -60,6 +63,8 @@ module serv_decode
    assign o_ctrl_jump = (opcode == OP_JAL) |
                         ((opcode == OP_BRANCH) & i_alu_cmp);
 
+   assign o_ctrl_auipc = (opcode == OP_AUIPC);
+
    assign o_rf_rd_en = running &
                        (opcode != OP_STORE) &
                        (opcode != OP_BRANCH);
@@ -67,7 +72,7 @@ module serv_decode
    assign o_rf_rs_en = cnt_en /*(running & (opcode == OP_OPIMM)) |
                        (state == SH_INIT) |
                        (state == MEM_INIT)*/;
-   wire      sub = 1'b0; //FIXME
+   wire      sub = (opcode == OP_OP) & i_i_rd_dat[30]; //FIXME: Change for addi?
 
    assign o_alu_en = cnt_en;
    assign o_alu_op = (o_funct3 == 3'b000) ? {1'b0, sub, 1'b0} :
@@ -89,8 +94,10 @@ module serv_decode
    assign o_rf_rs2_addr = i_i_rd_dat[24:20];
    assign o_offset_source = (opcode == OP_JAL) ? OFFSET_SOURCE_IMM : 1'b0;
 
-   assign o_op_b_source = (opcode == OP_OPIMM) ? OP_B_SOURCE_IMM :
-                          (opcode == OP_BRANCH) ? OP_B_SOURCE_RS2 : 1'bx;
+   assign o_op_b_source = (opcode == OP_OPIMM)  ? OP_B_SOURCE_IMM :
+                          (opcode == OP_BRANCH) ? OP_B_SOURCE_RS2 :
+                          (opcode == OP_OP)     ? OP_B_SOURCE_RS2 :
+                          1'bx;
 
    assign o_mem_dat_valid = (o_funct3[1:0] == 2'b00) ? cnt < 8 : 
                             (o_funct3[1:0] == 2'b01) ? cnt < 16 : 1'b1;
@@ -100,7 +107,9 @@ module serv_decode
 
    assign o_rd_source = (opcode == OP_JAL)   ? RD_SOURCE_CTRL :
                         (opcode == OP_OPIMM) ? RD_SOURCE_ALU  :
+                        (opcode == OP_OP)    ? RD_SOURCE_ALU  :
                         (opcode == OP_LUI)   ? RD_SOURCE_IMM  :
+                        (opcode == OP_AUIPC) ? RD_SOURCE_CTRL :
                         (opcode == OP_LOAD)  ? RD_SOURCE_MEM  : 2'bxx;
    
    always @(cnt, opcode, i_i_rd_dat) begin
@@ -114,7 +123,7 @@ module serv_decode
       else if (opcode == OP_OPIMM)
         if      (cnt > 10) o_imm = i_i_rd_dat[31];
         else               o_imm = i_i_rd_dat[cnt+20];
-      else if (opcode == OP_LUI)
+      else if ((opcode == OP_LUI) | (opcode == OP_AUIPC))
         if      (cnt > 11) o_imm = i_i_rd_dat[cnt];
         else               o_imm = 1'b0;
       else if (opcode == OP_LOAD)
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index 32ffff6..ffbd648 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -63,6 +63,7 @@ module serv_top
 
    wire          ctrl_en;
    wire          jump;
+   wire          auipc;
    wire          offset;
    wire          offset_source;
    wire          imm;
@@ -101,6 +102,7 @@ module serv_top
       .o_i_rd_rdy     (o_i_rd_rdy),
       .o_ctrl_en      (ctrl_en),
       .o_ctrl_jump    (jump),
+      .o_ctrl_auipc   (auipc),
       .o_funct3       (funct3),
       .o_alu_en       (alu_en),
       .o_alu_op       (alu_op),
@@ -130,6 +132,7 @@ module serv_top
       .i_en       (ctrl_en),
       .i_jump     (jump),
       .i_offset   (offset),
+      .i_auipc    (auipc),
       .o_rd       (ctrl_rd),
       .o_i_ca_adr (o_i_ca_adr),
       .o_i_ca_vld (o_i_ca_vld),