target/adi_v5_swd: remove double space from swd_cmd() parameters

[openocd.git] / src / target / adi_v5_swd.c

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 8d11537338156302aa58420b1d76f3fb81fdc315..f1fca40aed37832c853d610f8d023d3768226d75 100644 (file)
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -74,7 +74,7 @@ static void swd_clear_sticky_errors(struct adiv5_dap *dap)
        const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
        assert(swd);
 
-       swd->write_reg(swd_cmd(false,  false, DP_ABORT),
+       swd->write_reg(swd_cmd(false, false, DP_ABORT),
                STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
 }
 
@@ -96,7 +96,7 @@ static int swd_run_inner(struct adiv5_dap *dap)
 static int swd_connect(struct adiv5_dap *dap)
 {
        const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
-       uint32_t dpidr;
+       uint32_t dpidr = 0xdeadbeef;
        int status;
 
        /* FIXME validate transport config ... is the
@@ -118,26 +118,69 @@ static int swd_connect(struct adiv5_dap *dap)
                }
        }
 
-       /* Note, debugport_init() does setup too */
-       swd->switch_seq(JTAG_TO_SWD);
 
-       /* Clear link state, including the SELECT cache. */
-       dap->do_reconnect = false;
-       dap_invalidate_cache(dap);
+       int64_t timeout = timeval_ms() + 500;
+
+       do {
+               /* Note, debugport_init() does setup too */
+               swd->switch_seq(JTAG_TO_SWD);
+
+               /* Clear link state, including the SELECT cache. */
+               dap->do_reconnect = false;
+               dap_invalidate_cache(dap);
 
-       swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
+               status = swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
+               if (status == ERROR_OK) {
+                       status = swd_run_inner(dap);
+                       if (status == ERROR_OK)
+                               break;
+               }
 
-       /* force clear all sticky faults */
-       swd_clear_sticky_errors(dap);
+               alive_sleep(1);
 
-       status = swd_run_inner(dap);
+       } while (timeval_ms() < timeout);
 
-       if (status == ERROR_OK) {
-               LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr);
+       if (status != ERROR_OK) {
+               LOG_ERROR("Error connecting DP: cannot read IDR");
+               return status;
+       }
+
+       LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr);
+
+       do {
                dap->do_reconnect = false;
+
+               /* force clear all sticky faults */
+               swd_clear_sticky_errors(dap);
+
+               status = swd_run_inner(dap);
+               if (status != ERROR_WAIT)
+                       break;
+
+               alive_sleep(10);
+
+       } while (timeval_ms() < timeout);
+
+       /* 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;
+
+               swd->write_reg(swd_cmd(false, false, DP_ABORT),
+                       DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
+               status = swd_run_inner(dap);
+       }
+
+       if (status == ERROR_OK)
                status = dap_dp_init(dap);
-       } else
-               dap->do_reconnect = true;
 
        return status;
 }
@@ -282,7 +325,7 @@ static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
        if (retval != ERROR_OK)
                return retval;
 
-       swd->read_reg(swd_cmd(true,  true, reg), dap->last_read, ap->memaccess_tck);
+       swd->read_reg(swd_cmd(true, true, reg), dap->last_read, ap->memaccess_tck);
        dap->last_read = data;
 
        return check_sync(dap);
@@ -304,7 +347,7 @@ static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
        if (retval != ERROR_OK)
                return retval;
 
-       swd->write_reg(swd_cmd(false,  true, reg), data, ap->memaccess_tck);
+       swd->write_reg(swd_cmd(false, true, reg), data, ap->memaccess_tck);
 
        return check_sync(dap);
 }