From b8b34e7b1dc6bf6125e9ccda4cad304dbb86ec68 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Wed, 29 Apr 2020 16:07:16 +0200
Subject: [PATCH 01/43] adiv5: remove cfg for AP structure, cfg is only used
 local.

---
 src/target/adiv5.c | 6 ++++--
 src/target/adiv5.h | 1 -
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 7cccffa..034b48a 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -422,7 +422,6 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 	memcpy(ap, &tmpap, sizeof(*ap));
 	adiv5_dp_ref(dp);
 
-	ap->cfg = adiv5_ap_read(ap, ADIV5_AP_CFG);
 	ap->base = adiv5_ap_read(ap, ADIV5_AP_BASE);
 	ap->csw = adiv5_ap_read(ap, ADIV5_AP_CSW) &
 		~(ADIV5_AP_CSW_SIZE_MASK | ADIV5_AP_CSW_ADDRINC_MASK);
@@ -432,8 +431,11 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 		ap->csw &= ~ADIV5_AP_CSW_TRINPROG;
 	}
 
+#if defined(ENABLE_DEBUG)
+	uint32_t cfg = adiv5_ap_read(ap, ADIV5_AP_CFG);
 	DEBUG("AP %3d: IDR=%08"PRIx32" CFG=%08"PRIx32" BASE=%08"PRIx32" CSW=%08"PRIx32"\n",
-	      apsel, ap->idr, ap->cfg, ap->base, ap->csw);
+	      apsel, ap->idr, cfg, ap->base, ap->csw);
+#endif
 	return ap;
 }
 
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index 60d8933..79b084e 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -177,7 +177,6 @@ typedef struct ADIv5_AP_s {
 	uint8_t apsel;
 
 	uint32_t idr;
-	uint32_t cfg;
 	uint32_t base;
 	uint32_t csw;
 } ADIv5_AP_t;

From 16967b43288028dcdc2759bb2a25a53472571162 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Wed, 29 Apr 2020 16:13:26 +0200
Subject: [PATCH 02/43] adiv5: Remove only local dp_idcode used from ADIv5_DP_t
 struct.

---
 src/target/adiv5.c | 4 ++--
 src/target/adiv5.h | 1 -
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 034b48a..778cd0c 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -492,8 +492,8 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		);
 	DEBUG("RESET_SEQ %s\n", (platform_timeout_is_expired(&timeout)) ? "failed": "succeeded");
 
-	dp->dp_idcode =  adiv5_dp_read(dp, ADIV5_DP_IDCODE);
-	if ((dp->dp_idcode & ADIV5_DP_VERSION_MASK) == ADIV5_DPv2) {
+	uint32_t dp_idcode = adiv5_dp_read(dp, ADIV5_DP_IDCODE);
+	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);
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index 79b084e..c72a7a4 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -134,7 +134,6 @@ typedef struct ADIv5_DP_s {
 	int refcnt;
 
 	uint32_t idcode;
-	uint32_t dp_idcode; /* Contains DPvX revision*/
 	uint32_t targetid;  /* Contains IDCODE for DPv2 devices.*/
 
 	uint32_t (*dp_read)(struct ADIv5_DP_s *dp, uint16_t addr);

From 05adcd9bf5b36d099627aff6b73e463030ee417f Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Wed, 15 Apr 2020 17:36:26 +0200
Subject: [PATCH 03/43] remote.c: Compile only relevant functions.

Do no compile firmware functions when compiling pc-hosted.
---
 src/Makefile                           |  8 +++++++-
 src/command.c                          | 12 ++++++------
 src/gdb_packet.c                       |  2 +-
 src/include/gdb_if.h                   |  2 +-
 src/include/platform_support.h         |  2 +-
 src/main.c                             |  2 +-
 src/morse.c                            |  2 +-
 src/platforms/libftdi/Makefile.inc     |  3 ++-
 src/platforms/pc-hosted/Makefile.inc   |  3 ++-
 src/platforms/pc-stlinkv2/Makefile.inc |  3 ++-
 src/platforms/stm32/usbuart.c          |  2 +-
 src/remote.c                           |  2 ++
 src/target/cortexm.c                   |  6 +++---
 src/target/target.c                    |  2 +-
 14 files changed, 31 insertions(+), 20 deletions(-)

diff --git a/src/Makefile b/src/Makefile
index 0072715..f95721c 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -45,6 +45,7 @@ SRC =			\
 	nrf51.c		\
 	nxpke04.c	\
 	platform.c	\
+	remote.c	\
 	sam3x.c		\
 	sam4l.c		\
 	samd.c		\
@@ -72,11 +73,16 @@ endif
 
 ifndef OWN_HL
 SRC += jtag_scan.c jtagtap.c swdptap.c
-SRC += remote.c
 else
 CFLAGS += -DOWN_HL
 endif
 
+ifdef PC_HOSTED
+CFLAGS += -DPC_HOSTED=1
+else
+CFLAGS += -DPC_HOSTED=0
+endif
+
 OBJ = $(patsubst %.S,%.o,$(patsubst %.c,%.o,$(SRC)))
 
 $(TARGET): include/version.h $(OBJ)
diff --git a/src/command.c b/src/command.c
index 1059e33..372b8e9 100644
--- a/src/command.c
+++ b/src/command.c
@@ -60,7 +60,7 @@ static bool cmd_target_power(target *t, int argc, const char **argv);
 static bool cmd_traceswo(target *t, int argc, const char **argv);
 #endif
 static bool cmd_heapinfo(target *t, int argc, const char **argv);
-#if defined(PLATFORM_HAS_DEBUG) && !defined(PC_HOSTED)
+#if defined(PLATFORM_HAS_DEBUG) && (PC_HOSTED == 0)
 static bool cmd_debug_bmp(target *t, int argc, const char **argv);
 #endif
 
@@ -85,14 +85,14 @@ const struct command_s cmd_list[] = {
 #endif
 #endif
 	{"heapinfo", (cmd_handler)cmd_heapinfo, "Set semihosting heapinfo" },
-#if defined(PLATFORM_HAS_DEBUG) && !defined(PC_HOSTED)
+#if defined(PLATFORM_HAS_DEBUG) && (PC_HOSTED == 0)
 	{"debug_bmp", (cmd_handler)cmd_debug_bmp, "Output BMP \"debug\" strings to the second vcom: (enable|disable)"},
 #endif
 	{NULL, NULL, NULL}
 };
 
 bool connect_assert_srst;
-#if defined(PLATFORM_HAS_DEBUG) && !defined(PC_HOSTED)
+#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. */
@@ -135,7 +135,7 @@ bool cmd_version(target *t, int argc, char **argv)
 	(void)t;
 	(void)argc;
 	(void)argv;
-#if defined PC_HOSTED
+#if PC_HOSTED == 1
 	gdb_outf("Black Magic Probe, PC-Hosted for " PLATFORM_IDENT
 			 ", Version " FIRMWARE_VERSION "\n");
 #else
@@ -389,7 +389,7 @@ static bool cmd_traceswo(target *t, int argc, const char **argv)
 			}
 		}
 	}
-#if defined(PLATFORM_HAS_DEBUG) && !defined(PC_HOSTED) && defined(ENABLE_DEBUG)
+#if defined(PLATFORM_HAS_DEBUG) && (PC_HOSTED == 0) && defined(ENABLE_DEBUG)
 	if (debug_bmp) {
 #if TRACESWO_PROTOCOL == 2
 		gdb_outf("baudrate: %lu ", baudrate);
@@ -412,7 +412,7 @@ static bool cmd_traceswo(target *t, int argc, const char **argv)
 }
 #endif
 
-#if defined(PLATFORM_HAS_DEBUG) && !defined(PC_HOSTED)
+#if defined(PLATFORM_HAS_DEBUG) && (PC_HOSTED == 0)
 static bool cmd_debug_bmp(target *t, int argc, const char **argv)
 {
 	(void)t;
diff --git a/src/gdb_packet.c b/src/gdb_packet.c
index f0f1f1d..6a01c89 100644
--- a/src/gdb_packet.c
+++ b/src/gdb_packet.c
@@ -47,7 +47,7 @@ int gdb_getpacket(char *packet, int size)
 				packet[0] = gdb_if_getchar();
 				if (packet[0]==0x04) return 1;
 			} while ((packet[0] != '$') && (packet[0] != REMOTE_SOM));
-#ifndef OWN_HL
+#if PC_HOSTED == 0
 			if (packet[0]==REMOTE_SOM) {
 				/* This is probably a remote control packet
 				 * - get and handle it */
diff --git a/src/include/gdb_if.h b/src/include/gdb_if.h
index 1dc9e28..eed1b9a 100644
--- a/src/include/gdb_if.h
+++ b/src/include/gdb_if.h
@@ -21,7 +21,7 @@
 #ifndef __GDB_IF_H
 #define __GDB_IF_H
 
-#if !defined(NO_LIBOPENCM3)
+#if PC_HOSTED == 0
 #include <libopencm3/usb/usbd.h>
 void gdb_usb_out_cb(usbd_device *dev, uint8_t ep);
 #endif
diff --git a/src/include/platform_support.h b/src/include/platform_support.h
index 36bd7ae..a111354 100644
--- a/src/include/platform_support.h
+++ b/src/include/platform_support.h
@@ -24,7 +24,7 @@
 #	error "Include 'general.h' instead"
 #endif
 
-#if defined(PC_HOSTED)
+#if PC_HOSTED == 1
 void platform_init(int argc, char **argv);
 #else
 void platform_init(void);
diff --git a/src/main.c b/src/main.c
index 26e8ffd..8895954 100644
--- a/src/main.c
+++ b/src/main.c
@@ -33,7 +33,7 @@
 int
 main(int argc, char **argv)
 {
-#if defined(PC_HOSTED)
+#if PC_HOSTED == 1
 	platform_init(argc, argv);
 #else
 	(void) argc;
diff --git a/src/morse.c b/src/morse.c
index 55db0ba..9ba4814 100644
--- a/src/morse.c
+++ b/src/morse.c
@@ -59,7 +59,7 @@ static char morse_repeat;
 
 void morse(const char *msg, char repeat)
 {
-#if defined(PC_HOSTED)
+#if PC_HOSTED == 1
 	if (msg)
 		DEBUG("%s\n", msg);
 	(void) repeat;
diff --git a/src/platforms/libftdi/Makefile.inc b/src/platforms/libftdi/Makefile.inc
index 12c4696..fab3b06 100644
--- a/src/platforms/libftdi/Makefile.inc
+++ b/src/platforms/libftdi/Makefile.inc
@@ -1,5 +1,5 @@
 SYS = $(shell $(CC) -dumpmachine)
-CFLAGS += -DPC_HOSTED -DNO_LIBOPENCM3 -DENABLE_DEBUG
+CFLAGS += -DENABLE_DEBUG
 CFLAGS += $(shell pkg-config --cflags libftdi1)
 LDFLAGS += $(shell pkg-config --libs libftdi1)
 ifneq (, $(findstring mingw, $(SYS)))
@@ -10,3 +10,4 @@ endif
 VPATH += platforms/pc
 SRC += 	timing.c cl_utils.c utils.c
 CFLAGS +=-I ./target -I./platforms/pc
+PC_HOSTED = 1
\ No newline at end of file
diff --git a/src/platforms/pc-hosted/Makefile.inc b/src/platforms/pc-hosted/Makefile.inc
index 9493b0c..3d10dc1 100644
--- a/src/platforms/pc-hosted/Makefile.inc
+++ b/src/platforms/pc-hosted/Makefile.inc
@@ -1,6 +1,6 @@
 TARGET=blackmagic_hosted
 SYS = $(shell $(CC) -dumpmachine)
-CFLAGS += -DPC_HOSTED -DNO_LIBOPENCM3 -DENABLE_DEBUG
+CFLAGS += -DENABLE_DEBUG
 CFLAGS +=-I ./target -I./platforms/pc
 ifneq (, $(findstring mingw, $(SYS)))
 SRC += 	 serial_win.c
@@ -13,3 +13,4 @@ SRC += serial_unix.c
 endif
 VPATH += platforms/pc
 SRC += 	cl_utils.c timing.c utils.c
+PC_HOSTED = 1
diff --git a/src/platforms/pc-stlinkv2/Makefile.inc b/src/platforms/pc-stlinkv2/Makefile.inc
index 299a815..70f6f8e 100644
--- a/src/platforms/pc-stlinkv2/Makefile.inc
+++ b/src/platforms/pc-stlinkv2/Makefile.inc
@@ -1,6 +1,6 @@
 TARGET=blackmagic_stlinkv2
 SYS = $(shell $(CC) -dumpmachine)
-CFLAGS += -DPC_HOSTED -DNO_LIBOPENCM3 -DSTLINKV2 -DJTAG_HL -DENABLE_DEBUG
+CFLAGS += -DSTLINKV2 -DJTAG_HL -DENABLE_DEBUG
 CFLAGS +=-I ./target -I./platforms/pc
 LDFLAGS += -lusb-1.0
 ifneq (, $(findstring mingw, $(SYS)))
@@ -11,3 +11,4 @@ endif
 VPATH += platforms/pc
 SRC += 	timing.c stlinkv2.c cl_utils.c utils.c
 OWN_HL = 1
+PC_HOSTED = 1
diff --git a/src/platforms/stm32/usbuart.c b/src/platforms/stm32/usbuart.c
index f13ff10..d2dc215 100644
--- a/src/platforms/stm32/usbuart.c
+++ b/src/platforms/stm32/usbuart.c
@@ -263,7 +263,7 @@ enum {
 int rdi_write(int fn, const char *buf, size_t len)
 {
 	(void)fn;
-#if defined(PLATFORM_HAS_DEBUG) && !defined(PC_HOSTED)
+#if defined(PLATFORM_HAS_DEBUG)
 	if (debug_bmp)
 		return len - usbuart_debug_write(buf, len);
 #else
diff --git a/src/remote.c b/src/remote.c
index dc44ff7..354e1b6 100644
--- a/src/remote.c
+++ b/src/remote.c
@@ -56,6 +56,7 @@ uint64_t remotehston(uint32_t limit, char *s)
 	return ret;
 }
 
+#if PC_HOSTED == 0
 static void _respond(char respCode, uint64_t param)
 
 /* Send response to far end */
@@ -274,3 +275,4 @@ void remotePacketProcess(uint8_t i, char *packet)
 		break;
     }
 }
+#endif
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index 6f27379..051befd 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -38,7 +38,7 @@
 
 #include <unistd.h>
 
-#ifdef PC_HOSTED
+#if PC_HOSTED == 1
 
 /*
  * pc-hosted semihosting does keyboard, file and screen i/o on the system
@@ -1067,7 +1067,7 @@ static bool cortexm_vector_catch(target *t, int argc, char *argv[])
 #define SYS_WRITEC	0x03
 #define SYS_WRITE0	0x04
 
-#if !defined(PC_HOSTED)
+#if PC_HOSTED == 0
 /* probe memory access functions */
 static void probe_mem_read(target *t __attribute__((unused)), void *probe_dest, target_addr target_src, size_t len)
 {
@@ -1104,7 +1104,7 @@ static int cortexm_hostio_request(target *t)
 	DEBUG("syscall 0"PRIx32"%"PRIx32" (%"PRIx32" %"PRIx32" %"PRIx32" %"PRIx32")\n",
               syscall, params[0], params[1], params[2], params[3]);
 	switch (syscall) {
-#if defined(PC_HOSTED)
+#if PC_HOSTED == 1
 
 	/* code that runs in pc-hosted process. use linux system calls. */
 
diff --git a/src/target/target.c b/src/target/target.c
index 7171da8..520c629 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -337,7 +337,7 @@ void target_detach(target *t)
 {
 	t->detach(t);
 	t->attached = false;
-#if defined(PC_HOSTED)
+#if PC_HOSTED == 1
 # include "platform.h"
 	platform_buffer_flush();
 #endif

From 815e09bb1b0c76923aa91a383d21edb8c1c65879 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 18 Apr 2020 16:18:55 +0200
Subject: [PATCH 04/43] cl-utils: Add more arguments

---
 src/platforms/pc/cl_utils.c | 15 ++++++++++++---
 src/platforms/pc/cl_utils.h |  2 ++
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index f65c24c..ece6e6b 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -120,12 +120,13 @@ static void cl_help(char **argv, BMP_CL_OPTIONS_t *opt)
 	printf("\t-h\t\t: This help.\n");
 	printf("\t-v[1|2]\t\t: Increasing verbosity\n");
 	printf("\t-d \"path\"\t: Use serial device at \"path\"\n");
+	printf("\t-P <num>\t: Use device found as <num>");
 	printf("\t-s \"string\"\t: Use dongle with (partial) "
 		  "serial number \"string\"\n");
 	printf("\t-c \"string\"\t: Use ftdi dongle with type \"string\"\n");
-	printf("\t-C\t\t: Connect under reset\n");
 	printf("\t-n\t\t: Exit immediate if no device found\n");
 	printf("\tRun mode related options:\n");
+	printf("\t-C\t\t: Connect under reset\n");
 	printf("\t-t\t\t: Scan SWD, with no target found scan jtag and exit\n");
 	printf("\t-E\t\t: Erase flash until flash end or for given size\n");
 	printf("\t-V\t\t: Verify flash against binary file\n");
@@ -149,7 +150,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 	opt->opt_target_dev = 1;
 	opt->opt_flash_start = 0x08000000;
 	opt->opt_flash_size = 16 * 1024 *1024;
-	while((c = getopt(argc, argv, "Ehv::d:s:c:CnN:tVta:S:jprR")) != -1) {
+	while((c = getopt(argc, argv, "Ehv::d:s:I:c:CnN:tVta:S:jpP:rR")) != -1) {
 		switch(c) {
 		case 'c':
 			if (optarg)
@@ -181,6 +182,10 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 			if (optarg)
 				opt->opt_serial = optarg;
 			break;
+		case 'I':
+			if (optarg)
+				opt->opt_ident_string = optarg;
+			break;
 		case 'E':
 			opt->opt_mode = BMP_MODE_FLASH_ERASE;
 			break;
@@ -207,6 +212,10 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 			if (optarg)
 				opt->opt_target_dev = strtol(optarg, NULL, 0);
 			break;
+		case 'P':
+			if (optarg)
+				opt->opt_position = atoi(optarg);
+			break;
 		case 'S':
 			if (optarg) {
 				char *endptr;
@@ -398,7 +407,7 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 		}
 		if (read_file != -1)
 			close(read_file);
-		printf("Read/Verifed succeeded for %d bytes\n", bytes_read);
+		printf("Read/Verified succeeded for %d bytes\n", bytes_read);
 	}
   free_map:
 	if (map.size)
diff --git a/src/platforms/pc/cl_utils.h b/src/platforms/pc/cl_utils.h
index 6e89656..f25bd3f 100644
--- a/src/platforms/pc/cl_utils.h
+++ b/src/platforms/pc/cl_utils.h
@@ -43,6 +43,8 @@ typedef struct BMP_CL_OPTIONS_s {
 	char *opt_flash_file;
 	char *opt_device;
 	char *opt_serial;
+	char *opt_ident_string;
+	int  opt_position;
 	char *opt_cable;
 	int opt_debuglevel;
 	int opt_target_dev;

From c5d0902d4ce98b0c18bd3d57e030e9c4f934ae1b Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Wed, 22 Apr 2020 18:18:22 +0200
Subject: [PATCH 05/43] cl_utils: Fix memory leak.

---
 src/platforms/pc/cl_utils.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index ece6e6b..2a73c64 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -354,7 +354,7 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 		target_reset(t);
 	} else {
 #define WORKSIZE 1024
-		uint8_t *data = malloc(WORKSIZE);
+		uint8_t *data = alloca(WORKSIZE);
 		if (!data) {
 			printf("Can not malloc memory for flash read/verify operation\n");
 			return res;

From 8db979798c28fca6b6abbc79886c1550609caf54 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 25 Apr 2020 14:02:33 +0200
Subject: [PATCH 06/43] cl_utils.c: Report read/write speed.

---
 src/platforms/pc/cl_utils.c | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index 2a73c64..114fd9e 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -332,6 +332,7 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 	} else if (opt->opt_mode == BMP_MODE_FLASH_WRITE) {
 		DEBUG("Erase    %zu bytes at 0x%08" PRIx32 "\n", map.size,
 			  opt->opt_flash_start);
+		uint32_t start_time = platform_time_ms();
 		unsigned int erased = target_flash_erase(t, opt->opt_flash_start,
 												 map.size);
 		if (erased) {
@@ -352,6 +353,9 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 		}
 		target_flash_done(t);
 		target_reset(t);
+		uint32_t end_time = platform_time_ms();
+		printf("Flash Write succeeded for %d bytes, %8.3f kiB/s\n",
+			   (int)map.size, (((map.size * 1.0)/(end_time - start_time))));
 	} else {
 #define WORKSIZE 1024
 		uint8_t *data = alloca(WORKSIZE);
@@ -368,6 +372,7 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 			map.size;
 		int bytes_read = 0;
 		void *flash = map.data;
+		uint32_t start_time = platform_time_ms();
 		while (size) {
 			int worksize = (size > WORKSIZE) ? WORKSIZE : size;
 			int n_read = target_mem_read(t, data, flash_src, worksize);
@@ -405,9 +410,11 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 			if (size <= 0)
 				res = 0;
 		}
+		uint32_t end_time = platform_time_ms();
 		if (read_file != -1)
 			close(read_file);
-		printf("Read/Verified succeeded for %d bytes\n", bytes_read);
+		printf("Read/Verified succeeded for %d bytes, %8.3f kiB/s\n",
+			   bytes_read, (((bytes_read * 1.0)/(end_time - start_time))));
 	}
   free_map:
 	if (map.size)

From b5182e09d015029f8fce83a0e787355540a406c1 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 25 Apr 2020 13:15:31 +0200
Subject: [PATCH 07/43] cl_utils: When reading from flash to file, truncate the
 file.

---
 src/platforms/pc/cl_utils.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index 114fd9e..57af8d3 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -306,7 +306,7 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 		}
 	} else if (opt->opt_mode == BMP_MODE_FLASH_READ) {
 		/* Open as binary */
-		read_file = open(opt->opt_flash_file, O_CREAT | O_RDWR | O_BINARY,
+		read_file = open(opt->opt_flash_file, O_TRUNC | O_CREAT | O_RDWR | O_BINARY,
 						 S_IRUSR | S_IWUSR);
 		if (read_file == -1) {
 			printf("Error opening flashfile %s for read: %s\n",

From 7956fbc3612e81b21d330e702a9e86401d1b6099 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Wed, 29 Apr 2020 15:24:54 +0200
Subject: [PATCH 08/43] cl-utils: Print memory map when test is specified.

---
 src/platforms/pc/cl_utils.c | 40 +++++++++++++++++++++++++++++++++++--
 1 file changed, 38 insertions(+), 2 deletions(-)

diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index 57af8d3..45f6373 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -285,8 +285,6 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 	} else {
 		target_foreach(display_target, NULL);
 	}
-	if (opt->opt_mode == BMP_MODE_TEST)
-			return 0;
 	if (opt->opt_target_dev > num_targets) {
 		DEBUG("Given target nummer %d not available\n", opt->opt_target_dev);
 		return res;
@@ -296,6 +294,44 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 		DEBUG("Can not attach to target %d\n", opt->opt_target_dev);
 		goto target_detach;
 	}
+	if (opt->opt_mode == BMP_MODE_TEST) {
+		char map [1024], *p = map;
+		if (target_mem_map(t, map, sizeof(map))) {
+			while (*p && (*p == '<')) {
+				unsigned int start, size;
+				char *res;
+				int match;
+				match = strncmp(p, "<memory-map>", strlen("<memory-map>"));
+				if (!match) {
+					p  += strlen("<memory-map>");
+					continue;
+				}
+				match = strncmp(p, "<memory type=\"flash\" ", strlen("<memory type=\"flash\" "));
+				if (!match) {
+					unsigned int blocksize;
+					if (sscanf(p, "<memory type=\"flash\" start=\"%x\" length=\"%x\">"
+							  "<property name=\"blocksize\">%x</property></memory>",
+							  &start, &size, &blocksize))
+						printf("Flash Start: 0x%08x, length %#9x, blocksize %#8x\n",
+							   start, size, blocksize);
+					res = strstr(p, "</memory>");
+					p = res + strlen("</memory>");
+					continue;
+				}
+				match = strncmp(p, "<memory type=\"ram\" ", strlen("<memory type=\"ram\" "));
+				if (!match) {
+					if (sscanf(p, "<memory type=\"ram\" start=\"%x\" length=\"%x\"/",
+							   &start, &size))
+						printf("Ram   Start: 0x%08x, length %#9x\n", start, size);
+					res = strstr(p, "/>");
+					p = res + strlen("/>");
+					continue;
+				}
+				break;
+			}
+		}
+		goto target_detach;
+	}
 	int read_file = -1;
 	if ((opt->opt_mode == BMP_MODE_FLASH_WRITE) ||
 		(opt->opt_mode == BMP_MODE_FLASH_VERIFY)) {

From 60f39f55b48ced7af35c15abcf6861e2b3e5ba31 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sun, 26 Apr 2020 18:12:20 +0200
Subject: [PATCH 09/43] MSP432: Warn when hardware version not supported.

---
 src/target/msp432.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/target/msp432.c b/src/target/msp432.c
index 79cf241..4c29fd2 100644
--- a/src/target/msp432.c
+++ b/src/target/msp432.c
@@ -184,8 +184,10 @@ bool msp432_probe(target *t)
 		return false;
 
 	/* Check for the right HW revision: at least C, as no flash support for B */
-	if (target_mem_read32(t, HWREV_ADDR) < HWREV_MIN_VALUE)
+	if (target_mem_read32(t, HWREV_ADDR) < HWREV_MIN_VALUE) {
+		DEBUG("MSP432 Version not handled\n");
 		return false;
+	}
 
 	/* If we got till this point, we are most probably looking at a real TLV  */
 	/* Device Information structure. Now check for the correct device         */

From 966ac4036d67630b4c03c413df7355f420db8e39 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 25 Apr 2020 13:44:41 +0200
Subject: [PATCH 10/43] target.c: Check for valid flash structure.

---
 src/target/target.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/target/target.c b/src/target/target.c
index 520c629..765a401 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -256,6 +256,8 @@ int target_flash_write(target *t,
 	int ret = 0;
 	while (len) {
 		struct target_flash *f = flash_for_addr(t, dest);
+		if (!f)
+			return 1;
 		size_t tmptarget = MIN(dest + len, f->start + f->length);
 		size_t tmplen = tmptarget - dest;
 		ret |= target_flash_write_buffered(f, dest, src, tmplen);

From f3790b90e5ecf44525dc05f05a4e72f74baea411 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Fri, 17 Apr 2020 17:43:09 +0200
Subject: [PATCH 11/43] command.c: Simplify usage of serial_no serial number
 string.

---
 src/command.c | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/src/command.c b/src/command.c
index 372b8e9..12a5e4a 100644
--- a/src/command.c
+++ b/src/command.c
@@ -357,11 +357,7 @@ static bool cmd_target_power(target *t, int argc, const char **argv)
 #ifdef PLATFORM_HAS_TRACESWO
 static bool cmd_traceswo(target *t, int argc, const char **argv)
 {
-#if defined(STM32L0) || defined(STM32F3) || defined(STM32F4)
-	extern char serial_no[13];
-#else
-	extern char serial_no[9];
-#endif
+	extern char *serial_no;
 	(void)t;
 #if TRACESWO_PROTOCOL == 2
 	uint32_t baudrate = SWO_DEFAULT_BAUD;

From eee16cf08acb678450c60099fe1c9f905c3da533 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Fri, 17 Apr 2020 17:11:01 +0200
Subject: [PATCH 12/43] platforms/tm4c/jtagtap.c: Remove unreferenced and
 incomplete file.

---
 src/platforms/tm4c/jtagtap.c | 43 ------------------------------------
 1 file changed, 43 deletions(-)
 delete mode 100644 src/platforms/tm4c/jtagtap.c

diff --git a/src/platforms/tm4c/jtagtap.c b/src/platforms/tm4c/jtagtap.c
deleted file mode 100644
index 056a4fd..0000000
--- a/src/platforms/tm4c/jtagtap.c
+++ /dev/null
@@ -1,43 +0,0 @@
-#include "general.h"
-#include "jtagtap.h"
-
-int
-jtagtap_init(void)
-{
-	TMS_SET_MODE();
-
-	for(int i = 0; i <= 50; i++) jtagtap_next(1,0);
-	jtagtap_tms_seq(0xE73C, 16);
-	jtagtap_soft_reset();
-
-	return 0;
-}
-
-void
-jtagtap_reset(void)
-{
-#ifdef TRST_PORT
-	volatile int i;
-	gpio_clear(TRST_PORT, TRST_PIN);
-	for(i = 0; i < 10000; i++) asm("nop");
-	gpio_set(TRST_PORT, TRST_PIN);
-#endif
-	jtagtap_soft_reset();
-}
-
-uint8_t
-jtagtap_next(const uint8_t dTMS, const uint8_t dTDI)
-{
-	uint16_t ret;
-
-	gpio_set_val(TMS_PORT, TMS_PIN, dTMS);
-	gpio_set_val(TDI_PORT, TDI_PIN, dTDI);
-	gpio_set(TCK_PORT, TCK_PIN);
-	ret = gpio_get(TDO_PORT, TDO_PIN);
-	gpio_clear(TCK_PORT, TCK_PIN);
-
-	DEBUG("jtagtap_next(TMS = %d, TDI = %d) = %d\n", dTMS, dTDI, ret);
-
-	return ret != 0;
-}
-

From 783ec377d9c2a55e437d687ab786c889c6fe6ab6 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Fri, 24 Apr 2020 15:26:48 +0200
Subject: [PATCH 13/43] adiv5: Export extract.

---
 src/target/adiv5.c | 2 +-
 src/target/adiv5.h | 1 +
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 778cd0c..8c82a31 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -586,7 +586,7 @@ static void ap_mem_access_setup(ADIv5_AP_t *ap, uint32_t addr, enum align align)
 }
 
 /* Extract read data from data lane based on align and src address */
-static void * extract(void *dest, uint32_t src, uint32_t val, enum align align)
+void * extract(void *dest, uint32_t src, uint32_t val, enum align align)
 {
 	switch (align) {
 	case ALIGN_BYTE:
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index c72a7a4..1ae53bd 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -197,4 +197,5 @@ void adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
 void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
 						   size_t len, enum align align);
 uint64_t adiv5_ap_read_pidr(ADIv5_AP_t *ap, uint32_t addr);
+void * extract(void *dest, uint32_t src, uint32_t val, enum align align);
 #endif

From b0cf7d24bd4378044a5757fc276fa20969707196 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 25 Apr 2020 14:36:06 +0200
Subject: [PATCH 14/43] adiv5.c: Fix another leak.

---
 src/target/adiv5.c | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 8c82a31..5f0de60 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -317,9 +317,11 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 #endif
 
 		for (int i = 0; i < 960; i++) {
+			adiv5_dp_error(ap->dp);
 			uint32_t entry = adiv5_mem_read32(ap, addr + i*4);
 			if (adiv5_dp_error(ap->dp)) {
-				DEBUG("%sFault reading ROM table entry\n", indent);
+				DEBUG("%sFault reading ROM table entry %d\n", indent, i);
+				break;
 			}
 
 			if (entry == 0)
@@ -518,6 +520,7 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		if (ap->base == last_base) {
 			DEBUG("AP %d: Duplicate base\n", i);
 			adiv5_ap_cleanup(i);
+			free(ap);
 			/* FIXME: Should we expect valid APs behind duplicate ones? */
 			return;
 		}

From 1e10b96b03da71b1a101c108ca9511c8a613666e Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Wed, 15 Apr 2020 18:59:13 +0200
Subject: [PATCH 15/43] Allow multiple scan routines.

---
 src/command.c                            | 10 +++++++-
 src/include/target.h                     |  7 ++++++
 src/platforms/libftdi/platform.c         | 11 +++++++++
 src/platforms/pc-hosted/platform.c       | 11 +++++++++
 src/platforms/pc-stlinkv2/adiv5_jtagdp.c |  2 +-
 src/platforms/pc-stlinkv2/platform.c     | 30 ++++++++++++++++++++++++
 src/platforms/pc-stlinkv2/platform.h     |  2 +-
 src/platforms/pc/cl_utils.c              |  4 ++--
 8 files changed, 72 insertions(+), 5 deletions(-)

diff --git a/src/command.c b/src/command.c
index 12a5e4a..4d0d1e5 100644
--- a/src/command.c
+++ b/src/command.c
@@ -186,7 +186,11 @@ static bool cmd_jtag_scan(target *t, int argc, char **argv)
 	int devs = -1;
 	volatile struct exception e;
 	TRY_CATCH (e, EXCEPTION_ALL) {
+#if PC_HOSTED == 1
+		devs = platform_jtag_scan(argc > 1 ? irlens : NULL);
+#else
 		devs = jtag_scan(argc > 1 ? irlens : NULL);
+#endif
 	}
 	switch (e.type) {
 	case EXCEPTION_TIMEOUT:
@@ -220,8 +224,12 @@ bool cmd_swdp_scan(target *t, int argc, char **argv)
 	int devs = -1;
 	volatile struct exception e;
 	TRY_CATCH (e, EXCEPTION_ALL) {
+#if PC_HOSTED == 1
+		devs = platform_adiv5_swdp_scan();
+#else
 		devs = adiv5_swdp_scan();
-	}
+#endif
+		}
 	switch (e.type) {
 	case EXCEPTION_TIMEOUT:
 		gdb_outf("Timeout during scan. Is target stuck in WFI?\n");
diff --git a/src/include/target.h b/src/include/target.h
index cc6131e..b4b840c 100644
--- a/src/include/target.h
+++ b/src/include/target.h
@@ -34,6 +34,13 @@ typedef struct target_s target;
 typedef uint32_t target_addr;
 struct target_controller;
 
+#if defined(PC_HOSTED)
+int platform_adiv5_swdp_scan(void);
+int platform_jtag_scan(const uint8_t *lrlens);
+#endif
+int adiv5_swdp_scan(void);
+int jtag_scan(const uint8_t *lrlens);
+
 int adiv5_swdp_scan(void);
 int jtag_scan(const uint8_t *lrlens);
 
diff --git a/src/platforms/libftdi/platform.c b/src/platforms/libftdi/platform.c
index 8fad4cc..88ed4f2 100644
--- a/src/platforms/libftdi/platform.c
+++ b/src/platforms/libftdi/platform.c
@@ -21,6 +21,7 @@
 #include "gdb_if.h"
 #include "version.h"
 #include "platform.h"
+#include "target.h"
 
 #include <assert.h>
 #include <unistd.h>
@@ -180,6 +181,16 @@ cable_desc_t cable_desc[] = {
 	},
 };
 
+int platform_adiv5_swdp_scan(void)
+{
+	return adiv5_swdp_scan();
+}
+
+int platform_jtag_scan(const uint8_t *lrlens)
+{
+	return jtag_scan(lrlens);
+}
+
 void platform_init(int argc, char **argv)
 {
 	BMP_CL_OPTIONS_t cl_opts = {0};
diff --git a/src/platforms/pc-hosted/platform.c b/src/platforms/pc-hosted/platform.c
index 361574d..82f7996 100644
--- a/src/platforms/pc-hosted/platform.c
+++ b/src/platforms/pc-hosted/platform.c
@@ -23,6 +23,7 @@
 #include "version.h"
 #include "platform.h"
 #include "remote.h"
+#include "target.h"
 
 #include <assert.h>
 #include <sys/time.h>
@@ -33,6 +34,16 @@
 #include "cl_utils.h"
 static BMP_CL_OPTIONS_t cl_opts; /* Portable way to nullify the struct*/
 
+int platform_adiv5_swdp_scan(void)
+{
+	return adiv5_swdp_scan();
+}
+
+int platform_jtag_scan(const uint8_t *lrlens)
+{
+	return jtag_scan(lrlens);
+}
+
 void platform_init(int argc, char **argv)
 {
   cl_opts.opt_idstring = "Blackmagic Debug Probe Remote";
diff --git a/src/platforms/pc-stlinkv2/adiv5_jtagdp.c b/src/platforms/pc-stlinkv2/adiv5_jtagdp.c
index 4c228fd..30ff697 100644
--- a/src/platforms/pc-stlinkv2/adiv5_jtagdp.c
+++ b/src/platforms/pc-stlinkv2/adiv5_jtagdp.c
@@ -31,7 +31,7 @@
 struct jtag_dev_s jtag_devs[JTAG_MAX_DEVS+1];
 int jtag_dev_count;
 
-int jtag_scan(const uint8_t *irlens)
+int jtag_scan_stlinkv2(const uint8_t *irlens)
 {
 	uint32_t idcodes[JTAG_MAX_DEVS+1];
 	(void) *irlens;
diff --git a/src/platforms/pc-stlinkv2/platform.c b/src/platforms/pc-stlinkv2/platform.c
index e3f0a1f..f594af1 100644
--- a/src/platforms/pc-stlinkv2/platform.c
+++ b/src/platforms/pc-stlinkv2/platform.c
@@ -20,6 +20,8 @@
 #include "gdb_if.h"
 #include "version.h"
 #include "platform.h"
+#include "target.h"
+#include "target_internal.h"
 
 #include <assert.h>
 #include <unistd.h>
@@ -39,6 +41,34 @@ const char *platform_target_voltage(void)
 	return stlink_target_voltage();
 }
 
+static int adiv5_swdp_scan_stlinkv2(void)
+{
+	target_list_free();
+	ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
+	if (stlink_enter_debug_swd())
+		return 0;
+	dp->idcode = stlink_read_coreid();
+	dp->dp_read = stlink_dp_read;
+	dp->error = stlink_dp_error;
+	dp->low_access = stlink_dp_low_access;
+	dp->abort = stlink_dp_abort;
+
+	stlink_dp_error(dp);
+	adiv5_dp_init(dp);
+
+	return target_list?1:0;
+	return 0;
+}
+int platform_adiv5_swdp_scan(void)
+{
+	return adiv5_swdp_scan_stlinkv2();
+}
+
+int platform_jtag_scan(const uint8_t *lrlens)
+{
+	return jtag_scan_stlinkv2(lrlens);
+}
+
 void platform_init(int argc, char **argv)
 {
 	stlink_init(argc, argv);
diff --git a/src/platforms/pc-stlinkv2/platform.h b/src/platforms/pc-stlinkv2/platform.h
index 7db112d..c3f0bba 100644
--- a/src/platforms/pc-stlinkv2/platform.h
+++ b/src/platforms/pc-stlinkv2/platform.h
@@ -44,5 +44,5 @@ void stlink_check_detach(int state);
 void platform_buffer_flush(void);
 int platform_buffer_write(const uint8_t *data, int size);
 int platform_buffer_read(uint8_t *data, int size);
-
+int jtag_scan_stlinkv2(const uint8_t *irlens);
 #endif
diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index 45f6373..d68dc78 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -275,9 +275,9 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 		printf("Running in Test Mode\n");
 	printf("Target voltage: %s Volt\n", platform_target_voltage());
 	if (opt->opt_usejtag) {
-		num_targets = jtag_scan(NULL);
+		num_targets = platform_jtag_scan(NULL);
 	} else {
-		num_targets = adiv5_swdp_scan();
+		num_targets = platform_adiv5_swdp_scan();
 	}
 	if (!num_targets) {
 		DEBUG("No target found\n");

From e34a27f72c9689850e97df74c499538d7f18518d Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Thu, 16 Apr 2020 17:27:26 +0200
Subject: [PATCH 16/43] Detour swd primitives.

---
 src/Makefile                               | 11 ++--
 src/include/swdptap.h                      | 25 ++++-----
 src/include/target.h                       |  5 +-
 src/{platforms/common => include}/timing.h |  1 -
 src/platforms/common/swdptap.c             | 61 ++++++----------------
 src/platforms/libftdi/swdptap.c            | 49 ++++++-----------
 src/platforms/pc-hosted/swdptap.c          | 24 ++++++---
 src/platforms/pc-stlinkv2/adiv5_swdp.c     | 49 -----------------
 src/platforms/pc-stlinkv2/jtagtap.c        |  0
 src/platforms/pc-stlinkv2/platform.c       |  8 +++
 src/platforms/pc-stlinkv2/swdptap.c        |  0
 src/remote.c                               |  8 +--
 src/target/adiv5_swdp.c                    | 34 ++++++------
 src/{platforms/common => }/timing.c        |  0
 14 files changed, 100 insertions(+), 175 deletions(-)
 rename src/{platforms/common => include}/timing.h (99%)
 delete mode 100644 src/platforms/pc-stlinkv2/adiv5_swdp.c
 create mode 100644 src/platforms/pc-stlinkv2/jtagtap.c
 create mode 100644 src/platforms/pc-stlinkv2/swdptap.c
 rename src/{platforms/common => }/timing.c (100%)

diff --git a/src/Makefile b/src/Makefile
index f95721c..0d3cdf7 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -1,6 +1,6 @@
 PROBE_HOST ?= native
 PLATFORM_DIR = platforms/$(PROBE_HOST)
-VPATH += $(PLATFORM_DIR) platforms/common target
+VPATH += $(PLATFORM_DIR) target
 ENABLE_DEBUG ?=
 
 ifneq ($(V), 1)
@@ -9,8 +9,8 @@ Q := @
 endif
 
 CFLAGS += -Wall -Wextra -Werror -Wno-char-subscripts \
-	-std=gnu99 -g3 -MD \
-	-I. -Iinclude -Iplatforms/common -I$(PLATFORM_DIR)
+	-std=gnu99 -g3 -MD -I./target \
+	-I. -Iinclude -I$(PLATFORM_DIR)
 
 ifeq ($(ENABLE_DEBUG), 1)
 CFLAGS += -DENABLE_DEBUG
@@ -55,6 +55,7 @@ SRC =			\
 	stm32h7.c	\
 	stm32l0.c	\
 	stm32l4.c	\
+	swdptap.c	\
 	target.c	\
 
 include $(PLATFORM_DIR)/Makefile.inc
@@ -72,7 +73,7 @@ SRC += jtagtap_generic.c swdptap_generic.c
 endif
 
 ifndef OWN_HL
-SRC += jtag_scan.c jtagtap.c swdptap.c
+SRC += jtag_scan.c jtagtap.c
 else
 CFLAGS += -DOWN_HL
 endif
@@ -81,6 +82,8 @@ ifdef PC_HOSTED
 CFLAGS += -DPC_HOSTED=1
 else
 CFLAGS += -DPC_HOSTED=0
+VPATH += platforms/common
+CFLAGS += -Iplatforms/common
 endif
 
 OBJ = $(patsubst %.S,%.o,$(patsubst %.c,%.o,$(SRC)))
diff --git a/src/include/swdptap.h b/src/include/swdptap.h
index cdb6969..87cfe16 100644
--- a/src/include/swdptap.h
+++ b/src/include/swdptap.h
@@ -20,20 +20,17 @@
 
 #ifndef __SWDPTAP_H
 #define __SWDPTAP_H
+typedef struct swd_proc_s {
+	uint32_t (*swdptap_seq_in)(int ticks);
+	bool (*swdptap_seq_in_parity)(uint32_t *data, int ticks);
+	void (*swdptap_seq_out)(uint32_t MS, int ticks);
+	void (*swdptap_seq_out_parity)(uint32_t MS, int ticks);
+} swd_proc_t;
+extern swd_proc_t swd_proc;
 
+# if PC_HOSTED == 1
+int platform_swdptap_init(void);
+# else
 int swdptap_init(void);
-
-/* Primitive functions */
-bool swdptap_bit_in(void);
-void swdptap_bit_out(bool val);
-
-/* Low level functions, provided in swdptap_generic.c from the primitives
-   (indicate NO_OWN_LL in the Makefile.inc or libopencm specific in 
-   platforms/common*/
-uint32_t swdptap_seq_in(int ticks);
-bool swdptap_seq_in_parity(uint32_t *data, int ticks);
-void swdptap_seq_out(uint32_t MS, int ticks);
-void swdptap_seq_out_parity(uint32_t MS, int ticks);
-
+# endif
 #endif
-
diff --git a/src/include/target.h b/src/include/target.h
index b4b840c..106ac27 100644
--- a/src/include/target.h
+++ b/src/include/target.h
@@ -34,16 +34,13 @@ typedef struct target_s target;
 typedef uint32_t target_addr;
 struct target_controller;
 
-#if defined(PC_HOSTED)
+#if PC_HOSTED == 1
 int platform_adiv5_swdp_scan(void);
 int platform_jtag_scan(const uint8_t *lrlens);
 #endif
 int adiv5_swdp_scan(void);
 int jtag_scan(const uint8_t *lrlens);
 
-int adiv5_swdp_scan(void);
-int jtag_scan(const uint8_t *lrlens);
-
 bool target_foreach(void (*cb)(int i, target *t, void *context), void *context);
 void target_list_free(void);
 
diff --git a/src/platforms/common/timing.h b/src/include/timing.h
similarity index 99%
rename from src/platforms/common/timing.h
rename to src/include/timing.h
index cc7971e..4a55f18 100644
--- a/src/platforms/common/timing.h
+++ b/src/include/timing.h
@@ -27,4 +27,3 @@ struct platform_timeout {
 uint32_t platform_time_ms(void);
 
 #endif /* __TIMING_H */
-
diff --git a/src/platforms/common/swdptap.c b/src/platforms/common/swdptap.c
index 9d9638d..95cc68d 100644
--- a/src/platforms/common/swdptap.c
+++ b/src/platforms/common/swdptap.c
@@ -28,11 +28,6 @@ enum {
 	SWDIO_STATUS_DRIVE
 };
 
-int swdptap_init(void)
-{
-	return 0;
-}
-
 static void swdptap_turnaround(int dir)
 {
 	static int olddir = SWDIO_STATUS_FLOAT;
@@ -54,26 +49,7 @@ static void swdptap_turnaround(int dir)
 		SWDIO_MODE_DRIVE();
 }
 
-bool swdptap_bit_in(void)
-{
-	uint16_t ret;
-
-	swdptap_turnaround(SWDIO_STATUS_FLOAT);
-
-	ret = gpio_get(SWDIO_PORT, SWDIO_PIN);
-	gpio_set(SWCLK_PORT, SWCLK_PIN);
-	gpio_set(SWCLK_PORT, SWCLK_PIN);
-	gpio_clear(SWCLK_PORT, SWCLK_PIN);
-
-#ifdef DEBUG_SWD_BITS
-	DEBUG("%d", ret?1:0);
-#endif
-
-	return ret != 0;
-}
-
-uint32_t
-swdptap_seq_in(int ticks)
+static uint32_t swdptap_seq_in(int ticks)
 {
 	uint32_t index = 1;
 	uint32_t ret = 0;
@@ -97,8 +73,7 @@ swdptap_seq_in(int ticks)
 	return ret;
 }
 
-bool
-swdptap_seq_in_parity(uint32_t *ret, int ticks)
+static bool swdptap_seq_in_parity(uint32_t *ret, int ticks)
 {
 	uint32_t index = 1;
 	uint8_t parity = 0;
@@ -130,22 +105,7 @@ swdptap_seq_in_parity(uint32_t *ret, int ticks)
 	return parity;
 }
 
-void swdptap_bit_out(bool val)
-{
-#ifdef DEBUG_SWD_BITS
-	DEBUG("%d", val);
-#endif
-
-	swdptap_turnaround(SWDIO_STATUS_DRIVE);
-
-	gpio_set_val(SWDIO_PORT, SWDIO_PIN, val);
-	gpio_clear(SWCLK_PORT, SWCLK_PIN);
-	gpio_set(SWCLK_PORT, SWCLK_PIN);
-	gpio_set(SWCLK_PORT, SWCLK_PIN);
-	gpio_clear(SWCLK_PORT, SWCLK_PIN);
-}
-void
-swdptap_seq_out(uint32_t MS, int ticks)
+static void swdptap_seq_out(uint32_t MS, int ticks)
 {
 	int data = MS & 1;
 #ifdef DEBUG_SWD_BITS
@@ -163,8 +123,7 @@ swdptap_seq_out(uint32_t MS, int ticks)
 	}
 }
 
-void
-swdptap_seq_out_parity(uint32_t MS, int ticks)
+static void swdptap_seq_out_parity(uint32_t MS, int ticks)
 {
 	uint8_t parity = 0;
 	int data = MS & 1;
@@ -188,3 +147,15 @@ swdptap_seq_out_parity(uint32_t MS, int ticks)
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
 }
+
+swd_proc_t swd_proc;
+
+int swdptap_init(void)
+{
+	swd_proc.swdptap_seq_in  = swdptap_seq_in;
+	swd_proc.swdptap_seq_in_parity  = swdptap_seq_in_parity;
+	swd_proc.swdptap_seq_out = swdptap_seq_out;
+	swd_proc.swdptap_seq_out_parity  = swdptap_seq_out_parity;
+
+	return 0;
+}
diff --git a/src/platforms/libftdi/swdptap.c b/src/platforms/libftdi/swdptap.c
index a5c898c..d710d45 100644
--- a/src/platforms/libftdi/swdptap.c
+++ b/src/platforms/libftdi/swdptap.c
@@ -35,7 +35,14 @@ static uint8_t olddir = 0;
 #define MPSSE_TMS_SHIFT (MPSSE_WRITE_TMS | MPSSE_LSB |\
 						 MPSSE_BITMODE | MPSSE_WRITE_NEG)
 
-int swdptap_init(void)
+static bool swdptap_seq_in_parity(uint32_t *res, int ticks);
+static uint32_t swdptap_seq_in(int ticks);
+static void swdptap_seq_out(uint32_t MS, int ticks);
+static void swdptap_seq_out_parity(uint32_t MS, int ticks);
+
+swd_proc_t swd_proc;
+
+int platform_swdptap_init(void)
 {
 	if (!active_cable->bitbang_tms_in_pin) {
 		DEBUG("SWD not possible or missing item in cable description.\n");
@@ -70,6 +77,11 @@ int swdptap_init(void)
 	platform_buffer_write(ftdi_init, 9);
 	platform_buffer_flush();
 
+	swd_proc.swdptap_seq_in  = swdptap_seq_in;
+	swd_proc.swdptap_seq_in_parity  = swdptap_seq_in_parity;
+	swd_proc.swdptap_seq_out = swdptap_seq_out;
+	swd_proc.swdptap_seq_out_parity  = swdptap_seq_out_parity;
+
 	return 0;
 }
 
@@ -102,34 +114,7 @@ static void swdptap_turnaround(uint8_t dir)
 	platform_buffer_write(cmd, index);
 }
 
-bool swdptap_bit_in(void)
-{
-	swdptap_turnaround(1);
-	uint8_t cmd[4];
-	int index = 0;
-
-	cmd[index++] = active_cable->bitbang_tms_in_port_cmd;
-	cmd[index++] = MPSSE_TMS_SHIFT;
-	cmd[index++] = 0;
-	cmd[index++] = 0;
-	platform_buffer_write(cmd, index);
-	uint8_t data[1];
-	platform_buffer_read(data, 1);
-	return (data[0] &= active_cable->bitbang_tms_in_pin);
-}
-
-void swdptap_bit_out(bool val)
-{
-	swdptap_turnaround(0);
-	uint8_t cmd[3];
-
-	cmd[0] = MPSSE_TMS_SHIFT;
-	cmd[1] = 0;
-	cmd[2] = (val)? 1 : 0;
-	platform_buffer_write(cmd, 3);
-}
-
-bool swdptap_seq_in_parity(uint32_t *res, int ticks)
+static bool swdptap_seq_in_parity(uint32_t *res, int ticks)
 {
 	int index = ticks + 1;
 	uint8_t cmd[4];
@@ -158,7 +143,7 @@ bool swdptap_seq_in_parity(uint32_t *res, int ticks)
 	return parity;
 }
 
-uint32_t swdptap_seq_in(int ticks)
+static uint32_t swdptap_seq_in(int ticks)
 {
 	int index = ticks;
 	uint8_t cmd[4];
@@ -182,7 +167,7 @@ uint32_t swdptap_seq_in(int ticks)
 	return ret;
 }
 
-void swdptap_seq_out(uint32_t MS, int ticks)
+static void swdptap_seq_out(uint32_t MS, int ticks)
 {
 	uint8_t cmd[15];
 	unsigned int index = 0;
@@ -203,7 +188,7 @@ void swdptap_seq_out(uint32_t MS, int ticks)
 	platform_buffer_write(cmd, index);
 }
 
-void swdptap_seq_out_parity(uint32_t MS, int ticks)
+static void swdptap_seq_out_parity(uint32_t MS, int ticks)
 {
 	uint8_t parity = 0;
 	int steps = ticks;
diff --git a/src/platforms/pc-hosted/swdptap.c b/src/platforms/pc-hosted/swdptap.c
index e63ed52..c62a1f9 100644
--- a/src/platforms/pc-hosted/swdptap.c
+++ b/src/platforms/pc-hosted/swdptap.c
@@ -30,7 +30,14 @@
 #include "swdptap.h"
 #include "remote.h"
 
-int swdptap_init(void)
+static bool swdptap_seq_in_parity(uint32_t *res, int ticks);
+static uint32_t swdptap_seq_in(int ticks);
+static void swdptap_seq_out(uint32_t MS, int ticks);
+static void swdptap_seq_out_parity(uint32_t MS, int ticks);
+
+swd_proc_t swd_proc;
+
+int platform_swdptap_init(void)
 
 {
   uint8_t construct[PLATFORM_MAX_MSG_SIZE];
@@ -46,11 +53,15 @@ int swdptap_init(void)
       exit(-1);
     }
 
+	swd_proc.swdptap_seq_in  = swdptap_seq_in;
+	swd_proc.swdptap_seq_in_parity  = swdptap_seq_in_parity;
+	swd_proc.swdptap_seq_out = swdptap_seq_out;
+	swd_proc.swdptap_seq_out_parity  = swdptap_seq_out_parity;
+
   return 0;
 }
 
-
-bool swdptap_seq_in_parity(uint32_t *res, int ticks)
+static bool swdptap_seq_in_parity(uint32_t *res, int ticks)
 
 {
   uint8_t construct[PLATFORM_MAX_MSG_SIZE];
@@ -70,8 +81,7 @@ bool swdptap_seq_in_parity(uint32_t *res, int ticks)
   return (construct[0]!=REMOTE_RESP_OK);
 }
 
-
-uint32_t swdptap_seq_in(int ticks)
+static uint32_t swdptap_seq_in(int ticks)
 {
   uint8_t construct[PLATFORM_MAX_MSG_SIZE];
   int s;
@@ -89,7 +99,7 @@ uint32_t swdptap_seq_in(int ticks)
   return remotehston(-1,(char *)&construct[1]);
 }
 
-void swdptap_seq_out(uint32_t MS, int ticks)
+static void swdptap_seq_out(uint32_t MS, int ticks)
 {
   uint8_t construct[PLATFORM_MAX_MSG_SIZE];
   int s;
@@ -106,7 +116,7 @@ void swdptap_seq_out(uint32_t MS, int ticks)
 }
 
 
-void swdptap_seq_out_parity(uint32_t MS, int ticks)
+static void swdptap_seq_out_parity(uint32_t MS, int ticks)
 {
   uint8_t construct[PLATFORM_MAX_MSG_SIZE];
   int s;
diff --git a/src/platforms/pc-stlinkv2/adiv5_swdp.c b/src/platforms/pc-stlinkv2/adiv5_swdp.c
deleted file mode 100644
index 77fbbf6..0000000
--- a/src/platforms/pc-stlinkv2/adiv5_swdp.c
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2011  Black Sphere Technologies Ltd.
- * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- * Copyright (C) 2019 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/>.
- */
-
-/* This file implements the SW-DP specific functions of the
- * ARM Debug Interface v5 Architecure Specification, ARM doc IHI0031A.
- */
-
-#include "general.h"
-#include "target.h"
-#include "target_internal.h"
-#include "adiv5.h"
-#include "stlinkv2.h"
-
-int adiv5_swdp_scan(void)
-{
-	target_list_free();
-	ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
-	if (stlink_enter_debug_swd())
-		return 0;
-	dp->idcode = stlink_read_coreid();
-	dp->dp_read = stlink_dp_read;
-	dp->error = stlink_dp_error;
-	dp->low_access = stlink_dp_low_access;
-	dp->abort = stlink_dp_abort;
-
-	stlink_dp_error(dp);
-	adiv5_dp_init(dp);
-
-	return target_list?1:0;
-	return 0;
-}
diff --git a/src/platforms/pc-stlinkv2/jtagtap.c b/src/platforms/pc-stlinkv2/jtagtap.c
new file mode 100644
index 0000000..e69de29
diff --git a/src/platforms/pc-stlinkv2/platform.c b/src/platforms/pc-stlinkv2/platform.c
index f594af1..eb8c6eb 100644
--- a/src/platforms/pc-stlinkv2/platform.c
+++ b/src/platforms/pc-stlinkv2/platform.c
@@ -22,6 +22,7 @@
 #include "platform.h"
 #include "target.h"
 #include "target_internal.h"
+#include "swdptap.h"
 
 #include <assert.h>
 #include <unistd.h>
@@ -41,6 +42,13 @@ const char *platform_target_voltage(void)
 	return stlink_target_voltage();
 }
 
+int platform_swdptap_init(void)
+{
+	return 0;
+}
+
+swd_proc_t swd_proc;
+
 static int adiv5_swdp_scan_stlinkv2(void)
 {
 	target_list_free();
diff --git a/src/platforms/pc-stlinkv2/swdptap.c b/src/platforms/pc-stlinkv2/swdptap.c
new file mode 100644
index 0000000..e69de29
diff --git a/src/remote.c b/src/remote.c
index 354e1b6..d152a45 100644
--- a/src/remote.c
+++ b/src/remote.c
@@ -115,27 +115,27 @@ void remotePacketProcessSWD(uint8_t i, char *packet)
 
     case REMOTE_IN_PAR: /* = In parity ================================== */
 		ticks=remotehston(2,&packet[2]);
-		badParity=swdptap_seq_in_parity(&param, ticks);
+		badParity = swd_proc.swdptap_seq_in_parity(&param, ticks);
 		_respond(badParity?REMOTE_RESP_PARERR:REMOTE_RESP_OK,param);
 		break;
 
     case REMOTE_IN: /* = In ========================================= */
 		ticks=remotehston(2,&packet[2]);
-		param=swdptap_seq_in(ticks);
+		param = swd_proc.swdptap_seq_in(ticks);
 		_respond(REMOTE_RESP_OK,param);
 		break;
 
     case REMOTE_OUT: /* = Out ======================================== */
 		ticks=remotehston(2,&packet[2]);
 		param=remotehston(-1, &packet[4]);
-		swdptap_seq_out(param, ticks);
+		swd_proc.swdptap_seq_out(param, ticks);
 		_respond(REMOTE_RESP_OK, 0);
 		break;
 
     case REMOTE_OUT_PAR: /* = Out parity ================================= */
 		ticks=remotehston(2,&packet[2]);
 		param=remotehston(-1, &packet[4]);
-		swdptap_seq_out_parity(param, ticks);
+		swd_proc.swdptap_seq_out_parity(param, ticks);
 		_respond(REMOTE_RESP_OK, 0);
 		break;
 
diff --git a/src/target/adiv5_swdp.c b/src/target/adiv5_swdp.c
index 6ac1d7b..9cfdf49 100644
--- a/src/target/adiv5_swdp.c
+++ b/src/target/adiv5_swdp.c
@@ -53,24 +53,28 @@ int adiv5_swdp_scan(void)
 		return -1;
 	}
 
+#if PC_HOSTED == 1
+	if (platform_swdptap_init())
+#else
 	if (swdptap_init())
+#endif
 		return -1;
 
 	/* Switch from JTAG to SWD mode */
-	swdptap_seq_out(0xFFFFFFFF, 16);
-	swdptap_seq_out(0xFFFFFFFF, 32);
-	swdptap_seq_out(0xFFFFFFFF, 18);
-	swdptap_seq_out(0xE79E, 16); /* 0b0111100111100111 */
-	swdptap_seq_out(0xFFFFFFFF, 32);
-	swdptap_seq_out(0xFFFFFFFF, 18);
-	swdptap_seq_out(0, 16);
+	swd_proc.swdptap_seq_out(0xFFFFFFFF, 16);
+	swd_proc.swdptap_seq_out(0xFFFFFFFF, 32);
+	swd_proc.swdptap_seq_out(0xFFFFFFFF, 18);
+	swd_proc.swdptap_seq_out(0xE79E, 16); /* 0b0111100111100111 */
+	swd_proc.swdptap_seq_out(0xFFFFFFFF, 32);
+	swd_proc.swdptap_seq_out(0xFFFFFFFF, 18);
+	swd_proc.swdptap_seq_out(0, 16);
 
 	/* Read the SW-DP IDCODE register to syncronise */
 	/* This could be done with adiv_swdp_low_access(), but this doesn't
 	 * allow the ack to be checked here. */
-	swdptap_seq_out(0xA5, 8);
-	ack = swdptap_seq_in(3);
-	if((ack != SWDP_ACK_OK) || swdptap_seq_in_parity(&dp->idcode, 32)) {
+	swd_proc.swdptap_seq_out(0xA5, 8);
+	ack = swd_proc.swdptap_seq_in(3);
+	if((ack != SWDP_ACK_OK) || swd_proc.swdptap_seq_in_parity(&dp->idcode, 32)) {
 		DEBUG("\n");
 		free(dp);
 		return -1;
@@ -143,8 +147,8 @@ static uint32_t adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 
 	platform_timeout_set(&timeout, 2000);
 	do {
-		swdptap_seq_out(request, 8);
-		ack = swdptap_seq_in(3);
+		swd_proc.swdptap_seq_out(request, 8);
+		ack = swd_proc.swdptap_seq_in(3);
 	} while (ack == SWDP_ACK_WAIT && !platform_timeout_is_expired(&timeout));
 
 	if (ack == SWDP_ACK_WAIT)
@@ -159,10 +163,10 @@ static uint32_t adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 		raise_exception(EXCEPTION_ERROR, "SWDP invalid ACK");
 
 	if(RnW) {
-		if(swdptap_seq_in_parity(&response, 32))  /* Give up on parity error */
+		if(swd_proc.swdptap_seq_in_parity(&response, 32))  /* Give up on parity error */
 			raise_exception(EXCEPTION_ERROR, "SWDP Parity error");
 	} else {
-		swdptap_seq_out_parity(value, 32);
+		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
@@ -173,7 +177,7 @@ static uint32_t adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 		 * for a power-up request. If the next transaction (requiring
 		 * a power-up) occurs immediately, it will fail.
 		 */
-		swdptap_seq_out(0, 2);
+		swd_proc.swdptap_seq_out(0, 2);
 	}
 
 	return response;
diff --git a/src/platforms/common/timing.c b/src/timing.c
similarity index 100%
rename from src/platforms/common/timing.c
rename to src/timing.c

From 9969c984f3ca67f38677b061e2f91a1fb50cd310 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Thu, 16 Apr 2020 19:38:32 +0200
Subject: [PATCH 17/43] detour jtag primitives.

---
 src/Makefile                             |  8 +--
 src/include/jtagtap.h                    | 30 ++++++----
 src/platforms/common/jtagtap.c           | 31 +++++++---
 src/platforms/libftdi/jtagtap.c          | 30 +++++++---
 src/platforms/libftdi/platform.c         |  5 ++
 src/platforms/pc-hosted/jtagtap.c        | 33 ++++++++---
 src/platforms/pc-hosted/platform.c       |  5 ++
 src/platforms/pc-stlinkv2/adiv5_jtagdp.c | 73 ------------------------
 src/platforms/pc-stlinkv2/platform.c     |  6 ++
 src/platforms/pc-stlinkv2/stlinkv2.c     | 43 ++++++++++++++
 src/remote.c                             |  8 +--
 src/target/adiv5.h                       |  6 ++
 src/target/adiv5_jtagdp.c                | 23 ++++----
 src/target/jtag_scan.c                   | 47 ++++++++-------
 src/target/jtag_scan.h                   | 10 +---
 15 files changed, 203 insertions(+), 155 deletions(-)
 delete mode 100644 src/platforms/pc-stlinkv2/adiv5_jtagdp.c

diff --git a/src/Makefile b/src/Makefile
index 0d3cdf7..9430501 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -32,6 +32,8 @@ SRC =			\
 	gdb_packet.c	\
 	hex_utils.c	\
 	jtag_devs.c	\
+	jtag_scan.c	\
+	jtagtap.c	\
 	lmi.c		\
 	lpc_common.c	\
 	lpc11xx.c	\
@@ -72,12 +74,6 @@ ifdef NO_OWN_LL
 SRC += jtagtap_generic.c swdptap_generic.c
 endif
 
-ifndef OWN_HL
-SRC += jtag_scan.c jtagtap.c
-else
-CFLAGS += -DOWN_HL
-endif
-
 ifdef PC_HOSTED
 CFLAGS += -DPC_HOSTED=1
 else
diff --git a/src/include/jtagtap.h b/src/include/jtagtap.h
index 898b9c8..4c2ca43 100644
--- a/src/include/jtagtap.h
+++ b/src/include/jtagtap.h
@@ -21,13 +21,12 @@
 #ifndef __JTAGTAP_H
 #define __JTAGTAP_H
 
+typedef struct jtag_proc_s {
 /* Note: Signal names are as for the device under test. */
 
-int jtagtap_init(void);
+	void (*jtagtap_reset)(void);
 
-void jtagtap_reset(void);
-
-uint8_t jtagtap_next(const uint8_t TMS, const uint8_t TDI);
+	uint8_t (*jtagtap_next)(const uint8_t TMS, const uint8_t TDI);
 /* tap_next executes one state transision in the JTAG TAP state machine:
  * - Ensure TCK is low
  * - Assert the values of TMS and TDI
@@ -36,30 +35,39 @@ uint8_t jtagtap_next(const uint8_t TMS, const uint8_t TDI);
  * - Release TCK.
  */
 
-void jtagtap_tms_seq(uint32_t MS, int ticks);
-void jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks);
-void jtagtap_tdi_seq(const uint8_t final_tms, const uint8_t *DI, int ticks);
+	void (*jtagtap_tms_seq)(uint32_t MS, int ticks);
+	void (*jtagtap_tdi_tdo_seq)
+	(uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks);
 /* Shift out a sequence on MS and DI, capture data to DO.
  * - This is not endian safe: First byte will always be first shifted out.
  * - DO may be NULL to ignore captured data.
  * - DO may be point to the same address as DI.
  */
+	void (*jtagtap_tdi_seq)
+	(const uint8_t final_tms, const uint8_t *DI, int ticks);
+} jtag_proc_t;
+extern jtag_proc_t jtag_proc;
 
 /* generic soft reset: 1, 1, 1, 1, 1, 0 */
 #define jtagtap_soft_reset()	\
-	jtagtap_tms_seq(0x1F, 6)
+	jtag_proc.jtagtap_tms_seq(0x1F, 6)
 
 /* Goto Shift-IR: 1, 1, 0, 0 */
 #define jtagtap_shift_ir()		\
-	jtagtap_tms_seq(0x03, 4)
+	jtag_proc.jtagtap_tms_seq(0x03, 4)
 
 /* Goto Shift-DR: 1, 0, 0 */
 #define jtagtap_shift_dr()		\
-	jtagtap_tms_seq(0x01, 3)
+	jtag_proc.jtagtap_tms_seq(0x01, 3)
 
 /* Goto Run-test/Idle: 1, 1, 0 */
 #define jtagtap_return_idle()	\
-	jtagtap_tms_seq(0x01, 2)
+	jtag_proc.jtagtap_tms_seq(0x01, 2)
 
+# if PC_HOSTED == 1
+int platform_jtagtap_init(void);
+# else
+int jtagtap_init(void);
+# endif
 #endif
 
diff --git a/src/platforms/common/jtagtap.c b/src/platforms/common/jtagtap.c
index 0b5d4b4..a319a6b 100644
--- a/src/platforms/common/jtagtap.c
+++ b/src/platforms/common/jtagtap.c
@@ -26,10 +26,26 @@
 #include "jtagtap.h"
 #include "gdb_packet.h"
 
-int jtagtap_init(void)
+jtag_proc_t jtag_proc;
+
+static void jtagtap_reset(void);
+static void jtagtap_tms_seq(uint32_t MS, int ticks);
+static void jtagtap_tdi_tdo_seq(
+	uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks);
+static void jtagtap_tdi_seq(
+	const uint8_t final_tms, const uint8_t *DI, int ticks);
+static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI);
+
+int jtagtap_init()
 {
 	TMS_SET_MODE();
 
+	jtag_proc.jtagtap_reset = jtagtap_reset;
+	jtag_proc.jtagtap_next =jtagtap_next;
+	jtag_proc.jtagtap_tms_seq = jtagtap_tms_seq;
+	jtag_proc.jtagtap_tdi_tdo_seq = jtagtap_tdi_tdo_seq;
+	jtag_proc.jtagtap_tdi_seq = jtagtap_tdi_seq;
+
 	/* Go to JTAG mode for SWJ-DP */
 	for(int i = 0; i <= 50; i++) jtagtap_next(1, 0); /* Reset SW-DP */
 	jtagtap_tms_seq(0xE73C, 16);		/* SWD to JTAG sequence */
@@ -38,7 +54,7 @@ int jtagtap_init(void)
 	return 0;
 }
 
-void jtagtap_reset(void)
+static void jtagtap_reset(void)
 {
 #ifdef TRST_PORT
 	if (platform_hwversion() == 0) {
@@ -51,7 +67,7 @@ void jtagtap_reset(void)
 	jtagtap_soft_reset();
 }
 
-inline uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
+static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 {
 	uint16_t ret;
 
@@ -66,7 +82,7 @@ inline uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 	return ret != 0;
 }
 
-void jtagtap_tms_seq(uint32_t MS, int ticks)
+static void jtagtap_tms_seq(uint32_t MS, int ticks)
 {
 	gpio_set_val(TDI_PORT, TDI_PIN, 1);
 	int data = MS & 1;
@@ -80,8 +96,8 @@ void jtagtap_tms_seq(uint32_t MS, int ticks)
 	}
 }
 
-void
-jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks)
+static void jtagtap_tdi_tdo_seq(
+	uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks)
 {
 	uint8_t index = 1;
 	gpio_set_val(TMS_PORT, TMS_PIN, 0);
@@ -111,8 +127,7 @@ jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int
 	gpio_clear(TCK_PORT, TCK_PIN);
 }
 
-void
-jtagtap_tdi_seq(const uint8_t final_tms, const uint8_t *DI, int ticks)
+static void jtagtap_tdi_seq(const uint8_t final_tms, const uint8_t *DI, int ticks)
 {
 	uint8_t index = 1;
 	while(ticks--) {
diff --git a/src/platforms/libftdi/jtagtap.c b/src/platforms/libftdi/jtagtap.c
index f586c46..a928d19 100644
--- a/src/platforms/libftdi/jtagtap.c
+++ b/src/platforms/libftdi/jtagtap.c
@@ -33,7 +33,17 @@
 #include "general.h"
 #include "jtagtap.h"
 
-int jtagtap_init(void)
+jtag_proc_t jtag_proc;
+
+static void jtagtap_reset(void);
+static void jtagtap_tms_seq(uint32_t MS, int ticks);
+static void jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms,
+								const uint8_t *DI, int ticks);
+static void jtagtap_tdi_seq(
+	const uint8_t final_tms, const uint8_t *DI, int ticks);
+static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI);
+
+int platform_jtagtap_init(void)
 {
 	assert(ftdic != NULL);
 	int err = ftdi_usb_purge_buffers(ftdic);
@@ -65,6 +75,12 @@ int jtagtap_init(void)
 	platform_buffer_write(ftdi_init, 9);
 	platform_buffer_flush();
 
+	jtag_proc.jtagtap_reset = jtagtap_reset;
+	jtag_proc.jtagtap_next =jtagtap_next;
+	jtag_proc.jtagtap_tms_seq = jtagtap_tms_seq;
+	jtag_proc.jtagtap_tdi_tdo_seq = jtagtap_tdi_tdo_seq;
+	jtag_proc.jtagtap_tdi_seq = jtagtap_tdi_seq;
+
 	/* Go to JTAG mode for SWJ-DP */
 	for (int i = 0; i <= 50; i++)
 		jtagtap_next(1, 0);		/* Reset SW-DP */
@@ -79,8 +95,7 @@ void jtagtap_reset(void)
 	jtagtap_soft_reset();
 }
 
-void
-jtagtap_tms_seq(uint32_t MS, int ticks)
+static void jtagtap_tms_seq(uint32_t MS, int ticks)
 {
 	uint8_t tmp[3] = {MPSSE_WRITE_TMS | MPSSE_LSB | MPSSE_BITMODE| MPSSE_READ_NEG, 0, 0};
 	while(ticks >= 0) {
@@ -92,8 +107,8 @@ jtagtap_tms_seq(uint32_t MS, int ticks)
 	}
 }
 
-void
-jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks)
+static void jtagtap_tdi_tdo_seq(
+	uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks)
 {
 	int rsize, rticks;
 
@@ -157,12 +172,13 @@ jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int
 	}
 }
 
-void jtagtap_tdi_seq(const uint8_t final_tms, const uint8_t *DI, int ticks)
+static void jtagtap_tdi_seq(
+	const uint8_t final_tms, const uint8_t *DI, int ticks)
 {
 	return jtagtap_tdi_tdo_seq(NULL,  final_tms, DI, ticks);
 }
 
-uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
+static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 {
 	uint8_t ret;
 	uint8_t tmp[3] = {MPSSE_WRITE_TMS | MPSSE_DO_READ | MPSSE_LSB | MPSSE_BITMODE | MPSSE_WRITE_NEG, 0, 0};
diff --git a/src/platforms/libftdi/platform.c b/src/platforms/libftdi/platform.c
index 88ed4f2..93c89e9 100644
--- a/src/platforms/libftdi/platform.c
+++ b/src/platforms/libftdi/platform.c
@@ -191,6 +191,11 @@ int platform_jtag_scan(const uint8_t *lrlens)
 	return jtag_scan(lrlens);
 }
 
+int platform_jtag_dp_init()
+{
+	return 0;
+}
+
 void platform_init(int argc, char **argv)
 {
 	BMP_CL_OPTIONS_t cl_opts = {0};
diff --git a/src/platforms/pc-hosted/jtagtap.c b/src/platforms/pc-hosted/jtagtap.c
index 86e068d..bc5a816 100644
--- a/src/platforms/pc-hosted/jtagtap.c
+++ b/src/platforms/pc-hosted/jtagtap.c
@@ -35,9 +35,17 @@
 #include "remote.h"
 #include "jtagtap.h"
 
-/* See remote.c/.h for protocol information */
+jtag_proc_t jtag_proc;
 
-int jtagtap_init(void)
+static void jtagtap_reset(void);
+static void jtagtap_tms_seq(uint32_t MS, int ticks);
+static void jtagtap_tdi_tdo_seq(
+	uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks);
+static void jtagtap_tdi_seq(
+	const uint8_t final_tms, const uint8_t *DI, int ticks);
+static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI);
+
+int platform_jtagtap_init(void)
 {
   uint8_t construct[PLATFORM_MAX_MSG_SIZE];
   int s;
@@ -52,10 +60,18 @@ int jtagtap_init(void)
       exit(-1);
     }
 
+  jtag_proc.jtagtap_reset = jtagtap_reset;
+  jtag_proc.jtagtap_next =jtagtap_next;
+  jtag_proc.jtagtap_tms_seq = jtagtap_tms_seq;
+  jtag_proc.jtagtap_tdi_tdo_seq = jtagtap_tdi_tdo_seq;
+  jtag_proc.jtagtap_tdi_seq = jtagtap_tdi_seq;
+
   return 0;
 }
 
-void jtagtap_reset(void)
+/* See remote.c/.h for protocol information */
+
+static void jtagtap_reset(void)
 {
   uint8_t construct[PLATFORM_MAX_MSG_SIZE];
   int s;
@@ -71,7 +87,7 @@ void jtagtap_reset(void)
     }
 }
 
-void jtagtap_tms_seq(uint32_t MS, int ticks)
+static void jtagtap_tms_seq(uint32_t MS, int ticks)
 
 {
   uint8_t construct[PLATFORM_MAX_MSG_SIZE];
@@ -88,7 +104,8 @@ void jtagtap_tms_seq(uint32_t MS, int ticks)
     }
 }
 
-void jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks)
+static void jtagtap_tdi_tdo_seq(
+	uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks)
 {
   uint8_t construct[PLATFORM_MAX_MSG_SIZE];
   int s;
@@ -116,14 +133,14 @@ void jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms, const uint8_t *DI
   }
 }
 
-void jtagtap_tdi_seq(const uint8_t final_tms, const uint8_t *DI, int ticks)
-
+static void jtagtap_tdi_seq(
+	const uint8_t final_tms, const uint8_t *DI, int ticks)
 {
   return jtagtap_tdi_tdo_seq(NULL,  final_tms, DI, ticks);
 }
 
 
-uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
+static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 
 {
   uint8_t construct[PLATFORM_MAX_MSG_SIZE];
diff --git a/src/platforms/pc-hosted/platform.c b/src/platforms/pc-hosted/platform.c
index 82f7996..53f95ba 100644
--- a/src/platforms/pc-hosted/platform.c
+++ b/src/platforms/pc-hosted/platform.c
@@ -44,6 +44,11 @@ int platform_jtag_scan(const uint8_t *lrlens)
 	return jtag_scan(lrlens);
 }
 
+int platform_jtag_dp_init()
+{
+	return 0;
+}
+
 void platform_init(int argc, char **argv)
 {
   cl_opts.opt_idstring = "Blackmagic Debug Probe Remote";
diff --git a/src/platforms/pc-stlinkv2/adiv5_jtagdp.c b/src/platforms/pc-stlinkv2/adiv5_jtagdp.c
deleted file mode 100644
index 30ff697..0000000
--- a/src/platforms/pc-stlinkv2/adiv5_jtagdp.c
+++ /dev/null
@@ -1,73 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2019  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/>.
- */
-
-/* This file implements a subset of JTAG-DP specific functions of the
- * ARM Debug Interface v5 Architecure Specification, ARM doc IHI0031A
- * used in BMP.
- */
-
-#include "general.h"
-#include "target.h"
-#include "adiv5.h"
-#include "stlinkv2.h"
-#include "jtag_devs.h"
-
-struct jtag_dev_s jtag_devs[JTAG_MAX_DEVS+1];
-int jtag_dev_count;
-
-int jtag_scan_stlinkv2(const uint8_t *irlens)
-{
-	uint32_t idcodes[JTAG_MAX_DEVS+1];
-	(void) *irlens;
-	target_list_free();
-
-	jtag_dev_count = 0;
-	memset(&jtag_devs, 0, sizeof(jtag_devs));
-	if (stlink_enter_debug_jtag())
-		return 0;
-	jtag_dev_count = stlink_read_idcodes(idcodes);
-	/* Check for known devices and handle accordingly */
-	for(int i = 0; i < jtag_dev_count; i++)
-		jtag_devs[i].idcode = idcodes[i];
-	for(int i = 0; i < jtag_dev_count; i++)
-		for(int j = 0; dev_descr[j].idcode; j++)
-			if((jtag_devs[i].idcode & dev_descr[j].idmask) ==
-			   dev_descr[j].idcode) {
-				if(dev_descr[j].handler)
-					dev_descr[j].handler(&jtag_devs[i]);
-				break;
-			}
-
-	return jtag_dev_count;
-}
-
-void adiv5_jtag_dp_handler(jtag_dev_t *dev)
-{
-	ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
-
-	dp->dev = dev;
-	dp->idcode = dev->idcode;
-
-	dp->dp_read = stlink_dp_read;
-	dp->error = stlink_dp_error;
-	dp->low_access = stlink_dp_low_access;
-	dp->abort = stlink_dp_abort;
-
-	adiv5_dp_init(dp);
-}
diff --git a/src/platforms/pc-stlinkv2/platform.c b/src/platforms/pc-stlinkv2/platform.c
index eb8c6eb..07379b0 100644
--- a/src/platforms/pc-stlinkv2/platform.c
+++ b/src/platforms/pc-stlinkv2/platform.c
@@ -48,6 +48,7 @@ int platform_swdptap_init(void)
 }
 
 swd_proc_t swd_proc;
+jtag_proc_t jtag_proc;
 
 static int adiv5_swdp_scan_stlinkv2(void)
 {
@@ -107,3 +108,8 @@ int platform_buffer_read(uint8_t *data, int size)
 	(void) data;
 	return size;
 }
+
+int platform_jtagtap_init(void)
+{
+	return 0;
+}
diff --git a/src/platforms/pc-stlinkv2/stlinkv2.c b/src/platforms/pc-stlinkv2/stlinkv2.c
index ab855ca..b69b66a 100644
--- a/src/platforms/pc-stlinkv2/stlinkv2.c
+++ b/src/platforms/pc-stlinkv2/stlinkv2.c
@@ -29,6 +29,8 @@
 #include "adiv5.h"
 #include "stlinkv2.h"
 #include "exception.h"
+#include "jtag_devs.h"
+#include "target.h"
 
 #include <assert.h>
 #include <unistd.h>
@@ -1383,3 +1385,44 @@ uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 	stlink_read_dp_register(ap->apsel, addr, &ret);
 	return ret;
 }
+
+struct jtag_dev_s jtag_devs[JTAG_MAX_DEVS+1];
+int jtag_dev_count;
+jtag_proc_t jtag_proc;
+
+int jtag_scan_stlinkv2(const uint8_t *irlens)
+{
+	uint32_t idcodes[JTAG_MAX_DEVS+1];
+	(void) *irlens;
+	target_list_free();
+
+	jtag_dev_count = 0;
+	memset(&jtag_devs, 0, sizeof(jtag_devs));
+	if (stlink_enter_debug_jtag())
+		return 0;
+	jtag_dev_count = stlink_read_idcodes(idcodes);
+	/* Check for known devices and handle accordingly */
+	for(int i = 0; i < jtag_dev_count; i++)
+		jtag_devs[i].idcode = idcodes[i];
+	for(int i = 0; i < jtag_dev_count; i++)
+		for(int j = 0; dev_descr[j].idcode; j++)
+			if((jtag_devs[i].idcode & dev_descr[j].idmask) ==
+			   dev_descr[j].idcode) {
+				if(dev_descr[j].handler)
+					dev_descr[j].handler(&jtag_devs[i]);
+				break;
+			}
+
+	return jtag_dev_count;
+}
+
+int platform_jtag_dp_init(ADIv5_DP_t *dp)
+{
+	dp->dp_read = stlink_dp_read;
+	dp->error = stlink_dp_error;
+	dp->low_access = stlink_dp_low_access;
+	dp->abort = stlink_dp_abort;
+
+	return true;
+
+}
diff --git a/src/remote.c b/src/remote.c
index d152a45..30cd075 100644
--- a/src/remote.c
+++ b/src/remote.c
@@ -159,7 +159,7 @@ void remotePacketProcessJTAG(uint8_t i, char *packet)
 		break;
 
     case REMOTE_RESET: /* = reset ================================= */
-		jtagtap_reset();
+		jtag_proc.jtagtap_reset();
 		_respond(REMOTE_RESP_OK, 0);
 		break;
 
@@ -170,7 +170,7 @@ void remotePacketProcessJTAG(uint8_t i, char *packet)
 		if (i<4) {
 			_respond(REMOTE_RESP_ERR,REMOTE_ERROR_WRONGLEN);
 		} else {
-			jtagtap_tms_seq( MS, ticks);
+			jtag_proc.jtagtap_tms_seq( MS, ticks);
 			_respond(REMOTE_RESP_OK, 0);
 		}
 		break;
@@ -183,7 +183,7 @@ void remotePacketProcessJTAG(uint8_t i, char *packet)
 		} else {
 			ticks=remotehston(2,&packet[2]);
 			DI=remotehston(-1,&packet[4]);
-			jtagtap_tdi_tdo_seq((void *)&DO, (packet[1]==REMOTE_TDITDO_TMS), (void *)&DI, ticks);
+			jtag_proc.jtagtap_tdi_tdo_seq((void *)&DO, (packet[1]==REMOTE_TDITDO_TMS), (void *)&DI, ticks);
 
 			/* Mask extra bits on return value... */
 			DO &= (1LL << (ticks + 1)) - 1;
@@ -196,7 +196,7 @@ void remotePacketProcessJTAG(uint8_t i, char *packet)
 		if (i!=4) {
 			_respond(REMOTE_RESP_ERR,REMOTE_ERROR_WRONGLEN);
 		} else {
-			uint32_t dat=jtagtap_next( (packet[2]=='1'), (packet[3]=='1'));
+			uint32_t dat=jtag_proc.jtagtap_next( (packet[2]=='1'), (packet[3]=='1'));
 			_respond(REMOTE_RESP_OK,dat);
 		}
 		break;
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index 1ae53bd..cbab7a6 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -142,10 +142,15 @@ typedef struct ADIv5_DP_s {
                                uint16_t addr, uint32_t value);
 	void (*abort)(struct ADIv5_DP_s *dp, uint32_t abort);
 
+#if PC_HOSTED == 1
+	jtag_dev_t *dev;
+	uint8_t fault;
+#else
 	union {
 		jtag_dev_t *dev;
 		uint8_t fault;
 	};
+#endif
 } ADIv5_DP_t;
 
 static inline uint32_t adiv5_dp_read(ADIv5_DP_t *dp, uint16_t addr)
@@ -191,6 +196,7 @@ void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
 uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr);
 
 void adiv5_jtag_dp_handler(jtag_dev_t *dev);
+int platform_jtag_dp_init(ADIv5_DP_t *dp);
 
 void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len);
 void adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len);
diff --git a/src/target/adiv5_jtagdp.c b/src/target/adiv5_jtagdp.c
index e8d5879..d6a3cf8 100644
--- a/src/target/adiv5_jtagdp.c
+++ b/src/target/adiv5_jtagdp.c
@@ -55,13 +55,13 @@ void adiv5_jtag_dp_handler(jtag_dev_t *dev)
 	}
 
 	dp->dev = dev;
-	dp->idcode = dev->idcode;
-
-	dp->dp_read = adiv5_jtagdp_read;
-	dp->error = adiv5_jtagdp_error;
-	dp->low_access = adiv5_jtagdp_low_access;
-	dp->abort = adiv5_jtagdp_abort;
-
+	if ((PC_HOSTED == 0 ) || (!platform_jtag_dp_init(dp))) {
+		dp->idcode = dev->idcode;
+		dp->dp_read = adiv5_jtagdp_read;
+		dp->error = adiv5_jtagdp_error;
+		dp->low_access = adiv5_jtagdp_low_access;
+		dp->abort = adiv5_jtagdp_abort;
+	}
 	adiv5_dp_init(dp);
 }
 
@@ -90,11 +90,12 @@ static uint32_t adiv5_jtagdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 
 	request = ((uint64_t)value << 3) | ((addr >> 1) & 0x06) | (RnW?1:0);
 
-	jtag_dev_write_ir(dp->dev, APnDP ? IR_APACC : IR_DPACC);
+	jtag_dev_write_ir(&jtag_proc, dp->dev, APnDP ? IR_APACC : IR_DPACC);
 
 	platform_timeout_set(&timeout, 2000);
 	do {
-		jtag_dev_shift_dr(dp->dev, (uint8_t*)&response, (uint8_t*)&request, 35);
+		jtag_dev_shift_dr(&jtag_proc, dp->dev, (uint8_t*)&response,
+						  (uint8_t*)&request, 35);
 		ack = response & 0x07;
 	} while(!platform_timeout_is_expired(&timeout) && (ack == JTAGDP_ACK_WAIT));
 
@@ -110,6 +111,6 @@ static uint32_t adiv5_jtagdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 static void adiv5_jtagdp_abort(ADIv5_DP_t *dp, uint32_t abort)
 {
 	uint64_t request = (uint64_t)abort << 3;
-	jtag_dev_write_ir(dp->dev, IR_ABORT);
-	jtag_dev_shift_dr(dp->dev, NULL, (const uint8_t*)&request, 35);
+	jtag_dev_write_ir(&jtag_proc, dp->dev, IR_ABORT);
+	jtag_dev_shift_dr(&jtag_proc, dp->dev, NULL, (const uint8_t*)&request, 35);
 }
diff --git a/src/target/jtag_scan.c b/src/target/jtag_scan.c
index 91935ad..28642f9 100644
--- a/src/target/jtag_scan.c
+++ b/src/target/jtag_scan.c
@@ -67,8 +67,15 @@ int jtag_scan(const uint8_t *irlens)
 	 * in SW-DP mode.
 	 */
 	DEBUG("Resetting TAP\n");
+#if PC_HOSTED == 1
+	if (platform_jtagtap_init()) {
+		DEBUG("JTAG not available\n");
+		return 0;
+	}
+#else
 	jtagtap_init();
-	jtagtap_reset();
+#endif
+	jtag_proc.jtagtap_reset();
 
 	if (irlens) {
 		DEBUG("Given list of IR lengths, skipping probe\n");
@@ -80,7 +87,7 @@ int jtag_scan(const uint8_t *irlens)
 			uint32_t irout;
 			if(*irlens == 0)
 				break;
-			jtagtap_tdi_tdo_seq((uint8_t*)&irout, 0, ones, *irlens);
+			jtag_proc.jtagtap_tdi_tdo_seq((uint8_t*)&irout, 0, ones, *irlens);
 			if (!(irout & 1)) {
 				DEBUG("check failed: IR[0] != 1\n");
 				return -1;
@@ -97,7 +104,7 @@ int jtag_scan(const uint8_t *irlens)
 		jtagtap_shift_ir();
 
 		DEBUG("Scanning out IRs\n");
-		if(!jtagtap_next(0, 1)) {
+		if(!jtag_proc.jtagtap_next(0, 1)) {
 			DEBUG("jtag_scan: Sanity check failed: IR[0] shifted out as 0\n");
 			jtag_dev_count = -1;
 			return -1; /* must be 1 */
@@ -105,7 +112,7 @@ int jtag_scan(const uint8_t *irlens)
 		jtag_devs[0].ir_len = 1; j = 1;
 		while((jtag_dev_count <= JTAG_MAX_DEVS) &&
 		      (jtag_devs[jtag_dev_count].ir_len <= JTAG_MAX_IR_LEN)) {
-			if(jtagtap_next(0, 1)) {
+			if(jtag_proc.jtagtap_next(0, 1)) {
 				if(jtag_devs[jtag_dev_count].ir_len == 1) break;
 				jtag_devs[++jtag_dev_count].ir_len = 1;
 				jtag_devs[jtag_dev_count].ir_prescan = j;
@@ -126,7 +133,7 @@ int jtag_scan(const uint8_t *irlens)
 	}
 
 	DEBUG("Return to Run-Test/Idle\n");
-	jtagtap_next(1, 1);
+	jtag_proc.jtagtap_next(1, 1);
 	jtagtap_return_idle();
 
 	/* All devices should be in BYPASS now */
@@ -134,7 +141,7 @@ int jtag_scan(const uint8_t *irlens)
 	/* Count device on chain */
 	DEBUG("Change state to Shift-DR\n");
 	jtagtap_shift_dr();
-	for(i = 0; (jtagtap_next(0, 1) == 0) && (i <= jtag_dev_count); i++)
+	for(i = 0; (jtag_proc.jtagtap_next(0, 1) == 0) && (i <= jtag_dev_count); i++)
 		jtag_devs[i].dr_postscan = jtag_dev_count - i - 1;
 
 	if(i != jtag_dev_count) {
@@ -145,7 +152,7 @@ int jtag_scan(const uint8_t *irlens)
 	}
 
 	DEBUG("Return to Run-Test/Idle\n");
-	jtagtap_next(1, 1);
+	jtag_proc.jtagtap_next(1, 1);
 	jtagtap_return_idle();
 	if(!jtag_dev_count) {
 		return 0;
@@ -157,17 +164,17 @@ int jtag_scan(const uint8_t *irlens)
 					jtag_devs[i].ir_len;
 
 	/* Reset jtagtap: should take all devs to IDCODE */
-	jtagtap_reset();
+	jtag_proc.jtagtap_reset();
 	jtagtap_shift_dr();
 	for(i = 0; i < jtag_dev_count; i++) {
-		if(!jtagtap_next(0, 1)) continue;
+		if(!jtag_proc.jtagtap_next(0, 1)) continue;
 		jtag_devs[i].idcode = 1;
 		for(j = 2; j; j <<= 1)
-			if(jtagtap_next(0, 1)) jtag_devs[i].idcode |= j;
+			if(jtag_proc.jtagtap_next(0, 1)) jtag_devs[i].idcode |= j;
 
 	}
 	DEBUG("Return to Run-Test/Idle\n");
-	jtagtap_next(1, 1);
+	jtag_proc.jtagtap_next(1, 1);
 	jtagtap_return_idle();
 
 	/* Check for known devices and handle accordingly */
@@ -187,7 +194,7 @@ int jtag_scan(const uint8_t *irlens)
 	return jtag_dev_count;
 }
 
-void jtag_dev_write_ir(jtag_dev_t *d, uint32_t ir)
+void jtag_dev_write_ir(jtag_proc_t *jp, jtag_dev_t *d, uint32_t ir)
 {
 	if(ir == d->current_ir) return;
 	for(int i = 0; i < jtag_dev_count; i++)
@@ -195,21 +202,21 @@ void jtag_dev_write_ir(jtag_dev_t *d, uint32_t ir)
 	d->current_ir = ir;
 
 	jtagtap_shift_ir();
-	jtagtap_tdi_seq(0, ones, d->ir_prescan);
-	jtagtap_tdi_seq(d->ir_postscan?0:1, (void*)&ir, d->ir_len);
-	jtagtap_tdi_seq(1, ones, d->ir_postscan);
+	jp->jtagtap_tdi_seq(0, ones, d->ir_prescan);
+	jp->jtagtap_tdi_seq(d->ir_postscan?0:1, (void*)&ir, d->ir_len);
+	jp->jtagtap_tdi_seq(1, ones, d->ir_postscan);
 	jtagtap_return_idle();
 }
 
-void jtag_dev_shift_dr(jtag_dev_t *d, uint8_t *dout, const uint8_t *din, int ticks)
+void jtag_dev_shift_dr(jtag_proc_t *jp, jtag_dev_t *d, uint8_t *dout, const uint8_t *din, int ticks)
 {
 	jtagtap_shift_dr();
-	jtagtap_tdi_seq(0, ones, d->dr_prescan);
+	jp->jtagtap_tdi_seq(0, ones, d->dr_prescan);
 	if(dout)
-		jtagtap_tdi_tdo_seq((void*)dout, d->dr_postscan?0:1, (void*)din, ticks);
+		jp->jtagtap_tdi_tdo_seq((void*)dout, d->dr_postscan?0:1, (void*)din, ticks);
 	else
-		jtagtap_tdi_seq(d->dr_postscan?0:1, (void*)din, ticks);
-	jtagtap_tdi_seq(1, ones, d->dr_postscan);
+		jp->jtagtap_tdi_seq(d->dr_postscan?0:1, (void*)din, ticks);
+	jp->jtagtap_tdi_seq(1, ones, d->dr_postscan);
 	jtagtap_return_idle();
 }
 
diff --git a/src/target/jtag_scan.h b/src/target/jtag_scan.h
index cd46801..5134ec1 100644
--- a/src/target/jtag_scan.h
+++ b/src/target/jtag_scan.h
@@ -17,6 +17,7 @@
  * 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 <jtagtap.h>
 
 #ifndef __JTAG_SCAN_H
 #define __JTAG_SCAN_H
@@ -25,7 +26,6 @@
 #define JTAG_MAX_IR_LEN	16
 
 typedef struct jtag_dev_s {
-#if !defined(JTAG_HL)
 	union {
 		uint8_t dev;
 		uint8_t dr_prescan;
@@ -35,20 +35,16 @@ typedef struct jtag_dev_s {
 	uint8_t ir_len;
 	uint8_t ir_prescan;
 	uint8_t ir_postscan;
-#endif
 	uint32_t idcode;
 	const char *descr;
-#if !defined(JTAG_HL)
 	uint32_t current_ir;
-#endif
-
 } jtag_dev_t;
 
 extern struct jtag_dev_s jtag_devs[JTAG_MAX_DEVS+1];
 extern int jtag_dev_count;
 
-void jtag_dev_write_ir(jtag_dev_t *dev, uint32_t ir);
-void jtag_dev_shift_dr(jtag_dev_t *dev, uint8_t *dout, const uint8_t *din, int ticks);
+void jtag_dev_write_ir(jtag_proc_t *jp, jtag_dev_t *dev, uint32_t ir);
+void jtag_dev_shift_dr(jtag_proc_t *jp, jtag_dev_t *dev, uint8_t *dout, const uint8_t *din, int ticks);
 
 #endif
 

From 563df2d35464bee022b8133ea4f7bc53bd40a399 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 18 Apr 2020 12:24:40 +0200
Subject: [PATCH 18/43] Detour ADIv5 high-level functions.

---
 src/platforms/libftdi/platform.c     |   5 +
 src/platforms/pc-hosted/platform.c   |   5 +
 src/platforms/pc-stlinkv2/stlinkv2.c |  43 ++++----
 src/target/adiv5.c                   |  56 +++++++---
 src/target/adiv5.h                   |  52 ++++++++--
 src/target/cortexm.c                 | 150 ++++++++++++++-------------
 6 files changed, 198 insertions(+), 113 deletions(-)

diff --git a/src/platforms/libftdi/platform.c b/src/platforms/libftdi/platform.c
index 93c89e9..e2ceb59 100644
--- a/src/platforms/libftdi/platform.c
+++ b/src/platforms/libftdi/platform.c
@@ -310,3 +310,8 @@ const char *platform_target_voltage(void)
 {
 	return "not supported";
 }
+
+void platform_adiv5_dp_defaults(void *arg)
+{
+	(void) arg;
+}
diff --git a/src/platforms/pc-hosted/platform.c b/src/platforms/pc-hosted/platform.c
index 53f95ba..4be4cde 100644
--- a/src/platforms/pc-hosted/platform.c
+++ b/src/platforms/pc-hosted/platform.c
@@ -181,3 +181,8 @@ const char *platform_target_voltage(void)
 
   return (char *)&construct[1];
 }
+
+void platform_adiv5_dp_defaults(void *arg)
+{
+	(void) arg;
+}
diff --git a/src/platforms/pc-stlinkv2/stlinkv2.c b/src/platforms/pc-stlinkv2/stlinkv2.c
index b69b66a..0d5012f 100644
--- a/src/platforms/pc-stlinkv2/stlinkv2.c
+++ b/src/platforms/pc-stlinkv2/stlinkv2.c
@@ -1150,7 +1150,7 @@ uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 	return response;
 }
 
-bool adiv5_ap_setup(int ap)
+static bool stlink_ap_setup(int ap)
 {
 	if (ap > 7)
 		return false;
@@ -1172,7 +1172,7 @@ bool adiv5_ap_setup(int ap)
 	return true;
 }
 
-void adiv5_ap_cleanup(int ap)
+static void stlink_ap_cleanup(int ap)
 {
        uint8_t cmd[16] = {
                STLINK_DEBUG_COMMAND,
@@ -1195,7 +1195,7 @@ static int stlink_usb_get_rw_status(bool verbose)
 	return stlink_usb_error_check(data, verbose);
 }
 
-void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
+static void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 {
 	if (len == 0)
 		return;
@@ -1246,7 +1246,7 @@ void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 	DEBUG_STLINK("\n");
 }
 
-void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
+static void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 					  uint8_t *buffer)
 {
 	DEBUG_STLINK("Mem Write8 AP %d len %zu addr 0x%08" PRIx32 ": ",
@@ -1275,7 +1275,7 @@ void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 	}
 }
 
-void stlink_writemem16(ADIv5_AP_t *ap, uint32_t addr, size_t len,
+static void stlink_writemem16(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 					   uint16_t *buffer)
 {
 	DEBUG_STLINK("Mem Write16 AP %d len %zu addr 0x%08" PRIx32 ": ",
@@ -1295,7 +1295,7 @@ void stlink_writemem16(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 	stlink_usb_get_rw_status(true);
 }
 
-void stlink_writemem32(ADIv5_AP_t *ap, uint32_t addr, size_t len,
+static void stlink_writemem32(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 					   uint32_t *buffer)
 {
 	DEBUG_STLINK("Mem Write32 AP %d len %zu addr 0x%08" PRIx32 ": ",
@@ -1350,15 +1350,9 @@ void stlink_reg_write(ADIv5_AP_t *ap, int num, uint32_t val)
 	stlink_usb_error_check(res, true);
 }
 
-void
-adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
-{
-	stlink_readmem(ap, dest, src, len);
-}
-
-void
-adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
-					  size_t len, enum align align)
+static void stlink_mem_write_sized(	ADIv5_AP_t *ap, uint32_t dest,
+									const void *src, size_t len,
+									enum align align)
 {
 	if (len == 0)
 		return;
@@ -1374,12 +1368,12 @@ adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
 	}
 }
 
-void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+static void stlink_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 {
-	stlink_write_dp_register(ap->apsel, addr, value);
+       stlink_write_dp_register(ap->apsel, addr, value);
 }
 
-uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
+static uint32_t stlink_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 {
 	uint32_t ret;
 	stlink_read_dp_register(ap->apsel, addr, &ret);
@@ -1426,3 +1420,16 @@ int platform_jtag_dp_init(ADIv5_DP_t *dp)
 	return true;
 
 }
+
+void platform_adiv5_dp_defaults(ADIv5_DP_t *dp)
+{
+	dp->ap_regs_read = stlink_regs_read;
+	dp->ap_reg_read = stlink_reg_read;
+	dp->ap_reg_write = stlink_reg_write;
+	dp->ap_setup = stlink_ap_setup;
+	dp->ap_cleanup = stlink_ap_cleanup;
+	dp->ap_write = stlink_ap_write;
+	dp->ap_read = stlink_ap_read;
+	dp->mem_read = stlink_readmem;
+	dp->mem_write_sized = stlink_mem_write_sized;
+}
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 5f0de60..edac3c7 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -399,8 +399,6 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 	}
 	return res;
 }
-bool adiv5_ap_setup(int i);
-void adiv5_ap_cleanup(int i);
 
 ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 {
@@ -441,12 +439,32 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 	return ap;
 }
 
+static void ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
+static uint32_t ap_read(ADIv5_AP_t *ap, uint16_t addr);
+static void mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len);
+static void mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+							size_t len, enum align align);
 void adiv5_dp_init(ADIv5_DP_t *dp)
 {
 	volatile bool probed = false;
 	volatile uint32_t ctrlstat = 0;
 	adiv5_dp_ref(dp);
-
+#if PC_HOSTED  == 1
+	platform_adiv5_dp_defaults(dp);
+	if (!dp->ap_write)
+		dp->ap_write = ap_write;
+	if (!dp->ap_read)
+		dp->ap_read = ap_read;
+	if (!dp->mem_read)
+		dp->mem_read = mem_read;
+	if (!dp->mem_write_sized)
+		dp->mem_write_sized = mem_write_sized;
+#else
+	dp->ap_write = ap_write;
+	dp->ap_read = ap_read;
+	dp->mem_read = mem_read;
+	dp->mem_write_sized = mem_write_sized;
+#endif
 	volatile struct exception e;
 	TRY_CATCH (e, EXCEPTION_TIMEOUT) {
 		ctrlstat = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT);
@@ -507,11 +525,18 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 	int void_aps = 0;
 	for(int i = 0; (i < 256) && (void_aps < 8); i++) {
 		ADIv5_AP_t *ap = NULL;
-		if (adiv5_ap_setup(i))
+#if PC_HOSTED == 1
+		if ((!dp->ap_setup) || dp->ap_setup(i))
 			ap = adiv5_new_ap(dp, i);
+#else
+		ap = adiv5_new_ap(dp, i);
+#endif
 		if (ap == NULL) {
 			void_aps++;
-			adiv5_ap_cleanup(i);
+#if PC_HOSTED == 1
+			if (dp->ap_cleanup)
+				dp->ap_cleanup(i);
+#endif
 			if (i == 0)
 				return;
 			else
@@ -519,7 +544,10 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		}
 		if (ap->base == last_base) {
 			DEBUG("AP %d: Duplicate base\n", i);
-			adiv5_ap_cleanup(i);
+#if PC_HOSTED == 1
+			if (dp->ap_cleanup)
+				dp->ap_cleanup(i);
+#endif
 			free(ap);
 			/* FIXME: Should we expect valid APs behind duplicate ones? */
 			return;
@@ -562,11 +590,6 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 #define ALIGNOF(x) (((x) & 3) == 0 ? ALIGN_WORD : \
                     (((x) & 1) == 0 ? ALIGN_HALFWORD : ALIGN_BYTE))
 
-#if !defined(JTAG_HL)
-
-bool adiv5_ap_setup(int i) {(void)i; return true;}
-void adiv5_ap_cleanup(int i) {(void)i;}
-
 /* Program the CSW and TAR for sequencial access at a given width */
 static void ap_mem_access_setup(ADIv5_AP_t *ap, uint32_t addr, enum align align)
 {
@@ -606,7 +629,7 @@ void * extract(void *dest, uint32_t src, uint32_t val, enum align align)
 	return (uint8_t *)dest + (1 << align);
 }
 
-void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
+static void mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 {
 	uint32_t tmp;
 	uint32_t osrc = src;
@@ -636,8 +659,8 @@ void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 	extract(dest, src, tmp, align);
 }
 
-void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
-					  size_t len, enum align align)
+static void mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+							size_t len, enum align align)
 {
 	uint32_t odest = dest;
 
@@ -671,14 +694,14 @@ void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
 	}
 }
 
-void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+static void ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 {
 	adiv5_dp_write(ap->dp, ADIV5_DP_SELECT,
 			((uint32_t)ap->apsel << 24)|(addr & 0xF0));
 	adiv5_dp_write(ap->dp, addr, value);
 }
 
-uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
+static uint32_t ap_read(ADIv5_AP_t *ap, uint16_t addr)
 {
 	uint32_t ret;
 	adiv5_dp_write(ap->dp, ADIV5_DP_SELECT,
@@ -686,7 +709,6 @@ uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 	ret = adiv5_dp_read(ap->dp, addr);
 	return ret;
 }
-#endif
 
 void adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
 {
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index cbab7a6..d592921 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -129,6 +129,8 @@ enum align {
 	ALIGN_DWORD    = 3
 };
 
+typedef struct ADIv5_AP_s ADIv5_AP_t;
+
 /* Try to keep this somewhat absract for later adding SW-DP */
 typedef struct ADIv5_DP_s {
 	int refcnt;
@@ -142,6 +144,23 @@ typedef struct ADIv5_DP_s {
                                uint16_t addr, uint32_t value);
 	void (*abort)(struct ADIv5_DP_s *dp, uint32_t abort);
 
+#if PC_HOSTED == 1
+	bool (*ap_setup)(int i);
+	void (*ap_cleanup)(int i);
+    void (*ap_regs_read)(ADIv5_AP_t *ap, void *data);
+    uint32_t(*ap_reg_read)(ADIv5_AP_t *ap, int num);
+    void (*ap_reg_write)(ADIv5_AP_t *ap, int num, uint32_t value);
+	void (*read_block)(uint32_t addr, uint8_t *data, int size);
+	void (*dap_write_block_sized)(uint32_t addr, uint8_t *data,
+								  int size, enum align align);
+
+#endif
+	uint32_t (*ap_read)(ADIv5_AP_t *ap, uint16_t addr);
+	void (*ap_write)(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
+
+	void (*mem_read)(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len);
+	void (*mem_write_sized)(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+							size_t len, enum align align);
 #if PC_HOSTED == 1
 	jtag_dev_t *dev;
 	uint8_t fault;
@@ -174,7 +193,7 @@ static inline void adiv5_dp_abort(struct ADIv5_DP_s *dp, uint32_t abort)
 	return dp->abort(dp, abort);
 }
 
-typedef struct ADIv5_AP_s {
+struct ADIv5_AP_s {
 	int refcnt;
 
 	ADIv5_DP_t *dp;
@@ -183,25 +202,42 @@ typedef struct ADIv5_AP_s {
 	uint32_t idr;
 	uint32_t base;
 	uint32_t csw;
-} ADIv5_AP_t;
+};
+
+static inline uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
+{
+	return ap->dp->ap_read(ap, addr);
+}
+
+static inline void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+{
+	return ap->dp->ap_write(ap, addr, value);
+}
+
+static inline void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src,
+								  size_t len)
+{
+	return ap->dp->mem_read(ap, dest, src, len);
+}
+static inline void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest,
+								  const void *src, size_t len, enum align align)
+{
+	return ap->dp->mem_write_sized(ap, dest, src, len, align);
+}
 
-void adiv5_dp_init(ADIv5_DP_t *dp);
 void adiv5_dp_write(ADIv5_DP_t *dp, uint16_t addr, uint32_t value);
-
+void adiv5_dp_init(ADIv5_DP_t *dp);
+void platform_adiv5_dp_defaults(ADIv5_DP_t *dp);
 ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel);
 void adiv5_ap_ref(ADIv5_AP_t *ap);
 void adiv5_ap_unref(ADIv5_AP_t *ap);
 
 void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
-uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr);
 
 void adiv5_jtag_dp_handler(jtag_dev_t *dev);
 int platform_jtag_dp_init(ADIv5_DP_t *dp);
 
-void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len);
 void adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len);
-void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
-						   size_t len, enum align align);
 uint64_t adiv5_ap_read_pidr(ADIv5_AP_t *ap, uint32_t addr);
 void * extract(void *dest, uint32_t src, uint32_t val, enum align align);
 #endif
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index 051befd..a0abf85 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -478,88 +478,98 @@ static void cortexm_regs_read(target *t, void *data)
 	uint32_t *regs = data;
 	ADIv5_AP_t *ap = cortexm_ap(t);
 	unsigned i;
-#if defined(STLINKV2)
-	uint32_t base_regs[21];
-	extern void stlink_regs_read(ADIv5_AP_t *ap, void *data);
-	extern uint32_t stlink_reg_read(ADIv5_AP_t *ap, int idx);
-	stlink_regs_read(ap, base_regs);
-	for(i = 0; i < sizeof(regnum_cortex_m) / 4; i++)
-		*regs++ = base_regs[regnum_cortex_m[i]];
-	if (t->target_options & TOPT_FLAVOUR_V7MF)
-		for(size_t t = 0; t < sizeof(regnum_cortex_mf) / 4; t++)
-			*regs++ = stlink_reg_read(ap, regnum_cortex_mf[t]);
-#else
-	/* FIXME: Describe what's really going on here */
-	adiv5_ap_write(ap, ADIV5_AP_CSW, ap->csw | ADIV5_AP_CSW_SIZE_WORD);
-
-	/* Map the banked data registers (0x10-0x1c) to the
-	 * debug registers DHCSR, DCRSR, DCRDR and DEMCR respectively */
-	adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_TAR, CORTEXM_DHCSR);
-
-	/* Walk the regnum_cortex_m array, reading the registers it
-	 * calls out. */
-	adiv5_ap_write(ap, ADIV5_AP_DB(DB_DCRSR), regnum_cortex_m[0]); /* Required to switch banks */
-	*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
-	for(i = 1; i < sizeof(regnum_cortex_m) / 4; i++) {
-		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DB(DB_DCRSR),
-		                    regnum_cortex_m[i]);
-		*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
+#if PC_HOSTED == 1
+	if ((ap->dp->ap_reg_read) && (ap->dp->ap_regs_read)) {
+		uint32_t base_regs[21];
+		ap->dp->ap_regs_read(ap, base_regs);
+		for(i = 0; i < sizeof(regnum_cortex_m) / 4; i++)
+			*regs++ = base_regs[regnum_cortex_m[i]];
+		if (t->target_options & TOPT_FLAVOUR_V7MF)
+			for(size_t t = 0; t < sizeof(regnum_cortex_mf) / 4; t++)
+			*regs++ = ap->dp->ap_reg_read(ap, regnum_cortex_mf[t]);
 	}
-	if (t->target_options & TOPT_FLAVOUR_V7MF)
-		for(i = 0; i < sizeof(regnum_cortex_mf) / 4; i++) {
-			adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
-			                    ADIV5_AP_DB(DB_DCRSR),
-			                    regnum_cortex_mf[i]);
-			*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
-		}
+#else
+	if (0) {}
 #endif
+	else {
+		/* FIXME: Describe what's really going on here */
+		adiv5_ap_write(ap, ADIV5_AP_CSW, ap->csw | ADIV5_AP_CSW_SIZE_WORD);
+
+		/* Map the banked data registers (0x10-0x1c) to the
+		 * debug registers DHCSR, DCRSR, DCRDR and DEMCR respectively */
+		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_TAR,
+							CORTEXM_DHCSR);
+
+		/* Walk the regnum_cortex_m array, reading the registers it
+		 * calls out. */
+		adiv5_ap_write(ap, ADIV5_AP_DB(DB_DCRSR), regnum_cortex_m[0]);
+       /* Required to switch banks */
+		*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
+		for(i = 1; i < sizeof(regnum_cortex_m) / 4; i++) {
+		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DB(DB_DCRSR),
+			regnum_cortex_m[i]);
+		*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
+		}
+		if (t->target_options & TOPT_FLAVOUR_V7MF)
+			for(i = 0; i < sizeof(regnum_cortex_mf) / 4; i++) {
+				adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
+									ADIV5_AP_DB(DB_DCRSR),
+									regnum_cortex_mf[i]);
+				*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
+			}
+		}
 }
 
 static void cortexm_regs_write(target *t, const void *data)
 {
 	const uint32_t *regs = data;
 	ADIv5_AP_t *ap = cortexm_ap(t);
-#if defined(STLINKV2)
-	extern void stlink_reg_write(ADIv5_AP_t *ap, int num, uint32_t val);
-	for(size_t z = 0; z < sizeof(regnum_cortex_m) / 4; z++) {
-		stlink_reg_write(ap, regnum_cortex_m[z], *regs);
-		regs++;
-	}
-	if (t->target_options & TOPT_FLAVOUR_V7MF)
-		for(size_t z = 0; z < sizeof(regnum_cortex_mf) / 4; z++) {
-			stlink_reg_write(ap, regnum_cortex_mf[z], *regs);
+#if PC_HOSTED == 1
+	if (ap->dp->ap_reg_write) {
+		for (size_t z = 0; z < sizeof(regnum_cortex_m) / 4; z++) {
+			ap->dp->ap_reg_write(ap, regnum_cortex_m[z], *regs);
 			regs++;
+		}
+		if (t->target_options & TOPT_FLAVOUR_V7MF)
+			for(size_t z = 0; z < sizeof(regnum_cortex_mf) / 4; z++) {
+				ap->dp->ap_reg_write(ap, regnum_cortex_mf[z], *regs);
+				regs++;
+			}
 	}
 #else
-	unsigned i;
-
-	/* FIXME: Describe what's really going on here */
-	adiv5_ap_write(ap, ADIV5_AP_CSW, ap->csw | ADIV5_AP_CSW_SIZE_WORD);
-
-	/* Map the banked data registers (0x10-0x1c) to the
-	 * debug registers DHCSR, DCRSR, DCRDR and DEMCR respectively */
-	adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_TAR, CORTEXM_DHCSR);
-
-	/* Walk the regnum_cortex_m array, writing the registers it
-	 * calls out. */
-	adiv5_ap_write(ap, ADIV5_AP_DB(DB_DCRDR), *regs++); /* Required to switch banks */
-	adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DB(DB_DCRSR),
-	                    0x10000 | regnum_cortex_m[0]);
-	for(i = 1; i < sizeof(regnum_cortex_m) / 4; i++) {
-		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
-		                    ADIV5_AP_DB(DB_DCRDR), *regs++);
-		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DB(DB_DCRSR),
-		                    0x10000 | regnum_cortex_m[i]);
-	}
-	if (t->target_options & TOPT_FLAVOUR_V7MF)
-		for(i = 0; i < sizeof(regnum_cortex_mf) / 4; i++) {
-			adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
-			                    ADIV5_AP_DB(DB_DCRDR), *regs++);
-			adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
-			                    ADIV5_AP_DB(DB_DCRSR),
-			                    0x10000 | regnum_cortex_mf[i]);
-		}
+	if (0) {}
 #endif
+	else {
+		unsigned i;
+
+		/* FIXME: Describe what's really going on here */
+		adiv5_ap_write(ap, ADIV5_AP_CSW, ap->csw | ADIV5_AP_CSW_SIZE_WORD);
+
+		/* Map the banked data registers (0x10-0x1c) to the
+		 * debug registers DHCSR, DCRSR, DCRDR and DEMCR respectively */
+		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_TAR,
+							CORTEXM_DHCSR);
+		/* Walk the regnum_cortex_m array, writing the registers it
+		 * calls out. */
+		adiv5_ap_write(ap, ADIV5_AP_DB(DB_DCRDR), *regs++);
+        /* Required to switch banks */
+		adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DB(DB_DCRSR),
+							0x10000 | regnum_cortex_m[0]);
+		for(i = 1; i < sizeof(regnum_cortex_m) / 4; i++) {
+			adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
+								ADIV5_AP_DB(DB_DCRDR), *regs++);
+			adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_AP_DB(DB_DCRSR),
+								0x10000 | regnum_cortex_m[i]);
+		}
+		if (t->target_options & TOPT_FLAVOUR_V7MF)
+			for(i = 0; i < sizeof(regnum_cortex_mf) / 4; i++) {
+				adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
+									ADIV5_AP_DB(DB_DCRDR), *regs++);
+				adiv5_dp_low_access(ap->dp, ADIV5_LOW_WRITE,
+									ADIV5_AP_DB(DB_DCRSR),
+									0x10000 | regnum_cortex_mf[i]);
+			}
+	}
 }
 
 int cortexm_mem_write_sized(

From 9a46c6fd25cf41bca5a6073b8c0ea101ebb85cf9 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 18 Apr 2020 14:17:03 +0200
Subject: [PATCH 19/43] Allow BOARD_IDENT as function

---
 src/command.c                        | 2 +-
 src/platforms/libftdi/platform.h     | 2 +-
 src/platforms/pc-hosted/platform.h   | 2 +-
 src/platforms/pc-stlinkv2/platform.h | 2 +-
 src/remote.c                         | 2 +-
 5 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/command.c b/src/command.c
index 4d0d1e5..9f9878d 100644
--- a/src/command.c
+++ b/src/command.c
@@ -136,7 +136,7 @@ bool cmd_version(target *t, int argc, char **argv)
 	(void)argc;
 	(void)argv;
 #if PC_HOSTED == 1
-	gdb_outf("Black Magic Probe, PC-Hosted for " PLATFORM_IDENT
+	gdb_outf("Black Magic Probe, PC-Hosted for " PLATFORM_IDENT()
 			 ", Version " FIRMWARE_VERSION "\n");
 #else
 	gdb_outf("Black Magic Probe (Firmware " FIRMWARE_VERSION ") (Hardware Version %d)\n", platform_hwversion());
diff --git a/src/platforms/libftdi/platform.h b/src/platforms/libftdi/platform.h
index 122cdce..d174aaf 100644
--- a/src/platforms/libftdi/platform.h
+++ b/src/platforms/libftdi/platform.h
@@ -38,7 +38,7 @@
 
 #define PLATFORM_HAS_DEBUG
 
-#define PLATFORM_IDENT "FTDI/MPSSE"
+#define PLATFORM_IDENT() "FTDI/MPSSE"
 #define SET_RUN_STATE(state)
 #define SET_IDLE_STATE(state)
 #define SET_ERROR_STATE(state)
diff --git a/src/platforms/pc-hosted/platform.h b/src/platforms/pc-hosted/platform.h
index 8b1c71b..535e9ea 100644
--- a/src/platforms/pc-hosted/platform.h
+++ b/src/platforms/pc-hosted/platform.h
@@ -35,7 +35,7 @@
 #define PLATFORM_HAS_DEBUG
 #define PLATFORM_HAS_POWER_SWITCH
 #define PLATFORM_MAX_MSG_SIZE (256)
-#define PLATFORM_IDENT "PC-HOSTED"
+#define PLATFORM_IDENT() "PC-HOSTED"
 #define BOARD_IDENT PLATFORM_IDENT
 #define SET_RUN_STATE(state)
 #define SET_IDLE_STATE(state)
diff --git a/src/platforms/pc-stlinkv2/platform.h b/src/platforms/pc-stlinkv2/platform.h
index c3f0bba..4535c74 100644
--- a/src/platforms/pc-stlinkv2/platform.h
+++ b/src/platforms/pc-stlinkv2/platform.h
@@ -35,7 +35,7 @@
 
 #define PLATFORM_HAS_DEBUG
 
-#define PLATFORM_IDENT "StlinkV2/3"
+#define PLATFORM_IDENT() "StlinkV2/3"
 #define SET_RUN_STATE(state)
 void stlink_check_detach(int state);
 #define SET_IDLE_STATE(state) stlink_check_detach(state)
diff --git a/src/remote.c b/src/remote.c
index 30cd075..a1ade3b 100644
--- a/src/remote.c
+++ b/src/remote.c
@@ -243,7 +243,7 @@ void remotePacketProcessGEN(uint8_t i, char *packet)
 		break;
 
 #if !defined(BOARD_IDENT) && defined(PLATFORM_IDENT)
-# define BOARD_IDENT PLATFORM_IDENT
+# define BOARD_IDENT() PLATFORM_IDENT
 #endif
 	case REMOTE_START:
 		_respondS(REMOTE_RESP_OK, BOARD_IDENT " " FIRMWARE_VERSION);

From c3d509e6c0ec25c9e1cc766970e446cc9bba2f9b Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 18 Apr 2020 17:38:17 +0200
Subject: [PATCH 20/43] Clean up PLATFORM_HAS_DEBUG

Use only for firmware platforms.
---
 src/platforms/libftdi/platform.h     |  2 --
 src/platforms/pc-hosted/platform.h   |  1 -
 src/platforms/pc-stlinkv2/platform.h |  2 --
 src/target/adiv5.c                   | 10 +++++-----
 src/target/efm32.c                   |  2 +-
 5 files changed, 6 insertions(+), 11 deletions(-)

diff --git a/src/platforms/libftdi/platform.h b/src/platforms/libftdi/platform.h
index d174aaf..d4368ba 100644
--- a/src/platforms/libftdi/platform.h
+++ b/src/platforms/libftdi/platform.h
@@ -36,8 +36,6 @@
 #define FT2232_VID	0x0403
 #define FT2232_PID	0x6010
 
-#define PLATFORM_HAS_DEBUG
-
 #define PLATFORM_IDENT() "FTDI/MPSSE"
 #define SET_RUN_STATE(state)
 #define SET_IDLE_STATE(state)
diff --git a/src/platforms/pc-hosted/platform.h b/src/platforms/pc-hosted/platform.h
index 535e9ea..696639b 100644
--- a/src/platforms/pc-hosted/platform.h
+++ b/src/platforms/pc-hosted/platform.h
@@ -32,7 +32,6 @@
 #	endif
 #endif
 
-#define PLATFORM_HAS_DEBUG
 #define PLATFORM_HAS_POWER_SWITCH
 #define PLATFORM_MAX_MSG_SIZE (256)
 #define PLATFORM_IDENT() "PC-HOSTED"
diff --git a/src/platforms/pc-stlinkv2/platform.h b/src/platforms/pc-stlinkv2/platform.h
index 4535c74..b63b0e1 100644
--- a/src/platforms/pc-stlinkv2/platform.h
+++ b/src/platforms/pc-stlinkv2/platform.h
@@ -33,8 +33,6 @@
 #	endif
 #endif
 
-#define PLATFORM_HAS_DEBUG
-
 #define PLATFORM_IDENT() "StlinkV2/3"
 #define SET_RUN_STATE(state)
 void stlink_check_detach(int state);
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index edac3c7..973870e 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -74,7 +74,7 @@ enum cid_class {
 	cidc_unknown = 0x10
 };
 
-#ifdef PLATFORM_HAS_DEBUG
+#ifdef ENABLE_DEBUG
 /* The reserved ones only have an R in them, to save a bit of space. */
 static const char * const cidc_debug_strings[] =
 {
@@ -111,7 +111,7 @@ enum arm_arch {
 	aa_end
 };
 
-#ifdef PLATFORM_HAS_DEBUG
+#ifdef ENABLE_DEBUG
 #define PIDR_PN_BIT_STRINGS(...) __VA_ARGS__
 #else
 #define PIDR_PN_BIT_STRINGS(...)
@@ -155,7 +155,7 @@ static const struct {
 	uint16_t part_number;
 	enum arm_arch arch;
 	enum cid_class cidc;
-#ifdef PLATFORM_HAS_DEBUG
+#ifdef ENABLE_DEBUG
 	const char *type;
 	const char *full;
 #endif
@@ -277,7 +277,7 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 	uint64_t pidr = adiv5_ap_read_pidr(ap, addr);
 	uint32_t cidr = adiv5_ap_read_id(ap, addr + CIDR0_OFFSET);
 	bool res = false;
-#if defined(ENABLE_DEBUG) && defined(PLATFORM_HAS_DEBUG)
+#if defined(ENABLE_DEBUG)
 	char indent[recursion + 1];
 
 	for(int i = 0; i < recursion; i++) indent[i] = ' ';
@@ -303,7 +303,7 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 	/* ROM table */
 	if (cid_class == cidc_romtab) {
 		/* Check SYSMEM bit */
-#if defined(ENABLE_DEBUG) && defined(PLATFORM_HAS_DEBUG)
+#if defined(ENABLE_DEBUG)
 		uint32_t memtype = adiv5_mem_read32(ap, addr | ADIV5_ROM_MEMTYPE) &
 			ADIV5_ROM_MEMTYPE_SYSMEM;
 
diff --git a/src/target/efm32.c b/src/target/efm32.c
index ba40b43..82125e3 100644
--- a/src/target/efm32.c
+++ b/src/target/efm32.c
@@ -720,7 +720,7 @@ static int efm32_flash_write(struct target_flash *f,
 	int ret = cortexm_run_stub(t, SRAM_BASE, dest, STUB_BUFFER_BASE, len,
 							   device->msc_addr);
 
-#ifdef PLATFORM_HAS_DEBUG
+#ifdef ENABLE_DEBUG
 	/* Check the MSC_IF */
 	uint32_t msc = device->msc_addr;
 	uint32_t msc_if = target_mem_read32(t, EFM32_MSC_IF(msc));

From 3290def5f844866837f7d4422ffcc5f01b747e9c Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 18 Apr 2020 16:50:23 +0200
Subject: [PATCH 21/43] Add empty compilable project.

---
 src/platforms/hosted/Makefile.inc | 12 ++++
 src/platforms/hosted/jtagtap.c    |  0
 src/platforms/hosted/platform.c   | 92 +++++++++++++++++++++++++++++++
 src/platforms/hosted/platform.h   | 11 ++++
 src/platforms/hosted/swdptap.c    |  0
 5 files changed, 115 insertions(+)
 create mode 100644 src/platforms/hosted/Makefile.inc
 create mode 100644 src/platforms/hosted/jtagtap.c
 create mode 100644 src/platforms/hosted/platform.c
 create mode 100644 src/platforms/hosted/platform.h
 create mode 100644 src/platforms/hosted/swdptap.c

diff --git a/src/platforms/hosted/Makefile.inc b/src/platforms/hosted/Makefile.inc
new file mode 100644
index 0000000..7c96aa8
--- /dev/null
+++ b/src/platforms/hosted/Makefile.inc
@@ -0,0 +1,12 @@
+SYS = $(shell $(CC) -dumpmachine)
+CFLAGS += -DSTLINKV2 -DJTAG_HL -DENABLE_DEBUG
+CFLAGS +=-I ./target -I./platforms/pc
+LDFLAGS += -lusb-1.0
+ifneq (, $(findstring mingw, $(SYS)))
+LDFLAGS += -lws2_32
+else ifneq (, $(findstring cygwin, $(SYS)))
+LDFLAGS += -lws2_32
+endif
+VPATH += platforms/pc
+SRC += timing.c cl_utils.c utils.c
+PC_HOSTED = 1
diff --git a/src/platforms/hosted/jtagtap.c b/src/platforms/hosted/jtagtap.c
new file mode 100644
index 0000000..e69de29
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
new file mode 100644
index 0000000..3c7df6d
--- /dev/null
+++ b/src/platforms/hosted/platform.c
@@ -0,0 +1,92 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2019
+ * Written by 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/>.
+ */
+
+/* Handle different BMP pc-hosted platforms/
+ */
+
+#include "general.h"
+#include "swdptap.h"
+#include "jtagtap.h"
+
+typedef enum bmp_t{
+	BMP_TYPE_NONE = 0
+}bmp_t;
+
+bmp_t active_bmp = BMP_TYPE_NONE;
+swd_proc_t swd_proc;
+jtag_proc_t jtag_proc;
+
+void platform_init(int argc, char **argv)
+{
+	(void) argc;
+	(void) argv;
+	exit(-1);
+}
+
+int platform_adiv5_swdp_scan(void)
+{
+	return -1;
+}
+
+int platform_swdptap_init(void)
+{
+	return -1;
+}
+
+int platform_jtag_scan(const uint8_t *lrlens)
+{
+	(void) lrlens;
+	return -1;
+}
+
+int platform_jtagtap_init(void)
+{
+	return 0;
+}
+
+int platform_adiv5_dp_defaults(void *arg)
+{
+	(void)arg;
+	return -1;
+}
+
+int platform_jtag_dp_init()
+{
+        return 0;
+}
+
+char *platform_ident(void)
+{
+	switch (active_bmp) {
+	  case BMP_TYPE_NONE:
+		return "NONE";
+	}
+	return NULL;
+}
+
+const char *platform_target_voltage(void)
+{
+	return NULL;
+}
+
+void platform_srst_set_val(bool assert) {(void) assert;}
+
+bool platform_srst_get_val(void) { return false;}
+void platform_buffer_flush(void) {}
diff --git a/src/platforms/hosted/platform.h b/src/platforms/hosted/platform.h
new file mode 100644
index 0000000..545d7c1
--- /dev/null
+++ b/src/platforms/hosted/platform.h
@@ -0,0 +1,11 @@
+#ifndef __PLATFORM_H
+#include "timing.h"
+
+char *platform_ident(void);
+void platform_buffer_flush(void);
+
+#define PLATFORM_IDENT() "NONE"
+#define SET_IDLE_STATE(x)
+#define SET_RUN_STATE(x)
+
+#endif
diff --git a/src/platforms/hosted/swdptap.c b/src/platforms/hosted/swdptap.c
new file mode 100644
index 0000000..e69de29

From 386a4bfce78dc79101d4a9a8b82e4d2f12eda1de Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 18 Apr 2020 17:54:00 +0200
Subject: [PATCH 22/43] hosted: Device detection and selection.

---
 src/Makefile                      |   2 +-
 src/platforms/hosted/Makefile.inc |   2 +-
 src/platforms/hosted/platform.c   | 189 ++++++++++++++++++++++++++++--
 src/platforms/hosted/platform.h   |  22 ++++
 4 files changed, 205 insertions(+), 10 deletions(-)

diff --git a/src/Makefile b/src/Makefile
index 9430501..6c9d931 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -62,7 +62,7 @@ SRC =			\
 
 include $(PLATFORM_DIR)/Makefile.inc
 
-OPT_FLAGS ?= -O2
+OPT_FLAGS ?= -Og
 CFLAGS += $(OPT_FLAGS)
 LDFLAGS += $(OPT_FLAGS)
 
diff --git a/src/platforms/hosted/Makefile.inc b/src/platforms/hosted/Makefile.inc
index 7c96aa8..fc00abe 100644
--- a/src/platforms/hosted/Makefile.inc
+++ b/src/platforms/hosted/Makefile.inc
@@ -1,5 +1,5 @@
 SYS = $(shell $(CC) -dumpmachine)
-CFLAGS += -DSTLINKV2 -DJTAG_HL -DENABLE_DEBUG
+CFLAGS += -DENABLE_DEBUG -DPLATFORM_HAS_DEBUG
 CFLAGS +=-I ./target -I./platforms/pc
 LDFLAGS += -lusb-1.0
 ifneq (, $(findstring mingw, $(SYS)))
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 3c7df6d..a10a7a3 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2019
+ * Copyright (C) 2020
  * Written by Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
  *
  * This program is free software: you can redistribute it and/or modify
@@ -24,19 +24,182 @@
 #include "general.h"
 #include "swdptap.h"
 #include "jtagtap.h"
+#include "timing.h"
+#include "cl_utils.h"
+#include <signal.h>
 
-typedef enum bmp_t{
-	BMP_TYPE_NONE = 0
-}bmp_t;
+#define VENDOR_ID_BMP            0x1d50
+#define PRODUCT_ID_BMP           0x6018
+
+#define VENDOR_ID_STLINK         0x0483
+#define PRODUCT_ID_STLINK_MASK   0xffe0
+#define PRODUCT_ID_STLINK_GROUP  0x3740
+#define PRODUCT_ID_STLINKV1      0x3744
+#define PRODUCT_ID_STLINKV2      0x3748
+#define PRODUCT_ID_STLINKV21     0x374b
+#define PRODUCT_ID_STLINKV21_MSD 0x3752
+#define PRODUCT_ID_STLINKV3      0x374f
+#define PRODUCT_ID_STLINKV3E     0x374e
+
+bmp_info_t info;
 
-bmp_t active_bmp = BMP_TYPE_NONE;
 swd_proc_t swd_proc;
 jtag_proc_t jtag_proc;
 
+static void exit_function(void)
+{
+	libusb_exit(info.libusb_ctx);
+	fprintf(stderr, "INFO: Cleanup\n");
+}
+
+/* SIGTERM handler. */
+static void sigterm_handler(int sig)
+{
+	(void)sig;
+	exit(0);
+}
+
+static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
+{
+	libusb_device **devs;
+	int n_devs = libusb_get_device_list(info->libusb_ctx, &devs);
+    if (n_devs < 0) {
+        fprintf(stderr, "WARN:libusb_get_device_list() failed");
+		return -1;
+	}
+	bool report = false;
+	int found_debuggers;
+	struct libusb_device_descriptor desc;
+	char serial[64];
+	char manufacturer[128];
+	char product[128];
+	bmp_type_t type = BMP_TYPE_NONE;
+  rescan:
+	found_debuggers = 0;
+	for (int i = 0;  devs[i]; i++) {
+		libusb_device *dev =  devs[i];
+		int res = libusb_get_device_descriptor(dev, &desc);
+		if (res < 0) {
+            fprintf(stderr, "WARN: libusb_get_device_descriptor() failed: %s",
+					libusb_strerror(res));
+			libusb_free_device_list(devs, 0);
+			continue;
+		}
+		libusb_device_handle *handle;
+		res = libusb_open(dev, &handle);
+		if (res != LIBUSB_SUCCESS) {
+			fprintf(stderr,"WARN: Open failed\n");
+			continue;
+		}
+		res = libusb_get_string_descriptor_ascii(
+			handle, desc.iSerialNumber, (uint8_t*)serial,
+			sizeof(serial));
+		if (res <= 0) {
+			/* This can fail for many devices. Continue silent!*/
+			libusb_close(handle);
+			continue;
+		}
+		if (cl_opts->opt_serial && !strstr(serial, cl_opts->opt_serial)) {
+			libusb_close(handle);
+			continue;
+		}
+		res = libusb_get_string_descriptor_ascii(
+			handle, desc.iManufacturer, (uint8_t*)manufacturer,
+			sizeof(manufacturer));
+		if (res > 0) {
+			res = libusb_get_string_descriptor_ascii(
+				handle, desc.iProduct, (uint8_t*)product,
+				sizeof(product));
+			if (res <= 0) {
+				fprintf(stderr, "WARN:"
+						"libusb_get_string_descriptor_ascii "
+						"for ident_string failed: %s\n",
+						libusb_strerror(res));
+				libusb_close(handle);
+				continue;
+			}
+		}
+		if (cl_opts->opt_ident_string) {
+			char *match_manu = NULL;
+			char *match_product = NULL;
+			match_manu = strstr(manufacturer,	cl_opts->opt_ident_string);
+			match_product = strstr(product, cl_opts->opt_ident_string);
+			if (!match_manu && !match_product) {
+				libusb_close(handle);
+				continue;
+			}
+		}
+		/* Either serial and/or ident_string match or are not given.
+		 * Check type.*/
+		if ((desc.idVendor == VENDOR_ID_BMP) &&
+			(desc.idProduct == PRODUCT_ID_BMP)) {
+			type = BMP_TYPE_BMP;
+		} else if (desc.idVendor ==  VENDOR_ID_STLINK) {
+			if (desc.idProduct == PRODUCT_ID_STLINKV1) {
+				fprintf(stderr, "INFO: STLINKV1 not supported\n");
+				continue;
+			}
+			if ((desc.idProduct == PRODUCT_ID_STLINKV2) ||
+				(desc.idProduct == PRODUCT_ID_STLINKV21) ||
+				(desc.idProduct == PRODUCT_ID_STLINKV21_MSD) ||
+				(desc.idProduct == PRODUCT_ID_STLINKV3) ||
+				(desc.idProduct == PRODUCT_ID_STLINKV3E)) {
+				type = BMP_TYPE_STLINKV2;
+			}
+		} else {
+			continue;
+		}
+		found_debuggers ++;
+		if (report) {
+			printf("%2d: %s, %s, %s\n", found_debuggers,
+				   serial,
+				   manufacturer,product);
+		}
+		info->vid = desc.idVendor;
+		info->pid = desc.idProduct;
+		info->bmp_type = type;
+		strncpy(info->serial, serial, sizeof(info->serial));
+		strncpy(info->product, product, sizeof(info->product));
+		strncpy(info->manufacturer, manufacturer, sizeof(info->manufacturer));
+		if (cl_opts->opt_position &&
+			(cl_opts->opt_position == found_debuggers)) {
+			found_debuggers = 1;
+			break;
+		}
+	}
+	if (found_debuggers > 1) {
+		if (!report) {
+			printf("%d debuggers found! Select with -P <num>, -s <string> "
+				   "and/or -S <string>\n",
+				   found_debuggers);
+			report = true;
+			goto rescan;
+		}
+	}
+	libusb_free_device_list(devs, 0);
+	return 0;
+}
+
 void platform_init(int argc, char **argv)
 {
-	(void) argc;
-	(void) argv;
+	BMP_CL_OPTIONS_t cl_opts = {0};
+	cl_opts.opt_idstring = "Blackmagic PC-Hosted";
+	cl_init(&cl_opts, argc, argv);
+	atexit(exit_function);
+	signal(SIGTERM, sigterm_handler);
+	signal(SIGINT, sigterm_handler);
+	int res = libusb_init(&info.libusb_ctx);
+	if (res) {
+		fprintf(stderr, "Fatal: Failed to get USB context: %s\n",
+				libusb_strerror(res));
+		exit(-1);
+	}
+	if (find_debuggers(&cl_opts, &info)) {
+		exit(-1);
+	}
+	printf("Using %s %s %s\n", info.serial,
+		   info.manufacturer,
+		   info.product);
 	exit(-1);
 }
 
@@ -74,9 +237,19 @@ int platform_jtag_dp_init()
 
 char *platform_ident(void)
 {
-	switch (active_bmp) {
+	switch (info.bmp_type) {
 	  case BMP_TYPE_NONE:
 		return "NONE";
+	  case BMP_TYPE_BMP:
+		return "BMP";
+	  case BMP_TYPE_STLINKV2:
+		return "STLINKV2";
+	  case BMP_TYPE_LIBFTDI:
+		return "LIBFTDI";
+	  case BMP_TYPE_CMSIS_DAP:
+		return "CMSIS_DAP";
+	  case BMP_TYPE_JLINK:
+		return "JLINK";
 	}
 	return NULL;
 }
diff --git a/src/platforms/hosted/platform.h b/src/platforms/hosted/platform.h
index 545d7c1..0a28110 100644
--- a/src/platforms/hosted/platform.h
+++ b/src/platforms/hosted/platform.h
@@ -1,4 +1,6 @@
 #ifndef __PLATFORM_H
+#define __PLATFORM_H
+#include <libusb-1.0/libusb.h>
 #include "timing.h"
 
 char *platform_ident(void);
@@ -8,4 +10,24 @@ void platform_buffer_flush(void);
 #define SET_IDLE_STATE(x)
 #define SET_RUN_STATE(x)
 
+typedef enum bmp_type_s {
+	BMP_TYPE_NONE = 0,
+	BMP_TYPE_BMP,
+	BMP_TYPE_STLINKV2,
+	BMP_TYPE_LIBFTDI,
+	BMP_TYPE_CMSIS_DAP,
+	BMP_TYPE_JLINK
+} bmp_type_t;
+
+typedef struct bmp_info_s {
+	bmp_type_t bmp_type;
+	libusb_context *libusb_ctx;
+	unsigned int vid;
+	unsigned int pid;
+	char dev;
+	char serial[64];
+	char manufacturer[128];
+	char product[128];
+} bmp_info_t;
+
 #endif

From 52739def0187596c605145ab271f194947485389 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sun, 19 Apr 2020 11:55:56 +0200
Subject: [PATCH 23/43] Copy relevant stlink files.

---
 src/platforms/hosted/stlinkv2.c | 1437 +++++++++++++++++++++++++++++++
 src/platforms/hosted/stlinkv2.h |   52 ++
 2 files changed, 1489 insertions(+)
 create mode 100644 src/platforms/hosted/stlinkv2.c
 create mode 100644 src/platforms/hosted/stlinkv2.h

diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
new file mode 100644
index 0000000..5f141d2
--- /dev/null
+++ b/src/platforms/hosted/stlinkv2.c
@@ -0,0 +1,1437 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2019 Uwe Bonnes
+ *
+ * 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/>.
+ */
+/* Much code and ideas shamelessly taken form
+ * https://github.com/texane/stlink.git
+ * git://git.code.sf.net/p/openocd/code
+ * https://github.com/pavelrevak/pystlink
+ * https://github.com/pavelrevak/pystlink
+ *
+ * with some contribution.
+ */
+#include "general.h"
+#include "gdb_if.h"
+#include "adiv5.h"
+#include "stlinkv2.h"
+#include "exception.h"
+#include "jtag_devs.h"
+#include "target.h"
+
+#include <assert.h>
+#include <unistd.h>
+#include <signal.h>
+#include <ctype.h>
+#include <sys/time.h>
+
+#include "cl_utils.h"
+
+#if !defined(timersub)
+/* This is a copy from GNU C Library (GNU LGPL 2.1), sys/time.h. */
+# define timersub(a, b, result)					\
+	do {										\
+		(result)->tv_sec = (a)->tv_sec - (b)->tv_sec;	\
+		(result)->tv_usec = (a)->tv_usec - (b)->tv_usec;	\
+		if ((result)->tv_usec < 0) {						\
+			--(result)->tv_sec;								\
+			(result)->tv_usec += 1000000;					\
+		}													\
+	} while (0)
+#endif
+
+#define VENDOR_ID_STLINK		0x483
+#define PRODUCT_ID_STLINK_MASK	0xffe0
+#define PRODUCT_ID_STLINK_GROUP 0x3740
+#define PRODUCT_ID_STLINKV1		0x3744
+#define PRODUCT_ID_STLINKV2		0x3748
+#define PRODUCT_ID_STLINKV21	0x374b
+#define PRODUCT_ID_STLINKV21_MSD 0x3752
+#define PRODUCT_ID_STLINKV3		0x374f
+#define PRODUCT_ID_STLINKV3E	0x374e
+
+#define STLINK_SWIM_ERR_OK             0x00
+#define STLINK_SWIM_BUSY               0x01
+#define STLINK_DEBUG_ERR_OK            0x80
+#define STLINK_DEBUG_ERR_FAULT         0x81
+#define STLINK_JTAG_UNKNOWN_JTAG_CHAIN 0x04
+#define STLINK_NO_DEVICE_CONNECTED     0x05
+#define STLINK_JTAG_COMMAND_ERROR      0x08
+#define STLINK_JTAG_COMMAND_ERROR      0x08
+#define STLINK_JTAG_GET_IDCODE_ERROR   0x09
+#define STLINK_JTAG_DBG_POWER_ERROR    0x0b
+#define STLINK_SWD_AP_WAIT             0x10
+#define STLINK_SWD_AP_FAULT            0x11
+#define STLINK_SWD_AP_ERROR            0x12
+#define STLINK_SWD_AP_PARITY_ERROR     0x13
+#define STLINK_JTAG_WRITE_ERROR        0x0c
+#define STLINK_JTAG_WRITE_VERIF_ERROR  0x0d
+#define STLINK_SWD_DP_WAIT             0x14
+#define STLINK_SWD_DP_FAULT            0x15
+#define STLINK_SWD_DP_ERROR            0x16
+#define STLINK_SWD_DP_PARITY_ERROR     0x17
+
+#define STLINK_SWD_AP_WDATA_ERROR      0x18
+#define STLINK_SWD_AP_STICKY_ERROR     0x19
+#define STLINK_SWD_AP_STICKYORUN_ERROR 0x1a
+#define STLINK_BAD_AP_ERROR            0x1d
+#define STLINK_TOO_MANY_AP_ERROR       0x29
+#define STLINK_JTAG_UNKNOWN_CMD        0x42
+
+#define STLINK_CORE_RUNNING            0x80
+#define STLINK_CORE_HALTED             0x81
+#define STLINK_CORE_STAT_UNKNOWN       -1
+
+#define STLINK_GET_VERSION             0xF1
+#define STLINK_DEBUG_COMMAND           0xF2
+#define STLINK_DFU_COMMAND             0xF3
+#define STLINK_SWIM_COMMAND            0xF4
+#define STLINK_GET_CURRENT_MODE        0xF5
+#define STLINK_GET_TARGET_VOLTAGE      0xF7
+
+#define STLINK_DEV_DFU_MODE            0x00
+#define STLINK_DEV_MASS_MODE           0x01
+#define STLINK_DEV_DEBUG_MODE          0x02
+#define STLINK_DEV_SWIM_MODE           0x03
+#define STLINK_DEV_BOOTLOADER_MODE     0x04
+#define STLINK_DEV_UNKNOWN_MODE        -1
+
+#define STLINK_DFU_EXIT                0x07
+
+#define STLINK_SWIM_ENTER                  0x00
+#define STLINK_SWIM_EXIT                   0x01
+#define STLINK_SWIM_READ_CAP               0x02
+#define STLINK_SWIM_SPEED                  0x03
+#define STLINK_SWIM_ENTER_SEQ              0x04
+#define STLINK_SWIM_GEN_RST                0x05
+#define STLINK_SWIM_RESET                  0x06
+#define STLINK_SWIM_ASSERT_RESET           0x07
+#define STLINK_SWIM_DEASSERT_RESET         0x08
+#define STLINK_SWIM_READSTATUS             0x09
+#define STLINK_SWIM_WRITEMEM               0x0a
+#define STLINK_SWIM_READMEM                0x0b
+#define STLINK_SWIM_READBUF                0x0c
+
+#define STLINK_DEBUG_GETSTATUS             0x01
+#define STLINK_DEBUG_FORCEDEBUG            0x02
+#define STLINK_DEBUG_APIV1_RESETSYS        0x03
+#define STLINK_DEBUG_APIV1_READALLREGS     0x04
+#define STLINK_DEBUG_APIV1_READREG         0x05
+#define STLINK_DEBUG_APIV1_WRITEREG        0x06
+#define STLINK_DEBUG_READMEM_32BIT         0x07
+#define STLINK_DEBUG_WRITEMEM_32BIT        0x08
+#define STLINK_DEBUG_RUNCORE               0x09
+#define STLINK_DEBUG_STEPCORE              0x0a
+#define STLINK_DEBUG_APIV1_SETFP           0x0b
+#define STLINK_DEBUG_READMEM_8BIT          0x0c
+#define STLINK_DEBUG_WRITEMEM_8BIT         0x0d
+#define STLINK_DEBUG_APIV1_CLEARFP         0x0e
+#define STLINK_DEBUG_APIV1_WRITEDEBUGREG   0x0f
+#define STLINK_DEBUG_APIV1_SETWATCHPOINT   0x10
+
+#define STLINK_DEBUG_ENTER_JTAG_RESET      0x00
+#define STLINK_DEBUG_ENTER_SWD_NO_RESET    0xa3
+#define STLINK_DEBUG_ENTER_JTAG_NO_RESET   0xa4
+
+#define STLINK_DEBUG_APIV1_ENTER           0x20
+#define STLINK_DEBUG_EXIT                  0x21
+#define STLINK_DEBUG_READCOREID            0x22
+
+#define STLINK_DEBUG_APIV2_ENTER           0x30
+#define STLINK_DEBUG_APIV2_READ_IDCODES    0x31
+#define STLINK_DEBUG_APIV2_RESETSYS        0x32
+#define STLINK_DEBUG_APIV2_READREG         0x33
+#define STLINK_DEBUG_APIV2_WRITEREG        0x34
+#define STLINK_DEBUG_APIV2_WRITEDEBUGREG   0x35
+#define STLINK_DEBUG_APIV2_READDEBUGREG    0x36
+
+#define STLINK_DEBUG_APIV2_READALLREGS     0x3A
+#define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
+#define STLINK_DEBUG_APIV2_DRIVE_NRST      0x3C
+
+#define STLINK_DEBUG_APIV2_GETLASTRWSTATUS2 0x3E
+
+#define STLINK_DEBUG_APIV2_START_TRACE_RX  0x40
+#define STLINK_DEBUG_APIV2_STOP_TRACE_RX   0x41
+#define STLINK_DEBUG_APIV2_GET_TRACE_NB    0x42
+#define STLINK_DEBUG_APIV2_SWD_SET_FREQ    0x43
+#define STLINK_DEBUG_APIV2_JTAG_SET_FREQ   0x44
+#define STLINK_DEBUG_APIV2_READ_DAP_REG    0x45
+#define STLINK_DEBUG_APIV2_WRITE_DAP_REG   0x46
+#define STLINK_DEBUG_APIV2_READMEM_16BIT   0x47
+#define STLINK_DEBUG_APIV2_WRITEMEM_16BIT  0x48
+
+#define STLINK_DEBUG_APIV2_INIT_AP         0x4B
+#define STLINK_DEBUG_APIV2_CLOSE_AP_DBG    0x4C
+
+#define STLINK_APIV3_SET_COM_FREQ           0x61
+#define STLINK_APIV3_GET_COM_FREQ           0x62
+
+#define STLINK_APIV3_GET_VERSION_EX         0xFB
+
+#define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW   0x00
+#define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
+#define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
+
+
+#define STLINK_TRACE_SIZE               4096
+#define STLINK_TRACE_MAX_HZ             2000000
+
+#define STLINK_V3_MAX_FREQ_NB               10
+
+/** */
+enum stlink_mode {
+	STLINK_MODE_UNKNOWN = 0,
+	STLINK_MODE_DFU,
+	STLINK_MODE_MASS,
+	STLINK_MODE_DEBUG_JTAG,
+	STLINK_MODE_DEBUG_SWD,
+	STLINK_MODE_DEBUG_SWIM
+};
+
+enum transport_mode_t{
+	STLINK_MODE_SWD = 0,
+	STLINK_MODE_JTAG
+};
+
+typedef struct {
+	libusb_context* libusb_ctx;
+	uint16_t     vid;
+	uint16_t     pid;
+	uint8_t      transport_mode;
+	char         serial[32];
+	uint8_t      dap_select;
+	uint8_t      ep_tx;
+	uint8_t      ver_hw;     /* 20, 21 or 31 deciphered from USB PID.*/
+	uint8_t      ver_stlink; /* 2 or 3  from API.*/
+	uint8_t      ver_api;
+	uint8_t      ver_jtag;
+	uint8_t      ver_mass;
+	uint8_t      ver_swim;
+	uint8_t      ver_bridge;
+	uint16_t     block_size;
+	bool         ap_error;
+	libusb_device_handle *handle;
+	struct libusb_transfer* req_trans;
+	struct libusb_transfer* rep_trans;
+} stlink;
+
+stlink Stlink;
+
+static int stlink_usb_get_rw_status(bool verbose);
+
+static void exit_function(void)
+{
+	libusb_exit(NULL);
+	DEBUG("Cleanup\n");
+}
+
+/* SIGTERM handler. */
+static void sigterm_handler(int sig)
+{
+	(void)sig;
+	exit(0);
+}
+
+struct trans_ctx {
+#define TRANS_FLAGS_IS_DONE (1 << 0)
+#define TRANS_FLAGS_HAS_ERROR (1 << 1)
+    volatile unsigned long flags;
+};
+
+int debug_level = 0;
+bool has_attached = false;
+
+static int LIBUSB_CALL hotplug_callback_attach(
+	libusb_context *ctx, libusb_device *dev, libusb_hotplug_event event,
+	void *user_data)
+{
+	(void)ctx;
+	(void)dev;
+	(void)event;
+	(void)user_data;
+	has_attached = true;
+	return 1; /* deregister Callback*/
+}
+
+int device_detached = 0;
+static int LIBUSB_CALL hotplug_callback_detach(
+	libusb_context *ctx, libusb_device *dev, libusb_hotplug_event event,
+	void *user_data)
+{
+	(void)ctx;
+	(void)dev;
+	(void)event;
+	(void)user_data;
+	device_detached = 1;
+	return 1;  /* deregister Callback*/
+}
+
+void stlink_check_detach(int state)
+{
+	if (state == 1) {
+		/* Check for hotplug events */
+		struct timeval tv = {0,0};
+		libusb_handle_events_timeout_completed(
+			Stlink.libusb_ctx, &tv, &device_detached);
+		if (device_detached) {
+			DEBUG("Dongle was detached\n");
+			exit(0);
+		}
+	}
+}
+
+static void LIBUSB_CALL on_trans_done(struct libusb_transfer * trans)
+{
+    struct trans_ctx * const ctx = trans->user_data;
+
+    if (trans->status != LIBUSB_TRANSFER_COMPLETED)
+    {
+		DEBUG("on_trans_done: ");
+        if(trans->status == LIBUSB_TRANSFER_TIMED_OUT)
+        {
+            DEBUG("Timeout\n");
+        } else if (trans->status == LIBUSB_TRANSFER_CANCELLED) {
+            DEBUG("cancelled\n");
+        } else if (trans->status == LIBUSB_TRANSFER_NO_DEVICE) {
+            DEBUG("no device\n");
+        } else {
+            DEBUG("unknown\n");
+		}
+        ctx->flags |= TRANS_FLAGS_HAS_ERROR;
+    }
+    ctx->flags |= TRANS_FLAGS_IS_DONE;
+}
+
+static int submit_wait(struct libusb_transfer * trans) {
+	struct timeval start;
+	struct timeval now;
+	struct timeval diff;
+	struct trans_ctx trans_ctx;
+	enum libusb_error error;
+
+	trans_ctx.flags = 0;
+
+	/* brief intrusion inside the libusb interface */
+	trans->callback = on_trans_done;
+	trans->user_data = &trans_ctx;
+
+	if ((error = libusb_submit_transfer(trans))) {
+		DEBUG("libusb_submit_transfer(%d): %s\n", error,
+			  libusb_strerror(error));
+		exit(-1);
+	}
+
+	gettimeofday(&start, NULL);
+
+	while (trans_ctx.flags == 0) {
+		struct timeval timeout;
+		timeout.tv_sec = 1;
+		timeout.tv_usec = 0;
+		if (libusb_handle_events_timeout(Stlink.libusb_ctx, &timeout)) {
+			DEBUG("libusb_handle_events()\n");
+			return -1;
+		}
+
+		gettimeofday(&now, NULL);
+		timersub(&now, &start, &diff);
+		if (diff.tv_sec >= 1) {
+			libusb_cancel_transfer(trans);
+			DEBUG("libusb_handle_events() timeout\n");
+			return -1;
+		}
+	}
+
+	if (trans_ctx.flags & TRANS_FLAGS_HAS_ERROR) {
+		DEBUG("libusb_handle_events() | has_error\n");
+		return -1;
+	}
+
+	return 0;
+}
+#define STLINK_ERROR_DP_FAULT -2
+static int send_recv(uint8_t *txbuf, size_t txsize,
+					 uint8_t *rxbuf, size_t rxsize)
+{
+	int res = 0;
+	stlink_check_detach(1);
+	if( txsize) {
+		int txlen = txsize;
+		libusb_fill_bulk_transfer(Stlink.req_trans, Stlink.handle,
+								  Stlink.ep_tx | LIBUSB_ENDPOINT_OUT,
+								  txbuf, txlen,
+								  NULL, NULL,
+								  0
+			);
+		DEBUG_USB("  Send (%d): ", txlen);
+		for (int i = 0; i < txlen && i < 32 ; i++) {
+			DEBUG_USB("%02x", txbuf[i]);
+			if ((i & 7) == 7)
+				DEBUG_USB(".");
+		}
+		if (submit_wait(Stlink.req_trans)) {
+			DEBUG_USB("clear 2\n");
+			libusb_clear_halt(Stlink.handle,2);
+			return -1;
+		}
+	}
+	/* send_only */
+	if (rxsize != 0) {
+		/* read the response */
+		libusb_fill_bulk_transfer(Stlink.rep_trans, Stlink.handle,
+								  0x01| LIBUSB_ENDPOINT_IN,
+								  rxbuf, rxsize, NULL, NULL, 0);
+
+		if (submit_wait(Stlink.rep_trans)) {
+			DEBUG("clear 1\n");
+			libusb_clear_halt(Stlink.handle,1);
+			return -1;
+		}
+		res = Stlink.rep_trans->actual_length;
+		if (res >0) {
+			int i;
+			uint8_t *p = rxbuf;
+			DEBUG_USB(" Rec (%zu/%d)", rxsize, res);
+			for (i = 0; i < res && i < 32 ; i++) {
+				if ( i && ((i & 7) == 0))
+					DEBUG_USB(".");
+				DEBUG_USB("%02x", p[i]);
+			}
+		}
+	}
+	DEBUG_USB("\n");
+	return res;
+}
+
+/**
+    Converts an STLINK status code held in the first byte of a response to
+	readable error
+*/
+static int stlink_usb_error_check(uint8_t *data, bool verbose)
+{
+	switch (data[0]) {
+		case STLINK_DEBUG_ERR_OK:
+			return STLINK_ERROR_OK;
+		case STLINK_DEBUG_ERR_FAULT:
+			if (verbose)
+				DEBUG("SWD fault response (0x%x)\n", STLINK_DEBUG_ERR_FAULT);
+			return STLINK_ERROR_FAIL;
+		case STLINK_JTAG_UNKNOWN_JTAG_CHAIN:
+			if (verbose)
+				DEBUG("Unknown JTAG chain\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_NO_DEVICE_CONNECTED:
+			if (verbose)
+				DEBUG("No device connected\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_JTAG_COMMAND_ERROR:
+			if (verbose)
+				DEBUG("Command error\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_JTAG_GET_IDCODE_ERROR:
+			if (verbose)
+				DEBUG("Failure reading IDCODE\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_JTAG_DBG_POWER_ERROR:
+			if (verbose)
+				DEBUG("Failure powering DBG\n");
+			return STLINK_ERROR_WAIT;
+		case STLINK_SWD_AP_WAIT:
+			if (verbose)
+				DEBUG("wait status SWD_AP_WAIT (0x%x)\n", STLINK_SWD_AP_WAIT);
+			return STLINK_ERROR_WAIT;
+		case STLINK_SWD_DP_WAIT:
+			if (verbose)
+				DEBUG("wait status SWD_DP_WAIT (0x%x)\n", STLINK_SWD_DP_WAIT);
+			return STLINK_ERROR_WAIT;
+		case STLINK_JTAG_WRITE_ERROR:
+			if (verbose)
+				DEBUG("Write error\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_JTAG_WRITE_VERIF_ERROR:
+			if (verbose)
+				DEBUG("Write verify error, ignoring\n");
+			return STLINK_ERROR_OK;
+		case STLINK_SWD_AP_FAULT:
+			/* git://git.ac6.fr/openocd commit 657e3e885b9ee10
+			 * returns STLINK_ERROR_OK with the comment:
+			 * Change in error status when reading outside RAM.
+			 * This fix allows CDT plugin to visualize memory.
+			 */
+			Stlink.ap_error = true;
+			if (verbose)
+				DEBUG("STLINK_SWD_AP_FAULT\n");
+			return STLINK_ERROR_DP_FAULT;
+		case STLINK_SWD_AP_ERROR:
+			if (verbose)
+				DEBUG("STLINK_SWD_AP_ERROR\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_SWD_AP_PARITY_ERROR:
+			if (verbose)
+				DEBUG("STLINK_SWD_AP_PARITY_ERROR\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_SWD_DP_FAULT:
+			if (verbose)
+				DEBUG("STLINK_SWD_DP_FAULT\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_SWD_DP_ERROR:
+			if (verbose)
+				DEBUG("STLINK_SWD_DP_ERROR\n");
+			raise_exception(EXCEPTION_ERROR, "STLINK_SWD_DP_ERROR");
+			return STLINK_ERROR_FAIL;
+		case STLINK_SWD_DP_PARITY_ERROR:
+			if (verbose)
+				DEBUG("STLINK_SWD_DP_PARITY_ERROR\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_SWD_AP_WDATA_ERROR:
+			if (verbose)
+				DEBUG("STLINK_SWD_AP_WDATA_ERROR\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_SWD_AP_STICKY_ERROR:
+			if (verbose)
+				DEBUG("STLINK_SWD_AP_STICKY_ERROR\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_SWD_AP_STICKYORUN_ERROR:
+			if (verbose)
+				DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_BAD_AP_ERROR:
+			/* ADIV5 probe 256 APs, most of them are non exisitant.*/
+			return STLINK_ERROR_FAIL;
+		case STLINK_TOO_MANY_AP_ERROR:
+			/* TI TM4C duplicates AP. Error happens at AP9.*/
+			if (verbose)
+				DEBUG("STLINK_TOO_MANY_AP_ERROR\n");
+			return STLINK_ERROR_FAIL;
+		case STLINK_JTAG_UNKNOWN_CMD :
+			if (verbose)
+				DEBUG("STLINK_JTAG_UNKNOWN_CMD\n");
+			return STLINK_ERROR_FAIL;
+		default:
+			if (verbose)
+				DEBUG("unknown/unexpected STLINK status code 0x%x\n", data[0]);
+			return STLINK_ERROR_FAIL;
+	}
+}
+
+static int send_recv_retry(uint8_t *txbuf, size_t txsize,
+					 uint8_t *rxbuf, size_t rxsize)
+{
+	struct timeval start;
+	struct timeval now;
+	struct timeval diff;
+	gettimeofday(&start, NULL);
+	int res;
+	while(1) {
+		send_recv(txbuf, txsize, rxbuf, rxsize);
+		res = stlink_usb_error_check(rxbuf, 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("write_retry failed. ");
+			return res;
+		}
+	}
+	return res;
+}
+
+static int read_retry(uint8_t *txbuf, size_t txsize,
+					 uint8_t *rxbuf, size_t rxsize)
+{
+	struct timeval start;
+	struct timeval now;
+	struct timeval diff;
+	gettimeofday(&start, NULL);
+	int res;
+	while(1) {
+		send_recv(txbuf, txsize, rxbuf, rxsize);
+		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;
+		}
+	}
+	return res;
+}
+
+static int write_retry(uint8_t *cmdbuf, size_t cmdsize,
+					 uint8_t *txbuf, size_t txsize)
+{
+	struct timeval start;
+	struct timeval now;
+	struct timeval diff;
+	gettimeofday(&start, NULL);
+	int res;
+	while(1) {
+		send_recv(cmdbuf, cmdsize, NULL, 0);
+		send_recv(txbuf, txsize, NULL, 0);
+		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;
+		}
+	}
+	return res;
+}
+
+static void stlink_version(void)
+{
+	if (Stlink.ver_hw == 30) {
+		uint8_t cmd[16] = {STLINK_APIV3_GET_VERSION_EX};
+		uint8_t data[12];
+		int size = send_recv(cmd, 16, data, 12);
+		if (size == -1) {
+			printf("[!] send_recv STLINK_APIV3_GET_VERSION_EX\n");
+		}
+		Stlink.ver_stlink = data[0];
+		Stlink.ver_swim  =  data[1];
+		Stlink.ver_jtag  =  data[2];
+		Stlink.ver_mass  =  data[3];
+		Stlink.ver_bridge = data[4];
+		Stlink.block_size = 512;
+		Stlink.vid = data[3] <<  9 | data[8];
+		Stlink.pid = data[5] << 11 | data[10];
+	} else {
+		uint8_t cmd[16] = {STLINK_GET_VERSION};
+		uint8_t data[6];
+		int size = send_recv(cmd, 16, data, 6);
+		if (size == -1) {
+			printf("[!] send_recv STLINK_GET_VERSION_EX\n");
+		}
+		Stlink.vid = data[3] << 8 | data[2];
+		Stlink.pid = data[5] << 8 | data[4];
+		int  version = data[0] << 8 | data[1]; /* Big endian here!*/
+		Stlink.block_size = 64;
+		Stlink.ver_stlink = (version >> 12) & 0x0f;
+		Stlink.ver_jtag   = (version >>  6) & 0x3f;
+		if ((Stlink.pid == PRODUCT_ID_STLINKV21_MSD) ||
+			(Stlink.pid == PRODUCT_ID_STLINKV21)) {
+			Stlink.ver_mass = (version >> 0) & 0x3f;
+		} else {
+			Stlink.ver_swim = (version >> 0) & 0x3f;
+		}
+	}
+	DEBUG("V%dJ%d",Stlink.ver_stlink, Stlink.ver_jtag);
+	if (Stlink.ver_hw == 30) {
+		DEBUG("M%dB%dS%d", Stlink.ver_mass, Stlink.ver_bridge, Stlink.ver_swim);
+	} else if (Stlink.ver_hw == 20) {
+		DEBUG("S%d", Stlink.ver_swim);
+	} else if (Stlink.ver_hw == 21) {
+		DEBUG("M%d", Stlink.ver_mass);
+	}
+	DEBUG("\n");
+}
+
+void stlink_leave_state(void)
+{
+	uint8_t cmd[16] = {STLINK_GET_CURRENT_MODE};
+	uint8_t data[2];
+	send_recv(cmd, 16, data, 2);
+	if (data[0] == STLINK_DEV_DFU_MODE) {
+		uint8_t dfu_cmd[16] = {STLINK_DFU_COMMAND, STLINK_DFU_EXIT};
+		DEBUG("Leaving DFU Mode\n");
+		send_recv(dfu_cmd, 16, NULL, 0);
+	} else if (data[0] == STLINK_DEV_SWIM_MODE) {
+		uint8_t swim_cmd[16] = {STLINK_SWIM_COMMAND, STLINK_SWIM_EXIT};
+		DEBUG("Leaving SWIM Mode\n");
+		send_recv(swim_cmd, 16, NULL, 0);
+	} else if (data[0] == STLINK_DEV_DEBUG_MODE) {
+		uint8_t dbg_cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_EXIT};
+		DEBUG("Leaving DEBUG Mode\n");
+		send_recv(dbg_cmd, 16, NULL, 0);
+	} else if (data[0] == STLINK_DEV_BOOTLOADER_MODE) {
+		DEBUG("Leaving BOOTLOADER Mode\n");
+	} else if (data[0] == STLINK_DEV_MASS_MODE) {
+		DEBUG("Leaving MASS Mode\n");
+	} else {
+		DEBUG("Unknown Mode %02x\n", data[0]);
+	}
+}
+
+const char *stlink_target_voltage(void)
+{
+	uint8_t cmd[16] = {STLINK_GET_TARGET_VOLTAGE};
+	uint8_t data[8];
+	send_recv(cmd, 16, data, 8);
+	uint16_t adc[2];
+	adc[0] = data[0] | data[1] << 8; /* Calibration value? */
+	adc[1] = data[4] | data[5] << 8; /* Measured value?*/
+	float result  = 0.0;
+	if (adc[0])
+		result = 2.0 * adc[1] * 1.2 / adc[0];
+	static char res[6];
+	sprintf(res, "%4.2fV", result);
+	return res;
+}
+
+static void stlink_resetsys(void)
+{
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND ,STLINK_DEBUG_APIV2_RESETSYS};
+	uint8_t data[2];
+	send_recv(cmd, 16, data, 2);
+}
+
+void stlink_init(int argc, char **argv)
+{
+	BMP_CL_OPTIONS_t cl_opts = {0};
+	cl_opts.opt_idstring = "Blackmagic Debug Probe on StlinkV2/3";
+	cl_init(&cl_opts, argc, argv);
+	libusb_device **devs, *dev;
+	int r;
+	int ret = -1;
+	atexit(exit_function);
+	signal(SIGTERM, sigterm_handler);
+	signal(SIGINT, sigterm_handler);
+	libusb_init(&Stlink.libusb_ctx);
+	r = libusb_init(NULL);
+	if (r < 0)
+		DEBUG("Failed: %s", libusb_strerror(r));
+	bool hotplug = true;
+	if (!libusb_has_capability (LIBUSB_CAP_HAS_HOTPLUG)) {
+		printf("Hotplug capabilites are not supported on this platform\n");
+		hotplug = false;
+	}
+	ssize_t cnt;
+  rescan:
+	has_attached = 0;
+	memset(&Stlink, 0, sizeof(Stlink));
+	cnt = libusb_get_device_list(NULL, &devs);
+	if (cnt < 0) {
+		DEBUG("Failed: %s", libusb_strerror(r));
+		goto error;
+	}
+	int i = 0;
+	int nr_stlinks = 0;
+	while ((dev = devs[i++]) != NULL) {
+		struct libusb_device_descriptor desc;
+		int r = libusb_get_device_descriptor(dev, &desc);
+		if (r < 0) {
+			fprintf(stderr, "libusb_get_device_descriptor failed %s",
+					libusb_strerror(r));
+			goto error;
+		}
+		if ((desc.idVendor == VENDOR_ID_STLINK) &&
+			((desc.idProduct & PRODUCT_ID_STLINK_MASK) ==
+			 PRODUCT_ID_STLINK_GROUP)) {
+			if (desc.idProduct == PRODUCT_ID_STLINKV1)  { /* Reject V1 devices.*/
+				DEBUG("STLINKV1 not supported\n");
+				continue;
+			}
+			Stlink.vid = desc.idVendor;
+			Stlink.pid = desc.idProduct;
+			r = libusb_open(dev, &Stlink.handle);
+			if (r == LIBUSB_SUCCESS) {
+				uint8_t data[32];
+				uint16_t lang;
+				libusb_get_string_descriptor(
+					Stlink.handle, 0, 0, data, sizeof(data));
+				lang = data[2] << 8 | data[3];
+				unsigned char sernum[32];
+				if (desc.iSerialNumber) {
+					r = libusb_get_string_descriptor
+						(Stlink.handle, desc.iSerialNumber, lang,
+						  sernum, sizeof(sernum));
+				} else {
+					DEBUG("No serial number\n");
+				}
+				/* Older devices have hex values instead of ascii
+				 * in the serial string. Recode eventually!*/
+				bool readable = true;
+				uint16_t *p = (uint16_t *)sernum;
+				for (p += 1; *p; p++) {
+					bool isr = isalnum(*p);
+					readable &= isr;
+				}
+				char *s = Stlink.serial;
+				p = (uint16_t *)sernum;
+				for (p += 1; *p; p++, s++) {
+					if (readable)
+						*s = *p;
+					else
+						snprintf(s, 3, "%02x", *p & 0xff);
+				}
+				if (cl_opts.opt_serial && (!strncmp(Stlink.serial, cl_opts.opt_serial,
+													strlen(cl_opts.opt_serial))))
+					DEBUG("Found ");
+				if (desc.idProduct == PRODUCT_ID_STLINKV2) {
+					DEBUG("STLINKV20 serial %s\n", Stlink.serial);
+					Stlink.ver_hw = 20;
+					Stlink.ep_tx = 2;
+				} else if (desc.idProduct == PRODUCT_ID_STLINKV21) {
+					DEBUG("STLINKV21 serial %s\n", Stlink.serial);
+					Stlink.ver_hw = 21;
+					Stlink.ep_tx = 1;
+				} else if (desc.idProduct == PRODUCT_ID_STLINKV21_MSD) {
+					DEBUG("STLINKV21_MSD serial %s\n", Stlink.serial);
+					Stlink.ver_hw = 21;
+					Stlink.ep_tx = 1;
+				} else if (desc.idProduct == PRODUCT_ID_STLINKV3E) {
+					DEBUG("STLINKV3E serial %s\n", Stlink.serial);
+					Stlink.ver_hw = 30;
+					Stlink.ep_tx = 1;
+				} else if (desc.idProduct == PRODUCT_ID_STLINKV3) {
+					DEBUG("STLINKV3  serial %s\n", Stlink.serial);
+					Stlink.ver_hw = 30;
+					Stlink.ep_tx = 1;
+				} else {
+					DEBUG("Unknown STLINK variant, serial %s\n", Stlink.serial);
+				}
+				nr_stlinks++;
+				if (cl_opts.opt_serial) {
+					if (!strncmp(Stlink.serial, cl_opts.opt_serial,
+								 strlen(cl_opts.opt_serial))) {
+						break;
+					} else {
+						libusb_close(Stlink.handle);
+						Stlink.handle = 0;
+					}
+				}
+			} else {
+				DEBUG("Open failed %s\n", libusb_strerror(r));
+			}
+		}
+	}
+	libusb_free_device_list(devs, 1);
+	if (!Stlink.handle) {
+		if (nr_stlinks && cl_opts.opt_serial) {
+			DEBUG("No Stlink with given serial number %s\n",  cl_opts.opt_serial);
+		} else if (nr_stlinks > 1) {
+			DEBUG("Multiple Stlinks. Please specify serial number\n");
+			goto error;
+		} else {
+			DEBUG("No Stlink device found!\n");
+		}
+		if (hotplug && !cl_opts.opt_no_wait) {
+			libusb_hotplug_callback_handle hp;
+			int rc = libusb_hotplug_register_callback
+				(NULL, LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED, 0,
+				 VENDOR_ID_STLINK, LIBUSB_HOTPLUG_MATCH_ANY,
+				 LIBUSB_HOTPLUG_MATCH_ANY,
+				 hotplug_callback_attach, NULL, &hp);
+			if (LIBUSB_SUCCESS != rc) {
+				DEBUG("Error registering attach callback\n");
+				goto error;
+			}
+			DEBUG("Waiting for %sST device%s%s to attach\n",
+				  (cl_opts.opt_serial)? "" : "some ",
+				  (cl_opts.opt_serial)? " with serial ": "",
+				  (cl_opts.opt_serial)? cl_opts.opt_serial: "");
+			DEBUG("Terminate with ^C\n");
+			while (has_attached == 0) {
+				rc = libusb_handle_events (NULL);
+                if (rc < 0)
+					printf("libusb_handle_events() failed: %s\n",
+						   libusb_error_name(rc));
+			}
+			goto rescan;
+		}
+		goto error;
+	}
+	int config;
+	r = libusb_get_configuration(Stlink.handle, &config);
+	if (r) {
+		DEBUG("libusb_get_configuration failed %d: %s", r, libusb_strerror(r));
+		goto error_1;
+	}
+	DEBUG("Config %d\n", config);
+	if (config != 1) {
+		r = libusb_set_configuration(Stlink.handle, 0);
+		if (r) {
+			DEBUG("libusb_set_configuration failed %d: %s",
+				  r, libusb_strerror(r));
+			goto error_1;
+		}
+	}
+	r = libusb_claim_interface(Stlink.handle, 0);
+	if (r)
+	{
+		DEBUG("libusb_claim_interface failed %s\n", libusb_strerror(r));
+		goto error_1;
+	}
+	if (hotplug) { /* Allow gracefully exit when stlink is unplugged*/
+		libusb_hotplug_callback_handle hp;
+		int rc = libusb_hotplug_register_callback
+			(NULL, LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT, 0, Stlink.vid, Stlink.pid,
+			 LIBUSB_HOTPLUG_MATCH_ANY, hotplug_callback_detach, NULL, &hp);
+		if (LIBUSB_SUCCESS != rc) {
+			DEBUG("Error registering detach callback\n");
+			goto error;
+		}
+	}
+	Stlink.req_trans = libusb_alloc_transfer(0);
+	Stlink.rep_trans = libusb_alloc_transfer(0);
+	stlink_version();
+	if ((Stlink.ver_stlink < 3 && Stlink.ver_jtag < 32) ||
+		(Stlink.ver_stlink == 3 && Stlink.ver_jtag < 3)) {
+		/* Maybe the adapter is in some strange state. Try to reset */
+        int result = libusb_reset_device(Stlink.handle);
+		DEBUG("Trying reset\n");
+		if (result == LIBUSB_ERROR_BUSY) { /* Try again */
+			platform_delay(50);
+			result = libusb_reset_device(Stlink.handle);
+		}
+        if (result != LIBUSB_SUCCESS) {
+			DEBUG("libusb_reset_device failed\n");
+			goto error_1;
+		}
+		stlink_version();
+    }
+	if ((Stlink.ver_stlink < 3 && Stlink.ver_jtag < 32) ||
+		(Stlink.ver_stlink == 3 && Stlink.ver_jtag < 3)) {
+		DEBUG("Please update Firmware\n");
+		goto error_1;
+	}
+	stlink_leave_state();
+	stlink_resetsys();
+	if (cl_opts.opt_mode != BMP_MODE_DEBUG) {
+		ret = cl_execute(&cl_opts);
+	} else {
+		assert(gdb_if_init() == 0);
+		return;
+	}
+  error_1:
+	libusb_close(Stlink.handle);
+  error:
+	libusb_exit(Stlink.libusb_ctx);
+	exit(ret);
+}
+
+void stlink_srst_set_val(bool assert)
+{
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
+					  STLINK_DEBUG_APIV2_DRIVE_NRST,
+					  (assert)? STLINK_DEBUG_APIV2_DRIVE_NRST_LOW
+					  : STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH};
+	uint8_t data[2];
+	send_recv(cmd, 16, data, 2);
+	stlink_usb_error_check(data, true);
+}
+
+bool stlink_set_freq_divisor(uint16_t divisor)
+{
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
+					  STLINK_DEBUG_APIV2_SWD_SET_FREQ,
+					  divisor & 0xff, divisor >> 8};
+	uint8_t data[2];
+	send_recv(cmd, 16, data, 2);
+	if (stlink_usb_error_check(data, false))
+		return false;
+	return true;
+}
+
+bool stlink3_set_freq_divisor(uint16_t divisor)
+{
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
+					  STLINK_APIV3_GET_COM_FREQ,
+					  Stlink.transport_mode};
+	uint8_t data[52];
+	send_recv(cmd, 16, data, 52);
+	stlink_usb_error_check(data, true);
+	int size = data[8];
+	if (divisor > size)
+		divisor = size;
+	uint8_t *p = data + 12 + divisor * sizeof(uint32_t);
+	uint32_t freq = p[0] | p[1] << 8 | p[2] << 16 | p[3] << 24;
+	DEBUG("Selected %" PRId32 " khz\n", freq);
+	cmd[1] = STLINK_APIV3_SET_COM_FREQ;
+	cmd[2] = Stlink.transport_mode;
+	cmd[3] = 0;
+	p = data + 12 + divisor * sizeof(uint32_t);
+	cmd[4] = p[0];
+	cmd[5] = p[1];
+	cmd[6] = p[2];
+	cmd[7] = p[3];
+	send_recv(cmd, 16, data, 8);
+	return true;
+}
+
+int stlink_hwversion(void)
+{
+	return Stlink.ver_stlink;
+}
+
+int stlink_enter_debug_swd(void)
+{
+	stlink_leave_state();
+	Stlink.transport_mode = STLINK_MODE_SWD;
+	if (Stlink.ver_stlink == 3)
+		stlink3_set_freq_divisor(2);
+	else
+		stlink_set_freq_divisor(1);
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
+					  STLINK_DEBUG_APIV2_ENTER,
+					  STLINK_DEBUG_ENTER_SWD_NO_RESET};
+	uint8_t data[2];
+	DEBUG("Enter SWD\n");
+	send_recv_retry(cmd, 16, data, 2);
+	return stlink_usb_error_check(data, true);
+}
+
+int stlink_enter_debug_jtag(void)
+{
+	stlink_leave_state();
+	Stlink.transport_mode = STLINK_MODE_JTAG;
+	if (Stlink.ver_stlink == 3)
+		stlink3_set_freq_divisor(4);
+	else
+		stlink_set_freq_divisor(1);
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
+					  STLINK_DEBUG_APIV2_ENTER,
+					  STLINK_DEBUG_ENTER_JTAG_NO_RESET};
+	uint8_t data[2];
+	DEBUG("Enter JTAG\n");
+	send_recv(cmd, 16, data, 2);
+	return stlink_usb_error_check(data, true);
+}
+
+uint32_t stlink_read_coreid(void)
+{
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
+					  STLINK_DEBUG_READCOREID};
+	uint8_t data[4];
+	send_recv(cmd, 16, data, 4);
+	uint32_t id =  data[0] | data[1] << 8 | data[2] << 16 | data[3] << 24;
+	DEBUG("Read Core ID: 0x%08" PRIx32 "\n", id);
+	return id;
+}
+
+int stlink_read_idcodes(uint32_t *idcodes)
+{
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
+					   STLINK_DEBUG_APIV2_READ_IDCODES};
+	uint8_t data[12];
+	send_recv(cmd, 16, data, 12);
+	if (stlink_usb_error_check(data, true))
+		return 0;
+	uint8_t *p = data + 4;
+	idcodes[0] = p[0] | p[1] << 8 | p[2] << 16 | p[3] << 24;
+	p += 4;
+	idcodes[1] = p[0] | p[1] << 8 | p[2] << 16 | p[3] << 24;
+	return 2;
+}
+
+uint32_t stlink_dp_read(ADIv5_DP_t *dp, uint16_t addr)
+{
+	if (addr & ADIV5_APnDP) {
+		DEBUG_STLINK("AP read addr 0x%04" PRIx16 "\n", addr);
+		stlink_dp_low_access(dp, ADIV5_LOW_READ, addr, 0);
+		return stlink_dp_low_access(dp, ADIV5_LOW_READ,
+		                           ADIV5_DP_RDBUFF, 0);
+	} else {
+		DEBUG_STLINK("DP read addr 0x%04" PRIx16 "\n", addr);
+		return stlink_dp_low_access(dp, ADIV5_LOW_READ, addr, 0);
+	}
+}
+
+uint32_t stlink_dp_error(ADIv5_DP_t *dp)
+{
+	uint32_t err, clr = 0;
+
+	err = stlink_dp_read(dp, ADIV5_DP_CTRLSTAT) &
+		(ADIV5_DP_CTRLSTAT_STICKYORUN | ADIV5_DP_CTRLSTAT_STICKYCMP |
+		ADIV5_DP_CTRLSTAT_STICKYERR | ADIV5_DP_CTRLSTAT_WDATAERR);
+
+	if(err & ADIV5_DP_CTRLSTAT_STICKYORUN)
+		clr |= ADIV5_DP_ABORT_ORUNERRCLR;
+	if(err & ADIV5_DP_CTRLSTAT_STICKYCMP)
+		clr |= ADIV5_DP_ABORT_STKCMPCLR;
+	if(err & ADIV5_DP_CTRLSTAT_STICKYERR)
+		clr |= ADIV5_DP_ABORT_STKERRCLR;
+	if(err & ADIV5_DP_CTRLSTAT_WDATAERR)
+		clr |= ADIV5_DP_ABORT_WDERRCLR;
+
+	adiv5_dp_write(dp, ADIV5_DP_ABORT, clr);
+	dp->fault = 0;
+	if (err)
+		DEBUG("stlink_dp_error %d\n", err);
+	err |= Stlink.ap_error;
+	Stlink.ap_error = false;
+	return err;
+}
+
+void stlink_dp_abort(ADIv5_DP_t *dp, uint32_t abort)
+{
+	adiv5_dp_write(dp, ADIV5_DP_ABORT, abort);
+}
+
+int stlink_read_dp_register(uint16_t port, uint16_t addr, uint32_t *reg)
+{
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
+					  STLINK_DEBUG_APIV2_READ_DAP_REG,
+					  port & 0xff, port >> 8,
+					  addr & 0xff, addr >> 8};
+	if (port == STLINK_DEBUG_PORT_ACCESS  && Stlink.dap_select)
+		cmd[4] = ((Stlink.dap_select & 0xf) << 4) | (addr & 0xf);
+	else
+		cmd[4] = addr & 0xff;
+	DEBUG_STLINK("Read DP, Addr 0x%04" PRIx16 ": \n", addr);
+	uint8_t data[8];
+	int res = send_recv_retry(cmd, 16, data, 8);
+	if (res == STLINK_ERROR_OK) {
+		uint32_t ret = data[4] | data[5] << 8 | data[6] << 16 | data[7] << 24;
+		DEBUG_STLINK("0x%08" PRIx32" \n", ret);
+		*reg = ret;
+	} else {
+		DEBUG_STLINK("failed, res %d\n", res);
+	}
+	return res;
+}
+
+int stlink_write_dp_register(uint16_t port, uint16_t addr, uint32_t val)
+{
+	if (port == STLINK_DEBUG_PORT_ACCESS && addr == 8) {
+		Stlink.dap_select = val;
+		DEBUG_STLINK("Caching SELECT 0x%02" PRIx32 "\n", val);
+		return STLINK_ERROR_OK;
+	} else {
+		uint8_t cmd[16] = {
+			STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_WRITE_DAP_REG,
+			port & 0xff, port >> 8,
+			addr & 0xff, addr >> 8,
+			val & 0xff, (val >>  8) & 0xff, (val >> 16) & 0xff,
+			(val >> 24) & 0xff};
+		uint8_t data[2];
+		send_recv_retry(cmd, 16, data, 2);
+		DEBUG_STLINK("Write DP, Addr 0x%04" PRIx16 ": 0x%08" PRIx32
+			  " \n", addr, val);
+		return stlink_usb_error_check(data, true);
+	}
+}
+
+uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
+				      uint16_t addr, uint32_t value)
+{
+	uint32_t response = 0;
+	int res;
+	if (RnW) {
+		res = stlink_read_dp_register(
+			STLINK_DEBUG_PORT_ACCESS, addr, &response);
+		DEBUG_STLINK("SWD read addr %04" PRIx16 ": %08" PRIx32 "\n",
+					 addr, response);
+	} else {
+		DEBUG_STLINK("SWD write addr %04" PRIx16 ": %08" PRIx32 "\n",
+					 addr, value);
+		res = stlink_write_dp_register(STLINK_DEBUG_PORT_ACCESS, addr, value);
+	}
+	if (res == STLINK_ERROR_WAIT)
+		raise_exception(EXCEPTION_TIMEOUT, "DP ACK timeout");
+
+	if(res == STLINK_ERROR_DP_FAULT) {
+		dp->fault = 1;
+		return 0;
+	}
+	if(res == STLINK_ERROR_FAIL)
+		raise_exception(EXCEPTION_ERROR, "SWDP invalid ACK");
+
+	return response;
+}
+
+static bool stlink_ap_setup(int ap)
+{
+	if (ap > 7)
+		return false;
+	uint8_t cmd[16] = {
+		STLINK_DEBUG_COMMAND,
+		STLINK_DEBUG_APIV2_INIT_AP,
+		ap,
+	};
+	uint8_t data[2];
+	send_recv_retry(cmd, 16, data, 2);
+	DEBUG_STLINK("Open AP %d\n", ap);
+	int res = stlink_usb_error_check(data, true);
+	if (res) {
+		if (Stlink.ver_hw == 30) {
+			DEBUG("STLINKV3 only connects to STM8/32!\n");
+		}
+		return false;
+	}
+	return true;
+}
+
+static void stlink_ap_cleanup(int ap)
+{
+       uint8_t cmd[16] = {
+               STLINK_DEBUG_COMMAND,
+               STLINK_DEBUG_APIV2_CLOSE_AP_DBG,
+               ap,
+       };
+       uint8_t data[2];
+       send_recv(cmd, 16, data, 2);
+       DEBUG_STLINK("Close AP %d\n", ap);
+       stlink_usb_error_check(data, true);
+}
+static int stlink_usb_get_rw_status(bool verbose)
+{
+	uint8_t cmd[16] = {
+		STLINK_DEBUG_COMMAND,
+		STLINK_DEBUG_APIV2_GETLASTRWSTATUS2
+	};
+	uint8_t data[12];
+	send_recv(cmd, 16, data, 12);
+	return stlink_usb_error_check(data, verbose);
+}
+
+static void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
+{
+	if (len == 0)
+		return;
+	size_t read_len = len;
+	uint8_t type;
+	char *CMD;
+	if (src & 1 || len & 1) {
+		CMD = "READMEM_8BIT";
+		type = STLINK_DEBUG_READMEM_8BIT;
+		if (len > Stlink.block_size) {
+			DEBUG(" Too large!\n");
+			return;
+		}
+		if (len == 1)
+			read_len ++; /* Fix read length as in openocd*/
+	} else if (src & 3 || len & 3) {
+		CMD = "READMEM_16BIT";
+		type = STLINK_DEBUG_APIV2_READMEM_16BIT;
+	} else {
+		CMD = "READMEM_32BIT";
+		type = STLINK_DEBUG_READMEM_32BIT;
+
+	}
+	DEBUG_STLINK("%s len %zu addr 0x%08" PRIx32 " AP %d : ",
+				 CMD, len, src, ap->apsel);
+	uint8_t cmd[16] = {
+		STLINK_DEBUG_COMMAND,
+		type,
+		src & 0xff, (src >>  8) & 0xff, (src >> 16) & 0xff,
+		(src >> 24) & 0xff,
+		len & 0xff, len >> 8, ap->apsel};
+	int res = read_retry(cmd, 16, dest, read_len);
+	if (res == STLINK_ERROR_OK) {
+		uint8_t *p = (uint8_t*)dest;
+		for (size_t i = 0; i < len ; i++) {
+			DEBUG_STLINK("%02x", *p++);
+		}
+	} else {
+		/* FIXME: What is the right measure when failing?
+		 *
+		 * E.g. TM4C129 gets here when NRF probe reads 0x10000010
+		 * Approach taken:
+		 * Fill the memory with some fixed pattern so hopefully
+		 * the caller notices the error*/
+		DEBUG("stlink_readmem failed\n");
+		memset(dest, 0xff, len);
+	}
+	DEBUG_STLINK("\n");
+}
+
+static void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
+					  uint8_t *buffer)
+{
+	DEBUG_STLINK("Mem Write8 AP %d len %zu addr 0x%08" PRIx32 ": ",
+				 ap->apsel, len, addr);
+	for (size_t t = 0; t < len; t++) {
+		DEBUG_STLINK("%02x", buffer[t]);
+	}
+	DEBUG_STLINK("\n");
+	while (len) {
+		size_t length;
+		if (len > Stlink.block_size)
+			length = Stlink.block_size;
+		else
+			length = len;
+		uint8_t cmd[16] = {
+			STLINK_DEBUG_COMMAND,
+			STLINK_DEBUG_WRITEMEM_8BIT,
+			addr & 0xff, (addr >>  8) & 0xff, (addr >> 16) & 0xff,
+			(addr >> 24) & 0xff,
+			length & 0xff, length >> 8, ap->apsel};
+		send_recv(cmd, 16, NULL, 0);
+		send_recv((void*)buffer, length, NULL, 0);
+		stlink_usb_get_rw_status(true);
+		len -= length;
+		addr += length;
+	}
+}
+
+static void stlink_writemem16(ADIv5_AP_t *ap, uint32_t addr, size_t len,
+					   uint16_t *buffer)
+{
+	DEBUG_STLINK("Mem Write16 AP %d len %zu addr 0x%08" PRIx32 ": ",
+				 ap->apsel, len, addr);
+	for (size_t t = 0; t < len; t+=2) {
+		DEBUG_STLINK("%04x", buffer[t]);
+	}
+	DEBUG_STLINK("\n");
+	uint8_t cmd[16] = {
+		STLINK_DEBUG_COMMAND,
+		STLINK_DEBUG_APIV2_WRITEMEM_16BIT,
+		addr & 0xff, (addr >>  8) & 0xff, (addr >> 16) & 0xff,
+		(addr >> 24) & 0xff,
+		len & 0xff, len >> 8, ap->apsel};
+	send_recv(cmd, 16, NULL, 0);
+	send_recv((void*)buffer, len, NULL, 0);
+	stlink_usb_get_rw_status(true);
+}
+
+static void stlink_writemem32(ADIv5_AP_t *ap, uint32_t addr, size_t len,
+					   uint32_t *buffer)
+{
+	DEBUG_STLINK("Mem Write32 AP %d len %zu addr 0x%08" PRIx32 ": ",
+				 ap->apsel, len, addr);
+	for (size_t t = 0; t < len; t+=4) {
+		DEBUG_STLINK("%04x", buffer[t]);
+	}
+	DEBUG_STLINK("\n");
+	uint8_t cmd[16] = {
+		STLINK_DEBUG_COMMAND,
+		STLINK_DEBUG_WRITEMEM_32BIT,
+		addr & 0xff, (addr >>  8) & 0xff, (addr >> 16) & 0xff,
+		(addr >> 24) & 0xff,
+		len & 0xff, len >> 8, ap->apsel};
+	write_retry(cmd, 16, (void*)buffer, len);
+}
+
+void stlink_regs_read(ADIv5_AP_t *ap, void *data)
+{
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READALLREGS,
+					   ap->apsel};
+	uint8_t res[88];
+	DEBUG_STLINK("AP %d: Read all core registers\n", ap->apsel);
+	send_recv(cmd, 16, res, 88);
+	stlink_usb_error_check(res, true);
+	memcpy(data, res + 4, 84);
+}
+
+uint32_t stlink_reg_read(ADIv5_AP_t *ap, int num)
+{
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READREG, num,
+					   ap->apsel};
+	uint8_t res[8];
+	send_recv(cmd, 16, res, 8);
+	stlink_usb_error_check(res, true);
+	uint32_t ret = res[0] | res[1] << 8 | res[2] << 16 | res[3] << 24;
+	DEBUG_STLINK("AP %d: Read reg %02" PRId32 " val 0x%08" PRIx32 "\n",
+				 ap->apsel, num, ret);
+	return ret;
+}
+
+void stlink_reg_write(ADIv5_AP_t *ap, int num, uint32_t val)
+{
+	uint8_t cmd[16] = {
+		STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_WRITEREG, num,
+		val & 0xff, (val >>  8) & 0xff, (val >> 16) & 0xff,
+		(val >> 24) & 0xff, ap->apsel};
+	uint8_t res[2];
+	send_recv(cmd, 16, res, 2);
+	DEBUG_STLINK("AP %d: Write reg %02" PRId32 " val 0x%08" PRIx32 "\n",
+				 ap->apsel, num, val);
+	stlink_usb_error_check(res, true);
+}
+
+static void stlink_mem_write_sized(	ADIv5_AP_t *ap, uint32_t dest,
+									const void *src, size_t len,
+									enum align align)
+{
+	if (len == 0)
+		return;
+	switch(align) {
+	case ALIGN_BYTE: stlink_writemem8(ap, dest, len, (uint8_t *) src);
+		break;
+	case ALIGN_HALFWORD: stlink_writemem16(ap, dest, len, (uint16_t *) src);
+		break;
+	case ALIGN_WORD: stlink_writemem32(ap, dest, len, (uint32_t *) src);
+		break;
+	case ALIGN_DWORD: stlink_writemem32(ap, dest, len, (uint32_t *) src);
+		break;
+	}
+}
+
+static void stlink_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+{
+       stlink_write_dp_register(ap->apsel, addr, value);
+}
+
+static uint32_t stlink_ap_read(ADIv5_AP_t *ap, uint16_t addr)
+{
+	uint32_t ret;
+	stlink_read_dp_register(ap->apsel, addr, &ret);
+	return ret;
+}
+
+struct jtag_dev_s jtag_devs[JTAG_MAX_DEVS+1];
+int jtag_dev_count;
+jtag_proc_t jtag_proc;
+
+int jtag_scan_stlinkv2(const uint8_t *irlens)
+{
+	uint32_t idcodes[JTAG_MAX_DEVS+1];
+	(void) *irlens;
+	target_list_free();
+
+	jtag_dev_count = 0;
+	memset(&jtag_devs, 0, sizeof(jtag_devs));
+	if (stlink_enter_debug_jtag())
+		return 0;
+	jtag_dev_count = stlink_read_idcodes(idcodes);
+	/* Check for known devices and handle accordingly */
+	for(int i = 0; i < jtag_dev_count; i++)
+		jtag_devs[i].idcode = idcodes[i];
+	for(int i = 0; i < jtag_dev_count; i++)
+		for(int j = 0; dev_descr[j].idcode; j++)
+			if((jtag_devs[i].idcode & dev_descr[j].idmask) ==
+			   dev_descr[j].idcode) {
+				if(dev_descr[j].handler)
+					dev_descr[j].handler(&jtag_devs[i]);
+				break;
+			}
+
+	return jtag_dev_count;
+}
+
+int platform_jtag_dp_init(ADIv5_DP_t *dp)
+{
+	dp->dp_read = stlink_dp_read;
+	dp->error = stlink_dp_error;
+	dp->low_access = stlink_dp_low_access;
+	dp->abort = stlink_dp_abort;
+
+	return true;
+
+}
+
+int platform_adiv5_dp_defaults(ADIv5_DP_t *dp)
+{
+	dp->ap_regs_read = stlink_regs_read;
+	dp->ap_reg_read = stlink_reg_read;
+	dp->ap_reg_write = stlink_reg_write;
+	dp->ap_setup = stlink_ap_setup;
+	dp->ap_cleanup = stlink_ap_cleanup;
+	dp->ap_write = stlink_ap_write;
+	dp->ap_read = stlink_ap_read;
+	dp->mem_read = stlink_readmem;
+	dp->mem_write_sized = stlink_mem_write_sized;
+
+	return 0;
+}
diff --git a/src/platforms/hosted/stlinkv2.h b/src/platforms/hosted/stlinkv2.h
new file mode 100644
index 0000000..dea9a8b
--- /dev/null
+++ b/src/platforms/hosted/stlinkv2.h
@@ -0,0 +1,52 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2019 Uwe Bonnes
+ *
+ * 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/>.
+ */
+#if !defined(__STLINKV2_H_)
+
+#define STLINK_ERROR_FAIL -1
+#define STLINK_ERROR_OK 0
+#define STLINK_ERROR_WAIT 1
+
+#define STLINK_DEBUG_PORT_ACCESS            0xffff
+
+void stlink_init(int argc, char **argv);
+int stlink_hwversion(void);
+void stlink_leave_state(void);
+const char *stlink_target_voltage(void);
+void stlink_srst_set_val(bool assert);
+int stlink_enter_debug_swd(void);
+int stlink_enter_debug_jtag(void);
+int stlink_read_idcodes(uint32_t *);
+uint32_t stlink_read_coreid(void);
+int stlink_read_dp_register(uint16_t port, uint16_t addr, uint32_t *res);
+int stlink_write_dp_register(uint16_t port, uint16_t addr, uint32_t val);
+
+uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
+				      uint16_t addr, uint32_t value);
+uint32_t stlink_dp_read(ADIv5_DP_t *dp, uint16_t addr);
+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);
+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);
+extern  int debug_level;
+# define DEBUG_STLINK if (debug_level > 0) printf
+# define DEBUG_USB    if (debug_level > 1) printf
+#endif

From dd022fcb4416bb4a3021f24d34540473c07ee4a9 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 25 Apr 2020 14:39:53 +0200
Subject: [PATCH 24/43] platforms/pc: Add common usb utilities.

---
 src/platforms/hosted/Makefile.inc |   2 +-
 src/platforms/pc/cl_utils.h       |   1 +
 src/platforms/pc/libusb_utils.c   | 144 ++++++++++++++++++++++++++++++
 src/platforms/pc/libusb_utils.h   |  41 +++++++++
 src/platforms/pc/serial_unix.c    |   2 +-
 src/platforms/pc/serial_win.c     |   1 -
 6 files changed, 188 insertions(+), 3 deletions(-)
 create mode 100644 src/platforms/pc/libusb_utils.c
 create mode 100644 src/platforms/pc/libusb_utils.h

diff --git a/src/platforms/hosted/Makefile.inc b/src/platforms/hosted/Makefile.inc
index fc00abe..d11199f 100644
--- a/src/platforms/hosted/Makefile.inc
+++ b/src/platforms/hosted/Makefile.inc
@@ -8,5 +8,5 @@ else ifneq (, $(findstring cygwin, $(SYS)))
 LDFLAGS += -lws2_32
 endif
 VPATH += platforms/pc
-SRC += timing.c cl_utils.c utils.c
+SRC += timing.c cl_utils.c utils.c libusb_utils.c
 PC_HOSTED = 1
diff --git a/src/platforms/pc/cl_utils.h b/src/platforms/pc/cl_utils.h
index f25bd3f..676fbfb 100644
--- a/src/platforms/pc/cl_utils.h
+++ b/src/platforms/pc/cl_utils.h
@@ -53,6 +53,7 @@ typedef struct BMP_CL_OPTIONS_s {
 	char     *opt_idstring;
 }BMP_CL_OPTIONS_t;
 
+extern int cl_debuglevel;
 void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv);
 int cl_execute(BMP_CL_OPTIONS_t *opt);
 int serial_open(BMP_CL_OPTIONS_t *opt);
diff --git a/src/platforms/pc/libusb_utils.c b/src/platforms/pc/libusb_utils.c
new file mode 100644
index 0000000..26c95a0
--- /dev/null
+++ b/src/platforms/pc/libusb_utils.c
@@ -0,0 +1,144 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2020 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 "general.h"
+#include "cl_utils.h"
+#include <libusb_utils.h>
+
+static void LIBUSB_CALL on_trans_done(struct libusb_transfer *trans)
+{
+    struct trans_ctx * const ctx = trans->user_data;
+
+    if (trans->status != LIBUSB_TRANSFER_COMPLETED)
+    {
+		fprintf(stderr, "on_trans_done: ");
+        if(trans->status == LIBUSB_TRANSFER_TIMED_OUT)  {
+            fprintf(stderr, " Timeout\n");
+        } else if (trans->status == LIBUSB_TRANSFER_CANCELLED) {
+            fprintf(stderr, " cancelled\n");
+        } else if (trans->status == LIBUSB_TRANSFER_NO_DEVICE) {
+            fprintf(stderr, " no device\n");
+        } else {
+            fprintf(stderr, " unknown\n");
+		}
+        ctx->flags |= TRANS_FLAGS_HAS_ERROR;
+    }
+    ctx->flags |= TRANS_FLAGS_IS_DONE;
+}
+
+static int submit_wait(usb_link_t *link, struct libusb_transfer *trans) {
+	struct trans_ctx trans_ctx;
+	enum libusb_error error;
+
+	trans_ctx.flags = 0;
+
+	/* brief intrusion inside the libusb interface */
+	trans->callback = on_trans_done;
+	trans->user_data = &trans_ctx;
+
+	if ((error = libusb_submit_transfer(trans))) {
+		fprintf(stderr, "libusb_submit_transfer(%d): %s\n", error,
+			  libusb_strerror(error));
+		exit(-1);
+	}
+
+	uint32_t start_time = platform_time_ms();
+	while (trans_ctx.flags == 0) {
+		struct timeval timeout;
+		timeout.tv_sec = 1;
+		timeout.tv_usec = 0;
+		if (libusb_handle_events_timeout(link->ul_libusb_ctx, &timeout)) {
+			fprintf(stderr, "libusb_handle_events()\n");
+			return -1;
+		}
+		uint32_t now = platform_time_ms();
+		if (now - start_time > 1000) {
+			libusb_cancel_transfer(trans);
+			fprintf(stderr, "libusb_handle_events() timeout\n");
+			return -1;
+		}
+	}
+	if (trans_ctx.flags & TRANS_FLAGS_HAS_ERROR) {
+		fprintf(stderr, "libusb_handle_events() | has_error\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+/* One USB transaction */
+int send_recv(usb_link_t *link,
+					 uint8_t *txbuf, size_t txsize,
+					 uint8_t *rxbuf, size_t rxsize)
+{
+	int res = 0;
+	if( txsize) {
+		int txlen = txsize;
+		libusb_fill_bulk_transfer(link->req_trans,
+								  link->ul_libusb_device_handle,
+								  link->ep_tx | LIBUSB_ENDPOINT_OUT,
+								  txbuf, txlen,
+								  NULL, NULL, 0);
+		if (cl_debuglevel > 0) {
+			int i = 0;
+			printf(" Send (%3d): ", txlen);
+			for (; i < txlen; i++) {
+				printf("%02x", txbuf[i]);
+				if ((i & 7) == 7)
+					printf(".");
+				if ((i & 31) == 31)
+					printf("\n             ");
+			}
+			if (!(i & 31))
+				printf("\n");
+		}
+		if (submit_wait(link, link->req_trans)) {
+			libusb_clear_halt(link->ul_libusb_device_handle, link->ep_tx);
+			return -1;
+		}
+	}
+	/* send_only */
+	if (rxsize != 0) {
+		/* read the response */
+		libusb_fill_bulk_transfer(link->rep_trans, link->ul_libusb_device_handle,
+								  link->ep_rx | LIBUSB_ENDPOINT_IN,
+								  rxbuf, rxsize, NULL, NULL, 0);
+
+		if (submit_wait(link, link->rep_trans)) {
+			DEBUG("clear 1\n");
+			libusb_clear_halt(link->ul_libusb_device_handle, link->ep_rx);
+			return -1;
+		}
+		res = link->rep_trans->actual_length;
+		if (res >0) {
+			int i;
+			uint8_t *p = rxbuf;
+			if (cl_debuglevel > 0) {
+				printf(" Rec (%zu/%d)", rxsize, res);
+				for (i = 0; i < res && i < 32 ; i++) {
+					if ( i && ((i & 7) == 0))
+						printf(".");
+					printf("%02x", p[i]);
+				}
+			}
+		}
+	}
+	if (cl_debuglevel > 0)
+		printf("\n");
+	return res;
+}
diff --git a/src/platforms/pc/libusb_utils.h b/src/platforms/pc/libusb_utils.h
new file mode 100644
index 0000000..8f6d868
--- /dev/null
+++ b/src/platforms/pc/libusb_utils.h
@@ -0,0 +1,41 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2020 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/>.
+ */
+#if !defined(__LIBUSB_UTILS_H)
+#define      __LIBUSB_UTILS_H
+#include <libusb-1.0/libusb.h>
+
+struct trans_ctx {
+#define TRANS_FLAGS_IS_DONE (1 << 0)
+#define TRANS_FLAGS_HAS_ERROR (1 << 1)
+    volatile unsigned long flags;
+};
+
+typedef struct {
+	libusb_context        *ul_libusb_ctx;
+	libusb_device_handle  *ul_libusb_device_handle;
+	unsigned char         ep_tx;
+	unsigned char         ep_rx;
+	struct libusb_transfer* req_trans;
+	struct libusb_transfer* rep_trans;
+	void                  *priv;
+} usb_link_t;
+
+int send_recv(usb_link_t *link, uint8_t *txbuf, size_t txsize,
+			  uint8_t *rxbuf, size_t rxsize);
+#endif
diff --git a/src/platforms/pc/serial_unix.c b/src/platforms/pc/serial_unix.c
index cc18657..214bcc1 100644
--- a/src/platforms/pc/serial_unix.c
+++ b/src/platforms/pc/serial_unix.c
@@ -31,7 +31,7 @@
 #include "cl_utils.h"
 
 static int fd;  /* File descriptor for connection to GDB remote */
-extern int cl_debuglevel;
+
 /* A nice routine grabbed from
  * https://stackoverflow.com/questions/6947413/how-to-open-read-and-write-from-serial-port-in-c
  */
diff --git a/src/platforms/pc/serial_win.c b/src/platforms/pc/serial_win.c
index a39d1a1..f9da89f 100644
--- a/src/platforms/pc/serial_win.c
+++ b/src/platforms/pc/serial_win.c
@@ -24,7 +24,6 @@
 #include "cl_utils.h"
 
 HANDLE hComm;
-extern int cl_debuglevel;
 
 int serial_open(BMP_CL_OPTIONS_t *opt)
 {

From a09104c281a092ed5d69f2179980e0de33c5c380 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Mon, 20 Apr 2020 17:36:21 +0200
Subject: [PATCH 25/43] Build firmware executables as *.elf file

---
 .gitignore                        |  1 +
 src/Makefile                      | 14 ++++++++++----
 src/platforms/native/Makefile.inc |  2 +-
 src/platforms/stlink/Makefile.inc |  8 ++++----
 src/platforms/swlink/Makefile.inc |  4 ++--
 5 files changed, 18 insertions(+), 11 deletions(-)

diff --git a/.gitignore b/.gitignore
index 945fd3d..322deda 100644
--- a/.gitignore
+++ b/.gitignore
@@ -16,4 +16,5 @@ tags
 *.b#*
 blackmagic_upgrade
 *.exe
+*.elf
 .vscode
diff --git a/src/Makefile b/src/Makefile
index 6c9d931..35ec8f2 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -67,7 +67,11 @@ CFLAGS += $(OPT_FLAGS)
 LDFLAGS += $(OPT_FLAGS)
 
 ifndef TARGET
-TARGET=blackmagic
+ifdef PC_HOSTED
+TARGET = blackmagic
+else
+TARGET = blackmagic.elf
+endif
 endif
 
 ifdef NO_OWN_LL
@@ -96,19 +100,21 @@ $(TARGET): include/version.h $(OBJ)
 	@echo "  AS      $<"
 	$(Q)$(CC) $(CFLAGS) -c $< -o $@
 
-%.bin:	%
+ifndef PC_HOSTED
+%.bin:	%.elf
 	@echo "  OBJCOPY $@"
 	$(Q)$(OBJCOPY) -O binary $^ $@
 
-%.hex:	%
+%.hex:	%.elf
 	@echo "  OBJCOPY $@"
 	$(Q)$(OBJCOPY) -O ihex $^ $@
+endif
 
 .PHONY:	clean host_clean all_platforms FORCE
 
 clean:	host_clean
 	$(Q)echo "  CLEAN"
-	-$(Q)$(RM) *.o *.d *~ blackmagic $(HOSTFILES)
+	-$(Q)$(RM) *.o *.d *.elf *~ $(TARGET) $(HOSTFILES)
 	-$(Q)$(RM) platforms/*/*.o platforms/*/*.d mapfile include/version.h
 
 all_platforms:
diff --git a/src/platforms/native/Makefile.inc b/src/platforms/native/Makefile.inc
index d3cef33..6d3e8ea 100644
--- a/src/platforms/native/Makefile.inc
+++ b/src/platforms/native/Makefile.inc
@@ -30,7 +30,7 @@ SRC += 	cdcacm.c	\
 
 all:	blackmagic.bin blackmagic_dfu.bin blackmagic_dfu.hex
 
-blackmagic_dfu: usbdfu.o dfucore.o dfu_f1.o
+blackmagic_dfu.elf: usbdfu.o dfucore.o dfu_f1.o
 	@echo "  LD      $@"
 	$(Q)$(CC) $^ -o $@ $(LDFLAGS_BOOT)
 
diff --git a/src/platforms/stlink/Makefile.inc b/src/platforms/stlink/Makefile.inc
index a0cf7de..66feef6 100644
--- a/src/platforms/stlink/Makefile.inc
+++ b/src/platforms/stlink/Makefile.inc
@@ -37,18 +37,18 @@ SRC += 	cdcacm.c	\
 	stlink_common.c \
 
 ifeq ($(ST_BOOTLOADER), 1)
-all:	blackmagic.bin blackmagic_dfu.bin blackmagic_dfu.hex
+all:	blackmagic.bin
 else
 all:	blackmagic.bin blackmagic_dfu.bin blackmagic_dfu.hex dfu_upgrade.bin dfu_upgrade.hex
 endif
-blackmagic_dfu: usbdfu.o dfucore.o dfu_f1.o stlink_common.o
+blackmagic_dfu.elf: 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  stlink_common.o
+dfu_upgrade.elf: dfu_upgrade.o dfucore.o dfu_f1.o  stlink_common.o
 	@echo "  LD      $@"
 	$(Q)$(CC) $^ -o $@ $(LDFLAGS)
 
 host_clean:
-	-$(Q)$(RM) blackmagic.bin blackmagic_dfu blackmagic_dfu.bin blackmagic_dfu.hex dfu_upgrade dfu_upgrade.bin dfu_upgrade.hex
+	-$(Q)$(RM) *.bin *elf *hex
 
diff --git a/src/platforms/swlink/Makefile.inc b/src/platforms/swlink/Makefile.inc
index b3831f7..5ac76ec 100644
--- a/src/platforms/swlink/Makefile.inc
+++ b/src/platforms/swlink/Makefile.inc
@@ -31,11 +31,11 @@ SRC += 	cdcacm.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 platform_common.o
+blackmagic_dfu.elf: usbdfu.o dfucore.o dfu_f1.o platform_common.o
 	@echo "  LD      $@"
 	$(Q)$(CC) $^ -o $@ $(LDFLAGS_BOOT)
 
-dfu_upgrade: dfu_upgrade.o dfucore.o dfu_f1.o platform_common.o
+dfu_upgrade.elf: dfu_upgrade.o dfucore.o dfu_f1.o platform_common.o
 	@echo "  LD      $@"
 	$(Q)$(CC) $^ -o $@ $(LDFLAGS)
 

From 8fea3f6baa5fa26999d9113f6a83e36b6685b061 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sun, 19 Apr 2020 16:10:02 +0200
Subject: [PATCH 26/43] pc/serial_xx: Change arguments for the open call.

---
 src/platforms/hosted/platform.c    |  4 +++-
 src/platforms/pc-hosted/platform.c |  2 +-
 src/platforms/pc/cl_utils.h        |  4 +++-
 src/platforms/pc/serial_unix.c     | 13 ++++++-------
 src/platforms/pc/serial_win.c      | 12 +++++++-----
 5 files changed, 20 insertions(+), 15 deletions(-)

diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index a10a7a3..8f256f7 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -194,7 +194,9 @@ void platform_init(int argc, char **argv)
 				libusb_strerror(res));
 		exit(-1);
 	}
-	if (find_debuggers(&cl_opts, &info)) {
+	if (cl_opts.opt_device) {
+		info.bmp_type = BMP_TYPE_BMP;
+	} else if (find_debuggers(&cl_opts, &info)) {
 		exit(-1);
 	}
 	printf("Using %s %s %s\n", info.serial,
diff --git a/src/platforms/pc-hosted/platform.c b/src/platforms/pc-hosted/platform.c
index 4be4cde..849b926 100644
--- a/src/platforms/pc-hosted/platform.c
+++ b/src/platforms/pc-hosted/platform.c
@@ -60,7 +60,7 @@ void platform_init(int argc, char **argv)
   printf("License GPLv3+: GNU GPL version 3 or later "
 	 "<http://gnu.org/licenses/gpl.html>\n\n");
 
-  if (serial_open(&cl_opts))
+  if (serial_open(cl_opts.opt_device, cl_opts.opt_serial))
 	  exit(-1);
   int c=snprintf(construct,PLATFORM_MAX_MSG_SIZE,"%s",REMOTE_START_STR);
   platform_buffer_write((uint8_t *)construct,c);
diff --git a/src/platforms/pc/cl_utils.h b/src/platforms/pc/cl_utils.h
index 676fbfb..5c485b7 100644
--- a/src/platforms/pc/cl_utils.h
+++ b/src/platforms/pc/cl_utils.h
@@ -24,6 +24,8 @@
 #if !defined(__CL_UTILS_H)
 #define __CL_UTILS_H
 
+#define RESP_TIMEOUT (100)
+
 enum bmp_cl_mode {
 	BMP_MODE_DEBUG,
 	BMP_MODE_TEST,
@@ -56,6 +58,6 @@ typedef struct BMP_CL_OPTIONS_s {
 extern int cl_debuglevel;
 void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv);
 int cl_execute(BMP_CL_OPTIONS_t *opt);
-int serial_open(BMP_CL_OPTIONS_t *opt);
+int serial_open(BMP_CL_OPTIONS_t *opt, char *serial);
 void serial_close(void);
 #endif
diff --git a/src/platforms/pc/serial_unix.c b/src/platforms/pc/serial_unix.c
index 214bcc1..2d36259 100644
--- a/src/platforms/pc/serial_unix.c
+++ b/src/platforms/pc/serial_unix.c
@@ -69,10 +69,10 @@ static int set_interface_attribs(void)
 }
 #define BMP_IDSTRING "usb-Black_Sphere_Technologies_Black_Magic_Probe"
 #define DEVICE_BY_ID "/dev/serial/by-id/"
-int serial_open(BMP_CL_OPTIONS_t *opt)
+int serial_open(BMP_CL_OPTIONS_t *cl_opts, char *serial)
 {
 	char name[4096];
-	if (!opt->opt_device) {
+	if (!cl_opts->opt_device) {
 		/* Try to find some BMP if0*/
 		struct dirent *dp;
 		DIR *dir = opendir(DEVICE_BY_ID);
@@ -86,8 +86,7 @@ int serial_open(BMP_CL_OPTIONS_t *opt)
 			if ((strstr(dp->d_name, BMP_IDSTRING)) &&
 				(strstr(dp->d_name, "-if00"))) {
 				num_total++;
-				if (((opt->opt_serial) &&
-				 (!strstr(dp->d_name, opt->opt_serial))))
+				if ((serial) && (!strstr(dp->d_name, serial)))
 					continue;
 				num_devices++;
 				strcpy(name, DEVICE_BY_ID);
@@ -108,8 +107,8 @@ int serial_open(BMP_CL_OPTIONS_t *opt)
 						fprintf(stderr, "%s\n", dp->d_name);
 				}
 				closedir(dir);
-				if (opt->opt_serial)
-					fprintf(stderr, "Do no match given serial \"%s\"\n", opt->opt_serial);
+				if (serial)
+					fprintf(stderr, "Do no match given serial \"%s\"\n", serial);
 				else
 					fprintf(stderr, "Select Probe with -s <(Partial) Serial Number\n");
 			} else {
@@ -118,7 +117,7 @@ int serial_open(BMP_CL_OPTIONS_t *opt)
 			return -1;
 		}
 	} else {
-		strncpy(name, opt->opt_device, sizeof(name) - 1);
+		strncpy(name, cl_opts->opt_device, sizeof(name) - 1);
 	}
 	fd = open(name, O_RDWR | O_SYNC | O_NOCTTY);
 	if (fd < 0) {
diff --git a/src/platforms/pc/serial_win.c b/src/platforms/pc/serial_win.c
index f9da89f..3baec32 100644
--- a/src/platforms/pc/serial_win.c
+++ b/src/platforms/pc/serial_win.c
@@ -25,18 +25,20 @@
 
 HANDLE hComm;
 
-int serial_open(BMP_CL_OPTIONS_t *opt)
+int serial_open(BMP_CL_OPTIONS_t *cl_opts, char * serial)
 {
-	if (!opt->opt_device) {
+	(void) serial; /* FIXME: Does Windows allow open with USB serial no? */
+	if (!cl_opts->opt_device) {
 		fprintf(stderr,"Specify the serial device to use!\n");
 		return -1;
 	}
 	char device[256];
-	if (strstr(opt->opt_device, "\\\\.\\")) {
-		strncpy(device, opt->opt_device, sizeof(device) - 1);
+	if (strstr(device, "\\\\.\\")) {
+		strncpy(device, cl_opts->opt_device, sizeof(cl_opts->opt_device) - 1);
 	} else {
 		strcpy(device,  "\\\\.\\");
-		strncat(device, opt->opt_device, sizeof(device) - strlen(device) - 1);
+		strncat(device, cl_opts->opt_device,
+				sizeof(cl_opts->opt_device) - strlen(cl_opts->opt_device) - 1);
 	}
 	hComm = CreateFile(device,                //port name
                       GENERIC_READ | GENERIC_WRITE, //Read/Write

From 09602a9e74c0649995137e2833de85040e026a4f Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sun, 3 May 2020 17:12:01 +0200
Subject: [PATCH 27/43] serial_unix: Simplify timeout setting.

---
 src/platforms/pc/serial_unix.c | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/platforms/pc/serial_unix.c b/src/platforms/pc/serial_unix.c
index 2d36259..fd63dc1 100644
--- a/src/platforms/pc/serial_unix.c
+++ b/src/platforms/pc/serial_unix.c
@@ -155,15 +155,13 @@ int platform_buffer_read(uint8_t *data, int maxsize)
 	uint8_t *c;
 	int s;
 	int ret;
-	uint32_t endTime;
 	fd_set  rset;
 	struct timeval tv;
 
 	c = data;
 	tv.tv_sec = 0;
 
-	endTime = platform_time_ms() + RESP_TIMEOUT;
-	tv.tv_usec = 1000 * (endTime - platform_time_ms());
+	tv.tv_usec = 1000 * RESP_TIMEOUT;
 
 	/* Look for start of response */
 	do {

From 13c3c934d2bdc3fd8a3ec008395179345b0cc84d Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sun, 19 Apr 2020 15:50:50 +0200
Subject: [PATCH 28/43] Hosted: Add Stlink, remove old pc-stlinkv2 platform

---
 src/platforms/hosted/Makefile.inc      |    1 +
 src/platforms/hosted/platform.c        |  146 ++-
 src/platforms/hosted/platform.h        |    6 +
 src/platforms/hosted/stlinkv2.c        |  671 ++++-------
 src/platforms/hosted/stlinkv2.h        |   30 +-
 src/platforms/pc-stlinkv2/Makefile.inc |   14 -
 src/platforms/pc-stlinkv2/README.md    |   25 -
 src/platforms/pc-stlinkv2/jtagtap.c    |    0
 src/platforms/pc-stlinkv2/platform.c   |  115 --
 src/platforms/pc-stlinkv2/platform.h   |   46 -
 src/platforms/pc-stlinkv2/stlinkv2.c   | 1435 ------------------------
 src/platforms/pc-stlinkv2/stlinkv2.h   |   52 -
 src/platforms/pc-stlinkv2/swdptap.c    |    0
 src/platforms/pc/cl_utils.c            |   23 +-
 src/platforms/pc/cl_utils.h            |    1 -
 src/platforms/pc/libusb_utils.c        |    1 -
 16 files changed, 340 insertions(+), 2226 deletions(-)
 delete mode 100644 src/platforms/pc-stlinkv2/Makefile.inc
 delete mode 100644 src/platforms/pc-stlinkv2/README.md
 delete mode 100644 src/platforms/pc-stlinkv2/jtagtap.c
 delete mode 100644 src/platforms/pc-stlinkv2/platform.c
 delete mode 100644 src/platforms/pc-stlinkv2/platform.h
 delete mode 100644 src/platforms/pc-stlinkv2/stlinkv2.c
 delete mode 100644 src/platforms/pc-stlinkv2/stlinkv2.h
 delete mode 100644 src/platforms/pc-stlinkv2/swdptap.c

diff --git a/src/platforms/hosted/Makefile.inc b/src/platforms/hosted/Makefile.inc
index d11199f..94ed44b 100644
--- a/src/platforms/hosted/Makefile.inc
+++ b/src/platforms/hosted/Makefile.inc
@@ -9,4 +9,5 @@ LDFLAGS += -lws2_32
 endif
 VPATH += platforms/pc
 SRC += timing.c cl_utils.c utils.c libusb_utils.c
+SRC += stlinkv2.c
 PC_HOSTED = 1
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 8f256f7..69f340b 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -24,9 +24,14 @@
 #include "general.h"
 #include "swdptap.h"
 #include "jtagtap.h"
+#include "target.h"
+#include "target_internal.h"
+#include "adiv5.h"
 #include "timing.h"
 #include "cl_utils.h"
+#include "gdb_if.h"
 #include <signal.h>
+#include "stlinkv2.h"
 
 #define VENDOR_ID_BMP            0x1d50
 #define PRODUCT_ID_BMP           0x6018
@@ -48,8 +53,20 @@ jtag_proc_t jtag_proc;
 
 static void exit_function(void)
 {
-	libusb_exit(info.libusb_ctx);
-	fprintf(stderr, "INFO: Cleanup\n");
+	if(info.usb_link) {
+		libusb_free_transfer(info.usb_link->req_trans);
+		libusb_free_transfer(info.usb_link->rep_trans);
+		if (info.usb_link->ul_libusb_device_handle) {
+			libusb_release_interface (
+				info.usb_link->ul_libusb_device_handle, 0);
+			libusb_close(info.usb_link->ul_libusb_device_handle);
+		}
+	}
+	switch (info.bmp_type) {
+	default:
+		break;
+	}
+	fflush(stdout);
 }
 
 /* SIGTERM handler. */
@@ -82,7 +99,7 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 		if (res < 0) {
             fprintf(stderr, "WARN: libusb_get_device_descriptor() failed: %s",
 					libusb_strerror(res));
-			libusb_free_device_list(devs, 0);
+			libusb_free_device_list(devs, 1);
 			continue;
 		}
 		libusb_device_handle *handle;
@@ -119,13 +136,13 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 				continue;
 			}
 		}
+		libusb_close(handle);
 		if (cl_opts->opt_ident_string) {
 			char *match_manu = NULL;
 			char *match_product = NULL;
 			match_manu = strstr(manufacturer,	cl_opts->opt_ident_string);
 			match_product = strstr(product, cl_opts->opt_ident_string);
 			if (!match_manu && !match_product) {
-				libusb_close(handle);
 				continue;
 			}
 		}
@@ -135,16 +152,16 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 			(desc.idProduct == PRODUCT_ID_BMP)) {
 			type = BMP_TYPE_BMP;
 		} else if (desc.idVendor ==  VENDOR_ID_STLINK) {
-			if (desc.idProduct == PRODUCT_ID_STLINKV1) {
-				fprintf(stderr, "INFO: STLINKV1 not supported\n");
-				continue;
-			}
 			if ((desc.idProduct == PRODUCT_ID_STLINKV2) ||
 				(desc.idProduct == PRODUCT_ID_STLINKV21) ||
 				(desc.idProduct == PRODUCT_ID_STLINKV21_MSD) ||
 				(desc.idProduct == PRODUCT_ID_STLINKV3) ||
 				(desc.idProduct == PRODUCT_ID_STLINKV3E)) {
 				type = BMP_TYPE_STLINKV2;
+			} else {
+				if (desc.idProduct == PRODUCT_ID_STLINKV1)
+					fprintf(stderr, "INFO: STLINKV1 not supported\n");
+				continue;
 			}
 		} else {
 			continue;
@@ -176,8 +193,8 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 			goto rescan;
 		}
 	}
-	libusb_free_device_list(devs, 0);
-	return 0;
+	libusb_free_device_list(devs, 1);
+	return (found_debuggers == 1) ? 0 : -1;
 }
 
 void platform_init(int argc, char **argv)
@@ -202,39 +219,98 @@ void platform_init(int argc, char **argv)
 	printf("Using %s %s %s\n", info.serial,
 		   info.manufacturer,
 		   info.product);
-	exit(-1);
+	switch (info.bmp_type) {
+	case BMP_TYPE_STLINKV2:
+		if (stlink_init( &info))
+			exit(-1);
+		break;
+	default:
+		exit(-1);
+	}
+	int ret = -1;
+	if (cl_opts.opt_mode != BMP_MODE_DEBUG) {
+		ret = cl_execute(&cl_opts);
+	} else {
+		gdb_if_init();
+		return;
+	}
+	exit(ret);
 }
 
 int platform_adiv5_swdp_scan(void)
 {
-	return -1;
+	switch (info.bmp_type) {
+	case BMP_TYPE_STLINKV2:
+	{
+		target_list_free();
+		ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
+		if (!stlink_enter_debug_swd(&info, dp)) {
+			adiv5_dp_init(dp);
+			if (target_list)
+				return 1;
+		}
+		free(dp);
+		break;
+	}
+	default:
+		return 0;
+	}
+	return 0;
 }
 
 int platform_swdptap_init(void)
 {
+	switch (info.bmp_type) {
+	case BMP_TYPE_STLINKV2:
+		return 0;
+		break;
+	default:
+		return -1;
+	}
 	return -1;
 }
 
 int platform_jtag_scan(const uint8_t *lrlens)
 {
-	(void) lrlens;
+	switch (info.bmp_type) {
+	case BMP_TYPE_STLINKV2:
+		return jtag_scan_stlinkv2(&info, lrlens);
+	default:
+		return -1;
+	}
 	return -1;
 }
 
 int platform_jtagtap_init(void)
 {
-	return 0;
-}
-
-int platform_adiv5_dp_defaults(void *arg)
-{
-	(void)arg;
+	switch (info.bmp_type) {
+	case BMP_TYPE_STLINKV2:
+		return 0;
+	default:
+		return -1;
+	}
 	return -1;
 }
 
-int platform_jtag_dp_init()
+void platform_adiv5_dp_defaults(ADIv5_DP_t *dp)
 {
-        return 0;
+	switch (info.bmp_type) {
+	case BMP_TYPE_STLINKV2:
+		return stlink_adiv5_dp_defaults(dp);
+	default:
+		break;
+	}
+}
+
+int platform_jtag_dp_init(ADIv5_DP_t *dp)
+{
+	switch (info.bmp_type) {
+	case BMP_TYPE_STLINKV2:
+		return stlink_jtag_dp_init(dp);
+	default:
+		return -1;
+	}
+	return 0;
 }
 
 char *platform_ident(void)
@@ -258,10 +334,34 @@ char *platform_ident(void)
 
 const char *platform_target_voltage(void)
 {
+	switch (info.bmp_type) {
+	case BMP_TYPE_STLINKV2:
+		return stlink_target_voltage(&info);
+	default:
+		break;;
+	}
 	return NULL;
 }
 
-void platform_srst_set_val(bool assert) {(void) assert;}
+void platform_srst_set_val(bool assert)
+{
+	switch (info.bmp_type) {
+	case BMP_TYPE_STLINKV2:
+		return stlink_srst_set_val(&info, assert);
+	default:
+		break;
+	}
+}
+
+bool platform_srst_get_val(void)
+{
+	switch (info.bmp_type) {
+	case BMP_TYPE_STLINKV2:
+		return stlink_srst_get_val();
+	default:
+		break;
+	}
+	return false;
+}
 
-bool platform_srst_get_val(void) { return false;}
 void platform_buffer_flush(void) {}
diff --git a/src/platforms/hosted/platform.h b/src/platforms/hosted/platform.h
index 0a28110..5a9cea2 100644
--- a/src/platforms/hosted/platform.h
+++ b/src/platforms/hosted/platform.h
@@ -1,6 +1,8 @@
 #ifndef __PLATFORM_H
 #define __PLATFORM_H
 #include <libusb-1.0/libusb.h>
+#include "libusb_utils.h"
+
 #include "timing.h"
 
 char *platform_ident(void);
@@ -22,6 +24,8 @@ typedef enum bmp_type_s {
 typedef struct bmp_info_s {
 	bmp_type_t bmp_type;
 	libusb_context *libusb_ctx;
+	struct ftdi_context *ftdic;
+	usb_link_t *usb_link;
 	unsigned int vid;
 	unsigned int pid;
 	char dev;
@@ -30,4 +34,6 @@ typedef struct bmp_info_s {
 	char product[128];
 } bmp_info_t;
 
+extern bmp_info_t info;
+
 #endif
diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 5f141d2..133606f 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -40,19 +40,6 @@
 
 #include "cl_utils.h"
 
-#if !defined(timersub)
-/* This is a copy from GNU C Library (GNU LGPL 2.1), sys/time.h. */
-# define timersub(a, b, result)					\
-	do {										\
-		(result)->tv_sec = (a)->tv_sec - (b)->tv_sec;	\
-		(result)->tv_usec = (a)->tv_usec - (b)->tv_usec;	\
-		if ((result)->tv_usec < 0) {						\
-			--(result)->tv_sec;								\
-			(result)->tv_usec += 1000000;					\
-		}													\
-	} while (0)
-#endif
-
 #define VENDOR_ID_STLINK		0x483
 #define PRODUCT_ID_STLINK_MASK	0xffe0
 #define PRODUCT_ID_STLINK_GROUP 0x3740
@@ -212,7 +199,7 @@ typedef struct {
 	uint16_t     vid;
 	uint16_t     pid;
 	uint8_t      transport_mode;
-	char         serial[32];
+	bool         srst;
 	uint8_t      dap_select;
 	uint8_t      ep_tx;
 	uint8_t      ver_hw;     /* 20, 21 or 31 deciphered from USB PID.*/
@@ -224,197 +211,15 @@ typedef struct {
 	uint8_t      ver_bridge;
 	uint16_t     block_size;
 	bool         ap_error;
-	libusb_device_handle *handle;
-	struct libusb_transfer* req_trans;
-	struct libusb_transfer* rep_trans;
 } stlink;
 
 stlink Stlink;
 
 static int stlink_usb_get_rw_status(bool verbose);
 
-static void exit_function(void)
-{
-	libusb_exit(NULL);
-	DEBUG("Cleanup\n");
-}
-
-/* SIGTERM handler. */
-static void sigterm_handler(int sig)
-{
-	(void)sig;
-	exit(0);
-}
-
-struct trans_ctx {
-#define TRANS_FLAGS_IS_DONE (1 << 0)
-#define TRANS_FLAGS_HAS_ERROR (1 << 1)
-    volatile unsigned long flags;
-};
-
 int debug_level = 0;
-bool has_attached = false;
 
-static int LIBUSB_CALL hotplug_callback_attach(
-	libusb_context *ctx, libusb_device *dev, libusb_hotplug_event event,
-	void *user_data)
-{
-	(void)ctx;
-	(void)dev;
-	(void)event;
-	(void)user_data;
-	has_attached = true;
-	return 1; /* deregister Callback*/
-}
-
-int device_detached = 0;
-static int LIBUSB_CALL hotplug_callback_detach(
-	libusb_context *ctx, libusb_device *dev, libusb_hotplug_event event,
-	void *user_data)
-{
-	(void)ctx;
-	(void)dev;
-	(void)event;
-	(void)user_data;
-	device_detached = 1;
-	return 1;  /* deregister Callback*/
-}
-
-void stlink_check_detach(int state)
-{
-	if (state == 1) {
-		/* Check for hotplug events */
-		struct timeval tv = {0,0};
-		libusb_handle_events_timeout_completed(
-			Stlink.libusb_ctx, &tv, &device_detached);
-		if (device_detached) {
-			DEBUG("Dongle was detached\n");
-			exit(0);
-		}
-	}
-}
-
-static void LIBUSB_CALL on_trans_done(struct libusb_transfer * trans)
-{
-    struct trans_ctx * const ctx = trans->user_data;
-
-    if (trans->status != LIBUSB_TRANSFER_COMPLETED)
-    {
-		DEBUG("on_trans_done: ");
-        if(trans->status == LIBUSB_TRANSFER_TIMED_OUT)
-        {
-            DEBUG("Timeout\n");
-        } else if (trans->status == LIBUSB_TRANSFER_CANCELLED) {
-            DEBUG("cancelled\n");
-        } else if (trans->status == LIBUSB_TRANSFER_NO_DEVICE) {
-            DEBUG("no device\n");
-        } else {
-            DEBUG("unknown\n");
-		}
-        ctx->flags |= TRANS_FLAGS_HAS_ERROR;
-    }
-    ctx->flags |= TRANS_FLAGS_IS_DONE;
-}
-
-static int submit_wait(struct libusb_transfer * trans) {
-	struct timeval start;
-	struct timeval now;
-	struct timeval diff;
-	struct trans_ctx trans_ctx;
-	enum libusb_error error;
-
-	trans_ctx.flags = 0;
-
-	/* brief intrusion inside the libusb interface */
-	trans->callback = on_trans_done;
-	trans->user_data = &trans_ctx;
-
-	if ((error = libusb_submit_transfer(trans))) {
-		DEBUG("libusb_submit_transfer(%d): %s\n", error,
-			  libusb_strerror(error));
-		exit(-1);
-	}
-
-	gettimeofday(&start, NULL);
-
-	while (trans_ctx.flags == 0) {
-		struct timeval timeout;
-		timeout.tv_sec = 1;
-		timeout.tv_usec = 0;
-		if (libusb_handle_events_timeout(Stlink.libusb_ctx, &timeout)) {
-			DEBUG("libusb_handle_events()\n");
-			return -1;
-		}
-
-		gettimeofday(&now, NULL);
-		timersub(&now, &start, &diff);
-		if (diff.tv_sec >= 1) {
-			libusb_cancel_transfer(trans);
-			DEBUG("libusb_handle_events() timeout\n");
-			return -1;
-		}
-	}
-
-	if (trans_ctx.flags & TRANS_FLAGS_HAS_ERROR) {
-		DEBUG("libusb_handle_events() | has_error\n");
-		return -1;
-	}
-
-	return 0;
-}
 #define STLINK_ERROR_DP_FAULT -2
-static int send_recv(uint8_t *txbuf, size_t txsize,
-					 uint8_t *rxbuf, size_t rxsize)
-{
-	int res = 0;
-	stlink_check_detach(1);
-	if( txsize) {
-		int txlen = txsize;
-		libusb_fill_bulk_transfer(Stlink.req_trans, Stlink.handle,
-								  Stlink.ep_tx | LIBUSB_ENDPOINT_OUT,
-								  txbuf, txlen,
-								  NULL, NULL,
-								  0
-			);
-		DEBUG_USB("  Send (%d): ", txlen);
-		for (int i = 0; i < txlen && i < 32 ; i++) {
-			DEBUG_USB("%02x", txbuf[i]);
-			if ((i & 7) == 7)
-				DEBUG_USB(".");
-		}
-		if (submit_wait(Stlink.req_trans)) {
-			DEBUG_USB("clear 2\n");
-			libusb_clear_halt(Stlink.handle,2);
-			return -1;
-		}
-	}
-	/* send_only */
-	if (rxsize != 0) {
-		/* read the response */
-		libusb_fill_bulk_transfer(Stlink.rep_trans, Stlink.handle,
-								  0x01| LIBUSB_ENDPOINT_IN,
-								  rxbuf, rxsize, NULL, NULL, 0);
-
-		if (submit_wait(Stlink.rep_trans)) {
-			DEBUG("clear 1\n");
-			libusb_clear_halt(Stlink.handle,1);
-			return -1;
-		}
-		res = Stlink.rep_trans->actual_length;
-		if (res >0) {
-			int i;
-			uint8_t *p = rxbuf;
-			DEBUG_USB(" Rec (%zu/%d)", rxsize, res);
-			for (i = 0; i < res && i < 32 ; i++) {
-				if ( i && ((i & 7) == 0))
-					DEBUG_USB(".");
-				DEBUG_USB("%02x", p[i]);
-			}
-		}
-	}
-	DEBUG_USB("\n");
-	return res;
-}
 
 /**
     Converts an STLINK status code held in the first byte of a response to
@@ -530,19 +335,16 @@ static int stlink_usb_error_check(uint8_t *data, bool verbose)
 static int send_recv_retry(uint8_t *txbuf, size_t txsize,
 					 uint8_t *rxbuf, size_t rxsize)
 {
-	struct timeval start;
-	struct timeval now;
-	struct timeval diff;
-	gettimeofday(&start, NULL);
+	uint32_t start = platform_time_ms();
 	int res;
+	usb_link_t *link = info.usb_link;
 	while(1) {
-		send_recv(txbuf, txsize, rxbuf, rxsize);
+		send_recv(link, txbuf, txsize, rxbuf, rxsize);
 		res = stlink_usb_error_check(rxbuf, false);
 		if (res == STLINK_ERROR_OK)
 			return res;
-		gettimeofday(&now, NULL);
-		timersub(&now, &start, &diff);
-		if ((diff.tv_sec >= 1) || (res != STLINK_ERROR_WAIT)) {
+		uint32_t now = platform_time_ms();
+		if (((now - start) > 1000) || (res != STLINK_ERROR_WAIT)) {
 			DEBUG("write_retry failed. ");
 			return res;
 		}
@@ -553,19 +355,15 @@ static int send_recv_retry(uint8_t *txbuf, size_t txsize,
 static int read_retry(uint8_t *txbuf, size_t txsize,
 					 uint8_t *rxbuf, size_t rxsize)
 {
-	struct timeval start;
-	struct timeval now;
-	struct timeval diff;
-	gettimeofday(&start, NULL);
+	uint32_t start = platform_time_ms();
 	int res;
 	while(1) {
-		send_recv(txbuf, txsize, rxbuf, rxsize);
+		send_recv(info.usb_link, txbuf, txsize, rxbuf, rxsize);
 		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)) {
+		uint32_t now = platform_time_ms();
+		if (((now -start) > 1000) || (res != STLINK_ERROR_WAIT)) {
 			DEBUG("read_retry failed. ");
 			stlink_usb_get_rw_status(true);
 			return res;
@@ -577,20 +375,17 @@ static int read_retry(uint8_t *txbuf, size_t txsize,
 static int write_retry(uint8_t *cmdbuf, size_t cmdsize,
 					 uint8_t *txbuf, size_t txsize)
 {
-	struct timeval start;
-	struct timeval now;
-	struct timeval diff;
-	gettimeofday(&start, NULL);
+	uint32_t start = platform_time_ms();
 	int res;
+	usb_link_t *link = info.usb_link;
 	while(1) {
-		send_recv(cmdbuf, cmdsize, NULL, 0);
-		send_recv(txbuf, txsize, NULL, 0);
+		send_recv(link, cmdbuf, cmdsize, NULL, 0);
+		send_recv(link, txbuf, txsize, NULL, 0);
 		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)) {
+		uint32_t now = platform_time_ms();
+		if (((now - start) > 1000) || (res != STLINK_ERROR_WAIT)) {
 			stlink_usb_get_rw_status(true);
 			return res;
 		}
@@ -598,12 +393,12 @@ static int write_retry(uint8_t *cmdbuf, size_t cmdsize,
 	return res;
 }
 
-static void stlink_version(void)
+static void stlink_version(bmp_info_t *info)
 {
 	if (Stlink.ver_hw == 30) {
 		uint8_t cmd[16] = {STLINK_APIV3_GET_VERSION_EX};
 		uint8_t data[12];
-		int size = send_recv(cmd, 16, data, 12);
+		int size = send_recv(info->usb_link, cmd, 16, data, 12);
 		if (size == -1) {
 			printf("[!] send_recv STLINK_APIV3_GET_VERSION_EX\n");
 		}
@@ -618,7 +413,7 @@ static void stlink_version(void)
 	} else {
 		uint8_t cmd[16] = {STLINK_GET_VERSION};
 		uint8_t data[6];
-		int size = send_recv(cmd, 16, data, 6);
+		int size = send_recv(info->usb_link, cmd, 16, data, 6);
 		if (size == -1) {
 			printf("[!] send_recv STLINK_GET_VERSION_EX\n");
 		}
@@ -646,23 +441,24 @@ static void stlink_version(void)
 	DEBUG("\n");
 }
 
-void stlink_leave_state(void)
+static bool stlink_leave_state(bmp_info_t *info)
 {
 	uint8_t cmd[16] = {STLINK_GET_CURRENT_MODE};
 	uint8_t data[2];
-	send_recv(cmd, 16, data, 2);
+	send_recv(info->usb_link,cmd, 16, data, 2);
 	if (data[0] == STLINK_DEV_DFU_MODE) {
 		uint8_t dfu_cmd[16] = {STLINK_DFU_COMMAND, STLINK_DFU_EXIT};
 		DEBUG("Leaving DFU Mode\n");
-		send_recv(dfu_cmd, 16, NULL, 0);
+		send_recv(info->usb_link, dfu_cmd, 16, NULL, 0);
+		return true;
 	} else if (data[0] == STLINK_DEV_SWIM_MODE) {
 		uint8_t swim_cmd[16] = {STLINK_SWIM_COMMAND, STLINK_SWIM_EXIT};
 		DEBUG("Leaving SWIM Mode\n");
-		send_recv(swim_cmd, 16, NULL, 0);
+		send_recv(info->usb_link, swim_cmd, 16, NULL, 0);
 	} else if (data[0] == STLINK_DEV_DEBUG_MODE) {
 		uint8_t dbg_cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_EXIT};
 		DEBUG("Leaving DEBUG Mode\n");
-		send_recv(dbg_cmd, 16, NULL, 0);
+		send_recv(info->usb_link, dbg_cmd, 16, NULL, 0);
 	} else if (data[0] == STLINK_DEV_BOOTLOADER_MODE) {
 		DEBUG("Leaving BOOTLOADER Mode\n");
 	} else if (data[0] == STLINK_DEV_MASS_MODE) {
@@ -670,13 +466,14 @@ void stlink_leave_state(void)
 	} else {
 		DEBUG("Unknown Mode %02x\n", data[0]);
 	}
+	return false;
 }
 
-const char *stlink_target_voltage(void)
+const char *stlink_target_voltage(bmp_info_t *info)
 {
 	uint8_t cmd[16] = {STLINK_GET_TARGET_VOLTAGE};
 	uint8_t data[8];
-	send_recv(cmd, 16, data, 8);
+	send_recv(info->usb_link, cmd, 16, data, 8);
 	uint16_t adc[2];
 	adc[0] = data[0] | data[1] << 8; /* Calibration value? */
 	adc[1] = data[4] | data[5] << 8; /* Measured value?*/
@@ -688,268 +485,161 @@ const char *stlink_target_voltage(void)
 	return res;
 }
 
-static void stlink_resetsys(void)
+static void stlink_resetsys(bmp_info_t *info)
 {
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND ,STLINK_DEBUG_APIV2_RESETSYS};
 	uint8_t data[2];
-	send_recv(cmd, 16, data, 2);
+	send_recv(info->usb_link, cmd, 16, data, 2);
 }
 
-void stlink_init(int argc, char **argv)
+int stlink_init(bmp_info_t *info)
 {
-	BMP_CL_OPTIONS_t cl_opts = {0};
-	cl_opts.opt_idstring = "Blackmagic Debug Probe on StlinkV2/3";
-	cl_init(&cl_opts, argc, argv);
+	usb_link_t *sl = calloc(1, sizeof(usb_link_t));
+	if (!sl)
+		return -1;
+	info->usb_link = sl;
+	sl->ul_libusb_ctx = info->libusb_ctx;
 	libusb_device **devs, *dev;
-	int r;
-	int ret = -1;
-	atexit(exit_function);
-	signal(SIGTERM, sigterm_handler);
-	signal(SIGINT, sigterm_handler);
-	libusb_init(&Stlink.libusb_ctx);
-	r = libusb_init(NULL);
-	if (r < 0)
-		DEBUG("Failed: %s", libusb_strerror(r));
-	bool hotplug = true;
-	if (!libusb_has_capability (LIBUSB_CAP_HAS_HOTPLUG)) {
-		printf("Hotplug capabilites are not supported on this platform\n");
-		hotplug = false;
-	}
 	ssize_t cnt;
-  rescan:
-	has_attached = 0;
-	memset(&Stlink, 0, sizeof(Stlink));
-	cnt = libusb_get_device_list(NULL, &devs);
+	cnt = libusb_get_device_list(info->libusb_ctx, &devs);
 	if (cnt < 0) {
-		DEBUG("Failed: %s", libusb_strerror(r));
-		goto error;
+		fprintf(stderr, "FATAL: Stlink libusb_get_device_list failed\n");
+		return -1;
 	}
 	int i = 0;
-	int nr_stlinks = 0;
+	bool found = false;
 	while ((dev = devs[i++]) != NULL) {
 		struct libusb_device_descriptor desc;
 		int r = libusb_get_device_descriptor(dev, &desc);
 		if (r < 0) {
 			fprintf(stderr, "libusb_get_device_descriptor failed %s",
 					libusb_strerror(r));
-			goto error;
+			return -1;
 		}
-		if ((desc.idVendor == VENDOR_ID_STLINK) &&
-			((desc.idProduct & PRODUCT_ID_STLINK_MASK) ==
-			 PRODUCT_ID_STLINK_GROUP)) {
-			if (desc.idProduct == PRODUCT_ID_STLINKV1)  { /* Reject V1 devices.*/
-				DEBUG("STLINKV1 not supported\n");
-				continue;
-			}
-			Stlink.vid = desc.idVendor;
-			Stlink.pid = desc.idProduct;
-			r = libusb_open(dev, &Stlink.handle);
-			if (r == LIBUSB_SUCCESS) {
-				uint8_t data[32];
-				uint16_t lang;
-				libusb_get_string_descriptor(
-					Stlink.handle, 0, 0, data, sizeof(data));
-				lang = data[2] << 8 | data[3];
-				unsigned char sernum[32];
-				if (desc.iSerialNumber) {
-					r = libusb_get_string_descriptor
-						(Stlink.handle, desc.iSerialNumber, lang,
-						  sernum, sizeof(sernum));
-				} else {
-					DEBUG("No serial number\n");
-				}
-				/* Older devices have hex values instead of ascii
-				 * in the serial string. Recode eventually!*/
-				bool readable = true;
-				uint16_t *p = (uint16_t *)sernum;
-				for (p += 1; *p; p++) {
-					bool isr = isalnum(*p);
-					readable &= isr;
-				}
-				char *s = Stlink.serial;
-				p = (uint16_t *)sernum;
-				for (p += 1; *p; p++, s++) {
-					if (readable)
-						*s = *p;
-					else
-						snprintf(s, 3, "%02x", *p & 0xff);
-				}
-				if (cl_opts.opt_serial && (!strncmp(Stlink.serial, cl_opts.opt_serial,
-													strlen(cl_opts.opt_serial))))
-					DEBUG("Found ");
-				if (desc.idProduct == PRODUCT_ID_STLINKV2) {
-					DEBUG("STLINKV20 serial %s\n", Stlink.serial);
-					Stlink.ver_hw = 20;
-					Stlink.ep_tx = 2;
-				} else if (desc.idProduct == PRODUCT_ID_STLINKV21) {
-					DEBUG("STLINKV21 serial %s\n", Stlink.serial);
-					Stlink.ver_hw = 21;
-					Stlink.ep_tx = 1;
-				} else if (desc.idProduct == PRODUCT_ID_STLINKV21_MSD) {
-					DEBUG("STLINKV21_MSD serial %s\n", Stlink.serial);
-					Stlink.ver_hw = 21;
-					Stlink.ep_tx = 1;
-				} else if (desc.idProduct == PRODUCT_ID_STLINKV3E) {
-					DEBUG("STLINKV3E serial %s\n", Stlink.serial);
-					Stlink.ver_hw = 30;
-					Stlink.ep_tx = 1;
-				} else if (desc.idProduct == PRODUCT_ID_STLINKV3) {
-					DEBUG("STLINKV3  serial %s\n", Stlink.serial);
-					Stlink.ver_hw = 30;
-					Stlink.ep_tx = 1;
-				} else {
-					DEBUG("Unknown STLINK variant, serial %s\n", Stlink.serial);
-				}
-				nr_stlinks++;
-				if (cl_opts.opt_serial) {
-					if (!strncmp(Stlink.serial, cl_opts.opt_serial,
-								 strlen(cl_opts.opt_serial))) {
-						break;
-					} else {
-						libusb_close(Stlink.handle);
-						Stlink.handle = 0;
-					}
-				}
-			} else {
-				DEBUG("Open failed %s\n", libusb_strerror(r));
-			}
+		if ((desc.idVendor != info->vid) ||
+			(desc.idProduct != info->pid) ||
+			(libusb_open(dev, &sl->ul_libusb_device_handle)
+			 != LIBUSB_SUCCESS)) {
+			continue;
 		}
+		char serial[64];
+		r = libusb_get_string_descriptor_ascii(
+			sl->ul_libusb_device_handle, desc.iSerialNumber,
+			(uint8_t*)serial,sizeof(serial));
+		if (r <= 0 || !strstr(serial, info->serial)) {
+			libusb_close(sl->ul_libusb_device_handle);
+			continue;
+		}
+		found = true;
+		break;
 	}
 	libusb_free_device_list(devs, 1);
-	if (!Stlink.handle) {
-		if (nr_stlinks && cl_opts.opt_serial) {
-			DEBUG("No Stlink with given serial number %s\n",  cl_opts.opt_serial);
-		} else if (nr_stlinks > 1) {
-			DEBUG("Multiple Stlinks. Please specify serial number\n");
-			goto error;
-		} else {
-			DEBUG("No Stlink device found!\n");
-		}
-		if (hotplug && !cl_opts.opt_no_wait) {
-			libusb_hotplug_callback_handle hp;
-			int rc = libusb_hotplug_register_callback
-				(NULL, LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED, 0,
-				 VENDOR_ID_STLINK, LIBUSB_HOTPLUG_MATCH_ANY,
-				 LIBUSB_HOTPLUG_MATCH_ANY,
-				 hotplug_callback_attach, NULL, &hp);
-			if (LIBUSB_SUCCESS != rc) {
-				DEBUG("Error registering attach callback\n");
-				goto error;
-			}
-			DEBUG("Waiting for %sST device%s%s to attach\n",
-				  (cl_opts.opt_serial)? "" : "some ",
-				  (cl_opts.opt_serial)? " with serial ": "",
-				  (cl_opts.opt_serial)? cl_opts.opt_serial: "");
-			DEBUG("Terminate with ^C\n");
-			while (has_attached == 0) {
-				rc = libusb_handle_events (NULL);
-                if (rc < 0)
-					printf("libusb_handle_events() failed: %s\n",
-						   libusb_error_name(rc));
-			}
-			goto rescan;
-		}
-		goto error;
+	if (!found)
+		return 0;
+	if (info->pid == PRODUCT_ID_STLINKV2) {
+		Stlink.ver_hw = 20;
+		info->usb_link->ep_tx = 2;
+		Stlink.ep_tx = 2;
+	} else if ((info->pid == PRODUCT_ID_STLINKV21)||
+			   (info->pid == PRODUCT_ID_STLINKV21_MSD)) {
+		Stlink.ver_hw = 21;
+		info->usb_link->ep_tx = 1;
+		Stlink.ep_tx = 1;
+	} else if ((info->pid == PRODUCT_ID_STLINKV3) ||
+			   (info->pid == PRODUCT_ID_STLINKV3E)) {
+		Stlink.ver_hw = 30;
+		info->usb_link->ep_tx = 1;
+		Stlink.ep_tx = 1;
 	}
+	info->usb_link->ep_rx = 1;
 	int config;
-	r = libusb_get_configuration(Stlink.handle, &config);
+	int r = libusb_get_configuration(sl->ul_libusb_device_handle, &config);
 	if (r) {
-		DEBUG("libusb_get_configuration failed %d: %s", r, libusb_strerror(r));
-		goto error_1;
+		fprintf(stderr, "FATAL: Stlink libusb_get_configuration failed %d: %s",
+				 r, libusb_strerror(r));
+		return -1;
 	}
-	DEBUG("Config %d\n", config);
 	if (config != 1) {
-		r = libusb_set_configuration(Stlink.handle, 0);
+		r = libusb_set_configuration(sl->ul_libusb_device_handle, 0);
 		if (r) {
-			DEBUG("libusb_set_configuration failed %d: %s",
-				  r, libusb_strerror(r));
-			goto error_1;
+			fprintf(stderr, "FATAL: Stlinklibusb_set_configuration "
+				   "failed %d: %s", r, libusb_strerror(r));
+			return -1;
 		}
 	}
-	r = libusb_claim_interface(Stlink.handle, 0);
-	if (r)
-	{
-		DEBUG("libusb_claim_interface failed %s\n", libusb_strerror(r));
-		goto error_1;
+	r = libusb_claim_interface(sl->ul_libusb_device_handle, 0);
+	if (r) 	{
+		fprintf(stderr, "FATAL: Stlink libusb_claim_interface failed %s\n",
+				libusb_strerror(r));
+		return -1;
 	}
-	if (hotplug) { /* Allow gracefully exit when stlink is unplugged*/
-		libusb_hotplug_callback_handle hp;
-		int rc = libusb_hotplug_register_callback
-			(NULL, LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT, 0, Stlink.vid, Stlink.pid,
-			 LIBUSB_HOTPLUG_MATCH_ANY, hotplug_callback_detach, NULL, &hp);
-		if (LIBUSB_SUCCESS != rc) {
-			DEBUG("Error registering detach callback\n");
-			goto error;
-		}
-	}
-	Stlink.req_trans = libusb_alloc_transfer(0);
-	Stlink.rep_trans = libusb_alloc_transfer(0);
-	stlink_version();
+	sl->req_trans = libusb_alloc_transfer(0);
+	sl->rep_trans = libusb_alloc_transfer(0);
+	stlink_version(info);
 	if ((Stlink.ver_stlink < 3 && Stlink.ver_jtag < 32) ||
 		(Stlink.ver_stlink == 3 && Stlink.ver_jtag < 3)) {
 		/* Maybe the adapter is in some strange state. Try to reset */
-        int result = libusb_reset_device(Stlink.handle);
-		DEBUG("Trying reset\n");
+        int result = libusb_reset_device(sl->ul_libusb_device_handle);
+		fprintf(stderr, "INFO: Trying Stlink reset\n");
 		if (result == LIBUSB_ERROR_BUSY) { /* Try again */
 			platform_delay(50);
-			result = libusb_reset_device(Stlink.handle);
+			result = libusb_reset_device(sl->ul_libusb_device_handle);
 		}
         if (result != LIBUSB_SUCCESS) {
-			DEBUG("libusb_reset_device failed\n");
-			goto error_1;
+			fprintf(stderr, "FATAL: Stlink libusb_reset_device failed\n");
+			return -1;
 		}
-		stlink_version();
+		stlink_version(info);
     }
 	if ((Stlink.ver_stlink < 3 && Stlink.ver_jtag < 32) ||
 		(Stlink.ver_stlink == 3 && Stlink.ver_jtag < 3)) {
 		DEBUG("Please update Firmware\n");
-		goto error_1;
+		return -1;
 	}
-	stlink_leave_state();
-	stlink_resetsys();
-	if (cl_opts.opt_mode != BMP_MODE_DEBUG) {
-		ret = cl_execute(&cl_opts);
-	} else {
-		assert(gdb_if_init() == 0);
-		return;
+	if (stlink_leave_state(info)) {
+		printf("Stlink board was in DFU mode. Restart\n");
+		return -1;
 	}
-  error_1:
-	libusb_close(Stlink.handle);
-  error:
-	libusb_exit(Stlink.libusb_ctx);
-	exit(ret);
+	stlink_resetsys(info);
+	return 0;
 }
 
-void stlink_srst_set_val(bool assert)
+void stlink_srst_set_val(bmp_info_t *info, bool assert)
 {
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
 					  STLINK_DEBUG_APIV2_DRIVE_NRST,
 					  (assert)? STLINK_DEBUG_APIV2_DRIVE_NRST_LOW
 					  : STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH};
 	uint8_t data[2];
-	send_recv(cmd, 16, data, 2);
+	Stlink.srst = assert;
+	send_recv(info->usb_link, cmd, 16, data, 2);
 	stlink_usb_error_check(data, true);
 }
 
-bool stlink_set_freq_divisor(uint16_t divisor)
+bool stlink_srst_get_val(void)
+{
+	return Stlink.srst;
+}
+
+static bool stlink_set_freq_divisor(bmp_info_t *info, uint16_t divisor)
 {
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
 					  STLINK_DEBUG_APIV2_SWD_SET_FREQ,
 					  divisor & 0xff, divisor >> 8};
 	uint8_t data[2];
-	send_recv(cmd, 16, data, 2);
+	send_recv(info->usb_link, cmd, 16, data, 2);
 	if (stlink_usb_error_check(data, false))
 		return false;
 	return true;
 }
 
-bool stlink3_set_freq_divisor(uint16_t divisor)
+static bool stlink3_set_freq_divisor(bmp_info_t *info, uint16_t divisor)
 {
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
 					  STLINK_APIV3_GET_COM_FREQ,
 					  Stlink.transport_mode};
 	uint8_t data[52];
-	send_recv(cmd, 16, data, 52);
+	send_recv(info->usb_link, cmd, 16, data, 52);
 	stlink_usb_error_check(data, true);
 	int size = data[8];
 	if (divisor > size)
@@ -965,7 +655,7 @@ bool stlink3_set_freq_divisor(uint16_t divisor)
 	cmd[5] = p[1];
 	cmd[6] = p[2];
 	cmd[7] = p[3];
-	send_recv(cmd, 16, data, 8);
+	send_recv(info->usb_link, cmd, 16, data, 8);
 	return true;
 }
 
@@ -974,57 +664,40 @@ int stlink_hwversion(void)
 	return Stlink.ver_stlink;
 }
 
-int stlink_enter_debug_swd(void)
+static int stlink_enter_debug_jtag(bmp_info_t *info)
 {
-	stlink_leave_state();
-	Stlink.transport_mode = STLINK_MODE_SWD;
-	if (Stlink.ver_stlink == 3)
-		stlink3_set_freq_divisor(2);
-	else
-		stlink_set_freq_divisor(1);
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
-					  STLINK_DEBUG_APIV2_ENTER,
-					  STLINK_DEBUG_ENTER_SWD_NO_RESET};
-	uint8_t data[2];
-	DEBUG("Enter SWD\n");
-	send_recv_retry(cmd, 16, data, 2);
-	return stlink_usb_error_check(data, true);
-}
-
-int stlink_enter_debug_jtag(void)
-{
-	stlink_leave_state();
+	stlink_leave_state(info);
 	Stlink.transport_mode = STLINK_MODE_JTAG;
 	if (Stlink.ver_stlink == 3)
-		stlink3_set_freq_divisor(4);
+		stlink3_set_freq_divisor(info, 4);
 	else
-		stlink_set_freq_divisor(1);
+		stlink_set_freq_divisor(info, 1);
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
 					  STLINK_DEBUG_APIV2_ENTER,
 					  STLINK_DEBUG_ENTER_JTAG_NO_RESET};
 	uint8_t data[2];
 	DEBUG("Enter JTAG\n");
-	send_recv(cmd, 16, data, 2);
+	send_recv(info->usb_link, cmd, 16, data, 2);
 	return stlink_usb_error_check(data, true);
 }
 
-uint32_t stlink_read_coreid(void)
+static uint32_t stlink_read_coreid(void)
 {
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
 					  STLINK_DEBUG_READCOREID};
 	uint8_t data[4];
-	send_recv(cmd, 16, data, 4);
+	send_recv(info.usb_link, cmd, 16, data, 4);
 	uint32_t id =  data[0] | data[1] << 8 | data[2] << 16 | data[3] << 24;
 	DEBUG("Read Core ID: 0x%08" PRIx32 "\n", id);
 	return id;
 }
 
-int stlink_read_idcodes(uint32_t *idcodes)
+static int stlink_read_idcodes(bmp_info_t *info, uint32_t *idcodes)
 {
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
 					   STLINK_DEBUG_APIV2_READ_IDCODES};
 	uint8_t data[12];
-	send_recv(cmd, 16, data, 12);
+	send_recv(info->usb_link, cmd, 16, data, 12);
 	if (stlink_usb_error_check(data, true))
 		return 0;
 	uint8_t *p = data + 4;
@@ -1034,7 +707,10 @@ int stlink_read_idcodes(uint32_t *idcodes)
 	return 2;
 }
 
-uint32_t stlink_dp_read(ADIv5_DP_t *dp, uint16_t addr)
+uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
+							  uint16_t addr, uint32_t value);
+
+	static uint32_t stlink_dp_read(ADIv5_DP_t *dp, uint16_t addr)
 {
 	if (addr & ADIV5_APnDP) {
 		DEBUG_STLINK("AP read addr 0x%04" PRIx16 "\n", addr);
@@ -1078,7 +754,7 @@ void stlink_dp_abort(ADIv5_DP_t *dp, uint32_t abort)
 	adiv5_dp_write(dp, ADIV5_DP_ABORT, abort);
 }
 
-int stlink_read_dp_register(uint16_t port, uint16_t addr, uint32_t *reg)
+static int stlink_read_dp_register(uint16_t port, uint16_t addr, uint32_t *reg)
 {
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
 					  STLINK_DEBUG_APIV2_READ_DAP_REG,
@@ -1101,7 +777,7 @@ int stlink_read_dp_register(uint16_t port, uint16_t addr, uint32_t *reg)
 	return res;
 }
 
-int stlink_write_dp_register(uint16_t port, uint16_t addr, uint32_t val)
+static int stlink_write_dp_register(uint16_t port, uint16_t addr, uint32_t val)
 {
 	if (port == STLINK_DEBUG_PORT_ACCESS && addr == 8) {
 		Stlink.dap_select = val;
@@ -1160,7 +836,7 @@ static bool stlink_ap_setup(int ap)
 		ap,
 	};
 	uint8_t data[2];
-	send_recv_retry(cmd, 16, data, 2);
+	send_recv(info.usb_link, cmd, 16, data, 2);
 	DEBUG_STLINK("Open AP %d\n", ap);
 	int res = stlink_usb_error_check(data, true);
 	if (res) {
@@ -1180,7 +856,7 @@ static void stlink_ap_cleanup(int ap)
                ap,
        };
        uint8_t data[2];
-       send_recv(cmd, 16, data, 2);
+       send_recv(info.usb_link, cmd, 16, data, 2);
        DEBUG_STLINK("Close AP %d\n", ap);
        stlink_usb_error_check(data, true);
 }
@@ -1191,7 +867,7 @@ static int stlink_usb_get_rw_status(bool verbose)
 		STLINK_DEBUG_APIV2_GETLASTRWSTATUS2
 	};
 	uint8_t data[12];
-	send_recv(cmd, 16, data, 12);
+	send_recv(info.usb_link, cmd, 16, data, 12);
 	return stlink_usb_error_check(data, verbose);
 }
 
@@ -1246,9 +922,10 @@ static void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 	DEBUG_STLINK("\n");
 }
 
-static void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
-					  uint8_t *buffer)
+static void stlink_writemem8(usb_link_t *link, ADIv5_AP_t *ap, uint32_t addr,
+							 size_t len, uint8_t *buffer)
 {
+	(void)link;
 	DEBUG_STLINK("Mem Write8 AP %d len %zu addr 0x%08" PRIx32 ": ",
 				 ap->apsel, len, addr);
 	for (size_t t = 0; t < len; t++) {
@@ -1257,6 +934,7 @@ static void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 	DEBUG_STLINK("\n");
 	while (len) {
 		size_t length;
+		/* OpenOCD has some note about writemem8*/
 		if (len > Stlink.block_size)
 			length = Stlink.block_size;
 		else
@@ -1267,17 +945,18 @@ static void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 			addr & 0xff, (addr >>  8) & 0xff, (addr >> 16) & 0xff,
 			(addr >> 24) & 0xff,
 			length & 0xff, length >> 8, ap->apsel};
-		send_recv(cmd, 16, NULL, 0);
-		send_recv((void*)buffer, length, NULL, 0);
+		send_recv(link, cmd, 16, NULL, 0);
+		send_recv(link, (void*)buffer, length, NULL, 0);
 		stlink_usb_get_rw_status(true);
 		len -= length;
 		addr += length;
 	}
 }
 
-static void stlink_writemem16(ADIv5_AP_t *ap, uint32_t addr, size_t len,
-					   uint16_t *buffer)
+static void stlink_writemem16(usb_link_t *link, ADIv5_AP_t *ap, uint32_t addr,
+							  size_t len, uint16_t *buffer)
 {
+	(void)link;
 	DEBUG_STLINK("Mem Write16 AP %d len %zu addr 0x%08" PRIx32 ": ",
 				 ap->apsel, len, addr);
 	for (size_t t = 0; t < len; t+=2) {
@@ -1290,14 +969,15 @@ static void stlink_writemem16(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 		addr & 0xff, (addr >>  8) & 0xff, (addr >> 16) & 0xff,
 		(addr >> 24) & 0xff,
 		len & 0xff, len >> 8, ap->apsel};
-	send_recv(cmd, 16, NULL, 0);
-	send_recv((void*)buffer, len, NULL, 0);
+	send_recv(link, cmd, 16, NULL, 0);
+	send_recv(link, (void*)buffer, len, NULL, 0);
 	stlink_usb_get_rw_status(true);
 }
 
-static void stlink_writemem32(ADIv5_AP_t *ap, uint32_t addr, size_t len,
-					   uint32_t *buffer)
+static void stlink_writemem32(usb_link_t * link, ADIv5_AP_t *ap, uint32_t addr,
+							  size_t len, uint32_t *buffer)
 {
+	(void)link;
 	DEBUG_STLINK("Mem Write32 AP %d len %zu addr 0x%08" PRIx32 ": ",
 				 ap->apsel, len, addr);
 	for (size_t t = 0; t < len; t+=4) {
@@ -1313,23 +993,23 @@ static void stlink_writemem32(ADIv5_AP_t *ap, uint32_t addr, size_t len,
 	write_retry(cmd, 16, (void*)buffer, len);
 }
 
-void stlink_regs_read(ADIv5_AP_t *ap, void *data)
+static void stlink_regs_read(ADIv5_AP_t *ap, void *data)
 {
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READALLREGS,
 					   ap->apsel};
 	uint8_t res[88];
 	DEBUG_STLINK("AP %d: Read all core registers\n", ap->apsel);
-	send_recv(cmd, 16, res, 88);
+	send_recv(info.usb_link, cmd, 16, res, 88);
 	stlink_usb_error_check(res, true);
 	memcpy(data, res + 4, 84);
 }
 
-uint32_t stlink_reg_read(ADIv5_AP_t *ap, int num)
+static uint32_t stlink_reg_read(ADIv5_AP_t *ap, int num)
 {
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READREG, num,
 					   ap->apsel};
 	uint8_t res[8];
-	send_recv(cmd, 16, res, 8);
+	send_recv(info.usb_link, cmd, 16, res, 8);
 	stlink_usb_error_check(res, true);
 	uint32_t ret = res[0] | res[1] << 8 | res[2] << 16 | res[3] << 24;
 	DEBUG_STLINK("AP %d: Read reg %02" PRId32 " val 0x%08" PRIx32 "\n",
@@ -1337,14 +1017,14 @@ uint32_t stlink_reg_read(ADIv5_AP_t *ap, int num)
 	return ret;
 }
 
-void stlink_reg_write(ADIv5_AP_t *ap, int num, uint32_t val)
+static void stlink_reg_write(ADIv5_AP_t *ap, int num, uint32_t val)
 {
 	uint8_t cmd[16] = {
 		STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_WRITEREG, num,
 		val & 0xff, (val >>  8) & 0xff, (val >> 16) & 0xff,
 		(val >> 24) & 0xff, ap->apsel};
 	uint8_t res[2];
-	send_recv(cmd, 16, res, 2);
+	send_recv(info.usb_link, cmd, 16, res, 2);
 	DEBUG_STLINK("AP %d: Write reg %02" PRId32 " val 0x%08" PRIx32 "\n",
 				 ap->apsel, num, val);
 	stlink_usb_error_check(res, true);
@@ -1356,14 +1036,17 @@ static void stlink_mem_write_sized(	ADIv5_AP_t *ap, uint32_t dest,
 {
 	if (len == 0)
 		return;
+	usb_link_t *link = info.usb_link;
 	switch(align) {
-	case ALIGN_BYTE: stlink_writemem8(ap, dest, len, (uint8_t *) src);
+	case ALIGN_BYTE:
+		stlink_writemem8(link, ap, dest, len, (uint8_t *) src);
 		break;
-	case ALIGN_HALFWORD: stlink_writemem16(ap, dest, len, (uint16_t *) src);
+	case ALIGN_HALFWORD:
+		stlink_writemem16(link,ap, dest, len, (uint16_t *) src);
 		break;
-	case ALIGN_WORD: stlink_writemem32(ap, dest, len, (uint32_t *) src);
-		break;
-	case ALIGN_DWORD: stlink_writemem32(ap, dest, len, (uint32_t *) src);
+	case ALIGN_WORD:
+	case ALIGN_DWORD:
+		stlink_writemem32(link, ap, dest, len, (uint32_t *) src);
 		break;
 	}
 }
@@ -1384,7 +1067,7 @@ struct jtag_dev_s jtag_devs[JTAG_MAX_DEVS+1];
 int jtag_dev_count;
 jtag_proc_t jtag_proc;
 
-int jtag_scan_stlinkv2(const uint8_t *irlens)
+int jtag_scan_stlinkv2(bmp_info_t *info, const uint8_t *irlens)
 {
 	uint32_t idcodes[JTAG_MAX_DEVS+1];
 	(void) *irlens;
@@ -1392,9 +1075,9 @@ int jtag_scan_stlinkv2(const uint8_t *irlens)
 
 	jtag_dev_count = 0;
 	memset(&jtag_devs, 0, sizeof(jtag_devs));
-	if (stlink_enter_debug_jtag())
+	if (stlink_enter_debug_jtag(info))
 		return 0;
-	jtag_dev_count = stlink_read_idcodes(idcodes);
+	jtag_dev_count = stlink_read_idcodes(info, idcodes);
 	/* Check for known devices and handle accordingly */
 	for(int i = 0; i < jtag_dev_count; i++)
 		jtag_devs[i].idcode = idcodes[i];
@@ -1410,7 +1093,7 @@ int jtag_scan_stlinkv2(const uint8_t *irlens)
 	return jtag_dev_count;
 }
 
-int platform_jtag_dp_init(ADIv5_DP_t *dp)
+int stlink_jtag_dp_init(ADIv5_DP_t *dp)
 {
 	dp->dp_read = stlink_dp_read;
 	dp->error = stlink_dp_error;
@@ -1421,7 +1104,7 @@ int platform_jtag_dp_init(ADIv5_DP_t *dp)
 
 }
 
-int platform_adiv5_dp_defaults(ADIv5_DP_t *dp)
+void stlink_adiv5_dp_defaults(ADIv5_DP_t *dp)
 {
 	dp->ap_regs_read = stlink_regs_read;
 	dp->ap_reg_read = stlink_reg_read;
@@ -1432,6 +1115,30 @@ int platform_adiv5_dp_defaults(ADIv5_DP_t *dp)
 	dp->ap_read = stlink_ap_read;
 	dp->mem_read = stlink_readmem;
 	dp->mem_write_sized = stlink_mem_write_sized;
+}
 
+int stlink_enter_debug_swd(bmp_info_t *info, ADIv5_DP_t *dp)
+{
+	stlink_leave_state(info);
+	Stlink.transport_mode = STLINK_MODE_SWD;
+	if (Stlink.ver_stlink == 3)
+		stlink3_set_freq_divisor(info, 2);
+	else
+		stlink_set_freq_divisor(info, 1);
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
+					  STLINK_DEBUG_APIV2_ENTER,
+					  STLINK_DEBUG_ENTER_SWD_NO_RESET};
+	uint8_t data[2];
+	DEBUG("Enter SWD\n");
+	send_recv(info->usb_link, cmd, 16, data, 2);
+	if (stlink_usb_error_check(data, true))
+		return -1;
+	dp->idcode = stlink_read_coreid();
+	dp->dp_read = stlink_dp_read;
+	dp->error = stlink_dp_error;
+	dp->low_access = stlink_dp_low_access;
+	dp->abort = stlink_dp_abort;
+
+	stlink_dp_error(dp);
 	return 0;
 }
diff --git a/src/platforms/hosted/stlinkv2.h b/src/platforms/hosted/stlinkv2.h
index dea9a8b..1960a5b 100644
--- a/src/platforms/hosted/stlinkv2.h
+++ b/src/platforms/hosted/stlinkv2.h
@@ -24,28 +24,18 @@
 
 #define STLINK_DEBUG_PORT_ACCESS            0xffff
 
-void stlink_init(int argc, char **argv);
+int stlink_init(bmp_info_t *info);
 int stlink_hwversion(void);
-void stlink_leave_state(void);
-const char *stlink_target_voltage(void);
-void stlink_srst_set_val(bool assert);
-int stlink_enter_debug_swd(void);
-int stlink_enter_debug_jtag(void);
-int stlink_read_idcodes(uint32_t *);
-uint32_t stlink_read_coreid(void);
-int stlink_read_dp_register(uint16_t port, uint16_t addr, uint32_t *res);
-int stlink_write_dp_register(uint16_t port, uint16_t addr, uint32_t val);
+const char *stlink_target_voltage(bmp_info_t *info);
+void stlink_srst_set_val(bmp_info_t *info, bool assert);
+bool stlink_srst_get_val(void);
+int stlink_enter_debug_swd(bmp_info_t *info, ADIv5_DP_t *dp);
 
-uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
-				      uint16_t addr, uint32_t value);
-uint32_t stlink_dp_read(ADIv5_DP_t *dp, uint16_t addr);
-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);
-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);
+const char *stlink_target_voltage(bmp_info_t *info);
+void stlink_adiv5_dp_defaults(ADIv5_DP_t *dp);
+int stlink_jtag_dp_init(ADIv5_DP_t *dp);
+int jtag_scan_stlinkv2(bmp_info_t *info, const uint8_t *irlens);
+void stlink_exit_function(bmp_info_t *info);
 extern  int debug_level;
 # define DEBUG_STLINK if (debug_level > 0) printf
 # define DEBUG_USB    if (debug_level > 1) printf
diff --git a/src/platforms/pc-stlinkv2/Makefile.inc b/src/platforms/pc-stlinkv2/Makefile.inc
deleted file mode 100644
index 70f6f8e..0000000
--- a/src/platforms/pc-stlinkv2/Makefile.inc
+++ /dev/null
@@ -1,14 +0,0 @@
-TARGET=blackmagic_stlinkv2
-SYS = $(shell $(CC) -dumpmachine)
-CFLAGS += -DSTLINKV2 -DJTAG_HL -DENABLE_DEBUG
-CFLAGS +=-I ./target -I./platforms/pc
-LDFLAGS += -lusb-1.0
-ifneq (, $(findstring mingw, $(SYS)))
-LDFLAGS += -lws2_32
-else ifneq (, $(findstring cygwin, $(SYS)))
-LDFLAGS += -lws2_32
-endif
-VPATH += platforms/pc
-SRC += 	timing.c stlinkv2.c cl_utils.c utils.c
-OWN_HL = 1
-PC_HOSTED = 1
diff --git a/src/platforms/pc-stlinkv2/README.md b/src/platforms/pc-stlinkv2/README.md
deleted file mode 100644
index d70b4d7..0000000
--- a/src/platforms/pc-stlinkv2/README.md
+++ /dev/null
@@ -1,25 +0,0 @@
-ST-Link V2/3 with original STM firmware as Blackmagic Debug Probes
-
-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.
-
-Compile with "make PROBE_HOST=pc-stlinkv2"
-
-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.
-- ST-LINKV3 seem to only work on STM32 devices.
-- St-LINKV3 needs connect under reset on more devices than V2
-
-ToDo:
-- Implement an SWO server
\ No newline at end of file
diff --git a/src/platforms/pc-stlinkv2/jtagtap.c b/src/platforms/pc-stlinkv2/jtagtap.c
deleted file mode 100644
index e69de29..0000000
diff --git a/src/platforms/pc-stlinkv2/platform.c b/src/platforms/pc-stlinkv2/platform.c
deleted file mode 100644
index 07379b0..0000000
--- a/src/platforms/pc-stlinkv2/platform.c
+++ /dev/null
@@ -1,115 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2019 2019 Uwe Bonnes
- *
- * 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 "general.h"
-#include "gdb_if.h"
-#include "version.h"
-#include "platform.h"
-#include "target.h"
-#include "target_internal.h"
-#include "swdptap.h"
-
-#include <assert.h>
-#include <unistd.h>
-#include <signal.h>
-#include <sys/time.h>
-
-#include "adiv5.h"
-#include "stlinkv2.h"
-
-int platform_hwversion(void)
-{
-	return stlink_hwversion();
-}
-
-const char *platform_target_voltage(void)
-{
-	return stlink_target_voltage();
-}
-
-int platform_swdptap_init(void)
-{
-	return 0;
-}
-
-swd_proc_t swd_proc;
-jtag_proc_t jtag_proc;
-
-static int adiv5_swdp_scan_stlinkv2(void)
-{
-	target_list_free();
-	ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
-	if (stlink_enter_debug_swd())
-		return 0;
-	dp->idcode = stlink_read_coreid();
-	dp->dp_read = stlink_dp_read;
-	dp->error = stlink_dp_error;
-	dp->low_access = stlink_dp_low_access;
-	dp->abort = stlink_dp_abort;
-
-	stlink_dp_error(dp);
-	adiv5_dp_init(dp);
-
-	return target_list?1:0;
-	return 0;
-}
-int platform_adiv5_swdp_scan(void)
-{
-	return adiv5_swdp_scan_stlinkv2();
-}
-
-int platform_jtag_scan(const uint8_t *lrlens)
-{
-	return jtag_scan_stlinkv2(lrlens);
-}
-
-void platform_init(int argc, char **argv)
-{
-	stlink_init(argc, argv);
-}
-
-static bool srst_status = false;
-void platform_srst_set_val(bool assert)
-{
-	stlink_srst_set_val(assert);
-	srst_status = assert;
-}
-
-bool platform_srst_get_val(void) { return srst_status; }
-
-void platform_buffer_flush(void)
-{
-}
-
-int platform_buffer_write(const uint8_t *data, int size)
-{
-	(void) data;
-	(void) size;
-	return size;
-}
-
-int platform_buffer_read(uint8_t *data, int size)
-{
-	(void) data;
-	return size;
-}
-
-int platform_jtagtap_init(void)
-{
-	return 0;
-}
diff --git a/src/platforms/pc-stlinkv2/platform.h b/src/platforms/pc-stlinkv2/platform.h
deleted file mode 100644
index b63b0e1..0000000
--- a/src/platforms/pc-stlinkv2/platform.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2011  Black Sphere Technologies Ltd.
- * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- *
- * 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/>.
- */
-
-#ifndef __PLATFORM_H
-#define __PLATFORM_H
-
-#include <libusb-1.0/libusb.h>
-
-#include "timing.h"
-
-#ifndef _WIN32
-#	include <alloca.h>
-#else
-#	ifndef alloca
-#		define alloca __builtin_alloca
-#	endif
-#endif
-
-#define PLATFORM_IDENT() "StlinkV2/3"
-#define SET_RUN_STATE(state)
-void stlink_check_detach(int state);
-#define SET_IDLE_STATE(state) stlink_check_detach(state)
-//#define SET_ERROR_STATE(state)
-
-void platform_buffer_flush(void);
-int platform_buffer_write(const uint8_t *data, int size);
-int platform_buffer_read(uint8_t *data, int size);
-int jtag_scan_stlinkv2(const uint8_t *irlens);
-#endif
diff --git a/src/platforms/pc-stlinkv2/stlinkv2.c b/src/platforms/pc-stlinkv2/stlinkv2.c
deleted file mode 100644
index 0d5012f..0000000
--- a/src/platforms/pc-stlinkv2/stlinkv2.c
+++ /dev/null
@@ -1,1435 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2019 Uwe Bonnes
- *
- * 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/>.
- */
-/* Much code and ideas shamelessly taken form
- * https://github.com/texane/stlink.git
- * git://git.code.sf.net/p/openocd/code
- * https://github.com/pavelrevak/pystlink
- * https://github.com/pavelrevak/pystlink
- *
- * with some contribution.
- */
-#include "general.h"
-#include "gdb_if.h"
-#include "adiv5.h"
-#include "stlinkv2.h"
-#include "exception.h"
-#include "jtag_devs.h"
-#include "target.h"
-
-#include <assert.h>
-#include <unistd.h>
-#include <signal.h>
-#include <ctype.h>
-#include <sys/time.h>
-
-#include "cl_utils.h"
-
-#if !defined(timersub)
-/* This is a copy from GNU C Library (GNU LGPL 2.1), sys/time.h. */
-# define timersub(a, b, result)					\
-	do {										\
-		(result)->tv_sec = (a)->tv_sec - (b)->tv_sec;	\
-		(result)->tv_usec = (a)->tv_usec - (b)->tv_usec;	\
-		if ((result)->tv_usec < 0) {						\
-			--(result)->tv_sec;								\
-			(result)->tv_usec += 1000000;					\
-		}													\
-	} while (0)
-#endif
-
-#define VENDOR_ID_STLINK		0x483
-#define PRODUCT_ID_STLINK_MASK	0xffe0
-#define PRODUCT_ID_STLINK_GROUP 0x3740
-#define PRODUCT_ID_STLINKV1		0x3744
-#define PRODUCT_ID_STLINKV2		0x3748
-#define PRODUCT_ID_STLINKV21	0x374b
-#define PRODUCT_ID_STLINKV21_MSD 0x3752
-#define PRODUCT_ID_STLINKV3		0x374f
-#define PRODUCT_ID_STLINKV3E	0x374e
-
-#define STLINK_SWIM_ERR_OK             0x00
-#define STLINK_SWIM_BUSY               0x01
-#define STLINK_DEBUG_ERR_OK            0x80
-#define STLINK_DEBUG_ERR_FAULT         0x81
-#define STLINK_JTAG_UNKNOWN_JTAG_CHAIN 0x04
-#define STLINK_NO_DEVICE_CONNECTED     0x05
-#define STLINK_JTAG_COMMAND_ERROR      0x08
-#define STLINK_JTAG_COMMAND_ERROR      0x08
-#define STLINK_JTAG_GET_IDCODE_ERROR   0x09
-#define STLINK_JTAG_DBG_POWER_ERROR    0x0b
-#define STLINK_SWD_AP_WAIT             0x10
-#define STLINK_SWD_AP_FAULT            0x11
-#define STLINK_SWD_AP_ERROR            0x12
-#define STLINK_SWD_AP_PARITY_ERROR     0x13
-#define STLINK_JTAG_WRITE_ERROR        0x0c
-#define STLINK_JTAG_WRITE_VERIF_ERROR  0x0d
-#define STLINK_SWD_DP_WAIT             0x14
-#define STLINK_SWD_DP_FAULT            0x15
-#define STLINK_SWD_DP_ERROR            0x16
-#define STLINK_SWD_DP_PARITY_ERROR     0x17
-
-#define STLINK_SWD_AP_WDATA_ERROR      0x18
-#define STLINK_SWD_AP_STICKY_ERROR     0x19
-#define STLINK_SWD_AP_STICKYORUN_ERROR 0x1a
-#define STLINK_BAD_AP_ERROR            0x1d
-#define STLINK_TOO_MANY_AP_ERROR       0x29
-#define STLINK_JTAG_UNKNOWN_CMD        0x42
-
-#define STLINK_CORE_RUNNING            0x80
-#define STLINK_CORE_HALTED             0x81
-#define STLINK_CORE_STAT_UNKNOWN       -1
-
-#define STLINK_GET_VERSION             0xF1
-#define STLINK_DEBUG_COMMAND           0xF2
-#define STLINK_DFU_COMMAND             0xF3
-#define STLINK_SWIM_COMMAND            0xF4
-#define STLINK_GET_CURRENT_MODE        0xF5
-#define STLINK_GET_TARGET_VOLTAGE      0xF7
-
-#define STLINK_DEV_DFU_MODE            0x00
-#define STLINK_DEV_MASS_MODE           0x01
-#define STLINK_DEV_DEBUG_MODE          0x02
-#define STLINK_DEV_SWIM_MODE           0x03
-#define STLINK_DEV_BOOTLOADER_MODE     0x04
-#define STLINK_DEV_UNKNOWN_MODE        -1
-
-#define STLINK_DFU_EXIT                0x07
-
-#define STLINK_SWIM_ENTER                  0x00
-#define STLINK_SWIM_EXIT                   0x01
-#define STLINK_SWIM_READ_CAP               0x02
-#define STLINK_SWIM_SPEED                  0x03
-#define STLINK_SWIM_ENTER_SEQ              0x04
-#define STLINK_SWIM_GEN_RST                0x05
-#define STLINK_SWIM_RESET                  0x06
-#define STLINK_SWIM_ASSERT_RESET           0x07
-#define STLINK_SWIM_DEASSERT_RESET         0x08
-#define STLINK_SWIM_READSTATUS             0x09
-#define STLINK_SWIM_WRITEMEM               0x0a
-#define STLINK_SWIM_READMEM                0x0b
-#define STLINK_SWIM_READBUF                0x0c
-
-#define STLINK_DEBUG_GETSTATUS             0x01
-#define STLINK_DEBUG_FORCEDEBUG            0x02
-#define STLINK_DEBUG_APIV1_RESETSYS        0x03
-#define STLINK_DEBUG_APIV1_READALLREGS     0x04
-#define STLINK_DEBUG_APIV1_READREG         0x05
-#define STLINK_DEBUG_APIV1_WRITEREG        0x06
-#define STLINK_DEBUG_READMEM_32BIT         0x07
-#define STLINK_DEBUG_WRITEMEM_32BIT        0x08
-#define STLINK_DEBUG_RUNCORE               0x09
-#define STLINK_DEBUG_STEPCORE              0x0a
-#define STLINK_DEBUG_APIV1_SETFP           0x0b
-#define STLINK_DEBUG_READMEM_8BIT          0x0c
-#define STLINK_DEBUG_WRITEMEM_8BIT         0x0d
-#define STLINK_DEBUG_APIV1_CLEARFP         0x0e
-#define STLINK_DEBUG_APIV1_WRITEDEBUGREG   0x0f
-#define STLINK_DEBUG_APIV1_SETWATCHPOINT   0x10
-
-#define STLINK_DEBUG_ENTER_JTAG_RESET      0x00
-#define STLINK_DEBUG_ENTER_SWD_NO_RESET    0xa3
-#define STLINK_DEBUG_ENTER_JTAG_NO_RESET   0xa4
-
-#define STLINK_DEBUG_APIV1_ENTER           0x20
-#define STLINK_DEBUG_EXIT                  0x21
-#define STLINK_DEBUG_READCOREID            0x22
-
-#define STLINK_DEBUG_APIV2_ENTER           0x30
-#define STLINK_DEBUG_APIV2_READ_IDCODES    0x31
-#define STLINK_DEBUG_APIV2_RESETSYS        0x32
-#define STLINK_DEBUG_APIV2_READREG         0x33
-#define STLINK_DEBUG_APIV2_WRITEREG        0x34
-#define STLINK_DEBUG_APIV2_WRITEDEBUGREG   0x35
-#define STLINK_DEBUG_APIV2_READDEBUGREG    0x36
-
-#define STLINK_DEBUG_APIV2_READALLREGS     0x3A
-#define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
-#define STLINK_DEBUG_APIV2_DRIVE_NRST      0x3C
-
-#define STLINK_DEBUG_APIV2_GETLASTRWSTATUS2 0x3E
-
-#define STLINK_DEBUG_APIV2_START_TRACE_RX  0x40
-#define STLINK_DEBUG_APIV2_STOP_TRACE_RX   0x41
-#define STLINK_DEBUG_APIV2_GET_TRACE_NB    0x42
-#define STLINK_DEBUG_APIV2_SWD_SET_FREQ    0x43
-#define STLINK_DEBUG_APIV2_JTAG_SET_FREQ   0x44
-#define STLINK_DEBUG_APIV2_READ_DAP_REG    0x45
-#define STLINK_DEBUG_APIV2_WRITE_DAP_REG   0x46
-#define STLINK_DEBUG_APIV2_READMEM_16BIT   0x47
-#define STLINK_DEBUG_APIV2_WRITEMEM_16BIT  0x48
-
-#define STLINK_DEBUG_APIV2_INIT_AP         0x4B
-#define STLINK_DEBUG_APIV2_CLOSE_AP_DBG    0x4C
-
-#define STLINK_APIV3_SET_COM_FREQ           0x61
-#define STLINK_APIV3_GET_COM_FREQ           0x62
-
-#define STLINK_APIV3_GET_VERSION_EX         0xFB
-
-#define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW   0x00
-#define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
-#define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
-
-
-#define STLINK_TRACE_SIZE               4096
-#define STLINK_TRACE_MAX_HZ             2000000
-
-#define STLINK_V3_MAX_FREQ_NB               10
-
-/** */
-enum stlink_mode {
-	STLINK_MODE_UNKNOWN = 0,
-	STLINK_MODE_DFU,
-	STLINK_MODE_MASS,
-	STLINK_MODE_DEBUG_JTAG,
-	STLINK_MODE_DEBUG_SWD,
-	STLINK_MODE_DEBUG_SWIM
-};
-
-enum transport_mode_t{
-	STLINK_MODE_SWD = 0,
-	STLINK_MODE_JTAG
-};
-
-typedef struct {
-	libusb_context* libusb_ctx;
-	uint16_t     vid;
-	uint16_t     pid;
-	uint8_t      transport_mode;
-	char         serial[32];
-	uint8_t      dap_select;
-	uint8_t      ep_tx;
-	uint8_t      ver_hw;     /* 20, 21 or 31 deciphered from USB PID.*/
-	uint8_t      ver_stlink; /* 2 or 3  from API.*/
-	uint8_t      ver_api;
-	uint8_t      ver_jtag;
-	uint8_t      ver_mass;
-	uint8_t      ver_swim;
-	uint8_t      ver_bridge;
-	uint16_t     block_size;
-	bool         ap_error;
-	libusb_device_handle *handle;
-	struct libusb_transfer* req_trans;
-	struct libusb_transfer* rep_trans;
-} stlink;
-
-stlink Stlink;
-
-static int stlink_usb_get_rw_status(bool verbose);
-
-static void exit_function(void)
-{
-	libusb_exit(NULL);
-	DEBUG("Cleanup\n");
-}
-
-/* SIGTERM handler. */
-static void sigterm_handler(int sig)
-{
-	(void)sig;
-	exit(0);
-}
-
-struct trans_ctx {
-#define TRANS_FLAGS_IS_DONE (1 << 0)
-#define TRANS_FLAGS_HAS_ERROR (1 << 1)
-    volatile unsigned long flags;
-};
-
-int debug_level = 0;
-bool has_attached = false;
-
-static int LIBUSB_CALL hotplug_callback_attach(
-	libusb_context *ctx, libusb_device *dev, libusb_hotplug_event event,
-	void *user_data)
-{
-	(void)ctx;
-	(void)dev;
-	(void)event;
-	(void)user_data;
-	has_attached = true;
-	return 1; /* deregister Callback*/
-}
-
-int device_detached = 0;
-static int LIBUSB_CALL hotplug_callback_detach(
-	libusb_context *ctx, libusb_device *dev, libusb_hotplug_event event,
-	void *user_data)
-{
-	(void)ctx;
-	(void)dev;
-	(void)event;
-	(void)user_data;
-	device_detached = 1;
-	return 1;  /* deregister Callback*/
-}
-
-void stlink_check_detach(int state)
-{
-	if (state == 1) {
-		/* Check for hotplug events */
-		struct timeval tv = {0,0};
-		libusb_handle_events_timeout_completed(
-			Stlink.libusb_ctx, &tv, &device_detached);
-		if (device_detached) {
-			DEBUG("Dongle was detached\n");
-			exit(0);
-		}
-	}
-}
-
-static void LIBUSB_CALL on_trans_done(struct libusb_transfer * trans)
-{
-    struct trans_ctx * const ctx = trans->user_data;
-
-    if (trans->status != LIBUSB_TRANSFER_COMPLETED)
-    {
-		DEBUG("on_trans_done: ");
-        if(trans->status == LIBUSB_TRANSFER_TIMED_OUT)
-        {
-            DEBUG("Timeout\n");
-        } else if (trans->status == LIBUSB_TRANSFER_CANCELLED) {
-            DEBUG("cancelled\n");
-        } else if (trans->status == LIBUSB_TRANSFER_NO_DEVICE) {
-            DEBUG("no device\n");
-        } else {
-            DEBUG("unknown\n");
-		}
-        ctx->flags |= TRANS_FLAGS_HAS_ERROR;
-    }
-    ctx->flags |= TRANS_FLAGS_IS_DONE;
-}
-
-static int submit_wait(struct libusb_transfer * trans) {
-	struct timeval start;
-	struct timeval now;
-	struct timeval diff;
-	struct trans_ctx trans_ctx;
-	enum libusb_error error;
-
-	trans_ctx.flags = 0;
-
-	/* brief intrusion inside the libusb interface */
-	trans->callback = on_trans_done;
-	trans->user_data = &trans_ctx;
-
-	if ((error = libusb_submit_transfer(trans))) {
-		DEBUG("libusb_submit_transfer(%d): %s\n", error,
-			  libusb_strerror(error));
-		exit(-1);
-	}
-
-	gettimeofday(&start, NULL);
-
-	while (trans_ctx.flags == 0) {
-		struct timeval timeout;
-		timeout.tv_sec = 1;
-		timeout.tv_usec = 0;
-		if (libusb_handle_events_timeout(Stlink.libusb_ctx, &timeout)) {
-			DEBUG("libusb_handle_events()\n");
-			return -1;
-		}
-
-		gettimeofday(&now, NULL);
-		timersub(&now, &start, &diff);
-		if (diff.tv_sec >= 1) {
-			libusb_cancel_transfer(trans);
-			DEBUG("libusb_handle_events() timeout\n");
-			return -1;
-		}
-	}
-
-	if (trans_ctx.flags & TRANS_FLAGS_HAS_ERROR) {
-		DEBUG("libusb_handle_events() | has_error\n");
-		return -1;
-	}
-
-	return 0;
-}
-#define STLINK_ERROR_DP_FAULT -2
-static int send_recv(uint8_t *txbuf, size_t txsize,
-					 uint8_t *rxbuf, size_t rxsize)
-{
-	int res = 0;
-	stlink_check_detach(1);
-	if( txsize) {
-		int txlen = txsize;
-		libusb_fill_bulk_transfer(Stlink.req_trans, Stlink.handle,
-								  Stlink.ep_tx | LIBUSB_ENDPOINT_OUT,
-								  txbuf, txlen,
-								  NULL, NULL,
-								  0
-			);
-		DEBUG_USB("  Send (%d): ", txlen);
-		for (int i = 0; i < txlen && i < 32 ; i++) {
-			DEBUG_USB("%02x", txbuf[i]);
-			if ((i & 7) == 7)
-				DEBUG_USB(".");
-		}
-		if (submit_wait(Stlink.req_trans)) {
-			DEBUG_USB("clear 2\n");
-			libusb_clear_halt(Stlink.handle,2);
-			return -1;
-		}
-	}
-	/* send_only */
-	if (rxsize != 0) {
-		/* read the response */
-		libusb_fill_bulk_transfer(Stlink.rep_trans, Stlink.handle,
-								  0x01| LIBUSB_ENDPOINT_IN,
-								  rxbuf, rxsize, NULL, NULL, 0);
-
-		if (submit_wait(Stlink.rep_trans)) {
-			DEBUG("clear 1\n");
-			libusb_clear_halt(Stlink.handle,1);
-			return -1;
-		}
-		res = Stlink.rep_trans->actual_length;
-		if (res >0) {
-			int i;
-			uint8_t *p = rxbuf;
-			DEBUG_USB(" Rec (%zu/%d)", rxsize, res);
-			for (i = 0; i < res && i < 32 ; i++) {
-				if ( i && ((i & 7) == 0))
-					DEBUG_USB(".");
-				DEBUG_USB("%02x", p[i]);
-			}
-		}
-	}
-	DEBUG_USB("\n");
-	return res;
-}
-
-/**
-    Converts an STLINK status code held in the first byte of a response to
-	readable error
-*/
-static int stlink_usb_error_check(uint8_t *data, bool verbose)
-{
-	switch (data[0]) {
-		case STLINK_DEBUG_ERR_OK:
-			return STLINK_ERROR_OK;
-		case STLINK_DEBUG_ERR_FAULT:
-			if (verbose)
-				DEBUG("SWD fault response (0x%x)\n", STLINK_DEBUG_ERR_FAULT);
-			return STLINK_ERROR_FAIL;
-		case STLINK_JTAG_UNKNOWN_JTAG_CHAIN:
-			if (verbose)
-				DEBUG("Unknown JTAG chain\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_NO_DEVICE_CONNECTED:
-			if (verbose)
-				DEBUG("No device connected\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_JTAG_COMMAND_ERROR:
-			if (verbose)
-				DEBUG("Command error\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_JTAG_GET_IDCODE_ERROR:
-			if (verbose)
-				DEBUG("Failure reading IDCODE\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_JTAG_DBG_POWER_ERROR:
-			if (verbose)
-				DEBUG("Failure powering DBG\n");
-			return STLINK_ERROR_WAIT;
-		case STLINK_SWD_AP_WAIT:
-			if (verbose)
-				DEBUG("wait status SWD_AP_WAIT (0x%x)\n", STLINK_SWD_AP_WAIT);
-			return STLINK_ERROR_WAIT;
-		case STLINK_SWD_DP_WAIT:
-			if (verbose)
-				DEBUG("wait status SWD_DP_WAIT (0x%x)\n", STLINK_SWD_DP_WAIT);
-			return STLINK_ERROR_WAIT;
-		case STLINK_JTAG_WRITE_ERROR:
-			if (verbose)
-				DEBUG("Write error\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_JTAG_WRITE_VERIF_ERROR:
-			if (verbose)
-				DEBUG("Write verify error, ignoring\n");
-			return STLINK_ERROR_OK;
-		case STLINK_SWD_AP_FAULT:
-			/* git://git.ac6.fr/openocd commit 657e3e885b9ee10
-			 * returns STLINK_ERROR_OK with the comment:
-			 * Change in error status when reading outside RAM.
-			 * This fix allows CDT plugin to visualize memory.
-			 */
-			Stlink.ap_error = true;
-			if (verbose)
-				DEBUG("STLINK_SWD_AP_FAULT\n");
-			return STLINK_ERROR_DP_FAULT;
-		case STLINK_SWD_AP_ERROR:
-			if (verbose)
-				DEBUG("STLINK_SWD_AP_ERROR\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_SWD_AP_PARITY_ERROR:
-			if (verbose)
-				DEBUG("STLINK_SWD_AP_PARITY_ERROR\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_SWD_DP_FAULT:
-			if (verbose)
-				DEBUG("STLINK_SWD_DP_FAULT\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_SWD_DP_ERROR:
-			if (verbose)
-				DEBUG("STLINK_SWD_DP_ERROR\n");
-			raise_exception(EXCEPTION_ERROR, "STLINK_SWD_DP_ERROR");
-			return STLINK_ERROR_FAIL;
-		case STLINK_SWD_DP_PARITY_ERROR:
-			if (verbose)
-				DEBUG("STLINK_SWD_DP_PARITY_ERROR\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_SWD_AP_WDATA_ERROR:
-			if (verbose)
-				DEBUG("STLINK_SWD_AP_WDATA_ERROR\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_SWD_AP_STICKY_ERROR:
-			if (verbose)
-				DEBUG("STLINK_SWD_AP_STICKY_ERROR\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_SWD_AP_STICKYORUN_ERROR:
-			if (verbose)
-				DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_BAD_AP_ERROR:
-			/* ADIV5 probe 256 APs, most of them are non exisitant.*/
-			return STLINK_ERROR_FAIL;
-		case STLINK_TOO_MANY_AP_ERROR:
-			/* TI TM4C duplicates AP. Error happens at AP9.*/
-			if (verbose)
-				DEBUG("STLINK_TOO_MANY_AP_ERROR\n");
-			return STLINK_ERROR_FAIL;
-		case STLINK_JTAG_UNKNOWN_CMD :
-			if (verbose)
-				DEBUG("STLINK_JTAG_UNKNOWN_CMD\n");
-			return STLINK_ERROR_FAIL;
-		default:
-			if (verbose)
-				DEBUG("unknown/unexpected STLINK status code 0x%x\n", data[0]);
-			return STLINK_ERROR_FAIL;
-	}
-}
-
-static int send_recv_retry(uint8_t *txbuf, size_t txsize,
-					 uint8_t *rxbuf, size_t rxsize)
-{
-	struct timeval start;
-	struct timeval now;
-	struct timeval diff;
-	gettimeofday(&start, NULL);
-	int res;
-	while(1) {
-		send_recv(txbuf, txsize, rxbuf, rxsize);
-		res = stlink_usb_error_check(rxbuf, 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("write_retry failed. ");
-			return res;
-		}
-	}
-	return res;
-}
-
-static int read_retry(uint8_t *txbuf, size_t txsize,
-					 uint8_t *rxbuf, size_t rxsize)
-{
-	struct timeval start;
-	struct timeval now;
-	struct timeval diff;
-	gettimeofday(&start, NULL);
-	int res;
-	while(1) {
-		send_recv(txbuf, txsize, rxbuf, rxsize);
-		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;
-		}
-	}
-	return res;
-}
-
-static int write_retry(uint8_t *cmdbuf, size_t cmdsize,
-					 uint8_t *txbuf, size_t txsize)
-{
-	struct timeval start;
-	struct timeval now;
-	struct timeval diff;
-	gettimeofday(&start, NULL);
-	int res;
-	while(1) {
-		send_recv(cmdbuf, cmdsize, NULL, 0);
-		send_recv(txbuf, txsize, NULL, 0);
-		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;
-		}
-	}
-	return res;
-}
-
-static void stlink_version(void)
-{
-	if (Stlink.ver_hw == 30) {
-		uint8_t cmd[16] = {STLINK_APIV3_GET_VERSION_EX};
-		uint8_t data[12];
-		int size = send_recv(cmd, 16, data, 12);
-		if (size == -1) {
-			printf("[!] send_recv STLINK_APIV3_GET_VERSION_EX\n");
-		}
-		Stlink.ver_stlink = data[0];
-		Stlink.ver_swim  =  data[1];
-		Stlink.ver_jtag  =  data[2];
-		Stlink.ver_mass  =  data[3];
-		Stlink.ver_bridge = data[4];
-		Stlink.block_size = 512;
-		Stlink.vid = data[3] <<  9 | data[8];
-		Stlink.pid = data[5] << 11 | data[10];
-	} else {
-		uint8_t cmd[16] = {STLINK_GET_VERSION};
-		uint8_t data[6];
-		int size = send_recv(cmd, 16, data, 6);
-		if (size == -1) {
-			printf("[!] send_recv STLINK_GET_VERSION_EX\n");
-		}
-		Stlink.vid = data[3] << 8 | data[2];
-		Stlink.pid = data[5] << 8 | data[4];
-		int  version = data[0] << 8 | data[1]; /* Big endian here!*/
-		Stlink.block_size = 64;
-		Stlink.ver_stlink = (version >> 12) & 0x0f;
-		Stlink.ver_jtag   = (version >>  6) & 0x3f;
-		if ((Stlink.pid == PRODUCT_ID_STLINKV21_MSD) ||
-			(Stlink.pid == PRODUCT_ID_STLINKV21)) {
-			Stlink.ver_mass = (version >> 0) & 0x3f;
-		} else {
-			Stlink.ver_swim = (version >> 0) & 0x3f;
-		}
-	}
-	DEBUG("V%dJ%d",Stlink.ver_stlink, Stlink.ver_jtag);
-	if (Stlink.ver_hw == 30) {
-		DEBUG("M%dB%dS%d", Stlink.ver_mass, Stlink.ver_bridge, Stlink.ver_swim);
-	} else if (Stlink.ver_hw == 20) {
-		DEBUG("S%d", Stlink.ver_swim);
-	} else if (Stlink.ver_hw == 21) {
-		DEBUG("M%d", Stlink.ver_mass);
-	}
-	DEBUG("\n");
-}
-
-void stlink_leave_state(void)
-{
-	uint8_t cmd[16] = {STLINK_GET_CURRENT_MODE};
-	uint8_t data[2];
-	send_recv(cmd, 16, data, 2);
-	if (data[0] == STLINK_DEV_DFU_MODE) {
-		uint8_t dfu_cmd[16] = {STLINK_DFU_COMMAND, STLINK_DFU_EXIT};
-		DEBUG("Leaving DFU Mode\n");
-		send_recv(dfu_cmd, 16, NULL, 0);
-	} else if (data[0] == STLINK_DEV_SWIM_MODE) {
-		uint8_t swim_cmd[16] = {STLINK_SWIM_COMMAND, STLINK_SWIM_EXIT};
-		DEBUG("Leaving SWIM Mode\n");
-		send_recv(swim_cmd, 16, NULL, 0);
-	} else if (data[0] == STLINK_DEV_DEBUG_MODE) {
-		uint8_t dbg_cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_EXIT};
-		DEBUG("Leaving DEBUG Mode\n");
-		send_recv(dbg_cmd, 16, NULL, 0);
-	} else if (data[0] == STLINK_DEV_BOOTLOADER_MODE) {
-		DEBUG("Leaving BOOTLOADER Mode\n");
-	} else if (data[0] == STLINK_DEV_MASS_MODE) {
-		DEBUG("Leaving MASS Mode\n");
-	} else {
-		DEBUG("Unknown Mode %02x\n", data[0]);
-	}
-}
-
-const char *stlink_target_voltage(void)
-{
-	uint8_t cmd[16] = {STLINK_GET_TARGET_VOLTAGE};
-	uint8_t data[8];
-	send_recv(cmd, 16, data, 8);
-	uint16_t adc[2];
-	adc[0] = data[0] | data[1] << 8; /* Calibration value? */
-	adc[1] = data[4] | data[5] << 8; /* Measured value?*/
-	float result  = 0.0;
-	if (adc[0])
-		result = 2.0 * adc[1] * 1.2 / adc[0];
-	static char res[6];
-	sprintf(res, "%4.2fV", result);
-	return res;
-}
-
-static void stlink_resetsys(void)
-{
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND ,STLINK_DEBUG_APIV2_RESETSYS};
-	uint8_t data[2];
-	send_recv(cmd, 16, data, 2);
-}
-
-void stlink_init(int argc, char **argv)
-{
-	BMP_CL_OPTIONS_t cl_opts = {0};
-	cl_opts.opt_idstring = "Blackmagic Debug Probe on StlinkV2/3";
-	cl_init(&cl_opts, argc, argv);
-	libusb_device **devs, *dev;
-	int r;
-	int ret = -1;
-	atexit(exit_function);
-	signal(SIGTERM, sigterm_handler);
-	signal(SIGINT, sigterm_handler);
-	libusb_init(&Stlink.libusb_ctx);
-	r = libusb_init(NULL);
-	if (r < 0)
-		DEBUG("Failed: %s", libusb_strerror(r));
-	bool hotplug = true;
-	if (!libusb_has_capability (LIBUSB_CAP_HAS_HOTPLUG)) {
-		printf("Hotplug capabilites are not supported on this platform\n");
-		hotplug = false;
-	}
-	ssize_t cnt;
-  rescan:
-	has_attached = 0;
-	memset(&Stlink, 0, sizeof(Stlink));
-	cnt = libusb_get_device_list(NULL, &devs);
-	if (cnt < 0) {
-		DEBUG("Failed: %s", libusb_strerror(r));
-		goto error;
-	}
-	int i = 0;
-	int nr_stlinks = 0;
-	while ((dev = devs[i++]) != NULL) {
-		struct libusb_device_descriptor desc;
-		int r = libusb_get_device_descriptor(dev, &desc);
-		if (r < 0) {
-			fprintf(stderr, "libusb_get_device_descriptor failed %s",
-					libusb_strerror(r));
-			goto error;
-		}
-		if ((desc.idVendor == VENDOR_ID_STLINK) &&
-			((desc.idProduct & PRODUCT_ID_STLINK_MASK) ==
-			 PRODUCT_ID_STLINK_GROUP)) {
-			if (desc.idProduct == PRODUCT_ID_STLINKV1)  { /* Reject V1 devices.*/
-				DEBUG("STLINKV1 not supported\n");
-				continue;
-			}
-			Stlink.vid = desc.idVendor;
-			Stlink.pid = desc.idProduct;
-			r = libusb_open(dev, &Stlink.handle);
-			if (r == LIBUSB_SUCCESS) {
-				uint8_t data[32];
-				uint16_t lang;
-				libusb_get_string_descriptor(
-					Stlink.handle, 0, 0, data, sizeof(data));
-				lang = data[2] << 8 | data[3];
-				unsigned char sernum[32];
-				if (desc.iSerialNumber) {
-					r = libusb_get_string_descriptor
-						(Stlink.handle, desc.iSerialNumber, lang,
-						  sernum, sizeof(sernum));
-				} else {
-					DEBUG("No serial number\n");
-				}
-				/* Older devices have hex values instead of ascii
-				 * in the serial string. Recode eventually!*/
-				bool readable = true;
-				uint16_t *p = (uint16_t *)sernum;
-				for (p += 1; *p; p++) {
-					bool isr = isalnum(*p);
-					readable &= isr;
-				}
-				char *s = Stlink.serial;
-				p = (uint16_t *)sernum;
-				for (p += 1; *p; p++, s++) {
-					if (readable)
-						*s = *p;
-					else
-						snprintf(s, 3, "%02x", *p & 0xff);
-				}
-				if (cl_opts.opt_serial && (!strncmp(Stlink.serial, cl_opts.opt_serial,
-													strlen(cl_opts.opt_serial))))
-					DEBUG("Found ");
-				if (desc.idProduct == PRODUCT_ID_STLINKV2) {
-					DEBUG("STLINKV20 serial %s\n", Stlink.serial);
-					Stlink.ver_hw = 20;
-					Stlink.ep_tx = 2;
-				} else if (desc.idProduct == PRODUCT_ID_STLINKV21) {
-					DEBUG("STLINKV21 serial %s\n", Stlink.serial);
-					Stlink.ver_hw = 21;
-					Stlink.ep_tx = 1;
-				} else if (desc.idProduct == PRODUCT_ID_STLINKV21_MSD) {
-					DEBUG("STLINKV21_MSD serial %s\n", Stlink.serial);
-					Stlink.ver_hw = 21;
-					Stlink.ep_tx = 1;
-				} else if (desc.idProduct == PRODUCT_ID_STLINKV3E) {
-					DEBUG("STLINKV3E serial %s\n", Stlink.serial);
-					Stlink.ver_hw = 30;
-					Stlink.ep_tx = 1;
-				} else if (desc.idProduct == PRODUCT_ID_STLINKV3) {
-					DEBUG("STLINKV3  serial %s\n", Stlink.serial);
-					Stlink.ver_hw = 30;
-					Stlink.ep_tx = 1;
-				} else {
-					DEBUG("Unknown STLINK variant, serial %s\n", Stlink.serial);
-				}
-				nr_stlinks++;
-				if (cl_opts.opt_serial) {
-					if (!strncmp(Stlink.serial, cl_opts.opt_serial,
-								 strlen(cl_opts.opt_serial))) {
-						break;
-					} else {
-						libusb_close(Stlink.handle);
-						Stlink.handle = 0;
-					}
-				}
-			} else {
-				DEBUG("Open failed %s\n", libusb_strerror(r));
-			}
-		}
-	}
-	libusb_free_device_list(devs, 1);
-	if (!Stlink.handle) {
-		if (nr_stlinks && cl_opts.opt_serial) {
-			DEBUG("No Stlink with given serial number %s\n",  cl_opts.opt_serial);
-		} else if (nr_stlinks > 1) {
-			DEBUG("Multiple Stlinks. Please specify serial number\n");
-			goto error;
-		} else {
-			DEBUG("No Stlink device found!\n");
-		}
-		if (hotplug && !cl_opts.opt_no_wait) {
-			libusb_hotplug_callback_handle hp;
-			int rc = libusb_hotplug_register_callback
-				(NULL, LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED, 0,
-				 VENDOR_ID_STLINK, LIBUSB_HOTPLUG_MATCH_ANY,
-				 LIBUSB_HOTPLUG_MATCH_ANY,
-				 hotplug_callback_attach, NULL, &hp);
-			if (LIBUSB_SUCCESS != rc) {
-				DEBUG("Error registering attach callback\n");
-				goto error;
-			}
-			DEBUG("Waiting for %sST device%s%s to attach\n",
-				  (cl_opts.opt_serial)? "" : "some ",
-				  (cl_opts.opt_serial)? " with serial ": "",
-				  (cl_opts.opt_serial)? cl_opts.opt_serial: "");
-			DEBUG("Terminate with ^C\n");
-			while (has_attached == 0) {
-				rc = libusb_handle_events (NULL);
-                if (rc < 0)
-					printf("libusb_handle_events() failed: %s\n",
-						   libusb_error_name(rc));
-			}
-			goto rescan;
-		}
-		goto error;
-	}
-	int config;
-	r = libusb_get_configuration(Stlink.handle, &config);
-	if (r) {
-		DEBUG("libusb_get_configuration failed %d: %s", r, libusb_strerror(r));
-		goto error_1;
-	}
-	DEBUG("Config %d\n", config);
-	if (config != 1) {
-		r = libusb_set_configuration(Stlink.handle, 0);
-		if (r) {
-			DEBUG("libusb_set_configuration failed %d: %s",
-				  r, libusb_strerror(r));
-			goto error_1;
-		}
-	}
-	r = libusb_claim_interface(Stlink.handle, 0);
-	if (r)
-	{
-		DEBUG("libusb_claim_interface failed %s\n", libusb_strerror(r));
-		goto error_1;
-	}
-	if (hotplug) { /* Allow gracefully exit when stlink is unplugged*/
-		libusb_hotplug_callback_handle hp;
-		int rc = libusb_hotplug_register_callback
-			(NULL, LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT, 0, Stlink.vid, Stlink.pid,
-			 LIBUSB_HOTPLUG_MATCH_ANY, hotplug_callback_detach, NULL, &hp);
-		if (LIBUSB_SUCCESS != rc) {
-			DEBUG("Error registering detach callback\n");
-			goto error;
-		}
-	}
-	Stlink.req_trans = libusb_alloc_transfer(0);
-	Stlink.rep_trans = libusb_alloc_transfer(0);
-	stlink_version();
-	if ((Stlink.ver_stlink < 3 && Stlink.ver_jtag < 32) ||
-		(Stlink.ver_stlink == 3 && Stlink.ver_jtag < 3)) {
-		/* Maybe the adapter is in some strange state. Try to reset */
-        int result = libusb_reset_device(Stlink.handle);
-		DEBUG("Trying reset\n");
-		if (result == LIBUSB_ERROR_BUSY) { /* Try again */
-			platform_delay(50);
-			result = libusb_reset_device(Stlink.handle);
-		}
-        if (result != LIBUSB_SUCCESS) {
-			DEBUG("libusb_reset_device failed\n");
-			goto error_1;
-		}
-		stlink_version();
-    }
-	if ((Stlink.ver_stlink < 3 && Stlink.ver_jtag < 32) ||
-		(Stlink.ver_stlink == 3 && Stlink.ver_jtag < 3)) {
-		DEBUG("Please update Firmware\n");
-		goto error_1;
-	}
-	stlink_leave_state();
-	stlink_resetsys();
-	if (cl_opts.opt_mode != BMP_MODE_DEBUG) {
-		ret = cl_execute(&cl_opts);
-	} else {
-		assert(gdb_if_init() == 0);
-		return;
-	}
-  error_1:
-	libusb_close(Stlink.handle);
-  error:
-	libusb_exit(Stlink.libusb_ctx);
-	exit(ret);
-}
-
-void stlink_srst_set_val(bool assert)
-{
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
-					  STLINK_DEBUG_APIV2_DRIVE_NRST,
-					  (assert)? STLINK_DEBUG_APIV2_DRIVE_NRST_LOW
-					  : STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH};
-	uint8_t data[2];
-	send_recv(cmd, 16, data, 2);
-	stlink_usb_error_check(data, true);
-}
-
-bool stlink_set_freq_divisor(uint16_t divisor)
-{
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
-					  STLINK_DEBUG_APIV2_SWD_SET_FREQ,
-					  divisor & 0xff, divisor >> 8};
-	uint8_t data[2];
-	send_recv(cmd, 16, data, 2);
-	if (stlink_usb_error_check(data, false))
-		return false;
-	return true;
-}
-
-bool stlink3_set_freq_divisor(uint16_t divisor)
-{
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
-					  STLINK_APIV3_GET_COM_FREQ,
-					  Stlink.transport_mode};
-	uint8_t data[52];
-	send_recv(cmd, 16, data, 52);
-	stlink_usb_error_check(data, true);
-	int size = data[8];
-	if (divisor > size)
-		divisor = size;
-	uint8_t *p = data + 12 + divisor * sizeof(uint32_t);
-	uint32_t freq = p[0] | p[1] << 8 | p[2] << 16 | p[3] << 24;
-	DEBUG("Selected %" PRId32 " khz\n", freq);
-	cmd[1] = STLINK_APIV3_SET_COM_FREQ;
-	cmd[2] = Stlink.transport_mode;
-	cmd[3] = 0;
-	p = data + 12 + divisor * sizeof(uint32_t);
-	cmd[4] = p[0];
-	cmd[5] = p[1];
-	cmd[6] = p[2];
-	cmd[7] = p[3];
-	send_recv(cmd, 16, data, 8);
-	return true;
-}
-
-int stlink_hwversion(void)
-{
-	return Stlink.ver_stlink;
-}
-
-int stlink_enter_debug_swd(void)
-{
-	stlink_leave_state();
-	Stlink.transport_mode = STLINK_MODE_SWD;
-	if (Stlink.ver_stlink == 3)
-		stlink3_set_freq_divisor(2);
-	else
-		stlink_set_freq_divisor(1);
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
-					  STLINK_DEBUG_APIV2_ENTER,
-					  STLINK_DEBUG_ENTER_SWD_NO_RESET};
-	uint8_t data[2];
-	DEBUG("Enter SWD\n");
-	send_recv_retry(cmd, 16, data, 2);
-	return stlink_usb_error_check(data, true);
-}
-
-int stlink_enter_debug_jtag(void)
-{
-	stlink_leave_state();
-	Stlink.transport_mode = STLINK_MODE_JTAG;
-	if (Stlink.ver_stlink == 3)
-		stlink3_set_freq_divisor(4);
-	else
-		stlink_set_freq_divisor(1);
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
-					  STLINK_DEBUG_APIV2_ENTER,
-					  STLINK_DEBUG_ENTER_JTAG_NO_RESET};
-	uint8_t data[2];
-	DEBUG("Enter JTAG\n");
-	send_recv(cmd, 16, data, 2);
-	return stlink_usb_error_check(data, true);
-}
-
-uint32_t stlink_read_coreid(void)
-{
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
-					  STLINK_DEBUG_READCOREID};
-	uint8_t data[4];
-	send_recv(cmd, 16, data, 4);
-	uint32_t id =  data[0] | data[1] << 8 | data[2] << 16 | data[3] << 24;
-	DEBUG("Read Core ID: 0x%08" PRIx32 "\n", id);
-	return id;
-}
-
-int stlink_read_idcodes(uint32_t *idcodes)
-{
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
-					   STLINK_DEBUG_APIV2_READ_IDCODES};
-	uint8_t data[12];
-	send_recv(cmd, 16, data, 12);
-	if (stlink_usb_error_check(data, true))
-		return 0;
-	uint8_t *p = data + 4;
-	idcodes[0] = p[0] | p[1] << 8 | p[2] << 16 | p[3] << 24;
-	p += 4;
-	idcodes[1] = p[0] | p[1] << 8 | p[2] << 16 | p[3] << 24;
-	return 2;
-}
-
-uint32_t stlink_dp_read(ADIv5_DP_t *dp, uint16_t addr)
-{
-	if (addr & ADIV5_APnDP) {
-		DEBUG_STLINK("AP read addr 0x%04" PRIx16 "\n", addr);
-		stlink_dp_low_access(dp, ADIV5_LOW_READ, addr, 0);
-		return stlink_dp_low_access(dp, ADIV5_LOW_READ,
-		                           ADIV5_DP_RDBUFF, 0);
-	} else {
-		DEBUG_STLINK("DP read addr 0x%04" PRIx16 "\n", addr);
-		return stlink_dp_low_access(dp, ADIV5_LOW_READ, addr, 0);
-	}
-}
-
-uint32_t stlink_dp_error(ADIv5_DP_t *dp)
-{
-	uint32_t err, clr = 0;
-
-	err = stlink_dp_read(dp, ADIV5_DP_CTRLSTAT) &
-		(ADIV5_DP_CTRLSTAT_STICKYORUN | ADIV5_DP_CTRLSTAT_STICKYCMP |
-		ADIV5_DP_CTRLSTAT_STICKYERR | ADIV5_DP_CTRLSTAT_WDATAERR);
-
-	if(err & ADIV5_DP_CTRLSTAT_STICKYORUN)
-		clr |= ADIV5_DP_ABORT_ORUNERRCLR;
-	if(err & ADIV5_DP_CTRLSTAT_STICKYCMP)
-		clr |= ADIV5_DP_ABORT_STKCMPCLR;
-	if(err & ADIV5_DP_CTRLSTAT_STICKYERR)
-		clr |= ADIV5_DP_ABORT_STKERRCLR;
-	if(err & ADIV5_DP_CTRLSTAT_WDATAERR)
-		clr |= ADIV5_DP_ABORT_WDERRCLR;
-
-	adiv5_dp_write(dp, ADIV5_DP_ABORT, clr);
-	dp->fault = 0;
-	if (err)
-		DEBUG("stlink_dp_error %d\n", err);
-	err |= Stlink.ap_error;
-	Stlink.ap_error = false;
-	return err;
-}
-
-void stlink_dp_abort(ADIv5_DP_t *dp, uint32_t abort)
-{
-	adiv5_dp_write(dp, ADIV5_DP_ABORT, abort);
-}
-
-int stlink_read_dp_register(uint16_t port, uint16_t addr, uint32_t *reg)
-{
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
-					  STLINK_DEBUG_APIV2_READ_DAP_REG,
-					  port & 0xff, port >> 8,
-					  addr & 0xff, addr >> 8};
-	if (port == STLINK_DEBUG_PORT_ACCESS  && Stlink.dap_select)
-		cmd[4] = ((Stlink.dap_select & 0xf) << 4) | (addr & 0xf);
-	else
-		cmd[4] = addr & 0xff;
-	DEBUG_STLINK("Read DP, Addr 0x%04" PRIx16 ": \n", addr);
-	uint8_t data[8];
-	int res = send_recv_retry(cmd, 16, data, 8);
-	if (res == STLINK_ERROR_OK) {
-		uint32_t ret = data[4] | data[5] << 8 | data[6] << 16 | data[7] << 24;
-		DEBUG_STLINK("0x%08" PRIx32" \n", ret);
-		*reg = ret;
-	} else {
-		DEBUG_STLINK("failed, res %d\n", res);
-	}
-	return res;
-}
-
-int stlink_write_dp_register(uint16_t port, uint16_t addr, uint32_t val)
-{
-	if (port == STLINK_DEBUG_PORT_ACCESS && addr == 8) {
-		Stlink.dap_select = val;
-		DEBUG_STLINK("Caching SELECT 0x%02" PRIx32 "\n", val);
-		return STLINK_ERROR_OK;
-	} else {
-		uint8_t cmd[16] = {
-			STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_WRITE_DAP_REG,
-			port & 0xff, port >> 8,
-			addr & 0xff, addr >> 8,
-			val & 0xff, (val >>  8) & 0xff, (val >> 16) & 0xff,
-			(val >> 24) & 0xff};
-		uint8_t data[2];
-		send_recv_retry(cmd, 16, data, 2);
-		DEBUG_STLINK("Write DP, Addr 0x%04" PRIx16 ": 0x%08" PRIx32
-			  " \n", addr, val);
-		return stlink_usb_error_check(data, true);
-	}
-}
-
-uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
-				      uint16_t addr, uint32_t value)
-{
-	uint32_t response = 0;
-	int res;
-	if (RnW) {
-		res = stlink_read_dp_register(
-			STLINK_DEBUG_PORT_ACCESS, addr, &response);
-		DEBUG_STLINK("SWD read addr %04" PRIx16 ": %08" PRIx32 "\n",
-					 addr, response);
-	} else {
-		DEBUG_STLINK("SWD write addr %04" PRIx16 ": %08" PRIx32 "\n",
-					 addr, value);
-		res = stlink_write_dp_register(STLINK_DEBUG_PORT_ACCESS, addr, value);
-	}
-	if (res == STLINK_ERROR_WAIT)
-		raise_exception(EXCEPTION_TIMEOUT, "DP ACK timeout");
-
-	if(res == STLINK_ERROR_DP_FAULT) {
-		dp->fault = 1;
-		return 0;
-	}
-	if(res == STLINK_ERROR_FAIL)
-		raise_exception(EXCEPTION_ERROR, "SWDP invalid ACK");
-
-	return response;
-}
-
-static bool stlink_ap_setup(int ap)
-{
-	if (ap > 7)
-		return false;
-	uint8_t cmd[16] = {
-		STLINK_DEBUG_COMMAND,
-		STLINK_DEBUG_APIV2_INIT_AP,
-		ap,
-	};
-	uint8_t data[2];
-	send_recv_retry(cmd, 16, data, 2);
-	DEBUG_STLINK("Open AP %d\n", ap);
-	int res = stlink_usb_error_check(data, true);
-	if (res) {
-		if (Stlink.ver_hw == 30) {
-			DEBUG("STLINKV3 only connects to STM8/32!\n");
-		}
-		return false;
-	}
-	return true;
-}
-
-static void stlink_ap_cleanup(int ap)
-{
-       uint8_t cmd[16] = {
-               STLINK_DEBUG_COMMAND,
-               STLINK_DEBUG_APIV2_CLOSE_AP_DBG,
-               ap,
-       };
-       uint8_t data[2];
-       send_recv(cmd, 16, data, 2);
-       DEBUG_STLINK("Close AP %d\n", ap);
-       stlink_usb_error_check(data, true);
-}
-static int stlink_usb_get_rw_status(bool verbose)
-{
-	uint8_t cmd[16] = {
-		STLINK_DEBUG_COMMAND,
-		STLINK_DEBUG_APIV2_GETLASTRWSTATUS2
-	};
-	uint8_t data[12];
-	send_recv(cmd, 16, data, 12);
-	return stlink_usb_error_check(data, verbose);
-}
-
-static void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
-{
-	if (len == 0)
-		return;
-	size_t read_len = len;
-	uint8_t type;
-	char *CMD;
-	if (src & 1 || len & 1) {
-		CMD = "READMEM_8BIT";
-		type = STLINK_DEBUG_READMEM_8BIT;
-		if (len > Stlink.block_size) {
-			DEBUG(" Too large!\n");
-			return;
-		}
-		if (len == 1)
-			read_len ++; /* Fix read length as in openocd*/
-	} else if (src & 3 || len & 3) {
-		CMD = "READMEM_16BIT";
-		type = STLINK_DEBUG_APIV2_READMEM_16BIT;
-	} else {
-		CMD = "READMEM_32BIT";
-		type = STLINK_DEBUG_READMEM_32BIT;
-
-	}
-	DEBUG_STLINK("%s len %zu addr 0x%08" PRIx32 " AP %d : ",
-				 CMD, len, src, ap->apsel);
-	uint8_t cmd[16] = {
-		STLINK_DEBUG_COMMAND,
-		type,
-		src & 0xff, (src >>  8) & 0xff, (src >> 16) & 0xff,
-		(src >> 24) & 0xff,
-		len & 0xff, len >> 8, ap->apsel};
-	int res = read_retry(cmd, 16, dest, read_len);
-	if (res == STLINK_ERROR_OK) {
-		uint8_t *p = (uint8_t*)dest;
-		for (size_t i = 0; i < len ; i++) {
-			DEBUG_STLINK("%02x", *p++);
-		}
-	} else {
-		/* FIXME: What is the right measure when failing?
-		 *
-		 * E.g. TM4C129 gets here when NRF probe reads 0x10000010
-		 * Approach taken:
-		 * Fill the memory with some fixed pattern so hopefully
-		 * the caller notices the error*/
-		DEBUG("stlink_readmem failed\n");
-		memset(dest, 0xff, len);
-	}
-	DEBUG_STLINK("\n");
-}
-
-static void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
-					  uint8_t *buffer)
-{
-	DEBUG_STLINK("Mem Write8 AP %d len %zu addr 0x%08" PRIx32 ": ",
-				 ap->apsel, len, addr);
-	for (size_t t = 0; t < len; t++) {
-		DEBUG_STLINK("%02x", buffer[t]);
-	}
-	DEBUG_STLINK("\n");
-	while (len) {
-		size_t length;
-		if (len > Stlink.block_size)
-			length = Stlink.block_size;
-		else
-			length = len;
-		uint8_t cmd[16] = {
-			STLINK_DEBUG_COMMAND,
-			STLINK_DEBUG_WRITEMEM_8BIT,
-			addr & 0xff, (addr >>  8) & 0xff, (addr >> 16) & 0xff,
-			(addr >> 24) & 0xff,
-			length & 0xff, length >> 8, ap->apsel};
-		send_recv(cmd, 16, NULL, 0);
-		send_recv((void*)buffer, length, NULL, 0);
-		stlink_usb_get_rw_status(true);
-		len -= length;
-		addr += length;
-	}
-}
-
-static void stlink_writemem16(ADIv5_AP_t *ap, uint32_t addr, size_t len,
-					   uint16_t *buffer)
-{
-	DEBUG_STLINK("Mem Write16 AP %d len %zu addr 0x%08" PRIx32 ": ",
-				 ap->apsel, len, addr);
-	for (size_t t = 0; t < len; t+=2) {
-		DEBUG_STLINK("%04x", buffer[t]);
-	}
-	DEBUG_STLINK("\n");
-	uint8_t cmd[16] = {
-		STLINK_DEBUG_COMMAND,
-		STLINK_DEBUG_APIV2_WRITEMEM_16BIT,
-		addr & 0xff, (addr >>  8) & 0xff, (addr >> 16) & 0xff,
-		(addr >> 24) & 0xff,
-		len & 0xff, len >> 8, ap->apsel};
-	send_recv(cmd, 16, NULL, 0);
-	send_recv((void*)buffer, len, NULL, 0);
-	stlink_usb_get_rw_status(true);
-}
-
-static void stlink_writemem32(ADIv5_AP_t *ap, uint32_t addr, size_t len,
-					   uint32_t *buffer)
-{
-	DEBUG_STLINK("Mem Write32 AP %d len %zu addr 0x%08" PRIx32 ": ",
-				 ap->apsel, len, addr);
-	for (size_t t = 0; t < len; t+=4) {
-		DEBUG_STLINK("%04x", buffer[t]);
-	}
-	DEBUG_STLINK("\n");
-	uint8_t cmd[16] = {
-		STLINK_DEBUG_COMMAND,
-		STLINK_DEBUG_WRITEMEM_32BIT,
-		addr & 0xff, (addr >>  8) & 0xff, (addr >> 16) & 0xff,
-		(addr >> 24) & 0xff,
-		len & 0xff, len >> 8, ap->apsel};
-	write_retry(cmd, 16, (void*)buffer, len);
-}
-
-void stlink_regs_read(ADIv5_AP_t *ap, void *data)
-{
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READALLREGS,
-					   ap->apsel};
-	uint8_t res[88];
-	DEBUG_STLINK("AP %d: Read all core registers\n", ap->apsel);
-	send_recv(cmd, 16, res, 88);
-	stlink_usb_error_check(res, true);
-	memcpy(data, res + 4, 84);
-}
-
-uint32_t stlink_reg_read(ADIv5_AP_t *ap, int num)
-{
-	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READREG, num,
-					   ap->apsel};
-	uint8_t res[8];
-	send_recv(cmd, 16, res, 8);
-	stlink_usb_error_check(res, true);
-	uint32_t ret = res[0] | res[1] << 8 | res[2] << 16 | res[3] << 24;
-	DEBUG_STLINK("AP %d: Read reg %02" PRId32 " val 0x%08" PRIx32 "\n",
-				 ap->apsel, num, ret);
-	return ret;
-}
-
-void stlink_reg_write(ADIv5_AP_t *ap, int num, uint32_t val)
-{
-	uint8_t cmd[16] = {
-		STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_WRITEREG, num,
-		val & 0xff, (val >>  8) & 0xff, (val >> 16) & 0xff,
-		(val >> 24) & 0xff, ap->apsel};
-	uint8_t res[2];
-	send_recv(cmd, 16, res, 2);
-	DEBUG_STLINK("AP %d: Write reg %02" PRId32 " val 0x%08" PRIx32 "\n",
-				 ap->apsel, num, val);
-	stlink_usb_error_check(res, true);
-}
-
-static void stlink_mem_write_sized(	ADIv5_AP_t *ap, uint32_t dest,
-									const void *src, size_t len,
-									enum align align)
-{
-	if (len == 0)
-		return;
-	switch(align) {
-	case ALIGN_BYTE: stlink_writemem8(ap, dest, len, (uint8_t *) src);
-		break;
-	case ALIGN_HALFWORD: stlink_writemem16(ap, dest, len, (uint16_t *) src);
-		break;
-	case ALIGN_WORD: stlink_writemem32(ap, dest, len, (uint32_t *) src);
-		break;
-	case ALIGN_DWORD: stlink_writemem32(ap, dest, len, (uint32_t *) src);
-		break;
-	}
-}
-
-static void stlink_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
-{
-       stlink_write_dp_register(ap->apsel, addr, value);
-}
-
-static uint32_t stlink_ap_read(ADIv5_AP_t *ap, uint16_t addr)
-{
-	uint32_t ret;
-	stlink_read_dp_register(ap->apsel, addr, &ret);
-	return ret;
-}
-
-struct jtag_dev_s jtag_devs[JTAG_MAX_DEVS+1];
-int jtag_dev_count;
-jtag_proc_t jtag_proc;
-
-int jtag_scan_stlinkv2(const uint8_t *irlens)
-{
-	uint32_t idcodes[JTAG_MAX_DEVS+1];
-	(void) *irlens;
-	target_list_free();
-
-	jtag_dev_count = 0;
-	memset(&jtag_devs, 0, sizeof(jtag_devs));
-	if (stlink_enter_debug_jtag())
-		return 0;
-	jtag_dev_count = stlink_read_idcodes(idcodes);
-	/* Check for known devices and handle accordingly */
-	for(int i = 0; i < jtag_dev_count; i++)
-		jtag_devs[i].idcode = idcodes[i];
-	for(int i = 0; i < jtag_dev_count; i++)
-		for(int j = 0; dev_descr[j].idcode; j++)
-			if((jtag_devs[i].idcode & dev_descr[j].idmask) ==
-			   dev_descr[j].idcode) {
-				if(dev_descr[j].handler)
-					dev_descr[j].handler(&jtag_devs[i]);
-				break;
-			}
-
-	return jtag_dev_count;
-}
-
-int platform_jtag_dp_init(ADIv5_DP_t *dp)
-{
-	dp->dp_read = stlink_dp_read;
-	dp->error = stlink_dp_error;
-	dp->low_access = stlink_dp_low_access;
-	dp->abort = stlink_dp_abort;
-
-	return true;
-
-}
-
-void platform_adiv5_dp_defaults(ADIv5_DP_t *dp)
-{
-	dp->ap_regs_read = stlink_regs_read;
-	dp->ap_reg_read = stlink_reg_read;
-	dp->ap_reg_write = stlink_reg_write;
-	dp->ap_setup = stlink_ap_setup;
-	dp->ap_cleanup = stlink_ap_cleanup;
-	dp->ap_write = stlink_ap_write;
-	dp->ap_read = stlink_ap_read;
-	dp->mem_read = stlink_readmem;
-	dp->mem_write_sized = stlink_mem_write_sized;
-}
diff --git a/src/platforms/pc-stlinkv2/stlinkv2.h b/src/platforms/pc-stlinkv2/stlinkv2.h
deleted file mode 100644
index dea9a8b..0000000
--- a/src/platforms/pc-stlinkv2/stlinkv2.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2019 Uwe Bonnes
- *
- * 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/>.
- */
-#if !defined(__STLINKV2_H_)
-
-#define STLINK_ERROR_FAIL -1
-#define STLINK_ERROR_OK 0
-#define STLINK_ERROR_WAIT 1
-
-#define STLINK_DEBUG_PORT_ACCESS            0xffff
-
-void stlink_init(int argc, char **argv);
-int stlink_hwversion(void);
-void stlink_leave_state(void);
-const char *stlink_target_voltage(void);
-void stlink_srst_set_val(bool assert);
-int stlink_enter_debug_swd(void);
-int stlink_enter_debug_jtag(void);
-int stlink_read_idcodes(uint32_t *);
-uint32_t stlink_read_coreid(void);
-int stlink_read_dp_register(uint16_t port, uint16_t addr, uint32_t *res);
-int stlink_write_dp_register(uint16_t port, uint16_t addr, uint32_t val);
-
-uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
-				      uint16_t addr, uint32_t value);
-uint32_t stlink_dp_read(ADIv5_DP_t *dp, uint16_t addr);
-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);
-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);
-extern  int debug_level;
-# define DEBUG_STLINK if (debug_level > 0) printf
-# define DEBUG_USB    if (debug_level > 1) printf
-#endif
diff --git a/src/platforms/pc-stlinkv2/swdptap.c b/src/platforms/pc-stlinkv2/swdptap.c
deleted file mode 100644
index e69de29..0000000
diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index d68dc78..6fd4fe9 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -120,27 +120,29 @@ static void cl_help(char **argv, BMP_CL_OPTIONS_t *opt)
 	printf("\t-h\t\t: This help.\n");
 	printf("\t-v[1|2]\t\t: Increasing verbosity\n");
 	printf("\t-d \"path\"\t: Use serial device at \"path\"\n");
-	printf("\t-P <num>\t: Use device found as <num>");
+	printf("\t-P <num>\t: Use debugger found at position <num>\n");
+	printf("\t-n <num>\t: Use target device found at position <num>\n");
 	printf("\t-s \"string\"\t: Use dongle with (partial) "
 		  "serial number \"string\"\n");
 	printf("\t-c \"string\"\t: Use ftdi dongle with type \"string\"\n");
-	printf("\t-n\t\t: Exit immediate if no device found\n");
 	printf("\tRun mode related options:\n");
+	printf("\tDefault mode is to start the debug server at :2000\n");
+	printf("\t-j\t\t: Use JTAG. SWD is default.\n");
 	printf("\t-C\t\t: Connect under reset\n");
-	printf("\t-t\t\t: Scan SWD, with no target found scan jtag and exit\n");
+	printf("\t-t\t\t: Scan SWD and display information about connected"
+		   "devices\n");
 	printf("\t-E\t\t: Erase flash until flash end or for given size\n");
 	printf("\t-V\t\t: Verify flash against binary file\n");
 	printf("\t-r\t\t: Read flash and write to binary file\n");
 	printf("\t-p\t\t: Supplies power to the target (where applicable)\n");
 	printf("\t-R\t\t: Reset device\n");
-	printf("\t\tDefault mode is starting the debug server\n");
 	printf("\tFlash operation modifiers options:\n");
 	printf("\t-a <num>\t: Start flash operation at flash address <num>\n"
-		"\t\t\tDefault start is 0x08000000\n");
+		"\t\t\t  Default start is 0x08000000\n");
 	printf("\t-S <num>\t: Read <num> bytes. Default is until read fails.\n");
-	printf("\t-j\t\t: Use JTAG. SWD is default.\n");
 	printf("\t <file>\t\t: Use (binary) file <file> for flash operation\n"
-		   "\t\t\tGiven <file> writes to flash if neither -r or -V is given\n");
+		   "\t\t\t  Given <file> writes to flash if neither -r or -V is "
+		   "given\n");
 	exit(0);
 }
 
@@ -150,7 +152,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 	opt->opt_target_dev = 1;
 	opt->opt_flash_start = 0x08000000;
 	opt->opt_flash_size = 16 * 1024 *1024;
-	while((c = getopt(argc, argv, "Ehv::d:s:I:c:CnN:tVta:S:jpP:rR")) != -1) {
+	while((c = getopt(argc, argv, "Ehv::d:s:I:c:Cn:tVta:S:jpP:rR")) != -1) {
 		switch(c) {
 		case 'c':
 			if (optarg)
@@ -171,9 +173,6 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 		case 'C':
 			opt->opt_connect_under_reset = true;
 			break;
-		case 'n':
-			opt->opt_no_wait = true;
-			break;
 		case 'd':
 			if (optarg)
 				opt->opt_device = optarg;
@@ -208,7 +207,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 			if (optarg)
 				opt->opt_flash_start = strtol(optarg, NULL, 0);
 			break;
-		case 'N':
+		case 'n':
 			if (optarg)
 				opt->opt_target_dev = strtol(optarg, NULL, 0);
 			break;
diff --git a/src/platforms/pc/cl_utils.h b/src/platforms/pc/cl_utils.h
index 5c485b7..88bff7e 100644
--- a/src/platforms/pc/cl_utils.h
+++ b/src/platforms/pc/cl_utils.h
@@ -39,7 +39,6 @@ enum bmp_cl_mode {
 typedef struct BMP_CL_OPTIONS_s {
 	enum bmp_cl_mode opt_mode;
 	bool opt_usejtag;
-	bool opt_no_wait;
 	bool opt_tpwr;
 	bool opt_connect_under_reset;
 	char *opt_flash_file;
diff --git a/src/platforms/pc/libusb_utils.c b/src/platforms/pc/libusb_utils.c
index 26c95a0..2672490 100644
--- a/src/platforms/pc/libusb_utils.c
+++ b/src/platforms/pc/libusb_utils.c
@@ -18,7 +18,6 @@
  */
 #include "general.h"
 #include "cl_utils.h"
-#include <libusb_utils.h>
 
 static void LIBUSB_CALL on_trans_done(struct libusb_transfer *trans)
 {

From ab7991c3a6917f8aa82d6832b28f440aec432f52 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sun, 19 Apr 2020 18:22:06 +0200
Subject: [PATCH 29/43] hosted: Add bmp_remote, remove old pc-hosted platform.

---
 src/platforms/hosted/Makefile.inc     |   5 +
 src/platforms/hosted/bmp_remote.c     | 147 ++++++++++++++++++++
 src/platforms/hosted/bmp_remote.h     |  39 ++++++
 src/platforms/hosted/platform.c       |  34 ++++-
 src/platforms/hosted/platform.h       |   2 +
 src/platforms/hosted/remote_jtagtap.c | 165 ++++++++++++++++++++++
 src/platforms/hosted/remote_swdptap.c | 128 ++++++++++++++++++
 src/platforms/pc-hosted/Makefile.inc  |  16 ---
 src/platforms/pc-hosted/README.md     |  40 ------
 src/platforms/pc-hosted/jtagtap.c     | 161 ----------------------
 src/platforms/pc-hosted/platform.c    | 188 --------------------------
 src/platforms/pc-hosted/platform.h    |  54 --------
 src/platforms/pc-hosted/swdptap.c     | 133 ------------------
 13 files changed, 516 insertions(+), 596 deletions(-)
 create mode 100644 src/platforms/hosted/bmp_remote.c
 create mode 100644 src/platforms/hosted/bmp_remote.h
 create mode 100644 src/platforms/hosted/remote_jtagtap.c
 create mode 100644 src/platforms/hosted/remote_swdptap.c
 delete mode 100644 src/platforms/pc-hosted/Makefile.inc
 delete mode 100644 src/platforms/pc-hosted/README.md
 delete mode 100644 src/platforms/pc-hosted/jtagtap.c
 delete mode 100644 src/platforms/pc-hosted/platform.c
 delete mode 100644 src/platforms/pc-hosted/platform.h
 delete mode 100644 src/platforms/pc-hosted/swdptap.c

diff --git a/src/platforms/hosted/Makefile.inc b/src/platforms/hosted/Makefile.inc
index 94ed44b..7fb94dc 100644
--- a/src/platforms/hosted/Makefile.inc
+++ b/src/platforms/hosted/Makefile.inc
@@ -3,11 +3,16 @@ CFLAGS += -DENABLE_DEBUG -DPLATFORM_HAS_DEBUG
 CFLAGS +=-I ./target -I./platforms/pc
 LDFLAGS += -lusb-1.0
 ifneq (, $(findstring mingw, $(SYS)))
+SRC += 	 serial_win.c
 LDFLAGS += -lws2_32
 else ifneq (, $(findstring cygwin, $(SYS)))
+SRC += 	 serial_win.c
 LDFLAGS += -lws2_32
+else
+SRC += serial_unix.c
 endif
 VPATH += platforms/pc
 SRC += timing.c cl_utils.c utils.c libusb_utils.c
 SRC += stlinkv2.c
+SRC += bmp_remote.c remote_swdptap.c remote_jtagtap.c
 PC_HOSTED = 1
diff --git a/src/platforms/hosted/bmp_remote.c b/src/platforms/hosted/bmp_remote.c
new file mode 100644
index 0000000..06e828a
--- /dev/null
+++ b/src/platforms/hosted/bmp_remote.c
@@ -0,0 +1,147 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2011  Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ * Additions by Dave Marples <dave@marples.net>
+ * Additions by 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 "general.h"
+#include "gdb_if.h"
+#include "version.h"
+#include "platform.h"
+#include "remote.h"
+#include "target.h"
+#include "bmp_remote.h"
+
+#include <assert.h>
+#include <sys/time.h>
+#include <sys/time.h>
+#include <errno.h>
+#include <string.h>
+
+#include "adiv5.h"
+
+int remote_init(void)
+{
+	char construct[REMOTE_MAX_MSG_SIZE];
+	int c = snprintf(construct, REMOTE_MAX_MSG_SIZE, "%s", REMOTE_START_STR);
+	platform_buffer_write((uint8_t *)construct, c);
+	c = platform_buffer_read((uint8_t *)construct, REMOTE_MAX_MSG_SIZE);
+
+	if ((!c) || (construct[0] == REMOTE_RESP_ERR)) {
+		fprintf(stderr,"Remote Start failed, error %s\n",
+				c ? (char *)&(construct[1]) : "unknown");
+      return -1;
+    }
+
+	printf("Remote is %s\n", &construct[1]);
+	return 0;
+}
+
+bool remote_target_get_power(void)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s=snprintf((char *)construct, REMOTE_MAX_MSG_SIZE, "%s",
+			   REMOTE_PWR_GET_STR);
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+      fprintf(stderr," platform_target_get_power failed, error %s\n",
+			  s ? (char *)&(construct[1]) : "unknown");
+      exit (-1);
+    }
+
+	return (construct[1] == '1');
+}
+
+void remote_target_set_power(bool power)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE,REMOTE_PWR_SET_STR,
+				 power ? '1' : '0');
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+		fprintf(stderr, "platform_target_set_power failed, error %s\n",
+				s ? (char *)&(construct[1]) : "unknown");
+      exit(-1);
+    }
+}
+
+void remote_srst_set_val(bool assert)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE, REMOTE_SRST_SET_STR,
+				 assert ? '1' : '0');
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+		fprintf(stderr, "platform_srst_set_val failed, error %s\n",
+				s ? (char *)&(construct[1]) : "unknown");
+      exit(-1);
+    }
+}
+
+bool remote_srst_get_val(void)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE,"%s",
+				 REMOTE_SRST_GET_STR);
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+      fprintf(stderr, "platform_srst_set_val failed, error %s\n",
+			  s ? (char *)&(construct[1]) : "unknown");
+      exit(-1);
+    }
+	return (construct[1] == '1');
+}
+
+const char *remote_target_voltage(void)
+{
+	static uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE," %s",
+				 REMOTE_VOLTAGE_STR);
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+      fprintf(stderr, "platform_target_voltage failed, error %s\n",
+			  s ? (char *)&(construct[1]) : "unknown");
+      exit(- 1);
+    }
+	return (char *)&construct[1];
+}
diff --git a/src/platforms/hosted/bmp_remote.h b/src/platforms/hosted/bmp_remote.h
new file mode 100644
index 0000000..1b9ac41
--- /dev/null
+++ b/src/platforms/hosted/bmp_remote.h
@@ -0,0 +1,39 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2020 Uwe Bonnes
+ *
+ * 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/>.
+ */
+#if !defined(__BMP_REMOTE_H_)
+#define      __BMP_REMOTE_H_
+#include "swdptap.h"
+#include "jtagtap.h"
+
+#define REMOTE_MAX_MSG_SIZE (256)
+
+int platform_buffer_write(const uint8_t *data, int size);
+int platform_buffer_read(uint8_t *data, int size);
+
+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);
+const char *remote_target_voltage(void);
+void remote_target_set_power(bool power);
+void remote_srst_set_val(bool assert);
+bool remote_srst_get_val(void);
+const char *platform_target_voltage(void);
+#define __BMP_REMOTE_H_
+#endif
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 69f340b..55f8a52 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -31,6 +31,8 @@
 #include "cl_utils.h"
 #include "gdb_if.h"
 #include <signal.h>
+
+#include "bmp_remote.h"
 #include "stlinkv2.h"
 
 #define VENDOR_ID_BMP            0x1d50
@@ -220,6 +222,11 @@ void platform_init(int argc, char **argv)
 		   info.manufacturer,
 		   info.product);
 	switch (info.bmp_type) {
+	case BMP_TYPE_BMP:
+		if (serial_open(&cl_opts, info.serial))
+			exit(-1);
+		remote_init();
+		break;
 	case BMP_TYPE_STLINKV2:
 		if (stlink_init( &info))
 			exit(-1);
@@ -240,6 +247,7 @@ void platform_init(int argc, char **argv)
 int platform_adiv5_swdp_scan(void)
 {
 	switch (info.bmp_type) {
+	case BMP_TYPE_BMP:
 	case BMP_TYPE_STLINKV2:
 	{
 		target_list_free();
@@ -261,6 +269,8 @@ int platform_adiv5_swdp_scan(void)
 int platform_swdptap_init(void)
 {
 	switch (info.bmp_type) {
+	case BMP_TYPE_BMP:
+		return remote_swdptap_init(&swd_proc);
 	case BMP_TYPE_STLINKV2:
 		return 0;
 		break;
@@ -273,6 +283,7 @@ int platform_swdptap_init(void)
 int platform_jtag_scan(const uint8_t *lrlens)
 {
 	switch (info.bmp_type) {
+	case BMP_TYPE_BMP:
 	case BMP_TYPE_STLINKV2:
 		return jtag_scan_stlinkv2(&info, lrlens);
 	default:
@@ -284,6 +295,8 @@ int platform_jtag_scan(const uint8_t *lrlens)
 int platform_jtagtap_init(void)
 {
 	switch (info.bmp_type) {
+	case BMP_TYPE_BMP:
+		return remote_jtagtap_init(&jtag_proc);
 	case BMP_TYPE_STLINKV2:
 		return 0;
 	default:
@@ -305,10 +318,11 @@ void platform_adiv5_dp_defaults(ADIv5_DP_t *dp)
 int platform_jtag_dp_init(ADIv5_DP_t *dp)
 {
 	switch (info.bmp_type) {
+	case BMP_TYPE_BMP:
 	case BMP_TYPE_STLINKV2:
 		return stlink_jtag_dp_init(dp);
 	default:
-		return -1;
+		return 0;
 	}
 	return 0;
 }
@@ -322,7 +336,7 @@ char *platform_ident(void)
 		return "BMP";
 	  case BMP_TYPE_STLINKV2:
 		return "STLINKV2";
-	  case BMP_TYPE_LIBFTDI:
+	case BMP_TYPE_LIBFTDI:
 		return "LIBFTDI";
 	  case BMP_TYPE_CMSIS_DAP:
 		return "CMSIS_DAP";
@@ -335,10 +349,12 @@ char *platform_ident(void)
 const char *platform_target_voltage(void)
 {
 	switch (info.bmp_type) {
+	case BMP_TYPE_BMP:
+		return remote_target_voltage();
 	case BMP_TYPE_STLINKV2:
 		return stlink_target_voltage(&info);
 	default:
-		break;;
+		break;
 	}
 	return NULL;
 }
@@ -348,6 +364,8 @@ void platform_srst_set_val(bool assert)
 	switch (info.bmp_type) {
 	case BMP_TYPE_STLINKV2:
 		return stlink_srst_set_val(&info, assert);
+	case BMP_TYPE_BMP:
+		return remote_srst_set_val(assert);
 	default:
 		break;
 	}
@@ -356,6 +374,8 @@ void platform_srst_set_val(bool assert)
 bool platform_srst_get_val(void)
 {
 	switch (info.bmp_type) {
+	case BMP_TYPE_BMP:
+		return remote_srst_get_val();
 	case BMP_TYPE_STLINKV2:
 		return stlink_srst_get_val();
 	default:
@@ -364,4 +384,10 @@ bool platform_srst_get_val(void)
 	return false;
 }
 
-void platform_buffer_flush(void) {}
+void platform_buffer_flush(void)
+{
+	switch (info.bmp_type) {
+	default:
+		break;
+	}
+}
diff --git a/src/platforms/hosted/platform.h b/src/platforms/hosted/platform.h
index 5a9cea2..b0e5043 100644
--- a/src/platforms/hosted/platform.h
+++ b/src/platforms/hosted/platform.h
@@ -1,7 +1,9 @@
 #ifndef __PLATFORM_H
 #define __PLATFORM_H
+
 #include <libusb-1.0/libusb.h>
 #include "libusb_utils.h"
+#include <libftdi1/ftdi.h>
 
 #include "timing.h"
 
diff --git a/src/platforms/hosted/remote_jtagtap.c b/src/platforms/hosted/remote_jtagtap.c
new file mode 100644
index 0000000..7f64eee
--- /dev/null
+++ b/src/platforms/hosted/remote_jtagtap.c
@@ -0,0 +1,165 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2008  Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ * Modified by Dave Marples <dave@marples.net>
+ *
+ * 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/>.
+ */
+
+/* Low level JTAG implementation using FT2232 with libftdi.
+ *
+ * Issues:
+ * Should share interface with swdptap.c or at least clean up...
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+
+#include <assert.h>
+
+#include "general.h"
+#include "remote.h"
+#include "jtagtap.h"
+#include "bmp_remote.h"
+
+static void jtagtap_reset(void);
+static void jtagtap_tms_seq(uint32_t MS, int ticks);
+static void jtagtap_tdi_tdo_seq(
+	uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks);
+static void jtagtap_tdi_seq(
+	const uint8_t final_tms, const uint8_t *DI, int ticks);
+static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI);
+
+int remote_jtagtap_init(jtag_proc_t *jtag_proc)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE, "%s",
+				 REMOTE_JTAG_INIT_STR);
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+      fprintf(stderr, "jtagtap_init failed, error %s\n",
+			  s ? (char *)&(construct[1]) : "unknown");
+      exit(-1);
+    }
+
+	jtag_proc->jtagtap_reset = jtagtap_reset;
+	jtag_proc->jtagtap_next =jtagtap_next;
+	jtag_proc->jtagtap_tms_seq = jtagtap_tms_seq;
+	jtag_proc->jtagtap_tdi_tdo_seq = jtagtap_tdi_tdo_seq;
+	jtag_proc->jtagtap_tdi_seq = jtagtap_tdi_seq;
+
+	return 0;
+}
+
+/* See remote.c/.h for protocol information */
+
+static void jtagtap_reset(void)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE, "%s",
+				 REMOTE_JTAG_RESET_STR);
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+		fprintf(stderr, "jtagtap_reset failed, error %s\n",
+				s ? (char *)&(construct[1]) : "unknown");
+		exit(-1);
+    }
+}
+
+static void jtagtap_tms_seq(uint32_t MS, int ticks)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE,
+				 REMOTE_JTAG_TMS_STR, ticks, MS);
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+		fprintf(stderr, "jtagtap_tms_seq failed, error %s\n",
+				s ? (char *)&(construct[1]) : "unknown");
+		exit(-1);
+    }
+}
+
+static void jtagtap_tdi_tdo_seq(
+	uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	uint64_t DIl=*(uint64_t *)DI;
+
+	if(!ticks || !DI) return;
+
+	/* Reduce the length of DI according to the bits we're transmitting */
+	DIl &= (1LL << (ticks + 1))-1;
+
+	s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE,
+				 REMOTE_JTAG_TDIDO_STR,
+				 final_tms ? REMOTE_TDITDO_TMS : REMOTE_TDITDO_NOTMS,
+				 ticks, DIl);
+	platform_buffer_write(construct,s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+		fprintf(stderr, "jtagtap_tms_seq failed, error %s\n",
+				s ? (char *)&(construct[1]) : "unknown");
+		exit(-1);
+    }
+
+	if (DO) {
+		uint64_t DOl = remotehston(-1, (char *)&construct[1]);
+		*(uint64_t *)DO = DOl;
+	}
+}
+
+static void jtagtap_tdi_seq(
+	const uint8_t final_tms, const uint8_t *DI, int ticks)
+{
+	return jtagtap_tdi_tdo_seq(NULL,  final_tms, DI, ticks);
+}
+
+
+static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE, REMOTE_JTAG_NEXT,
+				 dTMS ? '1' : '0', dTDI ? '1' : '0');
+
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+		fprintf(stderr, "jtagtap_next failed, error %s\n",
+				s ? (char *)&(construct[1]) : "unknown");
+		exit(-1);
+    }
+
+	return remotehston(-1, (char *)&construct[1]);
+}
diff --git a/src/platforms/hosted/remote_swdptap.c b/src/platforms/hosted/remote_swdptap.c
new file mode 100644
index 0000000..5b2af4d
--- /dev/null
+++ b/src/platforms/hosted/remote_swdptap.c
@@ -0,0 +1,128 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2018 Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ * Modified by Dave Marples <dave@marples.net>
+ *
+ * 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/>.
+ */
+
+/* MPSSE bit-banging SW-DP interface over FTDI with loop unrolled.
+ * Speed is sensible.
+ */
+
+#include <stdio.h>
+#include <assert.h>
+
+#include "general.h"
+#include "remote.h"
+#include "bmp_remote.h"
+
+static bool swdptap_seq_in_parity(uint32_t *res, int ticks);
+static uint32_t swdptap_seq_in(int ticks);
+static void swdptap_seq_out(uint32_t MS, int ticks);
+static void swdptap_seq_out_parity(uint32_t MS, int ticks);
+
+int remote_swdptap_init(swd_proc_t *swd_proc)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = sprintf((char *)construct,"%s", REMOTE_SWDP_INIT_STR);
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+		fprintf(stderr, "swdptap_init failed, error %s\n",
+				s ? (char *)&(construct[1]) : "unknown");
+		exit(-1);
+    }
+
+	swd_proc->swdptap_seq_in  = swdptap_seq_in;
+	swd_proc->swdptap_seq_in_parity  = swdptap_seq_in_parity;
+	swd_proc->swdptap_seq_out = swdptap_seq_out;
+	swd_proc->swdptap_seq_out_parity  = swdptap_seq_out_parity;
+
+  return 0;
+}
+
+static bool swdptap_seq_in_parity(uint32_t *res, int ticks)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = sprintf((char *)construct, REMOTE_SWDP_IN_PAR_STR, ticks);
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((s<2) || (construct[0] == REMOTE_RESP_ERR)) {
+		fprintf(stderr, "swdptap_seq_in_parity failed, error %s\n",
+				s ? (char *)&(construct[1]) : "short response");
+		exit(-1);
+	}
+
+	*res=remotehston(-1, (char *)&construct[1]);
+	return (construct[0] != REMOTE_RESP_OK);
+}
+
+static uint32_t swdptap_seq_in(int ticks)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = sprintf((char *)construct, REMOTE_SWDP_IN_STR, ticks);
+	platform_buffer_write(construct,s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((s<2) || (construct[0] == REMOTE_RESP_ERR)) {
+      fprintf(stderr, "swdptap_seq_in failed, error %s\n",
+			  s ? (char *)&(construct[1]) : "short response");
+      exit(-1);
+    }
+
+	return remotehston(-1,(char *)&construct[1]);
+}
+
+static void swdptap_seq_out(uint32_t MS, int ticks)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = sprintf((char *)construct,REMOTE_SWDP_OUT_STR, ticks, MS);
+	platform_buffer_write(construct, s);
+
+	s=platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR)) {
+      fprintf(stderr, "swdptap_seq_out failed, error %s\n",
+			  s ? (char *)&(construct[1]) : "short response");
+      exit(-1);
+    }
+}
+
+static void swdptap_seq_out_parity(uint32_t MS, int ticks)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+
+	s = sprintf((char *)construct, REMOTE_SWDP_OUT_PAR_STR, ticks, MS);
+	platform_buffer_write(construct, s);
+
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((s < 1) || (construct[1] == REMOTE_RESP_ERR)){
+      fprintf(stderr, "swdptap_seq_out_parity failed, error %s\n",
+			  s ? (char *)&(construct[2]) : "short response");
+      exit(-1);
+    }
+}
diff --git a/src/platforms/pc-hosted/Makefile.inc b/src/platforms/pc-hosted/Makefile.inc
deleted file mode 100644
index 3d10dc1..0000000
--- a/src/platforms/pc-hosted/Makefile.inc
+++ /dev/null
@@ -1,16 +0,0 @@
-TARGET=blackmagic_hosted
-SYS = $(shell $(CC) -dumpmachine)
-CFLAGS += -DENABLE_DEBUG
-CFLAGS +=-I ./target -I./platforms/pc
-ifneq (, $(findstring mingw, $(SYS)))
-SRC += 	 serial_win.c
-LDFLAGS +=  -lws2_32
-else ifneq (, $(findstring cygwin, $(SYS)))
-SRC += 	 serial_win.c
-LDFLAGS +=  -lws2_32
-else
-SRC += serial_unix.c
-endif
-VPATH += platforms/pc
-SRC += 	cl_utils.c timing.c utils.c
-PC_HOSTED = 1
diff --git a/src/platforms/pc-hosted/README.md b/src/platforms/pc-hosted/README.md
deleted file mode 100644
index cca59d1..0000000
--- a/src/platforms/pc-hosted/README.md
+++ /dev/null
@@ -1,40 +0,0 @@
-PC Hosted variant
-
-THIS IS INCOMPLETE - ONLY SUPPORTS SWD AT THE MOMENT
-
-This variant will use any BMP probe with recent firmware as a remote
-actuator, with the actual probe code running on the PC. The BMP itself
-is 'dumb' and doesn't do anything (although any secondary serial port
-remains available).
-
-To use it, compile for the pc-hosted target and then connect to your normal
-BMP GDB port;
-
-src/blackmagic -s /dev/ttyACM0
-
-...you can then connect your gdb session to localhost:2000 for all your
-debugging goodness;
-
-$arm-eabi-none-gdb
-(gdb) monitor swdp_scan
-Target voltage: not supported
-Available Targets:
-No. Att Driver
- 1      STM32F1 medium density M3/M4
-(gdb) attach 1
-Attaching to program: Builds/blackmagic/src/blackmagic, Remote target
-0x08001978 in ?? ()
-(gdb) file src/blackmagic
-A program is being debugged already.
-Are you sure you want to change the file? (y or n) y
-Load new symbol table from "src/blackmagic"? (y or n) y
-Reading symbols from src/blackmagic...
-(gdb) load
-Loading section .text, size 0x1201c lma 0x8002000
-Loading section .data, size 0xd8 lma 0x801401c
-Start address 0x800d9fc, load size 73972
-Transfer rate: 2 KB/sec, 960 bytes/write.
-(gdb)
-
-...note that the speed of the probe in this way is about 10 times less than
-running native. This build is intended for debug and development only.
\ No newline at end of file
diff --git a/src/platforms/pc-hosted/jtagtap.c b/src/platforms/pc-hosted/jtagtap.c
deleted file mode 100644
index bc5a816..0000000
--- a/src/platforms/pc-hosted/jtagtap.c
+++ /dev/null
@@ -1,161 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2008  Black Sphere Technologies Ltd.
- * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- * Modified by Dave Marples <dave@marples.net>
- *
- * 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/>.
- */
-
-/* Low level JTAG implementation using FT2232 with libftdi.
- *
- * Issues:
- * Should share interface with swdptap.c or at least clean up...
- */
-
-#include <stdio.h>
-#include <unistd.h>
-#include <string.h>
-
-#include <assert.h>
-
-#include "general.h"
-#include "remote.h"
-#include "jtagtap.h"
-
-jtag_proc_t jtag_proc;
-
-static void jtagtap_reset(void);
-static void jtagtap_tms_seq(uint32_t MS, int ticks);
-static void jtagtap_tdi_tdo_seq(
-	uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks);
-static void jtagtap_tdi_seq(
-	const uint8_t final_tms, const uint8_t *DI, int ticks);
-static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI);
-
-int platform_jtagtap_init(void)
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=snprintf((char *)construct,PLATFORM_MAX_MSG_SIZE,"%s",REMOTE_JTAG_INIT_STR);
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-  if ((!s) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"jtagtap_init failed, error %s\n",s?(char *)&(construct[1]):"unknown");
-      exit(-1);
-    }
-
-  jtag_proc.jtagtap_reset = jtagtap_reset;
-  jtag_proc.jtagtap_next =jtagtap_next;
-  jtag_proc.jtagtap_tms_seq = jtagtap_tms_seq;
-  jtag_proc.jtagtap_tdi_tdo_seq = jtagtap_tdi_tdo_seq;
-  jtag_proc.jtagtap_tdi_seq = jtagtap_tdi_seq;
-
-  return 0;
-}
-
-/* See remote.c/.h for protocol information */
-
-static void jtagtap_reset(void)
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=snprintf((char *)construct,PLATFORM_MAX_MSG_SIZE,"%s",REMOTE_JTAG_RESET_STR);
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-  if ((!s) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"jtagtap_reset failed, error %s\n",s?(char *)&(construct[1]):"unknown");
-      exit(-1);
-    }
-}
-
-static void jtagtap_tms_seq(uint32_t MS, int ticks)
-
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=snprintf((char *)construct,PLATFORM_MAX_MSG_SIZE,REMOTE_JTAG_TMS_STR,ticks,MS);
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-  if ((!s) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"jtagtap_tms_seq failed, error %s\n",s?(char *)&(construct[1]):"unknown");
-      exit(-1);
-    }
-}
-
-static void jtagtap_tdi_tdo_seq(
-	uint8_t *DO, const uint8_t final_tms, const uint8_t *DI, int ticks)
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  uint64_t DIl=*(uint64_t *)DI;
-
-  if(!ticks || !DI) return;
-
-  /* Reduce the length of DI according to the bits we're transmitting */
-  DIl &= (1LL << (ticks + 1)) - 1;
-
-  s=snprintf((char *)construct,PLATFORM_MAX_MSG_SIZE,REMOTE_JTAG_TDIDO_STR,final_tms?REMOTE_TDITDO_TMS:REMOTE_TDITDO_NOTMS,ticks,DIl);
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-  if ((!s) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"jtagtap_tms_seq failed, error %s\n",s?(char *)&(construct[1]):"unknown");
-      exit(-1);
-    }
-
-  if (DO) {
-      uint64_t DOl = remotehston(-1, (char *)&construct[1]);
-      *(uint64_t *)DO = DOl;
-  }
-}
-
-static void jtagtap_tdi_seq(
-	const uint8_t final_tms, const uint8_t *DI, int ticks)
-{
-  return jtagtap_tdi_tdo_seq(NULL,  final_tms, DI, ticks);
-}
-
-
-static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
-
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=snprintf((char *)construct,PLATFORM_MAX_MSG_SIZE,REMOTE_JTAG_NEXT,dTMS?'1':'0',dTDI?'1':'0');
-
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-  if ((!s) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"jtagtap_next failed, error %s\n",s?(char *)&(construct[1]):"unknown");
-      exit(-1);
-    }
-
-  return remotehston(-1,(char *)&construct[1]);
-}
diff --git a/src/platforms/pc-hosted/platform.c b/src/platforms/pc-hosted/platform.c
deleted file mode 100644
index 849b926..0000000
--- a/src/platforms/pc-hosted/platform.c
+++ /dev/null
@@ -1,188 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2011  Black Sphere Technologies Ltd.
- * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- * Additions by Dave Marples <dave@marples.net>
- *
- * 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 "general.h"
-#include "gdb_if.h"
-#include "version.h"
-#include "platform.h"
-#include "remote.h"
-#include "target.h"
-
-#include <assert.h>
-#include <sys/time.h>
-#include <sys/time.h>
-#include <errno.h>
-#include <string.h>
-
-#include "cl_utils.h"
-static BMP_CL_OPTIONS_t cl_opts; /* Portable way to nullify the struct*/
-
-int platform_adiv5_swdp_scan(void)
-{
-	return adiv5_swdp_scan();
-}
-
-int platform_jtag_scan(const uint8_t *lrlens)
-{
-	return jtag_scan(lrlens);
-}
-
-int platform_jtag_dp_init()
-{
-	return 0;
-}
-
-void platform_init(int argc, char **argv)
-{
-  cl_opts.opt_idstring = "Blackmagic Debug Probe Remote";
-  cl_init(&cl_opts, argc, argv);
-  char construct[PLATFORM_MAX_MSG_SIZE];
-
-  printf("\nBlack Magic Probe (" FIRMWARE_VERSION ")\n");
-  printf("Copyright (C) 2019  Black Sphere Technologies Ltd.\n");
-  printf("License GPLv3+: GNU GPL version 3 or later "
-	 "<http://gnu.org/licenses/gpl.html>\n\n");
-
-  if (serial_open(cl_opts.opt_device, cl_opts.opt_serial))
-	  exit(-1);
-  int c=snprintf(construct,PLATFORM_MAX_MSG_SIZE,"%s",REMOTE_START_STR);
-  platform_buffer_write((uint8_t *)construct,c);
-  c=platform_buffer_read((uint8_t *)construct, PLATFORM_MAX_MSG_SIZE);
-
-  if ((!c) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"Remote Start failed, error %s\n",c?(char *)&(construct[1]):"unknown");
-      exit(-1);
-    }
-
-  printf("Remote is %s\n",&construct[1]);
-  if (cl_opts.opt_mode != BMP_MODE_DEBUG) {
-	  int ret = cl_execute(&cl_opts);
-	  if (cl_opts.opt_tpwr)
-		  platform_target_set_power(0);
-	  serial_close();
-	  exit(ret);
-  } else {
-	  assert(gdb_if_init() == 0);
-  }
-}
-
-bool platform_target_get_power(void)
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=snprintf((char *)construct,PLATFORM_MAX_MSG_SIZE,"%s",REMOTE_PWR_GET_STR);
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-
-  if ((!s) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"platform_target_get_power failed, error %s\n",s?(char *)&(construct[1]):"unknown");
-      exit(-1);
-    }
-
-  return (construct[1]=='1');
-}
-
-void platform_target_set_power(bool power)
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=snprintf((char *)construct,PLATFORM_MAX_MSG_SIZE,REMOTE_PWR_SET_STR,power?'1':'0');
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-
-  if ((!s) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"platform_target_set_power failed, error %s\n",s?(char *)&(construct[1]):"unknown");
-      exit(-1);
-    }
-}
-
-void platform_srst_set_val(bool assert)
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=snprintf((char *)construct,PLATFORM_MAX_MSG_SIZE,REMOTE_SRST_SET_STR,assert?'1':'0');
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-
-  if ((!s) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"platform_srst_set_val failed, error %s\n",s?(char *)&(construct[1]):"unknown");
-      exit(-1);
-    }
-}
-
-bool platform_srst_get_val(void)
-
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=snprintf((char *)construct,PLATFORM_MAX_MSG_SIZE,"%s",REMOTE_SRST_GET_STR);
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-
-  if ((!s) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"platform_srst_set_val failed, error %s\n",s?(char *)&(construct[1]):"unknown");
-      exit(-1);
-    }
-
-  return (construct[1]=='1');
-}
-
-void platform_buffer_flush(void)
-{
-
-}
-
-const char *platform_target_voltage(void)
-
-{
-  static uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=snprintf((char *)construct,PLATFORM_MAX_MSG_SIZE,"%s",REMOTE_VOLTAGE_STR);
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-
-  if ((!s) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"platform_target_voltage failed, error %s\n",s?(char *)&(construct[1]):"unknown");
-      exit(-1);
-    }
-
-  return (char *)&construct[1];
-}
-
-void platform_adiv5_dp_defaults(void *arg)
-{
-	(void) arg;
-}
diff --git a/src/platforms/pc-hosted/platform.h b/src/platforms/pc-hosted/platform.h
deleted file mode 100644
index 696639b..0000000
--- a/src/platforms/pc-hosted/platform.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2011  Black Sphere Technologies Ltd.
- * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- * Additions by Dave Marples <dave@marples.net>
- *
- * 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/>.
- */
-
-#ifndef __PLATFORM_H
-#define __PLATFORM_H
-
-#include "timing.h"
-
-#ifndef _WIN32
-#	include <alloca.h>
-#else
-#	ifndef alloca
-#		define alloca __builtin_alloca
-#	endif
-#endif
-
-#define PLATFORM_HAS_POWER_SWITCH
-#define PLATFORM_MAX_MSG_SIZE (256)
-#define PLATFORM_IDENT() "PC-HOSTED"
-#define BOARD_IDENT PLATFORM_IDENT
-#define SET_RUN_STATE(state)
-#define SET_IDLE_STATE(state)
-#define SET_ERROR_STATE(state)
-
-/* Allow 100mS for responses to reach us */
-#define RESP_TIMEOUT (100)
-
-void platform_buffer_flush(void);
-int platform_buffer_write(const uint8_t *data, int size);
-int platform_buffer_read(uint8_t *data, int size);
-static inline int platform_hwversion(void)
-{
-  return 0;
-}
-
-#endif
diff --git a/src/platforms/pc-hosted/swdptap.c b/src/platforms/pc-hosted/swdptap.c
deleted file mode 100644
index c62a1f9..0000000
--- a/src/platforms/pc-hosted/swdptap.c
+++ /dev/null
@@ -1,133 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2018 Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
- * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- * Modified by Dave Marples <dave@marples.net>
- *
- * 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/>.
- */
-
-/* MPSSE bit-banging SW-DP interface over FTDI with loop unrolled.
- * Speed is sensible.
- */
-
-#include <stdio.h>
-#include <assert.h>
-
-#include "general.h"
-#include "swdptap.h"
-#include "remote.h"
-
-static bool swdptap_seq_in_parity(uint32_t *res, int ticks);
-static uint32_t swdptap_seq_in(int ticks);
-static void swdptap_seq_out(uint32_t MS, int ticks);
-static void swdptap_seq_out_parity(uint32_t MS, int ticks);
-
-swd_proc_t swd_proc;
-
-int platform_swdptap_init(void)
-
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=sprintf((char *)construct,"%s",REMOTE_SWDP_INIT_STR);
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-  if ((!s) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"swdptap_init failed, error %s\n",s?(char *)&(construct[1]):"unknown");
-      exit(-1);
-    }
-
-	swd_proc.swdptap_seq_in  = swdptap_seq_in;
-	swd_proc.swdptap_seq_in_parity  = swdptap_seq_in_parity;
-	swd_proc.swdptap_seq_out = swdptap_seq_out;
-	swd_proc.swdptap_seq_out_parity  = swdptap_seq_out_parity;
-
-  return 0;
-}
-
-static bool swdptap_seq_in_parity(uint32_t *res, int ticks)
-
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=sprintf((char *)construct,REMOTE_SWDP_IN_PAR_STR,ticks);
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-  if ((s<2) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"swdptap_seq_in_parity failed, error %s\n",s?(char *)&(construct[1]):"short response");
-      exit(-1);
-    }
-
-  *res=remotehston(-1,(char *)&construct[1]);
-  return (construct[0]!=REMOTE_RESP_OK);
-}
-
-static uint32_t swdptap_seq_in(int ticks)
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=sprintf((char *)construct,REMOTE_SWDP_IN_STR,ticks);
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-  if ((s<2) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"swdptap_seq_in failed, error %s\n",s?(char *)&(construct[1]):"short response");
-      exit(-1);
-    }
-
-  return remotehston(-1,(char *)&construct[1]);
-}
-
-static void swdptap_seq_out(uint32_t MS, int ticks)
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=sprintf((char *)construct,REMOTE_SWDP_OUT_STR,ticks,MS);
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-  if ((s<1) || (construct[0]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"swdptap_seq_out failed, error %s\n",s?(char *)&(construct[1]):"short response");
-      exit(-1);
-    }
-}
-
-
-static void swdptap_seq_out_parity(uint32_t MS, int ticks)
-{
-  uint8_t construct[PLATFORM_MAX_MSG_SIZE];
-  int s;
-
-  s=sprintf((char *)construct,REMOTE_SWDP_OUT_PAR_STR,ticks,MS);
-  platform_buffer_write(construct,s);
-
-  s=platform_buffer_read(construct, PLATFORM_MAX_MSG_SIZE);
-  if ((s<1) || (construct[1]==REMOTE_RESP_ERR))
-    {
-      fprintf(stderr,"swdptap_seq_out_parity failed, error %s\n",s?(char *)&(construct[2]):"short response");
-      exit(-1);
-    }
-}

From ef816e318391c64014554140be65a5cafc6c8be2 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Tue, 28 Apr 2020 16:54:56 +0200
Subject: [PATCH 30/43] hosted: add libftdi implementation, remove old
 implementation.

---
 src/platforms/hosted/Makefile.inc             |  3 +
 .../{libftdi/platform.c => hosted/ftdi_bmp.c} | 70 ++++++-------------
 .../{libftdi/platform.h => hosted/ftdi_bmp.h} | 46 ++++--------
 .../jtagtap.c => hosted/libftdi_jtagtap.c}    | 62 ++++++++--------
 .../swdptap.c => hosted/libftdi_swdptap.c}    | 38 +++++-----
 src/platforms/hosted/platform.c               | 27 ++++++-
 src/platforms/libftdi/Makefile.inc            | 13 ----
 src/platforms/libftdi/README.md               | 14 ----
 8 files changed, 111 insertions(+), 162 deletions(-)
 rename src/platforms/{libftdi/platform.c => hosted/ftdi_bmp.c} (84%)
 rename src/platforms/{libftdi/platform.h => hosted/ftdi_bmp.h} (69%)
 rename src/platforms/{libftdi/jtagtap.c => hosted/libftdi_jtagtap.c} (76%)
 rename src/platforms/{libftdi/swdptap.c => hosted/libftdi_swdptap.c} (88%)
 delete mode 100644 src/platforms/libftdi/Makefile.inc
 delete mode 100644 src/platforms/libftdi/README.md

diff --git a/src/platforms/hosted/Makefile.inc b/src/platforms/hosted/Makefile.inc
index 7fb94dc..139c9cf 100644
--- a/src/platforms/hosted/Makefile.inc
+++ b/src/platforms/hosted/Makefile.inc
@@ -2,6 +2,8 @@ SYS = $(shell $(CC) -dumpmachine)
 CFLAGS += -DENABLE_DEBUG -DPLATFORM_HAS_DEBUG
 CFLAGS +=-I ./target -I./platforms/pc
 LDFLAGS += -lusb-1.0
+CFLAGS += $(shell pkg-config --cflags libftdi1)
+LDFLAGS += $(shell pkg-config --libs libftdi1)
 ifneq (, $(findstring mingw, $(SYS)))
 SRC += 	 serial_win.c
 LDFLAGS += -lws2_32
@@ -15,4 +17,5 @@ VPATH += platforms/pc
 SRC += timing.c cl_utils.c utils.c libusb_utils.c
 SRC += stlinkv2.c
 SRC += bmp_remote.c remote_swdptap.c remote_jtagtap.c
+SRC += ftdi_bmp.c libftdi_swdptap.c libftdi_jtagtap.c
 PC_HOSTED = 1
diff --git a/src/platforms/libftdi/platform.c b/src/platforms/hosted/ftdi_bmp.c
similarity index 84%
rename from src/platforms/libftdi/platform.c
rename to src/platforms/hosted/ftdi_bmp.c
index e2ceb59..1c65e23 100644
--- a/src/platforms/libftdi/platform.c
+++ b/src/platforms/hosted/ftdi_bmp.c
@@ -27,9 +27,9 @@
 #include <unistd.h>
 #include <sys/time.h>
 
-struct ftdi_context *ftdic;
+#include "ftdi_bmp.h"
 
-#include "cl_utils.h"
+struct ftdi_context *ftdic;
 
 #define BUF_SIZE 4096
 static uint8_t outbuf[BUF_SIZE];
@@ -181,39 +181,18 @@ cable_desc_t cable_desc[] = {
 	},
 };
 
-int platform_adiv5_swdp_scan(void)
+int ftdi_bmp_init(BMP_CL_OPTIONS_t *cl_opts, bmp_info_t *info)
 {
-	return adiv5_swdp_scan();
-}
-
-int platform_jtag_scan(const uint8_t *lrlens)
-{
-	return jtag_scan(lrlens);
-}
-
-int platform_jtag_dp_init()
-{
-	return 0;
-}
-
-void platform_init(int argc, char **argv)
-{
-	BMP_CL_OPTIONS_t cl_opts = {0};
-	cl_opts.opt_idstring = "Blackmagic Debug Probe for FTDI/MPSSE";
-	cl_opts.opt_cable = "ftdi";
-	cl_init(&cl_opts, argc, argv);
-
 	int err;
 	unsigned index = 0;
-	int ret = -1;
 	for(index = 0; index < sizeof(cable_desc)/sizeof(cable_desc[0]);
 		index++)
-		 if (strcmp(cable_desc[index].name, cl_opts.opt_cable) == 0)
+		 if (strcmp(cable_desc[index].name, cl_opts->opt_cable) == 0)
 		 break;
 
 	if (index == sizeof(cable_desc)/sizeof(cable_desc[0])){
-		fprintf(stderr, "No cable matching %s found\n", cl_opts.opt_cable);
-		exit(-1);
+		fprintf(stderr, "No cable matching %s found\n", cl_opts->opt_cable);
+		return -1;
 	}
 
 	active_cable = &cable_desc[index];
@@ -233,6 +212,7 @@ void platform_init(int argc, char **argv)
 			ftdi_get_error_string(ftdic));
 		abort();
 	}
+	info->ftdic = ftdic;
 	if((err = ftdi_set_interface(ftdic, active_cable->interface)) != 0) {
 		fprintf(stderr, "ftdi_set_interface: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
@@ -240,7 +220,7 @@ void platform_init(int argc, char **argv)
 	}
 	if((err = ftdi_usb_open_desc(
 		ftdic, active_cable->vendor, active_cable->product,
-		active_cable->description, cl_opts.opt_serial)) != 0) {
+		active_cable->description, cl_opts->opt_serial)) != 0) {
 		fprintf(stderr, "unable to open ftdi device: %d (%s)\n",
 			err, ftdi_get_error_string(ftdic));
 		goto error_1;
@@ -261,57 +241,47 @@ void platform_init(int argc, char **argv)
 			err, ftdi_get_error_string(ftdic));
 		goto error_2;
 	}
-	if (cl_opts.opt_mode != BMP_MODE_DEBUG) {
-		ret = cl_execute(&cl_opts);
-	} else {
-		assert(gdb_if_init() == 0);
-		return;
-	}
+	return 0;
   error_2:
 	ftdi_usb_close(ftdic);
   error_1:
 	ftdi_free(ftdic);
-	exit(ret);
+	return -1;
 }
 
-void platform_srst_set_val(bool assert)
+void libftdi_srst_set_val(bool assert)
 {
 	(void)assert;
-	platform_buffer_flush();
+	libftdi_buffer_flush();
 }
 
-bool platform_srst_get_val(void) { return false; }
+bool libftdi_srst_get_val(void) { return false; }
 
-void platform_buffer_flush(void)
+void libftdi_buffer_flush(void)
 {
 	assert(ftdi_write_data(ftdic, outbuf, bufptr) == bufptr);
-//	printf("FT2232 platform_buffer flush: %d bytes\n", bufptr);
+//	printf("FT2232 libftdi_buffer flush: %d bytes\n", bufptr);
 	bufptr = 0;
 }
 
-int platform_buffer_write(const uint8_t *data, int size)
+int libftdi_buffer_write(const uint8_t *data, int size)
 {
-	if((bufptr + size) / BUF_SIZE > 0) platform_buffer_flush();
+	if((bufptr + size) / BUF_SIZE > 0) libftdi_buffer_flush();
 	memcpy(outbuf + bufptr, data, size);
 	bufptr += size;
 	return size;
 }
 
-int platform_buffer_read(uint8_t *data, int size)
+int libftdi_buffer_read(uint8_t *data, int size)
 {
 	int index = 0;
 	outbuf[bufptr++] = SEND_IMMEDIATE;
-	platform_buffer_flush();
+	libftdi_buffer_flush();
 	while((index += ftdi_read_data(ftdic, data + index, size-index)) != size);
 	return size;
 }
 
-const char *platform_target_voltage(void)
+const char *libftdi_target_voltage(void)
 {
 	return "not supported";
 }
-
-void platform_adiv5_dp_defaults(void *arg)
-{
-	(void) arg;
-}
diff --git a/src/platforms/libftdi/platform.h b/src/platforms/hosted/ftdi_bmp.h
similarity index 69%
rename from src/platforms/libftdi/platform.h
rename to src/platforms/hosted/ftdi_bmp.h
index d4368ba..c44607b 100644
--- a/src/platforms/libftdi/platform.h
+++ b/src/platforms/hosted/ftdi_bmp.h
@@ -18,34 +18,12 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef __PLATFORM_H
-#define __PLATFORM_H
+#ifndef __FTDI_BMP_H
+#define __FTDI_BMP_H
 
-#include <libftdi1/ftdi.h>
-
-#include "timing.h"
-
-#ifndef _WIN32
-#	include <alloca.h>
-#else
-#	ifndef alloca
-#		define alloca __builtin_alloca
-#	endif
-#endif
-
-#define FT2232_VID	0x0403
-#define FT2232_PID	0x6010
-
-#define PLATFORM_IDENT() "FTDI/MPSSE"
-#define SET_RUN_STATE(state)
-#define SET_IDLE_STATE(state)
-#define SET_ERROR_STATE(state)
-
-extern struct ftdi_context *ftdic;
-
-void platform_buffer_flush(void);
-int platform_buffer_write(const uint8_t *data, int size);
-int platform_buffer_read(uint8_t *data, int size);
+#include "cl_utils.h"
+#include "swdptap.h"
+#include "jtagtap.h"
 
 typedef struct cable_desc_s {
 	int vendor;
@@ -65,15 +43,19 @@ typedef struct cable_desc_s {
 }cable_desc_t;
 
 extern cable_desc_t *active_cable;
+extern struct ftdi_context *ftdic;
 
-static inline int platform_hwversion(void)
-{
-	        return 0;
-}
+int ftdi_bmp_init(BMP_CL_OPTIONS_t *cl_opts, bmp_info_t *info);
+
+int libftdi_swdptap_init(swd_proc_t *swd_proc);
+int libftdi_jtagtap_init(jtag_proc_t *jtag_proc);
+void libftdi_buffer_flush(void);
+int libftdi_buffer_write(const uint8_t *data, int size);
+int libftdi_buffer_read(uint8_t *data, int size);
+const char *libftdi_target_voltage(void);
 
 #define MPSSE_TDI 2
 #define MPSSE_TDO 4
 #define MPSSE_TMS 8
 
 #endif
-
diff --git a/src/platforms/libftdi/jtagtap.c b/src/platforms/hosted/libftdi_jtagtap.c
similarity index 76%
rename from src/platforms/libftdi/jtagtap.c
rename to src/platforms/hosted/libftdi_jtagtap.c
index a928d19..1724d3f 100644
--- a/src/platforms/libftdi/jtagtap.c
+++ b/src/platforms/hosted/libftdi_jtagtap.c
@@ -31,9 +31,10 @@
 #include <assert.h>
 
 #include "general.h"
-#include "jtagtap.h"
+#include "ftdi_bmp.h"
 
-jtag_proc_t jtag_proc;
+extern cable_desc_t *active_cable;
+extern struct ftdi_context *ftdic;
 
 static void jtagtap_reset(void);
 static void jtagtap_tms_seq(uint32_t MS, int ticks);
@@ -43,7 +44,7 @@ static void jtagtap_tdi_seq(
 	const uint8_t final_tms, const uint8_t *DI, int ticks);
 static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI);
 
-int platform_jtagtap_init(void)
+int libftdi_jtagtap_init(jtag_proc_t *jtag_proc)
 {
 	assert(ftdic != NULL);
 	int err = ftdi_usb_purge_buffers(ftdic);
@@ -57,14 +58,14 @@ int platform_jtagtap_init(void)
 	if (err != 0) {
 		fprintf(stderr, "ftdi_set_bitmode: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
-		return -1;;
+		return -1;
 	}
 	/* Enable MPSSE controller. Pin directions are set later.*/
 	err = ftdi_set_bitmode(ftdic, 0, BITMODE_MPSSE);
 	if (err != 0) {
 		fprintf(stderr, "ftdi_set_bitmode: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
-		return -1;;
+		return -1;
 	}
 	uint8_t ftdi_init[9] = {TCK_DIVISOR, 0x00, 0x00, SET_BITS_LOW, 0,0,
 				SET_BITS_HIGH, 0,0};
@@ -72,37 +73,32 @@ int platform_jtagtap_init(void)
 	ftdi_init[5]= active_cable->dbus_ddr;
 	ftdi_init[7]= active_cable->cbus_data;
 	ftdi_init[8]= active_cable->cbus_ddr;
-	platform_buffer_write(ftdi_init, 9);
-	platform_buffer_flush();
+	libftdi_buffer_write(ftdi_init, 9);
+	libftdi_buffer_flush();
 
-	jtag_proc.jtagtap_reset = jtagtap_reset;
-	jtag_proc.jtagtap_next =jtagtap_next;
-	jtag_proc.jtagtap_tms_seq = jtagtap_tms_seq;
-	jtag_proc.jtagtap_tdi_tdo_seq = jtagtap_tdi_tdo_seq;
-	jtag_proc.jtagtap_tdi_seq = jtagtap_tdi_seq;
-
-	/* Go to JTAG mode for SWJ-DP */
-	for (int i = 0; i <= 50; i++)
-		jtagtap_next(1, 0);		/* Reset SW-DP */
-	jtagtap_tms_seq(0xE73C, 16);		/* SWD to JTAG sequence */
-	jtagtap_soft_reset();
+	jtag_proc->jtagtap_reset = jtagtap_reset;
+	jtag_proc->jtagtap_next =jtagtap_next;
+	jtag_proc->jtagtap_tms_seq = jtagtap_tms_seq;
+	jtag_proc->jtagtap_tdi_tdo_seq = jtagtap_tdi_tdo_seq;
+	jtag_proc->jtagtap_tdi_seq = jtagtap_tdi_seq;
 
 	return 0;
 }
 
-void jtagtap_reset(void)
+static void jtagtap_reset(void)
 {
 	jtagtap_soft_reset();
 }
 
 static void jtagtap_tms_seq(uint32_t MS, int ticks)
 {
-	uint8_t tmp[3] = {MPSSE_WRITE_TMS | MPSSE_LSB | MPSSE_BITMODE| MPSSE_READ_NEG, 0, 0};
+	uint8_t tmp[3] = {
+		MPSSE_WRITE_TMS | MPSSE_LSB | MPSSE_BITMODE| MPSSE_READ_NEG, 0, 0};
 	while(ticks >= 0) {
 		tmp[1] = ticks<7?ticks-1:6;
 		tmp[2] = 0x80 | (MS & 0x7F);
 
-		platform_buffer_write(tmp, 3);
+		libftdi_buffer_write(tmp, 3);
 		MS >>= 7; ticks -= 7;
 	}
 }
@@ -120,15 +116,16 @@ static void jtagtap_tdi_tdo_seq(
 	rticks = ticks & 7;
 	ticks >>= 3;
 	uint8_t data[3];
-	uint8_t cmd =  ((DO)? MPSSE_DO_READ : 0) | ((DI)? (MPSSE_DO_WRITE | MPSSE_WRITE_NEG) : 0) | MPSSE_LSB;
+	uint8_t cmd =  ((DO)? MPSSE_DO_READ : 0) |
+		((DI)? (MPSSE_DO_WRITE | MPSSE_WRITE_NEG) : 0) | MPSSE_LSB;
 	rsize = ticks;
 	if(ticks) {
 		data[0] = cmd;
 		data[1] = ticks - 1;
 		data[2] = 0;
-		platform_buffer_write(data, 3);
+		libftdi_buffer_write(data, 3);
 		if (DI)
-			platform_buffer_write(DI, ticks);
+			libftdi_buffer_write(DI, ticks);
 	}
 	if(rticks) {
 		int index = 0;
@@ -137,21 +134,22 @@ static void jtagtap_tdi_tdo_seq(
 		data[index++] = rticks - 1;
 		if (DI)
 			data[index++] = DI[ticks];
-		platform_buffer_write(data, index);
+		libftdi_buffer_write(data, index);
 	}
 	if(final_tms) {
 		int index = 0;
 		rsize++;
-		data[index++] = MPSSE_WRITE_TMS | ((DO)? MPSSE_DO_READ : 0) | MPSSE_LSB | MPSSE_BITMODE | MPSSE_WRITE_NEG;
+		data[index++] = MPSSE_WRITE_TMS | ((DO)? MPSSE_DO_READ : 0) |
+			MPSSE_LSB | MPSSE_BITMODE | MPSSE_WRITE_NEG;
 		data[index++] = 0;
 		if (DI)
 			data[index++] = (DI[ticks]) >> rticks?0x81 : 0x01;
-		platform_buffer_write(data, index);
+		libftdi_buffer_write(data, index);
 	}
 	if (DO) {
 		int index = 0;
 		uint8_t *tmp = alloca(ticks);
-		platform_buffer_read(tmp, rsize);
+		libftdi_buffer_read(tmp, rsize);
 		if(final_tms) rsize--;
 
 		while(rsize--) {
@@ -181,10 +179,11 @@ static void jtagtap_tdi_seq(
 static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 {
 	uint8_t ret;
-	uint8_t tmp[3] = {MPSSE_WRITE_TMS | MPSSE_DO_READ | MPSSE_LSB | MPSSE_BITMODE | MPSSE_WRITE_NEG, 0, 0};
+	uint8_t tmp[3] = {MPSSE_WRITE_TMS | MPSSE_DO_READ | MPSSE_LSB |
+					  MPSSE_BITMODE | MPSSE_WRITE_NEG, 0, 0};
 	tmp[2] = (dTDI?0x80:0) | (dTMS?0x01:0);
-	platform_buffer_write(tmp, 3);
-	platform_buffer_read(&ret, 1);
+	libftdi_buffer_write(tmp, 3);
+	libftdi_buffer_read(&ret, 1);
 
 	ret &= 0x80;
 
@@ -192,4 +191,3 @@ static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 
 	return ret;
 }
-
diff --git a/src/platforms/libftdi/swdptap.c b/src/platforms/hosted/libftdi_swdptap.c
similarity index 88%
rename from src/platforms/libftdi/swdptap.c
rename to src/platforms/hosted/libftdi_swdptap.c
index d710d45..0798901 100644
--- a/src/platforms/libftdi/swdptap.c
+++ b/src/platforms/hosted/libftdi_swdptap.c
@@ -26,7 +26,7 @@
 #include <assert.h>
 
 #include "general.h"
-#include "swdptap.h"
+#include "ftdi_bmp.h"
 
 static uint8_t olddir = 0;
 
@@ -40,9 +40,7 @@ static uint32_t swdptap_seq_in(int ticks);
 static void swdptap_seq_out(uint32_t MS, int ticks);
 static void swdptap_seq_out_parity(uint32_t MS, int ticks);
 
-swd_proc_t swd_proc;
-
-int platform_swdptap_init(void)
+int libftdi_swdptap_init(swd_proc_t *swd_proc)
 {
 	if (!active_cable->bitbang_tms_in_pin) {
 		DEBUG("SWD not possible or missing item in cable description.\n");
@@ -52,21 +50,21 @@ int platform_swdptap_init(void)
 	if (err != 0) {
 		fprintf(stderr, "ftdi_usb_purge_buffer: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
-		abort();
+		return -1;
 	}
 	/* Reset MPSSE controller. */
 	err = ftdi_set_bitmode(ftdic, 0,  BITMODE_RESET);
 	if (err != 0) {
 		fprintf(stderr, "ftdi_set_bitmode: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
-		return -1;;
+		return -1;
 	}
 	/* Enable MPSSE controller. Pin directions are set later.*/
 	err = ftdi_set_bitmode(ftdic, 0, BITMODE_MPSSE);
 	if (err != 0) {
 		fprintf(stderr, "ftdi_set_bitmode: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
-		return -1;;
+		return -1;
 	}
 	uint8_t ftdi_init[9] = {TCK_DIVISOR, 0x01, 0x00, SET_BITS_LOW, 0,0,
 				SET_BITS_HIGH, 0,0};
@@ -74,13 +72,13 @@ int platform_swdptap_init(void)
 	ftdi_init[5]= active_cable->dbus_ddr   & ~MPSSE_TD_MASK;
 	ftdi_init[7]= active_cable->cbus_data;
 	ftdi_init[8]= active_cable->cbus_ddr;
-	platform_buffer_write(ftdi_init, 9);
-	platform_buffer_flush();
+	libftdi_buffer_write(ftdi_init, 9);
+	libftdi_buffer_flush();
 
-	swd_proc.swdptap_seq_in  = swdptap_seq_in;
-	swd_proc.swdptap_seq_in_parity  = swdptap_seq_in_parity;
-	swd_proc.swdptap_seq_out = swdptap_seq_out;
-	swd_proc.swdptap_seq_out_parity  = swdptap_seq_out_parity;
+	swd_proc->swdptap_seq_in  = swdptap_seq_in;
+	swd_proc->swdptap_seq_in_parity  = swdptap_seq_in_parity;
+	swd_proc->swdptap_seq_out = swdptap_seq_out;
+	swd_proc->swdptap_seq_out_parity  = swdptap_seq_out_parity;
 
 	return 0;
 }
@@ -111,7 +109,7 @@ static void swdptap_turnaround(uint8_t dir)
 		cmd[index++] = active_cable->dbus_data |  MPSSE_MASK;
 		cmd[index++] = active_cable->dbus_ddr  & ~MPSSE_TD_MASK;
 	}
-	platform_buffer_write(cmd, index);
+	libftdi_buffer_write(cmd, index);
 }
 
 static bool swdptap_seq_in_parity(uint32_t *res, int ticks)
@@ -126,11 +124,11 @@ static bool swdptap_seq_in_parity(uint32_t *res, int ticks)
 	cmd[3] = 0;
 	swdptap_turnaround(1);
 	while (index--) {
-		platform_buffer_write(cmd, 4);
+		libftdi_buffer_write(cmd, 4);
 	}
 	uint8_t data[33];
 	unsigned int ret = 0;
-	platform_buffer_read(data, ticks + 1);
+	libftdi_buffer_read(data, ticks + 1);
 	if (data[ticks] & active_cable->bitbang_tms_in_pin)
 		parity ^= 1;
 	while (ticks--) {
@@ -155,11 +153,11 @@ static uint32_t swdptap_seq_in(int ticks)
 
 	swdptap_turnaround(1);
 	while (index--) {
-		platform_buffer_write(cmd, 4);
+		libftdi_buffer_write(cmd, 4);
 	}
 	uint8_t data[32];
 	uint32_t ret = 0;
-	platform_buffer_read(data, ticks);
+	libftdi_buffer_read(data, ticks);
 	while (ticks--) {
 		if (data[ticks] & active_cable->bitbang_tms_in_pin)
 			ret |= (1 << ticks);
@@ -185,7 +183,7 @@ static void swdptap_seq_out(uint32_t MS, int ticks)
 			ticks = 0;
 		}
 	}
-	platform_buffer_write(cmd, index);
+	libftdi_buffer_write(cmd, index);
 }
 
 static void swdptap_seq_out_parity(uint32_t MS, int ticks)
@@ -216,5 +214,5 @@ static void swdptap_seq_out_parity(uint32_t MS, int ticks)
 	cmd[index++] = MPSSE_TMS_SHIFT;
 	cmd[index++] = 0;
 	cmd[index++] = parity;
-	platform_buffer_write(cmd, index);
+	libftdi_buffer_write(cmd, index);
 }
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 55f8a52..c061e73 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -34,6 +34,7 @@
 
 #include "bmp_remote.h"
 #include "stlinkv2.h"
+#include "ftdi_bmp.h"
 
 #define VENDOR_ID_BMP            0x1d50
 #define PRODUCT_ID_BMP           0x6018
@@ -215,6 +216,13 @@ void platform_init(int argc, char **argv)
 	}
 	if (cl_opts.opt_device) {
 		info.bmp_type = BMP_TYPE_BMP;
+	} else if (cl_opts.opt_cable) {
+		/* check for libftdi devices*/
+		res = ftdi_bmp_init(&cl_opts, &info);
+		if (res)
+			exit(-1);
+		else
+			info.bmp_type = BMP_TYPE_LIBFTDI;
 	} else if (find_debuggers(&cl_opts, &info)) {
 		exit(-1);
 	}
@@ -231,6 +239,8 @@ void platform_init(int argc, char **argv)
 		if (stlink_init( &info))
 			exit(-1);
 		break;
+	case BMP_TYPE_LIBFTDI:
+		break;
 	default:
 		exit(-1);
 	}
@@ -248,6 +258,9 @@ int platform_adiv5_swdp_scan(void)
 {
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
+	case BMP_TYPE_LIBFTDI:
+		return adiv5_swdp_scan();
+		break;
 	case BMP_TYPE_STLINKV2:
 	{
 		target_list_free();
@@ -274,6 +287,8 @@ int platform_swdptap_init(void)
 	case BMP_TYPE_STLINKV2:
 		return 0;
 		break;
+	case BMP_TYPE_LIBFTDI:
+		return libftdi_swdptap_init(&swd_proc);
 	default:
 		return -1;
 	}
@@ -284,6 +299,8 @@ int platform_jtag_scan(const uint8_t *lrlens)
 {
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
+	case BMP_TYPE_LIBFTDI:
+		return jtag_scan(lrlens);
 	case BMP_TYPE_STLINKV2:
 		return jtag_scan_stlinkv2(&info, lrlens);
 	default:
@@ -299,6 +316,8 @@ int platform_jtagtap_init(void)
 		return remote_jtagtap_init(&jtag_proc);
 	case BMP_TYPE_STLINKV2:
 		return 0;
+	case BMP_TYPE_LIBFTDI:
+		return libftdi_jtagtap_init(&jtag_proc);
 	default:
 		return -1;
 	}
@@ -319,6 +338,8 @@ int platform_jtag_dp_init(ADIv5_DP_t *dp)
 {
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
+	case BMP_TYPE_LIBFTDI:
+		return 0;
 	case BMP_TYPE_STLINKV2:
 		return stlink_jtag_dp_init(dp);
 	default:
@@ -336,7 +357,7 @@ char *platform_ident(void)
 		return "BMP";
 	  case BMP_TYPE_STLINKV2:
 		return "STLINKV2";
-	case BMP_TYPE_LIBFTDI:
+	  case BMP_TYPE_LIBFTDI:
 		return "LIBFTDI";
 	  case BMP_TYPE_CMSIS_DAP:
 		return "CMSIS_DAP";
@@ -353,6 +374,8 @@ const char *platform_target_voltage(void)
 		return remote_target_voltage();
 	case BMP_TYPE_STLINKV2:
 		return stlink_target_voltage(&info);
+	case BMP_TYPE_LIBFTDI:
+		return libftdi_target_voltage();
 	default:
 		break;
 	}
@@ -387,6 +410,8 @@ bool platform_srst_get_val(void)
 void platform_buffer_flush(void)
 {
 	switch (info.bmp_type) {
+	case BMP_TYPE_LIBFTDI:
+		return libftdi_buffer_flush();
 	default:
 		break;
 	}
diff --git a/src/platforms/libftdi/Makefile.inc b/src/platforms/libftdi/Makefile.inc
deleted file mode 100644
index fab3b06..0000000
--- a/src/platforms/libftdi/Makefile.inc
+++ /dev/null
@@ -1,13 +0,0 @@
-SYS = $(shell $(CC) -dumpmachine)
-CFLAGS += -DENABLE_DEBUG
-CFLAGS += $(shell pkg-config --cflags libftdi1)
-LDFLAGS += $(shell pkg-config --libs libftdi1)
-ifneq (, $(findstring mingw, $(SYS)))
-LDFLAGS +=  -lusb-1.0 -lws2_32
-else ifneq (, $(findstring cygwin, $(SYS)))
-LDFLAGS +=  -lusb-1.0 -lws2_32
-endif
-VPATH += platforms/pc
-SRC += 	timing.c cl_utils.c utils.c
-CFLAGS +=-I ./target -I./platforms/pc
-PC_HOSTED = 1
\ No newline at end of file
diff --git a/src/platforms/libftdi/README.md b/src/platforms/libftdi/README.md
deleted file mode 100644
index 51676ca..0000000
--- a/src/platforms/libftdi/README.md
+++ /dev/null
@@ -1,14 +0,0 @@
-Compiling on windows
-
-You can crosscompile blackmagic for windows with mingw or on windows
-with cygwin. For compilation, headers for libftdi1 and libusb-1.0 are
-needed. For running, libftdi1.dll and libusb-1.0.dll are needed and
-the executable must be able to find them. Mingw on cygwin does not provide
-a libftdi package yet.
-
-To prepare libusb access to the ftdi device, run zadig https://zadig.akeo.ie/.
-Choose WinUSB(libusb-1.0) for the BMP Ftdi device.
-
-Running cygwin/blackmagic in a cygwin console, the program does not react
-on ^C. In another console, run "ps ax" to find the WINPID of the process
-and then "taskkill /F ?PID (WINPID)".

From da45281696b05089d694e6e9c7734968b9496865 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Mon, 20 Apr 2020 18:00:31 +0200
Subject: [PATCH 31/43] GetTargetVoltage: Print info about voltage only when
 info is relevant.

---
 src/command.c                           | 6 ++++--
 src/platforms/f4discovery/platform.c    | 2 +-
 src/platforms/hydrabus/platform.c       | 2 +-
 src/platforms/launchpad-icdi/platform.c | 2 +-
 src/platforms/pc/cl_utils.c             | 3 ++-
 src/platforms/swlink/platform.c         | 2 +-
 6 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/src/command.c b/src/command.c
index 9f9878d..fd7e0b4 100644
--- a/src/command.c
+++ b/src/command.c
@@ -171,7 +171,8 @@ static bool cmd_jtag_scan(target *t, int argc, char **argv)
 	(void)t;
 	uint8_t irlens[argc];
 
-	gdb_outf("Target voltage: %s\n", platform_target_voltage());
+	if (platform_target_voltage())
+		gdb_outf("Target voltage: %s\n", platform_target_voltage());
 
 	if (argc > 1) {
 		/* Accept a list of IR lengths on command line */
@@ -216,7 +217,8 @@ bool cmd_swdp_scan(target *t, int argc, char **argv)
 	(void)t;
 	(void)argc;
 	(void)argv;
-	gdb_outf("Target voltage: %s\n", platform_target_voltage());
+	if (platform_target_voltage())
+		gdb_outf("Target voltage: %s\n", platform_target_voltage());
 
 	if(connect_assert_srst)
 		platform_srst_set_val(true); /* will be deasserted after attach */
diff --git a/src/platforms/f4discovery/platform.c b/src/platforms/f4discovery/platform.c
index 875c157..7129ea1 100644
--- a/src/platforms/f4discovery/platform.c
+++ b/src/platforms/f4discovery/platform.c
@@ -100,7 +100,7 @@ bool platform_srst_get_val(void) { return false; }
 
 const char *platform_target_voltage(void)
 {
-	return "ABSENT!";
+	return NULL;
 }
 
 void platform_request_boot(void)
diff --git a/src/platforms/hydrabus/platform.c b/src/platforms/hydrabus/platform.c
index e0893e8..1b02976 100644
--- a/src/platforms/hydrabus/platform.c
+++ b/src/platforms/hydrabus/platform.c
@@ -85,7 +85,7 @@ bool platform_srst_get_val(void) { return false; }
 
 const char *platform_target_voltage(void)
 {
-	return "ABSENT!";
+	return NULL;
 }
 
 void platform_request_boot(void)
diff --git a/src/platforms/launchpad-icdi/platform.c b/src/platforms/launchpad-icdi/platform.c
index 70bb72a..d794372 100644
--- a/src/platforms/launchpad-icdi/platform.c
+++ b/src/platforms/launchpad-icdi/platform.c
@@ -115,7 +115,7 @@ void platform_delay(uint32_t ms)
 
 const char *platform_target_voltage(void)
 {
-	return "not supported";
+	return NULL;
 }
 
 char *serialno_read(char *s)
diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index 6fd4fe9..8719fe3 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -272,7 +272,8 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 	platform_srst_set_val(opt->opt_connect_under_reset);
 	if (opt->opt_mode == BMP_MODE_TEST)
 		printf("Running in Test Mode\n");
-	printf("Target voltage: %s Volt\n", platform_target_voltage());
+	if (platform_target_voltage())
+		printf("Target voltage: %s Volt\n", platform_target_voltage());
 	if (opt->opt_usejtag) {
 		num_targets = platform_jtag_scan(NULL);
 	} else {
diff --git a/src/platforms/swlink/platform.c b/src/platforms/swlink/platform.c
index c6d7588..18efbf3 100644
--- a/src/platforms/swlink/platform.c
+++ b/src/platforms/swlink/platform.c
@@ -177,7 +177,7 @@ const char *platform_target_voltage(void)
 		ret[2] = '0' + val_in_100mV % 10;
 		return ret;
 	}
-	return "ABSENT!";
+	return NULL;
 }
 
 void set_idle_state(int state)

From efce2ae892d0a1016d2d57cc5c9b33c54cb8b473 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sun, 19 Apr 2020 17:36:44 +0200
Subject: [PATCH 32/43] Do not compile jtagtap and swdptap when not needed.

---
 src/Makefile | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/Makefile b/src/Makefile
index 35ec8f2..ecb6c19 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -33,7 +33,6 @@ SRC =			\
 	hex_utils.c	\
 	jtag_devs.c	\
 	jtag_scan.c	\
-	jtagtap.c	\
 	lmi.c		\
 	lpc_common.c	\
 	lpc11xx.c	\
@@ -57,7 +56,6 @@ SRC =			\
 	stm32h7.c	\
 	stm32l0.c	\
 	stm32l4.c	\
-	swdptap.c	\
 	target.c	\
 
 include $(PLATFORM_DIR)/Makefile.inc
@@ -81,6 +79,7 @@ endif
 ifdef PC_HOSTED
 CFLAGS += -DPC_HOSTED=1
 else
+SRC += swdptap.c jtagtap.c
 CFLAGS += -DPC_HOSTED=0
 VPATH += platforms/common
 CFLAGS += -Iplatforms/common

From 4ba6afed28d7602b158254f7778f268f08f790e9 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 25 Apr 2020 14:51:55 +0200
Subject: [PATCH 33/43] Add jlink pc-hosted platform.

---
 src/platforms/hosted/Makefile.inc       |  26 +-
 src/platforms/hosted/jlink.c            | 239 +++++++++++++++
 src/platforms/hosted/jlink.h            |  53 ++++
 src/platforms/hosted/jlink_adiv5_swdp.c | 372 ++++++++++++++++++++++++
 src/platforms/hosted/jlink_jtagtap.c    | 176 +++++++++++
 src/platforms/hosted/platform.c         |  27 +-
 src/platforms/hosted/stlinkv2.c         |  10 +-
 7 files changed, 889 insertions(+), 14 deletions(-)
 create mode 100644 src/platforms/hosted/jlink.c
 create mode 100644 src/platforms/hosted/jlink.h
 create mode 100644 src/platforms/hosted/jlink_adiv5_swdp.c
 create mode 100644 src/platforms/hosted/jlink_jtagtap.c

diff --git a/src/platforms/hosted/Makefile.inc b/src/platforms/hosted/Makefile.inc
index 139c9cf..656f450 100644
--- a/src/platforms/hosted/Makefile.inc
+++ b/src/platforms/hosted/Makefile.inc
@@ -1,21 +1,35 @@
 SYS = $(shell $(CC) -dumpmachine)
 CFLAGS += -DENABLE_DEBUG -DPLATFORM_HAS_DEBUG
 CFLAGS +=-I ./target -I./platforms/pc
-LDFLAGS += -lusb-1.0
-CFLAGS += $(shell pkg-config --cflags libftdi1)
-LDFLAGS += $(shell pkg-config --libs libftdi1)
-ifneq (, $(findstring mingw, $(SYS)))
+
+ifneq (, $(findstring linux, $(SYS)))
+SRC += serial_unix.c
+CFLAGS += -fsanitize=address
+LDFLAGS += -lasan
+else ifneq (, $(findstring mingw, $(SYS)))
 SRC += 	 serial_win.c
 LDFLAGS += -lws2_32
+LDFLAGS += -lhid -lsetupapi
 else ifneq (, $(findstring cygwin, $(SYS)))
 SRC += 	 serial_win.c
 LDFLAGS += -lws2_32
-else
-SRC += serial_unix.c
+LDFLAGS += -lhid -lsetupapi
+#https://github.com/dmlc/xgboost/issues/1945 indicates macosx as indicator
+else ifneq (filter, macosx darwin, $(SYS)))
+LDFLAGS += -lhid -lsetupapi
+LDFLAGS += hidapi/mac/.libs/libhidapi.a -framework IOKit
+LDFLAGS += -framework CoreFoundation hidapi/mac/.libs/libhidapi.a
+CFLAGS += -Ihidapi/hidapi
 endif
+
+LDFLAGS += -lusb-1.0
+CFLAGS += $(shell pkg-config --cflags libftdi1)
+LDFLAGS += $(shell pkg-config --libs libftdi1)
+
 VPATH += platforms/pc
 SRC += timing.c cl_utils.c utils.c libusb_utils.c
 SRC += stlinkv2.c
 SRC += bmp_remote.c remote_swdptap.c remote_jtagtap.c
 SRC += ftdi_bmp.c libftdi_swdptap.c libftdi_jtagtap.c
+SRC += jlink.c jlink_adiv5_swdp.c jlink_jtagtap.c
 PC_HOSTED = 1
diff --git a/src/platforms/hosted/jlink.c b/src/platforms/hosted/jlink.c
new file mode 100644
index 0000000..d5453e0
--- /dev/null
+++ b/src/platforms/hosted/jlink.c
@@ -0,0 +1,239 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2020 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/>.
+ */
+/* Base on code from git://repo.or.cz/libjaylink.git
+ * and https://github.com/afaerber/jlink.git*/
+
+#include "general.h"
+#include "gdb_if.h"
+#include "adiv5.h"
+#include "exception.h"
+
+#include <assert.h>
+#include <unistd.h>
+#include <signal.h>
+#include <ctype.h>
+#include <sys/time.h>
+
+#include "cl_utils.h"
+#include "jlink.h"
+
+#define USB_PID_SEGGER       0x1366
+
+/* Only two devices PIDS tested so long */
+#define USB_VID_SEGGER_0101  0x0101
+#define USB_VID_SEGGER_0105  0x0105
+
+static void jlink_print_caps(bmp_info_t *info)
+{
+	uint8_t cmd[1] = {CMD_GET_CAPS};
+	uint8_t res[4];
+	send_recv(info->usb_link, cmd, 1, res, sizeof(res));
+	uint32_t caps = res[0] | (res[1] << 8) | (res[2] << 16) | (res[3] << 24);
+	printf("Caps %" PRIx32 "\n", caps);
+	if (caps & JLINK_CAP_GET_HW_VERSION) {
+		uint8_t cmd[1] = {CMD_GET_HW_VERSION};
+		send_recv(info->usb_link, cmd, 1, NULL, 0);
+		send_recv(info->usb_link, NULL, 0, res, sizeof(res));
+		printf("HW: Type %d, Major %d, Minor %d, Rev %d\n",
+			   res[3], res[2], res[1], res[0]);
+	}
+}
+static void jlink_print_speed(bmp_info_t *info)
+{
+	uint8_t cmd[1] = {CMD_GET_SPEED};
+	uint8_t res[6];
+	send_recv(info->usb_link, cmd, 1, res, sizeof(res));
+	uint32_t speed = res[0] | (res[1] << 8) | (res[2] << 16) | (res[3] << 24);
+	double freq_mhz = speed / 1000000.0;
+	uint16_t divisor = res[4] | (res[5] << 8);
+	printf("Emulator speed %3.1f MHz, Mindiv %d\n", freq_mhz, divisor);
+}
+
+static void jlink_print_version(bmp_info_t *info)
+{
+	uint8_t cmd[1] = {CMD_GET_VERSION};
+	uint8_t len_str[2];
+	send_recv(info->usb_link, cmd, 1, len_str, sizeof(len_str));
+	uint8_t version[0x70];
+	send_recv(info->usb_link, NULL, 0, version, sizeof(version));
+	printf("%s\n", version );
+}
+
+static void jlink_print_interfaces(bmp_info_t *info)
+{
+	uint8_t cmd[2] = {CMD_GET_SELECT_IF, JLINK_IF_GET_ACTIVE};
+	uint8_t res[4];
+	send_recv(info->usb_link, cmd, 2, res, sizeof(res));
+	cmd[1] = JLINK_IF_GET_AVAILABLE;
+	uint8_t res1[4];
+	send_recv(info->usb_link, cmd, 2, res1, sizeof(res1));
+	printf("%s active", (res[0] == SELECT_IF_SWD) ? "SWD":
+		   (res[0] == SELECT_IF_JTAG) ? "JTAG" : "NONE");
+	uint8_t other_interface = res1[0] - (res[0] + 1);
+	if (other_interface)
+		printf(", %s available",
+			   (other_interface == JLINK_IF_SWD) ? "SWD": "JTAG");
+	else
+		printf(", %s not available",
+			   ((res[0] + 1) == JLINK_IF_SWD) ? "JTAG": "SWD");
+	printf("\n");
+}
+
+static void jlink_info(bmp_info_t *info)
+{
+	jlink_print_version(info);
+	jlink_print_speed(info);
+	jlink_print_caps(info);
+	jlink_print_interfaces(info);
+}
+
+/* On success endpoints are set and return 0, !0 else */
+static int initialize_handle(bmp_info_t *info, libusb_device *dev)
+{
+	struct libusb_config_descriptor *config;
+	int ret =  libusb_get_active_config_descriptor(dev, &config);
+	if (ret != LIBUSB_SUCCESS) {
+		fprintf(stderr, "Failed to get configuration descriptor: %s.",
+				libusb_error_name(ret));
+		return -1;
+	}
+	const struct libusb_interface *interface;
+	bool found_interface = false;
+	const struct libusb_interface_descriptor *desc;
+	for (int i = 0; i < config->bNumInterfaces; i++) {
+		interface = &config->interface[i];
+		desc = &interface->altsetting[0];
+		if (desc->bInterfaceClass != LIBUSB_CLASS_VENDOR_SPEC)
+			continue;
+		if (desc->bInterfaceSubClass != LIBUSB_CLASS_VENDOR_SPEC)
+			continue;
+		if (desc->bNumEndpoints < 2)
+			continue;
+		found_interface = true;
+		if (libusb_claim_interface (
+				info->usb_link->ul_libusb_device_handle, i)) {
+			fprintf(stderr, " Can not claim handle\n");
+			found_interface = false;
+		}
+		break;
+	}
+	if (!found_interface) {
+		fprintf(stderr, "No suitable interface found.");
+		libusb_free_config_descriptor(config);
+		return -1;
+	}
+	for (int i = 0; i < desc->bNumEndpoints; i++) {
+		const struct libusb_endpoint_descriptor *epdesc = &desc->endpoint[i];
+		if (epdesc->bEndpointAddress & LIBUSB_ENDPOINT_IN) {
+			info->usb_link->ep_rx = epdesc->bEndpointAddress;
+		} else {
+			info->usb_link->ep_tx = epdesc->bEndpointAddress;
+		}
+	}
+	libusb_free_config_descriptor(config);
+	return 0;
+}
+/* Return 0 if single J-Link device connected or
+ * serial given matches one of several J-Link devices.
+ */
+int jlink_init(bmp_info_t *info)
+{
+	usb_link_t *jl = calloc(1, sizeof(usb_link_t));
+	if (!jl)
+		return -1;
+	info->usb_link = jl;
+	jl->ul_libusb_ctx = info->libusb_ctx;
+	int ret = -1;
+	libusb_device **devs;
+    if (libusb_get_device_list(info->libusb_ctx, &devs) < 0) {
+        fprintf(stderr, "libusb_get_device_list() failed");
+		return ret;
+	}
+	int i = 0;
+	for (;  devs[i]; i++) {
+		libusb_device *dev =  devs[i];
+		struct libusb_device_descriptor desc;
+		if (libusb_get_device_descriptor(dev, &desc) < 0) {
+            fprintf(stderr, "libusb_get_device_descriptor() failed");
+			goto error;;
+		}
+		if (desc.idVendor !=  USB_PID_SEGGER)
+			continue;
+		if ((desc.idProduct != USB_VID_SEGGER_0101) &&
+			(desc.idProduct != USB_VID_SEGGER_0105))
+			continue;
+		int res = libusb_open(dev, &jl->ul_libusb_device_handle);
+		if (res != LIBUSB_SUCCESS)
+			continue;
+		char buf[32];
+		res = libusb_get_string_descriptor_ascii(jl->ul_libusb_device_handle,
+			desc.iSerialNumber, (uint8_t*) buf, sizeof(buf));
+		if ((res <= 0) || (!strstr(buf, info->serial))) {
+			libusb_close(jl->ul_libusb_device_handle);
+			continue;
+		}
+		break;
+	}
+	if (!devs[i])
+		goto error;
+	if (initialize_handle(info, devs[i]))
+		goto error;
+	jl->req_trans = libusb_alloc_transfer(0);
+	jl->rep_trans = libusb_alloc_transfer(0);
+	if (!jl->req_trans || !jl->rep_trans ||
+		!jl->ep_tx || !jl->ep_rx) {
+		fprintf(stderr,"Device setup failed\n");
+		goto error;
+	}
+	libusb_free_device_list(devs, 1);
+	jlink_info(info);
+	return 0;
+  error:
+	libusb_free_device_list(devs, 1);
+	return -1;
+
+}
+
+const char *jlink_target_voltage(bmp_info_t *info)
+{
+        uint8_t cmd[1] = {CMD_GET_HW_STATUS};
+        uint8_t res[8];
+        send_recv(info->usb_link, cmd, 1, res, sizeof(res));
+        uint16_t mVolt = res[0] | (res[1] << 8);
+        static char ret[7];
+        sprintf(ret, "%2d.%03d", mVolt / 1000, mVolt % 1000);
+        return ret;
+}
+
+static bool srst_status = false;
+void jlink_srst_set_val(bmp_info_t *info, bool assert)
+{
+        uint8_t cmd[1];
+        cmd[0]= (assert)? CMD_HW_RESET0: CMD_HW_RESET1;
+        send_recv(info->usb_link, cmd, 1, NULL, 0);
+        platform_delay(2);
+        srst_status = assert;
+}
+
+bool jlink_srst_get_val(bmp_info_t *info) {
+        uint8_t cmd[1] = {CMD_GET_HW_STATUS};
+        uint8_t res[8];
+        send_recv(info->usb_link, cmd, 1, res, sizeof(res));
+        return !(res[6]);
+}
diff --git a/src/platforms/hosted/jlink.h b/src/platforms/hosted/jlink.h
new file mode 100644
index 0000000..27e81bf
--- /dev/null
+++ b/src/platforms/hosted/jlink.h
@@ -0,0 +1,53 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2019 Uwe Bonnes
+ *
+ * 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/>.
+ */
+#if !defined(__JLINK_H_)
+#define __JLINK_H_
+
+#include "jtagtap.h"
+
+/** @cond PRIVATE */
+#define CMD_GET_VERSION         0x01
+#define CMD_GET_HW_STATUS       0x07
+#define CMD_GET_SPEED           0xc0
+#define CMD_GET_SELECT_IF       0xc7
+#define CMD_HW_JTAG3            0xcf
+#define CMD_HW_RESET0           0xdc
+#define CMD_HW_RESET1           0xdd
+#define CMD_GET_CAPS            0xe8
+#define CMD_GET_EXT_CAPS        0xed
+#define CMD_GET_HW_VERSION      0xf0
+
+#define JLINK_IF_GET_ACTIVE    0xfe
+#define JLINK_IF_GET_AVAILABLE 0xff
+
+#define JLINK_CAP_GET_HW_VERSION  2
+#define JLINK_IF_JTAG             1
+#define JLINK_IF_SWD              2
+
+#define SELECT_IF_JTAG            0
+#define SELECT_IF_SWD             1
+
+
+int jlink_init(bmp_info_t *info);
+int jlink_swdp_scan(bmp_info_t *info);
+int jlink_jtagtap_init(bmp_info_t *info, jtag_proc_t *jtag_proc);
+const char *jlink_target_voltage(bmp_info_t *info);
+void jlink_srst_set_val(bmp_info_t *info, bool assert);
+bool jlink_srst_get_val(bmp_info_t *info);
+#endif
diff --git a/src/platforms/hosted/jlink_adiv5_swdp.c b/src/platforms/hosted/jlink_adiv5_swdp.c
new file mode 100644
index 0000000..06da0a1
--- /dev/null
+++ b/src/platforms/hosted/jlink_adiv5_swdp.c
@@ -0,0 +1,372 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2011  Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ * Copyright (C) 2019 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/>.
+ */
+
+/* This file implements the SW-DP specific functions of the
+ * ARM Debug Interface v5 Architecure Specification, ARM doc IHI0031A.
+ */
+
+#include "general.h"
+#include "exception.h"
+#include "target.h"
+#include "target_internal.h"
+#include "adiv5.h"
+#include "jlink.h"
+#include "cl_utils.h"
+
+#define SWDP_ACK_OK    0x01
+#define SWDP_ACK_WAIT  0x02
+#define SWDP_ACK_FAULT 0x04
+
+static uint32_t jlink_adiv5_swdp_read(ADIv5_DP_t *dp, uint16_t addr);
+
+static uint32_t jlink_adiv5_swdp_error(ADIv5_DP_t *dp);
+
+static uint32_t jlink_adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
+				      uint16_t addr, uint32_t value);
+
+static void jlink_adiv5_swdp_abort(ADIv5_DP_t *dp, uint32_t abort);
+
+enum {
+	SWDIO_WRITE = 0,
+	SWDIO_READ
+};
+
+/* Write at least 50 bits high, two bits low and read DP_IDR and put
+*  idle cyccles at the end*/
+static int line_reset(bmp_info_t *info)
+{
+        uint8_t cmd[44];
+        cmd[0]  = CMD_HW_JTAG3;
+        cmd[1]  = 0;
+        /* write 19 Bytes.*/
+        cmd[2]  = 19 * 8;
+        cmd[3]  = 0;
+        uint8_t *direction = cmd + 4;
+        direction[0]  = 0;
+        direction[1]  = 0;
+        direction[2]  = 0;
+        direction[3]  = 0;
+        direction[4]  = 0;
+        direction[5]  = 0xff;
+        direction[6]  = 0xff;
+        direction[7]  = 0xff;
+        direction[8]  = 0xff;
+        direction[9]  = 0xff;
+        direction[10] = 0xff;
+        direction[11] = 0xff;
+        direction[12] = 0xff;
+        direction[13] = 0xff;
+        direction[14] = 0x0;
+        direction[15] = 0x0;
+        direction[16] = 0x0;
+        direction[17] = 0x0;
+        direction[18] = 0xe0;
+        uint8_t *data = direction + 19;
+        data[5] =  0xff;
+        data[6] =  0xff;
+        data[7] =  0xff;
+        data[8] =  0xff;
+        data[9] =  0xff;
+        data[10] = 0xff;
+        data[11] = 0xff;
+        data[12] = 0;
+        data[13] = 0xa5;
+        data[18] = 0;
+
+        uint8_t res[18];
+        send_recv(info->usb_link, cmd, 42, res, 19);
+        send_recv(info->usb_link, NULL, 0, res,  1);
+
+        if (res[0] != 0) {
+                fprintf(stderr, "Line reset failed\n");
+                return -1;
+        }
+        return 0;
+}
+
+static int swdptap_init(bmp_info_t *info)
+{
+	if (cl_debuglevel)
+		printf("swdptap_init\n");
+	uint8_t cmd[2] = {CMD_GET_SELECT_IF, JLINK_IF_GET_AVAILABLE};
+	uint8_t res[4];
+	send_recv(info->usb_link, cmd, 2, res, sizeof(res));
+	if (!(res[0] & JLINK_IF_SWD))
+		return -1;
+	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);
+	return 0;
+}
+
+int jlink_swdp_scan(bmp_info_t *info)
+{
+	swdptap_init(info);
+	target_list_free();
+	ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
+	if (!dp) /* calloc failed: heap exhaustion */
+		return 0;
+	uint8_t cmd[44];
+	cmd[0]  = CMD_HW_JTAG3;
+	cmd[1]  = 0;
+	/* write 18 Bytes.*/
+	cmd[2]  = 17 * 8;
+	cmd[3]  = 0;
+	uint8_t *direction = cmd + 4;
+	direction[0]  = 0xff;
+	direction[1]  = 0xff;
+	direction[2]  = 0xff;
+	direction[3]  = 0xff;
+	direction[4]  = 0xff;
+	direction[5]  = 0xff;
+	direction[6]  = 0xff;
+	direction[7]  = 0xff;
+	direction[8]  = 0xff;
+	direction[9]  = 0xff;
+	direction[10] = 0xff;
+	direction[11] = 0xff;
+	direction[12] = 0xff;
+	direction[13] = 0xff;
+	direction[14] = 0xff;
+	direction[15] = 0xff;
+	direction[16] = 0xff;
+	uint8_t *data = direction + 17;
+	data[0] = 0xff;
+	data[1] = 0xff;
+	data[2] = 0xff;
+	data[3] = 0xff;
+	data[4] = 0xff;
+	data[5] = 0xff;
+	data[6] = 0xff;
+	data[7] = 0x9e;
+	data[8] = 0xe7;
+	data[9] = 0xff;
+	data[10] = 0xff;
+	data[11] = 0xff;
+	data[12] = 0xff;
+	data[13] = 0xff;
+	data[14] = 0xff;
+	data[15] = 0;
+	data[16] = 0;
+
+	uint8_t res[18];
+	send_recv(info->usb_link, cmd, 38, res, 17);
+	send_recv(info->usb_link, NULL, 0, res,  1);
+
+	if (res[0] != 0) {
+		fprintf(stderr, "Line reset failed\n");
+		return 0;
+	}
+	dp->idcode = jlink_adiv5_swdp_low_access(dp, 1, ADIV5_DP_IDCODE, 0);
+	dp->dp_read = jlink_adiv5_swdp_read;
+	dp->error = jlink_adiv5_swdp_error;
+	dp->low_access = jlink_adiv5_swdp_low_access;
+	dp->abort = jlink_adiv5_swdp_abort;
+
+	jlink_adiv5_swdp_error(dp);
+	adiv5_dp_init(dp);
+	return target_list?1:0;
+}
+
+void ap_decode_access(uint16_t addr, uint8_t RnW)
+{
+	if (RnW)
+		printf("Read ");
+	else
+		printf("Write ");
+	switch(addr) {
+	case 0x00:
+		if (RnW)
+			printf("DP_DPIDR: ");
+		else
+			printf("DP_ABORT: ");
+		break;
+	case 0x004: printf("CTRL/STAT: ");
+		break;
+	case 0x008:
+		if (RnW)
+			printf("RESEND: ");
+		else
+			printf("DP_SELECT: ");
+		break;
+	case 0x00c: printf("DP_RDBUFF: ");
+		break;
+	case 0x100: printf("AP_CSW: ");
+		break;
+	case 0x104: printf("AP_TAR: ");
+		break;
+	case 0x10c: printf("AP_DRW: ");
+		break;
+	case 0x1f8: printf("AP_BASE: ");
+		break;
+	case 0x1fc: printf("AP_IDR: ");
+		break;
+	}
+}
+
+static uint32_t jlink_adiv5_swdp_read(ADIv5_DP_t *dp, uint16_t addr)
+{
+	if (cl_debuglevel)
+		printf("jlink_adiv5_swdp_read 0x%04x\n", addr);
+	if (addr & ADIV5_APnDP) {
+		adiv5_dp_low_access(dp, ADIV5_LOW_READ, addr, 0);
+		return adiv5_dp_low_access(dp, ADIV5_LOW_READ,
+		                           ADIV5_DP_RDBUFF, 0);
+	} else {
+		return jlink_adiv5_swdp_low_access(dp, ADIV5_LOW_READ, addr, 0);
+	}
+}
+
+static uint32_t jlink_adiv5_swdp_error(ADIv5_DP_t *dp)
+{
+	uint32_t err, clr = 0;
+	err = jlink_adiv5_swdp_read(dp, ADIV5_DP_CTRLSTAT) &
+		(ADIV5_DP_CTRLSTAT_STICKYORUN | ADIV5_DP_CTRLSTAT_STICKYCMP |
+		ADIV5_DP_CTRLSTAT_STICKYERR | ADIV5_DP_CTRLSTAT_WDATAERR);
+
+	if(err & ADIV5_DP_CTRLSTAT_STICKYORUN)
+		clr |= ADIV5_DP_ABORT_ORUNERRCLR;
+	if(err & ADIV5_DP_CTRLSTAT_STICKYCMP)
+		clr |= ADIV5_DP_ABORT_STKCMPCLR;
+	if(err & ADIV5_DP_CTRLSTAT_STICKYERR)
+		clr |= ADIV5_DP_ABORT_STKERRCLR;
+	if(err & ADIV5_DP_CTRLSTAT_WDATAERR)
+		clr |= ADIV5_DP_ABORT_WDERRCLR;
+	if (clr)
+		adiv5_dp_write(dp, ADIV5_DP_ABORT, clr);
+	if (dp->fault)
+		err |= 0x8000;
+	dp->fault = 0;
+
+	return err;
+}
+
+static uint32_t jlink_adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
+				      uint16_t addr, uint32_t value)
+{
+	if (cl_debuglevel) {
+		if (RnW) {
+			if (cl_debuglevel > 0) {
+				ap_decode_access(addr, RnW);
+				printf(" start:\n");
+			}
+		}else{
+			ap_decode_access(addr, RnW);
+			printf(" %08" PRIx32 "\n", value);
+		}
+	}
+	bool APnDP = addr & ADIV5_APnDP;
+	uint8_t addr8 = addr & 0xff;
+	uint8_t request = 0x81;
+	uint32_t response = 0;
+	uint8_t ack;
+	platform_timeout timeout;
+
+	if(APnDP && dp->fault) return 0;
+
+	if(APnDP) request ^= 0x22;
+	if(RnW)   request ^= 0x24;
+
+	addr8 &= 0xC;
+	request |= (addr8 << 1) & 0x18;
+	if((addr8 == 4) || (addr8 == 8))
+		request ^= 0x20;
+	uint8_t cmd[16];
+	uint8_t res[8];
+	cmd[0] = CMD_HW_JTAG3;
+	cmd[1] = 0;
+	cmd[2] = 13;
+	cmd[3] = 0;
+	cmd[4] = 0xff;
+	cmd[5] = 0xe3;
+	cmd[6] = request << 2;
+	cmd[7] = request >> 6;
+	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)
+			raise_exception(EXCEPTION_ERROR, "Low access setup failed");
+		ack = res[1] >> 2;
+		ack &= 7;
+	} while (ack == SWDP_ACK_WAIT && !platform_timeout_is_expired(&timeout));
+	if (ack == SWDP_ACK_WAIT)
+		raise_exception(EXCEPTION_TIMEOUT, "SWDP ACK timeout");
+
+	if(ack == SWDP_ACK_FAULT) {
+		if (cl_debuglevel)
+			fprintf(stderr, "Fault\n");
+		dp->fault = 1;
+		return 0;
+	}
+
+	if(ack != SWDP_ACK_OK) {
+		if (cl_debuglevel)
+			fprintf(stderr, "Protocol\n");
+		line_reset(&info);
+		return 0;
+	}
+	cmd[3] = 0;
+	/* Always prepend an idle cycle (SWDIO = 0)!*/
+	if(RnW) {
+		memset(cmd + 4, 0, 10);
+		cmd[2] = 34;
+		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)
+			raise_exception(EXCEPTION_ERROR, "Low access read failed");
+		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 (cl_debuglevel ) {
+			ap_decode_access(addr, RnW);;
+			printf("0x%08" PRIx32 "\n", response);
+		}
+		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);
+		int bit_count  = __builtin_popcount(value);
+		cmd[13] |= ((bit_count & 1) ? 4 : 0);
+		send_recv(info.usb_link, cmd, 14, res, 5);
+		send_recv(info.usb_link, NULL, 0, res, 1);
+		if (res[0] != 0)
+			raise_exception(EXCEPTION_ERROR, "Low access write failed");
+	}
+	return response;
+}
+
+static void jlink_adiv5_swdp_abort(ADIv5_DP_t *dp, uint32_t abort)
+{
+	adiv5_dp_write(dp, ADIV5_DP_ABORT, abort);
+}
diff --git a/src/platforms/hosted/jlink_jtagtap.c b/src/platforms/hosted/jlink_jtagtap.c
new file mode 100644
index 0000000..d69d904
--- /dev/null
+++ b/src/platforms/hosted/jlink_jtagtap.c
@@ -0,0 +1,176 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2020 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/>.
+ */
+
+/* Low level JTAG implementation using jlink.
+ *
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+
+#include <assert.h>
+
+#include "general.h"
+#include "exception.h"
+
+#include "jlink.h"
+#include "cl_utils.h"
+
+static void jtagtap_reset(void)
+{
+	jtagtap_soft_reset();
+}
+
+static void jtagtap_tms_seq(uint32_t MS, int ticks)
+{
+	if (cl_debuglevel)
+		printf("jtagtap_tms_seq 0x%08" PRIx32 ", ticks %d\n", MS, ticks);
+	int len = (ticks + 7) / 8;
+	uint8_t cmd[12];
+	cmd[0] = CMD_HW_JTAG3;
+	cmd[1] = 0;
+	cmd[2] = ticks;
+	cmd[3] = 0;
+	uint8_t *tms = cmd + 4;
+	for (int i = 0; i < len; i++) {
+		*tms = MS & 0xff;
+		*(tms + len) = *tms;
+		tms++;
+		MS >>= 8;
+	}
+	uint8_t res[4];
+	send_recv(info.usb_link, cmd, 4 + 2 * len, res, len);
+	send_recv(info.usb_link, NULL, 0, res, 1);
+	if (res[0] != 0)
+		raise_exception(EXCEPTION_ERROR, "tagtap_tms_seq failed");
+}
+
+static void jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms,
+						 const uint8_t *DI, int ticks)
+{
+	if (!ticks)
+		return;
+	int len = (ticks + 7) / 8;
+	if (cl_debuglevel) {
+		printf("jtagtap_tdi_tdo %s, ticks %d, DI: ",
+			   (final_tms) ? "Final TMS" : "", ticks);
+		for (int i = 0; i < len; i++) {
+			printf("%02x", DI[i]);
+		}
+		printf("\n");
+	}
+	uint8_t *cmd = alloca(4 + 2 * len);
+	cmd[0] = CMD_HW_JTAG3;
+	cmd[1] = 0;
+	cmd[2] = ticks;
+	cmd[3] = 0;
+	uint8_t *tms = cmd + 4;
+	for (int i = 0; i < len; i++)
+		*tms++ = 0;
+	if (final_tms)
+		cmd[4 + (ticks - 1) / 8] |= (1 << ((ticks - 1) % 8));
+	uint8_t *tdi = tms;
+	if (DI)
+		for (int i = 0; i < len; i++)
+			*tdi++ = DI[i];
+	if (DO)
+		send_recv(info.usb_link, cmd, 4 + 2 * len, DO, len);
+	else
+		send_recv(info.usb_link, cmd, 4 + 2 * len, cmd, len);
+	uint8_t res[1];
+	send_recv(info.usb_link, NULL, 0, res, 1);
+	if (res[0] != 0)
+		raise_exception(EXCEPTION_ERROR, "jtagtap_tdi_tdi failed");
+}
+
+static void jtagtap_tdi_seq(const uint8_t final_tms, const uint8_t *DI,
+							int ticks)
+{
+	return jtagtap_tdi_tdo_seq(NULL,  final_tms, DI, ticks);
+}
+
+static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
+{
+	if (cl_debuglevel)
+		printf("jtagtap_next TMS 0x%02x, TDI %02x\n", dTMS, dTDI);
+	uint8_t cmd[6];
+	cmd[0] = CMD_HW_JTAG3;
+	cmd[1] = 0;
+	cmd[2] = 1;
+	cmd[3] = 0;
+	cmd[4] = (dTMS) ? 0xff : 0;
+	cmd[5] = (dTDI) ? 0xff : 0;
+	uint8_t ret[1];
+	send_recv(info.usb_link, cmd, 6, ret, 1);
+	uint8_t res[1];
+	send_recv(info.usb_link, NULL, 0, res, 1);
+	if (res[0] != 0)
+		raise_exception(EXCEPTION_ERROR, "jtagtap_next failed");
+	return (ret[0] & 1);
+}
+
+int jlink_jtagtap_init(bmp_info_t *info, jtag_proc_t *jtag_proc)
+{
+	if (cl_debuglevel)
+		printf("jtap_init\n");
+	uint8_t cmd_switch[2] = {CMD_GET_SELECT_IF, JLINK_IF_GET_AVAILABLE};
+	uint8_t res[4];
+	send_recv(info->usb_link, cmd_switch, 2, res, sizeof(res));
+	if (!(res[0] & JLINK_IF_JTAG)) {
+		fprintf(stderr, "JTAG not available\n");
+		return -1;
+	}
+	cmd_switch[1] = SELECT_IF_JTAG;
+	send_recv(info->usb_link, cmd_switch, 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);
+	uint8_t cmd[44];
+	cmd[0]  = CMD_HW_JTAG3;
+	cmd[1]  = 0;
+	/* write 8 Bytes.*/
+	cmd[2]  = 9 * 8;
+	cmd[3]  = 0;
+	uint8_t *tms = cmd + 4;
+	tms[0]  = 0xff;
+	tms[1]  = 0xff;
+	tms[2]  = 0xff;
+	tms[3]  = 0xff;
+	tms[4]  = 0xff;
+	tms[5]  = 0xff;
+	tms[6]  = 0xff;
+	tms[7]  = 0x3c;
+	tms[8]  = 0xe7;
+	send_recv(info->usb_link, cmd, 4 + 2 * 9, cmd, 9);
+	send_recv(info->usb_link, NULL, 0, res,  1);
+
+	if (res[0] != 0) {
+		fprintf(stderr, "Switch to JTAGt failed\n");
+		return 0;
+	}
+	jtag_proc->jtagtap_reset = jtagtap_reset;
+	jtag_proc->jtagtap_next =jtagtap_next;
+	jtag_proc->jtagtap_tms_seq = jtagtap_tms_seq;
+	jtag_proc->jtagtap_tdi_tdo_seq = jtagtap_tdi_tdo_seq;
+	jtag_proc->jtagtap_tdi_seq = jtagtap_tdi_seq;
+	return 0;
+}
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index c061e73..75df055 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -35,6 +35,7 @@
 #include "bmp_remote.h"
 #include "stlinkv2.h"
 #include "ftdi_bmp.h"
+#include "jlink.h"
 
 #define VENDOR_ID_BMP            0x1d50
 #define PRODUCT_ID_BMP           0x6018
@@ -49,6 +50,8 @@
 #define PRODUCT_ID_STLINKV3      0x374f
 #define PRODUCT_ID_STLINKV3E     0x374e
 
+#define VENDOR_ID_SEGGER         0x1366
+
 bmp_info_t info;
 
 swd_proc_t swd_proc;
@@ -166,7 +169,9 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 					fprintf(stderr, "INFO: STLINKV1 not supported\n");
 				continue;
 			}
-		} else {
+		} else if (desc.idVendor ==  VENDOR_ID_SEGGER) {
+			type = BMP_TYPE_JLINK;
+		} else{
 			continue;
 		}
 		found_debuggers ++;
@@ -226,7 +231,7 @@ void platform_init(int argc, char **argv)
 	} else if (find_debuggers(&cl_opts, &info)) {
 		exit(-1);
 	}
-	printf("Using %s %s %s\n", info.serial,
+	printf("Using %04x:%04x %s %s %s\n", info.vid, info.pid, info.serial,
 		   info.manufacturer,
 		   info.product);
 	switch (info.bmp_type) {
@@ -241,6 +246,10 @@ void platform_init(int argc, char **argv)
 		break;
 	case BMP_TYPE_LIBFTDI:
 		break;
+	case BMP_TYPE_JLINK:
+		if (jlink_init(&info))
+			exit(-1);
+		break;
 	default:
 		exit(-1);
 	}
@@ -273,6 +282,8 @@ int platform_adiv5_swdp_scan(void)
 		free(dp);
 		break;
 	}
+	case BMP_TYPE_JLINK:
+		return jlink_swdp_scan(&info);
 	default:
 		return 0;
 	}
@@ -285,8 +296,8 @@ int platform_swdptap_init(void)
 	case BMP_TYPE_BMP:
 		return remote_swdptap_init(&swd_proc);
 	case BMP_TYPE_STLINKV2:
+	case BMP_TYPE_JLINK:
 		return 0;
-		break;
 	case BMP_TYPE_LIBFTDI:
 		return libftdi_swdptap_init(&swd_proc);
 	default:
@@ -300,6 +311,7 @@ int platform_jtag_scan(const uint8_t *lrlens)
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 	case BMP_TYPE_LIBFTDI:
+	case BMP_TYPE_JLINK:
 		return jtag_scan(lrlens);
 	case BMP_TYPE_STLINKV2:
 		return jtag_scan_stlinkv2(&info, lrlens);
@@ -318,6 +330,8 @@ int platform_jtagtap_init(void)
 		return 0;
 	case BMP_TYPE_LIBFTDI:
 		return libftdi_jtagtap_init(&jtag_proc);
+	case BMP_TYPE_JLINK:
+		return jlink_jtagtap_init(&info, &jtag_proc);
 	default:
 		return -1;
 	}
@@ -339,6 +353,7 @@ int platform_jtag_dp_init(ADIv5_DP_t *dp)
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 	case BMP_TYPE_LIBFTDI:
+	case BMP_TYPE_JLINK:
 		return 0;
 	case BMP_TYPE_STLINKV2:
 		return stlink_jtag_dp_init(dp);
@@ -376,6 +391,8 @@ const char *platform_target_voltage(void)
 		return stlink_target_voltage(&info);
 	case BMP_TYPE_LIBFTDI:
 		return libftdi_target_voltage();
+	case BMP_TYPE_JLINK:
+		return jlink_target_voltage(&info);
 	default:
 		break;
 	}
@@ -389,6 +406,8 @@ void platform_srst_set_val(bool assert)
 		return stlink_srst_set_val(&info, assert);
 	case BMP_TYPE_BMP:
 		return remote_srst_set_val(assert);
+	case BMP_TYPE_JLINK:
+		return jlink_srst_set_val(&info, assert);
 	default:
 		break;
 	}
@@ -401,6 +420,8 @@ bool platform_srst_get_val(void)
 		return remote_srst_get_val();
 	case BMP_TYPE_STLINKV2:
 		return stlink_srst_get_val();
+	case BMP_TYPE_JLINK:
+		return jlink_srst_get_val(&info);
 	default:
 		break;
 	}
diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 133606f..2d50a72 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -332,7 +332,7 @@ static int stlink_usb_error_check(uint8_t *data, bool verbose)
 	}
 }
 
-static int send_recv_retry(uint8_t *txbuf, size_t txsize,
+static int stlink_send_recv_retry(uint8_t *txbuf, size_t txsize,
 					 uint8_t *rxbuf, size_t rxsize)
 {
 	uint32_t start = platform_time_ms();
@@ -400,7 +400,7 @@ static void stlink_version(bmp_info_t *info)
 		uint8_t data[12];
 		int size = send_recv(info->usb_link, cmd, 16, data, 12);
 		if (size == -1) {
-			printf("[!] send_recv STLINK_APIV3_GET_VERSION_EX\n");
+			printf("[!] stlink_send_recv STLINK_APIV3_GET_VERSION_EX\n");
 		}
 		Stlink.ver_stlink = data[0];
 		Stlink.ver_swim  =  data[1];
@@ -415,7 +415,7 @@ static void stlink_version(bmp_info_t *info)
 		uint8_t data[6];
 		int size = send_recv(info->usb_link, cmd, 16, data, 6);
 		if (size == -1) {
-			printf("[!] send_recv STLINK_GET_VERSION_EX\n");
+			printf("[!] stlink_send_recv STLINK_GET_VERSION_EX\n");
 		}
 		Stlink.vid = data[3] << 8 | data[2];
 		Stlink.pid = data[5] << 8 | data[4];
@@ -766,7 +766,7 @@ static int stlink_read_dp_register(uint16_t port, uint16_t addr, uint32_t *reg)
 		cmd[4] = addr & 0xff;
 	DEBUG_STLINK("Read DP, Addr 0x%04" PRIx16 ": \n", addr);
 	uint8_t data[8];
-	int res = send_recv_retry(cmd, 16, data, 8);
+	int res = stlink_send_recv_retry(cmd, 16, data, 8);
 	if (res == STLINK_ERROR_OK) {
 		uint32_t ret = data[4] | data[5] << 8 | data[6] << 16 | data[7] << 24;
 		DEBUG_STLINK("0x%08" PRIx32" \n", ret);
@@ -791,7 +791,7 @@ static int stlink_write_dp_register(uint16_t port, uint16_t addr, uint32_t val)
 			val & 0xff, (val >>  8) & 0xff, (val >> 16) & 0xff,
 			(val >> 24) & 0xff};
 		uint8_t data[2];
-		send_recv_retry(cmd, 16, data, 2);
+		stlink_send_recv_retry(cmd, 16, data, 2);
 		DEBUG_STLINK("Write DP, Addr 0x%04" PRIx16 ": 0x%08" PRIx32
 			  " \n", addr, val);
 		return stlink_usb_error_check(data, true);

From ef558eebb8e7c4648c95c3d1c16c3e40c4dde246 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 25 Apr 2020 14:55:48 +0200
Subject: [PATCH 34/43] CMSIS-DAP support.

Thanks to edbg, pyocd and openocd for code and ideas.
---
 .travis.yml                       |   2 +-
 src/platforms/hosted/Makefile.inc |  13 +-
 src/platforms/hosted/cmsis_dap.c  | 372 +++++++++++++++
 src/platforms/hosted/cmsis_dap.h  |  50 ++
 src/platforms/hosted/dap.c        | 753 ++++++++++++++++++++++++++++++
 src/platforms/hosted/dap.h        |  94 ++++
 src/platforms/hosted/platform.c   |  30 ++
 src/platforms/pc/cl_utils.c       |   2 +-
 src/platforms/pc/cl_utils.h       |   7 +
 9 files changed, 1318 insertions(+), 5 deletions(-)
 create mode 100644 src/platforms/hosted/cmsis_dap.c
 create mode 100644 src/platforms/hosted/cmsis_dap.h
 create mode 100644 src/platforms/hosted/dap.c
 create mode 100644 src/platforms/hosted/dap.h

diff --git a/.travis.yml b/.travis.yml
index 5806aef..c7dc424 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -10,7 +10,7 @@ before_install:
   - wget http://www.intra2net.com/en/developer/libftdi/download/libftdi1-1.2.tar.bz2.sig
   - gpg --trust-model always --verify libftdi1-1.2.tar.bz2.sig
   - tar -xjf libftdi1-1.2.tar.bz2
-  - sudo apt-get install -y build-essential libboost-all-dev gcc-arm-embedded libusb-1.0-0-dev
+  - sudo apt-get install -y build-essential libboost-all-dev gcc-arm-embedded libusb-1.0-0-dev libhidapi-dev
 
 install:
   - cd libftdi1-1.2
diff --git a/src/platforms/hosted/Makefile.inc b/src/platforms/hosted/Makefile.inc
index 656f450..dd1dfc4 100644
--- a/src/platforms/hosted/Makefile.inc
+++ b/src/platforms/hosted/Makefile.inc
@@ -9,14 +9,14 @@ LDFLAGS += -lasan
 else ifneq (, $(findstring mingw, $(SYS)))
 SRC += 	 serial_win.c
 LDFLAGS += -lws2_32
-LDFLAGS += -lhid -lsetupapi
+LDFLAGS += -lsetupapi
 else ifneq (, $(findstring cygwin, $(SYS)))
 SRC += 	 serial_win.c
 LDFLAGS += -lws2_32
-LDFLAGS += -lhid -lsetupapi
+LDFLAGS += -lsetupapi
 #https://github.com/dmlc/xgboost/issues/1945 indicates macosx as indicator
 else ifneq (filter, macosx darwin, $(SYS)))
-LDFLAGS += -lhid -lsetupapi
+LDFLAGS += -lsetupapi
 LDFLAGS += hidapi/mac/.libs/libhidapi.a -framework IOKit
 LDFLAGS += -framework CoreFoundation hidapi/mac/.libs/libhidapi.a
 CFLAGS += -Ihidapi/hidapi
@@ -26,6 +26,13 @@ LDFLAGS += -lusb-1.0
 CFLAGS += $(shell pkg-config --cflags libftdi1)
 LDFLAGS += $(shell pkg-config --libs libftdi1)
 
+ifeq ($(shell pkg-config --exists hidapi-libusb && echo 0), 0)
+CFLAGS += $(shell pkg-config --cflags hidapi-libusb)
+LDFLAGS += $(shell pkg-config --libs hidapi-libusb)
+CFLAGS  +=  -DCMSIS_DAP
+SRC += cmsis_dap.c dap.c
+endif
+
 VPATH += platforms/pc
 SRC += timing.c cl_utils.c utils.c libusb_utils.c
 SRC += stlinkv2.c
diff --git a/src/platforms/hosted/cmsis_dap.c b/src/platforms/hosted/cmsis_dap.c
new file mode 100644
index 0000000..04c3905
--- /dev/null
+++ b/src/platforms/hosted/cmsis_dap.c
@@ -0,0 +1,372 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2019-20 Uwe Bonnes
+ *
+ * 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/>.
+ */
+/* Modified from edbg.c
+ *   Links between bmp and edbg
+ *
+ * https://arm-software.github.io/CMSIS_5/DAP/html/index.html
+*/
+
+#include "general.h"
+#include "gdb_if.h"
+#include "adiv5.h"
+
+#include <assert.h>
+#include <unistd.h>
+#include <signal.h>
+#include <ctype.h>
+#include <sys/time.h>
+#include <hidapi.h>
+#include <wchar.h>
+
+#include "dap.h"
+#include "cmsis_dap.h"
+
+#include "cl_utils.h"
+#include "target.h"
+#include "target_internal.h"
+
+#define MAX_DEBUGGERS     20
+
+uint8_t dap_caps;
+uint8_t mode;
+
+/*- Variables ---------------------------------------------------------------*/
+static hid_device *handle = NULL;
+static uint8_t hid_buffer[1024 + 1];
+static int report_size = 512 + 1; // TODO: read actual report size
+/* LPC845 Breakout Board Rev. 0 report invalid response with > 65 bytes */
+int dap_init(bmp_info_t *info)
+{
+	if (hid_init())
+		return -1;
+	int size = strlen(info->serial);
+	wchar_t serial[size + 1], *wc = serial;
+	for (int i = 0; i < size; i++)
+		*wc++ = info->serial[i];
+	*wc = 0;
+	/* Blacklist devices that do not wirk with 513 byte report length
+	 * FIXME: Find a solution to decipher from the device.
+	 */
+	if ((info->vid == 0x1fc9) && (info->pid == 0x0132)) {
+		printf("Blacklist\n");
+		report_size = 64 + 1;
+	}
+	handle = hid_open(info->vid, info->pid, serial);
+	if (!handle)
+		return -1;
+	dap_disconnect();
+	size = dap_info(DAP_INFO_CAPABILITIES, hid_buffer, sizeof(hid_buffer));
+	dap_caps = hid_buffer[0];
+	printf(" Cap (0x%2x): %s%s%s", hid_buffer[0],
+		   (hid_buffer[0] & 1)? "SWD" : "",
+		   ((hid_buffer[0] & 3) == 3) ? "/" : "",
+		   (hid_buffer[0] & 2)? "JTAG" : "");
+	if (hid_buffer[0] & 4)
+		printf(", SWO_UART");
+	if (hid_buffer[0] & 8)
+		printf(", SWO_MANCHESTER");
+	if (hid_buffer[0] & 0x10)
+		printf(", Atomic Cmds");
+	printf("\n");
+	return 0;
+}
+
+static void dap_dp_abort(ADIv5_DP_t *dp, uint32_t abort)
+{
+	/* DP Write to Reg 0.*/
+	dap_write_reg(dp, ADIV5_DP_ABORT, abort);
+}
+
+static uint32_t dap_dp_error(ADIv5_DP_t *dp)
+{
+	uint32_t ctrlstat = dap_read_reg(dp, ADIV5_DP_CTRLSTAT);
+	uint32_t err = ctrlstat &
+		(ADIV5_DP_CTRLSTAT_STICKYORUN | ADIV5_DP_CTRLSTAT_STICKYCMP |
+		ADIV5_DP_CTRLSTAT_STICKYERR | ADIV5_DP_CTRLSTAT_WDATAERR);
+	uint32_t clr = 0;
+	if(err & ADIV5_DP_CTRLSTAT_STICKYORUN)
+		clr |= ADIV5_DP_ABORT_ORUNERRCLR;
+	if(err & ADIV5_DP_CTRLSTAT_STICKYCMP)
+		clr |= ADIV5_DP_ABORT_STKCMPCLR;
+	if(err & ADIV5_DP_CTRLSTAT_STICKYERR)
+		clr |= ADIV5_DP_ABORT_STKERRCLR;
+	if(err & ADIV5_DP_CTRLSTAT_WDATAERR)
+		clr |= ADIV5_DP_ABORT_WDERRCLR;
+	dap_write_reg(dp, ADIV5_DP_ABORT, clr);
+	dp->fault = 0;
+	return err;
+}
+
+static uint32_t  dap_dp_low_access(struct ADIv5_DP_s *dp, uint8_t RnW,
+                               uint16_t addr, uint32_t value)
+{
+	bool APnDP = addr & ADIV5_APnDP;
+	uint32_t res = 0;
+	uint8_t reg = (addr & 0xc) | ((APnDP)? 1 : 0);
+	if (RnW) {
+		res = dap_read_reg(dp, reg);
+	}
+	else {
+		dap_write_reg(dp, reg, value);
+	}
+	return res;
+}
+
+static uint32_t dap_dp_read_reg(ADIv5_DP_t *dp, uint16_t addr)
+{
+	return dap_read_reg(dp, addr);
+}
+
+void dap_exit_function(void)
+{
+	if (handle) {
+		dap_disconnect();
+		hid_close(handle);
+	}
+}
+
+int dbg_get_report_size(void)
+{
+	return report_size;
+}
+
+int dbg_dap_cmd(uint8_t *data, int size, int rsize)
+
+{
+	char cmd = data[0];
+	int res;
+
+	memset(hid_buffer, 0xff, report_size + 1);
+
+	hid_buffer[0] = 0x00; // Report ID??
+	memcpy(&hid_buffer[1], data, rsize);
+
+	if (cl_debuglevel & BMP_DEBUG_WIRE) {
+		printf("cmd :   ");
+		for(int i = 0; (i < 16) && (i < rsize + 1); i++)
+			printf("%02x.",	hid_buffer[i]);
+		printf("\n");
+	}
+	res = hid_write(handle, hid_buffer, rsize + 1);
+	if (res < 0) {
+		fprintf(stderr, "Error: %ls\n", hid_error(handle));
+		exit(-1);
+	}
+	if (size) {
+		res = hid_read(handle, hid_buffer, report_size + 1);
+		if (res < 0) {
+			fprintf(stderr, "debugger read(): %ls\n", hid_error(handle));
+			exit(-1);
+		}
+		if (size && hid_buffer[0] != cmd) {
+			printf("cmd %02x invalid response received %02x\n",
+				   cmd, hid_buffer[0]);
+		}
+		res--;
+		memcpy(data, &hid_buffer[1], (size < res) ? size : res);
+		if (cl_debuglevel & BMP_DEBUG_WIRE) {
+			printf("cmd res:");
+			for(int i = 0; (i < 16) && (i < size + 4); i++)
+				printf("%02x.",	hid_buffer[i]);
+			printf("\n");
+		}
+	}
+
+	return res;
+}
+#define ALIGNOF(x) (((x) & 3) == 0 ? ALIGN_WORD :					\
+                    (((x) & 1) == 0 ? ALIGN_HALFWORD : ALIGN_BYTE))
+
+static void dap_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
+{
+	if (len == 0)
+		return;
+	enum align align = MIN(ALIGNOF(src), ALIGNOF(len));
+#if 0
+	printf("memread @ %" PRIx32 " len %ld, align %d , start: \n",
+		   src, len, align);
+#endif
+	if (((unsigned)(1 << align)) == len)
+		return dap_read_single(ap, dest, src, align);
+	/* One word transfer for every byte/halfword/word
+	 * Total number of bytes in transfer*/
+	unsigned int max_size = (dbg_get_report_size() - 5) >> (2 - align);
+	while (len) {
+		dap_ap_mem_access_setup(ap, src, align);
+		/* Calculate length until next access setup is needed */
+		unsigned int blocksize = (src | 0x3ff) - src + 1;
+		if (blocksize > len)
+			blocksize = len;
+		while (blocksize) {
+			unsigned int transfersize = blocksize;
+			if (transfersize > max_size)
+				transfersize = max_size;
+			unsigned int res = dap_read_block(ap, dest, src, transfersize,
+											  align);
+			if (res) {
+//			    printf("mem_read failed %02x\n", res);
+				ap->dp->fault = 1;
+				return;
+			}
+			blocksize -= transfersize;
+			len       -= transfersize;
+			dest      += transfersize;
+			src       += transfersize;
+		}
+	}
+//	printf("memread res last data %08" PRIx32 "\n", ((uint32_t*)dest)[-1]);
+}
+
+static void dap_mem_write_sized(
+	ADIv5_AP_t *ap, uint32_t dest, const void *src,
+							size_t len, enum align align)
+{
+	if (len == 0)
+		return;
+#if 0
+	printf("memwrite @ %" PRIx32 " len %ld, align %d , %08x start: \n",
+		   dest, len, align, *(uint32_t *)src);
+#endif
+	if (((unsigned)(1 << align)) == len)
+		return dap_write_single(ap, dest, src, align);
+	unsigned int max_size = (dbg_get_report_size() - 5) >> (2 - align);
+	while (len) {
+		dap_ap_mem_access_setup(ap, dest, align);
+		unsigned int blocksize = (dest | 0x3ff) - dest + 1;
+		if (blocksize > len)
+			blocksize = len;
+		while (blocksize) {
+			unsigned int transfersize = blocksize;
+			if (transfersize > max_size)
+				transfersize = max_size;
+			unsigned int res = dap_write_block(ap, dest, src, transfersize,
+											   align);
+			if (res) {
+				printf("mem_write failed %02x\n", res);
+				ap->dp->fault = 1;
+				return;
+			}
+			blocksize -= transfersize;
+			len       -= transfersize;
+			dest      += transfersize;
+			src       += transfersize;
+		}
+	}
+//	printf("memwrite done\n");
+}
+
+int dap_enter_debug_swd(ADIv5_DP_t *dp)
+{
+	target_list_free();
+	if (!(dap_caps & DAP_CAP_SWD))
+		return -1;
+	mode =  DAP_CAP_SWD;
+	dap_swj_clock(2000000);
+	dap_transfer_configure(2, 128, 128);
+	dap_swd_configure(0);
+	dap_connect(false);
+	dap_led(0, 1);
+	dap_reset_link(false);
+
+	dp->idcode = dap_read_idcode(dp);
+	dp->dp_read = dap_dp_read_reg;
+	dp->error = dap_dp_error;
+	dp->low_access =  dap_dp_low_access;
+	dp->abort = dap_dp_abort; /* DP Write to Reg 0.*/
+	return 0;
+}
+
+void dap_adiv5_dp_defaults(ADIv5_DP_t *dp)
+{
+	if ((mode == DAP_CAP_JTAG) && dap_jtag_configure())
+		return;
+	dp->ap_read  = dap_ap_read;
+	dp->ap_write = dap_ap_write;
+	dp->mem_read = dap_mem_read;
+	dp->mem_write_sized =  dap_mem_write_sized;
+}
+
+static void cmsis_dap_jtagtap_reset(void)
+{
+	jtagtap_soft_reset();
+	/* Is there a way to know if TRST is available?*/
+}
+
+static void cmsis_dap_jtagtap_tms_seq(uint32_t MS, int ticks)
+{
+	uint8_t TMS[4] = {MS & 0xff, (MS >> 8) & 0xff, (MS >> 16) & 0xff,
+					  (MS >> 24) & 0xff};
+	dap_jtagtap_tdi_tdo_seq(NULL, false, TMS, NULL, ticks);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("tms_seq DI %08x %d\n", MS, ticks);
+}
+
+static void cmsis_dap_jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms,
+										  const uint8_t *DI, int ticks)
+{
+	dap_jtagtap_tdi_tdo_seq(DO, (final_tms), NULL, DI, ticks);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("jtagtap_tdi_tdo_seq %d, %02x-> %02x\n", ticks, DI[0], DO[0]);
+}
+
+static void  cmsis_dap_jtagtap_tdi_seq(const uint8_t final_tms,
+									   const uint8_t *DI, int ticks)
+{
+	dap_jtagtap_tdi_tdo_seq(NULL, (final_tms), NULL, DI, ticks);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("jtagtap_tdi_seq %d, %02x\n", ticks, DI[0]);
+}
+
+static uint8_t cmsis_dap_jtagtap_next(uint8_t dTMS, uint8_t dTDI)
+{
+	uint8_t tdo[1];
+	dap_jtagtap_tdi_tdo_seq(tdo, false, &dTMS, &dTDI, 1);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("next tms %02x tdi %02x tdo %02x\n", dTMS, dTDI, tdo[0]);
+	return (tdo[0] & 1);
+}
+
+int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc)
+{
+	if (cl_debuglevel)
+		printf("jtap_init\n");
+	if (!(dap_caps & DAP_CAP_JTAG))
+		return -1;
+	mode =  DAP_CAP_JTAG;
+	dap_disconnect();
+	dap_connect(true);
+	dap_swj_clock(2000000);
+	jtag_proc->jtagtap_reset       = cmsis_dap_jtagtap_reset;
+	jtag_proc->jtagtap_next        = cmsis_dap_jtagtap_next;
+	jtag_proc->jtagtap_tms_seq     = cmsis_dap_jtagtap_tms_seq;
+	jtag_proc->jtagtap_tdi_tdo_seq = cmsis_dap_jtagtap_tdi_tdo_seq;
+	jtag_proc->jtagtap_tdi_seq     = cmsis_dap_jtagtap_tdi_seq;
+	dap_reset_link(true);
+	return 0;
+}
+
+int dap_jtag_dp_init(ADIv5_DP_t *dp)
+{
+	dp->dp_read = dap_dp_read_reg;
+	dp->error = dap_dp_error;
+	dp->low_access = dap_dp_low_access;
+	dp->abort = dap_dp_abort;
+
+	return true;
+}
diff --git a/src/platforms/hosted/cmsis_dap.h b/src/platforms/hosted/cmsis_dap.h
new file mode 100644
index 0000000..10b1969
--- /dev/null
+++ b/src/platforms/hosted/cmsis_dap.h
@@ -0,0 +1,50 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2019 Uwe Bonnes
+ *
+ * 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/>.
+ */
+#if !defined(__CMSIS_DAP_H_)
+#define __CMSIS_DAP_H_
+
+#include "adiv5.h"
+#include "cl_utils.h"
+
+#if defined(CMSIS_DAP)
+int dap_init(bmp_info_t *info);
+int dap_enter_debug_swd(ADIv5_DP_t *dp);
+void dap_exit_function(void);
+void dap_adiv5_dp_defaults(ADIv5_DP_t *dp);
+int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc);
+int dap_jtag_dp_init(ADIv5_DP_t *dp);
+#else
+int dap_init(bmp_info_t *info) {(void)info; return -1;}
+int dap_enter_debug_swd(ADIv5_DP_t *dp) {(void)dp; return -1;}
+void dap_exit_function(void) {return;};
+void dap_adiv5_dp_defaults(ADIv5_DP_t *dp) {(void)dp; return; }
+int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc)
+{
+	(void)jtag_proc;
+	return -1;
+}
+int dap_jtag_dp_init(ADIv5_DP_t *dp)
+{
+	(void)dp;
+	return -1;
+}
+
+#endif
+
+#endif
diff --git a/src/platforms/hosted/dap.c b/src/platforms/hosted/dap.c
new file mode 100644
index 0000000..5f32dc8
--- /dev/null
+++ b/src/platforms/hosted/dap.c
@@ -0,0 +1,753 @@
+/*
+ * Copyright (c) 2013-2015, Alex Taradov <alex@taradov.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Modified for Blackmagic Probe
+ * Copyright (c) 2020 Uwe Bonnes bon@elektron.ikp.physik.tu-darmstadt.de
+ */
+
+/*- Includes ----------------------------------------------------------------*/
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include "dap.h"
+#include "jtag_scan.h"
+
+/*- Definitions -------------------------------------------------------------*/
+enum
+{
+	ID_DAP_INFO               = 0x00,
+	ID_DAP_LED                = 0x01,
+	ID_DAP_CONNECT            = 0x02,
+	ID_DAP_DISCONNECT         = 0x03,
+	ID_DAP_TRANSFER_CONFIGURE = 0x04,
+	ID_DAP_TRANSFER           = 0x05,
+	ID_DAP_TRANSFER_BLOCK     = 0x06,
+	ID_DAP_TRANSFER_ABORT     = 0x07,
+	ID_DAP_WRITE_ABORT        = 0x08,
+	ID_DAP_DELAY              = 0x09,
+	ID_DAP_RESET_TARGET       = 0x0a,
+	ID_DAP_SWJ_PINS           = 0x10,
+	ID_DAP_SWJ_CLOCK          = 0x11,
+	ID_DAP_SWJ_SEQUENCE       = 0x12,
+	ID_DAP_SWD_CONFIGURE      = 0x13,
+	ID_DAP_JTAG_SEQUENCE      = 0x14,
+	ID_DAP_JTAG_CONFIGURE     = 0x15,
+	ID_DAP_JTAG_IDCODE        = 0x16,
+};
+
+enum
+{
+	DAP_TRANSFER_APnDP        = 1 << 0,
+	DAP_TRANSFER_RnW          = 1 << 1,
+	DAP_TRANSFER_A2           = 1 << 2,
+	DAP_TRANSFER_A3           = 1 << 3,
+	DAP_TRANSFER_MATCH_VALUE  = 1 << 4,
+	DAP_TRANSFER_MATCH_MASK   = 1 << 5,
+};
+
+enum
+{
+	DAP_TRANSFER_INVALID      = 0,
+	DAP_TRANSFER_OK           = 1 << 0,
+	DAP_TRANSFER_WAIT         = 1 << 1,
+	DAP_TRANSFER_FAULT        = 1 << 2,
+	DAP_TRANSFER_ERROR        = 1 << 3,
+	DAP_TRANSFER_MISMATCH     = 1 << 4,
+	DAP_TRANSFER_NO_TARGET    = 7,
+};
+
+enum
+{
+	DAP_SWJ_SWCLK_TCK = 1 << 0,
+	DAP_SWJ_SWDIO_TMS = 1 << 1,
+	DAP_SWJ_TDI       = 1 << 2,
+	DAP_SWJ_TDO       = 1 << 3,
+	DAP_SWJ_nTRST     = 1 << 5,
+	DAP_SWJ_nRESET    = 1 << 7,
+};
+
+enum
+{
+	DAP_OK    = 0x00,
+	DAP_ERROR = 0xff,
+};
+
+enum
+{
+	DAP_JTAG_TMS         = 1 << 6,
+	DAP_JTAG_TDO_CAPTURE = 1 << 7,
+};
+
+enum
+{
+	SWD_DP_R_IDCODE    = 0x00,
+	SWD_DP_W_ABORT     = 0x00,
+	SWD_DP_R_CTRL_STAT = 0x04,
+	SWD_DP_W_CTRL_STAT = 0x04, // When CTRLSEL == 0
+	SWD_DP_W_WCR       = 0x04, // When CTRLSEL == 1
+	SWD_DP_R_RESEND    = 0x08,
+	SWD_DP_W_SELECT    = 0x08,
+	SWD_DP_R_RDBUFF    = 0x0c,
+};
+
+enum
+{
+	SWD_AP_CSW  = 0x00 | DAP_TRANSFER_APnDP,
+	SWD_AP_TAR  = 0x04 | DAP_TRANSFER_APnDP,
+	SWD_AP_DRW  = 0x0c | DAP_TRANSFER_APnDP,
+
+	SWD_AP_DB0  = 0x00 | DAP_TRANSFER_APnDP, // 0x10
+	SWD_AP_DB1  = 0x04 | DAP_TRANSFER_APnDP, // 0x14
+	SWD_AP_DB2  = 0x08 | DAP_TRANSFER_APnDP, // 0x18
+	SWD_AP_DB3  = 0x0c | DAP_TRANSFER_APnDP, // 0x1c
+
+	SWD_AP_CFG  = 0x04 | DAP_TRANSFER_APnDP, // 0xf4
+	SWD_AP_BASE = 0x08 | DAP_TRANSFER_APnDP, // 0xf8
+	SWD_AP_IDR  = 0x0c | DAP_TRANSFER_APnDP, // 0xfc
+};
+
+#define DP_ABORT_DAPABORT      (1 << 0)
+#define DP_ABORT_STKCMPCLR     (1 << 1)
+#define DP_ABORT_STKERRCLR     (1 << 2)
+#define DP_ABORT_WDERRCLR      (1 << 3)
+#define DP_ABORT_ORUNERRCLR    (1 << 4)
+
+#define DP_CST_ORUNDETECT      (1 << 0)
+#define DP_CST_STICKYORUN      (1 << 1)
+#define DP_CST_TRNMODE_NORMAL  (0 << 2)
+#define DP_CST_TRNMODE_VERIFY  (1 << 2)
+#define DP_CST_TRNMODE_COMPARE (2 << 2)
+#define DP_CST_STICKYCMP       (1 << 4)
+#define DP_CST_STICKYERR       (1 << 5)
+#define DP_CST_READOK          (1 << 6)
+#define DP_CST_WDATAERR        (1 << 7)
+#define DP_CST_MASKLANE(x)     ((x) << 8)
+#define DP_CST_TRNCNT(x)       ((x) << 12)
+#define DP_CST_CDBGRSTREQ      (1 << 26)
+#define DP_CST_CDBGRSTACK      (1 << 27)
+#define DP_CST_CDBGPWRUPREQ    (1 << 28)
+#define DP_CST_CDBGPWRUPACK    (1 << 29)
+#define DP_CST_CSYSPWRUPREQ    (1 << 30)
+#define DP_CST_CSYSPWRUPACK    (1 << 31)
+
+#define DP_SELECT_CTRLSEL      (1 << 0)
+#define DP_SELECT_APBANKSEL(x) ((x) << 4)
+#define DP_SELECT_APSEL(x)     ((x) << 24)
+
+#define AP_CSW_SIZE_BYTE       (0 << 0)
+#define AP_CSW_SIZE_HALF       (1 << 0)
+#define AP_CSW_SIZE_WORD       (2 << 0)
+#define AP_CSW_ADDRINC_OFF     (0 << 4)
+#define AP_CSW_ADDRINC_SINGLE  (1 << 4)
+#define AP_CSW_ADDRINC_PACKED  (2 << 4)
+#define AP_CSW_DEVICEEN        (1 << 6)
+#define AP_CSW_TRINPROG        (1 << 7)
+#define AP_CSW_SPIDEN          (1 << 23)
+#define AP_CSW_PROT(x)         ((x) << 24)
+#define AP_CSW_DBGSWENABLE     (1 << 31)
+
+/*- Implementations ---------------------------------------------------------*/
+
+//-----------------------------------------------------------------------------
+void dap_led(int index, int state)
+{
+	uint8_t buf[3];
+
+	buf[0] = ID_DAP_LED;
+	buf[1] = index;
+	buf[2] = state;
+	dbg_dap_cmd(buf, sizeof(buf), 3);
+}
+
+//-----------------------------------------------------------------------------
+void dap_connect(bool jtag)
+{
+	uint8_t buf[2];
+
+	buf[0] = ID_DAP_CONNECT;
+	buf[1] = (jtag) ? DAP_CAP_JTAG : DAP_CAP_SWD;
+	dbg_dap_cmd(buf, sizeof(buf), 2);
+
+}
+
+//-----------------------------------------------------------------------------
+void dap_disconnect(void)
+{
+	uint8_t buf[1];
+
+	buf[0] = ID_DAP_DISCONNECT;
+	dbg_dap_cmd(buf, sizeof(buf), 1);
+}
+
+//-----------------------------------------------------------------------------
+void dap_swj_clock(uint32_t clock)
+{
+	uint8_t buf[5];
+
+	buf[0] = ID_DAP_SWJ_CLOCK;
+	buf[1] = clock & 0xff;
+	buf[2] = (clock >> 8) & 0xff;
+	buf[3] = (clock >> 16) & 0xff;
+	buf[4] = (clock >> 24) & 0xff;
+	dbg_dap_cmd(buf, sizeof(buf), 5);
+
+}
+
+//-----------------------------------------------------------------------------
+void dap_transfer_configure(uint8_t idle, uint16_t count, uint16_t retry)
+{
+	uint8_t buf[6];
+
+	buf[0] = ID_DAP_TRANSFER_CONFIGURE;
+	buf[1] = idle;
+	buf[2] = count & 0xff;
+	buf[3] = (count >> 8) & 0xff;
+	buf[4] = retry & 0xff;
+	buf[5] = (retry >> 8) & 0xff;
+	dbg_dap_cmd(buf, sizeof(buf), 6);
+}
+
+//-----------------------------------------------------------------------------
+void dap_swd_configure(uint8_t cfg)
+{
+	uint8_t buf[2];
+
+	buf[0] = ID_DAP_SWD_CONFIGURE;
+	buf[1] = cfg;
+	dbg_dap_cmd(buf, sizeof(buf), 2);
+}
+
+//-----------------------------------------------------------------------------
+int dap_info(int info, uint8_t *data, int size)
+{
+	uint8_t buf[256];
+	int rsize;
+
+	buf[0] = ID_DAP_INFO;
+	buf[1] = info;
+	dbg_dap_cmd(buf, sizeof(buf), 2);
+
+	rsize = (size < buf[0]) ? size : buf[0];
+	memcpy(data, &buf[1], rsize);
+
+	if (rsize < size)
+		data[rsize] = 0;
+
+	return rsize;
+}
+
+void dap_reset_pin(int state)
+{
+	uint8_t buf[7];
+
+	buf[0] = ID_DAP_SWJ_PINS;
+	buf[1] = state ? DAP_SWJ_nRESET : 0; // Value
+	buf[2] = DAP_SWJ_nRESET; // Select
+	buf[3] = 0; // Wait
+	buf[4] = 0;
+	buf[5] = 0;
+	buf[6] = 0;
+	dbg_dap_cmd(buf, sizeof(buf), 7);
+}
+
+void dap_trst_reset(void)
+{
+	uint8_t buf[7];
+
+	buf[0] = ID_DAP_SWJ_PINS;
+	buf[1] = DAP_SWJ_nTRST;
+	buf[2] = 0;
+	buf[3] = 0;
+	buf[4] = 4; /* ~ 1 ms*/
+	buf[5] = 0;
+	buf[6] = 0;
+	dbg_dap_cmd(buf, sizeof(buf), 7);
+
+	buf[0] = ID_DAP_SWJ_PINS;
+	buf[1] = DAP_SWJ_nTRST;
+	buf[2] = DAP_SWJ_nTRST;
+	dbg_dap_cmd(buf, sizeof(buf), 7);
+}
+
+static void dap_line_reset(void)
+{
+	uint8_t buf[] = {
+		ID_DAP_SWJ_SEQUENCE,
+		64,
+		0xff,
+		0xff,
+		0xff,
+		0xff,
+		0xff,
+		0xff,
+		0xff,
+		0
+	};
+	dbg_dap_cmd(buf, sizeof(buf), 10);
+	if (buf[0])
+		printf("line reset failed\n");
+}
+
+static uint32_t wait_word(uint8_t *buf, int size, int len, uint8_t *dp_fault)
+{
+	do {
+		dbg_dap_cmd(buf, size, len);
+		if (buf[1] < DAP_TRANSFER_WAIT)
+			break;
+	} while (buf[1] == DAP_TRANSFER_WAIT);
+
+	if (buf[1] > DAP_TRANSFER_WAIT) {
+//	  printf("dap_read_reg fault\n");
+		*dp_fault = 1;
+	}
+	if (buf[1] == DAP_TRANSFER_ERROR) {
+		printf("dap_read_reg, protocoll error\n");
+		dap_line_reset();
+	}
+	uint32_t res =
+		((uint32_t)buf[5] << 24) | ((uint32_t)buf[4] << 16) |
+		((uint32_t)buf[3] << 8) | (uint32_t)buf[2];
+	return res;
+}
+
+//-----------------------------------------------------------------------------
+uint32_t dap_read_reg(ADIv5_DP_t *dp, uint8_t reg)
+{
+	uint8_t buf[8];
+	uint8_t dap_index = 0;
+	if (dp->dev)
+		dap_index = dp->dev->dev;
+	buf[0] = ID_DAP_TRANSFER;
+	buf[1] = dap_index;
+	buf[2] = 0x01; // Request size
+	buf[3] = reg | DAP_TRANSFER_RnW;
+	uint32_t res = wait_word(buf, 8, 4, &dp->fault);
+//  printf("\tdap_read_reg %02x %08x\n", reg, res);
+	return res;
+}
+
+//-----------------------------------------------------------------------------
+void dap_write_reg(ADIv5_DP_t *dp, uint8_t reg, uint32_t data)
+{
+	uint8_t buf[8];
+	// printf("\tdap_write_reg %02x %08x\n", reg, data);
+
+	buf[0] = ID_DAP_TRANSFER;
+	uint8_t dap_index = 0;
+	if (dp->dev)
+		dap_index = dp->dev->dev;
+	buf[1] = dap_index;
+	buf[2] = 0x01; // Request size
+	buf[3] = reg & ~DAP_TRANSFER_RnW;;
+	buf[4] = data & 0xff;
+	buf[5] = (data >> 8) & 0xff;
+	buf[6] = (data >> 16) & 0xff;
+	buf[7] = (data >> 24) & 0xff;
+	do {
+		dbg_dap_cmd(buf, sizeof(buf), 8);
+		if (buf[1] < DAP_TRANSFER_WAIT)
+			break;
+	} while (buf[1] == DAP_TRANSFER_WAIT);
+
+	if (buf[1] > DAP_TRANSFER_WAIT) {
+//	  printf("dap_write_reg %02x data %08x:fault\n", reg, data);
+		dp->fault = 1;
+	}
+	if (buf[1] == DAP_TRANSFER_ERROR) {
+//	  printf("dap_write_reg %02x data %08x: protocoll error\n", reg, data);
+		dap_line_reset();
+	}
+}
+
+unsigned int dap_read_block(ADIv5_AP_t *ap, void *dest, uint32_t src,
+							size_t len, enum align align)
+{
+	uint8_t buf[1024];
+	unsigned int sz = len >> align;
+	uint8_t dap_index = 0;
+	if (ap->dp->dev)
+		dap_index = ap->dp->dev->dev;
+    buf[0] = ID_DAP_TRANSFER_BLOCK;
+    buf[1] = dap_index;
+    buf[2] =  sz & 0xff;
+    buf[3] = (sz >> 8) & 0xff;
+    buf[4] = SWD_AP_DRW | DAP_TRANSFER_RnW;
+    dbg_dap_cmd(buf, 1023, 5 + 1);
+	unsigned int transferred = buf[0] + (buf[1] << 8);
+	if (buf[2] > DAP_TRANSFER_FAULT) {
+		printf("line_reset\n");
+		dap_line_reset();
+	}
+	if (sz != transferred) {
+		return 1;
+	} else if (align > ALIGN_HALFWORD) {
+		memcpy(dest, &buf[3], len);
+	} else {
+		uint32_t *p = (uint32_t *)&buf[3];
+		while(sz) {
+			dest = extract(dest, src, *p, align);
+			p++;
+			src  += (1 << align);
+			dest += (1 << align);
+			sz--;
+		}
+	}
+	return (buf[2] > DAP_TRANSFER_WAIT) ? 1 : 0;
+}
+
+unsigned int dap_write_block(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+							 size_t len, enum align align)
+{
+	uint8_t buf[1024];
+	unsigned int sz = len >> align;
+	uint8_t dap_index = 0;
+	if (ap->dp->dev)
+		dap_index = ap->dp->dev->dev;
+    buf[0] = ID_DAP_TRANSFER_BLOCK;
+    buf[1] = dap_index;
+    buf[2] =  sz & 0xff;
+    buf[3] = (sz >> 8) & 0xff;
+    buf[4] = SWD_AP_DRW;
+	if (align > ALIGN_HALFWORD) {
+		memcpy(&buf[5], src, len);
+	} else {
+		unsigned int size = len;
+		uint32_t *p = (uint32_t *)&buf[5];
+		while (size) {
+			uint32_t tmp = 0;
+			/* Pack data into correct data lane */
+			if (align == ALIGN_BYTE) {
+				tmp = ((uint32_t)*(uint8_t  *)src) << ((dest & 3) << 3);
+			} else {
+				tmp = ((uint32_t)*(uint16_t *)src) << ((dest & 2) << 3);
+			}
+			src = src + (1 << align);
+			dest += (1 << align);
+			size--;
+			*p++ = tmp;
+		}
+	}
+	dbg_dap_cmd(buf, 1023, 5 + (sz << 2));
+	if (buf[2] > DAP_TRANSFER_FAULT) {
+		dap_line_reset();
+	}
+	return (buf[2] > DAP_TRANSFER_WAIT) ? 1 : 0;
+}
+
+//-----------------------------------------------------------------------------
+void dap_reset_link(bool jtag)
+{
+	uint8_t buf[128], *p = buf;
+
+	//-------------
+	*p++ = ID_DAP_SWJ_SEQUENCE;
+	p++;
+	*p++ = 0xff;
+	*p++ = 0xff;
+	*p++ = 0xff;
+	*p++ = 0xff;
+	*p++ = 0xff;
+	*p++ = 0xff;
+	*p++ = 0xff;
+	if (jtag) {
+		*p++ = 0x3c;
+		*p++ = 0xe7;
+		*p++ = 0x1f;
+		buf[1] = ((p - &buf[2]) * 8) - 2;
+	} else {
+		*p++ = 0x9e;
+		*p++ = 0xe7;
+		*p++ = 0xff;
+		*p++ = 0xff;
+		*p++ = 0xff;
+		*p++ = 0xff;
+		*p++ = 0xff;
+		*p++ = 0xff;
+		*p++ = 0xff;
+		*p++ = 0x00;
+		buf[1] = (p - &buf[2]) * 8;
+	}
+	dbg_dap_cmd(buf, sizeof(buf), p - buf);
+
+	if (!jtag) {
+		//-------------
+		buf[0] = ID_DAP_TRANSFER;
+		buf[1] = 0; // DAP index
+		buf[2] = 1; // Request size
+		buf[3] = SWD_DP_R_IDCODE | DAP_TRANSFER_RnW;
+		dbg_dap_cmd(buf, sizeof(buf), 4);
+	}
+}
+
+//-----------------------------------------------------------------------------
+uint32_t dap_read_idcode(ADIv5_DP_t *dp)
+{
+	return dap_read_reg(dp, SWD_DP_R_IDCODE);
+}
+
+static uint8_t *mem_access_setup(ADIv5_AP_t *ap, uint8_t *p,
+								 uint32_t addr, enum align align)
+{
+	uint32_t csw = ap->csw | ADIV5_AP_CSW_ADDRINC_SINGLE;
+	switch (align) {
+	case ALIGN_BYTE:
+		csw |= ADIV5_AP_CSW_SIZE_BYTE;
+		break;
+	case ALIGN_HALFWORD:
+		csw |= ADIV5_AP_CSW_SIZE_HALFWORD;
+		break;
+	case ALIGN_DWORD:
+	case ALIGN_WORD:
+		csw |= ADIV5_AP_CSW_SIZE_WORD;
+		break;
+	}
+	uint8_t dap_index = 0;
+	if (ap->dp->dev)
+		dap_index = ap->dp->dev->dev;
+	*p++ = ID_DAP_TRANSFER;
+	*p++ = dap_index;
+	*p++ = 3; /* Nr transfers */
+	*p++ = SWD_DP_W_SELECT;
+	*p++ = ADIV5_AP_CSW & 0xF0;
+	*p++ = 0;
+	*p++ = 0;
+	*p++ = ap->apsel & 0xff;
+	*p++ = SWD_AP_CSW;
+	*p++ = (csw >>  0) & 0xff;
+	*p++ = (csw >>  8) & 0xff;
+	*p++ = (csw >> 16) & 0xff;
+	*p++ = (csw >> 24) & 0xff;
+	*p++ = SWD_AP_TAR ;
+	*p++ = (addr >>  0) & 0xff;
+	*p++ = (addr >>  8) & 0xff;
+	*p++ = (addr >> 16) & 0xff;
+	*p++ = (addr >> 24) & 0xff;
+	return p;
+}
+
+void dap_ap_mem_access_setup(ADIv5_AP_t *ap, uint32_t addr, enum align align)
+{
+	uint8_t buf[63];
+	uint8_t *p = mem_access_setup(ap, buf, addr, align);
+	dbg_dap_cmd(buf, sizeof(buf), p - buf);
+}
+
+uint32_t dap_ap_read(ADIv5_AP_t *ap, uint16_t addr)
+{
+//	printf("dap_ap_read_start\n");
+	uint8_t buf[63], *p = buf;
+	buf[0] = ID_DAP_TRANSFER;
+	uint8_t dap_index = 0;
+	if (ap->dp->dev)
+		dap_index = ap->dp->dev->dev;
+	*p++ = ID_DAP_TRANSFER;
+	*p++ = dap_index;
+	*p++ = 2; /* Nr transfers */
+	*p++ = SWD_DP_W_SELECT;
+	*p++ = (addr & 0xF0);
+	*p++ = 0;
+	*p++ = 0;
+	*p++ = ap->apsel & 0xff;
+	*p++ = (addr & 0x0c) | DAP_TRANSFER_RnW  |
+		((addr & 0x100) ?  DAP_TRANSFER_APnDP : 0);
+	uint32_t res = wait_word(buf, 63, p - buf, &ap->dp->fault);
+	return res;
+}
+
+void dap_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+{
+//	printf("dap_ap_write addr %04x value %08x\n", addr, value);
+	uint8_t buf[63], *p = buf;
+	uint8_t dap_index = 0;
+	if (ap->dp->dev)
+		dap_index = ap->dp->dev->dev;
+	*p++ = ID_DAP_TRANSFER;
+	*p++ = dap_index;
+	*p++ = 2; /* Nr transfers */
+	*p++ = SWD_DP_W_SELECT;
+	*p++ = (addr & 0xF0);
+	*p++ = 0;
+	*p++ = 0;
+	*p++ = ap->apsel & 0xff;
+	*p++ = (addr & 0x0c) | ((addr & 0x100) ?  DAP_TRANSFER_APnDP : 0);
+	*p++ = (value >>  0) & 0xff;
+	*p++ = (value >>  8) & 0xff;
+	*p++ = (value >> 16) & 0xff;
+	*p++ = (value >> 24) & 0xff;
+	dbg_dap_cmd(buf, sizeof(buf), p - buf);
+//	printf("dap_ap_write done\n");
+}
+
+void dap_read_single(ADIv5_AP_t *ap, void *dest, uint32_t src, enum align align)
+{
+	uint8_t buf[63];
+	uint8_t *p = mem_access_setup(ap, buf, src, align);
+	*p++ = SWD_AP_DRW | DAP_TRANSFER_RnW;
+	buf[2] = 4;
+	uint32_t tmp = wait_word(buf, 63, p - buf, &ap->dp->fault);
+	dest = extract(dest, src, tmp, align);
+}
+
+void dap_write_single(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+					  enum align align)
+{
+	uint8_t buf[63];
+	uint8_t *p = mem_access_setup(ap, buf, dest, align);
+	*p++ = SWD_AP_DRW;
+	uint32_t tmp = 0;
+	/* Pack data into correct data lane */
+	switch (align) {
+	case ALIGN_BYTE:
+		tmp = ((uint32_t)*(uint8_t *)src) << ((dest & 3) << 3);
+		break;
+	case ALIGN_HALFWORD:
+		tmp = ((uint32_t)*(uint16_t *)src) << ((dest & 2) << 3);
+		break;
+	case ALIGN_DWORD:
+	case ALIGN_WORD:
+		tmp = *(uint32_t *)src;
+		break;
+	}
+	*p++ = (tmp >>  0) & 0xff;
+	*p++ = (tmp >>  8) & 0xff;
+	*p++ = (tmp >> 16) & 0xff;
+	*p++ = (tmp >> 24) & 0xff;
+	buf[2] = 4;
+	dbg_dap_cmd(buf, sizeof(buf), p - buf);
+}
+
+void dap_jtagtap_tdi_tdo_seq(uint8_t *DO, bool final_tms, const uint8_t *TMS,
+							 const uint8_t *DI, int ticks)
+{
+	uint8_t buf[64];
+	if (!TMS) {
+		int last_byte = 0;
+		int last_bit = 0;
+		if (final_tms) {
+			last_byte = ticks >> 3;
+			last_bit = ticks & 7;
+			ticks --;
+		}
+		while (ticks) {
+			int transfers = ticks;
+			if (transfers > 64)
+				transfers = 64;
+			uint8_t *p = buf;
+			*p++ = ID_DAP_JTAG_SEQUENCE;
+			*p++ = 1;
+			*p++ = transfers | ((DO) ? DAP_JTAG_TDO_CAPTURE : 0);
+			int n_di_bytes = (transfers + 7) >> 3;
+			if (DI) {
+				p = memcpy(p, DI, n_di_bytes);
+				DI += n_di_bytes;
+			} else {
+				p = memset(p, 0xff, n_di_bytes);
+			}
+			p += n_di_bytes;
+			dbg_dap_cmd(buf, sizeof(buf), p - buf);
+			if (buf[0] != DAP_OK)
+				printf("Failed %02x\n", buf[0]);
+			if (DO) {
+				memcpy(DO, &buf[1], (transfers + 7) >> 3);
+				DO += (transfers + 7) >> 3;
+			}
+			ticks -= transfers;
+		}
+		if (final_tms) {
+			uint8_t *p = buf;
+			*p++ = ID_DAP_JTAG_SEQUENCE;
+			*p++ = 1;
+			*p++ = 1 | ((DO) ? DAP_JTAG_TDO_CAPTURE : 0) | DAP_JTAG_TMS;
+			if (DI) {
+				*p++ = ((DI[last_byte] & (1 << last_bit)) ? 1 : 0);
+			} else {
+				*p++ = 0;
+			}
+			dbg_dap_cmd(buf, sizeof(buf), p - buf);
+			if (buf[0] == DAP_ERROR)
+				printf("Failed %02x\n", buf[0]);
+			if (DO) {
+				if (buf[1] & 1)
+					DO[last_byte] |= (1 << last_bit);
+				else
+					DO[last_byte] &= ~(1 << last_bit);
+			}
+		}
+	} else {
+		while(ticks) {
+			uint8_t *p = buf;
+			int transfers = ticks;
+			if (transfers > 64)
+				transfers = 64;
+			p = buf;
+			*p++ = ID_DAP_JTAG_SEQUENCE;
+			*p++ = transfers;
+			for (int i = 0; i < transfers; i++) {
+				*p++ = 1 | ((DO) ? DAP_JTAG_TDO_CAPTURE : 0) |
+					((TMS[i >> 8] & (1 << (i & 7))) ? DAP_JTAG_TMS : 0);
+				if (DI)
+					*p++ = (DI[i >> 8] & (1 << (i & 7))) ? 1 : 0;
+				else
+					*p++ = 0x55;
+			}
+			dbg_dap_cmd(buf, sizeof(buf), p - buf);
+			if (buf[0] == DAP_ERROR)
+				printf("Failed %02x\n", buf[0]);
+			if (DO) {
+				for (int i = 0; i < transfers; i++) {
+					if (buf[i + 1])
+						DO[i >> 8] |= (1 << (i & 7));
+					else
+						DO[i >> 8] &= ~(1 << (i & 7));
+				}
+			}
+			ticks -= transfers;
+		}
+	}
+}
+
+int dap_jtag_configure(void)
+{
+	uint8_t buf[64], *p = &buf[2];
+	int i = 0;
+	for (; i < jtag_dev_count; i++) {
+		struct jtag_dev_s *jtag_dev = &jtag_devs[i];
+		*p++ = jtag_dev->ir_len;
+		printf("irlen %d\n", jtag_dev->ir_len);
+	}
+	if ((!i || i >= JTAG_MAX_DEVS))
+		return -1;
+	buf[0] = 0x15;
+	buf[1] = i;
+	dbg_dap_cmd(buf, sizeof(buf), p - buf);
+	if (buf[0] != DAP_OK)
+		printf("dap_jtag_configure Failed %02x\n", buf[0]);
+	return 0;
+}
diff --git a/src/platforms/hosted/dap.h b/src/platforms/hosted/dap.h
new file mode 100644
index 0000000..6a6fbb5
--- /dev/null
+++ b/src/platforms/hosted/dap.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2013-2015, Alex Taradov <alex@taradov.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _DAP_H_
+#define _DAP_H_
+
+/*- Includes ----------------------------------------------------------------*/
+#include <stdint.h>
+#include <stdbool.h>
+#include "adiv5.h"
+
+/*- Definitions -------------------------------------------------------------*/
+enum
+{
+  DAP_INFO_VENDOR        = 0x01,
+  DAP_INFO_PRODUCT       = 0x02,
+  DAP_INFO_SER_NUM       = 0x03,
+  DAP_INFO_FW_VER        = 0x04,
+  DAP_INFO_DEVICE_VENDOR = 0x05,
+  DAP_INFO_DEVICE_NAME   = 0x06,
+  DAP_INFO_CAPABILITIES  = 0xf0,
+  DAP_INFO_TDT           = 0xf1,
+  DAP_INFO_SWO_BUF_SIZE  = 0xfd,
+  DAP_INFO_PACKET_COUNT  = 0xfe,
+  DAP_INFO_PACKET_SIZE   = 0xff,
+};
+
+enum
+{
+  DAP_CAP_SWD            = (1 << 0),
+  DAP_CAP_JTAG           = (1 << 1),
+  DAP_CAP_SWO_UART       = (1 << 2),
+  DAP_CAP_SWO_MANCHESTER = (1 << 3),
+  DAP_CAP_ATOMIC_CMD     = (1 << 4),
+  DAP_CAP_TDT            = (1 << 5),
+  DAP_CAP_SWO_STREAMING  = (1 << 6),
+};
+
+/*- Prototypes --------------------------------------------------------------*/
+void dap_led(int index, int state);
+void dap_connect(bool jtag);
+void dap_disconnect(void);
+void dap_swj_clock(uint32_t clock);
+void dap_transfer_configure(uint8_t idle, uint16_t count, uint16_t retry);
+void dap_swd_configure(uint8_t cfg);
+int dap_info(int info, uint8_t *data, int size);
+void dap_reset_target(void);
+void dap_trst_reset(void);
+void dap_reset_target_hw(int state);
+void dap_reset_pin(int state);
+uint32_t dap_read_reg(ADIv5_DP_t *dp, uint8_t reg);
+void dap_write_reg(ADIv5_DP_t *dp, uint8_t reg, uint32_t data);
+void dap_reset_link(bool jtag);
+uint32_t dap_read_idcode(ADIv5_DP_t *dp);
+unsigned int dap_read_block(ADIv5_AP_t *ap, void *dest, uint32_t src,
+							size_t len,	enum align align);
+unsigned int dap_write_block(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+							 size_t len, enum align align);
+void dap_ap_mem_access_setup(ADIv5_AP_t *ap, uint32_t addr, enum align align);
+uint32_t dap_ap_read(ADIv5_AP_t *ap, uint16_t addr);
+void dap_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
+void dap_read_single(ADIv5_AP_t *ap, void *dest, uint32_t src, enum align align);
+void dap_write_single(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+					  enum align align);
+int dbg_dap_cmd(uint8_t *data, int size, int rsize);
+void dap_jtagtap_tdi_tdo_seq(uint8_t *DO, bool final_tms, const uint8_t *TMS,
+							 const uint8_t *DI, int ticks);
+int dap_jtag_configure(void);
+#endif // _DAP_H_
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 75df055..28bfd35 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -36,6 +36,7 @@
 #include "stlinkv2.h"
 #include "ftdi_bmp.h"
 #include "jlink.h"
+#include "cmsis_dap.h"
 
 #define VENDOR_ID_BMP            0x1d50
 #define PRODUCT_ID_BMP           0x6018
@@ -69,6 +70,9 @@ static void exit_function(void)
 		}
 	}
 	switch (info.bmp_type) {
+	case BMP_TYPE_CMSIS_DAP:
+		dap_exit_function();
+		break;
 	default:
 		break;
 	}
@@ -169,6 +173,8 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 					fprintf(stderr, "INFO: STLINKV1 not supported\n");
 				continue;
 			}
+		} else if ((strstr(manufacturer, "CMSIS")) || (strstr(product, "CMSIS"))) {
+			type = BMP_TYPE_CMSIS_DAP;
 		} else if (desc.idVendor ==  VENDOR_ID_SEGGER) {
 			type = BMP_TYPE_JLINK;
 		} else{
@@ -244,6 +250,10 @@ void platform_init(int argc, char **argv)
 		if (stlink_init( &info))
 			exit(-1);
 		break;
+	case BMP_TYPE_CMSIS_DAP:
+		if (dap_init( &info))
+			exit(-1);
+		break;
 	case BMP_TYPE_LIBFTDI:
 		break;
 	case BMP_TYPE_JLINK:
@@ -282,6 +292,18 @@ int platform_adiv5_swdp_scan(void)
 		free(dp);
 		break;
 	}
+	case BMP_TYPE_CMSIS_DAP:
+	{
+		target_list_free();
+		ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
+		if (!dap_enter_debug_swd(dp)) {
+			adiv5_dp_init(dp);
+			if (target_list)
+				return 1;
+		}
+		free(dp);
+		break;
+	}
 	case BMP_TYPE_JLINK:
 		return jlink_swdp_scan(&info);
 	default:
@@ -296,6 +318,7 @@ int platform_swdptap_init(void)
 	case BMP_TYPE_BMP:
 		return remote_swdptap_init(&swd_proc);
 	case BMP_TYPE_STLINKV2:
+	case BMP_TYPE_CMSIS_DAP:
 	case BMP_TYPE_JLINK:
 		return 0;
 	case BMP_TYPE_LIBFTDI:
@@ -312,6 +335,7 @@ int platform_jtag_scan(const uint8_t *lrlens)
 	case BMP_TYPE_BMP:
 	case BMP_TYPE_LIBFTDI:
 	case BMP_TYPE_JLINK:
+	case BMP_TYPE_CMSIS_DAP:
 		return jtag_scan(lrlens);
 	case BMP_TYPE_STLINKV2:
 		return jtag_scan_stlinkv2(&info, lrlens);
@@ -332,6 +356,8 @@ int platform_jtagtap_init(void)
 		return libftdi_jtagtap_init(&jtag_proc);
 	case BMP_TYPE_JLINK:
 		return jlink_jtagtap_init(&info, &jtag_proc);
+	case BMP_TYPE_CMSIS_DAP:
+		return cmsis_dap_jtagtap_init(&jtag_proc);
 	default:
 		return -1;
 	}
@@ -343,6 +369,8 @@ void platform_adiv5_dp_defaults(ADIv5_DP_t *dp)
 	switch (info.bmp_type) {
 	case BMP_TYPE_STLINKV2:
 		return stlink_adiv5_dp_defaults(dp);
+	case BMP_TYPE_CMSIS_DAP:
+		return dap_adiv5_dp_defaults(dp);
 	default:
 		break;
 	}
@@ -357,6 +385,8 @@ int platform_jtag_dp_init(ADIv5_DP_t *dp)
 		return 0;
 	case BMP_TYPE_STLINKV2:
 		return stlink_jtag_dp_init(dp);
+	case BMP_TYPE_CMSIS_DAP:
+		return dap_jtag_dp_init(dp);
 	default:
 		return 0;
 	}
diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index 8719fe3..aedd14d 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -165,7 +165,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 			if (optarg)
 				cl_debuglevel = strtol(optarg, NULL, 0);
 			else
-				cl_debuglevel = -1;
+				cl_debuglevel = 1;
 			break;
 		case 'j':
 			opt->opt_usejtag = true;
diff --git a/src/platforms/pc/cl_utils.h b/src/platforms/pc/cl_utils.h
index 88bff7e..b65d0e6 100644
--- a/src/platforms/pc/cl_utils.h
+++ b/src/platforms/pc/cl_utils.h
@@ -36,6 +36,13 @@ enum bmp_cl_mode {
 	BMP_MODE_FLASH_VERIFY
 };
 
+enum BMP_DEBUG {
+	BMP_DEBUG_NONE = 0,
+	BMP_DEBUG_INFO = 1,
+	BMP_DEBUG_PLATFORM = 2,
+	BMP_DEBUG_WIRE = 4
+};
+
 typedef struct BMP_CL_OPTIONS_s {
 	enum bmp_cl_mode opt_mode;
 	bool opt_usejtag;

From 921c23eb7420c78d63525424396963e778ed8b8d Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 2 May 2020 16:35:25 +0200
Subject: [PATCH 35/43] serial_unix: Return instead of exit in case of error.

---
 src/platforms/pc/serial_unix.c | 42 +++++++++++++++++-----------------
 1 file changed, 21 insertions(+), 21 deletions(-)

diff --git a/src/platforms/pc/serial_unix.c b/src/platforms/pc/serial_unix.c
index fd63dc1..2af20ba 100644
--- a/src/platforms/pc/serial_unix.c
+++ b/src/platforms/pc/serial_unix.c
@@ -40,7 +40,7 @@ static int set_interface_attribs(void)
 	struct termios tty;
 	memset (&tty, 0, sizeof tty);
 	if (tcgetattr (fd, &tty) != 0) {
-      fprintf(stderr,"error %d from tcgetattr", errno);
+      printf("error %d from tcgetattr", errno);
       return -1;
     }
 
@@ -62,7 +62,7 @@ static int set_interface_attribs(void)
 	tty.c_cflag &= ~CRTSCTS;
 
 	if (tcsetattr (fd, TCSANOW, &tty) != 0) {
-		fprintf(stderr,"error %d from tcsetattr", errno);
+		printf("error %d from tcsetattr", errno);
 		return -1;
     }
 	return 0;
@@ -77,7 +77,7 @@ int serial_open(BMP_CL_OPTIONS_t *cl_opts, char *serial)
 		struct dirent *dp;
 		DIR *dir = opendir(DEVICE_BY_ID);
 		if (!dir) {
-			fprintf(stderr, "No serial device found\n");
+			printf("No serial device found\n");
 			return -1;
 		}
 		int num_devices = 0;
@@ -95,24 +95,24 @@ int serial_open(BMP_CL_OPTIONS_t *cl_opts, char *serial)
 		}
 		closedir(dir);
 		if ((num_devices == 0) && (num_total == 0)){
-			fprintf(stderr, "No BMP probe found\n");
+			printf("No BMP probe found\n");
 			return -1;
 		} else if (num_devices != 1) {
-			fprintf(stderr, "Available Probes:\n");
+			printf("Available Probes:\n");
 			dir = opendir(DEVICE_BY_ID);
 			if (dir) {
 				while ((dp = readdir(dir)) != NULL) {
 					if ((strstr(dp->d_name, BMP_IDSTRING)) &&
 						(strstr(dp->d_name, "-if00")))
-						fprintf(stderr, "%s\n", dp->d_name);
+						printf("%s\n", dp->d_name);
 				}
 				closedir(dir);
 				if (serial)
-					fprintf(stderr, "Do no match given serial \"%s\"\n", serial);
+					printf("Do no match given serial \"%s\"\n", serial);
 				else
-					fprintf(stderr, "Select Probe with -s <(Partial) Serial Number\n");
+					printf("Select Probe with -s <(Partial) Serial Number\n");
 			} else {
-				fprintf(stderr, "Could not opendir %s: %s\n", name, strerror(errno));
+				printf("Could not opendir %s: %s\n", name, strerror(errno));
 			}
 			return -1;
 		}
@@ -121,7 +121,7 @@ int serial_open(BMP_CL_OPTIONS_t *cl_opts, char *serial)
 	}
 	fd = open(name, O_RDWR | O_SYNC | O_NOCTTY);
 	if (fd < 0) {
-		fprintf(stderr,"Couldn't open serial port %s\n", name);
+		printf("Couldn't open serial port %s\n", name);
 		return -1;
     }
 	/* BMP only offers an USB-Serial connection with no real serial
@@ -143,8 +143,8 @@ int platform_buffer_write(const uint8_t *data, int size)
 		printf("%s\n",data);
 	s = write(fd, data, size);
 	if (s < 0) {
-		fprintf(stderr, "Failed to write\n");
-		exit(-2);
+		printf("Failed to write\n");
+		return(-2);
     }
 
 	return size;
@@ -170,12 +170,12 @@ int platform_buffer_read(uint8_t *data, int maxsize)
 
 		ret = select(fd + 1, &rset, NULL, NULL, &tv);
 		if (ret < 0) {
-			fprintf(stderr,"Failed on select\n");
-			exit(-4);
+			printf("Failed on select\n");
+			return(-3);
 		}
 		if(ret == 0) {
-			fprintf(stderr,"Timeout on read RESP\n");
-			exit(-3);
+			printf("Timeout on read RESP\n");
+			return(-4);
 		}
 
 		s = read(fd, c, 1);
@@ -187,12 +187,12 @@ int platform_buffer_read(uint8_t *data, int maxsize)
 		FD_SET(fd, &rset);
 		ret = select(fd + 1, &rset, NULL, NULL, &tv);
 		if (ret < 0) {
-			fprintf(stderr,"Failed on select\n");
+			printf("Failed on select\n");
 			exit(-4);
 		}
 		if(ret == 0) {
-			fprintf(stderr,"Timeout on read\n");
-			exit(-3);
+			printf("Timeout on read\n");
+			return(-5);
 		}
 		s = read(fd, c, 1);
 		if (*c==REMOTE_EOM) {
@@ -205,7 +205,7 @@ int platform_buffer_read(uint8_t *data, int maxsize)
 		}
 	}while ((s >= 0) && ((c - data) < maxsize));
 
-	fprintf(stderr,"Failed to read\n");
-	exit(-3);
+	printf("Failed to read\n");
+	return(-6);
 	return 0;
 }

From b06c0ba8d5f8cf5dfaa604730d114e8f6140a8a2 Mon Sep 17 00:00:00 2001
From: Valmantas Paliksa <walmis@gmail.com>
Date: Tue, 17 Dec 2019 15:09:49 +0200
Subject: [PATCH 36/43] bmp_remote: Use high level functions.

Based on #570 (OpenOCD HLA interface driver for Blackmagic), but now
usefull for bmp-remote.
---
 src/include/platform_support.h        |   2 +
 src/platforms/hosted/bmp_remote.c     | 232 +++++++++++++++++++++++++-
 src/platforms/hosted/bmp_remote.h     |   8 +-
 src/platforms/hosted/platform.c       |   4 +-
 src/platforms/hosted/remote_swdptap.c |   1 -
 src/remote.c                          | 142 ++++++++++++++++
 src/remote.h                          |  34 ++++
 src/target/adiv5.c                    |  29 ++--
 src/target/adiv5.h                    |  18 ++
 src/target/adiv5_jtagdp.c             |  21 +--
 src/target/adiv5_swdp.c               |  31 ++--
 11 files changed, 465 insertions(+), 57 deletions(-)

diff --git a/src/include/platform_support.h b/src/include/platform_support.h
index a111354..88f4926 100644
--- a/src/include/platform_support.h
+++ b/src/include/platform_support.h
@@ -24,6 +24,8 @@
 #	error "Include 'general.h' instead"
 #endif
 
+#include "target.h"
+
 #if PC_HOSTED == 1
 void platform_init(int argc, char **argv);
 #else
diff --git a/src/platforms/hosted/bmp_remote.c b/src/platforms/hosted/bmp_remote.c
index 06e828a..817ec17 100644
--- a/src/platforms/hosted/bmp_remote.c
+++ b/src/platforms/hosted/bmp_remote.c
@@ -26,6 +26,8 @@
 #include "remote.h"
 #include "target.h"
 #include "bmp_remote.h"
+#include "cl_utils.h"
+#include "hex_utils.h"
 
 #include <assert.h>
 #include <sys/time.h>
@@ -35,7 +37,7 @@
 
 #include "adiv5.h"
 
-int remote_init(void)
+int remote_init(bool verbose)
 {
 	char construct[REMOTE_MAX_MSG_SIZE];
 	int c = snprintf(construct, REMOTE_MAX_MSG_SIZE, "%s", REMOTE_START_STR);
@@ -47,8 +49,19 @@ int remote_init(void)
 				c ? (char *)&(construct[1]) : "unknown");
       return -1;
     }
-
-	printf("Remote is %s\n", &construct[1]);
+	if (verbose)
+		printf("Remote is %s\n", &construct[1]);
+	char *p = strstr(&construct[1], "(Firmware v");
+	if (!p)
+		return -1;
+	int major = 0, minor = 0, step  = 0;
+	int res = sscanf(p, "(Firmware v%d.%d.%d", &major, &minor, &step);
+	if (res !=3)
+		return -1;
+	uint32_t version = major * 10000 + minor * 100 + step;
+	/* check that firmare is > 1.6.1 */
+	if (version < 10602)
+		return -1;
 	return 0;
 }
 
@@ -145,3 +158,216 @@ const char *remote_target_voltage(void)
     }
 	return (char *)&construct[1];
 }
+
+static uint32_t remote_adiv5_dp_read(ADIv5_DP_t *dp, uint16_t addr)
+{
+	(void)dp;
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE, REMOTE_DP_READ_STR,
+					 addr);
+	platform_buffer_write(construct, s);
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+		printf("%s error %d\n", __func__, s);
+	}
+    uint32_t dest[1];
+	unhexify(dest, (const char*)&construct[1], 4);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("dp_read @ %04x: 0x%08" PRIx32 "\n", addr, dest[0]);
+	return dest[0];
+}
+
+static uint32_t remote_adiv5_low_access(ADIv5_DP_t *dp, uint8_t RnW, uint16_t addr, uint32_t value)
+{
+	(void)dp;
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE, REMOTE_LOW_ACCESS_STR,
+					 RnW, addr, value);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("Dp_write @ %04x: 0x%08" PRIx32 "\n", addr, value);
+	platform_buffer_write(construct, s);
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+		printf("%s error %d\n", __func__, s);
+	}
+    uint32_t dest[1];
+	unhexify(dest, (const char*)&construct[1], 4);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("dp_read @ %04x: 0x%08" PRIx32 "\n", addr, dest[0]);
+	return dest[0];
+}
+
+static uint32_t remote_adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE,REMOTE_AP_READ_STR,
+					 ap->apsel, addr);
+	platform_buffer_write(construct, s);
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+		printf("%s error %d\n", __func__, s);
+	}
+    uint32_t dest[1];
+	unhexify(dest, (const char*)&construct[1], 4);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("ap_read @ %04x: 0x%08" PRIx32 "\n", addr, dest[0]);
+	return dest[0];
+}
+
+static void remote_adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+{
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE,REMOTE_AP_WRITE_STR,
+					 ap->apsel, addr, value);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("ap_write @ %04x: 0x%08" PRIx32 "\n", addr, value);
+	platform_buffer_write(construct, s);
+	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
+		printf("%s error %d\n", __func__, s);
+	}
+	return;
+}
+
+#if 0
+static void remote_mem_read(
+	ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
+{
+	(void)ap;
+	if (len == 0)
+		return;
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("memread @ %" PRIx32 " len %ld, start: \n",
+			   src, len);
+	uint8_t construct[REMOTE_MAX_MSG_SIZE];
+	int s;
+	int batchsize = (REMOTE_MAX_MSG_SIZE - 32) / 2;
+	while(len) {
+		int count = len;
+		if (count > batchsize)
+			count = batchsize;
+		s = snprintf(construct, REMOTE_MAX_MSG_SIZE,
+					 REMOTE_MEM_READ_STR, src, count);
+		platform_buffer_write(construct, s);
+
+		s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
+		if ((s > 0) && (construct[0] == REMOTE_RESP_OK)) {
+			unhexify(dest, (const char*)&construct[1], count);
+			src += count;
+			dest += count;
+			len -= count;
+
+			continue;
+		} else {
+			if(construct[0] == REMOTE_RESP_ERR) {
+				ap->dp->fault = 1;
+				printf("%s returned REMOTE_RESP_ERR at addr: 0x%08x\n",
+					   __func__, src);
+				break;
+			} else {
+				printf("%s error %d\n", __func__, s);
+				break;
+			}
+		}
+	}
+}
+#endif
+
+static void remote_ap_mem_read(
+	ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
+{
+	(void)ap;
+	if (len == 0)
+		return;
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("ap_memread @ %" PRIx32 " len %ld\n", src, len);
+	char construct[REMOTE_MAX_MSG_SIZE];
+	int batchsize = (REMOTE_MAX_MSG_SIZE - 0x20) / 2;
+	while(len) {
+		int s;
+		int count = len;
+		if (count > batchsize)
+			count = batchsize;
+		s = snprintf(construct, REMOTE_MAX_MSG_SIZE,
+					 REMOTE_AP_MEM_READ_STR, ap->apsel, ap->csw, src, count);
+		platform_buffer_write((uint8_t*)construct, s);
+		s = platform_buffer_read((uint8_t*)construct, REMOTE_MAX_MSG_SIZE);
+		if ((s > 0) && (construct[0] == REMOTE_RESP_OK)) {
+			unhexify(dest, (const char*)&construct[1], count);
+			src  += count;
+			dest += count;
+			len  -= count;
+			continue;
+		} else {
+			if(construct[0] == REMOTE_RESP_ERR) {
+				ap->dp->fault = 1;
+				printf("%s returned REMOTE_RESP_ERR at apsel %d, "
+					   "addr: 0x%08" PRIx32 "\n", __func__, ap->apsel, src);
+				break;
+			} else {
+				printf("%s error %d around 0x%08" PRIx32 "\n",
+					   __func__, s, src);
+				break;
+			}
+		}
+	}
+}
+
+static void remote_ap_mem_write_sized(
+	ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len,
+	enum align align)
+{
+	(void)ap;
+	if (len == 0)
+		return;
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("ap_mem_write_sized @ %" PRIx32 " len %ld, align %d\n",
+			   dest, len, 1 << align);
+	char construct[REMOTE_MAX_MSG_SIZE];
+	/* (5 * 1 (char)) + (2 * 2 (bytes)) + (3 * 8 (words)) */
+	int batchsize = (REMOTE_MAX_MSG_SIZE - 0x30) / 2;
+	while (len) {
+		int count = len;
+		if (count > batchsize)
+			count = batchsize;
+		int s = snprintf(construct, REMOTE_MAX_MSG_SIZE,
+						 REMOTE_AP_MEM_WRITE_SIZED_STR,
+						 ap->apsel, ap->csw, align, dest, count);
+		char *p = construct + s;
+		hexify(p, src, count);
+		p += 2 * count;
+		src  += count;
+		dest += count;
+		len  -= count;
+		*p++ = REMOTE_EOM;
+		*p   = 0;
+		platform_buffer_write((uint8_t*)construct, p - construct);
+
+		s = platform_buffer_read((uint8_t*)construct, REMOTE_MAX_MSG_SIZE);
+		if ((s > 0) && (construct[0] == REMOTE_RESP_OK))
+			continue;
+		if ((s > 0) && (construct[0] == REMOTE_RESP_ERR)) {
+			ap->dp->fault = 1;
+			printf("%s returned REMOTE_RESP_ERR at apsel %d, "
+				   "addr: 0x%08x\n", __func__, ap->apsel, dest);
+		} else {
+			printf("%s error %d around address 0x%08" PRIx32 "\n",
+				   __func__, s, dest);
+			break;
+		}
+	}
+}
+
+void remote_adiv5_dp_defaults(ADIv5_DP_t *dp)
+{
+	if (remote_init(false)) {
+		printf("Please update BMP firmware for substantial speed increase!\n");
+		return;
+	}
+	dp->low_access = remote_adiv5_low_access;
+	dp->dp_read    = remote_adiv5_dp_read;
+	dp->ap_write   = remote_adiv5_ap_write;
+	dp->ap_read    = remote_adiv5_ap_read;
+	dp->mem_read   = remote_ap_mem_read;
+	dp->mem_write_sized = remote_ap_mem_write_sized;
+}
diff --git a/src/platforms/hosted/bmp_remote.h b/src/platforms/hosted/bmp_remote.h
index 1b9ac41..ea21b7b 100644
--- a/src/platforms/hosted/bmp_remote.h
+++ b/src/platforms/hosted/bmp_remote.h
@@ -20,13 +20,16 @@
 #define      __BMP_REMOTE_H_
 #include "swdptap.h"
 #include "jtagtap.h"
+#include "adiv5.h"
+#include "target.h"
+#include "target_internal.h"
 
-#define REMOTE_MAX_MSG_SIZE (256)
+#define REMOTE_MAX_MSG_SIZE (1024)
 
 int platform_buffer_write(const uint8_t *data, int size);
 int platform_buffer_read(uint8_t *data, int size);
 
-int remote_init(void);
+int remote_init(bool verbose);
 int remote_swdptap_init(swd_proc_t *swd_proc);
 int remote_jtagtap_init(jtag_proc_t *jtag_proc);
 bool remote_target_get_power(void);
@@ -35,5 +38,6 @@ void remote_target_set_power(bool power);
 void remote_srst_set_val(bool assert);
 bool remote_srst_get_val(void);
 const char *platform_target_voltage(void);
+void remote_adiv5_dp_defaults(ADIv5_DP_t *dp);
 #define __BMP_REMOTE_H_
 #endif
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 28bfd35..430ed72 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -244,7 +244,7 @@ void platform_init(int argc, char **argv)
 	case BMP_TYPE_BMP:
 		if (serial_open(&cl_opts, info.serial))
 			exit(-1);
-		remote_init();
+		remote_init(true);
 		break;
 	case BMP_TYPE_STLINKV2:
 		if (stlink_init( &info))
@@ -367,6 +367,8 @@ int platform_jtagtap_init(void)
 void platform_adiv5_dp_defaults(ADIv5_DP_t *dp)
 {
 	switch (info.bmp_type) {
+	case BMP_TYPE_BMP:
+		return remote_adiv5_dp_defaults(dp);
 	case BMP_TYPE_STLINKV2:
 		return stlink_adiv5_dp_defaults(dp);
 	case BMP_TYPE_CMSIS_DAP:
diff --git a/src/platforms/hosted/remote_swdptap.c b/src/platforms/hosted/remote_swdptap.c
index 5b2af4d..93f74f7 100644
--- a/src/platforms/hosted/remote_swdptap.c
+++ b/src/platforms/hosted/remote_swdptap.c
@@ -39,7 +39,6 @@ int remote_swdptap_init(swd_proc_t *swd_proc)
 {
 	uint8_t construct[REMOTE_MAX_MSG_SIZE];
 	int s;
-
 	s = sprintf((char *)construct,"%s", REMOTE_SWDP_INIT_STR);
 	platform_buffer_write(construct, s);
 
diff --git a/src/remote.c b/src/remote.c
index a1ade3b..89887eb 100644
--- a/src/remote.c
+++ b/src/remote.c
@@ -25,7 +25,11 @@
 #include "jtagtap.h"
 #include "gdb_if.h"
 #include "version.h"
+#include "exception.h"
 #include <stdarg.h>
+#include "target/adiv5.h"
+#include "target.h"
+#include "hex_utils.h"
 
 
 #define NTOH(x) ((x<=9)?x+'0':'a'+x-10)
@@ -57,6 +61,30 @@ uint64_t remotehston(uint32_t limit, char *s)
 }
 
 #if PC_HOSTED == 0
+static void _send_buf(uint8_t* buffer, size_t len)
+{
+	uint8_t* p = buffer;
+	char hex[2];
+	do {
+		hexify(hex, (const void*)p++, 1);
+
+		gdb_if_putchar(hex[0], 0);
+		gdb_if_putchar(hex[1], 0);
+
+	} while (p<(buffer+len));
+}
+
+static void _respond_buf(char respCode, uint8_t* buffer, size_t len)
+{
+	gdb_if_putchar(REMOTE_RESP, 0);
+	gdb_if_putchar(respCode, 0);
+
+	_send_buf(buffer, len);
+
+	gdb_if_putchar(REMOTE_EOM, 1);
+}
+
+
 static void _respond(char respCode, uint64_t param)
 
 /* Send response to far end */
@@ -97,6 +125,16 @@ static void _respondS(char respCode, const char *s)
 	gdb_if_putchar(REMOTE_EOM,1);
 }
 
+static ADIv5_DP_t remote_dp = {
+	.dp_read = firmware_swdp_read,
+	.ap_read = firmware_ap_read,
+	.ap_write = firmware_ap_write,
+	.mem_read = firmware_mem_read,
+	.mem_write_sized = firmware_mem_write_sized,
+	.low_access = firmware_swdp_low_access,
+};
+
+
 void remotePacketProcessSWD(uint8_t i, char *packet)
 {
 	uint8_t ticks;
@@ -155,6 +193,8 @@ void remotePacketProcessJTAG(uint8_t i, char *packet)
 	switch (packet[1]) {
     case REMOTE_INIT: /* = initialise ================================= */
 		jtagtap_init();
+		remote_dp.dp_read = fw_adiv5_jtagdp_read;
+		remote_dp.low_access = fw_adiv5_jtagdp_low_access;
 		_respond(REMOTE_RESP_OK, 0);
 		break;
 
@@ -255,6 +295,104 @@ void remotePacketProcessGEN(uint8_t i, char *packet)
     }
 }
 
+void remotePacketProcessHL(uint8_t i, char *packet)
+
+{
+	(void)i;
+	SET_IDLE_STATE(0);
+
+	ADIv5_AP_t remote_ap;
+	/* Re-use packet buffer. Align to DWORD! */
+	void *src = (void *)(((uint32_t)packet + 7) & ~7);
+	char index = packet[1];
+	packet += 2;
+	remote_ap.apsel = remotehston(2, packet);
+	remote_ap.dp = &remote_dp;
+	switch (index) {
+	case REMOTE_DP_READ:
+		packet += 2;
+		uint16_t addr16 = remotehston(4, packet);
+		uint32_t data = adiv5_dp_read(&remote_dp, addr16);
+		_respond_buf(REMOTE_RESP_OK, (uint8_t*)&data, 4);
+		break;
+	case REMOTE_LOW_ACCESS:
+		packet += 2;
+		addr16 = remotehston(4, packet);
+		packet += 4;
+		uint32_t value = remotehston(8, packet);
+		data = remote_dp.low_access(&remote_dp, remote_ap.apsel, addr16, value);
+		_respond_buf(REMOTE_RESP_OK, (uint8_t*)&data, 4);
+		break;
+	case REMOTE_AP_READ:
+		packet += 2;
+		addr16 = remotehston(4, packet);
+		data = adiv5_ap_read(&remote_ap, addr16);
+		_respond_buf(REMOTE_RESP_OK, (uint8_t*)&data, 4);
+		break;
+	case REMOTE_AP_WRITE:
+		packet += 2;
+		addr16 = remotehston(4, packet);
+		packet += 4;
+		value = remotehston(8, packet);
+		adiv5_ap_write(&remote_ap, addr16, value);
+		_respond(REMOTE_RESP_OK, 0);
+		break;
+	case REMOTE_AP_MEM_READ:
+		packet += 2;
+		remote_ap.csw = remotehston(8, packet);
+		packet += 6;
+		/*fall through*/
+	case REMOTE_MEM_READ:
+		packet += 2;
+		uint32_t address = remotehston(8, packet);
+		packet += 8;
+		uint32_t count = remotehston(8, packet);
+		packet += 8;
+		adiv5_mem_read(&remote_ap, src, address, count);
+		if (remote_ap.dp->fault == 0) {
+			_respond_buf(REMOTE_RESP_OK, src, count);
+			break;
+		}
+		_respond(REMOTE_RESP_ERR, 0);
+		remote_ap.dp->fault = 0;
+		break;
+	case REMOTE_AP_MEM_WRITE_SIZED:
+		packet += 2;
+		remote_ap.csw = remotehston(8, packet);
+		packet += 6;
+		/*fall through*/
+	case REMOTE_MEM_WRITE_SIZED:
+		packet += 2;
+		enum align align = remotehston(2, packet);
+		packet += 2;
+		uint32_t dest = remotehston(8, packet);
+		packet+= 8;
+		size_t len = remotehston(8, packet);
+		packet += 8;
+		if (len & ((1 << align) - 1)) {
+			/* len  and align do not fit*/
+			_respond(REMOTE_RESP_ERR, 0);
+			break;
+		}
+		/* Read as stream of hexified bytes*/
+		unhexify(src, packet, len);
+		adiv5_mem_write_sized(&remote_ap, dest, src, len, align);
+		if (remote_ap.dp->fault) {
+			/* Errors handles on hosted side.*/
+			_respond(REMOTE_RESP_ERR, 0);
+			remote_ap.dp->fault = 0;
+			break;
+		}
+		_respond_buf(REMOTE_RESP_OK, src, len);
+		break;
+	default:
+		_respond(REMOTE_RESP_ERR,REMOTE_ERROR_UNRECOGNISED);
+		break;
+	}
+	SET_IDLE_STATE(1);
+}
+
+
 void remotePacketProcess(uint8_t i, char *packet)
 {
 	switch (packet[0]) {
@@ -270,6 +408,10 @@ void remotePacketProcess(uint8_t i, char *packet)
 		remotePacketProcessGEN(i,packet);
 		break;
 
+    case REMOTE_HL_PACKET:
+		remotePacketProcessHL(i, packet);
+		break;
+
     default: /* Oh dear, unrecognised, return an error */
 		_respond(REMOTE_RESP_ERR,REMOTE_ERROR_UNRECOGNISED);
 		break;
diff --git a/src/remote.h b/src/remote.h
index bddab64..dfb8a6d 100644
--- a/src/remote.h
+++ b/src/remote.h
@@ -83,6 +83,18 @@
 #define REMOTE_RESP_ERR    'E'
 #define REMOTE_RESP_NOTSUP 'N'
 
+/* High level protocol elements */
+#define REMOTE_HL_PACKET 'H'
+#define REMOTE_DP_READ      'd'
+#define REMOTE_LOW_ACCESS   'L'
+#define REMOTE_AP_READ      'a'
+#define REMOTE_AP_WRITE     'A'
+#define REMOTE_AP_MEM_READ  'M'
+#define REMOTE_MEM_READ           'h'
+#define REMOTE_MEM_WRITE_SIZED    'H'
+#define REMOTE_AP_MEM_WRITE_SIZED 'm'
+
+
 /* Generic protocol elements */
 #define REMOTE_GEN_PACKET  'G'
 
@@ -125,6 +137,28 @@
 #define REMOTE_JTAG_NEXT (char []){ REMOTE_SOM, REMOTE_JTAG_PACKET, REMOTE_NEXT, \
                                        '%','c','%','c',REMOTE_EOM, 0 }
 
+/* HL protocol elements */
+#define HEX '%', '0', '2', 'x'
+#define HEX_U32(x) '%', '0', '8', 'x'
+#define CHR(x) '%', 'c'
+
+#define REMOTE_MEM_READ_STR (char []){ REMOTE_SOM, REMOTE_HL_PACKET, REMOTE_MEM_READ, \
+			HEX_U32(address), HEX_U32(count), REMOTE_EOM, 0 }
+#define REMOTE_DP_READ_STR (char []){ REMOTE_SOM, REMOTE_HL_PACKET, REMOTE_DP_READ, \
+			'f', 'f','%', '0', '4', 'x', REMOTE_EOM, 0 }
+#define REMOTE_LOW_ACCESS_STR (char []){ REMOTE_SOM, REMOTE_HL_PACKET, REMOTE_LOW_ACCESS, \
+			'%','0', '2', 'x', '%', '0', '4', 'x', HEX_U32(csw), REMOTE_EOM, 0 }
+#define REMOTE_AP_READ_STR (char []){ REMOTE_SOM, REMOTE_HL_PACKET, REMOTE_AP_READ, \
+			'%','0','2','x', '%', '0', '4', 'x', REMOTE_EOM, 0 }
+#define REMOTE_AP_WRITE_STR (char []){ REMOTE_SOM, REMOTE_HL_PACKET, REMOTE_AP_WRITE, \
+			'%','0','2','x', '%', '0', '4', 'x', HEX_U32(csw), REMOTE_EOM, 0 }
+#define REMOTE_AP_MEM_READ_STR (char []){ REMOTE_SOM, REMOTE_HL_PACKET, REMOTE_AP_MEM_READ, \
+			'%','0','2','x',HEX_U32(csw), HEX_U32(address), HEX_U32(count), REMOTE_EOM, 0 }
+#define REMOTE_AP_MEM_WRITE_SIZED_STR (char []){ REMOTE_SOM, REMOTE_HL_PACKET, REMOTE_AP_MEM_WRITE_SIZED, \
+			'%', '0', '2', 'x', HEX_U32(csw), '%', '0', '2', 'x', HEX_U32(address), HEX_U32(count), 0}
+#define REMOTE_MEM_WRITE_SIZED_STR (char []){ REMOTE_SOM, REMOTE_HL_PACKET, REMOTE_AP_MEM_WRITE_SIZED, \
+			'%','0','2','x', HEX_U32(address), HEX_U32(count), 0}
+
 uint64_t remotehston(uint32_t limit, char *s);
 void remotePacketProcess(uint8_t i, char *packet);
 
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 973870e..d2f2142 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -439,11 +439,6 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 	return ap;
 }
 
-static void ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
-static uint32_t ap_read(ADIv5_AP_t *ap, uint16_t addr);
-static void mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len);
-static void mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
-							size_t len, enum align align);
 void adiv5_dp_init(ADIv5_DP_t *dp)
 {
 	volatile bool probed = false;
@@ -452,18 +447,18 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 #if PC_HOSTED  == 1
 	platform_adiv5_dp_defaults(dp);
 	if (!dp->ap_write)
-		dp->ap_write = ap_write;
+		dp->ap_write = firmware_ap_write;
 	if (!dp->ap_read)
-		dp->ap_read = ap_read;
+		dp->ap_read = firmware_ap_read;
 	if (!dp->mem_read)
-		dp->mem_read = mem_read;
+		dp->mem_read = firmware_mem_read;
 	if (!dp->mem_write_sized)
-		dp->mem_write_sized = mem_write_sized;
+		dp->mem_write_sized = firmware_mem_write_sized;
 #else
-	dp->ap_write = ap_write;
-	dp->ap_read = ap_read;
-	dp->mem_read = mem_read;
-	dp->mem_write_sized = mem_write_sized;
+	dp->ap_write = firmware_ap_write;
+	dp->ap_read = firmware_ap_read;
+	dp->mem_read = firmware_mem_read;
+	dp->mem_write_sized = firmware_mem_write_sized;
 #endif
 	volatile struct exception e;
 	TRY_CATCH (e, EXCEPTION_TIMEOUT) {
@@ -629,7 +624,7 @@ void * extract(void *dest, uint32_t src, uint32_t val, enum align align)
 	return (uint8_t *)dest + (1 << align);
 }
 
-static void mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
+void firmware_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 {
 	uint32_t tmp;
 	uint32_t osrc = src;
@@ -659,7 +654,7 @@ static void mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 	extract(dest, src, tmp, align);
 }
 
-static void mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+void firmware_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
 							size_t len, enum align align)
 {
 	uint32_t odest = dest;
@@ -694,14 +689,14 @@ static void mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
 	}
 }
 
-static void ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+void firmware_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 {
 	adiv5_dp_write(ap->dp, ADIV5_DP_SELECT,
 			((uint32_t)ap->apsel << 24)|(addr & 0xF0));
 	adiv5_dp_write(ap->dp, addr, value);
 }
 
-static uint32_t ap_read(ADIv5_AP_t *ap, uint16_t addr)
+uint32_t firmware_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 {
 	uint32_t ret;
 	adiv5_dp_write(ap->dp, ADIV5_DP_SELECT,
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index d592921..1a6132b 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -240,4 +240,22 @@ int platform_jtag_dp_init(ADIv5_DP_t *dp);
 void adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len);
 uint64_t adiv5_ap_read_pidr(ADIv5_AP_t *ap, uint32_t addr);
 void * extract(void *dest, uint32_t src, uint32_t val, enum align align);
+
+void firmware_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+							  size_t len, enum align align);
+void firmware_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src,
+					   size_t len);
+void firmware_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
+uint32_t firmware_ap_read(ADIv5_AP_t *ap, uint16_t addr);
+uint32_t firmware_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
+								  uint16_t addr, uint32_t value);
+uint32_t fw_adiv5_jtagdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
+					uint16_t addr, uint32_t value);
+uint32_t firmware_swdp_read(ADIv5_DP_t *dp, uint16_t addr);
+uint32_t fw_adiv5_jtagdp_read(ADIv5_DP_t *dp, uint16_t addr);
+
+uint32_t firmware_swdp_error(ADIv5_DP_t *dp);
+
+void firmware_swdp_abort(ADIv5_DP_t *dp, uint32_t abort);
+
 #endif
diff --git a/src/target/adiv5_jtagdp.c b/src/target/adiv5_jtagdp.c
index d6a3cf8..5ead978 100644
--- a/src/target/adiv5_jtagdp.c
+++ b/src/target/adiv5_jtagdp.c
@@ -37,13 +37,8 @@
 #define IR_DPACC	0xA
 #define IR_APACC	0xB
 
-static uint32_t adiv5_jtagdp_read(ADIv5_DP_t *dp, uint16_t addr);
-
 static uint32_t adiv5_jtagdp_error(ADIv5_DP_t *dp);
 
-static uint32_t adiv5_jtagdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
-					uint16_t addr, uint32_t value);
-
 static void adiv5_jtagdp_abort(ADIv5_DP_t *dp, uint32_t abort);
 
 void adiv5_jtag_dp_handler(jtag_dev_t *dev)
@@ -57,29 +52,29 @@ void adiv5_jtag_dp_handler(jtag_dev_t *dev)
 	dp->dev = dev;
 	if ((PC_HOSTED == 0 ) || (!platform_jtag_dp_init(dp))) {
 		dp->idcode = dev->idcode;
-		dp->dp_read = adiv5_jtagdp_read;
+		dp->dp_read = fw_adiv5_jtagdp_read;
 		dp->error = adiv5_jtagdp_error;
-		dp->low_access = adiv5_jtagdp_low_access;
+		dp->low_access = fw_adiv5_jtagdp_low_access;
 		dp->abort = adiv5_jtagdp_abort;
 	}
 	adiv5_dp_init(dp);
 }
 
-static uint32_t adiv5_jtagdp_read(ADIv5_DP_t *dp, uint16_t addr)
+uint32_t fw_adiv5_jtagdp_read(ADIv5_DP_t *dp, uint16_t addr)
 {
-	adiv5_jtagdp_low_access(dp, ADIV5_LOW_READ, addr, 0);
-	return adiv5_jtagdp_low_access(dp, ADIV5_LOW_READ,
+	fw_adiv5_jtagdp_low_access(dp, ADIV5_LOW_READ, addr, 0);
+	return fw_adiv5_jtagdp_low_access(dp, ADIV5_LOW_READ,
 					ADIV5_DP_RDBUFF, 0);
 }
 
 static uint32_t adiv5_jtagdp_error(ADIv5_DP_t *dp)
 {
-	adiv5_jtagdp_low_access(dp, ADIV5_LOW_READ, ADIV5_DP_CTRLSTAT, 0);
-	return adiv5_jtagdp_low_access(dp, ADIV5_LOW_WRITE,
+	fw_adiv5_jtagdp_low_access(dp, ADIV5_LOW_READ, ADIV5_DP_CTRLSTAT, 0);
+	return fw_adiv5_jtagdp_low_access(dp, ADIV5_LOW_WRITE,
 				ADIV5_DP_CTRLSTAT, 0xF0000032) & 0x32;
 }
 
-static uint32_t adiv5_jtagdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
+uint32_t fw_adiv5_jtagdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 					uint16_t addr, uint32_t value)
 {
 	bool APnDP = addr & ADIV5_APnDP;
diff --git a/src/target/adiv5_swdp.c b/src/target/adiv5_swdp.c
index 9cfdf49..87c687e 100644
--- a/src/target/adiv5_swdp.c
+++ b/src/target/adiv5_swdp.c
@@ -33,15 +33,6 @@
 #define SWDP_ACK_WAIT  0x02
 #define SWDP_ACK_FAULT 0x04
 
-static uint32_t adiv5_swdp_read(ADIv5_DP_t *dp, uint16_t addr);
-
-static uint32_t adiv5_swdp_error(ADIv5_DP_t *dp);
-
-static uint32_t adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
-				      uint16_t addr, uint32_t value);
-
-static void adiv5_swdp_abort(ADIv5_DP_t *dp, uint32_t abort);
-
 int adiv5_swdp_scan(void)
 {
 	uint32_t ack;
@@ -80,33 +71,33 @@ int adiv5_swdp_scan(void)
 		return -1;
 	}
 
-	dp->dp_read = adiv5_swdp_read;
-	dp->error = adiv5_swdp_error;
-	dp->low_access = adiv5_swdp_low_access;
-	dp->abort = adiv5_swdp_abort;
+	dp->dp_read = firmware_swdp_read;
+	dp->error = firmware_swdp_error;
+	dp->low_access = firmware_swdp_low_access;
+	dp->abort = firmware_swdp_abort;
 
-	adiv5_swdp_error(dp);
+	firmware_swdp_error(dp);
 	adiv5_dp_init(dp);
 
 	return target_list?1:0;
 }
 
-static uint32_t adiv5_swdp_read(ADIv5_DP_t *dp, uint16_t addr)
+uint32_t firmware_swdp_read(ADIv5_DP_t *dp, uint16_t addr)
 {
 	if (addr & ADIV5_APnDP) {
 		adiv5_dp_low_access(dp, ADIV5_LOW_READ, addr, 0);
 		return adiv5_dp_low_access(dp, ADIV5_LOW_READ,
 		                           ADIV5_DP_RDBUFF, 0);
 	} else {
-		return adiv5_swdp_low_access(dp, ADIV5_LOW_READ, addr, 0);
+		return firmware_swdp_low_access(dp, ADIV5_LOW_READ, addr, 0);
 	}
 }
 
-static uint32_t adiv5_swdp_error(ADIv5_DP_t *dp)
+ uint32_t firmware_swdp_error(ADIv5_DP_t *dp)
 {
 	uint32_t err, clr = 0;
 
-	err = adiv5_swdp_read(dp, ADIV5_DP_CTRLSTAT) &
+	err = firmware_swdp_read(dp, ADIV5_DP_CTRLSTAT) &
 		(ADIV5_DP_CTRLSTAT_STICKYORUN | ADIV5_DP_CTRLSTAT_STICKYCMP |
 		ADIV5_DP_CTRLSTAT_STICKYERR | ADIV5_DP_CTRLSTAT_WDATAERR);
 
@@ -125,7 +116,7 @@ static uint32_t adiv5_swdp_error(ADIv5_DP_t *dp)
 	return err;
 }
 
-static uint32_t adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
+uint32_t firmware_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 				      uint16_t addr, uint32_t value)
 {
 	bool APnDP = addr & ADIV5_APnDP;
@@ -183,7 +174,7 @@ static uint32_t adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 	return response;
 }
 
-static void adiv5_swdp_abort(ADIv5_DP_t *dp, uint32_t abort)
+void firmware_swdp_abort(ADIv5_DP_t *dp, uint32_t abort)
 {
 	adiv5_dp_write(dp, ADIV5_DP_ABORT, abort);
 }

From 64f3dff8a8575a60a4d8b534a1a4a0dc3896dd41 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sun, 3 May 2020 12:07:47 +0200
Subject: [PATCH 37/43] PC-Hosted: Better debug output.

---
 src/platforms/hosted/bmp_remote.c       |  18 +--
 src/platforms/hosted/cmsis_dap.c        |   2 -
 src/platforms/hosted/jlink_adiv5_swdp.c |  59 +--------
 src/platforms/hosted/jlink_jtagtap.c    |  20 ++-
 src/platforms/hosted/libftdi_jtagtap.c  |   6 +-
 src/platforms/hosted/libftdi_swdptap.c  |   8 +-
 src/platforms/hosted/platform.c         | 137 +++++++++++++++++++++
 src/platforms/hosted/stlinkv2.c         | 154 +++++++++---------------
 src/platforms/pc/libusb_utils.c         |   6 +-
 src/platforms/pc/serial_unix.c          |   6 +-
 src/target/adiv5.c                      |  35 +++---
 src/target/adiv5.h                      |  50 +++++---
 12 files changed, 279 insertions(+), 222 deletions(-)

diff --git a/src/platforms/hosted/bmp_remote.c b/src/platforms/hosted/bmp_remote.c
index 817ec17..a6461a5 100644
--- a/src/platforms/hosted/bmp_remote.c
+++ b/src/platforms/hosted/bmp_remote.c
@@ -172,19 +172,16 @@ static uint32_t remote_adiv5_dp_read(ADIv5_DP_t *dp, uint16_t addr)
 	}
     uint32_t dest[1];
 	unhexify(dest, (const char*)&construct[1], 4);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("dp_read @ %04x: 0x%08" PRIx32 "\n", addr, dest[0]);
 	return dest[0];
 }
 
-static uint32_t remote_adiv5_low_access(ADIv5_DP_t *dp, uint8_t RnW, uint16_t addr, uint32_t value)
+static uint32_t remote_adiv5_low_access(
+	ADIv5_DP_t *dp, uint8_t RnW, uint16_t addr, uint32_t value)
 {
 	(void)dp;
 	uint8_t construct[REMOTE_MAX_MSG_SIZE];
 	int s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE, REMOTE_LOW_ACCESS_STR,
 					 RnW, addr, value);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("Dp_write @ %04x: 0x%08" PRIx32 "\n", addr, value);
 	platform_buffer_write(construct, s);
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
@@ -192,8 +189,6 @@ static uint32_t remote_adiv5_low_access(ADIv5_DP_t *dp, uint8_t RnW, uint16_t ad
 	}
     uint32_t dest[1];
 	unhexify(dest, (const char*)&construct[1], 4);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("dp_read @ %04x: 0x%08" PRIx32 "\n", addr, dest[0]);
 	return dest[0];
 }
 
@@ -209,8 +204,6 @@ static uint32_t remote_adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 	}
     uint32_t dest[1];
 	unhexify(dest, (const char*)&construct[1], 4);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("ap_read @ %04x: 0x%08" PRIx32 "\n", addr, dest[0]);
 	return dest[0];
 }
 
@@ -219,8 +212,6 @@ static void remote_adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 	uint8_t construct[REMOTE_MAX_MSG_SIZE];
 	int s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE,REMOTE_AP_WRITE_STR,
 					 ap->apsel, addr, value);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("ap_write @ %04x: 0x%08" PRIx32 "\n", addr, value);
 	platform_buffer_write(construct, s);
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
@@ -279,8 +270,6 @@ static void remote_ap_mem_read(
 	(void)ap;
 	if (len == 0)
 		return;
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("ap_memread @ %" PRIx32 " len %ld\n", src, len);
 	char construct[REMOTE_MAX_MSG_SIZE];
 	int batchsize = (REMOTE_MAX_MSG_SIZE - 0x20) / 2;
 	while(len) {
@@ -320,9 +309,6 @@ static void remote_ap_mem_write_sized(
 	(void)ap;
 	if (len == 0)
 		return;
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("ap_mem_write_sized @ %" PRIx32 " len %ld, align %d\n",
-			   dest, len, 1 << align);
 	char construct[REMOTE_MAX_MSG_SIZE];
 	/* (5 * 1 (char)) + (2 * 2 (bytes)) + (3 * 8 (words)) */
 	int batchsize = (REMOTE_MAX_MSG_SIZE - 0x30) / 2;
diff --git a/src/platforms/hosted/cmsis_dap.c b/src/platforms/hosted/cmsis_dap.c
index 04c3905..395f308 100644
--- a/src/platforms/hosted/cmsis_dap.c
+++ b/src/platforms/hosted/cmsis_dap.c
@@ -41,8 +41,6 @@
 #include "target.h"
 #include "target_internal.h"
 
-#define MAX_DEBUGGERS     20
-
 uint8_t dap_caps;
 uint8_t mode;
 
diff --git a/src/platforms/hosted/jlink_adiv5_swdp.c b/src/platforms/hosted/jlink_adiv5_swdp.c
index 06da0a1..507f1db 100644
--- a/src/platforms/hosted/jlink_adiv5_swdp.c
+++ b/src/platforms/hosted/jlink_adiv5_swdp.c
@@ -104,8 +104,6 @@ static int line_reset(bmp_info_t *info)
 
 static int swdptap_init(bmp_info_t *info)
 {
-	if (cl_debuglevel)
-		printf("swdptap_init\n");
 	uint8_t cmd[2] = {CMD_GET_SELECT_IF, JLINK_IF_GET_AVAILABLE};
 	uint8_t res[4];
 	send_recv(info->usb_link, cmd, 2, res, sizeof(res));
@@ -190,46 +188,8 @@ int jlink_swdp_scan(bmp_info_t *info)
 	return target_list?1:0;
 }
 
-void ap_decode_access(uint16_t addr, uint8_t RnW)
-{
-	if (RnW)
-		printf("Read ");
-	else
-		printf("Write ");
-	switch(addr) {
-	case 0x00:
-		if (RnW)
-			printf("DP_DPIDR: ");
-		else
-			printf("DP_ABORT: ");
-		break;
-	case 0x004: printf("CTRL/STAT: ");
-		break;
-	case 0x008:
-		if (RnW)
-			printf("RESEND: ");
-		else
-			printf("DP_SELECT: ");
-		break;
-	case 0x00c: printf("DP_RDBUFF: ");
-		break;
-	case 0x100: printf("AP_CSW: ");
-		break;
-	case 0x104: printf("AP_TAR: ");
-		break;
-	case 0x10c: printf("AP_DRW: ");
-		break;
-	case 0x1f8: printf("AP_BASE: ");
-		break;
-	case 0x1fc: printf("AP_IDR: ");
-		break;
-	}
-}
-
 static uint32_t jlink_adiv5_swdp_read(ADIv5_DP_t *dp, uint16_t addr)
 {
-	if (cl_debuglevel)
-		printf("jlink_adiv5_swdp_read 0x%04x\n", addr);
 	if (addr & ADIV5_APnDP) {
 		adiv5_dp_low_access(dp, ADIV5_LOW_READ, addr, 0);
 		return adiv5_dp_low_access(dp, ADIV5_LOW_READ,
@@ -266,17 +226,6 @@ static uint32_t jlink_adiv5_swdp_error(ADIv5_DP_t *dp)
 static uint32_t jlink_adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 				      uint16_t addr, uint32_t value)
 {
-	if (cl_debuglevel) {
-		if (RnW) {
-			if (cl_debuglevel > 0) {
-				ap_decode_access(addr, RnW);
-				printf(" start:\n");
-			}
-		}else{
-			ap_decode_access(addr, RnW);
-			printf(" %08" PRIx32 "\n", value);
-		}
-	}
 	bool APnDP = addr & ADIV5_APnDP;
 	uint8_t addr8 = addr & 0xff;
 	uint8_t request = 0x81;
@@ -316,14 +265,14 @@ static uint32_t jlink_adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 		raise_exception(EXCEPTION_TIMEOUT, "SWDP ACK timeout");
 
 	if(ack == SWDP_ACK_FAULT) {
-		if (cl_debuglevel)
+		if (cl_debuglevel & BMP_DEBUG_PLATFORM)
 			fprintf(stderr, "Fault\n");
 		dp->fault = 1;
 		return 0;
 	}
 
 	if(ack != SWDP_ACK_OK) {
-		if (cl_debuglevel)
+		if (cl_debuglevel & BMP_DEBUG_PLATFORM)
 			fprintf(stderr, "Protocol\n");
 		line_reset(&info);
 		return 0;
@@ -342,10 +291,6 @@ 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 (cl_debuglevel ) {
-			ap_decode_access(addr, RnW);;
-			printf("0x%08" PRIx32 "\n", response);
-		}
 		if (bit_count & 1)  /* Give up on parity error */
 			raise_exception(EXCEPTION_ERROR, "SWDP Parity error");
 	} else {
diff --git a/src/platforms/hosted/jlink_jtagtap.c b/src/platforms/hosted/jlink_jtagtap.c
index d69d904..6ea1cbd 100644
--- a/src/platforms/hosted/jlink_jtagtap.c
+++ b/src/platforms/hosted/jlink_jtagtap.c
@@ -40,7 +40,7 @@ static void jtagtap_reset(void)
 
 static void jtagtap_tms_seq(uint32_t MS, int ticks)
 {
-	if (cl_debuglevel)
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
 		printf("jtagtap_tms_seq 0x%08" PRIx32 ", ticks %d\n", MS, ticks);
 	int len = (ticks + 7) / 8;
 	uint8_t cmd[12];
@@ -68,7 +68,7 @@ static void jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms,
 	if (!ticks)
 		return;
 	int len = (ticks + 7) / 8;
-	if (cl_debuglevel) {
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
 		printf("jtagtap_tdi_tdo %s, ticks %d, DI: ",
 			   (final_tms) ? "Final TMS" : "", ticks);
 		for (int i = 0; i < len; i++) {
@@ -103,12 +103,24 @@ static void jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms,
 static void jtagtap_tdi_seq(const uint8_t final_tms, const uint8_t *DI,
 							int ticks)
 {
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+		printf("jtagtap_tdi_seq %s:", (final_tms)? "final_tms" : "");
+		const uint8_t *p = DI;
+		unsigned int i = (ticks & 7) & ~7 ;
+		if (i > 16)
+			i = 16;
+		while (i--)
+			printf(" %02x", *p++);
+		if (ticks > (16 * 8))
+			printf(" ...");
+		printf("\n");
+	}
 	return jtagtap_tdi_tdo_seq(NULL,  final_tms, DI, ticks);
 }
 
 static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 {
-	if (cl_debuglevel)
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
 		printf("jtagtap_next TMS 0x%02x, TDI %02x\n", dTMS, dTDI);
 	uint8_t cmd[6];
 	cmd[0] = CMD_HW_JTAG3;
@@ -128,7 +140,7 @@ static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 
 int jlink_jtagtap_init(bmp_info_t *info, jtag_proc_t *jtag_proc)
 {
-	if (cl_debuglevel)
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
 		printf("jtap_init\n");
 	uint8_t cmd_switch[2] = {CMD_GET_SELECT_IF, JLINK_IF_GET_AVAILABLE};
 	uint8_t res[4];
diff --git a/src/platforms/hosted/libftdi_jtagtap.c b/src/platforms/hosted/libftdi_jtagtap.c
index 1724d3f..7a009ab 100644
--- a/src/platforms/hosted/libftdi_jtagtap.c
+++ b/src/platforms/hosted/libftdi_jtagtap.c
@@ -49,21 +49,21 @@ int libftdi_jtagtap_init(jtag_proc_t *jtag_proc)
 	assert(ftdic != NULL);
 	int err = ftdi_usb_purge_buffers(ftdic);
 	if (err != 0) {
-		fprintf(stderr, "ftdi_usb_purge_buffer: %d: %s\n",
+		printf("ftdi_usb_purge_buffer: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		abort();
 	}
 	/* Reset MPSSE controller. */
 	err = ftdi_set_bitmode(ftdic, 0,  BITMODE_RESET);
 	if (err != 0) {
-		fprintf(stderr, "ftdi_set_bitmode: %d: %s\n",
+		printf("ftdi_set_bitmode: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		return -1;
 	}
 	/* Enable MPSSE controller. Pin directions are set later.*/
 	err = ftdi_set_bitmode(ftdic, 0, BITMODE_MPSSE);
 	if (err != 0) {
-		fprintf(stderr, "ftdi_set_bitmode: %d: %s\n",
+		printf("ftdi_set_bitmode: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		return -1;
 	}
diff --git a/src/platforms/hosted/libftdi_swdptap.c b/src/platforms/hosted/libftdi_swdptap.c
index 0798901..6feb70d 100644
--- a/src/platforms/hosted/libftdi_swdptap.c
+++ b/src/platforms/hosted/libftdi_swdptap.c
@@ -43,26 +43,26 @@ static void swdptap_seq_out_parity(uint32_t MS, int ticks);
 int libftdi_swdptap_init(swd_proc_t *swd_proc)
 {
 	if (!active_cable->bitbang_tms_in_pin) {
-		DEBUG("SWD not possible or missing item in cable description.\n");
+		printf("SWD not possible or missing item in cable description.\n");
 		return -1;
 	}
 	int err = ftdi_usb_purge_buffers(ftdic);
 	if (err != 0) {
-		fprintf(stderr, "ftdi_usb_purge_buffer: %d: %s\n",
+		printf("ftdi_usb_purge_buffer: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		return -1;
 	}
 	/* Reset MPSSE controller. */
 	err = ftdi_set_bitmode(ftdic, 0,  BITMODE_RESET);
 	if (err != 0) {
-		fprintf(stderr, "ftdi_set_bitmode: %d: %s\n",
+		printf("ftdi_set_bitmode: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		return -1;
 	}
 	/* Enable MPSSE controller. Pin directions are set later.*/
 	err = ftdi_set_bitmode(ftdic, 0, BITMODE_MPSSE);
 	if (err != 0) {
-		fprintf(stderr, "ftdi_set_bitmode: %d: %s\n",
+		printf("ftdi_set_bitmode: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		return -1;
 	}
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 430ed72..45366e6 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -469,3 +469,140 @@ void platform_buffer_flush(void)
 		break;
 	}
 }
+
+static void ap_decode_access(uint16_t addr, uint8_t RnW)
+{
+	if (RnW)
+		printf("Read  ");
+	else
+		printf("Write ");
+	switch(addr) {
+	case 0x00:
+		if (RnW)
+			printf("DP_DPIDR :");
+		else
+			printf("DP_ABORT :");
+		break;
+	case 0x004: printf("CTRL/STAT:");
+		break;
+	case 0x008:
+		if (RnW)
+			printf("RESEND   :");
+		else
+			printf("DP_SELECT:");
+		break;
+	case 0x00c: printf("DP_RDBUFF:");
+		break;
+	case 0x100: printf("AP_CSW   :");
+		break;
+	case 0x104: printf("AP_TAR   :");
+		break;
+	case 0x10c: printf("AP_DRW   :");
+		break;
+	case 0x1f8: printf("AP_BASE  :");
+		break;
+	case 0x1fc: printf("AP_IDR   :");
+		break;
+	}
+}
+
+void adiv5_dp_write(ADIv5_DP_t *dp, uint16_t addr, uint32_t value)
+{
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+		ap_decode_access(addr, ADIV5_LOW_WRITE);
+		printf(" 0x%08" PRIx32 "\n", value);
+	}
+	dp->low_access(dp, ADIV5_LOW_WRITE, addr, value);
+}
+
+uint32_t adiv5_dp_read(ADIv5_DP_t *dp, uint16_t addr)
+{
+	uint32_t ret = dp->dp_read(dp, addr);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+		ap_decode_access(addr, ADIV5_LOW_READ);
+		printf(" 0x%08" PRIx32 "\n", ret);
+	}
+	return ret;
+}
+
+uint32_t adiv5_dp_error(ADIv5_DP_t *dp)
+{
+	uint32_t ret = dp->error(dp);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+		printf("DP Error 0x%08" PRIx32 "\n", ret);
+	}
+	return ret;
+}
+
+uint32_t adiv5_dp_low_access(struct ADIv5_DP_s *dp, uint8_t RnW,
+							 uint16_t addr, uint32_t value)
+{
+	uint32_t ret = dp->low_access(dp, RnW, addr, value);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+		ap_decode_access(addr, RnW);
+		printf(" 0x%08" PRIx32 "\n", (RnW)? ret : value);
+	}
+	return ret;
+}
+
+uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
+{
+	uint32_t ret = ap->dp->ap_read(ap, addr);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+		ap_decode_access(addr, ADIV5_LOW_READ);
+		printf(" 0x%08" PRIx32 "\n", ret);
+	}
+	return ret;
+}
+
+void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+{
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+		ap_decode_access(addr, ADIV5_LOW_WRITE);
+		printf(" 0x%08" PRIx32 "\n", value);
+	}
+	return ap->dp->ap_write(ap, addr, value);
+}
+
+void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
+{
+	ap->dp->mem_read(ap, dest, src, len);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+		printf("ap_memread @ %" PRIx32 " len %" PRIx32 ":", src, (uint32_t)len);
+		uint8_t *p = (uint8_t *) dest;
+		unsigned int i = len;
+		if (i > 16)
+			i = 16;
+		while (i--)
+			printf(" %02x", *p++);
+		if (len > 16)
+			printf(" ...");
+		printf("\n");
+	}
+	return;
+}
+void adiv5_mem_write_sized(	ADIv5_AP_t *ap, uint32_t dest, const void *src,
+							size_t len, enum align align)
+{
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+		printf("ap_mem_write_sized @ %" PRIx32 " len %" PRIx32 ", align %d:",
+			   dest, (uint32_t)len, 1 << align);
+		uint8_t *p = (uint8_t *) src;
+		unsigned int i = len;
+		if (i > 16)
+			i = 16;
+		while (i--)
+			printf(" %02x", *p++);
+		if (len > 16)
+			printf(" ...");
+		printf("\n");
+	}
+	return ap->dp->mem_write_sized(ap, dest, src, len, align);
+}
+
+void adiv5_dp_abort(struct ADIv5_DP_s *dp, uint32_t abort)
+{
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("Abort: %08" PRIx32 "\n", abort);
+	return dp->abort(dp, abort);
+}
diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index 2d50a72..b6ce189 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -232,43 +232,43 @@ static int stlink_usb_error_check(uint8_t *data, bool verbose)
 			return STLINK_ERROR_OK;
 		case STLINK_DEBUG_ERR_FAULT:
 			if (verbose)
-				DEBUG("SWD fault response (0x%x)\n", STLINK_DEBUG_ERR_FAULT);
+				printf("SWD fault response (0x%x)\n", STLINK_DEBUG_ERR_FAULT);
 			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_UNKNOWN_JTAG_CHAIN:
 			if (verbose)
-				DEBUG("Unknown JTAG chain\n");
+				printf("Unknown JTAG chain\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_NO_DEVICE_CONNECTED:
 			if (verbose)
-				DEBUG("No device connected\n");
+				printf("No device connected\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_COMMAND_ERROR:
 			if (verbose)
-				DEBUG("Command error\n");
+				printf("Command error\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_GET_IDCODE_ERROR:
 			if (verbose)
-				DEBUG("Failure reading IDCODE\n");
+				printf("Failure reading IDCODE\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_DBG_POWER_ERROR:
 			if (verbose)
-				DEBUG("Failure powering DBG\n");
+				printf("Failure powering DBG\n");
 			return STLINK_ERROR_WAIT;
 		case STLINK_SWD_AP_WAIT:
 			if (verbose)
-				DEBUG("wait status SWD_AP_WAIT (0x%x)\n", STLINK_SWD_AP_WAIT);
+				printf("wait status SWD_AP_WAIT (0x%x)\n", STLINK_SWD_AP_WAIT);
 			return STLINK_ERROR_WAIT;
 		case STLINK_SWD_DP_WAIT:
 			if (verbose)
-				DEBUG("wait status SWD_DP_WAIT (0x%x)\n", STLINK_SWD_DP_WAIT);
+				printf("wait status SWD_DP_WAIT (0x%x)\n", STLINK_SWD_DP_WAIT);
 			return STLINK_ERROR_WAIT;
 		case STLINK_JTAG_WRITE_ERROR:
 			if (verbose)
-				DEBUG("Write error\n");
+				printf("Write error\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_WRITE_VERIF_ERROR:
 			if (verbose)
-				DEBUG("Write verify error, ignoring\n");
+				printf("Write verify error, ignoring\n");
 			return STLINK_ERROR_OK;
 		case STLINK_SWD_AP_FAULT:
 			/* git://git.ac6.fr/openocd commit 657e3e885b9ee10
@@ -278,40 +278,40 @@ static int stlink_usb_error_check(uint8_t *data, bool verbose)
 			 */
 			Stlink.ap_error = true;
 			if (verbose)
-				DEBUG("STLINK_SWD_AP_FAULT\n");
+				printf("STLINK_SWD_AP_FAULT\n");
 			return STLINK_ERROR_DP_FAULT;
 		case STLINK_SWD_AP_ERROR:
 			if (verbose)
-				DEBUG("STLINK_SWD_AP_ERROR\n");
+				printf("STLINK_SWD_AP_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_AP_PARITY_ERROR:
 			if (verbose)
-				DEBUG("STLINK_SWD_AP_PARITY_ERROR\n");
+				printf("STLINK_SWD_AP_PARITY_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_DP_FAULT:
 			if (verbose)
-				DEBUG("STLINK_SWD_DP_FAULT\n");
+				printf("STLINK_SWD_DP_FAULT\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_DP_ERROR:
 			if (verbose)
-				DEBUG("STLINK_SWD_DP_ERROR\n");
+				printf("STLINK_SWD_DP_ERROR\n");
 			raise_exception(EXCEPTION_ERROR, "STLINK_SWD_DP_ERROR");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_DP_PARITY_ERROR:
 			if (verbose)
-				DEBUG("STLINK_SWD_DP_PARITY_ERROR\n");
+				printf("STLINK_SWD_DP_PARITY_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_AP_WDATA_ERROR:
 			if (verbose)
-				DEBUG("STLINK_SWD_AP_WDATA_ERROR\n");
+				printf("STLINK_SWD_AP_WDATA_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_AP_STICKY_ERROR:
 			if (verbose)
-				DEBUG("STLINK_SWD_AP_STICKY_ERROR\n");
+				printf("STLINK_SWD_AP_STICKY_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_AP_STICKYORUN_ERROR:
 			if (verbose)
-				DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR\n");
+				printf("STLINK_SWD_AP_STICKYORUN_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_BAD_AP_ERROR:
 			/* ADIV5 probe 256 APs, most of them are non exisitant.*/
@@ -319,15 +319,15 @@ static int stlink_usb_error_check(uint8_t *data, bool verbose)
 		case STLINK_TOO_MANY_AP_ERROR:
 			/* TI TM4C duplicates AP. Error happens at AP9.*/
 			if (verbose)
-				DEBUG("STLINK_TOO_MANY_AP_ERROR\n");
+				printf("STLINK_TOO_MANY_AP_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_UNKNOWN_CMD :
 			if (verbose)
-				DEBUG("STLINK_JTAG_UNKNOWN_CMD\n");
+				printf("STLINK_JTAG_UNKNOWN_CMD\n");
 			return STLINK_ERROR_FAIL;
 		default:
 			if (verbose)
-				DEBUG("unknown/unexpected STLINK status code 0x%x\n", data[0]);
+				printf("unknown/unexpected STLINK status code 0x%x\n", data[0]);
 			return STLINK_ERROR_FAIL;
 	}
 }
@@ -345,7 +345,7 @@ static int stlink_send_recv_retry(uint8_t *txbuf, size_t txsize,
 			return res;
 		uint32_t now = platform_time_ms();
 		if (((now - start) > 1000) || (res != STLINK_ERROR_WAIT)) {
-			DEBUG("write_retry failed. ");
+			printf("write_retry failed. ");
 			return res;
 		}
 	}
@@ -364,7 +364,7 @@ static int read_retry(uint8_t *txbuf, size_t txsize,
 			return res;
 		uint32_t now = platform_time_ms();
 		if (((now -start) > 1000) || (res != STLINK_ERROR_WAIT)) {
-			DEBUG("read_retry failed. ");
+			printf("read_retry failed. ");
 			stlink_usb_get_rw_status(true);
 			return res;
 		}
@@ -430,15 +430,15 @@ static void stlink_version(bmp_info_t *info)
 			Stlink.ver_swim = (version >> 0) & 0x3f;
 		}
 	}
-	DEBUG("V%dJ%d",Stlink.ver_stlink, Stlink.ver_jtag);
+	printf("V%dJ%d",Stlink.ver_stlink, Stlink.ver_jtag);
 	if (Stlink.ver_hw == 30) {
-		DEBUG("M%dB%dS%d", Stlink.ver_mass, Stlink.ver_bridge, Stlink.ver_swim);
+		printf("M%dB%dS%d", Stlink.ver_mass, Stlink.ver_bridge, Stlink.ver_swim);
 	} else if (Stlink.ver_hw == 20) {
-		DEBUG("S%d", Stlink.ver_swim);
+		printf("S%d", Stlink.ver_swim);
 	} else if (Stlink.ver_hw == 21) {
-		DEBUG("M%d", Stlink.ver_mass);
+		printf("M%d", Stlink.ver_mass);
 	}
-	DEBUG("\n");
+	printf("\n");
 }
 
 static bool stlink_leave_state(bmp_info_t *info)
@@ -448,23 +448,23 @@ static bool stlink_leave_state(bmp_info_t *info)
 	send_recv(info->usb_link,cmd, 16, data, 2);
 	if (data[0] == STLINK_DEV_DFU_MODE) {
 		uint8_t dfu_cmd[16] = {STLINK_DFU_COMMAND, STLINK_DFU_EXIT};
-		DEBUG("Leaving DFU Mode\n");
+		printf("Leaving DFU Mode\n");
 		send_recv(info->usb_link, dfu_cmd, 16, NULL, 0);
 		return true;
 	} else if (data[0] == STLINK_DEV_SWIM_MODE) {
 		uint8_t swim_cmd[16] = {STLINK_SWIM_COMMAND, STLINK_SWIM_EXIT};
-		DEBUG("Leaving SWIM Mode\n");
+		printf("Leaving SWIM Mode\n");
 		send_recv(info->usb_link, swim_cmd, 16, NULL, 0);
 	} else if (data[0] == STLINK_DEV_DEBUG_MODE) {
 		uint8_t dbg_cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_EXIT};
-		DEBUG("Leaving DEBUG Mode\n");
+		printf("Leaving DEBUG Mode\n");
 		send_recv(info->usb_link, dbg_cmd, 16, NULL, 0);
 	} else if (data[0] == STLINK_DEV_BOOTLOADER_MODE) {
-		DEBUG("Leaving BOOTLOADER Mode\n");
+		printf("Leaving BOOTLOADER Mode\n");
 	} else if (data[0] == STLINK_DEV_MASS_MODE) {
-		DEBUG("Leaving MASS Mode\n");
+		printf("Leaving MASS Mode\n");
 	} else {
-		DEBUG("Unknown Mode %02x\n", data[0]);
+		printf("Unknown Mode %02x\n", data[0]);
 	}
 	return false;
 }
@@ -593,7 +593,7 @@ int stlink_init(bmp_info_t *info)
     }
 	if ((Stlink.ver_stlink < 3 && Stlink.ver_jtag < 32) ||
 		(Stlink.ver_stlink == 3 && Stlink.ver_jtag < 3)) {
-		DEBUG("Please update Firmware\n");
+		printf("Please update Firmware\n");
 		return -1;
 	}
 	if (stlink_leave_state(info)) {
@@ -646,7 +646,7 @@ static bool stlink3_set_freq_divisor(bmp_info_t *info, uint16_t divisor)
 		divisor = size;
 	uint8_t *p = data + 12 + divisor * sizeof(uint32_t);
 	uint32_t freq = p[0] | p[1] << 8 | p[2] << 16 | p[3] << 24;
-	DEBUG("Selected %" PRId32 " khz\n", freq);
+	printf("Selected %" PRId32 " khz\n", freq);
 	cmd[1] = STLINK_APIV3_SET_COM_FREQ;
 	cmd[2] = Stlink.transport_mode;
 	cmd[3] = 0;
@@ -676,7 +676,7 @@ static int stlink_enter_debug_jtag(bmp_info_t *info)
 					  STLINK_DEBUG_APIV2_ENTER,
 					  STLINK_DEBUG_ENTER_JTAG_NO_RESET};
 	uint8_t data[2];
-	DEBUG("Enter JTAG\n");
+	printf("Enter JTAG\n");
 	send_recv(info->usb_link, cmd, 16, data, 2);
 	return stlink_usb_error_check(data, true);
 }
@@ -688,7 +688,7 @@ static uint32_t stlink_read_coreid(void)
 	uint8_t data[4];
 	send_recv(info.usb_link, cmd, 16, data, 4);
 	uint32_t id =  data[0] | data[1] << 8 | data[2] << 16 | data[3] << 24;
-	DEBUG("Read Core ID: 0x%08" PRIx32 "\n", id);
+	printf("Read Core ID: 0x%08" PRIx32 "\n", id);
 	return id;
 }
 
@@ -713,12 +713,10 @@ uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 	static uint32_t stlink_dp_read(ADIv5_DP_t *dp, uint16_t addr)
 {
 	if (addr & ADIV5_APnDP) {
-		DEBUG_STLINK("AP read addr 0x%04" PRIx16 "\n", addr);
 		stlink_dp_low_access(dp, ADIV5_LOW_READ, addr, 0);
 		return stlink_dp_low_access(dp, ADIV5_LOW_READ,
 		                           ADIV5_DP_RDBUFF, 0);
 	} else {
-		DEBUG_STLINK("DP read addr 0x%04" PRIx16 "\n", addr);
 		return stlink_dp_low_access(dp, ADIV5_LOW_READ, addr, 0);
 	}
 }
@@ -743,7 +741,7 @@ uint32_t stlink_dp_error(ADIv5_DP_t *dp)
 	adiv5_dp_write(dp, ADIV5_DP_ABORT, clr);
 	dp->fault = 0;
 	if (err)
-		DEBUG("stlink_dp_error %d\n", err);
+		printf("stlink_dp_error %d\n", err);
 	err |= Stlink.ap_error;
 	Stlink.ap_error = false;
 	return err;
@@ -764,15 +762,13 @@ static int stlink_read_dp_register(uint16_t port, uint16_t addr, uint32_t *reg)
 		cmd[4] = ((Stlink.dap_select & 0xf) << 4) | (addr & 0xf);
 	else
 		cmd[4] = addr & 0xff;
-	DEBUG_STLINK("Read DP, Addr 0x%04" PRIx16 ": \n", addr);
 	uint8_t data[8];
 	int res = stlink_send_recv_retry(cmd, 16, data, 8);
 	if (res == STLINK_ERROR_OK) {
 		uint32_t ret = data[4] | data[5] << 8 | data[6] << 16 | data[7] << 24;
-		DEBUG_STLINK("0x%08" PRIx32" \n", ret);
 		*reg = ret;
 	} else {
-		DEBUG_STLINK("failed, res %d\n", res);
+		printf("%s error %d\n", __func__, res);
 	}
 	return res;
 }
@@ -781,7 +777,8 @@ static int stlink_write_dp_register(uint16_t port, uint16_t addr, uint32_t val)
 {
 	if (port == STLINK_DEBUG_PORT_ACCESS && addr == 8) {
 		Stlink.dap_select = val;
-		DEBUG_STLINK("Caching SELECT 0x%02" PRIx32 "\n", val);
+		if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+			printf("Caching SELECT 0x%02" PRIx32 "\n", val);
 		return STLINK_ERROR_OK;
 	} else {
 		uint8_t cmd[16] = {
@@ -792,8 +789,6 @@ static int stlink_write_dp_register(uint16_t port, uint16_t addr, uint32_t val)
 			(val >> 24) & 0xff};
 		uint8_t data[2];
 		stlink_send_recv_retry(cmd, 16, data, 2);
-		DEBUG_STLINK("Write DP, Addr 0x%04" PRIx16 ": 0x%08" PRIx32
-			  " \n", addr, val);
 		return stlink_usb_error_check(data, true);
 	}
 }
@@ -806,11 +801,7 @@ uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 	if (RnW) {
 		res = stlink_read_dp_register(
 			STLINK_DEBUG_PORT_ACCESS, addr, &response);
-		DEBUG_STLINK("SWD read addr %04" PRIx16 ": %08" PRIx32 "\n",
-					 addr, response);
 	} else {
-		DEBUG_STLINK("SWD write addr %04" PRIx16 ": %08" PRIx32 "\n",
-					 addr, value);
 		res = stlink_write_dp_register(STLINK_DEBUG_PORT_ACCESS, addr, value);
 	}
 	if (res == STLINK_ERROR_WAIT)
@@ -837,11 +828,12 @@ static bool stlink_ap_setup(int ap)
 	};
 	uint8_t data[2];
 	send_recv(info.usb_link, cmd, 16, data, 2);
-	DEBUG_STLINK("Open AP %d\n", ap);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("Open AP %d\n", ap);
 	int res = stlink_usb_error_check(data, true);
 	if (res) {
 		if (Stlink.ver_hw == 30) {
-			DEBUG("STLINKV3 only connects to STM8/32!\n");
+			printf("STLINKV3 only connects to STM8/32!\n");
 		}
 		return false;
 	}
@@ -857,7 +849,8 @@ static void stlink_ap_cleanup(int ap)
        };
        uint8_t data[2];
        send_recv(info.usb_link, cmd, 16, data, 2);
-       DEBUG_STLINK("Close AP %d\n", ap);
+	   if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		   printf("Close AP %d\n", ap);
        stlink_usb_error_check(data, true);
 }
 static int stlink_usb_get_rw_status(bool verbose)
@@ -877,26 +870,19 @@ static void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 		return;
 	size_t read_len = len;
 	uint8_t type;
-	char *CMD;
 	if (src & 1 || len & 1) {
-		CMD = "READMEM_8BIT";
 		type = STLINK_DEBUG_READMEM_8BIT;
 		if (len > Stlink.block_size) {
-			DEBUG(" Too large!\n");
+			printf(" Too large!\n");
 			return;
 		}
 		if (len == 1)
 			read_len ++; /* Fix read length as in openocd*/
 	} else if (src & 3 || len & 3) {
-		CMD = "READMEM_16BIT";
 		type = STLINK_DEBUG_APIV2_READMEM_16BIT;
 	} else {
-		CMD = "READMEM_32BIT";
 		type = STLINK_DEBUG_READMEM_32BIT;
-
 	}
-	DEBUG_STLINK("%s len %zu addr 0x%08" PRIx32 " AP %d : ",
-				 CMD, len, src, ap->apsel);
 	uint8_t cmd[16] = {
 		STLINK_DEBUG_COMMAND,
 		type,
@@ -904,19 +890,14 @@ static void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 		(src >> 24) & 0xff,
 		len & 0xff, len >> 8, ap->apsel};
 	int res = read_retry(cmd, 16, dest, read_len);
-	if (res == STLINK_ERROR_OK) {
-		uint8_t *p = (uint8_t*)dest;
-		for (size_t i = 0; i < len ; i++) {
-			DEBUG_STLINK("%02x", *p++);
-		}
-	} else {
+	if (res != STLINK_ERROR_OK) {
 		/* FIXME: What is the right measure when failing?
 		 *
 		 * E.g. TM4C129 gets here when NRF probe reads 0x10000010
 		 * Approach taken:
 		 * Fill the memory with some fixed pattern so hopefully
 		 * the caller notices the error*/
-		DEBUG("stlink_readmem failed\n");
+		printf("stlink_readmem failed\n");
 		memset(dest, 0xff, len);
 	}
 	DEBUG_STLINK("\n");
@@ -926,12 +907,6 @@ static void stlink_writemem8(usb_link_t *link, ADIv5_AP_t *ap, uint32_t addr,
 							 size_t len, uint8_t *buffer)
 {
 	(void)link;
-	DEBUG_STLINK("Mem Write8 AP %d len %zu addr 0x%08" PRIx32 ": ",
-				 ap->apsel, len, addr);
-	for (size_t t = 0; t < len; t++) {
-		DEBUG_STLINK("%02x", buffer[t]);
-	}
-	DEBUG_STLINK("\n");
 	while (len) {
 		size_t length;
 		/* OpenOCD has some note about writemem8*/
@@ -957,12 +932,6 @@ static void stlink_writemem16(usb_link_t *link, ADIv5_AP_t *ap, uint32_t addr,
 							  size_t len, uint16_t *buffer)
 {
 	(void)link;
-	DEBUG_STLINK("Mem Write16 AP %d len %zu addr 0x%08" PRIx32 ": ",
-				 ap->apsel, len, addr);
-	for (size_t t = 0; t < len; t+=2) {
-		DEBUG_STLINK("%04x", buffer[t]);
-	}
-	DEBUG_STLINK("\n");
 	uint8_t cmd[16] = {
 		STLINK_DEBUG_COMMAND,
 		STLINK_DEBUG_APIV2_WRITEMEM_16BIT,
@@ -978,12 +947,6 @@ static void stlink_writemem32(usb_link_t * link, ADIv5_AP_t *ap, uint32_t addr,
 							  size_t len, uint32_t *buffer)
 {
 	(void)link;
-	DEBUG_STLINK("Mem Write32 AP %d len %zu addr 0x%08" PRIx32 ": ",
-				 ap->apsel, len, addr);
-	for (size_t t = 0; t < len; t+=4) {
-		DEBUG_STLINK("%04x", buffer[t]);
-	}
-	DEBUG_STLINK("\n");
 	uint8_t cmd[16] = {
 		STLINK_DEBUG_COMMAND,
 		STLINK_DEBUG_WRITEMEM_32BIT,
@@ -998,7 +961,8 @@ static void stlink_regs_read(ADIv5_AP_t *ap, void *data)
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READALLREGS,
 					   ap->apsel};
 	uint8_t res[88];
-	DEBUG_STLINK("AP %d: Read all core registers\n", ap->apsel);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("AP %d: Read all core registers\n", ap->apsel);
 	send_recv(info.usb_link, cmd, 16, res, 88);
 	stlink_usb_error_check(res, true);
 	memcpy(data, res + 4, 84);
@@ -1012,8 +976,9 @@ static uint32_t stlink_reg_read(ADIv5_AP_t *ap, int num)
 	send_recv(info.usb_link, cmd, 16, res, 8);
 	stlink_usb_error_check(res, true);
 	uint32_t ret = res[0] | res[1] << 8 | res[2] << 16 | res[3] << 24;
-	DEBUG_STLINK("AP %d: Read reg %02" PRId32 " val 0x%08" PRIx32 "\n",
-				 ap->apsel, num, ret);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("AP %d: Read reg %02" PRId32 " val 0x%08" PRIx32 "\n",
+			   ap->apsel, num, ret);
 	return ret;
 }
 
@@ -1025,8 +990,9 @@ static void stlink_reg_write(ADIv5_AP_t *ap, int num, uint32_t val)
 		(val >> 24) & 0xff, ap->apsel};
 	uint8_t res[2];
 	send_recv(info.usb_link, cmd, 16, res, 2);
-	DEBUG_STLINK("AP %d: Write reg %02" PRId32 " val 0x%08" PRIx32 "\n",
-				 ap->apsel, num, val);
+	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
+		printf("AP %d: Write reg %02" PRId32 " val 0x%08" PRIx32 "\n",
+			   ap->apsel, num, val);
 	stlink_usb_error_check(res, true);
 }
 
@@ -1129,7 +1095,7 @@ int stlink_enter_debug_swd(bmp_info_t *info, ADIv5_DP_t *dp)
 					  STLINK_DEBUG_APIV2_ENTER,
 					  STLINK_DEBUG_ENTER_SWD_NO_RESET};
 	uint8_t data[2];
-	DEBUG("Enter SWD\n");
+	printf("Enter SWD\n");
 	send_recv(info->usb_link, cmd, 16, data, 2);
 	if (stlink_usb_error_check(data, true))
 		return -1;
diff --git a/src/platforms/pc/libusb_utils.c b/src/platforms/pc/libusb_utils.c
index 2672490..1e090d5 100644
--- a/src/platforms/pc/libusb_utils.c
+++ b/src/platforms/pc/libusb_utils.c
@@ -93,7 +93,7 @@ int send_recv(usb_link_t *link,
 								  link->ep_tx | LIBUSB_ENDPOINT_OUT,
 								  txbuf, txlen,
 								  NULL, NULL, 0);
-		if (cl_debuglevel > 0) {
+		if (cl_debuglevel & BMP_DEBUG_WIRE) {
 			int i = 0;
 			printf(" Send (%3d): ", txlen);
 			for (; i < txlen; i++) {
@@ -127,7 +127,7 @@ int send_recv(usb_link_t *link,
 		if (res >0) {
 			int i;
 			uint8_t *p = rxbuf;
-			if (cl_debuglevel > 0) {
+			if (cl_debuglevel & BMP_DEBUG_WIRE) {
 				printf(" Rec (%zu/%d)", rxsize, res);
 				for (i = 0; i < res && i < 32 ; i++) {
 					if ( i && ((i & 7) == 0))
@@ -137,7 +137,7 @@ int send_recv(usb_link_t *link,
 			}
 		}
 	}
-	if (cl_debuglevel > 0)
+	if (cl_debuglevel & BMP_DEBUG_WIRE)
 		printf("\n");
 	return res;
 }
diff --git a/src/platforms/pc/serial_unix.c b/src/platforms/pc/serial_unix.c
index 2af20ba..0135956 100644
--- a/src/platforms/pc/serial_unix.c
+++ b/src/platforms/pc/serial_unix.c
@@ -139,8 +139,8 @@ int platform_buffer_write(const uint8_t *data, int size)
 {
 	int s;
 
-	if (cl_debuglevel)
-		printf("%s\n",data);
+	if (cl_debuglevel &  BMP_DEBUG_WIRE)
+		printf("%s\n", data);
 	s = write(fd, data, size);
 	if (s < 0) {
 		printf("Failed to write\n");
@@ -197,7 +197,7 @@ int platform_buffer_read(uint8_t *data, int maxsize)
 		s = read(fd, c, 1);
 		if (*c==REMOTE_EOM) {
 			*c = 0;
-			if (cl_debuglevel)
+			if (cl_debuglevel &  BMP_DEBUG_WIRE)
 				printf("       %s\n",data);
 			return (c - data);
 		} else {
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index d2f2142..2dc0a8d 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -241,11 +241,6 @@ void adiv5_ap_unref(ADIv5_AP_t *ap)
 	}
 }
 
-void adiv5_dp_write(ADIv5_DP_t *dp, uint16_t addr, uint32_t value)
-{
-	dp->low_access(dp, ADIV5_LOW_WRITE, addr, value);
-}
-
 static uint32_t adiv5_mem_read32(ADIv5_AP_t *ap, uint32_t addr)
 {
 	uint32_t ret;
@@ -483,29 +478,29 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 	 * correctly on STM32.	CDBGRSTACK is never asserted, and we
 	 * just wait forever.  This scenario is described in B2.4.1
 	 * so we have a timeout mechanism in addition to the sensing one.
-	 */
-
-	/* Write request for debug reset */
+	 *
+	 * Write request for debug reset */
 	adiv5_dp_write(dp, ADIV5_DP_CTRLSTAT,
 				   ctrlstat |= ADIV5_DP_CTRLSTAT_CDBGRSTREQ);
 
 	platform_timeout timeout;
-	platform_timeout_set(&timeout,200);
-	/* Wait for acknowledge */
-	while ((!platform_timeout_is_expired(&timeout)) &&
-		   (!((ctrlstat = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT)) & ADIV5_DP_CTRLSTAT_CDBGRSTACK))
-		);
-
+	platform_timeout_set(&timeout, 101);
 	/* Write request for debug reset release */
 	adiv5_dp_write(dp, ADIV5_DP_CTRLSTAT,
 				   ctrlstat &= ~ADIV5_DP_CTRLSTAT_CDBGRSTREQ);
-
-	platform_timeout_set(&timeout,200);
 	/* Wait for acknowledge */
-	while ((!platform_timeout_is_expired(&timeout)) &&
-		   (adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT) & ADIV5_DP_CTRLSTAT_CDBGRSTACK)
-		);
-	DEBUG("RESET_SEQ %s\n", (platform_timeout_is_expired(&timeout)) ? "failed": "succeeded");
+	while(1) {
+		platform_delay(20);
+		ctrlstat = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT);
+		if (ctrlstat & ADIV5_DP_CTRLSTAT_CDBGRSTACK) {
+			DEBUG("RESET_SEQ succeeded.\n");
+			break;
+		}
+		if (platform_timeout_is_expired(&timeout)) {
+			DEBUG("RESET_SEQ failed\n");
+			break;
+		}
+	}
 
 	uint32_t dp_idcode = adiv5_dp_read(dp, ADIV5_DP_IDCODE);
 	if ((dp_idcode & ADIV5_DP_VERSION_MASK) == ADIV5_DPv2) {
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index 1a6132b..e337e3d 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -172,6 +172,18 @@ typedef struct ADIv5_DP_s {
 #endif
 } ADIv5_DP_t;
 
+struct ADIv5_AP_s {
+	int refcnt;
+
+	ADIv5_DP_t *dp;
+	uint8_t apsel;
+
+	uint32_t idr;
+	uint32_t base;
+	uint32_t csw;
+};
+
+#if PC_HOSTED == 0
 static inline uint32_t adiv5_dp_read(ADIv5_DP_t *dp, uint16_t addr)
 {
 	return dp->dp_read(dp, addr);
@@ -193,17 +205,6 @@ static inline void adiv5_dp_abort(struct ADIv5_DP_s *dp, uint32_t abort)
 	return dp->abort(dp, abort);
 }
 
-struct ADIv5_AP_s {
-	int refcnt;
-
-	ADIv5_DP_t *dp;
-	uint8_t apsel;
-
-	uint32_t idr;
-	uint32_t base;
-	uint32_t csw;
-};
-
 static inline uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 {
 	return ap->dp->ap_read(ap, addr);
@@ -219,21 +220,39 @@ static inline void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src,
 {
 	return ap->dp->mem_read(ap, dest, src, len);
 }
-static inline void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest,
-								  const void *src, size_t len, enum align align)
+
+static inline void adiv5_mem_write_sized(
+	ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len,
+	enum align align)
 {
 	return ap->dp->mem_write_sized(ap, dest, src, len, align);
 }
 
+static inline void adiv5_dp_write(ADIv5_DP_t *dp, uint16_t addr, uint32_t value)
+{
+	dp->low_access(dp, ADIV5_LOW_WRITE, addr, value);
+}
+
+#else
+uint32_t adiv5_dp_read(ADIv5_DP_t *dp, uint16_t addr);
+uint32_t adiv5_dp_error(ADIv5_DP_t *dp);
+uint32_t adiv5_dp_low_access(struct ADIv5_DP_s *dp, uint8_t RnW,
+							 uint16_t addr, uint32_t value);
+void adiv5_dp_abort(struct ADIv5_DP_s *dp, uint32_t abort);
+uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr);
+void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
+void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len);
+void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest,
+						   const void *src, size_t len, enum align align);
 void adiv5_dp_write(ADIv5_DP_t *dp, uint16_t addr, uint32_t value);
+#endif
+
 void adiv5_dp_init(ADIv5_DP_t *dp);
 void platform_adiv5_dp_defaults(ADIv5_DP_t *dp);
 ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel);
 void adiv5_ap_ref(ADIv5_AP_t *ap);
 void adiv5_ap_unref(ADIv5_AP_t *ap);
 
-void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value);
-
 void adiv5_jtag_dp_handler(jtag_dev_t *dev);
 int platform_jtag_dp_init(ADIv5_DP_t *dp);
 
@@ -257,5 +276,4 @@ uint32_t fw_adiv5_jtagdp_read(ADIv5_DP_t *dp, uint16_t addr);
 uint32_t firmware_swdp_error(ADIv5_DP_t *dp);
 
 void firmware_swdp_abort(ADIv5_DP_t *dp, uint32_t abort);
-
 #endif

From 4698a26bab59236efc0217068b49ae734853fd86 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sun, 3 May 2020 19:17:03 +0200
Subject: [PATCH 38/43] common/swdptap: Slight speed increase and better setup
 time for write.

---
 src/platforms/common/swdptap.c | 34 +++++++++++++++++++++-------------
 1 file changed, 21 insertions(+), 13 deletions(-)

diff --git a/src/platforms/common/swdptap.c b/src/platforms/common/swdptap.c
index 95cc68d..29278cc 100644
--- a/src/platforms/common/swdptap.c
+++ b/src/platforms/common/swdptap.c
@@ -27,6 +27,14 @@ enum {
 	SWDIO_STATUS_FLOAT = 0,
 	SWDIO_STATUS_DRIVE
 };
+static void swdptap_turnaround(int dir) __attribute__ ((optimize(3)));
+static uint32_t swdptap_seq_in(int ticks) __attribute__ ((optimize(3)));
+static bool swdptap_seq_in_parity(uint32_t *ret, int ticks)
+	__attribute__ ((optimize(3)));
+static void swdptap_seq_out(uint32_t MS, int ticks)
+	__attribute__ ((optimize(3)));
+static void swdptap_seq_out_parity(uint32_t MS, int ticks)
+	__attribute__ ((optimize(3)));
 
 static void swdptap_turnaround(int dir)
 {
@@ -76,33 +84,33 @@ static uint32_t swdptap_seq_in(int ticks)
 static bool swdptap_seq_in_parity(uint32_t *ret, int ticks)
 {
 	uint32_t index = 1;
-	uint8_t parity = 0;
 	uint32_t res = 0;
 	bool bit;
 	int len = ticks;
 
 	swdptap_turnaround(SWDIO_STATUS_FLOAT);
 	while (len--) {
+		gpio_clear(SWCLK_PORT, SWCLK_PIN);
 		bit = gpio_get(SWDIO_PORT, SWDIO_PIN);
 		gpio_set(SWCLK_PORT, SWCLK_PIN);
 		if (bit) {
 			res |= index;
-			parity ^= 1;
 		}
 		index <<= 1;
-		gpio_clear(SWCLK_PORT, SWCLK_PIN);
 	}
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
+	int parity = __builtin_popcount(res);
 	bit = gpio_get(SWDIO_PORT, SWDIO_PIN);
 	gpio_set(SWCLK_PORT, SWCLK_PIN);
 	if (bit)
-		parity ^= 1;
+		parity++;
 	gpio_clear(SWCLK_PORT, SWCLK_PIN);
 #ifdef DEBUG_SWD_BITS
 	for (int i = 0; i < len; i++)
 		DEBUG("%d", (res & (1 << i)) ? 1 : 0);
 #endif
 	*ret = res;
-	return parity;
+	return (parity & 1);
 }
 
 static void swdptap_seq_out(uint32_t MS, int ticks)
@@ -115,30 +123,30 @@ static void swdptap_seq_out(uint32_t MS, int ticks)
 	swdptap_turnaround(SWDIO_STATUS_DRIVE);
 	while (ticks--) {
 		gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
+		gpio_clear(SWCLK_PORT, SWCLK_PIN);
 		MS >>= 1;
 		data = MS & 1;
 		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
-		gpio_clear(SWCLK_PORT, SWCLK_PIN);
 	}
+	gpio_clear(SWCLK_PORT, SWCLK_PIN);
 }
 
 static void swdptap_seq_out_parity(uint32_t MS, int ticks)
 {
-	uint8_t parity = 0;
+	int parity = __builtin_popcount(MS);
 	int data = MS & 1;
 #ifdef DEBUG_SWD_BITS
 	for (int i = 0; i < ticks; i++)
 		DEBUG("%d", (MS & (1 << i)) ? 1 : 0);
 #endif
 	swdptap_turnaround(SWDIO_STATUS_DRIVE);
-
+	gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
+	MS >>= 1;
 	while (ticks--) {
-		gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
-		parity ^= MS;
-		MS >>= 1;
-		gpio_set(SWCLK_PORT, SWCLK_PIN);
 		data = MS & 1;
+		gpio_set(SWCLK_PORT, SWCLK_PIN);
+		gpio_set_val(SWDIO_PORT, SWDIO_PIN, data);
+		MS >>= 1;
 		gpio_clear(SWCLK_PORT, SWCLK_PIN);
 	}
 	gpio_set_val(SWDIO_PORT, SWDIO_PIN, parity & 1);

From 9ade8b24f7a4f9cd75fcbffe225e22528e5af2da Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Tue, 5 May 2020 18:44:38 +0200
Subject: [PATCH 39/43] hosted: Add  Readme.md

---
 src/platforms/hosted/Readme.md | 54 ++++++++++++++++++++++++++++++++++
 1 file changed, 54 insertions(+)
 create mode 100644 src/platforms/hosted/Readme.md

diff --git a/src/platforms/hosted/Readme.md b/src/platforms/hosted/Readme.md
new file mode 100644
index 0000000..bfbfaf6
--- /dev/null
+++ b/src/platforms/hosted/Readme.md
@@ -0,0 +1,54 @@
+# PC-Hosted BMP
+Compile in src with "make PROBE_HOST=hosted"
+
+## Description
+PC-hosted BMP run on the PC and compiles as "blackmagic". When started,
+it either presents a list of available probes or starts the BMP process
+if either only one probe is attached to the PC or enough information is
+given on the command line to select one of several probes.
+
+When started without any other argument beside the probe selection, a
+GDB server is started as port 2000 and up. Connect to the server as you would
+connect to the BMP with the CDCACM GDB serial server. GDB functionality
+is the same, monitor option may vary.
+
+More arguments allow to
+### print information on the connected target "blackmagiv -t"
+### directly flash a binary file at 0x0800000  "blackmagic <file.bin>"
+or with the -S argument at some other address
+### read flash to binary file "blackmagic -r <file>.bin
+### verify flash against binary file "blackmagic -V <file>.bin
+
+Use "blackmagic -h" to see all options.
+
+## Used libraries:
+### libusb
+### libftdi, for FTDI support
+### hidapi-libusb, for CMSIS-DAP support
+
+## Supported debuggers
+REMOTE_BMP is a "normal" BMP usb connected
+
+|   Debugger   | Speed | Remarks
+| ------------ | ----- | ------
+| REMOTE_BMP   |  +++  | Requires recent firmware for decent speed
+| ST-Link V3   | ++++  | Requires recent firmware, Only STM32 devices supported!
+| ST-Link V2   |  +++  | Requires recent firmware, No CDCACM uart! Cortex only!
+| ST-Link V2/1 |  +++  | Requires recent firmware, Cortex only!
+| CMSIS-DAP    |  +++  | Speed varies with MCU implementing CMSIS-DAP
+| FTDI MPSSE   |   ++  | Requires a device descrition
+| JLINK        |    -  | Usefull to add BMP support for MCUs with built-in JLINK
+
+## Feedback
+### Issues and Pull request on https://github.com/blacksphere/blackmagic/
+### Discussions on Discord.
+You can find the Discord link here: https://1bitsquared.com/pages/chat
+### Blackmagic mailing list http://sourceforge.net/mail/?group_id=407419
+
+## Known deficiencies
+### For REMOTE_BMP
+#### On windows, the device node must be given on the command line
+Finding the device from USB VID/PID/Serial in not yet implemented
+### FTDI MPSSE
+#### No auto detection
+Cable description must be given on the command line

From c5c21bb958032b5cbc2831b48ce14f35e5eb0991 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sat, 9 May 2020 18:14:18 +0200
Subject: [PATCH 40/43] README.md: Mention the PC-Hosted GDB server.

---
 README.md | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/README.md b/README.md
index 14db294..c812677 100644
--- a/README.md
+++ b/README.md
@@ -23,3 +23,26 @@ makes [SWO viewing as simple as connecting to a serial port](https://github.com/
 See online documentation at https://github.com/blacksphere/blackmagic/wiki
 
 Binaries from the latest automated build are at http://builds.blacksphere.co.nz/blackmagic
+
+BLACKMAGIC
+==========
+
+You can also build blackmagic as a PC hosted application
+"make PROBE_HOST=hosted"
+
+This builds the same GDB server, that is running on the Black Magic Probe.
+While connection to the Black Magic Probe GDB server is via serial line,
+connection to the PC-Hosted GDB server is via TCP port 2000 for the first
+GDB server and higher for more invokations. Use "tar(get) ext(ented) :2000"
+to connect.
+PC-hosted BMP GDB server can talk to the Black Magic Probe itself,
+ST-LinkV2 and V3, CMSIS-DAP, JLINK and FTDI MPSSE based debuggers.
+
+When connected to a single BMP supported probe, starting "blackmagic" w/o any
+arguments starts the server. When several BMP supported probes are connected,
+their types, position and serial number is displayed and the program exits.
+Add "-P (position)" to the next invokation to select one.
+
+PC hosted BMP also allows to flash, read and verify a binary file, by default
+starting at 0x08000000. The "-t" argument displays information about the
+connected target. Use "-h " to get a list of supported options.

From dc3fd2eb066fa942f7f2197c7fc481f1b7dce9ae Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Sun, 17 May 2020 11:39:31 +0200
Subject: [PATCH 41/43] Classify debug messages

Firmware BMP with ENABLE_DEBUG=1 will print WARN and INFO as before.
PC-Hosted BMPwill alway print to stderr. Warn is printed unconditional,
INFO, GDB, TARGET, DONGLE and WIRE will print if their appropriate bit in
cl_debuglevel is set via the -v verbose command line argument.
INFO will go to stdout with -t or -l.
---
 src/exception.c                         |   2 +-
 src/gdb_main.c                          |  21 ++--
 src/gdb_packet.c                        |  24 ++--
 src/include/general.h                   | 112 ++++++++++++++++++-
 src/morse.c                             |   2 +-
 src/platforms/f4discovery/platform.h    |   2 -
 src/platforms/hosted/bmp_remote.c       |  53 ++++-----
 src/platforms/hosted/cmsis_dap.c        |  73 +++++-------
 src/platforms/hosted/dap.c              |  37 +++----
 src/platforms/hosted/ftdi_bmp.c         |  24 ++--
 src/platforms/hosted/jlink.c            |  27 +++--
 src/platforms/hosted/jlink_adiv5_swdp.c |  15 +--
 src/platforms/hosted/jlink_jtagtap.c    |  31 +++---
 src/platforms/hosted/libftdi_jtagtap.c  |  12 +-
 src/platforms/hosted/libftdi_swdptap.c  |   8 +-
 src/platforms/hosted/platform.c         |  98 ++++++++---------
 src/platforms/hosted/remote_jtagtap.c   |  11 +-
 src/platforms/hosted/remote_swdptap.c   |  25 +++--
 src/platforms/hosted/stlinkv2.c         | 140 ++++++++++++------------
 src/platforms/hosted/stlinkv2.h         |   3 -
 src/platforms/hydrabus/platform.h       |   2 -
 src/platforms/launchpad-icdi/platform.h |   2 -
 src/platforms/native/platform.h         |  17 +--
 src/platforms/pc/cl_utils.c             | 139 +++++++++++------------
 src/platforms/pc/cl_utils.h             |  10 +-
 src/platforms/pc/gdb_if.c               |  14 ++-
 src/platforms/pc/libusb_utils.c         |  57 +++++-----
 src/platforms/pc/serial_unix.c          |  41 ++++---
 src/platforms/pc/serial_win.c           |  29 +++--
 src/platforms/stlink/platform.h         |  10 +-
 src/platforms/swlink/platform.h         |  10 +-
 src/target/adiv5.c                      |  59 +++++-----
 src/target/adiv5_jtagdp.c               |   2 +-
 src/target/adiv5_swdp.c                 |   4 +-
 src/target/cortexa.c                    |  12 +-
 src/target/cortexm.c                    |  14 +--
 src/target/efm32.c                      |  18 +--
 src/target/jtag_scan.c                  |  31 +++---
 src/target/kinetis.c                    |   2 +-
 src/target/lmi.c                        |   2 +-
 src/target/lpc11xx.c                    |   4 +-
 src/target/lpc17xx.c                    |  11 +-
 src/target/lpc_common.c                 |   2 +-
 src/target/msp432.c                     |  20 ++--
 src/target/nrf51.c                      |   2 +-
 src/target/nxpke04.c                    |   2 +-
 src/target/sam3x.c                      |   6 +-
 src/target/sam4l.c                      |  31 +++---
 src/target/samd.c                       |   2 +-
 src/target/samx5x.c                     |  51 +++++----
 src/target/stm32f1.c                    |  14 +--
 src/target/stm32f4.c                    |  10 +-
 src/target/stm32h7.c                    |  42 +++----
 src/target/stm32l0.c                    |   4 +-
 src/target/stm32l4.c                    |  10 +-
 src/target/target.c                     |  14 +--
 56 files changed, 745 insertions(+), 675 deletions(-)

diff --git a/src/exception.c b/src/exception.c
index 0242cec..04c6b21 100644
--- a/src/exception.c
+++ b/src/exception.c
@@ -26,7 +26,7 @@ struct exception *innermost_exception;
 void raise_exception(uint32_t type, const char *msg)
 {
 	struct exception *e;
-	DEBUG("Exception: %s\n", msg);
+	DEBUG_WARN("Exception: %s\n", msg);
 	for (e = innermost_exception; e; e = e->outer) {
 		if (e->mask & type) {
 			e->type = type;
diff --git a/src/gdb_main.c b/src/gdb_main.c
index c0575a6..9157f2b 100644
--- a/src/gdb_main.c
+++ b/src/gdb_main.c
@@ -120,7 +120,8 @@ int gdb_main_loop(struct target_controller *tc, bool in_syscall)
 				gdb_putpacketz("E02");
 				break;
 			}
-			DEBUG("m packet: addr = %" PRIx32 ", len = %" PRIx32 "\n", addr, len);
+			DEBUG_GDB("m packet: addr = %" PRIx32 ", len = %" PRIx32 "\n",
+					  addr, len);
 			uint8_t mem[len];
 			if (target_mem_read(cur_target, mem, addr, len))
 				gdb_putpacketz("E01");
@@ -145,7 +146,8 @@ int gdb_main_loop(struct target_controller *tc, bool in_syscall)
 				gdb_putpacketz("E02");
 				break;
 			}
-			DEBUG("M packet: addr = %" PRIx32 ", len = %" PRIx32 "\n", addr, len);
+			DEBUG_GDB("M packet: addr = %" PRIx32 ", len = %" PRIx32 "\n",
+					  addr, len);
 			uint8_t mem[len];
 			unhexify(mem, pbuf + hex, len);
 			if (target_mem_write(cur_target, addr, mem, len))
@@ -242,7 +244,7 @@ int gdb_main_loop(struct target_controller *tc, bool in_syscall)
 			if (in_syscall) {
 				return hostio_reply(tc, pbuf, size);
 			} else {
-				DEBUG("*** F packet when not in syscall! '%s'\n", pbuf);
+				DEBUG_GDB("*** F packet when not in syscall! '%s'\n", pbuf);
 				gdb_putpacketz("");
 			}
 			break;
@@ -295,7 +297,8 @@ int gdb_main_loop(struct target_controller *tc, bool in_syscall)
 				gdb_putpacketz("E02");
 				break;
 			}
-			DEBUG("X packet: addr = %" PRIx32 ", len = %" PRIx32 "\n", addr, len);
+			DEBUG_GDB("X packet: addr = %" PRIx32 ", len = %" PRIx32 "\n",
+					  addr, len);
 			if (target_mem_write(cur_target, addr, pbuf+bin, len))
 				gdb_putpacketz("E01");
 			else
@@ -319,7 +322,7 @@ int gdb_main_loop(struct target_controller *tc, bool in_syscall)
 			break;
 
 		default: 	/* Packet not implemented */
-			DEBUG("*** Unsupported packet: %s\n", pbuf);
+			DEBUG_GDB("*** Unsupported packet: %s\n", pbuf);
 			gdb_putpacketz("");
 		}
 	}
@@ -410,7 +413,7 @@ handle_q_packet(char *packet, int len)
 		gdb_putpacket_f("C%lx", generic_crc32(cur_target, addr, alen));
 
 	} else {
-		DEBUG("*** Unsupported packet: %s\n", packet);
+		DEBUG_GDB("*** Unsupported packet: %s\n", packet);
 		gdb_putpacket("", 0);
 	}
 }
@@ -478,7 +481,7 @@ handle_v_packet(char *packet, int plen)
 
 	} else if (sscanf(packet, "vFlashErase:%08lx,%08lx", &addr, &len) == 2) {
 		/* Erase Flash Memory */
-		DEBUG("Flash Erase %08lX %08lX\n", addr, len);
+		DEBUG_GDB("Flash Erase %08lX %08lX\n", addr, len);
 		if(!cur_target) { gdb_putpacketz("EFF"); return; }
 
 		if(!flash_mode) {
@@ -495,7 +498,7 @@ handle_v_packet(char *packet, int plen)
 	} else if (sscanf(packet, "vFlashWrite:%08lx:%n", &addr, &bin) == 1) {
 		/* Write Flash Memory */
 		len = plen - bin;
-		DEBUG("Flash Write %08lX %08lX\n", addr, len);
+		DEBUG_GDB("Flash Write %08lX %08lX\n", addr, len);
 		if(cur_target && target_flash_write(cur_target, addr, (void*)packet + bin, len) == 0)
 			gdb_putpacketz("OK");
 		else
@@ -507,7 +510,7 @@ handle_v_packet(char *packet, int plen)
 		flash_mode = 0;
 
 	} else {
-		DEBUG("*** Unsupported packet: %s\n", packet);
+		DEBUG_GDB("*** Unsupported packet: %s\n", packet);
 		gdb_putpacket("", 0);
 	}
 }
diff --git a/src/gdb_packet.c b/src/gdb_packet.c
index 6a01c89..f6e7946 100644
--- a/src/gdb_packet.c
+++ b/src/gdb_packet.c
@@ -118,16 +118,16 @@ int gdb_getpacket(char *packet, int size)
 	gdb_if_putchar('+', 1); /* send ack */
 	packet[i] = 0;
 
-#ifdef DEBUG_GDBPACKET
-	DEBUG("%s : ", __func__);
+#if PC_HOSTED == 1
+	DEBUG_GDB_WIRE("%s : ", __func__);
 	for(int j = 0; j < i; j++) {
 		c = packet[j];
 		if ((c >= 32) && (c < 127))
-			DEBUG("%c", c);
+			DEBUG_GDB_WIRE("%c", c);
 		else
-			DEBUG("\\x%02X", c);
+			DEBUG_GDB_WIRE("\\x%02X", c);
 	}
-	DEBUG("\n");
+	DEBUG_GDB_WIRE("\n");
 #endif
 	return i;
 }
@@ -141,18 +141,16 @@ void gdb_putpacket(const char *packet, int size)
 	int tries = 0;
 
 	do {
-#ifdef DEBUG_GDBPACKET
-		DEBUG("%s : ", __func__);
-#endif
+		DEBUG_GDB_WIRE("%s : ", __func__);
 		csum = 0;
 		gdb_if_putchar('$', 0);
 		for(i = 0; i < size; i++) {
 			c = packet[i];
-#ifdef DEBUG_GDBPACKET
+#if PC_HOSTED == 1
 			if ((c >= 32) && (c < 127))
-				DEBUG("%c", c);
+				DEBUG_GDB_WIRE("%c", c);
 			else
-				DEBUG("\\x%02X", c);
+				DEBUG_GDB_WIRE("\\x%02X", c);
 #endif
 			if((c == '$') || (c == '#') || (c == '}')) {
 				gdb_if_putchar('}', 0);
@@ -167,9 +165,7 @@ void gdb_putpacket(const char *packet, int size)
 		snprintf(xmit_csum, sizeof(xmit_csum), "%02X", csum);
 		gdb_if_putchar(xmit_csum[0], 0);
 		gdb_if_putchar(xmit_csum[1], 1);
-#ifdef DEBUG_GDBPACKET
-		DEBUG("\n");
-#endif
+		DEBUG_GDB_WIRE("\n");
 	} while((gdb_if_getchar_to(2000) != '+') && (tries++ < 3));
 }
 
diff --git a/src/include/general.h b/src/include/general.h
index f8cb927..429bf58 100644
--- a/src/include/general.h
+++ b/src/include/general.h
@@ -37,8 +37,116 @@
 #include "platform.h"
 #include "platform_support.h"
 
-#ifndef DEBUG
-#define DEBUG	printf
+enum BMP_DEBUG {
+	BMP_DEBUG_NONE   =  0,
+	BMP_DEBUG_INFO   =  1,
+	BMP_DEBUG_GDB    =  2,
+	BMP_DEBUG_TARGET =  4,
+	BMP_DEBUG_PROBE =  8,
+	BMP_DEBUG_WIRE   = 0x10,
+	BMP_DEBUG_MAX    = 0x20,
+	BMP_DEBUG_STDOUT = 0x8000,
+};
+
+#if PC_HOSTED == 0
+/* For BMP debug output on a firmware BMP platform, using
+ * BMP PC-Hosted is the preferred way. Printing DEBUG_WARN
+ * and DEBUG_INFO is kept for comptibiluty.
+ */
+# if defined(ENABLE_DEBUG)
+#  define DEBUG_WARN printf
+#  define DEBUG_INFO printf
+# else
+#  define DEBUG_WARN(...)
+#  define DEBUG_INFO(...)
+# endif
+# define DEBUG_GDB(...)
+# define DEBUG_TARGET(...)
+# define DEBUG_PROBE(...)
+# define DEBUG_WIRE(...)
+# define DEBUG_GDB_WIRE(...)
+#else
+# include <stdarg.h>
+extern int cl_debuglevel;
+
+static inline void DEBUG_WARN(const char *format, ...)
+{
+	va_list ap;
+    va_start(ap, format);
+    vfprintf(stderr, format, ap);
+    va_end(ap);
+	return;
+}
+
+static inline void DEBUG_INFO(const char *format, ...)
+{
+	if (~cl_debuglevel & BMP_DEBUG_INFO)
+		return;
+	va_list ap;
+    va_start(ap, format);
+	if (cl_debuglevel & BMP_DEBUG_STDOUT)
+		vfprintf(stdout, format, ap);
+	else
+		vfprintf(stderr, format, ap);
+    va_end(ap);
+	return;
+}
+
+static inline void DEBUG_GDB(const char *format, ...)
+{
+	if (~cl_debuglevel & BMP_DEBUG_GDB)
+		return;
+	va_list ap;
+    va_start(ap, format);
+    vfprintf(stderr, format, ap);
+    va_end(ap);
+	return;
+}
+
+static inline void DEBUG_GDB_WIRE(const char *format, ...)
+{
+	if ((cl_debuglevel & (BMP_DEBUG_GDB | BMP_DEBUG_WIRE)) !=
+		(BMP_DEBUG_GDB | BMP_DEBUG_WIRE))
+		return;
+	va_list ap;
+    va_start(ap, format);
+    vfprintf(stderr, format, ap);
+    va_end(ap);
+	return;
+}
+
+static inline void DEBUG_TARGET(const char *format, ...)
+{
+	if (~cl_debuglevel & BMP_DEBUG_TARGET)
+		return;
+	va_list ap;
+    va_start(ap, format);
+    vfprintf(stderr, format, ap);
+    va_end(ap);
+	return;
+}
+
+static inline void DEBUG_PROBE(const char *format, ...)
+{
+	if (~cl_debuglevel & BMP_DEBUG_PROBE)
+		return;
+	va_list ap;
+    va_start(ap, format);
+    vfprintf(stderr, format, ap);
+    va_end(ap);
+	return;
+}
+
+static inline void DEBUG_WIRE(const char *format, ...)
+{
+	if (~cl_debuglevel & BMP_DEBUG_WIRE)
+		return;
+	va_list ap;
+    va_start(ap, format);
+    vfprintf(stderr, format, ap);
+    va_end(ap);
+	return;
+}
 #endif
 
 #define ALIGN(x, n) (((x) + (n) - 1) & ~((n) - 1))
diff --git a/src/morse.c b/src/morse.c
index 9ba4814..0fb947a 100644
--- a/src/morse.c
+++ b/src/morse.c
@@ -61,7 +61,7 @@ void morse(const char *msg, char repeat)
 {
 #if PC_HOSTED == 1
 	if (msg)
-		DEBUG("%s\n", msg);
+		DEBUG_WARN("%s\n", msg);
 	(void) repeat;
 #else
 morse_msg = morse_ptr = msg;
diff --git a/src/platforms/f4discovery/platform.h b/src/platforms/f4discovery/platform.h
index cb41f9a..dde94ed 100644
--- a/src/platforms/f4discovery/platform.h
+++ b/src/platforms/f4discovery/platform.h
@@ -135,8 +135,6 @@
 #define TRACE_IRQ   NVIC_TIM3_IRQ
 #define TRACE_ISR   tim3_isr
 
-#define DEBUG(...)
-
 #define gpio_set_val(port, pin, val) do {	\
 	if(val)					\
 		gpio_set((port), (pin));	\
diff --git a/src/platforms/hosted/bmp_remote.c b/src/platforms/hosted/bmp_remote.c
index a6461a5..22d7a49 100644
--- a/src/platforms/hosted/bmp_remote.c
+++ b/src/platforms/hosted/bmp_remote.c
@@ -4,7 +4,7 @@
  * Copyright (C) 2011  Black Sphere Technologies Ltd.
  * Written by Gareth McMullin <gareth@blacksphere.co.nz>
  * Additions by Dave Marples <dave@marples.net>
- * Additions by Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
+ * Modifications (C) 2020 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
@@ -45,12 +45,12 @@ int remote_init(bool verbose)
 	c = platform_buffer_read((uint8_t *)construct, REMOTE_MAX_MSG_SIZE);
 
 	if ((!c) || (construct[0] == REMOTE_RESP_ERR)) {
-		fprintf(stderr,"Remote Start failed, error %s\n",
+		DEBUG_WARN("Remote Start failed, error %s\n",
 				c ? (char *)&(construct[1]) : "unknown");
       return -1;
     }
 	if (verbose)
-		printf("Remote is %s\n", &construct[1]);
+		DEBUG_WARN("Remote is %s\n", &construct[1]);
 	char *p = strstr(&construct[1], "(Firmware v");
 	if (!p)
 		return -1;
@@ -77,8 +77,8 @@ bool remote_target_get_power(void)
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-      fprintf(stderr," platform_target_get_power failed, error %s\n",
-			  s ? (char *)&(construct[1]) : "unknown");
+      DEBUG_WARN(" platform_target_get_power failed, error %s\n",
+				 s ? (char *)&(construct[1]) : "unknown");
       exit (-1);
     }
 
@@ -97,7 +97,7 @@ void remote_target_set_power(bool power)
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-		fprintf(stderr, "platform_target_set_power failed, error %s\n",
+		DEBUG_WARN("platform_target_set_power failed, error %s\n",
 				s ? (char *)&(construct[1]) : "unknown");
       exit(-1);
     }
@@ -115,8 +115,8 @@ void remote_srst_set_val(bool assert)
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-		fprintf(stderr, "platform_srst_set_val failed, error %s\n",
-				s ? (char *)&(construct[1]) : "unknown");
+		DEBUG_WARN("platform_srst_set_val failed, error %s\n",
+				   s ? (char *)&(construct[1]) : "unknown");
       exit(-1);
     }
 }
@@ -133,8 +133,8 @@ bool remote_srst_get_val(void)
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-      fprintf(stderr, "platform_srst_set_val failed, error %s\n",
-			  s ? (char *)&(construct[1]) : "unknown");
+		DEBUG_WARN("platform_srst_set_val failed, error %s\n",
+				   s ? (char *)&(construct[1]) : "unknown");
       exit(-1);
     }
 	return (construct[1] == '1');
@@ -152,7 +152,7 @@ const char *remote_target_voltage(void)
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-      fprintf(stderr, "platform_target_voltage failed, error %s\n",
+      DEBUG_WARN("platform_target_voltage failed, error %s\n",
 			  s ? (char *)&(construct[1]) : "unknown");
       exit(- 1);
     }
@@ -168,10 +168,11 @@ static uint32_t remote_adiv5_dp_read(ADIv5_DP_t *dp, uint16_t addr)
 	platform_buffer_write(construct, s);
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-		printf("%s error %d\n", __func__, s);
+		DEBUG_WARN("%s error %d\n", __func__, s);
 	}
     uint32_t dest[1];
 	unhexify(dest, (const char*)&construct[1], 4);
+	DEBUG_PROBE("dp_read addr %04x: %08" PRIx32 "\n", dest[0]);
 	return dest[0];
 }
 
@@ -180,12 +181,12 @@ static uint32_t remote_adiv5_low_access(
 {
 	(void)dp;
 	uint8_t construct[REMOTE_MAX_MSG_SIZE];
-	int s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE, REMOTE_LOW_ACCESS_STR,
-					 RnW, addr, value);
+	int s = snprintf((char *)construct, REMOTE_MAX_MSG_SIZE,
+					 REMOTE_LOW_ACCESS_STR, RnW, addr, value);
 	platform_buffer_write(construct, s);
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-		printf("%s error %d\n", __func__, s);
+		DEBUG_WARN("%s error %d\n", __func__, s);
 	}
     uint32_t dest[1];
 	unhexify(dest, (const char*)&construct[1], 4);
@@ -200,7 +201,7 @@ static uint32_t remote_adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 	platform_buffer_write(construct, s);
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-		printf("%s error %d\n", __func__, s);
+		DEBUG_WARN("%s error %d\n", __func__, s);
 	}
     uint32_t dest[1];
 	unhexify(dest, (const char*)&construct[1], 4);
@@ -215,7 +216,7 @@ static void remote_adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 	platform_buffer_write(construct, s);
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-		printf("%s error %d\n", __func__, s);
+		DEBUG_WARN("%s error %d\n", __func__, s);
 	}
 	return;
 }
@@ -227,8 +228,7 @@ static void remote_mem_read(
 	(void)ap;
 	if (len == 0)
 		return;
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("memread @ %" PRIx32 " len %ld, start: \n",
+	DEBUG_WIRE("memread @ %" PRIx32 " len %ld, start: \n",
 			   src, len);
 	uint8_t construct[REMOTE_MAX_MSG_SIZE];
 	int s;
@@ -252,11 +252,11 @@ static void remote_mem_read(
 		} else {
 			if(construct[0] == REMOTE_RESP_ERR) {
 				ap->dp->fault = 1;
-				printf("%s returned REMOTE_RESP_ERR at addr: 0x%08x\n",
+				DEBUG_WARN("%s returned REMOTE_RESP_ERR at addr: 0x%08x\n",
 					   __func__, src);
 				break;
 			} else {
-				printf("%s error %d\n", __func__, s);
+				DEBUG_WARN("%s error %d\n", __func__, s);
 				break;
 			}
 		}
@@ -290,11 +290,11 @@ static void remote_ap_mem_read(
 		} else {
 			if(construct[0] == REMOTE_RESP_ERR) {
 				ap->dp->fault = 1;
-				printf("%s returned REMOTE_RESP_ERR at apsel %d, "
+				DEBUG_WARN("%s returned REMOTE_RESP_ERR at apsel %d, "
 					   "addr: 0x%08" PRIx32 "\n", __func__, ap->apsel, src);
 				break;
 			} else {
-				printf("%s error %d around 0x%08" PRIx32 "\n",
+				DEBUG_WARN("%s error %d around 0x%08" PRIx32 "\n",
 					   __func__, s, src);
 				break;
 			}
@@ -334,10 +334,10 @@ static void remote_ap_mem_write_sized(
 			continue;
 		if ((s > 0) && (construct[0] == REMOTE_RESP_ERR)) {
 			ap->dp->fault = 1;
-			printf("%s returned REMOTE_RESP_ERR at apsel %d, "
+			DEBUG_WARN("%s returned REMOTE_RESP_ERR at apsel %d, "
 				   "addr: 0x%08x\n", __func__, ap->apsel, dest);
 		} else {
-			printf("%s error %d around address 0x%08" PRIx32 "\n",
+			DEBUG_WARN("%s error %d around address 0x%08" PRIx32 "\n",
 				   __func__, s, dest);
 			break;
 		}
@@ -347,7 +347,8 @@ static void remote_ap_mem_write_sized(
 void remote_adiv5_dp_defaults(ADIv5_DP_t *dp)
 {
 	if (remote_init(false)) {
-		printf("Please update BMP firmware for substantial speed increase!\n");
+		DEBUG_WARN(
+			"Please update BMP firmware for substantial speed increase!\n");
 		return;
 	}
 	dp->low_access = remote_adiv5_low_access;
diff --git a/src/platforms/hosted/cmsis_dap.c b/src/platforms/hosted/cmsis_dap.c
index 395f308..d9fe402 100644
--- a/src/platforms/hosted/cmsis_dap.c
+++ b/src/platforms/hosted/cmsis_dap.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2019-20 Uwe Bonnes
+ * Copyright (C) 2019-20 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
@@ -62,7 +62,7 @@ int dap_init(bmp_info_t *info)
 	 * FIXME: Find a solution to decipher from the device.
 	 */
 	if ((info->vid == 0x1fc9) && (info->pid == 0x0132)) {
-		printf("Blacklist\n");
+		DEBUG_WARN("Blacklist\n");
 		report_size = 64 + 1;
 	}
 	handle = hid_open(info->vid, info->pid, serial);
@@ -71,17 +71,17 @@ int dap_init(bmp_info_t *info)
 	dap_disconnect();
 	size = dap_info(DAP_INFO_CAPABILITIES, hid_buffer, sizeof(hid_buffer));
 	dap_caps = hid_buffer[0];
-	printf(" Cap (0x%2x): %s%s%s", hid_buffer[0],
+	DEBUG_INFO(" Cap (0x%2x): %s%s%s", hid_buffer[0],
 		   (hid_buffer[0] & 1)? "SWD" : "",
 		   ((hid_buffer[0] & 3) == 3) ? "/" : "",
 		   (hid_buffer[0] & 2)? "JTAG" : "");
 	if (hid_buffer[0] & 4)
-		printf(", SWO_UART");
+		DEBUG_INFO(", SWO_UART");
 	if (hid_buffer[0] & 8)
-		printf(", SWO_MANCHESTER");
+		DEBUG_INFO(", SWO_MANCHESTER");
 	if (hid_buffer[0] & 0x10)
-		printf(", Atomic Cmds");
-	printf("\n");
+		DEBUG_INFO(", Atomic Cmds");
+	DEBUG_INFO("\n");
 	return 0;
 }
 
@@ -155,35 +155,31 @@ int dbg_dap_cmd(uint8_t *data, int size, int rsize)
 	hid_buffer[0] = 0x00; // Report ID??
 	memcpy(&hid_buffer[1], data, rsize);
 
-	if (cl_debuglevel & BMP_DEBUG_WIRE) {
-		printf("cmd :   ");
-		for(int i = 0; (i < 16) && (i < rsize + 1); i++)
-			printf("%02x.",	hid_buffer[i]);
-		printf("\n");
-	}
+	DEBUG_WIRE("cmd :   ");
+	for(int i = 0; (i < 16) && (i < rsize + 1); i++)
+		DEBUG_WIRE("%02x.",	hid_buffer[i]);
+	DEBUG_WIRE("\n");
 	res = hid_write(handle, hid_buffer, rsize + 1);
 	if (res < 0) {
-		fprintf(stderr, "Error: %ls\n", hid_error(handle));
+		DEBUG_WARN( "Error: %ls\n", hid_error(handle));
 		exit(-1);
 	}
 	if (size) {
 		res = hid_read(handle, hid_buffer, report_size + 1);
 		if (res < 0) {
-			fprintf(stderr, "debugger read(): %ls\n", hid_error(handle));
+			DEBUG_WARN( "debugger read(): %ls\n", hid_error(handle));
 			exit(-1);
 		}
 		if (size && hid_buffer[0] != cmd) {
-			printf("cmd %02x invalid response received %02x\n",
+			DEBUG_WARN("cmd %02x invalid response received %02x\n",
 				   cmd, hid_buffer[0]);
 		}
 		res--;
 		memcpy(data, &hid_buffer[1], (size < res) ? size : res);
-		if (cl_debuglevel & BMP_DEBUG_WIRE) {
-			printf("cmd res:");
-			for(int i = 0; (i < 16) && (i < size + 4); i++)
-				printf("%02x.",	hid_buffer[i]);
-			printf("\n");
-		}
+		DEBUG_WIRE("cmd res:");
+		for(int i = 0; (i < 16) && (i < size + 4); i++)
+			DEBUG_WIRE("%02x.",	hid_buffer[i]);
+		DEBUG_WIRE("\n");
 	}
 
 	return res;
@@ -196,10 +192,8 @@ static void dap_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 	if (len == 0)
 		return;
 	enum align align = MIN(ALIGNOF(src), ALIGNOF(len));
-#if 0
-	printf("memread @ %" PRIx32 " len %ld, align %d , start: \n",
+	DEBUG_WIRE("memread @ %" PRIx32 " len %ld, align %d , start: \n",
 		   src, len, align);
-#endif
 	if (((unsigned)(1 << align)) == len)
 		return dap_read_single(ap, dest, src, align);
 	/* One word transfer for every byte/halfword/word
@@ -218,7 +212,7 @@ static void dap_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 			unsigned int res = dap_read_block(ap, dest, src, transfersize,
 											  align);
 			if (res) {
-//			    printf("mem_read failed %02x\n", res);
+			    DEBUG_WIRE("mem_read failed %02x\n", res);
 				ap->dp->fault = 1;
 				return;
 			}
@@ -228,7 +222,7 @@ static void dap_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 			src       += transfersize;
 		}
 	}
-//	printf("memread res last data %08" PRIx32 "\n", ((uint32_t*)dest)[-1]);
+    DEBUG_WIRE("memread res last data %08" PRIx32 "\n", ((uint32_t*)dest)[-1]);
 }
 
 static void dap_mem_write_sized(
@@ -237,10 +231,8 @@ static void dap_mem_write_sized(
 {
 	if (len == 0)
 		return;
-#if 0
-	printf("memwrite @ %" PRIx32 " len %ld, align %d , %08x start: \n",
-		   dest, len, align, *(uint32_t *)src);
-#endif
+	DEBUG_WIRE("memwrite @ %" PRIx32 " len %ld, align %d , %08x start: \n",
+		dest, len, align, *(uint32_t *)src);
 	if (((unsigned)(1 << align)) == len)
 		return dap_write_single(ap, dest, src, align);
 	unsigned int max_size = (dbg_get_report_size() - 5) >> (2 - align);
@@ -256,7 +248,7 @@ static void dap_mem_write_sized(
 			unsigned int res = dap_write_block(ap, dest, src, transfersize,
 											   align);
 			if (res) {
-				printf("mem_write failed %02x\n", res);
+				DEBUG_WARN("mem_write failed %02x\n", res);
 				ap->dp->fault = 1;
 				return;
 			}
@@ -266,7 +258,7 @@ static void dap_mem_write_sized(
 			src       += transfersize;
 		}
 	}
-//	printf("memwrite done\n");
+	DEBUG_WIRE("memwrite done\n");
 }
 
 int dap_enter_debug_swd(ADIv5_DP_t *dp)
@@ -311,39 +303,34 @@ static void cmsis_dap_jtagtap_tms_seq(uint32_t MS, int ticks)
 	uint8_t TMS[4] = {MS & 0xff, (MS >> 8) & 0xff, (MS >> 16) & 0xff,
 					  (MS >> 24) & 0xff};
 	dap_jtagtap_tdi_tdo_seq(NULL, false, TMS, NULL, ticks);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("tms_seq DI %08x %d\n", MS, ticks);
+	DEBUG_PROBE("tms_seq DI %08x %d\n", MS, ticks);
 }
 
 static void cmsis_dap_jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms,
 										  const uint8_t *DI, int ticks)
 {
 	dap_jtagtap_tdi_tdo_seq(DO, (final_tms), NULL, DI, ticks);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("jtagtap_tdi_tdo_seq %d, %02x-> %02x\n", ticks, DI[0], DO[0]);
+	DEBUG_PROBE("jtagtap_tdi_tdo_seq %d, %02x-> %02x\n", ticks, DI[0], DO[0]);
 }
 
 static void  cmsis_dap_jtagtap_tdi_seq(const uint8_t final_tms,
 									   const uint8_t *DI, int ticks)
 {
 	dap_jtagtap_tdi_tdo_seq(NULL, (final_tms), NULL, DI, ticks);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("jtagtap_tdi_seq %d, %02x\n", ticks, DI[0]);
+	DEBUG_PROBE("jtagtap_tdi_seq %d, %02x\n", ticks, DI[0]);
 }
 
 static uint8_t cmsis_dap_jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 {
 	uint8_t tdo[1];
 	dap_jtagtap_tdi_tdo_seq(tdo, false, &dTMS, &dTDI, 1);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("next tms %02x tdi %02x tdo %02x\n", dTMS, dTDI, tdo[0]);
+	DEBUG_PROBE("next tms %02x tdi %02x tdo %02x\n", dTMS, dTDI, tdo[0]);
 	return (tdo[0] & 1);
 }
 
 int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc)
 {
-	if (cl_debuglevel)
-		printf("jtap_init\n");
+	DEBUG_PROBE("jtap_init\n");
 	if (!(dap_caps & DAP_CAP_JTAG))
 		return -1;
 	mode =  DAP_CAP_JTAG;
diff --git a/src/platforms/hosted/dap.c b/src/platforms/hosted/dap.c
index 5f32dc8..5db31c9 100644
--- a/src/platforms/hosted/dap.c
+++ b/src/platforms/hosted/dap.c
@@ -31,11 +31,8 @@
  */
 
 /*- Includes ----------------------------------------------------------------*/
-#include <stdio.h>
+#include <general.h>
 #include <stdlib.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <string.h>
 #include "dap.h"
 #include "jtag_scan.h"
 
@@ -312,7 +309,7 @@ static void dap_line_reset(void)
 	};
 	dbg_dap_cmd(buf, sizeof(buf), 10);
 	if (buf[0])
-		printf("line reset failed\n");
+		DEBUG_WARN("line reset failed\n");
 }
 
 static uint32_t wait_word(uint8_t *buf, int size, int len, uint8_t *dp_fault)
@@ -324,11 +321,11 @@ static uint32_t wait_word(uint8_t *buf, int size, int len, uint8_t *dp_fault)
 	} while (buf[1] == DAP_TRANSFER_WAIT);
 
 	if (buf[1] > DAP_TRANSFER_WAIT) {
-//	  printf("dap_read_reg fault\n");
+//	  DEBUG_WARN("dap_read_reg fault\n");
 		*dp_fault = 1;
 	}
 	if (buf[1] == DAP_TRANSFER_ERROR) {
-		printf("dap_read_reg, protocoll error\n");
+		DEBUG_WARN("dap_read_reg, protocoll error\n");
 		dap_line_reset();
 	}
 	uint32_t res =
@@ -349,7 +346,7 @@ uint32_t dap_read_reg(ADIv5_DP_t *dp, uint8_t reg)
 	buf[2] = 0x01; // Request size
 	buf[3] = reg | DAP_TRANSFER_RnW;
 	uint32_t res = wait_word(buf, 8, 4, &dp->fault);
-//  printf("\tdap_read_reg %02x %08x\n", reg, res);
+	DEBUG_WIRE("\tdap_read_reg %02x %08x\n", reg, res);
 	return res;
 }
 
@@ -357,7 +354,7 @@ uint32_t dap_read_reg(ADIv5_DP_t *dp, uint8_t reg)
 void dap_write_reg(ADIv5_DP_t *dp, uint8_t reg, uint32_t data)
 {
 	uint8_t buf[8];
-	// printf("\tdap_write_reg %02x %08x\n", reg, data);
+	DEBUG_PROBE("\tdap_write_reg %02x %08x\n", reg, data);
 
 	buf[0] = ID_DAP_TRANSFER;
 	uint8_t dap_index = 0;
@@ -377,11 +374,12 @@ void dap_write_reg(ADIv5_DP_t *dp, uint8_t reg, uint32_t data)
 	} while (buf[1] == DAP_TRANSFER_WAIT);
 
 	if (buf[1] > DAP_TRANSFER_WAIT) {
-//	  printf("dap_write_reg %02x data %08x:fault\n", reg, data);
+		DEBUG_PROBE("dap_write_reg %02x data %08x:fault\n", reg, data);
 		dp->fault = 1;
 	}
 	if (buf[1] == DAP_TRANSFER_ERROR) {
-//	  printf("dap_write_reg %02x data %08x: protocoll error\n", reg, data);
+		DEBUG_PROBE("dap_write_reg %02x data %08x: protocoll error\n",
+					reg, data);
 		dap_line_reset();
 	}
 }
@@ -402,7 +400,7 @@ unsigned int dap_read_block(ADIv5_AP_t *ap, void *dest, uint32_t src,
     dbg_dap_cmd(buf, 1023, 5 + 1);
 	unsigned int transferred = buf[0] + (buf[1] << 8);
 	if (buf[2] > DAP_TRANSFER_FAULT) {
-		printf("line_reset\n");
+		DEBUG_PROBE("line_reset\n");
 		dap_line_reset();
 	}
 	if (sz != transferred) {
@@ -561,7 +559,7 @@ void dap_ap_mem_access_setup(ADIv5_AP_t *ap, uint32_t addr, enum align align)
 
 uint32_t dap_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 {
-//	printf("dap_ap_read_start\n");
+	DEBUG_PROBE("dap_ap_read_start\n");
 	uint8_t buf[63], *p = buf;
 	buf[0] = ID_DAP_TRANSFER;
 	uint8_t dap_index = 0;
@@ -583,7 +581,7 @@ uint32_t dap_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 
 void dap_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 {
-//	printf("dap_ap_write addr %04x value %08x\n", addr, value);
+	DEBUG_PROBE("dap_ap_write addr %04x value %08x\n", addr, value);
 	uint8_t buf[63], *p = buf;
 	uint8_t dap_index = 0;
 	if (ap->dp->dev)
@@ -602,7 +600,6 @@ void dap_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 	*p++ = (value >> 16) & 0xff;
 	*p++ = (value >> 24) & 0xff;
 	dbg_dap_cmd(buf, sizeof(buf), p - buf);
-//	printf("dap_ap_write done\n");
 }
 
 void dap_read_single(ADIv5_AP_t *ap, void *dest, uint32_t src, enum align align)
@@ -673,7 +670,7 @@ void dap_jtagtap_tdi_tdo_seq(uint8_t *DO, bool final_tms, const uint8_t *TMS,
 			p += n_di_bytes;
 			dbg_dap_cmd(buf, sizeof(buf), p - buf);
 			if (buf[0] != DAP_OK)
-				printf("Failed %02x\n", buf[0]);
+				DEBUG_WARN("dap_jtagtap_tdi_tdo_seq failed %02x\n", buf[0]);
 			if (DO) {
 				memcpy(DO, &buf[1], (transfers + 7) >> 3);
 				DO += (transfers + 7) >> 3;
@@ -692,7 +689,7 @@ void dap_jtagtap_tdi_tdo_seq(uint8_t *DO, bool final_tms, const uint8_t *TMS,
 			}
 			dbg_dap_cmd(buf, sizeof(buf), p - buf);
 			if (buf[0] == DAP_ERROR)
-				printf("Failed %02x\n", buf[0]);
+				DEBUG_WARN("dap_jtagtap_tdi_tdo_seq failed %02x\n", buf[0]);
 			if (DO) {
 				if (buf[1] & 1)
 					DO[last_byte] |= (1 << last_bit);
@@ -719,7 +716,7 @@ void dap_jtagtap_tdi_tdo_seq(uint8_t *DO, bool final_tms, const uint8_t *TMS,
 			}
 			dbg_dap_cmd(buf, sizeof(buf), p - buf);
 			if (buf[0] == DAP_ERROR)
-				printf("Failed %02x\n", buf[0]);
+				DEBUG_WARN("dap_jtagtap_tdi_tdo_seq failed %02x\n", buf[0]);
 			if (DO) {
 				for (int i = 0; i < transfers; i++) {
 					if (buf[i + 1])
@@ -740,7 +737,7 @@ int dap_jtag_configure(void)
 	for (; i < jtag_dev_count; i++) {
 		struct jtag_dev_s *jtag_dev = &jtag_devs[i];
 		*p++ = jtag_dev->ir_len;
-		printf("irlen %d\n", jtag_dev->ir_len);
+		DEBUG_PROBE("irlen %d\n", jtag_dev->ir_len);
 	}
 	if ((!i || i >= JTAG_MAX_DEVS))
 		return -1;
@@ -748,6 +745,6 @@ int dap_jtag_configure(void)
 	buf[1] = i;
 	dbg_dap_cmd(buf, sizeof(buf), p - buf);
 	if (buf[0] != DAP_OK)
-		printf("dap_jtag_configure Failed %02x\n", buf[0]);
+		DEBUG_WARN("dap_jtag_configure Failed %02x\n", buf[0]);
 	return 0;
 }
diff --git a/src/platforms/hosted/ftdi_bmp.c b/src/platforms/hosted/ftdi_bmp.c
index 1c65e23..46f969f 100644
--- a/src/platforms/hosted/ftdi_bmp.c
+++ b/src/platforms/hosted/ftdi_bmp.c
@@ -190,54 +190,50 @@ int ftdi_bmp_init(BMP_CL_OPTIONS_t *cl_opts, bmp_info_t *info)
 		 if (strcmp(cable_desc[index].name, cl_opts->opt_cable) == 0)
 		 break;
 
-	if (index == sizeof(cable_desc)/sizeof(cable_desc[0])){
-		fprintf(stderr, "No cable matching %s found\n", cl_opts->opt_cable);
+	if (index == sizeof(cable_desc)/sizeof(cable_desc[0])) {
+		DEBUG_WARN( "No cable matching %s found\n", cl_opts->opt_cable);
 		return -1;
 	}
 
 	active_cable = &cable_desc[index];
 
-	printf("\nBlack Magic Probe (" FIRMWARE_VERSION ")\n");
-	printf("Copyright (C) 2015  Black Sphere Technologies Ltd.\n");
-	printf("License GPLv3+: GNU GPL version 3 or later "
-	       "<http://gnu.org/licenses/gpl.html>\n\n");
-
+	DEBUG_WARN("Black Magic Probe for FTDI/MPSSE\n");
 	if(ftdic) {
 		ftdi_usb_close(ftdic);
 		ftdi_free(ftdic);
 		ftdic = NULL;
 	}
 	if((ftdic = ftdi_new()) == NULL) {
-		fprintf(stderr, "ftdi_new: %s\n",
+		DEBUG_WARN( "ftdi_new: %s\n",
 			ftdi_get_error_string(ftdic));
 		abort();
 	}
 	info->ftdic = ftdic;
 	if((err = ftdi_set_interface(ftdic, active_cable->interface)) != 0) {
-		fprintf(stderr, "ftdi_set_interface: %d: %s\n",
+		DEBUG_WARN( "ftdi_set_interface: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		goto error_1;
 	}
 	if((err = ftdi_usb_open_desc(
 		ftdic, active_cable->vendor, active_cable->product,
 		active_cable->description, cl_opts->opt_serial)) != 0) {
-		fprintf(stderr, "unable to open ftdi device: %d (%s)\n",
+		DEBUG_WARN( "unable to open ftdi device: %d (%s)\n",
 			err, ftdi_get_error_string(ftdic));
 		goto error_1;
 	}
 
 	if((err = ftdi_set_latency_timer(ftdic, 1)) != 0) {
-		fprintf(stderr, "ftdi_set_latency_timer: %d: %s\n",
+		DEBUG_WARN( "ftdi_set_latency_timer: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		goto error_2;
 	}
 	if((err = ftdi_set_baudrate(ftdic, 1000000)) != 0) {
-		fprintf(stderr, "ftdi_set_baudrate: %d: %s\n",
+		DEBUG_WARN( "ftdi_set_baudrate: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		goto error_2;
 	}
 	if((err = ftdi_write_data_set_chunksize(ftdic, BUF_SIZE)) != 0) {
-		fprintf(stderr, "ftdi_write_data_set_chunksize: %d: %s\n",
+		DEBUG_WARN( "ftdi_write_data_set_chunksize: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		goto error_2;
 	}
@@ -260,7 +256,7 @@ bool libftdi_srst_get_val(void) { return false; }
 void libftdi_buffer_flush(void)
 {
 	assert(ftdi_write_data(ftdic, outbuf, bufptr) == bufptr);
-//	printf("FT2232 libftdi_buffer flush: %d bytes\n", bufptr);
+	DEBUG_WIRE("FT2232 libftdi_buffer flush: %d bytes\n", bufptr);
 	bufptr = 0;
 }
 
diff --git a/src/platforms/hosted/jlink.c b/src/platforms/hosted/jlink.c
index d5453e0..9b95742 100644
--- a/src/platforms/hosted/jlink.c
+++ b/src/platforms/hosted/jlink.c
@@ -45,12 +45,12 @@ static void jlink_print_caps(bmp_info_t *info)
 	uint8_t res[4];
 	send_recv(info->usb_link, cmd, 1, res, sizeof(res));
 	uint32_t caps = res[0] | (res[1] << 8) | (res[2] << 16) | (res[3] << 24);
-	printf("Caps %" PRIx32 "\n", caps);
+	DEBUG_INFO("Caps %" PRIx32 "\n", caps);
 	if (caps & JLINK_CAP_GET_HW_VERSION) {
 		uint8_t cmd[1] = {CMD_GET_HW_VERSION};
 		send_recv(info->usb_link, cmd, 1, NULL, 0);
 		send_recv(info->usb_link, NULL, 0, res, sizeof(res));
-		printf("HW: Type %d, Major %d, Minor %d, Rev %d\n",
+		DEBUG_INFO("HW: Type %d, Major %d, Minor %d, Rev %d\n",
 			   res[3], res[2], res[1], res[0]);
 	}
 }
@@ -62,7 +62,7 @@ static void jlink_print_speed(bmp_info_t *info)
 	uint32_t speed = res[0] | (res[1] << 8) | (res[2] << 16) | (res[3] << 24);
 	double freq_mhz = speed / 1000000.0;
 	uint16_t divisor = res[4] | (res[5] << 8);
-	printf("Emulator speed %3.1f MHz, Mindiv %d\n", freq_mhz, divisor);
+	DEBUG_INFO("Emulator speed %3.1f MHz, Mindiv %d\n", freq_mhz, divisor);
 }
 
 static void jlink_print_version(bmp_info_t *info)
@@ -72,7 +72,7 @@ static void jlink_print_version(bmp_info_t *info)
 	send_recv(info->usb_link, cmd, 1, len_str, sizeof(len_str));
 	uint8_t version[0x70];
 	send_recv(info->usb_link, NULL, 0, version, sizeof(version));
-	printf("%s\n", version );
+	DEBUG_INFO("%s\n", version );
 }
 
 static void jlink_print_interfaces(bmp_info_t *info)
@@ -83,16 +83,15 @@ static void jlink_print_interfaces(bmp_info_t *info)
 	cmd[1] = JLINK_IF_GET_AVAILABLE;
 	uint8_t res1[4];
 	send_recv(info->usb_link, cmd, 2, res1, sizeof(res1));
-	printf("%s active", (res[0] == SELECT_IF_SWD) ? "SWD":
+	DEBUG_INFO("%s active", (res[0] == SELECT_IF_SWD) ? "SWD":
 		   (res[0] == SELECT_IF_JTAG) ? "JTAG" : "NONE");
 	uint8_t other_interface = res1[0] - (res[0] + 1);
 	if (other_interface)
-		printf(", %s available",
+		DEBUG_INFO(", %s available\n",
 			   (other_interface == JLINK_IF_SWD) ? "SWD": "JTAG");
 	else
-		printf(", %s not available",
+		DEBUG_WARN(", %s not available\n",
 			   ((res[0] + 1) == JLINK_IF_SWD) ? "JTAG": "SWD");
-	printf("\n");
 }
 
 static void jlink_info(bmp_info_t *info)
@@ -109,7 +108,7 @@ static int initialize_handle(bmp_info_t *info, libusb_device *dev)
 	struct libusb_config_descriptor *config;
 	int ret =  libusb_get_active_config_descriptor(dev, &config);
 	if (ret != LIBUSB_SUCCESS) {
-		fprintf(stderr, "Failed to get configuration descriptor: %s.",
+		DEBUG_WARN( "Failed to get configuration descriptor: %s.",
 				libusb_error_name(ret));
 		return -1;
 	}
@@ -128,13 +127,13 @@ static int initialize_handle(bmp_info_t *info, libusb_device *dev)
 		found_interface = true;
 		if (libusb_claim_interface (
 				info->usb_link->ul_libusb_device_handle, i)) {
-			fprintf(stderr, " Can not claim handle\n");
+			DEBUG_WARN( " Can not claim handle\n");
 			found_interface = false;
 		}
 		break;
 	}
 	if (!found_interface) {
-		fprintf(stderr, "No suitable interface found.");
+		DEBUG_WARN( "No suitable interface found.");
 		libusb_free_config_descriptor(config);
 		return -1;
 	}
@@ -162,7 +161,7 @@ int jlink_init(bmp_info_t *info)
 	int ret = -1;
 	libusb_device **devs;
     if (libusb_get_device_list(info->libusb_ctx, &devs) < 0) {
-        fprintf(stderr, "libusb_get_device_list() failed");
+        DEBUG_WARN( "libusb_get_device_list() failed");
 		return ret;
 	}
 	int i = 0;
@@ -170,7 +169,7 @@ int jlink_init(bmp_info_t *info)
 		libusb_device *dev =  devs[i];
 		struct libusb_device_descriptor desc;
 		if (libusb_get_device_descriptor(dev, &desc) < 0) {
-            fprintf(stderr, "libusb_get_device_descriptor() failed");
+            DEBUG_WARN( "libusb_get_device_descriptor() failed");
 			goto error;;
 		}
 		if (desc.idVendor !=  USB_PID_SEGGER)
@@ -198,7 +197,7 @@ int jlink_init(bmp_info_t *info)
 	jl->rep_trans = libusb_alloc_transfer(0);
 	if (!jl->req_trans || !jl->rep_trans ||
 		!jl->ep_tx || !jl->ep_rx) {
-		fprintf(stderr,"Device setup failed\n");
+		DEBUG_WARN("Device setup failed\n");
 		goto error;
 	}
 	libusb_free_device_list(devs, 1);
diff --git a/src/platforms/hosted/jlink_adiv5_swdp.c b/src/platforms/hosted/jlink_adiv5_swdp.c
index 507f1db..5e7ba3b 100644
--- a/src/platforms/hosted/jlink_adiv5_swdp.c
+++ b/src/platforms/hosted/jlink_adiv5_swdp.c
@@ -3,7 +3,8 @@
  *
  * Copyright (C) 2011  Black Sphere Technologies Ltd.
  * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- * Copyright (C) 2019 Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
+ * Copyright (C) 2019 - 2020 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
@@ -96,7 +97,7 @@ static int line_reset(bmp_info_t *info)
         send_recv(info->usb_link, NULL, 0, res,  1);
 
         if (res[0] != 0) {
-                fprintf(stderr, "Line reset failed\n");
+                DEBUG_WARN( "Line reset failed\n");
                 return -1;
         }
         return 0;
@@ -174,7 +175,7 @@ int jlink_swdp_scan(bmp_info_t *info)
 	send_recv(info->usb_link, NULL, 0, res,  1);
 
 	if (res[0] != 0) {
-		fprintf(stderr, "Line reset failed\n");
+		DEBUG_WARN( "Line reset failed\n");
 		return 0;
 	}
 	dp->idcode = jlink_adiv5_swdp_low_access(dp, 1, ADIV5_DP_IDCODE, 0);
@@ -265,15 +266,15 @@ static uint32_t jlink_adiv5_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 		raise_exception(EXCEPTION_TIMEOUT, "SWDP ACK timeout");
 
 	if(ack == SWDP_ACK_FAULT) {
-		if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-			fprintf(stderr, "Fault\n");
+		if (cl_debuglevel & BMP_DEBUG_TARGET)
+			DEBUG_WARN( "Fault\n");
 		dp->fault = 1;
 		return 0;
 	}
 
 	if(ack != SWDP_ACK_OK) {
-		if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-			fprintf(stderr, "Protocol\n");
+		if (cl_debuglevel & BMP_DEBUG_TARGET)
+			DEBUG_WARN( "Protocol\n");
 		line_reset(&info);
 		return 0;
 	}
diff --git a/src/platforms/hosted/jlink_jtagtap.c b/src/platforms/hosted/jlink_jtagtap.c
index 6ea1cbd..4b20e14 100644
--- a/src/platforms/hosted/jlink_jtagtap.c
+++ b/src/platforms/hosted/jlink_jtagtap.c
@@ -40,8 +40,7 @@ static void jtagtap_reset(void)
 
 static void jtagtap_tms_seq(uint32_t MS, int ticks)
 {
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("jtagtap_tms_seq 0x%08" PRIx32 ", ticks %d\n", MS, ticks);
+	DEBUG_PROBE("jtagtap_tms_seq 0x%08" PRIx32 ", ticks %d\n", MS, ticks);
 	int len = (ticks + 7) / 8;
 	uint8_t cmd[12];
 	cmd[0] = CMD_HW_JTAG3;
@@ -68,13 +67,13 @@ static void jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms,
 	if (!ticks)
 		return;
 	int len = (ticks + 7) / 8;
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
-		printf("jtagtap_tdi_tdo %s, ticks %d, DI: ",
+	if (cl_debuglevel & BMP_DEBUG_PROBE) {
+		DEBUG_PROBE("jtagtap_tdi_tdo %s, ticks %d, DI: ",
 			   (final_tms) ? "Final TMS" : "", ticks);
 		for (int i = 0; i < len; i++) {
-			printf("%02x", DI[i]);
+			DEBUG_PROBE("%02x", DI[i]);
 		}
-		printf("\n");
+		DEBUG_PROBE("\n");
 	}
 	uint8_t *cmd = alloca(4 + 2 * len);
 	cmd[0] = CMD_HW_JTAG3;
@@ -103,25 +102,24 @@ static void jtagtap_tdi_tdo_seq(uint8_t *DO, const uint8_t final_tms,
 static void jtagtap_tdi_seq(const uint8_t final_tms, const uint8_t *DI,
 							int ticks)
 {
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
-		printf("jtagtap_tdi_seq %s:", (final_tms)? "final_tms" : "");
+	if (cl_debuglevel & BMP_DEBUG_PROBE) {
+		DEBUG_PROBE("jtagtap_tdi_seq %s:", (final_tms)? "final_tms" : "");
 		const uint8_t *p = DI;
 		unsigned int i = (ticks & 7) & ~7 ;
 		if (i > 16)
 			i = 16;
 		while (i--)
-			printf(" %02x", *p++);
+			DEBUG_PROBE(" %02x", *p++);
 		if (ticks > (16 * 8))
-			printf(" ...");
-		printf("\n");
+			DEBUG_PROBE(" ...");
+		DEBUG_PROBE("\n");
 	}
 	return jtagtap_tdi_tdo_seq(NULL,  final_tms, DI, ticks);
 }
 
 static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 {
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("jtagtap_next TMS 0x%02x, TDI %02x\n", dTMS, dTDI);
+	DEBUG_PROBE("jtagtap_next TMS 0x%02x, TDI %02x\n", dTMS, dTDI);
 	uint8_t cmd[6];
 	cmd[0] = CMD_HW_JTAG3;
 	cmd[1] = 0;
@@ -140,13 +138,12 @@ static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 
 int jlink_jtagtap_init(bmp_info_t *info, jtag_proc_t *jtag_proc)
 {
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("jtap_init\n");
+	DEBUG_PROBE("jtap_init\n");
 	uint8_t cmd_switch[2] = {CMD_GET_SELECT_IF, JLINK_IF_GET_AVAILABLE};
 	uint8_t res[4];
 	send_recv(info->usb_link, cmd_switch, 2, res, sizeof(res));
 	if (!(res[0] & JLINK_IF_JTAG)) {
-		fprintf(stderr, "JTAG not available\n");
+		DEBUG_WARN("JTAG not available\n");
 		return -1;
 	}
 	cmd_switch[1] = SELECT_IF_JTAG;
@@ -176,7 +173,7 @@ int jlink_jtagtap_init(bmp_info_t *info, jtag_proc_t *jtag_proc)
 	send_recv(info->usb_link, NULL, 0, res,  1);
 
 	if (res[0] != 0) {
-		fprintf(stderr, "Switch to JTAGt failed\n");
+		DEBUG_WARN("Switch to JTAG failed\n");
 		return 0;
 	}
 	jtag_proc->jtagtap_reset = jtagtap_reset;
diff --git a/src/platforms/hosted/libftdi_jtagtap.c b/src/platforms/hosted/libftdi_jtagtap.c
index 7a009ab..30c8578 100644
--- a/src/platforms/hosted/libftdi_jtagtap.c
+++ b/src/platforms/hosted/libftdi_jtagtap.c
@@ -49,21 +49,21 @@ int libftdi_jtagtap_init(jtag_proc_t *jtag_proc)
 	assert(ftdic != NULL);
 	int err = ftdi_usb_purge_buffers(ftdic);
 	if (err != 0) {
-		printf("ftdi_usb_purge_buffer: %d: %s\n",
+		DEBUG_WARN("ftdi_usb_purge_buffer: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		abort();
 	}
 	/* Reset MPSSE controller. */
 	err = ftdi_set_bitmode(ftdic, 0,  BITMODE_RESET);
 	if (err != 0) {
-		printf("ftdi_set_bitmode: %d: %s\n",
+		DEBUG_WARN("ftdi_set_bitmode: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		return -1;
 	}
 	/* Enable MPSSE controller. Pin directions are set later.*/
 	err = ftdi_set_bitmode(ftdic, 0, BITMODE_MPSSE);
 	if (err != 0) {
-		printf("ftdi_set_bitmode: %d: %s\n",
+		DEBUG_WARN("ftdi_set_bitmode: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		return -1;
 	}
@@ -111,7 +111,7 @@ static void jtagtap_tdi_tdo_seq(
 	if(!ticks) return;
 	if (!DI && !DO) return;
 
-//	printf("ticks: %d\n", ticks);
+//	DEBUG_PROBE("ticks: %d\n", ticks);
 	if(final_tms) ticks--;
 	rticks = ticks & 7;
 	ticks >>= 3;
@@ -153,7 +153,7 @@ static void jtagtap_tdi_tdo_seq(
 		if(final_tms) rsize--;
 
 		while(rsize--) {
-			/*if(rsize) printf("%02X ", tmp[index]);*/
+			if(rsize) DEBUG_WIRE("%02X ", tmp[index]);
 			*DO++ = tmp[index++];
 		}
 		if (rticks == 0)
@@ -166,7 +166,7 @@ static void jtagtap_tdi_tdo_seq(
 		if(rticks) {
 			*DO >>= (8-rticks);
 		}
-		/*printf("%02X\n", *DO);*/
+		DEBUG_WIRE("%02X\n", *DO);
 	}
 }
 
diff --git a/src/platforms/hosted/libftdi_swdptap.c b/src/platforms/hosted/libftdi_swdptap.c
index 6feb70d..91e5428 100644
--- a/src/platforms/hosted/libftdi_swdptap.c
+++ b/src/platforms/hosted/libftdi_swdptap.c
@@ -43,26 +43,26 @@ static void swdptap_seq_out_parity(uint32_t MS, int ticks);
 int libftdi_swdptap_init(swd_proc_t *swd_proc)
 {
 	if (!active_cable->bitbang_tms_in_pin) {
-		printf("SWD not possible or missing item in cable description.\n");
+		DEBUG_WARN("SWD not possible or missing item in cable description.\n");
 		return -1;
 	}
 	int err = ftdi_usb_purge_buffers(ftdic);
 	if (err != 0) {
-		printf("ftdi_usb_purge_buffer: %d: %s\n",
+		DEBUG_WARN("ftdi_usb_purge_buffer: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		return -1;
 	}
 	/* Reset MPSSE controller. */
 	err = ftdi_set_bitmode(ftdic, 0,  BITMODE_RESET);
 	if (err != 0) {
-		printf("ftdi_set_bitmode: %d: %s\n",
+		DEBUG_WARN("ftdi_set_bitmode: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		return -1;
 	}
 	/* Enable MPSSE controller. Pin directions are set later.*/
 	err = ftdi_set_bitmode(ftdic, 0, BITMODE_MPSSE);
 	if (err != 0) {
-		printf("ftdi_set_bitmode: %d: %s\n",
+		DEBUG_WARN("ftdi_set_bitmode: %d: %s\n",
 			err, ftdi_get_error_string(ftdic));
 		return -1;
 	}
diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 45366e6..0c55a94 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -1,8 +1,7 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2020
- * Written by Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
+ * Copyright (C) 2020 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
@@ -91,7 +90,7 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 	libusb_device **devs;
 	int n_devs = libusb_get_device_list(info->libusb_ctx, &devs);
     if (n_devs < 0) {
-        fprintf(stderr, "WARN:libusb_get_device_list() failed");
+        DEBUG_WARN( "WARN:libusb_get_device_list() failed");
 		return -1;
 	}
 	bool report = false;
@@ -107,7 +106,7 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 		libusb_device *dev =  devs[i];
 		int res = libusb_get_device_descriptor(dev, &desc);
 		if (res < 0) {
-            fprintf(stderr, "WARN: libusb_get_device_descriptor() failed: %s",
+            DEBUG_WARN( "WARN: libusb_get_device_descriptor() failed: %s",
 					libusb_strerror(res));
 			libusb_free_device_list(devs, 1);
 			continue;
@@ -115,7 +114,8 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 		libusb_device_handle *handle;
 		res = libusb_open(dev, &handle);
 		if (res != LIBUSB_SUCCESS) {
-			fprintf(stderr,"WARN: Open failed\n");
+			DEBUG_INFO("INFO: Open USB %04x:%04x failed\n",
+					   desc.idVendor, desc.idProduct);
 			continue;
 		}
 		res = libusb_get_string_descriptor_ascii(
@@ -138,7 +138,7 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 				handle, desc.iProduct, (uint8_t*)product,
 				sizeof(product));
 			if (res <= 0) {
-				fprintf(stderr, "WARN:"
+				DEBUG_WARN( "WARN:"
 						"libusb_get_string_descriptor_ascii "
 						"for ident_string failed: %s\n",
 						libusb_strerror(res));
@@ -170,7 +170,7 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 				type = BMP_TYPE_STLINKV2;
 			} else {
 				if (desc.idProduct == PRODUCT_ID_STLINKV1)
-					fprintf(stderr, "INFO: STLINKV1 not supported\n");
+					DEBUG_WARN( "INFO: STLINKV1 not supported\n");
 				continue;
 			}
 		} else if ((strstr(manufacturer, "CMSIS")) || (strstr(product, "CMSIS"))) {
@@ -182,7 +182,7 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 		}
 		found_debuggers ++;
 		if (report) {
-			printf("%2d: %s, %s, %s\n", found_debuggers,
+			DEBUG_WARN("%2d: %s, %s, %s\n", found_debuggers,
 				   serial,
 				   manufacturer,product);
 		}
@@ -200,7 +200,7 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 	}
 	if (found_debuggers > 1) {
 		if (!report) {
-			printf("%d debuggers found! Select with -P <num>, -s <string> "
+			DEBUG_WARN("%d debuggers found! Select with -P <num>, -s <string> "
 				   "and/or -S <string>\n",
 				   found_debuggers);
 			report = true;
@@ -221,7 +221,7 @@ void platform_init(int argc, char **argv)
 	signal(SIGINT, sigterm_handler);
 	int res = libusb_init(&info.libusb_ctx);
 	if (res) {
-		fprintf(stderr, "Fatal: Failed to get USB context: %s\n",
+		DEBUG_WARN( "Fatal: Failed to get USB context: %s\n",
 				libusb_strerror(res));
 		exit(-1);
 	}
@@ -237,7 +237,7 @@ void platform_init(int argc, char **argv)
 	} else if (find_debuggers(&cl_opts, &info)) {
 		exit(-1);
 	}
-	printf("Using %04x:%04x %s %s %s\n", info.vid, info.pid, info.serial,
+	DEBUG_WARN("Using %04x:%04x %s %s %s\n", info.vid, info.pid, info.serial,
 		   info.manufacturer,
 		   info.product);
 	switch (info.bmp_type) {
@@ -473,44 +473,44 @@ void platform_buffer_flush(void)
 static void ap_decode_access(uint16_t addr, uint8_t RnW)
 {
 	if (RnW)
-		printf("Read  ");
+		fprintf(stderr, "Read  ");
 	else
-		printf("Write ");
+		fprintf(stderr, "Write ");
 	switch(addr) {
 	case 0x00:
 		if (RnW)
-			printf("DP_DPIDR :");
+			fprintf(stderr, "DP_DPIDR :");
 		else
-			printf("DP_ABORT :");
+			fprintf(stderr, "DP_ABORT :");
 		break;
-	case 0x004: printf("CTRL/STAT:");
+	case 0x004: fprintf(stderr, "CTRL/STAT:");
 		break;
 	case 0x008:
 		if (RnW)
-			printf("RESEND   :");
+			fprintf(stderr, "RESEND   :");
 		else
-			printf("DP_SELECT:");
+			fprintf(stderr, "DP_SELECT:");
 		break;
-	case 0x00c: printf("DP_RDBUFF:");
+	case 0x00c: fprintf(stderr, "DP_RDBUFF:");
 		break;
-	case 0x100: printf("AP_CSW   :");
+	case 0x100: fprintf(stderr, "AP_CSW   :");
 		break;
-	case 0x104: printf("AP_TAR   :");
+	case 0x104: fprintf(stderr, "AP_TAR   :");
 		break;
-	case 0x10c: printf("AP_DRW   :");
+	case 0x10c: fprintf(stderr, "AP_DRW   :");
 		break;
-	case 0x1f8: printf("AP_BASE  :");
+	case 0x1f8: fprintf(stderr, "AP_BASE  :");
 		break;
-	case 0x1fc: printf("AP_IDR   :");
+	case 0x1fc: fprintf(stderr, "AP_IDR   :");
 		break;
 	}
 }
 
 void adiv5_dp_write(ADIv5_DP_t *dp, uint16_t addr, uint32_t value)
 {
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+	if (cl_debuglevel & BMP_DEBUG_TARGET) {
 		ap_decode_access(addr, ADIV5_LOW_WRITE);
-		printf(" 0x%08" PRIx32 "\n", value);
+		fprintf(stderr, " 0x%08" PRIx32 "\n", value);
 	}
 	dp->low_access(dp, ADIV5_LOW_WRITE, addr, value);
 }
@@ -518,9 +518,9 @@ void adiv5_dp_write(ADIv5_DP_t *dp, uint16_t addr, uint32_t value)
 uint32_t adiv5_dp_read(ADIv5_DP_t *dp, uint16_t addr)
 {
 	uint32_t ret = dp->dp_read(dp, addr);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+	if (cl_debuglevel & BMP_DEBUG_TARGET) {
 		ap_decode_access(addr, ADIV5_LOW_READ);
-		printf(" 0x%08" PRIx32 "\n", ret);
+		fprintf(stderr, " 0x%08" PRIx32 "\n", ret);
 	}
 	return ret;
 }
@@ -528,9 +528,7 @@ uint32_t adiv5_dp_read(ADIv5_DP_t *dp, uint16_t addr)
 uint32_t adiv5_dp_error(ADIv5_DP_t *dp)
 {
 	uint32_t ret = dp->error(dp);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
-		printf("DP Error 0x%08" PRIx32 "\n", ret);
-	}
+	DEBUG_TARGET( "DP Error 0x%08" PRIx32 "\n", ret);
 	return ret;
 }
 
@@ -538,9 +536,9 @@ uint32_t adiv5_dp_low_access(struct ADIv5_DP_s *dp, uint8_t RnW,
 							 uint16_t addr, uint32_t value)
 {
 	uint32_t ret = dp->low_access(dp, RnW, addr, value);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+	if (cl_debuglevel & BMP_DEBUG_TARGET) {
 		ap_decode_access(addr, RnW);
-		printf(" 0x%08" PRIx32 "\n", (RnW)? ret : value);
+		fprintf(stderr, " 0x%08" PRIx32 "\n", (RnW)? ret : value);
 	}
 	return ret;
 }
@@ -548,18 +546,18 @@ uint32_t adiv5_dp_low_access(struct ADIv5_DP_s *dp, uint8_t RnW,
 uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 {
 	uint32_t ret = ap->dp->ap_read(ap, addr);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+	if (cl_debuglevel & BMP_DEBUG_TARGET) {
 		ap_decode_access(addr, ADIV5_LOW_READ);
-		printf(" 0x%08" PRIx32 "\n", ret);
+		fprintf(stderr, " 0x%08" PRIx32 "\n", ret);
 	}
 	return ret;
 }
 
 void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 {
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
+	if (cl_debuglevel & BMP_DEBUG_TARGET) {
 		ap_decode_access(addr, ADIV5_LOW_WRITE);
-		printf(" 0x%08" PRIx32 "\n", value);
+		fprintf(stderr, " 0x%08" PRIx32 "\n", value);
 	}
 	return ap->dp->ap_write(ap, addr, value);
 }
@@ -567,42 +565,42 @@ void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 {
 	ap->dp->mem_read(ap, dest, src, len);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
-		printf("ap_memread @ %" PRIx32 " len %" PRIx32 ":", src, (uint32_t)len);
+	if (cl_debuglevel & BMP_DEBUG_TARGET) {
+		fprintf(stderr, "ap_memread @ %" PRIx32 " len %" PRIx32 ":",
+				src, (uint32_t)len);
 		uint8_t *p = (uint8_t *) dest;
 		unsigned int i = len;
 		if (i > 16)
 			i = 16;
 		while (i--)
-			printf(" %02x", *p++);
+			fprintf(stderr, " %02x", *p++);
 		if (len > 16)
-			printf(" ...");
-		printf("\n");
+			fprintf(stderr, " ...");
+		fprintf(stderr, "\n");
 	}
 	return;
 }
 void adiv5_mem_write_sized(	ADIv5_AP_t *ap, uint32_t dest, const void *src,
 							size_t len, enum align align)
 {
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM) {
-		printf("ap_mem_write_sized @ %" PRIx32 " len %" PRIx32 ", align %d:",
-			   dest, (uint32_t)len, 1 << align);
+	if (cl_debuglevel & BMP_DEBUG_TARGET) {
+		fprintf(stderr, "ap_mem_write_sized @ %" PRIx32 " len %" PRIx32
+				", align %d:", dest, (uint32_t)len, 1 << align);
 		uint8_t *p = (uint8_t *) src;
 		unsigned int i = len;
 		if (i > 16)
 			i = 16;
 		while (i--)
-			printf(" %02x", *p++);
+			fprintf(stderr, " %02x", *p++);
 		if (len > 16)
-			printf(" ...");
-		printf("\n");
+			fprintf(stderr, " ...");
+		fprintf(stderr, "\n");
 	}
 	return ap->dp->mem_write_sized(ap, dest, src, len, align);
 }
 
 void adiv5_dp_abort(struct ADIv5_DP_s *dp, uint32_t abort)
 {
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("Abort: %08" PRIx32 "\n", abort);
+	DEBUG_TARGET("Abort: %08" PRIx32 "\n", abort);
 	return dp->abort(dp, abort);
 }
diff --git a/src/platforms/hosted/remote_jtagtap.c b/src/platforms/hosted/remote_jtagtap.c
index 7f64eee..d98663b 100644
--- a/src/platforms/hosted/remote_jtagtap.c
+++ b/src/platforms/hosted/remote_jtagtap.c
@@ -4,6 +4,7 @@
  * Copyright (C) 2008  Black Sphere Technologies Ltd.
  * Written by Gareth McMullin <gareth@blacksphere.co.nz>
  * Modified by Dave Marples <dave@marples.net>
+ * Modified (c) 2020 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
@@ -55,7 +56,7 @@ int remote_jtagtap_init(jtag_proc_t *jtag_proc)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-      fprintf(stderr, "jtagtap_init failed, error %s\n",
+      DEBUG_WARN("jtagtap_init failed, error %s\n",
 			  s ? (char *)&(construct[1]) : "unknown");
       exit(-1);
     }
@@ -82,7 +83,7 @@ static void jtagtap_reset(void)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-		fprintf(stderr, "jtagtap_reset failed, error %s\n",
+		DEBUG_WARN("jtagtap_reset failed, error %s\n",
 				s ? (char *)&(construct[1]) : "unknown");
 		exit(-1);
     }
@@ -99,7 +100,7 @@ static void jtagtap_tms_seq(uint32_t MS, int ticks)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-		fprintf(stderr, "jtagtap_tms_seq failed, error %s\n",
+		DEBUG_WARN("jtagtap_tms_seq failed, error %s\n",
 				s ? (char *)&(construct[1]) : "unknown");
 		exit(-1);
     }
@@ -126,7 +127,7 @@ static void jtagtap_tdi_tdo_seq(
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-		fprintf(stderr, "jtagtap_tms_seq failed, error %s\n",
+		DEBUG_WARN("jtagtap_tms_seq failed, error %s\n",
 				s ? (char *)&(construct[1]) : "unknown");
 		exit(-1);
     }
@@ -156,7 +157,7 @@ static uint8_t jtagtap_next(uint8_t dTMS, uint8_t dTDI)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-		fprintf(stderr, "jtagtap_next failed, error %s\n",
+		DEBUG_WARN("jtagtap_next failed, error %s\n",
 				s ? (char *)&(construct[1]) : "unknown");
 		exit(-1);
     }
diff --git a/src/platforms/hosted/remote_swdptap.c b/src/platforms/hosted/remote_swdptap.c
index 93f74f7..134de84 100644
--- a/src/platforms/hosted/remote_swdptap.c
+++ b/src/platforms/hosted/remote_swdptap.c
@@ -1,9 +1,9 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2018 Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
  * Written by Gareth McMullin <gareth@blacksphere.co.nz>
  * Modified by Dave Marples <dave@marples.net>
+ * Modification (C) 2020 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
@@ -37,6 +37,7 @@ static void swdptap_seq_out_parity(uint32_t MS, int ticks);
 
 int remote_swdptap_init(swd_proc_t *swd_proc)
 {
+	DEBUG_WIRE("remote_swdptap_init\n");
 	uint8_t construct[REMOTE_MAX_MSG_SIZE];
 	int s;
 	s = sprintf((char *)construct,"%s", REMOTE_SWDP_INIT_STR);
@@ -44,7 +45,7 @@ int remote_swdptap_init(swd_proc_t *swd_proc)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((!s) || (construct[0] == REMOTE_RESP_ERR)) {
-		fprintf(stderr, "swdptap_init failed, error %s\n",
+		DEBUG_WARN("swdptap_init failed, error %s\n",
 				s ? (char *)&(construct[1]) : "unknown");
 		exit(-1);
     }
@@ -67,12 +68,14 @@ static bool swdptap_seq_in_parity(uint32_t *res, int ticks)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((s<2) || (construct[0] == REMOTE_RESP_ERR)) {
-		fprintf(stderr, "swdptap_seq_in_parity failed, error %s\n",
+		DEBUG_WARN("swdptap_seq_in_parity failed, error %s\n",
 				s ? (char *)&(construct[1]) : "short response");
 		exit(-1);
 	}
 
 	*res=remotehston(-1, (char *)&construct[1]);
+	DEBUG_PROBE("swdptap_seq_in_parity  %2d ticks: %08" PRIx32 " %s\n",
+				 ticks, *res, (construct[0] != REMOTE_RESP_OK) ? "ERR" : "OK");
 	return (construct[0] != REMOTE_RESP_OK);
 }
 
@@ -86,12 +89,14 @@ static uint32_t swdptap_seq_in(int ticks)
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((s<2) || (construct[0] == REMOTE_RESP_ERR)) {
-      fprintf(stderr, "swdptap_seq_in failed, error %s\n",
+      DEBUG_WARN("swdptap_seq_in failed, error %s\n",
 			  s ? (char *)&(construct[1]) : "short response");
       exit(-1);
     }
-
-	return remotehston(-1,(char *)&construct[1]);
+	uint32_t res = remotehston(-1,(char *)&construct[1]);
+	DEBUG_PROBE("swdptap_seq_in         %2d ticks: %08" PRIx32 "\n",
+				 ticks, res);
+	return res;
 }
 
 static void swdptap_seq_out(uint32_t MS, int ticks)
@@ -99,12 +104,14 @@ static void swdptap_seq_out(uint32_t MS, int ticks)
 	uint8_t construct[REMOTE_MAX_MSG_SIZE];
 	int s;
 
+	DEBUG_PROBE("swdptap_seq_out        %2d ticks: %08" PRIx32 "\n",
+				 ticks, MS);
 	s = sprintf((char *)construct,REMOTE_SWDP_OUT_STR, ticks, MS);
 	platform_buffer_write(construct, s);
 
 	s=platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((s < 1) || (construct[0] == REMOTE_RESP_ERR)) {
-      fprintf(stderr, "swdptap_seq_out failed, error %s\n",
+      DEBUG_WARN("swdptap_seq_out failed, error %s\n",
 			  s ? (char *)&(construct[1]) : "short response");
       exit(-1);
     }
@@ -115,12 +122,14 @@ static void swdptap_seq_out_parity(uint32_t MS, int ticks)
 	uint8_t construct[REMOTE_MAX_MSG_SIZE];
 	int s;
 
+	DEBUG_PROBE("swdptap_seq_out_parity %2d ticks: %08" PRIx32 "\n",
+				 ticks, MS);
 	s = sprintf((char *)construct, REMOTE_SWDP_OUT_PAR_STR, ticks, MS);
 	platform_buffer_write(construct, s);
 
 	s = platform_buffer_read(construct, REMOTE_MAX_MSG_SIZE);
 	if ((s < 1) || (construct[1] == REMOTE_RESP_ERR)){
-      fprintf(stderr, "swdptap_seq_out_parity failed, error %s\n",
+      DEBUG_WARN("swdptap_seq_out_parity failed, error %s\n",
 			  s ? (char *)&(construct[2]) : "short response");
       exit(-1);
     }
diff --git a/src/platforms/hosted/stlinkv2.c b/src/platforms/hosted/stlinkv2.c
index b6ce189..110bbc6 100644
--- a/src/platforms/hosted/stlinkv2.c
+++ b/src/platforms/hosted/stlinkv2.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2019 Uwe Bonnes
+ * Copyright (C) 2019-2020 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
@@ -232,43 +232,46 @@ static int stlink_usb_error_check(uint8_t *data, bool verbose)
 			return STLINK_ERROR_OK;
 		case STLINK_DEBUG_ERR_FAULT:
 			if (verbose)
-				printf("SWD fault response (0x%x)\n", STLINK_DEBUG_ERR_FAULT);
+				DEBUG_WARN("SWD fault response (0x%x)\n",
+						   STLINK_DEBUG_ERR_FAULT);
 			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_UNKNOWN_JTAG_CHAIN:
 			if (verbose)
-				printf("Unknown JTAG chain\n");
+				DEBUG_WARN("Unknown JTAG chain\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_NO_DEVICE_CONNECTED:
 			if (verbose)
-				printf("No device connected\n");
+				DEBUG_WARN("No device connected\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_COMMAND_ERROR:
 			if (verbose)
-				printf("Command error\n");
+				DEBUG_WARN("Command error\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_GET_IDCODE_ERROR:
 			if (verbose)
-				printf("Failure reading IDCODE\n");
+				DEBUG_WARN("Failure reading IDCODE\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_DBG_POWER_ERROR:
 			if (verbose)
-				printf("Failure powering DBG\n");
+				DEBUG_WARN("Failure powering DBG\n");
 			return STLINK_ERROR_WAIT;
 		case STLINK_SWD_AP_WAIT:
 			if (verbose)
-				printf("wait status SWD_AP_WAIT (0x%x)\n", STLINK_SWD_AP_WAIT);
+				DEBUG_WARN("wait status SWD_AP_WAIT (0x%x)\n",
+						   STLINK_SWD_AP_WAIT);
 			return STLINK_ERROR_WAIT;
 		case STLINK_SWD_DP_WAIT:
 			if (verbose)
-				printf("wait status SWD_DP_WAIT (0x%x)\n", STLINK_SWD_DP_WAIT);
+				DEBUG_WARN("wait status SWD_DP_WAIT (0x%x)\n",
+						   STLINK_SWD_DP_WAIT);
 			return STLINK_ERROR_WAIT;
 		case STLINK_JTAG_WRITE_ERROR:
 			if (verbose)
-				printf("Write error\n");
+				DEBUG_WARN("Write error\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_WRITE_VERIF_ERROR:
 			if (verbose)
-				printf("Write verify error, ignoring\n");
+				DEBUG_WARN("Write verify error, ignoring\n");
 			return STLINK_ERROR_OK;
 		case STLINK_SWD_AP_FAULT:
 			/* git://git.ac6.fr/openocd commit 657e3e885b9ee10
@@ -278,40 +281,40 @@ static int stlink_usb_error_check(uint8_t *data, bool verbose)
 			 */
 			Stlink.ap_error = true;
 			if (verbose)
-				printf("STLINK_SWD_AP_FAULT\n");
+				DEBUG_WARN("STLINK_SWD_AP_FAULT\n");
 			return STLINK_ERROR_DP_FAULT;
 		case STLINK_SWD_AP_ERROR:
 			if (verbose)
-				printf("STLINK_SWD_AP_ERROR\n");
+				DEBUG_WARN("STLINK_SWD_AP_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_AP_PARITY_ERROR:
 			if (verbose)
-				printf("STLINK_SWD_AP_PARITY_ERROR\n");
+				DEBUG_WARN("STLINK_SWD_AP_PARITY_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_DP_FAULT:
 			if (verbose)
-				printf("STLINK_SWD_DP_FAULT\n");
+				DEBUG_WARN("STLINK_SWD_DP_FAULT\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_DP_ERROR:
 			if (verbose)
-				printf("STLINK_SWD_DP_ERROR\n");
+				DEBUG_WARN("STLINK_SWD_DP_ERROR\n");
 			raise_exception(EXCEPTION_ERROR, "STLINK_SWD_DP_ERROR");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_DP_PARITY_ERROR:
 			if (verbose)
-				printf("STLINK_SWD_DP_PARITY_ERROR\n");
+				DEBUG_WARN("STLINK_SWD_DP_PARITY_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_AP_WDATA_ERROR:
 			if (verbose)
-				printf("STLINK_SWD_AP_WDATA_ERROR\n");
+				DEBUG_WARN("STLINK_SWD_AP_WDATA_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_AP_STICKY_ERROR:
 			if (verbose)
-				printf("STLINK_SWD_AP_STICKY_ERROR\n");
+				DEBUG_WARN("STLINK_SWD_AP_STICKY_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_SWD_AP_STICKYORUN_ERROR:
 			if (verbose)
-				printf("STLINK_SWD_AP_STICKYORUN_ERROR\n");
+				DEBUG_WARN("STLINK_SWD_AP_STICKYORUN_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_BAD_AP_ERROR:
 			/* ADIV5 probe 256 APs, most of them are non exisitant.*/
@@ -319,15 +322,15 @@ static int stlink_usb_error_check(uint8_t *data, bool verbose)
 		case STLINK_TOO_MANY_AP_ERROR:
 			/* TI TM4C duplicates AP. Error happens at AP9.*/
 			if (verbose)
-				printf("STLINK_TOO_MANY_AP_ERROR\n");
+				DEBUG_WARN("STLINK_TOO_MANY_AP_ERROR\n");
 			return STLINK_ERROR_FAIL;
 		case STLINK_JTAG_UNKNOWN_CMD :
 			if (verbose)
-				printf("STLINK_JTAG_UNKNOWN_CMD\n");
+				DEBUG_WARN("STLINK_JTAG_UNKNOWN_CMD\n");
 			return STLINK_ERROR_FAIL;
 		default:
 			if (verbose)
-				printf("unknown/unexpected STLINK status code 0x%x\n", data[0]);
+				DEBUG_WARN("unknown/unexpected STLINK status code 0x%x\n", data[0]);
 			return STLINK_ERROR_FAIL;
 	}
 }
@@ -345,7 +348,7 @@ static int stlink_send_recv_retry(uint8_t *txbuf, size_t txsize,
 			return res;
 		uint32_t now = platform_time_ms();
 		if (((now - start) > 1000) || (res != STLINK_ERROR_WAIT)) {
-			printf("write_retry failed. ");
+			DEBUG_WARN("write_retry failed. ");
 			return res;
 		}
 	}
@@ -364,7 +367,7 @@ static int read_retry(uint8_t *txbuf, size_t txsize,
 			return res;
 		uint32_t now = platform_time_ms();
 		if (((now -start) > 1000) || (res != STLINK_ERROR_WAIT)) {
-			printf("read_retry failed. ");
+			DEBUG_WARN("read_retry failed. ");
 			stlink_usb_get_rw_status(true);
 			return res;
 		}
@@ -400,7 +403,7 @@ static void stlink_version(bmp_info_t *info)
 		uint8_t data[12];
 		int size = send_recv(info->usb_link, cmd, 16, data, 12);
 		if (size == -1) {
-			printf("[!] stlink_send_recv STLINK_APIV3_GET_VERSION_EX\n");
+			DEBUG_WARN("[!] stlink_send_recv STLINK_APIV3_GET_VERSION_EX\n");
 		}
 		Stlink.ver_stlink = data[0];
 		Stlink.ver_swim  =  data[1];
@@ -415,7 +418,7 @@ static void stlink_version(bmp_info_t *info)
 		uint8_t data[6];
 		int size = send_recv(info->usb_link, cmd, 16, data, 6);
 		if (size == -1) {
-			printf("[!] stlink_send_recv STLINK_GET_VERSION_EX\n");
+			DEBUG_WARN("[!] stlink_send_recv STLINK_GET_VERSION_EX\n");
 		}
 		Stlink.vid = data[3] << 8 | data[2];
 		Stlink.pid = data[5] << 8 | data[4];
@@ -430,15 +433,15 @@ static void stlink_version(bmp_info_t *info)
 			Stlink.ver_swim = (version >> 0) & 0x3f;
 		}
 	}
-	printf("V%dJ%d",Stlink.ver_stlink, Stlink.ver_jtag);
+	DEBUG_INFO("V%dJ%d",Stlink.ver_stlink, Stlink.ver_jtag);
 	if (Stlink.ver_hw == 30) {
-		printf("M%dB%dS%d", Stlink.ver_mass, Stlink.ver_bridge, Stlink.ver_swim);
+		DEBUG_INFO("M%dB%dS%d", Stlink.ver_mass, Stlink.ver_bridge, Stlink.ver_swim);
 	} else if (Stlink.ver_hw == 20) {
-		printf("S%d", Stlink.ver_swim);
+		DEBUG_INFO("S%d", Stlink.ver_swim);
 	} else if (Stlink.ver_hw == 21) {
-		printf("M%d", Stlink.ver_mass);
+		DEBUG_INFO("M%d", Stlink.ver_mass);
 	}
-	printf("\n");
+	DEBUG_INFO("\n");
 }
 
 static bool stlink_leave_state(bmp_info_t *info)
@@ -448,23 +451,23 @@ static bool stlink_leave_state(bmp_info_t *info)
 	send_recv(info->usb_link,cmd, 16, data, 2);
 	if (data[0] == STLINK_DEV_DFU_MODE) {
 		uint8_t dfu_cmd[16] = {STLINK_DFU_COMMAND, STLINK_DFU_EXIT};
-		printf("Leaving DFU Mode\n");
+		DEBUG_INFO("Leaving DFU Mode\n");
 		send_recv(info->usb_link, dfu_cmd, 16, NULL, 0);
 		return true;
 	} else if (data[0] == STLINK_DEV_SWIM_MODE) {
 		uint8_t swim_cmd[16] = {STLINK_SWIM_COMMAND, STLINK_SWIM_EXIT};
-		printf("Leaving SWIM Mode\n");
+		DEBUG_INFO("Leaving SWIM Mode\n");
 		send_recv(info->usb_link, swim_cmd, 16, NULL, 0);
 	} else if (data[0] == STLINK_DEV_DEBUG_MODE) {
 		uint8_t dbg_cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_EXIT};
-		printf("Leaving DEBUG Mode\n");
+		DEBUG_INFO("Leaving DEBUG Mode\n");
 		send_recv(info->usb_link, dbg_cmd, 16, NULL, 0);
 	} else if (data[0] == STLINK_DEV_BOOTLOADER_MODE) {
-		printf("Leaving BOOTLOADER Mode\n");
+		DEBUG_INFO("Leaving BOOTLOADER Mode\n");
 	} else if (data[0] == STLINK_DEV_MASS_MODE) {
-		printf("Leaving MASS Mode\n");
+		DEBUG_INFO("Leaving MASS Mode\n");
 	} else {
-		printf("Unknown Mode %02x\n", data[0]);
+		DEBUG_INFO("Unknown Mode %02x\n", data[0]);
 	}
 	return false;
 }
@@ -503,7 +506,7 @@ int stlink_init(bmp_info_t *info)
 	ssize_t cnt;
 	cnt = libusb_get_device_list(info->libusb_ctx, &devs);
 	if (cnt < 0) {
-		fprintf(stderr, "FATAL: Stlink libusb_get_device_list failed\n");
+		DEBUG_WARN("FATAL: Stlink libusb_get_device_list failed\n");
 		return -1;
 	}
 	int i = 0;
@@ -512,7 +515,7 @@ int stlink_init(bmp_info_t *info)
 		struct libusb_device_descriptor desc;
 		int r = libusb_get_device_descriptor(dev, &desc);
 		if (r < 0) {
-			fprintf(stderr, "libusb_get_device_descriptor failed %s",
+			DEBUG_WARN("libusb_get_device_descriptor failed %s",
 					libusb_strerror(r));
 			return -1;
 		}
@@ -555,21 +558,21 @@ int stlink_init(bmp_info_t *info)
 	int config;
 	int r = libusb_get_configuration(sl->ul_libusb_device_handle, &config);
 	if (r) {
-		fprintf(stderr, "FATAL: Stlink libusb_get_configuration failed %d: %s",
+		DEBUG_WARN("FATAL: Stlink libusb_get_configuration failed %d: %s",
 				 r, libusb_strerror(r));
 		return -1;
 	}
 	if (config != 1) {
 		r = libusb_set_configuration(sl->ul_libusb_device_handle, 0);
 		if (r) {
-			fprintf(stderr, "FATAL: Stlinklibusb_set_configuration "
+			DEBUG_WARN("FATAL: Stlinklibusb_set_configuration "
 				   "failed %d: %s", r, libusb_strerror(r));
 			return -1;
 		}
 	}
 	r = libusb_claim_interface(sl->ul_libusb_device_handle, 0);
 	if (r) 	{
-		fprintf(stderr, "FATAL: Stlink libusb_claim_interface failed %s\n",
+		DEBUG_WARN("FATAL: Stlink libusb_claim_interface failed %s\n",
 				libusb_strerror(r));
 		return -1;
 	}
@@ -580,24 +583,24 @@ int stlink_init(bmp_info_t *info)
 		(Stlink.ver_stlink == 3 && Stlink.ver_jtag < 3)) {
 		/* Maybe the adapter is in some strange state. Try to reset */
         int result = libusb_reset_device(sl->ul_libusb_device_handle);
-		fprintf(stderr, "INFO: Trying Stlink reset\n");
+		DEBUG_WARN("INFO: Trying Stlink reset\n");
 		if (result == LIBUSB_ERROR_BUSY) { /* Try again */
 			platform_delay(50);
 			result = libusb_reset_device(sl->ul_libusb_device_handle);
 		}
         if (result != LIBUSB_SUCCESS) {
-			fprintf(stderr, "FATAL: Stlink libusb_reset_device failed\n");
+			DEBUG_WARN("FATAL: Stlink libusb_reset_device failed\n");
 			return -1;
 		}
 		stlink_version(info);
     }
 	if ((Stlink.ver_stlink < 3 && Stlink.ver_jtag < 32) ||
 		(Stlink.ver_stlink == 3 && Stlink.ver_jtag < 3)) {
-		printf("Please update Firmware\n");
+		DEBUG_WARN("Please update Firmware\n");
 		return -1;
 	}
 	if (stlink_leave_state(info)) {
-		printf("Stlink board was in DFU mode. Restart\n");
+		DEBUG_WARN("Stlink board was in DFU mode. Restart\n");
 		return -1;
 	}
 	stlink_resetsys(info);
@@ -646,7 +649,7 @@ static bool stlink3_set_freq_divisor(bmp_info_t *info, uint16_t divisor)
 		divisor = size;
 	uint8_t *p = data + 12 + divisor * sizeof(uint32_t);
 	uint32_t freq = p[0] | p[1] << 8 | p[2] << 16 | p[3] << 24;
-	printf("Selected %" PRId32 " khz\n", freq);
+	DEBUG_INFO("Selected %" PRId32 " khz\n", freq);
 	cmd[1] = STLINK_APIV3_SET_COM_FREQ;
 	cmd[2] = Stlink.transport_mode;
 	cmd[3] = 0;
@@ -676,7 +679,7 @@ static int stlink_enter_debug_jtag(bmp_info_t *info)
 					  STLINK_DEBUG_APIV2_ENTER,
 					  STLINK_DEBUG_ENTER_JTAG_NO_RESET};
 	uint8_t data[2];
-	printf("Enter JTAG\n");
+	DEBUG_INFO("Enter JTAG\n");
 	send_recv(info->usb_link, cmd, 16, data, 2);
 	return stlink_usb_error_check(data, true);
 }
@@ -688,7 +691,7 @@ static uint32_t stlink_read_coreid(void)
 	uint8_t data[4];
 	send_recv(info.usb_link, cmd, 16, data, 4);
 	uint32_t id =  data[0] | data[1] << 8 | data[2] << 16 | data[3] << 24;
-	printf("Read Core ID: 0x%08" PRIx32 "\n", id);
+	DEBUG_INFO("Read Core ID: 0x%08" PRIx32 "\n", id);
 	return id;
 }
 
@@ -741,7 +744,7 @@ uint32_t stlink_dp_error(ADIv5_DP_t *dp)
 	adiv5_dp_write(dp, ADIV5_DP_ABORT, clr);
 	dp->fault = 0;
 	if (err)
-		printf("stlink_dp_error %d\n", err);
+		DEBUG_WARN("stlink_dp_error %d\n", err);
 	err |= Stlink.ap_error;
 	Stlink.ap_error = false;
 	return err;
@@ -768,7 +771,7 @@ static int stlink_read_dp_register(uint16_t port, uint16_t addr, uint32_t *reg)
 		uint32_t ret = data[4] | data[5] << 8 | data[6] << 16 | data[7] << 24;
 		*reg = ret;
 	} else {
-		printf("%s error %d\n", __func__, res);
+		DEBUG_WARN("%s error %d\n", __func__, res);
 	}
 	return res;
 }
@@ -777,8 +780,7 @@ static int stlink_write_dp_register(uint16_t port, uint16_t addr, uint32_t val)
 {
 	if (port == STLINK_DEBUG_PORT_ACCESS && addr == 8) {
 		Stlink.dap_select = val;
-		if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-			printf("Caching SELECT 0x%02" PRIx32 "\n", val);
+	  DEBUG_PROBE("Caching SELECT 0x%02" PRIx32 "\n", val);
 		return STLINK_ERROR_OK;
 	} else {
 		uint8_t cmd[16] = {
@@ -828,12 +830,11 @@ static bool stlink_ap_setup(int ap)
 	};
 	uint8_t data[2];
 	send_recv(info.usb_link, cmd, 16, data, 2);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("Open AP %d\n", ap);
+	DEBUG_PROBE("Open AP %d\n", ap);
 	int res = stlink_usb_error_check(data, true);
 	if (res) {
 		if (Stlink.ver_hw == 30) {
-			printf("STLINKV3 only connects to STM8/32!\n");
+			DEBUG_WARN("STLINKV3 only connects to STM8/32!\n");
 		}
 		return false;
 	}
@@ -849,8 +850,7 @@ static void stlink_ap_cleanup(int ap)
        };
        uint8_t data[2];
        send_recv(info.usb_link, cmd, 16, data, 2);
-	   if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		   printf("Close AP %d\n", ap);
+	   DEBUG_PROBE("Close AP %d\n", ap);
        stlink_usb_error_check(data, true);
 }
 static int stlink_usb_get_rw_status(bool verbose)
@@ -873,7 +873,7 @@ static void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 	if (src & 1 || len & 1) {
 		type = STLINK_DEBUG_READMEM_8BIT;
 		if (len > Stlink.block_size) {
-			printf(" Too large!\n");
+			DEBUG_WARN(" Too large!\n");
 			return;
 		}
 		if (len == 1)
@@ -897,10 +897,11 @@ static void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 		 * Approach taken:
 		 * Fill the memory with some fixed pattern so hopefully
 		 * the caller notices the error*/
-		printf("stlink_readmem failed\n");
+		DEBUG_WARN("stlink_readmem failed\n");
 		memset(dest, 0xff, len);
 	}
-	DEBUG_STLINK("\n");
+	DEBUG_PROBE("stlink_readmem from %" PRIx32 " to %" PRIx32 ", len %" PRIx32
+				"\n", src, dest, (uint32_t) len);
 }
 
 static void stlink_writemem8(usb_link_t *link, ADIv5_AP_t *ap, uint32_t addr,
@@ -961,8 +962,7 @@ static void stlink_regs_read(ADIv5_AP_t *ap, void *data)
 	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READALLREGS,
 					   ap->apsel};
 	uint8_t res[88];
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("AP %d: Read all core registers\n", ap->apsel);
+	DEBUG_PROBE("AP %d: Read all core registers\n", ap->apsel);
 	send_recv(info.usb_link, cmd, 16, res, 88);
 	stlink_usb_error_check(res, true);
 	memcpy(data, res + 4, 84);
@@ -976,9 +976,8 @@ static uint32_t stlink_reg_read(ADIv5_AP_t *ap, int num)
 	send_recv(info.usb_link, cmd, 16, res, 8);
 	stlink_usb_error_check(res, true);
 	uint32_t ret = res[0] | res[1] << 8 | res[2] << 16 | res[3] << 24;
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("AP %d: Read reg %02" PRId32 " val 0x%08" PRIx32 "\n",
-			   ap->apsel, num, ret);
+	DEBUG_PROBE("AP %d: Read reg %02" PRId32 " val 0x%08" PRIx32 "\n",
+				 ap->apsel, num, ret);
 	return ret;
 }
 
@@ -990,9 +989,8 @@ static void stlink_reg_write(ADIv5_AP_t *ap, int num, uint32_t val)
 		(val >> 24) & 0xff, ap->apsel};
 	uint8_t res[2];
 	send_recv(info.usb_link, cmd, 16, res, 2);
-	if (cl_debuglevel & BMP_DEBUG_PLATFORM)
-		printf("AP %d: Write reg %02" PRId32 " val 0x%08" PRIx32 "\n",
-			   ap->apsel, num, val);
+	DEBUG_PROBE("AP %d: Write reg %02" PRId32 " val 0x%08" PRIx32 "\n",
+				 ap->apsel, num, val);
 	stlink_usb_error_check(res, true);
 }
 
@@ -1095,7 +1093,7 @@ int stlink_enter_debug_swd(bmp_info_t *info, ADIv5_DP_t *dp)
 					  STLINK_DEBUG_APIV2_ENTER,
 					  STLINK_DEBUG_ENTER_SWD_NO_RESET};
 	uint8_t data[2];
-	printf("Enter SWD\n");
+	DEBUG_INFO("Enter SWD\n");
 	send_recv(info->usb_link, cmd, 16, data, 2);
 	if (stlink_usb_error_check(data, true))
 		return -1;
diff --git a/src/platforms/hosted/stlinkv2.h b/src/platforms/hosted/stlinkv2.h
index 1960a5b..d42ec14 100644
--- a/src/platforms/hosted/stlinkv2.h
+++ b/src/platforms/hosted/stlinkv2.h
@@ -36,7 +36,4 @@ void stlink_adiv5_dp_defaults(ADIv5_DP_t *dp);
 int stlink_jtag_dp_init(ADIv5_DP_t *dp);
 int jtag_scan_stlinkv2(bmp_info_t *info, const uint8_t *irlens);
 void stlink_exit_function(bmp_info_t *info);
-extern  int debug_level;
-# define DEBUG_STLINK if (debug_level > 0) printf
-# define DEBUG_USB    if (debug_level > 1) printf
 #endif
diff --git a/src/platforms/hydrabus/platform.h b/src/platforms/hydrabus/platform.h
index 792cfba..f8c5a09 100644
--- a/src/platforms/hydrabus/platform.h
+++ b/src/platforms/hydrabus/platform.h
@@ -135,8 +135,6 @@
 #define TRACE_IRQ   NVIC_TIM3_IRQ
 #define TRACE_ISR   tim3_isr
 
-#define DEBUG(...)
-
 #define gpio_set_val(port, pin, val) do {	\
 	if(val)					\
 		gpio_set((port), (pin));	\
diff --git a/src/platforms/launchpad-icdi/platform.h b/src/platforms/launchpad-icdi/platform.h
index 7450aa7..d05f61c 100644
--- a/src/platforms/launchpad-icdi/platform.h
+++ b/src/platforms/launchpad-icdi/platform.h
@@ -98,8 +98,6 @@ extern const usbd_driver lm4f_usb_driver;
 #define vasprintf vasiprintf
 #define snprintf sniprintf
 
-#define DEBUG(...)
-
 #define SET_RUN_STATE(state)	{running_status = (state);}
 #define SET_IDLE_STATE(state)	{}
 #define SET_ERROR_STATE(state)	SET_IDLE_STATE(state)
diff --git a/src/platforms/native/platform.h b/src/platforms/native/platform.h
index 00043ac..0799ab4 100644
--- a/src/platforms/native/platform.h
+++ b/src/platforms/native/platform.h
@@ -30,10 +30,14 @@
 
 #define PLATFORM_HAS_TRACESWO
 #define PLATFORM_HAS_POWER_SWITCH
+
 #ifdef ENABLE_DEBUG
-#define PLATFORM_HAS_DEBUG
-#define USBUART_DEBUG
+# define PLATFORM_HAS_DEBUG
+# define USBUART_DEBUG
+extern bool debug_bmp;
+int usbuart_debug_write(const char *buf, size_t len);
 #endif
+
 #define BOARD_IDENT             "Black Magic Probe"
 #define BOARD_IDENT_DFU	        "Black Magic Probe (Upgrade)"
 #define BOARD_IDENT_UPD	        "Black Magic Probe (DFU Upgrade)"
@@ -165,15 +169,6 @@
 #define TRACE_IRQ   NVIC_TIM3_IRQ
 #define TRACE_ISR   tim3_isr
 
-#ifdef ENABLE_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);}
 #define SET_ERROR_STATE(state)	{gpio_set_val(LED_PORT, LED_ERROR, state);}
diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index aedd14d..860572a 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -1,8 +1,8 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2019
- * Written by Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
+ * Copyright (C) 2019 - 2020 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
@@ -63,7 +63,7 @@ static int bmp_mmap(char *file, struct mmap_data *map)
 	map->hFile = CreateFile(file, GENERIC_WRITE | GENERIC_READ, FILE_SHARE_READ,
 							NULL, OPEN_ALWAYS, 0, NULL);
 	if (map->hFile == INVALID_HANDLE_VALUE) {
-		DEBUG("Open file %s failed: %s\n", file, strerror(errno));
+		DEBUG_WARN("Open file %s failed: %s\n", file, strerror(errno));
 		return -1;
 	}
 	map->size = GetFileSize(map->hFile, NULL);
@@ -76,13 +76,13 @@ static int bmp_mmap(char *file, struct mmap_data *map)
 		NULL);             /* name of mapping object */
 
     if (map->hMapFile == NULL || map->hMapFile == INVALID_HANDLE_VALUE) {
-		DEBUG("Map file %s failed: %s\n", file, strerror(errno));
+		DEBUG_WARN("Map file %s failed: %s\n", file, strerror(errno));
 		CloseHandle(map->hFile);
 		return -1;
 	}
 	map->data = MapViewOfFile(map->hMapFile, FILE_MAP_READ, 0, 0, 0);
 	if (!map->data) {
-		printf("Could not create file mapping object (%s).\n",
+		DEBUG_WARN("Could not create file mapping object (%s).\n",
 			   strerror(errno));
         CloseHandle(map->hMapFile);
 		return -1;
@@ -90,7 +90,7 @@ static int bmp_mmap(char *file, struct mmap_data *map)
 #else
 	map->fd = open(file, O_RDONLY | O_BINARY);
 	if (map->fd < 0) {
-		DEBUG("Open file %s failed: %s\n", file, strerror(errno));
+		DEBUG_WARN("Open file %s failed: %s\n", file, strerror(errno));
 		return -1;
 	}
 	struct stat stat;
@@ -115,34 +115,37 @@ static void bmp_munmap(struct mmap_data *map)
 
 static void cl_help(char **argv, BMP_CL_OPTIONS_t *opt)
 {
-	printf("%s\n\n", opt->opt_idstring);
-	printf("Usage: %s [options]\n", argv[0]);
-	printf("\t-h\t\t: This help.\n");
-	printf("\t-v[1|2]\t\t: Increasing verbosity\n");
-	printf("\t-d \"path\"\t: Use serial device at \"path\"\n");
-	printf("\t-P <num>\t: Use debugger found at position <num>\n");
-	printf("\t-n <num>\t: Use target device found at position <num>\n");
-	printf("\t-s \"string\"\t: Use dongle with (partial) "
+	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("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");
+	DEBUG_WARN("\t\t\t  1 = INFO, 2 = GDB, 4 = TARGET, 8 = PROBE, 16 = WIRE\n");
+	DEBUG_WARN("Probe selection arguments:\n");
+	DEBUG_WARN("\t-d \"path\"\t: Use serial device at \"path\"\n");
+	DEBUG_WARN("\t-P <num>\t: Use debugger found at position <num>\n");
+	DEBUG_WARN("\t-n <num>\t: Use target device found at position <num>\n");
+	DEBUG_WARN("\t-s \"string\"\t: Use dongle with (partial) "
 		  "serial number \"string\"\n");
-	printf("\t-c \"string\"\t: Use ftdi dongle with type \"string\"\n");
-	printf("\tRun mode related options:\n");
-	printf("\tDefault mode is to start the debug server at :2000\n");
-	printf("\t-j\t\t: Use JTAG. SWD is default.\n");
-	printf("\t-C\t\t: Connect under reset\n");
-	printf("\t-t\t\t: Scan SWD and display information about connected"
-		   "devices\n");
-	printf("\t-E\t\t: Erase flash until flash end or for given size\n");
-	printf("\t-V\t\t: Verify flash against binary file\n");
-	printf("\t-r\t\t: Read flash and write to binary file\n");
-	printf("\t-p\t\t: Supplies power to the target (where applicable)\n");
-	printf("\t-R\t\t: Reset device\n");
-	printf("\tFlash operation modifiers options:\n");
-	printf("\t-a <num>\t: Start flash operation at flash address <num>\n"
+	DEBUG_WARN("\t-c \"string\"\t: Use ftdi dongle with type \"string\"\n");
+	DEBUG_WARN("Run mode related options:\n");
+	DEBUG_WARN("\tDefault mode is to start the debug server at :2000\n");
+	DEBUG_WARN("\t-j\t\t: Use JTAG. SWD is default.\n");
+	DEBUG_WARN("\t-C\t\t: Connect under reset\n");
+	DEBUG_WARN("\t-t\t\t: Scan SWD or JTAG and display information about \n"
+			   "\t\t\t  connected devices\n");
+	DEBUG_WARN("\t-E\t\t: Erase flash until flash end or for given size\n");
+	DEBUG_WARN("\t-V\t\t: Verify flash against binary file\n");
+	DEBUG_WARN("\t-r\t\t: Read flash and write to binary file\n");
+	DEBUG_WARN("\t-p\t\t: Supplies power to the target (where applicable)\n");
+	DEBUG_WARN("\t-R\t\t: Reset device\n");
+	DEBUG_WARN("Flash operation modifiers options:\n");
+	DEBUG_WARN("\tDefault action with given file is to write to flash\n");
+	DEBUG_WARN("\t-a <addr>\t: Start flash operation at flash address <addr>\n"
 		"\t\t\t  Default start is 0x08000000\n");
-	printf("\t-S <num>\t: Read <num> bytes. Default is until read fails.\n");
-	printf("\t <file>\t\t: Use (binary) file <file> for flash operation\n"
-		   "\t\t\t  Given <file> writes to flash if neither -r or -V is "
-		   "given\n");
+	DEBUG_WARN("\t-S <num>\t: Read <num> bytes. Default is until read fails.\n");
+	DEBUG_WARN("\t <file>\t\t: Use (binary) file <file> for flash operation\n");
 	exit(0);
 }
 
@@ -152,7 +155,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 	opt->opt_target_dev = 1;
 	opt->opt_flash_start = 0x08000000;
 	opt->opt_flash_size = 16 * 1024 *1024;
-	while((c = getopt(argc, argv, "Ehv::d:s:I:c:Cn:tVta:S:jpP:rR")) != -1) {
+	while((c = getopt(argc, argv, "Ehv:d:s:I:c:Cn:tVta:S:jpP:rR")) != -1) {
 		switch(c) {
 		case 'c':
 			if (optarg)
@@ -163,9 +166,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 			break;
 		case 'v':
 			if (optarg)
-				cl_debuglevel = strtol(optarg, NULL, 0);
-			else
-				cl_debuglevel = 1;
+				cl_debuglevel = strtol(optarg, NULL, 0) & (BMP_DEBUG_MAX - 1);
 			break;
 		case 'j':
 			opt->opt_usejtag = true;
@@ -190,6 +191,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 			break;
 		case 't':
 			opt->opt_mode = BMP_MODE_TEST;
+			cl_debuglevel |= BMP_DEBUG_INFO | BMP_DEBUG_STDOUT;
 			break;
 		case 'V':
 			opt->opt_mode = BMP_MODE_FLASH_VERIFY;
@@ -242,7 +244,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 	/* Checks */
 	if ((opt->opt_flash_file) && ((opt->opt_mode == BMP_MODE_TEST ) ||
 								  (opt->opt_mode == BMP_MODE_RESET))) {
-		printf("Ignoring filename in reset/test mode\n");
+		DEBUG_WARN("Ignoring filename in reset/test mode\n");
 		opt->opt_flash_file = NULL;
 	}
 }
@@ -250,7 +252,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 static void display_target(int i, target *t, void *context)
 {
 	(void)context;
-	DEBUG("*** %2d   %c  %s %s\n", i, target_attached(t)?'*':' ',
+	DEBUG_INFO("*** %2d   %c  %s %s\n", i, target_attached(t)?'*':' ',
 		  target_driver_name(t),
 		  (target_core_name(t)) ? target_core_name(t): "");
 }
@@ -261,37 +263,38 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 	int num_targets;
 #if defined(PLATFORM_HAS_POWER_SWITCH)
 	if (opt->opt_tpwr) {
-		printf("Powering up device");
+		DEBUG_INFO("Powering up device");
 		platform_target_set_power(true);
 		platform_delay(500);
 	}
 #endif
 	if (opt->opt_connect_under_reset)
-		printf("Connecting under reset\n");
+		DEBUG_INFO("Connecting under reset\n");
 	connect_assert_srst = opt->opt_connect_under_reset;
 	platform_srst_set_val(opt->opt_connect_under_reset);
 	if (opt->opt_mode == BMP_MODE_TEST)
-		printf("Running in Test Mode\n");
+		DEBUG_INFO("Running in Test Mode\n");
 	if (platform_target_voltage())
-		printf("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 {
 		num_targets = platform_adiv5_swdp_scan();
 	}
 	if (!num_targets) {
-		DEBUG("No target found\n");
+		DEBUG_WARN("No target found\n");
 		return res;
 	} else {
 		target_foreach(display_target, NULL);
 	}
 	if (opt->opt_target_dev > num_targets) {
-		DEBUG("Given target nummer %d not available\n", opt->opt_target_dev);
+		DEBUG_WARN("Given target nummer %d not available\n",
+				   opt->opt_target_dev);
 		return res;
 	}
 	target *t = target_attach_n(opt->opt_target_dev, NULL);
 	if (!t) {
-		DEBUG("Can not attach to target %d\n", opt->opt_target_dev);
+		DEBUG_WARN("Can not attach to target %d\n", opt->opt_target_dev);
 		goto target_detach;
 	}
 	if (opt->opt_mode == BMP_MODE_TEST) {
@@ -312,8 +315,8 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 					if (sscanf(p, "<memory type=\"flash\" start=\"%x\" length=\"%x\">"
 							  "<property name=\"blocksize\">%x</property></memory>",
 							  &start, &size, &blocksize))
-						printf("Flash Start: 0x%08x, length %#9x, blocksize %#8x\n",
-							   start, size, blocksize);
+						DEBUG_INFO("Flash Start: 0x%08x, length %#9x, "
+								   "blocksize %#8x\n", start, size, blocksize);
 					res = strstr(p, "</memory>");
 					p = res + strlen("</memory>");
 					continue;
@@ -322,7 +325,8 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 				if (!match) {
 					if (sscanf(p, "<memory type=\"ram\" start=\"%x\" length=\"%x\"/",
 							   &start, &size))
-						printf("Ram   Start: 0x%08x, length %#9x\n", start, size);
+						DEBUG_INFO("Ram   Start: 0x%08x, length %#9x\n",
+								   start, size);
 					res = strstr(p, "/>");
 					p = res + strlen("/>");
 					continue;
@@ -337,7 +341,7 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 		(opt->opt_mode == BMP_MODE_FLASH_VERIFY)) {
 		int mmap_res = bmp_mmap(opt->opt_flash_file, &map);
 		if (mmap_res) {
-			DEBUG("Can not map file: %s. Aborting!\n", strerror(errno));
+			DEBUG_WARN("Can not map file: %s. Aborting!\n", strerror(errno));
 			goto target_detach;
 		}
 	} else if (opt->opt_mode == BMP_MODE_FLASH_READ) {
@@ -345,8 +349,8 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 		read_file = open(opt->opt_flash_file, O_TRUNC | O_CREAT | O_RDWR | O_BINARY,
 						 S_IRUSR | S_IWUSR);
 		if (read_file == -1) {
-			printf("Error opening flashfile %s for read: %s\n",
-				   opt->opt_flash_file, strerror(errno));
+			DEBUG_WARN("Error opening flashfile %s for read: %s\n",
+					   opt->opt_flash_file, strerror(errno));
 			return res;
 		}
 	}
@@ -356,51 +360,52 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 	if (opt->opt_mode == BMP_MODE_RESET) {
 		target_reset(t);
 	} else if (opt->opt_mode == BMP_MODE_FLASH_ERASE) {
-		DEBUG("Erase %zu bytes at 0x%08" PRIx32 "\n", opt->opt_flash_size,
+		DEBUG_INFO("Erase %zu bytes at 0x%08" PRIx32 "\n", opt->opt_flash_size,
 			  opt->opt_flash_start);
 		unsigned int erased = target_flash_erase(t, opt->opt_flash_start,
 												 opt->opt_flash_size);
 		if (erased) {
-			DEBUG("Erased failed!\n");
+			DEBUG_WARN("Erased failed!\n");
 			goto free_map;
 		}
 		target_reset(t);
 	} else if (opt->opt_mode == BMP_MODE_FLASH_WRITE) {
-		DEBUG("Erase    %zu bytes at 0x%08" PRIx32 "\n", map.size,
+		DEBUG_INFO("Erase    %zu bytes at 0x%08" PRIx32 "\n", map.size,
 			  opt->opt_flash_start);
 		uint32_t start_time = platform_time_ms();
 		unsigned int erased = target_flash_erase(t, opt->opt_flash_start,
 												 map.size);
 		if (erased) {
-			DEBUG("Erased failed!\n");
+			DEBUG_WARN("Erased failed!\n");
 			goto free_map;
 		} else {
-			DEBUG("Flashing %zu bytes at 0x%08" PRIx32 "\n",
+			DEBUG_INFO("Flashing %zu bytes at 0x%08" PRIx32 "\n",
 				  map.size, opt->opt_flash_start);
 			unsigned int flashed = target_flash_write(t, opt->opt_flash_start,
 													  map.data, map.size);
 			/* Buffered write cares for padding*/
 			if (flashed) {
-				DEBUG("Flashing failed!\n");
+				DEBUG_WARN("Flashing failed!\n");
 			} else {
-				DEBUG("Success!\n");
+				DEBUG_INFO("Success!\n");
 				res = 0;
 			}
 		}
 		target_flash_done(t);
 		target_reset(t);
 		uint32_t end_time = platform_time_ms();
-		printf("Flash Write succeeded for %d bytes, %8.3f kiB/s\n",
+		DEBUG_WARN("Flash Write succeeded for %d bytes, %8.3f kiB/s\n",
 			   (int)map.size, (((map.size * 1.0)/(end_time - start_time))));
 	} else {
 #define WORKSIZE 1024
 		uint8_t *data = alloca(WORKSIZE);
 		if (!data) {
-			printf("Can not malloc memory for flash read/verify operation\n");
+			DEBUG_WARN("Can not malloc memory for flash read/verify "
+					   "operation\n");
 			return res;
 		}
 		if (opt->opt_mode == BMP_MODE_FLASH_READ)
-			printf("Reading flash from 0x%08" PRIx32 " for %zu"
+			DEBUG_INFO("Reading flash from 0x%08" PRIx32 " for %zu"
 				   " bytes to %s\n", opt->opt_flash_start,  opt->opt_flash_size,
 				   opt->opt_flash_file);
 		uint32_t flash_src = opt->opt_flash_start;
@@ -414,11 +419,11 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 			int n_read = target_mem_read(t, data, flash_src, worksize);
 			if (n_read) {
 				if (opt->opt_flash_size == 0) {/* we reached end of flash */
-					printf("Reached end of flash at size %" PRId32 "\n",
+					DEBUG_INFO("Reached end of flash at size %" PRId32 "\n",
 						   flash_src - opt->opt_flash_start);
 					break;
 				} else {
-					printf("Read failed at flash address 0x%08" PRIx32 "\n",
+					DEBUG_WARN("Read failed at flash address 0x%08" PRIx32 "\n",
 						   flash_src);
 					break;
 				}
@@ -428,15 +433,15 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 			if (opt->opt_mode == BMP_MODE_FLASH_VERIFY) {
 				int difference = memcmp(data, flash, worksize);
 				if (difference){
-					printf("Verify failed at flash region 0x%08" PRIx32 "\n",
-						   flash_src);
+					DEBUG_WARN("Verify failed at flash region 0x%08"
+							   PRIx32 "\n", flash_src);
 					return -1;
 				}
 				flash += worksize;
 			} else if (read_file != -1) {
 				int written = write(read_file, data, worksize);
 				if (written < worksize) {
-					printf("Read failed at flash region 0x%08" PRIx32 "\n",
+					DEBUG_WARN("Read failed at flash region 0x%08" PRIx32 "\n",
 						   flash_src);
 					return -1;
 				}
@@ -449,7 +454,7 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 		uint32_t end_time = platform_time_ms();
 		if (read_file != -1)
 			close(read_file);
-		printf("Read/Verified succeeded for %d bytes, %8.3f kiB/s\n",
+		DEBUG_WARN("Read/Verify succeeded for %d bytes, %8.3f kiB/s\n",
 			   bytes_read, (((bytes_read * 1.0)/(end_time - start_time))));
 	}
   free_map:
diff --git a/src/platforms/pc/cl_utils.h b/src/platforms/pc/cl_utils.h
index b65d0e6..6213e08 100644
--- a/src/platforms/pc/cl_utils.h
+++ b/src/platforms/pc/cl_utils.h
@@ -1,7 +1,7 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2019  Uwe Bonnes
+ * Copyright (C) 2019 - 2020  Uwe Bonnes
  * Written by Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
  *
  * This program is free software: you can redistribute it and/or modify
@@ -36,13 +36,6 @@ enum bmp_cl_mode {
 	BMP_MODE_FLASH_VERIFY
 };
 
-enum BMP_DEBUG {
-	BMP_DEBUG_NONE = 0,
-	BMP_DEBUG_INFO = 1,
-	BMP_DEBUG_PLATFORM = 2,
-	BMP_DEBUG_WIRE = 4
-};
-
 typedef struct BMP_CL_OPTIONS_s {
 	enum bmp_cl_mode opt_mode;
 	bool opt_usejtag;
@@ -61,7 +54,6 @@ typedef struct BMP_CL_OPTIONS_s {
 	char     *opt_idstring;
 }BMP_CL_OPTIONS_t;
 
-extern int cl_debuglevel;
 void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv);
 int cl_execute(BMP_CL_OPTIONS_t *opt);
 int serial_open(BMP_CL_OPTIONS_t *opt, char *serial);
diff --git a/src/platforms/pc/gdb_if.c b/src/platforms/pc/gdb_if.c
index 3199465..2657d84 100644
--- a/src/platforms/pc/gdb_if.c
+++ b/src/platforms/pc/gdb_if.c
@@ -89,7 +89,7 @@ int gdb_if_init(void)
 		}
 		break;
 	} while(1);
-	DEBUG("Listening on TCP: %4d\n", port);
+	DEBUG_WARN("Listening on TCP: %4d\n", port);
 
 	return 0;
 }
@@ -125,9 +125,11 @@ unsigned char gdb_if_getchar(void)
 						platform_delay(100);
 					} else {
 #if defined(_WIN32) || defined(__CYGWIN__)
-						DEBUG("error when accepting connection: %d", WSAGetLastError());
+						DEBUG_WARN("error when accepting connection: %d",
+								   WSAGetLastError());
 #else
-						DEBUG("error when accepting connection: %s", strerror(errno));
+						DEBUG_WARN("error when accepting connection: %s",
+								   strerror(errno));
 #endif
 						exit(1);
 					}
@@ -141,7 +143,7 @@ unsigned char gdb_if_getchar(void)
 					break;
 				}
 			}
-			DEBUG("Got connection\n");
+			DEBUG_INFO("Got connection\n");
 #if defined(_WIN32) || defined(__CYGWIN__)
 			opt = 0;
 			ioctlsocket(gdb_if_conn, FIONBIO, &opt);
@@ -154,9 +156,9 @@ unsigned char gdb_if_getchar(void)
 		if(i <= 0) {
 			gdb_if_conn = -1;
 #if defined(_WIN32) || defined(__CYGWIN__)
-			DEBUG("Dropped broken connection: %d\n", WSAGetLastError());
+			DEBUG_INFO("Dropped broken connection: %d\n", WSAGetLastError());
 #else
-			DEBUG("Dropped broken connection: %s\n", strerror(errno));
+			DEBUG_INFO("Dropped broken connection: %s\n", strerror(errno));
 #endif
 			/* Return '+' in case we were waiting for an ACK */
 			return '+';
diff --git a/src/platforms/pc/libusb_utils.c b/src/platforms/pc/libusb_utils.c
index 1e090d5..d7f3159 100644
--- a/src/platforms/pc/libusb_utils.c
+++ b/src/platforms/pc/libusb_utils.c
@@ -25,15 +25,15 @@ static void LIBUSB_CALL on_trans_done(struct libusb_transfer *trans)
 
     if (trans->status != LIBUSB_TRANSFER_COMPLETED)
     {
-		fprintf(stderr, "on_trans_done: ");
+		DEBUG_WARN("on_trans_done: ");
         if(trans->status == LIBUSB_TRANSFER_TIMED_OUT)  {
-            fprintf(stderr, " Timeout\n");
+            DEBUG_WARN(" Timeout\n");
         } else if (trans->status == LIBUSB_TRANSFER_CANCELLED) {
-            fprintf(stderr, " cancelled\n");
+            DEBUG_WARN(" cancelled\n");
         } else if (trans->status == LIBUSB_TRANSFER_NO_DEVICE) {
-            fprintf(stderr, " no device\n");
+            DEBUG_WARN(" no device\n");
         } else {
-            fprintf(stderr, " unknown\n");
+            DEBUG_WARN(" unknown\n");
 		}
         ctx->flags |= TRANS_FLAGS_HAS_ERROR;
     }
@@ -51,7 +51,7 @@ static int submit_wait(usb_link_t *link, struct libusb_transfer *trans) {
 	trans->user_data = &trans_ctx;
 
 	if ((error = libusb_submit_transfer(trans))) {
-		fprintf(stderr, "libusb_submit_transfer(%d): %s\n", error,
+		DEBUG_WARN("libusb_submit_transfer(%d): %s\n", error,
 			  libusb_strerror(error));
 		exit(-1);
 	}
@@ -62,18 +62,18 @@ static int submit_wait(usb_link_t *link, struct libusb_transfer *trans) {
 		timeout.tv_sec = 1;
 		timeout.tv_usec = 0;
 		if (libusb_handle_events_timeout(link->ul_libusb_ctx, &timeout)) {
-			fprintf(stderr, "libusb_handle_events()\n");
+			DEBUG_WARN("libusb_handle_events()\n");
 			return -1;
 		}
 		uint32_t now = platform_time_ms();
 		if (now - start_time > 1000) {
 			libusb_cancel_transfer(trans);
-			fprintf(stderr, "libusb_handle_events() timeout\n");
+			DEBUG_WARN("libusb_handle_events() timeout\n");
 			return -1;
 		}
 	}
 	if (trans_ctx.flags & TRANS_FLAGS_HAS_ERROR) {
-		fprintf(stderr, "libusb_handle_events() | has_error\n");
+		DEBUG_WARN("libusb_handle_events() | has_error\n");
 		return -1;
 	}
 
@@ -93,19 +93,17 @@ int send_recv(usb_link_t *link,
 								  link->ep_tx | LIBUSB_ENDPOINT_OUT,
 								  txbuf, txlen,
 								  NULL, NULL, 0);
-		if (cl_debuglevel & BMP_DEBUG_WIRE) {
-			int i = 0;
-			printf(" Send (%3d): ", txlen);
-			for (; i < txlen; i++) {
-				printf("%02x", txbuf[i]);
-				if ((i & 7) == 7)
-					printf(".");
-				if ((i & 31) == 31)
-					printf("\n             ");
-			}
-			if (!(i & 31))
-				printf("\n");
+		int i = 0;
+		DEBUG_WIRE(" Send (%3d): ", txlen);
+		for (; i < txlen; i++) {
+			DEBUG_WIRE("%02x", txbuf[i]);
+			if ((i & 7) == 7)
+				DEBUG_WIRE(".");
+			if ((i & 31) == 31)
+				DEBUG_WIRE("\n             ");
 		}
+		if (!(i & 31))
+			DEBUG_WIRE("\n");
 		if (submit_wait(link, link->req_trans)) {
 			libusb_clear_halt(link->ul_libusb_device_handle, link->ep_tx);
 			return -1;
@@ -119,7 +117,7 @@ int send_recv(usb_link_t *link,
 								  rxbuf, rxsize, NULL, NULL, 0);
 
 		if (submit_wait(link, link->rep_trans)) {
-			DEBUG("clear 1\n");
+			DEBUG_WARN("clear 1\n");
 			libusb_clear_halt(link->ul_libusb_device_handle, link->ep_rx);
 			return -1;
 		}
@@ -127,17 +125,14 @@ int send_recv(usb_link_t *link,
 		if (res >0) {
 			int i;
 			uint8_t *p = rxbuf;
-			if (cl_debuglevel & BMP_DEBUG_WIRE) {
-				printf(" Rec (%zu/%d)", rxsize, res);
-				for (i = 0; i < res && i < 32 ; i++) {
-					if ( i && ((i & 7) == 0))
-						printf(".");
-					printf("%02x", p[i]);
-				}
+			DEBUG_WIRE(" Rec (%zu/%d)", rxsize, res);
+			for (i = 0; i < res && i < 32 ; i++) {
+				if ( i && ((i & 7) == 0))
+					DEBUG_WIRE(".");
+				DEBUG_WIRE("%02x", p[i]);
 			}
 		}
 	}
-	if (cl_debuglevel & BMP_DEBUG_WIRE)
-		printf("\n");
+	DEBUG_WIRE("\n");
 	return res;
 }
diff --git a/src/platforms/pc/serial_unix.c b/src/platforms/pc/serial_unix.c
index 0135956..063ac85 100644
--- a/src/platforms/pc/serial_unix.c
+++ b/src/platforms/pc/serial_unix.c
@@ -2,7 +2,7 @@
  * This file is part of the Black Magic Debug project.
  *
  * Copyright (C) 2019  Dave Marples <dave@marples.net>
- * with additions from Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
+ * Modifications (C) 2020 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
@@ -40,7 +40,7 @@ static int set_interface_attribs(void)
 	struct termios tty;
 	memset (&tty, 0, sizeof tty);
 	if (tcgetattr (fd, &tty) != 0) {
-      printf("error %d from tcgetattr", errno);
+      DEBUG_WARN("error %d from tcgetattr", errno);
       return -1;
     }
 
@@ -62,7 +62,7 @@ static int set_interface_attribs(void)
 	tty.c_cflag &= ~CRTSCTS;
 
 	if (tcsetattr (fd, TCSANOW, &tty) != 0) {
-		printf("error %d from tcsetattr", errno);
+		DEBUG_WARN("error %d from tcsetattr", errno);
 		return -1;
     }
 	return 0;
@@ -77,7 +77,7 @@ int serial_open(BMP_CL_OPTIONS_t *cl_opts, char *serial)
 		struct dirent *dp;
 		DIR *dir = opendir(DEVICE_BY_ID);
 		if (!dir) {
-			printf("No serial device found\n");
+			DEBUG_WARN("No serial device found\n");
 			return -1;
 		}
 		int num_devices = 0;
@@ -95,24 +95,25 @@ int serial_open(BMP_CL_OPTIONS_t *cl_opts, char *serial)
 		}
 		closedir(dir);
 		if ((num_devices == 0) && (num_total == 0)){
-			printf("No BMP probe found\n");
+			DEBUG_WARN("No BMP probe found\n");
 			return -1;
 		} else if (num_devices != 1) {
-			printf("Available Probes:\n");
+			DEBUG_INFO("Available Probes:\n");
 			dir = opendir(DEVICE_BY_ID);
 			if (dir) {
 				while ((dp = readdir(dir)) != NULL) {
 					if ((strstr(dp->d_name, BMP_IDSTRING)) &&
 						(strstr(dp->d_name, "-if00")))
-						printf("%s\n", dp->d_name);
+						DEBUG_WARN("%s\n", dp->d_name);
 				}
 				closedir(dir);
 				if (serial)
-					printf("Do no match given serial \"%s\"\n", serial);
+					DEBUG_WARN("Do no match given serial \"%s\"\n", serial);
 				else
-					printf("Select Probe with -s <(Partial) Serial Number\n");
+					DEBUG_WARN("Select Probe with -s <(Partial) Serial "
+							   "Number\n");
 			} else {
-				printf("Could not opendir %s: %s\n", name, strerror(errno));
+				DEBUG_WARN("Could not opendir %s: %s\n", name, strerror(errno));
 			}
 			return -1;
 		}
@@ -121,7 +122,7 @@ int serial_open(BMP_CL_OPTIONS_t *cl_opts, char *serial)
 	}
 	fd = open(name, O_RDWR | O_SYNC | O_NOCTTY);
 	if (fd < 0) {
-		printf("Couldn't open serial port %s\n", name);
+		DEBUG_WARN("Couldn't open serial port %s\n", name);
 		return -1;
     }
 	/* BMP only offers an USB-Serial connection with no real serial
@@ -139,11 +140,10 @@ int platform_buffer_write(const uint8_t *data, int size)
 {
 	int s;
 
-	if (cl_debuglevel &  BMP_DEBUG_WIRE)
-		printf("%s\n", data);
+	DEBUG_WIRE("%s\n", data);
 	s = write(fd, data, size);
 	if (s < 0) {
-		printf("Failed to write\n");
+		DEBUG_WARN("Failed to write\n");
 		return(-2);
     }
 
@@ -170,11 +170,11 @@ int platform_buffer_read(uint8_t *data, int maxsize)
 
 		ret = select(fd + 1, &rset, NULL, NULL, &tv);
 		if (ret < 0) {
-			printf("Failed on select\n");
+			DEBUG_WARN("Failed on select\n");
 			return(-3);
 		}
 		if(ret == 0) {
-			printf("Timeout on read RESP\n");
+			DEBUG_WARN("Timeout on read RESP\n");
 			return(-4);
 		}
 
@@ -187,25 +187,24 @@ int platform_buffer_read(uint8_t *data, int maxsize)
 		FD_SET(fd, &rset);
 		ret = select(fd + 1, &rset, NULL, NULL, &tv);
 		if (ret < 0) {
-			printf("Failed on select\n");
+			DEBUG_WARN("Failed on select\n");
 			exit(-4);
 		}
 		if(ret == 0) {
-			printf("Timeout on read\n");
+			DEBUG_WARN("Timeout on read\n");
 			return(-5);
 		}
 		s = read(fd, c, 1);
 		if (*c==REMOTE_EOM) {
 			*c = 0;
-			if (cl_debuglevel &  BMP_DEBUG_WIRE)
-				printf("       %s\n",data);
+			DEBUG_WIRE("       %s\n",data);
 			return (c - data);
 		} else {
 			c++;
 		}
 	}while ((s >= 0) && ((c - data) < maxsize));
 
-	printf("Failed to read\n");
+	DEBUG_WARN("Failed to read\n");
 	return(-6);
 	return 0;
 }
diff --git a/src/platforms/pc/serial_win.c b/src/platforms/pc/serial_win.c
index 3baec32..2424d46 100644
--- a/src/platforms/pc/serial_win.c
+++ b/src/platforms/pc/serial_win.c
@@ -29,7 +29,7 @@ int serial_open(BMP_CL_OPTIONS_t *cl_opts, char * serial)
 {
 	(void) serial; /* FIXME: Does Windows allow open with USB serial no? */
 	if (!cl_opts->opt_device) {
-		fprintf(stderr,"Specify the serial device to use!\n");
+		DEBUG_WARN("Specify the serial device to use!\n");
 		return -1;
 	}
 	char device[256];
@@ -48,19 +48,19 @@ int serial_open(BMP_CL_OPTIONS_t *cl_opts, char * serial)
                       0,            // Non Overlapped I/O
                       NULL);        // Null for Comm Devices}
 	if (hComm == INVALID_HANDLE_VALUE) {
-		fprintf(stderr, "Could not open %s: %ld\n", device,
+		DEBUG_WARN("Could not open %s: %ld\n", device,
 				GetLastError());
 		return -1;
 	}
 	DCB dcbSerialParams;
 	dcbSerialParams.DCBlength = sizeof(dcbSerialParams);
 	if (!GetCommState(hComm, &dcbSerialParams)) {
-		fprintf(stderr, "GetCommState failed %ld\n", GetLastError());
+		DEBUG_WARN("GetCommState failed %ld\n", GetLastError());
 		return -1;
 	}
 	dcbSerialParams.ByteSize = 8;
 	if (!SetCommState(hComm, &dcbSerialParams)) {
-		fprintf(stderr, "SetCommState failed %ld\n", GetLastError());
+		DEBUG_WARN("SetCommState failed %ld\n", GetLastError());
 		return -1;
 	}
 	COMMTIMEOUTS timeouts = {0};
@@ -70,7 +70,7 @@ int serial_open(BMP_CL_OPTIONS_t *cl_opts, char * serial)
 	timeouts.WriteTotalTimeoutConstant   = 10;
 	timeouts.WriteTotalTimeoutMultiplier = 10;
 	if (!SetCommTimeouts(hComm, &timeouts)) {
-		fprintf(stderr, "SetCommTimeouts failed %ld\n", GetLastError());
+		DEBUG_WARN("SetCommTimeouts failed %ld\n", GetLastError());
 		return -1;
 	}
 	return 0;
@@ -83,14 +83,13 @@ void serial_close(void)
 
 int platform_buffer_write(const uint8_t *data, int size)
 {
-	if (cl_debuglevel)
-		printf("%s\n",data);
+	DEBUG_WIRE("%s\n",data);
 	int s = 0;
 
 	do {
 		DWORD written;
 		if (!WriteFile(hComm, data + s, size - s, &written, NULL)) {
-			fprintf(stderr, "Serial write failed %ld, written %d\n",
+			DEBUG_WARN("Serial write failed %ld, written %d\n",
 					GetLastError(), s);
 			return -1;
 		}
@@ -106,34 +105,32 @@ int platform_buffer_read(uint8_t *data, int maxsize)
 	uint32_t endTime = platform_time_ms() + RESP_TIMEOUT;
 	do {
 		if (!ReadFile(hComm, &response, 1, &s, NULL)) {
-			fprintf(stderr,"ERROR on read RESP\n");
+			DEBUG_WARN("ERROR on read RESP\n");
 			exit(-3);
 		}
 		if (platform_time_ms() > endTime) {
-			fprintf(stderr,"Timeout on read RESP\n");
+			DEBUG_WARN("Timeout on read RESP\n");
 			exit(-4);
 		}
 	} while (response != REMOTE_RESP);
 	uint8_t *c = data;
 	do {
 		if (!ReadFile(hComm, c, 1, &s, NULL)) {
-			fprintf(stderr,"Error on read\n");
+			DEBUG_WARN("Error on read\n");
 			exit(-3);
 		}
 		if (s > 0 ) {
-			if (cl_debuglevel)
-				printf("%c", *c);
+			DEBUG_WIRE("%c", *c);
 			if (*c == REMOTE_EOM) {
 				*c = 0;
-				if (cl_debuglevel)
-					printf("\n");
+				DEBUG_WIRE("\n");
 				return (c - data);
 			} else {
 				c++;
 			}
 		}
 	} while (((c - data) < maxsize) && (platform_time_ms() < endTime));
-	fprintf(stderr,"Failed to read EOM at %d\n",
+	DEBUG_WARN("Failed to read EOM at %d\n",
 			platform_time_ms() - startTime);
 	exit(-3);
 	return 0;
diff --git a/src/platforms/stlink/platform.h b/src/platforms/stlink/platform.h
index 1b57223..280d0aa 100644
--- a/src/platforms/stlink/platform.h
+++ b/src/platforms/stlink/platform.h
@@ -36,6 +36,8 @@
 #ifdef ENABLE_DEBUG
 # define PLATFORM_HAS_DEBUG
 # define USBUART_DEBUG
+extern bool debug_bmp;
+int usbuart_debug_write(const char *buf, size_t len);
 #endif
 
 #define BOARD_IDENT       "Black Magic Probe (STLINK), (Firmware " FIRMWARE_VERSION ")"
@@ -118,14 +120,6 @@
 #define USBUSART_TIM_IRQ NVIC_TIM4_IRQ
 #define USBUSART_TIM_ISR tim4_isr
 
-#ifdef ENABLE_DEBUG
-extern bool debug_bmp;
-int usbuart_debug_write(const char *buf, size_t len);
-# define DEBUG printf
-#else
-# define DEBUG(...)
-#endif
-
 /* On F103, only USART1 is on AHB2 and can reach 4.5 MBaud at 72 MHz.*/
 #define SWO_UART				USART1
 #define SWO_UART_DR				USART1_DR
diff --git a/src/platforms/swlink/platform.h b/src/platforms/swlink/platform.h
index 74f1d43..50baf05 100644
--- a/src/platforms/swlink/platform.h
+++ b/src/platforms/swlink/platform.h
@@ -33,6 +33,8 @@
 #ifdef ENABLE_DEBUG
 # define PLATFORM_HAS_DEBUG
 # define USBUART_DEBUG
+extern bool debug_bmp;
+int usbuart_debug_write(const char *buf, size_t len);
 #endif
 
 #define BOARD_IDENT			"Black Magic Probe (SWLINK), (Firmware " FIRMWARE_VERSION ")"
@@ -122,14 +124,6 @@
 #define TRACE_IC_IN TIM_IC_IN_TI2
 #define TRACE_TRIG_IN TIM_SMCR_TS_IT1FP2
 
-#ifdef ENABLE_DEBUG
-extern bool debug_bmp;
-int usbuart_debug_write(const char *buf, size_t len);
-# define DEBUG printf
-#else
-# define DEBUG(...)
-#endif
-
 /* On F103, only USART1 is on AHB2 and can reach 4.5 MBaud at 72 MHz.
  * USART1 is already used. sp maximum speed is 2.25 MBaud. */
 #define SWO_UART				USART2
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 2dc0a8d..8b04f01 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -3,6 +3,8 @@
  *
  * Copyright (C) 2015  Black Sphere Technologies Ltd.
  * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ * Copyright (C) 2018 - 2020 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
@@ -280,13 +282,13 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 #endif
 
 	if (adiv5_dp_error(ap->dp)) {
-		DEBUG("%sFault reading ID registers\n", indent);
+		DEBUG_WARN("%sFault reading ID registers\n", indent);
 		return false;
 	}
 
 	/* CIDR preamble sanity check */
 	if ((cidr & ~CID_CLASS_MASK) != CID_PREAMBLE) {
-		DEBUG("%s%d 0x%08" PRIx32": 0x%08" PRIx32
+		DEBUG_WARN("%s%d 0x%08" PRIx32": 0x%08" PRIx32
 			  " <- does not match preamble (0x%X)\n",
 			  indent + 1, num_entry, addr, cidr, CID_PREAMBLE);
 		return false;
@@ -303,19 +305,19 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 			ADIV5_ROM_MEMTYPE_SYSMEM;
 
 		if (adiv5_dp_error(ap->dp)) {
-			DEBUG("Fault reading ROM table entry\n");
+			DEBUG_WARN("Fault reading ROM table entry\n");
 		}
 
-		DEBUG("ROM: Table BASE=0x%" PRIx32 " SYSMEM=0x%" PRIx32 ", PIDR 0x%02"
-			  PRIx32 "%08" PRIx32 "\n", addr, memtype, (uint32_t)(pidr >> 32),
-			  (uint32_t)pidr);
+		DEBUG_INFO("ROM: Table BASE=0x%" PRIx32 " SYSMEM=0x%" PRIx32
+				   ", PIDR 0x%02" PRIx32 "%08" PRIx32 "\n", addr,
+				   memtype, (uint32_t)(pidr >> 32), (uint32_t)pidr);
 #endif
 
 		for (int i = 0; i < 960; i++) {
 			adiv5_dp_error(ap->dp);
 			uint32_t entry = adiv5_mem_read32(ap, addr + i*4);
 			if (adiv5_dp_error(ap->dp)) {
-				DEBUG("%sFault reading ROM table entry %d\n", indent, i);
+				DEBUG_WARN("%sFault reading ROM table entry %d\n", indent, i);
 				break;
 			}
 
@@ -323,7 +325,7 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 				break;
 
 			if (!(entry & ADIV5_ROM_ROMENTRY_PRESENT)) {
-				DEBUG("%s%d Entry 0x%" PRIx32 " -> Not present\n", indent,
+				DEBUG_INFO("%s%d Entry 0x%" PRIx32 " -> Not present\n", indent,
 					  i, entry);
 				continue;
 			}
@@ -333,13 +335,13 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 				ap, addr + (entry & ADIV5_ROM_ROMENTRY_OFFSET),
 				recursion + 1, i);
 		}
-		DEBUG("%sROM: Table END\n", indent);
+		DEBUG_INFO("%sROM: Table END\n", indent);
 	} else {
 		/* Check if the component was designed by ARM, we currently do not support,
 		 * any components by other designers.
 		 */
 		if ((pidr & ~(PIDR_REV_MASK | PIDR_PN_MASK)) != PIDR_ARM_BITS) {
-			DEBUG("%s0x%" PRIx32 ": 0x%02" PRIx32 "%08" PRIx32
+			DEBUG_WARN("%s0x%" PRIx32 ": 0x%02" PRIx32 "%08" PRIx32
 				  " <- does not match ARM JEP-106\n",
 				  indent, addr, (uint32_t)(pidr >> 32), (uint32_t)pidr);
 			return false;
@@ -353,7 +355,7 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 		int i;
 		for (i = 0; pidr_pn_bits[i].arch != aa_end; i++) {
 			if (pidr_pn_bits[i].part_number == part_number) {
-				DEBUG("%s%d 0x%" PRIx32 ": %s - %s %s (PIDR = 0x%02" PRIx32
+				DEBUG_INFO("%s%d 0x%" PRIx32 ": %s - %s %s (PIDR = 0x%02" PRIx32
 					  "%08" PRIx32 ")",
 					  indent + 1, num_entry, addr,
 					  cidc_debug_strings[cid_class],
@@ -364,29 +366,30 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 				 */
 				if ((pidr_pn_bits[i].cidc != cidc_unknown) &&
 				    (cid_class != pidr_pn_bits[i].cidc)) {
-					DEBUG("%sWARNING: \"%s\" !match expected \"%s\"\n", indent + 1,
-					      cidc_debug_strings[cid_class],
-					      cidc_debug_strings[pidr_pn_bits[i].cidc]);
+					DEBUG_WARN("%sWARNING: \"%s\" !match expected \"%s\"\n",
+							   indent + 1,
+							   cidc_debug_strings[cid_class],
+							   cidc_debug_strings[pidr_pn_bits[i].cidc]);
 				}
 				res = true;
 				switch (pidr_pn_bits[i].arch) {
 				case aa_cortexm:
-					DEBUG("%s-> cortexm_probe\n", indent + 1);
+					DEBUG_INFO("%s-> cortexm_probe\n", indent + 1);
 					cortexm_probe(ap, false);
 					break;
 				case aa_cortexa:
-					DEBUG("\n -> cortexa_probe\n");
+					DEBUG_INFO("\n -> cortexa_probe\n");
 					cortexa_probe(ap, addr);
 					break;
 				default:
-					DEBUG("\n");
+					DEBUG_INFO("\n");
 					break;
 				}
 				break;
 			}
 		}
 		if (pidr_pn_bits[i].arch == aa_end) {
-			DEBUG("%s0x%" PRIx32 ": %s - Unknown (PIDR = 0x%02" PRIx32
+			DEBUG_WARN("%s0x%" PRIx32 ": %s - Unknown (PIDR = 0x%02" PRIx32
 				  "%08" PRIx32 ")\n",
 				  indent, addr, cidc_debug_strings[cid_class],
 				  (uint32_t)(pidr >> 32), (uint32_t)pidr);
@@ -410,7 +413,7 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 	/* It's valid to so create a heap copy */
 	ap = malloc(sizeof(*ap));
 	if (!ap) {			/* malloc failed: heap exhaustion */
-		DEBUG("malloc: failed in %s\n", __func__);
+		DEBUG_WARN("malloc: failed in %s\n", __func__);
 		return NULL;
 	}
 
@@ -422,14 +425,14 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 		~(ADIV5_AP_CSW_SIZE_MASK | ADIV5_AP_CSW_ADDRINC_MASK);
 
 	if (ap->csw & ADIV5_AP_CSW_TRINPROG) {
-		DEBUG("AP transaction in progress.  Target may not be usable.\n");
+		DEBUG_WARN("AP transaction in progress.  Target may not be usable.\n");
 		ap->csw &= ~ADIV5_AP_CSW_TRINPROG;
 	}
 
 #if defined(ENABLE_DEBUG)
 	uint32_t cfg = adiv5_ap_read(ap, ADIV5_AP_CFG);
-	DEBUG("AP %3d: IDR=%08"PRIx32" CFG=%08"PRIx32" BASE=%08"PRIx32" CSW=%08"PRIx32"\n",
-	      apsel, ap->idr, cfg, ap->base, ap->csw);
+	DEBUG_INFO("AP %3d: IDR=%08"PRIx32" CFG=%08"PRIx32" BASE=%08" PRIx32
+			   " CSW=%08"PRIx32"\n", apsel, ap->idr, cfg, ap->base, ap->csw);
 #endif
 	return ap;
 }
@@ -460,7 +463,7 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		ctrlstat = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT);
 	}
 	if (e.type) {
-		DEBUG("DP not responding!  Trying abort sequence...\n");
+		DEBUG_WARN("DP not responding!  Trying abort sequence...\n");
 		adiv5_dp_abort(dp, ADIV5_DP_ABORT_DAPABORT);
 		ctrlstat = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT);
 	}
@@ -493,11 +496,11 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		platform_delay(20);
 		ctrlstat = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT);
 		if (ctrlstat & ADIV5_DP_CTRLSTAT_CDBGRSTACK) {
-			DEBUG("RESET_SEQ succeeded.\n");
+			DEBUG_INFO("RESET_SEQ succeeded.\n");
 			break;
 		}
 		if (platform_timeout_is_expired(&timeout)) {
-			DEBUG("RESET_SEQ failed\n");
+			DEBUG_INFO("RESET_SEQ failed\n");
 			break;
 		}
 	}
@@ -508,7 +511,7 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		adiv5_dp_write(dp, ADIV5_DP_SELECT, ADIV5_DP_BANK2);
 		dp->targetid = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT);
 		adiv5_dp_write(dp, ADIV5_DP_SELECT, ADIV5_DP_BANK0);
-		DEBUG("TARGETID %08" PRIx32 "\n", dp->targetid);
+		DEBUG_INFO("TARGETID %08" PRIx32 "\n", dp->targetid);
 	}
 	/* Probe for APs on this DP */
 	uint32_t last_base = 0;
@@ -533,7 +536,7 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 				continue;
 		}
 		if (ap->base == last_base) {
-			DEBUG("AP %d: Duplicate base\n", i);
+			DEBUG_WARN("AP %d: Duplicate base\n", i);
 #if PC_HOSTED == 1
 			if (dp->ap_cleanup)
 				dp->ap_cleanup(i);
@@ -569,7 +572,7 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		/* The rest should only be added after checking ROM table */
 		probed |= adiv5_component_probe(ap, ap->base, 0, 0);
 		if (!probed && (dp->idcode & 0xfff) == 0x477) {
-			DEBUG("-> cortexm_probe forced\n");
+			DEBUG_INFO("-> cortexm_probe forced\n");
 			cortexm_probe(ap, true);
 			probed = true;
 		}
diff --git a/src/target/adiv5_jtagdp.c b/src/target/adiv5_jtagdp.c
index 5ead978..e4fa9c7 100644
--- a/src/target/adiv5_jtagdp.c
+++ b/src/target/adiv5_jtagdp.c
@@ -45,7 +45,7 @@ void adiv5_jtag_dp_handler(jtag_dev_t *dev)
 {
 	ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
 	if (!dp) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
diff --git a/src/target/adiv5_swdp.c b/src/target/adiv5_swdp.c
index 87c687e..1691a92 100644
--- a/src/target/adiv5_swdp.c
+++ b/src/target/adiv5_swdp.c
@@ -40,7 +40,7 @@ int adiv5_swdp_scan(void)
 	target_list_free();
 	ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
 	if (!dp) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return -1;
 	}
 
@@ -66,7 +66,7 @@ int adiv5_swdp_scan(void)
 	swd_proc.swdptap_seq_out(0xA5, 8);
 	ack = swd_proc.swdptap_seq_in(3);
 	if((ack != SWDP_ACK_OK) || swd_proc.swdptap_seq_in_parity(&dp->idcode, 32)) {
-		DEBUG("\n");
+		DEBUG_WARN("Read SW-DP IDCODE failed %1" PRIx32 "\n", ack);
 		free(dp);
 		return -1;
 	}
diff --git a/src/target/cortexa.c b/src/target/cortexa.c
index 2d1f272..e7ca211 100644
--- a/src/target/cortexa.c
+++ b/src/target/cortexa.c
@@ -214,7 +214,7 @@ static uint32_t va_to_pa(target *t, uint32_t va)
 	if (par & 1)
 		priv->mmu_fault = true;
 	uint32_t pa = (par & ~0xfff) | (va & 0xfff);
-	DEBUG("%s: VA = 0x%08"PRIx32", PAR = 0x%08"PRIx32", PA = 0x%08"PRIX32"\n",
+	DEBUG_INFO("%s: VA = 0x%08"PRIx32", PAR = 0x%08"PRIx32", PA = 0x%08"PRIX32"\n",
               __func__, va, par, pa);
 	return pa;
 }
@@ -333,7 +333,7 @@ bool cortexa_probe(ADIv5_AP_t *apb, uint32_t debug_base)
 	adiv5_ap_ref(apb);
 	struct cortexa_priv *priv = calloc(1, sizeof(*priv));
 	if (!priv) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return false;
 	}
 
@@ -388,7 +388,7 @@ bool cortexa_attach(target *t)
 	dbgdscr |= DBGDSCR_HDBGEN | DBGDSCR_ITREN;
 	dbgdscr = (dbgdscr & ~DBGDSCR_EXTDCCMODE_MASK) | DBGDSCR_EXTDCCMODE_STALL;
 	apb_write(t, DBGDSCR, dbgdscr);
-	DEBUG("DBGDSCR = 0x%08"PRIx32"\n", dbgdscr);
+	DEBUG_INFO("DBGDSCR = 0x%08"PRIx32"\n", dbgdscr);
 
 	target_halt_request(t);
 	tries = 10;
@@ -630,7 +630,7 @@ static enum target_halt_reason cortexa_halt_poll(target *t, target_addr *watch)
 	if (!(dbgdscr & DBGDSCR_HALTED)) /* Not halted */
 		return TARGET_HALT_RUNNING;
 
-	DEBUG("%s: DBGDSCR = 0x%08"PRIx32"\n", __func__, dbgdscr);
+	DEBUG_INFO("%s: DBGDSCR = 0x%08"PRIx32"\n", __func__, dbgdscr);
 	/* Reenable DBGITR */
 	dbgdscr |= DBGDSCR_ITREN;
 	apb_write(t, DBGDSCR, dbgdscr);
@@ -657,7 +657,7 @@ void cortexa_halt_resume(target *t, bool step)
 	if (step) {
 		uint32_t addr = priv->reg_cache.r[15];
 		uint32_t bas = bp_bas(addr, (priv->reg_cache.cpsr & CPSR_THUMB) ? 2 : 4);
-		DEBUG("step 0x%08"PRIx32"  %"PRIx32"\n", addr, bas);
+		DEBUG_INFO("step 0x%08"PRIx32"  %"PRIx32"\n", addr, bas);
 		/* Set match any breakpoint */
 		apb_write(t, DBGBVR(0), priv->reg_cache.r[15] & ~3);
 		apb_write(t, DBGBCR(0), DBGBCR_INST_MISMATCH | bas |
@@ -693,7 +693,7 @@ void cortexa_halt_resume(target *t, bool step)
 	do {
 		apb_write(t, DBGDRCR, DBGDRCR_CSE | DBGDRCR_RRQ);
 		dbgdscr = apb_read(t, DBGDSCR);
-		DEBUG("%s: DBGDSCR = 0x%08"PRIx32"\n", __func__, dbgdscr);
+		DEBUG_INFO("%s: DBGDSCR = 0x%08"PRIx32"\n", __func__, dbgdscr);
 	} while (!(dbgdscr & DBGDSCR_RESTARTED) &&
 	         !platform_timeout_is_expired(&to));
 }
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index a0abf85..fd1bde1 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -2,8 +2,8 @@
  * This file is part of the Black Magic Debug project.
  *
  * Copyright (C) 2012  Black Sphere Technologies Ltd.
- * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- * and Koen De Vleeschauwer.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>,
+ * Koen De Vleeschauwer and Uwe Bonne
  *
  * 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
@@ -296,7 +296,7 @@ bool cortexm_probe(ADIv5_AP_t *ap, bool forced)
 	uint32_t identity = ap->idr & 0xff;
 	struct cortexm_priv *priv = calloc(1, sizeof(*priv));
 	if (!priv) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return false;
 	}
 
@@ -657,7 +657,7 @@ static void cortexm_reset(target *t)
 		   !platform_timeout_is_expired(&to));
 #if defined(PLATFORM_HAS_DEBUG)
 	if (platform_timeout_is_expired(&to))
-		DEBUG("Reset seem to be stuck low!\n");
+		DEBUG_WARN("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. */
@@ -1084,7 +1084,7 @@ static void probe_mem_read(target *t __attribute__((unused)), void *probe_dest,
 	uint8_t *dst = (uint8_t *)probe_dest;
 	uint8_t *src = (uint8_t *)target_src;
 
-	DEBUG("probe_mem_read\n");
+	DEBUG_INFO("probe_mem_read\n");
 	while (len--) *dst++=*src++;
 	return;
 }
@@ -1094,7 +1094,7 @@ static void probe_mem_write(target *t __attribute__((unused)), target_addr targe
 	uint8_t *dst = (uint8_t *)target_dest;
 	uint8_t *src = (uint8_t *)probe_src;
 
-	DEBUG("probe_mem_write\n");
+	DEBUG_INFO("probe_mem_write\n");
 	while (len--) *dst++=*src++;
 	return;
 }
@@ -1111,7 +1111,7 @@ static int cortexm_hostio_request(target *t)
 	uint32_t syscall = arm_regs[0];
 	int32_t ret = 0;
 
-	DEBUG("syscall 0"PRIx32"%"PRIx32" (%"PRIx32" %"PRIx32" %"PRIx32" %"PRIx32")\n",
+	DEBUG_INFO("syscall 0"PRIx32"%"PRIx32" (%"PRIx32" %"PRIx32" %"PRIx32" %"PRIx32")\n",
               syscall, params[0], params[1], params[2], params[3]);
 	switch (syscall) {
 #if PC_HOSTED == 1
diff --git a/src/target/efm32.c b/src/target/efm32.c
index 82125e3..f821878 100644
--- a/src/target/efm32.c
+++ b/src/target/efm32.c
@@ -543,7 +543,7 @@ static void efm32_add_flash(target *t, target_addr addr, size_t length,
 {
 	struct target_flash *f = calloc(1, sizeof(*f));
 	if (!f) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -724,7 +724,7 @@ static int efm32_flash_write(struct target_flash *f,
 	/* Check the MSC_IF */
 	uint32_t msc = device->msc_addr;
 	uint32_t msc_if = target_mem_read32(t, EFM32_MSC_IF(msc));
-	DEBUG("EFM32: Flash write done MSC_IF=%08"PRIx32"\n", msc_if);
+	DEBUG_INFO("EFM32: Flash write done MSC_IF=%08"PRIx32"\n", msc_if);
 #endif
 	return ret;
 }
@@ -985,7 +985,7 @@ void efm32_aap_probe(ADIv5_AP_t *ap)
 {
 	if ((ap->idr & EFM32_APP_IDR_MASK) == EFM32_AAP_IDR) {
 		/* It's an EFM32 AAP! */
-		DEBUG("EFM32: Found EFM32 AAP\n");
+		DEBUG_INFO("EFM32: Found EFM32 AAP\n");
 	} else {
 		return;
 	}
@@ -1002,7 +1002,7 @@ void efm32_aap_probe(ADIv5_AP_t *ap)
 	//efm32_aap_cmd_device_erase(t);
 
 	/* Read status */
-	DEBUG("EFM32: AAP STATUS=%08"PRIx32"\n", adiv5_ap_read(ap, AAP_STATUS));
+	DEBUG_INFO("EFM32: AAP STATUS=%08"PRIx32"\n", adiv5_ap_read(ap, AAP_STATUS));
 
 	sprintf(aap_driver_string,
 			"EFM32 Authentication Access Port rev.%d",
@@ -1032,15 +1032,15 @@ static bool efm32_aap_cmd_device_erase(target *t, int argc, const char **argv)
 
 	/* Read status */
 	status = adiv5_ap_read(ap, AAP_STATUS);
-	DEBUG("EFM32: AAP STATUS=%08"PRIx32"\n", status);
+	DEBUG_INFO("EFM32: AAP STATUS=%08"PRIx32"\n", status);
 
 	if (status & AAP_STATUS_ERASEBUSY) {
-		DEBUG("EFM32: AAP Erase in progress\n");
-		DEBUG("EFM32: -> ABORT\n");
+		DEBUG_WARN("EFM32: AAP Erase in progress\n");
+		DEBUG_WARN("EFM32: -> ABORT\n");
 		return false;
 	}
 
-	DEBUG("EFM32: Issuing DEVICEERASE...\n");
+	DEBUG_INFO("EFM32: Issuing DEVICEERASE...\n");
 	adiv5_ap_write(ap, AAP_CMDKEY, CMDKEY);
 	adiv5_ap_write(ap, AAP_CMD, 1);
 
@@ -1051,7 +1051,7 @@ static bool efm32_aap_cmd_device_erase(target *t, int argc, const char **argv)
 
 	/* Read status */
 	status = adiv5_ap_read(ap, AAP_STATUS);
-	DEBUG("EFM32: AAP STATUS=%08"PRIx32"\n", status);
+	DEBUG_INFO("EFM32: AAP STATUS=%08"PRIx32"\n", status);
 
 	return true;
 }
diff --git a/src/target/jtag_scan.c b/src/target/jtag_scan.c
index 28642f9..38fb4ff 100644
--- a/src/target/jtag_scan.c
+++ b/src/target/jtag_scan.c
@@ -66,10 +66,10 @@ int jtag_scan(const uint8_t *irlens)
 	/* Run throught the SWD to JTAG sequence for the case where an attached SWJ-DP is
 	 * in SW-DP mode.
 	 */
-	DEBUG("Resetting TAP\n");
+	DEBUG_INFO("Resetting TAP\n");
 #if PC_HOSTED == 1
 	if (platform_jtagtap_init()) {
-		DEBUG("JTAG not available\n");
+		DEBUG_WARN("JTAG not available\n");
 		return 0;
 	}
 #else
@@ -78,8 +78,8 @@ int jtag_scan(const uint8_t *irlens)
 	jtag_proc.jtagtap_reset();
 
 	if (irlens) {
-		DEBUG("Given list of IR lengths, skipping probe\n");
-		DEBUG("Change state to Shift-IR\n");
+		DEBUG_WARN("Given list of IR lengths, skipping probe\n");
+		DEBUG_INFO("Change state to Shift-IR\n");
 		jtagtap_shift_ir();
 		j = 0;
 		while((jtag_dev_count <= JTAG_MAX_DEVS) &&
@@ -89,7 +89,7 @@ int jtag_scan(const uint8_t *irlens)
 				break;
 			jtag_proc.jtagtap_tdi_tdo_seq((uint8_t*)&irout, 0, ones, *irlens);
 			if (!(irout & 1)) {
-				DEBUG("check failed: IR[0] != 1\n");
+				DEBUG_WARN("check failed: IR[0] != 1\n");
 				return -1;
 			}
 			jtag_devs[jtag_dev_count].ir_len = *irlens;
@@ -100,12 +100,13 @@ int jtag_scan(const uint8_t *irlens)
 			jtag_dev_count++;
 		}
 	} else {
-		DEBUG("Change state to Shift-IR\n");
+		DEBUG_INFO("Change state to Shift-IR\n");
 		jtagtap_shift_ir();
 
-		DEBUG("Scanning out IRs\n");
+		DEBUG_INFO("Scanning out IRs\n");
 		if(!jtag_proc.jtagtap_next(0, 1)) {
-			DEBUG("jtag_scan: Sanity check failed: IR[0] shifted out as 0\n");
+			DEBUG_WARN("jtag_scan: Sanity check failed: IR[0] shifted out "
+					   "as 0\n");
 			jtag_dev_count = -1;
 			return -1; /* must be 1 */
 		}
@@ -121,37 +122,37 @@ int jtag_scan(const uint8_t *irlens)
 			j++;
 		}
 		if(jtag_dev_count > JTAG_MAX_DEVS) {
-			DEBUG("jtag_scan: Maximum device count exceeded\n");
+			DEBUG_WARN("jtag_scan: Maximum device count exceeded\n");
 			jtag_dev_count = -1;
 			return -1;
 		}
 		if(jtag_devs[jtag_dev_count].ir_len > JTAG_MAX_IR_LEN) {
-			DEBUG("jtag_scan: Maximum IR length exceeded\n");
+			DEBUG_WARN("jtag_scan: Maximum IR length exceeded\n");
 			jtag_dev_count = -1;
 			return -1;
 		}
 	}
 
-	DEBUG("Return to Run-Test/Idle\n");
+	DEBUG_INFO("Return to Run-Test/Idle\n");
 	jtag_proc.jtagtap_next(1, 1);
 	jtagtap_return_idle();
 
 	/* All devices should be in BYPASS now */
 
 	/* Count device on chain */
-	DEBUG("Change state to Shift-DR\n");
+	DEBUG_INFO("Change state to Shift-DR\n");
 	jtagtap_shift_dr();
 	for(i = 0; (jtag_proc.jtagtap_next(0, 1) == 0) && (i <= jtag_dev_count); i++)
 		jtag_devs[i].dr_postscan = jtag_dev_count - i - 1;
 
 	if(i != jtag_dev_count) {
-		DEBUG("jtag_scan: Sanity check failed: "
+		DEBUG_WARN("jtag_scan: Sanity check failed: "
 			"BYPASS dev count doesn't match IR scan\n");
 		jtag_dev_count = -1;
 		return -1;
 	}
 
-	DEBUG("Return to Run-Test/Idle\n");
+	DEBUG_INFO("Return to Run-Test/Idle\n");
 	jtag_proc.jtagtap_next(1, 1);
 	jtagtap_return_idle();
 	if(!jtag_dev_count) {
@@ -173,7 +174,7 @@ int jtag_scan(const uint8_t *irlens)
 			if(jtag_proc.jtagtap_next(0, 1)) jtag_devs[i].idcode |= j;
 
 	}
-	DEBUG("Return to Run-Test/Idle\n");
+	DEBUG_INFO("Return to Run-Test/Idle\n");
 	jtag_proc.jtagtap_next(1, 1);
 	jtagtap_return_idle();
 
diff --git a/src/target/kinetis.c b/src/target/kinetis.c
index d172efe..ef88999 100644
--- a/src/target/kinetis.c
+++ b/src/target/kinetis.c
@@ -109,7 +109,7 @@ static void kl_gen_add_flash(target *t, uint32_t addr, size_t length,
 	struct target_flash *f;
 
 	if (!kf) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
diff --git a/src/target/lmi.c b/src/target/lmi.c
index fcb8a4f..d0e08ab 100644
--- a/src/target/lmi.c
+++ b/src/target/lmi.c
@@ -61,7 +61,7 @@ static void lmi_add_flash(target *t, size_t length)
 {
 	struct target_flash *f = calloc(1, sizeof(*f));
 	if (!f) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
diff --git a/src/target/lpc11xx.c b/src/target/lpc11xx.c
index 02c631d..e734fbf 100644
--- a/src/target/lpc11xx.c
+++ b/src/target/lpc11xx.c
@@ -137,7 +137,7 @@ lpc11xx_probe(target *t)
 		return true;
 	}
 	if (idcode) {
-		DEBUG("LPC11xx: Unknown IDCODE 0x%08" PRIx32 "\n", idcode);
+		DEBUG_INFO("LPC11xx: Unknown IDCODE 0x%08" PRIx32 "\n", idcode);
 	}
 	idcode = target_mem_read32(t, LPC8XX_DEVICE_ID);
 	switch (idcode) {
@@ -196,7 +196,7 @@ lpc11xx_probe(target *t)
 		return true;
 	}
 	if (idcode) {
-		DEBUG("LPC8xx: Unknown IDCODE 0x%08" PRIx32 "\n", idcode);
+		DEBUG_INFO("LPC8xx: Unknown IDCODE 0x%08" PRIx32 "\n", idcode);
 	}
 
 	return false;
diff --git a/src/target/lpc17xx.c b/src/target/lpc17xx.c
index ee2c2bd..d410181 100644
--- a/src/target/lpc17xx.c
+++ b/src/target/lpc17xx.c
@@ -138,17 +138,20 @@ lpc17xx_cmd_erase(target *t, int argc, const char *argv[])
 	struct flash_param param;
 
 	if (lpc17xx_iap_call(t, &param, IAP_CMD_PREPARE, 0, FLASH_NUM_SECTOR-1)) {
-		DEBUG("lpc17xx_cmd_erase: prepare failed %d\n", (unsigned int)param.result[0]);
+		DEBUG_WARN("lpc17xx_cmd_erase: prepare failed %d\n",
+				   (unsigned int)param.result[0]);
 		return false;
 	}
 
 	if (lpc17xx_iap_call(t, &param, IAP_CMD_ERASE, 0, FLASH_NUM_SECTOR-1, CPU_CLK_KHZ)) {
-		DEBUG("lpc17xx_cmd_erase: erase failed %d\n", (unsigned int)param.result[0]);
+		DEBUG_WARN("lpc17xx_cmd_erase: erase failed %d\n",
+				   (unsigned int)param.result[0]);
 		return false;
 	}
 
 	if (lpc17xx_iap_call(t, &param, IAP_CMD_BLANKCHECK, 0, FLASH_NUM_SECTOR-1)) {
-		DEBUG("lpc17xx_cmd_erase: blankcheck failed %d\n", (unsigned int)param.result[0]);
+		DEBUG_WARN("lpc17xx_cmd_erase: blankcheck failed %d\n",
+				   (unsigned int)param.result[0]);
 		return false;
 	}
 	tc_printf(t, "Erase OK.\n");
@@ -198,4 +201,4 @@ lpc17xx_iap_call(target *t, struct flash_param *param, enum iap_cmd cmd, ...) {
 	/* copy back just the parameters structure */
 	target_mem_read(t, (void *)param, IAP_RAM_BASE, sizeof(struct flash_param));
 	return param->result[0];
-}
\ No newline at end of file
+}
diff --git a/src/target/lpc_common.c b/src/target/lpc_common.c
index 2a35bb5..52bf87f 100644
--- a/src/target/lpc_common.c
+++ b/src/target/lpc_common.c
@@ -40,7 +40,7 @@ struct lpc_flash *lpc_add_flash(target *t, target_addr addr, size_t length)
 	struct target_flash *f;
 
 	if (!lf) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return NULL;
 	}
 
diff --git a/src/target/msp432.c b/src/target/msp432.c
index 4c29fd2..ec1908f 100644
--- a/src/target/msp432.c
+++ b/src/target/msp432.c
@@ -150,7 +150,7 @@ static void msp432_add_flash(target *t, uint32_t addr, size_t length, target_add
 	struct msp432_flash *mf = calloc(1, sizeof(*mf));
 	struct target_flash *f;
 	if (!mf) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -185,7 +185,7 @@ bool msp432_probe(target *t)
 
 	/* Check for the right HW revision: at least C, as no flash support for B */
 	if (target_mem_read32(t, HWREV_ADDR) < HWREV_MIN_VALUE) {
-		DEBUG("MSP432 Version not handled\n");
+		DEBUG_INFO("MSP432 Version not handled\n");
 		return false;
 	}
 
@@ -239,20 +239,21 @@ static bool msp432_sector_erase(struct target_flash *f, target_addr addr)
 
 	/* Unprotect sector */
 	uint32_t old_prot = msp432_sector_unprotect(mf, addr);
-	DEBUG("Flash protect: 0x%08"PRIX32"\n", target_mem_read32(t, mf->flash_protect_register));
+	DEBUG_WARN("Flash protect: 0x%08"PRIX32"\n",
+			   target_mem_read32(t, mf->flash_protect_register));
 
 	/* Prepare input data */
 	uint32_t regs[t->regs_size / sizeof(uint32_t)]; // Use of VLA
 	target_regs_read(t, regs);
 	regs[0] = addr; // Address of sector to erase in R0
 
-	DEBUG("Erasing sector at 0x%08"PRIX32"\n", addr);
+	DEBUG_INFO("Erasing sector at 0x%08"PRIX32"\n", addr);
 
 	/* Call ROM */
 	msp432_call_ROM(t, mf->FlashCtl_eraseSector, regs);
 
 	// Result value in R0 is true for success
-	DEBUG("ROM return value: %"PRIu32"\n", regs[0]);
+	DEBUG_INFO("ROM return value: %"PRIu32"\n", regs[0]);
 
 	/* Restore original protection */
 	target_mem_write32(t, mf->flash_protect_register, old_prot);
@@ -291,7 +292,8 @@ static int msp432_flash_write(struct target_flash *f, target_addr dest,
 	/* Unprotect sector, len is always < SECTOR_SIZE */
 	uint32_t old_prot = msp432_sector_unprotect(mf, dest);
 
-	DEBUG("Flash protect: 0x%08"PRIX32"\n", target_mem_read32(t, mf->flash_protect_register));
+	DEBUG_WARN("Flash protect: 0x%08"PRIX32"\n",
+			   target_mem_read32(t, mf->flash_protect_register));
 
 	/* Prepare input data */
 	uint32_t regs[t->regs_size / sizeof(uint32_t)]; // Use of VLA
@@ -300,14 +302,14 @@ static int msp432_flash_write(struct target_flash *f, target_addr dest,
 	regs[1] = dest;              // Flash address to be write to in R1
 	regs[2] = len;               // Size of buffer to be flashed in R2
 
-	DEBUG("Writing 0x%04" PRIX32 " bytes at 0x%08zu\n", dest, len);
+	DEBUG_INFO("Writing 0x%04" PRIX32 " bytes at 0x%08zu\n", dest, len);
 	/* Call ROM */
 	msp432_call_ROM(t, mf->FlashCtl_programMemory, regs);
 
 	/* Restore original protection */
 	target_mem_write32(t, mf->flash_protect_register, old_prot);
 
-	DEBUG("ROM return value: %"PRIu32"\n", regs[0]);
+	DEBUG_INFO("ROM return value: %"PRIu32"\n", regs[0]);
 	// Result value in R0 is true for success
 	return !regs[0];
 }
@@ -321,7 +323,7 @@ static bool msp432_cmd_erase_main(target *t, int argc, const char **argv)
 	/* Usually, this is not wanted, so go sector by sector...        */
 
 	uint32_t banksize = target_mem_read32(t, SYS_FLASH_SIZE) / 2;
-	DEBUG("Bank Size: 0x%08"PRIX32"\n", banksize);
+	DEBUG_INFO("Bank Size: 0x%08"PRIX32"\n", banksize);
 
 	/* Erase first bank */
 	struct target_flash *f = get_target_flash(t, MAIN_FLASH_BASE);
diff --git a/src/target/nrf51.c b/src/target/nrf51.c
index aaaac10..9fa7358 100644
--- a/src/target/nrf51.c
+++ b/src/target/nrf51.c
@@ -99,7 +99,7 @@ static void nrf51_add_flash(target *t,
 {
 	struct target_flash *f = calloc(1, sizeof(*f));
 	if (!f) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
diff --git a/src/target/nxpke04.c b/src/target/nxpke04.c
index 01d154b..19fad37 100644
--- a/src/target/nxpke04.c
+++ b/src/target/nxpke04.c
@@ -239,7 +239,7 @@ bool ke04_probe(target *t)
 	/* Add flash, all KE04 have same write and erase size */
 	struct target_flash *f = calloc(1, sizeof(*f));
 	if (!f) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return false;
 	}
 
diff --git a/src/target/sam3x.c b/src/target/sam3x.c
index a47623d..7e1be99 100644
--- a/src/target/sam3x.c
+++ b/src/target/sam3x.c
@@ -129,7 +129,7 @@ static void sam3_add_flash(target *t,
 	struct target_flash *f;
 
 	if (!sf) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -152,7 +152,7 @@ static void sam4_add_flash(target *t,
 	struct target_flash *f;
 
 	if (!sf) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -264,7 +264,7 @@ bool sam3x_probe(target *t)
 static int
 sam3x_flash_cmd(target *t, uint32_t base, uint8_t cmd, uint16_t arg)
 {
-	DEBUG("%s: base = 0x%08"PRIx32" cmd = 0x%02X, arg = 0x%06X\n",
+	DEBUG_INFO("%s: base = 0x%08"PRIx32" cmd = 0x%02X, arg = 0x%06X\n",
 		__func__, base, cmd, arg);
 	target_mem_write32(t, EEFC_FCR(base),
 	                   EEFC_FCR_FKEY | cmd | ((uint32_t)arg << 8));
diff --git a/src/target/sam4l.c b/src/target/sam4l.c
index f2903f2..06b96db 100644
--- a/src/target/sam4l.c
+++ b/src/target/sam4l.c
@@ -170,7 +170,7 @@ static void sam4l_add_flash(target *t, uint32_t addr, size_t length)
 {
 	struct target_flash *f = calloc(1, sizeof(struct target_flash));
 	if (!f) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -237,15 +237,15 @@ bool sam4l_probe(target *t)
 		target_add_ram(t, 0x20000000, ram_size);
 		flash_size = sam_nvp_size(t->idcode);
 		sam4l_add_flash(t, 0x0, flash_size);
-		DEBUG("\nSAM4L: RAM = 0x%x (%dK), FLASH = 0x%x (%dK)\n",
+		DEBUG_INFO("\nSAM4L: RAM = 0x%x (%dK), FLASH = 0x%x (%dK)\n",
 			(unsigned int) ram_size, (unsigned int) (ram_size / 1024),
 					(unsigned int) flash_size, (unsigned int)(flash_size / 1024));
 
 		/* enable SMAP if not, check for HCR and reset if set */
 		sam4l_extended_reset(t);
-		DEBUG("\nSAM4L: SAM4L Selected.\n");
+		DEBUG_INFO("\nSAM4L: SAM4L Selected.\n");
 		if (target_check_error(t)) {
-			DEBUG("SAM4L: target_check_error returned true\n");
+			DEBUG_WARN("SAM4L: target_check_error returned true\n");
 		}
 		return true;
 	}
@@ -261,11 +261,11 @@ sam4l_extended_reset(target *t)
 	uint32_t	reg;
 	int i;
 
-	DEBUG("SAM4L: Extended Reset\n");
+	DEBUG_INFO("SAM4L: Extended Reset\n");
 	/* enable SMAP in case we're dealing with a non-TCK SRST */
 	target_mem_write32(t, SMAP_CR, 0x1); /* enable SMAP */
 	reg = target_mem_read32(t, SMAP_SR);
-	DEBUG("\nSAM4L: SMAP_SR has 0x%08lx\n", (long unsigned int) reg);
+	DEBUG_INFO("\nSAM4L: SMAP_SR has 0x%08lx\n", (long unsigned int) reg);
 	if ((reg & SMAP_SR_HCR) != 0) {
 		/* write '1' bit to the status clear register */
 		target_mem_write32(t, SMAP_SCR, SMAP_SR_HCR);
@@ -275,7 +275,8 @@ sam4l_extended_reset(target *t)
 		}
 		/* not sure what to do if we can't reset that bit */
 		if (i > 249) {
-			DEBUG("\nSAM4L: Reset failed. SMAP_SR has 0x%08lx\n", (long unsigned int) reg);
+			DEBUG_INFO("\nSAM4L: Reset failed. SMAP_SR has 0x%08lx\n",
+					   (long unsigned int) reg);
 		}
 	}
 	/* reset bus error if for some reason SMAP was disabled */
@@ -298,8 +299,9 @@ sam4l_flash_command(target *t, uint32_t page, uint32_t cmd)
 	uint32_t cmd_reg;
 	uint32_t status;
 	int	timeout;
-	DEBUG("\nSAM4L: sam4l_flash_command: FSR: 0x%08x, page = %d, command = %d\n",
-		(unsigned int)(FLASHCALW_FSR), (int) page, (int) cmd);
+	DEBUG_INFO("\nSAM4L: sam4l_flash_command: FSR: 0x%08x, page = %d, "
+			   "command = %d\n", (unsigned int)(FLASHCALW_FSR),
+			   (int) page, (int) cmd);
 	/* wait for Flash controller ready */
 	for (timeout = 0; timeout < FLASH_TIMEOUT; timeout++) {
 		status = target_mem_read32(t, FLASHCALW_FSR);
@@ -308,14 +310,16 @@ sam4l_flash_command(target *t, uint32_t page, uint32_t cmd)
 		}
 	}
 	if (timeout == FLASH_TIMEOUT) {
-		DEBUG("\nSAM4L: sam4l_flash_command: Not ready! Status = 0x%08x\n", (unsigned int) status);
+		DEBUG_WARN("\nSAM4L: sam4l_flash_command: Not ready! "
+				   "Status = 0x%08x\n", (unsigned int) status);
 		return -1; /* Failed */
 	}
 	/* load up the new command */
 	cmd_reg = (cmd & FLASHCALW_FCMD_CMD_MASK) |
 			  ((page & FLASHCALW_FCMD_PAGEN_MASK) << FLASHCALW_FCMD_PAGEN_SHIFT) |
 		  	  (0xA5 << FLASHCALW_FCMD_KEY_SHIFT);
-	DEBUG("\nSAM4L: sam4l_flash_command: Wrting command word 0x%08x\n", (unsigned int) cmd_reg);
+	DEBUG_INFO("\nSAM4L: sam4l_flash_command: Wrting command word 0x%08x\n",
+			   (unsigned int) cmd_reg);
 	/* and kick it off */
 	target_mem_write32(t, FLASHCALW_FCMD, cmd_reg);
 	/* don't actually wait for it to finish, the next command will stall if it is not done */
@@ -334,7 +338,8 @@ sam4l_flash_write_buf(struct target_flash *f, target_addr addr, const void *src,
 	uint32_t ndx;
 	uint16_t page;
 
-	DEBUG("\nSAM4L: sam4l_flash_write_buf: addr = 0x%08lx, len %d\n", (long unsigned int) addr, (int) len);
+	DEBUG_INFO("\nSAM4L: sam4l_flash_write_buf: addr = 0x%08lx, len %d\n",
+			   (long unsigned int) addr, (int) len);
 	/* This will fail with unaligned writes, the write_buf version */
 	page = addr / SAM4L_PAGE_SIZE;
 
@@ -380,7 +385,7 @@ sam4l_flash_erase(struct target_flash *f, target_addr addr, size_t len)
 	target *t = f->t;
 	uint16_t page;
 
-	DEBUG("SAM4L: flash erase address 0x%08x for %d bytes\n",
+	DEBUG_INFO("SAM4L: flash erase address 0x%08x for %d bytes\n",
 		(unsigned int) addr, (unsigned int) len);
 	/*
 	 *  NB: if addr isn't aligned to a page boundary, or length
diff --git a/src/target/samd.c b/src/target/samd.c
index e66a85f..8345e0b 100644
--- a/src/target/samd.c
+++ b/src/target/samd.c
@@ -428,7 +428,7 @@ static void samd_add_flash(target *t, uint32_t addr, size_t length)
 {
 	struct target_flash *f = calloc(1, sizeof(*f));
 	if (!f) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
diff --git a/src/target/samx5x.c b/src/target/samx5x.c
index b7152f3..3604663 100644
--- a/src/target/samx5x.c
+++ b/src/target/samx5x.c
@@ -331,7 +331,7 @@ static void samx5x_add_flash(target *t, uint32_t addr, size_t length,
 {
 	struct target_flash *f = calloc(1, sizeof(*f));
 	if (!f) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_INFO("calloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -462,19 +462,19 @@ static void samx5x_unlock_current_address(target *t)
 static void samx5x_print_nvm_error(uint16_t errs)
 {
 	if (errs & SAMX5X_INTFLAG_ADDRE) {
-		DEBUG(" ADDRE");
+		DEBUG_WARN(" ADDRE");
 	}
 	if (errs & SAMX5X_INTFLAG_PROGE) {
-		DEBUG(" PROGE");
+		DEBUG_WARN(" PROGE");
 	}
 	if (errs & SAMX5X_INTFLAG_LOCKE) {
-		DEBUG(" LOCKE");
+		DEBUG_WARN(" LOCKE");
 	}
 	if (errs & SAMX5X_INTFLAG_NVME) {
-		DEBUG(" NVME");
+		DEBUG_WARN(" NVME");
 	}
 
-	DEBUG("\n");
+	DEBUG_WARN("\n");
 }
 
 static int samx5x_read_nvm_error(target *t)
@@ -499,7 +499,7 @@ static int samx5x_check_nvm_error(target *t)
 	if (!errs)
 		return 0;
 
-	DEBUG("NVM error(s) detected:");
+	DEBUG_WARN("NVM error(s) detected:");
 	samx5x_print_nvm_error(errs);
 	return -1;
 }
@@ -519,7 +519,7 @@ static int samx5x_flash_erase(struct target_flash *f, target_addr addr,
 	target *t = f->t;
 	uint16_t errs = samx5x_read_nvm_error(t);
 	if (errs) {
-		DEBUG(NVM_ERROR_BITS_MSG, "erase", addr, len);
+		DEBUG_INFO(NVM_ERROR_BITS_MSG, "erase", addr, len);
 		samx5x_print_nvm_error(errs);
 		samx5x_clear_nvm_error(t);
 	}
@@ -579,7 +579,7 @@ static int samx5x_flash_write(struct target_flash *f,
 	bool error = false;
 	uint16_t errs = samx5x_read_nvm_error(t);
 	if (errs) {
-		DEBUG(NVM_ERROR_BITS_MSG, "write", dest, len);
+		DEBUG_INFO(NVM_ERROR_BITS_MSG, "write", dest, len);
 		samx5x_print_nvm_error(errs);
 		samx5x_clear_nvm_error(t);
 	}
@@ -604,9 +604,8 @@ static int samx5x_flash_write(struct target_flash *f,
 		}
 
 	if (error || target_check_error(t) || samx5x_check_nvm_error(t)) {
-		DEBUG("Error writing flash page at 0x%08"PRIx32
-		      " (len 0x%08zx)\n",
-		      dest, len);
+		DEBUG_WARN("Error writing flash page at 0x%08"PRIx32
+		      " (len 0x%08zx)\n",  dest, len);
 		return -1;
 	}
 
@@ -623,9 +622,9 @@ static int samx5x_write_user_page(target *t, uint8_t *buffer)
 {
 	uint16_t errs = samx5x_read_nvm_error(t);
 	if (errs) {
-		DEBUG(NVM_ERROR_BITS_MSG, "erase and write",
-		      (uint32_t)SAMX5X_NVM_USER_PAGE,
-		      (size_t)SAMX5X_PAGE_SIZE);
+		DEBUG_INFO(NVM_ERROR_BITS_MSG, "erase and write",
+				   (uint32_t)SAMX5X_NVM_USER_PAGE,
+				   (size_t)SAMX5X_PAGE_SIZE);
 		samx5x_print_nvm_error(errs);
 		samx5x_clear_nvm_error(t);
 	}
@@ -692,13 +691,13 @@ static int samx5x_update_user_word(target *t, uint32_t addr, uint32_t value,
 		*value_written = new_word;
 
 	if (new_word != current_word) {
-		DEBUG("Writing user page word 0x%08"PRIx32
-		      " at offset 0x%03"PRIx32"\n", new_word, addr);
+		DEBUG_INFO("Writing user page word 0x%08"PRIx32
+				   " at offset 0x%03"PRIx32"\n", new_word, addr);
 		memcpy(buffer + addr, &new_word, 4);
 		return samx5x_write_user_page(t, buffer);
 	}
 	else {
-		DEBUG("Skipping user page write as no change would be made");
+		DEBUG_INFO("Skipping user page write as no change would be made");
 	}
 
 	return 0;
@@ -947,8 +946,8 @@ static bool samx5x_cmd_mbist(target *t, int argc, const char **argv)
 	(void)argc;
 	(void)argv;
 
-	DEBUG("Running MBIST for memory range 0x%08x-%08"PRIx32"\n",
-	      SAMX5X_RAM_START, samx5x_ram_size(t));
+	DEBUG_INFO("Running MBIST for memory range 0x%08x-%08"PRIx32"\n",
+			   SAMX5X_RAM_START, samx5x_ram_size(t));
 
 	/* Write the memory parameters to the DSU
 	 * Note that the two least significant bits of the address are
@@ -1019,8 +1018,8 @@ static bool samx5x_cmd_write8(target *t, int argc, const char **argv)
 		return false;
 	}
 
-	DEBUG("Writing 8-bit value 0x%02"PRIx32" at address 0x%08"PRIx32"\n",
-	      value, addr);
+	DEBUG_INFO("Writing 8-bit value 0x%02"PRIx32" at address 0x%08"PRIx32"\n",
+			   value, addr);
 	target_mem_write8(t, addr, (uint8_t)value);
 
 	return true;
@@ -1051,8 +1050,8 @@ static bool samx5x_cmd_write16(target *t, int argc, const char **argv)
 		return false;
 	}
 
-	DEBUG("Writing 16-bit value 0x%04"PRIx32" at address 0x%08"PRIx32"\n",
-	      value, addr);
+	DEBUG_INFO("Writing 16-bit value 0x%04"PRIx32" at address 0x%08"PRIx32"\n",
+			   value, addr);
 	target_mem_write16(t, addr, (uint16_t)value);
 
 	return true;
@@ -1078,8 +1077,8 @@ static bool samx5x_cmd_write32(target *t, int argc, const char **argv)
 		return false;
 	}
 
-	DEBUG("Writing 32-bit value 0x%08"PRIx32" at address 0x%08"PRIx32"\n",
-	      value, addr);
+	DEBUG_INFO("Writing 32-bit value 0x%08"PRIx32" at address 0x%08"PRIx32"\n",
+			   value, addr);
 	target_mem_write32(t, addr, value);
 
 	return true;
diff --git a/src/target/stm32f1.c b/src/target/stm32f1.c
index 5241b2c..84fdd77 100644
--- a/src/target/stm32f1.c
+++ b/src/target/stm32f1.c
@@ -99,7 +99,7 @@ static void stm32f1_add_flash(target *t,
 {
 	struct target_flash *f = calloc(1, sizeof(*f));
 	if (!f) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -130,7 +130,7 @@ bool stm32f1_probe(target *t)
 		if ((ap->idr >> 28) > 1) {
 			t->driver = "STM32F1 (clone) medium density";
 #if defined(PLATFORM_HAS_DEBUG)
-			DEBUG("Non-genuine STM32F1\n");
+			DEBUG_WARN("Non-genuine STM32F1\n");
 #endif
 		} else {
 			t->driver = "STM32F1 medium density";
@@ -216,7 +216,7 @@ static int stm32f1_flash_erase(struct target_flash *f,
 		/* Read FLASH_SR to poll for BSY bit */
 		while (target_mem_read32(t, FLASH_SR) & FLASH_SR_BSY)
 			if(target_check_error(t)) {
-				DEBUG("stm32f1 flash erase: comm error\n");
+				DEBUG_WARN("stm32f1 flash erase: comm error\n");
 				return -1;
 			}
 		if (len > f->blocksize)
@@ -229,7 +229,7 @@ static int stm32f1_flash_erase(struct target_flash *f,
 	/* Check for error */
 	uint32_t sr = target_mem_read32(t, FLASH_SR);
 	if ((sr & SR_ERROR_MASK) || !(sr & SR_EOP)) {
-		DEBUG("stm32f1 flash erase error 0x%" PRIx32 "\n", sr);
+		DEBUG_INFO("stm32f1 flash erase error 0x%" PRIx32 "\n", sr);
 		return -1;
 	}
 
@@ -248,14 +248,14 @@ static int stm32f1_flash_write(struct target_flash *f,
 	do {
 		sr = target_mem_read32(t, FLASH_SR);
 		if(target_check_error(t)) {
-			DEBUG("stm32f1 flash write: comm error\n");
+			DEBUG_WARN("stm32f1 flash write: comm error\n");
 			return -1;
 		}
 	} while (sr & FLASH_SR_BSY);
 
 	if (sr & SR_ERROR_MASK) {
-		DEBUG("stm32f1 flash write error 0x%" PRIx32 "\n", sr);
-			return -1;
+		DEBUG_WARN("stm32f1 flash write error 0x%" PRIx32 "\n", sr);
+		return -1;
 	}
 	return 0;
 }
diff --git a/src/target/stm32f4.c b/src/target/stm32f4.c
index 08d38eb..a593dd3 100644
--- a/src/target/stm32f4.c
+++ b/src/target/stm32f4.c
@@ -143,7 +143,7 @@ static void stm32f4_add_flash(target *t,
 	struct stm32f4_flash *sf = calloc(1, sizeof(*sf));
 	struct target_flash *f;
 	if (!sf) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -411,7 +411,7 @@ static int stm32f4_flash_erase(struct target_flash *f, target_addr addr,
 		/* Read FLASH_SR to poll for BSY bit */
 		while(target_mem_read32(t, FLASH_SR) & FLASH_SR_BSY)
 			if(target_check_error(t)) {
-				DEBUG("stm32f4 flash erase: comm error\n");
+				DEBUG_WARN("stm32f4 flash erase: comm error\n");
 				return -1;
 			}
 		if (len > f->blocksize)
@@ -426,7 +426,7 @@ static int stm32f4_flash_erase(struct target_flash *f, target_addr addr,
 	/* Check for error */
 	sr = target_mem_read32(t, FLASH_SR);
 	if(sr & SR_ERROR_MASK) {
-		DEBUG("stm32f4 flash erase: sr error: 0x%" PRIu32 "\n", sr);
+		DEBUG_WARN("stm32f4 flash erase: sr error: 0x%" PRIu32 "\n", sr);
 		return -1;
 	}
 	return 0;
@@ -450,13 +450,13 @@ static int stm32f4_flash_write(struct target_flash *f,
 	do {
 		sr = target_mem_read32(t, FLASH_SR);
 		if(target_check_error(t)) {
-			DEBUG("stm32f4 flash write: comm error\n");
+			DEBUG_WARN("stm32f4 flash write: comm error\n");
 			return -1;
 		}
 	} while (sr & FLASH_SR_BSY);
 
 	if (sr & SR_ERROR_MASK) {
-		DEBUG("stm32f4 flash write error 0x%" PRIx32 "\n", sr);
+		DEBUG_WARN("stm32f4 flash write error 0x%" PRIx32 "\n", sr);
 			return -1;
 	}
 	return 0;
diff --git a/src/target/stm32h7.c b/src/target/stm32h7.c
index cc61793..07ec635 100644
--- a/src/target/stm32h7.c
+++ b/src/target/stm32h7.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2017-2018 Uwe Bonnes bon@elektron.ikp.physik.tu-darmstadt.de
+ * Copyright (C) 2017-2020 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
@@ -165,7 +165,7 @@ static void stm32h7_add_flash(target *t,
 	struct target_flash *f;
 
 	if (!sf) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -288,19 +288,19 @@ static int stm32h7_flash_erase(struct target_flash *f, target_addr addr,
 		target_mem_write32(t, sf->regbase + FLASH_CR, cr);
 		cr |= FLASH_CR_START;
 		target_mem_write32(t, sf->regbase + FLASH_CR, cr);
-		DEBUG(" started cr %08" PRIx32 " sr %08" PRIx32 "\n",
-			  target_mem_read32(t, sf->regbase + FLASH_CR),
-			  target_mem_read32(t, sf->regbase + FLASH_SR));
+		DEBUG_INFO(" started cr %08" PRIx32 " sr %08" PRIx32 "\n",
+				   target_mem_read32(t, sf->regbase + FLASH_CR),
+				   target_mem_read32(t, sf->regbase + FLASH_SR));
 		do {
 			sr = target_mem_read32(t, sf->regbase + FLASH_SR);
 			if (target_check_error(t)) {
-				DEBUG("stm32h7_flash_erase: comm failed\n");
+				DEBUG_WARN("stm32h7_flash_erase: comm failed\n");
 				return -1;
 			}
 //			target_mem_write32(t, H7_IWDG_BASE, 0x0000aaaa);
 		}while (sr & (FLASH_SR_QW | FLASH_SR_BSY));
 		if (sr & FLASH_SR_ERROR_MASK) {
-			DEBUG("stm32h7_flash_erase: error, sr: %08" PRIx32 "\n", sr);
+			DEBUG_WARN("stm32h7_flash_erase: error, sr: %08" PRIx32 "\n", sr);
 			return -1;
 		}
 		start_sector++;
@@ -326,12 +326,12 @@ static int stm32h7_flash_write(struct target_flash *f, target_addr dest,
 	target_mem_write(t, dest, src, len);
 	while ((sr = target_mem_read32(t, sr_reg)) & FLASH_SR_BSY) {
 		if(target_check_error(t)) {
-			DEBUG("stm32h7_flash_write: BSY comm failed\n");
+			DEBUG_WARN("stm32h7_flash_write: BSY comm failed\n");
 			return -1;
 		}
 	}
 	if (sr & FLASH_SR_ERROR_MASK) {
-		DEBUG("stm32h7_flash_write: error sr %08" PRIx32 "\n", sr);
+		DEBUG_WARN("stm32h7_flash_write: error sr %08" PRIx32 "\n", sr);
 		return -1;
 	}
 	/* Close write windows.*/
@@ -357,23 +357,23 @@ static bool stm32h7_cmd_erase(target *t, int bank_mask)
 	/* Flash mass erase start instruction */
 	if (do_bank1) {
 		if (stm32h7_flash_unlock(t, BANK1_START) == false) {
-			DEBUG("ME: Unlock bank1 failed\n");
+			DEBUG_WARN("ME: Unlock bank1 failed\n");
 			goto done;
 		}
 		uint32_t regbase = FPEC1_BASE;
 		/* BER and start can be merged (3.3.10).*/
 		target_mem_write32(t, regbase + FLASH_CR, cr);
-		DEBUG("ME bank1 started\n");
+		DEBUG_INFO("ME bank1 started\n");
 	}
 	if (do_bank2) {
 		if (stm32h7_flash_unlock(t, BANK2_START) == false) {
-			DEBUG("ME: Unlock bank2 failed\n");
+			DEBUG_WARN("ME: Unlock bank2 failed\n");
 			goto done;
 		}
 		uint32_t regbase = FPEC2_BASE;
 		/* BER and start can be merged (3.3.10).*/
 		target_mem_write32(t, regbase + FLASH_CR, cr);
-		DEBUG("ME bank2 started\n");
+		DEBUG_INFO("ME bank2 started\n");
 	}
 
 	/* Read FLASH_SR to poll for QW bit */
@@ -383,7 +383,7 @@ static bool stm32h7_cmd_erase(target *t, int bank_mask)
 //			target_mem_write32(t, H7_IWDG_BASE, 0x0000aaaa);
 			tc_printf(t, "\b%c", spinner[spinindex++ % 4]);
 			if(target_check_error(t)) {
-				DEBUG("ME bank1: comm failed\n");
+				DEBUG_WARN("ME bank1: comm failed\n");
 				goto done;
 			}
 		}
@@ -394,7 +394,7 @@ static bool stm32h7_cmd_erase(target *t, int bank_mask)
 //			target_mem_write32(t, H7_IWDG_BASE 0x0000aaaa);
 			tc_printf(t, "\b%c", spinner[spinindex++ % 4]);
 			if(target_check_error(t)) {
-				DEBUG("ME bank2: comm failed\n");
+				DEBUG_WARN("ME bank2: comm failed\n");
 				goto done;
 			}
 		}
@@ -405,7 +405,7 @@ static bool stm32h7_cmd_erase(target *t, int bank_mask)
 		uint32_t regbase = FPEC1_BASE;
 		uint32_t sr = target_mem_read32(t, regbase + FLASH_SR);
 		if (sr & FLASH_SR_ERROR_MASK) {
-			DEBUG("ME bank1: sr %" PRIx32 "\n", sr);
+			DEBUG_WARN("ME bank1, error sr %" PRIx32 "\n", sr);
 			goto done;
 		}
 	}
@@ -414,7 +414,7 @@ static bool stm32h7_cmd_erase(target *t, int bank_mask)
 		uint32_t regbase = FPEC2_BASE;
 		uint32_t sr = target_mem_read32(t, regbase + FLASH_SR);
 		if (sr & FLASH_SR_ERROR_MASK) {
-			DEBUG("ME bank2: sr %" PRIx32 "\n", sr);
+			DEBUG_WARN("ME bank2, error: sr %" PRIx32 "\n", sr);
 			goto done;
 		}
 	}
@@ -467,13 +467,15 @@ static int stm32h7_crc_bank(target *t, uint32_t bank)
 	target_mem_write32(t, regbase + FLASH_CRCCR, crccr);
 	target_mem_write32(t, regbase + FLASH_CRCCR, crccr | FLASH_CRCCR_START_CRC);
 	uint32_t sr;
-	while ((sr = target_mem_read32(t, regbase + FLASH_SR)) & FLASH_SR_CRC_BUSY) {
+	while ((sr = target_mem_read32(t, regbase + FLASH_SR)) &
+		   FLASH_SR_CRC_BUSY) {
 		if(target_check_error(t)) {
-			DEBUG("CRC bank %d: comm failed\n", (bank < BANK2_START) ? 1 : 2);
+			DEBUG_WARN("CRC bank %d: comm failed\n",
+					   (bank < BANK2_START) ? 1 : 2);
 			return -1;
 		}
 		if (sr & FLASH_SR_ERROR_READ) {
-			DEBUG("CRC bank %d: error sr %08" PRIx32 "\n",
+			DEBUG_WARN("CRC bank %d: error sr %08" PRIx32 "\n",
 				  (bank < BANK2_START) ? 1 : 2, sr);
 			return -1;
 		}
diff --git a/src/target/stm32l0.c b/src/target/stm32l0.c
index 4f54dc2..088bd03 100644
--- a/src/target/stm32l0.c
+++ b/src/target/stm32l0.c
@@ -234,7 +234,7 @@ static void stm32l_add_flash(target *t,
 {
 	struct target_flash *f = calloc(1, sizeof(*f));
 	if (!f) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -251,7 +251,7 @@ static void stm32l_add_eeprom(target *t, uint32_t addr, size_t length)
 {
 	struct target_flash *f = calloc(1, sizeof(*f));
 	if (!f) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
diff --git a/src/target/stm32l4.c b/src/target/stm32l4.c
index bee108c..f3d9163 100644
--- a/src/target/stm32l4.c
+++ b/src/target/stm32l4.c
@@ -1,8 +1,8 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2015, 2017, 2018  Uwe Bonnes
- * Written by Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
+ * Copyright (C) 2015, 2017 - 2020  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
@@ -262,7 +262,7 @@ static void stm32l4_add_flash(target *t,
 	struct target_flash *f;
 
 	if (!sf) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -461,13 +461,13 @@ static int stm32l4_flash_write(struct target_flash *f,
 	do {
 		sr = target_mem_read32(t, FLASH_SR);
 		if (target_check_error(t)) {
-			DEBUG("stm32l4 flash write: comm error\n");
+			DEBUG_WARN("stm32l4 flash write: comm error\n");
 			return -1;
 		}
 	} while (sr & FLASH_SR_BSY);
 
 	if(sr & FLASH_SR_ERROR_MASK) {
-		DEBUG("stm32l4 flash write error: sr 0x%" PRIu32 "\n", sr);
+		DEBUG_WARN("stm32l4 flash write error: sr 0x%" PRIu32 "\n", sr);
 		return -1;
 	}
 	return 0;
diff --git a/src/target/target.c b/src/target/target.c
index 765a401..e1f7a65 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -39,7 +39,7 @@ target *target_new(void)
 {
 	target *t = (void*)calloc(1, sizeof(*t));
 	if (!t) {			/* calloc failed: heap exhaustion */
-		DEBUG("calloc: failed in %s\n", __func__);
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
 		return NULL;
 	}
 
@@ -124,7 +124,7 @@ void target_add_commands(target *t, const struct command_s *cmds, const char *na
 {
 	struct target_command_s *tc = malloc(sizeof(*tc));
 	if (!tc) {			/* malloc failed: heap exhaustion */
-		DEBUG("malloc: failed in %s\n", __func__);
+		DEBUG_WARN("malloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -168,7 +168,7 @@ void target_add_ram(target *t, target_addr start, uint32_t len)
 {
 	struct target_ram *ram = malloc(sizeof(*ram));
 	if (!ram) {			/* malloc failed: heap exhaustion */
-		DEBUG("malloc: failed in %s\n", __func__);
+		DEBUG_WARN("malloc: failed in %s\n", __func__);
 		return;
 	}
 
@@ -238,7 +238,7 @@ int target_flash_erase(target *t, target_addr addr, size_t len)
 	while (len) {
 		struct target_flash *f = flash_for_addr(t, addr);
 		if (!f) {
-			DEBUG("Erase stopped at 0x%06" PRIx32 "\n", addr);
+			DEBUG_WARN("Erase stopped at 0x%06" PRIx32 "\n", addr);
 			return ret;
 		}
 		size_t tmptarget = MIN(addr + len, f->start + f->length);
@@ -292,7 +292,7 @@ int target_flash_write_buffered(struct target_flash *f,
 		/* Allocate flash sector buffer */
 		f->buf = malloc(f->buf_size);
 		if (!f->buf) {			/* malloc failed: heap exhaustion */
-			DEBUG("malloc: failed in %s\n", __func__);
+			DEBUG_WARN("malloc: failed in %s\n", __func__);
 			return 1;
 		}
 		f->buf_addr = -1;
@@ -409,7 +409,7 @@ void target_set_cmdline(target *t, char *cmdline) {
 	len_dst = sizeof(t->cmdline)-1;
 	strncpy(t->cmdline, cmdline, len_dst -1);
 	t->cmdline[strlen(t->cmdline)]='\0';
-	DEBUG("cmdline: >%s<\n", t->cmdline);
+	DEBUG_INFO("cmdline: >%s<\n", t->cmdline);
 	}
 
 /* Set heapinfo for semihosting */
@@ -440,7 +440,7 @@ int target_breakwatch_set(target *t,
 		/* Success, make a heap copy */
 		struct breakwatch *bwm = malloc(sizeof bw);
 		if (!bwm) {			/* malloc failed: heap exhaustion */
-			DEBUG("malloc: failed in %s\n", __func__);
+			DEBUG_WARN("malloc: failed in %s\n", __func__);
 			return 1;
 		}
 		memcpy(bwm, &bw, sizeof(bw));

From 425002a38fd8283f5245f949e8104adbfe3ce475 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Tue, 12 May 2020 18:40:09 +0200
Subject: [PATCH 42/43] hosted: Add -l argument to list only available
 debuggers and exit.

---
 src/platforms/hosted/platform.c | 24 ++++++++++++++++++------
 src/platforms/pc/cl_utils.c     | 12 ++++++++----
 src/platforms/pc/cl_utils.h     |  1 +
 3 files changed, 27 insertions(+), 10 deletions(-)

diff --git a/src/platforms/hosted/platform.c b/src/platforms/hosted/platform.c
index 0c55a94..cd6fe65 100644
--- a/src/platforms/hosted/platform.c
+++ b/src/platforms/hosted/platform.c
@@ -100,6 +100,7 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 	char manufacturer[128];
 	char product[128];
 	bmp_type_t type = BMP_TYPE_NONE;
+	bool access_problems = false;
   rescan:
 	found_debuggers = 0;
 	for (int i = 0;  devs[i]; i++) {
@@ -114,8 +115,11 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 		libusb_device_handle *handle;
 		res = libusb_open(dev, &handle);
 		if (res != LIBUSB_SUCCESS) {
-			DEBUG_INFO("INFO: Open USB %04x:%04x failed\n",
-					   desc.idVendor, desc.idProduct);
+			if (!access_problems) {
+				DEBUG_INFO("INFO: Open USB %04x:%04x failed\n",
+						   desc.idVendor, desc.idProduct);
+				access_problems = true;
+			}
 			continue;
 		}
 		res = libusb_get_string_descriptor_ascii(
@@ -198,15 +202,23 @@ static int find_debuggers(	BMP_CL_OPTIONS_t *cl_opts,bmp_info_t *info)
 			break;
 		}
 	}
-	if (found_debuggers > 1) {
+	if ((found_debuggers > 1) ||
+		((found_debuggers == 1) && (cl_opts->opt_list_only))) {
 		if (!report) {
-			DEBUG_WARN("%d debuggers found! Select with -P <num>, -s <string> "
-				   "and/or -S <string>\n",
-				   found_debuggers);
+			if (found_debuggers > 1)
+				DEBUG_WARN("%d debuggers found!\nSelect with -P <pos>, "
+						   "-s <(partial)serial no.> "
+						   "and/or -S <(partial)description>\n",
+						   found_debuggers);
 			report = true;
 			goto rescan;
+		} else {
+			found_debuggers = 0;
 		}
 	}
+	if (!found_debuggers && access_problems)
+		DEBUG_WARN(
+			"No debugger found. Please check access rights to USB devices!\n");
 	libusb_free_device_list(devs, 1);
 	return (found_debuggers == 1) ? 0 : -1;
 }
diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index 860572a..2c55c6f 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -124,10 +124,10 @@ static void cl_help(char **argv, BMP_CL_OPTIONS_t *opt)
 	DEBUG_WARN("\t\t\t  1 = INFO, 2 = GDB, 4 = TARGET, 8 = PROBE, 16 = WIRE\n");
 	DEBUG_WARN("Probe selection arguments:\n");
 	DEBUG_WARN("\t-d \"path\"\t: Use serial device at \"path\"\n");
-	DEBUG_WARN("\t-P <num>\t: Use debugger found at position <num>\n");
+	DEBUG_WARN("\t-P <pos>\t: Use debugger found at position <pos>\n");
 	DEBUG_WARN("\t-n <num>\t: Use target device found at position <num>\n");
-	DEBUG_WARN("\t-s \"string\"\t: Use dongle with (partial) "
-		  "serial number \"string\"\n");
+	DEBUG_WARN("\t-s \"serial\"\t: Use dongle with (partial) "
+		  "serial number \"serial\"\n");
 	DEBUG_WARN("\t-c \"string\"\t: Use ftdi dongle with type \"string\"\n");
 	DEBUG_WARN("Run mode related options:\n");
 	DEBUG_WARN("\tDefault mode is to start the debug server at :2000\n");
@@ -155,7 +155,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 	opt->opt_target_dev = 1;
 	opt->opt_flash_start = 0x08000000;
 	opt->opt_flash_size = 16 * 1024 *1024;
-	while((c = getopt(argc, argv, "Ehv:d:s:I:c:Cn:tVta:S:jpP:rR")) != -1) {
+	while((c = getopt(argc, argv, "Ehv:d:s:I:c:CnltVta:S:jpP:rR")) != -1) {
 		switch(c) {
 		case 'c':
 			if (optarg)
@@ -171,6 +171,10 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 		case 'j':
 			opt->opt_usejtag = true;
 			break;
+		case 'l':
+			opt->opt_list_only = true;
+			cl_debuglevel |= BMP_DEBUG_STDOUT;
+			break;
 		case 'C':
 			opt->opt_connect_under_reset = true;
 			break;
diff --git a/src/platforms/pc/cl_utils.h b/src/platforms/pc/cl_utils.h
index 6213e08..3fedb7e 100644
--- a/src/platforms/pc/cl_utils.h
+++ b/src/platforms/pc/cl_utils.h
@@ -40,6 +40,7 @@ typedef struct BMP_CL_OPTIONS_s {
 	enum bmp_cl_mode opt_mode;
 	bool opt_usejtag;
 	bool opt_tpwr;
+	bool opt_list_only;
 	bool opt_connect_under_reset;
 	char *opt_flash_file;
 	char *opt_device;

From aabd07738eed14306dbdeee176cc990e325f551f Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Tue, 26 May 2020 12:24:53 +0200
Subject: [PATCH 43/43] cl_utils: Use start of flash in memory map as default
 flash start.

---
 src/platforms/pc/cl_utils.c | 73 ++++++++++++++++++++-----------------
 1 file changed, 40 insertions(+), 33 deletions(-)

diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index 2c55c6f..38d3c72 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -143,7 +143,7 @@ static void cl_help(char **argv, BMP_CL_OPTIONS_t *opt)
 	DEBUG_WARN("Flash operation modifiers options:\n");
 	DEBUG_WARN("\tDefault action with given file is to write to flash\n");
 	DEBUG_WARN("\t-a <addr>\t: Start flash operation at flash address <addr>\n"
-		"\t\t\t  Default start is 0x08000000\n");
+		"\t\t\t  Default start is start of flash in memory map\n");
 	DEBUG_WARN("\t-S <num>\t: Read <num> bytes. Default is until read fails.\n");
 	DEBUG_WARN("\t <file>\t\t: Use (binary) file <file> for flash operation\n");
 	exit(0);
@@ -153,7 +153,6 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 {
 	int c;
 	opt->opt_target_dev = 1;
-	opt->opt_flash_start = 0x08000000;
 	opt->opt_flash_size = 16 * 1024 *1024;
 	while((c = getopt(argc, argv, "Ehv:d:s:I:c:CnltVta:S:jpP:rR")) != -1) {
 		switch(c) {
@@ -301,45 +300,53 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 		DEBUG_WARN("Can not attach to target %d\n", opt->opt_target_dev);
 		goto target_detach;
 	}
-	if (opt->opt_mode == BMP_MODE_TEST) {
-		char map [1024], *p = map;
-		if (target_mem_map(t, map, sizeof(map))) {
-			while (*p && (*p == '<')) {
-				unsigned int start, size;
-				char *res;
-				int match;
-				match = strncmp(p, "<memory-map>", strlen("<memory-map>"));
-				if (!match) {
-					p  += strlen("<memory-map>");
-					continue;
-				}
-				match = strncmp(p, "<memory type=\"flash\" ", strlen("<memory type=\"flash\" "));
-				if (!match) {
-					unsigned int blocksize;
-					if (sscanf(p, "<memory type=\"flash\" start=\"%x\" length=\"%x\">"
-							  "<property name=\"blocksize\">%x</property></memory>",
-							  &start, &size, &blocksize))
+	/* Always scan memory map to find lowest flash */
+	char memory_map [1024], *p = memory_map;
+	uint32_t flash_start = 0xffffffff;
+	if (target_mem_map(t, memory_map, sizeof(memory_map))) {
+		while (*p && (*p == '<')) {
+			unsigned int start, size;
+			char *res;
+			int match;
+			match = strncmp(p, "<memory-map>", strlen("<memory-map>"));
+			if (!match) {
+				p  += strlen("<memory-map>");
+				continue;
+			}
+			match = strncmp(p, "<memory type=\"flash\" ", strlen("<memory type=\"flash\" "));
+			if (!match) {
+				unsigned int blocksize;
+				if (sscanf(p, "<memory type=\"flash\" start=\"%x\" length=\"%x\">"
+						   "<property name=\"blocksize\">%x</property></memory>",
+						   &start, &size, &blocksize)) {
+					if (opt->opt_mode == BMP_MODE_TEST)
 						DEBUG_INFO("Flash Start: 0x%08x, length %#9x, "
 								   "blocksize %#8x\n", start, size, blocksize);
-					res = strstr(p, "</memory>");
-					p = res + strlen("</memory>");
-					continue;
+					if (start < flash_start)
+						flash_start = start;
 				}
-				match = strncmp(p, "<memory type=\"ram\" ", strlen("<memory type=\"ram\" "));
-				if (!match) {
-					if (sscanf(p, "<memory type=\"ram\" start=\"%x\" length=\"%x\"/",
-							   &start, &size))
+				res = strstr(p, "</memory>");
+				p = res + strlen("</memory>");
+				continue;
+			}
+			match = strncmp(p, "<memory type=\"ram\" ", strlen("<memory type=\"ram\" "));
+			if (!match) {
+				if (sscanf(p, "<memory type=\"ram\" start=\"%x\" length=\"%x\"/",
+						   &start, &size))
+					if (opt->opt_mode == BMP_MODE_TEST)
 						DEBUG_INFO("Ram   Start: 0x%08x, length %#9x\n",
 								   start, size);
-					res = strstr(p, "/>");
-					p = res + strlen("/>");
-					continue;
-				}
-				break;
+				res = strstr(p, "/>");
+				p = res + strlen("/>");
+				continue;
 			}
+			break;
 		}
-		goto target_detach;
 	}
+	if (opt->opt_flash_start < flash_start)
+		opt->opt_flash_start = flash_start;
+	if (opt->opt_mode == BMP_MODE_TEST)
+		goto target_detach;
 	int read_file = -1;
 	if ((opt->opt_mode == BMP_MODE_FLASH_WRITE) ||
 		(opt->opt_mode == BMP_MODE_FLASH_VERIFY)) {