X-Git-Url: https://git.gag.com/?a=blobdiff_plain;ds=sidebyside;f=src%2Ftarget%2Farm_adi_v5.c;h=c5e0dd38a097b3500ad0f2f55f55e47906ed78af;hb=381ce4308c60c54e3a03d97e883302909b834875;hp=435d65979eb8017f77d05ac17576eaa3a6f37c04;hpb=75067c40424f0e3349f445ed4ec43476a89973da;p=fw%2Fopenocd

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 435d65979..c5e0dd38a 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -213,7 +213,7 @@ static int adi_jtag_scan_inout_check_u32(struct swjdp_common *swjdp,
 	return retval;
 }
 
-int jtagdp_transaction_endcheck(struct swjdp_common *swjdp)
+static int jtagdp_transaction_endcheck(struct swjdp_common *swjdp)
 {
 	int retval;
 	uint32_t ctrlstat;
@@ -277,7 +277,7 @@ int jtagdp_transaction_endcheck(struct swjdp_common *swjdp)
 
 			adi_jtag_scan_inout_check_u32(swjdp, JTAG_DP_DPACC,
 					DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
-			if ((retval = jtag_execute_queue()) != ERROR_OK)
+			if ((retval = dap_run(swjdp)) != ERROR_OK)
 				return retval;
 			swjdp->ack = swjdp->ack & 0x7;
 		}
@@ -323,20 +323,28 @@ int jtagdp_transaction_endcheck(struct swjdp_common *swjdp)
 						| SSTICKYERR, NULL);
 			adi_jtag_scan_inout_check_u32(swjdp, JTAG_DP_DPACC,
 					DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
-			if ((retval = jtag_execute_queue()) != ERROR_OK)
+			if ((retval = dap_run(swjdp)) != ERROR_OK)
 				return retval;
 
 			LOG_DEBUG("jtag-dp: CTRL/STAT 0x%" PRIx32, ctrlstat);
 
-			dap_ap_read_reg_u32(swjdp, AP_REG_CSW, &mem_ap_csw);
-			dap_ap_read_reg_u32(swjdp, AP_REG_TAR, &mem_ap_tar);
-			if ((retval = jtag_execute_queue()) != ERROR_OK)
+			retval = dap_queue_ap_read(swjdp,
+					AP_REG_CSW, &mem_ap_csw);
+			if (retval != ERROR_OK)
+				return retval;
+
+			retval = dap_queue_ap_read(swjdp,
+					AP_REG_TAR, &mem_ap_tar);
+			if (retval != ERROR_OK)
+				return retval;
+
+			if ((retval = dap_run(swjdp)) != ERROR_OK)
 				return retval;
 			LOG_ERROR("MEM_AP_CSW 0x%" PRIx32 ", MEM_AP_TAR 0x%"
 					PRIx32, mem_ap_csw, mem_ap_tar);
 
 		}
-		if ((retval = jtag_execute_queue()) != ERROR_OK)
+		if ((retval = dap_run(swjdp)) != ERROR_OK)
 			return retval;
 		return ERROR_JTAG_DEVICE_ERROR;
 	}
@@ -350,20 +358,6 @@ int jtagdp_transaction_endcheck(struct swjdp_common *swjdp)
  *                                                                         *
 ***************************************************************************/
 
-static int dap_dp_write_reg(struct swjdp_common *swjdp,
-		uint32_t value, uint8_t reg_addr)
-{
-	return adi_jtag_scan_inout_check_u32(swjdp, JTAG_DP_DPACC,
-			reg_addr, DPAP_WRITE, value, NULL);
-}
-
-static int dap_dp_read_reg(struct swjdp_common *swjdp,
-		uint32_t *value, uint8_t reg_addr)
-{
-	return adi_jtag_scan_inout_check_u32(swjdp, JTAG_DP_DPACC,
-			reg_addr, DPAP_READ, 0, value);
-}
-
 /**
  * Select one of the APs connected to the specified DAP.  The
  * selection is implicitly used with future AP transactions.
@@ -389,73 +383,6 @@ void dap_ap_select(struct swjdp_common *swjdp,uint8_t apsel)
 	}
 }
 
-/** Select the AP register bank matching bits 7:4 of ap_reg. */
-static int dap_ap_bankselect(struct swjdp_common *swjdp, uint32_t ap_reg)
-{
-	uint32_t select = (ap_reg & 0x000000F0);
-
-	if (select != swjdp->ap_bank_value)
-	{
-		swjdp->ap_bank_value = select;
-		select |= swjdp->apsel;
-		return dap_dp_write_reg(swjdp, select, DP_SELECT);
-	} else
-		return ERROR_OK;
-}
-
-static int dap_ap_write_reg(struct swjdp_common *swjdp,
-		uint32_t reg_addr, uint8_t *out_value_buf)
-{
-	int retval;
-
-	retval = dap_ap_bankselect(swjdp, reg_addr);
-	if (retval != ERROR_OK)
-		return retval;
-
-	return adi_jtag_ap_write_check(swjdp, reg_addr, out_value_buf);
-}
-
-/**
- * Asynchronous (queued) AP register write.
- *
- * @param swjdp The DAP whose currently selected AP will be written.
- * @param reg_addr Eight bit AP register address.
- * @param value Word to be written at reg_addr
- *
- * @return ERROR_OK if the transaction was properly queued, else a fault code.
- */
-int dap_ap_write_reg_u32(struct swjdp_common *swjdp,
-		uint32_t reg_addr, uint32_t value)
-{
-	uint8_t out_value_buf[4];
-
-	buf_set_u32(out_value_buf, 0, 32, value);
-	return dap_ap_write_reg(swjdp,
-			reg_addr, out_value_buf);
-}
-
-/**
- * Asynchronous (queued) AP register eread.
- *
- * @param swjdp The DAP whose currently selected AP will be read.
- * @param reg_addr Eight bit AP register address.
- * @param value Points to where the 32-bit (little-endian) word will be stored.
- *
- * @return ERROR_OK if the transaction was properly queued, else a fault code.
- */
-int dap_ap_read_reg_u32(struct swjdp_common *swjdp,
-		uint32_t reg_addr, uint32_t *value)
-{
-	int retval;
-
-	retval = dap_ap_bankselect(swjdp, reg_addr);
-	if (retval != ERROR_OK)
-		return retval;
-
-	return adi_jtag_scan_inout_check_u32(swjdp, JTAG_DP_APACC, reg_addr,
-			DPAP_READ, 0, value);
-}
-
 /**
  * Queue transactions setting up transfer parameters for the
  * currently selected MEM-AP.
@@ -483,7 +410,7 @@ int dap_setup_accessport(struct swjdp_common *swjdp, uint32_t csw, uint32_t tar)
 	if (csw != swjdp->ap_csw_value)
 	{
 		/* LOG_DEBUG("DAP: Set CSW %x",csw); */
-		retval = dap_ap_write_reg_u32(swjdp, AP_REG_CSW, csw);
+		retval = dap_queue_ap_write(swjdp, AP_REG_CSW, csw);
 		if (retval != ERROR_OK)
 			return retval;
 		swjdp->ap_csw_value = csw;
@@ -491,7 +418,7 @@ int dap_setup_accessport(struct swjdp_common *swjdp, uint32_t csw, uint32_t tar)
 	if (tar != swjdp->ap_tar_value)
 	{
 		/* LOG_DEBUG("DAP: Set TAR %x",tar); */
-		retval = dap_ap_write_reg_u32(swjdp, AP_REG_TAR, tar);
+		retval = dap_queue_ap_write(swjdp, AP_REG_TAR, tar);
 		if (retval != ERROR_OK)
 			return retval;
 		swjdp->ap_tar_value = tar;
@@ -526,7 +453,7 @@ int mem_ap_read_u32(struct swjdp_common *swjdp, uint32_t address,
 	if (retval != ERROR_OK)
 		return retval;
 
-	return dap_ap_read_reg_u32(swjdp, AP_REG_BD0 | (address & 0xC), value);
+	return dap_queue_ap_read(swjdp, AP_REG_BD0 | (address & 0xC), value);
 }
 
 /**
@@ -550,7 +477,7 @@ int mem_ap_read_atomic_u32(struct swjdp_common *swjdp, uint32_t address,
 	if (retval != ERROR_OK)
 		return retval;
 
-	return jtagdp_transaction_endcheck(swjdp);
+	return dap_run(swjdp);
 }
 
 /**
@@ -577,7 +504,7 @@ int mem_ap_write_u32(struct swjdp_common *swjdp, uint32_t address,
 	if (retval != ERROR_OK)
 		return retval;
 
-	return dap_ap_write_reg_u32(swjdp, AP_REG_BD0 | (address & 0xC),
+	return dap_queue_ap_write(swjdp, AP_REG_BD0 | (address & 0xC),
 			value);
 }
 
@@ -600,7 +527,7 @@ int mem_ap_write_atomic_u32(struct swjdp_common *swjdp, uint32_t address,
 	if (retval != ERROR_OK)
 		return retval;
 
-	return jtagdp_transaction_endcheck(swjdp);
+	return dap_run(swjdp);
 }
 
 /*****************************************************************************
@@ -653,10 +580,13 @@ int mem_ap_write_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer, int count,
 
 		for (writecount = 0; writecount < blocksize; writecount++)
 		{
-			dap_ap_write_reg(swjdp, AP_REG_DRW, buffer + 4 * writecount);
+			retval = dap_queue_ap_write(swjdp, AP_REG_DRW,
+				*(uint32_t *) (buffer + 4 * writecount));
+			if (retval != ERROR_OK)
+				break;
 		}
 
-		if (jtagdp_transaction_endcheck(swjdp) == ERROR_OK)
+		if (dap_run(swjdp) == ERROR_OK)
 		{
 			wcount = wcount - blocksize;
 			address = address + 4 * blocksize;
@@ -670,6 +600,7 @@ int mem_ap_write_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer, int count,
 		if (errorcount > 1)
 		{
 			LOG_WARNING("Block write error address 0x%" PRIx32 ", wcount 0x%x", address, wcount);
+			/* REVISIT return the *actual* fault code */
 			return ERROR_JTAG_DEVICE_ERROR;
 		}
 	}
@@ -732,12 +663,17 @@ static int mem_ap_write_buf_packed_u16(struct swjdp_common *swjdp,
 				}
 
 				memcpy(&outvalue, buffer, sizeof(uint32_t));
-				dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
-				if (jtagdp_transaction_endcheck(swjdp) != ERROR_OK)
+				retval = dap_queue_ap_write(swjdp,
+						AP_REG_DRW, outvalue);
+				if (retval != ERROR_OK)
+					break;
+
+				if (dap_run(swjdp) != ERROR_OK)
 				{
 					LOG_WARNING("Block write error address "
 						"0x%" PRIx32 ", count 0x%x",
 						address, count);
+					/* REVISIT return *actual* fault code */
 					return ERROR_JTAG_DEVICE_ERROR;
 				}
 			}
@@ -765,8 +701,14 @@ int mem_ap_write_buf_u16(struct swjdp_common *swjdp, uint8_t *buffer, int count,
 		uint16_t svalue;
 		memcpy(&svalue, buffer, sizeof(uint16_t));
 		uint32_t outvalue = (uint32_t)svalue << 8 * (address & 0x3);
-		dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
-		retval = jtagdp_transaction_endcheck(swjdp);
+		retval = dap_queue_ap_write(swjdp, AP_REG_DRW, outvalue);
+		if (retval != ERROR_OK)
+			break;
+
+		retval = dap_run(swjdp);
+		if (retval != ERROR_OK)
+			break;
+
 		count -= 2;
 		address += 2;
 		buffer += 2;
@@ -825,12 +767,17 @@ static int mem_ap_write_buf_packed_u8(struct swjdp_common *swjdp,
 				}
 
 				memcpy(&outvalue, buffer, sizeof(uint32_t));
-				dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
-				if (jtagdp_transaction_endcheck(swjdp) != ERROR_OK)
+				retval = dap_queue_ap_write(swjdp,
+						AP_REG_DRW, outvalue);
+				if (retval != ERROR_OK)
+					break;
+
+				if (dap_run(swjdp) != ERROR_OK)
 				{
 					LOG_WARNING("Block write error address "
 						"0x%" PRIx32 ", count 0x%x",
 						address, count);
+					/* REVISIT return *actual* fault code */
 					return ERROR_JTAG_DEVICE_ERROR;
 				}
 			}
@@ -856,8 +803,14 @@ int mem_ap_write_buf_u8(struct swjdp_common *swjdp, uint8_t *buffer, int count,
 	{
 		dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 		uint32_t outvalue = (uint32_t)*buffer << 8 * (address & 0x3);
-		dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
-		retval = jtagdp_transaction_endcheck(swjdp);
+		retval = dap_queue_ap_write(swjdp, AP_REG_DRW, outvalue);
+		if (retval != ERROR_OK)
+			break;
+
+		retval = dap_run(swjdp);
+		if (retval != ERROR_OK)
+			break;
+
 		count--;
 		address++;
 		buffer++;
@@ -866,14 +819,16 @@ int mem_ap_write_buf_u8(struct swjdp_common *swjdp, uint8_t *buffer, int count,
 	return retval;
 }
 
-/*********************************************************************************
-*                                                                                *
-* mem_ap_read_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)  *
-*                                                                                *
-* Read block fast in target order (little endian) into a buffer                  *
-*                                                                                *
-**********************************************************************************/
-int mem_ap_read_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
+/**
+ * Synchronously read a block of 32-bit words into a buffer
+ * @param swjdp The DAP connected to the MEM-AP.
+ * @param buffer where the words will be stored (in host byte order).
+ * @param count How many words to read.
+ * @param address Memory address from which to read words; all the
+ *	words must be readable by the currently selected MEM-AP.
+ */
+int mem_ap_read_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer,
+		int count, uint32_t address)
 {
 	int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
 	uint32_t adr = address;
@@ -884,8 +839,12 @@ int mem_ap_read_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer, int count,
 
 	while (wcount > 0)
 	{
-		/* Adjust to read blocks within boundaries aligned to the TAR autoincremnent size*/
-		blocksize = max_tar_block_size(swjdp->tar_autoincr_block, address);
+		/* Adjust to read blocks within boundaries aligned to the
+		 * TAR autoincrement size (at least 2^10).  Autoincrement
+		 * mode avoids an extra per-word roundtrip to update TAR.
+		 */
+		blocksize = max_tar_block_size(swjdp->tar_autoincr_block,
+				address);
 		if (wcount < blocksize)
 			blocksize = wcount;
 
@@ -893,7 +852,15 @@ int mem_ap_read_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer, int count,
 		if (blocksize == 0)
 			blocksize = 1;
 
-		dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
+		dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE,
+				address);
+
+		/* FIXME remove these three calls to adi_jtag_dp_scan(),
+		 * so this routine becomes transport-neutral.  Be careful
+		 * not to cause performance problems with JTAG; would it
+		 * suffice to loop over dap_queue_ap_read(), or would that
+		 * be slower when JTAG is the chosen transport?
+		 */
 
 		/* Scan out first read */
 		adi_jtag_dp_scan(swjdp, JTAG_DP_APACC, AP_REG_DRW,
@@ -915,7 +882,7 @@ int mem_ap_read_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer, int count,
 		adi_jtag_dp_scan(swjdp, JTAG_DP_DPACC, DP_RDBUFF,
 				DPAP_READ, 0, buffer + 4 * readcount,
 				&swjdp->ack);
-		if (jtagdp_transaction_endcheck(swjdp) == ERROR_OK)
+		if (dap_run(swjdp) == ERROR_OK)
 		{
 			wcount = wcount - blocksize;
 			address += 4 * blocksize;
@@ -928,7 +895,9 @@ int mem_ap_read_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer, int count,
 
 		if (errorcount > 1)
 		{
-			LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
+			LOG_WARNING("Block read error address 0x%" PRIx32
+				", count 0x%x", address, count);
+			/* REVISIT return the *actual* fault code */
 			return ERROR_JTAG_DEVICE_ERROR;
 		}
 	}
@@ -944,7 +913,8 @@ int mem_ap_read_buf_u32(struct swjdp_common *swjdp, uint8_t *buffer, int count,
 
 			for (i = 0; i < 4; i++)
 			{
-				*((uint8_t*)pBuffer) = (data >> 8 * (adr & 0x3));
+				*((uint8_t*)pBuffer) =
+						(data >> 8 * (adr & 0x3));
 				pBuffer++;
 				adr++;
 			}
@@ -981,10 +951,11 @@ static int mem_ap_read_buf_packed_u16(struct swjdp_common *swjdp,
 
 		do
 		{
-			dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue);
-			if (jtagdp_transaction_endcheck(swjdp) != ERROR_OK)
+			retval = dap_queue_ap_read(swjdp, AP_REG_DRW, &invalue);
+			if (dap_run(swjdp) != ERROR_OK)
 			{
 				LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
+				/* REVISIT return the *actual* fault code */
 				return ERROR_JTAG_DEVICE_ERROR;
 			}
 
@@ -1005,7 +976,16 @@ static int mem_ap_read_buf_packed_u16(struct swjdp_common *swjdp,
 	return retval;
 }
 
-int mem_ap_read_buf_u16(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
+/**
+ * Synchronously read a block of 16-bit halfwords into a buffer
+ * @param swjdp The DAP connected to the MEM-AP.
+ * @param buffer where the halfwords will be stored (in host byte order).
+ * @param count How many halfwords to read.
+ * @param address Memory address from which to read words; all the
+ *	words must be readable by the currently selected MEM-AP.
+ */
+int mem_ap_read_buf_u16(struct swjdp_common *swjdp, uint8_t *buffer,
+		int count, uint32_t address)
 {
 	uint32_t invalue, i;
 	int retval = ERROR_OK;
@@ -1016,8 +996,14 @@ int mem_ap_read_buf_u16(struct swjdp_common *swjdp, uint8_t *buffer, int count,
 	while (count > 0)
 	{
 		dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
-		dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue);
-		retval = jtagdp_transaction_endcheck(swjdp);
+		retval = dap_queue_ap_read(swjdp, AP_REG_DRW, &invalue);
+		if (retval != ERROR_OK)
+			break;
+
+		retval = dap_run(swjdp);
+		if (retval != ERROR_OK)
+			break;
+
 		if (address & 0x1)
 		{
 			for (i = 0; i < 2; i++)
@@ -1070,10 +1056,11 @@ static int mem_ap_read_buf_packed_u8(struct swjdp_common *swjdp,
 
 		do
 		{
-			dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue);
-			if (jtagdp_transaction_endcheck(swjdp) != ERROR_OK)
+			retval = dap_queue_ap_read(swjdp, AP_REG_DRW, &invalue);
+			if (dap_run(swjdp) != ERROR_OK)
 			{
 				LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
+				/* REVISIT return the *actual* fault code */
 				return ERROR_JTAG_DEVICE_ERROR;
 			}
 
@@ -1094,7 +1081,16 @@ static int mem_ap_read_buf_packed_u8(struct swjdp_common *swjdp,
 	return retval;
 }
 
-int mem_ap_read_buf_u8(struct swjdp_common *swjdp, uint8_t *buffer, int count, uint32_t address)
+/**
+ * Synchronously read a block of bytes into a buffer
+ * @param swjdp The DAP connected to the MEM-AP.
+ * @param buffer where the bytes will be stored.
+ * @param count How many bytes to read.
+ * @param address Memory address from which to read data; all the
+ *	data must be readable by the currently selected MEM-AP.
+ */
+int mem_ap_read_buf_u8(struct swjdp_common *swjdp, uint8_t *buffer,
+		int count, uint32_t address)
 {
 	uint32_t invalue;
 	int retval = ERROR_OK;
@@ -1105,8 +1101,11 @@ int mem_ap_read_buf_u8(struct swjdp_common *swjdp, uint8_t *buffer, int count, u
 	while (count > 0)
 	{
 		dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
-		dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue);
-		retval = jtagdp_transaction_endcheck(swjdp);
+		retval = dap_queue_ap_read(swjdp, AP_REG_DRW, &invalue);
+		retval = dap_run(swjdp);
+		if (retval != ERROR_OK)
+			break;
+
 		*((uint8_t*)buffer) = (invalue >> 8 * (address & 0x3));
 		count--;
 		address++;
@@ -1116,6 +1115,116 @@ int mem_ap_read_buf_u8(struct swjdp_common *swjdp, uint8_t *buffer, int count, u
 	return retval;
 }
 
+/*--------------------------------------------------------------------------*/
+
+static int jtag_idcode_q_read(struct swjdp_common *dap,
+		uint8_t *ack, uint32_t *data)
+{
+	struct arm_jtag *jtag_info = dap->jtag_info;
+	int retval;
+	struct scan_field fields[1];
+
+	jtag_set_end_state(TAP_IDLE);
+
+	/* This is a standard JTAG operation -- no DAP tweakage */
+	retval = arm_jtag_set_instr(jtag_info, JTAG_DP_IDCODE, NULL);
+	if (retval != ERROR_OK)
+		return retval;
+
+	fields[0].tap = jtag_info->tap;
+	fields[0].num_bits = 32;
+	fields[0].out_value = NULL;
+	fields[0].in_value = (void *) data;
+
+	jtag_add_dr_scan(1, fields, jtag_get_end_state());
+	retval = jtag_get_error();
+	if (retval != ERROR_OK)
+		return retval;
+
+	jtag_add_callback(arm_le_to_h_u32,
+			(jtag_callback_data_t) data);
+
+	return retval;
+}
+
+static int jtag_dp_q_read(struct swjdp_common *dap, unsigned reg,
+		uint32_t *data)
+{
+	return adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
+			reg, DPAP_READ, 0, data);
+}
+
+static int jtag_dp_q_write(struct swjdp_common *dap, unsigned reg,
+		uint32_t data)
+{
+	return adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
+			reg, DPAP_WRITE, data, NULL);
+}
+
+/** Select the AP register bank matching bits 7:4 of reg. */
+static int jtag_ap_q_bankselect(struct swjdp_common *dap, unsigned reg)
+{
+	uint32_t select = reg & 0x000000F0;
+
+	if (select == dap->ap_bank_value)
+		return ERROR_OK;
+	dap->ap_bank_value = select;
+
+	select |= dap->apsel;
+
+	return jtag_dp_q_write(dap, DP_SELECT, select);
+}
+
+static int jtag_ap_q_read(struct swjdp_common *dap, unsigned reg,
+		uint32_t *data)
+{
+	int retval = jtag_ap_q_bankselect(dap, reg);
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	return adi_jtag_scan_inout_check_u32(dap, JTAG_DP_APACC, reg,
+			DPAP_READ, 0, data);
+}
+
+static int jtag_ap_q_write(struct swjdp_common *dap, unsigned reg,
+		uint32_t data)
+{
+	uint8_t out_value_buf[4];
+
+	int retval = jtag_ap_q_bankselect(dap, 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);
+}
+
+static int jtag_ap_q_abort(struct swjdp_common *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);
+}
+
+static int jtag_dp_run(struct swjdp_common *dap)
+{
+	return jtagdp_transaction_endcheck(dap);
+}
+
+static const struct dap_ops jtag_dp_ops = {
+	.queue_idcode_read =	jtag_idcode_q_read,
+	.queue_dp_read =	jtag_dp_q_read,
+	.queue_dp_write =	jtag_dp_q_write,
+	.queue_ap_read =	jtag_ap_q_read,
+	.queue_ap_write =	jtag_ap_q_write,
+	.queue_ap_abort =	jtag_ap_q_abort,
+	.run =			jtag_dp_run,
+};
+
+/*--------------------------------------------------------------------------*/
+
 /**
  * Initialize a DAP.  This sets up the power domains, prepares the DP
  * for further use, and arranges to use AP #0 for all AP operations
@@ -1137,6 +1246,9 @@ int ahbap_debugport_init(struct swjdp_common *swjdp)
 
 	LOG_DEBUG(" ");
 
+	/* JTAG-DP or SWJ-DP, in JTAG mode */
+	swjdp->ops = &jtag_dp_ops;
+
 	/* Default MEM-AP setup.
 	 *
 	 * REVISIT AP #0 may be an inappropriate default for this.
@@ -1147,23 +1259,38 @@ int ahbap_debugport_init(struct swjdp_common *swjdp)
 	dap_ap_select(swjdp, 0);
 
 	/* DP initialization */
-	dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
-	dap_dp_write_reg(swjdp, SSTICKYERR, DP_CTRL_STAT);
-	dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
+
+	retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &dummy);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dap_queue_dp_write(swjdp, DP_CTRL_STAT, SSTICKYERR);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &dummy);
+	if (retval != ERROR_OK)
+		return retval;
 
 	swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
+	retval = dap_queue_dp_write(swjdp, DP_CTRL_STAT, swjdp->dp_ctrl_stat);
+	if (retval != ERROR_OK)
+		return retval;
 
-	dap_dp_write_reg(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
-	dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
-	if ((retval = jtag_execute_queue()) != ERROR_OK)
+	retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &ctrlstat);
+	if (retval != ERROR_OK)
+		return retval;
+	if ((retval = dap_run(swjdp)) != ERROR_OK)
 		return retval;
 
 	/* Check that we have debug power domains activated */
 	while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))
 	{
 		LOG_DEBUG("DAP: wait CDBGPWRUPACK");
-		dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
-		if ((retval = jtag_execute_queue()) != ERROR_OK)
+		retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &ctrlstat);
+		if (retval != ERROR_OK)
+			return retval;
+		if ((retval = dap_run(swjdp)) != ERROR_OK)
 			return retval;
 		alive_sleep(10);
 	}
@@ -1171,17 +1298,25 @@ int ahbap_debugport_init(struct swjdp_common *swjdp)
 	while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))
 	{
 		LOG_DEBUG("DAP: wait CSYSPWRUPACK");
-		dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
-		if ((retval = jtag_execute_queue()) != ERROR_OK)
+		retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &ctrlstat);
+		if (retval != ERROR_OK)
+			return retval;
+		if ((retval = dap_run(swjdp)) != ERROR_OK)
 			return retval;
 		alive_sleep(10);
 	}
 
-	dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
+	retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &dummy);
+	if (retval != ERROR_OK)
+		return retval;
 	/* With debug power on we can activate OVERRUN checking */
 	swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
-	dap_dp_write_reg(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
-	dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
+	retval = dap_queue_dp_write(swjdp, DP_CTRL_STAT, swjdp->dp_ctrl_stat);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &dummy);
+	if (retval != ERROR_OK)
+		return retval;
 
 	/*
 	 * REVISIT this isn't actually *initializing* anything in an AP,
@@ -1189,8 +1324,8 @@ int ahbap_debugport_init(struct swjdp_common *swjdp)
 	 * Should it?  If the ROM address is valid, is this the right
 	 * place to scan the table and do any topology detection?
 	 */
-	dap_ap_read_reg_u32(swjdp, AP_REG_IDR, &idreg);
-	dap_ap_read_reg_u32(swjdp, AP_REG_BASE, &romaddr);
+	retval = dap_queue_ap_read(swjdp, AP_REG_IDR, &idreg);
+	retval = dap_queue_ap_read(swjdp, AP_REG_BASE, &romaddr);
 
 	LOG_DEBUG("MEM-AP #%d ID Register 0x%" PRIx32
 		", Debug ROM Address 0x%" PRIx32,
@@ -1220,7 +1355,7 @@ is_dap_cid_ok(uint32_t cid3, uint32_t cid2, uint32_t cid1, uint32_t cid0)
 int dap_info_command(struct command_context *cmd_ctx,
 		struct swjdp_common *swjdp, int apsel)
 {
-
+	int retval;
 	uint32_t dbgbase, apid;
 	int romtable_present = 0;
 	uint8_t mem_ap;
@@ -1232,9 +1367,12 @@ int dap_info_command(struct command_context *cmd_ctx,
 
 	apselold = swjdp->apsel;
 	dap_ap_select(swjdp, apsel);
-	dap_ap_read_reg_u32(swjdp, AP_REG_BASE, &dbgbase);
-	dap_ap_read_reg_u32(swjdp, AP_REG_IDR, &apid);
-	jtagdp_transaction_endcheck(swjdp);
+	retval = dap_queue_ap_read(swjdp, AP_REG_BASE, &dbgbase);
+	retval = dap_queue_ap_read(swjdp, AP_REG_IDR, &apid);
+	retval = dap_run(swjdp);
+	if (retval != ERROR_OK)
+		return retval;
+
 	/* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
 	mem_ap = ((apid&0x10000) && ((apid&0x0F) != 0));
 	command_print(cmd_ctx, "AP ID register 0x%8.8" PRIx32, apid);
@@ -1286,7 +1424,10 @@ int dap_info_command(struct command_context *cmd_ctx,
 		mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFF8, &cid2);
 		mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFFC, &cid3);
 		mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000) | 0xFCC, &memtype);
-		jtagdp_transaction_endcheck(swjdp);
+		retval = dap_run(swjdp);
+		if (retval != ERROR_OK)
+			return retval;
+
 		if (!is_dap_cid_ok(cid3, cid2, cid1, cid0))
 			command_print(cmd_ctx, "\tCID3 0x%2.2" PRIx32
 					", CID2 0x%2.2" PRIx32
@@ -1503,10 +1644,6 @@ int dap_info_command(struct command_context *cmd_ctx,
 					type = "TI DAPCTL";
 					full = "";
 					break;
-				case 0x4e0:
-					type = "Cortex-M3 ETM";
-					full = "(Embedded Trace)";
-					break;
 				case 0x906:
 					type = "Coresight CTI";
 					full = "(Cross Trigger)";
@@ -1539,6 +1676,10 @@ int dap_info_command(struct command_context *cmd_ctx,
 					type = "Cortex-M3 TPIU";
 					full = "(Trace Port Interface Unit)";
 					break;
+				case 0x924:
+					type = "Cortex-M3 ETM";
+					full = "(Embedded Trace)";
+					break;
 				case 0xc08:
 					type = "Cortex-A8 Debug";
 					full = "(Debug Unit)";
@@ -1598,8 +1739,11 @@ DAP_COMMAND_HANDLER(dap_baseaddr_command)
 	 * though they're not common for now.  This should
 	 * use the ID register to verify it's a MEM-AP.
 	 */
-	dap_ap_read_reg_u32(swjdp, AP_REG_BASE, &baseaddr);
-	retval = jtagdp_transaction_endcheck(swjdp);
+	retval = dap_queue_ap_read(swjdp, AP_REG_BASE, &baseaddr);
+	retval = dap_run(swjdp);
+	if (retval != ERROR_OK)
+		return retval;
+
 	command_print(CMD_CTX, "0x%8.8" PRIx32, baseaddr);
 
 	if (apselsave != apsel)
@@ -1650,8 +1794,11 @@ DAP_COMMAND_HANDLER(dap_apsel_command)
 	}
 
 	dap_ap_select(swjdp, apsel);
-	dap_ap_read_reg_u32(swjdp, AP_REG_IDR, &apid);
-	retval = jtagdp_transaction_endcheck(swjdp);
+	retval = dap_queue_ap_read(swjdp, AP_REG_IDR, &apid);
+	retval = dap_run(swjdp);
+	if (retval != ERROR_OK)
+		return retval;
+
 	command_print(CMD_CTX, "ap %" PRIi32 " selected, identification register 0x%8.8" PRIx32,
 			apsel, apid);
 
@@ -1681,11 +1828,127 @@ DAP_COMMAND_HANDLER(dap_apid_command)
 	if (apselsave != apsel)
 		dap_ap_select(swjdp, apsel);
 
-	dap_ap_read_reg_u32(swjdp, AP_REG_IDR, &apid);
-	retval = jtagdp_transaction_endcheck(swjdp);
+	retval = dap_queue_ap_read(swjdp, AP_REG_IDR, &apid);
+	retval = dap_run(swjdp);
+	if (retval != ERROR_OK)
+		return retval;
+
 	command_print(CMD_CTX, "0x%8.8" PRIx32, apid);
 	if (apselsave != apsel)
 		dap_ap_select(swjdp, apselsave);
 
 	return retval;
 }
+
+/*
+ * This represents the bits which must be sent out on TMS/SWDIO to
+ * switch a DAP implemented using an SWJ-DP module into SWD mode.
+ * These bits are stored (and transmitted) LSB-first.
+ *
+ * See the DAP-Lite specification, section 2.2.5 for information
+ * about making the debug link select SWD or JTAG.  (Similar info
+ * is in a few other ARM documents.)
+ */
+static const uint8_t jtag2swd_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 sequence enables SWD and disables JTAG
+	 * NOTE: bits in the DP's IDCODE may expose the need for
+	 * an old/deprecated sequence (0xb6 0xed).
+	 */
+	0x9e, 0xe7,
+	/* 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,
+};
+
+/**
+ * Put the debug link into SWD mode, if the target supports it.
+ * The link's initial mode may be either JTAG (for example,
+ * with SWJ-DP after reset) or SWD.
+ *
+ * @param target Enters SWD mode (if possible).
+ *
+ * Note that targets using the JTAG-DP do not support SWD, and that
+ * some targets which could otherwise support it may have have been
+ * configured to disable SWD signaling
+ *
+ * @return ERROR_OK or else a fault code.
+ */
+int dap_to_swd(struct target *target)
+{
+	int retval;
+
+	LOG_DEBUG("Enter SWD 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(jtag2swd_bitseq),
+			jtag2swd_bitseq, TAP_INVALID);
+	if (retval == ERROR_OK)
+		retval = jtag_execute_queue();
+
+	/* REVISIT set up the DAP's ops vector for SWD mode. */
+
+	return retval;
+}
+
+/**
+ * This represents the bits which must be sent out on TMS/SWDIO to
+ * switch a DAP implemented using an SWJ-DP module into JTAG mode.
+ * These bits are stored (and transmitted) LSB-first.
+ *
+ * These bits are stored (and transmitted) LSB-first.
+ */
+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;
+}