vsllink: Add SWD support

Tested with stm32f1x, stm32f4x and kl25 targets using SWD transport.

Change-Id: I118d07371b53f402ea9ac73f874460a309c05100
Signed-off-by: Fatih Aşıcı <fatih.asici@gmail.com>
Signed-off-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Reviewed-on: http://openocd.zylin.com/1947
Tested-by: jenkins

4 files changed, 236 insertions, 30 deletions

diff --git a/src/jtag/drivers/versaloon/usbtoxxx/usbtoswd.c b/src/jtag/drivers/versaloon/usbtoxxx/usbtoswd.c
index b8d27af9..cd9d51c0 100644
--- a/src/jtag/drivers/versaloon/usbtoxxx/usbtoswd.c
+++ b/src/jtag/drivers/versaloon/usbtoxxx/usbtoswd.c
@@ -30,7 +30,7 @@
 #include "usbtoxxx.h"
 #include "usbtoxxx_internal.h"
 
-RESULT usbtoswd_callback(void *p, uint8_t *src, uint8_t *processed)
+RESULT usbtoswd_read_callback(void *p, uint8_t *src, uint8_t *processed)
 {
 	struct versaloon_pending_t *pending = (struct versaloon_pending_t *)p;
 
@@ -40,6 +40,18 @@ RESULT usbtoswd_callback(void *p, uint8_t *src, uint8_t *processed)
 	return ERROR_OK;
 }
 
+RESULT usbtoswd_write_callback(void *p, uint8_t *src, uint8_t *processed)
+{
+	struct versaloon_pending_t *pending = (struct versaloon_pending_t *)p;
+
+	if (pending->extra_data != NULL)
+		*((uint8_t *)pending->extra_data) = src[0];
+
+	/* mark it processed to ignore other input data */
+	*processed = 1;
+	return ERROR_OK;
+}
+
 RESULT usbtoswd_init(uint8_t interface_index)
 {
 	return usbtoxxx_init_command(USB_TO_SWD, interface_index);
@@ -69,7 +81,8 @@ RESULT usbtoswd_config(uint8_t interface_index, uint8_t trn, uint16_t retry,
 	return usbtoxxx_conf_command(USB_TO_SWD, interface_index, cfg_buf, 5);
 }
 
-RESULT usbtoswd_seqout(uint8_t interface_index, uint8_t *data, uint16_t bitlen)
+RESULT usbtoswd_seqout(uint8_t interface_index, const uint8_t *data,
+	uint16_t bitlen)
 {
 	uint16_t bytelen = (bitlen + 7) >> 3;
 
@@ -130,14 +143,16 @@ RESULT usbtoswd_transact(uint8_t interface_index, uint8_t request,
 		memset(buff + 1, 0, 4);
 
 	versaloon_set_extra_data(ack);
-	versaloon_set_callback(usbtoswd_callback);
 	if (request & 0x04) {
 		/* read */
-		return usbtoxxx_inout_command(USB_TO_SWD, interface_index, buff, 5, 5,
-			(uint8_t *)data, 1, 4, 0);
+		versaloon_set_callback(usbtoswd_read_callback);
 	} else {
 		/* write */
-		return usbtoxxx_inout_command(USB_TO_SWD, interface_index, buff, 5, 5,
-			NULL, 0, 0, 0);
+		versaloon_set_callback(usbtoswd_write_callback);
 	}
+
+	/* Input buffer must be passed even for write operations. Otherwise
+	 * the callback function is not called and ack value is not set. */
+	return usbtoxxx_inout_command(USB_TO_SWD, interface_index, buff, 5, 5,
+		(uint8_t *)data, 1, 4, 0);
 }
diff --git a/src/jtag/drivers/versaloon/usbtoxxx/usbtoxxx.h b/src/jtag/drivers/versaloon/usbtoxxx/usbtoxxx.h
index 83139fcc..fa53062b 100644
--- a/src/jtag/drivers/versaloon/usbtoxxx/usbtoxxx.h
+++ b/src/jtag/drivers/versaloon/usbtoxxx/usbtoxxx.h
@@ -183,7 +183,8 @@ RESULT usbtoswd_init(uint8_t interface_index);
 RESULT usbtoswd_fini(uint8_t interface_index);
 RESULT usbtoswd_config(uint8_t interface_index, uint8_t trn, uint16_t retry,
 		       uint16_t dly);
-RESULT usbtoswd_seqout(uint8_t interface_index, uint8_t *data, uint16_t bitlen);
+RESULT usbtoswd_seqout(uint8_t interface_index, const uint8_t *data,
+		       uint16_t bitlen);
 RESULT usbtoswd_seqin(uint8_t interface_index, uint8_t *data, uint16_t bitlen);
 RESULT usbtoswd_transact(uint8_t interface_index, uint8_t request,
 			 uint32_t *data, uint8_t *ack);
diff --git a/src/jtag/drivers/versaloon/versaloon.h b/src/jtag/drivers/versaloon/versaloon.h
index 2323aee9..58008537 100644
--- a/src/jtag/drivers/versaloon/versaloon.h
+++ b/src/jtag/drivers/versaloon/versaloon.h
@@ -59,7 +59,8 @@ struct interface_swd_t {
 	RESULT(*fini)(uint8_t interface_index);
 	RESULT(*config)(uint8_t interface_index, uint8_t trn, uint16_t retry,
 		uint16_t dly);
-	RESULT(*seqout)(uint8_t interface_index, uint8_t *data, uint16_t bitlen);
+	RESULT(*seqout)(uint8_t interface_index, const uint8_t *data,
+			uint16_t bitlen);
 	RESULT(*seqin)(uint8_t interface_index, uint8_t *data, uint16_t bitlen);
 	RESULT(*transact)(uint8_t interface_index, uint8_t request,
 		uint32_t *data, uint8_t *ack);
diff --git a/src/jtag/drivers/vsllink.c b/src/jtag/drivers/vsllink.c
index 95806726..1f861baa 100644
--- a/src/jtag/drivers/vsllink.c
+++ b/src/jtag/drivers/vsllink.c
@@ -28,6 +28,7 @@
 
 #include <jtag/interface.h>
 #include <jtag/commands.h>
+#include <jtag/swd.h>
 #include "usb_common.h"
 
 #include "versaloon/versaloon_include.h"
@@ -70,6 +71,12 @@ static void vsllink_tap_ensure_pending(int scans);
 static void vsllink_tap_append_scan(int length, uint8_t *buffer,
 		struct scan_command *command);
 
+/* VSLLink SWD functions */
+static int_least32_t vsllink_swd_frequency(struct adiv5_dap *dap,
+		int_least32_t hz);
+static int vsllink_swd_switch_seq(struct adiv5_dap *dap,
+		enum swd_special_seq seq);
+
 /* VSLLink lowlevel functions */
 struct vsllink {
 	struct usb_dev_handle *usb_handle;
@@ -88,6 +95,9 @@ static uint8_t *tms_buffer;
 static uint8_t *tdi_buffer;
 static uint8_t *tdo_buffer;
 
+static bool swd_mode;
+static int queued_retval;
+
 struct vsllink *vsllink_handle;
 
 static int vsllink_execute_queue(void)
@@ -232,6 +242,9 @@ static int vsllink_execute_queue(void)
 
 static int vsllink_speed(int speed)
 {
+	if (swd_mode)
+		return ERROR_OK;
+
 	versaloon_interface.adaptors.jtag_raw.config(0, (uint16_t)speed);
 	return versaloon_interface.adaptors.peripheral_commit();
 }
@@ -271,7 +284,12 @@ static int vsllink_quit(void)
 	versaloon_interface.adaptors.gpio.config(0, GPIO_SRST | GPIO_TRST,
 		0, 0, GPIO_SRST | GPIO_TRST);
 	versaloon_interface.adaptors.gpio.fini(0);
-	versaloon_interface.adaptors.jtag_raw.fini(0);
+
+	if (swd_mode)
+		versaloon_interface.adaptors.swd.fini(0);
+	else
+		versaloon_interface.adaptors.jtag_raw.fini(0);
+
 	versaloon_interface.adaptors.peripheral_commit();
 	versaloon_interface.fini();
 
@@ -281,7 +299,7 @@ static int vsllink_quit(void)
 	return ERROR_OK;
 }
 
-static int vsllink_init(void)
+static int vsllink_interface_init(void)
 {
 	vsllink_handle = vsllink_usb_open();
 	if (vsllink_handle == 0) {
@@ -301,22 +319,46 @@ static int vsllink_init(void)
 		return ERROR_FAIL;
 	}
 
-	/* malloc buffer size for tap */
-	tap_buffer_size = versaloon_interface.usb_setting.buf_size / 2 - 32;
-	vsllink_free_buffer();
-	tdi_buffer = malloc(tap_buffer_size);
-	tdo_buffer = malloc(tap_buffer_size);
-	tms_buffer = malloc(tap_buffer_size);
-	if ((NULL == tdi_buffer) || (NULL == tdo_buffer) || (NULL == tms_buffer)) {
-		vsllink_quit();
-		return ERROR_FAIL;
-	}
+	return ERROR_OK;
+}
+
+static int vsllink_init(void)
+{
+	int retval = vsllink_interface_init();
+	if (ERROR_OK != retval)
+		return retval;
 
-	versaloon_interface.adaptors.jtag_raw.init(0);
-	versaloon_interface.adaptors.jtag_raw.config(0, jtag_get_speed_khz());
 	versaloon_interface.adaptors.gpio.init(0);
-	versaloon_interface.adaptors.gpio.config(0, GPIO_SRST | GPIO_TRST,
-		GPIO_TRST, GPIO_SRST, GPIO_SRST);
+	versaloon_interface.adaptors.gpio.config(0, GPIO_SRST, 0, GPIO_SRST,
+		GPIO_SRST);
+	versaloon_interface.adaptors.delay.delayms(100);
+	versaloon_interface.adaptors.peripheral_commit();
+
+	if (swd_mode) {
+		versaloon_interface.adaptors.gpio.config(0, GPIO_TRST, 0,
+			GPIO_TRST, GPIO_TRST);
+		versaloon_interface.adaptors.swd.init(0);
+		vsllink_swd_frequency(NULL, jtag_get_speed_khz() * 1000);
+		vsllink_swd_switch_seq(NULL, JTAG_TO_SWD);
+
+	} else {
+		/* malloc buffer size for tap */
+		tap_buffer_size = versaloon_interface.usb_setting.buf_size / 2 - 32;
+		vsllink_free_buffer();
+		tdi_buffer = malloc(tap_buffer_size);
+		tdo_buffer = malloc(tap_buffer_size);
+		tms_buffer = malloc(tap_buffer_size);
+		if ((NULL == tdi_buffer) || (NULL == tdo_buffer) || (NULL == tms_buffer)) {
+			vsllink_quit();
+			return ERROR_FAIL;
+		}
+
+		versaloon_interface.adaptors.jtag_raw.init(0);
+		versaloon_interface.adaptors.jtag_raw.config(0, jtag_get_speed_khz());
+		versaloon_interface.adaptors.gpio.config(0, GPIO_SRST | GPIO_TRST,
+			GPIO_TRST, GPIO_SRST, GPIO_SRST);
+	}
+
 	if (ERROR_OK != versaloon_interface.adaptors.peripheral_commit())
 		return ERROR_FAIL;
 
@@ -442,10 +484,13 @@ static void vsllink_reset(int trst, int srst)
 	else
 		versaloon_interface.adaptors.gpio.config(0, GPIO_SRST, GPIO_SRST, 0, 0);
 
-	if (!trst)
-		versaloon_interface.adaptors.gpio.out(0, GPIO_TRST, GPIO_TRST);
-	else
-		versaloon_interface.adaptors.gpio.out(0, GPIO_TRST, 0);
+	if (!swd_mode) {
+		if (!trst)
+			versaloon_interface.adaptors.gpio.out(0, GPIO_TRST, GPIO_TRST);
+		else
+			versaloon_interface.adaptors.gpio.out(0, GPIO_TRST, 0);
+	}
+
 	versaloon_interface.adaptors.peripheral_commit();
 }
 
@@ -660,9 +705,143 @@ static int vsllink_jtag_execute(void)
 
 static int vsllink_tap_execute(void)
 {
+	if (swd_mode)
+		return ERROR_OK;
+
 	return vsllink_jtag_execute();
 }
 
+static int vsllink_swd_init(void)
+{
+	LOG_INFO("VSLLink SWD mode enabled");
+	swd_mode = true;
+
+	return ERROR_OK;
+}
+
+static int_least32_t vsllink_swd_frequency(struct adiv5_dap *dap,
+		int_least32_t hz)
+{
+	const int_least32_t delay2hz[] = {
+		1850000, 235000, 130000, 102000, 85000, 72000
+	};
+
+	if (hz > 0) {
+		uint16_t delay = UINT16_MAX;
+
+		for (uint16_t i = 0; i < ARRAY_SIZE(delay2hz); i++) {
+			if (hz >= delay2hz[i]) {
+				hz = delay2hz[i];
+				delay = i;
+				break;
+			}
+		}
+
+		if (delay == UINT16_MAX)
+			delay = (500000 / hz) - 1;
+
+		/* Calculate retry count after a WAIT response. This will give
+		 * a retry timeout at about ~250 ms. 54 is the number of bits
+		 * found in a transaction. */
+		uint16_t retry_count = 250 * hz / 1000 / 54;
+
+		LOG_DEBUG("SWD delay: %d, retry count: %d", delay, retry_count);
+
+		versaloon_interface.adaptors.swd.config(0, 2, retry_count, delay);
+		queued_retval = versaloon_interface.adaptors.peripheral_commit();
+	}
+
+	return hz;
+}
+
+static int vsllink_swd_switch_seq(struct adiv5_dap *dap,
+		enum swd_special_seq seq)
+{
+	switch (seq) {
+	case LINE_RESET:
+		LOG_DEBUG("SWD line reset");
+		versaloon_interface.adaptors.swd.seqout(0, swd_seq_line_reset,
+				swd_seq_line_reset_len);
+		break;
+	case JTAG_TO_SWD:
+		LOG_DEBUG("JTAG-to-SWD");
+		versaloon_interface.adaptors.swd.seqout(0, swd_seq_jtag_to_swd,
+				swd_seq_jtag_to_swd_len);
+		break;
+	case SWD_TO_JTAG:
+		LOG_DEBUG("SWD-to-JTAG");
+		versaloon_interface.adaptors.swd.seqout(0, swd_seq_swd_to_jtag,
+				swd_seq_swd_to_jtag_len);
+		break;
+	default:
+		LOG_ERROR("Sequence %d not supported", seq);
+		return ERROR_FAIL;
+	}
+
+	return versaloon_interface.adaptors.peripheral_commit();
+}
+
+static void vsllink_swd_read_reg(struct adiv5_dap *dap, uint8_t cmd,
+		uint32_t *value)
+{
+	if (queued_retval != ERROR_OK)
+		return;
+
+	int retval;
+	uint32_t val = 0;
+	uint8_t ack;
+
+	versaloon_interface.adaptors.swd.transact(0, cmd, &val, &ack);
+	retval = versaloon_interface.adaptors.peripheral_commit();
+
+	if (retval != ERROR_OK) {
+		queued_retval = ERROR_FAIL;
+		return;
+	}
+
+	if (ack != 0x01) {
+		queued_retval = ack;
+		return;
+	}
+
+	if (value)
+		*value = val;
+
+	queued_retval = retval;
+}
+
+static void vsllink_swd_write_reg(struct adiv5_dap *dap, uint8_t cmd,
+		uint32_t value)
+{
+	if (queued_retval != ERROR_OK)
+		return;
+
+	int retval;
+	uint8_t ack;
+
+	versaloon_interface.adaptors.swd.transact(0, cmd, &value, &ack);
+	retval = versaloon_interface.adaptors.peripheral_commit();
+
+	if (retval != ERROR_OK) {
+		queued_retval = ERROR_FAIL;
+		return;
+	}
+
+	if (ack != 0x01) {
+		queued_retval = ack;
+		return;
+	}
+
+	queued_retval = retval;
+}
+
+static int vsllink_swd_run_queue(struct adiv5_dap *dap)
+{
+	int retval = queued_retval;
+	queued_retval = ERROR_OK;
+	return retval;
+}
+
 /****************************************************************************
  * VSLLink USB low-level functions */
 
@@ -849,13 +1028,23 @@ static const struct command_registration vsllink_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
-static const char *vsllink_transports[] = {"jtag", "swd", NULL};
+static const char * const vsllink_transports[] = {"jtag", "swd", NULL};
+
+static const struct swd_driver vsllink_swd_driver = {
+	.init = vsllink_swd_init,
+	.frequency = vsllink_swd_frequency,
+	.switch_seq = vsllink_swd_switch_seq,
+	.read_reg = vsllink_swd_read_reg,
+	.write_reg = vsllink_swd_write_reg,
+	.run = vsllink_swd_run_queue,
+};
 
 struct jtag_interface vsllink_interface = {
 	.name = "vsllink",
 	.supported = DEBUG_CAP_TMS_SEQ,
 	.commands = vsllink_command_handlers,
 	.transports = vsllink_transports,
+	.swd = &vsllink_swd_driver,
 
 	.init = vsllink_init,
 	.quit = vsllink_quit,