X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fjtag%2Fdrivers%2Fstlink_usb.c;h=818fccd333b7e1004aef7faebff77373060533d3;hp=73169e1e4866c75aa57896d402f6a8e32aa0a651;hb=da15f9f8c29064f0124e60ac0ac21ddd11fdcc2c;hpb=faaa42283f5c06830fe997b80d6c64f6469c6cf0

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 73169e1e48..818fccd333 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -35,11 +35,13 @@
 /* project specific includes */
 #include <helper/binarybuffer.h>
 #include <helper/bits.h>
+#include <helper/system.h>
 #include <jtag/interface.h>
 #include <jtag/hla/hla_layout.h>
 #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>
 
@@ -77,7 +79,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)
 
@@ -89,20 +91,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,
@@ -165,6 +179,67 @@ 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;
+		} 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 {
 	/** */
@@ -208,6 +283,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;
 };
 
 /** */
@@ -412,21 +491,22 @@ 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)
-#define STLINK_F_HAS_SWD_SET_FREQ       BIT(1)
-#define STLINK_F_HAS_JTAG_SET_FREQ      BIT(2)
-#define STLINK_F_HAS_MEM_16BIT          BIT(3)
-#define STLINK_F_HAS_GETLASTRWSTATUS2   BIT(4)
-#define STLINK_F_HAS_DAP_REG            BIT(5)
-#define STLINK_F_QUIRK_JTAG_DP_READ     BIT(6)
-#define STLINK_F_HAS_AP_INIT            BIT(7)
-#define STLINK_F_HAS_DPBANKSEL          BIT(8)
-#define STLINK_F_HAS_RW8_512BYTES       BIT(9)
-#define STLINK_F_FIX_CLOSE_AP           BIT(10)
+#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_CSW                STLINK_F_HAS_DPBANKSEL
 
 #define STLINK_REGSEL_IS_FPU(x)         ((x) > 0x1F)
 
@@ -474,7 +554,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;
@@ -496,13 +576,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;
@@ -572,7 +647,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);
 
@@ -640,7 +715,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);
@@ -674,7 +749,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];
@@ -713,7 +788,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);
@@ -746,7 +821,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);
 
@@ -794,7 +869,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;
@@ -827,7 +902,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);
@@ -872,7 +947,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;
@@ -941,7 +1016,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]) {
@@ -1080,7 +1155,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);
 
@@ -1145,7 +1220,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);
 
@@ -1260,6 +1335,7 @@ static int stlink_usb_version(void *handle)
 			flags |= STLINK_F_FIX_CLOSE_AP;
 
 		/* Banked regs (DPv1 & DPv2) support from V2J32 */
+		/* Memory R/W supports CSW from V2J32 */
 		if (h->version.jtag >= 32)
 			flags |= STLINK_F_HAS_DPBANKSEL;
 
@@ -1290,6 +1366,7 @@ static int stlink_usb_version(void *handle)
 		flags |= STLINK_F_FIX_CLOSE_AP;
 
 		/* Banked regs (DPv1 & DPv2) support from V3J2 */
+		/* Memory R/W supports CSW from V3J2 */
 		if (h->version.jtag >= 2)
 			flags |= STLINK_F_HAS_DPBANKSEL;
 
@@ -1359,7 +1436,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;
@@ -1383,7 +1460,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;
@@ -1409,7 +1486,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);
 
@@ -1431,7 +1508,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
@@ -1479,7 +1556,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);
@@ -1532,7 +1609,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);
 
@@ -1573,7 +1650,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)
@@ -1630,7 +1707,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);
@@ -1808,7 +1886,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)
@@ -1841,7 +1919,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);
@@ -1871,7 +1949,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) {
@@ -1909,7 +1987,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);
 
@@ -1930,7 +2008,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);
 
@@ -1952,7 +2030,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;
@@ -1967,7 +2045,7 @@ static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
 			return res;
 
 		size_t bytes_avail = le_to_h_u16(h->databuf);
-		*size = bytes_avail < *size ? bytes_avail : *size - 1;
+		*size = bytes_avail < *size ? bytes_avail : *size;
 
 		if (*size > 0) {
 			res = stlink_usb_read_trace(handle, buf, *size);
@@ -2003,7 +2081,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");
@@ -2045,7 +2123,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);
@@ -2068,7 +2146,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);
 
@@ -2090,7 +2168,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);
@@ -2122,7 +2200,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);
 
@@ -2151,7 +2229,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);
@@ -2173,7 +2251,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);
@@ -2194,7 +2272,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
@@ -2218,7 +2296,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);
 
@@ -2243,7 +2321,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);
@@ -2283,14 +2361,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 */
 	}
 
@@ -2312,7 +2390,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;
@@ -2330,14 +2408,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)) {
@@ -2353,6 +2434,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)
@@ -2369,13 +2453,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)) {
@@ -2391,6 +2478,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);
 
@@ -2401,17 +2491,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");
@@ -2426,6 +2524,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);
 
@@ -2438,17 +2539,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");
@@ -2463,6 +2572,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);
 
@@ -2473,13 +2585,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) {
@@ -2495,6 +2615,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);
 
@@ -2507,13 +2630,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) {
@@ -2529,6 +2660,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);
 
@@ -2546,8 +2680,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;
@@ -2562,7 +2696,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);
 
@@ -2576,7 +2709,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
@@ -2587,11 +2719,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)
@@ -2603,16 +2734,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)
@@ -2626,8 +2758,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;
@@ -2669,7 +2808,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;
@@ -2683,14 +2822,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;
@@ -2706,6 +2845,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)
 {
@@ -2989,7 +3135,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);
@@ -3016,7 +3162,7 @@ static int stlink_close(void *handle)
  *    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
  */
-static char *stlink_usb_get_alternate_serial(libusb_device_handle *device,
+static char *stlink_usb_get_alternate_serial(struct libusb_device_handle *device,
 		struct libusb_device_descriptor *dev_desc)
 {
 	int usb_retval;
@@ -3068,7 +3214,7 @@ static char *stlink_usb_get_alternate_serial(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)
@@ -3088,7 +3234,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;
 
 	/*
@@ -3133,6 +3279,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;
@@ -3202,7 +3349,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];
@@ -3311,7 +3458,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 */
@@ -3482,7 +3629,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;
 	}
 
@@ -3493,8 +3640,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;
@@ -3538,8 +3685,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");
@@ -3589,7 +3736,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;
@@ -3608,7 +3755,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;
@@ -3634,7 +3781,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;
@@ -3657,7 +3804,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;
@@ -3723,9 +3870,6 @@ static struct hl_interface_param_s stlink_dap_param;
 static DECLARE_BITMAP(opened_ap, 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)
 {
@@ -3742,6 +3886,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;
@@ -3887,8 +4036,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;
@@ -3899,11 +4047,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 */
@@ -3917,12 +4061,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;
 
@@ -3938,54 +4081,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;
@@ -3993,7 +4122,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;
 }
 
 /** */
@@ -4003,8 +4132,151 @@ static int stlink_dap_op_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
 	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);
+		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:
+		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;
+	};
+}
+
+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;
+
+	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 = stlink_usb_count_buf_rw_queue(q, len);
+
+	int retval = stlink_usb_buf_rw_segment(handle, q, count);
+	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;
@@ -4019,7 +4291,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;
@@ -4028,12 +4300,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;
@@ -4042,15 +4309,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;
@@ -4065,6 +4327,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)
 {
@@ -4075,6 +4343,170 @@ 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];
+
+	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 (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == MEM_AP_REG_CSW) {
+			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];
+
+	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 (i && prev_q->cmd == CMD_AP_WRITE && prev_q->ap_w.ap == ap && prev_q->ap_w.reg == MEM_AP_REG_CSW) {
+			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 (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);
@@ -4090,7 +4522,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;
@@ -4113,7 +4545,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;
@@ -4222,6 +4654,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[] = {
 	{
@@ -4245,6 +4714,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
 };
 
@@ -4359,7 +4835,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 */
 };