+static int cortex_a_dfsr_to_error_code(uint32_t dfsr)
+{
+ uint32_t status, upper4;
+
+ if (dfsr & (1 << 9)) {
+ /* LPAE format. */
+ status = dfsr & 0x3f;
+ upper4 = status >> 2;
+ if (upper4 == 1 || upper4 == 2 || upper4 == 3 || upper4 == 15)
+ return ERROR_TARGET_TRANSLATION_FAULT;
+ else if (status == 33)
+ return ERROR_TARGET_UNALIGNED_ACCESS;
+ else
+ return ERROR_TARGET_DATA_ABORT;
+ } else {
+ /* Normal format. */
+ status = ((dfsr >> 6) & 0x10) | (dfsr & 0xf);
+ if (status == 1)
+ return ERROR_TARGET_UNALIGNED_ACCESS;
+ else if (status == 5 || status == 7 || status == 3 || status == 6 ||
+ status == 9 || status == 11 || status == 13 || status == 15)
+ return ERROR_TARGET_TRANSLATION_FAULT;
+ else
+ return ERROR_TARGET_DATA_ABORT;
+ }
+}
+
+static int cortex_a_write_apb_ab_memory_slow(struct target *target,
+ uint32_t size, uint32_t count, const uint8_t *buffer, uint32_t *dscr)
+{
+ /* Writes count objects of size size from *buffer. Old value of DSCR must
+ * be in *dscr; updated to new value. This is slow because it works for
+ * non-word-sized objects and (maybe) unaligned accesses. If size == 4 and
+ * the address is aligned, cortex_a_write_apb_ab_memory_fast should be
+ * preferred.
+ * Preconditions:
+ * - Address is in R0.
+ * - R0 is marked dirty.
+ */
+ struct armv7a_common *armv7a = target_to_armv7a(target);
+ struct adiv5_dap *swjdp = armv7a->arm.dap;
+ struct arm *arm = &armv7a->arm;
+ int retval;
+
+ /* Mark register R1 as dirty, to use for transferring data. */
+ arm_reg_current(arm, 1)->dirty = true;
+
+ /* Switch to non-blocking mode if not already in that mode. */
+ retval = cortex_a_set_dcc_mode(target, DSCR_EXT_DCC_NON_BLOCKING, dscr);
+ if (retval != ERROR_OK)
+ return retval;
+
+ /* Go through the objects. */
+ while (count) {
+ /* Write the value to store into DTRRX. */
+ uint32_t data, opcode;
+ if (size == 1)
+ data = *buffer;
+ else if (size == 2)
+ data = target_buffer_get_u16(target, buffer);
+ else
+ data = target_buffer_get_u32(target, buffer);
+ retval = mem_ap_sel_write_atomic_u32(swjdp, armv7a->debug_ap,
+ armv7a->debug_base + CPUDBG_DTRRX, data);
+ if (retval != ERROR_OK)
+ return retval;
+
+ /* Transfer the value from DTRRX to R1. */
+ retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, 1, 0, 5, 0), dscr);
+ if (retval != ERROR_OK)
+ return retval;
+
+ /* Write the value transferred to R1 into memory. */
+ if (size == 1)
+ opcode = ARMV4_5_STRB_IP(1, 0);
+ else if (size == 2)
+ opcode = ARMV4_5_STRH_IP(1, 0);
+ else
+ opcode = ARMV4_5_STRW_IP(1, 0);
+ retval = cortex_a_exec_opcode(target, opcode, dscr);
+ if (retval != ERROR_OK)
+ return retval;
+
+ /* Check for faults and return early. */
+ if (*dscr & (DSCR_STICKY_ABORT_PRECISE | DSCR_STICKY_ABORT_IMPRECISE))
+ return ERROR_OK; /* A data fault is not considered a system failure. */
+
+ /* Wait until DTRRX is empty (according to ARMv7-A/-R architecture
+ * manual section C8.4.3, checking InstrCmpl_l is not sufficient; one
+ * must also check RXfull_l). Most of the time this will be free
+ * because RXfull_l will be cleared immediately and cached in dscr. */
+ retval = cortex_a_wait_dscr_bits(target, DSCR_DTRRX_FULL_LATCHED, 0, dscr);
+ if (retval != ERROR_OK)
+ return retval;
+
+ /* Advance. */
+ buffer += size;
+ --count;
+ }
+
+ return ERROR_OK;
+}
+
+static int cortex_a_write_apb_ab_memory_fast(struct target *target,
+ uint32_t count, const uint8_t *buffer, uint32_t *dscr)
+{
+ /* Writes count objects of size 4 from *buffer. Old value of DSCR must be
+ * in *dscr; updated to new value. This is fast but only works for
+ * word-sized objects at aligned addresses.
+ * Preconditions:
+ * - Address is in R0 and must be a multiple of 4.
+ * - R0 is marked dirty.
+ */
+ struct armv7a_common *armv7a = target_to_armv7a(target);
+ struct adiv5_dap *swjdp = armv7a->arm.dap;
+ int retval;
+
+ /* Switch to fast mode if not already in that mode. */
+ retval = cortex_a_set_dcc_mode(target, DSCR_EXT_DCC_FAST_MODE, dscr);
+ if (retval != ERROR_OK)
+ return retval;
+
+ /* Latch STC instruction. */
+ retval = mem_ap_sel_write_atomic_u32(swjdp, armv7a->debug_ap,
+ armv7a->debug_base + CPUDBG_ITR, ARMV4_5_STC(0, 1, 0, 1, 14, 5, 0, 4));
+ if (retval != ERROR_OK)
+ return retval;
+
+ /* Transfer all the data and issue all the instructions. */
+ return mem_ap_sel_write_buf_noincr(swjdp, armv7a->debug_ap, buffer,
+ 4, count, armv7a->debug_base + CPUDBG_DTRRX);
+}
+
+static int cortex_a_write_apb_ab_memory(struct target *target,
+ uint32_t address, uint32_t size,
+ uint32_t count, const uint8_t *buffer)
+{
+ /* Write memory through APB-AP. */
+ int retval, final_retval;
+ struct armv7a_common *armv7a = target_to_armv7a(target);
+ struct adiv5_dap *swjdp = armv7a->arm.dap;
+ struct arm *arm = &armv7a->arm;
+ uint32_t dscr, orig_dfar, orig_dfsr, fault_dscr, fault_dfar, fault_dfsr;
+
+ LOG_DEBUG("Writing APB-AP memory address 0x%" PRIx32 " size %" PRIu32 " count %" PRIu32,
+ address, size, count);
+ if (target->state != TARGET_HALTED) {
+ LOG_WARNING("target not halted");
+ return ERROR_TARGET_NOT_HALTED;
+ }
+
+ if (!count)
+ return ERROR_OK;
+
+ /* Clear any abort. */
+ retval = mem_ap_sel_write_atomic_u32(swjdp, armv7a->debug_ap,
+ armv7a->debug_base + CPUDBG_DRCR, DRCR_CLEAR_EXCEPTIONS);
+ if (retval != ERROR_OK)
+ return retval;
+
+ /* Read DSCR. */
+ retval = mem_ap_sel_read_atomic_u32(swjdp, armv7a->debug_ap,
+ armv7a->debug_base + CPUDBG_DSCR, &dscr);
+ if (retval != ERROR_OK)
+ return retval;
+
+ /* Switch to non-blocking mode if not already in that mode. */
+ retval = cortex_a_set_dcc_mode(target, DSCR_EXT_DCC_NON_BLOCKING, &dscr);
+ if (retval != ERROR_OK)
+ goto out;
+
+ /* Mark R0 as dirty. */
+ arm_reg_current(arm, 0)->dirty = true;
+
+ /* Read DFAR and DFSR, as they will be modified in the event of a fault. */
+ retval = cortex_a_read_dfar_dfsr(target, &orig_dfar, &orig_dfsr, &dscr);
+ if (retval != ERROR_OK)
+ goto out;
+
+ /* Get the memory address into R0. */
+ retval = mem_ap_sel_write_atomic_u32(swjdp, armv7a->debug_ap,
+ armv7a->debug_base + CPUDBG_DTRRX, address);
+ if (retval != ERROR_OK)
+ goto out;
+ retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0), &dscr);
+ if (retval != ERROR_OK)
+ goto out;
+
+ if (size == 4 && (address % 4) == 0) {
+ /* We are doing a word-aligned transfer, so use fast mode. */
+ retval = cortex_a_write_apb_ab_memory_fast(target, count, buffer, &dscr);
+ } else {
+ /* Use slow path. */
+ retval = cortex_a_write_apb_ab_memory_slow(target, size, count, buffer, &dscr);
+ }
+
+out:
+ final_retval = retval;
+
+ /* Switch to non-blocking mode if not already in that mode. */
+ retval = cortex_a_set_dcc_mode(target, DSCR_EXT_DCC_NON_BLOCKING, &dscr);
+ if (final_retval == ERROR_OK)
+ final_retval = retval;
+
+ /* Wait for last issued instruction to complete. */
+ retval = cortex_a_wait_instrcmpl(target, &dscr, true);
+ if (final_retval == ERROR_OK)
+ final_retval = retval;
+
+ /* Wait until DTRRX is empty (according to ARMv7-A/-R architecture manual
+ * section C8.4.3, checking InstrCmpl_l is not sufficient; one must also
+ * check RXfull_l). Most of the time this will be free because RXfull_l
+ * will be cleared immediately and cached in dscr. However, don’t do this
+ * if there is fault, because then the instruction might not have completed
+ * successfully. */
+ if (!(dscr & DSCR_STICKY_ABORT_PRECISE)) {
+ retval = cortex_a_wait_dscr_bits(target, DSCR_DTRRX_FULL_LATCHED, 0, &dscr);
+ if (retval != ERROR_OK)
+ return retval;
+ }
+
+ /* If there were any sticky abort flags, clear them. */
+ if (dscr & (DSCR_STICKY_ABORT_PRECISE | DSCR_STICKY_ABORT_IMPRECISE)) {
+ fault_dscr = dscr;
+ mem_ap_sel_write_atomic_u32(swjdp, armv7a->debug_ap,
+ armv7a->debug_base + CPUDBG_DRCR, DRCR_CLEAR_EXCEPTIONS);
+ dscr &= ~(DSCR_STICKY_ABORT_PRECISE | DSCR_STICKY_ABORT_IMPRECISE);
+ } else {
+ fault_dscr = 0;
+ }
+
+ /* Handle synchronous data faults. */
+ if (fault_dscr & DSCR_STICKY_ABORT_PRECISE) {
+ if (final_retval == ERROR_OK) {
+ /* Final return value will reflect cause of fault. */
+ retval = cortex_a_read_dfar_dfsr(target, &fault_dfar, &fault_dfsr, &dscr);
+ if (retval == ERROR_OK) {
+ LOG_ERROR("data abort at 0x%08" PRIx32 ", dfsr = 0x%08" PRIx32, fault_dfar, fault_dfsr);
+ final_retval = cortex_a_dfsr_to_error_code(fault_dfsr);
+ } else
+ final_retval = retval;