From b1d4c6cac02808b1d4e84d0187dc6014bffd2446 Mon Sep 17 00:00:00 2001
From: James Hogan <james.hogan@imgtec.com>
Date: Tue, 4 Mar 2014 10:11:39 +0000
Subject: [PATCH] MIPS: PM: Add CPU PM callbacks for general CPU context

Add a CPU power management notifier callback for preserving general CPU
context. The CPU PM callbacks will be triggered by the powering down of
CPU cores, for example by cpuidle drivers & in the future by suspend to
RAM implementations.

The current state preserved is mostly related to the process context:
- FPU
- DSP
- ASID
- UserLocal
- Watch registers

Signed-off-by: James Hogan <james.hogan@imgtec.com>
Signed-off-by: Paul Burton <paul.burton@imgtec.com>
---
 arch/mips/Kconfig         |  1 +
 arch/mips/kernel/Makefile |  2 +
 arch/mips/kernel/pm.c     | 95 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 98 insertions(+)
 create mode 100644 arch/mips/kernel/pm.c

diff --git a/arch/mips/Kconfig b/arch/mips/Kconfig
index 5cd695f905a1..322bbe12412e 100644
--- a/arch/mips/Kconfig
+++ b/arch/mips/Kconfig
@@ -50,6 +50,7 @@ config MIPS
 	select CLONE_BACKWARDS
 	select HAVE_DEBUG_STACKOVERFLOW
 	select HAVE_CC_STACKPROTECTOR
+	select CPU_PM if CPU_IDLE
 
 menu "Machine selection"
 
diff --git a/arch/mips/kernel/Makefile b/arch/mips/kernel/Makefile
index 277dab301cea..97540a80b833 100644
--- a/arch/mips/kernel/Makefile
+++ b/arch/mips/kernel/Makefile
@@ -107,6 +107,8 @@ obj-$(CONFIG_JUMP_LABEL)	+= jump_label.o
 obj-$(CONFIG_MIPS_CM)		+= mips-cm.o
 obj-$(CONFIG_MIPS_CPC)		+= mips-cpc.o
 
+obj-$(CONFIG_CPU_PM)		+= pm.o
+
 #
 # DSP ASE supported for MIPS32 or MIPS64 Release 2 cores only. It is not
 # safe to unconditionnaly use the assembler -mdsp / -mdspr2 switches
diff --git a/arch/mips/kernel/pm.c b/arch/mips/kernel/pm.c
new file mode 100644
index 000000000000..112903f36b89
--- /dev/null
+++ b/arch/mips/kernel/pm.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) 2014 Imagination Technologies Ltd.
+ *
+ * This program is free software; you can redistribute	it and/or modify it
+ * under  the terms of	the GNU General	 Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * CPU PM notifiers for saving/restoring general CPU state.
+ */
+
+#include <linux/cpu_pm.h>
+#include <linux/init.h>
+
+#include <asm/dsp.h>
+#include <asm/fpu.h>
+#include <asm/mmu_context.h>
+#include <asm/watch.h>
+
+/**
+ * mips_cpu_save() - Save general CPU state.
+ * Ensures that general CPU context is saved, notably FPU and DSP.
+ */
+static int mips_cpu_save(void)
+{
+	/* Save FPU state */
+	lose_fpu(1);
+
+	/* Save DSP state */
+	save_dsp(current);
+
+	return 0;
+}
+
+/**
+ * mips_cpu_restore() - Restore general CPU state.
+ * Restores important CPU context.
+ */
+static void mips_cpu_restore(void)
+{
+	unsigned int cpu = smp_processor_id();
+
+	/* Restore ASID */
+	if (current->mm)
+		write_c0_entryhi(cpu_asid(cpu, current->mm));
+
+	/* Restore DSP state */
+	restore_dsp(current);
+
+	/* Restore UserLocal */
+	if (cpu_has_userlocal)
+		write_c0_userlocal(current_thread_info()->tp_value);
+
+	/* Restore watch registers */
+	__restore_watch();
+}
+
+/**
+ * mips_pm_notifier() - Notifier for preserving general CPU context.
+ * @self:	Notifier block.
+ * @cmd:	CPU PM event.
+ * @v:		Private data (unused).
+ *
+ * This is called when a CPU power management event occurs, and is used to
+ * ensure that important CPU context is preserved across a CPU power down.
+ */
+static int mips_pm_notifier(struct notifier_block *self, unsigned long cmd,
+			    void *v)
+{
+	int ret;
+
+	switch (cmd) {
+	case CPU_PM_ENTER:
+		ret = mips_cpu_save();
+		if (ret)
+			return NOTIFY_STOP;
+		break;
+	case CPU_PM_ENTER_FAILED:
+	case CPU_PM_EXIT:
+		mips_cpu_restore();
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block mips_pm_notifier_block = {
+	.notifier_call = mips_pm_notifier,
+};
+
+static int __init mips_pm_init(void)
+{
+	return cpu_pm_register_notifier(&mips_pm_notifier_block);
+}
+arch_initcall(mips_pm_init);