target/riscv: fix addressing in dm_read/dm_wirte

There was an error in `dm_read`/`dm_write`: DMI address was checked
against DM registers disregarding DM base address.

To solve the issue `dmi_address()` function was introduced.

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

src/target/riscv/riscv-013.c

@@ -691,6 +691,16 @@ static int dmi_write_exec(struct target *target, uint32_t address,
 	return dmi_op(target, NULL, NULL, DMI_OP_WRITE, address, value, true, ensure_success);
 }
 
+static uint32_t riscv_013_dmi_address(const struct target *target, uint32_t address)
+{
+	assert(target);
+	uint32_t base = 0;
+	RISCV013_INFO(info);
+	if (info && info->dm)
+		base = info->dm->base;
+	return address + base;
+}
+
 static int dm_op_timeout(struct target *target, uint32_t *data_in,
 		bool *dmi_busy_encountered, int op, uint32_t address,
 		uint32_t data_out, int timeout_sec, bool exec, bool ensure_success)
@@ -2769,8 +2779,7 @@ static int init_target(struct command_context *cmd_ctx,
 	generic_info->authdata_write = &riscv013_authdata_write;
 	generic_info->dmi_read = &dmi_read;
 	generic_info->dmi_write = &dmi_write;
-	generic_info->dm_read = &dm_read;
-	generic_info->dm_write = &dm_write;
+	generic_info->dmi_address = &riscv_013_dmi_address;
 	generic_info->read_memory = read_memory;
 	generic_info->hart_count = &riscv013_hart_count;
 	generic_info->data_bits = &riscv013_data_bits;

src/target/riscv/riscv.c

@@ -3690,143 +3690,134 @@ COMMAND_HANDLER(riscv_authdata_write)
 	return r->authdata_write(target, value, index);
 }
 
-COMMAND_HANDLER(riscv_dmi_read)
+uint32_t riscv_dmi_address(const struct target *target, uint32_t address) {
+	assert(target);
+	RISCV_INFO(r);
+	if (!r || !r->dmi_address)
+		return address;
+	return r->dmi_address(target, address);
+}
+
+static int riscv_dmi_read(struct target *target, uint32_t *value, uint32_t address)
 {
-	if (CMD_ARGC != 1) {
-		LOG_ERROR("Command takes 1 parameter");
-		return ERROR_COMMAND_SYNTAX_ERROR;
+	if (!target) {
+		LOG_ERROR("target is NULL!");
+		return ERROR_FAIL;
+	}
+	RISCV_INFO(r);
+	if (!r) {
+		LOG_TARGET_ERROR(target, "riscv_info is NULL!");
+		return ERROR_FAIL;
+	}
+	if (!r->dmi_read) {
+		LOG_TARGET_ERROR(target, "dmi_read is not implemented.");
+		return ERROR_FAIL;
 	}
+	return r->dmi_read(target, value, address);
+}
 
-	struct target *target = get_current_target(CMD_CTX);
+static int riscv_dmi_write(struct target *target, uint32_t address, uint32_t value)
+{
 	if (!target) {
 		LOG_ERROR("target is NULL!");
 		return ERROR_FAIL;
 	}
-
 	RISCV_INFO(r);
 	if (!r) {
 		LOG_TARGET_ERROR(target, "riscv_info is NULL!");
 		return ERROR_FAIL;
 	}
-
-	if (r->dmi_read) {
-		uint32_t address, value;
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
-		if (r->dmi_read(target, &value, address) != ERROR_OK)
-			return ERROR_FAIL;
-		command_print(CMD, "0x%" PRIx32, value);
-	} else {
-		LOG_TARGET_ERROR(target, "dmi_read is not implemented for this target.");
+	if (!r->dmi_write) {
+		LOG_TARGET_ERROR(target, "dmi_write is not implemented.");
 		return ERROR_FAIL;
 	}
-	return ERROR_OK;
+	const int result = r->dmi_write(target, address, value);
+	/* Invalidate our cached progbuf copy:
+	 * - if the user tinkered directly with a progbuf register
+	 * - if debug module was reset, in which case progbuf registers
+	 * may not retain their value.
+	 * FIXME: If there are multiple DMs on a single TAP, it is possible to
+	 * clobber progbuf or reset the DM of another target.
+	 */
+	const bool progbuf_touched = (address >= riscv_dmi_address(target, DM_PROGBUF0) &&
+			address <= riscv_dmi_address(target, DM_PROGBUF15));
+	const bool dm_deactivated = (address == riscv_dmi_address(target, DM_DMCONTROL) &&
+			(value & DM_DMCONTROL_DMACTIVE) == 0);
+	if (progbuf_touched || dm_deactivated) {
+		if (r->invalidate_cached_progbuf) {
+			/* Here the return value of invalidate_cached_progbuf()
+			 * is ignored. It is okay to do so for now, since the
+			 * only case an error is returned is a failure to
+			 * assign a DM to the target, which would have already
+			 * caused an error during dmi_write().
+			 * FIXME: invalidate_cached_progbuf() should be void.
+			 */
+			r->invalidate_cached_progbuf(target);
+		} else {
+			LOG_TARGET_DEBUG(target,
+					"invalidate_cached_progbuf() is not implemented.");
+		}
+	}
+	return result;
 }
 
-COMMAND_HANDLER(riscv_dmi_write)
+COMMAND_HANDLER(handle_riscv_dmi_read)
 {
-	if (CMD_ARGC != 2) {
-		LOG_ERROR("Command takes exactly 2 arguments");
+	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
 
-	struct target *target = get_current_target(CMD_CTX);
-	RISCV_INFO(r);
+	uint32_t address;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+
+	struct target * const target = get_current_target(CMD_CTX);
+	uint32_t value;
+	const int result = riscv_dmi_read(target, &value, address);
+	if (result == ERROR_OK)
+		command_print(CMD, "0x%" PRIx32, value);
+	return result;
+}
+
+COMMAND_HANDLER(handle_riscv_dmi_write)
+{
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	uint32_t address, value;
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
 
-	if (r->dmi_write) {
-		/* Perform the DMI write */
-		int retval = r->dmi_write(target, address, value);
-
-		/* Invalidate our cached progbuf copy:
-		   - if the user tinkered directly with a progbuf register
-		   - if debug module was reset, in which case progbuf registers
-		     may not retain their value.
-		*/
-		bool progbuf_touched = (address >= DM_PROGBUF0 && address <= DM_PROGBUF15);
-		bool dm_deactivated = (address == DM_DMCONTROL && (value & DM_DMCONTROL_DMACTIVE) == 0);
-		if (progbuf_touched || dm_deactivated) {
-			if (r->invalidate_cached_progbuf)
-				r->invalidate_cached_progbuf(target);
-		}
-
-		return retval;
-	}
-
-	LOG_TARGET_ERROR(target, "dmi_write is not implemented for this target.");
-	return ERROR_FAIL;
+	struct target * const target = get_current_target(CMD_CTX);
+	return riscv_dmi_write(target, address, value);
 }
 
-
-COMMAND_HANDLER(riscv_dm_read)
+COMMAND_HANDLER(handle_riscv_dm_read)
 {
-	if (CMD_ARGC != 1) {
-		LOG_ERROR("Command takes 1 parameter");
+	if (CMD_ARGC != 1)
 		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_TARGET_ERROR(target, "riscv_info is NULL!");
-		return ERROR_FAIL;
-	}
+	uint32_t address;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
 
-	if (r->dm_read) {
-		uint32_t address, value;
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
-		if (r->dm_read(target, &value, address) != ERROR_OK)
-			return ERROR_FAIL;
+	struct target * const target = get_current_target(CMD_CTX);
+	uint32_t value;
+	const int result = riscv_dmi_read(target, &value,
+			riscv_dmi_address(target, address));
+	if (result == ERROR_OK)
 		command_print(CMD, "0x%" PRIx32, value);
-	} else {
-		LOG_TARGET_ERROR(target, "dm_read is not implemented for this target.");
-		return ERROR_FAIL;
-	}
-	return ERROR_OK;
+	return result;
 }
 
-COMMAND_HANDLER(riscv_dm_write)
+COMMAND_HANDLER(handle_riscv_dm_write)
 {
-	if (CMD_ARGC != 2) {
-		LOG_ERROR("Command takes exactly 2 arguments");
+	if (CMD_ARGC != 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-
-	struct target *target = get_current_target(CMD_CTX);
-	RISCV_INFO(r);
 
 	uint32_t address, value;
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
 
-	if (r->dm_write) {
-		/* Perform the DM write */
-		int retval = r->dm_write(target, address, value);
-
-		/* Invalidate our cached progbuf copy:
-		   - if the user tinkered directly with a progbuf register
-		   - if debug module was reset, in which case progbuf registers
-		     may not retain their value.
-		*/
-		bool progbuf_touched = (address >= DM_PROGBUF0 && address <= DM_PROGBUF15);
-		bool dm_deactivated = (address == DM_DMCONTROL && (value & DM_DMCONTROL_DMACTIVE) == 0);
-		if (progbuf_touched || dm_deactivated) {
-			if (r->invalidate_cached_progbuf)
-				r->invalidate_cached_progbuf(target);
-		}
-
-		return retval;
-	}
-
-	LOG_TARGET_ERROR(target, "dm_write is not implemented for this target.");
-	return ERROR_FAIL;
+	struct target * const target = get_current_target(CMD_CTX);
+	return riscv_dmi_write(target, riscv_dmi_address(target, address), value);
 }
 
 COMMAND_HANDLER(riscv_reset_delays)
@@ -4653,28 +4644,28 @@ static const struct command_registration riscv_exec_command_handlers[] = {
 	},
 	{
 		.name = "dmi_read",
-		.handler = riscv_dmi_read,
+		.handler = handle_riscv_dmi_read,
 		.mode = COMMAND_ANY,
 		.usage = "address",
 		.help = "Perform a 32-bit DMI read at address, returning the value."
 	},
 	{
 		.name = "dmi_write",
-		.handler = riscv_dmi_write,
+		.handler = handle_riscv_dmi_write,
 		.mode = COMMAND_ANY,
 		.usage = "address value",
 		.help = "Perform a 32-bit DMI write of value at address."
 	},
 	{
 		.name = "dm_read",
-		.handler = riscv_dm_read,
+		.handler = handle_riscv_dm_read,
 		.mode = COMMAND_ANY,
 		.usage = "reg_address",
 		.help = "Perform a 32-bit read from DM register at reg_address, returning the value."
 	},
 	{
 		.name = "dm_write",
-		.handler = riscv_dm_write,
+		.handler = handle_riscv_dm_write,
 		.mode = COMMAND_ANY,
 		.usage = "reg_address value",
 		.help = "Write a 32-bit value to the DM register at reg_address."

src/target/riscv/riscv.h

@@ -242,8 +242,11 @@ struct riscv_info {
 	int (*dmi_read)(struct target *target, uint32_t *value, uint32_t address);
 	int (*dmi_write)(struct target *target, uint32_t address, uint32_t value);
 
-	int (*dm_read)(struct target *target, uint32_t *value, uint32_t address);
-	int (*dm_write)(struct target *target, uint32_t address, uint32_t value);
+	/* Get the DMI address of target's DM's register.
+	 * The function should return the passed address
+	 * if the target is not assigned a DM yet.
+	 */
+	uint32_t (*dmi_address)(const struct target *target, uint32_t address);
 
 	int (*sample_memory)(struct target *target,
 						 struct riscv_sample_buf *buf,