cortexar: improved virt_to_phys function

- halts/resumes within the function if needed
- checks whether the MMU is enabled before using MMU machinery

src/target/cortexar.c

@@ -316,6 +316,11 @@ static const uint16_t cortexar_spsr_encodings[5] = {
 /* Address Translate Stage 1 Current state PL1 Read */
 #define CORTEXAR_ATS1CPR 15U, ENCODE_CP_REG(7U, 8U, 0U, 0U)
 
+/* SCTLR System Control Register */
+#define CORTEXAR_SCTLR 15U, ENCODE_CP_REG(1U, 0U, 0U, 0U)
+
+#define CORTEXAR_SCTLR_MMU_ENABLED (1U << 0U)
+
 #define CORTEXAR_CPACR_CP10_FULL_ACCESS 0x00300000U
 #define CORTEXAR_CPACR_CP11_FULL_ACCESS 0x00c00000U
 
@@ -390,6 +395,7 @@ static void cortexar_reset(target_s *target);
 static target_halt_reason_e cortexar_halt_poll(target_s *target, target_addr_t *watch);
 static void cortexar_halt_request(target_s *target);
 static void cortexar_halt_resume(target_s *target, bool step);
+static bool cortexar_halt_and_wait(target_s *target);
 
 static int cortexar_breakwatch_set(target_s *target, breakwatch_s *breakwatch);
 static int cortexar_breakwatch_clear(target_s *target, breakwatch_s *breakwatch);
@@ -638,6 +644,54 @@ static void cortexar_coproc_write(target_s *const target, const uint8_t coproc,
 			ENCODE_CP_ACCESS(coproc & 0xfU, (op >> 8U) & 0x7U, 0U, (op >> 4U) & 0xfU, op & 0xfU, (op >> 12U) & 0x7U));
 }
 
+/*
+ * Perform a virtual to physical address translation.
+ * NB: Halts target if needed to ensure operations succeed. Trashes r0.
+ */
+static target_addr_t cortexar_virt_to_phys(target_s *const target, const target_addr_t virt_addr)
+{
+	/* Check if the target is PMSA and return early if it is */
+	if (!(target->target_options & TOPT_FLAVOUR_VIRT_MEM))
+		return virt_addr;
+
+	/* Halt if not already halted to ensure coproc read/write operations succeed */
+	const bool halted_in_function = cortexar_halt_and_wait(target);
+
+	/* Check if MMU is turned on; if not, return the virt_addr as-is. */
+	const uint32_t sctlr = cortexar_coproc_read(target, CORTEXAR_SCTLR);
+	if (!(sctlr & CORTEXAR_SCTLR_MMU_ENABLED)) {
+		return virt_addr;
+	}
+
+	/*
+	 * Now we know the target is VMSA and so has the address translation machinery,
+	 * start by loading r0 with the VA to translate and request its translation
+	 */
+	cortexar_core_reg_write(target, 0U, virt_addr);
+	cortexar_coproc_write(target, CORTEXAR_ATS1CPR, 0U);
+	/*
+	 * Ensure that's complete with a sync barrier, then read the result back
+	 * from the physical address register into r0 so we can extract the result
+	 */
+	cortexar_run_insn(target, ARM_ISB_INSN);
+	cortexar_coproc_read(target, CORTEXAR_PAR32);
+
+	const uint32_t phys_addr = cortexar_core_reg_read(target, 0U);
+	/* Check if the MMU indicated a translation failure, marking a fault if it did */
+	if (phys_addr & CORTEXAR_PAR32_FAULT) {
+		cortexar_priv_s *const priv = (cortexar_priv_s *)target->priv;
+		priv->core_status |= CORTEXAR_STATUS_MMU_FAULT;
+	}
+
+	/* Convert the physical address to a virtual one using the top 20 bits of PAR and the bottom 12 of the virtual. */
+	const target_addr_t address = (phys_addr & 0xfffff000U) | (virt_addr & 0x00000fffU);
+
+	if (halted_in_function)
+		cortexar_halt_resume(target, false);
+
+	return address;
+}
+
 static bool cortexar_oslock_unlock(target_s *const target)
 {
 	const uint32_t lock_status = cortex_dbg_read32(target, CORTEXAR_DBG_OSLSR);
@@ -1447,6 +1501,27 @@ static void cortexar_halt_resume(target_s *const target, const bool step)
 		status = cortex_dbg_read32(target, CORTEXAR_DBG_DSCR);
 }
 
+/*
+ * Halt the core and await halted status.
+ */
+static bool cortexar_halt_and_wait(target_s *const target)
+{
+	if (!(cortex_dbg_read32(target, CORTEXAR_DBG_DSCR) & CORTEXAR_DBG_DSCR_HALTED)) {
+		platform_timeout_s timeout;
+		cortexar_halt_request(target);
+		platform_timeout_set(&timeout, 250);
+		target_halt_reason_e reason = TARGET_HALT_RUNNING;
+		while (!platform_timeout_is_expired(&timeout) && reason == TARGET_HALT_RUNNING)
+			reason = target_halt_poll(target, NULL);
+		if (reason != TARGET_HALT_REQUEST) {
+			DEBUG_ERROR("Failed to halt the core\n");
+			return false;
+		}
+		return true;
+	}
+	return false;
+}
+
 void cortexar_invalidate_all_caches(target_s *const target)
 {
 	/* Extract the cache geometry */
@@ -1516,8 +1591,7 @@ static void cortexar_config_breakpoint(
 		mode |= CORTEXAR_DBG_BCR_BYTE_SELECT_ALL;
 
 	/* Configure the breakpoint slot */
-	/* Removed virt_to_phys use from this line as its logic needs to be fixed. */
-	cortex_dbg_write32(target, CORTEXAR_DBG_BVR + (slot << 2U), addr & ~3U);
+	cortex_dbg_write32(target, CORTEXAR_DBG_BVR + (slot << 2U), cortexar_virt_to_phys(target, addr & ~3U));
 	cortex_dbg_write32(
 		target, CORTEXAR_DBG_BCR + (slot << 2U), CORTEXAR_DBG_BCR_ENABLE | CORTEXAR_DBG_BCR_ALL_MODES | (mode & ~7U));
 }
@@ -1549,8 +1623,7 @@ static void cortexar_config_watchpoint(target_s *const target, const size_t slot
 	const uint32_t mode = cortexar_watchpoint_mode(breakwatch->type) | CORTEXAR_DBG_WCR_BYTE_SELECT(byte_mask);
 
 	/* Configure the watchpoint slot */
-	/* Removed virt_to_phys from this line as its logic needs to be fixed. */
-	cortex_dbg_write32(target, CORTEXAR_DBG_WVR + (slot << 2U), breakwatch->addr & ~3U);
+	cortex_dbg_write32(target, CORTEXAR_DBG_WVR + (slot << 2U), cortexar_virt_to_phys(target, breakwatch->addr & ~3U));
 	cortex_dbg_write32(
 		target, CORTEXAR_DBG_WCR + (slot << 2U), CORTEXAR_DBG_WCR_ENABLE | CORTEXAR_DBG_WCR_ALL_MODES | mode);
 }