diff --git a/debug_unit.sv b/debug_unit.sv
index 00626dac..7a51d411 100644
--- a/debug_unit.sv
+++ b/debug_unit.sv
@@ -70,6 +70,8 @@ module riscv_debug_unit
   input  logic [31:0] pc_ex_i,
 
   input  logic        data_load_event_i,
+  input  logic        instr_valid_id_i,
+
 
   input  logic        branch_in_ex_i,
   input  logic        branch_taken_i,
@@ -297,7 +299,7 @@ module riscv_debug_unit
       end
 
       RD_DBGS: begin
-        unique case (debug_addr_i[2:2])
+        unique case (addr_q[2:2])
           1'b0: dbg_rdata = npc_int; // DBG_NPC
           1'b1: dbg_rdata = ppc_int; // DBG_PPC
           default:;
@@ -380,8 +382,10 @@ module riscv_debug_unit
         stall_o        = 1'b1;
         debug_halted_o = 1'b1;
 
-        if (dbg_resume)
+        if (dbg_resume) begin
           stall_ns = RUNNING;
+          stall_o  = 1'b0;
+        end
       end
     endcase
 
@@ -448,7 +452,7 @@ module riscv_debug_unit
           pc_tracking_fsm_ns = IDEX;
       end else if (data_load_event_i) begin
         // for p.elw
-        pc_tracking_fsm_ns = IDEX;
+        pc_tracking_fsm_ns = instr_valid_id_i ? IDEX : IFEX;
       end
     end
   end
diff --git a/riscv_core.sv b/riscv_core.sv
index d3360709..fb73ead0 100644
--- a/riscv_core.sv
+++ b/riscv_core.sv
@@ -274,7 +274,7 @@ module riscv_core
   // interface to finish loading instructions
   assign core_busy_o = (data_load_event_ex & data_req_o) ? if_busy : (if_busy | ctrl_busy | lsu_busy);
 
-  assign dbg_busy = dbg_req | dbg_csr_req | dbg_jump_req | dbg_reg_wreq;
+  assign dbg_busy = dbg_req | dbg_csr_req | dbg_jump_req | dbg_reg_wreq | debug_req_i;
 
   assign clock_en = clock_en_i | core_busy_o | dbg_busy;
 
@@ -798,6 +798,7 @@ module riscv_core
     .pc_ex_i           ( pc_ex              ), // PC of last executed branch (in EX stage) or p.elw
 
     .data_load_event_i ( data_load_event_ex ),
+    .instr_valid_id_i  ( instr_valid_id     ),
 
     .branch_in_ex_i    ( branch_in_ex       ),
     .branch_taken_i    ( branch_decision    ),