meta-st-stm32mp/recipes-devtools/openocd/openocd-stm32mp/0001-M4-visible-rebase-on-b5d2b1224fed-fixes.patch

From 34d7a6bde003b73ca8767581c232675009b8ef41 Mon Sep 17 00:00:00 2001
From: Antonio Borneo <borneo.antonio@gmail.com>
Date: Fri, 13 Mar 2020 16:03:49 +0100
Subject: [PATCH] M4 visible, rebase on b5d2b1224fed, fixes

Change-Id: I7be2f54b197b3cf09813b8dee5cabbf6c99c4f95
Signed-off-by: Antonio Borneo <borneo.antonio@gmail.com>
---
 src/helper/log.h                              |   1 +
 src/jtag/aice/aice_usb.c                      |   4 +-
 src/jtag/core.c                               |   8 +-
 src/jtag/drivers/ft232r.c                     |   2 +-
 src/jtag/drivers/ftdi.c                       |  24 +-
 src/jtag/drivers/kitprog.c                    |   2 +-
 src/jtag/drivers/libusb_helper.c              |  39 +-
 src/jtag/drivers/libusb_helper.h              |   8 +-
 src/jtag/drivers/opendous.c                   |   2 +-
 src/jtag/drivers/openjtag.c                   |   2 +-
 src/jtag/drivers/osbdm.c                      |   2 +-
 src/jtag/drivers/stlink_usb.c                 | 523 +++++++++++++++++-
 .../usb_blaster/ublast2_access_libusb.c       |   8 +-
 src/target/arm_adi_v5.c                       |  28 +
 src/target/arm_adi_v5.h                       |  22 +
 src/target/armv8.c                            |  10 +-
 src/target/armv8_dpm.c                        |   7 +
 src/target/cortex_a.c                         |   4 +-
 tcl/board/stm32mp15x_dk2.cfg                  |  11 +
 tcl/board/stm32mp15x_ev1.cfg                  |  11 +
 tcl/board/stm32mp15x_ev1_jlink_jtag.cfg       |   9 +
 tcl/board/stm32mp15x_ev1_jlink_swd.cfg        |   9 +
 tcl/board/stm32mp15x_ev1_stlink_jtag.cfg      |   9 +
 tcl/board/stm32mp15x_ev1_stlink_swd.cfg       |   9 +
 tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg      |   9 +
 tcl/board/stm32mp15x_ev1_ulink2_swd.cfg       |   9 +
 tcl/target/stm32mp15x.cfg                     | 121 ++++
 27 files changed, 833 insertions(+), 60 deletions(-)

diff --git a/src/helper/log.h b/src/helper/log.h
index eac535deb..b81018d84 100644
--- a/src/helper/log.h
+++ b/src/helper/log.h
@@ -154,6 +154,7 @@ extern int debug_level;
 #define ERROR_WAIT						(-5)
 /* ERROR_TIMEOUT is already taken by winerror.h. */
 #define ERROR_TIMEOUT_REACHED			(-6)
+#define ERROR_OP_NOT_SUPPORTED				(-7)
 
 
 #endif /* OPENOCD_HELPER_LOG_H */
diff --git a/src/jtag/aice/aice_usb.c b/src/jtag/aice/aice_usb.c
index a5cccfef4..442754209 100644
--- a/src/jtag/aice/aice_usb.c
+++ b/src/jtag/aice/aice_usb.c
@@ -2111,7 +2111,7 @@ static int aice_usb_open(struct aice_port_param_s *param)
 	const uint16_t pids[] = { param->pid, 0 };
 	struct libusb_device_handle *devh;
 
-	if (jtag_libusb_open(vids, pids, NULL, &devh) != ERROR_OK)
+	if (jtag_libusb_open(vids, pids, NULL, &devh, NULL) != ERROR_OK)
 		return ERROR_FAIL;
 
 	/* BE ***VERY CAREFUL*** ABOUT MAKING CHANGES IN THIS
@@ -2135,7 +2135,7 @@ static int aice_usb_open(struct aice_port_param_s *param)
 	/* reopen jlink after usb_reset
 	 * on win32 this may take a second or two to re-enumerate */
 	int retval;
-	while ((retval = jtag_libusb_open(vids, pids, NULL, &devh)) != ERROR_OK) {
+	while ((retval = jtag_libusb_open(vids, pids, NULL, &devh, NULL)) != ERROR_OK) {
 		usleep(1000);
 		timeout--;
 		if (!timeout)
diff --git a/src/jtag/core.c b/src/jtag/core.c
index c5011e522..2d0c84205 100644
--- a/src/jtag/core.c
+++ b/src/jtag/core.c
@@ -2020,8 +2020,14 @@ int adapter_resets(int trst, int srst)
 
 		/* adapters without trst signal will eventually use tlr sequence */
 		jtag_add_reset(trst, srst);
+		/*
+		 * The jtag queue is still used for reset by some adapter. Flush it!
+		 * FIXME: To be removed when all adapter drivers will be updated!
+		 */
+		jtag_execute_queue();
 		return ERROR_OK;
-	} else if (transport_is_swd() || transport_is_hla()) {
+	} else if (transport_is_swd() || transport_is_hla() ||
+			   transport_is_dapdirect_swd() || transport_is_dapdirect_jtag()) {
 		if (trst == TRST_ASSERT) {
 			LOG_ERROR("transport %s has no trst signal",
 				get_current_transport()->name);
diff --git a/src/jtag/drivers/ft232r.c b/src/jtag/drivers/ft232r.c
index 4812362a3..c9ed304a8 100644
--- a/src/jtag/drivers/ft232r.c
+++ b/src/jtag/drivers/ft232r.c
@@ -257,7 +257,7 @@ static int ft232r_init(void)
 {
 	uint16_t avids[] = {ft232r_vid, 0};
 	uint16_t apids[] = {ft232r_pid, 0};
-	if (jtag_libusb_open(avids, apids, ft232r_serial_desc, &adapter)) {
+	if (jtag_libusb_open(avids, apids, ft232r_serial_desc, &adapter, NULL)) {
 		LOG_ERROR("ft232r not found: vid=%04x, pid=%04x, serial=%s\n",
 			ft232r_vid, ft232r_pid, (ft232r_serial_desc == NULL) ? "[any]" : ft232r_serial_desc);
 		return ERROR_JTAG_INIT_FAILED;
diff --git a/src/jtag/drivers/ftdi.c b/src/jtag/drivers/ftdi.c
index 121cb469f..3a685b8fc 100644
--- a/src/jtag/drivers/ftdi.c
+++ b/src/jtag/drivers/ftdi.c
@@ -524,17 +524,19 @@ static int ftdi_reset(int trst, int srst)
 
 	LOG_DEBUG_IO("reset trst: %i srst %i", trst, srst);
 
-	if (trst == 1) {
-		if (sig_ntrst)
-			ftdi_set_signal(sig_ntrst, '0');
-		else
-			LOG_ERROR("Can't assert TRST: nTRST signal is not defined");
-	} else if (sig_ntrst && jtag_get_reset_config() & RESET_HAS_TRST &&
-			trst == 0) {
-		if (jtag_get_reset_config() & RESET_TRST_OPEN_DRAIN)
-			ftdi_set_signal(sig_ntrst, 'z');
-		else
-			ftdi_set_signal(sig_ntrst, '1');
+	if (!swd_mode) {
+		if (trst == 1) {
+			if (sig_ntrst)
+				ftdi_set_signal(sig_ntrst, '0');
+			else
+				LOG_ERROR("Can't assert TRST: nTRST signal is not defined");
+		} else if (sig_ntrst && jtag_get_reset_config() & RESET_HAS_TRST &&
+				trst == 0) {
+			if (jtag_get_reset_config() & RESET_TRST_OPEN_DRAIN)
+				ftdi_set_signal(sig_ntrst, 'z');
+			else
+				ftdi_set_signal(sig_ntrst, '1');
+		}
 	}
 
 	if (srst == 1) {
diff --git a/src/jtag/drivers/kitprog.c b/src/jtag/drivers/kitprog.c
index 0c1e74c42..28ed7ac61 100644
--- a/src/jtag/drivers/kitprog.c
+++ b/src/jtag/drivers/kitprog.c
@@ -280,7 +280,7 @@ static int kitprog_usb_open(void)
 	const uint16_t pids[] = { PID, 0 };
 
 	if (jtag_libusb_open(vids, pids, kitprog_serial,
-			&kitprog_handle->usb_handle) != ERROR_OK) {
+			&kitprog_handle->usb_handle, NULL) != ERROR_OK) {
 		LOG_ERROR("Failed to open or find the device");
 		return ERROR_FAIL;
 	}
diff --git a/src/jtag/drivers/libusb_helper.c b/src/jtag/drivers/libusb_helper.c
index 5a8129cb9..fbbfb4114 100644
--- a/src/jtag/drivers/libusb_helper.c
+++ b/src/jtag/drivers/libusb_helper.c
@@ -58,7 +58,7 @@ static int jtag_libusb_error(int err)
 	}
 }
 
-static bool jtag_libusb_match(struct libusb_device_descriptor *dev_desc,
+static bool jtag_libusb_match_ids(struct libusb_device_descriptor *dev_desc,
 		const uint16_t vids[], const uint16_t pids[])
 {
 	for (unsigned i = 0; vids[i]; i++) {
@@ -123,9 +123,40 @@ static bool string_descriptor_equal(libusb_device_handle *device, uint8_t str_in
 	return matched;
 }
 
+static bool jtag_libusb_match_serial(libusb_device_handle *device,
+		struct libusb_device_descriptor *dev_desc, const char *serial,
+		adapter_get_alternate_serial_fn adapter_get_alternate_serial)
+{
+	if (string_descriptor_equal(device, dev_desc->iSerialNumber, serial))
+		return true;
+
+	/* check the alternate serial helper */
+	if (!adapter_get_alternate_serial)
+		return false;
+
+	/* get the alternate serial */
+	char *alternate_serial = adapter_get_alternate_serial(device, dev_desc);
+
+	/* check possible failures */
+	if (alternate_serial == NULL)
+		return false;
+
+	/* then compare and free the alternate serial */
+	bool match = false;
+	if (strcmp(serial, alternate_serial) == 0)
+		match = true;
+	else
+		LOG_DEBUG("Device alternate serial number '%s' doesn't match requested serial '%s'",
+				alternate_serial, serial);
+
+	free(alternate_serial);
+	return match;
+}
+
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		const char *serial,
-		struct libusb_device_handle **out)
+		struct libusb_device_handle **out,
+		adapter_get_alternate_serial_fn adapter_get_alternate_serial)
 {
 	int cnt, idx, errCode;
 	int retval = ERROR_FAIL;
@@ -143,7 +174,7 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		if (libusb_get_device_descriptor(devs[idx], &dev_desc) != 0)
 			continue;
 
-		if (!jtag_libusb_match(&dev_desc, vids, pids))
+		if (!jtag_libusb_match_ids(&dev_desc, vids, pids))
 			continue;
 
 		if (jtag_usb_get_location() && !jtag_libusb_location_equal(devs[idx]))
@@ -159,7 +190,7 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 
 		/* Device must be open to use libusb_get_string_descriptor_ascii. */
 		if (serial != NULL &&
-				!string_descriptor_equal(libusb_handle, dev_desc.iSerialNumber, serial)) {
+				!jtag_libusb_match_serial(libusb_handle, &dev_desc, serial, adapter_get_alternate_serial)) {
 			serial_mismatch = true;
 			libusb_close(libusb_handle);
 			continue;
diff --git a/src/jtag/drivers/libusb_helper.h b/src/jtag/drivers/libusb_helper.h
index 46e4954e7..74bb23c52 100644
--- a/src/jtag/drivers/libusb_helper.h
+++ b/src/jtag/drivers/libusb_helper.h
@@ -22,9 +22,15 @@
 
 #include <libusb.h>
 
+/* this callback should return a non NULL value only when the serial could not
+ * be retrieved by the standard 'libusb_get_string_descriptor_ascii' */
+typedef char * (*adapter_get_alternate_serial_fn)(libusb_device_handle *device,
+		struct libusb_device_descriptor *dev_desc);
+
 int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		const char *serial,
-		struct libusb_device_handle **out);
+		struct libusb_device_handle **out,
+		adapter_get_alternate_serial_fn adapter_get_alternate_serial);
 void jtag_libusb_close(struct libusb_device_handle *dev);
 int jtag_libusb_control_transfer(struct libusb_device_handle *dev,
 		uint8_t requestType, uint8_t request, uint16_t wValue,
diff --git a/src/jtag/drivers/opendous.c b/src/jtag/drivers/opendous.c
index 7298a2a10..6812ef649 100644
--- a/src/jtag/drivers/opendous.c
+++ b/src/jtag/drivers/opendous.c
@@ -715,7 +715,7 @@ struct opendous_jtag *opendous_usb_open(void)
 	struct opendous_jtag *result;
 
 	struct libusb_device_handle *devh;
-	if (jtag_libusb_open(opendous_probe->VID, opendous_probe->PID, NULL, &devh) != ERROR_OK)
+	if (jtag_libusb_open(opendous_probe->VID, opendous_probe->PID, NULL, &devh, NULL) != ERROR_OK)
 		return NULL;
 
 	jtag_libusb_set_configuration(devh, 0);
diff --git a/src/jtag/drivers/openjtag.c b/src/jtag/drivers/openjtag.c
index 6131df914..7eab5c130 100644
--- a/src/jtag/drivers/openjtag.c
+++ b/src/jtag/drivers/openjtag.c
@@ -449,7 +449,7 @@ static int openjtag_init_cy7c65215(void)
 	int ret;
 
 	usbh = NULL;
-	ret = jtag_libusb_open(cy7c65215_vids, cy7c65215_pids, NULL, &usbh);
+	ret = jtag_libusb_open(cy7c65215_vids, cy7c65215_pids, NULL, &usbh, NULL);
 	if (ret != ERROR_OK) {
 		LOG_ERROR("unable to open cy7c65215 device");
 		goto err;
diff --git a/src/jtag/drivers/osbdm.c b/src/jtag/drivers/osbdm.c
index aea126d0d..dc236660e 100644
--- a/src/jtag/drivers/osbdm.c
+++ b/src/jtag/drivers/osbdm.c
@@ -374,7 +374,7 @@ static int osbdm_flush(struct osbdm *osbdm, struct queue *queue)
 static int osbdm_open(struct osbdm *osbdm)
 {
 	(void)memset(osbdm, 0, sizeof(*osbdm));
-	if (jtag_libusb_open(osbdm_vid, osbdm_pid, NULL, &osbdm->devh) != ERROR_OK)
+	if (jtag_libusb_open(osbdm_vid, osbdm_pid, NULL, &osbdm->devh, NULL) != ERROR_OK)
 		return ERROR_FAIL;
 
 	if (libusb_claim_interface(osbdm->devh, 0) != ERROR_OK)
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index ca7a4df4e..6c0601d0e 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -47,6 +47,8 @@
 #define USE_LIBUSB_ASYNCIO
 #endif
 
+#define STLINK_SERIAL_LEN 24
+
 #define ENDPOINT_IN  0x80
 #define ENDPOINT_OUT 0x00
 
@@ -83,11 +85,22 @@
 #define STLINK_MAX_RW8		(64)
 #define STLINKV3_MAX_RW8	(512)
 
+/*
+ * FIXME: this definition + comment should be moved in a generic header file
+ * ARM IHI 0031E: TAR Automatic address increment is only guaranteed to
+ * operate on the 10 least significant bits of the address
+ */
+#define TAR_AUTOINCR_BLOCK      (1 << 10)
+
 /* "WAIT" responses will be retried (with exponential backoff) at
  * most this many times before failing to caller.
  */
 #define MAX_WAIT_RETRIES 8
 
+/* HLA is currently limited at AP#0 and no control on CSW */
+#define STLINK_HLA_AP_NUM       0
+#define STLINK_HLA_CSW          0
+
 enum stlink_jtag_api_version {
 	STLINK_JTAG_API_V1 = 1,
 	STLINK_JTAG_API_V2,
@@ -275,6 +288,10 @@ struct stlink_usb_handle_s {
 #define STLINK_DEBUG_APIV2_INIT_AP         0x4B
 #define STLINK_DEBUG_APIV2_CLOSE_AP_DBG    0x4C
 
+#define STLINK_DEBUG_WRITEMEM_32BIT_NO_ADDR_INC         0x50
+
+#define STLINK_DEBUG_READMEM_32BIT_NO_ADDR_INC          0x54
+
 #define STLINK_APIV3_SET_COM_FREQ           0x61
 #define STLINK_APIV3_GET_COM_FREQ           0x62
 
@@ -321,6 +338,9 @@ enum stlink_mode {
 
 /* aliases */
 #define STLINK_F_HAS_TARGET_VOLT        STLINK_F_HAS_TRACE
+#define STLINK_F_HAS_MEM_WR_NO_INC      STLINK_F_HAS_MEM_16BIT
+#define STLINK_F_HAS_MEM_RD_NO_INC      STLINK_F_HAS_DPBANKSEL
+#define STLINK_F_HAS_CSW                STLINK_F_HAS_DPBANKSEL
 
 struct speed_map {
 	int speed;
@@ -345,7 +365,6 @@ static const struct speed_map stlink_khz_to_speed_map_swd[] = {
 
 /* JTAG clock speed */
 static const struct speed_map stlink_khz_to_speed_map_jtag[] = {
-	{18000, 2},
 	{9000,  4},
 	{4500,  8},
 	{2250, 16},
@@ -1023,6 +1042,7 @@ static int stlink_usb_version(void *handle)
 			flags |= STLINK_F_QUIRK_JTAG_DP_READ;
 
 		/* API to read/write memory at 16 bit from J26 */
+		/* API to write memory without address increment from J26 */
 		if (h->version.jtag >= 26)
 			flags |= STLINK_F_HAS_MEM_16BIT;
 
@@ -1031,6 +1051,8 @@ static int stlink_usb_version(void *handle)
 			flags |= STLINK_F_HAS_AP_INIT;
 
 		/* Banked regs (DPv1 & DPv2) support from V2J32 */
+		/* API to read memory without address increment from V2J32 */
+		/* Memory R/W supports CSW from V2J32 */
 		if (h->version.jtag >= 32)
 			flags |= STLINK_F_HAS_DPBANKSEL;
 
@@ -1052,12 +1074,15 @@ static int stlink_usb_version(void *handle)
 		flags |= STLINK_F_HAS_DAP_REG;
 
 		/* API to read/write memory at 16 bit */
+		/* API to write memory without address increment */
 		flags |= STLINK_F_HAS_MEM_16BIT;
 
 		/* API required to init AP before any AP access */
 		flags |= STLINK_F_HAS_AP_INIT;
 
 		/* Banked regs (DPv1 & DPv2) support from V3J2 */
+		/* API to read memory without address increment from V3J2 */
+		/* Memory R/W supports CSW from V3J2 */
 		if (h->version.jtag >= 2)
 			flags |= STLINK_F_HAS_DPBANKSEL;
 
@@ -2086,8 +2111,8 @@ static int stlink_usb_get_rw_status(void *handle)
 }
 
 /** */
-static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
-			  uint8_t *buffer)
+static int stlink_usb_read_mem8(void *handle, uint8_t ap_num, uint32_t csw,
+		uint32_t addr, uint16_t len, uint8_t *buffer)
 {
 	int res;
 	uint16_t read_len = len;
@@ -2109,6 +2134,9 @@ static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
 	h->cmdidx += 4;
 	h_u16_to_le(h->cmdbuf+h->cmdidx, len);
 	h->cmdidx += 2;
+	h->cmdbuf[h->cmdidx++] = ap_num;
+	h_u24_to_le(h->cmdbuf+h->cmdidx, csw >> 8);
+	h->cmdidx += 3;
 
 	/* we need to fix read length for single bytes */
 	if (read_len == 1)
@@ -2125,8 +2153,8 @@ static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
 }
 
 /** */
-static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
-			   const uint8_t *buffer)
+static int stlink_usb_write_mem8(void *handle, uint8_t ap_num, uint32_t csw,
+		uint32_t addr, uint16_t len, const uint8_t *buffer)
 {
 	int res;
 	struct stlink_usb_handle_s *h = handle;
@@ -2147,6 +2175,9 @@ static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
 	h->cmdidx += 4;
 	h_u16_to_le(h->cmdbuf+h->cmdidx, len);
 	h->cmdidx += 2;
+	h->cmdbuf[h->cmdidx++] = ap_num;
+	h_u24_to_le(h->cmdbuf+h->cmdidx, csw >> 8);
+	h->cmdidx += 3;
 
 	res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
 
@@ -2157,8 +2188,8 @@ static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
 }
 
 /** */
-static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len,
-			  uint8_t *buffer)
+static int stlink_usb_read_mem16(void *handle, uint8_t ap_num, uint32_t csw,
+		uint32_t addr, uint16_t len, uint8_t *buffer)
 {
 	int res;
 	struct stlink_usb_handle_s *h = handle;
@@ -2182,6 +2213,9 @@ static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len,
 	h->cmdidx += 4;
 	h_u16_to_le(h->cmdbuf+h->cmdidx, len);
 	h->cmdidx += 2;
+	h->cmdbuf[h->cmdidx++] = ap_num;
+	h_u24_to_le(h->cmdbuf+h->cmdidx, csw >> 8);
+	h->cmdidx += 3;
 
 	res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
 
@@ -2194,8 +2228,8 @@ static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len,
 }
 
 /** */
-static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len,
-			   const uint8_t *buffer)
+static int stlink_usb_write_mem16(void *handle, uint8_t ap_num, uint32_t csw,
+		uint32_t addr, uint16_t len, const uint8_t *buffer)
 {
 	int res;
 	struct stlink_usb_handle_s *h = handle;
@@ -2219,6 +2253,9 @@ static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len,
 	h->cmdidx += 4;
 	h_u16_to_le(h->cmdbuf+h->cmdidx, len);
 	h->cmdidx += 2;
+	h->cmdbuf[h->cmdidx++] = ap_num;
+	h_u24_to_le(h->cmdbuf+h->cmdidx, csw >> 8);
+	h->cmdidx += 3;
 
 	res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
 
@@ -2229,8 +2266,8 @@ static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len,
 }
 
 /** */
-static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
-			  uint8_t *buffer)
+static int stlink_usb_read_mem32(void *handle, uint8_t ap_num, uint32_t csw,
+		uint32_t addr, uint16_t len, uint8_t *buffer)
 {
 	int res;
 	struct stlink_usb_handle_s *h = handle;
@@ -2251,6 +2288,9 @@ static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
 	h->cmdidx += 4;
 	h_u16_to_le(h->cmdbuf+h->cmdidx, len);
 	h->cmdidx += 2;
+	h->cmdbuf[h->cmdidx++] = ap_num;
+	h_u24_to_le(h->cmdbuf+h->cmdidx, csw >> 8);
+	h->cmdidx += 3;
 
 	res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
 
@@ -2263,8 +2303,8 @@ static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
 }
 
 /** */
-static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
-			   const uint8_t *buffer)
+static int stlink_usb_write_mem32(void *handle, uint8_t ap_num, uint32_t csw,
+		uint32_t addr, uint16_t len, const uint8_t *buffer)
 {
 	int res;
 	struct stlink_usb_handle_s *h = handle;
@@ -2285,6 +2325,9 @@ static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
 	h->cmdidx += 4;
 	h_u16_to_le(h->cmdbuf+h->cmdidx, len);
 	h->cmdidx += 2;
+	h->cmdbuf[h->cmdidx++] = ap_num;
+	h_u24_to_le(h->cmdbuf+h->cmdidx, csw >> 8);
+	h->cmdidx += 3;
 
 	res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
 
@@ -2294,6 +2337,80 @@ static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len,
 	return stlink_usb_get_rw_status(handle);
 }
 
+static int stlink_usb_read_mem32_noaddrinc(void *handle, uint8_t ap_num, uint32_t csw,
+		uint32_t addr, uint16_t len, uint8_t *buffer)
+{
+	int res;
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	if (!(h->version.flags & STLINK_F_HAS_MEM_RD_NO_INC))
+		return ERROR_COMMAND_NOTFOUND;
+
+	/* data must be a multiple of 4 and word aligned */
+	if (len % 4 || addr % 4) {
+		LOG_DEBUG("Invalid data alignment");
+		return ERROR_TARGET_UNALIGNED_ACCESS;
+	}
+
+	stlink_usb_init_buffer(handle, h->rx_ep, len);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT_NO_ADDR_INC;
+	h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+	h->cmdidx += 4;
+	h_u16_to_le(h->cmdbuf+h->cmdidx, len);
+	h->cmdidx += 2;
+	h->cmdbuf[h->cmdidx++] = ap_num;
+	h_u24_to_le(h->cmdbuf+h->cmdidx, csw >> 8);
+	h->cmdidx += 3;
+
+	res = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
+	if (res != ERROR_OK)
+		return res;
+
+	memcpy(buffer, h->databuf, len);
+
+	return stlink_usb_get_rw_status(handle);
+}
+
+static int stlink_usb_write_mem32_noaddrinc(void *handle, uint8_t ap_num, uint32_t csw,
+		uint32_t addr, uint16_t len, const uint8_t *buffer)
+{
+	int res;
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	if (!(h->version.flags & STLINK_F_HAS_MEM_WR_NO_INC))
+		return ERROR_COMMAND_NOTFOUND;
+
+	/* data must be a multiple of 4 and word aligned */
+	if (len % 4 || addr % 4) {
+		LOG_DEBUG("Invalid data alignment");
+		return ERROR_TARGET_UNALIGNED_ACCESS;
+	}
+
+	stlink_usb_init_buffer(handle, h->tx_ep, len);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT_NO_ADDR_INC;
+	h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+	h->cmdidx += 4;
+	h_u16_to_le(h->cmdbuf+h->cmdidx, len);
+	h->cmdidx += 2;
+	h->cmdbuf[h->cmdidx++] = ap_num;
+	h_u24_to_le(h->cmdbuf+h->cmdidx, csw >> 8);
+	h->cmdidx += 3;
+
+	res = stlink_usb_xfer_noerrcheck(handle, buffer, len);
+	if (res != ERROR_OK)
+		return res;
+
+	return stlink_usb_get_rw_status(handle);
+}
+
 static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t address)
 {
 	uint32_t max_tar_block = (tar_autoincr_block - ((tar_autoincr_block - 1) & address));
@@ -2302,8 +2419,8 @@ static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t addr
 	return max_tar_block;
 }
 
-static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
-		uint32_t count, uint8_t *buffer)
+static int stlink_usb_read_ap_mem(void *handle, uint8_t ap_num, uint32_t csw,
+		uint32_t addr, uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int retval = ERROR_OK;
 	uint32_t bytes_remaining;
@@ -2350,7 +2467,7 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 			if (addr & (size - 1)) {
 
 				uint32_t head_bytes = size - (addr & (size - 1));
-				retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
+				retval = stlink_usb_read_mem8(handle, ap_num, csw, addr, head_bytes, buffer);
 				if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 					usleep((1<<retries++) * 1000);
 					continue;
@@ -2364,13 +2481,13 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 			}
 
 			if (bytes_remaining & (size - 1))
-				retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
+				retval = stlink_usb_read_ap_mem(handle, ap_num, csw, addr, 1, bytes_remaining, buffer);
 			else if (size == 2)
-				retval = stlink_usb_read_mem16(handle, addr, bytes_remaining, buffer);
+				retval = stlink_usb_read_mem16(handle, ap_num, csw, addr, bytes_remaining, buffer);
 			else
-				retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
+				retval = stlink_usb_read_mem32(handle, ap_num, csw, addr, bytes_remaining, buffer);
 		} else
-			retval = stlink_usb_read_mem8(handle, addr, bytes_remaining, buffer);
+			retval = stlink_usb_read_mem8(handle, ap_num, csw, addr, bytes_remaining, buffer);
 
 		if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 			usleep((1<<retries++) * 1000);
@@ -2387,8 +2504,15 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 	return retval;
 }
 
-static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
-		uint32_t count, const uint8_t *buffer)
+static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
+		uint32_t count, uint8_t *buffer)
+{
+	return stlink_usb_read_ap_mem(handle, STLINK_HLA_AP_NUM, STLINK_HLA_CSW,
+								  addr, size, count, buffer);
+}
+
+static int stlink_usb_write_ap_mem(void *handle, uint8_t ap_num, uint32_t csw,
+		uint32_t addr, uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int retval = ERROR_OK;
 	uint32_t bytes_remaining;
@@ -2435,7 +2559,7 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 			if (addr & (size - 1)) {
 
 				uint32_t head_bytes = size - (addr & (size - 1));
-				retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
+				retval = stlink_usb_write_mem8(handle, ap_num, csw, addr, head_bytes, buffer);
 				if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 					usleep((1<<retries++) * 1000);
 					continue;
@@ -2449,14 +2573,14 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 			}
 
 			if (bytes_remaining & (size - 1))
-				retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
+				retval = stlink_usb_write_ap_mem(handle, ap_num, csw, addr, 1, bytes_remaining, buffer);
 			else if (size == 2)
-				retval = stlink_usb_write_mem16(handle, addr, bytes_remaining, buffer);
+				retval = stlink_usb_write_mem16(handle, ap_num, csw, addr, bytes_remaining, buffer);
 			else
-				retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
+				retval = stlink_usb_write_mem32(handle, ap_num, csw, addr, bytes_remaining, buffer);
 
 		} else
-			retval = stlink_usb_write_mem8(handle, addr, bytes_remaining, buffer);
+			retval = stlink_usb_write_mem8(handle, ap_num, csw, addr, bytes_remaining, buffer);
 		if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 			usleep((1<<retries++) * 1000);
 			continue;
@@ -2472,6 +2596,13 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 	return retval;
 }
 
+static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
+		uint32_t count, const uint8_t *buffer)
+{
+	return stlink_usb_write_ap_mem(handle, STLINK_HLA_AP_NUM, STLINK_HLA_CSW,
+								   addr, size, count, buffer);
+}
+
 /** */
 static int stlink_usb_override_target(const char *targetname)
 {
@@ -2745,6 +2876,85 @@ static int stlink_usb_close(void *handle)
 	return ERROR_OK;
 }
 
+/* Compute ST-Link serial number from the device descriptor
+ * this function will help to work-around a bug in old ST-Link/V2 DFU
+ * the buggy DFU returns an incorrect serial in the USB descriptor
+ * example for the following serial "57FF72067265575742132067"
+ *  - the correct descriptor serial is:
+ *    0x32, 0x03, 0x35, 0x00, 0x37, 0x00, 0x46, 0x00, 0x46, 0x00, 0x37, 0x00, 0x32, 0x00 ...
+ *    this contains the length (0x32 = 50), the type (0x3 = DT_STRING) and the serial in unicode format
+ *    the serial part is: 0x0035, 0x0037, 0x0046, 0x0046, 0x0037, 0x0032 ... >>  57FF72 ...
+ *    this format could be read correctly by 'libusb_get_string_descriptor_ascii'
+ *    so this case is managed by libusb_helper::string_descriptor_equal
+ *  - the buggy DFU is not doing any unicode conversion and returns a raw serial data in the descriptor
+ *    0x1a, 0x03, 0x57, 0x00, 0xFF, 0x00, 0x72, 0x00 ...
+ *            >>    57          FF          72       ...
+ *    based on the length (0x1a = 26) we could easily decide if we have to fixup the serial
+ *    and then we have just to convert the raw data into printable characters using sprintf
+ */
+char *stlink_usb_get_alternate_serial(libusb_device_handle *device,
+		struct libusb_device_descriptor *dev_desc)
+{
+	int usb_retval;
+	unsigned char desc_serial[(STLINK_SERIAL_LEN + 1) * 2];
+
+	if (dev_desc->iSerialNumber == 0)
+		return NULL;
+
+	/* get the LANGID from String Descriptor Zero */
+	usb_retval = libusb_get_string_descriptor(device, 0, 0, desc_serial,
+			sizeof(desc_serial));
+
+	if (usb_retval < LIBUSB_SUCCESS) {
+		LOG_ERROR("libusb_get_string_descriptor() failed: %s(%d)",
+				libusb_error_name(usb_retval), usb_retval);
+		return NULL;
+	} else if (usb_retval < 4) {
+		/* the size should be least 4 bytes to contain a minimum of 1 supported LANGID */
+		LOG_ERROR("could not get the LANGID");
+		return NULL;
+	}
+
+	uint32_t langid = desc_serial[2] | (desc_serial[3] << 8);
+
+	/* get the serial */
+	usb_retval = libusb_get_string_descriptor(device, dev_desc->iSerialNumber,
+			langid, desc_serial, sizeof(desc_serial));
+
+	unsigned char len = desc_serial[0];
+
+	if (usb_retval < LIBUSB_SUCCESS) {
+		LOG_ERROR("libusb_get_string_descriptor() failed: %s(%d)",
+				libusb_error_name(usb_retval), usb_retval);
+		return NULL;
+	} else if (desc_serial[1] != LIBUSB_DT_STRING || len > usb_retval) {
+		LOG_ERROR("invalid string in ST-LINK USB serial descriptor");
+		return NULL;
+	}
+
+	if (len == ((STLINK_SERIAL_LEN + 1) * 2)) {
+		/* good ST-Link adapter, this case is managed by
+		 * libusb::libusb_get_string_descriptor_ascii */
+		return NULL;
+	} else if (len != ((STLINK_SERIAL_LEN / 2 + 1) * 2)) {
+		LOG_ERROR("unexpected serial length (%d) in descriptor", len);
+		return NULL;
+	}
+
+	/* else (len == 26) => buggy ST-Link */
+
+	char *alternate_serial = malloc((STLINK_SERIAL_LEN + 1) * sizeof(char));
+	if (alternate_serial == NULL)
+		return NULL;
+
+	for (unsigned int i = 0; i < STLINK_SERIAL_LEN; i += 2)
+		sprintf(alternate_serial + i, "%02X", desc_serial[i + 2]);
+
+	alternate_serial[STLINK_SERIAL_LEN] = '\0';
+
+	return alternate_serial;
+}
+
 /** */
 static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 {
@@ -2778,7 +2988,8 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 	  in order to become operational.
 	 */
 	do {
-		if (jtag_libusb_open(param->vid, param->pid, param->serial, &h->fd) != ERROR_OK) {
+		if (jtag_libusb_open(param->vid, param->pid, param->serial,
+				&h->fd, stlink_usb_get_alternate_serial) != ERROR_OK) {
 			LOG_ERROR("open failed");
 			goto error_open;
 		}
@@ -2910,7 +3121,7 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 	h->max_mem_packet = (1 << 10);
 
 	uint8_t buffer[4];
-	err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
+	err = stlink_usb_read_mem32(h, STLINK_HLA_AP_NUM, STLINK_HLA_CSW, CPUID, 4, buffer);
 	if (err == ERROR_OK) {
 		uint32_t cpuid = le_to_h_u32(buffer);
 		int i = (cpuid >> 4) & 0xf;
@@ -3452,6 +3663,249 @@ static int stlink_dap_op_run(struct adiv5_dap *dap)
 	return saved_retval;
 }
 
+/*
+ * Explanation on CSW hack used for firmware V2J24~V2J31 and V3J1
+ *
+ * The API of these firmware versions don't provide a way to specify directly
+ * the value of CSW used for mem_ap accesses. The CSW value used by the
+ * firmware is not under user control. The lack of specific API would force
+ * the use of inefficient memory access through low-level AP register API.
+ *
+ * The CSW value internally computed by the firmware is cached by the firmware
+ * itself to minimize the writes to register MEM_AP_REG_CSW, writing it only
+ * to updated a new CSW value. The update is only triggered by a change in the
+ * CSW fields "size" and "addrinc" (note: with these firmware versions we only
+ * use "addrinc = true").
+ *
+ * With the command STLINK_DEBUG_APIV2_WRITE_DAP_REG we can write the register
+ * MEM_AP_REG_CSW; the firmware does not intercept this write and its internal
+ * cached CSW value get out-of-sync wrt the value in register MEM_AP_REG_CSW.
+ * The hack rely on this mismatch between firmware cache and MEM_AP_REG_CSW.
+ * Given the "size" of data we plan to use for memory access, we execute:
+ * 1) a dummy memory read of "size" to let the firmware compute a CSW value
+ *    coherent with "size". This will update the internal CSW cache and
+ *    (eventually) the register MEM_AP_REG_CSW;
+ * 2) a write of the desired "user" CSW value in MEM_AP_REG_CSW, including the
+ *    field "addrinc" ("size" has not to be changed wrt point 1);
+ * 3) a set of memory read/write commands of the same "size".
+ * While executing 3), the firmware will not modify the content of register
+ * MEM_AP_REG_CSW because accordingly to its cached value there is no need to
+ * update it!
+ * The memory accesses in 3) would then be executed with the desired CSW value
+ * programmed in 2).
+ *
+ * To simplify the code and only use one "size", the case of unaligned access
+ * is converted to a 8 bit access.
+ *
+ * Because of the available API, in step 3) we will use the normal access (with
+ * address auto-increment) also for addrinc==false, and this triggers another
+ * problem. The auto-increment of register MEM_AP_REG_TAR is guaranteed only
+ * on the 10 least significant bits. The firmware will update MEM_AP_REG_TAR
+ * every time the 10 bit increment is crossed, to avoid any potential
+ * wrap-around. This disrupts the address in MEM_AP_REG_TAR, that should
+ * instead be constant during the whole memory access. To prevent this, we
+ * split the memory access in chunks whose length cannot cause the 10 bits
+ * wrap-around so the firmware will not touch MEM_AP_REG_TAR.
+ *
+ * For write case only, the firmware operates per USB blocks, so it re-programs
+ * MEM_AP_REG_TAR every 64 bytes (on stlink/v1) or 512 bytes (on stlik/v3) and
+ * this limits the maximum size of each write chunk when addrinc==false.
+ */
+static int stlink_dap_hack_csw(struct adiv5_ap *ap, uint32_t size, bool addrinc)
+{
+	uint32_t csw;
+	uint8_t dummy[4];
+
+	switch (size) {
+		case 1:
+			csw = CSW_8BIT;
+			break;
+		case 2:
+			csw = CSW_16BIT;
+			break;
+		case 4:
+			/*
+			 * ST-Link sets autoinc only in 32 bits mode
+			 *
+			 * ARM IHI 0031D: Note:
+			 * It is IMPLEMENTATION DEFINED whether a MEM-AP supports transfer sizes other than Word. If a
+			 * MEM-AP only supports word transfers and Increment single is selected, the TAR always
+			 * increments by four after a successful DRW transaction.
+			 */
+			csw = addrinc ? (CSW_32BIT | CSW_ADDRINC_SINGLE) : CSW_32BIT;
+			break;
+		default:
+			return ERROR_FAIL;
+	}
+	csw |= ap->csw_default;
+
+	/* This mem read will change CSW. Ignore errors */
+	stlink_usb_read_ap_mem(stlink_dap_handle, ap->ap_num, STLINK_HLA_CSW, 0x00000000, size, 1, dummy);
+
+	return dap_queue_ap_write(ap, MEM_AP_REG_CSW, csw);
+}
+
+static int stlink_dap_hack_csw_mem_read(struct adiv5_ap *ap, uint8_t *buffer,
+		uint32_t size, uint32_t count, uint32_t address, bool addrinc)
+{
+	int retval;
+	uint32_t bytes_remaining, max_count;
+
+	/* unaligned access could be split in mix 32/16/8 bits. Force 8-bis only */
+	if ((size == 4 && address & 3) ||
+		(size == 2 && address & 1) ||
+		(size == 2 && !(stlink_dap_handle->version.flags & STLINK_F_HAS_MEM_16BIT))) {
+		count *= size;
+		size = 1;
+	}
+
+	retval = stlink_dap_hack_csw(ap, size, addrinc);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (addrinc)
+		return stlink_usb_read_ap_mem(stlink_dap_handle, ap->ap_num,
+			STLINK_HLA_CSW, address, size, count, buffer);
+
+	count *= size;
+	max_count = TAR_AUTOINCR_BLOCK - (address & (TAR_AUTOINCR_BLOCK - 1));
+	while (count) {
+		bytes_remaining = (count < max_count) ? count : max_count;
+
+		retval = stlink_usb_read_mem32(stlink_dap_handle, ap->ap_num,
+			STLINK_HLA_CSW, address, bytes_remaining, buffer);
+		if (retval != ERROR_OK)
+			return retval;
+
+		buffer += bytes_remaining;
+		count -= bytes_remaining;
+	}
+	return ERROR_OK;
+}
+
+static int stlink_dap_op_ap_mem_read(struct adiv5_ap *ap, uint8_t *buffer,
+		uint32_t size, uint32_t count, uint32_t address, bool addrinc)
+{
+	int retval;
+	uint32_t bytes_remaining;
+
+	if (!addrinc && size != 4)
+		return ERROR_OP_NOT_SUPPORTED;
+
+	retval = stlink_dap_op_run(ap->dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = stlink_dap_open_ap(ap->ap_num);
+	if (retval != ERROR_OK)
+		return retval;
+
+	dap_invalidate_cache(ap->dap);
+
+	if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_CSW))
+		return stlink_dap_hack_csw_mem_read(ap, buffer, size, count, address, addrinc);
+
+	if (addrinc)
+		return stlink_usb_read_ap_mem(stlink_dap_handle, ap->ap_num,
+			ap->csw_default, address, size, count, buffer);
+
+	count *= size;
+	while (count) {
+		bytes_remaining = (count < STLINK_DATA_SIZE) ? count : STLINK_DATA_SIZE;
+
+		retval = stlink_usb_read_mem32_noaddrinc(stlink_dap_handle, ap->ap_num,
+				ap->csw_default, address, bytes_remaining, buffer);
+		if (retval != ERROR_OK)
+			return retval;
+
+		buffer += bytes_remaining;
+		count -= bytes_remaining;
+	}
+	return ERROR_OK;
+}
+
+static int stlink_dap_hack_csw_mem_write(struct adiv5_ap *ap, const uint8_t *buffer,
+		uint32_t size, uint32_t count, uint32_t address, bool addrinc)
+{
+	int retval;
+	uint32_t bytes_remaining, max_count;
+
+	/* unaligned access could be split in mix 32/16/8 bits. Force 8-bis only */
+	if ((size == 4 && address & 3) ||
+		(size == 2 && address & 1) ||
+		(size == 2 && !(stlink_dap_handle->version.flags & STLINK_F_HAS_MEM_16BIT))) {
+		count *= size;
+		size = 1;
+	}
+
+	retval = stlink_dap_hack_csw(ap, size, addrinc);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (addrinc)
+		return stlink_usb_write_ap_mem(stlink_dap_handle, ap->ap_num,
+			STLINK_HLA_CSW, address, size, count, buffer);
+
+	count *= size;
+	max_count = TAR_AUTOINCR_BLOCK - (address & (TAR_AUTOINCR_BLOCK - 1));
+	if (max_count > stlink_usb_block(&stlink_dap_handle))
+		max_count = stlink_usb_block(&stlink_dap_handle);
+
+	while (count) {
+		bytes_remaining = (count < max_count) ? count : max_count;
+
+		retval = stlink_usb_write_mem32(stlink_dap_handle, ap->ap_num,
+			STLINK_HLA_CSW, address, bytes_remaining, buffer);
+		if (retval != ERROR_OK)
+			return retval;
+
+		buffer += bytes_remaining;
+		count -= bytes_remaining;
+	}
+	return ERROR_OK;
+}
+
+static int stlink_dap_op_ap_mem_write(struct adiv5_ap *ap, const uint8_t *buffer,
+		uint32_t size, uint32_t count, uint32_t address, bool addrinc)
+{
+	int retval;
+	uint32_t bytes_remaining;
+
+	if (!addrinc && size != 4)
+		return ERROR_OP_NOT_SUPPORTED;
+
+	retval = stlink_dap_op_run(ap->dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = stlink_dap_open_ap(ap->ap_num);
+	if (retval != ERROR_OK)
+		return retval;
+
+	dap_invalidate_cache(ap->dap);
+
+	if (!(stlink_dap_handle->version.flags & STLINK_F_HAS_CSW))
+		return stlink_dap_hack_csw_mem_write(ap, buffer, size, count, address, addrinc);
+
+	if (addrinc)
+		return stlink_usb_write_ap_mem(stlink_dap_handle, ap->ap_num,
+			ap->csw_default, address, size, count, buffer);
+
+	count *= size;
+	while (count) {
+		bytes_remaining = (count < STLINK_DATA_SIZE) ? count : STLINK_DATA_SIZE;
+
+		retval = stlink_usb_write_mem32_noaddrinc(stlink_dap_handle, ap->ap_num,
+				ap->csw_default, address, bytes_remaining, buffer);
+		if (retval != ERROR_OK)
+			return retval;
+
+		buffer += bytes_remaining;
+		count -= bytes_remaining;
+	}
+	return ERROR_OK;
+}
+
 /** */
 static void stlink_dap_op_quit(struct adiv5_dap *dap)
 {
@@ -3623,7 +4077,12 @@ static int stlink_dap_speed(int speed)
 /** */
 static int stlink_dap_khz(int khz, int *jtag_speed)
 {
-	*jtag_speed = khz;
+	if (khz == 0) {
+		LOG_ERROR("RCLK not supported");
+		return ERROR_FAIL;
+	}
+
+	*jtag_speed = stlink_speed(stlink_dap_handle, khz, true);
 	return ERROR_OK;
 }
 
@@ -3645,6 +4104,10 @@ static const struct dap_ops stlink_dap_ops = {
 	.run = stlink_dap_op_run,
 	.sync = NULL, /* optional */
 	.quit = stlink_dap_op_quit, /* optional */
+	.ap_ops = {
+		.ap_mem_read = stlink_dap_op_ap_mem_read,
+		.ap_mem_write = stlink_dap_op_ap_mem_write,
+	},
 };
 
 static const char *const stlink_dap_transport[] = { "dapdirect_jtag", "dapdirect_swd", NULL };
diff --git a/src/jtag/drivers/usb_blaster/ublast2_access_libusb.c b/src/jtag/drivers/usb_blaster/ublast2_access_libusb.c
index 4f7ee6300..b406c0321 100644
--- a/src/jtag/drivers/usb_blaster/ublast2_access_libusb.c
+++ b/src/jtag/drivers/usb_blaster/ublast2_access_libusb.c
@@ -195,7 +195,7 @@ static int ublast2_libusb_init(struct ublast_lowlevel *low)
 	bool renumeration = false;
 	int ret;
 
-	if (jtag_libusb_open(vids, pids, NULL, &temp) == ERROR_OK) {
+	if (jtag_libusb_open(vids, pids, NULL, &temp, NULL) == ERROR_OK) {
 		LOG_INFO("Altera USB-Blaster II (uninitialized) found");
 		LOG_INFO("Loading firmware...");
 		ret = load_usb_blaster_firmware(temp, low);
@@ -209,13 +209,15 @@ static int ublast2_libusb_init(struct ublast_lowlevel *low)
 	const uint16_t pids_renum[] = { low->ublast_pid, 0 };
 
 	if (renumeration == false) {
-		if (jtag_libusb_open(vids_renum, pids_renum, NULL, &low->libusb_dev) != ERROR_OK) {
+		if (jtag_libusb_open(vids_renum, pids_renum, NULL,
+				&low->libusb_dev, NULL) != ERROR_OK) {
 			LOG_ERROR("Altera USB-Blaster II not found");
 			return ERROR_FAIL;
 		}
 	} else {
 		int retry = 10;
-		while (jtag_libusb_open(vids_renum, pids_renum, NULL, &low->libusb_dev) != ERROR_OK && retry--) {
+		while (jtag_libusb_open(vids_renum, pids_renum, NULL,
+				&low->libusb_dev, NULL) != ERROR_OK && retry--) {
 			usleep(1000000);
 			LOG_INFO("Waiting for renumerate...");
 		}
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 2d47da3ea..ee0d1f329 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -596,24 +596,52 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 int mem_ap_read_buf(struct adiv5_ap *ap,
 		uint8_t *buffer, uint32_t size, uint32_t count, uint32_t address)
 {
+	int retval;
+
+	if (ap->dap->ops->ap_ops.ap_mem_read) {
+		retval = ap->dap->ops->ap_ops.ap_mem_read(ap, buffer, size, count, address, true);
+		if (retval != ERROR_OP_NOT_SUPPORTED)
+			return retval;
+	}
 	return mem_ap_read(ap, buffer, size, count, address, true);
 }
 
 int mem_ap_write_buf(struct adiv5_ap *ap,
 		const uint8_t *buffer, uint32_t size, uint32_t count, uint32_t address)
 {
+	int retval;
+
+	if (ap->dap->ops->ap_ops.ap_mem_write) {
+		retval = ap->dap->ops->ap_ops.ap_mem_write(ap, buffer, size, count, address, true);
+		if (retval != ERROR_OP_NOT_SUPPORTED)
+			return retval;
+	}
 	return mem_ap_write(ap, buffer, size, count, address, true);
 }
 
 int mem_ap_read_buf_noincr(struct adiv5_ap *ap,
 		uint8_t *buffer, uint32_t size, uint32_t count, uint32_t address)
 {
+	int retval;
+
+	if (ap->dap->ops->ap_ops.ap_mem_read) {
+		retval = ap->dap->ops->ap_ops.ap_mem_read(ap, buffer, size, count, address, false);
+		if (retval != ERROR_OP_NOT_SUPPORTED)
+			return retval;
+	}
 	return mem_ap_read(ap, buffer, size, count, address, false);
 }
 
 int mem_ap_write_buf_noincr(struct adiv5_ap *ap,
 		const uint8_t *buffer, uint32_t size, uint32_t count, uint32_t address)
 {
+	int retval;
+
+	if (ap->dap->ops->ap_ops.ap_mem_write) {
+		retval = ap->dap->ops->ap_ops.ap_mem_write(ap, buffer, size, count, address, false);
+		if (retval != ERROR_OP_NOT_SUPPORTED)
+			return retval;
+	}
 	return mem_ap_write(ap, buffer, size, count, address, false);
 }
 
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 17365bddb..984cebc0c 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -296,6 +296,26 @@ struct adiv5_dap {
 	bool ignore_syspwrupack;
 };
 
+/**
+ * Optional optimized implementation of AP access.
+ * Use of low-level primitives in struct dap_ops already allows implementing
+ * any type of AP access. Some adapter offer faster API at higher level for
+ * specific AP types or mode.
+ * Specific request not available or not implemented should return error
+ * ERROR_OP_NOT_SUPPORTED.
+ */
+struct ap_ops {
+	/** Optional interface-specific optimized AP memory read */
+	int (*ap_mem_read)(struct adiv5_ap *ap, uint8_t *buffer,
+			uint32_t size, uint32_t count, uint32_t address,
+			bool addrinc);
+
+	/** Optional interface-specific optimized AP memory write */
+	int (*ap_mem_write)(struct adiv5_ap *ap, const uint8_t *buffer,
+			uint32_t size, uint32_t count, uint32_t address,
+			bool addrinc);
+};
+
 /**
  * Transport-neutral representation of queued DAP transactions, supporting
  * both JTAG and SWD transports.  All submitted transactions are logically
@@ -336,6 +356,8 @@ struct dap_ops {
 
 	/** Optional; called at OpenOCD exit */
 	void (*quit)(struct adiv5_dap *dap);
+
+	struct ap_ops ap_ops;
 };
 
 /*
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 88b932073..089250704 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -399,7 +399,10 @@ static int armv8_read_reg32(struct armv8_common *armv8, int regnum, uint64_t *re
 				&value);
 		break;
 	case ARMV8_ESR_EL3: /* FIXME: no equivalent in aarch32? */
-		retval = ERROR_FAIL;
+		/* avoid false errors in state polling which may lead to reexamine the
+		 * target and re-execute examine-state and examine-end event handlers */
+		LOG_WARNING("Unsupported read from ESR_EL3 in AArch32 state");
+		retval = ERROR_OK;
 		break;
 	case ARMV8_SPSR_EL1: /* mapped to SPSR_svc */
 		retval = dpm->instr_read_data_r0(dpm,
@@ -533,7 +536,10 @@ static int armv8_write_reg32(struct armv8_common *armv8, int regnum, uint64_t va
 				value);
 		break;
 	case ARMV8_ESR_EL3: /* FIXME: no equivalent in aarch32? */
-		retval = ERROR_FAIL;
+		/* avoid false errors in state polling which may lead to reexamine the
+		 * target and re-execute examine-state and examine-end event handlers */
+		LOG_WARNING("Unsupported write to ESR_EL3 in AArch32 state");
+		retval = ERROR_OK;
 		break;
 	case ARMV8_SPSR_EL1: /* mapped to SPSR_svc */
 		retval = dpm->instr_write_data_r0(dpm,
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 081eed21b..5be52726c 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -681,6 +681,10 @@ static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 			LOG_DEBUG("READ: %s, hvalue=%16.8llx", r->name, (unsigned long long) hvalue);
 		}
 	}
+
+	if (retval != ERROR_OK)
+		LOG_ERROR("Failed to read %s register", r->name);
+
 	return retval;
 }
 
@@ -720,6 +724,9 @@ static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 		}
 	}
 
+	if (retval != ERROR_OK)
+		LOG_ERROR("Failed to write %s register", r->name);
+
 	return retval;
 }
 
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 729a173eb..f71b15524 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -1703,6 +1703,7 @@ static int cortex_a_assert_reset(struct target *target)
 
 static int cortex_a_deassert_reset(struct target *target)
 {
+	struct armv7a_common *armv7a = target_to_armv7a(target);
 	int retval;
 
 	LOG_DEBUG(" ");
@@ -1721,7 +1722,8 @@ static int cortex_a_deassert_reset(struct target *target)
 			LOG_WARNING("%s: ran after reset and before halt ...",
 				target_name(target));
 			if (target_was_examined(target)) {
-				retval = target_halt(target);
+				retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
+						armv7a->debug_base + CPUDBG_DRCR, DRCR_HALT);
 				if (retval != ERROR_OK)
 					return retval;
 			} else
diff --git a/tcl/board/stm32mp15x_dk2.cfg b/tcl/board/stm32mp15x_dk2.cfg
new file mode 100644
index 000000000..0233c6d75
--- /dev/null
+++ b/tcl/board/stm32mp15x_dk2.cfg
@@ -0,0 +1,11 @@
+# board MB1272B
+# http://www.st.com/en/evaluation-tools/stm32mp157a-dk1.html
+# http://www.st.com/en/evaluation-tools/stm32mp157c-dk2.html
+
+source [find interface/stlink-dap.cfg]
+
+transport select dapdirect_swd
+
+source [find target/stm32mp15x.cfg]
+
+reset_config srst_only
diff --git a/tcl/board/stm32mp15x_ev1.cfg b/tcl/board/stm32mp15x_ev1.cfg
new file mode 100644
index 000000000..a381e790b
--- /dev/null
+++ b/tcl/board/stm32mp15x_ev1.cfg
@@ -0,0 +1,11 @@
+# board set MB1262C + MB1263B
+# https://www.st.com/en/evaluation-tools/stm32mp157a-ev1.html
+# https://www.st.com/en/evaluation-tools/stm32mp157c-ev1.html
+
+source [find interface/stlink-dap.cfg]
+
+transport select dapdirect_swd
+
+source [find target/stm32mp15x.cfg]
+
+reset_config srst_only
diff --git a/tcl/board/stm32mp15x_ev1_jlink_jtag.cfg b/tcl/board/stm32mp15x_ev1_jlink_jtag.cfg
new file mode 100644
index 000000000..ce0c56cdb
--- /dev/null
+++ b/tcl/board/stm32mp15x_ev1_jlink_jtag.cfg
@@ -0,0 +1,9 @@
+# This is a STM32MP15x board set MB1262C + MB1263B.
+
+source [find interface/jlink.cfg]
+
+transport select jtag
+
+source [find target/stm32mp15x.cfg]
+
+reset_config trst_and_srst separate
diff --git a/tcl/board/stm32mp15x_ev1_jlink_swd.cfg b/tcl/board/stm32mp15x_ev1_jlink_swd.cfg
new file mode 100644
index 000000000..58286d3f8
--- /dev/null
+++ b/tcl/board/stm32mp15x_ev1_jlink_swd.cfg
@@ -0,0 +1,9 @@
+# This is a STM32MP15x board set MB1262C + MB1263B.
+
+source [find interface/jlink.cfg]
+
+transport select swd
+
+source [find target/stm32mp15x.cfg]
+
+reset_config srst_only
diff --git a/tcl/board/stm32mp15x_ev1_stlink_jtag.cfg b/tcl/board/stm32mp15x_ev1_stlink_jtag.cfg
new file mode 100644
index 000000000..b0cba47c3
--- /dev/null
+++ b/tcl/board/stm32mp15x_ev1_stlink_jtag.cfg
@@ -0,0 +1,9 @@
+# This is a STM32MP15x board set MB1262C + MB1263B.
+
+source [find interface/stlink-dap.cfg]
+
+transport select dapdirect_jtag
+
+source [find target/stm32mp15x.cfg]
+
+reset_config srst_only
diff --git a/tcl/board/stm32mp15x_ev1_stlink_swd.cfg b/tcl/board/stm32mp15x_ev1_stlink_swd.cfg
new file mode 100644
index 000000000..793b2c771
--- /dev/null
+++ b/tcl/board/stm32mp15x_ev1_stlink_swd.cfg
@@ -0,0 +1,9 @@
+# This is a STM32MP15x board set MB1262C + MB1263B.
+
+source [find interface/stlink-dap.cfg]
+
+transport select dapdirect_swd
+
+source [find target/stm32mp15x.cfg]
+
+reset_config srst_only
diff --git a/tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg b/tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg
new file mode 100644
index 000000000..fcf62d4f3
--- /dev/null
+++ b/tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg
@@ -0,0 +1,9 @@
+# This is a STM32MP15x board set MB1262C + MB1263B.
+
+source [find interface/cmsis-dap.cfg]
+
+transport select jtag
+
+source [find target/stm32mp15x.cfg]
+
+reset_config trst_and_srst separate
diff --git a/tcl/board/stm32mp15x_ev1_ulink2_swd.cfg b/tcl/board/stm32mp15x_ev1_ulink2_swd.cfg
new file mode 100644
index 000000000..2c0528bf1
--- /dev/null
+++ b/tcl/board/stm32mp15x_ev1_ulink2_swd.cfg
@@ -0,0 +1,9 @@
+# This is a STM32MP15x board set MB1262C + MB1263B.
+
+source [find interface/cmsis-dap.cfg]
+
+transport select swd
+
+source [find target/stm32mp15x.cfg]
+
+reset_config srst_only
diff --git a/tcl/target/stm32mp15x.cfg b/tcl/target/stm32mp15x.cfg
new file mode 100644
index 000000000..7f0d19c4d
--- /dev/null
+++ b/tcl/target/stm32mp15x.cfg
@@ -0,0 +1,121 @@
+# STMicroelectronics STM32MP15x (Single/Dual Cortex-A7 plus Cortex-M4)
+# http://www.st.com/stm32mp1
+
+# 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."
+	echo "ERROR: To use STLink switch to DAP mode, as in \"board/stm32mp15x_dk2.cfg\"."
+	shutdown
+}
+
+source [find target/swj-dp.tcl]
+
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME stm32mp15x
+}
+
+if { [info exists CPUTAPID] } {
+	set _CPUTAPID $CPUTAPID
+} else {
+	if { [using_jtag] } {
+		set _CPUTAPID 0x6ba00477
+	} else {
+		set _CPUTAPID 0x6ba02477
+	}
+}
+
+# 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
+if { [using_jtag] } {
+	jtag newtap $_CHIPNAME.clc tap -expected-id $_CLCTAPID -irlen 5
+}
+
+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.cm4 cortex_m -dap $_CHIPNAME.dap -ap-num 2 -defer-examine
+
+targets $_CHIPNAME.cpu0
+
+target smp $_CHIPNAME.cpu0 $_CHIPNAME.cpu1
+$_CHIPNAME.cpu0 cortex_a maskisr on
+$_CHIPNAME.cpu1 cortex_a maskisr on
+$_CHIPNAME.cpu0 cortex_a dacrfixup on
+$_CHIPNAME.cpu1 cortex_a dacrfixup on
+
+cti create $_CHIPNAME.cti.sys  -dap $_CHIPNAME.dap -ap-num 1 -ctibase 0xE0094000
+cti create $_CHIPNAME.cti.cpu0 -dap $_CHIPNAME.dap -ap-num 1 -ctibase 0xE00D8000
+cti create $_CHIPNAME.cti.cpu1 -dap $_CHIPNAME.dap -ap-num 1 -ctibase 0xE00D9000
+cti create $_CHIPNAME.cti.cm4  -dap $_CHIPNAME.dap -ap-num 2 -ctibase 0xE0043000
+
+# interface does not work while srst is asserted
+# this is target specific, valid for every board
+# Errata "2.3.5 Incorrect reset of glitch-free kernel clock switch" requires
+# srst to force VDDCORE power cycle or pull srst_core. Both cases reset the
+# debug unit, behavior equivalent to "srst_pulls_trst"
+reset_config srst_gates_jtag srst_pulls_trst
+
+adapter speed 5000
+adapter srst pulse_width 200
+# bootrom has an internal timeout of 1 second for detecting the boot flash.
+# wait at least 1 second to guarantee we are out of bootrom
+adapter srst delay 1100
+
+add_help_text axi_secure "Set secure mode for following AXI accesses"
+proc axi_secure {} {
+	$::_CHIPNAME.dap apsel 0
+	$::_CHIPNAME.dap apcsw 0x10006000
+}
+
+add_help_text axi_nsecure "Set non-secure mode for following AXI accesses"
+proc axi_nsecure {} {
+	$::_CHIPNAME.dap apsel 0
+	$::_CHIPNAME.dap apcsw 0x30006000
+}
+
+axi_secure
+
+proc dbgmcu_enable_debug {} {
+	# set debug enable bits in DBGMCU_CR to get ap2 and cm4 visible
+	catch {$::_CHIPNAME.ap1 mww 0xe0081004 0x00000007}
+}
+
+proc toggle_cpu0_dbg_claim0 {} {
+	# toggle CPU0 DBG_CLAIM[0]
+	$::_CHIPNAME.ap1 mww 0xe00d0fa0 1
+	$::_CHIPNAME.ap1 mww 0xe00d0fa4 1
+}
+
+proc detect_cpu1 {} {
+	$::_CHIPNAME.ap1 mem2array cpu1_prsr 32 0xE00D2314 1
+	set dual_core [expr $cpu1_prsr(0) & 1]
+	if {! $dual_core} {$::_CHIPNAME.cpu1 configure -defer-examine}
+}
+
+# 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;dap init;catch {$::_CHIPNAME.dap apid 1}}
+$_CHIPNAME.ap2  configure -event reset-deassert-pre  {dbgmcu_enable_debug}
+$_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_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         {$::_CHIPNAME.cm4 arp_examine}
-- 
2.25.2