Support 64-bit FPRs on RV32.

Because there is no instruction that moves just half of a 64-bit FPR
to/from a GPR, we need to use scratch memory for this operation. This
code can theoretically use:
1. DMI_DATA, if it is memory mapped in the target.
2. DMI_PROGBUF, if it is writable in the target.
3. A user-configured address.

I have only tested this code very lightly. One reason is that gdb thinks
that on RV32 harts every register is 32 bits wide. Another is that this
is mostly proof-of-concept to satisfy the small program buffer code
review, which I don't want to drag out forever.

Existing tests don't realize that floating support was broken with
RV32D, and don't realize that it still doesn't work because of the gdb
problem mentioned above.

This change improves Issue #110 but there's more work to be done.

Change-Id: I99b8a36e5fea26f1d9e16e36cf99adc7be26b944

3 files changed, 328 insertions, 34 deletions

diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 5ff4a6f..efcc895 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -28,6 +28,7 @@
 #include "batch.h"
 
 #define DMI_DATA1 (DMI_DATA0 + 1)
+#define DMI_PROGBUF1 (DMI_PROGBUF0 + 1)
 
 static void riscv013_on_step_or_resume(struct target *target, bool step);
 static void riscv013_step_or_resume_current_hart(struct target *target, bool step);
@@ -55,6 +56,12 @@ static void riscv013_fill_dmi_read_u64(struct target *target, char *buf, int a);
 static int riscv013_dmi_write_u64_bits(struct target *target);
 static void riscv013_fill_dmi_nop_u64(struct target *target, char *buf);
 static int register_read_direct(struct target *target, uint64_t *value, uint32_t number);
+static int register_write_direct(struct target *target, unsigned number,
+		uint64_t value);
+static int read_memory(struct target *target, target_addr_t address,
+		uint32_t size, uint32_t count, uint8_t *buffer);
+static int write_memory(struct target *target, target_addr_t address,
+		uint32_t size, uint32_t count, const uint8_t *buffer);
 
 /**
  * Since almost everything can be accomplish by scanning the dbus register, all
@@ -127,6 +134,12 @@ struct memory_cache_line {
 	bool dirty;
 };
 
+typedef enum {
+	YNM_MAYBE,
+	YNM_YES,
+	YNM_NO
+} yes_no_maybe_t;
+
 typedef struct {
 	/* Number of address bits in the dbus register. */
 	unsigned abits;
@@ -143,6 +156,10 @@ typedef struct {
 	 * reg_cache. */
 	uint64_t mstatus_actual;
 
+	yes_no_maybe_t progbuf_writable;
+	/* We only need the address so that we know the alignment of the buffer. */
+	riscv_addr_t progbuf_address;
+
 	/* Single buffer that contains all register names, instead of calling
 	 * malloc for each register. Needs to be freed when reg_list is freed. */
 	char *reg_names;
@@ -175,7 +192,12 @@ typedef struct {
 	// When a function returns some error due to a failure indicated by the
 	// target in cmderr, the caller can look here to see what that error was.
 	// (Compare with errno.)
-	unsigned cmderr;
+	uint8_t cmderr;
+
+	// Some fields from hartinfo.
+	uint8_t datasize;
+	uint8_t dataaccess;
+	int16_t dataaddr;
 } riscv013_info_t;
 
 static void decode_dmi(char *text, unsigned address, unsigned data)
@@ -737,6 +759,200 @@ static int register_write_abstract(struct target *target, uint32_t number,
 	return ERROR_OK;
 }
 
+static int examine_progbuf(struct target *target)
+{
+	riscv013_info_t *info = get_info(target);
+
+	if (info->progbuf_writable != YNM_MAYBE)
+		return ERROR_OK;
+
+	// Figure out if progbuf is writable.
+
+	if (info->progbufsize < 1) {
+		info->progbuf_writable = YNM_NO;
+		LOG_INFO("No program buffer present.");
+		return ERROR_OK;
+	}
+
+	uint64_t s0;
+	if (register_read_direct(target, &s0, GDB_REGNO_S0) != ERROR_OK)
+		return ERROR_FAIL;
+
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	riscv_program_insert(&program, auipc(S0));
+	if (riscv_program_exec(&program, target) != ERROR_OK)
+		return ERROR_FAIL;
+
+	if (register_read_direct(target, &info->progbuf_address, GDB_REGNO_S0) != ERROR_OK)
+		return ERROR_FAIL;
+
+	riscv_program_init(&program, target);
+	riscv_program_insert(&program, sw(S0, S0, 0));
+	int result = riscv_program_exec(&program, target);
+
+	if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
+		return ERROR_FAIL;
+
+	if (result != ERROR_OK) {
+		// This program might have failed if the program buffer is not
+		// writable.
+		info->progbuf_writable = YNM_NO;
+		return ERROR_OK;
+	}
+
+	uint32_t written = dmi_read(target, DMI_PROGBUF0);
+	if (written == (uint32_t) info->progbuf_address) {
+		LOG_INFO("progbuf is writable at 0x%" TARGET_PRIxADDR,
+				info->progbuf_address);
+		info->progbuf_writable = YNM_YES;
+
+	} else {
+		LOG_INFO("progbuf is not writeable at 0x%" TARGET_PRIxADDR,
+				info->progbuf_address);
+		info->progbuf_writable = YNM_NO;
+	}
+
+	return ERROR_OK;
+}
+
+typedef enum {
+	SPACE_DMI_DATA,
+	SPACE_DMI_PROGBUF,
+	SPACE_DMI_RAM
+} memory_space_t;
+
+typedef struct {
+	// How can the debugger access this memory?
+	memory_space_t memory_space;
+	// Memory address to access the scratch memory from the hart.
+	riscv_addr_t hart_address;
+	// Memory address to access the scratch memory from the debugger.
+	riscv_addr_t debug_address;
+} scratch_mem_t;
+
+/**
+ * Find some scratch memory to be used with the given program.
+ */
+static int scratch_find(struct target *target,
+		scratch_mem_t *scratch,
+		struct riscv_program *program,
+		unsigned size_bytes)
+{
+	riscv013_info_t *info = get_info(target);
+
+	riscv_addr_t alignment = 1;
+	while (alignment < size_bytes)
+		alignment *= 2;
+
+	if (info->dataaccess == 1) {
+		// Sign extend dataaddr.
+		scratch->hart_address = info->dataaddr;
+		if (info->dataaddr & (1<<11)) {
+			scratch->hart_address |= 0xfffffffffffff000ULL;
+		}
+		// Align.
+		scratch->hart_address = (scratch->hart_address + alignment - 1) & ~(alignment - 1);
+
+		if ((size_bytes + scratch->hart_address - info->dataaddr + 3) / 4 >=
+				info->datasize) {
+			scratch->memory_space = SPACE_DMI_DATA;
+			scratch->debug_address = (scratch->hart_address - info->dataaddr) / 4;
+			return ERROR_OK;
+		}
+	}
+
+	if (examine_progbuf(target) != ERROR_OK)
+		return ERROR_FAIL;
+
+	// Allow for ebreak at the end of the program.
+	unsigned program_size = (program->instruction_count + 1 ) * 4;
+	scratch->hart_address = (info->progbuf_address + program_size + alignment - 1) &
+		~(alignment - 1);
+	if ((size_bytes + scratch->hart_address - info->progbuf_address + 3) / 4 >=
+			info->progbufsize) {
+		scratch->memory_space = SPACE_DMI_PROGBUF;
+		scratch->debug_address = (scratch->hart_address - info->progbuf_address) / 4;
+		return ERROR_OK;
+	}
+
+	if (riscv_use_scratch_ram) {
+		scratch->hart_address = (riscv_use_scratch_ram + alignment - 1) &
+			~(alignment - 1);
+		scratch->memory_space = SPACE_DMI_RAM;
+		scratch->debug_address = scratch->hart_address;
+	}
+
+	LOG_ERROR("Couldn't find %d bytes of scratch RAM to use. Please configure "
+			"an address with 'riscv set_scratch_ram'.", size_bytes);
+	return ERROR_FAIL;
+}
+
+static int scratch_read64(struct target *target, scratch_mem_t *scratch,
+		uint64_t *value)
+{
+	switch (scratch->memory_space) {
+		case SPACE_DMI_DATA:
+			*value = dmi_read(target, DMI_DATA0 + scratch->debug_address);
+			*value |= ((uint64_t) dmi_read(target, DMI_DATA1 +
+						scratch->debug_address)) << 32;
+			break;
+		case SPACE_DMI_PROGBUF:
+			*value = dmi_read(target, DMI_PROGBUF0 + scratch->debug_address);
+			*value |= ((uint64_t) dmi_read(target, DMI_PROGBUF1 +
+						scratch->debug_address)) << 32;
+			break;
+		case SPACE_DMI_RAM:
+			{
+				uint8_t buffer[8];
+				if (read_memory(target, scratch->debug_address, 4, 2, buffer) != ERROR_OK)
+					return ERROR_FAIL;
+				*value = buffer[0] |
+					(((uint64_t) buffer[1]) << 8) |
+					(((uint64_t) buffer[2]) << 16) |
+					(((uint64_t) buffer[3]) << 24) |
+					(((uint64_t) buffer[4]) << 32) |
+					(((uint64_t) buffer[5]) << 40) |
+					(((uint64_t) buffer[6]) << 48) |
+					(((uint64_t) buffer[7]) << 56);
+			}
+			break;
+	}
+	return ERROR_OK;
+}
+
+static int scratch_write64(struct target *target, scratch_mem_t *scratch,
+		uint64_t value)
+{
+	switch (scratch->memory_space) {
+		case SPACE_DMI_DATA:
+			dmi_write(target, DMI_DATA0 + scratch->debug_address, value);
+			dmi_write(target, DMI_DATA1 + scratch->debug_address, value >> 32);
+			break;
+		case SPACE_DMI_PROGBUF:
+			dmi_write(target, DMI_PROGBUF0 + scratch->debug_address, value);
+			dmi_write(target, DMI_PROGBUF1 + scratch->debug_address, value >> 32);
+			break;
+		case SPACE_DMI_RAM:
+			{
+				uint8_t buffer[8] = {
+					value,
+					value >> 8,
+					value >> 16,
+					value >> 24,
+					value >> 32,
+					value >> 40,
+					value >> 48,
+					value >> 56
+				};
+				if (write_memory(target, scratch->debug_address, 4, 2, buffer) != ERROR_OK)
+					return ERROR_FAIL;
+			}
+			break;
+	}
+	return ERROR_OK;
+}
+
 static int register_write_direct(struct target *target, unsigned number,
 		uint64_t value)
 {
@@ -749,33 +965,49 @@ static int register_write_direct(struct target *target, unsigned number,
 		return ERROR_OK;
 
 	struct riscv_program program;
-
 	riscv_program_init(&program, target);
 
 	uint64_t s0;
 	if (register_read_direct(target, &s0, GDB_REGNO_S0) != ERROR_OK)
 		return ERROR_FAIL;
 
-	if (register_write_direct(target, GDB_REGNO_S0, value) != ERROR_OK)
-		return ERROR_FAIL;
+	if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31 &&
+			supports_extension(target, 'D') &&
+			riscv_xlen(target) < 64) {
+		/* There are no instructions to move all the bits from a register, so
+		 * we need to use some scratch RAM. */
+		riscv_program_insert(&program, fld(number - GDB_REGNO_FPR0, S0, 0));
+
+		scratch_mem_t scratch;
+		if (scratch_find(target, &scratch, &program, 8) != ERROR_OK)
+			return ERROR_FAIL;
+
+		if (register_write_direct(target, GDB_REGNO_S0, scratch.hart_address)
+				!= ERROR_OK)
+			return ERROR_FAIL;
+
+		if (scratch_write64(target, &scratch, value) != ERROR_OK)
+			return ERROR_FAIL;
+
+	} else {
+		if (register_write_direct(target, GDB_REGNO_S0, value) != ERROR_OK)
+			return ERROR_FAIL;
 
-	if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
-		if (supports_extension(target, 'D') && riscv_xlen(target) >= 64) {
-			riscv_program_insert(&program, fmv_d_x(number - GDB_REGNO_FPR0, S0));
+		if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
+			if (supports_extension(target, 'D')) {
+				riscv_program_insert(&program, fmv_d_x(number - GDB_REGNO_FPR0, S0));
+			} else {
+				riscv_program_insert(&program, fmv_s_x(number - GDB_REGNO_FPR0, S0));
+			}
+		} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
+			riscv_program_csrw(&program, S0, number);
 		} else {
-			riscv_program_insert(&program, fmv_s_x(number - GDB_REGNO_FPR0, S0));
+			LOG_ERROR("Unsupported register (enum gdb_regno)(%d)", number);
+			abort();
 		}
-	} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
-		riscv_program_csrw(&program, S0, number);
-	} else {
-		LOG_ERROR("Unsupported register (enum gdb_regno)(%d)", number);
-		abort();
 	}
 
 	int exec_out = riscv_program_exec(&program, target);
-	if (exec_out != ERROR_OK) {
-		riscv013_clear_abstract_error(target);
-	}
 
 	// Restore S0.
 	if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
@@ -791,21 +1023,38 @@ static int register_read_direct(struct target *target, uint64_t *value, uint32_t
 			riscv_xlen(target));
 
 	if (result != ERROR_OK) {
+		assert(number != GDB_REGNO_S0);
+
 		result = ERROR_OK;
 
 		struct riscv_program program;
 		riscv_program_init(&program, target);
-		assert(number != GDB_REGNO_S0);
+
+		scratch_mem_t scratch;
+		bool use_scratch = false;
 
 		uint64_t s0;
 		if (register_read_direct(target, &s0, GDB_REGNO_S0) != ERROR_OK)
 			return ERROR_FAIL;
 
 		// Write program to move data into s0.
+
 		if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
 			// TODO: Possibly set F in mstatus.
-			// TODO: Fully support D extension on RV32.
-			if (supports_extension(target, 'D') && riscv_xlen(target) >= 64) {
+			if (supports_extension(target, 'D') && riscv_xlen(target) < 64) {
+				/* There are no instructions to move all the bits from a
+				 * register, so we need to use some scratch RAM. */
+				riscv_program_insert(&program, fsd(number - GDB_REGNO_FPR0, S0,
+							0));
+
+				if (scratch_find(target, &scratch, &program, 8) != ERROR_OK)
+					return ERROR_FAIL;
+				use_scratch = true;
+
+				if (register_write_direct(target, GDB_REGNO_S0,
+							scratch.hart_address) != ERROR_OK)
+					return ERROR_FAIL;
+			} else if (supports_extension(target, 'D')) {
 				riscv_program_insert(&program, fmv_x_d(S0, number - GDB_REGNO_FPR0));
 			} else {
 				riscv_program_insert(&program, fmv_x_s(S0, number - GDB_REGNO_FPR0));
@@ -819,13 +1068,16 @@ static int register_read_direct(struct target *target, uint64_t *value, uint32_t
 
 		// Execute program.
 		result = riscv_program_exec(&program, target);
-		if (result != ERROR_OK) {
-			riscv013_clear_abstract_error(target);
+
+		if (use_scratch) {
+			if (scratch_read64(target, &scratch, value) != ERROR_OK)
+				return ERROR_FAIL;
+		} else {
+			// Read S0
+			if (register_read_direct(target, value, GDB_REGNO_S0) != ERROR_OK)
+				return ERROR_FAIL;
 		}
 
-		// Read S0
-		if (register_read_direct(target, value, GDB_REGNO_S0) != ERROR_OK)
-			return ERROR_FAIL;
 		// Restore S0.
 		if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
 			return ERROR_FAIL;
@@ -993,7 +1245,6 @@ 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 dmcontrol = dmi_read(target, DMI_DMCONTROL);
 	uint32_t dmstatus = dmi_read(target, DMI_DMSTATUS);
 	if (get_field(dmstatus, DMI_DMSTATUS_VERSION) != 2) {
 		LOG_ERROR("OpenOCD only supports Debug Module version 2, not %d "
@@ -1004,10 +1255,17 @@ static int examine(struct target *target)
 	// Reset the Debug Module.
 	dmi_write(target, DMI_DMCONTROL, 0);
 	dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE);
-	dmcontrol = dmi_read(target, DMI_DMCONTROL);
+	uint32_t dmcontrol = dmi_read(target, DMI_DMCONTROL);
+
+	uint32_t hartinfo = dmi_read(target, DMI_HARTINFO);
 
 	LOG_DEBUG("dmcontrol: 0x%08x", dmcontrol);
 	LOG_DEBUG("dmstatus:  0x%08x", dmstatus);
+	LOG_DEBUG("hartinfo:  0x%08x", hartinfo);
+
+	info->datasize = get_field(hartinfo, DMI_HARTINFO_DATASIZE);
+	info->dataaccess = get_field(hartinfo, DMI_HARTINFO_DATAACCESS);
+	info->dataaddr = get_field(hartinfo, DMI_HARTINFO_DATAADDR);
 
 	if (!get_field(dmcontrol, DMI_DMCONTROL_DMACTIVE)) {
 		LOG_ERROR("Debug Module did not become active. dmcontrol=0x%x",
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 820094d..b9be133 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -200,6 +200,9 @@ int riscv_command_timeout_sec = DEFAULT_COMMAND_TIMEOUT_SEC;
 /* Wall-clock timeout after reset. Settable via RISC-V Target commands.*/
 int riscv_reset_timeout_sec = DEFAULT_RESET_TIMEOUT_SEC;
 
+bool riscv_use_scratch_ram = false;
+uint64_t riscv_scratch_ram_address = 0;
+
 static uint32_t dtmcontrol_scan(struct target *target, uint32_t out)
 {
 	struct scan_field field;
@@ -1120,8 +1123,8 @@ int riscv_openocd_step(
 }
 
 /* Command Handlers */
-COMMAND_HANDLER(riscv_set_command_timeout_sec) {
-
+COMMAND_HANDLER(riscv_set_command_timeout_sec)
+{
 	if (CMD_ARGC != 1) {
 		LOG_ERROR("Command takes exactly 1 parameter");
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -1137,11 +1140,11 @@ COMMAND_HANDLER(riscv_set_command_timeout_sec) {
 	return ERROR_OK;
 }
 
-COMMAND_HANDLER(riscv_set_reset_timeout_sec) {
-
+COMMAND_HANDLER(riscv_set_reset_timeout_sec)
+{
 	if (CMD_ARGC != 1) {
-			LOG_ERROR("Command takes exactly 1 parameter");
-			return ERROR_COMMAND_SYNTAX_ERROR;
+		LOG_ERROR("Command takes exactly 1 parameter");
+		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 	int timeout = atoi(CMD_ARGV[0]);
 	if (timeout <= 0){
@@ -1153,6 +1156,29 @@ COMMAND_HANDLER(riscv_set_reset_timeout_sec) {
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(riscv_set_scratch_ram)
+{
+	if (CMD_ARGC != 1) {
+		LOG_ERROR("Command takes exactly 1 parameter");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+	if (!strcmp(CMD_ARGV[0], "none")) {
+		riscv_use_scratch_ram = false;
+		return ERROR_OK;
+	}
+
+	long long unsigned int address;
+	int result = sscanf(CMD_ARGV[0], "%Lx", &address);
+	if (result != (int) strlen(CMD_ARGV[0])) {
+		LOG_ERROR("%s is not a valid address for command.", CMD_ARGV[0]);
+		riscv_use_scratch_ram = false;
+		return ERROR_FAIL;
+	}
+
+	riscv_scratch_ram_address = address;
+	riscv_use_scratch_ram = true;
+	return ERROR_OK;
+}
 
 static const struct command_registration riscv_exec_command_handlers[] = {
 	{
@@ -1161,14 +1187,21 @@ static const struct command_registration riscv_exec_command_handlers[] = {
 		.mode = COMMAND_ANY,
 		.usage = "riscv set_command_timeout_sec [sec]",
 		.help = "Set the wall-clock timeout (in seconds) for individual commands"
-	 },
-	 {
+	},
+	{
 		.name = "set_reset_timeout_sec",
 		.handler = riscv_set_reset_timeout_sec,
 		.mode = COMMAND_ANY,
 		.usage = "riscv set_reset_timeout_sec [sec]",
 		.help = "Set the wall-clock timeout (in seconds) after reset is deasserted"
 	},
+	{
+		.name = "set_scratch_ram",
+		.handler = riscv_set_scratch_ram,
+		.mode = COMMAND_ANY,
+		.usage = "riscv set_scratch_ram none|[address]",
+		.help = "Set address of 16 bytes of scratch RAM the debugger can use, or 'none'."
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index f19f06c..4ff6127 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -111,6 +111,9 @@ extern int riscv_command_timeout_sec;
 /* Wall-clock timeout after reset. Settable via RISC-V Target commands.*/
 extern int riscv_reset_timeout_sec;
 
+extern bool riscv_use_scratch_ram;
+extern uint64_t riscv_scratch_ram_address;
+
 /* Everything needs the RISC-V specific info structure, so here's a nice macro
  * that provides that. */
 static inline riscv_info_t *riscv_info(const struct target *target) __attribute__((unused));