target/riscv: manage triggers available to OpenOCD for internal use

Before the change, if the user wrote to any `tdata*` register, OpenOCD
would sometimes start to disable all the triggers (by writing zeroes to
`tdata1`) and re-enable them again (by witing all trigger registers to the
values read before for each `tselect` value), e.g. on `step`
(see `disable/enable_triggers()`).

There are a couple of issues with such approach:
1. RISC-V Debug Specification does not require custom register types
   to support re-enabling by such sequence of writes (e.g. some custom
   trigger type may require writing a custom CSR to enable it).
2. OpenOCD may still overwrite these triggers when a user asks to set a
   new WP.

This commit introduces `riscv reserve_trigger ...` command to explicitly
mark the triggers OpenOCD should not touch.

Such approach allows to separate management of custom triggers and
offload it onto the user (e.g. disable/enable such triggers by setting up
an event handler on `step`-related events).

Change-Id: I3339000445185ab221368442a070f412bf44bfab
Signed-off-by: Evgeniy Naydanov <evgeniy.naydanov@syntacore.com>

4 files changed, 131 insertions, 87 deletions

diff --git a/doc/openocd.texi b/doc/openocd.texi
index 5bd4e89..16e8064 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -11524,6 +11524,21 @@ as in the mie CSR (defined in the RISC-V Privileged Spec).
 For details on this trigger type, see the RISC-V Debug Specification.
 @end deffn
 
+@deffn {Command} {riscv reserve_trigger} [index @option{on|off}]
+Manages the set of reserved triggers. Reserving a trigger results in OpenOCD
+not using it internally (e.g. skipping it when setting a watchpoint or a
+hardware breakpoint), so that the user or the application has unfettered
+control over the trigger. By default there are no reserved triggers.
+
+@enumerate
+@item @var{index} specifies the index of a trigger to reserve or free up.
+@item The second argument specifies whether the trigger should be reserved
+(@var{on}) or a prior reservation cancelled (@var{off}).
+@item If called without parameters, returns indices of reserved triggers.
+@end enumerate
+
+@end deffn
+
 @deffn {Command} {riscv itrigger clear}
 Clear the type 4 trigger that was set using @command{riscv itrigger set}.
 @end deffn
diff --git a/src/target/riscv/riscv-013_reg.c b/src/target/riscv/riscv-013_reg.c
index a71a01c..ed99a06 100644
--- a/src/target/riscv/riscv-013_reg.c
+++ b/src/target/riscv/riscv-013_reg.c
@@ -44,7 +44,6 @@ static int riscv013_reg_get(struct reg *reg)
 static int riscv013_reg_set(struct reg *reg, uint8_t *buf)
 {
 	struct target *target = riscv_reg_impl_get_target(reg);
-	RISCV_INFO(r);
 
 	char *str = buf_to_hex_str(buf, reg->size);
 	LOG_TARGET_DEBUG(target, "Write 0x%s to %s (valid=%d).", str, reg->name,
@@ -57,17 +56,6 @@ static int riscv013_reg_set(struct reg *reg, uint8_t *buf)
 			buf_get_u64(buf, 0, reg->size) == 0)
 		return ERROR_OK;
 
-	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 (riscv013_set_register_buf(target, reg->number, buf) != ERROR_OK)
 			return ERROR_FAIL;
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 267d20d..10ca8a0 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -527,6 +527,8 @@ static void riscv_deinit_target(struct target *target)
 	if (!info)
 		return;
 
+	free(info->reserved_triggers);
+
 	range_list_t *entry, *tmp;
 	list_for_each_entry_safe(entry, tmp, &info->hide_csr, list) {
 		free(entry->name);
@@ -620,6 +622,15 @@ static int find_first_trigger_by_id(struct target *target, int unique_id)
 static int set_trigger(struct target *target, unsigned int idx, riscv_reg_t tdata1, riscv_reg_t tdata2,
 	riscv_reg_t tdata1_ignore_mask)
 {
+	RISCV_INFO(r);
+	assert(r->reserved_triggers);
+	assert(idx < r->trigger_count);
+	if (r->reserved_triggers[idx]) {
+		LOG_TARGET_DEBUG(target,
+				"Trigger %u is reserved by 'reserve_trigger' command.", idx);
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
 	riscv_reg_t tdata1_rb, tdata2_rb;
 	// Select which trigger to use
 	if (riscv_reg_set(target, GDB_REGNO_TSELECT, idx) != ERROR_OK)
@@ -2453,87 +2464,48 @@ static int riscv_deassert_reset(struct target *target)
 	return tt->deassert_reset(target);
 }
 
-/* state must be riscv_reg_t state[RISCV_MAX_HWBPS] = {0}; */
-static int disable_triggers(struct target *target, riscv_reg_t *state)
+/* "wp_is_set" array must have at least "r->trigger_count" items. */
+static int disable_watchpoints(struct target *target, bool *wp_is_set)
 {
 	RISCV_INFO(r);
-
 	LOG_TARGET_DEBUG(target, "Disabling triggers.");
 
-	if (riscv_enumerate_triggers(target) != ERROR_OK)
-		return ERROR_FAIL;
-
-	if (r->manual_hwbp_set) {
-		/* Look at every trigger that may have been set. */
-		riscv_reg_t tselect;
-		if (riscv_reg_get(target, &tselect, GDB_REGNO_TSELECT) != ERROR_OK)
-			return ERROR_FAIL;
-		for (unsigned int t = 0; t < r->trigger_count; t++) {
-			if (riscv_reg_set(target, GDB_REGNO_TSELECT, t) != ERROR_OK)
-				return ERROR_FAIL;
-			riscv_reg_t tdata1;
-			if (riscv_reg_get(target, &tdata1, GDB_REGNO_TDATA1) != ERROR_OK)
+	/* TODO: The algorithm is flawed and may result in a situation described in
+	 * https://github.com/riscv-collab/riscv-openocd/issues/1108
+	 */
+	memset(wp_is_set, false, r->trigger_count);
+	struct watchpoint *watchpoint = target->watchpoints;
+	int i = 0;
+	while (watchpoint) {
+		LOG_TARGET_DEBUG(target, "Watchpoint %" PRIu32 ": set=%s",
+				watchpoint->unique_id,
+				wp_is_set[i] ? "true" : "false");
+		wp_is_set[i] = watchpoint->is_set;
+		if (watchpoint->is_set) {
+			if (riscv_remove_watchpoint(target, watchpoint) != ERROR_OK)
 				return ERROR_FAIL;
-			if (tdata1 & CSR_TDATA1_DMODE(riscv_xlen(target))) {
-				state[t] = tdata1;
-				if (riscv_reg_set(target, GDB_REGNO_TDATA1, 0) != ERROR_OK)
-					return ERROR_FAIL;
-			}
-		}
-		if (riscv_reg_set(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) {
-			LOG_TARGET_DEBUG(target, "Watchpoint %d: set=%d", i, watchpoint->is_set);
-			state[i] = watchpoint->is_set;
-			if (watchpoint->is_set) {
-				if (riscv_remove_watchpoint(target, watchpoint) != ERROR_OK)
-					return ERROR_FAIL;
-			}
-			watchpoint = watchpoint->next;
-			i++;
 		}
+		watchpoint = watchpoint->next;
+		i++;
 	}
 
 	return ERROR_OK;
 }
 
-static int enable_triggers(struct target *target, riscv_reg_t *state)
+static int enable_watchpoints(struct target *target, bool *wp_is_set)
 {
-	RISCV_INFO(r);
-
-	if (r->manual_hwbp_set) {
-		/* Look at every trigger that may have been set. */
-		riscv_reg_t tselect;
-		if (riscv_reg_get(target, &tselect, GDB_REGNO_TSELECT) != ERROR_OK)
-			return ERROR_FAIL;
-		for (unsigned int t = 0; t < r->trigger_count; t++) {
-			if (state[t] != 0) {
-				if (riscv_reg_set(target, GDB_REGNO_TSELECT, t) != ERROR_OK)
-					return ERROR_FAIL;
-				if (riscv_reg_set(target, GDB_REGNO_TDATA1, state[t]) != ERROR_OK)
-					return ERROR_FAIL;
-			}
-		}
-		if (riscv_reg_set(target, GDB_REGNO_TSELECT, tselect) != ERROR_OK)
-			return ERROR_FAIL;
-
-	} else {
-		struct watchpoint *watchpoint = target->watchpoints;
-		int i = 0;
-		while (watchpoint) {
-			LOG_TARGET_DEBUG(target, "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++;
+	struct watchpoint *watchpoint = target->watchpoints;
+	int i = 0;
+	while (watchpoint) {
+		LOG_TARGET_DEBUG(target, "Watchpoint %" PRIu32
+				": %s to be re-enabled.", watchpoint->unique_id,
+				wp_is_set[i] ? "needs " : "does not need");
+		if (wp_is_set[i]) {
+			if (riscv_add_watchpoint(target, watchpoint) != ERROR_OK)
+				return ERROR_FAIL;
 		}
+		watchpoint = watchpoint->next;
+		i++;
 	}
 
 	return ERROR_OK;
@@ -3781,10 +3753,17 @@ static int riscv_openocd_step_impl(struct target *target, int current,
 			return ERROR_FAIL;
 	}
 
-	riscv_reg_t trigger_state[RISCV_MAX_HWBPS] = {0};
-	if (disable_triggers(target, trigger_state) != ERROR_OK)
+	if (riscv_enumerate_triggers(target) != ERROR_OK)
 		return ERROR_FAIL;
 
+	RISCV_INFO(r);
+	bool *wps_to_enable = calloc(sizeof(*wps_to_enable), r->trigger_count);
+	if (disable_watchpoints(target, wps_to_enable) != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to temporarily disable "
+				"watchpoints before single-step.");
+		return ERROR_FAIL;
+	}
+
 	bool success = true;
 	uint64_t current_mstatus;
 	RISCV_INFO(info);
@@ -3814,9 +3793,10 @@ static int riscv_openocd_step_impl(struct target *target, int current,
 		}
 
 _exit:
-	if (enable_triggers(target, trigger_state) != ERROR_OK) {
+	if (enable_watchpoints(target, wps_to_enable) != ERROR_OK) {
 		success = false;
-		LOG_TARGET_ERROR(target, "Unable to enable triggers.");
+		LOG_TARGET_ERROR(target, "Failed to re-enable watchpoints "
+				"after single-step.");
 	}
 
 	if (breakpoint && (riscv_add_breakpoint(target, breakpoint) != ERROR_OK)) {
@@ -5031,6 +5011,57 @@ COMMAND_HANDLER(riscv_set_enable_trigger_feature)
 	return ERROR_OK;
 }
 
+static COMMAND_HELPER(report_reserved_triggers, struct target *target)
+{
+	RISCV_INFO(r);
+	if (riscv_enumerate_triggers(target) != ERROR_OK)
+		return ERROR_FAIL;
+	const char *separator = "";
+	for (riscv_reg_t t = 0; t < r->trigger_count; ++t) {
+		if (r->reserved_triggers[t]) {
+			command_print_sameline(CMD, "%s%" PRIu64, separator, t);
+			separator = " ";
+		}
+	}
+	command_print_sameline(CMD, "\n");
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_reserve_trigger)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	if (CMD_ARGC == 0)
+		return CALL_COMMAND_HANDLER(report_reserved_triggers, target);
+
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	riscv_reg_t t;
+	COMMAND_PARSE_NUMBER(u64, CMD_ARGV[0], t);
+
+	if (riscv_enumerate_triggers(target) != ERROR_OK)
+		return ERROR_FAIL;
+	RISCV_INFO(r);
+	if (r->trigger_count == 0) {
+		command_print(CMD, "Error: There are no triggers on the target.");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	if (t >= r->trigger_count) {
+		command_print(CMD, "Error: trigger with index %" PRIu64
+				" does not exist. There are only %u triggers"
+				" on the target (with indexes 0 .. %u).",
+				t, r->trigger_count, r->trigger_count - 1);
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	if (r->trigger_unique_id[t] != -1) {
+		command_print(CMD, "Error: trigger with index %" PRIu64
+				" is already in use and can not be reserved.", t);
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	COMMAND_PARSE_ON_OFF(CMD_ARGV[1], r->reserved_triggers[t]);
+	return ERROR_OK;
+}
+
 static const struct command_registration riscv_exec_command_handlers[] = {
 	{
 		.name = "dump_sample_buf",
@@ -5287,6 +5318,14 @@ static const struct command_registration riscv_exec_command_handlers[] = {
 		.usage = "[('eq'|'napot'|'ge_lt'|'all') ('wp'|'none')]",
 		.help = "Control whether OpenOCD is allowed to use certain RISC-V trigger features for watchpoints."
 	},
+	{
+		.name = "reserve_trigger",
+		.handler = handle_reserve_trigger,
+		/* TODO: Move this to COMMAND_ANY */
+		.mode = COMMAND_EXEC,
+		.usage = "[index ('on'|'off')]",
+		.help = "Controls which RISC-V triggers shall not be touched by OpenOCD.",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
@@ -5711,6 +5750,8 @@ int riscv_enumerate_triggers(struct target *target)
 				"Assuming that triggers are not implemented.");
 		r->triggers_enumerated = true;
 		r->trigger_count = 0;
+		free(r->reserved_triggers);
+		r->reserved_triggers = NULL;
 		return ERROR_OK;
 	}
 
@@ -5745,6 +5786,8 @@ int riscv_enumerate_triggers(struct target *target)
 	r->triggers_enumerated = true;
 	r->trigger_count = t;
 	LOG_TARGET_INFO(target, "Found %d triggers", r->trigger_count);
+	free(r->reserved_triggers);
+	r->reserved_triggers = calloc(sizeof(*r->reserved_triggers), t);
 	create_wp_trigger_cache(target);
 	return ERROR_OK;
 }
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index b29d209..9da8ab8 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -272,9 +272,7 @@ struct riscv_info {
 	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 need
-	 * to beware that we may hit a trigger that we didn't realize had been set. */
-	bool manual_hwbp_set;
+	bool *reserved_triggers;
 
 	/* Memory access methods to use, ordered by priority, highest to lowest. */
 	int mem_access_methods[RISCV_NUM_MEM_ACCESS_METHODS];