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 static 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 };
29 MODULE_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  */
35 static const int opps_div[] __initconst = {1, 2, 3, 4};
36 
37 static struct platform_device *armada_8k_pdev;
38 
39 struct 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. */
45 static 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, 0);
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 
72 static 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 
102 static 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 
130 static 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, 0);
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 
200 remove_opp:
201 	armada_8k_cpufreq_free_table(freq_tables);
202 	return ret;
203 }
204 module_init(armada_8k_cpufreq_init);
205 
206 static 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 }
213 module_exit(armada_8k_cpufreq_exit);
214 
215 MODULE_AUTHOR("Gregory Clement <gregory.clement@bootlin.com>");
216 MODULE_DESCRIPTION("Armada 8K cpufreq driver");
217 MODULE_LICENSE("GPL");
218