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 
mtk_ccifreq_set_voltage(struct mtk_ccifreq_drv * drv,int new_voltage)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(drv->proc_reg, new_voltage,
48 					    drv->soc_data->proc_max_volt);
49 		return ret;
50 	}
51 
52 	pre_voltage = regulator_get_voltage(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(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(drv->sram_reg, vsram,
72 						    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(drv->proc_reg, voltage,
83 						    soc_data->proc_max_volt);
84 			if (ret) {
85 				regulator_set_voltage(drv->sram_reg, pre_vsram,
86 						      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(drv->proc_reg, voltage,
93 						    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(drv->sram_reg, vsram,
104 						    soc_data->sram_max_volt);
105 			if (ret) {
106 				regulator_set_voltage(drv->proc_reg, pre_voltage,
107 						      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 
mtk_ccifreq_target(struct device * dev,unsigned long * freq,u32 flags)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(drv->cci_clk);
144 
145 	opp_rate = *freq;
146 	opp = devfreq_recommended_opp(dev, &opp_rate, 1);
147 	if (IS_ERR(opp)) {
148 		dev_err(dev, "failed to find opp for freq: %ld\n", opp_rate);
149 		ret = PTR_ERR(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(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, 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(drv->cci_clk, 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(cci_pll, *freq);
182 	if (ret) {
183 		dev_err(dev, "failed to set cci pll rate: %d\n", ret);
184 		clk_set_parent(drv->cci_clk, 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(drv->cci_clk, cci_pll);
190 	if (ret) {
191 		dev_err(dev, "failed to re-parent cci clock\n");
192 		mtk_ccifreq_set_voltage(drv, 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, 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(&drv->reg_lock);
210 
211 	return 0;
212 
213 out_restore_voltage:
214 	mtk_ccifreq_set_voltage(drv, pre_voltage);
215 
216 out_unlock:
217 	mutex_unlock(&drv->reg_lock);
218 	return ret;
219 }
220 
mtk_ccifreq_opp_notifier(struct notifier_block * nb,unsigned long event,void * data)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, volt);
238 		}
239 		mutex_unlock(&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 
mtk_ccifreq_probe(struct platform_device * pdev)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, 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(&pdev->dev);
265 	mutex_init(&drv->reg_lock);
266 	platform_set_drvdata(pdev, drv);
267 
268 	drv->cci_clk = devm_clk_get(dev, "cci");
269 	if (IS_ERR(drv->cci_clk)) {
270 		ret = PTR_ERR(drv->cci_clk);
271 		return dev_err_probe(dev, ret, "failed to get cci clk\n");
272 	}
273 
274 	drv->inter_clk = devm_clk_get(dev, "intermediate");
275 	if (IS_ERR(drv->inter_clk)) {
276 		ret = PTR_ERR(drv->inter_clk);
277 		return dev_err_probe(dev, ret,
278 				     "failed to get intermediate clk\n");
279 	}
280 
281 	drv->proc_reg = devm_regulator_get_optional(dev, "proc");
282 	if (IS_ERR(drv->proc_reg)) {
283 		ret = PTR_ERR(drv->proc_reg);
284 		return dev_err_probe(dev, ret,
285 				     "failed to get proc regulator\n");
286 	}
287 
288 	ret = regulator_enable(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, "sram");
295 	if (IS_ERR(drv->sram_reg)) {
296 		ret = PTR_ERR(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(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(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(drv->inter_clk);
329 	opp = dev_pm_opp_find_freq_ceil(dev, &rate);
330 	if (IS_ERR(opp)) {
331 		ret = PTR_ERR(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(drv->dev, &rate);
340 	if (IS_ERR(opp)) {
341 		dev_err(dev, "failed to get opp\n");
342 		ret = PTR_ERR(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, 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, 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, &mtk_ccifreq_profile,
363 					       DEVFREQ_GOV_PASSIVE,
364 					       passive_data);
365 	if (IS_ERR(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, &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(drv->cci_clk);
385 
386 out_free_resources:
387 	if (regulator_is_enabled(drv->proc_reg))
388 		regulator_disable(drv->proc_reg);
389 	if (drv->sram_reg && regulator_is_enabled(drv->sram_reg))
390 		regulator_disable(drv->sram_reg);
391 
392 	return ret;
393 }
394 
mtk_ccifreq_remove(struct platform_device * pdev)395 static void 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, &drv->opp_nb);
403 	dev_pm_opp_of_remove_table(dev);
404 	clk_disable_unprepare(drv->cci_clk);
405 	regulator_disable(drv->proc_reg);
406 	if (drv->sram_reg)
407 		regulator_disable(drv->sram_reg);
408 }
409 
410 static const struct mtk_ccifreq_platform_data mt8183_platform_data = {
411 	.min_volt_shift = 100000,
412 	.max_volt_shift = 200000,
413 	.proc_max_volt = 1150000,
414 };
415 
416 static const struct mtk_ccifreq_platform_data mt8186_platform_data = {
417 	.min_volt_shift = 100000,
418 	.max_volt_shift = 250000,
419 	.proc_max_volt = 1118750,
420 	.sram_min_volt = 850000,
421 	.sram_max_volt = 1118750,
422 };
423 
424 static const struct of_device_id mtk_ccifreq_machines[] = {
425 	{ .compatible = "mediatek,mt8183-cci", .data = &mt8183_platform_data },
426 	{ .compatible = "mediatek,mt8186-cci", .data = &mt8186_platform_data },
427 	{ },
428 };
429 MODULE_DEVICE_TABLE(of, mtk_ccifreq_machines);
430 
431 static struct platform_driver mtk_ccifreq_platdrv = {
432 	.probe	= mtk_ccifreq_probe,
433 	.remove_new = mtk_ccifreq_remove,
434 	.driver = {
435 		.name = "mtk-ccifreq",
436 		.of_match_table = mtk_ccifreq_machines,
437 	},
438 };
439 module_platform_driver(mtk_ccifreq_platdrv);
440 
441 MODULE_DESCRIPTION("MediaTek CCI devfreq driver");
442 MODULE_AUTHOR("Jia-Wei Chang <jia-wei.chang@mediatek.com>");
443 MODULE_LICENSE("GPL v2");
444