1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Copyright (c) 2020 MediaTek Inc. |
4 | */ |
5 | |
6 | #include <linux/bitfield.h> |
7 | #include <linux/cpufreq.h> |
8 | #include <linux/energy_model.h> |
9 | #include <linux/init.h> |
10 | #include <linux/iopoll.h> |
11 | #include <linux/kernel.h> |
12 | #include <linux/module.h> |
13 | #include <linux/of.h> |
14 | #include <linux/of_platform.h> |
15 | #include <linux/platform_device.h> |
16 | #include <linux/regulator/consumer.h> |
17 | #include <linux/slab.h> |
18 | |
19 | #define LUT_MAX_ENTRIES 32U |
20 | #define LUT_FREQ GENMASK(11, 0) |
21 | #define LUT_ROW_SIZE 0x4 |
22 | #define CPUFREQ_HW_STATUS BIT(0) |
23 | #define SVS_HW_STATUS BIT(1) |
24 | #define POLL_USEC 1000 |
25 | #define TIMEOUT_USEC 300000 |
26 | |
27 | enum { |
28 | REG_FREQ_LUT_TABLE, |
29 | REG_FREQ_ENABLE, |
30 | REG_FREQ_PERF_STATE, |
31 | REG_FREQ_HW_STATE, |
32 | REG_EM_POWER_TBL, |
33 | REG_FREQ_LATENCY, |
34 | |
35 | REG_ARRAY_SIZE, |
36 | }; |
37 | |
38 | struct mtk_cpufreq_data { |
39 | struct cpufreq_frequency_table *table; |
40 | void __iomem *reg_bases[REG_ARRAY_SIZE]; |
41 | struct resource *res; |
42 | void __iomem *base; |
43 | int nr_opp; |
44 | }; |
45 | |
46 | static const u16 cpufreq_mtk_offsets[REG_ARRAY_SIZE] = { |
47 | [REG_FREQ_LUT_TABLE] = 0x0, |
48 | [REG_FREQ_ENABLE] = 0x84, |
49 | [REG_FREQ_PERF_STATE] = 0x88, |
50 | [REG_FREQ_HW_STATE] = 0x8c, |
51 | [REG_EM_POWER_TBL] = 0x90, |
52 | [REG_FREQ_LATENCY] = 0x110, |
53 | }; |
54 | |
55 | static int __maybe_unused |
56 | mtk_cpufreq_get_cpu_power(struct device *cpu_dev, unsigned long *uW, |
57 | unsigned long *KHz) |
58 | { |
59 | struct mtk_cpufreq_data *data; |
60 | struct cpufreq_policy *policy; |
61 | int i; |
62 | |
63 | policy = cpufreq_cpu_get_raw(cpu: cpu_dev->id); |
64 | if (!policy) |
65 | return 0; |
66 | |
67 | data = policy->driver_data; |
68 | |
69 | for (i = 0; i < data->nr_opp; i++) { |
70 | if (data->table[i].frequency < *KHz) |
71 | break; |
72 | } |
73 | i--; |
74 | |
75 | *KHz = data->table[i].frequency; |
76 | /* Provide micro-Watts value to the Energy Model */ |
77 | *uW = readl_relaxed(data->reg_bases[REG_EM_POWER_TBL] + |
78 | i * LUT_ROW_SIZE); |
79 | |
80 | return 0; |
81 | } |
82 | |
83 | static int mtk_cpufreq_hw_target_index(struct cpufreq_policy *policy, |
84 | unsigned int index) |
85 | { |
86 | struct mtk_cpufreq_data *data = policy->driver_data; |
87 | |
88 | writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]); |
89 | |
90 | return 0; |
91 | } |
92 | |
93 | static unsigned int mtk_cpufreq_hw_get(unsigned int cpu) |
94 | { |
95 | struct mtk_cpufreq_data *data; |
96 | struct cpufreq_policy *policy; |
97 | unsigned int index; |
98 | |
99 | policy = cpufreq_cpu_get_raw(cpu); |
100 | if (!policy) |
101 | return 0; |
102 | |
103 | data = policy->driver_data; |
104 | |
105 | index = readl_relaxed(data->reg_bases[REG_FREQ_PERF_STATE]); |
106 | index = min(index, LUT_MAX_ENTRIES - 1); |
107 | |
108 | return data->table[index].frequency; |
109 | } |
110 | |
111 | static unsigned int mtk_cpufreq_hw_fast_switch(struct cpufreq_policy *policy, |
112 | unsigned int target_freq) |
113 | { |
114 | struct mtk_cpufreq_data *data = policy->driver_data; |
115 | unsigned int index; |
116 | |
117 | index = cpufreq_table_find_index_dl(policy, target_freq, efficiencies: false); |
118 | |
119 | writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]); |
120 | |
121 | return policy->freq_table[index].frequency; |
122 | } |
123 | |
124 | static int mtk_cpu_create_freq_table(struct platform_device *pdev, |
125 | struct mtk_cpufreq_data *data) |
126 | { |
127 | struct device *dev = &pdev->dev; |
128 | u32 temp, i, freq, prev_freq = 0; |
129 | void __iomem *base_table; |
130 | |
131 | data->table = devm_kcalloc(dev, LUT_MAX_ENTRIES + 1, |
132 | size: sizeof(*data->table), GFP_KERNEL); |
133 | if (!data->table) |
134 | return -ENOMEM; |
135 | |
136 | base_table = data->reg_bases[REG_FREQ_LUT_TABLE]; |
137 | |
138 | for (i = 0; i < LUT_MAX_ENTRIES; i++) { |
139 | temp = readl_relaxed(base_table + (i * LUT_ROW_SIZE)); |
140 | freq = FIELD_GET(LUT_FREQ, temp) * 1000; |
141 | |
142 | if (freq == prev_freq) |
143 | break; |
144 | |
145 | data->table[i].frequency = freq; |
146 | |
147 | dev_dbg(dev, "index=%d freq=%d\n" , i, data->table[i].frequency); |
148 | |
149 | prev_freq = freq; |
150 | } |
151 | |
152 | data->table[i].frequency = CPUFREQ_TABLE_END; |
153 | data->nr_opp = i; |
154 | |
155 | return 0; |
156 | } |
157 | |
158 | static int mtk_cpu_resources_init(struct platform_device *pdev, |
159 | struct cpufreq_policy *policy, |
160 | const u16 *offsets) |
161 | { |
162 | struct mtk_cpufreq_data *data; |
163 | struct device *dev = &pdev->dev; |
164 | struct resource *res; |
165 | struct of_phandle_args args; |
166 | void __iomem *base; |
167 | int ret, i; |
168 | int index; |
169 | |
170 | data = devm_kzalloc(dev, size: sizeof(*data), GFP_KERNEL); |
171 | if (!data) |
172 | return -ENOMEM; |
173 | |
174 | ret = of_perf_domain_get_sharing_cpumask(pcpu: policy->cpu, list_name: "performance-domains" , |
175 | cell_name: "#performance-domain-cells" , |
176 | cpumask: policy->cpus, pargs: &args); |
177 | if (ret < 0) |
178 | return ret; |
179 | |
180 | index = args.args[0]; |
181 | of_node_put(node: args.np); |
182 | |
183 | res = platform_get_resource(pdev, IORESOURCE_MEM, index); |
184 | if (!res) { |
185 | dev_err(dev, "failed to get mem resource %d\n" , index); |
186 | return -ENODEV; |
187 | } |
188 | |
189 | if (!request_mem_region(res->start, resource_size(res), res->name)) { |
190 | dev_err(dev, "failed to request resource %pR\n" , res); |
191 | return -EBUSY; |
192 | } |
193 | |
194 | base = ioremap(offset: res->start, size: resource_size(res)); |
195 | if (!base) { |
196 | dev_err(dev, "failed to map resource %pR\n" , res); |
197 | ret = -ENOMEM; |
198 | goto release_region; |
199 | } |
200 | |
201 | data->base = base; |
202 | data->res = res; |
203 | |
204 | for (i = REG_FREQ_LUT_TABLE; i < REG_ARRAY_SIZE; i++) |
205 | data->reg_bases[i] = base + offsets[i]; |
206 | |
207 | ret = mtk_cpu_create_freq_table(pdev, data); |
208 | if (ret) { |
209 | dev_info(dev, "Domain-%d failed to create freq table\n" , index); |
210 | return ret; |
211 | } |
212 | |
213 | policy->freq_table = data->table; |
214 | policy->driver_data = data; |
215 | |
216 | return 0; |
217 | release_region: |
218 | release_mem_region(res->start, resource_size(res)); |
219 | return ret; |
220 | } |
221 | |
222 | static int mtk_cpufreq_hw_cpu_init(struct cpufreq_policy *policy) |
223 | { |
224 | struct platform_device *pdev = cpufreq_get_driver_data(); |
225 | int sig, pwr_hw = CPUFREQ_HW_STATUS | SVS_HW_STATUS; |
226 | struct mtk_cpufreq_data *data; |
227 | unsigned int latency; |
228 | int ret; |
229 | |
230 | /* Get the bases of cpufreq for domains */ |
231 | ret = mtk_cpu_resources_init(pdev, policy, offsets: platform_get_drvdata(pdev)); |
232 | if (ret) { |
233 | dev_info(&pdev->dev, "CPUFreq resource init failed\n" ); |
234 | return ret; |
235 | } |
236 | |
237 | data = policy->driver_data; |
238 | |
239 | latency = readl_relaxed(data->reg_bases[REG_FREQ_LATENCY]) * 1000; |
240 | if (!latency) |
241 | latency = CPUFREQ_ETERNAL; |
242 | |
243 | policy->cpuinfo.transition_latency = latency; |
244 | policy->fast_switch_possible = true; |
245 | |
246 | /* HW should be in enabled state to proceed now */ |
247 | writel_relaxed(0x1, data->reg_bases[REG_FREQ_ENABLE]); |
248 | if (readl_poll_timeout(data->reg_bases[REG_FREQ_HW_STATE], sig, |
249 | (sig & pwr_hw) == pwr_hw, POLL_USEC, |
250 | TIMEOUT_USEC)) { |
251 | if (!(sig & CPUFREQ_HW_STATUS)) { |
252 | pr_info("cpufreq hardware of CPU%d is not enabled\n" , |
253 | policy->cpu); |
254 | return -ENODEV; |
255 | } |
256 | |
257 | pr_info("SVS of CPU%d is not enabled\n" , policy->cpu); |
258 | } |
259 | |
260 | return 0; |
261 | } |
262 | |
263 | static int mtk_cpufreq_hw_cpu_exit(struct cpufreq_policy *policy) |
264 | { |
265 | struct mtk_cpufreq_data *data = policy->driver_data; |
266 | struct resource *res = data->res; |
267 | void __iomem *base = data->base; |
268 | |
269 | /* HW should be in paused state now */ |
270 | writel_relaxed(0x0, data->reg_bases[REG_FREQ_ENABLE]); |
271 | iounmap(addr: base); |
272 | release_mem_region(res->start, resource_size(res)); |
273 | |
274 | return 0; |
275 | } |
276 | |
277 | static void mtk_cpufreq_register_em(struct cpufreq_policy *policy) |
278 | { |
279 | struct em_data_callback em_cb = EM_DATA_CB(mtk_cpufreq_get_cpu_power); |
280 | struct mtk_cpufreq_data *data = policy->driver_data; |
281 | |
282 | em_dev_register_perf_domain(dev: get_cpu_device(cpu: policy->cpu), nr_states: data->nr_opp, |
283 | cb: &em_cb, span: policy->cpus, microwatts: true); |
284 | } |
285 | |
286 | static struct cpufreq_driver cpufreq_mtk_hw_driver = { |
287 | .flags = CPUFREQ_NEED_INITIAL_FREQ_CHECK | |
288 | CPUFREQ_HAVE_GOVERNOR_PER_POLICY | |
289 | CPUFREQ_IS_COOLING_DEV, |
290 | .verify = cpufreq_generic_frequency_table_verify, |
291 | .target_index = mtk_cpufreq_hw_target_index, |
292 | .get = mtk_cpufreq_hw_get, |
293 | .init = mtk_cpufreq_hw_cpu_init, |
294 | .exit = mtk_cpufreq_hw_cpu_exit, |
295 | .register_em = mtk_cpufreq_register_em, |
296 | .fast_switch = mtk_cpufreq_hw_fast_switch, |
297 | .name = "mtk-cpufreq-hw" , |
298 | .attr = cpufreq_generic_attr, |
299 | }; |
300 | |
301 | static int mtk_cpufreq_hw_driver_probe(struct platform_device *pdev) |
302 | { |
303 | const void *data; |
304 | int ret, cpu; |
305 | struct device *cpu_dev; |
306 | struct regulator *cpu_reg; |
307 | |
308 | /* Make sure that all CPU supplies are available before proceeding. */ |
309 | for_each_possible_cpu(cpu) { |
310 | cpu_dev = get_cpu_device(cpu); |
311 | if (!cpu_dev) |
312 | return dev_err_probe(dev: &pdev->dev, err: -EPROBE_DEFER, |
313 | fmt: "Failed to get cpu%d device\n" , cpu); |
314 | |
315 | cpu_reg = devm_regulator_get(dev: cpu_dev, id: "cpu" ); |
316 | if (IS_ERR(ptr: cpu_reg)) |
317 | return dev_err_probe(dev: &pdev->dev, err: PTR_ERR(ptr: cpu_reg), |
318 | fmt: "CPU%d regulator get failed\n" , cpu); |
319 | } |
320 | |
321 | |
322 | data = of_device_get_match_data(dev: &pdev->dev); |
323 | if (!data) |
324 | return -EINVAL; |
325 | |
326 | platform_set_drvdata(pdev, data: (void *) data); |
327 | cpufreq_mtk_hw_driver.driver_data = pdev; |
328 | |
329 | ret = cpufreq_register_driver(driver_data: &cpufreq_mtk_hw_driver); |
330 | if (ret) |
331 | dev_err(&pdev->dev, "CPUFreq HW driver failed to register\n" ); |
332 | |
333 | return ret; |
334 | } |
335 | |
336 | static void mtk_cpufreq_hw_driver_remove(struct platform_device *pdev) |
337 | { |
338 | cpufreq_unregister_driver(driver_data: &cpufreq_mtk_hw_driver); |
339 | } |
340 | |
341 | static const struct of_device_id mtk_cpufreq_hw_match[] = { |
342 | { .compatible = "mediatek,cpufreq-hw" , .data = &cpufreq_mtk_offsets }, |
343 | {} |
344 | }; |
345 | MODULE_DEVICE_TABLE(of, mtk_cpufreq_hw_match); |
346 | |
347 | static struct platform_driver mtk_cpufreq_hw_driver = { |
348 | .probe = mtk_cpufreq_hw_driver_probe, |
349 | .remove_new = mtk_cpufreq_hw_driver_remove, |
350 | .driver = { |
351 | .name = "mtk-cpufreq-hw" , |
352 | .of_match_table = mtk_cpufreq_hw_match, |
353 | }, |
354 | }; |
355 | module_platform_driver(mtk_cpufreq_hw_driver); |
356 | |
357 | MODULE_AUTHOR("Hector Yuan <hector.yuan@mediatek.com>" ); |
358 | MODULE_DESCRIPTION("Mediatek cpufreq-hw driver" ); |
359 | MODULE_LICENSE("GPL v2" ); |
360 | |