From 40bdd0d98d711ade57e8ffde8ed5c9d09166452a Mon Sep 17 00:00:00 2001
From: Nathan Lynch <nathanl@linux.ibm.com>
Date: Tue, 10 Oct 2023 23:12:11 -0500
Subject: [PATCH] powerpc/pseries: implement freeze|thaw_secondary smp ops

This currently is hanging in the suspend path with:

  # cd /sys/power/
  # echo 1 > pm_debug_messages
  # echo core > pm_test
  # echo mem >state
  [   28.597952] PM: suspend entry (deep)
  [   28.604185] Filesystems sync: 0.005 seconds
  [   28.640432] Freezing user space processes
  [   28.644731] Freezing user space processes completed (elapsed 0.003 seconds)
  [   28.645398] OOM killer disabled.
  [   28.645579] Freezing remaining freezable tasks
  [   28.650206] Freezing remaining freezable tasks completed (elapsed 0.004 seconds)
  [   28.825795] PM: suspend devices took 0.170 seconds
  [   28.891106] Disabling non-boot CPUs ...
  [   28.897927] premature return from H_JOIN on CPU 2, retrying
  [   28.897943] premature return from H_JOIN on CPU 4, retrying
  [   28.898060] premature return from H_JOIN on CPU 1, retrying
  [   28.898071] premature return from H_JOIN on CPU 3, retrying
  [   28.898082] premature return from H_JOIN on CPU 5, retrying
  [   28.898096] premature return from H_JOIN on CPU 4, retrying
  [   28.898451] premature return from H_JOIN on CPU 3, retrying
  [   28.898469] premature return from H_JOIN on CPU 4, retrying
  [   28.898532] premature return from H_JOIN on CPU 2, retrying
  [   28.898595] premature return from H_JOIN on CPU 4, retrying
  [   33.897589] do_join: 932331 callbacks suppressed
---
 arch/powerpc/platforms/pseries/smp.c | 76 ++++++++++++++++++++++++++++
 1 file changed, 76 insertions(+)

diff --git a/arch/powerpc/platforms/pseries/smp.c b/arch/powerpc/platforms/pseries/smp.c
index c597711ef20a2..f05e3b4053cda 100644
--- a/arch/powerpc/platforms/pseries/smp.c
+++ b/arch/powerpc/platforms/pseries/smp.c
@@ -239,6 +239,78 @@ static __init void pSeries_smp_probe(void)
 	smp_ops->cause_ipi = dbell_or_ic_cause_ipi;
 }
 
+static void do_join(void *arg)
+{
+	cpumask_t *mask = arg;
+
+	cpumask_clear_cpu(smp_processor_id(), mask);
+
+	while (1) {
+		long hvrc;
+
+		hard_irq_disable();
+		hvrc = plpar_hcall_norets(H_JOIN);
+
+		switch (hvrc) {
+		case H_SUCCESS:
+			/*
+			 * The suspend is complete and this cpu has received a
+			 * prod, or we've received a stray prod from unrelated
+			 * code (e.g. paravirt spinlocks) and we need to join
+			 * again.
+			 *
+			 * This barrier orders the return from H_JOIN above vs
+			 * the load of info->done. It pairs with the barrier
+			 * in the wakeup/prod path below.
+			 */
+			smp_mb();
+			if (!cpumask_test_cpu(smp_processor_id(), mask)) {
+				pr_info_ratelimited("premature return from H_JOIN on CPU %i, retrying",
+						    smp_processor_id());
+				continue;
+			}
+			break;
+		case H_CONTINUE:
+		case H_BAD_MODE:
+		case H_HARDWARE:
+		default:
+			pr_err_ratelimited("Unexpected H_JOIN result %ld on CPU %i\n",
+					   hvrc, smp_processor_id());
+			break;
+		}
+
+		return;
+	}
+}
+
+static cpumask_var_t prod_mask;
+
+static int pseries_cpu_join(int cpu)
+{
+	cpumask_set_cpu(cpu, prod_mask);
+	smp_call_function_single(cpu, do_join, &prod_mask, 0);
+	while (cpumask_test_cpu(cpu, prod_mask))
+		cpu_relax();
+	return 0;
+}
+
+static int pseries_cpu_prod(int target_cpu)
+{
+	long hvrc;
+	int hwid;
+
+	hwid = get_hard_smp_processor_id(target_cpu);
+	cpumask_set_cpu(target_cpu, prod_mask);
+	smp_mb();
+	hvrc = plpar_hcall_norets(H_PROD, hwid);
+	if (hvrc == H_SUCCESS)
+		return 0;
+	pr_err_ratelimited("H_PROD of CPU %u (hwid %d) error: %ld\n",
+			   target_cpu, hwid, hvrc);
+
+	return -EIO;
+}
+
 static struct smp_ops_t pseries_smp_ops = {
 	.message_pass	= NULL,	/* Use smp_muxed_ipi_message_pass */
 	.cause_ipi	= NULL,	/* Filled at runtime by pSeries_smp_probe() */
@@ -248,6 +320,10 @@ static struct smp_ops_t pseries_smp_ops = {
 	.kick_cpu	= smp_pSeries_kick_cpu,
 	.setup_cpu	= smp_setup_cpu,
 	.cpu_bootable	= smp_generic_cpu_bootable,
+#ifdef CONFIG_PM_SLEEP_SMP
+	.freeze_secondary = pseries_cpu_join,
+	.thaw_secondary = pseries_cpu_prod,
+#endif
 };
 
 /* This is called very early */