1/* SPDX-License-Identifier: GPL-2.0 */
2/*
3 * Copyright (c) 2023, Linaro Ltd.
4 */
5
6#include <linux/clk.h>
7#include <linux/device.h>
8#include <linux/interconnect-clk.h>
9#include <linux/interconnect-provider.h>
10
11struct icc_clk_node {
12	struct clk *clk;
13	bool enabled;
14};
15
16struct icc_clk_provider {
17	struct icc_provider provider;
18	int num_clocks;
19	struct icc_clk_node clocks[] __counted_by(num_clocks);
20};
21
22#define to_icc_clk_provider(_provider) \
23	container_of(_provider, struct icc_clk_provider, provider)
24
25static int icc_clk_set(struct icc_node *src, struct icc_node *dst)
26{
27	struct icc_clk_node *qn = src->data;
28	int ret;
29
30	if (!qn || !qn->clk)
31		return 0;
32
33	if (!src->peak_bw) {
34		if (qn->enabled)
35			clk_disable_unprepare(qn->clk);
36		qn->enabled = false;
37
38		return 0;
39	}
40
41	if (!qn->enabled) {
42		ret = clk_prepare_enable(qn->clk);
43		if (ret)
44			return ret;
45		qn->enabled = true;
46	}
47
48	return clk_set_rate(qn->clk, icc_units_to_bps(src->peak_bw));
49}
50
51static int icc_clk_get_bw(struct icc_node *node, u32 *avg, u32 *peak)
52{
53	struct icc_clk_node *qn = node->data;
54
55	if (!qn || !qn->clk)
56		*peak = INT_MAX;
57	else
58		*peak = Bps_to_icc(clk_get_rate(qn->clk));
59
60	return 0;
61}
62
63/**
64 * icc_clk_register() - register a new clk-based interconnect provider
65 * @dev: device supporting this provider
66 * @first_id: an ID of the first provider's node
67 * @num_clocks: number of instances of struct icc_clk_data
68 * @data: data for the provider
69 *
70 * Registers and returns a clk-based interconnect provider. It is a simple
71 * wrapper around COMMON_CLK framework, allowing other devices to vote on the
72 * clock rate.
73 *
74 * Return: 0 on success, or an error code otherwise
75 */
76struct icc_provider *icc_clk_register(struct device *dev,
77				      unsigned int first_id,
78				      unsigned int num_clocks,
79				      const struct icc_clk_data *data)
80{
81	struct icc_clk_provider *qp;
82	struct icc_provider *provider;
83	struct icc_onecell_data *onecell;
84	struct icc_node *node;
85	int ret, i, j;
86
87	onecell = devm_kzalloc(dev, struct_size(onecell, nodes, 2 * num_clocks), GFP_KERNEL);
88	if (!onecell)
89		return ERR_PTR(-ENOMEM);
90
91	qp = devm_kzalloc(dev, struct_size(qp, clocks, num_clocks), GFP_KERNEL);
92	if (!qp)
93		return ERR_PTR(-ENOMEM);
94
95	qp->num_clocks = num_clocks;
96
97	provider = &qp->provider;
98	provider->dev = dev;
99	provider->get_bw = icc_clk_get_bw;
100	provider->set = icc_clk_set;
101	provider->aggregate = icc_std_aggregate;
102	provider->xlate = of_icc_xlate_onecell;
103	INIT_LIST_HEAD(&provider->nodes);
104	provider->data = onecell;
105
106	icc_provider_init(provider);
107
108	for (i = 0, j = 0; i < num_clocks; i++) {
109		qp->clocks[i].clk = data[i].clk;
110
111		node = icc_node_create(first_id + j);
112		if (IS_ERR(node)) {
113			ret = PTR_ERR(node);
114			goto err;
115		}
116
117		node->name = devm_kasprintf(dev, GFP_KERNEL, "%s_master", data[i].name);
118		node->data = &qp->clocks[i];
119		icc_node_add(node, provider);
120		/* link to the next node, slave */
121		icc_link_create(node, first_id + j + 1);
122		onecell->nodes[j++] = node;
123
124		node = icc_node_create(first_id + j);
125		if (IS_ERR(node)) {
126			ret = PTR_ERR(node);
127			goto err;
128		}
129
130		node->name = devm_kasprintf(dev, GFP_KERNEL, "%s_slave", data[i].name);
131		/* no data for slave node */
132		icc_node_add(node, provider);
133		onecell->nodes[j++] = node;
134	}
135
136	onecell->num_nodes = j;
137
138	ret = icc_provider_register(provider);
139	if (ret)
140		goto err;
141
142	return provider;
143
144err:
145	icc_nodes_remove(provider);
146
147	return ERR_PTR(ret);
148}
149EXPORT_SYMBOL_GPL(icc_clk_register);
150
151/**
152 * icc_clk_unregister() - unregister a previously registered clk interconnect provider
153 * @provider: provider returned by icc_clk_register()
154 */
155void icc_clk_unregister(struct icc_provider *provider)
156{
157	struct icc_clk_provider *qp = container_of(provider, struct icc_clk_provider, provider);
158	int i;
159
160	icc_provider_deregister(&qp->provider);
161	icc_nodes_remove(&qp->provider);
162
163	for (i = 0; i < qp->num_clocks; i++) {
164		struct icc_clk_node *qn = &qp->clocks[i];
165
166		if (qn->enabled)
167			clk_disable_unprepare(qn->clk);
168	}
169}
170EXPORT_SYMBOL_GPL(icc_clk_unregister);
171
172MODULE_LICENSE("GPL");
173MODULE_DESCRIPTION("Interconnect wrapper for clocks");
174MODULE_AUTHOR("Dmitry Baryshkov <dmitry.baryshkov@linaro.org>");
175