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/clk-provider.h>
8 #include <linux/cpufreq.h>
9 #include <linux/init.h>
10 #include <linux/interconnect.h>
11 #include <linux/interrupt.h>
12 #include <linux/kernel.h>
13 #include <linux/module.h>
14 #include <linux/of_address.h>
15 #include <linux/of_platform.h>
16 #include <linux/pm_opp.h>
17 #include <linux/pm_qos.h>
18 #include <linux/slab.h>
19 #include <linux/spinlock.h>
20 #include <linux/units.h>
21 
22 #define LUT_MAX_ENTRIES			40U
23 #define LUT_SRC				GENMASK(31, 30)
24 #define LUT_L_VAL			GENMASK(7, 0)
25 #define LUT_CORE_COUNT			GENMASK(18, 16)
26 #define LUT_VOLT			GENMASK(11, 0)
27 #define CLK_HW_DIV			2
28 #define LUT_TURBO_IND			1
29 
30 #define GT_IRQ_STATUS			BIT(2)
31 
32 struct qcom_cpufreq_soc_data {
33 	u32 reg_enable;
34 	u32 reg_domain_state;
35 	u32 reg_dcvs_ctrl;
36 	u32 reg_freq_lut;
37 	u32 reg_volt_lut;
38 	u32 reg_intr_clr;
39 	u32 reg_current_vote;
40 	u32 reg_perf_state;
41 	u8 lut_row_size;
42 };
43 
44 struct qcom_cpufreq_data {
45 	void __iomem *base;
46 	struct resource *res;
47 
48 	/*
49 	 * Mutex to synchronize between de-init sequence and re-starting LMh
50 	 * polling/interrupts
51 	 */
52 	struct mutex throttle_lock;
53 	int throttle_irq;
54 	char irq_name[15];
55 	bool cancel_throttle;
56 	struct delayed_work throttle_work;
57 	struct cpufreq_policy *policy;
58 	struct clk_hw cpu_clk;
59 
60 	bool per_core_dcvs;
61 
62 	struct freq_qos_request throttle_freq_req;
63 };
64 
65 static struct {
66 	struct qcom_cpufreq_data *data;
67 	const struct qcom_cpufreq_soc_data *soc_data;
68 } qcom_cpufreq;
69 
70 static unsigned long cpu_hw_rate, xo_rate;
71 static bool icc_scaling_enabled;
72 
73 static int qcom_cpufreq_set_bw(struct cpufreq_policy *policy,
74 			       unsigned long freq_khz)
75 {
76 	unsigned long freq_hz = freq_khz * 1000;
77 	struct dev_pm_opp *opp;
78 	struct device *dev;
79 	int ret;
80 
81 	dev = get_cpu_device(policy->cpu);
82 	if (!dev)
83 		return -ENODEV;
84 
85 	opp = dev_pm_opp_find_freq_exact(dev, freq_hz, true);
86 	if (IS_ERR(opp))
87 		return PTR_ERR(opp);
88 
89 	ret = dev_pm_opp_set_opp(dev, opp);
90 	dev_pm_opp_put(opp);
91 	return ret;
92 }
93 
94 static int qcom_cpufreq_update_opp(struct device *cpu_dev,
95 				   unsigned long freq_khz,
96 				   unsigned long volt)
97 {
98 	unsigned long freq_hz = freq_khz * 1000;
99 	int ret;
100 
101 	/* Skip voltage update if the opp table is not available */
102 	if (!icc_scaling_enabled)
103 		return dev_pm_opp_add(cpu_dev, freq_hz, volt);
104 
105 	ret = dev_pm_opp_adjust_voltage(cpu_dev, freq_hz, volt, volt, volt);
106 	if (ret) {
107 		dev_err(cpu_dev, "Voltage update failed freq=%ld\n", freq_khz);
108 		return ret;
109 	}
110 
111 	return dev_pm_opp_enable(cpu_dev, freq_hz);
112 }
113 
114 static int qcom_cpufreq_hw_target_index(struct cpufreq_policy *policy,
115 					unsigned int index)
116 {
117 	struct qcom_cpufreq_data *data = policy->driver_data;
118 	const struct qcom_cpufreq_soc_data *soc_data = qcom_cpufreq.soc_data;
119 	unsigned long freq = policy->freq_table[index].frequency;
120 	unsigned int i;
121 
122 	writel_relaxed(index, data->base + soc_data->reg_perf_state);
123 
124 	if (data->per_core_dcvs)
125 		for (i = 1; i < cpumask_weight(policy->related_cpus); i++)
126 			writel_relaxed(index, data->base + soc_data->reg_perf_state + i * 4);
127 
128 	if (icc_scaling_enabled)
129 		qcom_cpufreq_set_bw(policy, freq);
130 
131 	return 0;
132 }
133 
134 static unsigned long qcom_lmh_get_throttle_freq(struct qcom_cpufreq_data *data)
135 {
136 	unsigned int lval;
137 
138 	if (qcom_cpufreq.soc_data->reg_current_vote)
139 		lval = readl_relaxed(data->base + qcom_cpufreq.soc_data->reg_current_vote) & 0x3ff;
140 	else
141 		lval = readl_relaxed(data->base + qcom_cpufreq.soc_data->reg_domain_state) & 0xff;
142 
143 	return lval * xo_rate;
144 }
145 
146 /* Get the current frequency of the CPU (after throttling) */
147 static unsigned int qcom_cpufreq_hw_get(unsigned int cpu)
148 {
149 	struct qcom_cpufreq_data *data;
150 	struct cpufreq_policy *policy;
151 
152 	policy = cpufreq_cpu_get_raw(cpu);
153 	if (!policy)
154 		return 0;
155 
156 	data = policy->driver_data;
157 
158 	return qcom_lmh_get_throttle_freq(data) / HZ_PER_KHZ;
159 }
160 
161 /* Get the frequency requested by the cpufreq core for the CPU */
162 static unsigned int qcom_cpufreq_get_freq(unsigned int cpu)
163 {
164 	struct qcom_cpufreq_data *data;
165 	const struct qcom_cpufreq_soc_data *soc_data;
166 	struct cpufreq_policy *policy;
167 	unsigned int index;
168 
169 	policy = cpufreq_cpu_get_raw(cpu);
170 	if (!policy)
171 		return 0;
172 
173 	data = policy->driver_data;
174 	soc_data = qcom_cpufreq.soc_data;
175 
176 	index = readl_relaxed(data->base + soc_data->reg_perf_state);
177 	index = min(index, LUT_MAX_ENTRIES - 1);
178 
179 	return policy->freq_table[index].frequency;
180 }
181 
182 static unsigned int qcom_cpufreq_hw_fast_switch(struct cpufreq_policy *policy,
183 						unsigned int target_freq)
184 {
185 	struct qcom_cpufreq_data *data = policy->driver_data;
186 	const struct qcom_cpufreq_soc_data *soc_data = qcom_cpufreq.soc_data;
187 	unsigned int index;
188 	unsigned int i;
189 
190 	index = policy->cached_resolved_idx;
191 	writel_relaxed(index, data->base + soc_data->reg_perf_state);
192 
193 	if (data->per_core_dcvs)
194 		for (i = 1; i < cpumask_weight(policy->related_cpus); i++)
195 			writel_relaxed(index, data->base + soc_data->reg_perf_state + i * 4);
196 
197 	return policy->freq_table[index].frequency;
198 }
199 
200 static int qcom_cpufreq_hw_read_lut(struct device *cpu_dev,
201 				    struct cpufreq_policy *policy)
202 {
203 	u32 data, src, lval, i, core_count, prev_freq = 0, freq;
204 	u32 volt;
205 	struct cpufreq_frequency_table	*table;
206 	struct dev_pm_opp *opp;
207 	unsigned long rate;
208 	int ret;
209 	struct qcom_cpufreq_data *drv_data = policy->driver_data;
210 	const struct qcom_cpufreq_soc_data *soc_data = qcom_cpufreq.soc_data;
211 
212 	table = kcalloc(LUT_MAX_ENTRIES + 1, sizeof(*table), GFP_KERNEL);
213 	if (!table)
214 		return -ENOMEM;
215 
216 	ret = dev_pm_opp_of_add_table(cpu_dev);
217 	if (!ret) {
218 		/* Disable all opps and cross-validate against LUT later */
219 		icc_scaling_enabled = true;
220 		for (rate = 0; ; rate++) {
221 			opp = dev_pm_opp_find_freq_ceil(cpu_dev, &rate);
222 			if (IS_ERR(opp))
223 				break;
224 
225 			dev_pm_opp_put(opp);
226 			dev_pm_opp_disable(cpu_dev, rate);
227 		}
228 	} else if (ret != -ENODEV) {
229 		dev_err(cpu_dev, "Invalid opp table in device tree\n");
230 		kfree(table);
231 		return ret;
232 	} else {
233 		policy->fast_switch_possible = true;
234 		icc_scaling_enabled = false;
235 	}
236 
237 	for (i = 0; i < LUT_MAX_ENTRIES; i++) {
238 		data = readl_relaxed(drv_data->base + soc_data->reg_freq_lut +
239 				      i * soc_data->lut_row_size);
240 		src = FIELD_GET(LUT_SRC, data);
241 		lval = FIELD_GET(LUT_L_VAL, data);
242 		core_count = FIELD_GET(LUT_CORE_COUNT, data);
243 
244 		data = readl_relaxed(drv_data->base + soc_data->reg_volt_lut +
245 				      i * soc_data->lut_row_size);
246 		volt = FIELD_GET(LUT_VOLT, data) * 1000;
247 
248 		if (src)
249 			freq = xo_rate * lval / 1000;
250 		else
251 			freq = cpu_hw_rate / 1000;
252 
253 		if (freq != prev_freq && core_count != LUT_TURBO_IND) {
254 			if (!qcom_cpufreq_update_opp(cpu_dev, freq, volt)) {
255 				table[i].frequency = freq;
256 				dev_dbg(cpu_dev, "index=%d freq=%d, core_count %d\n", i,
257 				freq, core_count);
258 			} else {
259 				dev_warn(cpu_dev, "failed to update OPP for freq=%d\n", freq);
260 				table[i].frequency = CPUFREQ_ENTRY_INVALID;
261 			}
262 
263 		} else if (core_count == LUT_TURBO_IND) {
264 			table[i].frequency = CPUFREQ_ENTRY_INVALID;
265 		}
266 
267 		/*
268 		 * Two of the same frequencies with the same core counts means
269 		 * end of table
270 		 */
271 		if (i > 0 && prev_freq == freq) {
272 			struct cpufreq_frequency_table *prev = &table[i - 1];
273 
274 			/*
275 			 * Only treat the last frequency that might be a boost
276 			 * as the boost frequency
277 			 */
278 			if (prev->frequency == CPUFREQ_ENTRY_INVALID) {
279 				if (!qcom_cpufreq_update_opp(cpu_dev, prev_freq, volt)) {
280 					prev->frequency = prev_freq;
281 					prev->flags = CPUFREQ_BOOST_FREQ;
282 				} else {
283 					dev_warn(cpu_dev, "failed to update OPP for freq=%d\n",
284 						 freq);
285 				}
286 			}
287 
288 			break;
289 		}
290 
291 		prev_freq = freq;
292 	}
293 
294 	table[i].frequency = CPUFREQ_TABLE_END;
295 	policy->freq_table = table;
296 	dev_pm_opp_set_sharing_cpus(cpu_dev, policy->cpus);
297 
298 	return 0;
299 }
300 
301 static void qcom_get_related_cpus(int index, struct cpumask *m)
302 {
303 	struct device_node *cpu_np;
304 	struct of_phandle_args args;
305 	int cpu, ret;
306 
307 	for_each_possible_cpu(cpu) {
308 		cpu_np = of_cpu_device_node_get(cpu);
309 		if (!cpu_np)
310 			continue;
311 
312 		ret = of_parse_phandle_with_args(cpu_np, "qcom,freq-domain",
313 						 "#freq-domain-cells", 0,
314 						 &args);
315 		of_node_put(cpu_np);
316 		if (ret < 0)
317 			continue;
318 
319 		if (index == args.args[0])
320 			cpumask_set_cpu(cpu, m);
321 	}
322 }
323 
324 static void qcom_lmh_dcvs_notify(struct qcom_cpufreq_data *data)
325 {
326 	struct cpufreq_policy *policy = data->policy;
327 	int cpu = cpumask_first(policy->related_cpus);
328 	struct device *dev = get_cpu_device(cpu);
329 	unsigned long freq_hz, throttled_freq;
330 	struct dev_pm_opp *opp;
331 
332 	/*
333 	 * Get the h/w throttled frequency, normalize it using the
334 	 * registered opp table and use it to calculate thermal pressure.
335 	 */
336 	freq_hz = qcom_lmh_get_throttle_freq(data);
337 
338 	opp = dev_pm_opp_find_freq_floor(dev, &freq_hz);
339 	if (IS_ERR(opp) && PTR_ERR(opp) == -ERANGE)
340 		opp = dev_pm_opp_find_freq_ceil(dev, &freq_hz);
341 
342 	if (IS_ERR(opp)) {
343 		dev_warn(dev, "Can't find the OPP for throttling: %pe!\n", opp);
344 	} else {
345 		dev_pm_opp_put(opp);
346 	}
347 
348 	throttled_freq = freq_hz / HZ_PER_KHZ;
349 
350 	freq_qos_update_request(&data->throttle_freq_req, throttled_freq);
351 
352 	/* Update thermal pressure (the boost frequencies are accepted) */
353 	arch_update_thermal_pressure(policy->related_cpus, throttled_freq);
354 
355 	/*
356 	 * In the unlikely case policy is unregistered do not enable
357 	 * polling or h/w interrupt
358 	 */
359 	mutex_lock(&data->throttle_lock);
360 	if (data->cancel_throttle)
361 		goto out;
362 
363 	/*
364 	 * If h/w throttled frequency is higher than what cpufreq has requested
365 	 * for, then stop polling and switch back to interrupt mechanism.
366 	 */
367 	if (throttled_freq >= qcom_cpufreq_get_freq(cpu))
368 		enable_irq(data->throttle_irq);
369 	else
370 		mod_delayed_work(system_highpri_wq, &data->throttle_work,
371 				 msecs_to_jiffies(10));
372 
373 out:
374 	mutex_unlock(&data->throttle_lock);
375 }
376 
377 static void qcom_lmh_dcvs_poll(struct work_struct *work)
378 {
379 	struct qcom_cpufreq_data *data;
380 
381 	data = container_of(work, struct qcom_cpufreq_data, throttle_work.work);
382 	qcom_lmh_dcvs_notify(data);
383 }
384 
385 static irqreturn_t qcom_lmh_dcvs_handle_irq(int irq, void *data)
386 {
387 	struct qcom_cpufreq_data *c_data = data;
388 
389 	/* Disable interrupt and enable polling */
390 	disable_irq_nosync(c_data->throttle_irq);
391 	schedule_delayed_work(&c_data->throttle_work, 0);
392 
393 	if (qcom_cpufreq.soc_data->reg_intr_clr)
394 		writel_relaxed(GT_IRQ_STATUS,
395 			       c_data->base + qcom_cpufreq.soc_data->reg_intr_clr);
396 
397 	return IRQ_HANDLED;
398 }
399 
400 static const struct qcom_cpufreq_soc_data qcom_soc_data = {
401 	.reg_enable = 0x0,
402 	.reg_dcvs_ctrl = 0xbc,
403 	.reg_freq_lut = 0x110,
404 	.reg_volt_lut = 0x114,
405 	.reg_current_vote = 0x704,
406 	.reg_perf_state = 0x920,
407 	.lut_row_size = 32,
408 };
409 
410 static const struct qcom_cpufreq_soc_data epss_soc_data = {
411 	.reg_enable = 0x0,
412 	.reg_domain_state = 0x20,
413 	.reg_dcvs_ctrl = 0xb0,
414 	.reg_freq_lut = 0x100,
415 	.reg_volt_lut = 0x200,
416 	.reg_intr_clr = 0x308,
417 	.reg_perf_state = 0x320,
418 	.lut_row_size = 4,
419 };
420 
421 static const struct of_device_id qcom_cpufreq_hw_match[] = {
422 	{ .compatible = "qcom,cpufreq-hw", .data = &qcom_soc_data },
423 	{ .compatible = "qcom,cpufreq-epss", .data = &epss_soc_data },
424 	{}
425 };
426 MODULE_DEVICE_TABLE(of, qcom_cpufreq_hw_match);
427 
428 static int qcom_cpufreq_hw_lmh_init(struct cpufreq_policy *policy, int index)
429 {
430 	struct qcom_cpufreq_data *data = policy->driver_data;
431 	struct platform_device *pdev = cpufreq_get_driver_data();
432 	int ret;
433 
434 	/*
435 	 * Look for LMh interrupt. If no interrupt line is specified /
436 	 * if there is an error, allow cpufreq to be enabled as usual.
437 	 */
438 	data->throttle_irq = platform_get_irq_optional(pdev, index);
439 	if (data->throttle_irq == -ENXIO)
440 		return 0;
441 	if (data->throttle_irq < 0)
442 		return data->throttle_irq;
443 
444 	ret = freq_qos_add_request(&policy->constraints,
445 				   &data->throttle_freq_req, FREQ_QOS_MAX,
446 				   FREQ_QOS_MAX_DEFAULT_VALUE);
447 	if (ret < 0) {
448 		dev_err(&pdev->dev, "Failed to add freq constraint (%d)\n", ret);
449 		return ret;
450 	}
451 
452 	data->cancel_throttle = false;
453 	data->policy = policy;
454 
455 	mutex_init(&data->throttle_lock);
456 	INIT_DEFERRABLE_WORK(&data->throttle_work, qcom_lmh_dcvs_poll);
457 
458 	snprintf(data->irq_name, sizeof(data->irq_name), "dcvsh-irq-%u", policy->cpu);
459 	ret = request_threaded_irq(data->throttle_irq, NULL, qcom_lmh_dcvs_handle_irq,
460 				   IRQF_ONESHOT | IRQF_NO_AUTOEN, data->irq_name, data);
461 	if (ret) {
462 		dev_err(&pdev->dev, "Error registering %s: %d\n", data->irq_name, ret);
463 		return 0;
464 	}
465 
466 	ret = irq_set_affinity_and_hint(data->throttle_irq, policy->cpus);
467 	if (ret)
468 		dev_err(&pdev->dev, "Failed to set CPU affinity of %s[%d]\n",
469 			data->irq_name, data->throttle_irq);
470 
471 	return 0;
472 }
473 
474 static int qcom_cpufreq_hw_cpu_online(struct cpufreq_policy *policy)
475 {
476 	struct qcom_cpufreq_data *data = policy->driver_data;
477 	struct platform_device *pdev = cpufreq_get_driver_data();
478 	int ret;
479 
480 	if (data->throttle_irq <= 0)
481 		return 0;
482 
483 	mutex_lock(&data->throttle_lock);
484 	data->cancel_throttle = false;
485 	mutex_unlock(&data->throttle_lock);
486 
487 	ret = irq_set_affinity_and_hint(data->throttle_irq, policy->cpus);
488 	if (ret)
489 		dev_err(&pdev->dev, "Failed to set CPU affinity of %s[%d]\n",
490 			data->irq_name, data->throttle_irq);
491 
492 	return ret;
493 }
494 
495 static int qcom_cpufreq_hw_cpu_offline(struct cpufreq_policy *policy)
496 {
497 	struct qcom_cpufreq_data *data = policy->driver_data;
498 
499 	if (data->throttle_irq <= 0)
500 		return 0;
501 
502 	mutex_lock(&data->throttle_lock);
503 	data->cancel_throttle = true;
504 	mutex_unlock(&data->throttle_lock);
505 
506 	cancel_delayed_work_sync(&data->throttle_work);
507 	irq_set_affinity_and_hint(data->throttle_irq, NULL);
508 	disable_irq_nosync(data->throttle_irq);
509 
510 	return 0;
511 }
512 
513 static void qcom_cpufreq_hw_lmh_exit(struct qcom_cpufreq_data *data)
514 {
515 	if (data->throttle_irq <= 0)
516 		return;
517 
518 	freq_qos_remove_request(&data->throttle_freq_req);
519 	free_irq(data->throttle_irq, data);
520 }
521 
522 static int qcom_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
523 {
524 	struct platform_device *pdev = cpufreq_get_driver_data();
525 	struct device *dev = &pdev->dev;
526 	struct of_phandle_args args;
527 	struct device_node *cpu_np;
528 	struct device *cpu_dev;
529 	struct qcom_cpufreq_data *data;
530 	int ret, index;
531 
532 	cpu_dev = get_cpu_device(policy->cpu);
533 	if (!cpu_dev) {
534 		pr_err("%s: failed to get cpu%d device\n", __func__,
535 		       policy->cpu);
536 		return -ENODEV;
537 	}
538 
539 	cpu_np = of_cpu_device_node_get(policy->cpu);
540 	if (!cpu_np)
541 		return -EINVAL;
542 
543 	ret = of_parse_phandle_with_args(cpu_np, "qcom,freq-domain",
544 					 "#freq-domain-cells", 0, &args);
545 	of_node_put(cpu_np);
546 	if (ret)
547 		return ret;
548 
549 	index = args.args[0];
550 	data = &qcom_cpufreq.data[index];
551 
552 	/* HW should be in enabled state to proceed */
553 	if (!(readl_relaxed(data->base + qcom_cpufreq.soc_data->reg_enable) & 0x1)) {
554 		dev_err(dev, "Domain-%d cpufreq hardware not enabled\n", index);
555 		return -ENODEV;
556 	}
557 
558 	if (readl_relaxed(data->base + qcom_cpufreq.soc_data->reg_dcvs_ctrl) & 0x1)
559 		data->per_core_dcvs = true;
560 
561 	qcom_get_related_cpus(index, policy->cpus);
562 
563 	policy->driver_data = data;
564 	policy->dvfs_possible_from_any_cpu = true;
565 
566 	ret = qcom_cpufreq_hw_read_lut(cpu_dev, policy);
567 	if (ret) {
568 		dev_err(dev, "Domain-%d failed to read LUT\n", index);
569 		return ret;
570 	}
571 
572 	ret = dev_pm_opp_get_opp_count(cpu_dev);
573 	if (ret <= 0) {
574 		dev_err(cpu_dev, "Failed to add OPPs\n");
575 		return -ENODEV;
576 	}
577 
578 	if (policy_has_boost_freq(policy)) {
579 		ret = cpufreq_enable_boost_support();
580 		if (ret)
581 			dev_warn(cpu_dev, "failed to enable boost: %d\n", ret);
582 	}
583 
584 	return qcom_cpufreq_hw_lmh_init(policy, index);
585 }
586 
587 static int qcom_cpufreq_hw_cpu_exit(struct cpufreq_policy *policy)
588 {
589 	struct device *cpu_dev = get_cpu_device(policy->cpu);
590 	struct qcom_cpufreq_data *data = policy->driver_data;
591 	struct resource *res = data->res;
592 	void __iomem *base = data->base;
593 
594 	dev_pm_opp_remove_all_dynamic(cpu_dev);
595 	dev_pm_opp_of_cpumask_remove_table(policy->related_cpus);
596 	qcom_cpufreq_hw_lmh_exit(data);
597 	kfree(policy->freq_table);
598 	kfree(data);
599 	iounmap(base);
600 	release_mem_region(res->start, resource_size(res));
601 
602 	return 0;
603 }
604 
605 static void qcom_cpufreq_ready(struct cpufreq_policy *policy)
606 {
607 	struct qcom_cpufreq_data *data = policy->driver_data;
608 
609 	if (data->throttle_irq >= 0)
610 		enable_irq(data->throttle_irq);
611 }
612 
613 static struct freq_attr *qcom_cpufreq_hw_attr[] = {
614 	&cpufreq_freq_attr_scaling_available_freqs,
615 	&cpufreq_freq_attr_scaling_boost_freqs,
616 	NULL
617 };
618 
619 static struct cpufreq_driver cpufreq_qcom_hw_driver = {
620 	.flags		= CPUFREQ_NEED_INITIAL_FREQ_CHECK |
621 			  CPUFREQ_HAVE_GOVERNOR_PER_POLICY |
622 			  CPUFREQ_IS_COOLING_DEV,
623 	.verify		= cpufreq_generic_frequency_table_verify,
624 	.target_index	= qcom_cpufreq_hw_target_index,
625 	.get		= qcom_cpufreq_hw_get,
626 	.init		= qcom_cpufreq_hw_cpu_init,
627 	.exit		= qcom_cpufreq_hw_cpu_exit,
628 	.online		= qcom_cpufreq_hw_cpu_online,
629 	.offline	= qcom_cpufreq_hw_cpu_offline,
630 	.register_em	= cpufreq_register_em_with_opp,
631 	.fast_switch    = qcom_cpufreq_hw_fast_switch,
632 	.name		= "qcom-cpufreq-hw",
633 	.attr		= qcom_cpufreq_hw_attr,
634 	.ready		= qcom_cpufreq_ready,
635 };
636 
637 static unsigned long qcom_cpufreq_hw_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
638 {
639 	struct qcom_cpufreq_data *data = container_of(hw, struct qcom_cpufreq_data, cpu_clk);
640 
641 	return qcom_lmh_get_throttle_freq(data);
642 }
643 
644 static const struct clk_ops qcom_cpufreq_hw_clk_ops = {
645 	.recalc_rate = qcom_cpufreq_hw_recalc_rate,
646 };
647 
648 static int qcom_cpufreq_hw_driver_probe(struct platform_device *pdev)
649 {
650 	struct clk_hw_onecell_data *clk_data;
651 	struct device *dev = &pdev->dev;
652 	struct device_node *soc_node;
653 	struct device *cpu_dev;
654 	struct clk *clk;
655 	int ret, i, num_domains, reg_sz;
656 
657 	clk = clk_get(dev, "xo");
658 	if (IS_ERR(clk))
659 		return PTR_ERR(clk);
660 
661 	xo_rate = clk_get_rate(clk);
662 	clk_put(clk);
663 
664 	clk = clk_get(dev, "alternate");
665 	if (IS_ERR(clk))
666 		return PTR_ERR(clk);
667 
668 	cpu_hw_rate = clk_get_rate(clk) / CLK_HW_DIV;
669 	clk_put(clk);
670 
671 	cpufreq_qcom_hw_driver.driver_data = pdev;
672 
673 	/* Check for optional interconnect paths on CPU0 */
674 	cpu_dev = get_cpu_device(0);
675 	if (!cpu_dev)
676 		return -EPROBE_DEFER;
677 
678 	ret = dev_pm_opp_of_find_icc_paths(cpu_dev, NULL);
679 	if (ret)
680 		return ret;
681 
682 	/* Allocate qcom_cpufreq_data based on the available frequency domains in DT */
683 	soc_node = of_get_parent(dev->of_node);
684 	if (!soc_node)
685 		return -EINVAL;
686 
687 	ret = of_property_read_u32(soc_node, "#address-cells", &reg_sz);
688 	if (ret)
689 		goto of_exit;
690 
691 	ret = of_property_read_u32(soc_node, "#size-cells", &i);
692 	if (ret)
693 		goto of_exit;
694 
695 	reg_sz += i;
696 
697 	num_domains = of_property_count_elems_of_size(dev->of_node, "reg", sizeof(u32) * reg_sz);
698 	if (num_domains <= 0)
699 		return num_domains;
700 
701 	qcom_cpufreq.data = devm_kzalloc(dev, sizeof(struct qcom_cpufreq_data) * num_domains,
702 					 GFP_KERNEL);
703 	if (!qcom_cpufreq.data)
704 		return -ENOMEM;
705 
706 	qcom_cpufreq.soc_data = of_device_get_match_data(dev);
707 
708 	clk_data = devm_kzalloc(dev, struct_size(clk_data, hws, num_domains), GFP_KERNEL);
709 	if (!clk_data)
710 		return -ENOMEM;
711 
712 	clk_data->num = num_domains;
713 
714 	for (i = 0; i < num_domains; i++) {
715 		struct qcom_cpufreq_data *data = &qcom_cpufreq.data[i];
716 		struct clk_init_data clk_init = {};
717 		struct resource *res;
718 		void __iomem *base;
719 
720 		base = devm_platform_get_and_ioremap_resource(pdev, i, &res);
721 		if (IS_ERR(base)) {
722 			dev_err(dev, "Failed to map resource %pR\n", res);
723 			return PTR_ERR(base);
724 		}
725 
726 		data->base = base;
727 		data->res = res;
728 
729 		/* Register CPU clock for each frequency domain */
730 		clk_init.name = kasprintf(GFP_KERNEL, "qcom_cpufreq%d", i);
731 		if (!clk_init.name)
732 			return -ENOMEM;
733 
734 		clk_init.flags = CLK_GET_RATE_NOCACHE;
735 		clk_init.ops = &qcom_cpufreq_hw_clk_ops;
736 		data->cpu_clk.init = &clk_init;
737 
738 		ret = devm_clk_hw_register(dev, &data->cpu_clk);
739 		if (ret < 0) {
740 			dev_err(dev, "Failed to register clock %d: %d\n", i, ret);
741 			kfree(clk_init.name);
742 			return ret;
743 		}
744 
745 		clk_data->hws[i] = &data->cpu_clk;
746 		kfree(clk_init.name);
747 	}
748 
749 	ret = devm_of_clk_add_hw_provider(dev, of_clk_hw_onecell_get, clk_data);
750 	if (ret < 0) {
751 		dev_err(dev, "Failed to add clock provider\n");
752 		return ret;
753 	}
754 
755 	ret = cpufreq_register_driver(&cpufreq_qcom_hw_driver);
756 	if (ret)
757 		dev_err(dev, "CPUFreq HW driver failed to register\n");
758 	else
759 		dev_dbg(dev, "QCOM CPUFreq HW driver initialized\n");
760 
761 of_exit:
762 	of_node_put(soc_node);
763 
764 	return ret;
765 }
766 
767 static int qcom_cpufreq_hw_driver_remove(struct platform_device *pdev)
768 {
769 	return cpufreq_unregister_driver(&cpufreq_qcom_hw_driver);
770 }
771 
772 static struct platform_driver qcom_cpufreq_hw_driver = {
773 	.probe = qcom_cpufreq_hw_driver_probe,
774 	.remove = qcom_cpufreq_hw_driver_remove,
775 	.driver = {
776 		.name = "qcom-cpufreq-hw",
777 		.of_match_table = qcom_cpufreq_hw_match,
778 	},
779 };
780 
781 static int __init qcom_cpufreq_hw_init(void)
782 {
783 	return platform_driver_register(&qcom_cpufreq_hw_driver);
784 }
785 postcore_initcall(qcom_cpufreq_hw_init);
786 
787 static void __exit qcom_cpufreq_hw_exit(void)
788 {
789 	platform_driver_unregister(&qcom_cpufreq_hw_driver);
790 }
791 module_exit(qcom_cpufreq_hw_exit);
792 
793 MODULE_DESCRIPTION("QCOM CPUFREQ HW Driver");
794 MODULE_LICENSE("GPL v2");
795