target/arm_adi_v5: fix DP SELECT logic

The original code supported ADIv5 only, just one SELECT register
with some reserved bits - the pseudo value DP_SELECT_INVALID was
just fine to indicate the DP SELECT register is in an unknown state.

Added ADIv6 support required DP SELECT and SELECT1 registers
without reserved bits. Therefore DP_SELECT_INVALID value became
reachable as a (fortunately not really used) ADIv6 AP ADDR.

JTAG DPBANKSEL setting support introduced with ADIv6 does not
honor DP_SELECT_INVALID correctly: required select value
gets compared to DP_SELECT_INVALID value and the most common zero
bank does not trigger DP SELECT write.

DP banked registers need just to set DP SELECT. ADIv6 AP register
addressing scheme may use both DP SELECT and SELECT1. This further
complicates using a single invalid value.

Moreover the difference how the SWD line reset influences
DPBANKSEL field between ADIv5 and ADIv6 deserves better handling
than setting select cache to zero and then to DP_SELECT_INVALID
in a very specific code positions.

Introduce bool flags indicating the validity of each SELECT
register and one SWD specific for DPBANKSEL field.
Use the latter to prevent selecting DP BANK before taking
the connection out of reset by reading DPIDR.

Treat DP SELECT and SELECT1 individually in ADIv6 64-bit mode.

Update comments to reflect the difference between ADIv5 and ADIv6
in SWD line reset.

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

diff --git a/src/target/adi_v5_jtag.c b/src/target/adi_v5_jtag.c
index afdc0e577595f44ac74bf357adb04a82d8e29f2d..8d54a50fb0b3482f4278777d045f20715306ddc6 100644 (file)
--- a/src/target/adi_v5_jtag.c
+++ b/src/target/adi_v5_jtag.c
@@ -353,17 +353,25 @@ static int adi_jtag_dp_scan_u32(struct adiv5_dap *dap,
        uint64_t sel = (reg_addr >> 4) & DP_SELECT_DPBANK;
 
        /* No need to change SELECT or RDBUFF as they are not banked */
-       if (instr == JTAG_DP_DPACC && reg_addr != DP_SELECT && reg_addr != DP_RDBUFF &&
-                       sel != (dap->select & 0xf)) {
-               if (dap->select != DP_SELECT_INVALID)
-                       sel |= dap->select & ~0xfull;
-               dap->select = sel;
-               LOG_DEBUG("DP BANKSEL: %x", (uint32_t)sel);
+       if (instr == JTAG_DP_DPACC && reg_addr != DP_SELECT && reg_addr != DP_RDBUFF
+                       && (!dap->select_valid || sel != (dap->select & DP_SELECT_DPBANK))) {
+               /* Use the AP part of dap->select regardless of dap->select_valid:
+                * if !dap->select_valid
+                * dap->select contains a speculative value likely going to be used
+                * in the following swd_queue_ap_bankselect() */
+               sel |= dap->select & SELECT_AP_MASK;
+
+               LOG_DEBUG_IO("DP BANK SELECT: %" PRIx32, (uint32_t)sel);
+
                buf_set_u32(out_value_buf, 0, 32, (uint32_t)sel);
+
                retval = adi_jtag_dp_scan(dap, JTAG_DP_DPACC,
                                DP_SELECT, DPAP_WRITE, out_value_buf, NULL, 0, NULL);
                if (retval != ERROR_OK)
                        return retval;
+
+               dap->select = sel;
+               dap->select_valid = true;
        }
        buf_set_u32(out_value_buf, 0, 32, outvalue);
 
@@ -520,7 +528,10 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
                                        /* timeout happened */
                                        if (tmp->ack == JTAG_ACK_WAIT) {
                                                LOG_ERROR("Timeout during WAIT recovery");
-                                               dap->select = DP_SELECT_INVALID;
+                                               dap->select_valid = false;
+                                               dap->select1_valid = false;
+                                               /* Keep dap->select unchanged, the same AP and AP bank
+                                                * is likely going to be used further */
                                                jtag_ap_q_abort(dap, NULL);
                                                /* clear the sticky overrun condition */
                                                adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
@@ -580,7 +591,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 
                        /* TODO: ADIv6 DP SELECT1 handling */
 
-                       dap->select = DP_SELECT_INVALID;
+                       dap->select_valid = false;
                }
 
                list_for_each_entry_safe(el, tmp, &replay_list, lh) {
@@ -615,7 +626,10 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
                        if (retval == ERROR_OK) {
                                if (el->ack == JTAG_ACK_WAIT) {
                                        LOG_ERROR("Timeout during WAIT recovery");
-                                       dap->select = DP_SELECT_INVALID;
+                                       dap->select_valid = false;
+                                       dap->select1_valid = false;
+                                       /* Keep dap->select unchanged, the same AP and AP bank
+                                        * is likely going to be used further */
                                        jtag_ap_q_abort(dap, NULL);
                                        /* clear the sticky overrun condition */
                                        adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
@@ -748,41 +762,60 @@ static int jtag_dp_q_write(struct adiv5_dap *dap, unsigned reg,
        return retval;
 }
 
-/** Select the AP register bank matching bits 7:4 of reg. */
+/** Select the AP register bank */
 static int jtag_ap_q_bankselect(struct adiv5_ap *ap, unsigned reg)
 {
        int retval;
        struct adiv5_dap *dap = ap->dap;
        uint64_t sel;
 
-       if (is_adiv6(dap)) {
+       if (is_adiv6(dap))
                sel = ap->ap_num | (reg & 0x00000FF0);
-               if (sel == (dap->select & ~0xfull))
-                       return ERROR_OK;
+       else
+               sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
 
-               if (dap->select != DP_SELECT_INVALID)
-                       sel |= dap->select & 0xf;
-               dap->select = sel;
-               LOG_DEBUG("AP BANKSEL: %" PRIx64, sel);
+       uint64_t sel_diff = (sel ^ dap->select) & SELECT_AP_MASK;
+
+       bool set_select = !dap->select_valid || (sel_diff & 0xffffffffull);
+       bool set_select1 = is_adiv6(dap) && dap->asize > 32
+                                               && (!dap->select1_valid
+                                                       || sel_diff & (0xffffffffull << 32));
+
+       if (set_select && set_select1) {
+               /* Prepare DP bank for DP_SELECT1 now to save one write */
+               sel |= (DP_SELECT1 >> 4) & DP_SELECT_DPBANK;
+       } else {
+               /* Use the DP part of dap->select regardless of dap->select_valid:
+                * if !dap->select_valid
+                * dap->select contains a speculative value likely going to be used
+                * in the following swd_queue_dp_bankselect().
+                * Moreover dap->select_valid should never be false here as a DP bank
+                * is always selected before selecting an AP bank */
+               sel |= dap->select & DP_SELECT_DPBANK;
+       }
+
+       if (set_select) {
+               LOG_DEBUG_IO("AP BANK SELECT: %" PRIx32, (uint32_t)sel);
 
                retval = jtag_dp_q_write(dap, DP_SELECT, (uint32_t)sel);
-               if (retval != ERROR_OK)
+               if (retval != ERROR_OK) {
+                       dap->select_valid = false;
                        return retval;
-
-               if (dap->asize > 32)
-                       return jtag_dp_q_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
-               return ERROR_OK;
+               }
        }
 
-       /* ADIv5 */
-       sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
+       if (set_select1) {
+               LOG_DEBUG_IO("AP BANK SELECT1: %" PRIx32, (uint32_t)(sel >> 32));
 
-       if (sel == dap->select)
-               return ERROR_OK;
+               retval = jtag_dp_q_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
+               if (retval != ERROR_OK) {
+                       dap->select1_valid = false;
+                       return retval;
+               }
+       }
 
        dap->select = sel;
-
-       return jtag_dp_q_write(dap, DP_SELECT, (uint32_t)sel);
+       return ERROR_OK;
 }
 
 static int jtag_ap_q_read(struct adiv5_ap *ap, unsigned reg,

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 1b743657c231427abfad10fe0b95e6f365d945b2..968798b324719111d8eefdb1aa790ce442366c7b 100644 (file)
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -99,27 +99,31 @@ static inline int check_sync(struct adiv5_dap *dap)
        return do_sync ? swd_run_inner(dap) : ERROR_OK;
 }
 
-/** Select the DP register bank matching bits 7:4 of reg. */
+/** Select the DP register bank */
 static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned int reg)
 {
-       /* Only register address 0 and 4 are banked. */
+       /* Only register address 0 (ADIv6 only) and 4 are banked. */
        if ((reg & 0xf) > 4)
                return ERROR_OK;
 
-       uint64_t sel = (reg & 0x000000F0) >> 4;
-       if (dap->select != DP_SELECT_INVALID)
-               sel |= dap->select & ~0xfULL;
+       uint32_t sel = (reg >> 4) & DP_SELECT_DPBANK;
 
-       if (sel == dap->select)
+       /* DP register 0 is not mapped according to ADIv5
+        * whereas ADIv6 ensures DPBANKSEL = 0 after line reset */
+       if ((dap->select_valid || ((reg & 0xf) == 0 && dap->select_dpbanksel_valid))
+                       && (sel == (dap->select & DP_SELECT_DPBANK)))
                return ERROR_OK;
 
-       dap->select = sel;
+       /* Use the AP part of dap->select regardless of dap->select_valid:
+        * if !dap->select_valid
+        * dap->select contains a speculative value likely going to be used
+        * in the following swd_queue_ap_bankselect() */
+       sel |= (uint32_t)(dap->select & SELECT_AP_MASK);
 
-       int retval = swd_queue_dp_write_inner(dap, DP_SELECT, (uint32_t)sel);
-       if (retval != ERROR_OK)
-               dap->select = DP_SELECT_INVALID;
+       LOG_DEBUG_IO("DP BANK SELECT: %" PRIx32, sel);
 
-       return retval;
+       /* dap->select cache gets updated in the following call */
+       return swd_queue_dp_write_inner(dap, DP_SELECT, sel);
 }
 
 static int swd_queue_dp_read_inner(struct adiv5_dap *dap, unsigned int reg,
@@ -147,24 +151,31 @@ static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
        swd_finish_read(dap);
 
        if (reg == DP_SELECT) {
-               dap->select = data & (ADIV5_DP_SELECT_APSEL | ADIV5_DP_SELECT_APBANK | DP_SELECT_DPBANK);
+               dap->select = data | (dap->select & (0xffffffffull << 32));
 
                swd->write_reg(swd_cmd(false, false, reg), data, 0);
 
                retval = check_sync(dap);
-               if (retval != ERROR_OK)
-                       dap->select = DP_SELECT_INVALID;
+               dap->select_valid = (retval == ERROR_OK);
+               dap->select_dpbanksel_valid = dap->select_valid;
 
                return retval;
        }
 
+       if (reg == DP_SELECT1)
+               dap->select = ((uint64_t)data << 32) | (dap->select & 0xffffffffull);
+
        retval = swd_queue_dp_bankselect(dap, reg);
-       if (retval != ERROR_OK)
-               return retval;
+       if (retval == ERROR_OK) {
+               swd->write_reg(swd_cmd(false, false, reg), data, 0);
+
+               retval = check_sync(dap);
+       }
 
-       swd->write_reg(swd_cmd(false, false, reg), data, 0);
+       if (reg == DP_SELECT1)
+               dap->select1_valid = (retval == ERROR_OK);
 
-       return check_sync(dap);
+       return retval;
 }
 
 
@@ -177,19 +188,17 @@ static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr
        assert(dap_is_multidrop(dap));
 
        swd_send_sequence(dap, LINE_RESET);
-       /* From ARM IHI 0074C ADIv6.0, chapter B4.3.3 "Connection and line reset
-        * sequence":
-        * - line reset sets DP_SELECT_DPBANK to zero;
-        * - read of DP_DPIDR takes the connection out of reset;
-        * - write of DP_TARGETSEL keeps the connection in reset;
-        * - other accesses return protocol error (SWDIO not driven by target).
-        *
-        * Read DP_DPIDR to get out of reset. Initialize dap->select to zero to
-        * skip the write to DP_SELECT, avoiding the protocol error. Set again
-        * dap->select to DP_SELECT_INVALID because the rest of the register is
-        * unknown after line reset.
+       /*
+        * Zero dap->select and set dap->select_dpbanksel_valid
+        * to skip the write to DP_SELECT before DPIDR read, avoiding
+        * the protocol error.
+        * Clear the other validity flags because the rest of the DP
+        * SELECT and SELECT1 registers is unknown after line reset.
         */
        dap->select = 0;
+       dap->select_dpbanksel_valid = true;
+       dap->select_valid = false;
+       dap->select1_valid = false;
 
        retval = swd_queue_dp_write_inner(dap, DP_TARGETSEL, dap->multidrop_targetsel);
        if (retval != ERROR_OK)
@@ -209,8 +218,6 @@ static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr
                        return retval;
        }
 
-       dap->select = DP_SELECT_INVALID;
-
        retval = swd_queue_dp_read_inner(dap, DP_DLPIDR, &dlpidr);
        if (retval != ERROR_OK)
                return retval;
@@ -335,19 +342,20 @@ static int swd_connect_single(struct adiv5_dap *dap)
 
                /* The sequences to enter in SWD (JTAG_TO_SWD and DORMANT_TO_SWD) end
                 * with a SWD line reset sequence (50 clk with SWDIO high).
-                * From ARM IHI 0074C ADIv6.0, chapter B4.3.3 "Connection and line reset
-                * sequence":
-                * - line reset sets DP_SELECT_DPBANK to zero;
+                * From ARM IHI 0031F ADIv5.2 and ARM IHI 0074C ADIv6.0,
+                * chapter B4.3.3 "Connection and line reset sequence":
+                * - DPv3 (ADIv6) only: line reset sets DP_SELECT_DPBANK to zero;
                 * - read of DP_DPIDR takes the connection out of reset;
                 * - write of DP_TARGETSEL keeps the connection in reset;
                 * - other accesses return protocol error (SWDIO not driven by target).
                 *
-                * Read DP_DPIDR to get out of reset. Initialize dap->select to zero to
-                * skip the write to DP_SELECT, avoiding the protocol error. Set again
-                * dap->select to DP_SELECT_INVALID because the rest of the register is
-                * unknown after line reset.
+                * dap_invalidate_cache() sets dap->select to zero and all validity
+                * flags to invalid. Set dap->select_dpbanksel_valid only
+                * to skip the write to DP_SELECT, avoiding the protocol error.
+                * Read DP_DPIDR to get out of reset.
                 */
-               dap->select = 0;
+               dap->select_dpbanksel_valid = true;
+
                retval = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr);
                if (retval == ERROR_OK) {
                        retval = swd_run_inner(dap);
@@ -360,8 +368,6 @@ static int swd_connect_single(struct adiv5_dap *dap)
                dap->switch_through_dormant = !dap->switch_through_dormant;
        } while (timeval_ms() < timeout);
 
-       dap->select = DP_SELECT_INVALID;
-
        if (retval != ERROR_OK) {
                LOG_ERROR("Error connecting DP: cannot read IDR");
                return retval;
@@ -494,49 +500,55 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
        return swd_queue_dp_write_inner(dap, reg, data);
 }
 
-/** Select the AP register bank matching bits 7:4 of reg. */
+/** Select the AP register bank */
 static int swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg)
 {
        int retval;
        struct adiv5_dap *dap = ap->dap;
        uint64_t sel;
 
-       if (is_adiv6(dap)) {
+       if (is_adiv6(dap))
                sel = ap->ap_num | (reg & 0x00000FF0);
-               if (sel == (dap->select & ~0xfULL))
-                       return ERROR_OK;
-
-               if (dap->select != DP_SELECT_INVALID)
-                       sel |= dap->select & 0xf;
-               dap->select = sel;
-               LOG_DEBUG("AP BANKSEL: %" PRIx64, sel);
-
-               retval = swd_queue_dp_write(dap, DP_SELECT, (uint32_t)sel);
+       else
+               sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
 
-               if (retval == ERROR_OK && dap->asize > 32)
-                       retval = swd_queue_dp_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
+       uint64_t sel_diff = (sel ^ dap->select) & SELECT_AP_MASK;
 
-               if (retval != ERROR_OK)
-                       dap->select = DP_SELECT_INVALID;
+       bool set_select = !dap->select_valid || (sel_diff & 0xffffffffull);
+       bool set_select1 = is_adiv6(dap) && dap->asize > 32
+                                               && (!dap->select1_valid
+                                                       || sel_diff & (0xffffffffull << 32));
 
-               return retval;
+       if (set_select && set_select1) {
+               /* Prepare DP bank for DP_SELECT1 now to save one write */
+               sel |= (DP_SELECT1 & 0x000000f0) >> 4;
+       } else {
+               /* Use the DP part of dap->select regardless of dap->select_valid:
+                * if !dap->select_valid
+                * dap->select contains a speculative value likely going to be used
+                * in the following swd_queue_dp_bankselect().
+                * Moreover dap->select_valid should never be false here as a DP bank
+                * is always selected before selecting an AP bank */
+               sel |= dap->select & DP_SELECT_DPBANK;
        }
 
-       /* ADIv5 */
-       sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
-       if (dap->select != DP_SELECT_INVALID)
-               sel |= dap->select & DP_SELECT_DPBANK;
+       if (set_select) {
+               LOG_DEBUG_IO("AP BANK SELECT: %" PRIx32, (uint32_t)sel);
 
-       if (sel == dap->select)
-               return ERROR_OK;
+               retval = swd_queue_dp_write(dap, DP_SELECT, (uint32_t)sel);
+               if (retval != ERROR_OK)
+                       return retval;
+       }
 
-       dap->select = sel;
+       if (set_select1) {
+               LOG_DEBUG_IO("AP BANK SELECT1: %" PRIx32, (uint32_t)(sel >> 32));
 
-       retval = swd_queue_dp_write_inner(dap, DP_SELECT, sel);
-       if (retval != ERROR_OK)
-               dap->select = DP_SELECT_INVALID;
+               retval = swd_queue_dp_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
+               if (retval != ERROR_OK)
+                       return retval;
+       }
 
-       return retval;
+       return ERROR_OK;
 }
 
 static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg,

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index da5da3197d227965ba7d4e2f56eb138ab0a921b7..434bf50fe1074461b4dbcbfe8cd21a22921c878d 100644 (file)
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -655,7 +655,11 @@ int mem_ap_write_buf_noincr(struct adiv5_ap *ap,
  */
 void dap_invalidate_cache(struct adiv5_dap *dap)
 {
-       dap->select = DP_SELECT_INVALID;
+       dap->select = 0;        /* speculate the first AP access will select AP 0, bank 0 */
+       dap->select_valid = false;
+       dap->select1_valid = false;
+       dap->select_dpbanksel_valid = false;
+
        dap->last_read = NULL;
 
        int i;

diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index e21589363d61a560fd2e780cddbf335fb4d020a5..e07b577af683f047365b875bba7543fce4a418c8 100644 (file)
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -100,7 +100,11 @@
 #define ADIV5_DP_SELECT_APSEL  0xFF000000
 #define ADIV5_DP_SELECT_APBANK 0x000000F0
 #define DP_SELECT_DPBANK               0x0000000F
-#define DP_SELECT_INVALID              0x00FFFF00 /* Reserved bits one */
+/*
+ * Mask of AP ADDR in select cache, concatenating DP SELECT and DP_SELECT1.
+ * In case of ADIv5, the mask contains both APSEL and APBANKSEL fields.
+ */
+#define SELECT_AP_MASK                 (~(uint64_t)DP_SELECT_DPBANK)
 
 #define DP_APSEL_MAX                   (255) /* Strict limit for ADIv5, number of AP buffers for ADIv6 */
 #define DP_APSEL_INVALID               0xF00 /* more than DP_APSEL_MAX and not ADIv6 aligned 4k */
@@ -338,11 +342,21 @@ struct adiv5_dap {
        /* The current manually selected AP by the "dap apsel" command */
        uint64_t apsel;
 
+       /** Cache for DP SELECT and SELECT1 (ADIv6) register. */
+       uint64_t select;
+       /** Validity of DP SELECT cache. false will force register rewrite */
+       bool select_valid;
+       bool select1_valid;     /* ADIv6 only */
        /**
-        * Cache for DP_SELECT register. A value of DP_SELECT_INVALID
-        * indicates no cached value and forces rewrite of the register.
+        * Partial DPBANKSEL validity for SWD only.
+        * ADIv6 line reset sets DP SELECT DPBANKSEL to zero,
+        * ADIv5 does not.
+        * We can rely on it for the banked DP register 0 also on ADIv5
+        * as ADIv5 has no mapping for DP reg 0 - it is always DPIDR.
+        * It is important to avoid setting DP SELECT in connection
+        * reset state before reading DPIDR.
         */
-       uint64_t select;
+       bool select_dpbanksel_valid;
 
        /* information about current pending SWjDP-AHBAP transaction */
        uint8_t  ack;