3 files changed, 29 insertions, 0 deletions

diff --git a/arch/s390/include/asm/smp.h b/arch/s390/include/asm/smp.h
index 045e009fc164..ab47a69fdf07 100644
--- a/arch/s390/include/asm/smp.h
+++ b/arch/s390/include/asm/smp.h
@@ -33,6 +33,7 @@ extern struct save_area *zfcpdump_save_areas[NR_CPUS + 1];
 extern void smp_switch_to_ipl_cpu(void (*func)(void *), void *);
 extern void smp_switch_to_cpu(void (*)(void *), void *, unsigned long sp,
                               int from, int to);
+extern void smp_restart_with_online_cpu(void);
 extern void smp_restart_cpu(void);
 
 /*
@@ -64,6 +65,10 @@ static inline void smp_switch_to_ipl_cpu(void (*func)(void *), void *data)
         func(data);
 }
 
+static inline void smp_restart_with_online_cpu(void)
+{
+}
+
 #define smp_vcpu_scheduled      (1)
 
 #endif /* CONFIG_SMP */
diff --git a/arch/s390/kernel/ipl.c b/arch/s390/kernel/ipl.c
index 48c710206366..90769b4bc7f6 100644
--- a/arch/s390/kernel/ipl.c
+++ b/arch/s390/kernel/ipl.c
@@ -1738,6 +1738,7 @@ static struct kobj_attribute on_restart_attr =
 
 void do_restart(void)
 {
+        smp_restart_with_online_cpu();
         smp_send_stop();
         on_restart_trigger.action->fn(&on_restart_trigger);
         stop_run(&on_restart_trigger);
diff --git a/arch/s390/kernel/smp.c b/arch/s390/kernel/smp.c
index 6ab16ac64d29..e4572601e91e 100644
--- a/arch/s390/kernel/smp.c
+++ b/arch/s390/kernel/smp.c
@@ -97,6 +97,29 @@ static inline int cpu_stopped(int cpu)
         return raw_cpu_stopped(cpu_logical_map(cpu));
 }
 
+/*
+ * Ensure that PSW restart is done on an online CPU
+ */
+void smp_restart_with_online_cpu(void)
+{
+        int cpu;
+
+        for_each_online_cpu(cpu) {
+                if (stap() == __cpu_logical_map[cpu]) {
+                        /* We are online: Enable DAT again and return */
+                        __load_psw_mask(psw_kernel_bits & ~PSW_MASK_MCHECK);
+                        return;
+                }
+        }
+        /* We are not online: Do PSW restart on an online CPU */
+        while (sigp(cpu, sigp_restart) == sigp_busy)
+                cpu_relax();
+        /* And stop ourself */
+        while (raw_sigp(stap(), sigp_stop) == sigp_busy)
+                cpu_relax();
+        for (;;);
+}
+
 void smp_switch_to_ipl_cpu(void (*func)(void *), void *data)
 {
         struct _lowcore *lc, *current_lc;