[linux-block.git] / drivers / cpufreq / mediatek-cpufreq.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * Copyright (c) 2015 Linaro Ltd.
 * Author: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
 */

#include <linux/clk.h>
#include <linux/cpu.h>
#include <linux/cpufreq.h>
#include <linux/cpumask.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/platform_device.h>
#include <linux/pm_opp.h>
#include <linux/regulator/consumer.h>

#define MIN_VOLT_SHIFT		(100000)
#define MAX_VOLT_SHIFT		(200000)
#define MAX_VOLT_LIMIT		(1150000)
#define VOLT_TOL		(10000)

/*
 * The struct mtk_cpu_dvfs_info holds necessary information for doing CPU DVFS
 * on each CPU power/clock domain of Mediatek SoCs. Each CPU cluster in
 * Mediatek SoCs has two voltage inputs, Vproc and Vsram. In some cases the two
 * voltage inputs need to be controlled under a hardware limitation:
 * 100mV < Vsram - Vproc < 200mV
 *
 * When scaling the clock frequency of a CPU clock domain, the clock source
 * needs to be switched to another stable PLL clock temporarily until
 * the original PLL becomes stable at target frequency.
 */
struct mtk_cpu_dvfs_info {
	struct cpumask cpus;
	struct device *cpu_dev;
	struct regulator *proc_reg;
	struct regulator *sram_reg;
	struct clk *cpu_clk;
	struct clk *inter_clk;
	struct list_head list_head;
	int intermediate_voltage;
	bool need_voltage_tracking;
};

static LIST_HEAD(dvfs_info_list);

static struct mtk_cpu_dvfs_info *mtk_cpu_dvfs_info_lookup(int cpu)
{
	struct mtk_cpu_dvfs_info *info;

	list_for_each_entry(info, &dvfs_info_list, list_head) {
		if (cpumask_test_cpu(cpu, &info->cpus))
			return info;
	}

	return NULL;
}

static int mtk_cpufreq_voltage_tracking(struct mtk_cpu_dvfs_info *info,
					int new_vproc)
{
	struct regulator *proc_reg = info->proc_reg;
	struct regulator *sram_reg = info->sram_reg;
	int old_vproc, old_vsram, new_vsram, vsram, vproc, ret;

	old_vproc = regulator_get_voltage(proc_reg);
	if (old_vproc < 0) {
		dev_err(info->cpu_dev,
			"invalid Vproc value: %d\n", old_vproc);
		return old_vproc;
	}
	/* Vsram should not exceed the maximum allowed voltage of SoC. */
	new_vsram = min(new_vproc + MIN_VOLT_SHIFT, MAX_VOLT_LIMIT);

	if (old_vproc < new_vproc) {
		/*
		 * When scaling up voltages, Vsram and Vproc scale up step
		 * by step. At each step, set Vsram to (Vproc + 200mV) first,
		 * then set Vproc to (Vsram - 100mV).
		 * Keep doing it until Vsram and Vproc hit target voltages.
		 */
		do {
			old_vsram = regulator_get_voltage(sram_reg);
			if (old_vsram < 0) {
				dev_err(info->cpu_dev,
					"invalid Vsram value: %d\n", old_vsram);
				return old_vsram;
			}
			old_vproc = regulator_get_voltage(proc_reg);
			if (old_vproc < 0) {
				dev_err(info->cpu_dev,
					"invalid Vproc value: %d\n", old_vproc);
				return old_vproc;
			}

			vsram = min(new_vsram, old_vproc + MAX_VOLT_SHIFT);

			if (vsram + VOLT_TOL >= MAX_VOLT_LIMIT) {
				vsram = MAX_VOLT_LIMIT;

				/*
				 * If the target Vsram hits the maximum voltage,
				 * try to set the exact voltage value first.
				 */
				ret = regulator_set_voltage(sram_reg, vsram,
							    vsram);
				if (ret)
					ret = regulator_set_voltage(sram_reg,
							vsram - VOLT_TOL,
							vsram);

				vproc = new_vproc;
			} else {
				ret = regulator_set_voltage(sram_reg, vsram,
							    vsram + VOLT_TOL);

				vproc = vsram - MIN_VOLT_SHIFT;
			}
			if (ret)
				return ret;

			ret = regulator_set_voltage(proc_reg, vproc,
						    vproc + VOLT_TOL);
			if (ret) {
				regulator_set_voltage(sram_reg, old_vsram,
						      old_vsram);
				return ret;
			}
		} while (vproc < new_vproc || vsram < new_vsram);
	} else if (old_vproc > new_vproc) {
		/*
		 * When scaling down voltages, Vsram and Vproc scale down step
		 * by step. At each step, set Vproc to (Vsram - 200mV) first,
		 * then set Vproc to (Vproc + 100mV).
		 * Keep doing it until Vsram and Vproc hit target voltages.
		 */
		do {
			old_vproc = regulator_get_voltage(proc_reg);
			if (old_vproc < 0) {
				dev_err(info->cpu_dev,
					"invalid Vproc value: %d\n", old_vproc);
				return old_vproc;
			}
			old_vsram = regulator_get_voltage(sram_reg);
			if (old_vsram < 0) {
				dev_err(info->cpu_dev,
					"invalid Vsram value: %d\n", old_vsram);
				return old_vsram;
			}

			vproc = max(new_vproc, old_vsram - MAX_VOLT_SHIFT);
			ret = regulator_set_voltage(proc_reg, vproc,
						    vproc + VOLT_TOL);
			if (ret)
				return ret;

			if (vproc == new_vproc)
				vsram = new_vsram;
			else
				vsram = max(new_vsram, vproc + MIN_VOLT_SHIFT);

			if (vsram + VOLT_TOL >= MAX_VOLT_LIMIT) {
				vsram = MAX_VOLT_LIMIT;

				/*
				 * If the target Vsram hits the maximum voltage,
				 * try to set the exact voltage value first.
				 */
				ret = regulator_set_voltage(sram_reg, vsram,
							    vsram);
				if (ret)
					ret = regulator_set_voltage(sram_reg,
							vsram - VOLT_TOL,
							vsram);
			} else {
				ret = regulator_set_voltage(sram_reg, vsram,
							    vsram + VOLT_TOL);
			}

			if (ret) {
				regulator_set_voltage(proc_reg, old_vproc,
						      old_vproc);
				return ret;
			}
		} while (vproc > new_vproc + VOLT_TOL ||
			 vsram > new_vsram + VOLT_TOL);
	}

	return 0;
}

static int mtk_cpufreq_set_voltage(struct mtk_cpu_dvfs_info *info, int vproc)
{
	if (info->need_voltage_tracking)
		return mtk_cpufreq_voltage_tracking(info, vproc);
	else
		return regulator_set_voltage(info->proc_reg, vproc,
					     vproc + VOLT_TOL);
}

static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
				  unsigned int index)
{
	struct cpufreq_frequency_table *freq_table = policy->freq_table;
	struct clk *cpu_clk = policy->clk;
	struct clk *armpll = clk_get_parent(cpu_clk);
	struct mtk_cpu_dvfs_info *info = policy->driver_data;
	struct device *cpu_dev = info->cpu_dev;
	struct dev_pm_opp *opp;
	long freq_hz, old_freq_hz;
	int vproc, old_vproc, inter_vproc, target_vproc, ret;

	inter_vproc = info->intermediate_voltage;

	old_freq_hz = clk_get_rate(cpu_clk);
	old_vproc = regulator_get_voltage(info->proc_reg);
	if (old_vproc < 0) {
		dev_err(cpu_dev, "invalid Vproc value: %d\n", old_vproc);
		return old_vproc;
	}

	freq_hz = freq_table[index].frequency * 1000;

	opp = dev_pm_opp_find_freq_ceil(cpu_dev, &freq_hz);
	if (IS_ERR(opp)) {
		dev_err(cpu_dev, "cpu%d: failed to find OPP for %ld\n",
			policy->cpu, freq_hz);
		return PTR_ERR(opp);
	}
	vproc = dev_pm_opp_get_voltage(opp);
	dev_pm_opp_put(opp);

	/*
	 * If the new voltage or the intermediate voltage is higher than the
	 * current voltage, scale up voltage first.
	 */
	target_vproc = (inter_vproc > vproc) ? inter_vproc : vproc;
	if (old_vproc < target_vproc) {
		ret = mtk_cpufreq_set_voltage(info, target_vproc);
		if (ret) {
			dev_err(cpu_dev,
				"cpu%d: failed to scale up voltage!\n", policy->cpu);
			mtk_cpufreq_set_voltage(info, old_vproc);
			return ret;
		}
	}

	/* Reparent the CPU clock to intermediate clock. */
	ret = clk_set_parent(cpu_clk, info->inter_clk);
	if (ret) {
		dev_err(cpu_dev,
			"cpu%d: failed to re-parent cpu clock!\n", policy->cpu);
		mtk_cpufreq_set_voltage(info, old_vproc);
		WARN_ON(1);
		return ret;
	}

	/* Set the original PLL to target rate. */
	ret = clk_set_rate(armpll, freq_hz);
	if (ret) {
		dev_err(cpu_dev,
			"cpu%d: failed to scale cpu clock rate!\n", policy->cpu);
		clk_set_parent(cpu_clk, armpll);
		mtk_cpufreq_set_voltage(info, old_vproc);
		return ret;
	}

	/* Set parent of CPU clock back to the original PLL. */
	ret = clk_set_parent(cpu_clk, armpll);
	if (ret) {
		dev_err(cpu_dev,
			"cpu%d: failed to re-parent cpu clock!\n", policy->cpu);
		mtk_cpufreq_set_voltage(info, inter_vproc);
		WARN_ON(1);
		return ret;
	}

	/*
	 * If the new voltage is lower than the intermediate voltage or the
	 * original voltage, scale down to the new voltage.
	 */
	if (vproc < inter_vproc || vproc < old_vproc) {
		ret = mtk_cpufreq_set_voltage(info, vproc);
		if (ret) {
			dev_err(cpu_dev,
				"cpu%d: failed to scale down voltage!\n", policy->cpu);
			clk_set_parent(cpu_clk, info->inter_clk);
			clk_set_rate(armpll, old_freq_hz);
			clk_set_parent(cpu_clk, armpll);
			return ret;
		}
	}

	return 0;
}

#define DYNAMIC_POWER "dynamic-power-coefficient"

static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
{
	struct device *cpu_dev;
	struct dev_pm_opp *opp;
	unsigned long rate;
	int ret;

	cpu_dev = get_cpu_device(cpu);
	if (!cpu_dev) {
		dev_err(cpu_dev, "failed to get cpu%d device\n", cpu);
		return -ENODEV;
	}
	info->cpu_dev = cpu_dev;

	info->cpu_clk = clk_get(cpu_dev, "cpu");
	if (IS_ERR(info->cpu_clk)) {
		ret = PTR_ERR(info->cpu_clk);
		return dev_err_probe(cpu_dev, ret,
				     "cpu%d: failed to get cpu clk\n", cpu);
	}

	info->inter_clk = clk_get(cpu_dev, "intermediate");
	if (IS_ERR(info->inter_clk)) {
		ret = PTR_ERR(info->inter_clk);
		dev_err_probe(cpu_dev, ret,
			      "cpu%d: failed to get intermediate clk\n", cpu);
		goto out_free_resources;
	}

	info->proc_reg = regulator_get_optional(cpu_dev, "proc");
	if (IS_ERR(info->proc_reg)) {
		ret = PTR_ERR(info->proc_reg);
		dev_err_probe(cpu_dev, ret,
			      "cpu%d: failed to get proc regulator\n", cpu);
		goto out_free_resources;
	}

	ret = regulator_enable(info->proc_reg);
	if (ret) {
		dev_warn(cpu_dev, "cpu%d: failed to enable vproc\n", cpu);
		goto out_free_resources;
	}

	/* Both presence and absence of sram regulator are valid cases. */
	info->sram_reg = regulator_get_exclusive(cpu_dev, "sram");
	if (IS_ERR(info->sram_reg))
		info->sram_reg = NULL;
	else {
		ret = regulator_enable(info->sram_reg);
		if (ret) {
			dev_warn(cpu_dev, "cpu%d: failed to enable vsram\n", cpu);
			goto out_free_resources;
		}
	}

	/* Get OPP-sharing information from "operating-points-v2" bindings */
	ret = dev_pm_opp_of_get_sharing_cpus(cpu_dev, &info->cpus);
	if (ret) {
		dev_err(cpu_dev,
			"cpu%d: failed to get OPP-sharing information\n", cpu);
		goto out_free_resources;
	}

	ret = dev_pm_opp_of_cpumask_add_table(&info->cpus);
	if (ret) {
		dev_warn(cpu_dev, "cpu%d: no OPP table\n", cpu);
		goto out_free_resources;
	}

	ret = clk_prepare_enable(info->cpu_clk);
	if (ret)
		goto out_free_opp_table;

	ret = clk_prepare_enable(info->inter_clk);
	if (ret)
		goto out_disable_mux_clock;

	/* Search a safe voltage for intermediate frequency. */
	rate = clk_get_rate(info->inter_clk);
	opp = dev_pm_opp_find_freq_ceil(cpu_dev, &rate);
	if (IS_ERR(opp)) {
		dev_err(cpu_dev, "cpu%d: failed to get intermediate opp\n", cpu);
		ret = PTR_ERR(opp);
		goto out_disable_inter_clock;
	}
	info->intermediate_voltage = dev_pm_opp_get_voltage(opp);
	dev_pm_opp_put(opp);

	/*
	 * If SRAM regulator is present, software "voltage tracking" is needed
	 * for this CPU power domain.
	 */
	info->need_voltage_tracking = (info->sram_reg != NULL);

	return 0;

out_disable_inter_clock:
	clk_disable_unprepare(info->inter_clk);

out_disable_mux_clock:
	clk_disable_unprepare(info->cpu_clk);

out_free_opp_table:
	dev_pm_opp_of_cpumask_remove_table(&info->cpus);

out_free_resources:
	if (regulator_is_enabled(info->proc_reg))
		regulator_disable(info->proc_reg);
	if (info->sram_reg && regulator_is_enabled(info->sram_reg))
		regulator_disable(info->sram_reg);

	if (!IS_ERR(info->proc_reg))
		regulator_put(info->proc_reg);
	if (!IS_ERR(info->sram_reg))
		regulator_put(info->sram_reg);
	if (!IS_ERR(info->cpu_clk))
		clk_put(info->cpu_clk);
	if (!IS_ERR(info->inter_clk))
		clk_put(info->inter_clk);

	return ret;
}

static void mtk_cpu_dvfs_info_release(struct mtk_cpu_dvfs_info *info)
{
	if (!IS_ERR(info->proc_reg)) {
		regulator_disable(info->proc_reg);
		regulator_put(info->proc_reg);
	}
	if (!IS_ERR(info->sram_reg)) {
		regulator_disable(info->sram_reg);
		regulator_put(info->sram_reg);
	}
	if (!IS_ERR(info->cpu_clk)) {
		clk_disable_unprepare(info->cpu_clk);
		clk_put(info->cpu_clk);
	}
	if (!IS_ERR(info->inter_clk)) {
		clk_disable_unprepare(info->inter_clk);
		clk_put(info->inter_clk);
	}

	dev_pm_opp_of_cpumask_remove_table(&info->cpus);
}

static int mtk_cpufreq_init(struct cpufreq_policy *policy)
{
	struct mtk_cpu_dvfs_info *info;
	struct cpufreq_frequency_table *freq_table;
	int ret;

	info = mtk_cpu_dvfs_info_lookup(policy->cpu);
	if (!info) {
		dev_err(info->cpu_dev,
			"dvfs info for cpu%d is not initialized.\n", policy->cpu);
		return -EINVAL;
	}

	ret = dev_pm_opp_init_cpufreq_table(info->cpu_dev, &freq_table);
	if (ret) {
		dev_err(info->cpu_dev,
			"failed to init cpufreq table for cpu%d: %d\n",
			policy->cpu, ret);
		return ret;
	}

	cpumask_copy(policy->cpus, &info->cpus);
	policy->freq_table = freq_table;
	policy->driver_data = info;
	policy->clk = info->cpu_clk;

	return 0;
}

static int mtk_cpufreq_exit(struct cpufreq_policy *policy)
{
	struct mtk_cpu_dvfs_info *info = policy->driver_data;

	dev_pm_opp_free_cpufreq_table(info->cpu_dev, &policy->freq_table);

	return 0;
}

static struct cpufreq_driver mtk_cpufreq_driver = {
	.flags = CPUFREQ_NEED_INITIAL_FREQ_CHECK |
		 CPUFREQ_HAVE_GOVERNOR_PER_POLICY |
		 CPUFREQ_IS_COOLING_DEV,
	.verify = cpufreq_generic_frequency_table_verify,
	.target_index = mtk_cpufreq_set_target,
	.get = cpufreq_generic_get,
	.init = mtk_cpufreq_init,
	.exit = mtk_cpufreq_exit,
	.register_em = cpufreq_register_em_with_opp,
	.name = "mtk-cpufreq",
	.attr = cpufreq_generic_attr,
};

static int mtk_cpufreq_probe(struct platform_device *pdev)
{
	struct mtk_cpu_dvfs_info *info, *tmp;
	int cpu, ret;

	for_each_possible_cpu(cpu) {
		info = mtk_cpu_dvfs_info_lookup(cpu);
		if (info)
			continue;

		info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
		if (!info) {
			ret = -ENOMEM;
			goto release_dvfs_info_list;
		}

		ret = mtk_cpu_dvfs_info_init(info, cpu);
		if (ret) {
			dev_err(&pdev->dev,
				"failed to initialize dvfs info for cpu%d\n",
				cpu);
			goto release_dvfs_info_list;
		}

		list_add(&info->list_head, &dvfs_info_list);
	}

	ret = cpufreq_register_driver(&mtk_cpufreq_driver);
	if (ret) {
		dev_err(&pdev->dev, "failed to register mtk cpufreq driver\n");
		goto release_dvfs_info_list;
	}

	return 0;

release_dvfs_info_list:
	list_for_each_entry_safe(info, tmp, &dvfs_info_list, list_head) {
		mtk_cpu_dvfs_info_release(info);
		list_del(&info->list_head);
	}

	return ret;
}

static struct platform_driver mtk_cpufreq_platdrv = {
	.driver = {
		.name	= "mtk-cpufreq",
	},
	.probe		= mtk_cpufreq_probe,
};

/* List of machines supported by this driver */
static const struct of_device_id mtk_cpufreq_machines[] __initconst = {
	{ .compatible = "mediatek,mt2701", },
	{ .compatible = "mediatek,mt2712", },
	{ .compatible = "mediatek,mt7622", },
	{ .compatible = "mediatek,mt7623", },
	{ .compatible = "mediatek,mt8167", },
	{ .compatible = "mediatek,mt817x", },
	{ .compatible = "mediatek,mt8173", },
	{ .compatible = "mediatek,mt8176", },
	{ .compatible = "mediatek,mt8183", },
	{ .compatible = "mediatek,mt8365", },
	{ .compatible = "mediatek,mt8516", },

	{ }
};
MODULE_DEVICE_TABLE(of, mtk_cpufreq_machines);

static int __init mtk_cpufreq_driver_init(void)
{
	struct device_node *np;
	const struct of_device_id *match;
	struct platform_device *pdev;
	int err;

	np = of_find_node_by_path("/");
	if (!np)
		return -ENODEV;

	match = of_match_node(mtk_cpufreq_machines, np);
	of_node_put(np);
	if (!match) {
		pr_debug("Machine is not compatible with mtk-cpufreq\n");
		return -ENODEV;
	}

	err = platform_driver_register(&mtk_cpufreq_platdrv);
	if (err)
		return err;

	/*
	 * Since there's no place to hold device registration code and no
	 * device tree based way to match cpufreq driver yet, both the driver
	 * and the device registration codes are put here to handle defer
	 * probing.
	 */
	pdev = platform_device_register_simple("mtk-cpufreq", -1, NULL, 0);
	if (IS_ERR(pdev)) {
		pr_err("failed to register mtk-cpufreq platform device\n");
		platform_driver_unregister(&mtk_cpufreq_platdrv);
		return PTR_ERR(pdev);
	}

	return 0;
}
module_init(mtk_cpufreq_driver_init)

static void __exit mtk_cpufreq_driver_exit(void)
{
	platform_driver_unregister(&mtk_cpufreq_platdrv);
}
module_exit(mtk_cpufreq_driver_exit)

MODULE_DESCRIPTION("MediaTek CPUFreq driver");
MODULE_AUTHOR("Pi-Cheng Chen <pi-cheng.chen@linaro.org>");
MODULE_LICENSE("GPL v2");
Commit	Line	Data
1802d0be	1	// SPDX-License-Identifier: GPL-2.0-only
1453863f PCC	2	/*
	3	* Copyright (c) 2015 Linaro Ltd.
	4	* Author: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
1453863f PCC	5	*/
	6
	7	#include <linux/clk.h>
	8	#include <linux/cpu.h>
1453863f PCC	9	#include <linux/cpufreq.h>
1453863f PCC	10	#include <linux/cpumask.h>
3c2002ae	11	#include <linux/module.h>
1453863f PCC	12	#include <linux/of.h>
	13	#include <linux/platform_device.h>
	14	#include <linux/pm_opp.h>
	15	#include <linux/regulator/consumer.h>
1453863f PCC	16
	17	#define MIN_VOLT_SHIFT (100000)
	18	#define MAX_VOLT_SHIFT (200000)
	19	#define MAX_VOLT_LIMIT (1150000)
	20	#define VOLT_TOL (10000)
	21
	22	/*
	23	* The struct mtk_cpu_dvfs_info holds necessary information for doing CPU DVFS
	24	* on each CPU power/clock domain of Mediatek SoCs. Each CPU cluster in
	25	* Mediatek SoCs has two voltage inputs, Vproc and Vsram. In some cases the two
	26	* voltage inputs need to be controlled under a hardware limitation:
	27	* 100mV < Vsram - Vproc < 200mV
	28	*
	29	* When scaling the clock frequency of a CPU clock domain, the clock source
	30	* needs to be switched to another stable PLL clock temporarily until
	31	* the original PLL becomes stable at target frequency.
	32	*/
	33	struct mtk_cpu_dvfs_info {
89b56047	34	struct cpumask cpus;
1453863f PCC	35	struct device *cpu_dev;
	36	struct regulator *proc_reg;
	37	struct regulator *sram_reg;
	38	struct clk *cpu_clk;
	39	struct clk *inter_clk;
89b56047	40	struct list_head list_head;
1453863f PCC	41	int intermediate_voltage;
	42	bool need_voltage_tracking;
	43	};
	44
89b56047 PCC	45	static LIST_HEAD(dvfs_info_list);
	46
	47	static struct mtk_cpu_dvfs_info *mtk_cpu_dvfs_info_lookup(int cpu)
	48	{
	49	struct mtk_cpu_dvfs_info *info;
89b56047	50
d2499d05	51	list_for_each_entry(info, &dvfs_info_list, list_head) {
89b56047 PCC	52	if (cpumask_test_cpu(cpu, &info->cpus))
	53	return info;
	54	}
	55
	56	return NULL;
	57	}
	58
1453863f PCC	59	static int mtk_cpufreq_voltage_tracking(struct mtk_cpu_dvfs_info *info,
	60	int new_vproc)
	61	{
	62	struct regulator *proc_reg = info->proc_reg;
	63	struct regulator *sram_reg = info->sram_reg;
	64	int old_vproc, old_vsram, new_vsram, vsram, vproc, ret;
	65
	66	old_vproc = regulator_get_voltage(proc_reg);
40be4c3c	67	if (old_vproc < 0) {
9acc0f7a RBC	68	dev_err(info->cpu_dev,
9acc0f7a RBC	69	"invalid Vproc value: %d\n", old_vproc);
40be4c3c PCC	70	return old_vproc;
40be4c3c PCC	71	}
1453863f PCC	72	/* Vsram should not exceed the maximum allowed voltage of SoC. */
	73	new_vsram = min(new_vproc + MIN_VOLT_SHIFT, MAX_VOLT_LIMIT);
	74
	75	if (old_vproc < new_vproc) {
	76	/*
	77	* When scaling up voltages, Vsram and Vproc scale up step
	78	* by step. At each step, set Vsram to (Vproc + 200mV) first,
	79	* then set Vproc to (Vsram - 100mV).
	80	* Keep doing it until Vsram and Vproc hit target voltages.
	81	*/
	82	do {
	83	old_vsram = regulator_get_voltage(sram_reg);
40be4c3c	84	if (old_vsram < 0) {
9acc0f7a RBC	85	dev_err(info->cpu_dev,
9acc0f7a RBC	86	"invalid Vsram value: %d\n", old_vsram);
40be4c3c PCC	87	return old_vsram;
40be4c3c PCC	88	}
1453863f	89	old_vproc = regulator_get_voltage(proc_reg);
40be4c3c	90	if (old_vproc < 0) {
9acc0f7a RBC	91	dev_err(info->cpu_dev,
9acc0f7a RBC	92	"invalid Vproc value: %d\n", old_vproc);
40be4c3c PCC	93	return old_vproc;
40be4c3c PCC	94	}
1453863f PCC	95
	96	vsram = min(new_vsram, old_vproc + MAX_VOLT_SHIFT);
	97
	98	if (vsram + VOLT_TOL >= MAX_VOLT_LIMIT) {
	99	vsram = MAX_VOLT_LIMIT;
	100
	101	/*
	102	* If the target Vsram hits the maximum voltage,
	103	* try to set the exact voltage value first.
	104	*/
	105	ret = regulator_set_voltage(sram_reg, vsram,
	106	vsram);
	107	if (ret)
	108	ret = regulator_set_voltage(sram_reg,
	109	vsram - VOLT_TOL,
	110	vsram);
	111
	112	vproc = new_vproc;
	113	} else {
	114	ret = regulator_set_voltage(sram_reg, vsram,
	115	vsram + VOLT_TOL);
	116
	117	vproc = vsram - MIN_VOLT_SHIFT;
	118	}
	119	if (ret)
	120	return ret;
	121
	122	ret = regulator_set_voltage(proc_reg, vproc,
	123	vproc + VOLT_TOL);
	124	if (ret) {
	125	regulator_set_voltage(sram_reg, old_vsram,
	126	old_vsram);
	127	return ret;
	128	}
	129	} while (vproc < new_vproc \|\| vsram < new_vsram);
	130	} else if (old_vproc > new_vproc) {
	131	/*
	132	* When scaling down voltages, Vsram and Vproc scale down step
	133	* by step. At each step, set Vproc to (Vsram - 200mV) first,
	134	* then set Vproc to (Vproc + 100mV).
	135	* Keep doing it until Vsram and Vproc hit target voltages.
	136	*/
	137	do {
	138	old_vproc = regulator_get_voltage(proc_reg);
40be4c3c	139	if (old_vproc < 0) {
9acc0f7a RBC	140	dev_err(info->cpu_dev,
9acc0f7a RBC	141	"invalid Vproc value: %d\n", old_vproc);
40be4c3c PCC	142	return old_vproc;
40be4c3c PCC	143	}
1453863f	144	old_vsram = regulator_get_voltage(sram_reg);
40be4c3c	145	if (old_vsram < 0) {
9acc0f7a RBC	146	dev_err(info->cpu_dev,
9acc0f7a RBC	147	"invalid Vsram value: %d\n", old_vsram);
40be4c3c PCC	148	return old_vsram;
40be4c3c PCC	149	}
1453863f PCC	150
	151	vproc = max(new_vproc, old_vsram - MAX_VOLT_SHIFT);
	152	ret = regulator_set_voltage(proc_reg, vproc,
	153	vproc + VOLT_TOL);
	154	if (ret)
	155	return ret;
	156
	157	if (vproc == new_vproc)
	158	vsram = new_vsram;
	159	else
	160	vsram = max(new_vsram, vproc + MIN_VOLT_SHIFT);
	161
	162	if (vsram + VOLT_TOL >= MAX_VOLT_LIMIT) {
	163	vsram = MAX_VOLT_LIMIT;
	164
	165	/*
	166	* If the target Vsram hits the maximum voltage,
	167	* try to set the exact voltage value first.
	168	*/
	169	ret = regulator_set_voltage(sram_reg, vsram,
	170	vsram);
	171	if (ret)
	172	ret = regulator_set_voltage(sram_reg,
	173	vsram - VOLT_TOL,
	174	vsram);
	175	} else {
	176	ret = regulator_set_voltage(sram_reg, vsram,
	177	vsram + VOLT_TOL);
	178	}
	179
	180	if (ret) {
	181	regulator_set_voltage(proc_reg, old_vproc,
	182	old_vproc);
	183	return ret;
	184	}
	185	} while (vproc > new_vproc + VOLT_TOL \|\|
	186	vsram > new_vsram + VOLT_TOL);
	187	}
	188
	189	return 0;
	190	}
	191
	192	static int mtk_cpufreq_set_voltage(struct mtk_cpu_dvfs_info *info, int vproc)
	193	{
	194	if (info->need_voltage_tracking)
	195	return mtk_cpufreq_voltage_tracking(info, vproc);
	196	else
	197	return regulator_set_voltage(info->proc_reg, vproc,
	198	vproc + VOLT_TOL);
	199	}
	200
	201	static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
	202	unsigned int index)
	203	{
	204	struct cpufreq_frequency_table *freq_table = policy->freq_table;
	205	struct clk *cpu_clk = policy->clk;
	206	struct clk *armpll = clk_get_parent(cpu_clk);
	207	struct mtk_cpu_dvfs_info *info = policy->driver_data;
	208	struct device *cpu_dev = info->cpu_dev;
	209	struct dev_pm_opp *opp;
	210	long freq_hz, old_freq_hz;
	211	int vproc, old_vproc, inter_vproc, target_vproc, ret;
	212
	213	inter_vproc = info->intermediate_voltage;
214
215	old_freq_hz = clk_get_rate(cpu_clk);
216	old_vproc = regulator_get_voltage(info->proc_reg);
40be4c3c	217	if (old_vproc < 0) {
9acc0f7a	218	dev_err(cpu_dev, "invalid Vproc value: %d\n", old_vproc);
40be4c3c PCC	219	return old_vproc;
40be4c3c PCC	220	}
1453863f PCC	221
	222	freq_hz = freq_table[index].frequency * 1000;
	223
1453863f PCC	224	opp = dev_pm_opp_find_freq_ceil(cpu_dev, &freq_hz);
1453863f PCC	225	if (IS_ERR(opp)) {
9acc0f7a RBC	226	dev_err(cpu_dev, "cpu%d: failed to find OPP for %ld\n",
9acc0f7a RBC	227	policy->cpu, freq_hz);
1453863f PCC	228	return PTR_ERR(opp);
	229	}
	230	vproc = dev_pm_opp_get_voltage(opp);
8a31d9d9	231	dev_pm_opp_put(opp);
1453863f PCC	232
	233	/*
	234	* If the new voltage or the intermediate voltage is higher than the
	235	* current voltage, scale up voltage first.
	236	*/
	237	target_vproc = (inter_vproc > vproc) ? inter_vproc : vproc;
	238	if (old_vproc < target_vproc) {
	239	ret = mtk_cpufreq_set_voltage(info, target_vproc);
	240	if (ret) {
9acc0f7a RBC	241	dev_err(cpu_dev,
9acc0f7a RBC	242	"cpu%d: failed to scale up voltage!\n", policy->cpu);
1453863f PCC	243	mtk_cpufreq_set_voltage(info, old_vproc);
	244	return ret;
	245	}
	246	}
	247
	248	/* Reparent the CPU clock to intermediate clock. */
	249	ret = clk_set_parent(cpu_clk, info->inter_clk);
	250	if (ret) {
9acc0f7a RBC	251	dev_err(cpu_dev,
9acc0f7a RBC	252	"cpu%d: failed to re-parent cpu clock!\n", policy->cpu);
1453863f PCC	253	mtk_cpufreq_set_voltage(info, old_vproc);
	254	WARN_ON(1);
	255	return ret;
	256	}
	257
	258	/* Set the original PLL to target rate. */
	259	ret = clk_set_rate(armpll, freq_hz);
	260	if (ret) {
9acc0f7a RBC	261	dev_err(cpu_dev,
9acc0f7a RBC	262	"cpu%d: failed to scale cpu clock rate!\n", policy->cpu);
1453863f PCC	263	clk_set_parent(cpu_clk, armpll);
	264	mtk_cpufreq_set_voltage(info, old_vproc);
	265	return ret;
	266	}
	267
	268	/* Set parent of CPU clock back to the original PLL. */
	269	ret = clk_set_parent(cpu_clk, armpll);
	270	if (ret) {
9acc0f7a RBC	271	dev_err(cpu_dev,
9acc0f7a RBC	272	"cpu%d: failed to re-parent cpu clock!\n", policy->cpu);
1453863f PCC	273	mtk_cpufreq_set_voltage(info, inter_vproc);
	274	WARN_ON(1);
	275	return ret;
	276	}
	277
	278	/*
	279	* If the new voltage is lower than the intermediate voltage or the
	280	* original voltage, scale down to the new voltage.
	281	*/
	282	if (vproc < inter_vproc \|\| vproc < old_vproc) {
	283	ret = mtk_cpufreq_set_voltage(info, vproc);
	284	if (ret) {
9acc0f7a RBC	285	dev_err(cpu_dev,
9acc0f7a RBC	286	"cpu%d: failed to scale down voltage!\n", policy->cpu);
1453863f PCC	287	clk_set_parent(cpu_clk, info->inter_clk);
	288	clk_set_rate(armpll, old_freq_hz);
	289	clk_set_parent(cpu_clk, armpll);
	290	return ret;
	291	}
	292	}
	293
	294	return 0;
	295	}
	296
d2901603 DC	297	#define DYNAMIC_POWER "dynamic-power-coefficient"
d2901603 DC	298
1453863f PCC	299	static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
	300	{
	301	struct device *cpu_dev;
1453863f PCC	302	struct dev_pm_opp *opp;
	303	unsigned long rate;
	304	int ret;
	305
	306	cpu_dev = get_cpu_device(cpu);
	307	if (!cpu_dev) {
396dee97	308	dev_err(cpu_dev, "failed to get cpu%d device\n", cpu);
1453863f PCC	309	return -ENODEV;
1453863f PCC	310	}
396dee97	311	info->cpu_dev = cpu_dev;
1453863f	312
396dee97 JWC	313	info->cpu_clk = clk_get(cpu_dev, "cpu");
	314	if (IS_ERR(info->cpu_clk)) {
	315	ret = PTR_ERR(info->cpu_clk);
	316	return dev_err_probe(cpu_dev, ret,
	317	"cpu%d: failed to get cpu clk\n", cpu);
1453863f PCC	318	}
1453863f PCC	319
396dee97 JWC	320	info->inter_clk = clk_get(cpu_dev, "intermediate");
	321	if (IS_ERR(info->inter_clk)) {
	322	ret = PTR_ERR(info->inter_clk);
	323	dev_err_probe(cpu_dev, ret,
	324	"cpu%d: failed to get intermediate clk\n", cpu);
1453863f PCC	325	goto out_free_resources;
	326	}
	327
396dee97 JWC	328	info->proc_reg = regulator_get_optional(cpu_dev, "proc");
	329	if (IS_ERR(info->proc_reg)) {
	330	ret = PTR_ERR(info->proc_reg);
	331	dev_err_probe(cpu_dev, ret,
	332	"cpu%d: failed to get proc regulator\n", cpu);
1453863f PCC	333	goto out_free_resources;
	334	}
	335
4b9ceb75 JWC	336	ret = regulator_enable(info->proc_reg);
	337	if (ret) {
	338	dev_warn(cpu_dev, "cpu%d: failed to enable vproc\n", cpu);
	339	goto out_free_resources;
	340	}
	341
1453863f	342	/* Both presence and absence of sram regulator are valid cases. */
396dee97 JWC	343	info->sram_reg = regulator_get_exclusive(cpu_dev, "sram");
	344	if (IS_ERR(info->sram_reg))
	345	info->sram_reg = NULL;
4b9ceb75 JWC	346	else {
	347	ret = regulator_enable(info->sram_reg);
	348	if (ret) {
	349	dev_warn(cpu_dev, "cpu%d: failed to enable vsram\n", cpu);
	350	goto out_free_resources;
	351	}
	352	}
1453863f	353
a889331d PCC	354	/* Get OPP-sharing information from "operating-points-v2" bindings */
	355	ret = dev_pm_opp_of_get_sharing_cpus(cpu_dev, &info->cpus);
	356	if (ret) {
396dee97 JWC	357	dev_err(cpu_dev,
396dee97 JWC	358	"cpu%d: failed to get OPP-sharing information\n", cpu);
a889331d PCC	359	goto out_free_resources;
	360	}
	361
	362	ret = dev_pm_opp_of_cpumask_add_table(&info->cpus);
1453863f	363	if (ret) {
396dee97	364	dev_warn(cpu_dev, "cpu%d: no OPP table\n", cpu);
1453863f PCC	365	goto out_free_resources;
	366	}
	367
4b9ceb75 JWC	368	ret = clk_prepare_enable(info->cpu_clk);
	369	if (ret)
	370	goto out_free_opp_table;
	371
	372	ret = clk_prepare_enable(info->inter_clk);
	373	if (ret)
	374	goto out_disable_mux_clock;
	375
1453863f	376	/* Search a safe voltage for intermediate frequency. */
396dee97	377	rate = clk_get_rate(info->inter_clk);
1453863f PCC	378	opp = dev_pm_opp_find_freq_ceil(cpu_dev, &rate);
1453863f PCC	379	if (IS_ERR(opp)) {
396dee97	380	dev_err(cpu_dev, "cpu%d: failed to get intermediate opp\n", cpu);
1453863f	381	ret = PTR_ERR(opp);
4b9ceb75	382	goto out_disable_inter_clock;
1453863f PCC	383	}
1453863f PCC	384	info->intermediate_voltage = dev_pm_opp_get_voltage(opp);
8a31d9d9	385	dev_pm_opp_put(opp);
1453863f	386
1453863f PCC	387	/*
	388	* If SRAM regulator is present, software "voltage tracking" is needed
	389	* for this CPU power domain.
	390	*/
396dee97	391	info->need_voltage_tracking = (info->sram_reg != NULL);
1453863f PCC	392
	393	return 0;
	394
4b9ceb75 JWC	395	out_disable_inter_clock:
	396	clk_disable_unprepare(info->inter_clk);
	397
	398	out_disable_mux_clock:
	399	clk_disable_unprepare(info->cpu_clk);
	400
1453863f	401	out_free_opp_table:
a889331d	402	dev_pm_opp_of_cpumask_remove_table(&info->cpus);
1453863f PCC	403
1453863f PCC	404	out_free_resources:
4b9ceb75 JWC	405	if (regulator_is_enabled(info->proc_reg))
	406	regulator_disable(info->proc_reg);
	407	if (info->sram_reg && regulator_is_enabled(info->sram_reg))
	408	regulator_disable(info->sram_reg);
	409
396dee97 JWC	410	if (!IS_ERR(info->proc_reg))
	411	regulator_put(info->proc_reg);
	412	if (!IS_ERR(info->sram_reg))
	413	regulator_put(info->sram_reg);
	414	if (!IS_ERR(info->cpu_clk))
	415	clk_put(info->cpu_clk);
	416	if (!IS_ERR(info->inter_clk))
	417	clk_put(info->inter_clk);
1453863f PCC	418
	419	return ret;
	420	}
	421
	422	static void mtk_cpu_dvfs_info_release(struct mtk_cpu_dvfs_info *info)
	423	{
4b9ceb75 JWC	424	if (!IS_ERR(info->proc_reg)) {
4b9ceb75 JWC	425	regulator_disable(info->proc_reg);
1453863f	426	regulator_put(info->proc_reg);
4b9ceb75 JWC	427	}
	428	if (!IS_ERR(info->sram_reg)) {
	429	regulator_disable(info->sram_reg);
1453863f	430	regulator_put(info->sram_reg);
4b9ceb75 JWC	431	}
	432	if (!IS_ERR(info->cpu_clk)) {
	433	clk_disable_unprepare(info->cpu_clk);
1453863f	434	clk_put(info->cpu_clk);
4b9ceb75 JWC	435	}
	436	if (!IS_ERR(info->inter_clk)) {
	437	clk_disable_unprepare(info->inter_clk);
1453863f	438	clk_put(info->inter_clk);
4b9ceb75	439	}
1453863f	440
a889331d	441	dev_pm_opp_of_cpumask_remove_table(&info->cpus);
1453863f PCC	442	}
	443
	444	static int mtk_cpufreq_init(struct cpufreq_policy *policy)
	445	{
	446	struct mtk_cpu_dvfs_info *info;
	447	struct cpufreq_frequency_table *freq_table;
	448	int ret;
	449
89b56047 PCC	450	info = mtk_cpu_dvfs_info_lookup(policy->cpu);
89b56047 PCC	451	if (!info) {
9acc0f7a RBC	452	dev_err(info->cpu_dev,
9acc0f7a RBC	453	"dvfs info for cpu%d is not initialized.\n", policy->cpu);
89b56047	454	return -EINVAL;
1453863f PCC	455	}
	456
	457	ret = dev_pm_opp_init_cpufreq_table(info->cpu_dev, &freq_table);
	458	if (ret) {
9acc0f7a RBC	459	dev_err(info->cpu_dev,
	460	"failed to init cpufreq table for cpu%d: %d\n",
	461	policy->cpu, ret);
89b56047	462	return ret;
1453863f PCC	463	}
1453863f PCC	464
89b56047	465	cpumask_copy(policy->cpus, &info->cpus);
b563afba	466	policy->freq_table = freq_table;
1453863f PCC	467	policy->driver_data = info;
	468	policy->clk = info->cpu_clk;
	469
	470	return 0;
1453863f PCC	471	}
	472
	473	static int mtk_cpufreq_exit(struct cpufreq_policy *policy)
	474	{
	475	struct mtk_cpu_dvfs_info *info = policy->driver_data;
	476
1453863f	477	dev_pm_opp_free_cpufreq_table(info->cpu_dev, &policy->freq_table);
1453863f PCC	478
	479	return 0;
	480	}
	481
862e0104	482	static struct cpufreq_driver mtk_cpufreq_driver = {
5ae4a4b4	483	.flags = CPUFREQ_NEED_INITIAL_FREQ_CHECK \|
0db60d6b AK	484	CPUFREQ_HAVE_GOVERNOR_PER_POLICY \|
0db60d6b AK	485	CPUFREQ_IS_COOLING_DEV,
1453863f PCC	486	.verify = cpufreq_generic_frequency_table_verify,
	487	.target_index = mtk_cpufreq_set_target,
	488	.get = cpufreq_generic_get,
	489	.init = mtk_cpufreq_init,
	490	.exit = mtk_cpufreq_exit,
3701fd64	491	.register_em = cpufreq_register_em_with_opp,
1453863f PCC	492	.name = "mtk-cpufreq",
	493	.attr = cpufreq_generic_attr,
	494	};
	495
862e0104	496	static int mtk_cpufreq_probe(struct platform_device *pdev)
1453863f	497	{
d2499d05	498	struct mtk_cpu_dvfs_info info, tmp;
89b56047 PCC	499	int cpu, ret;
	500
	501	for_each_possible_cpu(cpu) {
	502	info = mtk_cpu_dvfs_info_lookup(cpu);
	503	if (info)
	504	continue;
	505
	506	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
	507	if (!info) {
	508	ret = -ENOMEM;
	509	goto release_dvfs_info_list;
	510	}
	511
	512	ret = mtk_cpu_dvfs_info_init(info, cpu);
	513	if (ret) {
	514	dev_err(&pdev->dev,
	515	"failed to initialize dvfs info for cpu%d\n",
	516	cpu);
	517	goto release_dvfs_info_list;
	518	}
	519
	520	list_add(&info->list_head, &dvfs_info_list);
	521	}
1453863f	522
862e0104	523	ret = cpufreq_register_driver(&mtk_cpufreq_driver);
89b56047 PCC	524	if (ret) {
	525	dev_err(&pdev->dev, "failed to register mtk cpufreq driver\n");
	526	goto release_dvfs_info_list;
	527	}
	528
	529	return 0;
	530
	531	release_dvfs_info_list:
d2499d05	532	list_for_each_entry_safe(info, tmp, &dvfs_info_list, list_head) {
89b56047	533	mtk_cpu_dvfs_info_release(info);
d2499d05	534	list_del(&info->list_head);
89b56047	535	}
1453863f PCC	536
	537	return ret;
	538	}
	539
862e0104	540	static struct platform_driver mtk_cpufreq_platdrv = {
1453863f	541	.driver = {
862e0104	542	.name = "mtk-cpufreq",
1453863f	543	},
862e0104	544	.probe = mtk_cpufreq_probe,
1453863f PCC	545	};
1453863f PCC	546
cf9a2438	547	/* List of machines supported by this driver */
862e0104	548	static const struct of_device_id mtk_cpufreq_machines[] __initconst = {
501c574f	549	{ .compatible = "mediatek,mt2701", },
a9596dbc	550	{ .compatible = "mediatek,mt2712", },
ccc03d86	551	{ .compatible = "mediatek,mt7622", },
501c574f	552	{ .compatible = "mediatek,mt7623", },
de4ca309	553	{ .compatible = "mediatek,mt8167", },
cf9a2438 DK	554	{ .compatible = "mediatek,mt817x", },
	555	{ .compatible = "mediatek,mt8173", },
	556	{ .compatible = "mediatek,mt8176", },
9176b425	557	{ .compatible = "mediatek,mt8183", },
70d99a8f	558	{ .compatible = "mediatek,mt8365", },
683df830	559	{ .compatible = "mediatek,mt8516", },
cf9a2438 DK	560
	561	{ }
	562	};
af6eca06	563	MODULE_DEVICE_TABLE(of, mtk_cpufreq_machines);
cf9a2438	564
862e0104	565	static int __init mtk_cpufreq_driver_init(void)
1453863f	566	{
cf9a2438 DK	567	struct device_node *np;
cf9a2438 DK	568	const struct of_device_id *match;
1453863f PCC	569	struct platform_device *pdev;
	570	int err;
	571
cf9a2438 DK	572	np = of_find_node_by_path("/");
cf9a2438 DK	573	if (!np)
1453863f PCC	574	return -ENODEV;
1453863f PCC	575
862e0104	576	match = of_match_node(mtk_cpufreq_machines, np);
cf9a2438 DK	577	of_node_put(np);
cf9a2438 DK	578	if (!match) {
cb8bd2ff	579	pr_debug("Machine is not compatible with mtk-cpufreq\n");
cf9a2438 DK	580	return -ENODEV;
	581	}
	582
862e0104	583	err = platform_driver_register(&mtk_cpufreq_platdrv);
1453863f PCC	584	if (err)
	585	return err;
	586
	587	/*
	588	* Since there's no place to hold device registration code and no
	589	* device tree based way to match cpufreq driver yet, both the driver
	590	* and the device registration codes are put here to handle defer
	591	* probing.
	592	*/
862e0104	593	pdev = platform_device_register_simple("mtk-cpufreq", -1, NULL, 0);
1453863f PCC	594	if (IS_ERR(pdev)) {
1453863f PCC	595	pr_err("failed to register mtk-cpufreq platform device\n");
2f05c19d	596	platform_driver_unregister(&mtk_cpufreq_platdrv);
1453863f PCC	597	return PTR_ERR(pdev);
	598	}
	599
	600	return 0;
	601	}
b7070187 JWC	602	module_init(mtk_cpufreq_driver_init)
	603
	604	static void __exit mtk_cpufreq_driver_exit(void)
	605	{
	606	platform_driver_unregister(&mtk_cpufreq_platdrv);
	607	}
	608	module_exit(mtk_cpufreq_driver_exit)
7e8a09e0 JC	609
	610	MODULE_DESCRIPTION("MediaTek CPUFreq driver");
	611	MODULE_AUTHOR("Pi-Cheng Chen <pi-cheng.chen@linaro.org>");
	612	MODULE_LICENSE("GPL v2");