Merge pull request #857 from UweBonnes/rp

Support the Raspberry Pico RP2040

    Implement multi-drop
    Recognize , erase, flash and usb_reset
    Attach to the rescue DP will reset but attach itself fails.

src/Makefile

@@ -48,6 +48,7 @@ SRC =			\
 	nxpke04.c	\
 	platform.c	\
 	remote.c	\
+	rp.c		\
 	sam3x.c		\
 	sam4l.c		\
 	samd.c		\

src/include/swdptap.h

@@ -1,36 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2011  Black Sphere Technologies Ltd.
- * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __SWDPTAP_H
-#define __SWDPTAP_H
-typedef struct swd_proc_s {
-	uint32_t (*swdptap_seq_in)(int ticks);
-	bool (*swdptap_seq_in_parity)(uint32_t *data, int ticks);
-	void (*swdptap_seq_out)(uint32_t MS, int ticks);
-	void (*swdptap_seq_out_parity)(uint32_t MS, int ticks);
-} swd_proc_t;
-extern swd_proc_t swd_proc;
-
-# if PC_HOSTED == 1
-int platform_swdptap_init(void);
-# else
-int swdptap_init(void);
-# endif
-#endif

src/platforms/common/swdptap.c

@@ -21,8 +21,8 @@
 /* This file implements the SW-DP interface. */
 
 #include "general.h"
-#include "swdptap.h"
 #include "timing.h"
+#include "adiv5.h"
 
 enum {
 	SWDIO_STATUS_FLOAT = 0,
@@ -204,14 +204,12 @@ static void swdptap_seq_out_parity(uint32_t MS, int ticks)
 	for(cnt = swd_delay_cnt; --cnt > 0;);
 }
 
-swd_proc_t swd_proc;
+int swdptap_init(ADIv5_DP_t *dp)
-
-int swdptap_init(void)
 {
-	swd_proc.swdptap_seq_in  = swdptap_seq_in;
+	dp->seq_in  = swdptap_seq_in;
-	swd_proc.swdptap_seq_in_parity  = swdptap_seq_in_parity;
+	dp->seq_in_parity  = swdptap_seq_in_parity;
-	swd_proc.swdptap_seq_out = swdptap_seq_out;
+	dp->seq_out = swdptap_seq_out;
-	swd_proc.swdptap_seq_out_parity  = swdptap_seq_out_parity;
+	dp->seq_out_parity  = swdptap_seq_out_parity;
 
 	return 0;
 }

src/platforms/hosted/bmp_remote.h

@@ -18,7 +18,6 @@
  */
 #if !defined(__BMP_REMOTE_H_)
 #define      __BMP_REMOTE_H_
-#include "swdptap.h"
 #include "jtagtap.h"
 #include "adiv5.h"
 #include "target.h"
@@ -30,7 +29,7 @@ int platform_buffer_write(const uint8_t *data, int size);
 int platform_buffer_read(uint8_t *data, int size);
 
 int remote_init(void);
-int remote_swdptap_init(swd_proc_t *swd_proc);
+int remote_swdptap_init(ADIv5_DP_t *dp);
 int remote_jtagtap_init(jtag_proc_t *jtag_proc);
 bool remote_target_get_power(void);
 const char *remote_target_voltage(void);

src/platforms/hosted/cmsis_dap.c

@@ -66,7 +66,7 @@ int dap_init(bmp_info_t *info)
 	 */
 	if ((info->vid == 0x1fc9) && (info->pid == 0x0132)) {
 		DEBUG_WARN("Blacklist\n");
-		report_size = 64 + 1;
+		report_size = 64;
 	}
 	handle = hid_open(info->vid, info->pid,  (serial[0]) ? serial : NULL);
 	if (!handle)
@@ -79,12 +79,12 @@ int dap_init(bmp_info_t *info)
 		if (sscanf((const char *)hid_buffer, "%d.%d.%d",
 				   &major, &minor, &sub)) {
 			if (sub == -1) {
-				if (minor > 10) {
+				if (minor >= 10) {
 					minor /= 10;
 					sub = 0;
 				}
 			}
-			has_swd_sequence = ((major > 0 ) && (minor > 1));
+			has_swd_sequence = ((major > 1 ) || ((major > 0 ) && (minor > 1)));
 		}
 	}
 	size = dap_info(DAP_INFO_CAPABILITIES, hid_buffer, sizeof(hid_buffer));
@@ -99,6 +99,11 @@ int dap_init(bmp_info_t *info)
 		DEBUG_INFO(", SWO_MANCHESTER");
 	if (dap_caps & 0x10)
 		DEBUG_INFO(", Atomic Cmds");
+	size = dap_info(DAP_INFO_PACKET_SIZE, hid_buffer, sizeof(hid_buffer));
+	if (size) {
+		report_size = hid_buffer[0];
+		DEBUG_INFO(", Reportsize %d", hid_buffer[0]);
+	}
 	DEBUG_INFO("\n");
 	return 0;
 }
@@ -111,6 +116,7 @@ static void dap_dp_abort(ADIv5_DP_t *dp, uint32_t abort)
 
 static uint32_t dap_dp_error(ADIv5_DP_t *dp)
 {
+	/* Not used for SWD debugging, so no TARGETID switch needed!*/
 	uint32_t ctrlstat = dap_read_reg(dp, ADIV5_DP_CTRLSTAT);
 	uint32_t err = ctrlstat &
 		(ADIV5_DP_CTRLSTAT_STICKYORUN | ADIV5_DP_CTRLSTAT_STICKYCMP |
@@ -170,38 +176,36 @@ int dbg_dap_cmd(uint8_t *data, int size, int rsize)
 	char cmd = data[0];
 	int res;
 
-	memset(hid_buffer, 0xff, report_size + 1);
+	memset(hid_buffer, 0, report_size + 1);
 
-	hid_buffer[0] = 0x00; // Report ID??
 	memcpy(&hid_buffer[1], data, rsize);
 
 	DEBUG_WIRE("cmd :   ");
-	for(int i = 0; (i < 16) && (i < rsize + 1); i++)
+	for(int i = 0; (i < 32) && (i < rsize + 1); i++)
 		DEBUG_WIRE("%02x.",	hid_buffer[i]);
 	DEBUG_WIRE("\n");
-	res = hid_write(handle, hid_buffer, rsize + 1);
+	/* Write must be as long as we expect the result, at least
+	 * for Dappermime 20210213 */
+	res = hid_write(handle, hid_buffer, report_size + 1);
 	if (res < 0) {
 		DEBUG_WARN( "Error: %ls\n", hid_error(handle));
 		exit(-1);
 	}
-	if (size) {
+	res = hid_read(handle, hid_buffer, report_size + 1);
-		res = hid_read(handle, hid_buffer, report_size + 1);
+	if (res < 0) {
-		if (res < 0) {
+		DEBUG_WARN( "debugger read(): %ls\n", hid_error(handle));
-			DEBUG_WARN( "debugger read(): %ls\n", hid_error(handle));
+		exit(-1);
-			exit(-1);
-		}
-		if (size && hid_buffer[0] != cmd) {
-			DEBUG_WARN("cmd %02x invalid response received %02x\n",
-				   cmd, hid_buffer[0]);
-		}
-		res--;
-		memcpy(data, &hid_buffer[1], (size < res) ? size : res);
-		DEBUG_WIRE("cmd res:");
-		for(int i = 0; (i < 16) && (i < size + 4); i++)
-			DEBUG_WIRE("%02x.",	hid_buffer[i]);
-		DEBUG_WIRE("\n");
 	}
-
+	DEBUG_WIRE("res %2d: ", res);
+	for(int i = 0; (i < 16) && (i < res + 1); i++)
+		DEBUG_WIRE("%02x.",	hid_buffer[i]);
+	DEBUG_WIRE("\n");
+	if (hid_buffer[0] != cmd) {
+		DEBUG_WARN("cmd %02x invalid response received %02x\n",
+				   cmd, hid_buffer[0]);
+	}
+	if (size)
+		memcpy(data, &hid_buffer[1], (size < res) ? size : res);
 	return res;
 }
 #define ALIGNOF(x) (((x) & 3) == 0 ? ALIGN_WORD :					\
@@ -281,26 +285,6 @@ static void dap_mem_write_sized(
 	DEBUG_WIRE("memwrite done\n");
 }
 
-int dap_enter_debug_swd(ADIv5_DP_t *dp)
-{
-	target_list_free();
-	if (!(dap_caps & DAP_CAP_SWD))
-		return -1;
-	mode =  DAP_CAP_SWD;
-	dap_transfer_configure(2, 128, 128);
-	dap_swd_configure(0);
-	dap_connect(false);
-	dap_led(0, 1);
-	dap_reset_link(false);
-
-	dp->idcode = dap_read_idcode(dp);
-	dp->dp_read = dap_dp_read_reg;
-	dp->error = dap_dp_error;
-	dp->low_access =  dap_dp_low_access;
-	dp->abort = dap_dp_abort; /* DP Write to Reg 0.*/
-	return 0;
-}
-
 void dap_adiv5_dp_defaults(ADIv5_DP_t *dp)
 {
 	if ((mode == DAP_CAP_JTAG) && dap_jtag_configure())
@@ -355,12 +339,12 @@ int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc)
 	mode =  DAP_CAP_JTAG;
 	dap_disconnect();
 	dap_connect(true);
+	dap_reset_link(true);
 	jtag_proc->jtagtap_reset       = cmsis_dap_jtagtap_reset;
 	jtag_proc->jtagtap_next        = cmsis_dap_jtagtap_next;
 	jtag_proc->jtagtap_tms_seq     = cmsis_dap_jtagtap_tms_seq;
 	jtag_proc->jtagtap_tdi_tdo_seq = cmsis_dap_jtagtap_tdi_tdo_seq;
 	jtag_proc->jtagtap_tdi_seq     = cmsis_dap_jtagtap_tdi_seq;
-	dap_reset_link(true);
 	return 0;
 }
 
@@ -373,3 +357,87 @@ int dap_jtag_dp_init(ADIv5_DP_t *dp)
 
 	return true;
 }
+
+#define SWD_SEQUENCE_IN 0x80
+#define DAP_SWD_SEQUENCE 0x1d
+/* DAP_SWD_SEQUENCE does not do auto turnaround*/
+static bool dap_dp_low_read(ADIv5_DP_t *dp, uint16_t addr, uint32_t *res)
+{
+	(void)dp;
+	unsigned int paket_request = make_packet_request(ADIV5_LOW_READ, addr);
+	uint8_t buf[32] = {
+		DAP_SWD_SEQUENCE,
+		5,
+		8,
+		paket_request,
+		4 + SWD_SEQUENCE_IN,  /* one turn-around + read 3 bit ACK */
+		32 + SWD_SEQUENCE_IN, /* read 32 bit data */
+		1 + SWD_SEQUENCE_IN,  /* read parity bit */
+		1,                    /* one bit turn around to drive SWDIO */
+		0
+	};
+	dbg_dap_cmd(buf, sizeof(buf), 9);
+	if (buf[0])
+		DEBUG_WARN("dap_dp_low_read failed\n");
+	uint32_t ack = (buf[1] >> 1) & 7;
+	uint32_t data = (buf[2] << 0) + (buf[3] << 8) + (buf[4] << 16)
+		+ (buf[5] << 24);
+	int parity = __builtin_parity(data);
+	bool ret = ((parity != buf[6]) || (ack != 1));
+	*res = data;
+	DEBUG_PROBE("dap_dp_low_read ack %d, res %08" PRIx32 ", parity %s\n", ack,
+			   data, (ret)? "ERR": "OK");
+	return ret;
+}
+
+static bool dap_dp_low_write(ADIv5_DP_t *dp, uint16_t addr, const uint32_t data)
+{
+	DEBUG_PROBE("dap_dp_low_write %08" PRIx32 "\n", data);
+	(void)dp;
+	unsigned int paket_request = make_packet_request(ADIV5_LOW_WRITE, addr);
+	uint8_t buf[32] = {
+		DAP_SWD_SEQUENCE,
+		5,
+		8,
+		paket_request,
+		4 + SWD_SEQUENCE_IN,  /* one turn-around + read 3 bit ACK */
+		1,                    /* one bit turn around to drive SWDIO */
+		0,
+		32,                   /* write 32 bit data */
+		(data >>  0) & 0xff,
+		(data >>  8) & 0xff,
+		(data >> 16) & 0xff,
+		(data >> 24) & 0xff,
+		1,                    /* write parity biT */
+		__builtin_parity(data)
+	};
+	dbg_dap_cmd(buf, sizeof(buf), 14);
+	if (buf[0])
+		DEBUG_WARN("dap_dp_low_write failed\n");
+	uint32_t ack = (buf[1] >> 1) & 7;
+	return (ack != SWDP_ACK_OK);
+}
+
+int dap_swdptap_init(ADIv5_DP_t *dp)
+{
+	if (!(dap_caps & DAP_CAP_SWD))
+		return 1;
+	mode =  DAP_CAP_SWD;
+	dap_transfer_configure(2, 128, 128);
+	dap_swd_configure(0);
+	dap_connect(false);
+	dap_led(0, 1);
+	dap_reset_link(false);
+	if (has_swd_sequence) {
+		/* DAP_SWD_SEQUENCE does not do auto turnaround, use own!*/
+		dp->dp_low_read = dap_dp_low_read;
+		dp->dp_low_write = dap_dp_low_write;
+	}
+	dp->seq_out = dap_swdptap_seq_out;
+	dp->seq_out_parity = dap_swdptap_seq_out_parity;
+	dp->dp_read = dap_dp_read_reg;
+	/* For error() use the TARGETID switching firmware_swdp_error */
+	dp->low_access = dap_dp_low_access;
+	dp->abort = dap_dp_abort;
+	return 0;
+}

src/platforms/hosted/cmsis_dap.h

@@ -1,7 +1,7 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2019 Uwe Bonnes
+ * Copyright (C) 2019 - 2021 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
@@ -24,12 +24,13 @@
 
 #if defined(CMSIS_DAP)
 int dap_init(bmp_info_t *info);
-int dap_enter_debug_swd(ADIv5_DP_t *dp);
 void dap_exit_function(void);
 void dap_adiv5_dp_defaults(ADIv5_DP_t *dp);
 int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc);
+int dap_swdptap_init(ADIv5_DP_t *dp);
 int dap_jtag_dp_init(ADIv5_DP_t *dp);
 uint32_t dap_swj_clock(uint32_t clock);
+void dap_swd_configure(uint8_t cfg);
 #else
 int dap_init(bmp_info_t *info)
 {
@@ -39,13 +40,14 @@ int dap_init(bmp_info_t *info)
 }
 # pragma GCC diagnostic push
 # pragma GCC diagnostic ignored "-Wunused-parameter"
-int dap_enter_debug_swd(ADIv5_DP_t *dp) {return -1;}
 uint32_t dap_swj_clock(uint32_t clock) {return 0;}
 void dap_exit_function(void) {};
 void dap_adiv5_dp_defaults(ADIv5_DP_t *dp) {};
 int cmsis_dap_jtagtap_init(jtag_proc_t *jtag_proc) {return -1;}
+int dap_swdptap_init(swd_proc_t *swd_proc) {return -1;}
 int dap_jtag_dp_init(ADIv5_DP_t *dp) {return -1;}
-# pragma GCC diagnostic pop
+void dap_swd_configure(uint8_t cfg) {};
+## pragma GCC diagnostic pop
 
 #endif
 

src/platforms/hosted/dap.c

@@ -27,7 +27,7 @@
  */
 
 /* Modified for Blackmagic Probe
- * Copyright (c) 2020 Uwe Bonnes bon@elektron.ikp.physik.tu-darmstadt.de
+ * Copyright (c) 2020-21 Uwe Bonnes bon@elektron.ikp.physik.tu-darmstadt.de
  */
 
 /*- Includes ----------------------------------------------------------------*/
@@ -57,6 +57,7 @@ enum
 	ID_DAP_JTAG_SEQUENCE      = 0x14,
 	ID_DAP_JTAG_CONFIGURE     = 0x15,
 	ID_DAP_JTAG_IDCODE        = 0x16,
+	ID_DAP_SWD_SEQUENCE       = 0x1D,
 };
 
 enum
@@ -197,7 +198,7 @@ void dap_connect(bool jtag)
 //-----------------------------------------------------------------------------
 void dap_disconnect(void)
 {
-	uint8_t buf[1];
+	uint8_t buf[65];
 
 	buf[0] = ID_DAP_DISCONNECT;
 	dbg_dap_cmd(buf, sizeof(buf), 1);
@@ -212,7 +213,7 @@ uint32_t dap_swj_clock(uint32_t clock)
 {
 	if (clock == 0)
 		return swj_clock;
-	uint8_t buf[5];
+	uint8_t buf[65];
 	buf[0] = ID_DAP_SWJ_CLOCK;
 	buf[1] = clock & 0xff;
 	buf[2] = (clock >> 8) & 0xff;
@@ -253,7 +254,7 @@ void dap_swd_configure(uint8_t cfg)
 //-----------------------------------------------------------------------------
 int dap_info(int info, uint8_t *data, int size)
 {
-	uint8_t buf[256];
+	uint8_t buf[32];
 	int rsize;
 
 	buf[0] = ID_DAP_INFO;
@@ -750,3 +751,34 @@ int dap_jtag_configure(void)
 		DEBUG_WARN("dap_jtag_configure Failed %02x\n", buf[0]);
 	return 0;
 }
+
+void dap_swdptap_seq_out(uint32_t MS, int ticks)
+{
+	uint8_t buf[] = {
+		ID_DAP_SWJ_SEQUENCE,
+		ticks,
+		(MS >>  0) & 0xff,
+		(MS >>  8) & 0xff,
+		(MS >> 16) & 0xff,
+		(MS >> 24) & 0xff
+	};
+	dbg_dap_cmd(buf, 1, sizeof(buf));
+	if (buf[0])
+		DEBUG_WARN("dap_swdptap_seq_out error\n");
+}
+
+void dap_swdptap_seq_out_parity(uint32_t MS, int ticks)
+{
+	uint8_t buf[] = {
+		ID_DAP_SWJ_SEQUENCE,
+		ticks + 1,
+		(MS >>  0) & 0xff,
+		(MS >>  8) & 0xff,
+		(MS >> 16) & 0xff,
+		(MS >> 24) & 0xff,
+		__builtin_parity(MS) & 1
+	};
+	dbg_dap_cmd(buf, 1, sizeof(buf));
+	if (buf[0])
+		DEBUG_WARN("dap_swdptap_seq_out error\n");
+}

src/platforms/hosted/dap.h

@@ -90,4 +90,6 @@ int dbg_dap_cmd(uint8_t *data, int size, int rsize);
 void dap_jtagtap_tdi_tdo_seq(uint8_t *DO, bool final_tms, const uint8_t *TMS,
 							 const uint8_t *DI, int ticks);
 int dap_jtag_configure(void);
+void dap_swdptap_seq_out(uint32_t MS, int ticks);
+void dap_swdptap_seq_out_parity(uint32_t MS, int ticks);
 #endif // _DAP_H_

src/platforms/hosted/ftdi_bmp.h

@@ -23,7 +23,6 @@
 #define __FTDI_BMP_H
 
 #include "cl_utils.h"
-#include "swdptap.h"
 #include "jtagtap.h"
 
 #include "bmp_hosted.h"
@@ -104,7 +103,7 @@ typedef struct cable_desc_s {
 # pragma GCC diagnostic push
 # pragma GCC diagnostic ignored "-Wunused-parameter"
 int ftdi_bmp_init(BMP_CL_OPTIONS_t *cl_opts, bmp_info_t *info) {return -1;};
-int libftdi_swdptap_init(swd_proc_t *swd_proc) {return -1;};
+int libftdi_swdptap_init(ADIv5_DP_t *dp) {return -1;};
 int libftdi_jtagtap_init(jtag_proc_t *jtag_proc) {return 0;};
 void libftdi_buffer_flush(void) {};
 int libftdi_buffer_write(const uint8_t *data, int size) {return size;};
@@ -124,7 +123,7 @@ extern struct ftdi_context *ftdic;
 extern data_desc_t active_state;
 
 int ftdi_bmp_init(BMP_CL_OPTIONS_t *cl_opts, bmp_info_t *info);
-int libftdi_swdptap_init(swd_proc_t *swd_proc);
+int libftdi_swdptap_init(ADIv5_DP_t *dp);
 int libftdi_jtagtap_init(jtag_proc_t *jtag_proc);
 void libftdi_buffer_flush(void);
 int libftdi_buffer_write(const uint8_t *data, int size);

src/platforms/hosted/jlink_adiv5_swdp.c

@@ -3,7 +3,7 @@
  *
  * Copyright (C) 2011  Black Sphere Technologies Ltd.
  * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- * Copyright (C) 2019 - 2020 Uwe Bonnes
+ * Copyright (C) 2019 - 2021 Uwe Bonnes
  *                           (bon@elektron.ikp.physik.tu-darmstadt.de)
  *
  * This program is free software: you can redistribute it and/or modify
@@ -103,7 +103,7 @@ static int line_reset(bmp_info_t *info)
         return 0;
 }
 
-static int swdptap_init(bmp_info_t *info)
+static int jlink_swdptap_init(bmp_info_t *info)
 {
 	uint8_t cmd[2] = {CMD_GET_SELECT_IF, JLINK_IF_GET_AVAILABLE};
 	uint8_t res[4];
@@ -119,7 +119,7 @@ static int swdptap_init(bmp_info_t *info)
 
 int jlink_swdp_scan(bmp_info_t *info)
 {
-	swdptap_init(info);
+	jlink_swdptap_init(info);
 	target_list_free();
 	uint8_t cmd[44];
 	cmd[0]  = CMD_HW_JTAG3;

src/platforms/hosted/libftdi_swdptap.c

@@ -1,8 +1,7 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2018 Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
+ * Copyright(C) 2018 - 2021 Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
- * Written by Gareth McMullin <gareth@blacksphere.co.nz>
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -168,7 +167,7 @@ bool libftdi_swd_possible(bool *do_mpsse, bool *direct_bb_swd)
 	return true;
 }
 
-int libftdi_swdptap_init(swd_proc_t *swd_proc)
+int libftdi_swdptap_init(ADIv5_DP_t *dp)
 {
 	if (!libftdi_swd_possible(&do_mpsse, &direct_bb_swd)) {
 		DEBUG_WARN("SWD not possible or missing item in cable description.\n");
@@ -208,11 +207,14 @@ int libftdi_swdptap_init(swd_proc_t *swd_proc)
 	libftdi_buffer_flush();
 	olddir = SWDIO_STATUS_FLOAT;
 
-	swd_proc->swdptap_seq_in  = swdptap_seq_in;
+	dp->seq_in  = swdptap_seq_in;
-	swd_proc->swdptap_seq_in_parity  = swdptap_seq_in_parity;
+	dp->seq_in_parity  = swdptap_seq_in_parity;
-	swd_proc->swdptap_seq_out = swdptap_seq_out;
+	dp->seq_out = swdptap_seq_out;
-	swd_proc->swdptap_seq_out_parity  = swdptap_seq_out_parity;
+	dp->seq_out_parity  = swdptap_seq_out_parity;
-
+	dp->dp_read = firmware_swdp_read;
+	dp->error = firmware_swdp_error;
+	dp->low_access = firmware_swdp_low_access;
+	dp->abort = firmware_swdp_abort;
 	return 0;
 }
 

src/platforms/hosted/platform.c

@@ -21,7 +21,6 @@
  */
 
 #include "general.h"
-#include "swdptap.h"
 #include "jtagtap.h"
 #include "target.h"
 #include "target_internal.h"
@@ -41,7 +40,6 @@
 
 bmp_info_t info;
 
-swd_proc_t swd_proc;
 jtag_proc_t jtag_proc;
 
 void gdb_ident(char *p, int count)
@@ -128,6 +126,7 @@ int platform_adiv5_swdp_scan(uint32_t targetid)
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
 	case BMP_TYPE_LIBFTDI:
+	case BMP_TYPE_CMSIS_DAP:
 		return adiv5_swdp_scan(targetid);
 		break;
 	case BMP_TYPE_STLINKV2:
@@ -143,19 +142,6 @@ int platform_adiv5_swdp_scan(uint32_t targetid)
 		}
 		break;
 	}
-	case BMP_TYPE_CMSIS_DAP:
-	{
-		target_list_free();
-		ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
-		if (dap_enter_debug_swd(dp)) {
-			free(dp);
-		} else {
-			adiv5_dp_init(dp);
-			if (target_list)
-				return 1;
-		}
-		break;
-	}
 	case BMP_TYPE_JLINK:
 		return jlink_swdp_scan(&info);
 	default:
@@ -164,17 +150,18 @@ int platform_adiv5_swdp_scan(uint32_t targetid)
 	return 0;
 }
 
-int platform_swdptap_init(void)
+int swdptap_init(ADIv5_DP_t *dp)
 {
 	switch (info.bmp_type) {
 	case BMP_TYPE_BMP:
-		return remote_swdptap_init(&swd_proc);
+		return remote_swdptap_init(dp);
-	case BMP_TYPE_STLINKV2:
 	case BMP_TYPE_CMSIS_DAP:
+		return dap_swdptap_init(dp);
+	case BMP_TYPE_STLINKV2:
 	case BMP_TYPE_JLINK:
 		return 0;
 	case BMP_TYPE_LIBFTDI:
-		return libftdi_swdptap_init(&swd_proc);
+		return libftdi_swdptap_init(dp);
 	default:
 		return -1;
 	}

src/platforms/hosted/remote_swdptap.c

@@ -3,7 +3,7 @@
  *
  * Written by Gareth McMullin <gareth@blacksphere.co.nz>
  * Modified by Dave Marples <dave@marples.net>
- * Modification (C) 2020 Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
+ * Modified 2020 - 2021 by Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -35,7 +35,7 @@ static uint32_t swdptap_seq_in(int ticks);
 static void swdptap_seq_out(uint32_t MS, int ticks);
 static void swdptap_seq_out_parity(uint32_t MS, int ticks);
 
-int remote_swdptap_init(swd_proc_t *swd_proc)
+int remote_swdptap_init(ADIv5_DP_t *dp)
 {
 	DEBUG_WIRE("remote_swdptap_init\n");
 	uint8_t construct[REMOTE_MAX_MSG_SIZE];
@@ -50,11 +50,14 @@ int remote_swdptap_init(swd_proc_t *swd_proc)
 		exit(-1);
     }
 
-	swd_proc->swdptap_seq_in  = swdptap_seq_in;
+	dp->seq_in  = swdptap_seq_in;
-	swd_proc->swdptap_seq_in_parity  = swdptap_seq_in_parity;
+	dp->seq_in_parity  = swdptap_seq_in_parity;
-	swd_proc->swdptap_seq_out = swdptap_seq_out;
+	dp->seq_out = swdptap_seq_out;
-	swd_proc->swdptap_seq_out_parity  = swdptap_seq_out_parity;
+	dp->seq_out_parity  = swdptap_seq_out_parity;
-
+	dp->dp_read = firmware_swdp_read;
+	dp->error = firmware_swdp_error;
+	dp->low_access = firmware_swdp_low_access;
+	dp->abort = firmware_swdp_abort;
   return 0;
 }
 

src/remote.c

@@ -3,6 +3,7 @@
  *
  * Copyright (C) 2019  Black Sphere Technologies Ltd.
  * Written by Dave Marples <dave@marples.net>
+ * Modified 2020 - 2021 by Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -21,7 +22,6 @@
 #include "general.h"
 #include "remote.h"
 #include "gdb_packet.h"
-#include "swdptap.h"
 #include "jtagtap.h"
 #include "gdb_if.h"
 #include "platform.h"
@@ -145,7 +145,7 @@ void remotePacketProcessSWD(uint8_t i, char *packet)
 		if (i==2) {
 			remote_dp.dp_read = firmware_swdp_read;
 			remote_dp.low_access = firmware_swdp_low_access;
-			swdptap_init();
+			swdptap_init(&remote_dp);
 			_respond(REMOTE_RESP_OK, 0);
 		} else {
 			_respond(REMOTE_RESP_ERR,REMOTE_ERROR_WRONGLEN);
@@ -154,27 +154,27 @@ void remotePacketProcessSWD(uint8_t i, char *packet)
 
     case REMOTE_IN_PAR: /* SI = In parity ============================= */
 		ticks=remotehston(2,&packet[2]);
-		badParity = swd_proc.swdptap_seq_in_parity(&param, ticks);
+		badParity = remote_dp.seq_in_parity(&param, ticks);
 		_respond(badParity?REMOTE_RESP_PARERR:REMOTE_RESP_OK,param);
 		break;
 
     case REMOTE_IN: /* Si = In ======================================= */
 		ticks=remotehston(2,&packet[2]);
-		param = swd_proc.swdptap_seq_in(ticks);
+		param = remote_dp.seq_in(ticks);
 		_respond(REMOTE_RESP_OK,param);
 		break;
 
     case REMOTE_OUT: /* So= Out ====================================== */
 		ticks=remotehston(2,&packet[2]);
 		param=remotehston(-1, &packet[4]);
-		swd_proc.swdptap_seq_out(param, ticks);
+		remote_dp.seq_out(param, ticks);
 		_respond(REMOTE_RESP_OK, 0);
 		break;
 
     case REMOTE_OUT_PAR: /* SO = Out parity ========================== */
 		ticks=remotehston(2,&packet[2]);
 		param=remotehston(-1, &packet[4]);
-		swd_proc.swdptap_seq_out_parity(param, ticks);
+		remote_dp.seq_out_parity(param, ticks);
 		_respond(REMOTE_RESP_OK, 0);
 		break;
 

src/target/adiv5.c

@@ -3,7 +3,7 @@
  *
  * Copyright (C) 2015  Black Sphere Technologies Ltd.
  * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- * Copyright (C) 2018 - 2020 Uwe Bonnes
+ * Copyright (C) 2018 - 2021 Uwe Bonnes
  *                           (bon@elektron.ikp.physik.tu-darmstadt.de)
  *
  * This program is free software: you can redistribute it and/or modify
@@ -620,6 +620,21 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 	return ap;
 }
 
+/* No real AP on RP2040. Special setup.*/
+static void rp_rescue_setup(ADIv5_DP_t *dp)
+{
+	ADIv5_AP_t *ap = malloc(sizeof(*ap));
+	if (!ap) {			/* malloc failed: heap exhaustion */
+		DEBUG_WARN("malloc: failed in %s\n", __func__);
+		return;
+	}
+	memset(ap, 0, sizeof(ADIv5_AP_t));
+	ap->dp = dp;
+	extern void rp_rescue_probe(ADIv5_AP_t *);
+	rp_rescue_probe(ap);
+	return;
+}
+
 void adiv5_dp_init(ADIv5_DP_t *dp)
 {
 #define DPIDR_PARTNO_MASK 0x0ff00000
@@ -630,8 +645,11 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		free(dp);
 		return;
 	}
-	DEBUG_INFO("DPIDR 0x%08" PRIx32 " (v%" PRId32 " %srev%" PRId32 ")\n",
+	if (dp->idcode == 0x10212927) {
-			   dp->idcode,
+		rp_rescue_setup(dp);
+		return;
+	}
+	DEBUG_INFO("DPIDR 0x%08" PRIx32 " (v%d %srev%d)\n", dp->idcode,
 			   (dp->idcode >> 12) & 0xf,
 			   (dp->idcode & 0x10000) ? "MINDP " : "", dp->idcode >> 28);
 	volatile uint32_t ctrlstat = 0;
@@ -707,13 +725,6 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		}
 	}
 
-	if ((dp->idcode & ADIV5_DP_VERSION_MASK) == ADIV5_DPv2) {
-		/* Read TargetID. Can be done with device in WFI, sleep or reset!*/
-		adiv5_dp_write(dp, ADIV5_DP_SELECT, ADIV5_DP_BANK2);
-		dp->targetid = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT);
-		adiv5_dp_write(dp, ADIV5_DP_SELECT, ADIV5_DP_BANK0);
-		DEBUG_INFO("TARGETID %08" PRIx32 "\n", dp->targetid);
-	}
 	/* Probe for APs on this DP */
 	uint32_t last_base = 0;
 	int void_aps = 0;

src/target/adiv5.h

@@ -27,18 +27,20 @@
 #define ADIV5_DP_REG(x)   (x)
 #define ADIV5_AP_REG(x)   (ADIV5_APnDP | (x))
 
+#define ADIV5_DP_BANK0    0x00
+#define ADIV5_DP_BANK1    0x10
+#define ADIV5_DP_BANK2    0x20
+#define ADIV5_DP_BANK3    0x30
+#define ADIV5_DP_BANK4    0x40
+
 /* ADIv5 DP Register addresses */
 #define ADIV5_DP_IDCODE   ADIV5_DP_REG(0x0)
 #define ADIV5_DP_ABORT    ADIV5_DP_REG(0x0)
 #define ADIV5_DP_CTRLSTAT ADIV5_DP_REG(0x4)
+#define ADIV5_DP_TARGETID  (ADIV5_DP_BANK2 | ADIV5_DP_REG(0x4))
 #define ADIV5_DP_SELECT   ADIV5_DP_REG(0x8)
 #define ADIV5_DP_RDBUFF   ADIV5_DP_REG(0xC)
-
+#define ADIV5_DP_TARGETSEL ADIV5_DP_REG(0xC)
-#define ADIV5_DP_BANK0    0
-#define ADIV5_DP_BANK1    1
-#define ADIV5_DP_BANK2    2
-#define ADIV5_DP_BANK3    3
-#define ADIV5_DP_BANK4    4
 
 #define ADIV5_DP_VERSION_MASK 0xf000
 #define ADIV5_DPv1            0x1000
@@ -86,7 +88,8 @@
 #define ADIV5_AP_BASE		ADIV5_AP_REG(0xF8)
 #define ADIV5_AP_IDR		ADIV5_AP_REG(0xFC)
 
-/* Known designers seen in SYSROM-PIDR. Ignore Bit 7 from the designer bits*/
+/* Known designers seen in SYSROM-PIDR. Ignore Bit 0 from
+ * the designer bits to get JEDEC Ids with bit 7 ignored.*/
 #define AP_DESIGNER_FREESCALE    0x00e
 #define AP_DESIGNER_TEXAS        0x017
 #define AP_DESIGNER_ATMEL        0x01f
@@ -100,6 +103,7 @@
 #define AP_DESIGNER_CS           0x555
 #define AP_DESIGNER_ENERGY_MICRO 0x673
 #define AP_DESIGNER_GIGADEVICE   0x751
+#define AP_DESIGNER_RASPBERRY    0x927
 
 /* AP Control and Status Word (CSW) */
 #define ADIV5_AP_CSW_DBGSWENABLE	(1u << 31)
@@ -137,6 +141,10 @@
 #define ADIV5_LOW_WRITE		0
 #define ADIV5_LOW_READ		1
 
+#define SWDP_ACK_OK    0x01
+#define SWDP_ACK_WAIT  0x02
+#define SWDP_ACK_FAULT 0x04
+
 enum align {
 	ALIGN_BYTE     = 0,
 	ALIGN_HALFWORD = 1,
@@ -153,6 +161,15 @@ typedef struct ADIv5_DP_s {
 	uint32_t idcode;
 	uint32_t targetid;  /* Contains IDCODE for DPv2 devices.*/
 
+	void (*seq_out)(uint32_t MS, int ticks);
+	void (*seq_out_parity)(uint32_t MS, int ticks);
+	uint32_t (*seq_in)(int ticks);
+	bool (*seq_in_parity)(uint32_t *ret, int ticks);
+	/* dp_low_write returns true if no OK resonse. */
+	bool (*dp_low_write)(struct ADIv5_DP_s *dp, uint16_t addr,
+						 const uint32_t data);
+	/* dp_low_read returns true with parity error */
+	bool (*dp_low_read)(struct ADIv5_DP_s *dp, uint16_t addr, uint32_t *data);
 	uint32_t (*dp_read)(struct ADIv5_DP_s *dp, uint16_t addr);
 	uint32_t (*error)(struct ADIv5_DP_s *dp);
 	uint32_t (*low_access)(struct ADIv5_DP_s *dp, uint8_t RnW,
@@ -195,6 +212,8 @@ struct ADIv5_AP_s {
 	uint16_t ap_partno;
 };
 
+unsigned int make_packet_request(uint8_t RnW, uint16_t addr);
+
 #if PC_HOSTED == 0
 static inline uint32_t adiv5_dp_read(ADIv5_DP_t *dp, uint16_t addr)
 {
@@ -269,6 +288,7 @@ void platform_add_jtag_dev(const int dev_index, const jtag_dev_t *jtag_dev);
 
 void adiv5_jtag_dp_handler(uint8_t jd_index, uint32_t j_idcode);
 int platform_jtag_dp_init(ADIv5_DP_t *dp);
+int swdptap_init(ADIv5_DP_t *dp);
 
 void adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len);
 uint64_t adiv5_ap_read_pidr(ADIv5_AP_t *ap, uint32_t addr);

src/target/adiv5_swdp.c

@@ -26,15 +26,10 @@
 #include "general.h"
 #include "exception.h"
 #include "adiv5.h"
-#include "swdptap.h"
 #include "target.h"
 #include "target_internal.h"
 
-#define SWDP_ACK_OK    0x01
+unsigned int make_packet_request(uint8_t RnW, uint16_t addr)
-#define SWDP_ACK_WAIT  0x02
-#define SWDP_ACK_FAULT 0x04
-
-static unsigned int make_packet_request(uint8_t  RnW, uint16_t addr)
 {
 	bool APnDP = addr & ADIV5_APnDP;
 	addr &= 0xff;
@@ -49,56 +44,144 @@ static unsigned int make_packet_request(uint8_t  RnW, uint16_t addr)
 	return request;
 }
 
+/* Provide bare DP access functions without timeout and exception */
+
+static void dp_line_reset(ADIv5_DP_t *dp)
+{
+	dp->seq_out(0xFFFFFFFF, 32);
+	dp->seq_out(0x0FFFFFFF, 32);
+}
+
+bool firmware_dp_low_write(ADIv5_DP_t *dp, uint16_t addr, const uint32_t data)
+{
+	unsigned int request =  make_packet_request(ADIV5_LOW_WRITE, addr & 0xf);
+	dp->seq_out(request, 8);
+	int res = dp->seq_in(3);
+	dp->seq_out_parity(data, 32);
+	return (res != 1);
+}
+
+static bool firmware_dp_low_read(ADIv5_DP_t *dp, uint16_t addr, uint32_t *res)
+{
+	unsigned int request =  make_packet_request(ADIV5_LOW_READ, addr & 0xf);
+	dp->seq_out(request, 8);
+	dp->seq_in(3);
+    return dp->seq_in_parity(res, 32);
+}
+
+/* Try first the dormant to SWD procedure.
+ * If target id given, scan DPs 0 .. 15 on that device and return.
+ * Otherwise
+ */
 int adiv5_swdp_scan(uint32_t targetid)
 {
-	uint32_t ack;
-	(void) targetid;
 	target_list_free();
-#if PC_HOSTED == 1
+	ADIv5_DP_t idp, *initial_dp = &idp;
-	if (platform_swdptap_init()) {
+	memset(initial_dp, 0, sizeof(ADIv5_DP_t));
-		exit(-1);
+	if (swdptap_init(initial_dp))
-	}
-#else
-	if (swdptap_init()) {
 		return -1;
+	/* Set defaults when no procedure given*/
+	if (!initial_dp->dp_low_write)
+		initial_dp->dp_low_write = firmware_dp_low_write;
+	if (!initial_dp->dp_low_read)
+		initial_dp->dp_low_read = firmware_dp_low_read;
+	if (!initial_dp->error)
+		initial_dp->error = firmware_swdp_error;
+	if (!initial_dp->dp_read)
+		initial_dp->dp_read = firmware_swdp_read;
+	if (!initial_dp->error)
+		initial_dp->error = firmware_swdp_error;
+	if (!initial_dp->low_access)
+       initial_dp->low_access = firmware_swdp_low_access;
+	/* DORMANT-> SWD sequence*/
+	initial_dp->seq_out(0xFFFFFFFF, 32);
+	initial_dp->seq_out(0xFFFFFFFF, 32);
+	/* 128 bit selection alert sequence for SW-DP-V2 */
+	initial_dp->seq_out(0x6209f392, 32);
+	initial_dp->seq_out(0x86852d95, 32);
+	initial_dp->seq_out(0xe3ddafe9, 32);
+	initial_dp->seq_out(0x19bc0ea2, 32);
+	/* 4 cycle low,
+	 * 0x1a Arm CoreSight SW-DP activation sequence
+	 * 20 bits start of reset another reset sequence*/
+	initial_dp->seq_out(0x1a0, 12);
+	uint32_t idcode = 0;
+	volatile uint32_t target_id;
+	bool is_v2 = true;
+	if (!targetid) {
+		/* Try to read ID */
+		dp_line_reset(initial_dp);
+		volatile struct exception e;
+		TRY_CATCH (e, EXCEPTION_ALL) {
+			idcode = initial_dp->low_access(initial_dp, ADIV5_LOW_READ,
+										   ADIV5_DP_IDCODE, 0);
+		}
+		if (e.type || initial_dp->fault) {
+			is_v2 = false;
+			DEBUG_WARN("Trying old JTAG to SWD sequence\n");
+			initial_dp->seq_out(0xFFFFFFFF, 32);
+			initial_dp->seq_out(0xFFFFFFFF, 32);
+			initial_dp->seq_out(0xE79E, 16); /* 0b0111100111100111 */
+			dp_line_reset(initial_dp);
+			initial_dp->fault = 0;
+			volatile struct exception e;
+			TRY_CATCH (e, EXCEPTION_ALL) {
+				idcode = initial_dp->low_access(initial_dp, ADIV5_LOW_READ,
+									  ADIV5_DP_IDCODE, 0);
+			}
+			if (e.type) {
+				DEBUG_WARN("No usable DP found\n");
+				return -1;
+			}
+		}
+		if ((idcode & ADIV5_DP_VERSION_MASK) == ADIV5_DPv2) {
+			is_v2 = true;
+			/* Read TargetID. Can be done with device in WFI, sleep or reset!*/
+			adiv5_dp_write(initial_dp, ADIV5_DP_SELECT, 2);
+			target_id = adiv5_dp_read(initial_dp, ADIV5_DP_CTRLSTAT);
+			adiv5_dp_write(initial_dp, ADIV5_DP_SELECT, 0);
+			DEBUG_INFO("TARGETID %08" PRIx32 "\n", target_id);
+			switch (target_id) {
+			case 0x01002927: /* RP2040 */
+				/* Release evt. handing RESCUE DP reset*/
+				adiv5_dp_write(initial_dp, ADIV5_DP_CTRLSTAT, 0);
+				break;
+			}
+			if (!initial_dp->dp_low_read)
+				/* E.g. CMSIS_DAP < V1.2 can not handle multu-drop!*/
+				is_v2 = false;
+		} else {
+			is_v2 = false;
+		}
+	} else {
+		target_id = targetid;
 	}
-#endif
+	int nr_dps = (is_v2) ? 16: 1;
+	uint32_t dp_targetid;
+	for (int i = 0; i < nr_dps; i++) {
+		if (is_v2) {
+			dp_line_reset(initial_dp);
+			dp_targetid = (i << 28) | (target_id & 0x0fffffff);
+			initial_dp->dp_low_write(initial_dp, ADIV5_DP_TARGETSEL,
+									dp_targetid);
+			if (initial_dp->dp_low_read(initial_dp, ADIV5_DP_IDCODE,
+										&idcode)) {
+				continue;
+			}
+		} else {
+			dp_targetid = 0;
+		}
+		ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
+		if (!dp) {			/* calloc failed: heap exhaustion */
+			DEBUG_WARN("calloc: failed in %s\n", __func__);
+			continue;
+		}
+		memcpy(dp, initial_dp, sizeof(ADIv5_DP_t));
+		dp->idcode = idcode;
+		dp->targetid = dp_targetid;
+		adiv5_dp_init(dp);
 
-	/* Switch from JTAG to SWD mode */
-	swd_proc.swdptap_seq_out(0xFFFFFFFF, 16);
-	swd_proc.swdptap_seq_out(0xFFFFFFFF, 32);
-	swd_proc.swdptap_seq_out(0xFFFFFFFF, 18);
-	swd_proc.swdptap_seq_out(0xE79E, 16); /* 0b0111100111100111 */
-	swd_proc.swdptap_seq_out(0xFFFFFFFF, 32);
-	swd_proc.swdptap_seq_out(0xFFFFFFFF, 18);
-	swd_proc.swdptap_seq_out(0, 16);
-
-	/* Read the SW-DP IDCODE register to syncronise */
-	/* This could be done with adiv_swdp_low_access(), but this doesn't
-	 * allow the ack to be checked here. */
-	uint32_t request = make_packet_request(ADIV5_LOW_READ, ADIV5_DP_IDCODE);
-	swd_proc.swdptap_seq_out(request, 8);
-	ack = swd_proc.swdptap_seq_in(3);
-	uint32_t idcode;
-	if((ack != SWDP_ACK_OK) || swd_proc.swdptap_seq_in_parity(&idcode, 32)) {
-		DEBUG_WARN("Read SW-DP IDCODE failed %1" PRIx32 "\n", ack);
-		return -1;
 	}
-
-	ADIv5_DP_t *dp = (void*)calloc(1, sizeof(*dp));
-	if (!dp) {			/* calloc failed: heap exhaustion */
-		DEBUG_WARN("calloc: failed in %s\n", __func__);
-		return -1;
-	}
-
-	dp->idcode = idcode;
-	dp->dp_read = firmware_swdp_read;
-	dp->error = firmware_swdp_error;
-	dp->low_access = firmware_swdp_low_access;
-	dp->abort = firmware_swdp_abort;
-
-	firmware_swdp_error(dp);
-	adiv5_dp_init(dp);
 	return target_list?1:0;
 }
 
@@ -117,7 +200,16 @@ uint32_t firmware_swdp_read(ADIv5_DP_t *dp, uint16_t addr)
 {
 	uint32_t err, clr = 0;
 
-	err = firmware_swdp_read(dp, ADIV5_DP_CTRLSTAT) &
+	if ((dp->idcode & ADIV5_DP_VERSION_MASK) == ADIV5_DPv2) {
+		/* On protocoll error target gets deselected.
+		 * With DP Change, another target needs selection.
+		 * => Reselect with right target! */
+		dp_line_reset(dp);
+		dp->dp_low_write(dp, ADIV5_DP_TARGETSEL, dp->targetid);
+		uint32_t dummy;
+		dp->dp_low_read(dp, ADIV5_DP_IDCODE, &dummy);
+	}
+	err = adiv5_dp_read(dp, ADIV5_DP_CTRLSTAT) &
 		(ADIV5_DP_CTRLSTAT_STICKYORUN | ADIV5_DP_CTRLSTAT_STICKYCMP |
 		ADIV5_DP_CTRLSTAT_STICKYERR | ADIV5_DP_CTRLSTAT_WDATAERR);
 
@@ -148,13 +240,11 @@ uint32_t firmware_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 
 	platform_timeout_set(&timeout, 2000);
 	do {
-		swd_proc.swdptap_seq_out(request, 8);
+		dp->seq_out(request, 8);
-		ack = swd_proc.swdptap_seq_in(3);
+		ack = dp->seq_in(3);
 		if (ack == SWDP_ACK_FAULT) {
-			/* On fault, abort() and repeat the command once.*/
+			dp->fault = 1;
-			firmware_swdp_error(dp);
+			return 0;
-			swd_proc.swdptap_seq_out(request, 8);
-			ack = swd_proc.swdptap_seq_in(3);
 		}
 	} while (ack == SWDP_ACK_WAIT && !platform_timeout_is_expired(&timeout));
 
@@ -170,10 +260,10 @@ uint32_t firmware_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 		raise_exception(EXCEPTION_ERROR, "SWDP invalid ACK");
 
 	if(RnW) {
-		if(swd_proc.swdptap_seq_in_parity(&response, 32))  /* Give up on parity error */
+		if(dp->seq_in_parity(&response, 32))  /* Give up on parity error */
 			raise_exception(EXCEPTION_ERROR, "SWDP Parity error");
 	} else {
-		swd_proc.swdptap_seq_out_parity(value, 32);
+		dp->seq_out_parity(value, 32);
 		/* ARM Debug Interface Architecture Specification ADIv5.0 to ADIv5.2
 		 * tells to clock the data through SW-DP to either :
 		 * - immediate start a new transaction
@@ -183,7 +273,7 @@ uint32_t firmware_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 		 * Implement last option to favour correctness over
 		 *   slight speed decrease
 		 */
-		swd_proc.swdptap_seq_out(0, 8);
+	dp->seq_out(0, 8);
 	}
 	return response;
 }

src/target/cortexm.c

@@ -437,7 +437,10 @@ bool cortexm_probe(ADIv5_AP_t *ap)
 						   "probed device\n", ap->ap_designer, ap->ap_partno);
 #endif
 		}
-		if (ap->ap_partno == 0x4c3)  { /* Cortex-M3 ROM */
+		if (ap->ap_partno == 0x4c0)  { /* Cortex-M0+ ROM */
+			if ((ap->dp->targetid & 0xfff) == AP_DESIGNER_RASPBERRY)
+				PROBE(rp_probe);
+		} else if (ap->ap_partno == 0x4c3)  { /* Cortex-M3 ROM */
 			PROBE(stm32f1_probe); /* Care for STM32F1 clones */
 			PROBE(lpc15xx_probe); /* Thanks to JojoS for testing */
 		} else if (ap->ap_partno == 0x471)  { /* Cortex-M0 ROM */
@@ -923,7 +926,7 @@ int cortexm_run_stub(target *t, uint32_t loadaddr,
 	regs[2] = r2;
 	regs[3] = r3;
 	regs[15] = loadaddr;
-	regs[16] = 0x1000000;
+	regs[REG_XPSR] = CORTEXM_XPSR_THUMB;
 	regs[19] = 0;
 
 	cortexm_regs_write(t, regs);
@@ -933,16 +936,36 @@ int cortexm_run_stub(target *t, uint32_t loadaddr,
 
 	/* Execute the stub */
 	enum target_halt_reason reason;
+#if defined(PLATFORM_HAS_DEBUG)
+	uint32_t arm_regs_start[t->regs_size];
+	target_regs_read(t, arm_regs_start);
+#endif
 	cortexm_halt_resume(t, 0);
-	while ((reason = cortexm_halt_poll(t, NULL)) == TARGET_HALT_RUNNING)
+	platform_timeout timeout;
-		;
+	platform_timeout_set(&timeout, 5000);
+	do {
+		if (platform_timeout_is_expired(&timeout)) {
+			cortexm_halt_request(t);
+#if defined(PLATFORM_HAS_DEBUG)
+			DEBUG_WARN("Stub hangs\n");
+			uint32_t arm_regs[t->regs_size];
+			target_regs_read(t, arm_regs);
+			for (unsigned int i = 0; i < 20; i++) {
+				DEBUG_WARN("%2d: %08" PRIx32 ", %08" PRIx32 "\n",
+						   i, arm_regs_start[i], arm_regs[i]);
+			}
+#endif
+			return -3;
+		}
+	} while ((reason = cortexm_halt_poll(t, NULL)) == TARGET_HALT_RUNNING);
 
 	if (reason == TARGET_HALT_ERROR)
 		raise_exception(EXCEPTION_ERROR, "Target lost in stub");
 
-	if (reason != TARGET_HALT_BREAKPOINT)
+	if (reason != TARGET_HALT_BREAKPOINT) {
+		DEBUG_WARN(" Reasone %d\n", reason);
 		return -2;
-
+	}
 	uint32_t pc = cortexm_pc_read(t);
 	uint16_t bkpt_instr = target_mem_read16(t, pc);
 	if (bkpt_instr >> 8 != 0xbe)

src/target/cortexm.h

@@ -168,6 +168,7 @@ extern unsigned cortexm_wait_timeout;
 #define REG_SPECIAL	19
 
 #define ARM_THUMB_BREAKPOINT 0xBE00
+#define CORTEXM_XPSR_THUMB (1 << 24)
 
 #define	CORTEXM_TOPT_INHIBIT_SRST (1 << 2)
 

src/target/rp.c

@@ -0,0 +1,397 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2021 Uwe Bonnes (bon@elektron.ikp.physik.tu-darmstadt.de)
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holders nor the names of
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
+ * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+/* This file implements Raspberry Pico (RP2040) target specific functions
+ * for detecting the device, providing the XML memory map but not yet
+ * Flash memory programming.
+ */
+
+#include "general.h"
+#include "target.h"
+#include "target_internal.h"
+#include "cortexm.h"
+
+#define RP_ID "Raspberry RP2040"
+#define BOOTROM_MAGIC ('M' | ('u' << 8) | (0x01 << 16))
+#define BOOTROM_MAGIC_ADDR 0x00000010
+#define XIP_FLASH_START    0x10000000
+#define SRAM_START         0x20000000
+
+struct rp_priv_s {
+	uint16_t _debug_trampoline;
+	uint16_t _debug_trampoline_end;
+	uint16_t _connect_internal_flash;
+	uint16_t _flash_exit_xip;
+	uint16_t _flash_range_erase;
+	uint16_t flash_range_program;
+	uint16_t _flash_flush_cache;
+	uint16_t _flash_enter_cmd_xip;
+	uint16_t reset_usb_boot;
+	bool     is_prepared;
+	uint32_t regs[0x20];/* Register playground*/
+};
+
+static bool rp2040_fill_table(struct rp_priv_s *priv, uint16_t *table, int max)
+{
+	uint16_t tag = *table++;
+	int check = 0;
+	while ((tag != 0) && max) {
+		uint16_t data = *table++;
+		check++;
+		max -= 2;
+		switch (tag) {
+		case ('D' | ('T' << 8)):
+			priv->_debug_trampoline = data;
+			break;
+		case ('D' | ('E' << 8)):
+			priv->_debug_trampoline_end = data;
+			break;
+		case ('I' | ('F' << 8)):
+			priv->_connect_internal_flash = data;
+			break;
+		case ('E' | ('X' << 8)):
+			priv->_flash_exit_xip = data;
+			break;
+		case ('R' | ('E' << 8)):
+			priv->_flash_range_erase = data;
+			break;
+		case ('R' | ('P' << 8)):
+			priv->flash_range_program = data;
+			break;
+		case ('F' | ('C' << 8)):
+			priv->_flash_flush_cache = data;
+			break;
+		case ('C' | ('X' << 8)):
+			priv->_flash_enter_cmd_xip = data;
+			break;
+		case ('U' | ('B' << 8)):
+			priv->reset_usb_boot = data;
+			break;
+		default:
+			check--;
+		}
+		tag = *table++;
+	}
+	DEBUG_TARGET("connect %04x debug_trampoline %04x end %04x\n",
+			   priv->_connect_internal_flash, priv->_debug_trampoline,
+			   priv->_debug_trampoline_end);
+	return (check != 9);
+}
+
+/* RP ROM functions calls
+ *
+ * timout == 0: Do not wait for poll, use for reset_usb_boot()
+ * timeout > 400 (ms) : display spinner
+ */
+static bool rp_rom_call(target *t, uint32_t *regs, uint32_t cmd,
+						uint32_t timeout)
+{
+	const char spinner[] = "|/-\\";
+	int spinindex = 0;
+	struct rp_priv_s *ps = (struct rp_priv_s*)t->target_storage;
+	regs[7] = cmd;
+	regs[REG_LR] = ps->_debug_trampoline_end;
+	regs[REG_PC] = ps->_debug_trampoline;
+	regs[REG_MSP] = 0x20042000;
+	regs[REG_XPSR] = CORTEXM_XPSR_THUMB;
+	uint32_t dbg_regs[t->regs_size / sizeof(uint32_t)];
+	target_regs_write(t, regs);
+	/* start the target and wait for it to halt again */
+	target_halt_resume(t, false);
+	if (!timeout)
+		return false;
+	DEBUG_INFO("Call cmd %04x\n", cmd);
+	platform_timeout to;
+	platform_timeout_set(&to, timeout);
+	do {
+		if (timeout > 400)
+			tc_printf(t, "\b%c", spinner[spinindex++ % 4]);
+		if (platform_timeout_is_expired(&to)) {
+			DEBUG_WARN("RP Run timout %d ms reached: ", timeout);
+			break;
+		}
+	} while (!target_halt_poll(t, NULL));
+	/* Debug */
+	target_regs_read(t, dbg_regs);
+	bool ret = ((dbg_regs[REG_PC] &~1) != (ps->_debug_trampoline_end & ~1));
+	if (ret) {
+		DEBUG_WARN("rp_rom_call cmd %04x failed, PC %08" PRIx32 "\n",
+				   cmd, dbg_regs[REG_PC]);
+	}
+	return ret;
+}
+
+static void rp_flash_prepare(target *t)
+{
+	struct rp_priv_s *ps = (struct rp_priv_s*)t->target_storage;
+	if (!ps->is_prepared) {
+		/* connect*/
+		rp_rom_call(t, ps->regs, ps->_connect_internal_flash,100);
+		/* exit_xip */
+		rp_rom_call(t, ps->regs, ps->_flash_exit_xip, 100);
+		ps->is_prepared = true;
+	}
+}
+
+/* FLASHCMD_SECTOR_ERASE  45/  400 ms
+ * 32k block erase       120/ 1600 ms
+ * 64k block erase       150/ 2000 ms
+ * chip erase           5000/25000 ms
+ * page programm           0.4/  3 ms
+ */
+static int rp_flash_erase(struct target_flash *f, target_addr addr,
+						  size_t len)
+{
+	if (addr & 0xfff) {
+		DEBUG_WARN("Unaligned erase\n");
+		return -1;
+	}
+	if (len & 0xfff) {
+		DEBUG_WARN("Unaligned len\n");
+		len = (len + 0xfff) & ~0xfff;
+	}
+	DEBUG_INFO("Erase addr %08" PRIx32 " len 0x%" PRIx32 "\n", addr, len);
+	target *t = f->t;
+	rp_flash_prepare(t);
+	struct rp_priv_s *ps = (struct rp_priv_s*)t->target_storage;
+	/* Register playground*/
+	/* erase */
+#define MAX_FLASH               (2 * 1024 * 1024)
+#define FLASHCMD_SECTOR_ERASE   0x20
+#define FLASHCMD_BLOCK32K_ERASE 0x52
+#define FLASHCMD_BLOCK64K_ERASE 0xd8
+#define FLASHCMD_CHIP_ERASE     0x72
+	addr -= XIP_FLASH_START;
+	if (len > MAX_FLASH)
+		len = MAX_FLASH;
+	while (len) {
+		ps->regs[0] = addr;
+		ps->regs[2] = -1;
+		if (len >= MAX_FLASH) { /* Bulk erase */
+			ps->regs[1] = MAX_FLASH;
+			ps->regs[3] = FLASHCMD_CHIP_ERASE;
+			DEBUG_WARN("BULK_ERASE\n");
+			rp_rom_call(t, ps->regs, ps->_flash_range_erase, 25100);
+			len = 0;
+		} else if (len >= (64 * 1024)) {
+			uint32_t chunk = len & ~((1 << 16) - 1);
+			ps->regs[1] = chunk;
+			ps->regs[3] = FLASHCMD_BLOCK64K_ERASE;
+			DEBUG_WARN("64k_ERASE\n");
+			rp_rom_call(t, ps->regs, ps->_flash_range_erase, 2100);
+			len -= chunk ;
+			addr += chunk;
+		} else if (len >= (32 * 1024)) {
+			uint32_t chunk = len & ~((1 << 15) - 1);
+			ps->regs[1] = chunk;
+			ps->regs[3] = FLASHCMD_BLOCK32K_ERASE;
+			DEBUG_WARN("32k_ERASE\n");
+			rp_rom_call(t, ps->regs, ps->_flash_range_erase, 1700);
+			len -= chunk;
+			addr += chunk;
+		} else {
+			ps->regs[1] = len;
+			ps->regs[2] = MAX_FLASH;
+			ps->regs[3] = FLASHCMD_SECTOR_ERASE;
+			DEBUG_WARN("Sector_ERASE\n");
+			rp_rom_call(t, ps->regs, ps->_flash_range_erase, 410);
+			len = 0;
+		}
+	}
+	DEBUG_INFO("\nErase done!\n");
+	return 0;
+}
+
+int rp_flash_write(struct target_flash *f,
+                    target_addr dest, const void *src, size_t len)
+{
+	DEBUG_INFO("RP Write %08" PRIx32 " len 0x%" PRIx32 "\n", dest, len);
+	if ((dest & 0xff) || (len & 0xff)) {
+		DEBUG_WARN("Unaligned erase\n");
+		return -1;
+	}
+	target *t = f->t;
+	rp_flash_prepare(t);
+	struct rp_priv_s *ps = (struct rp_priv_s*)t->target_storage;
+	/* Write payload to target ram */
+	dest -= XIP_FLASH_START;
+#define MAX_WRITE_CHUNK 0x1000
+	while (len) {
+		uint32_t chunksize = (len <= MAX_WRITE_CHUNK) ? len : MAX_WRITE_CHUNK;
+		target_mem_write(t, SRAM_START, src, chunksize);
+		/* Programm range */
+		ps->regs[0] = dest;
+		ps->regs[1] = SRAM_START;
+		ps->regs[2] = chunksize;
+		rp_rom_call(t, ps->regs, ps->flash_range_program,
+					(3 *  chunksize) >> 8); /* 3 ms per 256 byte page */
+		len -= chunksize;
+		src += chunksize;
+		dest += chunksize;
+	}
+	return 0;
+}
+
+static bool rp_cmd_reset_usb_boot(target *t, int argc, const char *argv[])
+{
+	struct rp_priv_s *ps = (struct rp_priv_s*)t->target_storage;
+	if (argc > 2) {
+		ps->regs[1] = atoi(argv[2]);
+	} else if (argc < 3) {
+		ps->regs[0] = atoi(argv[1]);
+	} else {
+		ps->regs[0] = 0;
+		ps->regs[1] = 0;
+	}
+	rp_rom_call(t, ps->regs, ps->reset_usb_boot, 0);
+	return true;
+}
+
+static bool rp_cmd_erase_mass(target *t, int argc, const char *argv[])
+{
+	(void) argc;
+	(void) argv;
+	struct target_flash f;
+	f.t = t;
+	return (rp_flash_erase(&f, XIP_FLASH_START, MAX_FLASH)) ? false: true;
+}
+
+const struct command_s rp_cmd_list[] = {
+	{"erase_mass", rp_cmd_erase_mass, "Erase entire flash memory"},
+	{"reset_usb_boot", rp_cmd_reset_usb_boot, "Reboot the device into BOOTSEL mode"},
+	{NULL, NULL, NULL}
+};
+
+static void rp_add_flash(target *t, uint32_t addr, size_t length)
+{
+        struct target_flash *f = calloc(1, sizeof(*f));
+        if (!f) {                       /* calloc failed: heap exhaustion */
+                DEBUG_WARN("calloc: failed in %s\n", __func__);
+                return;
+        }
+
+        f->start = addr;
+        f->length = length;
+        f->blocksize = 0x1000;
+        f->erase = rp_flash_erase;
+        f->write = rp_flash_write;
+        f->buf_size = 2048; /* Max buffer size used eotherwise */
+        target_add_flash(t, f);
+}
+
+bool rp_probe(target *t)
+{
+	/* Check bootrom magic*/
+	uint32_t boot_magic = target_mem_read32(t, BOOTROM_MAGIC_ADDR);
+	if ((boot_magic & 0x00ffffff) != BOOTROM_MAGIC) {
+		DEBUG_WARN("Wrong Bootmagic %08" PRIx32 " found\n!", boot_magic);
+		return false;
+	}
+#if defined(ENABLE_DEBUG)
+	if ((boot_magic >> 24) == 1)
+		DEBUG_WARN("Old Bootrom Version 1!\n");
+#endif
+#define RP_MAX_TABLE_SIZE  0x80
+	uint16_t *table =  alloca(RP_MAX_TABLE_SIZE);
+	uint16_t table_offset = target_mem_read32( t, BOOTROM_MAGIC_ADDR + 4);
+	if (!table || target_mem_read(t, table, table_offset, RP_MAX_TABLE_SIZE))
+		return false;
+	struct rp_priv_s *priv_storage = calloc(1, sizeof(struct rp_priv_s));
+	if (!priv_storage) {               /* calloc failed: heap exhaustion */
+		DEBUG_WARN("calloc: failed in %s\n", __func__);
+		return false;
+	}
+	if (rp2040_fill_table(priv_storage, table, RP_MAX_TABLE_SIZE)) {
+		free(priv_storage);
+		return false;
+	}
+ 	t->target_storage = (void*)priv_storage;
+	uint32_t bootsec[16];
+	target_mem_read( t, bootsec, XIP_FLASH_START, sizeof( bootsec));
+	int i;
+	for (i = 0; i < 16; i++)
+		if (bootsec[i])
+			break;
+	uint32_t size = 8 * 1024 *1024;
+	if (i == 16) {
+		DEBUG_WARN("Use default size\n");
+	} else {
+		/* Find out size of connected SPI Flash
+		 *
+		 * Flash needs valid content to be mapped
+		 * Low flash is mirrored when flash size is exceeded
+		 */
+		while (size) {
+			uint32_t mirrorsec[16];
+			target_mem_read(t, mirrorsec, XIP_FLASH_START + size,
+							sizeof( bootsec));
+			if (memcmp(bootsec, mirrorsec, sizeof( bootsec)))
+				break;
+			size >>= 1;
+		}
+	}
+	rp_add_flash(t, XIP_FLASH_START, size << 1);
+	t->driver = RP_ID;
+	target_add_ram(t, SRAM_START, 0x40000);
+	target_add_ram(t, 0x51000000,  0x1000);
+	target_add_commands(t, rp_cmd_list, RP_ID);
+	return true;
+}
+
+static bool rp_rescue_do_reset(target *t)
+{
+	ADIv5_AP_t *ap = (ADIv5_AP_t *)t->priv;
+	ap->dp->low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_DP_CTRLSTAT,
+						  ADIV5_DP_CTRLSTAT_CDBGPWRUPREQ);
+	ap->dp->low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_DP_CTRLSTAT, 0);
+	return false;
+}
+
+/* The RP Pico rescue DP provides no AP, so we need special handling
+ *
+ * Attach to this DP will do the reset, but will fail to attach!
+ */
+void rp_rescue_probe(ADIv5_AP_t *ap)
+{
+	target *t = target_new();
+	if (!t) {
+		return;
+	}
+
+	adiv5_ap_ref(ap);
+	t->attach = (void*)rp_rescue_do_reset;
+	t->priv = ap;
+	t->priv_free = (void*)adiv5_ap_unref;
+	t->driver = "Raspberry RP2040 Rescue(Attach to reset!)";
+}

src/target/swdptap_generic.c

@@ -18,7 +18,6 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #include "general.h"
-#include "swdptap.h"
 
 uint32_t swdptap_seq_in(int ticks)
 {

src/target/target.c

@@ -54,7 +54,6 @@ target *target_new(void)
 
 	t->attach = (void*)nop_function;
 	t->detach = (void*)nop_function;
-	t->check_error = (void*)nop_function;
 	t->mem_read = (void*)nop_function;
 	t->mem_write = (void*)nop_function;
 	t->reg_read = (void*)nop_function;
@@ -348,7 +347,13 @@ void target_detach(target *t)
 #endif
 }
 
-bool target_check_error(target *t) { return t->check_error(t); }
+bool target_check_error(target *t) {
+	if (t)
+		return t->check_error(t);
+	else
+		return false;
+}
+
 bool target_attached(target *t) { return t->attached; }
 
 /* Memory access functions */
@@ -579,6 +584,7 @@ void tc_printf(target *t, const char *fmt, ...)
 
 	va_start(ap, fmt);
 	t->tc->printf(t->tc, fmt, ap);
+	fflush(stdout);
 	va_end(ap);
 }
 

src/target/target_internal.h

@@ -192,4 +192,5 @@ bool kinetis_probe(target *t);
 bool efm32_probe(target *t);
 bool msp432_probe(target *t);
 bool ke04_probe(target *t);
+bool rp_probe(target *t);
 #endif