diff --git a/rtl/serv_decode.v b/rtl/serv_decode.v
index 05fe526..db44ff8 100644
--- a/rtl/serv_decode.v
+++ b/rtl/serv_decode.v
@@ -308,6 +308,8 @@ module serv_decode
    always @(posedge clk) begin
       if (state == INIT)
 	o_ctrl_jump <= take_branch;
+      if (state == IDLE)
+	o_ctrl_jump <= 1'b0;
 
       mtip_r <= i_mtip;
 
diff --git a/rtl/serv_mem_if.v b/rtl/serv_mem_if.v
index 28ca129..e2bca59 100644
--- a/rtl/serv_mem_if.v
+++ b/rtl/serv_mem_if.v
@@ -14,6 +14,7 @@ module serv_mem_if
    output wire 	      o_rd,
    output wire 	      o_misalign,
    input wire 	      i_trap,
+   output wire 	      o_adr,
    //External interface
    output wire [31:0] o_wb_adr,
    output wire [31:0] o_wb_dat,
@@ -39,6 +40,7 @@ module serv_mem_if
    wire 	 dat2_en;
    wire 	 dat3_en;
 
+   assign o_adr = adr;
 
    ser_add ser_add_rs1_plus_imm
      (
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index 2600b98..8451b45 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -106,6 +106,7 @@ module serv_top
    wire 	 mem_misalign;
 
    wire 	 bad_pc;
+   wire 	 bad_adr;
 
    wire 	 csr_en;
    wire [2:0] 	 csr_sel;
@@ -257,6 +258,7 @@ module serv_top
       .o_rd     (mem_rd),
       .o_misalign (mem_misalign),
       .i_trap   (trap),
+      .o_adr    (bad_adr),
       //External interface
       .o_wb_adr   (o_dbus_adr),
       .o_wb_dat   (o_dbus_dat),
@@ -277,7 +279,7 @@ module serv_top
       .i_csr_source (csr_source),
       .i_trap       (trap),
       .i_pc         (o_ibus_adr[0]),
-      .i_mtval      (mem_misalign ? o_dbus_adr[0] : bad_pc),
+      .i_mtval      (mem_misalign ? bad_adr : bad_pc),
       .i_mcause     (mcause),
       .i_d          (csr_d_sel ? csr_imm : rs1),
       .o_q          (csr_rd));