drivers: clk: add devicetree support

When using a devicetree, it is often useful to have clocks parsing.
This support adds clocks properties parsing and allow having clock
providers and users. Clocks drivers can also be declared with
CLK_DT_DECLARE. They will be probed automatically by the clock core.
On the user side, function clk_dt_get_by_name and clk_dt_get_by_idx
allows to retrieve a clock from the device tree description and match
it with the provider clocks. The core ensure the clocks are probed
hierarchically.
This support is enabled using CFG_DRIVERS_CLK_DT.

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

3 files changed, 507 insertions, 0 deletions

diff --git a/core/drivers/clk/clk_dt.c b/core/drivers/clk/clk_dt.c
new file mode 100644
index 00000000..2434004c
--- /dev/null
+++ b/core/drivers/clk/clk_dt.c
@@ -0,0 +1,392 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2021, Bootlin
+ */
+
+#include <assert.h>
+#include <drivers/clk.h>
+#include <drivers/clk_dt.h>
+#include <initcall.h>
+#include <kernel/boot.h>
+#include <kernel/panic.h>
+#include <libfdt.h>
+#include <stddef.h>
+
+struct clk_dt_provider {
+	int nodeoffset;
+	unsigned int clock_cells;
+	uint32_t phandle;
+	clk_dt_get_fn get_dt_clk;
+	void *data;
+	SLIST_ENTRY(clk_dt_provider) link;
+};
+
+static SLIST_HEAD(, clk_dt_provider) clk_dt_provider_list =
+				SLIST_HEAD_INITIALIZER(clk_dt_provider_list);
+
+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_fn get_dt_clk, void *data)
+{
+	struct clk_dt_provider *prv = NULL;
+	int clock_cells = 0;
+
+	prv = calloc(1, sizeof(*prv));
+	if (!prv)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	prv->get_dt_clk = get_dt_clk;
+	prv->data = data;
+	prv->nodeoffset = nodeoffset;
+	clock_cells = fdt_clock_cells(fdt, nodeoffset);
+	if (clock_cells < 0) {
+		free(prv);
+		return TEE_ERROR_GENERIC;
+	}
+	prv->clock_cells = clock_cells;
+	prv->phandle = fdt_get_phandle(fdt, nodeoffset);
+
+	SLIST_INSERT_HEAD(&clk_dt_provider_list, prv, link);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result clk_dt_release_provider(void)
+{
+	struct clk_dt_provider *prv = NULL;
+
+	while (!SLIST_EMPTY(&clk_dt_provider_list)) {
+		prv = SLIST_FIRST(&clk_dt_provider_list);
+		SLIST_REMOVE_HEAD(&clk_dt_provider_list, link);
+		free(prv);
+	}
+
+	return TEE_SUCCESS;
+}
+
+driver_init_late(clk_dt_release_provider);
+
+static struct clk_dt_provider *clk_get_provider_by_node(int nodeoffset)
+{
+	struct clk_dt_provider *prv = NULL;
+
+	SLIST_FOREACH(prv, &clk_dt_provider_list, link)
+		if (prv->nodeoffset == nodeoffset)
+			return prv;
+
+	return NULL;
+}
+
+static struct clk_dt_provider *clk_get_provider_by_phandle(uint32_t phandle)
+{
+	struct clk_dt_provider *prv = NULL;
+
+	SLIST_FOREACH(prv, &clk_dt_provider_list, link)
+		if (prv->phandle == phandle)
+			return prv;
+
+	return NULL;
+}
+
+static struct clk *clk_dt_get_from_provider(struct clk_dt_provider *prv,
+					    unsigned int clock_cells,
+					    const uint32_t *prop)
+{
+	unsigned int arg = 0;
+	struct clk *clk = NULL;
+	struct clk_dt_phandle_args pargs = { };
+
+	pargs.args_count = clock_cells;
+	pargs.args = calloc(pargs.args_count, sizeof(uint32_t));
+	if (!pargs.args)
+		return NULL;
+
+	for (arg = 0; arg < clock_cells; arg++)
+		pargs.args[arg] = fdt32_to_cpu(prop[arg + 1]);
+
+	clk = prv->get_dt_clk(&pargs, prv->data);
+	free(pargs.args);
+
+	return clk;
+}
+
+struct clk *clk_dt_get_by_name(const void *fdt, int nodeoffset,
+			       const char *name)
+{
+	int clk_id = 0;
+
+	clk_id = fdt_stringlist_search(fdt, nodeoffset, "clock-names", name);
+	if (clk_id < 0)
+		return NULL;
+
+	return clk_dt_get_by_idx(fdt, nodeoffset, clk_id);
+}
+
+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 clk_dt_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 = clk_get_provider_by_phandle(phandle);
+		if (!prv)
+			return NULL;
+
+		clock_cells = prv->clock_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;
+}
+
+struct clk *clk_dt_get_by_idx(const void *fdt, int nodeoffset,
+			      unsigned int clk_idx)
+{
+	return clk_dt_get_by_idx_prop("clocks", fdt, nodeoffset, clk_idx);
+}
+
+static const struct clk_driver *clk_get_compatible_driver(const char *compat)
+{
+	const struct dt_driver *drv = NULL;
+	const struct dt_device_match *dm = NULL;
+	const struct clk_driver *clk_drv = NULL;
+
+	for_each_dt_driver(drv) {
+		if (drv->type != DT_DRIVER_CLK)
+			continue;
+
+		clk_drv = (const struct clk_driver *)drv->driver;
+		for (dm = drv->match_table; dm && dm->compatible; dm++) {
+			if (strcmp(dm->compatible, compat) == 0)
+				return clk_drv;
+		}
+	}
+
+	return NULL;
+}
+
+/* Recursively called from parse_clock_property() */
+static TEE_Result clk_probe_clock_provider_node(const void *fdt, int node);
+
+static TEE_Result parse_clock_property(const void *fdt, int node)
+{
+	int len = 0;
+	int idx = 0;
+	int parent_node = 0;
+	int clock_cells = 0;
+	uint32_t phandle = 0;
+	const uint32_t *prop = NULL;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	prop = fdt_getprop(fdt, node, "clocks", &len);
+	if (!prop)
+		return TEE_SUCCESS;
+
+	len /= sizeof(uint32_t);
+	while (idx < len) {
+		phandle = fdt32_to_cpu(prop[idx]);
+
+		parent_node = fdt_node_offset_by_phandle(fdt, phandle);
+		if (parent_node < 0)
+			return TEE_ERROR_GENERIC;
+
+		/* Parent setup should not fail or clock won't be available */
+		res = clk_probe_clock_provider_node(fdt, parent_node);
+		if (res)
+			panic("Failed to setup parent clock");
+
+		clock_cells = fdt_clock_cells(fdt, parent_node);
+		if (clock_cells < 0)
+			return TEE_ERROR_GENERIC;
+
+		idx += 1 + clock_cells;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result clk_dt_node_clock_setup_driver(const void *fdt, int node)
+{
+	int idx = 0;
+	int len = 0;
+	int count = 0;
+	const char *compat = NULL;
+	TEE_Result res = TEE_ERROR_GENERIC;
+	const struct clk_driver *clk_drv = NULL;
+
+	count = fdt_stringlist_count(fdt, node, "compatible");
+	if (count < 0)
+		return TEE_ERROR_GENERIC;
+
+	for (idx = 0; idx < count; idx++) {
+		compat = fdt_stringlist_get(fdt, node, "compatible", idx, &len);
+		if (!compat)
+			return TEE_ERROR_GENERIC;
+
+		clk_drv = clk_get_compatible_driver(compat);
+		if (!clk_drv)
+			continue;
+
+		res = clk_drv->probe(fdt, node);
+		if (res != TEE_SUCCESS) {
+			EMSG("Failed to probe clock driver for compatible %s",
+			     compat);
+			panic();
+		} else {
+			return TEE_SUCCESS;
+		}
+	}
+
+	return TEE_ERROR_GENERIC;
+}
+
+static TEE_Result clk_probe_clock_provider_node(const void *fdt, int node)
+{
+	int len = 0;
+	int status = 0;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	status = _fdt_get_status(fdt, node);
+	if (!(status & DT_STATUS_OK_SEC))
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	/* Check if the node is a clock provider */
+	if (!fdt_getprop(fdt, node, "#clock-cells", &len))
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	/* Check if node has already been probed */
+	if (clk_get_provider_by_node(node))
+		return TEE_SUCCESS;
+
+	/* Check if the node has a clock property first to setup parent */
+	res = parse_clock_property(fdt, node);
+	if (res)
+		return res;
+
+	return clk_dt_node_clock_setup_driver(fdt, node);
+}
+
+static void clk_probe_node(const void *fdt, int parent_node)
+{
+	int child = 0;
+	int status = 0;
+	__maybe_unused TEE_Result res = TEE_ERROR_GENERIC;
+
+	fdt_for_each_subnode(child, fdt, parent_node) {
+		status = _fdt_get_status(fdt, child);
+		if (status == DT_STATUS_DISABLED)
+			continue;
+
+		res = clk_probe_clock_provider_node(fdt, child);
+		assert(res == TEE_SUCCESS || res == TEE_ERROR_ITEM_NOT_FOUND);
+
+		clk_probe_node(fdt, child);
+	}
+}
+
+static void parse_assigned_clock(const void *fdt, int nodeoffset)
+{
+	int rate_len = 0;
+	int clock_idx = 0;
+	struct clk *clk = NULL;
+	unsigned long rate = 0;
+	struct clk *parent = NULL;
+	const uint32_t *rate_prop = NULL;
+
+	rate_prop = fdt_getprop(fdt, nodeoffset, "assigned-clock-rates",
+				&rate_len);
+	rate_len /= sizeof(uint32_t);
+
+	while (1) {
+		clk = clk_dt_get_by_idx_prop("assigned-clocks", fdt, nodeoffset,
+					     clock_idx);
+		if (!clk)
+			return;
+
+		parent = clk_dt_get_by_idx_prop("assigned-clock-parents", fdt,
+						nodeoffset, clock_idx);
+		if (parent) {
+			if (clk_set_parent(clk, parent)) {
+				EMSG("Could not set clk %s parent to clock %s",
+				     clk->name, parent->name);
+				panic();
+			}
+		}
+
+		if (rate_prop && clock_idx <= rate_len) {
+			rate = fdt32_to_cpu(rate_prop[clock_idx]);
+			if (rate && clk_set_rate(clk, rate) != TEE_SUCCESS)
+				panic();
+		}
+
+		clock_idx++;
+	}
+}
+
+static void clk_probe_assigned(const void *fdt, int parent_node)
+{
+	int len = 0;
+	int child = 0;
+	int status = 0;
+
+	fdt_for_each_subnode(child, fdt, parent_node) {
+		clk_probe_assigned(fdt, child);
+
+		status = _fdt_get_status(fdt, child);
+		if (status == DT_STATUS_DISABLED)
+			continue;
+
+		if (fdt_getprop(fdt, child, "assigned-clocks", &len))
+			parse_assigned_clock(fdt, child);
+	}
+}
+
+static TEE_Result clk_dt_probe(void)
+{
+	const void *fdt = get_embedded_dt();
+
+	DMSG("Probing clocks from devicetree");
+	if (!fdt)
+		panic();
+
+	clk_probe_node(fdt, -1);
+
+	clk_probe_assigned(fdt, -1);
+
+	return TEE_SUCCESS;
+}
+early_init(clk_dt_probe);
diff --git a/core/drivers/clk/sub.mk b/core/drivers/clk/sub.mk
index 38bd8aa3..f8da73f7 100644
--- a/core/drivers/clk/sub.mk
+++ b/core/drivers/clk/sub.mk
@@ -1 +1,2 @@
 srcs-y += clk.c
+srcs-$(CFG_DRIVERS_CLK_DT) += clk_dt.c
\ No newline at end of file
diff --git a/core/include/drivers/clk_dt.h b/core/include/drivers/clk_dt.h
new file mode 100644
index 00000000..fded283d
--- /dev/null
+++ b/core/include/drivers/clk_dt.h
@@ -0,0 +1,114 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (c) 2021, Bootlin
+ */
+
+#ifndef __DRIVERS_CLK_DT_H
+#define __DRIVERS_CLK_DT_H
+
+#include <kernel/dt.h>
+#include <stdint.h>
+#include <sys/queue.h>
+
+/**
+ * struct clk_dt_phandle_args - Devicetree clock args
+ * @nodeoffset: Clock consumer node offset
+ * @args_count: Count of items in @args
+ * @args: Clocks consumer specifiers
+ */
+struct clk_dt_phandle_args {
+	int nodeoffset;
+	int args_count;
+	uint32_t *args;
+};
+
+/**
+ * struct clk_driver - Clock driver setup struct
+ * probe: probe function for the clock driver
+ */
+struct clk_driver {
+	TEE_Result (*probe)(const void *fdt, int nodeoffset);
+};
+
+/**
+ * CLK_DT_DECLARE - Declare a clock driver
+ * @__name: Clock driver name
+ * @__compat: Compatible string
+ * @__probe: Clock probe function
+ */
+#define CLK_DT_DECLARE(__name, __compat, __probe) \
+	static const struct clk_driver __name ## _driver = { \
+		.probe = __probe, \
+	}; \
+	static const struct dt_device_match __name ## _match_table[] = { \
+		{ .compatible = __compat }, \
+		{ } \
+	}; \
+	const struct dt_driver __name ## _dt_driver __dt_driver = { \
+		.name = # __name, \
+		.type = DT_DRIVER_CLK, \
+		.match_table = __name ## _match_table, \
+		.driver = &__name ## _driver, \
+	}
+
+/**
+ * clk_dt_get_by_idx - Get a clock at a specific index in "clocks" property
+ *
+ * @fdt: Device tree to work on
+ * @nodeoffset: Node offset of the subnode containing a clock property
+ * @clk_idx: Clock index to get
+ * Returns a clk struct pointer matching the clock at index clk_idx in clocks
+ * property or NULL if no clock match the given index.
+ */
+struct clk *clk_dt_get_by_idx(const void *fdt, int nodeoffset,
+			      unsigned int clk_idx);
+
+/**
+ * clk_dt_get_by_name - Get a clock matching a name in the "clock-names"
+ * property
+ *
+ * @fdt: Device tree to work on
+ * @nodeoffset: Node offset of the subnode containing a clock property
+ * @name: Clock name to get
+ * Returns a clk struct pointer matching the name in "clock-names" property or
+ * NULL if no clock match the given name.
+ */
+struct clk *clk_dt_get_by_name(const void *fdt, int nodeoffset,
+			       const char *name);
+
+/**
+ * clk_dt_get_fn - Typedef of function to get clock from devicetree properties
+ *
+ * @args: Pointer to devicetree description of the clock to parse
+ * @data: Pointer to data given at clk_dt_register_clk_provider() call
+ *
+ * Returns a clk struct pointer pointing to a clock matching the devicetree
+ * description or NULL if invalid description.
+ */
+typedef struct clk *(*clk_dt_get_fn)(struct clk_dt_phandle_args *args,
+				     void *data);
+
+/**
+ * clk_dt_register_clk_provider - Register a clock provider
+ *
+ * @fdt: Device tree to work on
+ * @nodeoffset: Node offset of the clock
+ * @get_dt_clk: Callback to match the devicetree clock with a clock struct
+ * @data: Data which will be passed to the get_dt_clk callback
+ * Returns TEE_Result value
+ */
+TEE_Result clk_dt_register_clk_provider(const void *fdt, int nodeoffset,
+					clk_dt_get_fn get_dt_clk, void *data);
+
+/**
+ * clk_dt_get_simple_clk: simple clock matching function for single clock
+ * providers
+ */
+static inline
+struct clk *clk_dt_get_simple_clk(struct clk_dt_phandle_args *args __unused,
+				  void *data)
+{
+	return data;
+}
+
+#endif /* __DRIVERS_CLK_DT_H */