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 
136 	nb_cpus = num_possible_cpus();
137 	freq_tables = kcalloc(nb_cpus, sizeof(*freq_tables), GFP_KERNEL);
138 	cpumask_copy(&cpus, cpu_possible_mask);
139 
140 	/*
141 	 * For each CPU, this loop registers the operating points
142 	 * supported (which are the nominal CPU frequency and full integer
143 	 * divisions of it).
144 	 */
145 	for_each_cpu(cpu, &cpus) {
146 		struct cpumask shared_cpus;
147 		struct device *cpu_dev;
148 		struct clk *clk;
149 
150 		cpu_dev = get_cpu_device(cpu);
151 
152 		if (!cpu_dev) {
153 			pr_err("Cannot get CPU %d\n", cpu);
154 			continue;
155 		}
156 
157 		clk = clk_get(cpu_dev, 0);
158 
159 		if (IS_ERR(clk)) {
160 			pr_err("Cannot get clock for CPU %d\n", cpu);
161 			ret = PTR_ERR(clk);
162 			goto remove_opp;
163 		}
164 
165 		ret = armada_8k_add_opp(clk, cpu_dev, freq_tables, opps_index);
166 		if (ret) {
167 			clk_put(clk);
168 			goto remove_opp;
169 		}
170 
171 		opps_index++;
172 		cpumask_clear(&shared_cpus);
173 		armada_8k_get_sharing_cpus(clk, &shared_cpus);
174 		dev_pm_opp_set_sharing_cpus(cpu_dev, &shared_cpus);
175 		cpumask_andnot(&cpus, &cpus, &shared_cpus);
176 		clk_put(clk);
177 	}
178 
179 	armada_8k_pdev = platform_device_register_simple("cpufreq-dt", -1,
180 							 NULL, 0);
181 	ret = PTR_ERR_OR_ZERO(armada_8k_pdev);
182 	if (ret)
183 		goto remove_opp;
184 
185 	platform_set_drvdata(armada_8k_pdev, freq_tables);
186 
187 	return 0;
188 
189 remove_opp:
190 	armada_8k_cpufreq_free_table(freq_tables);
191 	return ret;
192 }
193 module_init(armada_8k_cpufreq_init);
194 
195 static void __exit armada_8k_cpufreq_exit(void)
196 {
197 	struct freq_table *freq_tables = platform_get_drvdata(armada_8k_pdev);
198 
199 	platform_device_unregister(armada_8k_pdev);
200 	armada_8k_cpufreq_free_table(freq_tables);
201 }
202 module_exit(armada_8k_cpufreq_exit);
203 
204 MODULE_AUTHOR("Gregory Clement <gregory.clement@bootlin.com>");
205 MODULE_DESCRIPTION("Armada 8K cpufreq driver");
206 MODULE_LICENSE("GPL");
207