target/riscv: improve register caching (prep_*, cleanup_*)

Introduce riscv_write_register to prep_for_register/vector_access and
cleanup_after_register/vector_access.

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

1 files changed, 104 insertions, 87 deletions

diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index 0c94728..ca80e8a 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -1065,35 +1065,54 @@ static int is_vector_reg(enum gdb_regno gdb_regno)
 		gdb_regno == GDB_REGNO_VLENB;
 }
 
-static int prep_for_register_access(struct target *target, riscv_reg_t *mstatus,
-		enum gdb_regno regno)
+static int prep_for_register_access(struct target *target,
+		riscv_reg_t *orig_mstatus, enum gdb_regno regno)
 {
-	if (is_fpu_reg(regno) || is_vector_reg(regno)) {
-		if (register_read_direct(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;
-		}
-	} else {
-		*mstatus = 0;
-	}
+	assert(orig_mstatus);
+
+	if (!is_fpu_reg(regno) && !is_vector_reg(regno))
+		/* No special preparation needed */
+		return ERROR_OK;
+
+	LOG_TARGET_DEBUG(target, "Preparing mstatus to access %s",
+			gdb_regno_name(regno));
+
+	/* FIXME: On a running target, there is no way to make sure mstatus won't
+	 * change between reading and writing it, so
+	 * if target->state != TARGET_HALTED an error should be returned here.
+	 * However, this would not allow access to FPU registers on the running
+	 * target.
+	 * See https://github.com/riscv/riscv-openocd/pull/842#discussion_r1178500114
+	 */
+
+	if (riscv_get_register(target, orig_mstatus, GDB_REGNO_MSTATUS) != ERROR_OK)
+		return ERROR_FAIL;
+
+	riscv_reg_t new_mstatus = *orig_mstatus;
+	riscv_reg_t field_mask = is_fpu_reg(regno) ? MSTATUS_FS : MSTATUS_VS;
+
+	if ((new_mstatus & field_mask) != 0)
+		return ERROR_OK;
+
+	new_mstatus = set_field(new_mstatus, field_mask, 1);
+
+	if (riscv_write_register(target, GDB_REGNO_MSTATUS, new_mstatus) != ERROR_OK)
+		return ERROR_FAIL;
+
+	LOG_TARGET_DEBUG(target, "Prepared to access %s (mstatus=0x%" PRIx64 ")",
+			gdb_regno_name(regno), new_mstatus);
 	return ERROR_OK;
 }
 
 static int cleanup_after_register_access(struct target *target,
 		riscv_reg_t mstatus, enum gdb_regno 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;
+	if (!is_fpu_reg(regno) && !is_vector_reg(regno))
+		/* Mstatus was not changed for this register access. No need to restore it. */
+		return ERROR_OK;
+
+	LOG_TARGET_DEBUG(target, "Restoring mstatus to 0x%" PRIx64, mstatus);
+	return riscv_write_register(target, GDB_REGNO_MSTATUS, mstatus);
 }
 
 typedef enum {
@@ -1913,69 +1932,78 @@ static COMMAND_HELPER(riscv013_print_info, struct target *target)
 	return 0;
 }
 
-static int prep_for_vector_access(struct target *target, riscv_reg_t *saved_vtype,
-		riscv_reg_t *saved_vl, unsigned int *debug_vl, unsigned int *debug_vsew)
+static int try_set_vsew(struct target *target, unsigned int *debug_vsew)
 {
 	RISCV_INFO(r);
-	/* TODO: this continuous save/restore is terrible for performance. */
-	/* Write vtype and vl. */
+	unsigned int encoded_vsew =
+		(riscv_xlen(target) == 64 && r->vsew64_supported != YNM_NO) ? 3 : 2;
 
-	/* Save vtype and vl. */
-	if (register_read_direct(target, saved_vtype, GDB_REGNO_VTYPE) != ERROR_OK)
-		return ERROR_FAIL;
-	if (register_read_direct(target, saved_vl, GDB_REGNO_VL) != ERROR_OK)
+	/* Set standard element width to match XLEN, for vmv instruction to move
+	 * the least significant bits into a GPR.
+	 */
+	if (riscv_write_register(target, GDB_REGNO_VTYPE, encoded_vsew << 3) != ERROR_OK)
 		return ERROR_FAIL;
 
-	while (true) {
-		unsigned int encoded_vsew;
-		if (riscv_xlen(target) == 64 && r->vsew64_supported != YNM_NO) {
-			encoded_vsew = 3;
-			*debug_vsew = 64;
-		} else {
-			encoded_vsew = 2;
-			*debug_vsew = 32;
-		}
+	if (encoded_vsew == 3 && r->vsew64_supported == YNM_MAYBE) {
+		/* Check that it's supported. */
+		riscv_reg_t vtype;
 
-		/* Set standard element width to match XLEN, for vmv instruction to move
-		 * the least significant bits into a GPR. */
-		if (register_write_direct(target, GDB_REGNO_VTYPE, encoded_vsew << 3) != ERROR_OK)
+		if (riscv_get_register(target, &vtype, GDB_REGNO_VTYPE) != ERROR_OK)
 			return ERROR_FAIL;
-
-		if (*debug_vsew == 64 && r->vsew64_supported == YNM_MAYBE) {
-			/* Check that it's supported. */
-			uint64_t vtype;
-			if (register_read_direct(target, &vtype, GDB_REGNO_VTYPE) != ERROR_OK)
-				return ERROR_FAIL;
-			if (vtype >> (riscv_xlen(target) - 1)) {
-				r->vsew64_supported = YNM_NO;
-				/* Try again. */
-				continue;
-			} else {
-				r->vsew64_supported = YNM_YES;
-			}
+		if (vtype >> (riscv_xlen(target) - 1)) {
+			r->vsew64_supported = YNM_NO;
+			/* Try again. */
+			return try_set_vsew(target, debug_vsew);
 		}
-		break;
+		r->vsew64_supported = YNM_YES;
+	}
+	*debug_vsew = encoded_vsew == 3 ? 64 : 32;
+	return ERROR_OK;
+}
+
+static int prep_for_vector_access(struct target *target,
+		riscv_reg_t *orig_mstatus, riscv_reg_t *orig_vtype, riscv_reg_t *orig_vl,
+		unsigned int *debug_vl, unsigned int *debug_vsew)
+{
+	assert(orig_mstatus);
+	assert(orig_vtype);
+	assert(orig_vl);
+	assert(debug_vl);
+	assert(debug_vsew);
+
+	RISCV_INFO(r);
+	if (target->state != TARGET_HALTED) {
+		LOG_TARGET_ERROR(target,
+				"Unable to access vector register: target not halted");
+		return ERROR_FAIL;
 	}
+	if (prep_for_register_access(target, orig_mstatus, GDB_REGNO_VL) != ERROR_OK)
+		return ERROR_FAIL;
+
+	/* Save vtype and vl. */
+	if (riscv_get_register(target, orig_vtype, GDB_REGNO_VTYPE) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_get_register(target, orig_vl, GDB_REGNO_VL) != ERROR_OK)
+		return ERROR_FAIL;
 
+	if (try_set_vsew(target, debug_vsew) != ERROR_OK)
+		return ERROR_FAIL;
 	/* Set the number of elements to be updated with results from a vector
 	 * instruction, for the vslide1down instruction.
 	 * Set it so the entire V register is updated. */
 	*debug_vl = DIV_ROUND_UP(r->vlenb * 8, *debug_vsew);
-	if (register_write_direct(target, GDB_REGNO_VL, *debug_vl) != ERROR_OK)
-		return ERROR_FAIL;
-
-	return ERROR_OK;
+	return riscv_write_register(target, GDB_REGNO_VL, *debug_vl);
 }
 
-static int cleanup_after_vector_access(struct target *target, riscv_reg_t vtype,
-		riscv_reg_t vl)
+static int cleanup_after_vector_access(struct target *target,
+		riscv_reg_t mstatus, riscv_reg_t vtype, riscv_reg_t vl)
 {
 	/* Restore vtype and vl. */
-	if (register_write_direct(target, GDB_REGNO_VTYPE, vtype) != ERROR_OK)
+	if (riscv_write_register(target, GDB_REGNO_VTYPE, vtype) != ERROR_OK)
 		return ERROR_FAIL;
-	if (register_write_direct(target, GDB_REGNO_VL, vl) != ERROR_OK)
+	if (riscv_write_register(target, GDB_REGNO_VL, vl) != ERROR_OK)
 		return ERROR_FAIL;
-	return ERROR_OK;
+	return cleanup_after_register_access(target, mstatus, GDB_REGNO_VL);
 }
 
 static int riscv013_get_register_buf(struct target *target,
@@ -1989,13 +2017,10 @@ static int riscv013_get_register_buf(struct target *target,
 	if (riscv_save_register(target, GDB_REGNO_S0) != ERROR_OK)
 		return ERROR_FAIL;
 
-	riscv_reg_t mstatus;
-	if (prep_for_register_access(target, &mstatus, regno) != ERROR_OK)
-		return ERROR_FAIL;
-
-	riscv_reg_t vtype, vl;
+	riscv_reg_t mstatus, vtype, vl;
 	unsigned int debug_vl, debug_vsew;
-	if (prep_for_vector_access(target, &vtype, &vl, &debug_vl, &debug_vsew) != ERROR_OK)
+	if (prep_for_vector_access(target, &mstatus, &vtype, &vl,
+				&debug_vl, &debug_vsew) != ERROR_OK)
 		return ERROR_FAIL;
 
 	unsigned vnum = regno - GDB_REGNO_V0;
@@ -2022,16 +2047,14 @@ static int riscv013_get_register_buf(struct target *target,
 				return ERROR_FAIL;
 			buf_set_u64(value, debug_vsew * i, debug_vsew, v);
 		} else {
-			LOG_TARGET_ERROR(target, "Failed to execute vmv/vslide1down while reading %s",
-					 gdb_regno_name(regno));
+			LOG_TARGET_ERROR(target,
+					"Failed to execute vmv/vslide1down while reading %s",
+					gdb_regno_name(regno));
 			break;
 		}
 	}
 
-	if (cleanup_after_vector_access(target, vtype, vl) != ERROR_OK)
-		return ERROR_FAIL;
-
-	if (cleanup_after_register_access(target, mstatus, regno) != ERROR_OK)
+	if (cleanup_after_vector_access(target, mstatus, vtype, vl) != ERROR_OK)
 		return ERROR_FAIL;
 
 	return result;
@@ -2048,13 +2071,10 @@ static int riscv013_set_register_buf(struct target *target,
 	if (riscv_save_register(target, GDB_REGNO_S0) != ERROR_OK)
 		return ERROR_FAIL;
 
-	riscv_reg_t mstatus;
-	if (prep_for_register_access(target, &mstatus, regno) != ERROR_OK)
-		return ERROR_FAIL;
-
-	riscv_reg_t vtype, vl;
+	riscv_reg_t mstatus, vtype, vl;
 	unsigned int debug_vl, debug_vsew;
-	if (prep_for_vector_access(target, &vtype, &vl, &debug_vl, &debug_vsew) != ERROR_OK)
+	if (prep_for_vector_access(target, &mstatus, &vtype, &vl,
+				&debug_vl, &debug_vsew) != ERROR_OK)
 		return ERROR_FAIL;
 
 	unsigned vnum = regno - GDB_REGNO_V0;
@@ -2072,10 +2092,7 @@ static int riscv013_set_register_buf(struct target *target,
 			break;
 	}
 
-	if (cleanup_after_vector_access(target, vtype, vl) != ERROR_OK)
-		return ERROR_FAIL;
-
-	if (cleanup_after_register_access(target, mstatus, regno) != ERROR_OK)
+	if (cleanup_after_vector_access(target, mstatus, vtype, vl) != ERROR_OK)
 		return ERROR_FAIL;
 
 	return result;