core: dt_driver: factorize DT cells helper functions

Move/rename fdt_clock_cells() to fdt_get_dt_driver_cells().
and clk_dt_register_clk_provider() to dt_driver_register_provider().

Add helper function dt_driver_provider_cells() to get ::provider_cells
from a registered provider reference.

Acked-by: Jerome Forissier <jerome@forissier.org>
Acked-by: Clément Léger <clement.leger@bootlin.com>
Signed-off-by: Etienne Carriere <etienne.carriere@linaro.org>

4 files changed, 140 insertions, 58 deletions

diff --git a/core/drivers/clk/clk_dt.c b/core/drivers/clk/clk_dt.c
index 1941531d..3d3e803b 100644
--- a/core/drivers/clk/clk_dt.c
+++ b/core/drivers/clk/clk_dt.c
@@ -13,62 +13,6 @@
 #include <libfdt.h>
 #include <stddef.h>
 
-static int fdt_clock_cells(const void *fdt, int nodeoffset)
-{
-	const fdt32_t *c = NULL;
-	int len = 0;
-
-	c = fdt_getprop(fdt, nodeoffset, "#clock-cells", &len);
-	if (!c)
-		return len;
-
-	if (len != sizeof(*c))
-		return -FDT_ERR_BADNCELLS;
-
-	return (int)fdt32_to_cpu(*c);
-}
-
-TEE_Result clk_dt_register_clk_provider(const void *fdt, int nodeoffset,
-					clk_dt_get_func get_dt_clk, void *data)
-{
-	struct dt_driver_provider *prv = NULL;
-	int clock_cells = 0;
-
-	prv = calloc(1, sizeof(*prv));
-	if (!prv)
-		return TEE_ERROR_OUT_OF_MEMORY;
-
-	prv->get_of_device = (get_of_device_func)get_dt_clk;
-	prv->priv_data = data;
-	prv->nodeoffset = nodeoffset;
-	clock_cells = fdt_clock_cells(fdt, nodeoffset);
-	if (clock_cells < 0) {
-		free(prv);
-		return TEE_ERROR_GENERIC;
-	}
-	prv->provider_cells = clock_cells;
-	prv->phandle = fdt_get_phandle(fdt, nodeoffset);
-
-	SLIST_INSERT_HEAD(&dt_driver_provider_list, prv, link);
-
-	return TEE_SUCCESS;
-}
-
-static TEE_Result clk_dt_release_provider(void)
-{
-	struct dt_driver_provider *prv = NULL;
-
-	while (!SLIST_EMPTY(&dt_driver_provider_list)) {
-		prv = SLIST_FIRST(&dt_driver_provider_list);
-		SLIST_REMOVE_HEAD(&dt_driver_provider_list, link);
-		free(prv);
-	}
-
-	return TEE_SUCCESS;
-}
-
-driver_init_late(clk_dt_release_provider);
-
 static struct dt_driver_provider *clk_get_provider_by_node(int nodeoffset)
 {
 	struct dt_driver_provider *prv = NULL;
@@ -226,7 +170,8 @@ static TEE_Result parse_clock_property(const void *fdt, int node)
 		if (res)
 			panic("Failed to probe parent clock");
 
-		clock_cells = fdt_clock_cells(fdt, parent_node);
+		clock_cells = fdt_get_dt_driver_cells(fdt, parent_node,
+						      DT_DRIVER_CLK);
 		if (clock_cells < 0)
 			return TEE_ERROR_GENERIC;
 
diff --git a/core/include/drivers/clk_dt.h b/core/include/drivers/clk_dt.h
index 34ef51be..d41d34a9 100644
--- a/core/include/drivers/clk_dt.h
+++ b/core/include/drivers/clk_dt.h
@@ -86,8 +86,14 @@ typedef struct clk *(*clk_dt_get_func)(struct dt_driver_phandle_args *args,
  * @data: Data which will be passed to the get_dt_clk callback
  * Returns TEE_Result value
  */
+static inline
 TEE_Result clk_dt_register_clk_provider(const void *fdt, int nodeoffset,
-					clk_dt_get_func get_dt_clk, void *data);
+					clk_dt_get_func get_dt_clk, void *data)
+{
+	return dt_driver_register_provider(fdt, nodeoffset,
+					   (get_of_device_func)get_dt_clk,
+					   data, DT_DRIVER_CLK);
+}
 
 /**
  * clk_dt_get_simple_clk: simple clock matching function for single clock
diff --git a/core/include/kernel/dt_driver.h b/core/include/kernel/dt_driver.h
index e011ce99..394a741e 100644
--- a/core/include/kernel/dt_driver.h
+++ b/core/include/kernel/dt_driver.h
@@ -10,6 +10,7 @@
 #include <kernel/dt.h>
 #include <stdint.h>
 #include <sys/queue.h>
+#include <tee_api_types.h>
 
 /**
  * struct dt_driver_phandle_args - Devicetree phandle arguments
@@ -60,4 +61,39 @@ struct dt_driver_provider {
 
 SLIST_HEAD(dt_driver_prov_list, dt_driver_provider);
 extern struct dt_driver_prov_list dt_driver_provider_list;
+
+/**
+ * dt_driver_register_provider - Register a driver provider
+ *
+ * @fdt: Device tree to work on
+ * @nodeoffset: Node offset in the FDT
+ * @get_of_device: Function to match the devicetree with a device instance
+ * @data: Data which will be passed to the @get_of_device callback
+ * @type: Driver type
+ *
+ * @get_of_device returns a void *. Driver provider is expected to
+ * include a shim helper to cast to device reference into provider driver
+ * target structure reference (e.g (struct clk *) for clock devices).
+ */
+TEE_Result dt_driver_register_provider(const void *fdt, int nodeoffset,
+				       get_of_device_func get_of_device,
+				       void *data, enum dt_driver_type type);
+
+/*
+ * Return number cells used for phandle arguments by a driver provider
+ */
+unsigned int dt_driver_provider_cells(struct dt_driver_provider *prv);
+
+/*
+ * Get cells count of a device node given its dt_driver type
+ *
+ * @fdt: FDT base address
+ * @nodeoffset: Node offset on the FDT for the device
+ * @type: One of the supported DT_DRIVER_* value.
+ *
+ * Currently supports type DT_DRIVER_CLK.
+ * Return a positive cell count value (>= 0) or a negative FDT_ error code
+ */
+int fdt_get_dt_driver_cells(const void *fdt, int nodeoffset,
+			    enum dt_driver_type type);
 #endif /* __DT_DRIVER_H */
diff --git a/core/kernel/dt_driver.c b/core/kernel/dt_driver.c
index 4b08ee45..195fa653 100644
--- a/core/kernel/dt_driver.c
+++ b/core/kernel/dt_driver.c
@@ -4,8 +4,103 @@
  * Copyright (c) 2021, Bootlin
  */
 
+#include <initcall.h>
+#include <kernel/dt.h>
 #include <kernel/dt_driver.h>
+#include <libfdt.h>
+#include <malloc.h>
 #include <sys/queue.h>
+#include <tee_api_types.h>
 
 struct dt_driver_prov_list dt_driver_provider_list =
 	SLIST_HEAD_INITIALIZER(dt_driver_provider_list);
+
+/*
+ * Driver provider registering API functions
+ */
+
+TEE_Result dt_driver_register_provider(const void *fdt, int nodeoffset,
+				       get_of_device_func get_of_device,
+				       void *priv, enum dt_driver_type type)
+{
+	struct dt_driver_provider *prv = NULL;
+	int provider_cells = 0;
+	uint32_t phandle = 0;
+
+	provider_cells = fdt_get_dt_driver_cells(fdt, nodeoffset, type);
+	if (provider_cells < 0) {
+		DMSG("Failed to find provider cells: %d", provider_cells);
+		return TEE_ERROR_GENERIC;
+	}
+
+	phandle = fdt_get_phandle(fdt, nodeoffset);
+	if (!phandle || phandle == (uint32_t)-1) {
+		DMSG("Failed to find provide phandle");
+		return TEE_ERROR_GENERIC;
+	}
+
+	prv = calloc(1, sizeof(*prv));
+	if (!prv)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	prv->nodeoffset = nodeoffset;
+	prv->type = type;
+	prv->provider_cells = provider_cells;
+	prv->phandle = phandle;
+	prv->get_of_device = get_of_device;
+	prv->priv_data = priv;
+
+	SLIST_INSERT_HEAD(&dt_driver_provider_list, prv, link);
+
+	return TEE_SUCCESS;
+}
+
+/* Release driver provider references once all dt_drivers are initialized */
+static TEE_Result dt_driver_release_provider(void)
+{
+	struct dt_driver_provider *prv = NULL;
+
+	while (!SLIST_EMPTY(&dt_driver_provider_list)) {
+		prv = SLIST_FIRST(&dt_driver_provider_list);
+		SLIST_REMOVE_HEAD(&dt_driver_provider_list, link);
+		free(prv);
+	}
+
+	return TEE_SUCCESS;
+}
+
+driver_init_late(dt_driver_release_provider);
+
+/*
+ * Helper functions for dt_drivers querying driver provider information
+ */
+
+int fdt_get_dt_driver_cells(const void *fdt, int nodeoffset,
+			    enum dt_driver_type type)
+{
+	const char *cells_name = NULL;
+	const fdt32_t *c = NULL;
+	int len = 0;
+
+	switch (type) {
+	case DT_DRIVER_CLK:
+		cells_name = "#clock-cells";
+		break;
+	default:
+		panic();
+	}
+
+	c = fdt_getprop(fdt, nodeoffset, cells_name, &len);
+	if (!c)
+		return len;
+
+	if (len != sizeof(*c))
+		return -FDT_ERR_BADNCELLS;
+
+	return fdt32_to_cpu(*c);
+}
+
+unsigned int dt_driver_provider_cells(struct dt_driver_provider *prv)
+{
+	return prv->provider_cells;
+}