From ec65381ba98180dd0d87537f1a3cb30e223e6e6e Mon Sep 17 00:00:00 2001
From: liangkangnan <liangkangnan@163.com>
Date: Sun, 25 Apr 2021 17:14:09 +0800
Subject: [PATCH] temp commit

Signed-off-by: liangkangnan <liangkangnan@163.com>
---
 rtl.flist                                   |  19 +-
 rtl/core/clint.sv                           | 223 ------
 rtl/core/csr_reg.sv                         |  29 +-
 rtl/core/defines.sv                         |   8 +-
 rtl/core/exception.sv                       | 262 +++++++
 rtl/core/exu.sv                             |   6 +-
 rtl/core/exu_dispatch.sv                    |   4 +-
 rtl/core/idu.sv                             |   4 +-
 rtl/core/tinyriscv_core.sv                  |  30 +-
 rtl/debug/debug_rom.sv                      |  71 ++
 rtl/debug/jtag_def.sv                       | 130 ++++
 rtl/debug/jtag_dm.sv                        | 751 ++++++++++----------
 rtl/debug/jtag_dmi.sv                       |  76 ++
 rtl/debug/jtag_driver.sv                    | 580 +++++++--------
 rtl/debug/jtag_dtm.sv                       | 187 +++++
 rtl/debug/jtag_mem.sv                       | 194 +++++
 rtl/{utils/vld_rdy.sv => debug/jtag_sba.sv} | 104 ++-
 rtl/debug/jtag_tap.sv                       | 175 +++++
 rtl/debug/jtag_top.sv                       | 278 +++++---
 rtl/sys_bus/rib.sv                          | 373 ----------
 rtl/top/tinyriscv_soc_top.sv                |  58 +-
 rtl/utils/cdc_2phase.sv                     | 175 +++++
 rtl/utils/full_handshake_rx.sv              | 128 ----
 rtl/utils/full_handshake_tx.sv              | 147 ----
 rtl/utils/gen_buf.sv                        |  92 +--
 rtl/utils/gen_dff.sv                        | 346 ++++-----
 rtl/utils/gen_ram.sv                        | 144 ++--
 sim/jtag_compliance_test.cfg                |  30 +
 sim/jtag_debug.cfg                          |  31 +
 sim/tb_top_verilator.sv                     |   4 +-
 30 files changed, 2614 insertions(+), 2045 deletions(-)
 delete mode 100644 rtl/core/clint.sv
 create mode 100644 rtl/core/exception.sv
 create mode 100644 rtl/debug/debug_rom.sv
 create mode 100644 rtl/debug/jtag_def.sv
 create mode 100644 rtl/debug/jtag_dmi.sv
 create mode 100644 rtl/debug/jtag_dtm.sv
 create mode 100644 rtl/debug/jtag_mem.sv
 rename rtl/{utils/vld_rdy.sv => debug/jtag_sba.sv} (51%)
 create mode 100644 rtl/debug/jtag_tap.sv
 delete mode 100644 rtl/sys_bus/rib.sv
 create mode 100644 rtl/utils/cdc_2phase.sv
 delete mode 100644 rtl/utils/full_handshake_rx.sv
 delete mode 100644 rtl/utils/full_handshake_tx.sv
 create mode 100644 sim/jtag_compliance_test.cfg
 create mode 100644 sim/jtag_debug.cfg

diff --git a/rtl.flist b/rtl.flist
index 41c1b07..70b3a36 100644
--- a/rtl.flist
+++ b/rtl.flist
@@ -1,11 +1,11 @@
-
 +incdir+../rtl/core
++incdir+../rtl/debug
 
-../rtl/core/clint.sv
 ../rtl/core/csr_reg.sv
 ../rtl/core/csr.sv
 ../rtl/core/defines.sv
 ../rtl/core/divider.sv
+../rtl/core/exception.sv
 ../rtl/core/exu.sv
 ../rtl/core/exu_alu_datapath.sv
 ../rtl/core/exu_commit.sv
@@ -22,9 +22,15 @@
 ../rtl/core/tinyriscv_core.sv
 ../rtl/core/tracer.sv
 
+../rtl/debug/jtag_def.sv
+../rtl/debug/jtag_tap.sv
+../rtl/debug/jtag_dtm.sv
+../rtl/debug/jtag_dmi.sv
 ../rtl/debug/jtag_dm.sv
-../rtl/debug/jtag_driver.sv
+../rtl/debug/jtag_mem.sv
+../rtl/debug/jtag_sba.sv
 ../rtl/debug/jtag_top.sv
+../rtl/debug/debug_rom.sv
 
 ../rtl/perips/gpio.sv
 ../rtl/perips/ram.sv
@@ -32,18 +38,13 @@
 ../rtl/perips/timer.sv
 ../rtl/perips/uart.sv
 
-../rtl/sys_bus/rib.sv
 ../rtl/sys_bus/obi_interconnect.sv
 ../rtl/sys_bus/obi_interconnect_master_sel.sv
 ../rtl/sys_bus/obi_interconnect_slave_sel.sv
 
 ../rtl/top/tinyriscv_soc_top.sv
 
-../rtl/utils/full_handshake_rx.sv
-../rtl/utils/full_handshake_tx.sv
 ../rtl/utils/gen_buf.sv
 ../rtl/utils/gen_dff.sv
 ../rtl/utils/gen_ram.sv
-../rtl/utils/vld_rdy.sv
-
-
+../rtl/utils/cdc_2phase.sv
diff --git a/rtl/core/clint.sv b/rtl/core/clint.sv
deleted file mode 100644
index 9f1277a..0000000
--- a/rtl/core/clint.sv
+++ /dev/null
@@ -1,223 +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.                                          
- */
-
-`include "defines.sv"
-
-// core local interruptor module
-// 核心中断管理、仲裁模块
-module clint(
-
-    input wire clk,
-    input wire rst_n,
-
-    // from core
-    input wire[`INT_WIDTH-1:0] int_flag_i,      // 中断输入信号
-
-    // from exu
-    input wire inst_ecall_i,                    // ecall指令
-    input wire inst_ebreak_i,                   // ebreak指令
-    input wire inst_mret_i,                     // mret指令
-    input wire[31:0] inst_addr_i,               // 指令地址
-    input wire jump_flag_i,
-    input wire mem_access_misaligned_i,
-
-    // from csr_reg
-    input wire[31:0] csr_mtvec_i,               // mtvec寄存器
-    input wire[31:0] csr_mepc_i,                // mepc寄存器
-    input wire[31:0] csr_mstatus_i,             // mstatus寄存器
-
-    // to csr_reg
-    output reg csr_we_o,                        // 写CSR寄存器标志
-    output reg[31:0] csr_waddr_o,               // 写CSR寄存器地址
-    output reg[31:0] csr_wdata_o,               // 写CSR寄存器数据
-
-    // to pipe_ctrl
-    output wire stall_flag_o,                   // 流水线暂停标志
-    output wire[31:0] int_addr_o,               // 中断入口地址
-    output wire int_assert_o                    // 中断标志
-
-    );
-
-    // 中断状态定义
-    localparam S_INT_IDLE            = 4'b0001;
-    localparam S_INT_SYNC_ASSERT     = 4'b0010;
-    localparam S_INT_ASYNC_ASSERT    = 4'b0100;
-    localparam S_INT_MRET            = 4'b1000;
-
-    // 写CSR寄存器状态定义
-    localparam S_CSR_IDLE            = 5'b00001;
-    localparam S_CSR_MSTATUS         = 5'b00010;
-    localparam S_CSR_MEPC            = 5'b00100;
-    localparam S_CSR_MSTATUS_MRET    = 5'b01000;
-    localparam S_CSR_MCAUSE          = 5'b10000;
-
-    reg[3:0] int_state;
-    reg[4:0] csr_state;
-    reg[31:0] inst_addr;
-    reg[31:0] cause;
-
-    wire global_int_en = csr_mstatus_i[3];
-
-    assign stall_flag_o = ((int_state != S_INT_IDLE) | (csr_state != S_CSR_IDLE))? 1'b1: 1'b0;
-
-    // 将跳转标志放在流水线上传递
-    wire pc_state_jump_flag;
-    gen_rst_0_dff #(1) pc_state_dff(clk, rst_n, jump_flag_i, pc_state_jump_flag);
-
-    wire if_state_jump_flag;
-    gen_rst_0_dff #(1) if_state_dff(clk, rst_n, pc_state_jump_flag, if_state_jump_flag);
-
-    wire id_state_jump_flag;
-    gen_rst_0_dff #(1) id_state_dff(clk, rst_n, if_state_jump_flag, id_state_jump_flag);
-
-    wire ex_state_jump_flag;
-    gen_rst_0_dff #(1) ex_state_dff(clk, rst_n, id_state_jump_flag, ex_state_jump_flag);
-
-    wire[3:0] state_jump_flag = {pc_state_jump_flag, if_state_jump_flag, id_state_jump_flag, ex_state_jump_flag};
-    // 如果流水线没有冲刷完成则不响应中断
-    wire inst_addr_valid = (~(|state_jump_flag)) | ex_state_jump_flag;
-
-
-    // 中断仲裁逻辑
-    always @ (*) begin
-        // 同步中断
-        if (inst_ecall_i | inst_ebreak_i | mem_access_misaligned_i) begin
-            int_state = S_INT_SYNC_ASSERT;
-        // 异步中断
-        end else if ((int_flag_i != `INT_NONE) & global_int_en & inst_addr_valid) begin
-            int_state = S_INT_ASYNC_ASSERT;
-        // 中断返回
-        end else if (inst_mret_i) begin
-            int_state = S_INT_MRET;
-        // 无中断响应
-        end else begin
-            int_state = S_INT_IDLE;
-        end
-    end
-
-    // 写CSR寄存器状态切换
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            csr_state <= S_CSR_IDLE;
-            cause <= 32'h0;
-            inst_addr <= 32'h0;
-        end else begin
-            case (csr_state)
-                S_CSR_IDLE: begin
-                    case (int_state)
-                        // 同步中断
-                        S_INT_SYNC_ASSERT: begin
-                            csr_state <= S_CSR_MEPC;
-                            // 在中断处理函数里会将中断返回地址加4
-                            inst_addr <= inst_addr_i;
-                            cause <= inst_ebreak_i? 32'd3:
-                                     inst_ecall_i? 32'd11:
-                                     mem_access_misaligned_i? 32'd4:
-                                     32'd10;
-                        end
-                        // 异步中断
-                        S_INT_ASYNC_ASSERT: begin
-                            csr_state <= S_CSR_MEPC;
-                            inst_addr <= inst_addr_i;
-                            // 定时器中断
-                            cause <= 32'h80000004;
-                        end
-                        // 中断返回
-                        S_INT_MRET: begin
-                            csr_state <= S_CSR_MSTATUS_MRET;
-                        end
-                    endcase
-                end
-                S_CSR_MEPC: begin
-                    csr_state <= S_CSR_MSTATUS;
-                end
-                S_CSR_MSTATUS: begin
-                    csr_state <= S_CSR_MCAUSE;
-                end
-                S_CSR_MCAUSE: begin
-                    csr_state <= S_CSR_IDLE;
-                end
-                S_CSR_MSTATUS_MRET: begin
-                    csr_state <= S_CSR_IDLE;
-                end
-                default: begin
-                    csr_state <= S_CSR_IDLE;
-                end
-            endcase
-        end
-    end
-
-    // 发出中断信号前，先写几个CSR寄存器
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            csr_we_o <= 1'b0;
-            csr_waddr_o <= 32'h0;
-            csr_wdata_o <= 32'h0;
-        end else begin
-            case (csr_state)
-                // 将mepc寄存器的值设为当前指令地址
-                S_CSR_MEPC: begin
-                    csr_we_o <= 1'b1;
-                    csr_waddr_o <= {20'h0, `CSR_MEPC};
-                    csr_wdata_o <= inst_addr;
-                end
-                // 写中断产生的原因
-                S_CSR_MCAUSE: begin
-                    csr_we_o <= 1'b1;
-                    csr_waddr_o <= {20'h0, `CSR_MCAUSE};
-                    csr_wdata_o <= cause;
-                end
-                // 关闭全局中断
-                S_CSR_MSTATUS: begin
-                    csr_we_o <= 1'b1;
-                    csr_waddr_o <= {20'h0, `CSR_MSTATUS};
-                    csr_wdata_o <= {csr_mstatus_i[31:4], 1'b0, csr_mstatus_i[2:0]};
-                end
-                // 中断返回
-                S_CSR_MSTATUS_MRET: begin
-                    csr_we_o <= 1'b1;
-                    csr_waddr_o <= {20'h0, `CSR_MSTATUS};
-                    csr_wdata_o <= {csr_mstatus_i[31:4], csr_mstatus_i[7], csr_mstatus_i[2:0]};
-                end
-                default: begin
-                    csr_we_o <= 1'b0;
-                    csr_waddr_o <= 32'h0;
-                    csr_wdata_o <= 32'h0;
-                end
-            endcase
-        end
-    end
-
-    reg in_int_context;
-
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            in_int_context <= 1'b0;
-        end else begin
-            if (csr_state == S_CSR_MSTATUS_MRET) begin
-                in_int_context <= 1'b0;
-            end else if (csr_state != S_CSR_IDLE) begin
-                in_int_context <= 1'b1;
-            end
-        end
-    end
-
-    assign int_assert_o = (csr_state == S_CSR_MCAUSE) | (csr_state == S_CSR_MSTATUS_MRET);
-    assign int_addr_o = (csr_state == S_CSR_MCAUSE)? csr_mtvec_i:
-                        (csr_state == S_CSR_MSTATUS_MRET)? csr_mepc_i:
-                        32'h0;
-
-endmodule
diff --git a/rtl/core/csr_reg.sv b/rtl/core/csr_reg.sv
index a776c2b..205cd75 100644
--- a/rtl/core/csr_reg.sv
+++ b/rtl/core/csr_reg.sv
@@ -36,7 +36,9 @@ module csr_reg(
 
     output wire[31:0] mtvec_o,              // mtvec瀵勫瓨鍣ㄥ��
     output wire[31:0] mepc_o,               // mepc瀵勫瓨鍣ㄥ��
-    output wire[31:0] mstatus_o             // mstatus瀵勫瓨鍣ㄥ��
+    output wire[31:0] mstatus_o,            // mstatus瀵勫瓨鍣ㄥ��
+    output wire[31:0] mie_o,                // mie瀵勫瓨鍣ㄥ��
+    output wire[31:0] dpc_o                 // dpc瀵勫瓨鍣ㄥ��
 
     );
 
@@ -67,6 +69,9 @@ module csr_reg(
     reg[31:0] mhartid_d;
     wire[31:0] mhartid_q;
     reg mhartid_we;
+    reg[31:0] dpc_d;
+    wire[31:0] dpc_q;
+    reg dpc_we;
 
     reg[63:0] cycle;
 
@@ -83,6 +88,8 @@ module csr_reg(
     assign mtvec_o = mtvec_q;
     assign mepc_o = mepc_q;
     assign mstatus_o = mstatus_q;
+    assign mie_o = mie_q;
+    assign dpc_o = dpc_q;
 
     reg[31:0] exu_rdata;
 
@@ -122,6 +129,9 @@ module csr_reg(
             `CSR_MHARTID: begin
                 exu_rdata = mhartid_q;
             end
+            `CSR_DPC: begin
+                exu_rdata = dpc_q;
+            end
             default: begin
                 exu_rdata = 32'h0;
             end
@@ -154,6 +164,8 @@ module csr_reg(
         dscratch1_we = 1'b0;
         mhartid_d = mhartid_q;
         mhartid_we = 1'b0;
+        dpc_d = dpc_q;
+        dpc_we = 1'b0;
 
         if (we) begin
             case (waddr[11:0])
@@ -193,6 +205,10 @@ module csr_reg(
                     mhartid_d = wdata;
                     mhartid_we = 1'b1;
                 end
+                `CSR_DPC: begin
+                    dpc_d = wdata;
+                    dpc_we = 1'b1;
+                end
                 default:;
             endcase
         end
@@ -297,4 +313,15 @@ module csr_reg(
         .rdata_o(mhartid_q)
     );
 
+    // dpc
+    csr #(
+        .RESET_VAL(32'h0)
+    ) dpc_csr (
+        .clk(clk),
+        .rst_n(rst_n),
+        .wdata_i(dpc_d),
+        .we_i(dpc_we),
+        .rdata_o(dpc_q)
+    );
+
 endmodule
diff --git a/rtl/core/defines.sv b/rtl/core/defines.sv
index 20c0e40..0864ded 100644
--- a/rtl/core/defines.sv
+++ b/rtl/core/defines.sv
@@ -54,6 +54,7 @@
 `define INST_MRET    32'h30200073
 `define INST_ECALL   32'h00000073
 `define INST_EBREAK  32'h00100073
+`define INST_DRET    32'h7b200073
 
 // 鎸囦护璇戠爜淇℃伅
 `define DECINFO_GRP_BUS           2:0
@@ -118,12 +119,13 @@
 `define DECINFO_MEM_SH            (`DECINFO_GRP_WIDTH+6)
 `define DECINFO_MEM_SW            (`DECINFO_GRP_WIDTH+7)
 
-`define DECINFO_SYS_BUS_WIDTH     (`DECINFO_GRP_WIDTH+5)
+`define DECINFO_SYS_BUS_WIDTH     (`DECINFO_GRP_WIDTH+6)
 `define DECINFO_SYS_ECALL         (`DECINFO_GRP_WIDTH+0)
 `define DECINFO_SYS_EBREAK        (`DECINFO_GRP_WIDTH+1)
 `define DECINFO_SYS_NOP           (`DECINFO_GRP_WIDTH+2)
 `define DECINFO_SYS_MRET          (`DECINFO_GRP_WIDTH+3)
 `define DECINFO_SYS_FENCE         (`DECINFO_GRP_WIDTH+4)
+`define DECINFO_SYS_DRET          (`DECINFO_GRP_WIDTH+5)
 
 // 鏈�闀跨殑閭ｇ粍
 `define DECINFO_WIDTH             `DECINFO_CSR_BUS_WIDTH
@@ -137,6 +139,8 @@
 `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
-`define CSR_MHARTID     12'hF14
diff --git a/rtl/core/exception.sv b/rtl/core/exception.sv
new file mode 100644
index 0000000..7fa12d7
--- /dev/null
+++ b/rtl/core/exception.sv
@@ -0,0 +1,262 @@
+ /*                                                                      
+ Copyright 2021 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.                                          
+ */
+
+`include "defines.sv"
+
+`define CAUSE_IRQ_EXTERNAL_M    {1'b1, 31'd11}
+`define CAUSE_IRQ_SOFTWARE_M    {1'b1, 31'd3}
+`define CAUSE_IRQ_TIMER_M       {1'b1, 31'd7}
+
+`define CAUSE_EXCEP_ECALL_M     {1'b0, 31'd11}
+`define CAUSE_EXCEP_EBREAK_M    {1'b0, 31'd3}
+
+`define MIE_MTIE_BIT            7
+`define MIE_MEIE_BIT            11
+`define MIE_MSIE_BIT            3
+
+
+
+module exception (
+
+    input wire clk,
+    input wire rst_n,
+
+    input wire inst_valid_i,
+    input wire inst_ecall_i,                    // ecall鎸囦护
+    input wire inst_ebreak_i,                   // ebreak鎸囦护
+    input wire inst_mret_i,                     // mret鎸囦护
+    input wire inst_dret_i,                     // dret鎸囦护
+    input wire[31:0] inst_addr_i,               // 鎸囦护鍦板潃
+
+    input wire[31:0] mtvec_i,                   // mtvec瀵勫瓨鍣�
+    input wire[31:0] mepc_i,                    // mepc瀵勫瓨鍣�
+    input wire[31:0] mstatus_i,                 // mstatus瀵勫瓨鍣�
+    input wire[31:0] mie_i,                     // mie瀵勫瓨鍣�
+    input wire[31:0] dpc_i,                     // dpc瀵勫瓨鍣�
+
+    input wire irq_software_i,
+    input wire irq_timer_i,
+    input wire irq_external_i,
+    input wire[14:0] irq_fast_i,
+
+    input wire[31:0] debug_halt_addr_i,
+    input wire debug_req_i,
+
+    output wire csr_we_o,                        // 鍐機SR瀵勫瓨鍣ㄦ爣蹇�
+    output wire[31:0] csr_waddr_o,               // 鍐機SR瀵勫瓨鍣ㄥ湴鍧�
+    output wire[31:0] csr_wdata_o,               // 鍐機SR瀵勫瓨鍣ㄦ暟鎹�
+
+    output wire stall_flag_o,                   // 娴佹按绾挎殏鍋滄爣蹇�
+    output wire[31:0] int_addr_o,               // 涓柇鍏ュ彛鍦板潃
+    output wire int_assert_o                    // 涓柇鏍囧織
+
+    );
+
+
+    localparam ILLEGAL_INSTR_OFFSET         = 0;
+    localparam INSTR_ADDR_MISA_OFFSET       = 4;
+    localparam ECALL_OFFSET                 = 8;
+    localparam EBREAK_OFFSET                = 12;
+    localparam LOAD_MISA_OFFSET             = 16;
+    localparam STORE_MISA_OFFSET            = 20;
+    localparam RESERVED1_EXCEPTION_OFFSET   = 24;
+    localparam RESERVED2_EXCEPTION_OFFSET   = 28;
+
+    localparam EXTERNAL_INT_OFFSET          = 32;
+    localparam SOFTWARE_INT_OFFSET          = 36;
+    localparam TIMER_INT_OFFSET             = 40;
+    localparam FAST_INT_OFFSET              = 44;
+
+
+    localparam S_IDLE       = 3'b001;
+    localparam S_W_MEPC     = 3'b010;
+    localparam S_ASSERT     = 3'b100;
+
+
+    reg debug_mode_d, debug_mode_q;
+    reg[2:0] state_d, state_q;
+    reg[31:0] assert_addr_d, assert_addr_q;
+    reg[31:0] return_addr_d, return_addr_q;
+    reg csr_we;
+    reg[31:0] csr_waddr;
+    reg[31:0] csr_wdata;
+
+    wire global_int_en;
+
+    assign global_int_en = mstatus_i[3];
+
+    reg[3:0] fast_irq_id;
+    wire fast_irq_req;
+
+    always @ (*) begin
+        if      (irq_fast_i[ 0]) fast_irq_id = 4'd0;
+        else if (irq_fast_i[ 1]) fast_irq_id = 4'd1;
+        else if (irq_fast_i[ 2]) fast_irq_id = 4'd2;
+        else if (irq_fast_i[ 3]) fast_irq_id = 4'd3;
+        else if (irq_fast_i[ 4]) fast_irq_id = 4'd4;
+        else if (irq_fast_i[ 5]) fast_irq_id = 4'd5;
+        else if (irq_fast_i[ 6]) fast_irq_id = 4'd6;
+        else if (irq_fast_i[ 7]) fast_irq_id = 4'd7;
+        else if (irq_fast_i[ 8]) fast_irq_id = 4'd8;
+        else if (irq_fast_i[ 9]) fast_irq_id = 4'd9;
+        else if (irq_fast_i[10]) fast_irq_id = 4'd10;
+        else if (irq_fast_i[11]) fast_irq_id = 4'd11;
+        else if (irq_fast_i[12]) fast_irq_id = 4'd12;
+        else if (irq_fast_i[13]) fast_irq_id = 4'd13;
+        else                     fast_irq_id = 4'd14;
+    end
+
+    assign fast_irq_req = |irq_fast_i;
+
+    reg interrupt_req_tmp;
+    reg[31:0] interrupt_cause;
+    reg[31:0] interrupt_offset;
+    wire interrupt_req = inst_valid_i & interrupt_req_tmp;
+
+    always @ (*) begin
+        if (fast_irq_req) begin
+            interrupt_req_tmp = 1'b1;
+            interrupt_cause = {1'b1, {26{1'b0}}, 1'b1, fast_irq_id};
+            interrupt_offset = {fast_irq_id, 2'b0} + FAST_INT_OFFSET;
+        end else if (irq_external_i & mie_i[`MIE_MEIE_BIT]) begin
+            interrupt_req_tmp = 1'b1;
+            interrupt_cause = `CAUSE_IRQ_EXTERNAL_M;
+            interrupt_offset = EXTERNAL_INT_OFFSET;
+        end else if (irq_software_i & mie_i[`MIE_MSIE_BIT]) begin
+            interrupt_req_tmp = 1'b1;
+            interrupt_cause = `CAUSE_IRQ_SOFTWARE_M;
+            interrupt_offset = SOFTWARE_INT_OFFSET;
+        end else if (irq_timer_i & mie_i[`MIE_MTIE_BIT]) begin
+            interrupt_req_tmp = 1'b1;
+            interrupt_cause = `CAUSE_IRQ_TIMER_M;
+            interrupt_offset = TIMER_INT_OFFSET;
+        end else begin
+            interrupt_req_tmp = 1'b0;
+            interrupt_cause = 32'h0;
+            interrupt_offset = 32'h0;
+        end
+    end
+
+    reg exception_req;
+    reg[31:0] exception_cause;
+    reg[31:0] exception_offset;
+
+    always @ (*) begin
+        if (inst_ecall_i) begin
+            exception_req = 1'b1;
+            exception_cause = `CAUSE_EXCEP_ECALL_M;
+            exception_offset = ECALL_OFFSET;
+        end else if (inst_ebreak_i) begin
+            exception_req = 1'b1;
+            exception_cause = `CAUSE_EXCEP_EBREAK_M;
+            exception_offset = EBREAK_OFFSET;
+        end else begin
+            exception_req = 1'b0;
+            exception_cause = 32'h0;
+            exception_offset = 32'h0;
+        end
+    end
+
+    wire int_or_exception_req;
+    wire[31:0] int_or_exception_cause;
+    wire[31:0] int_or_exception_offset;
+
+    assign int_or_exception_req     = (interrupt_req & global_int_en) | exception_req;
+    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;
+
+    assign stall_flag_o = ((state_q != S_IDLE) & (state_q != S_ASSERT)) |
+                          (interrupt_req & global_int_en) | exception_req |
+                          debug_mode_req |
+                          inst_mret_i |
+                          inst_dret_i;
+
+    always @ (*) begin
+        state_d = state_q;
+        assert_addr_d = assert_addr_q;
+        debug_mode_d = debug_mode_q;
+        return_addr_d = return_addr_q;
+        csr_we = 1'b0;
+        csr_waddr = 32'h0;
+        csr_wdata = 32'h0;
+
+        case (state_q)
+            S_IDLE: begin
+                if (int_or_exception_req) begin
+                    csr_we = 1'b1;
+                    csr_waddr = {20'h0, `CSR_MCAUSE};
+                    csr_wdata = int_or_exception_cause;
+                    assert_addr_d = mtvec_i + int_or_exception_offset;
+                    return_addr_d = inst_addr_i;
+                    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;
+                    assert_addr_d = debug_halt_addr_i;
+                    state_d = S_ASSERT;
+                end else if (inst_mret_i) begin
+                    assert_addr_d = mepc_i;
+                    state_d = S_ASSERT;
+                end else if (inst_dret_i) begin
+                    assert_addr_d = dpc_i;
+                    state_d = S_ASSERT;
+                    debug_mode_d = 1'b0;
+                end
+            end
+
+            S_W_MEPC: begin
+                csr_we = 1'b1;
+                csr_waddr = {20'h0, `CSR_MEPC};
+                csr_wdata = return_addr_q;
+                state_d = S_ASSERT;
+            end
+
+            S_ASSERT: begin
+                csr_we = 1'b0;
+                state_d = S_IDLE;
+            end
+
+            default:;
+
+        endcase
+    end
+
+    assign csr_we_o = csr_we;
+    assign csr_waddr_o = csr_waddr;
+    assign csr_wdata_o = csr_wdata;
+
+    assign int_assert_o = (state_q == S_ASSERT);
+    assign int_addr_o   = assert_addr_q;
+
+    always @ (posedge clk or negedge rst_n) begin
+        if (!rst_n) begin
+            state_q <= S_IDLE;
+            assert_addr_q <= 32'h0;
+            debug_mode_q <= 1'b0;
+            return_addr_q <= 32'h0;
+        end else begin
+            state_q <= state_d;
+            assert_addr_q <= assert_addr_d;
+            debug_mode_q <= debug_mode_d;
+            return_addr_q <= return_addr_d;
+        end
+    end
+
+endmodule
diff --git a/rtl/core/exu.sv b/rtl/core/exu.sv
index 024d2c9..6bb6860 100644
--- a/rtl/core/exu.sv
+++ b/rtl/core/exu.sv
@@ -30,6 +30,7 @@ module exu(
     output wire inst_ecall_o,               // ecall鎸囦护
     output wire inst_ebreak_o,              // ebreak鎸囦护
     output wire inst_mret_o,                // mret鎸囦护
+    output wire inst_dret_o,                // dret鎸囦护
 
     // mem
     input wire[31:0] mem_rdata_i,           // 鍐呭瓨杈撳叆鏁版嵁
@@ -143,6 +144,7 @@ module exu(
     wire sys_op_ecall_o;
     wire sys_op_ebreak_o;
     wire sys_op_fence_o;
+    wire sys_op_dret_o;
 
     exu_dispatch u_exu_dispatch(
         // input
@@ -219,12 +221,14 @@ module exu(
         .sys_op_mret_o(sys_op_mret_o),
         .sys_op_ecall_o(sys_op_ecall_o),
         .sys_op_ebreak_o(sys_op_ebreak_o),
-        .sys_op_fence_o(sys_op_fence_o)
+        .sys_op_fence_o(sys_op_fence_o),
+        .sys_op_dret_o(sys_op_dret_o)
     );
 
     assign inst_ecall_o = sys_op_ecall_o;
     assign inst_ebreak_o = sys_op_ebreak_o;
     assign inst_mret_o = sys_op_mret_o;
+    assign inst_dret_o = sys_op_dret_o;
 
     wire[31:0] alu_res_o;
     wire[31:0] bjp_res_o;
diff --git a/rtl/core/exu_dispatch.sv b/rtl/core/exu_dispatch.sv
index 6b5a2f2..8f7192d 100644
--- a/rtl/core/exu_dispatch.sv
+++ b/rtl/core/exu_dispatch.sv
@@ -99,7 +99,8 @@ module exu_dispatch(
     output wire sys_op_mret_o,
     output wire sys_op_ecall_o,
     output wire sys_op_ebreak_o,
-    output wire sys_op_fence_o
+    output wire sys_op_fence_o,
+    output wire sys_op_dret_o
 
     );
 
@@ -206,5 +207,6 @@ module exu_dispatch(
     assign sys_op_ecall_o = sys_info[`DECINFO_SYS_ECALL];
     assign sys_op_ebreak_o = sys_info[`DECINFO_SYS_EBREAK];
     assign sys_op_fence_o = sys_info[`DECINFO_SYS_FENCE];
+    assign sys_op_dret_o = sys_info[`DECINFO_SYS_DRET];
 
 endmodule
diff --git a/rtl/core/idu.sv b/rtl/core/idu.sv
index 31016d6..a568143 100644
--- a/rtl/core/idu.sv
+++ b/rtl/core/idu.sv
@@ -156,6 +156,7 @@ module idu(
     wire inst_remu = opcode_0110011 & funct3_111 & funct7_0000001;
     wire inst_nop = (inst_i == `INST_NOP);
     wire inst_mret = (inst_i == `INST_MRET);
+    wire inst_dret = (inst_i == `INST_DRET);
 
     // 灏嗘寚浠ゅ垎绫�
     wire inst_type_load = opcode_0000011;
@@ -228,6 +229,7 @@ module idu(
     assign dec_sys_info_bus[`DECINFO_SYS_EBREAK] = inst_ebreak;
     assign dec_sys_info_bus[`DECINFO_SYS_NOP] = inst_nop;
     assign dec_sys_info_bus[`DECINFO_SYS_MRET] = inst_mret;
+    assign dec_sys_info_bus[`DECINFO_SYS_DRET] = inst_dret;
     assign dec_sys_info_bus[`DECINFO_SYS_FENCE] = inst_fence | inst_fence_i;
 
     // 鎸囦护涓殑绔嬪嵆鏁�
@@ -259,7 +261,7 @@ module idu(
     wire op_bjp = inst_jal | inst_jalr | inst_type_branch;
     wire op_muldiv = inst_type_muldiv;
     wire op_csr = inst_csrrw | inst_csrrwi | inst_csrrs | inst_csrrsi | inst_csrrc | inst_csrrci;
-    wire op_sys = inst_ebreak | inst_ecall | inst_nop | inst_mret | inst_fence | inst_fence_i;
+    wire op_sys = inst_ebreak | inst_ecall | inst_nop | inst_mret | inst_fence | inst_fence_i | inst_dret;
     wire op_mem = inst_type_load | inst_type_store;
 
     assign dec_info_bus_o = ({`DECINFO_WIDTH{op_alu}} & {{`DECINFO_WIDTH-`DECINFO_ALU_BUS_WIDTH{1'b0}}, dec_alu_info_bus}) |
diff --git a/rtl/core/tinyriscv_core.sv b/rtl/core/tinyriscv_core.sv
index 2f5680c..7bab2f0 100644
--- a/rtl/core/tinyriscv_core.sv
+++ b/rtl/core/tinyriscv_core.sv
@@ -49,7 +49,6 @@ module tinyriscv_core #(
     input wire irq_timer_i,
     input wire irq_external_i,
     input wire[14:0] irq_fast_i,
-    input wire irq_nm_i,
 
     // debug req signal
     input wire debug_req_i
@@ -114,6 +113,7 @@ module tinyriscv_core #(
     wire ex_inst_ecall_o;
     wire ex_inst_ebreak_o;
     wire ex_inst_mret_o;
+    wire ex_inst_dret_o;
     wire ex_inst_valid_o;
 
     // gpr_reg妯″潡杈撳嚭淇″彿
@@ -126,6 +126,8 @@ module tinyriscv_core #(
     wire[31:0] csr_mtvec_o;
     wire[31:0] csr_mepc_o;
     wire[31:0] csr_mstatus_o;
+    wire[31:0] csr_mie_o;
+    wire[31:0] csr_dpc_o;
 
     // pipe_ctrl妯″潡杈撳嚭淇″彿
     wire[31:0] ctrl_flush_addr_o;
@@ -197,7 +199,9 @@ module tinyriscv_core #(
         .clint_wdata_i(clint_csr_wdata_o),
         .mtvec_o(csr_mtvec_o),
         .mepc_o(csr_mepc_o),
-        .mstatus_o(csr_mstatus_o)
+        .mstatus_o(csr_mstatus_o),
+        .mie_o(csr_mie_o),
+        .dpc_o(csr_dpc_o)
     );
 
     ifu_idu u_ifu_idu(
@@ -283,6 +287,7 @@ module tinyriscv_core #(
         .inst_ecall_o(ex_inst_ecall_o),
         .inst_ebreak_o(ex_inst_ebreak_o),
         .inst_mret_o(ex_inst_mret_o),
+        .inst_dret_o(ex_inst_dret_o),
         .int_stall_i(clint_stall_flag_o),
         .csr_raddr_o(ex_csr_raddr_o),
         .csr_rdata_i(csr_ex_data_o),
@@ -300,19 +305,26 @@ module tinyriscv_core #(
         .rd_we_i(ie_rd_we_o)
     );
 
-    clint u_clint(
+    exception u_exception(
         .clk(clk),
         .rst_n(rst_n),
-        .int_flag_i(`INT_NONE),
+        .inst_valid_i(ex_inst_valid_o),
         .inst_ecall_i(ex_inst_ecall_o),
         .inst_ebreak_i(ex_inst_ebreak_o),
         .inst_mret_i(ex_inst_mret_o),
+        .inst_dret_i(ex_inst_dret_o),
         .inst_addr_i(ie_dec_pc_o),
-        .jump_flag_i(ex_jump_flag_o),
-        .mem_access_misaligned_i(ex_mem_access_misaligned_o),
-        .csr_mtvec_i(csr_mtvec_o),
-        .csr_mepc_i(csr_mepc_o),
-        .csr_mstatus_i(csr_mstatus_o),
+        .mtvec_i(csr_mtvec_o),
+        .mepc_i(csr_mepc_o),
+        .mstatus_i(csr_mstatus_o),
+        .mie_i(csr_mie_o),
+        .dpc_i(csr_dpc_o),
+        .irq_software_i(irq_software_i),
+        .irq_timer_i(irq_timer_i),
+        .irq_external_i(irq_external_i),
+        .irq_fast_i(irq_fast_i),
+        .debug_halt_addr_i(DEBUG_HALT_ADDR),
+        .debug_req_i(debug_req_i),
         .csr_we_o(clint_csr_we_o),
         .csr_waddr_o(clint_csr_waddr_o),
         .csr_wdata_o(clint_csr_wdata_o),
diff --git a/rtl/debug/debug_rom.sv b/rtl/debug/debug_rom.sv
new file mode 100644
index 0000000..229b7a5
--- /dev/null
+++ b/rtl/debug/debug_rom.sv
@@ -0,0 +1,71 @@
+/* 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.
+ *
+ * File: $filename.v
+ *
+ * Description: Auto-generated bootrom
+ */
+
+// Auto-generated code
+module debug_rom (
+  input  wire         clk_i,
+  input  wire         req_i,
+  input  wire [63:0]  addr_i,
+  output wire [63:0]  rdata_o
+);
+
+  localparam RomSize = 19;
+
+  wire [RomSize-1:0][63:0] mem;
+
+  assign mem = {
+    64'h00000000_7b200073,
+    64'h7b202473_7b302573,
+    64'h10852423_f1402473,
+    64'ha85ff06f_7b202473,
+    64'h7b302573_10052223,
+    64'h00100073_7b202473,
+    64'h7b302573_10052623,
+    64'h00c51513_00c55513,
+    64'h00000517_fd5ff06f,
+    64'hfa041ce3_00247413,
+    64'h40044403_00a40433,
+    64'hf1402473_02041c63,
+    64'h00147413_40044403,
+    64'h00a40433_10852023,
+    64'hf1402473_00c51513,
+    64'h00c55513_00000517,
+    64'h7b351073_7b241073,
+    64'h0ff0000f_04c0006f,
+    64'h07c0006f_00c0006f
+  };
+
+  reg [4:0] addr_q;
+
+  always @ (posedge clk_i) begin
+    if (req_i) begin
+      addr_q <= addr_i[7:3];
+    end
+  end
+
+  reg[63:0]  rdata;
+
+  // this prevents spurious Xes from propagating into
+  // the speculative fetch stage of the core
+  always @ (*) begin
+    rdata = 64'h0;
+    if (addr_q < 5'd19) begin
+        rdata = mem[addr_q];
+    end
+  end
+
+  assign rdata_o = rdata;
+
+endmodule
diff --git a/rtl/debug/jtag_def.sv b/rtl/debug/jtag_def.sv
new file mode 100644
index 0000000..5e3148d
--- /dev/null
+++ b/rtl/debug/jtag_def.sv
@@ -0,0 +1,130 @@
+ /*                                                                      
+ Copyright 2021 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.                                          
+ */
+
+
+`define DbgVersion013       4'h2
+`define ProgBufSize         5'h8
+`define DataCount           4'h2
+`define HaltAddress         64'h800
+`define ResumeAddress       `HaltAddress + 4
+`define ExceptionAddress    `HaltAddress + 8
+`define DataAddr            12'h380
+
+// dmi op
+`define DMI_OP_NOP          2'b00
+`define DMI_OP_READ         2'b01
+`define DMI_OP_WRITE        2'b10
+
+// DM regs addr
+`define Data0               6'h04
+`define Data1               6'h05
+`define Data2               6'h06
+`define Data3               6'h07
+`define DMControl           6'h10
+`define DMStatus            6'h11
+`define Hartinfo            6'h12
+`define AbstractCS          6'h16
+`define Command             6'h17
+`define ProgBuf0            6'h20
+`define ProgBuf1            6'h21
+`define ProgBuf2            6'h22
+`define ProgBuf3            6'h23
+`define ProgBuf4            6'h24
+`define ProgBuf5            6'h25
+`define ProgBuf6            6'h26
+`define ProgBuf7            6'h27
+`define ProgBuf8            6'h28
+`define ProgBuf9            6'h29
+`define ProgBuf10           6'h2A
+`define ProgBuf11           6'h2B
+`define ProgBuf12           6'h2C
+`define ProgBuf13           6'h2D
+`define ProgBuf14           6'h2E
+`define ProgBuf15           6'h2F
+`define SBAddress3          6'h37
+`define SBCS                6'h38
+`define SBAddress0          6'h39
+`define SBAddress1          6'h3A
+`define SBAddress2          6'h3B
+`define SBData0             6'h3C
+`define SBData1             6'h3D
+`define SBData2             6'h3E
+`define SBData3             6'h3F
+
+// dmstatus bit index
+`define Impebreak           22
+`define Allhavereset        19
+`define Anyhavereset        18
+`define Allresumeack        17
+`define Anyresumeack        16
+`define Allnonexistent      15
+`define Anynonexistent      14
+`define Allunavail          13
+`define Anyunavail          12
+`define Allrunning          11
+`define Anyrunning          10
+`define Allhalted           9
+`define Anyhalted           8
+`define Authenticated       7
+`define Authbusy            6
+`define Hasresethaltreq     5
+`define Confstrptrvalid     4
+`define Version             3:0
+
+// dmcontrol bit index
+`define Haltreq             31
+`define Resumereq           30
+`define Hartreset           29
+`define Ackhavereset        28
+`define Hasel               26
+`define Hartsello           25:16
+`define Hartselhi           15:6
+`define Setresethaltreq     3
+`define Clrresethaltreq     2
+`define Ndmreset            1
+`define Dmactive            0
+
+// abstractcs bit index
+`define Progbufsize         28:24
+`define Busy                12
+`define Cmderr              10:8
+`define Datacount           3:0
+
+// abstract command access register bit index
+`define Cmdtype             31:24
+`define Aarsize             22:20
+`define Aarpostincrement    19
+`define Postexec            18
+`define Transfer            17
+`define Write               16
+`define Regno               15:0
+
+// sbcs bit index
+`define Sbversion           31:29
+`define Sbbusyerror         22
+`define Sbbusy              21
+`define Sbreadonaddr        20
+`define Sbaccess            19:17
+`define Sbautoincrement     16
+`define Sbreadondata        15
+`define Sberror             14:12
+`define Sbasize             11:5
+`define Sbaccess128         4
+`define Sbaccess64          3
+`define Sbaccess32          2
+`define Sbaccess16          1
+`define Sbaccess8           0
+
diff --git a/rtl/debug/jtag_dm.sv b/rtl/debug/jtag_dm.sv
index 8965903..9de7be8 100644
--- a/rtl/debug/jtag_dm.sv
+++ b/rtl/debug/jtag_dm.sv
@@ -1,381 +1,370 @@
- /*                                                                      
- 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.                                          
- */
-
-`define DTM_OP_NOP        2'b00
-`define DTM_OP_READ       2'b01
-`define DTM_OP_WRITE      2'b10
-
-
-module jtag_dm #(
-    parameter DMI_ADDR_BITS = 6,
-    parameter DMI_DATA_BITS = 32,
-    parameter DMI_OP_BITS = 2)(
-
-    clk,
-    rst_n,
-
-    // rx
-    dm_ack_o,
-    dtm_req_valid_i,
-    dtm_req_data_i,
-
-    // tx
-    dtm_ack_i,
-    dm_resp_data_o,
-    dm_resp_valid_o,
-
-    dm_reg_we_o,
-    dm_reg_addr_o,
-    dm_reg_wdata_o,
-    dm_reg_rdata_i,
-    dm_mem_we_o,
-    dm_mem_addr_o,
-    dm_mem_wdata_o,
-    dm_mem_rdata_i,
-    dm_mem_sel_o,
-
-    req_valid_o,
-    req_ready_i,
-    rsp_valid_i,
-    rsp_ready_o,
-
-    dm_halt_req_o,
-    dm_reset_req_o
-
-    );
-
-    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 wire clk;
-    input wire rst_n;
-    output wire dm_ack_o;
-    input wire dtm_req_valid_i;
-    input wire[DTM_REQ_BITS-1:0] dtm_req_data_i;
-    input wire dtm_ack_i;
-    output wire[DM_RESP_BITS-1:0] dm_resp_data_o;
-    output wire dm_resp_valid_o;
-    output wire dm_reg_we_o;
-    output wire[4:0] dm_reg_addr_o;
-    output wire[31:0] dm_reg_wdata_o;
-    input wire[31:0] dm_reg_rdata_i;
-    output wire dm_mem_we_o;
-    output wire[31:0] dm_mem_addr_o;
-    output wire[31:0] dm_mem_wdata_o;
-    input wire[31:0] dm_mem_rdata_i;
-    output wire[3:0] dm_mem_sel_o;
-    output wire req_valid_o;
-    input wire req_ready_i;
-    input wire rsp_valid_i;
-    output wire rsp_ready_o;
-    output wire dm_halt_req_o;
-    output wire dm_reset_req_o;
-
-    // DM妯″潡瀵勫瓨鍣�
-    reg[31:0] dcsr;
-    reg[31:0] dmstatus;
-    reg[31:0] dmcontrol;
-    reg[31:0] hartinfo;
-    reg[31:0] abstractcs;
-    reg[31:0] data0;
-    reg[31:0] sbcs;
-    reg[31:0] sbaddress0;
-    reg[31:0] sbdata0;
-    reg[31:0] command;
-
-    // DM妯″潡瀵勫瓨鍣ㄥ湴鍧�
-    localparam DCSR       = 16'h7b0;
-    localparam DMSTATUS   = 6'h11;
-    localparam DMCONTROL  = 6'h10;
-    localparam HARTINFO   = 6'h12;
-    localparam ABSTRACTCS = 6'h16;
-    localparam DATA0      = 6'h04;
-    localparam SBCS       = 6'h38;
-    localparam SBADDRESS0 = 6'h39;
-    localparam SBDATA0    = 6'h3C;
-    localparam COMMAND    = 6'h17;
-    localparam DPC        = 16'h7b1;
-
-    localparam OP_SUCC    = 2'b00;
-
-    localparam STATE_IDLE = 3'b001;
-    localparam STATE_EXE  = 3'b010;
-    localparam STATE_END  = 3'b100;
-
-    reg[2:0] state;
-    reg[31:0] read_data;
-    reg dm_reg_we;
-    reg[4:0] dm_reg_addr;
-    reg[31:0] dm_reg_wdata;
-    reg dm_mem_we;
-    reg[31:0] dm_mem_addr;
-    reg[31:0] dm_mem_wdata;
-    reg[31:0] dm_mem_rdata;
-    reg dm_halt_req;
-    reg dm_reset_req;
-    reg need_resp;
-    reg is_read_reg;
-    wire rx_valid;
-    wire[DTM_REQ_BITS-1:0] rx_data;     // driver璇锋眰鏁版嵁
-    reg[DTM_REQ_BITS-1:0] rx_data_r;
-
-    wire[3:0] dm_mem_sel = (sbcs[19:17] == 3'd0)? 4'b0001:
-                           (sbcs[19:17] == 3'd1)? 4'b0011:
-                           4'b1111;
-    wire[2:0] address_inc_step = (sbcs[19:17] == 3'd0)? 3'd1:
-                                 (sbcs[19:17] == 3'd1)? 3'd2:
-                                 3'd4;
-    wire[31:0] sbaddress0_next = sbaddress0 + {29'h0, address_inc_step};
-    wire[DM_RESP_BITS-1:0] dm_resp_data;
-
-    wire[DMI_OP_BITS-1:0] op = rx_data_r[DMI_OP_BITS-1:0];
-    wire[DMI_DATA_BITS-1:0] data = rx_data_r[DMI_DATA_BITS+DMI_OP_BITS-1:DMI_OP_BITS];
-    wire[DMI_ADDR_BITS-1:0] address = rx_data_r[DTM_REQ_BITS-1:DMI_DATA_BITS+DMI_OP_BITS];
-
-    wire req_sys_bus = ~(address == DMSTATUS);
-
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            dm_mem_we <= 1'b0;
-            dm_reg_we <= 1'b0;
-            dm_halt_req <= 1'b0;
-            dm_reset_req <= 1'b0;
-            dm_mem_addr <= 32'h0;
-            dm_reg_addr <= 5'h0;
-            sbaddress0 <= 32'h0;
-            dcsr <= 32'h0;
-            hartinfo <= 32'h0;
-            sbcs <= 32'h20040404;
-            dmcontrol <= 32'h0;
-            abstractcs <= 32'h1000003;
-            data0 <= 32'h0;
-            sbdata0 <= 32'h0;
-            command <= 32'h0;
-            dm_reg_wdata <= 32'h0;
-            dm_mem_wdata <= 32'h0;
-            dm_mem_rdata <= 32'h0;
-            dmstatus <= 32'h430c82;
-            is_read_reg <= 1'b0;
-            read_data <= 32'h0;
-            need_resp <= 1'b0;
-            state <= STATE_IDLE;
-        end else begin
-            case (state)
-                STATE_IDLE: begin
-                    // 鎺ユ敹鍒癲river鐨勮姹�
-                    if (rx_valid) begin
-                        rx_data_r <= rx_data;
-                        state <= STATE_EXE;
-                    end
-                end
-                STATE_EXE: begin
-                    state <= STATE_END;
-                    need_resp <= 1'b1;
-                    case (op)
-                        `DTM_OP_READ: begin
-                            case (address)
-                                DMSTATUS: begin
-                                    read_data <= dmstatus;
-                                end
-                                DMCONTROL: begin
-                                    read_data <= dmcontrol;
-                                end
-                                HARTINFO: begin
-                                    read_data <= hartinfo;
-                                end
-                                SBCS: begin
-                                    read_data <= sbcs;
-                                end
-                                ABSTRACTCS: begin
-                                    read_data <= abstractcs;
-                                end
-                                DATA0: begin
-                                    if (is_read_reg == 1'b1) begin
-                                        read_data <= dm_reg_rdata_i;
-                                    end else begin
-                                        read_data <= data0;
-                                    end
-                                    is_read_reg <= 1'b0;
-                                end
-                                SBDATA0: begin
-                                    read_data <= dm_mem_rdata;
-                                    if (sbcs[16] == 1'b1) begin
-                                        sbaddress0 <= sbaddress0_next;
-                                    end
-                                    if (sbcs[15] == 1'b1) begin
-                                        dm_mem_addr <= sbaddress0_next;
-                                    end
-                                end
-                                default: begin
-                                    read_data <= {(DMI_DATA_BITS){1'b0}};
-                                end
-                            endcase
-                        end
-
-                        `DTM_OP_WRITE: begin
-                            read_data <= {(DMI_DATA_BITS){1'b0}};
-                            case (address)
-                                DMCONTROL: begin
-                                    // reset DM module
-                                    if (data[0] == 1'b0) begin
-                                        dcsr <= 32'hc0;
-                                        dmstatus <= 32'h430c82;  // not halted, all running
-                                        hartinfo <= 32'h0;
-                                        sbcs <= 32'h20040404;
-                                        abstractcs <= 32'h1000003;
-                                        dmcontrol <= data;
-                                        dm_halt_req <= 1'b0;
-                                        dm_reset_req <= 1'b0;
-                                    // DM is active
-                                    end else begin
-                                        // we have only one hart
-                                        dmcontrol <= (data & ~(32'h3fffc0)) | 32'h10000;
-                                        // halt
-                                        if (data[31] == 1'b1) begin
-                                            dm_halt_req <= 1'b1;
-                                            // clear ALLRUNNING ANYRUNNING and set ALLHALTED
-                                            dmstatus <= {dmstatus[31:12], 4'h3, dmstatus[7:0]};
-                                        // reset
-                                        end else if (data[1] == 1'b1) begin
-                                            dm_reset_req <= 1'b1;
-                                            dm_halt_req <= 1'b0;
-                                            dmstatus <= {dmstatus[31:12], 4'hc, dmstatus[7:0]};
-                                        // resume
-                                        end else if (dm_halt_req == 1'b1 && data[30] == 1'b1) begin
-                                            dm_halt_req <= 1'b0;
-                                            // set ALLRUNNING ANYRUNNING and clear ALLHALTED
-                                            dmstatus <= {dmstatus[31:12], 4'hc, dmstatus[7:0]};
-                                        end
-                                    end
-                                end
-                                COMMAND: begin
-                                    // access reg
-                                    if (data[31:24] == 8'h0) begin
-                                        if (data[22:20] > 3'h2) begin
-                                            abstractcs <= abstractcs | (1'b1 << 9);
-                                        end else begin
-                                            abstractcs <= abstractcs & (~(3'h7 << 8));
-                                            // read or write
-                                            if (data[18] == 1'b0) begin
-                                                dm_reg_addr <= data[15:0] - 16'h1000;
-                                                // read
-                                                if (data[16] == 1'b0) begin
-                                                    if (data[15:0] == DCSR) begin
-                                                        data0 <= dcsr;
-                                                    end else if (data[15:0] < 16'h1020) begin
-                                                        is_read_reg <= 1'b1;
-                                                    end
-                                                // write
-                                                end else begin
-                                                    if (data[15:0] < 16'h1020) begin
-                                                        dm_reg_we <= 1'b1;
-                                                        dm_reg_wdata <= data0;
-                                                    end
-                                                end
-                                            end
-                                        end
-                                    end
-                                end
-                                DATA0: begin
-                                    data0 <= data;
-                                end
-                                SBCS: begin
-                                    sbcs <= data;
-                                end
-                                SBADDRESS0: begin
-                                    sbaddress0 <= data;
-                                    if (sbcs[20] == 1'b1) begin
-                                        dm_mem_addr <= data;
-                                    end
-                                end
-                                SBDATA0: begin
-                                    sbdata0 <= data;
-                                    dm_mem_addr <= sbaddress0;
-                                    dm_mem_wdata <= data;
-                                    dm_mem_we <= 1'b1;
-                                    if (sbcs[16] == 1'b1) begin
-                                        sbaddress0 <= sbaddress0_next;
-                                    end
-                                end
-                            endcase
-                        end
-
-                        `DTM_OP_NOP: begin
-                            read_data <= {(DMI_DATA_BITS){1'b0}};
-                        end
-                    endcase
-                end
-                STATE_END: begin
-                    state <= STATE_IDLE;
-                    dm_mem_rdata <= dm_mem_rdata_i;
-                    need_resp <= 1'b0;
-                    dm_mem_we <= 1'b0;
-                    dm_reg_we <= 1'b0;
-                    dm_reset_req <= 1'b0;
-                end
-            endcase
-        end
-    end
-
-    wire jtag_req_hsked = (req_valid_o & req_ready_i);
-    wire jtag_rsp_hsked = (rsp_valid_i & rsp_ready_o);
-
-    assign rsp_ready_o = (~rst_n)? 1'b0: 1'b1;
-    assign dm_mem_sel_o = dm_mem_sel;
-
-
-    assign dm_reg_we_o = dm_reg_we;
-    assign dm_reg_addr_o = dm_reg_addr;
-    assign dm_reg_wdata_o = dm_reg_wdata;
-    assign dm_mem_we_o = dm_mem_we;
-    assign dm_mem_addr_o = dm_mem_addr;
-    assign dm_mem_wdata_o = dm_mem_wdata;
-
-    assign req_valid_o = (state != STATE_IDLE) & req_sys_bus;
-    assign dm_halt_req_o = dm_halt_req;
-    assign dm_reset_req_o = dm_reset_req;
-
-    assign dm_resp_data = {address, read_data, OP_SUCC};
-
-
-    full_handshake_tx #(
-        .DW(DM_RESP_BITS)
-    ) tx(
-        .clk(clk),
-        .rst_n(rst_n),
-        .ack_i(dtm_ack_i),
-        .req_i(need_resp),
-        .req_data_i(dm_resp_data),
-        .idle_o(),
-        .req_o(dm_resp_valid_o),
-        .req_data_o(dm_resp_data_o)
-    );
-
-    full_handshake_rx #(
-        .DW(DTM_REQ_BITS)
-    ) rx(
-        .clk(clk),
-        .rst_n(rst_n),
-        .req_i(dtm_req_valid_i),
-        .req_data_i(dtm_req_data_i),
-        .ack_o(dm_ack_o),
-        .recv_data_o(rx_data),
-        .recv_rdy_o(rx_valid)
-    );
-
-endmodule
+ /*                                                                      
+ 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.                                          
+ */
+
+`include "jtag_def.sv"
+
+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
+    )(
+
+    input wire                      clk,
+    input wire                      rst_n,
+
+    // from DMI
+    input  wire [DMI_REQ_BITS-1:0]  dmi_data_i,
+    input  wire                     dmi_valid_i,
+    output wire                     dm_ready_o,
+
+    // to DMI
+    output wire [DMI_RESP_BITS-1:0] dm_data_o,
+    output wire                     dm_valid_o,
+    input  wire                     dmi_ready_i,
+
+    output wire                     debug_req_o,
+    output wire                     ndmreset_o,
+
+    // jtag access mem devices(DM as master)
+    output wire                     master_req_o,
+    input  wire                     master_gnt_i,
+    input  wire                     master_rvalid_i,
+    output wire                     master_we_o,
+    output wire [3:0]               master_be_o,
+    output wire [31:0]              master_addr_o,
+    output wire [31:0]              master_wdata_o,
+    input  wire [31:0]              master_rdata_i,
+    input  wire                     master_err_i,
+
+    // core fetch instr or mem(DM as slave)
+    input  wire                     slave_req_i,
+    input  wire                     slave_we_i,
+    input  wire [31:0]              slave_addr_i,
+    input  wire [3:0]               slave_be_i,
+    input  wire [31:0]              slave_wdata_i,
+    output wire [31:0]              slave_rdata_o
+
+    );
+
+    localparam HARTINFO = {8'h0, 4'h2, 3'b0, 1'b1, `DataCount, `DataAddr};
+
+    wire halted;
+    wire resumeack;
+    wire sbbusy;
+    wire[DMI_OP_BITS-1:0] dm_op;
+    wire[DMI_ADDR_BITS-1:0] dm_op_addr;
+    wire[DMI_DATA_BITS-1:0] dm_op_data;
+
+    reg havereset_d, havereset_q;
+    reg clear_resumeack;
+    reg sbaddress_write_valid;
+    reg sbdata_write_valid;
+    reg[31:0] dm_resp_data_d, dm_resp_data_q;
+    wire[31:0] sba_sbaddress;
+    wire[31:0] dm_sbaddress;
+    wire resumereq;
+    wire cmdbusy;
+
+    // DM regs
+    reg[31:0] dmstatus;
+    reg[31:0] dmcontrol_d, dmcontrol_q;
+    reg[31:0] abstractcs;
+    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;
+
+    assign dm_sbaddress = sbaddress0_q;
+
+    assign dm_op      = dmi_data_i[DMI_OP_BITS-1:0];
+    assign dm_op_addr = dmi_data_i[DMI_REQ_BITS-1:DMI_DATA_BITS+DMI_OP_BITS];
+    assign dm_op_data = dmi_data_i[DMI_DATA_BITS+DMI_OP_BITS-1:DMI_OP_BITS];
+
+
+    localparam S_REQ  = 2'b01;
+    localparam S_RESP = 2'b10; 
+
+    reg[2:0] req_state_d, req_state_q;
+    reg dm_valid_d, dm_valid_q;
+
+    // response FSM
+    always @ (*) begin
+        req_state_d = req_state_q;
+        dm_valid_d = dm_valid_q;
+
+        case (req_state_q)
+            S_REQ: begin
+                if (dmi_valid_i & dm_ready_o) begin
+                    req_state_d = S_RESP;
+                    dm_valid_d = 1'b1;
+                end
+            end
+
+            S_RESP: begin
+                if (dmi_ready_i) begin
+                    dm_valid_d = 1'b0;
+                    req_state_d = S_REQ;
+                end
+            end
+
+            default:;
+        endcase
+    end
+
+    always @ (posedge clk or negedge rst_n) begin
+        if (!rst_n) begin
+            req_state_q <= S_REQ;
+            dm_valid_q <= 1'b0;
+        end else begin
+            req_state_q <= req_state_d;
+            dm_valid_q <= dm_valid_d;
+        end
+    end
+
+    // we always ready to receive dmi request
+    assign dm_ready_o = 1'b1;
+    assign dm_valid_o = dm_valid_q;
+    assign dm_data_o  = {{DMI_ADDR_BITS{1'b0}}, dm_resp_data_q, 2'b00};  // response successfully
+
+
+    // DMI read or write operation
+    always @ (*) begin
+        // dmstatus
+        dmstatus = 32'h0;
+        dmstatus[`Version]        = `DbgVersion013;
+        dmstatus[`Authenticated]  = 1'b1;
+        dmstatus[`Allresumeack]   = resumeack;
+        dmstatus[`Anyresumeack]   = resumeack;
+        dmstatus[`Allhavereset]   = havereset_q;
+        dmstatus[`Anyhavereset]   = havereset_q;
+        dmstatus[`Allhalted]      = halted;
+        dmstatus[`Anyhalted]      = halted;
+        dmstatus[`Allrunning]     = ~halted;
+        dmstatus[`Anyrunning]     = ~halted;
+
+        // abstractcs
+        cmderr_d = cmderr_q;
+        abstractcs = 32'h0;
+        abstractcs[`Datacount]   = `DataCount;
+        abstractcs[`Progbufsize] = `ProgBufSize;
+        abstractcs[`Busy]        = cmdbusy;
+        abstractcs[`Cmderr]      = cmderr_q;
+
+        havereset_d = havereset_q;
+        sbaddress0_d = sba_sbaddress;
+        dmcontrol_d = dmcontrol_q;
+        clear_resumeack = 1'b0;
+        sbaddress_write_valid = 1'b0;
+        sbdata_write_valid = 1'b0;
+
+        data0_d = data0_q;
+        sbcs_d = sbcs_q;
+        sbdata0_d = sbdata0_q;
+        dm_resp_data_d = dm_resp_data_q;
+
+        if (dmi_valid_i & dm_ready_o) begin
+            // read
+            if (dm_op == `DMI_OP_READ) begin
+                case (dm_op_addr)
+                    `DMStatus:  dm_resp_data_d = dmstatus;
+                    `DMControl: dm_resp_data_d = dmcontrol_q;
+                    `Hartinfo:  dm_resp_data_d = HARTINFO;
+                    `SBCS:      dm_resp_data_d = sbcs_q;
+                    `AbstractCS:dm_resp_data_d = abstractcs;
+                    default:;
+                endcase
+
+            // write
+            end else if (dm_op == `DMI_OP_WRITE) begin
+                case (dm_op_addr)
+                    `DMControl: begin
+                        dmcontrol_d = dm_op_data;
+                        if (dmcontrol_d[`Ackhavereset]) begin
+                            havereset_d = 1'b0;
+                        end
+                    end
+
+                    `Data0: begin
+                        data0_d = dm_op_data;
+                    end
+
+                    `SBCS: begin
+                        if (sbbusy) begin
+                            sbcs_d[`Sbbusyerror] = 1'b1;
+                        end else begin
+                            sbcs_d = dm_op_data;
+                            // write 1 to clear
+                            sbcs_d[`Sbbusyerror] = sbcs_q[`Sbbusyerror] & (~sbcs_d[`Sbbusyerror]);
+                            sbcs_d[`Sberror]     = sbcs_q[`Sberror]     & (~sbcs_d[`Sberror]);
+                        end
+                    end
+
+                    `SBAddress0: begin
+                        if (sbbusy | sbcs_d[`Sbbusyerror]) begin
+                            sbcs_d[`Sbbusyerror] = 1'b1;
+                        end else begin
+                            sbaddress0_d = dm_op_data;
+                            sbaddress_write_valid = (sbcs_q[`Sberror] == 3'b0);
+                        end
+                    end
+
+                    `SBData0: begin
+                        if (sbbusy | sbcs_d[`Sbbusyerror]) begin
+                            sbcs_d[`Sbbusyerror] = 1'b1;
+                        end else begin
+                            sbdata0_d = dm_op_data;
+                            sbdata_write_valid = (sbcs_q[`Sberror] == 3'b0);
+                        end
+                    end
+
+                    `AbstractCS: begin
+                        
+                    end
+
+                    default:;
+                endcase
+            // nop
+            end else begin
+
+            end
+        end
+
+        // dmcontrol
+        dmcontrol_d[`Hasel]           = 1'b0;
+        dmcontrol_d[`Hartreset]       = 1'b0;
+        dmcontrol_d[`Setresethaltreq] = 1'b0;
+        dmcontrol_d[`Clrresethaltreq] = 1'b0;
+        dmcontrol_d[`Ackhavereset]    = 1'b0;
+        dmcontrol_d[`Hartsello]       = 10'h1;
+        dmcontrol_d[`Hartselhi]       = 10'h0;
+        // 鏀跺埌resume璇锋眰鍚庢竻resume搴旂瓟
+        if (!dmcontrol_q[`Resumereq] && dmcontrol_d[`Resumereq]) begin
+            clear_resumeack = 1'b1;
+        end
+        // 鍙戝嚭resume鍚庡苟涓旀敹鍒板簲绛�
+        if (dmcontrol_q[`Resumereq] && resumeack) begin
+            // 娓卹esume璇锋眰浣�
+            dmcontrol_d[`Resumereq] = 1'b0;
+        end
+
+        // sbcs
+        sbcs_d[`Sbversion]            = 3'd1;
+        sbcs_d[`Sbbusy]               = sbbusy;
+        sbcs_d[`Sbasize]              = 7'd32;
+        sbcs_d[`Sbaccess128]          = 1'b0;
+        sbcs_d[`Sbaccess64]           = 1'b0;
+        sbcs_d[`Sbaccess32]           = 1'b1;
+        sbcs_d[`Sbaccess16]           = 1'b0;
+        sbcs_d[`Sbaccess8]            = 1'b0;
+        sbcs_d[`Sbaccess]             = 3'd2;
+
+        // 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];
+
+
+    always @ (posedge clk or negedge rst_n) begin
+        if (!rst_n) begin
+            dmcontrol_q <= 32'h0;
+            havereset_q <= 1'b1;
+            data0_q <= 32'h0;
+            sbcs_q <= 32'h0;
+            sbaddress0_q <= 32'h0;
+            sbdata0_q <= 32'h0;
+            dm_resp_data_q <= 32'h0;
+            cmderr_q <= 3'h0;
+        end else begin
+            if (!dmcontrol_q[`Dmactive]) begin
+                dmcontrol_q[`Haltreq] <= 1'b0;
+                dmcontrol_q[`Resumereq] <= 1'b0;
+                dmcontrol_q[`Hartreset] <= 1'b0;
+                dmcontrol_q[`Ackhavereset] <= 1'b0;
+                dmcontrol_q[`Hasel] <= 1'b0;
+                dmcontrol_q[`Hartsello] <= 10'b0;
+                dmcontrol_q[`Hartselhi] <= 10'b0;
+                dmcontrol_q[`Setresethaltreq] <= 1'b0;
+                dmcontrol_q[`Clrresethaltreq] <= 1'b0;
+                dmcontrol_q[`Ndmreset] <= 1'b0;
+                dmcontrol_q[`Dmactive] <= dmcontrol_d[`Dmactive];
+                data0_q <= 32'h0;
+                sbcs_q <= 32'h0;
+                sbaddress0_q <= 32'h0;
+                sbdata0_q <= 32'h0;
+                dm_resp_data_q <= 32'h0;
+                cmderr_q <= 3'h0;
+            end else begin
+                dmcontrol_q <= dmcontrol_d;
+                data0_q <= data0_d;
+                sbcs_q <= sbcs_d;
+                sbaddress0_q <= sbaddress0_d;
+                sbdata0_q <= sbdata0_d;
+                dm_resp_data_q <= dm_resp_data_d;
+                cmderr_q <= cmderr_d;
+            end
+            havereset_q <= havereset_d;
+        end
+    end
+
+    jtag_mem #(
+
+    ) u_jtag_mem (
+        .clk(clk),
+        .rst_n(rst_n),
+
+        .halted_o(halted),
+        .resumeack_o(resumeack),
+        .clear_resumeack_i(clear_resumeack),
+        .resumereq_i(resumereq),
+        .haltreq_i(debug_req_o),
+        .cmdbusy_o(cmdbusy),
+
+        .req_i(slave_req_i),
+        .we_i(slave_we_i),
+        .addr_i(slave_addr_i),
+        .be_i(slave_be_i),
+        .wdata_i(slave_wdata_i),
+        .rdata_o(slave_rdata_o)
+    );
+
+    jtag_sba #(
+
+    ) u_jtag_sba (
+        .clk(clk),
+        .rst_n(rst_n),
+        .sbbusy_o(sbbusy),
+        .master_req_o(master_req_o),
+        .master_gnt_i(master_gnt_i),
+        .master_rvalid_i(master_rvalid_i),
+        .master_we_o(master_we_o),
+        .master_be_o(master_be_o),
+        .master_addr_o(master_addr_o),
+        .master_wdata_o(master_wdata_o),
+        .master_rdata_i(master_rdata_i),
+        .master_err_i(master_err_i)
+    );
+
+endmodule
diff --git a/rtl/debug/jtag_dmi.sv b/rtl/debug/jtag_dmi.sv
new file mode 100644
index 0000000..e77bd32
--- /dev/null
+++ b/rtl/debug/jtag_dmi.sv
@@ -0,0 +1,76 @@
+ /*                                                                      
+ Copyright 2021 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_dmi #(
+    parameter DATA_WIDTH = 40
+    )(
+
+    // JTAG side(master side)
+    input  wire                     jtag_tck_i,
+    input  wire                     jtag_trst_ni,
+
+    input  wire [DATA_WIDTH-1:0]    jtag_data_i,
+    input  wire                     jtag_valid_i,
+    output wire                     jtag_ready_o,
+
+    output wire [DATA_WIDTH-1:0]    jtag_data_o,
+    output wire                     jtag_valid_o,
+    input  wire                     jtag_ready_i,
+
+    // Core side(slave side)
+    input  wire                     clk_i,
+    input  wire                     rst_ni,
+
+    input  wire [DATA_WIDTH-1:0]    core_data_i,
+    input  wire                     core_valid_i,
+    output wire                     core_ready_o,
+
+    output wire [DATA_WIDTH-1:0]    core_data_o,
+    output wire                     core_valid_o,
+    input  wire                     core_ready_i
+
+    );
+
+    cdc_2phase #(.DATA_WIDTH(DATA_WIDTH)) u_cdc_req (
+        .src_rst_ni  ( jtag_trst_ni     ),
+        .src_clk_i   ( jtag_tck_i       ),
+        .src_data_i  ( jtag_data_i      ),
+        .src_valid_i ( jtag_valid_i     ),
+        .src_ready_o ( jtag_ready_o     ),
+
+        .dst_rst_ni  ( rst_ni           ),
+        .dst_clk_i   ( clk_i            ),
+        .dst_data_o  ( core_data_o      ),
+        .dst_valid_o ( core_valid_o     ),
+        .dst_ready_i ( core_ready_i     )
+    );
+
+    cdc_2phase #(.DATA_WIDTH(DATA_WIDTH)) u_cdc_resp (
+        .src_rst_ni  ( rst_ni           ),
+        .src_clk_i   ( clk_i            ),
+        .src_data_i  ( core_data_i      ),
+        .src_valid_i ( core_valid_i     ),
+        .src_ready_o ( core_ready_o     ),
+
+        .dst_rst_ni  ( jtag_trst_ni     ),
+        .dst_clk_i   ( jtag_tck_i       ),
+        .dst_data_o  ( jtag_data_o      ),
+        .dst_valid_o ( jtag_valid_o     ),
+        .dst_ready_i ( jtag_ready_i     )
+    );
+
+endmodule
diff --git a/rtl/debug/jtag_driver.sv b/rtl/debug/jtag_driver.sv
index db8c2ca..ff4c843 100644
--- a/rtl/debug/jtag_driver.sv
+++ b/rtl/debug/jtag_driver.sv
@@ -1,290 +1,290 @@
- /*                                                                      
- 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
+ /*                                                                      
+ 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
new file mode 100644
index 0000000..7c2cac7
--- /dev/null
+++ b/rtl/debug/jtag_dtm.sv
@@ -0,0 +1,187 @@
+ /*                                                                      
+ Copyright 2021 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.                                          
+ */
+
+`include "jtag_def.sv"
+
+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
+    )(
+
+    input  wire                     jtag_tck_i,     // JTAG test clock pad
+    input  wire                     jtag_trst_ni,   // JTAG test reset pad
+
+    // to jtag_dmi
+    output wire [DTM_REQ_BITS-1:0]  dtm_data_o,
+    output wire                     dtm_valid_o,
+    // from jtag_dmi
+    input  wire                     dmi_ready_i,
+
+    // from jtag_dmi
+    input  wire [DMI_RESP_BITS-1:0] dmi_data_i,
+    input  wire                     dmi_valid_i,
+    // to jtag_dmi
+    output wire                     dtm_ready_o,
+
+    // from jtag_tap
+    input  wire                     tap_req_i,
+    input  wire [TAP_REQ_BITS-1:0]  tap_data_i,
+
+    // to jtag_tap
+    output wire [DTM_RESP_BITS-1:0] data_o,
+    output wire [31:0]              idcode_o,
+    output wire [31:0]              dtmcs_o
+
+    );
+
+    localparam IDCODE_VERSION       = 4'h1;
+    localparam IDCODE_PART_NUMBER   = 16'he200;
+    localparam IDCODE_MANUFLD       = 11'h537;
+
+    localparam DTM_VERSION          = 4'h1;
+
+    localparam S_IDLE               = 5'b00001;
+    localparam S_READ               = 5'b00010;
+    localparam S_WAIT_READ          = 5'b00100;
+    localparam S_WRITE              = 5'b01000;
+    localparam S_WAIT_WRITE         = 5'b10000;
+
+    reg[4:0] state_d;
+    reg[4:0] state_q;
+    reg dtm_valid;
+    reg[DTM_REQ_BITS-1:0] dtm_data_q;
+    reg[DTM_REQ_BITS-1:0] dtm_data_d;
+    reg[DTM_RESP_BITS-1:0] resp_tap_data_q;
+    reg is_busy;
+
+    wire[DTM_RESP_BITS-1:0] busy_response;
+    wire dtm_busy;
+    wire[DMI_OP_BITS-1:0] op;
+    wire[1:0] dtm_state;
+    wire[DMI_ADDR_BITS-1:0] addr_bits = DMI_ADDR_BITS[5:0];
+
+    assign idcode_o = {IDCODE_VERSION, IDCODE_PART_NUMBER, IDCODE_MANUFLD, 1'h1};
+    assign dtmcs_o = {14'b0,
+                      1'b0,         // dmihardreset
+                      1'b0,         // dmireset
+                      1'b0,
+                      3'h5,         // idle
+                      dtm_state,    // 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 dtm_state = is_busy ? 2'b01 : 2'b00;
+
+    assign op = tap_data_i[DMI_OP_BITS-1:0];
+
+
+    always @ (*) begin
+        state_d = state_q;
+        dtm_valid = 1'b0;
+        dtm_data_d = dtm_data_q;
+
+        case (state_q)
+            S_IDLE: begin
+                if (tap_req_i) begin
+                    if (op == `DMI_OP_READ) begin
+                        state_d = S_READ;
+                        dtm_data_d = tap_data_i;
+                    end else if (op == `DMI_OP_WRITE) begin
+                        state_d = S_WRITE;
+                        dtm_data_d = tap_data_i;
+                    end else begin
+                        state_d = S_IDLE;
+                    end
+                end else begin
+                    state_d = S_IDLE;
+                end
+            end
+
+            S_READ: begin
+                dtm_valid = 1'b1;
+                if (dmi_ready_i) begin
+                    state_d = S_WAIT_READ;
+                end
+            end
+
+            S_WAIT_READ: begin
+                if (dmi_valid_i) begin
+                    dtm_data_d = dmi_data_i;
+                    state_d = S_IDLE;
+                end
+            end
+
+            S_WRITE: begin
+                dtm_valid = 1'b1;
+                if (dmi_ready_i) begin
+                    state_d = S_WAIT_WRITE;
+                end
+            end
+
+            S_WAIT_WRITE: begin
+                if (dmi_valid_i) begin
+                    dtm_data_d = dmi_data_i;
+                    state_d = S_IDLE;
+                end
+            end
+
+            default: begin
+                dtm_data_d = {DTM_REQ_BITS{1'b0}};
+                state_d = S_IDLE;
+            end
+        endcase
+    end
+
+    assign dtm_valid_o = dtm_valid;
+    assign dtm_data_o = dtm_data_q;
+    // we will always be ready to core request
+    assign dtm_ready_o = 1'b1;
+
+    always @ (posedge jtag_tck_i or negedge jtag_trst_ni) begin
+        if (!jtag_trst_ni) begin
+            state_q <= S_IDLE;
+            dtm_data_q <= {DTM_REQ_BITS{1'b0}};
+        end else begin
+            state_q <= state_d;
+            dtm_data_q <= dtm_data_d;
+        end
+    end
+
+    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;
+        end else begin
+            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;
+
+endmodule
diff --git a/rtl/debug/jtag_mem.sv b/rtl/debug/jtag_mem.sv
new file mode 100644
index 0000000..8e63cd7
--- /dev/null
+++ b/rtl/debug/jtag_mem.sv
@@ -0,0 +1,194 @@
+ /*                                                                      
+ Copyright 2021 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_mem #(
+    )(
+
+    input wire                      clk,
+    input wire                      rst_n,
+
+    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                     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
+
+    );
+
+    // 16KB
+    localparam DbgAddressBits       = 12;
+    // 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 WhereToAddr          = 12'h300;
+    localparam FlagsBaseAddr        = 12'h400;
+    localparam FlagsEndAddr         = 12'h7FF;
+
+    localparam HaltedAddr           = 12'h100;
+    localparam GoingAddr            = 12'h104;
+    localparam ResumingAddr         = 12'h108;
+    localparam ExceptionAddr        = 12'h10C;
+
+    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;
+
+    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;
+    wire fwd_rom_d;
+    wire[63:0] rom_rdata;
+
+
+
+    // word mux for 32bit and 64bit buses
+    wire [63:0] word_mux;
+    assign word_mux = fwd_rom_q ? rom_rdata : rdata_q;
+    assign rdata_o = (word_enable32_q) ? word_mux[32 +: 32] : word_mux[0 +: 32];
+
+    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;
+
+        case (state_q)
+            S_IDLE: begin
+                if (resumereq_i && (!resuming_q) &&
+                    halted_q && (!haltreq_i)) begin
+                    state_d = S_RESUME;
+                end
+            end
+
+            S_RESUME: begin
+                resume = 1'b1;
+                if (resuming_q) begin
+                    state_d = S_IDLE;
+                end
+            end
+
+            default:;
+        endcase
+    end
+
+    always @ (*) begin
+        rdata_d = rdata_q;
+        halted_d = halted_q;
+        resuming_d = resuming_q;
+
+        if (clear_resumeack_i) begin
+            resuming_d = 1'b0;
+        end
+
+        // write
+        if (we_i) begin
+            case (addr_i[DbgAddressBits-1:0])
+                HaltedAddr: begin
+                    halted_d = 1'b1;
+                end
+
+                GoingAddr: begin
+
+                end
+
+                ResumingAddr: begin
+                    halted_d = 1'b0;
+                    resuming_d = 1'b1;
+                end
+
+                ExceptionAddr: begin
+
+                end
+
+                default:;
+            endcase
+        // read
+        end else begin
+            case (addr_i[DbgAddressBits-1:0])
+                // harts are polling for flags here
+                FlagsBaseAddr: begin
+                    rdata_d = {30'b0, resume, go};
+                end
+
+                default:;
+            endcase
+        end
+    end
+
+    wire[63:0] rom_addr;
+    assign rom_addr = {32'h0, addr_i};
+
+    assign fwd_rom_d = addr_i[DbgAddressBits-1:0] >= `HaltAddress;
+
+    debug_rom u_debug_rom (
+        .clk_i   ( clk       ),
+        .req_i   ( 1'b1      ),
+        .addr_i  ( rom_addr  ),
+        .rdata_o ( rom_rdata )
+    );
+
+    always @ (posedge clk or negedge rst_n) begin
+        if (!rst_n) begin
+            rdata_q <= 32'h0;
+            fwd_rom_q <= 1'b0;
+            word_enable32_q <= 1'b0;
+            halted_q <= 1'b0;
+            resuming_q <= 1'b0;
+        end else begin
+            rdata_q <= rdata_d;
+            fwd_rom_q <= fwd_rom_d;
+            word_enable32_q <= addr_i[2];
+            halted_q <= halted_d;
+            resuming_q <= resuming_d;
+        end
+    end
+
+    always @ (posedge clk or negedge rst_n) begin
+        if (!rst_n) begin
+            state_q <= S_IDLE;
+        end else begin
+            state_q <= state_d;
+        end
+    end
+
+endmodule
diff --git a/rtl/utils/vld_rdy.sv b/rtl/debug/jtag_sba.sv
similarity index 51%
rename from rtl/utils/vld_rdy.sv
rename to rtl/debug/jtag_sba.sv
index 92ab466..bc97911 100644
--- a/rtl/utils/vld_rdy.sv
+++ b/rtl/debug/jtag_sba.sv
@@ -1,57 +1,47 @@
- /*                                                                      
- 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 vld_rdy #(
-    parameter CUT_READY = 0)(
-
-    input wire clk,
-    input wire rst_n,
-
-    input wire vld_i,
-    output wire rdy_o,
-    input wire rdy_i,
-    output wire vld_o
-
-    );
-
-    wire vld_set;
-    wire vld_clr;
-    wire vld_ena;
-    wire vld_r;
-    wire vld_nxt;
-
-    // The valid will be set when input handshaked
-    assign vld_set = vld_i & rdy_o;
-    // The valid will be clr when output handshaked
-    assign vld_clr = vld_o & rdy_i;
-
-    assign vld_ena = vld_set | vld_clr;
-    assign vld_nxt = vld_set | (~vld_clr);
-
-    gen_en_dff #(1) vld_dff(clk, rst_n, vld_ena, vld_nxt, vld_r);
-
-    assign vld_o = vld_r;
-
-    if (CUT_READY == 1) begin
-        // If cut ready, then only accept when stage is not full
-        assign rdy_o = (~vld_r);
-    end else begin
-        // If not cut ready, then can accept when stage is not full or it is popping 
-        assign rdy_o = (~vld_r) | vld_clr;
-    end
-
-endmodule
+ /*                                                                      
+ Copyright 2021 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_sba #(
+
+    )(
+
+    input wire                      clk,
+    input wire                      rst_n,
+
+    output wire                     sbbusy_o,
+
+    output wire                     master_req_o,
+    input  wire                     master_gnt_i,
+    input  wire                     master_rvalid_i,
+    output wire                     master_we_o,
+    output wire [3:0]               master_be_o,
+    output wire [31:0]              master_addr_o,
+    output wire [31:0]              master_wdata_o,
+    input  wire [31:0]              master_rdata_i,
+    input  wire                     master_err_i
+
+    );
+
+
+    assign sbbusy_o = 1'b0;
+
+    assign master_req_o = 1'b0;
+    assign master_we_o = 1'b0;
+
+
+
+endmodule
diff --git a/rtl/debug/jtag_tap.sv b/rtl/debug/jtag_tap.sv
new file mode 100644
index 0000000..2e8740c
--- /dev/null
+++ b/rtl/debug/jtag_tap.sv
@@ -0,0 +1,175 @@
+ /*                                                                      
+ Copyright 2021 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_tap #(
+    parameter DMI_ADDR_BITS = 6,
+    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
+    )(
+
+    input wire  jtag_tck_i,     // JTAG test clock pad
+    input wire  jtag_tdi_i,     // JTAG test data input pad
+    input wire  jtag_tms_i,     // JTAG test mode select pad
+    input wire  jtag_trst_ni,   // JTAG test reset pad
+    output wire jtag_tdo_o,     // JTAG test data output pad
+
+    output wire tap_req_o,
+    output wire[TAP_REQ_BITS-1:0] tap_data_o,
+
+    input wire[DTM_RESP_BITS-1:0] dtm_data_i,
+    input wire[31:0] idcode_i,
+    input wire[31:0] dtmcs_i
+
+    );
+
+    localparam SHIFT_REG_BITS    = TAP_REQ_BITS;
+
+    localparam TEST_LOGIC_RESET  = 16'h0001;
+    localparam RUN_TEST_IDLE     = 16'h0002;
+    localparam SELECT_DR         = 16'h0004;
+    localparam CAPTURE_DR        = 16'h0008;
+    localparam SHIFT_DR          = 16'h0010;
+    localparam EXIT1_DR          = 16'h0020;
+    localparam PAUSE_DR          = 16'h0040;
+    localparam EXIT2_DR          = 16'h0080;
+    localparam UPDATE_DR         = 16'h0100;
+    localparam SELECT_IR         = 16'h0200;
+    localparam CAPTURE_IR        = 16'h0400;
+    localparam SHIFT_IR          = 16'h0800;
+    localparam EXIT1_IR          = 16'h1000;
+    localparam PAUSE_IR          = 16'h2000;
+    localparam EXIT2_IR          = 16'h4000;
+    localparam UPDATE_IR         = 16'h8000;
+
+    // DTM regs
+    localparam REG_BYPASS        = 5'b11111;
+    localparam REG_IDCODE        = 5'b00001;
+    localparam REG_DMI           = 5'b10001;
+    localparam REG_DTMCS         = 5'b10000;
+
+    reg[IR_BITS-1:0] ir_reg;
+    reg[SHIFT_REG_BITS-1:0] shift_reg;
+
+    reg[15:0] tap_state;
+    reg[15:0] next_state;
+
+    always @ (posedge jtag_tck_i or negedge jtag_trst_ni) begin
+        if (!jtag_trst_ni) begin
+            tap_state <= TEST_LOGIC_RESET;
+        end else begin
+            tap_state <= next_state;
+        end
+    end
+
+    // state switch
+    always @ (*) begin
+        case (tap_state)
+            TEST_LOGIC_RESET  : next_state = jtag_tms_i ? TEST_LOGIC_RESET : RUN_TEST_IDLE;
+            RUN_TEST_IDLE     : next_state = jtag_tms_i ? SELECT_DR        : RUN_TEST_IDLE;
+            SELECT_DR         : next_state = jtag_tms_i ? SELECT_IR        : CAPTURE_DR;
+            CAPTURE_DR        : next_state = jtag_tms_i ? EXIT1_DR         : SHIFT_DR;
+            SHIFT_DR          : next_state = jtag_tms_i ? EXIT1_DR         : SHIFT_DR;
+            EXIT1_DR          : next_state = jtag_tms_i ? UPDATE_DR        : PAUSE_DR;
+            PAUSE_DR          : next_state = jtag_tms_i ? EXIT2_DR         : PAUSE_DR;
+            EXIT2_DR          : next_state = jtag_tms_i ? UPDATE_DR        : SHIFT_DR;
+            UPDATE_DR         : next_state = jtag_tms_i ? SELECT_DR        : RUN_TEST_IDLE;
+            SELECT_IR         : next_state = jtag_tms_i ? TEST_LOGIC_RESET : CAPTURE_IR;
+            CAPTURE_IR        : next_state = jtag_tms_i ? EXIT1_IR         : SHIFT_IR;
+            SHIFT_IR          : next_state = jtag_tms_i ? EXIT1_IR         : SHIFT_IR;
+            EXIT1_IR          : next_state = jtag_tms_i ? UPDATE_IR        : PAUSE_IR;
+            PAUSE_IR          : next_state = jtag_tms_i ? EXIT2_IR         : PAUSE_IR;
+            EXIT2_IR          : next_state = jtag_tms_i ? UPDATE_IR        : SHIFT_IR;
+            UPDATE_IR         : next_state = jtag_tms_i ? SELECT_DR        : RUN_TEST_IDLE;
+            default           : next_state = TEST_LOGIC_RESET;
+        endcase
+    end
+
+    // IR or DR shift
+    always @ (posedge jtag_tck_i or negedge jtag_trst_ni) begin
+        if (!jtag_trst_ni) begin
+            shift_reg <= {SHIFT_REG_BITS{1'b0}};
+        end else begin
+            case (tap_state)
+                // IR
+                CAPTURE_IR: shift_reg <= {{(SHIFT_REG_BITS-1){1'b0}}, 1'b1}; //JTAG spec says it must be 2'b01
+                SHIFT_IR  : shift_reg <= {{(SHIFT_REG_BITS-IR_BITS){1'b0}}, jtag_tdi_i, 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_i};
+                                REG_DTMCS      : shift_reg <= {{(SHIFT_REG_BITS-DMI_DATA_BITS){1'b0}}, dtmcs_i};
+                                REG_DMI        : shift_reg <= dtm_data_i;
+                                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_i};    // in = out
+                                REG_IDCODE     : shift_reg <= {{(SHIFT_REG_BITS-DMI_DATA_BITS){1'b0}}, jtag_tdi_i, shift_reg[31:1]}; // right shift 1 bit
+                                REG_DTMCS      : shift_reg <= {{(SHIFT_REG_BITS-DMI_DATA_BITS){1'b0}}, jtag_tdi_i, shift_reg[31:1]}; // right shift 1 bit
+                                REG_DMI        : shift_reg <= {jtag_tdi_i, shift_reg[SHIFT_REG_BITS-1:1]}; // right shift 1 bit
+                                default        : shift_reg <= {{(SHIFT_REG_BITS-1){1'b0}} , jtag_tdi_i};
+                            endcase 
+            endcase
+        end
+    end
+
+    reg tap_req_q;
+    reg[TAP_REQ_BITS-1:0] tap_data_q;
+
+    // send request to DTM module
+    always @ (posedge jtag_tck_i or negedge jtag_trst_ni) begin
+        if (!jtag_trst_ni) begin
+            tap_req_q <= 1'b0;
+            tap_data_q <= {TAP_REQ_BITS{1'b0}};
+        end else begin
+            if ((tap_state == UPDATE_DR) && (ir_reg == REG_DMI)) begin
+                tap_req_q <= 1'b1;
+                tap_data_q <= shift_reg;
+            end else begin
+                tap_req_q <= 1'b0;
+            end
+        end
+    end
+
+    assign tap_req_o = tap_req_q;
+    assign tap_data_o = tap_data_q;
+
+    // ir_reg
+    always @ (negedge jtag_tck_i) begin
+        if (tap_state == TEST_LOGIC_RESET) begin
+            ir_reg <= REG_IDCODE;
+        end else if (tap_state == UPDATE_IR) begin
+            ir_reg <= shift_reg[IR_BITS-1:0];
+        end
+    end
+
+    reg jtag_tdo_q;
+
+    // TDO output
+    always @ (negedge jtag_tck_i) begin
+        if ((tap_state == SHIFT_IR) || (tap_state == SHIFT_DR)) begin
+            jtag_tdo_q <= shift_reg[0];
+        end else begin
+            jtag_tdo_q <= 1'b0;
+        end
+    end
+
+    assign jtag_tdo_o = jtag_tdo_q;
+
+endmodule
diff --git a/rtl/debug/jtag_top.sv b/rtl/debug/jtag_top.sv
index 089e058..a5233a4 100644
--- a/rtl/debug/jtag_top.sv
+++ b/rtl/debug/jtag_top.sv
@@ -1,117 +1,161 @@
- /*                                                                      
- 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.                                          
- */
-
-`include "../core/defines.sv"
-
-// JTAG椤跺眰妯″潡
-module jtag_top #(
-    parameter DMI_ADDR_BITS = 6,
-    parameter DMI_DATA_BITS = 32,
-    parameter DMI_OP_BITS = 2)(
-
-    input wire clk,
-    input wire jtag_rst_n,
-
-    input wire jtag_pin_TCK,
-    input wire jtag_pin_TMS,
-    input wire jtag_pin_TDI,
-    output wire jtag_pin_TDO,
-
-    output wire reg_we_o,
-    output wire[4:0] reg_addr_o,
-    output wire[31:0] reg_wdata_o,
-    input wire[31:0] reg_rdata_i,
-
-    output wire mem_we_o,
-    output wire[31:0] mem_addr_o,
-    output wire[31:0] mem_wdata_o,
-    input wire[31:0] mem_rdata_i,
-    output wire[3:0] mem_sel_o,
-
-    output wire req_valid_o,
-    input wire req_ready_i,
-    input wire rsp_valid_i,
-    output wire rsp_ready_o,
-
-    output wire halt_req_o,
-    output wire reset_req_o
-
-    );
-
-    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;
-
-    // jtag_driver杈撳嚭淇″彿
-    wire dtm_ack_o;
-    wire dtm_req_valid_o;
-    wire[DTM_REQ_BITS - 1:0] dtm_req_data_o;
-
-    // jtag_dm杈撳嚭淇″彿
-    wire dm_ack_o;
-    wire[DM_RESP_BITS-1:0] dm_resp_data_o;
-    wire dm_resp_valid_o;
-    wire dm_halt_req_o;
-    wire dm_reset_req_o;
-
-    jtag_driver #(
-        .DMI_ADDR_BITS(DMI_ADDR_BITS),
-        .DMI_DATA_BITS(DMI_DATA_BITS),
-        .DMI_OP_BITS(DMI_OP_BITS)
-    ) u_jtag_driver(
-        .rst_n(jtag_rst_n),
-        .jtag_TCK(jtag_pin_TCK),
-        .jtag_TDI(jtag_pin_TDI),
-        .jtag_TMS(jtag_pin_TMS),
-        .jtag_TDO(jtag_pin_TDO),
-        .dm_resp_i(dm_resp_valid_o),
-        .dm_resp_data_i(dm_resp_data_o),
-        .dtm_ack_o(dtm_ack_o),
-        .dm_ack_i(dm_ack_o),
-        .dtm_req_valid_o(dtm_req_valid_o),
-        .dtm_req_data_o(dtm_req_data_o)
-    );
-
-    jtag_dm #(
-        .DMI_ADDR_BITS(DMI_ADDR_BITS),
-        .DMI_DATA_BITS(DMI_DATA_BITS),
-        .DMI_OP_BITS(DMI_OP_BITS)
-    ) u_jtag_dm(
-        .clk(clk),
-        .rst_n(jtag_rst_n),
-        .dm_ack_o(dm_ack_o),
-        .dtm_req_valid_i(dtm_req_valid_o),
-        .dtm_req_data_i(dtm_req_data_o),
-        .dtm_ack_i(dtm_ack_o),
-        .dm_resp_data_o(dm_resp_data_o),
-        .dm_resp_valid_o(dm_resp_valid_o),
-        .dm_reg_we_o(reg_we_o),
-        .dm_reg_addr_o(reg_addr_o),
-        .dm_reg_wdata_o(reg_wdata_o),
-        .dm_reg_rdata_i(reg_rdata_i),
-        .dm_mem_we_o(mem_we_o),
-        .dm_mem_addr_o(mem_addr_o),
-        .dm_mem_wdata_o(mem_wdata_o),
-        .dm_mem_rdata_i(mem_rdata_i),
-        .dm_mem_sel_o(mem_sel_o),
-        .req_valid_o(req_valid_o),
-        .req_ready_i(req_ready_i),
-        .rsp_valid_i(rsp_valid_i),
-        .rsp_ready_o(rsp_ready_o),
-        .dm_halt_req_o(halt_req_o),
-        .dm_reset_req_o(reset_req_o)
-    );
-
-endmodule
+ /*                                                                      
+ Copyright 2021 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_top #(
+
+    )(
+
+    input wire                      clk_i,
+    input wire                      rst_ni,
+
+    output wire                     debug_req_o,
+    output wire                     ndmreset_o,
+
+    input wire                      jtag_tck_i,     // JTAG test clock pad
+    input wire                      jtag_tdi_i,     // JTAG test data input pad
+    input wire                      jtag_tms_i,     // JTAG test mode select pad
+    input wire                      jtag_trst_ni,   // JTAG test reset pad
+    output wire                     jtag_tdo_o,     // JTAG test data output pad
+
+    output wire                     master_req_o,
+    input  wire                     master_gnt_i,
+    input  wire                     master_rvalid_i,
+    output wire                     master_we_o,
+    output wire [3:0]               master_be_o,
+    output wire [31:0]              master_addr_o,
+    output wire [31:0]              master_wdata_o,
+    input  wire [31:0]              master_rdata_i,
+    input  wire                     master_err_i,
+
+    input  wire                     slave_req_i,
+    input  wire                     slave_we_i,
+    input  wire [31:0]              slave_addr_i,
+    input  wire [3:0]               slave_be_i,
+    input  wire [31:0]              slave_wdata_i,
+    output wire [31:0]              slave_rdata_o
+
+    );
+
+    // addr + data + op = 6 + 32 + 2 = 40
+    localparam DMI_DATA_WIDTH = 40;
+
+
+    wire [DMI_DATA_WIDTH-1:0]   dm_to_dmi_data;
+    wire                        dm_to_dmi_valid;
+    wire                        dm_to_dmi_ready;
+    wire [DMI_DATA_WIDTH-1:0]   dmi_to_dm_data;
+    wire                        dmi_to_dm_valid;
+    wire                        dmi_to_dm_ready;
+
+    jtag_dm #(
+
+    ) u_jtag_dm (
+        .clk            (clk_i),
+        .rst_n          (rst_ni),
+        .dmi_data_i     (dmi_to_dm_data),
+        .dmi_valid_i    (dmi_to_dm_valid),
+        .dm_ready_o     (dm_to_dmi_ready),
+        .dm_data_o      (dm_to_dmi_data),
+        .dm_valid_o     (dm_to_dmi_valid),
+        .dmi_ready_i    (dmi_to_dm_ready),
+        .debug_req_o    (debug_req_o),
+        .ndmreset_o     (ndmreset_o),
+        .master_req_o   (master_req_o),
+        .master_gnt_i   (master_gnt_i),
+        .master_rvalid_i(master_rvalid_i),
+        .master_we_o    (master_we_o),
+        .master_be_o    (master_be_o),
+        .master_addr_o  (master_addr_o),
+        .master_wdata_o (master_wdata_o),
+        .master_rdata_i (master_rdata_i),
+        .master_err_i   (master_err_i),
+        .slave_req_i    (slave_req_i),
+        .slave_we_i     (slave_we_i),
+        .slave_addr_i   (slave_addr_i),
+        .slave_be_i     (slave_be_i),
+        .slave_wdata_i  (slave_wdata_i),
+        .slave_rdata_o  (slave_rdata_o)
+    );
+
+    wire [DMI_DATA_WIDTH-1:0]   dtm_to_dmi_data;
+    wire                        dtm_to_dmi_valid;
+    wire                        dtm_to_dmi_ready;
+    wire [DMI_DATA_WIDTH-1:0]   dmi_to_dtm_data;
+    wire                        dmi_to_dtm_valid;
+    wire                        dmi_to_dtm_ready;
+
+    jtag_dmi #(
+
+    ) u_jtag_dmi (
+        .jtag_tck_i     (jtag_tck_i),
+        .jtag_trst_ni   (jtag_trst_ni),
+        .jtag_data_i    (dtm_to_dmi_data),
+        .jtag_valid_i   (dtm_to_dmi_valid),
+        .jtag_ready_o   (dmi_to_dtm_ready),
+        .jtag_data_o    (dmi_to_dtm_data),
+        .jtag_valid_o   (dmi_to_dtm_valid),
+        .jtag_ready_i   (dtm_to_dmi_ready),
+        .clk_i          (clk_i),
+        .rst_ni         (rst_ni),
+        .core_data_i    (dm_to_dmi_data),
+        .core_valid_i   (dm_to_dmi_valid),
+        .core_ready_o   (dmi_to_dm_ready),
+        .core_data_o    (dmi_to_dm_data),
+        .core_valid_o   (dmi_to_dm_valid),
+        .core_ready_i   (dm_to_dmi_ready)
+    );
+
+    wire                        tap_to_dtm_req;
+    wire [DMI_DATA_WIDTH-1:0]   tap_to_dtm_data;
+    wire [DMI_DATA_WIDTH-1:0]   dtm_to_tap_data;
+    wire [31:0]                 idcode;
+    wire [31:0]                 dtmcs;
+
+    jtag_dtm #(
+
+    ) u_jtag_dtm (
+        .jtag_tck_i     (jtag_tck_i),
+        .jtag_trst_ni   (jtag_trst_ni),
+        .dtm_data_o     (dtm_to_dmi_data),
+        .dtm_valid_o    (dtm_to_dmi_valid),
+        .dmi_ready_i    (dmi_to_dtm_ready),
+        .dmi_data_i     (dmi_to_dtm_data),
+        .dmi_valid_i    (dmi_to_dtm_valid),
+        .dtm_ready_o    (dtm_to_dmi_ready),
+        .tap_req_i      (tap_to_dtm_req),
+        .tap_data_i     (tap_to_dtm_data),
+        .data_o         (dtm_to_tap_data),
+        .idcode_o       (idcode),
+        .dtmcs_o        (dtmcs)
+    );
+
+    jtag_tap #(
+
+    ) u_jtag_tap (
+        .jtag_tck_i     (jtag_tck_i),
+        .jtag_tdi_i     (jtag_tdi_i),
+        .jtag_tms_i     (jtag_tms_i),
+        .jtag_trst_ni   (jtag_trst_ni),
+        .jtag_tdo_o     (jtag_tdo_o),
+        .tap_req_o      (tap_to_dtm_req),
+        .tap_data_o     (tap_to_dtm_data),
+        .dtm_data_i     (dtm_to_tap_data),
+        .idcode_i       (idcode),
+        .dtmcs_i        (dtmcs)
+    );
+
+endmodule
diff --git a/rtl/sys_bus/rib.sv b/rtl/sys_bus/rib.sv
deleted file mode 100644
index 4763f02..0000000
--- a/rtl/sys_bus/rib.sv
+++ /dev/null
@@ -1,373 +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.                                          
- */
-
-
-// RIB鎬荤嚎妯″潡
-module rib #(
-    parameter MASTER_NUM = 3,
-    parameter SLAVE_NUM = 2)(
-
-    input wire clk,
-    input wire rst_n,
-
-    // master 0 interface
-    input wire[31:0] m0_addr_i,
-    input wire[31:0] m0_data_i,
-    input wire[3:0] m0_sel_i,
-    input wire m0_req_vld_i,
-    input wire m0_rsp_rdy_i,
-    input wire m0_we_i,
-    output wire m0_req_rdy_o,
-    output wire m0_rsp_vld_o,
-    output wire[31:0] m0_data_o,
-
-    // master 1 interface
-    input wire[31:0] m1_addr_i,
-    input wire[31:0] m1_data_i,
-    input wire[3:0] m1_sel_i,
-    input wire m1_req_vld_i,
-    input wire m1_rsp_rdy_i,
-    input wire m1_we_i,
-    output wire m1_req_rdy_o,
-    output wire m1_rsp_vld_o,
-    output wire[31:0] m1_data_o,
-
-    // master 2 interface
-    input wire[31:0] m2_addr_i,
-    input wire[31:0] m2_data_i,
-    input wire[3:0] m2_sel_i,
-    input wire m2_req_vld_i,
-    input wire m2_rsp_rdy_i,
-    input wire m2_we_i,
-    output wire m2_req_rdy_o,
-    output wire m2_rsp_vld_o,
-    output wire[31:0] m2_data_o,
-
-    // master 3 interface
-    input wire[31:0] m3_addr_i,
-    input wire[31:0] m3_data_i,
-    input wire[3:0] m3_sel_i,
-    input wire m3_req_vld_i,
-    input wire m3_rsp_rdy_i,
-    input wire m3_we_i,
-    output wire m3_req_rdy_o,
-    output wire m3_rsp_vld_o,
-    output wire[31:0] m3_data_o,
-
-    // slave 0 interface
-    input wire[31:0] s0_data_i,
-    input wire s0_req_rdy_i,
-    input wire s0_rsp_vld_i,
-    output wire[31:0] s0_addr_o,
-    output wire[31:0] s0_data_o,
-    output wire[3:0] s0_sel_o,
-    output wire s0_req_vld_o,
-    output wire s0_rsp_rdy_o,
-    output wire s0_we_o,
-
-    // slave 1 interface
-    input wire[31:0] s1_data_i,
-    input wire s1_req_rdy_i,
-    input wire s1_rsp_vld_i,
-    output wire[31:0] s1_addr_o,
-    output wire[31:0] s1_data_o,
-    output wire[3:0] s1_sel_o,
-    output wire s1_req_vld_o,
-    output wire s1_rsp_rdy_o,
-    output wire s1_we_o,
-
-    // slave 2 interface
-    input wire[31:0] s2_data_i,
-    input wire s2_req_rdy_i,
-    input wire s2_rsp_vld_i,
-    output wire[31:0] s2_addr_o,
-    output wire[31:0] s2_data_o,
-    output wire[3:0] s2_sel_o,
-    output wire s2_req_vld_o,
-    output wire s2_rsp_rdy_o,
-    output wire s2_we_o,
-
-    // slave 3 interface
-    input wire[31:0] s3_data_i,
-    input wire s3_req_rdy_i,
-    input wire s3_rsp_vld_i,
-    output wire[31:0] s3_addr_o,
-    output wire[31:0] s3_data_o,
-    output wire[3:0] s3_sel_o,
-    output wire s3_req_vld_o,
-    output wire s3_rsp_rdy_o,
-    output wire s3_we_o,
-
-    // slave 4 interface
-    input wire[31:0] s4_data_i,
-    input wire s4_req_rdy_i,
-    input wire s4_rsp_vld_i,
-    output wire[31:0] s4_addr_o,
-    output wire[31:0] s4_data_o,
-    output wire[3:0] s4_sel_o,
-    output wire s4_req_vld_o,
-    output wire s4_rsp_rdy_o,
-    output wire s4_we_o
-
-    );
-
-    /////////////////////////////// mux master //////////////////////////////
-
-    wire[MASTER_NUM-1:0] master_req;
-    wire[31:0] master_addr[MASTER_NUM-1:0];
-    wire[31:0] master_data[MASTER_NUM-1:0];
-    wire[3:0] master_sel[MASTER_NUM-1:0];
-    wire[MASTER_NUM-1:0] master_rsp_rdy;
-    wire[MASTER_NUM-1:0] master_we;
-
-    genvar i;
-    generate
-
-    if (MASTER_NUM == 2) begin: if_m_num_2
-        assign master_req = {m0_req_vld_i, m1_req_vld_i};
-        assign master_rsp_rdy = {m0_rsp_rdy_i, m1_rsp_rdy_i};
-        assign master_we = {m0_we_i, m1_we_i};
-        wire[32*MASTER_NUM-1:0] m_addr = {m0_addr_i, m1_addr_i};
-        wire[32*MASTER_NUM-1:0] m_data = {m0_data_i, m1_data_i};
-        wire[4*MASTER_NUM-1:0] m_sel = {m0_sel_i, m1_sel_i};
-        for (i = 0; i < MASTER_NUM; i = i + 1) begin: for_m_num_2
-            assign master_addr[i] = m_addr[(i+1)*32-1:32*i];
-            assign master_data[i] = m_data[(i+1)*32-1:32*i];
-            assign master_sel[i] = m_sel[(i+1)*4-1:4*i];
-        end
-    end
-
-    if (MASTER_NUM == 3) begin: if_m_num_3
-        assign master_req = {m0_req_vld_i, m1_req_vld_i, m2_req_vld_i};
-        assign master_rsp_rdy = {m0_rsp_rdy_i, m1_rsp_rdy_i, m2_rsp_rdy_i};
-        assign master_we = {m0_we_i, m1_we_i, m2_we_i};
-        wire[32*MASTER_NUM-1:0] m_addr = {m0_addr_i, m1_addr_i, m2_addr_i};
-        wire[32*MASTER_NUM-1:0] m_data = {m0_data_i, m1_data_i, m2_data_i};
-        wire[4*MASTER_NUM-1:0] m_sel = {m0_sel_i, m1_sel_i, m2_sel_i};
-        for (i = 0; i < MASTER_NUM; i = i + 1) begin: for_m_num_3
-            assign master_addr[i] = m_addr[(i+1)*32-1:32*i];
-            assign master_data[i] = m_data[(i+1)*32-1:32*i];
-            assign master_sel[i] = m_sel[(i+1)*4-1:4*i];
-        end
-    end
-
-    if (MASTER_NUM == 4) begin: if_m_num_4
-        assign master_req = {m0_req_vld_i, m1_req_vld_i, m2_req_vld_i, m3_req_vld_i};
-        assign master_rsp_rdy = {m0_rsp_rdy_i, m1_rsp_rdy_i, m2_rsp_rdy_i, m3_rsp_rdy_i};
-        assign master_we = {m0_we_i, m1_we_i, m2_we_i, m3_we_i};
-        wire[32*MASTER_NUM-1:0] m_addr = {m0_addr_i, m1_addr_i, m2_addr_i, m3_addr_i};
-        wire[32*MASTER_NUM-1:0] m_data = {m0_data_i, m1_data_i, m2_data_i, m3_data_i};
-        wire[4*MASTER_NUM-1:0] m_sel = {m0_sel_i, m1_sel_i, m2_sel_i, m3_sel_i};
-        for (i = 0; i < MASTER_NUM; i = i + 1) begin: for_m_num_4
-            assign master_addr[i] = m_addr[(i+1)*32-1:32*i];
-            assign master_data[i] = m_data[(i+1)*32-1:32*i];
-            assign master_sel[i] = m_sel[(i+1)*4-1:4*i];
-        end
-    end
-
-    wire[MASTER_NUM-1:0] master_req_vec;
-    wire[MASTER_NUM-1:0] master_sel_vec;
-
-    // 浼樺厛绾т徊瑁佹満鍒讹紝LSB浼樺厛绾ф渶楂橈紝MSB浼樺厛绾ф渶浣�
-    for (i = 0; i < MASTER_NUM; i = i + 1) begin: m_arb
-        if (i == 0) begin: m_is_0
-            assign master_req_vec[i] = 1'b1;
-        end else begin: m_is_not_0
-            assign master_req_vec[i] = ~(|master_req[i-1:0]);
-        end
-        assign master_sel_vec[i] = master_req_vec[i] & master_req[i];
-    end
-
-    reg[31:0] mux_m_addr;
-    reg[31:0] mux_m_data;
-    reg[3:0] mux_m_sel;
-    reg mux_m_req_vld;
-    reg mux_m_rsp_rdy;
-    reg mux_m_we;
-
-    integer j;
-
-    always @ (*) begin: m_out
-        mux_m_addr = 32'h0;
-        mux_m_data = 32'h0;
-        mux_m_sel = 4'h0;
-        mux_m_req_vld = 1'b0;
-        mux_m_rsp_rdy = 1'b0;
-        mux_m_we = 1'b0;
-        for (j = 0; j < MASTER_NUM; j = j + 1) begin: m_sig_out
-            mux_m_addr    = mux_m_addr    | ({32{master_sel_vec[j]}} & master_addr[j]);
-            mux_m_data    = mux_m_data    | ({32{master_sel_vec[j]}} & master_data[j]);
-            mux_m_sel     = mux_m_sel     | ({4 {master_sel_vec[j]}} & master_sel[j]);
-            mux_m_req_vld = mux_m_req_vld | ({1 {master_sel_vec[j]}} & master_req[j]);
-            mux_m_rsp_rdy = mux_m_rsp_rdy | ({1 {master_sel_vec[j]}} & master_rsp_rdy[j]);
-            mux_m_we      = mux_m_we      | ({1 {master_sel_vec[j]}} & master_we[j]);
-        end
-    end
-
-    /////////////////////////////// mux slave /////////////////////////////////
-
-    wire[SLAVE_NUM-1:0] slave_sel;
-
-    // 璁块棶鍦板潃鐨勬渶楂�4浣嶅喅瀹氳璁块棶鐨勬槸鍝竴涓粠璁惧
-    // 鍥犳鏈�澶氭敮鎸�16涓粠璁惧
-    for (i = 0; i < SLAVE_NUM; i = i + 1) begin: s_sel
-        assign slave_sel[i] = (mux_m_addr[31:28] == i);
-    end
-
-    wire[SLAVE_NUM-1:0] slave_req_rdy;
-    wire[SLAVE_NUM-1:0] slave_rsp_vld;
-    wire[31:0] slave_data[SLAVE_NUM-1:0];
-
-    if (SLAVE_NUM == 2) begin: if_s_num_2
-        assign slave_req_rdy = {s1_req_rdy_i, s0_req_rdy_i};
-        assign slave_rsp_vld = {s1_rsp_vld_i, s0_rsp_vld_i};
-        wire[32*SLAVE_NUM-1:0] s_data = {s1_data_i, s0_data_i};
-        for (i = 0; i < SLAVE_NUM; i = i + 1) begin: for_s_num_2
-            assign slave_data[i] = s_data[(i+1)*32-1:32*i];
-        end
-    end
-
-    if (SLAVE_NUM == 3) begin: if_s_num_3
-        assign slave_req_rdy = {s2_req_rdy_i, s1_req_rdy_i, s0_req_rdy_i};
-        assign slave_rsp_vld = {s2_rsp_vld_i, s1_rsp_vld_i, s0_rsp_vld_i};
-        wire[32*SLAVE_NUM-1:0] s_data = {s2_data_i, s1_data_i, s0_data_i};
-        for (i = 0; i < SLAVE_NUM; i = i + 1) begin: for_s_num_3
-            assign slave_data[i] = s_data[(i+1)*32-1:32*i];
-        end
-    end
-
-    if (SLAVE_NUM == 4) begin: if_s_num_4
-        assign slave_req_rdy = {s3_req_rdy_i, s2_req_rdy_i, s1_req_rdy_i, s0_req_rdy_i};
-        assign slave_rsp_vld = {s3_rsp_vld_i, s2_rsp_vld_i, s1_rsp_vld_i, s0_rsp_vld_i};
-        wire[32*SLAVE_NUM-1:0] s_data = {s3_data_i, s2_data_i, s1_data_i, s0_data_i};
-        for (i = 0; i < SLAVE_NUM; i = i + 1) begin: for_s_num_4
-            assign slave_data[i] = s_data[(i+1)*32-1:32*i];
-        end
-    end
-
-    if (SLAVE_NUM == 5) begin: if_s_num_5
-        assign slave_req_rdy = {s4_req_rdy_i, s3_req_rdy_i, s2_req_rdy_i, s1_req_rdy_i, s0_req_rdy_i};
-        assign slave_rsp_vld = {s4_rsp_vld_i, s3_rsp_vld_i, s2_rsp_vld_i, s1_rsp_vld_i, s0_rsp_vld_i};
-        wire[32*SLAVE_NUM-1:0] s_data = {s4_data_i, s3_data_i, s2_data_i, s1_data_i, s0_data_i};
-        for (i = 0; i < SLAVE_NUM; i = i + 1) begin: for_s_num_5
-            assign slave_data[i] = s_data[(i+1)*32-1:32*i];
-        end
-    end
-
-    reg[31:0] mux_s_data;
-    reg mux_s_req_rdy;
-    reg mux_s_rsp_vld;
-
-    always @ (*) begin: s_out
-        mux_s_data = 32'h0;
-        mux_s_req_rdy = 1'b0;
-        mux_s_rsp_vld = 1'b0;
-        for (j = 0; j < SLAVE_NUM; j = j + 1) begin: s_sig_out
-            mux_s_data    = mux_s_data    | ({32{slave_sel[j]}} & slave_data[j]);
-            mux_s_req_rdy = mux_s_req_rdy | ({1 {slave_sel[j]}} & slave_req_rdy[j]);
-            mux_s_rsp_vld = mux_s_rsp_vld | ({1 {slave_sel[j]}} & slave_rsp_vld[j]);
-        end
-    end
-
-    /////////////////////////////// demux master //////////////////////////////
-
-    wire[MASTER_NUM-1:0] demux_m_req_rdy;
-    wire[MASTER_NUM-1:0] demux_m_rsp_vld;
-    wire[32*MASTER_NUM-1:0] demux_m_data;
-
-    for (i = 0; i < MASTER_NUM; i = i + 1) begin: demux_m_sig
-        assign demux_m_req_rdy[i]            = {1 {master_sel_vec[i]}} & mux_s_req_rdy;
-        assign demux_m_rsp_vld[i]            = {1 {master_sel_vec[i]}} & mux_s_rsp_vld;
-        assign demux_m_data[(i+1)*32-1:32*i] = {32{master_sel_vec[i]}} & mux_s_data;
-    end
-
-    if (MASTER_NUM == 2) begin: demux_m_sig_2
-        assign {m0_req_rdy_o, m1_req_rdy_o} = demux_m_req_rdy;
-        assign {m0_rsp_vld_o, m1_rsp_vld_o} = demux_m_rsp_vld;
-        assign {m0_data_o, m1_data_o} = demux_m_data;
-    end
-
-    if (MASTER_NUM == 3) begin: demux_m_sig_3
-        assign {m0_req_rdy_o, m1_req_rdy_o, m2_req_rdy_o} = demux_m_req_rdy;
-        assign {m0_rsp_vld_o, m1_rsp_vld_o, m2_rsp_vld_o} = demux_m_rsp_vld;
-        assign {m0_data_o, m1_data_o, m2_data_o} = demux_m_data;
-    end
-
-    if (MASTER_NUM == 4) begin: demux_m_sig_4
-        assign {m0_req_rdy_o, m1_req_rdy_o, m2_req_rdy_o, m3_req_rdy_o} = demux_m_req_rdy;
-        assign {m0_rsp_vld_o, m1_rsp_vld_o, m2_rsp_vld_o, m3_rsp_vld_o} = demux_m_rsp_vld;
-        assign {m0_data_o, m1_data_o, m2_data_o, m3_data_o} = demux_m_data;
-    end
-
-    /////////////////////////////// demux slave //////////////////////////////
-
-    wire[32*SLAVE_NUM-1:0] demux_s_addr;
-    wire[32*SLAVE_NUM-1:0] demux_s_data;
-    wire[4*SLAVE_NUM-1:0] demux_s_sel;
-    wire[SLAVE_NUM-1:0] demux_s_req_vld;
-    wire[SLAVE_NUM-1:0] demux_s_rsp_rdy;
-    wire[SLAVE_NUM-1:0] demux_s_we;
-
-    for (i = 0; i < SLAVE_NUM; i = i + 1) begin: demux_s_sig
-        // 鍘绘帀澶栬鍩哄湴鍧�锛屽彧淇濈暀offset
-        assign demux_s_addr[(i+1)*32-1:32*i] = {32{slave_sel[i]}} & {4'h0, mux_m_addr[27:0]};
-        assign demux_s_data[(i+1)*32-1:32*i] = {32{slave_sel[i]}} & mux_m_data;
-        assign demux_s_sel[(i+1)*4-1:4*i]    = {4 {slave_sel[i]}} & mux_m_sel;
-        assign demux_s_req_vld[i]            = {1 {slave_sel[i]}} & mux_m_req_vld;
-        assign demux_s_rsp_rdy[i]            = {1 {slave_sel[i]}} & mux_m_rsp_rdy;
-        assign demux_s_we[i]                 = {1 {slave_sel[i]}} & mux_m_we;
-    end
-
-    if (SLAVE_NUM == 2) begin: demux_s_sig_2
-        assign {s1_addr_o, s0_addr_o} = demux_s_addr;
-        assign {s1_data_o, s0_data_o} = demux_s_data;
-        assign {s1_sel_o, s0_sel_o} = demux_s_sel;
-        assign {s1_req_vld_o, s0_req_vld_o} = demux_s_req_vld;
-        assign {s1_rsp_rdy_o, s0_rsp_rdy_o} = demux_s_rsp_rdy;
-        assign {s1_we_o, s0_we_o} = demux_s_we;
-    end
-
-    if (SLAVE_NUM == 3) begin: demux_s_sig_3
-        assign {s2_addr_o, s1_addr_o, s0_addr_o} = demux_s_addr;
-        assign {s2_data_o, s1_data_o, s0_data_o} = demux_s_data;
-        assign {s2_sel_o, s1_sel_o, s0_sel_o} = demux_s_sel;
-        assign {s2_req_vld_o, s1_req_vld_o, s0_req_vld_o} = demux_s_req_vld;
-        assign {s2_rsp_rdy_o, s1_rsp_rdy_o, s0_rsp_rdy_o} = demux_s_rsp_rdy;
-        assign {s2_we_o, s1_we_o, s0_we_o} = demux_s_we;
-    end
-
-    if (SLAVE_NUM == 4) begin: demux_s_sig_4
-        assign {s3_addr_o, s2_addr_o, s1_addr_o, s0_addr_o} = demux_s_addr;
-        assign {s3_data_o, s2_data_o, s1_data_o, s0_data_o} = demux_s_data;
-        assign {s3_sel_o, s2_sel_o, s1_sel_o, s0_sel_o} = demux_s_sel;
-        assign {s3_req_vld_o, s2_req_vld_o, s1_req_vld_o, s0_req_vld_o} = demux_s_req_vld;
-        assign {s3_rsp_rdy_o, s2_rsp_rdy_o, s1_rsp_rdy_o, s0_rsp_rdy_o} = demux_s_rsp_rdy;
-        assign {s3_we_o, s2_we_o, s1_we_o, s0_we_o} = demux_s_we;
-    end
-
-    if (SLAVE_NUM == 5) begin: demux_s_sig_5
-        assign {s4_addr_o, s3_addr_o, s2_addr_o, s1_addr_o, s0_addr_o} = demux_s_addr;
-        assign {s4_data_o, s3_data_o, s2_data_o, s1_data_o, s0_data_o} = demux_s_data;
-        assign {s4_sel_o, s3_sel_o, s2_sel_o, s1_sel_o, s0_sel_o} = demux_s_sel;
-        assign {s4_req_vld_o, s3_req_vld_o, s2_req_vld_o, s1_req_vld_o, s0_req_vld_o} = demux_s_req_vld;
-        assign {s4_rsp_rdy_o, s3_rsp_rdy_o, s2_rsp_rdy_o, s1_rsp_rdy_o, s0_rsp_rdy_o} = demux_s_rsp_rdy;
-        assign {s4_we_o, s3_we_o, s2_we_o, s1_we_o, s0_we_o} = demux_s_we;
-    end
-
-    endgenerate
-
-
-endmodule
diff --git a/rtl/top/tinyriscv_soc_top.sv b/rtl/top/tinyriscv_soc_top.sv
index 7c403c9..cfe11c3 100644
--- a/rtl/top/tinyriscv_soc_top.sv
+++ b/rtl/top/tinyriscv_soc_top.sv
@@ -15,6 +15,7 @@
  */
 
 `include "../core/defines.sv"
+`include "../debug/jtag_def.sv"
 
 // tinyriscv soc椤跺眰妯″潡
 module tinyriscv_soc_top(
@@ -38,14 +39,18 @@ module tinyriscv_soc_top(
 
 
 
-    localparam int MASTERS = 2; //Number of master ports
-    localparam int SLAVES  = 2; //Number of slave ports
+    localparam int MASTERS = 3; // Number of master ports
+    localparam int SLAVES  = 3; // Number of slave ports
 
-    localparam int CoreD = 0;
-    localparam int CoreI = 1;
+    // masters
+    localparam int CoreD    = 0;
+    localparam int JtagHost = 1;
+    localparam int CoreI    = 2;
 
-    localparam int Rom = 0;
-    localparam int Ram = 1;
+    // slaves
+    localparam int Rom          = 0;
+    localparam int Ram          = 1;
+    localparam int JtagDevice   = 2;
 
 
     wire           master_req       [MASTERS];
@@ -80,12 +85,12 @@ module tinyriscv_soc_top(
 
     wire ndmreset;
     wire ndmreset_n;
+    wire debug_req;
 
-    assign ndmreset = 1'b0;
 
     tinyriscv_core #(
-        .DEBUG_HALT_ADDR(`DEBUG_ADDR_BASE + 16'h800),
-        .DEBUG_EXCEPTION_ADDR(`DEBUG_ADDR_BASE + 16'h808)
+        .DEBUG_HALT_ADDR(`DEBUG_ADDR_BASE + `HaltAddress),
+        .DEBUG_EXCEPTION_ADDR(`DEBUG_ADDR_BASE + `ExceptionAddress)
     ) u_tinyriscv_core (
         .clk(clk),
         .rst_n(ndmreset_n),
@@ -111,9 +116,8 @@ module tinyriscv_soc_top(
         .irq_timer_i(1'b0),
         .irq_external_i(1'b0),
         .irq_fast_i(15'b0),
-        .irq_nm_i(1'b0),
 
-        .debug_req_i(1'b0)
+        .debug_req_i(debug_req)
     );
 
 
@@ -182,7 +186,37 @@ module tinyriscv_soc_top(
         .rst_no(ndmreset_n)
     );
 
+    assign slave_addr_mask[JtagDevice] = `DEBUG_ADDR_MASK;
+    assign slave_addr_base[JtagDevice] = `DEBUG_ADDR_BASE;
+    // JTAG module
+    jtag_top #(
 
+    ) u_jtag_top (
+        .clk_i              (clk),
+        .rst_ni             (rst_ext_ni),
+        .debug_req_o        (debug_req),
+        .ndmreset_o         (ndmreset),
+        .jtag_tck_i         (sim_jtag_tck),
+        .jtag_tdi_i         (sim_jtag_tdi),
+        .jtag_tms_i         (sim_jtag_tms),
+        .jtag_trst_ni       (sim_jtag_trstn),
+        .jtag_tdo_o         (sim_jtag_tdo),
+        .master_req_o       (master_req[JtagHost]),
+        .master_gnt_i       (master_gnt[JtagHost]),
+        .master_rvalid_i    (master_rvalid[JtagHost]),
+        .master_we_o        (master_we[JtagHost]),
+        .master_be_o        (master_be[JtagHost]),
+        .master_addr_o      (master_addr[JtagHost]),
+        .master_wdata_o     (master_wdata[JtagHost]),
+        .master_rdata_i     (master_rdata[JtagHost]),
+        .master_err_i       (1'b0),
+        .slave_req_i        (slave_req[JtagDevice]),
+        .slave_we_i         (slave_we[JtagDevice]),
+        .slave_addr_i       (slave_addr[JtagDevice]),
+        .slave_be_i         (slave_be[JtagDevice]),
+        .slave_wdata_i      (slave_wdata[JtagDevice]),
+        .slave_rdata_o      (slave_rdata[JtagDevice])
+    );
 
 `ifdef VERILATOR
     sim_jtag #(
@@ -191,7 +225,7 @@ module tinyriscv_soc_top(
     ) u_sim_jtag (
         .clock                ( clk                  ),
         .reset                ( ~rst_ext_ni          ),
-        .enable               ( 1'b0                 ),
+        .enable               ( 1'b1                 ),
         .init_done            ( rst_ext_ni           ),
         .jtag_TCK             ( sim_jtag_tck         ),
         .jtag_TMS             ( sim_jtag_tms         ),
diff --git a/rtl/utils/cdc_2phase.sv b/rtl/utils/cdc_2phase.sv
new file mode 100644
index 0000000..eb75fa0
--- /dev/null
+++ b/rtl/utils/cdc_2phase.sv
@@ -0,0 +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 */
diff --git a/rtl/utils/full_handshake_rx.sv b/rtl/utils/full_handshake_rx.sv
deleted file mode 100644
index 67f86ec..0000000
--- a/rtl/utils/full_handshake_rx.sv
+++ /dev/null
@@ -1,128 +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.                                          
- */
-
-// 鏁版嵁鎺ユ敹绔ā鍧�
-// 璺ㄦ椂閽熷煙浼犺緭锛屽叏(鍥涙)鎻℃墜鍗忚
-// req = 1
-// ack_o = 1
-// req = 0
-// ack_o = 0
-module full_handshake_rx #(
-    parameter DW = 32)(             // RX瑕佹帴鏀舵暟鎹殑浣嶅
-
-    input wire clk,                 // RX绔椂閽熶俊鍙�
-    input wire rst_n,               // RX绔浣嶄俊鍙�
-
-    // from tx
-    input wire req_i,               // TX绔姹備俊鍙�
-    input wire[DW-1:0] req_data_i,  // TX绔緭鍏ユ暟鎹�
-
-    // to tx
-    output wire ack_o,              // RX绔簲绛擳X绔俊鍙�
-
-    // to rx
-    output wire[DW-1:0] recv_data_o,// RX绔帴鏀跺埌鐨勬暟鎹�
-    output wire recv_rdy_o          // RX绔槸鍚︽帴鏀跺埌鏁版嵁淇″彿
-
-    );
-
-    localparam STATE_IDLE     = 2'b01;
-    localparam STATE_DEASSERT = 2'b10;
-
-    reg[1:0] state;
-    reg[1:0] state_next;
-
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            state <= STATE_IDLE;
-        end else begin
-            state <= state_next;
-        end
-    end
-
-    always @ (*) begin
-        case (state)
-            // 绛夊緟TX璇锋眰淇″彿req=1
-            STATE_IDLE: begin
-                if (req == 1'b1) begin
-                    state_next = STATE_DEASSERT;
-                end else begin
-                    state_next = STATE_IDLE;
-                end
-            end
-            // 绛夊緟req=0
-            STATE_DEASSERT: begin
-                if (req) begin
-                    state_next = STATE_DEASSERT;
-                end else begin
-                    state_next = STATE_IDLE;
-                end
-            end
-            default: begin
-                state_next = STATE_IDLE;
-            end
-        endcase
-    end
-
-    reg req_d;
-    reg req;
-
-    // 灏嗚姹備俊鍙锋墦涓ゆ媿杩涜鍚屾
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            req_d <= 1'b0;
-            req <= 1'b0;
-        end else begin
-            req_d <= req_i;
-            req <= req_d;
-        end
-    end
-
-    reg[DW-1:0] recv_data;
-    reg recv_rdy;
-    reg ack;
-
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            ack <= 1'b0;
-            recv_rdy <= 1'b0;
-            recv_data <= {(DW){1'b0}};
-        end else begin
-            case (state)
-                STATE_IDLE: begin
-                    if (req == 1'b1) begin
-                        ack <= 1'b1;
-                        recv_rdy <= 1'b1;           // 杩欎釜淇″彿鍙細鎸佺画涓�涓椂閽�
-                        recv_data <= req_data_i;    // 杩欎釜淇″彿鍙細鎸佺画涓�涓椂閽�
-                    end
-                end
-                STATE_DEASSERT: begin
-                    recv_rdy <= 1'b0;
-                    recv_data <= {(DW){1'b0}};
-                    // req鎾ら攢鍚巃ck涔熸挙閿�
-                    if (req == 1'b0) begin
-                        ack <= 1'b0;
-                    end
-                end
-            endcase
-        end
-    end
-
-    assign ack_o = ack;
-    assign recv_rdy_o = recv_rdy;
-    assign recv_data_o = recv_data;
-
-endmodule
diff --git a/rtl/utils/full_handshake_tx.sv b/rtl/utils/full_handshake_tx.sv
deleted file mode 100644
index b4a2c14..0000000
--- a/rtl/utils/full_handshake_tx.sv
+++ /dev/null
@@ -1,147 +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.                                          
- */
-
-// 鏁版嵁鍙戦�佺妯″潡
-// 璺ㄦ椂閽熷煙浼犺緭锛屽叏(鍥涙)鎻℃墜鍗忚
-// req_o = 1
-// ack = 1
-// req_o = 0
-// ack = 0
-module full_handshake_tx #(
-    parameter DW = 32)(             // TX瑕佸彂閫佹暟鎹殑浣嶅
-
-    input wire clk,                 // TX绔椂閽熶俊鍙�
-    input wire rst_n,               // TX绔浣嶄俊鍙�
-
-    // from rx
-    input wire ack_i,               // RX绔簲绛斾俊鍙�
-
-    // from tx
-    input wire req_i,               // TX绔姹備俊鍙凤紝鍙渶鎸佺画涓�涓椂閽�
-    input wire[DW-1:0] req_data_i,  // TX绔鍙戦�佺殑鏁版嵁锛屽彧闇�鎸佺画涓�涓椂閽�
-
-    // to tx
-    output wire idle_o,             // TX绔槸鍚︾┖闂蹭俊鍙凤紝绌洪棽鎵嶈兘鍙戞暟鎹�
-
-    // to rx
-    output wire req_o,              // TX绔姹備俊鍙�
-    output wire[DW-1:0] req_data_o  // TX绔鍙戦�佺殑鏁版嵁
-
-    );
-
-    localparam STATE_IDLE     = 3'b001;
-    localparam STATE_ASSERT   = 3'b010;
-    localparam STATE_DEASSERT = 3'b100;
-
-    reg[2:0] state;
-    reg[2:0] state_next;
-
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            state <= STATE_IDLE;
-        end else begin
-            state <= state_next;
-        end
-    end
-
-    always @ (*) begin
-        case (state)
-            STATE_IDLE: begin
-                if (req_i == 1'b1) begin
-                    state_next = STATE_ASSERT;
-                end else begin
-                    state_next = STATE_IDLE;
-                end
-            end
-            // 绛夊緟ack=1
-            STATE_ASSERT: begin
-                if (!ack) begin
-                    state_next = STATE_ASSERT;
-                end else begin
-                    state_next = STATE_DEASSERT;
-                end
-            end
-            // 绛夊緟ack=0
-            STATE_DEASSERT: begin
-                if (!ack) begin
-                    state_next = STATE_IDLE;
-                end else begin
-                    state_next = STATE_DEASSERT;
-                end
-            end
-            default: begin
-                state_next = STATE_IDLE;
-            end
-        endcase
-    end
-
-    reg ack_d;
-    reg ack;
-
-    // 灏嗗簲绛斾俊鍙锋墦涓ゆ媿杩涜鍚屾
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            ack_d <= 1'b0;
-            ack <= 1'b0;
-        end else begin
-            ack_d <= ack_i;
-            ack <= ack_d;
-        end
-    end
-
-    reg req;
-    reg[DW-1:0] req_data;
-    reg idle;
-
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            idle <= 1'b1;
-            req <= 1'b0;
-            req_data <= {(DW){1'b0}};
-        end else begin
-            case (state)
-                // 閿佸瓨TX璇锋眰鏁版嵁锛屽湪鏀跺埌ack涔嬪墠涓�鐩翠繚鎸佹湁鏁�
-                STATE_IDLE: begin
-                    if (req_i == 1'b1) begin
-                        idle <= 1'b0;
-                        req <= req_i;
-                        req_data <= req_data_i;
-                    end else begin
-                        idle <= 1'b1;
-                        req <= 1'b0;
-                    end
-                end
-                // 鏀跺埌RX鐨刟ck涔嬪悗鎾ら攢TX璇锋眰
-                STATE_ASSERT: begin
-                    if (ack == 1'b1) begin
-                        req <= 1'b0;
-                        req_data <= {(DW){1'b0}};
-                    end
-                end
-                STATE_DEASSERT: begin
-                    if (!ack) begin
-                        idle <= 1'b1;
-                    end
-                end
-            endcase
-        end
-    end
-
-    assign idle_o = idle;
-    assign req_o = req;
-    assign req_data_o = req_data;
-
-endmodule
diff --git a/rtl/utils/gen_buf.sv b/rtl/utils/gen_buf.sv
index feacd43..67a8a3b 100644
--- a/rtl/utils/gen_buf.sv
+++ b/rtl/utils/gen_buf.sv
@@ -1,46 +1,46 @@
- /*                                                                      
- 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.                                          
- */
-
-// 灏嗚緭鍏ユ墦DP鎷嶅悗杈撳嚭
-module gen_ticks_sync #(
-    parameter DP = 2,
-    parameter DW = 32)(
-
-    input wire rst_n,
-    input wire clk,
-
-    input wire[DW-1:0] din,
-    output wire[DW-1:0] dout
-
-    );
-
-    wire[DW-1:0] sync_dat[DP-1:0];
-
-    genvar i;
-
-    generate 
-        for (i = 0; i < DP; i = i + 1) begin: ticks_sync
-            if (i == 0) begin: dp_is_0
-                gen_rst_0_dff #(DW) rst_0_dff(clk, rst_n, din, sync_dat[0]);
-            end else begin: dp_is_not_0
-                gen_rst_0_dff #(DW) rst_0_dff(clk, rst_n, sync_dat[i-1], sync_dat[i]);
-            end
-        end
-    endgenerate
-
-    assign dout = sync_dat[DP-1];
-  
-endmodule
+ /*                                                                      
+ 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.                                          
+ */
+
+// 灏嗚緭鍏ユ墦DP鎷嶅悗杈撳嚭
+module gen_ticks_sync #(
+    parameter DP = 2,
+    parameter DW = 32)(
+
+    input wire rst_n,
+    input wire clk,
+
+    input wire[DW-1:0] din,
+    output wire[DW-1:0] dout
+
+    );
+
+    wire[DW-1:0] sync_dat[DP-1:0];
+
+    genvar i;
+
+    generate 
+        for (i = 0; i < DP; i = i + 1) begin: ticks_sync
+            if (i == 0) begin: dp_is_0
+                gen_rst_0_dff #(DW) rst_0_dff(clk, rst_n, din, sync_dat[0]);
+            end else begin: dp_is_not_0
+                gen_rst_0_dff #(DW) rst_0_dff(clk, rst_n, sync_dat[i-1], sync_dat[i]);
+            end
+        end
+    endgenerate
+
+    assign dout = sync_dat[DP-1];
+  
+endmodule
diff --git a/rtl/utils/gen_dff.sv b/rtl/utils/gen_dff.sv
index b5d5ba7..d3e2ee8 100644
--- a/rtl/utils/gen_dff.sv
+++ b/rtl/utils/gen_dff.sv
@@ -1,173 +1,173 @@
- /*                                                                      
- 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 gen_pipe_dff #(
-    parameter DW = 32)(
-
-    input wire clk,
-    input wire rst_n,
-    input wire hold_en,
-
-    input wire[DW-1:0] def_val,
-    input wire[DW-1:0] din,
-    output wire[DW-1:0] qout
-
-    );
-
-    reg[DW-1:0] qout_r;
-
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n | hold_en) begin
-            qout_r <= def_val;
-        end else begin
-            qout_r <= din;
-        end
-    end
-
-    assign qout = qout_r;
-
-endmodule
-
-// 澶嶄綅鍚庤緭鍑轰负0鐨勮Е鍙戝櫒
-module gen_rst_0_dff #(
-    parameter DW = 32)(
-
-    input wire clk,
-    input wire rst_n,
-
-    input wire[DW-1:0] din,
-    output wire[DW-1:0] qout
-
-    );
-
-    reg[DW-1:0] qout_r;
-
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            qout_r <= {DW{1'b0}};
-        end else begin                  
-            qout_r <= din;
-        end
-    end
-
-    assign qout = qout_r;
-
-endmodule
-
-// 澶嶄綅鍚庤緭鍑轰负1鐨勮Е鍙戝櫒
-module gen_rst_1_dff #(
-    parameter DW = 32)(
-
-    input wire clk,
-    input wire rst_n,
-
-    input wire[DW-1:0] din,
-    output wire[DW-1:0] qout
-
-    );
-
-    reg[DW-1:0] qout_r;
-
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            qout_r <= {DW{1'b1}};
-        end else begin                  
-            qout_r <= din;
-        end
-    end
-
-    assign qout = qout_r;
-
-endmodule
-
-// 澶嶄綅鍚庤緭鍑轰负榛樿鍊肩殑瑙﹀彂鍣�
-module gen_rst_def_dff #(
-    parameter DW = 32)(
-
-    input wire clk,
-    input wire rst_n,
-    input wire[DW-1:0] def_val,
-
-    input wire[DW-1:0] din,
-    output wire[DW-1:0] qout
-
-    );
-
-    reg[DW-1:0] qout_r;
-
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            qout_r <= def_val;
-        end else begin                  
-            qout_r <= din;
-        end
-    end
-
-    assign qout = qout_r;
-
-endmodule
-
-// 甯︿娇鑳界銆佸浣嶅悗杈撳嚭涓�0鐨勮Е鍙戝櫒
-module gen_en_dff #(
-    parameter DW = 32)(
-
-    input wire clk,
-    input wire rst_n,
-
-    input wire en,
-    input wire[DW-1:0] din,
-    output wire[DW-1:0] qout
-
-    );
-
-    reg[DW-1:0] qout_r;
-
-    always @ (posedge clk or negedge rst_n) begin
-        if (!rst_n) begin
-            qout_r <= {DW{1'b0}};
-        end else if (en == 1'b1) begin
-            qout_r <= din;
-        end
-    end
-
-    assign qout = qout_r;
-
-endmodule
-
-// 甯︿娇鑳界銆佹病鏈夊浣嶇殑瑙﹀彂鍣�
-module gen_en_dffnr #(
-    parameter DW = 32)(
-
-    input wire clk,
-
-    input wire en,
-    input wire[DW-1:0] din,
-    output wire[DW-1:0] qout
-
-    );
-
-    reg[DW-1:0] qout_r;
-
-    always @ (posedge clk) begin
-        if (en == 1'b1) begin
-            qout_r <= din;
-        end
-    end
-
-    assign qout = qout_r;
-
-endmodule
+ /*                                                                      
+ 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 gen_pipe_dff #(
+    parameter DW = 32)(
+
+    input wire clk,
+    input wire rst_n,
+    input wire hold_en,
+
+    input wire[DW-1:0] def_val,
+    input wire[DW-1:0] din,
+    output wire[DW-1:0] qout
+
+    );
+
+    reg[DW-1:0] qout_r;
+
+    always @ (posedge clk or negedge rst_n) begin
+        if (!rst_n | hold_en) begin
+            qout_r <= def_val;
+        end else begin
+            qout_r <= din;
+        end
+    end
+
+    assign qout = qout_r;
+
+endmodule
+
+// 澶嶄綅鍚庤緭鍑轰负0鐨勮Е鍙戝櫒
+module gen_rst_0_dff #(
+    parameter DW = 32)(
+
+    input wire clk,
+    input wire rst_n,
+
+    input wire[DW-1:0] din,
+    output wire[DW-1:0] qout
+
+    );
+
+    reg[DW-1:0] qout_r;
+
+    always @ (posedge clk or negedge rst_n) begin
+        if (!rst_n) begin
+            qout_r <= {DW{1'b0}};
+        end else begin                  
+            qout_r <= din;
+        end
+    end
+
+    assign qout = qout_r;
+
+endmodule
+
+// 澶嶄綅鍚庤緭鍑轰负1鐨勮Е鍙戝櫒
+module gen_rst_1_dff #(
+    parameter DW = 32)(
+
+    input wire clk,
+    input wire rst_n,
+
+    input wire[DW-1:0] din,
+    output wire[DW-1:0] qout
+
+    );
+
+    reg[DW-1:0] qout_r;
+
+    always @ (posedge clk or negedge rst_n) begin
+        if (!rst_n) begin
+            qout_r <= {DW{1'b1}};
+        end else begin                  
+            qout_r <= din;
+        end
+    end
+
+    assign qout = qout_r;
+
+endmodule
+
+// 澶嶄綅鍚庤緭鍑轰负榛樿鍊肩殑瑙﹀彂鍣�
+module gen_rst_def_dff #(
+    parameter DW = 32)(
+
+    input wire clk,
+    input wire rst_n,
+    input wire[DW-1:0] def_val,
+
+    input wire[DW-1:0] din,
+    output wire[DW-1:0] qout
+
+    );
+
+    reg[DW-1:0] qout_r;
+
+    always @ (posedge clk or negedge rst_n) begin
+        if (!rst_n) begin
+            qout_r <= def_val;
+        end else begin                  
+            qout_r <= din;
+        end
+    end
+
+    assign qout = qout_r;
+
+endmodule
+
+// 甯︿娇鑳界銆佸浣嶅悗杈撳嚭涓�0鐨勮Е鍙戝櫒
+module gen_en_dff #(
+    parameter DW = 32)(
+
+    input wire clk,
+    input wire rst_n,
+
+    input wire en,
+    input wire[DW-1:0] din,
+    output wire[DW-1:0] qout
+
+    );
+
+    reg[DW-1:0] qout_r;
+
+    always @ (posedge clk or negedge rst_n) begin
+        if (!rst_n) begin
+            qout_r <= {DW{1'b0}};
+        end else if (en == 1'b1) begin
+            qout_r <= din;
+        end
+    end
+
+    assign qout = qout_r;
+
+endmodule
+
+// 甯︿娇鑳界銆佹病鏈夊浣嶇殑瑙﹀彂鍣�
+module gen_en_dffnr #(
+    parameter DW = 32)(
+
+    input wire clk,
+
+    input wire en,
+    input wire[DW-1:0] din,
+    output wire[DW-1:0] qout
+
+    );
+
+    reg[DW-1:0] qout_r;
+
+    always @ (posedge clk) begin
+        if (en == 1'b1) begin
+            qout_r <= din;
+        end
+    end
+
+    assign qout = qout_r;
+
+endmodule
diff --git a/rtl/utils/gen_ram.sv b/rtl/utils/gen_ram.sv
index 2879e39..a487a83 100644
--- a/rtl/utils/gen_ram.sv
+++ b/rtl/utils/gen_ram.sv
@@ -1,72 +1,72 @@
- /*                                                                      
- 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.                                          
- */
-
-`include "../core/defines.sv"
-
-
-module gen_ram #(
-    parameter DP = 512,
-    parameter DW = 32,
-    parameter MW = 4,
-    parameter AW = 32)(
-
-    input wire clk,
-    input wire[AW-1:0] addr_i,
-    input wire[DW-1:0] data_i,
-    input wire[MW-1:0] sel_i,
-    input wire we_i,
-
-	output wire[DW-1:0] data_o
-
-    );
-
-    reg[DW-1:0] ram [0:DP-1];
-    reg[AW-1:0] addr_r;
-    wire[MW-1:0] wen;
-    wire ren;
-
-    assign ren = (~we_i);
-    assign wen = ({MW{we_i}} & sel_i);
-
-    always @ (posedge clk) begin
-        if (ren) begin
-            addr_r <= addr_i;
-        end
-    end
-
-    assign data_o = ram[addr_r];
-
-    genvar i;
-
-    generate
-        for (i = 0; i < MW; i = i + 1) begin: sel_width
-            if ((8 * i + 8) > DW) begin: i_gt_8
-                always @ (posedge clk) begin: i_gt_8_ff
-                    if (wen[i]) begin: gt_8_wen
-                        ram[addr_i][DW-1:8*i] <= data_i[DW-1:8*i];
-                    end
-                end
-            end else begin: i_lt_8
-                always @ (posedge clk) begin: i_lt_8_ff
-                    if (wen[i]) begin: lt_8_wen
-                        ram[addr_i][8*i+7:8*i] <= data_i[8*i+7:8*i];
-                    end
-                end
-            end
-        end
-    endgenerate
-
-endmodule
+ /*                                                                      
+ 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.                                          
+ */
+
+`include "../core/defines.sv"
+
+
+module gen_ram #(
+    parameter DP = 512,
+    parameter DW = 32,
+    parameter MW = 4,
+    parameter AW = 32)(
+
+    input wire clk,
+    input wire[AW-1:0] addr_i,
+    input wire[DW-1:0] data_i,
+    input wire[MW-1:0] sel_i,
+    input wire we_i,
+
+	output wire[DW-1:0] data_o
+
+    );
+
+    reg[DW-1:0] ram [0:DP-1];
+    reg[AW-1:0] addr_r;
+    wire[MW-1:0] wen;
+    wire ren;
+
+    assign ren = (~we_i);
+    assign wen = ({MW{we_i}} & sel_i);
+
+    always @ (posedge clk) begin
+        if (ren) begin
+            addr_r <= addr_i;
+        end
+    end
+
+    assign data_o = ram[addr_r];
+
+    genvar i;
+
+    generate
+        for (i = 0; i < MW; i = i + 1) begin: sel_width
+            if ((8 * i + 8) > DW) begin: i_gt_8
+                always @ (posedge clk) begin: i_gt_8_ff
+                    if (wen[i]) begin: gt_8_wen
+                        ram[addr_i][DW-1:8*i] <= data_i[DW-1:8*i];
+                    end
+                end
+            end else begin: i_lt_8
+                always @ (posedge clk) begin: i_lt_8_ff
+                    if (wen[i]) begin: lt_8_wen
+                        ram[addr_i][8*i+7:8*i] <= data_i[8*i+7:8*i];
+                    end
+                end
+            end
+        end
+    endgenerate
+
+endmodule
diff --git a/sim/jtag_compliance_test.cfg b/sim/jtag_compliance_test.cfg
new file mode 100644
index 0000000..9de9b41
--- /dev/null
+++ b/sim/jtag_compliance_test.cfg
@@ -0,0 +1,30 @@
+debug_level 2
+adapter_khz     10000
+
+interface remote_bitbang
+remote_bitbang_host localhost
+
+remote_bitbang_port 9999
+
+set _CHIPNAME riscv
+jtag newtap $_CHIPNAME cpu -irlen 5 -expected-id 0x1e200a6f
+
+foreach t [jtag names] {
+    puts [format "TAP: %s\n" $t]
+}
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME riscv -chain-position $_TARGETNAME
+
+riscv set_reset_timeout_sec 2000
+riscv set_command_timeout_sec 2000
+
+# prefer to use sba for system bus access
+riscv set_prefer_sba on
+
+# dump jtag chain
+scan_chain
+
+init
+riscv test_compliance
+shutdown
diff --git a/sim/jtag_debug.cfg b/sim/jtag_debug.cfg
new file mode 100644
index 0000000..2ae4020
--- /dev/null
+++ b/sim/jtag_debug.cfg
@@ -0,0 +1,31 @@
+debug_level 2
+adapter_khz     10000
+
+interface remote_bitbang
+remote_bitbang_host localhost
+
+remote_bitbang_port 9999
+
+set _CHIPNAME riscv
+jtag newtap $_CHIPNAME cpu -irlen 5 -expected-id 0x1e200a6f
+
+foreach t [jtag names] {
+    puts [format "TAP: %s\n" $t]
+}
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME riscv -chain-position $_TARGETNAME
+
+riscv set_reset_timeout_sec 2000
+riscv set_command_timeout_sec 2000
+
+# prefer to use sba for system bus access
+riscv set_prefer_sba on
+
+# dump jtag chain
+scan_chain
+
+init
+
+halt
+echo "Ready for Remote Connections"
diff --git a/sim/tb_top_verilator.sv b/sim/tb_top_verilator.sv
index 01f9d11..58a3683 100644
--- a/sim/tb_top_verilator.sv
+++ b/sim/tb_top_verilator.sv
@@ -34,7 +34,7 @@ module tb_top_verilator #(
             $display("No firmware specified");
         end
     end
-
+/*
     always @(posedge clk_i or negedge rst_ni) begin
         if (!rst_ni) begin
 
@@ -65,7 +65,7 @@ module tb_top_verilator #(
             end
         end
     end
-
+*/
     tinyriscv_soc_top u_tinyriscv_soc_top(
         .clk(clk_i),
         .rst_ext_ni(rst_ni)