4 files changed, 123 insertions, 23 deletions

diff --git a/arch/arm64/include/asm/smp.h b/arch/arm64/include/asm/smp.h
index 7e34295f78e3..b1f68c19d170 100644
--- a/arch/arm64/include/asm/smp.h
+++ b/arch/arm64/include/asm/smp.h
@@ -66,4 +66,14 @@ extern volatile unsigned long secondary_holding_pen_release;
 extern void arch_send_call_function_single_ipi(int cpu);
 extern void arch_send_call_function_ipi_mask(const struct cpumask *mask);
 
+struct device_node;
+
+struct smp_enable_ops {
+        const char      *name;
+        int             (*init_cpu)(struct device_node *, int);
+        int             (*prepare_cpu)(int);
+};
+
+extern const struct smp_enable_ops smp_spin_table_ops;
+
 #endif /* ifndef __ASM_SMP_H */
diff --git a/arch/arm64/kernel/Makefile b/arch/arm64/kernel/Makefile
index a1cace45e970..ac1b6823334c 100644
--- a/arch/arm64/kernel/Makefile
+++ b/arch/arm64/kernel/Makefile
@@ -14,7 +14,7 @@ arm64-obj-y		:= cputable.o debug-monitors.o entry.o irq.o fpsimd.o	\
 arm64-obj-$(CONFIG_COMPAT)              += sys32.o kuser32.o signal32.o         \
                                            sys_compat.o
 arm64-obj-$(CONFIG_MODULES)             += arm64ksyms.o module.o
-arm64-obj-$(CONFIG_SMP)                 += smp.o
+arm64-obj-$(CONFIG_SMP)                 += smp.o smp_spin_table.o
 arm64-obj-$(CONFIG_HW_PERF_EVENTS)      += perf_event.o
 arm64-obj-$(CONFIG_HAVE_HW_BREAKPOINT)+= hw_breakpoint.o
 arm64-obj-$(CONFIG_EARLY_PRINTK)        += early_printk.o
diff --git a/arch/arm64/kernel/smp.c b/arch/arm64/kernel/smp.c
index 538300f2273d..7776922945af 100644
--- a/arch/arm64/kernel/smp.c
+++ b/arch/arm64/kernel/smp.c
@@ -233,7 +233,27 @@ void __init smp_prepare_boot_cpu(void)
 }
 
 static void (*smp_cross_call)(const struct cpumask *, unsigned int);
-static phys_addr_t cpu_release_addr[NR_CPUS];
+
+static const struct smp_enable_ops *enable_ops[] __initconst = {
+        &smp_spin_table_ops,
+        NULL,
+};
+
+static const struct smp_enable_ops *smp_enable_ops[NR_CPUS];
+
+static const struct smp_enable_ops * __init smp_get_enable_ops(const char *name)
+{
+        const struct smp_enable_ops *ops = enable_ops[0];
+
+        while (ops) {
+                if (!strcmp(name, ops->name))
+                        return ops;
+
+                ops++;
+        }
+
+        return NULL;
+}
 
 /*
  * Enumerate the possible CPU set from the device tree.
@@ -252,22 +272,22 @@ void __init smp_init_cpus(void)
                  * We currently support only the "spin-table" enable-method.
                  */
                 enable_method = of_get_property(dn, "enable-method", NULL);
-                if (!enable_method || strcmp(enable_method, "spin-table")) {
-                        pr_err("CPU %d: missing or invalid enable-method property: %s\n",
-                               cpu, enable_method);
+                if (!enable_method) {
+                        pr_err("CPU %d: missing enable-method property\n", cpu);
                         goto next;
                 }
 
-                /*
-                 * Determine the address from which the CPU is polling.
-                 */
-                if (of_property_read_u64(dn, "cpu-release-addr",
-                                         &cpu_release_addr[cpu])) {
-                        pr_err("CPU %d: missing or invalid cpu-release-addr property\n",
-                               cpu);
+                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);
                         goto next;
                 }
 
+                if (smp_enable_ops[cpu]->init_cpu(dn, cpu))
+                        goto next;
+
                 set_cpu_possible(cpu, true);
 next:
                 cpu++;
@@ -281,8 +301,7 @@ next:
 
 void __init smp_prepare_cpus(unsigned int max_cpus)
 {
-        int cpu;
-        void **release_addr;
+        int cpu, err;
         unsigned int ncores = num_possible_cpus();
 
         /*
@@ -291,30 +310,35 @@ void __init smp_prepare_cpus(unsigned int max_cpus)
         if (max_cpus > ncores)
                 max_cpus = ncores;
 
+        /* Don't bother if we're effectively UP */
+        if (max_cpus <= 1)
+                return;
+
         /*
          * Initialise the present map (which describes the set of CPUs
          * actually populated at the present time) and release the
          * secondaries from the bootloader.
+         *
+         * Make sure we online at most (max_cpus - 1) additional CPUs.
          */
+        max_cpus--;
         for_each_possible_cpu(cpu) {
                 if (max_cpus == 0)
                         break;
 
-                if (!cpu_release_addr[cpu])
+                if (cpu == smp_processor_id())
+                        continue;
+
+                if (!smp_enable_ops[cpu])
                         continue;
 
-                release_addr = __va(cpu_release_addr[cpu]);
-                release_addr[0] = (void *)__pa(secondary_holding_pen);
-                __flush_dcache_area(release_addr, sizeof(release_addr[0]));
+                err = smp_enable_ops[cpu]->prepare_cpu(cpu);
+                if (err)
+                        continue;
 
                 set_cpu_present(cpu, true);
                 max_cpus--;
         }
-
-        /*
-         * Send an event to wake up the secondaries.
-         */
-        sev();
 }
 
 
diff --git a/arch/arm64/kernel/smp_spin_table.c b/arch/arm64/kernel/smp_spin_table.c
new file mode 100644
index 000000000000..7c35fa682f76
--- /dev/null
+++ b/arch/arm64/kernel/smp_spin_table.c
@@ -0,0 +1,66 @@
+/*
+ * Spin Table SMP initialisation
+ *
+ * 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/>.
+ */
+
+#include <linux/init.h>
+#include <linux/of.h>
+#include <linux/smp.h>
+
+#include <asm/cacheflush.h>
+
+static phys_addr_t cpu_release_addr[NR_CPUS];
+
+static int __init smp_spin_table_init_cpu(struct device_node *dn, int cpu)
+{
+        /*
+         * Determine the address from which the CPU is polling.
+         */
+        if (of_property_read_u64(dn, "cpu-release-addr",
+                                 &cpu_release_addr[cpu])) {
+                pr_err("CPU %d: missing or invalid cpu-release-addr property\n",
+                       cpu);
+
+                return -1;
+        }
+
+        return 0;
+}
+
+static int __init smp_spin_table_prepare_cpu(int cpu)
+{
+        void **release_addr;
+
+        if (!cpu_release_addr[cpu])
+                return -ENODEV;
+
+        release_addr = __va(cpu_release_addr[cpu]);
+        release_addr[0] = (void *)__pa(secondary_holding_pen);
+        __flush_dcache_area(release_addr, sizeof(release_addr[0]));
+
+        /*
+         * Send an event to wake up the secondary CPU.
+         */
+        sev();
+
+        return 0;
+}
+
+const struct smp_enable_ops smp_spin_table_ops __initconst = {
+        .name           = "spin-table",
+        .init_cpu       = smp_spin_table_init_cpu,
+        .prepare_cpu    = smp_spin_table_prepare_cpu,
+};