diff --git a/plat/rpi/common/aarch64/plat_helpers.S b/plat/rpi/common/aarch64/plat_helpers.S index bc6b8c5a8..18873af7f 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 3747287c2..5fef777c3 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 @@ int plat_core_pos_by_mpidr(u_register_t mpidr) 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;