WIP on hardware breakpoints.

This is messy, but contains at least some bugfixes.

39/43 tests pass now.

Change-Id: Ic9e8dad2a0ceb237e28c93906d1cd60876a5766d

8 files changed, 207 insertions, 61 deletions

diff --git a/src/rtos/hwthread.c b/src/rtos/hwthread.c
index bfd299a..caa3f72 100644
--- a/src/rtos/hwthread.c
+++ b/src/rtos/hwthread.c
@@ -91,8 +91,11 @@ static int hwthread_update_threads(struct rtos *rtos)
 	int64_t current_thread = 0;
 	enum target_debug_reason current_reason = DBG_REASON_UNDEFINED;
 
+	LOG_DEBUG("current_thread=%i", (int)rtos->current_thread);
+	LOG_DEBUG("current_threadid=%i", (int)rtos->current_threadid);
+
 	if (rtos == NULL)
-		return -1;
+		return ERROR_FAIL;
 
 	target = rtos->target;
 
@@ -109,6 +112,7 @@ static int hwthread_update_threads(struct rtos *rtos)
 	} else
 		thread_list_size = 1;
 
+#if 0
 	if (thread_list_size == rtos->thread_count) {
 		/* Nothing changed. Exit early.
 		 * This is important because if we do recreate the data, we potentially
@@ -122,9 +126,12 @@ static int hwthread_update_threads(struct rtos *rtos)
 		 */
 		return ERROR_OK;
 	}
+#endif
 
-	/* wipe out previous thread details if any */
+	/* Wipe out previous thread details if any, but preserve threadid. */
+	int64_t current_threadid = rtos->current_threadid;
 	rtos_free_threadlist(rtos);
+	rtos->current_threadid = current_threadid;
 
 	/* create space for new thread details */
 	rtos->thread_details = malloc(sizeof(struct thread_detail) * thread_list_size);
@@ -194,6 +201,8 @@ static int hwthread_update_threads(struct rtos *rtos)
 			}
 
 			threads_found++;
+			LOG_DEBUG(">>> tid=%ld, debug_reason=%d, current_thread=%ld, current_reason=%d",
+					tid, curr->debug_reason, current_thread, current_reason);
 		}
 	} else {
 		hwthread_fill_thread(rtos, target, threads_found);
@@ -211,7 +220,8 @@ static int hwthread_update_threads(struct rtos *rtos)
 	else
 		rtos->current_thread = threadid_from_target(target);
 
-	LOG_DEBUG("%s current_thread=%i", __func__, (int)rtos->current_thread);
+	LOG_DEBUG("current_thread=%i", (int)rtos->current_thread);
+	LOG_DEBUG("current_threadid=%i", (int)rtos->current_threadid);
 	return 0;
 }
 
@@ -358,7 +368,10 @@ static int hwthread_thread_packet(struct connection *connection, const char *pac
 	struct target *curr = NULL;
 	int64_t current_threadid;
 
+	LOG_DEBUG(">>> %s", packet);
+
 	if (packet[0] == 'H' && packet[1] == 'g') {
+		/* Never reached, because this case is handled by rtos_thread_packet(). */
 		sscanf(packet, "Hg%16" SCNx64, &current_threadid);
 
 		if (current_threadid > 0) {
@@ -373,6 +386,7 @@ static int hwthread_thread_packet(struct connection *connection, const char *pac
 			target->rtos->current_thread = threadid_from_target(target);
 
 		target->rtos->current_threadid = current_threadid;
+		LOG_DEBUG("current_threadid=%ld", current_threadid);
 
 		gdb_put_packet(connection, "OK", 2);
 		return ERROR_OK;
diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index 5496008..a5b7fdb 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -432,6 +432,7 @@ int rtos_thread_packet(struct connection *connection, char const *packet, int pa
 				target->rtos->current_threadid = target->rtos->current_thread;
 			else
 				target->rtos->current_threadid = threadid;
+			LOG_DEBUG("current_threadid=%ld", target->rtos->current_threadid);
 		}
 		gdb_put_packet(connection, "OK", 2);
 		return ERROR_OK;
@@ -516,6 +517,7 @@ int rtos_get_gdb_reg_list(struct connection *connection)
 {
 	struct target *target = get_target_from_connection(connection);
 	int64_t current_threadid = target->rtos->current_threadid;
+	LOG_DEBUG("current_threadid=%ld", target->rtos->current_threadid);
 	if ((target->rtos != NULL) && (current_threadid != -1) &&
 			(current_threadid != 0) &&
 			((current_threadid != target->rtos->current_thread) ||
diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index e5a7e13..001837f 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -50,7 +50,9 @@ struct rtos {
 	symbol_table_elem_t *symbols;
 	struct target *target;
 	/*  add a context variable instead of global variable */
+	/* The thread currently selected by gdb. */
 	int64_t current_threadid;
+	/* The currently selected thread according to the target. */
 	threadid_t current_thread;
 	struct thread_detail *thread_details;
 	int thread_count;
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 4631ffb..2772e7a 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -144,6 +144,7 @@ static char gdb_running_type;
 
 static int gdb_last_signal(struct target *target)
 {
+	LOG_DEBUG(">>> [%d] debug_reason=%d", target->coreid, target->debug_reason);
 	switch (target->debug_reason) {
 		case DBG_REASON_DBGRQ:
 			return 0x2;		/* SIGINT */
@@ -733,17 +734,28 @@ static void gdb_signal_reply(struct target *target, struct connection *connectio
 	if (target->debug_reason == DBG_REASON_EXIT) {
 		sig_reply_len = snprintf(sig_reply, sizeof(sig_reply), "W00");
 	} else {
+		struct target *ct;
+		if (target->rtos != NULL) {
+			target->rtos->current_threadid = target->rtos->current_thread;
+			LOG_DEBUG("current_threadid=%ld", target->rtos->current_threadid);
+			target->rtos->gdb_target_for_threadid(connection, target->rtos->current_threadid, &ct);
+		} else {
+			ct = target;
+		}
+
 		if (gdb_connection->ctrl_c) {
 			signal_var = 0x2;
 		} else
-			signal_var = gdb_last_signal(target);
+			signal_var = gdb_last_signal(ct);
+		LOG_DEBUG(">>> ctrl_c=%d, signal_var=%d", gdb_connection->ctrl_c,
+				signal_var);
 
 		stop_reason[0] = '\0';
-		if (target->debug_reason == DBG_REASON_WATCHPOINT) {
+		if (ct->debug_reason == DBG_REASON_WATCHPOINT) {
 			enum watchpoint_rw hit_wp_type;
 			target_addr_t hit_wp_address;
 
-			if (watchpoint_hit(target, &hit_wp_type, &hit_wp_address) == ERROR_OK) {
+			if (watchpoint_hit(ct, &hit_wp_type, &hit_wp_address) == ERROR_OK) {
 
 				switch (hit_wp_type) {
 					case WPT_WRITE:
@@ -765,15 +777,10 @@ static void gdb_signal_reply(struct target *target, struct connection *connectio
 		}
 
 		current_thread[0] = '\0';
-		if (target->rtos != NULL) {
-			struct target *ct;
+		if (target->rtos != NULL)
 			snprintf(current_thread, sizeof(current_thread), "thread:%" PRIx64 ";",
 					target->rtos->current_thread);
-			target->rtos->current_threadid = target->rtos->current_thread;
-			target->rtos->gdb_target_for_threadid(connection, target->rtos->current_threadid, &ct);
-			if (!gdb_connection->ctrl_c)
-				signal_var = gdb_last_signal(ct);
-		}
+		LOG_DEBUG(">>> signal_var=%d", signal_var);
 
 		sig_reply_len = snprintf(sig_reply, sizeof(sig_reply), "T%2.2x%s%s",
 				signal_var, stop_reason, current_thread);
@@ -1652,7 +1659,7 @@ static int gdb_breakpoint_watchpoint_packet(struct connection *connection,
 	char *separator;
 	int retval;
 
-	LOG_DEBUG("-");
+	LOG_DEBUG("[%d]", target->coreid);
 
 	type = strtoul(packet + 1, &separator, 16);
 
@@ -3178,6 +3185,7 @@ static int gdb_input_inner(struct connection *connection)
 		}
 
 		if (packet_size > 0) {
+			LOG_DEBUG(">>> current_threadid=%ld", target->rtos ? target->rtos->current_threadid : 1234);
 			retval = ERROR_OK;
 			switch (packet[0]) {
 				case 'T':	/* Is thread alive? */
diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
index 58bcc86..9645f84 100644
--- a/src/target/breakpoints.c
+++ b/src/target/breakpoints.c
@@ -98,7 +98,9 @@ fail:
 			return retval;
 	}
 
-	LOG_DEBUG("added %s breakpoint at " TARGET_ADDR_FMT " of length 0x%8.8x, (BPID: %" PRIu32 ")",
+	LOG_DEBUG("[%d] added %s breakpoint at " TARGET_ADDR_FMT
+			" of length 0x%8.8x, (BPID: %" PRIu32 ")",
+		target->coreid,
 		breakpoint_type_strings[(*breakpoint_p)->type],
 		(*breakpoint_p)->address, (*breakpoint_p)->length,
 		(*breakpoint_p)->unique_id);
@@ -385,8 +387,8 @@ struct breakpoint *breakpoint_find(struct target *target, target_addr_t address)
 	return NULL;
 }
 
-int watchpoint_add(struct target *target, target_addr_t address, uint32_t length,
-	enum watchpoint_rw rw, uint32_t value, uint32_t mask)
+int watchpoint_add_internal(struct target *target, target_addr_t address,
+		uint32_t length, enum watchpoint_rw rw, uint32_t value, uint32_t mask)
 {
 	struct watchpoint *watchpoint = target->watchpoints;
 	struct watchpoint **watchpoint_p = &target->watchpoints;
@@ -451,6 +453,29 @@ bye:
 	return ERROR_OK;
 }
 
+int watchpoint_add(struct target *target, target_addr_t address,
+		uint32_t length, enum watchpoint_rw rw, uint32_t value, uint32_t mask)
+{
+	int retval = ERROR_OK;
+	if (target->smp) {
+		struct target_list *head;
+		struct target *curr;
+		head = target->head;
+
+		while (head != (struct target_list *)NULL) {
+			curr = head->target;
+			retval = watchpoint_add_internal(curr, address, length, rw, value,
+					mask);
+			if (retval != ERROR_OK)
+				return retval;
+			head = head->next;
+		}
+		return retval;
+	} else
+		return watchpoint_add_internal(target, address, length, rw, value,
+				mask);
+}
+
 static void watchpoint_free(struct target *target, struct watchpoint *watchpoint_to_remove)
 {
 	struct watchpoint *watchpoint = target->watchpoints;
@@ -502,6 +527,8 @@ int watchpoint_hit(struct target *target, enum watchpoint_rw *rw,
 	int retval;
 	struct watchpoint *hit_watchpoint;
 
+	LOG_DEBUG("[%d]", target->coreid);
+
 	retval = target_hit_watchpoint(target, &hit_watchpoint);
 	if (retval != ERROR_OK)
 		return ERROR_FAIL;
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 0d74d5c..2a7917d 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -2952,6 +2952,7 @@ static enum riscv_halt_reason riscv013_halt_reason(struct target *target)
 		 * already set when we connected. Force enumeration now, which has the
 		 * side effect of clearing any triggers we did not set. */
 		riscv_enumerate_triggers(target);
+		LOG_DEBUG("[%d] halted because of trigger", target->coreid);
 		return RISCV_HALT_TRIGGER;
 	case CSR_DCSR_CAUSE_STEP:
 		return RISCV_HALT_SINGLESTEP;
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 4ef417e..aa871e9 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -267,6 +267,7 @@ static int riscv_init_target(struct command_context *cmd_ctx,
 	riscv_semihosting_init(target);
 
 	target->debug_reason = DBG_REASON_DBGRQ;
+	LOG_DEBUG(">>> [%d] debug_reason=%d", target->coreid, target->debug_reason);
 
 	return ERROR_OK;
 }
@@ -489,8 +490,8 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 		if (result != ERROR_OK)
 			continue;
 
-		LOG_DEBUG("Using trigger %d (type %d) for bp %d", i, type,
-				trigger->unique_id);
+		LOG_DEBUG("[%d] Using trigger %d (type %d) for bp %d", target->coreid,
+				i, type, trigger->unique_id);
 		r->trigger_unique_id[i] = trigger->unique_id;
 		break;
 	}
@@ -512,6 +513,7 @@ static int add_trigger(struct target *target, struct trigger *trigger)
 
 int riscv_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
 {
+	LOG_DEBUG("[%d] @0x%" TARGET_PRIxADDR, target->coreid, breakpoint->address);
 	assert(breakpoint);
 	if (breakpoint->type == BKPT_SOFT) {
 		/** @todo check RVC for size/alignment */
@@ -585,7 +587,8 @@ static int remove_trigger(struct target *target, struct trigger *trigger)
 				"trigger.");
 		return ERROR_FAIL;
 	}
-	LOG_DEBUG("Stop using resource %d for bp %d", i, trigger->unique_id);
+	LOG_DEBUG("[%d] Stop using resource %d for bp %d", target->coreid, i,
+			trigger->unique_id);
 	for (int hartid = first_hart; hartid < riscv_count_harts(target); ++hartid) {
 		if (!riscv_hart_enabled(target, hartid))
 			continue;
@@ -646,12 +649,33 @@ static void trigger_from_watchpoint(struct trigger *trigger,
 
 int riscv_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
 {
+	LOG_DEBUG("[%d] @0x%" TARGET_PRIxADDR, target->coreid, watchpoint->address);
 	struct trigger trigger;
 	trigger_from_watchpoint(&trigger, watchpoint);
 
-	int result = add_trigger(target, &trigger);
-	if (result != ERROR_OK)
-		return result;
+	if (target->smp) {
+		struct target *failed = NULL;
+		for (struct target_list *list = target->head; list != NULL;
+				list = list->next) {
+			struct target *t = list->target;
+			if (add_trigger(t, &trigger) != ERROR_OK) {
+				failed = t;
+				break;
+			}
+		}
+		if (failed) {
+			for (struct target_list *list = target->head;
+					list->target != failed; list = list->next) {
+				struct target *t = list->target;
+				remove_trigger(t, &trigger);
+			}
+			return ERROR_FAIL;
+		}
+	} else {
+		int result = add_trigger(target, &trigger);
+		if (result != ERROR_OK)
+			return result;
+	}
 	watchpoint->set = true;
 
 	return ERROR_OK;
@@ -660,12 +684,26 @@ int riscv_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
 int riscv_remove_watchpoint(struct target *target,
 		struct watchpoint *watchpoint)
 {
+	LOG_DEBUG("[%d] @0x%" TARGET_PRIxADDR, target->coreid, watchpoint->address);
+
 	struct trigger trigger;
 	trigger_from_watchpoint(&trigger, watchpoint);
 
-	int result = remove_trigger(target, &trigger);
-	if (result != ERROR_OK)
-		return result;
+	if (target->smp) {
+		bool failed = false;
+		for (struct target_list *list = target->head; list != NULL;
+				list = list->next) {
+			struct target *t = list->target;
+			if (remove_trigger(t, &trigger) != ERROR_OK)
+				failed = true;
+		}
+		if (failed)
+			return ERROR_FAIL;
+	} else {
+		int result = remove_trigger(target, &trigger);
+		if (result != ERROR_OK)
+			return result;
+	}
 	watchpoint->set = false;
 
 	return ERROR_OK;
@@ -1143,6 +1181,31 @@ 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)
+{
+	switch (riscv_halt_reason(target, hartid)) {
+		case RISCV_HALT_BREAKPOINT:
+			target->debug_reason = DBG_REASON_BREAKPOINT;
+			break;
+		case RISCV_HALT_TRIGGER:
+			target->debug_reason = DBG_REASON_WATCHPOINT;
+			break;
+		case RISCV_HALT_INTERRUPT:
+			target->debug_reason = DBG_REASON_DBGRQ;
+			break;
+		case RISCV_HALT_SINGLESTEP:
+			target->debug_reason = DBG_REASON_SINGLESTEP;
+			break;
+		case RISCV_HALT_UNKNOWN:
+			target->debug_reason = DBG_REASON_UNDEFINED;
+			break;
+		case RISCV_HALT_ERROR:
+			return ERROR_FAIL;
+	}
+	LOG_DEBUG(">>> [%d] debug_reason=%d", target->coreid, target->debug_reason);
+	return ERROR_OK;
+}
+
 /*** OpenOCD Interface ***/
 int riscv_openocd_poll(struct target *target)
 {
@@ -1177,6 +1240,64 @@ int riscv_openocd_poll(struct target *target)
 		 * harts. */
 		for (int i = 0; i < riscv_count_harts(target); ++i)
 			riscv_halt_one_hart(target, i);
+
+	} else if (target->smp) {
+		bool halt_discovered = false;
+		bool newly_halted[128] = {0};
+		unsigned i = 0;
+		for (struct target_list *list = target->head; list != NULL;
+				list = list->next, i++) {
+			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;
+					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:
+					return ERROR_FAIL;
+			}
+		}
+
+		if (halt_discovered) {
+			LOG_DEBUG("Halt other targets in this SMP group.");
+			i = 0;
+			for (struct target_list *list = target->head; list != NULL;
+					list = list->next, i++) {
+				struct target *t = list->target;
+				riscv_info_t *r = riscv_info(t);
+				if (t->state != TARGET_HALTED) {
+					if (riscv_halt_one_hart(t, r->current_hartid) != ERROR_OK)
+						return ERROR_FAIL;
+					t->state = TARGET_HALTED;
+					if (set_debug_reason(t, r->current_hartid) != ERROR_OK)
+						return ERROR_FAIL;
+					newly_halted[i] = true;
+				}
+			}
+
+			/* Now that we have all our ducks in a row, tell the higher layers
+			 * what just happened. */
+			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);
+			}
+		}
+		return ERROR_OK;
+
 	} else {
 		enum riscv_poll_hart out = riscv_poll_hart(target,
 				riscv_current_hartid(target));
@@ -1190,25 +1311,8 @@ int riscv_openocd_poll(struct target *target)
 	}
 
 	target->state = TARGET_HALTED;
-	switch (riscv_halt_reason(target, halted_hart)) {
-	case RISCV_HALT_BREAKPOINT:
-		target->debug_reason = DBG_REASON_BREAKPOINT;
-		break;
-	case RISCV_HALT_TRIGGER:
-		target->debug_reason = DBG_REASON_WATCHPOINT;
-		break;
-	case RISCV_HALT_INTERRUPT:
-		target->debug_reason = DBG_REASON_DBGRQ;
-		break;
-	case RISCV_HALT_SINGLESTEP:
-		target->debug_reason = DBG_REASON_SINGLESTEP;
-		break;
-	case RISCV_HALT_UNKNOWN:
-		target->debug_reason = DBG_REASON_UNDEFINED;
-		break;
-	case RISCV_HALT_ERROR:
+	if (set_debug_reason(target, halted_hart) != ERROR_OK)
 		return ERROR_FAIL;
-	}
 
 	if (riscv_rtos_enabled(target)) {
 		target->rtos->current_threadid = halted_hart + 1;
@@ -1218,22 +1322,6 @@ int riscv_openocd_poll(struct target *target)
 
 	target->state = TARGET_HALTED;
 
-	if (target->smp) {
-		LOG_DEBUG("Halt other targets in this SMP group.");
-		struct target_list *targets = target->head;
-		int result = ERROR_OK;
-		while (targets) {
-			struct target *t = targets->target;
-			targets = targets->next;
-			if (t->state != TARGET_HALTED) {
-				if (riscv_halt_all_harts(t) != ERROR_OK)
-					result = ERROR_FAIL;
-			}
-		}
-		if (result != ERROR_OK)
-			return result;
-	}
-
 	if (target->debug_reason == DBG_REASON_BREAKPOINT) {
 		int retval;
 		if (riscv_semihosting(target, &retval) != 0)
@@ -1249,7 +1337,7 @@ int riscv_openocd_halt(struct target *target)
 	RISCV_INFO(r);
 	int result;
 
-	LOG_DEBUG("halting all harts");
+	LOG_DEBUG("[%d] halting all harts", target->coreid);
 
 	if (target->smp) {
 		LOG_DEBUG("Halt other targets in this SMP group.");
@@ -1278,6 +1366,7 @@ int riscv_openocd_halt(struct target *target)
 
 	target->state = TARGET_HALTED;
 	target->debug_reason = DBG_REASON_DBGRQ;
+	LOG_DEBUG(">>> [%d] debug_reason=%d", target->coreid, target->debug_reason);
 	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 	return result;
 }
@@ -1366,6 +1455,7 @@ int riscv_openocd_step(
 	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 	target->state = TARGET_HALTED;
 	target->debug_reason = DBG_REASON_SINGLESTEP;
+	LOG_DEBUG(">>> [%d] debug_reason=%d", target->coreid, target->debug_reason);
 	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 	return out;
 }
diff --git a/src/target/target.c b/src/target/target.c
index ffd82fb..8b94fc9 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1206,6 +1206,8 @@ int target_hit_watchpoint(struct target *target,
 		return ERROR_FAIL;
 	}
 
+	LOG_DEBUG("[%d]", target->coreid);
+
 	return target->type->hit_watchpoint(target, hit_watchpoint);
 }