cmsis-dap: add initial cmsis-dap support

This is based on work from:
https://github.com/TheShed/OpenOCD-CMSIS-DAP/tree/cmsis-dap

Main changes include moving over to using HIDAPI rather than libusb-1.0
and cleaning up to merge into master. Support for reset using srst has
also been added.

It has been tested on all the mbed boards as well as the Freedom board
from Freescale. These boards only implement SWD mode, however JTAG mode
has been tested with a Keil ULINK2 and a stm32 target - but requires a lot
more work.

Change-Id: I96d5ee1993bc9c0526219ab754c5aad3b55d812d
Signed-off-by: Spencer Oliver <spen@spen-soft.co.uk>
Signed-off-by: Andrey Yurovsky <yurovsky@gmail.com>
Reviewed-on: http://openocd.zylin.com/1542
Tested-by: jenkins

11 files changed, 1495 insertions, 19 deletions

diff --git a/src/Makefile.am b/src/Makefile.am
index 48af21d4..5e9c8509 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -75,7 +75,7 @@ libopenocd_la_LIBADD = \
 	$(top_builddir)/src/rtos/librtos.la \
 	$(top_builddir)/src/helper/libhelper.la \
 	$(LIBFTDI_LIBS) $(MINGWLDADD) \
-	$(LIBUSB1_LIBS) $(LIBUSB0_LIBS)
+	$(HIDAPI_LIBS) $(LIBUSB1_LIBS) $(LIBUSB0_LIBS)
 
 STARTUP_TCL_SRCS = \
 	$(srcdir)/helper/startup.tcl \
@@ -103,7 +103,7 @@ CLEANFILES = startup.tcl startup_tcl.c
 # we do not want generated file in the dist
 dist-hook:
 	rm -f $(distdir)/startup_tcl.c
-	
+
 MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
 
 # The "quick" target builds executables & reinstalls the executables
diff --git a/src/jtag/core.c b/src/jtag/core.c
index 08217352..24f6a511 100644
--- a/src/jtag/core.c
+++ b/src/jtag/core.c
@@ -134,8 +134,6 @@ static int jtag_speed;
 
 static struct jtag_interface *jtag;
 
-const struct swd_driver *swd;
-
 /* configuration */
 struct jtag_interface *jtag_interface;
 
@@ -1824,6 +1822,8 @@ void adapter_assert_reset(void)
 			jtag_add_reset(0, 1);
 	} else if (transport_is_swd())
 		swd_add_reset(1);
+	else if (transport_is_cmsis_dap())
+		swd_add_reset(1);  /* FIXME */
 	else if (get_current_transport() != NULL)
 		LOG_ERROR("reset is not supported on %s",
 			get_current_transport()->name);
@@ -1837,6 +1837,8 @@ void adapter_deassert_reset(void)
 		jtag_add_reset(0, 0);
 	else if (transport_is_swd())
 		swd_add_reset(0);
+	else if (transport_is_cmsis_dap())
+		swd_add_reset(0);  /* FIXME */
 	else if (get_current_transport() != NULL)
 		LOG_ERROR("reset is not supported on %s",
 			get_current_transport()->name);
diff --git a/src/jtag/drivers/Makefile.am b/src/jtag/drivers/Makefile.am
index 00f0165e..72fbb347 100644
--- a/src/jtag/drivers/Makefile.am
+++ b/src/jtag/drivers/Makefile.am
@@ -6,7 +6,8 @@ libocdjtagdrivers_la_LIBADD =
 libocdjtagdrivers_la_SOURCES = \
 	$(DRIVERFILES)
 
-libocdjtagdrivers_la_CPPFLAGS = $(AM_CPPFLAGS) $(LIBUSB1_CFLAGS) $(LIBUSB0_CFLAGS)
+libocdjtagdrivers_la_CPPFLAGS = $(AM_CPPFLAGS) $(LIBUSB1_CFLAGS) \
+	$(LIBUSB0_CFLAGS) $(HIDAPI_CFLAGS)
 
 ULINK_FIRMWARE = $(srcdir)/OpenULINK
 
@@ -122,6 +123,10 @@ if OPENJTAG
 DRIVERFILES += openjtag.c
 endif
 
+if CMSIS_DAP
+DRIVERFILES += cmsis_dap_usb.c
+endif
+
 noinst_HEADERS = \
 	bitbang.h \
 	bitq.h \
diff --git a/src/jtag/drivers/cmsis_dap_usb.c b/src/jtag/drivers/cmsis_dap_usb.c
new file mode 100644
index 00000000..5cd117fc
--- /dev/null
+++ b/src/jtag/drivers/cmsis_dap_usb.c
@@ -0,0 +1,1091 @@
+/***************************************************************************
+ *   Copyright (C) 2013 by mike brown                                      *
+ *   mike@theshedworks.org.uk                                              *
+ *                                                                         *
+ *   Copyright (C) 2013 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
+ ***************************************************************************/
+
+#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 <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.
+ */
+
+/* USB Config */
+
+/* Known vid/pid pairs:
+ * VID 0xc251: Keil Software
+ * PID 0xf001: LPC-Link-II CMSIS_DAP
+ * PID 0xf002: OPEN-SDA CMSIS_DAP (Freedom Board)
+ * PID 0x2722: Keil ULINK2 CMSIS-DAP
+ *
+ * VID 0x0d28: mbed Software
+ * PID 0x0204: MBED CMSIS-DAP
+ */
+
+#define MAX_USB_IDS 8
+/* vid = pid = 0 marks the end of the list */
+static uint16_t cmsis_dap_vid[MAX_USB_IDS + 1] = { 0xc251, 0xc251, 0xc251, 0x0d28, 0x03eb, 0 };
+static uint16_t cmsis_dap_pid[MAX_USB_IDS + 1] = { 0xf001, 0xf002, 0x2722, 0x0204, 0x2111, 0 };
+
+#define PACKET_SIZE       (64 + 1)	/* 64 bytes plus report id */
+#define USB_TIMEOUT       1000
+
+/* CMSIS-DAP General Commands */
+#define CMD_DAP_INFO              0x00
+#define CMD_DAP_LED               0x01
+#define CMD_DAP_CONNECT           0x02
+#define CMD_DAP_DISCONNECT        0x03
+#define CMD_DAP_WRITE_ABORT       0x08
+#define CMD_DAP_DELAY             0x09
+#define CMD_DAP_RESET_TARGET      0x0A
+
+/* CMD_INFO */
+#define INFO_ID_VID               0x00      /* string */
+#define INFO_ID_PID               0x02      /* string */
+#define INFO_ID_SERNUM            0x03      /* string */
+#define INFO_ID_FW_VER            0x04      /* string */
+#define INFO_ID_TD_VEND           0x05      /* string */
+#define INFO_ID_TD_NAME           0x06      /* string */
+#define INFO_ID_CAPS              0xf0      /* byte */
+#define INFO_ID_PKT_CNT           0xfe      /* byte */
+#define INFO_ID_PKT_SZ            0xff      /* short */
+
+#define INFO_CAPS_SWD             0x01
+#define INFO_CAPS_JTAG            0x02
+
+/* CMD_LED */
+#define LED_ID_CONNECT            0x00
+#define LED_ID_RUN                0x01
+
+#define LED_OFF                   0x00
+#define LED_ON                    0x01
+
+/* CMD_CONNECT */
+#define CONNECT_DEFAULT           0x00
+#define CONNECT_SWD               0x01
+#define CONNECT_JTAG              0x02
+
+/* CMSIS-DAP Common SWD/JTAG Commands */
+#define CMD_DAP_DELAY             0x09
+#define CMD_DAP_SWJ_PINS          0x10
+#define CMD_DAP_SWJ_CLOCK         0x11
+#define CMD_DAP_SWJ_SEQ           0x12
+
+/*
+ * PINS
+ * Bit 0: SWCLK/TCK
+ * Bit 1: SWDIO/TMS
+ * Bit 2: TDI
+ * Bit 3: TDO
+ * Bit 5: nTRST
+ * Bit 7: nRESET
+ */
+
+/* CMSIS-DAP SWD Commands */
+#define CMD_DAP_SWD_CONFIGURE     0x13
+
+/* CMSIS-DAP JTAG Commands */
+#define CMD_DAP_JTAG_SEQ          0x14
+#define CMD_DAP_JTAG_CONFIGURE    0x15
+#define CMD_DAP_JTAG_IDCODE       0x16
+
+/* CMSIS-DAP Transfer Commands */
+#define CMD_DAP_TFER_CONFIGURE    0x04
+#define CMD_DAP_TFER              0x05
+#define CMD_DAP_TFER_BLOCK        0x06
+#define CMD_DAP_TFER_ABORT        0x07
+
+/* DAP Status Code */
+#define DAP_OK                    0
+#define DAP_ERROR                 0xFF
+
+/* CMSIS-DAP Vendor Commands
+ * None as yet... */
+
+static char *info_caps_str[] = {
+	"SWD  Supported",
+	"JTAG Supported"
+};
+
+/* max clock speed (kHz) */
+#define DAP_MAX_CLOCK             5000
+
+struct cmsis_dap {
+	hid_device *dev_handle;
+	uint16_t packet_size;
+	uint16_t packet_count;
+	uint8_t *packet_buffer;
+	uint8_t caps;
+	uint8_t mode;
+};
+
+static struct cmsis_dap *cmsis_dap_handle;
+
+static int cmsis_dap_usb_open(void)
+{
+	hid_device *dev = NULL;
+	int i;
+
+	if (hid_init() != 0) {
+		LOG_ERROR("unable to open HIDAPI");
+		return ERROR_FAIL;
+	}
+
+	for (i = 0; cmsis_dap_vid[i] || cmsis_dap_pid[i]; i++) {
+		dev = hid_open(cmsis_dap_vid[i], cmsis_dap_pid[i], NULL);
+		if (dev != NULL)
+			break;
+	}
+
+	if (dev == NULL) {
+		LOG_ERROR("unable to open CMSIS-DAP device");
+		return ERROR_FAIL;
+	}
+
+	struct cmsis_dap *dap = malloc(sizeof(struct cmsis_dap));
+	if (dap == NULL) {
+		LOG_ERROR("unable to allocate memory");
+		return ERROR_FAIL;
+	}
+
+	dap->dev_handle = dev;
+	dap->caps = 0;
+	dap->mode = 0;
+
+	cmsis_dap_handle = dap;
+
+	/* allocate default packet buffer, may be changed later.
+	 * currently with HIDAPI we have no way of getting the output report length
+	 * without this info we cannot communicate with the adapter.
+	 * For the moment we ahve to hard code the packet size */
+
+	int packet_size = PACKET_SIZE;
+
+	/* atmel cmsis-dap uses 512 byte reports */
+	if (cmsis_dap_vid[i] == 0x03eb && cmsis_dap_pid[i] == 0x2111)
+		packet_size = 512 + 1;
+
+	cmsis_dap_handle->packet_buffer = malloc(packet_size);
+	cmsis_dap_handle->packet_size = packet_size;
+
+	if (cmsis_dap_handle->packet_buffer == NULL) {
+		LOG_ERROR("unable to allocate memory");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+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;
+	}
+
+	return;
+}
+
+/* Send a message and receive the reply */
+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);
+
+	/* write data to device */
+	int retval = hid_write(dap->dev_handle, dap->packet_buffer, dap->packet_size);
+	if (retval == -1) {
+		LOG_ERROR("error writing data: %ls", hid_error(dap->dev_handle));
+		return ERROR_FAIL;
+	}
+
+	/* get reply */
+	retval = hid_read_timeout(dap->dev_handle, dap->packet_buffer, dap->packet_size, USB_TIMEOUT);
+	if (retval == -1 || retval == 0) {
+		LOG_DEBUG("error reading data: %ls", hid_error(dap->dev_handle));
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int cmsis_dap_cmd_DAP_SWJ_Pins(uint8_t pins, uint8_t mask, uint32_t wait, uint8_t *input)
+{
+	int retval;
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	buffer[0] = 0;	/* report number */
+	buffer[1] = CMD_DAP_SWJ_PINS;
+	buffer[2] = pins;
+	buffer[3] = mask;
+	buffer[4] = wait & 0xff;
+	buffer[5] = (wait >> 8) & 0xff;
+	buffer[6] = (wait >> 16) & 0xff;
+	buffer[7] = (wait >> 24) & 0xff;
+	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 8);
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("CMSIS-DAP command CMD_DAP_SWJ_PINS failed.");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	if (input)
+		*input = buffer[1];
+
+	return ERROR_OK;
+}
+
+static int cmsis_dap_cmd_DAP_SWJ_Clock(uint32_t swj_clock)
+{
+	int retval;
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	/* set clock in Hz */
+	swj_clock *= 1000;
+	buffer[0] = 0;	/* report number */
+	buffer[1] = CMD_DAP_SWJ_CLOCK;
+	buffer[2] = swj_clock & 0xff;
+	buffer[3] = (swj_clock >> 8) & 0xff;
+	buffer[4] = (swj_clock >> 16) & 0xff;
+	buffer[5] = (swj_clock >> 24) & 0xff;
+	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 6);
+
+	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
+		LOG_ERROR("CMSIS-DAP command CMD_DAP_SWJ_CLOCK failed.");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	return ERROR_OK;
+}
+
+static int cmsis_dap_cmd_DAP_Info(uint8_t info, uint8_t **data)
+{
+	int retval;
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	buffer[0] = 0;	/* report number */
+	buffer[1] = CMD_DAP_INFO;
+	buffer[2] = info;
+	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 3);
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("CMSIS-DAP command CMD_INFO failed.");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	*data = &(buffer[1]);
+
+	return ERROR_OK;
+}
+
+static int cmsis_dap_cmd_DAP_LED(uint8_t leds)
+{
+	int retval;
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	buffer[0] = 0;	/* report number */
+	buffer[1] = CMD_DAP_LED;
+	buffer[2] = 0x00;
+	buffer[3] = leds;
+	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 4);
+
+	if (retval != ERROR_OK || buffer[1] != 0x00) {
+		LOG_ERROR("CMSIS-DAP command CMD_LED failed.");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	return ERROR_OK;
+}
+
+static int cmsis_dap_cmd_DAP_Connect(uint8_t mode)
+{
+	int retval;
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	buffer[0] = 0;	/* report number */
+	buffer[1] = CMD_DAP_CONNECT;
+	buffer[2] = mode;
+	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 3);
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("CMSIS-DAP command CMD_CONNECT failed.");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	if (buffer[1] != mode) {
+		LOG_ERROR("CMSIS-DAP failed to connect in mode (%d)", mode);
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	return ERROR_OK;
+}
+
+static int cmsis_dap_cmd_DAP_Disconnect(void)
+{
+	int retval;
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	buffer[0] = 0;	/* report number */
+	buffer[1] = CMD_DAP_DISCONNECT;
+	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 2);
+
+	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
+		LOG_ERROR("CMSIS-DAP command CMD_DISCONNECT failed.");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	return ERROR_OK;
+}
+
+static int cmsis_dap_cmd_DAP_TFER_Configure(uint8_t idle, uint16_t wait, uint16_t retry)
+{
+	int retval;
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	buffer[0] = 0;	/* report number */
+	buffer[1] = CMD_DAP_TFER_CONFIGURE;
+	buffer[2] = idle;
+	buffer[3] = wait & 0xff;
+	buffer[4] = (wait >> 8) & 0xff;
+	buffer[5] = retry & 0xff;
+	buffer[6] = (retry >> 8) & 0xff;
+	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 7);
+
+	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
+		LOG_ERROR("CMSIS-DAP command CMD_TFER_Configure failed.");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	return ERROR_OK;
+}
+
+static int cmsis_dap_cmd_DAP_SWD_Configure(uint8_t cfg)
+{
+	int retval;
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	buffer[0] = 0;	/* report number */
+	buffer[1] = CMD_DAP_SWD_CONFIGURE;
+	buffer[2] = cfg;
+	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 3);
+
+	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
+		LOG_ERROR("CMSIS-DAP command CMD_SWD_Configure failed.");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	return ERROR_OK;
+}
+
+#if 0
+static int cmsis_dap_cmd_DAP_Delay(uint16_t delay_us)
+{
+	int retval;
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	buffer[0] = 0;	/* report number */
+	buffer[1] = CMD_DAP_DELAY;
+	buffer[2] = delay_us & 0xff;
+	buffer[3] = (delay_us >> 8) & 0xff;
+	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, 4);
+
+	if (retval != ERROR_OK || buffer[1] != DAP_OK) {
+		LOG_ERROR("CMSIS-DAP command CMD_Delay failed.");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	return ERROR_OK;
+}
+#endif
+
+static int cmsis_dap_swd_read_reg(uint8_t cmd, uint32_t *value)
+{
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+	int retval;
+	uint32_t val;
+
+	DEBUG_IO("CMSIS-DAP: Read  Reg 0x%02" PRIx8, cmd);
+
+	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];
+	}
+
+	val = le_to_h_u32(&buffer[3]);
+	DEBUG_IO("0x%08" PRIx32, val);
+
+	if (value)
+		*value = val;
+
+	return retval;
+}
+
+static int cmsis_dap_swd_write_reg(uint8_t cmd, uint32_t value)
+{
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	DEBUG_IO("CMSIS-DAP: Write Reg 0x%02" PRIx8 " 0x%08" PRIx32, cmd, value);
+
+	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];
+	}
+
+	return retval;
+}
+
+static int cmsis_dap_swd_read_block(uint8_t cmd, uint32_t blocksize, uint8_t *dest_buf)
+{
+	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];
+		}
+
+		read_count *= 4;
+		memcpy(dest_buf, &buffer[4], read_count);
+
+		dest_buf += read_count;
+		blocksize -= tfer_sz;
+	}
+
+	return retval;
+}
+
+static int cmsis_dap_get_version_info(void)
+{
+	uint8_t *data;
+
+	/* INFO_ID_FW_VER - string */
+	int retval = cmsis_dap_cmd_DAP_Info(INFO_ID_FW_VER, &data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (data[0]) /* strlen */
+		LOG_INFO("CMSIS-DAP: FW Version = %s", &data[1]);
+
+	return ERROR_OK;
+}
+
+static int cmsis_dap_get_caps_info(void)
+{
+	uint8_t *data;
+
+	/* INFO_ID_CAPS - byte */
+	int retval = cmsis_dap_cmd_DAP_Info(INFO_ID_CAPS, &data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (data[0] == 1) {
+		uint8_t caps = data[1];
+
+		cmsis_dap_handle->caps = caps;
+
+		if (caps & INFO_CAPS_SWD)
+			LOG_INFO("CMSIS-DAP: %s", info_caps_str[0]);
+		if (caps & INFO_CAPS_JTAG)
+			LOG_INFO("CMSIS-DAP: %s", info_caps_str[1]);
+	}
+
+	return ERROR_OK;
+}
+
+static int cmsis_dap_get_status(void)
+{
+	uint8_t d;
+
+	int retval = cmsis_dap_cmd_DAP_SWJ_Pins(0, 0, 0, &d);
+
+	if (retval == ERROR_OK) {
+		LOG_INFO("SWCLK/TCK = %d SWDIO/TMS = %d TDI = %d TDO = %d nTRST = %d nRESET = %d",
+			(d & (0x01 << 0)) ? 1 : 0,	/* Bit 0: SWCLK/TCK */
+			(d & (0x01 << 1)) ? 1 : 0,	/* Bit 1: SWDIO/TMS */
+			(d & (0x01 << 2)) ? 1 : 0,	/* Bit 2: TDI */
+			(d & (0x01 << 3)) ? 1 : 0,	/* Bit 3: TDO */
+			(d & (0x01 << 5)) ? 1 : 0,	/* Bit 5: nTRST */
+			(d & (0x01 << 7)) ? 1 : 0);	/* Bit 7: nRESET */
+	}
+
+	return retval;
+}
+
+static int cmsis_dap_reset_link(void)
+{
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	LOG_DEBUG("CMSIS-DAP: cmsis_dap_reset_link");
+	LOG_INFO("DAP_SWJ Sequence (reset: 50+ '1' followed by 0)");
+
+	/* 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);
+
+	if (retval != ERROR_OK || buffer[1] != DAP_OK)
+		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);
+
+	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);
+
+	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);
+
+	if (retval != ERROR_OK || buffer[1] != DAP_OK)
+		return ERROR_FAIL;
+
+	DEBUG_IO("DAP Read IDCODE");
+
+	/* 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);
+
+	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]);
+
+	return retval;
+}
+
+static int cmsis_dap_init(void)
+{
+	int retval;
+	uint8_t *data;
+
+	if (cmsis_dap_handle == NULL) {
+
+		/* JTAG init */
+		retval = cmsis_dap_usb_open();
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = cmsis_dap_get_caps_info();
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* Connect in JTAG mode */
+		if (!(cmsis_dap_handle->caps & INFO_CAPS_JTAG)) {
+			LOG_ERROR("CMSIS-DAP: JTAG not supported");
+			return ERROR_JTAG_DEVICE_ERROR;
+		}
+
+		retval = cmsis_dap_cmd_DAP_Connect(CONNECT_JTAG);
+		if (retval != ERROR_OK)
+			return retval;
+
+		LOG_INFO("CMSIS-DAP: Interface Initialised (JTAG)");
+	}
+
+	retval = cmsis_dap_get_version_info();
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* INFO_ID_PKT_SZ - short */
+	retval = cmsis_dap_cmd_DAP_Info(INFO_ID_PKT_SZ, &data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (data[0] == 2) {  /* short */
+		uint16_t pkt_sz = data[1] + (data[2] << 8);
+
+		if (cmsis_dap_handle->packet_size != pkt_sz + 1) {
+			/* reallocate buffer */
+			cmsis_dap_handle->packet_size = pkt_sz + 1;
+			cmsis_dap_handle->packet_buffer = realloc(cmsis_dap_handle->packet_buffer,
+					cmsis_dap_handle->packet_size);
+			if (cmsis_dap_handle->packet_buffer == NULL) {
+				LOG_ERROR("unable to reallocate memory");
+				return ERROR_FAIL;
+			}
+		}
+
+		LOG_DEBUG("CMSIS-DAP: Packet Size = %" PRId16, pkt_sz);
+	}
+
+	/* INFO_ID_PKT_CNT - byte */
+	retval = cmsis_dap_cmd_DAP_Info(INFO_ID_PKT_CNT, &data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (data[0] == 1) { /* byte */
+		uint16_t pkt_cnt = data[1];
+		cmsis_dap_handle->packet_count = pkt_cnt;
+		LOG_DEBUG("CMSIS-DAP: Packet Count = %" PRId16, pkt_cnt);
+	}
+
+	retval = cmsis_dap_get_status();
+	if (retval != ERROR_OK)
+		return ERROR_FAIL;
+
+	/* Now try to connect to the target
+	 * TODO: This is all SWD only @ present */
+	retval = cmsis_dap_cmd_DAP_SWJ_Clock(100);		/* 100kHz */
+	if (retval != ERROR_OK)
+		return ERROR_FAIL;
+
+	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);
+	if (retval != ERROR_OK)
+		return ERROR_FAIL;
+
+	retval = cmsis_dap_cmd_DAP_LED(0x03);		/* Both LEDs on */
+	if (retval != ERROR_OK)
+		return ERROR_FAIL;
+
+	/* support connecting with srst asserted */
+	enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+	if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
+		if (jtag_reset_config & RESET_SRST_NO_GATING) {
+			retval = cmsis_dap_cmd_DAP_SWJ_Pins(0, (1 << 7), 0, NULL);
+			if (retval != ERROR_OK)
+				return ERROR_FAIL;
+			LOG_INFO("Connecting under reset");
+		}
+	}
+
+	retval = cmsis_dap_reset_link();
+	if (retval != ERROR_OK)
+		return ERROR_FAIL;
+
+	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)
+{
+	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)");
+	return ERROR_OK;
+}
+
+static int cmsis_dap_quit(void)
+{
+	cmsis_dap_cmd_DAP_Disconnect();
+	cmsis_dap_cmd_DAP_LED(0x00);		/* Both LEDs off */
+
+	cmsis_dap_usb_close(cmsis_dap_handle);
+
+	return ERROR_OK;
+}
+
+static void cmsis_dap_execute_reset(struct jtag_command *cmd)
+{
+	int retval = cmsis_dap_cmd_DAP_SWJ_Pins(cmd->cmd.reset->srst ? 0 : (1 << 7), \
+			(1 << 7), 0, NULL);
+	if (retval != ERROR_OK)
+		LOG_ERROR("CMSIS-DAP: Interface reset failed");
+}
+
+static void cmsis_dap_execute_sleep(struct jtag_command *cmd)
+{
+#if 0
+	int retval = cmsis_dap_cmd_DAP_Delay(cmd->cmd.sleep->us);
+	if (retval != ERROR_OK)
+#endif
+		jtag_sleep(cmd->cmd.sleep->us);
+}
+
+static void cmsis_dap_execute_command(struct jtag_command *cmd)
+{
+	switch (cmd->type) {
+		case JTAG_RESET:
+			cmsis_dap_execute_reset(cmd);
+			break;
+		case JTAG_SLEEP:
+			cmsis_dap_execute_sleep(cmd);
+			break;
+		default:
+			LOG_ERROR("BUG: unknown JTAG command type encountered");
+			exit(-1);
+	}
+}
+
+static int cmsis_dap_execute_queue(void)
+{
+	struct jtag_command *cmd = jtag_command_queue;
+
+	while (cmd != NULL) {
+		cmsis_dap_execute_command(cmd);
+		cmd = cmd->next;
+	}
+
+	return ERROR_OK;
+}
+
+static int cmsis_dap_speed(int speed)
+{
+	if (speed > DAP_MAX_CLOCK) {
+		LOG_INFO("reduce speed request: %dkHz to %dkHz maximum", speed, DAP_MAX_CLOCK);
+		speed = DAP_MAX_CLOCK;
+	}
+
+	if (speed == 0) {
+		LOG_INFO("RTCK not supported");
+		return ERROR_JTAG_NOT_IMPLEMENTED;
+	}
+
+	return cmsis_dap_cmd_DAP_SWJ_Clock(speed);
+}
+
+static int cmsis_dap_speed_div(int speed, int *khz)
+{
+	*khz = speed;
+	return ERROR_OK;
+}
+
+static int cmsis_dap_khz(int khz, int *jtag_speed)
+{
+	*jtag_speed = khz;
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(cmsis_dap_handle_info_command)
+{
+	if (cmsis_dap_get_version_info() == ERROR_OK)
+		cmsis_dap_get_status();
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
+{
+	if (CMD_ARGC > MAX_USB_IDS * 2) {
+		LOG_WARNING("ignoring extra IDs in cmsis_dap_vid_pid "
+			"(maximum is %d pairs)", MAX_USB_IDS);
+		CMD_ARGC = MAX_USB_IDS * 2;
+	}
+	if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
+		LOG_WARNING("incomplete cmsis_dap_vid_pid configuration directive");
+		if (CMD_ARGC < 2)
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		/* remove the incomplete trailing id */
+		CMD_ARGC -= 1;
+	}
+
+	unsigned i;
+	for (i = 0; i < CMD_ARGC; i += 2) {
+		COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], cmsis_dap_vid[i >> 1]);
+		COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], cmsis_dap_pid[i >> 1]);
+	}
+
+	/*
+	 * Explicitly terminate, in case there are multiples instances of
+	 * cmsis_dap_vid_pid.
+	 */
+	cmsis_dap_vid[i >> 1] = cmsis_dap_pid[i >> 1] = 0;
+
+	return ERROR_OK;
+}
+
+static const struct command_registration cmsis_dap_subcommand_handlers[] = {
+	{
+		.name = "info",
+		.handler = &cmsis_dap_handle_info_command,
+		.mode = COMMAND_EXEC,
+		.usage = "",
+		.help = "show cmsis-dap info",
+	},
+	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",
+		.mode = COMMAND_ANY,
+		.help = "perform CMSIS-DAP management",
+		.usage = "<cmd>",
+		.chain = cmsis_dap_subcommand_handlers,
+	},
+	{
+		.name = "cmsis_dap_vid_pid",
+		.handler = &cmsis_dap_handle_vid_pid_command,
+		.mode = COMMAND_CONFIG,
+		.help = "the vendor ID and product ID of the CMSIS-DAP device",
+		.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,
+	},
+	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
+};
+
+const char *cmsis_dap_transport[] = {"cmsis-dap", NULL};
+
+struct jtag_interface cmsis_dap_interface = {
+	.name = "cmsis-dap",
+	.commands = cmsis_dap_command_handlers,
+	.swd = &cmsis_dap_swd_driver,
+	.transports = cmsis_dap_transport,
+
+	.execute_queue = cmsis_dap_execute_queue,
+	.speed = cmsis_dap_speed,
+	.speed_div = cmsis_dap_speed_div,
+	.khz = cmsis_dap_khz,
+	.init = cmsis_dap_init,
+	.quit = cmsis_dap_quit,
+};
diff --git a/src/jtag/interface.h b/src/jtag/interface.h
index ac4eacc7..8dacd223 100644
--- a/src/jtag/interface.h
+++ b/src/jtag/interface.h
@@ -302,8 +302,6 @@ struct jtag_interface {
 
 extern const char *jtag_only[];
 
-extern const struct swd_driver *swd;
-
 void adapter_assert_reset(void);
 void adapter_deassert_reset(void);
 
diff --git a/src/jtag/interfaces.c b/src/jtag/interfaces.c
index eb447cdd..d0bc3827 100644
--- a/src/jtag/interfaces.c
+++ b/src/jtag/interfaces.c
@@ -128,6 +128,9 @@ extern struct jtag_interface aice_interface;
 #if BUILD_BCM2835GPIO == 1
 extern struct jtag_interface bcm2835gpio_interface;
 #endif
+#if BUILD_CMSIS_DAP == 1
+extern struct jtag_interface cmsis_dap_interface;
+#endif
 #endif /* standard drivers */
 
 /**
@@ -224,6 +227,9 @@ struct jtag_interface *jtag_interfaces[] = {
 #if BUILD_BCM2835GPIO == 1
 		&bcm2835gpio_interface,
 #endif
+#if BUILD_CMSIS_DAP == 1
+		&cmsis_dap_interface,
+#endif
 #endif /* standard drivers */
 		NULL,
 	};
diff --git a/src/jtag/swd.h b/src/jtag/swd.h
index 80d50ff8..f131ddbc 100644
--- a/src/jtag/swd.h
+++ b/src/jtag/swd.h
@@ -110,11 +110,17 @@ struct swd_driver {
 	  */
 	 int (*write_reg)(uint8_t cmd, uint32_t value);
 
-	/* XXX START WITH enough to:
-	 *	init (synch mode, WCR)
-	 *		for async, TRN > 1
-	 *	read IDCODE from DP
-	 */
+	 /**
+	  * Synchronous block read of an AP or DP register.
+	  *
+	  * @param cmd with APnDP/RnW/addr/parity bits
+	  * @param number of reads from register to be executed
+	  * @param buffer to store data read from register
+	  *
+	  * @return SWD_ACK_* code for the transaction
+	  *		or (negative) fault code
+	  */
+	 int (*read_block)(uint8_t cmd, uint32_t blocksize, uint8_t *buffer);
 
 	/**
 	 * Configures data collection from the Single-wire
@@ -135,5 +141,6 @@ int swd_init_reset(struct command_context *cmd_ctx);
 void swd_add_reset(int req_srst);
 
 bool transport_is_swd(void);
+bool transport_is_cmsis_dap(void);
 
 #endif /* SWD_H */
diff --git a/src/jtag/tcl.c b/src/jtag/tcl.c
index 3f63ffc1..ceea1930 100644
--- a/src/jtag/tcl.c
+++ b/src/jtag/tcl.c
@@ -59,8 +59,6 @@ static const Jim_Nvp nvp_jtag_tap_event[] = {
 	{ .name = NULL, .value = -1 }
 };
 
-extern struct jtag_interface *jtag_interface;
-
 struct jtag_tap *jtag_tap_by_jim_obj(Jim_Interp *interp, Jim_Obj *o)
 {
 	const char *cp = Jim_GetString(o, NULL);
diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 15607536..4d9ea418 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -91,6 +91,7 @@ ARM_DEBUG_SRC = \
 	arm_adi_v5.c \
 	adi_v5_jtag.c \
 	adi_v5_swd.c \
+	adi_v5_cmsis_dap.c \
 	embeddedice.c \
 	trace.c \
 	etb.c \
diff --git a/src/target/adi_v5_cmsis_dap.c b/src/target/adi_v5_cmsis_dap.c
new file mode 100644
index 00000000..3ff5bfe0
--- /dev/null
+++ b/src/target/adi_v5_cmsis_dap.c
@@ -0,0 +1,344 @@
+/***************************************************************************
+ *   Copyright (C) 2013 by mike brown                                      *
+ *   mike@theshedworks.org.uk                                              *
+ *                                                                         *
+ *   Copyright (C) 2013 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
+ ***************************************************************************/
+
+/**
+ * @file
+ * Utilities to support ARM "CMSIS-DAP", The CoreSight Debug Access Port.
+ * This is coupled to recent versions of ARM's "CoreSight" debug framework.
+ * This specific code is a transport level interface, with
+ * "target/arm_adi_v5.[hc]" code understanding operation semantics,
+ * shared with the SWD & JTAG transports.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "arm.h"
+#include "arm_adi_v5.h"
+#include <helper/time_support.h>
+
+#include <transport/transport.h>
+#include <jtag/interface.h>
+
+#include <jtag/swd.h>
+
+#define CMSIS_CMD_DP            (0   << 0)    /* set only for AP access */
+#define CMSIS_CMD_AP            (1   << 0)    /* set only for AP access */
+#define CMSIS_CMD_READ          (1   << 1)    /* set only for read access */
+#define CMSIS_CMD_WRITE         (0   << 1)    /* set only for read access */
+#define CMSIS_CMD_A32(n)        ((n)&0x0C)    /* bits A[3:2] of register addr */
+#define CMSIS_CMD_VAL_MATCH     (1   << 4)    /* Value Match */
+#define CMSIS_CMD_MATCH_MSK     (1   << 5)    /* Match Mask */
+
+/* YUK! - but this is currently a global.... */
+extern struct jtag_interface *jtag_interface;
+
+static int (cmsis_dap_queue_ap_abort)(struct adiv5_dap *dap, uint8_t *ack)
+{
+	LOG_DEBUG("CMSIS-ADI: cmsis_dap_queue_ap_abort");
+
+	/* FIXME: implement this properly cmsis-dap has DAP_WriteABORT()
+	 * for now just hack @ everything */
+	return jtag_interface->swd->write_reg(
+			(CMSIS_CMD_DP | CMSIS_CMD_WRITE | CMSIS_CMD_A32(DP_ABORT)), 0x1e);
+}
+
+static int cmsis_dap_queue_dp_read(struct adiv5_dap *dap, unsigned reg, uint32_t *data)
+{
+	LOG_DEBUG("CMSIS-ADI: cmsis_dap_queue_dp_read %d", reg);
+
+	int retval = jtag_interface->swd->read_reg(
+			(CMSIS_CMD_DP | CMSIS_CMD_READ | CMSIS_CMD_A32(reg)), data);
+
+	if (retval != ERROR_OK) {
+		/* fault response */
+		uint8_t ack = retval & 0xff;
+		cmsis_dap_queue_ap_abort(dap, &ack);
+	}
+
+	return retval;
+}
+
+static int cmsis_dap_queue_idcode_read(struct adiv5_dap *dap, uint8_t *ack, uint32_t *data)
+{
+	LOG_DEBUG("CMSIS-ADI: cmsis_dap_queue_idcode_read");
+
+	int retval = cmsis_dap_queue_dp_read(dap, DP_IDCODE, data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	*ack = retval;
+
+	return ERROR_OK;
+}
+
+static int (cmsis_dap_queue_dp_write)(struct adiv5_dap *dap, unsigned reg, uint32_t data)
+{
+	LOG_DEBUG("CMSIS-ADI: cmsis_dap_queue_dp_write %d 0x%08" PRIx32, reg, data);
+
+	/* setting the ORUNDETECT bit causes issues for some targets,
+	 * disable until we find out why */
+	if (reg == DP_CTRL_STAT) {
+		LOG_DEBUG("disabling overrun detection");
+		data &= ~CORUNDETECT;
+	}
+
+	int retval = jtag_interface->swd->write_reg(
+			(CMSIS_CMD_DP | CMSIS_CMD_WRITE | CMSIS_CMD_A32(reg)), data);
+
+	if (retval != ERROR_OK) {
+		/* fault response */
+		uint8_t ack = retval & 0xff;
+		cmsis_dap_queue_ap_abort(dap, &ack);
+	}
+
+	return retval;
+}
+
+/** Select the AP register bank matching bits 7:4 of reg. */
+static int cmsis_dap_ap_q_bankselect(struct adiv5_dap *dap, unsigned reg)
+{
+	uint32_t select_ap_bank = reg & 0x000000F0;
+
+	if (select_ap_bank == dap->ap_bank_value)
+		return ERROR_OK;
+
+	dap->ap_bank_value = select_ap_bank;
+	select_ap_bank |= dap->ap_current;
+
+	return cmsis_dap_queue_dp_write(dap, DP_SELECT, select_ap_bank);
+}
+
+static int (cmsis_dap_queue_ap_read)(struct adiv5_dap *dap, unsigned reg, uint32_t *data)
+{
+	LOG_DEBUG("CMSIS-ADI: cmsis_dap_queue_ap_read %d", reg);
+
+	int retval = cmsis_dap_ap_q_bankselect(dap, reg);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = jtag_interface->swd->read_reg(
+			(CMSIS_CMD_AP | CMSIS_CMD_READ | CMSIS_CMD_A32(reg)), data);
+
+	if (retval != ERROR_OK) {
+		/* fault response */
+		uint8_t ack = retval & 0xff;
+		cmsis_dap_queue_ap_abort(dap, &ack);
+	}
+
+	return retval;
+}
+
+static int (cmsis_dap_queue_ap_write)(struct adiv5_dap *dap, unsigned reg, uint32_t data)
+{
+	LOG_DEBUG("CMSIS-ADI: cmsis_dap_queue_ap_write %d 0x%08" PRIx32, reg, data);
+
+	/* TODO: CSW_DBGSWENABLE (bit31) causes issues for some targets
+	 * disable until we find out why */
+	if (reg == AP_REG_CSW)
+		data &= ~CSW_DBGSWENABLE;
+
+	int retval = cmsis_dap_ap_q_bankselect(dap, reg);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = jtag_interface->swd->write_reg(
+			(CMSIS_CMD_AP | CMSIS_CMD_WRITE | CMSIS_CMD_A32(reg)), data);
+
+	if (retval != ERROR_OK) {
+		/* fault response */
+		uint8_t ack = retval & 0xff;
+		cmsis_dap_queue_ap_abort(dap, &ack);
+	}
+
+	return retval;
+}
+
+static int (cmsis_dap_queue_ap_read_block)(struct adiv5_dap *dap, unsigned reg,
+		uint32_t blocksize, uint8_t *buffer)
+{
+	LOG_DEBUG("CMSIS-ADI: cmsis_dap_queue_ap_read_block 0x%08" PRIx32, blocksize);
+
+	int retval = jtag_interface->swd->read_block(
+			(CMSIS_CMD_AP | CMSIS_CMD_READ | CMSIS_CMD_A32(AP_REG_DRW)),
+			blocksize, buffer);
+
+	if (retval != ERROR_OK) {
+		/* fault response */
+		uint8_t ack = retval & 0xff;
+		cmsis_dap_queue_ap_abort(dap, &ack);
+	}
+
+	return retval;
+}
+
+/** Executes all queued DAP operations. */
+static int cmsis_dap_run(struct adiv5_dap *dap)
+{
+	LOG_DEBUG("CMSIS-ADI: cmsis_dap_run");
+	/* FIXME: for now the CMSIS-DAP interface hard-wires a zero-size queue. */
+
+	return ERROR_OK;
+}
+
+const struct dap_ops cmsis_dap_ops = {
+	.is_swd = true,
+	.queue_idcode_read   = cmsis_dap_queue_idcode_read,
+	.queue_dp_read       = cmsis_dap_queue_dp_read,
+	.queue_dp_write      = cmsis_dap_queue_dp_write,
+	.queue_ap_read       = cmsis_dap_queue_ap_read,
+	.queue_ap_write      = cmsis_dap_queue_ap_write,
+	.queue_ap_read_block = cmsis_dap_queue_ap_read_block,
+	.queue_ap_abort      = cmsis_dap_queue_ap_abort,
+	.run = cmsis_dap_run,
+};
+
+static const struct command_registration cmsis_dap_commands[] = {
+	{
+	/*
+	* Set up SWD and JTAG targets identically, unless/until
+	* infrastructure improves ...  meanwhile, ignore all
+	* JTAG-specific stuff like IR length for SWD.
+	*
+	* REVISIT can we verify "just one SWD DAP" here/early?
+	*/
+		.name = "newdap",
+		.jim_handler = jim_jtag_newtap,
+		.mode = COMMAND_CONFIG,
+		.help = "declare a new CMSIS-DAP"
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration cmsis_dap_handlers[] = {
+	{
+		.name = "cmsis-dap",
+		.mode = COMMAND_ANY,
+		.help = "cmsis_dap command group",
+		.chain = cmsis_dap_commands,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static int cmsis_dap_select(struct command_context *ctx)
+{
+	LOG_DEBUG("CMSIS-ADI: cmsis_dap_select");
+
+	int retval = register_commands(ctx, NULL, cmsis_dap_handlers);
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* FIXME: This needs a real overhaul!! FIXME
+	 * be sure driver is in SWD mode; start
+	 * with hardware default TRN (1), it can be changed later
+	 * we use a bogus 'swd' driver to implement cmsis-dap as it is quite similar */
+
+	const struct swd_driver *swd = jtag_interface->swd;
+	if (!swd || !swd->read_reg || !swd->write_reg || !swd->init) {
+		LOG_ERROR("no SWD driver?");
+		return ERROR_FAIL;
+	}
+
+	retval = swd->init(1);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("unable to init CMSIS-DAP driver");
+		return retval;
+	}
+
+	return retval;
+}
+
+static int cmsis_dap_init(struct command_context *ctx)
+{
+	struct target *target = get_current_target(ctx);
+	struct arm *arm = target_to_arm(target);
+	struct adiv5_dap *dap = arm->dap;
+	uint32_t idcode;
+	int status;
+
+	LOG_DEBUG("CMSIS-ADI: cmsis_dap_init");
+
+	/* Force the DAP's ops vector for CMSIS-DAP mode.
+	 * messy - is there a better way? */
+	arm->dap->ops = &cmsis_dap_ops;
+
+	/* FIXME validate transport config ... is the
+	 * configured DAP present (check IDCODE)?
+	 * Is *only* one DAP configured?
+	 *
+	 * MUST READ IDCODE
+	 */
+
+	/* Note, debugport_init() does setup too */
+
+#if 0
+	const struct swd_driver *swd = jtag_interface->swd;
+	if (!swd || !swd->read_reg || !swd->write_reg || !swd->init) {
+		LOG_ERROR("no SWD driver?");
+		return ERROR_FAIL;
+	}
+
+	int retval = swd->init(1);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("unable to init CMSIS-DAP driver");
+		return retval;
+	}
+#endif
+
+	uint8_t ack;
+
+	status = cmsis_dap_queue_idcode_read(dap, &ack, &idcode);
+
+	if (status == ERROR_OK)
+		LOG_INFO("IDCODE 0x%08" PRIx32, idcode);
+
+	/* force clear all sticky faults */
+	cmsis_dap_queue_ap_abort(dap, &ack);
+
+	/* this is a workaround to get polling working */
+	jtag_add_reset(0, 0);
+
+	return status;
+}
+
+static struct transport cmsis_dap_transport = {
+	.name = "cmsis-dap",
+	.select = cmsis_dap_select,
+	.init = cmsis_dap_init,
+};
+
+static void cmsis_dap_constructor(void) __attribute__((constructor));
+static void cmsis_dap_constructor(void)
+{
+	transport_register(&cmsis_dap_transport);
+}
+
+/** Returns true if the current debug session
+ * is using CMSIS-DAP as its transport.
+ */
+bool transport_is_cmsis_dap(void)
+{
+	return get_current_transport() == &cmsis_dap_transport;
+}
diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 027e0669..6ff858a8 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -55,10 +55,16 @@
 
 #include <jtag/swd.h>
 
+/* YUK! - but this is currently a global.... */
+extern struct jtag_interface *jtag_interface;
+
 static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
 		uint32_t *data)
 {
 	/* REVISIT status return vs ack ... */
+	const struct swd_driver *swd = jtag_interface->swd;
+	assert(swd);
+
 	return swd->read_reg(swd_cmd(true,  false, reg), data);
 }
 
@@ -77,6 +83,9 @@ static int (swd_queue_dp_write)(struct adiv5_dap *dap, unsigned reg,
 		uint32_t data)
 {
 	/* REVISIT status return vs ack ... */
+	const struct swd_driver *swd = jtag_interface->swd;
+	assert(swd);
+
 	return swd->write_reg(swd_cmd(false,  false, reg), data);
 }
 
@@ -86,6 +95,9 @@ static int (swd_queue_ap_read)(struct adiv5_dap *dap, unsigned reg,
 {
 	/* REVISIT  APSEL ... */
 	/* REVISIT status return ... */
+	const struct swd_driver *swd = jtag_interface->swd;
+	assert(swd);
+
 	return swd->read_reg(swd_cmd(true,  true, reg), data);
 }
 
@@ -94,6 +106,9 @@ static int (swd_queue_ap_write)(struct adiv5_dap *dap, unsigned reg,
 {
 	/* REVISIT  APSEL ... */
 	/* REVISIT status return ... */
+	const struct swd_driver *swd = jtag_interface->swd;
+	assert(swd);
+
 	return swd->write_reg(swd_cmd(false,  true, reg), data);
 }
 
@@ -185,8 +200,6 @@ int dap_to_swd(struct target *target)
 	return retval;
 }
 
-
-
 COMMAND_HANDLER(handle_swd_wcr)
 {
 	int retval;
@@ -280,7 +293,6 @@ static const struct command_registration swd_handlers[] = {
 
 static int swd_select(struct command_context *ctx)
 {
-	struct target *target = get_current_target(ctx);
 	int retval;
 
 	retval = register_commands(ctx, NULL, swd_handlers);
@@ -288,6 +300,8 @@ static int swd_select(struct command_context *ctx)
 	if (retval != ERROR_OK)
 		return retval;
 
+	const struct swd_driver *swd = jtag_interface->swd;
+
 	 /* be sure driver is in SWD mode; start
 	  * with hardware default TRN (1), it can be changed later
 	  */
@@ -296,14 +310,20 @@ static int swd_select(struct command_context *ctx)
 		return ERROR_FAIL;
 	}
 
-	 retval = swd->init(1);
+	retval = swd->init(1);
 	if (retval != ERROR_OK) {
 		LOG_DEBUG("can't init SWD driver");
 		return retval;
 	}
 
 	/* force DAP into SWD mode (not JTAG) */
-	retval = dap_to_swd(target);
+	/*retval = dap_to_swd(target);*/
+
+	if (ctx->current_target) {
+		/* force DAP into SWD mode (not JTAG) */
+		struct target *target = get_current_target(ctx);
+		retval = dap_to_swd(target);
+	}
 
 	return retval;
 }
@@ -316,6 +336,10 @@ static int swd_init(struct command_context *ctx)
 	uint32_t idcode;
 	int status;
 
+	/* Force the DAP's ops vector for SWD mode.
+	 * messy - is there a better way? */
+	arm->dap->ops = &swd_dap_ops;
+
 	/* FIXME validate transport config ... is the
 	 * configured DAP present (check IDCODE)?
 	 * Is *only* one DAP configured?