ADIv5: Implement function to lookup CoreSight component

This patch implements "dap_lookup_cs_component()", which allows to lookup CS
component by it's identification.

Signed-off-by: Marek Vasut <marek.vasut@gmail.com>

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 4950121afaad36fc87d1291261b5bc023d26355d..81edba4e3ee91adfe9370afbbf2905f2a27f1427 100644 (file)
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -1062,6 +1062,47 @@ int dap_get_debugbase(struct adiv5_dap *dap, int apsel,
        return ERROR_OK;
 }
 
+int dap_lookup_cs_component(struct adiv5_dap *dap, int apsel,
+                       uint32_t dbgbase, uint8_t type, uint32_t *addr)
+{
+       uint32_t apselold;
+       uint32_t romentry, entry_offset = 0, component_base, devtype;
+       int retval = ERROR_FAIL;
+
+       if (apsel >= 256)
+               return ERROR_INVALID_ARGUMENTS;
+
+       apselold = dap->apsel;
+       dap_ap_select(dap, apsel);
+
+       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, apselold);
+
+       return retval;
+}
+
 static int dap_info_command(struct command_context *cmd_ctx,
                struct adiv5_dap *dap, int apsel)
 {

diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 27a2f2f8a907b7e836f5b669b1d33f4d8ee21b16..6c1808a5cb68c007fa86dd38faf5a76b41c36a85 100644 (file)
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -383,6 +383,10 @@ int ahbap_debugport_init(struct adiv5_dap *swjdp);
 int dap_get_debugbase(struct adiv5_dap *dap, int apsel,
                        uint32_t *dbgbase, uint32_t *apid);
 
+/* Lookup CoreSight component */
+int dap_lookup_cs_component(struct adiv5_dap *dap, int apsel,
+                       uint32_t dbgbase, uint8_t type, uint32_t *addr);
+
 struct target;
 
 /* Put debug link into SWD mode */