arm_adi_v5: make dap_lookup_cs_component() traverse subtables and handle multicore

When looking for a debug base address of a core, one should search
through all the ROM tables, not just the top-level one.

This code also assumes that the first found entry (in a depth-first
search) will correspond to core 0, the second to core 1 etc.

The patch is supposed to be an alternative implementation of
http://openocd.zylin.com/#/c/1313/.

Change-Id: Ifc88971a02fe3d9c00d9bf72a822ade5804d4e09
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/1920
Tested-by: jenkins
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>

3 files changed, 35 insertions, 8 deletions

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index a18101da..9f660702 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -857,15 +857,16 @@ int dap_get_debugbase(struct adiv5_dap *dap, int ap,
 }
 
 int dap_lookup_cs_component(struct adiv5_dap *dap, int ap,
-			uint32_t dbgbase, uint8_t type, uint32_t *addr)
+			uint32_t dbgbase, uint8_t type, uint32_t *addr, int32_t *idx)
 {
 	uint32_t ap_old;
 	uint32_t romentry, entry_offset = 0, component_base, devtype;
-	int retval = ERROR_FAIL;
+	int retval;
 
 	if (ap >= 256)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
+	*addr = 0;
 	ap_old = dap->ap_current;
 	dap_ap_select(dap, ap);
 
@@ -879,15 +880,33 @@ int dap_lookup_cs_component(struct adiv5_dap *dap, int ap,
 			+ (romentry & 0xFFFFF000);
 
 		if (romentry & 0x1) {
+			uint32_t c_cid1;
+			retval = mem_ap_read_atomic_u32(dap, component_base | 0xff4, &c_cid1);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Can't read component with base address 0x%" PRIx32
+					  ", the corresponding core might be turned off", component_base);
+				return retval;
+			}
+			if (((c_cid1 >> 4) & 0x0f) == 1) {
+				retval = dap_lookup_cs_component(dap, ap, component_base,
+							type, addr, idx);
+				if (retval == ERROR_OK)
+					break;
+				if (retval != ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
+					return retval;
+			}
+
 			retval = mem_ap_read_atomic_u32(dap,
 					(component_base & 0xfffff000) | 0xfcc,
 					&devtype);
 			if (retval != ERROR_OK)
 				return retval;
 			if ((devtype & 0xff) == type) {
-				*addr = component_base;
-				retval = ERROR_OK;
-				break;
+				if (!*idx) {
+					*addr = component_base;
+					break;
+				} else
+					(*idx)--;
 			}
 		}
 		entry_offset += 4;
@@ -895,7 +914,10 @@ int dap_lookup_cs_component(struct adiv5_dap *dap, int ap,
 
 	dap_ap_select(dap, ap_old);
 
-	return retval;
+	if (!*addr)
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+
+	return ERROR_OK;
 }
 
 static int dap_rom_display(struct command_context *cmd_ctx,
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 047900ad..dee3117e 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -470,7 +470,7 @@ int dap_find_ap(struct adiv5_dap *dap,
 
 /* Lookup CoreSight component */
 int dap_lookup_cs_component(struct adiv5_dap *dap, int ap,
-			uint32_t dbgbase, uint8_t type, uint32_t *addr);
+			uint32_t dbgbase, uint8_t type, uint32_t *addr, int32_t *idx);
 
 struct target;
 
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 7d58ab34..0393a442 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -2393,14 +2393,19 @@ static int cortex_a_examine_first(struct target *target)
 		uint32_t dbgbase;
 		/* Get ROM Table base */
 		uint32_t apid;
+		int32_t coreidx = target->coreid;
+		LOG_DEBUG("%s's dbgbase is not set, trying to detect using the ROM table",
+			  target->cmd_name);
 		retval = dap_get_debugbase(swjdp, 1, &dbgbase, &apid);
 		if (retval != ERROR_OK)
 			return retval;
 		/* Lookup 0x15 -- Processor DAP */
 		retval = dap_lookup_cs_component(swjdp, 1, dbgbase, 0x15,
-				&armv7a->debug_base);
+				&armv7a->debug_base, &coreidx);
 		if (retval != ERROR_OK)
 			return retval;
+		LOG_DEBUG("Detected core %" PRId32 " dbgbase: %08" PRIx32,
+			  coreidx, armv7a->debug_base);
 	} else
 		armv7a->debug_base = target->dbgbase;