diff --git a/Makefile b/Makefile
index 3f7a53cb..b0d56566 100644
--- a/Makefile
+++ b/Makefile
@@ -5,14 +5,6 @@ endif
 
 PC_HOSTED =
 NO_LIBOPENCM3 =
-ifeq ($(PROBE_HOST), libftdi)
-	PC_HOSTED = true
-	NO_LIBOPENCM3 = true
-endif
-ifeq ($(PROBE_HOST), pc-stlinkv2)
-	PC_HOSTED = true
-	NO_LIBOPENCM3 = true
-endif
 ifeq ($(PROBE_HOST), hosted)
 	PC_HOSTED = true
 	NO_LIBOPENCM3 = true
diff --git a/scripts/dfu-convert.py b/scripts/dfu-convert.py
old mode 100644
new mode 100755
index 5a0a904b..63240724
--- a/scripts/dfu-convert.py
+++ b/scripts/dfu-convert.py
@@ -1,13 +1,19 @@
-#!/usr/bin/python2
+#!/usr/bin/env python
 
 # Written by Antonio Galea - 2010/11/18
 # Distributed under Gnu LGPL 3.0
 # see http://www.gnu.org/licenses/lgpl-3.0.txt
+#
+# Add support for Python 3 inspired by script found in Bitcraze repository
 
 import sys,struct,zlib,os
 from optparse import OptionParser
 from intelhex import IntelHex
 
+python3 = False
+if (sys.version_info > (3, 0)):
+      python3 = True
+
 DEFAULT_DEVICE="0x0483:0xdf11"
 
 def named(tuple,names):
@@ -21,11 +27,11 @@ def compute_crc(data):
   return 0xFFFFFFFF & -zlib.crc32(data) -1
 
 def parse(file,dump_images=False):
-  print 'File: "%s"' % file
+  print('File: "%s"' % file)
   data = open(file,'rb').read()
   crc = compute_crc(data[:-4])
   prefix, data = consume('<5sBIB',data,'signature version size targets')
-  print '%(signature)s v%(version)d, image size: %(size)d, targets: %(targets)d' % prefix
+  print('%(signature)s v%(version)d, image size: %(size)d, targets: %(targets)d' % prefix)
   for t in range(prefix['targets']):
     tprefix, data  = consume('<6sBI255s2I',data,'signature altsetting named name size elements')
     tprefix['num'] = t
@@ -33,40 +39,57 @@ def parse(file,dump_images=False):
       tprefix['name'] = cstring(tprefix['name'])
     else:
       tprefix['name'] = ''
-    print '%(signature)s %(num)d, alt setting: %(altsetting)s, name: "%(name)s", size: %(size)d, elements: %(elements)d' % tprefix
+    print('%(signature)s %(num)d, alt setting: %(altsetting)s, name: "%(name)s", size: %(size)d, elements: %(elements)d' % tprefix)
     tsize = tprefix['size']
     target, data = data[:tsize], data[tsize:]
     for e in range(tprefix['elements']):
       eprefix, target = consume('<2I',target,'address size')
       eprefix['num'] = e
-      print '  %(num)d, address: 0x%(address)08x, size: %(size)d' % eprefix
+      print('  %(num)d, address: 0x%(address)08x, size: %(size)d' % eprefix)
       esize = eprefix['size']
       image, target = target[:esize], target[esize:]
       if dump_images:
         out = '%s.target%d.image%d.bin' % (file,t,e)
         open(out,'wb').write(image)
-        print '    DUMPED IMAGE TO "%s"' % out
+        print('    DUMPED IMAGE TO "%s"' % out)
     if len(target):
-      print "target %d: PARSE ERROR" % t
+      print("target %d: PARSE ERROR" % t)
   suffix = named(struct.unpack('<4H3sBI',data[:16]),'device product vendor dfu ufd len crc')
-  print 'usb: %(vendor)04x:%(product)04x, device: 0x%(device)04x, dfu: 0x%(dfu)04x, %(ufd)s, %(len)d, 0x%(crc)08x' % suffix
+  print('usb: %(vendor)04x:%(product)04x, device: 0x%(device)04x, dfu: 0x%(dfu)04x, %(ufd)s, %(len)d, 0x%(crc)08x' % suffix)
   if crc != suffix['crc']:
-    print "CRC ERROR: computed crc32 is 0x%08x" % crc
+    print("CRC ERROR: computed crc32 is 0x%08x" % crc)
   data = data[16:]
   if data:
-    print "PARSE ERROR"
+    print("PARSE ERROR")
 
 def build(file,targets,device=DEFAULT_DEVICE):
   data = ''
+  if (python3):
+        data = b''
   for t,target in enumerate(targets):
     tdata = ''
+    if (python3):
+          tdata = b''
     for image in target:
       tdata += struct.pack('<2I',image['address'],len(image['data']))+image['data']
-    tdata = struct.pack('<6sBI255s2I','Target',0,1,'ST...',len(tdata),len(target)) + tdata
+
+    trgt = 'Target'
+    st = 'ST...'
+    if (python3):
+      trgt = b'Target'
+      st = b'ST...'
+    tdata = struct.pack('<6sBI255s2I',trgt,0,1,st,len(tdata),len(target)) + tdata
     data += tdata
-  data  = struct.pack('<5sBIB','DfuSe',1,len(data)+11,len(targets)) + data
+
+  dfu_se = 'DfuSe'
+  ufd = 'UFD'
+  if (python3):
+    dfu_se = b'DfuSe'
+    ufd = b'UFD'
+  data  = struct.pack('<5sBIB',dfu_se,1,len(data)+11,len(targets)) + data
+
   v,d=map(lambda x: int(x,0) & 0xFFFF, device.split(':',1))
-  data += struct.pack('<4H3sB',0,d,v,0x011a,'UFD',16)
+  data += struct.pack('<4H3sB',0,d,v,0x011a,ufd,16)
   crc   = compute_crc(data)
   data += struct.pack('<I',crc)
   open(file,'wb').write(data)
@@ -96,15 +119,15 @@ if __name__=="__main__":
         try:
           address,binfile = arg.split(':',1)
         except ValueError:
-          print "Address:file couple '%s' invalid." % arg
+          print("Address:file couple '%s' invalid." % arg)
           sys.exit(1)
         try:
           address = int(address,0) & 0xFFFFFFFF
         except ValueError:
-          print "Address %s invalid." % address
+          print("Address %s invalid." % address)
           sys.exit(1)
         if not os.path.isfile(binfile):
-          print "Unreadable file '%s'." % binfile
+          print("Unreadable file '%s'." % binfile)
           sys.exit(1)
         target.append({ 'address': address, 'data': open(binfile,'rb').read() })
     
@@ -116,7 +139,7 @@ if __name__=="__main__":
         try:
           address = address & 0xFFFFFFFF
         except ValueError:
-          print "Address %s invalid." % address
+          print("Address %s invalid." % address)
           sys.exit(1)
         target.append({ 'address': address, 'data': data })
     
@@ -127,13 +150,13 @@ if __name__=="__main__":
     try:
       v,d=map(lambda x: int(x,0) & 0xFFFF, device.split(':',1))
     except:
-      print "Invalid device '%s'." % device
+      print("Invalid device '%s'." % device)
       sys.exit(1)
     build(outfile,[target],device)
   elif len(args)==1:
     infile = args[0]
     if not os.path.isfile(infile):
-      print "Unreadable file '%s'." % infile
+      print("Unreadable file '%s'." % infile)
       sys.exit(1)
     parse(infile, dump_images=options.dump_images)
   else:
diff --git a/src/command.c b/src/command.c
index 390ad367..b32a83a8 100644
--- a/src/command.c
+++ b/src/command.c
@@ -114,7 +114,7 @@ bool connect_assert_srst;
 #if defined(PLATFORM_HAS_DEBUG) && (PC_HOSTED == 0)
 bool debug_bmp;
 #endif
-long cortexm_wait_timeout = 2000; /* Timeout to wait for Cortex to react on halt command. */
+unsigned cortexm_wait_timeout = 2000; /* Timeout to wait for Cortex to react on halt command. */
 
 int command_process(target *t, char *cmd)
 {
diff --git a/src/gdb_packet.c b/src/gdb_packet.c
index f6e79463..6f31e138 100644
--- a/src/gdb_packet.c
+++ b/src/gdb_packet.c
@@ -81,6 +81,12 @@ int gdb_getpacket(char *packet, int size)
 						break;
 					}
 				}
+				/* Reset the packet buffer start character to zero, because function
+				 * 'remotePacketProcess()' above overwrites this buffer, and
+				 * an arbitrary character may have been placed there. If this is a '$'
+				 * character, this will cause this loop to be terminated, which is wrong.
+				 */
+				packet[0] = 0;
 			}
 #endif
 	    } while (packet[0] != '$');
diff --git a/src/platforms/f4discovery/Makefile.inc b/src/platforms/f4discovery/Makefile.inc
index e1f22e25..8596882b 100644
--- a/src/platforms/f4discovery/Makefile.inc
+++ b/src/platforms/f4discovery/Makefile.inc
@@ -1,4 +1,5 @@
 CROSS_COMPILE ?= arm-none-eabi-
+BMP_BOOTLOADER ?=
 CC = $(CROSS_COMPILE)gcc
 OBJCOPY = $(CROSS_COMPILE)objcopy
 
@@ -7,12 +8,20 @@ CFLAGS += -Istm32/include -mcpu=cortex-m4 -mthumb \
 	-DSTM32F4 -DF4DISCOVERY -I../libopencm3/include \
 	-Iplatforms/stm32
 
-LDFLAGS = -lopencm3_stm32f4 \
+LDFLAGS_BOOT = -lopencm3_stm32f4 \
 	-Wl,-T,platforms/stm32/f4discovery.ld -nostartfiles -lc -lnosys \
 	-Wl,-Map=mapfile -mthumb -mcpu=cortex-m4 -Wl,-gc-sections \
 	-mfloat-abi=hard -mfpu=fpv4-sp-d16 \
 	-L../libopencm3/lib
 
+ifeq ($(BMP_BOOTLOADER), 1)
+$(info  Load address 0x08004000 for BMPBootloader)
+LDFLAGS = $(LDFLAGS_BOOT) -Wl,-Ttext=0x8004000
+CFLAGS += -DUSE_BMP_SERIAL
+else
+LDFLAGS = $(LDFLAGS_BOOT)
+endif
+
 VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
@@ -23,7 +32,18 @@ SRC += 	cdcacm.c	\
 	timing.c	\
 	timing_stm32.c	\
 
+ifneq ($(BMP_BOOTLOADER), 1)
 all:	blackmagic.bin
+else
+all:	blackmagic.bin  blackmagic_dfu.bin blackmagic_dfu.hex
+blackmagic_dfu: usbdfu.o dfucore.o dfu_f4.o
+	$(CC) $^ -o $@ $(LDFLAGS_BOOT)
 
+blackmagic_dfu.bin:    blackmagic_dfu
+	$(OBJCOPY) -O binary $^ $@
+
+blackmagic_dfu.hex:    blackmagic_dfu
+	$(OBJCOPY) -O ihex $^ $@
+endif
 host_clean:
 	-$(Q)$(RM) blackmagic.bin
diff --git a/src/platforms/f4discovery/platform.c b/src/platforms/f4discovery/platform.c
index 04ac3785..53705138 100644
--- a/src/platforms/f4discovery/platform.c
+++ b/src/platforms/f4discovery/platform.c
@@ -71,8 +71,8 @@ void platform_init(void)
 	rcc_periph_clock_enable(RCC_CRC);
 
 	/* Set up USB Pins and alternate function*/
-	gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO11 | GPIO12);
-	gpio_set_af(GPIOA, GPIO_AF10, GPIO11 | GPIO12);
+	gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE,  GPIO9 | GPIO11 | GPIO12);
+	gpio_set_af(GPIOA, GPIO_AF10, GPIO9 | GPIO10 | GPIO11 | GPIO12);
 
 	GPIOC_OSPEEDR &=~0xF30;
 	GPIOC_OSPEEDR |= 0xA20;
diff --git a/src/platforms/f4discovery/platform.h b/src/platforms/f4discovery/platform.h
index dde94ed0..29e2b861 100644
--- a/src/platforms/f4discovery/platform.h
+++ b/src/platforms/f4discovery/platform.h
@@ -33,6 +33,7 @@
 
 #define PLATFORM_HAS_TRACESWO
 #define BOARD_IDENT "Black Magic Probe (F4Discovery), (Firmware " FIRMWARE_VERSION ")"
+#define  BOARD_IDENT_DFU  "Black Magic Firmware Upgrade (F4Discovery)"
 #define DFU_IDENT   "Black Magic Firmware Upgrade (F4Discovery)"
 
 /* Important pin mappings for STM32 implementation:
diff --git a/src/platforms/stlink/dfu_upgrade.c b/src/platforms/f4discovery/usbdfu.c
similarity index 54%
rename from src/platforms/stlink/dfu_upgrade.c
rename to src/platforms/f4discovery/usbdfu.c
index aeb559d3..4b493ea0 100644
--- a/src/platforms/stlink/dfu_upgrade.c
+++ b/src/platforms/f4discovery/usbdfu.c
@@ -27,59 +27,47 @@
 #include "general.h"
 #include "platform.h"
 
-uint32_t app_address = 0x08000000;
-static uint16_t led_upgrade;
-static uint32_t led2_state = 0;
-extern uint32_t _stack;
-static uint32_t rev;
+uint32_t app_address = 0x08004000;
+extern char _ebss[];
 
 void dfu_detach(void)
 {
-	platform_request_boot();
-	scb_reset_core();
+	scb_reset_system();
 }
 
 int main(void)
 {
-	rev = detect_rev();
-	rcc_clock_setup_in_hse_8mhz_out_72mhz();
-	if (rev == 0)
-		led_upgrade = GPIO8;
-	else
-		led_upgrade = GPIO9;
+	volatile uint32_t *magic = (uint32_t *)_ebss;
+	rcc_periph_clock_enable(RCC_GPIOA);
+	if (gpio_get(GPIOA, GPIO0) ||
+	   ((magic[0] == BOOTMAGIC0) && (magic[1] == BOOTMAGIC1))) {
+		magic[0] = 0;
+		magic[1] = 0;
+	} else {
+		dfu_jump_app_if_valid();
+	}
+	rcc_clock_setup_pll(&rcc_hse_8mhz_3v3[RCC_CLOCK_3V3_168MHZ]);
 
-	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
-	systick_set_reload(900000);
+	/* Assert blue LED as indicator we are in the bootloader */
+	rcc_periph_clock_enable(RCC_GPIOD);
+	gpio_mode_setup(LED_PORT, GPIO_MODE_OUTPUT,
+					GPIO_PUPD_NONE, LED_BOOTLOADER);
+	gpio_set(LED_PORT, LED_BOOTLOADER);
 
-	dfu_protect(UPD_MODE);
+	/* Enable peripherals */
+	rcc_periph_clock_enable(RCC_OTGFS);
 
-	systick_interrupt_enable();
-	systick_counter_enable();
-
-	if (rev > 1) /* Reconnect USB */
-		gpio_set(GPIOA, GPIO15);
-	dfu_init(&st_usbfs_v1_usb_driver, UPD_MODE);
+	/* Set up USB Pins and alternate function*/
+	gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO11 | GPIO12);
+	gpio_set_af(GPIOA, GPIO_AF10, GPIO11 | GPIO12);
 
+ 	dfu_protect(false);
+	dfu_init(&USB_DRIVER);
 	dfu_main();
+
 }
 
 void dfu_event(void)
 {
 }
 
-void sys_tick_handler(void)
-{
-	if (rev == 0) {
-		gpio_toggle(GPIOA, led_upgrade);
-	} else {
-		if (led2_state & 1) {
-			gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
-				GPIO_CNF_OUTPUT_PUSHPULL, led_upgrade);
-			gpio_set(GPIOA, led_upgrade);
-		} else {
-			gpio_set_mode(GPIOA, GPIO_MODE_INPUT,
-				GPIO_CNF_INPUT_ANALOG, led_upgrade);
-		}
-		led2_state++;
-	}
-}
diff --git a/src/platforms/hosted/bmp_remote.c b/src/platforms/hosted/bmp_remote.c
index 12387151..b6d97f18 100644
--- a/src/platforms/hosted/bmp_remote.c
+++ b/src/platforms/hosted/bmp_remote.c
@@ -37,7 +37,7 @@
 
 #include "adiv5.h"
 
-int remote_init(bool verbose)
+int remote_init(void)
 {
 	char construct[REMOTE_MAX_MSG_SIZE];
 	int c = snprintf(construct, REMOTE_MAX_MSG_SIZE, "%s", REMOTE_START_STR);
@@ -49,9 +49,8 @@ int remote_init(bool verbose)
 				c ? (char *)&(construct[1]) : "unknown");
       return -1;
     }
-	if (verbose)
-		DEBUG_WARN("Remote is %s\n", &construct[1]);
-        return 0;
+	DEBUG_PROBE("Remote is %s\n", &construct[1]);
+	return 0;
 }
 
 bool remote_target_get_power(void)
diff --git a/src/platforms/hosted/bmp_remote.h b/src/platforms/hosted/bmp_remote.h
index f7c7dc80..5d135a38 100644
--- a/src/platforms/hosted/bmp_remote.h
+++ b/src/platforms/hosted/bmp_remote.h
@@ -29,7 +29,7 @@
 int platform_buffer_write(const uint8_t *data, int size);
 int platform_buffer_read(uint8_t *data, int size);
 
-int remote_init(bool verbose);
+int remote_init(void);
 int remote_swdptap_init(swd_proc_t *swd_proc);
 int remote_jtagtap_init(jtag_proc_t *jtag_proc);
 bool remote_target_get_power(void);
diff --git a/src/platforms/hosted/jlink.c b/src/platforms/hosted/jlink.c
index 9b95742c..7515de5a 100644
--- a/src/platforms/hosted/jlink.c
+++ b/src/platforms/hosted/jlink.c
@@ -90,7 +90,7 @@ static void jlink_print_interfaces(bmp_info_t *info)
 		DEBUG_INFO(", %s available\n",
 			   (other_interface == JLINK_IF_SWD) ? "SWD": "JTAG");
 	else
-		DEBUG_WARN(", %s not available\n",
+		DEBUG_INFO(", %s not available\n",
 			   ((res[0] + 1) == JLINK_IF_SWD) ? "JTAG": "SWD");
 }
 
diff --git a/src/platforms/hosted/jlink_adiv5_swdp.c b/src/platforms/hosted/jlink_adiv5_swdp.c
index 6895b9ad..28514f4e 100644
--- a/src/platforms/hosted/jlink_adiv5_swdp.c
+++ b/src/platforms/hosted/jlink_adiv5_swdp.c
@@ -113,10 +113,7 @@ static int swdptap_init(bmp_info_t *info)
 	cmd[1] = SELECT_IF_SWD;
 	send_recv(info->usb_link, cmd, 2, res, sizeof(res));
 	platform_delay(10);
-	/* Set speed 256 kHz*/
-	unsigned int speed = 2000;
-	uint8_t jtag_speed[3] = {5, speed & 0xff, speed >> 8};
-	send_recv(info->usb_link, jtag_speed, 3, NULL, 0);
+	/* SWD speed is fixed. Do not set it here*/
 	return 0;
 }
 
@@ -178,7 +175,15 @@ int jlink_swdp_scan(bmp_info_t *info)
 	ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
 	if (!dp) /* calloc failed: heap exhaustion */
 		return 0;
-	dp->idcode = jlink_adiv5_swdp_low_access(dp, 1, ADIV5_DP_IDCODE, 0);
+	volatile struct exception e;
+	TRY_CATCH (e, EXCEPTION_ALL) {
+		dp->idcode = jlink_adiv5_swdp_low_access(dp, 1, ADIV5_DP_IDCODE, 0);
+	}
+	if (e.type) {
+		DEBUG_WARN("DP not responding for IDCODE! Reset stuck low?\n");
+		free(dp);
+		return 0;
+	}
 	dp->dp_read = jlink_adiv5_swdp_read;
 	dp->error = jlink_adiv5_swdp_error;
 	dp->low_access = jlink_adiv5_swdp_low_access;
@@ -186,8 +191,6 @@ int jlink_swdp_scan(bmp_info_t *info)
 
 	jlink_adiv5_swdp_error(dp);
 	adiv5_dp_init(dp);
-	if (!target_list)
-		free(dp);
 	return target_list?1:0;
 }
 
@@ -249,20 +252,26 @@ static uint32_t jlink_adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 	uint8_t res[8];
 	cmd[0] = CMD_HW_JTAG3;
 	cmd[1] = 0;
-	cmd[2] = 13;
+	/* It seems, JLINK samples read data at end of previous clock.
+	 * So target data read must start at the 12'th clock, while
+	 * write starts as expected at the 14'th clock (8 cmd, 3 response,
+	 * 2 turn around.
+	 */
+	cmd[2] = (RnW) ? 11 : 13;
 	cmd[3] = 0;
-	cmd[4] = 0xff;
-	cmd[5] = 0xe3;
-	cmd[6] = request << 2;
-	cmd[7] = request >> 6;
+	cmd[4] = 0xff; /* 8 bits command OUT */
+	cmd[5] = 0xf0; /* one IN bit to turn around to read, read 2
+					  (read) or 3 (write) IN bits for response and
+					  and one OUT bit to turn around to write on write*/
+	cmd[6] = request;
+	cmd[7] = 0x00;
 	platform_timeout_set(&timeout, 2000);
 	do {
 		send_recv(info.usb_link, cmd,  8, res, 2);
-		send_recv(info.usb_link, NULL, 0, res, 1);
-		if (res[0] != 0)
+		send_recv(info.usb_link, NULL, 0, res + 2 , 1);
+		if (res[2] != 0)
 			raise_exception(EXCEPTION_ERROR, "Low access setup failed");
-		ack = res[1] >> 2;
-		ack &= 7;
+		ack = res[1] & 7;
 	} while (ack == SWDP_ACK_WAIT && !platform_timeout_is_expired(&timeout));
 	if (ack == SWDP_ACK_WAIT)
 		raise_exception(EXCEPTION_TIMEOUT, "SWDP ACK timeout");
@@ -276,17 +285,15 @@ static uint32_t jlink_adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 
 	if(ack != SWDP_ACK_OK) {
 		if (cl_debuglevel & BMP_DEBUG_TARGET)
-			DEBUG_WARN( "Protocol\n");
+			DEBUG_WARN( "Protocol %d\n", ack);
 		line_reset(&info);
 		return 0;
 	}
-	cmd[3] = 0;
-	/* Always prepend an idle cycle (SWDIO = 0)!*/
+	/* Always append 8 idle cycle (SWDIO = 0)!*/
 	if(RnW) {
 		memset(cmd + 4, 0, 10);
-		cmd[2] = 34;
+		cmd[2] = 33 + 2; /* 2 idle cycles */
 		cmd[8] = 0xfe;
-		cmd[13] = 0;
 		send_recv(info.usb_link, cmd, 14, res, 5);
 		send_recv(info.usb_link, NULL, 0, res + 5, 1);
 		if (res[5] != 0)
@@ -294,19 +301,19 @@ static uint32_t jlink_adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 		response = res[0] | res[1] << 8 | res[2] << 16 | res[3] << 24;
 		int parity = res[4] & 1;
 		int bit_count  = __builtin_popcount (response) + parity;
-		if (bit_count & 1)  /* Give up on parity error */
+		if (bit_count & 1)	/* Give up on parity error */
 			raise_exception(EXCEPTION_ERROR, "SWDP Parity error");
 	} else {
-		cmd[2] = 35;
-		memset(cmd + 4, 0xff, 5);
-		cmd[ 9] = ((value <<  2) & 0xfc);
-		cmd[10] = ((value >>  6) & 0xff);
-		cmd[11] = ((value >> 14) & 0xff);
-		cmd[12] = ((value >> 22) & 0xff);
-		cmd[13] = ((value >> 30) & 0x03);
+		cmd[2] = 33 + 8; /* 8 idle cycle  to move data through SW-DP */
+		memset(cmd + 4, 0xff, 6);
+		cmd[10] = ((value >>  0) & 0xff);
+		cmd[11] = ((value >>  8) & 0xff);
+		cmd[12] = ((value >> 16) & 0xff);
+		cmd[13] = ((value >> 24) & 0xff);
 		int bit_count  = __builtin_popcount(value);
-		cmd[13] |= ((bit_count & 1) ? 4 : 0);
-		send_recv(info.usb_link, cmd, 14, res, 5);
+		cmd[14] = bit_count & 1;
+		cmd[15] = 0;
+		send_recv(info.usb_link, cmd, 16, res, 6);
 		send_recv(info.usb_link, NULL, 0, res, 1);
 		if (res[0] != 0)
 			raise_exception(EXCEPTION_ERROR, "Low access write failed");
diff --git a/src/platforms/hosted/libftdi_swdptap.c b/src/platforms/hosted/libftdi_swdptap.c
index 575b7138..99ee0068 100644
--- a/src/platforms/hosted/libftdi_swdptap.c
+++ b/src/platforms/hosted/libftdi_swdptap.c
@@ -361,9 +361,19 @@ static void swdptap_seq_out(uint32_t MS, int ticks)
 	}
 }
 
+/* ARM Debug Interface Architecture Specification ADIv5.0 to ADIv5.2
+ * tells to clock the data through SW-DP to either :
+ * - immediate start a new transaction
+ * - continue to drive idle cycles
+ * - or clock at least 8 idle cycles
+ *
+ * Implement last option to favour correctness over
+ *   slight speed decrease
+ */
 static void swdptap_seq_out_parity(uint32_t MS, int ticks)
 {
-	int parity = __builtin_parity(MS & ((1LL << ticks) - 1)) & 1;
+	(void) ticks;
+	int parity = __builtin_parity(MS) & 1;
 	unsigned int index = 0;
 	swdptap_turnaround(SWDIO_STATUS_DRIVE);
 	if (do_mpsse) {
@@ -373,26 +383,26 @@ static void swdptap_seq_out_parity(uint32_t MS, int ticks)
 		DI[2] = (MS >> 16) & 0xff;
 		DI[3] = (MS >> 24) & 0xff;
 		DI[4] = parity;
-		libftdi_jtagtap_tdi_tdo_seq(NULL, 0, DI, ticks + 1);
+		DI[5] = 0;
+		libftdi_jtagtap_tdi_tdo_seq(NULL, 0, DI, 32 + 1 + 8);
 	} else {
 		uint8_t cmd[32];
 		int steps = ticks;
 		while (steps) {
 			cmd[index++] = MPSSE_TMS_SHIFT;
+			cmd[index++] = 6;
 			if (steps >= 7) {
-				cmd[index++] = 6;
 				cmd[index++] = MS & 0x7f;
 				MS >>= 7;
 				steps -= 7;
 			} else {
-				cmd[index++] = steps - 1;
-				cmd[index++] = MS & 0x7f;
+				cmd[index++] = (MS & 0x7f) | (parity << 4);
 				steps = 0;
 			}
 		}
 		cmd[index++] = MPSSE_TMS_SHIFT;
+		cmd[index++] = 4;
 		cmd[index++] = 0;
-		cmd[index++] = parity;
 		libftdi_buffer_write(cmd, index);
 	}
 }
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index a0fe7744..f70ef859 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -36,6 +36,7 @@
 #include "ftdi_bmp.h"
 #include "jlink.h"
 #include "cmsis_dap.h"
+#include "version.h"
 
 #define VENDOR_ID_STLINK         0x0483
 #define PRODUCT_ID_STLINK_MASK   0xffe0
@@ -302,14 +303,16 @@ void platform_init(int argc, char **argv)
 	} else if (find_debuggers(&cl_opts, &info)) {
 		exit(-1);
 	}
-	DEBUG_WARN("Using %04x:%04x %s %s %s\n", info.vid, info.pid, info.serial,
+	DEBUG_INFO("BMP hosted %s\n for ST-Link V2/3, CMSIS_DAP, JLINK and "
+			   "LIBFTDI/MPSSE\n", FIRMWARE_VERSION);
+	DEBUG_INFO("Using %04x:%04x %s %s\n %s\n", info.vid, info.pid, info.serial,
 		   info.manufacturer,
 		   info.product);
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 		if (serial_open(&cl_opts, info.serial))
 			exit(-1);
-		remote_init(true);
+		remote_init();
 		break;
 	case BMP_TYPE_STLINKV2:
 		if (stlink_init( &info))
@@ -358,7 +361,6 @@ int platform_adiv5_swdp_scan(void)
 			if (target_list)
 				return 1;
 		}
-		free(dp);
 		break;
 	}
 	case BMP_TYPE_CMSIS_DAP:
@@ -372,7 +374,6 @@ int platform_adiv5_swdp_scan(void)
 			if (target_list)
 				return 1;
 		}
-		free(dp);
 		break;
 	}
 	case BMP_TYPE_JLINK:
diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 42941c8f..1ca7c156 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -31,6 +31,7 @@
 #include "exception.h"
 #include "jtag_devs.h"
 #include "target.h"
+#include "cortexm.h"
 
 #include <assert.h>
 #include <unistd.h>
@@ -348,7 +349,8 @@ static int stlink_send_recv_retry(uint8_t *txbuf, size_t txsize,
 		if (res == STLINK_ERROR_OK)
 			return res;
 		uint32_t now = platform_time_ms();
-		if (((now - start) > 1000) || (res != STLINK_ERROR_WAIT)) {
+		if (((now - start) > cortexm_wait_timeout) ||
+			(res != STLINK_ERROR_WAIT)) {
 			DEBUG_WARN("write_retry failed. ");
 			return res;
 		}
@@ -434,7 +436,8 @@ static void stlink_version(bmp_info_t *info)
 			Stlink.ver_swim = (version >> 0) & 0x3f;
 		}
 	}
-	DEBUG_INFO("V%dJ%d",Stlink.ver_stlink, Stlink.ver_jtag);
+	DEBUG_INFO("STLink firmware version: V%dJ%d",Stlink.ver_stlink,
+			   Stlink.ver_jtag);
 	if (Stlink.ver_hw == 30) {
 		DEBUG_INFO("M%dB%dS%d", Stlink.ver_mass, Stlink.ver_bridge, Stlink.ver_swim);
 	} else if (Stlink.ver_hw == 20) {
@@ -829,8 +832,8 @@ static bool stlink_ap_setup(int ap)
 		ap,
 	};
 	uint8_t data[2];
-	send_recv(info.usb_link, cmd, 16, data, 2);
 	DEBUG_PROBE("Open AP %d\n", ap);
+	stlink_send_recv_retry(cmd, 16, data, 2);
 	int res = stlink_usb_error_check(data, true);
 	if (res) {
 		if (Stlink.ver_hw == 30) {
diff --git a/src/platforms/native/usbdfu.c b/src/platforms/native/usbdfu.c
index 020a9760..db44002c 100644
--- a/src/platforms/native/usbdfu.c
+++ b/src/platforms/native/usbdfu.c
@@ -42,7 +42,7 @@ int main(void)
 	if(gpio_get(GPIOB, GPIO12))
 		dfu_jump_app_if_valid();
 
-	dfu_protect(DFU_MODE);
+	dfu_protect(false);
 
 	rcc_clock_setup_in_hse_8mhz_out_72mhz();
 	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
@@ -60,7 +60,7 @@ int main(void)
 	gpio_set_mode(LED_PORT, GPIO_MODE_OUTPUT_2_MHZ,
 			GPIO_CNF_OUTPUT_PUSHPULL, LED_0 | LED_1 | LED_2);
 
-	dfu_init(&st_usbfs_v1_usb_driver, DFU_MODE);
+	dfu_init(&st_usbfs_v1_usb_driver);
 
 	/* Configure the USB pull up pin. */
 	gpio_set(GPIOA, GPIO8);
diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index b1471754..a17154f2 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -28,7 +28,7 @@
 #include <errno.h>
 #include <fcntl.h>
 #include <sys/stat.h>
-
+#include "version.h"
 #include "target.h"
 #include "target_internal.h"
 
@@ -116,8 +116,8 @@ static void bmp_munmap(struct mmap_data *map)
 static void cl_help(char **argv, BMP_CL_OPTIONS_t *opt)
 {
 	DEBUG_WARN("%s for: \n", opt->opt_idstring);
-	DEBUG_WARN("\tBMP Firmware, ST-Link V2/3, CMSIS_DAP, JLINK and "
-			   "LIBFTDI/MPSSE\n\n");
+	DEBUG_WARN("\tBMP hosted %s\n\t\tfor ST-Link V2/3, CMSIS_DAP, JLINK and "
+			   "LIBFTDI/MPSSE\n\n", FIRMWARE_VERSION);
 	DEBUG_WARN("Usage: %s [options]\n", argv[0]);
 	DEBUG_WARN("\t-h\t\t: This help.\n");
 	DEBUG_WARN("\t-v[bitmask]\t: Increasing verbosity. Bitmask:\n");
@@ -298,8 +298,7 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 	platform_srst_set_val(opt->opt_connect_under_reset);
 	if (opt->opt_mode == BMP_MODE_TEST)
 		DEBUG_INFO("Running in Test Mode\n");
-	if (platform_target_voltage())
-		DEBUG_INFO("Target voltage: %s Volt\n", platform_target_voltage());
+	DEBUG_INFO("Target voltage: %s Volt\n", platform_target_voltage());
 	if (opt->opt_usejtag) {
 		num_targets = platform_jtag_scan(NULL);
 	} else {
diff --git a/src/platforms/pc/cl_utils.h b/src/platforms/pc/cl_utils.h
index f6f7b1fa..15606725 100644
--- a/src/platforms/pc/cl_utils.h
+++ b/src/platforms/pc/cl_utils.h
@@ -24,7 +24,7 @@
 #if !defined(__CL_UTILS_H)
 #define __CL_UTILS_H
 
-#define RESP_TIMEOUT (100)
+#include "cortexm.h"
 
 enum bmp_cl_mode {
 	BMP_MODE_DEBUG,
diff --git a/src/platforms/pc/serial_unix.c b/src/platforms/pc/serial_unix.c
index aa7d30d3..11000351 100644
--- a/src/platforms/pc/serial_unix.c
+++ b/src/platforms/pc/serial_unix.c
@@ -29,6 +29,7 @@
 #include "general.h"
 #include "remote.h"
 #include "cl_utils.h"
+#include "cortexm.h"
 
 static int fd;  /* File descriptor for connection to GDB remote */
 
@@ -189,7 +190,7 @@ int platform_buffer_read(uint8_t *data, int maxsize)
 	c = data;
 	tv.tv_sec = 0;
 
-	tv.tv_usec = 1000 * RESP_TIMEOUT;
+	tv.tv_usec = 1000 * cortexm_wait_timeout;
 
 	/* Look for start of response */
 	do {
diff --git a/src/platforms/pc/serial_win.c b/src/platforms/pc/serial_win.c
index 42904281..86ac3643 100644
--- a/src/platforms/pc/serial_win.c
+++ b/src/platforms/pc/serial_win.c
@@ -145,7 +145,7 @@ int platform_buffer_read(uint8_t *data, int maxsize)
 	DWORD s;
 	uint8_t response = 0;
 	uint32_t startTime = platform_time_ms();
-	uint32_t endTime = platform_time_ms() + RESP_TIMEOUT;
+	uint32_t endTime = platform_time_ms() + cortexm_wait_timeout;
 	do {
 		if (!ReadFile(hComm, &response, 1, &s, NULL)) {
 			DEBUG_WARN("ERROR on read RESP\n");
diff --git a/src/platforms/stlink/Makefile.inc b/src/platforms/stlink/Makefile.inc
index 66feef6b..a1d74f5f 100644
--- a/src/platforms/stlink/Makefile.inc
+++ b/src/platforms/stlink/Makefile.inc
@@ -39,16 +39,12 @@ SRC += 	cdcacm.c	\
 ifeq ($(ST_BOOTLOADER), 1)
 all:	blackmagic.bin
 else
-all:	blackmagic.bin blackmagic_dfu.bin blackmagic_dfu.hex dfu_upgrade.bin dfu_upgrade.hex
+all:	blackmagic.bin blackmagic_dfu.bin blackmagic_dfu.hex
 endif
 blackmagic_dfu.elf: usbdfu.o dfucore.o dfu_f1.o stlink_common.o
 	@echo "  LD      $@"
 	$(Q)$(CC) $^ -o $@ $(LDFLAGS_BOOT)
 
-dfu_upgrade.elf: dfu_upgrade.o dfucore.o dfu_f1.o  stlink_common.o
-	@echo "  LD      $@"
-	$(Q)$(CC) $^ -o $@ $(LDFLAGS)
-
 host_clean:
 	-$(Q)$(RM) *.bin *elf *hex
 
diff --git a/src/platforms/stlink/platform.h b/src/platforms/stlink/platform.h
index 99e3555f..ee776f6a 100644
--- a/src/platforms/stlink/platform.h
+++ b/src/platforms/stlink/platform.h
@@ -42,9 +42,7 @@ int usbuart_debug_write(const char *buf, size_t len);
 
 #define BOARD_IDENT       "Black Magic Probe (STLINK), (Firmware " FIRMWARE_VERSION ")"
 #define BOARD_IDENT_DFU   "Black Magic (Upgrade) for STLink/Discovery, (Firmware " FIRMWARE_VERSION ")"
-#define BOARD_IDENT_UPD   "Black Magic (DFU Upgrade) for STLink/Discovery, (Firmware " FIRMWARE_VERSION ")"
 #define DFU_IDENT         "Black Magic Firmware Upgrade (STLINK)"
-#define UPD_IFACE_STRING  "@Internal Flash   /0x08000000/8*001Kg"
 
 /* Hardware definitions... */
 #define TDI_PORT	GPIOA
diff --git a/src/platforms/stlink/usbdfu.c b/src/platforms/stlink/usbdfu.c
index d016075a..8b1cf7cf 100644
--- a/src/platforms/stlink/usbdfu.c
+++ b/src/platforms/stlink/usbdfu.c
@@ -63,7 +63,7 @@ int main(void)
 
 	if(((GPIOA_CRL & 0x40) == 0x40) && stlink_test_nrst())
 		dfu_jump_app_if_valid();
-	dfu_protect(DFU_MODE);
+	dfu_protect(false);
 
 	rcc_clock_setup_in_hse_8mhz_out_72mhz();
 	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
@@ -75,7 +75,7 @@ int main(void)
 
 	if (rev > 1)
 		gpio_set(GPIOA, GPIO15);
-	dfu_init(&st_usbfs_v1_usb_driver, DFU_MODE);
+	dfu_init(&st_usbfs_v1_usb_driver);
 
 	dfu_main();
 }
diff --git a/src/platforms/stm32/dfu_f1.c b/src/platforms/stm32/dfu_f1.c
index b4b92089..fc9f4187 100644
--- a/src/platforms/stm32/dfu_f1.c
+++ b/src/platforms/stm32/dfu_f1.c
@@ -62,9 +62,9 @@ uint32_t dfu_poll_timeout(uint8_t cmd, uint32_t addr, uint16_t blocknum)
 	return 100;
 }
 
-void dfu_protect(dfu_mode_t mode)
+void dfu_protect(bool enable)
 {
-    if (mode == DFU_MODE) {
+    if (enable) {
 #ifdef DFU_SELF_PROTECT
 	if ((FLASH_WRPR & 0x03) != 0x00) {
 		flash_unlock();
diff --git a/src/platforms/stm32/dfu_f4.c b/src/platforms/stm32/dfu_f4.c
index 0e3f9354..5d0c25aa 100644
--- a/src/platforms/stm32/dfu_f4.c
+++ b/src/platforms/stm32/dfu_f4.c
@@ -47,21 +47,20 @@ static void get_sector_num(uint32_t addr)
 	}
 	if (!sector_addr[i])
 		return;
-	sector_num = i;
+	sector_num = i & 0x1f;
 }
 
 void dfu_check_and_do_sector_erase(uint32_t addr)
 {
 	if(addr == sector_addr[sector_num]) {
-		flash_erase_sector((sector_num & 0x1f)<<3, FLASH_PROGRAM_X32);
+		flash_erase_sector(sector_num, FLASH_CR_PROGRAM_X32);
 	}
 }
 
 void dfu_flash_program_buffer(uint32_t baseaddr, void *buf, int len)
 {
 	for(int i = 0; i < len; i += 4)
-		flash_program_word(baseaddr + i, *(uint32_t*)(buf+i),
-		                   FLASH_PROGRAM_X32);
+		flash_program_word(baseaddr + i, *(uint32_t*)(buf+i));
 }
 
 uint32_t dfu_poll_timeout(uint8_t cmd, uint32_t addr, uint16_t blocknum)
@@ -78,14 +77,16 @@ uint32_t dfu_poll_timeout(uint8_t cmd, uint32_t addr, uint16_t blocknum)
 	return 26;
 }
 
-void dfu_protect_enable(void)
+void dfu_protect(bool enable)
 {
+	if (enable) {
 #ifdef DFU_SELF_PROTECT
-	if ((FLASH_OPTCR & 0x10000) != 0) {
-		flash_program_option_bytes(FLASH_OPTCR & ~0x10000);
-		flash_lock_option_bytes();
-	}
+		if ((FLASH_OPTCR & 0x10000) != 0) {
+			flash_program_option_bytes(FLASH_OPTCR & ~0x10000);
+			flash_lock_option_bytes();
+		}
 #endif
+	}
 }
 
 void dfu_jump_app_if_valid(void)
@@ -93,14 +94,17 @@ void dfu_jump_app_if_valid(void)
 	/* Boot the application if it's valid */
 	/* Vector table may be anywhere in 128 kByte RAM
 	   CCM not handled*/
-	if((*(volatile uint32_t*)APP_ADDRESS & 0x2FFC0000) == 0x20000000) {
-		/* Set vector table base address */
-		SCB_VTOR = APP_ADDRESS & 0x1FFFFF; /* Max 2 MByte Flash*/
+	if((*(volatile uint32_t*)app_address & 0x2FFC0000) == 0x20000000) {
+#if defined(STM32F7)		/* Set vector table base address */
+		SCB_VTOR = app_address & 0xFFFFFF00;
+#else
+		SCB_VTOR = app_address & 0x1FFFFF; /* Max 2 MByte Flash*/
+#endif
 		/* Initialise master stack pointer */
 		asm volatile ("msr msp, %0"::"g"
-		              (*(volatile uint32_t*)APP_ADDRESS));
+		              (*(volatile uint32_t*)app_address));
 		/* Jump to application */
-		(*(void(**)())(APP_ADDRESS + 4))();
+		(*(void(**)())(app_address + 4))();
 	}
 }
 
diff --git a/src/platforms/stm32/dfucore.c b/src/platforms/stm32/dfucore.c
index 4467f4eb..84e588e8 100644
--- a/src/platforms/stm32/dfucore.c
+++ b/src/platforms/stm32/dfucore.c
@@ -18,6 +18,7 @@
  */
 
 #include "general.h"
+#include <libopencm3/stm32/desig.h>
 
 #include <string.h>
 #if defined(STM32F1HD)
@@ -28,8 +29,11 @@
 #	define DFU_IFACE_STRING  "@Internal Flash   /0x08000000/8*001Ka,000*001Kg"
 #   define DFU_IFACE_STRING_OFFSET 38
 #   define DFU_IFACE_PAGESIZE 1
-#elif defined(STM32F4)
-#	define DFU_IFACE_STRING  "/0x08000000/1*016Ka,3*016Kg,1*064Kg,7*128Kg"
+#elif defined(STM32F4) ||  defined(STM32F7)
+#   define FLASH_BASE         0x08000000U
+#   define DFU_IFACE_PAGESIZE 128
+#   define DFU_IFACE_STRING_OFFSET 54
+#	define DFU_IFACE_STRING  "@Internal Flash   /0x08000000/1*016Ka,3*016Kg,1*064Kg,000*128Kg"
 #endif
 #include <libopencm3/stm32/flash.h>
 
@@ -129,15 +133,6 @@ static const char *usb_strings[] = {
 	if_string,
 };
 
-static char upd_if_string[] = UPD_IFACE_STRING;
-static const char *usb_strings_upd[] = {
-	"Black Sphere Technologies",
-	BOARD_IDENT_UPD,
-	serial_no,
-	/* This string is used by ST Microelectronics' DfuSe utility */
-	upd_if_string,
-};
-
 static uint32_t get_le32(const void *vp)
 {
 	const uint8_t *p = vp;
@@ -290,12 +285,12 @@ static enum usbd_request_return_codes usbdfu_control_request(usbd_device *dev,
 	return USBD_REQ_NOTSUPP;
 }
 
-void dfu_init(const usbd_driver *driver, dfu_mode_t mode)
+void dfu_init(const usbd_driver *driver)
 {
 	get_dev_unique_id(serial_no);
 
 	usbdev = usbd_init(driver, &dev, &config,
-			   (mode == DFU_MODE)?usb_strings:usb_strings_upd, 4,
+			   usb_strings, 4,
 			   usbd_control_buffer, sizeof(usbd_control_buffer));
 
 	usbd_register_control_callback(usbdev,
@@ -335,25 +330,12 @@ static void set_dfu_iface_string(uint32_t size)
 	*p++ = size + '0';
 }
 #else
-# define set_dfu_iface_string()
+# define set_dfu_iface_string(x)
 #endif
 
 static char *get_dev_unique_id(char *s)
 {
-#if defined(STM32F4) || defined(STM32F2)
-#	define UNIQUE_SERIAL_R 0x1FFF7A10
-#	define FLASH_SIZE_R    0x1fff7A22
-#elif defined(STM32F3)
-#	define UNIQUE_SERIAL_R 0x1FFFF7AC
-#	define FLASH_SIZE_R    0x1fff77cc
-#elif defined(STM32L1)
-#	define UNIQUE_SERIAL_R 0x1ff80050
-#	define FLASH_SIZE_R    0x1FF8004C
-#else
-#	define UNIQUE_SERIAL_R 0x1FFFF7E8;
-#	define FLASH_SIZE_R    0x1ffff7e0
-#endif
-	volatile uint32_t *unique_id_p = (volatile uint32_t *)UNIQUE_SERIAL_R;
+	volatile uint32_t *unique_id_p = (volatile uint32_t *)DESIG_UNIQUE_ID_BASE;
 	uint32_t unique_id = *unique_id_p +
 			*(unique_id_p + 1) +
 			*(unique_id_p + 2);
@@ -362,17 +344,11 @@ static char *get_dev_unique_id(char *s)
 
 	/* Calculated the upper flash limit from the exported data
 	   in theparameter block*/
-	fuse_flash_size = *(uint32_t *) FLASH_SIZE_R & 0xfff;
+	fuse_flash_size = desig_get_flash_size();
 	if (fuse_flash_size == 0x40) /* Handle F103x8 as F103xB! */
 		fuse_flash_size = 0x80;
 	set_dfu_iface_string(fuse_flash_size - 8);
 	max_address = FLASH_BASE + (fuse_flash_size << 10);
-	/* If bootloader pages are write protected or device is read
-	 * protected, deny bootloader update.
-	 * User can still force updates, at his own risk!
-	 */
-	if (((FLASH_WRPR & 0x03) != 0x03) || (FLASH_OBR & FLASH_OBR_RDPRT_EN))
-		upd_if_string[30] = '0';
 	/* Fetch serial number from chip's unique ID */
 	for(i = 0; i < 8; i++) {
 		s[7-i] = ((unique_id >> (4*i)) & 0xF) + '0';
diff --git a/src/platforms/stm32/serialno.c b/src/platforms/stm32/serialno.c
index 69503804..23cda103 100644
--- a/src/platforms/stm32/serialno.c
+++ b/src/platforms/stm32/serialno.c
@@ -21,7 +21,7 @@
 
 char *serial_no_read(char *s, int max)
 {
-#if defined(STM32F1)
+#if defined(STM32F1) || defined(USE_BMP_SERIAL)
 /* Only STM32F103 has no DFU Bootloader. Generate a ID comatible
  * with the BMP Bootloader since ages.
  */
diff --git a/src/platforms/stm32/usbdfu.h b/src/platforms/stm32/usbdfu.h
index 3f5b3f8b..aaa949a8 100644
--- a/src/platforms/stm32/usbdfu.h
+++ b/src/platforms/stm32/usbdfu.h
@@ -27,20 +27,15 @@
 #define CMD_ERASE	0x41
 extern uint32_t app_address;
 
-typedef enum {
-    DFU_MODE = 0,
-    UPD_MODE = 1
-} dfu_mode_t;
-
 /* dfucore.c - DFU core, common to libopencm3 platforms. */
-void dfu_init(const usbd_driver *driver, dfu_mode_t mode);
+void dfu_init(const usbd_driver *driver);
 void dfu_main(void);
 
 /* Device specific functions */
 void dfu_check_and_do_sector_erase(uint32_t sector);
 void dfu_flash_program_buffer(uint32_t baseaddr, void *buf, int len);
 uint32_t dfu_poll_timeout(uint8_t cmd, uint32_t addr, uint16_t blocknum);
-void dfu_protect(dfu_mode_t mode);
+void dfu_protect(bool enable);
 void dfu_jump_app_if_valid(void);
 void dfu_event(void);
 
diff --git a/src/platforms/swlink/Makefile.inc b/src/platforms/swlink/Makefile.inc
index 5ac76ec7..5cad72ef 100644
--- a/src/platforms/swlink/Makefile.inc
+++ b/src/platforms/swlink/Makefile.inc
@@ -29,16 +29,12 @@ SRC += 	cdcacm.c	\
 	traceswoasync.c	\
 	platform_common.c \
 
-all:	blackmagic.bin blackmagic_dfu.bin blackmagic_dfu.hex dfu_upgrade.bin dfu_upgrade.hex
+all:	blackmagic.bin blackmagic_dfu.bin blackmagic_dfu.hex
 
 blackmagic_dfu.elf: usbdfu.o dfucore.o dfu_f1.o platform_common.o
 	@echo "  LD      $@"
 	$(Q)$(CC) $^ -o $@ $(LDFLAGS_BOOT)
 
-dfu_upgrade.elf: dfu_upgrade.o dfucore.o dfu_f1.o platform_common.o
-	@echo "  LD      $@"
-	$(Q)$(CC) $^ -o $@ $(LDFLAGS)
-
 host_clean:
 	-$(Q)$(RM) blackmagic.bin blackmagic_dfu blackmagic_dfu.bin blackmagic_dfu.hex
 
diff --git a/src/platforms/swlink/dfu_upgrade.c b/src/platforms/swlink/dfu_upgrade.c
deleted file mode 100644
index c34123a7..00000000
--- a/src/platforms/swlink/dfu_upgrade.c
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2018 Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <string.h>
-#include <libopencm3/cm3/systick.h>
-#include <libopencm3/stm32/rcc.h>
-#include <libopencm3/stm32/gpio.h>
-#include <libopencm3/cm3/scb.h>
-
-#include "usbdfu.h"
-#include "general.h"
-#include "platform.h"
-
-uint32_t app_address = 0x08000000;
-extern uint32_t _stack;
-static uint32_t rev;
-
-void dfu_detach(void)
-{
-	platform_request_boot();
-	scb_reset_core();
-}
-
-int main(void)
-{
-	rev = detect_rev();
-	rcc_clock_setup_in_hse_8mhz_out_72mhz();
-	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
-	systick_set_reload(900000);
-
-	dfu_protect(UPD_MODE);
-
-	systick_interrupt_enable();
-	systick_counter_enable();
-
-	dfu_init(&st_usbfs_v1_usb_driver, UPD_MODE);
-
-	dfu_main();
-}
-
-void dfu_event(void)
-{
-}
-
-void sys_tick_handler(void)
-{
-	if (rev == 0) {
-		gpio_toggle(GPIOA, GPIO8);
-	} else {
-		gpio_toggle(GPIOC, GPIO13);
-	}
-}
diff --git a/src/platforms/swlink/platform.h b/src/platforms/swlink/platform.h
index aadf0428..c8a9f2e4 100644
--- a/src/platforms/swlink/platform.h
+++ b/src/platforms/swlink/platform.h
@@ -39,9 +39,7 @@ int usbuart_debug_write(const char *buf, size_t len);
 
 #define BOARD_IDENT			"Black Magic Probe (SWLINK), (Firmware " FIRMWARE_VERSION ")"
 #define BOARD_IDENT_DFU		"Black Magic (Upgrade), SWLINK, (Firmware " FIRMWARE_VERSION ")"
-#define BOARD_IDENT_UPD		"Black Magic (DFU Upgrade), SWLINK, (Firmware " FIRMWARE_VERSION ")"
 #define DFU_IDENT			"Black Magic Firmware Upgrade (SWLINK)"
-#define UPD_IFACE_STRING	"@Internal Flash   /0x08000000/8*001Kg"
 
 /* Hardware definitions... */
 #define TMS_PORT	GPIOA
diff --git a/src/platforms/swlink/usbdfu.c b/src/platforms/swlink/usbdfu.c
index 527ad44c..c6a2b282 100644
--- a/src/platforms/swlink/usbdfu.c
+++ b/src/platforms/swlink/usbdfu.c
@@ -75,7 +75,7 @@ int main(void)
 	if(((GPIOA_CRL & 0x40) == 0x40) && normal_boot)
 		dfu_jump_app_if_valid();
 
-	dfu_protect(DFU_MODE);
+	dfu_protect(false);
 
 	rcc_clock_setup_in_hse_8mhz_out_72mhz();
 	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
@@ -84,7 +84,7 @@ int main(void)
 	systick_interrupt_enable();
 	systick_counter_enable();
 
-	dfu_init(&st_usbfs_v1_usb_driver, DFU_MODE);
+	dfu_init(&st_usbfs_v1_usb_driver);
 
 	dfu_main();
 }
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index cf2983d6..a45bb1d8 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -614,6 +614,14 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 
 void adiv5_dp_init(ADIv5_DP_t *dp)
 {
+#define DPIDR_PARTNO_MASK 0x0ff00000
+/* Check IDCODE for a valid designer and sensible PARTNO*/
+	if (((dp->idcode & 0xfff) == 0)  ||
+		((dp->idcode & DPIDR_PARTNO_MASK)) == DPIDR_PARTNO_MASK) {
+		DEBUG_WARN("Invalid DP idcode %08" PRIx32 "\n", dp->idcode);
+		free(dp);
+		return;
+	}
 	DEBUG_INFO("DPIDR 0x%08" PRIx32 " (v%d %srev%d)\n", dp->idcode,
 			   (dp->idcode >> 12) & 0xf,
 			   (dp->idcode & 0x10000) ? "MINDP " : "", dp->idcode >> 28);
@@ -664,7 +672,6 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 			return;
 		}
 	}
-
 	/* This AP reset logic is described in ADIv5, but fails to work
 	 * correctly on STM32.	CDBGRSTACK is never asserted, and we
 	 * just wait forever.  This scenario is described in B2.4.1
@@ -691,8 +698,7 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		}
 	}
 
-	uint32_t dp_idcode = adiv5_dp_read(dp, ADIV5_DP_IDCODE);
-	if ((dp_idcode & ADIV5_DP_VERSION_MASK) == ADIV5_DPv2) {
+	if ((dp->idcode & ADIV5_DP_VERSION_MASK) == ADIV5_DPv2) {
 		/* Read TargetID. Can be done with device in WFI, sleep or reset!*/
 		adiv5_dp_write(dp, ADIV5_DP_SELECT, ADIV5_DP_BANK2);
 		dp->targetid = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT);
@@ -731,6 +737,7 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 				dp->ap_cleanup(i);
 #endif
 			adiv5_ap_unref(ap);
+			adiv5_dp_unref(dp);
 			/* FIXME: Should we expect valid APs behind duplicate ones? */
 			return;
 		}
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index 3af8c9bc..38c1cccb 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -97,7 +97,9 @@
 #define AP_DESIGNER_ARM          0x43b
 /*LPC845 with designer 501. Strange!? */
 #define AP_DESIGNER_SPECULAR     0x501
+#define AP_DESIGNER_CS           0x555
 #define AP_DESIGNER_ENERGY_MICRO 0x673
+#define AP_DESIGNER_GIGADEVICE   0x751
 
 /* AP Control and Status Word (CSW) */
 #define ADIV5_AP_CSW_DBGSWENABLE	(1u << 31)
diff --git a/src/target/adiv5_jtagdp.c b/src/target/adiv5_jtagdp.c
index 12dad85a..11f11ed7 100644
--- a/src/target/adiv5_jtagdp.c
+++ b/src/target/adiv5_jtagdp.c
@@ -50,8 +50,8 @@ void adiv5_jtag_dp_handler(uint8_t jd_index, uint32_t j_idcode)
 	}
 
 	dp->dp_jd_index = jd_index;
+	dp->idcode = j_idcode;
 	if ((PC_HOSTED == 0 ) || (!platform_jtag_dp_init(dp))) {
-		dp->idcode = j_idcode;
 		dp->dp_read = fw_adiv5_jtagdp_read;
 		dp->error = adiv5_jtagdp_error;
 		dp->low_access = fw_adiv5_jtagdp_low_access;
diff --git a/src/target/adiv5_swdp.c b/src/target/adiv5_swdp.c
index 4351dd17..0678064b 100644
--- a/src/target/adiv5_swdp.c
+++ b/src/target/adiv5_swdp.c
@@ -82,8 +82,6 @@ int adiv5_swdp_scan(void)
 
 	firmware_swdp_error(dp);
 	adiv5_dp_init(dp);
-	if (!target_list)
-		free(dp);
 	return target_list?1:0;
 }
 
@@ -169,19 +167,17 @@ uint32_t firmware_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 			raise_exception(EXCEPTION_ERROR, "SWDP Parity error");
 	} else {
 		swd_proc.swdptap_seq_out_parity(value, 32);
-		/* RM0377 Rev. 8 Chapter 27.5.4 for STM32L0x1 states:
-		 * Because of the asynchronous clock domains SWCLK and HCLK,
-		 * two extra SWCLK cycles are needed after a write transaction
-		 * (after the parity bit) to make the write effective
-		 * internally. These cycles should be applied while driving
-		 * the line low (IDLE state)
-		 * This is particularly important when writing the CTRL/STAT
-		 * for a power-up request. If the next transaction (requiring
-		 * a power-up) occurs immediately, it will fail.
+		/* ARM Debug Interface Architecture Specification ADIv5.0 to ADIv5.2
+		 * tells to clock the data through SW-DP to either :
+		 * - immediate start a new transaction
+		 * - continue to drive idle cycles
+		 * - or clock at least 8 idle cycles
+		 *
+		 * Implement last option to favour correctness over
+		 *   slight speed decrease
 		 */
-		swd_proc.swdptap_seq_out(0, 2);
+		swd_proc.swdptap_seq_out(0, 8);
 	}
-
 	return response;
 }
 
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index 99252761..f717073e 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -387,6 +387,12 @@ bool cortexm_probe(ADIv5_AP_t *ap)
 			target_halt_resume(t, 0);
 		}
 		break;
+	case AP_DESIGNER_CS:
+		PROBE(stm32f1_probe);
+		break;
+	case AP_DESIGNER_GIGADEVICE:
+		PROBE(gd32f1_probe);
+		break;
 	case AP_DESIGNER_STM:
 		PROBE(stm32f1_probe);
 		PROBE(stm32f4_probe);
@@ -432,20 +438,19 @@ bool cortexm_probe(ADIv5_AP_t *ap)
 						   "probed device\n", ap->ap_designer, ap->ap_partno);
 #endif
 		}
-		if (ap->ap_partno == 0x4c3)  /* Cortex-M3 ROM */
+		if (ap->ap_partno == 0x4c3)  { /* Cortex-M3 ROM */
 			PROBE(stm32f1_probe); /* Care for STM32F1 clones */
-		else if (ap->ap_partno == 0x471) { /* Cortex-M0 ROM */
+			PROBE(lpc15xx_probe); /* Thanks to JojoS for testing */
+		} else if (ap->ap_partno == 0x471)  { /* Cortex-M0 ROM */
 			PROBE(lpc11xx_probe); /* LPC24C11 */
 			PROBE(lpc43xx_probe);
-		}
-		else if (ap->ap_partno == 0x4c4) { /* Cortex-M4 ROM */
+		} else if (ap->ap_partno == 0x4c4) { /* Cortex-M4 ROM */
 			PROBE(lpc43xx_probe);
 			PROBE(lpc546xx_probe);
 			PROBE(kinetis_probe); /* Older K-series */
 		}
 		/* Info on PIDR of these parts wanted! */
 		PROBE(sam3x_probe);
-		PROBE(lpc15xx_probe);
 		PROBE(lmi_probe);
 		PROBE(ke04_probe);
 		PROBE(lpc17xx_probe);
@@ -536,8 +541,6 @@ void cortexm_detach(target *t)
 	target_mem_write32(t, CORTEXM_DEMCR, ap->ap_cortexm_demcr);
 	/* Disable debug */
 	target_mem_write32(t, CORTEXM_DHCSR, CORTEXM_DHCSR_DBGKEY);
-	/* Add some clock cycles to get the CPU running again.*/
-	target_mem_read32(t, 0);
 }
 
 enum { DB_DHCSR, DB_DCRSR, DB_DCRDR, DB_DEMCR };
@@ -837,8 +840,6 @@ static void cortexm_halt_resume(target *t, bool step)
 		target_mem_write32(t, CORTEXM_ICIALLU, 0);
 
 	target_mem_write32(t, CORTEXM_DHCSR, dhcsr);
-	/* Add some clock cycles to get the CPU running again.*/
-	target_mem_read32(t, 0);
 }
 
 static int cortexm_fault_unwind(target *t)
diff --git a/src/target/cortexm.h b/src/target/cortexm.h
index 82087859..812a683e 100644
--- a/src/target/cortexm.h
+++ b/src/target/cortexm.h
@@ -22,7 +22,7 @@
 #include "target.h"
 #include "adiv5.h"
 
-extern long cortexm_wait_timeout;
+extern unsigned cortexm_wait_timeout;
 /* Private peripheral bus base address */
 #define CORTEXM_PPB_BASE	0xE0000000
 
diff --git a/src/target/jtag_devs.c b/src/target/jtag_devs.c
index b835e1f1..af55a636 100644
--- a/src/target/jtag_devs.c
+++ b/src/target/jtag_devs.c
@@ -43,6 +43,8 @@ jtag_dev_descr_t dev_descr[] = {
 		.descr = "ST Microelectronics: STM32, Value Line, High density."},
 	{.idcode = 0x06411041, .idmask = 0xFFFFFFFF,
 		.descr = "ST Microelectronics: STM32F2xx."},
+	{.idcode = 0x06422041, .idmask = 0xFFFFFFFF,
+		.descr = "ST Microelectronics: STM32F3xx."},
 	{.idcode = 0x06413041 , .idmask = 0xFFFFFFFF,
 		.descr = "ST Microelectronics: STM32F4xx."},
 	{.idcode = 0x0BB11477 , .idmask = 0xFFFFFFFF,
diff --git a/src/target/jtag_scan.c b/src/target/jtag_scan.c
index d8927f73..127f1ffd 100644
--- a/src/target/jtag_scan.c
+++ b/src/target/jtag_scan.c
@@ -191,6 +191,18 @@ int jtag_scan(const uint8_t *irlens)
 	/*Transfer needed device information to firmware jtag_devs*/
 	for(i = 0; i < jtag_dev_count; i++)
 		platform_add_jtag_dev(i, &jtag_devs[i]);
+	for(i = 0; i < jtag_dev_count; i++) {
+		DEBUG_INFO("Idcode 0x%08" PRIx32, jtag_devs[i].jd_idcode);
+		for(j = 0; dev_descr[j].idcode; j++) {
+			if((jtag_devs[i].jd_idcode & dev_descr[j].idmask) ==
+			   dev_descr[j].idcode) {
+				DEBUG_INFO(": %s",
+						  (dev_descr[j].descr) ? dev_descr[j].descr : "unknown");
+				break;
+			}
+		}
+		DEBUG_INFO("\n");
+	}
 #endif
 
 	/* Check for known devices and handle accordingly */
diff --git a/src/target/stm32f1.c b/src/target/stm32f1.c
index 8c1fd429..53c347b2 100644
--- a/src/target/stm32f1.c
+++ b/src/target/stm32f1.c
@@ -116,6 +116,38 @@ static void stm32f1_add_flash(target *t,
 	target_add_flash(t, f);
 }
 
+/**
+    \brief identify the correct gd32 f1/f3 chip
+    GD32 : STM32 compatible chip
+*/
+bool gd32f1_probe(target *t)
+{
+	uint16_t stored_idcode = t->idcode;
+	// M3 & M4 & riscV only afaik
+	t->idcode = target_mem_read32(t, DBGMCU_IDCODE) & 0xfff;
+	uint32_t signature= target_mem_read32(t, FLASHSIZE);
+	uint32_t flashSize=signature & 0xFFFF;
+	uint32_t ramSize=signature >>16 ;
+	switch(t->idcode) {
+	case 0x414:  /* Gigadevice gd32f303 */
+		t->driver = "GD32F3";
+		break;
+	case 0x410:  /* Gigadevice gd32f103 */
+        t->driver = "GD32F1";
+		break;
+	default:
+        t->idcode = stored_idcode;
+		return false;
+	}
+	target_add_ram(t, 0x20000000, ramSize*1024);
+	stm32f1_add_flash(t, 0x8000000, flashSize*1024, 0x400);
+	target_add_commands(t, stm32f1_cmd_list, t->driver);
+	return true;
+}
+/**
+    \brief identify the stm32f1 chip
+*/
+
 bool stm32f1_probe(target *t)
 {
 	uint16_t stored_idcode = t->idcode;
@@ -126,6 +158,7 @@ bool stm32f1_probe(target *t)
 	size_t flash_size;
 	size_t block_size = 0x400;
 	switch(t->idcode) {
+	case 0x29b: /* CS clone */
 	case 0x410:  /* Medium density */
 	case 0x412:  /* Low density */
 	case 0x420:  /* Value Line, Low-/Medium density */
diff --git a/src/target/target_internal.h b/src/target/target_internal.h
index 58475bca..7299ee8f 100644
--- a/src/target/target_internal.h
+++ b/src/target/target_internal.h
@@ -169,6 +169,7 @@ int tc_system(target *t, target_addr cmd, size_t cmdlen);
 /* Probe for various targets.
  * Actual functions implemented in their respective drivers.
  */
+bool gd32f1_probe(target *t);
 bool stm32f1_probe(target *t);
 bool stm32f4_probe(target *t);
 bool stm32h7_probe(target *t);