Merge pull request #937 from UweBonnes/CMSIS-DAP

CMSIS-DAP/ADIv5 fixes for #936 and #925

src/exception.c

@@ -26,7 +26,6 @@ struct exception *innermost_exception;
 void raise_exception(uint32_t type, const char *msg)
 {
 	struct exception *e;
-	DEBUG_WARN("Exception: %s\n", msg);
 	for (e = innermost_exception; e; e = e->outer) {
 		if (e->mask & type) {
 			e->type = type;
@@ -35,6 +34,7 @@ void raise_exception(uint32_t type, const char *msg)
 			longjmp(e->jmpbuf, type);
 		}
 	}
+	DEBUG_WARN("Unhandled exception: %s\n", msg);
 	abort();
 }
 

src/platforms/common/cdcacm.c

@@ -174,7 +174,7 @@ static const struct usb_iface_assoc_descriptor gdb_assoc = {
 	.bFunctionClass = USB_CLASS_CDC,
 	.bFunctionSubClass = USB_CDC_SUBCLASS_ACM,
 	.bFunctionProtocol = USB_CDC_PROTOCOL_NONE,
-	.iFunction = 0,
+	.iFunction = 4,
 };
 
 /* Serial ACM interface */
@@ -277,7 +277,7 @@ static const struct usb_iface_assoc_descriptor uart_assoc = {
 	.bFunctionClass = USB_CLASS_CDC,
 	.bFunctionSubClass = USB_CDC_SUBCLASS_ACM,
 	.bFunctionProtocol = USB_CDC_PROTOCOL_NONE,
-	.iFunction = 0,
+	.iFunction = 5,
 };
 
 const struct usb_dfu_descriptor dfu_function = {
@@ -396,7 +396,6 @@ static const struct usb_config_descriptor config = {
 static char serial_no[DFU_SERIAL_LENGTH];
 
 #define BOARD_IDENT "Black Magic Probe " PLATFORM_IDENT FIRMWARE_VERSION
-#define DFU_IDENT   "Black Magic Firmware Upgrade " PLATFORM_IDENT FIRMWARE_VERSION
 
 static const char *usb_strings[] = {
 	"Black Sphere Technologies",
@@ -404,7 +403,7 @@ static const char *usb_strings[] = {
 	serial_no,
 	"Black Magic GDB Server",
 	"Black Magic UART Port",
-	DFU_IDENT,
+	"Black Magic DFU",
 #if defined(PLATFORM_HAS_TRACESWO)
 	"Black Magic Trace Capture",
 #endif

src/platforms/hosted/Makefile.inc

@@ -18,7 +18,7 @@ CFLAGS  +=  -DHOSTED_BMP_ONLY=$(HOSTED_BMP_ONLY)
 
 ifneq (, $(findstring linux, $(SYS)))
 SRC += serial_unix.c
-HIDAPILIB = hidapi-libusb
+HIDAPILIB = hidapi-hidraw
 ifeq ($(ASAN), 1)
 CFLAGS += -fsanitize=address  -Wno-format-truncation
 LDFLAGS += -lasan

src/platforms/hosted/bmp_libusb.c

@@ -68,6 +68,7 @@ static bmp_type_t find_cmsis_dap_interface(libusb_device *dev,bmp_info_t *info)
 	if (res != LIBUSB_SUCCESS) {
 		DEBUG_INFO("INFO: libusb_open() failed: %s\n",
 					libusb_strerror(res));
+		libusb_free_config_descriptor(conf);
 		return type;
 	}
 
@@ -90,13 +91,9 @@ static bmp_type_t find_cmsis_dap_interface(libusb_device *dev,bmp_info_t *info)
 		if (!strstr(interface_string, "CMSIS")) {
 			continue;
 		}
+		type = BMP_TYPE_CMSIS_DAP;
 
-		if (interface->bInterfaceClass == 0x03) {
+		if (interface->bInterfaceClass == 0xff && interface->bNumEndpoints == 2) {
-			type = BMP_TYPE_CMSIS_DAP_V1;
-
-		} else if (interface->bInterfaceClass == 0xff && interface->bNumEndpoints == 2) {
-			type = BMP_TYPE_CMSIS_DAP_V2;
-
 			info->interface_num = interface->bInterfaceNumber;
 
 			for (int j = 0; j < interface->bNumEndpoints; j++) {
@@ -110,10 +107,10 @@ static bmp_type_t find_cmsis_dap_interface(libusb_device *dev,bmp_info_t *info)
 			}
 
 			/* V2 is preferred, return early. */
-			return type;
+			break;
 		}
 	}
-
+	libusb_free_config_descriptor(conf);
 	return type;
 }
 
@@ -154,7 +151,6 @@ int find_debuggers(BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 	char *active_cable = NULL;
 	bool ftdi_unknown = false;
   rescan:
-	type = BMP_TYPE_NONE;
 	found_debuggers = 0;
 	serial[0] = 0;
 	manufacturer[0] = 0;
@@ -163,6 +159,7 @@ int find_debuggers(BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 	active_cable = NULL;
 	ftdi_unknown = false;
 	for (int i = 0;  devs[i]; i++) {
+		type = BMP_TYPE_NONE;
 		libusb_device *dev =  devs[i];
 		int res = libusb_get_device_descriptor(dev, &desc);
 		if (res < 0) {
@@ -227,11 +224,12 @@ int find_debuggers(BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 				   ((type = find_cmsis_dap_interface(dev, info)) != BMP_TYPE_NONE)) {
 			/* find_cmsis_dap_interface has set valid type*/
 		} else if ((strstr(manufacturer, "CMSIS")) || (strstr(product, "CMSIS"))) {
-			type = BMP_TYPE_CMSIS_DAP_V1;
+			type = BMP_TYPE_CMSIS_DAP;
 		} else if (desc.idVendor ==  VENDOR_ID_STLINK) {
 			if ((desc.idProduct == PRODUCT_ID_STLINKV2) ||
 				(desc.idProduct == PRODUCT_ID_STLINKV21) ||
 				(desc.idProduct == PRODUCT_ID_STLINKV21_MSD) ||
+				(desc.idProduct == PRODUCT_ID_STLINKV3_NO_MSD) ||
 				(desc.idProduct == PRODUCT_ID_STLINKV3_BL) ||
 				(desc.idProduct == PRODUCT_ID_STLINKV3) ||
 				(desc.idProduct == PRODUCT_ID_STLINKV3E)) {

src/platforms/hosted/bmp_remote.c

@@ -42,7 +42,7 @@ int remote_init(void)
 	platform_buffer_write((uint8_t *)construct, c);
 	c = platform_buffer_read((uint8_t *)construct, REMOTE_MAX_MSG_SIZE);
 
-	if ((!c) || (construct[0] == REMOTE_RESP_ERR)) {
+	if ((c < 1) || (construct[0] == REMOTE_RESP_ERR)) {
 		DEBUG_WARN("Remote Start failed, error %s\n",
 				c ? (char *)&(construct[1]) : "unknown");
       return -1;
@@ -62,7 +62,7 @@ bool remote_target_get_power(void)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 
-	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR)) {
       DEBUG_WARN(" platform_target_get_power failed, error %s\n",
 				 s ? (char *)&(construct[1]) : "unknown");
       exit (-1);
@@ -82,7 +82,7 @@ bool remote_target_set_power(bool power)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 
-	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR)) {
 		DEBUG_WARN("platform_target_set_power failed, error %s\n",
 				s ? (char *)&(construct[1]) : "unknown");
 		return false;
@@ -101,7 +101,7 @@ void remote_srst_set_val(bool assert)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 
-	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR)) {
 		DEBUG_WARN("platform_srst_set_val failed, error %s\n",
 				   s ? (char *)&(construct[1]) : "unknown");
       exit(-1);
@@ -119,7 +119,7 @@ bool remote_srst_get_val(void)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 
-	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR)) {
 		DEBUG_WARN("platform_srst_set_val failed, error %s\n",
 				   s ? (char *)&(construct[1]) : "unknown");
       exit(-1);
@@ -137,7 +137,7 @@ void remote_max_frequency_set(uint32_t freq)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 
-	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR)) {
 		DEBUG_WARN("Update Firmware to allow to set max SWJ frequency\n");
     }
 }
@@ -153,7 +153,7 @@ uint32_t remote_max_frequency_get(void)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 
-	if ((!s) || (construct[0] == REMOTE_RESP_ERR))
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR))
 		return FREQ_FIXED;
 
 	uint32_t freq[1];
@@ -172,7 +172,7 @@ const char *remote_target_voltage(void)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 
-	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR)) {
       DEBUG_WARN("platform_target_voltage failed, error %s\n",
 			  s ? (char *)&(construct[1]) : "unknown");
       exit(- 1);
@@ -188,7 +188,7 @@ static uint32_t remote_adiv5_dp_read(ADIv5_DP_t *dp, uint16_t addr)
 					 dp->dp_jd_index, addr);
 	platform_buffer_write(construct, s);
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
-	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR)) {
 		DEBUG_WARN("%s error %d\n", __func__, s);
 	}
     uint32_t dest[1];
@@ -206,7 +206,7 @@ static uint32_t remote_adiv5_low_access(
 					 REMOTE_LOW_ACCESS_STR, dp->dp_jd_index, RnW, addr, value);
 	platform_buffer_write(construct, s);
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
-	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR)) {
 		DEBUG_WARN("%s error %d\n", __func__, s);
 	}
     uint32_t dest[1];
@@ -221,7 +221,7 @@ static uint32_t remote_adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 					 ap->dp->dp_jd_index, ap->apsel, addr);
 	platform_buffer_write(construct, s);
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
-	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR)) {
 		DEBUG_WARN("%s error %d\n", __func__, s);
 	}
     uint32_t dest[1];
@@ -236,7 +236,7 @@ static void remote_adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 					ap->dp->dp_jd_index,  ap->apsel, addr, value);
 	platform_buffer_write(construct, s);
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
-	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR)) {
 		DEBUG_WARN("%s error %d\n", __func__, s);
 	}
 	return;
@@ -372,7 +372,7 @@ void remote_adiv5_dp_defaults(ADIv5_DP_t *dp)
 					 REMOTE_HL_CHECK_STR);
 	platform_buffer_write(construct, s);
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
-	if ((!s) || (construct[0] == REMOTE_RESP_ERR) ||
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR) ||
 		((construct[1] - '0') <  REMOTE_HL_VERSION)) {
 		DEBUG_WARN(
 			"Please update BMP firmware for substantial speed increase!\n");

src/platforms/hosted/cmsis_dap.c

@@ -45,8 +45,13 @@
 uint8_t dap_caps;
 uint8_t mode;
 
+typedef enum cmsis_type_s {
+	CMSIS_TYPE_NONE = 0,
+	CMSIS_TYPE_HID,
+	CMSIS_TYPE_BULK
+} cmsis_type_t;
 /*- Variables ---------------------------------------------------------------*/
-static bmp_type_t type;
+static cmsis_type_t type;
 static libusb_device_handle *usb_handle = NULL;
 static uint8_t in_ep;
 static uint8_t out_ep;
@@ -58,10 +63,11 @@ static bool has_swd_sequence = false;
 /* LPC845 Breakout Board Rev. 0 report invalid response with > 65 bytes */
 int dap_init(bmp_info_t *info)
 {
-	type = info->bmp_type;
+	type = (info->in_ep && info->out_ep) ? CMSIS_TYPE_BULK : CMSIS_TYPE_HID;
 	int size;
 
-	if (type == BMP_TYPE_CMSIS_DAP_V1) {
+	if (type == CMSIS_TYPE_HID) {
+		DEBUG_INFO("Using hid transfer\n");
 		if (hid_init())
 			return -1;
 		size = strlen(info->serial);
@@ -77,9 +83,12 @@ int dap_init(bmp_info_t *info)
 			report_size = 64 + 1;
 		}
 		handle = hid_open(info->vid, info->pid,  (serial[0]) ? serial : NULL);
-		if (!handle)
+		if (!handle) {
+			DEBUG_WARN("hid_open failed\n");
 			return -1;
-	} else if (type == BMP_TYPE_CMSIS_DAP_V2) {
+		}
+	} else if (type == CMSIS_TYPE_BULK) {
+		DEBUG_INFO("Using bulk transfer\n");
 		usb_handle = libusb_open_device_with_vid_pid(info->libusb_ctx, info->vid, info->pid);
 		if (!usb_handle) {
 			DEBUG_WARN("WARN: libusb_open_device_with_vid_pid() failed\n");
@@ -120,10 +129,17 @@ int dap_init(bmp_info_t *info)
 		DEBUG_INFO(", SWO_MANCHESTER");
 	if (dap_caps & 0x10)
 		DEBUG_INFO(", Atomic Cmds");
+	if (has_swd_sequence)
+		DEBUG_INFO(", DAP_SWD_Sequence");
 	DEBUG_INFO("\n");
 	return 0;
 }
 
+void dap_srst_set_val(bool assert)
+{
+	dap_reset_pin(!assert);
+}
+
 static void dap_dp_abort(ADIv5_DP_t *dp, uint32_t abort)
 {
 	/* DP Write to Reg 0.*/
@@ -175,12 +191,12 @@ static uint32_t dap_dp_read_reg(ADIv5_DP_t *dp, uint16_t addr)
 
 void dap_exit_function(void)
 {
-	if (type == BMP_TYPE_CMSIS_DAP_V1) {
+	if (type == CMSIS_TYPE_HID) {
 		if (handle) {
 			dap_disconnect();
 			hid_close(handle);
 		}
-	} else if (type == BMP_TYPE_CMSIS_DAP_V2) {
+	} else if (type == CMSIS_TYPE_BULK) {
 		if (usb_handle) {
 			dap_disconnect();
 			libusb_close(usb_handle);
@@ -208,38 +224,43 @@ int dbg_dap_cmd(uint8_t *data, int size, int rsize)
 	for(int i = 0; (i < 32) && (i < rsize + 1); i++)
 		DEBUG_WIRE("%02x.",	buffer[i]);
 	DEBUG_WIRE("\n");
-	if (type == BMP_TYPE_CMSIS_DAP_V1) {
+	if (type == CMSIS_TYPE_HID) {
-		res = hid_write(handle, buffer, rsize + 1);
+		res = hid_write(handle, buffer, 65);
 		if (res < 0) {
 			DEBUG_WARN( "Error: %ls\n", hid_error(handle));
 			exit(-1);
 		}
-		res = hid_read(handle, buffer, report_size + 1);
+		res = hid_read_timeout(handle, buffer, 65, 1000);
 		if (res < 0) {
 			DEBUG_WARN( "debugger read(): %ls\n", hid_error(handle));
 			exit(-1);
+		} else if (res == 0) {
+			DEBUG_WARN( "timeout\n");
+			exit(-1);
 		}
-	} else if (type == BMP_TYPE_CMSIS_DAP_V2) {
+	} else if (type == CMSIS_TYPE_BULK) {
 		int transferred = 0;
 
-		res = libusb_bulk_transfer(usb_handle, out_ep, buffer + 1, rsize, &transferred, 0);
+		res = libusb_bulk_transfer(usb_handle, out_ep, data, rsize, &transferred, 500);
 		if (res < 0) {
-			DEBUG_WARN( "OUT error\n" );
+			DEBUG_WARN("OUT error: %d\n", res);
+			return res;
 		}
-		res = libusb_bulk_transfer(usb_handle, in_ep, buffer, report_size, &transferred, 0);
+		res = libusb_bulk_transfer(usb_handle, in_ep, buffer, report_size, &transferred, 500);
 		if (res < 0) {
-			DEBUG_WARN( "IN error\n" );
+			DEBUG_WARN("IN error: %d\n", res);
+			return res;
 		}
 		res = transferred;
 	}
-	if (buffer[0] != cmd) {
-		DEBUG_WARN("cmd %02x invalid response received %02x\n",
-				   cmd, buffer[0]);
-	}
 	DEBUG_WIRE("cmd res:");
 	for(int i = 0; (i < 16) && (i < size + 1); i++)
 		DEBUG_WIRE("%02x.",	buffer[i]);
 	DEBUG_WIRE("\n");
+	if (buffer[0] != cmd) {
+		DEBUG_WARN("cmd %02x not implemented\n", cmd);
+		buffer[1] = 0xff /*DAP_ERROR*/;
+	}
 	if (size)
 		memcpy(data, &buffer[1], (size < res) ? size : res);
 	return res;
@@ -258,7 +279,7 @@ static void dap_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 		return dap_read_single(ap, dest, src, align);
 	/* One word transfer for every byte/halfword/word
 	 * Total number of bytes in transfer*/
-	unsigned int max_size = (dbg_get_report_size() - 5) >> (2 - align);
+	unsigned int max_size = ((dbg_get_report_size() - 6) >> (2 - align)) & ~3;
 	while (len) {
 		dap_ap_mem_access_setup(ap, src, align);
 		/* Calculate length until next access setup is needed */
@@ -295,7 +316,7 @@ static void dap_mem_write_sized(
 		dest, len, align, *(uint32_t *)src);
 	if (((unsigned)(1 << align)) == len)
 		return dap_write_single(ap, dest, src, align);
-	unsigned int max_size = (dbg_get_report_size() - 5) >> (2 - align);
+	unsigned int max_size = ((dbg_get_report_size() - 6) >> (2 - align) & ~3);
 	while (len) {
 		dap_ap_mem_access_setup(ap, dest, align);
 		unsigned int blocksize = (dest | 0x3ff) - dest + 1;
@@ -396,36 +417,6 @@ int dap_jtag_dp_init(ADIv5_DP_t *dp)
 
 #define SWD_SEQUENCE_IN 0x80
 #define DAP_SWD_SEQUENCE 0x1d
-/* DAP_SWD_SEQUENCE does not do auto turnaround*/
-static bool dap_dp_low_read(ADIv5_DP_t *dp, uint16_t addr, uint32_t *res)
-{
-	(void)dp;
-	unsigned int paket_request = make_packet_request(ADIV5_LOW_READ, addr);
-	uint8_t buf[32] = {
-		DAP_SWD_SEQUENCE,
-		5,
-		8,
-		paket_request,
-		4 + SWD_SEQUENCE_IN,  /* one turn-around + read 3 bit ACK */
-		32 + SWD_SEQUENCE_IN, /* read 32 bit data */
-		1 + SWD_SEQUENCE_IN,  /* read parity bit */
-		1,                    /* one bit turn around to drive SWDIO */
-		0
-	};
-	dbg_dap_cmd(buf, sizeof(buf), 9);
-	if (buf[0])
-		DEBUG_WARN("dap_dp_low_read failed\n");
-	uint32_t ack = (buf[1] >> 1) & 7;
-	uint32_t data = (buf[2] << 0) + (buf[3] << 8) + (buf[4] << 16)
-		+ (buf[5] << 24);
-	int parity = __builtin_parity(data);
-	bool ret = ((parity != buf[6]) || (ack != 1));
-	*res = data;
-	DEBUG_PROBE("dap_dp_low_read ack %d, res %08" PRIx32 ", parity %s\n", ack,
-			   data, (ret)? "ERR": "OK");
-	return ret;
-}
-
 static bool dap_dp_low_write(ADIv5_DP_t *dp, uint16_t addr, const uint32_t data)
 {
 	DEBUG_PROBE("dap_dp_low_write %08" PRIx32 "\n", data);
@@ -464,15 +455,13 @@ int dap_swdptap_init(ADIv5_DP_t *dp)
 	dap_connect(false);
 	dap_led(0, 1);
 	dap_reset_link(false);
-	if (has_swd_sequence) {
+	if ((has_swd_sequence)  && dap_sequence_test()) {
 		/* DAP_SWD_SEQUENCE does not do auto turnaround, use own!*/
-		dp->dp_low_read = dap_dp_low_read;
 		dp->dp_low_write = dap_dp_low_write;
 	} else {
-		dp->error = dap_dp_error;
+		dp->dp_low_write = NULL;
 	}
 	dp->seq_out = dap_swdptap_seq_out;
-	dp->seq_out_parity = dap_swdptap_seq_out_parity;
 	dp->dp_read = dap_dp_read_reg;
 	/* For error() use the TARGETID switching firmware_swdp_error */
 	dp->low_access = dap_dp_low_access;

src/platforms/hosted/cmsis_dap.h

@@ -31,6 +31,7 @@ int dap_swdptap_init(ADIv5_DP_t *dp);
 int dap_jtag_dp_init(ADIv5_DP_t *dp);
 uint32_t dap_swj_clock(uint32_t clock);
 void dap_swd_configure(uint8_t cfg);
+void dap_srst_set_val(bool assert);
 #else
 int dap_init(bmp_info_t *info)
 {
@@ -47,6 +48,7 @@ int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc) {return -1;}
 int dap_swdptap_init(ADIv5_DP_t *dp) {return -1;}
 int dap_jtag_dp_init(ADIv5_DP_t *dp) {return -1;}
 void dap_swd_configure(uint8_t cfg) {};
+void dap_srst_set_val(assert) {};
 # pragma GCC diagnostic pop
 
 #endif

src/platforms/hosted/dap.c

@@ -32,6 +32,7 @@
 
 /*- Includes ----------------------------------------------------------------*/
 #include <general.h>
+#include "exception.h"
 #include "dap.h"
 #include "jtag_scan.h"
 
@@ -197,7 +198,7 @@ void dap_connect(bool jtag)
 //-----------------------------------------------------------------------------
 void dap_disconnect(void)
 {
-	uint8_t buf[1];
+	uint8_t buf[65];
 
 	buf[0] = ID_DAP_DISCONNECT;
 	dbg_dap_cmd(buf, sizeof(buf), 1);
@@ -277,10 +278,7 @@ void dap_reset_pin(int state)
 	buf[1] = state ? DAP_SWJ_nRESET : 0; // Value
 	buf[2] = DAP_SWJ_nRESET; // Select
 	buf[3] = 0; // Wait
-	buf[4] = 0;
+	dbg_dap_cmd(buf, sizeof(buf), 4);
-	buf[5] = 0;
-	buf[6] = 0;
-	dbg_dap_cmd(buf, sizeof(buf), 7);
 }
 
 void dap_trst_reset(void)
@@ -326,21 +324,19 @@ static uint32_t wait_word(uint8_t *buf, int size, int len, uint8_t *dp_fault)
 	uint8_t cmd_copy[len];
 	memcpy(cmd_copy, buf, len);
 	do {
+		memcpy(buf, cmd_copy, len);
 		dbg_dap_cmd(buf, size, len);
 		if (buf[1] < DAP_TRANSFER_WAIT)
 			break;
-		if (buf[1] == DAP_TRANSFER_WAIT)
-			memcpy(buf, cmd_copy, len);
 	} while (buf[1] == DAP_TRANSFER_WAIT);
 
-	if (buf[1] > DAP_TRANSFER_WAIT) {
+	if(buf[1] == SWDP_ACK_FAULT) {
-//	  DEBUG_WARN("dap_read_reg fault\n");
 		*dp_fault = 1;
+		return 0;
 	}
-	if (buf[1] == DAP_TRANSFER_ERROR) {
+
-		DEBUG_WARN("dap_read_reg, protocoll error\n");
+	if(buf[1] != SWDP_ACK_OK)
-		dap_line_reset();
+		raise_exception(EXCEPTION_ERROR, "SWDP invalid ACK");
-	}
 	uint32_t res =
 		((uint32_t)buf[5] << 24) | ((uint32_t)buf[4] << 16) |
 		((uint32_t)buf[3] << 8) | (uint32_t)buf[2];
@@ -378,7 +374,10 @@ void dap_write_reg(ADIv5_DP_t *dp, uint8_t reg, uint32_t data)
 	buf[5] = (data >> 8) & 0xff;
 	buf[6] = (data >> 16) & 0xff;
 	buf[7] = (data >> 24) & 0xff;
+	uint8_t cmd_copy[8];
+	memcpy(cmd_copy, buf, 8);
 	do {
+		memcpy(buf, cmd_copy, 8);
 		dbg_dap_cmd(buf, sizeof(buf), 8);
 		if (buf[1] < DAP_TRANSFER_WAIT)
 			break;
@@ -566,7 +565,7 @@ void dap_ap_mem_access_setup(ADIv5_AP_t *ap, uint32_t addr, enum align align)
 
 uint32_t dap_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 {
-	DEBUG_PROBE("dap_ap_read_start\n");
+	DEBUG_PROBE("dap_ap_read_start addr %x\n", addr);
 	uint8_t buf[63], *p = buf;
 	buf[0] = ID_DAP_TRANSFER;
 	uint8_t dap_index = 0;
@@ -582,6 +581,9 @@ uint32_t dap_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 	*p++ = (addr & 0x0c) | DAP_TRANSFER_RnW  |
 		((addr & 0x100) ?  DAP_TRANSFER_APnDP : 0);
 	uint32_t res = wait_word(buf, 63, p - buf, &ap->dp->fault);
+	if ((buf[0] != 2) || (buf[1] != 1)) {
+		DEBUG_WARN("dap_ap_read error %x\n", buf[1]);
+	}
 	return res;
 }
 
@@ -605,6 +607,9 @@ void dap_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 	*p++ = (value >> 16) & 0xff;
 	*p++ = (value >> 24) & 0xff;
 	dbg_dap_cmd(buf, sizeof(buf), p - buf);
+	if ((buf[0] != 2) || (buf[1] != 1)) {
+		DEBUG_WARN("dap_ap_write error %x\n", buf[1]);
+	}
 }
 
 void dap_read_single(ADIv5_AP_t *ap, void *dest, uint32_t src, enum align align)
@@ -612,7 +617,8 @@ void dap_read_single(ADIv5_AP_t *ap, void *dest, uint32_t src, enum align align)
 	uint8_t buf[63];
 	uint8_t *p = mem_access_setup(ap, buf, src, align);
 	*p++ = SWD_AP_DRW | DAP_TRANSFER_RnW;
-	buf[2] = 4;
+	*p++ = SWD_DP_R_RDBUFF | DAP_TRANSFER_RnW;
+	buf[2] = 5;
 	uint32_t tmp = wait_word(buf, 63, p - buf, &ap->dp->fault);
 	dest = extract(dest, src, tmp, align);
 }
@@ -758,7 +764,7 @@ int dap_jtag_configure(void)
 
 void dap_swdptap_seq_out(uint32_t MS, int ticks)
 {
-	uint8_t buf[] = {
+	uint8_t buf[64] = {
 		ID_DAP_SWJ_SEQUENCE,
 		ticks,
 		(MS >>  0) & 0xff,
@@ -766,7 +772,7 @@ void dap_swdptap_seq_out(uint32_t MS, int ticks)
 		(MS >> 16) & 0xff,
 		(MS >> 24) & 0xff
 	};
-	dbg_dap_cmd(buf, 1, sizeof(buf));
+	dbg_dap_cmd(buf, 64, 2 + ((ticks +7) >> 3));
 	if (buf[0])
 		DEBUG_WARN("dap_swdptap_seq_out error\n");
 }
@@ -787,6 +793,17 @@ void dap_swdptap_seq_out_parity(uint32_t MS, int ticks)
 		DEBUG_WARN("dap_swdptap_seq_out error\n");
 }
 
+bool dap_sequence_test(void)
+{
+	uint8_t buf[4] = {
+		ID_DAP_SWD_SEQUENCE,
+		1,
+		0 /* one idle cycle */
+	};
+	dbg_dap_cmd(buf, sizeof(buf), 3);
+	return (buf[0] == DAP_OK);
+}
+
 #define SWD_SEQUENCE_IN 0x80
 uint32_t dap_swdptap_seq_in(int ticks)
 {

src/platforms/hosted/dap.h

@@ -69,6 +69,7 @@ void dap_transfer_configure(uint8_t idle, uint16_t count, uint16_t retry);
 void dap_swd_configure(uint8_t cfg);
 int dap_info(int info, uint8_t *data, int size);
 void dap_reset_target(void);
+void dap_srst_set_val(bool assert);
 void dap_trst_reset(void);
 void dap_reset_target_hw(int state);
 void dap_reset_pin(int state);
@@ -92,4 +93,5 @@ void dap_jtagtap_tdi_tdo_seq(uint8_t *DO, bool final_tms, const uint8_t *TMS,
 int dap_jtag_configure(void);
 void dap_swdptap_seq_out(uint32_t MS, int ticks);
 void dap_swdptap_seq_out_parity(uint32_t MS, int ticks);
+bool dap_sequence_test(void);
 #endif // _DAP_H_

src/platforms/hosted/ftdi_bmp.c

@@ -379,7 +379,7 @@ int ftdi_bmp_init(BMP_CL_OPTIONS_t *cl_opts, bmp_info_t *info)
 	case TYPE_2232H:
 	case TYPE_4232H:
 	case TYPE_232H:
-		ftdi_init[index++] = EN_DIV_5;
+		ftdi_init[index++] = DIS_DIV_5;
 		break;
 	case TYPE_2232C:
 		break;

src/platforms/hosted/platform.c

@@ -52,8 +52,7 @@ static void exit_function(void)
 {
 	libusb_exit_function(&info);
 	switch (info.bmp_type) {
-	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP:
-	case BMP_TYPE_CMSIS_DAP_V2:
 		dap_exit_function();
 		break;
 	default:
@@ -93,8 +92,7 @@ void platform_init(int argc, char **argv)
 		if (stlink_init( &info))
 			exit(-1);
 		break;
-	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP:
-	case BMP_TYPE_CMSIS_DAP_V2:
 		if (dap_init( &info))
 			exit(-1);
 		break;
@@ -126,8 +124,7 @@ int platform_adiv5_swdp_scan(uint32_t targetid)
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 	case BMP_TYPE_LIBFTDI:
-	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP:
-	case BMP_TYPE_CMSIS_DAP_V2:
 		return adiv5_swdp_scan(targetid);
 		break;
 	case BMP_TYPE_STLINKV2:
@@ -156,8 +153,7 @@ int swdptap_init(ADIv5_DP_t *dp)
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 		return remote_swdptap_init(dp);
-	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP:
-	case BMP_TYPE_CMSIS_DAP_V2:
 		return dap_swdptap_init(dp);
 	case BMP_TYPE_STLINKV2:
 	case BMP_TYPE_JLINK:
@@ -184,8 +180,7 @@ int platform_jtag_scan(const uint8_t *lrlens)
 	case BMP_TYPE_BMP:
 	case BMP_TYPE_LIBFTDI:
 	case BMP_TYPE_JLINK:
-	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP:
-	case BMP_TYPE_CMSIS_DAP_V2:
 		return jtag_scan(lrlens);
 	case BMP_TYPE_STLINKV2:
 		return jtag_scan_stlinkv2(&info, lrlens);
@@ -206,8 +201,7 @@ int platform_jtagtap_init(void)
 		return libftdi_jtagtap_init(&jtag_proc);
 	case BMP_TYPE_JLINK:
 		return jlink_jtagtap_init(&info, &jtag_proc);
-	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP:
-	case BMP_TYPE_CMSIS_DAP_V2:
 		return cmsis_dap_jtagtap_init(&jtag_proc);
 	default:
 		return -1;
@@ -217,6 +211,7 @@ int platform_jtagtap_init(void)
 
 void platform_adiv5_dp_defaults(ADIv5_DP_t *dp)
 {
+	dp->dp_bmp_type = info.bmp_type;
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 		if (cl_opts.opt_no_hl) {
@@ -226,8 +221,7 @@ void platform_adiv5_dp_defaults(ADIv5_DP_t *dp)
 		return remote_adiv5_dp_defaults(dp);
 	case BMP_TYPE_STLINKV2:
 		return stlink_adiv5_dp_defaults(dp);
-	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP:
-	case BMP_TYPE_CMSIS_DAP_V2:
 		return dap_adiv5_dp_defaults(dp);
 	default:
 		break;
@@ -243,8 +237,7 @@ int platform_jtag_dp_init(ADIv5_DP_t *dp)
 		return 0;
 	case BMP_TYPE_STLINKV2:
 		return stlink_jtag_dp_init(dp);
-	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP:
-	case BMP_TYPE_CMSIS_DAP_V2:
 		return dap_jtag_dp_init(dp);
 	default:
 		return 0;
@@ -263,10 +256,8 @@ char *platform_ident(void)
 		return "STLINKV2";
 	  case BMP_TYPE_LIBFTDI:
 		return "LIBFTDI";
-	  case BMP_TYPE_CMSIS_DAP_V1:
+	  case BMP_TYPE_CMSIS_DAP:
-		return "CMSIS_DAP_V1";
+		return "CMSIS_DAP";
-	  case BMP_TYPE_CMSIS_DAP_V2:
-		return "CMSIS_DAP_V2";
 	  case BMP_TYPE_JLINK:
 		return "JLINK";
 	}
@@ -301,6 +292,8 @@ void platform_srst_set_val(bool assert)
 		return jlink_srst_set_val(&info, assert);
 	case BMP_TYPE_LIBFTDI:
 		return libftdi_srst_set_val(assert);
+	case BMP_TYPE_CMSIS_DAP:
+		return dap_srst_set_val(assert);
 	default:
 		break;
 	}
@@ -331,8 +324,7 @@ void platform_max_frequency_set(uint32_t freq)
 	case BMP_TYPE_BMP:
 		remote_max_frequency_set(freq);
 		break;
-	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP:
-	case BMP_TYPE_CMSIS_DAP_V2:
 		dap_swj_clock(freq);
 		break;
 	case BMP_TYPE_LIBFTDI:
@@ -363,8 +355,7 @@ uint32_t platform_max_frequency_get(void)
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 		return remote_max_frequency_get();
-	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP:
-	case BMP_TYPE_CMSIS_DAP_V2:
 		return dap_swj_clock(0);
 		break;
 	case BMP_TYPE_LIBFTDI:

src/platforms/hosted/platform.h

@@ -23,6 +23,7 @@ void platform_buffer_flush(void);
 #define PRODUCT_ID_STLINKV2		 0x3748
 #define PRODUCT_ID_STLINKV21	 0x374b
 #define PRODUCT_ID_STLINKV21_MSD 0x3752
+#define PRODUCT_ID_STLINKV3_NO_MSD 0x3754
 #define PRODUCT_ID_STLINKV3_BL	 0x374d
 #define PRODUCT_ID_STLINKV3		 0x374f
 #define PRODUCT_ID_STLINKV3E	 0x374e
@@ -34,8 +35,7 @@ typedef enum bmp_type_s {
 	BMP_TYPE_BMP,
 	BMP_TYPE_STLINKV2,
 	BMP_TYPE_LIBFTDI,
-	BMP_TYPE_CMSIS_DAP_V1,
+	BMP_TYPE_CMSIS_DAP,
-	BMP_TYPE_CMSIS_DAP_V2,
 	BMP_TYPE_JLINK
 } bmp_type_t;
 

src/platforms/hosted/stlinkv2.c

@@ -553,6 +553,7 @@ int stlink_init(bmp_info_t *info)
 	case PRODUCT_ID_STLINKV3_BL:
 	case PRODUCT_ID_STLINKV3:
 	case PRODUCT_ID_STLINKV3E:
+	case PRODUCT_ID_STLINKV3_NO_MSD:
 		Stlink.ver_hw = 30;
 		info->usb_link->ep_tx = 1;
 		Stlink.ep_tx = 1;
@@ -763,9 +764,10 @@ uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 	int res;
 	if (RnW) {
 		res = stlink_read_dp_register(
-			STLINK_DEBUG_PORT_ACCESS, addr, &response);
+			(addr < 0x100) ? STLINK_DEBUG_PORT_ACCESS : 0, addr, &response);
 	} else {
-		res = stlink_write_dp_register(STLINK_DEBUG_PORT_ACCESS, addr, value);
+		res = stlink_write_dp_register(
+			(addr < 0x100) ? STLINK_DEBUG_PORT_ACCESS : 0, addr, value);
 	}
 	if (res == STLINK_ERROR_WAIT)
 		raise_exception(EXCEPTION_TIMEOUT, "DP ACK timeout");
@@ -1048,7 +1050,7 @@ int stlink_enter_debug_swd(bmp_info_t *info, ADIv5_DP_t *dp)
 	uint8_t data[2];
 	stlink_send_recv_retry(cmd, 16, data, 2);
 	if (stlink_usb_error_check(data, true))
-		return -1;
+		exit( -1);
 	dp->idcode = stlink_read_coreid();
 	dp->dp_read = stlink_dp_read;
 	dp->error = stlink_dp_error;
@@ -1056,6 +1058,12 @@ int stlink_enter_debug_swd(bmp_info_t *info, ADIv5_DP_t *dp)
 	dp->abort = stlink_dp_abort;
 
 	stlink_dp_error(dp);
+	if ((dp->idcode & ADIV5_DP_VERSION_MASK) == ADIV5_DPv2) {
+		adiv5_dp_write(dp, ADIV5_DP_SELECT, 2);
+		dp->targetid = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT);
+		adiv5_dp_write(dp, ADIV5_DP_SELECT, 0);
+		DEBUG_INFO("TARGETID 0x%08" PRIx32 "\n", dp->targetid);
+	}
 	return 0;
 }
 

src/target/adiv5.c

@@ -323,24 +323,32 @@ static uint32_t cortexm_initial_halt(ADIv5_AP_t *ap)
 		CORTEXM_DHCSR_C_HALT;
 	uint32_t dhcsr_valid = CORTEXM_DHCSR_S_HALT | CORTEXM_DHCSR_C_DEBUGEN;
 	bool reset_seen = false;
-	bool is_mindp = (ap->dp->idcode & ADIV5_MINDP);
+	bool use_low_access = (!(ap->dp->idcode & ADIV5_MINDP));
-#if  PC_HOSTED == 1
-	bool use_low_access = (!(ap->dp->ap_setup) && !is_mindp);
-#else
-	bool use_low_access = (!is_mindp);
-#endif
 	if (use_low_access) {
 		/* ap_mem_access_setup() sets ADIV5_AP_CSW_ADDRINC_SINGLE -> unusable!*/
 		adiv5_ap_write(ap, ADIV5_AP_CSW, ap->csw | ADIV5_AP_CSW_SIZE_WORD);
 		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_TAR, CORTEXM_DHCSR);
 	}
+	 /* Workaround for CMSIS-DAP Bulk orbtrace
+	  * High values of TRNCNT lead to NO_ACK answer from debugger.
+	  *
+	  * However CMSIS/HID even with highest value has few chances to catch
+	  * a STM32F767 mostly sleeping in WFI!
+	  */
+	uint32_t start_time = platform_time_ms();
+	int trncnt = 0x80;
 	while (!platform_timeout_is_expired(&to)) {
 		uint32_t dhcsr ;
 		if (use_low_access) {
-			adiv5_dp_write(ap->dp, ADIV5_DP_CTRLSTAT,
+			adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_DP_CTRLSTAT,
-						   ctrlstat | (0xfff * ADIV5_DP_CTRLSTAT_TRNCNT));
+								ctrlstat | (trncnt * ADIV5_DP_CTRLSTAT_TRNCNT));
 			adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DRW,
 								dhcsr_ctl);
+			if (trncnt < 0xfff) {
+				trncnt += (platform_time_ms() -  start_time) * 8;
+			} else {
+				trncnt = 0xfff;
+			}
 			dhcsr = adiv5_dp_low_access(
 				ap->dp, ADIV5_LOW_READ, ADIV5_AP_DRW, 0);
 		} else {

src/target/adiv5.h

@@ -170,11 +170,9 @@ typedef struct ADIv5_DP_s {
 	void (*seq_out_parity)(uint32_t MS, int ticks);
 	uint32_t (*seq_in)(int ticks);
 	bool (*seq_in_parity)(uint32_t *ret, int ticks);
-	/* dp_low_write returns true if no OK resonse. */
+	/* dp_low_write returns true if no OK resonse, but ignores errors */
 	bool (*dp_low_write)(struct ADIv5_DP_s *dp, uint16_t addr,
 						 const uint32_t data);
-	/* dp_low_read returns true with parity error */
-	bool (*dp_low_read)(struct ADIv5_DP_s *dp, uint16_t addr, uint32_t *data);
 	uint32_t (*dp_read)(struct ADIv5_DP_s *dp, uint16_t addr);
 	uint32_t (*error)(struct ADIv5_DP_s *dp);
 	uint32_t (*low_access)(struct ADIv5_DP_s *dp, uint8_t RnW,
@@ -182,6 +180,7 @@ typedef struct ADIv5_DP_s {
 	void (*abort)(struct ADIv5_DP_s *dp, uint32_t abort);
 
 #if PC_HOSTED == 1
+	bmp_type_t dp_bmp_type;
 	bool (*ap_setup)(int i);
 	void (*ap_cleanup)(int i);
     void (*ap_regs_read)(ADIv5_AP_t *ap, void *data);

src/target/adiv5_swdp.c

@@ -61,14 +61,6 @@ bool firmware_dp_low_write(ADIv5_DP_t *dp, uint16_t addr, const uint32_t data)
 	return (res != 1);
 }
 
-static bool firmware_dp_low_read(ADIv5_DP_t *dp, uint16_t addr, uint32_t *res)
-{
-	unsigned int request =  make_packet_request(ADIV5_LOW_READ, addr & 0xf);
-	dp->seq_out(request, 8);
-	dp->seq_in(3);
-    return dp->seq_in_parity(res, 32);
-}
-
 /* Try first the dormant to SWD procedure.
  * If target id given, scan DPs 0 .. 15 on that device and return.
  * Otherwise
@@ -78,7 +70,6 @@ int adiv5_swdp_scan(uint32_t targetid)
 	target_list_free();
 	ADIv5_DP_t idp = {
 		.dp_low_write = firmware_dp_low_write,
-		.dp_low_read = firmware_dp_low_read,
 		.error = firmware_swdp_error,
 		.dp_read = firmware_swdp_read,
 		.low_access = firmware_swdp_low_access,
@@ -101,18 +92,17 @@ int adiv5_swdp_scan(uint32_t targetid)
 	initial_dp->seq_out(0x1a0, 12);
 	uint32_t idcode = 0;
 	volatile uint32_t target_id;
-	bool is_v2 = true;
+	bool scan_multidrop = true;
-	if (!targetid || (initial_dp->error != firmware_swdp_error)) {
+	if (!targetid || !initial_dp->dp_low_write) {
 		/* No targetID given on the command line or probe can not
 		 * handle multi-drop. Try to read ID */
 		dp_line_reset(initial_dp);
 		volatile struct exception e;
 		TRY_CATCH (e, EXCEPTION_ALL) {
-			idcode = initial_dp->low_access(initial_dp, ADIV5_LOW_READ,
+			idcode = initial_dp->dp_read(initial_dp, ADIV5_DP_IDCODE);
-										   ADIV5_DP_IDCODE, 0);
 		}
 		if (e.type || initial_dp->fault) {
-			is_v2 = false;
+			scan_multidrop = false;
 			DEBUG_WARN("Trying old JTAG to SWD sequence\n");
 			initial_dp->seq_out(0xFFFFFFFF, 32);
 			initial_dp->seq_out(0xFFFFFFFF, 32);
@@ -121,16 +111,15 @@ int adiv5_swdp_scan(uint32_t targetid)
 			initial_dp->fault = 0;
 			volatile struct exception e2;
 			TRY_CATCH (e2, EXCEPTION_ALL) {
-				idcode = initial_dp->low_access(initial_dp, ADIV5_LOW_READ,
+				idcode = initial_dp->dp_read(initial_dp, ADIV5_DP_IDCODE);
-									  ADIV5_DP_IDCODE, 0);
 			}
-			if (e2.type) {
+			if (e2.type || initial_dp->fault) {
 				DEBUG_WARN("No usable DP found\n");
 				return 0;
 			}
 		}
 		if ((idcode & ADIV5_DP_VERSION_MASK) == ADIV5_DPv2) {
-			is_v2 = true;
+			scan_multidrop = true;
 			/* Read TargetID. Can be done with device in WFI, sleep or reset!*/
 			adiv5_dp_write(initial_dp, ADIV5_DP_SELECT, 2);
 			target_id = adiv5_dp_read(initial_dp, ADIV5_DP_CTRLSTAT);
@@ -142,31 +131,34 @@ int adiv5_swdp_scan(uint32_t targetid)
 				adiv5_dp_write(initial_dp, ADIV5_DP_CTRLSTAT, 0);
 				break;
 			}
-			if (initial_dp->error != firmware_swdp_error) {
+			if (!initial_dp->dp_low_write) {
 				DEBUG_WARN("CMSIS_DAP < V1.2 can not handle multi-drop!\n");
 				/* E.g. CMSIS_DAP < V1.2 can not handle multi-drop!*/
-				is_v2 = false;
+				scan_multidrop = false;
 			}
 		} else {
-			is_v2 = false;
+			scan_multidrop = false;
 		}
 	} else {
 		target_id = targetid;
 	}
-	int nr_dps = (is_v2) ? 16: 1;
+	volatile int nr_dps = (scan_multidrop) ? 16: 1;
-	uint32_t dp_targetid;
+	volatile uint32_t dp_targetid;
-	for (int i = 0; i < nr_dps; i++) {
+	for (volatile int i = 0; i < nr_dps; i++) {
-		if (is_v2) {
+		if (scan_multidrop) {
 			dp_line_reset(initial_dp);
 			dp_targetid = (i << 28) | (target_id & 0x0fffffff);
 			initial_dp->dp_low_write(initial_dp, ADIV5_DP_TARGETSEL,
 									dp_targetid);
-			if (initial_dp->dp_low_read(initial_dp, ADIV5_DP_IDCODE,
+			volatile struct exception e;
-										&idcode)) {
+			TRY_CATCH (e, EXCEPTION_ALL) {
+				idcode = initial_dp->dp_read(initial_dp, ADIV5_DP_IDCODE);
+			}
+			if (e.type || initial_dp->fault) {
 				continue;
 			}
 		} else {
-			dp_targetid = 0;
+			dp_targetid = target_id;
 		}
 		ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
 		if (!dp) {			/* calloc failed: heap exhaustion */
@@ -195,17 +187,17 @@ uint32_t firmware_swdp_read(ADIv5_DP_t *dp, uint16_t addr)
 
  uint32_t firmware_swdp_error(ADIv5_DP_t *dp)
 {
-	uint32_t err, clr = 0;
+	if ((dp->fault && (dp->idcode & ADIV5_DP_VERSION_MASK) == ADIV5_DPv2) &&
-
+		dp->dp_low_write) {
-	if ((dp->idcode & ADIV5_DP_VERSION_MASK) == ADIV5_DPv2) {
 		/* On protocoll error target gets deselected.
 		 * With DP Change, another target needs selection.
 		 * => Reselect with right target! */
 		dp_line_reset(dp);
 		dp->dp_low_write(dp, ADIV5_DP_TARGETSEL, dp->targetid);
-		uint32_t dummy;
+		dp->dp_read(dp, ADIV5_DP_IDCODE);
-		dp->dp_low_read(dp, ADIV5_DP_IDCODE, &dummy);
+		/* Exception here is unexpected, so do not catch */
 	}
+	uint32_t err, clr = 0;
 	err = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT) &
 		(ADIV5_DP_CTRLSTAT_STICKYORUN | ADIV5_DP_CTRLSTAT_STICKYCMP |
 		ADIV5_DP_CTRLSTAT_STICKYERR | ADIV5_DP_CTRLSTAT_WDATAERR);

src/target/cortexm.c

@@ -478,6 +478,9 @@ bool cortexm_probe(ADIv5_AP_t *ap)
 
 bool cortexm_attach(target *t)
 {
+	ADIv5_AP_t *ap = cortexm_ap(t);
+	ap->dp->fault = 1; /* Force switch to this multi-drop device*/
+	target_check_error(t);
 	struct cortexm_priv *priv = t->priv;
 	unsigned i;
 	uint32_t r;

src/target/stm32l4.c

@@ -507,22 +507,31 @@ static void stm32l4_detach(target *t)
 
 bool stm32l4_probe(target *t)
 {
-	uint32_t idcode_reg = STM32L4_DBGMCU_IDCODE_PHYS;
 	ADIv5_AP_t *ap = cortexm_ap(t);
-	if (ap->dp->idcode == 0x0Be12477) {
+	uint32_t idcode;
-		idcode_reg = STM32L5_DBGMCU_IDCODE_PHYS;
+	if (ap->dp->targetid > 1) { /* STM32L552 has in valid TARGETID 1 */
-		if ((stm32l4_flash_read32(t, FLASH_OPTR)) & L5_FLASH_OPTR_TZEN) {
+		idcode = (ap->dp->targetid >> 16) & 0xfff;
-			DEBUG_WARN("STM32L5 Trust Zone enabled\n");
+	} else {
-		}
+		uint32_t idcode_reg = STM32L4_DBGMCU_IDCODE_PHYS;
+		if (ap->dp->idcode == 0x0Be12477)
+			idcode_reg = STM32L5_DBGMCU_IDCODE_PHYS;
+		idcode = target_mem_read32(t, idcode_reg) & 0xfff;
+		DEBUG_INFO("Idcode %08" PRIx32 "\n", idcode);
 	}
-	uint32_t idcode = target_mem_read32(t, idcode_reg) & 0xfff;
-	DEBUG_INFO("Read %" PRIx32 ": %" PRIx32 "\n", idcode_reg, idcode);
 
 	struct stm32l4_info const *chip = stm32l4_get_chip_info(idcode);
 
 	if( !chip->idcode )	/* Not found */
 		return false;
 
+	switch (idcode) {
+		case ID_STM32L55:
+			if ((stm32l4_flash_read32(t, FLASH_OPTR)) & L5_FLASH_OPTR_TZEN) {
+				DEBUG_WARN("STM32L5 Trust Zone enabled\n");
+				t->core = "M33(TZ)";
+				break;
+			}
+	}
 	t->driver = chip->designator;
 	t->attach = stm32l4_attach;
 	t->detach = stm32l4_detach;