From 27b371f5353dac83830d2f4229cdfda1f63d99dd Mon Sep 17 00:00:00 2001
From: Robin Murphy <Robin.Murphy@arm.com>
Date: Wed, 13 Aug 2014 11:19:53 +0100
Subject: [PATCH] Support PSCI reboot/shutdown

Hack in the extra function IDs in lieu of 'proper' PSCi 0.2 support.

Signed-off-by: Robin Murphy <robin.murphy@arm.com>
---
 arch/arm64/boot/dts/juno.dts |  2 ++
 arch/arm64/kernel/psci.c     | 24 ++++++++++++++++++++++++
 2 files changed, 26 insertions(+)

diff --git a/arch/arm64/boot/dts/juno.dts b/arch/arm64/boot/dts/juno.dts
index 34a7b92..224d3c4 100644
--- a/arch/arm64/boot/dts/juno.dts
+++ b/arch/arm64/boot/dts/juno.dts
@@ -201,6 +201,8 @@
 		cpu_off = <0x84000002>;
 		cpu_on = <0xc4000003>;
 		migrate = <0xc4000005>;
+		sys_poweroff = <0x84000008>;
+		sys_reset = <0x84000009>;
 	};
 
 	pci0: pci@30000000 {
diff --git a/arch/arm64/kernel/psci.c b/arch/arm64/kernel/psci.c
index 0e32ab4..51439b3 100644
--- a/arch/arm64/kernel/psci.c
+++ b/arch/arm64/kernel/psci.c
@@ -18,6 +18,7 @@
 #include <linux/cpuidle.h>
 #include <linux/init.h>
 #include <linux/of.h>
+#include <linux/pm.h>
 #include <linux/smp.h>
 #include <linux/slab.h>
 
@@ -27,6 +28,7 @@
 #include <asm/psci.h>
 #include <asm/smp_plat.h>
 #include <asm/suspend.h>
+#include <asm/system_misc.h>
 
 #define PSCI_POWER_STATE_TYPE_STANDBY		0
 #define PSCI_POWER_STATE_TYPE_POWER_DOWN	1
@@ -54,6 +56,8 @@ enum psci_function {
 	PSCI_FN_CPU_ON,
 	PSCI_FN_CPU_OFF,
 	PSCI_FN_MIGRATE,
+	PSCI_FN_SYSTEM_RESET,
+	PSCI_FN_SYSTEM_OFF,
 	PSCI_FN_MAX,
 };
 
@@ -187,6 +191,16 @@ static int psci_migrate(unsigned long cpuid)
 	return psci_to_linux_errno(err);
 }
 
+static void psci_sys_reset(enum reboot_mode reboot_mode, const char *cmd)
+{
+	invoke_psci_fn(psci_function_id[PSCI_FN_SYSTEM_RESET], 0, 0, 0);
+}
+
+static void psci_sys_poweroff(void)
+{
+	invoke_psci_fn(psci_function_id[PSCI_FN_SYSTEM_OFF], 0, 0, 0);
+}
+
 static const struct of_device_id psci_of_match[] __initconst = {
 	{ .compatible = "arm,psci",	},
 	{},
@@ -309,6 +323,16 @@ void __init psci_init(void)
 		psci_ops.migrate = psci_migrate;
 	}
 
+	if (!of_property_read_u32(np, "sys_reset", &id)) {
+		psci_function_id[PSCI_FN_SYSTEM_RESET] = id;
+		arm_pm_restart = psci_sys_reset;
+	}
+
+	if (!of_property_read_u32(np, "sys_poweroff", &id)) {
+		psci_function_id[PSCI_FN_SYSTEM_OFF] = id;
+		pm_power_off = psci_sys_poweroff;
+	}
+
 out_put_node:
 	of_node_put(np);
 	return;
-- 
2.7.4