Access memory through the scope of current privilege level (#386)

* add opcode for csrrsi and csrrci

* enable MMU while reading/writing memory using progbuf

* fix style issues

* keep old behavior for progbufsize<4, perform r/w/csr only when necessary

* do not pass progbufsize, only write mstatus if changed

* add config option to enable virtualization feature

* throw error if virt enabled but unavaliable, outsource modify_privilege

* support virtualization for read_memory_progbuf_one

7 files changed, 143 insertions, 0 deletions

diff --git a/doc/openocd.texi b/doc/openocd.texi
index e70b53a..7337686 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -9491,6 +9491,12 @@ When on, prefer to use System Bus Access to access memory.  When off, prefer to
 use the Program Buffer to access memory.
 @end deffn
 
+@deffn Command {riscv riscv_enable_virtual} on|off
+When on, memory accesses are performed on physical or virtual memory depending
+on the current system configuration. When off, all memory accessses are performed
+on physical memory.
+@end deffn
+
 @deffn Command {riscv resume_order} normal|reversed
 Some software assumes all harts are executing nearly continuously. Such
 software may be sensitive to the order that harts are resumed in. On harts
diff --git a/src/target/riscv/opcodes.h b/src/target/riscv/opcodes.h
index de85aad..52e9a3b 100644
--- a/src/target/riscv/opcodes.h
+++ b/src/target/riscv/opcodes.h
@@ -143,6 +143,18 @@ static uint32_t csrrw(unsigned int rd, unsigned int rs, unsigned int csr)
 	return (csr << 20) | (rs << 15) | (rd << 7) | MATCH_CSRRW;
 }
 
+static uint32_t csrrci(unsigned int rd, unsigned int zimm, unsigned int csr) __attribute__ ((unused));
+static uint32_t csrrci(unsigned int rd, unsigned int zimm, unsigned int csr)
+{
+	return (csr << 20) | (zimm << 15) | (rd << 7) | MATCH_CSRRCI;
+}
+
+static uint32_t csrrsi(unsigned int rd, unsigned int zimm, unsigned int csr) __attribute__ ((unused));
+static uint32_t csrrsi(unsigned int rd, unsigned int zimm, unsigned int csr)
+{
+	return (csr << 20) | (zimm << 15) | (rd << 7) | MATCH_CSRRSI;
+}
+
 static uint32_t fsw(unsigned int src, unsigned int base, uint16_t offset) __attribute__ ((unused));
 static uint32_t fsw(unsigned int src, unsigned int base, uint16_t offset)
 {
diff --git a/src/target/riscv/program.c b/src/target/riscv/program.c
index f59ed82..bd08ac1 100644
--- a/src/target/riscv/program.c
+++ b/src/target/riscv/program.c
@@ -109,6 +109,18 @@ int riscv_program_lbr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno
 	return riscv_program_insert(p, lb(d, b, offset));
 }
 
+int riscv_program_csrrsi(struct riscv_program *p, enum gdb_regno d, unsigned int z, enum gdb_regno csr)
+{
+	assert(csr >= GDB_REGNO_CSR0 && csr <= GDB_REGNO_CSR4095);
+	return riscv_program_insert(p, csrrsi(d, z, csr - GDB_REGNO_CSR0));
+}
+
+int riscv_program_csrrci(struct riscv_program *p, enum gdb_regno d, unsigned int z, enum gdb_regno csr)
+{
+	assert(csr >= GDB_REGNO_CSR0 && csr <= GDB_REGNO_CSR4095);
+	return riscv_program_insert(p, csrrci(d, z, csr - GDB_REGNO_CSR0));
+}
+
 int riscv_program_csrr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno csr)
 {
 	assert(csr >= GDB_REGNO_CSR0 && csr <= GDB_REGNO_CSR4095);
diff --git a/src/target/riscv/program.h b/src/target/riscv/program.h
index 310460c..cc3ffd4 100644
--- a/src/target/riscv/program.h
+++ b/src/target/riscv/program.h
@@ -63,6 +63,8 @@ int riscv_program_swr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno
 int riscv_program_shr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
 int riscv_program_sbr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
 
+int riscv_program_csrrsi(struct riscv_program *p, enum gdb_regno d, unsigned int z, enum gdb_regno csr);
+int riscv_program_csrrci(struct riscv_program *p, enum gdb_regno d, unsigned int z, enum gdb_regno csr);
 int riscv_program_csrr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno csr);
 int riscv_program_csrw(struct riscv_program *p, enum gdb_regno s, enum gdb_regno csr);
 
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index cfa9291..6b7da47 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -1580,6 +1580,13 @@ static int examine(struct target *target)
 				r->impebreak);
 	}
 
+	if (info->progbufsize < 4 && riscv_enable_virtual) {
+		LOG_ERROR("set_enable_virtual is not available on this target. It "
+				"requires a program buffer size of at least 4. (progbufsize=%d) "
+				"Use `riscv set_enable_virtual off` to continue."
+					, info->progbufsize);
+	}
+
 	/* Before doing anything else we must first enumerate the harts. */
 	if (dm->hart_count < 0) {
 		for (int i = 0; i < MIN(RISCV_MAX_HARTS, 1 << info->hartsellen); ++i) {
@@ -2083,6 +2090,39 @@ static int read_sbcs_nonbusy(struct target *target, uint32_t *sbcs)
 	}
 }
 
+static int modify_privilege(struct target *target, uint64_t *mstatus, uint64_t *mstatus_old)
+{
+	RISCV013_INFO(info);
+
+	if (riscv_enable_virtual && info->progbufsize >= 4) {
+		/* Read DCSR */
+		uint64_t dcsr;
+		if (register_read(target, &dcsr, GDB_REGNO_DCSR) != ERROR_OK)
+			return ERROR_FAIL;
+
+		/* Read and save MSTATUS */
+		if (register_read(target, mstatus, GDB_REGNO_MSTATUS) != ERROR_OK)
+			return ERROR_FAIL;
+		*mstatus_old = *mstatus;
+
+		/* If we come from m-mode with mprv set, we want to keep mpp */
+		if (get_field(dcsr, DCSR_PRV) < 3) {
+			/* MPP = PRIV */
+			*mstatus = set_field(*mstatus, MSTATUS_MPP, get_field(dcsr, DCSR_PRV));
+
+			/* MPRV = 1 */
+			*mstatus = set_field(*mstatus, MSTATUS_MPRV, 1);
+
+			/* Write MSTATUS */
+			if (*mstatus != *mstatus_old)
+				if (register_write_direct(target, GDB_REGNO_MSTATUS, *mstatus) != ERROR_OK)
+					return ERROR_FAIL;
+		}
+	}
+
+	return ERROR_OK;
+}
+
 static int read_memory_bus_v0(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
@@ -2461,6 +2501,13 @@ error:
 static int read_memory_progbuf_one(struct target *target, target_addr_t address,
 		uint32_t size, uint8_t *buffer)
 {
+	RISCV013_INFO(info);
+
+	uint64_t mstatus = 0;
+	uint64_t mstatus_old = 0;
+	if (modify_privilege(target, &mstatus, &mstatus_old) != ERROR_OK)
+		return ERROR_FAIL;
+
 	uint64_t s0;
 
 	if (register_read(target, &s0, GDB_REGNO_S0) != ERROR_OK)
@@ -2469,6 +2516,8 @@ static int read_memory_progbuf_one(struct target *target, target_addr_t address,
 	/* Write the program (load, increment) */
 	struct riscv_program program;
 	riscv_program_init(&program, target);
+	if (riscv_enable_virtual && info->progbufsize >= 4 && get_field(mstatus, MSTATUS_MPRV))
+		riscv_program_csrrsi(&program, GDB_REGNO_ZERO, CSR_DCSR_MPRVEN, GDB_REGNO_DCSR);
 	switch (size) {
 		case 1:
 			riscv_program_lbr(&program, GDB_REGNO_S0, GDB_REGNO_S0, 0);
@@ -2483,6 +2532,8 @@ static int read_memory_progbuf_one(struct target *target, target_addr_t address,
 			LOG_ERROR("Unsupported size: %d", size);
 			return ERROR_FAIL;
 	}
+	if (riscv_enable_virtual && info->progbufsize >= 4 && get_field(mstatus, MSTATUS_MPRV))
+		riscv_program_csrrci(&program, GDB_REGNO_ZERO,  CSR_DCSR_MPRVEN, GDB_REGNO_DCSR);
 
 	if (riscv_program_ebreak(&program) != ERROR_OK)
 		return ERROR_FAIL;
@@ -2506,6 +2557,11 @@ static int read_memory_progbuf_one(struct target *target, target_addr_t address,
 	if (riscv_set_register(target, GDB_REGNO_S0, s0) != ERROR_OK)
 		return ERROR_FAIL;
 
+	/* Restore MSTATUS */
+	if (mstatus != mstatus_old)
+		if (register_write_direct(target, GDB_REGNO_MSTATUS, mstatus_old))
+			return ERROR_FAIL;
+
 	return ERROR_OK;
 }
 
@@ -2515,6 +2571,8 @@ static int read_memory_progbuf_one(struct target *target, target_addr_t address,
 static int read_memory_progbuf(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
+	RISCV013_INFO(info);
+
 	int result = ERROR_OK;
 
 	LOG_DEBUG("reading %d words of %d bytes from 0x%" TARGET_PRIxADDR, count,
@@ -2530,6 +2588,11 @@ static int read_memory_progbuf(struct target *target, target_addr_t address,
 	if (count == 1)
 		return read_memory_progbuf_one(target, address, size, buffer);
 
+	uint64_t mstatus = 0;
+	uint64_t mstatus_old = 0;
+	if (modify_privilege(target, &mstatus, &mstatus_old) != ERROR_OK)
+		return ERROR_FAIL;
+
 	/* s0 holds the next address to write to
 	 * s1 holds the next data value to write
 	 */
@@ -2542,6 +2605,9 @@ static int read_memory_progbuf(struct target *target, target_addr_t address,
 	/* Write the program (load, increment) */
 	struct riscv_program program;
 	riscv_program_init(&program, target);
+	if (riscv_enable_virtual && info->progbufsize >= 4 && get_field(mstatus, MSTATUS_MPRV))
+		riscv_program_csrrsi(&program, GDB_REGNO_ZERO, CSR_DCSR_MPRVEN, GDB_REGNO_DCSR);
+
 	switch (size) {
 		case 1:
 			riscv_program_lbr(&program, GDB_REGNO_S1, GDB_REGNO_S0, 0);
@@ -2556,6 +2622,8 @@ static int read_memory_progbuf(struct target *target, target_addr_t address,
 			LOG_ERROR("Unsupported size: %d", size);
 			return ERROR_FAIL;
 	}
+	if (riscv_enable_virtual && info->progbufsize >= 4 && get_field(mstatus, MSTATUS_MPRV))
+		riscv_program_csrrci(&program, GDB_REGNO_ZERO,  CSR_DCSR_MPRVEN, GDB_REGNO_DCSR);
 	riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, size);
 
 	if (riscv_program_ebreak(&program) != ERROR_OK)
@@ -2593,6 +2661,12 @@ static int read_memory_progbuf(struct target *target, target_addr_t address,
 
 	riscv_set_register(target, GDB_REGNO_S0, s0);
 	riscv_set_register(target, GDB_REGNO_S1, s1);
+
+	/* Restore MSTATUS */
+	if (mstatus != mstatus_old)
+		if (register_write_direct(target, GDB_REGNO_MSTATUS, mstatus_old))
+			return ERROR_FAIL;
+
 	return result;
 }
 
@@ -2796,6 +2870,11 @@ static int write_memory_progbuf(struct target *target, target_addr_t address,
 
 	select_dmi(target);
 
+	uint64_t mstatus = 0;
+	uint64_t mstatus_old = 0;
+	if (modify_privilege(target, &mstatus, &mstatus_old) != ERROR_OK)
+		return ERROR_FAIL;
+
 	/* s0 holds the next address to write to
 	 * s1 holds the next data value to write
 	 */
@@ -2810,6 +2889,8 @@ static int write_memory_progbuf(struct target *target, target_addr_t address,
 	/* Write the program (store, increment) */
 	struct riscv_program program;
 	riscv_program_init(&program, target);
+	if (riscv_enable_virtual && info->progbufsize >= 4 && get_field(mstatus, MSTATUS_MPRV))
+		riscv_program_csrrsi(&program, GDB_REGNO_ZERO, CSR_DCSR_MPRVEN, GDB_REGNO_DCSR);
 
 	switch (size) {
 		case 1:
@@ -2827,6 +2908,8 @@ static int write_memory_progbuf(struct target *target, target_addr_t address,
 			goto error;
 	}
 
+	if (riscv_enable_virtual && info->progbufsize >= 4 && get_field(mstatus, MSTATUS_MPRV))
+		riscv_program_csrrci(&program, GDB_REGNO_ZERO,  CSR_DCSR_MPRVEN, GDB_REGNO_DCSR);
 	riscv_program_addi(&program, GDB_REGNO_S0, GDB_REGNO_S0, size);
 
 	result = riscv_program_ebreak(&program);
@@ -2963,6 +3046,11 @@ error:
 	if (register_write_direct(target, GDB_REGNO_S0, s0) != ERROR_OK)
 		return ERROR_FAIL;
 
+	/* Restore MSTATUS */
+	if (mstatus != mstatus_old)
+		if (register_write_direct(target, GDB_REGNO_MSTATUS, mstatus_old))
+			return ERROR_FAIL;
+
 	if (execute_fence(target) != ERROR_OK)
 		return ERROR_FAIL;
 
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index 2e3813d..86a6dc3 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -252,6 +252,8 @@ int riscv_reset_timeout_sec = DEFAULT_RESET_TIMEOUT_SEC;
 
 bool riscv_prefer_sba;
 
+bool riscv_enable_virtual;
+
 typedef struct {
 	uint16_t low, high;
 } range_t;
@@ -1884,6 +1886,16 @@ COMMAND_HANDLER(riscv_set_prefer_sba)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(riscv_set_enable_virtual)
+{
+	if (CMD_ARGC != 1) {
+		LOG_ERROR("Command takes exactly 1 parameter");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+	COMMAND_PARSE_ON_OFF(CMD_ARGV[0], riscv_enable_virtual);
+	return ERROR_OK;
+}
+
 void parse_error(const char *string, char c, unsigned position)
 {
 	char buf[position+2];
@@ -2234,6 +2246,15 @@ static const struct command_registration riscv_exec_command_handlers[] = {
 			"When off, prefer to use the Program Buffer to access memory."
 	},
 	{
+		.name = "set_enable_virtual",
+		.handler = riscv_set_enable_virtual,
+		.mode = COMMAND_ANY,
+		.usage = "riscv set_enable_virtual on|off",
+		.help = "When on, memory accesses are performed on physical or virtual "
+				"memory depending on the current system configuration. "
+				"When off, all memory accessses are performed on physical memory."
+	},
+	{
 		.name = "expose_csrs",
 		.handler = riscv_set_expose_csrs,
 		.mode = COMMAND_ANY,
diff --git a/src/target/riscv/riscv.h b/src/target/riscv/riscv.h
index 53ee1f4..a0d912c 100644
--- a/src/target/riscv/riscv.h
+++ b/src/target/riscv/riscv.h
@@ -155,6 +155,8 @@ extern int riscv_reset_timeout_sec;
 
 extern bool riscv_prefer_sba;
 
+extern bool riscv_enable_virtual;
+
 /* 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));