+ swd->write_reg(swd_cmd(false, false, DP_ABORT),
+ STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
+}
+
+static int swd_run_inner(struct adiv5_dap *dap)
+{
+ const struct swd_driver *swd = jtag_interface->swd;
+ int retval;
+
+ retval = swd->run();
+
+ if (retval != ERROR_OK) {
+ /* fault response */
+ dap->do_reconnect = true;
+ }
+
+ return retval;
+}
+
+static int swd_connect(struct adiv5_dap *dap)
+{
+ uint32_t dpidr;
+ int status;
+
+ /* FIXME validate transport config ... is the
+ * configured DAP present (check IDCODE)?
+ * Is *only* one DAP configured?
+ *
+ * MUST READ DPIDR
+ */
+
+ /* 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)
+ swd_add_reset(1);
+ else
+ LOG_WARNING("\'srst_nogate\' reset_config option is required");
+ }
+ }
+
+ /* Note, debugport_init() does setup too */
+ jtag_interface->swd->switch_seq(JTAG_TO_SWD);
+
+ /* Clear link state, including the SELECT cache. */
+ dap->do_reconnect = false;
+ dap->select = DP_SELECT_INVALID;
+
+ swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
+
+ /* force clear all sticky faults */
+ swd_clear_sticky_errors(dap);
+
+ status = swd_run_inner(dap);
+
+ if (status == ERROR_OK) {
+ LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr);
+ dap->do_reconnect = false;
+ } else
+ dap->do_reconnect = true;
+
+ return status;
+}
+
+static inline int check_sync(struct adiv5_dap *dap)
+{
+ return do_sync ? swd_run_inner(dap) : ERROR_OK;
+}
+
+static int swd_check_reconnect(struct adiv5_dap *dap)
+{
+ if (dap->do_reconnect)
+ return swd_connect(dap);
+
+ return ERROR_OK;