Merge commit '05ee88915520d1dd82da94a016a9374a1f3a8129' into from_upstream

Conflicts:
	src/jtag/drivers/xds110.c
	src/target/riscv/riscv.c
	src/target/riscv/riscv_semihosting.c
	tcl/target/esp_common.cfg

Change-Id: If0c02817df03b7fd700cc84b4da2c02d36737d28

49 files changed, 706 insertions, 318 deletions

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 5a16b3a..d25c7d3 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -846,8 +846,10 @@ static int aarch64_resume(struct target *target, int current,
 	struct armv8_common *armv8 = target_to_armv8(target);
 	armv8->last_run_control_op = ARMV8_RUNCONTROL_RESUME;
 
-	if (target->state != TARGET_HALTED)
+	if (target->state != TARGET_HALTED) {
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
+	}
 
 	/*
 	 * If this target is part of a SMP group, prepare the others
@@ -1089,13 +1091,14 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	int saved_retval = ERROR_OK;
+	int poll_retval;
 	int retval;
 	uint32_t edecr;
 
 	armv8->last_run_control_op = ARMV8_RUNCONTROL_STEP;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1171,6 +1174,8 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	if (retval == ERROR_TARGET_TIMEOUT)
 		saved_retval = aarch64_halt_one(target, HALT_SYNC);
 
+	poll_retval = aarch64_poll(target);
+
 	/* restore EDECR */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_EDECR, edecr);
@@ -1187,6 +1192,9 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	if (saved_retval != ERROR_OK)
 		return saved_retval;
 
+	if (poll_retval != ERROR_OK)
+		return poll_retval;
+
 	return ERROR_OK;
 }
 
@@ -2135,7 +2143,7 @@ static int aarch64_write_cpu_memory(struct target *target,
 	uint32_t dscr;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2353,7 +2361,7 @@ static int aarch64_read_cpu_memory(struct target *target,
 			address, size, count);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2693,6 +2701,9 @@ static int aarch64_examine(struct target *target)
 	if (retval == ERROR_OK)
 		retval = aarch64_init_debug_access(target);
 
+	if (retval == ERROR_OK)
+		retval = aarch64_poll(target);
+
 	return retval;
 }
 
@@ -2790,8 +2801,8 @@ static int aarch64_mmu(struct target *target, int *enabled)
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = &aarch64->armv8_common;
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("%s: target %s not halted", __func__, target_name(target));
-		return ERROR_TARGET_INVALID;
+		LOG_TARGET_ERROR(target, "not halted");
+		return ERROR_TARGET_NOT_HALTED;
 	}
 	if (armv8->is_armv8r)
 		*enabled = 0;
@@ -3010,8 +3021,10 @@ COMMAND_HANDLER(aarch64_mcrmrc_command)
 		return ERROR_FAIL;
 	}
 
-	if (target->state != TARGET_HALTED)
+	if (target->state != TARGET_HALTED) {
+		command_print(CMD, "Error: [%s] not halted", target_name(target));
 		return ERROR_TARGET_NOT_HALTED;
+	}
 
 	if (arm->core_state == ARM_STATE_AARCH64) {
 		command_print(CMD, "%s: not 32-bit arm target", target_name(target));
diff --git a/src/target/arc.c b/src/target/arc.c
index 9ae3ae6..2ca6be1 100644
--- a/src/target/arc.c
+++ b/src/target/arc.c
@@ -846,21 +846,17 @@ static int arc_save_context(struct target *target)
 	memset(aux_addrs, 0xff, aux_regs_size);
 
 	for (i = 0; i < MIN(arc->num_core_regs, regs_to_scan); i++) {
-		struct reg *reg = &(reg_list[i]);
+		struct reg *reg = reg_list + i;
 		struct arc_reg_desc *arc_reg = reg->arch_info;
-		if (!reg->valid && reg->exist) {
-			core_addrs[core_cnt] = arc_reg->arch_num;
-			core_cnt += 1;
-		}
+		if (!reg->valid && reg->exist)
+			core_addrs[core_cnt++] = arc_reg->arch_num;
 	}
 
 	for (i = arc->num_core_regs; i < regs_to_scan; i++) {
-		struct reg *reg = &(reg_list[i]);
+		struct reg *reg = reg_list + i;
 		struct arc_reg_desc *arc_reg = reg->arch_info;
-		if (!reg->valid && reg->exist) {
-			aux_addrs[aux_cnt] = arc_reg->arch_num;
-			aux_cnt += 1;
-		}
+		if (!reg->valid && reg->exist)
+			aux_addrs[aux_cnt++] = arc_reg->arch_num;
 	}
 
 	/* Read data from target. */
@@ -884,30 +880,30 @@ static int arc_save_context(struct target *target)
 	/* Parse core regs */
 	core_cnt = 0;
 	for (i = 0; i < MIN(arc->num_core_regs, regs_to_scan); i++) {
-		struct reg *reg = &(reg_list[i]);
+		struct reg *reg = reg_list + i;
 		struct arc_reg_desc *arc_reg = reg->arch_info;
 		if (!reg->valid && reg->exist) {
 			target_buffer_set_u32(target, reg->value, core_values[core_cnt]);
-			core_cnt += 1;
 			reg->valid = true;
 			reg->dirty = false;
 			LOG_DEBUG("Get core register regnum=%u, name=%s, value=0x%08" PRIx32,
 				i, arc_reg->name, core_values[core_cnt]);
+			core_cnt++;
 		}
 	}
 
 	/* Parse aux regs */
 	aux_cnt = 0;
 	for (i = arc->num_core_regs; i < regs_to_scan; i++) {
-		struct reg *reg = &(reg_list[i]);
+		struct reg *reg = reg_list + i;
 		struct arc_reg_desc *arc_reg = reg->arch_info;
 		if (!reg->valid && reg->exist) {
 			target_buffer_set_u32(target, reg->value, aux_values[aux_cnt]);
-			aux_cnt += 1;
 			reg->valid = true;
 			reg->dirty = false;
 			LOG_DEBUG("Get aux register regnum=%u, name=%s, value=0x%08" PRIx32,
 				i, arc_reg->name, aux_values[aux_cnt]);
+			aux_cnt++;
 		}
 	}
 
@@ -1262,7 +1258,7 @@ static int arc_resume(struct target *target, int current, target_addr_t address,
 	CHECK_RETVAL(arc_reset_caches_states(target));
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1675,7 +1671,7 @@ static int arc_add_breakpoint(struct target *target, struct breakpoint *breakpoi
 		return arc_set_breakpoint(target, breakpoint);
 
 	} else {
-		LOG_WARNING(" > core was not halted, please try again.");
+		LOG_TARGET_ERROR(target, "not halted (add breakpoint)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 }
@@ -1687,7 +1683,7 @@ static int arc_remove_breakpoint(struct target *target,
 		if (breakpoint->is_set)
 			CHECK_RETVAL(arc_unset_breakpoint(target, breakpoint));
 	} else {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted (remove breakpoint)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1909,7 +1905,7 @@ static int arc_add_watchpoint(struct target *target,
 	struct watchpoint *watchpoint)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1922,7 +1918,7 @@ static int arc_remove_watchpoint(struct target *target,
 	struct watchpoint *watchpoint)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2010,7 +2006,7 @@ static int arc_step(struct target *target, int current, target_addr_t address,
 	struct reg *pc = &(arc->core_and_aux_cache->reg_list[arc->pc_index_in_cache]);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/arc_mem.c b/src/target/arc_mem.c
index c4814d2..3264b66 100644
--- a/src/target/arc_mem.c
+++ b/src/target/arc_mem.c
@@ -162,7 +162,7 @@ int arc_mem_write(struct target *target, target_addr_t address, uint32_t size,
 		address, size, count);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/arm.h b/src/target/arm.h
index fd61d5f..f3abd6c 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -58,6 +58,11 @@ enum arm_arch {
 	ARM_ARCH_V8M,
 };
 
+/** Known ARM implementor IDs */
+enum arm_implementor {
+	ARM_IMPLEMENTOR_ARM = 0x41,
+};
+
 /**
  * Represent state of an ARM core.
  *
diff --git a/src/target/arm11.c b/src/target/arm11.c
index e48bcf3..50aaa86 100644
--- a/src/target/arm11.c
+++ b/src/target/arm11.c
@@ -449,7 +449,7 @@ static int arm11_resume(struct target *target, int current,
 
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -551,7 +551,7 @@ static int arm11_step(struct target *target, int current,
 		target_state_name(target));
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target was not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -798,7 +798,7 @@ static int arm11_read_memory_inner(struct target *target,
 	int retval;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target was not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -896,7 +896,7 @@ static int arm11_write_memory_inner(struct target *target,
 	int retval;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target was not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/arm720t.c b/src/target/arm720t.c
index c330dff..beab632 100644
--- a/src/target/arm720t.c
+++ b/src/target/arm720t.c
@@ -241,8 +241,8 @@ static int arm720t_arch_state(struct target *target)
 static int arm720_mmu(struct target *target, int *enabled)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("%s: target not halted", __func__);
-		return ERROR_TARGET_INVALID;
+		LOG_TARGET_ERROR(target, "not halted");
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	*enabled = target_to_arm720(target)->armv4_5_mmu.mmu_enabled;
diff --git a/src/target/arm7_9_common.c b/src/target/arm7_9_common.c
index f60777d..ad814e0 100644
--- a/src/target/arm7_9_common.c
+++ b/src/target/arm7_9_common.c
@@ -184,7 +184,7 @@ static int arm7_9_set_breakpoint(struct target *target, struct breakpoint *break
 		breakpoint->type);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -451,11 +451,12 @@ static int arm7_9_set_watchpoint(struct target *target, struct watchpoint *watch
 	struct arm7_9_common *arm7_9 = target_to_arm7_9(target);
 	int rw_mask = 1;
 	uint32_t mask;
+	const uint32_t wp_data_mask = watchpoint->mask;
 
 	mask = watchpoint->length - 1;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -469,8 +470,8 @@ static int arm7_9_set_watchpoint(struct target *target, struct watchpoint *watch
 			watchpoint->address);
 		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], mask);
 		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK],
-			watchpoint->mask);
-		if (watchpoint->mask != 0xffffffffu)
+			wp_data_mask);
+		if (wp_data_mask != (uint32_t)WATCHPOINT_IGNORE_DATA_VALUE_MASK)
 			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_VALUE],
 				watchpoint->value);
 		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK],
@@ -488,8 +489,8 @@ static int arm7_9_set_watchpoint(struct target *target, struct watchpoint *watch
 			watchpoint->address);
 		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], mask);
 		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK],
-			watchpoint->mask);
-		if (watchpoint->mask != 0xffffffffu)
+			wp_data_mask);
+		if (wp_data_mask != (uint32_t)WATCHPOINT_IGNORE_DATA_VALUE_MASK)
 			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_VALUE],
 				watchpoint->value);
 		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK],
@@ -524,7 +525,7 @@ static int arm7_9_unset_watchpoint(struct target *target, struct watchpoint *wat
 	struct arm7_9_common *arm7_9 = target_to_arm7_9(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1259,7 +1260,7 @@ static int arm7_9_debug_entry(struct target *target)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1390,7 +1391,7 @@ static int arm7_9_full_context(struct target *target)
 	LOG_DEBUG("-");
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1506,7 +1507,7 @@ static int arm7_9_restore_context(struct target *target)
 	LOG_DEBUG("-");
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1709,7 +1710,7 @@ int arm7_9_resume(struct target *target,
 	LOG_DEBUG("-");
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1907,7 +1908,7 @@ int arm7_9_step(struct target *target, int current, target_addr_t address, int h
 	int err, retval;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2118,7 +2119,7 @@ int arm7_9_read_memory(struct target *target,
 		address, size, count);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2291,7 +2292,7 @@ int arm7_9_write_memory(struct target *target,
 #endif
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/arm920t.c b/src/target/arm920t.c
index f4c3f42..53b4d9d 100644
--- a/src/target/arm920t.c
+++ b/src/target/arm920t.c
@@ -533,8 +533,8 @@ int arm920t_arch_state(struct target *target)
 static int arm920_mmu(struct target *target, int *enabled)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("%s: target not halted", __func__);
-		return ERROR_TARGET_INVALID;
+		LOG_TARGET_ERROR(target, "not halted");
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	*enabled = target_to_arm920(target)->armv4_5_mmu.mmu_enabled;
@@ -1455,9 +1455,9 @@ COMMAND_HANDLER(arm920t_handle_cp15_command)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD, "target must be stopped for "
+		command_print(CMD, "Error: target must be stopped for "
 			"\"%s\" command", CMD_NAME);
-		return ERROR_OK;
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	/* one argument, read a register.
diff --git a/src/target/arm926ejs.c b/src/target/arm926ejs.c
index 807d211..add90c9 100644
--- a/src/target/arm926ejs.c
+++ b/src/target/arm926ejs.c
@@ -754,8 +754,8 @@ static int arm926ejs_mmu(struct target *target, int *enabled)
 	struct arm926ejs_common *arm926ejs = target_to_arm926(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
-		return ERROR_TARGET_INVALID;
+		LOG_TARGET_ERROR(target, "not halted");
+		return ERROR_TARGET_NOT_HALTED;
 	}
 	*enabled = arm926ejs->armv4_5_mmu.mmu_enabled;
 	return ERROR_OK;
diff --git a/src/target/arm946e.c b/src/target/arm946e.c
index 06dab4e..03f7e44 100644
--- a/src/target/arm946e.c
+++ b/src/target/arm946e.c
@@ -574,7 +574,7 @@ COMMAND_HANDLER(arm946e_handle_cp15)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
+		command_print(CMD, "Error: target must be stopped for \"%s\" command", CMD_NAME);
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -624,7 +624,7 @@ COMMAND_HANDLER(arm946e_handle_idcache)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
+		command_print(CMD, "Error: target must be stopped for \"%s\" command", CMD_NAME);
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/arm966e.c b/src/target/arm966e.c
index 3e60172..8598d29 100644
--- a/src/target/arm966e.c
+++ b/src/target/arm966e.c
@@ -174,8 +174,8 @@ COMMAND_HANDLER(arm966e_handle_cp15_command)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
-		return ERROR_OK;
+		command_print(CMD, "Error: target must be stopped for \"%s\" command", CMD_NAME);
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	/* one or more argument, access a single register (write if second argument is given */
diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c
index fd6fb26..ab9b50e 100644
--- a/src/target/arm_dpm.c
+++ b/src/target/arm_dpm.c
@@ -918,7 +918,7 @@ static int dpm_watchpoint_setup(struct arm_dpm *dpm, unsigned index_t,
 	uint32_t control;
 
 	/* this hardware doesn't support data value matching or masking */
-	if (wp->value || wp->mask != ~(uint32_t)0) {
+	if (wp->mask != WATCHPOINT_IGNORE_DATA_VALUE_MASK) {
 		LOG_DEBUG("watchpoint values and masking not supported");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
index f35d67a..8e3f224 100644
--- a/src/target/armv4_5.c
+++ b/src/target/armv4_5.c
@@ -578,7 +578,7 @@ static int armv4_5_get_core_reg(struct reg *reg)
 	struct target *target = reg_arch_info->target;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -600,7 +600,7 @@ static int armv4_5_set_core_reg(struct reg *reg, uint8_t *buf)
 	uint32_t value = buf_get_u32(buf, 0, 32);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -817,8 +817,8 @@ COMMAND_HANDLER(handle_armv4_5_reg_command)
 	}
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD, "error: target must be halted for register accesses");
-		return ERROR_FAIL;
+		command_print(CMD, "Error: target must be halted for register accesses");
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	if (arm->core_type != ARM_CORE_TYPE_STD) {
@@ -833,7 +833,7 @@ COMMAND_HANDLER(handle_armv4_5_reg_command)
 	}
 
 	if (!arm->full_context) {
-		command_print(CMD, "error: target doesn't support %s",
+		command_print(CMD, "Error: target doesn't support %s",
 			CMD_NAME);
 		return ERROR_FAIL;
 	}
@@ -1018,8 +1018,10 @@ COMMAND_HANDLER(handle_armv4_5_mcrmrc)
 		return ERROR_FAIL;
 	}
 
-	if (target->state != TARGET_HALTED)
+	if (target->state != TARGET_HALTED) {
+		command_print(CMD, "Error: [%s] not halted", target_name(target));
 		return ERROR_TARGET_NOT_HALTED;
+	}
 
 	int cpnum;
 	uint32_t op1;
@@ -1307,7 +1309,7 @@ int armv4_5_run_algorithm_inner(struct target *target,
 	}
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted (run target algo)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/armv4_5_mmu.c b/src/target/armv4_5_mmu.c
index 9942f49..0c09cb4 100644
--- a/src/target/armv4_5_mmu.c
+++ b/src/target/armv4_5_mmu.c
@@ -107,8 +107,10 @@ int armv4_5_mmu_read_physical(struct target *target,
 {
 	int retval;
 
-	if (target->state != TARGET_HALTED)
+	if (target->state != TARGET_HALTED) {
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
+	}
 
 	/* disable MMU and data (or unified) cache */
 	retval = armv4_5_mmu->disable_mmu_caches(target, 1, 1, 0);
@@ -135,8 +137,10 @@ int armv4_5_mmu_write_physical(struct target *target,
 {
 	int retval;
 
-	if (target->state != TARGET_HALTED)
+	if (target->state != TARGET_HALTED) {
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
+	}
 
 	/* disable MMU and data (or unified) cache */
 	retval = armv4_5_mmu->disable_mmu_caches(target, 1, 1, 0);
diff --git a/src/target/armv7a_cache.c b/src/target/armv7a_cache.c
index 995a856..e1f0dfa 100644
--- a/src/target/armv7a_cache.c
+++ b/src/target/armv7a_cache.c
@@ -22,7 +22,7 @@ static int armv7a_l1_d_cache_sanity_check(struct target *target)
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("%s: target not halted", __func__);
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -40,7 +40,7 @@ static int armv7a_l1_i_cache_sanity_check(struct target *target)
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("%s: target not halted", __func__);
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index 8c9ff90..d508af7 100644
--- a/src/target/armv7m.c
+++ b/src/target/armv7m.c
@@ -525,7 +525,7 @@ int armv7m_start_algorithm(struct target *target,
 	}
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted (start target algo)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/armv8.c b/src/target/armv8.c
index e647c3b..d197477 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -964,7 +964,7 @@ int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 	};
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s not halted", target_name(target));
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index d1eefe5..9ba6b54 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -1210,7 +1210,7 @@ static int dpmv8_watchpoint_setup(struct arm_dpm *dpm, unsigned index_t,
 	uint32_t control;
 
 	/* this hardware doesn't support data value matching or masking */
-	if (wp->value || wp->mask != ~(uint32_t)0) {
+	if (wp->mask != WATCHPOINT_IGNORE_DATA_VALUE_MASK) {
 		LOG_DEBUG("watchpoint values and masking not supported");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
diff --git a/src/target/avr32_ap7k.c b/src/target/avr32_ap7k.c
index bf1445b..bbbf236 100644
--- a/src/target/avr32_ap7k.c
+++ b/src/target/avr32_ap7k.c
@@ -309,7 +309,7 @@ static int avr32_ap7k_resume(struct target *target, int current,
 	int retval;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -431,7 +431,7 @@ static int avr32_ap7k_read_memory(struct target *target, target_addr_t address,
 		count);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -471,7 +471,7 @@ static int avr32_ap7k_write_memory(struct target *target, target_addr_t address,
 		count);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
index c2774cc..0d7850e 100644
--- a/src/target/breakpoints.c
+++ b/src/target/breakpoints.c
@@ -448,7 +448,7 @@ struct breakpoint *breakpoint_find(struct target *target, target_addr_t address)
 }
 
 static int watchpoint_add_internal(struct target *target, target_addr_t address,
-		uint32_t length, enum watchpoint_rw rw, uint32_t value, uint32_t mask)
+		uint32_t length, enum watchpoint_rw rw, uint64_t value, uint64_t mask)
 {
 	struct watchpoint *watchpoint = target->watchpoints;
 	struct watchpoint **watchpoint_p = &target->watchpoints;
@@ -515,7 +515,7 @@ bye:
 }
 
 int watchpoint_add(struct target *target, target_addr_t address,
-		uint32_t length, enum watchpoint_rw rw, uint32_t value, uint32_t mask)
+		uint32_t length, enum watchpoint_rw rw, uint64_t value, uint64_t mask)
 {
 	if (target->smp) {
 		struct target_list *head;
diff --git a/src/target/breakpoints.h b/src/target/breakpoints.h
index 8a3fbb2..afca9b7 100644
--- a/src/target/breakpoints.h
+++ b/src/target/breakpoints.h
@@ -36,11 +36,13 @@ struct breakpoint {
 	int linked_brp;
 };
 
+#define WATCHPOINT_IGNORE_DATA_VALUE_MASK (~(uint64_t)0)
+
 struct watchpoint {
 	target_addr_t address;
 	uint32_t length;
-	uint32_t mask;
-	uint32_t value;
+	uint64_t mask;
+	uint64_t value;
 	enum watchpoint_rw rw;
 	bool is_set;
 	unsigned int number;
@@ -69,7 +71,7 @@ static inline void breakpoint_hw_set(struct breakpoint *breakpoint, unsigned int
 void watchpoint_clear_target(struct target *target);
 int watchpoint_add(struct target *target,
 		target_addr_t address, uint32_t length,
-		enum watchpoint_rw rw, uint32_t value, uint32_t mask);
+		enum watchpoint_rw rw, uint64_t value, uint64_t mask);
 void watchpoint_remove(struct target *target, target_addr_t address);
 
 /* report type and address of just hit watchpoint */
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index d9688be..abfd6ac 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -1147,7 +1147,7 @@ static int cortex_a_step(struct target *target, int current, target_addr_t addre
 	int retval;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2225,7 +2225,7 @@ static int cortex_a_write_cpu_memory(struct target *target,
 	LOG_DEBUG("Writing CPU memory address 0x%" PRIx32 " size %"  PRIu32 " count %"  PRIu32,
 			  address, size, count);
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2542,7 +2542,7 @@ static int cortex_a_read_cpu_memory(struct target *target,
 	LOG_DEBUG("Reading CPU memory address 0x%" PRIx32 " size %"  PRIu32 " count %"  PRIu32,
 			  address, size, count);
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -3183,8 +3183,8 @@ static int cortex_a_mmu(struct target *target, int *enabled)
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("%s: target not halted", __func__);
-		return ERROR_TARGET_INVALID;
+		LOG_TARGET_ERROR(target, "not halted");
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	if (armv7a->is_armv7r)
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index ebc3bac..87a8845 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -50,63 +50,63 @@
 /* Supported Cortex-M Cores */
 static const struct cortex_m_part_info cortex_m_parts[] = {
 	{
-		.partno = CORTEX_M0_PARTNO,
+		.impl_part = CORTEX_M0_PARTNO,
 		.name = "Cortex-M0",
 		.arch = ARM_ARCH_V6M,
 	},
 	{
-		.partno = CORTEX_M0P_PARTNO,
+		.impl_part = CORTEX_M0P_PARTNO,
 		.name = "Cortex-M0+",
 		.arch = ARM_ARCH_V6M,
 	},
 	{
-		.partno = CORTEX_M1_PARTNO,
+		.impl_part = CORTEX_M1_PARTNO,
 		.name = "Cortex-M1",
 		.arch = ARM_ARCH_V6M,
 	},
 	{
-		.partno = CORTEX_M3_PARTNO,
+		.impl_part = CORTEX_M3_PARTNO,
 		.name = "Cortex-M3",
 		.arch = ARM_ARCH_V7M,
 		.flags = CORTEX_M_F_TAR_AUTOINCR_BLOCK_4K,
 	},
 	{
-		.partno = CORTEX_M4_PARTNO,
+		.impl_part = CORTEX_M4_PARTNO,
 		.name = "Cortex-M4",
 		.arch = ARM_ARCH_V7M,
 		.flags = CORTEX_M_F_HAS_FPV4 | CORTEX_M_F_TAR_AUTOINCR_BLOCK_4K,
 	},
 	{
-		.partno = CORTEX_M7_PARTNO,
+		.impl_part = CORTEX_M7_PARTNO,
 		.name = "Cortex-M7",
 		.arch = ARM_ARCH_V7M,
 		.flags = CORTEX_M_F_HAS_FPV5,
 	},
 	{
-		.partno = CORTEX_M23_PARTNO,
+		.impl_part = CORTEX_M23_PARTNO,
 		.name = "Cortex-M23",
 		.arch = ARM_ARCH_V8M,
 	},
 	{
-		.partno = CORTEX_M33_PARTNO,
+		.impl_part = CORTEX_M33_PARTNO,
 		.name = "Cortex-M33",
 		.arch = ARM_ARCH_V8M,
 		.flags = CORTEX_M_F_HAS_FPV5,
 	},
 	{
-		.partno = CORTEX_M35P_PARTNO,
+		.impl_part = CORTEX_M35P_PARTNO,
 		.name = "Cortex-M35P",
 		.arch = ARM_ARCH_V8M,
 		.flags = CORTEX_M_F_HAS_FPV5,
 	},
 	{
-		.partno = CORTEX_M55_PARTNO,
+		.impl_part = CORTEX_M55_PARTNO,
 		.name = "Cortex-M55",
 		.arch = ARM_ARCH_V8M,
 		.flags = CORTEX_M_F_HAS_FPV5,
 	},
 	{
-		.partno = STAR_MC1_PARTNO,
+		.impl_part = STAR_MC1_PARTNO,
 		.name = "STAR-MC1",
 		.arch = ARM_ARCH_V8M,
 		.flags = CORTEX_M_F_HAS_FPV5,
@@ -1214,7 +1214,7 @@ static int cortex_m_restore_one(struct target *target, bool current,
 	struct reg *r;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_TARGET_ERROR(target, "target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1391,7 +1391,7 @@ static int cortex_m_step(struct target *target, int current,
 	bool isr_timed_out = false;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_TARGET_WARNING(target, "target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2046,8 +2046,14 @@ int cortex_m_add_watchpoint(struct target *target, struct watchpoint *watchpoint
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
 
-	/* hardware doesn't support data value masking */
-	if (watchpoint->mask != ~(uint32_t)0) {
+	/* REVISIT This DWT may well be able to watch for specific data
+	 * values.  Requires comparator #1 to set DATAVMATCH and match
+	 * the data, and another comparator (DATAVADDR0) matching addr.
+	 *
+	 * NOTE: hardware doesn't support data value masking, so we'll need
+	 * to check that mask is zero
+	 */
+	if (watchpoint->mask != WATCHPOINT_IGNORE_DATA_VALUE_MASK) {
 		LOG_TARGET_DEBUG(target, "watchpoint value masks not supported");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
@@ -2068,18 +2074,6 @@ int cortex_m_add_watchpoint(struct target *target, struct watchpoint *watchpoint
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
 
-	/* Caller doesn't seem to be able to describe watching for data
-	 * values of zero; that flags "no value".
-	 *
-	 * REVISIT This DWT may well be able to watch for specific data
-	 * values.  Requires comparator #1 to set DATAVMATCH and match
-	 * the data, and another comparator (DATAVADDR0) matching addr.
-	 */
-	if (watchpoint->value) {
-		LOG_TARGET_DEBUG(target, "data value watchpoint not YET supported");
-		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-	}
-
 	cortex_m->dwt_comp_available--;
 	LOG_TARGET_DEBUG(target, "dwt_comp_available: %d", cortex_m->dwt_comp_available);
 
@@ -2092,7 +2086,7 @@ int cortex_m_remove_watchpoint(struct target *target, struct watchpoint *watchpo
 
 	/* REVISIT why check? DWT can be updated with core running ... */
 	if (target->state != TARGET_HALTED) {
-		LOG_TARGET_WARNING(target, "target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2532,18 +2526,18 @@ int cortex_m_examine(struct target *target)
 		if (retval != ERROR_OK)
 			return retval;
 
-		/* Get ARCH and CPU types */
-		const enum cortex_m_partno core_partno = (cpuid & ARM_CPUID_PARTNO_MASK) >> ARM_CPUID_PARTNO_POS;
+		/* Inspect implementor/part to look for recognized cores  */
+		unsigned int impl_part = cpuid & (ARM_CPUID_IMPLEMENTOR_MASK | ARM_CPUID_PARTNO_MASK);
 
 		for (unsigned int n = 0; n < ARRAY_SIZE(cortex_m_parts); n++) {
-			if (core_partno == cortex_m_parts[n].partno) {
+			if (impl_part == cortex_m_parts[n].impl_part) {
 				cortex_m->core_info = &cortex_m_parts[n];
 				break;
 			}
 		}
 
 		if (!cortex_m->core_info) {
-			LOG_TARGET_ERROR(target, "Cortex-M PARTNO 0x%x is unrecognized", core_partno);
+			LOG_TARGET_ERROR(target, "Cortex-M CPUID: 0x%x is unrecognized", cpuid);
 			return ERROR_FAIL;
 		}
 
@@ -2555,7 +2549,7 @@ int cortex_m_examine(struct target *target)
 				(uint8_t)((cpuid >> 0) & 0xf));
 
 		cortex_m->maskints_erratum = false;
-		if (core_partno == CORTEX_M7_PARTNO) {
+		if (impl_part == CORTEX_M7_PARTNO) {
 			uint8_t rev, patch;
 			rev = (cpuid >> 20) & 0xf;
 			patch = (cpuid >> 0) & 0xf;
@@ -2948,8 +2942,8 @@ COMMAND_HANDLER(handle_cortex_m_mask_interrupts_command)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
-		return ERROR_OK;
+		command_print(CMD, "Error: target must be stopped for \"%s\" command", CMD_NAME);
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	if (CMD_ARGC > 0) {
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index a1c43b5..806ff59 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -31,22 +31,31 @@
 
 #define CPUID		0xE000ED00
 
-#define ARM_CPUID_PARTNO_POS    4
-#define ARM_CPUID_PARTNO_MASK	(0xFFF << ARM_CPUID_PARTNO_POS)
+#define ARM_CPUID_IMPLEMENTOR_POS	24
+#define ARM_CPUID_IMPLEMENTOR_MASK	(0xFF << ARM_CPUID_IMPLEMENTOR_POS)
+#define ARM_CPUID_PARTNO_POS		4
+#define ARM_CPUID_PARTNO_MASK		(0xFFF << ARM_CPUID_PARTNO_POS)
 
-enum cortex_m_partno {
+#define ARM_MAKE_CPUID(impl, partno)	((((impl) << ARM_CPUID_IMPLEMENTOR_POS) & ARM_CPUID_IMPLEMENTOR_MASK) | \
+	(((partno) << ARM_CPUID_PARTNO_POS)  & ARM_CPUID_PARTNO_MASK))
+
+/** Known Arm Cortex masked CPU Ids
+ * This includes the implementor and part number, but _not_ the revision or
+ * patch fields.
+ */
+enum cortex_m_impl_part {
 	CORTEX_M_PARTNO_INVALID,
-	STAR_MC1_PARTNO    = 0x132,
-	CORTEX_M0_PARTNO   = 0xC20,
-	CORTEX_M1_PARTNO   = 0xC21,
-	CORTEX_M3_PARTNO   = 0xC23,
-	CORTEX_M4_PARTNO   = 0xC24,
-	CORTEX_M7_PARTNO   = 0xC27,
-	CORTEX_M0P_PARTNO  = 0xC60,
-	CORTEX_M23_PARTNO  = 0xD20,
-	CORTEX_M33_PARTNO  = 0xD21,
-	CORTEX_M35P_PARTNO = 0xD31,
-	CORTEX_M55_PARTNO  = 0xD22,
+	STAR_MC1_PARTNO    = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0x132), /* FIXME - confirm implementor! */
+	CORTEX_M0_PARTNO   = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xC20),
+	CORTEX_M1_PARTNO   = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xC21),
+	CORTEX_M3_PARTNO   = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xC23),
+	CORTEX_M4_PARTNO   = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xC24),
+	CORTEX_M7_PARTNO   = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xC27),
+	CORTEX_M0P_PARTNO  = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xC60),
+	CORTEX_M23_PARTNO  = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xD20),
+	CORTEX_M33_PARTNO  = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xD21),
+	CORTEX_M35P_PARTNO = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xD31),
+	CORTEX_M55_PARTNO  = ARM_MAKE_CPUID(ARM_IMPLEMENTOR_ARM, 0xD22),
 };
 
 /* Relevant Cortex-M flags, used in struct cortex_m_part_info.flags */
@@ -55,7 +64,7 @@ enum cortex_m_partno {
 #define CORTEX_M_F_TAR_AUTOINCR_BLOCK_4K  BIT(2)
 
 struct cortex_m_part_info {
-	enum cortex_m_partno partno;
+	enum cortex_m_impl_part impl_part;
 	const char *name;
 	enum arm_arch arch;
 	uint32_t flags;
@@ -292,11 +301,11 @@ target_to_cortex_m_safe(struct target *target)
 }
 
 /**
- * @returns cached value of Cortex-M part number
+ * @returns cached value of the cpuid, masked for implementation and part.
  * or CORTEX_M_PARTNO_INVALID if the magic number does not match
  * or core_info is not initialised.
  */
-static inline enum cortex_m_partno cortex_m_get_partno_safe(struct target *target)
+static inline enum cortex_m_impl_part cortex_m_get_impl_part(struct target *target)
 {
 	struct cortex_m_common *cortex_m = target_to_cortex_m_safe(target);
 	if (!cortex_m)
@@ -305,7 +314,7 @@ static inline enum cortex_m_partno cortex_m_get_partno_safe(struct target *targe
 	if (!cortex_m->core_info)
 		return CORTEX_M_PARTNO_INVALID;
 
-	return cortex_m->core_info->partno;
+	return cortex_m->core_info->impl_part;
 }
 
 int cortex_m_examine(struct target *target);
diff --git a/src/target/dsp563xx.c b/src/target/dsp563xx.c
index af60345..16fd149 100644
--- a/src/target/dsp563xx.c
+++ b/src/target/dsp563xx.c
@@ -1296,7 +1296,7 @@ static int dsp563xx_step(struct target *target,
 	struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1381,7 +1381,7 @@ static int dsp563xx_run_algorithm(struct target *target,
 	struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted (run target algo)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1705,7 +1705,7 @@ static int dsp563xx_write_memory_core(struct target *target,
 		count);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/esirisc.c b/src/target/esirisc.c
index f86d28d..561edb2 100644
--- a/src/target/esirisc.c
+++ b/src/target/esirisc.c
@@ -318,8 +318,10 @@ static int esirisc_flush_caches(struct target *target)
 
 	LOG_DEBUG("-");
 
-	if (target->state != TARGET_HALTED)
+	if (target->state != TARGET_HALTED) {
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
+	}
 
 	int retval = esirisc_jtag_flush_caches(jtag_info);
 	if (retval != ERROR_OK) {
@@ -855,8 +857,10 @@ static int esirisc_resume_or_step(struct target *target, int current, target_add
 
 	LOG_DEBUG("-");
 
-	if (target->state != TARGET_HALTED)
+	if (target->state != TARGET_HALTED) {
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
+	}
 
 	if (!debug_execution) {
 		target_free_all_working_areas(target);
diff --git a/src/target/espressif/Makefile.am b/src/target/espressif/Makefile.am
index 14625d4..776818f 100644
--- a/src/target/espressif/Makefile.am
+++ b/src/target/espressif/Makefile.am
@@ -2,21 +2,23 @@
 
 noinst_LTLIBRARIES += %D%/libespressif.la
 %C%_libespressif_la_SOURCES = \
-       %D%/esp_xtensa.c \
-       %D%/esp_xtensa.h \
-       %D%/esp_xtensa_smp.c \
-       %D%/esp_xtensa_smp.h \
-       %D%/esp_xtensa_semihosting.c \
-       %D%/esp_xtensa_semihosting.h \
-       %D%/esp_xtensa_apptrace.c \
-       %D%/esp_xtensa_apptrace.h \
-       %D%/esp32_apptrace.c \
-       %D%/esp32_apptrace.h \
-       %D%/esp32.c \
-       %D%/esp32s2.c \
-       %D%/esp32s3.c \
-       %D%/esp32_sysview.c \
-       %D%/esp32_sysview.h \
-       %D%/segger_sysview.h \
-       %D%/esp_semihosting.c \
-       %D%/esp_semihosting.h
+	%D%/esp_xtensa.c \
+	%D%/esp_xtensa.h \
+	%D%/esp_xtensa_smp.c \
+	%D%/esp_xtensa_smp.h \
+	%D%/esp_xtensa_semihosting.c \
+	%D%/esp_xtensa_semihosting.h \
+	%D%/esp_xtensa_apptrace.c \
+	%D%/esp_xtensa_apptrace.h \
+	%D%/esp32_apptrace.c \
+	%D%/esp32_apptrace.h \
+	%D%/esp32.c \
+	%D%/esp32s2.c \
+	%D%/esp32s3.c \
+	%D%/esp.c \
+	%D%/esp.h \
+	%D%/esp32_sysview.c \
+	%D%/esp32_sysview.h \
+	%D%/segger_sysview.h \
+	%D%/esp_semihosting.c \
+	%D%/esp_semihosting.h
diff --git a/src/target/espressif/esp.c b/src/target/espressif/esp.c
new file mode 100644
index 0000000..9583d64
--- /dev/null
+++ b/src/target/espressif/esp.c
@@ -0,0 +1,77 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+/***************************************************************************
+ *   Espressif chips common target API for OpenOCD                         *
+ *   Copyright (C) 2021 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/log.h>
+#include <helper/binarybuffer.h>
+#include "target/target.h"
+#include "esp.h"
+
+int esp_dbgstubs_table_read(struct target *target, struct esp_dbg_stubs *dbg_stubs)
+{
+	uint32_t table_size, table_start_id, desc_entry_id, gcov_entry_id;
+	uint32_t entries[ESP_DBG_STUB_ENTRY_MAX] = {0};
+	uint8_t entry_buff[sizeof(entries)] = {0}; /* to avoid endiannes issues */
+
+	LOG_TARGET_DEBUG(target, "Read debug stubs info %" PRIx32 " / %d", dbg_stubs->base, dbg_stubs->entries_count);
+
+	/* First of, read 2 entries to get magic num and table size */
+	int res = target_read_buffer(target, dbg_stubs->base, sizeof(uint32_t) * 2, entry_buff);
+	if (res != ERROR_OK) {
+		LOG_ERROR("%s: Failed to read first debug stub entry!", target_name(target));
+		return res;
+	}
+	entries[0] = target_buffer_get_u32(target, entry_buff);
+	entries[1] = target_buffer_get_u32(target, entry_buff + sizeof(uint32_t));
+
+	if (entries[0] != ESP_DBG_STUB_MAGIC_NUM_VAL) {
+		/* idf with the old table entry structure */
+		table_size = 2;
+		table_start_id = 0;
+		desc_entry_id = 0;
+		gcov_entry_id = 1;
+	} else {
+		table_size = entries[1];
+		table_start_id = ESP_DBG_STUB_TABLE_START;
+		desc_entry_id = ESP_DBG_STUB_TABLE_START;
+		gcov_entry_id = ESP_DBG_STUB_ENTRY_FIRST;
+
+		/* discard unsupported entries */
+		if (table_size > ESP_DBG_STUB_ENTRY_MAX)
+			table_size = ESP_DBG_STUB_ENTRY_MAX;
+
+		/* now read the remaining entries */
+		res = target_read_buffer(target, dbg_stubs->base + 2 * sizeof(uint32_t), sizeof(uint32_t) * table_size - 2,
+			entry_buff + sizeof(uint32_t) * 2);
+		if (res != ERROR_OK) {
+			LOG_TARGET_ERROR(target, "Failed to read debug stubs info!");
+			return res;
+		}
+		for (unsigned int i = 2; i < table_size; ++i)
+			entries[i] = target_buffer_get_u32(target, entry_buff + sizeof(uint32_t) * i);
+
+		dbg_stubs->entries[ESP_DBG_STUB_CAPABILITIES] = entries[ESP_DBG_STUB_CAPABILITIES];
+	}
+
+	dbg_stubs->entries[ESP_DBG_STUB_DESC] = entries[desc_entry_id];
+	dbg_stubs->entries[ESP_DBG_STUB_ENTRY_GCOV] = entries[gcov_entry_id];
+
+	for (enum esp_dbg_stub_id i = ESP_DBG_STUB_DESC; i < ESP_DBG_STUB_ENTRY_MAX; i++) {
+		LOG_DEBUG("Check dbg stub %d - %x", i, dbg_stubs->entries[i]);
+		if (dbg_stubs->entries[i]) {
+			LOG_DEBUG("New dbg stub %d at %x", dbg_stubs->entries_count, dbg_stubs->entries[i]);
+			dbg_stubs->entries_count++;
+		}
+	}
+	if (dbg_stubs->entries_count < table_size - table_start_id)
+		LOG_WARNING("Not full dbg stub table %d of %d", dbg_stubs->entries_count, table_size - table_start_id);
+
+	return ERROR_OK;
+}
diff --git a/src/target/espressif/esp.h b/src/target/espressif/esp.h
new file mode 100644
index 0000000..3ba2b8b
--- /dev/null
+++ b/src/target/espressif/esp.h
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+/***************************************************************************
+ *   Espressif chips common target API for OpenOCD                         *
+ *   Copyright (C) 2021 Espressif Systems Ltd.                             *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ESP_H
+#define OPENOCD_TARGET_ESP_H
+
+#include <stdint.h>
+#include <helper/bits.h>
+
+/* must be in sync with ESP-IDF version */
+/** Size of the pre-compiled target buffer for stub trampoline.
+ * @note Must be in sync with ESP-IDF version */
+#define ESP_DBG_STUBS_CODE_BUF_SIZE         32	/* TODO: move this info to esp_dbg_stubs_desc */
+/** Size of the pre-compiled target buffer for stack.
+ * @note Must be in sync with ESP-IDF version */
+#define ESP_DBG_STUBS_STACK_MIN_SIZE        2048/* TODO: move this info to esp_dbg_stubs_desc */
+
+/**
+ * Debug stubs table entries IDs
+ *
+ * @note Must be in sync with ESP-IDF version
+ */
+enum esp_dbg_stub_id {
+	ESP_DBG_STUB_ENTRY_MAGIC_NUM,
+	ESP_DBG_STUB_TABLE_SIZE,
+	ESP_DBG_STUB_TABLE_START,
+	ESP_DBG_STUB_DESC = ESP_DBG_STUB_TABLE_START,	/*< Stubs descriptor ID */
+	ESP_DBG_STUB_ENTRY_FIRST,
+	ESP_DBG_STUB_ENTRY_GCOV = ESP_DBG_STUB_ENTRY_FIRST,	/*< GCOV stub ID */
+	ESP_DBG_STUB_CAPABILITIES,
+	/* add new stub entries here */
+	ESP_DBG_STUB_ENTRY_MAX,
+};
+
+#define ESP_DBG_STUB_MAGIC_NUM_VAL      0xFEEDBEEF
+#define ESP_DBG_STUB_CAP_GCOV_THREAD    BIT(0)
+
+/**
+ * Debug stubs descriptor. ID: ESP_DBG_STUB_DESC
+ *
+ * @note Must be in sync with ESP-IDF version
+ */
+struct esp_dbg_stubs_desc {
+	/** Address of pre-compiled target buffer for stub trampoline.
+	 * Size of the buffer is ESP_DBG_STUBS_CODE_BUF_SIZE
+	 */
+	uint32_t tramp_addr;
+	/** Pre-compiled target buffer's addr for stack. The size of the buffer is ESP_DBG_STUBS_STACK_MIN_SIZE.
+	 * Target has the buffer which is used for the stack of onboard algorithms.
+	 * If stack size required by algorithm exceeds ESP_DBG_STUBS_STACK_MIN_SIZE,
+	 * it should be allocated using onboard function pointed by 'data_alloc' and
+	 * freed by 'data_free'. They fit to the minimal stack. See below.
+	 */
+	uint32_t min_stack_addr;
+	/** Address of malloc-like function to allocate buffer on target. */
+	uint32_t data_alloc;
+	/** Address of free-like function to free buffer allocated with data_alloc. */
+	uint32_t data_free;
+};
+
+/**
+ * Debug stubs info.
+ */
+struct esp_dbg_stubs {
+	/** Address. */
+	uint32_t base;
+	/** Table contents. */
+	uint32_t entries[ESP_DBG_STUB_ENTRY_MAX];
+	/** Number of table entries. */
+	uint32_t entries_count;
+	/** Debug stubs decsriptor. */
+	struct esp_dbg_stubs_desc desc;
+};
+
+struct esp_common {
+	struct esp_dbg_stubs dbg_stubs;
+};
+
+int esp_dbgstubs_table_read(struct target *target, struct esp_dbg_stubs *dbg_stubs);
+
+#endif	/* OPENOCD_TARGET_ESP_H */
diff --git a/src/target/espressif/esp32.c b/src/target/espressif/esp32.c
index 74bbe50..b510f28 100644
--- a/src/target/espressif/esp32.c
+++ b/src/target/espressif/esp32.c
@@ -24,20 +24,8 @@ implementation.
 */
 
 /* ESP32 memory map */
-#define ESP32_DRAM_LOW            0x3ffae000
-#define ESP32_DRAM_HIGH           0x40000000
-#define ESP32_IROM_MASK_LOW       0x40000000
-#define ESP32_IROM_MASK_HIGH      0x40064f00
-#define ESP32_IRAM_LOW            0x40070000
-#define ESP32_IRAM_HIGH           0x400a0000
-#define ESP32_RTC_IRAM_LOW        0x400c0000
-#define ESP32_RTC_IRAM_HIGH       0x400c2000
-#define ESP32_RTC_DRAM_LOW        0x3ff80000
-#define ESP32_RTC_DRAM_HIGH       0x3ff82000
 #define ESP32_RTC_DATA_LOW        0x50000000
 #define ESP32_RTC_DATA_HIGH       0x50002000
-#define ESP32_EXTRAM_DATA_LOW     0x3f800000
-#define ESP32_EXTRAM_DATA_HIGH    0x3fc00000
 #define ESP32_DR_REG_LOW          0x3ff00000
 #define ESP32_DR_REG_HIGH         0x3ff71000
 #define ESP32_SYS_RAM_LOW         0x60000000UL
@@ -266,7 +254,10 @@ static int esp32_disable_wdts(struct target *target)
 
 static int esp32_on_halt(struct target *target)
 {
-	return esp32_disable_wdts(target);
+	int ret = esp32_disable_wdts(target);
+	if (ret == ERROR_OK)
+		ret = esp_xtensa_smp_on_halt(target);
+	return ret;
 }
 
 static int esp32_arch_state(struct target *target)
diff --git a/src/target/espressif/esp32_apptrace.c b/src/target/espressif/esp32_apptrace.c
index 8842241..125f366 100644
--- a/src/target/espressif/esp32_apptrace.c
+++ b/src/target/espressif/esp32_apptrace.c
@@ -1252,7 +1252,7 @@ static int esp32_sysview_start(struct esp32_apptrace_cmd_ctx *ctx)
 {
 	uint8_t cmds[] = { SEGGER_SYSVIEW_COMMAND_ID_START };
 	uint32_t fired_target_num = 0;
-	struct esp32_apptrace_target_state target_state[ESP32_APPTRACE_MAX_CORES_NUM] = {0};
+	struct esp32_apptrace_target_state target_state[ESP32_APPTRACE_MAX_CORES_NUM] = {{0}};
 	struct esp32_sysview_cmd_data *cmd_data = ctx->cmd_priv;
 
 	/* get current block id */
diff --git a/src/target/espressif/esp32s2.c b/src/target/espressif/esp32s2.c
index a11d05f..dadc130 100644
--- a/src/target/espressif/esp32s2.c
+++ b/src/target/espressif/esp32s2.c
@@ -17,33 +17,12 @@
 #include "esp_xtensa.h"
 #include "esp_xtensa_semihosting.h"
 
-/* Overall memory map
- * TODO: read memory configuration from target registers */
-#define ESP32_S2_IROM_MASK_LOW          0x40000000
-#define ESP32_S2_IROM_MASK_HIGH         0x40020000
-#define ESP32_S2_IRAM_LOW               0x40020000
-#define ESP32_S2_IRAM_HIGH              0x40070000
-#define ESP32_S2_DRAM_LOW               0x3ffb0000
-#define ESP32_S2_DRAM_HIGH              0x40000000
-#define ESP32_S2_RTC_IRAM_LOW           0x40070000
-#define ESP32_S2_RTC_IRAM_HIGH          0x40072000
-#define ESP32_S2_RTC_DRAM_LOW           0x3ff9e000
-#define ESP32_S2_RTC_DRAM_HIGH          0x3ffa0000
 #define ESP32_S2_RTC_DATA_LOW           0x50000000
 #define ESP32_S2_RTC_DATA_HIGH          0x50002000
-#define ESP32_S2_EXTRAM_DATA_LOW        0x3f500000
-#define ESP32_S2_EXTRAM_DATA_HIGH       0x3ff80000
 #define ESP32_S2_DR_REG_LOW             0x3f400000
 #define ESP32_S2_DR_REG_HIGH            0x3f4d3FFC
 #define ESP32_S2_SYS_RAM_LOW            0x60000000UL
 #define ESP32_S2_SYS_RAM_HIGH           (ESP32_S2_SYS_RAM_LOW + 0x20000000UL)
-/* ESP32-S2 DROM mapping is not contiguous. */
-/* IDF declares this as 0x3F000000..0x3FF80000, but there are peripheral registers mapped to
- * 0x3f400000..0x3f4d3FFC. */
-#define ESP32_S2_DROM0_LOW              ESP32_S2_DROM_LOW
-#define ESP32_S2_DROM0_HIGH             ESP32_S2_DR_REG_LOW
-#define ESP32_S2_DROM1_LOW              ESP32_S2_DR_REG_HIGH
-#define ESP32_S2_DROM1_HIGH             ESP32_S2_DROM_HIGH
 
 /* ESP32 WDT */
 #define ESP32_S2_WDT_WKEY_VALUE         0x50d83aa1
@@ -385,7 +364,10 @@ static int esp32s2_arch_state(struct target *target)
 
 static int esp32s2_on_halt(struct target *target)
 {
-	return esp32s2_disable_wdts(target);
+	int ret = esp32s2_disable_wdts(target);
+	if (ret == ERROR_OK)
+		ret = esp_xtensa_on_halt(target);
+	return ret;
 }
 
 static int esp32s2_step(struct target *target, int current, target_addr_t address, int handle_breakpoints)
diff --git a/src/target/espressif/esp32s3.c b/src/target/espressif/esp32s3.c
index 4855678..5036956 100644
--- a/src/target/espressif/esp32s3.c
+++ b/src/target/espressif/esp32s3.c
@@ -24,16 +24,6 @@ implementation.
 */
 
 /* ESP32_S3 memory map */
-#define ESP32_S3_IRAM_LOW               0x40370000
-#define ESP32_S3_IRAM_HIGH              0x403E0000
-#define ESP32_S3_IROM_MASK_LOW          0x40000000
-#define ESP32_S3_IROM_MASK_HIGH         0x40060000
-#define ESP32_S3_DRAM_LOW               0x3FC88000
-#define ESP32_S3_DRAM_HIGH              0x3FD00000
-#define ESP32_S3_RTC_IRAM_LOW           0x600FE000
-#define ESP32_S3_RTC_IRAM_HIGH          0x60100000
-#define ESP32_S3_RTC_DRAM_LOW           0x600FE000
-#define ESP32_S3_RTC_DRAM_HIGH          0x60100000
 #define ESP32_S3_RTC_DATA_LOW           0x50000000
 #define ESP32_S3_RTC_DATA_HIGH          0x50002000
 #define ESP32_S3_EXTRAM_DATA_LOW        0x3D000000
@@ -283,7 +273,10 @@ static int esp32s3_disable_wdts(struct target *target)
 
 static int esp32s3_on_halt(struct target *target)
 {
-	return esp32s3_disable_wdts(target);
+	int ret = esp32s3_disable_wdts(target);
+	if (ret == ERROR_OK)
+		ret = esp_xtensa_smp_on_halt(target);
+	return ret;
 }
 
 static int esp32s3_arch_state(struct target *target)
diff --git a/src/target/espressif/esp_xtensa.c b/src/target/espressif/esp_xtensa.c
index 44764ae..0bd2cdd 100644
--- a/src/target/espressif/esp_xtensa.c
+++ b/src/target/espressif/esp_xtensa.c
@@ -17,6 +17,49 @@
 #include "esp_xtensa.h"
 #include "esp_semihosting.h"
 
+#define ESP_XTENSA_DBGSTUBS_UPDATE_DATA_ENTRY(_e_) \
+	do { \
+		uint32_t __internal_val = (_e_); \
+		if (!xtensa_data_addr_valid(target, __internal_val)) { \
+			LOG_ERROR("No valid stub data entry found (0x%" PRIx32 ")!", __internal_val); \
+			return;	\
+		} \
+	} while (0)
+
+#define ESP_XTENSA_DBGSTUBS_UPDATE_CODE_ENTRY(_e_) \
+	do { \
+		uint32_t __internal_val = (_e_); \
+		if (__internal_val == 0) { \
+			LOG_ERROR("No valid stub code entry found (0x%" PRIx32 ")!", __internal_val); \
+			return;	\
+		} \
+	} while (0)
+
+static void esp_xtensa_dbgstubs_info_update(struct target *target);
+static void esp_xtensa_dbgstubs_addr_check(struct target *target);
+
+static int esp_xtensa_dbgstubs_restore(struct target *target)
+{
+	struct esp_xtensa_common *esp_xtensa = target_to_esp_xtensa(target);
+
+	if (esp_xtensa->esp.dbg_stubs.base == 0)
+		return ERROR_OK;
+
+	LOG_TARGET_INFO(target, "Restore debug stubs address %" PRIx32, esp_xtensa->esp.dbg_stubs.base);
+	int res = esp_xtensa_apptrace_status_reg_write(target, esp_xtensa->esp.dbg_stubs.base);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to write trace status (%d)!", res);
+		return res;
+	}
+	return ERROR_OK;
+}
+int esp_xtensa_on_halt(struct target *target)
+{
+	/* debug stubs can be used in HALTED state only, so it is OK to get info about them here */
+	esp_xtensa_dbgstubs_info_update(target);
+	return ERROR_OK;
+}
+
 int esp_xtensa_init_arch_info(struct target *target,
 	struct esp_xtensa_common *esp_xtensa,
 	struct xtensa_debug_module_config *dm_cfg,
@@ -39,6 +82,11 @@ void esp_xtensa_target_deinit(struct target *target)
 {
 	LOG_DEBUG("start");
 
+	if (target_was_examined(target)) {
+		int ret = esp_xtensa_dbgstubs_restore(target);
+		if (ret != ERROR_OK)
+			return;
+	}
 	xtensa_target_deinit(target);
 	free(target_to_esp_xtensa(target));	/* same as free(xtensa) */
 }
@@ -50,7 +98,68 @@ int esp_xtensa_arch_state(struct target *target)
 
 int esp_xtensa_poll(struct target *target)
 {
-	return xtensa_poll(target);
+	struct xtensa *xtensa = target_to_xtensa(target);
+	struct esp_xtensa_common *esp_xtensa_common = target_to_esp_xtensa(target);
+
+	int ret = xtensa_poll(target);
+
+	if (xtensa_dm_power_status_get(&xtensa->dbg_mod) & PWRSTAT_COREWASRESET(xtensa)) {
+		LOG_TARGET_DEBUG(target, "Clear debug stubs info");
+		memset(&esp_xtensa_common->esp.dbg_stubs, 0, sizeof(esp_xtensa_common->esp.dbg_stubs));
+	}
+	if (target->state != TARGET_DEBUG_RUNNING)
+		esp_xtensa_dbgstubs_addr_check(target);
+	return ret;
+}
+
+static void esp_xtensa_dbgstubs_addr_check(struct target *target)
+{
+	struct esp_xtensa_common *esp_xtensa = target_to_esp_xtensa(target);
+	uint32_t vec_addr = 0;
+
+	if (esp_xtensa->esp.dbg_stubs.base != 0)
+		return;
+
+	int res = esp_xtensa_apptrace_status_reg_read(target, &vec_addr);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to read debug stubs address location (%d)!", res);
+		return;
+	}
+	if (xtensa_data_addr_valid(target, vec_addr)) {
+		LOG_TARGET_INFO(target, "Detected debug stubs @ %" PRIx32, vec_addr);
+		res = esp_xtensa_apptrace_status_reg_write(target, 0);
+		if (res != ERROR_OK)
+			LOG_ERROR("Failed to clear debug stubs address location (%d)!", res);
+		esp_xtensa->esp.dbg_stubs.base = vec_addr;
+	}
+}
+
+static void esp_xtensa_dbgstubs_info_update(struct target *target)
+{
+	struct esp_xtensa_common *esp_xtensa = target_to_esp_xtensa(target);
+
+	if (esp_xtensa->esp.dbg_stubs.base == 0 || esp_xtensa->esp.dbg_stubs.entries_count != 0)
+		return;
+
+	int res = esp_dbgstubs_table_read(target, &esp_xtensa->esp.dbg_stubs);
+	if (res != ERROR_OK)
+		return;
+	if (esp_xtensa->esp.dbg_stubs.entries_count == 0)
+		return;
+
+	/* read debug stubs descriptor */
+	ESP_XTENSA_DBGSTUBS_UPDATE_DATA_ENTRY(esp_xtensa->esp.dbg_stubs.entries[ESP_DBG_STUB_DESC]);
+	res = target_read_buffer(target, esp_xtensa->esp.dbg_stubs.entries[ESP_DBG_STUB_DESC],
+		sizeof(struct esp_dbg_stubs_desc),
+		(uint8_t *)&esp_xtensa->esp.dbg_stubs.desc);
+	if (res != ERROR_OK) {
+		LOG_ERROR("Failed to read debug stubs descriptor (%d)!", res);
+		return;
+	}
+	ESP_XTENSA_DBGSTUBS_UPDATE_CODE_ENTRY(esp_xtensa->esp.dbg_stubs.desc.tramp_addr);
+	ESP_XTENSA_DBGSTUBS_UPDATE_DATA_ENTRY(esp_xtensa->esp.dbg_stubs.desc.min_stack_addr);
+	ESP_XTENSA_DBGSTUBS_UPDATE_CODE_ENTRY(esp_xtensa->esp.dbg_stubs.desc.data_alloc);
+	ESP_XTENSA_DBGSTUBS_UPDATE_CODE_ENTRY(esp_xtensa->esp.dbg_stubs.desc.data_free);
 }
 
 int esp_xtensa_breakpoint_add(struct target *target, struct breakpoint *breakpoint)
diff --git a/src/target/espressif/esp_xtensa.h b/src/target/espressif/esp_xtensa.h
index 8807f0c..00f67a3 100644
--- a/src/target/espressif/esp_xtensa.h
+++ b/src/target/espressif/esp_xtensa.h
@@ -10,12 +10,13 @@
 
 #include <target/target.h>
 #include <target/xtensa/xtensa.h>
-#include "esp_xtensa.h"
 #include "esp_semihosting.h"
+#include "esp.h"
 #include "esp_xtensa_apptrace.h"
 
 struct esp_xtensa_common {
 	struct xtensa xtensa;	/* must be the first element */
+	struct esp_common esp;
 	struct esp_semihost_data semihost;
 	struct esp_xtensa_apptrace_info apptrace;
 };
@@ -36,5 +37,6 @@ void esp_xtensa_queue_tdi_idle(struct target *target);
 int esp_xtensa_breakpoint_add(struct target *target, struct breakpoint *breakpoint);
 int esp_xtensa_breakpoint_remove(struct target *target, struct breakpoint *breakpoint);
 int esp_xtensa_poll(struct target *target);
+int esp_xtensa_on_halt(struct target *target);
 
 #endif	/* OPENOCD_TARGET_ESP_XTENSA_H */
diff --git a/src/target/espressif/esp_xtensa_smp.c b/src/target/espressif/esp_xtensa_smp.c
index 1d03774..1d70be9 100644
--- a/src/target/espressif/esp_xtensa_smp.c
+++ b/src/target/espressif/esp_xtensa_smp.c
@@ -112,6 +112,21 @@ int esp_xtensa_smp_soft_reset_halt(struct target *target)
 	return ERROR_OK;
 }
 
+int esp_xtensa_smp_on_halt(struct target *target)
+{
+	struct target_list *head;
+
+	if (!target->smp)
+		return esp_xtensa_on_halt(target);
+
+	foreach_smp_target(head, target->smp_targets) {
+		int res = esp_xtensa_on_halt(head->target);
+		if (res != ERROR_OK)
+			return res;
+	}
+	return ERROR_OK;
+}
+
 static struct target *get_halted_esp_xtensa_smp(struct target *target, int32_t coreid)
 {
 	struct target_list *head;
@@ -131,6 +146,7 @@ int esp_xtensa_smp_poll(struct target *target)
 	enum target_state old_state = target->state;
 	struct esp_xtensa_smp_common *esp_xtensa_smp = target_to_esp_xtensa_smp(target);
 	struct esp_xtensa_common *esp_xtensa = target_to_esp_xtensa(target);
+	uint32_t old_dbg_stubs_base = esp_xtensa->esp.dbg_stubs.base;
 	struct target_list *head;
 	struct target *curr;
 	bool other_core_resume_req = false;
@@ -148,6 +164,16 @@ int esp_xtensa_smp_poll(struct target *target)
 	if (ret != ERROR_OK)
 		return ret;
 
+	if (esp_xtensa->esp.dbg_stubs.base && old_dbg_stubs_base != esp_xtensa->esp.dbg_stubs.base) {
+		/* debug stubs base is set only in PRO-CPU TRAX register, so sync this info */
+		foreach_smp_target(head, target->smp_targets) {
+			curr = head->target;
+			if (curr == target)
+				continue;
+			target_to_esp_xtensa(curr)->esp.dbg_stubs.base = esp_xtensa->esp.dbg_stubs.base;
+		}
+	}
+
 	if (target->smp) {
 		if (target->state == TARGET_RESET) {
 			esp_xtensa_smp->examine_other_cores = ESP_XTENSA_SMP_EXAMINE_OTHER_CORES;
diff --git a/src/target/espressif/esp_xtensa_smp.h b/src/target/espressif/esp_xtensa_smp.h
index aeb1d61..4e4f3b3 100644
--- a/src/target/espressif/esp_xtensa_smp.h
+++ b/src/target/espressif/esp_xtensa_smp.h
@@ -37,6 +37,7 @@ int esp_xtensa_smp_step(struct target *target,
 int esp_xtensa_smp_assert_reset(struct target *target);
 int esp_xtensa_smp_deassert_reset(struct target *target);
 int esp_xtensa_smp_soft_reset_halt(struct target *target);
+int esp_xtensa_smp_on_halt(struct target *target);
 int esp_xtensa_smp_watchpoint_add(struct target *target, struct watchpoint *watchpoint);
 int esp_xtensa_smp_watchpoint_remove(struct target *target, struct watchpoint *watchpoint);
 int esp_xtensa_smp_handle_target_event(struct target *target, enum target_event event, void *priv);
diff --git a/src/target/hla_target.c b/src/target/hla_target.c
index 8c35a90..c1bda99 100644
--- a/src/target/hla_target.c
+++ b/src/target/hla_target.c
@@ -449,7 +449,7 @@ static int adapter_resume(struct target *target, int current,
 			address, handle_breakpoints, debug_execution);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -538,7 +538,7 @@ static int adapter_step(struct target *target, int current,
 	LOG_DEBUG("%s", __func__);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/mips32.c b/src/target/mips32.c
index 1a34f73..ce16a7b 100644
--- a/src/target/mips32.c
+++ b/src/target/mips32.c
@@ -438,7 +438,7 @@ int mips32_run_algorithm(struct target *target, int num_mem_params,
 	}
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted (run target algo)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -921,8 +921,8 @@ COMMAND_HANDLER(mips32_handle_cp0_command)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
-		return ERROR_OK;
+		command_print(CMD, "Error: target must be stopped for \"%s\" command", CMD_NAME);
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	/* two or more argument, access a single register/select (write if third argument is given) */
diff --git a/src/target/mips_mips64.c b/src/target/mips_mips64.c
index 640b4c8..bf60953 100644
--- a/src/target/mips_mips64.c
+++ b/src/target/mips_mips64.c
@@ -606,7 +606,7 @@ static int mips_mips64_resume(struct target *target, int current,
 		address = mips64_extend_sign(address);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted %d", target->state);
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -706,7 +706,7 @@ static int mips_mips64_step(struct target *target, int current,
 	int retval = ERROR_OK;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -804,7 +804,7 @@ static int mips_mips64_remove_breakpoint(struct target *target,
 	int retval = ERROR_OK;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -865,7 +865,7 @@ static int mips_mips64_remove_watchpoint(struct target *target,
 	int retval = ERROR_OK;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -886,7 +886,7 @@ static int mips_mips64_read_memory(struct target *target, uint64_t address,
 	void *t;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted %d", target->state);
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1014,7 +1014,7 @@ static int mips_mips64_write_memory(struct target *target, uint64_t address,
 	int retval;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/openrisc/or1k.c b/src/target/openrisc/or1k.c
index d73bca2..8c38610 100644
--- a/src/target/openrisc/or1k.c
+++ b/src/target/openrisc/or1k.c
@@ -789,7 +789,7 @@ static int or1k_resume_or_step(struct target *target, int current,
 		  address, step ? "yes" : "no", handle_breakpoints ? "yes" : "no");
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1026,7 +1026,7 @@ static int or1k_read_memory(struct target *target, target_addr_t address,
 	LOG_DEBUG("Read memory at 0x%08" TARGET_PRIxADDR ", size: %" PRIu32 ", count: 0x%08" PRIx32, address, size, count);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1053,7 +1053,7 @@ static int or1k_write_memory(struct target *target, target_addr_t address,
 	LOG_DEBUG("Write memory at 0x%08" TARGET_PRIxADDR ", size: %" PRIu32 ", count: 0x%08" PRIx32, address, size, count);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("Target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/register.c b/src/target/register.c
index 2287125..e4f22f8 100644
--- a/src/target/register.c
+++ b/src/target/register.c
@@ -93,9 +93,8 @@ void register_unlink_cache(struct reg_cache **cache_p, const struct reg_cache *c
 /** Marks the contents of the register cache as invalid (and clean). */
 void register_cache_invalidate(struct reg_cache *cache)
 {
-	struct reg *reg = cache->reg_list;
-
-	for (unsigned int n = cache->num_regs; n != 0; n--, reg++) {
+	for (unsigned int n = 0; n < cache->num_regs; n++) {
+		struct reg *reg = &cache->reg_list[n];
 		if (!reg->exist)
 			continue;
 		reg->valid = false;
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 3d605c1..7be96cf 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -2603,7 +2603,7 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 	RISCV_INFO(info);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_TARGET_WARNING(target, "Target not halted.");
+		LOG_TARGET_ERROR(target, "not halted (run target algo)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/semihosting_common.c b/src/target/semihosting_common.c
index 6c91876..f7acc60 100644
--- a/src/target/semihosting_common.c
+++ b/src/target/semihosting_common.c
@@ -223,7 +223,10 @@ static ssize_t semihosting_write(struct semihosting *semihosting, int fd, void *
 		return semihosting_redirect_write(semihosting, buf, size);
 
 	/* default write */
-	return write(fd, buf, size);
+	int result = write(fd, buf, size);
+	if (result == -1)
+		semihosting->sys_errno = errno;
+	return result;
 }
 
 static ssize_t semihosting_redirect_read(struct semihosting *semihosting, void *buf, int size)
@@ -268,7 +271,8 @@ static inline ssize_t semihosting_read(struct semihosting *semihosting, int fd,
 
 	/* default read */
 	ssize_t result = read(fd, buf, size);
-	semihosting->sys_errno = errno;
+	if (result == -1)
+		semihosting->sys_errno = errno;
 
 	return result;
 }
@@ -294,6 +298,66 @@ static inline int semihosting_getchar(struct semihosting *semihosting, int fd)
  */
 static char *semihosting_user_op_params;
 
+const char *semihosting_opcode_to_str(const uint64_t opcode)
+{
+	switch (opcode) {
+		case SEMIHOSTING_SYS_CLOSE:
+			return "CLOSE";
+		case SEMIHOSTING_SYS_CLOCK:
+			return "CLOCK";
+		case SEMIHOSTING_SYS_ELAPSED:
+			return "ELAPSED";
+		case SEMIHOSTING_SYS_ERRNO:
+			return "ERRNO";
+		case SEMIHOSTING_SYS_EXIT:
+			return "EXIT";
+		case SEMIHOSTING_SYS_EXIT_EXTENDED:
+			return "EXIT_EXTENDED";
+		case SEMIHOSTING_SYS_FLEN:
+			return "FLEN";
+		case SEMIHOSTING_SYS_GET_CMDLINE:
+			return "GET_CMDLINE";
+		case SEMIHOSTING_SYS_HEAPINFO:
+			return "HEAPINFO";
+		case SEMIHOSTING_SYS_ISERROR:
+			return "ISERROR";
+		case SEMIHOSTING_SYS_ISTTY:
+			return "ISTTY";
+		case SEMIHOSTING_SYS_OPEN:
+			return "OPEN";
+		case SEMIHOSTING_SYS_READ:
+			return "READ";
+		case SEMIHOSTING_SYS_READC:
+			return "READC";
+		case SEMIHOSTING_SYS_REMOVE:
+			return "REMOVE";
+		case SEMIHOSTING_SYS_RENAME:
+			return "RENAME";
+		case SEMIHOSTING_SYS_SEEK:
+			return "SEEK";
+		case SEMIHOSTING_SYS_SYSTEM:
+			return "SYSTEM";
+		case SEMIHOSTING_SYS_TICKFREQ:
+			return "TICKFREQ";
+		case SEMIHOSTING_SYS_TIME:
+			return "TIME";
+		case SEMIHOSTING_SYS_TMPNAM:
+			return "TMPNAM";
+		case SEMIHOSTING_SYS_WRITE:
+			return "WRITE";
+		case SEMIHOSTING_SYS_WRITEC:
+			return "WRITEC";
+		case SEMIHOSTING_SYS_WRITE0:
+			return "WRITE0";
+		case SEMIHOSTING_USER_CMD_0X100 ... SEMIHOSTING_USER_CMD_0X1FF:
+			return "USER_CMD";
+		case SEMIHOSTING_ARM_RESERVED_START ... SEMIHOSTING_ARM_RESERVED_END:
+			return "ARM_RESERVED_CMD";
+		default:
+			return "<unknown>";
+	}
+}
+
 /**
  * Portable implementation of ARM semihosting calls.
  * Performs the currently pending semihosting operation
@@ -323,8 +387,9 @@ int semihosting_common(struct target *target)
 	/* Enough space to hold 4 long words. */
 	uint8_t fields[4*8];
 
-	LOG_DEBUG("op=0x%x, param=0x%" PRIx64, semihosting->op,
-		semihosting->param);
+	LOG_DEBUG("op=0x%x (%s), param=0x%" PRIx64, semihosting->op,
+			  semihosting_opcode_to_str(semihosting->op),
+			  semihosting->param);
 
 	switch (semihosting->op) {
 
@@ -388,12 +453,7 @@ int semihosting_common(struct target *target)
 							(fd == 0) ? "stdin" :
 							(fd == 1) ? "stdout" : "stderr");
 					/* Just pretend success */
-					if (semihosting->is_fileio) {
-						semihosting->result = 0;
-					} else {
-						semihosting->result = 0;
-						semihosting->sys_errno = 0;
-					}
+					semihosting->result = 0;
 					break;
 				}
 				/* Close the descriptor */
@@ -403,7 +463,8 @@ int semihosting_common(struct target *target)
 					fileio_info->param_1 = fd;
 				} else {
 					semihosting->result = close(fd);
-					semihosting->sys_errno = errno;
+					if (semihosting->result == -1)
+						semihosting->sys_errno = errno;
 					LOG_DEBUG("close(%d)=%" PRId64, fd, semihosting->result);
 				}
 			}
@@ -781,7 +842,8 @@ int semihosting_common(struct target *target)
 				int fd = semihosting_get_field(target, 0, fields);
 				// isatty() on Windows may return any non-zero value if fd is a terminal
 				semihosting->result = isatty(fd) ? 1 : 0;
-				semihosting->sys_errno = errno;
+				if (semihosting->result == 0)
+					semihosting->sys_errno = errno;
 				LOG_DEBUG("isatty(%d)=%" PRId64, fd, semihosting->result);
 			}
 			break;
@@ -873,14 +935,16 @@ int semihosting_common(struct target *target)
 							semihosting->result = -1;
 							semihosting->sys_errno = EINVAL;
 						} else if (strcmp((char *)fn, ":tt") == 0) {
-							if (mode == 0)
+							if (mode == 0) {
 								semihosting->result = 0;
-							else if (mode == 4)
+							} else if (mode == 4) {
 								semihosting->result = 1;
-							else if (mode == 8)
+							} else if (mode == 8) {
 								semihosting->result = 2;
-							else
+							} else {
 								semihosting->result = -1;
+								semihosting->sys_errno = EINVAL;
+							}
 						} else {
 							semihosting->hit_fileio = true;
 							fileio_info->identifier = "open";
@@ -895,25 +959,23 @@ int semihosting_common(struct target *target)
 							 * - 0-3 ("r") for stdin,
 							 * - 4-7 ("w") for stdout,
 							 * - 8-11 ("a") for stderr */
+							int fd;
 							if (mode < 4) {
-								int fd = dup(STDIN_FILENO);
-								semihosting->result = fd;
+								fd = dup(STDIN_FILENO);
 								semihosting->stdin_fd = fd;
-								semihosting->sys_errno = errno;
-								LOG_DEBUG("dup(STDIN)=%" PRId64, semihosting->result);
+								LOG_DEBUG("dup(STDIN)=%d", fd);
 							} else if (mode < 8) {
-								int fd = dup(STDOUT_FILENO);
-								semihosting->result = fd;
+								fd = dup(STDOUT_FILENO);
 								semihosting->stdout_fd = fd;
-								semihosting->sys_errno = errno;
-								LOG_DEBUG("dup(STDOUT)=%" PRId64, semihosting->result);
+								LOG_DEBUG("dup(STDOUT)=%d", fd);
 							} else {
-								int fd = dup(STDERR_FILENO);
-								semihosting->result = fd;
+								fd = dup(STDERR_FILENO);
 								semihosting->stderr_fd = fd;
-								semihosting->sys_errno = errno;
-								LOG_DEBUG("dup(STDERR)=%" PRId64, semihosting->result);
+								LOG_DEBUG("dup(STDERR)=%d", fd);
 							}
+							semihosting->result = fd;
+							if (fd == -1)
+								semihosting->sys_errno = errno;
 						} else {
 							/* cygwin requires the permission setting
 							 * otherwise it will fail to reopen a previously
@@ -921,7 +983,8 @@ int semihosting_common(struct target *target)
 							semihosting->result = open((char *)fn,
 									open_host_modeflags[mode],
 									0644);
-							semihosting->sys_errno = errno;
+							if (semihosting->result == -1)
+								semihosting->sys_errno = errno;
 							LOG_DEBUG("open('%s')=%" PRId64, fn, semihosting->result);
 						}
 					}
@@ -1070,7 +1133,8 @@ int semihosting_common(struct target *target)
 						}
 						fn[len] = 0;
 						semihosting->result = remove((char *)fn);
-						semihosting->sys_errno = errno;
+						if (semihosting->result == -1)
+							semihosting->sys_errno = errno;
 						LOG_DEBUG("remove('%s')=%" PRId64, fn, semihosting->result);
 
 						free(fn);
@@ -1139,7 +1203,9 @@ int semihosting_common(struct target *target)
 						fn2[len2] = 0;
 						semihosting->result = rename((char *)fn1,
 								(char *)fn2);
-						semihosting->sys_errno = errno;
+						// rename() on Windows returns nonzero on error
+						if (semihosting->result != 0)
+							semihosting->sys_errno = errno;
 						LOG_DEBUG("rename('%s', '%s')=%" PRId64 " %d", fn1, fn2, semihosting->result, errno);
 						free(fn1);
 						free(fn2);
@@ -1184,7 +1250,8 @@ int semihosting_common(struct target *target)
 					fileio_info->param_3 = SEEK_SET;
 				} else {
 					semihosting->result = lseek(fd, pos, SEEK_SET);
-					semihosting->sys_errno = errno;
+					if (semihosting->result == -1)
+						semihosting->sys_errno = errno;
 					LOG_DEBUG("lseek(%d, %d)=%" PRId64, fd, (int)pos, semihosting->result);
 					if (semihosting->result == pos)
 						semihosting->result = 0;
@@ -1322,7 +1389,6 @@ int semihosting_common(struct target *target)
 							return retval;
 						}
 						semihosting->result = semihosting_write(semihosting, fd, buf, len);
-						semihosting->sys_errno = errno;
 						LOG_DEBUG("write(%d, 0x%" PRIx64 ", %zu)=%" PRId64,
 							fd,
 							addr,
@@ -1470,8 +1536,9 @@ int semihosting_common(struct target *target)
 			retval = target_read_buffer(target, addr, len,
 					(uint8_t *)(semihosting_user_op_params));
 			if (retval != ERROR_OK) {
-				LOG_ERROR("Failed to read from target, semihosting op=0x%x",
-						semihosting->op);
+				LOG_ERROR("Failed to read from target, semihosting op=0x%x (%s)",
+						semihosting->op,
+						semihosting_opcode_to_str(semihosting->op));
 				free(semihosting_user_op_params);
 				semihosting_user_op_params = NULL;
 				return retval;
@@ -1609,7 +1676,6 @@ static int semihosting_common_fileio_end(struct target *target, int result,
 	semihosting->hit_fileio = false;
 
 	semihosting->result = result;
-	semihosting->sys_errno = fileio_errno;
 
 	/*
 	 * Some fileio results do not match up with what the semihosting
@@ -1631,6 +1697,17 @@ static int semihosting_common_fileio_end(struct target *target, int result,
 			break;
 	}
 
+	bool fileio_failed = false;
+	if (semihosting->op == SEMIHOSTING_SYS_ISTTY)
+		fileio_failed = (semihosting->result == 0);
+	else if (semihosting->op == SEMIHOSTING_SYS_RENAME)
+		fileio_failed = (semihosting->result != 0);
+	else
+		fileio_failed = (semihosting->result == -1);
+
+	if (fileio_failed)
+		semihosting->sys_errno = fileio_errno;
+
 	return semihosting->post_result(target);
 }
 
diff --git a/src/target/semihosting_common.h b/src/target/semihosting_common.h
index 7c5f748..a1848b4 100644
--- a/src/target/semihosting_common.h
+++ b/src/target/semihosting_common.h
@@ -65,6 +65,8 @@ enum semihosting_operation_numbers {
 	SEMIHOSTING_SYS_WRITE = 0x05,
 	SEMIHOSTING_SYS_WRITEC = 0x03,
 	SEMIHOSTING_SYS_WRITE0 = 0x04,
+	SEMIHOSTING_ARM_RESERVED_START = 0x32,
+	SEMIHOSTING_ARM_RESERVED_END = 0xFF,
 	SEMIHOSTING_USER_CMD_0X100 = 0x100, /* First user cmd op code */
 	SEMIHOSTING_USER_CMD_0X107 = 0x107, /* Last supported user cmd op code */
 	SEMIHOSTING_USER_CMD_0X1FF = 0x1FF, /* Last user cmd op code */
@@ -186,6 +188,13 @@ struct semihosting {
 	int (*post_result)(struct target *target);
 };
 
+/**
+ * @brief Convert the syscall opcode to a human-readable string
+ * @param[in] opcode Syscall opcode
+ * @return String representation of syscall opcode
+ */
+const char *semihosting_opcode_to_str(uint64_t opcode);
+
 int semihosting_common_init(struct target *target, void *setup,
 	void *post_result);
 int semihosting_common(struct target *target);
diff --git a/src/target/stm8.c b/src/target/stm8.c
index 91a59d7..ad4a452 100644
--- a/src/target/stm8.c
+++ b/src/target/stm8.c
@@ -994,7 +994,7 @@ static int stm8_resume(struct target *target, int current,
 			handle_breakpoints, debug_execution);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1303,7 +1303,7 @@ static int stm8_step(struct target *target, int current,
 	struct breakpoint *breakpoint = NULL;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1518,7 +1518,7 @@ static int stm8_remove_breakpoint(struct target *target,
 	struct stm8_common *stm8 = target_to_stm8(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1654,7 +1654,7 @@ static int stm8_remove_watchpoint(struct target *target,
 	struct stm8_common *stm8 = target_to_stm8(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
diff --git a/src/target/target.c b/src/target/target.c
index 4443584..a7942ac 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1335,7 +1335,7 @@ int target_add_breakpoint(struct target *target,
 		struct breakpoint *breakpoint)
 {
 	if ((target->state != TARGET_HALTED) && (breakpoint->type != BKPT_HARD)) {
-		LOG_WARNING("target %s is not halted (add breakpoint)", target_name(target));
+		LOG_TARGET_ERROR(target, "not halted (add breakpoint)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->add_breakpoint(target, breakpoint);
@@ -1345,7 +1345,7 @@ int target_add_context_breakpoint(struct target *target,
 		struct breakpoint *breakpoint)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted (add context breakpoint)", target_name(target));
+		LOG_TARGET_ERROR(target, "not halted (add context breakpoint)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->add_context_breakpoint(target, breakpoint);
@@ -1355,7 +1355,7 @@ int target_add_hybrid_breakpoint(struct target *target,
 		struct breakpoint *breakpoint)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted (add hybrid breakpoint)", target_name(target));
+		LOG_TARGET_ERROR(target, "not halted (add hybrid breakpoint)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->add_hybrid_breakpoint(target, breakpoint);
@@ -1371,7 +1371,7 @@ int target_add_watchpoint(struct target *target,
 		struct watchpoint *watchpoint)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted (add watchpoint)", target_name(target));
+		LOG_TARGET_ERROR(target, "not halted (add watchpoint)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->add_watchpoint(target, watchpoint);
@@ -1385,7 +1385,7 @@ int target_hit_watchpoint(struct target *target,
 		struct watchpoint **hit_watchpoint)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted (hit watchpoint)", target->cmd_name);
+		LOG_TARGET_ERROR(target, "not halted (hit watchpoint)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1467,7 +1467,7 @@ int target_step(struct target *target,
 int target_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted (gdb fileio)", target->cmd_name);
+		LOG_TARGET_ERROR(target, "not halted (gdb fileio)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->get_gdb_fileio_info(target, fileio_info);
@@ -1476,7 +1476,7 @@ int target_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fi
 int target_gdb_fileio_end(struct target *target, int retcode, int fileio_errno, bool ctrl_c)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target %s is not halted (gdb fileio end)", target->cmd_name);
+		LOG_TARGET_ERROR(target, "not halted (gdb fileio end)");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	return target->type->gdb_fileio_end(target, retcode, fileio_errno, ctrl_c);
@@ -4063,8 +4063,8 @@ COMMAND_HANDLER(handle_wp_command)
 		while (watchpoint) {
 			command_print(CMD, "address: " TARGET_ADDR_FMT
 					", len: 0x%8.8" PRIx32
-					", r/w/a: %i, value: 0x%8.8" PRIx32
-					", mask: 0x%8.8" PRIx32,
+					", r/w/a: %i, value: 0x%8.8" PRIx64
+					", mask: 0x%8.8" PRIx64,
 					watchpoint->address,
 					watchpoint->length,
 					(int)watchpoint->rw,
@@ -4078,15 +4078,20 @@ COMMAND_HANDLER(handle_wp_command)
 	enum watchpoint_rw type = WPT_ACCESS;
 	target_addr_t addr = 0;
 	uint32_t length = 0;
-	uint32_t data_value = 0x0;
-	uint32_t data_mask = 0xffffffff;
+	uint64_t data_value = 0x0;
+	uint64_t data_mask = WATCHPOINT_IGNORE_DATA_VALUE_MASK;
+	bool mask_specified = false;
 
 	switch (CMD_ARGC) {
 	case 5:
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[4], data_mask);
+		COMMAND_PARSE_NUMBER(u64, CMD_ARGV[4], data_mask);
+		mask_specified = true;
 		/* fall through */
 	case 4:
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[3], data_value);
+		COMMAND_PARSE_NUMBER(u64, CMD_ARGV[3], data_value);
+		// if user specified only data value without mask - the mask should be 0
+		if (!mask_specified)
+			data_mask = 0;
 		/* fall through */
 	case 3:
 		switch (CMD_ARGV[2][0]) {
@@ -6668,8 +6673,8 @@ COMMAND_HANDLER(handle_ps_command)
 	struct target *target = get_current_target(CMD_CTX);
 	char *display;
 	if (target->state != TARGET_HALTED) {
-		LOG_INFO("target not halted !!");
-		return ERROR_OK;
+		command_print(CMD, "Error: [%s] not halted", target_name(target));
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	if ((target->rtos) && (target->rtos->type)
@@ -6700,8 +6705,8 @@ COMMAND_HANDLER(handle_test_mem_access_command)
 	int retval = ERROR_OK;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_INFO("target not halted !!");
-		return ERROR_FAIL;
+		command_print(CMD, "Error: [%s] not halted", target_name(target));
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	if (CMD_ARGC != 1)
diff --git a/src/target/xscale.c b/src/target/xscale.c
index 066ff8c..fbf4351 100644
--- a/src/target/xscale.c
+++ b/src/target/xscale.c
@@ -1118,7 +1118,7 @@ static int xscale_resume(struct target *target, int current,
 	LOG_DEBUG("-");
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1382,7 +1382,7 @@ static int xscale_step(struct target *target, int current,
 	int retval;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1632,7 +1632,7 @@ static int xscale_full_context(struct target *target)
 	LOG_DEBUG("-");
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1705,7 +1705,7 @@ static int xscale_restore_banked(struct target *target)
 	int i, j;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1781,7 +1781,7 @@ static int xscale_read_memory(struct target *target, target_addr_t address,
 		count);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1880,7 +1880,7 @@ static int xscale_write_memory(struct target *target, target_addr_t address,
 		count);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2073,7 +2073,7 @@ static int xscale_set_breakpoint(struct target *target,
 	struct xscale_common *xscale = target_to_xscale(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2163,7 +2163,7 @@ static int xscale_unset_breakpoint(struct target *target,
 	struct xscale_common *xscale = target_to_xscale(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2210,7 +2210,7 @@ static int xscale_remove_breakpoint(struct target *target, struct breakpoint *br
 	struct xscale_common *xscale = target_to_xscale(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2232,7 +2232,7 @@ static int xscale_set_watchpoint(struct target *target,
 	uint32_t dbcon_value = buf_get_u32(dbcon->value, 0, 32);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2296,7 +2296,7 @@ static int xscale_add_watchpoint(struct target *target,
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
 
-	if (watchpoint->value)
+	if (watchpoint->mask != WATCHPOINT_IGNORE_DATA_VALUE_MASK)
 		LOG_WARNING("xscale does not support value, mask arguments; ignoring");
 
 	/* check that length is a power of two */
@@ -2336,7 +2336,7 @@ static int xscale_unset_watchpoint(struct target *target,
 	uint32_t dbcon_value = buf_get_u32(dbcon->value, 0, 32);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2369,7 +2369,7 @@ static int xscale_remove_watchpoint(struct target *target, struct watchpoint *wa
 	struct xscale_common *xscale = target_to_xscale(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2490,7 +2490,7 @@ static int xscale_read_trace(struct target *target)
 	unsigned int num_checkpoints = 0;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_WARNING("target must be stopped to read trace data");
+		LOG_TARGET_ERROR(target, "must be stopped to read trace data");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -3131,8 +3131,8 @@ static int xscale_mmu(struct target *target, int *enabled)
 	struct xscale_common *xscale = target_to_xscale(target);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
-		return ERROR_TARGET_INVALID;
+		LOG_TARGET_ERROR(target, "not halted");
+		return ERROR_TARGET_NOT_HALTED;
 	}
 	*enabled = xscale->armv4_5_mmu.mmu_enabled;
 	return ERROR_OK;
@@ -3149,8 +3149,8 @@ COMMAND_HANDLER(xscale_handle_mmu_command)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
-		return ERROR_OK;
+		command_print(CMD, "Error: target must be stopped for \"%s\" command", CMD_NAME);
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	if (CMD_ARGC >= 1) {
@@ -3179,8 +3179,8 @@ COMMAND_HANDLER(xscale_handle_idcache_command)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
-		return ERROR_OK;
+		command_print(CMD, "Error: target must be stopped for \"%s\" command", CMD_NAME);
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	bool icache = false;
@@ -3347,8 +3347,8 @@ COMMAND_HANDLER(xscale_handle_trace_buffer_command)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
-		return ERROR_OK;
+		command_print(CMD, "Error: target must be stopped for \"%s\" command", CMD_NAME);
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	if (CMD_ARGC >= 1) {
@@ -3451,8 +3451,8 @@ COMMAND_HANDLER(xscale_handle_dump_trace_command)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
-		return ERROR_OK;
+		command_print(CMD, "Error: target must be stopped for \"%s\" command", CMD_NAME);
+		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	if (CMD_ARGC < 1)
@@ -3514,8 +3514,8 @@ COMMAND_HANDLER(xscale_handle_cp15)
 		return retval;
 
 	if (target->state != TARGET_HALTED) {
-		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
-		return ERROR_OK;
+		command_print(CMD, "Error: target must be stopped for \"%s\" command", CMD_NAME);
+		return ERROR_TARGET_NOT_HALTED;
 	}
 	uint32_t reg_no = 0;
 	struct reg *reg = NULL;
diff --git a/src/target/xtensa/xtensa.c b/src/target/xtensa/xtensa.c
index 431c36a..c575b53 100644
--- a/src/target/xtensa/xtensa.c
+++ b/src/target/xtensa/xtensa.c
@@ -1541,7 +1541,7 @@ int xtensa_prepare_resume(struct target *target,
 		debug_execution);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_TARGET_WARNING(target, "target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 	xtensa->halt_request = false;
@@ -1667,7 +1667,7 @@ int xtensa_do_step(struct target *target, int current, target_addr_t address, in
 		current, address, handle_breakpoints);
 
 	if (target->state != TARGET_HALTED) {
-		LOG_TARGET_WARNING(target, "target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1941,7 +1941,7 @@ int xtensa_read_memory(struct target *target, target_addr_t address, uint32_t si
 	bool bswap = xtensa->target->endianness == TARGET_BIG_ENDIAN;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_TARGET_WARNING(target, "target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2037,7 +2037,7 @@ int xtensa_write_memory(struct target *target,
 	bool fill_head_tail = false;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_TARGET_WARNING(target, "target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -2566,11 +2566,11 @@ int xtensa_watchpoint_add(struct target *target, struct watchpoint *watchpoint)
 	xtensa_reg_val_t dbreakcval;
 
 	if (target->state != TARGET_HALTED) {
-		LOG_TARGET_WARNING(target, "target not halted");
+		LOG_TARGET_ERROR(target, "not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	if (watchpoint->mask != ~(uint32_t)0) {
+	if (watchpoint->mask != WATCHPOINT_IGNORE_DATA_VALUE_MASK) {
 		LOG_TARGET_ERROR(target, "watchpoint value masks not supported");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}