1 files changed, 59 insertions, 22 deletions

diff --git a/arch/arm/kernel/process.c b/arch/arm/kernel/process.c
index 3d0c6fb74ae4..423bb2019451 100644
--- a/arch/arm/kernel/process.c
+++ b/arch/arm/kernel/process.c
@@ -57,7 +57,7 @@ static const char *isa_modes[] = {
   "ARM" , "Thumb" , "Jazelle", "ThumbEE"
 };
 
-extern void setup_mm_for_reboot(char mode);
+extern void setup_mm_for_reboot(void);
 
 static volatile int hlt_counter;
 
@@ -92,18 +92,24 @@ static int __init hlt_setup(char *__unused)
 __setup("nohlt", nohlt_setup);
 __setup("hlt", hlt_setup);
 
-void arm_machine_restart(char mode, const char *cmd)
+extern void call_with_stack(void (*fn)(void *), void *arg, void *sp);
+typedef void (*phys_reset_t)(unsigned long);
+
+/*
+ * A temporary stack to use for CPU reset. This is static so that we
+ * don't clobber it with the identity mapping. When running with this
+ * stack, any references to the current task *will not work* so you
+ * should really do as little as possible before jumping to your reset
+ * code.
+ */
+static u64 soft_restart_stack[16];
+
+static void __soft_restart(void *addr)
 {
-        /* Disable interrupts first */
-        local_irq_disable();
-        local_fiq_disable();
+        phys_reset_t phys_reset;
 
-        /*
-         * Tell the mm system that we are going to reboot -
-         * we may need it to insert some 1:1 mappings so that
-         * soft boot works.
-         */
-        setup_mm_for_reboot(mode);
+        /* Take out a flat memory mapping. */
+        setup_mm_for_reboot();
 
         /* Clean and invalidate caches */
         flush_cache_all();
@@ -114,18 +120,41 @@ void arm_machine_restart(char mode, const char *cmd)
         /* Push out any further dirty data, and ensure cache is empty */
         flush_cache_all();
 
-        /*
-         * Now call the architecture specific reboot code.
-         */
-        arch_reset(mode, cmd);
+        /* Switch to the identity mapping. */
+        phys_reset = (phys_reset_t)(unsigned long)virt_to_phys(cpu_reset);
+        phys_reset((unsigned long)addr);
 
-        /*
-         * Whoops - the architecture was unable to reboot.
-         * Tell the user!
-         */
-        mdelay(1000);
-        printk("Reboot failed -- System halted\n");
-        while (1);
+        /* Should never get here. */
+        BUG();
+}
+
+void soft_restart(unsigned long addr)
+{
+        u64 *stack = soft_restart_stack + ARRAY_SIZE(soft_restart_stack);
+
+        /* Disable interrupts first */
+        local_irq_disable();
+        local_fiq_disable();
+
+        /* Disable the L2 if we're the last man standing. */
+        if (num_online_cpus() == 1)
+                outer_disable();
+
+        /* Change to the new stack and continue with the reset. */
+        call_with_stack(__soft_restart, (void *)addr, (void *)stack);
+
+        /* Should never get here. */
+        BUG();
+}
+
+void arm_machine_restart(char mode, const char *cmd)
+{
+        /* Disable interrupts first */
+        local_irq_disable();
+        local_fiq_disable();
+
+        /* Call the architecture specific reboot code. */
+        arch_reset(mode, cmd);
 }
 
 /*
@@ -253,7 +282,15 @@ void machine_power_off(void)
 void machine_restart(char *cmd)
 {
         machine_shutdown();
+
         arm_pm_restart(reboot_mode, cmd);
+
+        /* Give a grace period for failure to restart of 1s */
+        mdelay(1000);
+
+        /* Whoops - the platform was unable to reboot. Tell the user! */
+        printk("Reboot failed -- System halted\n");
+        while (1);
 }
 
 void __show_regs(struct pt_regs *regs)