diff --git a/src/platforms/hosted/cmsis_dap.c b/src/platforms/hosted/cmsis_dap.c
index 68f8849..b6ce2c4 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 45178e4..d23726d 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 0cf63e3..24ba7c7 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 470742c..d7e1124 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 7e11de7..f535c25 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 b20ae27..ea105fe 100644
--- a/src/target/adiv5_swdp.c
+++ b/src/target/adiv5_swdp.c
@@ -146,6 +146,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 +164,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;
 		}