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(dev: cpu_dev, id: 0); |
61 | if (IS_ERR(ptr: clk)) { |
62 | pr_warn("Cannot get clock for CPU %d\n" , cpu); |
63 | } else { |
64 | if (clk_is_match(p: clk, q: cur_clk)) |
65 | cpumask_set_cpu(cpu, dstp: 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(dev: cpu_dev, freq, u_volt: 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(dev: freq_tables[opps_index].cpu_dev, |
123 | freq: freq_tables[opps_index].freq[i]); |
124 | } |
125 | } |
126 | |
127 | kfree(objp: 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, matches: armada_8k_cpufreq_of_match, |
138 | NULL); |
139 | if (!node || !of_device_is_available(device: 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(n: nb_cpus, size: sizeof(*freq_tables), GFP_KERNEL); |
147 | if (!freq_tables) |
148 | return -ENOMEM; |
149 | cpumask_copy(dstp: &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(dev: cpu_dev, id: 0); |
169 | |
170 | if (IS_ERR(ptr: clk)) { |
171 | pr_err("Cannot get clock for CPU %d\n" , cpu); |
172 | ret = PTR_ERR(ptr: 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(dstp: &shared_cpus); |
184 | armada_8k_get_sharing_cpus(cur_clk: clk, cpumask: &shared_cpus); |
185 | dev_pm_opp_set_sharing_cpus(cpu_dev, cpumask: &shared_cpus); |
186 | cpumask_andnot(dstp: &cpus, src1p: &cpus, src2p: &shared_cpus); |
187 | clk_put(clk); |
188 | } |
189 | |
190 | armada_8k_pdev = platform_device_register_simple(name: "cpufreq-dt" , id: -1, |
191 | NULL, num: 0); |
192 | ret = PTR_ERR_OR_ZERO(ptr: armada_8k_pdev); |
193 | if (ret) |
194 | goto remove_opp; |
195 | |
196 | platform_set_drvdata(pdev: armada_8k_pdev, data: 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(pdev: 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 | |