6 files changed, 72 insertions, 82 deletions

diff --git a/kernel/power/Kconfig b/kernel/power/Kconfig
index 696387ffe49c..fdb377636505 100644
--- a/kernel/power/Kconfig
+++ b/kernel/power/Kconfig
@@ -28,7 +28,7 @@ config PM_DEBUG
 
 config SOFTWARE_SUSPEND
         bool "Software Suspend (EXPERIMENTAL)"
-        depends on EXPERIMENTAL && PM && SWAP
+        depends on EXPERIMENTAL && PM && SWAP && (SUSPEND_SMP || !SMP)
         ---help---
           Enable the possibility of suspending the machine.
           It doesn't need APM.
@@ -72,3 +72,7 @@ config PM_STD_PARTITION
           suspended image to. It will simply pick the first available swap 
           device.
 
+config SUSPEND_SMP
+        bool
+        depends on HOTPLUG_CPU && X86 && PM
+        default y
diff --git a/kernel/power/Makefile b/kernel/power/Makefile
index fbdc634135a7..2f438d0eaa13 100644
--- a/kernel/power/Makefile
+++ b/kernel/power/Makefile
@@ -3,9 +3,9 @@ ifeq ($(CONFIG_PM_DEBUG),y)
 EXTRA_CFLAGS    +=      -DDEBUG
 endif
 
-swsusp-smp-$(CONFIG_SMP)        += smp.o
-
 obj-y                           := main.o process.o console.o pm.o
-obj-$(CONFIG_SOFTWARE_SUSPEND)  += swsusp.o $(swsusp-smp-y) disk.o
+obj-$(CONFIG_SOFTWARE_SUSPEND)  += swsusp.o disk.o
+
+obj-$(CONFIG_SUSPEND_SMP)       += smp.o
 
 obj-$(CONFIG_MAGIC_SYSRQ)       += poweroff.o
diff --git a/kernel/power/disk.c b/kernel/power/disk.c
index 02b6764034dc..fb8de63c2919 100644
--- a/kernel/power/disk.c
+++ b/kernel/power/disk.c
@@ -117,8 +117,8 @@ static void finish(void)
 {
         device_resume();
         platform_finish();
-        enable_nonboot_cpus();
         thaw_processes();
+        enable_nonboot_cpus();
         pm_restore_console();
 }
 
@@ -131,28 +131,35 @@ static int prepare_processes(void)
 
         sys_sync();
 
+        disable_nonboot_cpus();
+
         if (freeze_processes()) {
                 error = -EBUSY;
-                return error;
+                goto thaw;
         }
 
         if (pm_disk_mode == PM_DISK_PLATFORM) {
                 if (pm_ops && pm_ops->prepare) {
                         if ((error = pm_ops->prepare(PM_SUSPEND_DISK)))
-                                return error;
+                                goto thaw;
                 }
         }
 
         /* Free memory before shutting down devices. */
         free_some_memory();
-
         return 0;
+thaw:
+        thaw_processes();
+        enable_nonboot_cpus();
+        pm_restore_console();
+        return error;
 }
 
 static void unprepare_processes(void)
 {
-        enable_nonboot_cpus();
+        platform_finish();
         thaw_processes();
+        enable_nonboot_cpus();
         pm_restore_console();
 }
 
@@ -160,15 +167,9 @@ static int prepare_devices(void)
 {
         int error;
 
-        disable_nonboot_cpus();
-        if ((error = device_suspend(PMSG_FREEZE))) {
+        if ((error = device_suspend(PMSG_FREEZE)))
                 printk("Some devices failed to suspend\n");
-                platform_finish();
-                enable_nonboot_cpus();
-                return error;
-        }
-
-        return 0;
+        return error;
 }
 
 /**
@@ -185,9 +186,9 @@ int pm_suspend_disk(void)
         int error;
 
         error = prepare_processes();
-        if (!error) {
-                error = prepare_devices();
-        }
+        if (error)
+                return error;
+        error = prepare_devices();
 
         if (error) {
                 unprepare_processes();
@@ -250,7 +251,7 @@ static int software_resume(void)
 
         if ((error = prepare_processes())) {
                 swsusp_close();
-                goto Cleanup;
+                goto Done;
         }
 
         pr_debug("PM: Reading swsusp image.\n");
diff --git a/kernel/power/main.c b/kernel/power/main.c
index 4cdebc972ff2..c94cb9e95090 100644
--- a/kernel/power/main.c
+++ b/kernel/power/main.c
@@ -55,6 +55,13 @@ static int suspend_prepare(suspend_state_t state)
 
         pm_prepare_console();
 
+        disable_nonboot_cpus();
+
+        if (num_online_cpus() != 1) {
+                error = -EPERM;
+                goto Enable_cpu;
+        }
+
         if (freeze_processes()) {
                 error = -EAGAIN;
                 goto Thaw;
@@ -75,6 +82,8 @@ static int suspend_prepare(suspend_state_t state)
                 pm_ops->finish(state);
  Thaw:
         thaw_processes();
+ Enable_cpu:
+        enable_nonboot_cpus();
         pm_restore_console();
         return error;
 }
@@ -113,6 +122,7 @@ static void suspend_finish(suspend_state_t state)
         if (pm_ops && pm_ops->finish)
                 pm_ops->finish(state);
         thaw_processes();
+        enable_nonboot_cpus();
         pm_restore_console();
 }
 
@@ -150,12 +160,6 @@ static int enter_state(suspend_state_t state)
                 goto Unlock;
         }
 
-        /* Suspend is hard to get right on SMP. */
-        if (num_online_cpus() != 1) {
-                error = -EPERM;
-                goto Unlock;
-        }
-
         pr_debug("PM: Preparing system for %s sleep\n", pm_states[state]);
         if ((error = suspend_prepare(state)))
                 goto Unlock;
diff --git a/kernel/power/smp.c b/kernel/power/smp.c
index 457c2302ed42..bbe23079c62c 100644
--- a/kernel/power/smp.c
+++ b/kernel/power/smp.c
@@ -13,73 +13,52 @@
 #include <linux/interrupt.h>
 #include <linux/suspend.h>
 #include <linux/module.h>
+#include <linux/cpu.h>
 #include <asm/atomic.h>
 #include <asm/tlbflush.h>
 
-static atomic_t cpu_counter, freeze;
-
-
-static void smp_pause(void * data)
-{
-        struct saved_context ctxt;
-        __save_processor_state(&ctxt);
-        printk("Sleeping in:\n");
-        dump_stack();
-        atomic_inc(&cpu_counter);
-        while (atomic_read(&freeze)) {
-                /* FIXME: restore takes place at random piece inside this.
-                   This should probably be written in assembly, and
-                   preserve general-purpose registers, too
-
-                   What about stack? We may need to move to new stack here.
-
-                   This should better be ran with interrupts disabled.
-                 */
-                cpu_relax();
-                barrier();
-        }
-        atomic_dec(&cpu_counter);
-        __restore_processor_state(&ctxt);
-}
-
-static cpumask_t oldmask;
+/* This is protected by pm_sem semaphore */
+static cpumask_t frozen_cpus;
 
 void disable_nonboot_cpus(void)
 {
-        oldmask = current->cpus_allowed;
-        set_cpus_allowed(current, cpumask_of_cpu(0));
-        printk("Freezing CPUs (at %d)", raw_smp_processor_id());
-        current->state = TASK_INTERRUPTIBLE;
-        schedule_timeout(HZ);
-        printk("...");
-        BUG_ON(raw_smp_processor_id() != 0);
-
-        /* FIXME: for this to work, all the CPUs must be running
-         * "idle" thread (or we deadlock). Is that guaranteed? */
+        int cpu, error;
 
-        atomic_set(&cpu_counter, 0);
-        atomic_set(&freeze, 1);
-        smp_call_function(smp_pause, NULL, 0, 0);
-        while (atomic_read(&cpu_counter) < (num_online_cpus() - 1)) {
-                cpu_relax();
-                barrier();
+        error = 0;
+        cpus_clear(frozen_cpus);
+        printk("Freezing cpus ...\n");
+        for_each_online_cpu(cpu) {
+                if (cpu == 0)
+                        continue;
+                error = cpu_down(cpu);
+                if (!error) {
+                        cpu_set(cpu, frozen_cpus);
+                        printk("CPU%d is down\n", cpu);
+                        continue;
+                }
+                printk("Error taking cpu %d down: %d\n", cpu, error);
         }
-        printk("ok\n");
+        BUG_ON(smp_processor_id() != 0);
+        if (error)
+                panic("cpus not sleeping");
 }
 
 void enable_nonboot_cpus(void)
 {
-        printk("Restarting CPUs");
-        atomic_set(&freeze, 0);
-        while (atomic_read(&cpu_counter)) {
-                cpu_relax();
-                barrier();
-        }
-        printk("...");
-        set_cpus_allowed(current, oldmask);
-        schedule();
-        printk("ok\n");
+        int cpu, error;
 
+        printk("Thawing cpus ...\n");
+        for_each_cpu_mask(cpu, frozen_cpus) {
+                error = smp_prepare_cpu(cpu);
+                if (!error)
+                        error = cpu_up(cpu);
+                if (!error) {
+                        printk("CPU%d is up\n", cpu);
+                        continue;
+                }
+                printk("Error taking cpu %d up: %d\n", cpu, error);
+                panic("Not enough cpus");
+        }
+        cpus_clear(frozen_cpus);
 }
 
-
diff --git a/kernel/power/swsusp.c b/kernel/power/swsusp.c
index 53f9f8720ee4..339b5c3735bd 100644
--- a/kernel/power/swsusp.c
+++ b/kernel/power/swsusp.c
@@ -1193,8 +1193,10 @@ static const char * sanity_check(void)
                 return "version";
         if (strcmp(swsusp_info.uts.machine,system_utsname.machine))
                 return "machine";
+#if 0
         if(swsusp_info.cpus != num_online_cpus())
                 return "number of cpus";
+#endif
         return NULL;
 }