diff --git a/src/platforms/pc-stlinkv2/README.md b/src/platforms/pc-stlinkv2/README.md
index ad2f78d8..d70b4d7b 100644
--- a/src/platforms/pc-stlinkv2/README.md
+++ b/src/platforms/pc-stlinkv2/README.md
@@ -1,18 +1,25 @@
-Stlink V2/3 with original STM firmware as Blackmagic Debug Probes
+ST-Link V2/3 with original STM firmware as Blackmagic Debug Probes
 
-Recent STM Stlink firmware revision (V3 and V2 >= J32) expose nearly all
-functionality that BMP needs. This branch implements blackmagic debug probe
-for the STM Stlink as a proof of concept.
+Recent STM ST-LINK  firmware revision (V3 and V2 >= J32) expose all
+functionality that BMP needs. This platform implements blackmagic debug
+probe for the STM ST-LINK.
 Use at your own risk, but report or better fix problems.
 
-Run the resulting blackmagic_stlinkv2 executable to start the gdb server
+Compile with "make PROBE_HOST=pc-stlinkv2"
 
-Crosscompling for windows with mingw succeeds.
+Run the resulting blackmagic_stlinkv2 executable to start the gdb server.
+
+You can also use on the command line alone, e.g
+- "blackmagic_stlinkv2 -t" to scan and display the results of the scan
+- "blackmagic_stlinkv2 <file.bin>" to flash <file.bin> at 0x08000000
+- "blackmagic_stlinkv2 -h" for more options
+
+Cross-compling for windows with mingw succeeds.
 
 Drawback:
 - JTAG does not work for chains with multiple devices.
-- STLinkV3 does only work on STM32 devices.
+- ST-LINKV3 seem to only work on STM32 devices.
+- St-LINKV3 needs connect under reset on more devices than V2
 
-This branch may get forced push. In case of problems:
-- git reset --hard master
-- git rebase
+ToDo:
+- Implement an SWO server
\ No newline at end of file
diff --git a/src/platforms/pc-stlinkv2/stlinkv2.c b/src/platforms/pc-stlinkv2/stlinkv2.c
index 7793ab44..ab855ca0 100644
--- a/src/platforms/pc-stlinkv2/stlinkv2.c
+++ b/src/platforms/pc-stlinkv2/stlinkv2.c
@@ -229,6 +229,8 @@ typedef struct {
 
 stlink Stlink;
 
+static int stlink_usb_get_rw_status(bool verbose);
+
 static void exit_function(void)
 {
 	libusb_exit(NULL);
@@ -556,13 +558,14 @@ static int read_retry(uint8_t *txbuf, size_t txsize,
 	int res;
 	while(1) {
 		send_recv(txbuf, txsize, rxbuf, rxsize);
-		res = stlink_usb_get_rw_status();
+		res = stlink_usb_get_rw_status(false);
 		if (res == STLINK_ERROR_OK)
 			return res;
 		gettimeofday(&now, NULL);
 		timersub(&now, &start, &diff);
 		if ((diff.tv_sec >= 1) || (res != STLINK_ERROR_WAIT)) {
 			DEBUG("read_retry failed. ");
+			stlink_usb_get_rw_status(true);
 			return res;
 		}
 	}
@@ -580,12 +583,13 @@ static int write_retry(uint8_t *cmdbuf, size_t cmdsize,
 	while(1) {
 		send_recv(cmdbuf, cmdsize, NULL, 0);
 		send_recv(txbuf, txsize, NULL, 0);
-		res = stlink_usb_get_rw_status();
+		res = stlink_usb_get_rw_status(false);
 		if (res == STLINK_ERROR_OK)
 			return res;
 		gettimeofday(&now, NULL);
 		timersub(&now, &start, &diff);
 		if ((diff.tv_sec >= 1) || (res != STLINK_ERROR_WAIT)) {
+			stlink_usb_get_rw_status(true);
 			return res;
 		}
 	}
@@ -981,7 +985,7 @@ int stlink_enter_debug_swd(void)
 					  STLINK_DEBUG_ENTER_SWD_NO_RESET};
 	uint8_t data[2];
 	DEBUG("Enter SWD\n");
-	send_recv(cmd, 16, data, 2);
+	send_recv_retry(cmd, 16, data, 2);
 	return stlink_usb_error_check(data, true);
 }
 
@@ -1178,7 +1182,7 @@ void adiv5_ap_cleanup(int ap)
        DEBUG_STLINK("Close AP %d\n", ap);
        stlink_usb_error_check(data, true);
 }
-int stlink_usb_get_rw_status(void)
+static int stlink_usb_get_rw_status(bool verbose)
 {
 	uint8_t cmd[16] = {
 		STLINK_DEBUG_COMMAND,
@@ -1186,7 +1190,7 @@ int stlink_usb_get_rw_status(void)
 	};
 	uint8_t data[12];
 	send_recv(cmd, 16, data, 12);
-	return stlink_usb_error_check(data, true);
+	return stlink_usb_error_check(data, verbose);
 }
 
 void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
@@ -1263,7 +1267,7 @@ void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 			length & 0xff, length >> 8, ap->apsel};
 		send_recv(cmd, 16, NULL, 0);
 		send_recv((void*)buffer, length, NULL, 0);
-		stlink_usb_get_rw_status();
+		stlink_usb_get_rw_status(true);
 		len -= length;
 		addr += length;
 	}
@@ -1286,7 +1290,7 @@ void stlink_writemem16(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 		len & 0xff, len >> 8, ap->apsel};
 	send_recv(cmd, 16, NULL, 0);
 	send_recv((void*)buffer, len, NULL, 0);
-	stlink_usb_get_rw_status();
+	stlink_usb_get_rw_status(true);
 }
 
 void stlink_writemem32(ADIv5_AP_t *ap, uint32_t addr, size_t len,
diff --git a/src/platforms/pc-stlinkv2/stlinkv2.h b/src/platforms/pc-stlinkv2/stlinkv2.h
index 04b8fd79..dea9a8b7 100644
--- a/src/platforms/pc-stlinkv2/stlinkv2.h
+++ b/src/platforms/pc-stlinkv2/stlinkv2.h
@@ -43,7 +43,6 @@ uint32_t stlink_dp_error(ADIv5_DP_t *dp);
 void stlink_dp_abort(ADIv5_DP_t *dp, uint32_t abort);
 int stlink_open_ap(uint8_t ap);
 void stlink_close_ap(uint8_t ap);
-int stlink_usb_get_rw_status(void);
 void stlink_regs_read(ADIv5_AP_t *ap, void *data);
 uint32_t stlink_reg_read(ADIv5_AP_t *ap, int idx);
 void stlink_reg_write(ADIv5_AP_t *ap, int num, uint32_t val);
diff --git a/src/platforms/stlink/README.md b/src/platforms/stlink/README.md
index b0ce5d09..7ca359a2 100644
--- a/src/platforms/stlink/README.md
+++ b/src/platforms/stlink/README.md
@@ -1,32 +1,30 @@
 # Blackmagic for ST -Link Adapters
 
-For STlinkV3 and StlinkV2/1, as found on all Nucleo and recent Discovery
+For ST-LINK V3 and ST-LINKV 2/1, as found on all Nucleo and recent Discovery
 boards, use the pc-stlinkv2 branch, running on the PC and with original,
 recent ST firmware.
 
-Only if you have a Stlinkv2 with STM32F103C8 versus the STM32F103CB on V2/1
+Only if you have a ST-LINK V22 with STM32F103C8 versus the STM32F103CB on V2/1
 and you want to rewire and use the UART, consider reflashing the the Stlink
 firmware.
 
-On StlinkV2, the original ST Bootloader can also be used with
+On ST-LINK V2, the original ST Bootloader can also be used with
 
 - Compile firmware with "make PROBE_HOST=stlink ST_BOOTLOADER=1"
 
 - Upload firmware with stlink-tool from [stlink-tool](https://github.com/jeanthom/stlink-tool.git).
   Before upload, replug the stlink to enter the bootloader.
 
-- After each stlink replug, use call "stlink-tool" without arguments
-  to enter BMP
-
-Drawback: After each USB replug, DFU needs to be left explicit!
-On Linux, add someting like :
+- After each stlink replug, call "stlink-tool" without arguments
+  to enter BMP or on Linux use some udev rule like
 
 `> cat /etc/udev/rules.d/98-stlink.rules`
 
  `SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3748", ACTION=="add", RUN+="<path-to>/stlink-tool"`
 
-for automatic switch to BMP on replug. However this defeats reflashing further
-BMP reflash as long as this rule is active.
+- To enter Bootloader again either replug or use "dfu-util -e".
+  With more than one DFU device connected, you need to specify
+  the needed device
 
 ## Versions
 
@@ -75,7 +73,7 @@ the 2 jumper shortening the 4-pin connector like this:
 
 
 ## BMP version detection and handling
-All stlink variants
+All ST-LINK variants
 PC13/14 open -> Standalone ST-LINKV2 or baite, some STM32 Disco w/o accessible
 UART RX/TX
 
@@ -83,8 +81,13 @@ PC13 low -> SWIM internal connection
 
 PC13/PC14 both low -> ST-LinkV2 on some F4_Diso boards.
 
-## Reflashing BMP back to ST-LINKv2 original firmware
+## Reflashing BMP back to ST-LINK V2 original firmware
 
 If you built the firmware to use ST-Link v2's bootloader (with `ST_BOOTLOADER=1`), you should be able to reflash back to original firmware using the [STLinkUpgrade utility](https://www.st.com/en/development-tools/stsw-link007.html). Do not check the "Change type" checkbox unless you know what you are doing, as it will change the USB VID:PID and change how it operates.
 
-Replug STLink before flashing if the utility doesn't detect it.
+Replug ST-LINK before flashing if the utility doesn't detect it.
+##ST-LINK V2.1 Force Bootloader entry
+On ST-LINK V2/2-1 boards with the original bootloader, you can force
+bootloader entry with asserting NRST of the STM32F103CB of the USB
+powered board. Serveral attempts may be needed.
+https://www.carminenoviello.com/2016/02/26/restore-st-link-interface-bad-update-2-26-15-firmware/
diff --git a/src/platforms/stlink/stlink_common.c b/src/platforms/stlink/stlink_common.c
index bb63ca29..4b67e727 100644
--- a/src/platforms/stlink/stlink_common.c
+++ b/src/platforms/stlink/stlink_common.c
@@ -93,7 +93,9 @@ uint32_t detect_rev(void)
 
 void platform_request_boot(void)
 {
-#if defined(DST_BOOTLOADER)
+#if defined(ST_BOOTLOADER)
+	/* Disconnect USB cable by resetting USB Device*/
+	rcc_periph_reset_pulse(RST_USB);
 	scb_reset_system();
 #else
 	uint32_t crl = GPIOA_CRL;
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index a0e5ff39..38f38e4e 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -216,7 +216,7 @@ static const struct {
 
 extern bool cortexa_probe(ADIv5_AP_t *apb, uint32_t debug_base);
 
-void adiv5_dp_ref(ADIv5_DP_t *dp)
+static void adiv5_dp_ref(ADIv5_DP_t *dp)
 {
 	dp->refcnt++;
 }
@@ -226,7 +226,7 @@ void adiv5_ap_ref(ADIv5_AP_t *ap)
 	ap->refcnt++;
 }
 
-void adiv5_dp_unref(ADIv5_DP_t *dp)
+static void adiv5_dp_unref(ADIv5_DP_t *dp)
 {
 	if (--(dp->refcnt) == 0)
 		free(dp);
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index d05f6144..65f47197 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -186,9 +186,7 @@ void adiv5_dp_init(ADIv5_DP_t *dp);
 void adiv5_dp_write(ADIv5_DP_t *dp, uint16_t addr, uint32_t value);
 
 ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel);
-void adiv5_dp_ref(ADIv5_DP_t *dp);
 void adiv5_ap_ref(ADIv5_AP_t *ap);
-void adiv5_dp_unref(ADIv5_DP_t *dp);
 void adiv5_ap_unref(ADIv5_AP_t *ap);
 
 void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index 95473ef5..b81b4d1d 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -53,11 +53,12 @@ const struct command_s cortexm_cmd_list[] = {
 static void cortexm_regs_read(target *t, void *data);
 static void cortexm_regs_write(target *t, const void *data);
 static uint32_t cortexm_pc_read(target *t);
-ssize_t cortexm_reg_read(target *t, int reg, void *data, size_t max);
-ssize_t cortexm_reg_write(target *t, int reg, const void *data, size_t max);
+static ssize_t cortexm_reg_read(target *t, int reg, void *data, size_t max);
+static ssize_t cortexm_reg_write(target *t, int reg, const void *data, size_t max);
 
 static void cortexm_reset(target *t);
 static enum target_halt_reason cortexm_halt_poll(target *t, target_addr *watch);
+static void cortexm_halt_resume(target *t, bool step);
 static void cortexm_halt_request(target *t);
 static int cortexm_fault_unwind(target *t);
 
@@ -549,7 +550,7 @@ int cortexm_mem_write_sized(
 	return target_check_error(t);
 }
 
-int dcrsr_regnum(target *t, unsigned reg)
+static int dcrsr_regnum(target *t, unsigned reg)
 {
 	if (reg < sizeof(regnum_cortex_m) / 4) {
 		return regnum_cortex_m[reg];
@@ -561,7 +562,7 @@ int dcrsr_regnum(target *t, unsigned reg)
 		return -1;
 	}
 }
-ssize_t cortexm_reg_read(target *t, int reg, void *data, size_t max)
+static ssize_t cortexm_reg_read(target *t, int reg, void *data, size_t max)
 {
 	if (max < 4)
 		return -1;
@@ -571,7 +572,7 @@ ssize_t cortexm_reg_read(target *t, int reg, void *data, size_t max)
 	return 4;
 }
 
-ssize_t cortexm_reg_write(target *t, int reg, const void *data, size_t max)
+static ssize_t cortexm_reg_write(target *t, int reg, const void *data, size_t max)
 {
 	if (max < 4)
 		return -1;
@@ -598,36 +599,43 @@ static void cortexm_pc_write(target *t, const uint32_t val)
  * using the core debug registers in the NVIC. */
 static void cortexm_reset(target *t)
 {
+	/* Read DHCSR here to clear S_RESET_ST bit before reset */
+	target_mem_read32(t, CORTEXM_DHCSR);
+	platform_timeout to;
 	if ((t->target_options & CORTEXM_TOPT_INHIBIT_SRST) == 0) {
 		platform_srst_set_val(true);
 		platform_srst_set_val(false);
+		/* Some NRF52840 users saw invalid SWD transaction with
+		 * native/firmware without this delay.*/
+		platform_delay(10);
+	}
+	uint32_t dhcsr = target_mem_read32(t, CORTEXM_DHCSR);
+	if ((dhcsr & CORTEXM_DHCSR_S_RESET_ST) == 0) {
+		/* No reset seen yet, maybe as SRST is not connected, or device has
+         * CORTEXM_TOPT_INHIBIT_SRST set.
+		 * Trigger reset by AIRCR.*/
+		target_mem_write32(t, CORTEXM_AIRCR,
+						   CORTEXM_AIRCR_VECTKEY | CORTEXM_AIRCR_SYSRESETREQ);
 	}
-
-	/* Read DHCSR here to clear S_RESET_ST bit before reset */
-	target_mem_read32(t, CORTEXM_DHCSR);
-
-	/* Request system reset from NVIC: SRST doesn't work correctly */
-	/* This could be VECTRESET: 0x05FA0001 (reset only core)
-	 *          or SYSRESETREQ: 0x05FA0004 (system reset)
-	 */
-	target_mem_write32(t, CORTEXM_AIRCR,
-	                   CORTEXM_AIRCR_VECTKEY | CORTEXM_AIRCR_SYSRESETREQ);
-
 	/* If target needs to do something extra (see Atmel SAM4L for example) */
 	if (t->extended_reset != NULL) {
 		t->extended_reset(t);
 	}
-
-	/* Poll for release from reset */
-	while (target_mem_read32(t, CORTEXM_DHCSR) & CORTEXM_DHCSR_S_RESET_ST);
-
+	/* Wait for CORTEXM_DHCSR_S_RESET_ST to read 0, meaning reset released.*/
+	platform_timeout_set(&to, 1000);
+	while ((target_mem_read32(t, CORTEXM_DHCSR) & CORTEXM_DHCSR_S_RESET_ST) &&
+		   !platform_timeout_is_expired(&to));
+#if defined(PLATFORM_HAS_DEBUG)
+	if (platform_timeout_is_expired(&to))
+		DEBUG("Reset seem to be stuck low!\n");
+#endif
+	/* 10 ms delay to ensure that things such as the STM32 HSI clock
+	 * have started up fully. */
+	platform_delay(10);
 	/* Reset DFSR flags */
 	target_mem_write32(t, CORTEXM_DFSR, CORTEXM_DFSR_RESETALL);
-
-	/* 1ms delay to ensure that things such as the stm32f1 HSI clock have started
-	 * up fully.
-	 */
-	platform_delay(1);
+	/* Make sure we ignore any initial DAP error */
+	target_check_error(t);
 }
 
 static void cortexm_halt_request(target *t)
@@ -706,7 +714,7 @@ static enum target_halt_reason cortexm_halt_poll(target *t, target_addr *watch)
 	return TARGET_HALT_BREAKPOINT;
 }
 
-void cortexm_halt_resume(target *t, bool step)
+static void cortexm_halt_resume(target *t, bool step)
 {
 	struct cortexm_priv *priv = t->priv;
 	uint32_t dhcsr = CORTEXM_DHCSR_DBGKEY | CORTEXM_DHCSR_C_DEBUGEN;
diff --git a/src/target/cortexm.h b/src/target/cortexm.h
index 927d9d66..01077620 100644
--- a/src/target/cortexm.h
+++ b/src/target/cortexm.h
@@ -175,7 +175,6 @@ ADIv5_AP_t *cortexm_ap(target *t);
 
 bool cortexm_attach(target *t);
 void cortexm_detach(target *t);
-void cortexm_halt_resume(target *t, bool step);
 int cortexm_run_stub(target *t, uint32_t loadaddr,
                      uint32_t r0, uint32_t r1, uint32_t r2, uint32_t r3);
 int cortexm_mem_write_sized(
diff --git a/src/target/lmi.c b/src/target/lmi.c
index 54cd19ce..14ae3495 100644
--- a/src/target/lmi.c
+++ b/src/target/lmi.c
@@ -104,7 +104,7 @@ bool lmi_probe(target *t)
 	return false;
 }
 
-int lmi_flash_erase(struct target_flash *f, target_addr addr, size_t len)
+static int lmi_flash_erase(struct target_flash *f, target_addr addr, size_t len)
 {
 	target  *t = f->t;
 
@@ -128,7 +128,7 @@ int lmi_flash_erase(struct target_flash *f, target_addr addr, size_t len)
 	return 0;
 }
 
-int lmi_flash_write(struct target_flash *f,
+static int lmi_flash_write(struct target_flash *f,
                     target_addr dest, const void *src, size_t len)
 {
 	target  *t = f->t;
diff --git a/src/target/samd.c b/src/target/samd.c
index 3507d020..2a5c7e03 100644
--- a/src/target/samd.c
+++ b/src/target/samd.c
@@ -329,7 +329,7 @@ samd20_revB_detach(target *t)
 static void
 samd20_revB_halt_resume(target *t, bool step)
 {
-	cortexm_halt_resume(t, step);
+	target_halt_resume(t, step);
 
 	/* ---- Additional ---- */
 	/* Exit extended reset */
diff --git a/src/target/stm32f4.c b/src/target/stm32f4.c
index 4f874f63..68b37cdc 100644
--- a/src/target/stm32f4.c
+++ b/src/target/stm32f4.c
@@ -518,7 +518,7 @@ static bool stm32f4_cmd_erase_mass(target *t, int argc, const char **argv)
  * * Documentation for F413 with OPTCR default = 0ffffffed seems wrong!
  */
 
-bool optcr_mask(target *t, uint32_t *val)
+static bool optcr_mask(target *t, uint32_t *val)
 {
 	switch (t->idcode) {
 	case ID_STM32F20X: