diff --git a/src/platforms/hosted/cmsis_dap.c b/src/platforms/hosted/cmsis_dap.c
index 68f8849a..b6ce2c4b 100644
--- a/src/platforms/hosted/cmsis_dap.c
+++ b/src/platforms/hosted/cmsis_dap.c
@@ -116,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 |
@@ -180,7 +181,7 @@ int dbg_dap_cmd(uint8_t *data, int size, int rsize)
 	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");
 	/* Write must be as long as we expect the result, at least
@@ -284,16 +285,6 @@ static void dap_mem_write_sized(
 	DEBUG_WIRE("memwrite done\n");
 }
 
-int dap_enter_debug_swd(ADIv5_DP_t *dp)
-{
-	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())
@@ -367,6 +358,66 @@ 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))
@@ -378,14 +429,15 @@ int dap_swdptap_init(ADIv5_DP_t *dp)
 	dap_led(0, 1);
 	dap_reset_link(false);
 	if (has_swd_sequence) {
-		dp->seq_in = dap_swdptap_seq_in;
-		dp->seq_in_parity = dap_swdptap_seq_in_parity;
-		dp->seq_out = dap_swdptap_seq_out;
-		dp->seq_out_parity = dap_swdptap_seq_out_parity;
-		dp->dp_read = dap_dp_read_reg;
-		dp->error = dap_dp_error;
-		dp->low_access = dap_dp_low_access;
-		dp->abort = dap_dp_abort;
+		/* 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;
 }
diff --git a/src/platforms/hosted/cmsis_dap.h b/src/platforms/hosted/cmsis_dap.h
index 45178e47..d23726db 100644
--- a/src/platforms/hosted/cmsis_dap.h
+++ b/src/platforms/hosted/cmsis_dap.h
@@ -24,7 +24,6 @@
 
 #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);
@@ -41,7 +40,6 @@ 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) {};
diff --git a/src/platforms/hosted/dap.c b/src/platforms/hosted/dap.c
index 0cf63e32..24ba7c70 100644
--- a/src/platforms/hosted/dap.c
+++ b/src/platforms/hosted/dap.c
@@ -782,44 +782,3 @@ void dap_swdptap_seq_out_parity(uint32_t MS, int ticks)
 	if (buf[0])
 		DEBUG_WARN("dap_swdptap_seq_out error\n");
 }
-
-#define SWD_SEQUENCE_IN 0x80
-uint32_t dap_swdptap_seq_in(int ticks)
-{
-	uint8_t buf[16] = {
-		ID_DAP_SWD_SEQUENCE,
-		1,
-		ticks + SWD_SEQUENCE_IN
-	};
-	dbg_dap_cmd(buf, 2 + ((ticks + 7) >> 3), 3);
-	uint32_t res = 0;
-	int len = (ticks + 7) >> 3;
-	while (len--) {
-		res <<= 8;
-		res += buf[len + 1];
-	}
-	return res;
-}
-
-bool dap_swdptap_seq_in_parity(uint32_t *ret, int ticks)
-{
-	(void)ticks;
-	uint8_t buf[16] = {
-		ID_DAP_SWD_SEQUENCE,
-		2,
-		32 + SWD_SEQUENCE_IN,
-		1 + SWD_SEQUENCE_IN,
-	};
-	dbg_dap_cmd(buf, 7, 4);
-	uint32_t res = 0;
-	int len = 4;
-	while (len--) {
-		res <<= 8;
-		res += buf[len + 1];
-	}
-	*ret = res;
-	unsigned int parity = __builtin_parity(res) & 1;
-	parity ^= (buf[5] % 1);
-	DEBUG_WARN("Res %08" PRIx32" %d\n", *ret, parity & 1);
-	return (!parity & 1);
-}
diff --git a/src/platforms/hosted/dap.h b/src/platforms/hosted/dap.h
index 470742ca..d7e11249 100644
--- a/src/platforms/hosted/dap.h
+++ b/src/platforms/hosted/dap.h
@@ -92,6 +92,4 @@ void dap_jtagtap_tdi_tdo_seq(uint8_t *DO, bool final_tms, const uint8_t *TMS,
 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);
-uint32_t dap_swdptap_seq_in(int ticks);
-bool dap_swdptap_seq_in_parity(uint32_t *ret, int ticks);
 #endif // _DAP_H_
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 7e11de78..f535c25a 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -628,6 +628,7 @@ static void rp_rescue_setup(ADIv5_DP_t *dp)
 		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);
@@ -646,7 +647,6 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 	}
 	if (dp->idcode == 0x10212927) {
 		rp_rescue_setup(dp);
-		free(dp);
 		return;
 	}
 	DEBUG_INFO("DPIDR 0x%08" PRIx32 " (v%d %srev%d)\n", dp->idcode,
diff --git a/src/target/adiv5_swdp.c b/src/target/adiv5_swdp.c
index 771fc2f0..74dc61be 100644
--- a/src/target/adiv5_swdp.c
+++ b/src/target/adiv5_swdp.c
@@ -116,13 +116,14 @@ int adiv5_swdp_scan(uint32_t targetid)
 			idcode = initial_dp->low_access(initial_dp, ADIV5_LOW_READ,
 										   ADIV5_DP_IDCODE, 0);
 		}
-		if (e.type) {
+		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,
@@ -146,6 +147,9 @@ int adiv5_swdp_scan(uint32_t targetid)
 				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;
 		}
@@ -161,8 +165,9 @@ int adiv5_swdp_scan(uint32_t targetid)
 			initial_dp->dp_low_write(initial_dp, ADIV5_DP_TARGETSEL,
 									dp_targetid);
 			if (initial_dp->dp_low_read(initial_dp, ADIV5_DP_IDCODE,
-										&idcode))
+										&idcode)) {
 				continue;
+			}
 		} else {
 			dp_targetid = 0;
 		}
@@ -238,10 +243,8 @@ uint32_t firmware_swdp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
 		dp->seq_out(request, 8);
 		ack = dp->seq_in(3);
 		if (ack == SWDP_ACK_FAULT) {
-			/* On fault, abort() and repeat the command once.*/
-			dp->error(dp);
-			dp->seq_out(request, 8);
-			ack = dp->seq_in(3);
+			dp->fault = 1;
+			return 0;
 		}
 	} while (ack == SWDP_ACK_WAIT && !platform_timeout_is_expired(&timeout));
 
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index 9fb89a31..1fec2a09 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -926,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);
@@ -936,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)
diff --git a/src/target/cortexm.h b/src/target/cortexm.h
index 812a683e..8bd95d36 100644
--- a/src/target/cortexm.h
+++ b/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)
 
diff --git a/src/target/rp.c b/src/target/rp.c
index 0787c64e..5d95085c 100644
--- a/src/target/rp.c
+++ b/src/target/rp.c
@@ -42,21 +42,346 @@
 #include "target_internal.h"
 #include "cortexm.h"
 
-bool rp_probe(target *t)
+#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)
 {
-	t->driver = "Raspberry RP2040";
-	target_add_ram(t, 0x20000000, 0x40000);
+	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;
+}
 
-static bool rp_rescue_reset(target *t, int argc, const char **argv);
-
-const struct command_s rp_rescue_cmd_list[] = {
-	{"rescue_reset", (cmd_handler)rp_rescue_reset, "Hard reset to bootrom and halt"},
+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();
@@ -65,20 +390,8 @@ void rp_rescue_probe(ADIv5_AP_t *ap)
 	}
 
 	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";
-
-	target_add_commands(t, rp_rescue_cmd_list, t->driver);
-}
-
-static bool rp_rescue_reset(target *t, int argc, const char **argv)
-{
-	(void)argc;
-	(void)argv;
-	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 true;
+	t->driver = "Raspberry RP2040 Rescue(Attach to reset!)";
 }
diff --git a/src/target/target.c b/src/target/target.c
index eb43f375..c17e9158 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -584,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);
 }