1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (c) 2020 MediaTek Inc.
4  */
5 
6 #include <linux/bitfield.h>
7 #include <linux/cpufreq.h>
8 #include <linux/energy_model.h>
9 #include <linux/init.h>
10 #include <linux/iopoll.h>
11 #include <linux/kernel.h>
12 #include <linux/module.h>
13 #include <linux/of_address.h>
14 #include <linux/of_platform.h>
15 #include <linux/slab.h>
16 
17 #define LUT_MAX_ENTRIES			32U
18 #define LUT_FREQ			GENMASK(11, 0)
19 #define LUT_ROW_SIZE			0x4
20 #define CPUFREQ_HW_STATUS		BIT(0)
21 #define SVS_HW_STATUS			BIT(1)
22 #define POLL_USEC			1000
23 #define TIMEOUT_USEC			300000
24 
25 enum {
26 	REG_FREQ_LUT_TABLE,
27 	REG_FREQ_ENABLE,
28 	REG_FREQ_PERF_STATE,
29 	REG_FREQ_HW_STATE,
30 	REG_EM_POWER_TBL,
31 	REG_FREQ_LATENCY,
32 
33 	REG_ARRAY_SIZE,
34 };
35 
36 struct mtk_cpufreq_data {
37 	struct cpufreq_frequency_table *table;
38 	void __iomem *reg_bases[REG_ARRAY_SIZE];
39 	struct resource *res;
40 	void __iomem *base;
41 	int nr_opp;
42 };
43 
44 static const u16 cpufreq_mtk_offsets[REG_ARRAY_SIZE] = {
45 	[REG_FREQ_LUT_TABLE]	= 0x0,
46 	[REG_FREQ_ENABLE]	= 0x84,
47 	[REG_FREQ_PERF_STATE]	= 0x88,
48 	[REG_FREQ_HW_STATE]	= 0x8c,
49 	[REG_EM_POWER_TBL]	= 0x90,
50 	[REG_FREQ_LATENCY]	= 0x110,
51 };
52 
53 static int __maybe_unused
54 mtk_cpufreq_get_cpu_power(struct device *cpu_dev, unsigned long *mW,
55 			  unsigned long *KHz)
56 {
57 	struct mtk_cpufreq_data *data;
58 	struct cpufreq_policy *policy;
59 	int i;
60 
61 	policy = cpufreq_cpu_get_raw(cpu_dev->id);
62 	if (!policy)
63 		return 0;
64 
65 	data = policy->driver_data;
66 
67 	for (i = 0; i < data->nr_opp; i++) {
68 		if (data->table[i].frequency < *KHz)
69 			break;
70 	}
71 	i--;
72 
73 	*KHz = data->table[i].frequency;
74 	*mW = readl_relaxed(data->reg_bases[REG_EM_POWER_TBL] +
75 			    i * LUT_ROW_SIZE) / 1000;
76 
77 	return 0;
78 }
79 
80 static int mtk_cpufreq_hw_target_index(struct cpufreq_policy *policy,
81 				       unsigned int index)
82 {
83 	struct mtk_cpufreq_data *data = policy->driver_data;
84 
85 	writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]);
86 
87 	return 0;
88 }
89 
90 static unsigned int mtk_cpufreq_hw_get(unsigned int cpu)
91 {
92 	struct mtk_cpufreq_data *data;
93 	struct cpufreq_policy *policy;
94 	unsigned int index;
95 
96 	policy = cpufreq_cpu_get_raw(cpu);
97 	if (!policy)
98 		return 0;
99 
100 	data = policy->driver_data;
101 
102 	index = readl_relaxed(data->reg_bases[REG_FREQ_PERF_STATE]);
103 	index = min(index, LUT_MAX_ENTRIES - 1);
104 
105 	return data->table[index].frequency;
106 }
107 
108 static unsigned int mtk_cpufreq_hw_fast_switch(struct cpufreq_policy *policy,
109 					       unsigned int target_freq)
110 {
111 	struct mtk_cpufreq_data *data = policy->driver_data;
112 	unsigned int index;
113 
114 	index = cpufreq_table_find_index_dl(policy, target_freq, false);
115 
116 	writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]);
117 
118 	return policy->freq_table[index].frequency;
119 }
120 
121 static int mtk_cpu_create_freq_table(struct platform_device *pdev,
122 				     struct mtk_cpufreq_data *data)
123 {
124 	struct device *dev = &pdev->dev;
125 	u32 temp, i, freq, prev_freq = 0;
126 	void __iomem *base_table;
127 
128 	data->table = devm_kcalloc(dev, LUT_MAX_ENTRIES + 1,
129 				   sizeof(*data->table), GFP_KERNEL);
130 	if (!data->table)
131 		return -ENOMEM;
132 
133 	base_table = data->reg_bases[REG_FREQ_LUT_TABLE];
134 
135 	for (i = 0; i < LUT_MAX_ENTRIES; i++) {
136 		temp = readl_relaxed(base_table + (i * LUT_ROW_SIZE));
137 		freq = FIELD_GET(LUT_FREQ, temp) * 1000;
138 
139 		if (freq == prev_freq)
140 			break;
141 
142 		data->table[i].frequency = freq;
143 
144 		dev_dbg(dev, "index=%d freq=%d\n", i, data->table[i].frequency);
145 
146 		prev_freq = freq;
147 	}
148 
149 	data->table[i].frequency = CPUFREQ_TABLE_END;
150 	data->nr_opp = i;
151 
152 	return 0;
153 }
154 
155 static int mtk_cpu_resources_init(struct platform_device *pdev,
156 				  struct cpufreq_policy *policy,
157 				  const u16 *offsets)
158 {
159 	struct mtk_cpufreq_data *data;
160 	struct device *dev = &pdev->dev;
161 	struct resource *res;
162 	void __iomem *base;
163 	int ret, i;
164 	int index;
165 
166 	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
167 	if (!data)
168 		return -ENOMEM;
169 
170 	index = of_perf_domain_get_sharing_cpumask(policy->cpu, "performance-domains",
171 						   "#performance-domain-cells",
172 						   policy->cpus);
173 	if (index < 0)
174 		return index;
175 
176 	res = platform_get_resource(pdev, IORESOURCE_MEM, index);
177 	if (!res) {
178 		dev_err(dev, "failed to get mem resource %d\n", index);
179 		return -ENODEV;
180 	}
181 
182 	if (!request_mem_region(res->start, resource_size(res), res->name)) {
183 		dev_err(dev, "failed to request resource %pR\n", res);
184 		return -EBUSY;
185 	}
186 
187 	base = ioremap(res->start, resource_size(res));
188 	if (!base) {
189 		dev_err(dev, "failed to map resource %pR\n", res);
190 		ret = -ENOMEM;
191 		goto release_region;
192 	}
193 
194 	data->base = base;
195 	data->res = res;
196 
197 	for (i = REG_FREQ_LUT_TABLE; i < REG_ARRAY_SIZE; i++)
198 		data->reg_bases[i] = base + offsets[i];
199 
200 	ret = mtk_cpu_create_freq_table(pdev, data);
201 	if (ret) {
202 		dev_info(dev, "Domain-%d failed to create freq table\n", index);
203 		return ret;
204 	}
205 
206 	policy->freq_table = data->table;
207 	policy->driver_data = data;
208 
209 	return 0;
210 release_region:
211 	release_mem_region(res->start, resource_size(res));
212 	return ret;
213 }
214 
215 static int mtk_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
216 {
217 	struct platform_device *pdev = cpufreq_get_driver_data();
218 	int sig, pwr_hw = CPUFREQ_HW_STATUS | SVS_HW_STATUS;
219 	struct mtk_cpufreq_data *data;
220 	unsigned int latency;
221 	int ret;
222 
223 	/* Get the bases of cpufreq for domains */
224 	ret = mtk_cpu_resources_init(pdev, policy, platform_get_drvdata(pdev));
225 	if (ret) {
226 		dev_info(&pdev->dev, "CPUFreq resource init failed\n");
227 		return ret;
228 	}
229 
230 	data = policy->driver_data;
231 
232 	latency = readl_relaxed(data->reg_bases[REG_FREQ_LATENCY]) * 1000;
233 	if (!latency)
234 		latency = CPUFREQ_ETERNAL;
235 
236 	policy->cpuinfo.transition_latency = latency;
237 	policy->fast_switch_possible = true;
238 
239 	/* HW should be in enabled state to proceed now */
240 	writel_relaxed(0x1, data->reg_bases[REG_FREQ_ENABLE]);
241 	if (readl_poll_timeout(data->reg_bases[REG_FREQ_HW_STATE], sig,
242 			       (sig & pwr_hw) == pwr_hw, POLL_USEC,
243 			       TIMEOUT_USEC)) {
244 		if (!(sig & CPUFREQ_HW_STATUS)) {
245 			pr_info("cpufreq hardware of CPU%d is not enabled\n",
246 				policy->cpu);
247 			return -ENODEV;
248 		}
249 
250 		pr_info("SVS of CPU%d is not enabled\n", policy->cpu);
251 	}
252 
253 	return 0;
254 }
255 
256 static int mtk_cpufreq_hw_cpu_exit(struct cpufreq_policy *policy)
257 {
258 	struct mtk_cpufreq_data *data = policy->driver_data;
259 	struct resource *res = data->res;
260 	void __iomem *base = data->base;
261 
262 	/* HW should be in paused state now */
263 	writel_relaxed(0x0, data->reg_bases[REG_FREQ_ENABLE]);
264 	iounmap(base);
265 	release_mem_region(res->start, resource_size(res));
266 
267 	return 0;
268 }
269 
270 static void mtk_cpufreq_register_em(struct cpufreq_policy *policy)
271 {
272 	struct em_data_callback em_cb = EM_DATA_CB(mtk_cpufreq_get_cpu_power);
273 	struct mtk_cpufreq_data *data = policy->driver_data;
274 
275 	em_dev_register_perf_domain(get_cpu_device(policy->cpu), data->nr_opp,
276 				    &em_cb, policy->cpus, true);
277 }
278 
279 static struct cpufreq_driver cpufreq_mtk_hw_driver = {
280 	.flags		= CPUFREQ_NEED_INITIAL_FREQ_CHECK |
281 			  CPUFREQ_HAVE_GOVERNOR_PER_POLICY |
282 			  CPUFREQ_IS_COOLING_DEV,
283 	.verify		= cpufreq_generic_frequency_table_verify,
284 	.target_index	= mtk_cpufreq_hw_target_index,
285 	.get		= mtk_cpufreq_hw_get,
286 	.init		= mtk_cpufreq_hw_cpu_init,
287 	.exit		= mtk_cpufreq_hw_cpu_exit,
288 	.register_em	= mtk_cpufreq_register_em,
289 	.fast_switch	= mtk_cpufreq_hw_fast_switch,
290 	.name		= "mtk-cpufreq-hw",
291 	.attr		= cpufreq_generic_attr,
292 };
293 
294 static int mtk_cpufreq_hw_driver_probe(struct platform_device *pdev)
295 {
296 	const void *data;
297 	int ret;
298 
299 	data = of_device_get_match_data(&pdev->dev);
300 	if (!data)
301 		return -EINVAL;
302 
303 	platform_set_drvdata(pdev, (void *) data);
304 	cpufreq_mtk_hw_driver.driver_data = pdev;
305 
306 	ret = cpufreq_register_driver(&cpufreq_mtk_hw_driver);
307 	if (ret)
308 		dev_err(&pdev->dev, "CPUFreq HW driver failed to register\n");
309 
310 	return ret;
311 }
312 
313 static int mtk_cpufreq_hw_driver_remove(struct platform_device *pdev)
314 {
315 	return cpufreq_unregister_driver(&cpufreq_mtk_hw_driver);
316 }
317 
318 static const struct of_device_id mtk_cpufreq_hw_match[] = {
319 	{ .compatible = "mediatek,cpufreq-hw", .data = &cpufreq_mtk_offsets },
320 	{}
321 };
322 
323 static struct platform_driver mtk_cpufreq_hw_driver = {
324 	.probe = mtk_cpufreq_hw_driver_probe,
325 	.remove = mtk_cpufreq_hw_driver_remove,
326 	.driver = {
327 		.name = "mtk-cpufreq-hw",
328 		.of_match_table = mtk_cpufreq_hw_match,
329 	},
330 };
331 module_platform_driver(mtk_cpufreq_hw_driver);
332 
333 MODULE_AUTHOR("Hector Yuan <hector.yuan@mediatek.com>");
334 MODULE_DESCRIPTION("Mediatek cpufreq-hw driver");
335 MODULE_LICENSE("GPL v2");
336