Merge tag 'arm64-for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git/cmarinas/linux-aarch64

Pull arm64 patches from Catalin Marinas:

 - SMP support for the PSCI booting protocol (power state coordination
   interface).

 - Simple earlyprintk support.

 - Platform devices populated by default from the DT (SoC-agnostic).

 - CONTEXTIDR support (used by external trace tools).

* tag 'arm64-for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git/cmarinas/linux-aarch64:
  arm64: mm: update CONTEXTIDR register to contain PID of current process
  arm64: atomics: fix grossly inconsistent asm constraints for exclusives
  arm64: compat: use compat_uptr_t type for compat_ucontext.uc_link
  arm64: Select ARCH_WANT_FRAME_POINTERS
  arm64: Add kvm_para.h and xor.h generic headers
  arm64: SMP: enable PSCI boot method
  arm64: psci: add support for PSCI invocations from the kernel
  arm64: SMP: rework the SMP code to be enabling method agnostic
  arm64: perf: add guest vs host discrimination
  arm64: add COMPAT_PSR_*_BIT flags
  arm64: Add simple earlyprintk support
  arm64: Populate the platform devices

11 files changed, 560 insertions, 29 deletions

diff --git a/arch/arm64/kernel/Makefile b/arch/arm64/kernel/Makefile
index 74239c31e25a..7b4b564961d4 100644
--- a/arch/arm64/kernel/Makefile
+++ b/arch/arm64/kernel/Makefile
@@ -9,14 +9,15 @@ AFLAGS_head.o		:= -DTEXT_OFFSET=$(TEXT_OFFSET)
 arm64-obj-y             := cputable.o debug-monitors.o entry.o irq.o fpsimd.o   \
                            entry-fpsimd.o process.o ptrace.o setup.o signal.o   \
                            sys.o stacktrace.o time.o traps.o io.o vdso.o        \
-                           hyp-stub.o
+                           hyp-stub.o psci.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 smp_psci.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
 
 obj-y                                   += $(arm64-obj-y) vdso/
 obj-m                                   += $(arm64-obj-m)
diff --git a/arch/arm64/kernel/early_printk.c b/arch/arm64/kernel/early_printk.c
new file mode 100644
index 000000000000..7e320a2edb9b
--- /dev/null
+++ b/arch/arm64/kernel/early_printk.c
@@ -0,0 +1,118 @@
+/*
+ * Earlyprintk support.
+ *
+ * Copyright (C) 2012 ARM Ltd.
+ * Author: Catalin Marinas <catalin.marinas@arm.com>
+ *
+ * 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/kernel.h>
+#include <linux/console.h>
+#include <linux/init.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/io.h>
+
+#include <linux/amba/serial.h>
+
+static void __iomem *early_base;
+static void (*printch)(char ch);
+
+/*
+ * PL011 single character TX.
+ */
+static void pl011_printch(char ch)
+{
+        while (readl_relaxed(early_base + UART01x_FR) & UART01x_FR_TXFF)
+                ;
+        writeb_relaxed(ch, early_base + UART01x_DR);
+        while (readl_relaxed(early_base + UART01x_FR) & UART01x_FR_BUSY)
+                ;
+}
+
+struct earlycon_match {
+        const char *name;
+        void (*printch)(char ch);
+};
+
+static const struct earlycon_match earlycon_match[] __initconst = {
+        { .name = "pl011", .printch = pl011_printch, },
+        {}
+};
+
+static void early_write(struct console *con, const char *s, unsigned n)
+{
+        while (n-- > 0) {
+                if (*s == '\n')
+                        printch('\r');
+                printch(*s);
+                s++;
+        }
+}
+
+static struct console early_console = {
+        .name =         "earlycon",
+        .write =        early_write,
+        .flags =        CON_PRINTBUFFER | CON_BOOT,
+        .index =        -1,
+};
+
+/*
+ * Parse earlyprintk=... parameter in the format:
+ *
+ *   <name>[,<addr>][,<options>]
+ *
+ * and register the early console. It is assumed that the UART has been
+ * initialised by the bootloader already.
+ */
+static int __init setup_early_printk(char *buf)
+{
+        const struct earlycon_match *match = earlycon_match;
+        phys_addr_t paddr = 0;
+
+        if (!buf) {
+                pr_warning("No earlyprintk arguments passed.\n");
+                return 0;
+        }
+
+        while (match->name) {
+                size_t len = strlen(match->name);
+                if (!strncmp(buf, match->name, len)) {
+                        buf += len;
+                        break;
+                }
+                match++;
+        }
+        if (!match->name) {
+                pr_warning("Unknown earlyprintk arguments: %s\n", buf);
+                return 0;
+        }
+
+        /* I/O address */
+        if (!strncmp(buf, ",0x", 3)) {
+                char *e;
+                paddr = simple_strtoul(buf + 1, &e, 16);
+                buf = e;
+        }
+        /* no options parsing yet */
+
+        if (paddr)
+                early_base = early_io_map(paddr, EARLYCON_IOBASE);
+
+        printch = match->printch;
+        register_console(&early_console);
+
+        return 0;
+}
+
+early_param("earlyprintk", setup_early_printk);
diff --git a/arch/arm64/kernel/head.S b/arch/arm64/kernel/head.S
index 368ad1f7c36c..0a0a49756826 100644
--- a/arch/arm64/kernel/head.S
+++ b/arch/arm64/kernel/head.S
@@ -82,10 +82,8 @@
 
 #ifdef CONFIG_ARM64_64K_PAGES
 #define MM_MMUFLAGS     PTE_ATTRINDX(MT_NORMAL) | PTE_FLAGS
-#define IO_MMUFLAGS     PTE_ATTRINDX(MT_DEVICE_nGnRE) | PTE_XN | PTE_FLAGS
 #else
 #define MM_MMUFLAGS     PMD_ATTRINDX(MT_NORMAL) | PMD_FLAGS
-#define IO_MMUFLAGS     PMD_ATTRINDX(MT_DEVICE_nGnRE) | PMD_SECT_XN | PMD_FLAGS
 #endif
 
 /*
@@ -368,6 +366,7 @@ ENDPROC(__calc_phys_offset)
  *   - identity mapping to enable the MMU (low address, TTBR0)
  *   - first few MB of the kernel linear mapping to jump to once the MMU has
  *     been enabled, including the FDT blob (TTBR1)
+ *   - UART mapping if CONFIG_EARLY_PRINTK is enabled (TTBR1)
  */
 __create_page_tables:
         pgtbl   x25, x26, x24                   // idmap_pg_dir and swapper_pg_dir addresses
@@ -420,6 +419,15 @@ __create_page_tables:
         sub     x6, x6, #1                      // inclusive range
         create_block_map x0, x7, x3, x5, x6
 1:
+#ifdef CONFIG_EARLY_PRINTK
+        /*
+         * Create the pgd entry for the UART mapping. The full mapping is done
+         * later based earlyprintk kernel parameter.
+         */
+        ldr     x5, =EARLYCON_IOBASE            // UART virtual address
+        add     x0, x26, #2 * PAGE_SIZE         // section table address
+        create_pgd_entry x26, x0, x5, x6, x7
+#endif
         ret
 ENDPROC(__create_page_tables)
         .ltorg
diff --git a/arch/arm64/kernel/perf_event.c b/arch/arm64/kernel/perf_event.c
index f7073c7b1ca9..1e49e5eb81e9 100644
--- a/arch/arm64/kernel/perf_event.c
+++ b/arch/arm64/kernel/perf_event.c
@@ -1331,6 +1331,11 @@ void perf_callchain_user(struct perf_callchain_entry *entry,
 {
         struct frame_tail __user *tail;
 
+        if (perf_guest_cbs && perf_guest_cbs->is_in_guest()) {
+                /* We don't support guest os callchain now */
+                return;
+        }
+
         tail = (struct frame_tail __user *)regs->regs[29];
 
         while (entry->nr < PERF_MAX_STACK_DEPTH &&
@@ -1355,8 +1360,40 @@ void perf_callchain_kernel(struct perf_callchain_entry *entry,
 {
         struct stackframe frame;
 
+        if (perf_guest_cbs && perf_guest_cbs->is_in_guest()) {
+                /* We don't support guest os callchain now */
+                return;
+        }
+
         frame.fp = regs->regs[29];
         frame.sp = regs->sp;
         frame.pc = regs->pc;
         walk_stackframe(&frame, callchain_trace, entry);
 }
+
+unsigned long perf_instruction_pointer(struct pt_regs *regs)
+{
+        if (perf_guest_cbs && perf_guest_cbs->is_in_guest())
+                return perf_guest_cbs->get_guest_ip();
+
+        return instruction_pointer(regs);
+}
+
+unsigned long perf_misc_flags(struct pt_regs *regs)
+{
+        int misc = 0;
+
+        if (perf_guest_cbs && perf_guest_cbs->is_in_guest()) {
+                if (perf_guest_cbs->is_user_mode())
+                        misc |= PERF_RECORD_MISC_GUEST_USER;
+                else
+                        misc |= PERF_RECORD_MISC_GUEST_KERNEL;
+        } else {
+                if (user_mode(regs))
+                        misc |= PERF_RECORD_MISC_USER;
+                else
+                        misc |= PERF_RECORD_MISC_KERNEL;
+        }
+
+        return misc;
+}
diff --git a/arch/arm64/kernel/process.c b/arch/arm64/kernel/process.c
index c7002d40a9b0..0337cdb0667b 100644
--- a/arch/arm64/kernel/process.c
+++ b/arch/arm64/kernel/process.c
@@ -45,9 +45,10 @@
 
 #include <asm/compat.h>
 #include <asm/cacheflush.h>
+#include <asm/fpsimd.h>
+#include <asm/mmu_context.h>
 #include <asm/processor.h>
 #include <asm/stacktrace.h>
-#include <asm/fpsimd.h>
 
 static void setup_restart(void)
 {
@@ -314,6 +315,7 @@ struct task_struct *__switch_to(struct task_struct *prev,
         /* the actual thread switch */
         last = cpu_switch_to(prev, next);
 
+        contextidr_thread_switch(next);
         return last;
 }
 
diff --git a/arch/arm64/kernel/psci.c b/arch/arm64/kernel/psci.c
new file mode 100644
index 000000000000..14f73c445ff5
--- /dev/null
+++ b/arch/arm64/kernel/psci.c
@@ -0,0 +1,211 @@
+/*
+ * 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.
+ *
+ * Copyright (C) 2013 ARM Limited
+ *
+ * Author: Will Deacon <will.deacon@arm.com>
+ */
+
+#define pr_fmt(fmt) "psci: " fmt
+
+#include <linux/init.h>
+#include <linux/of.h>
+
+#include <asm/compiler.h>
+#include <asm/errno.h>
+#include <asm/psci.h>
+
+struct psci_operations psci_ops;
+
+static int (*invoke_psci_fn)(u64, u64, u64, u64);
+
+enum psci_function {
+        PSCI_FN_CPU_SUSPEND,
+        PSCI_FN_CPU_ON,
+        PSCI_FN_CPU_OFF,
+        PSCI_FN_MIGRATE,
+        PSCI_FN_MAX,
+};
+
+static u32 psci_function_id[PSCI_FN_MAX];
+
+#define PSCI_RET_SUCCESS                0
+#define PSCI_RET_EOPNOTSUPP             -1
+#define PSCI_RET_EINVAL                 -2
+#define PSCI_RET_EPERM                  -3
+
+static int psci_to_linux_errno(int errno)
+{
+        switch (errno) {
+        case PSCI_RET_SUCCESS:
+                return 0;
+        case PSCI_RET_EOPNOTSUPP:
+                return -EOPNOTSUPP;
+        case PSCI_RET_EINVAL:
+                return -EINVAL;
+        case PSCI_RET_EPERM:
+                return -EPERM;
+        };
+
+        return -EINVAL;
+}
+
+#define PSCI_POWER_STATE_ID_MASK        0xffff
+#define PSCI_POWER_STATE_ID_SHIFT       0
+#define PSCI_POWER_STATE_TYPE_MASK      0x1
+#define PSCI_POWER_STATE_TYPE_SHIFT     16
+#define PSCI_POWER_STATE_AFFL_MASK      0x3
+#define PSCI_POWER_STATE_AFFL_SHIFT     24
+
+static u32 psci_power_state_pack(struct psci_power_state state)
+{
+        return  ((state.id & PSCI_POWER_STATE_ID_MASK)
+                        << PSCI_POWER_STATE_ID_SHIFT)   |
+                ((state.type & PSCI_POWER_STATE_TYPE_MASK)
+                        << PSCI_POWER_STATE_TYPE_SHIFT) |
+                ((state.affinity_level & PSCI_POWER_STATE_AFFL_MASK)
+                        << PSCI_POWER_STATE_AFFL_SHIFT);
+}
+
+/*
+ * The following two functions are invoked via the invoke_psci_fn pointer
+ * and will not be inlined, allowing us to piggyback on the AAPCS.
+ */
+static noinline int __invoke_psci_fn_hvc(u64 function_id, u64 arg0, u64 arg1,
+                                         u64 arg2)
+{
+        asm volatile(
+                        __asmeq("%0", "x0")
+                        __asmeq("%1", "x1")
+                        __asmeq("%2", "x2")
+                        __asmeq("%3", "x3")
+                        "hvc    #0\n"
+                : "+r" (function_id)
+                : "r" (arg0), "r" (arg1), "r" (arg2));
+
+        return function_id;
+}
+
+static noinline int __invoke_psci_fn_smc(u64 function_id, u64 arg0, u64 arg1,
+                                         u64 arg2)
+{
+        asm volatile(
+                        __asmeq("%0", "x0")
+                        __asmeq("%1", "x1")
+                        __asmeq("%2", "x2")
+                        __asmeq("%3", "x3")
+                        "smc    #0\n"
+                : "+r" (function_id)
+                : "r" (arg0), "r" (arg1), "r" (arg2));
+
+        return function_id;
+}
+
+static int psci_cpu_suspend(struct psci_power_state state,
+                            unsigned long entry_point)
+{
+        int err;
+        u32 fn, power_state;
+
+        fn = psci_function_id[PSCI_FN_CPU_SUSPEND];
+        power_state = psci_power_state_pack(state);
+        err = invoke_psci_fn(fn, power_state, entry_point, 0);
+        return psci_to_linux_errno(err);
+}
+
+static int psci_cpu_off(struct psci_power_state state)
+{
+        int err;
+        u32 fn, power_state;
+
+        fn = psci_function_id[PSCI_FN_CPU_OFF];
+        power_state = psci_power_state_pack(state);
+        err = invoke_psci_fn(fn, power_state, 0, 0);
+        return psci_to_linux_errno(err);
+}
+
+static int psci_cpu_on(unsigned long cpuid, unsigned long entry_point)
+{
+        int err;
+        u32 fn;
+
+        fn = psci_function_id[PSCI_FN_CPU_ON];
+        err = invoke_psci_fn(fn, cpuid, entry_point, 0);
+        return psci_to_linux_errno(err);
+}
+
+static int psci_migrate(unsigned long cpuid)
+{
+        int err;
+        u32 fn;
+
+        fn = psci_function_id[PSCI_FN_MIGRATE];
+        err = invoke_psci_fn(fn, cpuid, 0, 0);
+        return psci_to_linux_errno(err);
+}
+
+static const struct of_device_id psci_of_match[] __initconst = {
+        { .compatible = "arm,psci",     },
+        {},
+};
+
+int __init psci_init(void)
+{
+        struct device_node *np;
+        const char *method;
+        u32 id;
+        int err = 0;
+
+        np = of_find_matching_node(NULL, psci_of_match);
+        if (!np)
+                return -ENODEV;
+
+        pr_info("probing function IDs from device-tree\n");
+
+        if (of_property_read_string(np, "method", &method)) {
+                pr_warning("missing \"method\" property\n");
+                err = -ENXIO;
+                goto out_put_node;
+        }
+
+        if (!strcmp("hvc", method)) {
+                invoke_psci_fn = __invoke_psci_fn_hvc;
+        } else if (!strcmp("smc", method)) {
+                invoke_psci_fn = __invoke_psci_fn_smc;
+        } else {
+                pr_warning("invalid \"method\" property: %s\n", method);
+                err = -EINVAL;
+                goto out_put_node;
+        }
+
+        if (!of_property_read_u32(np, "cpu_suspend", &id)) {
+                psci_function_id[PSCI_FN_CPU_SUSPEND] = id;
+                psci_ops.cpu_suspend = psci_cpu_suspend;
+        }
+
+        if (!of_property_read_u32(np, "cpu_off", &id)) {
+                psci_function_id[PSCI_FN_CPU_OFF] = id;
+                psci_ops.cpu_off = psci_cpu_off;
+        }
+
+        if (!of_property_read_u32(np, "cpu_on", &id)) {
+                psci_function_id[PSCI_FN_CPU_ON] = id;
+                psci_ops.cpu_on = psci_cpu_on;
+        }
+
+        if (!of_property_read_u32(np, "migrate", &id)) {
+                psci_function_id[PSCI_FN_MIGRATE] = id;
+                psci_ops.migrate = psci_migrate;
+        }
+
+out_put_node:
+        of_node_put(np);
+        return err;
+}
diff --git a/arch/arm64/kernel/setup.c b/arch/arm64/kernel/setup.c
index 7665a9bfdb1e..113db863f832 100644
--- a/arch/arm64/kernel/setup.c
+++ b/arch/arm64/kernel/setup.c
@@ -39,6 +39,7 @@
 #include <linux/proc_fs.h>
 #include <linux/memblock.h>
 #include <linux/of_fdt.h>
+#include <linux/of_platform.h>
 
 #include <asm/cputype.h>
 #include <asm/elf.h>
@@ -49,6 +50,7 @@
 #include <asm/tlbflush.h>
 #include <asm/traps.h>
 #include <asm/memblock.h>
+#include <asm/psci.h>
 
 unsigned int processor_id;
 EXPORT_SYMBOL(processor_id);
@@ -260,6 +262,8 @@ void __init setup_arch(char **cmdline_p)
 
         unflatten_device_tree();
 
+        psci_init();
+
 #ifdef CONFIG_SMP
         smp_init_cpus();
 #endif
@@ -289,6 +293,13 @@ static int __init topology_init(void)
 }
 subsys_initcall(topology_init);
 
+static int __init arm64_device_probe(void)
+{
+        of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
+        return 0;
+}
+device_initcall(arm64_device_probe);
+
 static const char *hwcap_str[] = {
         "fp",
         "asimd",
diff --git a/arch/arm64/kernel/signal32.c b/arch/arm64/kernel/signal32.c
index a4db3d22aac4..41db148a7eb9 100644
--- a/arch/arm64/kernel/signal32.c
+++ b/arch/arm64/kernel/signal32.c
@@ -76,7 +76,7 @@ struct compat_sigcontext {
 
 struct compat_ucontext {
         compat_ulong_t                  uc_flags;
-        struct compat_ucontext          *uc_link;
+        compat_uptr_t                   uc_link;
         compat_stack_t                  uc_stack;
         struct compat_sigcontext        uc_mcontext;
         compat_sigset_t                 uc_sigmask;
@@ -703,7 +703,7 @@ int compat_setup_rt_frame(int usig, struct k_sigaction *ka, siginfo_t *info,
         err |= copy_siginfo_to_user32(&frame->info, info);
 
         __put_user_error(0, &frame->sig.uc.uc_flags, err);
-        __put_user_error(NULL, &frame->sig.uc.uc_link, err);
+        __put_user_error(0, &frame->sig.uc.uc_link, err);
 
         memset(&stack, 0, sizeof(stack));
         stack.ss_sp = (compat_uptr_t)current->sas_ss_sp;
diff --git a/arch/arm64/kernel/smp.c b/arch/arm64/kernel/smp.c
index 538300f2273d..bdd34597254b 100644
--- a/arch/arm64/kernel/smp.c
+++ b/arch/arm64/kernel/smp.c
@@ -233,7 +233,28 @@ 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,
+        &smp_psci_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 +273,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 +302,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 +311,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_psci.c b/arch/arm64/kernel/smp_psci.c
new file mode 100644
index 000000000000..112091684c22
--- /dev/null
+++ b/arch/arm64/kernel/smp_psci.c
@@ -0,0 +1,52 @@
+/*
+ * PSCI 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/psci.h>
+
+static int __init smp_psci_init_cpu(struct device_node *dn, int cpu)
+{
+        return 0;
+}
+
+static int __init smp_psci_prepare_cpu(int cpu)
+{
+        int err;
+
+        if (!psci_ops.cpu_on) {
+                pr_err("psci: no cpu_on method, not booting CPU%d\n", cpu);
+                return -ENODEV;
+        }
+
+        err = psci_ops.cpu_on(cpu, __pa(secondary_holding_pen));
+        if (err) {
+                pr_err("psci: failed to boot CPU%d (%d)\n", cpu, err);
+                return err;
+        }
+
+        return 0;
+}
+
+const struct smp_enable_ops smp_psci_ops __initconst = {
+        .name           = "psci",
+        .init_cpu       = smp_psci_init_cpu,
+        .prepare_cpu    = smp_psci_prepare_cpu,
+};
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,
+};