+ /* Set DTR mode to Fast (2) */
+ dscr = (dscr & ~DSCR_EXT_DCC_MASK) | DSCR_EXT_DCC_FAST_MODE;
+ retval = mem_ap_sel_write_atomic_u32(swjdp, armv7a->debug_ap,
+ armv7a->debug_base + CPUDBG_DSCR, dscr);
+ if (retval != ERROR_OK)
+ goto error_free_buff_w;
+
+ /* Copy the destination address into R0 */
+ /* - pend an instruction MRC p14, 0, R0, c5, c0 */
+ retval = mem_ap_sel_write_atomic_u32(swjdp, armv7a->debug_ap,
+ armv7a->debug_base + CPUDBG_ITR, ARMV4_5_MRC(14, 0, 0, 0, 5, 0));
+ if (retval != ERROR_OK)
+ goto error_unset_dtr_w;
+ /* Write address into DTRRX, which triggers previous instruction */
+ retval = mem_ap_sel_write_atomic_u32(swjdp, armv7a->debug_ap,
+ armv7a->debug_base + CPUDBG_DTRRX, address & (~0x3));
+ if (retval != ERROR_OK)
+ goto error_unset_dtr_w;
+
+ /* Write the data transfer instruction into the ITR
+ * (STC p14, c5, [R0], 4)
+ */
+ 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)
+ goto error_unset_dtr_w;
+
+ /* Do the write */
+ retval = mem_ap_sel_write_buf_u32_noincr(swjdp, armv7a->debug_ap,
+ tmp_buff, (total_u32)<<2, armv7a->debug_base + CPUDBG_DTRRX);
+ if (retval != ERROR_OK)
+ goto error_unset_dtr_w;
+
+
+ /* Switch DTR mode back to non-blocking (0) */
+ dscr = (dscr & ~DSCR_EXT_DCC_MASK) | DSCR_EXT_DCC_NON_BLOCKING;
+ retval = mem_ap_sel_write_atomic_u32(swjdp, armv7a->debug_ap,
+ armv7a->debug_base + CPUDBG_DSCR, dscr);
+ if (retval != ERROR_OK)
+ goto error_unset_dtr_w;
+
+ /* Check for sticky abort flags in the DSCR */
+ retval = mem_ap_sel_read_atomic_u32(swjdp, armv7a->debug_ap,
+ armv7a->debug_base + CPUDBG_DSCR, &dscr);
+ if (retval != ERROR_OK)
+ goto error_free_buff_w;
+ if (dscr & (DSCR_STICKY_ABORT_PRECISE | DSCR_STICKY_ABORT_IMPRECISE)) {
+ /* Abort occurred - clear it and exit */
+ LOG_ERROR("abort occurred - dscr = 0x%08x", dscr);
+ mem_ap_sel_write_atomic_u32(swjdp, armv7a->debug_ap,
+ armv7a->debug_base + CPUDBG_DRCR, 1<<2);
+ goto error_free_buff_w;