430 lines
10 KiB
C
430 lines
10 KiB
C
/*
|
|
* 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 <linux/init.h>
|
|
#include <linux/module.h>
|
|
#include <linux/cpufreq.h>
|
|
#include <linux/cpu.h>
|
|
#include <linux/cpu_cooling.h>
|
|
#include <linux/of.h>
|
|
#include <linux/clk.h>
|
|
#include <linux/err.h>
|
|
#include <linux/pm_opp.h>
|
|
#include <linux/slab.h>
|
|
#include <linux/pwm.h>
|
|
#include <linux/delay.h>
|
|
|
|
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 <frank@allwinnertech.com>");
|
|
MODULE_DESCRIPTION("SUNXI DVFS BY PWM");
|
|
MODULE_LICENSE("GPL");
|