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 */
 28static const int opps_div[] __initconst = {1, 2, 3, 4};
 29
 30static struct platform_device *armada_8k_pdev;
 31
 32struct 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. */
 38static 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
 65static 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
 95static 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
123static 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");
131	if (!node || !of_device_is_available(node)) {
132		of_node_put(node);
133		return -ENODEV;
134	}
135	of_node_put(node);
136
137	nb_cpus = num_possible_cpus();
138	freq_tables = kcalloc(nb_cpus, sizeof(*freq_tables), GFP_KERNEL);
139	if (!freq_tables)
140		return -ENOMEM;
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
192remove_opp:
193	armada_8k_cpufreq_free_table(freq_tables);
194	return ret;
195}
196module_init(armada_8k_cpufreq_init);
197
198static 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}
205module_exit(armada_8k_cpufreq_exit);
206
207static const struct of_device_id __maybe_unused armada_8k_cpufreq_of_match[] = {
208	{ .compatible = "marvell,ap806-cpu-clock" },
209	{ },
210};
211MODULE_DEVICE_TABLE(of, armada_8k_cpufreq_of_match);
212
213MODULE_AUTHOR("Gregory Clement <gregory.clement@bootlin.com>");
214MODULE_DESCRIPTION("Armada 8K cpufreq driver");
215MODULE_LICENSE("GPL");