target/aarch64: fix semihosting on SMP

Semihosting worked only on the first/gdb assigned core of a SMP group.
If a semihosting call was issued on another core, aarch64_update_halt_gdb()
emitted 'halted' event on core0 before semihosting decoding started.

Use target's smp_halt_event_postponed flag to keep events from emitting
until semihosting is decoded. If a semihosting call is confirmed,
clear flags and do not send 'halted' event for any core of SMP group.

Change-Id: Ie7eff7e493c2a4df3039f49fce1744d996050a59
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: https://review.openocd.org/c/openocd/+/9246
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>
Tested-by: jenkins

src/target/aarch64.c

@@ -39,7 +39,8 @@ struct aarch64_private_config {
 	struct arm_cti *cti;
 };
 
-static int aarch64_poll_smp(struct target *target, bool smp);
+static int aarch64_poll_smp(struct target *target, bool smp,
+							bool postpone_event);
 static int aarch64_debug_entry(struct target *target);
 static int aarch64_restore_context(struct target *target, bool bpwp);
 static int aarch64_set_breakpoint(struct target *target,
@@ -471,7 +472,6 @@ static int aarch64_halt_smp(struct target *target, bool exc_target)
 
 static int aarch64_update_halt_gdb(struct target *target, enum target_debug_reason debug_reason)
 {
-	struct target *gdb_target = NULL;
 	struct target_list *head;
 	struct target *curr;
 
@@ -480,7 +480,7 @@ static int aarch64_update_halt_gdb(struct target *target, enum target_debug_reas
 		aarch64_halt_smp(target, true);
 	}
 
-	/* poll all targets in the group, but skip the target that serves GDB */
+	/* poll all targets in the group */
 	foreach_smp_target(head, target->smp_targets) {
 		curr = head->target;
 		/* skip calling context */
@@ -491,31 +491,43 @@ static int aarch64_update_halt_gdb(struct target *target, enum target_debug_reas
 		/* skip targets that were already halted */
 		if (curr->state == TARGET_HALTED)
 			continue;
-		/* remember the gdb_service->target */
-		if (curr->gdb_service)
-			gdb_target = curr->gdb_service->target;
-		/* skip it */
-		if (curr == gdb_target)
-			continue;
 
 		const bool smp = false;
-		aarch64_poll_smp(curr, smp);
+		const bool postpone_event = true;
-	}
+		aarch64_poll_smp(curr, smp, postpone_event);
-
-	/* after all targets were updated, poll the gdb serving target */
-	if (gdb_target && gdb_target != target) {
-		const bool smp = false;
-		aarch64_poll_smp(gdb_target, smp);
 	}
 
 	return ERROR_OK;
 }
 
+enum postponed_halt_events_op {
+	POSTPONED_HALT_EVENT_CLEAR,
+	POSTPONED_HALT_EVENT_EMIT
+};
+
+static void aarch64_smp_postponed_halt_events(struct list_head *smp_targets,
+											enum postponed_halt_events_op op)
+{
+	struct target_list *head;
+	foreach_smp_target(head, smp_targets) {
+		struct target *t = head->target;
+		if (!t->smp_halt_event_postponed)
+			continue;
+
+		if (op == POSTPONED_HALT_EVENT_EMIT) {
+			LOG_TARGET_DEBUG(t, "sending postponed target event 'halted'");
+			target_call_event_callbacks(t, TARGET_EVENT_HALTED);
+		}
+		t->smp_halt_event_postponed = false;
+	}
+}
+
 /*
  * Aarch64 Run control
  */
 
-static int aarch64_poll_smp(struct target *target, bool smp)
+static int aarch64_poll_smp(struct target *target, bool smp,
+							bool postpone_event)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 	enum target_state prev_target_state;
@@ -553,13 +565,21 @@ static int aarch64_poll_smp(struct target *target, bool smp)
 			if (smp)
 				aarch64_update_halt_gdb(target, debug_reason);
 
-			if (arm_semihosting(target, &retval) != 0)
+			if (arm_semihosting(target, &retval) != 0) {
+				if (smp)
+					aarch64_smp_postponed_halt_events(target->smp_targets,
+													POSTPONED_HALT_EVENT_CLEAR);
+
 				return retval;
+			}
 
 			switch (prev_target_state) {
 			case TARGET_RUNNING:
 			case TARGET_UNKNOWN:
 			case TARGET_RESET:
+				if (postpone_event)
+					target->smp_halt_event_postponed = true;
+				else
 					target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 				break;
 			case TARGET_DEBUG_RUNNING:
@@ -568,6 +588,10 @@ static int aarch64_poll_smp(struct target *target, bool smp)
 			default:
 				break;
 			}
+
+			if (smp)
+				aarch64_smp_postponed_halt_events(target->smp_targets,
+												POSTPONED_HALT_EVENT_EMIT);
 		}
 	} else if (prsr & PRSR_RESET) {
 		target->state = TARGET_RESET;
@@ -580,7 +604,8 @@ static int aarch64_poll_smp(struct target *target, bool smp)
 
 static int aarch64_poll(struct target *target)
 {
-	return aarch64_poll_smp(target, target->smp != 0);
+	const bool postpone_event = false;
+	return aarch64_poll_smp(target, target->smp != 0, postpone_event);
 }
 
 static int aarch64_halt(struct target *target)
@@ -866,7 +891,7 @@ static int aarch64_step_restart_smp(struct target *target)
 		retval = aarch64_do_restart_one(curr, RESTART_LAZY);
 		if (retval != ERROR_OK)
 			break;
-}
+	}
 
 	return retval;
 }