4 files changed, 35 insertions, 6 deletions

diff --git a/src/rtos/hwthread.c b/src/rtos/hwthread.c
index a8f1b59..4b8e22b 100644
--- a/src/rtos/hwthread.c
+++ b/src/rtos/hwthread.c
@@ -43,6 +43,8 @@ static int hwthread_read_buffer(struct rtos *rtos, target_addr_t address,
 		uint32_t size, uint8_t *buffer);
 static int hwthread_write_buffer(struct rtos *rtos, target_addr_t address,
 		uint32_t size, const uint8_t *buffer);
+struct target *hwthread_swbp_target(struct rtos *rtos, target_addr_t address,
+				    uint32_t length, enum breakpoint_type type);
 
 #define HW_THREAD_NAME_STR_SIZE (32)
 
@@ -66,6 +68,7 @@ const struct rtos_type hwthread_rtos = {
 	.needs_fake_step = hwthread_needs_fake_step,
 	.read_buffer = hwthread_read_buffer,
 	.write_buffer = hwthread_write_buffer,
+	.swbp_target = hwthread_swbp_target
 };
 
 struct hwthread_params {
@@ -444,3 +447,9 @@ static int hwthread_write_buffer(struct rtos *rtos, target_addr_t address,
 
 	return target_write_buffer(curr, address, size, buffer);
 }
+
+struct target *hwthread_swbp_target(struct rtos *rtos, target_addr_t address,
+				    uint32_t length, enum breakpoint_type type)
+{
+	return hwthread_find_thread(rtos->target, rtos->current_thread);
+}
diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index fafbda2..8141658 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -836,3 +836,11 @@ int rtos_write_buffer(struct target *target, target_addr_t address,
 		return target->rtos->type->write_buffer(target->rtos, address, size, buffer);
 	return ERROR_NOT_IMPLEMENTED;
 }
+
+struct target *rtos_swbp_target(struct target *target, target_addr_t address,
+				uint32_t length, enum breakpoint_type type)
+{
+	if (target->rtos->type->swbp_target)
+		return target->rtos->type->swbp_target(target->rtos, address, length, type);
+	return target;
+}
diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index 2e41c14..0cad6ee 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -20,6 +20,7 @@
 #define OPENOCD_RTOS_RTOS_H
 
 #include "server/server.h"
+#include "target/breakpoints.h"
 #include "target/target.h"
 #include <helper/jim-nvp.h>
 
@@ -103,6 +104,12 @@ struct rtos_type {
 			uint8_t *buffer);
 	int (*write_buffer)(struct rtos *rtos, target_addr_t address, uint32_t size,
 			const uint8_t *buffer);
+	/* When a software breakpoint is set, it is set on only one target,
+	 * because we assume memory is shared across them. By default this is the
+	 * first target in the SMP group. Override this function to have
+	 * breakpoint_add() use a different target. */
+	struct target * (*swbp_target)(struct rtos *rtos, target_addr_t address,
+				     uint32_t length, enum breakpoint_type type);
 };
 
 struct stack_register_offset {
@@ -166,5 +173,7 @@ int rtos_read_buffer(struct target *target, target_addr_t address,
 		uint32_t size, uint8_t *buffer);
 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);
 
 #endif /* OPENOCD_RTOS_RTOS_H */
diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
index dd901ef..938a7ec 100644
--- a/src/target/breakpoints.c
+++ b/src/target/breakpoints.c
@@ -26,6 +26,7 @@
 #include "target.h"
 #include <helper/log.h>
 #include "breakpoints.h"
+#include "rtos/rtos.h"
 
 static const char * const breakpoint_type_strings[] = {
 	"hardware",
@@ -218,14 +219,16 @@ int breakpoint_add(struct target *target,
 {
 	int retval = ERROR_OK;
 	if (target->smp) {
-		struct target_list *head;
-		struct target *curr;
-		head = target->head;
-		if (type == BKPT_SOFT)
-			return breakpoint_add_internal(head->target, address, length, type);
+		struct target_list *head = target->head;
+		if (type == BKPT_SOFT) {
+			struct target *curr = head->target;
+			if (target->rtos)
+				curr = rtos_swbp_target(target, address, length, type);
+			return breakpoint_add_internal(curr, address, length, type);
+		}
 
 		while (head) {
-			curr = head->target;
+			struct target *curr = head->target;
 			retval = breakpoint_add_internal(curr, address, length, type);
 			if (retval != ERROR_OK)
 				return retval;