adi_v5_swd: Avoid special handling the SELECT cache during connect

[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 c080964d07a1275840406af7ad0e8a51143542aa..771b46090e175c2d91b694fda67517afb08e286b 100644 (file)
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -112,9 +112,9 @@ static int swd_connect(struct adiv5_dap *dap)
        /* Note, debugport_init() does setup too */
        jtag_interface->swd->switch_seq(JTAG_TO_SWD);
 
-       /* Make sure we don't try to perform any other accesses before the DPIDR read. */
+       /* Clear link state, including the SELECT cache. */
        dap->do_reconnect = false;
-       dap->select = 0;
+       dap->select = DP_SELECT_INVALID;
 
        swd_queue_dp_read(dap, DP_IDCODE, &idcode);
 
@@ -158,19 +158,20 @@ static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
 /** Select the DP register bank matching bits 7:4 of reg. */
 static void swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned reg)
 {
-       if (reg == DP_SELECT)
+       /* Only register address 4 is banked. */
+       if ((reg & 0xf) != 4)
                return;
 
        uint32_t select_dp_bank = (reg & 0x000000F0) >> 4;
-       uint32_t select = select_dp_bank
+       uint32_t sel = select_dp_bank
                        | (dap->select & (DP_SELECT_APSEL | DP_SELECT_APBANK));
 
-       if (select == dap->select)
+       if (sel == dap->select)
                return;
 
-       dap->select = select;
+       dap->select = sel;
 
-       swd_queue_dp_write(dap, DP_SELECT, select);
+       swd_queue_dp_write(dap, DP_SELECT, sel);
 }
 
 static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
@@ -210,16 +211,16 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 static void swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg)
 {
        struct adiv5_dap *dap = ap->dap;
-       uint32_t select = ((uint32_t)ap->ap_num << 24)
+       uint32_t sel = ((uint32_t)ap->ap_num << 24)
                        | (reg & 0x000000F0)
                        | (dap->select & DP_SELECT_DPBANK);
 
-       if (select == dap->select)
+       if (sel == dap->select)
                return;
 
-       dap->select = select;
+       dap->select = sel;
 
-       swd_queue_dp_write(dap, DP_SELECT, select);
+       swd_queue_dp_write(dap, DP_SELECT, sel);
 }
 
 static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg,