diff -drupN a/drivers/cpufreq/sunxi-cpufreq-pwm.c b/drivers/cpufreq/sunxi-cpufreq-pwm.c --- a/drivers/cpufreq/sunxi-cpufreq-pwm.c 1970-01-01 03:00:00.000000000 +0300 +++ b/drivers/cpufreq/sunxi-cpufreq-pwm.c 2022-06-12 05:28:14.000000000 +0300 @@ -0,0 +1,429 @@ +/* + * Copyright (c) 2018 softwinner. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct pwm_info_table { + int period_ns; + int vol_base; + int vol_max; + int polarity; + int pwm_id; +}; + +struct cpufreq_soc_data { + /* If it exists, switch to this clock + * before changing the frequency each time. */ + u32 periph_clk; +}; + +struct cpufreq_pwm_data { + struct pwm_info_table pwm_info; + struct pwm_device *pwm_dev; + struct device *cpu; + struct clk *pll_clk; + struct clk *cpu_clk; + struct clk *periph_clk; +}; + +static struct thermal_cooling_device *cdev; +static int dvfs_pwm_enale; +#define ENABLE 1 + +static int pwm_interface_init(struct cpufreq_pwm_data *data) +{ + struct device_node *pwm_info_np; + struct pwm_info_table *info = &data->pwm_info; + int rc = 0; + + pwm_info_np = of_find_node_by_path("/pwm_dvfs_info"); + if (unlikely(!pwm_info_np)) + return -ENODEV; + + rc = of_property_read_u32(pwm_info_np, "period_ns", + &info->period_ns); + rc |= of_property_read_u32(pwm_info_np, "vol_base", + &info->vol_base); + rc |= of_property_read_u32(pwm_info_np, "vol_max", + &info->vol_max); + rc |= of_property_read_u32(pwm_info_np, "polarity", + &info->polarity); + rc |= of_property_read_u32(pwm_info_np, "pwm_id", + &info->pwm_id); + if (rc) + goto out; + + data->pwm_dev = pwm_request(info->pwm_id, "pwm-cpu"); + if (IS_ERR(data->pwm_dev)) { + rc = PTR_ERR(data->pwm_dev); + goto out; + } + + rc = pwm_config(data->pwm_dev, info->period_ns, info->period_ns); + if (rc) + goto out; + + rc = pwm_set_polarity(data->pwm_dev, info->polarity); + if (rc) + goto out; + + rc = pwm_enable(data->pwm_dev); + if (rc) + goto out; + +out: + of_node_put(pwm_info_np); + return rc; +} + +static int sunxi_set_cpufreq(struct cpufreq_policy *policy, + unsigned int freq) +{ + struct device *cpu = get_cpu_device(policy->cpu); + struct cpufreq_pwm_data *data = + (struct cpufreq_pwm_data *)(policy->driver_data); + int rc; + + if (!data->periph_clk) { + rc = dev_pm_opp_set_rate(cpu, freq * 1000); + if (rc) + pr_err("Set cpu frequency to %dKHz failed!\n", freq); + goto out; + } + + /* Reparent the CPU clock */ + rc = clk_set_parent(data->cpu_clk, data->periph_clk); + if (WARN_ON(rc)) { + pr_err("cpu%d: failed to re-parent cpu clock!\n", + policy->cpu); + goto out; + } + + /* Set the original PLL to target rate */ + rc = clk_set_rate(data->pll_clk, freq * 1000); + if (rc) { + pr_err("cpu%d: failed to scale cpu clock rate!\n", + policy->cpu); + clk_set_parent(data->cpu_clk, data->pll_clk); + goto out; + } + + /* Set parent of CPU clock back to the original PLL */ + rc = clk_set_parent(data->cpu_clk, data->pll_clk); + if (WARN_ON(rc)) { + pr_err("cpu%d: failed to re-parent cpu clock!\n", + policy->cpu); + goto out; + } +out: + return rc; +} + +static int sunxi_set_cpuvol(struct cpufreq_policy *policy, + unsigned int freq) +{ + struct cpufreq_pwm_data *data = + (struct cpufreq_pwm_data *)(policy->driver_data); + struct pwm_info_table *pwm_info = &data->pwm_info; + struct device *cpu = get_cpu_device(policy->cpu); + unsigned long volt, freq_hz = freq * 1000; + struct dev_pm_opp *opp; + long duty_ns; + int rc = 0; + + rcu_read_lock(); + opp = dev_pm_opp_find_freq_ceil(cpu, &freq_hz); + if (IS_ERR(opp)) { + rcu_read_unlock(); + pr_err("failed to find OPP for %ld\n", freq_hz); + return PTR_ERR(opp); + } + volt = dev_pm_opp_get_voltage(opp); + rcu_read_unlock(); + + if (volt <= pwm_info->vol_base) + duty_ns = 0; + else if (volt >= pwm_info->vol_max) + duty_ns = pwm_info->period_ns; + else if (pwm_info->vol_base < volt && volt < pwm_info->vol_max) { + /* Div 1000 for convert to mV */ + duty_ns = ((volt - pwm_info->vol_base) / 1000) + * pwm_info->period_ns + / ((pwm_info->vol_max - pwm_info->vol_base) / 1000); + } + + pr_info("duty_ns:%ld\tperiod_ns:%d\n", duty_ns, pwm_info->period_ns); + rc = pwm_config(data->pwm_dev, duty_ns, pwm_info->period_ns); + if (rc) + pr_err("Set cpu voltage to %lduV failed!\n", volt); + + return rc; +} + +static int sunxi_cpufreq_target_index(struct cpufreq_policy *policy, + unsigned int index) +{ + unsigned long freq = policy->freq_table[index].frequency; + unsigned long old_freq = clk_get_rate(policy->clk) / 1000; + int rc = 0; + + if (freq > old_freq) { + /* Add vol at first */ + if (dvfs_pwm_enale) { + rc = sunxi_set_cpuvol(policy, freq); + usleep_range(300, 500); + if (rc) + goto out; + } + + rc = sunxi_set_cpufreq(policy, freq); + if (rc) + goto out; + } else if (freq < old_freq) { + /* Decrease freq at first */ + rc = sunxi_set_cpufreq(policy, freq); + if (rc) + goto out; + + if (dvfs_pwm_enale) { + rc = sunxi_set_cpuvol(policy, freq); + if (rc) + goto out; + } + } + +out: + return rc; +} + +static const struct cpufreq_soc_data sun8i_18_data = { + .periph_clk = 1, +}; + +static const struct of_device_id sunxi_cpufreq_match_list[] = { + { .compatible = "allwinner,sun8iw18p1", .data = &sun8i_18_data }, + {} +}; + +static const struct of_device_id *sunxi_cpufreq_match_node(void) +{ + struct device_node *np; + const struct of_device_id *match; + + np = of_find_node_by_path("/"); + match = of_match_node(sunxi_cpufreq_match_list, np); + of_node_put(np); + + return match; +} + +static int sunxi_cpufreq_init(struct cpufreq_policy *policy) +{ + struct device *cpu = get_cpu_device(policy->cpu); + const struct cpufreq_soc_data *soc_data; + struct cpufreq_frequency_table *freq_table; + struct device_node *dvfs_table_np; + struct cpufreq_pwm_data *pwm_data; + const struct of_device_id *match; + int transition_latency; + int ret; + + dvfs_table_np = of_find_node_by_path("/opp_l_table0"); + if (!dvfs_table_np) + return -ENODEV; + of_node_put(dvfs_table_np); + + pwm_data = kzalloc(sizeof(*pwm_data), GFP_KERNEL); + if (!pwm_data) + return -ENOMEM; + + pwm_data->pll_clk = of_clk_get(cpu->of_node, 0); + if (IS_ERR_OR_NULL(pwm_data->pll_clk)) { + ret = PTR_ERR(pwm_data->pll_clk); + goto out_free; + } + + match = sunxi_cpufreq_match_node(); + if (!match) { + ret = -ENODEV; + goto out_put_clk; + } + + soc_data = match->data; + if (!soc_data) { + ret = -EINVAL; + goto out_put_clk; + } + + if (soc_data->periph_clk) { + pwm_data->cpu_clk = of_clk_get(cpu->of_node, 1); + if (IS_ERR_OR_NULL(pwm_data->cpu_clk)) { + ret = PTR_ERR(pwm_data->cpu_clk); + goto out_put_clk; + } + + pwm_data->periph_clk = of_clk_get(cpu->of_node, 2); + if (IS_ERR_OR_NULL(pwm_data->periph_clk)) { + ret = PTR_ERR(pwm_data->periph_clk); + goto out_put_cpu_clk; + } + } + + /* Get OPP-sharing information from "operating-points-v2" bindings */ + ret = dev_pm_opp_of_get_sharing_cpus(cpu, policy->cpus); + if (ret) + goto out_put_periph_clk; + + dev_pm_opp_of_cpumask_add_table(policy->cpus); + + ret = dev_pm_opp_get_opp_count(cpu); + if (ret <= 0) { + pr_info("OPP table is not ready, deferring probe\n"); + ret = -EPROBE_DEFER; + goto out_free_opp; + } + + ret = dev_pm_opp_init_cpufreq_table(cpu, &freq_table); + if (ret) + goto out_free_opp; + + ret = cpufreq_table_validate_and_show(policy, freq_table); + if (ret) + goto out_free_opp; + + transition_latency = dev_pm_opp_get_max_transition_latency(cpu); + if (!transition_latency) + transition_latency = CPUFREQ_ETERNAL; + + policy->cpuinfo.transition_latency = transition_latency; + policy->clk = pwm_data->pll_clk; + + policy->driver_data = pwm_data; + + ret = pwm_interface_init(pwm_data); + if (!ret) + dvfs_pwm_enale = ENABLE; + + return 0; + +out_free_opp: + dev_pm_opp_free_cpufreq_table(cpu, &policy->freq_table); + dev_pm_opp_of_cpumask_remove_table(policy->cpus); +out_put_periph_clk: + if (pwm_data->periph_clk) + clk_put(pwm_data->periph_clk); +out_put_cpu_clk: + if (pwm_data->cpu_clk) + clk_put(pwm_data->cpu_clk); +out_put_clk: + clk_put(pwm_data->pll_clk); +out_free: + kfree(pwm_data); + + return ret; +} + +static int sunxi_cpufreq_exit(struct cpufreq_policy *policy) +{ + struct cpufreq_pwm_data *pwm_data = policy->driver_data; + struct device *cpu; + + cpu = get_cpu_device(policy->cpu); + + clk_put(policy->clk); + if (pwm_data->cpu_clk) + clk_put(pwm_data->cpu_clk); + if (pwm_data->periph_clk) + clk_put(pwm_data->periph_clk); + kfree(pwm_data); + cpufreq_cooling_unregister(cdev); + dev_pm_opp_free_cpufreq_table(cpu, &policy->freq_table); + dev_pm_opp_of_cpumask_remove_table(policy->related_cpus); + + return 0; +} + +static void sunxi_cpufreq_ready(struct cpufreq_policy *policy) +{ + struct device *cpu = get_cpu_device(policy->cpu); + struct device_node *np; + + np = of_node_get(cpu->of_node); + if (WARN_ON(!np)) + return; + + if (of_find_property(np, "#cooling-cells", NULL)) { + u32 power_coefficient = 0; + + of_property_read_u32(np, "dynamic-power-coefficient", + &power_coefficient); + + cdev = of_cpufreq_power_cooling_register(np, + policy->related_cpus, power_coefficient, NULL); + if (IS_ERR(cdev)) { + dev_err(cpu, + "running cpufreq without cooling device: %ld\n", + PTR_ERR(cdev)); + cdev = NULL; + } + } + of_node_put(np); +} + +static struct freq_attr *sunxi_cpufreq_attr[] = { + &cpufreq_freq_attr_scaling_available_freqs, + NULL, +}; + +static struct cpufreq_driver sunxi_cpufreq_driver = { + .name = "cpufreq-pwm", + .flags = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK, + .attr = sunxi_cpufreq_attr, + .init = sunxi_cpufreq_init, + .get = cpufreq_generic_get, + .target_index = sunxi_cpufreq_target_index, + .exit = sunxi_cpufreq_exit, + .ready = sunxi_cpufreq_ready, + .verify = cpufreq_generic_frequency_table_verify, +}; + +static int __init sunxi_cpufreq_initcall(void) +{ + int ret; + + ret = cpufreq_register_driver(&sunxi_cpufreq_driver); + if (ret) + pr_err("Failed register driver\n"); + + return ret; +} +module_init(sunxi_cpufreq_initcall); + +MODULE_AUTHOR("frank "); +MODULE_DESCRIPTION("SUNXI DVFS BY PWM"); +MODULE_LICENSE("GPL");