From 1f7299ce20ecc6dc6905dc5ec8c47d3944a4425d Mon Sep 17 00:00:00 2001
From: Anup Patel <anup.patel@linaro.org>
Date: Mon, 25 Nov 2013 17:05:51 +0530
Subject: [PATCH 7/7] ARM64: psci: Add support for system reboot and poweroff

We have PSCI SYSTEM_OFF and SYSTEM_RESET function call emulation
available when running as Guest using KVM ARM64.

This patch implements system reboot and poweroff using PSCI
SYSTEM_OFF and SYSTEM_RESET.

Signed-off-by: Anup Patel <anup.patel@linaro.org>
Signed-off-by: Pranavkumar Sawargaonkar <pranavkumar@linaro.org>
---
 arch/arm64/kernel/psci.c |   36 ++++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/arch/arm64/kernel/psci.c b/arch/arm64/kernel/psci.c
index 4f97db3..9469ffa 100644
--- a/arch/arm64/kernel/psci.c
+++ b/arch/arm64/kernel/psci.c
@@ -17,12 +17,14 @@
 
 #include <linux/init.h>
 #include <linux/of.h>
+#include <linux/pm.h>
 #include <linux/smp.h>
 
 #include <asm/compiler.h>
 #include <asm/cpu_ops.h>
 #include <asm/errno.h>
 #include <asm/psci.h>
+#include <asm/system_misc.h>
 #include <asm/smp_plat.h>
 
 #define PSCI_POWER_STATE_TYPE_STANDBY		0
@@ -51,6 +53,8 @@ enum psci_function {
 	PSCI_FN_CPU_ON,
 	PSCI_FN_CPU_OFF,
 	PSCI_FN_MIGRATE,
+	PSCI_FN_SYSTEM_OFF,
+	PSCI_FN_SYSTEM_RESET,
 	PSCI_FN_MAX,
 };
 
@@ -171,6 +175,28 @@ static int psci_migrate(unsigned long cpuid)
 	return psci_to_linux_errno(err);
 }
 
+static void psci_power_off(void)
+{
+	int err;
+	u32 fn;
+
+	fn = psci_function_id[PSCI_FN_SYSTEM_OFF];
+	err = invoke_psci_fn(fn, 0, 0, 0);
+	if (err)
+		pr_warning("%s: failed\n", __func__);
+}
+
+static void psci_restart(enum reboot_mode reboot_mode, const char *cmd)
+{
+	int err;
+	u32 fn;
+
+	fn = psci_function_id[PSCI_FN_SYSTEM_RESET];
+	err = invoke_psci_fn(fn, 0, 0, 0);
+	if (err)
+		pr_warning("%s: failed\n", __func__);
+}
+
 static const struct of_device_id psci_of_match[] __initconst = {
 	{ .compatible = "arm,psci",	},
 	{},
@@ -225,6 +251,16 @@ int __init psci_init(void)
 		psci_ops.migrate = psci_migrate;
 	}
 
+	if (!of_property_read_u32(np, "system_off", &id)) {
+		psci_function_id[PSCI_FN_SYSTEM_OFF] = id;
+		pm_power_off = psci_power_off;
+	}
+
+	if (!of_property_read_u32(np, "system_reset", &id)) {
+		psci_function_id[PSCI_FN_SYSTEM_RESET] = id;
+		arm_pm_restart = psci_restart;
+	}
+
 out_put_node:
 	of_node_put(np);
 	return err;
-- 
1.7.9.5