diff --git a/arch/arm64/include/asm/cputype.h b/arch/arm64/include/asm/cputype.h
index 9397a17bec0ba41184f253fb0d3e82fa0de62a3d..3780b2e7f88fc56c1b4443f3d428062f5faeac21 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 0000000000000000000000000000000000000000..ed43a0d2b1b2c19aaf4e3d2ecca6eecc655bea94
--- /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 .
+ */
+
+#ifndef __ASM_SMP_PLAT_H
+#define __ASM_SMP_PLAT_H
+
+#include
+
+/*
+ * 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 9c023d714f44fcdfcac65b3c43bd2215f900caed..6a9a5329259065a99ceea12c3fe391f5996c448b 100644
--- a/arch/arm64/kernel/setup.c
+++ b/arch/arm64/kernel/setup.c
@@ -47,6 +47,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -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 a57a373d305f5ca9dcc3a79404a2885a5ef78dae..d4dcc6515253e869f898feb2da50c0a390872837 100644
--- a/arch/arm64/kernel/smp.c
+++ b/arch/arm64/kernel/smp.c
@@ -43,6 +43,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -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 @@ void __init smp_init_cpus(void)
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)