css/sgi: rework the core position calculation function
The MT bit in MPIDR is always set for SGI platforms and so the
core position calculation code is updated to take into account
the thread affinity value as well.
Change-Id: I7b2a52707f607dc3859c6bbcd2b145b7987cb4ed
Signed-off-by: Chandni Cherukuri <chandni.cherukuri@arm.com>
Signed-off-by: Vishwanatha HG <vishwanatha.hg@arm.com>
diff --git a/plat/arm/css/sgi/aarch64/sgi_helper.S b/plat/arm/css/sgi/aarch64/sgi_helper.S
index c435d8b..aaa5156 100644
--- a/plat/arm/css/sgi/aarch64/sgi_helper.S
+++ b/plat/arm/css/sgi/aarch64/sgi_helper.S
@@ -31,19 +31,37 @@
endfunc plat_is_my_cpu_primary
/* -----------------------------------------------------
- * unsigned int plat_arm_calc_core_pos(uint64_t mpidr)
- * Helper function to calculate the core position.
- * -----------------------------------------------------
- */
+ * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
+ *
+ * Helper function to calculate the core position.
+ * (ClusterId * CSS_SGI_MAX_CPUS_PER_CLUSTER * CSS_SGI_MAX_PE_PER_CPU) +
+ * (CPUId * CSS_SGI_MAX_PE_PER_CPU) +
+ * ThreadId
+ *
+ * which can be simplified as:
+ *
+ * ((ClusterId * CSS_SGI_MAX_CPUS_PER_CLUSTER + CPUId) *
+ * CSS_SGI_MAX_PE_PER_CPU) + ThreadId
+ * ------------------------------------------------------
+ */
+
func plat_arm_calc_core_pos
- mrs x2, mpidr_el1
- ands x2, x2, #MPIDR_MT_MASK
- beq 1f
- lsr x0, x0, #MPIDR_AFF1_SHIFT
-1:
- and x1, x0, #MPIDR_CPU_MASK
- and x0, x0, #MPIDR_CLUSTER_MASK
- add x0, x1, x0, LSR #6
- and x0, x0, #MPIDR_AFFLVL_MASK
+ mov x3, x0
+
+ /*
+ * The MT bit in MPIDR is always set for SGI platforms
+ * and the affinity level 0 corresponds to thread affinity level.
+ */
+
+ /* Extract individual affinity fields from MPIDR */
+ ubfx x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
+ ubfx x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
+ ubfx x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
+
+ /* Compute linear position */
+ mov x4, #CSS_SGI_MAX_CPUS_PER_CLUSTER
+ madd x1, x2, x4, x1
+ mov x5, #CSS_SGI_MAX_PE_PER_CPU
+ madd x0, x1, x5, x0
ret
endfunc plat_arm_calc_core_pos