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