adi_v5_swd: Avoid special handling the SELECT cache during connect

[openocd] / src / target / adi_v5_swd.c

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 2cb61f830bedc42dd95549feda8df0636a21c87e..771b46090e175c2d91b694fda67517afb08e286b 100644 (file)
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -63,7 +63,7 @@ static void swd_finish_read(struct adiv5_dap *dap)
 {
        const struct swd_driver *swd = jtag_interface->swd;
        if (dap->last_read != NULL) {
-               swd->read_reg(dap, swd_cmd(true, false, DP_RDBUFF), dap->last_read);
+               swd->read_reg(swd_cmd(true, false, DP_RDBUFF), dap->last_read, 0);
                dap->last_read = NULL;
        }
 }
@@ -78,8 +78,8 @@ static void swd_clear_sticky_errors(struct adiv5_dap *dap)
        const struct swd_driver *swd = jtag_interface->swd;
        assert(swd);
 
-       swd->write_reg(dap, swd_cmd(false,  false, DP_ABORT),
-               STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+       swd->write_reg(swd_cmd(false,  false, DP_ABORT),
+               STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
 }
 
 static int swd_run_inner(struct adiv5_dap *dap)
@@ -87,7 +87,7 @@ static int swd_run_inner(struct adiv5_dap *dap)
        const struct swd_driver *swd = jtag_interface->swd;
        int retval;
 
-       retval = swd->run(dap);
+       retval = swd->run();
 
        if (retval != ERROR_OK) {
                /* fault response */
@@ -110,9 +110,11 @@ static int swd_connect(struct adiv5_dap *dap)
         */
 
        /* Note, debugport_init() does setup too */
-       jtag_interface->swd->switch_seq(dap, JTAG_TO_SWD);
+       jtag_interface->swd->switch_seq(JTAG_TO_SWD);
 
+       /* Clear link state, including the SELECT cache. */
        dap->do_reconnect = false;
+       dap->select = DP_SELECT_INVALID;
 
        swd_queue_dp_read(dap, DP_IDCODE, &idcode);
 
@@ -148,26 +150,28 @@ static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
        const struct swd_driver *swd = jtag_interface->swd;
        assert(swd);
 
-       swd->write_reg(dap, swd_cmd(false,  false, DP_ABORT),
-               DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR);
+       swd->write_reg(swd_cmd(false,  false, DP_ABORT),
+               DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
        return check_sync(dap);
 }
 
 /** Select the DP register bank matching bits 7:4 of reg. */
 static void swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned reg)
 {
-       uint32_t select_dp_bank = (reg & 0x000000F0) >> 4;
-
-       if (reg == DP_SELECT)
+       /* Only register address 4 is banked. */
+       if ((reg & 0xf) != 4)
                return;
 
-       if (select_dp_bank == dap->dp_bank_value)
+       uint32_t select_dp_bank = (reg & 0x000000F0) >> 4;
+       uint32_t sel = select_dp_bank
+                       | (dap->select & (DP_SELECT_APSEL | DP_SELECT_APBANK));
+
+       if (sel == dap->select)
                return;
 
-       dap->dp_bank_value = select_dp_bank;
-       select_dp_bank |= dap->ap_current | dap->ap_bank_value;
+       dap->select = sel;
 
-       swd_queue_dp_write(dap, DP_SELECT, select_dp_bank);
+       swd_queue_dp_write(dap, DP_SELECT, sel);
 }
 
 static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
@@ -181,7 +185,7 @@ static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg,
                return retval;
 
        swd_queue_dp_bankselect(dap, reg);
-       swd->read_reg(dap, swd_cmd(true,  false, reg), data);
+       swd->read_reg(swd_cmd(true,  false, reg), data, 0);
 
        return check_sync(dap);
 }
@@ -198,55 +202,61 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
 
        swd_finish_read(dap);
        swd_queue_dp_bankselect(dap, reg);
-       swd->write_reg(dap, swd_cmd(false,  false, reg), data);
+       swd->write_reg(swd_cmd(false,  false, reg), data, 0);
 
        return check_sync(dap);
 }
 
 /** Select the AP register bank matching bits 7:4 of reg. */
-static void swd_queue_ap_bankselect(struct adiv5_dap *dap, unsigned reg)
+static void swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg)
 {
-       uint32_t select_ap_bank = reg & 0x000000F0;
+       struct adiv5_dap *dap = ap->dap;
+       uint32_t sel = ((uint32_t)ap->ap_num << 24)
+                       | (reg & 0x000000F0)
+                       | (dap->select & DP_SELECT_DPBANK);
 
-       if (select_ap_bank == dap->ap_bank_value)
+       if (sel == dap->select)
                return;
 
-       dap->ap_bank_value = select_ap_bank;
-       select_ap_bank |= dap->ap_current | dap->dp_bank_value;
+       dap->select = sel;
 
-       swd_queue_dp_write(dap, DP_SELECT, select_ap_bank);
+       swd_queue_dp_write(dap, DP_SELECT, sel);
 }
 
-static int swd_queue_ap_read(struct adiv5_dap *dap, unsigned reg,
+static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
                uint32_t *data)
 {
        const struct swd_driver *swd = jtag_interface->swd;
        assert(swd);
 
+       struct adiv5_dap *dap = ap->dap;
+
        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);
+       swd_queue_ap_bankselect(ap, reg);
+       swd->read_reg(swd_cmd(true,  true, reg), dap->last_read, ap->memaccess_tck);
        dap->last_read = data;
 
        return check_sync(dap);
 }
 
-static int swd_queue_ap_write(struct adiv5_dap *dap, unsigned reg,
+static int swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg,
                uint32_t data)
 {
        const struct swd_driver *swd = jtag_interface->swd;
        assert(swd);
 
+       struct adiv5_dap *dap = ap->dap;
+
        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);
+       swd_queue_ap_bankselect(ap, reg);
+       swd->write_reg(swd_cmd(false,  true, reg), data, ap->memaccess_tck);
 
        return check_sync(dap);
 }
@@ -259,8 +269,6 @@ static int swd_run(struct adiv5_dap *dap)
 }
 
 const struct dap_ops swd_dap_ops = {
-       .is_swd = true,
-
        .queue_dp_read = swd_queue_dp_read,
        .queue_dp_write = swd_queue_dp_write,
        .queue_ap_read = swd_queue_ap_read,