arm64: smp_plat: add get_logical_index

The PSCI MIGRATE_INFO_UP_CPU call returns a physical ID, which we will
need to map back to a Linux logical ID.

Implement a reusable get_logical_index to map from a physical ID to a
logical ID.

Signed-off-by: Mark Rutland <[email protected]>
Acked-by: Catalin Marinas <[email protected]>
Acked-by: Lorenzo Pieralisi <[email protected]>
Reviewed-by: Hanjun Guo <[email protected]>
Tested-by: Hanjun Guo <[email protected]>
Cc: Will Deacon <[email protected]>

arch/arm64/include/asm/smp_plat.h

@@ -19,6 +19,8 @@
 #ifndef __ASM_SMP_PLAT_H
 #define __ASM_SMP_PLAT_H
 
+#include <linux/cpumask.h>
+
 #include <asm/types.h>
 
 struct mpidr_hash {
@@ -39,6 +41,20 @@ static inline u32 mpidr_hash_size(void)
  */
 extern u64 __cpu_logical_map[NR_CPUS];
 #define cpu_logical_map(cpu)    __cpu_logical_map[cpu]
+/*
+ * Retrieve logical cpu index corresponding to a given MPIDR.Aff*
+ *  - mpidr: MPIDR.Aff* bits to be used for the look-up
+ *
+ * Returns the cpu logical index or -EINVAL on look-up error
+ */
+static inline int get_logical_index(u64 mpidr)
+{
+	int cpu;
+	for (cpu = 0; cpu < nr_cpu_ids; cpu++)
+		if (cpu_logical_map(cpu) == mpidr)
+			return cpu;
+	return -EINVAL;
+}
 
 void __init do_post_cpus_up_work(void);