jtag/drivers: Add support for TI XDS110 debug probe

Add support for the XDS110 debug probe using the APIs in the
probe's firmware. Includes support for older versions of the
firmware (with reduced performance) and support for a newer
version that includes OpenOCD specific APIs. Tested on various
TI LauchPads including MSP432P4, MSP432E4, CC2650, CC2652, and
CC3220SF.

Updated to add better support for swd switch. Removed issues found with
clang static analysis.

Updated to add rules entry for the XDS110 probe and Tiva DFU mode (which
affects both XDS110 and ICDI probes).

Change-Id: Ib274143111a68e67e80003797c6a68e3e80976b2
Signed-off-by: Edward Fewell <efewell@ti.com>
Reviewed-on: http://openocd.zylin.com/4322
Tested-by: jenkins
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>

7 files changed, 2009 insertions, 2 deletions

diff --git a/configure.ac b/configure.ac
index 11f8042..d4338df 100644
--- a/configure.ac
+++ b/configure.ac
@@ -115,7 +115,8 @@ m4_define([USB1_ADAPTERS],
 	[[ulink], [Keil ULINK JTAG Programmer], [ULINK]],
 	[[usb_blaster_2], [Altera USB-Blaster II Compatible], [USB_BLASTER_2]],
 	[[ft232r], [Bitbang mode of FT232R based devices], [FT232R]],
-	[[vsllink], [Versaloon-Link JTAG Programmer], [VSLLINK]]])
+	[[vsllink], [Versaloon-Link JTAG Programmer], [VSLLINK]],
+	[[xds110], [TI XDS110 Debug Probe], [XDS110]]])
 
 m4_define([USB_ADAPTERS],
 	[[[osbdm], [OSBDM (JTAG only) Programmer], [OSBDM]],
diff --git a/contrib/60-openocd.rules b/contrib/60-openocd.rules
index da760f8..af092c1 100644
--- a/contrib/60-openocd.rules
+++ b/contrib/60-openocd.rules
@@ -128,6 +128,12 @@ ATTRS{idVendor}=="1781", ATTRS{idProduct}=="0c63", MODE="660", GROUP="plugdev",
 # TI/Luminary Stellaris In-Circuit Debug Interface (ICDI) Board
 ATTRS{idVendor}=="1cbe", ATTRS{idProduct}=="00fd", MODE="660", GROUP="plugdev", TAG+="uaccess"
 
+# TI XDS110 Debug Probe (Launchpads and Standalone)
+ATTRS{idVendor}=="0451", ATTRS{idProduct}=="bef3", MODE="660", GROUP="plugdev", TAG+="uaccess"
+
+# TI Tiva-based ICDI and XDS110 probes in DFU mode
+ATTRS{idVendor}=="1cbe", ATTRS{idProduct}=="00ff", MODE="660", GROUP="plugdev", TAG+="uaccess"
+
 # Ambiq Micro EVK and Debug boards.
 ATTRS{idVendor}=="2aec", ATTRS{idProduct}=="6010", MODE="664", GROUP="plugdev", TAG+="uaccess"
 ATTRS{idVendor}=="2aec", ATTRS{idProduct}=="6011", MODE="664", GROUP="plugdev", TAG+="uaccess"
diff --git a/doc/openocd.texi b/doc/openocd.texi
index 3944572..4b964ed 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -531,6 +531,12 @@ debuggers to ARM Cortex based targets @url{http://www.keil.com/support/man/docs/
 
 @item @b{Keil ULINK v1}
 @* Link: @url{http://www.keil.com/ulink1/}
+
+@item @b{TI XDS110 Debug Probe}
+@* The XDS110 is included as the embedded debug probe on many Texas Instruments
+LaunchPad evaluation boards.
+@* Link: @url{http://processors.wiki.ti.com/index.php/XDS110}
+@* Link: @url{http://processors.wiki.ti.com/index.php/XDS_Emulation_Software_Package#XDS110_Support_Utilities}
 @end itemize
 
 @section IBM PC Parallel Printer Port Based
diff --git a/src/jtag/drivers/Makefile.am b/src/jtag/drivers/Makefile.am
index b3b4d21..ccef018 100644
--- a/src/jtag/drivers/Makefile.am
+++ b/src/jtag/drivers/Makefile.am
@@ -156,10 +156,12 @@ endif
 if IMX_GPIO
 DRIVERFILES += %D%/imx_gpio.c
 endif
-
 if KITPROG
 DRIVERFILES += %D%/kitprog.c
 endif
+if XDS110
+DRIVERFILES += %D%/xds110.c
+endif
 
 DRIVERHEADERS = \
 	%D%/bitbang.h \
diff --git a/src/jtag/drivers/xds110.c b/src/jtag/drivers/xds110.c
new file mode 100644
index 0000000..90b6990
--- /dev/null
+++ b/src/jtag/drivers/xds110.c
@@ -0,0 +1,1974 @@
+/***************************************************************************
+ *   Copyright (C) 2017 by Texas Instruments, Inc.                         *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <transport/transport.h>
+#include <jtag/swd.h>
+#include <jtag/interface.h>
+#include <jtag/commands.h>
+#include <jtag/tcl.h>
+#include <libusb.h>
+
+/* XDS110 USB serial number length */
+#define XDS110_SERIAL_LEN 8
+
+/* Firmware version that introduced OpenOCD support via block accesses */
+#define OCD_FIRMWARE_VERSION 0x02030011
+#define OCD_FIRMWARE_UPGRADE \
+	"XDS110: upgrade to version 2.3.0.11+ for improved support"
+
+/***************************************************************************
+ *   USB Connection Buffer Definitions                                     *
+ ***************************************************************************/
+
+/* Max USB packet size for up to USB 3.0 */
+#define MAX_PACKET 1024
+
+/*
+ * Maximum data payload that can be handled in a single call
+ * Limitation is the size of the buffers in the XDS110 firmware
+ */
+#define MAX_DATA_BLOCK 4096
+
+#ifndef USB_PAYLOAD_SIZE
+/* Largest data block plus parameters */
+#define USB_PAYLOAD_SIZE (MAX_DATA_BLOCK + 60)
+#endif
+#define MAX_RESULT_QUEUE (MAX_DATA_BLOCK / 4)
+
+/***************************************************************************
+ *   USB Connection Endpoints                                              *
+ ***************************************************************************/
+
+/* Bulk endpoints used by the XDS110 debug interface */
+#define INTERFACE_DEBUG (2)
+#define ENDPOINT_DEBUG_IN (3 | LIBUSB_ENDPOINT_IN)
+#define ENDPOINT_DEBUG_OUT (2 | LIBUSB_ENDPOINT_OUT)
+
+/***************************************************************************
+ *   XDS110 Firmware API Definitions                                       *
+ ***************************************************************************/
+
+/*
+ * Default values controlling how the host communicates commands
+ * with XDS110 firmware (automatic retry count and wait timeout)
+ */
+#define DEFAULT_ATTEMPTS (1)
+#define DEFAULT_TIMEOUT  (4000)
+
+/* XDS110 API error codes */
+#define SC_ERR_NONE             0
+#define SC_ERR_XDS110_FAIL   -261
+#define SC_ERR_SWD_WAIT      -613
+#define SC_ERR_SWD_FAULT     -614
+#define SC_ERR_SWD_PROTOCOL  -615
+#define SC_ERR_SWD_PARITY    -616
+#define SC_ERR_SWD_DEVICE_ID -617
+
+/* TCK frequency limits */
+#define XDS110_MIN_TCK_SPEED  100 /* kHz */
+#define XDS110_MAX_TCK_SPEED 2500 /* kHz */
+#define XDS110_TCK_PULSE_INCREMENT 66.0
+
+/* Scan mode on connect */
+#define MODE_JTAG 1
+
+/* XDS110 API JTAG state definitions */
+#define XDS_JTAG_STATE_RESET       1
+#define XDS_JTAG_STATE_IDLE        2
+#define XDS_JTAG_STATE_SHIFT_DR    3
+#define XDS_JTAG_STATE_SHIFT_IR    4
+#define XDS_JTAG_STATE_PAUSE_DR    5
+#define XDS_JTAG_STATE_PAUSE_IR    6
+#define XDS_JTAG_STATE_EXIT1_DR    8
+#define XDS_JTAG_STATE_EXIT1_IR    9
+#define XDS_JTAG_STATE_EXIT2_DR   10
+#define XDS_JTAG_STATE_EXIT2_IR   11
+#define XDS_JTAG_STATE_SELECT_DR  12
+#define XDS_JTAG_STATE_SELECT_IR  13
+#define XDS_JTAG_STATE_UPDATE_DR  14
+#define XDS_JTAG_STATE_UPDATE_IR  15
+#define XDS_JTAG_STATE_CAPTURE_DR 16
+#define XDS_JTAG_STATE_CAPTURE_IR 17
+
+/* XDS110 API JTAG transit definitions */
+#define XDS_JTAG_TRANSIT_QUICKEST    1
+#define XDS_JTAG_TRANSIT_VIA_CAPTURE 2
+#define XDS_JTAG_TRANSIT_VIA_IDLE    3
+
+/* DAP register definitions as used by XDS110 APIs */
+
+#define DAP_AP 0 /* DAP AP register type */
+#define DAP_DP 1 /* DAP DP register type */
+
+#define DAP_DP_IDCODE 0x0 /* DAP DP IDCODE register (read only) */
+#define DAP_DP_ABORT  0x0 /* DAP DP ABORT register (write only) */
+#define DAP_DP_STAT   0x4 /* DAP DP STAT register (for read only) */
+#define DAP_DP_CTRL   0x4 /* DAP DP CTRL register (for write only) */
+#define DAP_DP_ADDR   0x8 /* DAP DP SELECT register (legacy name) */
+#define DAP_DP_RESEND 0x8 /* DAP DP RESEND register (read only) */
+#define DAP_DP_SELECT 0x8 /* DAP DP SELECT register (write only) */
+#define DAP_DP_RDBUFF 0xc /* DAP DP RDBUFF Read Buffer register */
+
+#define DAP_AP_CSW  0x00 /* DAP AP Control Status Word */
+#define DAP_AP_TAR  0x04 /* DAP AP Transfer Address */
+#define DAP_AP_DRW  0x0C /* DAP AP Data Read/Write */
+#define DAP_AP_BD0  0x10 /* DAP AP Banked Data 0 */
+#define DAP_AP_BD1  0x14 /* DAP AP Banked Data 1 */
+#define DAP_AP_BD2  0x18 /* DAP AP Banked Data 2 */
+#define DAP_AP_BD3  0x1C /* DAP AP Banked Data 3 */
+#define DAP_AP_RTBL 0xF8 /* DAP AP Debug ROM Table */
+#define DAP_AP_IDR  0xFC /* DAP AP Identification Register */
+
+/* Command packet definitions */
+
+#define XDS_OUT_LEN 1 /* command (byte) */
+#define XDS_IN_LEN  4 /* error code (int) */
+
+/* XDS API Commands */
+#define XDS_CONNECT      0x01 /* Connect JTAG connection */
+#define XDS_DISCONNECT   0x02 /* Disconnect JTAG connection */
+#define XDS_VERSION      0x03 /* Get firmware version and hardware ID */
+#define XDS_SET_TCK      0x04 /* Set TCK delay (to set TCK frequency) */
+#define XDS_SET_TRST     0x05 /* Assert or deassert nTRST signal */
+#define XDS_CYCLE_TCK    0x07 /* Toggle TCK for a number of cycles */
+#define XDS_GOTO_STATE   0x09 /* Go to requested JTAG state */
+#define XDS_JTAG_SCAN    0x0c /* Send and receive JTAG scan */
+#define XDS_SET_SRST     0x0e /* Assert or deassert nSRST signal */
+#define CMAPI_CONNECT    0x0f /* CMAPI connect */
+#define CMAPI_DISCONNECT 0x10 /* CMAPI disconnect */
+#define CMAPI_ACQUIRE    0x11 /* CMAPI acquire */
+#define CMAPI_RELEASE    0x12 /* CMAPI release */
+#define CMAPI_REG_READ   0x15 /* CMAPI DAP register read */
+#define CMAPI_REG_WRITE  0x16 /* CMAPI DAP register write */
+#define SWD_CONNECT      0x17 /* Switch from JTAG to SWD connection */
+#define SWD_DISCONNECT   0x18 /* Switch from SWD to JTAG connection */
+#define CJTAG_CONNECT    0x2b /* Switch from JTAG to cJTAG connection */
+#define CJTAG_DISCONNECT 0x2c /* Switch from cJTAG to JTAG connection */
+#define OCD_DAP_REQUEST  0x3a /* Handle block of DAP requests */
+#define OCD_SCAN_REQUEST 0x3b /* Handle block of JTAG scan requests */
+#define OCD_PATHMOVE     0x3c /* Handle PATHMOVE to navigate JTAG states */
+
+#define CMD_IR_SCAN      1
+#define CMD_DR_SCAN      2
+#define CMD_RUNTEST      3
+#define CMD_STABLECLOCKS 4
+
+/* Array to convert from OpenOCD tap_state_t to XDS JTAG state */
+const uint32_t xds_jtag_state[] = {
+	XDS_JTAG_STATE_EXIT2_DR,   /* TAP_DREXIT2   = 0x0 */
+	XDS_JTAG_STATE_EXIT1_DR,   /* TAP_DREXIT1   = 0x1 */
+	XDS_JTAG_STATE_SHIFT_DR,   /* TAP_DRSHIFT   = 0x2 */
+	XDS_JTAG_STATE_PAUSE_DR,   /* TAP_DRPAUSE   = 0x3 */
+	XDS_JTAG_STATE_SELECT_IR,  /* TAP_IRSELECT  = 0x4 */
+	XDS_JTAG_STATE_UPDATE_DR,  /* TAP_DRUPDATE  = 0x5 */
+	XDS_JTAG_STATE_CAPTURE_DR, /* TAP_DRCAPTURE = 0x6 */
+	XDS_JTAG_STATE_SELECT_DR,  /* TAP_DRSELECT  = 0x7 */
+	XDS_JTAG_STATE_EXIT2_IR,   /* TAP_IREXIT2   = 0x8 */
+	XDS_JTAG_STATE_EXIT1_IR,   /* TAP_IREXIT1   = 0x9 */
+	XDS_JTAG_STATE_SHIFT_IR,   /* TAP_IRSHIFT   = 0xa */
+	XDS_JTAG_STATE_PAUSE_IR,   /* TAP_IRPAUSE   = 0xb */
+	XDS_JTAG_STATE_IDLE,       /* TAP_IDLE      = 0xc */
+	XDS_JTAG_STATE_UPDATE_IR,  /* TAP_IRUPDATE  = 0xd */
+	XDS_JTAG_STATE_CAPTURE_IR, /* TAP_IRCAPTURE = 0xe */
+	XDS_JTAG_STATE_RESET,      /* TAP_RESET     = 0xf */
+};
+
+struct scan_result {
+	bool first;
+	uint8_t *buffer;
+	uint32_t num_bits;
+};
+
+struct xds110_info {
+	/* USB connection handles and data buffers */
+	libusb_context *ctx;
+	libusb_device_handle *dev;
+	unsigned char read_payload[USB_PAYLOAD_SIZE];
+	unsigned char write_packet[3];
+	unsigned char write_payload[USB_PAYLOAD_SIZE];
+	/* Status flags */
+	bool is_connected;
+	bool is_cmapi_connected;
+	bool is_cmapi_acquired;
+	bool is_swd_mode;
+	bool is_ap_dirty;
+	/* DAP register caches */
+	uint32_t select;
+	uint32_t rdbuff;
+	bool use_rdbuff;
+	/* TCK speed and delay count*/
+	uint32_t speed;
+	uint32_t delay_count;
+	/* XDS110 serial number */
+	char serial[XDS110_SERIAL_LEN + 1];
+	/* XDS110 firmware and hardware version */
+	uint32_t firmware;
+	uint16_t hardware;
+	/* Transaction queues */
+	unsigned char txn_requests[MAX_DATA_BLOCK];
+	uint32_t *txn_dap_results[MAX_DATA_BLOCK / 4];
+	struct scan_result txn_scan_results[MAX_DATA_BLOCK / 4];
+	uint32_t txn_request_size;
+	uint32_t txn_result_size;
+	uint32_t txn_result_count;
+};
+
+static struct xds110_info xds110 = {
+	.ctx = NULL,
+	.dev = NULL,
+	.is_connected = false,
+	.is_cmapi_connected = false,
+	.is_cmapi_acquired = false,
+	.is_swd_mode = false,
+	.is_ap_dirty = false,
+	.speed = XDS110_MAX_TCK_SPEED,
+	.delay_count = 0,
+	.serial = {0},
+	.firmware = 0,
+	.hardware = 0,
+	.txn_request_size = 0,
+	.txn_result_size = 0,
+	.txn_result_count = 0
+};
+
+static inline void xds110_set_u32(uint8_t *buffer, uint32_t value)
+{
+	buffer[3] = (value >> 24) & 0xff;
+	buffer[2] = (value >> 16) & 0xff;
+	buffer[1] = (value >> 8) & 0xff;
+	buffer[0] = (value >> 0) & 0xff;
+}
+
+static inline void xds110_set_u16(uint8_t *buffer, uint16_t value)
+{
+	buffer[1] = (value >> 8) & 0xff;
+	buffer[0] = (value >> 0) & 0xff;
+}
+
+static inline uint32_t xds110_get_u32(uint8_t *buffer)
+{
+	uint32_t value = (((uint32_t)buffer[3]) << 24) |
+					 (((uint32_t)buffer[2]) << 16) |
+					 (((uint32_t)buffer[1]) << 8)  |
+					 (((uint32_t)buffer[0]) << 0);
+	return value;
+}
+
+static inline uint16_t xds110_get_u16(uint8_t *buffer)
+{
+	uint16_t value = (((uint32_t)buffer[1]) << 8) |
+					 (((uint32_t)buffer[0]) << 0);
+	return value;
+}
+
+/***************************************************************************
+ *   usb connection routines                                               *
+ *                                                                         *
+ *   The following functions handle connecting, reading, and writing to    *
+ *   the XDS110 over USB using the libusb library.                         *
+ ***************************************************************************/
+
+static bool usb_connect(void)
+{
+	libusb_context *ctx  = NULL;
+	libusb_device **list = NULL;
+	libusb_device_handle *dev  = NULL;
+
+	struct libusb_device_descriptor desc;
+
+	uint16_t vid = 0x0451;
+	uint16_t pid = 0xbef3;
+	ssize_t count = 0;
+	ssize_t i = 0;
+	int result = 0;
+	bool found = false;
+
+	/* Initialize libusb context */
+	result = libusb_init(&ctx);
+
+	if (0 == result) {
+		/* Get list of USB devices attached to system */
+		count = libusb_get_device_list(ctx, &list);
+		if (count <= 0) {
+			result = -1;
+			list = NULL;
+		}
+	}
+
+	if (0 == result) {
+		/* Scan through list of devices for any XDS110s */
+		for (i = 0; i < count; i++) {
+			/* Check for device VID/PID match */
+			libusb_get_device_descriptor(list[i], &desc);
+			if (desc.idVendor == vid && desc.idProduct == pid) {
+				result = libusb_open(list[i], &dev);
+				if (0 == result) {
+					const int MAX_DATA = 256;
+					unsigned char data[MAX_DATA + 1];
+					*data = '\0';
+
+					/* May be the requested device if serial number matches */
+					if (0 == xds110.serial[0]) {
+						/* No serial number given; match first XDS110 found */
+						found = true;
+						break;
+					} else {
+						/* Get the device's serial number string */
+						result = libusb_get_string_descriptor_ascii(dev,
+									desc.iSerialNumber, data, MAX_DATA);
+						if (0 < result &&
+							0 == strcmp((char *)data, (char *)xds110.serial)) {
+							found = true;
+							break;
+						}
+					}
+
+					/* If we fall though to here, we don't want this device */
+					libusb_close(dev);
+					dev = NULL;
+				}
+			}
+		}
+	}
+
+	/*
+	 * We can fall through the for() loop with two possible exit conditions:
+	 * 1) found the right XDS110, and that device is open
+	 * 2) didn't find the XDS110, and no devices are currently open
+	 */
+
+	if (NULL != list) {
+		/* Free the device list, we're done with it */
+		libusb_free_device_list(list, 1);
+	}
+
+	if (found) {
+		/* Save the context and device handles */
+		xds110.ctx = ctx;
+		xds110.dev = dev;
+
+		/* Set libusb to auto detach kernel and disable debug messages */
+		(void)libusb_set_auto_detach_kernel_driver(dev, 1);
+		libusb_set_debug(ctx, LIBUSB_LOG_LEVEL_NONE);
+
+		/* Claim the debug interface on the XDS110 */
+		result = libusb_claim_interface(dev, INTERFACE_DEBUG);
+	} else {
+		/* Couldn't find an XDS110, flag the error */
+		result = -1;
+	}
+
+	/* On an error, clean up what we can */
+	if (0 != result) {
+		if (NULL != dev) {
+			/* Release the debug and data interface on the XDS110 */
+			(void)libusb_release_interface(dev, INTERFACE_DEBUG);
+			libusb_close(dev);
+		}
+		if (NULL != ctx)
+			libusb_exit(ctx);
+		xds110.ctx = NULL;
+		xds110.dev = NULL;
+	}
+
+	/* Log the results */
+	if (0 == result)
+		LOG_INFO("XDS110: connected");
+	else
+		LOG_ERROR("XDS110: failed to connect");
+
+	return (0 == result) ? true : false;
+}
+
+static void usb_disconnect(void)
+{
+	if (NULL != xds110.dev) {
+		/* Release the debug and data interface on the XDS110 */
+		(void)libusb_release_interface(xds110.dev, INTERFACE_DEBUG);
+		libusb_close(xds110.dev);
+		xds110.dev = NULL;
+	}
+	if (NULL != xds110.ctx) {
+		libusb_exit(xds110.ctx);
+		xds110.ctx = NULL;
+	}
+
+	LOG_INFO("XDS110: disconnected");
+}
+
+static bool usb_read(unsigned char *buffer, int size, int *bytes_read,
+	int timeout)
+{
+	int result;
+
+	if (NULL == xds110.dev || NULL == buffer || NULL == bytes_read)
+		return false;
+
+	/* Force a non-zero timeout to prevent blocking */
+	if (0 == timeout)
+		timeout = DEFAULT_TIMEOUT;
+
+	result = libusb_bulk_transfer(xds110.dev, ENDPOINT_DEBUG_IN, buffer, size,
+				bytes_read, timeout);
+
+	return (0 == result) ? true : false;
+}
+
+static bool usb_write(unsigned char *buffer, int size, int *written)
+{
+	int bytes_written = 0;
+	int result = LIBUSB_SUCCESS;
+	int retries = 0;
+
+	if (NULL == xds110.dev || NULL == buffer)
+		return false;
+
+	result = libusb_bulk_transfer(xds110.dev, ENDPOINT_DEBUG_OUT, buffer,
+				size, &bytes_written, 0);
+
+	while (LIBUSB_ERROR_PIPE == result && retries < 3) {
+		/* Try clearing the pipe stall and retry transfer */
+		libusb_clear_halt(xds110.dev, ENDPOINT_DEBUG_OUT);
+		result = libusb_bulk_transfer(xds110.dev, ENDPOINT_DEBUG_OUT, buffer,
+					size, &bytes_written, 0);
+		retries++;
+	}
+
+	if (NULL != written)
+		*written = bytes_written;
+
+	return (0 == result && size == bytes_written) ? true : false;
+}
+
+static bool usb_get_response(uint32_t *total_bytes_read, uint32_t timeout)
+{
+	static unsigned char buffer[MAX_PACKET];
+	int bytes_read;
+	uint16_t size;
+	uint16_t count;
+	bool success;
+
+	size = 0;
+	success = true;
+	while (success) {
+		success = usb_read(buffer, sizeof(buffer), &bytes_read, timeout);
+		if (success) {
+			/*
+			 * Validate that this appears to be a good response packet
+			 * First check it contains enough data for header and error
+			 * code, plus the first character is the start character
+			 */
+			if (bytes_read >= 7 && '*' == buffer[0]) {
+				/* Extract the payload size */
+				size = xds110_get_u16(&buffer[1]);
+				/* Sanity test on payload size */
+				if (USB_PAYLOAD_SIZE >= size && 4 <= size) {
+					/* Check we didn't get more data than expected */
+					if ((bytes_read - 3) <= size) {
+						/* Packet appears to be valid, move on */
+						break;
+					}
+				}
+			}
+		}
+		/*
+		 * Somehow received an invalid packet, retry till we
+		 * time out or a valid response packet is received
+		 */
+	}
+
+	/* Abort now if we didn't receive a valid response */
+	if (!success) {
+		if (NULL != total_bytes_read)
+			*total_bytes_read = 0;
+		return false;
+	}
+
+	/* Build the return payload into xds110.read_payload */
+
+	/* Copy over payload data from received buffer (skipping header) */
+	count = 0;
+	bytes_read -= 3;
+	memcpy((void *)&xds110.read_payload[count], (void *)&buffer[3], bytes_read);
+	count += bytes_read;
+	/*
+	 * Drop timeout to just 1/2 second. Once the XDS110 starts sending
+	 * a response, the remaining packets should arrive in short order
+	 */
+	if (timeout > 500)
+		timeout = 500; /* ms */
+
+	/* If there's more data to retrieve, get it now */
+	while ((count < size) && success) {
+		success = usb_read(buffer, sizeof(buffer), &bytes_read, timeout);
+		if (success) {
+			if ((count + bytes_read) > size) {
+				/* Read too much data, not a valid packet, abort */
+				success = false;
+			} else {
+				/* Copy this data over to xds110.read_payload */
+				memcpy((void *)&xds110.read_payload[count], (void *)buffer,
+					bytes_read);
+				count += bytes_read;
+			}
+		}
+	}
+
+	if (!success)
+		count = 0;
+	if (NULL != total_bytes_read)
+		*total_bytes_read = count;
+
+	return success;
+}
+
+static bool usb_send_command(uint16_t size)
+{
+	int written;
+	bool success = true;
+
+	/* Check the packet length */
+	if (size > USB_PAYLOAD_SIZE)
+		return false;
+
+	/* Place the start character into the packet buffer */
+	xds110.write_packet[0] = '*';
+
+	/* Place the payload size into the packet buffer */
+	xds110_set_u16(&xds110.write_packet[1], size);
+
+	/* Adjust size to include header */
+	size += 3;
+
+	/* Send the data via the USB connection */
+	success = usb_write(xds110.write_packet, (int)size, &written);
+
+	/* Check if the correct number of bytes was written */
+	if (written != (int)size)
+		success = false;
+
+	return success;
+}
+
+/***************************************************************************
+ *   XDS110 firmware API routines                                          *
+ *                                                                         *
+ *   The following functions handle calling into the XDS110 firmware to    *
+ *   perform requested debug actions.                                      *
+ ***************************************************************************/
+
+static bool xds_execute(uint32_t out_length, uint32_t in_length,
+	uint32_t attempts, uint32_t timeout)
+{
+	bool done = false;
+	bool success = true;
+	int error = 0;
+	uint32_t bytes_read = 0;
+
+	if (NULL == xds110.dev)
+		return false;
+
+	while (!done && attempts > 0) {
+		attempts--;
+
+		/* Send command to XDS110 */
+		success = usb_send_command(out_length);
+
+		if (success) {
+			/* Get response from XDS110 */
+			success = usb_get_response(&bytes_read, timeout);
+		}
+
+		if (success) {
+			/* Check for valid response from XDS code handling */
+			if (bytes_read != in_length) {
+				/* Unexpected amount of data returned */
+				success = false;
+			} else {
+				/* Extract error code from return packet */
+				error = (int)xds110_get_u32(&xds110.read_payload[0]);
+				done = true;
+			}
+		}
+	}
+
+	if (!success)
+		error = SC_ERR_XDS110_FAIL;
+
+	if (0 != error)
+		success = false;
+
+	return success;
+}
+
+static bool xds_connect(void)
+{
+	bool success;
+
+	xds110.write_payload[0] = XDS_CONNECT;
+
+	success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool xds_disconnect(void)
+{
+	bool success;
+
+	xds110.write_payload[0] = XDS_DISCONNECT;
+
+	success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool xds_version(uint32_t *firmware_id, uint16_t *hardware_id)
+{
+	uint8_t *fw_id_pntr = &xds110.read_payload[XDS_IN_LEN + 0]; /* 32-bits */
+	uint8_t *hw_id_pntr = &xds110.read_payload[XDS_IN_LEN + 4]; /* 16-bits */
+
+	bool success;
+
+	xds110.write_payload[0] = XDS_VERSION;
+
+	success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN + 6, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	if (success) {
+		if (NULL != firmware_id)
+			*firmware_id = xds110_get_u32(fw_id_pntr);
+		if (NULL != hardware_id)
+			*hardware_id = xds110_get_u16(hw_id_pntr);
+	}
+
+	return success;
+}
+
+static bool xds_set_tck_delay(uint32_t delay)
+{
+	uint8_t *delay_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 32-bits */
+
+	bool success;
+
+	xds110.write_payload[0] = XDS_SET_TCK;
+
+	xds110_set_u32(delay_pntr, delay);
+
+	success = xds_execute(XDS_OUT_LEN + 4, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool xds_set_trst(uint8_t trst)
+{
+	uint8_t *trst_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 8-bits */
+
+	bool success;
+
+	xds110.write_payload[0] = XDS_SET_TRST;
+
+	*trst_pntr = trst;
+
+	success = xds_execute(XDS_OUT_LEN + 1, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool xds_cycle_tck(uint32_t count)
+{
+	uint8_t *count_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 32-bits */
+
+	bool success;
+
+	xds110.write_payload[0] = XDS_CYCLE_TCK;
+
+	xds110_set_u32(count_pntr, count);
+
+	success = xds_execute(XDS_OUT_LEN + 4, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool xds_goto_state(uint32_t state)
+{
+	uint8_t *state_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 32-bits */
+	uint8_t *transit_pntr = &xds110.write_payload[XDS_OUT_LEN+4]; /* 32-bits */
+
+	bool success;
+
+	xds110.write_payload[0] = XDS_GOTO_STATE;
+
+	xds110_set_u32(state_pntr, state);
+	xds110_set_u32(transit_pntr, XDS_JTAG_TRANSIT_QUICKEST);
+
+	success = xds_execute(XDS_OUT_LEN+8, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool xds_jtag_scan(uint32_t shift_state, uint16_t shift_bits,
+	uint32_t end_state, uint8_t *data_out, uint8_t *data_in)
+{
+	uint8_t *bits_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 16-bits */
+	uint8_t *path_pntr = &xds110.write_payload[XDS_OUT_LEN + 2]; /* 8-bits */
+	uint8_t *trans1_pntr = &xds110.write_payload[XDS_OUT_LEN + 3]; /* 8-bits */
+	uint8_t *end_pntr = &xds110.write_payload[XDS_OUT_LEN + 4]; /* 8-bits */
+	uint8_t *trans2_pntr = &xds110.write_payload[XDS_OUT_LEN + 5]; /* 8-bits */
+	uint8_t *pre_pntr = &xds110.write_payload[XDS_OUT_LEN + 6]; /* 16-bits */
+	uint8_t *pos_pntr = &xds110.write_payload[XDS_OUT_LEN + 8]; /* 16-bits */
+	uint8_t *delay_pntr = &xds110.write_payload[XDS_OUT_LEN + 10]; /* 16-bits */
+	uint8_t *rep_pntr = &xds110.write_payload[XDS_OUT_LEN + 12]; /* 16-bits */
+	uint8_t *out_pntr = &xds110.write_payload[XDS_OUT_LEN + 14]; /* 16-bits */
+	uint8_t *in_pntr = &xds110.write_payload[XDS_OUT_LEN + 16]; /* 16-bits */
+	uint8_t *data_out_pntr = &xds110.write_payload[XDS_OUT_LEN + 18];
+	uint8_t *data_in_pntr = &xds110.read_payload[XDS_IN_LEN+0];
+
+	uint16_t total_bytes = DIV_ROUND_UP(shift_bits, 8);
+
+	bool success;
+
+	xds110.write_payload[0] = XDS_JTAG_SCAN;
+
+	xds110_set_u16(bits_pntr, shift_bits); /* bits to scan */
+	*path_pntr = (uint8_t)(shift_state & 0xff); /* IR vs DR path */
+	*trans1_pntr = (uint8_t)XDS_JTAG_TRANSIT_QUICKEST; /* start state route */
+	*end_pntr = (uint8_t)(end_state & 0xff); /* JTAG state after scan */
+	*trans2_pntr = (uint8_t)XDS_JTAG_TRANSIT_QUICKEST; /* end state route */
+	xds110_set_u16(pre_pntr, 0); /* number of preamble bits */
+	xds110_set_u16(pos_pntr, 0); /* number of postamble bits */
+	xds110_set_u16(delay_pntr, 0); /* number of extra TCKs after scan */
+	xds110_set_u16(rep_pntr, 1); /* number of repetitions */
+	xds110_set_u16(out_pntr, total_bytes); /* out buffer offset (if repeats) */
+	xds110_set_u16(in_pntr, total_bytes); /* in buffer offset (if repeats) */
+
+	memcpy((void *)data_out_pntr, (void *)data_out, total_bytes);
+
+	success = xds_execute(XDS_OUT_LEN + 18 + total_bytes,
+		XDS_IN_LEN + total_bytes, DEFAULT_ATTEMPTS, DEFAULT_TIMEOUT);
+
+	if (success)
+		memcpy((void *)data_in, (void *)data_in_pntr, total_bytes);
+
+	return success;
+}
+
+static bool xds_set_srst(uint8_t srst)
+{
+	uint8_t *srst_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 8-bits */
+
+	bool success;
+
+	xds110.write_payload[0] = XDS_SET_SRST;
+
+	*srst_pntr = srst;
+
+	success = xds_execute(XDS_OUT_LEN + 1, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool cmapi_connect(uint32_t *idcode)
+{
+	uint8_t *idcode_pntr = &xds110.read_payload[XDS_IN_LEN + 0]; /* 32-bits */
+
+	bool success;
+
+	xds110.write_payload[0] = CMAPI_CONNECT;
+
+	success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN+4, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	if (success) {
+		if (NULL != idcode)
+			*idcode = xds110_get_u32(idcode_pntr);
+	}
+
+	return success;
+}
+
+static bool cmapi_disconnect(void)
+{
+	bool success;
+
+	xds110.write_payload[0] = CMAPI_DISCONNECT;
+
+	success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool cmapi_acquire(void)
+{
+	bool success;
+
+	xds110.write_payload[0] = CMAPI_ACQUIRE;
+
+	success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool cmapi_release(void)
+{
+	bool success;
+
+	xds110.write_payload[0] = CMAPI_RELEASE;
+
+	success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool cmapi_read_dap_reg(uint32_t type, uint32_t ap_num,
+	uint32_t address, uint32_t *value)
+{
+	uint8_t *type_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 8-bits */
+	uint8_t *ap_num_pntr = &xds110.write_payload[XDS_OUT_LEN + 1]; /* 8-bits */
+	uint8_t *address_pntr = &xds110.write_payload[XDS_OUT_LEN + 2]; /* 8-bits */
+	uint8_t *value_pntr = &xds110.read_payload[XDS_IN_LEN + 0]; /* 32-bits */
+
+	bool success;
+
+	xds110.write_payload[0] = CMAPI_REG_READ;
+
+	*type_pntr = (uint8_t)(type & 0xff);
+	*ap_num_pntr = (uint8_t)(ap_num & 0xff);
+	*address_pntr = (uint8_t)(address & 0xff);
+
+	success = xds_execute(XDS_OUT_LEN + 3, XDS_IN_LEN + 4, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	if (success) {
+		if (NULL != value)
+			*value = xds110_get_u32(value_pntr);
+	}
+
+	return success;
+}
+
+static bool cmapi_write_dap_reg(uint32_t type, uint32_t ap_num,
+	uint32_t address, uint32_t *value)
+{
+	uint8_t *type_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 8-bits */
+	uint8_t *ap_num_pntr = &xds110.write_payload[XDS_OUT_LEN + 1]; /* 8-bits */
+	uint8_t *address_pntr = &xds110.write_payload[XDS_OUT_LEN + 2]; /* 8-bits */
+	uint8_t *value_pntr = &xds110.write_payload[XDS_OUT_LEN + 3]; /* 32-bits */
+
+	bool success;
+
+	if (NULL == value)
+		return false;
+
+	xds110.write_payload[0] = CMAPI_REG_WRITE;
+
+	*type_pntr = (uint8_t)(type & 0xff);
+	*ap_num_pntr = (uint8_t)(ap_num & 0xff);
+	*address_pntr = (uint8_t)(address & 0xff);
+	xds110_set_u32(value_pntr, *value);
+
+	success = xds_execute(XDS_OUT_LEN + 7, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool swd_connect(void)
+{
+	bool success;
+
+	xds110.write_payload[0] = SWD_CONNECT;
+
+	success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool swd_disconnect(void)
+{
+	bool success;
+
+	xds110.write_payload[0] = SWD_DISCONNECT;
+
+	success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool cjtag_connect(uint32_t format)
+{
+	uint8_t *format_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 32-bits */
+
+	bool success;
+
+	xds110.write_payload[0] = CJTAG_CONNECT;
+
+	xds110_set_u32(format_pntr, format);
+
+	success = xds_execute(XDS_OUT_LEN + 4, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool cjtag_disconnect(void)
+{
+	bool success;
+
+	xds110.write_payload[0] = CJTAG_DISCONNECT;
+
+	success = xds_execute(XDS_OUT_LEN, XDS_IN_LEN, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+static bool ocd_dap_request(uint8_t *dap_requests, uint32_t request_size,
+	uint32_t *dap_results, uint32_t result_count)
+{
+	uint8_t *request_pntr = &xds110.write_payload[XDS_OUT_LEN + 0];
+	uint8_t *result_pntr = &xds110.read_payload[XDS_IN_LEN + 0];
+
+	bool success;
+
+	if (NULL == dap_requests || NULL == dap_results)
+		return false;
+
+	xds110.write_payload[0] = OCD_DAP_REQUEST;
+
+	memcpy((void *)request_pntr, (void *)dap_requests, request_size);
+
+	success = xds_execute(XDS_OUT_LEN + request_size,
+				XDS_IN_LEN + (result_count * 4), DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	if (success && (result_count > 0))
+		memcpy((void *)dap_results, (void *)result_pntr, result_count * 4);
+
+	return success;
+}
+
+static bool ocd_scan_request(uint8_t *scan_requests, uint32_t request_size,
+	uint8_t *scan_results, uint32_t result_size)
+{
+	uint8_t *request_pntr = &xds110.write_payload[XDS_OUT_LEN + 0];
+	uint8_t *result_pntr = &xds110.read_payload[XDS_IN_LEN + 0];
+
+	bool success;
+
+	if (NULL == scan_requests || NULL == scan_results)
+		return false;
+
+	xds110.write_payload[0] = OCD_SCAN_REQUEST;
+
+	memcpy((void *)request_pntr, (void *)scan_requests, request_size);
+
+	success = xds_execute(XDS_OUT_LEN + request_size,
+				XDS_IN_LEN + result_size, DEFAULT_ATTEMPTS,
+				DEFAULT_TIMEOUT);
+
+	if (success && (result_size > 0))
+		memcpy((void *)scan_results, (void *)result_pntr, result_size);
+
+	return success;
+}
+
+static bool ocd_pathmove(uint32_t num_states, uint8_t *path)
+{
+	uint8_t *num_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 32-bits */
+	uint8_t *path_pntr = &xds110.write_payload[XDS_OUT_LEN + 4];
+
+	bool success;
+
+	if (NULL == path)
+		return false;
+
+	xds110.write_payload[0] = OCD_PATHMOVE;
+
+	xds110_set_u32(num_pntr, num_states);
+
+	memcpy((void *)path_pntr, (void *)path, num_states);
+
+	success = xds_execute(XDS_OUT_LEN + 4 + num_states, XDS_IN_LEN,
+				DEFAULT_ATTEMPTS, DEFAULT_TIMEOUT);
+
+	return success;
+}
+
+/***************************************************************************
+ *   swd driver interface                                                  *
+ *                                                                         *
+ *   The following functions provide SWD support to OpenOCD.               *
+ ***************************************************************************/
+
+static int xds110_swd_init(void)
+{
+	xds110.is_swd_mode = true;
+	return ERROR_OK;
+}
+
+static int xds110_swd_switch_seq(enum swd_special_seq seq)
+{
+	uint32_t idcode;
+	bool success;
+
+	switch (seq) {
+	case LINE_RESET:
+		LOG_ERROR("Sequence SWD line reset (%d) not supported", seq);
+		return ERROR_FAIL;
+	case JTAG_TO_SWD:
+		LOG_DEBUG("JTAG-to-SWD");
+		xds110.is_swd_mode = false;
+		xds110.is_cmapi_connected = false;
+		xds110.is_cmapi_acquired = false;
+		/* Run sequence to put target in SWD mode */
+		success = swd_connect();
+		/* Re-iniitialize CMAPI API for DAP access */
+		if (success) {
+			xds110.is_swd_mode = true;
+			success = cmapi_connect(&idcode);
+			if (success) {
+				xds110.is_cmapi_connected = true;
+				success = cmapi_acquire();
+			}
+		}
+		break;
+	case SWD_TO_JTAG:
+		LOG_DEBUG("SWD-to-JTAG");
+		xds110.is_swd_mode = false;
+		xds110.is_cmapi_connected = false;
+		xds110.is_cmapi_acquired = false;
+		/* Run sequence to put target in JTAG mode */
+		success = swd_disconnect();
+		if (success) {
+			/* Re-initialize JTAG interface */
+			success = cjtag_connect(MODE_JTAG);
+		}
+		break;
+	default:
+		LOG_ERROR("Sequence %d not supported", seq);
+		return ERROR_FAIL;
+	}
+
+	if (success)
+		return ERROR_OK;
+	else
+		return ERROR_FAIL;
+}
+
+static bool xds110_legacy_read_reg(uint8_t cmd, uint32_t *value)
+{
+	/* Make sure this is a read request */
+	bool is_read_request = (0 != (SWD_CMD_RnW & cmd));
+	/* Determine whether this is a DP or AP register access */
+	uint32_t type = (0 != (SWD_CMD_APnDP & cmd)) ? DAP_AP : DAP_DP;
+	/* Determine the AP number from cached SELECT value */
+	uint32_t ap_num = (xds110.select & 0xff000000) >> 24;
+	/* Extract register address from command */
+	uint32_t address = ((cmd & SWD_CMD_A32) >> 1);
+	/* Extract bank address from cached SELECT value */
+	uint32_t bank = (xds110.select & 0x000000f0);
+
+	uint32_t reg_value = 0;
+	uint32_t temp_value = 0;
+
+	bool success;
+
+	if (!is_read_request)
+		return false;
+
+	if (DAP_AP == type) {
+		/* Add bank address to register address for CMAPI call */
+		address |= bank;
+	}
+
+	if (DAP_DP == type && DAP_DP_RDBUFF == address && xds110.use_rdbuff) {
+		/* If RDBUFF is cached and this is a DP RDBUFF read, use the cache */
+		reg_value = xds110.rdbuff;
+		success = true;
+	} else if (DAP_AP == type && DAP_AP_DRW == address && xds110.use_rdbuff) {
+		/* If RDBUFF is cached and this is an AP DRW read, use the cache, */
+		/* but still call into the firmware to get the next read. */
+		reg_value = xds110.rdbuff;
+		success = cmapi_read_dap_reg(type, ap_num, address, &temp_value);
+	} else {
+		success = cmapi_read_dap_reg(type, ap_num, address, &temp_value);
+		if (success)
+			reg_value = temp_value;
+	}
+
+	/* Mark that we have consumed or invalidated the RDBUFF cache */
+	xds110.use_rdbuff = false;
+
+	/* Handle result of read attempt */
+	if (!success)
+		LOG_ERROR("XDS110: failed to read DAP register");
+	else if (NULL != value)
+		*value = reg_value;
+
+	if (success && DAP_AP == type) {
+		/*
+		 * On a successful DAP AP read, we actually have the value from RDBUFF,
+		 * the firmware will have run the AP request and made the RDBUFF read
+		 */
+		xds110.use_rdbuff = true;
+		xds110.rdbuff = temp_value;
+	}
+
+	return success;
+}
+
+static bool xds110_legacy_write_reg(uint8_t cmd, uint32_t value)
+{
+	/* Make sure this isn't a read request */
+	bool is_read_request = (0 != (SWD_CMD_RnW & cmd));
+	/* Determine whether this is a DP or AP register access */
+	uint32_t type = (0 != (SWD_CMD_APnDP & cmd)) ? DAP_AP : DAP_DP;
+	/* Determine the AP number from cached SELECT value */
+	uint32_t ap_num = (xds110.select & 0xff000000) >> 24;
+	/* Extract register address from command */
+	uint32_t address = ((cmd & SWD_CMD_A32) >> 1);
+	/* Extract bank address from cached SELECT value */
+	uint32_t bank = (xds110.select & 0x000000f0);
+
+	bool success;
+
+	if (is_read_request)
+		return false;
+
+	/* Invalidate the RDBUFF cache */
+	xds110.use_rdbuff = false;
+
+	if (DAP_AP == type) {
+		/* Add bank address to register address for CMAPI call */
+		address |= bank;
+		/* Any write to an AP register invalidates the firmware's cache */
+		xds110.is_ap_dirty = true;
+	} else if (DAP_DP_SELECT == address) {
+		/* Any write to the SELECT register invalidates the firmware's cache */
+		xds110.is_ap_dirty = true;
+	}
+
+	success = cmapi_write_dap_reg(type, ap_num, address, &value);
+
+	if (!success) {
+		LOG_ERROR("XDS110: failed to write DAP register");
+	} else {
+		/*
+		 * If the debugger wrote to SELECT, cache the value
+		 * to use to build the apNum and address values above
+		 */
+		if ((DAP_DP == type) && (DAP_DP_SELECT == address))
+			xds110.select = value;
+	}
+
+	return success;
+}
+
+static int xds110_swd_run_queue(void)
+{
+	static uint32_t dap_results[MAX_RESULT_QUEUE];
+	uint8_t cmd;
+	uint32_t request;
+	uint32_t result;
+	uint32_t value;
+	bool success = true;
+
+	if (0 == xds110.txn_request_size)
+		return ERROR_OK;
+
+	/* Terminate request queue */
+	xds110.txn_requests[xds110.txn_request_size++] = 0;
+
+	if (xds110.firmware >= OCD_FIRMWARE_VERSION) {
+		/* XDS110 firmware has the API to directly handle the queue */
+		success = ocd_dap_request(xds110.txn_requests,
+			xds110.txn_request_size, dap_results, xds110.txn_result_count);
+	} else {
+		/* Legacy firmware needs to handle queue via discrete DAP calls */
+		request = 0;
+		result = 0;
+		while (xds110.txn_requests[request] != 0) {
+			cmd = xds110.txn_requests[request++];
+			if (0 == (SWD_CMD_RnW & cmd)) {
+				/* DAP register write command */
+				value  = (uint32_t)(xds110.txn_requests[request++]) <<  0;
+				value |= (uint32_t)(xds110.txn_requests[request++]) <<  8;
+				value |= (uint32_t)(xds110.txn_requests[request++]) << 16;
+				value |= (uint32_t)(xds110.txn_requests[request++]) << 24;
+				if (success)
+					success = xds110_legacy_write_reg(cmd, value);
+			} else {
+				/* DAP register read command */
+				value = 0;
+				if (success)
+					success = xds110_legacy_read_reg(cmd, &value);
+				dap_results[result++] = value;
+			}
+		}
+	}
+
+	/* Transfer results into caller's buffers */
+	for (result = 0; result < xds110.txn_result_count; result++)
+		if (0 != xds110.txn_dap_results[result])
+			*xds110.txn_dap_results[result] = dap_results[result];
+
+	xds110.txn_request_size = 0;
+	xds110.txn_result_size = 0;
+	xds110.txn_result_count = 0;
+
+	return (success) ? ERROR_OK : ERROR_FAIL;
+}
+
+static void xds110_swd_queue_cmd(uint8_t cmd, uint32_t *value)
+{
+	/* Check if this is a read or write request */
+	bool is_read_request = (0 != (SWD_CMD_RnW & cmd));
+	/* Determine whether this is a DP or AP register access */
+	uint32_t type = (0 != (SWD_CMD_APnDP & cmd)) ? DAP_AP : DAP_DP;
+	/* Extract register address from command */
+	uint32_t address = ((cmd & SWD_CMD_A32) >> 1);
+	uint32_t request_size = (is_read_request) ? 1 : 5;
+
+	/* Check if new request would be too large to fit */
+	if (((xds110.txn_request_size + request_size + 1) > MAX_DATA_BLOCK) ||
+		((xds110.txn_result_count + 1) > MAX_RESULT_QUEUE))
+		xds110_swd_run_queue();
+
+	/* Set the START bit in cmd to ensure cmd is not zero */
+	/* (a value of zero is used to terminate the buffer) */
+	cmd |= SWD_CMD_START;
+
+	/* Add request to queue; queue is built marshalled for XDS110 call */
+	if (is_read_request) {
+		/* Queue read request, save pointer to pass back result */
+		xds110.txn_requests[xds110.txn_request_size++] = cmd;
+		xds110.txn_dap_results[xds110.txn_result_count++] = value;
+		xds110.txn_result_size += 4;
+	} else {
+		/* Check for and prevent sticky overrun detection */
+		if (DAP_DP == type && DAP_DP_CTRL == address &&
+			(*value & CORUNDETECT)) {
+			LOG_DEBUG("XDS110: refusing to enable sticky overrun detection");
+			*value &= ~CORUNDETECT;
+		}
+		/* Queue write request, add value directly to queue buffer */
+		xds110.txn_requests[xds110.txn_request_size++] = cmd;
+		xds110.txn_requests[xds110.txn_request_size++] = (*value >>  0) & 0xff;
+		xds110.txn_requests[xds110.txn_request_size++] = (*value >>  8) & 0xff;
+		xds110.txn_requests[xds110.txn_request_size++] = (*value >> 16) & 0xff;
+		xds110.txn_requests[xds110.txn_request_size++] = (*value >> 24) & 0xff;
+	}
+}
+
+static void xds110_swd_read_reg(uint8_t cmd, uint32_t *value,
+	uint32_t ap_delay_clk)
+{
+	xds110_swd_queue_cmd(cmd, value);
+}
+static void xds110_swd_write_reg(uint8_t cmd, uint32_t value,
+	uint32_t ap_delay_clk)
+{
+	xds110_swd_queue_cmd(cmd, &value);
+}
+
+/***************************************************************************
+ *   jtag interface                                                        *
+ *                                                                         *
+ *   The following functions provide XDS110 interface to OpenOCD.          *
+ ***************************************************************************/
+
+static void xds110_show_info(void)
+{
+	uint32_t firmware = xds110.firmware;
+
+	LOG_INFO("XDS110: firmware version = %d.%d.%d.%d",
+		(((firmware >> 28) & 0xf) * 10) + ((firmware >> 24) & 0xf),
+		(((firmware >> 20) & 0xf) * 10) + ((firmware >> 16) & 0xf),
+		(((firmware >> 12) & 0xf) * 10) + ((firmware >>  8) & 0xf),
+		(((firmware >>  4) & 0xf) * 10) + ((firmware >>  0) & 0xf));
+	LOG_INFO("XDS110: hardware version = 0x%04x", xds110.hardware);
+	if (0 != xds110.serial[0])
+		LOG_INFO("XDS110: serial number = %s)", xds110.serial);
+	if (xds110.is_swd_mode) {
+		LOG_INFO("XDS110: connected to target via SWD");
+		LOG_INFO("XDS110: SWCLK set to %d kHz", xds110.speed);
+	} else {
+		LOG_INFO("XDS110: connected to target via JTAG");
+		LOG_INFO("XDS110: TCK set to %d kHz", xds110.speed);
+	}
+
+	/* Alert user that there's a better firmware to use */
+	if (firmware < OCD_FIRMWARE_VERSION) {
+		LOG_WARNING("XDS110: the firmware is not optimized for OpenOCD");
+		LOG_WARNING(OCD_FIRMWARE_UPGRADE);
+	}
+}
+
+static int xds110_quit(void)
+{
+	if (xds110.is_cmapi_acquired) {
+		(void)cmapi_release();
+		xds110.is_cmapi_acquired = false;
+	}
+	if (xds110.is_cmapi_connected) {
+		(void)cmapi_disconnect();
+		xds110.is_cmapi_connected = false;
+	}
+	if (xds110.is_connected) {
+		if (xds110.is_swd_mode) {
+			/* Switch out of SWD mode */
+			(void)swd_disconnect();
+		} else {
+			/* Switch out of cJTAG mode */
+			(void)cjtag_disconnect();
+		}
+		/* Tell firmware we're disconnecting */
+		(void)xds_disconnect();
+		xds110.is_connected = false;
+	}
+	/* Close down the USB connection to the XDS110 debug probe */
+	usb_disconnect();
+
+	return ERROR_OK;
+}
+
+static int xds110_init(void)
+{
+	bool success;
+
+	/* Establish USB connection to the XDS110 debug probe */
+	success = usb_connect();
+
+	if (success) {
+		/* Send connect message to XDS110 firmware */
+		success = xds_connect();
+		if (success)
+			xds110.is_connected = true;
+	}
+
+	if (success) {
+		uint32_t firmware;
+		uint16_t hardware;
+
+		/* Retrieve version IDs from firmware */
+		/* Version numbers are stored in BCD format */
+		success = xds_version(&firmware, &hardware);
+		if (success) {
+			/* Save the firmware and hardware version */
+			xds110.firmware = firmware;
+			xds110.hardware = hardware;
+		}
+	}
+
+	if (success) {
+		success = xds_set_trst(0);
+		if (success)
+			success = xds_cycle_tck(50);
+		if (success)
+			success = xds_set_trst(1);
+		if (success)
+			success = xds_cycle_tck(50);
+	}
+
+	if (success) {
+		if (xds110.is_swd_mode) {
+			/* Switch to SWD if needed */
+			success = swd_connect();
+		} else {
+			success = cjtag_connect(MODE_JTAG);
+		}
+	}
+
+	if (success && xds110.is_swd_mode) {
+		uint32_t idcode;
+
+		/* Connect to CMAPI interface in XDS110 */
+		success = cmapi_connect(&idcode);
+
+		/* Acquire exclusive access to CMAPI interface */
+		if (success) {
+			xds110.is_cmapi_connected = true;
+			success = cmapi_acquire();
+			if (success)
+				xds110.is_cmapi_acquired = true;
+		}
+	}
+
+	if (!success)
+		xds110_quit();
+
+	if (success)
+		xds110_show_info();
+
+	return (success) ? ERROR_OK : ERROR_FAIL;
+}
+
+static void xds110_legacy_scan(uint32_t shift_state, uint32_t total_bits,
+	uint32_t end_state, uint8_t *data_out, uint8_t *data_in)
+{
+	(void)xds_jtag_scan(shift_state, total_bits, end_state, data_out, data_in);
+}
+
+static void xds110_legacy_runtest(uint32_t clocks, uint32_t end_state)
+{
+	xds_goto_state(XDS_JTAG_STATE_IDLE);
+	xds_cycle_tck(clocks);
+	xds_goto_state(end_state);
+}
+
+static void xds110_legacy_stableclocks(uint32_t clocks)
+{
+	xds_cycle_tck(clocks);
+}
+
+static void xds110_flush(void)
+{
+	uint8_t command;
+	uint32_t clocks;
+	uint32_t shift_state;
+	uint32_t end_state;
+	uint32_t bits;
+	uint32_t bytes;
+	uint32_t request;
+	uint32_t result;
+	uint8_t *data_out;
+	uint8_t data_in[MAX_DATA_BLOCK];
+	uint8_t *data_pntr;
+
+	if (0 == xds110.txn_request_size)
+		return;
+
+	/* Terminate request queue */
+	xds110.txn_requests[xds110.txn_request_size++] = 0;
+
+	if (xds110.firmware >= OCD_FIRMWARE_VERSION) {
+		/* Updated firmware has the API to directly handle the queue */
+		(void)ocd_scan_request(xds110.txn_requests, xds110.txn_request_size,
+			data_in, xds110.txn_result_size);
+	} else {
+		/* Legacy firmware needs to handle queue via discrete JTAG calls */
+		request = 0;
+		result = 0;
+		while (xds110.txn_requests[request] != 0) {
+			command = xds110.txn_requests[request++];
+			switch (command) {
+				case CMD_IR_SCAN:
+				case CMD_DR_SCAN:
+					if (command == CMD_IR_SCAN)
+						shift_state = XDS_JTAG_STATE_SHIFT_IR;
+					else
+						shift_state = XDS_JTAG_STATE_SHIFT_DR;
+					end_state = (uint32_t)(xds110.txn_requests[request++]);
+					bits  = (uint32_t)(xds110.txn_requests[request++]) << 0;
+					bits |= (uint32_t)(xds110.txn_requests[request++]) << 8;
+					data_out = &xds110.txn_requests[request];
+					bytes = DIV_ROUND_UP(bits, 8);
+					xds110_legacy_scan(shift_state, bits, end_state, data_out,
+						&data_in[result]);
+					result += bytes;
+					request += bytes;
+					break;
+				case CMD_RUNTEST:
+					clocks  = (uint32_t)(xds110.txn_requests[request++]) <<  0;
+					clocks |= (uint32_t)(xds110.txn_requests[request++]) <<  8;
+					clocks |= (uint32_t)(xds110.txn_requests[request++]) << 16;
+					clocks |= (uint32_t)(xds110.txn_requests[request++]) << 24;
+					end_state = (uint32_t)xds110.txn_requests[request++];
+					xds110_legacy_runtest(clocks, end_state);
+					break;
+				case CMD_STABLECLOCKS:
+					clocks  = (uint32_t)(xds110.txn_requests[request++]) <<  0;
+					clocks |= (uint32_t)(xds110.txn_requests[request++]) <<  8;
+					clocks |= (uint32_t)(xds110.txn_requests[request++]) << 16;
+					clocks |= (uint32_t)(xds110.txn_requests[request++]) << 24;
+					xds110_legacy_stableclocks(clocks);
+					break;
+				default:
+					LOG_ERROR("BUG: unknown JTAG command type 0x%x encountered",
+						command);
+					exit(-1);
+					break;
+			}
+		}
+	}
+
+	/* Transfer results into caller's buffers from data_in buffer */
+	bits = 0; /* Bit offset into current scan result */
+	data_pntr = data_in;
+	for (result = 0; result < xds110.txn_result_count; result++) {
+		if (xds110.txn_scan_results[result].first) {
+			if (bits != 0) {
+				bytes = DIV_ROUND_UP(bits, 8);
+				data_pntr += bytes;
+			}
+			bits = 0;
+		}
+		if (xds110.txn_scan_results[result].buffer != 0)
+			bit_copy(xds110.txn_scan_results[result].buffer, 0, data_pntr,
+				bits, xds110.txn_scan_results[result].num_bits);
+		bits += xds110.txn_scan_results[result].num_bits;
+	}
+
+	xds110.txn_request_size = 0;
+	xds110.txn_result_size = 0;
+	xds110.txn_result_count = 0;
+}
+
+static void xds110_execute_reset(struct jtag_command *cmd)
+{
+	char trst;
+	char srst;
+
+	if (cmd->cmd.reset->trst != -1) {
+		if (cmd->cmd.reset->trst == 0) {
+			/* Deassert nTRST (active low) */
+			trst = 1;
+		} else {
+			/* Assert nTRST (active low) */
+			trst = 0;
+		}
+		(void)xds_set_trst(trst);
+	}
+
+	if (cmd->cmd.reset->srst != -1) {
+		if (cmd->cmd.reset->srst == 0) {
+			/* Deassert nSRST (active low) */
+			srst = 1;
+		} else {
+			/* Assert nSRST (active low) */
+			srst = 0;
+		}
+		(void)xds_set_srst(srst);
+	}
+}
+
+static void xds110_execute_sleep(struct jtag_command *cmd)
+{
+	jtag_sleep(cmd->cmd.sleep->us);
+	return;
+}
+
+static void xds110_execute_tlr_reset(struct jtag_command *cmd)
+{
+	(void)xds_goto_state(XDS_JTAG_STATE_RESET);
+
+	return;
+}
+
+static void xds110_execute_pathmove(struct jtag_command *cmd)
+{
+	uint32_t i;
+	uint32_t num_states;
+	uint8_t *path;
+
+	num_states = (uint32_t)cmd->cmd.pathmove->num_states;
+
+	if (num_states == 0)
+		return;
+
+	path = (uint8_t *)malloc(num_states * sizeof(uint8_t));
+	if (path == 0) {
+		LOG_ERROR("XDS110: unable to allocate memory");
+		return;
+	}
+
+	/* Convert requested path states into XDS API states */
+	for (i = 0; i < num_states; i++)
+		path[i] = (uint8_t)xds_jtag_state[cmd->cmd.pathmove->path[i]];
+
+	if (xds110.firmware >= OCD_FIRMWARE_VERSION) {
+		/* Updated firmware fully supports pathmove */
+		(void)ocd_pathmove(num_states, path);
+	} else {
+		/* Notify user that legacy firmware simply cannot handle pathmove */
+		LOG_ERROR("XDS110: the firmware does not support pathmove command");
+		LOG_ERROR(OCD_FIRMWARE_UPGRADE);
+		/* If pathmove is required, then debug is not possible */
+		exit(-1);
+	}
+
+	free((void *)path);
+
+	return;
+}
+
+static void xds110_queue_scan(struct jtag_command *cmd)
+{
+	int i;
+	uint32_t offset;
+	uint32_t total_fields;
+	uint32_t total_bits;
+	uint32_t total_bytes;
+	uint8_t end_state;
+	uint8_t *buffer;
+
+	/* Calculate the total number of bits to scan */
+	total_bits = 0;
+	total_fields = 0;
+	for (i = 0; i < cmd->cmd.scan->num_fields; i++) {
+		total_fields++;
+		total_bits += (uint32_t)cmd->cmd.scan->fields[i].num_bits;
+	}
+
+	if (total_bits == 0)
+		return;
+
+	total_bytes = DIV_ROUND_UP(total_bits, 8);
+
+	/* Check if new request would be too large to fit */
+	if (((xds110.txn_request_size + 1 + total_bytes + sizeof(end_state) + 1)
+		> MAX_DATA_BLOCK) || ((xds110.txn_result_count + total_fields) >
+		MAX_RESULT_QUEUE))
+		xds110_flush();
+
+	/* Check if this single request is too large to fit */
+	if ((1 + total_bytes + sizeof(end_state) + 1) > MAX_DATA_BLOCK) {
+		LOG_ERROR("BUG: JTAG scan request is too large to handle (%d bits)",
+			total_bits);
+		/* Failing to run this scan mucks up debug on this target */
+		exit(-1);
+	}
+
+	if (cmd->cmd.scan->ir_scan)
+		xds110.txn_requests[xds110.txn_request_size++] = CMD_IR_SCAN;
+	else
+		xds110.txn_requests[xds110.txn_request_size++] = CMD_DR_SCAN;
+
+	end_state = (uint8_t)xds_jtag_state[cmd->cmd.scan->end_state];
+	xds110.txn_requests[xds110.txn_request_size++] = end_state;
+
+	xds110.txn_requests[xds110.txn_request_size++] = (total_bits >> 0) & 0xff;
+	xds110.txn_requests[xds110.txn_request_size++] = (total_bits >> 8) & 0xff;
+
+	/* Build request data by flattening fields into single buffer */
+	/* also populate the results array to return the results when run */
+	offset = 0;
+	buffer = &xds110.txn_requests[xds110.txn_request_size];
+	/* Clear data out buffer to default value of all zeros */
+	memset((void *)buffer, 0x00, total_bytes);
+	for (i = 0; i < cmd->cmd.scan->num_fields; i++) {
+		if (cmd->cmd.scan->fields[i].out_value != 0) {
+			/* Copy over data to scan out into request buffer */
+			bit_copy(buffer, offset, cmd->cmd.scan->fields[i].out_value, 0,
+				cmd->cmd.scan->fields[i].num_bits);
+		}
+		offset += cmd->cmd.scan->fields[i].num_bits;
+		xds110.txn_scan_results[xds110.txn_result_count].first = (i == 0);
+		xds110.txn_scan_results[xds110.txn_result_count].num_bits =
+			cmd->cmd.scan->fields[i].num_bits;
+		xds110.txn_scan_results[xds110.txn_result_count++].buffer =
+			cmd->cmd.scan->fields[i].in_value;
+	}
+	xds110.txn_request_size += total_bytes;
+	xds110.txn_result_size += total_bytes;
+
+	return;
+}
+
+static void xds110_queue_runtest(struct jtag_command *cmd)
+{
+	uint32_t clocks = (uint32_t)cmd->cmd.stableclocks->num_cycles;
+	uint8_t end_state = (uint8_t)xds_jtag_state[cmd->cmd.runtest->end_state];
+
+	/* Check if new request would be too large to fit */
+	if ((xds110.txn_request_size + 1 + sizeof(clocks) + sizeof(end_state) + 1)
+		> MAX_DATA_BLOCK)
+		xds110_flush();
+
+	/* Queue request and cycle count directly to queue buffer */
+	xds110.txn_requests[xds110.txn_request_size++] = CMD_RUNTEST;
+	xds110.txn_requests[xds110.txn_request_size++] = (clocks >>  0) & 0xff;
+	xds110.txn_requests[xds110.txn_request_size++] = (clocks >>  8) & 0xff;
+	xds110.txn_requests[xds110.txn_request_size++] = (clocks >> 16) & 0xff;
+	xds110.txn_requests[xds110.txn_request_size++] = (clocks >> 24) & 0xff;
+	xds110.txn_requests[xds110.txn_request_size++] = end_state;
+
+	return;
+}
+
+static void xds110_queue_stableclocks(struct jtag_command *cmd)
+{
+	uint32_t clocks = (uint32_t)cmd->cmd.stableclocks->num_cycles;
+
+	/* Check if new request would be too large to fit */
+	if ((xds110.txn_request_size + 1 + sizeof(clocks) + 1) > MAX_DATA_BLOCK)
+		xds110_flush();
+
+	/* Queue request and cycle count directly to queue buffer */
+	xds110.txn_requests[xds110.txn_request_size++] = CMD_STABLECLOCKS;
+	xds110.txn_requests[xds110.txn_request_size++] = (clocks >>  0) & 0xff;
+	xds110.txn_requests[xds110.txn_request_size++] = (clocks >>  8) & 0xff;
+	xds110.txn_requests[xds110.txn_request_size++] = (clocks >> 16) & 0xff;
+	xds110.txn_requests[xds110.txn_request_size++] = (clocks >> 24) & 0xff;
+
+	return;
+}
+
+static void xds110_execute_command(struct jtag_command *cmd)
+{
+	switch (cmd->type) {
+		case JTAG_RESET:
+			xds110_flush();
+			xds110_execute_reset(cmd);
+			break;
+		case JTAG_SLEEP:
+			xds110_flush();
+			xds110_execute_sleep(cmd);
+			break;
+		case JTAG_TLR_RESET:
+			xds110_flush();
+			xds110_execute_tlr_reset(cmd);
+			break;
+		case JTAG_PATHMOVE:
+			xds110_flush();
+			xds110_execute_pathmove(cmd);
+			break;
+		case JTAG_SCAN:
+			xds110_queue_scan(cmd);
+			break;
+		case JTAG_RUNTEST:
+			xds110_queue_runtest(cmd);
+			break;
+		case JTAG_STABLECLOCKS:
+			xds110_queue_stableclocks(cmd);
+			break;
+		case JTAG_TMS:
+		default:
+			LOG_ERROR("BUG: unknown JTAG command type 0x%x encountered",
+				cmd->type);
+			exit(-1);
+	}
+}
+
+static int xds110_execute_queue(void)
+{
+	struct jtag_command *cmd = jtag_command_queue;
+
+	while (cmd != NULL) {
+		xds110_execute_command(cmd);
+		cmd = cmd->next;
+	}
+
+	xds110_flush();
+
+	return ERROR_OK;
+}
+
+static int xds110_speed(int speed)
+{
+	bool success;
+
+	if (speed == 0) {
+		LOG_INFO("XDS110: RTCK not supported");
+		return ERROR_JTAG_NOT_IMPLEMENTED;
+	}
+
+	if (speed > XDS110_MAX_TCK_SPEED) {
+		LOG_INFO("XDS110: reduce speed request: %dkHz to %dkHz maximum",
+			speed, XDS110_MAX_TCK_SPEED);
+		speed = XDS110_MAX_TCK_SPEED;
+	}
+
+	if (speed < XDS110_MIN_TCK_SPEED) {
+		LOG_INFO("XDS110: increase speed request: %dkHz to %dkHz minimum",
+			speed, XDS110_MIN_TCK_SPEED);
+		speed = XDS110_MIN_TCK_SPEED;
+	}
+
+	/* The default is the maximum frequency the XDS110 can support */
+	uint32_t freq_to_use = XDS110_MAX_TCK_SPEED * 1000; /* Hz */
+	uint32_t delay_count = 0;
+
+	if (XDS110_MAX_TCK_SPEED != speed) {
+		freq_to_use = speed * 1000; /* Hz */
+
+		/* Calculate the delay count value */
+		double one_giga = 1000000000;
+		/* Get the pulse duration for the maximum frequency supported in ns */
+		double max_freq_pulse_duration = one_giga /
+			(XDS110_MAX_TCK_SPEED * 1000);
+
+		/* Convert frequency to pulse duration */
+		double freq_to_pulse_width_in_ns = one_giga / freq_to_use;
+
+		/*
+		 * Start with the pulse duration for the maximum frequency. Keep
+		 * decrementing the time added by each count value till the requested
+		 * frequency pulse is less than the calculated value.
+		 */
+		double current_value = max_freq_pulse_duration;
+
+		while (current_value < freq_to_pulse_width_in_ns) {
+			current_value += XDS110_TCK_PULSE_INCREMENT;
+			++delay_count;
+		}
+
+		/*
+		 * Determine which delay count yields the best match.
+		 * The one obtained above or one less.
+		 */
+		if (delay_count) {
+			double diff_freq_1 = freq_to_use -
+				(one_giga / (max_freq_pulse_duration +
+				(XDS110_TCK_PULSE_INCREMENT * delay_count)));
+			double diff_freq_2 = (one_giga / (max_freq_pulse_duration +
+				(XDS110_TCK_PULSE_INCREMENT * (delay_count - 1)))) -
+				freq_to_use;
+
+			/* One less count value yields a better match */
+			if (diff_freq_1 > diff_freq_2)
+				--delay_count;
+		}
+	}
+
+	/* Send the delay count to the XDS110 firmware */
+	success = xds_set_tck_delay(delay_count);
+
+	if (success) {
+		xds110.delay_count = delay_count;
+		xds110.speed = speed;
+	}
+
+	return (success) ? ERROR_OK : ERROR_FAIL;
+}
+
+static int xds110_speed_div(int speed, int *khz)
+{
+	*khz = speed;
+	return ERROR_OK;
+}
+
+static int xds110_khz(int khz, int *jtag_speed)
+{
+	*jtag_speed = khz;
+	return ERROR_OK;
+}
+
+static int_least32_t xds110_swd_frequency(int_least32_t hz)
+{
+	if (hz > 0)
+		xds110_speed(hz / 1000);
+	return hz;
+}
+
+COMMAND_HANDLER(xds110_handle_info_command)
+{
+	xds110_show_info();
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(xds110_handle_serial_command)
+{
+	wchar_t serial[XDS110_SERIAL_LEN + 1];
+
+	xds110.serial[0] = 0;
+
+	if (CMD_ARGC == 1) {
+		size_t len = mbstowcs(0, CMD_ARGV[0], 0);
+		if (len > XDS110_SERIAL_LEN) {
+			LOG_ERROR("XDS110: serial number is limited to %d characters",
+				XDS110_SERIAL_LEN);
+			return ERROR_FAIL;
+		}
+		if ((size_t)-1 == mbstowcs(serial, CMD_ARGV[0], len + 1)) {
+			LOG_ERROR("XDS110: unable to convert serial number");
+			return ERROR_FAIL;
+		}
+
+		for (uint32_t i = 0; i < len; i++)
+			xds110.serial[i] = (char)serial[i];
+
+		xds110.serial[len] = 0;
+	} else {
+		LOG_ERROR("XDS110: expected exactly one argument to xds110_serial "
+			"<serial-number>");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static const struct command_registration xds110_subcommand_handlers[] = {
+	{
+		.name = "info",
+		.handler = &xds110_handle_info_command,
+		.mode = COMMAND_EXEC,
+		.usage = "",
+		.help = "show XDS110 info",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration xds110_command_handlers[] = {
+	{
+		.name = "xds110",
+		.mode = COMMAND_ANY,
+		.help = "perform XDS110 management",
+		.usage = "<cmd>",
+		.chain = xds110_subcommand_handlers,
+	},
+	{
+		.name = "xds110_serial",
+		.handler = &xds110_handle_serial_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the XDS110 probe serial number",
+		.usage = "serial_string",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct swd_driver xds110_swd_driver = {
+	.init = xds110_swd_init,
+	.frequency = xds110_swd_frequency,
+	.switch_seq = xds110_swd_switch_seq,
+	.read_reg = xds110_swd_read_reg,
+	.write_reg = xds110_swd_write_reg,
+	.run = xds110_swd_run_queue,
+};
+
+static const char * const xds110_transport[] = { "swd", "jtag", NULL };
+
+struct jtag_interface xds110_interface = {
+	.name = "xds110",
+	.commands = xds110_command_handlers,
+	.swd = &xds110_swd_driver,
+	.transports = xds110_transport,
+
+	.execute_queue = xds110_execute_queue,
+	.speed = xds110_speed,
+	.speed_div = xds110_speed_div,
+	.khz = xds110_khz,
+	.init = xds110_init,
+	.quit = xds110_quit,
+};
diff --git a/src/jtag/interfaces.c b/src/jtag/interfaces.c
index ddeadb4..286a73a 100644
--- a/src/jtag/interfaces.c
+++ b/src/jtag/interfaces.c
@@ -132,6 +132,9 @@ extern struct jtag_interface kitprog_interface;
 #if BUILD_IMX_GPIO == 1
 extern struct jtag_interface imx_gpio_interface;
 #endif
+#if BUILD_XDS110 == 1
+extern struct jtag_interface xds110_interface;
+#endif
 #endif /* standard drivers */
 
 /**
@@ -234,6 +237,9 @@ struct jtag_interface *jtag_interfaces[] = {
 #if BUILD_IMX_GPIO == 1
 		&imx_gpio_interface,
 #endif
+#if BUILD_XDS110 == 1
+		&xds110_interface,
+#endif
 #endif /* standard drivers */
 		NULL,
 	};
diff --git a/tcl/interface/xds110.cfg b/tcl/interface/xds110.cfg
new file mode 100644
index 0000000..495e202
--- /dev/null
+++ b/tcl/interface/xds110.cfg
@@ -0,0 +1,12 @@
+#
+# Texas Instruments XDS110
+#
+# http://processors.wiki.ti.com/index.php/XDS110
+# http://processors.wiki.ti.com/index.php/Emulation_Software_Package#XDS110_Support_Utilities
+#
+
+interface xds110
+
+# Use serial number option to use a specific XDS110
+# when more than one are connected to the host.
+#xds110_serial 00000000