jtag/drivers/cmsis_dap_bulk: use asynchronous libusb transfer

The synchronous libusb_bulk_transfer() always waits
for the transfer to complete. Therefore it does not allow issuing
multiple USB requests as used on HID backend.

Switch to asynchrounous libusb_submit_transfer().

With this patch a good USB FS based CMSIS-DAPv2 adapter
almost doubles the throughput:

 adapter speed: 20000 kHz
 poll off

 > load_image /run/user/1000/ram256k.bin 0x20000000
 262144 bytes written at address 0x20000000
 downloaded 262144 bytes in 0.428576s (597.327 KiB/s)

 > dump_image /dev/null 0x20000000 0x40000
 dumped 262144 bytes in 0.572875s (446.869 KiB/s)

Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Change-Id: Ic6168ea4eca4f6bd1d8ad541a07a8d70427cc509
Reviewed-on: https://review.openocd.org/c/openocd/+/7365
Reviewed-by: zapb <dev@zapb.de>
Tested-by: jenkins

4 files changed, 319 insertions, 63 deletions

diff --git a/src/jtag/drivers/cmsis_dap.c b/src/jtag/drivers/cmsis_dap.c
index 0383243..bc86eb4 100644
--- a/src/jtag/drivers/cmsis_dap.c
+++ b/src/jtag/drivers/cmsis_dap.c
@@ -225,6 +225,12 @@ struct pending_scan_result {
 	unsigned int buffer_offset;
 };
 
+/* Read mode */
+enum cmsis_dap_blocking {
+	CMSIS_DAP_NON_BLOCKING,
+	CMSIS_DAP_BLOCKING
+};
+
 /* Each block in FIFO can contain up to pending_queue_len transfers */
 static unsigned int pending_queue_len;
 static unsigned int tfer_max_command_size;
@@ -315,7 +321,7 @@ static void cmsis_dap_flush_read(struct cmsis_dap *dap)
 	 * USB close/open so we need to flush up to 64 old packets
 	 * to be sure all buffers are empty */
 	for (i = 0; i < 64; i++) {
-		int retval = dap->backend->read(dap, 10);
+		int retval = dap->backend->read(dap, 10, NULL);
 		if (retval == ERROR_TIMEOUT_REACHED)
 			break;
 	}
@@ -326,10 +332,13 @@ static void cmsis_dap_flush_read(struct cmsis_dap *dap)
 /* Send a message and receive the reply */
 static int cmsis_dap_xfer(struct cmsis_dap *dap, int txlen)
 {
+	if (dap->write_count + dap->read_count) {
+		LOG_ERROR("internal: queue not empty before xfer");
+	}
 	if (dap->pending_fifo_block_count) {
 		LOG_ERROR("pending %u blocks, flushing", dap->pending_fifo_block_count);
 		while (dap->pending_fifo_block_count) {
-			dap->backend->read(dap, 10);
+			dap->backend->read(dap, 10, NULL);
 			dap->pending_fifo_block_count--;
 		}
 		dap->pending_fifo_put_idx = 0;
@@ -342,7 +351,7 @@ static int cmsis_dap_xfer(struct cmsis_dap *dap, int txlen)
 		return retval;
 
 	/* get reply */
-	retval = dap->backend->read(dap, LIBUSB_TIMEOUT_MS);
+	retval = dap->backend->read(dap, LIBUSB_TIMEOUT_MS, NULL);
 	if (retval < 0)
 		return retval;
 
@@ -750,6 +759,16 @@ static int cmsis_dap_cmd_dap_swo_data(
 }
 
 
+static void cmsis_dap_swd_discard_all_pending(struct cmsis_dap *dap)
+{
+	for (unsigned int i = 0; i < MAX_PENDING_REQUESTS; i++)
+		dap->pending_fifo[i].transfer_count = 0;
+
+	dap->pending_fifo_put_idx = 0;
+	dap->pending_fifo_get_idx = 0;
+	dap->pending_fifo_block_count = 0;
+}
+
 static void cmsis_dap_swd_write_from_queue(struct cmsis_dap *dap)
 {
 	uint8_t *command = dap->command;
@@ -770,8 +789,10 @@ static void cmsis_dap_swd_write_from_queue(struct cmsis_dap *dap)
 		goto skip;
 	}
 
-	if (block->transfer_count == 0)
+	if (block->transfer_count == 0) {
+		LOG_ERROR("internal: write an empty queue?!");
 		goto skip;
+	}
 
 	bool block_cmd = !cmsis_dap_handle->swd_cmds_differ
 					 && block->transfer_count >= CMD_DAP_TFER_BLOCK_MIN_OPS;
@@ -831,14 +852,12 @@ static void cmsis_dap_swd_write_from_queue(struct cmsis_dap *dap)
 	if (retval < 0) {
 		queued_retval = retval;
 		goto skip;
-	} else {
-		queued_retval = ERROR_OK;
 	}
 
 	dap->pending_fifo_put_idx = (dap->pending_fifo_put_idx + 1) % dap->packet_count;
 	dap->pending_fifo_block_count++;
 	if (dap->pending_fifo_block_count > dap->packet_count)
-		LOG_ERROR("too much pending writes %u", dap->pending_fifo_block_count);
+		LOG_ERROR("internal: too much pending writes %u", dap->pending_fifo_block_count);
 
 	return;
 
@@ -846,21 +865,47 @@ skip:
 	block->transfer_count = 0;
 }
 
-static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, int timeout_ms)
+static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, enum cmsis_dap_blocking blocking)
 {
+	int retval;
 	struct pending_request_block *block = &dap->pending_fifo[dap->pending_fifo_get_idx];
 
-	if (dap->pending_fifo_block_count == 0)
-		LOG_ERROR("no pending write");
+	if (dap->pending_fifo_block_count == 0) {
+		LOG_ERROR("internal: no pending write when reading?!");
+		return;
+	}
+
+	if (queued_retval != ERROR_OK) {
+		/* keep reading blocks until the pipeline is empty */
+		retval = dap->backend->read(dap, 10, NULL);
+		if (retval == ERROR_TIMEOUT_REACHED || retval == 0) {
+			/* timeout means that we flushed the pipeline,
+			 * we can safely discard remaining pending requests */
+			cmsis_dap_swd_discard_all_pending(dap);
+			return;
+		}
+		goto skip;
+	}
 
 	/* get reply */
-	int retval = dap->backend->read(dap, timeout_ms);
-	if (retval == ERROR_TIMEOUT_REACHED && timeout_ms < LIBUSB_TIMEOUT_MS)
+	struct timeval tv = {
+		.tv_sec = 0,
+		.tv_usec = 0
+	};
+	retval = dap->backend->read(dap, LIBUSB_TIMEOUT_MS, blocking ? NULL : &tv);
+	bool timeout = (retval == ERROR_TIMEOUT_REACHED || retval == 0);
+	if (timeout && blocking == CMSIS_DAP_NON_BLOCKING)
 		return;
 
 	if (retval <= 0) {
-		LOG_DEBUG("error reading data");
+		LOG_DEBUG("error reading adapter response");
 		queued_retval = ERROR_FAIL;
+		if (timeout) {
+			/* timeout means that we flushed the pipeline,
+			 * we can safely discard remaining pending requests */
+			cmsis_dap_swd_discard_all_pending(dap);
+			return;
+		}
 		goto skip;
 	}
 
@@ -899,8 +944,9 @@ static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, int timeout_ms)
 		LOG_ERROR("CMSIS-DAP transfer count mismatch: expected %d, got %d",
 			  block->transfer_count, transfer_count);
 
-	LOG_DEBUG_IO("Received results of %d queued transactions FIFO index %u timeout %i",
-		 transfer_count, dap->pending_fifo_get_idx, timeout_ms);
+	LOG_DEBUG_IO("Received results of %d queued transactions FIFO index %u, %s mode",
+				 transfer_count, dap->pending_fifo_get_idx,
+				 blocking ? "blocking" : "nonblocking");
 
 	for (unsigned int i = 0; i < transfer_count; i++) {
 		struct pending_transfer_result *transfer = &(block->transfers[i]);
@@ -932,13 +978,15 @@ skip:
 
 static int cmsis_dap_swd_run_queue(void)
 {
-	if (cmsis_dap_handle->pending_fifo_block_count)
-		cmsis_dap_swd_read_process(cmsis_dap_handle, 0);
+	if (cmsis_dap_handle->write_count + cmsis_dap_handle->read_count) {
+		if (cmsis_dap_handle->pending_fifo_block_count)
+			cmsis_dap_swd_read_process(cmsis_dap_handle, CMSIS_DAP_NON_BLOCKING);
 
-	cmsis_dap_swd_write_from_queue(cmsis_dap_handle);
+		cmsis_dap_swd_write_from_queue(cmsis_dap_handle);
+	}
 
 	while (cmsis_dap_handle->pending_fifo_block_count)
-		cmsis_dap_swd_read_process(cmsis_dap_handle, LIBUSB_TIMEOUT_MS);
+		cmsis_dap_swd_read_process(cmsis_dap_handle, CMSIS_DAP_BLOCKING);
 
 	cmsis_dap_handle->pending_fifo_put_idx = 0;
 	cmsis_dap_handle->pending_fifo_get_idx = 0;
@@ -979,10 +1027,16 @@ static unsigned int cmsis_dap_tfer_resp_size(unsigned int write_count,
 
 static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 {
+	/* TARGETSEL register write cannot be queued */
+	if (swd_cmd(false, false, DP_TARGETSEL) == cmd) {
+		queued_retval = cmsis_dap_swd_run_queue();
+
+		cmsis_dap_metacmd_targetsel(data);
+		return;
+	}
+
 	/* Compute sizes of the DAP Transfer command and the expected response
 	 * for all queued and this operation */
-	bool targetsel_cmd = swd_cmd(false, false, DP_TARGETSEL) == cmd;
-
 	unsigned int write_count = cmsis_dap_handle->write_count;
 	unsigned int read_count = cmsis_dap_handle->read_count;
 	bool block_cmd;
@@ -1003,20 +1057,18 @@ static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 													block_cmd);
 	unsigned int max_transfer_count = block_cmd ? 65535 : 255;
 
-	/* Does the DAP Transfer command and the expected response fit into one packet?
-	 * Run the queue also before a targetsel - it cannot be queued */
+	/* Does the DAP Transfer command and also its expected response fit into one packet? */
 	if (cmd_size > tfer_max_command_size
 			|| resp_size > tfer_max_response_size
-			|| targetsel_cmd
 			|| write_count + read_count > max_transfer_count) {
 		if (cmsis_dap_handle->pending_fifo_block_count)
-			cmsis_dap_swd_read_process(cmsis_dap_handle, 0);
+			cmsis_dap_swd_read_process(cmsis_dap_handle, CMSIS_DAP_NON_BLOCKING);
 
 		/* Not enough room in the queue. Run the queue. */
 		cmsis_dap_swd_write_from_queue(cmsis_dap_handle);
 
 		if (cmsis_dap_handle->pending_fifo_block_count >= cmsis_dap_handle->packet_count)
-			cmsis_dap_swd_read_process(cmsis_dap_handle, LIBUSB_TIMEOUT_MS);
+			cmsis_dap_swd_read_process(cmsis_dap_handle, CMSIS_DAP_BLOCKING);
 	}
 
 	assert(cmsis_dap_handle->pending_fifo[cmsis_dap_handle->pending_fifo_put_idx].transfer_count < pending_queue_len);
@@ -1024,11 +1076,6 @@ static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 	if (queued_retval != ERROR_OK)
 		return;
 
-	if (targetsel_cmd) {
-		cmsis_dap_metacmd_targetsel(data);
-		return;
-	}
-
 	struct pending_request_block *block = &cmsis_dap_handle->pending_fifo[cmsis_dap_handle->pending_fifo_put_idx];
 	struct pending_transfer_result *transfer = &(block->transfers[block->transfer_count]);
 	transfer->data = data;
@@ -1160,6 +1207,9 @@ static int cmsis_dap_swd_switch_seq(enum swd_special_seq seq)
 	unsigned int s_len;
 	int retval;
 
+	if (swd_mode)
+		queued_retval = cmsis_dap_swd_run_queue();
+
 	if (seq != LINE_RESET &&
 			(output_pins & (SWJ_PIN_SRST | SWJ_PIN_TRST))
 				== (SWJ_PIN_SRST | SWJ_PIN_TRST)) {
@@ -1296,7 +1346,7 @@ static int cmsis_dap_init(void)
 	if (data[0] == 2) {  /* short */
 		uint16_t pkt_sz = data[1] + (data[2] << 8);
 		if (pkt_sz != cmsis_dap_handle->packet_size) {
-			free(cmsis_dap_handle->packet_buffer);
+			cmsis_dap_handle->backend->packet_buffer_free(cmsis_dap_handle);
 			retval = cmsis_dap_handle->backend->packet_buffer_alloc(cmsis_dap_handle, pkt_sz);
 			if (retval != ERROR_OK)
 				goto init_err;
diff --git a/src/jtag/drivers/cmsis_dap.h b/src/jtag/drivers/cmsis_dap.h
index a8554de..187aeb5 100644
--- a/src/jtag/drivers/cmsis_dap.h
+++ b/src/jtag/drivers/cmsis_dap.h
@@ -61,9 +61,11 @@ struct cmsis_dap_backend {
 	const char *name;
 	int (*open)(struct cmsis_dap *dap, uint16_t vids[], uint16_t pids[], const char *serial);
 	void (*close)(struct cmsis_dap *dap);
-	int (*read)(struct cmsis_dap *dap, int timeout_ms);
+	int (*read)(struct cmsis_dap *dap, int transfer_timeout_ms,
+			    struct timeval *wait_timeout);
 	int (*write)(struct cmsis_dap *dap, int len, int timeout_ms);
 	int (*packet_buffer_alloc)(struct cmsis_dap *dap, unsigned int pkt_sz);
+	void (*packet_buffer_free)(struct cmsis_dap *dap);
 };
 
 extern const struct cmsis_dap_backend cmsis_dap_hid_backend;
diff --git a/src/jtag/drivers/cmsis_dap_usb_bulk.c b/src/jtag/drivers/cmsis_dap_usb_bulk.c
index 6599c41..aaab580 100644
--- a/src/jtag/drivers/cmsis_dap_usb_bulk.c
+++ b/src/jtag/drivers/cmsis_dap_usb_bulk.c
@@ -28,8 +28,29 @@
 #include <libusb.h>
 #include <helper/log.h>
 #include <helper/replacements.h>
+#include <jtag/jtag.h>	/* ERROR_JTAG_DEVICE_ERROR only */
 
 #include "cmsis_dap.h"
+#include "libusb_helper.h"
+
+#if !defined(LIBUSB_API_VERSION) || (LIBUSB_API_VERSION < 0x01000105) \
+		|| defined(_WIN32) || defined(__CYGWIN__)
+	#define libusb_dev_mem_alloc(dev, sz) malloc(sz)
+	#define libusb_dev_mem_free(dev, buffer, sz) free(buffer)
+#endif
+
+enum {
+	CMSIS_DAP_TRANSFER_PENDING = 0,	/* must be 0, used in libusb_handle_events_completed */
+	CMSIS_DAP_TRANSFER_IDLE,
+	CMSIS_DAP_TRANSFER_COMPLETED
+};
+
+struct cmsis_dap_bulk_transfer {
+	struct libusb_transfer *transfer;
+	uint8_t *buffer;
+	int status;		/* either CMSIS_DAP_TRANSFER_ enum or error code */
+	int transferred;
+};
 
 struct cmsis_dap_backend_data {
 	struct libusb_context *usb_ctx;
@@ -37,12 +58,16 @@ struct cmsis_dap_backend_data {
 	unsigned int ep_out;
 	unsigned int ep_in;
 	int interface;
+
+	struct cmsis_dap_bulk_transfer command_transfers[MAX_PENDING_REQUESTS];
+	struct cmsis_dap_bulk_transfer response_transfers[MAX_PENDING_REQUESTS];
 };
 
 static int cmsis_dap_usb_interface = -1;
 
 static void cmsis_dap_usb_close(struct cmsis_dap *dap);
 static int cmsis_dap_usb_alloc(struct cmsis_dap *dap, unsigned int pkt_sz);
+static void cmsis_dap_usb_free(struct cmsis_dap *dap);
 
 static int cmsis_dap_usb_open(struct cmsis_dap *dap, uint16_t vids[], uint16_t pids[], const char *serial)
 {
@@ -358,6 +383,24 @@ static int cmsis_dap_usb_open(struct cmsis_dap *dap, uint16_t vids[], uint16_t p
 			dap->bdata->ep_in = ep_in;
 			dap->bdata->interface = interface_num;
 
+			for (unsigned int idx = 0; idx < MAX_PENDING_REQUESTS; idx++) {
+				dap->bdata->command_transfers[idx].status = CMSIS_DAP_TRANSFER_IDLE;
+				dap->bdata->command_transfers[idx].transfer = libusb_alloc_transfer(0);
+				if (!dap->bdata->command_transfers[idx].transfer) {
+					LOG_ERROR("unable to allocate USB transfer");
+					cmsis_dap_usb_close(dap);
+					return ERROR_FAIL;
+				}
+
+				dap->bdata->response_transfers[idx].status = CMSIS_DAP_TRANSFER_IDLE;
+				dap->bdata->response_transfers[idx].transfer = libusb_alloc_transfer(0);
+				if (!dap->bdata->response_transfers[idx].transfer) {
+					LOG_ERROR("unable to allocate USB transfer");
+					cmsis_dap_usb_close(dap);
+					return ERROR_FAIL;
+				}
+			}
+
 			err = cmsis_dap_usb_alloc(dap, packet_size);
 			if (err != ERROR_OK)
 				cmsis_dap_usb_close(dap);
@@ -376,65 +419,178 @@ static int cmsis_dap_usb_open(struct cmsis_dap *dap, uint16_t vids[], uint16_t p
 
 static void cmsis_dap_usb_close(struct cmsis_dap *dap)
 {
+	for (unsigned int i = 0; i < MAX_PENDING_REQUESTS; i++) {
+		libusb_free_transfer(dap->bdata->command_transfers[i].transfer);
+		libusb_free_transfer(dap->bdata->response_transfers[i].transfer);
+	}
+	cmsis_dap_usb_free(dap);
 	libusb_release_interface(dap->bdata->dev_handle, dap->bdata->interface);
 	libusb_close(dap->bdata->dev_handle);
 	libusb_exit(dap->bdata->usb_ctx);
 	free(dap->bdata);
 	dap->bdata = NULL;
-	free(dap->packet_buffer);
-	dap->packet_buffer = NULL;
 }
 
-static int cmsis_dap_usb_read(struct cmsis_dap *dap, int timeout_ms)
+static void LIBUSB_CALL cmsis_dap_usb_callback(struct libusb_transfer *transfer)
+{
+	struct cmsis_dap_bulk_transfer *tr;
+
+	tr = (struct cmsis_dap_bulk_transfer *)transfer->user_data;
+	if (transfer->status == LIBUSB_TRANSFER_COMPLETED) {
+		tr->status = CMSIS_DAP_TRANSFER_COMPLETED;
+		tr->transferred = transfer->actual_length;
+	} else if (transfer->status == LIBUSB_TRANSFER_TIMED_OUT) {
+		tr->status = ERROR_TIMEOUT_REACHED;
+	} else {
+		tr->status = ERROR_JTAG_DEVICE_ERROR;
+	}
+}
+
+static int cmsis_dap_usb_read(struct cmsis_dap *dap, int transfer_timeout_ms,
+							  struct timeval *wait_timeout)
 {
 	int transferred = 0;
 	int err;
+	struct cmsis_dap_bulk_transfer *tr;
+	tr = &dap->bdata->response_transfers[dap->pending_fifo_get_idx];
+
+	if (tr->status == CMSIS_DAP_TRANSFER_IDLE) {
+		libusb_fill_bulk_transfer(tr->transfer,
+								  dap->bdata->dev_handle, dap->bdata->ep_in,
+								  tr->buffer, dap->packet_size,
+								  &cmsis_dap_usb_callback, tr,
+								  transfer_timeout_ms);
+		LOG_DEBUG_IO("submit read @ %u", dap->pending_fifo_get_idx);
+		tr->status = CMSIS_DAP_TRANSFER_PENDING;
+		err = libusb_submit_transfer(tr->transfer);
+		if (err) {
+			tr->status = CMSIS_DAP_TRANSFER_IDLE;
+			LOG_ERROR("error submitting USB read: %s", libusb_strerror(err));
+			return ERROR_FAIL;
+		}
+	}
 
-	err = libusb_bulk_transfer(dap->bdata->dev_handle, dap->bdata->ep_in,
-							dap->packet_buffer, dap->packet_size, &transferred, timeout_ms);
-	if (err) {
-		if (err == LIBUSB_ERROR_TIMEOUT) {
-			return ERROR_TIMEOUT_REACHED;
-		} else {
-			LOG_ERROR("error reading data: %s", libusb_strerror(err));
+	struct timeval tv = {
+		.tv_sec = transfer_timeout_ms / 1000,
+		.tv_usec = transfer_timeout_ms % 1000 * 1000
+	};
+
+	while (tr->status == CMSIS_DAP_TRANSFER_PENDING) {
+		err = libusb_handle_events_timeout_completed(dap->bdata->usb_ctx,
+												 wait_timeout ? wait_timeout : &tv,
+												 &tr->status);
+		if (err) {
+			LOG_ERROR("error handling USB events: %s", libusb_strerror(err));
 			return ERROR_FAIL;
 		}
+		if (wait_timeout)
+			break;
 	}
 
-	memset(&dap->packet_buffer[transferred], 0, dap->packet_buffer_size - transferred);
+	if (tr->status < 0 || tr->status == CMSIS_DAP_TRANSFER_COMPLETED) {
+		/* Check related command request for an error */
+		struct cmsis_dap_bulk_transfer *tr_cmd;
+		tr_cmd = &dap->bdata->command_transfers[dap->pending_fifo_get_idx];
+		if (tr_cmd->status < 0) {
+			err = tr_cmd->status;
+			tr_cmd->status = CMSIS_DAP_TRANSFER_IDLE;
+			if (err != ERROR_TIMEOUT_REACHED)
+				LOG_ERROR("error writing USB data");
+			else
+				LOG_DEBUG("command write USB timeout @ %u", dap->pending_fifo_get_idx);
+
+			return err;
+		}
+		if (tr_cmd->status == CMSIS_DAP_TRANSFER_COMPLETED)
+			tr_cmd->status = CMSIS_DAP_TRANSFER_IDLE;
+	}
+
+	if (tr->status < 0) {
+		err = tr->status;
+		tr->status = CMSIS_DAP_TRANSFER_IDLE;
+		if (err != ERROR_TIMEOUT_REACHED)
+			LOG_ERROR("error reading USB data");
+		else
+			LOG_DEBUG("USB timeout @ %u", dap->pending_fifo_get_idx);
+
+		return err;
+	}
+
+	if (tr->status == CMSIS_DAP_TRANSFER_COMPLETED) {
+		transferred = tr->transferred;
+		LOG_DEBUG_IO("completed read @ %u, transferred %i",
+					 dap->pending_fifo_get_idx, transferred);
+		memcpy(dap->packet_buffer, tr->buffer, transferred);
+		memset(&dap->packet_buffer[transferred], 0, dap->packet_buffer_size - transferred);
+		tr->status = CMSIS_DAP_TRANSFER_IDLE;
+	}
 
 	return transferred;
 }
 
 static int cmsis_dap_usb_write(struct cmsis_dap *dap, int txlen, int timeout_ms)
 {
-	int transferred = 0;
 	int err;
+	struct cmsis_dap_bulk_transfer *tr;
+	tr = &dap->bdata->command_transfers[dap->pending_fifo_put_idx];
+
+	if (tr->status == CMSIS_DAP_TRANSFER_PENDING) {
+		LOG_ERROR("busy command USB transfer at %u", dap->pending_fifo_put_idx);
+		struct timeval tv = {
+			.tv_sec = timeout_ms / 1000,
+			.tv_usec = timeout_ms % 1000 * 1000
+		};
+		libusb_handle_events_timeout_completed(dap->bdata->usb_ctx, &tv, &tr->status);
+	}
+	if (tr->status < 0) {
+		if (tr->status != ERROR_TIMEOUT_REACHED)
+			LOG_ERROR("error writing USB data, late detect");
+		else
+			LOG_DEBUG("USB write timeout @ %u, late detect", dap->pending_fifo_get_idx);
+		tr->status = CMSIS_DAP_TRANSFER_IDLE;
+	}
+	if (tr->status == CMSIS_DAP_TRANSFER_COMPLETED) {
+		LOG_ERROR("USB write: late transfer competed");
+		tr->status = CMSIS_DAP_TRANSFER_IDLE;
+	}
+	if (tr->status != CMSIS_DAP_TRANSFER_IDLE) {
+		libusb_cancel_transfer(tr->transfer);
+		/* TODO: switch to less verbose errors and wait for USB working again */
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	memcpy(tr->buffer, dap->packet_buffer, txlen);
+
+	libusb_fill_bulk_transfer(tr->transfer,
+							  dap->bdata->dev_handle, dap->bdata->ep_out,
+							  tr->buffer, txlen,
+							  &cmsis_dap_usb_callback, tr,
+							  timeout_ms);
 
-	/* skip the first byte that is only used by the HID backend */
-	err = libusb_bulk_transfer(dap->bdata->dev_handle, dap->bdata->ep_out,
-							dap->packet_buffer, txlen, &transferred, timeout_ms);
+	LOG_DEBUG_IO("submit write @ %u", dap->pending_fifo_put_idx);
+	tr->status = CMSIS_DAP_TRANSFER_PENDING;
+	err = libusb_submit_transfer(tr->transfer);
 	if (err) {
-		if (err == LIBUSB_ERROR_TIMEOUT) {
-			return ERROR_TIMEOUT_REACHED;
-		} else {
-			LOG_ERROR("error writing data: %s", libusb_strerror(err));
-			return ERROR_FAIL;
-		}
+		if (err == LIBUSB_ERROR_BUSY)
+			libusb_cancel_transfer(tr->transfer);
+		else
+			tr->status = CMSIS_DAP_TRANSFER_IDLE;
+
+		LOG_ERROR("error submitting USB write: %s", libusb_strerror(err));
+		return ERROR_FAIL;
 	}
 
-	return transferred;
+	return ERROR_OK;
 }
 
 static int cmsis_dap_usb_alloc(struct cmsis_dap *dap, unsigned int pkt_sz)
 {
-	uint8_t *buf = malloc(pkt_sz);
-	if (!buf) {
+	dap->packet_buffer = malloc(pkt_sz);
+	if (!dap->packet_buffer) {
 		LOG_ERROR("unable to allocate CMSIS-DAP packet buffer");
 		return ERROR_FAIL;
 	}
 
-	dap->packet_buffer = buf;
 	dap->packet_size = pkt_sz;
 	dap->packet_buffer_size = pkt_sz;
 	/* Prevent sending zero size USB packets */
@@ -443,9 +599,41 @@ static int cmsis_dap_usb_alloc(struct cmsis_dap *dap, unsigned int pkt_sz)
 	dap->command = dap->packet_buffer;
 	dap->response = dap->packet_buffer;
 
+	for (unsigned int i = 0; i < MAX_PENDING_REQUESTS; i++) {
+		dap->bdata->command_transfers[i].buffer =
+			libusb_dev_mem_alloc(dap->bdata->dev_handle, pkt_sz);
+		if (!dap->bdata->command_transfers[i].buffer) {
+			LOG_ERROR("unable to allocate CMSIS-DAP packet buffer");
+			return ERROR_FAIL;
+		}
+		dap->bdata->response_transfers[i].buffer =
+			libusb_dev_mem_alloc(dap->bdata->dev_handle, pkt_sz);
+		if (!dap->bdata->response_transfers[i].buffer) {
+			LOG_ERROR("unable to allocate CMSIS-DAP packet buffer");
+			return ERROR_FAIL;
+		}
+	}
+
 	return ERROR_OK;
 }
 
+static void cmsis_dap_usb_free(struct cmsis_dap *dap)
+{
+	for (unsigned int i = 0; i < MAX_PENDING_REQUESTS; i++) {
+		libusb_dev_mem_free(dap->bdata->dev_handle,
+			dap->bdata->command_transfers[i].buffer, dap->packet_size);
+		dap->bdata->command_transfers[i].buffer = NULL;
+		libusb_dev_mem_free(dap->bdata->dev_handle,
+			dap->bdata->response_transfers[i].buffer, dap->packet_size);
+		dap->bdata->response_transfers[i].buffer = NULL;
+	}
+
+	free(dap->packet_buffer);
+	dap->packet_buffer = NULL;
+	dap->command = NULL;
+	dap->response = NULL;
+}
+
 COMMAND_HANDLER(cmsis_dap_handle_usb_interface_command)
 {
 	if (CMD_ARGC == 1)
@@ -474,4 +662,5 @@ const struct cmsis_dap_backend cmsis_dap_usb_backend = {
 	.read = cmsis_dap_usb_read,
 	.write = cmsis_dap_usb_write,
 	.packet_buffer_alloc = cmsis_dap_usb_alloc,
+	.packet_buffer_free = cmsis_dap_usb_free,
 };
diff --git a/src/jtag/drivers/cmsis_dap_usb_hid.c b/src/jtag/drivers/cmsis_dap_usb_hid.c
index 52dfd76..6338886 100644
--- a/src/jtag/drivers/cmsis_dap_usb_hid.c
+++ b/src/jtag/drivers/cmsis_dap_usb_hid.c
@@ -36,6 +36,7 @@ struct cmsis_dap_backend_data {
 
 static void cmsis_dap_hid_close(struct cmsis_dap *dap);
 static int cmsis_dap_hid_alloc(struct cmsis_dap *dap, unsigned int pkt_sz);
+static void cmsis_dap_hid_free(struct cmsis_dap *dap);
 
 static int cmsis_dap_hid_open(struct cmsis_dap *dap, uint16_t vids[], uint16_t pids[], const char *serial)
 {
@@ -165,14 +166,21 @@ static void cmsis_dap_hid_close(struct cmsis_dap *dap)
 	hid_exit();
 	free(dap->bdata);
 	dap->bdata = NULL;
-	free(dap->packet_buffer);
-	dap->packet_buffer = NULL;
+	cmsis_dap_hid_free(dap);
 }
 
-static int cmsis_dap_hid_read(struct cmsis_dap *dap, int timeout_ms)
+static int cmsis_dap_hid_read(struct cmsis_dap *dap, int transfer_timeout_ms,
+							  struct timeval *wait_timeout)
 {
-	int retval = hid_read_timeout(dap->bdata->dev_handle, dap->packet_buffer, dap->packet_buffer_size, timeout_ms);
-
+	int timeout_ms;
+	if (wait_timeout)
+		timeout_ms = wait_timeout->tv_usec / 1000 + wait_timeout->tv_sec * 1000;
+	else
+		timeout_ms = transfer_timeout_ms;
+
+	int retval = hid_read_timeout(dap->bdata->dev_handle,
+								  dap->packet_buffer, dap->packet_buffer_size,
+								  timeout_ms);
 	if (retval == 0) {
 		return ERROR_TIMEOUT_REACHED;
 	} else if (retval == -1) {
@@ -222,6 +230,12 @@ static int cmsis_dap_hid_alloc(struct cmsis_dap *dap, unsigned int pkt_sz)
 	return ERROR_OK;
 }
 
+static void cmsis_dap_hid_free(struct cmsis_dap *dap)
+{
+	free(dap->packet_buffer);
+	dap->packet_buffer = NULL;
+}
+
 const struct cmsis_dap_backend cmsis_dap_hid_backend = {
 	.name = "hid",
 	.open = cmsis_dap_hid_open,
@@ -229,4 +243,5 @@ const struct cmsis_dap_backend cmsis_dap_hid_backend = {
 	.read = cmsis_dap_hid_read,
 	.write = cmsis_dap_hid_write,
 	.packet_buffer_alloc = cmsis_dap_hid_alloc,
+	.packet_buffer_free = cmsis_dap_hid_free,
 };