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