2 files changed, 22 insertions, 0 deletions

diff --git a/arch/arm/common/bL_switcher.c b/arch/arm/common/bL_switcher.c
index f4878a36047a..63bbc4f70564 100644
--- a/arch/arm/common/bL_switcher.c
+++ b/arch/arm/common/bL_switcher.c
@@ -532,6 +532,26 @@ static int bL_switcher_halve_cpus(void)
         return 0;
 }
 
+/* Determine the logical CPU a given physical CPU is grouped on. */
+int bL_switcher_get_logical_index(u32 mpidr)
+{
+        int cpu;
+
+        if (!bL_switcher_active)
+                return -EUNATCH;
+
+        mpidr &= MPIDR_HWID_BITMASK;
+        for_each_online_cpu(cpu) {
+                int pairing = bL_switcher_cpu_pairing[cpu];
+                if (pairing == -1)
+                        continue;
+                if ((mpidr == cpu_logical_map(cpu)) ||
+                    (mpidr == cpu_logical_map(pairing)))
+                        return cpu;
+        }
+        return -EINVAL;
+}
+
 static void bL_switcher_trace_trigger_cpu(void *__always_unused info)
 {
         trace_cpu_migrate_current(get_ns(), read_mpidr());
diff --git a/arch/arm/include/asm/bL_switcher.h b/arch/arm/include/asm/bL_switcher.h
index 8ada5a885c70..1714800fa113 100644
--- a/arch/arm/include/asm/bL_switcher.h
+++ b/arch/arm/include/asm/bL_switcher.h
@@ -55,6 +55,7 @@ bool bL_switcher_get_enabled(void);
 void bL_switcher_put_enabled(void);
 
 int bL_switcher_trace_trigger(void);
+int bL_switcher_get_logical_index(u32 mpidr);
 
 #else
 static inline int bL_switcher_register_notifier(struct notifier_block *nb)
@@ -70,6 +71,7 @@ static inline int bL_switcher_unregister_notifier(struct notifier_block *nb)
 static inline bool bL_switcher_get_enabled(void) { return false; }
 static inline void bL_switcher_put_enabled(void) { }
 static inline int bL_switcher_trace_trigger(void) { return 0; }
+static inline int bL_switcher_get_logical_index(u32 mpidr) { return -EUNATCH; }
 #endif /* CONFIG_BL_SWITCHER */
 
 #endif