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
29struct busfreq_table {
30    unsigned long freq;
31    unsigned long volt;
32};
33
34struct 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
59static 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
68static 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
110static 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
144static 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
176static 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
201static 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
232static 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
260static 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
330static 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
369static 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
419static 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
444MODULE_DEVICE_TABLE(of, rockchip_busfreq_of_match);
445
446static 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
478static 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
487module_platform_driver(rockchip_busfreq_driver);
488
489MODULE_LICENSE("GPL v2");
490MODULE_AUTHOR("Tony Xie <tony.xie@rock-chips.com>");
491MODULE_DESCRIPTION("rockchip busfreq driver with devfreq framework");
492