X-Git-Url: https://review.openocd.org/gitweb?a=blobdiff_plain;f=src%2Ftarget%2Fcortex_m.c;h=ac308b43ba6e1815ddb533ac996c3e361dc4efd5;hb=722f5797069bc233c8e1b71bdab283766d6be9b3;hp=34a19e741d8683156e86a5d2af2608fc122a77bf;hpb=4b998cb5f5d2066e5e0197fcc460831b09813f70;p=openocd.git

diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 34a19e741d..ac308b43ba 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -39,6 +39,7 @@
 #include "arm_opcodes.h"
 #include "arm_semihosting.h"
 #include <helper/time_support.h>
+#include <rtt/rtt.h>
 
 /* NOTE:  most of this should work fine for the Cortex-M1 and
  * Cortex-M0 cores too, although they're ARMv6-M not ARMv7-M.
@@ -56,8 +57,8 @@ static int cortex_m_store_core_reg_u32(struct target *target,
 		uint32_t num, uint32_t value);
 static void cortex_m_dwt_free(struct target *target);
 
-static int cortexm_dap_read_coreregister_u32(struct target *target,
-	uint32_t *value, int regnum)
+static int cortex_m_load_core_reg_u32(struct target *target,
+		uint32_t regsel, uint32_t *value)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	int retval;
@@ -71,7 +72,7 @@ static int cortexm_dap_read_coreregister_u32(struct target *target,
 			return retval;
 	}
 
-	retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DCRSR, regnum);
+	retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DCRSR, regsel);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -89,8 +90,8 @@ static int cortexm_dap_read_coreregister_u32(struct target *target,
 	return retval;
 }
 
-static int cortexm_dap_write_coreregister_u32(struct target *target,
-	uint32_t value, int regnum)
+static int cortex_m_store_core_reg_u32(struct target *target,
+		uint32_t regsel, uint32_t value)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	int retval;
@@ -108,12 +109,12 @@ static int cortexm_dap_write_coreregister_u32(struct target *target,
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = mem_ap_write_atomic_u32(armv7m->debug_ap, DCB_DCRSR, regnum | DCRSR_WnR);
+	retval = mem_ap_write_atomic_u32(armv7m->debug_ap, DCB_DCRSR, regsel | DCRSR_WnR);
 	if (retval != ERROR_OK)
 		return retval;
 
 	if (target->dbg_msg_enabled) {
-		/* restore DCB_DCRDR - this needs to be in a seperate
+		/* restore DCB_DCRDR - this needs to be in a separate
 		 * transaction otherwise the emulated DCC channel breaks */
 		if (retval == ERROR_OK)
 			retval = mem_ap_write_atomic_u32(armv7m->debug_ap, DCB_DCRDR, dcrdr);
@@ -129,13 +130,90 @@ static int cortex_m_write_debug_halt_mask(struct target *target,
 	struct armv7m_common *armv7m = &cortex_m->armv7m;
 
 	/* mask off status bits */
-	cortex_m->dcb_dhcsr &= ~((0xFFFF << 16) | mask_off);
+	cortex_m->dcb_dhcsr &= ~((0xFFFFul << 16) | mask_off);
 	/* create new register mask */
 	cortex_m->dcb_dhcsr |= DBGKEY | C_DEBUGEN | mask_on;
 
 	return mem_ap_write_atomic_u32(armv7m->debug_ap, DCB_DHCSR, cortex_m->dcb_dhcsr);
 }
 
+static int cortex_m_set_maskints(struct target *target, bool mask)
+{
+	struct cortex_m_common *cortex_m = target_to_cm(target);
+	if (!!(cortex_m->dcb_dhcsr & C_MASKINTS) != mask)
+		return cortex_m_write_debug_halt_mask(target, mask ? C_MASKINTS : 0, mask ? 0 : C_MASKINTS);
+	else
+		return ERROR_OK;
+}
+
+static int cortex_m_set_maskints_for_halt(struct target *target)
+{
+	struct cortex_m_common *cortex_m = target_to_cm(target);
+	switch (cortex_m->isrmasking_mode) {
+		case CORTEX_M_ISRMASK_AUTO:
+			/* interrupts taken at resume, whether for step or run -> no mask */
+			return cortex_m_set_maskints(target, false);
+
+		case CORTEX_M_ISRMASK_OFF:
+			/* interrupts never masked */
+			return cortex_m_set_maskints(target, false);
+
+		case CORTEX_M_ISRMASK_ON:
+			/* interrupts always masked */
+			return cortex_m_set_maskints(target, true);
+
+		case CORTEX_M_ISRMASK_STEPONLY:
+			/* interrupts masked for single step only -> mask now if MASKINTS
+			 * erratum, otherwise only mask before stepping */
+			return cortex_m_set_maskints(target, cortex_m->maskints_erratum);
+	}
+	return ERROR_OK;
+}
+
+static int cortex_m_set_maskints_for_run(struct target *target)
+{
+	switch (target_to_cm(target)->isrmasking_mode) {
+		case CORTEX_M_ISRMASK_AUTO:
+			/* interrupts taken at resume, whether for step or run -> no mask */
+			return cortex_m_set_maskints(target, false);
+
+		case CORTEX_M_ISRMASK_OFF:
+			/* interrupts never masked */
+			return cortex_m_set_maskints(target, false);
+
+		case CORTEX_M_ISRMASK_ON:
+			/* interrupts always masked */
+			return cortex_m_set_maskints(target, true);
+
+		case CORTEX_M_ISRMASK_STEPONLY:
+			/* interrupts masked for single step only -> no mask */
+			return cortex_m_set_maskints(target, false);
+	}
+	return ERROR_OK;
+}
+
+static int cortex_m_set_maskints_for_step(struct target *target)
+{
+	switch (target_to_cm(target)->isrmasking_mode) {
+		case CORTEX_M_ISRMASK_AUTO:
+			/* the auto-interrupt should already be done -> mask */
+			return cortex_m_set_maskints(target, true);
+
+		case CORTEX_M_ISRMASK_OFF:
+			/* interrupts never masked */
+			return cortex_m_set_maskints(target, false);
+
+		case CORTEX_M_ISRMASK_ON:
+			/* interrupts always masked */
+			return cortex_m_set_maskints(target, true);
+
+		case CORTEX_M_ISRMASK_STEPONLY:
+			/* interrupts masked for single step only -> mask */
+			return cortex_m_set_maskints(target, true);
+	}
+	return ERROR_OK;
+}
+
 static int cortex_m_clear_halt(struct target *target)
 {
 	struct cortex_m_common *cortex_m = target_to_cm(target);
@@ -237,11 +315,8 @@ static int cortex_m_endreset_event(struct target *target)
 			return retval;
 	}
 
-	/* Restore proper interrupt masking setting. */
-	if (cortex_m->isrmasking_mode == CORTEX_M_ISRMASK_ON)
-		cortex_m_write_debug_halt_mask(target, C_MASKINTS, 0);
-	else
-		cortex_m_write_debug_halt_mask(target, 0, C_MASKINTS);
+	/* Restore proper interrupt masking setting for running CPU. */
+	cortex_m_set_maskints_for_run(target);
 
 	/* Enable features controlled by ITM and DWT blocks, and catch only
 	 * the vectors we were told to pay attention to.
@@ -265,7 +340,7 @@ static int cortex_m_endreset_event(struct target *target)
 		return retval;
 	}
 
-	cortex_m->fpb_enabled = 1;
+	cortex_m->fpb_enabled = true;
 
 	/* Restore FPB registers */
 	for (i = 0; i < cortex_m->fp_num_code + cortex_m->fp_num_lit; i++) {
@@ -318,7 +393,9 @@ static int cortex_m_examine_debug_reason(struct target *target)
 			target->debug_reason = DBG_REASON_WATCHPOINT;
 		else if (cortex_m->nvic_dfsr & DFSR_VCATCH)
 			target->debug_reason = DBG_REASON_BREAKPOINT;
-		else	/* EXTERNAL, HALTED */
+		else if (cortex_m->nvic_dfsr & DFSR_EXTERNAL)
+			target->debug_reason = DBG_REASON_DBGRQ;
+		else	/* HALTED */
 			target->debug_reason = DBG_REASON_UNDEFINED;
 	}
 
@@ -369,6 +446,14 @@ static int cortex_m_examine_exception_reason(struct target *target)
 			if (retval != ERROR_OK)
 				return retval;
 			break;
+		case 7:	/* Secure Fault */
+			retval = mem_ap_read_u32(armv7m->debug_ap, NVIC_SFSR, &except_sr);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = mem_ap_read_u32(armv7m->debug_ap, NVIC_SFAR, &except_ar);
+			if (retval != ERROR_OK)
+				return retval;
+			break;
 		case 11:	/* SVCall */
 			break;
 		case 12:	/* Debug Monitor */
@@ -405,6 +490,10 @@ static int cortex_m_debug_entry(struct target *target)
 
 	LOG_DEBUG(" ");
 
+	/* Do this really early to minimize the window where the MASKINTS erratum
+	 * can pile up pending interrupts. */
+	cortex_m_set_maskints_for_halt(target);
+
 	cortex_m_clear_halt(target);
 	retval = mem_ap_read_atomic_u32(armv7m->debug_ap, DCB_DHCSR, &cortex_m->dcb_dhcsr);
 	if (retval != ERROR_OK)
@@ -414,6 +503,18 @@ static int cortex_m_debug_entry(struct target *target)
 	if (retval != ERROR_OK)
 		return retval;
 
+	/* examine PE security state */
+	bool secure_state = false;
+	if (armv7m->arm.is_armv8m) {
+		uint32_t dscsr;
+
+		retval = mem_ap_read_u32(armv7m->debug_ap, DCB_DSCSR, &dscsr);
+		if (retval != ERROR_OK)
+			return retval;
+
+		secure_state = (dscsr & DSCSR_CDS) == DSCSR_CDS;
+	}
+
 	/* Examine target state and mode
 	 * First load register accessible through core debug port */
 	int num_regs = arm->core_cache->num_regs;
@@ -427,12 +528,6 @@ static int cortex_m_debug_entry(struct target *target)
 	r = arm->cpsr;
 	xPSR = buf_get_u32(r->value, 0, 32);
 
-	/* For IT instructions xPSR must be reloaded on resume and clear on debug exec */
-	if (xPSR & 0xf00) {
-		r->dirty = r->valid;
-		cortex_m_store_core_reg_u32(target, 16, xPSR & ~0xff);
-	}
-
 	/* Are we in an exception handler */
 	if (xPSR & 0x1FF) {
 		armv7m->exception_number = (xPSR & 0x1FF);
@@ -441,7 +536,7 @@ static int cortex_m_debug_entry(struct target *target)
 		arm->map = armv7m_msp_reg_map;
 	} else {
 		unsigned control = buf_get_u32(arm->core_cache
-				->reg_list[ARMV7M_CONTROL].value, 0, 2);
+				->reg_list[ARMV7M_CONTROL].value, 0, 3);
 
 		/* is this thread privileged? */
 		arm->core_mode = control & 1
@@ -460,9 +555,10 @@ static int cortex_m_debug_entry(struct target *target)
 	if (armv7m->exception_number)
 		cortex_m_examine_exception_reason(target);
 
-	LOG_DEBUG("entered debug state in core mode: %s at PC 0x%" PRIx32 ", target->state: %s",
+	LOG_DEBUG("entered debug state in core mode: %s at PC 0x%" PRIx32 ", cpu in %s state, target->state: %s",
 		arm_mode_name(arm->core_mode),
 		buf_get_u32(arm->pc->value, 0, 32),
+		secure_state ? "Secure" : "Non-Secure",
 		target_state_name(target));
 
 	if (armv7m->post_debug_entry) {
@@ -555,13 +651,9 @@ static int cortex_m_poll(struct target *target)
 		}
 	}
 
-	/* REVISIT when S_SLEEP is set, it's in a Sleep or DeepSleep state.
-	 * How best to model low power modes?
-	 */
-
 	if (target->state == TARGET_UNKNOWN) {
-		/* check if processor is retiring instructions */
-		if (cortex_m->dcb_dhcsr & S_RETIRE_ST) {
+		/* check if processor is retiring instructions or sleeping */
+		if (cortex_m->dcb_dhcsr & S_RETIRE_ST || cortex_m->dcb_dhcsr & S_SLEEP) {
 			target->state = TARGET_RUNNING;
 			retval = ERROR_OK;
 		}
@@ -614,6 +706,10 @@ static int cortex_m_halt(struct target *target)
 	/* Write to Debug Halting Control and Status Register */
 	cortex_m_write_debug_halt_mask(target, C_HALT, 0);
 
+	/* Do this really early to minimize the window where the MASKINTS erratum
+	 * can pile up pending interrupts. */
+	cortex_m_set_maskints_for_halt(target);
+
 	target->debug_reason = DBG_REASON_DBGRQ;
 
 	return ERROR_OK;
@@ -626,11 +722,16 @@ static int cortex_m_soft_reset_halt(struct target *target)
 	uint32_t dcb_dhcsr = 0;
 	int retval, timeout = 0;
 
-	/* soft_reset_halt is deprecated on cortex_m as the same functionality
-	 * can be obtained by using 'reset halt' and 'cortex_m reset_config vectreset'
-	 * As this reset only used VC_CORERESET it would only ever reset the cortex_m
+	/* on single cortex_m MCU soft_reset_halt should be avoided as same functionality
+	 * can be obtained by using 'reset halt' and 'cortex_m reset_config vectreset'.
+	 * As this reset only uses VC_CORERESET it would only ever reset the cortex_m
 	 * core, not the peripherals */
-	LOG_WARNING("soft_reset_halt is deprecated, please use 'reset halt' instead.");
+	LOG_DEBUG("soft_reset_halt is discouraged, please use 'reset halt' instead.");
+
+	/* Set C_DEBUGEN */
+	retval = cortex_m_write_debug_halt_mask(target, 0, C_STEP | C_MASKINTS);
+	if (retval != ERROR_OK)
+		return retval;
 
 	/* Enter debug state on reset; restore DEMCR in endreset_event() */
 	retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DEMCR,
@@ -717,15 +818,19 @@ static int cortex_m_resume(struct target *target, int current,
 		 * in parallel with disabled interrupts can cause local faults
 		 * to not be taken.
 		 *
-		 * REVISIT this clearly breaks non-debug execution, since the
-		 * PRIMASK register state isn't saved/restored...  workaround
-		 * by never resuming app code after debug execution.
+		 * This breaks non-debug (application) execution if not
+		 * called from armv7m_start_algorithm() which saves registers.
 		 */
 		buf_set_u32(r->value, 0, 1, 1);
 		r->dirty = true;
 		r->valid = true;
 
-		/* Make sure we are in Thumb mode */
+		/* Make sure we are in Thumb mode, set xPSR.T bit */
+		/* armv7m_start_algorithm() initializes entire xPSR register.
+		 * This duplicity handles the case when cortex_m_resume()
+		 * is used with the debug_execution flag directly,
+		 * not called through armv7m_start_algorithm().
+		 */
 		r = armv7m->arm.cpsr;
 		buf_set_u32(r->value, 24, 1, 1);
 		r->dirty = true;
@@ -767,6 +872,7 @@ static int cortex_m_resume(struct target *target, int current,
 	}
 
 	/* Restart core */
+	cortex_m_set_maskints_for_run(target);
 	cortex_m_write_debug_halt_mask(target, 0, C_HALT);
 
 	target->debug_reason = DBG_REASON_NOTHALTED;
@@ -829,10 +935,12 @@ static int cortex_m_step(struct target *target, int current,
 	 * a normal step, otherwise we have to manually step over the bkpt
 	 * instruction - as such simulate a step */
 	if (bkpt_inst_found == false) {
-		/* Automatic ISR masking mode off: Just step over the next instruction */
-		if ((cortex_m->isrmasking_mode != CORTEX_M_ISRMASK_AUTO))
+		if (cortex_m->isrmasking_mode != CORTEX_M_ISRMASK_AUTO) {
+			/* Automatic ISR masking mode off: Just step over the next
+			 * instruction, with interrupts on or off as appropriate. */
+			cortex_m_set_maskints_for_step(target);
 			cortex_m_write_debug_halt_mask(target, C_STEP, C_HALT);
-		else {
+		} else {
 			/* Process interrupts during stepping in a way they don't interfere
 			 * debugging.
 			 *
@@ -859,7 +967,7 @@ static int cortex_m_step(struct target *target, int current,
 			 * just step over the instruction with interrupts disabled.
 			 *
 			 * The documentation has no information about this, it was found by observation
-			 * on STM32F1 and STM32F2. Proper explanation welcome. STM32F0 dosen't seem to
+			 * on STM32F1 and STM32F2. Proper explanation welcome. STM32F0 doesn't seem to
 			 * suffer from this problem.
 			 *
 			 * To add some confusion: pc_value has bit 0 always set, while the breakpoint
@@ -871,10 +979,10 @@ static int cortex_m_step(struct target *target, int current,
 				LOG_DEBUG("Stepping over next instruction with interrupts disabled");
 				cortex_m_write_debug_halt_mask(target, C_HALT | C_MASKINTS, 0);
 				cortex_m_write_debug_halt_mask(target, C_STEP, C_HALT);
-				/* Re-enable interrupts */
-				cortex_m_write_debug_halt_mask(target, C_HALT, C_MASKINTS);
-			}
-			else {
+				/* Re-enable interrupts if appropriate */
+				cortex_m_write_debug_halt_mask(target, C_HALT, 0);
+				cortex_m_set_maskints_for_halt(target);
+			} else {
 
 				/* Set a temporary break point */
 				if (breakpoint) {
@@ -891,12 +999,17 @@ static int cortex_m_step(struct target *target, int current,
 				bool tmp_bp_set = (retval == ERROR_OK);
 
 				/* No more breakpoints left, just do a step */
-				if (!tmp_bp_set)
+				if (!tmp_bp_set) {
+					cortex_m_set_maskints_for_step(target);
 					cortex_m_write_debug_halt_mask(target, C_STEP, C_HALT);
-				else {
+					/* Re-enable interrupts if appropriate */
+					cortex_m_write_debug_halt_mask(target, C_HALT, 0);
+					cortex_m_set_maskints_for_halt(target);
+				} else {
 					/* Start the core */
 					LOG_DEBUG("Starting core to serve pending interrupts");
 					int64_t t_start = timeval_ms();
+					cortex_m_set_maskints_for_run(target);
 					cortex_m_write_debug_halt_mask(target, 0, C_HALT | C_STEP);
 
 					/* Wait for pending handlers to complete or timeout */
@@ -924,12 +1037,14 @@ static int cortex_m_step(struct target *target, int current,
 							"leaving target running");
 					} else {
 						/* Step over next instruction with interrupts disabled */
+						cortex_m_set_maskints_for_step(target);
 						cortex_m_write_debug_halt_mask(target,
 							C_HALT | C_MASKINTS,
 							0);
 						cortex_m_write_debug_halt_mask(target, C_STEP, C_HALT);
-						/* Re-enable interrupts */
-						cortex_m_write_debug_halt_mask(target, C_HALT, C_MASKINTS);
+						/* Re-enable interrupts if appropriate */
+						cortex_m_write_debug_halt_mask(target, C_HALT, 0);
+						cortex_m_set_maskints_for_halt(target);
 					}
 				}
 			}
@@ -1031,8 +1146,7 @@ static int cortex_m_assert_reset(struct target *target)
 
 	if (!target->reset_halt) {
 		/* Set/Clear C_MASKINTS in a separate operation */
-		if (cortex_m->dcb_dhcsr & C_MASKINTS)
-			cortex_m_write_debug_halt_mask(target, 0, C_MASKINTS);
+		cortex_m_set_maskints_for_run(target);
 
 		/* clear any debug flags before resuming */
 		cortex_m_clear_halt(target);
@@ -1089,11 +1203,13 @@ static int cortex_m_assert_reset(struct target *target)
 		if (retval3 != ERROR_OK)
 			LOG_DEBUG("Ignoring AP write error right after reset");
 
-		retval3 = dap_dp_init(armv7m->debug_ap->dap);
-		if (retval3 != ERROR_OK)
+		retval3 = dap_dp_init_or_reconnect(armv7m->debug_ap->dap);
+		if (retval3 != ERROR_OK) {
 			LOG_ERROR("DP initialisation failed");
-
-		else {
+			/* The error return value must not be propagated in this case.
+			 * SYSRESETREQ or VECTRESET have been possibly triggered
+			 * so reset processing should continue */
+		} else {
 			/* I do not know why this is necessary, but it
 			 * fixes strange effects (step/resume cause NMI
 			 * after reset) on LM3S6918 -- Michael Schwingen
@@ -1104,7 +1220,7 @@ static int cortex_m_assert_reset(struct target *target)
 	}
 
 	target->state = TARGET_RESET;
-	jtag_add_sleep(50000);
+	jtag_sleep(50000);
 
 	register_cache_invalidate(cortex_m->armv7m.arm.core_cache);
 
@@ -1136,7 +1252,8 @@ static int cortex_m_deassert_reset(struct target *target)
 	if ((jtag_reset_config & RESET_HAS_SRST) &&
 	    !(jtag_reset_config & RESET_SRST_NO_GATING) &&
 		target_was_examined(target)) {
-		int retval = dap_dp_init(armv7m->debug_ap->dap);
+
+		int retval = dap_dp_init_or_reconnect(armv7m->debug_ap->dap);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("DP initialisation failed");
 			return retval;
@@ -1164,7 +1281,7 @@ int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint
 			fp_num++;
 		if (fp_num >= cortex_m->fp_num_code) {
 			LOG_ERROR("Can not find free FPB Comparator!");
-			return ERROR_FAIL;
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 		}
 		breakpoint->set = fp_num + 1;
 		fpcr_value = breakpoint->address | 1;
@@ -1180,7 +1297,7 @@ int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint
 			LOG_ERROR("Unhandled Cortex-M Flash Patch Breakpoint architecture revision");
 			return ERROR_FAIL;
 		}
-		comparator_list[fp_num].used = 1;
+		comparator_list[fp_num].used = true;
 		comparator_list[fp_num].fpcr_value = fpcr_value;
 		target_write_u32(target, comparator_list[fp_num].fpcr_address,
 			comparator_list[fp_num].fpcr_value);
@@ -1195,7 +1312,7 @@ int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint
 				return retval;
 			}
 
-			cortex_m->fpb_enabled = 1;
+			cortex_m->fpb_enabled = true;
 		}
 	} else if (breakpoint->type == BKPT_SOFT) {
 		uint8_t code[4];
@@ -1254,23 +1371,17 @@ int cortex_m_unset_breakpoint(struct target *target, struct breakpoint *breakpoi
 			LOG_DEBUG("Invalid FP Comparator number in breakpoint");
 			return ERROR_OK;
 		}
-		comparator_list[fp_num].used = 0;
+		comparator_list[fp_num].used = false;
 		comparator_list[fp_num].fpcr_value = 0;
 		target_write_u32(target, comparator_list[fp_num].fpcr_address,
 			comparator_list[fp_num].fpcr_value);
 	} else {
 		/* restore original instruction (kept in target endianness) */
-		if (breakpoint->length == 4) {
-			retval = target_write_memory(target, breakpoint->address & 0xFFFFFFFE, 4, 1,
-					breakpoint->orig_instr);
-			if (retval != ERROR_OK)
-				return retval;
-		} else {
-			retval = target_write_memory(target, breakpoint->address & 0xFFFFFFFE, 2, 1,
+		retval = target_write_memory(target, breakpoint->address & 0xFFFFFFFE,
+					breakpoint->length, 1,
 					breakpoint->orig_instr);
-			if (retval != ERROR_OK)
-				return retval;
-		}
+		if (retval != ERROR_OK)
+			return retval;
 	}
 	breakpoint->set = false;
 
@@ -1279,13 +1390,6 @@ int cortex_m_unset_breakpoint(struct target *target, struct breakpoint *breakpoi
 
 int cortex_m_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
 {
-	struct cortex_m_common *cortex_m = target_to_cm(target);
-
-	if ((breakpoint->type == BKPT_HARD) && (cortex_m->fp_code_available < 1)) {
-		LOG_INFO("no flash patch comparator unit available for hardware breakpoint");
-		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-	}
-
 	if (breakpoint->length == 3) {
 		LOG_DEBUG("Using a two byte breakpoint for 32bit Thumb-2 request");
 		breakpoint->length = 2;
@@ -1296,46 +1400,22 @@ int cortex_m_add_breakpoint(struct target *target, struct breakpoint *breakpoint
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
 
-	if (breakpoint->type == BKPT_HARD)
-		cortex_m->fp_code_available--;
-
 	return cortex_m_set_breakpoint(target, breakpoint);
 }
 
 int cortex_m_remove_breakpoint(struct target *target, struct breakpoint *breakpoint)
 {
-	struct cortex_m_common *cortex_m = target_to_cm(target);
-
-	/* REVISIT why check? FPB can be updated with core running ... */
-	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
-		return ERROR_TARGET_NOT_HALTED;
-	}
-
-	if (breakpoint->set)
-		cortex_m_unset_breakpoint(target, breakpoint);
-
-	if (breakpoint->type == BKPT_HARD)
-		cortex_m->fp_code_available++;
+	if (!breakpoint->set)
+		return ERROR_OK;
 
-	return ERROR_OK;
+	return cortex_m_unset_breakpoint(target, breakpoint);
 }
 
-int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint)
+static int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint)
 {
 	int dwt_num = 0;
-	uint32_t mask, temp;
 	struct cortex_m_common *cortex_m = target_to_cm(target);
 
-	/* watchpoint params were validated earlier */
-	mask = 0;
-	temp = watchpoint->length;
-	while (temp) {
-		temp >>= 1;
-		mask++;
-	}
-	mask--;
-
 	/* REVISIT Don't fully trust these "not used" records ... users
 	 * may set up breakpoints by hand, e.g. dual-address data value
 	 * watchpoint using comparator #1; comparator #0 matching cycle
@@ -1351,18 +1431,29 @@ int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint
 		LOG_ERROR("Can not find free DWT Comparator");
 		return ERROR_FAIL;
 	}
-	comparator->used = 1;
+	comparator->used = true;
 	watchpoint->set = dwt_num + 1;
 
 	comparator->comp = watchpoint->address;
 	target_write_u32(target, comparator->dwt_comparator_address + 0,
 		comparator->comp);
 
-	comparator->mask = mask;
-	target_write_u32(target, comparator->dwt_comparator_address + 4,
-		comparator->mask);
+	if ((cortex_m->dwt_devarch & 0x1FFFFF) != DWT_DEVARCH_ARMV8M) {
+		uint32_t mask = 0, temp;
+
+		/* watchpoint params were validated earlier */
+		temp = watchpoint->length;
+		while (temp) {
+			temp >>= 1;
+			mask++;
+		}
+		mask--;
+
+		comparator->mask = mask;
+		target_write_u32(target, comparator->dwt_comparator_address + 4,
+			comparator->mask);
 
-	switch (watchpoint->rw) {
+		switch (watchpoint->rw) {
 		case WPT_READ:
 			comparator->function = 5;
 			break;
@@ -1372,7 +1463,26 @@ int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint
 		case WPT_ACCESS:
 			comparator->function = 7;
 			break;
+		}
+	} else {
+		uint32_t data_size = watchpoint->length >> 1;
+		comparator->mask = (watchpoint->length >> 1) | 1;
+
+		switch (watchpoint->rw) {
+		case WPT_ACCESS:
+			comparator->function = 4;
+			break;
+		case WPT_WRITE:
+			comparator->function = 5;
+			break;
+		case WPT_READ:
+			comparator->function = 6;
+			break;
+		}
+		comparator->function = comparator->function | (1 << 4) |
+				(data_size << 10);
 	}
+
 	target_write_u32(target, comparator->dwt_comparator_address + 8,
 		comparator->function);
 
@@ -1384,7 +1494,7 @@ int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint
 	return ERROR_OK;
 }
 
-int cortex_m_unset_watchpoint(struct target *target, struct watchpoint *watchpoint)
+static int cortex_m_unset_watchpoint(struct target *target, struct watchpoint *watchpoint)
 {
 	struct cortex_m_common *cortex_m = target_to_cm(target);
 	struct cortex_m_dwt_comparator *comparator;
@@ -1408,7 +1518,7 @@ int cortex_m_unset_watchpoint(struct target *target, struct watchpoint *watchpoi
 	}
 
 	comparator = cortex_m->dwt_comparator_list + dwt_num;
-	comparator->used = 0;
+	comparator->used = false;
 	comparator->function = 0;
 	target_write_u32(target, comparator->dwt_comparator_address + 8,
 		comparator->function);
@@ -1498,176 +1608,6 @@ void cortex_m_enable_watchpoints(struct target *target)
 	}
 }
 
-static int cortex_m_load_core_reg_u32(struct target *target,
-		uint32_t num, uint32_t *value)
-{
-	int retval;
-
-	/* NOTE:  we "know" here that the register identifiers used
-	 * in the v7m header match the Cortex-M3 Debug Core Register
-	 * Selector values for R0..R15, xPSR, MSP, and PSP.
-	 */
-	switch (num) {
-		case 0 ... 18:
-			/* read a normal core register */
-			retval = cortexm_dap_read_coreregister_u32(target, value, num);
-
-			if (retval != ERROR_OK) {
-				LOG_ERROR("JTAG failure %i", retval);
-				return ERROR_JTAG_DEVICE_ERROR;
-			}
-			LOG_DEBUG("load from core reg %i  value 0x%" PRIx32 "", (int)num, *value);
-			break;
-
-		case ARMV7M_FPSCR:
-			/* Floating-point Status and Registers */
-			retval = target_write_u32(target, DCB_DCRSR, 0x21);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_read_u32(target, DCB_DCRDR, value);
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_DEBUG("load from FPSCR  value 0x%" PRIx32, *value);
-			break;
-
-		case ARMV7M_S0 ... ARMV7M_S31:
-			/* Floating-point Status and Registers */
-			retval = target_write_u32(target, DCB_DCRSR, num - ARMV7M_S0 + 0x40);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_read_u32(target, DCB_DCRDR, value);
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_DEBUG("load from FPU reg S%d  value 0x%" PRIx32,
-				  (int)(num - ARMV7M_S0), *value);
-			break;
-
-		case ARMV7M_PRIMASK:
-		case ARMV7M_BASEPRI:
-		case ARMV7M_FAULTMASK:
-		case ARMV7M_CONTROL:
-			/* Cortex-M3 packages these four registers as bitfields
-			 * in one Debug Core register.  So say r0 and r2 docs;
-			 * it was removed from r1 docs, but still works.
-			 */
-			cortexm_dap_read_coreregister_u32(target, value, 20);
-
-			switch (num) {
-				case ARMV7M_PRIMASK:
-					*value = buf_get_u32((uint8_t *)value, 0, 1);
-					break;
-
-				case ARMV7M_BASEPRI:
-					*value = buf_get_u32((uint8_t *)value, 8, 8);
-					break;
-
-				case ARMV7M_FAULTMASK:
-					*value = buf_get_u32((uint8_t *)value, 16, 1);
-					break;
-
-				case ARMV7M_CONTROL:
-					*value = buf_get_u32((uint8_t *)value, 24, 2);
-					break;
-			}
-
-			LOG_DEBUG("load from special reg %i value 0x%" PRIx32 "", (int)num, *value);
-			break;
-
-		default:
-			return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	return ERROR_OK;
-}
-
-static int cortex_m_store_core_reg_u32(struct target *target,
-		uint32_t num, uint32_t value)
-{
-	int retval;
-	uint32_t reg;
-	struct armv7m_common *armv7m = target_to_armv7m(target);
-
-	/* NOTE:  we "know" here that the register identifiers used
-	 * in the v7m header match the Cortex-M3 Debug Core Register
-	 * Selector values for R0..R15, xPSR, MSP, and PSP.
-	 */
-	switch (num) {
-		case 0 ... 18:
-			retval = cortexm_dap_write_coreregister_u32(target, value, num);
-			if (retval != ERROR_OK) {
-				struct reg *r;
-
-				LOG_ERROR("JTAG failure");
-				r = armv7m->arm.core_cache->reg_list + num;
-				r->dirty = r->valid;
-				return ERROR_JTAG_DEVICE_ERROR;
-			}
-			LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value);
-			break;
-
-		case ARMV7M_FPSCR:
-			/* Floating-point Status and Registers */
-			retval = target_write_u32(target, DCB_DCRDR, value);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_write_u32(target, DCB_DCRSR, 0x21 | (1<<16));
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_DEBUG("write FPSCR value 0x%" PRIx32, value);
-			break;
-
-		case ARMV7M_S0 ... ARMV7M_S31:
-			/* Floating-point Status and Registers */
-			retval = target_write_u32(target, DCB_DCRDR, value);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_write_u32(target, DCB_DCRSR, (num - ARMV7M_S0 + 0x40) | (1<<16));
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_DEBUG("write FPU reg S%d  value 0x%" PRIx32,
-				  (int)(num - ARMV7M_S0), value);
-			break;
-
-		case ARMV7M_PRIMASK:
-		case ARMV7M_BASEPRI:
-		case ARMV7M_FAULTMASK:
-		case ARMV7M_CONTROL:
-			/* Cortex-M3 packages these four registers as bitfields
-			 * in one Debug Core register.  So say r0 and r2 docs;
-			 * it was removed from r1 docs, but still works.
-			 */
-			cortexm_dap_read_coreregister_u32(target, &reg, 20);
-
-			switch (num) {
-				case ARMV7M_PRIMASK:
-					buf_set_u32((uint8_t *)&reg, 0, 1, value);
-					break;
-
-				case ARMV7M_BASEPRI:
-					buf_set_u32((uint8_t *)&reg, 8, 8, value);
-					break;
-
-				case ARMV7M_FAULTMASK:
-					buf_set_u32((uint8_t *)&reg, 16, 1, value);
-					break;
-
-				case ARMV7M_CONTROL:
-					buf_set_u32((uint8_t *)&reg, 24, 2, value);
-					break;
-			}
-
-			cortexm_dap_write_coreregister_u32(target, reg, 20);
-
-			LOG_DEBUG("write special reg %i value 0x%" PRIx32 " ", (int)num, value);
-			break;
-
-		default:
-			return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	return ERROR_OK;
-}
-
 static int cortex_m_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
@@ -1708,6 +1648,8 @@ void cortex_m_deinit_target(struct target *target)
 {
 	struct cortex_m_common *cortex_m = target_to_cm(target);
 
+	armv7m_trace_tpiu_exit(target);
+
 	free(cortex_m->fp_comparator_list);
 
 	cortex_m_dwt_free(target);
@@ -1723,28 +1665,23 @@ int cortex_m_profiling(struct target *target, uint32_t *samples,
 	struct timeval timeout, now;
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	uint32_t reg_value;
-	bool use_pcsr = false;
-	int retval = ERROR_OK;
-	struct reg *reg;
-
-	gettimeofday(&timeout, NULL);
-	timeval_add_time(&timeout, seconds, 0);
+	int retval;
 
 	retval = target_read_u32(target, DWT_PCSR, &reg_value);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("Error while reading PCSR");
 		return retval;
 	}
-
-	if (reg_value != 0) {
-		use_pcsr = true;
-		LOG_INFO("Starting Cortex-M profiling. Sampling DWT_PCSR as fast as we can...");
-	} else {
-		LOG_INFO("Starting profiling. Halting and resuming the"
-			 " target as often as we can...");
-		reg = register_get_by_name(target->reg_cache, "pc", 1);
+	if (reg_value == 0) {
+		LOG_INFO("PCSR sampling not supported on this processor.");
+		return target_profiling_default(target, samples, max_num_samples, num_samples, seconds);
 	}
 
+	gettimeofday(&timeout, NULL);
+	timeval_add_time(&timeout, seconds, 0);
+
+	LOG_INFO("Starting Cortex-M profiling. Sampling DWT_PCSR as fast as we can...");
+
 	/* Make sure the target is running */
 	target_poll(target);
 	if (target->state == TARGET_HALTED)
@@ -1758,40 +1695,21 @@ int cortex_m_profiling(struct target *target, uint32_t *samples,
 	uint32_t sample_count = 0;
 
 	for (;;) {
-		if (use_pcsr) {
-			if (armv7m && armv7m->debug_ap) {
-				uint32_t read_count = max_num_samples - sample_count;
-				if (read_count > 1024)
-					read_count = 1024;
-
-				retval = mem_ap_read_buf_noincr(armv7m->debug_ap,
-							(void *)&samples[sample_count],
-							4, read_count, DWT_PCSR);
-				sample_count += read_count;
-			} else {
-				target_read_u32(target, DWT_PCSR, &samples[sample_count++]);
-			}
+		if (armv7m && armv7m->debug_ap) {
+			uint32_t read_count = max_num_samples - sample_count;
+			if (read_count > 1024)
+				read_count = 1024;
+
+			retval = mem_ap_read_buf_noincr(armv7m->debug_ap,
+						(void *)&samples[sample_count],
+						4, read_count, DWT_PCSR);
+			sample_count += read_count;
 		} else {
-			target_poll(target);
-			if (target->state == TARGET_HALTED) {
-				reg_value = buf_get_u32(reg->value, 0, 32);
-				/* current pc, addr = 0, do not handle breakpoints, not debugging */
-				retval = target_resume(target, 1, 0, 0, 0);
-				samples[sample_count++] = reg_value;
-				target_poll(target);
-				alive_sleep(10); /* sleep 10ms, i.e. <100 samples/second. */
-			} else if (target->state == TARGET_RUNNING) {
-				/* We want to quickly sample the PC. */
-				retval = target_halt(target);
-			} else {
-				LOG_INFO("Target not halted or running");
-				retval = ERROR_OK;
-				break;
-			}
+			target_read_u32(target, DWT_PCSR, &samples[sample_count++]);
 		}
 
 		if (retval != ERROR_OK) {
-			LOG_ERROR("Error while reading %s", use_pcsr ? "PCSR" : "target pc");
+			LOG_ERROR("Error while reading PCSR");
 			return retval;
 		}
 
@@ -1888,7 +1806,7 @@ static void cortex_m_dwt_addreg(struct target *t, struct reg *r, const struct dw
 {
 	struct dwt_reg_state *state;
 
-	state = calloc(1, sizeof *state);
+	state = calloc(1, sizeof(*state));
 	if (!state)
 		return;
 	state->addr = d->addr;
@@ -1901,7 +1819,7 @@ static void cortex_m_dwt_addreg(struct target *t, struct reg *r, const struct dw
 	r->type = &dwt_reg_type;
 }
 
-void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target)
+static void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target)
 {
 	uint32_t dwtcr;
 	struct reg_cache *cache;
@@ -1915,6 +1833,9 @@ void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target)
 		return;
 	}
 
+	target_read_u32(target, DWT_DEVARCH, &cm->dwt_devarch);
+	LOG_DEBUG("DWT_DEVARCH: 0x%" PRIx32, cm->dwt_devarch);
+
 	cm->dwt_num_comp = (dwtcr >> 28) & 0xF;
 	cm->dwt_comp_available = cm->dwt_num_comp;
 	cm->dwt_comparator_list = calloc(cm->dwt_num_comp,
@@ -1926,7 +1847,7 @@ fail0:
 		return;
 	}
 
-	cache = calloc(1, sizeof *cache);
+	cache = calloc(1, sizeof(*cache));
 	if (!cache) {
 fail1:
 		free(cm->dwt_comparator_list);
@@ -1934,7 +1855,7 @@ fail1:
 	}
 	cache->name = "Cortex-M DWT registers";
 	cache->num_regs = 2 + cm->dwt_num_comp * 3;
-	cache->reg_list = calloc(cache->num_regs, sizeof *cache->reg_list);
+	cache->reg_list = calloc(cache->num_regs, sizeof(*cache->reg_list));
 	if (!cache->reg_list) {
 		free(cache);
 		goto fail1;
@@ -2003,6 +1924,15 @@ static void cortex_m_dwt_free(struct target *target)
 #define MVFR1_DEFAULT_M7_SP 0x11000011
 #define MVFR1_DEFAULT_M7_DP 0x12000011
 
+static int cortex_m_find_mem_ap(struct adiv5_dap *swjdp,
+		struct adiv5_ap **debug_ap)
+{
+	if (dap_find_ap(swjdp, AP_TYPE_AHB3_AP, debug_ap) == ERROR_OK)
+		return ERROR_OK;
+
+	return dap_find_ap(swjdp, AP_TYPE_AHB5_AP, debug_ap);
+}
+
 int cortex_m_examine(struct target *target)
 {
 	int retval;
@@ -2017,7 +1947,7 @@ int cortex_m_examine(struct target *target)
 	if (!armv7m->stlink) {
 		if (cortex_m->apsel == DP_APSEL_INVALID) {
 			/* Search for the MEM-AP */
-			retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7m->debug_ap);
+			retval = cortex_m_find_mem_ap(swjdp, &armv7m->debug_ap);
 			if (retval != ERROR_OK) {
 				LOG_ERROR("Could not find MEM-AP to control the core");
 				return retval;
@@ -2045,14 +1975,39 @@ int cortex_m_examine(struct target *target)
 		/* Get CPU Type */
 		i = (cpuid >> 4) & 0xf;
 
+		/* Check if it is an ARMv8-M core */
+		armv7m->arm.is_armv8m = true;
+
+		switch (cpuid & ARM_CPUID_PARTNO_MASK) {
+			case CORTEX_M23_PARTNO:
+				i = 23;
+				break;
+			case CORTEX_M33_PARTNO:
+				i = 33;
+				break;
+			case CORTEX_M35P_PARTNO:
+				i = 35;
+				break;
+			case CORTEX_M55_PARTNO:
+				i = 55;
+				break;
+			default:
+				armv7m->arm.is_armv8m = false;
+				break;
+		}
+
+
 		LOG_DEBUG("Cortex-M%d r%" PRId8 "p%" PRId8 " processor detected",
 				i, (uint8_t)((cpuid >> 20) & 0xf), (uint8_t)((cpuid >> 0) & 0xf));
+		cortex_m->maskints_erratum = false;
 		if (i == 7) {
 			uint8_t rev, patch;
 			rev = (cpuid >> 20) & 0xf;
 			patch = (cpuid >> 0) & 0xf;
-			if ((rev == 0) && (patch < 2))
-				LOG_WARNING("Silicon bug: single stepping will enter pending exception handler!");
+			if ((rev == 0) && (patch < 2)) {
+				LOG_WARNING("Silicon bug: single stepping may enter pending exception handler!");
+				cortex_m->maskints_erratum = true;
+			}
 		}
 		LOG_DEBUG("cpuid: 0x%8.8" PRIx32 "", cpuid);
 
@@ -2068,7 +2023,7 @@ int cortex_m_examine(struct target *target)
 				LOG_DEBUG("Cortex-M%d floating point feature FPv4_SP found", i);
 				armv7m->fp_feature = FPv4_SP;
 			}
-		} else if (i == 7) {
+		} else if (i == 7 || i == 33 || i == 35 || i == 55) {
 			target_read_u32(target, MVFR0, &mvfr0);
 			target_read_u32(target, MVFR1, &mvfr1);
 
@@ -2093,7 +2048,6 @@ int cortex_m_examine(struct target *target)
 			for (idx = ARMV7M_NUM_CORE_REGS_NOFP;
 			     idx < armv7m->arm.core_cache->num_regs;
 			     idx++) {
-				free(armv7m->arm.core_cache->reg_list[idx].value);
 				free(armv7m->arm.core_cache->reg_list[idx].feature);
 				free(armv7m->arm.core_cache->reg_list[idx].reg_data_type);
 			}
@@ -2110,6 +2064,19 @@ int cortex_m_examine(struct target *target)
 				armv7m->debug_ap->tar_autoincr_block = (1 << 10);
 		}
 
+		/* Enable debug requests */
+		retval = target_read_u32(target, DCB_DHCSR, &cortex_m->dcb_dhcsr);
+		if (retval != ERROR_OK)
+			return retval;
+		if (!(cortex_m->dcb_dhcsr & C_DEBUGEN)) {
+			uint32_t dhcsr = (cortex_m->dcb_dhcsr | C_DEBUGEN) & ~(C_HALT | C_STEP | C_MASKINTS);
+
+			retval = target_write_u32(target, DCB_DHCSR, DBGKEY | (dhcsr & 0x0000FFFFUL));
+			if (retval != ERROR_OK)
+				return retval;
+			cortex_m->dcb_dhcsr = dhcsr;
+		}
+
 		/* Configure trace modules */
 		retval = target_write_u32(target, DCB_DEMCR, TRCENA | armv7m->demcr);
 		if (retval != ERROR_OK)
@@ -2127,7 +2094,6 @@ int cortex_m_examine(struct target *target)
 		/* bits [14:12] and [7:4] */
 		cortex_m->fp_num_code = ((fpcr >> 8) & 0x70) | ((fpcr >> 4) & 0xF);
 		cortex_m->fp_num_lit = (fpcr >> 8) & 0xF;
-		cortex_m->fp_code_available = cortex_m->fp_num_code;
 		/* Detect flash patch revision, see RM DDI 0403E.b page C1-817.
 		   Revision is zero base, fp_rev == 1 means Rev.2 ! */
 		cortex_m->fp_rev = (fpcr >> 28) & 0xf;
@@ -2269,21 +2235,27 @@ static int cortex_m_init_arch_info(struct target *target,
 	armv7m->load_core_reg_u32 = cortex_m_load_core_reg_u32;
 	armv7m->store_core_reg_u32 = cortex_m_store_core_reg_u32;
 
-	target_register_timer_callback(cortex_m_handle_target_request, 1, 1, target);
+	target_register_timer_callback(cortex_m_handle_target_request, 1,
+		TARGET_TIMER_TYPE_PERIODIC, target);
 
 	return ERROR_OK;
 }
 
 static int cortex_m_target_create(struct target *target, Jim_Interp *interp)
 {
-	struct cortex_m_common *cortex_m = calloc(1, sizeof(struct cortex_m_common));
-	cortex_m->common_magic = CORTEX_M_COMMON_MAGIC;
 	struct adiv5_private_config *pc;
 
 	pc = (struct adiv5_private_config *)target->private_config;
 	if (adiv5_verify_config(pc) != ERROR_OK)
 		return ERROR_FAIL;
 
+	struct cortex_m_common *cortex_m = calloc(1, sizeof(struct cortex_m_common));
+	if (cortex_m == NULL) {
+		LOG_ERROR("No memory creating target");
+		return ERROR_FAIL;
+	}
+
+	cortex_m->common_magic = CORTEX_M_COMMON_MAGIC;
 	cortex_m->apsel = pc->ap_num;
 
 	cortex_m_init_arch_info(target, cortex_m, pc->dap);
@@ -2293,11 +2265,11 @@ static int cortex_m_target_create(struct target *target, Jim_Interp *interp)
 
 /*--------------------------------------------------------------------------*/
 
-static int cortex_m_verify_pointer(struct command_context *cmd_ctx,
+static int cortex_m_verify_pointer(struct command_invocation *cmd,
 	struct cortex_m_common *cm)
 {
 	if (cm->common_magic != CORTEX_M_COMMON_MAGIC) {
-		command_print(cmd_ctx, "target is not a Cortex-M");
+		command_print(cmd, "target is not a Cortex-M");
 		return ERROR_TARGET_INVALID;
 	}
 	return ERROR_OK;
@@ -2331,10 +2303,15 @@ COMMAND_HANDLER(handle_cortex_m_vector_catch_command)
 		{ "reset",      VC_CORERESET, },
 	};
 
-	retval = cortex_m_verify_pointer(CMD_CTX, cortex_m);
+	retval = cortex_m_verify_pointer(CMD, cortex_m);
 	if (retval != ERROR_OK)
 		return retval;
 
+	if (!target_was_examined(target)) {
+		LOG_ERROR("Target not examined yet");
+		return ERROR_FAIL;
+	}
+
 	retval = mem_ap_read_atomic_u32(armv7m->debug_ap, DCB_DEMCR, &demcr);
 	if (retval != ERROR_OK)
 		return retval;
@@ -2386,7 +2363,7 @@ write:
 	}
 
 	for (unsigned i = 0; i < ARRAY_SIZE(vec_ids); i++) {
-		command_print(CMD_CTX, "%9s: %s", vec_ids[i].name,
+		command_print(CMD, "%9s: %s", vec_ids[i].name,
 			(demcr & vec_ids[i].mask) ? "catch" : "ignore");
 	}
 
@@ -2403,17 +2380,18 @@ COMMAND_HANDLER(handle_cortex_m_mask_interrupts_command)
 		{ .name = "auto", .value = CORTEX_M_ISRMASK_AUTO },
 		{ .name = "off", .value = CORTEX_M_ISRMASK_OFF },
 		{ .name = "on", .value = CORTEX_M_ISRMASK_ON },
+		{ .name = "steponly", .value = CORTEX_M_ISRMASK_STEPONLY },
 		{ .name = NULL, .value = -1 },
 	};
 	const Jim_Nvp *n;
 
 
-	retval = cortex_m_verify_pointer(CMD_CTX, cortex_m);
+	retval = cortex_m_verify_pointer(CMD, cortex_m);
 	if (retval != ERROR_OK)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD_CTX, "target must be stopped for \"%s\" command", CMD_NAME);
+		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
 		return ERROR_OK;
 	}
 
@@ -2422,16 +2400,11 @@ COMMAND_HANDLER(handle_cortex_m_mask_interrupts_command)
 		if (n->name == NULL)
 			return ERROR_COMMAND_SYNTAX_ERROR;
 		cortex_m->isrmasking_mode = n->value;
-
-
-		if (cortex_m->isrmasking_mode == CORTEX_M_ISRMASK_ON)
-			cortex_m_write_debug_halt_mask(target, C_HALT | C_MASKINTS, 0);
-		else
-			cortex_m_write_debug_halt_mask(target, C_HALT, C_MASKINTS);
+		cortex_m_set_maskints_for_halt(target);
 	}
 
 	n = Jim_Nvp_value2name_simple(nvp_maskisr_modes, cortex_m->isrmasking_mode);
-	command_print(CMD_CTX, "cortex_m interrupt mask %s", n->name);
+	command_print(CMD, "cortex_m interrupt mask %s", n->name);
 
 	return ERROR_OK;
 }
@@ -2443,7 +2416,7 @@ COMMAND_HANDLER(handle_cortex_m_reset_config_command)
 	int retval;
 	char *reset_config;
 
-	retval = cortex_m_verify_pointer(CMD_CTX, cortex_m);
+	retval = cortex_m_verify_pointer(CMD, cortex_m);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -2476,7 +2449,7 @@ COMMAND_HANDLER(handle_cortex_m_reset_config_command)
 			break;
 	}
 
-	command_print(CMD_CTX, "cortex_m reset_config %s", reset_config);
+	command_print(CMD, "cortex_m reset_config %s", reset_config);
 
 	return ERROR_OK;
 }
@@ -2487,7 +2460,7 @@ static const struct command_registration cortex_m_exec_command_handlers[] = {
 		.handler = handle_cortex_m_mask_interrupts_command,
 		.mode = COMMAND_EXEC,
 		.help = "mask cortex_m interrupts",
-		.usage = "['auto'|'on'|'off']",
+		.usage = "['auto'|'on'|'off'|'steponly']",
 	},
 	{
 		.name = "vector_catch",
@@ -2519,6 +2492,9 @@ static const struct command_registration cortex_m_command_handlers[] = {
 		.usage = "",
 		.chain = cortex_m_exec_command_handlers,
 	},
+	{
+		.chain = rtt_target_command_handlers,
+	},
 	COMMAND_REGISTRATION_DONE
 };