+ retval = dpm->instr_write_data_dcc(dpm,
+ ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address);
+ }
+
+ if (retval != ERROR_OK)
+ return retval;
+
+ if (size == 4 && (address % 4) == 0)
+ retval = aarch64_write_cpu_memory_fast(target, count, buffer, &dscr);
+ else
+ retval = aarch64_write_cpu_memory_slow(target, size, count, buffer, &dscr);
+
+ if (retval != ERROR_OK) {
+ /* Unset DTR mode */
+ mem_ap_read_atomic_u32(armv8->debug_ap,
+ armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+ dscr &= ~DSCR_MA;
+ mem_ap_write_atomic_u32(armv8->debug_ap,
+ armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+ }
+
+ /* Check for sticky abort flags in the DSCR */
+ retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+ armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+ if (retval != ERROR_OK)
+ return retval;
+
+ dpm->dscr = dscr;
+ if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
+ /* Abort occurred - clear it and exit */
+ LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
+ armv8_dpm_handle_exception(dpm, true);
+ return ERROR_FAIL;
+ }
+
+ /* Done */
+ return ERROR_OK;
+}
+
+static int aarch64_read_cpu_memory_slow(struct target *target,
+ uint32_t size, uint32_t count, uint8_t *buffer, uint32_t *dscr)
+{
+ struct armv8_common *armv8 = target_to_armv8(target);
+ struct arm_dpm *dpm = &armv8->dpm;
+ struct arm *arm = &armv8->arm;
+ int retval;
+
+ armv8_reg_current(arm, 1)->dirty = true;
+
+ /* change DCC to normal mode (if necessary) */
+ if (*dscr & DSCR_MA) {
+ *dscr &= DSCR_MA;
+ retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+ armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+ if (retval != ERROR_OK)
+ return retval;
+ }
+
+ while (count) {
+ uint32_t opcode, data;
+
+ if (size == 1)
+ opcode = armv8_opcode(armv8, ARMV8_OPC_LDRB_IP);
+ else if (size == 2)
+ opcode = armv8_opcode(armv8, ARMV8_OPC_LDRH_IP);
+ else
+ opcode = armv8_opcode(armv8, ARMV8_OPC_LDRW_IP);
+ retval = dpm->instr_execute(dpm, opcode);
+ if (retval != ERROR_OK)
+ return retval;
+
+ if (arm->core_state == ARM_STATE_AARCH64)
+ retval = dpm->instr_execute(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 1));
+ else
+ retval = dpm->instr_execute(dpm, ARMV4_5_MCR(14, 0, 1, 0, 5, 0));
+ if (retval != ERROR_OK)
+ return retval;
+
+ retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+ armv8->debug_base + CPUV8_DBG_DTRTX, &data);
+ if (retval != ERROR_OK)
+ return retval;
+
+ if (size == 1)
+ *buffer = (uint8_t)data;
+ else if (size == 2)
+ target_buffer_set_u16(target, buffer, (uint16_t)data);
+ else
+ target_buffer_set_u32(target, buffer, data);
+
+ /* Advance */
+ buffer += size;
+ --count;
+ }
+
+ return ERROR_OK;
+}
+
+static int aarch64_read_cpu_memory_fast(struct target *target,
+ uint32_t count, uint8_t *buffer, uint32_t *dscr)
+{
+ struct armv8_common *armv8 = target_to_armv8(target);
+ struct arm_dpm *dpm = &armv8->dpm;
+ struct arm *arm = &armv8->arm;
+ int retval;
+ uint32_t value;