diff --git a/src/target/adiv5_swdp.c b/src/target/adiv5_swdp.c
index 21f4024..385f640 100644
--- a/src/target/adiv5_swdp.c
+++ b/src/target/adiv5_swdp.c
@@ -145,20 +145,6 @@ uint32_t adiv5_swdp_scan(uint32_t targetid)
 			adiv5_dp_write(initial_dp, ADIV5_DP_SELECT, ADIV5_DP_BANK2);
 			dp_targetid = adiv5_dp_read(initial_dp, ADIV5_DP_TARGETID);
 			adiv5_dp_write(initial_dp, ADIV5_DP_SELECT, ADIV5_DP_BANK0);
-
-			const uint16_t tdesigner =
-				(dp_targetid & ADIV5_DP_TARGETID_TDESIGNER_MASK) >> ADIV5_DP_TARGETID_TDESIGNER_OFFSET;
-			const uint16_t tpartno = (dp_targetid & ADIV5_DP_TARGETID_TPARTNO_MASK) >> ADIV5_DP_TARGETID_TPARTNO_OFFSET;
-			/* convert it to our internal representation, See JEP-106 code list */
-			const uint16_t designer_code = (tdesigner & ADIV5_DP_DESIGNER_JEP106_CONT_MASK) << 1U |
-			                               (tdesigner & ADIV5_DP_DESIGNER_JEP106_CODE_MASK);
-
-			if (designer_code == JEP106_MANUFACTURER_RASPBERRY && tpartno == 0x1002) {
-				/* RP2040 */
-				/* Release evt. handing RESCUE DP reset*/
-
-				adiv5_dp_write(initial_dp, ADIV5_DP_CTRLSTAT, 0);
-			}
 		}
 	}
 
diff --git a/src/target/rp.c b/src/target/rp.c
index d38f326..66aab4e 100644
--- a/src/target/rp.c
+++ b/src/target/rp.c
@@ -644,8 +644,21 @@ static bool rp_cmd_reset_usb_boot(target *t, int argc, const char **argv)
 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);
+	uint32_t ctrlstat = ap->dp->low_access(ap->dp, ADIV5_LOW_READ, ADIV5_DP_CTRLSTAT, 0);
+	ap->dp->low_access(ap->dp, ADIV5_LOW_WRITE, ADIV5_DP_CTRLSTAT, ctrlstat | ADIV5_DP_CTRLSTAT_CDBGPWRUPREQ);
+	platform_timeout timeout;
+	platform_timeout_set(&timeout, 100);
+	while (true) {
+		ctrlstat = ap->dp->low_access(ap->dp, ADIV5_LOW_READ, ADIV5_DP_CTRLSTAT, 0);
+		if (!(ctrlstat & ADIV5_DP_CTRLSTAT_CDBGRSTACK)) {
+			DEBUG_INFO("RP RESCUE succeeded.\n");
+			break;
+		}
+		if (platform_timeout_is_expired(&timeout)) {
+			DEBUG_INFO("RP RESCUE failed\n");
+			break;
+		}
+	}
 	return false;
 }