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
 32static unsigned long cpu_hw_rate, xo_rate;
 33static struct platform_device *global_pdev;
 34static bool icc_scaling_enabled;
 35
 36static 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
 57static 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
 77static 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
 93static 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
111static 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
128static 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
218static 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
241static 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;
310error:
311	devm_iounmap(dev, base);
312	return ret;
313}
314
315static 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
328static 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
334static 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
348static 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
388static int qcom_cpufreq_hw_driver_remove(struct platform_device *pdev)
389{
390	return cpufreq_unregister_driver(&cpufreq_qcom_hw_driver);
391}
392
393static const struct of_device_id qcom_cpufreq_hw_match[] = {
394	{ .compatible = "qcom,cpufreq-hw" },
395	{}
396};
397MODULE_DEVICE_TABLE(of, qcom_cpufreq_hw_match);
398
399static 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
408static int __init qcom_cpufreq_hw_init(void)
409{
410	return platform_driver_register(&qcom_cpufreq_hw_driver);
411}
412postcore_initcall(qcom_cpufreq_hw_init);
413
414static void __exit qcom_cpufreq_hw_exit(void)
415{
416	platform_driver_unregister(&qcom_cpufreq_hw_driver);
417}
418module_exit(qcom_cpufreq_hw_exit);
419
420MODULE_DESCRIPTION("QCOM CPUFREQ HW Driver");
421MODULE_LICENSE("GPL v2");