Add `authdata_read` and `authdata_write` commands.

They can be used to authenticate to a Debug Module.

There's a bit of a chicken and egg problem here, because the RISCV
commands aren't available until the target is initialized, but
initialization involves examine(), which can't interact with the target
until authentication has happened. So to use this you run `init`, which
will print out an error, and then run the `riscv authdata_read` and
`riscv authdata_write` commands. When authdata_write() notices that the
authenticated bit went high, it will call examine() again.

Example usage (very simple challenge-response protocol):
```
init

set challenge [ocd_riscv authdata_read]
riscv authdata_write [expr $challenge + 1]

reset halt
```

Change-Id: Id9ead00a7eca111e5ec879c4af4586c30af51f4d

3 files changed, 260 insertions, 60 deletions

diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 22a4c87..d693cb5 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -19,6 +19,7 @@
 #include "target/register.h"
 #include "target/breakpoints.h"
 #include "helper/time_support.h"
+#include "helper/list.h"
 #include "riscv.h"
 #include "rtos/riscv_debug.h"
 #include "debug_defines.h"
@@ -136,6 +137,13 @@ typedef enum {
 } yes_no_maybe_t;
 
 typedef struct {
+	struct list_head list;
+	int abs_chain_position;
+	/* Indicates we already reset this DM, so don't need to do it again. */
+	bool was_reset;
+} dm013_info_t;
+
+typedef struct {
 	/* Number of address bits in the dbus register. */
 	unsigned abits;
 	/* Number of abstract command data registers. */
@@ -189,14 +197,47 @@ typedef struct {
 
 	/* The width of the hartsel field. */
 	unsigned hartsellen;
+
+	dm013_info_t *dm;
 } riscv013_info_t;
 
+LIST_HEAD(dm_list);
+
 static riscv013_info_t *get_info(const struct target *target)
 {
 	riscv_info_t *info = (riscv_info_t *) target->arch_info;
 	return (riscv013_info_t *) info->version_specific;
 }
 
+/**
+ * Return the DM structure for this target. If there isn't one, find it in the
+ * global list of DMs. If it's not in there, then create one and initialize it
+ * to 0.
+ */
+static dm013_info_t *get_dm(struct target *target)
+{
+	RISCV013_INFO(info);
+	if (info->dm)
+		return info->dm;
+
+	int abs_chain_position = target->tap->abs_chain_position;
+
+	dm013_info_t *entry;
+	list_for_each_entry(entry, &dm_list, list) {
+		if (entry->abs_chain_position == abs_chain_position) {
+			info->dm = entry;
+			return entry;
+		}
+	}
+
+	dm013_info_t *dm = calloc(1, sizeof(dm013_info_t));
+	dm->abs_chain_position = abs_chain_position;
+	list_add(&dm->list, &dm_list);
+	info->dm = dm;
+
+	return dm;
+}
+
 static uint32_t hartsel_mask(const struct target *target)
 {
 	RISCV013_INFO(info);
@@ -525,6 +566,19 @@ static int dmi_write(struct target *target, uint16_t address, uint64_t value)
 	return ERROR_OK;
 }
 
+int dmstatus_read(struct target *target, uint32_t *dmstatus,
+		bool authenticated)
+{
+	*dmstatus = dmi_read(target, DMI_DMSTATUS);
+	if (authenticated && !get_field(*dmstatus, DMI_DMSTATUS_AUTHENTICATED)) {
+		LOG_ERROR("Debugger is not authenticated to target Debug Module. "
+				"(dmstatus=0x%x). Use `riscv authdata_read` and "
+				"`riscv_authdata_write` commands to authenticate.", *dmstatus);
+		return ERROR_FAIL;
+	}
+	return ERROR_OK;
+}
+
 static void increase_ac_busy_delay(struct target *target)
 {
 	riscv013_info_t *info = get_info(target);
@@ -1111,57 +1165,31 @@ static int register_read_direct(struct target *target, uint64_t *value, uint32_t
 	return result;
 }
 
-/*** OpenOCD target functions. ***/
-
-static int init_target(struct command_context *cmd_ctx,
-		struct target *target)
+int wait_for_authbusy(struct target *target, uint32_t *dmstatus)
 {
-	LOG_DEBUG("init");
-	riscv_info_t *generic_info = (riscv_info_t *) target->arch_info;
-
-	generic_info->get_register = &riscv013_get_register;
-	generic_info->set_register = &riscv013_set_register;
-	generic_info->select_current_hart = &riscv013_select_current_hart;
-	generic_info->is_halted = &riscv013_is_halted;
-	generic_info->halt_current_hart = &riscv013_halt_current_hart;
-	generic_info->resume_current_hart = &riscv013_resume_current_hart;
-	generic_info->step_current_hart = &riscv013_step_current_hart;
-	generic_info->on_halt = &riscv013_on_halt;
-	generic_info->on_resume = &riscv013_on_resume;
-	generic_info->on_step = &riscv013_on_step;
-	generic_info->halt_reason = &riscv013_halt_reason;
-	generic_info->read_debug_buffer = &riscv013_read_debug_buffer;
-	generic_info->write_debug_buffer = &riscv013_write_debug_buffer;
-	generic_info->execute_debug_buffer = &riscv013_execute_debug_buffer;
-	generic_info->fill_dmi_write_u64 = &riscv013_fill_dmi_write_u64;
-	generic_info->fill_dmi_read_u64 = &riscv013_fill_dmi_read_u64;
-	generic_info->fill_dmi_nop_u64 = &riscv013_fill_dmi_nop_u64;
-	generic_info->dmi_write_u64_bits = &riscv013_dmi_write_u64_bits;
-	generic_info->version_specific = calloc(1, sizeof(riscv013_info_t));
-	if (!generic_info->version_specific)
-		return ERROR_FAIL;
-	riscv013_info_t *info = get_info(target);
-
-	info->progbufsize = -1;
-
-	info->dmi_busy_delay = 0;
-	info->bus_master_read_delay = 0;
-	info->bus_master_write_delay = 0;
-	info->ac_busy_delay = 0;
-
-	/* Assume all these abstract commands are supported until we learn
-	 * otherwise.
-	 * TODO: The spec allows eg. one CSR to be able to be accessed abstractly
-	 * while another one isn't. We don't track that this closely here, but in
-	 * the future we probably should. */
-	info->abstract_read_csr_supported = true;
-	info->abstract_write_csr_supported = true;
-	info->abstract_read_fpr_supported = true;
-	info->abstract_write_fpr_supported = true;
+	time_t start = time(NULL);
+	while (1) {
+		uint32_t value;
+		if (dmstatus_read(target, &value, false) != ERROR_OK)
+			return ERROR_FAIL;
+		if (dmstatus)
+			*dmstatus = value;
+		if (!get_field(value, DMI_DMSTATUS_AUTHBUSY))
+			break;
+		if (time(NULL) - start > riscv_command_timeout_sec) {
+			LOG_ERROR("Timed out after %ds waiting for authbusy to go low (dmstatus=0x%x). "
+					"Increase the timeout with riscv set_command_timeout_sec.",
+					riscv_command_timeout_sec,
+					value);
+			return ERROR_FAIL;
+		}
+	}
 
 	return ERROR_OK;
 }
 
+/*** OpenOCD target functions. ***/
+
 static void deinit_target(struct target *target)
 {
 	LOG_DEBUG("riscv_deinit_target()");
@@ -1195,7 +1223,9 @@ static int examine(struct target *target)
 	info->abits = get_field(dtmcontrol, DTM_DTMCS_ABITS);
 	info->dtmcontrol_idle = get_field(dtmcontrol, DTM_DTMCS_IDLE);
 
-	uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
+	uint32_t dmstatus;
+	if (dmstatus_read(target, &dmstatus, false) != ERROR_OK)
+		return ERROR_FAIL;
 	LOG_DEBUG("dmstatus:  0x%08x", dmstatus);
 	if (get_field(dmstatus, DMI_DMSTATUS_VERSION) != 2) {
 		LOG_ERROR("OpenOCD only supports Debug Module version 2, not %d "
@@ -1204,8 +1234,12 @@ static int examine(struct target *target)
 	}
 
 	/* Reset the Debug Module. */
-	dmi_write(target, DMI_DMCONTROL, 0);
-	dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE);
+	dm013_info_t *dm = get_dm(target);
+	if (!dm->was_reset) {
+		dmi_write(target, DMI_DMCONTROL, 0);
+		dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE);
+		dm->was_reset = true;
+	}
 
 	uint32_t max_hartsel_mask = ((1L<<10)-1) << DMI_DMCONTROL_HARTSEL_OFFSET;
 	dmi_write(target, DMI_DMCONTROL, max_hartsel_mask | DMI_DMCONTROL_DMACTIVE);
@@ -1232,9 +1266,14 @@ static int examine(struct target *target)
 	info->dataaddr = get_field(hartinfo, DMI_HARTINFO_DATAADDR);
 
 	if (!get_field(dmstatus, DMI_DMSTATUS_AUTHENTICATED)) {
-		LOG_ERROR("Authentication required by RISC-V core but not "
-				"supported by OpenOCD. dmcontrol=0x%x", dmcontrol);
-		return ERROR_FAIL;
+		LOG_ERROR("Debugger is not authenticated to target Debug Module. "
+				"(dmstatus=0x%x). Use `riscv authdata_read` and "
+				"`riscv_authdata_write` commands to authenticate.", dmstatus);
+		/* If we return ERROR_FAIL here, then in a multicore setup the next
+		 * core won't be examined, which means we won't set up the
+		 * authentication commands for them, which means the config script
+		 * needs to be a lot more complex. */
+		return ERROR_OK;
 	}
 
 	info->sbcs = dmi_read(target, DMI_SBCS);
@@ -1267,7 +1306,9 @@ static int examine(struct target *target)
 		r->current_hartid = i;
 		riscv013_select_current_hart(target);
 
-		uint32_t s = dmi_read(target, DMI_DMSTATUS);
+		uint32_t s;
+		if (dmstatus_read(target, &s, true) != ERROR_OK)
+			return ERROR_FAIL;
 		if (get_field(s, DMI_DMSTATUS_ANYNONEXISTENT))
 			break;
 		r->hart_count = i + 1;
@@ -1340,6 +1381,86 @@ static int examine(struct target *target)
 	return ERROR_OK;
 }
 
+int riscv013_authdata_read(struct target *target, uint32_t *value)
+{
+	if (wait_for_authbusy(target, NULL) != ERROR_OK)
+		return ERROR_FAIL;
+
+	*value = dmi_read(target, DMI_AUTHDATA);
+	return ERROR_OK;
+}
+
+int riscv013_authdata_write(struct target *target, uint32_t value)
+{
+	uint32_t before, after;
+	if (wait_for_authbusy(target, &before) != ERROR_OK)
+		return ERROR_FAIL;
+
+	dmi_write(target, DMI_AUTHDATA, value);
+
+	if (wait_for_authbusy(target, &after) != ERROR_OK)
+		return ERROR_FAIL;
+
+	if (!get_field(before, DMI_DMSTATUS_AUTHENTICATED) &&
+			get_field(after, DMI_DMSTATUS_AUTHENTICATED)) {
+		LOG_INFO("authdata_write resulted in successful authentication");
+		return examine(target);
+	}
+
+	return ERROR_OK;
+}
+
+static int init_target(struct command_context *cmd_ctx,
+		struct target *target)
+{
+	LOG_DEBUG("init");
+	riscv_info_t *generic_info = (riscv_info_t *) target->arch_info;
+
+	generic_info->get_register = &riscv013_get_register;
+	generic_info->set_register = &riscv013_set_register;
+	generic_info->select_current_hart = &riscv013_select_current_hart;
+	generic_info->is_halted = &riscv013_is_halted;
+	generic_info->halt_current_hart = &riscv013_halt_current_hart;
+	generic_info->resume_current_hart = &riscv013_resume_current_hart;
+	generic_info->step_current_hart = &riscv013_step_current_hart;
+	generic_info->on_halt = &riscv013_on_halt;
+	generic_info->on_resume = &riscv013_on_resume;
+	generic_info->on_step = &riscv013_on_step;
+	generic_info->halt_reason = &riscv013_halt_reason;
+	generic_info->read_debug_buffer = &riscv013_read_debug_buffer;
+	generic_info->write_debug_buffer = &riscv013_write_debug_buffer;
+	generic_info->execute_debug_buffer = &riscv013_execute_debug_buffer;
+	generic_info->fill_dmi_write_u64 = &riscv013_fill_dmi_write_u64;
+	generic_info->fill_dmi_read_u64 = &riscv013_fill_dmi_read_u64;
+	generic_info->fill_dmi_nop_u64 = &riscv013_fill_dmi_nop_u64;
+	generic_info->dmi_write_u64_bits = &riscv013_dmi_write_u64_bits;
+	generic_info->authdata_read = &riscv013_authdata_read;
+	generic_info->authdata_write = &riscv013_authdata_write;
+	generic_info->version_specific = calloc(1, sizeof(riscv013_info_t));
+	if (!generic_info->version_specific)
+		return ERROR_FAIL;
+	riscv013_info_t *info = get_info(target);
+
+	info->progbufsize = -1;
+
+	info->dmi_busy_delay = 0;
+	info->bus_master_read_delay = 0;
+	info->bus_master_write_delay = 0;
+	info->ac_busy_delay = 0;
+
+	/* Assume all these abstract commands are supported until we learn
+	 * otherwise.
+	 * TODO: The spec allows eg. one CSR to be able to be accessed abstractly
+	 * while another one isn't. We don't track that this closely here, but in
+	 * the future we probably should. */
+	info->abstract_read_csr_supported = true;
+	info->abstract_write_csr_supported = true;
+	info->abstract_read_fpr_supported = true;
+	info->abstract_write_fpr_supported = true;
+
+	return ERROR_OK;
+}
+
 static int assert_reset(struct target *target)
 {
 	RISCV_INFO(r);
@@ -1416,7 +1537,8 @@ static int deassert_reset(struct target *target)
 	if (target->reset_halt) {
 		LOG_DEBUG("Waiting for hart to be halted.");
 		do {
-			dmstatus = dmi_read(target, DMI_DMSTATUS);
+			if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
+				return ERROR_FAIL;
 			if (time(NULL) - start > riscv_reset_timeout_sec) {
 				LOG_ERROR("Hart didn't halt coming out of reset in %ds; "
 						"dmstatus=0x%x; "
@@ -1433,7 +1555,8 @@ static int deassert_reset(struct target *target)
 	} else {
 		LOG_DEBUG("Waiting for hart to be running.");
 		do {
-			dmstatus = dmi_read(target, DMI_DMSTATUS);
+			if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
+				return ERROR_FAIL;
 			if (get_field(dmstatus, DMI_DMSTATUS_ANYHALTED) ||
 					get_field(dmstatus, DMI_DMSTATUS_ANYUNAVAIL)) {
 				LOG_ERROR("Unexpected hart status during reset. dmstatus=0x%x",
@@ -2306,7 +2429,9 @@ static int riscv013_halt_current_hart(struct target *target)
 			break;
 
 	if (!riscv_is_halted(target)) {
-		uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
+		uint32_t dmstatus;
+		if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
+			return ERROR_FAIL;
 		dmcontrol = dmi_read(target, DMI_DMCONTROL);
 
 		LOG_ERROR("unable to halt hart %d", r->current_hartid);
@@ -2348,7 +2473,9 @@ static int riscv013_on_halt(struct target *target)
 
 static bool riscv013_is_halted(struct target *target)
 {
-	uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
+	uint32_t dmstatus;
+	if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
+		return false;
 	if (get_field(dmstatus, DMI_DMSTATUS_ANYUNAVAIL))
 		LOG_ERROR("hart %d is unavailiable", riscv_current_hartid(target));
 	if (get_field(dmstatus, DMI_DMSTATUS_ANYNONEXISTENT))
@@ -2481,9 +2608,11 @@ static int riscv013_step_or_resume_current_hart(struct target *target, bool step
 	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_RESUMEREQ, 1);
 	dmi_write(target, DMI_DMCONTROL, dmcontrol);
 
+	uint32_t dmstatus;
 	for (size_t i = 0; i < 256; ++i) {
 		usleep(10);
-		uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
+		if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
+			return ERROR_FAIL;
 		if (get_field(dmstatus, DMI_DMSTATUS_ALLRESUMEACK) == 0)
 			continue;
 		if (step && get_field(dmstatus, DMI_DMSTATUS_ALLHALTED) == 0)
@@ -2494,7 +2623,8 @@ static int riscv013_step_or_resume_current_hart(struct target *target, bool step
 		return ERROR_OK;
 	}
 
-	uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
+	if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
+		return ERROR_FAIL;
 	dmcontrol = dmi_read(target, DMI_DMCONTROL);
 	LOG_ERROR("unable to resume hart %d", r->current_hartid);
 	LOG_ERROR("  dmcontrol=0x%08x", dmcontrol);
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 354908a..60191d8 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -1212,6 +1212,7 @@ COMMAND_HANDLER(riscv_set_scratch_ram)
 		return ERROR_OK;
 	}
 
+	// TODO: use COMMAND_PARSE_NUMBER
 	long long unsigned int address;
 	int result = sscanf(CMD_ARGV[0], "%llx", &address);
 	if (result != (int) strlen(CMD_ARGV[0])) {
@@ -1307,6 +1308,58 @@ COMMAND_HANDLER(riscv_set_expose_csrs)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(riscv_authdata_read)
+{
+	if (CMD_ARGC != 0) {
+		LOG_ERROR("Command takes no parameters");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	struct target *target = get_current_target(CMD_CTX);
+	if (!target) {
+		LOG_ERROR("target is NULL!");
+		return ERROR_FAIL;
+	}
+
+	RISCV_INFO(r);
+	if (!r) {
+		LOG_ERROR("riscv_info is NULL!");
+		return ERROR_FAIL;
+	}
+
+	if (r->authdata_read) {
+		uint32_t value;
+		if (r->authdata_read(target, &value) != ERROR_OK)
+			return ERROR_FAIL;
+		command_print(CMD_CTX, "0x%" PRIx32, value);
+		return ERROR_OK;
+	} else {
+		LOG_ERROR("authdata_read is not implemented for this target.");
+		return ERROR_FAIL;
+	}
+}
+
+COMMAND_HANDLER(riscv_authdata_write)
+{
+	if (CMD_ARGC != 1) {
+		LOG_ERROR("Command takes exactly 1 argument");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	struct target *target = get_current_target(CMD_CTX);
+	RISCV_INFO(r);
+
+	uint32_t value;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], value);
+
+	if (r->authdata_write) {
+		return r->authdata_write(target, value);
+	} else {
+		LOG_ERROR("authdata_write is not implemented for this target.");
+		return ERROR_FAIL;
+	}
+}
+
 static const struct command_registration riscv_exec_command_handlers[] = {
 	{
 		.name = "set_command_timeout_sec",
@@ -1333,11 +1386,25 @@ static const struct command_registration riscv_exec_command_handlers[] = {
 		.name = "expose_csrs",
 		.handler = riscv_set_expose_csrs,
 		.mode = COMMAND_ANY,
-		.usage = "riscv expose_csrs n0[-m0][,n0[-m0]]...",
+		.usage = "riscv expose_csrs n0[-m0][,n1[-m1]]...",
 		.help = "Configure a list of inclusive ranges for CSRs to expose in "
 				"addition to the standard ones. This must be executed before "
 				"`init`."
 	},
+	{
+		.name = "authdata_read",
+		.handler = riscv_authdata_read,
+		.mode = COMMAND_ANY,
+		.usage = "riscv authdata_read",
+		.help = "Return the 32-bit value read from authdata."
+	},
+	{
+		.name = "authdata_write",
+		.handler = riscv_authdata_write,
+		.mode = COMMAND_ANY,
+		.usage = "riscv authdata_write value",
+		.help = "Write the 32-bit value to authdata."
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 478731d..a5c8789 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -110,6 +110,9 @@ typedef struct {
 	void (*fill_dmi_write_u64)(struct target *target, char *buf, int a, uint64_t d);
 	void (*fill_dmi_read_u64)(struct target *target, char *buf, int a);
 	void (*fill_dmi_nop_u64)(struct target *target, char *buf);
+
+	int (*authdata_read)(struct target *target, uint32_t *value);
+	int (*authdata_write)(struct target *target, uint32_t value);
 } riscv_info_t;
 
 /* Wall-clock timeout for a command/access. Settable via RISC-V Target commands.*/