diff --git a/kernel/stop_machine.c b/kernel/stop_machine.c
index 688d6b3..91fbb10 100644
--- a/kernel/stop_machine.c
+++ b/kernel/stop_machine.c
@@ -219,12 +219,27 @@
 static int cpu_stop_queue_two_works(int cpu1, struct cpu_stop_work *work1,
 				    int cpu2, struct cpu_stop_work *work2)
 {
+	struct cpu_stopper *stopper1 = per_cpu_ptr(&cpu_stopper, cpu1);
+	struct cpu_stopper *stopper2 = per_cpu_ptr(&cpu_stopper, cpu2);
+	int err;
+
 	lg_double_lock(&stop_cpus_lock, cpu1, cpu2);
-	cpu_stop_queue_work(cpu1, work1);
-	cpu_stop_queue_work(cpu2, work2);
+	spin_lock_irq(&stopper1->lock);
+	spin_lock_nested(&stopper2->lock, SINGLE_DEPTH_NESTING);
+
+	err = -ENOENT;
+	if (!stopper1->enabled || !stopper2->enabled)
+		goto unlock;
+
+	err = 0;
+	__cpu_stop_queue_work(stopper1, work1);
+	__cpu_stop_queue_work(stopper2, work2);
+unlock:
+	spin_unlock(&stopper2->lock);
+	spin_unlock_irq(&stopper1->lock);
 	lg_double_unlock(&stop_cpus_lock, cpu1, cpu2);
 
-	return 0;
+	return err;
 }
 /**
  * stop_two_cpus - stops two cpus
@@ -261,12 +276,8 @@
 	set_state(&msdata, MULTI_STOP_PREPARE);
 
 	/*
-	 * If we observe both CPUs active we know _cpu_down() cannot yet have
-	 * queued its stop_machine works and therefore ours will get executed
-	 * first. Or its not either one of our CPUs that's getting unplugged,
-	 * in which case we don't care.
-	 *
-	 * This relies on the stopper workqueues to be FIFO.
+	 * We do not want to migrate to inactive CPU. FIXME: move this
+	 * into migrate_swap_stop() callback.
 	 */
 	if (!cpu_active(cpu1) || !cpu_active(cpu2)) {
 		preempt_enable();