1 files changed, 51 insertions, 4 deletions

diff --git a/sys/dev/psci/smccc.c b/sys/dev/psci/smccc.c
index 08ad6d84fc3c..e40a60336d98 100644
--- a/sys/dev/psci/smccc.c
+++ b/sys/dev/psci/smccc.c
@@ -34,7 +34,9 @@
 
 #include <sys/param.h>
 #include <sys/systm.h>
+#include <sys/bus.h>
 #include <sys/kernel.h>
+#include <sys/module.h>
 
 #include <dev/psci/psci.h>
 #include <dev/psci/smccc.h>
@@ -66,6 +68,33 @@ smccc_init(void)
 	}
 }
 
+static int
+smccc_probe(device_t dev)
+{
+	int32_t version;
+
+	/*
+	 * If the version is not implemented then we treat it as SMCCC 1.0
+	 */
+	if (psci_features(SMCCC_VERSION) == PSCI_RETVAL_NOT_SUPPORTED ||
+	    (version = arm_smccc_invoke(SMCCC_VERSION, NULL)) <= 0) {
+		device_set_desc(dev, "ARM SMCCC v1.0");
+		return (0);
+	}
+
+	device_set_descf(dev, "ARM SMCCC v%d.%d", SMCCC_VERSION_MAJOR(version),
+	    SMCCC_VERSION_MINOR(version));
+
+	return (0);
+}
+
+static int
+smccc_attach(device_t dev)
+{
+	bus_attach_children(dev);
+	return (0);
+}
+
 uint32_t
 smccc_get_version(void)
 {
@@ -79,9 +108,9 @@ smccc_arch_features(uint32_t smccc_func_id)
 
 	MPASS(smccc_version != 0);
 	if (smccc_version == SMCCC_VERSION_1_0)
-		return (PSCI_RETVAL_NOT_SUPPORTED);
+		return (SMCCC_RET_NOT_SUPPORTED);
 
-	return (psci_call(SMCCC_ARCH_FEATURES, smccc_func_id, 0, 0));
+	return (arm_smccc_invoke(SMCCC_ARCH_FEATURES, smccc_func_id, NULL));
 }
 
 /*
@@ -95,7 +124,7 @@ smccc_arch_workaround_1(void)
 	MPASS(smccc_version != 0);
 	KASSERT(smccc_version != SMCCC_VERSION_1_0,
 	    ("SMCCC arch workaround 1 called with an invalid SMCCC interface"));
-	return (psci_call(SMCCC_ARCH_WORKAROUND_1, 0, 0, 0));
+	return (arm_smccc_invoke(SMCCC_ARCH_WORKAROUND_1, NULL));
 }
 
 int
@@ -105,5 +134,23 @@ smccc_arch_workaround_2(int enable)
 	MPASS(smccc_version != 0);
 	KASSERT(smccc_version != SMCCC_VERSION_1_0,
 	    ("SMCCC arch workaround 2 called with an invalid SMCCC interface"));
-	return (psci_call(SMCCC_ARCH_WORKAROUND_2, enable, 0, 0));
+	return (arm_smccc_invoke(SMCCC_ARCH_WORKAROUND_2, enable, NULL));
 }
+
+static device_method_t smccc_methods[] = {
+	DEVMETHOD(device_probe,		smccc_probe),
+	DEVMETHOD(device_attach,	smccc_attach),
+
+	DEVMETHOD(bus_add_child,	bus_generic_add_child),
+
+	DEVMETHOD_END
+};
+
+static driver_t smccc_driver = {
+	"smccc",
+	smccc_methods,
+	0,
+};
+
+EARLY_DRIVER_MODULE(smccc, psci, smccc_driver, 0, 0,
+    BUS_PASS_CPU + BUS_PASS_ORDER_FIRST);