target/adi_v5_swd: add support for SWD multidrop

Based on Graham Sanderson's http://review.openocd.org/4935

Unlike Graham Sanderson's version this patch does not add
any multidrop specific queuing.

Multidrop SWD is handled mostly by the same code as single SWD,
just a selection sequence is prepended to a SWD operation
as needed.

This is a minimal working implementation without checking for
configuration errors (mixing multidrop and non multidrop DPs,
multiple use of the same selection id etc...).

Multidrop switching likely demands changes in the adapter code.

Change-Id: I99a5742c209b49c0483e800f6105cb5e59a897d9
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: https://review.openocd.org/c/openocd/+/6141
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 6e403e8b22ea2eeb9a9a8a568eed753a8fadeac6..c23cee157d53751e0a24f59879f078b76ac83617 100644 (file)
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -56,8 +56,9 @@
 /* for debug, set do_sync to true to force synchronous transfers */
 static bool do_sync;
 
+static struct adiv5_dap *swd_multidrop_selected_dap;
+
 
-static int swd_run(struct adiv5_dap *dap);
 static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
                uint32_t data);
 
@@ -176,46 +177,154 @@ static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
        return check_sync(dap);
 }
 
-static int swd_connect(struct adiv5_dap *dap)
+
+static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr,
+               uint32_t *dlpidr_ptr, bool clear_sticky)
 {
-       const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
-       uint32_t dpidr = 0xdeadbeef;
-       int status;
+       int retval;
+       uint32_t dpidr, dlpidr;
 
-       /* FIXME validate transport config ... is the
-        * configured DAP present (check IDCODE)?
-        * Is *only* one DAP configured?
-        *
-        * MUST READ DPIDR
-        */
+       assert(dap_is_multidrop(dap));
 
-       /* 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();
+       swd_send_sequence(dap, LINE_RESET);
 
-               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");
+       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 {
-               /* Note, debugport_init() does setup too */
-               swd->switch_seq(JTAG_TO_SWD);
+               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;
 
-               status = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr);
-               if (status == ERROR_OK) {
-                       status = swd_run_inner(dap);
-                       if (status == ERROR_OK)
+               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 {
+               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;
                }
 
@@ -223,12 +332,12 @@ static int swd_connect(struct adiv5_dap *dap)
 
        } while (timeval_ms() < timeout);
 
-       if (status != ERROR_OK) {
+       if (retval != ERROR_OK) {
                LOG_ERROR("Error connecting DP: cannot read IDR");
-               return status;
+               return retval;
        }
 
-       LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr);
+       LOG_INFO("SWD DPIDR 0x%08" PRIx32, dpidr);
 
        do {
                dap->do_reconnect = false;
@@ -236,14 +345,42 @@ static int swd_connect(struct adiv5_dap *dap)
                /* force clear all sticky faults */
                swd_clear_sticky_errors(dap);
 
-               status = swd_run_inner(dap);
-               if (status != ERROR_WAIT)
+               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"
@@ -257,9 +394,11 @@ static int swd_connect(struct adiv5_dap *dap)
 
                dap->do_reconnect = false;
 
-               swd->write_reg(swd_cmd(false, false, DP_ABORT),
-                       DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
-               status = swd_run_inner(dap);
+               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)
@@ -281,6 +420,14 @@ static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
        const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
        assert(swd);
 
+       /* TODO: Send DAPABORT in swd_multidrop_select_inner()
+        * in the case the multidrop dap is not selected?
+        * swd_queue_ap_abort() is not currently used anyway...
+        */
+       int retval = swd_multidrop_select(dap);
+       if (retval != ERROR_OK)
+               return retval;
+
        swd->write_reg(swd_cmd(false, false, DP_ABORT),
                DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
        return check_sync(dap);
@@ -293,6 +440,10 @@ static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
        if (retval != ERROR_OK)
                return retval;
 
+       retval = swd_multidrop_select(dap);
+       if (retval != ERROR_OK)
+               return retval;
+
        return swd_queue_dp_read_inner(dap, reg, data);
 }
 
@@ -306,6 +457,10 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
        if (retval != ERROR_OK)
                return retval;
 
+       retval = swd_multidrop_select(dap);
+       if (retval != ERROR_OK)
+               return retval;
+
        return swd_queue_dp_write_inner(dap, reg, data);
 }
 
@@ -340,6 +495,10 @@ static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
        if (retval != ERROR_OK)
                return retval;
 
+       retval = swd_multidrop_select(dap);
+       if (retval != ERROR_OK)
+               return retval;
+
        retval = swd_queue_ap_bankselect(ap, reg);
        if (retval != ERROR_OK)
                return retval;
@@ -361,7 +520,12 @@ static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
        if (retval != ERROR_OK)
                return retval;
 
+       retval = swd_multidrop_select(dap);
+       if (retval != ERROR_OK)
+               return retval;
+
        swd_finish_read(dap);
+
        retval = swd_queue_ap_bankselect(ap, reg);
        if (retval != ERROR_OK)
                return retval;
@@ -374,7 +538,12 @@ static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
 /** Executes all queued DAP operations. */
 static int swd_run(struct adiv5_dap *dap)
 {
+       int retval = swd_multidrop_select(dap);
+       if (retval != ERROR_OK)
+               return retval;
+
        swd_finish_read(dap);
+
        return swd_run_inner(dap);
 }
 
@@ -382,9 +551,27 @@ static int swd_run(struct adiv5_dap *dap)
 static void swd_quit(struct adiv5_dap *dap)
 {
        const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
+       static bool done;
+
+       /* There is no difference if the sequence is sent at the last
+        * or the first swd_quit() call, send it just once */
+       if (done)
+               return;
+
+       done = true;
+       if (dap_is_multidrop(dap)) {
+               swd->switch_seq(SWD_TO_DORMANT);
+               /* Revisit!
+                * Leaving DPs in dormant state was tested and offers some safety
+                * against DPs mismatch in case of unintentional use of non-multidrop SWD.
+                * To put SWJ-DPs to power-on state issue
+                * swd->switch_seq(DORMANT_TO_JTAG);
+                */
+       } else {
+               swd->switch_seq(SWD_TO_JTAG);
+       }
 
-       swd->switch_seq(SWD_TO_JTAG);
-       /* flush the queue before exit */
+       /* flush the queue to shift out the sequence before exit */
        swd->run();
 }
 

diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 1c96dc87ac8c0952d224c5702ef07473a98e0ef6..758cfa096d1984e896adac25011286eaca3b6442 100644 (file)
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -65,6 +65,10 @@
 
 #define DLCR_TO_TRN(dlcr) ((uint32_t)(1 + ((3 & (dlcr)) >> 8))) /* 1..4 clocks */
 
+/* Fields of DP_DPIDR register */
+#define DP_DPIDR_VERSION_SHIFT 12
+#define DP_DPIDR_VERSION_MASK  (0xFUL << DP_DPIDR_VERSION_SHIFT)
+
 /* Fields of the DP's AP ABORT register */
 #define DAPABORT        (1UL << 0)
 #define STKCMPCLR       (1UL << 1) /* SWD-only */
@@ -89,6 +93,8 @@
 #define CSYSPWRUPREQ    (1UL << 30)
 #define CSYSPWRUPACK    (1UL << 31)
 
+#define DP_DLPIDR_PROTVSN      1u
+
 #define DP_SELECT_APSEL 0xFF000000
 #define DP_SELECT_APBANK 0x000000F0
 #define DP_SELECT_DPBANK 0x0000000F