diff --git a/src/command.c b/src/command.c
index 648ec88..ca3ae01 100644
--- a/src/command.c
+++ b/src/command.c
@@ -43,6 +43,12 @@ struct command_s {
 	const char *help;
 };
 
+enum assert_srst_t{
+	ASSERT_NEVER = 0,
+	ASSERT_UNTIL_SCAN,
+	ASSERT_UNTIL_ATTACH
+};
+
 static bool cmd_version(void);
 static bool cmd_help(target *t);
 
@@ -50,7 +56,8 @@ static bool cmd_jtag_scan(target *t, int argc, char **argv);
 static bool cmd_swdp_scan(void);
 static bool cmd_targets(void);
 static bool cmd_morse(void);
-static bool cmd_connect_srst(target *t, int argc, const char **argv);
+static bool cmd_assert_srst(target *t, int argc, const char **argv);
+static bool cmd_halt_timeout(target *t, int argc, const char **argv);
 static bool cmd_hard_srst(void);
 #ifdef PLATFORM_HAS_POWER_SWITCH
 static bool cmd_target_power(target *t, int argc, const char **argv);
@@ -69,7 +76,8 @@ const struct command_s cmd_list[] = {
 	{"swdp_scan", (cmd_handler)cmd_swdp_scan, "Scan SW-DP for devices" },
 	{"targets", (cmd_handler)cmd_targets, "Display list of available targets" },
 	{"morse", (cmd_handler)cmd_morse, "Display morse error message" },
-	{"connect_srst", (cmd_handler)cmd_connect_srst, "Configure connect under SRST: (enable|disable)" },
+	{"assert_srst", (cmd_handler)cmd_assert_srst, "Assert SRST until:(never(default)| scan | attach)" },
+	{"halt_timeout", (cmd_handler)cmd_halt_timeout, "Timeout (ms) to wait until Cortex-M is halted: (Default 2000)" },
 	{"hard_srst", (cmd_handler)cmd_hard_srst, "Force a pulse on the hard SRST line - disconnects target" },
 #ifdef PLATFORM_HAS_POWER_SWITCH
 	{"tpwr", (cmd_handler)cmd_target_power, "Supplies power to the target: (enable|disable)"},
@@ -83,10 +91,11 @@ const struct command_s cmd_list[] = {
 	{NULL, NULL, NULL}
 };
 
-static bool connect_assert_srst;
+static enum assert_srst_t assert_srst;
 #ifdef PLATFORM_HAS_DEBUG
 bool debug_bmp;
 #endif
+long cortexm_wait_timeout = 2000; /* Timeout to wait for Cortex to react on halt command. */
 
 int command_process(target *t, char *cmd)
 {
@@ -159,8 +168,10 @@ static bool cmd_jtag_scan(target *t, int argc, char **argv)
 		irlens[argc-1] = 0;
 	}
 
-	if(connect_assert_srst)
-		platform_srst_set_val(true); /* will be deasserted after attach */
+	if(assert_srst != ASSERT_NEVER)
+		platform_srst_set_val(true);
+	if(assert_srst == ASSERT_UNTIL_SCAN)
+		platform_srst_set_val(false);
 
 	int devs = -1;
 	volatile struct exception e;
@@ -190,8 +201,10 @@ bool cmd_swdp_scan(void)
 {
 	gdb_outf("Target voltage: %s\n", platform_target_voltage());
 
-	if(connect_assert_srst)
-		platform_srst_set_val(true); /* will be deasserted after attach */
+	if(assert_srst != ASSERT_NEVER)
+		platform_srst_set_val(true);
+	if(assert_srst == ASSERT_UNTIL_SCAN)
+		platform_srst_set_val(false);
 
 	int devs = -1;
 	volatile struct exception e;
@@ -244,14 +257,30 @@ bool cmd_morse(void)
 	return true;
 }
 
-static bool cmd_connect_srst(target *t, int argc, const char **argv)
+static bool cmd_assert_srst(target *t, int argc, const char **argv)
 {
 	(void)t;
-	if (argc == 1)
-		gdb_outf("Assert SRST during connect: %s\n",
-			 connect_assert_srst ? "enabled" : "disabled");
-	else
-		connect_assert_srst = !strcmp(argv[1], "enable");
+	if (argc > 1) {
+		if (!strcmp(argv[1], "attach"))
+			assert_srst = ASSERT_UNTIL_ATTACH;
+		else if (!strcmp(argv[1], "scan"))
+			assert_srst = ASSERT_UNTIL_SCAN;
+		else
+			assert_srst = ASSERT_NEVER;
+	}
+	gdb_outf("Assert SRST %s\n",
+			 (assert_srst == ASSERT_UNTIL_ATTACH) ? "until attach" :
+			 (assert_srst == ASSERT_UNTIL_SCAN) ? "until scan" : "never");
+	return true;
+}
+
+static bool cmd_halt_timeout(target *t, int argc, const char **argv)
+{
+	(void)t;
+	if (argc > 1)
+		cortexm_wait_timeout = atol(argv[1]);
+	gdb_outf("Cortex-M timeout to wait for device haltes: %d\n",
+				 cortexm_wait_timeout);
 	return true;
 }
 
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index b6236d8..109cab3 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -246,11 +246,12 @@ static uint32_t adiv5_mem_read32(ADIv5_AP_t *ap, uint32_t addr)
 	return ret;
 }
 
-static void adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
+static bool adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
 {
 	addr &= ~3;
 	uint64_t pidr = 0;
 	uint32_t cidr = 0;
+	bool res = false;
 
 	/* Assemble logical Product ID register value. */
 	for (int i = 0; i < 4; i++) {
@@ -270,14 +271,14 @@ static void adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
 
 	if (adiv5_dp_error(ap->dp)) {
 		DEBUG("Fault reading ID registers\n");
-		return;
+		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",
                       addr, cidr, CID_PREAMBLE);
-		return;
+		return false;
 	}
 
 	/* Extract Component ID class nibble */
@@ -296,7 +297,7 @@ static void adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
 			if ((entry & 1) == 0)
 				continue;
 
-			adiv5_component_probe(ap, addr + (entry & ~0xfff));
+			res |= adiv5_component_probe(ap, addr + (entry & ~0xfff));
 		}
 	} else {
 		/* Check if the component was designed by ARM, we currently do not support,
@@ -305,7 +306,7 @@ static void adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
 		if ((pidr & ~(PIDR_REV_MASK | PIDR_PN_MASK)) != PIDR_ARM_BITS) {
 			DEBUG("0x%"PRIx32": 0x%"PRIx64" <- does not match ARM JEP-106\n",
                               addr, pidr);
-			return;
+			return false;
 		}
 
 		/* Extract part number from the part id register. */
@@ -329,6 +330,7 @@ static void adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
 					      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");
@@ -349,6 +351,7 @@ static void adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
 			      cidc_debug_strings[cid_class], pidr);
 		}
 	}
+	return res;
 }
 
 ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
@@ -388,6 +391,7 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 
 void adiv5_dp_init(ADIv5_DP_t *dp)
 {
+	volatile bool probed = false;
 	volatile uint32_t ctrlstat = 0;
 
 	adiv5_dp_ref(dp);
@@ -452,7 +456,11 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		 */
 
 		/* The rest sould only be added after checking ROM table */
-		adiv5_component_probe(ap, ap->base);
+		probed |= adiv5_component_probe(ap, ap->base);
+		if (!probed && (dp->idcode & 0xfff) == 0x477) {
+			DEBUG("-> cortexm_probe forced\n");
+			cortexm_probe(ap);
+		}
 	}
 	adiv5_dp_unref(dp);
 }
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index dba2349..86e46fd 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -237,6 +237,33 @@ static void cortexm_priv_free(void *priv)
 	free(priv);
 }
 
+static bool cortexm_forced_halt(target *t)
+{
+	uint32_t start_time = platform_time_ms();
+	platform_srst_set_val(false);
+	/* Wait until SRST is released.*/
+	while (platform_time_ms() < start_time + 2000) {
+		if (!platform_srst_get_val())
+			break;
+	}
+	if (platform_srst_get_val())
+		return false;
+	uint32_t dhcsr = 0;
+	start_time = platform_time_ms();
+	/* Try hard to halt the target. STM32F7 in  WFI
+	   needs multiple writes!*/
+	while (platform_time_ms() < start_time + cortexm_wait_timeout) {
+		dhcsr = target_mem_read32(t, CORTEXM_DHCSR);
+		if (dhcsr == (CORTEXM_DHCSR_S_HALT | CORTEXM_DHCSR_S_REGRDY |
+					  CORTEXM_DHCSR_C_HALT | CORTEXM_DHCSR_C_DEBUGEN))
+			break;
+		target_halt_request(t);
+	}
+	if (dhcsr != 0x00030003)
+		return false;
+	return true;
+}
+
 bool cortexm_probe(ADIv5_AP_t *ap)
 {
 	target *t;
@@ -296,8 +323,10 @@ bool cortexm_probe(ADIv5_AP_t *ap)
 		target_check_error(t);
 	}
 
+	if (!cortexm_forced_halt(t))
+		return false;
 #define PROBE(x) \
-	do { if ((x)(t)) return true; else target_check_error(t); } while (0)
+	do { if ((x)(t)) {target_halt_resume(t, 0); return true;} else target_check_error(t); } while (0)
 
 	PROBE(stm32f1_probe);
 	PROBE(stm32f4_probe);
@@ -325,16 +354,12 @@ bool cortexm_attach(target *t)
 	struct cortexm_priv *priv = t->priv;
 	unsigned i;
 	uint32_t r;
-	int tries;
 
 	/* Clear any pending fault condition */
 	target_check_error(t);
 
 	target_halt_request(t);
-	tries = 10;
-	while(!platform_srst_get_val() && !target_halt_poll(t, NULL) && --tries)
-		platform_delay(200);
-	if(!tries)
+	if (!cortexm_forced_halt(t))
 		return false;
 
 	/* Request halt on reset */
@@ -370,8 +395,6 @@ bool cortexm_attach(target *t)
 	target_mem_write32(t, CORTEXM_FPB_CTRL,
 			CORTEXM_FPB_CTRL_KEY | CORTEXM_FPB_CTRL_ENABLE);
 
-	platform_srst_set_val(false);
-
 	return true;
 }
 
diff --git a/src/target/cortexm.h b/src/target/cortexm.h
index 9aa4733..e6362f4 100644
--- a/src/target/cortexm.h
+++ b/src/target/cortexm.h
@@ -22,6 +22,7 @@
 #include "target.h"
 #include "adiv5.h"
 
+extern long cortexm_wait_timeout;
 /* Private peripheral bus base address */
 #define CORTEXM_PPB_BASE	0xE0000000