stlink: support of ST-LINK TCP server using stlink-dap and hla

Quote: The ST-LINK TCP server is an application to share the debug
interface of a single ST-LINK board among several host applications,
typically a debugging tool and a monitoring tool.

Note: ST-Link TCP server does not support the SWIM transport.

ST-LINK TCP server allows several applications to connect to the same
ST-Link through sockets (TCP).

To use ST-LINK TCP server:
 - using stlink-dap : use 'st-link backend tcp [port]'
 - using hla : use 'hla_stlink_backend tcp [port]'

the default port value is 7184

Change-Id: I9b79f65267f04b1e978709934892160e65bd2d6d
Signed-off-by: Tarek BOCHKATI <tarek.bouchkati@gmail.com>
Reviewed-on: http://openocd.zylin.com/5633
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

4 files changed, 546 insertions, 19 deletions

diff --git a/doc/openocd.texi b/doc/openocd.texi
index a5b9436..34f1bb6 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -3126,6 +3126,15 @@ Specifies the adapter layout to use.
 Pairs of vendor IDs and product IDs of the device.
 @end deffn
 
+@deffn {Config Command} {hla_stlink_backend} (usb | tcp [port])
+@emph{ST-Link only:} Choose between 'exclusive' USB communication (the default backend) or
+'shared' mode using ST-Link TCP server (the default port is 7184).
+
+@emph{Note:} ST-Link TCP server is a binary application provided by ST
+available from @url{https://www.st.com/en/development-tools/st-link-server.html,
+ST-LINK server software module}.
+@end deffn
+
 @deffn {Command} {hla_command} command
 Execute a custom adapter-specific command. The @var{command} string is
 passed as is to the underlying adapter layout handler.
@@ -3146,6 +3155,17 @@ An error is returned for any AP number above the maximum allowed value.
 @emph{Note:} Either these same adapters and their older versions are
 also supported by @ref{hla_interface, the hla interface driver}.
 
+@deffn {Config Command} {st-link backend} (usb | tcp [port])
+Choose between 'exclusive' USB communication (the default backend) or
+'shared' mode using ST-Link TCP server (the default port is 7184).
+
+@emph{Note:} ST-Link TCP server is a binary application provided by ST
+available from @url{https://www.st.com/en/development-tools/st-link-server.html,
+ST-LINK server software module}.
+
+@emph{Note:} ST-Link TCP server does not support the SWIM transport.
+@end deffn
+
 @deffn {Config Command} {st-link serial} serial
 Specifies the serial number of the adapter.
 @end deffn
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 6224070..813af55 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -45,6 +45,16 @@
 
 #include <target/cortex_m.h>
 
+#include <helper/system.h>
+
+#ifdef HAVE_ARPA_INET_H
+#include <arpa/inet.h>
+#endif
+
+#ifdef HAVE_NETINET_TCP_H
+#include <netinet/tcp.h>
+#endif
+
 #include "libusb_helper.h"
 
 #ifdef HAVE_LIBUSB1
@@ -129,6 +139,21 @@ struct stlink_usb_priv_s {
 	struct libusb_transfer *trans;
 };
 
+struct stlink_tcp_priv_s {
+	/** */
+	int fd;
+	/** */
+	bool connected;
+	/** */
+	uint32_t device_id;
+	/** */
+	uint32_t connect_id;
+	/** */
+	uint8_t *send_buf;
+	/** */
+	uint8_t *recv_buf;
+};
+
 struct stlink_backend_s {
 	/** */
 	int (*open)(void *handle, struct hl_interface_param_s *param);
@@ -147,6 +172,7 @@ struct stlink_usb_handle_s {
 	/** */
 	union {
 		struct stlink_usb_priv_s usb_backend_priv;
+		struct stlink_tcp_priv_s tcp_backend_priv;
 	};
 	/** */
 	uint8_t rx_ep;
@@ -155,13 +181,13 @@ struct stlink_usb_handle_s {
 	/** */
 	uint8_t trace_ep;
 	/** */
-	uint8_t cmdbuf[STLINK_SG_SIZE];
+	uint8_t *cmdbuf;
 	/** */
 	uint8_t cmdidx;
 	/** */
 	uint8_t direction;
 	/** */
-	uint8_t databuf[STLINK_DATA_SIZE];
+	uint8_t *databuf;
 	/** */
 	uint32_t max_mem_packet;
 	/** */
@@ -349,6 +375,39 @@ static inline int stlink_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, i
 #define REQUEST_SENSE        0x03
 #define REQUEST_SENSE_LENGTH 18
 
+/* STLINK TCP commands */
+#define STLINK_TCP_CMD_REFRESH_DEVICE_LIST   0x00
+#define STLINK_TCP_CMD_GET_NB_DEV            0x01
+#define STLINK_TCP_CMD_GET_DEV_INFO          0x02
+#define STLINK_TCP_CMD_OPEN_DEV              0x03
+#define STLINK_TCP_CMD_CLOSE_DEV             0x04
+#define STLINK_TCP_CMD_SEND_USB_CMD          0x05
+#define STLINK_TCP_CMD_GET_SERVER_VERSION    0x06
+#define STLINK_TCP_CMD_GET_NB_OF_DEV_CLIENTS 0x07
+
+/* STLINK TCP constants */
+#define OPENOCD_STLINK_TCP_API_VERSION       1
+#define STLINK_TCP_REQUEST_WRITE             0
+#define STLINK_TCP_REQUEST_READ              1
+#define STLINK_TCP_REQUEST_READ_SWO          3
+#define STLINK_TCP_SS_SIZE                   4
+#define STLINK_TCP_USB_CMD_SIZE              32
+#define STLINK_TCP_SERIAL_SIZE               32
+#define STLINK_TCP_SEND_BUFFER_SIZE          10240
+#define STLINK_TCP_RECV_BUFFER_SIZE          10240
+
+/* STLINK TCP command status */
+#define STLINK_TCP_SS_OK                     0x00000001
+#define STLINK_TCP_SS_MEMORY_PROBLEM         0x00001000
+#define STLINK_TCP_SS_TIMEOUT                0x00001001
+#define STLINK_TCP_SS_BAD_PARAMETER          0x00001002
+#define STLINK_TCP_SS_OPEN_ERR               0x00001003
+#define STLINK_TCP_SS_TRUNCATED_DATA         0x00001052
+#define STLINK_TCP_SS_CMD_NOT_AVAILABLE      0x00001053
+#define STLINK_TCP_SS_TCP_ERROR              0x00002001
+#define STLINK_TCP_SS_TCP_CANT_CONNECT       0x00002002
+#define STLINK_TCP_SS_WIN32_ERROR            0x00010000
+
 /*
  * Map the relevant features, quirks and workaround for specific firmware
  * version of stlink
@@ -763,6 +822,117 @@ static int stlink_usb_usb_xfer_noerrcheck(void *handle, const uint8_t *buf, int
 	return ERROR_OK;
 }
 
+
+static int stlink_tcp_send_cmd(void *handle, int send_size, int recv_size, bool check_tcp_status)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	assert(handle != NULL);
+
+	/* send the TCP command */
+	int sent_size = send(h->tcp_backend_priv.fd, (void *)h->tcp_backend_priv.send_buf, send_size, 0);
+	if (sent_size != send_size) {
+		LOG_ERROR("failed to send USB CMD");
+		if (sent_size == -1)
+			LOG_DEBUG("socket send error: %s (errno %d)", strerror(errno), errno);
+		else
+			LOG_DEBUG("sent size %d (expected %d)", sent_size, send_size);
+		return ERROR_FAIL;
+	}
+
+	keep_alive();
+
+	/* read the TCP response */
+	int received_size = recv(h->tcp_backend_priv.fd, (void *)h->tcp_backend_priv.recv_buf, recv_size, 0);
+	if (received_size != recv_size) {
+		LOG_ERROR("failed to receive USB CMD response");
+		if (received_size == -1)
+			LOG_DEBUG("socket recv error: %s (errno %d)", strerror(errno), errno);
+		else
+			LOG_DEBUG("received size %d (expected %d)", received_size, recv_size);
+		return ERROR_FAIL;
+	}
+
+	if (check_tcp_status) {
+		uint32_t tcp_ss = le_to_h_u32(h->tcp_backend_priv.recv_buf);
+		if (tcp_ss != STLINK_TCP_SS_OK) {
+			LOG_ERROR("TCP error status 0x%X", tcp_ss);
+			return ERROR_FAIL;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+/** */
+static int stlink_tcp_xfer_noerrcheck(void *handle, const uint8_t *buf, int size)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	int send_size = STLINK_TCP_USB_CMD_SIZE;
+	int recv_size = STLINK_TCP_SS_SIZE;
+
+	assert(handle != NULL);
+
+	/* prepare the TCP command */
+	h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_SEND_USB_CMD;
+	memset(&h->tcp_backend_priv.send_buf[1], 0, 3); /* reserved for alignment and future use, must be zero */
+	h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.connect_id);
+	/* tcp_backend_priv.send_buf[8..23] already contains the constructed stlink command */
+	h->tcp_backend_priv.send_buf[24] = h->direction;
+	memset(&h->tcp_backend_priv.send_buf[25], 0, 3);  /* reserved for alignment and future use, must be zero */
+
+	h_u32_to_le(&h->tcp_backend_priv.send_buf[28], size);
+
+	/*
+	 * if the xfer is a write request (tx_ep)
+	 *  > then buf content will be copied
+	 * into &cmdbuf[32].
+	 * else : the xfer is a read or trace read request (rx_ep or trace_ep)
+	 *  > the buf content will be filled from &databuf[4].
+	 *
+	 * note : if h->direction is trace_ep, h->cmdbuf is zeros.
+	 */
+
+	if (h->direction == h->tx_ep) { /* STLINK_TCP_REQUEST_WRITE */
+		send_size += size;
+		if (send_size > STLINK_TCP_SEND_BUFFER_SIZE) {
+			LOG_ERROR("STLINK_TCP command buffer overflow");
+			return ERROR_FAIL;
+		}
+		memcpy(&h->tcp_backend_priv.send_buf[32], buf, size);
+	} else { /* STLINK_TCP_REQUEST_READ or STLINK_TCP_REQUEST_READ_SWO */
+		recv_size += size;
+		if (recv_size > STLINK_TCP_RECV_BUFFER_SIZE) {
+			LOG_ERROR("STLINK_TCP data buffer overflow");
+			return ERROR_FAIL;
+		}
+	}
+
+	int ret = stlink_tcp_send_cmd(h, send_size, recv_size, true);
+	if (ret != ERROR_OK)
+		return ret;
+
+	if (h->direction != h->tx_ep) {
+		/* the read data is located in tcp_backend_priv.recv_buf[4] */
+		/* most of the case it will be copying the data from tcp_backend_priv.recv_buf[4]
+		 * to handle->cmd_buff which are the same, so let's avoid unnecessary copying */
+		if (buf != &h->tcp_backend_priv.recv_buf[4])
+			memcpy((uint8_t *)buf, &h->tcp_backend_priv.recv_buf[4], size);
+	}
+
+	return ERROR_OK;
+}
+
+/** */
+static int stlink_tcp_read_trace(void *handle, const uint8_t *buf, int size)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	stlink_usb_init_buffer(h, h->trace_ep, 0);
+	return stlink_tcp_xfer_noerrcheck(handle, buf, size);
+}
+
 /**
     Converts an STLINK status code held in the first byte of a response
     to an openocd error, logs any error/wait status as debug output.
@@ -2768,17 +2938,55 @@ static int stlink_usb_usb_close(void *handle)
 {
 	struct stlink_usb_handle_s *h = handle;
 
-	if (h && h->usb_backend_priv.fd) {
+	if (!h)
+		return ERROR_OK;
+
+	if (h->usb_backend_priv.fd) {
 		stlink_usb_exit_mode(h);
 		/* do not check return code, it prevent
 		us from closing jtag_libusb */
 		jtag_libusb_close(h->usb_backend_priv.fd);
 	}
 
+	free(h->cmdbuf);
+	free(h->databuf);
+
 	return ERROR_OK;
 }
 
 /** */
+static int stlink_tcp_close(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	if (!h)
+		return ERROR_OK;
+
+	int ret = ERROR_OK;
+	if (h->tcp_backend_priv.connected) {
+		if (h->tcp_backend_priv.connect_id) {
+			stlink_usb_exit_mode(h);
+
+			/* close the stlink */
+			h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_CLOSE_DEV;
+			memset(&h->tcp_backend_priv.send_buf[1], 0, 4); /* reserved */
+			h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.connect_id);
+			ret = stlink_tcp_send_cmd(h, 8, 4, true);
+			if (ret != ERROR_OK)
+				LOG_ERROR("cannot close the STLINK");
+		}
+
+		if (close_socket(h->tcp_backend_priv.fd) != 0)
+			LOG_ERROR("error closing the socket, errno: %s", strerror(errno));
+	}
+
+	free(h->tcp_backend_priv.send_buf);
+	free(h->tcp_backend_priv.recv_buf);
+
+	return ret;
+}
+
+/** */
 static int stlink_close(void *handle)
 {
 	if (handle != NULL) {
@@ -2877,6 +3085,12 @@ static int stlink_usb_usb_open(void *handle, struct hl_interface_param_s *param)
 	struct stlink_usb_handle_s *h = handle;
 	int err, retry_count = 1;
 
+	h->cmdbuf = malloc(STLINK_SG_SIZE);
+	h->databuf = malloc(STLINK_DATA_SIZE);
+
+	if (h->cmdbuf == NULL || h->databuf == NULL)
+		return ERROR_FAIL;
+
 	/*
 	  On certain host USB configurations(e.g. MacBook Air)
 	  STLINKv2 dongle seems to have its FW in a funky state if,
@@ -2973,6 +3187,217 @@ static int stlink_usb_usb_open(void *handle, struct hl_interface_param_s *param)
 	return ERROR_OK;
 }
 
+/** */
+static int stlink_tcp_open(void *handle, struct hl_interface_param_s *param)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int ret;
+
+	/* SWIM is not supported using stlink-server */
+	if (h->st_mode ==  STLINK_MODE_DEBUG_SWIM) {
+		LOG_ERROR("stlink-server does not support SWIM mode");
+		return ERROR_FAIL;
+	}
+
+	h->tcp_backend_priv.send_buf = malloc(STLINK_TCP_SEND_BUFFER_SIZE);
+	h->tcp_backend_priv.recv_buf = malloc(STLINK_TCP_RECV_BUFFER_SIZE);
+
+	if (h->tcp_backend_priv.send_buf == NULL || h->tcp_backend_priv.recv_buf == NULL)
+		return ERROR_FAIL;
+
+	h->cmdbuf = &h->tcp_backend_priv.send_buf[8];
+	h->databuf = &h->tcp_backend_priv.recv_buf[4];
+
+	/* configure directions */
+	h->rx_ep = STLINK_TCP_REQUEST_READ;
+	h->tx_ep = STLINK_TCP_REQUEST_WRITE;
+	h->trace_ep = STLINK_TCP_REQUEST_READ_SWO;
+
+	h->tcp_backend_priv.fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
+	h->tcp_backend_priv.connected = false;
+	h->tcp_backend_priv.device_id = 0;
+	h->tcp_backend_priv.connect_id = 0;
+
+	if (h->tcp_backend_priv.fd == -1) {
+		LOG_ERROR("error creating the socket, errno: %s", strerror(errno));
+		return ERROR_FAIL;
+	}
+
+	struct sockaddr_in serv;
+	memset(&serv, 0, sizeof(struct sockaddr_in));
+	serv.sin_family = AF_INET;
+	serv.sin_port = htons(param->stlink_tcp_port);
+	serv.sin_addr.s_addr = inet_addr("127.0.0.1");
+
+	LOG_DEBUG("socket : %x", h->tcp_backend_priv.fd);
+
+	int optval = 1;
+	if (setsockopt(h->tcp_backend_priv.fd, IPPROTO_TCP, TCP_NODELAY, (const void *)&optval, sizeof(int)) == -1) {
+		LOG_ERROR("cannot set sock option 'TCP_NODELAY', errno: %s", strerror(errno));
+		return ERROR_FAIL;
+	}
+
+	optval = STLINK_TCP_RECV_BUFFER_SIZE;
+	if (setsockopt(h->tcp_backend_priv.fd, SOL_SOCKET, SO_RCVBUF, (const void *)&optval, sizeof(int)) == -1) {
+		LOG_ERROR("cannot set sock option 'SO_RCVBUF', errno: %s", strerror(errno));
+		return ERROR_FAIL;
+	}
+
+	optval = STLINK_TCP_SEND_BUFFER_SIZE;
+	if (setsockopt(h->tcp_backend_priv.fd, SOL_SOCKET, SO_SNDBUF, (const void *)&optval, sizeof(int)) == -1) {
+		LOG_ERROR("cannot set sock option 'SO_SNDBUF', errno: %s", strerror(errno));
+		return ERROR_FAIL;
+	}
+
+	if (connect(h->tcp_backend_priv.fd, (const struct sockaddr *)&serv, sizeof(serv)) == -1) {
+		LOG_ERROR("cannot connect to stlink server, errno: %s", strerror(errno));
+		return ERROR_FAIL;
+	}
+
+	h->tcp_backend_priv.connected = true;
+
+	LOG_INFO("connected to stlink-server");
+
+	/* print stlink-server version */
+	h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_GET_SERVER_VERSION;
+	h->tcp_backend_priv.send_buf[1] = OPENOCD_STLINK_TCP_API_VERSION;
+	memset(&h->tcp_backend_priv.send_buf[2], 0, 2); /* reserved */
+	ret = stlink_tcp_send_cmd(h, 4, 16, false);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("cannot get the stlink-server version");
+		return ERROR_FAIL;
+	}
+
+	uint32_t api_ver = le_to_h_u32(&h->tcp_backend_priv.recv_buf[0]);
+	uint32_t ver_major = le_to_h_u32(&h->tcp_backend_priv.recv_buf[4]);
+	uint32_t ver_minor = le_to_h_u32(&h->tcp_backend_priv.recv_buf[8]);
+	uint32_t ver_build = le_to_h_u32(&h->tcp_backend_priv.recv_buf[12]);
+	LOG_INFO("stlink-server API v%d, version %d.%d.%d",
+			api_ver, ver_major, ver_minor, ver_build);
+
+	/* in stlink-server API v1 sending more than 1428 bytes will cause stlink-server
+	 * to crash in windows: select a safe default value (1K) */
+	if (api_ver < 2)
+		h->max_mem_packet = (1 << 10);
+
+	/* refresh stlink list (re-enumerate) */
+	h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_REFRESH_DEVICE_LIST;
+	h->tcp_backend_priv.send_buf[1] = 0; /* don't clear the list, just refresh it */
+	ret = stlink_tcp_send_cmd(h, 2, 4, true);
+	if (ret != ERROR_OK)
+		return ret;
+
+	/* get the number of connected stlinks */
+	h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_GET_NB_DEV;
+	ret = stlink_tcp_send_cmd(h, 1, 4, false);
+	if (ret != ERROR_OK)
+		return ret;
+
+	uint32_t connected_stlinks = le_to_h_u32(h->tcp_backend_priv.recv_buf);
+
+	if (connected_stlinks == 0) {
+		LOG_ERROR("no ST-LINK detected");
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("%d ST-LINK detected", connected_stlinks);
+
+	if (connected_stlinks > 255) {
+		LOG_WARNING("STLink server cannot handle more than 255 ST-LINK connected");
+		connected_stlinks = 255;
+	}
+
+	/* list all connected ST-Link and seek for the requested vid:pid and serial */
+	char serial[STLINK_TCP_SERIAL_SIZE + 1] = {0};
+	uint8_t stlink_used;
+	bool stlink_id_matched = false;
+	bool stlink_serial_matched = (param->serial == NULL);
+
+	for (uint32_t stlink_id = 0; stlink_id < connected_stlinks; stlink_id++) {
+		/* get the stlink info */
+		h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_GET_DEV_INFO;
+		h->tcp_backend_priv.send_buf[1] = (uint8_t)stlink_id;
+		memset(&h->tcp_backend_priv.send_buf[2], 0, 2); /* reserved */
+		h_u32_to_le(&h->tcp_backend_priv.send_buf[4], 41); /* size of TDeviceInfo2 */
+		ret = stlink_tcp_send_cmd(h, 8, 45, true);
+		if (ret != ERROR_OK)
+			return ret;
+
+		h->tcp_backend_priv.device_id = le_to_h_u32(&h->tcp_backend_priv.recv_buf[4]);
+		memcpy(serial, &h->tcp_backend_priv.recv_buf[8], STLINK_TCP_SERIAL_SIZE);
+		h->vid = le_to_h_u16(&h->tcp_backend_priv.recv_buf[40]);
+		h->pid = le_to_h_u16(&h->tcp_backend_priv.recv_buf[42]);
+		stlink_used = h->tcp_backend_priv.recv_buf[44];
+
+		/* check the vid:pid */
+		for (int i = 0; param->vid[i]; i++) {
+			if (param->vid[i] == h->vid && param->pid[i] == h->pid) {
+				stlink_id_matched = true;
+				break;
+			}
+		}
+
+		if (!stlink_id_matched)
+			continue;
+
+		/* check the serial if specified */
+		if (param->serial) {
+			/* ST-Link server fixes the buggy serial returned by old ST-Link DFU
+			 * for further details refer to stlink_usb_get_alternate_serial
+			 * so if the user passes the buggy serial, we need to fix it before
+			 * comparing with the serial returned by ST-Link server */
+			if (strlen(param->serial) == STLINK_SERIAL_LEN / 2) {
+				char fixed_serial[STLINK_SERIAL_LEN + 1];
+
+				for (unsigned int i = 0; i < STLINK_SERIAL_LEN; i += 2)
+					sprintf(fixed_serial + i, "%02X", param->serial[i / 2]);
+
+				fixed_serial[STLINK_SERIAL_LEN] = '\0';
+
+				stlink_serial_matched = strcmp(fixed_serial, serial) == 0;
+			} else
+				stlink_serial_matched = strcmp(param->serial, serial) == 0;
+		}
+
+		if (!stlink_serial_matched)
+			LOG_DEBUG("Device serial number '%s' doesn't match requested serial '%s'",
+					serial, param->serial);
+		else /* exit the search loop if there is match */
+			break;
+	}
+
+	if (!stlink_id_matched) {
+		LOG_ERROR("ST-LINK open failed (vid/pid mismatch)");
+		return ERROR_FAIL;
+	}
+
+	if (!stlink_serial_matched) {
+		LOG_ERROR("ST-LINK open failed (serial mismatch)");
+		return ERROR_FAIL;
+	}
+
+	/* check if device is 'exclusively' used by another application */
+	if (stlink_used) {
+		LOG_ERROR("the selected device is already used");
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("transport: vid: 0x%04x pid: 0x%04x serial: %s", h->vid, h->pid, serial);
+
+	/* now let's open the stlink */
+	h->tcp_backend_priv.send_buf[0] = STLINK_TCP_CMD_OPEN_DEV;
+	memset(&h->tcp_backend_priv.send_buf[1], 0, 4); /* reserved */
+	h_u32_to_le(&h->tcp_backend_priv.send_buf[4], h->tcp_backend_priv.device_id);
+	ret = stlink_tcp_send_cmd(h, 8, 8, true);
+	if (ret != ERROR_OK)
+		return ret;
+
+	h->tcp_backend_priv.connect_id = le_to_h_u32(&h->tcp_backend_priv.recv_buf[4]);
+
+	/* get stlink version */
+	return stlink_usb_version(h);
+}
+
 static struct stlink_backend_s stlink_usb_backend = {
 	.open = stlink_usb_usb_open,
 	.close = stlink_usb_usb_close,
@@ -2980,6 +3405,13 @@ static struct stlink_backend_s stlink_usb_backend = {
 	.read_trace = stlink_usb_usb_read_trace,
 };
 
+static struct stlink_backend_s stlink_tcp_backend = {
+	.open = stlink_tcp_open,
+	.close = stlink_tcp_close,
+	.xfer_noerrcheck = stlink_tcp_xfer_noerrcheck,
+	.read_trace = stlink_tcp_read_trace,
+};
+
 static int stlink_open(struct hl_interface_param_s *param, enum stlink_mode mode, void **fd)
 {
 	struct stlink_usb_handle_s *h;
@@ -3001,7 +3433,10 @@ static int stlink_open(struct hl_interface_param_s *param, enum stlink_mode mode
 			  param->serial ? param->serial : "");
 	}
 
-	h->backend = &stlink_usb_backend;
+	if (param->use_stlink_tcp)
+		h->backend = &stlink_tcp_backend;
+	else
+		h->backend = &stlink_usb_backend;
 
 	if (stlink_usb_open(h, param) != ERROR_OK)
 		goto error_open;
@@ -3051,23 +3486,26 @@ static int stlink_open(struct hl_interface_param_s *param, enum stlink_mode mode
 		return ERROR_OK;
 	}
 
-	/* get cpuid, so we can determine the max page size
-	 * start with a safe default */
-	h->max_mem_packet = (1 << 10);
+	/* set max_mem_packet if it was not set by the low-level interface */
+	if (h->max_mem_packet == 0) {
+		/* get cpuid, so we can determine the max page size
+		 * start with a safe default */
+		h->max_mem_packet = (1 << 10);
 
-	uint8_t buffer[4];
-	stlink_usb_open_ap(h, 0);
-	err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
-	if (err == ERROR_OK) {
-		uint32_t cpuid = le_to_h_u32(buffer);
-		int i = (cpuid >> 4) & 0xf;
-		if (i == 4 || i == 3) {
-			/* Cortex-M3/M4 has 4096 bytes autoincrement range */
-			h->max_mem_packet = (1 << 12);
+		uint8_t buffer[4];
+		stlink_usb_open_ap(h, 0);
+		err = stlink_usb_read_mem32(h, CPUID, 4, buffer);
+		if (err == ERROR_OK) {
+			uint32_t cpuid = le_to_h_u32(buffer);
+			int i = (cpuid >> 4) & 0xf;
+			if (i == 4 || i == 3) {
+				/* Cortex-M3/M4 has 4096 bytes autoincrement range */
+				h->max_mem_packet = (1 << 12);
+			}
 		}
-	}
 
-	LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
+		LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet);
+	}
 
 	*fd = h;
 
@@ -3750,6 +4188,32 @@ COMMAND_HANDLER(stlink_dap_vid_pid)
 }
 
 /** */
+COMMAND_HANDLER(stlink_dap_backend_command)
+{
+	/* default values */
+	bool use_stlink_tcp = false;
+	uint16_t stlink_tcp_port = 7184;
+
+	if (CMD_ARGC == 0 || CMD_ARGC > 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	else if (strcmp(CMD_ARGV[0], "usb") == 0) {
+		if (CMD_ARGC > 1)
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		/* else use_stlink_tcp = false (already the case ) */
+	} else if (strcmp(CMD_ARGV[0], "tcp") == 0) {
+		use_stlink_tcp = true;
+		if (CMD_ARGC == 2)
+			COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], stlink_tcp_port);
+	} else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	stlink_dap_param.use_stlink_tcp = use_stlink_tcp;
+	stlink_dap_param.stlink_tcp_port = stlink_tcp_port;
+
+	return ERROR_OK;
+}
+
+/** */
 static const struct command_registration stlink_dap_subcommand_handlers[] = {
 	{
 		.name = "serial",
@@ -3765,6 +4229,13 @@ static const struct command_registration stlink_dap_subcommand_handlers[] = {
 		.help = "USB VID and PID of the adapter",
 		.usage = "(vid pid)+",
 	},
+	{
+		.name = "backend",
+		.handler = &stlink_dap_backend_command,
+		.mode = COMMAND_CONFIG,
+		.help = "select which ST-Link backend to use",
+		.usage = "usb | tcp [port]",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/jtag/hla/hla_interface.c b/src/jtag/hla/hla_interface.c
index 6691a9a..04e01d2 100644
--- a/src/jtag/hla/hla_interface.c
+++ b/src/jtag/hla/hla_interface.c
@@ -35,7 +35,7 @@
 
 #include <target/target.h>
 
-static struct hl_interface_s hl_if = { {0, 0, { 0 }, { 0 }, HL_TRANSPORT_UNKNOWN, false, -1}, 0, 0 };
+static struct hl_interface_s hl_if = { {0, 0, { 0 }, { 0 }, HL_TRANSPORT_UNKNOWN, false, -1, false, 7184}, 0, 0 };
 
 int hl_interface_open(enum hl_transports tr)
 {
@@ -292,6 +292,31 @@ COMMAND_HANDLER(hl_interface_handle_vid_pid_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(hl_interface_handle_stlink_backend_command)
+{
+	/* default values */
+	bool use_stlink_tcp = false;
+	uint16_t stlink_tcp_port = 7184;
+
+	if (CMD_ARGC == 0 || CMD_ARGC > 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	else if (strcmp(CMD_ARGV[0], "usb") == 0) {
+		if (CMD_ARGC > 1)
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		/* else use_stlink_tcp = false (already the case ) */
+	} else if (strcmp(CMD_ARGV[0], "tcp") == 0) {
+		use_stlink_tcp = true;
+		if (CMD_ARGC == 2)
+			COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], stlink_tcp_port);
+	} else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	hl_if.param.use_stlink_tcp = use_stlink_tcp;
+	hl_if.param.stlink_tcp_port = stlink_tcp_port;
+
+	return ERROR_OK;
+}
+
 COMMAND_HANDLER(interface_handle_hla_command)
 {
 	if (CMD_ARGC != 1)
@@ -336,6 +361,13 @@ static const struct command_registration hl_interface_command_handlers[] = {
 	 .help = "the vendor and product ID of the adapter",
 	 .usage = "(vid pid)* ",
 	 },
+	{
+	 .name = "hla_stlink_backend",
+	 .handler = &hl_interface_handle_stlink_backend_command,
+	 .mode = COMMAND_CONFIG,
+	 .help = "select which ST-Link backend to use",
+	 .usage = "usb | tcp [port]",
+	},
 	 {
 	 .name = "hla_command",
 	 .handler = &interface_handle_hla_command,
diff --git a/src/jtag/hla/hla_interface.h b/src/jtag/hla/hla_interface.h
index b6e4a8b..a1c95cd 100644
--- a/src/jtag/hla/hla_interface.h
+++ b/src/jtag/hla/hla_interface.h
@@ -46,6 +46,10 @@ struct hl_interface_param_s {
 	bool connect_under_reset;
 	/** Initial interface clock clock speed */
 	int initial_interface_speed;
+	/** */
+	bool use_stlink_tcp;
+	/** */
+	uint16_t stlink_tcp_port;
 };
 
 struct hl_interface_s {