1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (c) 2020, The Linux Foundation. All rights reserved.
4 * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved.
5 */
6
7 #include <linux/bitfield.h>
8 #include <linux/clk.h>
9 #include <linux/interconnect.h>
10 #include <linux/interconnect-provider.h>
11 #include <linux/module.h>
12 #include <linux/of.h>
13 #include <linux/of_platform.h>
14 #include <linux/slab.h>
15
16 #include "bcm-voter.h"
17 #include "icc-common.h"
18 #include "icc-rpmh.h"
19
20 /* QNOC QoS */
21 #define QOSGEN_MAINCTL_LO(p, qp) (0x8 + (p->port_offsets[qp]))
22 #define QOS_SLV_URG_MSG_EN_MASK GENMASK(3, 3)
23 #define QOS_DFLT_PRIO_MASK GENMASK(6, 4)
24 #define QOS_DISABLE_MASK GENMASK(24, 24)
25
26 /**
27 * qcom_icc_set_qos - initialize static QoS configurations
28 * @qp: qcom icc provider to which @node belongs
29 * @node: qcom icc node to operate on
30 */
qcom_icc_set_qos(struct qcom_icc_provider * qp,struct qcom_icc_node * node)31 static void qcom_icc_set_qos(struct qcom_icc_provider *qp,
32 struct qcom_icc_node *node)
33 {
34 const struct qcom_icc_qosbox *qos = node->qosbox;
35 int port;
36
37 for (port = 0; port < qos->num_ports; port++) {
38 regmap_update_bits(qp->regmap, QOSGEN_MAINCTL_LO(qos, port),
39 QOS_DISABLE_MASK,
40 FIELD_PREP(QOS_DISABLE_MASK, qos->prio_fwd_disable));
41
42 regmap_update_bits(qp->regmap, QOSGEN_MAINCTL_LO(qos, port),
43 QOS_DFLT_PRIO_MASK,
44 FIELD_PREP(QOS_DFLT_PRIO_MASK, qos->prio));
45
46 regmap_update_bits(qp->regmap, QOSGEN_MAINCTL_LO(qos, port),
47 QOS_SLV_URG_MSG_EN_MASK,
48 FIELD_PREP(QOS_SLV_URG_MSG_EN_MASK, qos->urg_fwd));
49 }
50 }
51
52 /**
53 * qcom_icc_pre_aggregate - cleans up stale values from prior icc_set
54 * @node: icc node to operate on
55 */
qcom_icc_pre_aggregate(struct icc_node * node)56 void qcom_icc_pre_aggregate(struct icc_node *node)
57 {
58 size_t i;
59 struct qcom_icc_node *qn;
60 struct qcom_icc_provider *qp;
61
62 qn = node->data;
63 qp = to_qcom_provider(node->provider);
64
65 for (i = 0; i < QCOM_ICC_NUM_BUCKETS; i++) {
66 qn->sum_avg[i] = 0;
67 qn->max_peak[i] = 0;
68 }
69
70 for (i = 0; i < qn->num_bcms; i++)
71 qcom_icc_bcm_voter_add(qp->voter, qn->bcms[i]);
72 }
73 EXPORT_SYMBOL_GPL(qcom_icc_pre_aggregate);
74
75 /**
76 * qcom_icc_aggregate - aggregate bw for buckets indicated by tag
77 * @node: node to aggregate
78 * @tag: tag to indicate which buckets to aggregate
79 * @avg_bw: new bw to sum aggregate
80 * @peak_bw: new bw to max aggregate
81 * @agg_avg: existing aggregate avg bw val
82 * @agg_peak: existing aggregate peak bw val
83 */
qcom_icc_aggregate(struct icc_node * node,u32 tag,u32 avg_bw,u32 peak_bw,u32 * agg_avg,u32 * agg_peak)84 int qcom_icc_aggregate(struct icc_node *node, u32 tag, u32 avg_bw,
85 u32 peak_bw, u32 *agg_avg, u32 *agg_peak)
86 {
87 size_t i;
88 struct qcom_icc_node *qn;
89
90 qn = node->data;
91
92 if (!tag)
93 tag = QCOM_ICC_TAG_ALWAYS;
94
95 for (i = 0; i < QCOM_ICC_NUM_BUCKETS; i++) {
96 if (tag & BIT(i)) {
97 qn->sum_avg[i] += avg_bw;
98 qn->max_peak[i] = max_t(u32, qn->max_peak[i], peak_bw);
99 }
100
101 if (node->init_avg || node->init_peak) {
102 qn->sum_avg[i] = max_t(u64, qn->sum_avg[i], node->init_avg);
103 qn->max_peak[i] = max_t(u64, qn->max_peak[i], node->init_peak);
104 }
105 }
106
107 *agg_avg += avg_bw;
108 *agg_peak = max_t(u32, *agg_peak, peak_bw);
109
110 return 0;
111 }
112 EXPORT_SYMBOL_GPL(qcom_icc_aggregate);
113
114 /**
115 * qcom_icc_set - set the constraints based on path
116 * @src: source node for the path to set constraints on
117 * @dst: destination node for the path to set constraints on
118 *
119 * Return: 0 on success, or an error code otherwise
120 */
qcom_icc_set(struct icc_node * src,struct icc_node * dst)121 int qcom_icc_set(struct icc_node *src, struct icc_node *dst)
122 {
123 struct qcom_icc_provider *qp;
124 struct icc_node *node;
125
126 if (!src)
127 node = dst;
128 else
129 node = src;
130
131 qp = to_qcom_provider(node->provider);
132
133 qcom_icc_bcm_voter_commit(qp->voter);
134
135 return 0;
136 }
137 EXPORT_SYMBOL_GPL(qcom_icc_set);
138
139 /**
140 * qcom_icc_bcm_init - populates bcm aux data and connect qnodes
141 * @bcm: bcm to be initialized
142 * @dev: associated provider device
143 *
144 * Return: 0 on success, or an error code otherwise
145 */
qcom_icc_bcm_init(struct qcom_icc_bcm * bcm,struct device * dev)146 int qcom_icc_bcm_init(struct qcom_icc_bcm *bcm, struct device *dev)
147 {
148 struct qcom_icc_node *qn;
149 const struct bcm_db *data;
150 size_t data_count;
151 int i;
152
153 /* BCM is already initialised*/
154 if (bcm->addr)
155 return 0;
156
157 bcm->addr = cmd_db_read_addr(bcm->name);
158 if (!bcm->addr) {
159 dev_err(dev, "%s could not find RPMh address\n",
160 bcm->name);
161 return -EINVAL;
162 }
163
164 data = cmd_db_read_aux_data(bcm->name, &data_count);
165 if (IS_ERR(data)) {
166 dev_err(dev, "%s command db read error (%ld)\n",
167 bcm->name, PTR_ERR(data));
168 return PTR_ERR(data);
169 }
170 if (!data_count) {
171 dev_err(dev, "%s command db missing or partial aux data\n",
172 bcm->name);
173 return -EINVAL;
174 }
175
176 bcm->aux_data.unit = le32_to_cpu(data->unit);
177 bcm->aux_data.width = le16_to_cpu(data->width);
178 bcm->aux_data.vcd = data->vcd;
179 bcm->aux_data.reserved = data->reserved;
180 INIT_LIST_HEAD(&bcm->list);
181 INIT_LIST_HEAD(&bcm->ws_list);
182
183 if (!bcm->vote_scale)
184 bcm->vote_scale = 1000;
185
186 /* Link Qnodes to their respective BCMs */
187 for (i = 0; i < bcm->num_nodes; i++) {
188 qn = bcm->nodes[i];
189 qn->bcms[qn->num_bcms] = bcm;
190 qn->num_bcms++;
191 }
192
193 return 0;
194 }
195 EXPORT_SYMBOL_GPL(qcom_icc_bcm_init);
196
197 /**
198 * qcom_icc_rpmh_configure_qos - configure QoS parameters
199 * @qp: qcom icc provider associated with QoS endpoint nodes
200 *
201 * Return: 0 on success, or an error code otherwise
202 */
qcom_icc_rpmh_configure_qos(struct qcom_icc_provider * qp)203 static int qcom_icc_rpmh_configure_qos(struct qcom_icc_provider *qp)
204 {
205 struct qcom_icc_node *qnode;
206 size_t i;
207 int ret;
208
209 ret = clk_bulk_prepare_enable(qp->num_clks, qp->clks);
210 if (ret)
211 return ret;
212
213 for (i = 0; i < qp->num_nodes; i++) {
214 qnode = qp->nodes[i];
215 if (!qnode)
216 continue;
217
218 if (qnode->qosbox)
219 qcom_icc_set_qos(qp, qnode);
220 }
221
222 clk_bulk_disable_unprepare(qp->num_clks, qp->clks);
223
224 return ret;
225 }
226
qcom_icc_rpmh_probe(struct platform_device * pdev)227 int qcom_icc_rpmh_probe(struct platform_device *pdev)
228 {
229 const struct qcom_icc_desc *desc;
230 struct device *dev = &pdev->dev;
231 struct icc_onecell_data *data;
232 struct icc_provider *provider;
233 struct qcom_icc_node * const *qnodes, *qn;
234 struct qcom_icc_provider *qp;
235 struct icc_node *node;
236 size_t num_nodes, i, j;
237 int ret;
238
239 desc = of_device_get_match_data(dev);
240 if (!desc)
241 return -EINVAL;
242
243 qnodes = desc->nodes;
244 num_nodes = desc->num_nodes;
245
246 qp = devm_kzalloc(dev, sizeof(*qp), GFP_KERNEL);
247 if (!qp)
248 return -ENOMEM;
249
250 data = devm_kzalloc(dev, struct_size(data, nodes, num_nodes), GFP_KERNEL);
251 if (!data)
252 return -ENOMEM;
253 data->num_nodes = num_nodes;
254
255 provider = &qp->provider;
256 provider->dev = dev;
257 provider->set = qcom_icc_set;
258 provider->pre_aggregate = qcom_icc_pre_aggregate;
259 provider->aggregate = qcom_icc_aggregate;
260 provider->xlate_extended = qcom_icc_xlate_extended;
261 provider->data = data;
262
263 icc_provider_init(provider);
264
265 qp->dev = dev;
266 qp->bcms = desc->bcms;
267 qp->nodes = desc->nodes;
268 qp->num_bcms = desc->num_bcms;
269 qp->num_nodes = desc->num_nodes;
270
271 qp->voter = of_bcm_voter_get(qp->dev, NULL);
272 if (IS_ERR(qp->voter))
273 return PTR_ERR(qp->voter);
274
275 for (i = 0; i < qp->num_bcms; i++)
276 qcom_icc_bcm_init(qp->bcms[i], dev);
277
278 for (i = 0; i < num_nodes; i++) {
279 qn = qnodes[i];
280 if (!qn)
281 continue;
282
283 if (desc->alloc_dyn_id) {
284 if (!qn->node)
285 qn->node = icc_node_create_dyn();
286 node = qn->node;
287 } else {
288 node = icc_node_create(qn->id);
289 }
290
291 if (IS_ERR(node)) {
292 ret = PTR_ERR(node);
293 goto err_remove_nodes;
294 }
295
296 ret = icc_node_set_name(node, provider, qn->name);
297 if (ret) {
298 icc_node_destroy(node->id);
299 goto err_remove_nodes;
300 }
301
302 node->data = qn;
303 icc_node_add(node, provider);
304
305 for (j = 0; j < qn->num_links; j++) {
306 if (desc->alloc_dyn_id)
307 icc_link_nodes(node, &qn->link_nodes[j]->node);
308 else
309 icc_link_create(node, qn->links[j]);
310 }
311
312 data->nodes[i] = node;
313 }
314
315 if (desc->config) {
316 struct resource *res;
317 void __iomem *base;
318
319 base = devm_platform_get_and_ioremap_resource(pdev, 0, &res);
320 if (IS_ERR(base))
321 goto skip_qos_config;
322
323 qp->regmap = devm_regmap_init_mmio(dev, base, desc->config);
324 if (IS_ERR(qp->regmap)) {
325 dev_info(dev, "Skipping QoS, regmap failed; %ld\n", PTR_ERR(qp->regmap));
326 goto skip_qos_config;
327 }
328
329 qp->num_clks = devm_clk_bulk_get_all(qp->dev, &qp->clks);
330 if (qp->num_clks == -EPROBE_DEFER)
331 return dev_err_probe(dev, qp->num_clks, "Failed to get QoS clocks\n");
332
333 if (qp->num_clks < 0 || (!qp->num_clks && desc->qos_requires_clocks)) {
334 dev_info(dev, "Skipping QoS, failed to get clk: %d\n", qp->num_clks);
335 goto skip_qos_config;
336 }
337
338 ret = qcom_icc_rpmh_configure_qos(qp);
339 if (ret)
340 dev_info(dev, "Failed to program QoS: %d\n", ret);
341 }
342
343 skip_qos_config:
344 ret = icc_provider_register(provider);
345 if (ret)
346 goto err_remove_nodes;
347
348 platform_set_drvdata(pdev, qp);
349
350 /* Populate child NoC devices if any */
351 if (of_get_child_count(dev->of_node) > 0) {
352 ret = of_platform_populate(dev->of_node, NULL, NULL, dev);
353 if (ret)
354 goto err_deregister_provider;
355 }
356
357 return 0;
358
359 err_deregister_provider:
360 icc_provider_deregister(provider);
361 err_remove_nodes:
362 icc_nodes_remove(provider);
363
364 return ret;
365 }
366 EXPORT_SYMBOL_GPL(qcom_icc_rpmh_probe);
367
qcom_icc_rpmh_remove(struct platform_device * pdev)368 void qcom_icc_rpmh_remove(struct platform_device *pdev)
369 {
370 struct qcom_icc_provider *qp = platform_get_drvdata(pdev);
371
372 icc_provider_deregister(&qp->provider);
373 icc_nodes_remove(&qp->provider);
374 }
375 EXPORT_SYMBOL_GPL(qcom_icc_rpmh_remove);
376
377 MODULE_DESCRIPTION("Qualcomm RPMh interconnect driver");
378 MODULE_LICENSE("GPL v2");
379