cortexr: Implemented a slow path memory writer for cortexr_mem_write()

Author: dragonmux

src/target/cortexr.c

@@ -220,14 +220,22 @@ static const uint16_t cortexr_spsr_encodings[5] = {
  * Instruction encodings for indirect loads and stores of data via the CPU
  * LDRB -> Load Register Byte (immediate) (DDI0406C §A8.8.69, pg419)
  * LDRH -> Load Register Halfword (immediate) (DDI0406C §A8.8.81, pg443)
+ * STRB -> Store Register Byte (immediate) (DDI0406C §A8.8.208, pg681)
+ * STRH -> Store Register Halfword (immediate) (DDI0406C §A8.8.218, pg701)
  *
  * The first is `LDRB r1, [r0], #+1` to load a uint8_t from [r0] into r1 and increment the
  * address in r0 by 1, writing the new address back to r0.
  * The second is `LDRH r1, [r0], #+2` to load a uint16_t from [r0] into r1 and increment
  * the address in r0 by 2, writing the new address back to r0.
+ * The third is `STRB r1, [r0], #+1` to store a uint8_t to [r0] from r1 and increment the
+ * address in r0 by 1, writing the new address back to r0.
+ * The fourth is `STRH r1, [r0], #+2` to store a uint16_t to [r0] from r1 and increment
+ * the address in r0 by 2, writing the new address back to r0.
  */
 #define ARM_LDRB_R0_R1_INSN 0xe4f01001U
 #define ARM_LDRH_R0_R1_INSN 0xe0f010b2U
+#define ARM_STRB_R1_R0_INSN 0xe4e01001U
+#define ARM_STRH_R1_R0_INSN 0xe0e010b2U
 
 /* Coprocessor register definitions */
 #define CORTEXR_CPACR 15U, ENCODE_CP_REG(1U, 0U, 0U, 2U)
@@ -822,6 +830,45 @@ static inline bool cortexr_mem_write_fast(target_s *const target, const uint32_t
 	return true; /* Signal success */
 }
 
+/* Slow path for cortexr_mem_write(). Trashes r0 and r1. */
+static bool cortexr_mem_write_slow(
+	target_s *const target, target_addr_t addr, const uint8_t *const data, const size_t length)
+{
+	size_t offset = 0;
+	/* If the address is odd, write a byte to get onto an even address */
+	if (addr & 1U) {
+		cortexr_core_reg_write(target, 1U, data[offset++]);
+		if (!cortexr_run_insn(target, ARM_STRB_R1_R0_INSN))
+			return false;
+		++addr;
+	}
+	/* If the address is now even but only 16-bit aligned, write a uint16_t to get onto 32-bit alignment */
+	if ((addr & 2U) && length - offset >= 2U) {
+		cortexr_core_reg_write(target, 1U, read_le2(data, offset));
+		if (!cortexr_run_insn(target, ARM_STRH_R1_R0_INSN))
+			return false;
+		offset += 2U;
+	}
+	/* Use the fast path to write as much as possible before doing a slow path fixup at the end */
+	if (!cortexr_mem_write_fast(target, (uint32_t *)(data + offset), (length - offset) >> 2U))
+		return false;
+	const uint8_t remainder = (length - offset) & 3U;
+	/* If the remainder needs at least 2 more bytes write, do this first */
+	if (remainder & 2U) {
+		cortexr_core_reg_write(target, 1U, read_le2(data, offset));
+		if (!cortexr_run_insn(target, ARM_STRH_R1_R0_INSN))
+			return false;
+		offset += 2U;
+	}
+	/* Finally, fix things up if a final byte is required. */
+	if (remainder & 1U) {
+		cortexr_core_reg_write(target, 1U, data[offset]);
+		if (!cortexr_run_insn(target, ARM_STRB_R1_R0_INSN))
+			return false;
+	}
+	return true; /* Signal success */
+}
+
 /*
  * This writes memory by jumping from the debug unit bus to the system bus.
  * NB: This requires the core to be halted! Uses instruction launches on
@@ -844,7 +891,7 @@ static void cortexr_mem_write(target_s *const target, const target_addr_t dest,
 	if ((dest & 3U) == 0U && (len & 3U) == 0U)
 		cortexr_mem_write_fast(target, (const uint32_t *)src, len >> 2U);
 	else
-		adiv5_mem_write(cortex_ap(target), dest, src, len);
+		cortexr_mem_write_slow(target, dest, (const uint8_t *)src, len);
 	/* Deal with any data faults that occured */
 	cortexr_mem_handle_fault(target, __func__, fault_status, fault_addr);
 }