X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fjtag%2Fdrivers%2Fstlink_usb.c;h=b51a0c2bacd23d51232657e510ed3c0848c54d48;hp=1a1963425a87428d276efdc3d4e1d14af41cb137;hb=bac3ef96b79ac7b7b44585f2c3a2adcd8732f7a2;hpb=bc944c832bcef45674fddebdf27a87e2abfdb168

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 1a1963425a..b51a0c2bac 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -33,6 +33,7 @@
 #endif
 
 /* project specific includes */
+#include <helper/align.h>
 #include <helper/binarybuffer.h>
 #include <helper/bits.h>
 #include <helper/system.h>
@@ -41,6 +42,7 @@
 #include <jtag/hla/hla_transport.h>
 #include <jtag/hla/hla_interface.h>
 #include <jtag/swim.h>
+#include <target/arm_adi_v5.h>
 #include <target/target.h>
 #include <transport/transport.h>
 
@@ -78,7 +80,7 @@
 #define STLINK_V2_1_TRACE_EP  (2|ENDPOINT_IN)
 
 #define STLINK_SG_SIZE        (31)
-#define STLINK_DATA_SIZE      (4096)
+#define STLINK_DATA_SIZE      (6144)
 #define STLINK_CMD_SIZE_V2    (16)
 #define STLINK_CMD_SIZE_V1    (10)
 
@@ -90,20 +92,32 @@
 #define STLINK_V3E_PID          (0x374E)
 #define STLINK_V3S_PID          (0x374F)
 #define STLINK_V3_2VCP_PID      (0x3753)
+#define STLINK_V3E_NO_MSD_PID   (0x3754)
 
 /*
  * ST-Link/V1, ST-Link/V2 and ST-Link/V2.1 are full-speed USB devices and
  * this limits the bulk packet size and the 8bit read/writes to max 64 bytes.
  * STLINK-V3 is a high speed USB 2.0 and the limit is 512 bytes from FW V3J6.
+ *
+ * For 16 and 32bit read/writes stlink handles USB packet split and the limit
+ * is the internal buffer size of 6144 bytes.
+ * TODO: override ADIv5 layer's tar_autoincr_block that limits the transfer
+ * to 1024 or 4096 bytes
  */
-#define STLINK_MAX_RW8		(64)
-#define STLINKV3_MAX_RW8	(512)
+#define STLINK_MAX_RW8          (64)
+#define STLINKV3_MAX_RW8        (512)
+#define STLINK_MAX_RW16_32      STLINK_DATA_SIZE
+#define STLINK_SWIM_DATA_SIZE   STLINK_DATA_SIZE
 
 /* "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,
@@ -166,6 +180,68 @@ struct stlink_backend_s {
 	int (*read_trace)(void *handle, const uint8_t *buf, int size);
 };
 
+/* TODO: make queue size dynamic */
+/* TODO: don't allocate queue for HLA */
+#define MAX_QUEUE_DEPTH (4096)
+
+enum queue_cmd {
+	CMD_DP_READ = 1,
+	CMD_DP_WRITE,
+
+	CMD_AP_READ,
+	CMD_AP_WRITE,
+
+	/*
+	 * encode the bytes size in the enum's value. This makes easy to extract it
+	 * with a simple logic AND, by using the macro CMD_MEM_AP_2_SIZE() below
+	 */
+	CMD_MEM_AP_READ8   = 0x10 + 1,
+	CMD_MEM_AP_READ16  = 0x10 + 2,
+	CMD_MEM_AP_READ32  = 0x10 + 4,
+
+	CMD_MEM_AP_WRITE8  = 0x20 + 1,
+	CMD_MEM_AP_WRITE16 = 0x20 + 2,
+	CMD_MEM_AP_WRITE32 = 0x20 + 4,
+};
+
+#define CMD_MEM_AP_2_SIZE(cmd) ((cmd) & 7)
+
+struct dap_queue {
+	enum queue_cmd cmd;
+	union {
+		struct dp_r {
+			unsigned int reg;
+			struct adiv5_dap *dap;
+			uint32_t *p_data;
+		} dp_r;
+		struct dp_w {
+			unsigned int reg;
+			struct adiv5_dap *dap;
+			uint32_t data;
+		} dp_w;
+		struct ap_r {
+			unsigned int reg;
+			struct adiv5_ap *ap;
+			uint32_t *p_data;
+		} ap_r;
+		struct ap_w {
+			unsigned int reg;
+			struct adiv5_ap *ap;
+			uint32_t data;
+			bool changes_csw_default;
+		} ap_w;
+		struct mem_ap {
+			uint32_t addr;
+			struct adiv5_ap *ap;
+			union {
+				uint32_t *p_data;
+				uint32_t data;
+			};
+			uint32_t csw;
+		} mem_ap;
+	};
+};
+
 /** */
 struct stlink_usb_handle_s {
 	/** */
@@ -209,6 +285,10 @@ struct stlink_usb_handle_s {
 	/** reconnect is needed next time we try to query the
 	 * status */
 	bool reconnect_pending;
+	/** queue of dap_direct operations */
+	struct dap_queue queue[MAX_QUEUE_DEPTH];
+	/** first element available in the queue */
+	unsigned int queue_index;
 };
 
 /** */
@@ -356,6 +436,12 @@ static inline int stlink_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, i
 #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_APIV2_RW_MISC_OUT     0x51
+#define STLINK_DEBUG_APIV2_RW_MISC_IN      0x52
+
+#define STLINK_DEBUG_READMEM_32BIT_NO_ADDR_INC          0x54
+
 #define STLINK_APIV3_SET_COM_FREQ           0x61
 #define STLINK_APIV3_GET_COM_FREQ           0x62
 
@@ -413,21 +499,25 @@ static inline int stlink_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, i
  * Map the relevant features, quirks and workaround for specific firmware
  * version of stlink
  */
-#define STLINK_F_HAS_TRACE              BIT(0)  /* v2>=j13 || v3 */
-#define STLINK_F_HAS_SWD_SET_FREQ       BIT(1)  /* v2>=j22 */
-#define STLINK_F_HAS_JTAG_SET_FREQ      BIT(2)  /* v2>=j24 */
-#define STLINK_F_HAS_MEM_16BIT          BIT(3)  /* v2>=j26 || v3 */
-#define STLINK_F_HAS_GETLASTRWSTATUS2   BIT(4)  /* v2>=j15 || v3 */
-#define STLINK_F_HAS_DAP_REG            BIT(5)  /* v2>=j24 || v3 */
-#define STLINK_F_QUIRK_JTAG_DP_READ     BIT(6)  /* v2>=j24 && v2<j32 */
-#define STLINK_F_HAS_AP_INIT            BIT(7)  /* v2>=j28 || v3 */
-#define STLINK_F_HAS_DPBANKSEL          BIT(8)  /* v2>=j32 || v3>=j2 */
-#define STLINK_F_HAS_RW8_512BYTES       BIT(9)  /* v3>=j6 */
-#define STLINK_F_FIX_CLOSE_AP           BIT(10) /* v2>=j29 || v3 */
+#define STLINK_F_HAS_TRACE              BIT(0)  /* v2>=j13 || v3     */
+#define STLINK_F_HAS_GETLASTRWSTATUS2   BIT(1)  /* v2>=j15 || v3     */
+#define STLINK_F_HAS_SWD_SET_FREQ       BIT(2)  /* v2>=j22           */
+#define STLINK_F_HAS_JTAG_SET_FREQ      BIT(3)  /* v2>=j24           */
+#define STLINK_F_QUIRK_JTAG_DP_READ     BIT(4)  /* v2>=j24 && v2<j32 */
+#define STLINK_F_HAS_DAP_REG            BIT(5)  /* v2>=j24 || v3     */
+#define STLINK_F_HAS_MEM_16BIT          BIT(6)  /* v2>=j26 || v3     */
+#define STLINK_F_HAS_AP_INIT            BIT(7)  /* v2>=j28 || v3     */
+#define STLINK_F_FIX_CLOSE_AP           BIT(8)  /* v2>=j29 || v3     */
+#define STLINK_F_HAS_DPBANKSEL          BIT(9)  /* v2>=j32 || v3>=j2 */
+#define STLINK_F_HAS_RW8_512BYTES       BIT(10) /*            v3>=j6 */
 
 /* aliases */
 #define STLINK_F_HAS_TARGET_VOLT        STLINK_F_HAS_TRACE
 #define STLINK_F_HAS_FPU_REG            STLINK_F_HAS_GETLASTRWSTATUS2
+#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_RW_MISC            STLINK_F_HAS_DPBANKSEL
+#define STLINK_F_HAS_CSW                STLINK_F_HAS_DPBANKSEL
 
 #define STLINK_REGSEL_IS_FPU(x)         ((x) > 0x1F)
 
@@ -475,7 +565,7 @@ static unsigned int stlink_usb_block(void *handle)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (h->version.flags & STLINK_F_HAS_RW8_512BYTES)
 		return STLINKV3_MAX_RW8;
@@ -497,13 +587,8 @@ static void sync_transfer_wait_for_completion(struct libusb_transfer *transfer)
 {
 	int r, *completed = transfer->user_data;
 
-	/* Assuming a single libusb context exists.  There no existing interface into this
-	 * module to pass a libusb context.
-	 */
-	struct libusb_context *ctx = NULL;
-
 	while (!*completed) {
-		r = libusb_handle_events_completed(ctx, completed);
+		r = jtag_libusb_handle_events_completed(completed);
 		if (r < 0) {
 			if (r == LIBUSB_ERROR_INTERRUPTED)
 				continue;
@@ -573,7 +658,7 @@ static int jtag_libusb_bulk_transfer_n(
 		transfers[i].transfer_size = 0;
 		transfers[i].transfer = libusb_alloc_transfer(0);
 
-		if (transfers[i].transfer == NULL) {
+		if (!transfers[i].transfer) {
 			for (size_t j = 0; j < i; ++j)
 				libusb_free_transfer(transfers[j].transfer);
 
@@ -641,7 +726,7 @@ static int stlink_usb_xfer_v1_get_status(void *handle)
 	struct stlink_usb_handle_s *h = handle;
 	int tr, ret;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	/* read status */
 	memset(h->cmdbuf, 0, STLINK_SG_SIZE);
@@ -675,7 +760,7 @@ static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	size_t n_transfers = 0;
 	struct jtag_xfer transfers[2];
@@ -714,7 +799,7 @@ static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int
 	struct stlink_usb_handle_s *h = handle;
 	int tr, ret;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	ret = jtag_libusb_bulk_write(h->usb_backend_priv.fd, h->tx_ep, (char *)h->cmdbuf,
 				     cmdsize, STLINK_WRITE_TIMEOUT, &tr);
@@ -747,7 +832,7 @@ static int stlink_usb_xfer_v1_get_sense(void *handle)
 	int res;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 16);
 
@@ -795,7 +880,7 @@ static int stlink_usb_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, int
 	int err, cmdsize = STLINK_CMD_SIZE_V2;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (h->version.stlink == 1) {
 		cmdsize = STLINK_SG_SIZE;
@@ -828,7 +913,7 @@ static int stlink_tcp_send_cmd(void *handle, int send_size, int recv_size, bool
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	/* send the TCP command */
 	int sent_size = send(h->tcp_backend_priv.fd, (void *)h->tcp_backend_priv.send_buf, send_size, 0);
@@ -873,7 +958,7 @@ static int stlink_tcp_xfer_noerrcheck(void *handle, const uint8_t *buf, int size
 	int send_size = STLINK_TCP_USB_CMD_SIZE;
 	int recv_size = STLINK_TCP_SS_SIZE;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	/* prepare the TCP command */
 	h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_SEND_USB_CMD;
@@ -942,7 +1027,7 @@ static int stlink_usb_error_check(void *handle)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
 		switch (h->databuf[0]) {
@@ -1081,7 +1166,7 @@ static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	assert(h->version.flags & STLINK_F_HAS_TRACE);
 
@@ -1146,7 +1231,7 @@ static int stlink_usb_version(void *handle)
 	char *p;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 6);
 
@@ -1249,6 +1334,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;
 
@@ -1261,6 +1347,8 @@ static int stlink_usb_version(void *handle)
 			flags |= STLINK_F_FIX_CLOSE_AP;
 
 		/* 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;
 
@@ -1282,6 +1370,7 @@ 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 */
@@ -1291,6 +1380,8 @@ static int stlink_usb_version(void *handle)
 		flags |= STLINK_F_FIX_CLOSE_AP;
 
 		/* 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;
 
@@ -1360,7 +1451,7 @@ static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
 		return ERROR_COMMAND_NOTFOUND;
@@ -1384,7 +1475,7 @@ static int stlink_usb_set_jtagclk(void *handle, uint16_t clk_divisor)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
 		return ERROR_COMMAND_NOTFOUND;
@@ -1410,7 +1501,7 @@ static int stlink_usb_current_mode(void *handle, uint8_t *mode)
 	int res;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
@@ -1432,7 +1523,7 @@ static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
 	int rx_size = 0;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	/* on api V2 we are able the read the latest command
 	 * status
@@ -1480,7 +1571,7 @@ static int stlink_usb_mode_leave(void *handle, enum stlink_mode type)
 	int res;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	/* command with no reply, use a valid endpoint but zero size */
 	stlink_usb_init_buffer(handle, h->rx_ep, 0);
@@ -1533,7 +1624,7 @@ static int stlink_usb_exit_mode(void *handle)
 	uint8_t mode;
 	enum stlink_mode emode;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	res = stlink_usb_current_mode(handle, &mode);
 
@@ -1574,7 +1665,7 @@ static int stlink_usb_init_mode(void *handle, bool connect_under_reset, int init
 	enum stlink_mode emode;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	res = stlink_usb_exit_mode(handle);
 	if (res != ERROR_OK)
@@ -1631,7 +1722,8 @@ static int stlink_usb_init_mode(void *handle, bool connect_under_reset, int init
 		}
 	}
 
-	if (h->version.jtag_api == STLINK_JTAG_API_V3) {
+	if (h->version.jtag_api == STLINK_JTAG_API_V3 &&
+			(emode == STLINK_MODE_DEBUG_JTAG || emode == STLINK_MODE_DEBUG_SWD)) {
 		struct speed_map map[STLINK_V3_MAX_FREQ_NB];
 
 		stlink_get_com_freq(h, (emode == STLINK_MODE_DEBUG_JTAG), map);
@@ -1809,7 +1901,7 @@ static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, con
 	unsigned int datalen = 0;
 	int cmdsize = STLINK_CMD_SIZE_V2;
 
-	if (len > STLINK_DATA_SIZE)
+	if (len > STLINK_SWIM_DATA_SIZE)
 		return ERROR_FAIL;
 
 	if (h->version.stlink == 1)
@@ -1842,7 +1934,7 @@ static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint
 	struct stlink_usb_handle_s *h = handle;
 	int res;
 
-	if (len > STLINK_DATA_SIZE)
+	if (len > STLINK_SWIM_DATA_SIZE)
 		return ERROR_FAIL;
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 0);
@@ -1872,7 +1964,7 @@ static int stlink_usb_idcode(void *handle, uint32_t *idcode)
 	int res, offset;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	/* there is no swim read core id cmd */
 	if (h->st_mode == STLINK_MODE_DEBUG_SWIM) {
@@ -1910,7 +2002,7 @@ static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *v
 	struct stlink_usb_handle_s *h = handle;
 	int res;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 8);
 
@@ -1931,7 +2023,7 @@ static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
@@ -1953,7 +2045,7 @@ static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (h->trace.enabled && (h->version.flags & STLINK_F_HAS_TRACE)) {
 		int res;
@@ -2004,7 +2096,7 @@ static enum target_state stlink_usb_state(void *handle)
 	int res;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (h->reconnect_pending) {
 		LOG_INFO("Previous state query failed, trying to reconnect");
@@ -2046,7 +2138,7 @@ static int stlink_usb_assert_srst(void *handle, int srst)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (h->st_mode == STLINK_MODE_DEBUG_SWIM)
 		return stlink_swim_assert_reset(handle, srst);
@@ -2069,7 +2161,7 @@ static void stlink_usb_trace_disable(void *handle)
 	int res = ERROR_OK;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	assert(h->version.flags & STLINK_F_HAS_TRACE);
 
@@ -2091,7 +2183,7 @@ static int stlink_usb_trace_enable(void *handle)
 	int res;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (h->version.flags & STLINK_F_HAS_TRACE) {
 		stlink_usb_init_buffer(handle, h->rx_ep, 10);
@@ -2123,7 +2215,7 @@ static int stlink_usb_reset(void *handle)
 	struct stlink_usb_handle_s *h = handle;
 	int retval;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
@@ -2152,7 +2244,7 @@ static int stlink_usb_run(void *handle)
 	int res;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (h->version.jtag_api != STLINK_JTAG_API_V1) {
 		res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
@@ -2174,7 +2266,7 @@ static int stlink_usb_halt(void *handle)
 	int res;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (h->version.jtag_api != STLINK_JTAG_API_V1) {
 		res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
@@ -2195,7 +2287,7 @@ static int stlink_usb_step(void *handle)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (h->version.jtag_api != STLINK_JTAG_API_V1) {
 		/* TODO: this emulates the v1 api, it should really use a similar auto mask isr
@@ -2219,7 +2311,7 @@ static int stlink_usb_read_regs(void *handle)
 	int res;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 88);
 
@@ -2244,7 +2336,7 @@ static int stlink_usb_read_reg(void *handle, unsigned int regsel, uint32_t *val)
 	int res;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
 		res = stlink_usb_write_debug_reg(h, DCB_DCRSR, regsel & 0x7f);
@@ -2284,14 +2376,14 @@ static int stlink_usb_write_reg(void *handle, unsigned int regsel, uint32_t val)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (STLINK_REGSEL_IS_FPU(regsel) && !(h->version.flags & STLINK_F_HAS_FPU_REG)) {
 		int res = stlink_usb_write_debug_reg(h, DCB_DCRDR, val);
 		if (res != ERROR_OK)
 			return res;
 
-		return stlink_usb_write_debug_reg(h, DCB_DCRSR, DCRSR_WnR | (regsel & 0x7f));
+		return stlink_usb_write_debug_reg(h, DCB_DCRSR, DCRSR_WNR | (regsel & 0x7f));
 		/* FIXME: poll DHCSR.S_REGRDY after write DCRSR */
 	}
 
@@ -2313,7 +2405,7 @@ static int stlink_usb_get_rw_status(void *handle)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		return ERROR_OK;
@@ -2331,14 +2423,17 @@ 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;
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
+
+	if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
+		return ERROR_COMMAND_NOTFOUND;
 
 	/* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
 	if (len > stlink_usb_block(h)) {
@@ -2354,6 +2449,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)
@@ -2370,13 +2468,16 @@ 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;
 
-	assert(handle != NULL);
+	assert(handle);
+
+	if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
+		return ERROR_COMMAND_NOTFOUND;
 
 	/* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
 	if (len > stlink_usb_block(h)) {
@@ -2392,6 +2493,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);
 
@@ -2402,17 +2506,25 @@ 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;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
 		return ERROR_COMMAND_NOTFOUND;
 
+	if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
+		return ERROR_COMMAND_NOTFOUND;
+
+	if (len > STLINK_MAX_RW16_32) {
+		LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
+		return ERROR_FAIL;
+	}
+
 	/* data must be a multiple of 2 and half-word aligned */
 	if (len % 2 || addr % 2) {
 		LOG_DEBUG("Invalid data alignment");
@@ -2427,6 +2539,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);
 
@@ -2439,17 +2554,25 @@ 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;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
 		return ERROR_COMMAND_NOTFOUND;
 
+	if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
+		return ERROR_COMMAND_NOTFOUND;
+
+	if (len > STLINK_MAX_RW16_32) {
+		LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
+		return ERROR_FAIL;
+	}
+
 	/* data must be a multiple of 2 and half-word aligned */
 	if (len % 2 || addr % 2) {
 		LOG_DEBUG("Invalid data alignment");
@@ -2464,6 +2587,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);
 
@@ -2474,13 +2600,21 @@ 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;
 
-	assert(handle != NULL);
+	assert(handle);
+
+	if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
+		return ERROR_COMMAND_NOTFOUND;
+
+	if (len > STLINK_MAX_RW16_32) {
+		LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
+		return ERROR_FAIL;
+	}
 
 	/* data must be a multiple of 4 and word aligned */
 	if (len % 4 || addr % 4) {
@@ -2496,6 +2630,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);
 
@@ -2508,13 +2645,21 @@ 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;
 
-	assert(handle != NULL);
+	assert(handle);
+
+	if ((ap_num != 0 || csw != 0) && !(h->version.flags & STLINK_F_HAS_CSW))
+		return ERROR_COMMAND_NOTFOUND;
+
+	if (len > STLINK_MAX_RW16_32) {
+		LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
+		return ERROR_FAIL;
+	}
 
 	/* data must be a multiple of 4 and word aligned */
 	if (len % 4 || addr % 4) {
@@ -2530,6 +2675,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);
 
@@ -2539,6 +2687,88 @@ 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)
+{
+	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;
+
+	if (len > STLINK_MAX_RW16_32) {
+		LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
+		return ERROR_FAIL;
+	}
+
+	/* 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;
+
+	int retval = stlink_usb_xfer_noerrcheck(handle, h->databuf, len);
+	if (retval != ERROR_OK)
+		return retval;
+
+	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)
+{
+	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;
+
+	if (len > STLINK_MAX_RW16_32) {
+		LOG_DEBUG("max buffer (%d) length exceeded", STLINK_MAX_RW16_32);
+		return ERROR_FAIL;
+	}
+
+	/* 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;
+
+	int retval = stlink_usb_xfer_noerrcheck(handle, buffer, len);
+	if (retval != ERROR_OK)
+		return retval;
+
+	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));
@@ -2547,8 +2777,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;
@@ -2563,7 +2793,6 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 		size = 1;
 
 	while (count) {
-
 		bytes_remaining = (size != 1) ?
 				stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
 
@@ -2577,7 +2806,6 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 		 * as 8bit access.
 		 */
 		if (size != 1) {
-
 			/* When in jtag mode the stlink uses the auto-increment functionality.
 			 * However it expects us to pass the data correctly, this includes
 			 * alignment and any page boundaries. We already do this as part of the
@@ -2588,11 +2816,10 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 
 			/* we first need to check for any unaligned bytes */
 			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);
+					usleep((1 << retries++) * 1000);
 					continue;
 				}
 				if (retval != ERROR_OK)
@@ -2604,16 +2831,17 @@ 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);
-		} else
-			retval = stlink_usb_read_mem8(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, ap_num, csw, addr, bytes_remaining, buffer);
+		}
 
 		if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
-			usleep((1<<retries++) * 1000);
+			usleep((1 << retries++) * 1000);
 			continue;
 		}
 		if (retval != ERROR_OK)
@@ -2627,8 +2855,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;
@@ -2670,7 +2905,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;
@@ -2684,14 +2919,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;
@@ -2707,6 +2942,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)
 {
@@ -2990,7 +3232,7 @@ static int stlink_tcp_close(void *handle)
 /** */
 static int stlink_close(void *handle)
 {
-	if (handle != NULL) {
+	if (handle) {
 		struct stlink_usb_handle_s *h = handle;
 
 		stlink_usb_close(handle);
@@ -3069,7 +3311,7 @@ static char *stlink_usb_get_alternate_serial(struct libusb_device_handle *device
 	/* else (len == 26) => buggy ST-Link */
 
 	char *alternate_serial = malloc((STLINK_SERIAL_LEN + 1) * sizeof(char));
-	if (alternate_serial == NULL)
+	if (!alternate_serial)
 		return NULL;
 
 	for (unsigned int i = 0; i < STLINK_SERIAL_LEN; i += 2)
@@ -3089,7 +3331,7 @@ static int stlink_usb_usb_open(void *handle, struct hl_interface_param_s *param)
 	h->cmdbuf = malloc(STLINK_SG_SIZE);
 	h->databuf = malloc(STLINK_DATA_SIZE);
 
-	if (h->cmdbuf == NULL || h->databuf == NULL)
+	if (!h->cmdbuf || !h->databuf)
 		return ERROR_FAIL;
 
 	/*
@@ -3134,6 +3376,7 @@ static int stlink_usb_usb_open(void *handle, struct hl_interface_param_s *param)
 			case STLINK_V3E_PID:
 			case STLINK_V3S_PID:
 			case STLINK_V3_2VCP_PID:
+			case STLINK_V3E_NO_MSD_PID:
 				h->version.stlink = 3;
 				h->tx_ep = STLINK_V2_1_TX_EP;
 				h->trace_ep = STLINK_V2_1_TRACE_EP;
@@ -3203,7 +3446,7 @@ static int stlink_tcp_open(void *handle, struct hl_interface_param_s *param)
 	h->tcp_backend_priv.send_buf = malloc(STLINK_TCP_SEND_BUFFER_SIZE);
 	h->tcp_backend_priv.recv_buf = malloc(STLINK_TCP_RECV_BUFFER_SIZE);
 
-	if (h->tcp_backend_priv.send_buf == NULL || h->tcp_backend_priv.recv_buf == NULL)
+	if (!h->tcp_backend_priv.send_buf || !h->tcp_backend_priv.recv_buf)
 		return ERROR_FAIL;
 
 	h->cmdbuf = &h->tcp_backend_priv.send_buf[8];
@@ -3312,7 +3555,7 @@ static int stlink_tcp_open(void *handle, struct hl_interface_param_s *param)
 	char serial[STLINK_TCP_SERIAL_SIZE + 1] = {0};
 	uint8_t stlink_used;
 	bool stlink_id_matched = false;
-	bool stlink_serial_matched = (param->serial == NULL);
+	bool stlink_serial_matched = (!param->serial);
 
 	for (uint32_t stlink_id = 0; stlink_id < connected_stlinks; stlink_id++) {
 		/* get the stlink info */
@@ -3483,7 +3726,7 @@ static int stlink_open(struct hl_interface_param_s *param, enum stlink_mode mode
 			goto error_open;
 		}
 		*fd = h;
-		h->max_mem_packet = STLINK_DATA_SIZE;
+		h->max_mem_packet = STLINK_SWIM_DATA_SIZE;
 		return ERROR_OK;
 	}
 
@@ -3494,8 +3737,8 @@ static int stlink_open(struct hl_interface_param_s *param, enum stlink_mode mode
 		h->max_mem_packet = (1 << 10);
 
 		uint8_t buffer[4];
-		stlink_usb_open_ap(h, 0);
-		err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
+		stlink_usb_open_ap(h, STLINK_HLA_AP_NUM);
+		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;
@@ -3539,8 +3782,8 @@ static int stlink_config_trace(void *handle, bool enabled,
 		return ERROR_OK;
 	}
 
-	assert(trace_freq != NULL);
-	assert(prescaler != NULL);
+	assert(trace_freq);
+	assert(prescaler);
 
 	if (pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART) {
 		LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
@@ -3590,7 +3833,7 @@ static int stlink_usb_init_access_port(void *handle, unsigned char ap_num)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
 		return ERROR_COMMAND_NOTFOUND;
@@ -3609,7 +3852,7 @@ static int stlink_usb_close_access_port(void *handle, unsigned char ap_num)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (!(h->version.flags & STLINK_F_HAS_AP_INIT))
 		return ERROR_COMMAND_NOTFOUND;
@@ -3628,6 +3871,53 @@ static int stlink_usb_close_access_port(void *handle, unsigned char ap_num)
 
 }
 
+static int stlink_usb_rw_misc_out(void *handle, uint32_t items, const uint8_t *buffer)
+{
+	struct stlink_usb_handle_s *h = handle;
+	unsigned int buflen = ALIGN_UP(items, 4) + 4 * items;
+
+	LOG_DEBUG_IO("%s(%" PRIu32 ")", __func__, items);
+
+	assert(handle != NULL);
+
+	if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
+		return ERROR_COMMAND_NOTFOUND;
+
+	stlink_usb_init_buffer(handle, h->tx_ep, buflen);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RW_MISC_OUT;
+	h_u32_to_le(&h->cmdbuf[2], items);
+
+	return stlink_usb_xfer_noerrcheck(handle, buffer, buflen);
+}
+
+static int stlink_usb_rw_misc_in(void *handle, uint32_t items, uint8_t *buffer)
+{
+	struct stlink_usb_handle_s *h = handle;
+	unsigned int buflen = 2 * 4 * items;
+
+	LOG_DEBUG_IO("%s(%" PRIu32 ")", __func__, items);
+
+	assert(handle != NULL);
+
+	if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
+		return ERROR_COMMAND_NOTFOUND;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, buflen);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RW_MISC_IN;
+
+	int res = stlink_usb_xfer_noerrcheck(handle, h->databuf, buflen);
+	if (res != ERROR_OK)
+		return res;
+
+	memcpy(buffer, h->databuf, buflen);
+
+	return ERROR_OK;
+}
+
 /** */
 static int stlink_read_dap_register(void *handle, unsigned short dap_port,
 			unsigned short addr, uint32_t *val)
@@ -3635,7 +3925,7 @@ static int stlink_read_dap_register(void *handle, unsigned short dap_port,
 	struct stlink_usb_handle_s *h = handle;
 	int retval;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (!(h->version.flags & STLINK_F_HAS_DAP_REG))
 		return ERROR_COMMAND_NOTFOUND;
@@ -3658,7 +3948,7 @@ static int stlink_write_dap_register(void *handle, unsigned short dap_port,
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	assert(handle != NULL);
+	assert(handle);
 
 	if (!(h->version.flags & STLINK_F_HAS_DAP_REG))
 		return ERROR_COMMAND_NOTFOUND;
@@ -3722,11 +4012,9 @@ struct hl_layout_api_s stlink_usb_layout_api = {
 static struct stlink_usb_handle_s *stlink_dap_handle;
 static struct hl_interface_param_s stlink_dap_param;
 static DECLARE_BITMAP(opened_ap, DP_APSEL_MAX + 1);
+static uint32_t last_csw_default[DP_APSEL_MAX + 1];
 static int stlink_dap_error = ERROR_OK;
 
-static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
-		uint32_t *data);
-
 /** */
 static int stlink_dap_record_error(int error)
 {
@@ -3743,6 +4031,11 @@ static int stlink_dap_get_and_clear_error(void)
 	return retval;
 }
 
+static int stlink_dap_get_error(void)
+{
+	return stlink_dap_error;
+}
+
 static int stlink_usb_open_ap(void *handle, unsigned short apsel)
 {
 	struct stlink_usb_handle_s *h = handle;
@@ -3764,6 +4057,7 @@ static int stlink_usb_open_ap(void *handle, unsigned short apsel)
 
 	LOG_DEBUG("AP %d enabled", apsel);
 	set_bit(apsel, opened_ap);
+	last_csw_default[apsel] = 0;
 	return ERROR_OK;
 }
 
@@ -3847,6 +4141,8 @@ static int stlink_dap_op_connect(struct adiv5_dap *dap)
 
 	dap->do_reconnect = false;
 	dap_invalidate_cache(dap);
+	for (unsigned int i = 0; i <= DP_APSEL_MAX; i++)
+		last_csw_default[i] = 0;
 
 	retval = dap_dp_init(dap);
 	if (retval != ERROR_OK) {
@@ -3888,8 +4184,7 @@ static int stlink_dap_op_send_sequence(struct adiv5_dap *dap, enum swd_special_s
 }
 
 /** */
-static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
-		uint32_t *data)
+static int stlink_dap_dp_read(struct adiv5_dap *dap, unsigned int reg, uint32_t *data)
 {
 	uint32_t dummy;
 	int retval;
@@ -3900,11 +4195,7 @@ static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
 			return ERROR_COMMAND_NOTFOUND;
 		}
 
-	retval = stlink_dap_check_reconnect(dap);
-	if (retval != ERROR_OK)
-		return retval;
-
-	data = data ? : &dummy;
+	data = data ? data : &dummy;
 	if (stlink_dap_handle->version.flags & STLINK_F_QUIRK_JTAG_DP_READ
 		&& stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG) {
 		/* Quirk required in JTAG. Read RDBUFF to get the data */
@@ -3918,12 +4209,11 @@ static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
 					STLINK_DEBUG_PORT_ACCESS, reg, data);
 	}
 
-	return stlink_dap_record_error(retval);
+	return retval;
 }
 
 /** */
-static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
-		uint32_t data)
+static int stlink_dap_dp_write(struct adiv5_dap *dap, unsigned int reg, uint32_t data)
 {
 	int retval;
 
@@ -3939,54 +4229,40 @@ static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 		data &= ~DP_SELECT_DPBANK;
 	}
 
-	retval = stlink_dap_check_reconnect(dap);
-	if (retval != ERROR_OK)
-		return retval;
-
 	/* ST-Link does not like that we set CORUNDETECT */
 	if (reg == DP_CTRL_STAT)
 		data &= ~CORUNDETECT;
 
 	retval = stlink_write_dap_register(stlink_dap_handle,
 				STLINK_DEBUG_PORT_ACCESS, reg, data);
-	return stlink_dap_record_error(retval);
+	return retval;
 }
 
 /** */
-static int stlink_dap_op_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
-		uint32_t *data)
+static int stlink_dap_ap_read(struct adiv5_ap *ap, unsigned int reg, uint32_t *data)
 {
 	struct adiv5_dap *dap = ap->dap;
 	uint32_t dummy;
 	int retval;
 
-	retval = stlink_dap_check_reconnect(dap);
-	if (retval != ERROR_OK)
-		return retval;
-
 	if (reg != AP_REG_IDR) {
 		retval = stlink_dap_open_ap(ap->ap_num);
 		if (retval != ERROR_OK)
 			return retval;
 	}
-	data = data ? : &dummy;
+	data = data ? data : &dummy;
 	retval = stlink_read_dap_register(stlink_dap_handle, ap->ap_num, reg,
 				 data);
 	dap->stlink_flush_ap_write = false;
-	return stlink_dap_record_error(retval);
+	return retval;
 }
 
 /** */
-static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
-		uint32_t data)
+static int stlink_dap_ap_write(struct adiv5_ap *ap, unsigned int reg, uint32_t data)
 {
 	struct adiv5_dap *dap = ap->dap;
 	int retval;
 
-	retval = stlink_dap_check_reconnect(dap);
-	if (retval != ERROR_OK)
-		return retval;
-
 	retval = stlink_dap_open_ap(ap->ap_num);
 	if (retval != ERROR_OK)
 		return retval;
@@ -3994,7 +4270,7 @@ static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
 	retval = stlink_write_dap_register(stlink_dap_handle, ap->ap_num, reg,
 				data);
 	dap->stlink_flush_ap_write = true;
-	return stlink_dap_record_error(retval);
+	return retval;
 }
 
 /** */
@@ -4004,8 +4280,304 @@ static int stlink_dap_op_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
 	return ERROR_OK;
 }
 
+#define RW_MISC_CMD_ADDRESS     1
+#define RW_MISC_CMD_WRITE       2
+#define RW_MISC_CMD_READ        3
+#define RW_MISC_CMD_APNUM       5
+
+static int stlink_usb_misc_rw_segment(void *handle, const struct dap_queue *q, unsigned int len, unsigned int items)
+{
+	uint8_t buf[2 * 4 * items];
+
+	LOG_DEBUG("Queue: %u commands in %u items", len, items);
+
+	int ap_num = DP_APSEL_INVALID;
+	unsigned int cmd_index = 0;
+	unsigned int val_index = ALIGN_UP(items, 4);
+	for (unsigned int i = 0; i < len; i++) {
+		if (ap_num != q[i].mem_ap.ap->ap_num) {
+			ap_num = q[i].mem_ap.ap->ap_num;
+			buf[cmd_index++] = RW_MISC_CMD_APNUM;
+			h_u32_to_le(&buf[val_index], ap_num);
+			val_index += 4;
+		}
+
+		switch (q[i].cmd) {
+		case CMD_MEM_AP_READ32:
+			buf[cmd_index++] = RW_MISC_CMD_READ;
+			h_u32_to_le(&buf[val_index], q[i].mem_ap.addr);
+			val_index += 4;
+			break;
+		case CMD_MEM_AP_WRITE32:
+			buf[cmd_index++] = RW_MISC_CMD_ADDRESS;
+			h_u32_to_le(&buf[val_index], q[i].mem_ap.addr);
+			val_index += 4;
+			buf[cmd_index++] = RW_MISC_CMD_WRITE;
+			h_u32_to_le(&buf[val_index], q[i].mem_ap.data);
+			val_index += 4;
+			break;
+		default:
+			/* Not supposed to happen */
+			return ERROR_FAIL;
+		}
+	}
+	/* pad after last command */
+	while (!IS_ALIGNED(cmd_index, 4))
+		buf[cmd_index++] = 0;
+
+	int retval = stlink_usb_rw_misc_out(handle, items, buf);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = stlink_usb_rw_misc_in(handle, items, buf);
+	if (retval != ERROR_OK)
+		return retval;
+
+	ap_num = DP_APSEL_INVALID;
+	val_index = 0;
+	unsigned int err_index = 4 * items;
+	for (unsigned int i = 0; i < len; i++) {
+		uint32_t errcode = le_to_h_u32(&buf[err_index]);
+		if (errcode != STLINK_DEBUG_ERR_OK) {
+			LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
+			return ERROR_FAIL;
+		}
+		if (ap_num != q[i].mem_ap.ap->ap_num) {
+			ap_num = q[i].mem_ap.ap->ap_num;
+			err_index += 4;
+			val_index += 4;
+			errcode = le_to_h_u32(&buf[err_index]);
+			if (errcode != STLINK_DEBUG_ERR_OK) {
+				LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
+				return ERROR_FAIL;
+			}
+		}
+
+		if (q[i].cmd == CMD_MEM_AP_READ32) {
+			*q[i].mem_ap.p_data = le_to_h_u32(&buf[val_index]);
+		} else { /* q[i]->cmd == CMD_MEM_AP_WRITE32 */
+			err_index += 4;
+			val_index += 4;
+			errcode = le_to_h_u32(&buf[err_index]);
+			if (errcode != STLINK_DEBUG_ERR_OK) {
+				LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
+				return ERROR_FAIL;
+			}
+		}
+		err_index += 4;
+		val_index += 4;
+	}
+
+	return ERROR_OK;
+}
+
+static int stlink_usb_buf_rw_segment(void *handle, const struct dap_queue *q, unsigned int count)
+{
+	uint32_t bufsize = count * CMD_MEM_AP_2_SIZE(q[0].cmd);
+	uint8_t buf[bufsize];
+	uint8_t ap_num = q[0].mem_ap.ap->ap_num;
+	uint32_t addr = q[0].mem_ap.addr;
+	uint32_t csw = q[0].mem_ap.csw;
+
+	int retval = stlink_dap_open_ap(ap_num);
+	if (retval != ERROR_OK)
+		return retval;
+
+	switch (q[0].cmd) {
+	case CMD_MEM_AP_WRITE8:
+		for (unsigned int i = 0; i < count; i++)
+			buf[i] = q[i].mem_ap.data >> 8 * (q[i].mem_ap.addr & 3);
+		return stlink_usb_write_mem8(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
+
+	case CMD_MEM_AP_WRITE16:
+		for (unsigned int i = 0; i < count; i++)
+			h_u16_to_le(&buf[2 * i], q[i].mem_ap.data >> 8 * (q[i].mem_ap.addr & 2));
+		return stlink_usb_write_mem16(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
+
+	case CMD_MEM_AP_WRITE32:
+		for (unsigned int i = 0; i < count; i++)
+			h_u32_to_le(&buf[4 * i], q[i].mem_ap.data);
+		if (count > 1 && q[0].mem_ap.addr == q[1].mem_ap.addr)
+			return stlink_usb_write_mem32_noaddrinc(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
+		else
+			return stlink_usb_write_mem32(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
+
+	case CMD_MEM_AP_READ8:
+		retval = stlink_usb_read_mem8(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
+		if (retval == ERROR_OK)
+			for (unsigned int i = 0; i < count; i++)
+				*q[i].mem_ap.p_data = buf[i] << 8 * (q[i].mem_ap.addr & 3);
+		return retval;
+
+	case CMD_MEM_AP_READ16:
+		retval = stlink_usb_read_mem16(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
+		if (retval == ERROR_OK)
+			for (unsigned int i = 0; i < count; i++)
+				*q[i].mem_ap.p_data = le_to_h_u16(&buf[2 * i]) << 8 * (q[i].mem_ap.addr & 2);
+		return retval;
+
+	case CMD_MEM_AP_READ32:
+		if (count > 1 && q[0].mem_ap.addr == q[1].mem_ap.addr)
+			retval = stlink_usb_read_mem32_noaddrinc(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
+		else
+			retval = stlink_usb_read_mem32(stlink_dap_handle, ap_num, csw, addr, bufsize, buf);
+		if (retval == ERROR_OK)
+			for (unsigned int i = 0; i < count; i++)
+				*q[i].mem_ap.p_data = le_to_h_u32(&buf[4 * i]);
+		return retval;
+
+	default:
+		return ERROR_FAIL;
+	};
+}
+
+/* TODO: recover these values with cmd STLINK_DEBUG_APIV2_RW_MISC_GET_MAX (0x53) */
+#define STLINK_V2_RW_MISC_SIZE (64)
+#define STLINK_V3_RW_MISC_SIZE (1227)
+
+static int stlink_usb_count_misc_rw_queue(void *handle, const struct dap_queue *q, unsigned int len,
+		unsigned int *pkt_items)
+{
+	struct stlink_usb_handle_s *h = handle;
+	unsigned int i, items = 0;
+	int ap_num = DP_APSEL_INVALID;
+	unsigned int misc_max_items = (h->version.stlink == 2) ? STLINK_V2_RW_MISC_SIZE : STLINK_V3_RW_MISC_SIZE;
+
+	if (!(h->version.flags & STLINK_F_HAS_RW_MISC))
+		return 0;
+	/*
+	 * RW_MISC sequence doesn't lock the st-link, so are not safe in shared mode.
+	 * Don't use it with TCP backend to prevent any issue in case of sharing.
+	 * This further degrades the performance, on top of TCP server overhead.
+	 */
+	if (h->backend == &stlink_tcp_backend)
+		return 0;
+
+	for (i = 0; i < len; i++) {
+		if (q[i].cmd != CMD_MEM_AP_READ32 && q[i].cmd != CMD_MEM_AP_WRITE32)
+			break;
+		unsigned int count = 1;
+		if (ap_num != q[i].mem_ap.ap->ap_num) {
+			count++;
+			ap_num = q[i].mem_ap.ap->ap_num;
+		}
+		if (q[i].cmd == CMD_MEM_AP_WRITE32)
+			count++;
+		if (items + count > misc_max_items)
+			break;
+		items += count;
+	}
+
+	*pkt_items = items;
+
+	return i;
+}
+
+static int stlink_usb_count_buf_rw_queue(const struct dap_queue *q, unsigned int len)
+{
+	uint32_t incr = CMD_MEM_AP_2_SIZE(q[0].cmd);
+	unsigned int len_max;
+
+	if (incr == 1)
+		len_max = stlink_usb_block(stlink_dap_handle);
+	else
+		len_max = STLINK_MAX_RW16_32 / incr;
+
+	/* check for no address increment, 32 bits only */
+	if (len > 1 && incr == 4 && q[0].mem_ap.addr == q[1].mem_ap.addr)
+		incr = 0;
+
+	if (len > len_max)
+		len = len_max;
+
+	for (unsigned int i = 1; i < len; i++)
+		if (q[i].cmd != q[0].cmd ||
+			q[i].mem_ap.ap != q[0].mem_ap.ap ||
+			q[i].mem_ap.csw != q[0].mem_ap.csw ||
+			q[i].mem_ap.addr != q[i - 1].mem_ap.addr + incr)
+			return i;
+
+	return len;
+}
+
+static int stlink_usb_mem_rw_queue(void *handle, const struct dap_queue *q, unsigned int len, unsigned int *skip)
+{
+	unsigned int count, misc_items = 0;
+	int retval;
+
+	unsigned int count_misc = stlink_usb_count_misc_rw_queue(handle, q, len, &misc_items);
+	unsigned int count_buf = stlink_usb_count_buf_rw_queue(q, len);
+
+	if (count_misc > count_buf) {
+		count = count_misc;
+		retval = stlink_usb_misc_rw_segment(handle, q, count, misc_items);
+	} else {
+		count = count_buf;
+		retval = stlink_usb_buf_rw_segment(handle, q, count_buf);
+	}
+	if (retval != ERROR_OK)
+		return retval;
+
+	*skip = count;
+	return ERROR_OK;
+}
+
+static void stlink_dap_run_internal(struct adiv5_dap *dap)
+{
+	int retval = stlink_dap_check_reconnect(dap);
+	if (retval != ERROR_OK) {
+		stlink_dap_handle->queue_index = 0;
+		stlink_dap_record_error(retval);
+		return;
+	}
+
+	unsigned int i = stlink_dap_handle->queue_index;
+	struct dap_queue *q = &stlink_dap_handle->queue[0];
+
+	while (i && stlink_dap_get_error() == ERROR_OK) {
+		unsigned int skip = 1;
+
+		switch (q->cmd) {
+		case CMD_DP_READ:
+			retval = stlink_dap_dp_read(q->dp_r.dap, q->dp_r.reg, q->dp_r.p_data);
+			break;
+		case CMD_DP_WRITE:
+			retval = stlink_dap_dp_write(q->dp_w.dap, q->dp_w.reg, q->dp_w.data);
+			break;
+		case CMD_AP_READ:
+			retval = stlink_dap_ap_read(q->ap_r.ap, q->ap_r.reg, q->ap_r.p_data);
+			break;
+		case CMD_AP_WRITE:
+			/* ignore increment packed, not supported */
+			if (q->ap_w.reg == MEM_AP_REG_CSW)
+				q->ap_w.data &= ~CSW_ADDRINC_PACKED;
+			retval = stlink_dap_ap_write(q->ap_w.ap, q->ap_w.reg, q->ap_w.data);
+			break;
+
+		case CMD_MEM_AP_READ8:
+		case CMD_MEM_AP_READ16:
+		case CMD_MEM_AP_READ32:
+		case CMD_MEM_AP_WRITE8:
+		case CMD_MEM_AP_WRITE16:
+		case CMD_MEM_AP_WRITE32:
+			retval = stlink_usb_mem_rw_queue(stlink_dap_handle, q, i, &skip);
+			break;
+
+		default:
+			LOG_ERROR("ST-Link: Unknown queue command %d", q->cmd);
+			retval = ERROR_FAIL;
+			break;
+		}
+		stlink_dap_record_error(retval);
+		q += skip;
+		i -= skip;
+	}
+
+	stlink_dap_handle->queue_index = 0;
+}
+
 /** */
-static int stlink_dap_op_run(struct adiv5_dap *dap)
+static int stlink_dap_run_finalize(struct adiv5_dap *dap)
 {
 	uint32_t ctrlstat, pwrmask;
 	int retval, saved_retval;
@@ -4020,7 +4592,7 @@ static int stlink_dap_op_run(struct adiv5_dap *dap)
 	 */
 	if (dap->stlink_flush_ap_write) {
 		dap->stlink_flush_ap_write = false;
-		retval = stlink_dap_op_queue_dp_read(dap, DP_RDBUFF, NULL);
+		retval = stlink_dap_dp_read(dap, DP_RDBUFF, NULL);
 		if (retval != ERROR_OK) {
 			dap->do_reconnect = true;
 			return retval;
@@ -4029,12 +4601,7 @@ static int stlink_dap_op_run(struct adiv5_dap *dap)
 
 	saved_retval = stlink_dap_get_and_clear_error();
 
-	retval = stlink_dap_op_queue_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
-	if (retval != ERROR_OK) {
-		dap->do_reconnect = true;
-		return retval;
-	}
-	retval = stlink_dap_get_and_clear_error();
+	retval = stlink_dap_dp_read(dap, DP_CTRL_STAT, &ctrlstat);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("Fail reading CTRL/STAT register. Force reconnect");
 		dap->do_reconnect = true;
@@ -4043,15 +4610,10 @@ static int stlink_dap_op_run(struct adiv5_dap *dap)
 
 	if (ctrlstat & SSTICKYERR) {
 		if (stlink_dap_handle->st_mode == STLINK_MODE_DEBUG_JTAG)
-			retval = stlink_dap_op_queue_dp_write(dap, DP_CTRL_STAT,
+			retval = stlink_dap_dp_write(dap, DP_CTRL_STAT,
 					ctrlstat & (dap->dp_ctrl_stat | SSTICKYERR));
 		else
-			retval = stlink_dap_op_queue_dp_write(dap, DP_ABORT, STKERRCLR);
-		if (retval != ERROR_OK) {
-			dap->do_reconnect = true;
-			return retval;
-		}
-		retval = stlink_dap_get_and_clear_error();
+			retval = stlink_dap_dp_write(dap, DP_ABORT, STKERRCLR);
 		if (retval != ERROR_OK) {
 			dap->do_reconnect = true;
 			return retval;
@@ -4066,6 +4628,12 @@ static int stlink_dap_op_run(struct adiv5_dap *dap)
 	return saved_retval;
 }
 
+static int stlink_dap_op_queue_run(struct adiv5_dap *dap)
+{
+	stlink_dap_run_internal(dap);
+	return stlink_dap_run_finalize(dap);
+}
+
 /** */
 static void stlink_dap_op_quit(struct adiv5_dap *dap)
 {
@@ -4076,6 +4644,182 @@ static void stlink_dap_op_quit(struct adiv5_dap *dap)
 		LOG_ERROR("Error closing APs");
 }
 
+static int stlink_dap_op_queue_dp_read(struct adiv5_dap *dap, unsigned int reg,
+	uint32_t *data)
+{
+	if (stlink_dap_get_error() != ERROR_OK)
+		return ERROR_OK;
+
+	unsigned int i = stlink_dap_handle->queue_index++;
+	struct dap_queue *q = &stlink_dap_handle->queue[i];
+	q->cmd = CMD_DP_READ;
+	q->dp_r.reg = reg;
+	q->dp_r.dap = dap;
+	q->dp_r.p_data = data;
+
+	if (i == MAX_QUEUE_DEPTH - 1)
+		stlink_dap_run_internal(dap);
+
+	return ERROR_OK;
+}
+
+static int stlink_dap_op_queue_dp_write(struct adiv5_dap *dap, unsigned int reg,
+	uint32_t data)
+{
+	if (stlink_dap_get_error() != ERROR_OK)
+		return ERROR_OK;
+
+	unsigned int i = stlink_dap_handle->queue_index++;
+	struct dap_queue *q = &stlink_dap_handle->queue[i];
+	q->cmd = CMD_DP_WRITE;
+	q->dp_w.reg = reg;
+	q->dp_w.dap = dap;
+	q->dp_w.data = data;
+
+	if (i == MAX_QUEUE_DEPTH - 1)
+		stlink_dap_run_internal(dap);
+
+	return ERROR_OK;
+}
+
+static int stlink_dap_op_queue_ap_read(struct adiv5_ap *ap, unsigned int reg,
+	uint32_t *data)
+{
+	if (stlink_dap_get_error() != ERROR_OK)
+		return ERROR_OK;
+
+	unsigned int i = stlink_dap_handle->queue_index++;
+	struct dap_queue *q = &stlink_dap_handle->queue[i];
+
+	/* test STLINK_F_HAS_CSW implicitly tests STLINK_F_HAS_MEM_16BIT, STLINK_F_HAS_MEM_RD_NO_INC
+	 * and STLINK_F_HAS_RW_MISC */
+	if ((stlink_dap_handle->version.flags & STLINK_F_HAS_CSW) &&
+			(reg == MEM_AP_REG_DRW || reg == MEM_AP_REG_BD0 || reg == MEM_AP_REG_BD1 ||
+			 reg == MEM_AP_REG_BD2 || reg == MEM_AP_REG_BD3)) {
+		/* de-queue previous write-TAR */
+		struct dap_queue *prev_q = q - 1;
+		if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == MEM_AP_REG_TAR) {
+			stlink_dap_handle->queue_index = i;
+			i--;
+			q = prev_q;
+			prev_q--;
+		}
+		/* de-queue previous write-CSW if it didn't changed ap->csw_default */
+		if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == MEM_AP_REG_CSW &&
+				!prev_q->ap_w.changes_csw_default) {
+			stlink_dap_handle->queue_index = i;
+			q = prev_q;
+		}
+
+		switch (ap->csw_value & CSW_SIZE_MASK) {
+		case CSW_8BIT:
+			q->cmd = CMD_MEM_AP_READ8;
+			break;
+		case CSW_16BIT:
+			q->cmd = CMD_MEM_AP_READ16;
+			break;
+		case CSW_32BIT:
+			q->cmd = CMD_MEM_AP_READ32;
+			break;
+		default:
+			LOG_ERROR("ST-Link: Unsupported CSW size %d", ap->csw_value & CSW_SIZE_MASK);
+			stlink_dap_record_error(ERROR_FAIL);
+			return ERROR_FAIL;
+		}
+
+		q->mem_ap.addr = (reg == MEM_AP_REG_DRW) ? ap->tar_value : ((ap->tar_value & ~0x0f) | (reg & 0x0c));
+		q->mem_ap.ap = ap;
+		q->mem_ap.p_data = data;
+		q->mem_ap.csw = ap->csw_default;
+
+		/* force TAR and CSW update */
+		ap->tar_valid = false;
+		ap->csw_value = 0;
+	} else {
+		q->cmd = CMD_AP_READ;
+		q->ap_r.reg = reg;
+		q->ap_r.ap = ap;
+		q->ap_r.p_data = data;
+	}
+
+	if (i == MAX_QUEUE_DEPTH - 1)
+		stlink_dap_run_internal(ap->dap);
+
+	return ERROR_OK;
+}
+
+static int stlink_dap_op_queue_ap_write(struct adiv5_ap *ap, unsigned int reg,
+	uint32_t data)
+{
+	if (stlink_dap_get_error() != ERROR_OK)
+		return ERROR_OK;
+
+	unsigned int i = stlink_dap_handle->queue_index++;
+	struct dap_queue *q = &stlink_dap_handle->queue[i];
+
+	/* test STLINK_F_HAS_CSW implicitly tests STLINK_F_HAS_MEM_16BIT, STLINK_F_HAS_MEM_WR_NO_INC
+	 * and STLINK_F_HAS_RW_MISC */
+	if ((stlink_dap_handle->version.flags & STLINK_F_HAS_CSW) &&
+			(reg == MEM_AP_REG_DRW || reg == MEM_AP_REG_BD0 || reg == MEM_AP_REG_BD1 ||
+			 reg == MEM_AP_REG_BD2 || reg == MEM_AP_REG_BD3)) {
+		/* de-queue previous write-TAR */
+		struct dap_queue *prev_q = q - 1;
+		if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == MEM_AP_REG_TAR) {
+			stlink_dap_handle->queue_index = i;
+			i--;
+			q = prev_q;
+			prev_q--;
+		}
+		/* de-queue previous write-CSW if it didn't changed ap->csw_default */
+		if (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == MEM_AP_REG_CSW &&
+				!prev_q->ap_w.changes_csw_default) {
+			stlink_dap_handle->queue_index = i;
+			q = prev_q;
+		}
+
+		switch (ap->csw_value & CSW_SIZE_MASK) {
+		case CSW_8BIT:
+			q->cmd = CMD_MEM_AP_WRITE8;
+			break;
+		case CSW_16BIT:
+			q->cmd = CMD_MEM_AP_WRITE16;
+			break;
+		case CSW_32BIT:
+			q->cmd = CMD_MEM_AP_WRITE32;
+			break;
+		default:
+			LOG_ERROR("ST-Link: Unsupported CSW size %d", ap->csw_value & CSW_SIZE_MASK);
+			stlink_dap_record_error(ERROR_FAIL);
+			return ERROR_FAIL;
+		}
+
+		q->mem_ap.addr = (reg == MEM_AP_REG_DRW) ? ap->tar_value : ((ap->tar_value & ~0x0f) | (reg & 0x0c));
+		q->mem_ap.ap = ap;
+		q->mem_ap.data = data;
+		q->mem_ap.csw = ap->csw_default;
+
+		/* force TAR and CSW update */
+		ap->tar_valid = false;
+		ap->csw_value = 0;
+	} else {
+		q->cmd = CMD_AP_WRITE;
+		q->ap_w.reg = reg;
+		q->ap_w.ap = ap;
+		q->ap_w.data = data;
+		if (reg == MEM_AP_REG_CSW && ap->csw_default != last_csw_default[ap->ap_num]) {
+			q->ap_w.changes_csw_default = true;
+			last_csw_default[ap->ap_num] = ap->csw_default;
+		} else {
+			q->ap_w.changes_csw_default = false;
+		}
+	}
+
+	if (i == MAX_QUEUE_DEPTH - 1)
+		stlink_dap_run_internal(ap->dap);
+
+	return ERROR_OK;
+}
+
 static int stlink_swim_op_srst(void)
 {
 	return stlink_swim_generate_rst(stlink_dap_handle);
@@ -4091,7 +4835,7 @@ static int stlink_swim_op_read_mem(uint32_t addr, uint32_t size,
 	count *= size;
 
 	while (count) {
-		bytes_remaining = (count > STLINK_DATA_SIZE) ? STLINK_DATA_SIZE : count;
+		bytes_remaining = (count > STLINK_SWIM_DATA_SIZE) ? STLINK_SWIM_DATA_SIZE : count;
 		retval = stlink_swim_readbytes(stlink_dap_handle, addr, bytes_remaining, buffer);
 		if (retval != ERROR_OK)
 			return retval;
@@ -4114,7 +4858,7 @@ static int stlink_swim_op_write_mem(uint32_t addr, uint32_t size,
 	count *= size;
 
 	while (count) {
-		bytes_remaining = (count > STLINK_DATA_SIZE) ? STLINK_DATA_SIZE : count;
+		bytes_remaining = (count > STLINK_SWIM_DATA_SIZE) ? STLINK_SWIM_DATA_SIZE : count;
 		retval = stlink_swim_writebytes(stlink_dap_handle, addr, bytes_remaining, buffer);
 		if (retval != ERROR_OK)
 			return retval;
@@ -4223,6 +4967,43 @@ COMMAND_HANDLER(stlink_dap_backend_command)
 	return ERROR_OK;
 }
 
+#define BYTES_PER_LINE 16
+COMMAND_HANDLER(stlink_dap_cmd_command)
+{
+	unsigned int rx_n, tx_n;
+	struct stlink_usb_handle_s *h = stlink_dap_handle;
+
+	if (CMD_ARGC < 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], rx_n);
+	tx_n = CMD_ARGC - 1;
+	if (tx_n > STLINK_SG_SIZE || rx_n > STLINK_DATA_SIZE) {
+		LOG_ERROR("max %x byte sent and %d received", STLINK_SG_SIZE, STLINK_DATA_SIZE);
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	stlink_usb_init_buffer(h, h->rx_ep, rx_n);
+
+	for (unsigned int i = 0; i < tx_n; i++) {
+		uint8_t byte;
+		COMMAND_PARSE_NUMBER(u8, CMD_ARGV[i + 1], byte);
+		h->cmdbuf[h->cmdidx++] = byte;
+	}
+
+	int retval = stlink_usb_xfer_noerrcheck(h, h->databuf, rx_n);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Error %d", retval);
+		return retval;
+	}
+
+	for (unsigned int i = 0; i < rx_n; i++)
+		command_print_sameline(CMD, "0x%02x%c", h->databuf[i],
+			((i == (rx_n - 1)) || ((i % BYTES_PER_LINE) == (BYTES_PER_LINE - 1))) ? '\n' : ' ');
+
+	return ERROR_OK;
+}
+
 /** */
 static const struct command_registration stlink_dap_subcommand_handlers[] = {
 	{
@@ -4246,6 +5027,13 @@ static const struct command_registration stlink_dap_subcommand_handlers[] = {
 		.help = "select which ST-Link backend to use",
 		.usage = "usb | tcp [port]",
 	},
+	{
+		.name = "cmd",
+		.handler = stlink_dap_cmd_command,
+		.mode = COMMAND_EXEC,
+		.help = "send arbitrary command",
+		.usage = "rx_n (tx_byte)+",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -4360,7 +5148,7 @@ static const struct dap_ops stlink_dap_ops = {
 	.queue_ap_read = stlink_dap_op_queue_ap_read,
 	.queue_ap_write = stlink_dap_op_queue_ap_write,
 	.queue_ap_abort = stlink_dap_op_queue_ap_abort,
-	.run = stlink_dap_op_run,
+	.run = stlink_dap_op_queue_run,
 	.sync = NULL, /* optional */
 	.quit = stlink_dap_op_quit, /* optional */
 };