diff --git a/scripts/dfu.py b/scripts/dfu.py
index 27c25dba..5e1e5925 100644
--- a/scripts/dfu.py
+++ b/scripts/dfu.py
@@ -84,6 +84,8 @@ class dfu_device(object):
 			self.index = self.iface.interfaceNumber
 		else:	self.index = self.iface
 
+	def release(self):
+		self.handle.releaseInterface()
 	def detach(self, wTimeout=255):
 		self.handle.controlMsg(usb.ENDPOINT_OUT | usb.TYPE_CLASS | 
 			usb.RECIP_INTERFACE, DFU_DETACH, 
diff --git a/scripts/stm32_mem.py b/scripts/stm32_mem.py
index 780da327..33d35f9d 100755
--- a/scripts/stm32_mem.py
+++ b/scripts/stm32_mem.py
@@ -2,6 +2,7 @@
 #
 # stm32_mem.py: STM32 memory access using USB DFU class
 # Copyright (C) 2011  Black Sphere Technologies 
+# Copyright (C) 2017  Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
 # Written by Gareth McMullin <gareth@blacksphere.co.nz>
 #
 # This program is free software: you can redistribute it and/or modify
@@ -57,7 +58,17 @@ def stm32_write(dev, data):
 			sleep(status.bwPollTimeout / 1000.0)
 		if status.bState == dfu.STATE_DFU_DOWNLOAD_IDLE:
 			break
-	
+
+def stm32_read(dev):
+	data = dev.upload(2, 1024)
+	while True:
+		status = dev.get_status()
+		if status.bState == dfu.STATE_DFU_DOWNLOAD_BUSY:
+			sleep(status.bwPollTimeout / 1000.0)
+		if status.bState == dfu.STATE_DFU_UPLOAD_IDLE:
+			break
+	return data
+
 def stm32_manifest(dev):
 	dev.download(0, "")
 	while True:
@@ -68,6 +79,51 @@ def stm32_manifest(dev):
 		sleep(status.bwPollTimeout / 1000.0)
 		if status.bState == dfu.STATE_DFU_MANIFEST:
 			break
+def stm32_scan(args):
+	devs = dfu.finddevs()
+	bmp = 0
+	if not devs:
+		print "No DFU devices found!"
+		exit(-1)
+
+	for dev in devs:
+		dfudev = dfu.dfu_device(*dev)
+		man = dfudev.handle.getString(dfudev.dev.iManufacturer, 30)
+		if man == "Black Sphere Technologies": bmp = bmp + 1
+	if bmp == 0 :
+		print "No compatible device found\n"
+		exit(-1)
+	if bmp > 1 and not args.serial_target :
+		print "Found multiple devices:\n"
+		for dev in devs:
+			dfudev = dfu.dfu_device(*dev)
+			man = dfudev.handle.getString(dfudev.dev.iManufacturer, 30)
+			product = dfudev.handle.getString(dfudev.dev.iProduct, 96)
+			serial_no = dfudev.handle.getString(dfudev.dev.iSerialNumber, 30)
+			print "Device ID:\t %04x:%04x" % (dfudev.dev.idVendor, dfudev.dev.idProduct)
+			print "Manufacturer:\t %s" % man
+			print "Product:\t %s" % product
+			print "Serial:\t\t %s\n" % serial_no
+		print "Select device with serial number!"
+		exit (-1)
+
+	for dev in devs:
+		dfudev = dfu.dfu_device(*dev)
+		man = dfudev.handle.getString(dfudev.dev.iManufacturer, 30)
+		product = dfudev.handle.getString(dfudev.dev.iProduct, 96)
+		serial_no = dfudev.handle.getString(dfudev.dev.iSerialNumber, 30)
+		if args.serial_target:
+			if man == "Black Sphere Technologies" and serial_no ==	args.serial_target: break
+		else:
+			if man == "Black Sphere Technologies": break
+	print "Device ID:\t %04x:%04x" % (dfudev.dev.idVendor, dfudev.dev.idProduct)
+	print "Manufacturer:\t %s" % man
+	print "Product:\t %s" % product
+	print "Serial:\t\t %s" % serial_no
+	if args.serial_target and serial_no !=	args.serial_target:
+		print "Serial number doesn't match!\n"
+		exit(-2)
+	return dfudev
 
 if __name__ == "__main__":
 	print
@@ -79,55 +135,56 @@ if __name__ == "__main__":
 	parser = argparse.ArgumentParser()
 	parser.add_argument("progfile", help="Binary file to program")
 	parser.add_argument("-s", "--serial_target", help="Match Serial Number")
+	parser.add_argument("-a", "--address", help="Start address for firmware")
+	parser.add_argument("-m", "--manifest", help="Start application, if in DFU mode", action='store_true')
 	args = parser.parse_args()
-	devs = dfu.finddevs()
-	if not devs:
-		print "No devices found!"
-		exit(-1)
-
-	for dev in devs:
-		dfudev = dfu.dfu_device(*dev)
-		man = dfudev.handle.getString(dfudev.dev.iManufacturer, 30)
-		product = dfudev.handle.getString(dfudev.dev.iProduct, 64)
-		serial_no = dfudev.handle.getString(dfudev.dev.iSerialNumber, 30)
-		if args.serial_target:
-			if man == "Black Sphere Technologies" and serial_no ==  args.serial_target: break
-			if man == "STMicroelectronics" and serial_no == args.serial_target: break
-                else:
-			if man == "Black Sphere Technologies": break
-			if man == "STMicroelectronics": break
-
-	print "Device %s: ID %04x:%04x %s - %s\n\tSerial %s" % (
-                dfudev.dev.filename, dfudev.dev.idVendor,
-                dfudev.dev.idProduct, man, product, serial_no)
-
-	if args.serial_target and serial_no !=  args.serial_target:
-                print "Serial number doesn't match!\n"
-                exit(-2)
+	dfudev = stm32_scan(args)
 	try:
 		state = dfudev.get_state()
 	except:
+		if args.manifest : exit(0)
 		print "Failed to read device state! Assuming APP_IDLE"
 		state = dfu.STATE_APP_IDLE
 	if state == dfu.STATE_APP_IDLE:
 		dfudev.detach()
-		print "Run again to upgrade firmware."
+		dfudev.release()
+		print "Invoking DFU Device"
+		timeout = 0
+		while True :
+			sleep(0.5)
+			timeout = timeout + 0.5
+			dfudev = stm32_scan(args)
+			if dfudev: break
+			if timeout > 5 :
+				print "Error: DFU device did not appear"
+				exit(-1)
+	if args.manifest :
+		stm32_manifest(dfudev)
+		print "Invoking Application Device"
 		exit(0)
-	
 	dfudev.make_idle()
+	file = open(args.progfile, "rb")
+	bin = file.read()
 
-	bin = open(args.progfile, "rb").read()
-
-	if "F4" in product:
-		addr = 0x8004000
-	else:
-		addr = 0x8002000
+	product = dfudev.handle.getString(dfudev.dev.iProduct, 64)
+	if args.address :
+		start = int(args.address, 0)
+	else :
+		if "F4" in product:
+			start = 0x8004000
+		else:
+			start = 0x8002000
+	addr = start
 	while bin:
 		print ("Programming memory at 0x%08X\r" % addr),
 		stdout.flush()
 		try:
+# STM DFU bootloader erases always.
+# BPM Bootloader only erases once per sector
+# To support the STM DFU bootloader, the interface descriptor must
+# get evaluated and erase called only once per sector!
 			stm32_erase(dfudev, addr)
-                except:
+		except:
 			print "\nErase Timed out\n"
 			break
 		try:
@@ -136,10 +193,34 @@ if __name__ == "__main__":
 			print "\nSet Address Timed out\n"
 			break
 		stm32_write(dfudev, bin[:1024])
-
 		bin = bin[1024:]
 		addr += 1024
-
+	file.seek(0)
+	bin = file.read()
+	len = len(bin)
+	addr = start
+        print
+        while bin:
+		try:
+			stm32_set_address(dfudev, addr)
+			data = stm32_read(dfudev)
+		except:
+# Abort silent if bootloader does not support upload
+			break
+		print ("Verifying memory at   0x%08X\r" % addr),
+                stdout.flush()
+		if len > 1024 :
+			size = 1024
+		else :
+			size = len
+		if bin[:size] != bytearray(data[:size]) :
+			print ("\nMitmatch in block at	0x%08X" % addr)
+			break;
+		bin = bin[1024:]
+		addr += 1024
+		len -= 1024
+		if len <= 0 :
+			print "\nVerified!"
 	stm32_manifest(dfudev)
 
-	print "\nAll operations complete!\n"
+	print "All operations complete!\n"
diff --git a/src/platforms/native/platform.c b/src/platforms/native/platform.c
index 338cd857..1804e175 100644
--- a/src/platforms/native/platform.c
+++ b/src/platforms/native/platform.c
@@ -304,57 +304,3 @@ static void setup_vbus_irq(void)
 
 	exti15_10_isr();
 }
-
-#ifdef ENABLE_DEBUG
-enum {
-	RDI_SYS_OPEN = 0x01,
-	RDI_SYS_WRITE = 0x05,
-	RDI_SYS_ISTTY = 0x09,
-};
-
-int rdi_write(int fn, const char *buf, size_t len)
-{
-	(void)fn;
-	if (debug_bmp)
-		return len - usbuart_debug_write(buf, len);
-
-	return 0;
-}
-
-struct ex_frame {
-	union {
-		int syscall;
-		int retval;
-	};
-	const int *params;
-	uint32_t r2, r3, r12, lr, pc;
-};
-
-void debug_monitor_handler_c(struct ex_frame *sp)
-{
-	/* Return to after breakpoint instruction */
-	sp->pc += 2;
-
-	switch (sp->syscall) {
-	case RDI_SYS_OPEN:
-		sp->retval = 1;
-		break;
-	case RDI_SYS_WRITE:
-		sp->retval = rdi_write(sp->params[0], (void*)sp->params[1], sp->params[2]);
-		break;
-	case RDI_SYS_ISTTY:
-		sp->retval = 1;
-		break;
-	default:
-		sp->retval = -1;
-	}
-
-}
-
-asm(".globl debug_monitor_handler\n"
-    ".thumb_func\n"
-    "debug_monitor_handler: \n"
-    "    mov r0, sp\n"
-    "    b debug_monitor_handler_c\n");
-
-#endif
diff --git a/src/platforms/stlink/Bootloader_Upgrade b/src/platforms/stlink/Bootloader_Upgrade
new file mode 100644
index 00000000..f9c93f0c
--- /dev/null
+++ b/src/platforms/stlink/Bootloader_Upgrade
@@ -0,0 +1,22 @@
+Bootloader Upgrade on Stlink
+============================
+
+Beside accessing the SWD pins direct like explained on
+https://embdev.net/articles/STM_Discovery_as_Black_Magic_Probe
+an updated bootloader can also be programmed via DFU. This requires three
+steps:
+1. Prepare bootloader update
+Normal BMP has to be replaced by the upgrade programm:
+ dfu-util -s 0x08002000:leave -D dfu_upgrade.bin
+Wait until Dual color led flashes red, indicating DFU is active for the
+bootloader.
+
+2. Flash new bootloader
+ dfu-util -s 0x08000000 -D blackmagic_dfu.bin
+Wait until Dual color led flashes green, indicating bootloader is active.
+
+If not, unplug USB, keep black reset button pressed, replug USB.
+Wait until Dual color led flashes green.
+
+3. Flash BMP
+ dfu-util -s 0x08002000:leave:force -D blackmagic.bin
diff --git a/src/platforms/stlink/Flashsize_F103 b/src/platforms/stlink/Flashsize_F103
index d4ece0f3..c6b55a24 100644
--- a/src/platforms/stlink/Flashsize_F103
+++ b/src/platforms/stlink/Flashsize_F103
@@ -24,16 +24,30 @@ Ignoring the chip marking and using an F103C8 blindly as a F103Cb is done
 already with few problems on many china boards (e.g. blue pill). Probably
 this second approach will work for many of the older STLinks.
 
-Use at your own risk!
+dfu-util cares for the size and refuses to programm above the announced size:
+ > dfu-util -S E4D078EA -s 0x08002000:leave -D blackmagic.bin
+ dfu-util 0.9
+ ...
+ dfu-util: Last page at 0x0801093f is not writeable
 
-With DFU upload available in the bootloader, you can verify by uploading the
-binary from flash and comparing it against the binary downloaded.
-- Download new BMP binary (if not already done)
-  dfu-util -s 0x08002000:leave:force -D blackmagic.bin
-- Get length of binary
+Flash above the announced size with recent bootloader/BMP:
+==========================================================
+script/stm32_mem.py does not care for the announced size:
+ > ../scripts/stm32_mem.py blackmagic.bin
+ ...
+ USB Device Firmware Upgrade - Host Utility -- version 1.2
+ ...
+ Programming memory at 0x08010800
+ All operations complete!
+
+Get length of binary
   > ls -l blackmagic.bin
-  -rwxr-xr-x 1 bon users 57372 15. Apr 14:17 blackmagic.bin
-- Upload binary from flash
-  > dfu-util -s 0x08002000:leave:force:57372 -U blackmagic.bin.1
-- Compare
+ -rwxr-xr-x 1 bon users 59712 21. Sep 22:47 blackmagic.bin
+Actual file size may differ!
+
+Upload binary from flash with the exact size
+  > dfu-util -s 0x08002000:leave:force:59712 -U blackmagic.bin.1
+
+Compare
   > diff blackmagic.bin*
+No differences should get reported!
diff --git a/src/platforms/stlink/Makefile.inc b/src/platforms/stlink/Makefile.inc
index d7edcf46..18486e83 100644
--- a/src/platforms/stlink/Makefile.inc
+++ b/src/platforms/stlink/Makefile.inc
@@ -8,11 +8,17 @@ CFLAGS += -mcpu=cortex-m3 -mthumb \
 	-I platforms/stm32
 LDFLAGS_BOOT := $(LDFLAGS) --specs=nano.specs \
 	-lopencm3_stm32f1 -Wl,--defsym,_stack=0x20005000 \
-	-Wl,-T,platforms/stm32/stlink.ld -nostartfiles -lc -lnosys \
+	-Wl,-T,platforms/stm32/stlink.ld -nostartfiles -lc \
 	-Wl,-Map=mapfile -mthumb -mcpu=cortex-m3 -Wl,-gc-sections \
 	-L../libopencm3/lib
 LDFLAGS = $(LDFLAGS_BOOT) -Wl,-Ttext=0x8002000
 
+ifeq ($(ENABLE_DEBUG), 1)
+LDFLAGS += --specs=rdimon.specs
+else
+LDFLAGS += --specs=nosys.specs
+endif
+
 VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
@@ -20,14 +26,15 @@ SRC += 	cdcacm.c	\
 	serialno.c	\
 	timing.c	\
 	timing_stm32.c	\
+	stlink_common.c \
 
 all:	blackmagic.bin blackmagic_dfu.bin blackmagic_dfu.hex dfu_upgrade.bin dfu_upgrade.hex
 
-blackmagic_dfu: usbdfu.o dfucore.o dfu_f1.o
+blackmagic_dfu: usbdfu.o dfucore.o dfu_f1.o stlink_common.o
 	@echo "  LD      $@"
 	$(Q)$(CC) $^ -o $@ $(LDFLAGS_BOOT)
 
-dfu_upgrade: dfu_upgrade.o dfucore.o dfu_f1.o
+dfu_upgrade: dfu_upgrade.o dfucore.o dfu_f1.o  stlink_common.o
 	@echo "  LD      $@"
 	$(Q)$(CC) $^ -o $@ $(LDFLAGS)
 
diff --git a/src/platforms/stlink/dfu_upgrade.c b/src/platforms/stlink/dfu_upgrade.c
index f5fc503d..804144a1 100644
--- a/src/platforms/stlink/dfu_upgrade.c
+++ b/src/platforms/stlink/dfu_upgrade.c
@@ -24,84 +24,40 @@
 #include <libopencm3/cm3/scb.h>
 
 #include "usbdfu.h"
-uint32_t app_address = 0x08000000;
+#include "general.h"
+#include "platform.h"
 
-static uint8_t rev;
-static uint16_t led_idle_run;
+uint32_t app_address = 0x08000000;
+static uint16_t led_upgrade;
 static uint32_t led2_state = 0;
+extern uint32_t _stack;
+static uint32_t rev;
 
 void dfu_detach(void)
 {
-	/* Disconnect USB cable by resetting USB Device
-	   and pulling USB_DP low*/
-	rcc_periph_reset_pulse(RST_USB);
-	rcc_periph_clock_enable(RCC_USB);
-	rcc_periph_clock_enable(RCC_GPIOA);
-	gpio_clear(GPIOA, GPIO12);
-	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
-		GPIO_CNF_OUTPUT_OPENDRAIN, GPIO12);
-	/* Pull PB0 (T_NRST) low
-	 */
-	rcc_periph_clock_enable(RCC_GPIOB);
-	gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_2_MHZ,
-		GPIO_CNF_OUTPUT_OPENDRAIN, GPIO0);
-	gpio_clear(GPIOB, GPIO0);
-	SCB_VTOR = 0;
+	platform_request_boot();
 	scb_reset_core();
 }
 
-void stlink_set_rev(void)
-{
-	int i;
-
-	/* First, get Board revision by pulling PC13/14 up. Read
-	 *  11 for ST-Link V1, e.g. on VL Discovery, tag as rev 0
-	 *  10 for ST-Link V2, e.g. on F4 Discovery, tag as rev 1
-	 */
-	rcc_periph_clock_enable(RCC_GPIOC);
-	gpio_set_mode(GPIOC, GPIO_MODE_INPUT,
-			GPIO_CNF_INPUT_PULL_UPDOWN, GPIO14 | GPIO13);
-	gpio_set(GPIOC, GPIO14 | GPIO13);
-	for (i = 0; i < 100; i++)
-		rev = (~(gpio_get(GPIOC, GPIO14 | GPIO13)) >> 13) & 3;
-
-	switch (rev) {
-	case 0:
-		led_idle_run = GPIO8;
-		break;
-	default:
-		led_idle_run = GPIO9;
-	}
-	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
-			GPIO_CNF_OUTPUT_PUSHPULL, led_idle_run);
-}
-
 int main(void)
 {
-
+	rev = detect_rev();
 	rcc_clock_setup_in_hse_8mhz_out_72mhz();
-
-	stlink_set_rev();
+	if (rev == 0)
+		led_upgrade = GPIO8;
+	else
+		led_upgrade = GPIO9;
 
 	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
 	systick_set_reload(900000);
 
 	dfu_protect(UPD_MODE);
 
-	/* Handle USB disconnect/connect */
-	/* Just in case: Disconnect USB cable by resetting USB Device
-	 * and pulling USB_DP low
-	 * Device will reconnect automatically as Pull-Up is hard wired*/
-	rcc_periph_reset_pulse(RST_USB);
-	rcc_periph_clock_enable(RCC_USB);
-	rcc_periph_clock_enable(RCC_GPIOA);
-	gpio_clear(GPIOA, GPIO12);
-	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
-		GPIO_CNF_OUTPUT_OPENDRAIN, GPIO12);
-
 	systick_interrupt_enable();
 	systick_counter_enable();
 
+	if (rev > 1) /* Reconnect USB */
+		gpio_set(GPIOA, GPIO15);
 	dfu_init(&stm32f103_usb_driver, UPD_MODE);
 
 	dfu_main();
@@ -114,15 +70,15 @@ void dfu_event(void)
 void sys_tick_handler(void)
 {
 	if (rev == 0) {
-		gpio_toggle(GPIOA, led_idle_run);
+		gpio_toggle(GPIOA, led_upgrade);
 	} else {
 		if (led2_state & 1) {
 			gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
-				GPIO_CNF_OUTPUT_PUSHPULL, led_idle_run);
-			gpio_set(GPIOA, led_idle_run);
+				GPIO_CNF_OUTPUT_PUSHPULL, led_upgrade);
+			gpio_set(GPIOA, led_upgrade);
 		} else {
 			gpio_set_mode(GPIOA, GPIO_MODE_INPUT,
-				GPIO_CNF_INPUT_ANALOG, led_idle_run);
+				GPIO_CNF_INPUT_ANALOG, led_upgrade);
 		}
 		led2_state++;
 	}
diff --git a/src/platforms/stlink/platform.c b/src/platforms/stlink/platform.c
index dcc14932..5e2ca3bf 100644
--- a/src/platforms/stlink/platform.c
+++ b/src/platforms/stlink/platform.c
@@ -28,62 +28,38 @@
 
 #include <libopencm3/stm32/rcc.h>
 #include <libopencm3/cm3/scb.h>
+#include <libopencm3/cm3/scs.h>
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/stm32/usart.h>
 #include <libopencm3/usb/usbd.h>
 #include <libopencm3/stm32/adc.h>
 
 uint8_t running_status;
-volatile uint32_t timeout_counter;
 
 uint16_t led_idle_run;
-/* Pins PC[14:13] are used to detect hardware revision. Read
- * 11 for STLink V1 e.g. on VL Discovery, tag as hwversion 0
- * 10 for STLink V2 e.g. on F4 Discovery, tag as hwversion 1
- */
+uint16_t srst_pin;
+static uint32_t rev;
+
 int platform_hwversion(void)
 {
-	static int hwversion = -1;
-	int i;
-	if (hwversion == -1) {
-		gpio_set_mode(GPIOC, GPIO_MODE_INPUT,
-		              GPIO_CNF_INPUT_PULL_UPDOWN, GPIO14 | GPIO13);
-		gpio_set(GPIOC, GPIO14 | GPIO13);
-		for (i = 0; i<10; i++)
-			hwversion = ~(gpio_get(GPIOC, GPIO14 | GPIO13) >> 13) & 3;
-		switch (hwversion)
-		{
-		case 0:
-			led_idle_run = GPIO8;
-			break;
-		default:
-			led_idle_run = GPIO9;
-		}
-	}
-	return hwversion;
+	return rev;
 }
 
 void platform_init(void)
 {
+	rev = detect_rev();
+	SCS_DEMCR |= SCS_DEMCR_VC_MON_EN;
+#ifdef ENABLE_DEBUG
+	void initialise_monitor_handles(void);
+	initialise_monitor_handles();
+#endif
 	rcc_clock_setup_in_hse_8mhz_out_72mhz();
-
-	/* Enable peripherals */
-	rcc_periph_clock_enable(RCC_USB);
-	rcc_periph_clock_enable(RCC_GPIOA);
-	rcc_periph_clock_enable(RCC_GPIOB);
-	rcc_periph_clock_enable(RCC_GPIOC);
-	rcc_periph_clock_enable(RCC_AFIO);
-	rcc_periph_clock_enable(RCC_CRC);
-
-	/* On Rev 1 unconditionally activate MCO on PORTA8 with HSE
-	 * platform_hwversion() also needed to initialize led_idle_run!
-	 */
-	if (platform_hwversion() == 1)
-	{
-		RCC_CFGR &= ~(0xf << 24);
-		RCC_CFGR |= (RCC_CFGR_MCO_HSECLK << 24);
-		gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ,
-		GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO8);
+	if (rev == 0) {
+		led_idle_run = GPIO8;
+		srst_pin = SRST_PIN_V1;
+	} else {
+		led_idle_run = GPIO9;
+		srst_pin = SRST_PIN_V2;
 	}
 	/* Setup GPIO ports */
 	gpio_set_mode(TMS_PORT, GPIO_MODE_OUTPUT_50_MHZ,
@@ -92,8 +68,6 @@ void platform_init(void)
 	              GPIO_CNF_OUTPUT_PUSHPULL, TCK_PIN);
 	gpio_set_mode(TDI_PORT, GPIO_MODE_OUTPUT_50_MHZ,
 	              GPIO_CNF_OUTPUT_PUSHPULL, TDI_PIN);
-	uint16_t srst_pin = platform_hwversion() == 0 ?
-	                    SRST_PIN_V1 : SRST_PIN_V2;
 	gpio_set(SRST_PORT, srst_pin);
 	gpio_set_mode(SRST_PORT, GPIO_MODE_OUTPUT_50_MHZ,
 	              GPIO_CNF_OUTPUT_OPENDRAIN, srst_pin);
@@ -106,51 +80,28 @@ void platform_init(void)
 	SCB_VTOR = (uint32_t)&vector_table;
 
 	platform_timing_init();
+	if (rev > 1) /* Reconnect USB */
+		gpio_set(GPIOA, GPIO15);
 	cdcacm_init();
-	usbuart_init();
+	/* Don't enable UART if we're being debugged. */
+	if (!(SCS_DEMCR & SCS_DEMCR_TRCENA))
+		usbuart_init();
 }
 
 void platform_srst_set_val(bool assert)
 {
-	uint16_t pin;
-	pin = platform_hwversion() == 0 ? SRST_PIN_V1 : SRST_PIN_V2;
 	if (assert)
-		gpio_clear(SRST_PORT, pin);
+		gpio_clear(SRST_PORT, srst_pin);
 	else
-		gpio_set(SRST_PORT, pin);
+		gpio_set(SRST_PORT, srst_pin);
 }
 
 bool platform_srst_get_val()
 {
-	uint16_t pin;
-	pin = platform_hwversion() == 0 ? SRST_PIN_V1 : SRST_PIN_V2;
-	return gpio_get(SRST_PORT, pin) == 0;
+	return gpio_get(SRST_PORT, srst_pin) == 0;
 }
 
 const char *platform_target_voltage(void)
 {
 	return "unknown";
 }
-
-void platform_request_boot(void)
-{
-	/* Disconnect USB cable by resetting USB Device and pulling USB_DP low*/
-	rcc_periph_reset_pulse(RST_USB);
-	rcc_periph_clock_enable(RCC_USB);
-	rcc_periph_clock_enable(RCC_GPIOA);
-	gpio_clear(GPIOA, GPIO12);
-	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
-	              GPIO_CNF_OUTPUT_OPENDRAIN, GPIO12);
-
-	/* Assert bootloader pin */
-	uint32_t crl = GPIOA_CRL;
-	rcc_periph_clock_enable(RCC_GPIOA);
-	/* Enable Pull on GPIOA1. We don't rely on the external pin
-	 * really pulled, but only on the value of the CNF register
-	 * changed from the reset value
-	 */
-	crl &= 0xffffff0f;
-	crl |= 0x80;
-	GPIOA_CRL = crl;
-}
-
diff --git a/src/platforms/stlink/platform.h b/src/platforms/stlink/platform.h
index f74a1af2..f531444a 100644
--- a/src/platforms/stlink/platform.h
+++ b/src/platforms/stlink/platform.h
@@ -33,6 +33,11 @@
 #include <libopencm3/stm32/f1/memorymap.h>
 #include <libopencm3/usb/usbd.h>
 
+#ifdef ENABLE_DEBUG
+# define PLATFORM_HAS_DEBUG
+# define USBUART_DEBUG
+#endif
+
 #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 ")"
@@ -102,7 +107,13 @@
 #define USBUSART_TIM_IRQ NVIC_TIM4_IRQ
 #define USBUSART_TIM_ISR tim4_isr
 
-#define DEBUG(...)
+#ifdef ENABLE_DEBUG
+extern bool debug_bmp;
+int usbuart_debug_write(const char *buf, size_t len);
+# define DEBUG printf
+#else
+# define DEBUG(...)
+#endif
 
 extern uint16_t led_idle_run;
 #define LED_IDLE_RUN            led_idle_run
@@ -110,6 +121,8 @@ extern uint16_t led_idle_run;
 #define SET_IDLE_STATE(state)	{gpio_set_val(LED_PORT, led_idle_run, state);}
 #define SET_ERROR_STATE(x)
 
+extern uint32_t detect_rev(void);
+
 /* Use newlib provided integer only stdio functions */
 #define sscanf siscanf
 #define sprintf siprintf
diff --git a/src/platforms/stlink/stlink_common.c b/src/platforms/stlink/stlink_common.c
new file mode 100644
index 00000000..487563d8
--- /dev/null
+++ b/src/platforms/stlink/stlink_common.c
@@ -0,0 +1,102 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2017 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 <libopencm3/cm3/scb.h>
+#include <libopencm3/stm32/rcc.h>
+#include <libopencm3/stm32/gpio.h>
+
+/* return 0 for stlink V1, 1 for stlink V2 and 2 for stlink V2.1 */
+uint32_t detect_rev(void)
+{
+	uint32_t rev;
+	int res;
+
+	while (RCC_CFGR & 0xf) /* Switch back to HSI. */
+		RCC_CFGR &= ~3;
+	rcc_periph_clock_enable(RCC_GPIOA);
+	rcc_periph_clock_enable(RCC_GPIOB);
+	rcc_periph_clock_enable(RCC_GPIOC);
+	rcc_periph_clock_enable(RCC_USB);
+	rcc_periph_reset_pulse(RST_USB);
+	rcc_periph_clock_enable(RCC_AFIO);
+	rcc_periph_clock_enable(RCC_CRC);
+	/* First, get Board revision by pulling PC13/14 up. Read
+	 *  11 for ST-Link V1, e.g. on VL Discovery, tag as rev 0
+	 *  00 for ST-Link V2, e.g. on F4 Discovery, tag as rev 1
+	 *  01 for ST-Link V2, else,                 tag as rev 1
+	 */
+	gpio_set_mode(GPIOC, GPIO_MODE_INPUT,
+				  GPIO_CNF_INPUT_PULL_UPDOWN, GPIO14 | GPIO13);
+	gpio_set(GPIOC, GPIO14 | GPIO13);
+	for (int i = 0; i < 100; i ++)
+		res = gpio_get(GPIOC, GPIO13);
+	if (res)
+		rev = 0;
+	else {
+		/* Check for V2.1 boards.
+		 * PA15/TDI is USE_RENUM, pulled with 10 k to U5V on V2.1,
+		 * Otherwise unconnected. Enable pull low. If still high.
+		 * it is V2.1.*/
+		rcc_periph_clock_enable(RCC_AFIO);
+		AFIO_MAPR |= 0x02000000; /* Release from TDI.*/
+		gpio_set_mode(GPIOA, GPIO_MODE_INPUT,
+                                 GPIO_CNF_INPUT_PULL_UPDOWN, GPIO15);
+		gpio_clear(GPIOA, GPIO15);
+		for (int i = 0; i < 100; i++)
+			res =  gpio_get(GPIOA, GPIO15);
+		if (res) {
+			rev = 2;
+			/* Pull PWR_ENn low.*/
+			gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_2_MHZ,
+						  GPIO_CNF_OUTPUT_OPENDRAIN, GPIO15);
+			gpio_clear(GPIOB, GPIO15);
+			/* Pull USB_RENUM low!*/
+			gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
+						  GPIO_CNF_OUTPUT_OPENDRAIN, GPIO15);
+			gpio_clear(GPIOA, GPIO15);
+		} else
+			/* Catch F4 Disco board with both resistors fitted.*/
+			rev = 1;
+		/* On Rev > 0 unconditionally activate MCO on PORTA8 with HSE! */
+		RCC_CFGR &= ~(0xf << 24);
+		RCC_CFGR |= (RCC_CFGR_MCO_HSECLK << 24);
+		gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ,
+		GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO8);
+	}
+	if (rev < 2) {
+		gpio_clear(GPIOA, GPIO12);
+		gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
+					  GPIO_CNF_OUTPUT_OPENDRAIN, GPIO12);
+	}
+	return rev;
+}
+
+void platform_request_boot(void)
+{
+	uint32_t crl = GPIOA_CRL;
+	/* Assert bootloader marker.
+	 * Enable Pull on GPIOA1. We don't rely on the external pin
+	 * really pulled, but only on the value of the CNF register
+	 * changed from the reset value
+	 */
+	crl &= 0xffffff0f;
+	crl |= 0x80;
+	GPIOA_CRL = crl;
+	SCB_VTOR = 0;
+}
diff --git a/src/platforms/stlink/usbdfu.c b/src/platforms/stlink/usbdfu.c
index f5c30a98..fca3bf64 100644
--- a/src/platforms/stlink/usbdfu.c
+++ b/src/platforms/stlink/usbdfu.c
@@ -24,108 +24,57 @@
 #include <libopencm3/cm3/scb.h>
 
 #include "usbdfu.h"
+#include "general.h"
+#include "platform.h"
 
-static uint8_t rev;
-static uint16_t led_idle_run;
+static uint32_t rev;
+static uint16_t led_bootloader;
+static uint16_t pin_nrst;
 static uint32_t led2_state = 0;
 
 uint32_t app_address = 0x08002000;
 
-static int stlink_test_nrst(void)
+static bool stlink_test_nrst(void)
 {
-	/* Test if JRST/NRST is pulled down*/
 	uint16_t nrst;
-	uint16_t pin;
-	uint32_t systick_value;
-
-	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
-	systick_set_reload(0xffffff); /* no underflow for about 16.7 seconds*/
-	systick_counter_enable();
-	/* systick ist now running with 1 MHz, systick counts down */
-
-	/* First, get Board revision by pulling PC13/14 up. Read
-	 *  11 for ST-Link V1, e.g. on VL Discovery, tag as rev 0
-	 *  10 for ST-Link V2, e.g. on F4 Discovery, tag as rev 1
-	 */
-	rcc_periph_clock_enable(RCC_GPIOC);
-	gpio_set_mode(GPIOC, GPIO_MODE_INPUT,
-				  GPIO_CNF_INPUT_PULL_UPDOWN, GPIO14 | GPIO13);
-	gpio_set(GPIOC, GPIO14 | GPIO13);
-	systick_value = systick_get_value();
-	while (systick_get_value() > (systick_value - 1000)); /* Wait 1 msec*/
-	rev = (~(gpio_get(GPIOC, GPIO14 | GPIO13)) >> 13) & 3;
-	switch (rev) {
-	case 0:
-		pin = GPIO1;
-		led_idle_run = GPIO8;
-		break;
-	default:
-		pin = GPIO0;
-		led_idle_run = GPIO9;
-	}
-	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
-			GPIO_CNF_OUTPUT_PUSHPULL, led_idle_run);
-	rcc_periph_clock_enable(RCC_GPIOB);
 	gpio_set_mode(GPIOB, GPIO_MODE_INPUT,
-	              GPIO_CNF_INPUT_PULL_UPDOWN, pin | GPIO15);
-	if (gpio_get(GPIOB, GPIO15)) {
-		/* ST890 is active low */
-		gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_2_MHZ,
-		              GPIO_CNF_OUTPUT_PUSHPULL, GPIO15);
-		gpio_clear(GPIOB, GPIO15);
-	}
-	gpio_set(GPIOB, pin);
-	systick_value = systick_get_value();
-	while (systick_get_value() > (systick_value - 20000)); /* Wait 20 msec*/
-	nrst = gpio_get(GPIOB, pin);
-	systick_counter_disable();
-	return (nrst) ? 1 : 0;
+	              GPIO_CNF_INPUT_PULL_UPDOWN, pin_nrst);
+	gpio_set(GPIOB, pin_nrst);
+	for (int i = 0; i < 10000; i++)
+		nrst = gpio_get(GPIOB, pin_nrst);
+	return (nrst) ? true : false;
 }
 
 void dfu_detach(void)
 {
-	/* Disconnect USB cable by resetting USB Device
-	   and pulling USB_DP low*/
-	rcc_periph_reset_pulse(RST_USB);
-	rcc_periph_clock_enable(RCC_USB);
-	rcc_periph_clock_enable(RCC_GPIOA);
-	gpio_clear(GPIOA, GPIO12);
-	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
-		GPIO_CNF_OUTPUT_OPENDRAIN, GPIO12);
 	scb_reset_system();
 }
 
 int main(void)
 {
-	/* Check the force bootloader pin*/
-	rcc_periph_clock_enable(RCC_GPIOA);
-	/* Check value of GPIOA1 configuration. This pin is unconnected on
-	 * STLink V1 and V2. If we have a value other than the reset value (0x4),
-	 * we have a warm start and request Bootloader entry
-	 */
+	rev = detect_rev();
+	if (rev == 0) {
+		led_bootloader = GPIO8;
+		pin_nrst = GPIO1;
+	} else {
+		led_bootloader = GPIO9;
+		pin_nrst = GPIO0;
+	}
+
 	if(((GPIOA_CRL & 0x40) == 0x40) && stlink_test_nrst())
 		dfu_jump_app_if_valid();
-
 	dfu_protect(DFU_MODE);
 
 	rcc_clock_setup_in_hse_8mhz_out_72mhz();
 	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
 	systick_set_reload(900000);
 
-	/* Handle USB disconnect/connect */
-	/* Just in case: Disconnect USB cable by resetting USB Device
-	 * and pulling USB_DP low
-	 * Device will reconnect automatically as Pull-Up is hard wired*/
-	rcc_periph_reset_pulse(RST_USB);
-	rcc_periph_clock_enable(RCC_USB);
-	rcc_periph_clock_enable(RCC_GPIOA);
-	gpio_clear(GPIOA, GPIO12);
-	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
-		GPIO_CNF_OUTPUT_OPENDRAIN, GPIO12);
 
 	systick_interrupt_enable();
 	systick_counter_enable();
 
+	if (rev > 1)
+		gpio_set(GPIOA, GPIO15);
 	dfu_init(&stm32f103_usb_driver, DFU_MODE);
 
 	dfu_main();
@@ -138,16 +87,16 @@ void dfu_event(void)
 void sys_tick_handler(void)
 {
 	if (rev == 0) {
-		gpio_toggle(GPIOA, led_idle_run);
+		gpio_toggle(GPIOA, led_bootloader);
 	} else {
 		if (led2_state & 1) {
 			gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
-				GPIO_CNF_OUTPUT_PUSHPULL, led_idle_run);
+				GPIO_CNF_OUTPUT_PUSHPULL, led_bootloader);
+			gpio_clear(GPIOA, led_bootloader);
 		} else {
 			gpio_set_mode(GPIOA, GPIO_MODE_INPUT,
-				GPIO_CNF_INPUT_ANALOG, led_idle_run);
+				GPIO_CNF_INPUT_ANALOG, led_bootloader);
 		}
 		led2_state++;
 	}
 }
-
diff --git a/src/platforms/stm32/usbuart.c b/src/platforms/stm32/usbuart.c
index ed74d36c..a4a772e1 100644
--- a/src/platforms/stm32/usbuart.c
+++ b/src/platforms/stm32/usbuart.c
@@ -250,3 +250,57 @@ void USBUSART_TIM_ISR(void)
 	/* process FIFO */
 	usbuart_run();
 }
+
+#ifdef ENABLE_DEBUG
+enum {
+	RDI_SYS_OPEN = 0x01,
+	RDI_SYS_WRITE = 0x05,
+	RDI_SYS_ISTTY = 0x09,
+};
+
+int rdi_write(int fn, const char *buf, size_t len)
+{
+	(void)fn;
+	if (debug_bmp)
+		return len - usbuart_debug_write(buf, len);
+
+	return 0;
+}
+
+struct ex_frame {
+	union {
+		int syscall;
+		int retval;
+	};
+	const int *params;
+	uint32_t r2, r3, r12, lr, pc;
+};
+
+void debug_monitor_handler_c(struct ex_frame *sp)
+{
+	/* Return to after breakpoint instruction */
+	sp->pc += 2;
+
+	switch (sp->syscall) {
+	case RDI_SYS_OPEN:
+		sp->retval = 1;
+		break;
+	case RDI_SYS_WRITE:
+		sp->retval = rdi_write(sp->params[0], (void*)sp->params[1], sp->params[2]);
+		break;
+	case RDI_SYS_ISTTY:
+		sp->retval = 1;
+		break;
+	default:
+		sp->retval = -1;
+	}
+
+}
+
+asm(".globl debug_monitor_handler\n"
+    ".thumb_func\n"
+    "debug_monitor_handler: \n"
+    "    mov r0, sp\n"
+    "    b debug_monitor_handler_c\n");
+
+#endif
diff --git a/src/platforms/swlink/Makefile.inc b/src/platforms/swlink/Makefile.inc
index 1a979c57..aeee95ac 100644
--- a/src/platforms/swlink/Makefile.inc
+++ b/src/platforms/swlink/Makefile.inc
@@ -8,11 +8,17 @@ CFLAGS += -mcpu=cortex-m3 -mthumb \
 	-I platforms/stm32
 LDFLAGS_BOOT := $(LDFLAGS) --specs=nano.specs \
 	-lopencm3_stm32f1 -Wl,--defsym,_stack=0x20005000 \
-	-Wl,-T,platforms/stm32/stlink.ld -nostartfiles -lc -lnosys \
+	-Wl,-T,platforms/stm32/stlink.ld -nostartfiles -lc\
 	-Wl,-Map=mapfile -mthumb -mcpu=cortex-m3 -Wl,-gc-sections \
 	-L../libopencm3/lib
 LDFLAGS = $(LDFLAGS_BOOT) -Wl,-Ttext=0x8002000
 
+ifeq ($(ENABLE_DEBUG), 1)
+LDFLAGS += --specs=rdimon.specs
+else
+LDFLAGS += --specs=nosys.specs
+endif
+
 VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
diff --git a/src/platforms/swlink/platform.c b/src/platforms/swlink/platform.c
index aace5a12..651ab9b1 100644
--- a/src/platforms/swlink/platform.c
+++ b/src/platforms/swlink/platform.c
@@ -28,6 +28,7 @@
 
 #include <libopencm3/stm32/f1/rcc.h>
 #include <libopencm3/cm3/scb.h>
+#include <libopencm3/cm3/scs.h>
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/stm32/usart.h>
 #include <libopencm3/usb/usbd.h>
@@ -36,6 +37,11 @@
 void platform_init(void)
 {
 	uint32_t data;
+	SCS_DEMCR |= SCS_DEMCR_VC_MON_EN;
+#ifdef ENABLE_DEBUG
+	void initialise_monitor_handles(void);
+	initialise_monitor_handles();
+#endif
 	rcc_clock_setup_in_hse_8mhz_out_72mhz();
 
 	/* Enable peripherals */
@@ -83,6 +89,9 @@ void platform_init(void)
 
 	platform_timing_init();
 	cdcacm_init();
+	/* Don't enable UART if we're being debugged. */
+	if (!(SCS_DEMCR & SCS_DEMCR_TRCENA))
+		usbuart_init();
 	usbuart_init();
 }
 
diff --git a/src/platforms/swlink/platform.h b/src/platforms/swlink/platform.h
index eef60b69..8c2be74f 100644
--- a/src/platforms/swlink/platform.h
+++ b/src/platforms/swlink/platform.h
@@ -106,7 +106,15 @@
 #define TRACE_IC_IN TIM_IC_IN_TI2
 #define TRACE_TRIG_IN TIM_SMCR_TS_IT1FP2
 
-#define DEBUG(...)
+#ifdef ENABLE_DEBUG
+# define PLATFORM_HAS_DEBUG
+# define USBUART_DEBUG
+extern bool debug_bmp;
+int usbuart_debug_write(const char *buf, size_t len);
+# define DEBUG printf
+#else
+# define DEBUG(...)
+#endif
 
 #define SET_RUN_STATE(state)	{running_status = (state);}
 #define SET_IDLE_STATE(state)	{gpio_set_val(LED_PORT, LED_IDLE_RUN, state);}
diff --git a/src/target/cortexa.c b/src/target/cortexa.c
index 9ef239c5..e5626734 100644
--- a/src/target/cortexa.c
+++ b/src/target/cortexa.c
@@ -60,7 +60,6 @@ static uint32_t read_gpreg(target *t, uint8_t regno);
 struct cortexa_priv {
 	uint32_t base;
 	ADIv5_AP_t *apb;
-	ADIv5_AP_t *ahb;
 	struct {
 		uint32_t r[16];
 		uint32_t cpsr;
@@ -217,19 +216,6 @@ static uint32_t va_to_pa(target *t, uint32_t va)
 	return pa;
 }
 
-static void cortexa_mem_read(target *t, void *dest, target_addr src, size_t len)
-{
-	/* Clean cache before reading */
-	for (uint32_t cl = src & ~(CACHE_LINE_LENGTH-1);
-	     cl < src + len; cl += CACHE_LINE_LENGTH) {
-		write_gpreg(t, 0, cl);
-		apb_write(t, DBGITR, MCR | DCCMVAC);
-	}
-
-	ADIv5_AP_t *ahb = ((struct cortexa_priv*)t->priv)->ahb;
-	adiv5_mem_read(ahb, dest, va_to_pa(t, src), len);
-}
-
 static void cortexa_slow_mem_read(target *t, void *dest, target_addr src, size_t len)
 {
 	struct cortexa_priv *priv = t->priv;
@@ -269,18 +255,6 @@ static void cortexa_slow_mem_read(target *t, void *dest, target_addr src, size_t
 	}
 }
 
-static void cortexa_mem_write(target *t, target_addr dest, const void *src, size_t len)
-{
-	/* Clean and invalidate cache before writing */
-	for (uint32_t cl = dest & ~(CACHE_LINE_LENGTH-1);
-	     cl < dest + len; cl += CACHE_LINE_LENGTH) {
-		write_gpreg(t, 0, cl);
-		apb_write(t, DBGITR, MCR | DCCIMVAC);
-	}
-	ADIv5_AP_t *ahb = ((struct cortexa_priv*)t->priv)->ahb;
-	adiv5_mem_write(ahb, va_to_pa(t, dest), src, len);
-}
-
 static void cortexa_slow_mem_write_bytes(target *t, target_addr dest, const uint8_t *src, size_t len)
 {
 	struct cortexa_priv *priv = t->priv;
@@ -338,8 +312,7 @@ static void cortexa_slow_mem_write(target *t, target_addr dest, const void *src,
 static bool cortexa_check_error(target *t)
 {
 	struct cortexa_priv *priv = t->priv;
-	ADIv5_AP_t *ahb = priv->ahb;
-	bool err = (ahb && (adiv5_dp_error(ahb->dp)) != 0) || priv->mmu_fault;
+	bool err = priv->mmu_fault;
 	priv->mmu_fault = false;
 	return err;
 }
@@ -355,25 +328,8 @@ bool cortexa_probe(ADIv5_AP_t *apb, uint32_t debug_base)
 	t->priv = priv;
 	t->priv_free = free;
 	priv->apb = apb;
-	/* FIXME Find a better way to find the AHB.  This is likely to be
-	 * device specific. */
-	priv->ahb = adiv5_new_ap(apb->dp, 0);
-	adiv5_ap_ref(priv->ahb);
-	if (false) {
-		/* FIXME: This used to be if ((priv->ahb->idr & 0xfffe00f) == 0x4770001)
-		 * Accessing memory directly through the AHB is much faster, but can
-		 * result in data inconsistencies if the L2 cache is enabled.
-		 */
-		/* This is an AHB */
-		t->mem_read = cortexa_mem_read;
-		t->mem_write = cortexa_mem_write;
-	} else {
-		/* This is not an AHB, fall back to slow APB access */
-		adiv5_ap_unref(priv->ahb);
-		priv->ahb = NULL;
-		t->mem_read = cortexa_slow_mem_read;
-		t->mem_write = cortexa_slow_mem_write;
-	}
+	t->mem_read = cortexa_slow_mem_read;
+	t->mem_write = cortexa_slow_mem_write;
 
 	priv->base = debug_base;
 	/* Set up APB CSW, we won't touch this again */
@@ -709,11 +665,11 @@ static int cortexa_breakwatch_set(target *t, struct breakwatch *bw)
 		case 2:
 			bw->reserved[0] = target_mem_read16(t, bw->addr);
 			target_mem_write16(t, bw->addr, 0xBE00);
-			return 0;
+			return target_check_error(t);
 		case 4:
 			bw->reserved[0] = target_mem_read32(t, bw->addr);
 			target_mem_write32(t, bw->addr, 0xE1200070);
-			return 0;
+			return target_check_error(t);
 		default:
 			return -1;
 		}
@@ -755,10 +711,10 @@ static int cortexa_breakwatch_clear(target *t, struct breakwatch *bw)
 		switch (bw->size) {
 		case 2:
 			target_mem_write16(t, bw->addr, i);
-			return 0;
+			return target_check_error(t);
 		case 4:
 			target_mem_write32(t, bw->addr, i);
-			return 0;
+			return target_check_error(t);
 		default:
 			return -1;
 		}
diff --git a/src/target/stm32f4.c b/src/target/stm32f4.c
index 477584b5..c6a9a5fa 100644
--- a/src/target/stm32f4.c
+++ b/src/target/stm32f4.c
@@ -40,21 +40,20 @@ static bool stm32f4_cmd_option(target *t, int argc, char *argv[]);
 static bool stm32f4_cmd_psize(target *t, int argc, char *argv[]);
 
 const struct command_s stm32f4_cmd_list[] = {
-	{"erase_mass", (cmd_handler)stm32f4_cmd_erase_mass, "Erase entire flash memory"},
+	{"erase_mass", (cmd_handler)stm32f4_cmd_erase_mass,
+	 "Erase entire flash memory"},
 	{"option", (cmd_handler)stm32f4_cmd_option, "Manipulate option bytes"},
-	{"psize", (cmd_handler)stm32f4_cmd_psize, "Configure flash write parallelism: (x8|x32)"},
+	{"psize", (cmd_handler)stm32f4_cmd_psize,
+	 "Configure flash write parallelism: (x8|x32(default))"},
 	{NULL, NULL, NULL}
 };
 
 
-static int stm32f4_flash_erase(struct target_flash *f, target_addr addr, size_t len);
+static int stm32f4_flash_erase(struct target_flash *f, target_addr addr,
+							   size_t len);
 static int stm32f4_flash_write(struct target_flash *f,
                                target_addr dest, const void *src, size_t len);
 
-static const char stm32f4_driver_str[] = "STM32F4xx";
-static const char stm32f7_driver_str[] = "STM32F7xx";
-static const char stm32f2_driver_str[] = "STM32F2xx";
-
 /* Flash Program ad Erase Controller Register Map */
 #define FPEC_BASE	0x40023C00
 #define FLASH_ACR	(FPEC_BASE+0x00)
@@ -71,6 +70,7 @@ static const char stm32f2_driver_str[] = "STM32F2xx";
 #define FLASH_CR_PSIZE16	(1 << 8)
 #define FLASH_CR_PSIZE32	(2 << 8)
 #define FLASH_CR_PSIZE64	(3 << 8)
+#define FLASH_CR_MER1		(1 << 15)
 #define FLASH_CR_STRT		(1 << 16)
 #define FLASH_CR_EOPIE		(1 << 24)
 #define FLASH_CR_ERRIE		(1 << 25)
@@ -81,6 +81,8 @@ static const char stm32f2_driver_str[] = "STM32F2xx";
 
 #define FLASH_OPTCR_OPTLOCK	(1 << 0)
 #define FLASH_OPTCR_OPTSTRT	(1 << 1)
+#define FLASH_OPTCR_nDBANK	(1 << 29)
+#define FLASH_OPTCR_DB1M	(1 << 30)
 
 #define KEY1 0x45670123
 #define KEY2 0xCDEF89AB
@@ -91,6 +93,9 @@ static const char stm32f2_driver_str[] = "STM32F2xx";
 #define SR_ERROR_MASK	0xF2
 #define SR_EOP		0x01
 
+#define F4_FLASHSIZE	0x1FFF7A22
+#define F7_FLASHSIZE	0x1FF0F442
+#define F72X_FLASHSIZE	0x1FF07A22
 #define DBGMCU_IDCODE	0xE0042000
 #define ARM_CPUID	0xE000ED00
 
@@ -125,6 +130,7 @@ struct stm32f4_flash {
 	struct target_flash f;
 	uint8_t base_sector;
 	uint8_t psize;
+	uint8_t bank_split;
 };
 
 enum ID_STM32F47 {
@@ -146,7 +152,7 @@ enum ID_STM32F47 {
 
 static void stm32f4_add_flash(target *t,
                               uint32_t addr, size_t length, size_t blocksize,
-                              uint8_t base_sector)
+                              unsigned int base_sector, int split)
 {
 	struct stm32f4_flash *sf = calloc(1, sizeof(*sf));
 	struct target_flash *f = &sf->f;
@@ -159,104 +165,155 @@ static void stm32f4_add_flash(target *t,
 	f->erased = 0xff;
 	sf->base_sector = base_sector;
 	sf->psize = 32;
+	sf->bank_split = split;
 	target_add_flash(t, f);
 }
 
 bool stm32f4_probe(target *t)
 {
-	bool f2 = false;
 	uint32_t idcode;
+	const char* designator = NULL;
+	bool dual_bank = false;
+	bool has_ccmram = false;
+	bool is_f7  = false;
+	bool large_sectors = false;
+	uint32_t flashsize_base = F4_FLASHSIZE;
 
 	idcode = target_mem_read32(t, DBGMCU_IDCODE);
 	idcode &= 0xFFF;
 
-	if (idcode == ID_STM32F20X)
-	{
+	if (idcode == ID_STM32F20X) {
 		/* F405 revision A have a wrong IDCODE, use ARM_CPUID to make the
 		 * distinction with F205. Revision is also wrong (0x2000 instead
 		 * of 0x1000). See F40x/F41x errata. */
 		uint32_t cpuid = target_mem_read32(t, ARM_CPUID);
 		if ((cpuid & 0xFFF0) == 0xC240)
 			idcode = ID_STM32F40X;
-		else
-			f2 = true;
 	}
-
 	switch(idcode) {
+	case ID_STM32F40X:
+		designator = "STM32F40x";
+		has_ccmram = true;
+		break;
 	case ID_STM32F42X: /* 427/437 */
+		designator = "STM32F42x";
+		has_ccmram = true;
+		dual_bank  = true;
+		break;
 	case ID_STM32F46X: /* 469/479 */
-		/* Second bank for 2M parts. */
-		stm32f4_add_flash(t, 0x8100000, 0x10000, 0x4000, 12);
-		stm32f4_add_flash(t, 0x8110000, 0x10000, 0x10000, 16);
-		stm32f4_add_flash(t, 0x8120000, 0xE0000, 0x20000, 17);
-		/* Fall through for stuff common to F40x/F41x */
+		designator = "STM32F47x";
+		has_ccmram = true;
+		dual_bank  = true;
+		break;
 	case ID_STM32F20X: /* F205 */
-	case ID_STM32F40X: /* F405 */
-		if (!f2)
-			target_add_ram(t, 0x10000000, 0x10000);
-		/* Fall through for devices w/o CCMRAM */
+		designator = "STM32F2";
+		break;
 	case ID_STM32F446: /* F446 */
+		designator = "STM32F446";
+		break;
 	case ID_STM32F401C: /* F401 B/C RM0368 Rev.3 */
+		designator = "STM32F401C";
+		break;
 	case ID_STM32F411: /* F411     RM0383 Rev.4 */
+		designator = "STM32F411";
+		break;
 	case ID_STM32F412: /* F412     RM0402 Rev.4, 256 kB Ram */
+		designator = "STM32F412";
+		break;
 	case ID_STM32F401E: /* F401 D/E RM0368 Rev.3 */
-		t->driver = f2 ? stm32f2_driver_str : stm32f4_driver_str;
-		target_add_ram(t, 0x20000000, 0x40000);
-		stm32f4_add_flash(t, 0x8000000, 0x10000, 0x4000, 0);
-		stm32f4_add_flash(t, 0x8010000, 0x10000, 0x10000, 4);
-		stm32f4_add_flash(t, 0x8020000, 0xE0000, 0x20000, 5);
-		target_add_commands(t, stm32f4_cmd_list, f2 ? "STM32F2" :
-		                    "STM32F4");
+		designator = "STM32F401E";
 		break;
 	case ID_STM32F413: /* F413     RM0430 Rev.2, 320 kB Ram, 1.5 MB flash. */
-		t->driver = stm32f4_driver_str;
-		target_add_ram(t, 0x20000000, 0x50000);
-		stm32f4_add_flash(t, 0x8000000, 0x10000, 0x4000, 0);
-		stm32f4_add_flash(t, 0x8010000, 0x10000, 0x10000, 4);
-		stm32f4_add_flash(t, 0x8020000, 0x160000, 0x20000, 5);
-		target_add_commands(t, stm32f4_cmd_list, "STM32F413");
+		designator = "STM32F413";
 		break;
 	case ID_STM32F74X: /* F74x RM0385 Rev.4 */
-		t->driver = stm32f7_driver_str;
-		target_add_ram(t, 0x00000000, 0x4000);
-		target_add_ram(t, 0x20000000, 0x50000);
-		/* AXIM Flash access */
-		stm32f4_add_flash(t, 0x8000000, 0x20000, 0x8000, 0);
-		stm32f4_add_flash(t, 0x8020000, 0x20000, 0x20000, 4);
-		stm32f4_add_flash(t, 0x8040000, 0xC0000, 0x40000, 5);
-		/* Flash aliased as ITCM */
-		stm32f4_add_flash(t, 0x0200000, 0x20000, 0x8000, 0);
-		stm32f4_add_flash(t, 0x0220000, 0x20000, 0x20000, 4);
-		stm32f4_add_flash(t, 0x0240000, 0xC0000, 0x40000, 5);
-		target_add_commands(t, stm32f4_cmd_list, "STM32F74x");
+		designator = "STM32F74x";
+		is_f7 = true;
+		large_sectors = true;
+		flashsize_base = F7_FLASHSIZE;
 		break;
 	case ID_STM32F76X: /* F76x F77x RM0410 */
-		t->driver = stm32f7_driver_str;
-		target_add_ram(t, 0x00000000, 0x4000);
-		target_add_ram(t, 0x20000000, 0x80000);
-		/* AXIM Flash access */
-		stm32f4_add_flash(t, 0x8000000, 0x020000, 0x8000, 0);
-		stm32f4_add_flash(t, 0x8020000, 0x020000, 0x20000, 4);
-		stm32f4_add_flash(t, 0x8040000, 0x1C0000, 0x40000, 5);
-		/* Flash aliased as ITCM */
-		stm32f4_add_flash(t, 0x200000, 0x020000, 0x8000, 0);
-		stm32f4_add_flash(t, 0x220000, 0x020000, 0x20000, 4);
-		stm32f4_add_flash(t, 0x240000, 0x1C0000, 0x40000, 5);
-		target_add_commands(t, stm32f4_cmd_list, "STM32F76x");
+		designator = "STM32F76x";
+		is_f7 = true;
+		dual_bank = true;
+		flashsize_base = F7_FLASHSIZE;
 		break;
 	case ID_STM32F72X: /* F72x F73x RM0431 */
-		t->driver = stm32f7_driver_str;
-		target_add_ram(t, 0x00000000, 0x2000);
-		target_add_ram(t, 0x20000000, 0x40000);
-		stm32f4_add_flash(t, 0x8000000, 0x010000, 0x4000,  0);
-		stm32f4_add_flash(t, 0x8010000, 0x010000, 0x10000, 4);
-		stm32f4_add_flash(t, 0x8020000, 0x060000, 0x20000, 3);
-		target_add_commands(t, stm32f4_cmd_list, "STM32F72x");
+		designator = "STM32F72x";
+		is_f7 = true;
+		flashsize_base = F72X_FLASHSIZE;
 		break;
 	default:
 		return false;
 	}
+	target_mem_write32(t, DBGMCU_CR, DBG_STANDBY| DBG_STOP | DBG_SLEEP);
+	t->driver = designator;
+	target_add_commands(t, stm32f4_cmd_list, designator);
 	t->idcode = idcode;
+	bool use_dual_bank = false;
+	uint32_t flashsize = target_mem_read32(t, flashsize_base) & 0xffff;
+	if (is_f7) {
+		target_add_ram(t, 0x00000000, 0x4000);  /* 16 k ITCM Ram */
+		target_add_ram(t, 0x20000000, 0x10000); /* 64 k DTCM Ram */
+		if (dual_bank) {
+			uint32_t optcr;
+			optcr = target_mem_read32(t, FLASH_OPTCR);
+			use_dual_bank =  !(optcr & FLASH_OPTCR_nDBANK);
+		}
+	} else {
+		if (has_ccmram)
+			target_add_ram(t, 0x10000000, 0x10000); /* 64 k CCM Ram*/
+		target_add_ram(t, 0x20000000, 0x10000);     /* 64 k RAM */
+		if (dual_bank) {
+			use_dual_bank = true;
+			if (flashsize < 0x800) {
+				/* Check Dual-bank on 1 Mbyte Flash memory devices*/
+				uint32_t optcr;
+				optcr = target_mem_read32(t, FLASH_OPTCR);
+				use_dual_bank = !(optcr & FLASH_OPTCR_DB1M);
+			}
+		}
+	}
+	int split = 0;
+	uint32_t banksize;
+	if (use_dual_bank) {
+		banksize = flashsize << 9; /* flas split on two sectors. */
+		split = (flashsize == 0x400) ? 8 : 12;
+	}
+	else
+		banksize = flashsize << 10;
+	if (large_sectors) {
+		uint32_t remains = banksize - 0x40000;
+		/* 256 k in small sectors.*/
+		stm32f4_add_flash(t, ITCM_BASE, 0x20000,  0x8000, 0, split);
+		stm32f4_add_flash(t, 0x0220000, 0x20000, 0x20000, 4, split);
+		stm32f4_add_flash(t, 0x0240000, remains, 0x40000, 5, split);
+		stm32f4_add_flash(t, AXIM_BASE, 0x20000,  0x8000, 0, split);
+		stm32f4_add_flash(t, 0x8020000, 0x20000, 0x20000, 4, split);
+		stm32f4_add_flash(t, 0x8040000, remains, 0x40000, 5, split);
+	} else {
+		uint32_t remains = banksize - 0x20000; /* 128 k in small sectors.*/
+		if (is_f7) {
+			stm32f4_add_flash(t, ITCM_BASE, 0x10000,  0x4000,  0, split);
+			stm32f4_add_flash(t, 0x0210000, 0x10000, 0x10000,  4, split);
+			stm32f4_add_flash(t, 0x0220000, remains, 0x20000,  5, split);
+		}
+		stm32f4_add_flash(t, 0x8000000, 0x10000,  0x4000,  0, split);
+		stm32f4_add_flash(t, 0x8010000, 0x10000, 0x10000,  4, split);
+		stm32f4_add_flash(t, 0x8020000, remains, 0x20000,  5, split);
+		if (use_dual_bank) {
+			if (is_f7) {
+				uint32_t bk1 = ITCM_BASE + banksize;
+				stm32f4_add_flash(t, bk1          , 0x10000,  0x4000, 0, split);
+				stm32f4_add_flash(t, bk1 + 0x10000, 0x10000, 0x10000, 4, split);
+				stm32f4_add_flash(t, bk1 + 0x20000, remains, 0x20000, 5, split);
+			}
+			uint32_t bk2 = 0x8000000 + banksize;
+			stm32f4_add_flash(t, bk2          , 0x10000,  0x4000, 16, split);
+			stm32f4_add_flash(t, bk2 + 0x10000, 0x10000, 0x10000, 20, split);
+			stm32f4_add_flash(t, bk2 + 0x20000, remains, 0x20000, 21, split);
+		}
+	}
 	return true;
 }
 
@@ -269,14 +326,14 @@ static void stm32f4_flash_unlock(target *t)
 	}
 }
 
-static int stm32f4_flash_erase(struct target_flash *f, target_addr addr, size_t len)
+static int stm32f4_flash_erase(struct target_flash *f, target_addr addr,
+							   size_t len)
 {
 	target *t = f->t;
-	uint16_t sr;
+	struct stm32f4_flash *sf = (struct stm32f4_flash *)f;
+	uint32_t sr;
 	/* No address translation is needed here, as we erase by sector number */
-	uint8_t sector = ((struct stm32f4_flash *)f)->base_sector +
-	                  (addr - f->start)/f->blocksize;
-
+	uint8_t sector = sf->base_sector + (addr - f->start)/f->blocksize;
 	stm32f4_flash_unlock(t);
 
 	while(len) {
@@ -289,18 +346,22 @@ static int stm32f4_flash_erase(struct target_flash *f, target_addr addr, size_t
 
 		/* Read FLASH_SR to poll for BSY bit */
 		while(target_mem_read32(t, FLASH_SR) & FLASH_SR_BSY)
-			if(target_check_error(t))
+			if(target_check_error(t)) {
+				DEBUG("stm32f4 flash erase: comm error\n");
 				return -1;
-
+			}
 		len -= f->blocksize;
 		sector++;
+		if ((sf->bank_split) && (sector == sf->bank_split))
+			sector = 16;
 	}
 
 	/* Check for error */
 	sr = target_mem_read32(t, FLASH_SR);
-	if(sr & SR_ERROR_MASK)
+	if(sr & SR_ERROR_MASK) {
+		DEBUG("stm32f4 flash erase: sr error: 0x%" PRIu32 "\n", sr);
 		return -1;
-
+	}
 	return 0;
 }
 
@@ -328,13 +389,18 @@ static bool stm32f4_cmd_erase_mass(target *t)
 {
 	const char spinner[] = "|/-\\";
 	int spinindex = 0;
+	struct target_flash *f = t->flash;
+	struct stm32f4_flash *sf = (struct stm32f4_flash *)f;
 
 	tc_printf(t, "Erasing flash... This may take a few seconds.  ");
 	stm32f4_flash_unlock(t);
 
 	/* Flash mass erase start instruction */
-	target_mem_write32(t, FLASH_CR, FLASH_CR_MER);
-	target_mem_write32(t, FLASH_CR, FLASH_CR_STRT | FLASH_CR_MER);
+	uint32_t cr =  FLASH_CR_MER;
+	if (sf->bank_split)
+		cr |=  FLASH_CR_MER1;
+	target_mem_write32(t, FLASH_CR, cr);
+	target_mem_write32(t, FLASH_CR, cr | FLASH_CR_STRT);
 
 	/* Read FLASH_SR to poll for BSY bit */
 	while (target_mem_read32(t, FLASH_SR) & FLASH_SR_BSY) {
@@ -347,7 +413,7 @@ static bool stm32f4_cmd_erase_mass(target *t)
 	tc_printf(t, "\n");
 
 	/* Check for error */
-	uint16_t sr = target_mem_read32(t, FLASH_SR);
+	uint32_t sr = target_mem_read32(t, FLASH_SR);
 	if ((sr & SR_ERROR_MASK) || !(sr & SR_EOP))
 		return false;
 
@@ -529,8 +595,9 @@ static bool stm32f4_cmd_option(target *t, int argc, char *argv[])
 	val[0] |= (target_mem_read32(t, start    ) & 0xffff);
 	if (readcount > 1) {
 		if (start == 0x1FFFC000) /* F4 */ {
-			val[1] = target_mem_read32(t, start +  8 - 0x10000);
+			val[1] = target_mem_read32(t, 0x1ffec008);
 			val[1] &= 0xffff;
+			val[1] <<= 16;
 		} else {
 			val[1] =  (target_mem_read32(t, start + 0x18) & 0xffff) << 16;
 			val[1] |= (target_mem_read32(t, start + 0x10) & 0xffff);
@@ -543,9 +610,9 @@ static bool stm32f4_cmd_option(target *t, int argc, char *argv[])
 	optcr_mask(t, val);
 	tc_printf(t, "OPTCR: 0x%08X ", val[0]);
 	if (readcount > 1)
-		tc_printf(t, "OPTCR1: 0x%08X ", val[1]);
+		tc_printf(t, "OPTCR1: 0x%08lx ", val[1]);
 	if (readcount > 2)
-		tc_printf(t, "OPTCR2: 0x%08X" , val[2]);
+		tc_printf(t, "OPTCR2: 0x%08lx" , val[2]);
 	tc_printf(t, "\n");
 	return true;
 }