Merge branch 'master' into from_upstream

Conflicts:
	README
	contrib/loaders/flash/fespi/Makefile
	src/flash/nor/fespi.c
	src/flash/nor/spi.c

Change-Id: I78a4e73685cc95daace95e9d16066a6fb51034fb

21 files changed, 1549 insertions, 204 deletions

diff --git a/src/jtag/adapter.c b/src/jtag/adapter.c
index 2035788..3fb52a7 100644
--- a/src/jtag/adapter.c
+++ b/src/jtag/adapter.c
@@ -35,6 +35,7 @@
 #include "interface.h"
 #include "interfaces.h"
 #include <transport/transport.h>
+#include <jtag/drivers/jtag_usb_common.h>
 
 #ifdef HAVE_STRINGS_H
 #include <strings.h>
@@ -456,8 +457,55 @@ COMMAND_HANDLER(handle_adapter_khz_command)
 	return retval;
 }
 
+#ifndef HAVE_JTAG_MINIDRIVER_H
+#ifdef HAVE_LIBUSB_GET_PORT_NUMBERS
+COMMAND_HANDLER(handle_usb_location_command)
+{
+	if (CMD_ARGC == 1)
+		jtag_usb_set_location(CMD_ARGV[0]);
+
+	command_print(CMD_CTX, "adapter usb location: %s", jtag_usb_get_location());
+
+	return ERROR_OK;
+}
+#endif /* HAVE_LIBUSB_GET_PORT_NUMBERS */
+
+static const struct command_registration adapter_usb_command_handlers[] = {
+#ifdef HAVE_LIBUSB_GET_PORT_NUMBERS
+	{
+		.name = "location",
+		.handler = &handle_usb_location_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the USB bus location of the USB device",
+		.usage = "<bus>-port[.port]...",
+	},
+#endif /* HAVE_LIBUSB_GET_PORT_NUMBERS */
+	COMMAND_REGISTRATION_DONE
+};
+#endif /* MINIDRIVER */
+
+static const struct command_registration adapter_command_handlers[] = {
+#ifndef HAVE_JTAG_MINIDRIVER_H
+	{
+		.name = "usb",
+		.mode = COMMAND_ANY,
+		.help = "usb adapter command group",
+		.usage = "",
+		.chain = adapter_usb_command_handlers,
+	},
+#endif /* MINIDRIVER */
+	COMMAND_REGISTRATION_DONE
+};
+
 static const struct command_registration interface_command_handlers[] = {
 	{
+		.name = "adapter",
+		.mode = COMMAND_ANY,
+		.help = "adapter command group",
+		.usage = "",
+		.chain = adapter_command_handlers,
+	},
+	{
 		.name = "adapter_khz",
 		.handler = handle_adapter_khz_command,
 		.mode = COMMAND_ANY,
diff --git a/src/jtag/aice/aice_transport.c b/src/jtag/aice/aice_transport.c
index 57c93f2..0658318 100644
--- a/src/jtag/aice/aice_transport.c
+++ b/src/jtag/aice/aice_transport.c
@@ -340,7 +340,7 @@ aice_transport_jtag_subcommand_handlers[] = {
 	},
 	{
 		.name = "configure",
-		.mode = COMMAND_EXEC,
+		.mode = COMMAND_ANY,
 		.jim_handler = jim_jtag_configure,
 		.help = "Provide a Tcl handler for the specified "
 			"TAP event.",
diff --git a/src/jtag/aice/aice_usb.c b/src/jtag/aice/aice_usb.c
index d77b26b..f67ea7c 100644
--- a/src/jtag/aice/aice_usb.c
+++ b/src/jtag/aice/aice_usb.c
@@ -2139,11 +2139,8 @@ static int aice_usb_open(struct aice_port_param_s *param)
 
 	unsigned int aice_read_ep;
 	unsigned int aice_write_ep;
-#ifdef HAVE_LIBUSB1
+
 	jtag_libusb_choose_interface(devh, &aice_read_ep, &aice_write_ep, -1, -1, -1, LIBUSB_TRANSFER_TYPE_BULK);
-#else
-	jtag_libusb_choose_interface(devh, &aice_read_ep, &aice_write_ep, -1, -1, -1, USB_ENDPOINT_TYPE_BULK);
-#endif
 	LOG_DEBUG("aice_read_ep=0x%x, aice_write_ep=0x%x", aice_read_ep, aice_write_ep);
 
 	aice_handler.usb_read_ep = aice_read_ep;
diff --git a/src/jtag/drivers/Makefile.am b/src/jtag/drivers/Makefile.am
index ccef018..572cd24 100644
--- a/src/jtag/drivers/Makefile.am
+++ b/src/jtag/drivers/Makefile.am
@@ -19,6 +19,7 @@ DRIVERFILES =
 
 # Standard Driver: common files
 DRIVERFILES += %D%/driver.c
+DRIVERFILES += %D%/jtag_usb_common.c
 
 if USE_LIBUSB1
 DRIVERFILES += %D%/libusb1_common.c
@@ -166,6 +167,7 @@ endif
 DRIVERHEADERS = \
 	%D%/bitbang.h \
 	%D%/bitq.h \
+	%D%/jtag_usb_common.h \
 	%D%/libusb0_common.h \
 	%D%/libusb1_common.h \
 	%D%/libusb_common.h \
diff --git a/src/jtag/drivers/bitbang.c b/src/jtag/drivers/bitbang.c
index 722a5f2..da4fb33 100644
--- a/src/jtag/drivers/bitbang.c
+++ b/src/jtag/drivers/bitbang.c
@@ -30,9 +30,6 @@
 #include <jtag/interface.h>
 #include <jtag/commands.h>
 
-/* YUK! - but this is currently a global.... */
-extern struct jtag_interface *jtag_interface;
-
 /**
  * Function bitbang_stableclocks
  * issues a number of clock cycles while staying in a stable state.
diff --git a/src/jtag/drivers/cmsis_dap_usb.c b/src/jtag/drivers/cmsis_dap_usb.c
index 035cc44..bd8d0be 100644
--- a/src/jtag/drivers/cmsis_dap_usb.c
+++ b/src/jtag/drivers/cmsis_dap_usb.c
@@ -1096,7 +1096,7 @@ static int cmsis_dap_init(void)
 
 	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);
+			retval = cmsis_dap_cmd_DAP_SWJ_Pins(0, SWJ_PIN_SRST, 0, NULL);
 			if (retval != ERROR_OK)
 				return ERROR_FAIL;
 			LOG_INFO("Connecting under reset");
@@ -1670,6 +1670,30 @@ COMMAND_HANDLER(cmsis_dap_handle_info_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(cmsis_dap_handle_cmd_command)
+{
+	int retval;
+	unsigned i;
+	uint8_t *buffer = cmsis_dap_handle->packet_buffer;
+
+	buffer[0] = 0;	/* report number */
+
+	for (i = 0; i < CMD_ARGC; i++)
+		buffer[i + 1] = strtoul(CMD_ARGV[i], NULL, 16);
+
+	retval = cmsis_dap_usb_xfer(cmsis_dap_handle, CMD_ARGC + 1);
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("CMSIS-DAP command failed.");
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	LOG_INFO("Returned data %02" PRIx8 " %02" PRIx8 " %02" PRIx8 " %02" PRIx8,
+		buffer[1], buffer[2], buffer[3], buffer[4]);
+
+	return ERROR_OK;
+}
+
 COMMAND_HANDLER(cmsis_dap_handle_vid_pid_command)
 {
 	if (CMD_ARGC > MAX_USB_IDS * 2) {
@@ -1729,6 +1753,13 @@ static const struct command_registration cmsis_dap_subcommand_handlers[] = {
 		.usage = "",
 		.help = "show cmsis-dap info",
 	},
+	{
+		.name = "cmd",
+		.handler = &cmsis_dap_handle_cmd_command,
+		.mode = COMMAND_EXEC,
+		.usage = "",
+		.help = "issue cmsis-dap command",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/jtag/drivers/ft232r.c b/src/jtag/drivers/ft232r.c
index fc3d75f..e16bcdc 100644
--- a/src/jtag/drivers/ft232r.c
+++ b/src/jtag/drivers/ft232r.c
@@ -39,24 +39,9 @@
 #include <time.h>
 
 /*
- *   Bit 7 (0x80, pin 6, RI ): unused.
- *   Bit 6 (0x40, pin 10,DCD): /SYSRST output.
- *   Bit 5 (0x20, pin 9, DSR): unused.
- *   Bit 4 (0x10, pin 2, DTR): /TRST output.
- *   Bit 3 (0x08, pin 11,CTS): TMS output.
- *   Bit 2 (0x04, pin 3, RTS): TDO input.
- *   Bit 1 (0x02, pin 5, RXD): TDI output.
- *   Bit 0 (0x01, pin 1, TXD): TCK output.
- *
  * Sync bit bang mode is implemented as described in FTDI Application
  * Note AN232R-01: "Bit Bang Modes for the FT232R and FT245R".
  */
-#define TCK			(1 << 0)
-#define TDI			(1 << 1)
-#define READ_TDO		(1 << 2)
-#define TMS			(1 << 3)
-#define NTRST			(1 << 4)
-#define NSYSRST			(1 << 6)
 
 /*
  * USB endpoints.
@@ -81,7 +66,7 @@
 #define SIO_WRITE_EEPROM	0x91
 #define SIO_ERASE_EEPROM	0x92
 
-#define FT232R_BUF_SIZE		4000
+#define FT232R_BUF_SIZE_EXTRA	4096
 
 static char *ft232r_serial_desc;
 static uint16_t ft232r_vid = 0x0403; /* FTDI */
@@ -92,6 +77,33 @@ static uint8_t *ft232r_output;
 static size_t ft232r_output_len;
 
 /**
+ * FT232R GPIO bit number to RS232 name
+ */
+#define FT232R_BIT_COUNT 8
+static char *ft232r_bit_name_array[FT232R_BIT_COUNT] = {
+	"TXD", /* 0: pin 1  TCK output */
+	"RXD", /* 1: pin 5  TDI output */
+	"RTS", /* 2: pin 3  TDO input */
+	"CTS", /* 3: pin 11 TMS output */
+	"DTR", /* 4: pin 2  /TRST output */
+	"DSR", /* 5: pin 9  unused */
+	"DCD", /* 6: pin 10 /SYSRST output */
+	"RI"   /* 7: pin 6  unused */
+};
+
+static int tck_gpio; /* initialized to 0 by default */
+static int tdi_gpio = 1;
+static int tdo_gpio = 2;
+static int tms_gpio = 3;
+static int ntrst_gpio = 4;
+static int nsysrst_gpio = 6;
+static size_t ft232r_buf_size = FT232R_BUF_SIZE_EXTRA;
+/** 0xFFFF disables restore by default, after exit serial port will not work.
+ *  0x15 sets TXD RTS DTR as outputs, after exit serial port will continue to work.
+ */
+static uint16_t ft232r_restore_bitmode = 0xFFFF;
+
+/**
  * Perform sync bitbang output/input transaction.
  * Before call, an array ft232r_output[] should be filled with data to send.
  * Counter ft232r_output_len contains the number of bytes to send.
@@ -160,20 +172,35 @@ static int ft232r_send_recv(void)
 	return ERROR_OK;
 }
 
+void ft232r_increase_buf_size(size_t new_buf_size)
+{
+	uint8_t *new_buf_ptr;
+	if (new_buf_size >= ft232r_buf_size) {
+		new_buf_size += FT232R_BUF_SIZE_EXTRA;
+		new_buf_ptr = realloc(ft232r_output, new_buf_size);
+		if (new_buf_ptr != NULL) {
+			ft232r_output = new_buf_ptr;
+			ft232r_buf_size = new_buf_size;
+		}
+	}
+}
+
 /**
  * Add one TCK/TMS/TDI sample to send buffer.
  */
 static void ft232r_write(int tck, int tms, int tdi)
 {
-	unsigned out_value = NTRST | NSYSRST;
+	unsigned out_value = (1<<ntrst_gpio) | (1<<nsysrst_gpio);
 	if (tck)
-		out_value |= TCK;
+		out_value |= (1<<tck_gpio);
 	if (tms)
-		out_value |= TMS;
+		out_value |= (1<<tms_gpio);
 	if (tdi)
-		out_value |= TDI;
+		out_value |= (1<<tdi_gpio);
 
-	if (ft232r_output_len >= FT232R_BUF_SIZE) {
+	ft232r_increase_buf_size(ft232r_output_len);
+
+	if (ft232r_output_len >= ft232r_buf_size) {
 		/* FIXME: should we just execute queue here? */
 		LOG_ERROR("ft232r_write: buffer overflow");
 		return;
@@ -187,20 +214,22 @@ static void ft232r_write(int tck, int tms, int tdi)
  */
 static void ft232r_reset(int trst, int srst)
 {
-	unsigned out_value = NTRST | NSYSRST;
+	unsigned out_value = (1<<ntrst_gpio) | (1<<nsysrst_gpio);
 	LOG_DEBUG("ft232r_reset(%d,%d)", trst, srst);
 
 	if (trst == 1)
-		out_value &= ~NTRST;		/* switch /TRST low */
+		out_value &= ~(1<<ntrst_gpio);		/* switch /TRST low */
 	else if (trst == 0)
-		out_value |= NTRST;			/* switch /TRST high */
+		out_value |= (1<<ntrst_gpio);			/* switch /TRST high */
 
 	if (srst == 1)
-		out_value &= ~NSYSRST;		/* switch /SYSRST low */
+		out_value &= ~(1<<nsysrst_gpio);		/* switch /SYSRST low */
 	else if (srst == 0)
-		out_value |= NSYSRST;		/* switch /SYSRST high */
+		out_value |= (1<<nsysrst_gpio);		/* switch /SYSRST high */
+
+	ft232r_increase_buf_size(ft232r_output_len);
 
-	if (ft232r_output_len >= FT232R_BUF_SIZE) {
+	if (ft232r_output_len >= ft232r_buf_size) {
 		/* FIXME: should we just execute queue here? */
 		LOG_ERROR("ft232r_write: buffer overflow");
 		return;
@@ -236,7 +265,10 @@ static int ft232r_init(void)
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	libusb_detach_kernel_driver(adapter, 0);
+	if (ft232r_restore_bitmode == 0xFFFF) /* serial port will not be restored after jtag: */
+		libusb_detach_kernel_driver(adapter, 0);
+	else /* serial port will be restored after jtag: */
+		libusb_set_auto_detach_kernel_driver(adapter, 1); /* 1: DONT_DETACH_SIO_MODULE */
 
 	if (jtag_libusb_claim_interface(adapter, 0)) {
 		LOG_ERROR("unable to claim interface");
@@ -254,7 +286,7 @@ static int ft232r_init(void)
 	/* Sync bit bang mode. */
 	if (jtag_libusb_control_transfer(adapter,
 		LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_OUT,
-		SIO_SET_BITMODE, TCK | TDI | TMS | NTRST | NSYSRST | 0x400,
+		SIO_SET_BITMODE, (1<<tck_gpio) | (1<<tdi_gpio) | (1<<tms_gpio) | (1<<ntrst_gpio) | (1<<nsysrst_gpio) | 0x400,
 		0, 0, 0, 1000) != 0) {
 		LOG_ERROR("cannot set sync bitbang mode");
 		return ERROR_JTAG_INIT_FAILED;
@@ -279,7 +311,7 @@ static int ft232r_init(void)
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	ft232r_output = malloc(FT232R_BUF_SIZE);
+	ft232r_output = malloc(ft232r_buf_size);
 	if (ft232r_output == NULL) {
 		LOG_ERROR("Unable to allocate memory for the buffer");
 		return ERROR_JTAG_INIT_FAILED;
@@ -290,11 +322,24 @@ static int ft232r_init(void)
 
 static int ft232r_quit(void)
 {
+	/* to restore serial port: set TXD RTS DTR as outputs, others as inputs, disable sync bit bang mode. */
+	if (ft232r_restore_bitmode != 0xFFFF) {
+		if (jtag_libusb_control_transfer(adapter,
+			LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_OUT,
+			SIO_SET_BITMODE, ft232r_restore_bitmode,
+			0, 0, 0, 1000) != 0) {
+			LOG_ERROR("cannot set bitmode to restore serial port");
+		}
+	}
+
 	if (jtag_libusb_release_interface(adapter, 0) != 0)
 		LOG_ERROR("usb release interface failed");
 
 	jtag_libusb_close(adapter);
-	free(ft232r_output);
+
+	free(ft232r_output); /* free used memory */
+	ft232r_output = NULL; /* reset pointer to memory */
+	ft232r_buf_size = FT232R_BUF_SIZE_EXTRA; /* reset next initial buffer size */
 
 	return ERROR_OK;
 }
@@ -331,6 +376,27 @@ static int ft232r_khz(int khz, int *divisor)
 	return ERROR_OK;
 }
 
+static char *ft232r_bit_number_to_name(int bit)
+{
+	if (bit >= 0 && bit < FT232R_BIT_COUNT)
+		return ft232r_bit_name_array[bit];
+	return "?";
+}
+
+static int ft232r_bit_name_to_number(const char *name)
+{
+	int i;
+	if (name[0] >= '0' && name[0] <= '9' && name[1] == '\0') {
+		i = atoi(name);
+		if (i >= 0 && i < FT232R_BIT_COUNT)
+			return i;
+	}
+	for (i = 0; i < FT232R_BIT_COUNT; i++)
+		if (strcasecmp(name, ft232r_bit_name_array[i]) == 0)
+			return i;
+	return -1;
+}
+
 COMMAND_HANDLER(ft232r_handle_serial_desc_command)
 {
 	if (CMD_ARGC == 1)
@@ -357,6 +423,145 @@ COMMAND_HANDLER(ft232r_handle_vid_pid_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(ft232r_handle_jtag_nums_command)
+{
+	if (CMD_ARGC == 4) {
+		tck_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+		tms_gpio = ft232r_bit_name_to_number(CMD_ARGV[1]);
+		tdi_gpio = ft232r_bit_name_to_number(CMD_ARGV[2]);
+		tdo_gpio = ft232r_bit_name_to_number(CMD_ARGV[3]);
+	} else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (tck_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	if (tms_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	if (tdi_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	if (tdo_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R nums: TCK = %d %s, TMS = %d %s, TDI = %d %s, TDO = %d %s",
+			tck_gpio, ft232r_bit_number_to_name(tck_gpio),
+			tms_gpio, ft232r_bit_number_to_name(tms_gpio),
+			tdi_gpio, ft232r_bit_number_to_name(tdi_gpio),
+			tdo_gpio, ft232r_bit_number_to_name(tdo_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_tck_num_command)
+{
+	if (CMD_ARGC == 1)
+		tck_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (tck_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R num: TCK = %d %s", tck_gpio, ft232r_bit_number_to_name(tck_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_tms_num_command)
+{
+	if (CMD_ARGC == 1)
+		tms_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (tms_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R num: TMS = %d %s", tms_gpio, ft232r_bit_number_to_name(tms_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_tdo_num_command)
+{
+	if (CMD_ARGC == 1)
+		tdo_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (tdo_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R num: TDO = %d %s", tdo_gpio, ft232r_bit_number_to_name(tdo_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_tdi_num_command)
+{
+	if (CMD_ARGC == 1)
+		tdi_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (tdi_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R num: TDI = %d %s", tdi_gpio, ft232r_bit_number_to_name(tdi_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_trst_num_command)
+{
+	if (CMD_ARGC == 1)
+		ntrst_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (ntrst_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R num: TRST = %d %s", ntrst_gpio, ft232r_bit_number_to_name(ntrst_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_srst_num_command)
+{
+	if (CMD_ARGC == 1)
+		nsysrst_gpio = ft232r_bit_name_to_number(CMD_ARGV[0]);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (nsysrst_gpio < 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R num: SRST = %d %s", nsysrst_gpio, ft232r_bit_number_to_name(nsysrst_gpio));
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(ft232r_handle_restore_serial_command)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0], ft232r_restore_bitmode);
+	else if (CMD_ARGC != 0)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,
+			"FT232R restore serial: 0x%04X (%s)",
+			ft232r_restore_bitmode, ft232r_restore_bitmode == 0xFFFF ? "disabled" : "enabled");
+
+	return ERROR_OK;
+}
+
 static const struct command_registration ft232r_command_handlers[] = {
 	{
 		.name = "ft232r_serial_desc",
@@ -372,6 +577,62 @@ static const struct command_registration ft232r_command_handlers[] = {
 		.help = "USB VID and PID of the adapter",
 		.usage = "vid pid",
 	},
+	{
+		.name = "ft232r_jtag_nums",
+		.handler = ft232r_handle_jtag_nums_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio numbers for tck, tms, tdi, tdo. (in that order)",
+		.usage = "<0-7|TXD-RI> <0-7|TXD-RI> <0-7|TXD-RI> <0-7|TXD-RI>",
+	},
+	{
+		.name = "ft232r_tck_num",
+		.handler = ft232r_handle_tck_num_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tck.",
+		.usage = "<0-7|TXD|RXD|RTS|CTS|DTR|DSR|DCD|RI>",
+	},
+	{
+		.name = "ft232r_tms_num",
+		.handler = ft232r_handle_tms_num_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tms.",
+		.usage = "<0-7|TXD|RXD|RTS|CTS|DTR|DSR|DCD|RI>",
+	},
+	{
+		.name = "ft232r_tdo_num",
+		.handler = ft232r_handle_tdo_num_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tdo.",
+		.usage = "<0-7|TXD|RXD|RTS|CTS|DTR|DSR|DCD|RI>",
+	},
+	{
+		.name = "ft232r_tdi_num",
+		.handler = ft232r_handle_tdi_num_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tdi.",
+		.usage = "<0-7|TXD|RXD|RTS|CTS|DTR|DSR|DCD|RI>",
+	},
+	{
+		.name = "ft232r_srst_num",
+		.handler = ft232r_handle_srst_num_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for srst.",
+		.usage = "<0-7|TXD|RXD|RTS|CTS|DTR|DSR|DCD|RI>",
+	},
+	{
+		.name = "ft232r_trst_num",
+		.handler = ft232r_handle_trst_num_command,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for trst.",
+		.usage = "<0-7|TXD|RXD|RTS|CTS|DTR|DSR|DCD|RI>",
+	},
+	{
+		.name = "ft232r_restore_serial",
+		.handler = ft232r_handle_restore_serial_command,
+		.mode = COMMAND_CONFIG,
+		.help = "bitmode control word that restores serial port.",
+		.usage = "bitmode_control_word",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -553,7 +814,7 @@ static void syncbb_scan(bool ir_scan, enum scan_type type, uint8_t *buffer, int
 			int bcval = 1 << (bit_cnt % 8);
 			int val = ft232r_output[bit0_index + bit_cnt*2 + 1];
 
-			if (val & READ_TDO)
+			if (val & (1<<tdo_gpio))
 				buffer[bytec] |= bcval;
 			else
 				buffer[bytec] &= ~bcval;
diff --git a/src/jtag/drivers/imx_gpio.c b/src/jtag/drivers/imx_gpio.c
index 2a822af..f42692c 100644
--- a/src/jtag/drivers/imx_gpio.c
+++ b/src/jtag/drivers/imx_gpio.c
@@ -160,10 +160,10 @@ static int imx_gpio_swd_write(int tck, int tms, int tdi)
 static int imx_gpio_reset(int trst, int srst)
 {
 	if (trst_gpio != -1)
-		trst ? gpio_set(trst_gpio) : gpio_clear(trst_gpio);
+		trst ? gpio_clear(trst_gpio) : gpio_set(trst_gpio);
 
 	if (srst_gpio != -1)
-		srst ? gpio_set(srst_gpio) : gpio_clear(srst_gpio);
+		srst ? gpio_clear(srst_gpio) : gpio_set(srst_gpio);
 
 	return ERROR_OK;
 }
diff --git a/src/jtag/drivers/jlink.c b/src/jtag/drivers/jlink.c
index e74965e..1eae827 100644
--- a/src/jtag/drivers/jlink.c
+++ b/src/jtag/drivers/jlink.c
@@ -2130,7 +2130,7 @@ skip:
 static void jlink_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data, uint32_t ap_delay_clk)
 {
 	uint8_t data_parity_trn[DIV_ROUND_UP(32 + 1, 8)];
-	if (tap_length + 46 + 8 + ap_delay_clk >= sizeof(tdi_buffer) * 8 ||
+	if (tap_length + 46 + 8 + ap_delay_clk >= swd_buffer_size * 8 ||
 	    pending_scan_results_length == MAX_PENDING_SCAN_RESULTS) {
 		/* Not enough room in the queue. Run the queue. */
 		queued_retval = jlink_swd_run_queue();
diff --git a/src/jtag/drivers/jtag_usb_common.c b/src/jtag/drivers/jtag_usb_common.c
new file mode 100644
index 0000000..637e6c7
--- /dev/null
+++ b/src/jtag/drivers/jtag_usb_common.c
@@ -0,0 +1,85 @@
+/*
+ * SPDX-License-Identifier: GPL-2.0+
+ * Copyright (c) 2018 Pengutronix, Oleksij Rempel <kernel@pengutronix.de>
+ */
+
+#include <helper/log.h>
+
+#include "jtag_usb_common.h"
+
+static char *jtag_usb_location;
+/*
+ * 1 char: bus
+ * 2 * 7 chars: max 7 ports
+ * 1 char: test for overflow
+ * ------
+ * 16 chars
+ */
+#define JTAG_USB_MAX_LOCATION_LENGHT	16
+
+void jtag_usb_set_location(const char *location)
+{
+	if (strnlen(location, JTAG_USB_MAX_LOCATION_LENGHT) ==
+	    JTAG_USB_MAX_LOCATION_LENGHT)
+		LOG_WARNING("usb location string is too long!!\n");
+
+	if (jtag_usb_location)
+		free(jtag_usb_location);
+
+	jtag_usb_location = strndup(location, JTAG_USB_MAX_LOCATION_LENGHT);
+}
+
+const char *jtag_usb_get_location(void)
+{
+	return jtag_usb_location;
+}
+
+bool jtag_usb_location_equal(uint8_t dev_bus, uint8_t *port_path,
+			     size_t path_len)
+{
+	size_t path_step, string_lengh;
+	char *ptr, *loc;
+	bool equal = false;
+
+	/* strtok need non const char */
+	loc = strndup(jtag_usb_get_location(), JTAG_USB_MAX_LOCATION_LENGHT);
+	string_lengh = strnlen(loc, JTAG_USB_MAX_LOCATION_LENGHT);
+
+	ptr = strtok(loc, "-");
+	if (ptr == NULL) {
+		LOG_WARNING("no '-' in usb path\n");
+		goto done;
+	}
+
+	string_lengh -= 1;
+	/* check bus mismatch */
+	if (atoi(ptr) != dev_bus)
+		goto done;
+
+	path_step = 0;
+	while (path_step < path_len) {
+		ptr = strtok(NULL, ".");
+
+		/* no more tokens in path */
+		if (ptr == NULL)
+			break;
+
+		/* path mismatch at some step */
+		if (path_step < path_len && atoi(ptr) != port_path[path_step])
+			break;
+
+		path_step++;
+		string_lengh -= 2;
+	};
+
+	/* walked the full path, all elements match */
+	if (path_step == path_len && !string_lengh)
+		equal = true;
+	else
+		LOG_WARNING("excluded by device path option: %s\n",
+			    jtag_usb_get_location());
+
+done:
+	free(loc);
+	return equal;
+}
diff --git a/src/jtag/drivers/jtag_usb_common.h b/src/jtag/drivers/jtag_usb_common.h
new file mode 100644
index 0000000..8c03742
--- /dev/null
+++ b/src/jtag/drivers/jtag_usb_common.h
@@ -0,0 +1,14 @@
+/*
+ * SPDX-License-Identifier: GPL-2.0+
+ * Copyright (c) 2018 Pengutronix, Oleksij Rempel <kernel@pengutronix.de>
+ */
+
+#ifndef OPENOCD_JTAG_USB_COMMON_H
+#define OPENOCD_JTAG_USB_COMMON_H
+
+void jtag_usb_set_location(const char *location);
+const char *jtag_usb_get_location(void);
+bool jtag_usb_location_equal(uint8_t dev_bus, uint8_t *port_path,
+			     size_t path_len);
+
+#endif /* OPENOCD_JTAG_USB_COMMON_H */
diff --git a/src/jtag/drivers/jtag_vpi.c b/src/jtag/drivers/jtag_vpi.c
index 1a42b3a..35c7031 100644
--- a/src/jtag/drivers/jtag_vpi.c
+++ b/src/jtag/drivers/jtag_vpi.c
@@ -29,6 +29,10 @@
 #include <arpa/inet.h>
 #endif
 
+#ifndef _WIN32
+#include <netinet/tcp.h>
+#endif
+
 #define NO_TAP_SHIFT	0
 #define TAP_SHIFT	1
 
@@ -368,6 +372,8 @@ static int jtag_vpi_execute_queue(void)
 
 static int jtag_vpi_init(void)
 {
+	int flag = 1;
+
 	sockfd = socket(AF_INET, SOCK_STREAM, 0);
 	if (sockfd < 0) {
 		LOG_ERROR("Could not create socket");
@@ -395,6 +401,13 @@ static int jtag_vpi_init(void)
 		return ERROR_COMMAND_CLOSE_CONNECTION;
 	}
 
+	if (serv_addr.sin_addr.s_addr == htonl(INADDR_LOOPBACK)) {
+		/* This increases performance drematically for local
+		 * connections, which is the most likely arrangement
+		 * for a VPI connection. */
+		setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(int));
+	}
+
 	LOG_INFO("Connection to %s : %u succeed", server_address, server_port);
 
 	return ERROR_OK;
diff --git a/src/jtag/drivers/libusb0_common.c b/src/jtag/drivers/libusb0_common.c
index 1825543..14a8b61 100644
--- a/src/jtag/drivers/libusb0_common.c
+++ b/src/jtag/drivers/libusb0_common.c
@@ -67,7 +67,8 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		const char *serial,
 		struct jtag_libusb_device_handle **out)
 {
-	int retval = -ENODEV;
+	int retval = ERROR_FAIL;
+	bool serial_mismatch = false;
 	struct jtag_libusb_device_handle *libusb_handle;
 	usb_init();
 
@@ -83,21 +84,27 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 
 			libusb_handle = usb_open(dev);
 			if (NULL == libusb_handle) {
-				retval = -errno;
+				LOG_ERROR("usb_open() failed with %s", usb_strerror());
 				continue;
 			}
 
 			/* Device must be open to use libusb_get_string_descriptor_ascii. */
 			if (serial != NULL &&
 					!string_descriptor_equal(libusb_handle, dev->descriptor.iSerialNumber, serial)) {
+				serial_mismatch = true;
 				usb_close(libusb_handle);
 				continue;
 			}
 			*out = libusb_handle;
-			retval = 0;
+			retval = ERROR_OK;
+			serial_mismatch = false;
 			break;
 		}
 	}
+
+	if (serial_mismatch)
+		LOG_INFO("No device matches the serial string");
+
 	return retval;
 }
 
diff --git a/src/jtag/drivers/libusb0_common.h b/src/jtag/drivers/libusb0_common.h
index baa9e3c..676f43a 100644
--- a/src/jtag/drivers/libusb0_common.h
+++ b/src/jtag/drivers/libusb0_common.h
@@ -38,6 +38,7 @@
 #define LIBUSB_RECIPIENT_DEVICE			USB_RECIP_DEVICE
 #define LIBUSB_ENDPOINT_OUT				USB_ENDPOINT_OUT
 #define LIBUSB_ENDPOINT_IN				USB_ENDPOINT_IN
+#define LIBUSB_TRANSFER_TYPE_BULK		USB_ENDPOINT_TYPE_BULK
 
 static inline int jtag_libusb_claim_interface(jtag_libusb_device_handle *devh,
 				       int iface)
diff --git a/src/jtag/drivers/libusb1_common.c b/src/jtag/drivers/libusb1_common.c
index a1db86f..d96ac76 100644
--- a/src/jtag/drivers/libusb1_common.c
+++ b/src/jtag/drivers/libusb1_common.c
@@ -20,8 +20,15 @@
 #ifdef HAVE_CONFIG_H
 #include "config.h"
 #endif
-#include "log.h"
+#include <jtag/drivers/jtag_usb_common.h>
 #include "libusb1_common.h"
+#include "log.h"
+
+/*
+ * comment from libusb:
+ * As per the USB 3.0 specs, the current maximum limit for the depth is 7.
+ */
+#define MAX_USB_PORTS	7
 
 static struct libusb_context *jtag_libusb_context; /**< Libusb context **/
 static libusb_device **devs; /**< The usb device list **/
@@ -38,6 +45,31 @@ static bool jtag_libusb_match(struct libusb_device_descriptor *dev_desc,
 	return false;
 }
 
+#ifdef HAVE_LIBUSB_GET_PORT_NUMBERS
+static bool jtag_libusb_location_equal(libusb_device *device)
+{
+	uint8_t port_path[MAX_USB_PORTS];
+	uint8_t dev_bus;
+	int path_len;
+
+	path_len = libusb_get_port_numbers(device, port_path, MAX_USB_PORTS);
+	if (path_len == LIBUSB_ERROR_OVERFLOW) {
+		LOG_WARNING("cannot determine path to usb device! (more than %i ports in path)\n",
+			MAX_USB_PORTS);
+		return false;
+	}
+	dev_bus = libusb_get_bus_number(device);
+
+	return jtag_usb_location_equal(dev_bus, port_path, path_len);
+}
+#else /* HAVE_LIBUSB_GET_PORT_NUMBERS */
+static bool jtag_libusb_location_equal(libusb_device *device)
+{
+	return true;
+}
+#endif /* HAVE_LIBUSB_GET_PORT_NUMBERS */
+
+
 /* Returns true if the string descriptor indexed by str_index in device matches string */
 static bool string_descriptor_equal(libusb_device_handle *device, uint8_t str_index,
 									const char *string)
@@ -72,6 +104,7 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 {
 	int cnt, idx, errCode;
 	int retval = ERROR_FAIL;
+	bool serial_mismatch = false;
 	struct jtag_libusb_device_handle *libusb_handle = NULL;
 
 	if (libusb_init(&jtag_libusb_context) < 0)
@@ -88,6 +121,9 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		if (!jtag_libusb_match(&dev_desc, vids, pids))
 			continue;
 
+		if (jtag_usb_get_location() && !jtag_libusb_location_equal(devs[idx]))
+			continue;
+
 		errCode = libusb_open(devs[idx], &libusb_handle);
 
 		if (errCode) {
@@ -99,6 +135,7 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		/* Device must be open to use libusb_get_string_descriptor_ascii. */
 		if (serial != NULL &&
 				!string_descriptor_equal(libusb_handle, dev_desc.iSerialNumber, serial)) {
+			serial_mismatch = true;
 			libusb_close(libusb_handle);
 			continue;
 		}
@@ -106,10 +143,15 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[],
 		/* Success. */
 		*out = libusb_handle;
 		retval = ERROR_OK;
+		serial_mismatch = false;
 		break;
 	}
 	if (cnt >= 0)
 		libusb_free_device_list(devs, 1);
+
+	if (serial_mismatch)
+		LOG_INFO("No device matches the serial string");
+
 	return retval;
 }
 
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index d9ca53e..1a3cc61 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -41,6 +41,10 @@
 
 #include "libusb_common.h"
 
+#ifdef HAVE_LIBUSB1
+#define USE_LIBUSB_ASYNCIO
+#endif
+
 #define ENDPOINT_IN  0x80
 #define ENDPOINT_OUT 0x00
 
@@ -63,10 +67,19 @@
 #define STLINK_V1_PID         (0x3744)
 #define STLINK_V2_PID         (0x3748)
 #define STLINK_V2_1_PID       (0x374B)
+#define STLINK_V2_1_NO_MSD_PID  (0x3752)
+#define STLINK_V3_USBLOADER_PID (0x374D)
+#define STLINK_V3E_PID          (0x374E)
+#define STLINK_V3S_PID          (0x374F)
+#define STLINK_V3_2VCP_PID      (0x3753)
 
-/* the current implementation of the stlink limits
- * 8bit read/writes to max 64 bytes. */
+/*
+ * ST-Link/V1, ST-Link/V2 and ST-Link/V2.1 are full-speed USB devices and
+ * this limits the bulk packet size and the 8bit read/writes to max 64 bytes.
+ * STLINK-V3 is a high speed USB 2.0 and the limit is 512 bytes.
+ */
 #define STLINK_MAX_RW8		(64)
+#define STLINKV3_MAX_RW8	(512)
 
 /* "WAIT" responses will be retried (with exponential backoff) at
  * most this many times before failing to caller.
@@ -76,6 +89,7 @@
 enum stlink_jtag_api_version {
 	STLINK_JTAG_API_V1 = 1,
 	STLINK_JTAG_API_V2,
+	STLINK_JTAG_API_V3,
 };
 
 /** */
@@ -86,8 +100,10 @@ struct stlink_usb_version {
 	int jtag;
 	/** */
 	int swim;
-	/** highest supported jtag api version */
-	enum stlink_jtag_api_version jtag_api_max;
+	/** jtag api version supported */
+	enum stlink_jtag_api_version jtag_api;
+	/** one bit for each feature supported. See macros STLINK_F_* */
+	uint32_t flags;
 };
 
 /** */
@@ -120,8 +136,6 @@ struct stlink_usb_handle_s {
 	uint16_t vid;
 	/** */
 	uint16_t pid;
-	/** this is the currently used jtag api */
-	enum stlink_jtag_api_version jtag_api;
 	/** */
 	struct {
 		/** whether SWO tracing is enabled or not */
@@ -204,7 +218,6 @@ struct stlink_usb_handle_s {
 #define STLINK_SWIM_READMEM                0x0b
 #define STLINK_SWIM_READBUF                0x0c
 
-#define STLINK_DEBUG_ENTER_JTAG            0x00
 #define STLINK_DEBUG_GETSTATUS             0x01
 #define STLINK_DEBUG_FORCEDEBUG            0x02
 #define STLINK_DEBUG_APIV1_RESETSYS        0x03
@@ -222,8 +235,9 @@ struct stlink_usb_handle_s {
 #define STLINK_DEBUG_APIV1_WRITEDEBUGREG   0x0f
 #define STLINK_DEBUG_APIV1_SETWATCHPOINT   0x10
 
-#define STLINK_DEBUG_ENTER_JTAG            0x00
-#define STLINK_DEBUG_ENTER_SWD             0xa3
+#define STLINK_DEBUG_ENTER_JTAG_RESET      0x00
+#define STLINK_DEBUG_ENTER_SWD_NO_RESET    0xa3
+#define STLINK_DEBUG_ENTER_JTAG_NO_RESET   0xa4
 
 #define STLINK_DEBUG_APIV1_ENTER           0x20
 #define STLINK_DEBUG_EXIT                  0x21
@@ -241,10 +255,21 @@ struct stlink_usb_handle_s {
 #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
 #define STLINK_DEBUG_APIV2_DRIVE_NRST      0x3C
 
+#define STLINK_DEBUG_APIV2_GETLASTRWSTATUS2 0x3E
+
 #define STLINK_DEBUG_APIV2_START_TRACE_RX  0x40
 #define STLINK_DEBUG_APIV2_STOP_TRACE_RX   0x41
 #define STLINK_DEBUG_APIV2_GET_TRACE_NB    0x42
 #define STLINK_DEBUG_APIV2_SWD_SET_FREQ    0x43
+#define STLINK_DEBUG_APIV2_JTAG_SET_FREQ   0x44
+
+#define STLINK_DEBUG_APIV2_READMEM_16BIT   0x47
+#define STLINK_DEBUG_APIV2_WRITEMEM_16BIT  0x48
+
+#define STLINK_APIV3_SET_COM_FREQ           0x61
+#define STLINK_APIV3_GET_COM_FREQ           0x62
+
+#define STLINK_APIV3_GET_VERSION_EX         0xFB
 
 #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW   0x00
 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
@@ -252,7 +277,8 @@ struct stlink_usb_handle_s {
 
 #define STLINK_TRACE_SIZE               4096
 #define STLINK_TRACE_MAX_HZ             2000000
-#define STLINK_TRACE_MIN_VERSION        13
+
+#define STLINK_V3_MAX_FREQ_NB               10
 
 /** */
 enum stlink_mode {
@@ -267,10 +293,26 @@ enum stlink_mode {
 #define REQUEST_SENSE        0x03
 #define REQUEST_SENSE_LENGTH 18
 
-static const struct {
+/*
+ * Map the relevant features, quirks and workaround for specific firmware
+ * version of stlink
+ */
+#define STLINK_F_HAS_TRACE              (1UL << 0)
+#define STLINK_F_HAS_SWD_SET_FREQ       (1UL << 1)
+#define STLINK_F_HAS_JTAG_SET_FREQ      (1UL << 2)
+#define STLINK_F_HAS_MEM_16BIT          (1UL << 3)
+#define STLINK_F_HAS_GETLASTRWSTATUS2   (1UL << 4)
+
+/* aliases */
+#define STLINK_F_HAS_TARGET_VOLT        STLINK_F_HAS_TRACE
+
+struct speed_map {
 	int speed;
 	int speed_divisor;
-} stlink_khz_to_speed_map[] = {
+};
+
+/* SWD clock speed */
+static const struct speed_map stlink_khz_to_speed_map_swd[] = {
 	{4000, 0},
 	{1800, 1}, /* default */
 	{1200, 2},
@@ -285,10 +327,189 @@ static const struct {
 	{5,  798}
 };
 
+/* JTAG clock speed */
+static const struct speed_map stlink_khz_to_speed_map_jtag[] = {
+	{18000, 2},
+	{9000,  4},
+	{4500,  8},
+	{2250, 16},
+	{1125, 32}, /* default */
+	{562,  64},
+	{281, 128},
+	{140, 256}
+};
+
 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
 static int stlink_swim_status(void *handle);
 
 /** */
+static unsigned int stlink_usb_block(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	if (h->version.stlink == 3)
+		return STLINKV3_MAX_RW8;
+	else
+		return STLINK_MAX_RW8;
+}
+
+
+
+#ifdef USE_LIBUSB_ASYNCIO
+
+static LIBUSB_CALL void sync_transfer_cb(struct libusb_transfer *transfer)
+{
+	int *completed = transfer->user_data;
+	*completed = 1;
+	/* caller interprets result and frees transfer */
+}
+
+
+static void sync_transfer_wait_for_completion(struct libusb_transfer *transfer)
+{
+	int r, *completed = transfer->user_data;
+
+	/* Assuming a single libusb context exists.  There no existing interface into this
+	 * module to pass a libusb context.
+	 */
+	struct libusb_context *ctx = NULL;
+
+	while (!*completed) {
+		r = libusb_handle_events_completed(ctx, completed);
+		if (r < 0) {
+			if (r == LIBUSB_ERROR_INTERRUPTED)
+				continue;
+			libusb_cancel_transfer(transfer);
+			continue;
+		}
+	}
+}
+
+
+static int transfer_error_status(const struct libusb_transfer *transfer)
+{
+	int r = 0;
+
+	switch (transfer->status) {
+		case LIBUSB_TRANSFER_COMPLETED:
+			r = 0;
+			break;
+		case LIBUSB_TRANSFER_TIMED_OUT:
+			r = LIBUSB_ERROR_TIMEOUT;
+			break;
+		case LIBUSB_TRANSFER_STALL:
+			r = LIBUSB_ERROR_PIPE;
+			break;
+		case LIBUSB_TRANSFER_OVERFLOW:
+			r = LIBUSB_ERROR_OVERFLOW;
+			break;
+		case LIBUSB_TRANSFER_NO_DEVICE:
+			r = LIBUSB_ERROR_NO_DEVICE;
+			break;
+		case LIBUSB_TRANSFER_ERROR:
+		case LIBUSB_TRANSFER_CANCELLED:
+			r = LIBUSB_ERROR_IO;
+			break;
+		default:
+			r = LIBUSB_ERROR_OTHER;
+			break;
+	}
+
+	return r;
+}
+
+struct jtag_xfer {
+	int ep;
+	uint8_t *buf;
+	size_t size;
+	/* Internal */
+	int retval;
+	int completed;
+	size_t transfer_size;
+	struct libusb_transfer *transfer;
+};
+
+static int jtag_libusb_bulk_transfer_n(
+		jtag_libusb_device_handle * dev_handle,
+		struct jtag_xfer *transfers,
+		size_t n_transfers,
+		int timeout)
+{
+	int retval = 0;
+	int returnval = ERROR_OK;
+
+
+	for (size_t i = 0; i < n_transfers; ++i) {
+		transfers[i].retval = 0;
+		transfers[i].completed = 0;
+		transfers[i].transfer_size = 0;
+		transfers[i].transfer = libusb_alloc_transfer(0);
+
+		if (transfers[i].transfer == NULL) {
+			for (size_t j = 0; j < i; ++j)
+				libusb_free_transfer(transfers[j].transfer);
+
+			LOG_DEBUG("ERROR, failed to alloc usb transfers");
+			for (size_t k = 0; k < n_transfers; ++k)
+				transfers[k].retval = LIBUSB_ERROR_NO_MEM;
+			return ERROR_FAIL;
+		}
+	}
+
+	for (size_t i = 0; i < n_transfers; ++i) {
+		libusb_fill_bulk_transfer(
+				transfers[i].transfer,
+				dev_handle,
+				transfers[i].ep, transfers[i].buf, transfers[i].size,
+				sync_transfer_cb, &transfers[i].completed, timeout);
+		transfers[i].transfer->type = LIBUSB_TRANSFER_TYPE_BULK;
+
+		retval = libusb_submit_transfer(transfers[i].transfer);
+		if (retval < 0) {
+			LOG_DEBUG("ERROR, failed to submit transfer %zu, error %d", i, retval);
+
+			/* Probably no point continuing to submit transfers once a submission fails.
+			 * As a result, tag all remaining transfers as errors.
+			 */
+			for (size_t j = i; j < n_transfers; ++j)
+				transfers[j].retval = retval;
+
+			returnval = ERROR_FAIL;
+			break;
+		}
+	}
+
+	/* Wait for every submitted USB transfer to complete.
+	*/
+	for (size_t i = 0; i < n_transfers; ++i) {
+		if (transfers[i].retval == 0) {
+			sync_transfer_wait_for_completion(transfers[i].transfer);
+
+			retval = transfer_error_status(transfers[i].transfer);
+			if (retval) {
+				returnval = ERROR_FAIL;
+				transfers[i].retval = retval;
+				LOG_DEBUG("ERROR, transfer %zu failed, error %d", i, retval);
+			} else {
+				/* Assuming actual_length is only valid if there is no transfer error.
+				 */
+				transfers[i].transfer_size = transfers[i].transfer->actual_length;
+			}
+		}
+
+		libusb_free_transfer(transfers[i].transfer);
+		transfers[i].transfer = NULL;
+	}
+
+	return returnval;
+}
+
+#endif
+
+
+/** */
 static int stlink_usb_xfer_v1_get_status(void *handle)
 {
 	struct stlink_usb_handle_s *h = handle;
@@ -321,7 +542,45 @@ static int stlink_usb_xfer_v1_get_status(void *handle)
 	return ERROR_OK;
 }
 
-/** */
+#ifdef USE_LIBUSB_ASYNCIO
+static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	size_t n_transfers = 0;
+	struct jtag_xfer transfers[2];
+
+	memset(transfers, 0, sizeof(transfers));
+
+	transfers[0].ep = h->tx_ep;
+	transfers[0].buf = h->cmdbuf;
+	transfers[0].size = cmdsize;
+
+	++n_transfers;
+
+	if (h->direction == h->tx_ep && size) {
+		transfers[1].ep = h->tx_ep;
+		transfers[1].buf = (uint8_t *)buf;
+		transfers[1].size = size;
+
+		++n_transfers;
+	} else if (h->direction == h->rx_ep && size) {
+		transfers[1].ep = h->rx_ep;
+		transfers[1].buf = (uint8_t *)buf;
+		transfers[1].size = size;
+
+		++n_transfers;
+	}
+
+	return jtag_libusb_bulk_transfer_n(
+			h->fd,
+			transfers,
+			n_transfers,
+			STLINK_WRITE_TIMEOUT);
+}
+#else
 static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int size)
 {
 	struct stlink_usb_handle_s *h = handle;
@@ -349,6 +608,7 @@ static int stlink_usb_xfer_rw(void *handle, int cmdsize, const uint8_t *buf, int
 
 	return ERROR_OK;
 }
+#endif
 
 /** */
 static int stlink_usb_xfer_v1_get_sense(void *handle)
@@ -438,7 +698,7 @@ static int stlink_usb_error_check(void *handle)
 	}
 
 	/* TODO: no error checking yet on api V1 */
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		h->databuf[0] = STLINK_DEBUG_ERR_OK;
 
 	switch (h->databuf[0]) {
@@ -526,7 +786,9 @@ static int stlink_cmd_allow_retry(void *handle, const uint8_t *buf, int size)
 
 		res = stlink_usb_error_check(handle);
 		if (res == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
-			usleep((1<<retries++) * 1000);
+			useconds_t delay_us = (1<<retries++) * 1000;
+			LOG_DEBUG("stlink_cmd_allow_retry ERROR_WAIT, retry %d, delaying %u microseconds", retries, delay_us);
+			usleep(delay_us);
 			continue;
 		}
 		return res;
@@ -540,7 +802,7 @@ static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
 
 	assert(handle != NULL);
 
-	assert(h->version.stlink >= 2);
+	assert(h->version.flags & STLINK_F_HAS_TRACE);
 
 	if (jtag_libusb_bulk_read(h->fd, h->trace_ep, (char *)buf,
 			size, STLINK_READ_TIMEOUT) != size) {
@@ -602,7 +864,11 @@ static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t siz
 static int stlink_usb_version(void *handle)
 {
 	int res;
-	uint16_t v;
+	uint32_t flags;
+	uint16_t version;
+	uint8_t v, x, y, jtag, swim, msd, bridge = 0;
+	char v_str[5 * (1 + 3) + 1]; /* VvJjMmBbSs */
+	char *p;
 	struct stlink_usb_handle_s *h = handle;
 
 	assert(handle != NULL);
@@ -616,27 +882,131 @@ static int stlink_usb_version(void *handle)
 	if (res != ERROR_OK)
 		return res;
 
-	v = (h->databuf[0] << 8) | h->databuf[1];
+	version = be_to_h_u16(h->databuf);
+	v = (version >> 12) & 0x0f;
+	x = (version >> 6) & 0x3f;
+	y = version & 0x3f;
+
+	h->vid = le_to_h_u16(h->databuf + 2);
+	h->pid = le_to_h_u16(h->databuf + 4);
+
+	switch (h->pid) {
+	case STLINK_V2_1_PID:
+	case STLINK_V2_1_NO_MSD_PID:
+		if ((x <= 22 && y == 7) || (x >= 25 && y >= 7 && y <= 12)) {
+			/* MxSy : STM8 V2.1 - SWIM only */
+			msd = x;
+			swim = y;
+			jtag = 0;
+		} else {
+			/* JxMy : STM32 V2.1 - JTAG/SWD only */
+			jtag = x;
+			msd = y;
+			swim = 0;
+		}
+		break;
+	default:
+		jtag = x;
+		swim = y;
+		msd = 0;
+		break;
+	}
+
+	/* STLINK-V3 requires a specific command */
+	if (v == 3 && x == 0 && y == 0) {
+		stlink_usb_init_buffer(handle, h->rx_ep, 16);
 
-	h->version.stlink = (v >> 12) & 0x0f;
-	h->version.jtag = (v >> 6) & 0x3f;
-	h->version.swim = v & 0x3f;
-	h->vid = buf_get_u32(h->databuf, 16, 16);
-	h->pid = buf_get_u32(h->databuf, 32, 16);
+		h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_VERSION_EX;
 
-	/* set the supported jtag api version
-	 * API V2 is supported since JTAG V11
-	 */
-	if (h->version.jtag >= 11)
-		h->version.jtag_api_max = STLINK_JTAG_API_V2;
-	else
-		h->version.jtag_api_max = STLINK_JTAG_API_V1;
+		res = stlink_usb_xfer(handle, h->databuf, 12);
+		if (res != ERROR_OK)
+			return res;
+
+		v = h->databuf[0];
+		swim = h->databuf[1];
+		jtag = h->databuf[2];
+		msd  = h->databuf[3];
+		bridge = h->databuf[4];
+		h->vid = le_to_h_u16(h->databuf + 8);
+		h->pid = le_to_h_u16(h->databuf + 10);
+	}
+
+	h->version.stlink = v;
+	h->version.jtag = jtag;
+	h->version.swim = swim;
+
+	flags = 0;
+	switch (h->version.stlink) {
+	case 1:
+		/* ST-LINK/V1 from J11 switch to api-v2 (and support SWD) */
+		if (h->version.jtag >= 11)
+			h->version.jtag_api = STLINK_JTAG_API_V2;
+		else
+			h->version.jtag_api = STLINK_JTAG_API_V1;
+
+		break;
+	case 2:
+		/* all ST-LINK/V2 and ST-Link/V2.1 use api-v2 */
+		h->version.jtag_api = STLINK_JTAG_API_V2;
+
+		/* API for trace from J13 */
+		/* API for target voltage from J13 */
+		if (h->version.jtag >= 13)
+			flags |= STLINK_F_HAS_TRACE;
+
+		/* preferred API to get last R/W status from J15 */
+		if (h->version.jtag >= 15)
+			flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
+
+		/* API to set SWD frequency from J22 */
+		if (h->version.jtag >= 22)
+			flags |= STLINK_F_HAS_SWD_SET_FREQ;
+
+		/* API to set JTAG frequency from J24 */
+		if (h->version.jtag >= 24)
+			flags |= STLINK_F_HAS_JTAG_SET_FREQ;
+
+		/* API to read/write memory at 16 bit from J26 */
+		if (h->version.jtag >= 26)
+			flags |= STLINK_F_HAS_MEM_16BIT;
+
+		break;
+	case 3:
+		/* all STLINK-V3 use api-v3 */
+		h->version.jtag_api = STLINK_JTAG_API_V3;
+
+		/* STLINK-V3 is a superset of ST-LINK/V2 */
+
+		/* API for trace */
+		/* API for target voltage */
+		flags |= STLINK_F_HAS_TRACE;
+
+		/* preferred API to get last R/W status */
+		flags |= STLINK_F_HAS_GETLASTRWSTATUS2;
 
-	LOG_INFO("STLINK v%d JTAG v%d API v%d SWIM v%d VID 0x%04X PID 0x%04X",
-		h->version.stlink,
-		h->version.jtag,
-		(h->version.jtag_api_max == STLINK_JTAG_API_V1) ? 1 : 2,
-		h->version.swim,
+		/* API to read/write memory at 16 bit */
+		flags |= STLINK_F_HAS_MEM_16BIT;
+
+		break;
+	default:
+		break;
+	}
+	h->version.flags = flags;
+
+	p = v_str;
+	p += sprintf(p, "V%d", v);
+	if (jtag || !msd)
+		p += sprintf(p, "J%d", jtag);
+	if (msd)
+		p += sprintf(p, "M%d", msd);
+	if (bridge)
+		p += sprintf(p, "B%d", bridge);
+	if (swim || !msd)
+		p += sprintf(p, "S%d", swim);
+
+	LOG_INFO("STLINK %s (API v%d) VID:PID %04X:%04X",
+		v_str,
+		h->version.jtag_api,
 		h->vid,
 		h->pid);
 
@@ -648,8 +1018,8 @@ static int stlink_usb_check_voltage(void *handle, float *target_voltage)
 	struct stlink_usb_handle_s *h = handle;
 	uint32_t adc_results[2];
 
-	/* only supported by stlink/v2 and for firmware >= 13 */
-	if (h->version.stlink == 1 || h->version.jtag < 13)
+	/* no error message, simply quit with error */
+	if (!(h->version.flags & STLINK_F_HAS_TARGET_VOLT))
 		return ERROR_COMMAND_NOTFOUND;
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 8);
@@ -681,8 +1051,7 @@ static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
 
 	assert(handle != NULL);
 
-	/* only supported by stlink/v2 and for firmware >= 22 */
-	if (h->version.stlink == 1 || h->version.jtag < 22)
+	if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
 		return ERROR_COMMAND_NOTFOUND;
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
@@ -700,6 +1069,30 @@ static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor)
 	return ERROR_OK;
 }
 
+static int stlink_usb_set_jtagclk(void *handle, uint16_t clk_divisor)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
+		return ERROR_COMMAND_NOTFOUND;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 2);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_JTAG_SET_FREQ;
+	h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor);
+	h->cmdidx += 2;
+
+	int result = stlink_cmd_allow_retry(handle, h->databuf, 2);
+
+	if (result != ERROR_OK)
+		return result;
+
+	return ERROR_OK;
+}
+
 /** */
 static int stlink_usb_current_mode(void *handle, uint8_t *mode)
 {
@@ -734,7 +1127,7 @@ static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
 	 * status
 	 * TODO: we need the test on api V1 too
 	 */
-	if (h->jtag_api == STLINK_JTAG_API_V2)
+	if (h->version.jtag_api != STLINK_JTAG_API_V1)
 		rx_size = 2;
 
 	stlink_usb_init_buffer(handle, h->rx_ep, rx_size);
@@ -742,19 +1135,19 @@ static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
 	switch (type) {
 		case STLINK_MODE_DEBUG_JTAG:
 			h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-			if (h->jtag_api == STLINK_JTAG_API_V1)
+			if (h->version.jtag_api == STLINK_JTAG_API_V1)
 				h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
 			else
 				h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
-			h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG;
+			h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG_NO_RESET;
 			break;
 		case STLINK_MODE_DEBUG_SWD:
 			h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-			if (h->jtag_api == STLINK_JTAG_API_V1)
+			if (h->version.jtag_api == STLINK_JTAG_API_V1)
 				h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER;
 			else
 				h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER;
-			h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD;
+			h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD_NO_RESET;
 			break;
 		case STLINK_MODE_DEBUG_SWIM:
 			h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
@@ -1192,7 +1585,7 @@ static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val)
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEDEBUGREG;
 	else
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEDEBUGREG;
@@ -1211,7 +1604,7 @@ static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size)
 
 	assert(handle != NULL);
 
-	if (h->trace.enabled && h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
+	if (h->trace.enabled && (h->version.flags & STLINK_F_HAS_TRACE)) {
 		int res;
 
 		stlink_usb_init_buffer(handle, h->rx_ep, 10);
@@ -1284,7 +1677,7 @@ static enum target_state stlink_usb_state(void *handle)
 		h->reconnect_pending = false;
 	}
 
-	if (h->jtag_api == STLINK_JTAG_API_V2) {
+	if (h->version.jtag_api != STLINK_JTAG_API_V1) {
 		res = stlink_usb_v2_get_status(handle);
 		if (res == TARGET_UNKNOWN)
 			h->reconnect_pending = true;
@@ -1340,7 +1733,7 @@ static void stlink_usb_trace_disable(void *handle)
 
 	assert(handle != NULL);
 
-	assert(h->version.jtag >= STLINK_TRACE_MIN_VERSION);
+	assert(h->version.flags & STLINK_F_HAS_TRACE);
 
 	LOG_DEBUG("Tracing: disable");
 
@@ -1362,7 +1755,7 @@ static int stlink_usb_trace_enable(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->version.jtag >= STLINK_TRACE_MIN_VERSION) {
+	if (h->version.flags & STLINK_F_HAS_TRACE) {
 		stlink_usb_init_buffer(handle, h->rx_ep, 10);
 
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
@@ -1401,7 +1794,7 @@ static int stlink_usb_reset(void *handle)
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
 
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_RESETSYS;
 	else
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_RESETSYS;
@@ -1426,7 +1819,7 @@ static int stlink_usb_run(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->jtag_api == STLINK_JTAG_API_V2) {
+	if (h->version.jtag_api != STLINK_JTAG_API_V1) {
 		res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN);
 
 		return res;
@@ -1448,7 +1841,7 @@ static int stlink_usb_halt(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->jtag_api == STLINK_JTAG_API_V2) {
+	if (h->version.jtag_api != STLINK_JTAG_API_V1) {
 		res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN);
 
 		return res;
@@ -1469,7 +1862,7 @@ static int stlink_usb_step(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->jtag_api == STLINK_JTAG_API_V2) {
+	if (h->version.jtag_api != STLINK_JTAG_API_V1) {
 		/* TODO: this emulates the v1 api, it should really use a similar auto mask isr
 		 * that the Cortex-M3 currently does. */
 		stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN);
@@ -1496,7 +1889,7 @@ static int stlink_usb_read_regs(void *handle)
 	stlink_usb_init_buffer(handle, h->rx_ep, 84);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READALLREGS;
 	else
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS;
@@ -1517,16 +1910,16 @@ static int stlink_usb_read_reg(void *handle, int num, uint32_t *val)
 
 	assert(handle != NULL);
 
-	stlink_usb_init_buffer(handle, h->rx_ep, h->jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
+	stlink_usb_init_buffer(handle, h->rx_ep, h->version.jtag_api == STLINK_JTAG_API_V1 ? 4 : 8);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_READREG;
 	else
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG;
 	h->cmdbuf[h->cmdidx++] = num;
 
-	if (h->jtag_api == STLINK_JTAG_API_V1) {
+	if (h->version.jtag_api == STLINK_JTAG_API_V1) {
 		res = stlink_usb_xfer(handle, h->databuf, 4);
 		if (res != ERROR_OK)
 			return res;
@@ -1551,7 +1944,7 @@ static int stlink_usb_write_reg(void *handle, int num, uint32_t val)
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_WRITEREG;
 	else
 		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEREG;
@@ -1569,15 +1962,21 @@ static int stlink_usb_get_rw_status(void *handle)
 
 	assert(handle != NULL);
 
-	if (h->jtag_api == STLINK_JTAG_API_V1)
+	if (h->version.jtag_api == STLINK_JTAG_API_V1)
 		return ERROR_OK;
 
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
-	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
+	if (h->version.flags & STLINK_F_HAS_GETLASTRWSTATUS2) {
+		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS2;
 
-	res = stlink_usb_xfer(handle, h->databuf, 2);
+		res = stlink_usb_xfer(handle, h->databuf, 12);
+	} else {
+		h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS;
+
+		res = stlink_usb_xfer(handle, h->databuf, 2);
+	}
 
 	if (res != ERROR_OK)
 		return res;
@@ -1595,9 +1994,9 @@ static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len,
 
 	assert(handle != NULL);
 
-	/* max 8bit read/write is 64bytes */
-	if (len > STLINK_MAX_RW8) {
-		LOG_DEBUG("max buffer length exceeded");
+	/* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
+	if (len > stlink_usb_block(h)) {
+		LOG_DEBUG("max buffer (%d) length exceeded", stlink_usb_block(h));
 		return ERROR_FAIL;
 	}
 
@@ -1633,9 +2032,9 @@ static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
 
 	assert(handle != NULL);
 
-	/* max 8bit read/write is 64bytes */
-	if (len > STLINK_MAX_RW8) {
-		LOG_DEBUG("max buffer length exceeded");
+	/* max 8 bit read/write is 64 bytes or 512 bytes for v3 */
+	if (len > stlink_usb_block(h)) {
+		LOG_DEBUG("max buffer length (%d) exceeded", stlink_usb_block(h));
 		return ERROR_FAIL;
 	}
 
@@ -1657,6 +2056,78 @@ static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len,
 }
 
 /** */
+static int stlink_usb_read_mem16(void *handle, uint32_t addr, uint16_t len,
+			  uint8_t *buffer)
+{
+	int res;
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
+		return ERROR_COMMAND_NOTFOUND;
+
+	/* data must be a multiple of 2 and half-word aligned */
+	if (len % 2 || addr % 2) {
+		LOG_DEBUG("Invalid data alignment");
+		return ERROR_TARGET_UNALIGNED_ACCESS;
+	}
+
+	stlink_usb_init_buffer(handle, h->rx_ep, len);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READMEM_16BIT;
+	h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+	h->cmdidx += 4;
+	h_u16_to_le(h->cmdbuf+h->cmdidx, len);
+	h->cmdidx += 2;
+
+	res = stlink_usb_xfer(handle, h->databuf, len);
+
+	if (res != ERROR_OK)
+		return res;
+
+	memcpy(buffer, h->databuf, len);
+
+	return stlink_usb_get_rw_status(handle);
+}
+
+/** */
+static int stlink_usb_write_mem16(void *handle, uint32_t addr, uint16_t len,
+			   const uint8_t *buffer)
+{
+	int res;
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	if (!(h->version.flags & STLINK_F_HAS_MEM_16BIT))
+		return ERROR_COMMAND_NOTFOUND;
+
+	/* data must be a multiple of 2 and half-word aligned */
+	if (len % 2 || addr % 2) {
+		LOG_DEBUG("Invalid data alignment");
+		return ERROR_TARGET_UNALIGNED_ACCESS;
+	}
+
+	stlink_usb_init_buffer(handle, h->tx_ep, len);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_WRITEMEM_16BIT;
+	h_u32_to_le(h->cmdbuf+h->cmdidx, addr);
+	h->cmdidx += 4;
+	h_u16_to_le(h->cmdbuf+h->cmdidx, len);
+	h->cmdidx += 2;
+
+	res = stlink_usb_xfer(handle, buffer, len);
+
+	if (res != ERROR_OK)
+		return res;
+
+	return stlink_usb_get_rw_status(handle);
+}
+
+/** */
 static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len,
 			  uint8_t *buffer)
 {
@@ -1741,10 +2212,14 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 	/* calculate byte count */
 	count *= size;
 
+	/* switch to 8 bit if stlink does not support 16 bit memory read */
+	if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
+		size = 1;
+
 	while (count) {
 
-		bytes_remaining = (size == 4) ? \
-				stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
+		bytes_remaining = (size != 1) ? \
+				stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
 
 		if (count < bytes_remaining)
 			bytes_remaining = count;
@@ -1754,22 +2229,26 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 			if (retval != ERROR_OK)
 				return retval;
 		} else
-		/* the stlink only supports 8/32bit memory read/writes
-		 * honour 32bit, all others will be handled as 8bit access */
-		if (size == 4) {
+		/*
+		 * all stlink support 8/32bit memory read/writes and only from
+		 * stlink V2J26 there is support for 16 bit memory read/write.
+		 * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
+		 * as 8bit access.
+		 */
+		if (size != 1) {
 
-			/* When in jtag mode the stlink uses the auto-increment functinality.
+			/* When in jtag mode the stlink uses the auto-increment functionality.
 			 * However it expects us to pass the data correctly, this includes
 			 * alignment and any page boundaries. We already do this as part of the
 			 * adi_v5 implementation, but the stlink is a hla adapter and so this
-			 * needs implementiong manually.
+			 * needs implementing manually.
 			 * currently this only affects jtag mode, according to ST they do single
 			 * access in SWD mode - but this may change and so we do it for both modes */
 
 			/* we first need to check for any unaligned bytes */
-			if (addr % 4) {
+			if (addr & (size - 1)) {
 
-				uint32_t head_bytes = 4 - (addr % 4);
+				uint32_t head_bytes = size - (addr & (size - 1));
 				retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer);
 				if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 					usleep((1<<retries++) * 1000);
@@ -1783,8 +2262,10 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 				bytes_remaining -= head_bytes;
 			}
 
-			if (bytes_remaining % 4)
+			if (bytes_remaining & (size - 1))
 				retval = stlink_usb_read_mem(handle, addr, 1, bytes_remaining, buffer);
+			else if (size == 2)
+				retval = stlink_usb_read_mem16(handle, addr, bytes_remaining, buffer);
 			else
 				retval = stlink_usb_read_mem32(handle, addr, bytes_remaining, buffer);
 		} else
@@ -1816,10 +2297,14 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 	/* calculate byte count */
 	count *= size;
 
+	/* switch to 8 bit if stlink does not support 16 bit memory read */
+	if (size == 2 && !(h->version.flags & STLINK_F_HAS_MEM_16BIT))
+		size = 1;
+
 	while (count) {
 
-		bytes_remaining = (size == 4) ? \
-				stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8;
+		bytes_remaining = (size != 1) ? \
+				stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h);
 
 		if (count < bytes_remaining)
 			bytes_remaining = count;
@@ -1829,22 +2314,26 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 			if (retval != ERROR_OK)
 				return retval;
 		} else
-		/* the stlink only supports 8/32bit memory read/writes
-		 * honour 32bit, all others will be handled as 8bit access */
-		if (size == 4) {
+		/*
+		 * all stlink support 8/32bit memory read/writes and only from
+		 * stlink V2J26 there is support for 16 bit memory read/write.
+		 * Honour 32 bit and, if possible, 16 bit too. Otherwise, handle
+		 * as 8bit access.
+		 */
+		if (size != 1) {
 
-			/* When in jtag mode the stlink uses the auto-increment functinality.
+			/* When in jtag mode the stlink uses the auto-increment functionality.
 			 * However it expects us to pass the data correctly, this includes
 			 * alignment and any page boundaries. We already do this as part of the
 			 * adi_v5 implementation, but the stlink is a hla adapter and so this
-			 * needs implementiong manually.
+			 * needs implementing manually.
 			 * currently this only affects jtag mode, according to ST they do single
 			 * access in SWD mode - but this may change and so we do it for both modes */
 
 			/* we first need to check for any unaligned bytes */
-			if (addr % 4) {
+			if (addr & (size - 1)) {
 
-				uint32_t head_bytes = 4 - (addr % 4);
+				uint32_t head_bytes = size - (addr & (size - 1));
 				retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer);
 				if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 					usleep((1<<retries++) * 1000);
@@ -1858,8 +2347,10 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 				bytes_remaining -= head_bytes;
 			}
 
-			if (bytes_remaining % 4)
+			if (bytes_remaining & (size - 1))
 				retval = stlink_usb_write_mem(handle, addr, 1, bytes_remaining, buffer);
+			else if (size == 2)
+				retval = stlink_usb_write_mem16(handle, addr, bytes_remaining, buffer);
 			else
 				retval = stlink_usb_write_mem32(handle, addr, bytes_remaining, buffer);
 
@@ -1886,70 +2377,223 @@ static int stlink_usb_override_target(const char *targetname)
 	return !strcmp(targetname, "cortex_m");
 }
 
-static int stlink_speed(void *handle, int khz, bool query)
+static int stlink_speed_swim(void *handle, int khz, bool query)
 {
-	unsigned i;
-	int speed_index = -1;
-	int speed_diff = INT_MAX;
-	struct stlink_usb_handle_s *h = handle;
-
-	if (h && (h->transport == HL_TRANSPORT_SWIM)) {
-		/*
+	/*
 			we dont care what the khz rate is
 			we only have low and high speed...
 			before changing speed the SWIM_CSR HS bit
 			must be updated
-		 */
-		if (khz == 0)
-			stlink_swim_speed(handle, 0);
-		else
-			stlink_swim_speed(handle, 1);
-		return khz;
-	}
+	 */
+	if (khz == 0)
+		stlink_swim_speed(handle, 0);
+	else
+		stlink_swim_speed(handle, 1);
+	return khz;
+}
 
-	/* only supported by stlink/v2 and for firmware >= 22 */
-	if (h && (h->version.stlink == 1 || h->version.jtag < 22))
-		return khz;
+static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query)
+{
+	unsigned int i;
+	int speed_index = -1;
+	int speed_diff = INT_MAX;
+	int last_valid_speed = -1;
+	bool match = true;
 
-	for (i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++) {
-		if (khz == stlink_khz_to_speed_map[i].speed) {
+	for (i = 0; i < map_size; i++) {
+		if (!map[i].speed)
+			continue;
+		last_valid_speed = i;
+		if (khz == map[i].speed) {
 			speed_index = i;
 			break;
 		} else {
-			int current_diff = khz - stlink_khz_to_speed_map[i].speed;
+			int current_diff = khz - map[i].speed;
 			/* get abs value for comparison */
 			current_diff = (current_diff > 0) ? current_diff : -current_diff;
-			if ((current_diff < speed_diff) && khz >= stlink_khz_to_speed_map[i].speed) {
+			if ((current_diff < speed_diff) && khz >= map[i].speed) {
 				speed_diff = current_diff;
 				speed_index = i;
 			}
 		}
 	}
 
-	bool match = true;
-
 	if (speed_index == -1) {
 		/* this will only be here if we cannot match the slow speed.
 		 * use the slowest speed we support.*/
-		speed_index = ARRAY_SIZE(stlink_khz_to_speed_map) - 1;
+		speed_index = last_valid_speed;
 		match = false;
-	} else if (i == ARRAY_SIZE(stlink_khz_to_speed_map))
+	} else if (i == map_size)
 		match = false;
 
 	if (!match && query) {
 		LOG_INFO("Unable to match requested speed %d kHz, using %d kHz", \
-				khz, stlink_khz_to_speed_map[speed_index].speed);
+				khz, map[speed_index].speed);
 	}
 
-	if (h && !query) {
-		int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map[speed_index].speed_divisor);
+	return speed_index;
+}
+
+static int stlink_speed_swd(void *handle, int khz, bool query)
+{
+	int speed_index;
+	struct stlink_usb_handle_s *h = handle;
+
+	/* old firmware cannot change it */
+	if (!(h->version.flags & STLINK_F_HAS_SWD_SET_FREQ))
+		return khz;
+
+	speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_swd,
+		ARRAY_SIZE(stlink_khz_to_speed_map_swd), khz, query);
+
+	if (!query) {
+		int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map_swd[speed_index].speed_divisor);
 		if (result != ERROR_OK) {
 			LOG_ERROR("Unable to set adapter speed");
 			return khz;
 		}
 	}
 
-	return stlink_khz_to_speed_map[speed_index].speed;
+	return stlink_khz_to_speed_map_swd[speed_index].speed;
+}
+
+static int stlink_speed_jtag(void *handle, int khz, bool query)
+{
+	int speed_index;
+	struct stlink_usb_handle_s *h = handle;
+
+	/* old firmware cannot change it */
+	if (!(h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ))
+		return khz;
+
+	speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_jtag,
+		ARRAY_SIZE(stlink_khz_to_speed_map_jtag), khz, query);
+
+	if (!query) {
+		int result = stlink_usb_set_jtagclk(h, stlink_khz_to_speed_map_jtag[speed_index].speed_divisor);
+		if (result != ERROR_OK) {
+			LOG_ERROR("Unable to set adapter speed");
+			return khz;
+		}
+	}
+
+	return stlink_khz_to_speed_map_jtag[speed_index].speed;
+}
+
+void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size)
+{
+	unsigned int i;
+
+	LOG_DEBUG("Supported clock speeds are:");
+	for (i = 0; i < map_size; i++)
+		if (map[i].speed)
+			LOG_DEBUG("%d kHz", map[i].speed);
+}
+
+static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int i;
+
+	if (h->version.jtag_api != STLINK_JTAG_API_V3) {
+		LOG_ERROR("Unknown command");
+		return 0;
+	}
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 16);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_COM_FREQ;
+	h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
+
+	int res = stlink_usb_xfer(handle, h->databuf, 52);
+
+	int size = h->databuf[8];
+
+	if (size > STLINK_V3_MAX_FREQ_NB)
+		size = STLINK_V3_MAX_FREQ_NB;
+
+	for (i = 0; i < size; i++) {
+		map[i].speed = le_to_h_u32(&h->databuf[12 + 4 * i]);
+		map[i].speed_divisor = i;
+	}
+
+	/* set to zero all the next entries */
+	for (i = size; i < STLINK_V3_MAX_FREQ_NB; i++)
+		map[i].speed = 0;
+
+	return res;
+}
+
+static int stlink_set_com_freq(void *handle, bool is_jtag, unsigned int frequency)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	if (h->version.jtag_api != STLINK_JTAG_API_V3) {
+		LOG_ERROR("Unknown command");
+		return 0;
+	}
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 16);
+
+	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_APIV3_SET_COM_FREQ;
+	h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0;
+	h->cmdbuf[h->cmdidx++] = 0;
+
+	h_u32_to_le(&h->cmdbuf[4], frequency);
+
+	return stlink_usb_xfer(handle, h->databuf, 8);
+}
+
+static int stlink_speed_v3(void *handle, bool is_jtag, int khz, bool query)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int speed_index;
+	struct speed_map map[STLINK_V3_MAX_FREQ_NB];
+
+	stlink_get_com_freq(h, is_jtag, map);
+
+	speed_index = stlink_match_speed_map(map, ARRAY_SIZE(map), khz, query);
+
+	if (!query) {
+		int result = stlink_set_com_freq(h, is_jtag, map[speed_index].speed);
+		if (result != ERROR_OK) {
+			LOG_ERROR("Unable to set adapter speed");
+			return khz;
+		}
+	}
+	return map[speed_index].speed;
+}
+
+static int stlink_speed(void *handle, int khz, bool query)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	if (!handle)
+		return khz;
+
+	switch (h->transport) {
+	case HL_TRANSPORT_SWIM:
+		return stlink_speed_swim(handle, khz, query);
+		break;
+	case HL_TRANSPORT_SWD:
+		if (h->version.jtag_api == STLINK_JTAG_API_V3)
+			return stlink_speed_v3(handle, false, khz, query);
+		else
+			return stlink_speed_swd(handle, khz, query);
+		break;
+	case HL_TRANSPORT_JTAG:
+		if (h->version.jtag_api == STLINK_JTAG_API_V3)
+			return stlink_speed_v3(handle, true, khz, query);
+		else
+			return stlink_speed_jtag(handle, khz, query);
+		break;
+	default:
+		break;
+	}
+
+	return khz;
 }
 
 /** */
@@ -2005,7 +2649,6 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 {
 	int err, retry_count = 1;
 	struct stlink_usb_handle_s *h;
-	enum stlink_jtag_api_version api;
 
 	LOG_DEBUG("stlink_usb_open");
 
@@ -2060,9 +2703,17 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 			case STLINK_V1_PID:
 				h->version.stlink = 1;
 				h->tx_ep = STLINK_TX_EP;
-				h->trace_ep = STLINK_TRACE_EP;
+				break;
+			case STLINK_V3_USBLOADER_PID:
+			case STLINK_V3E_PID:
+			case STLINK_V3S_PID:
+			case STLINK_V3_2VCP_PID:
+				h->version.stlink = 3;
+				h->tx_ep = STLINK_V2_1_TX_EP;
+				h->trace_ep = STLINK_V2_1_TRACE_EP;
 				break;
 			case STLINK_V2_1_PID:
+			case STLINK_V2_1_NO_MSD_PID:
 				h->version.stlink = 2;
 				h->tx_ep = STLINK_V2_1_TX_EP;
 				h->trace_ep = STLINK_V2_1_TRACE_EP;
@@ -2113,6 +2764,9 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 
 	switch (h->transport) {
 		case HL_TRANSPORT_SWD:
+			if (h->version.jtag_api == STLINK_JTAG_API_V1)
+				err = ERROR_FAIL;
+			/* fall-through */
 		case HL_TRANSPORT_JTAG:
 			if (h->version.jtag == 0)
 				err = ERROR_FAIL;
@@ -2131,13 +2785,6 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 		goto error_open;
 	}
 
-	api = h->version.jtag_api_max;
-
-	LOG_INFO("using stlink api v%d", api);
-
-	/* set the used jtag api, this will default to the newest supported version */
-	h->jtag_api = api;
-
 	/* initialize the debug hardware */
 	err = stlink_usb_init_mode(h, param->connect_under_reset);
 
@@ -2157,13 +2804,23 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 		return ERROR_OK;
 	}
 
-	/* clock speed only supported by stlink/v2 and for firmware >= 22 */
-	if (h->version.stlink >= 2 && h->version.jtag >= 22) {
-		LOG_DEBUG("Supported clock speeds are:");
+	if (h->transport == HL_TRANSPORT_JTAG) {
+		if (h->version.flags & STLINK_F_HAS_JTAG_SET_FREQ) {
+			stlink_dump_speed_map(stlink_khz_to_speed_map_jtag, ARRAY_SIZE(stlink_khz_to_speed_map_jtag));
+			stlink_speed(h, param->initial_interface_speed, false);
+		}
+	} else if (h->transport == HL_TRANSPORT_SWD) {
+		if (h->version.flags & STLINK_F_HAS_SWD_SET_FREQ) {
+			stlink_dump_speed_map(stlink_khz_to_speed_map_swd, ARRAY_SIZE(stlink_khz_to_speed_map_swd));
+			stlink_speed(h, param->initial_interface_speed, false);
+		}
+	}
 
-		for (unsigned i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++)
-			LOG_DEBUG("%d kHz", stlink_khz_to_speed_map[i].speed);
+	if (h->version.jtag_api == STLINK_JTAG_API_V3) {
+		struct speed_map map[STLINK_V3_MAX_FREQ_NB];
 
+		stlink_get_com_freq(h, (h->transport == HL_TRANSPORT_JTAG), map);
+		stlink_dump_speed_map(map, ARRAY_SIZE(map));
 		stlink_speed(h, param->initial_interface_speed, false);
 	}
 
@@ -2199,7 +2856,7 @@ int stlink_config_trace(void *handle, bool enabled, enum tpiu_pin_protocol pin_p
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	if (enabled && (h->jtag_api < 2 ||
+	if (enabled && (!(h->version.flags & STLINK_F_HAS_TRACE) ||
 			pin_protocol != TPIU_PIN_PROTOCOL_ASYNC_UART)) {
 		LOG_ERROR("The attached ST-LINK version doesn't support this trace mode");
 		return ERROR_FAIL;
diff --git a/src/jtag/drivers/sysfsgpio.c b/src/jtag/drivers/sysfsgpio.c
index 5535c71..90e93f5 100644
--- a/src/jtag/drivers/sysfsgpio.c
+++ b/src/jtag/drivers/sysfsgpio.c
@@ -457,62 +457,70 @@ static const struct command_registration sysfsgpio_command_handlers[] = {
 		.handler = &sysfsgpio_handle_jtag_gpionums,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio numbers for tck, tms, tdi, tdo. (in that order)",
-		.usage = "(tck tms tdi tdo)* ",
+		.usage = "[tck tms tdi tdo]",
 	},
 	{
 		.name = "sysfsgpio_tck_num",
 		.handler = &sysfsgpio_handle_jtag_gpionum_tck,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for tck.",
+		.usage = "[tck]",
 	},
 	{
 		.name = "sysfsgpio_tms_num",
 		.handler = &sysfsgpio_handle_jtag_gpionum_tms,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for tms.",
+		.usage = "[tms]",
 	},
 	{
 		.name = "sysfsgpio_tdo_num",
 		.handler = &sysfsgpio_handle_jtag_gpionum_tdo,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for tdo.",
+		.usage = "[tdo]",
 	},
 	{
 		.name = "sysfsgpio_tdi_num",
 		.handler = &sysfsgpio_handle_jtag_gpionum_tdi,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for tdi.",
+		.usage = "[tdi]",
 	},
 	{
 		.name = "sysfsgpio_srst_num",
 		.handler = &sysfsgpio_handle_jtag_gpionum_srst,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for srst.",
+		.usage = "[srst]",
 	},
 	{
 		.name = "sysfsgpio_trst_num",
 		.handler = &sysfsgpio_handle_jtag_gpionum_trst,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for trst.",
+		.usage = "[trst]",
 	},
 	{
 		.name = "sysfsgpio_swd_nums",
 		.handler = &sysfsgpio_handle_swd_gpionums,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio numbers for swclk, swdio. (in that order)",
-		.usage = "(swclk swdio)* ",
+		.usage = "[swclk swdio]",
 	},
 	{
 		.name = "sysfsgpio_swclk_num",
 		.handler = &sysfsgpio_handle_swd_gpionum_swclk,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for swclk.",
+		.usage = "[swclk]",
 	},
 	{
 		.name = "sysfsgpio_swdio_num",
 		.handler = &sysfsgpio_handle_swd_gpionum_swdio,
 		.mode = COMMAND_CONFIG,
 		.help = "gpio number for swdio.",
+		.usage = "[swdio]",
 	},
 	COMMAND_REGISTRATION_DONE
 };
@@ -561,6 +569,8 @@ static void cleanup_all_fds(void)
 	cleanup_fd(tdo_fd, tdo_gpio);
 	cleanup_fd(trst_fd, trst_gpio);
 	cleanup_fd(srst_fd, srst_gpio);
+	cleanup_fd(swclk_fd, swclk_gpio);
+	cleanup_fd(swdio_fd, swdio_gpio);
 }
 
 static bool sysfsgpio_jtag_mode_possible(void)
diff --git a/src/jtag/drivers/usb_common.c b/src/jtag/drivers/usb_common.c
index 54be6a6..1b7602d 100644
--- a/src/jtag/drivers/usb_common.c
+++ b/src/jtag/drivers/usb_common.c
@@ -19,6 +19,7 @@
 #include "config.h"
 #endif
 #include "usb_common.h"
+#include "log.h"
 
 
 static bool jtag_usb_match(struct usb_device *dev,
@@ -45,10 +46,12 @@ int jtag_usb_open(const uint16_t vids[], const uint16_t pids[],
 				continue;
 
 			*out = usb_open(dev);
-			if (NULL == *out)
-				return -errno;
-			return 0;
+			if (NULL == *out) {
+				LOG_ERROR("usb_open() failed with %s", usb_strerror());
+				return ERROR_FAIL;
+			}
+			return ERROR_OK;
 		}
 	}
-	return -ENODEV;
+	return ERROR_FAIL;
 }
diff --git a/src/jtag/drivers/xds110.c b/src/jtag/drivers/xds110.c
index b97eef2..20b8178 100644
--- a/src/jtag/drivers/xds110.c
+++ b/src/jtag/drivers/xds110.c
@@ -29,6 +29,13 @@
 /* XDS110 USB serial number length */
 #define XDS110_SERIAL_LEN 8
 
+/* XDS110 stand-alone probe voltage supply limits */
+#define XDS110_MIN_VOLTAGE 1800
+#define XDS110_MAX_VOLTAGE 3600
+
+/* XDS110 stand-alone probe hardware ID */
+#define XDS110_STAND_ALONE_ID 0x21
+
 /* Firmware version that introduced OpenOCD support via block accesses */
 #define OCD_FIRMWARE_VERSION 0x02030011
 #define OCD_FIRMWARE_UPGRADE \
@@ -162,6 +169,7 @@
 #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 XDS_SET_SUPPLY   0x32 /* Set up stand-alone probe upply voltage */
 #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 */
@@ -219,6 +227,8 @@ struct xds110_info {
 	uint32_t delay_count;
 	/* XDS110 serial number */
 	char serial[XDS110_SERIAL_LEN + 1];
+	/* XDS110 voltage supply setting */
+	uint32_t voltage;
 	/* XDS110 firmware and hardware version */
 	uint32_t firmware;
 	uint16_t hardware;
@@ -242,6 +252,7 @@ static struct xds110_info xds110 = {
 	.speed = XDS110_MAX_TCK_SPEED,
 	.delay_count = 0,
 	.serial = {0},
+	.voltage = 0,
 	.firmware = 0,
 	.hardware = 0,
 	.txn_request_size = 0,
@@ -601,10 +612,15 @@ static bool xds_execute(uint32_t out_length, uint32_t in_length,
 			if (bytes_read != in_length) {
 				/* Unexpected amount of data returned */
 				success = false;
+				LOG_DEBUG("XDS110: command 0x%02x return %d bytes, expected %d",
+					xds110.write_payload[0], bytes_read, in_length);
 			} else {
 				/* Extract error code from return packet */
 				error = (int)xds110_get_u32(&xds110.read_payload[0]);
 				done = true;
+				if (SC_ERR_NONE != error)
+					LOG_DEBUG("XDS110: command 0x%02x returned error %d",
+						xds110.write_payload[0], error);
 			}
 		}
 	}
@@ -952,6 +968,24 @@ static bool cjtag_disconnect(void)
 	return success;
 }
 
+static bool xds_set_supply(uint32_t voltage)
+{
+	uint8_t *volts_pntr = &xds110.write_payload[XDS_OUT_LEN + 0]; /* 32-bits */
+	uint8_t *source_pntr = &xds110.write_payload[XDS_OUT_LEN + 4]; /* 8-bits */
+
+	bool success;
+
+	xds110.write_payload[0] = XDS_SET_SUPPLY;
+
+	xds110_set_u32(volts_pntr, voltage);
+	*source_pntr = (uint8_t)(0 != voltage ? 1 : 0);
+
+	success = xds_execute(XDS_OUT_LEN + 5, 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)
 {
@@ -1318,7 +1352,7 @@ static void xds110_show_info(void)
 		(((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);
+		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);
@@ -1391,6 +1425,20 @@ static int xds110_init(void)
 	}
 
 	if (success) {
+		/* Set supply voltage for stand-alone probes */
+		if (XDS110_STAND_ALONE_ID == xds110.hardware) {
+			success = xds_set_supply(xds110.voltage);
+			/* Allow time for target device to power up */
+			/* (CC32xx takes up to 1300 ms before debug is enabled) */
+			alive_sleep(1500);
+		} else if (0 != xds110.voltage) {
+			/* Voltage supply not a feature of embedded probes */
+			LOG_WARNING(
+				"XDS110: ignoring supply voltage, not supported on this probe");
+		}
+	}
+
+	if (success) {
 		success = xds_set_trst(0);
 		if (success)
 			success = xds_cycle_tck(50);
@@ -1569,6 +1617,9 @@ static void xds110_execute_reset(struct jtag_command *cmd)
 			srst = 0;
 		}
 		(void)xds_set_srst(srst);
+
+		/* Toggle TCK to trigger HIB on CC13x/CC26x devices */
+		(void)xds_cycle_tck(60000);
 	}
 }
 
@@ -1918,6 +1969,31 @@ COMMAND_HANDLER(xds110_handle_serial_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(xds110_handle_supply_voltage_command)
+{
+	uint32_t voltage = 0;
+
+	if (CMD_ARGC == 1) {
+		COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], voltage);
+		if (voltage == 0 || (voltage >= XDS110_MIN_VOLTAGE && voltage
+			<= XDS110_MAX_VOLTAGE)) {
+			/* Requested voltage is in range */
+			xds110.voltage = voltage;
+		} else {
+			LOG_ERROR("XDS110: voltage must be 0 or between %d and %d "
+				"millivolts", XDS110_MIN_VOLTAGE, XDS110_MAX_VOLTAGE);
+			return ERROR_FAIL;
+		}
+		xds110.voltage = voltage;
+	} else {
+		LOG_ERROR("XDS110: expected one argument to xds110_supply_voltage "
+			"<millivolts>");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
 static const struct command_registration xds110_subcommand_handlers[] = {
 	{
 		.name = "info",
@@ -1944,6 +2020,13 @@ static const struct command_registration xds110_command_handlers[] = {
 		.help = "set the XDS110 probe serial number",
 		.usage = "serial_string",
 	},
+	{
+		.name = "xds110_supply_voltage",
+		.handler = &xds110_handle_supply_voltage_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the XDS110 probe supply voltage",
+		.usage = "supply_voltage (millivolts)",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/jtag/swd.h b/src/jtag/swd.h
index 52f41d5..3ff4de0 100644
--- a/src/jtag/swd.h
+++ b/src/jtag/swd.h
@@ -53,16 +53,25 @@ static inline uint8_t swd_cmd(bool is_read, bool is_ap, uint8_t regnum)
 
 /* SWD_ACK_* bits are defined in <target/arm_adi_v5.h> */
 
+/*
+ * The following sequences are updated to
+ * ARM(tm) Debug Interface v5 Architecture Specification    ARM IHI 0031E
+ */
+
 /**
- * Line reset.
+ * SWD Line reset.
  *
- * Line reset is at least 50 SWCLK cycles with SWDIO driven high, followed
- * by at least one idle (low) cycle.
+ * SWD Line reset is at least 50 SWCLK cycles with SWDIO driven high,
+ * followed by at least two idle (low) cycle.
+ * Bits are stored (and transmitted) LSB-first.
  */
 static const uint8_t swd_seq_line_reset[] = {
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x03
+	/* At least 50 SWCLK cycles with SWDIO high */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	/* At least 2 idle (low) cycles */
+	0x00,
 };
-static const unsigned swd_seq_line_reset_len = 51;
+static const unsigned swd_seq_line_reset_len = 64;
 
 /**
  * JTAG-to-SWD sequence.
@@ -71,36 +80,53 @@ static const unsigned swd_seq_line_reset_len = 51;
  * high, putting either interface logic into reset state, followed by a
  * specific 16-bit sequence and finally a line reset in case the SWJ-DP was
  * already in SWD mode.
+ * Bits are stored (and transmitted) LSB-first.
  */
 static const uint8_t swd_seq_jtag_to_swd[] = {
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x7b, 0x9e,
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0f,
+	/* At least 50 TCK/SWCLK cycles with TMS/SWDIO high */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	/* Switching sequence from JTAG to SWD */
+	0x9e, 0xe7,
+	/* At least 50 TCK/SWCLK cycles with TMS/SWDIO high */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	/* At least 2 idle (low) cycles */
+	0x00,
 };
-static const unsigned swd_seq_jtag_to_swd_len = 118;
+static const unsigned swd_seq_jtag_to_swd_len = 136;
 
 /**
  * SWD-to-JTAG sequence.
  *
  * The SWD-to-JTAG sequence is at least 50 TCK/SWCLK cycles with TMS/SWDIO
  * high, putting either interface logic into reset state, followed by a
- * specific 16-bit sequence and finally at least 5 TCK cycles to put the
- * JTAG TAP in TLR.
+ * specific 16-bit sequence and finally at least 5 TCK/SWCLK cycles with
+ * TMS/SWDIO high to put the JTAG TAP in Test-Logic-Reset state.
+ * Bits are stored (and transmitted) LSB-first.
  */
 static const uint8_t swd_seq_swd_to_jtag[] = {
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf3, 0x9c, 0xff
+	/* At least 50 TCK/SWCLK cycles with TMS/SWDIO high */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	/* Switching sequence from SWD to JTAG */
+	0x3c, 0xe7,
+	/* At least 5 TCK/SWCLK cycles with TMS/SWDIO high */
+	0xff,
 };
-static const unsigned swd_seq_swd_to_jtag_len = 71;
+static const unsigned swd_seq_swd_to_jtag_len = 80;
 
 /**
  * SWD-to-dormant sequence.
  *
  * This is at least 50 SWCLK cycles with SWDIO high to put the interface
  * in reset state, followed by a specific 16-bit sequence.
+ * Bits are stored (and transmitted) LSB-first.
  */
 static const uint8_t swd_seq_swd_to_dormant[] = {
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf3, 0x8e, 0x03
+	/* At least 50 SWCLK cycles with SWDIO high */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 /* Switching sequence from SWD to dormant */
+	0xbc, 0xe3,
 };
-static const unsigned swd_seq_swd_to_dormant_len = 66;
+static const unsigned swd_seq_swd_to_dormant_len = 72;
 
 /**
  * Dormant-to-SWD sequence.
@@ -110,14 +136,82 @@ static const unsigned swd_seq_swd_to_dormant_len = 66;
  * sequence, followed by 4 TCK/SWCLK cycles with TMS/SWDIO low, followed by
  * a specific protocol-dependent activation code. For SWD the activation code
  * is an 8-bit sequence. The sequence ends with a line reset.
+ * Bits are stored (and transmitted) LSB-first.
  */
 static const uint8_t swd_seq_dormant_to_swd[] = {
+	/* At least 8 SWCLK cycles with SWDIO high */
+	0xff,
+	/* Selection alert sequence */
+	0x92, 0xf3, 0x09, 0x62, 0x95, 0x2d, 0x85, 0x86,
+	0xe9, 0xaf, 0xdd, 0xe3, 0xa2, 0x0e, 0xbc, 0x19,
+	/*
+	 * 4 SWCLK cycles with SWDIO low ...
+	 * + SWD activation code 0x1a ...
+	 * + at least 8 SWCLK cycles with SWDIO high
+	 */
+	0xa0, /* ((0x00)      & GENMASK(3, 0)) | ((0x1a << 4) & GENMASK(7, 4)) */
+	0xf1, /* ((0x1a >> 4) & GENMASK(3, 0)) | ((0xff << 4) & GENMASK(7, 4)) */
+	0xff,
+	/* At least 50 SWCLK cycles with SWDIO high */
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	/* At least 2 idle (low) cycles */
+	0x00,
+};
+static const unsigned swd_seq_dormant_to_swd_len = 224;
+
+/**
+ * JTAG-to-dormant sequence.
+ *
+ * This is at least 5 TCK cycles with TMS high to put the interface
+ * in test-logic-reset state, followed by a specific 31-bit sequence.
+ * Bits are stored (and transmitted) LSB-first.
+ */
+static const uint8_t swd_seq_jtag_to_dormant[] = {
+	/* At least 5 TCK cycles with TMS high */
+	0xff,
+	/*
+	 * Still one TCK cycle with TMS high followed by 31 bits JTAG-to-DS
+	 * select sequence 0xba, 0xbb, 0xbb, 0x33,
+	 */
+	0x75, /* ((0xff >> 7) & GENMASK(0, 0)) | ((0xba << 1) & GENMASK(7, 1)) */
+	0x77, /* ((0xba >> 7) & GENMASK(0, 0)) | ((0xbb << 1) & GENMASK(7, 1)) */
+	0x77, /* ((0xbb >> 7) & GENMASK(0, 0)) | ((0xbb << 1) & GENMASK(7, 1)) */
+	0x67, /* ((0xbb >> 7) & GENMASK(0, 0)) | ((0x33 << 1) & GENMASK(7, 1)) */
+};
+static const unsigned swd_seq_jtag_to_dormant_len = 40;
+
+/**
+ * Dormant-to-JTAG sequence.
+ *
+ * This is at least 8 TCK/SWCLK cycles with TMS/SWDIO high to abort any ongoing
+ * selection alert sequence, followed by a specific 128-bit selection alert
+ * sequence, followed by 4 TCK/SWCLK cycles with TMS/SWDIO low, followed by
+ * a specific protocol-dependent activation code. For JTAG there are two
+ * possible activation codes:
+ * - "JTAG-Serial": 12 bits 0x00, 0x00
+ * - "Arm CoreSight JTAG-DP": 8 bits 0x0a
+ * We use "JTAG-Serial" only, which seams more generic.
+ * Since the target TAP can be either in Run/Test Idle or in Test-Logic-Reset
+ * states, Arm recommends to put the TAP in Run/Test Idle using one TCK cycle
+ * with TMS low. To keep the sequence length multiple of 8, 8 TCK cycle with
+ * TMS low are sent (allowed by JTAG state machine).
+ * Bits are stored (and transmitted) LSB-first.
+ */
+static const uint8_t swd_seq_dormant_to_jtag[] = {
+	/* At least 8 TCK/SWCLK cycles with TMS/SWDIO high */
 	0xff,
+	/* Selection alert sequence */
 	0x92, 0xf3, 0x09, 0x62, 0x95, 0x2d, 0x85, 0x86,
 	0xe9, 0xaf, 0xdd, 0xe3, 0xa2, 0x0e, 0xbc, 0x19,
-	0x10, 0xfa, 0xff, 0xff, 0xff, 0xff, 0xff, 0x3f
+	/*
+	 * 4 TCK/SWCLK cycles with TMS/SWDIO low ...
+	 * + 12 bits JTAG-serial activation code 0x00, 0x00
+	 */
+	0x00, 0x00,
+	/* put the TAP in Run/Test Idle */
+	0x00,
 };
-static const unsigned swd_seq_dormant_to_swd_len = 199;
+static const unsigned swd_seq_dormant_to_jtag_len = 160;
 
 enum swd_special_seq {
 	LINE_RESET,
diff --git a/src/jtag/tcl.c b/src/jtag/tcl.c
index e32f0ca..7683014 100644
--- a/src/jtag/tcl.c
+++ b/src/jtag/tcl.c
@@ -894,7 +894,7 @@ static const struct command_registration jtag_subcommand_handlers[] = {
 	},
 	{
 		.name = "configure",
-		.mode = COMMAND_EXEC,
+		.mode = COMMAND_ANY,
 		.jim_handler = jim_jtag_configure,
 		.help = "Provide a Tcl handler for the specified "
 			"TAP event.",