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 *uW,
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 	/* Provide micro-Watts value to the Energy Model */
75 	*uW = readl_relaxed(data->reg_bases[REG_EM_POWER_TBL] +
76 			    i * LUT_ROW_SIZE);
77 
78 	return 0;
79 }
80 
81 static int mtk_cpufreq_hw_target_index(struct cpufreq_policy *policy,
82 				       unsigned int index)
83 {
84 	struct mtk_cpufreq_data *data = policy->driver_data;
85 
86 	writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]);
87 
88 	return 0;
89 }
90 
91 static unsigned int mtk_cpufreq_hw_get(unsigned int cpu)
92 {
93 	struct mtk_cpufreq_data *data;
94 	struct cpufreq_policy *policy;
95 	unsigned int index;
96 
97 	policy = cpufreq_cpu_get_raw(cpu);
98 	if (!policy)
99 		return 0;
100 
101 	data = policy->driver_data;
102 
103 	index = readl_relaxed(data->reg_bases[REG_FREQ_PERF_STATE]);
104 	index = min(index, LUT_MAX_ENTRIES - 1);
105 
106 	return data->table[index].frequency;
107 }
108 
109 static unsigned int mtk_cpufreq_hw_fast_switch(struct cpufreq_policy *policy,
110 					       unsigned int target_freq)
111 {
112 	struct mtk_cpufreq_data *data = policy->driver_data;
113 	unsigned int index;
114 
115 	index = cpufreq_table_find_index_dl(policy, target_freq, false);
116 
117 	writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]);
118 
119 	return policy->freq_table[index].frequency;
120 }
121 
122 static int mtk_cpu_create_freq_table(struct platform_device *pdev,
123 				     struct mtk_cpufreq_data *data)
124 {
125 	struct device *dev = &pdev->dev;
126 	u32 temp, i, freq, prev_freq = 0;
127 	void __iomem *base_table;
128 
129 	data->table = devm_kcalloc(dev, LUT_MAX_ENTRIES + 1,
130 				   sizeof(*data->table), GFP_KERNEL);
131 	if (!data->table)
132 		return -ENOMEM;
133 
134 	base_table = data->reg_bases[REG_FREQ_LUT_TABLE];
135 
136 	for (i = 0; i < LUT_MAX_ENTRIES; i++) {
137 		temp = readl_relaxed(base_table + (i * LUT_ROW_SIZE));
138 		freq = FIELD_GET(LUT_FREQ, temp) * 1000;
139 
140 		if (freq == prev_freq)
141 			break;
142 
143 		data->table[i].frequency = freq;
144 
145 		dev_dbg(dev, "index=%d freq=%d\n", i, data->table[i].frequency);
146 
147 		prev_freq = freq;
148 	}
149 
150 	data->table[i].frequency = CPUFREQ_TABLE_END;
151 	data->nr_opp = i;
152 
153 	return 0;
154 }
155 
156 static int mtk_cpu_resources_init(struct platform_device *pdev,
157 				  struct cpufreq_policy *policy,
158 				  const u16 *offsets)
159 {
160 	struct mtk_cpufreq_data *data;
161 	struct device *dev = &pdev->dev;
162 	struct resource *res;
163 	struct of_phandle_args args;
164 	void __iomem *base;
165 	int ret, i;
166 	int index;
167 
168 	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
169 	if (!data)
170 		return -ENOMEM;
171 
172 	ret = of_perf_domain_get_sharing_cpumask(policy->cpu, "performance-domains",
173 						 "#performance-domain-cells",
174 						 policy->cpus, &args);
175 	if (ret < 0)
176 		return ret;
177 
178 	index = args.args[0];
179 	of_node_put(args.np);
180 
181 	res = platform_get_resource(pdev, IORESOURCE_MEM, index);
182 	if (!res) {
183 		dev_err(dev, "failed to get mem resource %d\n", index);
184 		return -ENODEV;
185 	}
186 
187 	if (!request_mem_region(res->start, resource_size(res), res->name)) {
188 		dev_err(dev, "failed to request resource %pR\n", res);
189 		return -EBUSY;
190 	}
191 
192 	base = ioremap(res->start, resource_size(res));
193 	if (!base) {
194 		dev_err(dev, "failed to map resource %pR\n", res);
195 		ret = -ENOMEM;
196 		goto release_region;
197 	}
198 
199 	data->base = base;
200 	data->res = res;
201 
202 	for (i = REG_FREQ_LUT_TABLE; i < REG_ARRAY_SIZE; i++)
203 		data->reg_bases[i] = base + offsets[i];
204 
205 	ret = mtk_cpu_create_freq_table(pdev, data);
206 	if (ret) {
207 		dev_info(dev, "Domain-%d failed to create freq table\n", index);
208 		return ret;
209 	}
210 
211 	policy->freq_table = data->table;
212 	policy->driver_data = data;
213 
214 	return 0;
215 release_region:
216 	release_mem_region(res->start, resource_size(res));
217 	return ret;
218 }
219 
220 static int mtk_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
221 {
222 	struct platform_device *pdev = cpufreq_get_driver_data();
223 	int sig, pwr_hw = CPUFREQ_HW_STATUS | SVS_HW_STATUS;
224 	struct mtk_cpufreq_data *data;
225 	unsigned int latency;
226 	int ret;
227 
228 	/* Get the bases of cpufreq for domains */
229 	ret = mtk_cpu_resources_init(pdev, policy, platform_get_drvdata(pdev));
230 	if (ret) {
231 		dev_info(&pdev->dev, "CPUFreq resource init failed\n");
232 		return ret;
233 	}
234 
235 	data = policy->driver_data;
236 
237 	latency = readl_relaxed(data->reg_bases[REG_FREQ_LATENCY]) * 1000;
238 	if (!latency)
239 		latency = CPUFREQ_ETERNAL;
240 
241 	policy->cpuinfo.transition_latency = latency;
242 	policy->fast_switch_possible = true;
243 
244 	/* HW should be in enabled state to proceed now */
245 	writel_relaxed(0x1, data->reg_bases[REG_FREQ_ENABLE]);
246 	if (readl_poll_timeout(data->reg_bases[REG_FREQ_HW_STATE], sig,
247 			       (sig & pwr_hw) == pwr_hw, POLL_USEC,
248 			       TIMEOUT_USEC)) {
249 		if (!(sig & CPUFREQ_HW_STATUS)) {
250 			pr_info("cpufreq hardware of CPU%d is not enabled\n",
251 				policy->cpu);
252 			return -ENODEV;
253 		}
254 
255 		pr_info("SVS of CPU%d is not enabled\n", policy->cpu);
256 	}
257 
258 	return 0;
259 }
260 
261 static int mtk_cpufreq_hw_cpu_exit(struct cpufreq_policy *policy)
262 {
263 	struct mtk_cpufreq_data *data = policy->driver_data;
264 	struct resource *res = data->res;
265 	void __iomem *base = data->base;
266 
267 	/* HW should be in paused state now */
268 	writel_relaxed(0x0, data->reg_bases[REG_FREQ_ENABLE]);
269 	iounmap(base);
270 	release_mem_region(res->start, resource_size(res));
271 
272 	return 0;
273 }
274 
275 static void mtk_cpufreq_register_em(struct cpufreq_policy *policy)
276 {
277 	struct em_data_callback em_cb = EM_DATA_CB(mtk_cpufreq_get_cpu_power);
278 	struct mtk_cpufreq_data *data = policy->driver_data;
279 
280 	em_dev_register_perf_domain(get_cpu_device(policy->cpu), data->nr_opp,
281 				    &em_cb, policy->cpus, true);
282 }
283 
284 static struct cpufreq_driver cpufreq_mtk_hw_driver = {
285 	.flags		= CPUFREQ_NEED_INITIAL_FREQ_CHECK |
286 			  CPUFREQ_HAVE_GOVERNOR_PER_POLICY |
287 			  CPUFREQ_IS_COOLING_DEV,
288 	.verify		= cpufreq_generic_frequency_table_verify,
289 	.target_index	= mtk_cpufreq_hw_target_index,
290 	.get		= mtk_cpufreq_hw_get,
291 	.init		= mtk_cpufreq_hw_cpu_init,
292 	.exit		= mtk_cpufreq_hw_cpu_exit,
293 	.register_em	= mtk_cpufreq_register_em,
294 	.fast_switch	= mtk_cpufreq_hw_fast_switch,
295 	.name		= "mtk-cpufreq-hw",
296 	.attr		= cpufreq_generic_attr,
297 };
298 
299 static int mtk_cpufreq_hw_driver_probe(struct platform_device *pdev)
300 {
301 	const void *data;
302 	int ret;
303 
304 	data = of_device_get_match_data(&pdev->dev);
305 	if (!data)
306 		return -EINVAL;
307 
308 	platform_set_drvdata(pdev, (void *) data);
309 	cpufreq_mtk_hw_driver.driver_data = pdev;
310 
311 	ret = cpufreq_register_driver(&cpufreq_mtk_hw_driver);
312 	if (ret)
313 		dev_err(&pdev->dev, "CPUFreq HW driver failed to register\n");
314 
315 	return ret;
316 }
317 
318 static int mtk_cpufreq_hw_driver_remove(struct platform_device *pdev)
319 {
320 	cpufreq_unregister_driver(&cpufreq_mtk_hw_driver);
321 
322 	return 0;
323 }
324 
325 static const struct of_device_id mtk_cpufreq_hw_match[] = {
326 	{ .compatible = "mediatek,cpufreq-hw", .data = &cpufreq_mtk_offsets },
327 	{}
328 };
329 MODULE_DEVICE_TABLE(of, mtk_cpufreq_hw_match);
330 
331 static struct platform_driver mtk_cpufreq_hw_driver = {
332 	.probe = mtk_cpufreq_hw_driver_probe,
333 	.remove = mtk_cpufreq_hw_driver_remove,
334 	.driver = {
335 		.name = "mtk-cpufreq-hw",
336 		.of_match_table = mtk_cpufreq_hw_match,
337 	},
338 };
339 module_platform_driver(mtk_cpufreq_hw_driver);
340 
341 MODULE_AUTHOR("Hector Yuan <hector.yuan@mediatek.com>");
342 MODULE_DESCRIPTION("Mediatek cpufreq-hw driver");
343 MODULE_LICENSE("GPL v2");
344