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