adapter/xds110: Hide '(dis)connected' message

Print a debug message rather than an info message because this
information is not of importance for normal users.

Change-Id: Ie91565df455ffc0bfe976d1782dd4318bfd2d30b
Signed-off-by: Marc Schink <dev@zapb.de>
Reviewed-on: https://review.openocd.org/c/openocd/+/8986
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

configure.ac

@@ -177,8 +177,8 @@ m4_define([REMOTE_BITBANG_ADAPTER],
 m4_define([LIBJAYLINK_ADAPTERS],
 	[[[jlink], [SEGGER J-Link Programmer], [JLINK]]])
 
-m4_define([PCIE_ADAPTERS],
-	[[[xlnx_pcie_xvc], [Xilinx XVC/PCIe], [XLNX_PCIE_XVC]]])
+m4_define([XVC_ADAPTERS],
+	[[[xlnx_xvc], [Xilinx XVC PCIe and AXI drives], [XLNX_XVC]]])
 
 m4_define([SERIAL_PORT_ADAPTERS],
 	[[[buspirate], [Bus Pirate], [BUS_PIRATE]]])
@@ -335,7 +335,7 @@ AC_ARG_ADAPTERS([
   JTAG_DPI_ADAPTER,
   JTAG_VPI_ADAPTER,
   RSHIM_ADAPTER,
-  PCIE_ADAPTERS,
+  XVC_ADAPTERS,
   LIBJAYLINK_ADAPTERS
   ],[auto])
 
@@ -644,7 +644,7 @@ PROCESS_ADAPTERS([DMEM_ADAPTER], ["x$is_linux" = "xyes"], [Linux /dev/mem])
 PROCESS_ADAPTERS([SYSFSGPIO_ADAPTER], ["x$is_linux" = "xyes"], [Linux sysfs])
 PROCESS_ADAPTERS([REMOTE_BITBANG_ADAPTER], [true], [unused])
 PROCESS_ADAPTERS([LIBJAYLINK_ADAPTERS], ["x$use_internal_libjaylink" = "xyes" -o "x$use_libjaylink" = "xyes"], [libjaylink-0.2])
-PROCESS_ADAPTERS([PCIE_ADAPTERS], ["x$is_linux" = "xyes" -a "x$ac_cv_header_linux_pci_h" = "xyes"], [Linux build])
+PROCESS_ADAPTERS([XVC_ADAPTERS], ["x$is_linux" = "xyes" -a "x$ac_cv_header_linux_pci_h" = "xyes"], [Linux build])
 PROCESS_ADAPTERS([SERIAL_PORT_ADAPTERS], ["x$can_build_buspirate" = "xyes"],
                                          [internal error: validation should happen beforehand])
 PROCESS_ADAPTERS([LINUXSPIDEV_ADAPTER], ["x$is_linux" = "xyes" -a "x$ac_cv_header_linux_spi_spidev_h" = "xyes"],
@@ -840,7 +840,8 @@ m4_foreach([adapterTuple], [USB1_ADAPTERS,
 	DMEM_ADAPTER,
 	SYSFSGPIO_ADAPTER,
 	REMOTE_BITBANG_ADAPTER,
-	LIBJAYLINK_ADAPTERS, PCIE_ADAPTERS, SERIAL_PORT_ADAPTERS,
+	LIBJAYLINK_ADAPTERS, XVC_ADAPTERS,
+	SERIAL_PORT_ADAPTERS,
 	LINUXSPIDEV_ADAPTER,
 	VDEBUG_ADAPTER,
 	JTAG_DPI_ADAPTER,

contrib/60-openocd.rules

@@ -190,6 +190,10 @@ ATTRS{idVendor}=="138e", ATTRS{idProduct}=="9000", MODE="660", GROUP="plugdev",
 # Debug Board for Neo1973
 ATTRS{idVendor}=="1457", ATTRS{idProduct}=="5118", MODE="660", GROUP="plugdev", TAG+="uaccess"
 
+# Microchip RISC-V Debug
+ATTRS{idVendor}=="1514", ATTRS{idProduct}=="2008", MODE="660", GROUP="plugdev", TAG+="uaccess"
+ATTRS{idVendor}=="1514", ATTRS{idProduct}=="200a", MODE="660", GROUP="plugdev", TAG+="uaccess"
+
 # OSBDM
 ATTRS{idVendor}=="15a2", ATTRS{idProduct}=="0042", MODE="660", GROUP="plugdev", TAG+="uaccess"
 ATTRS{idVendor}=="15a2", ATTRS{idProduct}=="0058", MODE="660", GROUP="plugdev", TAG+="uaccess"

doc/openocd.texi

@@ -613,6 +613,12 @@ emulation model of target hardware.
 
 @item @b{xlnx_pcie_xvc}
 @* A JTAG driver exposing Xilinx Virtual Cable over PCI Express to OpenOCD as JTAG/SWD interface.
+@* Link: @url{https://www.xilinx.com/products/intellectual-property/debug-bridge.html}
+
+@item @b{xlnx_axi_xvc}
+@* A JTAG driver exposing JTAG to OpenOCD over AXI-mapped registers.
+@* Link: @url{https://docs.amd.com/r/en-US/pg437-axi-jtag/Introduction}
+@* Link: @url{https://china.xilinx.com/support/documentation/application_notes/xapp1251-xvc-zynq-petalinux.pdf}
 
 @item @b{linuxspidev}
 @* A SPI based SWD driver using Linux SPI devices.
@@ -3352,6 +3358,21 @@ The string will be of the format "DDDD:BB:SS.F" such as "0000:65:00.1".
 @end deffn
 @end deffn
 
+@deffn {Interface Driver} {xlnx_axi_xvc}
+This driver supports the Xilinx JTAG mapping over AXI using the AXI to JTAG
+Converter or the AXI-to-JTAG mode of the debug bridge.
+It is commonly found in Xilinx MPSoC based designs. It allows debugging
+fabric based JTAG/SWD devices such as Cortex-M1/M3 or RISC-V softcores. Access to this
+is exposed via extended capability registers in the AXI-mapped configuration space.
+
+@deffn {Config Command} {xlnx_axi_xvc dev_addr} addr
+Specifies the address of the AXI-mapped registers via parameter @var{addr}.
+
+The correct value for @var{addr} is specified in the "Address Editor" tab
+in Vivado.
+@end deffn
+@end deffn
+
 @deffn {Interface Driver} {bcm2835gpio}
 This GPIO interface is present in Raspberry Pi 0-4 which is a cheap
 single-board computer exposing some GPIOs on its expansion header.

src/jtag/drivers/Makefile.am

@@ -179,8 +179,8 @@ endif
 if LINUXSPIDEV
 DRIVERFILES += %D%/linuxspidev.c
 endif
-if XLNX_PCIE_XVC
-DRIVERFILES += %D%/xlnx-pcie-xvc.c
+if XLNX_XVC
+DRIVERFILES += %D%/xlnx-xvc.c
 endif
 if BCM2835GPIO
 DRIVERFILES += %D%/bcm2835gpio.c

src/jtag/drivers/xds110.c

@@ -428,7 +428,7 @@ static bool usb_connect(void)
 
 	/* Log the results */
 	if (result == 0)
-		LOG_INFO("XDS110: connected");
+		LOG_DEBUG("XDS110: connected");
 	else
 		LOG_ERROR("XDS110: failed to connect");
 
@@ -448,7 +448,7 @@ static void usb_disconnect(void)
 		xds110.ctx = NULL;
 	}
 
-	LOG_INFO("XDS110: disconnected");
+	LOG_DEBUG("XDS110: disconnected");
 }
 
 static bool usb_read(unsigned char *buffer, int size, int *bytes_read,

src/jtag/drivers/xlnx-xvc.c

@@ -1,8 +1,14 @@
 // SPDX-License-Identifier: GPL-2.0-only
 
 /*
- * Copyright (c) 2019 Google, LLC.
- * Author: Moritz Fischer <moritzf@google.com>
+ *	 Copyright (C) 2019 Google, LLC.
+ *	 Moritz Fischer <moritzf@google.com>
+ *
+ *	 Copyright (C) 2021 Western Digital Corporation or its affiliates
+ *	 Jeremy Garff <jeremy.garff@wdc.com>
+ *
+ *	 Copyright (C) 2024 Inria
+ *	 Nicolas Derumigny <nicolas.derumigny@inria.fr>
  */
 
 #ifdef HAVE_CONFIG_H
@@ -11,13 +17,14 @@
 
 #include <stdint.h>
 #include <stdlib.h>
-#include <math.h>
 #include <unistd.h>
 #include <linux/pci.h>
+#include <sys/mman.h>
 
 #include <jtag/interface.h>
-#include <jtag/swd.h>
 #include <jtag/commands.h>
+#include <jtag/interface.h>
+#include <jtag/swd.h>
 #include <helper/replacements.h>
 #include <helper/bits.h>
 
@@ -28,14 +35,24 @@
 
 #define PCIE_EXT_CAP_LST	0x100
 
-#define XLNX_XVC_EXT_CAP	0x00
-#define XLNX_XVC_VSEC_HDR	0x04
-#define XLNX_XVC_LEN_REG	0x0C
-#define XLNX_XVC_TMS_REG	0x10
-#define XLNX_XVC_TDX_REG	0x14
+#define XLNX_PCIE_XVC_EXT_CAP	0x00
+#define XLNX_PCIE_XVC_VSEC_HDR	0x04
+#define XLNX_PCIE_XVC_LEN_REG	0x0C
+#define XLNX_PCIE_XVC_TMS_REG	0x10
+#define XLNX_PCIE_XVC_TDX_REG	0x14
+
+#define XLNX_PCIE_XVC_CAP_SIZE	0x20
+#define XLNX_PCIE_XVC_VSEC_ID	0x8
+
+#define XLNX_AXI_XVC_LEN_REG	0x00
+#define XLNX_AXI_XVC_TMS_REG	0x04
+#define XLNX_AXI_XVC_TDI_REG	0x08
+#define XLNX_AXI_XVC_TDO_REG	0x0c
+#define XLNX_AXI_XVC_CTRL_REG	0x10
+#define XLNX_AXI_XVC_MAX_REG	0x18
+
+#define XLNX_AXI_XVC_CTRL_REG_ENABLE_MASK	0x01
 
-#define XLNX_XVC_CAP_SIZE	0x20
-#define XLNX_XVC_VSEC_ID	0x8
 #define XLNX_XVC_MAX_BITS	0x20
 
 #define MASK_ACK(x) (((x) >> 9) & 0x7)
@@ -47,8 +64,23 @@ struct xlnx_pcie_xvc {
 	char *device;
 };
 
+struct xlnx_axi_xvc {
+	int fd;
+	uint32_t *base;
+	char *device_addr;
+	// Defaults to `/dev/mem` if NULL
+	char *device_file;
+};
+
+enum xlnx_xvc_type_t {
+	PCIE,
+	AXI
+};
+
 static struct xlnx_pcie_xvc xlnx_pcie_xvc_state;
 static struct xlnx_pcie_xvc *xlnx_pcie_xvc = &xlnx_pcie_xvc_state;
+static struct xlnx_axi_xvc xlnx_axi_xvc_state;
+static struct xlnx_axi_xvc *xlnx_axi_xvc = &xlnx_axi_xvc_state;
 
 static int xlnx_pcie_xvc_read_reg(const int offset, uint32_t *val)
 {
@@ -60,9 +92,9 @@ static int xlnx_pcie_xvc_read_reg(const int offset, uint32_t *val)
 	 * space accessor functions
 	 */
 	err = pread(xlnx_pcie_xvc->fd, &res, sizeof(res),
-		    xlnx_pcie_xvc->offset + offset);
+			xlnx_pcie_xvc->offset + offset);
 	if (err != sizeof(res)) {
-		LOG_ERROR("Failed to read offset %x", offset);
+		LOG_ERROR("Failed to read offset 0x%x", offset);
 		return ERROR_JTAG_DEVICE_ERROR;
 	}
 
@@ -72,6 +104,19 @@ static int xlnx_pcie_xvc_read_reg(const int offset, uint32_t *val)
 	return ERROR_OK;
 }
 
+static int xlnx_axi_xvc_read_reg(const int offset, uint32_t *val)
+{
+	uintptr_t b = ((uintptr_t)xlnx_axi_xvc->base) + offset;
+	volatile uint32_t *w = (uint32_t *)b;
+
+	if (val) {
+		__atomic_thread_fence(__ATOMIC_SEQ_CST);
+		*val = *w;
+	}
+
+	return ERROR_OK;
+}
+
 static int xlnx_pcie_xvc_write_reg(const int offset, const uint32_t val)
 {
 	int err;
@@ -81,9 +126,9 @@ static int xlnx_pcie_xvc_write_reg(const int offset, const uint32_t val)
 	 * space accessor functions
 	 */
 	err = pwrite(xlnx_pcie_xvc->fd, &val, sizeof(val),
-		     xlnx_pcie_xvc->offset + offset);
+			 xlnx_pcie_xvc->offset + offset);
 	if (err != sizeof(val)) {
-		LOG_ERROR("Failed to write offset: %x with value: %" PRIx32,
+		LOG_ERROR("Failed to write offset: 0x%x with value: %" PRIx32,
 			  offset, val);
 		return ERROR_JTAG_DEVICE_ERROR;
 	}
@@ -91,37 +136,117 @@ static int xlnx_pcie_xvc_write_reg(const int offset, const uint32_t val)
 	return ERROR_OK;
 }
 
+static int xlnx_axi_xvc_write_reg(const int offset, const uint32_t val)
+{
+	uintptr_t b = ((uintptr_t)xlnx_axi_xvc->base) + offset;
+	volatile uint32_t *w = (uint32_t *)b;
+
+	*w = val;
+	__atomic_thread_fence(__ATOMIC_SEQ_CST);
+
+	return ERROR_OK;
+}
+
 static int xlnx_pcie_xvc_transact(size_t num_bits, uint32_t tms, uint32_t tdi,
 				  uint32_t *tdo)
 {
 	int err;
 
-	err = xlnx_pcie_xvc_write_reg(XLNX_XVC_LEN_REG, num_bits);
+	err = xlnx_pcie_xvc_write_reg(XLNX_PCIE_XVC_LEN_REG, num_bits);
 	if (err != ERROR_OK)
 		return err;
 
-	err = xlnx_pcie_xvc_write_reg(XLNX_XVC_TMS_REG, tms);
+	err = xlnx_pcie_xvc_write_reg(XLNX_PCIE_XVC_TMS_REG, tms);
 	if (err != ERROR_OK)
 		return err;
 
-	err = xlnx_pcie_xvc_write_reg(XLNX_XVC_TDX_REG, tdi);
+	err = xlnx_pcie_xvc_write_reg(XLNX_PCIE_XVC_TDX_REG, tdi);
 	if (err != ERROR_OK)
 		return err;
 
-	err = xlnx_pcie_xvc_read_reg(XLNX_XVC_TDX_REG, tdo);
+	err = xlnx_pcie_xvc_read_reg(XLNX_PCIE_XVC_TDX_REG, tdo);
 	if (err != ERROR_OK)
 		return err;
 
 	if (tdo)
 		LOG_DEBUG_IO("Transact num_bits: %zu, tms: %" PRIx32 ", tdi: %" PRIx32 ", tdo: %" PRIx32,
-			     num_bits, tms, tdi, *tdo);
+				 num_bits, tms, tdi, *tdo);
 	else
 		LOG_DEBUG_IO("Transact num_bits: %zu, tms: %" PRIx32 ", tdi: %" PRIx32 ", tdo: <null>",
-			     num_bits, tms, tdi);
+				 num_bits, tms, tdi);
 	return ERROR_OK;
 }
 
-static int xlnx_pcie_xvc_execute_stableclocks(struct jtag_command *cmd)
+static int xlnx_axi_xvc_transact(size_t num_bits, uint32_t tms, uint32_t tdi,
+				  uint32_t *tdo)
+{
+	uint32_t ctrl;
+	int done = 0;
+	int err;
+
+	err = xlnx_axi_xvc_write_reg(XLNX_AXI_XVC_LEN_REG, num_bits);
+	if (err != ERROR_OK)
+		return err;
+
+	err = xlnx_axi_xvc_write_reg(XLNX_AXI_XVC_TMS_REG, tms);
+	if (err != ERROR_OK)
+		return err;
+
+	err = xlnx_axi_xvc_write_reg(XLNX_AXI_XVC_TDI_REG, tdi);
+	if (err != ERROR_OK)
+		return err;
+
+	err = xlnx_axi_xvc_write_reg(XLNX_AXI_XVC_CTRL_REG, XLNX_AXI_XVC_CTRL_REG_ENABLE_MASK);
+	if (err != ERROR_OK)
+		return err;
+
+	while (!done) {
+		err = xlnx_axi_xvc_read_reg(XLNX_AXI_XVC_CTRL_REG, &ctrl);
+		if (err != ERROR_OK)
+			return err;
+
+		if (!(ctrl & XLNX_AXI_XVC_CTRL_REG_ENABLE_MASK))
+			done = 1;
+
+		/*
+		 There is no delay here intentionally.	The usleep()
+		 function doesn't block and burns CPU cycles anyway.
+		 The turnaround time is fast enough at high JTAG rates
+		 that adding the call can slow down the overall
+		 throughput.	So we'll just sacrifice the CPU to get
+		 best performance.
+
+		 Additionally there is no timeout.	The underlying
+		 hardware is guaranteed to unset the enable bit within
+		 32 JTAG clock cycles.	There is no hardware condition
+		 that will keep it set forever.  Essentially, the hardware
+		 is also our timeout mechanism.
+		*/
+	}
+
+	err = xlnx_axi_xvc_read_reg(XLNX_AXI_XVC_TDO_REG, tdo);
+	if (err != ERROR_OK)
+		return err;
+
+	if (tdo)
+		LOG_DEBUG_IO("Transact num_bits: %zu, tms: 0x%x, tdi: 0x%x, tdo: 0x%x",
+				 num_bits, tms, tdi, *tdo);
+	else
+		LOG_DEBUG_IO("Transact num_bits: %zu, tms: 0x%x, tdi: 0x%x, tdo: <null>",
+				 num_bits, tms, tdi);
+	return ERROR_OK;
+}
+
+static int xlnx_xvc_transact(size_t num_bits, uint32_t tms, uint32_t tdi,
+		uint32_t *tdo, enum xlnx_xvc_type_t xvc_type)
+{
+	if (xvc_type == PCIE)
+		return xlnx_pcie_xvc_transact(num_bits, tms, tdi, tdo);
+	assert(xvc_type == AXI);
+	return xlnx_axi_xvc_transact(num_bits, tms, tdi, tdo);
+}
+
+static int xlnx_xvc_execute_stableclocks(struct jtag_command *cmd, enum xlnx_xvc_type_t xvc_type)
 {
 	int tms = tap_get_state() == TAP_RESET ? 1 : 0;
 	size_t left = cmd->cmd.stableclocks->num_cycles;
@@ -132,7 +257,7 @@ static int xlnx_pcie_xvc_execute_stableclocks(struct jtag_command *cmd)
 
 	while (left) {
 		write = MIN(XLNX_XVC_MAX_BITS, left);
-		err = xlnx_pcie_xvc_transact(write, tms, 0, NULL);
+		err = xlnx_xvc_transact(write, tms, 0, NULL, xvc_type);
 		if (err != ERROR_OK)
 			return err;
 		left -= write;
@@ -141,12 +266,12 @@ static int xlnx_pcie_xvc_execute_stableclocks(struct jtag_command *cmd)
 	return ERROR_OK;
 }
 
-static int xlnx_pcie_xvc_execute_statemove(size_t skip)
+static int xlnx_xvc_execute_statemove(size_t skip, enum xlnx_xvc_type_t xvc_type)
 {
 	uint8_t tms_scan = tap_get_tms_path(tap_get_state(),
-					    tap_get_end_state());
+						tap_get_end_state());
 	int tms_count = tap_get_tms_path_len(tap_get_state(),
-					     tap_get_end_state());
+						 tap_get_end_state());
 	int err;
 
 	LOG_DEBUG("statemove starting at (skip: %zu) %s end in %s", skip,
@@ -154,7 +279,7 @@ static int xlnx_pcie_xvc_execute_statemove(size_t skip)
 		  tap_state_name(tap_get_end_state()));
 
 
-	err = xlnx_pcie_xvc_transact(tms_count - skip, tms_scan >> skip, 0, NULL);
+	err = xlnx_xvc_transact(tms_count - skip, tms_scan >> skip, 0, NULL, xvc_type);
 	if (err != ERROR_OK)
 		return err;
 
@@ -163,7 +288,8 @@ static int xlnx_pcie_xvc_execute_statemove(size_t skip)
 	return ERROR_OK;
 }
 
-static int xlnx_pcie_xvc_execute_runtest(struct jtag_command *cmd)
+static int xlnx_xvc_execute_runtest(struct jtag_command *cmd,
+		enum xlnx_xvc_type_t xvc_type)
 {
 	int err = ERROR_OK;
 
@@ -175,7 +301,7 @@ static int xlnx_pcie_xvc_execute_runtest(struct jtag_command *cmd)
 
 	if (tap_get_state() != TAP_IDLE) {
 		tap_set_end_state(TAP_IDLE);
-		err = xlnx_pcie_xvc_execute_statemove(0);
+		err = xlnx_xvc_execute_statemove(0, xvc_type);
 		if (err != ERROR_OK)
 			return err;
 	};
@@ -185,7 +311,7 @@ static int xlnx_pcie_xvc_execute_runtest(struct jtag_command *cmd)
 
 	while (left) {
 		write = MIN(XLNX_XVC_MAX_BITS, left);
-		err = xlnx_pcie_xvc_transact(write, 0, 0, NULL);
+		err = xlnx_xvc_transact(write, 0, 0, NULL, xvc_type);
 		if (err != ERROR_OK)
 			return err;
 		left -= write;
@@ -193,12 +319,13 @@ static int xlnx_pcie_xvc_execute_runtest(struct jtag_command *cmd)
 
 	tap_set_end_state(tmp_state);
 	if (tap_get_state() != tap_get_end_state())
-		err = xlnx_pcie_xvc_execute_statemove(0);
+		err = xlnx_xvc_execute_statemove(0, xvc_type);
 
 	return err;
 }
 
-static int xlnx_pcie_xvc_execute_pathmove(struct jtag_command *cmd)
+static int xlnx_xvc_execute_pathmove(struct jtag_command *cmd,
+		enum xlnx_xvc_type_t xvc_type)
 {
 	unsigned int num_states = cmd->cmd.pathmove->num_states;
 	enum tap_state *path = cmd->cmd.pathmove->path;
@@ -210,9 +337,9 @@ static int xlnx_pcie_xvc_execute_pathmove(struct jtag_command *cmd)
 
 	for (unsigned int i = 0; i < num_states; i++) {
 		if (path[i] == tap_state_transition(tap_get_state(), false)) {
-			err = xlnx_pcie_xvc_transact(1, 0, 0, NULL);
+			err = xlnx_xvc_transact(1, 0, 0, NULL, xvc_type);
 		} else if (path[i] == tap_state_transition(tap_get_state(), true)) {
-			err = xlnx_pcie_xvc_transact(1, 1, 0, NULL);
+			err = xlnx_xvc_transact(1, 1, 0, NULL, xvc_type);
 		} else {
 			LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition.",
 				  tap_state_name(tap_get_state()),
@@ -229,7 +356,8 @@ static int xlnx_pcie_xvc_execute_pathmove(struct jtag_command *cmd)
 	return ERROR_OK;
 }
 
-static int xlnx_pcie_xvc_execute_scan(struct jtag_command *cmd)
+static int xlnx_xvc_execute_scan(struct jtag_command *cmd,
+		enum xlnx_xvc_type_t xvc_type)
 {
 	enum scan_type type = jtag_scan_type(cmd->cmd.scan);
 	enum tap_state saved_end_state = cmd->cmd.scan->end_state;
@@ -253,13 +381,13 @@ static int xlnx_pcie_xvc_execute_scan(struct jtag_command *cmd)
 	 */
 	if (ir_scan && tap_get_state() != TAP_IRSHIFT) {
 		tap_set_end_state(TAP_IRSHIFT);
-		err = xlnx_pcie_xvc_execute_statemove(0);
+		err = xlnx_xvc_execute_statemove(0, xvc_type);
 		if (err != ERROR_OK)
 			goto out_err;
 		tap_set_end_state(saved_end_state);
 	} else if (!ir_scan && (tap_get_state() != TAP_DRSHIFT)) {
 		tap_set_end_state(TAP_DRSHIFT);
-		err = xlnx_pcie_xvc_execute_statemove(0);
+		err = xlnx_xvc_execute_statemove(0, xvc_type);
 		if (err != ERROR_OK)
 			goto out_err;
 		tap_set_end_state(saved_end_state);
@@ -271,8 +399,8 @@ static int xlnx_pcie_xvc_execute_scan(struct jtag_command *cmd)
 		/* the last TMS should be a 1, to leave the state */
 		tms = left <= XLNX_XVC_MAX_BITS ? BIT(write - 1) : 0;
 		tdi = (type != SCAN_IN) ? buf_get_u32(rd_ptr, 0, write) : 0;
-		err = xlnx_pcie_xvc_transact(write, tms, tdi, type != SCAN_OUT ?
-					     &tdo : NULL);
+		err = xlnx_xvc_transact(write, tms, tdi, type != SCAN_OUT ?
+						 &tdo : NULL, xvc_type);
 		if (err != ERROR_OK)
 			goto out_err;
 		left -= write;
@@ -285,7 +413,7 @@ static int xlnx_pcie_xvc_execute_scan(struct jtag_command *cmd)
 	free(buf);
 
 	if (tap_get_state() != tap_get_end_state())
-		err = xlnx_pcie_xvc_execute_statemove(1);
+		err = xlnx_xvc_execute_statemove(1, xvc_type);
 
 	return err;
 
@@ -294,19 +422,14 @@ out_err:
 	return err;
 }
 
-static void xlnx_pcie_xvc_execute_reset(struct jtag_command *cmd)
-{
-	LOG_DEBUG("reset trst: %i srst: %i", cmd->cmd.reset->trst,
-		  cmd->cmd.reset->srst);
-}
-
-static void xlnx_pcie_xvc_execute_sleep(struct jtag_command *cmd)
+static void xlnx_xvc_execute_sleep(struct jtag_command *cmd)
 {
 	LOG_DEBUG("sleep %" PRIu32 "", cmd->cmd.sleep->us);
 	usleep(cmd->cmd.sleep->us);
 }
 
-static int xlnx_pcie_xvc_execute_tms(struct jtag_command *cmd)
+static int xlnx_xvc_execute_tms(struct jtag_command *cmd,
+		enum xlnx_xvc_type_t xvc_type)
 {
 	const size_t num_bits = cmd->cmd.tms->num_bits;
 	const uint8_t *bits = cmd->cmd.tms->bits;
@@ -320,7 +443,7 @@ static int xlnx_pcie_xvc_execute_tms(struct jtag_command *cmd)
 	while (left) {
 		write = MIN(XLNX_XVC_MAX_BITS, left);
 		tms = buf_get_u32(bits, 0, write);
-		err = xlnx_pcie_xvc_transact(write, tms, 0, NULL);
+		err = xlnx_xvc_transact(write, tms, 0, NULL, xvc_type);
 		if (err != ERROR_OK)
 			return err;
 		left -= write;
@@ -330,29 +453,30 @@ static int xlnx_pcie_xvc_execute_tms(struct jtag_command *cmd)
 	return ERROR_OK;
 }
 
-static int xlnx_pcie_xvc_execute_command(struct jtag_command *cmd)
+static int xlnx_xvc_execute_command(struct jtag_command *cmd,
+		enum xlnx_xvc_type_t xvc_type)
 {
 	LOG_DEBUG("%s: cmd->type: %u", __func__, cmd->type);
 	switch (cmd->type) {
 	case JTAG_STABLECLOCKS:
-		return xlnx_pcie_xvc_execute_stableclocks(cmd);
+		return xlnx_xvc_execute_stableclocks(cmd, xvc_type);
 	case JTAG_RUNTEST:
-		return xlnx_pcie_xvc_execute_runtest(cmd);
+		return xlnx_xvc_execute_runtest(cmd, xvc_type);
 	case JTAG_TLR_RESET:
 		tap_set_end_state(cmd->cmd.statemove->end_state);
-		return xlnx_pcie_xvc_execute_statemove(0);
+		return xlnx_xvc_execute_statemove(0, xvc_type);
 	case JTAG_PATHMOVE:
-		return xlnx_pcie_xvc_execute_pathmove(cmd);
+		return xlnx_xvc_execute_pathmove(cmd, xvc_type);
 	case JTAG_SCAN:
-		return xlnx_pcie_xvc_execute_scan(cmd);
+		return xlnx_xvc_execute_scan(cmd, xvc_type);
 	case JTAG_RESET:
-		xlnx_pcie_xvc_execute_reset(cmd);
+		LOG_INFO("WARN: XVC driver has no reset.");
 		break;
 	case JTAG_SLEEP:
-		xlnx_pcie_xvc_execute_sleep(cmd);
+		xlnx_xvc_execute_sleep(cmd);
 		break;
 	case JTAG_TMS:
-		return xlnx_pcie_xvc_execute_tms(cmd);
+		return xlnx_xvc_execute_tms(cmd, xvc_type);
 	default:
 		LOG_ERROR("BUG: Unknown JTAG command type encountered.");
 		return ERROR_JTAG_QUEUE_FAILED;
@@ -361,13 +485,14 @@ static int xlnx_pcie_xvc_execute_command(struct jtag_command *cmd)
 	return ERROR_OK;
 }
 
-static int xlnx_pcie_xvc_execute_queue(struct jtag_command *cmd_queue)
+static int xlnx_xvc_execute_queue(struct jtag_command *cmd_queue,
+		enum xlnx_xvc_type_t xvc_type)
 {
 	struct jtag_command *cmd = cmd_queue;
 	int ret;
 
 	while (cmd) {
-		ret = xlnx_pcie_xvc_execute_command(cmd);
+		ret = xlnx_xvc_execute_command(cmd, xvc_type);
 
 		if (ret != ERROR_OK)
 			return ret;
@@ -378,6 +503,15 @@ static int xlnx_pcie_xvc_execute_queue(struct jtag_command *cmd_queue)
 	return ERROR_OK;
 }
 
+static int xlnx_pcie_xvc_execute_queue(struct jtag_command *cmd_queue)
+{
+	return xlnx_xvc_execute_queue(cmd_queue, PCIE);
+}
+
+static int xlnx_axi_xvc_execute_queue(struct jtag_command *cmd_queue)
+{
+	return xlnx_xvc_execute_queue(cmd_queue, AXI);
+}
 
 static int xlnx_pcie_xvc_init(void)
 {
@@ -399,8 +533,8 @@ static int xlnx_pcie_xvc_init(void)
 	 * vendor specific header */
 	xlnx_pcie_xvc->offset = PCIE_EXT_CAP_LST;
 	while (xlnx_pcie_xvc->offset <= PCI_CFG_SPACE_EXP_SIZE - sizeof(cap) &&
-	       xlnx_pcie_xvc->offset >= PCIE_EXT_CAP_LST) {
-		err = xlnx_pcie_xvc_read_reg(XLNX_XVC_EXT_CAP, &cap);
+		   xlnx_pcie_xvc->offset >= PCIE_EXT_CAP_LST) {
+		err = xlnx_pcie_xvc_read_reg(XLNX_PCIE_XVC_EXT_CAP, &cap);
 		if (err != ERROR_OK)
 			return err;
 		LOG_DEBUG("Checking capability at 0x%x; id=0x%04" PRIx32 " version=0x%" PRIx32 " next=0x%" PRIx32,
@@ -409,7 +543,7 @@ static int xlnx_pcie_xvc_init(void)
 			 PCI_EXT_CAP_VER(cap),
 			 PCI_EXT_CAP_NEXT(cap));
 		if (PCI_EXT_CAP_ID(cap) == PCI_EXT_CAP_ID_VNDR) {
-			err = xlnx_pcie_xvc_read_reg(XLNX_XVC_VSEC_HDR, &vh);
+			err = xlnx_pcie_xvc_read_reg(XLNX_PCIE_XVC_VSEC_HDR, &vh);
 			if (err != ERROR_OK)
 				return err;
 			LOG_DEBUG("Checking possible match at 0x%x; id: 0x%" PRIx32 "; rev: 0x%" PRIx32 "; length: 0x%" PRIx32,
@@ -417,14 +551,14 @@ static int xlnx_pcie_xvc_init(void)
 				 PCI_VNDR_HEADER_ID(vh),
 				 PCI_VNDR_HEADER_REV(vh),
 				 PCI_VNDR_HEADER_LEN(vh));
-			if ((PCI_VNDR_HEADER_ID(vh) == XLNX_XVC_VSEC_ID) &&
-			    (PCI_VNDR_HEADER_LEN(vh) == XLNX_XVC_CAP_SIZE))
+			if ((PCI_VNDR_HEADER_ID(vh) == XLNX_PCIE_XVC_VSEC_ID) &&
+				(PCI_VNDR_HEADER_LEN(vh) == XLNX_PCIE_XVC_CAP_SIZE))
 				break;
 		}
 		xlnx_pcie_xvc->offset = PCI_EXT_CAP_NEXT(cap);
 	}
-	if ((xlnx_pcie_xvc->offset > PCI_CFG_SPACE_EXP_SIZE - XLNX_XVC_CAP_SIZE) ||
-	     xlnx_pcie_xvc->offset < PCIE_EXT_CAP_LST) {
+	if ((xlnx_pcie_xvc->offset > PCI_CFG_SPACE_EXP_SIZE - XLNX_PCIE_XVC_CAP_SIZE) ||
+		 xlnx_pcie_xvc->offset < PCIE_EXT_CAP_LST) {
 		close(xlnx_pcie_xvc->fd);
 		return ERROR_JTAG_INIT_FAILED;
 	}
@@ -434,6 +568,44 @@ static int xlnx_pcie_xvc_init(void)
 	return ERROR_OK;
 }
 
+static int xlnx_axi_xvc_init(void)
+{
+	uint64_t baseaddr;
+
+	if (xlnx_axi_xvc->device_addr) {
+		baseaddr = strtoul(xlnx_axi_xvc->device_addr, NULL, 0);
+	} else {
+		LOG_ERROR("Please set device addr.");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	if (xlnx_axi_xvc->device_file) {
+		LOG_INFO("Opening %s for AXI communication", xlnx_axi_xvc->device_file);
+		xlnx_axi_xvc->fd = open(xlnx_axi_xvc->device_file, O_RDWR | O_SYNC);
+	} else {
+		LOG_INFO("Opening /dev/mem for AXI communication");
+		xlnx_axi_xvc->fd = open("/dev/mem", O_RDWR | O_SYNC);
+	}
+
+	if (xlnx_axi_xvc->fd < 0) {
+		LOG_ERROR("Failed to open device file, check permissions.");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	xlnx_axi_xvc->base = mmap(0, XLNX_AXI_XVC_MAX_REG, PROT_READ | PROT_WRITE,
+								  MAP_SHARED, xlnx_axi_xvc->fd, baseaddr);
+	if (xlnx_axi_xvc->base == MAP_FAILED) {
+		LOG_ERROR("mmap() failed, check permissions.");
+		close(xlnx_axi_xvc->fd);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	LOG_INFO("Mapped Xilinx XVC/AXI vaddr %p paddr 0x%" PRIx64,
+			 xlnx_axi_xvc->base, baseaddr);
+
+	return ERROR_OK;
+}
+
 static int xlnx_pcie_xvc_quit(void)
 {
 	int err;
@@ -445,21 +617,55 @@ static int xlnx_pcie_xvc_quit(void)
 	return ERROR_OK;
 }
 
+static int xlnx_axi_xvc_quit(void)
+{
+	int err;
+
+	munmap(xlnx_axi_xvc->base, XLNX_AXI_XVC_MAX_REG);
+	free(xlnx_pcie_xvc->device);
+	free(xlnx_axi_xvc->device_file);
+	free(xlnx_axi_xvc->device_addr);
+
+	err = close(xlnx_axi_xvc->fd);
+	if (err)
+		return err;
+
+	return ERROR_OK;
+}
+
 COMMAND_HANDLER(xlnx_pcie_xvc_handle_config_command)
 {
-	if (CMD_ARGC < 1)
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	/* we can't really free this in a safe manner, so at least
-	 * limit the memory we're leaking by freeing the old one first
-	 * before allocating a new one ...
-	 */
 	free(xlnx_pcie_xvc->device);
 
 	xlnx_pcie_xvc->device = strdup(CMD_ARGV[0]);
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(xlnx_axi_xvc_handle_dev_addr_command)
+{
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	free(xlnx_axi_xvc->device_addr);
+
+	xlnx_axi_xvc->device_addr = strdup(CMD_ARGV[0]);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(xlnx_axi_xvc_handle_dev_file_command)
+{
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	free(xlnx_axi_xvc->device_file);
+
+	xlnx_axi_xvc->device_file = strdup(CMD_ARGV[0]);
+	return ERROR_OK;
+}
+
 static const struct command_registration xlnx_pcie_xvc_subcommand_handlers[] = {
 	{
 		.name = "config",
@@ -482,11 +688,45 @@ static const struct command_registration xlnx_pcie_xvc_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
+static const struct command_registration xlnx_axi_xvc_subcommand_handlers[] = {
+	{
+		.name = "dev_addr",
+		.handler = xlnx_axi_xvc_handle_dev_addr_command,
+		.mode = COMMAND_CONFIG,
+		.help = "Configure XVC/AXI JTAG device memory address",
+		.usage = "addr",
+	},
+	{
+		.name = "dev_file",
+		.handler = xlnx_axi_xvc_handle_dev_file_command,
+		.mode = COMMAND_CONFIG,
+		.help = "Configure XVC/AXI JTAG device file location",
+		.usage = "addr",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration xlnx_axi_xvc_command_handlers[] = {
+	{
+		.name = "xlnx_axi_xvc",
+		.mode = COMMAND_ANY,
+		.help = "perform xlnx_axi_xvc management",
+		.chain = xlnx_axi_xvc_subcommand_handlers,
+		.usage = "",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
 static struct jtag_interface xlnx_pcie_xvc_jtag_ops = {
 	.execute_queue = &xlnx_pcie_xvc_execute_queue,
 };
 
-static int xlnx_pcie_xvc_swd_sequence(const uint8_t *seq, size_t length)
+static struct jtag_interface xlnx_axi_xvc_jtag_ops = {
+	.execute_queue = &xlnx_axi_xvc_execute_queue,
+};
+
+static int xlnx_xvc_swd_sequence(const uint8_t *seq, size_t length,
+		enum xlnx_xvc_type_t xvc_type)
 {
 	size_t left, write;
 	uint32_t send;
@@ -496,7 +736,7 @@ static int xlnx_pcie_xvc_swd_sequence(const uint8_t *seq, size_t length)
 	while (left) {
 		write = MIN(XLNX_XVC_MAX_BITS, left);
 		send = buf_get_u32(seq, 0, write);
-		err = xlnx_pcie_xvc_transact(write, send, 0, NULL);
+		err = xlnx_xvc_transact(write, send, 0, NULL, xvc_type);
 		if (err != ERROR_OK)
 			return err;
 		left -= write;
@@ -506,21 +746,22 @@ static int xlnx_pcie_xvc_swd_sequence(const uint8_t *seq, size_t length)
 	return ERROR_OK;
 }
 
-static int xlnx_pcie_xvc_swd_switch_seq(enum swd_special_seq seq)
+static int xlnx_xvc_swd_switch_seq(enum swd_special_seq seq,
+		enum xlnx_xvc_type_t xvc_type)
 {
 	switch (seq) {
 	case LINE_RESET:
 		LOG_DEBUG("SWD line reset");
-		return xlnx_pcie_xvc_swd_sequence(swd_seq_line_reset,
-						  swd_seq_line_reset_len);
+		return xlnx_xvc_swd_sequence(swd_seq_line_reset,
+						  swd_seq_line_reset_len, xvc_type);
 	case JTAG_TO_SWD:
 		LOG_DEBUG("JTAG-to-SWD");
-		return xlnx_pcie_xvc_swd_sequence(swd_seq_jtag_to_swd,
-						  swd_seq_jtag_to_swd_len);
+		return xlnx_xvc_swd_sequence(swd_seq_jtag_to_swd,
+						  swd_seq_jtag_to_swd_len, xvc_type);
 	case SWD_TO_JTAG:
 		LOG_DEBUG("SWD-to-JTAG");
-		return xlnx_pcie_xvc_swd_sequence(swd_seq_swd_to_jtag,
-						  swd_seq_swd_to_jtag_len);
+		return xlnx_xvc_swd_sequence(swd_seq_swd_to_jtag,
+						  swd_seq_swd_to_jtag_len, xvc_type);
 	default:
 		LOG_ERROR("Sequence %d not supported", seq);
 		return ERROR_FAIL;
@@ -529,19 +770,31 @@ static int xlnx_pcie_xvc_swd_switch_seq(enum swd_special_seq seq)
 	return ERROR_OK;
 }
 
-static int queued_retval;
-
-static void xlnx_pcie_xvc_swd_write_reg(uint8_t cmd, uint32_t value,
-					uint32_t ap_delay_clk);
-
-static void swd_clear_sticky_errors(void)
+static int xlnx_pcie_xvc_swd_switch_seq(enum swd_special_seq seq)
 {
-	xlnx_pcie_xvc_swd_write_reg(swd_cmd(false,  false, DP_ABORT),
-		STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
+	return xlnx_xvc_swd_switch_seq(seq, PCIE);
 }
 
-static void xlnx_pcie_xvc_swd_read_reg(uint8_t cmd, uint32_t *value,
-				       uint32_t ap_delay_clk)
+static int xlnx_axi_xvc_swd_switch_seq(enum swd_special_seq seq)
+{
+	return xlnx_xvc_swd_switch_seq(seq, AXI);
+}
+
+static int queued_retval;
+
+static void xlnx_xvc_swd_write_reg(uint8_t cmd, uint32_t value,
+					uint32_t ap_delay_clk,
+					enum xlnx_xvc_type_t xvc_type);
+
+static void swd_clear_sticky_errors(enum xlnx_xvc_type_t xvc_type)
+{
+	xlnx_xvc_swd_write_reg(swd_cmd(false,  false, DP_ABORT),
+		STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0, xvc_type);
+}
+
+static void xlnx_xvc_swd_read_reg(uint8_t cmd, uint32_t *value,
+					uint32_t ap_delay_clk,
+					enum xlnx_xvc_type_t xvc_type)
 {
 	uint32_t res, ack, rpar;
 	int err;
@@ -550,23 +803,23 @@ static void xlnx_pcie_xvc_swd_read_reg(uint8_t cmd, uint32_t *value,
 
 	cmd |= SWD_CMD_START | SWD_CMD_PARK;
 	/* cmd + ack */
-	err = xlnx_pcie_xvc_transact(12, cmd, 0, &res);
+	err = xlnx_xvc_transact(12, cmd, 0, &res, xvc_type);
 	if (err != ERROR_OK)
 		goto err_out;
 
 	ack = MASK_ACK(res);
 
 	/* read data */
-	err = xlnx_pcie_xvc_transact(32, 0, 0, &res);
+	err = xlnx_xvc_transact(32, 0, 0, &res, xvc_type);
 	if (err != ERROR_OK)
 		goto err_out;
 
 	/* parity + trn */
-	err = xlnx_pcie_xvc_transact(2, 0, 0, &rpar);
+	err = xlnx_xvc_transact(2, 0, 0, &rpar, xvc_type);
 	if (err != ERROR_OK)
 		goto err_out;
 
-	LOG_DEBUG("%s %s %s reg %X = %08"PRIx32,
+	LOG_DEBUG("%s %s %s reg %X = %08" PRIx32,
 		  ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ?
 		  "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
 		  cmd & SWD_CMD_APNDP ? "AP" : "DP",
@@ -583,19 +836,19 @@ static void xlnx_pcie_xvc_swd_read_reg(uint8_t cmd, uint32_t *value,
 		if (value)
 			*value = res;
 		if (cmd & SWD_CMD_APNDP)
-			err = xlnx_pcie_xvc_transact(ap_delay_clk, 0, 0, NULL);
+			err = xlnx_xvc_transact(ap_delay_clk, 0, 0, NULL, xvc_type);
 		queued_retval = err;
 		return;
 	case SWD_ACK_WAIT:
 		LOG_DEBUG_IO("SWD_ACK_WAIT");
-		swd_clear_sticky_errors();
+		swd_clear_sticky_errors(xvc_type);
 		return;
 	case SWD_ACK_FAULT:
 		LOG_DEBUG_IO("SWD_ACK_FAULT");
 		queued_retval = ack;
 		return;
 	default:
-		LOG_DEBUG_IO("No valid acknowledge: ack=%02"PRIx32, ack);
+		LOG_DEBUG_IO("No valid acknowledge: ack=%02" PRIx32, ack);
 		queued_retval = ack;
 		return;
 	}
@@ -603,8 +856,21 @@ err_out:
 	queued_retval = err;
 }
 
-static void xlnx_pcie_xvc_swd_write_reg(uint8_t cmd, uint32_t value,
+static void xlnx_pcie_xvc_swd_read_reg(uint8_t cmd, uint32_t *value,
 					uint32_t ap_delay_clk)
+{
+	xlnx_xvc_swd_read_reg(cmd, value, ap_delay_clk, PCIE);
+}
+
+static void xlnx_axi_xvc_swd_read_reg(uint8_t cmd, uint32_t *value,
+					uint32_t ap_delay_clk)
+{
+	xlnx_xvc_swd_read_reg(cmd, value, ap_delay_clk, AXI);
+}
+
+static void xlnx_xvc_swd_write_reg(uint8_t cmd, uint32_t value,
+					uint32_t ap_delay_clk,
+					enum xlnx_xvc_type_t xvc_type)
 {
 	uint32_t res, ack;
 	int err;
@@ -613,23 +879,23 @@ static void xlnx_pcie_xvc_swd_write_reg(uint8_t cmd, uint32_t value,
 
 	cmd |= SWD_CMD_START | SWD_CMD_PARK;
 	/* cmd + trn + ack */
-	err = xlnx_pcie_xvc_transact(13, cmd, 0, &res);
+	err = xlnx_xvc_transact(13, cmd, 0, &res, xvc_type);
 	if (err != ERROR_OK)
 		goto err_out;
 
 	ack = MASK_ACK(res);
 
 	/* write data */
-	err = xlnx_pcie_xvc_transact(32, value, 0, NULL);
+	err = xlnx_xvc_transact(32, value, 0, NULL, xvc_type);
 	if (err != ERROR_OK)
 		goto err_out;
 
 	/* parity + trn */
-	err = xlnx_pcie_xvc_transact(2, parity_u32(value), 0, NULL);
+	err = xlnx_xvc_transact(2, parity_u32(value), 0, NULL, xvc_type);
 	if (err != ERROR_OK)
 		goto err_out;
 
-	LOG_DEBUG("%s %s %s reg %X = %08"PRIx32,
+	LOG_DEBUG("%s %s %s reg %X = %08" PRIx32,
 		  ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ?
 		  "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
 		  cmd & SWD_CMD_APNDP ? "AP" : "DP",
@@ -640,19 +906,19 @@ static void xlnx_pcie_xvc_swd_write_reg(uint8_t cmd, uint32_t value,
 	switch (ack) {
 	case SWD_ACK_OK:
 		if (cmd & SWD_CMD_APNDP)
-			err = xlnx_pcie_xvc_transact(ap_delay_clk, 0, 0, NULL);
+			err = xlnx_xvc_transact(ap_delay_clk, 0, 0, NULL, xvc_type);
 		queued_retval = err;
 		return;
 	case SWD_ACK_WAIT:
 		LOG_DEBUG_IO("SWD_ACK_WAIT");
-		swd_clear_sticky_errors();
+		swd_clear_sticky_errors(xvc_type);
 		return;
 	case SWD_ACK_FAULT:
 		LOG_DEBUG_IO("SWD_ACK_FAULT");
 		queued_retval = ack;
 		return;
 	default:
-		LOG_DEBUG_IO("No valid acknowledge: ack=%02"PRIx32, ack);
+		LOG_DEBUG_IO("No valid acknowledge: ack=%02" PRIx32, ack);
 		queued_retval = ack;
 		return;
 	}
@@ -661,12 +927,24 @@ err_out:
 	queued_retval = err;
 }
 
-static int xlnx_pcie_xvc_swd_run_queue(void)
+static void xlnx_pcie_xvc_swd_write_reg(uint8_t cmd, uint32_t value,
+					uint32_t ap_delay_clk)
+{
+	xlnx_xvc_swd_write_reg(cmd, value, ap_delay_clk, PCIE);
+}
+
+static void xlnx_axi_xvc_swd_write_reg(uint8_t cmd, uint32_t value,
+					uint32_t ap_delay_clk)
+{
+	xlnx_xvc_swd_write_reg(cmd, value, ap_delay_clk, AXI);
+}
+
+static int xlnx_xvc_swd_run_queue(enum xlnx_xvc_type_t xvc_type)
 {
 	int err;
 
 	/* we want at least 8 idle cycles between each transaction */
-	err = xlnx_pcie_xvc_transact(8, 0, 0, NULL);
+	err = xlnx_xvc_transact(8, 0, 0, NULL, xvc_type);
 	if (err != ERROR_OK)
 		return err;
 
@@ -677,19 +955,37 @@ static int xlnx_pcie_xvc_swd_run_queue(void)
 	return err;
 }
 
-static int xlnx_pcie_xvc_swd_init(void)
+static int xlnx_pcie_xvc_swd_run_queue(void)
+{
+	return xlnx_xvc_swd_run_queue(PCIE);
+}
+
+static int xlnx_axi_xvc_swd_run_queue(void)
+{
+	return xlnx_xvc_swd_run_queue(AXI);
+}
+
+static int xlnx_xvc_swd_init(void)
 {
 	return ERROR_OK;
 }
 
 static const struct swd_driver xlnx_pcie_xvc_swd_ops = {
-	.init = xlnx_pcie_xvc_swd_init,
+	.init = xlnx_xvc_swd_init,
 	.switch_seq = xlnx_pcie_xvc_swd_switch_seq,
 	.read_reg = xlnx_pcie_xvc_swd_read_reg,
 	.write_reg = xlnx_pcie_xvc_swd_write_reg,
 	.run = xlnx_pcie_xvc_swd_run_queue,
 };
 
+static const struct swd_driver xlnx_axi_xvc_swd_ops = {
+	.init = xlnx_xvc_swd_init,
+	.switch_seq = xlnx_axi_xvc_swd_switch_seq,
+	.read_reg = xlnx_axi_xvc_swd_read_reg,
+	.write_reg = xlnx_axi_xvc_swd_write_reg,
+	.run = xlnx_axi_xvc_swd_run_queue,
+};
+
 struct adapter_driver xlnx_pcie_xvc_adapter_driver = {
 	.name = "xlnx_pcie_xvc",
 	.transport_ids = TRANSPORT_JTAG | TRANSPORT_SWD,
@@ -702,3 +998,16 @@ struct adapter_driver xlnx_pcie_xvc_adapter_driver = {
 	.jtag_ops = &xlnx_pcie_xvc_jtag_ops,
 	.swd_ops  = &xlnx_pcie_xvc_swd_ops,
 };
+
+struct adapter_driver xlnx_axi_xvc_adapter_driver = {
+	.name = "xlnx_axi_xvc",
+	.transport_ids = TRANSPORT_JTAG | TRANSPORT_SWD,
+	.transport_preferred_id = TRANSPORT_JTAG,
+	.commands = xlnx_axi_xvc_command_handlers,
+
+	.init = &xlnx_axi_xvc_init,
+	.quit = &xlnx_axi_xvc_quit,
+
+	.jtag_ops = &xlnx_axi_xvc_jtag_ops,
+	.swd_ops  = &xlnx_axi_xvc_swd_ops,
+};

src/jtag/interface.h

@@ -411,6 +411,7 @@ extern struct adapter_driver usbprog_adapter_driver;
 extern struct adapter_driver vdebug_adapter_driver;
 extern struct adapter_driver vsllink_adapter_driver;
 extern struct adapter_driver xds110_adapter_driver;
+extern struct adapter_driver xlnx_axi_xvc_adapter_driver;
 extern struct adapter_driver xlnx_pcie_xvc_adapter_driver;
 
 #endif /* OPENOCD_JTAG_INTERFACE_H */

src/jtag/interfaces.c

@@ -155,8 +155,9 @@ struct adapter_driver *adapter_drivers[] = {
 #if BUILD_XDS110 == 1
 		&xds110_adapter_driver,
 #endif
-#if BUILD_XLNX_PCIE_XVC == 1
-		&xlnx_pcie_xvc_adapter_driver,
+#if BUILD_XLNX_XVC == 1
+	&xlnx_pcie_xvc_adapter_driver,
+	&xlnx_axi_xvc_adapter_driver,
 #endif
 
 		NULL,

src/jtag/startup.tcl

@@ -413,6 +413,12 @@ proc xlnx_pcie_xvc_config args {
 	eval xlnx_pcie_xvc config $args
 }
 
+lappend _telnet_autocomplete_skip xlnx_axi_xvc_config
+proc xlnx_axi_xvc_config args {
+	echo "DEPRECATED! use 'xlnx_axi_xvc config' not 'xlnx_axi_xvc_config'"
+	eval xlnx_axi_xvc config $args
+}
+
 lappend _telnet_autocomplete_skip ulink_download_firmware
 proc ulink_download_firmware args {
 	echo "DEPRECATED! use 'ulink download_firmware' not 'ulink_download_firmware'"

tcl/board/microchip/pic64gx-curiosity-kit.cfg

@@ -0,0 +1,10 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+#
+# Microchip RISC-V board
+#
+# https://www.microchip.com/en-us/products/microprocessors/64-bit-mpus/pic64gx
+#
+adapter speed 6000
+
+source [find interface/microchip/embedded_flashpro5.cfg]
+source [find target/microchip/pic64gx.cfg]

tcl/board/st/stm32mp135f-dk.cfg

@@ -7,6 +7,6 @@ source [find interface/stlink.cfg]
 
 transport select swd
 
-source [find target/stm32mp13x.cfg]
+source [find target/st/stm32mp13x.cfg]
 
 reset_config srst_only

tcl/board/st/stm32mp157f-dk2.cfg

@@ -3,11 +3,12 @@
 # board MB1272B
 # http://www.st.com/en/evaluation-tools/stm32mp157a-dk1.html
 # http://www.st.com/en/evaluation-tools/stm32mp157c-dk2.html
+# http://www.st.com/en/evaluation-tools/stm32mp157f-dk2.html
 
 source [find interface/stlink.cfg]
 
 transport select swd
 
-source [find target/stm32mp15x.cfg]
+source [find target/st/stm32mp15x.cfg]
 
 reset_config srst_only

tcl/board/st/stm32mp235f-dk.cfg

@@ -0,0 +1,12 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# MB1605 with stm32mp23x
+# https://www.st.com/en/evaluation-tools/stm32mp257f-dk.html
+
+source [find interface/stlink.cfg]
+
+transport select swd
+
+source [find target/st/stm32mp23x.cfg]
+
+reset_config srst_only

tcl/board/st/stm32mp257f-dk.cfg

@@ -0,0 +1,12 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# MB1605
+# https://www.st.com/en/evaluation-tools/stm32mp257f-dk.html
+
+source [find interface/stlink.cfg]
+
+transport select swd
+
+source [find target/st/stm32mp25x.cfg]
+
+reset_config srst_only

tcl/file_renaming.cfg

@@ -17,6 +17,8 @@ set _file_renaming {
 	board/nordic_nrf51822_mkit.cfg	board/nordic/nrf51822-mkit.cfg
 	board/nordic_nrf51_dk.cfg		board/nordic/nrf51-dk.cfg
 	board/nordic_nrf52_dk.cfg		board/nordic/nrf52-dk.cfg
+	board/stm32mp13x_dk.cfg			board/st/stm32mp135f-dk.cfg
+	board/stm32mp15x_dk2.cfg		board/st/stm32mp157f-dk2.cfg
 	target/nrf51.cfg	target/nordic/nrf51.cfg
 	target/nrf52.cfg	target/nordic/nrf52.cfg
 	target/nrf53.cfg	target/nordic/nrf53.cfg

tcl/interface/microchip/embedded_flashpro5.cfg

@@ -0,0 +1,40 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+#
+# Embedded FlashPro5
+#
+# https://www.microchip.com/en-us/development-tool/flashpro5
+#
+
+adapter driver ftdi
+
+# vidpid 1514:2008 = embedded flashpro5
+# vidpid 1514:200a = pic64gx
+ftdi vid_pid 0x1514 0x2008 0x1514 0x200a
+
+# That FTDI has 4 channels (channel 0 and 1 are MPSSE-capable, 2 and 3 are bitbang
+ftdi channel 0
+
+# Initial Layout - data[0..15] direction[0..15]
+ftdi layout_init 0x0018 0xfdfb
+#     Signal        Data    Direction       Notes
+# AD0 TCK           0       1 (out)         Port A TCK
+# AD1 TDI           0       1 (out)         Port A TDI
+# AD2 TDO           0       0 (in)          PORT A TDO
+# AD3 TMS           1       1 (out)         Port A TMS
+# AD4 GPIOL0        1       1 (out)         Port A TRST
+# AD5 GPIOL1        0       1 (out)         (unused)
+# AD6 GPIOL2        0       1 (out)         (unused)
+# AD7 GPIOL3        0       1 (out)         (unused)
+
+# BD0 TCK           0       1 (out)         FTDI_UART_B_TXD
+# BD1 TDI           0       0 (in)          FTDI_UART_B_RXD
+# BD2 TDO           0       1 (out)         (unused)
+# BD3 TMS           0       1 (out)         (unused)
+# BD4 GPIOL0        0       1 (out)         (unused)
+# BD5 GPIOL1        0       1 (out)         (unused)
+# BD6 GPIOL2        0       1 (out)         (unused)
+# BD7 GPIOL2        0       1 (out)         (unused)
+
+# Signals definition
+ftdi layout_signal nTRST -data 0x0010 -oe 0x0010

tcl/target/microchip/mpfs.cfg

@@ -0,0 +1,75 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+#
+# Target: MPFS PolarFire SoC-series processors by Microchip Technologies
+#
+# https://www.microchip.com/en-us/products/fpgas-and-plds/system-on-chip-fpgas/polarfire-soc-fpgas
+#
+
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME mpfs
+}
+
+# Process COREID variable
+if { ![exists COREID] } {
+	set COREID -1
+}
+
+transport select jtag
+
+# PolarFire SoC (MPFS) hart id to name lookup table
+array set hart_names {
+	0	e51
+	1	u54_1
+	2	u54_2
+	3	u54_3
+	4	u54_4
+}
+
+# MPFS devices table
+set mpfs_cpu_tap_info {
+	MPFS025  0x0f8531cf
+	MPFS095  0x0f8181cf
+	MPFS160  0x0f8191cf
+	MPFS250  0x0f81a1cf
+	MPFS460  0x0f81b1cf
+	RTPFS160 0x0f8991cf
+	RTPFS460 0x0f89b1cf
+}
+
+proc expected_ids {tap_list} {
+	set str ""
+	dict for {key value} $tap_list {
+		append str "-expected-id" " " $value " "
+	}
+
+	return $str
+}
+
+set irlen 8
+set expected_ids [expected_ids $mpfs_cpu_tap_info]
+eval jtag newtap $_CHIPNAME cpu -irlen $irlen $expected_ids -ignore-version
+
+if {$COREID == -1} {
+	# Single debug connection to all HART's
+	set _TARGETNAME_0 $_CHIPNAME.$hart_names(0)
+	set _TARGETNAME_1 $_CHIPNAME.$hart_names(1)
+	set _TARGETNAME_2 $_CHIPNAME.$hart_names(2)
+	set _TARGETNAME_3 $_CHIPNAME.$hart_names(3)
+	set _TARGETNAME_4 $_CHIPNAME.$hart_names(4)
+
+	target create $_TARGETNAME_0 riscv -chain-position $_CHIPNAME.cpu -coreid 0 -rtos hwthread
+	target create $_TARGETNAME_1 riscv -chain-position $_CHIPNAME.cpu -coreid 1 -rtos hwthread
+	target create $_TARGETNAME_2 riscv -chain-position $_CHIPNAME.cpu -coreid 2 -rtos hwthread
+	target create $_TARGETNAME_3 riscv -chain-position $_CHIPNAME.cpu -coreid 3 -rtos hwthread
+	target create $_TARGETNAME_4 riscv -chain-position $_CHIPNAME.cpu -coreid 4 -rtos hwthread
+	target smp $_TARGETNAME_0 $_TARGETNAME_1 $_TARGETNAME_2 $_TARGETNAME_3 $_TARGETNAME_4
+} else {
+	# Debug connection to a specific hart
+	set _TARGETNAME_0 $_CHIPNAME.$hart_names($COREID)
+	target create $_TARGETNAME_0 riscv -chain-position $_CHIPNAME.cpu -coreid $COREID
+}
+
+# Only TRSTn supported
+reset_config trst_only

tcl/target/microchip/pic64gx.cfg

@@ -0,0 +1,69 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+#
+# Target: Pic64gx processor by Microchip Technologies
+#
+# https://www.microchip.com/en-us/products/microprocessors/64-bit-mpus/pic64gx
+#
+
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME pic64gx
+}
+
+# Process COREID variable
+if {![exists COREID]} {
+	set COREID -1
+}
+
+transport select jtag
+
+# PIC64GX hart id to name lookup table
+array set hart_names {
+	0	e51
+	1	u54_1
+	2	u54_2
+	3	u54_3
+	4	u54_4
+}
+
+# PIC64GX table
+set pic64gx_tap_info {
+	PIC64GX1000 0x0f8531cf
+}
+
+proc expected_ids {tap_list} {
+	set str ""
+	dict for {key value} $tap_list {
+		append str "-expected-id" " " $value " "
+	}
+
+	return $str
+}
+
+set irlen 8
+set expected_ids [expected_ids $pic64gx_tap_info]
+eval jtag newtap $_CHIPNAME cpu -irlen $irlen $expected_ids -ignore-version
+
+if {$COREID == -1} {
+	# Single debug connection to all harts
+	set _TARGETNAME_0 $_CHIPNAME.$hart_names(0)
+	set _TARGETNAME_1 $_CHIPNAME.$hart_names(1)
+	set _TARGETNAME_2 $_CHIPNAME.$hart_names(2)
+	set _TARGETNAME_3 $_CHIPNAME.$hart_names(3)
+	set _TARGETNAME_4 $_CHIPNAME.$hart_names(4)
+
+	target create $_TARGETNAME_0 riscv -chain-position $_CHIPNAME.cpu -coreid 0 -rtos hwthread
+	target create $_TARGETNAME_1 riscv -chain-position $_CHIPNAME.cpu -coreid 1 -rtos hwthread
+	target create $_TARGETNAME_2 riscv -chain-position $_CHIPNAME.cpu -coreid 2 -rtos hwthread
+	target create $_TARGETNAME_3 riscv -chain-position $_CHIPNAME.cpu -coreid 3 -rtos hwthread
+	target create $_TARGETNAME_4 riscv -chain-position $_CHIPNAME.cpu -coreid 4 -rtos hwthread
+	target smp $_TARGETNAME_0 $_TARGETNAME_1 $_TARGETNAME_2 $_TARGETNAME_3 $_TARGETNAME_4
+} else {
+	# Debug connection to a specific hart
+	set _TARGETNAME_0 $_CHIPNAME.$hart_names($COREID)
+	target create $_TARGETNAME_0 riscv -chain-position $_CHIPNAME.cpu -coreid $COREID
+}
+
+# Only TRSTn supported
+reset_config trst_only

tcl/target/st/stm32mp13x.cfg

@@ -46,7 +46,7 @@ dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.tap -ignore-syspwrupack
 # NOTE: do not change the order of target create
 target create $_CHIPNAME.ap1 mem_ap -dap $_CHIPNAME.dap -ap-num 1
 target create $_CHIPNAME.axi mem_ap -dap $_CHIPNAME.dap -ap-num 0
-target create $_CHIPNAME.cpu cortex_a -dap $_CHIPNAME.dap -ap-num 1 -coreid 0 -dbgbase 0xE00D0000
+target create $_CHIPNAME.cpu cortex_a -dap $_CHIPNAME.dap -ap-num 1 -coreid 0 -dbgbase 0xE00D0000 -defer-examine
 
 $_CHIPNAME.cpu cortex_a maskisr on
 $_CHIPNAME.cpu cortex_a dacrfixup on
@@ -76,27 +76,59 @@ proc axi_nsecure {} {
 
 axi_secure
 
-proc dbgmcu_enable_debug {} {
+# mmw with target selection
+proc target_mmw {target reg setbits clearbits} {
+	set val [eval $target read_memory $reg 32 1]
+	set val [expr {($val & ~$clearbits) | $setbits}]
+	eval $target mww $reg $val
+}
+
+lappend _telnet_autocomplete_skip _enable_debug
+# Uses AP1
+proc _enable_debug {} {
 	# keep clock enabled in low-power
-	## catch {$::_CHIPNAME.ap1 mww 0xe0081004 0x00000004}
+	catch {$::_CHIPNAME.ap1 mww 0xe0081004 0x00000004}
 	# freeze watchdog 1 and 2 on core halted
 	catch {$::_CHIPNAME.ap1 mww 0xe008102c 0x00000004}
 	catch {$::_CHIPNAME.ap1 mww 0xe008104c 0x00000008}
 }
 
-proc toggle_cpu_dbg_claim0 {} {
-	# toggle CPU0 DBG_CLAIM[0]
-	$::_CHIPNAME.ap1 mww 0xe00d0fa0 1
-	$::_CHIPNAME.ap1 mww 0xe00d0fa4 1
+lappend _telnet_autocomplete_skip _handshake_with_wrapper
+# Uses AP1
+proc _handshake_with_wrapper { halt } {
+	set dbgmcu_cr 0
+	catch {set dbgmcu_cr [eval $::_CHIPNAME.ap1 read_memory 0xe0081004 32 1]}
+	if {[expr {($dbgmcu_cr & 0x07) == 0x00}]} {
+		echo "\nWARNING: FSBL wrapper not detected. Board in dev boot mode?\n"
+		return
+	}
+
+	if { $halt } {
+		$::_CHIPNAME.ap1 arp_halt
+		$::_CHIPNAME.ap1 mww 0xe00d0300 0
+		target_mmw $::_CHIPNAME.ap1 0xe00d0088 0x00004000 0
+	}
+
+	$::_CHIPNAME.ap1 mww 0xe0081004 0x7
 }
 
 # FIXME: most of handlers below will be removed once reset framework get merged
+$_CHIPNAME.ap1 configure -event reset-assert-post {
+	adapter assert srst
+}
+
 $_CHIPNAME.ap1 configure -event reset-deassert-pre {
 	adapter deassert srst deassert trst
-	catch {dap init}
-	catch {$::_CHIPNAME.dap apid 1}
+	$::_CHIPNAME.ap1 arp_examine
+	_handshake_with_wrapper $halt
+	_enable_debug
+	$::_CHIPNAME.cpu arp_examine
+	if { $halt } {
+		$::_CHIPNAME.cpu arp_halt
+	}
+}
+
+$_CHIPNAME.ap1 configure -event examine-end {
+	_enable_debug
+	$::_CHIPNAME.cpu arp_examine
 }
-$_CHIPNAME.cpu configure -event reset-deassert-pre  {$::_CHIPNAME.cpu arp_examine}
-$_CHIPNAME.cpu configure -event reset-deassert-post {toggle_cpu_dbg_claim0; dbgmcu_enable_debug}
-$_CHIPNAME.ap1 configure -event examine-start       {dap init}
-$_CHIPNAME.ap1 configure -event examine-end         {dbgmcu_enable_debug}

tcl/target/st/stm32mp15x.cfg

@@ -18,6 +18,17 @@ if { [info exists CHIPNAME] } {
 	set _CHIPNAME stm32mp15x
 }
 
+# Set to 0 to prevent CPU examine. Default examine them
+if { ! [info exists EN_CA7_0] } {
+	set EN_CA7_0 1
+}
+if { ! [info exists EN_CA7_1] } {
+	set EN_CA7_1 1
+}
+if { ! [info exists EN_CM4] } {
+	set EN_CM4   1
+}
+
 if { [info exists CPUTAPID] } {
 	set _CPUTAPID $CPUTAPID
 } else {
@@ -42,20 +53,21 @@ if { [using_jtag] } {
 
 dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.tap -ignore-syspwrupack
 
-# FIXME: Cortex-M code requires target accessible during reset, but this is not possible in STM32MP1
-# so defer-examine it until the reset framework get merged
 # NOTE: keep ap-num and dbgbase to speed-up examine after reset
 # NOTE: do not change the order of target create
 target create $_CHIPNAME.ap1 mem_ap -dap $_CHIPNAME.dap -ap-num 1
 target create $_CHIPNAME.ap2 mem_ap -dap $_CHIPNAME.dap -ap-num 2
 target create $_CHIPNAME.axi mem_ap -dap $_CHIPNAME.dap -ap-num 0
-target create $_CHIPNAME.cpu0 cortex_a -dap $_CHIPNAME.dap -ap-num 1 -coreid 0 -dbgbase 0xE00D0000
-target create $_CHIPNAME.cpu1 cortex_a -dap $_CHIPNAME.dap -ap-num 1 -coreid 1 -dbgbase 0xE00D2000
+target create $_CHIPNAME.cpu0 cortex_a -dap $_CHIPNAME.dap -ap-num 1 -coreid 0 -dbgbase 0xE00D0000 -defer-examine
+target create $_CHIPNAME.cpu1 cortex_a -dap $_CHIPNAME.dap -ap-num 1 -coreid 1 -dbgbase 0xE00D2000 -defer-examine
 target create $_CHIPNAME.cm4 cortex_m -dap $_CHIPNAME.dap -ap-num 2 -defer-examine
 
 targets $_CHIPNAME.cpu0
 
 target smp $_CHIPNAME.cpu0 $_CHIPNAME.cpu1
+$_CHIPNAME.cpu0 configure -rtos hwthread
+$_CHIPNAME.cpu1 configure -rtos hwthread
+
 $_CHIPNAME.cpu0 cortex_a maskisr on
 $_CHIPNAME.cpu1 cortex_a maskisr on
 $_CHIPNAME.cpu0 cortex_a dacrfixup on
@@ -96,7 +108,16 @@ proc axi_nsecure {} {
 
 axi_secure
 
-proc dbgmcu_enable_debug {} {
+# mmw with target selection
+proc target_mmw {target reg setbits clearbits} {
+	set val [eval $target read_memory $reg 32 1]
+	set val [expr {($val & ~$clearbits) | $setbits}]
+	eval $target mww $reg $val
+}
+
+lappend _telnet_autocomplete_skip _enable_debug
+# Uses AP1
+proc _enable_debug {} {
 	# set debug enable bits in DBGMCU_CR to get ap2 and cm4 visible
 	catch {$::_CHIPNAME.ap1 mww 0xe0081004 0x00000007}
 	# freeze watchdog 1 and 2 on cores halted
@@ -104,30 +125,97 @@ proc dbgmcu_enable_debug {} {
 	catch {$::_CHIPNAME.ap1 mww 0xe008104c 0x00000008}
 }
 
-proc toggle_cpu0_dbg_claim0 {} {
-	# toggle CPU0 DBG_CLAIM[0]
-	$::_CHIPNAME.ap1 mww 0xe00d0fa0 1
-	$::_CHIPNAME.ap1 mww 0xe00d0fa4 1
+lappend _telnet_autocomplete_skip _handshake_with_wrapper
+# Uses AP1
+proc _handshake_with_wrapper { halt } {
+	set dbgmcu_cr 0
+	catch {set dbgmcu_cr [eval $::_CHIPNAME.ap1 read_memory 0xe0081004 32 1]}
+	if {[expr {($dbgmcu_cr & 0x07) == 0x00}]} {
+		echo "\nWARNING: FSBL wrapper not detected. Board in dev boot mode?\n"
+		return
+	}
+
+	if { $halt }  {
+		$::_CHIPNAME.ap1 arp_halt
+		if { $::EN_CA7_0 } {
+			$::_CHIPNAME.ap1 arp_halt
+			$::_CHIPNAME.ap1 mww 0xe00d0300 0
+			target_mmw $::_CHIPNAME.ap1 0xe00d0088 0x00004000 0
+		}
+	}
+
+	$::_CHIPNAME.ap1 mww 0xe0081004 0x7
 }
 
-proc detect_cpu1 {} {
+lappend _telnet_autocomplete_skip _detect_cpu1
+# Uses AP1
+proc _detect_cpu1 {} {
+	if { !$::EN_CA7_1 } {
+		return
+	}
+
 	set cpu1_prsr [$::_CHIPNAME.ap1 read_memory 0xE00D2314 32 1]
 	set dual_core [expr {$cpu1_prsr & 1}]
-	if {! $dual_core} {$::_CHIPNAME.cpu1 configure -defer-examine}
+	if { !$dual_core } {
+		set ::EN_CA7_1 0
+	}
 }
 
-proc rcc_enable_traceclk {} {
+lappend _telnet_autocomplete_skip _rcc_enable_traceclk
+proc _rcc_enable_traceclk {} {
 	$::_CHIPNAME.ap2 mww 0x5000080c 0x301
 }
 
 # FIXME: most of handler below will be removed once reset framework get merged
-$_CHIPNAME.ap1  configure -event reset-deassert-pre  {adapter deassert srst deassert trst;catch {dap init};catch {$::_CHIPNAME.dap apid 1}}
-$_CHIPNAME.ap2  configure -event reset-deassert-pre  {dbgmcu_enable_debug;rcc_enable_traceclk}
-$_CHIPNAME.cpu0 configure -event reset-deassert-pre  {$::_CHIPNAME.cpu0 arp_examine}
-$_CHIPNAME.cpu1 configure -event reset-deassert-pre  {$::_CHIPNAME.cpu1 arp_examine allow-defer}
-$_CHIPNAME.cpu0 configure -event reset-deassert-post {toggle_cpu0_dbg_claim0}
-$_CHIPNAME.cm4  configure -event reset-deassert-post {$::_CHIPNAME.cm4 arp_examine;if {[$::_CHIPNAME.ap2 curstate] == "halted"} {$::_CHIPNAME.cm4 arp_poll;$::_CHIPNAME.cm4 arp_poll;$::_CHIPNAME.cm4 arp_halt}}
-$_CHIPNAME.ap1  configure -event examine-start       {dap init}
-$_CHIPNAME.ap2  configure -event examine-start       {dbgmcu_enable_debug}
-$_CHIPNAME.cpu0 configure -event examine-end         {detect_cpu1}
-$_CHIPNAME.ap2  configure -event examine-end         {rcc_enable_traceclk;$::_CHIPNAME.cm4 arp_examine}
+$_CHIPNAME.cm4  configure -event reset-assert { }
+
+$_CHIPNAME.ap1  configure -event reset-assert-post {
+	adapter assert srst
+}
+
+$_CHIPNAME.ap1  configure -event reset-deassert-pre {
+	adapter deassert srst deassert trst
+	$::_CHIPNAME.ap1 arp_examine
+	_handshake_with_wrapper $halt
+	if { $::EN_CA7_0 } {
+		$::_CHIPNAME.cpu0 arp_examine
+		if { $halt } {
+			$::_CHIPNAME.cpu0 arp_halt
+		}
+	}
+	if { $::EN_CA7_1 } {
+		$::_CHIPNAME.cpu1 arp_examine
+		if { $halt } {
+			$::_CHIPNAME.cpu1 arp_halt
+		}
+	}
+	_enable_debug
+}
+
+$_CHIPNAME.ap2  configure -event reset-deassert-pre {
+	_rcc_enable_traceclk
+	if { $::EN_CM4 } {
+		$::_CHIPNAME.cm4 arp_examine
+		if { $halt } {
+			$::_CHIPNAME.cm4 arp_halt
+		}
+	}
+}
+
+$_CHIPNAME.ap1 configure -event examine-end {
+	_enable_debug
+	_detect_cpu1
+	if { $::EN_CA7_0 } {
+		$::_CHIPNAME.cpu0 arp_examine
+	}
+	if { $::EN_CA7_1 } {
+		$::_CHIPNAME.cpu1 arp_examine
+	}
+}
+
+$_CHIPNAME.ap2  configure -event examine-end {
+	_rcc_enable_traceclk
+	if { $::EN_CM4 } {
+		$::_CHIPNAME.cm4 arp_examine
+	}
+}

tcl/target/st/stm32mp21x.cfg

@@ -0,0 +1,222 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# STMicroelectronics STM32MP21x
+# STM32MP21x devices support both JTAG and SWD transports.
+
+# HLA does not support multi-cores nor custom CSW nor AP other than 0
+if { [using_hla] } {
+	echo "ERROR: HLA transport cannot work with this target."
+	shutdown
+}
+
+source [find target/swj-dp.tcl]
+source [find mem_helper.tcl]
+
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME stm32mp21x
+}
+
+# Set to 0 to prevent CPU examine. Default examine them
+if { ! [info exists EN_CA35] } {
+	set EN_CA35 1
+}
+if { ! [info exists EN_CM33] } {
+	set EN_CM33 1
+}
+
+set _ENDIAN little
+
+# jtag scan chain
+if { [info exists CPUTAPID] } {
+	set _CPUTAPID $CPUTAPID
+} else {
+	if { [using_jtag] } {
+		set _CPUTAPID 0x6ba00477
+	} {
+		set _CPUTAPID 0x6ba02477
+	}
+}
+
+# Chip Level TAP Controller, only in jtag mode
+if { [info exists CLCTAPID] } {
+	set _CLCTAPID $CLCTAPID
+} else {
+	set _CLCTAPID 0x16503041
+}
+
+swj_newdap $_CHIPNAME tap -expected-id $_CPUTAPID -irlen 4 -ircapture 0x01 -irmask 0x0f
+if { [using_jtag] } {
+	swj_newdap $_CHIPNAME.clc tap -expected-id $_CLCTAPID -irlen 5
+}
+
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.tap
+
+# define AXI & APB Memory Access Ports
+# NOTE: do not change the order of target create
+target create $_CHIPNAME.ap0 mem_ap -dap $_CHIPNAME.dap -ap-num 0
+target create $_CHIPNAME.ap1 mem_ap -dap $_CHIPNAME.dap -ap-num 1
+target create $_CHIPNAME.axi mem_ap -dap $_CHIPNAME.dap -ap-num 2
+target create $_CHIPNAME.ap3 mem_ap -dap $_CHIPNAME.dap -ap-num 3 -defer-examine
+
+# define the Cortex-A35
+cti create $_CHIPNAME.cti.a35 -dap $_CHIPNAME.dap -ap-num 1 -baseaddr 0x80220000
+target create $_CHIPNAME.a35 aarch64 -dap $_CHIPNAME.dap -ap-num 1 -dbgbase 0x80210000 \
+	-cti $_CHIPNAME.cti.a35 -defer-examine
+
+# define the Cortex-M33
+target create $_CHIPNAME.m33 cortex_m -dap $_CHIPNAME.dap -ap-num 3 -defer-examine
+cti create $_CHIPNAME.cti.m33 -dap $_CHIPNAME.dap -ap-num 3 -baseaddr 0xe0042000
+
+# define the system CTIs
+cti create $_CHIPNAME.cti.sys0 -dap $_CHIPNAME.dap -ap-num 1 -baseaddr 0x80080000
+cti create $_CHIPNAME.cti.sys1 -dap $_CHIPNAME.dap -ap-num 1 -baseaddr 0x80090000
+
+swo  create $_CHIPNAME.swo  -dap $_CHIPNAME.dap -ap-num 1 -baseaddr 0x800A0000
+tpiu create $_CHIPNAME.tpiu -dap $_CHIPNAME.dap -ap-num 1 -baseaddr 0x80040000
+
+targets $_CHIPNAME.a35
+
+reset_config srst_pulls_trst
+
+adapter speed 5000
+adapter srst pulse_width 200
+# wait 1 seconds for bootrom
+adapter srst delay 1000
+
+# set CSW for AXI
+$_CHIPNAME.dap apsel 2
+$_CHIPNAME.dap apcsw 0x12800000
+
+# mmw with target selection
+proc target_mmw {target reg setbits clearbits} {
+	set val [eval $target read_memory $reg 32 1]
+	set val [expr {($val & ~$clearbits) | $setbits}]
+	eval $target mww $reg $val
+}
+
+lappend _telnet_autocomplete_skip _enable_debug
+# Uses AP0 and AXI
+proc _enable_debug {} {
+	# Enable DBGMCU clock in RC
+	$::_CHIPNAME.axi mww 0x44200520 0x500
+
+	# set debug enable bits in DBGMCU_CR to get ap3/cm33 visible
+	$::_CHIPNAME.ap0 mww 0x80001004 0x7
+
+	# Freeze watchdogs on CPU halt
+	$::_CHIPNAME.axi mww 0x440a003c 0x00000026
+	$::_CHIPNAME.axi mww 0x440a0040 0x00000038
+}
+
+lappend _telnet_autocomplete_skip _rcc_enable_traceclk
+# Uses AXI
+proc _rcc_enable_traceclk {} {
+	# set bit TRACEEN in RCC_DBGCFGR to clock TPIU
+	target_mmw $::_CHIPNAME.axi 0x44200520 0x200 0
+}
+
+lappend _telnet_autocomplete_skip _handshake_with_wrapper
+# Uses AP0, AP1 and AP3
+proc _handshake_with_wrapper { halt } {
+	set dbgmcu_cr 0
+	catch {set dbgmcu_cr [eval $::_CHIPNAME.ap0 read_memory 0x80001004 32 1]}
+	if {[expr {($dbgmcu_cr & 0x07) == 0x00}]} {
+		echo "\nWARNING: FSBL wrapper not detected. Board in dev boot mode?\n"
+		return
+	}
+
+	if { $halt } {
+		if { $::EN_CA35 } {
+			$::_CHIPNAME.ap1 arp_examine
+			$::_CHIPNAME.ap1 arp_halt
+			$::_CHIPNAME.ap1 mww 0x80210300 0
+			target_mmw $::_CHIPNAME.ap1 0x80210088 0x00004000 0
+		}
+		if { $::EN_CM33 } {
+			$::_CHIPNAME.ap3 arp_examine
+			$::_CHIPNAME.ap3 arp_halt
+			$::_CHIPNAME.ap3 mww 0xe000edf0 0xa05f0001
+		}
+	}
+
+	# alert wrapper that debugger is ready
+	$::_CHIPNAME.ap0 mww 0x80001004 0x07
+}
+
+lappend _telnet_autocomplete_skip _enable_dbgmcu_on_devboot
+# In DEV BOOT the BootROM does not completes the sequence to enable the
+# visibility of DBGMCU on AP0.
+# Write a value in DBGMCU_DBG_AUTH_DEV from CID1.
+# Returns 1 if DEV BOOT is detected
+# Uses AP2 (AXI bus)
+proc _enable_dbgmcu_on_devboot {} {
+	$::_CHIPNAME.axi mww 0x44230004 0
+	set boot_pins [expr {[$::_CHIPNAME.axi read_memory 0x44230000 32 1] & 0xf}]
+	if {$boot_pins != 0x3 && $boot_pins != 0xc} {
+		return 0
+	}
+
+	set rifsc_rimc_cr [$::_CHIPNAME.axi read_memory 0x42080c00 32 1]
+	if {$rifsc_rimc_cr != 0x00008710} {
+		echo "RIFSC_RIMC_CR modified, skip activation of DBGMCU"
+		return 1
+	}
+
+	# Enable DBGMCU clock in RC
+	$::_CHIPNAME.axi mww 0x44200520 0x500
+
+	# Change DAP (AXI) CID, write in DBGMCU, set back DAP CID
+	$::_CHIPNAME.axi mww 0x42080c00 0x00008110
+	$::_CHIPNAME.axi mww 0x440A0104 1
+	$::_CHIPNAME.axi mww 0x42080c00 0x00008710
+	return 1
+}
+
+$_CHIPNAME.m33 configure -event reset-assert { }
+
+$_CHIPNAME.axi configure -event reset-assert-post {
+	adapter assert srst
+}
+
+$_CHIPNAME.axi configure -event reset-deassert-pre {
+	adapter deassert srst deassert trst
+	$::_CHIPNAME.axi arp_examine
+	set is_dev_boot [_enable_dbgmcu_on_devboot]
+	if { !$is_dev_boot } {
+		_handshake_with_wrapper $halt
+	}
+	_enable_debug
+	_rcc_enable_traceclk
+	if { $::EN_CA35 } {
+		$::_CHIPNAME.a35 arp_examine
+		if { $halt } {
+			$::_CHIPNAME.a35 arp_halt
+		}
+	}
+	if { $::EN_CM33 } {
+		$::_CHIPNAME.ap3 arp_examine
+		$::_CHIPNAME.m33 arp_examine
+		if { $halt } {
+			$::_CHIPNAME.ap3 arp_halt
+			$::_CHIPNAME.m33 arp_halt
+		}
+	}
+}
+
+$_CHIPNAME.axi configure -event examine-end {
+	set is_dev_boot [_enable_dbgmcu_on_devboot]
+	if { $is_dev_boot } {
+		echo "Dev boot detected"
+	}
+	_enable_debug
+	_rcc_enable_traceclk
+	if { $::EN_CA35 } {
+		$::_CHIPNAME.a35 arp_examine
+	}
+	if { $::EN_CM33 } {
+		$::_CHIPNAME.ap3 arp_examine
+		$::_CHIPNAME.m33 arp_examine
+	}
+}

tcl/target/st/stm32mp23x.cfg

@@ -0,0 +1,215 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# STMicroelectronics STM32MP23x
+# STM32MP23x devices support both JTAG and SWD transports.
+
+# HLA does not support multi-cores nor custom CSW nor AP other than 0
+if { [using_hla] } {
+	echo "ERROR: HLA transport cannot work with this target."
+	shutdown
+}
+
+source [find target/swj-dp.tcl]
+source [find mem_helper.tcl]
+
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME stm32mp23x
+}
+
+# Set to 0 to prevent CPU examine. Default examine them
+if { ! [info exists EN_CA35_0] } {
+	set EN_CA35_0 1
+}
+if { ! [info exists EN_CA35_1] } {
+	set EN_CA35_1 1
+}
+if { ! [info exists EN_CM33] } {
+	set EN_CM33   1
+}
+
+set _ENDIAN little
+
+# jtag scan chain
+if { [info exists CPUTAPID] } {
+	set _CPUTAPID $CPUTAPID
+} else {
+	if { [using_jtag] } {
+		set _CPUTAPID 0x6ba00477
+	} {
+		set _CPUTAPID 0x6ba02477
+	}
+}
+
+# Chip Level TAP Controller, only in jtag mode
+if { [info exists CLCTAPID] } {
+	set _CLCTAPID $CLCTAPID
+} else {
+	set _CLCTAPID 0x16505041
+}
+
+swj_newdap $_CHIPNAME tap -expected-id $_CPUTAPID -irlen 4 -ircapture 0x01 -irmask 0x0f
+if { [using_jtag] } {
+	swj_newdap $_CHIPNAME.clc tap -expected-id $_CLCTAPID -irlen 5
+}
+
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.tap
+
+# define AXI & APB Memory Access Ports
+# NOTE: do not change the order of target create
+target create $_CHIPNAME.ap0 mem_ap -dap $_CHIPNAME.dap -ap-num 0
+target create $_CHIPNAME.axi mem_ap -dap $_CHIPNAME.dap -ap-num 4
+target create $_CHIPNAME.ap8 mem_ap -dap $_CHIPNAME.dap -ap-num 8 -defer-examine
+
+# define the first Cortex-A35
+cti create $_CHIPNAME.cti.a35_0 -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x80220000
+target create $_CHIPNAME.a35_0 aarch64 -dap $_CHIPNAME.dap -ap-num 0 -dbgbase 0x80210000 \
+	-cti $_CHIPNAME.cti.a35_0 -defer-examine
+
+# define the second Cortex-A35
+cti create $_CHIPNAME.cti.a35_1 -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x80320000
+target create $_CHIPNAME.a35_1 aarch64 -dap $_CHIPNAME.dap -ap-num 0 -dbgbase 0x80310000 \
+	-cti $_CHIPNAME.cti.a35_1 -defer-examine
+
+# define the Cortex-M33
+target create $_CHIPNAME.m33 cortex_m -dap $_CHIPNAME.dap -ap-num 8 -defer-examine
+cti create $_CHIPNAME.cti.m33 -dap $_CHIPNAME.dap -ap-num 8 -baseaddr 0xe0042000
+
+# define the system CTIs
+cti create $_CHIPNAME.cti.sys0 -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x80090000
+cti create $_CHIPNAME.cti.sys1 -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x800a0000
+
+swo  create $_CHIPNAME.swo  -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x800b0000
+tpiu create $_CHIPNAME.tpiu -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x80050000
+
+targets $_CHIPNAME.a35_0
+
+target smp $_CHIPNAME.a35_0 $_CHIPNAME.a35_1
+$_CHIPNAME.a35_0 configure -rtos hwthread
+$_CHIPNAME.a35_1 configure -rtos hwthread
+
+reset_config srst_gates_jtag srst_pulls_trst
+
+adapter speed 5000
+adapter srst pulse_width 200
+# wait 1 seconds for bootrom
+adapter srst delay 1000
+
+# set CSW for AXI
+$_CHIPNAME.dap apsel 4
+$_CHIPNAME.dap apcsw 0x12800000
+
+# mmw with target selection
+proc target_mmw {target reg setbits clearbits} {
+	set val [eval $target read_memory $reg 32 1]
+	set val [expr {($val & ~$clearbits) | $setbits}]
+	eval $target mww $reg $val
+}
+
+lappend _telnet_autocomplete_skip _enable_debug
+# Uses AP0 and AXI
+proc _enable_debug {} {
+	# set debug enable bits in DBGMCU_CR to get ap8/cm33 visible
+	$::_CHIPNAME.ap0 mww 0x80010004 0x17
+
+	# Freeze watchdogs on CPU halt
+	$::_CHIPNAME.axi mww 0x4a010008 0x00000000
+	$::_CHIPNAME.axi mww 0x4a01003c 0x00000026
+	$::_CHIPNAME.axi mww 0x4a010040 0x00000038
+	$::_CHIPNAME.axi mww 0x4a010044 0x00000400
+	$::_CHIPNAME.axi mww 0x4a010048 0x00000400
+	$::_CHIPNAME.axi mww 0x4a01004c 0x00000600
+}
+
+lappend _telnet_autocomplete_skip _rcc_enable_traceclk
+# Uses AXI
+proc _rcc_enable_traceclk {} {
+	# set bit TRACEEN in RCC_DBGCFGR to clock TPIU
+	target_mmw $::_CHIPNAME.axi 0x44200520 0x200 0
+}
+
+lappend _telnet_autocomplete_skip _handshake_with_wrapper
+# Uses AP0
+proc _handshake_with_wrapper { halt } {
+	set dbgmcu_cr 0
+	catch {set dbgmcu_cr [eval $::_CHIPNAME.ap0 read_memory 0x80010004 32 1]}
+	if {[expr {($dbgmcu_cr & 0x07) == 0x00}]} {
+		echo "\nWARNING: FSBL wrapper not detected. Board in dev boot mode?\n"
+		return;
+	}
+
+	if { $halt } {
+		if { $::EN_CA35_0 || $::EN_CA35_1 } {
+			$::_CHIPNAME.ap0 arp_examine
+			$::_CHIPNAME.ap0 arp_halt
+		}
+		if { $::EN_CA35_0 } {
+			$::_CHIPNAME.ap0 mww 0x80210300 0
+			target_mmw $::_CHIPNAME.ap0 0x80210088 0x00004000 0
+		}
+		if { $::EN_CA35_1 } {
+			$::_CHIPNAME.ap0 mww 0x80310300 0
+			target_mmw $::_CHIPNAME.ap0 0x80310088 0x00004000 0
+		}
+		if { $::EN_CM33 } {
+			$::_CHIPNAME.ap8 arp_examine
+			$::_CHIPNAME.ap8 arp_halt
+			$::_CHIPNAME.ap8 mww 0xe000edf0 0xa05f0001
+		}
+	}
+
+	# alert wrapper that debugger is ready
+	$::_CHIPNAME.ap0 mww 0x80010004 0x17
+}
+
+$_CHIPNAME.m33 configure -event reset-assert { }
+
+$_CHIPNAME.axi configure -event reset-assert-post {
+	adapter assert srst
+}
+
+$_CHIPNAME.axi configure -event reset-deassert-pre {
+	adapter deassert srst deassert trst
+
+	$::_CHIPNAME.ap0 arp_examine
+	_handshake_with_wrapper $halt
+
+	$::_CHIPNAME.axi arp_examine
+	_enable_debug
+	_rcc_enable_traceclk
+	if { $::EN_CA35_0 } {
+		$::_CHIPNAME.a35_0 arp_examine
+		if { $halt } {
+			$::_CHIPNAME.a35_0 arp_halt
+		}
+	}
+	if { $::EN_CA35_1 } {
+		$::_CHIPNAME.a35_1 arp_examine
+		if { $halt } {
+			$::_CHIPNAME.a35_1 arp_halt
+		}
+	}
+	if { $::EN_CM33 } {
+		$::_CHIPNAME.ap8 arp_examine
+		$::_CHIPNAME.m33 arp_examine
+		if { $halt } {
+			$::_CHIPNAME.m33 arp_halt
+		}
+	}
+}
+
+$_CHIPNAME.axi configure -event examine-end {
+	_enable_debug
+	_rcc_enable_traceclk
+	if { $::EN_CA35_0 } {
+		$::_CHIPNAME.a35_0 arp_examine
+	}
+	if { $::EN_CA35_1 } {
+		$::_CHIPNAME.a35_1 arp_examine
+	}
+	if { $::EN_CM33 } {
+		$::_CHIPNAME.ap8 arp_examine
+		$::_CHIPNAME.m33 arp_examine
+	}
+}

tcl/target/st/stm32mp25x.cfg

@@ -0,0 +1,247 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+# STMicroelectronics STM32MP25x
+# STM32MP25x devices support both JTAG and SWD transports.
+
+# HLA does not support multi-cores nor custom CSW nor AP other than 0
+if { [using_hla] } {
+	echo "ERROR: HLA transport cannot work with this target."
+	shutdown
+}
+
+source [find target/swj-dp.tcl]
+source [find mem_helper.tcl]
+
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME stm32mp25x
+}
+
+# Set to 0 to prevent CPU examine. Default examine them
+if { ! [info exists EN_CA35_0] } {
+	set EN_CA35_0 1
+}
+if { ! [info exists EN_CA35_1] } {
+	set EN_CA35_1 1
+}
+if { ! [info exists EN_CM33] } {
+	set EN_CM33   1
+}
+if { ! [info exists EN_CM0P] } {
+	set EN_CM0P   1
+}
+
+set _ENDIAN little
+
+# jtag scan chain
+if { [info exists CPUTAPID] } {
+	set _CPUTAPID $CPUTAPID
+} else {
+	if { [using_jtag] } {
+		set _CPUTAPID 0x6ba00477
+	} {
+		set _CPUTAPID 0x6ba02477
+	}
+}
+
+# Chip Level TAP Controller, only in jtag mode
+if { [info exists CLCTAPID] } {
+	set _CLCTAPID $CLCTAPID
+} else {
+	set _CLCTAPID 0x16505041
+}
+
+swj_newdap $_CHIPNAME tap -expected-id $_CPUTAPID -irlen 4 -ircapture 0x01 -irmask 0x0f
+if { [using_jtag] } {
+	swj_newdap $_CHIPNAME.clc tap -expected-id $_CLCTAPID -irlen 5
+}
+
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.tap
+
+# define AXI & APB Memory Access Ports
+# NOTE: do not change the order of target create
+target create $_CHIPNAME.ap0 mem_ap -dap $_CHIPNAME.dap -ap-num 0
+target create $_CHIPNAME.axi mem_ap -dap $_CHIPNAME.dap -ap-num 4
+target create $_CHIPNAME.ap2 mem_ap -dap $_CHIPNAME.dap -ap-num 2 -defer-examine
+target create $_CHIPNAME.ap8 mem_ap -dap $_CHIPNAME.dap -ap-num 8 -defer-examine
+
+# define the first Cortex-A35
+cti create $_CHIPNAME.cti.a35_0 -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x80220000
+target create $_CHIPNAME.a35_0 aarch64 -dap $_CHIPNAME.dap -ap-num 0 -dbgbase 0x80210000 \
+	-cti $_CHIPNAME.cti.a35_0 -defer-examine
+
+# define the second Cortex-A35
+cti create $_CHIPNAME.cti.a35_1 -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x80320000
+target create $_CHIPNAME.a35_1 aarch64 -dap $_CHIPNAME.dap -ap-num 0 -dbgbase 0x80310000 \
+	-cti $_CHIPNAME.cti.a35_1 -defer-examine
+
+# define the Cortex-M33
+target create $_CHIPNAME.m33 cortex_m -dap $_CHIPNAME.dap -ap-num 8 -defer-examine
+cti create $_CHIPNAME.cti.m33 -dap $_CHIPNAME.dap -ap-num 8 -baseaddr 0xe0042000
+
+# define the Cortex-M0+
+target create $_CHIPNAME.m0p cortex_m -dap $_CHIPNAME.dap -ap-num 2 -defer-examine
+cti create $_CHIPNAME.cti.m0p -dap $_CHIPNAME.dap -ap-num 2 -baseaddr 0xf0000000
+
+# define the system CTIs
+cti create $_CHIPNAME.cti.sys0 -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x80090000
+cti create $_CHIPNAME.cti.sys1 -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x800a0000
+
+swo  create $_CHIPNAME.swo  -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x800b0000
+tpiu create $_CHIPNAME.tpiu -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0x80050000
+
+targets $_CHIPNAME.a35_0
+
+target smp $_CHIPNAME.a35_0 $_CHIPNAME.a35_1
+$_CHIPNAME.a35_0 configure -rtos hwthread
+$_CHIPNAME.a35_1 configure -rtos hwthread
+
+reset_config srst_gates_jtag srst_pulls_trst
+
+adapter speed 5000
+adapter srst pulse_width 200
+# wait 1 seconds for bootrom
+adapter srst delay 1000
+
+# set CSW for AXI
+$_CHIPNAME.dap apsel 4
+$_CHIPNAME.dap apcsw 0x12800000
+
+# mmw with target selection
+proc target_mmw {target reg setbits clearbits} {
+	set val [eval $target read_memory $reg 32 1]
+	set val [expr {($val & ~$clearbits) | $setbits}]
+	eval $target mww $reg $val
+}
+
+lappend _telnet_autocomplete_skip _enable_ap2_cm0p
+proc _enable_ap2_cm0p {} {
+	# set bits C3LPEN and C3EN in RCC_C3CFGR to enable AP2 and CM0+ clock
+	target_mmw $::_CHIPNAME.axi 0x54200490 6 0
+}
+
+lappend _telnet_autocomplete_skip _enable_debug
+# Uses AP0 and AXI
+proc _enable_debug {} {
+	# set debug enable bits in DBGMCU_CR to get ap2/cm0+ and ap8/cm33 visible
+	# set DBG_SWD_SEL_N bit in DBGMCU_CR to get ap2/cm0+ on main debug interface
+	$::_CHIPNAME.ap0 mww 0x80010004 0x17
+
+	if { $::EN_CM0P } {
+		_enable_ap2_cm0p
+	}
+
+	# Freeze watchdogs on CPU halt
+	$::_CHIPNAME.axi mww 0x4a010008 0x00000000
+	$::_CHIPNAME.axi mww 0x4a01003c 0x00000026
+	$::_CHIPNAME.axi mww 0x4a010040 0x00000038
+	$::_CHIPNAME.axi mww 0x4a010044 0x00000400
+	$::_CHIPNAME.axi mww 0x4a010048 0x00000400
+	$::_CHIPNAME.axi mww 0x4a01004c 0x00000600
+}
+
+lappend _telnet_autocomplete_skip _rcc_enable_traceclk
+# Uses AXI
+proc _rcc_enable_traceclk {} {
+	# set bit TRACEEN in RCC_DBGCFGR to clock TPIU
+	target_mmw $::_CHIPNAME.axi 0x44200520 0x200 0
+}
+
+lappend _telnet_autocomplete_skip _handshake_with_wrapper
+# Uses AP0
+proc _handshake_with_wrapper { halt } {
+	set dbgmcu_cr 0
+	catch {set dbgmcu_cr [eval $::_CHIPNAME.ap0 read_memory 0x80010004 32 1]}
+	if {[expr {($dbgmcu_cr & 0x07) == 0x00}]} {
+		echo "\nWARNING: FSBL wrapper not detected. Board in dev boot mode?\n"
+		return;
+	}
+
+	if { $halt } {
+		if { $::EN_CA35_0 || $::EN_CA35_1 } {
+			$::_CHIPNAME.ap0 arp_examine
+			$::_CHIPNAME.ap0 arp_halt
+		}
+		if { $::EN_CA35_0 } {
+			$::_CHIPNAME.ap0 mww 0x80210300 0
+			target_mmw $::_CHIPNAME.ap0 0x80210088 0x00004000 0
+		}
+		if { $::EN_CA35_1 } {
+			$::_CHIPNAME.ap0 mww 0x80310300 0
+			target_mmw $::_CHIPNAME.ap0 0x80310088 0x00004000 0
+		}
+		if { $::EN_CM33 } {
+			$::_CHIPNAME.ap8 arp_examine
+			$::_CHIPNAME.ap8 arp_halt
+			$::_CHIPNAME.ap8 mww 0xe000edf0 0xa05f0001
+		}
+	}
+
+	# alert wrapper that debugger is ready
+	$::_CHIPNAME.ap0 mww 0x80010004 0x17
+}
+
+$_CHIPNAME.m33 configure -event reset-assert { }
+$_CHIPNAME.m0p configure -event reset-assert { }
+
+$_CHIPNAME.axi configure -event reset-assert-post {
+	adapter assert srst
+}
+
+$_CHIPNAME.axi configure -event reset-deassert-pre {
+	adapter deassert srst deassert trst
+
+	$::_CHIPNAME.ap0 arp_examine
+	_handshake_with_wrapper $halt
+
+	$::_CHIPNAME.axi arp_examine
+	_enable_debug
+	_rcc_enable_traceclk
+	if { $::EN_CA35_0 } {
+		$::_CHIPNAME.a35_0 arp_examine
+		if { $halt } {
+			$::_CHIPNAME.a35_0 arp_halt
+		}
+	}
+	if { $::EN_CA35_1 } {
+		$::_CHIPNAME.a35_1 arp_examine
+		if { $halt } {
+			$::_CHIPNAME.a35_1 arp_halt
+		}
+	}
+	if { $::EN_CM0P } {
+		$::_CHIPNAME.ap2 arp_examine
+		$::_CHIPNAME.m0p arp_examine
+	}
+	if { $::EN_CM33 } {
+		$::_CHIPNAME.ap8 arp_examine
+		$::_CHIPNAME.m33 arp_examine
+		if { $halt } {
+			$::_CHIPNAME.m33 arp_halt
+		}
+	}
+}
+
+$_CHIPNAME.m0p configure -event examine-start {
+	_enable_ap2_cm0p
+}
+
+$_CHIPNAME.axi configure -event examine-end {
+	_enable_debug
+	_rcc_enable_traceclk
+	if { $::EN_CA35_0 } {
+		$::_CHIPNAME.a35_0 arp_examine
+	}
+	if { $::EN_CA35_1 } {
+		$::_CHIPNAME.a35_1 arp_examine
+	}
+	if { $::EN_CM33 } {
+		$::_CHIPNAME.ap8 arp_examine
+		$::_CHIPNAME.m33 arp_examine
+	}
+	if { $::EN_CM0P } {
+		$::_CHIPNAME.ap2 arp_examine
+		$::_CHIPNAME.m0p arp_examine
+	}
+}