+ return aarch64_init_debug_access(target);
+}
+
+static int aarch64_write_cpu_memory_slow(struct target *target,
+ uint32_t size, uint32_t count, const 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 data, opcode;
+
+ /* write the data to store into DTRRX */
+ 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_write_atomic_u32(armv8->debug_ap,
+ armv8->debug_base + CPUV8_DBG_DTRRX, data);
+ if (retval != ERROR_OK)
+ return retval;
+
+ if (arm->core_state == ARM_STATE_AARCH64)
+ retval = dpm->instr_execute(dpm, ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 1));
+ else
+ retval = dpm->instr_execute(dpm, ARMV4_5_MRC(14, 0, 1, 0, 5, 0));
+ if (retval != ERROR_OK)
+ return retval;
+
+ if (size == 1)
+ opcode = armv8_opcode(armv8, ARMV8_OPC_STRB_IP);
+ else if (size == 2)
+ opcode = armv8_opcode(armv8, ARMV8_OPC_STRH_IP);
+ else
+ opcode = armv8_opcode(armv8, ARMV8_OPC_STRW_IP);
+ retval = dpm->instr_execute(dpm, opcode);
+ if (retval != ERROR_OK)
+ return retval;
+
+ /* Advance */
+ buffer += size;
+ --count;
+ }
+
+ return ERROR_OK;
+}
+
+static int aarch64_write_cpu_memory_fast(struct target *target,
+ uint32_t count, const uint8_t *buffer, uint32_t *dscr)
+{
+ struct armv8_common *armv8 = target_to_armv8(target);
+ struct arm *arm = &armv8->arm;
+ int retval;
+
+ armv8_reg_current(arm, 1)->dirty = true;
+
+ /* Step 1.d - Change DCC to memory mode */
+ *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;
+
+
+ /* Step 2.a - Do the write */
+ retval = mem_ap_write_buf_noincr(armv8->debug_ap,
+ buffer, 4, count, armv8->debug_base + CPUV8_DBG_DTRRX);
+ if (retval != ERROR_OK)
+ return retval;
+
+ /* Step 3.a - Switch DTR mode back to Normal mode */
+ *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;
+