1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (c) 2018, The Linux Foundation. All rights reserved.
4  */
5 
6 #include <linux/bitfield.h>
7 #include <linux/cpufreq.h>
8 #include <linux/init.h>
9 #include <linux/interconnect.h>
10 #include <linux/kernel.h>
11 #include <linux/module.h>
12 #include <linux/of_address.h>
13 #include <linux/of_platform.h>
14 #include <linux/pm_opp.h>
15 #include <linux/slab.h>
16 
17 #define LUT_MAX_ENTRIES			40U
18 #define LUT_SRC				GENMASK(31, 30)
19 #define LUT_L_VAL			GENMASK(7, 0)
20 #define LUT_CORE_COUNT			GENMASK(18, 16)
21 #define LUT_VOLT			GENMASK(11, 0)
22 #define LUT_ROW_SIZE			32
23 #define CLK_HW_DIV			2
24 #define LUT_TURBO_IND			1
25 
26 /* Register offsets */
27 #define REG_ENABLE			0x0
28 #define REG_FREQ_LUT			0x110
29 #define REG_VOLT_LUT			0x114
30 #define REG_PERF_STATE			0x920
31 
32 static unsigned long cpu_hw_rate, xo_rate;
33 static struct platform_device *global_pdev;
34 static bool icc_scaling_enabled;
35 
36 static int qcom_cpufreq_set_bw(struct cpufreq_policy *policy,
37 			       unsigned long freq_khz)
38 {
39 	unsigned long freq_hz = freq_khz * 1000;
40 	struct dev_pm_opp *opp;
41 	struct device *dev;
42 	int ret;
43 
44 	dev = get_cpu_device(policy->cpu);
45 	if (!dev)
46 		return -ENODEV;
47 
48 	opp = dev_pm_opp_find_freq_exact(dev, freq_hz, true);
49 	if (IS_ERR(opp))
50 		return PTR_ERR(opp);
51 
52 	ret = dev_pm_opp_set_bw(dev, opp);
53 	dev_pm_opp_put(opp);
54 	return ret;
55 }
56 
57 static int qcom_cpufreq_update_opp(struct device *cpu_dev,
58 				   unsigned long freq_khz,
59 				   unsigned long volt)
60 {
61 	unsigned long freq_hz = freq_khz * 1000;
62 	int ret;
63 
64 	/* Skip voltage update if the opp table is not available */
65 	if (!icc_scaling_enabled)
66 		return dev_pm_opp_add(cpu_dev, freq_hz, volt);
67 
68 	ret = dev_pm_opp_adjust_voltage(cpu_dev, freq_hz, volt, volt, volt);
69 	if (ret) {
70 		dev_err(cpu_dev, "Voltage update failed freq=%ld\n", freq_khz);
71 		return ret;
72 	}
73 
74 	return dev_pm_opp_enable(cpu_dev, freq_hz);
75 }
76 
77 static int qcom_cpufreq_hw_target_index(struct cpufreq_policy *policy,
78 					unsigned int index)
79 {
80 	void __iomem *perf_state_reg = policy->driver_data;
81 	unsigned long freq = policy->freq_table[index].frequency;
82 
83 	writel_relaxed(index, perf_state_reg);
84 
85 	if (icc_scaling_enabled)
86 		qcom_cpufreq_set_bw(policy, freq);
87 
88 	arch_set_freq_scale(policy->related_cpus, freq,
89 			    policy->cpuinfo.max_freq);
90 	return 0;
91 }
92 
93 static unsigned int qcom_cpufreq_hw_get(unsigned int cpu)
94 {
95 	void __iomem *perf_state_reg;
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 	perf_state_reg = policy->driver_data;
104 
105 	index = readl_relaxed(perf_state_reg);
106 	index = min(index, LUT_MAX_ENTRIES - 1);
107 
108 	return policy->freq_table[index].frequency;
109 }
110 
111 static unsigned int qcom_cpufreq_hw_fast_switch(struct cpufreq_policy *policy,
112 						unsigned int target_freq)
113 {
114 	void __iomem *perf_state_reg = policy->driver_data;
115 	unsigned int index;
116 	unsigned long freq;
117 
118 	index = policy->cached_resolved_idx;
119 	writel_relaxed(index, perf_state_reg);
120 
121 	freq = policy->freq_table[index].frequency;
122 	arch_set_freq_scale(policy->related_cpus, freq,
123 			    policy->cpuinfo.max_freq);
124 
125 	return freq;
126 }
127 
128 static int qcom_cpufreq_hw_read_lut(struct device *cpu_dev,
129 				    struct cpufreq_policy *policy,
130 				    void __iomem *base)
131 {
132 	u32 data, src, lval, i, core_count, prev_freq = 0, freq;
133 	u32 volt;
134 	struct cpufreq_frequency_table	*table;
135 	struct dev_pm_opp *opp;
136 	unsigned long rate;
137 	int ret;
138 
139 	table = kcalloc(LUT_MAX_ENTRIES + 1, sizeof(*table), GFP_KERNEL);
140 	if (!table)
141 		return -ENOMEM;
142 
143 	ret = dev_pm_opp_of_add_table(cpu_dev);
144 	if (!ret) {
145 		/* Disable all opps and cross-validate against LUT later */
146 		icc_scaling_enabled = true;
147 		for (rate = 0; ; rate++) {
148 			opp = dev_pm_opp_find_freq_ceil(cpu_dev, &rate);
149 			if (IS_ERR(opp))
150 				break;
151 
152 			dev_pm_opp_put(opp);
153 			dev_pm_opp_disable(cpu_dev, rate);
154 		}
155 	} else if (ret != -ENODEV) {
156 		dev_err(cpu_dev, "Invalid opp table in device tree\n");
157 		return ret;
158 	} else {
159 		policy->fast_switch_possible = true;
160 		icc_scaling_enabled = false;
161 	}
162 
163 	for (i = 0; i < LUT_MAX_ENTRIES; i++) {
164 		data = readl_relaxed(base + REG_FREQ_LUT +
165 				      i * LUT_ROW_SIZE);
166 		src = FIELD_GET(LUT_SRC, data);
167 		lval = FIELD_GET(LUT_L_VAL, data);
168 		core_count = FIELD_GET(LUT_CORE_COUNT, data);
169 
170 		data = readl_relaxed(base + REG_VOLT_LUT +
171 				      i * LUT_ROW_SIZE);
172 		volt = FIELD_GET(LUT_VOLT, data) * 1000;
173 
174 		if (src)
175 			freq = xo_rate * lval / 1000;
176 		else
177 			freq = cpu_hw_rate / 1000;
178 
179 		if (freq != prev_freq && core_count != LUT_TURBO_IND) {
180 			table[i].frequency = freq;
181 			qcom_cpufreq_update_opp(cpu_dev, freq, volt);
182 			dev_dbg(cpu_dev, "index=%d freq=%d, core_count %d\n", i,
183 				freq, core_count);
184 		} else if (core_count == LUT_TURBO_IND) {
185 			table[i].frequency = CPUFREQ_ENTRY_INVALID;
186 		}
187 
188 		/*
189 		 * Two of the same frequencies with the same core counts means
190 		 * end of table
191 		 */
192 		if (i > 0 && prev_freq == freq) {
193 			struct cpufreq_frequency_table *prev = &table[i - 1];
194 
195 			/*
196 			 * Only treat the last frequency that might be a boost
197 			 * as the boost frequency
198 			 */
199 			if (prev->frequency == CPUFREQ_ENTRY_INVALID) {
200 				prev->frequency = prev_freq;
201 				prev->flags = CPUFREQ_BOOST_FREQ;
202 				qcom_cpufreq_update_opp(cpu_dev, prev_freq, volt);
203 			}
204 
205 			break;
206 		}
207 
208 		prev_freq = freq;
209 	}
210 
211 	table[i].frequency = CPUFREQ_TABLE_END;
212 	policy->freq_table = table;
213 	dev_pm_opp_set_sharing_cpus(cpu_dev, policy->cpus);
214 
215 	return 0;
216 }
217 
218 static void qcom_get_related_cpus(int index, struct cpumask *m)
219 {
220 	struct device_node *cpu_np;
221 	struct of_phandle_args args;
222 	int cpu, ret;
223 
224 	for_each_possible_cpu(cpu) {
225 		cpu_np = of_cpu_device_node_get(cpu);
226 		if (!cpu_np)
227 			continue;
228 
229 		ret = of_parse_phandle_with_args(cpu_np, "qcom,freq-domain",
230 						 "#freq-domain-cells", 0,
231 						 &args);
232 		of_node_put(cpu_np);
233 		if (ret < 0)
234 			continue;
235 
236 		if (index == args.args[0])
237 			cpumask_set_cpu(cpu, m);
238 	}
239 }
240 
241 static int qcom_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
242 {
243 	struct device *dev = &global_pdev->dev;
244 	struct of_phandle_args args;
245 	struct device_node *cpu_np;
246 	struct device *cpu_dev;
247 	struct resource *res;
248 	void __iomem *base;
249 	int ret, index;
250 
251 	cpu_dev = get_cpu_device(policy->cpu);
252 	if (!cpu_dev) {
253 		pr_err("%s: failed to get cpu%d device\n", __func__,
254 		       policy->cpu);
255 		return -ENODEV;
256 	}
257 
258 	cpu_np = of_cpu_device_node_get(policy->cpu);
259 	if (!cpu_np)
260 		return -EINVAL;
261 
262 	ret = of_parse_phandle_with_args(cpu_np, "qcom,freq-domain",
263 					 "#freq-domain-cells", 0, &args);
264 	of_node_put(cpu_np);
265 	if (ret)
266 		return ret;
267 
268 	index = args.args[0];
269 
270 	res = platform_get_resource(global_pdev, IORESOURCE_MEM, index);
271 	if (!res)
272 		return -ENODEV;
273 
274 	base = devm_ioremap(dev, res->start, resource_size(res));
275 	if (!base)
276 		return -ENOMEM;
277 
278 	/* HW should be in enabled state to proceed */
279 	if (!(readl_relaxed(base + REG_ENABLE) & 0x1)) {
280 		dev_err(dev, "Domain-%d cpufreq hardware not enabled\n", index);
281 		ret = -ENODEV;
282 		goto error;
283 	}
284 
285 	qcom_get_related_cpus(index, policy->cpus);
286 	if (!cpumask_weight(policy->cpus)) {
287 		dev_err(dev, "Domain-%d failed to get related CPUs\n", index);
288 		ret = -ENOENT;
289 		goto error;
290 	}
291 
292 	policy->driver_data = base + REG_PERF_STATE;
293 
294 	ret = qcom_cpufreq_hw_read_lut(cpu_dev, policy, base);
295 	if (ret) {
296 		dev_err(dev, "Domain-%d failed to read LUT\n", index);
297 		goto error;
298 	}
299 
300 	ret = dev_pm_opp_get_opp_count(cpu_dev);
301 	if (ret <= 0) {
302 		dev_err(cpu_dev, "Failed to add OPPs\n");
303 		ret = -ENODEV;
304 		goto error;
305 	}
306 
307 	dev_pm_opp_of_register_em(cpu_dev, policy->cpus);
308 
309 	return 0;
310 error:
311 	devm_iounmap(dev, base);
312 	return ret;
313 }
314 
315 static int qcom_cpufreq_hw_cpu_exit(struct cpufreq_policy *policy)
316 {
317 	struct device *cpu_dev = get_cpu_device(policy->cpu);
318 	void __iomem *base = policy->driver_data - REG_PERF_STATE;
319 
320 	dev_pm_opp_remove_all_dynamic(cpu_dev);
321 	dev_pm_opp_of_cpumask_remove_table(policy->related_cpus);
322 	kfree(policy->freq_table);
323 	devm_iounmap(&global_pdev->dev, base);
324 
325 	return 0;
326 }
327 
328 static struct freq_attr *qcom_cpufreq_hw_attr[] = {
329 	&cpufreq_freq_attr_scaling_available_freqs,
330 	&cpufreq_freq_attr_scaling_boost_freqs,
331 	NULL
332 };
333 
334 static struct cpufreq_driver cpufreq_qcom_hw_driver = {
335 	.flags		= CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK |
336 			  CPUFREQ_HAVE_GOVERNOR_PER_POLICY |
337 			  CPUFREQ_IS_COOLING_DEV,
338 	.verify		= cpufreq_generic_frequency_table_verify,
339 	.target_index	= qcom_cpufreq_hw_target_index,
340 	.get		= qcom_cpufreq_hw_get,
341 	.init		= qcom_cpufreq_hw_cpu_init,
342 	.exit		= qcom_cpufreq_hw_cpu_exit,
343 	.fast_switch    = qcom_cpufreq_hw_fast_switch,
344 	.name		= "qcom-cpufreq-hw",
345 	.attr		= qcom_cpufreq_hw_attr,
346 };
347 
348 static int qcom_cpufreq_hw_driver_probe(struct platform_device *pdev)
349 {
350 	struct device *cpu_dev;
351 	struct clk *clk;
352 	int ret;
353 
354 	clk = clk_get(&pdev->dev, "xo");
355 	if (IS_ERR(clk))
356 		return PTR_ERR(clk);
357 
358 	xo_rate = clk_get_rate(clk);
359 	clk_put(clk);
360 
361 	clk = clk_get(&pdev->dev, "alternate");
362 	if (IS_ERR(clk))
363 		return PTR_ERR(clk);
364 
365 	cpu_hw_rate = clk_get_rate(clk) / CLK_HW_DIV;
366 	clk_put(clk);
367 
368 	global_pdev = pdev;
369 
370 	/* Check for optional interconnect paths on CPU0 */
371 	cpu_dev = get_cpu_device(0);
372 	if (!cpu_dev)
373 		return -EPROBE_DEFER;
374 
375 	ret = dev_pm_opp_of_find_icc_paths(cpu_dev, NULL);
376 	if (ret)
377 		return ret;
378 
379 	ret = cpufreq_register_driver(&cpufreq_qcom_hw_driver);
380 	if (ret)
381 		dev_err(&pdev->dev, "CPUFreq HW driver failed to register\n");
382 	else
383 		dev_dbg(&pdev->dev, "QCOM CPUFreq HW driver initialized\n");
384 
385 	return ret;
386 }
387 
388 static int qcom_cpufreq_hw_driver_remove(struct platform_device *pdev)
389 {
390 	return cpufreq_unregister_driver(&cpufreq_qcom_hw_driver);
391 }
392 
393 static const struct of_device_id qcom_cpufreq_hw_match[] = {
394 	{ .compatible = "qcom,cpufreq-hw" },
395 	{}
396 };
397 MODULE_DEVICE_TABLE(of, qcom_cpufreq_hw_match);
398 
399 static struct platform_driver qcom_cpufreq_hw_driver = {
400 	.probe = qcom_cpufreq_hw_driver_probe,
401 	.remove = qcom_cpufreq_hw_driver_remove,
402 	.driver = {
403 		.name = "qcom-cpufreq-hw",
404 		.of_match_table = qcom_cpufreq_hw_match,
405 	},
406 };
407 
408 static int __init qcom_cpufreq_hw_init(void)
409 {
410 	return platform_driver_register(&qcom_cpufreq_hw_driver);
411 }
412 postcore_initcall(qcom_cpufreq_hw_init);
413 
414 static void __exit qcom_cpufreq_hw_exit(void)
415 {
416 	platform_driver_unregister(&qcom_cpufreq_hw_driver);
417 }
418 module_exit(qcom_cpufreq_hw_exit);
419 
420 MODULE_DESCRIPTION("QCOM CPUFREQ HW Driver");
421 MODULE_LICENSE("GPL v2");
422