adi_v5_swd: Avoid special handling the SELECT cache during connect

The cache is forced to zero to match the value expected by the DPIDR read
so the connect sequence is not destroyed by a SELECT update.

However, DPIDR and in fact all registers except address 4 are independent
of the current DPBANKSEL value. Change swd_queue_dp_bankselect() to use
this fact and avoid touching SELECT for those registers.

Change-Id: I0cd11925fb6adef481bbf45cc24ea2c6dab4b6fb
Signed-off-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Reviewed-on: http://openocd.zylin.com/3231
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index f7a199c56c4735d0ddd4e5202c8df9bcff3365ad..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);
 
@@ -123,8 +123,6 @@ static int swd_connect(struct adiv5_dap *dap)
 
        status = swd_run_inner(dap);
 
-       dap->select = DP_SELECT_INVALID;
-
        if (status == ERROR_OK) {
                LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode);
                dap->do_reconnect = false;
@@ -160,7 +158,8 @@ 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;