jtag/vdebug: add support for DAP6

This change implements the support for the ARM Debug Interface v6.
The DAP-level interface properly selects the DP Banks and AP address.
Sample ARM configuration DAP and JTAG scripts have been updated.

Change-Id: I7df87ef764bca587697c778810443649a7f46c2b
Signed-off-by: Jacek Wuwer <jacekmw8@gmail.com>
Reviewed-on: https://review.openocd.org/c/openocd/+/8067
Tested-by: jenkins
Reviewed-by: Ian Thompson <ianst@cadence.com>
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

10 files changed, 200 insertions, 91 deletions

diff --git a/src/jtag/drivers/vdebug.c b/src/jtag/drivers/vdebug.c
index d51d248..6d9016e 100644
--- a/src/jtag/drivers/vdebug.c
+++ b/src/jtag/drivers/vdebug.c
@@ -53,7 +53,7 @@
 #include "helper/log.h"
 #include "helper/list.h"
 
-#define VD_VERSION 47
+#define VD_VERSION 48
 #define VD_BUFFER_LEN 4024
 #define VD_CHEADER_LEN 24
 #define VD_SHEADER_LEN 16
@@ -66,7 +66,8 @@
  * @brief List of transactor types
  */
 enum {
-	VD_BFM_JTDP   = 0x0001,  /* transactor DAP JTAG DP */
+	VD_BFM_TPIU   = 0x0000,  /* transactor trace TPIU */
+	VD_BFM_DAP6   = 0x0001,  /* transactor DAP ADI V6 */
 	VD_BFM_SWDP   = 0x0002,  /* transactor DAP SWD DP */
 	VD_BFM_AHB    = 0x0003,  /* transactor AMBA AHB */
 	VD_BFM_APB    = 0x0004,  /* transactor AMBA APB */
@@ -467,14 +468,14 @@ static int vdebug_run_reg_queue(int hsock, struct vd_shm *pm, unsigned int count
 				for (unsigned int j = 0; j < num; j++)
 					memcpy(&data[j * awidth], &pm->rd8[(rwords + j) * awidth], awidth);
 			}
-			LOG_DEBUG_IO("read  %04x AS:%02x RG:%02x O:%05x @%03x D:%08x", le_to_h_u16(pm->wid) - count + req,
-				aspace, addr, (vdc.trans_first << 14) | (vdc.trans_last << 15), waddr,
+			LOG_DEBUG("read  %04x AS:%1x RG:%1x O:%05x @%03x D:%08x", le_to_h_u16(pm->wid) - count + req,
+				aspace, addr << 2, (vdc.trans_first << 14) | (vdc.trans_last << 15), waddr,
 				(num ? le_to_h_u32(&pm->rd8[rwords * 4]) : 0xdead));
 			rwords += num * wwidth;
 			waddr += sizeof(uint64_t) / 4; /* waddr past header */
 		} else {
-			LOG_DEBUG_IO("write %04x AS:%02x RG:%02x O:%05x @%03x D:%08x", le_to_h_u16(pm->wid) - count + req,
-				aspace, addr, (vdc.trans_first << 14) | (vdc.trans_last << 15), waddr,
+			LOG_DEBUG("write %04x AS:%1x RG:%1x O:%05x @%03x D:%08x", le_to_h_u16(pm->wid) - count + req,
+				aspace, addr << 2, (vdc.trans_first << 14) | (vdc.trans_last << 15), waddr,
 				le_to_h_u32(&pm->wd8[(waddr + num + 1) * 4]));
 			waddr += sizeof(uint64_t) / 4 + (num * wwidth * awidth + 3) / 4;
 		}
@@ -518,7 +519,7 @@ static int vdebug_open(int hsock, struct vd_shm *pm, const char *path,
 		rc = VD_ERR_VERSION;
 	} else {
 		pm->cmd = VD_CMD_CONNECT;
-		pm->type = type;               /* BFM type to connect to, here JTAG */
+		pm->type = type;               /* BFM type to connect to */
 		h_u32_to_le(pm->rwdata, sig_mask | VD_SIG_BUF | (VD_SIG_BUF << 16));
 		h_u16_to_le(pm->wbytes, strlen(path) + 1);
 		h_u16_to_le(pm->rbytes, 12);
@@ -922,7 +923,7 @@ static int vdebug_reset(int trst, int srst)
 
 static int vdebug_jtag_tms_seq(const uint8_t *tms, int num, uint8_t f_flush)
 {
-	LOG_INFO("tms  len:%d tms:%x", num, *tms);
+	LOG_DEBUG_IO("tms  len:%d tms:%x", num, *tms);
 
 	return vdebug_jtag_shift_tap(vdc.hsocket, pbuf, num, *tms, 0, NULL, 0, 0, NULL, f_flush);
 }
@@ -930,7 +931,7 @@ static int vdebug_jtag_tms_seq(const uint8_t *tms, int num, uint8_t f_flush)
 static int vdebug_jtag_path_move(struct pathmove_command *cmd, uint8_t f_flush)
 {
 	uint8_t tms[DIV_ROUND_UP(cmd->num_states, 8)];
-	LOG_INFO("path num states %d", cmd->num_states);
+	LOG_DEBUG_IO("path num states %d", cmd->num_states);
 
 	memset(tms, 0, DIV_ROUND_UP(cmd->num_states, 8));
 
@@ -950,7 +951,7 @@ static int vdebug_jtag_tlr(tap_state_t state, uint8_t f_flush)
 	tap_state_t cur = tap_get_state();
 	uint8_t tms_pre = tap_get_tms_path(cur, state);
 	uint8_t num_pre = tap_get_tms_path_len(cur, state);
-	LOG_INFO("tlr  from %x to %x", cur, state);
+	LOG_DEBUG_IO("tlr  from %x to %x", cur, state);
 	if (cur != state) {
 		rc = vdebug_jtag_shift_tap(vdc.hsocket, pbuf, num_pre, tms_pre, 0, NULL, 0, 0, NULL, f_flush);
 		tap_set_state(state);
@@ -970,7 +971,7 @@ static int vdebug_jtag_scan(struct scan_command *cmd, uint8_t f_flush)
 	uint8_t tms_post = tap_get_tms_path(state, cmd->end_state);
 	uint8_t num_post = tap_get_tms_path_len(state, cmd->end_state);
 	int num_bits = jtag_scan_size(cmd);
-	LOG_DEBUG("scan len:%d fields:%d ir/!dr:%d state cur:%x end:%x",
+	LOG_DEBUG_IO("scan len:%d fields:%d ir/!dr:%d state cur:%x end:%x",
 			  num_bits, cmd->num_fields, cmd->ir_scan, cur, cmd->end_state);
 	for (int i = 0; i < cmd->num_fields; i++) {
 		uint8_t cur_num_pre = i == 0 ? num_pre : 0;
@@ -996,7 +997,7 @@ static int vdebug_jtag_runtest(int cycles, tap_state_t state, uint8_t f_flush)
 	tap_state_t cur = tap_get_state();
 	uint8_t tms_pre = tap_get_tms_path(cur, state);
 	uint8_t num_pre = tap_get_tms_path_len(cur, state);
-	LOG_DEBUG("idle len:%d state cur:%x end:%x", cycles, cur, state);
+	LOG_DEBUG_IO("idle len:%d state cur:%x end:%x", cycles, cur, state);
 	int rc = vdebug_jtag_shift_tap(vdc.hsocket, pbuf, num_pre, tms_pre, cycles, NULL, 0, 0, NULL, f_flush);
 	if (cur != state)
 		tap_set_state(state);
@@ -1006,7 +1007,7 @@ static int vdebug_jtag_runtest(int cycles, tap_state_t state, uint8_t f_flush)
 
 static int vdebug_jtag_stableclocks(int num, uint8_t f_flush)
 {
-	LOG_INFO("stab len:%d state cur:%x", num, tap_get_state());
+	LOG_DEBUG("stab len:%d state cur:%x", num, tap_get_state());
 
 	return vdebug_jtag_shift_tap(vdc.hsocket, pbuf, 0, 0, num, NULL, 0, 0, NULL, f_flush);
 }
@@ -1081,6 +1082,41 @@ static int vdebug_jtag_execute_queue(void)
 	return rc;
 }
 
+static int vdebug_dap_bankselect(struct adiv5_ap *ap, unsigned int reg)
+{
+	int rc = ERROR_OK;
+	uint64_t sel;
+
+	if (is_adiv6(ap->dap)) {
+		sel = ap->ap_num | (reg & 0x00000FF0);
+		if (sel != (ap->dap->select & ~0xfull)) {
+			sel |= ap->dap->select & DP_SELECT_DPBANK;
+			if (ap->dap->asize > 32)
+				sel |= (DP_SELECT1 >> 4) & DP_SELECT_DPBANK;
+			ap->dap->select = sel;
+			ap->dap->select_valid = true;
+			rc = vdebug_reg_write(vdc.hsocket, pbuf, DP_SELECT >> 2, (uint32_t)sel, VD_ASPACE_DP, 0);
+			if (rc == ERROR_OK) {
+				ap->dap->select_valid = true;
+				if (ap->dap->asize > 32)
+					rc = vdebug_reg_write(vdc.hsocket, pbuf, (DP_SELECT1 & DP_SELECT_DPBANK) >> 2,
+					(uint32_t)(sel >> 32), VD_ASPACE_DP, 0);
+				if (rc == ERROR_OK)
+					ap->dap->select1_valid = true;
+			}
+		}
+	} else {    /* ADIv5 */
+		sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
+		if (sel != ap->dap->select) {
+			ap->dap->select = sel;
+			rc = vdebug_reg_write(vdc.hsocket, pbuf, DP_SELECT >> 2, (uint32_t)sel, VD_ASPACE_DP, 0);
+			if (rc == ERROR_OK)
+				ap->dap->select_valid = true;
+		}
+	}
+	return rc;
+}
+
 static int vdebug_dap_connect(struct adiv5_dap *dap)
 {
 	return dap_dp_init(dap);
@@ -1093,20 +1129,29 @@ static int vdebug_dap_send_sequence(struct adiv5_dap *dap, enum swd_special_seq
 
 static int vdebug_dap_queue_dp_read(struct adiv5_dap *dap, unsigned int reg, uint32_t *data)
 {
+	if (reg != DP_SELECT && reg != DP_RDBUFF
+		&& (!dap->select_valid || ((reg >> 4) & DP_SELECT_DPBANK) != (dap->select & DP_SELECT_DPBANK))) {
+		dap->select = (dap->select & ~DP_SELECT_DPBANK) | ((reg >> 4) & DP_SELECT_DPBANK);
+		vdebug_reg_write(vdc.hsocket, pbuf, DP_SELECT >> 2, dap->select, VD_ASPACE_DP, 0);
+		dap->select_valid = true;
+	}
 	return vdebug_reg_read(vdc.hsocket, pbuf, (reg & DP_SELECT_DPBANK) >> 2, data, VD_ASPACE_DP, 0);
 }
 
 static int vdebug_dap_queue_dp_write(struct adiv5_dap *dap, unsigned int reg, uint32_t data)
 {
+	if (reg != DP_SELECT && reg != DP_RDBUFF
+		&& (!dap->select_valid || ((reg >> 4) & DP_SELECT_DPBANK) != (dap->select & DP_SELECT_DPBANK))) {
+		dap->select = (dap->select & ~DP_SELECT_DPBANK) | ((reg >> 4) & DP_SELECT_DPBANK);
+		vdebug_reg_write(vdc.hsocket, pbuf, DP_SELECT >> 2, dap->select, VD_ASPACE_DP, 0);
+		dap->select_valid = true;
+	}
 	return vdebug_reg_write(vdc.hsocket, pbuf, (reg & DP_SELECT_DPBANK) >> 2, data, VD_ASPACE_DP, 0);
 }
 
 static int vdebug_dap_queue_ap_read(struct adiv5_ap *ap, unsigned int reg, uint32_t *data)
 {
-	if ((reg & ADIV5_DP_SELECT_APBANK) != ap->dap->select) {
-		vdebug_reg_write(vdc.hsocket, pbuf, DP_SELECT >> 2, reg & ADIV5_DP_SELECT_APBANK, VD_ASPACE_DP, 0);
-		ap->dap->select = reg & ADIV5_DP_SELECT_APBANK;
-	}
+	vdebug_dap_bankselect(ap, reg);
 
 	vdebug_reg_read(vdc.hsocket, pbuf, (reg & DP_SELECT_DPBANK) >> 2, NULL, VD_ASPACE_AP, 0);
 
@@ -1115,11 +1160,7 @@ static int vdebug_dap_queue_ap_read(struct adiv5_ap *ap, unsigned int reg, uint3
 
 static int vdebug_dap_queue_ap_write(struct adiv5_ap *ap, unsigned int reg, uint32_t data)
 {
-	if ((reg & ADIV5_DP_SELECT_APBANK) != ap->dap->select) {
-		vdebug_reg_write(vdc.hsocket, pbuf, DP_SELECT >> 2, reg & ADIV5_DP_SELECT_APBANK, VD_ASPACE_DP, 0);
-		ap->dap->select = reg & ADIV5_DP_SELECT_APBANK;
-	}
-
+	vdebug_dap_bankselect(ap, reg);
 	return vdebug_reg_write(vdc.hsocket, pbuf, (reg & DP_SELECT_DPBANK) >> 2, data, VD_ASPACE_AP, 0);
 }
 
@@ -1175,7 +1216,7 @@ COMMAND_HANDLER(vdebug_set_bfm)
 		break;
 	}
 	if (transport_is_dapdirect_swd())
-		vdc.bfm_type = VD_BFM_SWDP;
+		vdc.bfm_type = strstr(vdc.bfm_path, "dap6") ? VD_BFM_DAP6 : VD_BFM_SWDP;
 	else
 		vdc.bfm_type = VD_BFM_JTAG;
 	LOG_DEBUG("bfm_path: %s clk_period %ups", vdc.bfm_path, vdc.bfm_period);
diff --git a/tcl/board/vd_a53x2_dap.cfg b/tcl/board/vd_a53x2_dap.cfg
index 4cf5594..bcf8b44 100644
--- a/tcl/board/vd_a53x2_dap.cfg
+++ b/tcl/board/vd_a53x2_dap.cfg
@@ -4,10 +4,13 @@
 
 source [find interface/vdebug.cfg]
 
-set _CORES 2
-set _CHIPNAME a53
-set _MEMSTART 0x00000000
-set _MEMSIZE 0x1000000
+set CORES 2
+set CHIPNAME a53
+set ACCESSPORT 0
+set MEMSTART 0x00000000
+set MEMSIZE 0x1000000
+set DBGBASE {0x80810000 0x80910000}
+set CTIBASE {0x80820000 0x80920000}
 
 # vdebug select transport
 transport select dapdirect_swd
@@ -19,11 +22,9 @@ adapter srst delay 5
 # BFM hierarchical path and input clk period
 vdebug bfm_path tbench.u_vd_swdp_bfm 10ns
 
-# DMA Memories to access backdoor (up to 4)
-vdebug mem_path tbench.u_memory.mem_array $_MEMSTART $_MEMSIZE
+# DMA Memories to access backdoor (up to 20)
+vdebug mem_path tbench.u_memory.mem_array $MEMSTART $MEMSIZE
 
-source [find target/swj-dp.tcl]
-
-swj_newdap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf
+swd newdap $CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf
 
 source [find target/vd_aarch64.cfg]
diff --git a/tcl/board/vd_a53x2_jtag.cfg b/tcl/board/vd_a53x2_jtag.cfg
index a5e8d24..0c3eebd 100644
--- a/tcl/board/vd_a53x2_jtag.cfg
+++ b/tcl/board/vd_a53x2_jtag.cfg
@@ -4,11 +4,14 @@
 
 source [find interface/vdebug.cfg]
 
-set _CORES 2
-set _CHIPNAME a53
-set _MEMSTART 0x00000000
-set _MEMSIZE 0x1000000
-set _CPUTAPID 0x5ba00477
+set CORES 2
+set CHIPNAME a53
+set ACCESSPORT 0
+set MEMSTART 0x00000000
+set MEMSIZE 0x1000000
+set DBGBASE {0x80810000 0x80910000}
+set CTIBASE {0x80820000 0x80920000}
+set CPUTAPID 0x5ba00477
 
 # vdebug select transport
 transport select jtag
@@ -21,11 +24,10 @@ adapter srst delay 5
 # BFM hierarchical path and input clk period
 vdebug bfm_path tbench.u_vd_jtag_bfm 10ns
 
-# DMA Memories to access backdoor (up to 4)
-vdebug mem_path tbench.u_memory.mem_array $_MEMSTART $_MEMSIZE
-
-jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
+# DMA Memories to access backdoor (up to 20)
+vdebug mem_path tbench.u_memory.mem_array $MEMSTART $MEMSIZE
 
+jtag newtap $CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $CPUTAPID
 jtag arp_init-reset
 
 source [find target/vd_aarch64.cfg]
diff --git a/tcl/board/vd_a75x4_dap.cfg b/tcl/board/vd_a75x4_dap.cfg
new file mode 100644
index 0000000..5c2a2ef
--- /dev/null
+++ b/tcl/board/vd_a75x4_dap.cfg
@@ -0,0 +1,30 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+# Cadence virtual debug interface
+# Arm Cortex A53x2 through DAP
+
+source [find interface/vdebug.cfg]
+
+set CORES 4
+set CHIPNAME a75
+set ACCESSPORT 0x00040000
+set MEMSTART 0x00000000
+set MEMSIZE 0x1000000
+set DBGBASE {0x01010000 0x01110000 0x01210000 0x01310000}
+set CTIBASE {0x01020000 0x01120000 0x01220000 0x01320000}
+
+# vdebug select transport
+transport select dapdirect_swd
+
+# JTAG reset config, frequency and reset delay
+adapter speed 200000
+adapter srst delay 5
+
+# BFM hierarchical path and input clk period
+vdebug bfm_path tbench.u_vd_dap6_bfm 2250ps
+
+# DMA Memories to access backdoor (up to 20)
+#vdebug mem_path tbench.u_memory.mem_array $_MEMSTART $_MEMSIZE
+
+swd newdap $CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf
+
+source [find target/vd_aarch64.cfg]
diff --git a/tcl/board/vd_a75x4_jtag.cfg b/tcl/board/vd_a75x4_jtag.cfg
new file mode 100644
index 0000000..c94a719
--- /dev/null
+++ b/tcl/board/vd_a75x4_jtag.cfg
@@ -0,0 +1,30 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+# Cadence virtual debug interface
+# Arm Cortex A53x2 through DAP
+
+source [find interface/vdebug.cfg]
+
+set CORES 4
+set CHIPNAME a75
+set ACCESSPORT 0x00040000
+set MEMSTART 0x00000000
+set MEMSIZE 0x1000000
+set DBGBASE {0x01010000 0x01110000 0x01210000 0x01310000}
+set CTIBASE {0x01020000 0x01120000 0x01220000 0x01320000}
+set CPUTAPID 0x4ba06477
+
+# vdebug select transport
+transport select jtag
+
+# JTAG reset config, frequency and reset delay
+reset_config trst_and_srst
+adapter speed 1500000
+adapter srst delay 5
+
+# BFM hierarchical path and input clk period
+vdebug bfm_path tbench.u_vd_jtag_bfm 333ps
+
+jtag newtap $CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $CPUTAPID
+jtag arp_init-reset
+
+source [find target/vd_aarch64.cfg]
diff --git a/tcl/board/vd_m4_dap.cfg b/tcl/board/vd_m4_dap.cfg
index 691b623..5d3605a 100644
--- a/tcl/board/vd_m4_dap.cfg
+++ b/tcl/board/vd_m4_dap.cfg
@@ -4,9 +4,9 @@
 
 source [find interface/vdebug.cfg]
 
-set _CHIPNAME m4
-set _MEMSTART 0x00000000
-set _MEMSIZE 0x10000
+set CHIPNAME m4
+set MEMSTART 0x00000000
+set MEMSIZE 0x10000
 
 # vdebug select transport
 transport select dapdirect_swd
@@ -16,11 +16,9 @@ adapter srst delay 5
 # BFM hierarchical path and input clk period
 vdebug bfm_path tbench.u_vd_swdp_bfm 20ns
 
-# DMA Memories to access backdoor (up to 4)
-vdebug mem_path tbench.u_mcu.u_sys.u_rom.rom $_MEMSTART $_MEMSIZE
+# DMA Memories to access backdoor (up to 20)
+vdebug mem_path tbench.u_mcu.u_sys.u_rom.rom $MEMSTART $MEMSIZE
 
-source [find target/swj-dp.tcl]
-
-swj_newdap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf
+swd newdap $CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf
 
 source [find target/vd_cortex_m.cfg]
diff --git a/tcl/board/vd_m4_jtag.cfg b/tcl/board/vd_m4_jtag.cfg
index 4c795eb..3b32e17 100644
--- a/tcl/board/vd_m4_jtag.cfg
+++ b/tcl/board/vd_m4_jtag.cfg
@@ -4,10 +4,10 @@
 
 source [find interface/vdebug.cfg]
 
-set _CHIPNAME m4
-set _MEMSTART 0x00000000
-set _MEMSIZE 0x10000
-set _CPUTAPID 0x4ba00477
+set CHIPNAME m4
+set MEMSTART 0x00000000
+set MEMSIZE 0x10000
+set CPUTAPID 0x4ba00477
 
 # vdebug select transport
 transport select jtag
@@ -20,11 +20,10 @@ adapter srst delay 5
 # BFM hierarchical path and input clk period
 vdebug bfm_path tbench.u_vd_jtag_bfm 20ns
 
-# DMA Memories to access backdoor (up to 4)
-vdebug mem_path tbench.u_mcu.u_sys.u_rom.rom $_MEMSTART $_MEMSIZE
-
-jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
+# DMA Memories to access backdoor (up to 20)
+vdebug mem_path tbench.u_mcu.u_sys.u_rom.rom $MEMSTART $MEMSIZE
 
+jtag newtap $CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $CPUTAPID
 jtag arp_init-reset
 
 source [find target/vd_cortex_m.cfg]
diff --git a/tcl/board/vd_m7_jtag.cfg b/tcl/board/vd_m7_jtag.cfg
index 880ef9b..9a89584 100644
--- a/tcl/board/vd_m7_jtag.cfg
+++ b/tcl/board/vd_m7_jtag.cfg
@@ -4,10 +4,10 @@
 
 source [find interface/vdebug.cfg]
 
-set _CHIPNAME m7
-set _MEMSTART 0x00000000
-set _MEMSIZE 0x100000
-set _CPUTAPID 0x0ba02477
+set CHIPNAME m7
+set MEMSTART 0x00000000
+set MEMSIZE 0x100000
+set CPUTAPID 0x0ba02477
 
 # vdebug select JTAG transport
 transport select jtag
@@ -20,11 +20,10 @@ adapter srst delay 5
 # BFM hierarchical path and input clk period
 vdebug bfm_path tbench.u_vd_jtag_bfm 10ns
 
-# DMA Memories to access backdoor (up to 4)
-vdebug mem_path tbench.u_mcu.u_sys.u_itcm_ram.Mem $_MEMSTART $_MEMSIZE
-
-jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
+# DMA Memories to access backdoor (up to 20)
+vdebug mem_path tbench.u_mcu.u_sys.u_itcm_ram.Mem $MEMSTART $MEMSIZE
 
+jtag newtap $CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $CPUTAPID
 jtag arp_init-reset
 
 source [find target/vd_cortex_m.cfg]
diff --git a/tcl/target/vd_aarch64.cfg b/tcl/target/vd_aarch64.cfg
index 619134a..177416b 100644
--- a/tcl/target/vd_aarch64.cfg
+++ b/tcl/target/vd_aarch64.cfg
@@ -2,36 +2,44 @@
 # Cadence virtual debug interface
 # Arm v8 64b Cortex A
 
-if {![info exists _CORES]} {
-	set _CORES 1
+if {![info exists CORES]} {
+	set CORES 1
 }
-if {![info exists _CHIPNAME]} {
-	set _CHIPNAME aarch64
+if {![info exists CHIPNAME]} {
+	set CHIPNAME aarch64
+}
+if {[info exists ACCESSPORT]} {
+	set _APNUM "-ap-num $ACCESSPORT"
+	if { $ACCESSPORT > 0xff } {
+		set _DAP6 "-adiv6"
+	} else {
+		set _DAP6 "-adiv5"
+	}
+} else {
+	set _APNUM ""
 }
-set _TARGETNAME $_CHIPNAME.cpu
-set _CTINAME $_CHIPNAME.cti
 
-set DBGBASE {0x80810000 0x80910000}
-set CTIBASE {0x80820000 0x80920000}
+set _TARGETNAME $CHIPNAME.cpu
+set _CTINAME $CHIPNAME.cti
+set _DAPNAME $CHIPNAME.dap
 
-dap create $_CHIPNAME.dap -chain-position $_TARGETNAME
-$_CHIPNAME.dap apsel 1
+dap create $_DAPNAME $_DAP6 -chain-position $_TARGETNAME
 
-for { set _core 0 } { $_core < $_CORES } { incr _core } \
+for { set _core 0 } { $_core < $CORES } { incr _core } \
 {
-	cti create $_CTINAME.$_core -dap $_CHIPNAME.dap -ap-num 1 -baseaddr [lindex $CTIBASE $_core]
-	set _command "target create $_TARGETNAME.$_core aarch64 -dap $_CHIPNAME.dap \
-	-dbgbase [lindex $DBGBASE $_core] -cti $_CTINAME.$_core -coreid $_core"
+	set _cmd "cti create $_CTINAME.$_core -dap $_DAPNAME $_APNUM -baseaddr [lindex $CTIBASE $_core]"
+	eval $_cmd
+	set _cmd "target create $_TARGETNAME.$_core aarch64 -dap $_DAPNAME $_APNUM -dbgbase [lindex $DBGBASE $_core] -cti $_CTINAME.$_core -coreid $_core"
 	if { $_core != 0 } {
 		# non-boot core examination may fail
-		set _command "$_command -defer-examine"
-		set _smp_command "$_smp_command $_TARGETNAME.$_core"
+		set _cmd "$_cmd -defer-examine"
+		set _smp_cmd "$_smp_cmd $_TARGETNAME.$_core"
 	} else {
-		set _smp_command "target smp $_TARGETNAME.$_core"
+		set _smp_cmd "target smp $_TARGETNAME.$_core"
 	}
-	eval $_command
+	eval $_cmd
 }
-eval $_smp_command
+eval $_smp_cmd
 
-# default target is core 0
-targets $_TARGETNAME.0
+set _TARGETCUR $_TARGETNAME.0
+targets $_TARGETCUR
diff --git a/tcl/target/vd_cortex_m.cfg b/tcl/target/vd_cortex_m.cfg
index 4d7b0df..7db9d3a 100644
--- a/tcl/target/vd_cortex_m.cfg
+++ b/tcl/target/vd_cortex_m.cfg
@@ -2,11 +2,12 @@
 # Cadence virtual debug interface
 # ARM Cortex M
 
-if {![info exists _CHIPNAME]} {
-	set _CHIPNAME cortex_m
+if {![info exists CHIPNAME]} {
+	set CHIPNAME cortex_m
 }
-set _TARGETNAME $_CHIPNAME.cpu
+set _TARGETNAME $CHIPNAME.cpu
+set _DAPNAME $CHIPNAME.dap
 
-dap create $_CHIPNAME.dap -chain-position $_TARGETNAME
+dap create $_DAPNAME -chain-position $_TARGETNAME
 
-target create $_TARGETNAME cortex_m -dap $_CHIPNAME.dap
+target create $_TARGETNAME cortex_m -dap $_DAPNAME