diff --git a/rtl/ser_lt.v b/rtl/ser_lt.v
index 799cff5..eefe917 100644
--- a/rtl/ser_lt.v
+++ b/rtl/ser_lt.v
@@ -1,19 +1,20 @@
 `default_nettype none
 module ser_lt
   (
-   input wire i_clk,
-   input wire i_a,
-   input wire i_b,
-   input wire i_clr,
-   output reg o_q);
+   input wire  i_clk,
+   input wire  i_a,
+   input wire  i_b,
+   input wire  i_clr,
+   input wire  i_sign,
+   output wire o_q);
 
-   reg        lt_r = 1'b0;
+   reg        lt_r;
+
+   wire       lt = (i_sign ? (i_a & ~i_b) : (~i_a & i_b)) | ((i_a == i_b) & lt_r);
+   assign o_q = lt;
 
-   wire       lt = (~i_a & i_b) | ((i_a == i_b) & lt_r);
    always @(posedge i_clk) begin
       lt_r <= lt & ~i_clr;
-      if (~i_clr)
-        o_q <= lt;
    end
 
 endmodule
diff --git a/rtl/ser_shift.v b/rtl/ser_shift.v
index ef9a261..7c3622a 100644
--- a/rtl/ser_shift.v
+++ b/rtl/ser_shift.v
@@ -2,6 +2,7 @@
 module ser_shift
   (
    input wire 	    i_clk,
+   input wire 	    i_rst,
    input wire 	    i_load,
    input wire [4:0] i_shamt,
    input wire 	    i_signed,
@@ -17,6 +18,7 @@ module ser_shift
 
    shift_reg #(.LEN (32)) sh_reg
      (.clk (i_clk),
+      .i_rst (i_rst),
       .i_en (i_load),
       .i_d  (i_d),
       .o_q  (shiftreg[0]),
diff --git a/rtl/serv_alu.v b/rtl/serv_alu.v
index 14902dc..59dbcc1 100644
--- a/rtl/serv_alu.v
+++ b/rtl/serv_alu.v
@@ -7,6 +7,7 @@ module serv_alu
    input wire 	    i_rs1,
    input wire 	    i_op_b,
    input wire 	    i_init,
+   input wire 	    i_cnt_done,
    input wire 	    i_sub,
    input wire 	    i_cmp_sel,
    input wire 	    i_cmp_neg,
@@ -25,11 +26,12 @@ module serv_alu
    wire        result_lt;
    wire        result_sh;
 
+   reg 	       result_lt_r;
+
    wire [4:0]  shamt;
 
    reg         en_r;
    wire        v;
-   reg         msb_lt = 1'b0;
    reg         init_r;
    wire        shamt_l;
    wire        shamt_ser;
@@ -49,6 +51,7 @@ module serv_alu
 
    shift_reg #(.LEN (5)) shamt_reg
      (.clk (clk),
+      .i_rst (i_rst),
       .i_en (i_shamt_en),
       .i_d  (shamt_ser),
       .o_q  (shamt[0]),
@@ -57,6 +60,7 @@ module serv_alu
    ser_shift shift
      (
       .i_clk (clk),
+      .i_rst (i_rst),
       .i_load (i_init),
       .i_shamt (shamt),
       .i_signed (i_sh_signed),
@@ -102,17 +106,17 @@ module serv_alu
       .i_a   (i_rs1),
       .i_b   (i_op_b),
       .i_clr (!i_init),
+      .i_sign (i_cnt_done & !i_cmp_uns),
       .o_q   (result_lt));
 
    reg last_eq;
 
-   wire       result_lt2 = last_eq ? result_lt : msb_lt;
    assign plus_1 = i_en & !en_r;
-   assign o_cmp = i_cmp_neg^((i_cmp_sel == ALU_CMP_EQ) ? result_eq : result_lt2);
+   assign o_cmp = i_cmp_neg^((i_cmp_sel == ALU_CMP_EQ) ? (result_eq & (i_rs1 == i_op_b)) : result_lt);
 
    assign o_rd = (i_rd_sel == ALU_RESULT_ADD) ? result_add :
                  (i_rd_sel == ALU_RESULT_SR)  ? result_sh :
-                 (i_rd_sel == ALU_RESULT_LT)  ? (result_lt2 & init_r & ~i_init) :
+                 (i_rd_sel == ALU_RESULT_LT)  ? (result_lt_r & init_r & ~i_init) :
                  (i_rd_sel == ALU_RESULT_XOR) ? i_rs1^i_op_b :
                  (i_rd_sel == ALU_RESULT_OR)  ? i_rs1|i_op_b :
                  (i_rd_sel == ALU_RESULT_AND) ? i_rs1&i_op_b :
@@ -121,7 +125,7 @@ module serv_alu
    always @(posedge clk) begin
       if (i_init) begin
          last_eq <= i_rs1 == i_op_b;
-         msb_lt <= i_cmp_uns ? (~i_rs1 & i_op_b) : (i_rs1 & ~i_op_b);
+	 result_lt_r <= result_lt;
       end
 
       en_r <= i_en;
diff --git a/rtl/serv_ctrl.v b/rtl/serv_ctrl.v
index 4a18f26..a9fc79d 100644
--- a/rtl/serv_ctrl.v
+++ b/rtl/serv_ctrl.v
@@ -56,6 +56,7 @@ module serv_ctrl
    pc_reg
      (
       .clk (clk),
+      .i_rst (i_rst),
       .i_en (i_pc_en),
       .i_d  (new_pc),
       .o_q  (pc),
diff --git a/rtl/serv_decode.v b/rtl/serv_decode.v
index a8723af..ecd5439 100644
--- a/rtl/serv_decode.v
+++ b/rtl/serv_decode.v
@@ -12,7 +12,7 @@ module serv_decode
    output wire 	     o_cnt_done,
    output wire 	     o_ctrl_en,
    output wire 	     o_ctrl_pc_en,
-   output wire 	     o_ctrl_jump,
+   output reg 	     o_ctrl_jump,
    output wire 	     o_ctrl_jalr,
    output wire 	     o_ctrl_auipc,
    output wire 	     o_ctrl_lui,
@@ -103,7 +103,7 @@ module serv_decode
    assign e_op = (opcode[4:2] == 3'b111) & !(|o_funct3);
 
    assign o_ctrl_pc_en  = running | o_ctrl_trap;
-   assign o_ctrl_jump = (opcode[4:2] == 3'b110) & (opcode[0] | i_alu_cmp);
+   wire take_branch = (opcode[4:2] == 3'b110) & (opcode[0] | i_alu_cmp);
 
    assign o_ctrl_jalr = opcode[4] & (opcode[2:0] == 3'b001);
 
@@ -306,6 +306,9 @@ module serv_decode
    reg 	pending_irq;
 
    always @(posedge clk) begin
+      if (state == INIT)
+	o_ctrl_jump <= take_branch;
+
       mtip_r <= i_mtip;
 
       if (i_mtip & !mtip_r & i_timer_irq_en)
@@ -327,7 +330,7 @@ module serv_decode
 	   stage_one_done <= 1'b1;
 
            if (cnt_done)
-             state <= (i_mem_misalign | (o_ctrl_jump & i_ctrl_misalign)) ? TRAP :
+             state <= (i_mem_misalign | (take_branch & i_ctrl_misalign)) ? TRAP :
 		      mem_op ? IDLE : RUN;
         end
         RUN : begin
@@ -348,6 +351,9 @@ module serv_decode
       if (i_rst) begin
 	 state <= IDLE;
 	 cnt   <= 5'd0;
+	 pending_irq <= 1'b0;
+	 stage_one_done <= 1'b0;
+	 o_ctrl_jump <= 1'b0;
       end
    end
 endmodule
diff --git a/rtl/serv_mem_if.v b/rtl/serv_mem_if.v
index 279b4a1..28ca129 100644
--- a/rtl/serv_mem_if.v
+++ b/rtl/serv_mem_if.v
@@ -19,7 +19,7 @@ module serv_mem_if
    output wire [31:0] o_wb_dat,
    output wire [3:0]  o_wb_sel,
    output wire 	      o_wb_we ,
-   output reg 	      o_wb_cyc = 1'b0,
+   output reg 	      o_wb_cyc,
    input wire [31:0]  i_wb_rdt,
    input wire 	      i_wb_ack);
 
@@ -50,13 +50,16 @@ module serv_mem_if
       .q   (adr),
       .o_v ());
 
-   shift_reg #(32) shift_reg_adr
+   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_en & i_trap)),
       .i_d   (adr),
-      .o_q   (o_wb_adr[0]),
-      .o_par (o_wb_adr[31:1])
+      .o_q   (o_wb_adr[2]),
+      .o_par (o_wb_adr[31:3])
       );
 
    wire 	 dat_cur = (dat_sel == 3) ? dat3[0] :
@@ -144,5 +147,10 @@ module serv_mem_if
       else if (init_r & !i_init & !i_trap) begin //Optimize?
          o_wb_cyc <= 1'b1;
       end
+      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_regfile.v b/rtl/serv_regfile.v
index ee44597..b9d3711 100644
--- a/rtl/serv_regfile.v
+++ b/rtl/serv_regfile.v
@@ -2,6 +2,7 @@
 module serv_regfile
   (
    input wire 	    i_clk,
+   input wire 	    i_rst,
    input wire 	    i_go,
    output reg 	    o_ready,
    input wire 	    i_rd_en,
@@ -17,6 +18,10 @@ module serv_regfile
    always @(posedge i_clk) begin
      o_ready <= t;
      t <= i_go;
+      if (i_rst) begin
+	 o_ready <= 1'b0;
+	 t <= 1'b0;
+      end
    end
 
    reg rd_r;
@@ -43,6 +48,9 @@ module serv_regfile
 	 rs2 <= rdata[1];
       end
       rs1_r <= rs1_tmp;
+      if (i_rst) begin
+	 wcnt <= 5'd0;
+      end
    end
 
    wire rs1_tmp = (rs1_en ? rdata[0] : rs1);
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index 90df991..2600b98 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -215,6 +215,7 @@ module serv_top
       .i_rs1      (rs1),
       .i_op_b     (op_b),
       .i_init     (alu_init),
+      .i_cnt_done (cnt_done),
       .i_sub      (alu_sub),
       .i_cmp_sel  (alu_cmp_sel),
       .i_cmp_neg  (alu_cmp_neg),
@@ -229,6 +230,7 @@ module serv_top
    serv_regfile regfile
      (
       .i_clk      (clk),
+      .i_rst      (i_rst),
       .i_go       (i_ibus_ack | i_dbus_ack),
       .o_ready    (rf_ready),
       .i_rd_en    (rd_en),
diff --git a/rtl/shift_reg.v b/rtl/shift_reg.v
index 0b3d504..9288265 100644
--- a/rtl/shift_reg.v
+++ b/rtl/shift_reg.v
@@ -3,6 +3,7 @@ module shift_reg
     parameter INIT = 0)
   (
    input wire 		 clk,
+   input wire 		 i_rst,
    input wire 		 i_en,
    input wire 		 i_d,
    output wire 		 o_q,
@@ -12,6 +13,8 @@ module shift_reg
    assign o_q = data[0];
    assign o_par = data[LEN-1:1];
    always @(posedge clk)
-     if (i_en)
+     if (i_rst)
+       data <= INIT;
+     else if (i_en)
        data <= {i_d, data[LEN-1:1]};
 endmodule