icc-clk.c

  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	onecell->num_nodes = 2 * num_clocks;
 91
 92	qp = devm_kzalloc(dev, struct_size(qp, clocks, num_clocks), GFP_KERNEL);
 93	if (!qp)
 94		return ERR_PTR(-ENOMEM);
 95
 96	qp->num_clocks = num_clocks;
 97
 98	provider = &qp->provider;
 99	provider->dev = dev;
100	provider->get_bw = icc_clk_get_bw;
101	provider->set = icc_clk_set;
102	provider->aggregate = icc_std_aggregate;
103	provider->xlate = of_icc_xlate_onecell;
104	INIT_LIST_HEAD(&provider->nodes);
105	provider->data = onecell;
106
107	icc_provider_init(provider);
108
109	for (i = 0, j = 0; i < num_clocks; i++) {
110		qp->clocks[i].clk = data[i].clk;
111
112		node = icc_node_create(first_id + data[i].master_id);
113		if (IS_ERR(node)) {
114			ret = PTR_ERR(node);
115			goto err;
116		}
117
118		node->name = devm_kasprintf(dev, GFP_KERNEL, "%s_master", data[i].name);
119		if (!node->name) {
120			icc_node_destroy(node->id);
121			ret = -ENOMEM;
122			goto err;
123		}
124
125		node->data = &qp->clocks[i];
126		icc_node_add(node, provider);
127		/* link to the next node, slave */
128		icc_link_create(node, first_id + data[i].slave_id);
129		onecell->nodes[j++] = node;
130
131		node = icc_node_create(first_id + data[i].slave_id);
132		if (IS_ERR(node)) {
133			ret = PTR_ERR(node);
134			goto err;
135		}
136
137		node->name = devm_kasprintf(dev, GFP_KERNEL, "%s_slave", data[i].name);
138		if (!node->name) {
139			icc_node_destroy(node->id);
140			ret = -ENOMEM;
141			goto err;
142		}
143
144		/* no data for slave node */
145		icc_node_add(node, provider);
146		onecell->nodes[j++] = node;
147	}
148
149	ret = icc_provider_register(provider);
150	if (ret)
151		goto err;
152
153	return provider;
154
155err:
156	icc_nodes_remove(provider);
157
158	return ERR_PTR(ret);
159}
160EXPORT_SYMBOL_GPL(icc_clk_register);
161
162static void devm_icc_release(void *res)
163{
164	icc_clk_unregister(res);
165}
166
167int devm_icc_clk_register(struct device *dev, unsigned int first_id,
168			  unsigned int num_clocks, const struct icc_clk_data *data)
169{
170	struct icc_provider *prov;
171
172	prov = icc_clk_register(dev, first_id, num_clocks, data);
173	if (IS_ERR(prov))
174		return PTR_ERR(prov);
175
176	return devm_add_action_or_reset(dev, devm_icc_release, prov);
177}
178EXPORT_SYMBOL_GPL(devm_icc_clk_register);
179
180/**
181 * icc_clk_unregister() - unregister a previously registered clk interconnect provider
182 * @provider: provider returned by icc_clk_register()
183 */
184void icc_clk_unregister(struct icc_provider *provider)
185{
186	struct icc_clk_provider *qp = container_of(provider, struct icc_clk_provider, provider);
187	int i;
188
189	icc_provider_deregister(&qp->provider);
190	icc_nodes_remove(&qp->provider);
191
192	for (i = 0; i < qp->num_clocks; i++) {
193		struct icc_clk_node *qn = &qp->clocks[i];
194
195		if (qn->enabled)
196			clk_disable_unprepare(qn->clk);
197	}
198}
199EXPORT_SYMBOL_GPL(icc_clk_unregister);
200
201MODULE_LICENSE("GPL");
202MODULE_DESCRIPTION("Interconnect wrapper for clocks");
203MODULE_AUTHOR("Dmitry Baryshkov <dmitry.baryshkov@linaro.org>");