diff --git a/rtl/serv_csr.v b/rtl/serv_csr.v
index 33ecc3a..65d27f0 100644
--- a/rtl/serv_csr.v
+++ b/rtl/serv_csr.v
@@ -8,8 +8,7 @@ module serv_csr
    input       i_trap,
    input       i_pc,
    input       i_mtval,
-   input       i_load_misaligned,
-   input       i_store_misaligned,
+   input [3:0] i_mcause,
    input       i_d,
    output      o_q);
 
@@ -23,15 +22,17 @@ module serv_csr
     mie SCWi
     */
 
-   reg [31:0] 	mtvec = 32'h0;
-   reg [31:0] 	mepc  = 32'h0;
-   reg [31:0] 	mtval = 32'h0;
-   reg [31:0] 	mcause = 32'h0;
+   reg [31:0] 	mtvec    = 32'h0;
+   reg [31:0] 	mscratch = 32'h0;
+   reg [31:0] 	mepc     = 32'h0;
+   reg [31:0] 	mcause   = 32'h0;
+   reg [31:0] 	mtval    = 32'h0;
 
-   wire 	mtvec_en  = i_en & (i_csr_sel == CSR_SEL_MTVEC);
-   wire 	mepc_en   = i_en & (i_trap | (i_csr_sel == CSR_SEL_MEPC));
-   wire 	mcause_en = i_en & (i_csr_sel == CSR_SEL_MCAUSE);
-   wire 	mtval_en  = i_en & (i_trap | (i_csr_sel == CSR_SEL_MTVAL));
+   wire 	mtvec_en    = i_en & (i_csr_sel == CSR_SEL_MTVEC);
+   wire 	mscratch_en = i_en & (i_csr_sel == CSR_SEL_MSCRATCH);
+   wire 	mepc_en     = i_en & (i_trap | (i_csr_sel == CSR_SEL_MEPC));
+   wire 	mcause_en   = i_en & (i_csr_sel == CSR_SEL_MCAUSE);
+   wire 	mtval_en    = i_en & (i_trap | (i_csr_sel == CSR_SEL_MTVAL));
 
    wire 	csr_in;
    wire 	csr_out;
@@ -42,19 +43,22 @@ module serv_csr
 		   (i_csr_source == CSR_SOURCE_CSR) ? csr_out :
 		   1'bx;
 
-   assign csr_out = (i_csr_sel == CSR_SEL_MTVEC)  ? mtvec[0]  :
-		    (i_csr_sel == CSR_SEL_MEPC)   ? mepc[0]   :
-		    (i_csr_sel == CSR_SEL_MCAUSE) ? mcause[0] :
-		    (i_csr_sel == CSR_SEL_MTVAL)  ? mtval[0]  :
+   assign csr_out = (i_csr_sel == CSR_SEL_MTVEC)    ? mtvec[0]  :
+		    (i_csr_sel == CSR_SEL_MSCRATCH) ? mscratch[0]   :
+		    (i_csr_sel == CSR_SEL_MEPC)    ? mepc[0]   :
+		    (i_csr_sel == CSR_SEL_MCAUSE)  ? mcause[0] :
+		    (i_csr_sel == CSR_SEL_MTVAL)   ? mtval[0]  :
 		    1'bx;
 
+
    assign o_q = csr_out;
 
    always @(posedge i_clk) begin
-      if (i_load_misaligned)
-	mcause[3:0] <= 4'd4;
-      if (i_store_misaligned)
-	mcause[3:0] <= 4'd6;
+      if (i_trap)
+	mcause[3:0] <= i_mcause;
+
+      if (mscratch_en)
+	mscratch <= {csr_in, mscratch[31:1]};
 
       if (mtvec_en)
 	mtvec <= {csr_in, mtvec[31:1]};
diff --git a/rtl/serv_decode.v b/rtl/serv_decode.v
index 928dd16..dae1068 100644
--- a/rtl/serv_decode.v
+++ b/rtl/serv_decode.v
@@ -39,6 +39,9 @@ module serv_decode
    output 	    o_csr_en,
    output reg [2:0] o_csr_sel,
    output reg [1:0] o_csr_source,
+   output reg [3:0] o_csr_mcause,
+   output 	    o_csr_imm,
+   output 	    o_csr_d_sel,
    output reg [2:0] o_funct3,
    output reg 	    o_imm,
    output 	    o_offset_source,
@@ -75,7 +78,7 @@ module serv_decode
    wire      csr_op;
    wire      slt_op;
    wire      branch_op;
-
+   wire      e_op;
    wire      jump_misaligned;
 
    reg       signbit;
@@ -93,6 +96,8 @@ module serv_decode
 
    assign branch_op = (opcode == OP_BRANCH);
 
+   assign e_op = (opcode == OP_SYSTEM) & !(|o_funct3);
+
    assign o_ctrl_pc_en  = running | o_ctrl_trap;
    assign o_ctrl_jump = (opcode == OP_JAL) |
                         (opcode == OP_JALR) |
@@ -126,7 +131,8 @@ module serv_decode
 
    assign o_alu_cmp_neg = branch_op & o_funct3[0];
 
-   assign o_csr_en = ((opcode == OP_SYSTEM) & (|o_funct3) | o_ctrl_mret | o_ctrl_trap) & (running | o_ctrl_trap);
+   assign o_csr_en = ((((opcode == OP_SYSTEM) & (|o_funct3)) |
+		     o_ctrl_mret) & running) | o_ctrl_trap;
 
    always @(o_funct3) begin
       casez (o_funct3)
@@ -162,11 +168,12 @@ module serv_decode
 	2'b11   : o_csr_source = CSR_SOURCE_CLR;
 	default : o_csr_source = 2'bxx;
       endcase
-      if ((o_rf_rs1_addr == 5'd0) | o_ctrl_trap | o_ctrl_mret)
+      if (((o_rf_rs1_addr == 5'd0) & o_funct3[1]) | o_ctrl_trap | o_ctrl_mret)
 	o_csr_source = CSR_SOURCE_CSR;
 
       casez(imm[31:20])
 	12'h305 : o_csr_sel = CSR_SEL_MTVEC;
+	12'h340 : o_csr_sel = CSR_SEL_MSCRATCH;
 	12'h341 : o_csr_sel = CSR_SEL_MEPC;
 	12'h342 : o_csr_sel = CSR_SEL_MCAUSE;
 	12'h343 : o_csr_sel = CSR_SEL_MTVAL;
@@ -184,6 +191,10 @@ module serv_decode
       if (o_ctrl_mret)
 	o_csr_sel = CSR_SEL_MEPC;
    end
+
+   assign o_csr_imm = (cnt < 5) ? o_rf_rs1_addr[cnt] : 1'b0;
+   assign o_csr_d_sel = o_funct3[2];
+
    assign o_alu_shamt_en = (cnt < 5) & (state == INIT);
    assign o_alu_sh_signed = signbit;
    assign o_alu_sh_right = o_funct3[2];
@@ -284,6 +295,19 @@ module serv_decode
 
    assign o_ctrl_trap = (state == TRAP);
 
+   always @(i_mem_misalign, o_mem_cmd, e_op, imm) begin
+      o_csr_mcause[3:0] <= 4'd0;
+      if (i_mem_misalign & !o_mem_cmd)
+	o_csr_mcause[3:0] <= 4'd4;
+      if (i_mem_misalign & o_mem_cmd)
+	o_csr_mcause[3:0] <= 4'd6;
+      if (e_op & !imm[20])
+	o_csr_mcause[3:0] <= 4'd11;
+      if (e_op & imm[20])
+	o_csr_mcause[3:0] <= 4'd3;
+      //if (o_ctrl_jump & i_ctrl_misalign)
+   end
+
    always @(posedge clk) begin
       state <= state;
       case (state)
@@ -294,13 +318,15 @@ module serv_decode
                   slt_op | (opcode == OP_JAL) | (opcode == OP_JALR) |
                   mem_op | shift_op)
 		state <= INIT;
+	      if (e_op)
+		state <= TRAP;
 	   end
 	   if (i_mem_dbus_ack)
 	     state <= RUN;
         end
         INIT : begin
            if (cnt_done)
-             state <= (i_mem_misalign | (o_ctrl_jump & i_ctrl_misalign) /*| jal_misalign*/) ? TRAP :
+             state <= (i_mem_misalign | (o_ctrl_jump & i_ctrl_misalign)) ? TRAP :
 		      mem_op ? IDLE : RUN;
         end
         RUN : begin
diff --git a/rtl/serv_mem_if.v b/rtl/serv_mem_if.v
index bb76f67..8bad984 100644
--- a/rtl/serv_mem_if.v
+++ b/rtl/serv_mem_if.v
@@ -96,7 +96,7 @@ module serv_mem_if
 	misalign[0] <= (!is_byte & adr);
       if (init_r & !init_2r)
 	misalign[1] <= (is_word & adr);
-      if (i_trap)
+      if (!i_en)
 	misalign <= 2'b00;
 
       if (i_en & !en_r)
diff --git a/rtl/serv_params.vh b/rtl/serv_params.vh
index 4b36d6a..6f014ac 100644
--- a/rtl/serv_params.vh
+++ b/rtl/serv_params.vh
@@ -36,11 +36,11 @@ localparam [0:0]
 */
 
 localparam [2:0]
-  CSR_SEL_MTVEC   = 3'd0,
-  CSR_SEL_MEPC    = 3'd1,
-  CSR_SEL_MTVAL   = 3'd2,
-  CSR_SEL_MCAUSE  = 3'd3
-;
+  CSR_SEL_MSCRATCH = 3'd0,
+  CSR_SEL_MEPC     = 3'd1,
+  CSR_SEL_MCAUSE   = 3'd2,
+  CSR_SEL_MTVAL    = 3'd3,
+  CSR_SEL_MTVEC    = 3'd4;
 
 localparam [1:0]
   CSR_SOURCE_CSR = 2'b00,
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index ccb0a29..b88bdf4 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -108,6 +108,10 @@ module serv_top
    wire 	 csr_en;
    wire [2:0] 	 csr_sel;
    wire [1:0]	 csr_source;
+   wire 	 csr_imm;
+   wire 	 csr_d_sel;
+
+   wire [3:0] 	 mcause;
 
    parameter RESET_PC = 32'd8;
 
@@ -152,6 +156,9 @@ module serv_top
       .o_csr_en       (csr_en),
       .o_csr_sel      (csr_sel),
       .o_csr_source   (csr_source),
+      .o_csr_mcause   (mcause),
+      .o_csr_imm      (csr_imm),
+      .o_csr_d_sel    (csr_d_sel),
       .o_imm          (imm),
       .o_offset_source (offset_source),
       .o_op_b_source  (op_b_source),
@@ -256,9 +263,8 @@ module serv_top
       .i_trap       (trap),
       .i_pc         (o_ibus_adr[0]),
       .i_mtval      (mem_misalign ? o_dbus_adr[0] : bad_pc),
-      .i_load_misaligned (mem_misalign & !mem_cmd),
-      .i_store_misaligned (mem_misalign & mem_cmd),
-      .i_d          (rs1/* FIXME csr_d*/),
+      .i_mcause     (mcause),
+      .i_d          (csr_d_sel ? csr_imm : rs1),
       .o_q          (csr_rd));
 
 `ifdef RISCV_FORMAL