1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3  * CPUFreq support for Armada 8K
4  *
5  * Copyright (C) 2018 Marvell
6  *
7  * Omri Itach <omrii@marvell.com>
8  * Gregory Clement <gregory.clement@bootlin.com>
9  */
10 
11 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
12 
13 #include <linux/clk.h>
14 #include <linux/cpu.h>
15 #include <linux/err.h>
16 #include <linux/init.h>
17 #include <linux/kernel.h>
18 #include <linux/module.h>
19 #include <linux/of.h>
20 #include <linux/platform_device.h>
21 #include <linux/pm_opp.h>
22 #include <linux/slab.h>
23 
24 /*
25  * Setup the opps list with the divider for the max frequency, that
26  * will be filled at runtime.
27  */
28 static const int opps_div[] __initconst = {1, 2, 3, 4};
29 
30 static struct platform_device *armada_8k_pdev;
31 
32 struct freq_table {
33 	struct device *cpu_dev;
34 	unsigned int freq[ARRAY_SIZE(opps_div)];
35 };
36 
37 /* If the CPUs share the same clock, then they are in the same cluster. */
38 static void __init armada_8k_get_sharing_cpus(struct clk *cur_clk,
39 					      struct cpumask *cpumask)
40 {
41 	int cpu;
42 
43 	for_each_possible_cpu(cpu) {
44 		struct device *cpu_dev;
45 		struct clk *clk;
46 
47 		cpu_dev = get_cpu_device(cpu);
48 		if (!cpu_dev) {
49 			pr_warn("Failed to get cpu%d device\n", cpu);
50 			continue;
51 		}
52 
53 		clk = clk_get(cpu_dev, 0);
54 		if (IS_ERR(clk)) {
55 			pr_warn("Cannot get clock for CPU %d\n", cpu);
56 		} else {
57 			if (clk_is_match(clk, cur_clk))
58 				cpumask_set_cpu(cpu, cpumask);
59 
60 			clk_put(clk);
61 		}
62 	}
63 }
64 
65 static int __init armada_8k_add_opp(struct clk *clk, struct device *cpu_dev,
66 				    struct freq_table *freq_tables,
67 				    int opps_index)
68 {
69 	unsigned int cur_frequency;
70 	unsigned int freq;
71 	int i, ret;
72 
73 	/* Get nominal (current) CPU frequency. */
74 	cur_frequency = clk_get_rate(clk);
75 	if (!cur_frequency) {
76 		dev_err(cpu_dev, "Failed to get clock rate for this CPU\n");
77 		return -EINVAL;
78 	}
79 
80 	freq_tables[opps_index].cpu_dev = cpu_dev;
81 
82 	for (i = 0; i < ARRAY_SIZE(opps_div); i++) {
83 		freq = cur_frequency / opps_div[i];
84 
85 		ret = dev_pm_opp_add(cpu_dev, freq, 0);
86 		if (ret)
87 			return ret;
88 
89 		freq_tables[opps_index].freq[i] = freq;
90 	}
91 
92 	return 0;
93 }
94 
95 static void armada_8k_cpufreq_free_table(struct freq_table *freq_tables)
96 {
97 	int opps_index, nb_cpus = num_possible_cpus();
98 
99 	for (opps_index = 0 ; opps_index <= nb_cpus; opps_index++) {
100 		int i;
101 
102 		/* If cpu_dev is NULL then we reached the end of the array */
103 		if (!freq_tables[opps_index].cpu_dev)
104 			break;
105 
106 		for (i = 0; i < ARRAY_SIZE(opps_div); i++) {
107 			/*
108 			 * A 0Hz frequency is not valid, this meant
109 			 * that it was not yet initialized so there is
110 			 * no more opp to free
111 			 */
112 			if (freq_tables[opps_index].freq[i] == 0)
113 				break;
114 
115 			dev_pm_opp_remove(freq_tables[opps_index].cpu_dev,
116 					  freq_tables[opps_index].freq[i]);
117 		}
118 	}
119 
120 	kfree(freq_tables);
121 }
122 
123 static int __init armada_8k_cpufreq_init(void)
124 {
125 	int ret = 0, opps_index = 0, cpu, nb_cpus;
126 	struct freq_table *freq_tables;
127 	struct device_node *node;
128 	struct cpumask cpus;
129 
130 	node = of_find_compatible_node(NULL, NULL, "marvell,ap806-cpu-clock");
131 	if (!node || !of_device_is_available(node)) {
132 		of_node_put(node);
133 		return -ENODEV;
134 	}
135 	of_node_put(node);
136 
137 	nb_cpus = num_possible_cpus();
138 	freq_tables = kcalloc(nb_cpus, sizeof(*freq_tables), GFP_KERNEL);
139 	cpumask_copy(&cpus, cpu_possible_mask);
140 
141 	/*
142 	 * For each CPU, this loop registers the operating points
143 	 * supported (which are the nominal CPU frequency and full integer
144 	 * divisions of it).
145 	 */
146 	for_each_cpu(cpu, &cpus) {
147 		struct cpumask shared_cpus;
148 		struct device *cpu_dev;
149 		struct clk *clk;
150 
151 		cpu_dev = get_cpu_device(cpu);
152 
153 		if (!cpu_dev) {
154 			pr_err("Cannot get CPU %d\n", cpu);
155 			continue;
156 		}
157 
158 		clk = clk_get(cpu_dev, 0);
159 
160 		if (IS_ERR(clk)) {
161 			pr_err("Cannot get clock for CPU %d\n", cpu);
162 			ret = PTR_ERR(clk);
163 			goto remove_opp;
164 		}
165 
166 		ret = armada_8k_add_opp(clk, cpu_dev, freq_tables, opps_index);
167 		if (ret) {
168 			clk_put(clk);
169 			goto remove_opp;
170 		}
171 
172 		opps_index++;
173 		cpumask_clear(&shared_cpus);
174 		armada_8k_get_sharing_cpus(clk, &shared_cpus);
175 		dev_pm_opp_set_sharing_cpus(cpu_dev, &shared_cpus);
176 		cpumask_andnot(&cpus, &cpus, &shared_cpus);
177 		clk_put(clk);
178 	}
179 
180 	armada_8k_pdev = platform_device_register_simple("cpufreq-dt", -1,
181 							 NULL, 0);
182 	ret = PTR_ERR_OR_ZERO(armada_8k_pdev);
183 	if (ret)
184 		goto remove_opp;
185 
186 	platform_set_drvdata(armada_8k_pdev, freq_tables);
187 
188 	return 0;
189 
190 remove_opp:
191 	armada_8k_cpufreq_free_table(freq_tables);
192 	return ret;
193 }
194 module_init(armada_8k_cpufreq_init);
195 
196 static void __exit armada_8k_cpufreq_exit(void)
197 {
198 	struct freq_table *freq_tables = platform_get_drvdata(armada_8k_pdev);
199 
200 	platform_device_unregister(armada_8k_pdev);
201 	armada_8k_cpufreq_free_table(freq_tables);
202 }
203 module_exit(armada_8k_cpufreq_exit);
204 
205 MODULE_AUTHOR("Gregory Clement <gregory.clement@bootlin.com>");
206 MODULE_DESCRIPTION("Armada 8K cpufreq driver");
207 MODULE_LICENSE("GPL");
208