From 89e86fd65a59f269ae5e0dd1027a427ced86c93e Mon Sep 17 00:00:00 2001
From: Antonio Borneo <antonio.borneo@st.com>
Date: Fri, 9 Nov 2018 11:24:37 +0100
Subject: [PATCH 3/4] Align to community code for cache coherency and reset
 framework

---
 src/helper/startup.tcl            |   9 +-
 src/jtag/drivers/stlink_usb.c     |  56 +++-
 src/server/gdb_server.c           |  15 +-
 src/target/aarch64.c              |   1 +
 src/target/arm.h                  |   2 +
 src/target/arm11.c                |   1 +
 src/target/arm720t.c              |   1 +
 src/target/arm7tdmi.c             |   1 +
 src/target/arm920t.c              |   1 +
 src/target/arm926ejs.c            |   1 +
 src/target/arm946e.c              |   1 +
 src/target/arm966e.c              |   1 +
 src/target/arm9tdmi.c             |   1 +
 src/target/arm_adi_v5.c           |  23 +-
 src/target/arm_dpm.c              |   2 +-
 src/target/arm_dpm.h              |   1 +
 src/target/armv4_5.c              |  14 +
 src/target/armv7a.h               |   2 -
 src/target/armv8.c                |   5 +
 src/target/cortex_a.c             | 458 ++----------------------------
 src/target/cortex_a.h             |   3 -
 src/target/cortex_m.c             |   1 +
 src/target/fa526.c                |   1 +
 src/target/feroceon.c             |   2 +
 src/target/hla_target.c           |   1 +
 src/target/startup.tcl            |  10 +-
 src/target/stm8.c                 |   6 +
 src/target/target.c               |   7 +
 src/target/target.h               |   7 +
 src/target/target_type.h          |   9 +
 src/target/xscale.c               |   1 +
 tcl/target/stm32mp15x.cfg         |  53 ++--
 tcl/target/stm32mp15x_stpmic1.cfg |  97 +++----
 33 files changed, 265 insertions(+), 529 deletions(-)

diff --git a/src/helper/startup.tcl b/src/helper/startup.tcl
index 4ca2cabc2..2578de930 100644
--- a/src/helper/startup.tcl
+++ b/src/helper/startup.tcl
@@ -12,15 +12,18 @@ proc exit {} {
 
 # All commands are registered with an 'ocd_' prefix, while the "real"
 # command is a wrapper that calls this function.  Its primary purpose is
-# to discard 'handler' command output,
+# to discard 'handler' command output.
+# Due to the two nested proc calls, this wrapper has to explicitly run
+# the wrapped command in the stack frame two levels above.
 proc ocd_bouncer {name args} {
 	set cmd [format "ocd_%s" $name]
 	set type [eval ocd_command type $cmd $args]
 	set errcode error
+	set skiplevel [expr [eval info level] > 1 ? 2 : 1]
 	if {$type == "native"} {
-		return [eval $cmd $args]
+		return [uplevel $skiplevel $cmd $args]
 	} else {if {$type == "simple"} {
-		set errcode [catch {eval $cmd $args}]
+		set errcode [catch {uplevel $skiplevel $cmd $args}]
 		if {$errcode == 0} {
 			return ""
 		} else {
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index f55a608a1..b27d04318 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -524,6 +524,7 @@ static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(stlink_dap_arp_init);
 /**
     Converts an STLINK status code held in the first byte of a response
     to an openocd error, logs any error/wait status as debug output.
@@ -628,7 +629,8 @@ static int stlink_usb_error_check(void *handle)
 			LOG_DEBUG("STLINK_SWD_AP_STICKY_ERROR");
 			/* Seen when reading address out of range (0xFFFFFFFF) */
 			/* Seems usb_reset helps the following r/w accesses */
-			stlink_usb_reset(h);
+			// stlink_usb_reset(h);
+			stlink_dap_arp_init(NULL);
 			return ERROR_FAIL;
 		case STLINK_SWD_AP_STICKYORUN_ERROR:
 			LOG_DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR");
@@ -758,6 +760,9 @@ static int stlink_usb_version(void *handle)
 	int res;
 	uint16_t v;
 	uint8_t m_version = 0;
+	uint8_t bridge = 0;
+	char v_str[5 * (1 + 3) + 1]; /* VvJjMmBbSs */
+	char *p;
 	struct stlink_usb_handle_s *h = handle;
 
 	assert(handle != NULL);
@@ -775,6 +780,7 @@ static int stlink_usb_version(void *handle)
 		h->version.swim = h->databuf[1];
 		h->version.jtag = h->databuf[2];
 		m_version = h->databuf[3];
+		bridge = h->databuf[4];
 
 		h->vid = (h->databuf[9] << 8) | h->databuf[8];
 		h->pid = (h->databuf[11] << 8) | h->databuf[10];
@@ -806,13 +812,20 @@ static int stlink_usb_version(void *handle)
 			h->version.jtag_api_max = STLINK_JTAG_API_V1;
 	}
 
-	LOG_INFO("STLINK v%d%s JTAG v%d API v%d %s%d VID 0x%04X PID 0x%04X",
-		h->version.stlink,
-		(h->pid == STLINK_V2_1_PID) ? ".1" : "",
-		h->version.jtag,
+	p = v_str;
+	p += sprintf(p, "V%d", h->version.stlink);
+	if (h->version.jtag || !m_version)
+		p += sprintf(p, "J%d", h->version.jtag);
+	if (m_version)
+		p += sprintf(p, "M%d", m_version);
+	if (bridge)
+		p += sprintf(p, "B%d", bridge);
+	if (h->version.swim || !m_version)
+		p += sprintf(p, "S%d", h->version.swim);
+
+	LOG_INFO("STLINK %s (API v%d) VID:PID %04X:%04X",
+		v_str,
 		h->version.jtag_api_max,
-		(h->pid == STLINK_V2_1_PID || (h->version.stlink == 3 && h->version.swim == 0)) ? "M" : "SWIM v",
-		(h->version.stlink == 3 && h->version.swim == 0) ? m_version : h->version.swim,
 		h->vid,
 		h->pid);
 
@@ -3329,6 +3342,27 @@ COMMAND_HANDLER(stlink_dap_serial_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(stlink_dap_arp_init)
+{
+	if (stlink_dap_param.transport == HL_TRANSPORT_JTAG ||
+		(stlink_dap_param.transport == HL_TRANSPORT_SWD && stlink_dap_handle->version.stlink == 3)) {
+		//stlink_usb_reset(stlink_dap_handle);
+		stlink_usb_mode_leave(stlink_dap_handle, STLINK_MODE_DEBUG_JTAG);
+		stlink_usb_mode_enter(stlink_dap_handle, stlink_get_mode(stlink_dap_param.transport));
+		/* exit jtag closes all the opened AP; reopen them! */
+		for (int apsel = 0; apsel <= DP_APSEL_MAX; apsel++)
+			if (test_bit(apsel, opened_ap)) {
+				clear_bit(apsel, opened_ap);
+				stlink_dap_open_ap(apsel);
+			}
+		/* equivalent to
+			jtag_add_tlr();
+			jtag_execute_queue();
+		but let's stay away from jtag code */
+	}
+	return ERROR_OK;
+}
+
 static const struct command_registration stlink_dap_subcommand_handlers[] = {
 	{
 		.name = "serial",
@@ -3337,6 +3371,14 @@ static const struct command_registration stlink_dap_subcommand_handlers[] = {
 		.help = "set the serial number of the device that should be used",
 		.usage = "<serial number>",
 	},
+	{
+		.name = "arp_init",
+		.handler = &stlink_dap_arp_init,
+		.mode = COMMAND_ANY,
+		.help = "Validates JTAG scan chain against the list of "
+				"declared TAPs.",
+		.usage = "",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index fc5f967ef..8284968d3 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -2183,6 +2183,7 @@ static int gdb_generate_target_description(struct target *target, char **tdesc_o
 	int retval = ERROR_OK;
 	struct reg **reg_list = NULL;
 	int reg_list_size;
+	char const *architecture;
 	char const **features = NULL;
 	char const **arch_defined_types = NULL;
 	int feature_list_size = 0;
@@ -2224,6 +2225,12 @@ static int gdb_generate_target_description(struct target *target, char **tdesc_o
 			"<!DOCTYPE target SYSTEM \"gdb-target.dtd\">\n"
 			"<target version=\"1.0\">\n");
 
+	/* generate architecture element if supported by target */
+	architecture = target_get_gdb_arch(target);
+	if (architecture != NULL)
+		xml_printf(&retval, &tdesc, &pos, &size,
+				"<architecture>%s</architecture>\n", architecture);
+
 	/* generate target description according to register list */
 	if (features != NULL) {
 		while (features[current_feature]) {
@@ -2373,6 +2380,8 @@ static int gdb_target_description_supported(struct target *target, int *supporte
 	char const **features = NULL;
 	int feature_list_size = 0;
 
+	char const *architecture = target_get_gdb_arch(target);
+
 	retval = target_get_gdb_reg_list(target, &reg_list,
 			&reg_list_size, REG_CLASS_ALL);
 	if (retval != ERROR_OK) {
@@ -2394,7 +2403,7 @@ static int gdb_target_description_supported(struct target *target, int *supporte
 	}
 
 	if (supported) {
-		if (feature_list_size)
+		if (architecture || feature_list_size)
 			*supported = 1;
 		else
 			*supported = 0;
@@ -2826,7 +2835,7 @@ static bool gdb_handle_vcont_packet(struct connection *connection, const char *p
 			if (gdb_connection->sync) {
 				gdb_connection->sync = false;
 				if (ct->state == TARGET_HALTED) {
-					LOG_WARNING("stepi ignored. GDB will now fetch the register state " \
+					LOG_DEBUG("stepi ignored. GDB will now fetch the register state " \
 									"from the target.");
 					gdb_sig_halted(connection);
 					log_remove_callback(gdb_log_callback, connection);
@@ -3250,7 +3259,7 @@ static int gdb_input_inner(struct connection *connection)
 						 * make only the single stepping have the sync feature...
 						 */
 						nostep = true;
-						LOG_WARNING("stepi ignored. GDB will now fetch the register state " \
+						LOG_DEBUG("stepi ignored. GDB will now fetch the register state " \
 								"from the target.");
 					}
 					gdb_con->sync = false;
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 454de9e92..8fab3b5d7 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -2824,6 +2824,7 @@ struct target_type aarch64_target = {
 	.deassert_reset = aarch64_deassert_reset,
 
 	/* REVISIT allow exporting VFP3 registers ... */
+	.get_gdb_arch = armv8_get_gdb_arch,
 	.get_gdb_reg_list = armv8_get_gdb_reg_list,
 
 	.read_memory = aarch64_read_memory,
diff --git a/src/target/arm.h b/src/target/arm.h
index 316ff9ab8..ea83d3867 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -263,9 +263,11 @@ struct reg_cache *armv8_build_reg_cache(struct target *target);
 extern const struct command_registration arm_command_handlers[];
 
 int arm_arch_state(struct target *target);
+const char *arm_get_gdb_arch(struct target *target);
 int arm_get_gdb_reg_list(struct target *target,
 		struct reg **reg_list[], int *reg_list_size,
 		enum target_register_class reg_class);
+const char *armv8_get_gdb_arch(struct target *target);
 int armv8_get_gdb_reg_list(struct target *target,
 		struct reg **reg_list[], int *reg_list_size,
 		enum target_register_class reg_class);
diff --git a/src/target/arm11.c b/src/target/arm11.c
index 13fbd207a..443866149 100644
--- a/src/target/arm11.c
+++ b/src/target/arm11.c
@@ -1362,6 +1362,7 @@ struct target_type arm11_target = {
 	.assert_reset = arm11_assert_reset,
 	.deassert_reset = arm11_deassert_reset,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm11_read_memory,
diff --git a/src/target/arm720t.c b/src/target/arm720t.c
index bcbfa9dff..3d12aba22 100644
--- a/src/target/arm720t.c
+++ b/src/target/arm720t.c
@@ -560,6 +560,7 @@ struct target_type arm720t_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm720t_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm720t_read_memory,
diff --git a/src/target/arm7tdmi.c b/src/target/arm7tdmi.c
index 9dcb302d9..e1e91c3a0 100644
--- a/src/target/arm7tdmi.c
+++ b/src/target/arm7tdmi.c
@@ -699,6 +699,7 @@ struct target_type arm7tdmi_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm7_9_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm7_9_read_memory,
diff --git a/src/target/arm920t.c b/src/target/arm920t.c
index 7927a2bea..39d75902c 100644
--- a/src/target/arm920t.c
+++ b/src/target/arm920t.c
@@ -1693,6 +1693,7 @@ struct target_type arm920t_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm920t_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm920t_read_memory,
diff --git a/src/target/arm926ejs.c b/src/target/arm926ejs.c
index 58de77850..07c519a53 100644
--- a/src/target/arm926ejs.c
+++ b/src/target/arm926ejs.c
@@ -804,6 +804,7 @@ struct target_type arm926ejs_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm926ejs_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm7_9_read_memory,
diff --git a/src/target/arm946e.c b/src/target/arm946e.c
index 06c9fc30d..5e25d71ec 100644
--- a/src/target/arm946e.c
+++ b/src/target/arm946e.c
@@ -756,6 +756,7 @@ struct target_type arm946e_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm7_9_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	/* .read_memory = arm7_9_read_memory, */
diff --git a/src/target/arm966e.c b/src/target/arm966e.c
index 0429c54b5..c9d7f01aa 100644
--- a/src/target/arm966e.c
+++ b/src/target/arm966e.c
@@ -259,6 +259,7 @@ struct target_type arm966e_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm7_9_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm7_9_read_memory,
diff --git a/src/target/arm9tdmi.c b/src/target/arm9tdmi.c
index 82b430f88..6425027f4 100644
--- a/src/target/arm9tdmi.c
+++ b/src/target/arm9tdmi.c
@@ -902,6 +902,7 @@ struct target_type arm9tdmi_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm7_9_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm7_9_read_memory,
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 51d228b8a..7d5353a32 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -680,6 +680,15 @@ int dap_dp_init(struct adiv5_dap *dap)
 
 	dap_invalidate_cache(dap);
 
+	/*
+	 * Early initialize dap->dp_ctrl_stat.
+	 * In jtag mode only, if the following atomic reads fail and set the
+	 * sticky error, it will trigger the clearing of the sticky. Without this
+	 * initialization system and debug power would be disabled while clearing
+	 * the sticky error bit.
+	 */
+	dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
+
 	for (size_t i = 0; i < 30; i++) {
 		/* DP initialization */
 
@@ -688,7 +697,18 @@ int dap_dp_init(struct adiv5_dap *dap)
 			break;
 	}
 
-	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, SSTICKYERR);
+	/*
+	 * This write operation clears the sticky error bit in jtag mode only and
+	 * is ignored in swd mode. It also powers-up system and debug domains in
+	 * both jtag and swd modes, if not done before.
+	 * Actually we do not need to clear the sticky error here because it has
+	 * been already cleared (if it was set) in the previous atomic read. This
+	 * write could be removed, but this initial part of dap_dp_init() is the
+	 * result of years of fine tuning and there are strong concerns about any
+	 * unnecessary code change. It doesn't harm, so let's keep it here and
+	 * preserve the historical sequence of read/write operations!
+	 */
+	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat | SSTICKYERR);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -696,7 +716,6 @@ int dap_dp_init(struct adiv5_dap *dap)
 	if (retval != ERROR_OK)
 		return retval;
 
-	dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
 	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat);
 	if (retval != ERROR_OK)
 		return retval;
diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c
index f9b30c187..89c487ae3 100644
--- a/src/target/arm_dpm.c
+++ b/src/target/arm_dpm.c
@@ -168,7 +168,7 @@ static int dpm_read_reg_u64(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 }
 
 /* just read the register -- rely on the core mode being right */
-static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
 	uint32_t value;
 	int retval;
diff --git a/src/target/arm_dpm.h b/src/target/arm_dpm.h
index ac70ce3e1..0901611d2 100644
--- a/src/target/arm_dpm.h
+++ b/src/target/arm_dpm.h
@@ -152,6 +152,7 @@ struct arm_dpm {
 int arm_dpm_setup(struct arm_dpm *dpm);
 int arm_dpm_initialize(struct arm_dpm *dpm);
 
+int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum);
 int arm_dpm_read_current_registers(struct arm_dpm *);
 int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode);
 
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
index 96a63e497..af8fd9a7f 100644
--- a/src/target/armv4_5.c
+++ b/src/target/armv4_5.c
@@ -1179,6 +1179,20 @@ const struct command_registration arm_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
+/*
+ * gdb for arm targets (e.g. arm-none-eabi-gdb) supports several variants
+ * of arm architecture. You can list them using the autocompletion of gdb
+ * command prompt by typing "set architecture " and then press TAB key.
+ * The default, selected automatically, is "arm".
+ * Let's use the default value, here, to make gdb-multiarch behave in the
+ * same way as a gdb for arm. This can be changed later on. User can still
+ * set the specific architecture variant with the gdb command.
+ */
+const char *arm_get_gdb_arch(struct target *target)
+{
+	return "arm";
+}
+
 int arm_get_gdb_reg_list(struct target *target,
 	struct reg **reg_list[], int *reg_list_size,
 	enum target_register_class reg_class)
diff --git a/src/target/armv7a.h b/src/target/armv7a.h
index 1a83522b6..46cf49799 100644
--- a/src/target/armv7a.h
+++ b/src/target/armv7a.h
@@ -106,8 +106,6 @@ struct armv7a_common {
 	struct arm_dpm dpm;
 	uint32_t debug_base;
 	struct adiv5_ap *debug_ap;
-	struct adiv5_ap *memory_ap;
-	bool memory_ap_available;
 	/* mdir */
 	uint8_t multi_processor_system;
 	uint8_t cluster_id;
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 75ada896d..39ce7e129 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -1668,6 +1668,11 @@ const struct command_registration armv8_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
+const char *armv8_get_gdb_arch(struct target *target)
+{
+	return "aarch64";
+}
+
 int armv8_get_gdb_reg_list(struct target *target,
 	struct reg **reg_list[], int *reg_list_size,
 	enum target_register_class reg_class)
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index f17edddb2..b281233ca 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -71,10 +71,6 @@ static int cortex_a_set_hybrid_breakpoint(struct target *target,
 	struct breakpoint *breakpoint);
 static int cortex_a_unset_breakpoint(struct target *target,
 	struct breakpoint *breakpoint);
-static int cortex_a_dap_read_coreregister_u32(struct target *target,
-	uint32_t *value, int regnum);
-static int cortex_a_dap_write_coreregister_u32(struct target *target,
-	uint32_t value, int regnum);
 static int cortex_a_mmu(struct target *target, int *enabled);
 static int cortex_a_mmu_modify(struct target *target, int enable);
 static int cortex_a_virt2phys(struct target *target,
@@ -307,172 +303,6 @@ static int cortex_a_exec_opcode(struct target *target,
 	return retval;
 }
 
-/**************************************************************************
-Read core register with very few exec_opcode, fast but needs work_area.
-This can cause problems with MMU active.
-**************************************************************************/
-static int cortex_a_read_regs_through_mem(struct target *target, uint32_t address,
-	uint32_t *regfile)
-{
-	int retval = ERROR_OK;
-	struct armv7a_common *armv7a = target_to_armv7a(target);
-
-	retval = cortex_a_dap_read_coreregister_u32(target, regfile, 0);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = cortex_a_dap_write_coreregister_u32(target, address, 0);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = cortex_a_exec_opcode(target, ARMV4_5_STMIA(0, 0xFFFE, 0, 0), NULL);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_read_buf(armv7a->memory_ap,
-			(uint8_t *)(&regfile[1]), 4, 15, address);
-
-	return retval;
-}
-
-static int cortex_a_dap_read_coreregister_u32(struct target *target,
-	uint32_t *value, int regnum)
-{
-	int retval = ERROR_OK;
-	uint8_t reg = regnum&0xFF;
-	uint32_t dscr = 0;
-	struct armv7a_common *armv7a = target_to_armv7a(target);
-
-	if (reg > 17)
-		return retval;
-
-	if (reg < 15) {
-		/* Rn to DCCTX, "MCR p14, 0, Rn, c0, c5, 0"  0xEE00nE15 */
-		retval = cortex_a_exec_opcode(target,
-				ARMV4_5_MCR(14, 0, reg, 0, 5, 0),
-				&dscr);
-		if (retval != ERROR_OK)
-			return retval;
-	} else if (reg == 15) {
-		/* "MOV r0, r15"; then move r0 to DCCTX */
-		retval = cortex_a_exec_opcode(target, 0xE1A0000F, &dscr);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = cortex_a_exec_opcode(target,
-				ARMV4_5_MCR(14, 0, 0, 0, 5, 0),
-				&dscr);
-		if (retval != ERROR_OK)
-			return retval;
-	} else {
-		/* "MRS r0, CPSR" or "MRS r0, SPSR"
-		 * then move r0 to DCCTX
-		 */
-		retval = cortex_a_exec_opcode(target, ARMV4_5_MRS(0, reg & 1), &dscr);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = cortex_a_exec_opcode(target,
-				ARMV4_5_MCR(14, 0, 0, 0, 5, 0),
-				&dscr);
-		if (retval != ERROR_OK)
-			return retval;
-	}
-
-	/* Wait for DTRRXfull then read DTRRTX */
-	int64_t then = timeval_ms();
-	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
-		retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-				armv7a->debug_base + CPUDBG_DSCR, &dscr);
-		if (retval != ERROR_OK)
-			return retval;
-		if (timeval_ms() > then + 1000) {
-			LOG_ERROR("Timeout waiting for cortex_a_exec_opcode");
-			return ERROR_FAIL;
-		}
-	}
-
-	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_DTRTX, value);
-	LOG_DEBUG("read DCC 0x%08" PRIx32, *value);
-
-	return retval;
-}
-
-static int cortex_a_dap_write_coreregister_u32(struct target *target,
-	uint32_t value, int regnum)
-{
-	int retval = ERROR_OK;
-	uint8_t Rd = regnum&0xFF;
-	uint32_t dscr;
-	struct armv7a_common *armv7a = target_to_armv7a(target);
-
-	LOG_DEBUG("register %i, value 0x%08" PRIx32, regnum, value);
-
-	/* Check that DCCRX is not full */
-	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_DSCR, &dscr);
-	if (retval != ERROR_OK)
-		return retval;
-	if (dscr & DSCR_DTR_RX_FULL) {
-		LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
-		/* Clear DCCRX with MRC(p14, 0, Rd, c0, c5, 0), opcode  0xEE100E15 */
-		retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
-				&dscr);
-		if (retval != ERROR_OK)
-			return retval;
-	}
-
-	if (Rd > 17)
-		return retval;
-
-	/* Write DTRRX ... sets DSCR.DTRRXfull but exec_opcode() won't care */
-	LOG_DEBUG("write DCC 0x%08" PRIx32, value);
-	retval = mem_ap_write_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_DTRRX, value);
-	if (retval != ERROR_OK)
-		return retval;
-
-	if (Rd < 15) {
-		/* DCCRX to Rn, "MRC p14, 0, Rn, c0, c5, 0", 0xEE10nE15 */
-		retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, Rd, 0, 5, 0),
-				&dscr);
-
-		if (retval != ERROR_OK)
-			return retval;
-	} else if (Rd == 15) {
-		/* DCCRX to R0, "MRC p14, 0, R0, c0, c5, 0", 0xEE100E15
-		 * then "mov r15, r0"
-		 */
-		retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
-				&dscr);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = cortex_a_exec_opcode(target, 0xE1A0F000, &dscr);
-		if (retval != ERROR_OK)
-			return retval;
-	} else {
-		/* DCCRX to R0, "MRC p14, 0, R0, c0, c5, 0", 0xEE100E15
-		 * then "MSR CPSR_cxsf, r0" or "MSR SPSR_cxsf, r0" (all fields)
-		 */
-		retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
-				&dscr);
-		if (retval != ERROR_OK)
-			return retval;
-		retval = cortex_a_exec_opcode(target, ARMV4_5_MSR_GP(0, 0xF, Rd & 1),
-				&dscr);
-		if (retval != ERROR_OK)
-			return retval;
-
-		/* "Prefetch flush" after modifying execution status in CPSR */
-		if (Rd == 16) {
-			retval = cortex_a_exec_opcode(target,
-					ARMV4_5_MCR(15, 0, 0, 7, 5, 4),
-					&dscr);
-			if (retval != ERROR_OK)
-				return retval;
-		}
-	}
-
-	return retval;
-}
-
 /* Write to memory mapped registers directly with no cache or mmu handling */
 static int cortex_a_dap_write_memap_register_u32(struct target *target,
 	uint32_t address,
@@ -1183,14 +1013,11 @@ static int cortex_a_resume(struct target *target, int current,
 
 static int cortex_a_debug_entry(struct target *target)
 {
-	int i;
-	uint32_t regfile[16], cpsr, spsr, dscr;
+	uint32_t dscr;
 	int retval = ERROR_OK;
-	struct working_area *regfile_working_area = NULL;
 	struct cortex_a_common *cortex_a = target_to_cortex_a(target);
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	struct arm *arm = &armv7a->arm;
-	struct reg *reg;
 
 	LOG_DEBUG("dscr = 0x%08" PRIx32, cortex_a->cpudbg_dscr);
 
@@ -1227,68 +1054,16 @@ static int cortex_a_debug_entry(struct target *target)
 		arm_dpm_report_wfar(&armv7a->dpm, wfar);
 	}
 
-	/* REVISIT fast_reg_read is never set ... */
-
-	/* Examine target state and mode */
-	if (cortex_a->fast_reg_read)
-		target_alloc_working_area(target, 64, &regfile_working_area);
-
-
-	/* First load register acessible through core debug port*/
-	if (!regfile_working_area)
-		retval = arm_dpm_read_current_registers(&armv7a->dpm);
-	else {
-		retval = cortex_a_read_regs_through_mem(target,
-				regfile_working_area->address, regfile);
-
-		target_free_working_area(target, regfile_working_area);
-		if (retval != ERROR_OK)
-			return retval;
-
-		/* read Current PSR */
-		retval = cortex_a_dap_read_coreregister_u32(target, &cpsr, 16);
-		/*  store current cpsr */
-		if (retval != ERROR_OK)
-			return retval;
-
-		LOG_DEBUG("cpsr: %8.8" PRIx32, cpsr);
-
-		arm_set_cpsr(arm, cpsr);
-
-		/* update cache */
-		for (i = 0; i <= ARM_PC; i++) {
-			reg = arm_reg_current(arm, i);
-
-			buf_set_u32(reg->value, 0, 32, regfile[i]);
-			reg->valid = 1;
-			reg->dirty = 0;
-		}
-
-		/* Fixup PC Resume Address */
-		if (cpsr & (1 << 5)) {
-			/* T bit set for Thumb or ThumbEE state */
-			regfile[ARM_PC] -= 4;
-		} else {
-			/* ARM state */
-			regfile[ARM_PC] -= 8;
-		}
-
-		reg = arm->pc;
-		buf_set_u32(reg->value, 0, 32, regfile[ARM_PC]);
-		reg->dirty = reg->valid;
-	}
+	/* First load register accessible through core debug port */
+	retval = arm_dpm_read_current_registers(&armv7a->dpm);
+	if (retval != ERROR_OK)
+		return retval;
 
 	if (arm->spsr) {
-		/* read Saved PSR */
-		retval = cortex_a_dap_read_coreregister_u32(target, &spsr, 17);
-		/*  store current spsr */
+		/* read SPSR */
+		retval = dpm_read_reg(&armv7a->dpm, arm->spsr, 17);
 		if (retval != ERROR_OK)
 			return retval;
-
-		reg = arm->spsr;
-		buf_set_u32(reg->value, 0, 32, spsr);
-		reg->valid = 1;
-		reg->dirty = 0;
 	}
 
 #if 0
@@ -2784,9 +2559,6 @@ static int cortex_a_read_phys_memory(struct target *target,
 	target_addr_t address, uint32_t size,
 	uint32_t count, uint8_t *buffer)
 {
-	struct armv7a_common *armv7a = target_to_armv7a(target);
-	struct adiv5_dap *swjdp = armv7a->arm.dap;
-	uint8_t apsel = swjdp->apsel;
 	int retval;
 
 	if (!count || !buffer)
@@ -2795,9 +2567,6 @@ static int cortex_a_read_phys_memory(struct target *target,
 	LOG_DEBUG("Reading memory at real address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
 		address, size, count);
 
-	if (armv7a->memory_ap_available && (apsel == armv7a->memory_ap->ap_num))
-		return mem_ap_read_buf(armv7a->memory_ap, buffer, size, count, address);
-
 	/* read memory through the CPU */
 	cortex_a_prep_memaccess(target, 1);
 	retval = cortex_a_read_cpu_memory(target, address, size, count, buffer);
@@ -2822,79 +2591,10 @@ static int cortex_a_read_memory(struct target *target, target_addr_t address,
 	return retval;
 }
 
-static int cortex_a_read_memory_ahb(struct target *target, target_addr_t address,
-	uint32_t size, uint32_t count, uint8_t *buffer)
-{
-	int mmu_enabled = 0;
-	target_addr_t virt, phys;
-	int retval;
-	struct armv7a_common *armv7a = target_to_armv7a(target);
-	struct adiv5_dap *swjdp = armv7a->arm.dap;
-	uint8_t apsel = swjdp->apsel;
-
-	if (!armv7a->memory_ap_available || (apsel != armv7a->memory_ap->ap_num))
-		return target_read_memory(target, address, size, count, buffer);
-
-	/* cortex_a handles unaligned memory access */
-	LOG_DEBUG("Reading memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
-		address, size, count);
-
-	/* flush the data cache */
-	if (armv7a->armv7a_mmu.armv7a_cache.auto_cache_enabled)
-		armv7a_l1_d_cache_clean_virt(target, address, size * count);
-
-	/* determine if MMU was enabled on target stop */
-	if (!armv7a->is_armv7r) {
-		retval = cortex_a_mmu(target, &mmu_enabled);
-		if (retval != ERROR_OK)
-			return retval;
-	}
-
-	if (!count || !buffer)
-		return ERROR_COMMAND_SYNTAX_ERROR;
-
-	if (!mmu_enabled)
-		return mem_ap_read_buf(armv7a->memory_ap, buffer, size, count, address);
-
-	/*
-	 * TODO: here we are pessimistic and we use the smaller page of 4kB, but
-	 * cortex_a_virt2phys() does a full table walk, so can return the actual
-	 * size of page (4kB or 64kB) or section (1MB or 16Mb).
-	 */
-	while (count) {
-		target_addr_t page_size = 0x1000;
-		uint32_t current_count;
-
-		virt = address;
-		retval = cortex_a_virt2phys(target, virt, &phys);
-		if (retval != ERROR_OK)
-			return retval;
-
-		current_count = (page_size - (address & (page_size - 1))) / size;
-		if (current_count > count)
-			current_count = count;
-
-		LOG_DEBUG("Reading at virtual address 0x%" PRIx32 " bytes. "
-			  "Translating v:" TARGET_ADDR_FMT " to r:" TARGET_ADDR_FMT,
-			  size * current_count, virt, phys);
-
-		retval = mem_ap_read_buf(armv7a->memory_ap, buffer, size, current_count, phys);
-		if (retval != ERROR_OK)
-			return retval;
-		count -= current_count;
-		address += size * current_count;
-		buffer += size * current_count;
-	}
-	return ERROR_OK;
-}
-
 static int cortex_a_write_phys_memory(struct target *target,
 	target_addr_t address, uint32_t size,
 	uint32_t count, const uint8_t *buffer)
 {
-	struct armv7a_common *armv7a = target_to_armv7a(target);
-	struct adiv5_dap *swjdp = armv7a->arm.dap;
-	uint8_t apsel = swjdp->apsel;
 	int retval;
 
 	if (!count || !buffer)
@@ -2903,9 +2603,6 @@ static int cortex_a_write_phys_memory(struct target *target,
 	LOG_DEBUG("Writing memory to real address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
 		address, size, count);
 
-	if (armv7a->memory_ap_available && (apsel == armv7a->memory_ap->ap_num))
-		return mem_ap_write_buf(armv7a->memory_ap, buffer, size, count, address);
-
 	/* write memory through the CPU */
 	cortex_a_prep_memaccess(target, 1);
 	retval = cortex_a_write_cpu_memory(target, address, size, count, buffer);
@@ -2932,74 +2629,6 @@ static int cortex_a_write_memory(struct target *target, target_addr_t address,
 	return retval;
 }
 
-static int cortex_a_write_memory_ahb(struct target *target, target_addr_t address,
-	uint32_t size, uint32_t count, const uint8_t *buffer)
-{
-	int mmu_enabled = 0;
-	target_addr_t virt, phys;
-	int retval;
-	struct armv7a_common *armv7a = target_to_armv7a(target);
-	struct adiv5_dap *swjdp = armv7a->arm.dap;
-	uint8_t apsel = swjdp->apsel;
-
-	if (!armv7a->memory_ap_available || (apsel != armv7a->memory_ap->ap_num))
-		return target_write_memory(target, address, size, count, buffer);
-
-	/* cortex_a handles unaligned memory access */
-	LOG_DEBUG("Writing memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
-		address, size, count);
-
-	/* flush and invalidate the data cache */
-	if (armv7a->armv7a_mmu.armv7a_cache.auto_cache_enabled)
-		armv7a_cache_flush_virt(target, address, size * count);
-	/* invalidate instruction cache */
-	armv7a_l1_i_cache_inval_virt(target, address, size * count);
-
-	/* determine if MMU was enabled on target stop */
-	if (!armv7a->is_armv7r) {
-		retval = cortex_a_mmu(target, &mmu_enabled);
-		if (retval != ERROR_OK)
-			return retval;
-	}
-
-	if (!count || !buffer)
-		return ERROR_COMMAND_SYNTAX_ERROR;
-
-	if (!mmu_enabled)
-		return mem_ap_write_buf(armv7a->memory_ap, buffer, size, count, address);
-
-	/*
-	 * TODO: here we are pessimistic and we use the smaller page of 4kB, but
-	 * cortex_a_virt2phys() does a full table walk, so can return the actual
-	 * size of page (4kB or 64kB) or section (1MB or 16Mb).
-	 */
-	while (count) {
-		target_addr_t page_size = 0x1000;
-		uint32_t current_count;
-
-		virt = address;
-		retval = cortex_a_virt2phys(target, virt, &phys);
-		if (retval != ERROR_OK)
-			return retval;
-
-		current_count = (page_size - (address & (page_size - 1))) / size;
-		if (current_count > count)
-			current_count = count;
-
-		LOG_DEBUG("Writing to virtual address 0x%" PRIx32 " bytes. "
-			  "Translating v:" TARGET_ADDR_FMT " to r:" TARGET_ADDR_FMT,
-			  size * current_count, virt, phys);
-
-		retval = mem_ap_write_buf(armv7a->memory_ap, buffer, size, current_count, phys);
-		if (retval != ERROR_OK)
-			return retval;
-		count -= current_count;
-		address += size * current_count;
-		buffer += size * current_count;
-	}
-	return ERROR_OK;
-}
-
 static int cortex_a_read_buffer(struct target *target, target_addr_t address,
 				uint32_t count, uint8_t *buffer)
 {
@@ -3009,7 +2638,7 @@ static int cortex_a_read_buffer(struct target *target, target_addr_t address,
 	 * will have something to do with the size we leave to it. */
 	for (size = 1; size < 4 && count >= size * 2 + (address & size); size *= 2) {
 		if (address & size) {
-			int retval = cortex_a_read_memory_ahb(target, address, size, 1, buffer);
+			int retval = target_read_memory(target, address, size, 1, buffer);
 			if (retval != ERROR_OK)
 				return retval;
 			address += size;
@@ -3022,7 +2651,7 @@ static int cortex_a_read_buffer(struct target *target, target_addr_t address,
 	for (; size > 0; size /= 2) {
 		uint32_t aligned = count - count % size;
 		if (aligned > 0) {
-			int retval = cortex_a_read_memory_ahb(target, address, size, aligned / size, buffer);
+			int retval = target_read_memory(target, address, size, aligned / size, buffer);
 			if (retval != ERROR_OK)
 				return retval;
 			address += aligned;
@@ -3043,7 +2672,7 @@ static int cortex_a_write_buffer(struct target *target, target_addr_t address,
 	 * will have something to do with the size we leave to it. */
 	for (size = 1; size < 4 && count >= size * 2 + (address & size); size *= 2) {
 		if (address & size) {
-			int retval = cortex_a_write_memory_ahb(target, address, size, 1, buffer);
+			int retval = target_write_memory(target, address, size, 1, buffer);
 			if (retval != ERROR_OK)
 				return retval;
 			address += size;
@@ -3056,7 +2685,7 @@ static int cortex_a_write_buffer(struct target *target, target_addr_t address,
 	for (; size > 0; size /= 2) {
 		uint32_t aligned = count - count % size;
 		if (aligned > 0) {
-			int retval = cortex_a_write_memory_ahb(target, address, size, aligned / size, buffer);
+			int retval = target_write_memory(target, address, size, aligned / size, buffer);
 			if (retval != ERROR_OK)
 				return retval;
 			address += aligned;
@@ -3117,21 +2746,7 @@ static int cortex_a_examine_first(struct target *target)
 
 	int i;
 	int retval = ERROR_OK;
-	uint32_t didr, cpuid, dbg_osreg, dp_ctrl_stat;
-
-	/*
-	 * Some device turn off CSYSPWRUPACK or CDBGPWRUPACK at reset.
-	 * Check them and eventually re init the DP.
-	 */
-	retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &dp_ctrl_stat);
-	if (retval != ERROR_OK
-		|| ((dp_ctrl_stat & CDBGPWRUPACK) == 0)
-		|| (!swjdp->ignore_syspwrupack && ((dp_ctrl_stat & CSYSPWRUPACK) == 0))) {
-		LOG_INFO("DP disabled after reset. Re-initialize it!");
-		retval = dap_dp_init(swjdp);
-		if (retval != ERROR_OK)
-			LOG_ERROR("DP initialization failed");
-	}
+	uint32_t didr, cpuid, dbg_osreg;
 
 	/* Search for the APB-AP - it is needed for access to debug registers */
 	retval = dap_find_ap(swjdp, AP_TYPE_APB_AP, &armv7a->debug_ap);
@@ -3148,25 +2763,6 @@ static int cortex_a_examine_first(struct target *target)
 
 	armv7a->debug_ap->memaccess_tck = 80;
 
-	/* Search for the AXI-AP or the AHB-AP.
-	 * REVISIT: We should make sure the AP's MEMTYPE says it
-	 * can access system memory. */
-	armv7a->memory_ap_available = false;
-	retval = dap_find_ap(swjdp, AP_TYPE_AXI_AP, &armv7a->memory_ap);
-	if (retval == ERROR_OK)
-		retval = mem_ap_init(armv7a->memory_ap);
-	if (retval != ERROR_OK) {
-		retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7a->memory_ap);
-		if (retval == ERROR_OK)
-			retval = mem_ap_init(armv7a->memory_ap);
-	}
-	if (retval != ERROR_OK) {
-		/* AHB-AP and AXI-AP not found or unavailable - use the CPU */
-		LOG_DEBUG("Nor AHB-AP nor AXI-AP available for memory access");
-	} else {
-		armv7a->memory_ap_available = true;
-	}
-
 	if (!target->dbgbase_set) {
 		uint32_t dbgbase;
 		/* Get ROM Table base */
@@ -3326,8 +2922,6 @@ static int cortex_a_init_arch_info(struct target *target,
 	cortex_a->common_magic = CORTEX_A_COMMON_MAGIC;
 	armv7a->arm.dap = dap;
 
-	cortex_a->fast_reg_read = 0;
-
 	/* register arch-specific functions */
 	armv7a->examine_debug_reason = NULL;
 
@@ -3412,10 +3006,7 @@ static int cortex_a_mmu(struct target *target, int *enabled)
 static int cortex_a_virt2phys(struct target *target,
 	target_addr_t virt, target_addr_t *phys)
 {
-	int retval = ERROR_FAIL;
-	struct armv7a_common *armv7a = target_to_armv7a(target);
-	struct adiv5_dap *swjdp = armv7a->arm.dap;
-	uint8_t apsel = swjdp->apsel;
+	int retval;
 	int mmu_enabled = 0;
 
 	/*
@@ -3430,23 +3021,12 @@ static int cortex_a_virt2phys(struct target *target,
 		return ERROR_OK;
 	}
 
-	if (armv7a->memory_ap_available && (apsel == armv7a->memory_ap->ap_num)) {
-		uint32_t ret;
-		retval = armv7a_mmu_translate_va(target,
-				virt, &ret);
-		if (retval != ERROR_OK)
-			goto done;
-		*phys = ret;
-	} else {/*  use this method if armv7a->memory_ap not selected
-		 *  mmu must be enable in order to get a correct translation */
-		retval = cortex_a_mmu_modify(target, 1);
-		if (retval != ERROR_OK)
-			goto done;
-		retval = armv7a_mmu_translate_va_pa(target, (uint32_t)virt,
+	/* mmu must be enable in order to get a correct translation */
+	retval = cortex_a_mmu_modify(target, 1);
+	if (retval != ERROR_OK)
+		return retval;
+	return armv7a_mmu_translate_va_pa(target, (uint32_t)virt,
 						    (uint32_t *)phys, 1);
-	}
-done:
-	return retval;
 }
 
 COMMAND_HANDLER(cortex_a_handle_cache_info_command)
@@ -3666,6 +3246,7 @@ struct target_type cortexa_target = {
 	.deassert_reset = cortex_a_post_deassert_reset,
 
 	/* REVISIT allow exporting VFP3 registers ... */
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = cortex_a_read_memory,
@@ -3746,6 +3327,7 @@ struct target_type cortexr4_target = {
 	.deassert_reset = cortex_a_post_deassert_reset,
 
 	/* REVISIT allow exporting VFP3 registers ... */
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = cortex_a_read_phys_memory,
diff --git a/src/target/cortex_a.h b/src/target/cortex_a.h
index ff0343208..197a5992d 100644
--- a/src/target/cortex_a.h
+++ b/src/target/cortex_a.h
@@ -93,9 +93,6 @@ struct cortex_a_common {
 	int brp_num_available;
 	struct cortex_a_brp *brp_list;
 
-	/* Use cortex_a_read_regs_through_mem for fast register reads */
-	int fast_reg_read;
-
 	uint32_t cpuid;
 	uint32_t didr;
 
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 42eb4b299..79975aa89 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -2495,6 +2495,7 @@ struct target_type cortexm_target = {
 /*	.deassert_reset = cortex_m_post_deassert_reset,*/
 	.soft_reset_halt = cortex_m_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = armv7m_get_gdb_reg_list,
 
 	.read_memory = cortex_m_read_memory,
diff --git a/src/target/fa526.c b/src/target/fa526.c
index 9f6b80551..bb9f7353a 100644
--- a/src/target/fa526.c
+++ b/src/target/fa526.c
@@ -364,6 +364,7 @@ struct target_type fa526_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm920t_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm920t_read_memory,
diff --git a/src/target/feroceon.c b/src/target/feroceon.c
index 6b14ab6a8..21963e55e 100644
--- a/src/target/feroceon.c
+++ b/src/target/feroceon.c
@@ -710,6 +710,7 @@ struct target_type feroceon_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm926ejs_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm7_9_read_memory,
@@ -747,6 +748,7 @@ struct target_type dragonite_target = {
 	.deassert_reset = arm7_9_deassert_reset,
 	.soft_reset_halt = arm7_9_soft_reset_halt,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = arm7_9_read_memory,
diff --git a/src/target/hla_target.c b/src/target/hla_target.c
index 9ebf24168..567a61dc3 100644
--- a/src/target/hla_target.c
+++ b/src/target/hla_target.c
@@ -820,6 +820,7 @@ struct target_type hla_target = {
 	.resume = adapter_resume,
 	.step = adapter_step,
 
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = armv7m_get_gdb_reg_list,
 
 	.read_memory = adapter_read_memory,
diff --git a/src/target/startup.tcl b/src/target/startup.tcl
index 10ee27c2d..416c20871 100644
--- a/src/target/startup.tcl
+++ b/src/target/startup.tcl
@@ -231,6 +231,9 @@ proc ocd_process_reset_inner { MODE } {
 		# Use TRST or TMS/TCK operations to reset all the tap controllers.
 		# TAP reset events get reported; they might enable some taps.
 		init_reset $MODE
+
+		# after resetting the JTAG chain, re-initialize all existing DAPs
+		dap init
 	}
 
 	foreach t $targets {
@@ -250,9 +253,10 @@ proc ocd_process_reset_inner { MODE } {
 
 	# Deassert SRST
 	reset_deassert_initial $MODE
-	if { !$early_reset_init } {
-		if [using_jtag] { jtag arp_init }
-	}
+	if [using_stlink_jtag] { st-link arp_init }
+	if [using_stlink_swd] { st-link arp_init }
+	if [using_jtag] { jtag arp_init }
+	dap init
 
 	foreach t $targets {
 		$t invoke-event reset-deassert-post
diff --git a/src/target/stm8.c b/src/target/stm8.c
index 5a3438a64..9a57e4490 100644
--- a/src/target/stm8.c
+++ b/src/target/stm8.c
@@ -1189,6 +1189,11 @@ static int stm8_write_core_reg(struct target *target, unsigned int num)
 	return ERROR_OK;
 }
 
+static const char *stm8_get_gdb_arch(struct target *target)
+{
+	return "stm8";
+}
+
 static int stm8_get_gdb_reg_list(struct target *target, struct reg **reg_list[],
 		int *reg_list_size, enum target_register_class reg_class)
 {
@@ -2199,6 +2204,7 @@ struct target_type stm8_target = {
 	.assert_reset = stm8_reset_assert,
 	.deassert_reset = stm8_reset_deassert,
 
+	.get_gdb_arch = stm8_get_gdb_arch,
 	.get_gdb_reg_list = stm8_get_gdb_reg_list,
 
 	.read_memory = stm8_read_memory,
diff --git a/src/target/target.c b/src/target/target.c
index 906f1be9d..4a20e0edb 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1255,6 +1255,13 @@ int target_hit_watchpoint(struct target *target,
 	return target->type->hit_watchpoint(target, hit_watchpoint);
 }
 
+const char *target_get_gdb_arch(struct target *target)
+{
+	if (target->type->get_gdb_arch == NULL)
+		return NULL;
+	return target->type->get_gdb_arch(target);
+}
+
 int target_get_gdb_reg_list(struct target *target,
 		struct reg **reg_list[], int *reg_list_size,
 		enum target_register_class reg_class)
diff --git a/src/target/target.h b/src/target/target.h
index b24abc0f1..2532bfbbf 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -492,6 +492,13 @@ int target_remove_watchpoint(struct target *target,
 int target_hit_watchpoint(struct target *target,
 		struct watchpoint **watchpoint);
 
+/**
+ * Obtain the architecture for GDB.
+ *
+ * This routine is a wrapper for target->type->get_gdb_arch.
+ */
+const char *target_get_gdb_arch(struct target *target);
+
 /**
  * Obtain the registers for GDB.
  *
diff --git a/src/target/target_type.h b/src/target/target_type.h
index 24b29ea2f..e041bbd7f 100644
--- a/src/target/target_type.h
+++ b/src/target/target_type.h
@@ -104,6 +104,15 @@ struct target_type {
 	 */
 	int (*reset_clear_internal_state)(struct target *target);
 
+	/**
+	 * Target architecture for GDB.
+	 *
+	 * The string returned by this function will not be automatically freed;
+	 * if dynamic allocation is used for this value, it must be managed by
+	 * the target, ideally by caching the result for subsequent calls.
+	 */
+	const char *(*get_gdb_arch)(struct target *target);
+
 	/**
 	 * Target register access for GDB.  Do @b not call this function
 	 * directly, use target_get_gdb_reg_list() instead.
diff --git a/src/target/xscale.c b/src/target/xscale.c
index 87a3d0f78..3ac455390 100644
--- a/src/target/xscale.c
+++ b/src/target/xscale.c
@@ -3703,6 +3703,7 @@ struct target_type xscale_target = {
 	.deassert_reset = xscale_deassert_reset,
 
 	/* REVISIT on some cores, allow exporting iwmmxt registers ... */
+	.get_gdb_arch = arm_get_gdb_arch,
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
 	.read_memory = xscale_read_memory,
diff --git a/tcl/target/stm32mp15x.cfg b/tcl/target/stm32mp15x.cfg
index 5d7e230c1..0d10021ef 100644
--- a/tcl/target/stm32mp15x.cfg
+++ b/tcl/target/stm32mp15x.cfg
@@ -35,10 +35,22 @@ if { [info exists CPUTAPID] } {
    }
 }
 
+# Chip Level TAP Controller, only in jtag mode
+if { [info exists CLCTAPID] } {
+	set _CLCTAPID $CLCTAPID
+} else {
+	set _CLCTAPID 0x06500041
+}
+
 swj_newdap $_CHIPNAME tap -expected-id $_CPUTAPID -irlen 4 -ircapture 0x01 -irmask 0x0f
-dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.tap
+if { [using_jtag] } {
+	swj_newdap $_CHIPNAME.clc tap -expected-id $_CLCTAPID -irlen 5
+}
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.tap -ignore-syspwrupack
 target create $_CHIPNAME.cpu0 cortex_a -dap $_CHIPNAME.dap -coreid 0 -dbgbase 0xE00D0000
 target create $_CHIPNAME.cpu1 cortex_a -dap $_CHIPNAME.dap -coreid 1 -dbgbase 0xE00D2000
+target create $_CHIPNAME.axi mem_ap -dap $_CHIPNAME.dap -ap-num 0
+target create $_CHIPNAME.ap1 mem_ap -dap $_CHIPNAME.dap -ap-num 1
 
 target smp $_CHIPNAME.cpu0 $_CHIPNAME.cpu1
 $_CHIPNAME.cpu0 cortex_a maskisr on
@@ -49,13 +61,15 @@ $_CHIPNAME.cpu1 cortex_a dacrfixup on
 # Only in ENG_MODE the CM4 is available at reset
 if { [info exists ENG_MODE] } {
 	target create $_CHIPNAME.cpu2 cortex_m -dap $_CHIPNAME.dap
+	target create $_CHIPNAME.ap2 mem_ap -dap $_CHIPNAME.dap -ap-num 2
 } else {
 	target create $_CHIPNAME.cpu2 cortex_m -dap $_CHIPNAME.dap -defer-examine
+	target create $_CHIPNAME.ap2 mem_ap -dap $_CHIPNAME.dap -ap-num 2 -defer-examine
 }
 
-$_CHIPNAME.cpu0 configure -work-area-phys 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0
-$_CHIPNAME.cpu1 configure -work-area-phys 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0
-$_CHIPNAME.cpu2 configure -work-area-phys 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+#$_CHIPNAME.cpu0 configure -work-area-phys 0x2ffc0000 -work-area-virt 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+#$_CHIPNAME.cpu1 configure -work-area-phys 0x2ffc0000 -work-area-virt 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+#$_CHIPNAME.cpu2 configure -work-area-phys 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0
 
 if { [info exists GDB_PORT] } {
 	set _GDB_PORT $GDB_PORT
@@ -71,25 +85,29 @@ $_CHIPNAME.cpu2 configure -gdb-port [expr $_GDB_PORT + 0]
 
 $_CHIPNAME.cpu0 configure -event reset-deassert-post { delayed_reset_halt_cpu0 }
 
-# Reset-halt stops in bootrom when only cpu0 can be attached.
+# In production mode, reset-halt stops in bootrom when only
+# cpu0 can be attached due to security mode.
 # Set a breakpoint to stop cpu0 when cpu1 can be attached too.
 proc delayed_reset_halt_cpu0 { } {
 	global _CHIPNAME
+	global ENG_MODE
 	global arp_reset_mode
 
-	set rom_halt_pc 0x000079ac
-
-	# only "reset halt" or "reset init"
-	if { 0 != [ string compare "$arp_reset_mode" "run" ] } {
+	if { [info exists ENG_MODE] || ([string compare "$arp_reset_mode" "run"] == 0) } {
+		# default reset-deassert-post handler
+		arp_reset_default_handler post $_CHIPNAME.cpu0
+	} else {
+		# only in production mode for "reset halt" or "reset init"
 		cortex_a smp_off
 		targets $_CHIPNAME.cpu0
 		arp_reset_default_handler post $_CHIPNAME.cpu0
 
-		# quick test for cut1.0, check bootrom
+		set rom_halt_pc 0x000079ac
+		# quick test for rev.A, check bootrom
 		set v ""
-		ap0_non_secure_access
-		mem2array v 32 0x0000a000 1 phys
-		ap0_secure_access
+		axi_non_secure_access
+		$_CHIPNAME.axi mem2array v 32 0x0000a000 1 phys
+		axi_secure_access
 		if { $v(0) == 0x1e494610 } {
 			set rom_halt_pc 0x0000688c
 		}
@@ -148,6 +166,8 @@ $_CHIPNAME.cpu2 configure -event gdb-attach {
 	if { ! [info exists ENG_MODE] } {
 		$_CHIPNAME.cpu2 arp_examine
 		$_CHIPNAME.cpu2 arp_poll
+		$_CHIPNAME.ap2  arp_examine
+		$_CHIPNAME.ap2  arp_poll
 	}
 	halt
 }
@@ -156,19 +176,18 @@ $_CHIPNAME.cpu0 configure -event gdb-detach { catch { resume }; clients_dec_and_
 $_CHIPNAME.cpu1 configure -event gdb-detach { catch { resume }; clients_dec_and_shutdown }
 $_CHIPNAME.cpu2 configure -event gdb-detach { catch { resume }; clients_dec_and_shutdown }
 
-proc ap0_secure_access {} {
+proc axi_secure_access {} {
 	global _CHIPNAME
 
 	$_CHIPNAME.dap apsel 0
 	$_CHIPNAME.dap apcsw 0x10006000
 }
 
-proc ap0_non_secure_access {} {
+proc axi_non_secure_access {} {
 	global _CHIPNAME
 
 	$_CHIPNAME.dap apsel 0
 	$_CHIPNAME.dap apcsw 0x30006000
 }
 
-$_CHIPNAME.cpu0 configure -event examine-end { ap0_secure_access }
-$_CHIPNAME.cpu1 configure -event examine-end { ap0_secure_access }
+axi_secure_access
diff --git a/tcl/target/stm32mp15x_stpmic1.cfg b/tcl/target/stm32mp15x_stpmic1.cfg
index c08044196..a6446c640 100644
--- a/tcl/target/stm32mp15x_stpmic1.cfg
+++ b/tcl/target/stm32mp15x_stpmic1.cfg
@@ -15,16 +15,20 @@ proc wait_state {condition} {
 	return -code 1 "Timed out"
 }
 
-proc mrw_phys {reg} {
+proc $_CHIPNAME.axi_mrw {reg} {
+	global _CHIPNAME
+
 	set value ""
-	mem2array value 32 $reg 1 phys
+	$_CHIPNAME.axi mem2array value 32 $reg 1
 	return $value(0)
 }
 
-proc mmw_phys {reg setbits clearbits} {
-	set old [mrw_phys $reg]
+proc $_CHIPNAME.axi_mmw {reg setbits clearbits} {
+	global _CHIPNAME
+
+	set old [$_CHIPNAME.axi_mrw $reg]
 	set new [expr ($old & ~$clearbits) | $setbits]
-	mww phys $reg $new
+	$_CHIPNAME.axi mww $reg $new
 }
 
 # Set mask_reset bits in PMIC to keep all powers on at next reset event
@@ -33,9 +37,6 @@ proc mmw_phys {reg setbits clearbits} {
 proc pmic_init {} {
 	global _CHIPNAME
 
-	targets $_CHIPNAME.cpu0
-	$_CHIPNAME.dap apsel 0
-
 	set RCC_BASE   0x50000000
 	set GPIO_BASE  0x54004000
 	set I2C_BASE   0x5c002000
@@ -44,38 +45,38 @@ proc pmic_init {} {
 	# timing depending from HSIDIV[1:0] = {0 1 2 3} means hsi_ker_ck = {64 32 16 8} MHz
 	set I2C_TIMING {0x10b17db5 0x00b07cb4 0x00503d58 0x00201d2b}
 
-	mww phys [expr $RCC_BASE + 0x00c] 1; # set HSION = 1 (enable HSI)
-	mww phys [expr $RCC_BASE + 0x0c0] 2; # set I2C46SRC[2:0] = 2 (hsi_ker_ck)
-
-	mww phys [expr $RCC_BASE + 0x208] 4; # set I2C4EN = 1
-	mww phys [expr $RCC_BASE + 0x188] 4; # assert I2C4RST
-	mww phys [expr $RCC_BASE + 0x18c] 4; # deassert I2C4RST
-
-	mww phys [expr $RCC_BASE + 0x210] 1; # set GPIOZEN = 1 (enable GPIOZ)
-	mmw_phys [expr $GPIO_BASE + 0x00] 0x00000a00 0x00000f00; # GPIOZ4/5 AF
-	mmw_phys [expr $GPIO_BASE + 0x04] 0x00000030 0x00000000; # GPIOZ4/5 open drain
-	mmw_phys [expr $GPIO_BASE + 0x08] 0x00000000 0x00000f00; # GPIOZ4/5 low speed
-	mmw_phys [expr $GPIO_BASE + 0x0c] 0x00000000 0x00000f00; # GPIOZ4/5 no pull-up/down
-	mmw_phys [expr $GPIO_BASE + 0x20] 0x00660000 0x00ff0000; # GPIOZ4/5 AF6
-
-	set hsidiv [expr [mrw_phys [expr $RCC_BASE + 0x018]] & 0x3]
-	mww phys [expr $I2C_BASE + 0x010] [lindex $I2C_TIMING $hsidiv]
-	mww phys [expr $I2C_BASE + 0x000] 1; # set PE = 1
-	mww phys [expr $I2C_BASE + 0x004] 0x02043066
-	wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS
-	mww phys [expr $I2C_BASE + 0x028] 0x18
-	wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS
-	mww phys [expr $I2C_BASE + 0x028] 0x0f
-	wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS
-	mww phys [expr $I2C_BASE + 0x028] 0x00
-	wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS
-	mww phys [expr $I2C_BASE + 0x028] 0x6f
-	wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000020) == 0}}; # wait STOPF
-	mww phys [expr $I2C_BASE + 0x01c] 0x00000020
-	mww phys [expr $I2C_BASE + 0x004] 0x01ff14fe
-
-#	mww phys [expr $RCC_BASE + 0x20c] 4; # set I2C4EN = 0 (disable I2C4)
-#	mww phys [expr $RCC_BASE + 0x214] 1; # set GPIOZEN = 0 (disable GPIOZ)
+	$_CHIPNAME.axi mww [expr $RCC_BASE + 0x00c] 1; # set HSION = 1 (enable HSI)
+	$_CHIPNAME.axi mww [expr $RCC_BASE + 0x0c0] 2; # set I2C46SRC[2:0] = 2 (hsi_ker_ck)
+
+	$_CHIPNAME.axi mww [expr $RCC_BASE + 0x208] 4; # set I2C4EN = 1
+	$_CHIPNAME.axi mww [expr $RCC_BASE + 0x188] 4; # assert I2C4RST
+	$_CHIPNAME.axi mww [expr $RCC_BASE + 0x18c] 4; # deassert I2C4RST
+
+	$_CHIPNAME.axi mww [expr $RCC_BASE + 0x210] 1; # set GPIOZEN = 1 (enable GPIOZ)
+	$_CHIPNAME.axi_mmw [expr $GPIO_BASE + 0x00] 0x00000a00 0x00000f00; # GPIOZ4/5 AF
+	$_CHIPNAME.axi_mmw [expr $GPIO_BASE + 0x04] 0x00000030 0x00000000; # GPIOZ4/5 open drain
+	$_CHIPNAME.axi_mmw [expr $GPIO_BASE + 0x08] 0x00000000 0x00000f00; # GPIOZ4/5 low speed
+	$_CHIPNAME.axi_mmw [expr $GPIO_BASE + 0x0c] 0x00000000 0x00000f00; # GPIOZ4/5 no pull-up/down
+	$_CHIPNAME.axi_mmw [expr $GPIO_BASE + 0x20] 0x00660000 0x00ff0000; # GPIOZ4/5 AF6
+
+	set hsidiv [expr [$_CHIPNAME.axi_mrw [expr $RCC_BASE + 0x018]] & 0x3]
+	$_CHIPNAME.axi mww [expr $I2C_BASE + 0x010] [lindex $I2C_TIMING $hsidiv]
+	$_CHIPNAME.axi mww [expr $I2C_BASE + 0x000] 1; # set PE = 1
+	$_CHIPNAME.axi mww [expr $I2C_BASE + 0x004] 0x02043066
+	wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS
+	$_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x18
+	wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS
+	$_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x0f
+	wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS
+	$_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x00
+	wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS
+	$_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x6f
+	wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000020) == 0}}; # wait STOPF
+	$_CHIPNAME.axi mww [expr $I2C_BASE + 0x01c] 0x00000020
+	$_CHIPNAME.axi mww [expr $I2C_BASE + 0x004] 0x01ff14fe
+
+#	$_CHIPNAME.axi mww [expr $RCC_BASE + 0x20c] 4; # set I2C4EN = 0 (disable I2C4)
+#	$_CHIPNAME.axi mww [expr $RCC_BASE + 0x214] 1; # set GPIOZEN = 0 (disable GPIOZ)
 }
 
 # Set bit 16 in TAMP_BACKUP_REGISTER(20) to signal to SPL and TF-A that we
@@ -84,22 +85,16 @@ proc pmic_init {} {
 proc set_pmic_in_backup_reg {} {
 	global _CHIPNAME
 
-	targets $_CHIPNAME.cpu0
-	$_CHIPNAME.dap apsel 0
-
-	mmw_phys 0x5c00a150 0x00010000 0x00000000
+	$_CHIPNAME.axi_mmw 0x5c00a150 0x00010000 0x00000000
 }
 
 proc clear_pmic_in_backup_reg {} {
 	global _CHIPNAME
 
-	targets $_CHIPNAME.cpu0
-	$_CHIPNAME.dap apsel 0
-
-	mmw_phys 0x5c00a150 0x00000000 0x00010000
+	$_CHIPNAME.axi_mmw 0x5c00a150 0x00000000 0x00010000
 }
 
-# Wrap around init/shutdown. Attention: CTRL-C will not invoke shutdown
+# Wrap around init/shutdown. Typing CTRL-C will also invoke shutdown
 rename init _init
 proc init {} {
 	_init
@@ -108,6 +103,8 @@ proc init {} {
 
 rename shutdown _shutdown
 proc shutdown {} {
-	clear_pmic_in_backup_reg
+	catch { clear_pmic_in_backup_reg }
 	_shutdown
 }
+
+reset_config srst_pulls_trst
-- 
2.19.1