Merge pull request #767 from riscv/unavailable

Handle harts becoming unavailable while they're being debugged.

6 files changed, 303 insertions, 172 deletions

diff --git a/src/rtos/hwthread.c b/src/rtos/hwthread.c
index 4902e02..28f9727 100644
--- a/src/rtos/hwthread.c
+++ b/src/rtos/hwthread.c
@@ -113,7 +113,8 @@ static int hwthread_update_threads(struct rtos *rtos)
 		foreach_smp_target(head, target->smp_targets) {
 			struct target *curr = head->target;
 
-			if (!target_was_examined(curr))
+			if (!target_was_examined(curr) ||
+					curr->state == TARGET_UNAVAILABLE)
 				continue;
 
 			++thread_list_size;
@@ -134,7 +135,8 @@ static int hwthread_update_threads(struct rtos *rtos)
 		foreach_smp_target(head, target->smp_targets) {
 			struct target *curr = head->target;
 
-			if (!target_was_examined(curr))
+			if (!target_was_examined(curr) ||
+					curr->state == TARGET_UNAVAILABLE)
 				continue;
 
 			threadid_t tid = threadid_from_target(curr);
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 67dfb9c..edea5f5 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -154,6 +154,29 @@ static int gdb_use_target_description = 1;
 /* current processing free-run type, used by file-I/O */
 static char gdb_running_type;
 
+/* Find an available target in the SMP group that gdb is connected to. For
+ * commands that affect an entire SMP group (like memory access and run control)
+ * this will give better results than returning the unavailable target and having
+ * the command fail.  If gdb was aware that targets can be unavailable we
+ * wouldn't need this logic.
+ */
+struct target *get_available_target_from_connection(struct connection *connection)
+{
+	struct gdb_service *gdb_service = connection->service->priv;
+	struct target *target = gdb_service->target;
+	if (target->state == TARGET_UNAVAILABLE && target->smp) {
+		struct target_list *tlist;
+		foreach_smp_target(tlist, target->smp_targets) {
+			struct target *t = tlist->target;
+			if (t->state != TARGET_UNAVAILABLE)
+				return t;
+		}
+		/* If we can't find an available target, just return the
+		 * original. */
+	}
+	return target;
+}
+
 static int gdb_last_signal(struct target *target)
 {
 	switch (target->debug_reason) {
@@ -976,9 +999,9 @@ static int gdb_target_callback_event_handler(struct target *target,
 		enum target_event event, void *priv)
 {
 	struct connection *connection = priv;
-	struct gdb_service *gdb_service = connection->service->priv;
+	struct target *gdb_target = get_available_target_from_connection(connection);
 
-	if (gdb_service->target != target)
+	if (gdb_target != target)
 		return ERROR_OK;
 
 	switch (event) {
@@ -1503,7 +1526,7 @@ static int gdb_error(struct connection *connection, int retval)
 static int gdb_read_memory_packet(struct connection *connection,
 		char const *packet, int packet_size)
 {
-	struct target *target = get_target_from_connection(connection);
+	struct target *target = get_available_target_from_connection(connection);
 	char *separator;
 	uint64_t addr = 0;
 	uint32_t len = 0;
@@ -1578,7 +1601,7 @@ static int gdb_read_memory_packet(struct connection *connection,
 static int gdb_write_memory_packet(struct connection *connection,
 		char const *packet, int packet_size)
 {
-	struct target *target = get_target_from_connection(connection);
+	struct target *target = get_available_target_from_connection(connection);
 	char *separator;
 	uint64_t addr = 0;
 	uint32_t len = 0;
@@ -1629,7 +1652,7 @@ static int gdb_write_memory_packet(struct connection *connection,
 static int gdb_write_memory_binary_packet(struct connection *connection,
 		char const *packet, int packet_size)
 {
-	struct target *target = get_target_from_connection(connection);
+	struct target *target = get_available_target_from_connection(connection);
 	char *separator;
 	uint64_t addr = 0;
 	uint32_t len = 0;
@@ -1708,7 +1731,7 @@ static int gdb_write_memory_binary_packet(struct connection *connection,
 static int gdb_step_continue_packet(struct connection *connection,
 		char const *packet, int packet_size)
 {
-	struct target *target = get_target_from_connection(connection);
+	struct target *target = get_available_target_from_connection(connection);
 	int current = 0;
 	uint64_t address = 0x0;
 	int retval = ERROR_OK;
@@ -1736,7 +1759,7 @@ static int gdb_step_continue_packet(struct connection *connection,
 static int gdb_breakpoint_watchpoint_packet(struct connection *connection,
 		char const *packet, int packet_size)
 {
-	struct target *target = get_target_from_connection(connection);
+	struct target *target = get_available_target_from_connection(connection);
 	int type;
 	enum breakpoint_type bp_type = BKPT_SOFT /* dummy init to avoid warning */;
 	enum watchpoint_rw wp_type = WPT_READ /* dummy init to avoid warning */;
@@ -1918,7 +1941,7 @@ static int gdb_memory_map(struct connection *connection,
 	 * have to regenerate it a couple of times.
 	 */
 
-	struct target *target = get_target_from_connection(connection);
+	struct target *target = get_available_target_from_connection(connection);
 	struct flash_bank *p;
 	char *xml = NULL;
 	int size = 0;
@@ -2988,7 +3011,7 @@ static int gdb_query_packet(struct connection *connection,
 static bool gdb_handle_vcont_packet(struct connection *connection, const char *packet, int packet_size)
 {
 	struct gdb_connection *gdb_connection = connection->priv;
-	struct target *target = get_target_from_connection(connection);
+	struct target *target = get_available_target_from_connection(connection);
 	const char *parse = packet;
 	int retval;
 
@@ -3227,7 +3250,7 @@ static void gdb_restart_inferior(struct connection *connection, const char *pack
 
 static bool gdb_handle_vrun_packet(struct connection *connection, const char *packet, int packet_size)
 {
-	struct target *target = get_target_from_connection(connection);
+	struct target *target = get_available_target_from_connection(connection);
 	const char *parse = packet;
 
 	/* Skip "vRun" */
@@ -3273,7 +3296,7 @@ static int gdb_v_packet(struct connection *connection,
 	struct gdb_connection *gdb_connection = connection->priv;
 	int result;
 
-	struct target *target = get_target_from_connection(connection);
+	struct target *target = get_available_target_from_connection(connection);
 
 	if (strncmp(packet, "vCont", 5) == 0) {
 		bool handled;
@@ -3444,7 +3467,7 @@ static int gdb_detach(struct connection *connection)
 static int gdb_fileio_response_packet(struct connection *connection,
 		char const *packet, int packet_size)
 {
-	struct target *target = get_target_from_connection(connection);
+	struct target *target = get_available_target_from_connection(connection);
 	char *separator;
 	char *parsing_point;
 	int fileio_retcode = strtoul(packet + 1, &separator, 16);
@@ -3731,10 +3754,11 @@ static int gdb_input_inner(struct connection *connection)
 		}
 
 		if (gdb_con->ctrl_c) {
-			if (target->state == TARGET_RUNNING) {
-				struct target *t = target;
-				if (target->rtos)
-					target->rtos->gdb_target_for_threadid(connection, target->rtos->current_threadid, &t);
+			struct target *available_target = get_available_target_from_connection(connection);
+			if (available_target->state == TARGET_RUNNING) {
+				struct target *t = available_target;
+				if (available_target->rtos)
+					available_target->rtos->gdb_target_for_threadid(connection, target->rtos->current_threadid, &t);
 				retval = target_halt(t);
 				if (retval == ERROR_OK)
 					retval = target_poll(t);
diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
index 9e32b5d..1055ed9 100644
--- a/src/target/breakpoints.c
+++ b/src/target/breakpoints.c
@@ -85,7 +85,7 @@ static int breakpoint_add_internal(struct target *target,
 			reason = "resource not available";
 			goto fail;
 		case ERROR_TARGET_NOT_HALTED:
-			reason = "target running";
+			reason = "target not halted";
 			goto fail;
 		default:
 			reason = "unknown reason";
@@ -222,6 +222,8 @@ int breakpoint_add(struct target *target,
 		struct target_list *list_node;
 		foreach_smp_target(list_node, target->smp_targets) {
 			struct target *curr = list_node->target;
+			if (curr->state == TARGET_UNAVAILABLE)
+				continue;
 			int retval = breakpoint_add_internal(curr, address, length, type);
 			if (retval != ERROR_OK)
 				return retval;
@@ -243,6 +245,8 @@ int context_breakpoint_add(struct target *target,
 
 		foreach_smp_target(head, target->smp_targets) {
 			struct target *curr = head->target;
+			if (curr->state == TARGET_UNAVAILABLE)
+				continue;
 			int retval = context_breakpoint_add_internal(curr, asid, length, type);
 			if (retval != ERROR_OK)
 				return retval;
@@ -265,6 +269,8 @@ int hybrid_breakpoint_add(struct target *target,
 
 		foreach_smp_target(head, target->smp_targets) {
 			struct target *curr = head->target;
+			if (curr->state == TARGET_UNAVAILABLE)
+				continue;
 			int retval = hybrid_breakpoint_add_internal(curr, address, asid, length, type);
 			if (retval != ERROR_OK)
 				return retval;
@@ -441,7 +447,7 @@ int watchpoint_add_internal(struct target *target, target_addr_t address,
 			reason = "resource not available";
 			goto bye;
 		case ERROR_TARGET_NOT_HALTED:
-			reason = "target running";
+			reason = "target not halted";
 			goto bye;
 		default:
 			reason = "unrecognized error";
@@ -473,6 +479,8 @@ int watchpoint_add(struct target *target, target_addr_t address,
 
 		foreach_smp_target(head, target->smp_targets) {
 			struct target *curr = head->target;
+			if (curr->state == TARGET_UNAVAILABLE)
+				continue;
 			int retval = watchpoint_add_internal(curr, address, length, rw, value, mask);
 			if (retval != ERROR_OK)
 				return retval;
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index a205424..4e9152c 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -4281,7 +4281,7 @@ static int riscv013_halt_go(struct target *target)
 		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_ALLRUNNING))
+		if (!get_field(dmstatus, DM_DMSTATUS_ANYRUNNING))
 			break;
 	}
 
@@ -4304,9 +4304,32 @@ static int riscv013_halt_go(struct target *target)
 		target_list_t *entry;
 		list_for_each_entry(entry, &dm->target_list, list) {
 			struct target *t = entry->target;
-			t->state = TARGET_HALTED;
-			if (t->debug_reason == DBG_REASON_NOTHALTED)
-				t->debug_reason = DBG_REASON_DBGRQ;
+			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 (dmi_write(target, DM_DMCONTROL, dmcontrol) != ERROR_OK)
+					return ERROR_FAIL;
+				dm->current_hartid = info->index;
+				if (dmi_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;
+			}
 		}
 	}
 	/* The "else" case is handled in halt_go(). */
@@ -4353,7 +4376,7 @@ static enum riscv_halt_reason riscv013_halt_reason(struct target *target)
 
 	switch (get_field(dcsr, CSR_DCSR_CAUSE)) {
 	case CSR_DCSR_CAUSE_EBREAK:
-		return RISCV_HALT_BREAKPOINT;
+		return RISCV_HALT_EBREAK;
 	case CSR_DCSR_CAUSE_TRIGGER:
 		/* We could get here before triggers are enumerated if a trigger was
 		 * already set when we connected. Force enumeration now, which has the
@@ -4856,6 +4879,8 @@ static int riscv013_step_or_resume_current_hart(struct target *target,
 		usleep(10);
 		if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
 			return ERROR_FAIL;
+		if (get_field(dmstatus, DM_DMSTATUS_ALLUNAVAIL))
+			return ERROR_FAIL;
 		if (get_field(dmstatus, DM_DMSTATUS_ALLRESUMEACK) == 0)
 			continue;
 		if (step && get_field(dmstatus, DM_DMSTATUS_ALLHALTED) == 0)
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index c4809a0..958d112 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -1283,7 +1283,7 @@ int set_debug_reason(struct target *target, enum riscv_halt_reason halt_reason)
 	RISCV_INFO(r);
 	r->trigger_hit = -1;
 	switch (halt_reason) {
-		case RISCV_HALT_BREAKPOINT:
+		case RISCV_HALT_EBREAK:
 			target->debug_reason = DBG_REASON_BREAKPOINT;
 			break;
 		case RISCV_HALT_TRIGGER:
@@ -1604,43 +1604,50 @@ int riscv_resume(
 {
 	LOG_DEBUG("handle_breakpoints=%d", handle_breakpoints);
 	int result = ERROR_OK;
+
+	struct list_head *targets;
+
+	LIST_HEAD(single_target_list);
+	struct target_list single_target_entry = {
+		.lh = {NULL, NULL},
+		.target = target
+	};
+
 	if (target->smp && !single_hart) {
-		struct target_list *tlist;
-		foreach_smp_target_direction(resume_order == RO_NORMAL,
-									 tlist, target->smp_targets) {
-			struct target *t = tlist->target;
+		targets = target->smp_targets;
+	} else {
+		/* Make a list that just contains a single target, so we can
+		 * share code below. */
+		list_add(&single_target_entry.lh, &single_target_list);
+		targets = &single_target_list;
+	}
+
+	struct target_list *tlist;
+	foreach_smp_target_direction(resume_order == RO_NORMAL, tlist, targets) {
+		struct target *t = tlist->target;
+		if (t->state != TARGET_UNAVAILABLE) {
 			if (resume_prep(t, current, address, handle_breakpoints,
 						debug_execution) != ERROR_OK)
 				result = ERROR_FAIL;
 		}
+	}
 
-		foreach_smp_target_direction(resume_order == RO_NORMAL,
-									 tlist, target->smp_targets) {
-			struct target *t = tlist->target;
-			riscv_info_t *i = riscv_info(t);
-			if (i->prepped) {
-				if (resume_go(t, current, address, handle_breakpoints,
-							debug_execution) != ERROR_OK)
-					result = ERROR_FAIL;
-			}
+	foreach_smp_target_direction(resume_order == RO_NORMAL, tlist, targets) {
+		struct target *t = tlist->target;
+		riscv_info_t *i = riscv_info(t);
+		if (i->prepped) {
+			if (resume_go(t, current, address, handle_breakpoints,
+						debug_execution) != ERROR_OK)
+				result = ERROR_FAIL;
 		}
+	}
 
-		foreach_smp_target_direction(resume_order == RO_NORMAL,
-									 tlist, target->smp_targets) {
-			struct target *t = tlist->target;
+	foreach_smp_target_direction(resume_order == RO_NORMAL, tlist, targets) {
+		struct target *t = tlist->target;
+		if (t->state != TARGET_UNAVAILABLE) {
 			if (resume_finish(t, debug_execution) != ERROR_OK)
 				result = ERROR_FAIL;
 		}
-
-	} else {
-		if (resume_prep(target, current, address, handle_breakpoints,
-					debug_execution) != ERROR_OK)
-			result = ERROR_FAIL;
-		if (resume_go(target, current, address, handle_breakpoints,
-					debug_execution) != ERROR_OK)
-			result = ERROR_FAIL;
-		if (resume_finish(target, debug_execution) != ERROR_OK)
-			return ERROR_FAIL;
 	}
 
 	return result;
@@ -2175,44 +2182,126 @@ static int riscv_checksum_memory(struct target *target,
 
 /*** OpenOCD Helper Functions ***/
 
-enum riscv_poll_hart {
-	RPH_NO_CHANGE,
-	RPH_DISCOVERED_HALTED,
-	RPH_DISCOVERED_RUNNING,
-	RPH_ERROR
+enum riscv_next_action {
+	RPH_NONE,
+	RPH_RESUME,
+	RPH_REMAIN_HALTED
 };
-static enum riscv_poll_hart riscv_poll_hart(struct target *target, int hartid)
+static int riscv_poll_hart(struct target *target, enum riscv_next_action *next_action)
 {
 	RISCV_INFO(r);
 
 	LOG_TARGET_DEBUG(target, "polling, target->state=%d", target->state);
 
+	*next_action = RPH_NONE;
+
+	enum riscv_hart_state previous_riscv_state = 0;
+	enum target_state previous_target_state = target->state;
+	switch (target->state) {
+		case TARGET_UNKNOWN:
+			/* Special case, handled further down. */
+			previous_riscv_state = RISCV_STATE_UNAVAILABLE;	/* Need to assign something. */
+			break;
+		case TARGET_RUNNING:
+			previous_riscv_state = RISCV_STATE_RUNNING;
+			break;
+		case TARGET_HALTED:
+			previous_riscv_state = RISCV_STATE_HALTED;
+			break;
+		case TARGET_RESET:
+			previous_riscv_state = RISCV_STATE_HALTED;
+			break;
+		case TARGET_DEBUG_RUNNING:
+			previous_riscv_state = RISCV_STATE_RUNNING;
+			break;
+		case TARGET_UNAVAILABLE:
+			previous_riscv_state = RISCV_STATE_UNAVAILABLE;
+			break;
+	}
+
 	/* If OpenOCD thinks we're running but this hart is halted then it's time
 	 * to raise an event. */
 	enum riscv_hart_state state;
 	if (riscv_get_hart_state(target, &state) != ERROR_OK)
 		return ERROR_FAIL;
-	bool halted = (state == RISCV_STATE_HALTED);
 
-	if (halted && timeval_ms() - r->last_activity > 100) {
+	if (state == RISCV_STATE_NON_EXISTENT) {
+		LOG_TARGET_ERROR(target, "Hart is non-existent!");
+		return ERROR_FAIL;
+	}
+
+	if (state == RISCV_STATE_HALTED && timeval_ms() - r->last_activity > 100) {
 		/* If we've been idle for a while, flush the register cache. Just in case
 		 * OpenOCD is going to be disconnected without shutting down cleanly. */
 		if (riscv_flush_registers(target) != ERROR_OK)
 			return ERROR_FAIL;
 	}
 
-	if (target->state != TARGET_HALTED && halted) {
-		LOG_DEBUG("  triggered a halt");
-		r->on_halt(target);
-		return RPH_DISCOVERED_HALTED;
-	} else if (target->state != TARGET_RUNNING && target->state != TARGET_DEBUG_RUNNING && !halted) {
-		LOG_DEBUG("  triggered running");
-		target->state = TARGET_RUNNING;
-		target->debug_reason = DBG_REASON_NOTHALTED;
-		return RPH_DISCOVERED_RUNNING;
+	if (target->state == TARGET_UNKNOWN || state != previous_riscv_state) {
+		switch (state) {
+			case RISCV_STATE_HALTED:
+				LOG_TARGET_DEBUG(target, "  triggered a halt; previous_target_state=%d",
+					previous_target_state);
+				target->state = TARGET_HALTED;
+				enum riscv_halt_reason halt_reason = riscv_halt_reason(target);
+				if (set_debug_reason(target, halt_reason) != ERROR_OK)
+					return ERROR_FAIL;
+
+				if (halt_reason == RISCV_HALT_EBREAK) {
+					int retval;
+					/* Detect if this EBREAK is a semihosting request. If so, handle it. */
+					switch (riscv_semihosting(target, &retval)) {
+						case SEMI_NONE:
+							break;
+						case SEMI_WAITING:
+							/* This hart should remain halted. */
+							*next_action = RPH_REMAIN_HALTED;
+							break;
+						case SEMI_HANDLED:
+							/* This hart should be resumed, along with any other
+							* harts that halted due to haltgroups. */
+							*next_action = RPH_RESUME;
+							return ERROR_OK;
+						case SEMI_ERROR:
+							return retval;
+					}
+				}
+
+				r->on_halt(target);
+
+				/* We shouldn't do the callbacks yet. What if
+				 * there are multiple harts that halted at the
+				 * same time? We need to set debug reason on each
+				 * of them before calling a callback, which is
+				 * going to figure out the "current thread". */
+
+				r->halted_needs_event_callback = true;
+				if (previous_target_state == TARGET_DEBUG_RUNNING)
+					r->halted_callback_event = TARGET_EVENT_DEBUG_HALTED;
+				else
+					r->halted_callback_event = TARGET_EVENT_HALTED;
+				break;
+
+			case RISCV_STATE_RUNNING:
+				LOG_TARGET_DEBUG(target, "  triggered running");
+				target->state = TARGET_RUNNING;
+				target->debug_reason = DBG_REASON_NOTHALTED;
+				break;
+
+			case RISCV_STATE_UNAVAILABLE:
+				LOG_TARGET_DEBUG(target, "  became unavailable");
+				LOG_TARGET_INFO(target, "became unavailable.");
+				target->state = TARGET_UNAVAILABLE;
+				break;
+
+			case RISCV_STATE_NON_EXISTENT:
+				LOG_TARGET_ERROR(target, "Hart is non-existent!");
+				target->state = TARGET_UNAVAILABLE;
+				break;
+		}
 	}
 
-	return RPH_NO_CHANGE;
+	return ERROR_OK;
 }
 
 int sample_memory(struct target *target)
@@ -2266,120 +2355,99 @@ exit:
 int riscv_openocd_poll(struct target *target)
 {
 	LOG_DEBUG("polling all harts");
-	enum target_state old_state = target->state;
 
-	if (target->smp) {
-		unsigned should_remain_halted = 0;
-		unsigned should_resume = 0;
-		struct target_list *list;
-		foreach_smp_target(list, target->smp_targets) {
-			struct target *t = list->target;
-			if (!target_was_examined(t))
-				continue;
-			enum riscv_poll_hart out = riscv_poll_hart(t, t->coreid);
-			switch (out) {
-			case RPH_NO_CHANGE:
-				break;
-			case RPH_DISCOVERED_RUNNING:
-				t->state = TARGET_RUNNING;
-				t->debug_reason = DBG_REASON_NOTHALTED;
-				break;
-			case RPH_DISCOVERED_HALTED:
-				t->state = TARGET_HALTED;
-				enum riscv_halt_reason halt_reason =
-					riscv_halt_reason(t);
-				if (set_debug_reason(t, halt_reason) != ERROR_OK)
-					return ERROR_FAIL;
+	struct list_head *targets;
 
-				if (halt_reason == RISCV_HALT_BREAKPOINT) {
-					int retval;
-					switch (riscv_semihosting(t, &retval)) {
-					case SEMI_NONE:
-					case SEMI_WAITING:
-						/* This hart should remain halted. */
-						should_remain_halted++;
-						break;
-					case SEMI_HANDLED:
-						/* This hart should be resumed, along with any other
-							 * harts that halted due to haltgroups. */
-						should_resume++;
-						break;
-					case SEMI_ERROR:
-						return retval;
-					}
-				} else if (halt_reason != RISCV_HALT_GROUP) {
-					should_remain_halted++;
-				}
-				break;
+	LIST_HEAD(single_target_list);
+	struct target_list single_target_entry = {
+		.lh = {NULL, NULL},
+		.target = target
+	};
 
-			case RPH_ERROR:
-				return ERROR_FAIL;
-			}
-		}
+	if (target->smp) {
+		targets = target->smp_targets;
+	} else {
+		/* Make a list that just contains a single target, so we can
+		 * share code below. */
+		list_add(&single_target_entry.lh, &single_target_list);
+		targets = &single_target_list;
+	}
+
+	unsigned should_remain_halted = 0;
+	unsigned should_resume = 0;
+	unsigned halted = 0;
+	unsigned running = 0;
+	struct target_list *entry;
+	foreach_smp_target(entry, targets) {
+		struct target *t = entry->target;
+		riscv_info_t *info = riscv_info(t);
+
+		/* Clear here just in case there were errors and we never got to
+		 * check this flag further down. */
+		info->halted_needs_event_callback = false;
+
+		if (!target_was_examined(t))
+			continue;
 
-		LOG_DEBUG("should_remain_halted=%d, should_resume=%d",
-				  should_remain_halted, should_resume);
-		if (should_remain_halted && should_resume) {
-			LOG_WARNING("%d harts should remain halted, and %d should resume.",
-						should_remain_halted, should_resume);
-		}
-		if (should_remain_halted) {
-			LOG_DEBUG("halt all");
-			riscv_halt(target);
-		} else if (should_resume) {
-			LOG_DEBUG("resume all");
-			riscv_resume(target, true, 0, 0, 0, false);
-		}
+		enum riscv_next_action next_action;
+		if (riscv_poll_hart(t, &next_action) != ERROR_OK)
+			return ERROR_FAIL;
 
-		/* Sample memory if any target is running. */
-		foreach_smp_target(list, target->smp_targets) {
-			struct target *t = list->target;
-			if (t->state == TARGET_RUNNING) {
-				sample_memory(target);
+		switch (next_action) {
+			case RPH_NONE:
+				if (t->state == TARGET_HALTED)
+					halted++;
+				if (t->state == TARGET_RUNNING ||
+					t->state == TARGET_DEBUG_RUNNING)
+					running++;
+				break;
+			case RPH_REMAIN_HALTED:
+				should_remain_halted++;
+				break;
+			case RPH_RESUME:
+				should_resume++;
 				break;
-			}
 		}
+	}
 
-		return ERROR_OK;
-
+	LOG_DEBUG("should_remain_halted=%d, should_resume=%d",
+				should_remain_halted, should_resume);
+	if (should_remain_halted && should_resume) {
+		LOG_WARNING("%d harts should remain halted, and %d should resume.",
+					should_remain_halted, should_resume);
+	}
+	if (should_remain_halted) {
+		LOG_TARGET_DEBUG(target, "halt all; should_remain_halted=%d",
+			should_remain_halted);
+		riscv_halt(target);
+	} else if (should_resume) {
+		LOG_DEBUG("resume all");
+		riscv_resume(target, true, 0, 0, 0, false);
+	} else if (halted && running) {
+		LOG_TARGET_DEBUG(target, "halt all; halted=%d",
+			halted);
+		riscv_halt(target);
 	} else {
-		enum riscv_poll_hart out = riscv_poll_hart(target, target->coreid);
-		if (out == RPH_NO_CHANGE || out == RPH_DISCOVERED_RUNNING) {
-			if (target->state == TARGET_RUNNING)
-				sample_memory(target);
-			return ERROR_OK;
-		} else if (out == RPH_ERROR) {
-			return ERROR_FAIL;
+		/* For targets that were discovered to be halted, call the
+		 * appropriate callback. */
+		foreach_smp_target(entry, targets)
+		{
+			struct target *t = entry->target;
+			riscv_info_t *info = riscv_info(t);
+			if (info->halted_needs_event_callback) {
+				target_call_event_callbacks(t, info->halted_callback_event);
+				info->halted_needs_event_callback = false;
+			}
 		}
-
-		LOG_TARGET_DEBUG(target, "hart halted");
-
-		target->state = TARGET_HALTED;
-		enum riscv_halt_reason halt_reason = riscv_halt_reason(target);
-		if (set_debug_reason(target, halt_reason) != ERROR_OK)
-			return ERROR_FAIL;
-		target->state = TARGET_HALTED;
 	}
 
-	if (target->debug_reason == DBG_REASON_BREAKPOINT) {
-		int retval;
-		switch (riscv_semihosting(target, &retval)) {
-			case SEMI_NONE:
-			case SEMI_WAITING:
-				target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-				break;
-			case SEMI_HANDLED:
-				if (riscv_resume(target, true, 0, 0, 0, false) != ERROR_OK)
-					return ERROR_FAIL;
-				break;
-			case SEMI_ERROR:
-				return retval;
+	/* Sample memory if any target is running. */
+	foreach_smp_target(entry, targets) {
+		struct target *t = entry->target;
+		if (t->state == TARGET_RUNNING) {
+			sample_memory(target);
+			break;
 		}
-	} else {
-		if (old_state == TARGET_DEBUG_RUNNING)
-			target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
-		else
-			target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 	}
 
 	return ERROR_OK;
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 8e951d6..657dafb 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -50,7 +50,7 @@ enum riscv_mem_access_method {
 
 enum riscv_halt_reason {
 	RISCV_HALT_INTERRUPT,
-	RISCV_HALT_BREAKPOINT,
+	RISCV_HALT_EBREAK,
 	RISCV_HALT_SINGLESTEP,
 	RISCV_HALT_TRIGGER,
 	RISCV_HALT_UNKNOWN,
@@ -151,6 +151,10 @@ typedef struct {
 	/* This target was selected using hasel. */
 	bool selected;
 
+	/* Used by riscv_openocd_poll(). */
+	bool halted_needs_event_callback;
+	enum target_event halted_callback_event;
+
 	enum riscv_isrmasking_mode isrmask_mode;
 
 	/* Helper functions that target the various RISC-V debug spec