[Skiboot] [PATCH 29/34] cpu: Add support for nap mode on P8

[Benjamin Herrenschmidt]

This allows us to send threads to nap mode when either idle (waiting
for a job) or when in a sleep delay (time_wait*).

We only enable the functionality after the 0x100 vector has been
patched, and we disable it before transferring control to Linux.

Signed-off-by: Benjamin Herrenschmidt <benh at kernel.crashing.org>
---
 core/cpu.c    | 93 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 core/init.c   | 10 ++++++-
 include/cpu.h |  4 +++
 3 files changed, 106 insertions(+), 1 deletion(-)

diff --git a/core/cpu.c b/core/cpu.c
index 3f2ebd9..6cda5d0 100644
--- a/core/cpu.c
+++ b/core/cpu.c
@@ -28,6 +28,7 @@
 #include <affinity.h>
 #include <chip.h>
 #include <timebase.h>
+#include <interrupts.h>
 #include <ccan/str/str.h>
 #include <ccan/container_of/container_of.h>
 
@@ -49,6 +50,7 @@ static struct lock reinit_lock = LOCK_UNLOCKED;
 static bool hile_supported;
 static unsigned long hid0_hile;
 static unsigned long hid0_attn;
+static bool pm_enabled;
 
 unsigned long cpu_secondary_start __force_data = 0;
 
@@ -79,6 +81,17 @@ unsigned long __attrconst cpu_stack_top(unsigned int pir)
 		NORMAL_STACK_SIZE - STACK_TOP_GAP;
 }
 
+static void cpu_wake(struct cpu_thread *cpu)
+{
+	/* Is it idle ? If not, no need to wake */
+	sync();
+	if (!cpu->in_idle)
+		return;
+
+	/* Poke IPI */
+	icp_kick_cpu(cpu);
+}
+
 static struct cpu_thread *cpu_find_job_target(void)
 {
 	struct cpu_thread *cpu, *best, *me = this_cpu();
@@ -194,6 +207,8 @@ struct cpu_job *__cpu_queue_job(struct cpu_thread *cpu,
 		cpu->job_has_no_return = true;
 	else
 		cpu->job_count++;
+	if (pm_enabled)
+		cpu_wake(cpu);
 	unlock(&cpu->job_lock);
 
 	return job;
@@ -279,9 +294,87 @@ static void cpu_idle_default(enum cpu_wake_cause wake_on __unused)
 	cpu_relax();
 }
 
+static void cpu_idle_p8(enum cpu_wake_cause wake_on)
+{
+	uint64_t lpcr = mfspr(SPR_LPCR) & ~SPR_LPCR_P8_PECE;
+	struct cpu_thread *cpu = this_cpu();
+
+	if (!pm_enabled) {
+		cpu_idle_default(wake_on);
+		return;
+	}
+
+	/* If we are waking on job, whack DEC to highest value */
+	if (wake_on == cpu_wake_on_job)
+		mtspr(SPR_DEC, 0x7fffffff);
+
+	/* Clean up ICP, be ready for IPIs */
+	icp_prep_for_pm();
+
+	/* Setup wakup cause in LPCR */
+	lpcr |= SPR_LPCR_P8_PECE2 | SPR_LPCR_P8_PECE3;
+	mtspr(SPR_LPCR, lpcr);
+
+	/* Synchronize with wakers */
+	if (wake_on == cpu_wake_on_job) {
+		/* Mark ourselves in idle so other CPUs know to send an IPI */
+		cpu->in_idle = true;
+		sync();
+
+		/* Check for jobs again */
+		if (cpu_check_jobs(cpu) || !pm_enabled)
+			goto skip_sleep;
+	} else {
+		/* Mark outselves sleeping so cpu_set_pm_enable knows to
+		 * send an IPI
+		 */
+		cpu->in_sleep = true;
+		sync();
+
+		/* Check if PM got disabled */
+		if (!pm_enabled)
+			goto skip_sleep;
+	}
+
+	/* Enter nap */
+	enter_pm_state(false);
+
+skip_sleep:
+	/* Restore */
+	sync();
+	cpu->in_idle = false;
+	cpu->in_sleep = false;
+	reset_cpu_icp();
+}
+
+void cpu_set_pm_enable(bool enabled)
+{
+	struct cpu_thread *cpu;
+
+	prlog(PR_INFO, "CPU: %sing power management\n",
+	      enabled ? "enabl" : "disabl");
+
+	pm_enabled = enabled;
+
+	if (enabled)
+		return;
+
+	/* If disabling, take everybody out of PM */
+	sync();
+	for_each_available_cpu(cpu) {
+		while (cpu->in_sleep || cpu->in_idle) {
+			icp_kick_cpu(cpu);
+			cpu_relax();
+		}
+	}
+}
+
 void cpu_idle(enum cpu_wake_cause wake_on)
 {
 	switch(proc_gen) {
+	case proc_gen_p8:
+		cpu_idle_p8(wake_on);
+		break;
 	default:
 		cpu_idle_default(wake_on);
 		break;
diff --git a/core/init.c b/core/init.c
index 237ccfb..1cf4cc2 100644
--- a/core/init.c
+++ b/core/init.c
@@ -360,8 +360,10 @@ static bool load_kernel(void)
 		 * If the kernel is at 0, restore it as it was overwritten
 		 * by our vectors.
 		 */
-		if (kernel_entry < 0x2000)
+		if (kernel_entry < 0x2000) {
+			cpu_set_pm_enable(false);
 			memcpy(NULL, old_vectors, 0x2000);
+		}
 	} else {
 		if (!kernel_size)
 			printf("INIT: Assuming kernel at %p\n",
@@ -478,6 +480,9 @@ void __noreturn load_and_boot_kernel(bool is_reboot)
 
 	mem_dump_free();
 
+	/* Take processours out of nap */
+	cpu_set_pm_enable(false);
+
 	printf("INIT: Starting kernel at 0x%llx, fdt at %p (size 0x%x)\n",
 	       kernel_entry, fdt, fdt_totalsize(fdt));
 
@@ -765,6 +770,9 @@ void __noreturn __nomcount main_cpu_entry(const void *fdt, u32 master_cpu)
 	 */
 	setup_reset_vector();
 
+	/* We can now do NAP mode */
+	cpu_set_pm_enable(true);
+
 	/*
 	 * Sycnhronize time bases. Thi resets all the TB values to a small
 	 * value (so they appear to go backward at this point), and synchronize
diff --git a/include/cpu.h b/include/cpu.h
index 4164151..341e73d 100644
--- a/include/cpu.h
+++ b/include/cpu.h
@@ -66,6 +66,8 @@ struct cpu_thread {
 	bool				in_mcount;
 	bool				in_poller;
 	bool				in_reinit;
+	bool				in_sleep;
+	bool				in_idle;
 	uint32_t			hbrt_spec_wakeup; /* primary only */
 	uint64_t			save_l2_fir_action1;
 	uint64_t			current_token;
@@ -251,6 +253,8 @@ extern void cpu_process_jobs(void);
 extern void cpu_process_local_jobs(void);
 /* Check if there's any job pending */
 bool cpu_check_jobs(struct cpu_thread *cpu);
+/* Enable/disable PM */
+void cpu_set_pm_enable(bool pm_enabled);
 
 static inline void cpu_give_self_os(void)
 {
-- 
2.7.4