Merge tag 'pinctrl-v3.19-1' of git://git.kernel.org/pub/scm/linux/kernel/git/linusw...

[cascardo/linux.git] / arch / arm / mach-socfpga / platsmp.c

diff --git a/arch/arm/mach-socfpga/platsmp.c b/arch/arm/mach-socfpga/platsmp.c
index 16ca97b..c64d89b 100644 (file)
--- a/arch/arm/mach-socfpga/platsmp.c
+++ b/arch/arm/mach-socfpga/platsmp.c
@@ -34,17 +34,21 @@ static int socfpga_boot_secondary(unsigned int cpu, struct task_struct *idle)
        int trampoline_size = &secondary_trampoline_end - &secondary_trampoline;
 
        if (socfpga_cpu1start_addr) {
+               /* This will put CPU #1 into reset. */
+               writel(RSTMGR_MPUMODRST_CPU1,
+                      rst_manager_base_addr + SOCFPGA_RSTMGR_MODMPURST);
+
                memcpy(phys_to_virt(0), &secondary_trampoline, trampoline_size);
 
-               __raw_writel(virt_to_phys(socfpga_secondary_startup),
-                       (sys_manager_base_addr + (socfpga_cpu1start_addr & 0x000000ff)));
+               writel(virt_to_phys(socfpga_secondary_startup),
+                      sys_manager_base_addr + (socfpga_cpu1start_addr & 0x000000ff));
 
                flush_cache_all();
                smp_wmb();
                outer_clean_range(0, trampoline_size);
 
-               /* This will release CPU #1 out of reset.*/
-               __raw_writel(0, rst_manager_base_addr + 0x10);
+               /* This will release CPU #1 out of reset. */
+               writel(0, rst_manager_base_addr + SOCFPGA_RSTMGR_MODMPURST);
        }
 
        return 0;
@@ -86,10 +90,9 @@ static void __init socfpga_smp_prepare_cpus(unsigned int max_cpus)
  */
 static void socfpga_cpu_die(unsigned int cpu)
 {
-       cpu_do_idle();
-
-       /* We should have never returned from idle */
-       panic("cpu %d unexpectedly exit from shutdown\n", cpu);
+       /* Do WFI. If we wake up early, go back into WFI */
+       while (1)
+               cpu_do_idle();
 }
 
 struct smp_operations socfpga_smp_ops __initdata = {