+ const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
+
+ 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)
+{
+ int retval;
+ uint32_t dpidr, dlpidr;
+
+ assert(dap_is_multidrop(dap));
+
+ /* Send JTAG_TO_DORMANT and DORMANT_TO_SWD just once
+ * and then use shorter LINE_RESET until communication fails */
+ if (!swd_multidrop_in_swd_state) {
+ swd_send_sequence(dap, JTAG_TO_DORMANT);
+ swd_send_sequence(dap, DORMANT_TO_SWD);
+ } else {
+ swd_send_sequence(dap, LINE_RESET);
+ }
+
+ /*
+ * Zero dap->select and set dap->select_dpbanksel_valid
+ * to skip the write to DP_SELECT before DPIDR read, avoiding
+ * the protocol error.
+ * Clear the other validity flags because the rest of the DP
+ * SELECT and SELECT1 registers is unknown after line reset.
+ */
+ dap->select = 0;
+ dap->select_dpbanksel_valid = true;
+ dap->select_valid = false;
+ dap->select1_valid = false;
+
+ retval = swd_queue_dp_write_inner(dap, DP_TARGETSEL, dap->multidrop_targetsel);
+ if (retval != ERROR_OK)
+ return retval;
+
+ retval = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr);
+ if (retval != ERROR_OK)
+ return retval;
+
+ if (clear_sticky) {
+ /* Clear all sticky errors (including ORUN) */
+ swd_clear_sticky_errors(dap);
+ } else {
+ /* Ideally just clear ORUN flag which is set by reset */
+ retval = swd_queue_dp_write_inner(dap, DP_ABORT, ORUNERRCLR);
+ if (retval != ERROR_OK)
+ return retval;
+ }
+
+ retval = swd_queue_dp_read_inner(dap, DP_DLPIDR, &dlpidr);
+ if (retval != ERROR_OK)
+ return retval;
+
+ retval = swd_run_inner(dap);
+ if (retval != ERROR_OK)
+ return retval;
+
+ if ((dpidr & DP_DPIDR_VERSION_MASK) < (2UL << DP_DPIDR_VERSION_SHIFT)) {
+ LOG_INFO("Read DPIDR 0x%08" PRIx32
+ " has version < 2. A non multidrop capable device connected?",
+ dpidr);
+ return ERROR_FAIL;
+ }
+
+ /* TODO: check TARGETID if DLIPDR is same for more than one DP */
+ uint32_t expected_dlpidr = DP_DLPIDR_PROTVSN |
+ (dap->multidrop_targetsel & DP_TARGETSEL_INSTANCEID_MASK);
+ if (dlpidr != expected_dlpidr) {
+ LOG_INFO("Read incorrect DLPIDR 0x%08" PRIx32
+ " (possibly CTRL/STAT value)",
+ dlpidr);
+ return ERROR_FAIL;
+ }
+
+ LOG_DEBUG_IO("Selected DP_TARGETSEL 0x%08" PRIx32, dap->multidrop_targetsel);
+ swd_multidrop_selected_dap = dap;
+ swd_multidrop_in_swd_state = true;
+
+ if (dpidr_ptr)
+ *dpidr_ptr = dpidr;
+
+ if (dlpidr_ptr)
+ *dlpidr_ptr = dlpidr;
+
+ return retval;
+}
+
+static int swd_multidrop_select(struct adiv5_dap *dap)
+{
+ if (!dap_is_multidrop(dap))
+ return ERROR_OK;
+
+ if (swd_multidrop_selected_dap == dap)
+ return ERROR_OK;
+
+ int retval = ERROR_OK;
+ for (unsigned int retry = 0; ; retry++) {
+ bool clear_sticky = retry > 0;
+
+ retval = swd_multidrop_select_inner(dap, NULL, NULL, clear_sticky);
+ if (retval == ERROR_OK)
+ break;
+
+ swd_multidrop_selected_dap = NULL;
+ if (retry > 3) {
+ LOG_ERROR("Failed to select multidrop %s", adiv5_dap_name(dap));
+ dap->do_reconnect = true;
+ return retval;
+ }
+
+ LOG_DEBUG("Failed to select multidrop %s, retrying...",
+ adiv5_dap_name(dap));
+ }
+
+ dap->do_reconnect = false;
+ return retval;
+}
+
+static int swd_connect_multidrop(struct adiv5_dap *dap)
+{