[S390] Force PSW restart on online CPU

PSW restart can be triggered on offline CPUs. If this happens, currently
the PSW restart code fails, because functions like smp_processor_id()
do not work on offline CPUs. This patch fixes this as follows:

If PSW restart is triggered on an offline CPU, the PSW restart (sigp restart)
is done a second time on another CPU that is online and the old CPU is
stopped afterwards.

Signed-off-by: Michael Holzheu <[e-mail]>
Signed-off-by: Martin Schwidefsky <[e-mail]>

arch/s390/include/asm/smp.h

···
 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);
 
 /*
···
 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)

arch/s390/kernel/ipl.c

···
 
 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);

arch/s390/kernel/smp.c

···
 	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;