X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_jtag.c;h=c2100eb47195df2f73812726f8c6cabda415fad5;hp=272d308fd2780f17615d0589d5e79c2a882a7da4;hb=HEAD;hpb=d0e763ac7ef6aa17b17bd00ccdfbccfb4eacda69

diff --git a/src/target/adi_v5_jtag.c b/src/target/adi_v5_jtag.c
index 272d308fd2..8d54a50fb0 100644
--- a/src/target/adi_v5_jtag.c
+++ b/src/target/adi_v5_jtag.c
@@ -1,3 +1,5 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
 /***************************************************************************
  *   Copyright (C) 2006 by Magnus Lundin
  *   lundin@mlu.mine.nu
@@ -10,24 +12,13 @@
  *
  *   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
@@ -38,23 +29,26 @@
 #include "arm_adi_v5.h"
 #include <helper/time_support.h>
 #include <helper/list.h>
+#include <jtag/swd.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";
 
@@ -72,8 +66,8 @@ static const char *dap_reg_name(int instr, int reg_addr)
 		case DP_RDBUFF:
 			reg_name =  "RDBUFF";
 			break;
-		case DP_WCR:
-			reg_name =  "WCR";
+		case DP_DLCR:
+			reg_name =  "DLCR";
 			break;
 		default:
 			reg_name = "UNK";
@@ -82,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;
@@ -126,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;
@@ -139,46 +124,115 @@ struct dap_cmd {
 	uint8_t outvalue_buf[4];
 };
 
-static void log_dap_cmd(const char *header, struct dap_cmd *el)
+#define MAX_DAP_COMMAND_NUM 65536
+
+struct dap_cmd_pool {
+	struct list_head lh;
+	struct dap_cmd cmd;
+};
+
+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
 }
 
-static struct dap_cmd *dap_cmd_new(uint8_t instr,
-		uint8_t reg_addr, uint8_t RnW,
+static int jtag_limit_queue_size(struct adiv5_dap *dap)
+{
+	if (dap->cmd_pool_size < MAX_DAP_COMMAND_NUM)
+		return ERROR_OK;
+
+	return dap_run(dap);
+}
+
+static struct dap_cmd *dap_cmd_new(struct adiv5_dap *dap, uint8_t instr,
+		uint16_t reg_addr, uint8_t rnw,
 		uint8_t *outvalue, uint8_t *invalue,
 		uint32_t memaccess_tck)
 {
-	struct dap_cmd *cmd;
 
-	cmd = (struct dap_cmd *)calloc(1, sizeof(struct dap_cmd));
-	if (cmd != NULL) {
-		INIT_LIST_HEAD(&cmd->lh);
-		cmd->instr = instr;
-		cmd->reg_addr = reg_addr;
-		cmd->RnW = RnW;
-		if (outvalue != NULL)
-			memcpy(cmd->outvalue_buf, outvalue, 4);
-		cmd->invalue = (invalue != NULL) ? invalue : cmd->invalue_buf;
-		cmd->memaccess_tck = memaccess_tck;
+	struct dap_cmd_pool *pool = NULL;
+
+	if (list_empty(&dap->cmd_pool)) {
+		pool = calloc(1, sizeof(struct dap_cmd_pool));
+		if (!pool)
+			return NULL;
+	} else {
+		pool = list_first_entry(&dap->cmd_pool, struct dap_cmd_pool, lh);
+		list_del(&pool->lh);
 	}
 
+	INIT_LIST_HEAD(&pool->lh);
+	dap->cmd_pool_size++;
+
+	struct dap_cmd *cmd = &pool->cmd;
+	INIT_LIST_HEAD(&cmd->lh);
+	cmd->instr = instr;
+	cmd->reg_addr = reg_addr;
+	cmd->rnw = rnw;
+	if (outvalue)
+		memcpy(cmd->outvalue_buf, outvalue, 4);
+	cmd->invalue = (invalue) ? invalue : cmd->invalue_buf;
+	cmd->memaccess_tck = memaccess_tck;
+
 	return cmd;
 }
 
-static void flush_journal(struct list_head *lh)
+static void dap_cmd_release(struct adiv5_dap *dap, struct dap_cmd *cmd)
+{
+	struct dap_cmd_pool *pool = container_of(cmd, struct dap_cmd_pool, cmd);
+	if (dap->cmd_pool_size > MAX_DAP_COMMAND_NUM)
+		free(pool);
+	else
+		list_add(&pool->lh, &dap->cmd_pool);
+
+	dap->cmd_pool_size--;
+}
+
+static void flush_journal(struct adiv5_dap *dap, struct list_head *lh)
 {
 	struct dap_cmd *el, *tmp;
 
+	list_for_each_entry_safe(el, tmp, lh, lh) {
+		list_del(&el->lh);
+		dap_cmd_release(dap, el);
+	}
+}
+
+static void jtag_quit(struct adiv5_dap *dap)
+{
+	struct dap_cmd_pool *el, *tmp;
+	struct list_head *lh = &dap->cmd_pool;
+
 	list_for_each_entry_safe(el, tmp, lh, lh) {
 		list_del(&el->lh);
 		free(el);
@@ -204,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.
@@ -219,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;
 }
@@ -247,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.
  *
@@ -258,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
@@ -266,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(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;
@@ -293,16 +344,38 @@ 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) & DP_SELECT_DPBANK;
+
+	/* 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
+			&& (!dap->select_valid || sel != (dap->select & DP_SELECT_DPBANK))) {
+		/* Use the AP part of dap->select regardless of dap->select_valid:
+		 * if !dap->select_valid
+		 * dap->select contains a speculative value likely going to be used
+		 * in the following swd_queue_ap_bankselect() */
+		sel |= dap->select & SELECT_AP_MASK;
+
+		LOG_DEBUG_IO("DP BANK SELECT: %" PRIx32, (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;
 
+		dap->select = sel;
+		dap->select_valid = true;
+	}
 	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;
@@ -318,7 +391,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;
@@ -328,21 +401,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)
@@ -357,7 +430,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 	int retval;
 	struct dap_cmd *el, *tmp, *prev = NULL;
 	int found_wait = 0;
-	uint64_t time_now;
+	int64_t time_now;
 	LIST_HEAD(replay_list);
 
 	/* make sure all queued transactions are complete */
@@ -367,14 +440,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;
 		}
@@ -386,15 +464,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));
@@ -404,8 +483,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
@@ -415,9 +494,9 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 				* To complete the READ, we just keep polling RDBUFF
 				* until the WAIT condition clears
 				*/
-				tmp = dap_cmd_new(JTAG_DP_DPACC,
+				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;
 				}
@@ -427,8 +506,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));
@@ -437,7 +517,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;
 					}
@@ -446,15 +526,23 @@ 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_valid = false;
+						dap->select1_valid = false;
+						/* Keep dap->select unchanged, the same AP and AP bank
+						 * is likely going to be used further */
 						jtag_ap_q_abort(dap, NULL);
+						/* clear the sticky overrun condition */
+						adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
+							DP_CTRL_STAT, DPAP_WRITE,
+							dap->dp_ctrl_stat | SSTICKYORUN, NULL, 0);
 						retval = ERROR_JTAG_DEVICE_ERROR;
 					}
 				}
 
 				/* we're done with this command, release it */
-				free(tmp);
+				dap_cmd_release(dap, tmp);
 
 				if (retval != ERROR_OK)
 					goto done;
@@ -469,16 +557,16 @@ 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);
 	}
 
 	/* we're done with the journal, flush it */
-	flush_journal(&dap->cmd_journal);
+	flush_journal(dap, &dap->cmd_journal);
 
 	/* 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,
@@ -489,15 +577,21 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 		/* restore SELECT register first */
 		if (!list_empty(&replay_list)) {
 			el = list_first_entry(&replay_list, struct dap_cmd, lh);
-			tmp = dap_cmd_new(JTAG_DP_DPACC,
-					  DP_SELECT, DPAP_WRITE, (uint8_t *)&el->dp_select, NULL, 0);
-			if (tmp == NULL) {
+
+			uint8_t out_value_buf[4];
+			buf_set_u32(out_value_buf, 0, 32, (uint32_t)(el->dp_select));
+
+			tmp = dap_cmd_new(dap, JTAG_DP_DPACC,
+					  DP_SELECT, DPAP_WRITE, out_value_buf, NULL, 0);
+			if (!tmp) {
 				retval = ERROR_JTAG_DEVICE_ERROR;
 				goto done;
 			}
 			list_add(&tmp->lh, &replay_list);
 
-			dap->select = DP_SELECT_INVALID;
+			/* TODO: ADIv6 DP SELECT1 handling */
+
+			dap->select_valid = false;
 		}
 
 		list_for_each_entry_safe(el, tmp, &replay_list, lh) {
@@ -506,8 +600,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));
@@ -516,17 +610,33 @@ 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_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,
+						dap->dp_ctrl_stat | SSTICKYORUN, NULL, 0);
+				if (retval != ERROR_OK)
+					break;
 			} 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_valid = false;
+					dap->select1_valid = false;
+					/* Keep dap->select unchanged, the same AP and AP bank
+					 * is likely going to be used further */
 					jtag_ap_q_abort(dap, NULL);
+					/* clear the sticky overrun condition */
+					adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
+						DP_CTRL_STAT, DPAP_WRITE,
+						dap->dp_ctrl_stat | SSTICKYORUN, NULL, 0);
 					retval = ERROR_JTAG_DEVICE_ERROR;
+					break;
 				}
 			} else
 				break;
@@ -534,15 +644,15 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 	}
 
  done:
-	flush_journal(&replay_list);
-	flush_journal(&dap->cmd_journal);
+	flush_journal(dap, &replay_list);
+	flush_journal(dap, &dap->cmd_journal);
 	return retval;
 }
 
 static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
 {
 	int retval;
-	uint32_t ctrlstat;
+	uint32_t ctrlstat, pwrmask;
 
 	/* too expensive to call keep_alive() here */
 
@@ -560,11 +670,12 @@ static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
 	if (ctrlstat & SSTICKYERR) {
 		LOG_DEBUG("jtag-dp: CTRL/STAT 0x%" PRIx32, ctrlstat);
 		/* Check power to debug regions */
-		if ((ctrlstat & (CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ | CSYSPWRUPACK)) !=
-						(CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ | CSYSPWRUPACK)) {
+		pwrmask = CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ;
+		if (!dap->ignore_syspwrupack)
+			pwrmask |= CSYSPWRUPACK;
+		if ((ctrlstat & pwrmask) != pwrmask) {
 			LOG_ERROR("Debug regions are unpowered, an unexpected reset might have happened");
-			retval = ERROR_JTAG_DEVICE_ERROR;
-			goto done;
+			dap->do_reconnect = true;
 		}
 
 		if (ctrlstat & SSTICKYERR)
@@ -572,30 +683,67 @@ 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;
 
-		if (ctrlstat & SSTICKYERR) {
-			retval = ERROR_JTAG_DEVICE_ERROR;
-			goto done;
-		}
+		retval = ERROR_JTAG_DEVICE_ERROR;
 	}
 
  done:
-	flush_journal(&dap->cmd_journal);
+	flush_journal(dap, &dap->cmd_journal);
 	return retval;
 }
 
 /*--------------------------------------------------------------------------*/
 
+static int jtag_connect(struct adiv5_dap *dap)
+{
+	dap->do_reconnect = false;
+	return dap_dp_init(dap);
+}
+
+static int jtag_check_reconnect(struct adiv5_dap *dap)
+{
+	if (dap->do_reconnect)
+		return jtag_connect(dap);
+
+	return ERROR_OK;
+}
+
+static int jtag_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq)
+{
+	int retval;
+
+	switch (seq) {
+	case JTAG_TO_SWD:
+		retval =  jtag_add_tms_seq(swd_seq_jtag_to_swd_len,
+				swd_seq_jtag_to_swd, TAP_INVALID);
+		break;
+	case SWD_TO_JTAG:
+		retval = jtag_add_tms_seq(swd_seq_swd_to_jtag_len,
+				swd_seq_swd_to_jtag, TAP_RESET);
+		break;
+	default:
+		LOG_ERROR("Sequence %d not supported", seq);
+		return ERROR_FAIL;
+	}
+	if (retval == ERROR_OK)
+		retval = jtag_execute_queue();
+	return retval;
+}
+
 static int jtag_dp_q_read(struct adiv5_dap *dap, unsigned reg,
 		uint32_t *data)
 {
-	int retval =  adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC, reg,
+	int retval = jtag_limit_queue_size(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval =  adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC, reg,
 			DPAP_READ, 0, dap->last_read, 0, NULL);
 	dap->last_read = data;
 	return retval;
@@ -604,30 +752,84 @@ static int jtag_dp_q_read(struct adiv5_dap *dap, unsigned reg,
 static int jtag_dp_q_write(struct adiv5_dap *dap, unsigned reg,
 		uint32_t data)
 {
-	int retval =  adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
+	int retval = jtag_limit_queue_size(dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval =  adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
 			reg, DPAP_WRITE, data, dap->last_read, 0, NULL);
 	dap->last_read = NULL;
 	return retval;
 }
 
-/** Select the AP register bank matching bits 7:4 of reg. */
+/** Select the AP register bank */
 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 (sel == dap->select)
-		return ERROR_OK;
+	if (is_adiv6(dap))
+		sel = ap->ap_num | (reg & 0x00000FF0);
+	else
+		sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
+
+	uint64_t sel_diff = (sel ^ dap->select) & SELECT_AP_MASK;
+
+	bool set_select = !dap->select_valid || (sel_diff & 0xffffffffull);
+	bool set_select1 = is_adiv6(dap) && dap->asize > 32
+						&& (!dap->select1_valid
+							|| sel_diff & (0xffffffffull << 32));
+
+	if (set_select && set_select1) {
+		/* Prepare DP bank for DP_SELECT1 now to save one write */
+		sel |= (DP_SELECT1 >> 4) & DP_SELECT_DPBANK;
+	} else {
+		/* Use the DP part of dap->select regardless of dap->select_valid:
+		 * if !dap->select_valid
+		 * dap->select contains a speculative value likely going to be used
+		 * in the following swd_queue_dp_bankselect().
+		 * Moreover dap->select_valid should never be false here as a DP bank
+		 * is always selected before selecting an AP bank */
+		sel |= dap->select & DP_SELECT_DPBANK;
+	}
 
-	dap->select = sel;
+	if (set_select) {
+		LOG_DEBUG_IO("AP BANK SELECT: %" PRIx32, (uint32_t)sel);
+
+		retval = jtag_dp_q_write(dap, DP_SELECT, (uint32_t)sel);
+		if (retval != ERROR_OK) {
+			dap->select_valid = false;
+			return retval;
+		}
+	}
 
-	return jtag_dp_q_write(dap, DP_SELECT, sel);
+	if (set_select1) {
+		LOG_DEBUG_IO("AP BANK SELECT1: %" PRIx32, (uint32_t)(sel >> 32));
+
+		retval = jtag_dp_q_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
+		if (retval != ERROR_OK) {
+			dap->select1_valid = false;
+			return retval;
+		}
+	}
+
+	dap->select = sel;
+	return ERROR_OK;
 }
 
 static int jtag_ap_q_read(struct adiv5_ap *ap, unsigned reg,
 		uint32_t *data)
 {
-	int retval = jtag_ap_q_bankselect(ap, reg);
+	int retval = jtag_limit_queue_size(ap->dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = jtag_check_reconnect(ap->dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = jtag_ap_q_bankselect(ap, reg);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -641,7 +843,15 @@ static int jtag_ap_q_read(struct adiv5_ap *ap, unsigned reg,
 static int jtag_ap_q_write(struct adiv5_ap *ap, unsigned reg,
 		uint32_t data)
 {
-	int retval = jtag_ap_q_bankselect(ap, reg);
+	int retval = jtag_limit_queue_size(ap->dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = jtag_check_reconnect(ap->dap);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = jtag_ap_q_bankselect(ap, reg);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -686,6 +896,8 @@ static int jtag_dp_sync(struct adiv5_dap *dap)
  * part of DAP setup
 */
 const struct dap_ops jtag_dp_ops = {
+	.connect             = jtag_connect,
+	.send_sequence       = jtag_send_sequence,
 	.queue_dp_read       = jtag_dp_q_read,
 	.queue_dp_write      = jtag_dp_q_write,
 	.queue_ap_read       = jtag_ap_q_read,
@@ -693,53 +905,5 @@ const struct dap_ops jtag_dp_ops = {
 	.queue_ap_abort      = jtag_ap_q_abort,
 	.run                 = jtag_dp_run,
 	.sync                = jtag_dp_sync,
+	.quit                = jtag_quit,
 };
-
-
-static const uint8_t swd2jtag_bitseq[] = {
-	/* More than 50 TCK/SWCLK cycles with TMS/SWDIO high,
-	 * putting both JTAG and SWD logic into reset state.
-	 */
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
-	/* Switching equence disables SWD and enables JTAG
-	 * NOTE: bits in the DP's IDCODE can expose the need for
-	 * the old/deprecated sequence (0xae 0xde).
-	 */
-	0x3c, 0xe7,
-	/* At least 50 TCK/SWCLK cycles with TMS/SWDIO high,
-	 * putting both JTAG and SWD logic into reset state.
-	 * NOTE:  some docs say "at least 5".
-	 */
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
-};
-
-/** Put the debug link into JTAG mode, if the target supports it.
- * The link's initial mode may be either SWD or JTAG.
- *
- * @param target Enters JTAG mode (if possible).
- *
- * Note that targets implemented with SW-DP do not support JTAG, and
- * that some targets which could otherwise support it may have been
- * configured to disable JTAG signaling
- *
- * @return ERROR_OK or else a fault code.
- */
-int dap_to_jtag(struct target *target)
-{
-	int retval;
-
-	LOG_DEBUG("Enter JTAG mode");
-
-	/* REVISIT it's nasty to need to make calls to a "jtag"
-	 * subsystem if the link isn't in JTAG mode...
-	 */
-
-	retval = jtag_add_tms_seq(8 * sizeof(swd2jtag_bitseq),
-			swd2jtag_bitseq, TAP_RESET);
-	if (retval == ERROR_OK)
-		retval = jtag_execute_queue();
-
-	/* REVISIT set up the DAP's ops vector for JTAG mode. */
-
-	return retval;
-}