This is an automated email from Gerrit.

"Tomas Vanek <van...@fbl.cz>" just uploaded a new patch set to Gerrit, which 
you can find at https://review.openocd.org/c/openocd/+/8728

-- gerrit

commit 997c5cd878b505d5355b2b2f8879dc058e865e7b
Author: Tomas Vanek <van...@fbl.cz>
Date:   Tue Jan 28 11:01:31 2025 +0100

    target/adi_v5_swd, drivers: allow error passing from SWD read/write reg
    
    SWD driver read/write reg operations were originally meant as pure
    queuing and therefore do not return any error code.
    
    The real SWD implementation in an adapter driver either executes
    SWD operation immediately (bitbang etc...) or eventually runs
    the queue as soon as it's getting full.
    
    In both cases the adapter driver code needs to pass an eventual error
    to the caller. The queuing of the return value complicates the code
    of the driver and is more error prone than standard error passing.
    
    Get rid of the queued retval concept partially.
    Change swd->read/write_reg() adapter driver methods to return error
    code.
    Check returned error code in ADI SWD layer and request reconnect
    as error recovery after an error.
    Remove queued retval from bitbang style adapter drivers.
    
    To prevent oversizing the patch other adapter drivers return
    unconditional ERROR_OK from read/write_reg() for the moment.
    This should be functionally identical to the previous code
    and the drivers could be retval-de-queued one by one.
    
    Change-Id: Ib9c671b656998a91c0e7626beed185c9c156d32b
    Signed-off-by: Tomas Vanek <van...@fbl.cz>

diff --git a/src/jtag/drivers/bitbang.c b/src/jtag/drivers/bitbang.c
index 0a49feb003..7f9c95b734 100644
--- a/src/jtag/drivers/bitbang.c
+++ b/src/jtag/drivers/bitbang.c
@@ -35,7 +35,7 @@
  */
 static int bitbang_stableclocks(unsigned int num_cycles);
 
-static void bitbang_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk);
+static int bitbang_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk);
 
 const struct bitbang_interface *bitbang_interface;
 
@@ -384,8 +384,6 @@ int bitbang_execute_queue(struct jtag_command *cmd_queue)
        return retval;
 }
 
-static int queued_retval;
-
 static int bitbang_swd_init(void)
 {
        LOG_DEBUG("bitbang_swd_init");
@@ -467,15 +465,10 @@ static void swd_clear_sticky_errors(void)
                STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
 }
 
-static void bitbang_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
+static int bitbang_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
 {
        assert(cmd & SWD_CMD_RNW);
 
-       if (queued_retval != ERROR_OK) {
-               LOG_DEBUG("Skip bitbang_swd_read_reg because queued_retval=%d", 
queued_retval);
-               return;
-       }
-
        int64_t timeout = timeval_ms() + SWD_WAIT_TIMEOUT;
        for (unsigned int retry = 0;; retry++) {
                uint8_t trn_ack_data_parity_trn[DIV_ROUND_UP(4 + 3 + 32 + 1 + 
4, 8)];
@@ -510,32 +503,25 @@ static void bitbang_swd_read_reg(uint8_t cmd, uint32_t 
*value, uint32_t ap_delay
                        LOG_DEBUG("SWD WAIT: retried %u times", retry);
 
                if (ack != SWD_ACK_OK) {
-                       queued_retval = swd_ack_to_error_code(ack);
-                       return;
+                       return swd_ack_to_error_code(ack);
                }
 
                if (parity != parity_u32(data)) {
                        LOG_ERROR("Wrong parity detected");
-                       queued_retval = ERROR_FAIL;
-                       return;
+                       return ERROR_SWD_FAIL;
                }
                if (value)
                        *value = data;
                if (cmd & SWD_CMD_APNDP)
                        bitbang_swd_exchange(true, NULL, 0, ap_delay_clk);
-               return;
+               return ERROR_OK;
        }
 }
 
-static void bitbang_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
+static int bitbang_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
 {
        assert(!(cmd & SWD_CMD_RNW));
 
-       if (queued_retval != ERROR_OK) {
-               LOG_DEBUG("Skip bitbang_swd_write_reg because 
queued_retval=%d", queued_retval);
-               return;
-       }
-
        int64_t timeout = timeval_ms() + SWD_WAIT_TIMEOUT;
 
        /* Devices do not reply to DP_TARGETSEL write cmd, ignore received ack 
*/
@@ -589,13 +575,12 @@ static void bitbang_swd_write_reg(uint8_t cmd, uint32_t 
value, uint32_t ap_delay
                        LOG_DEBUG("SWD WAIT: retried %u times", retry);
 
                if (check_ack && ack != SWD_ACK_OK) {
-                       queued_retval = swd_ack_to_error_code(ack);
-                       return;
+                       return swd_ack_to_error_code(ack);
                }
 
                if (cmd & SWD_CMD_APNDP)
                        bitbang_swd_exchange(true, NULL, 0, ap_delay_clk);
-               return;
+               return ERROR_OK;
        }
 }
 
@@ -605,10 +590,7 @@ static int bitbang_swd_run_queue(void)
         * ensure that data is clocked through the AP. */
        bitbang_swd_exchange(true, NULL, 0, 8);
 
-       int retval = queued_retval;
-       queued_retval = ERROR_OK;
-       LOG_DEBUG_IO("SWD queue return value: %02x", retval);
-       return retval;
+       return ERROR_OK;
 }
 
 const struct swd_driver bitbang_swd = {
diff --git a/src/jtag/drivers/buspirate.c b/src/jtag/drivers/buspirate.c
index 6f8df5adad..6b5f394b0f 100644
--- a/src/jtag/drivers/buspirate.c
+++ b/src/jtag/drivers/buspirate.c
@@ -98,7 +98,6 @@ enum {
 
 /* SWD mode specific */
 static bool swd_mode;
-static int  queued_retval;
 static char swd_features;
 
 static int buspirate_fd = -1;
@@ -1401,18 +1400,13 @@ static void buspirate_swd_clear_sticky_errors(void)
                STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
 }
 
-static void buspirate_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
+static int buspirate_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
 {
        uint8_t tmp[16];
 
        LOG_DEBUG("buspirate_swd_read_reg");
        assert(cmd & SWD_CMD_RNW);
 
-       if (queued_retval != ERROR_OK) {
-               LOG_DEBUG("Skip buspirate_swd_read_reg because 
queued_retval=%d", queued_retval);
-               return;
-       }
-
        cmd |= SWD_CMD_START | SWD_CMD_PARK;
        uint8_t ack = buspirate_swd_write_header(cmd);
 
@@ -1445,26 +1439,23 @@ static void buspirate_swd_read_reg(uint8_t cmd, 
uint32_t *value, uint32_t ap_del
        case SWD_ACK_OK:
                if (parity != parity_u32(data)) {
                        LOG_DEBUG("Read data parity mismatch %x %x", parity, 
parity_u32(data));
-                       queued_retval = ERROR_FAIL;
-                       return;
+                       return ERROR_SWD_FAIL;
                }
                if (value)
                        *value = data;
                if (cmd & SWD_CMD_APNDP)
                        buspirate_swd_idle_clocks(ap_delay_clk);
-               return;
+               return ERROR_OK;
        case SWD_ACK_WAIT:
                LOG_DEBUG("SWD_ACK_WAIT");
                buspirate_swd_clear_sticky_errors();
-               return;
+               return ERROR_WAIT;
        case SWD_ACK_FAULT:
                LOG_DEBUG("SWD_ACK_FAULT");
-               queued_retval = ack;
-               return;
+               return ERROR_SWD_FAULT;
        default:
                LOG_DEBUG("No valid acknowledge: ack=%d", ack);
-               queued_retval = ack;
-               return;
+               return ERROR_SWD_FAIL;
        }
 }
 
@@ -1475,11 +1466,6 @@ static void buspirate_swd_write_reg(uint8_t cmd, 
uint32_t value, uint32_t ap_del
        LOG_DEBUG("buspirate_swd_write_reg");
        assert(!(cmd & SWD_CMD_RNW));
 
-       if (queued_retval != ERROR_OK) {
-               LOG_DEBUG("Skip buspirate_swd_write_reg because 
queued_retval=%d", queued_retval);
-               return;
-       }
-
        cmd |= SWD_CMD_START | SWD_CMD_PARK;
        uint8_t ack = buspirate_swd_write_header(cmd);
 
@@ -1503,19 +1489,17 @@ static void buspirate_swd_write_reg(uint8_t cmd, 
uint32_t value, uint32_t ap_del
        case SWD_ACK_OK:
                if (cmd & SWD_CMD_APNDP)
                        buspirate_swd_idle_clocks(ap_delay_clk);
-               return;
+               return ERROR_OK;
        case SWD_ACK_WAIT:
                LOG_DEBUG("SWD_ACK_WAIT");
                buspirate_swd_clear_sticky_errors();
-               return;
+               return ERROR_WAIT;
        case SWD_ACK_FAULT:
                LOG_DEBUG("SWD_ACK_FAULT");
-               queued_retval = ack;
-               return;
+               return ERROR_SWD_FAULT;
        default:
                LOG_DEBUG("No valid acknowledge: ack=%d", ack);
-               queued_retval = ack;
-               return;
+               return ERROR_SWD_FAIL;
        }
 }
 
@@ -1526,8 +1510,5 @@ static int buspirate_swd_run_queue(void)
         * ensure that data is clocked through the AP. */
        buspirate_swd_idle_clocks(8);
 
-       int retval = queued_retval;
-       queued_retval = ERROR_OK;
-       LOG_DEBUG("SWD queue return value: %02x", retval);
-       return retval;
+       return ERROR_OK;
 }
diff --git a/src/jtag/drivers/cmsis_dap.c b/src/jtag/drivers/cmsis_dap.c
index e9fd93ad1b..5f03d43c91 100644
--- a/src/jtag/drivers/cmsis_dap.c
+++ b/src/jtag/drivers/cmsis_dap.c
@@ -1102,16 +1102,20 @@ static void cmsis_dap_swd_queue_cmd(uint8_t cmd, 
uint32_t *dst, uint32_t data)
        block->transfer_count++;
 }
 
-static void cmsis_dap_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
+static int cmsis_dap_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
 {
        assert(!(cmd & SWD_CMD_RNW));
        cmsis_dap_swd_queue_cmd(cmd, NULL, value);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
-static void cmsis_dap_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
+static int cmsis_dap_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
 {
        assert(cmd & SWD_CMD_RNW);
        cmsis_dap_swd_queue_cmd(cmd, value, 0);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
 static int cmsis_dap_get_serial_info(void)
diff --git a/src/jtag/drivers/ftdi.c b/src/jtag/drivers/ftdi.c
index 42ecda117b..514a367430 100644
--- a/src/jtag/drivers/ftdi.c
+++ b/src/jtag/drivers/ftdi.c
@@ -1181,16 +1181,20 @@ static void ftdi_swd_queue_cmd(uint8_t cmd, uint32_t 
*dst, uint32_t data, uint32
 
 }
 
-static void ftdi_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
+static int ftdi_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
 {
        assert(cmd & SWD_CMD_RNW);
        ftdi_swd_queue_cmd(cmd, value, 0, ap_delay_clk);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
-static void ftdi_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
+static int ftdi_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
 {
        assert(!(cmd & SWD_CMD_RNW));
        ftdi_swd_queue_cmd(cmd, NULL, value, ap_delay_clk);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
 static int ftdi_swd_switch_seq(enum swd_special_seq seq)
diff --git a/src/jtag/drivers/jlink.c b/src/jtag/drivers/jlink.c
index 74660744a7..85af4756b9 100644
--- a/src/jtag/drivers/jlink.c
+++ b/src/jtag/drivers/jlink.c
@@ -1947,16 +1947,20 @@ static int jlink_swd_init(void)
        return ERROR_OK;
 }
 
-static void jlink_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
+static int jlink_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
 {
        assert(!(cmd & SWD_CMD_RNW));
        jlink_swd_queue_cmd(cmd, NULL, value, ap_delay_clk);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
-static void jlink_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
+static int jlink_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
 {
        assert(cmd & SWD_CMD_RNW);
        jlink_swd_queue_cmd(cmd, value, 0, ap_delay_clk);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
 /***************************************************************************/
diff --git a/src/jtag/drivers/kitprog.c b/src/jtag/drivers/kitprog.c
index 98b0d16681..1d42f8aa53 100644
--- a/src/jtag/drivers/kitprog.c
+++ b/src/jtag/drivers/kitprog.c
@@ -629,16 +629,20 @@ static int kitprog_swd_init(void)
        return ERROR_OK;
 }
 
-static void kitprog_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
+static int kitprog_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
 {
        assert(!(cmd & SWD_CMD_RNW));
        kitprog_swd_queue_cmd(cmd, NULL, value);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
-static void kitprog_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
+static int kitprog_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
 {
        assert(cmd & SWD_CMD_RNW);
        kitprog_swd_queue_cmd(cmd, value, 0);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
 /*************** swd lowlevel functions ********************/
diff --git a/src/jtag/drivers/linuxspidev.c b/src/jtag/drivers/linuxspidev.c
index 737d2bef83..77b8fa7375 100644
--- a/src/jtag/drivers/linuxspidev.c
+++ b/src/jtag/drivers/linuxspidev.c
@@ -107,7 +107,7 @@ static uint8_t *queue_rx_buf;
 static uint8_t *tx_flip_buf;
 
 static int spidev_swd_switch_seq(enum swd_special_seq seq);
-static void spidev_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk);
+static int spidev_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk);
 
 static void spi_exchange(const uint8_t *tx_data, uint8_t *rx_data, unsigned 
int len)
 {
@@ -473,16 +473,20 @@ static void spidev_swd_queue_cmd(uint8_t cmd, uint32_t 
*dst, uint32_t data, uint
        queue_fill++;
 }
 
-static void spidev_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
+static int spidev_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
 {
        assert(cmd & SWD_CMD_RNW);
        spidev_swd_queue_cmd(cmd, value, 0, ap_delay_clk);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
-static void spidev_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
+static int spidev_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
 {
        assert(!(cmd & SWD_CMD_RNW));
        spidev_swd_queue_cmd(cmd, NULL, value, ap_delay_clk);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
 static int spidev_swd_switch_seq(enum swd_special_seq seq)
diff --git a/src/jtag/drivers/vsllink.c b/src/jtag/drivers/vsllink.c
index ca142177ec..8d9a413649 100644
--- a/src/jtag/drivers/vsllink.c
+++ b/src/jtag/drivers/vsllink.c
@@ -736,14 +736,18 @@ static int vsllink_swd_switch_seq(enum swd_special_seq 
seq)
        return ERROR_OK;
 }
 
-static void vsllink_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
+static int vsllink_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t 
ap_delay_clk)
 {
        versaloon_interface.adaptors.swd.transact(0, cmd, value, NULL);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
-static void vsllink_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
+static int vsllink_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t 
ap_delay_clk)
 {
        versaloon_interface.adaptors.swd.transact(0, cmd, &value, NULL);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
 static int vsllink_swd_run_queue(void)
diff --git a/src/jtag/drivers/xds110.c b/src/jtag/drivers/xds110.c
index 6e12b9a790..5f5401321c 100644
--- a/src/jtag/drivers/xds110.c
+++ b/src/jtag/drivers/xds110.c
@@ -1342,17 +1342,21 @@ static void xds110_swd_queue_cmd(uint8_t cmd, uint32_t 
*value)
        }
 }
 
-static void xds110_swd_read_reg(uint8_t cmd, uint32_t *value,
+static int xds110_swd_read_reg(uint8_t cmd, uint32_t *value,
        uint32_t ap_delay_clk)
 {
        assert(cmd & SWD_CMD_RNW);
        xds110_swd_queue_cmd(cmd, value);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
-static void xds110_swd_write_reg(uint8_t cmd, uint32_t value,
+static int xds110_swd_write_reg(uint8_t cmd, uint32_t value,
        uint32_t ap_delay_clk)
 {
        assert(!(cmd & SWD_CMD_RNW));
        xds110_swd_queue_cmd(cmd, &value);
+
+       return ERROR_OK;        /* TODO: return error instead of queuing it */
 }
 
 /***************************************************************************
diff --git a/src/jtag/drivers/xlnx-pcie-xvc.c b/src/jtag/drivers/xlnx-pcie-xvc.c
index d90a022cda..d7aab77a3f 100644
--- a/src/jtag/drivers/xlnx-pcie-xvc.c
+++ b/src/jtag/drivers/xlnx-pcie-xvc.c
@@ -529,9 +529,7 @@ static int xlnx_pcie_xvc_swd_switch_seq(enum 
swd_special_seq seq)
        return ERROR_OK;
 }
 
-static int queued_retval;
-
-static void xlnx_pcie_xvc_swd_write_reg(uint8_t cmd, uint32_t value,
+static int xlnx_pcie_xvc_swd_write_reg(uint8_t cmd, uint32_t value,
                                        uint32_t ap_delay_clk);
 
 static void swd_clear_sticky_errors(void)
@@ -540,7 +538,7 @@ static void swd_clear_sticky_errors(void)
                STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
 }
 
-static void xlnx_pcie_xvc_swd_read_reg(uint8_t cmd, uint32_t *value,
+static int xlnx_pcie_xvc_swd_read_reg(uint8_t cmd, uint32_t *value,
                                       uint32_t ap_delay_clk)
 {
        uint32_t res, ack, rpar;
@@ -552,19 +550,19 @@ static void xlnx_pcie_xvc_swd_read_reg(uint8_t cmd, 
uint32_t *value,
        /* cmd + ack */
        err = xlnx_pcie_xvc_transact(12, cmd, 0, &res);
        if (err != ERROR_OK)
-               goto err_out;
+               return err;
 
        ack = MASK_ACK(res);
 
        /* read data */
        err = xlnx_pcie_xvc_transact(32, 0, 0, &res);
        if (err != ERROR_OK)
-               goto err_out;
+               return err;
 
        /* parity + trn */
        err = xlnx_pcie_xvc_transact(2, 0, 0, &rpar);
        if (err != ERROR_OK)
-               goto err_out;
+               return err;
 
        LOG_DEBUG("%s %s %s reg %X = %08"PRIx32,
                  ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ?
@@ -577,33 +575,27 @@ static void xlnx_pcie_xvc_swd_read_reg(uint8_t cmd, 
uint32_t *value,
        case SWD_ACK_OK:
                if (MASK_PAR(rpar) != parity_u32(res)) {
                        LOG_DEBUG_IO("Wrong parity detected");
-                       queued_retval = ERROR_FAIL;
-                       return;
+                       return ERROR_SWD_FAIL;
                }
                if (value)
                        *value = res;
                if (cmd & SWD_CMD_APNDP)
-                       err = xlnx_pcie_xvc_transact(ap_delay_clk, 0, 0, NULL);
-               queued_retval = err;
-               return;
+                       return xlnx_pcie_xvc_transact(ap_delay_clk, 0, 0, NULL);
+               return ERROR_OK;
        case SWD_ACK_WAIT:
                LOG_DEBUG_IO("SWD_ACK_WAIT");
                swd_clear_sticky_errors();
-               return;
+               return ERROR_WAIT;
        case SWD_ACK_FAULT:
                LOG_DEBUG_IO("SWD_ACK_FAULT");
-               queued_retval = ack;
-               return;
+               return ERROR_SWD_FAULT;
        default:
-               LOG_DEBUG_IO("No valid acknowledge: ack=%02"PRIx32, ack);
-               queued_retval = ack;
-               return;
+               LOG_DEBUG_IO("No valid acknowledge: ack=%02" PRIx32, ack);
+               return ERROR_SWD_FAIL;
        }
-err_out:
-       queued_retval = err;
 }
 
-static void xlnx_pcie_xvc_swd_write_reg(uint8_t cmd, uint32_t value,
+static int xlnx_pcie_xvc_swd_write_reg(uint8_t cmd, uint32_t value,
                                        uint32_t ap_delay_clk)
 {
        uint32_t res, ack;
@@ -615,19 +607,19 @@ static void xlnx_pcie_xvc_swd_write_reg(uint8_t cmd, 
uint32_t value,
        /* cmd + trn + ack */
        err = xlnx_pcie_xvc_transact(13, cmd, 0, &res);
        if (err != ERROR_OK)
-               goto err_out;
+               return err;
 
        ack = MASK_ACK(res);
 
        /* write data */
        err = xlnx_pcie_xvc_transact(32, value, 0, NULL);
        if (err != ERROR_OK)
-               goto err_out;
+               return err;
 
        /* parity + trn */
        err = xlnx_pcie_xvc_transact(2, parity_u32(value), 0, NULL);
        if (err != ERROR_OK)
-               goto err_out;
+               return err;
 
        LOG_DEBUG("%s %s %s reg %X = %08"PRIx32,
                  ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ?
@@ -640,25 +632,19 @@ static void xlnx_pcie_xvc_swd_write_reg(uint8_t cmd, 
uint32_t value,
        switch (ack) {
        case SWD_ACK_OK:
                if (cmd & SWD_CMD_APNDP)
-                       err = xlnx_pcie_xvc_transact(ap_delay_clk, 0, 0, NULL);
-               queued_retval = err;
-               return;
+                       return xlnx_pcie_xvc_transact(ap_delay_clk, 0, 0, NULL);
+               return ERROR_OK;
        case SWD_ACK_WAIT:
                LOG_DEBUG_IO("SWD_ACK_WAIT");
                swd_clear_sticky_errors();
-               return;
+               return ERROR_WAIT;
        case SWD_ACK_FAULT:
                LOG_DEBUG_IO("SWD_ACK_FAULT");
-               queued_retval = ack;
-               return;
+               return ERROR_SWD_FAULT;
        default:
-               LOG_DEBUG_IO("No valid acknowledge: ack=%02"PRIx32, ack);
-               queued_retval = ack;
-               return;
+               LOG_DEBUG_IO("No valid acknowledge: ack=%02" PRIx32, ack);
+               return ERROR_SWD_FAIL;
        }
-
-err_out:
-       queued_retval = err;
 }
 
 static int xlnx_pcie_xvc_swd_run_queue(void)
@@ -667,12 +653,6 @@ static int xlnx_pcie_xvc_swd_run_queue(void)
 
        /* we want at least 8 idle cycles between each transaction */
        err = xlnx_pcie_xvc_transact(8, 0, 0, NULL);
-       if (err != ERROR_OK)
-               return err;
-
-       err = queued_retval;
-       queued_retval = ERROR_OK;
-       LOG_DEBUG("SWD queue return value: %02x", err);
 
        return err;
 }
diff --git a/src/jtag/swd.h b/src/jtag/swd.h
index 3fe1365b58..12e4bc06de 100644
--- a/src/jtag/swd.h
+++ b/src/jtag/swd.h
@@ -270,8 +270,9 @@ struct swd_driver {
         * @param Where to store value to read from register
         * @param ap_delay_hint Number of idle cycles that may be
         * needed after an AP access to avoid WAITs
+        * @return ERROR_OK if the command was queued or executed or a fault 
code
         */
-       void (*read_reg)(uint8_t cmd, uint32_t *value, uint32_t ap_delay_hint);
+       int (*read_reg)(uint8_t cmd, uint32_t *value, uint32_t ap_delay_hint);
 
        /**
         * Queued write of an AP or DP register.
@@ -280,8 +281,9 @@ struct swd_driver {
         * @param Value to be written to the register
         * @param ap_delay_hint Number of idle cycles that may be
         * needed after an AP access to avoid WAITs
+        * @return ERROR_OK if the command was queued or executed or a fault 
code
         */
-       void (*write_reg)(uint8_t cmd, uint32_t value, uint32_t ap_delay_hint);
+       int (*write_reg)(uint8_t cmd, uint32_t value, uint32_t ap_delay_hint);
 
        /**
         * Execute any queued transactions and collect the result.
diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index dda1b0674a..16192f1f53 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -63,22 +63,27 @@ static int swd_send_sequence(struct adiv5_dap *dap, enum 
swd_special_seq seq)
        return swd->switch_seq(seq);
 }
 
-static void swd_finish_read(struct adiv5_dap *dap)
+static int swd_finish_read(struct adiv5_dap *dap)
 {
        const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
-       if (dap->last_read) {
-               swd->read_reg(swd_cmd(true, false, DP_RDBUFF), dap->last_read, 
0);
-               dap->last_read = NULL;
-       }
+       if (!dap->last_read)
+               return ERROR_OK;
+
+       int retval = swd->read_reg(swd_cmd(true, false, DP_RDBUFF),
+                                                          dap->last_read, 0);
+       dap->last_read = NULL;
+       return retval;
 }
 
-static void swd_clear_sticky_errors(struct adiv5_dap *dap)
+static int swd_clear_sticky_errors(struct adiv5_dap *dap)
 {
        const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
        assert(swd);
 
-       swd->write_reg(swd_cmd(false, false, DP_ABORT),
+       int retval = swd->write_reg(swd_cmd(false, false, DP_ABORT),
                STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
+
+       return retval;
 }
 
 static int swd_run_inner(struct adiv5_dap *dap)
@@ -129,7 +134,9 @@ static int swd_queue_dp_read_inner(struct adiv5_dap *dap, 
unsigned int reg,
        if (retval != ERROR_OK)
                return retval;
 
-       swd->read_reg(swd_cmd(true, false, reg), data, 0);
+       retval = swd->read_reg(swd_cmd(true, false, reg), data, 0);
+       if (retval != ERROR_OK)
+               return retval;
 
        return check_sync(dap);
 }
@@ -137,16 +144,20 @@ 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 = ERROR_OK;
        const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
        assert(swd);
 
-       swd_finish_read(dap);
+       int retval = swd_finish_read(dap);
+       if (retval != ERROR_OK)
+               return retval;
+
 
        if (reg == DP_SELECT) {
                dap->select = data | (dap->select & (0xffffffffull << 32));
 
-               swd->write_reg(swd_cmd(false, false, reg), data, 0);
+               retval = swd->write_reg(swd_cmd(false, false, reg), data, 0);
+               if (retval != ERROR_OK)
+                       return retval;
 
                retval = check_sync(dap);
                dap->select_valid = (retval == ERROR_OK);
@@ -164,7 +175,9 @@ static int swd_queue_dp_write_inner(struct adiv5_dap *dap, 
unsigned int reg,
                retval = swd_queue_dp_bankselect(dap, reg);
 
        if (retval == ERROR_OK) {
-               swd->write_reg(swd_cmd(false, false, reg), data, 0);
+               retval = swd->write_reg(swd_cmd(false, false, reg), data, 0);
+               if (retval != ERROR_OK)
+                       return retval;
 
                retval = check_sync(dap);
        }
@@ -215,13 +228,13 @@ static int swd_multidrop_select_inner(struct adiv5_dap 
*dap, uint32_t *dpidr_ptr
 
        if (clear_sticky) {
                /* Clear all sticky errors (including ORUN) */
-               swd_clear_sticky_errors(dap);
+               retval = swd_clear_sticky_errors(dap);
        } else {
                /* Ideally just clear ORUN flag which is set by reset */
                retval = swd_queue_dp_write_inner(dap, DP_ABORT, ORUNERRCLR);
-               if (retval != ERROR_OK)
-                       return retval;
        }
+       if (retval != ERROR_OK)
+               return retval;
 
        retval = swd_queue_dp_read_inner(dap, DP_DLPIDR, &dlpidr);
        if (retval != ERROR_OK)
@@ -388,9 +401,11 @@ static int swd_connect_single(struct adiv5_dap *dap)
                dap->do_reconnect = false;
 
                /* force clear all sticky faults */
-               swd_clear_sticky_errors(dap);
+               retval = swd_clear_sticky_errors(dap);
+
+               if (retval == ERROR_OK)
+                       retval = swd_run_inner(dap);
 
-               retval = swd_run_inner(dap);
                if (retval != ERROR_WAIT)
                        break;
 
@@ -459,6 +474,16 @@ static int swd_connect(struct adiv5_dap *dap)
        return status;
 }
 
+static void swd_error_recovery(struct adiv5_dap *dap, int err)
+{
+       /* TODO: do not reconnect on all errors, just clear sticky
+        * after SWD FAULT */
+       if (err != ERROR_OK) {
+               dap->do_reconnect = true;
+               LOG_DEBUG_IO("Reconnect on next SWD op");
+       }
+}
+
 static int swd_check_reconnect(struct adiv5_dap *dap)
 {
        if (dap->do_reconnect)
@@ -480,8 +505,11 @@ static int swd_queue_ap_abort(struct adiv5_dap *dap, 
uint8_t *ack)
        if (retval != ERROR_OK)
                return retval;
 
-       swd->write_reg(swd_cmd(false, false, DP_ABORT),
+       retval = swd->write_reg(swd_cmd(false, false, DP_ABORT),
                DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0);
+       if (retval != ERROR_OK)
+               return retval;
+
        return check_sync(dap);
 }
 
@@ -496,6 +524,7 @@ static int swd_queue_dp_read(struct adiv5_dap *dap, 
unsigned int reg,
        if (retval != ERROR_OK)
                return retval;
 
+       swd_error_recovery(dap, retval);
        return swd_queue_dp_read_inner(dap, reg, data);
 }
 
@@ -513,7 +542,9 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, 
unsigned int reg,
        if (retval != ERROR_OK)
                return retval;
 
-       return swd_queue_dp_write_inner(dap, reg, data);
+       retval = swd_queue_dp_write_inner(dap, reg, data);
+       swd_error_recovery(dap, retval);
+       return retval;
 }
 
 /** Select the AP register bank */
@@ -552,16 +583,20 @@ static int swd_queue_ap_bankselect(struct adiv5_ap *ap, 
unsigned int reg)
                LOG_DEBUG_IO("AP BANK SELECT: %" PRIx32, (uint32_t)sel);
 
                retval = swd_queue_dp_write(dap, DP_SELECT, (uint32_t)sel);
-               if (retval != ERROR_OK)
+               if (retval != ERROR_OK) {
+                       swd_error_recovery(dap, retval);
                        return retval;
+               }
        }
 
        if (set_select1) {
                LOG_DEBUG_IO("AP BANK SELECT1: %" PRIx32, (uint32_t)(sel >> 
32));
 
                retval = swd_queue_dp_write(dap, DP_SELECT1, (uint32_t)(sel >> 
32));
-               if (retval != ERROR_OK)
+               if (retval != ERROR_OK) {
+                       swd_error_recovery(dap, retval);
                        return retval;
+               }
        }
 
        return ERROR_OK;
@@ -586,7 +621,13 @@ static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned 
int reg,
        if (retval != ERROR_OK)
                return retval;
 
-       swd->read_reg(swd_cmd(true, true, reg), dap->last_read, 
ap->memaccess_tck);
+       retval = swd->read_reg(swd_cmd(true, true, reg),
+                                                  dap->last_read, 
ap->memaccess_tck);
+       if (retval != ERROR_OK) {
+               swd_error_recovery(dap, retval);
+               return retval;
+       }
+
        dap->last_read = data;
 
        return check_sync(dap);
@@ -607,13 +648,21 @@ static int swd_queue_ap_write(struct adiv5_ap *ap, 
unsigned int reg,
        if (retval != ERROR_OK)
                return retval;
 
-       swd_finish_read(dap);
+       retval = swd_finish_read(dap);
+       if (retval != ERROR_OK) {
+               swd_error_recovery(dap, retval);
+               return retval;
+       }
 
        retval = swd_queue_ap_bankselect(ap, reg);
        if (retval != ERROR_OK)
                return retval;
 
-       swd->write_reg(swd_cmd(false, true, reg), data, ap->memaccess_tck);
+       retval = swd->write_reg(swd_cmd(false, true, reg), data, 
ap->memaccess_tck);
+       if (retval != ERROR_OK) {
+               swd_error_recovery(dap, retval);
+               return retval;
+       }
 
        return check_sync(dap);
 }
@@ -625,14 +674,15 @@ static int swd_run(struct adiv5_dap *dap)
        if (retval != ERROR_OK)
                return retval;
 
-       swd_finish_read(dap);
-
-       retval = swd_run_inner(dap);
+       retval = swd_finish_read(dap);
        if (retval != ERROR_OK) {
-               /* fault response */
-               dap->do_reconnect = true;
+               swd_error_recovery(dap, retval);
+               return retval;
        }
 
+       retval = swd_run_inner(dap);
+       swd_error_recovery(dap, retval);
+
        return retval;
 }
 

-- 

Reply via email to