[PATCH 3/8] reboot-mode: add PSCI reboot-mode driver

Add a PSCI backend for the reboot-mode framework that triggers a
SYSTEM_RESET2 vendor-specific reset described by a "mode-*" property under
a "reboot-mode" subnode of the PSCI device tree node (for example
"mode-edl = <0x80000000 0x00000001>" to enter Qualcomm EDL/download mode).

All device tree parsing and name matching already live in the reboot-mode
uclass; this driver only turns a decoded set of magic cells into a
firmware call.

U-Boot's psci_system_reset2() client takes only a 32-bit cookie and ORs in
PSCI_RESET2_TYPE_VENDOR itself, so the trigger folds the low cells into a
cookie and rejects a 3-cell mode whose cookie_hi is non-zero rather than
silently truncating it.

psci_bind() binds the driver to the "reboot-mode" subnode when
CONFIG_REBOOT_MODE_PSCI is enabled, using device_bind_driver_to_node().

Signed-off-by: Balaji Selvanathan <[email protected]>
---
 drivers/firmware/psci.c                | 16 ++++++++++
 drivers/reboot-mode/Kconfig            | 11 +++++++
 drivers/reboot-mode/Makefile           |  1 +
 drivers/reboot-mode/reboot-mode-psci.c | 58 ++++++++++++++++++++++++++++++++++
 4 files changed, 86 insertions(+)

diff --git a/drivers/firmware/psci.c b/drivers/firmware/psci.c
index 2e3223e1c32..781a848e350 100644
--- a/drivers/firmware/psci.c
+++ b/drivers/firmware/psci.c
@@ -188,6 +188,22 @@ static int psci_bind(struct udevice *dev)
 			pr_debug("PSCI System Reset was not bound.\n");
 	}
 
+	/*
+	 * Bind the PSCI reboot-mode driver to the "reboot-mode" subnode so it
+	 * can trigger SYSTEM_RESET2 vendor resets described by "mode-*"
+	 * properties there (for example EDL/download mode).
+	 */
+	if (CONFIG_IS_ENABLED(REBOOT_MODE_PSCI)) {
+		ofnode rm_node = ofnode_find_subnode(dev_ofnode(dev),
+						     "reboot-mode");
+
+		if (ofnode_valid(rm_node) &&
+		    device_bind_driver_to_node(dev, "reboot-mode-psci",
+					       "reboot-mode-psci", rm_node,
+					       NULL))
+			pr_warn("PSCI reboot-mode was not bound.\n");
+	}
+
 	/* From PSCI v1.0 onward we can discover services through ARM_SMCCC_FEATURE */
 	if (IS_ENABLED(CONFIG_ARM_SMCCC_FEATURES) && device_is_compatible(dev, "arm,psci-1.0"))
 		dev_or_flags(dev, DM_FLAG_PROBE_AFTER_BIND);
diff --git a/drivers/reboot-mode/Kconfig b/drivers/reboot-mode/Kconfig
index 3fdb4218a8b..109a196a3c9 100644
--- a/drivers/reboot-mode/Kconfig
+++ b/drivers/reboot-mode/Kconfig
@@ -39,4 +39,15 @@ config REBOOT_MODE_NVMEM
 	  Use any kind of non-volatile memory (EEPROM, RTC, etc) to control the
 	  reboot mode.
 
+config REBOOT_MODE_PSCI
+	bool "Use PSCI SYSTEM_RESET2 vendor resets as reboot modes"
+	depends on DM_REBOOT_MODE && ARM_SMCCC
+	help
+	  Trigger PSCI SYSTEM_RESET2 vendor-specific resets (for example
+	  rebooting a Qualcomm SoC into EDL/download mode) via the reboot-mode
+	  framework. Each reset is described by a "mode-*" property under the
+	  "reboot-mode" subnode of the psci device tree node, so the magic
+	  values are not hard-coded in the driver. The modes can then be
+	  triggered from the "reset" command by name.
+
 endmenu
diff --git a/drivers/reboot-mode/Makefile b/drivers/reboot-mode/Makefile
index 48c8ab7fe71..56c7078258b 100644
--- a/drivers/reboot-mode/Makefile
+++ b/drivers/reboot-mode/Makefile
@@ -8,3 +8,4 @@ obj-$(CONFIG_DM_REBOOT_MODE) += reboot-mode-uclass.o
 obj-$(CONFIG_DM_REBOOT_MODE_GPIO) += reboot-mode-gpio.o
 obj-$(CONFIG_DM_REBOOT_MODE_RTC) += reboot-mode-rtc.o
 obj-$(CONFIG_REBOOT_MODE_NVMEM) += reboot-mode-nvmem.o
+obj-$(CONFIG_REBOOT_MODE_PSCI) += reboot-mode-psci.o
diff --git a/drivers/reboot-mode/reboot-mode-psci.c b/drivers/reboot-mode/reboot-mode-psci.c
new file mode 100644
index 00000000000..88133c68ab4
--- /dev/null
+++ b/drivers/reboot-mode/reboot-mode-psci.c
@@ -0,0 +1,58 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) Qualcomm Technologies, Inc. and/or its subsidiaries.
+ *
+ */
+
+#include <dm.h>
+#include <dm/device_compat.h>
+#include <reboot-mode/reboot-mode.h>
+#include <asm/psci.h>
+#include <asm/system.h>
+#include <linux/errno.h>
+
+static int psci_reboot_mode_trigger(struct udevice *dev, const u32 *magic,
+				    int count)
+{
+	u32 reset_type = magic[0];
+	u64 cookie = 0;
+	int i;
+
+	if (count < 1 || count > REBOOT_MODE_MAX_MAGIC)
+		return -EINVAL;
+
+	/*
+	 * U-Boot's psci_system_reset2() takes only a 32-bit cookie, whereas
+	 * the binding allows a 64-bit cookie in the 3-cell form (magic[1] is
+	 * cookie_hi, magic[2] is cookie_lo). Reject a non-zero high half
+	 * rather than silently truncating it.
+	 */
+	if (count == REBOOT_MODE_MAX_MAGIC && magic[1]) {
+		dev_err(dev, "64-bit reset cookie is not supported\n");
+		return -EINVAL;
+	}
+
+	for (i = 1; i < count; i++)
+		cookie = (cookie << 32) | magic[i];
+
+	if (psci_features(ARM_PSCI_1_1_FN64_SYSTEM_RESET2) !=
+	    ARM_PSCI_RET_SUCCESS) {
+		dev_err(dev, "SYSTEM_RESET2 is not supported by firmware\n");
+		return -EPROTONOSUPPORT;
+	}
+
+	/* Does not return on success. */
+	psci_system_reset2(reset_type, (u32)cookie);
+
+	return -EINPROGRESS;
+}
+
+static const struct reboot_mode_ops psci_reboot_mode_ops = {
+	.trigger = psci_reboot_mode_trigger,
+};
+
+U_BOOT_DRIVER(psci_reboot_mode) = {
+	.name = "reboot-mode-psci",
+	.id = UCLASS_REBOOT_MODE,
+	.ops = &psci_reboot_mode_ops,
+};

-- 
2.34.1