xen/arm: Dissociate logical and hardware CPU ID

Introduce cpu_logical_map to associate a logical CPU ID to an hardware CPU ID.
This map will be filled during Xen boot via the device tree. Each CPU node
contains a "reg" property which contains the hardware ID (ie MPIDR[0:23]).

Also move /cpus parsing later so we can use the dt_* API.

Signed-off-by: Julien Grall <julien.grall@linaro.org>
Acked-by: Ian Campbell <ian.campbell@citrix.com>

4 files changed, 119 insertions, 49 deletions

diff --git a/xen/arch/arm/setup.c b/xen/arch/arm/setup.c
index 62147b21ad..1d0b5e902b 100644
--- a/xen/arch/arm/setup.c
+++ b/xen/arch/arm/setup.c
@@ -498,6 +498,114 @@ void __init setup_cache(void)
     cacheline_bytes = 1U << (4 + (ccsid & 0x7));
 }
 
+/* Parse the device tree and build the logical map array containing
+ * MPIDR values related to logical cpus
+ * Code base on Linux arch/arm/kernel/devtree.c
+ */
+static void __init init_cpus_maps(void)
+{
+    register_t mpidr;
+    struct dt_device_node *cpus = dt_find_node_by_path("/cpus");
+    struct dt_device_node *cpu;
+    unsigned int i, j;
+    unsigned int cpuidx = 1;
+    static u32 tmp_map[NR_CPUS] __initdata =
+    {
+        [0 ... NR_CPUS - 1] = MPIDR_INVALID
+    };
+    bool_t bootcpu_valid = 0;
+
+    mpidr = boot_cpu_data.mpidr.bits & MPIDR_HWID_MASK;
+
+    if ( !cpus )
+    {
+        printk(XENLOG_WARNING "WARNING: Can't find /cpus in the device tree.\n"
+               "Using only 1 CPU\n");
+        return;
+    }
+
+    dt_for_each_child_node( cpus, cpu )
+    {
+        u32 hwid;
+
+        if ( !dt_device_type_is_equal(cpu, "cpu") )
+            continue;
+
+        if ( !dt_property_read_u32(cpu, "reg", &hwid) )
+        {
+            printk(XENLOG_WARNING "cpu node `%s`: missing reg property\n",
+                   dt_node_full_name(cpu));
+            continue;
+        }
+
+        /*
+         * 8 MSBs must be set to 0 in the DT since the reg property
+         * defines the MPIDR[23:0]
+         */
+        if ( hwid & ~MPIDR_HWID_MASK )
+        {
+            printk(XENLOG_WARNING "cpu node `%s`: invalid hwid value (0x%x)\n",
+                   dt_node_full_name(cpu), hwid);
+            continue;
+        }
+
+        /*
+         * Duplicate MPIDRs are a recipe for disaster. Scan all initialized
+         * entries and check for duplicates. If any found just skip the node.
+         * temp values values are initialized to MPIDR_INVALID to avoid
+         * matching valid MPIDR[23:0] values.
+         */
+        for ( j = 0; j < cpuidx; j++ )
+        {
+            if ( tmp_map[j] == hwid )
+            {
+                printk(XENLOG_WARNING "cpu node `%s`: duplicate /cpu reg properties in the DT\n",
+                       dt_node_full_name(cpu));
+                continue;
+            }
+        }
+
+        /*
+         * Build a stashed array of MPIDR values. Numbering scheme requires
+         * that if detected the boot CPU must be assigned logical id 0. Other
+         * CPUs get sequential indexes starting from 1. If a CPU node
+         * with a reg property matching the boot CPU MPIDR is detected,
+         * this is recorded and so that the logical map build from DT is
+         * validated and can be used to set the map.
+         */
+        if ( hwid == mpidr )
+        {
+            i = 0;
+            bootcpu_valid = 1;
+        }
+        else
+            i = cpuidx++;
+
+        if ( cpuidx > NR_CPUS )
+        {
+            printk(XENLOG_WARNING "DT /cpu %u node greater than max cores %u, capping them\n",
+                   cpuidx, NR_CPUS);
+            cpuidx = NR_CPUS;
+            break;
+        }
+
+        tmp_map[i] = hwid;
+    }
+
+    if ( !bootcpu_valid )
+    {
+        printk(XENLOG_WARNING "DT missing boot CPU MPIDR[23:0]\n"
+               "Using only 1 CPU\n");
+        return;
+    }
+
+    for ( i = 0; i < cpuidx; i++ )
+    {
+        cpumask_set_cpu(i, &cpu_possible_map);
+        cpu_logical_map(i) = tmp_map[i];
+    }
+}
+
 /* C entry point for boot CPU */
 void __init start_xen(unsigned long boot_phys_offset,
                       unsigned long fdt_paddr,
@@ -517,7 +625,6 @@ void __init start_xen(unsigned long boot_phys_offset,
         + (fdt_paddr & ((1 << SECOND_SHIFT) - 1));
     fdt_size = device_tree_early_init(device_tree_flattened);
 
-    cpus = smp_get_max_cpus();
     cmdline_parse(device_tree_bootargs(device_tree_flattened));
 
     setup_pagetables(boot_phys_offset, get_xen_paddr());
@@ -534,6 +641,9 @@ void __init start_xen(unsigned long boot_phys_offset,
 
     processor_id();
 
+    init_cpus_maps();
+    cpus = smp_get_max_cpus();
+
     platform_init();
 
     init_xen_time();
diff --git a/xen/arch/arm/smpboot.c b/xen/arch/arm/smpboot.c
index 945f4739a9..8ea4750fdf 100644
--- a/xen/arch/arm/smpboot.c
+++ b/xen/arch/arm/smpboot.c
@@ -39,6 +39,9 @@ EXPORT_SYMBOL(cpu_possible_map);
 
 struct cpuinfo_arm cpu_data[NR_CPUS];
 
+/* CPU logical map: map xen cpuid to an MPIDR */
+u32 __cpu_logical_map[NR_CPUS] = { [0 ... NR_CPUS-1] = MPIDR_INVALID };
+
 /* Fake one node for now. See also include/asm-arm/numa.h */
 nodemask_t __read_mostly node_online_map = { { [0] = 1UL } };
 
@@ -85,6 +88,7 @@ smp_clear_cpu_maps (void)
     cpumask_clear(&cpu_online_map);
     cpumask_set_cpu(0, &cpu_online_map);
     cpumask_set_cpu(0, &cpu_possible_map);
+    cpu_logical_map(0) = READ_SYSREG(MPIDR_EL1) & MPIDR_HWID_MASK;
 }
 
 int __init
diff --git a/xen/common/device_tree.c b/xen/common/device_tree.c
index 0ece2499ad..9a16650ee1 100644
--- a/xen/common/device_tree.c
+++ b/xen/common/device_tree.c
@@ -118,18 +118,6 @@ static bool_t __init device_tree_node_matches(const void *fdt, int node,
         && (name[match_len] == '@' || name[match_len] == '\0');
 }
 
-static bool_t __init device_tree_type_matches(const void *fdt, int node,
-                                       const char *match)
-{
-    const void *prop;
-
-    prop = fdt_getprop(fdt, node, "device_type", NULL);
-    if ( prop == NULL )
-        return 0;
-
-    return !dt_node_cmp(prop, match);
-}
-
 static bool_t __init device_tree_node_compatible(const void *fdt, int node,
                                                  const char *match)
 {
@@ -348,40 +336,6 @@ static void __init process_memory_node(const void *fdt, int node,
     }
 }
 
-static void __init process_cpu_node(const void *fdt, int node,
-                                    const char *name,
-                                    u32 address_cells, u32 size_cells)
-{
-    const struct fdt_property *prop;
-    u32 cpuid;
-    int len;
-
-    prop = fdt_get_property(fdt, node, "reg", &len);
-    if ( !prop )
-    {
-        early_printk("fdt: node `%s': missing `reg' property\n", name);
-        return;
-    }
-
-    if ( len < sizeof (cpuid) )
-    {
-        dt_printk("fdt: node `%s': `reg` property length is too short\n",
-                  name);
-        return;
-    }
-
-    cpuid = dt_read_number((const __be32 *)prop->data, 1);
-
-    /* TODO: handle non-contiguous CPU ID */
-    if ( cpuid >= NR_CPUS )
-    {
-        dt_printk("fdt: node `%s': reg(0x%x) >= NR_CPUS(%d)\n",
-                  name, cpuid, NR_CPUS);
-        return;
-    }
-    cpumask_set_cpu(cpuid, &cpu_possible_map);
-}
-
 static void __init process_multiboot_node(const void *fdt, int node,
                                           const char *name,
                                           u32 address_cells, u32 size_cells)
@@ -435,8 +389,6 @@ static int __init early_scan_node(const void *fdt,
 {
     if ( device_tree_node_matches(fdt, node, "memory") )
         process_memory_node(fdt, node, name, address_cells, size_cells);
-    else if ( device_tree_type_matches(fdt, node, "cpu") )
-        process_cpu_node(fdt, node, name, address_cells, size_cells);
     else if ( device_tree_node_compatible(fdt, node, "xen,multiboot-module" ) )
         process_multiboot_node(fdt, node, name, address_cells, size_cells);
 
diff --git a/xen/include/asm-arm/processor.h b/xen/include/asm-arm/processor.h
index 808567e5df..06464227e3 100644
--- a/xen/include/asm-arm/processor.h
+++ b/xen/include/asm-arm/processor.h
@@ -13,6 +13,7 @@
 #define MPIDR_AFF0_SHIFT    (0)
 #define MPIDR_AFF0_MASK     (0xff << MPIDR_AFF0_SHIFT)
 #define MPIDR_HWID_MASK     0xffffff
+#define MPIDR_INVALID       (~MPIDR_HWID_MASK)
 
 /* TTBCR Translation Table Base Control Register */
 #define TTBCR_EAE    0x80000000
@@ -230,6 +231,9 @@ extern void identify_cpu(struct cpuinfo_arm *);
 extern struct cpuinfo_arm cpu_data[];
 #define current_cpu_data cpu_data[smp_processor_id()]
 
+extern u32 __cpu_logical_map[];
+#define cpu_logical_map(cpu) __cpu_logical_map[cpu]
+
 union hsr {
     uint32_t bits;
     struct {