1 files changed, 11 insertions, 8 deletions

diff --git a/arch/arm/mach-socfpga/platsmp.c b/arch/arm/mach-socfpga/platsmp.c
index 16ca97b039f9..c64d89b7c0ca 100644
--- 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 = {