diff --git a/rtl/serv_mem_if.v b/rtl/serv_mem_if.v
index e2bca59..8dd9b83 100644
--- a/rtl/serv_mem_if.v
+++ b/rtl/serv_mem_if.v
@@ -12,7 +12,7 @@ module serv_mem_if
    input wire 	      i_rs2,
    input wire 	      i_imm,
    output wire 	      o_rd,
-   output wire 	      o_misalign,
+   output reg 	      o_misalign,
    input wire 	      i_trap,
    output wire 	      o_adr,
    //External interface
@@ -26,8 +26,7 @@ module serv_mem_if
 
    wire          wb_en = o_wb_cyc & i_wb_ack;
    reg           init_r;
-   reg           en_r;
-   reg           en_2r;
+   reg 		 init_2r;
    wire          adr;
    reg           signbit = 1'b0;
 
@@ -58,7 +57,7 @@ module serv_mem_if
      (
       .clk   (i_clk),
       .i_rst (i_rst),
-      .i_en  (i_init | (i_en & i_trap)),
+      .i_en  (i_init | i_trap),
       .i_d   (adr),
       .o_q   (o_wb_adr[2]),
       .o_par (o_wb_adr[31:3])
@@ -77,25 +76,22 @@ module serv_mem_if
    wire is_half = i_funct3[0];
    wire is_byte = !(|i_funct3[1:0]);
 
-   assign o_misalign = (|misalign);
 
 
    wire       upper_half = bytepos[1];
-
+/*
    assign o_wb_sel = (is_word ? 4'b1111 :
 		      is_half ? {{2{upper_half}}, ~{2{upper_half}}} :
 		      4'd1 << bytepos);
-/*
+*/
    assign o_wb_sel[3] = is_word | (is_half & bytepos[1]) | (bytepos == 2'b11);
    assign o_wb_sel[2] = (bytepos == 2'b10) | is_word;
    assign o_wb_sel[1] = ((is_word | is_half) & !bytepos[1]) | (bytepos == 2'b01);
    assign o_wb_sel[0] = (bytepos == 2'b00);
-*/
+
    assign o_wb_we = i_cmd;
    reg [1:0]  bytepos;
 
-   reg 	      init_2r = 1'b0;
-   reg [1:0]  misalign = 2'b00;
 
    wire       wbyte0 = (i_bytecnt == 2'b00);
    wire       wbyte1 = ((i_bytecnt == 2'b01) & !bytepos[0]);
@@ -125,23 +121,14 @@ module serv_mem_if
 	{dat3,dat2,dat1,dat0} <= i_wb_rdt;
 
       if (i_init & !init_r)
-	misalign[0] <= (!is_byte & adr);
-      if (init_r & !init_2r)
-	misalign[1] <= (is_word & adr);
-      if (!i_en)
-	misalign <= 2'b00;
-
-      if (i_en & !en_r)
         bytepos[0] <= adr;
-      if (en_r & !en_2r)
+      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;
 
-
-      en_r <= i_en;
-      en_2r <= en_r;
       init_r <= i_init;
       init_2r <= init_r;
       if (wb_en)