diff --git a/rtl/serv_csr.v b/rtl/serv_csr.v
index 3d8b774..d3b426e 100644
--- a/rtl/serv_csr.v
+++ b/rtl/serv_csr.v
@@ -5,6 +5,10 @@ module serv_csr
    input wire 	    i_run,
    input wire [4:2] i_cnt,
    input wire [3:2] i_cnt_r,
+   input wire 	    i_e_op,
+   input wire 	    i_ebreak,
+   input wire 	    i_mem_cmd,
+   input wire 	    i_mem_misalign,
    //From mpram
    input wire 	    i_rf_csr_out,
    //to mpram
@@ -17,7 +21,6 @@ module serv_csr
    input wire 	    i_mcause_en,
    input wire [1:0] i_csr_source,
    input wire 	    i_trap,
-   input wire [3:0] i_mcause,
    input wire 	    i_d,
    output wire 	    o_q);
 
@@ -70,7 +73,10 @@ module serv_csr
 
       if (i_trap) begin
 	 mcause31  <= timer_irq;
-	 mcause3_0 <= timer_irq ? 4'd7 : i_mcause[3:0];
+	 mcause3_0 <= timer_irq ? 4'd7 :
+		      i_e_op ? {!i_ebreak, 3'b011} :
+		      i_mem_misalign ? {2'b01, i_mem_cmd, 1'b0} :
+		      4'd0;
       end
 
       if (i_mcause_en & i_run) begin
diff --git a/rtl/serv_state.v b/rtl/serv_state.v
index 578d513..2ca0edd 100644
--- a/rtl/serv_state.v
+++ b/rtl/serv_state.v
@@ -13,9 +13,7 @@ module serv_state
    input wire 	     i_mem_op,
    input wire 	     i_shift_op,
    input wire 	     i_slt_op,
-   input wire 	     i_mem_cmd,
    input wire 	     i_e_op,
-   input wire 	     i_ebreak,
    input wire [4:0]  i_rs1_addr,
    output wire 	     o_init,
    output wire 	     o_run,
@@ -31,7 +29,6 @@ module serv_state
    output wire 	     o_dbus_cyc,
    output wire [1:0] o_mem_bytecnt,
    input wire 	     i_mem_misalign,
-   output reg [3:0]  o_csr_mcause,
    output wire 	     o_cnt_done,
    output wire 	     o_bufreg_hold,
    output wire 	     o_csr_imm);
@@ -72,14 +69,6 @@ module serv_state
    //slt*, branch/jump, shift, load/store
    wire two_stage_op = i_slt_op | i_mem_op | i_branch_op | i_shift_op;
 
-   always @(posedge i_clk) begin
-      o_csr_mcause[3:0] <= 4'd0;
-      if (i_mem_misalign)
-	o_csr_mcause[3:0] <= {2'b01, i_mem_cmd, 1'b0};
-      if (i_e_op)
-	o_csr_mcause <= {!i_ebreak,3'b011};
-   end
-
    reg 	stage_two_pending;
 
    reg 	pending_irq;
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index 091ac2d..e098cec 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -126,9 +126,6 @@ module serv_top
    wire [1:0] 	 csr_addr;
    wire 	 csr_pc;
 
-
-   wire [3:0] 	 mcause;
-
    parameter RESET_PC = 32'd8;
 
    wire 	 new_irq;
@@ -151,9 +148,7 @@ module serv_top
       .i_mem_op       (mem_op),
       .i_shift_op     (shift_op),
       .i_slt_op       (slt_op),
-      .i_mem_cmd      (o_dbus_we),
       .i_e_op         (e_op),
-      .i_ebreak       (ebreak),
       .i_rs1_addr     (rs1_addr),
       .o_init         (init),
       .o_run          (run),
@@ -171,8 +166,7 @@ module serv_top
       .o_dbus_cyc     (o_dbus_cyc),
       .o_mem_bytecnt  (mem_bytecnt),
       .i_mem_misalign (mem_misalign),
-      .o_csr_imm      (csr_imm),
-      .o_csr_mcause   (mcause));
+      .o_csr_imm      (csr_imm));
 
    serv_decode decode
      (
@@ -370,6 +364,10 @@ module serv_top
       .i_run        (run),
       .i_cnt        (cnt[4:2]),
       .i_cnt_r      (cnt_r[3:2]),
+      .i_e_op       (e_op),
+      .i_ebreak     (ebreak),
+      .i_mem_cmd    (o_dbus_we),
+      .i_mem_misalign (mem_misalign),
       .i_rf_csr_out (rf_csr_out),
       .o_csr_in     (csr_in),
       .i_mtip       (i_timer_irq),
@@ -379,7 +377,6 @@ module serv_top
       .i_mcause_en  (csr_mcause_en ),
       .i_csr_source (csr_source),
       .i_trap       (trap),
-      .i_mcause     (mcause),
       .i_d          (csr_d_sel ? csr_imm : rs1),
       .o_q          (csr_rd));