diff --git a/src/platforms/hosted/bmp_hosted.h b/src/platforms/hosted/bmp_hosted.h
index 721c648b..fb6b7cfe 100644
--- a/src/platforms/hosted/bmp_hosted.h
+++ b/src/platforms/hosted/bmp_hosted.h
@@ -38,6 +38,9 @@ typedef struct bmp_info_s {
 	usb_link_t *usb_link;
 	unsigned int vid;
 	unsigned int pid;
+	uint8_t interface_num;
+	uint8_t in_ep;
+	uint8_t out_ep;
 #endif
 } bmp_info_t;
 
diff --git a/src/platforms/hosted/bmp_libusb.c b/src/platforms/hosted/bmp_libusb.c
index 0264f25b..743f25b1 100644
--- a/src/platforms/hosted/bmp_libusb.c
+++ b/src/platforms/hosted/bmp_libusb.c
@@ -24,18 +24,6 @@
 #include "ftdi_bmp.h"
 #include "version.h"
 
-#define VENDOR_ID_STLINK         0x0483
-#define PRODUCT_ID_STLINK_MASK   0xffe0
-#define PRODUCT_ID_STLINK_GROUP  0x3740
-#define PRODUCT_ID_STLINKV1      0x3744
-#define PRODUCT_ID_STLINKV2      0x3748
-#define PRODUCT_ID_STLINKV21     0x374b
-#define PRODUCT_ID_STLINKV21_MSD 0x3752
-#define PRODUCT_ID_STLINKV3      0x374f
-#define PRODUCT_ID_STLINKV3E     0x374e
-
-#define VENDOR_ID_SEGGER         0x1366
-
 #define NO_SERIAL_NUMBER "<no serial number>"
 
 void bmp_ident(bmp_info_t *info)
@@ -62,6 +50,73 @@ void libusb_exit_function(bmp_info_t *info)
 	}
 }
 
+static bmp_type_t find_cmsis_dap_interface(libusb_device *dev,bmp_info_t *info) {
+	bmp_type_t type = BMP_TYPE_NONE;
+
+	struct libusb_config_descriptor *conf;
+	char interface_string[128];
+
+	int res = libusb_get_active_config_descriptor(dev, &conf);
+	if (res < 0) {
+		DEBUG_WARN( "WARN: libusb_get_active_config_descriptor() failed: %s",
+				libusb_strerror(res));
+		return type;
+	}
+
+	libusb_device_handle *handle;
+	res = libusb_open(dev, &handle);
+	if (res != LIBUSB_SUCCESS) {
+		DEBUG_INFO("INFO: libusb_open() failed: %s\n",
+					libusb_strerror(res));
+		return type;
+	}
+
+	for (int i = 0; i < conf->bNumInterfaces; i++) {
+		const struct libusb_interface_descriptor *interface = &conf->interface[i].altsetting[0];
+
+		if (!interface->iInterface) {
+			continue;
+		}
+
+		res = libusb_get_string_descriptor_ascii(
+			handle, interface->iInterface, (uint8_t*)interface_string,
+			sizeof(interface_string));
+		if (res < 0) {
+			DEBUG_WARN( "WARN: libusb_get_string_descriptor_ascii() failed: %s\n",
+					libusb_strerror(res));
+			continue;
+		}
+
+		if (!strstr(interface_string, "CMSIS")) {
+			continue;
+		}
+
+		if (interface->bInterfaceClass == 0x03) {
+			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++) {
+				uint8_t n = interface->endpoint[j].bEndpointAddress;
+
+				if (n & 0x80) {
+					info->in_ep = n;
+				} else {
+					info->out_ep = n;
+				}
+			}
+
+			/* V2 is preferred, return early. */
+			return type;
+		}
+	}
+
+	return type;
+}
+
 int find_debuggers(BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 {
 	libusb_device **devs;
@@ -171,12 +226,15 @@ int find_debuggers(BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 				DEBUG_WARN("BMP in botloader mode found. Restart or reflash!\n");
 				continue;
 			}
+		} else if ((type = find_cmsis_dap_interface(dev, info)) != BMP_TYPE_NONE) {
+			/* type was set by the expression */
 		} else if ((strstr(manufacturer, "CMSIS")) || (strstr(product, "CMSIS"))) {
-			type = BMP_TYPE_CMSIS_DAP;
+			type = BMP_TYPE_CMSIS_DAP_V1;
 		} 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_BL) ||
 				(desc.idProduct == PRODUCT_ID_STLINKV3) ||
 				(desc.idProduct == PRODUCT_ID_STLINKV3E)) {
 				type = BMP_TYPE_STLINKV2;
diff --git a/src/platforms/hosted/cmsis_dap.c b/src/platforms/hosted/cmsis_dap.c
index b6ce2c4b..19c49e39 100644
--- a/src/platforms/hosted/cmsis_dap.c
+++ b/src/platforms/hosted/cmsis_dap.c
@@ -46,37 +46,58 @@ uint8_t dap_caps;
 uint8_t mode;
 
 /*- Variables ---------------------------------------------------------------*/
+static bmp_type_t type;
+static libusb_device_handle *usb_handle = NULL;
+static uint8_t in_ep;
+static uint8_t out_ep;
 static hid_device *handle = NULL;
-static uint8_t hid_buffer[1024 + 1];
+static uint8_t buffer[1024 + 1];
 static int report_size = 64 + 1; // TODO: read actual report size
 static bool has_swd_sequence = false;
 
 /* LPC845 Breakout Board Rev. 0 report invalid response with > 65 bytes */
 int dap_init(bmp_info_t *info)
 {
-	if (hid_init())
-		return -1;
-	int size = strlen(info->serial);
-	wchar_t serial[64] = {0}, *wc = serial;
-	for (int i = 0; i < size; i++)
-		*wc++ = info->serial[i];
-	*wc = 0;
-	/* Blacklist devices that do not work with 513 byte report length
-	 * FIXME: Find a solution to decipher from the device.
-	 */
-	if ((info->vid == 0x1fc9) && (info->pid == 0x0132)) {
-		DEBUG_WARN("Blacklist\n");
-		report_size = 64;
+	type = info->bmp_type;
+	int size;
+
+	if (type == BMP_TYPE_CMSIS_DAP_V1) {
+		if (hid_init())
+			return -1;
+		size = strlen(info->serial);
+		wchar_t serial[64] = {0}, *wc = serial;
+		for (int i = 0; i < size; i++)
+			*wc++ = info->serial[i];
+		*wc = 0;
+		/* Blacklist devices that do not work with 513 byte report length
+		* FIXME: Find a solution to decipher from the device.
+		*/
+		if ((info->vid == 0x1fc9) && (info->pid == 0x0132)) {
+			DEBUG_WARN("Blacklist\n");
+			report_size = 64 + 1;
+		}
+		handle = hid_open(info->vid, info->pid,  (serial[0]) ? serial : NULL);
+		if (!handle)
+			return -1;
+	} else if (type == BMP_TYPE_CMSIS_DAP_V2) {
+		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");
+			return -1;
+		}
+		if (libusb_claim_interface(usb_handle, info->interface_num) < 0) {
+			DEBUG_WARN("WARN: libusb_claim_interface() failed\n");
+			return -1;
+		}
+		in_ep = info->in_ep;
+		out_ep = info->out_ep;
 	}
-	handle = hid_open(info->vid, info->pid,  (serial[0]) ? serial : NULL);
-	if (!handle)
-		return -1;
 	dap_disconnect();
-	size = dap_info(DAP_INFO_FW_VER, hid_buffer, sizeof(hid_buffer));
+	size = dap_info(DAP_INFO_FW_VER, buffer, sizeof(buffer));
 	if (size) {
-		DEBUG_INFO("Ver %s, ", hid_buffer);
+		DEBUG_INFO("Ver %s, ", buffer);
 		int major = -1, minor = -1, sub = -1;
-		if (sscanf((const char *)hid_buffer, "%d.%d.%d",
+		if (sscanf((const char *)buffer, "%d.%d.%d",
 				   &major, &minor, &sub)) {
 			if (sub == -1) {
 				if (minor >= 10) {
@@ -87,8 +108,8 @@ int dap_init(bmp_info_t *info)
 			has_swd_sequence = ((major > 1 ) || ((major > 0 ) && (minor > 1)));
 		}
 	}
-	size = dap_info(DAP_INFO_CAPABILITIES, hid_buffer, sizeof(hid_buffer));
-	dap_caps = hid_buffer[0];
+	size = dap_info(DAP_INFO_CAPABILITIES, buffer, sizeof(buffer));
+	dap_caps = buffer[0];
 	DEBUG_INFO("Cap (0x%2x): %s%s%s", dap_caps,
 		   (dap_caps & 1)? "SWD" : "",
 		   ((dap_caps & 3) == 3) ? "/" : "",
@@ -99,11 +120,6 @@ int dap_init(bmp_info_t *info)
 		DEBUG_INFO(", SWO_MANCHESTER");
 	if (dap_caps & 0x10)
 		DEBUG_INFO(", Atomic Cmds");
-	size = dap_info(DAP_INFO_PACKET_SIZE, hid_buffer, sizeof(hid_buffer));
-	if (size) {
-		report_size = hid_buffer[0];
-		DEBUG_INFO(", Reportsize %d", hid_buffer[0]);
-	}
 	DEBUG_INFO("\n");
 	return 0;
 }
@@ -159,9 +175,16 @@ static uint32_t dap_dp_read_reg(ADIv5_DP_t *dp, uint16_t addr)
 
 void dap_exit_function(void)
 {
-	if (handle) {
-		dap_disconnect();
-		hid_close(handle);
+	if (type == BMP_TYPE_CMSIS_DAP_V1) {
+		if (handle) {
+			dap_disconnect();
+			hid_close(handle);
+		}
+	} else if (type == BMP_TYPE_CMSIS_DAP_V2) {
+		if (usb_handle) {
+			dap_disconnect();
+			libusb_close(usb_handle);
+		}
 	}
 }
 
@@ -174,38 +197,51 @@ int dbg_dap_cmd(uint8_t *data, int size, int rsize)
 
 {
 	char cmd = data[0];
-	int res;
+	int res = -1;
 
-	memset(hid_buffer, 0, report_size + 1);
+	memset(buffer, 0xff, report_size + 1);
 
-	memcpy(&hid_buffer[1], data, rsize);
+	buffer[0] = 0x00; // Report ID??
+	memcpy(&buffer[1], data, rsize);
 
 	DEBUG_WIRE("cmd :   ");
 	for(int i = 0; (i < 32) && (i < rsize + 1); i++)
-		DEBUG_WIRE("%02x.",	hid_buffer[i]);
+		DEBUG_WIRE("%02x.",	buffer[i]);
 	DEBUG_WIRE("\n");
-	/* Write must be as long as we expect the result, at least
-	 * for Dappermime 20210213 */
-	res = hid_write(handle, hid_buffer, report_size + 1);
-	if (res < 0) {
-		DEBUG_WARN( "Error: %ls\n", hid_error(handle));
-		exit(-1);
+	if (type == BMP_TYPE_CMSIS_DAP_V1) {
+		res = hid_write(handle, buffer, rsize + 1);
+		if (res < 0) {
+			DEBUG_WARN( "Error: %ls\n", hid_error(handle));
+			exit(-1);
+		}
+		res = hid_read(handle, buffer, report_size + 1);
+		if (res < 0) {
+			DEBUG_WARN( "debugger read(): %ls\n", hid_error(handle));
+			exit(-1);
+		}
+	} else if (type == BMP_TYPE_CMSIS_DAP_V2) {
+		int transferred = 0;
+
+		res = libusb_bulk_transfer(usb_handle, out_ep, buffer + 1, rsize, &transferred, 0);
+		if (res < 0) {
+			DEBUG_WARN( "OUT error\n" );
+		}
+		res = libusb_bulk_transfer(usb_handle, in_ep, buffer, report_size, &transferred, 0);
+		if (res < 0) {
+			DEBUG_WARN( "IN error\n" );
+		}
+		res = transferred;
 	}
-	res = hid_read(handle, hid_buffer, report_size + 1);
-	if (res < 0) {
-		DEBUG_WARN( "debugger read(): %ls\n", hid_error(handle));
-		exit(-1);
-	}
-	DEBUG_WIRE("res %2d: ", res);
-	for(int i = 0; (i < 16) && (i < res + 1); i++)
-		DEBUG_WIRE("%02x.",	hid_buffer[i]);
-	DEBUG_WIRE("\n");
-	if (hid_buffer[0] != cmd) {
+	if (buffer[0] != cmd) {
 		DEBUG_WARN("cmd %02x invalid response received %02x\n",
-				   cmd, hid_buffer[0]);
+				   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 (size)
-		memcpy(data, &hid_buffer[1], (size < res) ? size : res);
+		memcpy(data, &buffer[1], (size < res) ? size : res);
 	return res;
 }
 #define ALIGNOF(x) (((x) & 3) == 0 ? ALIGN_WORD :					\
@@ -432,6 +468,8 @@ int dap_swdptap_init(ADIv5_DP_t *dp)
 		/* 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->seq_out = dap_swdptap_seq_out;
 	dp->seq_out_parity = dap_swdptap_seq_out_parity;
diff --git a/src/platforms/hosted/dap.c b/src/platforms/hosted/dap.c
index 24ba7c70..9d93ab7c 100644
--- a/src/platforms/hosted/dap.c
+++ b/src/platforms/hosted/dap.c
@@ -198,7 +198,7 @@ void dap_connect(bool jtag)
 //-----------------------------------------------------------------------------
 void dap_disconnect(void)
 {
-	uint8_t buf[65];
+	uint8_t buf[1];
 
 	buf[0] = ID_DAP_DISCONNECT;
 	dbg_dap_cmd(buf, sizeof(buf), 1);
@@ -213,7 +213,7 @@ uint32_t dap_swj_clock(uint32_t clock)
 {
 	if (clock == 0)
 		return swj_clock;
-	uint8_t buf[65];
+	uint8_t buf[5];
 	buf[0] = ID_DAP_SWJ_CLOCK;
 	buf[1] = clock & 0xff;
 	buf[2] = (clock >> 8) & 0xff;
@@ -254,7 +254,7 @@ void dap_swd_configure(uint8_t cfg)
 //-----------------------------------------------------------------------------
 int dap_info(int info, uint8_t *data, int size)
 {
-	uint8_t buf[32];
+	uint8_t buf[256];
 	int rsize;
 
 	buf[0] = ID_DAP_INFO;
@@ -420,7 +420,6 @@ unsigned int dap_read_block(ADIv5_AP_t *ap, void *dest, uint32_t src,
 			dest = extract(dest, src, *p, align);
 			p++;
 			src  += (1 << align);
-			dest += (1 << align);
 			sz--;
 		}
 	}
@@ -782,3 +781,43 @@ void dap_swdptap_seq_out_parity(uint32_t MS, int ticks)
 	if (buf[0])
 		DEBUG_WARN("dap_swdptap_seq_out error\n");
 }
+
+#define SWD_SEQUENCE_IN 0x80
+uint32_t dap_swdptap_seq_in(int ticks)
+{
+	uint8_t buf[5] = {
+		ID_DAP_SWD_SEQUENCE,
+		1,
+		ticks + SWD_SEQUENCE_IN
+	};
+	dbg_dap_cmd(buf, 2 + ((ticks + 7) >> 3), 3);
+	uint32_t res = 0;
+	int len = (ticks + 7) >> 3;
+	while (len--) {
+		res <<= 8;
+		res += buf[len + 1];
+	}
+	return res;
+}
+
+bool dap_swdptap_seq_in_parity(uint32_t *ret, int ticks)
+{
+	(void)ticks;
+	uint8_t buf[8] = {
+		ID_DAP_SWD_SEQUENCE,
+		1,
+		33 + SWD_SEQUENCE_IN,
+	};
+	dbg_dap_cmd(buf, 7, 4);
+	uint32_t res = 0;
+	int len = 4;
+	while (len--) {
+		res <<= 8;
+		res += buf[len + 1];
+	}
+	*ret = res;
+	unsigned int parity = __builtin_parity(res) & 1;
+	parity ^= (buf[5] % 1);
+	DEBUG_WARN("Res %08" PRIx32" %d\n", *ret, parity & 1);
+	return (!(parity & 1));
+}
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index d46e344f..a408fde4 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -52,7 +52,8 @@ static void exit_function(void)
 {
 	libusb_exit_function(&info);
 	switch (info.bmp_type) {
-	case BMP_TYPE_CMSIS_DAP:
+	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP_V2:
 		dap_exit_function();
 		break;
 	default:
@@ -92,7 +93,8 @@ void platform_init(int argc, char **argv)
 		if (stlink_init( &info))
 			exit(-1);
 		break;
-	case BMP_TYPE_CMSIS_DAP:
+	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP_V2:
 		if (dap_init( &info))
 			exit(-1);
 		break;
@@ -121,12 +123,11 @@ int platform_adiv5_swdp_scan(uint32_t targetid)
 {
 	info.is_jtag = false;
 	platform_max_frequency_set(cl_opts.opt_max_swj_frequency);
-	if (targetid && (info.bmp_type != BMP_TYPE_BMP))
-		DEBUG_WARN("Ignoring TARGETID for now!\n");
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 	case BMP_TYPE_LIBFTDI:
-	case BMP_TYPE_CMSIS_DAP:
+	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP_V2:
 		return adiv5_swdp_scan(targetid);
 		break;
 	case BMP_TYPE_STLINKV2:
@@ -155,7 +156,8 @@ 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:
+	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP_V2:
 		return dap_swdptap_init(dp);
 	case BMP_TYPE_STLINKV2:
 	case BMP_TYPE_JLINK:
@@ -182,7 +184,8 @@ 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:
+	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP_V2:
 		return jtag_scan(lrlens);
 	case BMP_TYPE_STLINKV2:
 		return jtag_scan_stlinkv2(&info, lrlens);
@@ -203,7 +206,8 @@ 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:
+	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP_V2:
 		return cmsis_dap_jtagtap_init(&jtag_proc);
 	default:
 		return -1;
@@ -222,7 +226,8 @@ 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:
+	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP_V2:
 		return dap_adiv5_dp_defaults(dp);
 	default:
 		break;
@@ -238,7 +243,8 @@ 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:
+	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP_V2:
 		return dap_jtag_dp_init(dp);
 	default:
 		return 0;
@@ -257,8 +263,10 @@ char *platform_ident(void)
 		return "STLINKV2";
 	  case BMP_TYPE_LIBFTDI:
 		return "LIBFTDI";
-	  case BMP_TYPE_CMSIS_DAP:
-		return "CMSIS_DAP";
+	  case BMP_TYPE_CMSIS_DAP_V1:
+		return "CMSIS_DAP_V1";
+	  case BMP_TYPE_CMSIS_DAP_V2:
+		return "CMSIS_DAP_V2";
 	  case BMP_TYPE_JLINK:
 		return "JLINK";
 	}
@@ -319,7 +327,8 @@ void platform_max_frequency_set(uint32_t freq)
 	case BMP_TYPE_BMP:
 		remote_max_frequency_set(freq);
 		break;
-	case BMP_TYPE_CMSIS_DAP:
+	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP_V2:
 		dap_swj_clock(freq);
 		break;
 	case BMP_TYPE_LIBFTDI:
@@ -350,7 +359,8 @@ 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:
+	case BMP_TYPE_CMSIS_DAP_V1:
+	case BMP_TYPE_CMSIS_DAP_V2:
 		return dap_swj_clock(0);
 		break;
 	case BMP_TYPE_LIBFTDI:
diff --git a/src/platforms/hosted/platform.h b/src/platforms/hosted/platform.h
index 129686fa..bf3fa141 100644
--- a/src/platforms/hosted/platform.h
+++ b/src/platforms/hosted/platform.h
@@ -14,12 +14,26 @@ void platform_buffer_flush(void);
 #define PRODUCT_ID_BMP_BL        0x6017
 #define PRODUCT_ID_BMP           0x6018
 
+#define VENDOR_ID_STLINK		 0x0483
+#define PRODUCT_ID_STLINK_MASK	 0xffe0
+#define PRODUCT_ID_STLINK_GROUP  0x3740
+#define PRODUCT_ID_STLINKV1		 0x3744
+#define PRODUCT_ID_STLINKV2		 0x3748
+#define PRODUCT_ID_STLINKV21	 0x374b
+#define PRODUCT_ID_STLINKV21_MSD 0x3752
+#define PRODUCT_ID_STLINKV3_BL	 0x374d
+#define PRODUCT_ID_STLINKV3		 0x374f
+#define PRODUCT_ID_STLINKV3E	 0x374e
+
+#define VENDOR_ID_SEGGER         0x1366
+
 typedef enum bmp_type_s {
 	BMP_TYPE_NONE = 0,
 	BMP_TYPE_BMP,
 	BMP_TYPE_STLINKV2,
 	BMP_TYPE_LIBFTDI,
-	BMP_TYPE_CMSIS_DAP,
+	BMP_TYPE_CMSIS_DAP_V1,
+	BMP_TYPE_CMSIS_DAP_V2,
 	BMP_TYPE_JLINK
 } bmp_type_t;
 
diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index b22b58a0..211ad10b 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -42,16 +42,6 @@
 
 #include "cl_utils.h"
 
-#define VENDOR_ID_STLINK		0x483
-#define PRODUCT_ID_STLINK_MASK	0xffe0
-#define PRODUCT_ID_STLINK_GROUP 0x3740
-#define PRODUCT_ID_STLINKV1		0x3744
-#define PRODUCT_ID_STLINKV2		0x3748
-#define PRODUCT_ID_STLINKV21	0x374b
-#define PRODUCT_ID_STLINKV21_MSD 0x3752
-#define PRODUCT_ID_STLINKV3		0x374f
-#define PRODUCT_ID_STLINKV3E	0x374e
-
 #define STLINK_SWIM_ERR_OK             0x00
 #define STLINK_SWIM_BUSY               0x01
 #define STLINK_DEBUG_ERR_OK            0x80
@@ -399,6 +389,9 @@ static int write_retry(uint8_t *cmdbuf, size_t cmdsize,
 	return res;
 }
 
+/* Version data is at 0x080103f8 with STLINKV3 bootloader flashed with
+ * STLinkUpgrade_v3[3|5].jar
+ */
 static void stlink_version(bmp_info_t *info)
 {
 	if (Stlink.ver_hw == 30) {
@@ -543,20 +536,29 @@ int stlink_init(bmp_info_t *info)
 	libusb_free_device_list(devs, 1);
 	if (!found)
 		return 0;
-	if (info->pid == PRODUCT_ID_STLINKV2) {
+	if (info->vid != VENDOR_ID_STLINK)
+		return 0;
+	switch (info->pid) {
+	case PRODUCT_ID_STLINKV2:
 		Stlink.ver_hw = 20;
 		info->usb_link->ep_tx = 2;
 		Stlink.ep_tx = 2;
-	} else if ((info->pid == PRODUCT_ID_STLINKV21)||
-			   (info->pid == PRODUCT_ID_STLINKV21_MSD)) {
+		break;
+	case PRODUCT_ID_STLINKV21 :
+	case PRODUCT_ID_STLINKV21_MSD:
 		Stlink.ver_hw = 21;
 		info->usb_link->ep_tx = 1;
 		Stlink.ep_tx = 1;
-	} else if ((info->pid == PRODUCT_ID_STLINKV3) ||
-			   (info->pid == PRODUCT_ID_STLINKV3E)) {
+		break;
+	case PRODUCT_ID_STLINKV3_BL:
+	case PRODUCT_ID_STLINKV3:
+	case PRODUCT_ID_STLINKV3E:
 		Stlink.ver_hw = 30;
 		info->usb_link->ep_tx = 1;
 		Stlink.ep_tx = 1;
+		break;
+	default:
+		DEBUG_INFO("Unhandled STM32 device\n");
 	}
 	info->usb_link->ep_rx = 1;
 	int config;
diff --git a/src/platforms/stm32/usbuart.c b/src/platforms/stm32/usbuart.c
index 05b57df6..786bbc38 100644
--- a/src/platforms/stm32/usbuart.c
+++ b/src/platforms/stm32/usbuart.c
@@ -31,7 +31,7 @@
 #include "general.h"
 #include "cdcacm.h"
 
-#ifdef STM32F4
+#ifdef DMA_STREAM0
 #define dma_channel_reset(dma, channel) dma_stream_reset(dma, channel)
 #define dma_enable_channel(dma, channel) dma_enable_stream(dma, channel)
 #define dma_disable_channel(dma, channel) dma_disable_stream(dma, channel)
@@ -125,7 +125,7 @@ void usbuart_init(void)
 	dma_set_memory_size(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, DMA_MSIZE_8BIT);
 	dma_set_priority(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, DMA_PL_HIGH);
 	dma_enable_transfer_complete_interrupt(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN);
-#ifdef STM32F4
+#ifdef DMA_STREAM0
 	dma_set_transfer_mode(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, DMA_SxCR_DIR_MEM_TO_PERIPHERAL);
 	dma_channel_select(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, USBUSART_DMA_TRG);
 	dma_set_dma_flow_control(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN);
@@ -146,7 +146,7 @@ void usbuart_init(void)
 	dma_set_priority(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN, DMA_PL_HIGH);
 	dma_enable_half_transfer_interrupt(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
 	dma_enable_transfer_complete_interrupt(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
-#ifdef STM32F4
+#ifdef DMA_STREAM0
 	dma_set_transfer_mode(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN, DMA_SxCR_DIR_PERIPHERAL_TO_MEM);
 	dma_channel_select(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN, USBUSART_DMA_TRG);
 	dma_set_dma_flow_control(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index f535c25a..f8e6b58f 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -470,8 +470,19 @@ static void adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 		if (recursion == 0) {
 			ap->ap_designer = designer;
 			ap->ap_partno   = partno;
-			if ((ap->ap_designer == AP_DESIGNER_ATMEL) && (ap->ap_partno == 0xcd0))
-				cortexm_probe(ap);
+			if ((ap->ap_designer == AP_DESIGNER_ATMEL) && (ap->ap_partno == 0xcd0)) {
+#define SAMX5X_DSU_CTRLSTAT			0x41002100
+#define SAMX5X_STATUSB_PROT			(1 << 16)
+				uint32_t ctrlstat = adiv5_mem_read32(ap, SAMX5X_DSU_CTRLSTAT);
+				if (ctrlstat & SAMX5X_STATUSB_PROT) {
+					/* A protected SAMx5x device is found.
+					 * Handle it here, as access only to limited memory region
+					 * is allowed
+					 */
+					cortexm_probe(ap);
+					return;
+				}
+			}
 		}
 		for (int i = 0; i < 960; i++) {
 			adiv5_dp_error(ap->dp);
diff --git a/src/target/adiv5_swdp.c b/src/target/adiv5_swdp.c
index 74dc61be..6477c800 100644
--- a/src/target/adiv5_swdp.c
+++ b/src/target/adiv5_swdp.c
@@ -108,8 +108,9 @@ int adiv5_swdp_scan(uint32_t targetid)
 	uint32_t idcode = 0;
 	volatile uint32_t target_id;
 	bool is_v2 = true;
-	if (!targetid) {
-		/* Try to read ID */
+	if (!targetid || (initial_dp->error != firmware_swdp_error)) {
+		/* 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) {
@@ -147,9 +148,11 @@ int adiv5_swdp_scan(uint32_t targetid)
 				adiv5_dp_write(initial_dp, ADIV5_DP_CTRLSTAT, 0);
 				break;
 			}
-			if (!initial_dp->dp_low_read)
-				/* E.g. CMSIS_DAP < V1.2 can not handle multu-drop!*/
+			if (initial_dp->error != firmware_swdp_error) {
+				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;
+			}
 		} else {
 			is_v2 = false;
 		}
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index 1fec2a09..32c262da 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -327,8 +327,9 @@ bool cortexm_probe(ADIv5_AP_t *ap)
 		t->core = "M0";
 		break;
 	default:
-		DEBUG_WARN("Unexpected CortexM CPUID partno %04" PRIx32 "\n",
-				   cpuid_partno);
+		if (ap->ap_designer != AP_DESIGNER_ATMEL) /* Protected Atmel device?*/{
+			DEBUG_WARN("Unexpected CortexM CPUID partno %04x\n", cpuid_partno);
+		}
 	}
 	DEBUG_INFO("CPUID 0x%08" PRIx32 " (%s var %" PRIx32 " rev %" PRIx32 ")\n",
 			   t->cpuid,
diff --git a/src/target/efm32.c b/src/target/efm32.c
index f8218786..6ad4f32f 100644
--- a/src/target/efm32.c
+++ b/src/target/efm32.c
@@ -584,7 +584,10 @@ static efm32_device_t const * efm32_get_device(size_t index)
 /**
  * Probe
  */
-static char efm32_variant_string[60];
+struct efm32_priv_s {
+	char efm32_variant_string[60];
+};
+
 bool efm32_probe(target *t)
 {
 	uint8_t di_version = 1;
@@ -636,13 +639,17 @@ bool efm32_probe(target *t)
 	uint32_t ram_size   = ram_kib   * 0x400;
 	uint32_t flash_page_size = device->flash_page_size;
 
-	snprintf(efm32_variant_string, sizeof(efm32_variant_string), "%c\b%c\b%s %d F%d %s",
+	struct efm32_priv_s *priv_storage = calloc(1, sizeof(*priv_storage));
+	t->target_storage = (void*)priv_storage;
+
+	snprintf(priv_storage->efm32_variant_string,
+			 sizeof(priv_storage->efm32_variant_string), "%c\b%c\b%s %d F%d %s",
 			di_version + 48, (uint8_t)device_index + 32,
 			device->name, part_number, flash_kib, device->description);
 
 	/* Setup Target */
 	t->target_options |= CORTEXM_TOPT_INHIBIT_SRST;
-	t->driver = efm32_variant_string;
+	t->driver = priv_storage->efm32_variant_string;
 	tc_printf(t, "flash size %d page size %d\n", flash_size, flash_page_size);
 	target_add_ram (t, SRAM_BASE, ram_size);
 	efm32_add_flash(t, 0x00000000, flash_size, flash_page_size);
@@ -980,7 +987,10 @@ static bool nop_function(void)
 /**
  * AAP Probe
  */
-char aap_driver_string[42];
+struct efm32_aap_priv_s {
+       char aap_driver_string[42];
+};
+
 void efm32_aap_probe(ADIv5_AP_t *ap)
 {
 	if ((ap->idr & EFM32_APP_IDR_MASK) == EFM32_AAP_IDR) {
@@ -1004,10 +1014,11 @@ void efm32_aap_probe(ADIv5_AP_t *ap)
 	/* Read status */
 	DEBUG_INFO("EFM32: AAP STATUS=%08"PRIx32"\n", adiv5_ap_read(ap, AAP_STATUS));
 
-	sprintf(aap_driver_string,
+	struct efm32_aap_priv_s *priv_storage = calloc(1, sizeof(*priv_storage));
+	sprintf(priv_storage->aap_driver_string,
 			"EFM32 Authentication Access Port rev.%d",
 			aap_revision);
-	t->driver = aap_driver_string;
+	t->driver = priv_storage->aap_driver_string;
 	t->attach = (void*)nop_function;
 	t->detach = (void*)nop_function;
 	t->check_error = (void*)nop_function;
diff --git a/src/target/kinetis.c b/src/target/kinetis.c
index 98f9f764..22551580 100644
--- a/src/target/kinetis.c
+++ b/src/target/kinetis.c
@@ -74,7 +74,6 @@
 #define K64_WRITE_LEN 8
 
 static bool kinetis_cmd_unsafe(target *t, int argc, char *argv[]);
-static bool unsafe_enabled;
 
 const struct command_s kinetis_cmd_list[] = {
 	{"unsafe", (cmd_handler)kinetis_cmd_unsafe, "Allow programming security byte (enable|disable)"},
@@ -85,9 +84,9 @@ static bool kinetis_cmd_unsafe(target *t, int argc, char *argv[])
 {
 	if (argc == 1) {
 		tc_printf(t, "Allow programming security byte: %s\n",
-			  unsafe_enabled ? "enabled" : "disabled");
+			  t->unsafe_enabled ? "enabled" : "disabled");
 	} else {
-		parse_enable_or_disable(argv[1], &unsafe_enabled);
+		parse_enable_or_disable(argv[1], &t->unsafe_enabled);
 	}
 	return true;
 }
@@ -336,7 +335,7 @@ bool kinetis_probe(target *t)
 	default:
 		return false;
 	}
-	unsafe_enabled = false;
+	t->unsafe_enabled = false;
 	target_add_commands(t, kinetis_cmd_list, t->driver);
 	return true;
 }
@@ -403,7 +402,7 @@ static int kl_gen_flash_write(struct target_flash *f,
 	struct kinetis_flash *kf = (struct kinetis_flash *)f;
 
 	/* Ensure we don't write something horrible over the security byte */
-	if (!unsafe_enabled &&
+	if (!f->t->unsafe_enabled &&
 	    (dest <= FLASH_SECURITY_BYTE_ADDRESS) &&
 	    ((dest + len) > FLASH_SECURITY_BYTE_ADDRESS)) {
 		((uint8_t*)src)[FLASH_SECURITY_BYTE_ADDRESS - dest] =
@@ -437,7 +436,7 @@ static int kl_gen_flash_done(struct target_flash *f)
 {
 	struct kinetis_flash *kf = (struct kinetis_flash *)f;
 
-	if (unsafe_enabled)
+	if (f->t->unsafe_enabled)
 		return 0;
 
 	if (target_mem_read8(f->t, FLASH_SECURITY_BYTE_ADDRESS) ==
@@ -527,13 +526,12 @@ void kinetis_mdm_probe(ADIv5_AP_t *ap)
 
 /* This is needed as a separate command, as there's no way to  *
  * tell a KE04 from other kinetis in kinetis_mdm_probe()       */
-static bool ke04_mode = false;
 static bool kinetis_mdm_cmd_ke04_mode(target *t, int argc, const char **argv)
 {
 	(void)argc;
 	(void)argv;
 	/* Set a flag to ignore part of the status and assert reset */
-	ke04_mode = true;
+	t->ke04_mode = true;
 	tc_printf(t, "Mass erase for KE04 now allowed\n");
 	return true;
 }
@@ -544,7 +542,7 @@ static bool kinetis_mdm_cmd_erase_mass(target *t, int argc, const char **argv)
 	ADIv5_AP_t *ap = t->priv;
 
 	/* Keep the MCU in reset as stated in KL25PxxM48SF0RM */
-	if(ke04_mode)
+	if(t->ke04_mode)
 		adiv5_ap_write(ap, MDM_CONTROL, MDM_CONTROL_SYS_RESET);
 
 	uint32_t status, control;
@@ -553,13 +551,13 @@ static bool kinetis_mdm_cmd_erase_mass(target *t, int argc, const char **argv)
 	tc_printf(t, "Requesting mass erase (status = 0x%"PRIx32")\n", status);
 
 	/* This flag does not exist on KE04 */
-	if (!(status & MDM_STATUS_MASS_ERASE_ENABLED) && !ke04_mode) {
+	if (!(status & MDM_STATUS_MASS_ERASE_ENABLED) && !t->ke04_mode) {
 		tc_printf(t, "ERROR: Mass erase disabled!\n");
 		return false;
 	}
 
 	/* Flag is not persistent */
-	ke04_mode = false;
+	t->ke04_mode = false;
 
 	if (!(status & MDM_STATUS_FLASH_READY)) {
 		tc_printf(t, "ERROR: Flash not ready!\n");
diff --git a/src/target/nxpke04.c b/src/target/nxpke04.c
index 19fad37a..c577fc2c 100644
--- a/src/target/nxpke04.c
+++ b/src/target/nxpke04.c
@@ -126,7 +126,6 @@ static int ke04_flash_done(struct target_flash *f);
 static bool kinetis_cmd_unsafe(target *t, int argc, char *argv[]);
 static bool ke04_cmd_sector_erase(target *t, int argc, char *argv[]);
 static bool ke04_cmd_mass_erase(target *t, int argc, char *argv[]);
-static bool unsafe_enabled;
 
 const struct command_s ke_cmd_list[] = {
 	{"unsafe", (cmd_handler)kinetis_cmd_unsafe, "Allow programming security byte (enable|disable)"},
@@ -173,9 +172,9 @@ static bool kinetis_cmd_unsafe(target *t, int argc, char *argv[])
 {
 	if (argc == 1) {
 		tc_printf(t, "Allow programming security byte: %s\n",
-			  unsafe_enabled ? "enabled" : "disabled");
+			  t->unsafe_enabled ? "enabled" : "disabled");
 	} else {
-		parse_enable_or_disable(argv[1], &unsafe_enabled);
+		parse_enable_or_disable(argv[1], &t->unsafe_enabled);
 	}
 	return true;
 }
@@ -253,7 +252,6 @@ bool ke04_probe(target *t)
 	target_add_flash(t, f);
 
 	/* Add target specific commands */
-	unsafe_enabled = false;
 	target_add_commands(t, ke_cmd_list, t->driver);
 
 	return true;
@@ -343,7 +341,8 @@ static int ke04_flash_write(struct target_flash *f,
                               target_addr dest, const void *src, size_t len)
 {
 	/* Ensure we don't write something horrible over the security byte */
-	if (!unsafe_enabled &&
+	target *t = f->t;
+	if (!t->unsafe_enabled &&
 	    (dest <= FLASH_SECURITY_BYTE_ADDRESS) &&
 	    ((dest + len) > FLASH_SECURITY_BYTE_ADDRESS)) {
 		((uint8_t*)src)[FLASH_SECURITY_BYTE_ADDRESS - dest] =
@@ -364,7 +363,8 @@ static int ke04_flash_write(struct target_flash *f,
 
 static int ke04_flash_done(struct target_flash *f)
 {
-	if (unsafe_enabled)
+	target *t = f->t;
+	if (t->unsafe_enabled)
 		return 0;
 
 	if (target_mem_read8(f->t, FLASH_SECURITY_BYTE_ADDRESS) ==
diff --git a/src/target/samd.c b/src/target/samd.c
index 525a43b7..02879fbd 100644
--- a/src/target/samd.c
+++ b/src/target/samd.c
@@ -441,7 +441,10 @@ static void samd_add_flash(target *t, uint32_t addr, size_t length)
 	target_add_flash(t, f);
 }
 
-static char samd_variant_string[60];
+struct samd_priv_s {
+	char samd_variant_string[60];
+};
+
 bool samd_probe(target *t)
 {
 	ADIv5_AP_t *ap = cortexm_ap(t);
@@ -460,6 +463,9 @@ bool samd_probe(target *t)
 	if ((did & SAMD_DID_MASK) != SAMD_DID_CONST_VALUE)
 		return false;
 
+	struct samd_priv_s *priv_storage = calloc(1, sizeof(*priv_storage));
+	t->target_storage = (void*)priv_storage;
+
 	uint32_t ctrlstat = target_mem_read32(t, SAMD_DSU_CTRLSTAT);
 	struct samd_descr samd = samd_parse_device_id(did);
 
@@ -468,14 +474,14 @@ bool samd_probe(target *t)
 
 	/* Part String */
 	if (protected) {
-		sprintf(samd_variant_string,
+		sprintf(priv_storage->samd_variant_string,
 		        "Atmel SAM%c%d%c%d%c%s (rev %c) (PROT=1)",
 		        samd.family,
 		        samd.series, samd.pin, samd.mem,
 		        samd.variant,
 		        samd.package, samd.revision);
 	} else {
-		sprintf(samd_variant_string,
+		sprintf(priv_storage->samd_variant_string,
 		        "Atmel SAM%c%d%c%d%c%s (rev %c)",
 		        samd.family,
 		        samd.series, samd.pin, samd.mem,
@@ -484,7 +490,7 @@ bool samd_probe(target *t)
 	}
 
 	/* Setup Target */
-	t->driver = samd_variant_string;
+	t->driver = priv_storage->samd_variant_string;
 	t->reset = samd_reset;
 
 	if (samd.series == 20 && samd.revision == 'B') {
diff --git a/src/target/samx5x.c b/src/target/samx5x.c
index 3604663a..d0abff60 100644
--- a/src/target/samx5x.c
+++ b/src/target/samx5x.c
@@ -344,7 +344,10 @@ static void samx5x_add_flash(target *t, uint32_t addr, size_t length,
 	target_add_flash(t, f);
 }
 
-static char samx5x_variant_string[60];
+struct samx5x_priv_s {
+	char samx5x_variant_string[60];
+};
+
 bool samx5x_probe(target *t)
 {
 	ADIv5_AP_t *ap = cortexm_ap(t);
@@ -370,20 +373,25 @@ bool samx5x_probe(target *t)
 	bool protected = (ctrlstat & SAMX5X_STATUSB_PROT);
 
 	/* Part String */
+	struct samx5x_priv_s *priv_storage = calloc(1, sizeof(*priv_storage));
+	t->target_storage = (void*)priv_storage;
+
 	if (protected) {
-		snprintf(samx5x_variant_string, sizeof(samx5x_variant_string),
+		snprintf(priv_storage->samx5x_variant_string,
+				 sizeof(priv_storage->samx5x_variant_string),
 			 "Microchip SAM%c%d%c%dA (rev %c) (PROT=1)",
 			 samx5x.series_letter, samx5x.series_number,
 			 samx5x.pin, samx5x.mem, samx5x.revision);
 	} else {
-		snprintf(samx5x_variant_string, sizeof(samx5x_variant_string),
+		snprintf(priv_storage->samx5x_variant_string,
+				 sizeof(priv_storage->samx5x_variant_string),
 			 "Microchip SAM%c%d%c%dA (rev %c)",
 			 samx5x.series_letter, samx5x.series_number,
 			 samx5x.pin, samx5x.mem, samx5x.revision);
 	}
 
 	/* Setup Target */
-	t->driver = samx5x_variant_string;
+	t->driver = priv_storage->samx5x_variant_string;
 	t->reset = samx5x_reset;
 
 	if (protected) {
@@ -519,7 +527,7 @@ static int samx5x_flash_erase(struct target_flash *f, target_addr addr,
 	target *t = f->t;
 	uint16_t errs = samx5x_read_nvm_error(t);
 	if (errs) {
-		DEBUG_INFO(NVM_ERROR_BITS_MSG, "erase", addr, len);
+		DEBUG_WARN(NVM_ERROR_BITS_MSG, "erase", addr, len);
 		samx5x_print_nvm_error(errs);
 		samx5x_clear_nvm_error(t);
 	}
@@ -533,11 +541,15 @@ static int samx5x_flash_erase(struct target_flash *f, target_addr addr,
 		SAMX5X_PAGE_SIZE;
 	lock_region_size = flash_size >> 5;
 
-	if (addr < (15 - bootprot) * 8192)
-		return -1;
+	if (addr < (15 - bootprot) * 8192) {
+            DEBUG_WARN("Bootprot\n");
+            return -1;
+        }
 
-	if (~runlock & (1 << addr / lock_region_size))
-		return -1;
+	if (~runlock & (1 << addr / lock_region_size)) {
+            DEBUG_WARN("runlock\n");
+            return -1;
+        }
 
 	while (len) {
 		target_mem_write32(t, SAMX5X_NVMC_ADDRESS, addr);
@@ -553,11 +565,15 @@ static int samx5x_flash_erase(struct target_flash *f, target_addr addr,
 		/* Poll for NVM Ready */
 		while ((target_mem_read32(t, SAMX5X_NVMC_STATUS) &
 			SAMX5X_STATUS_READY) == 0)
-			if (target_check_error(t) || samx5x_check_nvm_error(t))
-				return -1;
+                    if (target_check_error(t) || samx5x_check_nvm_error(t)) {
+                        DEBUG_WARN("NVM Ready\n");
+                        return -1;
+                    }
 
-		if (target_check_error(t) || samx5x_check_nvm_error(t))
-			return -1;
+		if (target_check_error(t) || samx5x_check_nvm_error(t)) {
+                    DEBUG_WARN("Error\n");
+                    return -1;
+                }
 
 		/* Lock */
 		samx5x_lock_current_address(t);
diff --git a/src/target/target_internal.h b/src/target/target_internal.h
index ebc791f9..17f95800 100644
--- a/src/target/target_internal.h
+++ b/src/target/target_internal.h
@@ -114,6 +114,10 @@ struct target_s {
 	uint16_t t_designer;
 	uint16_t idcode;
 	void *target_storage;
+	union {
+		bool unsafe_enabled;
+		bool ke04_mode;
+	};
 
 	struct target_ram *ram;
 	struct target_flash *flash;