1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * Copyright (C) 2022 MediaTek Inc. |
4 | */ |
5 | |
6 | #include <linux/clk.h> |
7 | #include <linux/devfreq.h> |
8 | #include <linux/minmax.h> |
9 | #include <linux/module.h> |
10 | #include <linux/of.h> |
11 | #include <linux/platform_device.h> |
12 | #include <linux/pm_opp.h> |
13 | #include <linux/regulator/consumer.h> |
14 | |
15 | struct mtk_ccifreq_platform_data { |
16 | int min_volt_shift; |
17 | int max_volt_shift; |
18 | int proc_max_volt; |
19 | int sram_min_volt; |
20 | int sram_max_volt; |
21 | }; |
22 | |
23 | struct mtk_ccifreq_drv { |
24 | struct device *dev; |
25 | struct devfreq *devfreq; |
26 | struct regulator *proc_reg; |
27 | struct regulator *sram_reg; |
28 | struct clk *cci_clk; |
29 | struct clk *inter_clk; |
30 | int inter_voltage; |
31 | unsigned long pre_freq; |
32 | /* Avoid race condition for regulators between notify and policy */ |
33 | struct mutex reg_lock; |
34 | struct notifier_block opp_nb; |
35 | const struct mtk_ccifreq_platform_data *soc_data; |
36 | int vtrack_max; |
37 | }; |
38 | |
39 | static int mtk_ccifreq_set_voltage(struct mtk_ccifreq_drv *drv, int new_voltage) |
40 | { |
41 | const struct mtk_ccifreq_platform_data *soc_data = drv->soc_data; |
42 | struct device *dev = drv->dev; |
43 | int pre_voltage, pre_vsram, new_vsram, vsram, voltage, ret; |
44 | int retry_max = drv->vtrack_max; |
45 | |
46 | if (!drv->sram_reg) { |
47 | ret = regulator_set_voltage(regulator: drv->proc_reg, min_uV: new_voltage, |
48 | max_uV: drv->soc_data->proc_max_volt); |
49 | return ret; |
50 | } |
51 | |
52 | pre_voltage = regulator_get_voltage(regulator: drv->proc_reg); |
53 | if (pre_voltage < 0) { |
54 | dev_err(dev, "invalid vproc value: %d\n" , pre_voltage); |
55 | return pre_voltage; |
56 | } |
57 | |
58 | pre_vsram = regulator_get_voltage(regulator: drv->sram_reg); |
59 | if (pre_vsram < 0) { |
60 | dev_err(dev, "invalid vsram value: %d\n" , pre_vsram); |
61 | return pre_vsram; |
62 | } |
63 | |
64 | new_vsram = clamp(new_voltage + soc_data->min_volt_shift, |
65 | soc_data->sram_min_volt, soc_data->sram_max_volt); |
66 | |
67 | do { |
68 | if (pre_voltage <= new_voltage) { |
69 | vsram = clamp(pre_voltage + soc_data->max_volt_shift, |
70 | soc_data->sram_min_volt, new_vsram); |
71 | ret = regulator_set_voltage(regulator: drv->sram_reg, min_uV: vsram, |
72 | max_uV: soc_data->sram_max_volt); |
73 | if (ret) |
74 | return ret; |
75 | |
76 | if (vsram == soc_data->sram_max_volt || |
77 | new_vsram == soc_data->sram_min_volt) |
78 | voltage = new_voltage; |
79 | else |
80 | voltage = vsram - soc_data->min_volt_shift; |
81 | |
82 | ret = regulator_set_voltage(regulator: drv->proc_reg, min_uV: voltage, |
83 | max_uV: soc_data->proc_max_volt); |
84 | if (ret) { |
85 | regulator_set_voltage(regulator: drv->sram_reg, min_uV: pre_vsram, |
86 | max_uV: soc_data->sram_max_volt); |
87 | return ret; |
88 | } |
89 | } else if (pre_voltage > new_voltage) { |
90 | voltage = max(new_voltage, |
91 | pre_vsram - soc_data->max_volt_shift); |
92 | ret = regulator_set_voltage(regulator: drv->proc_reg, min_uV: voltage, |
93 | max_uV: soc_data->proc_max_volt); |
94 | if (ret) |
95 | return ret; |
96 | |
97 | if (voltage == new_voltage) |
98 | vsram = new_vsram; |
99 | else |
100 | vsram = max(new_vsram, |
101 | voltage + soc_data->min_volt_shift); |
102 | |
103 | ret = regulator_set_voltage(regulator: drv->sram_reg, min_uV: vsram, |
104 | max_uV: soc_data->sram_max_volt); |
105 | if (ret) { |
106 | regulator_set_voltage(regulator: drv->proc_reg, min_uV: pre_voltage, |
107 | max_uV: soc_data->proc_max_volt); |
108 | return ret; |
109 | } |
110 | } |
111 | |
112 | pre_voltage = voltage; |
113 | pre_vsram = vsram; |
114 | |
115 | if (--retry_max < 0) { |
116 | dev_err(dev, |
117 | "over loop count, failed to set voltage\n" ); |
118 | return -EINVAL; |
119 | } |
120 | } while (voltage != new_voltage || vsram != new_vsram); |
121 | |
122 | return 0; |
123 | } |
124 | |
125 | static int mtk_ccifreq_target(struct device *dev, unsigned long *freq, |
126 | u32 flags) |
127 | { |
128 | struct mtk_ccifreq_drv *drv = dev_get_drvdata(dev); |
129 | struct clk *cci_pll; |
130 | struct dev_pm_opp *opp; |
131 | unsigned long opp_rate; |
132 | int voltage, pre_voltage, inter_voltage, target_voltage, ret; |
133 | |
134 | if (!drv) |
135 | return -EINVAL; |
136 | |
137 | if (drv->pre_freq == *freq) |
138 | return 0; |
139 | |
140 | mutex_lock(&drv->reg_lock); |
141 | |
142 | inter_voltage = drv->inter_voltage; |
143 | cci_pll = clk_get_parent(clk: drv->cci_clk); |
144 | |
145 | opp_rate = *freq; |
146 | opp = devfreq_recommended_opp(dev, freq: &opp_rate, flags: 1); |
147 | if (IS_ERR(ptr: opp)) { |
148 | dev_err(dev, "failed to find opp for freq: %ld\n" , opp_rate); |
149 | ret = PTR_ERR(ptr: opp); |
150 | goto out_unlock; |
151 | } |
152 | |
153 | voltage = dev_pm_opp_get_voltage(opp); |
154 | dev_pm_opp_put(opp); |
155 | |
156 | pre_voltage = regulator_get_voltage(regulator: drv->proc_reg); |
157 | if (pre_voltage < 0) { |
158 | dev_err(dev, "invalid vproc value: %d\n" , pre_voltage); |
159 | ret = pre_voltage; |
160 | goto out_unlock; |
161 | } |
162 | |
163 | /* scale up: set voltage first then freq. */ |
164 | target_voltage = max(inter_voltage, voltage); |
165 | if (pre_voltage <= target_voltage) { |
166 | ret = mtk_ccifreq_set_voltage(drv, new_voltage: target_voltage); |
167 | if (ret) { |
168 | dev_err(dev, "failed to scale up voltage\n" ); |
169 | goto out_restore_voltage; |
170 | } |
171 | } |
172 | |
173 | /* switch the cci clock to intermediate clock source. */ |
174 | ret = clk_set_parent(clk: drv->cci_clk, parent: drv->inter_clk); |
175 | if (ret) { |
176 | dev_err(dev, "failed to re-parent cci clock\n" ); |
177 | goto out_restore_voltage; |
178 | } |
179 | |
180 | /* set the original clock to target rate. */ |
181 | ret = clk_set_rate(clk: cci_pll, rate: *freq); |
182 | if (ret) { |
183 | dev_err(dev, "failed to set cci pll rate: %d\n" , ret); |
184 | clk_set_parent(clk: drv->cci_clk, parent: cci_pll); |
185 | goto out_restore_voltage; |
186 | } |
187 | |
188 | /* switch the cci clock back to the original clock source. */ |
189 | ret = clk_set_parent(clk: drv->cci_clk, parent: cci_pll); |
190 | if (ret) { |
191 | dev_err(dev, "failed to re-parent cci clock\n" ); |
192 | mtk_ccifreq_set_voltage(drv, new_voltage: inter_voltage); |
193 | goto out_unlock; |
194 | } |
195 | |
196 | /* |
197 | * If the new voltage is lower than the intermediate voltage or the |
198 | * original voltage, scale down to the new voltage. |
199 | */ |
200 | if (voltage < inter_voltage || voltage < pre_voltage) { |
201 | ret = mtk_ccifreq_set_voltage(drv, new_voltage: voltage); |
202 | if (ret) { |
203 | dev_err(dev, "failed to scale down voltage\n" ); |
204 | goto out_unlock; |
205 | } |
206 | } |
207 | |
208 | drv->pre_freq = *freq; |
209 | mutex_unlock(lock: &drv->reg_lock); |
210 | |
211 | return 0; |
212 | |
213 | out_restore_voltage: |
214 | mtk_ccifreq_set_voltage(drv, new_voltage: pre_voltage); |
215 | |
216 | out_unlock: |
217 | mutex_unlock(lock: &drv->reg_lock); |
218 | return ret; |
219 | } |
220 | |
221 | static int mtk_ccifreq_opp_notifier(struct notifier_block *nb, |
222 | unsigned long event, void *data) |
223 | { |
224 | struct dev_pm_opp *opp = data; |
225 | struct mtk_ccifreq_drv *drv; |
226 | unsigned long freq, volt; |
227 | |
228 | drv = container_of(nb, struct mtk_ccifreq_drv, opp_nb); |
229 | |
230 | if (event == OPP_EVENT_ADJUST_VOLTAGE) { |
231 | mutex_lock(&drv->reg_lock); |
232 | freq = dev_pm_opp_get_freq(opp); |
233 | |
234 | /* current opp item is changed */ |
235 | if (freq == drv->pre_freq) { |
236 | volt = dev_pm_opp_get_voltage(opp); |
237 | mtk_ccifreq_set_voltage(drv, new_voltage: volt); |
238 | } |
239 | mutex_unlock(lock: &drv->reg_lock); |
240 | } |
241 | |
242 | return 0; |
243 | } |
244 | |
245 | static struct devfreq_dev_profile mtk_ccifreq_profile = { |
246 | .target = mtk_ccifreq_target, |
247 | }; |
248 | |
249 | static int mtk_ccifreq_probe(struct platform_device *pdev) |
250 | { |
251 | struct device *dev = &pdev->dev; |
252 | struct mtk_ccifreq_drv *drv; |
253 | struct devfreq_passive_data *passive_data; |
254 | struct dev_pm_opp *opp; |
255 | unsigned long rate, opp_volt; |
256 | int ret; |
257 | |
258 | drv = devm_kzalloc(dev, size: sizeof(*drv), GFP_KERNEL); |
259 | if (!drv) |
260 | return -ENOMEM; |
261 | |
262 | drv->dev = dev; |
263 | drv->soc_data = (const struct mtk_ccifreq_platform_data *) |
264 | of_device_get_match_data(dev: &pdev->dev); |
265 | mutex_init(&drv->reg_lock); |
266 | platform_set_drvdata(pdev, data: drv); |
267 | |
268 | drv->cci_clk = devm_clk_get(dev, id: "cci" ); |
269 | if (IS_ERR(ptr: drv->cci_clk)) { |
270 | ret = PTR_ERR(ptr: drv->cci_clk); |
271 | return dev_err_probe(dev, err: ret, fmt: "failed to get cci clk\n" ); |
272 | } |
273 | |
274 | drv->inter_clk = devm_clk_get(dev, id: "intermediate" ); |
275 | if (IS_ERR(ptr: drv->inter_clk)) { |
276 | ret = PTR_ERR(ptr: drv->inter_clk); |
277 | return dev_err_probe(dev, err: ret, |
278 | fmt: "failed to get intermediate clk\n" ); |
279 | } |
280 | |
281 | drv->proc_reg = devm_regulator_get_optional(dev, id: "proc" ); |
282 | if (IS_ERR(ptr: drv->proc_reg)) { |
283 | ret = PTR_ERR(ptr: drv->proc_reg); |
284 | return dev_err_probe(dev, err: ret, |
285 | fmt: "failed to get proc regulator\n" ); |
286 | } |
287 | |
288 | ret = regulator_enable(regulator: drv->proc_reg); |
289 | if (ret) { |
290 | dev_err(dev, "failed to enable proc regulator\n" ); |
291 | return ret; |
292 | } |
293 | |
294 | drv->sram_reg = devm_regulator_get_optional(dev, id: "sram" ); |
295 | if (IS_ERR(ptr: drv->sram_reg)) { |
296 | ret = PTR_ERR(ptr: drv->sram_reg); |
297 | if (ret == -EPROBE_DEFER) |
298 | goto out_free_resources; |
299 | |
300 | drv->sram_reg = NULL; |
301 | } else { |
302 | ret = regulator_enable(regulator: drv->sram_reg); |
303 | if (ret) { |
304 | dev_err(dev, "failed to enable sram regulator\n" ); |
305 | goto out_free_resources; |
306 | } |
307 | } |
308 | |
309 | /* |
310 | * We assume min voltage is 0 and tracking target voltage using |
311 | * min_volt_shift for each iteration. |
312 | * The retry_max is 3 times of expected iteration count. |
313 | */ |
314 | drv->vtrack_max = 3 * DIV_ROUND_UP(max(drv->soc_data->sram_max_volt, |
315 | drv->soc_data->proc_max_volt), |
316 | drv->soc_data->min_volt_shift); |
317 | |
318 | ret = clk_prepare_enable(clk: drv->cci_clk); |
319 | if (ret) |
320 | goto out_free_resources; |
321 | |
322 | ret = dev_pm_opp_of_add_table(dev); |
323 | if (ret) { |
324 | dev_err(dev, "failed to add opp table: %d\n" , ret); |
325 | goto out_disable_cci_clk; |
326 | } |
327 | |
328 | rate = clk_get_rate(clk: drv->inter_clk); |
329 | opp = dev_pm_opp_find_freq_ceil(dev, freq: &rate); |
330 | if (IS_ERR(ptr: opp)) { |
331 | ret = PTR_ERR(ptr: opp); |
332 | dev_err(dev, "failed to get intermediate opp: %d\n" , ret); |
333 | goto out_remove_opp_table; |
334 | } |
335 | drv->inter_voltage = dev_pm_opp_get_voltage(opp); |
336 | dev_pm_opp_put(opp); |
337 | |
338 | rate = U32_MAX; |
339 | opp = dev_pm_opp_find_freq_floor(dev: drv->dev, freq: &rate); |
340 | if (IS_ERR(ptr: opp)) { |
341 | dev_err(dev, "failed to get opp\n" ); |
342 | ret = PTR_ERR(ptr: opp); |
343 | goto out_remove_opp_table; |
344 | } |
345 | |
346 | opp_volt = dev_pm_opp_get_voltage(opp); |
347 | dev_pm_opp_put(opp); |
348 | ret = mtk_ccifreq_set_voltage(drv, new_voltage: opp_volt); |
349 | if (ret) { |
350 | dev_err(dev, "failed to scale to highest voltage %lu in proc_reg\n" , |
351 | opp_volt); |
352 | goto out_remove_opp_table; |
353 | } |
354 | |
355 | passive_data = devm_kzalloc(dev, size: sizeof(*passive_data), GFP_KERNEL); |
356 | if (!passive_data) { |
357 | ret = -ENOMEM; |
358 | goto out_remove_opp_table; |
359 | } |
360 | |
361 | passive_data->parent_type = CPUFREQ_PARENT_DEV; |
362 | drv->devfreq = devm_devfreq_add_device(dev, profile: &mtk_ccifreq_profile, |
363 | DEVFREQ_GOV_PASSIVE, |
364 | data: passive_data); |
365 | if (IS_ERR(ptr: drv->devfreq)) { |
366 | ret = -EPROBE_DEFER; |
367 | dev_err(dev, "failed to add devfreq device: %ld\n" , |
368 | PTR_ERR(drv->devfreq)); |
369 | goto out_remove_opp_table; |
370 | } |
371 | |
372 | drv->opp_nb.notifier_call = mtk_ccifreq_opp_notifier; |
373 | ret = dev_pm_opp_register_notifier(dev, nb: &drv->opp_nb); |
374 | if (ret) { |
375 | dev_err(dev, "failed to register opp notifier: %d\n" , ret); |
376 | goto out_remove_opp_table; |
377 | } |
378 | return 0; |
379 | |
380 | out_remove_opp_table: |
381 | dev_pm_opp_of_remove_table(dev); |
382 | |
383 | out_disable_cci_clk: |
384 | clk_disable_unprepare(clk: drv->cci_clk); |
385 | |
386 | out_free_resources: |
387 | if (regulator_is_enabled(regulator: drv->proc_reg)) |
388 | regulator_disable(regulator: drv->proc_reg); |
389 | if (drv->sram_reg && regulator_is_enabled(regulator: drv->sram_reg)) |
390 | regulator_disable(regulator: drv->sram_reg); |
391 | |
392 | return ret; |
393 | } |
394 | |
395 | static int mtk_ccifreq_remove(struct platform_device *pdev) |
396 | { |
397 | struct device *dev = &pdev->dev; |
398 | struct mtk_ccifreq_drv *drv; |
399 | |
400 | drv = platform_get_drvdata(pdev); |
401 | |
402 | dev_pm_opp_unregister_notifier(dev, nb: &drv->opp_nb); |
403 | dev_pm_opp_of_remove_table(dev); |
404 | clk_disable_unprepare(clk: drv->cci_clk); |
405 | regulator_disable(regulator: drv->proc_reg); |
406 | if (drv->sram_reg) |
407 | regulator_disable(regulator: drv->sram_reg); |
408 | |
409 | return 0; |
410 | } |
411 | |
412 | static const struct mtk_ccifreq_platform_data mt8183_platform_data = { |
413 | .min_volt_shift = 100000, |
414 | .max_volt_shift = 200000, |
415 | .proc_max_volt = 1150000, |
416 | }; |
417 | |
418 | static const struct mtk_ccifreq_platform_data mt8186_platform_data = { |
419 | .min_volt_shift = 100000, |
420 | .max_volt_shift = 250000, |
421 | .proc_max_volt = 1118750, |
422 | .sram_min_volt = 850000, |
423 | .sram_max_volt = 1118750, |
424 | }; |
425 | |
426 | static const struct of_device_id mtk_ccifreq_machines[] = { |
427 | { .compatible = "mediatek,mt8183-cci" , .data = &mt8183_platform_data }, |
428 | { .compatible = "mediatek,mt8186-cci" , .data = &mt8186_platform_data }, |
429 | { }, |
430 | }; |
431 | MODULE_DEVICE_TABLE(of, mtk_ccifreq_machines); |
432 | |
433 | static struct platform_driver mtk_ccifreq_platdrv = { |
434 | .probe = mtk_ccifreq_probe, |
435 | .remove = mtk_ccifreq_remove, |
436 | .driver = { |
437 | .name = "mtk-ccifreq" , |
438 | .of_match_table = mtk_ccifreq_machines, |
439 | }, |
440 | }; |
441 | module_platform_driver(mtk_ccifreq_platdrv); |
442 | |
443 | MODULE_DESCRIPTION("MediaTek CCI devfreq driver" ); |
444 | MODULE_AUTHOR("Jia-Wei Chang <jia-wei.chang@mediatek.com>" ); |
445 | MODULE_LICENSE("GPL v2" ); |
446 | |