STLINK: Test device version (v1/v2) on usb transfers and add sg support

This patch test the device version and differentiate between
v1 and v2 devices.

Change-Id: Ie79bf2c5534211493b004329fb6d5b9d4ea5453b
Signed-off-by: Mathias K <kesmtp@freenet.de>
Reviewed-on: http://openocd.zylin.com/396
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>

2 files changed, 194 insertions, 3 deletions

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 2a4c2f4..64d3364 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -71,6 +71,8 @@ struct stlink_usb_handle_s {
 	uint16_t vid;
 	/** */
 	uint16_t pid;
+	/** */
+	uint32_t sg_tag;
 };
 
 #define STLINK_OK			0x80
@@ -131,7 +133,158 @@ enum stlink_mode {
 };
 
 /** */
-static int stlink_usb_recv(void *handle, const uint8_t *txbuf, int txsize, uint8_t *rxbuf,
+static void stlink_usb_recv_v1_create_cmd(char *b, int s, uint32_t tag, uint32_t rxsize,
+			uint8_t flag, uint8_t lun, uint8_t length)
+{
+	int i = 0;
+
+	memset(b, 0x00, s);
+
+	/* fill the send buffer */
+	strcpy(b, "USBC");
+	i += 4;
+
+	buf_set_u32(b+i, 0, 32, tag);
+	i += 4;
+	buf_set_u32(b+i, 0, 32, rxsize);
+	i += 4;
+	b[i++] = flag;
+	b[i++] = lun;
+	b[i++] = length;
+}
+
+/** */
+static int stlink_usb_recv_v1_mass_storage_cmd(void *handle, const uint8_t *txbuf, int txsize, uint8_t *rxbuf,
+		    int rxsize)
+{
+	char sg_buffer[31];
+	struct stlink_usb_handle_s *h;
+
+	assert(handle != NULL);
+
+	h = (struct stlink_usb_handle_s *)handle;
+	h->sg_tag = (h->sg_tag + 1) & 1;
+
+	stlink_usb_recv_v1_create_cmd(sg_buffer, 31, h->sg_tag, rxsize, STLINK_TX_EP, 0x00, txsize);
+
+	memcpy(sg_buffer+15, txbuf, 10);
+
+	if (jtag_libusb_bulk_write(h->fd, STLINK_TX_EP, (char *)sg_buffer, 31,
+				   1000) != 31) {
+		printf("send failed\n");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+#define REQUEST_SENSE		0x03
+#define REQUEST_SENSE_LENGTH	18
+
+/** */
+static int stlink_usb_recv_v1_get_status(void *handle, char *sg_buffer, int len)
+{
+	struct stlink_usb_handle_s *h;
+
+	assert(handle != NULL);
+
+	h = (struct stlink_usb_handle_s *)handle;
+
+	/* read status */
+	memset(sg_buffer, 0x00, len);
+
+	if (jtag_libusb_bulk_read(h->fd, STLINK_RX_EP, (char *)sg_buffer,
+				len, 1000) != len)
+		return ERROR_FAIL;
+
+	uint32_t t1, t2;
+
+	t1 = buf_get_u32(sg_buffer+0, 0, 32);
+	t2 = buf_get_u32(sg_buffer+4, 0, 32);
+
+	/* check for USBS */
+	if (t1 != 0x53425355)
+		return ERROR_FAIL;
+
+	return ERROR_OK;
+}
+
+/** */
+static int stlink_usb_recv_v1_get_sense(void *handle)
+{
+	struct stlink_usb_handle_s *h;
+	char cdb[16];
+	char sg_buffer[31];
+
+	assert(handle != NULL);
+
+	h = (struct stlink_usb_handle_s *)handle;
+	h->sg_tag = (h->sg_tag + 1) & 1;
+
+	cdb[0] = REQUEST_SENSE;
+	cdb[4] = REQUEST_SENSE_LENGTH;
+
+	stlink_usb_recv_v1_create_cmd(sg_buffer, 31, h->sg_tag, REQUEST_SENSE_LENGTH, STLINK_TX_EP,
+			0x00, 16);
+
+	memcpy(sg_buffer+15, cdb, 16);
+
+	if (jtag_libusb_bulk_write(h->fd, STLINK_TX_EP, (char *)sg_buffer, 16,
+				   1000) != 16)
+		return ERROR_FAIL;
+
+	if (jtag_libusb_bulk_read(h->fd, STLINK_RX_EP, (char *)cdb,
+				  16, 1000) != 16)
+		return ERROR_FAIL;
+
+	if (stlink_usb_recv_v1_get_status(handle, sg_buffer, 13) != ERROR_OK)
+		return ERROR_FAIL;
+	/* check for sense */
+	if (sg_buffer[12] != 0)
+		return ERROR_FAIL;
+
+	/* if (sense[0] != 0x70 && sense[0] != 0x71) */
+
+	return ERROR_OK;
+}
+
+/** */
+static int stlink_usb_recv_v1(void *handle, const uint8_t *txbuf, int txsize, uint8_t *rxbuf,
+		    int rxsize)
+{
+	int err;
+	char sg_buffer[31];
+	struct stlink_usb_handle_s *h;
+
+	assert(handle != NULL);
+
+	h = (struct stlink_usb_handle_s *)handle;
+
+	err = stlink_usb_recv_v1_mass_storage_cmd(handle, txbuf, txsize, rxbuf, rxsize);
+
+	if (err != ERROR_OK)
+		return err;
+
+	if (rxsize && rxbuf) {
+		if (jtag_libusb_bulk_read(h->fd, STLINK_RX_EP, (char *)rxbuf,
+					  rxsize, 1000) != rxsize) {
+			LOG_DEBUG("jtag_libusb_bulk_read");
+			return ERROR_FAIL;
+		}
+	}
+
+	if (stlink_usb_recv_v1_get_status(handle, sg_buffer, 13) != ERROR_OK)
+		return ERROR_FAIL;
+	/* check for sense */
+	if (sg_buffer[12] == 1) {
+		LOG_DEBUG("get sense");
+		err = stlink_usb_recv_v1_get_sense(handle);
+	}
+	return err;
+}
+
+/** */
+static int stlink_usb_recv_v2(void *handle, const uint8_t *txbuf, int txsize, uint8_t *rxbuf,
 		    int rxsize)
 {
 	struct stlink_usb_handle_s *h;
@@ -154,6 +307,25 @@ static int stlink_usb_recv(void *handle, const uint8_t *txbuf, int txsize, uint8
 }
 
 /** */
+static int stlink_usb_recv(void *handle, const uint8_t *txbuf, int txsize, uint8_t *rxbuf,
+		    int rxsize)
+{
+	struct stlink_usb_handle_s *h;
+
+	assert(handle != NULL);
+
+	h = (struct stlink_usb_handle_s *)handle;
+
+	if (h->version.stlink == 1) {
+		return stlink_usb_recv_v1(handle, txbuf, txsize, rxbuf, rxsize);
+	} else {
+		if (txsize < STLINK_CMD_SIZE)
+			txsize = STLINK_CMD_SIZE;
+		return stlink_usb_recv_v2(handle, txbuf, txsize, rxbuf, rxsize);
+	}
+}
+
+/** */
 static void stlink_usb_init_buffer(void *handle)
 {
 	struct stlink_usb_handle_s *h;
@@ -363,6 +535,8 @@ static int stlink_usb_init_mode(void *handle)
 			emode = STLINK_MODE_DEBUG_JTAG;
 			break;
 		case STLINK_TRANSPORT_SWIM:
+			emode = STLINK_MODE_DEBUG_SWIM;
+			break;
 		default:
 			emode = STLINK_MODE_UNKNOWN;
 			break;
@@ -758,8 +932,8 @@ static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)
 	const uint16_t vids[] = { param->vid, 0 };
 	const uint16_t pids[] = { param->pid, 0 };
 
-	LOG_DEBUG("vid: %04x pid: %04x", param->vid,
-		  param->pid);
+	LOG_DEBUG("transport: %d vid: %04x pid: %04x", param->transport,
+		param->vid, param->pid);
 
 	if (jtag_libusb_open(vids, pids, &h->fd) != ERROR_OK) {
 		LOG_ERROR("open failed");
@@ -773,6 +947,16 @@ static int stlink_usb_open(struct stlink_interface_param_s *param, void **fd)
 		return ERROR_FAIL;
 	}
 
+	/* wrap version for first read */
+	switch (param->pid) {
+		case 0x3744:
+			h->version.stlink = 1;
+			break;
+		default:
+			h->version.stlink = 2;
+			break;
+	}
+
 	/* get the device version */
 	err = stlink_usb_version(h);
 
diff --git a/src/jtag/stlink/stlink_layout.c b/src/jtag/stlink/stlink_layout.c
index 0681d2a..4ef9b76 100644
--- a/src/jtag/stlink/stlink_layout.c
+++ b/src/jtag/stlink/stlink_layout.c
@@ -67,6 +67,13 @@ static const struct stlink_layout stlink_layouts[] = {
 	 .close = stlink_layout_close,
 	 .api = &stlink_usb_layout_api,
 	 },
+	{
+	 .name = "sg",
+	 .type = STLINK_LAYOUT_SG,
+	 .open = stlink_layout_open,
+	 .close = stlink_layout_close,
+	 .api = &stlink_usb_layout_api,
+	 },
 	{.name = NULL, /* END OF TABLE */ },
 };