fix(rpi): consider MT when calculating core index from MPIDR
RPi 5 has newer Armv8.2 cores where the MT bit is set to indicate that
the lowest affinity level represents a thread, but there is only one
thread per core.
To deal with this, simply right shift MPIDR by one affinity level to get
the cluster and core IDs back into Aff1 and Aff0 as expected.
Change-Id: I2bafba38f82fd9a6ef6f2fdf2c089b754279a6de
Signed-off-by: Mario Bălănică <mariobalanica02@gmail.com>
diff --git a/plat/rpi/common/aarch64/plat_helpers.S b/plat/rpi/common/aarch64/plat_helpers.S
index bc6b8c5..18873af 100644
--- a/plat/rpi/common/aarch64/plat_helpers.S
+++ b/plat/rpi/common/aarch64/plat_helpers.S
@@ -27,10 +27,19 @@
*
* This function uses the plat_rpi3_calc_core_pos()
* definition to get the index of the calling CPU.
+ *
+ * When MT is set, lowest affinity represents the thread ID.
+ * Since we only support one thread per core, discard this field
+ * so cluster and core IDs go back into Aff1 and Aff0 respectively.
+ * The upper bits are also affected, but plat_rpi3_calc_core_pos()
+ * does not use them.
* -----------------------------------------------------
*/
func plat_my_core_pos
mrs x0, mpidr_el1
+ tst x0, #MPIDR_MT_MASK
+ lsr x1, x0, #MPIDR_AFFINITY_BITS
+ csel x0, x1, x0, ne
b plat_rpi3_calc_core_pos
endfunc plat_my_core_pos
diff --git a/plat/rpi/common/rpi3_topology.c b/plat/rpi/common/rpi3_topology.c
index 3747287..5fef777 100644
--- a/plat/rpi/common/rpi3_topology.c
+++ b/plat/rpi/common/rpi3_topology.c
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2024, Arm Limited and Contributors. All rights reserved.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
@@ -39,12 +39,27 @@
unsigned int cluster_id, cpu_id;
mpidr &= MPIDR_AFFINITY_MASK;
+
+ /*
+ * When MT is set, lowest affinity represents the thread ID.
+ * Since we only support one thread per core, discard this field
+ * so cluster and core IDs go back into Aff1 and Aff0 respectively.
+ * The upper bits are also affected, but plat_rpi3_calc_core_pos()
+ * does not use them.
+ */
+ if ((read_mpidr() & MPIDR_MT_MASK) != 0) {
+ if (MPIDR_AFFLVL0_VAL(mpidr) != 0) {
+ return -1;
+ }
+ mpidr >>= MPIDR_AFFINITY_BITS;
+ }
+
if (mpidr & ~(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)) {
return -1;
}
- cluster_id = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
- cpu_id = (mpidr >> MPIDR_AFF0_SHIFT) & MPIDR_AFFLVL_MASK;
+ cluster_id = MPIDR_AFFLVL1_VAL(mpidr);
+ cpu_id = MPIDR_AFFLVL0_VAL(mpidr);
if (cluster_id >= PLATFORM_CLUSTER_COUNT) {
return -1;