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