1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (c) 2018, Fuzhou Rockchip Electronics Co., Ltd.
4 * Author: Tony Xie <tony.xie@rock-chips.com>
5 */
6
7 #include <linux/arm-smccc.h>
8 #include <linux/clk.h>
9 #include <linux/cpufreq.h>
10 #include <linux/delay.h>
11 #include <linux/devfreq.h>
12 #include <linux/module.h>
13 #include <linux/of.h>
14 #include <linux/pm_opp.h>
15 #include <linux/platform_device.h>
16 #include <linux/regulator/consumer.h>
17 #include <linux/rockchip/rockchip_sip.h>
18 #include <linux/slab.h>
19 #include <linux/string.h>
20 #include <soc/rockchip/rockchip_opp_select.h>
21
22 #define CLUSTER0 0
23 #define CLUSTER1 1
24 #define MAX_CLUSTERS 2
25
26 #define to_rockchip_bus_clk_nb(nb) container_of(nb, struct rockchip_bus, clk_nb)
27 #define to_rockchip_bus_cpufreq_nb(nb) container_of(nb, struct rockchip_bus, cpufreq_nb)
28
29 struct busfreq_table {
30 unsigned long freq;
31 unsigned long volt;
32 };
33
34 struct rockchip_bus {
35 struct device *dev;
36 struct regulator *regulator;
37 struct clk *clk;
38 struct notifier_block clk_nb;
39 struct notifier_block cpufreq_nb;
40 struct busfreq_table *freq_table;
41
42 unsigned int max_state;
43
44 unsigned long cur_volt;
45 unsigned long cur_rate;
46
47 /*
48 * Busfreq-policy-cpufreq:
49 * If the cpu frequency of two clusters are both less than or equal to
50 * cpu_high_freq, change bus rate to low_rate, otherwise change it to
51 * high_rate.
52 */
53 unsigned long high_rate;
54 unsigned long low_rate;
55 unsigned int cpu_high_freq;
56 unsigned int cpu_freq[MAX_CLUSTERS];
57 };
58
rockchip_sip_bus_smc_config(u32 bus_id,u32 cfg,u32 enable_msk)59 static int rockchip_sip_bus_smc_config(u32 bus_id, u32 cfg, u32 enable_msk)
60 {
61 struct arm_smccc_res res;
62
63 res = sip_smc_bus_config(bus_id, cfg, enable_msk);
64
65 return res.a0;
66 }
67
rockchip_bus_smc_config(struct rockchip_bus * bus)68 static int rockchip_bus_smc_config(struct rockchip_bus *bus)
69 {
70 struct device *dev = bus->dev;
71 struct device_node *np = dev->of_node;
72 struct device_node *child;
73 unsigned int enable_msk, bus_id, cfg;
74 int ret;
75
76 for_each_available_child_of_node(np, child)
77 {
78 ret = of_property_read_u32_index(child, "bus-id", 0, &bus_id);
79 if (ret) {
80 continue;
81 }
82
83 ret = of_property_read_u32_index(child, "cfg-val", 0, &cfg);
84 if (ret) {
85 dev_info(dev, "get cfg-val error\n");
86 continue;
87 }
88
89 if (!cfg) {
90 dev_info(dev, "cfg-val invalid\n");
91 continue;
92 }
93
94 ret = of_property_read_u32_index(child, "enable-msk", 0, &enable_msk);
95 if (ret) {
96 dev_info(dev, "get enable_msk error\n");
97 continue;
98 }
99
100 ret = rockchip_sip_bus_smc_config(bus_id, cfg, enable_msk);
101 if (ret) {
102 dev_info(dev, "bus smc config error: %x!\n", ret);
103 break;
104 }
105 }
106
107 return 0;
108 }
109
rockchip_bus_set_freq_table(struct rockchip_bus * bus)110 static int rockchip_bus_set_freq_table(struct rockchip_bus *bus)
111 {
112 struct device *dev = bus->dev;
113 struct dev_pm_opp *opp;
114 unsigned long freq;
115 int i, count;
116
117 count = dev_pm_opp_get_opp_count(dev);
118 if (count <= 0) {
119 return -EINVAL;
120 }
121
122 bus->max_state = count;
123 bus->freq_table = devm_kcalloc(dev, bus->max_state, sizeof(*bus->freq_table), GFP_KERNEL);
124 if (!bus->freq_table) {
125 bus->max_state = 0;
126 return -ENOMEM;
127 }
128
129 for (i = 0, freq = 0; i < bus->max_state; i++, freq++) {
130 opp = dev_pm_opp_find_freq_ceil(dev, &freq);
131 if (IS_ERR(opp)) {
132 devm_kfree(dev, bus->freq_table);
133 bus->max_state = 0;
134 return PTR_ERR(opp);
135 }
136 bus->freq_table[i].volt = dev_pm_opp_get_voltage(opp);
137 bus->freq_table[i].freq = freq;
138 dev_pm_opp_put(opp);
139 }
140
141 return 0;
142 }
143
rockchip_bus_power_control_init(struct rockchip_bus * bus)144 static int rockchip_bus_power_control_init(struct rockchip_bus *bus)
145 {
146 struct device *dev = bus->dev;
147 int ret = 0;
148
149 bus->clk = devm_clk_get(dev, "bus");
150 if (IS_ERR(bus->clk)) {
151 dev_err(dev, "failed to get bus clock\n");
152 return PTR_ERR(bus->clk);
153 }
154
155 bus->regulator = devm_regulator_get(dev, "bus");
156 if (IS_ERR(bus->regulator)) {
157 dev_err(dev, "failed to get bus regulator\n");
158 return PTR_ERR(bus->regulator);
159 }
160
161 ret = rockchip_init_opp_table(dev, NULL, "leakage", "pvtm");
162 if (ret < 0) {
163 dev_err(dev, "failed to get OPP table\n");
164 return ret;
165 }
166
167 ret = rockchip_bus_set_freq_table(bus);
168 if (ret < 0) {
169 dev_err(dev, "failed to set bus freq table\n");
170 return ret;
171 }
172
173 return 0;
174 }
175
rockchip_bus_clkfreq_target(struct device * dev,unsigned long freq)176 static int rockchip_bus_clkfreq_target(struct device *dev, unsigned long freq)
177 {
178 struct rockchip_bus *bus = dev_get_drvdata(dev);
179 unsigned long target_volt = bus->freq_table[bus->max_state - 1].volt;
180 int i;
181
182 for (i = 0; i < bus->max_state; i++) {
183 if (freq <= bus->freq_table[i].freq) {
184 target_volt = bus->freq_table[i].volt;
185 break;
186 }
187 }
188
189 if (bus->cur_volt != target_volt) {
190 dev_dbg(bus->dev, "target_volt: %lu\n", target_volt);
191 if (regulator_set_voltage(bus->regulator, target_volt, INT_MAX)) {
192 dev_err(dev, "failed to set voltage %lu uV\n", target_volt);
193 return -EINVAL;
194 }
195 bus->cur_volt = target_volt;
196 }
197
198 return 0;
199 }
200
rockchip_bus_clk_notifier(struct notifier_block * nb,unsigned long event,void * data)201 static int rockchip_bus_clk_notifier(struct notifier_block *nb, unsigned long event, void *data)
202 {
203 struct clk_notifier_data *ndata = data;
204 struct rockchip_bus *bus = to_rockchip_bus_clk_nb(nb);
205 int ret = 0;
206
207 dev_dbg(bus->dev, "event %lu, old_rate %lu, new_rate: %lu\n", event, ndata->old_rate, ndata->new_rate);
208
209 switch (event) {
210 case PRE_RATE_CHANGE:
211 if (ndata->new_rate > ndata->old_rate) {
212 ret = rockchip_bus_clkfreq_target(bus->dev, ndata->new_rate);
213 }
214 break;
215 case POST_RATE_CHANGE:
216 if (ndata->new_rate < ndata->old_rate) {
217 ret = rockchip_bus_clkfreq_target(bus->dev, ndata->new_rate);
218 }
219 break;
220 case ABORT_RATE_CHANGE:
221 if (ndata->new_rate > ndata->old_rate) {
222 ret = rockchip_bus_clkfreq_target(bus->dev, ndata->old_rate);
223 }
224 break;
225 default:
226 break;
227 }
228
229 return notifier_from_errno(ret);
230 }
231
rockchip_bus_clkfreq(struct rockchip_bus * bus)232 static int rockchip_bus_clkfreq(struct rockchip_bus *bus)
233 {
234 struct device *dev = bus->dev;
235 unsigned long init_rate;
236 int ret = 0;
237
238 ret = rockchip_bus_power_control_init(bus);
239 if (ret) {
240 dev_err(dev, "failed to init power control\n");
241 return ret;
242 }
243
244 init_rate = clk_get_rate(bus->clk);
245 ret = rockchip_bus_clkfreq_target(dev, init_rate);
246 if (ret) {
247 return ret;
248 }
249
250 bus->clk_nb.notifier_call = rockchip_bus_clk_notifier;
251 ret = clk_notifier_register(bus->clk, &bus->clk_nb);
252 if (ret) {
253 dev_err(dev, "failed to register clock notifier\n");
254 return ret;
255 }
256
257 return 0;
258 }
259
rockchip_bus_cpufreq_target(struct device * dev,unsigned long freq,u32 flags)260 static int rockchip_bus_cpufreq_target(struct device *dev, unsigned long freq, u32 flags)
261 {
262 struct rockchip_bus *bus = dev_get_drvdata(dev);
263 struct dev_pm_opp *opp;
264 unsigned long target_volt, target_rate = freq;
265 int ret = 0;
266
267 if (!bus->regulator) {
268 dev_dbg(dev, "%luHz -> %luHz\n", bus->cur_rate, target_rate);
269 ret = clk_set_rate(bus->clk, target_rate);
270 if (ret) {
271 dev_err(bus->dev, "failed to set bus rate %lu\n", target_rate);
272 } else {
273 bus->cur_rate = target_rate;
274 }
275 return ret;
276 }
277
278 opp = devfreq_recommended_opp(dev, &target_rate, flags);
279 if (IS_ERR(opp)) {
280 dev_err(dev, "failed to recommended opp %lu\n", target_rate);
281 return PTR_ERR(opp);
282 }
283 target_volt = dev_pm_opp_get_voltage(opp);
284 dev_pm_opp_put(opp);
285
286 if (bus->cur_rate == target_rate) {
287 if (bus->cur_volt == target_volt) {
288 return 0;
289 }
290 ret = regulator_set_voltage(bus->regulator, target_volt, INT_MAX);
291 if (ret) {
292 dev_err(dev, "failed to set voltage %lu\n", target_volt);
293 return ret;
294 }
295 bus->cur_volt = target_volt;
296 return 0;
297 } else if (!bus->cur_volt) {
298 bus->cur_volt = regulator_get_voltage(bus->regulator);
299 }
300
301 if (bus->cur_rate < target_rate) {
302 ret = regulator_set_voltage(bus->regulator, target_volt, INT_MAX);
303 if (ret) {
304 dev_err(dev, "failed to set voltage %lu\n", target_volt);
305 return ret;
306 }
307 }
308
309 ret = clk_set_rate(bus->clk, target_rate);
310 if (ret) {
311 dev_err(dev, "failed to set bus rate %lu\n", target_rate);
312 return ret;
313 }
314
315 if (bus->cur_rate > target_rate) {
316 ret = regulator_set_voltage(bus->regulator, target_volt, INT_MAX);
317 if (ret) {
318 dev_err(dev, "failed to set voltage %lu\n", target_volt);
319 return ret;
320 }
321 }
322
323 dev_dbg(dev, "%luHz %luuV -> %luHz %luuV\n", bus->cur_rate, bus->cur_volt, target_rate, target_volt);
324 bus->cur_rate = target_rate;
325 bus->cur_volt = target_volt;
326
327 return ret;
328 }
329
rockchip_bus_cpufreq_notifier(struct notifier_block * nb,unsigned long event,void * data)330 static int rockchip_bus_cpufreq_notifier(struct notifier_block *nb, unsigned long event, void *data)
331 {
332 struct rockchip_bus *bus = to_rockchip_bus_cpufreq_nb(nb);
333 struct cpufreq_freqs *freqs = data;
334 int id = topology_physical_package_id(freqs->policy->cpu);
335 if (id < 0 || id >= MAX_CLUSTERS) {
336 return NOTIFY_DONE;
337 }
338
339 bus->cpu_freq[id] = freqs->new;
340
341 if (!bus->cpu_freq[CLUSTER0] || !bus->cpu_freq[CLUSTER1]) {
342 return NOTIFY_DONE;
343 }
344
345 switch (event) {
346 case CPUFREQ_PRECHANGE:
347 if ((bus->cpu_freq[CLUSTER0] > bus->cpu_high_freq || bus->cpu_freq[CLUSTER1] > bus->cpu_high_freq) &&
348 bus->cur_rate != bus->high_rate) {
349 dev_dbg(bus->dev, "cpu%d freq=%d %d, up cci rate to %lu\n", freqs->policy->cpu, bus->cpu_freq[CLUSTER0],
350 bus->cpu_freq[CLUSTER1], bus->high_rate);
351 rockchip_bus_cpufreq_target(bus->dev, bus->high_rate, 0);
352 }
353 break;
354 case CPUFREQ_POSTCHANGE:
355 if (bus->cpu_freq[CLUSTER0] <= bus->cpu_high_freq && bus->cpu_freq[CLUSTER1] <= bus->cpu_high_freq &&
356 bus->cur_rate != bus->low_rate) {
357 dev_dbg(bus->dev, "cpu%d freq=%d %d, down cci rate to %lu\n", freqs->policy->cpu,
358 bus->cpu_freq[CLUSTER0], bus->cpu_freq[CLUSTER1], bus->low_rate);
359 rockchip_bus_cpufreq_target(bus->dev, bus->low_rate, 0);
360 }
361 break;
362 default:
363 break;
364 }
365
366 return NOTIFY_OK;
367 }
368
rockchip_bus_cpufreq(struct rockchip_bus * bus)369 static int rockchip_bus_cpufreq(struct rockchip_bus *bus)
370 {
371 struct device *dev = bus->dev;
372 struct device_node *np = dev->of_node;
373 unsigned int freq;
374 int ret = 0;
375
376 if (of_parse_phandle(dev->of_node, "operating-points-v2", 0)) {
377 ret = rockchip_bus_power_control_init(bus);
378 if (ret) {
379 dev_err(dev, "failed to init power control\n");
380 return ret;
381 }
382 } else {
383 bus->clk = devm_clk_get(dev, "bus");
384 if (IS_ERR(bus->clk)) {
385 dev_err(dev, "failed to get bus clock\n");
386 return PTR_ERR(bus->clk);
387 }
388 bus->regulator = NULL;
389 }
390
391 ret = of_property_read_u32(np, "cpu-high-freq", &bus->cpu_high_freq);
392 if (ret) {
393 dev_err(dev, "failed to get cpu-high-freq\n");
394 return ret;
395 }
396 ret = of_property_read_u32(np, "cci-high-freq", &freq);
397 if (ret) {
398 dev_err(dev, "failed to get cci-high-freq\n");
399 return ret;
400 }
401 bus->high_rate = (unsigned long)freq * 0x3e8;
402 ret = of_property_read_u32(np, "cci-low-freq", &freq);
403 if (ret) {
404 dev_err(dev, "failed to get cci-low-freq\n");
405 return ret;
406 }
407 bus->low_rate = (unsigned long)freq * 0x3e8;
408
409 bus->cpufreq_nb.notifier_call = rockchip_bus_cpufreq_notifier;
410 ret = cpufreq_register_notifier(&bus->cpufreq_nb, CPUFREQ_TRANSITION_NOTIFIER);
411 if (ret) {
412 dev_err(dev, "failed to register cpufreq notifier\n");
413 return ret;
414 }
415
416 return 0;
417 }
418
419 static const struct of_device_id rockchip_busfreq_of_match[] = {
420 {
421 .compatible = "rockchip,px30-bus",
422 },
423 {
424 .compatible = "rockchip,rk1808-bus",
425 },
426 {
427 .compatible = "rockchip,rk3288-bus",
428 },
429 {
430 .compatible = "rockchip,rk3368-bus",
431 },
432 {
433 .compatible = "rockchip,rk3399-bus",
434 },
435 {
436 .compatible = "rockchip,rk3568-bus",
437 },
438 {
439 .compatible = "rockchip,rv1126-bus",
440 },
441 {},
442 };
443
444 MODULE_DEVICE_TABLE(of, rockchip_busfreq_of_match);
445
rockchip_busfreq_probe(struct platform_device * pdev)446 static int rockchip_busfreq_probe(struct platform_device *pdev)
447 {
448 struct device *dev = &pdev->dev;
449 struct device_node *np = dev->of_node;
450 struct rockchip_bus *bus;
451 const char *policy_name;
452 int ret = 0;
453
454 bus = devm_kzalloc(dev, sizeof(*bus), GFP_KERNEL);
455 if (!bus) {
456 return -ENOMEM;
457 }
458 bus->dev = dev;
459 platform_set_drvdata(pdev, bus);
460
461 ret = of_property_read_string(np, "rockchip,busfreq-policy", &policy_name);
462 if (ret) {
463 dev_info(dev, "failed to get busfreq policy\n");
464 return ret;
465 }
466
467 if (!strcmp(policy_name, "smc")) {
468 ret = rockchip_bus_smc_config(bus);
469 } else if (!strcmp(policy_name, "clkfreq")) {
470 ret = rockchip_bus_clkfreq(bus);
471 } else if (!strcmp(policy_name, "cpufreq")) {
472 ret = rockchip_bus_cpufreq(bus);
473 }
474
475 return ret;
476 }
477
478 static struct platform_driver rockchip_busfreq_driver = {
479 .probe = rockchip_busfreq_probe,
480 .driver =
481 {
482 .name = "rockchip,bus",
483 .of_match_table = rockchip_busfreq_of_match,
484 },
485 };
486
487 module_platform_driver(rockchip_busfreq_driver);
488
489 MODULE_LICENSE("GPL v2");
490 MODULE_AUTHOR("Tony Xie <tony.xie@rock-chips.com>");
491 MODULE_DESCRIPTION("rockchip busfreq driver with devfreq framework");
492