diff --git a/plat/nvidia/tegra/common/aarch64/tegra_helpers.S b/plat/nvidia/tegra/common/aarch64/tegra_helpers.S index 3575f6860..b47be6dc4 100644 --- a/plat/nvidia/tegra/common/aarch64/tegra_helpers.S +++ b/plat/nvidia/tegra/common/aarch64/tegra_helpers.S @@ -144,17 +144,20 @@ func plat_is_my_cpu_primary ret endfunc plat_is_my_cpu_primary - /* ----------------------------------------------------- + /* ---------------------------------------------------------- * unsigned int plat_my_core_pos(void); * - * result: CorePos = CoreId + (ClusterId << 2) - * ----------------------------------------------------- + * result: CorePos = CoreId + (ClusterId * cpus per cluster) + * ---------------------------------------------------------- */ func plat_my_core_pos mrs x0, mpidr_el1 and x1, x0, #MPIDR_CPU_MASK and x0, x0, #MPIDR_CLUSTER_MASK - add x0, x1, x0, LSR #6 + lsr x0, x0, #MPIDR_AFFINITY_BITS + mov x2, #PLATFORM_MAX_CPUS_PER_CLUSTER + mul x0, x0, x2 + add x0, x1, x0 ret endfunc plat_my_core_pos @@ -176,14 +179,17 @@ endfunc plat_get_my_entrypoint /* ----------------------------------------------------- * int platform_get_core_pos(int mpidr); * - * With this function: CorePos = (ClusterId * 4) + - * CoreId + * result: CorePos = (ClusterId * cpus per cluster) + + * CoreId * ----------------------------------------------------- */ func platform_get_core_pos and x1, x0, #MPIDR_CPU_MASK and x0, x0, #MPIDR_CLUSTER_MASK - add x0, x1, x0, LSR #6 + lsr x0, x0, #MPIDR_AFFINITY_BITS + mov x2, #PLATFORM_MAX_CPUS_PER_CLUSTER + mul x0, x0, x2 + add x0, x1, x0 ret endfunc platform_get_core_pos diff --git a/plat/nvidia/tegra/common/tegra_topology.c b/plat/nvidia/tegra/common/tegra_topology.c index 4f6cf932e..14631a776 100644 --- a/plat/nvidia/tegra/common/tegra_topology.c +++ b/plat/nvidia/tegra/common/tegra_topology.c @@ -23,10 +23,14 @@ int32_t plat_core_pos_by_mpidr(u_register_t mpidr) u_register_t cluster_id, cpu_id; int32_t result; - cluster_id = (mpidr >> (u_register_t)MPIDR_AFF1_SHIFT) & (u_register_t)MPIDR_AFFLVL_MASK; - cpu_id = (mpidr >> (u_register_t)MPIDR_AFF0_SHIFT) & (u_register_t)MPIDR_AFFLVL_MASK; + cluster_id = (mpidr >> (u_register_t)MPIDR_AFF1_SHIFT) & + (u_register_t)MPIDR_AFFLVL_MASK; + cpu_id = (mpidr >> (u_register_t)MPIDR_AFF0_SHIFT) & + (u_register_t)MPIDR_AFFLVL_MASK; - result = (int32_t)cpu_id + ((int32_t)cluster_id * 4); + /* CorePos = CoreId + (ClusterId * cpus per cluster) */ + result = (int32_t)cpu_id + ((int32_t)cluster_id * + PLATFORM_MAX_CPUS_PER_CLUSTER); if (cluster_id >= (u_register_t)PLATFORM_CLUSTER_COUNT) { result = PSCI_E_NOT_PRESENT;