[linux-block.git] / drivers / cpufreq / armada-8k-cpufreq.c

// SPDX-License-Identifier: GPL-2.0+
/*
 * CPUFreq support for Armada 8K
 *
 * Copyright (C) 2018 Marvell
 *
 * Omri Itach <omrii@marvell.com>
 * Gregory Clement <gregory.clement@bootlin.com>
 */

#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt

#include <linux/clk.h>
#include <linux/cpu.h>
#include <linux/err.h>
#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/platform_device.h>
#include <linux/pm_opp.h>
#include <linux/slab.h>

/*
 * Setup the opps list with the divider for the max frequency, that
 * will be filled at runtime.
 */
static const int opps_div[] __initconst = {1, 2, 3, 4};

static struct platform_device *armada_8k_pdev;

struct freq_table {
	struct device *cpu_dev;
	unsigned int freq[ARRAY_SIZE(opps_div)];
};

/* If the CPUs share the same clock, then they are in the same cluster. */
static void __init armada_8k_get_sharing_cpus(struct clk *cur_clk,
					      struct cpumask *cpumask)
{
	int cpu;

	for_each_possible_cpu(cpu) {
		struct device *cpu_dev;
		struct clk *clk;

		cpu_dev = get_cpu_device(cpu);
		if (!cpu_dev) {
			pr_warn("Failed to get cpu%d device\n", cpu);
			continue;
		}

		clk = clk_get(cpu_dev, 0);
		if (IS_ERR(clk)) {
			pr_warn("Cannot get clock for CPU %d\n", cpu);
		} else {
			if (clk_is_match(clk, cur_clk))
				cpumask_set_cpu(cpu, cpumask);

			clk_put(clk);
		}
	}
}

static int __init armada_8k_add_opp(struct clk *clk, struct device *cpu_dev,
				    struct freq_table *freq_tables,
				    int opps_index)
{
	unsigned int cur_frequency;
	unsigned int freq;
	int i, ret;

	/* Get nominal (current) CPU frequency. */
	cur_frequency = clk_get_rate(clk);
	if (!cur_frequency) {
		dev_err(cpu_dev, "Failed to get clock rate for this CPU\n");
		return -EINVAL;
	}

	freq_tables[opps_index].cpu_dev = cpu_dev;

	for (i = 0; i < ARRAY_SIZE(opps_div); i++) {
		freq = cur_frequency / opps_div[i];

		ret = dev_pm_opp_add(cpu_dev, freq, 0);
		if (ret)
			return ret;

		freq_tables[opps_index].freq[i] = freq;
	}

	return 0;
}

static void armada_8k_cpufreq_free_table(struct freq_table *freq_tables)
{
	int opps_index, nb_cpus = num_possible_cpus();

	for (opps_index = 0 ; opps_index <= nb_cpus; opps_index++) {
		int i;

		/* If cpu_dev is NULL then we reached the end of the array */
		if (!freq_tables[opps_index].cpu_dev)
			break;

		for (i = 0; i < ARRAY_SIZE(opps_div); i++) {
			/*
			 * A 0Hz frequency is not valid, this meant
			 * that it was not yet initialized so there is
			 * no more opp to free
			 */
			if (freq_tables[opps_index].freq[i] == 0)
				break;

			dev_pm_opp_remove(freq_tables[opps_index].cpu_dev,
					  freq_tables[opps_index].freq[i]);
		}
	}

	kfree(freq_tables);
}

static int __init armada_8k_cpufreq_init(void)
{
	int ret = 0, opps_index = 0, cpu, nb_cpus;
	struct freq_table *freq_tables;
	struct device_node *node;
	struct cpumask cpus;

	node = of_find_compatible_node(NULL, NULL, "marvell,ap806-cpu-clock");
	if (!node || !of_device_is_available(node)) {
		of_node_put(node);
		return -ENODEV;
	}
	of_node_put(node);

	nb_cpus = num_possible_cpus();
	freq_tables = kcalloc(nb_cpus, sizeof(*freq_tables), GFP_KERNEL);
	if (!freq_tables)
		return -ENOMEM;
	cpumask_copy(&cpus, cpu_possible_mask);

	/*
	 * For each CPU, this loop registers the operating points
	 * supported (which are the nominal CPU frequency and full integer
	 * divisions of it).
	 */
	for_each_cpu(cpu, &cpus) {
		struct cpumask shared_cpus;
		struct device *cpu_dev;
		struct clk *clk;

		cpu_dev = get_cpu_device(cpu);

		if (!cpu_dev) {
			pr_err("Cannot get CPU %d\n", cpu);
			continue;
		}

		clk = clk_get(cpu_dev, 0);

		if (IS_ERR(clk)) {
			pr_err("Cannot get clock for CPU %d\n", cpu);
			ret = PTR_ERR(clk);
			goto remove_opp;
		}

		ret = armada_8k_add_opp(clk, cpu_dev, freq_tables, opps_index);
		if (ret) {
			clk_put(clk);
			goto remove_opp;
		}

		opps_index++;
		cpumask_clear(&shared_cpus);
		armada_8k_get_sharing_cpus(clk, &shared_cpus);
		dev_pm_opp_set_sharing_cpus(cpu_dev, &shared_cpus);
		cpumask_andnot(&cpus, &cpus, &shared_cpus);
		clk_put(clk);
	}

	armada_8k_pdev = platform_device_register_simple("cpufreq-dt", -1,
							 NULL, 0);
	ret = PTR_ERR_OR_ZERO(armada_8k_pdev);
	if (ret)
		goto remove_opp;

	platform_set_drvdata(armada_8k_pdev, freq_tables);

	return 0;

remove_opp:
	armada_8k_cpufreq_free_table(freq_tables);
	return ret;
}
module_init(armada_8k_cpufreq_init);

static void __exit armada_8k_cpufreq_exit(void)
{
	struct freq_table *freq_tables = platform_get_drvdata(armada_8k_pdev);

	platform_device_unregister(armada_8k_pdev);
	armada_8k_cpufreq_free_table(freq_tables);
}
module_exit(armada_8k_cpufreq_exit);

static const struct of_device_id __maybe_unused armada_8k_cpufreq_of_match[] = {
	{ .compatible = "marvell,ap806-cpu-clock" },
	{ },
};
MODULE_DEVICE_TABLE(of, armada_8k_cpufreq_of_match);

MODULE_AUTHOR("Gregory Clement <gregory.clement@bootlin.com>");
MODULE_DESCRIPTION("Armada 8K cpufreq driver");
MODULE_LICENSE("GPL");
Commit	Line	Data
f525a670 GC	1	// SPDX-License-Identifier: GPL-2.0+
	2	/*
	3	* CPUFreq support for Armada 8K
	4	*
	5	* Copyright (C) 2018 Marvell
	6	*
	7	* Omri Itach <omrii@marvell.com>
	8	* Gregory Clement <gregory.clement@bootlin.com>
	9	*/
	10
	11	#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
	12
	13	#include <linux/clk.h>
	14	#include <linux/cpu.h>
	15	#include <linux/err.h>
	16	#include <linux/init.h>
	17	#include <linux/kernel.h>
	18	#include <linux/module.h>
	19	#include <linux/of.h>
	20	#include <linux/platform_device.h>
	21	#include <linux/pm_opp.h>
	22	#include <linux/slab.h>
	23
	24	/*
	25	* Setup the opps list with the divider for the max frequency, that
	26	* will be filled at runtime.
	27	*/
	28	static const int opps_div[] __initconst = {1, 2, 3, 4};
	29
	30	static struct platform_device *armada_8k_pdev;
	31
	32	struct freq_table {
	33	struct device *cpu_dev;
	34	unsigned int freq[ARRAY_SIZE(opps_div)];
	35	};
	36
	37	/* If the CPUs share the same clock, then they are in the same cluster. */
	38	static void __init armada_8k_get_sharing_cpus(struct clk *cur_clk,
	39	struct cpumask *cpumask)
	40	{
	41	int cpu;
	42
	43	for_each_possible_cpu(cpu) {
	44	struct device *cpu_dev;
	45	struct clk *clk;
	46
	47	cpu_dev = get_cpu_device(cpu);
	48	if (!cpu_dev) {
	49	pr_warn("Failed to get cpu%d device\n", cpu);
	50	continue;
	51	}
	52
	53	clk = clk_get(cpu_dev, 0);
	54	if (IS_ERR(clk)) {
	55	pr_warn("Cannot get clock for CPU %d\n", cpu);
	56	} else {
	57	if (clk_is_match(clk, cur_clk))
	58	cpumask_set_cpu(cpu, cpumask);
	59
	60	clk_put(clk);
	61	}
	62	}
	63	}
	64
65	static int __init armada_8k_add_opp(struct clk clk, struct device cpu_dev,
66	struct freq_table *freq_tables,
67	int opps_index)
68	{
69	unsigned int cur_frequency;
70	unsigned int freq;
71	int i, ret;
72
73	/* Get nominal (current) CPU frequency. */
74	cur_frequency = clk_get_rate(clk);
75	if (!cur_frequency) {
76	dev_err(cpu_dev, "Failed to get clock rate for this CPU\n");
77	return -EINVAL;
78	}
79
80	freq_tables[opps_index].cpu_dev = cpu_dev;
81
82	for (i = 0; i < ARRAY_SIZE(opps_div); i++) {
83	freq = cur_frequency / opps_div[i];
84
85	ret = dev_pm_opp_add(cpu_dev, freq, 0);
86	if (ret)
87	return ret;
88
89	freq_tables[opps_index].freq[i] = freq;
90	}
91
92	return 0;
93	}
94
95	static void armada_8k_cpufreq_free_table(struct freq_table *freq_tables)
96	{
97	int opps_index, nb_cpus = num_possible_cpus();
98
99	for (opps_index = 0 ; opps_index <= nb_cpus; opps_index++) {
100	int i;
101
102	/* If cpu_dev is NULL then we reached the end of the array */
103	if (!freq_tables[opps_index].cpu_dev)
104	break;
105
106	for (i = 0; i < ARRAY_SIZE(opps_div); i++) {
107	/*
108	* A 0Hz frequency is not valid, this meant
109	* that it was not yet initialized so there is
110	* no more opp to free
111	*/
112	if (freq_tables[opps_index].freq[i] == 0)
113	break;
114
115	dev_pm_opp_remove(freq_tables[opps_index].cpu_dev,
116	freq_tables[opps_index].freq[i]);
117	}
118	}
119
120	kfree(freq_tables);
121	}
122
123	static int __init armada_8k_cpufreq_init(void)
124	{
125	int ret = 0, opps_index = 0, cpu, nb_cpus;
126	struct freq_table *freq_tables;
127	struct device_node *node;
128	struct cpumask cpus;
129
130	node = of_find_compatible_node(NULL, NULL, "marvell,ap806-cpu-clock");
d3c1e33f JL	131	if (!node \|\| !of_device_is_available(node)) {
d3c1e33f JL	132	of_node_put(node);
f525a670	133	return -ENODEV;
d3c1e33f	134	}
b623fa32	135	of_node_put(node);
f525a670 GC	136
	137	nb_cpus = num_possible_cpus();
	138	freq_tables = kcalloc(nb_cpus, sizeof(*freq_tables), GFP_KERNEL);
3355c91b HK	139	if (!freq_tables)
3355c91b HK	140	return -ENOMEM;
f525a670 GC	141	cpumask_copy(&cpus, cpu_possible_mask);
	142
	143	/*
	144	* For each CPU, this loop registers the operating points
	145	* supported (which are the nominal CPU frequency and full integer
	146	* divisions of it).
	147	*/
	148	for_each_cpu(cpu, &cpus) {
	149	struct cpumask shared_cpus;
	150	struct device *cpu_dev;
	151	struct clk *clk;
	152
	153	cpu_dev = get_cpu_device(cpu);
	154
	155	if (!cpu_dev) {
	156	pr_err("Cannot get CPU %d\n", cpu);
	157	continue;
	158	}
	159
	160	clk = clk_get(cpu_dev, 0);
	161
	162	if (IS_ERR(clk)) {
	163	pr_err("Cannot get clock for CPU %d\n", cpu);
	164	ret = PTR_ERR(clk);
	165	goto remove_opp;
	166	}
	167
	168	ret = armada_8k_add_opp(clk, cpu_dev, freq_tables, opps_index);
	169	if (ret) {
	170	clk_put(clk);
	171	goto remove_opp;
	172	}
	173
	174	opps_index++;
	175	cpumask_clear(&shared_cpus);
	176	armada_8k_get_sharing_cpus(clk, &shared_cpus);
	177	dev_pm_opp_set_sharing_cpus(cpu_dev, &shared_cpus);
	178	cpumask_andnot(&cpus, &cpus, &shared_cpus);
	179	clk_put(clk);
	180	}
	181
	182	armada_8k_pdev = platform_device_register_simple("cpufreq-dt", -1,
	183	NULL, 0);
	184	ret = PTR_ERR_OR_ZERO(armada_8k_pdev);
	185	if (ret)
	186	goto remove_opp;
	187
	188	platform_set_drvdata(armada_8k_pdev, freq_tables);
	189
	190	return 0;
	191
	192	remove_opp:
	193	armada_8k_cpufreq_free_table(freq_tables);
	194	return ret;
	195	}
	196	module_init(armada_8k_cpufreq_init);
	197
	198	static void __exit armada_8k_cpufreq_exit(void)
	199	{
	200	struct freq_table *freq_tables = platform_get_drvdata(armada_8k_pdev);
	201
	202	platform_device_unregister(armada_8k_pdev);
	203	armada_8k_cpufreq_free_table(freq_tables);
	204	}
205	module_exit(armada_8k_cpufreq_exit);
206
925a5bce PR	207	static const struct of_device_id __maybe_unused armada_8k_cpufreq_of_match[] = {
	208	{ .compatible = "marvell,ap806-cpu-clock" },
	209	{ },
	210	};
	211	MODULE_DEVICE_TABLE(of, armada_8k_cpufreq_of_match);
	212
f525a670 GC	213	MODULE_AUTHOR("Gregory Clement <gregory.clement@bootlin.com>");
	214	MODULE_DESCRIPTION("Armada 8K cpufreq driver");
	215	MODULE_LICENSE("GPL");