openocd: src/target: replace the GPL-2.0-or-later license tag
[fw/openocd] / src / target / adi_v5_jtag.c
index 239253d2016dfa00fa8c86614ee2a70348a5fdda..a70fad36896fd6352ded0bac120ac64732b8e6ed 100644 (file)
@@ -1,3 +1,5 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
 /***************************************************************************
  *   Copyright (C) 2006 by Magnus Lundin
  *   lundin@mlu.mine.nu
  *
  *   Copyright (C) 2009-2010 by David Brownell
  *
- *   This program is free software; you can redistribute it and/or modify
- *   it under the terms of the GNU General Public License as published by
- *   the Free Software Foundation; either version 2 of the License, or
- *   (at your option) any later version.
- *
- *   This program is distributed in the hope that it will be useful,
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *   GNU General Public License for more details.
- *
- *   You should have received a copy of the GNU General Public License
- *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *   Copyright (C) 2020-2021, Ampere Computing LLC                              *
  ***************************************************************************/
 
 /**
  * @file
  * This file implements JTAG transport support for cores implementing
- the ARM Debug Interface version 5 (ADIv5).
+ the ARM Debug Interface version 5 (ADIv5) and version 6 (ADIv6).
  */
 
 #ifdef HAVE_CONFIG_H
 /*#define DEBUG_WAIT*/
 
 /* JTAG instructions/registers for JTAG-DP and SWJ-DP */
-#define JTAG_DP_ABORT          0x8
-#define JTAG_DP_DPACC          0xA
-#define JTAG_DP_APACC          0xB
-#define JTAG_DP_IDCODE         0xE
+#define JTAG_DP_ABORT          0xF8
+#define JTAG_DP_DPACC          0xFA
+#define JTAG_DP_APACC          0xFB
+#define JTAG_DP_IDCODE         0xFE
 
 /* three-bit ACK values for DPACC and APACC reads */
-#define JTAG_ACK_OK_FAULT      0x2
-#define JTAG_ACK_WAIT          0x1
+#define JTAG_ACK_WAIT       0x1     /* ADIv5 and ADIv6 */
+#define JTAG_ACK_OK_FAULT   0x2     /* ADIv5 */
+#define JTAG_ACK_FAULT      0x2     /* ADIv6 */
+#define JTAG_ACK_OK         0x4     /* ADIV6 */
 
 static int jtag_ap_q_abort(struct adiv5_dap *dap, uint8_t *ack);
 
 #ifdef DEBUG_WAIT
-static const char *dap_reg_name(int instr, int reg_addr)
+static const char *dap_reg_name(struct adiv5_dap *dap, uint8_t instr, uint16_t reg_addr)
 {
        char *reg_name = "UNK";
 
@@ -83,41 +76,32 @@ static const char *dap_reg_name(int instr, int reg_addr)
        }
 
        if (instr == JTAG_DP_APACC) {
-               switch (reg_addr) {
-               case MEM_AP_REG_CSW:
+               if (reg_addr == MEM_AP_REG_CSW(dap))
                        reg_name = "CSW";
-                       break;
-               case MEM_AP_REG_TAR:
+               else if (reg_addr == MEM_AP_REG_TAR(dap))
                        reg_name = "TAR";
-                       break;
-               case MEM_AP_REG_DRW:
+               else if (reg_addr == MEM_AP_REG_TAR64(dap))
+                       reg_name = "TAR64";
+               else if (reg_addr == MEM_AP_REG_DRW(dap))
                        reg_name = "DRW";
-                       break;
-               case MEM_AP_REG_BD0:
+               else if (reg_addr == MEM_AP_REG_BD0(dap))
                        reg_name = "BD0";
-                       break;
-               case MEM_AP_REG_BD1:
+               else if (reg_addr == MEM_AP_REG_BD1(dap))
                        reg_name = "BD1";
-                       break;
-               case MEM_AP_REG_BD2:
+               else if (reg_addr == MEM_AP_REG_BD2(dap))
                        reg_name = "BD2";
-                       break;
-               case MEM_AP_REG_BD3:
+               else if (reg_addr == MEM_AP_REG_BD3(dap))
                        reg_name = "BD3";
-                       break;
-               case MEM_AP_REG_CFG:
+               else if (reg_addr == MEM_AP_REG_CFG(dap))
                        reg_name = "CFG";
-                       break;
-               case MEM_AP_REG_BASE:
+               else if (reg_addr == MEM_AP_REG_BASE(dap))
                        reg_name = "BASE";
-                       break;
-               case AP_REG_IDR:
+               else if (reg_addr == MEM_AP_REG_BASE64(dap))
+                       reg_name = "BASE64";
+               else if (reg_addr == AP_REG_IDR(dap))
                        reg_name = "IDR";
-                       break;
-               default:
+               else
                        reg_name = "UNK";
-                       break;
-               }
        }
 
        return reg_name;
@@ -127,12 +111,12 @@ static const char *dap_reg_name(int instr, int reg_addr)
 struct dap_cmd {
        struct list_head lh;
        uint8_t instr;
-       uint8_t reg_addr;
-       uint8_t RnW;
+       uint16_t reg_addr;
+       uint8_t rnw;
        uint8_t *invalue;
        uint8_t ack;
        uint32_t memaccess_tck;
-       uint32_t dp_select;
+       uint64_t dp_select;
 
        struct scan_field fields[2];
        uint8_t out_addr_buf;
@@ -147,17 +131,38 @@ struct dap_cmd_pool {
        struct dap_cmd cmd;
 };
 
-static void log_dap_cmd(const char *header, struct dap_cmd *el)
+static void log_dap_cmd(struct adiv5_dap *dap, const char *header, struct dap_cmd *el)
 {
 #ifdef DEBUG_WAIT
+       const char *ack;
+       switch (el->ack) {
+       case JTAG_ACK_WAIT:         /* ADIv5 and ADIv6 */
+               ack = "WAIT";
+               break;
+       case JTAG_ACK_OK_FAULT:     /* ADIv5, same value as JTAG_ACK_FAULT */
+       /* case JTAG_ACK_FAULT: */  /* ADIv6 */
+               if (is_adiv6(dap))
+                       ack = "FAULT";
+               else
+                       ack = "OK";
+               break;
+       case JTAG_ACK_OK:           /* ADIv6 */
+               if (is_adiv6(dap)) {
+                       ack = "OK";
+                       break;
+               }
+               /* fall-through */
+       default:
+               ack = "INVAL";
+               break;
+       }
        LOG_DEBUG("%s: %2s %6s %5s 0x%08x 0x%08x %2s", header,
                el->instr == JTAG_DP_APACC ? "AP" : "DP",
-               dap_reg_name(el->instr, el->reg_addr),
-               el->RnW == DPAP_READ ? "READ" : "WRITE",
+               dap_reg_name(dap, el->instr, el->reg_addr),
+               el->rnw == DPAP_READ ? "READ" : "WRITE",
                buf_get_u32(el->outvalue_buf, 0, 32),
                buf_get_u32(el->invalue, 0, 32),
-               el->ack == JTAG_ACK_OK_FAULT ? "OK" :
-                       (el->ack == JTAG_ACK_WAIT ? "WAIT" : "INVAL"));
+               ack);
 #endif
 }
 
@@ -170,7 +175,7 @@ static int jtag_limit_queue_size(struct adiv5_dap *dap)
 }
 
 static struct dap_cmd *dap_cmd_new(struct adiv5_dap *dap, uint8_t instr,
-               uint8_t reg_addr, uint8_t RnW,
+               uint16_t reg_addr, uint8_t rnw,
                uint8_t *outvalue, uint8_t *invalue,
                uint32_t memaccess_tck)
 {
@@ -179,7 +184,7 @@ static struct dap_cmd *dap_cmd_new(struct adiv5_dap *dap, uint8_t instr,
 
        if (list_empty(&dap->cmd_pool)) {
                pool = calloc(1, sizeof(struct dap_cmd_pool));
-               if (pool == NULL)
+               if (!pool)
                        return NULL;
        } else {
                pool = list_first_entry(&dap->cmd_pool, struct dap_cmd_pool, lh);
@@ -193,10 +198,10 @@ static struct dap_cmd *dap_cmd_new(struct adiv5_dap *dap, uint8_t instr,
        INIT_LIST_HEAD(&cmd->lh);
        cmd->instr = instr;
        cmd->reg_addr = reg_addr;
-       cmd->RnW = RnW;
-       if (outvalue != NULL)
+       cmd->rnw = rnw;
+       if (outvalue)
                memcpy(cmd->outvalue_buf, outvalue, 4);
-       cmd->invalue = (invalue != NULL) ? invalue : cmd->invalue_buf;
+       cmd->invalue = (invalue) ? invalue : cmd->invalue_buf;
        cmd->memaccess_tck = memaccess_tck;
 
        return cmd;
@@ -253,9 +258,9 @@ static int adi_jtag_dp_scan_cmd(struct adiv5_dap *dap, struct dap_cmd *cmd, uint
         * For APACC access with any sticky error flag set, this is discarded.
         */
        cmd->fields[0].num_bits = 3;
-       buf_set_u32(&cmd->out_addr_buf, 0, 3, ((cmd->reg_addr >> 1) & 0x6) | (cmd->RnW & 0x1));
+       buf_set_u32(&cmd->out_addr_buf, 0, 3, ((cmd->reg_addr >> 1) & 0x6) | (cmd->rnw & 0x1));
        cmd->fields[0].out_value = &cmd->out_addr_buf;
-       cmd->fields[0].in_value = (ack != NULL) ? ack : &cmd->ack;
+       cmd->fields[0].in_value = (ack) ? ack : &cmd->ack;
 
        /* NOTE: if we receive JTAG_ACK_WAIT, the previous operation did not
         * complete; data we write is discarded, data we read is unpredictable.
@@ -268,17 +273,14 @@ static int adi_jtag_dp_scan_cmd(struct adiv5_dap *dap, struct dap_cmd *cmd, uint
 
        jtag_add_dr_scan(tap, 2, cmd->fields, TAP_IDLE);
 
-       /* Add specified number of tck clocks after starting memory bus
-        * access, giving the hardware time to complete the access.
+       /* Add specified number of tck clocks after starting AP register
+        * access or memory bus access, giving the hardware time to complete
+        * the access.
         * They provide more time for the (MEM) AP to complete the read ...
-        * See "Minimum Response Time" for JTAG-DP, in the ADIv5 spec.
+        * See "Minimum Response Time" for JTAG-DP, in the ADIv5/ADIv6 spec.
         */
-       if (cmd->instr == JTAG_DP_APACC) {
-               if (((cmd->reg_addr == MEM_AP_REG_DRW)
-                       || ((cmd->reg_addr & 0xF0) == MEM_AP_REG_BD0))
-                       && (cmd->memaccess_tck != 0))
-                       jtag_add_runtest(cmd->memaccess_tck, TAP_IDLE);
-       }
+       if (cmd->instr == JTAG_DP_APACC && cmd->memaccess_tck != 0)
+               jtag_add_runtest(cmd->memaccess_tck, TAP_IDLE);
 
        return ERROR_OK;
 }
@@ -296,10 +298,10 @@ static int adi_jtag_dp_scan_cmd_sync(struct adiv5_dap *dap, struct dap_cmd *cmd,
 
 /**
  * Scan DPACC or APACC using target ordered uint8_t buffers.  No endianness
- * conversions are performed.  See section 4.4.3 of the ADIv5 spec, which
+ * conversions are performed.  See section 4.4.3 of the ADIv5/ADIv6 spec, which
  * discusses operations which access these registers.
  *
- * Note that only one scan is performed.  If RnW is set, a separate scan
+ * Note that only one scan is performed.  If rnw is set, a separate scan
  * will be needed to collect the data which was read; the "invalue" collects
  * the posted result of a preceding operation, not the current one.
  *
@@ -307,7 +309,7 @@ static int adi_jtag_dp_scan_cmd_sync(struct adiv5_dap *dap, struct dap_cmd *cmd,
  * @param instr JTAG_DP_APACC (AP access) or JTAG_DP_DPACC (DP access)
  * @param reg_addr two significant bits; A[3:2]; for APACC access, the
  *     SELECT register has more addressing bits.
- * @param RnW false iff outvalue will be written to the DP or AP
+ * @param rnw false iff outvalue will be written to the DP or AP
  * @param outvalue points to a 32-bit (little-endian) integer
  * @param invalue NULL, or points to a 32-bit (little-endian) integer
  * @param ack points to where the three bit JTAG_ACK_* code will be stored
@@ -315,15 +317,15 @@ static int adi_jtag_dp_scan_cmd_sync(struct adiv5_dap *dap, struct dap_cmd *cmd,
  */
 
 static int adi_jtag_dp_scan(struct adiv5_dap *dap,
-               uint8_t instr, uint8_t reg_addr, uint8_t RnW,
+               uint8_t instr, uint16_t reg_addr, uint8_t rnw,
                uint8_t *outvalue, uint8_t *invalue,
                uint32_t memaccess_tck, uint8_t *ack)
 {
        struct dap_cmd *cmd;
        int retval;
 
-       cmd = dap_cmd_new(dap, instr, reg_addr, RnW, outvalue, invalue, memaccess_tck);
-       if (cmd != NULL)
+       cmd = dap_cmd_new(dap, instr, reg_addr, rnw, outvalue, invalue, memaccess_tck);
+       if (cmd)
                cmd->dp_select = dap->select;
        else
                return ERROR_JTAG_DEVICE_ERROR;
@@ -342,16 +344,30 @@ static int adi_jtag_dp_scan(struct adiv5_dap *dap,
  * must be different).
  */
 static int adi_jtag_dp_scan_u32(struct adiv5_dap *dap,
-               uint8_t instr, uint8_t reg_addr, uint8_t RnW,
+               uint8_t instr, uint16_t reg_addr, uint8_t rnw,
                uint32_t outvalue, uint32_t *invalue,
                uint32_t memaccess_tck, uint8_t *ack)
 {
        uint8_t out_value_buf[4];
        int retval;
-
+       uint64_t sel = (reg_addr >> 4) & 0xf;
+
+       /* No need to change SELECT or RDBUFF as they are not banked */
+       if (instr == JTAG_DP_DPACC && reg_addr != DP_SELECT && reg_addr != DP_RDBUFF &&
+                       sel != (dap->select & 0xf)) {
+               if (dap->select != DP_SELECT_INVALID)
+                       sel |= dap->select & ~0xfull;
+               dap->select = sel;
+               LOG_DEBUG("DP BANKSEL: %x", (uint32_t)sel);
+               buf_set_u32(out_value_buf, 0, 32, (uint32_t)sel);
+               retval = adi_jtag_dp_scan(dap, JTAG_DP_DPACC,
+                               DP_SELECT, DPAP_WRITE, out_value_buf, NULL, 0, NULL);
+               if (retval != ERROR_OK)
+                       return retval;
+       }
        buf_set_u32(out_value_buf, 0, 32, outvalue);
 
-       retval = adi_jtag_dp_scan(dap, instr, reg_addr, RnW,
+       retval = adi_jtag_dp_scan(dap, instr, reg_addr, rnw,
                        out_value_buf, (uint8_t *)invalue, memaccess_tck, ack);
        if (retval != ERROR_OK)
                return retval;
@@ -367,7 +383,7 @@ static int adi_jtag_finish_read(struct adiv5_dap *dap)
 {
        int retval = ERROR_OK;
 
-       if (dap->last_read != NULL) {
+       if (dap->last_read) {
                retval = adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
                                DP_RDBUFF, DPAP_READ, 0, dap->last_read, 0, NULL);
                dap->last_read = NULL;
@@ -377,21 +393,21 @@ static int adi_jtag_finish_read(struct adiv5_dap *dap)
 }
 
 static int adi_jtag_scan_inout_check_u32(struct adiv5_dap *dap,
-               uint8_t instr, uint8_t reg_addr, uint8_t RnW,
+               uint8_t instr, uint16_t reg_addr, uint8_t rnw,
                uint32_t outvalue, uint32_t *invalue, uint32_t memaccess_tck)
 {
        int retval;
 
        /* Issue the read or write */
        retval = adi_jtag_dp_scan_u32(dap, instr, reg_addr,
-                       RnW, outvalue, NULL, memaccess_tck, NULL);
+                       rnw, outvalue, NULL, memaccess_tck, NULL);
        if (retval != ERROR_OK)
                return retval;
 
        /* For reads,  collect posted value; RDBUFF has no other effect.
         * Assumes read gets acked with OK/FAULT, and CTRL_STAT says "OK".
         */
-       if ((RnW == DPAP_READ) && (invalue != NULL)) {
+       if ((rnw == DPAP_READ) && (invalue)) {
                retval = adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
                                DP_RDBUFF, DPAP_READ, 0, invalue, 0, NULL);
                if (retval != ERROR_OK)
@@ -416,14 +432,19 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 
        /* skip all completed transactions up to the first WAIT */
        list_for_each_entry(el, &dap->cmd_journal, lh) {
-               if (el->ack == JTAG_ACK_OK_FAULT) {
-                       log_dap_cmd("LOG", el);
+               /*
+                * JTAG_ACK_OK_FAULT (ADIv5) and JTAG_ACK_FAULT (ADIv6) are equal so
+                * the following statement is checking to see if an acknowledgment of
+                * OK or FAULT is generated for ADIv5 or ADIv6
+                */
+               if (el->ack == JTAG_ACK_OK_FAULT || (is_adiv6(dap) && el->ack == JTAG_ACK_OK)) {
+                       log_dap_cmd(dap, "LOG", el);
                } else if (el->ack == JTAG_ACK_WAIT) {
                        found_wait = 1;
                        break;
                } else {
                        LOG_ERROR("Invalid ACK (%1x) in DAP response", el->ack);
-                       log_dap_cmd("ERR", el);
+                       log_dap_cmd(dap, "ERR", el);
                        retval = ERROR_JTAG_DEVICE_ERROR;
                        goto done;
                }
@@ -435,15 +456,16 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
         */
        if (found_wait && el != list_first_entry(&dap->cmd_journal, struct dap_cmd, lh)) {
                prev = list_entry(el->lh.prev, struct dap_cmd, lh);
-               if (prev->RnW == DPAP_READ) {
-                       log_dap_cmd("PND", prev);
+               if (prev->rnw == DPAP_READ) {
+                       log_dap_cmd(dap, "PND", prev);
                        /* search for the next OK transaction, it contains
                         * the result of the previous READ */
                        tmp = el;
                        list_for_each_entry_from(tmp, &dap->cmd_journal, lh) {
-                               if (tmp->ack == JTAG_ACK_OK_FAULT) {
+                               /* The following check covers OK and FAULT ACKs for both ADIv5 and ADIv6 */
+                               if (tmp->ack == JTAG_ACK_OK_FAULT || (is_adiv6(dap) && tmp->ack == JTAG_ACK_OK)) {
                                        /* recover the read value */
-                                       log_dap_cmd("FND", tmp);
+                                       log_dap_cmd(dap, "FND", tmp);
                                        if (el->invalue != el->invalue_buf) {
                                                uint32_t invalue = le_to_h_u32(tmp->invalue);
                                                memcpy(el->invalue, &invalue, sizeof(uint32_t));
@@ -453,8 +475,8 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
                                }
                        }
 
-                       if (prev != NULL) {
-                               log_dap_cmd("LST", el);
+                       if (prev) {
+                               log_dap_cmd(dap, "LST", el);
 
                                /*
                                * At this point we're sure that no previous
@@ -466,7 +488,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
                                */
                                tmp = dap_cmd_new(dap, JTAG_DP_DPACC,
                                                DP_RDBUFF, DPAP_READ, NULL, NULL, 0);
-                               if (tmp == NULL) {
+                               if (!tmp) {
                                        retval = ERROR_JTAG_DEVICE_ERROR;
                                        goto done;
                                }
@@ -476,8 +498,9 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
                                        retval = adi_jtag_dp_scan_cmd_sync(dap, tmp, NULL);
                                        if (retval != ERROR_OK)
                                                break;
-                                       if (tmp->ack == JTAG_ACK_OK_FAULT) {
-                                               log_dap_cmd("FND", tmp);
+                                       /* The following check covers OK and FAULT ACKs for both ADIv5 and ADIv6 */
+                                       if (tmp->ack == JTAG_ACK_OK_FAULT || (is_adiv6(dap) && tmp->ack == JTAG_ACK_OK)) {
+                                               log_dap_cmd(dap, "FND", tmp);
                                                if (el->invalue != el->invalue_buf) {
                                                        uint32_t invalue = le_to_h_u32(tmp->invalue);
                                                        memcpy(el->invalue, &invalue, sizeof(uint32_t));
@@ -486,7 +509,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
                                        }
                                        if (tmp->ack != JTAG_ACK_WAIT) {
                                                LOG_ERROR("Invalid ACK (%1x) in DAP response", tmp->ack);
-                                               log_dap_cmd("ERR", tmp);
+                                               log_dap_cmd(dap, "ERR", tmp);
                                                retval = ERROR_JTAG_DEVICE_ERROR;
                                                break;
                                        }
@@ -495,7 +518,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 
                                if (retval == ERROR_OK) {
                                        /* timeout happened */
-                                       if (tmp->ack != JTAG_ACK_OK_FAULT) {
+                                       if (tmp->ack == JTAG_ACK_WAIT) {
                                                LOG_ERROR("Timeout during WAIT recovery");
                                                dap->select = DP_SELECT_INVALID;
                                                jtag_ap_q_abort(dap, NULL);
@@ -523,7 +546,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 
        /* move all remaining transactions over to the replay list */
        list_for_each_entry_safe_from(el, tmp, &dap->cmd_journal, lh) {
-               log_dap_cmd("REP", el);
+               log_dap_cmd(dap, "REP", el);
                list_move_tail(&el->lh, &replay_list);
        }
 
@@ -532,7 +555,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 
        /* check for overrun condition in the last batch of transactions */
        if (found_wait) {
-               LOG_INFO("DAP transaction stalled (WAIT) - slowing down");
+               LOG_INFO("DAP transaction stalled (WAIT) - slowing down and resending");
                /* clear the sticky overrun condition */
                retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
                                DP_CTRL_STAT, DPAP_WRITE,
@@ -545,7 +568,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
                        el = list_first_entry(&replay_list, struct dap_cmd, lh);
                        tmp = dap_cmd_new(dap, JTAG_DP_DPACC,
                                          DP_SELECT, DPAP_WRITE, (uint8_t *)&el->dp_select, NULL, 0);
-                       if (tmp == NULL) {
+                       if (!tmp) {
                                retval = ERROR_JTAG_DEVICE_ERROR;
                                goto done;
                        }
@@ -560,8 +583,8 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
                                retval = adi_jtag_dp_scan_cmd_sync(dap, el, NULL);
                                if (retval != ERROR_OK)
                                        break;
-                               log_dap_cmd("REC", el);
-                               if (el->ack == JTAG_ACK_OK_FAULT) {
+                               log_dap_cmd(dap, "REC", el);
+                               if (el->ack == JTAG_ACK_OK_FAULT || (is_adiv6(dap) && el->ack == JTAG_ACK_OK)) {
                                        if (el->invalue != el->invalue_buf) {
                                                uint32_t invalue = le_to_h_u32(el->invalue);
                                                memcpy(el->invalue, &invalue, sizeof(uint32_t));
@@ -570,11 +593,11 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
                                }
                                if (el->ack != JTAG_ACK_WAIT) {
                                        LOG_ERROR("Invalid ACK (%1x) in DAP response", el->ack);
-                                       log_dap_cmd("ERR", el);
+                                       log_dap_cmd(dap, "ERR", el);
                                        retval = ERROR_JTAG_DEVICE_ERROR;
                                        break;
                                }
-                               LOG_INFO("DAP transaction stalled during replay (WAIT) - resending");
+                               LOG_DEBUG("DAP transaction stalled during replay (WAIT) - resending");
                                /* clear the sticky overrun condition */
                                retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
                                                DP_CTRL_STAT, DPAP_WRITE,
@@ -584,7 +607,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
                        } while (timeval_ms() - time_now < 1000);
 
                        if (retval == ERROR_OK) {
-                               if (el->ack != JTAG_ACK_OK_FAULT) {
+                               if (el->ack == JTAG_ACK_WAIT) {
                                        LOG_ERROR("Timeout during WAIT recovery");
                                        dap->select = DP_SELECT_INVALID;
                                        jtag_ap_q_abort(dap, NULL);
@@ -640,10 +663,10 @@ static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
                if (ctrlstat & SSTICKYORUN)
                        LOG_DEBUG("JTAG-DP STICKY OVERRUN");
 
-               /* Clear Sticky Error Bits */
+               /* Clear Sticky Error and Sticky Overrun Bits */
                retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
                                DP_CTRL_STAT, DPAP_WRITE,
-                               dap->dp_ctrl_stat | SSTICKYERR, NULL, 0);
+                               dap->dp_ctrl_stat | SSTICKYERR | SSTICKYORUN, NULL, 0);
                if (retval != ERROR_OK)
                        goto done;
 
@@ -722,15 +745,38 @@ static int jtag_dp_q_write(struct adiv5_dap *dap, unsigned reg,
 /** Select the AP register bank matching bits 7:4 of reg. */
 static int jtag_ap_q_bankselect(struct adiv5_ap *ap, unsigned reg)
 {
+       int retval;
        struct adiv5_dap *dap = ap->dap;
-       uint32_t sel = ((uint32_t)ap->ap_num << 24) | (reg & 0x000000F0);
+       uint64_t sel;
+
+       if (is_adiv6(dap)) {
+               sel = ap->ap_num | (reg & 0x00000FF0);
+               if (sel == (dap->select & ~0xfull))
+                       return ERROR_OK;
+
+               if (dap->select != DP_SELECT_INVALID)
+                       sel |= dap->select & 0xf;
+               dap->select = sel;
+               LOG_DEBUG("AP BANKSEL: %" PRIx64, sel);
+
+               retval = jtag_dp_q_write(dap, DP_SELECT, (uint32_t)sel);
+               if (retval != ERROR_OK)
+                       return retval;
+
+               if (dap->asize > 32)
+                       return jtag_dp_q_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
+               return ERROR_OK;
+       }
+
+       /* ADIv5 */
+       sel = (ap->ap_num << 24) | (reg & 0x000000F0);
 
        if (sel == dap->select)
                return ERROR_OK;
 
        dap->select = sel;
 
-       return jtag_dp_q_write(dap, DP_SELECT, sel);
+       return jtag_dp_q_write(dap, DP_SELECT, (uint32_t)sel);
 }
 
 static int jtag_ap_q_read(struct adiv5_ap *ap, unsigned reg,