Reading v* registers appears to work.vector2

Can't really test it though, because gdb doesn't print them right.

Change-Id: I8d66339371c564a493d32f15c3d114b738a455c5

4 files changed, 234 insertions, 127 deletions

diff --git a/src/target/riscv/program.c b/src/target/riscv/program.c
index 8645ed6..b370401 100644
--- a/src/target/riscv/program.c
+++ b/src/target/riscv/program.c
@@ -56,7 +56,8 @@ int riscv_program_exec(struct riscv_program *p, struct target *t)
 	if (riscv_program_ebreak(p) != ERROR_OK) {
 		LOG_ERROR("Unable to write ebreak");
 		for (size_t i = 0; i < riscv_debug_buffer_size(p->target); ++i)
-			LOG_ERROR("ram[%02x]: DASM(0x%08lx) [0x%08lx]", (int)i, (long)p->debug_buffer[i], (long)p->debug_buffer[i]);
+			LOG_ERROR("ram[%02x]: DASM(0x%08lx) [0x%08lx]", (int)i,
+					(long)p->debug_buffer[i], (long)p->debug_buffer[i]);
 		return ERROR_FAIL;
 	}
 
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index ba3a3a0..271dced 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -1055,6 +1055,54 @@ static int examine_progbuf(struct target *target)
 	return ERROR_OK;
 }
 
+static int is_fpu_reg(uint32_t gdb_regno)
+{
+	return (gdb_regno >= GDB_REGNO_FPR0 && gdb_regno <= GDB_REGNO_FPR31) ||
+		(gdb_regno == GDB_REGNO_CSR0 + CSR_FFLAGS) ||
+		(gdb_regno == GDB_REGNO_CSR0 + CSR_FRM) ||
+		(gdb_regno == GDB_REGNO_CSR0 + CSR_FCSR);
+}
+
+static int is_vector_reg(uint32_t gdb_regno)
+{
+	return (gdb_regno >= GDB_REGNO_V0 && gdb_regno <= GDB_REGNO_V31) ||
+		gdb_regno == GDB_REGNO_VSTART ||
+		gdb_regno == GDB_REGNO_VXSAT ||
+		gdb_regno == GDB_REGNO_VXRM ||
+		gdb_regno == GDB_REGNO_VL ||
+		gdb_regno == GDB_REGNO_VTYPE ||
+		gdb_regno == GDB_REGNO_VLENB;
+}
+
+int prep_for_register_access(struct target *target, uint64_t *mstatus,
+		int regno)
+{
+	if (is_fpu_reg(regno) || is_vector_reg(regno)) {
+		if (register_read(target, mstatus, GDB_REGNO_MSTATUS) != ERROR_OK)
+			return ERROR_FAIL;
+		if (is_fpu_reg(regno) && (*mstatus & MSTATUS_FS) == 0) {
+			if (register_write_direct(target, GDB_REGNO_MSTATUS,
+						set_field(*mstatus, MSTATUS_FS, 1)) != ERROR_OK)
+				return ERROR_FAIL;
+		} else if (is_vector_reg(regno) && (*mstatus & MSTATUS_VS) == 0) {
+			if (register_write_direct(target, GDB_REGNO_MSTATUS,
+						set_field(*mstatus, MSTATUS_VS, 1)) != ERROR_OK)
+				return ERROR_FAIL;
+		}
+	}
+	return ERROR_OK;
+}
+
+int cleanup_after_register_access(struct target *target, uint64_t mstatus,
+		int regno)
+{
+	if ((is_fpu_reg(regno) && (mstatus & MSTATUS_FS) == 0) ||
+			(is_vector_reg(regno) && (mstatus & MSTATUS_VS) == 0))
+		if (register_write_direct(target, GDB_REGNO_MSTATUS, mstatus) != ERROR_OK)
+			return ERROR_FAIL;
+	return ERROR_OK;
+}
+
 typedef enum {
 	SPACE_DMI_DATA,
 	SPACE_DMI_PROGBUF,
@@ -1253,6 +1301,10 @@ static int register_write_direct(struct target *target, unsigned number,
 	if (register_read(target, &s0, GDB_REGNO_S0) != ERROR_OK)
 		return ERROR_FAIL;
 
+	uint64_t mstatus;
+	if (prep_for_register_access(target, &mstatus, number) != ERROR_OK)
+		return ERROR_FAIL;
+
 	scratch_mem_t scratch;
 	bool use_scratch = false;
 	if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31 &&
@@ -1277,6 +1329,10 @@ static int register_write_direct(struct target *target, unsigned number,
 			return ERROR_FAIL;
 		}
 
+	} else if (number == GDB_REGNO_VTYPE) {
+		riscv_program_insert(&program, csrr(S0, CSR_VL));
+		riscv_program_insert(&program, vsetvli(ZERO, S0, value));
+
 	} else {
 		if (register_write_direct(target, GDB_REGNO_S0, value) != ERROR_OK)
 			return ERROR_FAIL;
@@ -1313,6 +1369,9 @@ static int register_write_direct(struct target *target, unsigned number,
 	if (use_scratch)
 		scratch_release(target, &scratch);
 
+	if (cleanup_after_register_access(target, mstatus, number) != ERROR_OK)
+		return ERROR_FAIL;
+
 	/* Restore S0. */
 	if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
 		return ERROR_FAIL;
@@ -1337,25 +1396,6 @@ static int register_read(struct target *target, uint64_t *value, uint32_t number
 	return ERROR_OK;
 }
 
-static int is_fpu_reg(uint32_t gdb_regno)
-{
-	return (gdb_regno >= GDB_REGNO_FPR0 && gdb_regno <= GDB_REGNO_FPR31) ||
-		(gdb_regno == GDB_REGNO_CSR0 + CSR_FFLAGS) ||
-		(gdb_regno == GDB_REGNO_CSR0 + CSR_FRM) ||
-		(gdb_regno == GDB_REGNO_CSR0 + CSR_FCSR);
-}
-
-static int is_vector_reg(uint32_t gdb_regno)
-{
-	return (gdb_regno >= GDB_REGNO_V0 && gdb_regno <= GDB_REGNO_V31) ||
-		gdb_regno == GDB_REGNO_VSTART ||
-		gdb_regno == GDB_REGNO_VXSAT ||
-		gdb_regno == GDB_REGNO_VXRM ||
-		gdb_regno == GDB_REGNO_VL ||
-		gdb_regno == GDB_REGNO_VTYPE ||
-		gdb_regno == GDB_REGNO_VLENB;
-}
-
 /** Actually read registers from the target right now. */
 static int register_read_direct(struct target *target, uint64_t *value, uint32_t number)
 {
@@ -1381,19 +1421,8 @@ static int register_read_direct(struct target *target, uint64_t *value, uint32_t
 		/* Write program to move data into s0. */
 
 		uint64_t mstatus;
-		if (is_fpu_reg(number) || is_vector_reg(number)) {
-			if (register_read(target, &mstatus, GDB_REGNO_MSTATUS) != ERROR_OK)
-				return ERROR_FAIL;
-			if (is_fpu_reg(number) && (mstatus & MSTATUS_FS) == 0) {
-				if (register_write_direct(target, GDB_REGNO_MSTATUS,
-							set_field(mstatus, MSTATUS_FS, 1)) != ERROR_OK)
-					return ERROR_FAIL;
-			} else if (is_vector_reg(number) && (mstatus & MSTATUS_VS) == 0) {
-				if (register_write_direct(target, GDB_REGNO_MSTATUS,
-							set_field(mstatus, MSTATUS_VS, 1)) != ERROR_OK)
-					return ERROR_FAIL;
-			}
-		}
+		if (prep_for_register_access(target, &mstatus, number) != ERROR_OK)
+			return ERROR_FAIL;
 
 		if (number >= GDB_REGNO_FPR0 && number <= GDB_REGNO_FPR31) {
 			if (riscv_supports_extension(target, riscv_current_hartid(target), 'D')
@@ -1420,78 +1449,6 @@ static int register_read_direct(struct target *target, uint64_t *value, uint32_t
 			}
 		} else if (number >= GDB_REGNO_CSR0 && number <= GDB_REGNO_CSR4095) {
 			riscv_program_csrr(&program, S0, number);
-		} else if (number >= GDB_REGNO_V0 && number <= GDB_REGNO_V31) {
-			/*
-			 * The idea is that you read a vector register destructively: read
-			 * element 0 using vmv.x.s into t0; send t0 to the debugger; then
-			 * vslide1down with t0 as the scalar argument. This is effectively
-			 * rotating the vector one element at a time, so after vl steps,
-			 * the vector register is back to its original value."
-			 *
-			 * The two instructions vmv.x.s and vmv.s.x are described in
-			 * section 17.1.
-			 *
-			 * The vmv.x.s instruction copies a single SEW-wide element from
-			 * index 0 of the source vector register to a destination integer
-			 * register. 
-			 *
-			 * The vmv.s.x instruction copies the scalar integer register to
-			 * element 0 of the destination vector register. 
-			 *
-			 * Executing the two instructions in the PROGBUF and reading or
-			 * writing the x register in a loop would not be that much slower
-			 * than executing the instruction to move a vector reg to memory
-			 * then reading memory sequentially.
-			 *
-			 * I recommend not using memory-based vector register read/writes -
-			 * it would add user complication to have to allocate target memory
-			 * for a vector register buffer just for debug - and require adding
-			 * to the linker script to do this allocation and then informing
-			 * the debugger where it is. */
-			/* TODO: this continuous save/restore is terrible for performance. */
-
-			uint64_t vtype, vl;
-			/* Save vtype and vl. */
-			if (register_read(target, &vtype, GDB_REGNO_VTYPE) != ERROR_OK)
-				return ERROR_FAIL;
-			if (register_read(target, &vl, GDB_REGNO_VL) != ERROR_OK)
-				return ERROR_FAIL;
-
-			/* Restore vtype and vl. */
-			unsigned encoded_vsew;
-			switch (riscv_xlen(target)) {
-				case 32:
-					encoded_vsew = 2;
-					break;
-				case 64:
-					encoded_vsew = 3;
-					break;
-				default:
-					LOG_ERROR("Unsupported xlen: %d", riscv_xlen(target));
-					return ERROR_FAIL;
-			}
-			if (register_write_direct(target, GDB_REGNO_VTYPE, encoded_vsew << 2) != ERROR_OK)
-				return ERROR_FAIL;
-			unsigned debug_vl = DIV_ROUND_UP(r->vlenb[r->current_hartid] * 8,
-					riscv_xlen(target));
-			if (register_write_direct(target, GDB_REGNO_VL, debug_vl) != ERROR_OK)
-				return ERROR_FAIL;
-
-			unsigned vnum = number - GDB_REGNO_V0;
-			riscv_program_insert(&program, vmv_x_s(S0, vnum));
-			riscv_program_insert(&program, vslide1down_vx(vnum, vnum, S0, false));
-			for (unsigned i = 0; i < debug_vl; i++) {
-				if (riscv_program_exec(&program, target) != ERROR_OK)
-					return ERROR_FAIL;
-				if (register_read_direct(target, value, GDB_REGNO_S0) != ERROR_OK)
-					return ERROR_FAIL;
-			}
-
-			/* Restore vtype and vl. */
-			if (register_write_direct(target, GDB_REGNO_VTYPE, vtype) != ERROR_OK)
-				return ERROR_FAIL;
-			if (register_write_direct(target, GDB_REGNO_VL, vl) != ERROR_OK)
-				return ERROR_FAIL;
 		} else {
 			LOG_ERROR("Unsupported register: %s", gdb_regno_name(number));
 			return ERROR_FAIL;
@@ -1512,10 +1469,8 @@ static int register_read_direct(struct target *target, uint64_t *value, uint32_t
 				return ERROR_FAIL;
 		}
 
-		if ((is_fpu_reg(number) && (mstatus & MSTATUS_FS) == 0) ||
-				(is_vector_reg(number) && (mstatus & MSTATUS_VS) == 0))
-			if (register_write_direct(target, GDB_REGNO_MSTATUS, mstatus) != ERROR_OK)
-				return ERROR_FAIL;
+		if (cleanup_after_register_access(target, mstatus, number) != ERROR_OK)
+			return ERROR_FAIL;
 
 		/* Restore S0. */
 		if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
@@ -1881,6 +1836,114 @@ static unsigned riscv013_data_bits(struct target *target)
 	return riscv_xlen(target);
 }
 
+static int riscv013_get_register_buf(struct target *target,
+		uint8_t *value, int regno)
+{
+	RISCV_INFO(r);
+	assert(regno >= GDB_REGNO_V0 && regno <= GDB_REGNO_V31);
+
+	riscv_reg_t s0;
+	if (register_read(target, &s0, GDB_REGNO_S0) != ERROR_OK)
+		return ERROR_FAIL;
+
+	uint64_t mstatus;
+	if (prep_for_register_access(target, &mstatus, regno) != ERROR_OK)
+		return ERROR_FAIL;
+
+	/*
+	 * The idea is that you read a vector register destructively: read
+	 * element 0 using vmv.x.s into t0; send t0 to the debugger; then
+	 * vslide1down with t0 as the scalar argument. This is effectively
+	 * rotating the vector one element at a time, so after vl steps,
+	 * the vector register is back to its original value."
+	 *
+	 * The two instructions vmv.x.s and vmv.s.x are described in
+	 * section 17.1.
+	 *
+	 * The vmv.x.s instruction copies a single SEW-wide element from
+	 * index 0 of the source vector register to a destination integer
+	 * register.
+	 *
+	 * The vmv.s.x instruction copies the scalar integer register to
+	 * element 0 of the destination vector register.
+	 *
+	 * Executing the two instructions in the PROGBUF and reading or
+	 * writing the x register in a loop would not be that much slower
+	 * than executing the instruction to move a vector reg to memory
+	 * then reading memory sequentially.
+	 *
+	 * I recommend not using memory-based vector register read/writes -
+	 * it would add user complication to have to allocate target memory
+	 * for a vector register buffer just for debug - and require adding
+	 * to the linker script to do this allocation and then informing
+	 * the debugger where it is. */
+
+	/* TODO: this continuous save/restore is terrible for performance. */
+
+	uint64_t vtype, vl;
+	/* Save vtype and vl. */
+	if (register_read(target, &vtype, GDB_REGNO_VTYPE) != ERROR_OK)
+		return ERROR_FAIL;
+	if (register_read(target, &vl, GDB_REGNO_VL) != ERROR_OK)
+		return ERROR_FAIL;
+
+	/* Write vtype and vl. */
+	unsigned encoded_vsew;
+	switch (riscv_xlen(target)) {
+		case 32:
+			encoded_vsew = 2;
+			break;
+		case 64:
+			encoded_vsew = 3;
+			break;
+		default:
+			LOG_ERROR("Unsupported xlen: %d", riscv_xlen(target));
+			return ERROR_FAIL;
+	}
+	if (register_write_direct(target, GDB_REGNO_VTYPE, encoded_vsew << 2) != ERROR_OK)
+		return ERROR_FAIL;
+	unsigned debug_vl = DIV_ROUND_UP(r->vlenb[r->current_hartid] * 8,
+			riscv_xlen(target));
+	if (register_write_direct(target, GDB_REGNO_VL, debug_vl) != ERROR_OK)
+		return ERROR_FAIL;
+
+	unsigned vnum = regno - GDB_REGNO_V0;
+	unsigned xlen = riscv_xlen(target);
+
+	struct riscv_program program;
+	riscv_program_init(&program, target);
+	riscv_program_insert(&program, vmv_x_s(S0, vnum));
+	riscv_program_insert(&program, vslide1down_vx(vnum, vnum, S0, false));
+	for (unsigned i = 0; i < debug_vl; i++) {
+		if (riscv_program_exec(&program, target) != ERROR_OK)
+			return ERROR_FAIL;
+		uint64_t v;
+		if (register_read_direct(target, &v, GDB_REGNO_S0) != ERROR_OK)
+			return ERROR_FAIL;
+		buf_set_u64(value, xlen * i, xlen, v);
+	}
+
+	/* Restore vtype and vl. */
+	if (register_write_direct(target, GDB_REGNO_VTYPE, vtype) != ERROR_OK)
+		return ERROR_FAIL;
+	if (register_write_direct(target, GDB_REGNO_VL, vl) != ERROR_OK)
+		return ERROR_FAIL;
+
+	if (cleanup_after_register_access(target, mstatus, regno) != ERROR_OK)
+		return ERROR_FAIL;
+	if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
+		return ERROR_FAIL;
+
+	return ERROR_OK;
+}
+
+static int riscv013_set_register_buf(struct target *target,
+		int regno, const uint8_t *value)
+{
+	assert(regno >= GDB_REGNO_V0 && regno <= GDB_REGNO_V31);
+	return ERROR_FAIL;
+}
+
 static int init_target(struct command_context *cmd_ctx,
 		struct target *target)
 {
@@ -1889,6 +1952,8 @@ static int init_target(struct command_context *cmd_ctx,
 
 	generic_info->get_register = &riscv013_get_register;
 	generic_info->set_register = &riscv013_set_register;
+	generic_info->get_register_buf = &riscv013_get_register_buf;
+	generic_info->set_register_buf = &riscv013_set_register_buf;
 	generic_info->select_current_hart = &riscv013_select_current_hart;
 	generic_info->is_halted = &riscv013_is_halted;
 	generic_info->resume_go = &riscv013_resume_go;
@@ -3505,10 +3570,12 @@ static int riscv013_get_register(struct target *target,
 
 	int result = ERROR_OK;
 	if (rid == GDB_REGNO_PC) {
+		/* TODO: move this into riscv.c. */
 		result = register_read(target, value, GDB_REGNO_DPC);
 		LOG_DEBUG("[%d] read PC from DPC: 0x%" PRIx64, target->coreid, *value);
 	} else if (rid == GDB_REGNO_PRIV) {
 		uint64_t dcsr;
+		/* TODO: move this into riscv.c. */
 		result = register_read(target, &dcsr, GDB_REGNO_DCSR);
 		*value = get_field(dcsr, CSR_DCSR_PRV);
 	} else {
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index c26ba56..afcbf91 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -3229,6 +3229,10 @@ const char *gdb_regno_name(enum gdb_regno regno)
 			return "priv";
 		case GDB_REGNO_SATP:
 			return "satp";
+		case GDB_REGNO_VTYPE:
+			return "vtype";
+		case GDB_REGNO_VL:
+			return "vl";
 		case GDB_REGNO_V0:
 			return "v0";
 		case GDB_REGNO_V1:
@@ -3310,20 +3314,35 @@ static int register_get(struct reg *reg)
 {
 	riscv_reg_info_t *reg_info = reg->arch_info;
 	struct target *target = reg_info->target;
-	uint64_t value;
-	int result = riscv_get_register(target, &value, reg->number);
-	if (result != ERROR_OK)
-		return result;
-	buf_set_u64(reg->value, 0, reg->size, value);
+	RISCV_INFO(r);
+
+	if (reg->number >= GDB_REGNO_V0 && reg->number <= GDB_REGNO_V31) {
+		if (!r->get_register_buf) {
+			LOG_ERROR("Reading register %s not supported on this RISC-V target.",
+					gdb_regno_name(reg->number));
+			return ERROR_FAIL;
+		}
+
+		if (r->get_register_buf(target, reg->value, reg->number) != ERROR_OK)
+			return ERROR_FAIL;
+	} else {
+		uint64_t value;
+		int result = riscv_get_register(target, &value, reg->number);
+		if (result != ERROR_OK)
+			return result;
+		buf_set_u64(reg->value, 0, reg->size, value);
+	}
 	/* CSRs (and possibly other extension) registers may change value at any
 	 * time. */
 	if (reg->number <= GDB_REGNO_XPR31 ||
 			(reg->number >= GDB_REGNO_FPR0 && reg->number <= GDB_REGNO_FPR31) ||
+			(reg->number >= GDB_REGNO_V0 && reg->number <= GDB_REGNO_V31) ||
 			reg->number == GDB_REGNO_PC)
 		reg->valid = true;
-	LOG_DEBUG("[%d]{%d} read 0x%" PRIx64 " from %s (valid=%d)",
-			target->coreid, riscv_current_hartid(target), value, reg->name,
-			reg->valid);
+	char *str = buf_to_str(reg->value, reg->size, 16);
+	LOG_DEBUG("[%d]{%d} read 0x%s from %s (valid=%d)", target->coreid,
+			riscv_current_hartid(target), str, reg->name, reg->valid);
+	free(str);
 	return ERROR_OK;
 }
 
@@ -3331,22 +3350,37 @@ static int register_set(struct reg *reg, uint8_t *buf)
 {
 	riscv_reg_info_t *reg_info = reg->arch_info;
 	struct target *target = reg_info->target;
+	RISCV_INFO(r);
 
-	uint64_t value = buf_get_u64(buf, 0, reg->size);
+	char *str = buf_to_str(reg->value, reg->size, 16);
+	LOG_DEBUG("[%d]{%d} write 0x%s to %s (valid=%d)", target->coreid,
+			riscv_current_hartid(target), str, reg->name, reg->valid);
+	free(str);
 
-	LOG_DEBUG("[%d]{%d} write 0x%" PRIx64 " to %s (valid=%d)",
-			target->coreid, riscv_current_hartid(target), value, reg->name,
-			reg->valid);
-	struct reg *r = &target->reg_cache->reg_list[reg->number];
+	memcpy(reg->value, buf, DIV_ROUND_UP(reg->size, 8));
 	/* CSRs (and possibly other extension) registers may change value at any
 	 * time. */
 	if (reg->number <= GDB_REGNO_XPR31 ||
 			(reg->number >= GDB_REGNO_FPR0 && reg->number <= GDB_REGNO_FPR31) ||
+			(reg->number >= GDB_REGNO_V0 && reg->number <= GDB_REGNO_V31) ||
 			reg->number == GDB_REGNO_PC)
-		r->valid = true;
-	memcpy(r->value, buf, (r->size + 7) / 8);
+		reg->valid = true;
+
+	if (reg->number >= GDB_REGNO_V0 && reg->number <= GDB_REGNO_V31) {
+		if (!r->set_register_buf) {
+			LOG_ERROR("Reading register %s not supported on this RISC-V target.",
+					gdb_regno_name(reg->number));
+			return ERROR_FAIL;
+		}
+
+		if (r->set_register_buf(target, reg->number, reg->value) != ERROR_OK)
+			return ERROR_FAIL;
+	} else {
+		uint64_t value = buf_get_u64(buf, 0, reg->size);
+		if (riscv_set_register(target, reg->number, value) != ERROR_OK)
+			return ERROR_FAIL;
+	}
 
-	riscv_set_register(target, reg->number, value);
 	return ERROR_OK;
 }
 
@@ -3835,9 +3869,11 @@ int riscv_init_registers(struct target *target)
 			r->size = 8;
 
 		} else if (number >= GDB_REGNO_V0 && number <= GDB_REGNO_V31) {
-			r->caller_save = true;
+			/* TODO: These should be caller saved, but if I say it's true then
+			 * gdb only tries to save 32 bits, which makes OpenOCD unhappy when
+			 * it tries to write a 32-bit value to a larger register. */
+			r->caller_save = false;
 			r->exist = riscv_supports_extension(target, hartid, 'V');
-			//r->reg_data_type = &type_arch_defined;
 			r->size = info->vlenb[hartid] * 8;
 			sprintf(reg_name, "v%d", number - GDB_REGNO_V0);
 			r->group = "vector";
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 509c3d7..c9e15a6 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -111,6 +111,9 @@ typedef struct {
 		riscv_reg_t *value, int hid, int rid);
 	int (*set_register)(struct target *, int hartid, int regid,
 			uint64_t value);
+	int (*get_register_buf)(struct target *target, uint8_t *buf, int regno);
+	int (*set_register_buf)(struct target *target, int regno,
+			const uint8_t *buf);
 	int (*select_current_hart)(struct target *);
 	bool (*is_halted)(struct target *target);
 	/* Resume this target, as well as every other prepped target that can be