+
+ return swd->run();
+}
+
+static inline int check_sync(struct adiv5_dap *dap)
+{
+ return do_sync ? swd_run_inner(dap) : ERROR_OK;
+}
+
+/** Select the DP register bank */
+static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned int reg)
+{
+ /* Only register address 0 (ADIv6 only) and 4 are banked. */
+ if (is_adiv6(dap) ? (reg & 0xf) > 4 : (reg & 0xf) != 4)
+ return ERROR_OK;
+
+ uint32_t sel = (reg >> 4) & DP_SELECT_DPBANK;
+
+ /* ADIv6 ensures DPBANKSEL = 0 after line reset */
+ if ((dap->select_valid || (is_adiv6(dap) && dap->select_dpbanksel_valid))
+ && (sel == (dap->select & DP_SELECT_DPBANK)))
+ return ERROR_OK;
+
+ /* Use the AP part of dap->select regardless of dap->select_valid:
+ * if !dap->select_valid
+ * dap->select contains a speculative value likely going to be used
+ * in the following swd_queue_ap_bankselect() */
+ sel |= (uint32_t)(dap->select & SELECT_AP_MASK);
+
+ LOG_DEBUG_IO("DP BANK SELECT: %" PRIx32, sel);
+
+ /* dap->select cache gets updated in the following call */
+ return swd_queue_dp_write_inner(dap, DP_SELECT, sel);
+}
+
+static int swd_queue_dp_read_inner(struct adiv5_dap *dap, unsigned int reg,
+ uint32_t *data)
+{
+ const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
+ assert(swd);
+
+ int retval = swd_queue_dp_bankselect(dap, reg);
+ if (retval != ERROR_OK)
+ return retval;
+
+ swd->read_reg(swd_cmd(true, false, reg), data, 0);
+
+ return check_sync(dap);
+}
+
+static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
+ uint32_t data)
+{
+ int retval = ERROR_OK;
+ const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
+ assert(swd);
+
+ swd_finish_read(dap);
+
+ if (reg == DP_SELECT) {
+ dap->select = data | (dap->select & (0xffffffffull << 32));
+
+ swd->write_reg(swd_cmd(false, false, reg), data, 0);
+
+ retval = check_sync(dap);
+ dap->select_valid = (retval == ERROR_OK);
+ dap->select_dpbanksel_valid = dap->select_valid;
+
+ return retval;
+ }
+
+ if (reg == DP_SELECT1)
+ dap->select = ((uint64_t)data << 32) | (dap->select & 0xffffffffull);
+
+ /* DP_ABORT write is not banked.
+ * Prevent writing DP_SELECT before as it would fail on locked up DP */
+ if (reg != DP_ABORT)
+ retval = swd_queue_dp_bankselect(dap, reg);
+
+ if (retval == ERROR_OK) {
+ swd->write_reg(swd_cmd(false, false, reg), data, 0);
+
+ retval = check_sync(dap);
+ }
+
+ if (reg == DP_SELECT1)
+ dap->select1_valid = (retval == ERROR_OK);
+
+ return retval;
+}
+
+
+static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr,
+ uint32_t *dlpidr_ptr, bool clear_sticky)
+{