diff --git a/rtl/core/csr_reg.sv b/rtl/core/csr_reg.sv
index 205cd75..9abdb36 100644
--- a/rtl/core/csr_reg.sv
+++ b/rtl/core/csr_reg.sv
@@ -38,7 +38,8 @@ module csr_reg(
     output wire[31:0] mepc_o,               // mepc寄存器值
     output wire[31:0] mstatus_o,            // mstatus寄存器值
     output wire[31:0] mie_o,                // mie寄存器值
-    output wire[31:0] dpc_o                 // dpc寄存器值
+    output wire[31:0] dpc_o,                // dpc寄存器值
+    output wire[31:0] dcsr_o                // dcsr寄存器值
 
     );
 
@@ -72,6 +73,9 @@ module csr_reg(
     reg[31:0] dpc_d;
     wire[31:0] dpc_q;
     reg dpc_we;
+    reg[31:0] dcsr_d;
+    wire[31:0] dcsr_q;
+    reg dcsr_we;
 
     reg[63:0] cycle;
 
@@ -90,6 +94,7 @@ module csr_reg(
     assign mstatus_o = mstatus_q;
     assign mie_o = mie_q;
     assign dpc_o = dpc_q;
+    assign dcsr_o = dcsr_q;
 
     reg[31:0] exu_rdata;
 
@@ -132,6 +137,9 @@ module csr_reg(
             `CSR_DPC: begin
                 exu_rdata = dpc_q;
             end
+            `CSR_DCSR: begin
+                exu_rdata = dcsr_q;
+            end
             default: begin
                 exu_rdata = 32'h0;
             end
@@ -166,6 +174,8 @@ module csr_reg(
         mhartid_we = 1'b0;
         dpc_d = dpc_q;
         dpc_we = 1'b0;
+        dcsr_d = dcsr_q;
+        dcsr_we = 1'b0;
 
         if (we) begin
             case (waddr[11:0])
@@ -209,6 +219,10 @@ module csr_reg(
                     dpc_d = wdata;
                     dpc_we = 1'b1;
                 end
+                `CSR_DCSR: begin
+                    dcsr_d = wdata;
+                    dcsr_we = 1'b1;
+                end
                 default:;
             endcase
         end
@@ -324,4 +338,15 @@ module csr_reg(
         .rdata_o(dpc_q)
     );
 
+    // dcsr
+    csr #(
+        .RESET_VAL(32'h0)
+    ) dcsr_csr (
+        .clk(clk),
+        .rst_n(rst_n),
+        .wdata_i(dcsr_d),
+        .we_i(dcsr_we),
+        .rdata_o(dcsr_q)
+    );
+
 endmodule
diff --git a/rtl/core/exception.sv b/rtl/core/exception.sv
index 7fa12d7..3dbbf18 100644
--- a/rtl/core/exception.sv
+++ b/rtl/core/exception.sv
@@ -46,6 +46,7 @@ module exception (
     input wire[31:0] mstatus_i,                 // mstatus寄存器
     input wire[31:0] mie_i,                     // mie寄存器
     input wire[31:0] dpc_i,                     // dpc寄存器
+    input wire[31:0] dcsr_i,                    // dcsr寄存器
 
     input wire irq_software_i,
     input wire irq_timer_i,
@@ -55,9 +56,9 @@ module exception (
     input wire[31:0] debug_halt_addr_i,
     input wire debug_req_i,
 
-    output wire csr_we_o,                        // 写CSR寄存器标志
-    output wire[31:0] csr_waddr_o,               // 写CSR寄存器地址
-    output wire[31:0] csr_wdata_o,               // 写CSR寄存器数据
+    output wire csr_we_o,                       // 写CSR寄存器标志
+    output wire[31:0] csr_waddr_o,              // 写CSR寄存器地址
+    output wire[31:0] csr_wdata_o,              // 写CSR寄存器数据
 
     output wire stall_flag_o,                   // 流水线暂停标志
     output wire[31:0] int_addr_o,               // 中断入口地址
@@ -159,7 +160,7 @@ module exception (
             exception_req = 1'b1;
             exception_cause = `CAUSE_EXCEP_ECALL_M;
             exception_offset = ECALL_OFFSET;
-        end else if (inst_ebreak_i) begin
+        end else if (inst_ebreak_i & (!dcsr_i[15])) begin
             exception_req = 1'b1;
             exception_cause = `CAUSE_EXCEP_EBREAK_M;
             exception_offset = EBREAK_OFFSET;
@@ -178,7 +179,7 @@ module exception (
     assign int_or_exception_cause   = exception_req ? exception_cause  : interrupt_cause;
     assign int_or_exception_offset  = exception_req ? exception_offset : interrupt_offset;
 
-    wire debug_mode_req = (~debug_mode_q) & debug_req_i & inst_valid_i;
+    wire debug_mode_req = ((~debug_mode_q) & debug_req_i & inst_valid_i) | (inst_ebreak_i & dcsr_i[15]);
 
     assign stall_flag_o = ((state_q != S_IDLE) & (state_q != S_ASSERT)) |
                           (interrupt_req & global_int_en) | exception_req |
@@ -206,9 +207,11 @@ module exception (
                     state_d = S_W_MEPC;
                 end else if (debug_mode_req) begin
                     debug_mode_d = 1'b1;
-                    csr_we = 1'b1;
-                    csr_waddr = {20'h0, `CSR_DPC};
-                    csr_wdata = inst_addr_i;
+                    if (!inst_ebreak_i) begin
+                        csr_we = 1'b1;
+                        csr_waddr = {20'h0, `CSR_DPC};
+                        csr_wdata = inst_addr_i;
+                    end
                     assert_addr_d = debug_halt_addr_i;
                     state_d = S_ASSERT;
                 end else if (inst_mret_i) begin
diff --git a/rtl/core/tinyriscv_core.sv b/rtl/core/tinyriscv_core.sv
index 7bab2f0..488dac4 100644
--- a/rtl/core/tinyriscv_core.sv
+++ b/rtl/core/tinyriscv_core.sv
@@ -128,6 +128,7 @@ module tinyriscv_core #(
     wire[31:0] csr_mstatus_o;
     wire[31:0] csr_mie_o;
     wire[31:0] csr_dpc_o;
+    wire[31:0] csr_dcsr_o;
 
     // pipe_ctrl模块输出信号
     wire[31:0] ctrl_flush_addr_o;
@@ -201,7 +202,8 @@ module tinyriscv_core #(
         .mepc_o(csr_mepc_o),
         .mstatus_o(csr_mstatus_o),
         .mie_o(csr_mie_o),
-        .dpc_o(csr_dpc_o)
+        .dpc_o(csr_dpc_o),
+        .dcsr_o(csr_dcsr_o)
     );
 
     ifu_idu u_ifu_idu(
@@ -319,6 +321,7 @@ module tinyriscv_core #(
         .mstatus_i(csr_mstatus_o),
         .mie_i(csr_mie_o),
         .dpc_i(csr_dpc_o),
+        .dcsr_i(csr_dcsr_o),
         .irq_software_i(irq_software_i),
         .irq_timer_i(irq_timer_i),
         .irq_external_i(irq_external_i),
diff --git a/rtl/debug/jtag_def.sv b/rtl/debug/jtag_def.sv
index 5e3148d..5828dbe 100644
--- a/rtl/debug/jtag_def.sv
+++ b/rtl/debug/jtag_def.sv
@@ -17,11 +17,14 @@
 
 `define DbgVersion013       4'h2
 `define ProgBufSize         5'h8
-`define DataCount           4'h2
+`define DataCount           4'h1
 `define HaltAddress         64'h800
 `define ResumeAddress       `HaltAddress + 4
 `define ExceptionAddress    `HaltAddress + 8
-`define DataAddr            12'h380
+`define DataBaseAddr        12'h380
+`define AbstCmdBaseAddr     12'h338
+`define AbstCmdCount        12'd10
+`define ProgbufBaseAddr     `AbstCmdBaseAddr + (4 * `AbstCmdCount)
 
 // dmi op
 `define DMI_OP_NOP          2'b00
@@ -33,11 +36,13 @@
 `define Data1               6'h05
 `define Data2               6'h06
 `define Data3               6'h07
+`define Data4               6'h08
 `define DMControl           6'h10
 `define DMStatus            6'h11
 `define Hartinfo            6'h12
 `define AbstractCS          6'h16
 `define Command             6'h17
+`define AbstractAuto        8'h18
 `define ProgBuf0            6'h20
 `define ProgBuf1            6'h21
 `define ProgBuf2            6'h22
@@ -128,3 +133,21 @@
 `define Sbaccess16          1
 `define Sbaccess8           0
 
+// abstractauto
+`define AutoexecData        11:0
+`define AutoexecProgbuf     31:16
+
+`define CSR_CYCLE       12'hc00
+`define CSR_CYCLEH      12'hc80
+`define CSR_MTVEC       12'h305
+`define CSR_MCAUSE      12'h342
+`define CSR_MEPC        12'h341
+`define CSR_MIE         12'h304
+`define CSR_MSTATUS     12'h300
+`define CSR_MSCRATCH    12'h340
+`define CSR_MHARTID     12'hF14
+`define CSR_DCSR        12'h7b0
+`define CSR_DPC         12'h7b1
+`define CSR_DSCRATCH0   12'h7b2
+`define CSR_DSCRATCH1   12'h7b3
+
diff --git a/rtl/debug/jtag_dm.sv b/rtl/debug/jtag_dm.sv
index 56ba53a..9780dd1 100644
--- a/rtl/debug/jtag_dm.sv
+++ b/rtl/debug/jtag_dm.sv
@@ -20,8 +20,8 @@ module jtag_dm #(
     parameter DMI_ADDR_BITS  = 6,
     parameter DMI_DATA_BITS  = 32,
     parameter DMI_OP_BITS    = 2,
-    localparam DMI_REQ_BITS  = DMI_ADDR_BITS + DMI_DATA_BITS + DMI_OP_BITS,
-    localparam DMI_RESP_BITS = DMI_REQ_BITS
+    parameter DMI_REQ_BITS  = DMI_ADDR_BITS + DMI_DATA_BITS + DMI_OP_BITS,
+    parameter DMI_RESP_BITS = DMI_REQ_BITS
     )(
 
     input wire                      clk,
@@ -39,6 +39,7 @@ module jtag_dm #(
 
     output wire                     debug_req_o,
     output wire                     ndmreset_o,
+    output wire                     halted_o,
 
     // jtag access mem devices(DM as master)
     output wire                     master_req_o,
@@ -61,7 +62,10 @@ module jtag_dm #(
 
     );
 
-    localparam HARTINFO = {8'h0, 4'h2, 3'b0, 1'b1, `DataCount, `DataAddr};
+    localparam HARTINFO = {8'h0, 4'h2, 3'b0, 1'b1, `DataCount, `DataBaseAddr};
+
+    localparam CmdErrorNone         = 3'h0;
+    localparam CmdErrorBusy         = 3'h1;
 
     wire halted;
     wire resumeack;
@@ -77,7 +81,11 @@ module jtag_dm #(
     reg sbdata_write_valid;
     reg sbdata_read_valid;
     reg[31:0] sbcs;
+    reg[31:0] a_abstractcs;
     reg[31:0] dm_resp_data_d, dm_resp_data_q;
+    reg cmd_valid_d, cmd_valid_q;
+    reg[15:0] abstractautoprogbuf;
+    reg[3:0] progbuf_index;
     wire[31:0] sba_sbaddress;
     wire[31:0] dm_sbaddress;
     wire resumereq;
@@ -85,17 +93,23 @@ module jtag_dm #(
     wire sbdata_valid;
     wire[31:0] sbdata;
     wire[19:0] hartsel;
+    wire data_valid;
+    wire[31:0] data0;
+    wire cmderror_valid;
+    wire[2:0] cmderror;
 
     // DM regs
     reg[31:0] dmstatus;
     reg[31:0] dmcontrol_d, dmcontrol_q;
     reg[31:0] abstractcs;
+    reg[31:0] abstractauto_d, abstractauto_q;
     reg[31:0] sbcs_d, sbcs_q;
     reg[31:0] sbdata0_d, sbdata0_q;
     reg[31:0] sbaddress0_d, sbaddress0_q;
     reg[31:0] command_d, command_q;
     reg[31:0] data0_d, data0_q;
     reg[2:0] cmderr_d, cmderr_q;
+    reg[`ProgBufSize-1:0][31:0] progbuf_d, progbuf_q;
 
     assign dm_sbaddress = sbaddress0_q;
 
@@ -176,6 +190,8 @@ module jtag_dm #(
         abstractcs[`Progbufsize] = `ProgBufSize;
         abstractcs[`Busy]        = cmdbusy;
         abstractcs[`Cmderr]      = cmderr_q;
+        a_abstractcs = 32'h0;
+        abstractauto_d = abstractauto_q;
 
         havereset_d = havereset_q;
         sbaddress0_d = sba_sbaddress;
@@ -185,6 +201,12 @@ module jtag_dm #(
         sbdata_write_valid = 1'b0;
         sbdata_read_valid = 1'b0;
         sbcs = 32'h0;
+        command_d = command_q;
+        cmd_valid_d = 1'b0;
+
+        progbuf_index = 4'h0;
+        progbuf_d = progbuf_q;
+        abstractautoprogbuf = abstractauto_q[`AutoexecProgbuf];
 
         data0_d = data0_q;
         sbcs_d = sbcs_q;
@@ -200,6 +222,8 @@ module jtag_dm #(
                     `Hartinfo  :dm_resp_data_d = HARTINFO;
                     `SBCS      :dm_resp_data_d = sbcs_q;
                     `AbstractCS:dm_resp_data_d = abstractcs;
+                    `AbstractAuto: dm_resp_data_d = abstractauto_q;
+                    `Command   :dm_resp_data_d = 32'h0;
                     `SBAddress0:dm_resp_data_d = sbaddress0_q;
                     `SBData0   : begin
                         if (sbbusy || sbcs_q[`Sbbusyerror]) begin
@@ -209,7 +233,26 @@ module jtag_dm #(
                             dm_resp_data_d = sbdata0_q;
                         end
                     end
-                    default:;
+                    `Data0: begin
+                        dm_resp_data_d = data0_q;
+                        if (!cmdbusy) begin
+                            cmd_valid_d = abstractauto_q[0];
+                        end else if (cmderr_q == CmdErrorNone) begin
+                            cmderr_d = CmdErrorBusy;
+                        end
+                    end
+                    `ProgBuf0, `ProgBuf1, `ProgBuf2, `ProgBuf3,
+                    `ProgBuf4, `ProgBuf5, `ProgBuf6, `ProgBuf7,
+                    `ProgBuf8, `ProgBuf9: begin
+                        progbuf_index = dm_op_addr[3:0];
+                        dm_resp_data_d = progbuf_q[progbuf_index];
+                        if (!cmdbusy) begin
+                            cmd_valid_d = abstractautoprogbuf[progbuf_index];
+                        end else if (cmderr_q == CmdErrorNone) begin
+                            cmderr_d = CmdErrorBusy;
+                        end
+                    end
+                    default: dm_resp_data_d = 32'h0;
                 endcase
             // write
             end else if (dm_op == `DMI_OP_WRITE) begin
@@ -222,7 +265,12 @@ module jtag_dm #(
                     end
 
                     `Data0: begin
-                        data0_d = dm_op_data;
+                        if (!cmdbusy) begin
+                            data0_d = dm_op_data;
+                            cmd_valid_d = abstractauto_q[0];
+                        end else if (cmderr_q == CmdErrorNone) begin
+                            cmderr_d = CmdErrorBusy;
+                        end
                     end
 
                     `SBCS: begin
@@ -255,8 +303,44 @@ module jtag_dm #(
                         end
                     end
 
+                    `Command: begin
+                        if (!cmdbusy) begin
+                            cmd_valid_d = 1'b1;
+                            command_d = dm_op_data;
+                        end else if (cmderr_q == CmdErrorNone) begin
+                            cmderr_d = CmdErrorBusy;
+                        end
+                    end
+
                     `AbstractCS: begin
-                        
+                        a_abstractcs = dm_op_data;
+                        if (!cmdbusy) begin
+                            cmderr_d = (~a_abstractcs[`Cmderr]) & cmderr_q;
+                        end else if (cmderr_q == CmdErrorNone) begin
+                            cmderr_d = CmdErrorBusy;
+                        end
+                    end
+
+                    `AbstractAuto: begin
+                        if (!cmdbusy) begin
+                            abstractauto_d                   = 32'h0;
+                            abstractauto_d[`AutoexecData]    = {11'h0, dm_op_data[0]};
+                            abstractauto_d[`AutoexecProgbuf] = dm_op_data[`ProgBufSize-1+16:16];
+                        end else if (cmderr_q == CmdErrorNone) begin
+                            cmderr_d = CmdErrorBusy;
+                        end
+                    end
+
+                    `ProgBuf0, `ProgBuf1, `ProgBuf2, `ProgBuf3,
+                    `ProgBuf4, `ProgBuf5, `ProgBuf6, `ProgBuf7,
+                    `ProgBuf8, `ProgBuf9: begin
+                        if (!cmdbusy) begin
+                            progbuf_index = dm_op_addr[3:0];
+                            progbuf_d[progbuf_index] = dm_op_data;
+                            cmd_valid_d = abstractautoprogbuf[progbuf_index];
+                        end else if (cmderr_q == CmdErrorNone) begin
+                            cmderr_d = CmdErrorBusy;
+                        end
                     end
 
                     default:;
@@ -297,17 +381,42 @@ module jtag_dm #(
             sbdata0_d = sbdata;
         end
 
+        if (cmderror_valid) begin
+            cmderr_d = cmderror;
+        end
+
+        if (data_valid) begin
+            data0_d = data0;
+        end
+
         // set the havereset flag when we did a ndmreset
         if (ndmreset_o) begin
             havereset_d = 1'b1;
         end
     end
 
-
     assign debug_req_o = dmcontrol_q[`Haltreq];
     assign ndmreset_o  = dmcontrol_q[`Ndmreset];
     assign resumereq   = dmcontrol_q[`Resumereq];
+    assign halted_o    = halted;
 
+    genvar i;
+
+    generate
+        for (i = 0; i < `ProgBufSize; i = i + 1) begin: gen_progbuf
+            always @ (posedge clk or negedge rst_n) begin
+                if (!rst_n) begin
+                    progbuf_q[i] <= 32'h0;
+                end else begin
+                    if (!dmcontrol_q[`Dmactive]) begin
+                        progbuf_q[i] <= 32'h0;
+                    end else begin
+                        progbuf_q[i] <= progbuf_d[i];
+                    end
+                end
+            end
+        end
+    endgenerate
 
     always @ (posedge clk or negedge rst_n) begin
         if (!rst_n) begin
@@ -319,6 +428,9 @@ module jtag_dm #(
             sbdata0_q <= 32'h0;
             dm_resp_data_q <= 32'h0;
             cmderr_q <= 3'h0;
+            command_q <= 32'h0;
+            abstractauto_q <= 32'h0;
+            cmd_valid_q <= 1'b0;
         end else begin
             if (!dmcontrol_q[`Dmactive]) begin
                 dmcontrol_q[`Haltreq] <= 1'b0;
@@ -338,6 +450,9 @@ module jtag_dm #(
                 sbdata0_q <= 32'h0;
                 dm_resp_data_q <= 32'h0;
                 cmderr_q <= 3'h0;
+                command_q <= 32'h0;
+                abstractauto_q <= 32'h0;
+                cmd_valid_q <= 1'b0;
             end else begin
                 dmcontrol_q <= dmcontrol_d;
                 data0_q <= data0_d;
@@ -346,14 +461,15 @@ module jtag_dm #(
                 sbdata0_q <= sbdata0_d;
                 dm_resp_data_q <= dm_resp_data_d;
                 cmderr_q <= cmderr_d;
+                command_q <= command_d;
+                abstractauto_q <= abstractauto_d;
+                cmd_valid_q <= cmd_valid_d;
             end
             havereset_q <= havereset_d;
         end
     end
 
-    jtag_mem #(
-
-    ) u_jtag_mem (
+    jtag_mem u_jtag_mem (
         .clk(clk),
         .rst_n(rst_n),
 
@@ -362,6 +478,15 @@ module jtag_dm #(
         .clear_resumeack_i(clear_resumeack),
         .resumereq_i(resumereq),
         .haltreq_i(debug_req_o),
+
+        .progbuf_i(),
+        .data_i(data0_q),
+        .data_o(data0),
+        .data_valid_o(data_valid),
+        .cmd_valid_i(cmd_valid_q),
+        .cmd_i(command_q),
+        .cmderror_valid_o(cmderror_valid),
+        .cmderror_o(cmderror),
         .cmdbusy_o(cmdbusy),
 
         .req_i(slave_req_i),
@@ -372,9 +497,7 @@ module jtag_dm #(
         .rdata_o(slave_rdata_o)
     );
 
-    jtag_sba #(
-
-    ) u_jtag_sba (
+    jtag_sba u_jtag_sba (
         .clk(clk),
         .rst_n(rst_n),
         .sbaddress_i(sbaddress0_q),
diff --git a/rtl/debug/jtag_driver.sv b/rtl/debug/jtag_driver.sv
deleted file mode 100644
index ff4c843..0000000
--- a/rtl/debug/jtag_driver.sv
+++ /dev/null
@@ -1,290 +0,0 @@
- /*                                                                      
- Copyright 2020 Blue Liang, liangkangnan@163.com
-                                                                         
- Licensed under the Apache License, Version 2.0 (the "License");         
- you may not use this file except in compliance with the License.        
- You may obtain a copy of the License at                                 
-                                                                         
-     http://www.apache.org/licenses/LICENSE-2.0                          
-                                                                         
- Unless required by applicable law or agreed to in writing, software    
- distributed under the License is distributed on an "AS IS" BASIS,       
- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- See the License for the specific language governing permissions and     
- limitations under the License.                                          
- */
-
-
-module jtag_driver #(
-    parameter DMI_ADDR_BITS = 6,
-    parameter DMI_DATA_BITS = 32,
-    parameter DMI_OP_BITS = 2)(
-
-    rst_n,
-
-    jtag_TCK,
-    jtag_TDI,
-    jtag_TMS,
-    jtag_TDO,
-
-    // rx
-    dm_resp_i,
-    dm_resp_data_i,
-    dtm_ack_o,
-
-    // tx
-    dm_ack_i,
-    dtm_req_valid_o,
-    dtm_req_data_o
-
-    );
-
-    parameter IDCODE_VERSION  = 4'h1;
-    parameter IDCODE_PART_NUMBER = 16'he200;
-    parameter IDCODE_MANUFLD = 11'h537;
-
-    parameter DTM_VERSION  = 4'h1;
-    parameter IR_BITS = 5;
-
-    parameter DM_RESP_BITS = DMI_ADDR_BITS + DMI_DATA_BITS + DMI_OP_BITS;
-    parameter DTM_REQ_BITS = DMI_ADDR_BITS + DMI_DATA_BITS + DMI_OP_BITS;
-    parameter SHIFT_REG_BITS = DTM_REQ_BITS;
-
-    // input and output
-    input wire rst_n;
-    input wire jtag_TCK;
-    input wire jtag_TDI;
-    input wire jtag_TMS;
-    output reg jtag_TDO;
-    input wire dm_resp_i;
-    input wire[DM_RESP_BITS - 1:0] dm_resp_data_i;
-    output wire dtm_ack_o;
-    input wire dm_ack_i;
-    output wire dtm_req_valid_o;
-    output wire[DTM_REQ_BITS - 1:0] dtm_req_data_o;
-
-    // JTAG StateMachine
-    parameter TEST_LOGIC_RESET  = 4'h0;
-    parameter RUN_TEST_IDLE     = 4'h1;
-    parameter SELECT_DR         = 4'h2;
-    parameter CAPTURE_DR        = 4'h3;
-    parameter SHIFT_DR          = 4'h4;
-    parameter EXIT1_DR          = 4'h5;
-    parameter PAUSE_DR          = 4'h6;
-    parameter EXIT2_DR          = 4'h7;
-    parameter UPDATE_DR         = 4'h8;
-    parameter SELECT_IR         = 4'h9;
-    parameter CAPTURE_IR        = 4'hA;
-    parameter SHIFT_IR          = 4'hB;
-    parameter EXIT1_IR          = 4'hC;
-    parameter PAUSE_IR          = 4'hD;
-    parameter EXIT2_IR          = 4'hE;
-    parameter UPDATE_IR         = 4'hF;
-
-    // DTM regs
-    parameter REG_BYPASS       = 5'b11111;
-    parameter REG_IDCODE       = 5'b00001;
-    parameter REG_DMI          = 5'b10001;
-    parameter REG_DTMCS        = 5'b10000;
-
-    reg[IR_BITS - 1:0] ir_reg;
-    reg[SHIFT_REG_BITS - 1:0] shift_reg;
-    reg[3:0] jtag_state;
-    wire is_busy;
-    reg sticky_busy;
-    reg dtm_req_valid;
-    reg[DTM_REQ_BITS - 1:0] dtm_req_data;
-    reg[DM_RESP_BITS - 1:0] dm_resp_data;
-    reg dm_is_busy;
-
-    wire[5:0] addr_bits = DMI_ADDR_BITS[5:0];
-    wire [SHIFT_REG_BITS - 1:0] busy_response;
-    wire [SHIFT_REG_BITS - 1:0] none_busy_response;
-    wire[31:0] idcode;
-    wire[31:0] dtmcs;
-    wire[1:0] dmi_stat;
-    wire dtm_reset;
-    wire tx_idle;
-    wire rx_valid;
-    wire[DM_RESP_BITS - 1:0] rx_data;
-    wire tx_valid;
-    wire[DTM_REQ_BITS - 1:0] tx_data;
-
-    assign dtm_reset = shift_reg[16];
-    assign idcode = {IDCODE_VERSION, IDCODE_PART_NUMBER, IDCODE_MANUFLD, 1'h1};
-    assign dtmcs = {14'b0,
-                    1'b0,  // dmihardreset
-                    1'b0,  // dmireset
-                    1'b0,
-                    3'h5,  // idle
-                    dmi_stat,      // dmistat
-                    addr_bits,    // abits
-                    DTM_VERSION}; // version
-
-    assign busy_response = {{(DMI_ADDR_BITS + DMI_DATA_BITS){1'b0}}, {(DMI_OP_BITS){1'b1}}};  // op = 2'b11
-    assign none_busy_response = dm_resp_data;
-    assign is_busy = sticky_busy | dm_is_busy;
-    assign dmi_stat = is_busy ? 2'b01 : 2'b00;
-
-    // state switch
-    always @(posedge jtag_TCK or negedge rst_n) begin
-        if (!rst_n) begin
-            jtag_state <= TEST_LOGIC_RESET;
-        end else begin
-            case (jtag_state)
-                TEST_LOGIC_RESET  : jtag_state <= jtag_TMS ? TEST_LOGIC_RESET : RUN_TEST_IDLE;
-                RUN_TEST_IDLE     : jtag_state <= jtag_TMS ? SELECT_DR        : RUN_TEST_IDLE;
-                SELECT_DR         : jtag_state <= jtag_TMS ? SELECT_IR        : CAPTURE_DR;
-                CAPTURE_DR        : jtag_state <= jtag_TMS ? EXIT1_DR         : SHIFT_DR;
-                SHIFT_DR          : jtag_state <= jtag_TMS ? EXIT1_DR         : SHIFT_DR;
-                EXIT1_DR          : jtag_state <= jtag_TMS ? UPDATE_DR        : PAUSE_DR;
-                PAUSE_DR          : jtag_state <= jtag_TMS ? EXIT2_DR         : PAUSE_DR;
-                EXIT2_DR          : jtag_state <= jtag_TMS ? UPDATE_DR        : SHIFT_DR;
-                UPDATE_DR         : jtag_state <= jtag_TMS ? SELECT_DR        : RUN_TEST_IDLE;
-                SELECT_IR         : jtag_state <= jtag_TMS ? TEST_LOGIC_RESET : CAPTURE_IR;
-                CAPTURE_IR        : jtag_state <= jtag_TMS ? EXIT1_IR         : SHIFT_IR;
-                SHIFT_IR          : jtag_state <= jtag_TMS ? EXIT1_IR         : SHIFT_IR;
-                EXIT1_IR          : jtag_state <= jtag_TMS ? UPDATE_IR        : PAUSE_IR;
-                PAUSE_IR          : jtag_state <= jtag_TMS ? EXIT2_IR         : PAUSE_IR;
-                EXIT2_IR          : jtag_state <= jtag_TMS ? UPDATE_IR        : SHIFT_IR;
-                UPDATE_IR         : jtag_state <= jtag_TMS ? SELECT_DR        : RUN_TEST_IDLE; 
-            endcase
-        end
-    end
-
-    // IR or DR shift
-    always @(posedge jtag_TCK) begin
-        case (jtag_state)
-            // IR
-            CAPTURE_IR: shift_reg <= {{(SHIFT_REG_BITS - 1){1'b0}}, 1'b1}; //JTAG spec says it must be b01
-            SHIFT_IR  : shift_reg <= {{(SHIFT_REG_BITS - IR_BITS){1'b0}}, jtag_TDI, shift_reg[IR_BITS - 1:1]}; // right shift 1 bit
-            // DR
-            CAPTURE_DR: case (ir_reg) 
-                            REG_BYPASS     : shift_reg <= {(SHIFT_REG_BITS){1'b0}};
-                            REG_IDCODE     : shift_reg <= {{(SHIFT_REG_BITS - DMI_DATA_BITS){1'b0}}, idcode};
-                            REG_DTMCS      : shift_reg <= {{(SHIFT_REG_BITS - DMI_DATA_BITS){1'b0}}, dtmcs};
-                            REG_DMI        : shift_reg <=  is_busy ? busy_response : none_busy_response;
-                            default:
-                                shift_reg <= {(SHIFT_REG_BITS){1'b0}};
-                        endcase
-            SHIFT_DR  : case (ir_reg) 
-                            REG_BYPASS     : shift_reg <= {{(SHIFT_REG_BITS - 1){1'b0}}, jtag_TDI}; // in = out
-                            REG_IDCODE     : shift_reg <= {{(SHIFT_REG_BITS - DMI_DATA_BITS){1'b0}}, jtag_TDI, shift_reg[31:1]}; // right shift 1 bit
-                            REG_DTMCS      : shift_reg <= {{(SHIFT_REG_BITS - DMI_DATA_BITS){1'b0}}, jtag_TDI, shift_reg[31:1]}; // right shift 1 bit
-                            REG_DMI        : shift_reg <= {jtag_TDI, shift_reg[SHIFT_REG_BITS - 1:1]}; // right shift 1 bit
-                            default:
-                                shift_reg <= {{(SHIFT_REG_BITS - 1){1'b0}} , jtag_TDI};
-                        endcase 
-        endcase
-    end
-
-    // start access DM module
-    always @(posedge jtag_TCK or negedge rst_n) begin
-        if (!rst_n) begin
-            dtm_req_valid <= 1'b0;
-            dtm_req_data <= {DTM_REQ_BITS{1'b0}};
-        end else begin
-            if (jtag_state == UPDATE_DR) begin
-                if (ir_reg == REG_DMI) begin
-                    // if DM can be access
-                    if (!is_busy & tx_idle) begin
-                        dtm_req_valid <= 1'b1;
-                        dtm_req_data <= shift_reg;
-                    end
-                end
-            end else begin
-                dtm_req_valid <= 1'b0;
-            end
-        end
-    end
-
-    assign tx_valid = dtm_req_valid;
-    assign tx_data = dtm_req_data;
-
-    // DTM reset
-    always @ (posedge jtag_TCK or negedge rst_n) begin
-        if (!rst_n) begin
-            sticky_busy <= 1'b0;
-        end else begin
-            if (jtag_state == UPDATE_DR) begin
-                if (ir_reg == REG_DTMCS & dtm_reset) begin
-                    sticky_busy <= 1'b0;
-                end
-            end else if (jtag_state == CAPTURE_DR) begin
-                if (ir_reg == REG_DMI) begin
-                    sticky_busy <= is_busy;
-                end
-            end
-        end
-    end
-
-    // receive DM response data
-    always @ (posedge jtag_TCK or negedge rst_n) begin
-        if (!rst_n) begin
-            dm_resp_data <= {DM_RESP_BITS{1'b0}};
-        end else begin
-            if (rx_valid) begin
-                dm_resp_data <= rx_data;
-            end
-        end
-    end
-
-    // tx busy
-    always @ (posedge jtag_TCK or negedge rst_n) begin
-        if (!rst_n) begin
-            dm_is_busy <= 1'b0;
-        end else begin
-            if (dtm_req_valid) begin
-                dm_is_busy <= 1'b1;
-            end else if (rx_valid) begin
-                dm_is_busy <= 1'b0;
-            end
-        end
-    end
-
-    // TAP reset
-    always @(negedge jtag_TCK) begin
-        if (jtag_state == TEST_LOGIC_RESET) begin
-            ir_reg <= REG_IDCODE;
-        end else if (jtag_state == UPDATE_IR) begin
-            ir_reg <= shift_reg[IR_BITS - 1:0];
-        end
-    end
-
-    // TDO output
-    always @(negedge jtag_TCK) begin
-        if (jtag_state == SHIFT_IR) begin
-            jtag_TDO <= shift_reg[0];
-        end else if (jtag_state == SHIFT_DR) begin
-            jtag_TDO <= shift_reg[0];
-        end else begin
-            jtag_TDO <= 1'b0;
-        end
-    end
-
-    full_handshake_tx #(
-        .DW(DTM_REQ_BITS)
-    ) tx(
-        .clk(jtag_TCK),
-        .rst_n(rst_n),
-        .ack_i(dm_ack_i),
-        .req_i(tx_valid),
-        .req_data_i(tx_data),
-        .idle_o(tx_idle),
-        .req_o(dtm_req_valid_o),
-        .req_data_o(dtm_req_data_o)
-    );
-
-    full_handshake_rx #(
-        .DW(DM_RESP_BITS)
-    ) rx(
-        .clk(jtag_TCK),
-        .rst_n(rst_n),
-        .req_i(dm_resp_i),
-        .req_data_i(dm_resp_data_i),
-        .ack_o(dtm_ack_o),
-        .recv_data_o(rx_data),
-        .recv_rdy_o(rx_valid)
-    );
-
-endmodule
diff --git a/rtl/debug/jtag_dtm.sv b/rtl/debug/jtag_dtm.sv
index 7c2cac7..f94926d 100644
--- a/rtl/debug/jtag_dtm.sv
+++ b/rtl/debug/jtag_dtm.sv
@@ -20,10 +20,10 @@ module jtag_dtm #(
     parameter DMI_ADDR_BITS  = 6,
     parameter DMI_DATA_BITS  = 32,
     parameter DMI_OP_BITS    = 2,
-    localparam TAP_REQ_BITS  = DMI_ADDR_BITS + DMI_DATA_BITS + DMI_OP_BITS,
-    localparam DTM_RESP_BITS = TAP_REQ_BITS,
-    localparam DTM_REQ_BITS  = DTM_RESP_BITS,
-    localparam DMI_RESP_BITS = DTM_REQ_BITS
+    parameter TAP_REQ_BITS  = DMI_ADDR_BITS + DMI_DATA_BITS + DMI_OP_BITS,
+    parameter DTM_RESP_BITS = TAP_REQ_BITS,
+    parameter DTM_REQ_BITS  = DTM_RESP_BITS,
+    parameter DMI_RESP_BITS = DTM_REQ_BITS
     )(
 
     input  wire                     jtag_tck_i,     // JTAG test clock pad
diff --git a/rtl/debug/jtag_mem.sv b/rtl/debug/jtag_mem.sv
index 8e63cd7..de211e8 100644
--- a/rtl/debug/jtag_mem.sv
+++ b/rtl/debug/jtag_mem.sv
@@ -14,27 +14,35 @@
  limitations under the License.                                          
  */
 
+`include "jtag_def.sv"
 
+module jtag_mem(
 
-module jtag_mem #(
-    )(
+    input wire                           clk,
+    input wire                           rst_n,
 
-    input wire                      clk,
-    input wire                      rst_n,
+    output wire                          halted_o,
+    output wire                          resumeack_o,
+    input  wire                          clear_resumeack_i,
+    input  wire                          resumereq_i,
+    input  wire                          haltreq_i,
 
-    output wire                     halted_o,
-    output wire                     resumeack_o,
-    output wire                     cmdbusy_o,
-    input  wire                     clear_resumeack_i,
-    input  wire                     resumereq_i,
-    input  wire                     haltreq_i,
+    input  wire [`ProgBufSize-1:0][31:0] progbuf_i,
+    input  wire [31:0]                   data_i,
+    output wire [31:0]                   data_o,
+    output wire                          data_valid_o,
+    input  wire                          cmd_valid_i,
+    input  wire [31:0]                   cmd_i,
+    output wire                          cmderror_valid_o,
+    output wire [2:0]                    cmderror_o,
+    output wire                          cmdbusy_o,
 
-    input  wire                     req_i,
-    input  wire                     we_i,
-    input  wire [31:0]              addr_i,
-    input  wire [3:0]               be_i,
-    input  wire [31:0]              wdata_i,
-    output wire [31:0]              rdata_o
+    input  wire                          req_i,
+    input  wire                          we_i,
+    input  wire [31:0]                   addr_i,
+    input  wire [3:0]                    be_i,
+    input  wire [31:0]                   wdata_i,
+    output wire [31:0]                   rdata_o
 
     );
 
@@ -43,12 +51,33 @@ module jtag_mem #(
     // x10/a0
     localparam LoadBaseAddr         = 5'd10;
 
-    localparam DataBaseAddr         = `DataAddr;
-    localparam DataEndAddr          = (`DataAddr + 4 * `DataCount - 1);
-    localparam ProgBufBaseAddr      = (`DataAddr - 4 * `ProgBufSize);
-    localparam ProgBufEndAddr       = (`DataAddr - 1);
-    localparam AbstractCmdBaseAddr  = (ProgBufBaseAddr - 4 * 10);
-    localparam AbstractCmdEndAddr   = (ProgBufBaseAddr - 1);
+    localparam Data0Addr            = `DataBaseAddr;
+    localparam Data1Addr            = `DataBaseAddr + 4;
+    localparam Data2Addr            = `DataBaseAddr + 8;
+    localparam Data3Addr            = `DataBaseAddr + 12;
+    localparam Data4Addr            = `DataBaseAddr + 16;
+
+    localparam Progbuf0Addr         = `ProgbufBaseAddr;
+    localparam Progbuf1Addr         = `ProgbufBaseAddr + 4;
+    localparam Progbuf2Addr         = `ProgbufBaseAddr + 8;
+    localparam Progbuf3Addr         = `ProgbufBaseAddr + 12;
+    localparam Progbuf4Addr         = `ProgbufBaseAddr + 16;
+    localparam Progbuf5Addr         = `ProgbufBaseAddr + 20;
+    localparam Progbuf6Addr         = `ProgbufBaseAddr + 24;
+    localparam Progbuf7Addr         = `ProgbufBaseAddr + 28;
+    localparam Progbuf8Addr         = `ProgbufBaseAddr + 32;
+    localparam Progbuf9Addr         = `ProgbufBaseAddr + 36;
+
+    localparam AbstractCmd0Addr     = `AbstCmdBaseAddr;
+    localparam AbstractCmd1Addr     = `AbstCmdBaseAddr + 4;
+    localparam AbstractCmd2Addr     = `AbstCmdBaseAddr + 8;
+    localparam AbstractCmd3Addr     = `AbstCmdBaseAddr + 12;
+    localparam AbstractCmd4Addr     = `AbstCmdBaseAddr + 16;
+    localparam AbstractCmd5Addr     = `AbstCmdBaseAddr + 20;
+    localparam AbstractCmd6Addr     = `AbstCmdBaseAddr + 24;
+    localparam AbstractCmd7Addr     = `AbstCmdBaseAddr + 28;
+    localparam AbstractCmd8Addr     = `AbstCmdBaseAddr + 32;
+    localparam AbstractCmd9Addr     = `AbstCmdBaseAddr + 36;
 
     localparam WhereToAddr          = 12'h300;
     localparam FlagsBaseAddr        = 12'h400;
@@ -59,23 +88,114 @@ module jtag_mem #(
     localparam ResumingAddr         = 12'h108;
     localparam ExceptionAddr        = 12'h10C;
 
+    localparam CmdAccessRegister    = 8'h0;
+    localparam CmdQuickAccess       = 8'h1;
+    localparam CmdAccessMemory      = 8'h2;
+
+    localparam CmdErrorNone         = 3'h0;
+    localparam CmdErrorHaltResume   = 3'h4;
+    localparam CmdErrorNotSupport   = 3'h2;
+    localparam CmdErrorException    = 3'h3;
+
+    localparam illegal              = 32'h00000000;
+    localparam nop                  = 32'h00000013;
+    localparam ebreak               = 32'h00100073;
+
     localparam S_IDLE               = 4'b0001;
     localparam S_RESUME             = 4'b0010;
     localparam S_GO                 = 4'b0100;
     localparam S_CMD_EXECUTING      = 4'b1000;
 
-    reg[3:0] state_d, state_q;
 
+    function automatic [31:0] jal;
+        input [4:0] rd;
+        input [20:0] imm;
+
+        jal = {imm[20], imm[10:1], imm[11], imm[19:12], rd, 7'h6f};
+    endfunction
+
+    function automatic [31:0] slli;
+        input [4:0] rd;
+        input [4:0] rs1;
+        input [5:0] shamt;
+
+        slli = {6'b0, shamt[5:0], rs1, 3'h1, rd, 7'h13};
+    endfunction
+
+    function automatic [31:0] srli;
+        input [4:0] rd;
+        input [4:0] rs1;
+        input [5:0] shamt;
+
+        srli = {6'b0, shamt[5:0], rs1, 3'h5, rd, 7'h13};
+    endfunction
+
+    function automatic [31:0] load;
+        input [2:0]  size;
+        input [4:0]  dest;
+        input [4:0]  base;
+        input [11:0] offset;
+
+        load = {offset[11:0], base, size, dest, 7'h03};
+    endfunction
+
+    function automatic [31:0] auipc;
+        input [4:0]  rd;
+        input [20:0] imm;
+
+        auipc = {imm[20], imm[10:1], imm[11], imm[19:12], rd, 7'h17};
+    endfunction
+
+    function automatic [31:0] store;
+        input [2:0]  size;
+        input [4:0]  src;
+        input [4:0]  base;
+        input [11:0] offset;
+
+        store = {offset[11:5], src, base, size, offset[4:0], 7'h23};
+    endfunction
+
+    function automatic [31:0] csrw;
+        input [11:0] csr;
+        input [4:0] rs1;
+
+        csrw = {csr, rs1, 3'h1, 5'h0, 7'h73};
+    endfunction
+
+    function automatic [31:0] csrr;
+        input [11:0] csr;
+        input [4:0] dest;
+
+        csrr = {csr, 5'h0, 3'h2, dest, 7'h73};
+    endfunction
+
+
+    reg[3:0] state_d, state_q;
     reg[31:0] rdata_d, rdata_q;
     reg halted_d, halted_q;
     reg resuming_d, resuming_q;
     reg resume, go, going;
     reg fwd_rom_q;
     reg word_enable32_q;
+    reg data_valid;
+    reg cmdbusy;
     wire fwd_rom_d;
     wire[63:0] rom_rdata;
-
-
+    reg[31:0] data_bits;
+    reg[9:0][31:0] abstract_cmd;
+    reg unsupported_command;
+    reg cmderror_valid;
+    reg[2:0] cmderror;
+    reg exception;
+    wire[11:0] progbuf_baseaddr = Progbuf0Addr;
+    wire[11:0] abstractcmd_baseaddr = AbstractCmd0Addr;
+    wire[7:0] cmd_type = cmd_i[31:24];
+    wire cmd_postexec = cmd_i[18];
+    wire cmd_transfer = cmd_i[17];
+    wire cmd_write = cmd_i[16];
+    wire[15:0] cmd_regno = cmd_i[15:0];
+    wire[2:0] cmd_aarsize = cmd_i[22:20];
+    wire cmd_aarpostincrement = cmd_i[19];
 
     // word mux for 32bit and 64bit buses
     wire [63:0] word_mux;
@@ -84,38 +204,84 @@ module jtag_mem #(
 
     assign halted_o = halted_q;
     assign resumeack_o = resuming_q;
-    assign cmdbusy_o = 1'b0;
 
 
     always @ (*) begin
         state_d = state_q;
         resume = 1'b0;
         go = 1'b0;
+        cmdbusy = 1'b1;
+        cmderror_valid = 1'b0;
+        cmderror = CmdErrorNone;
 
         case (state_q)
             S_IDLE: begin
+                cmdbusy = 1'b0;
                 if (resumereq_i && (!resuming_q) &&
                     halted_q && (!haltreq_i)) begin
                     state_d = S_RESUME;
                 end
+                if (cmd_valid_i) begin
+                    if (!halted_q) begin
+                        cmderror_valid = 1'b1;
+                        cmderror = CmdErrorHaltResume;
+                    end else if (unsupported_command) begin
+                        cmderror_valid = 1'b1;
+                        cmderror = CmdErrorNotSupport;
+                    end else begin
+                        state_d = S_GO;
+                    end
+                end
+            end
+
+            S_GO: begin
+                cmdbusy = 1'b1;
+                go = 1'b1;
+                if (going) begin
+                    state_d = S_CMD_EXECUTING;
+                end
             end
 
             S_RESUME: begin
                 resume = 1'b1;
+                cmdbusy = 1'b1;
                 if (resuming_q) begin
                     state_d = S_IDLE;
                 end
             end
 
+            S_CMD_EXECUTING: begin
+                cmdbusy = 1'b1;
+                go = 1'b0;
+                if (halted_q) begin
+                    state_d = S_IDLE;
+                end
+            end
+
             default:;
         endcase
+
+        if (exception) begin
+            cmderror_valid = 1'b1;
+            cmderror = CmdErrorException;
+        end
     end
 
+    assign cmderror_valid_o = cmderror_valid;
+    assign cmderror_o = cmderror;
+    assign cmdbusy_o = cmdbusy;
+
     always @ (*) begin
         rdata_d = rdata_q;
         halted_d = halted_q;
         resuming_d = resuming_q;
 
+        going = 1'b0;
+        exception = 1'b0;
+
+        data_valid = 1'b0;
+        data_bits = data_i;
+
         if (clear_resumeack_i) begin
             resuming_d = 1'b0;
         end
@@ -128,7 +294,7 @@ module jtag_mem #(
                 end
 
                 GoingAddr: begin
-
+                    going = 1'b1;
                 end
 
                 ResumingAddr: begin
@@ -137,7 +303,20 @@ module jtag_mem #(
                 end
 
                 ExceptionAddr: begin
+                    exception = 1'b1;
+                end
 
+                Data0Addr, Data1Addr, Data2Addr,
+                Data3Addr, Data4Addr: begin
+                    data_valid = 1'b1;
+                    if (be_i[0])
+                        data_bits[7:0] = wdata_i[7:0];
+                    if (be_i[1])
+                        data_bits[15:8] = wdata_i[15:8];
+                    if (be_i[2])
+                        data_bits[23:16] = wdata_i[23:16];
+                    if (be_i[3])
+                        data_bits[31:24] = wdata_i[31:24];
                 end
 
                 default:;
@@ -145,14 +324,144 @@ module jtag_mem #(
         // read
         end else begin
             case (addr_i[DbgAddressBits-1:0])
+                WhereToAddr: begin
+                    if (cmdbusy & (cmd_type == CmdAccessRegister)) begin
+                        // execute program buf
+                        if (cmd_postexec) begin
+                            rdata_d = jal(5'h0, {9'h0, progbuf_baseaddr-WhereToAddr});
+                        // execute command
+                        end else begin
+                            rdata_d = jal(5'h0, {9'h0, abstractcmd_baseaddr-WhereToAddr});
+                        end
+                    end
+                end
+
                 // harts are polling for flags here
                 FlagsBaseAddr: begin
                     rdata_d = {30'b0, resume, go};
                 end
 
+                Data0Addr, Data1Addr, Data2Addr, Data3Addr,
+                Data4Addr: begin
+                    rdata_d = data_i;
+                end
+
+                Progbuf0Addr, Progbuf1Addr, Progbuf2Addr, Progbuf3Addr,
+                Progbuf4Addr, Progbuf5Addr, Progbuf6Addr, Progbuf7Addr,
+                Progbuf8Addr, Progbuf9Addr: begin
+                    rdata_d = progbuf_i[addr_i[DbgAddressBits-1:3] -
+                              progbuf_baseaddr[DbgAddressBits-1:3]];
+                end
+
+                AbstractCmd0Addr, AbstractCmd1Addr, AbstractCmd2Addr, AbstractCmd3Addr,
+                AbstractCmd4Addr, AbstractCmd5Addr, AbstractCmd6Addr, AbstractCmd7Addr,
+                AbstractCmd8Addr, AbstractCmd9Addr: begin
+                    rdata_d = abstract_cmd[addr_i[DbgAddressBits-1:3] -
+                              abstractcmd_baseaddr[DbgAddressBits-1:3]];
+                end
+
                 default:;
             endcase
         end
+
+    end
+
+    assign data_valid_o = data_valid;
+    assign data_o = data_bits;
+
+    always @ (*) begin
+        unsupported_command = 1'b0;
+
+        abstract_cmd[0] = illegal;
+        abstract_cmd[1] = auipc(5'd10, 21'd0);          // auipc a0, 0
+        abstract_cmd[2] = srli(5'd10, 5'd10, 6'd12);    // srli a0, a0, 12
+        abstract_cmd[3] = slli(5'd10, 5'd10, 6'd12);    // slli a0, a0, 12
+        abstract_cmd[4] = nop;
+        abstract_cmd[5] = nop;
+        abstract_cmd[6] = nop;
+        abstract_cmd[7] = nop;
+        abstract_cmd[8] = csrr(`CSR_DSCRATCH1, 5'd10);  // csrr dscratch1, a0  恢复a0寄存器的值
+        abstract_cmd[9] = ebreak;
+
+        case (cmd_type)
+            CmdAccessRegister: begin
+                // unsupported reg size
+                if (cmd_aarsize > 3'h2 || cmd_aarpostincrement || cmd_regno >= 16'h1020) begin
+                    abstract_cmd[0] = ebreak;
+                    unsupported_command = 1'b1;
+                end else begin
+                    // store a0 in dscratch1
+                    abstract_cmd[0] = csrw(`CSR_DSCRATCH1, 5'd10);  // csrw dscratch1, a0  保存a0寄存器的值
+                    // write regs
+                    if (cmd_transfer && cmd_write) begin
+                        // a0
+                        if (cmd_regno[12] && (cmd_regno[4:0] == 5'd10)) begin
+                            // store s0 in dscratch
+                            abstract_cmd[4] = csrw(`CSR_DSCRATCH0, 5'd8);
+                            // load from data register
+                            abstract_cmd[5] = load(cmd_aarsize, 5'd8, LoadBaseAddr, `DataBaseAddr);
+                            // and store it in the corresponding CSR
+                            abstract_cmd[6] = csrw(`CSR_DSCRATCH1, 5'd8);
+                            // restore s0 again from dscratch
+                            abstract_cmd[7] = csrr(`CSR_DSCRATCH0, 5'd8);
+                        // GPR access
+                        end else if (cmd_regno[12]) begin
+                            abstract_cmd[4] = load(cmd_aarsize, cmd_regno[4:0], LoadBaseAddr, `DataBaseAddr);
+                        // CSR access
+                        end else begin
+                            // data register to CSR
+                            // store s0 in dscratch
+                            abstract_cmd[4] = csrw(`CSR_DSCRATCH0, 5'd8);
+                            // load from data register
+                            abstract_cmd[5] = load(cmd_aarsize, 5'd8, LoadBaseAddr, `DataBaseAddr);
+                            // and store it in the corresponding CSR
+                            abstract_cmd[6] = csrw(cmd_regno[11:0], 5'd8);
+                            // restore s0 again from dscratch
+                            abstract_cmd[7] = csrr(`CSR_DSCRATCH0, 5'd8);
+                        end
+                    // read regs
+                    end else if (cmd_transfer && (!cmd_write)) begin
+                        // a0
+                        if (cmd_regno[12] && (cmd_regno[4:0] == 5'd10)) begin
+                            // store s0 in dscratch
+                            abstract_cmd[4] = csrw(`CSR_DSCRATCH0, 5'd8);
+                            // read value from CSR into s0
+                            abstract_cmd[5] = csrr(`CSR_DSCRATCH1, 5'd8);
+                            // and store s0 into data section
+                            abstract_cmd[6] = store(cmd_aarsize, 5'd8, LoadBaseAddr, `DataBaseAddr);
+                            // restore s0 again from dscratch
+                            abstract_cmd[7] = csrr(`CSR_DSCRATCH0, 5'd8);
+                        // GPR access
+                        end else if (cmd_regno[12]) begin
+                            abstract_cmd[4] = store(cmd_aarsize, cmd_regno[4:0], LoadBaseAddr, `DataBaseAddr);
+                        // CSR access
+                        end else begin
+                            // CSR register to data
+                            // store s0 in dscratch
+                            abstract_cmd[4] = csrw(`CSR_DSCRATCH0, 5'd8);
+                            // read value from CSR into s0
+                            abstract_cmd[5] = csrr(cmd_regno[11:0], 5'd8);
+                            // and store s0 into data section
+                            abstract_cmd[6] = store(cmd_aarsize, 5'd8, LoadBaseAddr, `DataBaseAddr);
+                            // restore s0 again from dscratch
+                            abstract_cmd[7] = csrr(`CSR_DSCRATCH0, 5'd8);
+                        end
+                    end
+                end
+                if (cmd_postexec && (!unsupported_command)) begin
+                    // issue a nop, we will automatically run into the program buffer
+                    abstract_cmd[9] = nop;
+                end
+            end
+
+            // not supported at the moment:
+            // CmdQuickAccess:;
+            // CmdAccessMemory:;
+            default: begin
+                unsupported_command = 1'b1;
+                abstract_cmd[0] = ebreak;
+            end
+        endcase
     end
 
     wire[63:0] rom_addr;
diff --git a/rtl/debug/jtag_sba.sv b/rtl/debug/jtag_sba.sv
index be7a3d9..9e6b372 100644
--- a/rtl/debug/jtag_sba.sv
+++ b/rtl/debug/jtag_sba.sv
@@ -15,9 +15,7 @@
  */
 
 
-module jtag_sba #(
-
-    )(
+module jtag_sba(
 
     input  wire                     clk,
     input  wire                     rst_n,
diff --git a/rtl/debug/jtag_tap.sv b/rtl/debug/jtag_tap.sv
index 2e8740c..8473d87 100644
--- a/rtl/debug/jtag_tap.sv
+++ b/rtl/debug/jtag_tap.sv
@@ -20,8 +20,8 @@ module jtag_tap #(
     parameter DMI_DATA_BITS = 32,
     parameter DMI_OP_BITS   = 2,
     parameter IR_BITS       = 5,
-    localparam TAP_REQ_BITS  = DMI_ADDR_BITS + DMI_DATA_BITS + DMI_OP_BITS,
-    localparam DTM_RESP_BITS = TAP_REQ_BITS
+    parameter TAP_REQ_BITS  = DMI_ADDR_BITS + DMI_DATA_BITS + DMI_OP_BITS,
+    parameter DTM_RESP_BITS = TAP_REQ_BITS
     )(
 
     input wire  jtag_tck_i,     // JTAG test clock pad
diff --git a/rtl/debug/jtag_top.sv b/rtl/debug/jtag_top.sv
index a5233a4..0675f7e 100644
--- a/rtl/debug/jtag_top.sv
+++ b/rtl/debug/jtag_top.sv
@@ -15,15 +15,14 @@
  */
 
 
-module jtag_top #(
-
-    )(
+module jtag_top(
 
     input wire                      clk_i,
     input wire                      rst_ni,
 
     output wire                     debug_req_o,
     output wire                     ndmreset_o,
+    output wire                     halted_o,
 
     input wire                      jtag_tck_i,     // JTAG test clock pad
     input wire                      jtag_tdi_i,     // JTAG test data input pad
@@ -74,6 +73,7 @@ module jtag_top #(
         .dmi_ready_i    (dmi_to_dm_ready),
         .debug_req_o    (debug_req_o),
         .ndmreset_o     (ndmreset_o),
+        .halted_o       (halted_o),
         .master_req_o   (master_req_o),
         .master_gnt_i   (master_gnt_i),
         .master_rvalid_i(master_rvalid_i),
diff --git a/rtl/sys_bus/obi_interconnect.sv b/rtl/sys_bus/obi_interconnect.sv
index 131eb82..9e99096 100644
--- a/rtl/sys_bus/obi_interconnect.sv
+++ b/rtl/sys_bus/obi_interconnect.sv
@@ -79,8 +79,8 @@ module obi_interconnect #(
     // slave信号赋值
     generate
         for (s = 0; s < SLAVES; s = s + 1) begin: slave_signal
-            assign slave_req_o[s]   = master_req_i[master_sel_int[s]];
-            assign slave_we_o[s]    = master_we_i[master_sel_int[s]];
+            assign slave_req_o[s]   = master_req_i[master_sel_int[s]] & granted_master[s];
+            assign slave_we_o[s]    = master_we_i[master_sel_int[s]]  & granted_master[s];
             assign slave_be_o[s]    = master_be_i[master_sel_int[s]];
             assign slave_addr_o[s]  = master_addr_i[master_sel_int[s]];
             assign slave_wdata_o[s] = master_wdata_i[master_sel_int[s]];
diff --git a/rtl/top/tinyriscv_soc_top.sv b/rtl/top/tinyriscv_soc_top.sv
index cfe11c3..8816c3e 100644
--- a/rtl/top/tinyriscv_soc_top.sv
+++ b/rtl/top/tinyriscv_soc_top.sv
@@ -37,15 +37,13 @@ module tinyriscv_soc_top(
 
     );
 
-
-
-    localparam int MASTERS = 3; // Number of master ports
-    localparam int SLAVES  = 3; // Number of slave ports
+    localparam int MASTERS      = 3; // Number of master ports
+    localparam int SLAVES       = 3; // Number of slave ports
 
     // masters
-    localparam int CoreD    = 0;
-    localparam int JtagHost = 1;
-    localparam int CoreI    = 2;
+    localparam int CoreD        = 0;
+    localparam int JtagHost     = 1;
+    localparam int CoreI        = 2;
 
     // slaves
     localparam int Rom          = 0;
@@ -86,54 +84,55 @@ module tinyriscv_soc_top(
     wire ndmreset;
     wire ndmreset_n;
     wire debug_req;
+    wire core_halted;
 
+    assign halted_ind_pin = core_halted;
 
     tinyriscv_core #(
         .DEBUG_HALT_ADDR(`DEBUG_ADDR_BASE + `HaltAddress),
         .DEBUG_EXCEPTION_ADDR(`DEBUG_ADDR_BASE + `ExceptionAddress)
     ) u_tinyriscv_core (
-        .clk(clk),
-        .rst_n(ndmreset_n),
+        .clk            (clk),
+        .rst_n          (ndmreset_n),
 
-        .instr_req_o(master_req[CoreI]),
-        .instr_gnt_i(master_gnt[CoreI]),
-        .instr_rvalid_i(master_rvalid[CoreI]),
-        .instr_addr_o(master_addr[CoreI]),
-        .instr_rdata_i(master_rdata[CoreI]),
-        .instr_err_i(1'b0),
+        .instr_req_o    (master_req[CoreI]),
+        .instr_gnt_i    (master_gnt[CoreI]),
+        .instr_rvalid_i (master_rvalid[CoreI]),
+        .instr_addr_o   (master_addr[CoreI]),
+        .instr_rdata_i  (master_rdata[CoreI]),
+        .instr_err_i    (1'b0),
 
-        .data_req_o(master_req[CoreD]),
-        .data_gnt_i(master_gnt[CoreD]),
-        .data_rvalid_i(master_rvalid[CoreD]),
-        .data_we_o(master_we[CoreD]),
-        .data_be_o(master_be[CoreD]),
-        .data_addr_o(master_addr[CoreD]),
-        .data_wdata_o(master_wdata[CoreD]),
-        .data_rdata_i(master_rdata[CoreD]),
-        .data_err_i(1'b0),
+        .data_req_o     (master_req[CoreD]),
+        .data_gnt_i     (master_gnt[CoreD]),
+        .data_rvalid_i  (master_rvalid[CoreD]),
+        .data_we_o      (master_we[CoreD]),
+        .data_be_o      (master_be[CoreD]),
+        .data_addr_o    (master_addr[CoreD]),
+        .data_wdata_o   (master_wdata[CoreD]),
+        .data_rdata_i   (master_rdata[CoreD]),
+        .data_err_i     (1'b0),
 
-        .irq_software_i(1'b0),
-        .irq_timer_i(1'b0),
-        .irq_external_i(1'b0),
-        .irq_fast_i(15'b0),
+        .irq_software_i (1'b0),
+        .irq_timer_i    (1'b0),
+        .irq_external_i (1'b0),
+        .irq_fast_i     (15'b0),
 
-        .debug_req_i(debug_req)
+        .debug_req_i    (debug_req)
     );
 
-
     assign slave_addr_mask[Rom] = `ROM_ADDR_MASK;
     assign slave_addr_base[Rom] = `ROM_ADDR_BASE;
     // 指令存储器
     rom #(
         .DP(`ROM_DEPTH)
-    ) u_rom(
-        .clk(clk),
-        .rst_n(ndmreset_n),
-        .addr_i(slave_addr[Rom]),
-        .data_i(slave_wdata[Rom]),
-        .sel_i(slave_be[Rom]),
-        .we_i(slave_we[Rom]),
-        .data_o(slave_rdata[Rom])
+    ) u_rom (
+        .clk    (clk),
+        .rst_n  (ndmreset_n),
+        .addr_i (slave_addr[Rom]),
+        .data_i (slave_wdata[Rom]),
+        .sel_i  (slave_be[Rom]),
+        .we_i   (slave_we[Rom]),
+        .data_o (slave_rdata[Rom])
     );
 
     assign slave_addr_mask[Ram] = `RAM_ADDR_MASK;
@@ -141,49 +140,48 @@ module tinyriscv_soc_top(
     // 数据存储器
     ram #(
         .DP(`RAM_DEPTH)
-    ) u_ram(
-        .clk(clk),
-        .rst_n(ndmreset_n),
-        .addr_i(slave_addr[Ram]),
-        .data_i(slave_wdata[Ram]),
-        .sel_i(slave_be[Ram]),
-        .we_i(slave_we[Ram]),
-        .data_o(slave_rdata[Ram])
+    ) u_ram (
+        .clk    (clk),
+        .rst_n  (ndmreset_n),
+        .addr_i (slave_addr[Ram]),
+        .data_i (slave_wdata[Ram]),
+        .sel_i  (slave_be[Ram]),
+        .we_i   (slave_we[Ram]),
+        .data_o (slave_rdata[Ram])
     );
 
     obi_interconnect #(
         .MASTERS(MASTERS),
         .SLAVES(SLAVES)
     ) bus (
-        .clk_i(clk),
-        .rst_ni(ndmreset_n),
-        .master_req_i(master_req),
-        .master_gnt_o(master_gnt),
-        .master_rvalid_o(master_rvalid),
-        .master_we_i(master_we),
-        .master_be_i(master_be),
-        .master_addr_i(master_addr),
-        .master_wdata_i(master_wdata),
-        .master_rdata_o(master_rdata),
-        .slave_addr_mask_i(slave_addr_mask),
-        .slave_addr_base_i(slave_addr_base),
-        .slave_req_o(slave_req),
-        .slave_gnt_i(slave_gnt),
-        .slave_rvalid_i(slave_rvalid),
-        .slave_we_o(slave_we),
-        .slave_be_o(slave_be),
-        .slave_addr_o(slave_addr),
-        .slave_wdata_o(slave_wdata),
-        .slave_rdata_i(slave_rdata)
+        .clk_i              (clk),
+        .rst_ni             (ndmreset_n),
+        .master_req_i       (master_req),
+        .master_gnt_o       (master_gnt),
+        .master_rvalid_o    (master_rvalid),
+        .master_we_i        (master_we),
+        .master_be_i        (master_be),
+        .master_addr_i      (master_addr),
+        .master_wdata_i     (master_wdata),
+        .master_rdata_o     (master_rdata),
+        .slave_addr_mask_i  (slave_addr_mask),
+        .slave_addr_base_i  (slave_addr_base),
+        .slave_req_o        (slave_req),
+        .slave_gnt_i        (slave_gnt),
+        .slave_rvalid_i     (slave_rvalid),
+        .slave_we_o         (slave_we),
+        .slave_be_o         (slave_be),
+        .slave_addr_o       (slave_addr),
+        .slave_wdata_o      (slave_wdata),
+        .slave_rdata_i      (slave_rdata)
     );
 
-
     rst_gen #(
         .RESET_FIFO_DEPTH(5)
     ) u_rst (
-        .clk(clk),
-        .rst_ni(rst_ext_ni & (~ndmreset)),
-        .rst_no(ndmreset_n)
+        .clk    (clk),
+        .rst_ni (rst_ext_ni & (~ndmreset)),
+        .rst_no (ndmreset_n)
     );
 
     assign slave_addr_mask[JtagDevice] = `DEBUG_ADDR_MASK;
@@ -196,6 +194,7 @@ module tinyriscv_soc_top(
         .rst_ni             (rst_ext_ni),
         .debug_req_o        (debug_req),
         .ndmreset_o         (ndmreset),
+        .halted_o           (core_halted),
         .jtag_tck_i         (sim_jtag_tck),
         .jtag_tdi_i         (sim_jtag_tdi),
         .jtag_tms_i         (sim_jtag_tms),
diff --git a/rtl/utils/cdc_2phase.sv b/rtl/utils/cdc_2phase.sv
index eb75fa0..d17e9e5 100644
--- a/rtl/utils/cdc_2phase.sv
+++ b/rtl/utils/cdc_2phase.sv
@@ -1,175 +1,175 @@
-// Copyright 2018 ETH Zurich and University of Bologna.
-//
-// Copyright and related rights are licensed under the Solderpad Hardware
-// License, Version 0.51 (the "License"); you may not use this file except in
-// compliance with the License. You may obtain a copy of the License at
-// http://solderpad.org/licenses/SHL-0.51. Unless required by applicable law
-// or agreed to in writing, software, hardware and materials distributed under
-// this License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
-// CONDITIONS OF ANY KIND, either express or implied. See the License for the
-// specific language governing permissions and limitations under the License.
-//
-// Fabian Schuiki <fschuiki@iis.ee.ethz.ch>
-
-/// A two-phase clock domain crossing.
-///
-/// CONSTRAINT: Requires max_delay of min_period(src_clk_i, dst_clk_i) through
-/// the paths async_req, async_ack, async_data.
-/* verilator lint_off DECLFILENAME */
-module cdc_2phase #(
-  parameter DATA_WIDTH = 32
-)(
-  input  wire                   src_rst_ni,
-  input  wire                   src_clk_i,
-  input  wire [DATA_WIDTH-1:0]  src_data_i,
-  input  wire                   src_valid_i,
-  output wire                   src_ready_o,
-
-  input  wire                   dst_rst_ni,
-  input  wire                   dst_clk_i,
-  output wire [DATA_WIDTH-1:0]  dst_data_o,
-  output wire                   dst_valid_o,
-  input  wire                   dst_ready_i
-);
-
-  // Asynchronous handshake signals.
-  (* dont_touch = "true" *) wire async_req;
-  (* dont_touch = "true" *) wire async_ack;
-  (* dont_touch = "true" *) wire[DATA_WIDTH-1:0] async_data;
-
-  // The sender in the source domain.
-  cdc_2phase_src #(.DATA_WIDTH(DATA_WIDTH)) i_src (
-    .rst_ni       ( src_rst_ni  ),
-    .clk_i        ( src_clk_i   ),
-    .data_i       ( src_data_i  ),
-    .valid_i      ( src_valid_i ),
-    .ready_o      ( src_ready_o ),
-    .async_req_o  ( async_req   ),
-    .async_ack_i  ( async_ack   ),
-    .async_data_o ( async_data  )
-  );
-
-  // The receiver in the destination domain.
-  cdc_2phase_dst #(.DATA_WIDTH(DATA_WIDTH)) i_dst (
-    .rst_ni       ( dst_rst_ni  ),
-    .clk_i        ( dst_clk_i   ),
-    .data_o       ( dst_data_o  ),
-    .valid_o      ( dst_valid_o ),
-    .ready_i      ( dst_ready_i ),
-    .async_req_i  ( async_req   ),
-    .async_ack_o  ( async_ack   ),
-    .async_data_i ( async_data  )
-  );
-
-endmodule
-
-
-/// Half of the two-phase clock domain crossing located in the source domain.
-module cdc_2phase_src #(
-  parameter DATA_WIDTH = 32
-)(
-  input  wire                   rst_ni,
-  input  wire                   clk_i,
-  input  wire [DATA_WIDTH-1:0]  data_i,
-  input  wire                   valid_i,
-  output wire                   ready_o,
-  output wire                   async_req_o,
-  input  wire                   async_ack_i,
-  output wire [DATA_WIDTH-1:0]  async_data_o
-);
-
-  (* dont_touch = "true" *)
-  reg req_src_q, ack_src_q, ack_q;
-  (* dont_touch = "true" *)
-  reg[DATA_WIDTH-1:0] data_src_q;
-
-  // The req_src and data_src registers change when a new data item is accepted.
-  always @(posedge clk_i or negedge rst_ni) begin
-    if (!rst_ni) begin
-      req_src_q  <= 0;
-      data_src_q <= {DATA_WIDTH{1'b0}};
-    end else if (valid_i && ready_o) begin
-      req_src_q  <= ~req_src_q;
-      data_src_q <= data_i;
-    end
-  end
-
-  // The ack_src and ack registers act as synchronization stages.
-  always @(posedge clk_i or negedge rst_ni) begin
-    if (!rst_ni) begin
-      ack_src_q <= 0;
-      ack_q     <= 0;
-    end else begin
-      ack_src_q <= async_ack_i;
-      ack_q     <= ack_src_q;
-    end
-  end
-
-  // Output assignments.
-  assign ready_o = (req_src_q == ack_q);
-  assign async_req_o = req_src_q;
-  assign async_data_o = data_src_q;
-
-endmodule
-
-
-/// Half of the two-phase clock domain crossing located in the destination
-/// domain.
-module cdc_2phase_dst #(
-  parameter DATA_WIDTH = 32
-)(
-  input  wire                   rst_ni,
-  input  wire                   clk_i,
-  output wire [DATA_WIDTH-1:0]  data_o,
-  output wire                   valid_o,
-  input  wire                   ready_i,
-  input  wire                   async_req_i,
-  output wire                   async_ack_o,
-  input  wire [DATA_WIDTH-1:0]  async_data_i
-);
-
-  (* dont_touch = "true" *)
-  (* async_reg = "true" *)
-  reg req_dst_q, req_q0, req_q1, ack_dst_q;
-  (* dont_touch = "true" *)
-  reg[DATA_WIDTH-1:0] data_dst_q;
-
-  // The ack_dst register changes when a new data item is accepted.
-  always @(posedge clk_i or negedge rst_ni) begin
-    if (!rst_ni) begin
-      ack_dst_q  <= 0;
-    end else if (valid_o && ready_i) begin
-      ack_dst_q  <= ~ack_dst_q;
-    end
-  end
-
-  // The data_dst register changes when a new data item is presented. This is
-  // indicated by the async_req line changing levels.
-  always @(posedge clk_i or negedge rst_ni) begin
-    if (!rst_ni) begin
-      data_dst_q <= '0;
-    end else if (req_q0 != req_q1 && !valid_o) begin
-      data_dst_q <= async_data_i;
-    end
-  end
-
-  // The req_dst and req registers act as synchronization stages.
-  always @(posedge clk_i or negedge rst_ni) begin
-    if (!rst_ni) begin
-      req_dst_q <= 0;
-      req_q0    <= 0;
-      req_q1    <= 0;
-    end else begin
-      req_dst_q <= async_req_i;
-      req_q0    <= req_dst_q;
-      req_q1    <= req_q0;
-    end
-  end
-
-  // Output assignments.
-  assign valid_o = (ack_dst_q != req_q1);
-  assign data_o = data_dst_q;
-  assign async_ack_o = ack_dst_q;
-
-endmodule
-/* verilator lint_on DECLFILENAME */
+// Copyright 2018 ETH Zurich and University of Bologna.
+//
+// Copyright and related rights are licensed under the Solderpad Hardware
+// License, Version 0.51 (the "License"); you may not use this file except in
+// compliance with the License. You may obtain a copy of the License at
+// http://solderpad.org/licenses/SHL-0.51. Unless required by applicable law
+// or agreed to in writing, software, hardware and materials distributed under
+// this License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
+// CONDITIONS OF ANY KIND, either express or implied. See the License for the
+// specific language governing permissions and limitations under the License.
+//
+// Fabian Schuiki <fschuiki@iis.ee.ethz.ch>
+
+/// A two-phase clock domain crossing.
+///
+/// CONSTRAINT: Requires max_delay of min_period(src_clk_i, dst_clk_i) through
+/// the paths async_req, async_ack, async_data.
+/* verilator lint_off DECLFILENAME */
+module cdc_2phase #(
+  parameter DATA_WIDTH = 32
+)(
+  input  wire                   src_rst_ni,
+  input  wire                   src_clk_i,
+  input  wire [DATA_WIDTH-1:0]  src_data_i,
+  input  wire                   src_valid_i,
+  output wire                   src_ready_o,
+
+  input  wire                   dst_rst_ni,
+  input  wire                   dst_clk_i,
+  output wire [DATA_WIDTH-1:0]  dst_data_o,
+  output wire                   dst_valid_o,
+  input  wire                   dst_ready_i
+);
+
+  // Asynchronous handshake signals.
+  (* dont_touch = "true" *) wire async_req;
+  (* dont_touch = "true" *) wire async_ack;
+  (* dont_touch = "true" *) wire[DATA_WIDTH-1:0] async_data;
+
+  // The sender in the source domain.
+  cdc_2phase_src #(.DATA_WIDTH(DATA_WIDTH)) i_src (
+    .rst_ni       ( src_rst_ni  ),
+    .clk_i        ( src_clk_i   ),
+    .data_i       ( src_data_i  ),
+    .valid_i      ( src_valid_i ),
+    .ready_o      ( src_ready_o ),
+    .async_req_o  ( async_req   ),
+    .async_ack_i  ( async_ack   ),
+    .async_data_o ( async_data  )
+  );
+
+  // The receiver in the destination domain.
+  cdc_2phase_dst #(.DATA_WIDTH(DATA_WIDTH)) i_dst (
+    .rst_ni       ( dst_rst_ni  ),
+    .clk_i        ( dst_clk_i   ),
+    .data_o       ( dst_data_o  ),
+    .valid_o      ( dst_valid_o ),
+    .ready_i      ( dst_ready_i ),
+    .async_req_i  ( async_req   ),
+    .async_ack_o  ( async_ack   ),
+    .async_data_i ( async_data  )
+  );
+
+endmodule
+
+
+/// Half of the two-phase clock domain crossing located in the source domain.
+module cdc_2phase_src #(
+  parameter DATA_WIDTH = 32
+)(
+  input  wire                   rst_ni,
+  input  wire                   clk_i,
+  input  wire [DATA_WIDTH-1:0]  data_i,
+  input  wire                   valid_i,
+  output wire                   ready_o,
+  output wire                   async_req_o,
+  input  wire                   async_ack_i,
+  output wire [DATA_WIDTH-1:0]  async_data_o
+);
+
+  (* dont_touch = "true" *)
+  reg req_src_q, ack_src_q, ack_q;
+  (* dont_touch = "true" *)
+  reg[DATA_WIDTH-1:0] data_src_q;
+
+  // The req_src and data_src registers change when a new data item is accepted.
+  always @(posedge clk_i or negedge rst_ni) begin
+    if (!rst_ni) begin
+      req_src_q  <= 0;
+      data_src_q <= {DATA_WIDTH{1'b0}};
+    end else if (valid_i && ready_o) begin
+      req_src_q  <= ~req_src_q;
+      data_src_q <= data_i;
+    end
+  end
+
+  // The ack_src and ack registers act as synchronization stages.
+  always @(posedge clk_i or negedge rst_ni) begin
+    if (!rst_ni) begin
+      ack_src_q <= 0;
+      ack_q     <= 0;
+    end else begin
+      ack_src_q <= async_ack_i;
+      ack_q     <= ack_src_q;
+    end
+  end
+
+  // Output assignments.
+  assign ready_o = (req_src_q == ack_q);
+  assign async_req_o = req_src_q;
+  assign async_data_o = data_src_q;
+
+endmodule
+
+
+/// Half of the two-phase clock domain crossing located in the destination
+/// domain.
+module cdc_2phase_dst #(
+  parameter DATA_WIDTH = 32
+)(
+  input  wire                   rst_ni,
+  input  wire                   clk_i,
+  output wire [DATA_WIDTH-1:0]  data_o,
+  output wire                   valid_o,
+  input  wire                   ready_i,
+  input  wire                   async_req_i,
+  output wire                   async_ack_o,
+  input  wire [DATA_WIDTH-1:0]  async_data_i
+);
+
+  (* dont_touch = "true" *)
+  (* async_reg = "true" *)
+  reg req_dst_q, req_q0, req_q1, ack_dst_q;
+  (* dont_touch = "true" *)
+  reg[DATA_WIDTH-1:0] data_dst_q;
+
+  // The ack_dst register changes when a new data item is accepted.
+  always @(posedge clk_i or negedge rst_ni) begin
+    if (!rst_ni) begin
+      ack_dst_q  <= 0;
+    end else if (valid_o && ready_i) begin
+      ack_dst_q  <= ~ack_dst_q;
+    end
+  end
+
+  // The data_dst register changes when a new data item is presented. This is
+  // indicated by the async_req line changing levels.
+  always @(posedge clk_i or negedge rst_ni) begin
+    if (!rst_ni) begin
+      data_dst_q <= '0;
+    end else if (req_q0 != req_q1 && !valid_o) begin
+      data_dst_q <= async_data_i;
+    end
+  end
+
+  // The req_dst and req registers act as synchronization stages.
+  always @(posedge clk_i or negedge rst_ni) begin
+    if (!rst_ni) begin
+      req_dst_q <= 0;
+      req_q0    <= 0;
+      req_q1    <= 0;
+    end else begin
+      req_dst_q <= async_req_i;
+      req_q0    <= req_dst_q;
+      req_q1    <= req_q0;
+    end
+  end
+
+  // Output assignments.
+  assign valid_o = (ack_dst_q != req_q1);
+  assign data_o = data_dst_q;
+  assign async_ack_o = ack_dst_q;
+
+endmodule
+/* verilator lint_on DECLFILENAME */
diff --git a/sim/tb_top_verilator.sv b/sim/tb_top_verilator.sv
index 58a3683..0fdd2fc 100644
--- a/sim/tb_top_verilator.sv
+++ b/sim/tb_top_verilator.sv
@@ -34,38 +34,42 @@ module tb_top_verilator #(
             $display("No firmware specified");
         end
     end
-/*
+
+    reg result_printed;
+
     always @(posedge clk_i or negedge rst_ni) begin
         if (!rst_ni) begin
-
+            result_printed <= 1'b0;
         end else begin
-            if (x26 == 32'b1) begin
-                if (x27 == 32'b1) begin
-                    $display("~~~~~~~~~~~~~~~~~~~ TEST_PASS ~~~~~~~~~~~~~~~~~~~");
-                    $display("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
-                    $display("~~~~~~~~~ #####     ##     ####    #### ~~~~~~~~~");
-                    $display("~~~~~~~~~ #    #   #  #   #       #     ~~~~~~~~~");
-                    $display("~~~~~~~~~ #    #  #    #   ####    #### ~~~~~~~~~");
-                    $display("~~~~~~~~~ #####   ######       #       #~~~~~~~~~");
-                    $display("~~~~~~~~~ #       #    #  #    #  #    #~~~~~~~~~");
-                    $display("~~~~~~~~~ #       #    #   ####    #### ~~~~~~~~~");
-                    $display("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
-                end else begin
-                    $display("~~~~~~~~~~~~~~~~~~~ TEST_FAIL ~~~~~~~~~~~~~~~~~~~~");
-                    $display("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
-                    $display("~~~~~~~~~~######    ##       #    #     ~~~~~~~~~~");
-                    $display("~~~~~~~~~~#        #  #      #    #     ~~~~~~~~~~");
-                    $display("~~~~~~~~~~#####   #    #     #    #     ~~~~~~~~~~");
-                    $display("~~~~~~~~~~#       ######     #    #     ~~~~~~~~~~");
-                    $display("~~~~~~~~~~#       #    #     #    #     ~~~~~~~~~~");
-                    $display("~~~~~~~~~~#       #    #     #    ######~~~~~~~~~~");
-                    $display("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
+            if (!result_printed) begin
+                if (x26 == 32'b1) begin
+                    if (x27 == 32'b1) begin
+                        $display("~~~~~~~~~~~~~~~~~~~ TEST_PASS ~~~~~~~~~~~~~~~~~~~");
+                        $display("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
+                        $display("~~~~~~~~~ #####     ##     ####    #### ~~~~~~~~~");
+                        $display("~~~~~~~~~ #    #   #  #   #       #     ~~~~~~~~~");
+                        $display("~~~~~~~~~ #    #  #    #   ####    #### ~~~~~~~~~");
+                        $display("~~~~~~~~~ #####   ######       #       #~~~~~~~~~");
+                        $display("~~~~~~~~~ #       #    #  #    #  #    #~~~~~~~~~");
+                        $display("~~~~~~~~~ #       #    #   ####    #### ~~~~~~~~~");
+                        $display("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
+                    end else begin
+                        $display("~~~~~~~~~~~~~~~~~~~ TEST_FAIL ~~~~~~~~~~~~~~~~~~~~");
+                        $display("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
+                        $display("~~~~~~~~~~######    ##       #    #     ~~~~~~~~~~");
+                        $display("~~~~~~~~~~#        #  #      #    #     ~~~~~~~~~~");
+                        $display("~~~~~~~~~~#####   #    #     #    #     ~~~~~~~~~~");
+                        $display("~~~~~~~~~~#       ######     #    #     ~~~~~~~~~~");
+                        $display("~~~~~~~~~~#       #    #     #    #     ~~~~~~~~~~");
+                        $display("~~~~~~~~~~#       #    #     #    ######~~~~~~~~~~");
+                        $display("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
+                    end
+                    result_printed <= 1'b1;
                 end
-                $finish;
             end
         end
     end
-*/
+
     tinyriscv_soc_top u_tinyriscv_soc_top(
         .clk(clk_i),
         .rst_ext_ni(rst_ni)