firmware: psci: bind arm smccc features when discovered

Use PSCI device to query Arm SMCCC v1.1 support from secure monitor
and if so, bind drivers for the SMCCC features that monitor supports.

Drivers willing to be bound from Arm SMCCC features discovery can use
macro ARM_SMCCC_FEATURE_DRIVER() to register to smccc feature discovery,
providing target driver name and a callback function that returns
whether or not the SMCCC feature is supported by the system.

Signed-off-by: Etienne Carriere <etienne.carriere@linaro.org>

2 files changed, 88 insertions, 1 deletions

diff --git a/drivers/firmware/Kconfig b/drivers/firmware/Kconfig
index ef958b3a7a4..f10d1aaf4b6 100644
--- a/drivers/firmware/Kconfig
+++ b/drivers/firmware/Kconfig
@@ -37,4 +37,12 @@ config ZYNQMP_FIRMWARE
 	  Say yes to enable ZynqMP firmware interface driver.
 	  If in doubt, say N.
 
+config ARM_SMCCC_FEATURES
+	bool "Arm SMCCC features discovery"
+	depends on ARM_PSCI_FW
+	help
+	  Discover Arm SMCCC features for which a U-Boot driver is defined. When enabled,
+	  the PSCI driver is always probed and binds dirvers registered to the Arm SMCCC
+	  services if any and reported as supported by the SMCCC firmware.
+
 source "drivers/firmware/scmi/Kconfig"
diff --git a/drivers/firmware/psci.c b/drivers/firmware/psci.c
index f845ba67f8f..ef3e9836461 100644
--- a/drivers/firmware/psci.c
+++ b/drivers/firmware/psci.c
@@ -11,9 +11,11 @@
 #include <dm.h>
 #include <efi_loader.h>
 #include <irq_func.h>
+#include <linker_lists.h>
 #include <log.h>
 #include <sysreset.h>
 #include <asm/system.h>
+#include <dm/device-internal.h>
 #include <dm/lists.h>
 #include <linux/arm-smccc.h>
 #include <linux/delay.h>
@@ -95,6 +97,76 @@ static bool psci_is_system_reset2_supported(void)
 	return false;
 }
 
+static void smccc_invoke_hvc(unsigned long a0, unsigned long a1,
+			     unsigned long a2, unsigned long a3,
+			     unsigned long a4, unsigned long a5,
+			     unsigned long a6, unsigned long a7,
+			     struct arm_smccc_res *res)
+{
+	arm_smccc_hvc(a0, a1, a2, a3, a4, a5, a6, a7, res);
+}
+
+static void smccc_invoke_smc(unsigned long a0, unsigned long a1,
+			     unsigned long a2, unsigned long a3,
+			     unsigned long a4, unsigned long a5,
+			     unsigned long a6, unsigned long a7,
+			     struct arm_smccc_res *res)
+{
+	arm_smccc_smc(a0, a1, a2, a3, a4, a5, a6, a7, res);
+}
+
+static int bind_smccc_features(struct udevice *dev, int psci_method)
+{
+	struct psci_plat_data *pdata = dev_get_plat(dev);
+	struct arm_smccc_feature *feature;
+	size_t feature_cnt, n;
+
+	if (!IS_ENABLED(CONFIG_ARM_SMCCC_FEATURES))
+		return 0;
+
+	/*
+	 * SMCCC features discovery invoke SMCCC standard function ID
+	 * ARM_SMCCC_ARCH_FEATURES but this sequence requires that this
+	 * standard ARM_SMCCC_ARCH_FEATURES function ID itself is supported.
+	 * It is queried here with invoking PSCI_FEATURES known available
+	 * from PSCI 1.0.
+	 */
+	if (!device_is_compatible(dev, "arm,psci-1.0") ||
+	    PSCI_VERSION_MAJOR(psci_0_2_get_version()) == 0)
+		return 0;
+
+	if (request_psci_features(ARM_SMCCC_ARCH_FEATURES) ==
+	    PSCI_RET_NOT_SUPPORTED)
+		return 0;
+
+	if (psci_method == PSCI_METHOD_HVC)
+		pdata->invoke_fn = smccc_invoke_hvc;
+	else
+		pdata->invoke_fn = smccc_invoke_smc;
+
+	feature_cnt = ll_entry_count(struct arm_smccc_feature, arm_smccc_feature);
+	feature = ll_entry_start(struct arm_smccc_feature, arm_smccc_feature);
+
+	for (n = 0; n < feature_cnt; n++, feature++) {
+		const char *drv_name = feature->driver_name;
+		struct udevice *dev2;
+		int ret;
+
+		if (!feature->is_supported || !feature->is_supported(pdata->invoke_fn))
+			continue;
+
+		ret = device_bind_driver(dev, drv_name, drv_name, &dev2);
+		if (ret) {
+			pr_warn("%s was not bound: %d, ignore\n", drv_name, ret);
+			continue;
+		}
+
+		dev_set_parent_plat(dev2, dev_get_plat(dev));
+	}
+
+	return 0;
+}
+
 static int psci_bind(struct udevice *dev)
 {
 	/* No SYSTEM_RESET support for PSCI 0.1 */
@@ -109,6 +181,10 @@ static int psci_bind(struct udevice *dev)
 			pr_debug("PSCI System Reset 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);
+
 	return 0;
 }
 
@@ -136,7 +212,7 @@ static int psci_probe(struct udevice *dev)
 		return -EINVAL;
 	}
 
-	return 0;
+	return bind_smccc_features(dev, psci_method);
 }
 
 /**
@@ -240,4 +316,7 @@ U_BOOT_DRIVER(psci) = {
 	.of_match = psci_of_match,
 	.bind = psci_bind,
 	.probe = psci_probe,
+#ifdef CONFIG_ARM_SMCCC_FEATURES
+	.plat_auto = sizeof(struct psci_plat_data),
+#endif
 };