+ /* Set Normal access mode */
+ dscr = (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;
+
+ if (arm->core_state == ARM_STATE_AARCH64) {
+ /* Write X0 with value 'address' using write procedure */
+ /* Step 1.a+b - Write the address for read access into DBGDTR_EL0 */
+ /* Step 1.c - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
+ retval = dpm->instr_write_data_dcc_64(dpm,
+ ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address);
+ } else {
+ /* Write R0 with value 'address' using write procedure */
+ /* Step 1.a+b - Write the address for read access into DBGDTRRX */
+ /* Step 1.c - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
+ 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);