diff --git a/drivers/devfreq/rockchip_bus.c b/drivers/devfreq/rockchip_bus.c index ba3dc426e08b..437ca9f77c79 100644 --- a/drivers/devfreq/rockchip_bus.c +++ b/drivers/devfreq/rockchip_bus.c @@ -5,14 +5,50 @@ */ #include +#include +#include #include +#include #include #include +#include #include +#include #include #include #include +#define CLUSTER0 0 +#define CLUSTER1 1 +#define MAX_CLUSTERS 2 + +#define to_rockchip_bus_clk_nb(nb) \ + container_of(nb, struct rockchip_bus, clk_nb) +#define to_rockchip_bus_cpufreq_nb(nb) \ + container_of(nb, struct rockchip_bus, cpufreq_nb) + +struct rockchip_bus { + struct device *dev; + struct regulator *regulator; + struct clk *clk; + struct notifier_block clk_nb; + struct notifier_block cpufreq_nb; + + unsigned long cur_volt; + unsigned long cur_rate; + + /* + * Busfreq-policy-cpufreq: + * If the cpu frequency of two clusters are both less than or equal to + * cpu_high_freq, change bus rate to low_rate, otherwise change it to + * high_rate. + */ + unsigned long high_rate; + unsigned long low_rate; + unsigned int cpu_high_freq; + unsigned int cpu_freq[MAX_CLUSTERS]; +}; + static int rockchip_sip_soc_bus_div(u32 bus_id, u32 timer, u32 enable_msk) { struct arm_smccc_res res; @@ -22,16 +58,9 @@ static int rockchip_sip_soc_bus_div(u32 bus_id, u32 timer, u32 enable_msk) return res.a0; } -static const struct of_device_id rockchip_busfreq_of_match[] = { - { .compatible = "rockchip,px30-bus", }, - { }, -}; - -MODULE_DEVICE_TABLE(of, rockchip_busfreq_of_match); - -static int rockchip_busfreq_probe(struct platform_device *pdev) +static int rockchip_bus_autocs(struct rockchip_bus *bus) { - struct device *dev = &pdev->dev; + struct device *dev = bus->dev; struct device_node *np = dev->of_node; struct device_node *child; int ret, enable_msk, bus_id, sip_timer; @@ -72,10 +101,346 @@ static int rockchip_busfreq_probe(struct platform_device *pdev) return 0; } +static int rockchip_bus_power_control_init(struct rockchip_bus *bus) +{ + struct device *dev = bus->dev; + int ret = 0; + + bus->clk = devm_clk_get(dev, "bus"); + if (IS_ERR(bus->clk)) { + dev_err(dev, "failed to get bus clock\n"); + return PTR_ERR(bus->clk); + } + + bus->regulator = devm_regulator_get(dev, "bus"); + if (IS_ERR(bus->regulator)) { + dev_err(dev, "failed to get bus regulator\n"); + return PTR_ERR(bus->regulator); + } + + ret = dev_pm_opp_of_add_table(dev); + if (ret < 0) { + dev_err(dev, "failed to get OPP table\n"); + return ret; + } + + return 0; +} + +static int rockchip_bus_clkfreq_target(struct device *dev, unsigned long freq, + u32 flags) +{ + struct rockchip_bus *bus = dev_get_drvdata(dev); + struct dev_pm_opp *opp; + unsigned long target_volt, target_rate = freq; + + rcu_read_lock(); + + opp = devfreq_recommended_opp(dev, &target_rate, flags); + if (IS_ERR(opp)) { + dev_err(dev, "failed to recommended opp %lu\n", target_rate); + rcu_read_unlock(); + return PTR_ERR(opp); + } + target_volt = dev_pm_opp_get_voltage(opp); + + rcu_read_unlock(); + + if (bus->cur_volt != target_volt) { + dev_dbg(bus->dev, "target_volt: %lu\n", target_volt); + if (regulator_set_voltage(bus->regulator, target_volt, + INT_MAX)) { + dev_err(dev, "failed to set voltage %lu uV\n", + target_volt); + return -EINVAL; + } + bus->cur_volt = target_volt; + } + + return 0; +} + +static int rockchip_bus_clk_notifier(struct notifier_block *nb, + unsigned long event, void *data) +{ + struct clk_notifier_data *ndata = data; + struct rockchip_bus *bus = to_rockchip_bus_clk_nb(nb); + int ret = 0; + + dev_dbg(bus->dev, "event %lu, old_rate %lu, new_rate: %lu\n", + event, ndata->old_rate, ndata->new_rate); + + switch (event) { + case PRE_RATE_CHANGE: + if (ndata->new_rate > ndata->old_rate) + ret = rockchip_bus_clkfreq_target(bus->dev, + ndata->new_rate, 0); + break; + case POST_RATE_CHANGE: + if (ndata->new_rate < ndata->old_rate) + ret = rockchip_bus_clkfreq_target(bus->dev, + ndata->new_rate, 0); + break; + case ABORT_RATE_CHANGE: + if (ndata->new_rate > ndata->old_rate) + ret = rockchip_bus_clkfreq_target(bus->dev, + ndata->old_rate, 0); + break; + default: + break; + } + + return notifier_from_errno(ret); +} + +static int rockchip_bus_clkfreq(struct rockchip_bus *bus) +{ + struct device *dev = bus->dev; + unsigned long init_rate; + int ret = 0; + + ret = rockchip_bus_power_control_init(bus); + if (ret) { + dev_err(dev, "failed to init power control\n"); + return ret; + } + + init_rate = clk_get_rate(bus->clk); + ret = rockchip_bus_clkfreq_target(dev, init_rate, 0); + if (ret) + return ret; + + bus->clk_nb.notifier_call = rockchip_bus_clk_notifier; + ret = clk_notifier_register(bus->clk, &bus->clk_nb); + if (ret) { + dev_err(dev, "failed to register clock notifier\n"); + return ret; + } + + return 0; +} + +static int rockchip_bus_cpufreq_target(struct device *dev, unsigned long freq, + u32 flags) +{ + struct rockchip_bus *bus = dev_get_drvdata(dev); + struct dev_pm_opp *opp; + unsigned long target_volt, target_rate = freq; + int ret = 0; + + if (!bus->regulator) { + dev_dbg(dev, "%luHz -> %luHz\n", bus->cur_rate, target_rate); + ret = clk_set_rate(bus->clk, target_rate); + if (ret) + dev_err(bus->dev, "failed to set bus rate %lu\n", + target_rate); + else + bus->cur_rate = target_rate; + return ret; + } + + rcu_read_lock(); + + opp = devfreq_recommended_opp(dev, &target_rate, flags); + if (IS_ERR(opp)) { + dev_err(dev, "failed to recommended opp %lu\n", target_rate); + rcu_read_unlock(); + return PTR_ERR(opp); + } + target_volt = dev_pm_opp_get_voltage(opp); + + rcu_read_unlock(); + + if (bus->cur_rate == target_rate) { + if (bus->cur_volt == target_volt) + return 0; + ret = regulator_set_voltage(bus->regulator, target_volt, + INT_MAX); + if (ret) { + dev_err(dev, "failed to set voltage %lu\n", + target_volt); + return ret; + } + bus->cur_volt = target_volt; + return 0; + } else if (!bus->cur_volt) { + bus->cur_volt = regulator_get_voltage(bus->regulator); + } + + if (bus->cur_rate < target_rate) { + ret = regulator_set_voltage(bus->regulator, target_volt, + INT_MAX); + if (ret) { + dev_err(dev, "failed to set voltage %lu\n", + target_volt); + return ret; + } + } + + ret = clk_set_rate(bus->clk, target_rate); + if (ret) { + dev_err(dev, "failed to set bus rate %lu\n", target_rate); + return ret; + } + + if (bus->cur_rate > target_rate) { + ret = regulator_set_voltage(bus->regulator, target_volt, + INT_MAX); + if (ret) { + dev_err(dev, "failed to set voltage %lu\n", + target_volt); + return ret; + } + } + + dev_dbg(dev, "%luHz %luuV -> %luHz %luuV\n", bus->cur_rate, + bus->cur_volt, target_rate, target_volt); + bus->cur_rate = target_rate; + bus->cur_volt = target_volt; + + return ret; +} + +static int rockchip_bus_cpufreq_notifier(struct notifier_block *nb, + unsigned long event, void *data) +{ + struct rockchip_bus *bus = to_rockchip_bus_cpufreq_nb(nb); + struct cpufreq_freqs *freqs = data; + int id = topology_physical_package_id(freqs->cpu); + + if (id < 0 || id >= MAX_CLUSTERS) + return NOTIFY_DONE; + + bus->cpu_freq[id] = freqs->new; + + if (!bus->cpu_freq[CLUSTER0] || !bus->cpu_freq[CLUSTER1]) + return NOTIFY_DONE; + + switch (event) { + case CPUFREQ_PRECHANGE: + if ((bus->cpu_freq[CLUSTER0] > bus->cpu_high_freq || + bus->cpu_freq[CLUSTER1] > bus->cpu_high_freq) && + bus->cur_rate != bus->high_rate) { + dev_dbg(bus->dev, "cpu%d freq=%d %d, up cci rate to %lu\n", + freqs->cpu, + bus->cpu_freq[CLUSTER0], + bus->cpu_freq[CLUSTER1], + bus->high_rate); + rockchip_bus_cpufreq_target(bus->dev, bus->high_rate, + 0); + } + break; + case CPUFREQ_POSTCHANGE: + if (bus->cpu_freq[CLUSTER0] <= bus->cpu_high_freq && + bus->cpu_freq[CLUSTER1] <= bus->cpu_high_freq && + bus->cur_rate != bus->low_rate) { + dev_dbg(bus->dev, "cpu%d freq=%d %d, down cci rate to %lu\n", + freqs->cpu, + bus->cpu_freq[CLUSTER0], + bus->cpu_freq[CLUSTER1], + bus->low_rate); + rockchip_bus_cpufreq_target(bus->dev, bus->low_rate, + 0); + } + break; + } + + return NOTIFY_OK; +} + +static int rockchip_bus_cpufreq(struct rockchip_bus *bus) +{ + struct device *dev = bus->dev; + struct device_node *np = dev->of_node; + unsigned int freq; + int ret = 0; + + if (of_parse_phandle(dev->of_node, "operating-points-v2", 0)) { + ret = rockchip_bus_power_control_init(bus); + if (ret) { + dev_err(dev, "failed to init power control\n"); + return ret; + } + } else { + bus->clk = devm_clk_get(dev, "bus"); + if (IS_ERR(bus->clk)) { + dev_err(dev, "failed to get bus clock\n"); + return PTR_ERR(bus->clk); + } + bus->regulator = NULL; + } + + ret = of_property_read_u32(np, "cpu-high-freq", &bus->cpu_high_freq); + if (ret) { + dev_err(dev, "failed to get cpu-high-freq\n"); + return ret; + } + ret = of_property_read_u32(np, "cci-high-freq", &freq); + if (ret) { + dev_err(dev, "failed to get cci-high-freq\n"); + return ret; + } + bus->high_rate = freq * 1000; + ret = of_property_read_u32(np, "cci-low-freq", &freq); + if (ret) { + dev_err(dev, "failed to get cci-low-freq\n"); + return ret; + } + bus->low_rate = freq * 1000; + + bus->cpufreq_nb.notifier_call = rockchip_bus_cpufreq_notifier; + ret = cpufreq_register_notifier(&bus->cpufreq_nb, + CPUFREQ_TRANSITION_NOTIFIER); + if (ret) { + dev_err(dev, "failed to register cpufreq notifier\n"); + return ret; + } + + return 0; +} + +static const struct of_device_id rockchip_busfreq_of_match[] = { + { .compatible = "rockchip,px30-bus", }, + { }, +}; + +MODULE_DEVICE_TABLE(of, rockchip_busfreq_of_match); + +static int rockchip_busfreq_probe(struct platform_device *pdev) +{ + struct device *dev = &pdev->dev; + struct device_node *np = dev->of_node; + struct rockchip_bus *bus; + const char *policy_name; + int ret = 0; + + bus = devm_kzalloc(dev, sizeof(*bus), GFP_KERNEL); + if (!bus) + return -ENOMEM; + bus->dev = dev; + platform_set_drvdata(pdev, bus); + + ret = of_property_read_string(np, "rockchip,busfreq-policy", + &policy_name); + if (ret) { + dev_info(dev, "failed to get busfreq policy\n"); + return ret; + } + + if (!strcmp(policy_name, "autocs")) + ret = rockchip_bus_autocs(bus); + else if (!strcmp(policy_name, "clkfreq")) + ret = rockchip_bus_clkfreq(bus); + else if (!strcmp(policy_name, "cpufreq")) + ret = rockchip_bus_cpufreq(bus); + + return ret; +} + static struct platform_driver rockchip_busfreq_driver = { .probe = rockchip_busfreq_probe, .driver = { - .name = "rockchip,soc_bus", + .name = "rockchip,bus", .of_match_table = rockchip_busfreq_of_match, }, }; @@ -84,4 +449,4 @@ module_platform_driver(rockchip_busfreq_driver); MODULE_LICENSE("GPL v2"); MODULE_AUTHOR("Tony Xie "); -MODULE_DESCRIPTION("rockchip soc bus driver with devfreq framework"); +MODULE_DESCRIPTION("rockchip busfreq driver with devfreq framework");