[PATCH 2/2] firmware: meson: sm: make power-off work on S4

[christian . koever-draxl]

From: Christian Stefan Kövér-Draxl <ckoever@xxxxxxxxxx>

On S4 the firmware implements PSCI SYSTEM_OFF as DDR self-refresh. A
press of the power key afterwards jumps back to the kernel resume
address, where nothing is waiting, and the board hangs until it is
unplugged.

The vendor kernel avoids this in two steps, both needed:

 - set the firmware shutdown flag (SMC 0x82000042, argument 1) right
   before SYSTEM_OFF, which makes the firmware cold-boot on the power
   key instead of resuming;
 - take the secondary CPUs down through PSCI CPU_OFF first. With them
   only parked in the kernel, the firmware wake-up path hangs as well.

Register a power-off prepare handler that stops the secondary CPUs and
a high priority power-off handler that sets the flag, for the
amlogic,meson-s4-sm compatible only.

Tested on an S905W2 board: poweroff followed by the power key
cold-boots the board.

Signed-off-by: Christian Stefan Kövér-Draxl <ckoever@xxxxxxxxxx>
---
 drivers/firmware/meson/meson_sm.c       | 46 +++++++++++++++++++++++++
 include/linux/firmware/meson/meson_sm.h |  1 +
 2 files changed, 47 insertions(+)

diff --git a/drivers/firmware/meson/meson_sm.c b/drivers/firmware/meson/meson_sm.c
index ab9751a59b55..5410386603c7 100644
--- a/drivers/firmware/meson/meson_sm.c
+++ b/drivers/firmware/meson/meson_sm.c
@@ -10,6 +10,7 @@
 
 #include <linux/arm-smccc.h>
 #include <linux/bug.h>
+#include <linux/cpu.h>
 #include <linux/io.h>
 #include <linux/module.h>
 #include <linux/of.h>
@@ -17,6 +18,7 @@
 #include <linux/platform_device.h>
 #include <linux/printk.h>
 #include <linux/property.h>
+#include <linux/reboot.h>
 #include <linux/types.h>
 #include <linux/sizes.h>
  #include <linux/slab.h>
@@ -48,6 +50,7 @@ static const struct meson_sm_chip gxbb_chip = {
 		CMD(SM_THERMAL_CALIB_READ,	0x82000047),
 		CMD(SM_A1_PWRC_SET,		0x82000093),
 		CMD(SM_A1_PWRC_GET,		0x82000095),
+		CMD(SM_SET_POWEROFF_FLAG,	0x82000042),
 		{ /* sentinel */ },
 	},
 };
@@ -303,6 +306,45 @@ static struct attribute *meson_sm_sysfs_attrs[] = {
 };
 ATTRIBUTE_GROUPS(meson_sm_sysfs);
 
+/*
+ * The S4 firmware implements PSCI SYSTEM_OFF as DDR self-refresh: on the
+ * power key it jumps back to the kernel instead of booting, which hangs the
+ * board. Setting the shutdown flag first makes it cold-boot instead, and the
+ * secondary CPUs must have been taken down through PSCI CPU_OFF beforehand,
+ * otherwise the wake-up path hangs as well.
+ */
+static int meson_sm_power_off_prepare(struct sys_off_data *data)
+{
+	if (freeze_secondary_cpus(0))
+		pr_warn("meson-sm: failed to stop secondary CPUs\n");
+
+	return NOTIFY_DONE;
+}
+
+static int meson_sm_power_off(struct sys_off_data *data)
+{
+	meson_sm_call(data->cb_data, SM_SET_POWEROFF_FLAG, NULL, 1, 0, 0, 0, 0);
+
+	return NOTIFY_DONE;
+}
+
+static int meson_sm_register_power_off(struct device *dev,
+				       struct meson_sm_firmware *fw)
+{
+	int ret;
+
+	ret = devm_register_sys_off_handler(dev, SYS_OFF_MODE_POWER_OFF_PREPARE,
+					    SYS_OFF_PRIO_DEFAULT,
+					    meson_sm_power_off_prepare, NULL);
+	if (ret)
+		return ret;
+
+	/* Run before the PSCI handler, which never returns */
+	return devm_register_sys_off_handler(dev, SYS_OFF_MODE_POWER_OFF,
+					     SYS_OFF_PRIO_HIGH,
+					     meson_sm_power_off, fw);
+}
+
 static const struct of_device_id meson_sm_ids[] = {
 	{ .compatible = "amlogic,meson-gxbb-sm", .data = &gxbb_chip },
 	{ /* sentinel */ },
@@ -343,6 +385,10 @@ static int __init meson_sm_probe(struct platform_device *pdev)
 	if (devm_of_platform_populate(dev))
 		goto unmap_out_base;
 
+	if (of_device_is_compatible(dev->of_node, "amlogic,meson-s4-sm") &&
+	    meson_sm_register_power_off(dev, fw))
+		dev_warn(dev, "failed to register power-off handlers\n");
+
 	pr_info("secure-monitor enabled\n");
 
 	return 0;
diff --git a/include/linux/firmware/meson/meson_sm.h b/include/linux/firmware/meson/meson_sm.h
index 3ebc2bd9a976..3f2fa160e32f 100644
--- a/include/linux/firmware/meson/meson_sm.h
+++ b/include/linux/firmware/meson/meson_sm.h
@@ -15,6 +15,7 @@ enum {
 	SM_THERMAL_CALIB_READ,
 	SM_A1_PWRC_SET,
 	SM_A1_PWRC_GET,
+	SM_SET_POWEROFF_FLAG,
 };
 
 struct meson_sm_firmware;
-- 
2.56.0