Step/resume off manual hardware triggers (#486)

* Accommodate users setting custom triggers.

RISC-V hardware supports many more triggers than gdb can communicate to
OpenOCD. Accommodate users that set triggers by writing tdata* directly,
by disable/step/reenable when a user has done that.

Note that users must set dmode in tdata1 for this behavior to work
properly. Triggers with dmode=0 are assumed to be set and handled by the
software that is being debugged.

Change-Id: Ib0751689c5553aae3a273395b10f5b98326fa066

* Enumerate triggers when resuming from a trigger

Otherwise when we connect to a target that's already halted due to a
trigger, we won't correctly step past it.

Change-Id: I23b9482fa9597af826770f9cebf247b7ba59f65c

* Also disable/reenable triggers around single step.

Gdb is smart enough to disable/step/resume if it set the triggers, but
if a user set them manually it also needs to happen.

Change-Id: I1251bd47199b6f15f61a93e3a521a53f2b677c5f

* Fix whitespace.

Change-Id: Icc240aecbc7e3e36ce4e4d784f5703304334ca13

2 files changed, 119 insertions, 30 deletions

diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 192b8c5..65b60cd 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -1213,54 +1213,120 @@ int riscv_resume_prep_all_harts(struct target *target)
 	return ERROR_OK;
 }
 
-/**
- * Get everything ready to resume.
- */
-static int resume_prep(struct target *target, int current,
-		target_addr_t address, int handle_breakpoints, int debug_execution)
+/* state must be riscv_reg_t state[RISCV_MAX_HWBPS] = {0}; */
+static int disable_triggers(struct target *target, riscv_reg_t *state)
 {
 	RISCV_INFO(r);
-	LOG_DEBUG("[%d]", target->coreid);
 
-	if (!current)
-		riscv_set_register(target, GDB_REGNO_PC, address);
+	LOG_DEBUG("deal with triggers");
 
-	if (target->debug_reason == DBG_REASON_WATCHPOINT) {
-		/* To be able to run off a trigger, disable all the triggers, step, and
-		 * then resume as usual. */
-		struct watchpoint *watchpoint = target->watchpoints;
-		bool trigger_temporarily_cleared[RISCV_MAX_HWBPS] = {0};
+	if (riscv_enumerate_triggers(target) != ERROR_OK)
+		return ERROR_FAIL;
 
+	int hartid = riscv_current_hartid(target);
+	if (r->manual_hwbp_set) {
+		/* Look at every trigger that may have been set. */
+		riscv_reg_t tselect;
+		if (riscv_get_register(target, &tselect, GDB_REGNO_TSELECT) != ERROR_OK)
+			return ERROR_FAIL;
+		for (unsigned t = 0; t < r->trigger_count[hartid]; t++) {
+			if (riscv_set_register(target, GDB_REGNO_TSELECT, t) != ERROR_OK)
+				return ERROR_FAIL;
+			riscv_reg_t tdata1;
+			if (riscv_get_register(target, &tdata1, GDB_REGNO_TDATA1) != ERROR_OK)
+				return ERROR_FAIL;
+			if (tdata1 & MCONTROL_DMODE(riscv_xlen(target))) {
+				state[t] = tdata1;
+				if (riscv_set_register(target, GDB_REGNO_TDATA1, 0) != ERROR_OK)
+					return ERROR_FAIL;
+			}
+		}
+		if (riscv_set_register(target, GDB_REGNO_TSELECT, tselect) != ERROR_OK)
+			return ERROR_FAIL;
+
+	} else {
+		/* Just go through the triggers we manage. */
+		struct watchpoint *watchpoint = target->watchpoints;
 		int i = 0;
-		int result = ERROR_OK;
-		while (watchpoint && result == ERROR_OK) {
+		while (watchpoint) {
 			LOG_DEBUG("watchpoint %d: set=%d", i, watchpoint->set);
-			trigger_temporarily_cleared[i] = watchpoint->set;
-			if (watchpoint->set)
-				result = riscv_remove_watchpoint(target, watchpoint);
+			state[i] = watchpoint->set;
+			if (watchpoint->set) {
+				if (riscv_remove_watchpoint(target, watchpoint) != ERROR_OK)
+					return ERROR_FAIL;
+			}
 			watchpoint = watchpoint->next;
 			i++;
 		}
+	}
 
-		if (result == ERROR_OK)
-			result = old_or_new_riscv_step(target, true, 0, false);
+	return ERROR_OK;
+}
 
-		watchpoint = target->watchpoints;
-		i = 0;
+static int enable_triggers(struct target *target, riscv_reg_t *state)
+{
+	RISCV_INFO(r);
+
+	int hartid = riscv_current_hartid(target);
+
+	if (r->manual_hwbp_set) {
+		/* Look at every trigger that may have been set. */
+		riscv_reg_t tselect;
+		if (riscv_get_register(target, &tselect, GDB_REGNO_TSELECT) != ERROR_OK)
+			return ERROR_FAIL;
+		for (unsigned t = 0; t < r->trigger_count[hartid]; t++) {
+			if (state[t] != 0) {
+				if (riscv_set_register(target, GDB_REGNO_TSELECT, t) != ERROR_OK)
+					return ERROR_FAIL;
+				if (riscv_set_register(target, GDB_REGNO_TDATA1, state[t]) != ERROR_OK)
+					return ERROR_FAIL;
+			}
+		}
+		if (riscv_set_register(target, GDB_REGNO_TSELECT, tselect) != ERROR_OK)
+			return ERROR_FAIL;
+
+	} else {
+		struct watchpoint *watchpoint = target->watchpoints;
+		int i = 0;
 		while (watchpoint) {
-			LOG_DEBUG("watchpoint %d: cleared=%d", i, trigger_temporarily_cleared[i]);
-			if (trigger_temporarily_cleared[i]) {
-				if (result == ERROR_OK)
-					result = riscv_add_watchpoint(target, watchpoint);
-				else
-					riscv_add_watchpoint(target, watchpoint);
+			LOG_DEBUG("watchpoint %d: cleared=%" PRId64, i, state[i]);
+			if (state[i]) {
+				if (riscv_add_watchpoint(target, watchpoint) != ERROR_OK)
+					return ERROR_FAIL;
 			}
 			watchpoint = watchpoint->next;
 			i++;
 		}
+	}
 
-		if (result != ERROR_OK)
-			return result;
+	return ERROR_OK;
+}
+
+/**
+ * Get everything ready to resume.
+ */
+static int resume_prep(struct target *target, int current,
+		target_addr_t address, int handle_breakpoints, int debug_execution)
+{
+	RISCV_INFO(r);
+	LOG_DEBUG("[%d]", target->coreid);
+
+	if (!current)
+		riscv_set_register(target, GDB_REGNO_PC, address);
+
+	if (target->debug_reason == DBG_REASON_WATCHPOINT) {
+		/* To be able to run off a trigger, disable all the triggers, step, and
+		 * then resume as usual. */
+		riscv_reg_t trigger_state[RISCV_MAX_HWBPS] = {0};
+
+		if (disable_triggers(target, trigger_state) != ERROR_OK)
+			return ERROR_FAIL;
+
+		if (old_or_new_riscv_step(target, true, 0, false) != ERROR_OK)
+			return ERROR_FAIL;
+
+		if (enable_triggers(target, trigger_state) != ERROR_OK)
+			return ERROR_FAIL;
 	}
 
 	if (r->is_halted) {
@@ -2133,6 +2199,10 @@ int riscv_openocd_step(
 	if (!current)
 		riscv_set_register(target, GDB_REGNO_PC, address);
 
+	riscv_reg_t trigger_state[RISCV_MAX_HWBPS] = {0};
+	if (disable_triggers(target, trigger_state) != ERROR_OK)
+		return ERROR_FAIL;
+
 	int out = riscv_step_rtos_hart(target);
 	if (out != ERROR_OK) {
 		LOG_ERROR("unable to step rtos hart");
@@ -2140,6 +2210,10 @@ int riscv_openocd_step(
 	}
 
 	register_cache_invalidate(target->reg_cache);
+
+	if (enable_triggers(target, trigger_state) != ERROR_OK)
+		return ERROR_FAIL;
+
 	target->state = TARGET_RUNNING;
 	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 	target->state = TARGET_HALTED;
@@ -3563,6 +3637,17 @@ static int register_set(struct reg *reg, uint8_t *buf)
 	memcpy(reg->value, buf, DIV_ROUND_UP(reg->size, 8));
 	reg->valid = gdb_regno_cacheable(reg->number, true);
 
+	if (reg->number == GDB_REGNO_TDATA1 ||
+			reg->number == GDB_REGNO_TDATA2) {
+		r->manual_hwbp_set = true;
+		/* When enumerating triggers, we clear any triggers with DMODE set,
+		 * assuming they were left over from a previous debug session. So make
+		 * sure that is done before a user might be setting their own triggers.
+		 */
+		if (riscv_enumerate_triggers(target) != ERROR_OK)
+			return ERROR_FAIL;
+	}
+
 	if (reg->number >= GDB_REGNO_V0 && reg->number <= GDB_REGNO_V31) {
 		if (!r->set_register_buf) {
 			LOG_ERROR("Writing register %s not supported on this RISC-V target.",
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index d446aa8..2714d9b 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -170,6 +170,10 @@ typedef struct {
 	struct reg_data_type_union_field vector_fields[5];
 	struct reg_data_type_union vector_union;
 	struct reg_data_type type_vector;
+
+	/* Set when trigger registers are changed by the user. This indicates we eed
+	 * to beware that we may hit a trigger that we didn't realize had been set. */
+	bool manual_hwbp_set;
 } riscv_info_t;
 
 typedef struct {