X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Fadi_v5_jtag.c;h=36d5cad5135d4b8ef5bc2dcd6abec668750b86ca;hp=4c0f040788999b8efe073f56e6f7371614c2afba;hb=f19ac83152b54a204b8148815a538d868973e1e1;hpb=bf4cf766310768198cfa766467d47bdb180f9b27

diff --git a/src/target/adi_v5_jtag.c b/src/target/adi_v5_jtag.c
index 4c0f040788..36d5cad513 100644
--- a/src/target/adi_v5_jtag.c
+++ b/src/target/adi_v5_jtag.c
@@ -21,9 +21,7 @@
  *   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, write to the
- *   Free Software Foundation, Inc.,
- *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
  ***************************************************************************/
 
 /**
@@ -39,6 +37,9 @@
 #include "arm.h"
 #include "arm_adi_v5.h"
 #include <helper/time_support.h>
+#include <helper/list.h>
+
+/*#define DEBUG_WAIT*/
 
 /* JTAG instructions/registers for JTAG-DP and SWJ-DP */
 #define JTAG_DP_ABORT		0x8
@@ -52,12 +53,198 @@
 
 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)
+{
+	char *reg_name = "UNK";
+
+	if (instr == JTAG_DP_DPACC) {
+		switch (reg_addr) {
+		case DP_ABORT:
+			reg_name =  "ABORT";
+			break;
+		case DP_CTRL_STAT:
+			reg_name =  "CTRL/STAT";
+			break;
+		case DP_SELECT:
+			reg_name = "SELECT";
+			break;
+		case DP_RDBUFF:
+			reg_name =  "RDBUFF";
+			break;
+		case DP_WCR:
+			reg_name =  "WCR";
+			break;
+		default:
+			reg_name = "UNK";
+			break;
+		}
+	}
+
+	if (instr == JTAG_DP_APACC) {
+		switch (reg_addr) {
+		case MEM_AP_REG_CSW:
+			reg_name = "CSW";
+			break;
+		case MEM_AP_REG_TAR:
+			reg_name = "TAR";
+			break;
+		case MEM_AP_REG_DRW:
+			reg_name = "DRW";
+			break;
+		case MEM_AP_REG_BD0:
+			reg_name = "BD0";
+			break;
+		case MEM_AP_REG_BD1:
+			reg_name = "BD1";
+			break;
+		case MEM_AP_REG_BD2:
+			reg_name = "BD2";
+			break;
+		case MEM_AP_REG_BD3:
+			reg_name = "BD3";
+			break;
+		case MEM_AP_REG_CFG:
+			reg_name = "CFG";
+			break;
+		case MEM_AP_REG_BASE:
+			reg_name = "BASE";
+			break;
+		case AP_REG_IDR:
+			reg_name = "IDR";
+			break;
+		default:
+			reg_name = "UNK";
+			break;
+		}
+	}
+
+	return reg_name;
+}
+#endif
+
+struct dap_cmd {
+	struct list_head lh;
+	uint8_t instr;
+	uint8_t reg_addr;
+	uint8_t RnW;
+	uint8_t *invalue;
+	uint8_t ack;
+	uint32_t memaccess_tck;
+	uint32_t dp_select;
+
+	struct scan_field fields[2];
+	uint8_t out_addr_buf;
+	uint8_t invalue_buf[4];
+	uint8_t outvalue_buf[4];
+};
+
+static void log_dap_cmd(const char *header, struct dap_cmd *el)
+{
+#ifdef DEBUG_WAIT
+	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",
+		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"));
+#endif
+}
+
+static struct dap_cmd *dap_cmd_new(uint8_t instr,
+		uint8_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;
+	}
+
+	return cmd;
+}
+
+static void flush_journal(struct list_head *lh)
+{
+	struct dap_cmd *el, *tmp;
+
+	list_for_each_entry_safe(el, tmp, lh, lh) {
+		list_del(&el->lh);
+		free(el);
+	}
+}
+
 /***************************************************************************
  *
  * DPACC and APACC scanchain access through JTAG-DP (or SWJ-DP)
  *
 ***************************************************************************/
 
+static int adi_jtag_dp_scan_cmd(struct adiv5_dap *dap, struct dap_cmd *cmd, uint8_t *ack)
+{
+	struct jtag_tap *tap = dap->tap;
+	int retval;
+
+	retval = arm_jtag_set_instr(tap, cmd->instr, NULL, TAP_IDLE);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Scan out a read or write operation using some DP or AP register.
+	 * 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));
+	cmd->fields[0].out_value = &cmd->out_addr_buf;
+	cmd->fields[0].in_value = (ack != NULL) ? 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.
+	 * When overrun detect is active, STICKYORUN is set.
+	 */
+
+	cmd->fields[1].num_bits = 32;
+	cmd->fields[1].out_value = cmd->outvalue_buf;
+	cmd->fields[1].in_value = cmd->invalue;
+
+	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.
+	 * They provide more time for the (MEM) AP to complete the read ...
+	 * See "Minimum Response Time" for JTAG-DP, in the ADIv5 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);
+	}
+
+	return ERROR_OK;
+}
+
+static int adi_jtag_dp_scan_cmd_sync(struct adiv5_dap *dap, struct dap_cmd *cmd, uint8_t *ack)
+{
+	int retval;
+
+	retval = adi_jtag_dp_scan_cmd(dap, cmd, ack);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return jtag_execute_queue();
+}
+
 /**
  * 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
@@ -75,53 +262,28 @@ static int jtag_ap_q_abort(struct adiv5_dap *dap, uint8_t *ack);
  * @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
+ * @param memaccess_tck number of idle cycles to add after AP access
  */
 
 static int adi_jtag_dp_scan(struct adiv5_dap *dap,
 		uint8_t instr, uint8_t reg_addr, uint8_t RnW,
-		uint8_t *outvalue, uint8_t *invalue, uint8_t *ack)
+		uint8_t *outvalue, uint8_t *invalue,
+		uint32_t memaccess_tck, uint8_t *ack)
 {
-	struct arm_jtag *jtag_info = dap->jtag_info;
-	struct scan_field fields[2];
-	uint8_t out_addr_buf;
+	struct dap_cmd *cmd;
 	int retval;
 
-	retval = arm_jtag_set_instr(jtag_info, instr, NULL, TAP_IDLE);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* Scan out a read or write operation using some DP or AP register.
-	 * For APACC access with any sticky error flag set, this is discarded.
-	 */
-	fields[0].num_bits = 3;
-	buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
-	fields[0].out_value = &out_addr_buf;
-	fields[0].in_value = ack;
-
-	/* NOTE: if we receive JTAG_ACK_WAIT, the previous operation did not
-	 * complete; data we write is discarded, data we read is unpredictable.
-	 * When overrun detect is active, STICKYORUN is set.
-	 */
-
-	fields[1].num_bits = 32;
-	fields[1].out_value = outvalue;
-	fields[1].in_value = invalue;
-
-	jtag_add_dr_scan(jtag_info->tap, 2, fields, TAP_IDLE);
+	cmd = dap_cmd_new(instr, reg_addr, RnW, outvalue, invalue, memaccess_tck);
+	if (cmd != NULL)
+		cmd->dp_select = dap->select;
+	else
+		return ERROR_JTAG_DEVICE_ERROR;
 
-	/* Add specified number of tck clocks after starting 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.
-	 */
-	if ((instr == JTAG_DP_APACC)
-			&& ((reg_addr == MEM_AP_REG_DRW)
-				|| ((reg_addr & 0xF0) == MEM_AP_REG_BD0))
-			&& (dap->ap[dap_ap_get_select(dap)].memaccess_tck != 0))
-		jtag_add_runtest(dap->ap[dap_ap_get_select(dap)].memaccess_tck,
-				TAP_IDLE);
+	retval = adi_jtag_dp_scan_cmd(dap, cmd, ack);
+	if (retval == ERROR_OK)
+		list_add_tail(&cmd->lh,	&dap->cmd_journal);
 
-	return ERROR_OK;
+	return retval;
 }
 
 /**
@@ -132,7 +294,8 @@ static int adi_jtag_dp_scan(struct adiv5_dap *dap,
  */
 static int adi_jtag_dp_scan_u32(struct adiv5_dap *dap,
 		uint8_t instr, uint8_t reg_addr, uint8_t RnW,
-		uint32_t outvalue, uint32_t *invalue, uint8_t *ack)
+		uint32_t outvalue, uint32_t *invalue,
+		uint32_t memaccess_tck, uint8_t *ack)
 {
 	uint8_t out_value_buf[4];
 	int retval;
@@ -140,7 +303,7 @@ static int adi_jtag_dp_scan_u32(struct adiv5_dap *dap,
 	buf_set_u32(out_value_buf, 0, 32, outvalue);
 
 	retval = adi_jtag_dp_scan(dap, instr, reg_addr, RnW,
-			out_value_buf, (uint8_t *)invalue, ack);
+			out_value_buf, (uint8_t *)invalue, memaccess_tck, ack);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -151,192 +314,280 @@ static int adi_jtag_dp_scan_u32(struct adiv5_dap *dap,
 	return retval;
 }
 
-/**
- * Utility to write AP registers.
- */
-static inline int adi_jtag_ap_write_check(struct adiv5_dap *dap,
-		uint8_t reg_addr, uint8_t *outvalue)
+static int adi_jtag_finish_read(struct adiv5_dap *dap)
 {
-	return adi_jtag_dp_scan(dap, JTAG_DP_APACC, reg_addr, DPAP_WRITE,
-			outvalue, NULL, NULL);
+	int retval = ERROR_OK;
+
+	if (dap->last_read != NULL) {
+		retval = adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
+				DP_RDBUFF, DPAP_READ, 0, dap->last_read, 0, NULL);
+		dap->last_read = NULL;
+	}
+
+	return retval;
 }
 
 static int adi_jtag_scan_inout_check_u32(struct adiv5_dap *dap,
 		uint8_t instr, uint8_t reg_addr, uint8_t RnW,
-		uint32_t outvalue, uint32_t *invalue)
+		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, 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 != NULL)) {
 		retval = adi_jtag_dp_scan_u32(dap, JTAG_DP_DPACC,
-				DP_RDBUFF, DPAP_READ, 0, invalue, &dap->ack);
-	return retval;
+				DP_RDBUFF, DPAP_READ, 0, invalue, 0, NULL);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return jtag_execute_queue();
 }
 
-static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
+static int jtagdp_overrun_check(struct adiv5_dap *dap)
 {
 	int retval;
-	uint32_t ctrlstat;
+	struct dap_cmd *el, *tmp, *prev = NULL;
+	int found_wait = 0;
+	int64_t time_now;
+	LIST_HEAD(replay_list);
 
-	/* too expensive to call keep_alive() here */
+	/* make sure all queued transactions are complete */
+	retval = jtag_execute_queue();
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* 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);
+		} 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);
+			retval = ERROR_JTAG_DEVICE_ERROR;
+			goto done;
+		}
+	}
 
-	/* Here be dragons!
-	 *
-	 * It is easy to be in a JTAG clock range where the target
-	 * is not operating in a stable fashion. This happens
-	 * for a few reasons:
-	 *
-	 * - the user may construct a simple test case to try to see
-	 * if a higher JTAG clock works to eke out more performance.
-	 * This simple case may pass, but more complex situations can
-	 * fail.
-	 *
-	 * - The mostly works JTAG clock rate and the complete failure
-	 * JTAG clock rate may be as much as 2-4x apart. This seems
-	 * to be especially true on RC oscillator driven parts.
-	 *
-	 * So: even if calling adi_jtag_scan_inout_check_u32() multiple
-	 * times here seems to "make things better here", it is just
-	 * hiding problems with too high a JTAG clock.
-	 *
-	 * Note that even if some parts have RCLK/RTCK, that doesn't
-	 * mean that RCLK/RTCK is the *correct* rate to run the JTAG
-	 * interface at, i.e. RCLK/RTCK rates can be "too high", especially
-	 * before the RC oscillator phase is not yet complete.
+	/*
+	 * If we found a stalled transaction and a previous transaction
+	 * exists, check if it's a READ access.
 	 */
+	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);
+			/* 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) {
+					/* recover the read value */
+					log_dap_cmd("FND", tmp);
+					if (el->invalue != el->invalue_buf) {
+						uint32_t invalue = le_to_h_u32(tmp->invalue);
+						memcpy(el->invalue, &invalue, sizeof(uint32_t));
+					}
+					prev = NULL;
+					break;
+				}
+			}
 
-	/* Post CTRL/STAT read; discard any previous posted read value
-	 * but collect its ACK status.
-	 */
-	retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
-			DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = jtag_execute_queue();
-	if (retval != ERROR_OK)
-		return retval;
+			if (prev != NULL) {
+				log_dap_cmd("LST", el);
+
+				/*
+				* At this point we're sure that no previous
+				* transaction completed and the DAP/AP is still
+				* in busy state. We know that the next "OK" scan
+				* will return the READ result we need to recover.
+				* To complete the READ, we just keep polling RDBUFF
+				* until the WAIT condition clears
+				*/
+				tmp = dap_cmd_new(JTAG_DP_DPACC,
+						DP_RDBUFF, DPAP_READ, NULL, NULL, 0);
+				if (tmp == NULL) {
+					retval = ERROR_JTAG_DEVICE_ERROR;
+					goto done;
+				}
+				/* synchronously retry the command until it succeeds */
+				time_now = timeval_ms();
+				do {
+					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);
+						if (el->invalue != el->invalue_buf) {
+							uint32_t invalue = le_to_h_u32(tmp->invalue);
+							memcpy(el->invalue, &invalue, sizeof(uint32_t));
+						}
+						break;
+					}
+					if (tmp->ack != JTAG_ACK_WAIT) {
+						LOG_ERROR("Invalid ACK (%1x) in DAP response", tmp->ack);
+						log_dap_cmd("ERR", tmp);
+						retval = ERROR_JTAG_DEVICE_ERROR;
+						break;
+					}
+
+				} while (timeval_ms() - time_now < 1000);
+
+				if (retval == ERROR_OK) {
+					/* timeout happened */
+					if (tmp->ack != JTAG_ACK_OK_FAULT) {
+						LOG_ERROR("Timeout during WAIT recovery");
+						jtag_ap_q_abort(dap, NULL);
+						retval = ERROR_JTAG_DEVICE_ERROR;
+					}
+				}
+
+				/* we're done with this command, release it */
+				free(tmp);
+
+				if (retval != ERROR_OK)
+					goto done;
 
-	dap->ack = dap->ack & 0x7;
+			}
+			/* make el->invalue point to the default invalue
+			* so that we can safely retry it without clobbering
+			* the result we just recovered */
+			el->invalue = el->invalue_buf;
+		}
+	}
 
-	/* common code path avoids calling timeval_ms() */
-	if (dap->ack != JTAG_ACK_OK_FAULT) {
-		long long then = timeval_ms();
+	/* 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);
+		list_move_tail(&el->lh, &replay_list);
+	}
 
-		while (dap->ack != JTAG_ACK_OK_FAULT) {
-			if (dap->ack == JTAG_ACK_WAIT) {
-				if ((timeval_ms()-then) > 1000) {
-					LOG_WARNING("Timeout (1000ms) waiting "
-						"for ACK=OK/FAULT "
-						"in JTAG-DP transaction - aborting");
+	/* we're done with the journal, flush it */
+	flush_journal(&dap->cmd_journal);
 
-					uint8_t ack;
-					int abort_ret = jtag_ap_q_abort(dap, &ack);
+	/* check for overrun condition in the last batch of transactions */
+	if (found_wait) {
+		LOG_INFO("DAP transaction stalled (WAIT) - slowing down");
+		/* 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)
+			goto done;
+
+		/* 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) {
+				retval = ERROR_JTAG_DEVICE_ERROR;
+				goto done;
+			}
+			list_add(&tmp->lh, &replay_list);
 
-					if (abort_ret != 0)
-						LOG_WARNING("Abort failed : return=%d ack=%d", abort_ret, ack);
+			dap->select = DP_SELECT_INVALID;
+		}
 
-					return ERROR_JTAG_DEVICE_ERROR;
+		list_for_each_entry_safe(el, tmp, &replay_list, lh) {
+			time_now = timeval_ms();
+			do {
+				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) {
+					if (el->invalue != el->invalue_buf) {
+						uint32_t invalue = le_to_h_u32(el->invalue);
+						memcpy(el->invalue, &invalue, sizeof(uint32_t));
+					}
+					break;
 				}
-			} else {
-				LOG_WARNING("Invalid ACK %#x "
-						"in JTAG-DP transaction",
-						dap->ack);
-				return ERROR_JTAG_DEVICE_ERROR;
-			}
+				if (el->ack != JTAG_ACK_WAIT) {
+					LOG_ERROR("Invalid ACK (%1x) in DAP response", el->ack);
+					log_dap_cmd("ERR", el);
+					retval = ERROR_JTAG_DEVICE_ERROR;
+					break;
+				}
+			} while (timeval_ms() - time_now < 1000);
 
-			retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
-					DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = jtag_execute_queue();
-			if (retval != ERROR_OK)
-				return retval;
-			dap->ack = dap->ack & 0x7;
+			if (retval == ERROR_OK) {
+				if (el->ack != JTAG_ACK_OK_FAULT) {
+					LOG_ERROR("Timeout during WAIT recovery");
+					jtag_ap_q_abort(dap, NULL);
+					retval = ERROR_JTAG_DEVICE_ERROR;
+				}
+			} else
+				break;
 		}
 	}
 
+ done:
+	flush_journal(&replay_list);
+	flush_journal(&dap->cmd_journal);
+	return retval;
+}
+
+static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
+{
+	int retval;
+	uint32_t ctrlstat;
+
+	/* too expensive to call keep_alive() here */
+
+	/* Post CTRL/STAT read; discard any previous posted read value
+	 * but collect its ACK status.
+	 */
+	retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
+			DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat, 0);
+	if (retval != ERROR_OK)
+		goto done;
+
 	/* REVISIT also STICKYCMP, for pushed comparisons (nyet used) */
 
-	/* Check for STICKYERR and STICKYORUN */
-	if (ctrlstat & (SSTICKYORUN | SSTICKYERR)) {
-		LOG_DEBUG("jtag-dp: CTRL/STAT error, 0x%" PRIx32, ctrlstat);
+	/* Check for STICKYERR */
+	if (ctrlstat & SSTICKYERR) {
+		LOG_DEBUG("jtag-dp: CTRL/STAT 0x%" PRIx32, ctrlstat);
 		/* Check power to debug regions */
-		if ((ctrlstat & 0xf0000000) != 0xf0000000) {
+		if ((ctrlstat & (CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ | CSYSPWRUPACK)) !=
+						(CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ | CSYSPWRUPACK)) {
 			LOG_ERROR("Debug regions are unpowered, an unexpected reset might have happened");
-			return ERROR_JTAG_DEVICE_ERROR;
-		} else {
-			uint32_t mem_ap_csw, mem_ap_tar;
-
-			/* Maybe print information about last intended
-			 * MEM-AP access; but not if autoincrementing.
-			 * *Real* CSW and TAR values are always shown.
-			 */
-			if (dap->ap[dap_ap_get_select(dap)].tar_value != (uint32_t) -1)
-				LOG_DEBUG("MEM-AP Cached values: "
-					"ap_bank 0x%" PRIx32
-					", ap_csw 0x%" PRIx32
-					", ap_tar 0x%" PRIx32,
-					dap->ap_bank_value,
-					dap->ap[dap_ap_get_select(dap)].csw_value,
-					dap->ap[dap_ap_get_select(dap)].tar_value);
-
-			if (ctrlstat & SSTICKYORUN)
-				LOG_ERROR("JTAG-DP OVERRUN - check clock, "
-					"memaccess, or reduce jtag speed");
-
-			if (ctrlstat & SSTICKYERR)
-				LOG_ERROR("JTAG-DP STICKY ERROR");
-
-			/* Clear Sticky Error Bits */
-			retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
-					DP_CTRL_STAT, DPAP_WRITE,
-					dap->dp_ctrl_stat | SSTICKYORUN
-						| SSTICKYERR, NULL);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
-					DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = jtag_execute_queue();
-			if (retval != ERROR_OK)
-				return retval;
-
-			LOG_DEBUG("jtag-dp: CTRL/STAT 0x%" PRIx32, ctrlstat);
-
-			retval = dap_queue_ap_read(dap,
-					MEM_AP_REG_CSW, &mem_ap_csw);
-			if (retval != ERROR_OK)
-				return retval;
-
-			retval = dap_queue_ap_read(dap,
-					MEM_AP_REG_TAR, &mem_ap_tar);
-			if (retval != ERROR_OK)
-				return retval;
-
-			retval = jtag_execute_queue();
-			if (retval != ERROR_OK)
-				return retval;
-			LOG_ERROR("MEM_AP_CSW 0x%" PRIx32 ", MEM_AP_TAR 0x%"
-					PRIx32, mem_ap_csw, mem_ap_tar);
-
+			retval = ERROR_JTAG_DEVICE_ERROR;
+			goto done;
 		}
-		retval = jtag_execute_queue();
+
+		if (ctrlstat & SSTICKYERR)
+			LOG_ERROR("JTAG-DP STICKY ERROR");
+		if (ctrlstat & SSTICKYORUN)
+			LOG_DEBUG("JTAG-DP STICKY OVERRUN");
+
+		/* Clear Sticky Error Bits */
+		retval = adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
+				DP_CTRL_STAT, DPAP_WRITE,
+				dap->dp_ctrl_stat | SSTICKYERR, NULL, 0);
 		if (retval != ERROR_OK)
-			return retval;
-		return ERROR_JTAG_DEVICE_ERROR;
+			goto done;
+
+		if (ctrlstat & SSTICKYERR) {
+			retval = ERROR_JTAG_DEVICE_ERROR;
+			goto done;
+		}
 	}
 
-	return ERROR_OK;
+ done:
+	flush_journal(&dap->cmd_journal);
+	return retval;
 }
 
 /*--------------------------------------------------------------------------*/
@@ -344,67 +595,91 @@ static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
 static int jtag_dp_q_read(struct adiv5_dap *dap, unsigned reg,
 		uint32_t *data)
 {
-	return adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
-			reg, DPAP_READ, 0, data);
+	int 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;
 }
 
 static int jtag_dp_q_write(struct adiv5_dap *dap, unsigned reg,
 		uint32_t data)
 {
-	return adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
-			reg, DPAP_WRITE, data, NULL);
+	int 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. */
-static int jtag_ap_q_bankselect(struct adiv5_dap *dap, unsigned reg)
+static int jtag_ap_q_bankselect(struct adiv5_ap *ap, unsigned reg)
 {
-	uint32_t select_ap_bank = reg & 0x000000F0;
+	struct adiv5_dap *dap = ap->dap;
+	uint32_t sel = ((uint32_t)ap->ap_num << 24) | (reg & 0x000000F0);
 
-	if (select_ap_bank == dap->ap_bank_value)
+	if (sel == dap->select)
 		return ERROR_OK;
-	dap->ap_bank_value = select_ap_bank;
 
-	select_ap_bank |= dap->ap_current;
+	dap->select = sel;
 
-	return jtag_dp_q_write(dap, DP_SELECT, select_ap_bank);
+	return jtag_dp_q_write(dap, DP_SELECT, sel);
 }
 
-static int jtag_ap_q_read(struct adiv5_dap *dap, unsigned reg,
+static int jtag_ap_q_read(struct adiv5_ap *ap, unsigned reg,
 		uint32_t *data)
 {
-	int retval = jtag_ap_q_bankselect(dap, reg);
-
+	int retval = jtag_ap_q_bankselect(ap, reg);
 	if (retval != ERROR_OK)
 		return retval;
 
-	return adi_jtag_scan_inout_check_u32(dap, JTAG_DP_APACC, reg,
-			DPAP_READ, 0, data);
+	retval =  adi_jtag_dp_scan_u32(ap->dap, JTAG_DP_APACC, reg,
+			DPAP_READ, 0, ap->dap->last_read, ap->memaccess_tck, NULL);
+	ap->dap->last_read = data;
+
+	return retval;
 }
 
-static int jtag_ap_q_write(struct adiv5_dap *dap, unsigned reg,
+static int jtag_ap_q_write(struct adiv5_ap *ap, unsigned reg,
 		uint32_t data)
 {
-	uint8_t out_value_buf[4];
-
-	int retval = jtag_ap_q_bankselect(dap, reg);
+	int retval = jtag_ap_q_bankselect(ap, reg);
 	if (retval != ERROR_OK)
 		return retval;
 
-	buf_set_u32(out_value_buf, 0, 32, data);
-
-	return adi_jtag_ap_write_check(dap, reg, out_value_buf);
+	retval =  adi_jtag_dp_scan_u32(ap->dap, JTAG_DP_APACC, reg,
+			DPAP_WRITE, data, ap->dap->last_read, ap->memaccess_tck, NULL);
+	ap->dap->last_read = NULL;
+	return retval;
 }
 
 static int jtag_ap_q_abort(struct adiv5_dap *dap, uint8_t *ack)
 {
 	/* for JTAG, this is the only valid ABORT register operation */
-	return adi_jtag_dp_scan_u32(dap, JTAG_DP_ABORT,
-			0, DPAP_WRITE, 1, NULL, ack);
+	int retval =  adi_jtag_dp_scan_u32(dap, JTAG_DP_ABORT,
+			0, DPAP_WRITE, 1, NULL, 0, NULL);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return jtag_execute_queue();
 }
 
 static int jtag_dp_run(struct adiv5_dap *dap)
 {
-	return jtagdp_transaction_endcheck(dap);
+	int retval;
+	int retval2 = ERROR_OK;
+
+	retval = adi_jtag_finish_read(dap);
+	if (retval != ERROR_OK)
+		goto done;
+	retval2 = jtagdp_overrun_check(dap);
+	retval = jtagdp_transaction_endcheck(dap);
+
+ done:
+	return (retval2 != ERROR_OK) ? retval2 : retval;
+}
+
+static int jtag_dp_sync(struct adiv5_dap *dap)
+{
+	return jtagdp_overrun_check(dap);
 }
 
 /* FIXME don't export ... just initialize as
@@ -417,6 +692,7 @@ const struct dap_ops jtag_dp_ops = {
 	.queue_ap_write      = jtag_ap_q_write,
 	.queue_ap_abort      = jtag_ap_q_abort,
 	.run                 = jtag_dp_run,
+	.sync                = jtag_dp_sync,
 };