target/adi_v5_swd, cortex_m: properly handle more cases requiring reconnect

[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 31c219e59643d002ed575716233d9bd42729d985..2cb61f830bedc42dd95549feda8df0636a21c87e 100644 (file)
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -70,6 +70,8 @@ static void swd_finish_read(struct adiv5_dap *dap)
 
 static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
                uint32_t data);
+static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
+               uint32_t *data);
 
 static void swd_clear_sticky_errors(struct adiv5_dap *dap)
 {
@@ -83,22 +85,64 @@ static void swd_clear_sticky_errors(struct adiv5_dap *dap)
 static int swd_run_inner(struct adiv5_dap *dap)
 {
        const struct swd_driver *swd = jtag_interface->swd;
+       int retval;
 
-       int retval = swd->run(dap);
+       retval = swd->run(dap);
 
        if (retval != ERROR_OK) {
                /* fault response */
-               swd_clear_sticky_errors(dap);
+               dap->do_reconnect = true;
        }
 
        return retval;
 }
 
+static int swd_connect(struct adiv5_dap *dap)
+{
+       uint32_t idcode;
+       int status;
+
+       /* FIXME validate transport config ... is the
+        * configured DAP present (check IDCODE)?
+        * Is *only* one DAP configured?
+        *
+        * MUST READ IDCODE
+        */
+
+       /* Note, debugport_init() does setup too */
+       jtag_interface->swd->switch_seq(dap, JTAG_TO_SWD);
+
+       dap->do_reconnect = false;
+
+       swd_queue_dp_read(dap, DP_IDCODE, &idcode);
+
+       /* force clear all sticky faults */
+       swd_clear_sticky_errors(dap);
+
+       status = swd_run_inner(dap);
+
+       if (status == ERROR_OK) {
+               LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode);
+               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;
+}
+
 static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
 {
        const struct swd_driver *swd = jtag_interface->swd;
@@ -132,19 +176,26 @@ static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
        const struct swd_driver *swd = jtag_interface->swd;
        assert(swd);
 
+       int retval = swd_check_reconnect(dap);
+       if (retval != ERROR_OK)
+               return retval;
+
        swd_queue_dp_bankselect(dap, reg);
        swd->read_reg(dap, swd_cmd(true,  false, reg), data);
 
        return check_sync(dap);
 }
 
-
 static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
                uint32_t data)
 {
        const struct swd_driver *swd = jtag_interface->swd;
        assert(swd);
 
+       int retval = swd_check_reconnect(dap);
+       if (retval != ERROR_OK)
+               return retval;
+
        swd_finish_read(dap);
        swd_queue_dp_bankselect(dap, reg);
        swd->write_reg(dap, swd_cmd(false,  false, reg), data);
@@ -172,6 +223,10 @@ static int swd_queue_ap_read(struct adiv5_dap *dap, unsigned reg,
        const struct swd_driver *swd = jtag_interface->swd;
        assert(swd);
 
+       int retval = swd_check_reconnect(dap);
+       if (retval != ERROR_OK)
+               return retval;
+
        swd_queue_ap_bankselect(dap, reg);
        swd->read_reg(dap, swd_cmd(true,  true, reg), dap->last_read);
        dap->last_read = data;
@@ -185,6 +240,10 @@ static int swd_queue_ap_write(struct adiv5_dap *dap, unsigned reg,
        const struct swd_driver *swd = jtag_interface->swd;
        assert(swd);
 
+       int retval = swd_check_reconnect(dap);
+       if (retval != ERROR_OK)
+               return retval;
+
        swd_finish_read(dap);
        swd_queue_ap_bankselect(dap, reg);
        swd->write_reg(dap, swd_cmd(false,  true, reg), data);
@@ -253,6 +312,11 @@ int dap_to_swd(struct target *target)
        struct arm *arm = target_to_arm(target);
        int retval;
 
+       if (!arm->dap) {
+               LOG_ERROR("SWD mode is not available");
+               return ERROR_FAIL;
+       }
+
        LOG_DEBUG("Enter SWD mode");
 
        /* REVISIT it's ugly to need to make calls to a "jtag"
@@ -380,7 +444,7 @@ static int swd_select(struct command_context *ctx)
                return ERROR_FAIL;
        }
 
-       retval = swd->init(1);
+       retval = swd->init();
        if (retval != ERROR_OK) {
                LOG_DEBUG("can't init SWD driver");
                return retval;
@@ -403,36 +467,11 @@ static int swd_init(struct command_context *ctx)
        struct target *target = get_current_target(ctx);
        struct arm *arm = target_to_arm(target);
        struct adiv5_dap *dap = arm->dap;
-       uint32_t idcode;
-       int status;
-
        /* Force the DAP's ops vector for SWD mode.
         * messy - is there a better way? */
        arm->dap->ops = &swd_dap_ops;
 
-       /* FIXME validate transport config ... is the
-        * configured DAP present (check IDCODE)?
-        * Is *only* one DAP configured?
-        *
-        * MUST READ IDCODE
-        */
-
- /* Note, debugport_init() does setup too */
-
-       swd_queue_dp_read(dap, DP_IDCODE, &idcode);
-
-       /* force clear all sticky faults */
-       swd_clear_sticky_errors(dap);
-
-       status = swd_run(dap);
-
-       if (status == ERROR_OK)
-               LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode);
-
-       /* this is a workaround to get polling working */
-       jtag_add_reset(0, 0);
-
-       return status;
+       return swd_connect(dap);
 }
 
 static struct transport swd_transport = {