core: dt_driver: factorize clk_dt_get_from_provider()

Implement dt_driver_device_from_node_idx_prop() for
clk_dt_get_by_idx_prop() to get target reference instance.

Move/rename clk_dt_get_from_provider() to
dt_driver_device_from_provider_prop()

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>

3 files changed, 79 insertions, 54 deletions

diff --git a/core/drivers/clk/clk_dt.c b/core/drivers/clk/clk_dt.c
index 2a4e3c61..cf9876b3 100644
--- a/core/drivers/clk/clk_dt.c
+++ b/core/drivers/clk/clk_dt.c
@@ -13,30 +13,6 @@
 #include <libfdt.h>
 #include <stddef.h>
 
-static struct clk *clk_dt_get_from_provider(struct dt_driver_provider *prv,
-					    unsigned int clock_cells,
-					    const uint32_t *prop)
-{
-	unsigned int arg = 0;
-	struct clk *clk = NULL;
-	struct dt_driver_phandle_args *pargs = NULL;
-
-	pargs = calloc(1, clock_cells * sizeof(uint32_t *) +
-		       sizeof(*pargs));
-	if (!pargs)
-		return NULL;
-
-	pargs->args_count = clock_cells;
-	for (arg = 0; arg < clock_cells; arg++)
-		pargs->args[arg] = fdt32_to_cpu(prop[arg + 1]);
-
-	clk = prv->get_of_device(pargs, prv->priv_data);
-
-	free(pargs);
-
-	return clk;
-}
-
 struct clk *clk_dt_get_by_name(const void *fdt, int nodeoffset,
 			       const char *name)
 {
@@ -53,37 +29,10 @@ static struct clk *clk_dt_get_by_idx_prop(const char *prop_name,
 					  const void *fdt, int nodeoffset,
 					  unsigned int clk_idx)
 {
-	int len = 0;
-	int idx = 0;
-	int idx32 = 0;
-	int clock_cells = 0;
-	uint32_t phandle = 0;
-	const uint32_t *prop = NULL;
-	struct dt_driver_provider *prv = NULL;
-
-	prop = fdt_getprop(fdt, nodeoffset, prop_name, &len);
-	if (!prop)
-		return NULL;
-
-	while (idx < len) {
-		idx32 = idx / sizeof(uint32_t);
-		phandle = fdt32_to_cpu(prop[idx32]);
-
-		prv = dt_driver_get_provider_by_node(phandle);
-		if (!prv)
-			return NULL;
+	void *device = dt_driver_device_from_node_idx_prop(prop_name, fdt,
+							   nodeoffset, clk_idx);
 
-		clock_cells = prv->provider_cells;
-		if (clk_idx) {
-			clk_idx--;
-			idx += sizeof(phandle) + clock_cells * sizeof(uint32_t);
-			continue;
-		}
-
-		return clk_dt_get_from_provider(prv, clock_cells, &prop[idx32]);
-	}
-
-	return NULL;
+	return (struct clk *)device;
 }
 
 struct clk *clk_dt_get_by_idx(const void *fdt, int nodeoffset,
diff --git a/core/include/kernel/dt_driver.h b/core/include/kernel/dt_driver.h
index e858ad7a..0197f72e 100644
--- a/core/include/kernel/dt_driver.h
+++ b/core/include/kernel/dt_driver.h
@@ -80,6 +80,22 @@ TEE_Result dt_driver_register_provider(const void *fdt, int nodeoffset,
 				       void *data, enum dt_driver_type type);
 
 /*
+ * dt_driver_device_from_node_idx_prop - Return a device instance based on a
+ *	property name and FDT information
+ *
+ * @prop_name: DT property name, e.g. "clocks" for clock resources
+ * @fdt: FDT base address
+ * @nodeoffset: node offset in the FDT
+ * @prop_idx: index of the phandle data in the property
+ *
+ * Return a device opaque reference, e.g. a struct clk pointer for a clock
+ * driver, or NULL if not found.
+ */
+void *dt_driver_device_from_node_idx_prop(const char *prop_name,
+					  const void *fdt, int nodeoffset,
+					  unsigned int prop_idx);
+
+/*
  * Return driver provider reference from its node offset value in the FDT
  */
 struct dt_driver_provider *dt_driver_get_provider_by_node(int nodeoffset);
diff --git a/core/kernel/dt_driver.c b/core/kernel/dt_driver.c
index 2a46e7a2..353a48b7 100644
--- a/core/kernel/dt_driver.c
+++ b/core/kernel/dt_driver.c
@@ -126,3 +126,63 @@ struct dt_driver_provider *dt_driver_get_provider_by_phandle(uint32_t phandle)
 
 	return NULL;
 }
+
+static void *device_from_provider_prop(struct dt_driver_provider *prv,
+				       const uint32_t *prop)
+{
+	struct dt_driver_phandle_args *pargs = NULL;
+	unsigned int n = 0;
+	void *device = NULL;
+
+	pargs = calloc(1, prv->provider_cells * sizeof(uint32_t *) +
+		       sizeof(*pargs));
+	if (!pargs)
+		return NULL;
+
+	pargs->args_count = prv->provider_cells;
+	for (n = 0; n < prv->provider_cells; n++)
+		pargs->args[n] = fdt32_to_cpu(prop[n + 1]);
+
+	device = prv->get_of_device(pargs, prv->priv_data);
+
+	free(pargs);
+
+	return device;
+}
+
+void *dt_driver_device_from_node_idx_prop(const char *prop_name,
+					  const void *fdt, int nodeoffset,
+					  unsigned int prop_idx)
+{
+	int len = 0;
+	int idx = 0;
+	int idx32 = 0;
+	int prv_cells = 0;
+	uint32_t phandle = 0;
+	const uint32_t *prop = NULL;
+	struct dt_driver_provider *prv = NULL;
+
+	prop = fdt_getprop(fdt, nodeoffset, prop_name, &len);
+	if (!prop)
+		return NULL;
+
+	while (idx < len) {
+		idx32 = idx / sizeof(uint32_t);
+		phandle = fdt32_to_cpu(prop[idx32]);
+
+		prv = dt_driver_get_provider_by_phandle(phandle);
+		if (!prv)
+			return NULL;
+
+		prv_cells = dt_driver_provider_cells(prv);
+		if (prop_idx) {
+			prop_idx--;
+			idx += sizeof(phandle) + prv_cells * sizeof(uint32_t);
+			continue;
+		}
+
+		return device_from_provider_prop(prv, prop + idx32);
+	}
+
+	return NULL;
+}