arm64: kernel: initialise cpu_logical_map from the DT

When booting the kernel, the cpu logical id map must be initialised
using device tree data passed by FW or through an embedded blob.

This patch parses the reg property in device tree "cpu" nodes,
retrieves the corresponding CPUs hardware identifiers (MPIDR) and
initialises the cpu logical map accordingly.

The device tree HW identifiers are considered valid if all CPU nodes
contain a "reg" property, there are no duplicate "reg" entries and the
DT defines a CPU node whose "reg" property defines affinity levels
that matches those of the boot CPU.

The primary CPU is assigned cpu logical number 0 to keep the current
convention valid.

Based on a0ae02405076ac32bd17ece976e914b5b6075bb0 (ARM: kernel: add
device tree init map function).

Signed-off-by: Javi Merino <javi.merino@arm.com>
Signed-off-by: Catalin Marinas <catalin.marinas@arm.com>

4 files changed, 121 insertions, 7 deletions

diff --git a/arch/arm64/include/asm/cputype.h b/arch/arm64/include/asm/cputype.h
index 9397a17bec0b..3780b2e7f88f 100644
--- a/arch/arm64/include/asm/cputype.h
+++ b/arch/arm64/include/asm/cputype.h
@@ -28,6 +28,8 @@
 
 #define INVALID_HWID            ULONG_MAX
 
+#define MPIDR_HWID_BITMASK      0xff00ffffff
+
 #define read_cpuid(reg) ({                                              \
         u64 __val;                                                      \
         asm("mrs        %0, " reg : "=r" (__val));                      \
diff --git a/arch/arm64/include/asm/smp_plat.h b/arch/arm64/include/asm/smp_plat.h
new file mode 100644
index 000000000000..ed43a0d2b1b2
--- /dev/null
+++ b/arch/arm64/include/asm/smp_plat.h
@@ -0,0 +1,30 @@
+/*
+ * Definitions specific to SMP platforms.
+ *
+ * Copyright (C) 2013 ARM Ltd.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __ASM_SMP_PLAT_H
+#define __ASM_SMP_PLAT_H
+
+#include <asm/types.h>
+
+/*
+ * Logical CPU mapping.
+ */
+extern u64 __cpu_logical_map[NR_CPUS];
+#define cpu_logical_map(cpu)    __cpu_logical_map[cpu]
+
+#endif /* __ASM_SMP_PLAT_H */
diff --git a/arch/arm64/kernel/setup.c b/arch/arm64/kernel/setup.c
index 9c023d714f44..6a9a53292590 100644
--- a/arch/arm64/kernel/setup.c
+++ b/arch/arm64/kernel/setup.c
@@ -47,6 +47,7 @@
 #include <asm/cputable.h>
 #include <asm/sections.h>
 #include <asm/setup.h>
+#include <asm/smp_plat.h>
 #include <asm/cacheflush.h>
 #include <asm/tlbflush.h>
 #include <asm/traps.h>
@@ -241,6 +242,8 @@ static void __init request_standard_resources(void)
         }
 }
 
+u64 __cpu_logical_map[NR_CPUS] = { [0 ... NR_CPUS-1] = INVALID_HWID };
+
 void __init setup_arch(char **cmdline_p)
 {
         setup_processor();
@@ -265,6 +268,7 @@ void __init setup_arch(char **cmdline_p)
 
         psci_init();
 
+        cpu_logical_map(0) = read_cpuid_mpidr() & MPIDR_HWID_BITMASK;
 #ifdef CONFIG_SMP
         smp_init_cpus();
 #endif
diff --git a/arch/arm64/kernel/smp.c b/arch/arm64/kernel/smp.c
index a57a373d305f..d4dcc6515253 100644
--- a/arch/arm64/kernel/smp.c
+++ b/arch/arm64/kernel/smp.c
@@ -43,6 +43,7 @@
 #include <asm/pgtable.h>
 #include <asm/pgalloc.h>
 #include <asm/processor.h>
+#include <asm/smp_plat.h>
 #include <asm/sections.h>
 #include <asm/tlbflush.h>
 #include <asm/ptrace.h>
@@ -96,7 +97,7 @@ static int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
         /*
          * Update the pen release flag.
          */
-        write_pen_release(cpu);
+        write_pen_release(cpu_logical_map(cpu));
 
         /*
          * Send an event, causing the secondaries to read pen_release.
@@ -257,15 +258,77 @@ static const struct smp_enable_ops * __init smp_get_enable_ops(const char *name)
 }
 
 /*
- * Enumerate the possible CPU set from the device tree.
+ * Enumerate the possible CPU set from the device tree and build the
+ * cpu logical map array containing MPIDR values related to logical
+ * cpus. Assumes that cpu_logical_map(0) has already been initialized.
  */
 void __init smp_init_cpus(void)
 {
         const char *enable_method;
         struct device_node *dn = NULL;
-        int cpu = 0;
+        int i, cpu = 1;
+        bool bootcpu_valid = false;
 
         while ((dn = of_find_node_by_type(dn, "cpu"))) {
+                u64 hwid;
+
+                /*
+                 * A cpu node with missing "reg" property is
+                 * considered invalid to build a cpu_logical_map
+                 * entry.
+                 */
+                if (of_property_read_u64(dn, "reg", &hwid)) {
+                        pr_err("%s: missing reg property\n", dn->full_name);
+                        goto next;
+                }
+
+                /*
+                 * Non affinity bits must be set to 0 in the DT
+                 */
+                if (hwid & ~MPIDR_HWID_BITMASK) {
+                        pr_err("%s: invalid reg property\n", dn->full_name);
+                        goto next;
+                }
+
+                /*
+                 * Duplicate MPIDRs are a recipe for disaster. Scan
+                 * all initialized entries and check for
+                 * duplicates. If any is found just ignore the cpu.
+                 * cpu_logical_map was initialized to INVALID_HWID to
+                 * avoid matching valid MPIDR values.
+                 */
+                for (i = 1; (i < cpu) && (i < NR_CPUS); i++) {
+                        if (cpu_logical_map(i) == hwid) {
+                                pr_err("%s: duplicate cpu reg properties in the DT\n",
+                                        dn->full_name);
+                                goto next;
+                        }
+                }
+
+                /*
+                 * The numbering scheme requires that the boot CPU
+                 * must be assigned logical id 0. Record it so that
+                 * the logical map built from DT is validated and can
+                 * be used.
+                 */
+                if (hwid == cpu_logical_map(0)) {
+                        if (bootcpu_valid) {
+                                pr_err("%s: duplicate boot cpu reg property in DT\n",
+                                        dn->full_name);
+                                goto next;
+                        }
+
+                        bootcpu_valid = true;
+
+                        /*
+                         * cpu_logical_map has already been
+                         * initialized and the boot cpu doesn't need
+                         * the enable-method so continue without
+                         * incrementing cpu.
+                         */
+                        continue;
+                }
+
                 if (cpu >= NR_CPUS)
                         goto next;
 
@@ -274,22 +337,24 @@ void __init smp_init_cpus(void)
                  */
                 enable_method = of_get_property(dn, "enable-method", NULL);
                 if (!enable_method) {
-                        pr_err("CPU %d: missing enable-method property\n", cpu);
+                        pr_err("%s: missing enable-method property\n",
+                                dn->full_name);
                         goto next;
                 }
 
                 smp_enable_ops[cpu] = smp_get_enable_ops(enable_method);
 
                 if (!smp_enable_ops[cpu]) {
-                        pr_err("CPU %d: invalid enable-method property: %s\n",
-                               cpu, enable_method);
+                        pr_err("%s: invalid enable-method property: %s\n",
+                               dn->full_name, enable_method);
                         goto next;
                 }
 
                 if (smp_enable_ops[cpu]->init_cpu(dn, cpu))
                         goto next;
 
-                set_cpu_possible(cpu, true);
+                pr_debug("cpu logical map 0x%llx\n", hwid);
+                cpu_logical_map(cpu) = hwid;
 next:
                 cpu++;
         }
@@ -298,6 +363,19 @@ next:
         if (cpu > NR_CPUS)
                 pr_warning("no. of cores (%d) greater than configured maximum of %d - clipping\n",
                            cpu, NR_CPUS);
+
+        if (!bootcpu_valid) {
+                pr_err("DT missing boot CPU MPIDR, not enabling secondaries\n");
+                return;
+        }
+
+        /*
+         * All the cpus that made it to the cpu_logical_map have been
+         * validated so set them as possible cpus.
+         */
+        for (i = 0; i < NR_CPUS; i++)
+                if (cpu_logical_map(i) != INVALID_HWID)
+                        set_cpu_possible(i, true);
 }
 
 void __init smp_prepare_cpus(unsigned int max_cpus)