target/riscv: Fix memory access when MMU is enabled and address could…

…n't be translated

Now:
1) If mmu is disabled, virt2phys succeeded and returns physical address
2) If mmu is enbaled, but translation fails, read/write_memory fails

Change-Id: I312309c660239014b3278cb77cadc5618de8e4de
Signed-off-by: Kirill Radkin <[email protected]>

src/target/riscv/riscv.c

@@ -2282,17 +2282,19 @@ static int riscv_address_translate(struct target *target,
 static int riscv_virt2phys_v(struct target *target, target_addr_t virtual, target_addr_t *physical)
 {
 	riscv_reg_t vsatp;
-	if (riscv_get_register(target, &vsatp, GDB_REGNO_VSATP) != ERROR_OK) {
-		LOG_TARGET_ERROR(target, "Failed to read vsatp register.");
+	int result = riscv_get_register(target, &vsatp, GDB_REGNO_VSATP);
+	if (result != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to read vsatp register. Error: %d", result);
 		return ERROR_FAIL;
 	}
 	/* vsatp is identical to satp, so we can use the satp macros. */
 	unsigned int xlen = riscv_xlen(target);
 	int vsatp_mode = get_field(vsatp, RISCV_SATP_MODE(xlen));
 	LOG_TARGET_DEBUG(target, "VS-stage translation mode: %d", vsatp_mode);
 	riscv_reg_t hgatp;
-	if (riscv_get_register(target, &hgatp, GDB_REGNO_HGATP) != ERROR_OK) {
-		LOG_TARGET_ERROR(target, "Failed to read hgatp register.");
+	result = riscv_get_register(target, &hgatp, GDB_REGNO_HGATP);
+	if (result != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to read hgatp register. Error: %d", result);
 		return ERROR_FAIL;
 	}
 	int hgatp_mode = get_field(hgatp, RISCV_HGATP_MODE(xlen));
@@ -2315,6 +2317,8 @@ static int riscv_virt2phys_v(struct target *target, target_addr_t virtual, targe
 			break;
 		case SATP_MODE_OFF:
 			vsatp_info = NULL;
+			LOG_TARGET_DEBUG(target, "vsatp mode is %d. No VS-stage translation. (vsatp: 0x%" PRIx64 ")",
+				vsatp_mode, vsatp);
 			break;
 		default:
 			LOG_TARGET_ERROR(target,
@@ -2340,6 +2344,8 @@ static int riscv_virt2phys_v(struct target *target, target_addr_t virtual, targe
 			break;
 		case HGATP_MODE_OFF:
 			hgatp_info = NULL;
+			LOG_TARGET_DEBUG(target, "hgatp mode is %d. No G-stage translation. (hgatp: 0x%" PRIx64 ")",
+				hgatp_mode, hgatp);
 			break;
 		default:
 			LOG_TARGET_ERROR(target,
@@ -2390,23 +2396,28 @@ static int riscv_virt2phys(struct target *target, target_addr_t virtual, target_
 	int enabled;
 	if (riscv_mmu(target, &enabled) != ERROR_OK)
 		return ERROR_FAIL;
-	if (!enabled)
-		return ERROR_FAIL;
-
+	if (!enabled) {
+		*physical = virtual;
+		LOG_TARGET_DEBUG(target, "MMU is disabled. 0x%" TARGET_PRIxADDR " -> 0x%" TARGET_PRIxADDR, virtual, *physical);
+		return ERROR_OK;
+	}
 
 	riscv_reg_t priv;
-	if (riscv_get_register(target, &priv, GDB_REGNO_PRIV) != ERROR_OK) {
-		LOG_TARGET_ERROR(target, "Failed to read priv register.");
+	int result = riscv_get_register(target, &priv, GDB_REGNO_PRIV);
+	if (result != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to read priv register. Error: %d", result);
 		return ERROR_FAIL;
 	}
 
 	if (priv & VIRT_PRIV_V)
 		return riscv_virt2phys_v(target, virtual, physical);
 
 	riscv_reg_t satp_value;
-	int result = riscv_get_register(target, &satp_value, GDB_REGNO_SATP);
-	if (result != ERROR_OK)
-		return result;
+	result = riscv_get_register(target, &satp_value, GDB_REGNO_SATP);
+	if (result != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Failed to read SATP register. Error: %d", result);
+		return ERROR_FAIL;
+	}
 
 	unsigned int xlen = riscv_xlen(target);
 	int satp_mode = get_field(satp_value, RISCV_SATP_MODE(xlen));
@@ -2456,11 +2467,14 @@ static int riscv_read_memory(struct target *target, target_addr_t address,
 	}
 
 	target_addr_t physical_addr;
-	if (target->type->virt2phys(target, address, &physical_addr) == ERROR_OK)
-		address = physical_addr;
+	int result = target->type->virt2phys(target, address, &physical_addr);
+	if (result != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Address translation failed.");
+		return result;
+	}
 
 	RISCV_INFO(r);
-	return r->read_memory(target, address, size, count, buffer, size);
+	return r->read_memory(target, physical_addr, size, count, buffer, size);
 }
 
 static int riscv_write_phys_memory(struct target *target, target_addr_t phys_address,
@@ -2481,8 +2495,11 @@ static int riscv_write_memory(struct target *target, target_addr_t address,
 	}
 
 	target_addr_t physical_addr;
-	if (target->type->virt2phys(target, address, &physical_addr) == ERROR_OK)
-		address = physical_addr;
+	int result = target->type->virt2phys(target, address, &physical_addr);
+	if (result != ERROR_OK) {
+		LOG_TARGET_ERROR(target, "Address translation failed.");
+		return result;
+	}
 
 	struct target_type *tt = get_target_type(target);
 	if (!tt)