1
2
3
4
5
6
7
8
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
26
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
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
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
103 if (!freq_tables[opps_index].cpu_dev)
104 break;
105
106 for (i = 0; i < ARRAY_SIZE(opps_div); i++) {
107
108
109
110
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
145
146
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");
216
