diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 4536e5c68..67dfb9c28 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -1785,7 +1785,13 @@ static int gdb_breakpoint_watchpoint_packet(struct connection *connection,
 		case 0:
 		case 1:
 			if (packet[0] == 'Z') {
-				retval = breakpoint_add(target, address, size, bp_type);
+				struct target *bp_target = target;
+				if (target->rtos && bp_type == BKPT_SOFT) {
+					bp_target = rtos_swbp_target(target, address, size, bp_type);
+					if (!bp_target)
+						return ERROR_FAIL;
+				}
+				retval = breakpoint_add(bp_target, address, size, bp_type);
 				if (retval != ERROR_OK) {
 					retval = gdb_error(connection, retval);
 					if (retval != ERROR_OK)
diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
index 3bfdbde91..9e32b5df2 100644
--- a/src/target/breakpoints.c
+++ b/src/target/breakpoints.c
@@ -218,19 +218,10 @@ int breakpoint_add(struct target *target,
 	uint32_t length,
 	enum breakpoint_type type)
 {
-	if (target->smp) {
-		struct target_list *head;
-
-		if (type == BKPT_SOFT) {
-			head = list_first_entry(target->smp_targets, struct target_list, lh);
-			struct target *curr = head->target;
-			if (target->rtos)
-				curr = rtos_swbp_target(target, address, length, type);
-			return breakpoint_add_internal(curr, address, length, type);
-		}
-
-		foreach_smp_target(head, target->smp_targets) {
-			struct target *curr = head->target;
+	if (target->smp && type == BKPT_HARD) {
+		struct target_list *list_node;
+		foreach_smp_target(list_node, target->smp_targets) {
+			struct target *curr = list_node->target;
 			int retval = breakpoint_add_internal(curr, address, length, type);
 			if (retval != ERROR_OK)
 				return retval;