diff --git a/rtl/debug/jtag_dm.sv b/rtl/debug/jtag_dm.sv
index 014596e..7673fd6 100644
--- a/rtl/debug/jtag_dm.sv
+++ b/rtl/debug/jtag_dm.sv
@@ -161,7 +161,7 @@ module jtag_dm #(
     end
 
     // we always ready to receive dmi request
-    assign dm_ready_o = 1'b1;
+    assign dm_ready_o = ~sbbusy;
     assign dm_valid_o = dm_valid_q;
     assign dm_data_o  = {{DMI_ADDR_BITS{1'b0}}, dm_resp_data_q, 2'b00};  // response successfully
 
diff --git a/rtl/debug/jtag_dtm.sv b/rtl/debug/jtag_dtm.sv
index 327acb9..3a11130 100644
--- a/rtl/debug/jtag_dtm.sv
+++ b/rtl/debug/jtag_dtm.sv
@@ -44,6 +44,7 @@ module jtag_dtm #(
     // from jtag_tap
     input  wire                     tap_req_i,
     input  wire [TAP_REQ_BITS-1:0]  tap_data_i,
+    input  wire                     dmireset_i,
 
     // to jtag_tap
     output wire [DTM_RESP_BITS-1:0] data_o,
@@ -71,6 +72,7 @@ module jtag_dtm #(
     reg[DTM_REQ_BITS-1:0] dtm_data_d;
     reg[DTM_RESP_BITS-1:0] resp_tap_data_q;
     reg is_busy;
+    reg stick_busy;
 
     wire[DTM_RESP_BITS-1:0] busy_response;
     wire dtm_busy;
@@ -90,11 +92,10 @@ module jtag_dtm #(
 
     assign busy_response = {{(DMI_ADDR_BITS + DMI_DATA_BITS){1'b0}}, {(DMI_OP_BITS){1'b1}}};  // op = 2'b11
 
-    assign dmistat = is_busy ? 2'b11 : 2'b00;
+    assign dmistat = (stick_busy | is_busy) ? 2'b11 : 2'b00;
 
     assign op = tap_data_i[DMI_OP_BITS-1:0];
 
-
     always @ (*) begin
         state_d = state_q;
         dtm_valid = 1'b0;
@@ -169,19 +170,22 @@ module jtag_dtm #(
 
     always @ (posedge jtag_tck_i or negedge jtag_trst_ni) begin
         if (!jtag_trst_ni) begin
-            resp_tap_data_q <= {DTM_RESP_BITS{1'b0}};
             is_busy <= 1'b0;
+            stick_busy <= 1'b0;
         end else begin
+            if (dmireset_i) begin
+                stick_busy <= 1'b0;
+            end else if ((state_q != S_IDLE) && tap_req_i) begin
+                stick_busy <= 1'b1;
+            end
             if (state_q != S_IDLE) begin
-                resp_tap_data_q <= busy_response;
                 is_busy <= 1'b1;
             end else begin
-                resp_tap_data_q <= dtm_data_q;
                 is_busy <= 1'b0;
             end
         end
     end
 
-    assign data_o = resp_tap_data_q;
+    assign data_o = (stick_busy | is_busy) ? busy_response : dtm_data_q;
 
 endmodule
diff --git a/rtl/debug/jtag_sba.sv b/rtl/debug/jtag_sba.sv
index 9e6b372..e933d51 100644
--- a/rtl/debug/jtag_sba.sv
+++ b/rtl/debug/jtag_sba.sv
@@ -165,7 +165,7 @@ module jtag_sba(
                 end
             end
 
-            default:;
+            default: state_d = S_IDLE;
         endcase
 
         if ((sbaccess_i > 3'h2) & (state_q != S_IDLE)) begin
@@ -181,7 +181,8 @@ module jtag_sba(
     assign master_addr_o    = master_addr;
     assign master_wdata_o   = master_wdata;
 
-    assign sbdata_valid_o   = master_rvalid_i;
+    assign sbdata_valid_o   = master_rvalid_i &
+                              ((state_q == S_WAIT_READ) || (state_q == S_WAIT_WRITE));
     assign sbdata_o         = master_rdata_i;
     assign sberror_o        = sberror;
     assign sbaddress_o      = sbaddress;
diff --git a/rtl/debug/jtag_tap.sv b/rtl/debug/jtag_tap.sv
index c155860..7f4be37 100644
--- a/rtl/debug/jtag_tap.sv
+++ b/rtl/debug/jtag_tap.sv
@@ -32,6 +32,7 @@ module jtag_tap #(
 
     output wire tap_req_o,
     output wire[TAP_REQ_BITS-1:0] tap_data_o,
+    output wire dmireset_o,
 
     input wire[DTM_RESP_BITS-1:0] dtm_data_i,
     input wire[31:0] idcode_i,
@@ -172,4 +173,20 @@ module jtag_tap #(
 
     assign jtag_tdo_o = jtag_tdo_q;
 
+    reg dmireset_q;
+
+    always @ (posedge jtag_tck_i or negedge jtag_trst_ni) begin
+        if (!jtag_trst_ni) begin
+            dmireset_q <= 1'b0;
+        end else begin
+            if ((tap_state == UPDATE_DR) && (ir_reg == REG_DTMCS)) begin
+                dmireset_q <= shift_reg[16];
+            end else begin
+                dmireset_q <= 1'b0;
+            end
+        end
+    end
+
+    assign dmireset_o = dmireset_q;
+
 endmodule
diff --git a/rtl/debug/jtag_top.sv b/rtl/debug/jtag_top.sv
index 202bac4..49c9d04 100644
--- a/rtl/debug/jtag_top.sv
+++ b/rtl/debug/jtag_top.sv
@@ -124,6 +124,7 @@ module jtag_top(
     wire [DMI_DATA_WIDTH-1:0]   dtm_to_tap_data;
     wire [31:0]                 idcode;
     wire [31:0]                 dtmcs;
+    wire                        dmireset;
 
     jtag_dtm #(
 
@@ -138,6 +139,7 @@ module jtag_top(
         .dtm_ready_o    (dtm_to_dmi_ready),
         .tap_req_i      (tap_to_dtm_req),
         .tap_data_i     (tap_to_dtm_data),
+        .dmireset_i     (dmireset),
         .data_o         (dtm_to_tap_data),
         .idcode_o       (idcode),
         .dtmcs_o        (dtmcs)
@@ -153,6 +155,7 @@ module jtag_top(
         .jtag_tdo_o     (jtag_tdo_o),
         .tap_req_o      (tap_to_dtm_req),
         .tap_data_o     (tap_to_dtm_data),
+        .dmireset_o     (dmireset),
         .dtm_data_i     (dtm_to_tap_data),
         .idcode_i       (idcode),
         .dtmcs_i        (dtmcs)