Fix semihosting for multicore targets (#478)

* WIP making semihosting work with -rtos hwthread.

Change-Id: Icb46f3eeedc1391e8fdc73c3ad8036f20267eb2e

* More WIP.

Change-Id: I670a6e1ba2a13a6ef2ae303a99559a16fdd1bbfb

* Fix halting due to a trigger.

Change-Id: Ie7caa8dde9518bcd5440e34cf31ed0d30ebf29ad

* Fix multicore semihosting without halt groups.

Change-Id: I53587e5234308ed2cc30a7132c86e4c94eb176c4

* WIP

Change-Id: I40630543b08d8b533726cb3f63aa60a62be8ef40

* Fix single core semihosting.

This was the last bug!

Change-Id: I593abac027fa9707f48b7f58163d7089574a0e28

* Fix whitespace.

Change-Id: I285c152970b87864c63803fae61312e5b79dfe6d

5 files changed, 132 insertions, 76 deletions

diff --git a/src/rtos/riscv_debug.c b/src/rtos/riscv_debug.c
index 2b6e931..d136530 100644
--- a/src/rtos/riscv_debug.c
+++ b/src/rtos/riscv_debug.c
@@ -267,7 +267,7 @@ static int riscv_gdb_v_packet(struct connection *connection, const char *packet,
 		target_call_event_callbacks(target, TARGET_EVENT_GDB_START);
 		target_call_event_callbacks(target, TARGET_EVENT_RESUME_START);
 		riscv_set_all_rtos_harts(target);
-		riscv_resume(target, 1, 0, 0, 0);
+		riscv_resume(target, 1, 0, 0, 0, false);
 		target->state = TARGET_RUNNING;
 		gdb_set_frontend_state_running(connection);
 		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 36ae829..bbe77e9 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -3645,7 +3645,6 @@ struct target_type riscv013_target = {
 
 	.poll = &riscv_openocd_poll,
 	.halt = &riscv_halt,
-	.resume = &riscv_resume,
 	.step = &riscv_openocd_step,
 
 	.assert_reset = assert_reset,
@@ -4539,7 +4538,7 @@ int riscv013_test_compliance(struct target *target)
 
 	/* resumereq */
 	/* This bit is not actually readable according to the spec, so nothing to check.*/
-	COMPLIANCE_MUST_PASS(riscv_resume(target, true, 0, false, false));
+	COMPLIANCE_MUST_PASS(riscv_resume(target, true, 0, false, false, false));
 
 	/* Halt all harts again so the test can continue.*/
 	COMPLIANCE_MUST_PASS(riscv_halt(target));
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index c939a9c..4c3f8fd 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -1059,11 +1059,13 @@ int halt_prep(struct target *target)
 		if (!riscv_hart_enabled(target, i))
 			continue;
 
-		LOG_DEBUG("prep hart %d", i);
+		LOG_DEBUG("[%s] prep hart, debug_reason=%d", target_name(target),
+				  target->debug_reason);
 		if (riscv_set_current_hartid(target, i) != ERROR_OK)
 			return ERROR_FAIL;
 		if (riscv_is_halted(target)) {
-			LOG_DEBUG("Hart %d is already halted.", i);
+			LOG_DEBUG("Hart %d is already halted (reason=%d).", i,
+					  target->debug_reason);
 		} else {
 			if (r->halt_prep(target) != ERROR_OK)
 				return ERROR_FAIL;
@@ -1305,7 +1307,7 @@ static int resume_finish(struct target *target)
  * @par single_hart When true, only resume a single hart even if SMP is
  * configured.  This is used to run algorithms on just one hart.
  */
-int riscv_resume_internal(
+int riscv_resume(
 		struct target *target,
 		int current,
 		target_addr_t address,
@@ -1353,10 +1355,10 @@ int riscv_resume_internal(
 	return result;
 }
 
-int riscv_resume(struct target *target, int current, target_addr_t address,
+static int riscv_target_resume(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints, int debug_execution)
 {
-	return riscv_resume_internal(target, current, address, handle_breakpoints,
+	return riscv_resume(target, current, address, handle_breakpoints,
 			debug_execution, false);
 }
 
@@ -1718,7 +1720,7 @@ static int riscv_run_algorithm(struct target *target, int num_mem_params,
 
 	/* Run algorithm */
 	LOG_DEBUG("resume at 0x%" TARGET_PRIxADDR, entry_point);
-	if (riscv_resume_internal(target, 0, entry_point, 0, 0, true) != ERROR_OK)
+	if (riscv_resume(target, 0, entry_point, 0, 0, true) != ERROR_OK)
 		return ERROR_FAIL;
 
 	int64_t start = timeval_ms();
@@ -1921,9 +1923,9 @@ static enum riscv_poll_hart riscv_poll_hart(struct target *target, int hartid)
 	return RPH_NO_CHANGE;
 }
 
-int set_debug_reason(struct target *target, int hartid)
+int set_debug_reason(struct target *target, enum riscv_halt_reason halt_reason)
 {
-	switch (riscv_halt_reason(target, hartid)) {
+	switch (halt_reason) {
 		case RISCV_HALT_BREAKPOINT:
 			target->debug_reason = DBG_REASON_BREAKPOINT;
 			break;
@@ -1943,6 +1945,7 @@ int set_debug_reason(struct target *target, int hartid)
 		case RISCV_HALT_ERROR:
 			return ERROR_FAIL;
 	}
+	LOG_DEBUG("[%s] debug_reason=%d", target_name(target), target->debug_reason);
 	return ERROR_OK;
 }
 
@@ -1973,7 +1976,8 @@ int riscv_openocd_poll(struct target *target)
 		LOG_DEBUG("  hart %d halted", halted_hart);
 
 		target->state = TARGET_HALTED;
-		if (set_debug_reason(target, halted_hart) != ERROR_OK)
+		enum riscv_halt_reason halt_reason = riscv_halt_reason(target, halted_hart);
+		if (set_debug_reason(target, halt_reason) != ERROR_OK)
 			return ERROR_FAIL;
 
 		target->rtos->current_threadid = halted_hart + 1;
@@ -1989,45 +1993,75 @@ int riscv_openocd_poll(struct target *target)
 		riscv_halt(target);
 
 	} else if (target->smp) {
-		bool halt_discovered = false;
+		unsigned halts_discovered = 0;
+		unsigned total_targets = 0;
 		bool newly_halted[128] = {0};
+		unsigned should_remain_halted = 0;
+		unsigned should_resume = 0;
 		unsigned i = 0;
 		for (struct target_list *list = target->head; list != NULL;
 				list = list->next, i++) {
+			total_targets++;
 			struct target *t = list->target;
 			riscv_info_t *r = riscv_info(t);
 			assert(i < DIM(newly_halted));
 			enum riscv_poll_hart out = riscv_poll_hart(t, r->current_hartid);
 			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:
-					halt_discovered = true;
-					newly_halted[i] = true;
-					t->state = TARGET_HALTED;
-					if (set_debug_reason(t, r->current_hartid) != ERROR_OK)
-						return ERROR_FAIL;
-					break;
-				case RPH_ERROR:
+			case RPH_NO_CHANGE:
+				if (t->state == TARGET_HALTED)
+					should_remain_halted++;
+				break;
+			case RPH_DISCOVERED_RUNNING:
+				t->state = TARGET_RUNNING;
+				t->debug_reason = DBG_REASON_NOTHALTED;
+				break;
+			case RPH_DISCOVERED_HALTED:
+				halts_discovered++;
+				newly_halted[i] = true;
+				t->state = TARGET_HALTED;
+				enum riscv_halt_reason halt_reason =
+					riscv_halt_reason(t, r->current_hartid);
+				if (set_debug_reason(t, halt_reason) != ERROR_OK)
 					return ERROR_FAIL;
-			}
-		}
 
-		if (halt_discovered) {
-			i = 0;
-			for (struct target_list *list = target->head; list != NULL;
-					list = list->next, i++) {
-				struct target *t = list->target;
-				if (newly_halted[i])
-					target_call_event_callbacks(t, TARGET_EVENT_HALTED);
+				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;
+
+			case RPH_ERROR:
+				return ERROR_FAIL;
 			}
+		}
 
-			LOG_DEBUG("Halt other targets in this SMP group.");
+		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);
 		}
 		return ERROR_OK;
 
@@ -2042,18 +2076,30 @@ int riscv_openocd_poll(struct target *target)
 		halted_hart = riscv_current_hartid(target);
 		LOG_DEBUG("  hart %d halted", halted_hart);
 
-		if (set_debug_reason(target, halted_hart) != ERROR_OK)
+		enum riscv_halt_reason halt_reason = riscv_halt_reason(target, halted_hart);
+		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;
-		if (riscv_semihosting(target, &retval) != 0)
-			return 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;
+		}
+	} else {
+		target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 	}
 
-	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 	return ERROR_OK;
 }
 
@@ -2777,7 +2823,7 @@ struct target_type riscv_target = {
 	.poll = old_or_new_riscv_poll,
 
 	.halt = riscv_halt,
-	.resume = riscv_resume,
+	.resume = riscv_target_resume,
 	.step = old_or_new_riscv_step,
 
 	.assert_reset = riscv_assert_reset,
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index e3bd635..d446aa8 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -239,7 +239,8 @@ int riscv_resume(
 	int current,
 	target_addr_t address,
 	int handle_breakpoints,
-	int debug_execution
+	int debug_execution,
+	bool single_hart
 );
 
 int riscv_openocd_step(
@@ -337,7 +338,13 @@ int riscv_hit_watchpoint(struct target *target, struct watchpoint **hit_wp_addre
 int riscv_init_registers(struct target *target);
 
 void riscv_semihosting_init(struct target *target);
-int riscv_semihosting(struct target *target, int *retval);
+typedef enum {
+	SEMI_NONE,		/* Not halted for a semihosting call. */
+	SEMI_HANDLED,	/* Call handled, and target was resumed. */
+	SEMI_WAITING,	/* Call handled, target is halted waiting until we can resume. */
+	SEMI_ERROR		/* Something went wrong. */
+} semihosting_result_t;
+semihosting_result_t riscv_semihosting(struct target *target, int *retval);
 
 void riscv_add_bscan_tunneled_scan(struct target *target, struct scan_field *field,
 		riscv_bscan_tunneled_scan_context_t *ctxt);
diff --git a/src/target/riscv/riscv_semihosting.c b/src/target/riscv/riscv_semihosting.c
index c4b6653..ece29ce 100644
--- a/src/target/riscv/riscv_semihosting.c
+++ b/src/target/riscv/riscv_semihosting.c
@@ -60,35 +60,35 @@ void riscv_semihosting_init(struct target *target)
 /**
  * Check for and process a semihosting request using the ARM protocol). This
  * is meant to be called when the target is stopped due to a debug mode entry.
- * If the value 0 is returned then there was nothing to process. A non-zero
- * return value signifies that a request was processed and the target resumed,
- * or an error was encountered, in which case the caller must return
- * immediately.
  *
  * @param target Pointer to the target to process.
  * @param retval Pointer to a location where the return code will be stored
  * @return non-zero value if a request was processed or an error encountered
  */
-int riscv_semihosting(struct target *target, int *retval)
+semihosting_result_t riscv_semihosting(struct target *target, int *retval)
 {
 	struct semihosting *semihosting = target->semihosting;
-	if (!semihosting)
-		return 0;
+	if (!semihosting) {
+		LOG_DEBUG("   -> NONE (!semihosting)");
+		return SEMI_NONE;
+	}
 
-	if (!semihosting->is_active)
-		return 0;
+	if (!semihosting->is_active) {
+		LOG_DEBUG("   -> NONE (!semihosting->is_active)");
+		return SEMI_NONE;
+	}
 
-	riscv_reg_t dpc;
-	int result = riscv_get_register(target, &dpc, GDB_REGNO_DPC);
+	riscv_reg_t pc;
+	int result = riscv_get_register(target, &pc, GDB_REGNO_PC);
 	if (result != ERROR_OK)
-		return 0;
+		return SEMI_ERROR;
 
 	uint8_t tmp[12];
 
 	/* Read the current instruction, including the bracketing */
-	*retval = target_read_memory(target, dpc - 4, 2, 6, tmp);
+	*retval = target_read_memory(target, pc - 4, 2, 6, tmp);
 	if (*retval != ERROR_OK)
-		return 0;
+		return SEMI_ERROR;
 
 	/*
 	 * The instructions that trigger a semihosting call,
@@ -101,12 +101,12 @@ int riscv_semihosting(struct target *target, int *retval)
 	uint32_t pre = target_buffer_get_u32(target, tmp);
 	uint32_t ebreak = target_buffer_get_u32(target, tmp + 4);
 	uint32_t post = target_buffer_get_u32(target, tmp + 8);
-	LOG_DEBUG("check %08x %08x %08x from 0x%" PRIx64 "-4", pre, ebreak, post, dpc);
+	LOG_DEBUG("check %08x %08x %08x from 0x%" PRIx64 "-4", pre, ebreak, post, pc);
 
 	if (pre != 0x01f01013 || ebreak != 0x00100073 || post != 0x40705013) {
-
 		/* Not the magic sequence defining semihosting. */
-		return 0;
+		LOG_DEBUG("   -> NONE (no magic)");
+		return SEMI_NONE;
 	}
 
 	/*
@@ -114,18 +114,21 @@ int riscv_semihosting(struct target *target, int *retval)
 	 * operation to complete.
 	 */
 	if (!semihosting->hit_fileio) {
-
 		/* RISC-V uses A0 and A1 to pass function arguments */
 		riscv_reg_t r0;
 		riscv_reg_t r1;
 
 		result = riscv_get_register(target, &r0, GDB_REGNO_A0);
-		if (result != ERROR_OK)
-			return 0;
+		if (result != ERROR_OK) {
+			LOG_DEBUG("   -> ERROR (couldn't read a0)");
+			return SEMI_ERROR;
+		}
 
 		result = riscv_get_register(target, &r1, GDB_REGNO_A1);
-		if (result != ERROR_OK)
-			return 0;
+		if (result != ERROR_OK) {
+			LOG_DEBUG("   -> ERROR (couldn't read a1)");
+			return SEMI_ERROR;
+		}
 
 		semihosting->op = r0;
 		semihosting->param = r1;
@@ -136,11 +139,12 @@ int riscv_semihosting(struct target *target, int *retval)
 			*retval = semihosting_common(target);
 			if (*retval != ERROR_OK) {
 				LOG_ERROR("Failed semihosting operation");
-				return 0;
+				return SEMI_ERROR;
 			}
 		} else {
 			/* Unknown operation number, not a semihosting call. */
-			return 0;
+			LOG_DEBUG("   -> NONE (unknown operation number)");
+			return SEMI_NONE;
 		}
 	}
 
@@ -150,16 +154,16 @@ int riscv_semihosting(struct target *target, int *retval)
 	 */
 	if (semihosting->is_resumable && !semihosting->hit_fileio) {
 		/* Resume right after the EBREAK 4 bytes instruction. */
-		*retval = target_resume(target, 0, dpc+4, 0, 0);
-		if (*retval != ERROR_OK) {
-			LOG_ERROR("Failed to resume target");
-			return 0;
-		}
+		*retval = riscv_set_register(target, GDB_REGNO_PC, pc + 4);
+		if (*retval != ERROR_OK)
+			return SEMI_ERROR;
 
-		return 1;
+		LOG_DEBUG("   -> HANDLED");
+		return SEMI_HANDLED;
 	}
 
-	return 0;
+	LOG_DEBUG("   -> WAITING");
+	return SEMI_WAITING;
 }
 
 /* -------------------------------------------------------------------------
@@ -171,7 +175,7 @@ int riscv_semihosting(struct target *target, int *retval)
  */
 static int riscv_semihosting_setup(struct target *target, int enable)
 {
-	LOG_DEBUG("enable=%d", enable);
+	LOG_DEBUG("[%s] enable=%d", target_name(target), enable);
 
 	struct semihosting *semihosting = target->semihosting;
 	if (semihosting)