+
+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));
+
+ swd_send_sequence(dap, LINE_RESET);
+
+ 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;
+
+ 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));
+ return retval;
+ }
+
+ LOG_DEBUG("Failed to select multidrop %s, retrying...",
+ adiv5_dap_name(dap));
+ }
+
+ return retval;
+}
+
+static int swd_connect_multidrop(struct adiv5_dap *dap)
+{
+ int retval;
+ uint32_t dpidr = 0xdeadbeef;
+ uint32_t dlpidr = 0xdeadbeef;
+ int64_t timeout = timeval_ms() + 500;
+
+ do {
+ swd_send_sequence(dap, JTAG_TO_DORMANT);
+ swd_send_sequence(dap, DORMANT_TO_SWD);
+
+ /* Clear link state, including the SELECT cache. */
+ dap->do_reconnect = false;
+ dap_invalidate_cache(dap);
+ swd_multidrop_selected_dap = NULL;
+
+ retval = swd_multidrop_select_inner(dap, &dpidr, &dlpidr, true);
+ if (retval == ERROR_OK)
+ break;
+
+ alive_sleep(1);
+
+ } while (timeval_ms() < timeout);
+
+ if (retval != ERROR_OK) {
+ swd_multidrop_selected_dap = NULL;
+ LOG_ERROR("Failed to connect multidrop %s", adiv5_dap_name(dap));
+ return retval;
+ }
+
+ LOG_INFO("SWD DPIDR 0x%08" PRIx32 ", DLPIDR 0x%08" PRIx32,
+ dpidr, dlpidr);
+
+ return retval;
+}
+
+static int swd_connect_single(struct adiv5_dap *dap)
+{
+ int retval;
+ uint32_t dpidr = 0xdeadbeef;
+ int64_t timeout = timeval_ms() + 500;
+
+ do {
+ if (dap->switch_through_dormant) {
+ swd_send_sequence(dap, JTAG_TO_DORMANT);
+ swd_send_sequence(dap, DORMANT_TO_SWD);
+ } else {
+ swd_send_sequence(dap, JTAG_TO_SWD);
+ }
+
+ /* Clear link state, including the SELECT cache. */
+ dap->do_reconnect = false;
+ dap_invalidate_cache(dap);
+
+ retval = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr);
+ if (retval == ERROR_OK) {
+ retval = swd_run_inner(dap);
+ if (retval == ERROR_OK)
+ break;
+ }
+
+ alive_sleep(1);
+
+ dap->switch_through_dormant = !dap->switch_through_dormant;
+ } while (timeval_ms() < timeout);
+
+ if (retval != ERROR_OK) {
+ LOG_ERROR("Error connecting DP: cannot read IDR");
+ return retval;
+ }
+
+ LOG_INFO("SWD DPIDR 0x%08" PRIx32, dpidr);
+
+ do {
+ dap->do_reconnect = false;
+
+ /* force clear all sticky faults */
+ swd_clear_sticky_errors(dap);
+
+ retval = swd_run_inner(dap);
+ if (retval != ERROR_WAIT)
+ break;
+
+ alive_sleep(10);
+
+ } while (timeval_ms() < timeout);
+
+ return retval;
+}
+
+static int swd_connect(struct adiv5_dap *dap)
+{
+ int status;
+
+ /* FIXME validate transport config ... is the
+ * configured DAP present (check IDCODE)?
+ */
+
+ /* Check if we should reset srst already when connecting, but not if reconnecting. */
+ if (!dap->do_reconnect) {
+ enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+ if (jtag_reset_config & RESET_CNCT_UNDER_SRST) {
+ if (jtag_reset_config & RESET_SRST_NO_GATING)
+ adapter_assert_reset();
+ else
+ LOG_WARNING("\'srst_nogate\' reset_config option is required");
+ }
+ }
+
+ if (dap_is_multidrop(dap))
+ status = swd_connect_multidrop(dap);
+ else
+ status = swd_connect_single(dap);
+
+ /* IHI 0031E B4.3.2:
+ * "A WAIT response must not be issued to the ...
+ * ... writes to the ABORT register"
+ * swd_clear_sticky_errors() writes to the ABORT register only.
+ *
+ * Unfortunately at least Microchip SAMD51/E53/E54 returns WAIT
+ * in a corner case. Just try if ABORT resolves the problem.
+ */
+ if (status == ERROR_WAIT) {
+ LOG_WARNING("Connecting DP: stalled AP operation, issuing ABORT");
+
+ dap->do_reconnect = false;
+
+ status = swd_queue_dp_write_inner(dap, DP_ABORT,
+ DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+
+ if (status == ERROR_OK)
+ status = swd_run_inner(dap);
+ }
+
+ if (status == ERROR_OK)
+ status = dap_dp_init(dap);
+
+ return status;
+}
+
+static int swd_check_reconnect(struct adiv5_dap *dap)