diff --git a/src/Makefile b/src/Makefile
index 7ccdc1a..396bfec 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -34,9 +34,7 @@ SRC =			\
 	gdb_hostio.c	\
 	gdb_packet.c	\
 	hex_utils.c	\
-	jtag_scan.c	\
-	jtagtap.c	\
-	jtagtap_generic.c	\
+	jtag_devs.c	\
 	lmi.c		\
 	lpc_common.c	\
 	lpc11xx.c	\
@@ -58,15 +56,25 @@ SRC =			\
 	stm32h7.c	\
 	stm32l0.c	\
 	stm32l4.c	\
-	swdptap.c	\
-	swdptap_generic.c	\
 	target.c	\
 
 include $(PLATFORM_DIR)/Makefile.inc
 
+ifndef TARGET
+TARGET=blackmagic
+endif
+
+ifndef SWD_HL
+SRC += swdptap.c swdptap_generic.c
+endif
+
+ifndef JTAG_HL
+SRC += jtag_scan.c jtagtap.c jtagtap_generic.c
+endif
+
 OBJ = $(SRC:.c=.o)
 
-blackmagic: include/version.h $(OBJ)
+$(TARGET): include/version.h $(OBJ)
 	@echo "  LD      $@"
 	$(Q)$(CC) -o $@ $(OBJ) $(LDFLAGS)
 
diff --git a/src/command.c b/src/command.c
index 0390309..85b77a0 100644
--- a/src/command.c
+++ b/src/command.c
@@ -225,7 +225,8 @@ bool cmd_swdp_scan(void)
 static void display_target(int i, target *t, void *context)
 {
 	(void)context;
-	gdb_outf("%2d   %c  %s\n", i, target_attached(t)?'*':' ', target_driver_name(t));
+	gdb_outf("%2d   %c  %s %s\n", i, target_attached(t)?'*':' ',
+			 target_driver_name(t), target_core_name(t));
 }
 
 bool cmd_targets(void)
diff --git a/src/include/general.h b/src/include/general.h
index cc7f350..91b230b 100644
--- a/src/include/general.h
+++ b/src/include/general.h
@@ -35,7 +35,6 @@
 #include "platform_support.h"
 
 #ifndef DEBUG
-#include <stdio.h>
 #define DEBUG	printf
 #endif
 
diff --git a/src/include/target.h b/src/include/target.h
index 20c8ded..ee3e5fb 100644
--- a/src/include/target.h
+++ b/src/include/target.h
@@ -46,6 +46,7 @@ target *target_attach_n(int n, struct target_controller *);
 void target_detach(target *t);
 bool target_attached(target *t);
 const char *target_driver_name(target *t);
+const char *target_core_name(target *t);
 
 /* Memory access functions */
 bool target_mem_map(target *t, char *buf, size_t len);
diff --git a/src/platforms/libftdi/Makefile.inc b/src/platforms/libftdi/Makefile.inc
index cecb459..c22d101 100644
--- a/src/platforms/libftdi/Makefile.inc
+++ b/src/platforms/libftdi/Makefile.inc
@@ -1,5 +1,5 @@
 SYS = $(shell $(CC) -dumpmachine)
-CFLAGS += -DLIBFTDI
+CFLAGS += -DLIBFTDI -DENABLE_DEBUG
 LDFLAGS += -lftdi1
 ifneq (, $(findstring mingw, $(SYS)))
 LDFLAGS +=  -lusb-1.0 -lws2_32
@@ -7,4 +7,5 @@ CFLAGS += -Wno-cast-function-type
 else ifneq (, $(findstring cygwin, $(SYS)))
 LDFLAGS +=  -lusb-1.0 -lws2_32
 endif
+VPATH += platforms/pc
 SRC += 	timing.c	\
diff --git a/src/platforms/pc-stlinkv2/Makefile.inc b/src/platforms/pc-stlinkv2/Makefile.inc
new file mode 100644
index 0000000..7fa2479
--- /dev/null
+++ b/src/platforms/pc-stlinkv2/Makefile.inc
@@ -0,0 +1,15 @@
+TARGET=blackmagic_stlinkv2
+SYS = $(shell $(CC) -dumpmachine)
+CFLAGS += -DLIBFTDI  -DSTLINKV2 -DJTAG_HL -DENABLE_DEBUG
+CFLAGS +=-I ./target
+LDFLAGS += -lusb-1.0
+ifneq (, $(findstring mingw, $(SYS)))
+LDFLAGS += -lws2_32
+CFLAGS += -Wno-cast-function-type
+else ifneq (, $(findstring cygwin, $(SYS)))
+LDFLAGS += -lws2_32
+endif
+VPATH += platforms/pc
+SRC += 	timing.c stlinkv2.c
+SWD_HL = 1
+JTAG_HL = 1
diff --git a/src/platforms/pc-stlinkv2/README.md b/src/platforms/pc-stlinkv2/README.md
new file mode 100644
index 0000000..2d145ad
--- /dev/null
+++ b/src/platforms/pc-stlinkv2/README.md
@@ -0,0 +1,16 @@
+Stlink V2/3 with original STM firmware as Blackmagic Debug Probes
+
+Recent STM Stlink firmware revision (V3 and V2 >= J32) expose nearly all
+functionality that BMP needs. This branch implements blackmagic debug probe
+for the STM Stlink as a proof of concept.
+Use at your own risk, but report or better fix problems.
+
+Run the resulting blackmagic_stlinkv2 executabel to start the gdb server
+
+CrosscCompling for windows with mingw succeeds.
+
+Drawback: JTAG does not work for chains with multiple devices.
+
+This branch may get forced push. In case of problems:
+- git reset --hard master
+- git rebase
diff --git a/src/platforms/pc-stlinkv2/adiv5_jtagdp.c b/src/platforms/pc-stlinkv2/adiv5_jtagdp.c
new file mode 100644
index 0000000..4c228fd
--- /dev/null
+++ b/src/platforms/pc-stlinkv2/adiv5_jtagdp.c
@@ -0,0 +1,73 @@
+/*
+ * 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(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/adiv5_swdp.c b/src/platforms/pc-stlinkv2/adiv5_swdp.c
new file mode 100644
index 0000000..77fbbf6
--- /dev/null
+++ b/src/platforms/pc-stlinkv2/adiv5_swdp.c
@@ -0,0 +1,49 @@
+/*
+ * 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/platform.c b/src/platforms/pc-stlinkv2/platform.c
new file mode 100644
index 0000000..61e1796
--- /dev/null
+++ b/src/platforms/pc-stlinkv2/platform.c
@@ -0,0 +1,97 @@
+/*
+ * 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 <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();
+}
+
+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;
+}
+
+#if defined(_WIN32) && !defined(__MINGW32__)
+#warning "This vasprintf() is dubious!"
+int vasprintf(char **strp, const char *fmt, va_list ap)
+{
+	int size = 128, ret = 0;
+
+	*strp = malloc(size);
+	while(*strp && ((ret = vsnprintf(*strp, size, fmt, ap)) == size))
+		*strp = realloc(*strp, size <<= 1);
+
+	return ret;
+}
+#endif
+
+void platform_delay(uint32_t ms)
+{
+	usleep(ms * 1000);
+}
+
+uint32_t platform_time_ms(void)
+{
+	struct timeval tv;
+	gettimeofday(&tv, NULL);
+	return (tv.tv_sec * 1000) + (tv.tv_usec / 1000);
+}
diff --git a/src/platforms/pc-stlinkv2/platform.h b/src/platforms/pc-stlinkv2/platform.h
new file mode 100644
index 0000000..5ecba6b
--- /dev/null
+++ b/src/platforms/pc-stlinkv2/platform.h
@@ -0,0 +1,46 @@
+/*
+ * 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_HAS_DEBUG
+
+#define SET_RUN_STATE(state)
+#define SET_IDLE_STATE(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);
+
+#endif
diff --git a/src/platforms/pc-stlinkv2/stlinkv2.c b/src/platforms/pc-stlinkv2/stlinkv2.c
new file mode 100644
index 0000000..eadc42d
--- /dev/null
+++ b/src/platforms/pc-stlinkv2/stlinkv2.c
@@ -0,0 +1,1251 @@
+/*
+ * 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 <assert.h>
+#include <unistd.h>
+#include <signal.h>
+#include <sys/time.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_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;
+	uint8_t      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;
+	libusb_device_handle *handle;
+	struct libusb_transfer* req_trans;
+	struct libusb_transfer* rep_trans;
+} stlink;
+
+stlink Stlink;
+
+static void exit_function(void)
+{
+	libusb_exit(NULL);
+	DEBUG_STLINK("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;
+
+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;
+	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 (%" PRI_SIZET "/%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.
+			 */
+			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");
+			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_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_STLINK("Failed: ");
+			stlink_usb_error_check(rxbuf, true);
+			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;
+	uint8_t cmd[16] = {
+		STLINK_DEBUG_COMMAND,
+		STLINK_DEBUG_APIV2_GETLASTRWSTATUS2
+	};
+	uint8_t data[12];
+	while(1) {
+		send_recv(txbuf, txsize, rxbuf, rxsize);
+		send_recv(cmd, 16, data, 12);
+		res = stlink_usb_error_check(data, 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_STLINK("Failed: ");
+			stlink_usb_error_check(data, 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;
+	uint8_t cmd[16] = {
+		STLINK_DEBUG_COMMAND,
+		STLINK_DEBUG_APIV2_GETLASTRWSTATUS2
+	};
+	uint8_t data[12];
+	while(1) {
+		send_recv(cmdbuf, cmdsize, NULL, 0);
+		send_recv(txbuf, txsize, NULL, 0);
+		send_recv(cmd, 16, data, 12);
+		res = stlink_usb_error_check(data, 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_STLINK("failed");
+			stlink_usb_error_check(data, 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("BOOTLOADER Mode\n");
+	} else if (data[0] == STLINK_DEV_MASS_MODE) {
+		DEBUG("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_help(char **argv)
+{
+	DEBUG("Blackmagic Debug Probe on STM StlinkV2 and 3\n\n");
+	DEBUG("Usage: %s [options]\n", argv[0]);
+	DEBUG("\t-v[1|2]\t\t: Increasing verbosity\n");
+	DEBUG("\t-s \"string\"\t: Use Stlink with (partial) "
+		  "serial number \"string\"\n");
+	DEBUG("\t-h\t\t: This help.\n");
+	exit(0);
+}
+
+void stlink_init(int argc, char **argv)
+{
+	libusb_device **devs, *dev;
+	int r;
+	atexit(exit_function);
+	signal(SIGTERM, sigterm_handler);
+	signal(SIGINT, sigterm_handler);
+	libusb_init(&Stlink.libusb_ctx);
+	char *serial = NULL;
+	int c;
+	while((c = getopt(argc, argv, "s:v:h")) != -1) {
+		switch(c) {
+		case 's':
+			serial = optarg;
+			break;
+		case 'v':
+			if (optarg)
+				debug_level = atoi(optarg);
+			break;
+		case 'h':
+			stlink_help(argv);
+			break;
+		}
+	}
+	r = libusb_init(NULL);
+	if (r < 0)
+		DEBUG("Failed: %s", libusb_strerror(r));
+	ssize_t cnt = libusb_get_device_list(NULL, &devs);
+	if (cnt < 0) {
+		libusb_exit(NULL);
+		DEBUG("Failed: %s", libusb_strerror(r));
+		goto error;
+	}
+	int i = 0;
+	bool multiple_devices = 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;
+		}
+		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;
+			}
+			if (Stlink.handle) {
+				libusb_close(Stlink.handle);
+				multiple_devices = (serial)? false : true;
+			}
+			r = libusb_open(dev, &Stlink.handle);
+			if (r == LIBUSB_SUCCESS) {
+				if (desc.iSerialNumber) {
+					r = libusb_get_string_descriptor_ascii
+						(Stlink.handle,desc.iSerialNumber, Stlink.serial,
+						 sizeof(Stlink.serial));
+				} else {
+					DEBUG("No serial number\n");
+				}
+				if (serial && (!strncmp((char*)Stlink.serial, serial, strlen(serial))))
+					DEBUG("Found ");
+				if (!serial || (!strncmp((char*)Stlink.serial, serial, strlen(serial)))) {
+					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);
+					}
+				}
+				if (serial && (!strncmp((char*)Stlink.serial, serial, strlen(serial))))
+					break;
+			} else {
+				DEBUG("Open failed %s\n", libusb_strerror(r));
+			}
+		}
+	}
+	if (multiple_devices) {
+		DEBUG("Multiple Stlinks. Please specify serial number\n");
+		goto error_1;
+	}
+	if (!Stlink.handle) {
+		DEBUG("No Stlink device found!\n");
+		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;
+	}
+	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) {
+		DEBUG("Please update Firmware\n");
+		goto error_1;
+	} else if (Stlink.ver_stlink == 3 && Stlink.ver_jtag < 3) {
+		DEBUG("Please update Firmware\n");
+		goto error_1;
+	}
+	stlink_resetsys();
+	stlink_leave_state();
+	assert(gdb_if_init() == 0);
+	return;
+  error_1:
+	libusb_close(Stlink.handle);
+  error:
+	libusb_free_device_list(devs, 1);
+	exit(-1);
+}
+
+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");
+	if (send_recv_retry(cmd, 16, data, 2) != STLINK_ERROR_OK)
+		return -1;
+	uint8_t cmd1[16] = {STLINK_DEBUG_COMMAND,
+						STLINK_DEBUG_READCOREID};
+	uint8_t data1[4];
+	send_recv(cmd1, 16, data1, 4);
+	stlink_usb_error_check(data, false);
+	return 0;
+}
+
+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;
+
+	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;
+}
+
+bool adiv5_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);
+	stlink_usb_error_check(data, true);
+	return true;
+}
+
+void div5_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);
+}
+int stlink_usb_get_rw_status(void)
+{
+	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, true);
+}
+
+void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t 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;
+		}
+	} 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 %" PRI_SIZET " 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, len);
+	if (res == STLINK_ERROR_OK) {
+		uint8_t *p = (uint8_t*)dest;
+		for (size_t i = 0; i < len ; i++) {
+			DEBUG_STLINK("%02x", *p++);
+		}
+	}
+	DEBUG_STLINK("\n");
+}
+
+void stlink_writemem8(ADIv5_AP_t *ap, uint32_t addr, size_t len,
+					  uint8_t *buffer)
+{
+	DEBUG_STLINK("Mem Write8 AP %d len %" PRI_SIZET " 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();
+		len -= length;
+		addr += length;
+	}
+}
+
+void stlink_writemem16(ADIv5_AP_t *ap, uint32_t addr, size_t len,
+					   uint16_t *buffer)
+{
+	DEBUG_STLINK("Mem Write16 AP %d len %" PRI_SIZET " 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();
+}
+
+void stlink_writemem32(ADIv5_AP_t *ap, uint32_t addr, size_t len,
+					   uint32_t *buffer)
+{
+	DEBUG_STLINK("Mem Write32 AP %d len %" PRI_SIZET " 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(void *data)
+{
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READALLREGS};
+	uint8_t res[88];
+	DEBUG_STLINK("Read all core registers\n");
+	send_recv(cmd, 16, res, 88);
+	stlink_usb_error_check(res, true);
+	memcpy(data, res + 4, 84);
+}
+
+uint32_t stlink_reg_read(int num)
+{
+	uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READREG, num};
+	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("Read reg %02" PRId32 " val 0x%08" PRIx32 "\n", num, ret);
+	return ret;
+}
+
+void stlink_reg_write(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
+	};
+	uint8_t res[2];
+	send_recv(cmd, 16, res, 2);
+	DEBUG_STLINK("Write reg %02" PRId32 " val 0x%08" PRIx32 "\n", num, 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)
+{
+	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;
+	}
+}
+
+void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+{
+	stlink_write_dp_register(ap->apsel, addr, value);
+}
+
+uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
+{
+	uint32_t ret;
+	stlink_read_dp_register(ap->apsel, addr, &ret);
+	return ret;
+}
diff --git a/src/platforms/pc-stlinkv2/stlinkv2.h b/src/platforms/pc-stlinkv2/stlinkv2.h
new file mode 100644
index 0000000..1653741
--- /dev/null
+++ b/src/platforms/pc-stlinkv2/stlinkv2.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(__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);
+int stlink_usb_get_rw_status(void);
+void stlink_regs_read(void *data);
+uint32_t stlink_reg_read(int idx);
+void stlink_reg_write(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/libftdi/gdb_if.c b/src/platforms/pc/gdb_if.c
similarity index 74%
rename from src/platforms/libftdi/gdb_if.c
rename to src/platforms/pc/gdb_if.c
index 810a33e..5da42da 100644
--- a/src/platforms/libftdi/gdb_if.c
+++ b/src/platforms/pc/gdb_if.c
@@ -36,12 +36,16 @@
 
 #include <stdio.h>
 #include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <unistd.h>
 
 #include "general.h"
 #include "gdb_if.h"
 
 static int gdb_if_serv, gdb_if_conn;
-
+#define DEFAULT_PORT 2000
+#define NUM_GDB_SERVER 4
 int gdb_if_init(void)
 {
 #if defined(_WIN32) || defined(__CYGWIN__)
@@ -50,20 +54,40 @@ int gdb_if_init(void)
 #endif
 	struct sockaddr_in addr;
 	int opt;
+	int port = DEFAULT_PORT - 1;
 
-	addr.sin_family = AF_INET;
-	addr.sin_port = htons(2000);
-	addr.sin_addr.s_addr = htonl(INADDR_ANY);
+	do {
+		port ++;
+		if (port > DEFAULT_PORT + NUM_GDB_SERVER)
+			return - 1;
+		addr.sin_family = AF_INET;
+		addr.sin_port = htons(port);
+		addr.sin_addr.s_addr = htonl(INADDR_ANY);
 
-	assert((gdb_if_serv = socket(PF_INET, SOCK_STREAM, 0)) != -1);
-	opt = 1;
-	assert(setsockopt(gdb_if_serv, SOL_SOCKET, SO_REUSEADDR, (void*)&opt, sizeof(opt)) != -1);
-	assert(setsockopt(gdb_if_serv, IPPROTO_TCP, TCP_NODELAY, (void*)&opt, sizeof(opt)) != -1);
+		gdb_if_serv = socket(PF_INET, SOCK_STREAM, 0);
+		if (gdb_if_serv == -1)
+			continue;
 
-	assert(bind(gdb_if_serv, (void*)&addr, sizeof(addr)) != -1);
-	assert(listen(gdb_if_serv, 1) != -1);
-
-	DEBUG("Listening on TCP:2000\n");
+		opt = 1;
+		if (setsockopt(gdb_if_serv, SOL_SOCKET, SO_REUSEADDR, (void*)&opt, sizeof(opt)) == -1) {
+			close(gdb_if_serv);
+			continue;
+		}
+		if (setsockopt(gdb_if_serv, IPPROTO_TCP, TCP_NODELAY, (void*)&opt, sizeof(opt)) == -1) {
+			close(gdb_if_serv);
+			continue;
+		}
+		if (bind(gdb_if_serv, (void*)&addr, sizeof(addr)) == -1) {
+			close(gdb_if_serv);
+			continue;
+		}
+		if (listen(gdb_if_serv, 1) == -1) {
+			close(gdb_if_serv);
+			continue;
+		}
+		break;
+	} while(1);
+	DEBUG("Listening on TCP: %4d\n", port);
 
 	return 0;
 }
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 3379164..a959f19 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -174,6 +174,7 @@ static const struct {
 	{0x00c, aa_cortexm,   cidc_gipc,    PIDR_PN_BIT_STRINGS("Cortex-M4 SCS",  "(System Control Space)")},
 	{0x00d, aa_nosupport, cidc_unknown, PIDR_PN_BIT_STRINGS("CoreSight ETM11", "(Embedded Trace)")},
 	{0x00e, aa_nosupport, cidc_unknown, PIDR_PN_BIT_STRINGS("Cortex-M7 FBP",  "(Flash Patch and Breakpoint)")},
+	{0x101, aa_nosupport, cidc_unknown, PIDR_PN_BIT_STRINGS("System TSGEN",   "(Time Stamp Generator)")},
 	{0x490, aa_nosupport, cidc_unknown, PIDR_PN_BIT_STRINGS("Cortex-A15 GIC", "(Generic Interrupt Controller)")},
 	{0x4c7, aa_nosupport, cidc_unknown, PIDR_PN_BIT_STRINGS("Cortex-M7 PPB",  "(Private Peripheral Bus ROM Table)")},
 	{0x906, aa_nosupport, cidc_unknown, PIDR_PN_BIT_STRINGS("CoreSight CTI",  "(Cross Trigger)")},
@@ -234,6 +235,7 @@ void adiv5_dp_unref(ADIv5_DP_t *dp)
 void adiv5_ap_unref(ADIv5_AP_t *ap)
 {
 	if (--(ap->refcnt) == 0) {
+		DEBUG("Unref AP\n");
 		adiv5_dp_unref(ap->dp);
 		free(ap);
 	}
@@ -251,12 +253,19 @@ static uint32_t adiv5_mem_read32(ADIv5_AP_t *ap, uint32_t addr)
 	return ret;
 }
 
-static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
+static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion, int num_entry)
 {
+	(void) num_entry;
 	addr &= ~3;
 	uint64_t pidr = 0;
 	uint32_t cidr = 0;
 	bool res = false;
+#if defined(ENABLE_DEBUG)
+	char indent[recursion];
+
+	for(int i = 0; i < recursion; i++) indent[i] = ' ';
+	indent[recursion] = 0;
+#endif
 
 	/* Assemble logical Product ID register value. */
 	for (int i = 0; i < 4; i++) {
@@ -275,14 +284,15 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
 	}
 
 	if (adiv5_dp_error(ap->dp)) {
-		DEBUG("Fault reading ID registers\n");
+		DEBUG("%sFault reading ID registers\n", indent);
 		return false;
 	}
 
 	/* CIDR preamble sanity check */
 	if ((cidr & ~CID_CLASS_MASK) != CID_PREAMBLE) {
-		DEBUG("0x%"PRIx32": 0x%"PRIx32" <- does not match preamble (0x%X)\n",
-                      addr, cidr, CID_PREAMBLE);
+		DEBUG("%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;
 	}
 
@@ -300,39 +310,37 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
 			DEBUG("Fault reading ROM table entry\n");
 		}
 
-		DEBUG("\nROM: Table BASE=0x%"PRIx32" SYSMEM=0x%"PRIx32"\n",
+		DEBUG("ROM: Table BASE=0x%"PRIx32" SYSMEM=0x%"PRIx32"\n",
 			  addr, memtype);
 #endif
 
 		for (int i = 0; i < 960; i++) {
 			uint32_t entry = adiv5_mem_read32(ap, addr + i*4);
 			if (adiv5_dp_error(ap->dp)) {
-				DEBUG("Fault reading ROM table entry\n");
+				DEBUG("%sFault reading ROM table entry\n", indent);
 			}
 
 			if (entry == 0)
 				break;
 
 			if (!(entry & ADIV5_ROM_ROMENTRY_PRESENT)) {
-				DEBUG("%d Entry 0x%"PRIx32" -> Not present\n", i, entry);
+				DEBUG("%s%d Entry 0x%"PRIx32" -> Not present\n", indent, i, entry);
 				continue;
 			}
 
-			DEBUG("%d Entry 0x%"PRIx32" -> 0x%"PRIx32"\n",
-				  i, entry, addr + (entry & ADIV5_ROM_ROMENTRY_OFFSET));
-
 			/* Probe recursively */
-			res |= adiv5_component_probe(ap,
-										 addr + (entry & ADIV5_ROM_ROMENTRY_OFFSET));
+			res |= adiv5_component_probe(
+				ap, addr + (entry & ADIV5_ROM_ROMENTRY_OFFSET),
+				recursion + 1, i);
 		}
-		DEBUG("ROM: Table END\n\n");
+		DEBUG("%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("0x%"PRIx32": 0x%"PRIx64" <- does not match ARM JEP-106\n",
-                              addr, pidr);
+			DEBUG("%s0x%"PRIx32": 0x%"PRIx64" <- does not match ARM JEP-106\n",
+				  indent, addr, pidr);
 			return false;
 		}
 
@@ -344,47 +352,49 @@ static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
 		int i;
 		for (i = 0; pidr_pn_bits[i].arch != aa_end; i++) {
 			if (pidr_pn_bits[i].part_number == part_number) {
-				DEBUG("0x%"PRIx32": %s - %s %s (PIDR = 0x%"PRIx64")\n", addr,
-				      cidc_debug_strings[cid_class],
-				      pidr_pn_bits[i].type,
-				      pidr_pn_bits[i].full, pidr);
+				DEBUG("%s%d 0x%"PRIx32": %s - %s %s (PIDR = 0x%"PRIx64")",
+					  indent + 1, num_entry, addr, cidc_debug_strings[cid_class],
+				      pidr_pn_bits[i].type, pidr_pn_bits[i].full, pidr);
 				/* Perform sanity check, if we know what to expect as component ID
 				 * class.
 				 */
 				if ((pidr_pn_bits[i].cidc != cidc_unknown) &&
 				    (cid_class != pidr_pn_bits[i].cidc)) {
-					DEBUG("WARNING: \"%s\" !match expected \"%s\"\n",
+					DEBUG("%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("-> cortexm_probe\n");
+					DEBUG("%s-> cortexm_probe\n", indent + 1);
 					cortexm_probe(ap, false);
 					break;
 				case aa_cortexa:
-					DEBUG("-> cortexa_probe\n");
+					DEBUG("%s-> cortexa_probe\n", indent + 1);
 					cortexa_probe(ap, addr);
 					break;
 				default:
+					DEBUG("\n");
 					break;
 				}
 				break;
 			}
 		}
 		if (pidr_pn_bits[i].arch == aa_end) {
-			DEBUG("0x%"PRIx32": %s - Unknown (PIDR = 0x%"PRIx64")\n", addr,
-			      cidc_debug_strings[cid_class], pidr);
+			DEBUG("%s0x%"PRIx32": %s - Unknown (PIDR = 0x%"PRIx64")\n",
+				  indent, addr, cidc_debug_strings[cid_class], pidr);
 		}
 	}
 	return res;
 }
 
+bool __attribute__((weak)) adiv5_ap_setup(int i) {(void)i; return true;}
+void __attribute__((weak)) adiv5_ap_cleanup(int i) {(void)i;}
+
 ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 {
 	ADIv5_AP_t *ap, tmpap;
-
 	/* Assume valid and try to read IDR */
 	memset(&tmpap, 0, sizeof(tmpap));
 	tmpap.dp = dp;
@@ -394,7 +404,6 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 
 	if(!tmpap.idr) /* IDR Invalid */
 		return NULL;
-
 	/* It's valid to so create a heap copy */
 	ap = malloc(sizeof(*ap));
 	if (!ap) {			/* malloc failed: heap exhaustion */
@@ -415,18 +424,15 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 		ap->csw &= ~ADIV5_AP_CSW_TRINPROG;
 	}
 
-	DEBUG(" AP %3d: IDR=%08"PRIx32" CFG=%08"PRIx32" BASE=%08"PRIx32" CSW=%08"PRIx32"\n",
+	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);
-
 	return ap;
 }
 
-
 void adiv5_dp_init(ADIv5_DP_t *dp)
 {
 	volatile bool probed = false;
 	volatile uint32_t ctrlstat = 0;
-
 	adiv5_dp_ref(dp);
 
 	volatile struct exception e;
@@ -479,10 +485,13 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 	}
 	/* Probe for APs on this DP */
 	for(int i = 0; i < 256; i++) {
-		ADIv5_AP_t *ap = adiv5_new_ap(dp, i);
-		if (ap == NULL)
+		ADIv5_AP_t *ap = NULL;
+		if (adiv5_ap_setup(i))
+			ap = adiv5_new_ap(dp, i);
+		if (ap == NULL) {
+			adiv5_ap_cleanup(i);
 			continue;
-
+		}
 		extern void kinetis_mdm_probe(ADIv5_AP_t *);
 		kinetis_mdm_probe(ap);
 
@@ -504,7 +513,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);
+		probed |= adiv5_component_probe(ap, ap->base, 0, 0);
 		if (!probed && (dp->idcode & 0xfff) == 0x477) {
 			DEBUG("-> cortexm_probe forced\n");
 			cortexm_probe(ap, true);
@@ -556,7 +565,7 @@ static void * extract(void *dest, uint32_t src, uint32_t val, enum align align)
 	return (uint8_t *)dest + (1 << align);
 }
 
-void
+void  __attribute__((weak))
 adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 {
 	uint32_t tmp;
@@ -587,7 +596,7 @@ adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 	extract(dest, src, tmp, align);
 }
 
-void
+void  __attribute__((weak))
 adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
 					  size_t len, enum align align)
 {
@@ -623,21 +632,16 @@ adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
 	}
 }
 
-void
-adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
-{
-	enum align align = MIN(ALIGNOF(dest), ALIGNOF(len));
-	adiv5_mem_write_sized(ap, dest, src, len, align);
-}
-
-void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
+void  __attribute__((weak))
+adiv5_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)
+uint32_t  __attribute__((weak))
+adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 {
 	uint32_t ret;
 	adiv5_dp_write(ap->dp, ADIV5_DP_SELECT,
@@ -645,3 +649,9 @@ uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr)
 	ret = adiv5_dp_read(ap->dp, addr);
 	return ret;
 }
+
+void adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
+{
+	enum align align = MIN(ALIGNOF(dest), ALIGNOF(len));
+	adiv5_mem_write_sized(ap, dest, src, len, align);
+}
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index b72ca7e..7def372 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -5,7 +5,8 @@
  * 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
+ * it under tSchreibe Objekte: 100% (21/21), 3.20 KiB | 3.20 MiB/s, Fertig.
+he 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.
  *
@@ -269,6 +270,7 @@ bool cortexm_probe(ADIv5_AP_t *ap, bool forced)
 	}
 
 	adiv5_ap_ref(ap);
+	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__);
@@ -284,6 +286,20 @@ bool cortexm_probe(ADIv5_AP_t *ap, bool forced)
 	t->mem_write = cortexm_mem_write;
 
 	t->driver = cortexm_driver_str;
+	switch (identity) {
+	case 0x11: /* M3/M4 */
+		t->core = "M3/M4";
+		break;
+	case 0x21: /* M0 */
+		t->core = "M0";
+		break;
+	case 0x31: /* M0+ */
+		t->core = "M0+";
+		break;
+	case 0x01: /* M7 */
+		t->core = "M7";
+		break;
+	}
 
 	t->attach = cortexm_attach;
 	t->detach = cortexm_detach;
@@ -433,8 +449,17 @@ enum { DB_DHCSR, DB_DCRSR, DB_DCRDR, DB_DEMCR };
 
 static void cortexm_regs_read(target *t, void *data)
 {
-	ADIv5_AP_t *ap = cortexm_ap(t);
 	uint32_t *regs = data;
+#if defined(STLINKV2)
+	extern void stlink_regs_read(void *data);
+	extern uint32_t stlink_reg_read(int idx);
+	stlink_regs_read(data);
+	regs += sizeof(regnum_cortex_m);
+	if (t->target_options & TOPT_FLAVOUR_V7MF)
+		for(size_t t = 0; t < sizeof(regnum_cortex_mf) / 4; t++)
+			*regs++ = stlink_reg_read(regnum_cortex_mf[t]);
+#else
+	ADIv5_AP_t *ap = cortexm_ap(t);
 	unsigned i;
 
 	/* FIXME: Describe what's really going on here */
@@ -460,12 +485,25 @@ static void cortexm_regs_read(target *t, void *data)
 			                    regnum_cortex_mf[i]);
 			*regs++ = adiv5_dp_read(ap->dp, ADIV5_AP_DB(DB_DCRDR));
 		}
+#endif
 }
 
 static void cortexm_regs_write(target *t, const void *data)
 {
-	ADIv5_AP_t *ap = cortexm_ap(t);
 	const uint32_t *regs = data;
+#if defined(STLINKV2)
+	extern void stlink_reg_write(int num, uint32_t val);
+	for(size_t z = 1; z < sizeof(regnum_cortex_m) / 4; z++) {
+		stlink_reg_write(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(regnum_cortex_mf[z], *regs);
+			regs++;
+		}
+	}
+#else
+	ADIv5_AP_t *ap = cortexm_ap(t);
 	unsigned i;
 
 	/* FIXME: Describe what's really going on here */
@@ -494,6 +532,7 @@ static void cortexm_regs_write(target *t, const void *data)
 			                    ADIV5_AP_DB(DB_DCRSR),
 			                    0x10000 | regnum_cortex_mf[i]);
 		}
+#endif
 }
 
 int cortexm_mem_write_sized(
diff --git a/src/target/jtag_devs.c b/src/target/jtag_devs.c
new file mode 100644
index 0000000..b835e1f
--- /dev/null
+++ b/src/target/jtag_devs.c
@@ -0,0 +1,59 @@
+/*
+ * 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/>.
+ */
+
+#include "general.h"
+#include "jtag_scan.h"
+#include "adiv5.h"
+#include "jtag_devs.h"
+
+jtag_dev_descr_t dev_descr[] = {
+	{.idcode = 0x0BA00477, .idmask = 0x0FFF0FFF,
+		.descr = "ARM Limited: ADIv5 JTAG-DP port.",
+		.handler = adiv5_jtag_dp_handler},
+	{.idcode = 0x06410041, .idmask = 0x0FFFFFFF,
+		.descr = "ST Microelectronics: STM32, Medium density."},
+	{.idcode = 0x06412041, .idmask = 0x0FFFFFFF,
+		.descr = "ST Microelectronics: STM32, Low density."},
+	{.idcode = 0x06414041, .idmask = 0x0FFFFFFF,
+		.descr = "ST Microelectronics: STM32, High density."},
+	{.idcode = 0x06416041, .idmask = 0x0FFFFFFF,
+		.descr = "ST Microelectronics: STM32L."},
+	{.idcode = 0x06418041, .idmask = 0x0FFFFFFF,
+		.descr = "ST Microelectronics: STM32, Connectivity Line."},
+	{.idcode = 0x06420041, .idmask = 0x0FFFFFFF,
+		.descr = "ST Microelectronics: STM32, Value Line."},
+	{.idcode = 0x06428041, .idmask = 0x0FFFFFFF,
+		.descr = "ST Microelectronics: STM32, Value Line, High density."},
+	{.idcode = 0x06411041, .idmask = 0xFFFFFFFF,
+		.descr = "ST Microelectronics: STM32F2xx."},
+	{.idcode = 0x06413041 , .idmask = 0xFFFFFFFF,
+		.descr = "ST Microelectronics: STM32F4xx."},
+	{.idcode = 0x0BB11477 , .idmask = 0xFFFFFFFF,
+		.descr = "NPX: LPC11C24."},
+	{.idcode = 0x4BA00477 , .idmask = 0xFFFFFFFF,
+		.descr = "NXP: LPC17xx family."},
+/* Just for fun, unsupported */
+	{.idcode = 0x8940303F, .idmask = 0xFFFFFFFF, .descr = "ATMEL: ATMega16."},
+	{.idcode = 0x0792603F, .idmask = 0xFFFFFFFF, .descr = "ATMEL: AT91SAM9261."},
+	{.idcode = 0x20270013, .idmask = 0xFFFFFFFF, .descr = "Intel: i80386ex."},
+	{.idcode = 0x07B7617F, .idmask = 0xFFFFFFFF, .descr = "Broadcom: BCM2835."},
+	{.idcode = 0x4BA00477, .idmask = 0xFFFFFFFF, .descr = "Broadcom: BCM2836."},
+	{.idcode = 0, .idmask = 0, .descr = "Unknown"},
+};
diff --git a/src/target/jtag_devs.h b/src/target/jtag_devs.h
new file mode 100644
index 0000000..47d17d0
--- /dev/null
+++ b/src/target/jtag_devs.h
@@ -0,0 +1,28 @@
+/*
+ * 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/>.
+ */
+
+typedef const struct jtag_dev_descr_s {
+	const uint32_t idcode;
+	const uint32_t idmask;
+	const char * const descr;
+	void (*const handler)(jtag_dev_t *dev);
+} jtag_dev_descr_t;
+extern jtag_dev_descr_t dev_descr[];
+
diff --git a/src/target/jtag_scan.c b/src/target/jtag_scan.c
index 5487efc..91935ad 100644
--- a/src/target/jtag_scan.c
+++ b/src/target/jtag_scan.c
@@ -28,50 +28,11 @@
 #include "jtag_scan.h"
 #include "target.h"
 #include "adiv5.h"
+#include "jtag_devs.h"
 
 struct jtag_dev_s jtag_devs[JTAG_MAX_DEVS+1];
 int jtag_dev_count;
 
-static const struct jtag_dev_descr_s {
-	const uint32_t idcode;
-	const uint32_t idmask;
-	const char * const descr;
-	void (*const handler)(jtag_dev_t *dev);
-} dev_descr[] = {
-	{.idcode = 0x0BA00477, .idmask = 0x0FFF0FFF,
-		.descr = "ARM Limited: ADIv5 JTAG-DP port.",
-		.handler = adiv5_jtag_dp_handler},
-	{.idcode = 0x06410041, .idmask = 0x0FFFFFFF,
-		.descr = "ST Microelectronics: STM32, Medium density."},
-	{.idcode = 0x06412041, .idmask = 0x0FFFFFFF,
-		.descr = "ST Microelectronics: STM32, Low density."},
-	{.idcode = 0x06414041, .idmask = 0x0FFFFFFF,
-		.descr = "ST Microelectronics: STM32, High density."},
-	{.idcode = 0x06416041, .idmask = 0x0FFFFFFF,
-		.descr = "ST Microelectronics: STM32L."},
-	{.idcode = 0x06418041, .idmask = 0x0FFFFFFF,
-		.descr = "ST Microelectronics: STM32, Connectivity Line."},
-	{.idcode = 0x06420041, .idmask = 0x0FFFFFFF,
-		.descr = "ST Microelectronics: STM32, Value Line."},
-	{.idcode = 0x06428041, .idmask = 0x0FFFFFFF,
-		.descr = "ST Microelectronics: STM32, Value Line, High density."},
-	{.idcode = 0x06411041, .idmask = 0xFFFFFFFF,
-		.descr = "ST Microelectronics: STM32F2xx."},
-	{.idcode = 0x06413041 , .idmask = 0xFFFFFFFF,
-		.descr = "ST Microelectronics: STM32F4xx."},
-	{.idcode = 0x0BB11477 , .idmask = 0xFFFFFFFF,
-		.descr = "NPX: LPC11C24."},
-	{.idcode = 0x4BA00477 , .idmask = 0xFFFFFFFF,
-		.descr = "NXP: LPC17xx family."},
-/* Just for fun, unsupported */
-	{.idcode = 0x8940303F, .idmask = 0xFFFFFFFF, .descr = "ATMEL: ATMega16."},
-	{.idcode = 0x0792603F, .idmask = 0xFFFFFFFF, .descr = "ATMEL: AT91SAM9261."},
-	{.idcode = 0x20270013, .idmask = 0xFFFFFFFF, .descr = "Intel: i80386ex."},
-	{.idcode = 0x07B7617F, .idmask = 0xFFFFFFFF, .descr = "Broadcom: BCM2835."},
-	{.idcode = 0x4BA00477, .idmask = 0xFFFFFFFF, .descr = "Broadcom: BCM2836."},
-	{.idcode = 0, .idmask = 0, .descr = "Unknown"},
-};
-
 /* bucket of ones for don't care TDI */
 static const uint8_t ones[] = "\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF";
 
diff --git a/src/target/jtag_scan.h b/src/target/jtag_scan.h
index 3c723f4..cd46801 100644
--- a/src/target/jtag_scan.h
+++ b/src/target/jtag_scan.h
@@ -25,6 +25,7 @@
 #define JTAG_MAX_IR_LEN	16
 
 typedef struct jtag_dev_s {
+#if !defined(JTAG_HL)
 	union {
 		uint8_t dev;
 		uint8_t dr_prescan;
@@ -34,11 +35,12 @@ 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;
 
diff --git a/src/target/target.c b/src/target/target.c
index ed5f36c..48dc748 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -425,6 +425,11 @@ const char *target_driver_name(target *t)
 	return t->driver;
 }
 
+const char *target_core_name(target *t)
+{
+	return t->core;
+}
+
 uint32_t target_mem_read32(target *t, uint32_t addr)
 {
 	uint32_t ret;
diff --git a/src/target/target_internal.h b/src/target/target_internal.h
index 6c18d07..5f4e41e 100644
--- a/src/target/target_internal.h
+++ b/src/target/target_internal.h
@@ -115,6 +115,7 @@ struct target_s {
 
 	/* Other stuff */
 	const char *driver;
+	const char *core;
 	struct target_command_s *commands;
 
 	struct target_s *next;