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