GNU Linux-libre 5.19-rc6-gnu
[releases.git] / drivers / devfreq / governor_passive.c
1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * linux/drivers/devfreq/governor_passive.c
4  *
5  * Copyright (C) 2016 Samsung Electronics
6  * Author: Chanwoo Choi <cw00.choi@samsung.com>
7  * Author: MyungJoo Ham <myungjoo.ham@samsung.com>
8  */
9
10 #include <linux/module.h>
11 #include <linux/cpu.h>
12 #include <linux/cpufreq.h>
13 #include <linux/cpumask.h>
14 #include <linux/slab.h>
15 #include <linux/device.h>
16 #include <linux/devfreq.h>
17 #include <linux/units.h>
18 #include "governor.h"
19
20 static struct devfreq_cpu_data *
21 get_parent_cpu_data(struct devfreq_passive_data *p_data,
22                     struct cpufreq_policy *policy)
23 {
24         struct devfreq_cpu_data *parent_cpu_data;
25
26         if (!p_data || !policy)
27                 return NULL;
28
29         list_for_each_entry(parent_cpu_data, &p_data->cpu_data_list, node)
30                 if (parent_cpu_data->first_cpu == cpumask_first(policy->related_cpus))
31                         return parent_cpu_data;
32
33         return NULL;
34 }
35
36 static void delete_parent_cpu_data(struct devfreq_passive_data *p_data)
37 {
38         struct devfreq_cpu_data *parent_cpu_data, *tmp;
39
40         list_for_each_entry_safe(parent_cpu_data, tmp, &p_data->cpu_data_list, node) {
41                 list_del(&parent_cpu_data->node);
42
43                 if (parent_cpu_data->opp_table)
44                         dev_pm_opp_put_opp_table(parent_cpu_data->opp_table);
45
46                 kfree(parent_cpu_data);
47         }
48 }
49
50 static unsigned long get_target_freq_by_required_opp(struct device *p_dev,
51                                                 struct opp_table *p_opp_table,
52                                                 struct opp_table *opp_table,
53                                                 unsigned long *freq)
54 {
55         struct dev_pm_opp *opp = NULL, *p_opp = NULL;
56         unsigned long target_freq;
57
58         if (!p_dev || !p_opp_table || !opp_table || !freq)
59                 return 0;
60
61         p_opp = devfreq_recommended_opp(p_dev, freq, 0);
62         if (IS_ERR(p_opp))
63                 return 0;
64
65         opp = dev_pm_opp_xlate_required_opp(p_opp_table, opp_table, p_opp);
66         dev_pm_opp_put(p_opp);
67
68         if (IS_ERR(opp))
69                 return 0;
70
71         target_freq = dev_pm_opp_get_freq(opp);
72         dev_pm_opp_put(opp);
73
74         return target_freq;
75 }
76
77 static int get_target_freq_with_cpufreq(struct devfreq *devfreq,
78                                         unsigned long *target_freq)
79 {
80         struct devfreq_passive_data *p_data =
81                                 (struct devfreq_passive_data *)devfreq->data;
82         struct devfreq_cpu_data *parent_cpu_data;
83         struct cpufreq_policy *policy;
84         unsigned long cpu, cpu_cur, cpu_min, cpu_max, cpu_percent;
85         unsigned long dev_min, dev_max;
86         unsigned long freq = 0;
87         int ret = 0;
88
89         for_each_online_cpu(cpu) {
90                 policy = cpufreq_cpu_get(cpu);
91                 if (!policy) {
92                         ret = -EINVAL;
93                         continue;
94                 }
95
96                 parent_cpu_data = get_parent_cpu_data(p_data, policy);
97                 if (!parent_cpu_data) {
98                         cpufreq_cpu_put(policy);
99                         continue;
100                 }
101
102                 /* Get target freq via required opps */
103                 cpu_cur = parent_cpu_data->cur_freq * HZ_PER_KHZ;
104                 freq = get_target_freq_by_required_opp(parent_cpu_data->dev,
105                                         parent_cpu_data->opp_table,
106                                         devfreq->opp_table, &cpu_cur);
107                 if (freq) {
108                         *target_freq = max(freq, *target_freq);
109                         cpufreq_cpu_put(policy);
110                         continue;
111                 }
112
113                 /* Use interpolation if required opps is not available */
114                 devfreq_get_freq_range(devfreq, &dev_min, &dev_max);
115
116                 cpu_min = parent_cpu_data->min_freq;
117                 cpu_max = parent_cpu_data->max_freq;
118                 cpu_cur = parent_cpu_data->cur_freq;
119
120                 cpu_percent = ((cpu_cur - cpu_min) * 100) / (cpu_max - cpu_min);
121                 freq = dev_min + mult_frac(dev_max - dev_min, cpu_percent, 100);
122
123                 *target_freq = max(freq, *target_freq);
124                 cpufreq_cpu_put(policy);
125         }
126
127         return ret;
128 }
129
130 static int get_target_freq_with_devfreq(struct devfreq *devfreq,
131                                         unsigned long *freq)
132 {
133         struct devfreq_passive_data *p_data
134                         = (struct devfreq_passive_data *)devfreq->data;
135         struct devfreq *parent_devfreq = (struct devfreq *)p_data->parent;
136         unsigned long child_freq = ULONG_MAX;
137         int i, count;
138
139         /* Get target freq via required opps */
140         child_freq = get_target_freq_by_required_opp(parent_devfreq->dev.parent,
141                                                 parent_devfreq->opp_table,
142                                                 devfreq->opp_table, freq);
143         if (child_freq)
144                 goto out;
145
146         /* Use interpolation if required opps is not available */
147         for (i = 0; i < parent_devfreq->max_state; i++)
148                 if (parent_devfreq->freq_table[i] == *freq)
149                         break;
150
151         if (i == parent_devfreq->max_state)
152                 return -EINVAL;
153
154         if (i < devfreq->max_state) {
155                 child_freq = devfreq->freq_table[i];
156         } else {
157                 count = devfreq->max_state;
158                 child_freq = devfreq->freq_table[count - 1];
159         }
160
161 out:
162         *freq = child_freq;
163
164         return 0;
165 }
166
167 static int devfreq_passive_get_target_freq(struct devfreq *devfreq,
168                                            unsigned long *freq)
169 {
170         struct devfreq_passive_data *p_data =
171                                 (struct devfreq_passive_data *)devfreq->data;
172         int ret;
173
174         if (!p_data)
175                 return -EINVAL;
176
177         /*
178          * If the devfreq device with passive governor has the specific method
179          * to determine the next frequency, should use the get_target_freq()
180          * of struct devfreq_passive_data.
181          */
182         if (p_data->get_target_freq)
183                 return p_data->get_target_freq(devfreq, freq);
184
185         switch (p_data->parent_type) {
186         case DEVFREQ_PARENT_DEV:
187                 ret = get_target_freq_with_devfreq(devfreq, freq);
188                 break;
189         case CPUFREQ_PARENT_DEV:
190                 ret = get_target_freq_with_cpufreq(devfreq, freq);
191                 break;
192         default:
193                 ret = -EINVAL;
194                 dev_err(&devfreq->dev, "Invalid parent type\n");
195                 break;
196         }
197
198         return ret;
199 }
200
201 static int cpufreq_passive_notifier_call(struct notifier_block *nb,
202                                          unsigned long event, void *ptr)
203 {
204         struct devfreq_passive_data *p_data =
205                         container_of(nb, struct devfreq_passive_data, nb);
206         struct devfreq *devfreq = (struct devfreq *)p_data->this;
207         struct devfreq_cpu_data *parent_cpu_data;
208         struct cpufreq_freqs *freqs = ptr;
209         unsigned int cur_freq;
210         int ret;
211
212         if (event != CPUFREQ_POSTCHANGE || !freqs)
213                 return 0;
214
215         parent_cpu_data = get_parent_cpu_data(p_data, freqs->policy);
216         if (!parent_cpu_data || parent_cpu_data->cur_freq == freqs->new)
217                 return 0;
218
219         cur_freq = parent_cpu_data->cur_freq;
220         parent_cpu_data->cur_freq = freqs->new;
221
222         mutex_lock(&devfreq->lock);
223         ret = devfreq_update_target(devfreq, freqs->new);
224         mutex_unlock(&devfreq->lock);
225         if (ret) {
226                 parent_cpu_data->cur_freq = cur_freq;
227                 dev_err(&devfreq->dev, "failed to update the frequency.\n");
228                 return ret;
229         }
230
231         return 0;
232 }
233
234 static int cpufreq_passive_unregister_notifier(struct devfreq *devfreq)
235 {
236         struct devfreq_passive_data *p_data
237                         = (struct devfreq_passive_data *)devfreq->data;
238         int ret;
239
240         if (p_data->nb.notifier_call) {
241                 ret = cpufreq_unregister_notifier(&p_data->nb,
242                                         CPUFREQ_TRANSITION_NOTIFIER);
243                 if (ret < 0)
244                         return ret;
245         }
246
247         delete_parent_cpu_data(p_data);
248
249         return 0;
250 }
251
252 static int cpufreq_passive_register_notifier(struct devfreq *devfreq)
253 {
254         struct devfreq_passive_data *p_data
255                         = (struct devfreq_passive_data *)devfreq->data;
256         struct device *dev = devfreq->dev.parent;
257         struct opp_table *opp_table = NULL;
258         struct devfreq_cpu_data *parent_cpu_data;
259         struct cpufreq_policy *policy;
260         struct device *cpu_dev;
261         unsigned int cpu;
262         int ret;
263
264         p_data->cpu_data_list
265                 = (struct list_head)LIST_HEAD_INIT(p_data->cpu_data_list);
266
267         p_data->nb.notifier_call = cpufreq_passive_notifier_call;
268         ret = cpufreq_register_notifier(&p_data->nb, CPUFREQ_TRANSITION_NOTIFIER);
269         if (ret) {
270                 dev_err(dev, "failed to register cpufreq notifier\n");
271                 p_data->nb.notifier_call = NULL;
272                 goto err;
273         }
274
275         for_each_possible_cpu(cpu) {
276                 policy = cpufreq_cpu_get(cpu);
277                 if (!policy) {
278                         ret = -EPROBE_DEFER;
279                         goto err;
280                 }
281
282                 parent_cpu_data = get_parent_cpu_data(p_data, policy);
283                 if (parent_cpu_data) {
284                         cpufreq_cpu_put(policy);
285                         continue;
286                 }
287
288                 parent_cpu_data = kzalloc(sizeof(*parent_cpu_data),
289                                                 GFP_KERNEL);
290                 if (!parent_cpu_data) {
291                         ret = -ENOMEM;
292                         goto err_put_policy;
293                 }
294
295                 cpu_dev = get_cpu_device(cpu);
296                 if (!cpu_dev) {
297                         dev_err(dev, "failed to get cpu device\n");
298                         ret = -ENODEV;
299                         goto err_free_cpu_data;
300                 }
301
302                 opp_table = dev_pm_opp_get_opp_table(cpu_dev);
303                 if (IS_ERR(opp_table)) {
304                         dev_err(dev, "failed to get opp_table of cpu%d\n", cpu);
305                         ret = PTR_ERR(opp_table);
306                         goto err_free_cpu_data;
307                 }
308
309                 parent_cpu_data->dev = cpu_dev;
310                 parent_cpu_data->opp_table = opp_table;
311                 parent_cpu_data->first_cpu = cpumask_first(policy->related_cpus);
312                 parent_cpu_data->cur_freq = policy->cur;
313                 parent_cpu_data->min_freq = policy->cpuinfo.min_freq;
314                 parent_cpu_data->max_freq = policy->cpuinfo.max_freq;
315
316                 list_add_tail(&parent_cpu_data->node, &p_data->cpu_data_list);
317                 cpufreq_cpu_put(policy);
318         }
319
320         mutex_lock(&devfreq->lock);
321         ret = devfreq_update_target(devfreq, 0L);
322         mutex_unlock(&devfreq->lock);
323         if (ret)
324                 dev_err(dev, "failed to update the frequency\n");
325
326         return ret;
327
328 err_free_cpu_data:
329         kfree(parent_cpu_data);
330 err_put_policy:
331         cpufreq_cpu_put(policy);
332 err:
333
334         return ret;
335 }
336
337 static int devfreq_passive_notifier_call(struct notifier_block *nb,
338                                 unsigned long event, void *ptr)
339 {
340         struct devfreq_passive_data *data
341                         = container_of(nb, struct devfreq_passive_data, nb);
342         struct devfreq *devfreq = (struct devfreq *)data->this;
343         struct devfreq *parent = (struct devfreq *)data->parent;
344         struct devfreq_freqs *freqs = (struct devfreq_freqs *)ptr;
345         unsigned long freq = freqs->new;
346         int ret = 0;
347
348         mutex_lock_nested(&devfreq->lock, SINGLE_DEPTH_NESTING);
349         switch (event) {
350         case DEVFREQ_PRECHANGE:
351                 if (parent->previous_freq > freq)
352                         ret = devfreq_update_target(devfreq, freq);
353
354                 break;
355         case DEVFREQ_POSTCHANGE:
356                 if (parent->previous_freq < freq)
357                         ret = devfreq_update_target(devfreq, freq);
358                 break;
359         }
360         mutex_unlock(&devfreq->lock);
361
362         if (ret < 0)
363                 dev_warn(&devfreq->dev,
364                         "failed to update devfreq using passive governor\n");
365
366         return NOTIFY_DONE;
367 }
368
369 static int devfreq_passive_unregister_notifier(struct devfreq *devfreq)
370 {
371         struct devfreq_passive_data *p_data
372                         = (struct devfreq_passive_data *)devfreq->data;
373         struct devfreq *parent = (struct devfreq *)p_data->parent;
374         struct notifier_block *nb = &p_data->nb;
375
376         return devfreq_unregister_notifier(parent, nb, DEVFREQ_TRANSITION_NOTIFIER);
377 }
378
379 static int devfreq_passive_register_notifier(struct devfreq *devfreq)
380 {
381         struct devfreq_passive_data *p_data
382                         = (struct devfreq_passive_data *)devfreq->data;
383         struct devfreq *parent = (struct devfreq *)p_data->parent;
384         struct notifier_block *nb = &p_data->nb;
385
386         if (!parent)
387                 return -EPROBE_DEFER;
388
389         nb->notifier_call = devfreq_passive_notifier_call;
390         return devfreq_register_notifier(parent, nb, DEVFREQ_TRANSITION_NOTIFIER);
391 }
392
393 static int devfreq_passive_event_handler(struct devfreq *devfreq,
394                                 unsigned int event, void *data)
395 {
396         struct devfreq_passive_data *p_data
397                         = (struct devfreq_passive_data *)devfreq->data;
398         int ret = 0;
399
400         if (!p_data)
401                 return -EINVAL;
402
403         p_data->this = devfreq;
404
405         switch (event) {
406         case DEVFREQ_GOV_START:
407                 if (p_data->parent_type == DEVFREQ_PARENT_DEV)
408                         ret = devfreq_passive_register_notifier(devfreq);
409                 else if (p_data->parent_type == CPUFREQ_PARENT_DEV)
410                         ret = cpufreq_passive_register_notifier(devfreq);
411                 break;
412         case DEVFREQ_GOV_STOP:
413                 if (p_data->parent_type == DEVFREQ_PARENT_DEV)
414                         WARN_ON(devfreq_passive_unregister_notifier(devfreq));
415                 else if (p_data->parent_type == CPUFREQ_PARENT_DEV)
416                         WARN_ON(cpufreq_passive_unregister_notifier(devfreq));
417                 break;
418         default:
419                 break;
420         }
421
422         return ret;
423 }
424
425 static struct devfreq_governor devfreq_passive = {
426         .name = DEVFREQ_GOV_PASSIVE,
427         .flags = DEVFREQ_GOV_FLAG_IMMUTABLE,
428         .get_target_freq = devfreq_passive_get_target_freq,
429         .event_handler = devfreq_passive_event_handler,
430 };
431
432 static int __init devfreq_passive_init(void)
433 {
434         return devfreq_add_governor(&devfreq_passive);
435 }
436 subsys_initcall(devfreq_passive_init);
437
438 static void __exit devfreq_passive_exit(void)
439 {
440         int ret;
441
442         ret = devfreq_remove_governor(&devfreq_passive);
443         if (ret)
444                 pr_err("%s: failed remove governor %d\n", __func__, ret);
445 }
446 module_exit(devfreq_passive_exit);
447
448 MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>");
449 MODULE_AUTHOR("MyungJoo Ham <myungjoo.ham@samsung.com>");
450 MODULE_DESCRIPTION("DEVFREQ Passive governor");
451 MODULE_LICENSE("GPL v2");