X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fjtag%2Fdrivers%2Fcmsis_dap_usb.c;h=ff8b8ffaf767ef6c5bfb2d5afdc10139df1239c6;hp=504b3d00879e28a5db1aeec145e6fabeeee12c0d;hb=a6c4eb03455f6e97fc25183aae249d6ccdcbfb0f;hpb=0c7db47e58e5fe267219735630cd004b504e8535

diff --git a/src/jtag/drivers/cmsis_dap_usb.c b/src/jtag/drivers/cmsis_dap_usb.c
index 504b3d0087..ff8b8ffaf7 100644
--- a/src/jtag/drivers/cmsis_dap_usb.c
+++ b/src/jtag/drivers/cmsis_dap_usb.c
@@ -1,4 +1,7 @@
 /***************************************************************************
+ *   Copyright (C) 2014 by Paul Fertser                                    *
+ *   fercerpav@gmail.com                                                   *
+ *                                                                         *
  *   Copyright (C) 2013 by mike brown                                      *
  *   mike@theshedworks.org.uk                                              *
  *                                                                         *
@@ -33,12 +36,6 @@
 
 #include <hidapi.h>
 
-#ifdef _DEBUG_JTAG_IO_
- #define DEBUG_IO(expr...) LOG_DEBUG(expr)
-#else
- #define DEBUG_IO(expr...) do {} while (0)
-#endif
-
 /*
  * See CMSIS-DAP documentation:
  * Version 0.01 - Beta.
@@ -60,6 +57,8 @@
 /* vid = pid = 0 marks the end of the list */
 static uint16_t cmsis_dap_vid[MAX_USB_IDS + 1] = { 0 };
 static uint16_t cmsis_dap_pid[MAX_USB_IDS + 1] = { 0 };
+static wchar_t *cmsis_dap_serial;
+static bool swd_mode;
 
 #define PACKET_SIZE       (64 + 1)	/* 64 bytes plus report id */
 #define USB_TIMEOUT       1000
@@ -136,7 +135,7 @@ static uint16_t cmsis_dap_pid[MAX_USB_IDS + 1] = { 0 };
 /* CMSIS-DAP Vendor Commands
  * None as yet... */
 
-static char *info_caps_str[] = {
+static const char * const info_caps_str[] = {
 	"SWD  Supported",
 	"JTAG Supported"
 };
@@ -153,6 +152,17 @@ struct cmsis_dap {
 	uint8_t mode;
 };
 
+struct pending_transfer_result {
+	uint8_t cmd;
+	uint32_t data;
+	void *buffer;
+};
+
+static int pending_transfer_count, pending_queue_len;
+static struct pending_transfer_result *pending_transfers;
+
+static int queued_retval;
+
 static struct cmsis_dap *cmsis_dap_handle;
 
 static int cmsis_dap_usb_open(void)
@@ -161,33 +171,55 @@ static int cmsis_dap_usb_open(void)
 	int i;
 	struct hid_device_info *devs, *cur_dev;
 	unsigned short target_vid, target_pid;
+	wchar_t *target_serial = NULL;
+
+	bool found = false;
+	bool serial_found = false;
 
 	target_vid = 0;
 	target_pid = 0;
 
 	/*
-	The CMSIS-DAP specification stipulates:
-	"The Product String must contain "CMSIS-DAP" somewhere in the string. This is used by the
-	debuggers to idenify a CMSIS-DAP compliant Debug Unit that is connected to a host computer."
-	*/
+	 * The CMSIS-DAP specification stipulates:
+	 * "The Product String must contain "CMSIS-DAP" somewhere in the string. This is used by the
+	 * debuggers to identify a CMSIS-DAP compliant Debug Unit that is connected to a host computer."
+	 */
 	devs = hid_enumerate(0x0, 0x0);
 	cur_dev = devs;
 	while (NULL != cur_dev) {
-		if ((0 == cmsis_dap_vid[0]) && (NULL != cur_dev->product_string)
-		    && wcsstr(cur_dev->product_string, L"CMSIS-DAP")) {
-			/*
-			if the user hasn't specified VID:PID *and*
-			product string contains "CMSIS-DAP", pick it
-			*/
-			break;
+		if (0 == cmsis_dap_vid[0]) {
+			if (NULL == cur_dev->product_string) {
+				LOG_DEBUG("Cannot read product string of device 0x%x:0x%x",
+					  cur_dev->vendor_id, cur_dev->product_id);
+			} else {
+				if (wcsstr(cur_dev->product_string, L"CMSIS-DAP")) {
+					/* if the user hasn't specified VID:PID *and*
+					 * product string contains "CMSIS-DAP", pick it
+					 */
+					found = true;
+				}
+			}
 		} else {
-			/*
-			otherwise, exhaustively compare against all VID:PID in list
-			*/
+			/* otherwise, exhaustively compare against all VID:PID in list */
 			for (i = 0; cmsis_dap_vid[i] || cmsis_dap_pid[i]; i++) {
 				if ((cmsis_dap_vid[i] == cur_dev->vendor_id) && (cmsis_dap_pid[i] == cur_dev->product_id))
-					break;
+					found = true;
 			}
+
+			if (cmsis_dap_vid[i] || cmsis_dap_pid[i])
+				found = true;
+		}
+
+		if (found) {
+			/* we have found an adapter, so exit further checks */
+			/* check serial number matches if given */
+			if (cmsis_dap_serial != NULL) {
+				if (wcscmp(cmsis_dap_serial, cur_dev->serial_number) == 0) {
+					serial_found = true;
+					break;
+				}
+			} else
+				break;
 		}
 
 		cur_dev = cur_dev->next;
@@ -196,19 +228,26 @@ static int cmsis_dap_usb_open(void)
 	if (NULL != cur_dev) {
 		target_vid = cur_dev->vendor_id;
 		target_pid = cur_dev->product_id;
+		if (serial_found)
+			target_serial = cmsis_dap_serial;
 	}
 
 	hid_free_enumeration(devs);
 
+	if (target_vid == 0 && target_pid == 0) {
+		LOG_ERROR("unable to find CMSIS-DAP device");
+		return ERROR_FAIL;
+	}
+
 	if (hid_init() != 0) {
 		LOG_ERROR("unable to open HIDAPI");
 		return ERROR_FAIL;
 	}
 
-	dev = hid_open(target_vid, target_pid, NULL);
+	dev = hid_open(target_vid, target_pid, target_serial);
 
 	if (dev == NULL) {
-		LOG_ERROR("unable to open CMSIS-DAP device");
+		LOG_ERROR("unable to open CMSIS-DAP device 0x%x:0x%x", target_vid, target_pid);
 		return ERROR_FAIL;
 	}
 
@@ -232,7 +271,9 @@ static int cmsis_dap_usb_open(void)
 	int packet_size = PACKET_SIZE;
 
 	/* atmel cmsis-dap uses 512 byte reports */
-	if (target_vid == 0x03eb && target_pid == 0x2111)
+	/* TODO: HID report descriptor should be parsed instead of
+	 * hardcoding a match by VID */
+	if (target_vid == 0x03eb)
 		packet_size = 512 + 1;
 
 	cmsis_dap_handle->packet_buffer = malloc(packet_size);
@@ -251,13 +292,13 @@ static void cmsis_dap_usb_close(struct cmsis_dap *dap)
 	hid_close(dap->dev_handle);
 	hid_exit();
 
-	if (cmsis_dap_handle->packet_buffer)
-		free(cmsis_dap_handle->packet_buffer);
-
-	if (cmsis_dap_handle) {
-		free(cmsis_dap_handle);
-		cmsis_dap_handle = NULL;
-	}
+	free(cmsis_dap_handle->packet_buffer);
+	free(cmsis_dap_handle);
+	cmsis_dap_handle = NULL;
+	free(cmsis_dap_serial);
+	cmsis_dap_serial = NULL;
+	free(pending_transfers);
+	pending_transfers = NULL;
 
 	return;
 }
@@ -266,7 +307,7 @@ static void cmsis_dap_usb_close(struct cmsis_dap *dap)
 static int cmsis_dap_usb_xfer(struct cmsis_dap *dap, int txlen)
 {
 	/* Pad the rest of the TX buffer with 0's */
-	memset(dap->packet_buffer + txlen, 0, dap->packet_size - 1 - txlen);
+	memset(dap->packet_buffer + txlen, 0, dap->packet_size - txlen);
 
 	/* write data to device */
 	int retval = hid_write(dap->dev_handle, dap->packet_buffer, dap->packet_size);
@@ -413,7 +454,7 @@ static int cmsis_dap_cmd_DAP_Disconnect(void)
 	return ERROR_OK;
 }
 
-static int cmsis_dap_cmd_DAP_TFER_Configure(uint8_t idle, uint16_t delay, uint16_t retry)
+static int cmsis_dap_cmd_DAP_TFER_Configure(uint8_t idle, uint16_t retry_count, uint16_t match_retry)
 {
 	int retval;
 	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
@@ -421,10 +462,10 @@ static int cmsis_dap_cmd_DAP_TFER_Configure(uint8_t idle, uint16_t delay, uint16
 	buffer[0] = 0;	/* report number */
 	buffer[1] = CMD_DAP_TFER_CONFIGURE;
 	buffer[2] = idle;
-	buffer[3] = delay & 0xff;
-	buffer[4] = (delay >> 8) & 0xff;
-	buffer[5] = retry & 0xff;
-	buffer[6] = (retry >> 8) & 0xff;
+	buffer[3] = retry_count & 0xff;
+	buffer[4] = (retry_count >> 8) & 0xff;
+	buffer[5] = match_retry & 0xff;
+	buffer[6] = (match_retry >> 8) & 0xff;
 	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 7);
 
 	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
@@ -474,99 +515,139 @@ static int cmsis_dap_cmd_DAP_Delay(uint16_t delay_us)
 }
 #endif
 
-static int cmsis_dap_swd_read_reg(uint8_t cmd, uint32_t *value)
+static int cmsis_dap_swd_run_queue(void)
 {
 	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
-	int retval;
-	uint32_t val;
 
-	DEBUG_IO("CMSIS-DAP: Read  Reg 0x%02" PRIx8, cmd);
+	LOG_DEBUG("Executing %d queued transactions", pending_transfer_count);
 
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_TFER;
-	buffer[2] = 0x00;
-	buffer[3] = 0x01;
-	buffer[4] = cmd;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 5);
-
-	/* TODO - need better response checking */
-	if (retval != ERROR_OK || buffer[1] != 0x01) {
-		LOG_ERROR("CMSIS-DAP: Read Error (0x%02" PRIx8 ")", buffer[2]);
-		return buffer[2];
+	if (queued_retval != ERROR_OK) {
+		LOG_DEBUG("Skipping due to previous errors: %d", queued_retval);
+		goto skip;
 	}
 
-	val = le_to_h_u32(&buffer[3]);
-	DEBUG_IO("0x%08" PRIx32, val);
-
-	if (value)
-		*value = val;
-
-	return retval;
-}
+	if (!pending_transfer_count)
+		goto skip;
+
+	size_t idx = 0;
+	buffer[idx++] = 0;	/* report number */
+	buffer[idx++] = CMD_DAP_TFER;
+	buffer[idx++] = 0x00;	/* DAP Index */
+	buffer[idx++] = pending_transfer_count;
+
+	for (int i = 0; i < pending_transfer_count; i++) {
+		uint8_t cmd = pending_transfers[i].cmd;
+		uint32_t data = pending_transfers[i].data;
+
+		LOG_DEBUG("%s %s reg %x %"PRIx32,
+				cmd & SWD_CMD_APnDP ? "AP" : "DP",
+				cmd & SWD_CMD_RnW ? "read" : "write",
+			  (cmd & SWD_CMD_A32) >> 1, data);
+
+		/* When proper WAIT handling is implemented in the
+		 * common SWD framework, this kludge can be
+		 * removed. However, this might lead to minor
+		 * performance degradation as the adapter wouldn't be
+		 * able to automatically retry anything (because ARM
+		 * has forgotten to implement sticky error flags
+		 * clearing). See also comments regarding
+		 * cmsis_dap_cmd_DAP_TFER_Configure() and
+		 * cmsis_dap_cmd_DAP_SWD_Configure() in
+		 * cmsis_dap_init().
+		 */
+		if (!(cmd & SWD_CMD_RnW) &&
+		    !(cmd & SWD_CMD_APnDP) &&
+		    (cmd & SWD_CMD_A32) >> 1 == DP_CTRL_STAT &&
+		    (data & CORUNDETECT)) {
+			LOG_DEBUG("refusing to enable sticky overrun detection");
+			data &= ~CORUNDETECT;
+		}
 
-static int cmsis_dap_swd_write_reg(uint8_t cmd, uint32_t value)
-{
-	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+		buffer[idx++] = (cmd >> 1) & 0x0f;
+		if (!(cmd & SWD_CMD_RnW)) {
+			buffer[idx++] = (data) & 0xff;
+			buffer[idx++] = (data >> 8) & 0xff;
+			buffer[idx++] = (data >> 16) & 0xff;
+			buffer[idx++] = (data >> 24) & 0xff;
+		}
+	}
 
-	DEBUG_IO("CMSIS-DAP: Write Reg 0x%02" PRIx8 " 0x%08" PRIx32, cmd, value);
+	queued_retval = cmsis_dap_usb_xfer(cmsis_dap_handle, idx);
+	if (queued_retval != ERROR_OK)
+		goto skip;
+
+	idx = 2;
+	uint8_t ack = buffer[idx] & 0x07;
+	if (ack != SWD_ACK_OK || (buffer[idx] & 0x08)) {
+		LOG_DEBUG("SWD ack not OK: %d %s", buffer[idx-1],
+			  ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
+		queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
+		goto skip;
+	}
+	idx++;
+
+	if (pending_transfer_count != buffer[1])
+		LOG_ERROR("CMSIS-DAP transfer count mismatch: expected %d, got %d",
+			  pending_transfer_count, buffer[1]);
+
+	for (int i = 0; i < buffer[1]; i++) {
+		if (pending_transfers[i].cmd & SWD_CMD_RnW) {
+			static uint32_t last_read;
+			uint32_t data = le_to_h_u32(&buffer[idx]);
+			uint32_t tmp = data;
+			idx += 4;
+
+			LOG_DEBUG("Read result: %"PRIx32, data);
+
+			/* Imitate posted AP reads */
+			if ((pending_transfers[i].cmd & SWD_CMD_APnDP) ||
+			    ((pending_transfers[i].cmd & SWD_CMD_A32) >> 1 == DP_RDBUFF)) {
+				tmp = last_read;
+				last_read = data;
+			}
 
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_TFER;
-	buffer[2] = 0x00;
-	buffer[3] = 0x01;
-	buffer[4] = cmd;
-	buffer[5] = (value) & 0xff;
-	buffer[6] = (value >> 8) & 0xff;
-	buffer[7] = (value >> 16) & 0xff;
-	buffer[8] = (value >> 24) & 0xff;
-	int retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 9);
-
-	if (buffer[1] != 0x01) {
-		LOG_ERROR("CMSIS-DAP: Write Error (0x%02" PRIx8 ")", buffer[2]);
-		retval = buffer[2];
+			if (pending_transfers[i].buffer)
+				*(uint32_t *)pending_transfers[i].buffer = tmp;
+		}
 	}
 
+skip:
+	pending_transfer_count = 0;
+	int retval = queued_retval;
+	queued_retval = ERROR_OK;
+
 	return retval;
 }
 
-static int cmsis_dap_swd_read_block(uint8_t cmd, uint32_t blocksize, uint8_t *dest_buf)
+static void cmsis_dap_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 {
-	uint8_t *buffer;
-	int tfer_sz;
-	int retval = ERROR_OK;
-	uint16_t read_count;
-
-	DEBUG_IO("CMSIS-DAP: Read Block 0x%02" PRIx8 " %" PRIu32, cmd, blocksize);
-
-	while (blocksize) {
-
-		buffer = cmsis_dap_handle->packet_buffer;
-		tfer_sz = blocksize;
-		if (tfer_sz > 15)
-			tfer_sz = 8;
-
-		buffer[0] = 0;	/* report number */
-		buffer[1] = CMD_DAP_TFER_BLOCK;
-		buffer[2] = 0x00;
-		buffer[3] = tfer_sz;
-		buffer[4] = 0x00;
-		buffer[5] = cmd;
-		retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 6);
-
-		read_count = le_to_h_u16(&buffer[1]);
-		if (read_count != tfer_sz) {
-			LOG_ERROR("CMSIS-DAP: Block Read Error (0x%02" PRIx8 ")", buffer[3]);
-			retval = buffer[3];
-		}
+	if (pending_transfer_count == pending_queue_len) {
+		/* Not enough room in the queue. Run the queue. */
+		queued_retval = cmsis_dap_swd_run_queue();
+	}
 
-		read_count *= 4;
-		memcpy(dest_buf, &buffer[4], read_count);
+	if (queued_retval != ERROR_OK)
+		return;
 
-		dest_buf += read_count;
-		blocksize -= tfer_sz;
+	pending_transfers[pending_transfer_count].data = data;
+	pending_transfers[pending_transfer_count].cmd = cmd;
+	if (cmd & SWD_CMD_RnW) {
+		/* Queue a read transaction */
+		pending_transfers[pending_transfer_count].buffer = dst;
 	}
+	pending_transfer_count++;
+}
 
-	return retval;
+static void cmsis_dap_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk)
+{
+	assert(!(cmd & SWD_CMD_RnW));
+	cmsis_dap_swd_queue_cmd(cmd, NULL, value);
+}
+
+static void cmsis_dap_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay_clk)
+{
+	assert(cmd & SWD_CMD_RnW);
+	cmsis_dap_swd_queue_cmd(cmd, value, 0);
 }
 
 static int cmsis_dap_get_version_info(void)
@@ -626,115 +707,85 @@ static int cmsis_dap_get_status(void)
 	return retval;
 }
 
-static int cmsis_dap_reset_link(void)
+static int cmsis_dap_swd_switch_seq(enum swd_special_seq seq)
 {
 	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+	const uint8_t *s;
+	unsigned int s_len;
+	int retval;
 
-	LOG_DEBUG("CMSIS-DAP: cmsis_dap_reset_link");
-	LOG_INFO("DAP_SWJ Sequence (reset: 50+ '1' followed by 0)");
+	/* First disconnect before connecting, Atmel EDBG needs it for SAMD/R/L/C */
+	cmsis_dap_cmd_DAP_Disconnect();
 
-	/* reset line with SWDIO high for >50 cycles */
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_SWJ_SEQ;
-	buffer[2] = 7 * 8;
-	buffer[3] = 0xff;
-	buffer[4] = 0xff;
-	buffer[5] = 0xff;
-	buffer[6] = 0xff;
-	buffer[7] = 0xff;
-	buffer[8] = 0xff;
-	buffer[9] = 0xff;
-	int retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 10);
+	/* When we are reconnecting, DAP_Connect needs to be rerun, at
+	 * least on Keil ULINK-ME */
+	retval = cmsis_dap_cmd_DAP_Connect(seq == LINE_RESET || seq == JTAG_TO_SWD ?
+					   CONNECT_SWD : CONNECT_JTAG);
+	if (retval != ERROR_OK)
+		return retval;
 
-	if (retval != ERROR_OK || buffer[1] != DAP_OK)
+	switch (seq) {
+	case LINE_RESET:
+		LOG_DEBUG("SWD line reset");
+		s = swd_seq_line_reset;
+		s_len = swd_seq_line_reset_len;
+		break;
+	case JTAG_TO_SWD:
+		LOG_DEBUG("JTAG-to-SWD");
+		s = swd_seq_jtag_to_swd;
+		s_len = swd_seq_jtag_to_swd_len;
+		break;
+	case SWD_TO_JTAG:
+		LOG_DEBUG("SWD-to-JTAG");
+		s = swd_seq_swd_to_jtag;
+		s_len = swd_seq_swd_to_jtag_len;
+		break;
+	default:
+		LOG_ERROR("Sequence %d not supported", seq);
 		return ERROR_FAIL;
+	}
 
-	/* 16bit JTAG-SWD sequence */
 	buffer[0] = 0;	/* report number */
 	buffer[1] = CMD_DAP_SWJ_SEQ;
-	buffer[2] = 2 * 8;
-	buffer[3] = 0x9e;
-	buffer[4] = 0xe7;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 5);
+	buffer[2] = s_len;
+	bit_copy(&buffer[3], 0, s, 0, s_len);
 
-	if (retval != ERROR_OK || buffer[1] != DAP_OK)
-		return ERROR_FAIL;
-
-	/* another reset just incase */
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_SWJ_SEQ;
-	buffer[2] = 7 * 8;
-	buffer[3] = 0xff;
-	buffer[4] = 0xff;
-	buffer[5] = 0xff;
-	buffer[6] = 0xff;
-	buffer[7] = 0xff;
-	buffer[8] = 0xff;
-	buffer[9] = 0xff;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 10);
+	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, DIV_ROUND_UP(s_len, 8) + 3);
 
 	if (retval != ERROR_OK || buffer[1] != DAP_OK)
 		return ERROR_FAIL;
 
-	/* 16 cycle idle period */
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_SWJ_SEQ;
-	buffer[2] = 2 * 8;
-	buffer[3] = 0x00;
-	buffer[4] = 0x00;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 5);
+	return ERROR_OK;
+}
 
-	if (retval != ERROR_OK || buffer[1] != DAP_OK)
-		return ERROR_FAIL;
+static int cmsis_dap_swd_open(void)
+{
+	int retval;
 
-	DEBUG_IO("DAP Read IDCODE");
+	if (cmsis_dap_handle == NULL) {
+		/* SWD init */
+		retval = cmsis_dap_usb_open();
+		if (retval != ERROR_OK)
+			return retval;
 
-	/* read the id code is always the next sequence */
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_TFER;
-	buffer[2] = 0x00;
-	buffer[3] = 0x01;
-	buffer[4] = 0x02;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 5);
+		retval = cmsis_dap_get_caps_info();
+		if (retval != ERROR_OK)
+			return retval;
+	}
 
+	if (!(cmsis_dap_handle->caps & INFO_CAPS_SWD)) {
+		LOG_ERROR("CMSIS-DAP: SWD not supported");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	retval = cmsis_dap_cmd_DAP_Connect(CONNECT_SWD);
 	if (retval != ERROR_OK)
 		return retval;
 
-	if (buffer[1] == 0) {
-		LOG_DEBUG("Result 0x%02" PRIx8 " 0x%02" PRIx8, buffer[1], buffer[2]);
-
-		LOG_DEBUG("DAP Reset Target");
-		buffer[0] = 0;	/* report number */
-		buffer[1] = CMD_DAP_RESET_TARGET;
-		retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 2);
-		LOG_DEBUG("Result 0x%02" PRIx8 " 0x%02" PRIx8, buffer[1], buffer[2]);
-
-		LOG_DEBUG("DAP Write Abort");
-		buffer[0] = 0;	/* report number */
-		buffer[1] = CMD_DAP_WRITE_ABORT;
-		buffer[2] = 0x00;
-		buffer[3] = 0x1e/*0x1f*/;
-		buffer[4] = 0x00;
-		buffer[5] = 0x00;
-		buffer[6] = 0x00;
-		retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 7);
-		LOG_DEBUG("Result 0x%02" PRIx8, buffer[1]);
-
-		return 0x80 + buffer[1];
-	}
-
-	LOG_DEBUG("DAP Write Abort");
-	buffer[0] = 0;	/* report number */
-	buffer[1] = CMD_DAP_WRITE_ABORT;
-	buffer[2] = 0x00;
-	buffer[3] = 0x1e;
-	buffer[4] = 0x00;
-	buffer[5] = 0x00;
-	buffer[6] = 0x00;
-	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 7);
-	LOG_DEBUG("Result 0x%02" PRIx8, buffer[1]);
+	/* Add more setup here.??... */
 
-	return retval;
+	LOG_INFO("CMSIS-DAP: Interface Initialised (SWD)");
+	return ERROR_OK;
 }
 
 static int cmsis_dap_init(void)
@@ -742,6 +793,12 @@ static int cmsis_dap_init(void)
 	int retval;
 	uint8_t *data;
 
+	if (swd_mode) {
+		retval = cmsis_dap_swd_open();
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
 	if (cmsis_dap_handle == NULL) {
 
 		/* JTAG init */
@@ -778,6 +835,16 @@ static int cmsis_dap_init(void)
 	if (data[0] == 2) {  /* short */
 		uint16_t pkt_sz = data[1] + (data[2] << 8);
 
+		/* 4 bytes of command header + 5 bytes per register
+		 * write. For bulk read sequences just 4 bytes are
+		 * needed per transfer, so this is suboptimal. */
+		pending_queue_len = (pkt_sz - 4) / 5;
+		pending_transfers = malloc(pending_queue_len * sizeof(*pending_transfers));
+		if (!pending_transfers) {
+			LOG_ERROR("Unable to allocate memory for CMSIS-DAP queue");
+			return ERROR_FAIL;
+		}
+
 		if (cmsis_dap_handle->packet_size != pkt_sz + 1) {
 			/* reallocate buffer */
 			cmsis_dap_handle->packet_size = pkt_sz + 1;
@@ -809,14 +876,19 @@ static int cmsis_dap_init(void)
 
 	/* Now try to connect to the target
 	 * TODO: This is all SWD only @ present */
-	retval = cmsis_dap_cmd_DAP_SWJ_Clock(100);		/* 100kHz */
+	retval = cmsis_dap_cmd_DAP_SWJ_Clock(jtag_get_speed_khz());
 	if (retval != ERROR_OK)
 		return ERROR_FAIL;
 
+	/* Ask CMSIS-DAP to automatically retry on receiving WAIT for
+	 * up to 64 times. This must be changed to 0 if sticky
+	 * overrun detection is enabled. */
 	retval = cmsis_dap_cmd_DAP_TFER_Configure(0, 64, 0);
 	if (retval != ERROR_OK)
 		return ERROR_FAIL;
-	retval = cmsis_dap_cmd_DAP_SWD_Configure(0x00);
+	/* Data Phase (bit 2) must be set to 1 if sticky overrun
+	 * detection is enabled */
+	retval = cmsis_dap_cmd_DAP_SWD_Configure(0);	/* 1 TRN, no Data Phase */
 	if (retval != ERROR_OK)
 		return ERROR_FAIL;
 
@@ -836,47 +908,16 @@ static int cmsis_dap_init(void)
 		}
 	}
 
-	retval = cmsis_dap_reset_link();
-	if (retval != ERROR_OK)
-		return ERROR_FAIL;
-
-	cmsis_dap_cmd_DAP_LED(0x00);				/* Both LEDs off */
+	cmsis_dap_cmd_DAP_LED(0x00);			/* Both LEDs off */
 
 	LOG_INFO("CMSIS-DAP: Interface ready");
 
 	return ERROR_OK;
 }
 
-static int cmsis_dap_swd_init(uint8_t trn)
+static int cmsis_dap_swd_init(void)
 {
-	int retval;
-
-	DEBUG_IO("CMSIS-DAP: cmsis_dap_swd_init");
-
-	if (cmsis_dap_handle == NULL) {
-
-		/* SWD init */
-		retval = cmsis_dap_usb_open();
-		if (retval != ERROR_OK)
-			return retval;
-
-		retval = cmsis_dap_get_caps_info();
-		if (retval != ERROR_OK)
-			return retval;
-	}
-
-	if (!(cmsis_dap_handle->caps & INFO_CAPS_SWD)) {
-		LOG_ERROR("CMSIS-DAP: SWD not supported");
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	retval = cmsis_dap_cmd_DAP_Connect(CONNECT_SWD);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* Add more setup here.??... */
-
-	LOG_INFO("CMSIS-DAP: Interface Initialised (SWD)");
+	swd_mode = true;
 	return ERROR_OK;
 }
 
@@ -961,6 +1002,14 @@ static int cmsis_dap_khz(int khz, int *jtag_speed)
 	return ERROR_OK;
 }
 
+static int_least32_t cmsis_dap_swd_frequency(int_least32_t hz)
+{
+	if (hz > 0)
+		cmsis_dap_speed(hz / 1000);
+
+	return hz;
+}
+
 COMMAND_HANDLER(cmsis_dap_handle_info_command)
 {
 	if (cmsis_dap_get_version_info() == ERROR_OK)
@@ -999,6 +1048,27 @@ COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(cmsis_dap_handle_serial_command)
+{
+	if (CMD_ARGC == 1) {
+		size_t len = mbstowcs(NULL, CMD_ARGV[0], 0);
+		cmsis_dap_serial = calloc(len + 1, sizeof(wchar_t));
+		if (cmsis_dap_serial == NULL) {
+			LOG_ERROR("unable to allocate memory");
+			return ERROR_OK;
+		}
+		if (mbstowcs(cmsis_dap_serial, CMD_ARGV[0], len + 1) == (size_t)-1) {
+			free(cmsis_dap_serial);
+			cmsis_dap_serial = NULL;
+			LOG_ERROR("unable to convert serial");
+		}
+	} else {
+		LOG_ERROR("expected exactly one argument to cmsis_dap_serial <serial-number>");
+	}
+
+	return ERROR_OK;
+}
+
 static const struct command_registration cmsis_dap_subcommand_handlers[] = {
 	{
 		.name = "info",
@@ -1010,73 +1080,6 @@ static const struct command_registration cmsis_dap_subcommand_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
-COMMAND_HANDLER(cmsis_dap_reset_command)
-{
-	LOG_DEBUG("cmsis_dap_reset_command");
-	return ERROR_OK;
-}
-
-COMMAND_HANDLER(cmsis_dap_jtag_command)
-{
-	LOG_DEBUG("cmsis_dap_jtag_command");
-	return ERROR_OK;
-}
-
-static const struct command_registration cmsis_dap_jtag_subcommand_handlers[] = {
-	{
-		.name = "init",
-		.mode = COMMAND_ANY,
-		.handler = cmsis_dap_jtag_command,
-		.usage = ""
-	 },
-	{
-		.name = "arp_init",
-		.mode = COMMAND_ANY,
-		.handler = cmsis_dap_jtag_command,
-		.usage = ""
-	 },
-	{
-		.name = "arp_init-reset",
-		.mode = COMMAND_ANY,
-		.handler = cmsis_dap_reset_command,
-		.usage = ""
-	 },
-	{
-		.name = "tapisenabled",
-		.mode = COMMAND_EXEC,
-		.jim_handler = jim_jtag_tap_enabler,
-	 },
-	{
-		.name = "tapenable",
-		.mode = COMMAND_EXEC,
-		.jim_handler = jim_jtag_tap_enabler,
-	 },
-	{
-		.name = "tapdisable",
-		.mode = COMMAND_EXEC,
-		.handler = cmsis_dap_jtag_command,
-		.usage = "",
-	 },
-	{
-		.name = "configure",
-		.mode = COMMAND_EXEC,
-		.handler = cmsis_dap_jtag_command,
-		.usage = "",
-	 },
-	{
-		.name = "cget",
-		.mode = COMMAND_EXEC,
-		.jim_handler = jim_jtag_configure,
-	 },
-	{
-		.name = "names",
-		.mode = COMMAND_ANY,
-		.handler = cmsis_dap_jtag_command,
-		.usage = "",
-	 },
-	 COMMAND_REGISTRATION_DONE
-};
-
 static const struct command_registration cmsis_dap_command_handlers[] = {
 	{
 		.name = "cmsis-dap",
@@ -1093,24 +1096,25 @@ static const struct command_registration cmsis_dap_command_handlers[] = {
 		.usage = "(vid pid)* ",
 	},
 	{
-		/* this is currently a nasty hack so we get
-		 * reset working with non jtag interfaces */
-		.name = "jtag",
-		.mode = COMMAND_ANY,
-		.usage = "",
-		.chain = cmsis_dap_jtag_subcommand_handlers,
+		.name = "cmsis_dap_serial",
+		.handler = &cmsis_dap_handle_serial_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the serial number of the adapter",
+		.usage = "serial_string",
 	},
 	COMMAND_REGISTRATION_DONE
 };
 
 static const struct swd_driver cmsis_dap_swd_driver = {
-	.init       = cmsis_dap_swd_init,
-	.read_reg   = cmsis_dap_swd_read_reg,
-	.write_reg  = cmsis_dap_swd_write_reg,
-	.read_block = cmsis_dap_swd_read_block
+	.init = cmsis_dap_swd_init,
+	.frequency = cmsis_dap_swd_frequency,
+	.switch_seq = cmsis_dap_swd_switch_seq,
+	.read_reg = cmsis_dap_swd_read_reg,
+	.write_reg = cmsis_dap_swd_write_reg,
+	.run = cmsis_dap_swd_run_queue,
 };
 
-const char *cmsis_dap_transport[] = {"cmsis-dap", NULL};
+static const char * const cmsis_dap_transport[] = { "swd", NULL };
 
 struct jtag_interface cmsis_dap_interface = {
 	.name = "cmsis-dap",