[PATCH 3/3] target/aarch64: ensure MMU is enabled when modifying breakpoints

[Vimoon Z. <vim...@ci...>]

From: "vimoon.zheng" <vim...@ci...>

Introduce aarch64_modify_code to handle code modifications correctly when the MMU is enabled.
This function translates virtual to physical addresses before writing and ensures the MMU remains active.
Also, call instr_cpsr_sync in aarch64_mmu_modify to synchronize CPSR changes if the operation succeeds.
---
 src/target/aarch64.c | 39 ++++++++++++++++++++++++++++++---------
 1 file changed, 30 insertions(+), 9 deletions(-)

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 0ed27dd20..a5de2891d 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -125,6 +125,7 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 {
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct arm_dpm *dpm = &armv8->dpm;
 	int retval = ERROR_OK;
 	enum arm_mode target_mode = ARM_MODE_ANY;
 	uint32_t instr = 0;
@@ -190,6 +191,8 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 
 	if (target_mode != ARM_MODE_ANY)
 		armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
+	if (!retval)
+		retval = dpm->instr_cpsr_sync(dpm);
 
 	return retval;
 }
@@ -1285,6 +1288,30 @@ static int aarch64_restore_context(struct target *target, bool bpwp)
  * Cortex-A8 Breakpoint and watchpoint functions
  */
 
+static int aarch64_modify_code(struct target *target, target_addr_t addr, uint8_t *code, uint32_t size, uint32_t count)
+{
+	int retval;
+	target_addr_t readl_addr;
+	bool mmu_enabled;
+
+	retval = aarch64_mmu(target, &mmu_enabled);
+	if (retval)
+		return retval;
+
+	if (mmu_enabled) {
+		retval = aarch64_virt2phys(target, addr & 0xFFFFFFFFFFFFFFFEULL, &readl_addr);
+		if (retval)
+			return retval;
+		retval = target_write_phys_memory(target, readl_addr, size, count, code);
+		if (retval)
+			return retval;
+		retval = aarch64_mmu_modify(target, 1);
+	} else {
+		retval = target_write_memory(target, addr & 0xFFFFFFFFFFFFFFFEULL, size, count, code);
+	}
+	return retval;
+}
+
 /* Setup hardware Breakpoint Register Pair */
 static int aarch64_set_breakpoint(struct target *target,
 	struct breakpoint *breakpoint, uint8_t matchmode)
@@ -1380,9 +1407,7 @@ static int aarch64_set_breakpoint(struct target *target,
 				breakpoint->address & 0xFFFFFFFFFFFFFFFEULL,
 				breakpoint->length);
 
-		retval = target_write_memory(target,
-				breakpoint->address & 0xFFFFFFFFFFFFFFFEULL,
-				breakpoint->length, 1, code);
+		retval = aarch64_modify_code(target, breakpoint->address, code, breakpoint->length, 1);
 		if (retval != ERROR_OK)
 			return retval;
 
@@ -1651,15 +1676,11 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 				breakpoint->length);
 
 		if (breakpoint->length == 4) {
-			retval = target_write_memory(target,
-					breakpoint->address & 0xFFFFFFFFFFFFFFFEULL,
-					4, 1, breakpoint->orig_instr);
+			retval = aarch64_modify_code(target, breakpoint->address, breakpoint->orig_instr, 4, 1);
 			if (retval != ERROR_OK)
 				return retval;
 		} else {
-			retval = target_write_memory(target,
-					breakpoint->address & 0xFFFFFFFFFFFFFFFEULL,
-					2, 1, breakpoint->orig_instr);
+			retval = aarch64_modify_code(target, breakpoint->address, breakpoint->orig_instr, 2, 1);
 			if (retval != ERROR_OK)
 				return retval;
 		}
-- 
2.52.0