1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (c) 2020, The Linux Foundation. All rights reserved.
4  */
5 
6 #include <linux/interconnect.h>
7 #include <linux/interconnect-provider.h>
8 #include <linux/module.h>
9 #include <linux/of.h>
10 #include <linux/of_device.h>
11 #include <linux/slab.h>
12 
13 #include "bcm-voter.h"
14 #include "icc-rpmh.h"
15 
16 /**
17  * qcom_icc_pre_aggregate - cleans up stale values from prior icc_set
18  * @node: icc node to operate on
19  */
20 void qcom_icc_pre_aggregate(struct icc_node *node)
21 {
22 	size_t i;
23 	struct qcom_icc_node *qn;
24 	struct qcom_icc_provider *qp;
25 
26 	qn = node->data;
27 	qp = to_qcom_provider(node->provider);
28 
29 	for (i = 0; i < QCOM_ICC_NUM_BUCKETS; i++) {
30 		qn->sum_avg[i] = 0;
31 		qn->max_peak[i] = 0;
32 	}
33 
34 	for (i = 0; i < qn->num_bcms; i++)
35 		qcom_icc_bcm_voter_add(qp->voter, qn->bcms[i]);
36 }
37 EXPORT_SYMBOL_GPL(qcom_icc_pre_aggregate);
38 
39 /**
40  * qcom_icc_aggregate - aggregate bw for buckets indicated by tag
41  * @node: node to aggregate
42  * @tag: tag to indicate which buckets to aggregate
43  * @avg_bw: new bw to sum aggregate
44  * @peak_bw: new bw to max aggregate
45  * @agg_avg: existing aggregate avg bw val
46  * @agg_peak: existing aggregate peak bw val
47  */
48 int qcom_icc_aggregate(struct icc_node *node, u32 tag, u32 avg_bw,
49 		       u32 peak_bw, u32 *agg_avg, u32 *agg_peak)
50 {
51 	size_t i;
52 	struct qcom_icc_node *qn;
53 
54 	qn = node->data;
55 
56 	if (!tag)
57 		tag = QCOM_ICC_TAG_ALWAYS;
58 
59 	for (i = 0; i < QCOM_ICC_NUM_BUCKETS; i++) {
60 		if (tag & BIT(i)) {
61 			qn->sum_avg[i] += avg_bw;
62 			qn->max_peak[i] = max_t(u32, qn->max_peak[i], peak_bw);
63 		}
64 
65 		if (node->init_avg || node->init_peak) {
66 			qn->sum_avg[i] = max_t(u64, qn->sum_avg[i], node->init_avg);
67 			qn->max_peak[i] = max_t(u64, qn->max_peak[i], node->init_peak);
68 		}
69 	}
70 
71 	*agg_avg += avg_bw;
72 	*agg_peak = max_t(u32, *agg_peak, peak_bw);
73 
74 	return 0;
75 }
76 EXPORT_SYMBOL_GPL(qcom_icc_aggregate);
77 
78 /**
79  * qcom_icc_set - set the constraints based on path
80  * @src: source node for the path to set constraints on
81  * @dst: destination node for the path to set constraints on
82  *
83  * Return: 0 on success, or an error code otherwise
84  */
85 int qcom_icc_set(struct icc_node *src, struct icc_node *dst)
86 {
87 	struct qcom_icc_provider *qp;
88 	struct icc_node *node;
89 
90 	if (!src)
91 		node = dst;
92 	else
93 		node = src;
94 
95 	qp = to_qcom_provider(node->provider);
96 
97 	qcom_icc_bcm_voter_commit(qp->voter);
98 
99 	return 0;
100 }
101 EXPORT_SYMBOL_GPL(qcom_icc_set);
102 
103 struct icc_node_data *qcom_icc_xlate_extended(struct of_phandle_args *spec, void *data)
104 {
105 	struct icc_node_data *ndata;
106 	struct icc_node *node;
107 
108 	node = of_icc_xlate_onecell(spec, data);
109 	if (IS_ERR(node))
110 		return ERR_CAST(node);
111 
112 	ndata = kzalloc(sizeof(*ndata), GFP_KERNEL);
113 	if (!ndata)
114 		return ERR_PTR(-ENOMEM);
115 
116 	ndata->node = node;
117 
118 	if (spec->args_count == 2)
119 		ndata->tag = spec->args[1];
120 
121 	if (spec->args_count > 2)
122 		pr_warn("%pOF: Too many arguments, path tag is not parsed\n", spec->np);
123 
124 	return ndata;
125 }
126 EXPORT_SYMBOL_GPL(qcom_icc_xlate_extended);
127 
128 /**
129  * qcom_icc_bcm_init - populates bcm aux data and connect qnodes
130  * @bcm: bcm to be initialized
131  * @dev: associated provider device
132  *
133  * Return: 0 on success, or an error code otherwise
134  */
135 int qcom_icc_bcm_init(struct qcom_icc_bcm *bcm, struct device *dev)
136 {
137 	struct qcom_icc_node *qn;
138 	const struct bcm_db *data;
139 	size_t data_count;
140 	int i;
141 
142 	/* BCM is already initialised*/
143 	if (bcm->addr)
144 		return 0;
145 
146 	bcm->addr = cmd_db_read_addr(bcm->name);
147 	if (!bcm->addr) {
148 		dev_err(dev, "%s could not find RPMh address\n",
149 			bcm->name);
150 		return -EINVAL;
151 	}
152 
153 	data = cmd_db_read_aux_data(bcm->name, &data_count);
154 	if (IS_ERR(data)) {
155 		dev_err(dev, "%s command db read error (%ld)\n",
156 			bcm->name, PTR_ERR(data));
157 		return PTR_ERR(data);
158 	}
159 	if (!data_count) {
160 		dev_err(dev, "%s command db missing or partial aux data\n",
161 			bcm->name);
162 		return -EINVAL;
163 	}
164 
165 	bcm->aux_data.unit = le32_to_cpu(data->unit);
166 	bcm->aux_data.width = le16_to_cpu(data->width);
167 	bcm->aux_data.vcd = data->vcd;
168 	bcm->aux_data.reserved = data->reserved;
169 	INIT_LIST_HEAD(&bcm->list);
170 	INIT_LIST_HEAD(&bcm->ws_list);
171 
172 	if (!bcm->vote_scale)
173 		bcm->vote_scale = 1000;
174 
175 	/* Link Qnodes to their respective BCMs */
176 	for (i = 0; i < bcm->num_nodes; i++) {
177 		qn = bcm->nodes[i];
178 		qn->bcms[qn->num_bcms] = bcm;
179 		qn->num_bcms++;
180 	}
181 
182 	return 0;
183 }
184 EXPORT_SYMBOL_GPL(qcom_icc_bcm_init);
185 
186 int qcom_icc_rpmh_probe(struct platform_device *pdev)
187 {
188 	const struct qcom_icc_desc *desc;
189 	struct device *dev = &pdev->dev;
190 	struct icc_onecell_data *data;
191 	struct icc_provider *provider;
192 	struct qcom_icc_node **qnodes, *qn;
193 	struct qcom_icc_provider *qp;
194 	struct icc_node *node;
195 	size_t num_nodes, i, j;
196 	int ret;
197 
198 	desc = of_device_get_match_data(dev);
199 	if (!desc)
200 		return -EINVAL;
201 
202 	qnodes = desc->nodes;
203 	num_nodes = desc->num_nodes;
204 
205 	qp = devm_kzalloc(dev, sizeof(*qp), GFP_KERNEL);
206 	if (!qp)
207 		return -ENOMEM;
208 
209 	data = devm_kzalloc(dev, struct_size(data, nodes, num_nodes), GFP_KERNEL);
210 	if (!data)
211 		return -ENOMEM;
212 
213 	provider = &qp->provider;
214 	provider->dev = dev;
215 	provider->set = qcom_icc_set;
216 	provider->pre_aggregate = qcom_icc_pre_aggregate;
217 	provider->aggregate = qcom_icc_aggregate;
218 	provider->xlate_extended = qcom_icc_xlate_extended;
219 	INIT_LIST_HEAD(&provider->nodes);
220 	provider->data = data;
221 
222 	qp->dev = dev;
223 	qp->bcms = desc->bcms;
224 	qp->num_bcms = desc->num_bcms;
225 
226 	qp->voter = of_bcm_voter_get(qp->dev, NULL);
227 	if (IS_ERR(qp->voter))
228 		return PTR_ERR(qp->voter);
229 
230 	ret = icc_provider_add(provider);
231 	if (ret)
232 		return ret;
233 
234 	for (i = 0; i < qp->num_bcms; i++)
235 		qcom_icc_bcm_init(qp->bcms[i], dev);
236 
237 	for (i = 0; i < num_nodes; i++) {
238 		qn = qnodes[i];
239 		if (!qn)
240 			continue;
241 
242 		node = icc_node_create(qn->id);
243 		if (IS_ERR(node)) {
244 			ret = PTR_ERR(node);
245 			goto err;
246 		}
247 
248 		node->name = qn->name;
249 		node->data = qn;
250 		icc_node_add(node, provider);
251 
252 		for (j = 0; j < qn->num_links; j++)
253 			icc_link_create(node, qn->links[j]);
254 
255 		data->nodes[i] = node;
256 	}
257 
258 	data->num_nodes = num_nodes;
259 	platform_set_drvdata(pdev, qp);
260 
261 	return 0;
262 err:
263 	icc_nodes_remove(provider);
264 	icc_provider_del(provider);
265 	return ret;
266 }
267 EXPORT_SYMBOL_GPL(qcom_icc_rpmh_probe);
268 
269 int qcom_icc_rpmh_remove(struct platform_device *pdev)
270 {
271 	struct qcom_icc_provider *qp = platform_get_drvdata(pdev);
272 
273 	icc_nodes_remove(&qp->provider);
274 	return icc_provider_del(&qp->provider);
275 }
276 EXPORT_SYMBOL_GPL(qcom_icc_rpmh_remove);
277 
278 MODULE_LICENSE("GPL v2");
279