firmware/br-ext-chip-allwinner/board/v83x/kernel/patches/00000-drivers_cpufreq_sunxi-cpufreq-pwm.c.patch

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 <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");