Merge pull request #466 from UweBonnes/stlinkv2

Add a new platform, running on the USB host and talking to an unmodified StlinkV2/3

src/Makefile

@@ -34,9 +34,7 @@ SRC =			\
 	gdb_hostio.c	\
 	gdb_packet.c	\
 	hex_utils.c	\
-	jtag_scan.c	\
+	jtag_devs.c	\
-	jtagtap.c	\
-	jtagtap_generic.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)
 

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)

src/include/general.h

@@ -35,7 +35,6 @@
 #include "platform_support.h"
 
 #ifndef DEBUG
-#include <stdio.h>
 #define DEBUG	printf
 #endif
 

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);

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	\

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

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

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);
+}

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;
+}

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);
+}

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

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

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;
+	do {
-	addr.sin_port = htons(2000);
+		port ++;
-	addr.sin_addr.s_addr = htonl(INADDR_ANY);
+		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);
+		gdb_if_serv = socket(PF_INET, SOCK_STREAM, 0);
-	opt = 1;
+		if (gdb_if_serv == -1)
-	assert(setsockopt(gdb_if_serv, SOL_SOCKET, SO_REUSEADDR, (void*)&opt, sizeof(opt)) != -1);
+			continue;
-	assert(setsockopt(gdb_if_serv, IPPROTO_TCP, TCP_NODELAY, (void*)&opt, sizeof(opt)) != -1);
 
-	assert(bind(gdb_if_serv, (void*)&addr, sizeof(addr)) != -1);
+		opt = 1;
-	assert(listen(gdb_if_serv, 1) != -1);
+		if (setsockopt(gdb_if_serv, SOL_SOCKET, SO_REUSEADDR, (void*)&opt, sizeof(opt)) == -1) {
-
+			close(gdb_if_serv);
-	DEBUG("Listening on TCP:2000\n");
+			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;
 }

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",
+		DEBUG("%s%d 0x%08" PRIx32": 0x%08" PRIx32
-                      addr, cidr, CID_PREAMBLE);
+			  " <- 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,
+			res |= adiv5_component_probe(
-										 addr + (entry & ADIV5_ROM_ROMENTRY_OFFSET));
+				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",
+			DEBUG("%s0x%"PRIx32": 0x%"PRIx64" <- does not match ARM JEP-106\n",
-                              addr, pidr);
+				  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,
+				DEBUG("%s%d 0x%"PRIx32": %s - %s %s (PIDR = 0x%"PRIx64")",
-				      cidc_debug_strings[cid_class],
+					  indent + 1, num_entry, addr, cidc_debug_strings[cid_class],
-				      pidr_pn_bits[i].type,
+				      pidr_pn_bits[i].type, pidr_pn_bits[i].full, pidr);
-				      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,
+			DEBUG("%s0x%"PRIx32": %s - Unknown (PIDR = 0x%"PRIx64")\n",
-			      cidc_debug_strings[cid_class], pidr);
+				  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);
+		ADIv5_AP_t *ap = NULL;
-		if (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
+void  __attribute__((weak))
-adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
+adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
-{
-	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)
 {
 	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);
+}

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(

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"},
+};

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[];
+

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";
 

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;
 

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;

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;