X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Ftarget%2Farm_adi_v5.c;h=7f89f2eed9d1ad1d947b385dbf55c62b615f0ec6;hb=40c425e4005b6c00cab3ff2cd620cd3d6ae66514;hp=9a33d820131500b7d626d81d6911436f2eee2d91;hpb=e7a1ec64bface364305e9c355811838e9f310bf3;p=fw%2Fopenocd

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 9a33d8201..7f89f2eed 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -100,13 +100,13 @@ static uint32_t max_tar_block_size(uint32_t tar_autoincr_block, uint32_t address
  * @param apsel Number of the AP to (implicitly) use with further
  *	transactions.  This normally identifies a MEM-AP.
  */
-void dap_ap_select(struct adiv5_dap *dap,uint8_t apsel)
+void dap_ap_select(struct adiv5_dap *dap,uint8_t ap)
 {
-	uint32_t select_apsel = (apsel << 24) & 0xFF000000;
+	uint32_t new_ap = (ap << 24) & 0xFF000000;
 
-	if (select_apsel != dap->apsel)
+	if (new_ap != dap->ap_current)
 	{
-		dap->apsel = select_apsel;
+		dap->ap_current = new_ap;
 		/* Switching AP invalidates cached values.
 		 * Values MUST BE UPDATED BEFORE AP ACCESS.
 		 */
@@ -270,11 +270,11 @@ int mem_ap_write_atomic_u32(struct adiv5_dap *dap, uint32_t address,
 * Write a buffer in target order (little endian)                             *
 *                                                                            *
 *****************************************************************************/
-int mem_ap_write_buf_u32(struct adiv5_dap *dap, uint8_t *buffer, int count, uint32_t address)
+int mem_ap_write_buf_u32(struct adiv5_dap *dap, const uint8_t *buffer, int count, uint32_t address)
 {
 	int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK;
 	uint32_t adr = address;
-	uint8_t* pBuffer = buffer;
+	const uint8_t* pBuffer = buffer;
 
 	count >>= 2;
 	wcount = count;
@@ -316,12 +316,12 @@ int mem_ap_write_buf_u32(struct adiv5_dap *dap, uint8_t *buffer, int count, uint
 		for (writecount = 0; writecount < blocksize; writecount++)
 		{
 			retval = dap_queue_ap_write(dap, AP_REG_DRW,
-				*(uint32_t *) (buffer + 4 * writecount));
+				*(uint32_t *) ((void *) (buffer + 4 * writecount)));
 			if (retval != ERROR_OK)
 				break;
 		}
 
-		if (dap_run(dap) == ERROR_OK)
+		if ((retval = dap_run(dap)) == ERROR_OK)
 		{
 			wcount = wcount - blocksize;
 			address = address + 4 * blocksize;
@@ -335,8 +335,7 @@ int mem_ap_write_buf_u32(struct adiv5_dap *dap, uint8_t *buffer, int count, uint
 		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;
+			return retval;
 		}
 	}
 
@@ -344,7 +343,7 @@ int mem_ap_write_buf_u32(struct adiv5_dap *dap, uint8_t *buffer, int count, uint
 }
 
 static int mem_ap_write_buf_packed_u16(struct adiv5_dap *dap,
-		uint8_t *buffer, int count, uint32_t address)
+		const uint8_t *buffer, int count, uint32_t address)
 {
 	int retval = ERROR_OK;
 	int wcount, blocksize, writecount, i;
@@ -376,13 +375,14 @@ static int mem_ap_write_buf_packed_u16(struct adiv5_dap *dap,
 
 			if (nbytes < 4)
 			{
-				if (mem_ap_write_buf_u16(dap, buffer,
-						nbytes, address) != ERROR_OK)
+				retval = mem_ap_write_buf_u16(dap, buffer,
+						nbytes, address);
+				if (retval != ERROR_OK)
 				{
 					LOG_WARNING("Block write error address "
 						"0x%" PRIx32 ", count 0x%x",
 						address, count);
-					return ERROR_JTAG_DEVICE_ERROR;
+					return retval;
 				}
 
 				address += nbytes >> 1;
@@ -405,13 +405,12 @@ static int mem_ap_write_buf_packed_u16(struct adiv5_dap *dap,
 				if (retval != ERROR_OK)
 					break;
 
-				if (dap_run(dap) != ERROR_OK)
+				if ((retval = dap_run(dap)) != 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;
+					return retval;
 				}
 			}
 
@@ -425,7 +424,7 @@ static int mem_ap_write_buf_packed_u16(struct adiv5_dap *dap,
 	return retval;
 }
 
-int mem_ap_write_buf_u16(struct adiv5_dap *dap, uint8_t *buffer, int count, uint32_t address)
+int mem_ap_write_buf_u16(struct adiv5_dap *dap, const uint8_t *buffer, int count, uint32_t address)
 {
 	int retval = ERROR_OK;
 
@@ -457,7 +456,7 @@ int mem_ap_write_buf_u16(struct adiv5_dap *dap, uint8_t *buffer, int count, uint
 }
 
 static int mem_ap_write_buf_packed_u8(struct adiv5_dap *dap,
-		uint8_t *buffer, int count, uint32_t address)
+		const uint8_t *buffer, int count, uint32_t address)
 {
 	int retval = ERROR_OK;
 	int wcount, blocksize, writecount, i;
@@ -485,12 +484,13 @@ static int mem_ap_write_buf_packed_u8(struct adiv5_dap *dap,
 
 			if (nbytes < 4)
 			{
-				if (mem_ap_write_buf_u8(dap, buffer, nbytes, address) != ERROR_OK)
+				retval = mem_ap_write_buf_u8(dap, buffer, nbytes, address);
+				if (retval != ERROR_OK)
 				{
 					LOG_WARNING("Block write error address "
 						"0x%" PRIx32 ", count 0x%x",
 						address, count);
-					return ERROR_JTAG_DEVICE_ERROR;
+					return retval;
 				}
 
 				address += nbytes;
@@ -513,13 +513,12 @@ static int mem_ap_write_buf_packed_u8(struct adiv5_dap *dap,
 				if (retval != ERROR_OK)
 					break;
 
-				if (dap_run(dap) != ERROR_OK)
+				if ((retval = dap_run(dap)) != 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;
+					return retval;
 				}
 			}
 
@@ -533,7 +532,7 @@ static int mem_ap_write_buf_packed_u8(struct adiv5_dap *dap,
 	return retval;
 }
 
-int mem_ap_write_buf_u8(struct adiv5_dap *dap, uint8_t *buffer, int count, uint32_t address)
+int mem_ap_write_buf_u8(struct adiv5_dap *dap, const uint8_t *buffer, int count, uint32_t address)
 {
 	int retval = ERROR_OK;
 
@@ -710,11 +709,12 @@ static int mem_ap_read_buf_packed_u16(struct adiv5_dap *dap,
 		do
 		{
 			retval = dap_queue_ap_read(dap, AP_REG_DRW, &invalue);
-			if (dap_run(dap) != ERROR_OK)
+			if (retval != ERROR_OK)
+				return retval;
+			if ((retval = dap_run(dap)) != 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;
+				return retval;
 			}
 
 			nbytes = MIN((readcount << 1), 4);
@@ -819,11 +819,12 @@ static int mem_ap_read_buf_packed_u8(struct adiv5_dap *dap,
 		do
 		{
 			retval = dap_queue_ap_read(dap, AP_REG_DRW, &invalue);
-			if (dap_run(dap) != ERROR_OK)
+			if (retval != ERROR_OK)
+				return retval;
+			if ((retval = dap_run(dap)) != 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;
+				return retval;
 			}
 
 			nbytes = MIN(readcount, 4);
@@ -881,6 +882,258 @@ int mem_ap_read_buf_u8(struct adiv5_dap *dap, uint8_t *buffer,
 	return retval;
 }
 
+/*--------------------------------------------------------------------*/
+/*          Wrapping function with selection of AP                    */
+/*--------------------------------------------------------------------*/
+int mem_ap_sel_read_u32(struct adiv5_dap *swjdp, uint8_t ap,
+		uint32_t address, uint32_t *value)
+{
+	dap_ap_select(swjdp, ap);
+	return mem_ap_read_u32(swjdp, address, value);
+}
+
+int mem_ap_sel_write_u32(struct adiv5_dap *swjdp, uint8_t ap,
+		uint32_t address, uint32_t value)
+{
+	dap_ap_select(swjdp, ap);
+	return mem_ap_write_u32(swjdp, address, value);
+}
+
+int mem_ap_sel_read_atomic_u32(struct adiv5_dap *swjdp, uint8_t ap,
+		uint32_t address, uint32_t *value)
+{
+	dap_ap_select(swjdp, ap);
+	return mem_ap_read_atomic_u32(swjdp, address, value);
+}
+
+int mem_ap_sel_write_atomic_u32(struct adiv5_dap *swjdp, uint8_t ap,
+		uint32_t address, uint32_t value)
+{
+	dap_ap_select(swjdp, ap);
+	return mem_ap_write_atomic_u32(swjdp, address, value);
+}
+
+int mem_ap_sel_read_buf_u8(struct adiv5_dap *swjdp, uint8_t ap,
+		uint8_t *buffer, int count, uint32_t address)
+{
+	dap_ap_select(swjdp, ap);
+	return mem_ap_read_buf_u8(swjdp, buffer, count, address);
+}
+
+int mem_ap_sel_read_buf_u16(struct adiv5_dap *swjdp, uint8_t ap,
+		uint8_t *buffer, int count, uint32_t address)
+{
+	dap_ap_select(swjdp, ap);
+	return mem_ap_read_buf_u16(swjdp, buffer, count, address);
+}
+
+int mem_ap_sel_read_buf_u32(struct adiv5_dap *swjdp, uint8_t ap,
+		uint8_t *buffer, int count, uint32_t address)
+{
+	dap_ap_select(swjdp, ap);
+	return mem_ap_read_buf_u32(swjdp, buffer, count, address);
+}
+
+int mem_ap_sel_write_buf_u8(struct adiv5_dap *swjdp, uint8_t ap,
+		const uint8_t *buffer, int count, uint32_t address)
+{
+	dap_ap_select(swjdp, ap);
+	return mem_ap_write_buf_u8(swjdp, buffer, count, address);
+}
+
+int mem_ap_sel_write_buf_u16(struct adiv5_dap *swjdp, uint8_t ap,
+		const uint8_t *buffer, int count, uint32_t address)
+{
+	dap_ap_select(swjdp, ap);
+	return mem_ap_write_buf_u16(swjdp, buffer, count, address);
+}
+
+int mem_ap_sel_write_buf_u32(struct adiv5_dap *swjdp, uint8_t ap,
+		const uint8_t *buffer, int count, uint32_t address)
+{
+	dap_ap_select(swjdp, ap);
+	return mem_ap_write_buf_u32(swjdp, buffer, count, address);
+}
+
+#define MDM_REG_STAT		0x00
+#define MDM_REG_CTRL		0x04
+#define MDM_REG_ID		0xfc
+
+#define MDM_STAT_FMEACK		(1<<0)
+#define MDM_STAT_FREADY		(1<<1)
+#define MDM_STAT_SYSSEC		(1<<2)
+#define MDM_STAT_SYSRES		(1<<3)
+#define MDM_STAT_FMEEN		(1<<5)
+#define MDM_STAT_BACKDOOREN	(1<<6)
+#define MDM_STAT_LPEN		(1<<7)
+#define MDM_STAT_VLPEN		(1<<8)
+#define MDM_STAT_LLSMODEXIT	(1<<9)
+#define MDM_STAT_VLLSXMODEXIT	(1<<10)
+#define MDM_STAT_CORE_HALTED	(1<<16)
+#define MDM_STAT_CORE_SLEEPDEEP	(1<<17)
+#define MDM_STAT_CORESLEEPING	(1<<18)
+
+#define MEM_CTRL_FMEIP		(1<<0)
+#define MEM_CTRL_DBG_DIS	(1<<1)
+#define MEM_CTRL_DBG_REQ	(1<<2)
+#define MEM_CTRL_SYS_RES_REQ	(1<<3)
+#define MEM_CTRL_CORE_HOLD_RES	(1<<4)
+#define MEM_CTRL_VLLSX_DBG_REQ	(1<<5)
+#define MEM_CTRL_VLLSX_DBG_ACK	(1<<6)
+#define MEM_CTRL_VLLSX_STAT_ACK	(1<<7)
+
+/**
+ *
+ */
+int dap_syssec_kinetis_mdmap(struct adiv5_dap *dap)
+{
+	uint32_t val;
+	int retval;
+	enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+	dap_ap_select(dap, 1);
+
+	/* first check mdm-ap id register */
+	retval = dap_queue_ap_read(dap, MDM_REG_ID, &val);
+	if (retval != ERROR_OK)
+		return retval;
+	dap_run(dap);
+
+	if ( val != 0x001C0000 )
+	{
+		LOG_DEBUG("id doesn't match %08X != 0x001C0000",val);
+		dap_ap_select(dap, 0);
+		return ERROR_FAIL;
+	}
+
+	/* read and parse status register
+	 * it's important that the device is out of
+	 * reset here
+	 */
+	retval = dap_queue_ap_read(dap, MDM_REG_STAT, &val);
+	if (retval != ERROR_OK)
+		return retval;
+	dap_run(dap);
+
+	LOG_DEBUG("MDM_REG_STAT %08X",val);
+
+	if ( (val & (MDM_STAT_SYSSEC|MDM_STAT_FREADY)) != (MDM_STAT_FREADY) )
+	{
+		LOG_DEBUG("MDMAP: system is secured, masserase needed");
+
+		if ( !(val & MDM_STAT_FMEEN) )
+		{
+			LOG_DEBUG("MDMAP: masserase is disabled");
+		}
+		else
+		{
+			/* we need to assert reset */
+			if ( jtag_reset_config & RESET_HAS_SRST )
+			{
+				/* default to asserting srst */
+				if (jtag_reset_config & RESET_SRST_PULLS_TRST)
+				{
+					jtag_add_reset(1, 1);
+				}
+				else
+				{
+					jtag_add_reset(0, 1);
+				}
+			}
+			else
+			{
+				LOG_DEBUG("SRST not configured");
+				dap_ap_select(dap, 0);
+				return ERROR_FAIL;
+			}
+
+			while(1)
+			{
+				retval = dap_queue_ap_write(dap, MDM_REG_CTRL, MEM_CTRL_FMEIP);
+				if (retval != ERROR_OK)
+					return retval;
+				dap_run(dap);
+				/* read status register and wait for ready */
+				retval = dap_queue_ap_read(dap, MDM_REG_STAT, &val);
+				if (retval != ERROR_OK)
+					return retval;
+				dap_run(dap);
+				LOG_DEBUG("MDM_REG_STAT %08X",val);
+
+				if ( (val&1))
+					break;
+			}
+
+			while(1)
+			{
+				retval = dap_queue_ap_write(dap, MDM_REG_CTRL, 0);
+				if (retval != ERROR_OK)
+					return retval;
+				dap_run(dap);
+				/* read status register */
+				retval = dap_queue_ap_read(dap, MDM_REG_STAT, &val);
+				if (retval != ERROR_OK)
+					return retval;
+				dap_run(dap);
+				LOG_DEBUG("MDM_REG_STAT %08X",val);
+				/* read control register and wait for ready */
+				retval = dap_queue_ap_read(dap, MDM_REG_CTRL, &val);
+				if (retval != ERROR_OK)
+					return retval;
+				dap_run(dap);
+				LOG_DEBUG("MDM_REG_CTRL %08X",val);
+
+				if ( val == 0x00 )
+					break;
+			}
+		}
+	}
+
+	dap_ap_select(dap, 0);
+
+	return ERROR_OK;
+}
+
+/** */
+struct dap_syssec_filter {
+	/** */
+	uint32_t idcode;
+	/** */
+	int (*dap_init)(struct adiv5_dap *dap);
+};
+
+/** */
+static struct dap_syssec_filter dap_syssec_filter_data[] = {
+	{ 0x4BA00477, dap_syssec_kinetis_mdmap }
+};
+
+
+/**
+ *
+ */
+int dap_syssec(struct adiv5_dap *dap)
+{
+	unsigned int i;
+	struct jtag_tap *tap;
+
+	for(i=0;i<sizeof(dap_syssec_filter_data);i++)
+	{
+		tap = dap->jtag_info->tap;
+
+		while (tap != NULL)
+		{
+			if ( tap->hasidcode && (dap_syssec_filter_data[i].idcode == tap->idcode) )
+			{
+				LOG_DEBUG("DAP: mdmap_init for idcode: %08x",tap->idcode);
+				dap_syssec_filter_data[i].dap_init(dap);
+			}
+			tap = tap->next_tap;
+		}
+	}
+
+	return ERROR_OK;
+}
+
 /*--------------------------------------------------------------------------*/
 
 
@@ -905,15 +1158,18 @@ extern const struct dap_ops jtag_dp_ops;
  */
 int ahbap_debugport_init(struct adiv5_dap *dap)
 {
-	uint32_t idreg, romaddr, dummy;
 	uint32_t ctrlstat;
 	int cnt = 0;
 	int retval;
 
 	LOG_DEBUG(" ");
 
-	/* JTAG-DP or SWJ-DP, in JTAG mode */
-	dap->ops = &jtag_dp_ops;
+	/* JTAG-DP or SWJ-DP, in JTAG mode
+	 * ... for SWD mode this is patched as part
+	 * of link switchover
+	 */
+	if (!dap->ops)
+		dap->ops = &jtag_dp_ops;
 
 	/* Default MEM-AP setup.
 	 *
@@ -921,12 +1177,12 @@ int ahbap_debugport_init(struct adiv5_dap *dap)
 	 * Should we probe, or take a hint from the caller?
 	 * Presumably we can ignore the possibility of multiple APs.
 	 */
-	dap->apsel = !0;
+	dap->ap_current = !0;
 	dap_ap_select(dap, 0);
 
 	/* DP initialization */
 
-	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, &dummy);
+	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -934,7 +1190,7 @@ int ahbap_debugport_init(struct adiv5_dap *dap)
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, &dummy);
+	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -972,7 +1228,7 @@ int ahbap_debugport_init(struct adiv5_dap *dap)
 		alive_sleep(10);
 	}
 
-	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, &dummy);
+	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
 	if (retval != ERROR_OK)
 		return retval;
 	/* With debug power on we can activate OVERRUN checking */
@@ -980,25 +1236,11 @@ int ahbap_debugport_init(struct adiv5_dap *dap)
 	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat);
 	if (retval != ERROR_OK)
 		return retval;
-	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, &dummy);
+	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
 	if (retval != ERROR_OK)
 		return retval;
 
-	/*
-	 * REVISIT this isn't actually *initializing* anything in an AP,
-	 * and doesn't care if it's a MEM-AP at all (much less AHB-AP).
-	 * Should it?  If the ROM address is valid, is this the right
-	 * place to scan the table and do any topology detection?
-	 */
-	retval = dap_queue_ap_read(dap, AP_REG_IDR, &idreg);
-	retval = dap_queue_ap_read(dap, AP_REG_BASE, &romaddr);
-
-	if ((retval = dap_run(dap)) != ERROR_OK)
-		return retval;
-
-	LOG_DEBUG("MEM-AP #%" PRId32 " ID Register 0x%" PRIx32
-		", Debug ROM Address 0x%" PRIx32,
-		dap->apsel, idreg, romaddr);
+	dap_syssec(dap);
 
 	return ERROR_OK;
 }
@@ -1021,27 +1263,107 @@ is_dap_cid_ok(uint32_t cid3, uint32_t cid2, uint32_t cid1, uint32_t cid0)
 			&& ((cid1 & 0x0f) == 0) && cid0 == 0x0d;
 }
 
-static int dap_info_command(struct command_context *cmd_ctx,
-		struct adiv5_dap *dap, int apsel)
+int dap_get_debugbase(struct adiv5_dap *dap, int ap,
+			uint32_t *out_dbgbase, uint32_t *out_apid)
 {
+	uint32_t ap_old;
 	int retval;
 	uint32_t dbgbase, apid;
-	int romtable_present = 0;
-	uint8_t mem_ap;
-	uint32_t apselold;
 
 	/* AP address is in bits 31:24 of DP_SELECT */
-	if (apsel >= 256)
+	if (ap >= 256)
 		return ERROR_INVALID_ARGUMENTS;
 
-	apselold = dap->apsel;
-	dap_ap_select(dap, apsel);
+	ap_old = dap->ap_current;
+	dap_ap_select(dap, ap);
+
 	retval = dap_queue_ap_read(dap, AP_REG_BASE, &dbgbase);
+	if (retval != ERROR_OK)
+		return retval;
 	retval = dap_queue_ap_read(dap, AP_REG_IDR, &apid);
+	if (retval != ERROR_OK)
+		return retval;
 	retval = dap_run(dap);
 	if (retval != ERROR_OK)
 		return retval;
 
+	/* Excavate the device ID code */
+	struct jtag_tap *tap = dap->jtag_info->tap;
+	while (tap != NULL) {
+		if (tap->hasidcode)
+			break;
+		tap = tap->next_tap;
+	}
+	if (tap == NULL || !tap->hasidcode)
+		return ERROR_OK;
+
+	dap_ap_select(dap, ap_old);
+
+	/* The asignment happens only here to prevent modification of these
+	 * values before they are certain. */
+	*out_dbgbase = dbgbase;
+	*out_apid = apid;
+
+	return ERROR_OK;
+}
+
+int dap_lookup_cs_component(struct adiv5_dap *dap, int ap,
+			uint32_t dbgbase, uint8_t type, uint32_t *addr)
+{
+	uint32_t ap_old;
+	uint32_t romentry, entry_offset = 0, component_base, devtype;
+	int retval = ERROR_FAIL;
+
+	if (ap >= 256)
+		return ERROR_INVALID_ARGUMENTS;
+
+	ap_old = dap->ap_current;
+	dap_ap_select(dap, ap);
+
+	do
+	{
+		retval = mem_ap_read_atomic_u32(dap, (dbgbase&0xFFFFF000) |
+						entry_offset, &romentry);
+		if (retval != ERROR_OK)
+			return retval;
+
+		component_base = (dbgbase & 0xFFFFF000)
+			+ (romentry & 0xFFFFF000);
+
+		if (romentry & 0x1) {
+			retval = mem_ap_read_atomic_u32(dap,
+					(component_base & 0xfffff000) | 0xfcc,
+					&devtype);
+			if ((devtype & 0xff) == type) {
+				*addr = component_base;
+				retval = ERROR_OK;
+				break;
+			}
+		}
+		entry_offset += 4;
+	} while (romentry > 0);
+
+	dap_ap_select(dap, ap_old);
+
+	return retval;
+}
+
+static int dap_info_command(struct command_context *cmd_ctx,
+		struct adiv5_dap *dap, int ap)
+{
+	int retval;
+	uint32_t dbgbase, apid;
+	int romtable_present = 0;
+	uint8_t mem_ap;
+	uint32_t ap_old;
+
+	retval = dap_get_debugbase(dap, ap, &dbgbase, &apid);
+	if (retval != ERROR_OK)
+		return retval;
+
+	ap_old = dap->ap_current;
+	dap_ap_select(dap, ap);
+
 	/* 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);
@@ -1072,7 +1394,7 @@ static int dap_info_command(struct command_context *cmd_ctx,
 	}
 	else
 	{
-		command_print(cmd_ctx, "No AP found at this apsel 0x%x", apsel);
+		command_print(cmd_ctx, "No AP found at this ap 0x%x", ap);
 	}
 
 	romtable_present = ((mem_ap) && (dbgbase != 0xFFFFFFFF));
@@ -1088,11 +1410,21 @@ static int dap_info_command(struct command_context *cmd_ctx,
 			command_print(cmd_ctx, "\tROM table in legacy format");
 
 		/* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
-		mem_ap_read_u32(dap, (dbgbase&0xFFFFF000) | 0xFF0, &cid0);
-		mem_ap_read_u32(dap, (dbgbase&0xFFFFF000) | 0xFF4, &cid1);
-		mem_ap_read_u32(dap, (dbgbase&0xFFFFF000) | 0xFF8, &cid2);
-		mem_ap_read_u32(dap, (dbgbase&0xFFFFF000) | 0xFFC, &cid3);
-		mem_ap_read_u32(dap, (dbgbase&0xFFFFF000) | 0xFCC, &memtype);
+		retval = mem_ap_read_u32(dap, (dbgbase&0xFFFFF000) | 0xFF0, &cid0);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = mem_ap_read_u32(dap, (dbgbase&0xFFFFF000) | 0xFF4, &cid1);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = mem_ap_read_u32(dap, (dbgbase&0xFFFFF000) | 0xFF8, &cid2);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = mem_ap_read_u32(dap, (dbgbase&0xFFFFF000) | 0xFFC, &cid3);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = mem_ap_read_u32(dap, (dbgbase&0xFFFFF000) | 0xFCC, &memtype);
+		if (retval != ERROR_OK)
+			return retval;
 		retval = dap_run(dap);
 		if (retval != ERROR_OK)
 			return retval;
@@ -1114,7 +1446,9 @@ static int dap_info_command(struct command_context *cmd_ctx,
 		entry_offset = 0;
 		do
 		{
-			mem_ap_read_atomic_u32(dap, (dbgbase&0xFFFFF000) | entry_offset, &romentry);
+			retval = mem_ap_read_atomic_u32(dap, (dbgbase&0xFFFFF000) | entry_offset, &romentry);
+			if (retval != ERROR_OK)
+				return retval;
 			command_print(cmd_ctx, "\tROMTABLE[0x%x] = 0x%" PRIx32 "",entry_offset,romentry);
 			if (romentry&0x01)
 			{
@@ -1130,33 +1464,51 @@ static int dap_info_command(struct command_context *cmd_ctx,
 				/* IDs are in last 4K section */
 
 
-				mem_ap_read_atomic_u32(dap,
+				retval = mem_ap_read_atomic_u32(dap,
 					component_base + 0xFE0, &c_pid0);
+				if (retval != ERROR_OK)
+					return retval;
 				c_pid0 &= 0xff;
-				mem_ap_read_atomic_u32(dap,
+				retval = mem_ap_read_atomic_u32(dap,
 					component_base + 0xFE4, &c_pid1);
+				if (retval != ERROR_OK)
+					return retval;
 				c_pid1 &= 0xff;
-				mem_ap_read_atomic_u32(dap,
+				retval = mem_ap_read_atomic_u32(dap,
 					component_base + 0xFE8, &c_pid2);
+				if (retval != ERROR_OK)
+					return retval;
 				c_pid2 &= 0xff;
-				mem_ap_read_atomic_u32(dap,
+				retval = mem_ap_read_atomic_u32(dap,
 					component_base + 0xFEC, &c_pid3);
+				if (retval != ERROR_OK)
+					return retval;
 				c_pid3 &= 0xff;
-				mem_ap_read_atomic_u32(dap,
+				retval = mem_ap_read_atomic_u32(dap,
 					component_base + 0xFD0, &c_pid4);
+				if (retval != ERROR_OK)
+					return retval;
 				c_pid4 &= 0xff;
 
-				mem_ap_read_atomic_u32(dap,
+				retval = mem_ap_read_atomic_u32(dap,
 					component_base + 0xFF0, &c_cid0);
+				if (retval != ERROR_OK)
+					return retval;
 				c_cid0 &= 0xff;
-				mem_ap_read_atomic_u32(dap,
+				retval = mem_ap_read_atomic_u32(dap,
 					component_base + 0xFF4, &c_cid1);
+				if (retval != ERROR_OK)
+					return retval;
 				c_cid1 &= 0xff;
-				mem_ap_read_atomic_u32(dap,
+				retval = mem_ap_read_atomic_u32(dap,
 					component_base + 0xFF8, &c_cid2);
+				if (retval != ERROR_OK)
+					return retval;
 				c_cid2 &= 0xff;
-				mem_ap_read_atomic_u32(dap,
+				retval = mem_ap_read_atomic_u32(dap,
 					component_base + 0xFFC, &c_cid3);
+				if (retval != ERROR_OK)
+					return retval;
 				c_cid3 &= 0xff;
 
 
@@ -1177,9 +1529,11 @@ static int dap_info_command(struct command_context *cmd_ctx,
 					unsigned minor;
 					char *major = "Reserved", *subtype = "Reserved";
 
-					mem_ap_read_atomic_u32(dap,
+					retval = mem_ap_read_atomic_u32(dap,
 							(component_base & 0xfffff000) | 0xfcc,
 							&devtype);
+					if (retval != ERROR_OK)
+						return retval;
 					minor = (devtype >> 4) & 0x0f;
 					switch (devtype & 0x0f) {
 					case 0:
@@ -1300,8 +1654,9 @@ static int dap_info_command(struct command_context *cmd_ctx,
 
 				/* Part number interpretations are from Cortex
 				 * core specs, the CoreSight components TRM
-				 * (ARM DDI 0314H), and ETM specs; also from
-				 * chip observation (e.g. TI SDTI).
+				 * (ARM DDI 0314H), CoreSight System Design
+				 * Guide (ARM DGI 0012D) and ETM specs; also
+				 * from chip observation (e.g. TI SDTI).
 				 */
 				part_num = (c_pid0 & 0xff);
 				part_num |= (c_pid1 & 0x0f) << 8;
@@ -1371,6 +1726,10 @@ static int dap_info_command(struct command_context *cmd_ctx,
 					type = "Cortex-M3 ETM";
 					full = "(Embedded Trace)";
 					break;
+				case 0x930:
+					type = "Cortex-R4 ETM";
+					full = "(Embedded Trace)";
+					break;
 				case 0xc08:
 					type = "Cortex-A8 Debug";
 					full = "(Debug Unit)";
@@ -1397,7 +1756,7 @@ static int dap_info_command(struct command_context *cmd_ctx,
 	{
 		command_print(cmd_ctx, "\tNo ROM table present");
 	}
-	dap_ap_select(dap, apselold);
+	dap_ap_select(dap, ap_old);
 
 	return ERROR_OK;
 }
@@ -1429,10 +1788,9 @@ COMMAND_HANDLER(dap_baseaddr_command)
 	struct arm *arm = target_to_arm(target);
 	struct adiv5_dap *dap = arm->dap;
 
-	uint32_t apsel, apselsave, baseaddr;
+	uint32_t apsel, baseaddr;
 	int retval;
 
-	apselsave = dap->apsel;
 	switch (CMD_ARGC) {
 	case 0:
 		apsel = dap->apsel;
@@ -1447,8 +1805,7 @@ COMMAND_HANDLER(dap_baseaddr_command)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
-	if (apselsave != apsel)
-		dap_ap_select(dap, apsel);
+	dap_ap_select(dap, apsel);
 
 	/* NOTE:  assumes we're talking to a MEM-AP, which
 	 * has a base address.  There are other kinds of AP,
@@ -1456,15 +1813,14 @@ COMMAND_HANDLER(dap_baseaddr_command)
 	 * use the ID register to verify it's a MEM-AP.
 	 */
 	retval = dap_queue_ap_read(dap, AP_REG_BASE, &baseaddr);
+	if (retval != ERROR_OK)
+		return retval;
 	retval = dap_run(dap);
 	if (retval != ERROR_OK)
 		return retval;
 
 	command_print(CMD_CTX, "0x%8.8" PRIx32, baseaddr);
 
-	if (apselsave != apsel)
-		dap_ap_select(dap, apselsave);
-
 	return retval;
 }
 
@@ -1517,8 +1873,12 @@ COMMAND_HANDLER(dap_apsel_command)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
+	dap->apsel = apsel;
 	dap_ap_select(dap, apsel);
+
 	retval = dap_queue_ap_read(dap, AP_REG_IDR, &apid);
+	if (retval != ERROR_OK)
+		return retval;
 	retval = dap_run(dap);
 	if (retval != ERROR_OK)
 		return retval;
@@ -1535,10 +1895,9 @@ COMMAND_HANDLER(dap_apid_command)
 	struct arm *arm = target_to_arm(target);
 	struct adiv5_dap *dap = arm->dap;
 
-	uint32_t apsel, apselsave, apid;
+	uint32_t apsel, apid;
 	int retval;
 
-	apselsave = dap->apsel;
 	switch (CMD_ARGC) {
 	case 0:
 		apsel = dap->apsel;
@@ -1553,17 +1912,16 @@ COMMAND_HANDLER(dap_apid_command)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
-	if (apselsave != apsel)
-		dap_ap_select(dap, apsel);
+	dap_ap_select(dap, apsel);
 
 	retval = dap_queue_ap_read(dap, AP_REG_IDR, &apid);
+	if (retval != ERROR_OK)
+		return retval;
 	retval = dap_run(dap);
 	if (retval != ERROR_OK)
 		return retval;
 
 	command_print(CMD_CTX, "0x%8.8" PRIx32, apid);
-	if (apselsave != apsel)
-		dap_ap_select(dap, apselsave);
 
 	return retval;
 }