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
11 struct icc_clk_node {
12 struct clk *clk;
13 bool enabled;
14 };
15
16 struct 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
icc_clk_set(struct icc_node * src,struct icc_node * dst)25 static 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
icc_clk_get_bw(struct icc_node * node,u32 * avg,u32 * peak)51 static 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 */
icc_clk_register(struct device * dev,unsigned int first_id,unsigned int num_clocks,const struct icc_clk_data * data)76 struct 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
155 err:
156 icc_nodes_remove(provider);
157
158 return ERR_PTR(ret);
159 }
160 EXPORT_SYMBOL_GPL(icc_clk_register);
161
devm_icc_release(void * res)162 static void devm_icc_release(void *res)
163 {
164 icc_clk_unregister(res);
165 }
166
devm_icc_clk_register(struct device * dev,unsigned int first_id,unsigned int num_clocks,const struct icc_clk_data * data)167 int 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 }
178 EXPORT_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 */
icc_clk_unregister(struct icc_provider * provider)184 void 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 }
199 EXPORT_SYMBOL_GPL(icc_clk_unregister);
200
201 MODULE_LICENSE("GPL");
202 MODULE_DESCRIPTION("Interconnect wrapper for clocks");
203 MODULE_AUTHOR("Dmitry Baryshkov <dmitry.baryshkov@linaro.org>");
204