arm_fpga: Fix MPIDR topology checks

The plat_core_pos_by_mpidr() implementation for the Arm FPGA port has
some issues, which leads to problems when matching GICv3 redistributors
with cores:
- The power domain tree was not taking multithreading into account, so
  we ended up with the wrong mapping between MPIDRs and core IDs.
- Before even considering an MPIDR, we try to make sure Aff2 is 0.
  Unfortunately this is the cluster ID when the MT bit is set.
- We mask off the MT bit in MPIDR, before basing decisions on it.
- When detecting the MT bit, we are properly calculating the thread ID,
  but don't account for the shift in the core and cluster ID checks.

Those problems lead to early rejections of MPIDRs values, in particular
when called from the GIC code. As a result, CPU_ON for secondary cores
was failing for most of the cores.

Fix this by properly handling the MT bit in plat_core_pos_by_mpidr(),
also pulling in FPGA_MAX_PE_PER_CPU when populating the power domain
tree.

Change-Id: I71b2255fc0d27bfe5806511df479ab38e4e33fc4
Signed-off-by: Andre Przywara <andre.przywara@arm.com>

diff --git a/plat/arm/board/arm_fpga/fpga_topology.c b/plat/arm/board/arm_fpga/fpga_topology.c
index a705429ac925633e052d0a74ed19d5a32bd61a09..a2908d72787253f9192fe2dfd65b1b2b573df4e5 100644 (file)
--- a/plat/arm/board/arm_fpga/fpga_topology.c
+++ b/plat/arm/board/arm_fpga/fpga_topology.c
@@ -26,7 +26,7 @@ const unsigned char *plat_get_power_domain_tree_desc(void)
        fpga_power_domain_tree_desc[1] = FPGA_MAX_CLUSTER_COUNT;
 
        for (i = 0; i < FPGA_MAX_CLUSTER_COUNT; i++) {
-               fpga_power_domain_tree_desc[i + 2] = FPGA_MAX_CPUS_PER_CLUSTER;
+               fpga_power_domain_tree_desc[i + 2] = FPGA_MAX_CPUS_PER_CLUSTER * FPGA_MAX_PE_PER_CPU;
        }
 
        return fpga_power_domain_tree_desc;
@@ -36,35 +36,37 @@ int plat_core_pos_by_mpidr(u_register_t mpidr)
 {
        unsigned int cluster_id, cpu_id, thread_id;
 
-       mpidr &= MPIDR_AFFINITY_MASK;
-       if (mpidr & ~(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK)) {
-               return -1;
-       }
+       /*
+        * The image running on the FPGA may or may not implement
+        * multithreading, and it shouldn't be assumed this is consistent
+        * across all CPUs.
+        * This ensures that any passed mpidr values reflect the status of the
+        * primary CPU's MT bit.
+        */
+       mpidr |= (read_mpidr_el1() & MPIDR_MT_MASK);
+       mpidr &= MPID_MASK;
 
        if (mpidr & MPIDR_MT_MASK) {
                thread_id = MPIDR_AFFLVL0_VAL(mpidr);
+               cpu_id = MPIDR_AFFLVL1_VAL(mpidr);
+               cluster_id = MPIDR_AFFLVL2_VAL(mpidr);
        } else {
                thread_id = 0;
+               cpu_id = MPIDR_AFFLVL0_VAL(mpidr);
+               cluster_id = MPIDR_AFFLVL1_VAL(mpidr);
        }
 
-       cpu_id = MPIDR_AFFLVL1_VAL(mpidr);
-       cluster_id = MPIDR_AFFLVL2_VAL(mpidr);
-
        if (cluster_id >= FPGA_MAX_CLUSTER_COUNT) {
                return -1;
-       } else if (cpu_id >= FPGA_MAX_CPUS_PER_CLUSTER) {
-               return -1;
-       } else if (thread_id >= FPGA_MAX_PE_PER_CPU) {
+       }
+
+       if (cpu_id >= FPGA_MAX_CPUS_PER_CLUSTER) {
                return -1;
        }
 
-       /*
-        * The image running on the FPGA may or may not implement multithreading,
-        * and it shouldn't be assumed this is consistent across all CPUs.
-        * This ensures that any passed mpidr values reflect the status of the
-        * primary CPU's MT bit.
-        */
-       mpidr |= (read_mpidr_el1() & MPIDR_MT_MASK);
+       if (thread_id >= FPGA_MAX_PE_PER_CPU) {
+               return -1;
+       }
 
        /* Calculate the correct core, catering for multi-threaded images */
        return (int) plat_fpga_calc_core_pos(mpidr);