X-Git-Url: https://review.openocd.org/gitweb?a=blobdiff_plain;f=src%2Ftarget%2Farm_adi_v5.c;h=9ea7b5a145ea7f27b8ac8734e5d0cbeb6c24ffc5;hb=566faa17de575ff58beffbc4a8961bf3f8199b93;hp=4950121afaad36fc87d1291261b5bc023d26355d;hpb=0649fb2f6c7e1bea138769ecc2ec8dc17ae98044;p=openocd.git

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 4950121afa..9ea7b5a145 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;
@@ -343,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;
@@ -424,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;
 
@@ -456,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;
@@ -532,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;
 
@@ -882,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;
+}
+
 /*--------------------------------------------------------------------------*/
 
 
@@ -906,15 +1158,18 @@ extern const struct dap_ops jtag_dp_ops;
  */
 int ahbap_debugport_init(struct adiv5_dap *dap)
 {
-	uint32_t 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.
 	 *
@@ -922,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;
 
@@ -935,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;
 
@@ -973,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 */
@@ -981,10 +1236,12 @@ 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;
 
+	dap_syssec(dap);
+
 	return ERROR_OK;
 }
 
@@ -1006,29 +1263,19 @@ is_dap_cid_ok(uint32_t cid3, uint32_t cid2, uint32_t cid1, uint32_t cid0)
 			&& ((cid1 & 0x0f) == 0) && cid0 == 0x0d;
 }
 
-struct broken_cpu {
-	uint32_t	dbgbase;
-	uint32_t	apid;
-	uint32_t	correct_dbgbase;
-	char		*model;
-} broken_cpus[] = {
-	{ 0x80000000, 0x04770002, 0x60000000, "imx51" },
-};
-
-int dap_get_debugbase(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 apselold;
+	uint32_t ap_old;
 	int retval;
-	unsigned int i;
 	uint32_t dbgbase, apid;
 
 	/* 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)
@@ -1040,19 +1287,17 @@ int dap_get_debugbase(struct adiv5_dap *dap, int apsel,
 	if (retval != ERROR_OK)
 		return retval;
 
-	/* Some CPUs are messed up, so fixup if needed. */
-	for (i = 0; i < sizeof(broken_cpus)/sizeof(struct broken_cpu); i++)
-		if (broken_cpus[i].dbgbase == dbgbase &&
-			broken_cpus[i].apid == apid) {
-			LOG_WARNING("Found broken CPU (%s), trying to fixup "
-				"ROM Table location from 0x%08x to 0x%08x",
-				broken_cpus[i].model, dbgbase,
-				broken_cpus[i].correct_dbgbase);
-			dbgbase = broken_cpus[i].correct_dbgbase;
+	/* 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, apselold);
+	dap_ap_select(dap, ap_old);
 
 	/* The asignment happens only here to prevent modification of these
 	 * values before they are certain. */
@@ -1062,21 +1307,62 @@ int dap_get_debugbase(struct adiv5_dap *dap, int apsel,
 	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 apsel)
+		struct adiv5_dap *dap, int ap)
 {
 	int retval;
-	uint32_t dbgbase, apid;
+	uint32_t dbgbase = 0, apid = 0; /* Silence gcc by initializing */
 	int romtable_present = 0;
 	uint8_t mem_ap;
-	uint32_t apselold;
+	uint32_t ap_old;
 
-	retval = dap_get_debugbase(dap, apsel, &dbgbase, &apid);
+	retval = dap_get_debugbase(dap, ap, &dbgbase, &apid);
 	if (retval != ERROR_OK)
 		return retval;
 
-	apselold = dap->apsel;
-	dap_ap_select(dap, apsel);
+	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));
@@ -1108,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));
@@ -1368,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;
@@ -1439,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)";
@@ -1465,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;
 }
@@ -1497,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;
@@ -1515,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,
@@ -1532,9 +1821,6 @@ COMMAND_HANDLER(dap_baseaddr_command)
 
 	command_print(CMD_CTX, "0x%8.8" PRIx32, baseaddr);
 
-	if (apselsave != apsel)
-		dap_ap_select(dap, apselsave);
-
 	return retval;
 }
 
@@ -1587,7 +1873,9 @@ 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;
@@ -1607,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;
@@ -1625,8 +1912,7 @@ 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)
@@ -1636,8 +1922,6 @@ COMMAND_HANDLER(dap_apid_command)
 		return retval;
 
 	command_print(CMD_CTX, "0x%8.8" PRIx32, apid);
-	if (apselsave != apsel)
-		dap_ap_select(dap, apselsave);
 
 	return retval;
 }