From 6a8bf7128b1f5cd373f03ab9e7bd5527d38d0bc5 Mon Sep 17 00:00:00 2001 From: dragonmux Date: Wed, 11 Oct 2023 16:55:31 +0100 Subject: [PATCH] cortexr: Implemented a slow path memory writer for `cortexr_mem_write()` --- src/target/cortexr.c | 49 +++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 48 insertions(+), 1 deletion(-) diff --git a/src/target/cortexr.c b/src/target/cortexr.c index 6484c05228b..f491d2b5f7e 100644 --- a/src/target/cortexr.c +++ b/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); }