From f6ffede8b62e7092141d54e75238fde1ee00d0c2 Mon Sep 17 00:00:00 2001
From: Greg Savin <greg.savin@sifive.com>
Date: Mon, 7 Feb 2022 09:28:31 -0800
Subject: fix missing thread ID in stop reply when smp-configured hart (but not
 hart 0) single-stepped (#675)

---
 src/rtos/rtos.c         | 24 ++++++++++++++++++++++--
 src/rtos/rtos.h         |  1 +
 src/server/gdb_server.c | 13 ++++++++-----
 3 files changed, 31 insertions(+), 7 deletions(-)

(limited to 'src')

diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index 8141658..b9adf0a 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -22,6 +22,7 @@
 
 #include "rtos.h"
 #include "target/target.h"
+#include "target/smp.h"
 #include "helper/log.h"
 #include "helper/binarybuffer.h"
 #include "server/gdb_server.h"
@@ -789,10 +790,29 @@ static int rtos_try_next(struct target *target)
 	return 1;
 }
 
-int rtos_update_threads(struct target *target)
+struct rtos *rtos_of_target(struct target *target)
 {
+	/* Primarily consider the rtos field of the target itself, secondarily consider
+	 * rtos field SMP leader target, then consider rtos field of any other target in the SMP group.
+	 * Otherwise NULL return means that no associated non-zero rtos field could be found. */
+
+	struct target_list *pos;
+
 	if ((target->rtos) && (target->rtos->type))
-		target->rtos->type->update_threads(target->rtos);
+		return target->rtos;
+
+	foreach_smp_target(pos, target->head)
+		if ((pos->target->rtos) && (pos->target->rtos->type))
+			return pos->target->rtos;
+
+	return NULL;
+}
+
+int rtos_update_threads(struct target *target)
+{
+	struct rtos *rtos = rtos_of_target(target);
+	if (rtos)
+		rtos->type->update_threads(rtos);
 	return ERROR_OK;
 }
 
diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index 0cad6ee..828fd26 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -175,5 +175,6 @@ int rtos_write_buffer(struct target *target, target_addr_t address,
 		uint32_t size, const uint8_t *buffer);
 struct target *rtos_swbp_target(struct target *target, target_addr_t address,
 				uint32_t length, enum breakpoint_type type);
+struct rtos *rtos_of_target(struct target *target);
 
 #endif /* OPENOCD_RTOS_RTOS_H */
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 0269e58..20cf670 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -778,9 +778,12 @@ static void gdb_signal_reply(struct target *target, struct connection *connectio
 		sig_reply_len = snprintf(sig_reply, sizeof(sig_reply), "W00");
 	} else {
 		struct target *ct;
-		if (target->rtos) {
-			target->rtos->current_threadid = target->rtos->current_thread;
-			target->rtos->gdb_target_for_threadid(connection, target->rtos->current_threadid, &ct);
+		struct rtos *rtos;
+
+		rtos = rtos_of_target(target);
+		if (rtos) {
+			rtos->current_threadid = rtos->current_thread;
+			rtos->gdb_target_for_threadid(connection, rtos->current_threadid, &ct);
 		} else {
 			ct = target;
 		}
@@ -817,9 +820,9 @@ static void gdb_signal_reply(struct target *target, struct connection *connectio
 		}
 
 		current_thread[0] = '\0';
-		if (target->rtos)
+		if (rtos)
 			snprintf(current_thread, sizeof(current_thread), "thread:%" PRIx64 ";",
-					target->rtos->current_thread);
+					rtos->current_thread);
 
 		sig_reply_len = snprintf(sig_reply, sizeof(sig_reply), "T%2.2x%s%s",
 				signal_var, stop_reason, current_thread);
-- 
cgit v1.1