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

2 files changed, 81 insertions, 21 deletions

diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 192b8c5..aa00295 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -1228,35 +1228,80 @@ static int resume_prep(struct target *target, int current,
 	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};
+		LOG_DEBUG("deal with triggers");
+		riscv_reg_t trigger_temporarily_cleared[RISCV_MAX_HWBPS] = {0};
 
-		int i = 0;
 		int result = ERROR_OK;
-		while (watchpoint && result == ERROR_OK) {
-			LOG_DEBUG("watchpoint %d: set=%d", i, watchpoint->set);
-			trigger_temporarily_cleared[i] = watchpoint->set;
-			if (watchpoint->set)
-				result = riscv_remove_watchpoint(target, watchpoint);
-			watchpoint = watchpoint->next;
-			i++;
+		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))) {
+					trigger_temporarily_cleared[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;
+			while (watchpoint && result == ERROR_OK) {
+				LOG_DEBUG("watchpoint %d: set=%d", i, watchpoint->set);
+				trigger_temporarily_cleared[i] = watchpoint->set;
+				if (watchpoint->set)
+					result = riscv_remove_watchpoint(target, watchpoint);
+				watchpoint = watchpoint->next;
+				i++;
+			}
 		}
 
 		if (result == ERROR_OK)
 			result = old_or_new_riscv_step(target, true, 0, false);
 
-		watchpoint = target->watchpoints;
-		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);
+		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 (trigger_temporarily_cleared[t] != 0) {
+					if (riscv_set_register(target, GDB_REGNO_TSELECT, t) != ERROR_OK)
+						return ERROR_FAIL;
+					if (riscv_set_register(target, GDB_REGNO_TDATA1, trigger_temporarily_cleared[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=%" PRId64, 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);
+				}
+				watchpoint = watchpoint->next;
+				i++;
 			}
-			watchpoint = watchpoint->next;
-			i++;
 		}
 
 		if (result != ERROR_OK)
@@ -3563,6 +3608,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 {