Handling harts becoming available when haltedexamine_unavailable_harts

Extending handling of unavailable harts

Ensuring target is selected when resuming it

Making halt_set_dcsr_ebreak more robust

OK if target is unavailble after waiting for halt

Can also use progbuf only for dcsr_ebreak

Using progbuf for set_dcsr_break now works

Handle unavailability when deasserting reset

6 files changed, 251 insertions, 236 deletions

diff --git a/src/target/riscv/opcodes.h b/src/target/riscv/opcodes.h
index 59c3413..70fca25 100644
--- a/src/target/riscv/opcodes.h
+++ b/src/target/riscv/opcodes.h
@@ -71,6 +71,12 @@ static uint32_t jal(unsigned int rd, uint32_t imm)
 	return imm_j(imm) | inst_rd(rd) | MATCH_JAL;
 }
 
+static uint32_t csrci(unsigned int csr, uint16_t imm) __attribute__ ((unused));
+static uint32_t csrci(unsigned int csr, uint16_t imm)
+{
+	return imm_i(csr) | inst_rs1(imm) | MATCH_CSRRCI;
+}
+
 static uint32_t csrsi(unsigned int csr, uint16_t imm) __attribute__ ((unused));
 static uint32_t csrsi(unsigned int csr, uint16_t imm)
 {
@@ -143,6 +149,12 @@ static uint32_t csrr(unsigned int rd, unsigned int csr)
 	return imm_i(csr) | inst_rd(rd) | MATCH_CSRRS;
 }
 
+static uint32_t csrrc(unsigned int rd, unsigned int rs, unsigned int csr) __attribute__ ((unused));
+static uint32_t csrrc(unsigned int rd, unsigned int rs, unsigned int csr)
+{
+	return imm_i(csr) | inst_rs1(rs) | inst_rd(rd) | MATCH_CSRRC;
+}
+
 static uint32_t csrrs(unsigned int rd, unsigned int rs, unsigned int csr) __attribute__ ((unused));
 static uint32_t csrrs(unsigned int rd, unsigned int rs, unsigned int csr)
 {
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 9492b4e..44826ef 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -32,7 +32,7 @@
 #include "debug_reg_printer.h"
 #include "field_helpers.h"
 
-static int riscv013_on_step_or_resume(struct target *target, bool step);
+static int riscv013_on_step_or_resume(struct target *target, bool skip, bool step);
 static int riscv013_step_or_resume_current_hart(struct target *target,
 		bool step);
 static int riscv013_clear_abstract_error(struct target *target);
@@ -45,7 +45,7 @@ static int riscv013_set_register(struct target *target, enum gdb_regno regid,
 static int dm013_select_hart(struct target *target, int hart_index);
 static int riscv013_halt_prep(struct target *target);
 static int riscv013_halt_go(struct target *target);
-static int riscv013_halt_current_hart(struct target *target);
+static int riscv013_halt_target(struct target *target);
 static int riscv013_resume_go(struct target *target);
 static int riscv013_step_current_hart(struct target *target);
 static int riscv013_on_step(struct target *target);
@@ -197,6 +197,7 @@ typedef struct {
 	uint8_t datasize;
 	uint8_t dataaccess;
 	int16_t dataaddr;
+	uint8_t nscratch;
 
 	/* The width of the hartsel field. */
 	unsigned hartsellen;
@@ -1776,28 +1777,66 @@ static int set_dcsr_ebreak(struct target *target, bool step)
 
 	RISCV_INFO(r);
 	RISCV013_INFO(info);
-	riscv_reg_t original_dcsr, dcsr;
-	/* We want to twiddle some bits in the debug CSR so debugging works. */
-	if (riscv_get_register(target, &dcsr, GDB_REGNO_DCSR) != ERROR_OK)
-		return ERROR_FAIL;
-	original_dcsr = dcsr;
-	dcsr = set_field(dcsr, CSR_DCSR_STEP, step);
-	dcsr = set_field(dcsr, CSR_DCSR_EBREAKM, r->riscv_ebreakm);
-	dcsr = set_field(dcsr, CSR_DCSR_EBREAKS, r->riscv_ebreaks && riscv_supports_extension(target, 'S'));
-	dcsr = set_field(dcsr, CSR_DCSR_EBREAKU, r->riscv_ebreaku && riscv_supports_extension(target, 'U'));
-	dcsr = set_field(dcsr, CSR_DCSR_EBREAKVS, r->riscv_ebreaku && riscv_supports_extension(target, 'H'));
-	dcsr = set_field(dcsr, CSR_DCSR_EBREAKVU, r->riscv_ebreaku && riscv_supports_extension(target, 'H'));
-	if (dcsr != original_dcsr &&
-			riscv_set_register(target, GDB_REGNO_DCSR, dcsr) != ERROR_OK)
-		return ERROR_FAIL;
+
+	if ((info->nscratch >= 1) && has_sufficient_progbuf(target, 8)) {
+		uint64_t set_ebreak_bits = 0;
+		uint64_t clr_ebreak_bits = 0;
+		if (r->riscv_ebreakm)
+			set_ebreak_bits |= CSR_DCSR_EBREAKM;
+		else
+			clr_ebreak_bits |= CSR_DCSR_EBREAKM;
+		if (r->riscv_ebreaks && riscv_supports_extension(target, 'S'))
+			set_ebreak_bits |= CSR_DCSR_EBREAKS;
+		else
+			clr_ebreak_bits |= CSR_DCSR_EBREAKS;
+		if (r->riscv_ebreaku && riscv_supports_extension(target, 'U'))
+			set_ebreak_bits |= CSR_DCSR_EBREAKU;
+		else
+			clr_ebreak_bits |= CSR_DCSR_EBREAKU;
+		if (r->riscv_ebreaku && riscv_supports_extension(target, 'H'))
+			set_ebreak_bits |= CSR_DCSR_EBREAKVS;
+		else
+			clr_ebreak_bits |= CSR_DCSR_EBREAKVS;
+		if (r->riscv_ebreaku && riscv_supports_extension(target, 'H'))
+			set_ebreak_bits |= CSR_DCSR_EBREAKVU;
+		else
+			clr_ebreak_bits |= CSR_DCSR_EBREAKVU;
+		struct riscv_program program;
+		riscv_program_init(&program, target);
+		riscv_program_insert(&program, csrw(S0, CSR_DSCRATCH0));
+		riscv_program_insert(&program, lui(S0, set_ebreak_bits));
+		riscv_program_insert(&program, csrrs(ZERO, S0, CSR_DCSR));
+		riscv_program_insert(&program, lui(S0, clr_ebreak_bits));
+		riscv_program_insert(&program, csrrc(ZERO, S0, CSR_DCSR));
+		if (step)
+			riscv_program_insert(&program, csrsi(CSR_DCSR, 0x4));
+		else
+			riscv_program_insert(&program, csrci(CSR_DCSR, 0x4));
+		riscv_program_insert(&program, csrr(S0, CSR_DSCRATCH0));
+		if (riscv_program_exec(&program, target) != ERROR_OK)
+			return ERROR_FAIL;
+	} else {
+		riscv_reg_t original_dcsr, dcsr;
+		/* We want to twiddle some bits in the debug CSR so debugging works. */
+		if (riscv_get_register(target, &dcsr, GDB_REGNO_DCSR) != ERROR_OK)
+			return ERROR_FAIL;
+		original_dcsr = dcsr;
+		dcsr = set_field(dcsr, CSR_DCSR_STEP, step);
+		dcsr = set_field(dcsr, CSR_DCSR_EBREAKM, r->riscv_ebreakm);
+		dcsr = set_field(dcsr, CSR_DCSR_EBREAKS, r->riscv_ebreaks && riscv_supports_extension(target, 'S'));
+		dcsr = set_field(dcsr, CSR_DCSR_EBREAKU, r->riscv_ebreaku && riscv_supports_extension(target, 'U'));
+		dcsr = set_field(dcsr, CSR_DCSR_EBREAKVS, r->riscv_ebreaku && riscv_supports_extension(target, 'H'));
+		dcsr = set_field(dcsr, CSR_DCSR_EBREAKVU, r->riscv_ebreaku && riscv_supports_extension(target, 'H'));
+		if (dcsr != original_dcsr &&
+				riscv_set_register(target, GDB_REGNO_DCSR, dcsr) != ERROR_OK)
+			return ERROR_FAIL;
+	}
 	info->dcsr_ebreak_is_set = true;
 	return ERROR_OK;
 }
 
 static int halt_set_dcsr_ebreak(struct target *target)
 {
-//	RISCV_INFO(r);
-	RISCV013_INFO(info);
 	LOG_TARGET_DEBUG(target, "Halt to set DCSR.ebreak*");
 
 	/* Remove this hart from the halt group.  This won't work on all targets
@@ -1830,68 +1869,42 @@ static int halt_set_dcsr_ebreak(struct target *target)
 		targets = target->smp_targets;
 		foreach_smp_target(entry, targets) {
 			struct target *t = entry->target;
-			riscv013_info_t *i = get_info(t);
-			if (i->haltgroup_supported) {
-				if (dm013_select_target(t) != ERROR_OK)
-					return ERROR_FAIL;
-				bool supported;
-				if (set_group(t, &supported, t->smp, HALT_GROUP) != ERROR_OK)
-					return ERROR_FAIL;
-				if (!supported)
-					LOG_TARGET_ERROR(target, "Couldn't place hart %d in halt group %d. "
-								 "Some harts may be unexpectedly halted.", t->coreid, t->smp);
+			if (riscv013_halt_prep(t) != ERROR_OK) {
+				LOG_TARGET_INFO(target, "riscv013_halt_prep failed - t_coreid: %d", t->coreid);
+				return ERROR_FAIL;
 			}
 		}
 	}
 
-	if (info->haltgroup_supported) {
-		if (dm013_select_target(target) != ERROR_OK)
-			return ERROR_FAIL;
-		bool supported;
-		if (set_group(target, &supported, 0, HALT_GROUP) != ERROR_OK)
-			return ERROR_FAIL;
-		if (!supported)
-			LOG_TARGET_ERROR(target, "Couldn't place hart in halt group 0. "
-						 "Some harts may be unexpectedly halted.");
-	}
-
 	int result = ERROR_OK;
-
-//	r->prepped = true;
-
-	if (dm013_select_target(target) != ERROR_OK)
-		return ERROR_FAIL;
-	if (riscv013_halt_current_hart(target) == ERROR_OK) {
-//		if (dm013_select_target(target) != ERROR_OK)
-//			return ERROR_FAIL;
-//		if (set_dcsr_ebreak(target, false) == ERROR_OK) {
-			if (dm013_select_target(target) != ERROR_OK)
-				return ERROR_FAIL;
-			if (riscv013_step_or_resume_current_hart(target, false) == ERROR_OK) {
+	int halt_result = ERROR_OK;
+	int resume_result = ERROR_OK;
+
+	halt_result = riscv013_halt_go(target);
+	if (halt_result == ERROR_OK) {
+		if (riscv013_on_step_or_resume(target, true, false) == ERROR_OK) {
+			resume_result = riscv013_step_or_resume_current_hart(target, false);
+			if (resume_result == ERROR_OK) {
 				target->state = TARGET_RUNNING;
 				target->debug_reason = DBG_REASON_NOTHALTED;
+			} else if (resume_result == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
+				LOG_TARGET_INFO(target, "riscv013_resume_go aborted");
+				target->state = TARGET_UNAVAILABLE;
 			} else {
+				LOG_TARGET_INFO(target, "riscv013_resume_go failed");
 				result = ERROR_FAIL;
 			}
-//		} else {
-//			result = ERROR_FAIL;
-//		}
+		} else {
+			LOG_TARGET_INFO(target, "resume_prep failed");
+			result = ERROR_FAIL;
+		}
+	} else if (halt_result == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
+		LOG_TARGET_INFO(target, "riscv013_halt_go aborted");
 	} else {
+		LOG_TARGET_INFO(target, "riscv013_halt_go failed");
 		result = ERROR_FAIL;
 	}
 
-	/* Add it back to the halt group. */
-	if (info->haltgroup_supported) {
-		if (dm013_select_target(target) != ERROR_OK)
-			return ERROR_FAIL;
-		bool supported;
-		if (set_group(target, &supported, target->smp, HALT_GROUP) != ERROR_OK)
-			return ERROR_FAIL;
-		if (!supported)
-			LOG_TARGET_ERROR(target, "Couldn't place hart back in halt group %d. "
-						 "Some harts may be unexpectedly halted.", target->smp);
-	}
-
 	return result;
 }
 
@@ -2023,6 +2036,7 @@ static int examine(struct target *target)
 	info->datasize = get_field(hartinfo, DM_HARTINFO_DATASIZE);
 	info->dataaccess = get_field(hartinfo, DM_HARTINFO_DATAACCESS);
 	info->dataaddr = get_field(hartinfo, DM_HARTINFO_DATAADDR);
+	info->nscratch = get_field(hartinfo, DM_HARTINFO_NSCRATCH);
 
 	if (!get_field(dmstatus, DM_DMSTATUS_AUTHENTICATED)) {
 		LOG_TARGET_ERROR(target, "Debugger is not authenticated to target Debug Module. "
@@ -2060,9 +2074,6 @@ static int examine(struct target *target)
 			if (get_field(s, DM_DMSTATUS_ANYHAVERESET))
 				dm_write(target, DM_DMCONTROL,
 						set_dmcontrol_hartsel(DM_DMCONTROL_DMACTIVE | DM_DMCONTROL_ACKHAVERESET, i));
-			if (get_field(s, DM_DMSTATUS_HASRESETHALTREQ))
-				dm_write(target, DM_DMCONTROL,
-						set_dmcontrol_hartsel(DM_DMCONTROL_DMACTIVE | DM_DMCONTROL_CLRRESETHALTREQ, i));
 		}
 
 		LOG_TARGET_DEBUG(target, "Detected %d harts.", dm->hart_count);
@@ -2083,16 +2094,17 @@ static int examine(struct target *target)
 	if (riscv_get_hart_state(target, &state_at_examine_start) != ERROR_OK)
 		return ERROR_FAIL;
 
-	/* Skip full examination of hart if it is unavailable */
+	/* Skip full examination and reporting of hart if it is currently unavailable */
 	const bool hart_unavailable_at_examine_start = state_at_examine_start == RISCV_STATE_UNAVAILABLE;
 	if (hart_unavailable_at_examine_start) {
-		LOG_TARGET_DEBUG(target, "Did not fully examine hart %d as it was unavailable, deferring examine.", info->index);
+		LOG_TARGET_DEBUG(target, "Did not fully examine hart %d as it was currently unavailable, deferring examine.", info->index);
+		target->state = TARGET_UNAVAILABLE;
 		target->defer_examine = true;
 		return ERROR_OK;
 	}
 	const bool hart_halted_at_examine_start = state_at_examine_start == RISCV_STATE_HALTED;
 	if (!hart_halted_at_examine_start) {
-		if (riscv013_halt_current_hart(target) != ERROR_OK) {
+		if (riscv013_halt_target(target) != ERROR_OK) {
 			LOG_TARGET_ERROR(target, "Fatal: Hart %d failed to halt during %s",
 					info->index, __func__);
 			return ERROR_FAIL;
@@ -2741,7 +2753,7 @@ static int riscv013_get_hart_state(struct target *target, enum riscv_hart_state
 	if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
 		return ERROR_FAIL;
 	if (get_field(dmstatus, DM_DMSTATUS_ANYHAVERESET)) {
-//		LOG_TARGET_INFO(target, "Hart unexpectedly reset!");
+		LOG_TARGET_DEBUG(target, "Hart unexpectedly reset!");
 		info->dcsr_ebreak_is_set = false;
 		/* TODO: Can we make this more obvious to eg. a gdb user? */
 		uint32_t dmcontrol = DM_DMCONTROL_DMACTIVE |
@@ -2776,6 +2788,29 @@ static int riscv013_get_hart_state(struct target *target, enum riscv_hart_state
 	return ERROR_FAIL;
 }
 
+static int handle_became_available(struct target *target,
+		enum riscv_hart_state previous_riscv_state)
+{
+	if (dm013_select_target(target) != ERROR_OK) {
+		LOG_TARGET_INFO(target, "dm013_select_target failed");
+		return ERROR_FAIL;
+	}
+	target->state = TARGET_HALTED;
+	int result = riscv013_step_or_resume_current_hart(target, false);
+	if (result == ERROR_OK) {
+		target->state = TARGET_RUNNING;
+		target->debug_reason = DBG_REASON_NOTHALTED;
+	} else if (result == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
+		LOG_TARGET_INFO(target, "riscv013_step_or_resume_current_hart aborted");
+		target->state = TARGET_UNAVAILABLE;
+		return ERROR_OK;
+	} else {
+		LOG_TARGET_INFO(target, "riscv013_step_or_resume_current_hart failed");
+		return ERROR_FAIL;
+	}
+	return ERROR_OK;
+}
+
 static int handle_became_unavailable(struct target *target,
 		enum riscv_hart_state previous_riscv_state)
 {
@@ -2830,6 +2865,7 @@ static int init_target(struct command_context *cmd_ctx,
 	generic_info->data_bits = &riscv013_data_bits;
 	generic_info->print_info = &riscv013_print_info;
 
+	generic_info->handle_became_available = &handle_became_available;
 	generic_info->handle_became_unavailable = &handle_became_unavailable;
 	generic_info->tick = &tick;
 
@@ -2943,7 +2979,9 @@ static int deassert_reset(struct target *target)
 
 	info->dmi_busy_delay = orig_dmi_busy_delay;
 
-	if (target->reset_halt) {
+	if (get_field(dmstatus, DM_DMSTATUS_ALLUNAVAIL)) {
+		target->state = TARGET_UNAVAILABLE;
+	} else if (target->reset_halt) {
 		target->state = TARGET_HALTED;
 		target->debug_reason = DBG_REASON_DBGRQ;
 	} else {
@@ -4956,166 +4994,62 @@ static int dm013_select_hart(struct target *target, int hart_index)
 	return ERROR_OK;
 }
 
-/* Select all harts that were prepped and that are selectable, clearing the
- * prepped flag on the harts that actually were selected. */
-static int select_prepped_harts(struct target *target)
+static int riscv013_halt_prep(struct target *target)
 {
-	RISCV_INFO(r);
-	dm013_info_t *dm = get_dm(target);
-	if (!dm)
-		return ERROR_FAIL;
-	if (!dm->hasel_supported) {
-		r->prepped = false;
-		return dm013_select_target(target);
-	}
-
-	assert(dm->hart_count);
-	unsigned hawindow_count = (dm->hart_count + 31) / 32;
-	uint32_t *hawindow = calloc(hawindow_count, sizeof(uint32_t));
-	if (!hawindow)
-		return ERROR_FAIL;
-
-	target_list_t *entry;
-	unsigned total_selected = 0;
-	unsigned int selected_index = 0;
-	list_for_each_entry(entry, &dm->target_list, list) {
-		struct target *t = entry->target;
-		struct riscv_info *info = riscv_info(t);
-		riscv013_info_t *info_013 = get_info(t);
-		unsigned int index = info_013->index;
-		LOG_TARGET_DEBUG(target, "index=%d, prepped=%d", index, info->prepped);
-		if (info->prepped) {
-			info_013->selected = true;
-			hawindow[index / 32] |= 1 << (index % 32);
-			info->prepped = false;
-			total_selected++;
-			selected_index = index;
-		}
-	}
-
-	if (total_selected == 0) {
-		LOG_TARGET_ERROR(target, "No harts were prepped!");
-		free(hawindow);
-		return ERROR_FAIL;
-	} else if (total_selected == 1) {
-		/* Don't use hasel if we only need to talk to one hart. */
-		free(hawindow);
-		return dm013_select_hart(target, selected_index);
-	}
+	LOG_TARGET_DEBUG(target, "grouping hart");
 
-	if (dm013_select_hart(target, HART_INDEX_MULTIPLE) != ERROR_OK) {
-		free(hawindow);
-		return ERROR_FAIL;
-	}
-
-	for (unsigned i = 0; i < hawindow_count; i++) {
-		if (dm_write(target, DM_HAWINDOWSEL, i) != ERROR_OK) {
-			free(hawindow);
-			return ERROR_FAIL;
-		}
-		if (dm_write(target, DM_HAWINDOW, hawindow[i]) != ERROR_OK) {
-			free(hawindow);
-			return ERROR_FAIL;
+	if (target->smp) {
+		/* Let's make sure that all non-halted harts are in the same halt group */
+		riscv013_info_t *info = get_info(target);
+		if (info->haltgroup_supported) {
+			if (dm013_select_target(target) != ERROR_OK)
+				return ERROR_FAIL;
+			bool supported;
+			if (set_group(target, &supported, target->smp, HALT_GROUP) != ERROR_OK)
+				return ERROR_FAIL;
+			if (!supported)
+				LOG_TARGET_ERROR(target, "Couldn't place hart %d in halt group %d. "
+							 "Some harts may be unexpectedly halted.", target->coreid, target->smp);
 		}
 	}
 
-	free(hawindow);
-	return ERROR_OK;
-}
-
-static int riscv013_halt_prep(struct target *target)
-{
 	return ERROR_OK;
 }
 
 static int riscv013_halt_go(struct target *target)
 {
-	dm013_info_t *dm = get_dm(target);
-	if (!dm)
-		return ERROR_FAIL;
-
-	if (select_prepped_harts(target) != ERROR_OK)
-		return ERROR_FAIL;
-
 	LOG_TARGET_DEBUG(target, "halting hart");
 
-	/* Issue the halt command, and then wait for the current hart to halt. */
-	uint32_t dmcontrol = DM_DMCONTROL_DMACTIVE | DM_DMCONTROL_HALTREQ;
-	dmcontrol = set_dmcontrol_hartsel(dmcontrol, dm->current_hartid);
-	dm_write(target, DM_DMCONTROL, dmcontrol);
-	uint32_t dmstatus;
-	for (size_t i = 0; i < 256; ++i) {
-		if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
-			return ERROR_FAIL;
-		/* When no harts are running, there's no point in continuing this loop. */
-		if (!get_field(dmstatus, DM_DMSTATUS_ANYRUNNING))
-			break;
-	}
-
-	/* We declare success if no harts are running. One or more of them may be
-	 * unavailable, though. */
-
-	if ((get_field(dmstatus, DM_DMSTATUS_ANYRUNNING))) {
-		if (dm_read(target, &dmcontrol, DM_DMCONTROL) != ERROR_OK)
-			return ERROR_FAIL;
-
-		LOG_TARGET_ERROR(target, "Unable to halt. dmcontrol=0x%08x, dmstatus=0x%08x",
-				  dmcontrol, dmstatus);
+	if (dm013_select_target(target) != ERROR_OK) {
 		return ERROR_FAIL;
 	}
-
-	dmcontrol = set_field(dmcontrol, DM_DMCONTROL_HALTREQ, 0);
-	dm_write(target, DM_DMCONTROL, dmcontrol);
-
-	if (dm->current_hartid == HART_INDEX_MULTIPLE) {
-		target_list_t *entry;
-		list_for_each_entry(entry, &dm->target_list, list) {
-			struct target *t = entry->target;
-			uint32_t t_dmstatus;
-			if (get_field(dmstatus, DM_DMSTATUS_ALLHALTED) ||
-					get_field(dmstatus, DM_DMSTATUS_ALLUNAVAIL)) {
-				/* All harts are either halted or unavailable. No
-				 * need to read dmstatus for each hart. */
-				t_dmstatus = dmstatus;
-			} else {
-				/* Only some harts were halted/unavailable. Read
-				 * dmstatus for this one to see what its status
-				 * is. */
-				riscv013_info_t *info = get_info(t);
-				dmcontrol = set_dmcontrol_hartsel(dmcontrol, info->index);
-				if (dm_write(target, DM_DMCONTROL, dmcontrol) != ERROR_OK)
-					return ERROR_FAIL;
-				dm->current_hartid = info->index;
-				if (dm_read(target, &t_dmstatus, DM_DMSTATUS) != ERROR_OK)
-					return ERROR_FAIL;
-			}
-			/* Set state for the current target based on its dmstatus. */
-			if (get_field(t_dmstatus, DM_DMSTATUS_ALLHALTED)) {
-				t->state = TARGET_HALTED;
-				if (t->debug_reason == DBG_REASON_NOTHALTED)
-					t->debug_reason = DBG_REASON_DBGRQ;
-			} else if (get_field(t_dmstatus, DM_DMSTATUS_ALLUNAVAIL)) {
-				t->state = TARGET_UNAVAILABLE;
-			}
+	if (target->smp) {
+		/* Let's make sure that harts we want to halt are placed in another group */
+		riscv013_info_t *info = get_info(target);
+		if (info->haltgroup_supported) {
+			bool supported;
+			if (set_group(target, &supported, 0, HALT_GROUP) != ERROR_OK)
+				return ERROR_FAIL;
+			if (!supported)
+				LOG_TARGET_ERROR(target, "Couldn't place hart in halt group 0. "
+							 "Some harts may be unexpectedly halted.");
 		}
+	}
 
-	} else {
-		/* Set state for the current target based on its dmstatus. */
-		if (get_field(dmstatus, DM_DMSTATUS_ALLHALTED)) {
-			target->state = TARGET_HALTED;
-			if (target->debug_reason == DBG_REASON_NOTHALTED)
-				target->debug_reason = DBG_REASON_DBGRQ;
-		} else if (get_field(dmstatus, DM_DMSTATUS_ALLUNAVAIL)) {
-			target->state = TARGET_UNAVAILABLE;
-		}
+	int result = riscv013_halt_target(target);
+	if (result == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	else if (result != ERROR_OK) {
+		return ERROR_FAIL;
 	}
 
 	return ERROR_OK;
 }
 
-static int riscv013_halt_current_hart(struct target *target)
+static int riscv013_halt_target(struct target *target)
 {
-	LOG_TARGET_DEBUG(target, "halting current hart");
+	LOG_TARGET_DEBUG(target, "halting one hart");
 
 	dm013_info_t *dm = get_dm(target);
 	/* Issue the halt command, and then wait for the current hart to halt. */
@@ -5152,6 +5086,7 @@ static int riscv013_halt_current_hart(struct target *target)
 			target->debug_reason = DBG_REASON_DBGRQ;
 	} else if (get_field(dmstatus, DM_DMSTATUS_ALLUNAVAIL)) {
 		target->state = TARGET_UNAVAILABLE;
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 	}
 
 	return ERROR_OK;
@@ -5159,9 +5094,6 @@ static int riscv013_halt_current_hart(struct target *target)
 
 static int riscv013_resume_go(struct target *target)
 {
-	if (select_prepped_harts(target) != ERROR_OK)
-		return ERROR_FAIL;
-
 	return riscv013_step_or_resume_current_hart(target, false);
 }
 
@@ -5173,12 +5105,12 @@ static int riscv013_step_current_hart(struct target *target)
 static int riscv013_resume_prep(struct target *target)
 {
 	assert(target->state == TARGET_HALTED);
-	return riscv013_on_step_or_resume(target, false);
+	return riscv013_on_step_or_resume(target, false, false);
 }
 
 static int riscv013_on_step(struct target *target)
 {
-	return riscv013_on_step_or_resume(target, true);
+	return riscv013_on_step_or_resume(target, false, true);
 }
 
 static enum riscv_halt_reason riscv013_halt_reason(struct target *target)
@@ -5314,17 +5246,17 @@ void riscv013_fill_dm_nop_u64(struct target *target, char *buf)
 	riscv013_fill_dmi_nop_u64(target, buf);
 }
 
-static int maybe_execute_fence_i(struct target *target)
+static int maybe_execute_fence_i(struct target *target, bool skip)
 {
-	if (has_sufficient_progbuf(target, 2))
+	if (!skip && has_sufficient_progbuf(target, 2))
 		return execute_fence(target);
 	return ERROR_OK;
 }
 
 /* Helper Functions. */
-static int riscv013_on_step_or_resume(struct target *target, bool step)
+static int riscv013_on_step_or_resume(struct target *target, bool skip, bool step)
 {
-	if (maybe_execute_fence_i(target) != ERROR_OK)
+	if (maybe_execute_fence_i(target, skip) != ERROR_OK)
 		return ERROR_FAIL;
 
 	if (set_dcsr_ebreak(target, step) != ERROR_OK)
@@ -5344,8 +5276,14 @@ static int riscv013_step_or_resume_current_hart(struct target *target,
 	}
 	LOG_TARGET_DEBUG(target, "resuming (for step?=%d)", step);
 
-	if (riscv_flush_registers(target) != ERROR_OK)
+	if (riscv_flush_registers(target) != ERROR_OK) {
+		LOG_TARGET_INFO(target, "riscv_flush_registers failed");
 		return ERROR_FAIL;
+	}
+
+	if (dm013_select_target(target) != ERROR_OK) {
+		return ERROR_FAIL;
+	}
 
 	dm013_info_t *dm = get_dm(target);
 	/* Issue the resume command, and then wait for the current hart to resume. */
@@ -5358,24 +5296,50 @@ static int riscv013_step_or_resume_current_hart(struct target *target,
 	uint32_t dmstatus;
 	for (size_t i = 0; i < 256; ++i) {
 		usleep(10);
-		if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
-			return ERROR_FAIL;
-		if (get_field(dmstatus, DM_DMSTATUS_ALLUNAVAIL))
+		if (dmstatus_read(target, &dmstatus, true) != ERROR_OK) {
+			LOG_TARGET_INFO(target, "dmstatus_read failed");
 			return ERROR_FAIL;
+		}
+		if (get_field(dmstatus, DM_DMSTATUS_ALLUNAVAIL)) {
+			LOG_TARGET_INFO(target, "dmstatus_read aborted");
+			target->state = TARGET_UNAVAILABLE;
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+		if (get_field(dmstatus, DM_DMSTATUS_ANYHAVERESET)) {
+			dmcontrol = dmcontrol | DM_DMCONTROL_ACKHAVERESET;
+		}
 		if (get_field(dmstatus, DM_DMSTATUS_ALLRESUMEACK) == 0)
 			continue;
 		if (step && get_field(dmstatus, DM_DMSTATUS_ALLHALTED) == 0)
 			continue;
 
 		dm_write(target, DM_DMCONTROL, dmcontrol);
+
+		if (target->smp) {
+			/* Let's make sure that this hart is placed back with all non-halted harts */
+			riscv013_info_t *info = get_info(target);
+			if (info->haltgroup_supported) {
+				bool supported;
+				if (set_group(target, &supported, target->smp, HALT_GROUP) != ERROR_OK) {
+					LOG_TARGET_INFO(target, "set_group failed");
+					return ERROR_FAIL;
+				}
+				if (!supported)
+					LOG_TARGET_ERROR(target, "Couldn't place hart back in halt group %d. "
+								 "Some harts may be unexpectedly halted.", target->smp);
+			}
+		}
+
 		return ERROR_OK;
 	}
 
 	dm_write(target, DM_DMCONTROL, dmcontrol);
 
 	LOG_TARGET_ERROR(target, "unable to resume");
-	if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
+	if (dmstatus_read(target, &dmstatus, true) != ERROR_OK) {
+		LOG_TARGET_INFO(target, "dmstatus_read failed");
 		return ERROR_FAIL;
+	}
 	LOG_TARGET_ERROR(target, "  dmstatus=0x%08x", dmstatus);
 
 	if (step) {
@@ -5384,6 +5348,7 @@ static int riscv013_step_or_resume_current_hart(struct target *target,
 		return ERROR_OK;
 	}
 
+	LOG_TARGET_INFO(target, "resume_current_hart failed");
 	return ERROR_FAIL;
 }
 
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 0b2963c..bdbb8e4 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -3046,11 +3046,21 @@ static int riscv_poll_hart(struct target *target, enum riscv_next_action *next_a
 			return ERROR_FAIL;
 	}
 
+	/* The hart apparently became unavailable while halted, so we want to resume it */
+	if (state == RISCV_STATE_HALTED && previous_riscv_state == RISCV_STATE_UNAVAILABLE) {
+		if (r->handle_became_available &&
+				r->handle_became_available(target, previous_riscv_state) != ERROR_OK)
+			return ERROR_FAIL;
+		if (riscv_get_hart_state(target, &state) != ERROR_OK)
+			return ERROR_FAIL;
+	}
+
 	if (target->state == TARGET_UNKNOWN || state != previous_riscv_state) {
 		switch (state) {
 			case RISCV_STATE_HALTED:
-				if (previous_riscv_state == RISCV_STATE_UNAVAILABLE)
+				if (previous_riscv_state == RISCV_STATE_UNAVAILABLE) {
 					LOG_TARGET_INFO(target, "became available (halted)");
+				}
 
 				LOG_TARGET_DEBUG(target, "  triggered a halt; previous_target_state=%d",
 					previous_target_state);
@@ -3097,8 +3107,9 @@ static int riscv_poll_hart(struct target *target, enum riscv_next_action *next_a
 				break;
 
 			case RISCV_STATE_RUNNING:
-				if (previous_riscv_state == RISCV_STATE_UNAVAILABLE)
+				if (previous_riscv_state == RISCV_STATE_UNAVAILABLE) {
 					LOG_TARGET_INFO(target, "became available (running)");
+				}
 
 				LOG_TARGET_DEBUG(target, "  triggered running");
 				target->state = TARGET_RUNNING;
@@ -3109,8 +3120,16 @@ static int riscv_poll_hart(struct target *target, enum riscv_next_action *next_a
 				break;
 
 			case RISCV_STATE_UNAVAILABLE:
-				LOG_TARGET_DEBUG(target, "  became unavailable");
-				LOG_TARGET_INFO(target, "became unavailable.");
+				if (previous_riscv_state == RISCV_STATE_HALTED) {
+					LOG_TARGET_DEBUG(target, "  became unavailable (halted)");
+					LOG_TARGET_INFO(target, "became unavailable (halted).");
+				} else if (previous_riscv_state == RISCV_STATE_RUNNING) {
+					LOG_TARGET_DEBUG(target, "  became unavailable (running)");
+					LOG_TARGET_INFO(target, "became unavailable (running).");
+				} else {
+					LOG_TARGET_DEBUG(target, "  became unavailable");
+					LOG_TARGET_INFO(target, "became unavailable.");
+				}
 				target->state = TARGET_UNAVAILABLE;
 				if (r->handle_became_unavailable &&
 						r->handle_became_unavailable(target, previous_riscv_state) != ERROR_OK)
@@ -3213,8 +3232,10 @@ int riscv_openocd_poll(struct target *target)
 			continue;
 
 		enum riscv_next_action next_action;
-		if (riscv_poll_hart(t, &next_action) != ERROR_OK)
+		if (riscv_poll_hart(t, &next_action) != ERROR_OK) {
+			LOG_TARGET_INFO(target, "riscv_poll_hart failed - next_action: %d", next_action);
 			return ERROR_FAIL;
+		}
 
 		switch (next_action) {
 			case RPH_NONE:
@@ -3271,8 +3292,10 @@ int riscv_openocd_poll(struct target *target)
 	foreach_smp_target(entry, targets) {
 		struct target *t = entry->target;
 		struct riscv_info *info = riscv_info(t);
-		if (info->tick && info->tick(t) != ERROR_OK)
+		if (info->tick && info->tick(t) != ERROR_OK) {
+			LOG_TARGET_INFO(target, "info_tick failed - t_coreid: %d", t->coreid);
 			return ERROR_FAIL;
+		}
 	}
 
 	/* Sample memory if any target is running. */
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 8f28232..8ac4ed1 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -213,6 +213,8 @@ struct riscv_info {
 		enum riscv_hart_state previous_riscv_state);
 	int (*handle_became_running)(struct target *target,
 		enum riscv_hart_state previous_riscv_state);
+	int (*handle_became_available)(struct target *target,
+		enum riscv_hart_state previous_riscv_state);
 	int (*handle_became_unavailable)(struct target *target,
 		enum riscv_hart_state previous_riscv_state);
 
diff --git a/src/target/startup.tcl b/src/target/startup.tcl
index 75e0edc..8ddac0c 100644
--- a/src/target/startup.tcl
+++ b/src/target/startup.tcl
@@ -131,6 +131,11 @@ proc ocd_process_reset_inner { MODE } {
 				}
 			}
 
+			# no need to wait for a target that is unavailable anyway
+			if { [$t curstate] == "unavailable" } {
+				continue
+			}
+
 			# Wait up to 1 second for target to halt. Why 1sec? Cause
 			# the JTAG tap reset signal might be hooked to a slow
 			# resistor/capacitor circuit - and it might take a while
@@ -142,8 +147,11 @@ proc ocd_process_reset_inner { MODE } {
 			# Did we succeed?
 			set s [$t curstate]
 
+			if { $s == "unavailable" } {
+				continue
+			}
 			if { $s != "halted" } {
-				return -code error [format "TARGET: %s - Not halted" $t]
+				return -code error [format "TARGET: %s - Not halted (%s)" $t $s]
 			}
 		}
 	}
@@ -160,6 +168,9 @@ proc ocd_process_reset_inner { MODE } {
 			if { ![$t was_examined] && [$t examine_deferred] } {
 				continue
 			}
+			if { [$t curstate] == "unavailable" } {
+				continue
+			}
 
 			set err [catch "$t arp_waitstate halted 5000"]
 			# Did it halt?
diff --git a/src/target/target.c b/src/target/target.c
index e7d043f..d3e7c6c 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -486,8 +486,10 @@ int target_poll(struct target *target)
 	}
 
 	retval = target->type->poll(target);
-	if (retval != ERROR_OK)
+	if (retval != ERROR_OK) {
+		LOG_TARGET_INFO(target, "target->type->poll failed");
 		return retval;
+	}
 
 	if (target->halt_issued) {
 		if (target->state == TARGET_HALTED)
@@ -687,7 +689,7 @@ int target_examine_one(struct target *target)
 	}
 
 	if (target->defer_examine) {
-		LOG_USER("[%s] Target unavailable for full examination.", target_name(target));
+		LOG_USER("[%s] Target currently unavailable for full examination.", target_name(target));
 		target->defer_examine = defer_state;
 		target_reset_examined(target);
 	} else {
@@ -696,7 +698,7 @@ int target_examine_one(struct target *target)
 	}
 	target_call_event_callbacks(target, TARGET_EVENT_EXAMINE_END);
 
-	LOG_TARGET_INFO(target, "Examination succeed");
+	LOG_TARGET_DEBUG(target, "Examination succeed");
 	return ERROR_OK;
 }