ipdbg: fix double free of virtual-ir data

[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 6d6f287b05f313f545ce105f46115746ee8a759a..12310058844522e7293f3fcfda74cbd2c9ec12d9 100644 (file)
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -84,16 +84,8 @@ static void swd_clear_sticky_errors(struct adiv5_dap *dap)
 static int swd_run_inner(struct adiv5_dap *dap)
 {
        const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
-       int retval;
-
-       retval = swd->run();
-
-       if (retval != ERROR_OK) {
-               /* fault response */
-               dap->do_reconnect = true;
-       }
 
-       return retval;
+       return swd->run();
 }
 
 static inline int check_sync(struct adiv5_dap *dap)
@@ -105,14 +97,13 @@ static inline int check_sync(struct adiv5_dap *dap)
 static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned int reg)
 {
        /* Only register address 0 (ADIv6 only) and 4 are banked. */
-       if ((reg & 0xf) > 4)
+       if (is_adiv6(dap) ? (reg & 0xf) > 4 : (reg & 0xf) != 4)
                return ERROR_OK;
 
        uint32_t sel = (reg >> 4) & DP_SELECT_DPBANK;
 
-       /* 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))
+       /* ADIv6 ensures DPBANKSEL = 0 after line reset */
+       if ((dap->select_valid || (is_adiv6(dap) && dap->select_dpbanksel_valid))
                        && (sel == (dap->select & DP_SELECT_DPBANK)))
                return ERROR_OK;
 
@@ -146,7 +137,7 @@ static int swd_queue_dp_read_inner(struct adiv5_dap *dap, unsigned int reg,
 static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
                uint32_t data)
 {
-       int retval;
+       int retval = ERROR_OK;
        const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
        assert(swd);
 
@@ -167,7 +158,11 @@ static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
        if (reg == DP_SELECT1)
                dap->select = ((uint64_t)data << 32) | (dap->select & 0xffffffffull);
 
-       retval = swd_queue_dp_bankselect(dap, reg);
+       /* DP_ABORT write is not banked.
+        * Prevent writing DP_SELECT before as it would fail on locked up DP */
+       if (reg != DP_ABORT)
+               retval = swd_queue_dp_bankselect(dap, reg);
+
        if (retval == ERROR_OK) {
                swd->write_reg(swd_cmd(false, false, reg), data, 0);
 
@@ -285,15 +280,15 @@ static int swd_multidrop_select(struct adiv5_dap *dap)
                swd_multidrop_selected_dap = NULL;
                if (retry > 3) {
                        LOG_ERROR("Failed to select multidrop %s", adiv5_dap_name(dap));
+                       dap->do_reconnect = true;
                        return retval;
                }
 
                LOG_DEBUG("Failed to select multidrop %s, retrying...",
                                  adiv5_dap_name(dap));
-               /* we going to retry localy, do not ask for full reconnect */
-               dap->do_reconnect = false;
        }
 
+       dap->do_reconnect = false;
        return retval;
 }
 
@@ -632,7 +627,13 @@ static int swd_run(struct adiv5_dap *dap)
 
        swd_finish_read(dap);
 
-       return swd_run_inner(dap);
+       retval = swd_run_inner(dap);
+       if (retval != ERROR_OK) {
+               /* fault response */
+               dap->do_reconnect = true;
+       }
+
+       return retval;
 }
 
 /** Put the SWJ-DP back to JTAG mode */