drivers/devfreq/governor_passive.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * linux/drivers/devfreq/governor_passive.c
0004  *
0005  * Copyright (C) 2016 Samsung Electronics
0006  * Author: Chanwoo Choi <cw00.choi@samsung.com>
0007  * Author: MyungJoo Ham <myungjoo.ham@samsung.com>
0008  */
0009
0010 #include <linux/module.h>
0011 #include <linux/cpu.h>
0012 #include <linux/cpufreq.h>
0013 #include <linux/cpumask.h>
0014 #include <linux/slab.h>
0015 #include <linux/device.h>
0016 #include <linux/devfreq.h>
0017 #include <linux/units.h>
0018 #include "governor.h"
0019
0020 static struct devfreq_cpu_data *
0021 get_parent_cpu_data(struct devfreq_passive_data *p_data,
0022             struct cpufreq_policy *policy)
0023 {
0024     struct devfreq_cpu_data *parent_cpu_data;
0025
0026     if (!p_data || !policy)
0027         return NULL;
0028
0029     list_for_each_entry(parent_cpu_data, &p_data->cpu_data_list, node)
0030         if (parent_cpu_data->first_cpu == cpumask_first(policy->related_cpus))
0031             return parent_cpu_data;
0032
0033     return NULL;
0034 }
0035
0036 static void delete_parent_cpu_data(struct devfreq_passive_data *p_data)
0037 {
0038     struct devfreq_cpu_data *parent_cpu_data, *tmp;
0039
0040     list_for_each_entry_safe(parent_cpu_data, tmp, &p_data->cpu_data_list, node) {
0041         list_del(&parent_cpu_data->node);
0042
0043         if (parent_cpu_data->opp_table)
0044             dev_pm_opp_put_opp_table(parent_cpu_data->opp_table);
0045
0046         kfree(parent_cpu_data);
0047     }
0048 }
0049
0050 static unsigned long get_target_freq_by_required_opp(struct device *p_dev,
0051                         struct opp_table *p_opp_table,
0052                         struct opp_table *opp_table,
0053                         unsigned long *freq)
0054 {
0055     struct dev_pm_opp *opp = NULL, *p_opp = NULL;
0056     unsigned long target_freq;
0057
0058     if (!p_dev || !p_opp_table || !opp_table || !freq)
0059         return 0;
0060
0061     p_opp = devfreq_recommended_opp(p_dev, freq, 0);
0062     if (IS_ERR(p_opp))
0063         return 0;
0064
0065     opp = dev_pm_opp_xlate_required_opp(p_opp_table, opp_table, p_opp);
0066     dev_pm_opp_put(p_opp);
0067
0068     if (IS_ERR(opp))
0069         return 0;
0070
0071     target_freq = dev_pm_opp_get_freq(opp);
0072     dev_pm_opp_put(opp);
0073
0074     return target_freq;
0075 }
0076
0077 static int get_target_freq_with_cpufreq(struct devfreq *devfreq,
0078                     unsigned long *target_freq)
0079 {
0080     struct devfreq_passive_data *p_data =
0081                 (struct devfreq_passive_data *)devfreq->data;
0082     struct devfreq_cpu_data *parent_cpu_data;
0083     struct cpufreq_policy *policy;
0084     unsigned long cpu, cpu_cur, cpu_min, cpu_max, cpu_percent;
0085     unsigned long dev_min, dev_max;
0086     unsigned long freq = 0;
0087     int ret = 0;
0088
0089     for_each_online_cpu(cpu) {
0090         policy = cpufreq_cpu_get(cpu);
0091         if (!policy) {
0092             ret = -EINVAL;
0093             continue;
0094         }
0095
0096         parent_cpu_data = get_parent_cpu_data(p_data, policy);
0097         if (!parent_cpu_data) {
0098             cpufreq_cpu_put(policy);
0099             continue;
0100         }
0101
0102         /* Get target freq via required opps */
0103         cpu_cur = parent_cpu_data->cur_freq * HZ_PER_KHZ;
0104         freq = get_target_freq_by_required_opp(parent_cpu_data->dev,
0105                     parent_cpu_data->opp_table,
0106                     devfreq->opp_table, &cpu_cur);
0107         if (freq) {
0108             *target_freq = max(freq, *target_freq);
0109             cpufreq_cpu_put(policy);
0110             continue;
0111         }
0112
0113         /* Use interpolation if required opps is not available */
0114         devfreq_get_freq_range(devfreq, &dev_min, &dev_max);
0115
0116         cpu_min = parent_cpu_data->min_freq;
0117         cpu_max = parent_cpu_data->max_freq;
0118         cpu_cur = parent_cpu_data->cur_freq;
0119
0120         cpu_percent = ((cpu_cur - cpu_min) * 100) / (cpu_max - cpu_min);
0121         freq = dev_min + mult_frac(dev_max - dev_min, cpu_percent, 100);
0122
0123         *target_freq = max(freq, *target_freq);
0124         cpufreq_cpu_put(policy);
0125     }
0126
0127     return ret;
0128 }
0129
0130 static int get_target_freq_with_devfreq(struct devfreq *devfreq,
0131                     unsigned long *freq)
0132 {
0133     struct devfreq_passive_data *p_data
0134             = (struct devfreq_passive_data *)devfreq->data;
0135     struct devfreq *parent_devfreq = (struct devfreq *)p_data->parent;
0136     unsigned long child_freq = ULONG_MAX;
0137     int i, count;
0138
0139     /* Get target freq via required opps */
0140     child_freq = get_target_freq_by_required_opp(parent_devfreq->dev.parent,
0141                         parent_devfreq->opp_table,
0142                         devfreq->opp_table, freq);
0143     if (child_freq)
0144         goto out;
0145
0146     /* Use interpolation if required opps is not available */
0147     for (i = 0; i < parent_devfreq->max_state; i++)
0148         if (parent_devfreq->freq_table[i] == *freq)
0149             break;
0150
0151     if (i == parent_devfreq->max_state)
0152         return -EINVAL;
0153
0154     if (i < devfreq->max_state) {
0155         child_freq = devfreq->freq_table[i];
0156     } else {
0157         count = devfreq->max_state;
0158         child_freq = devfreq->freq_table[count - 1];
0159     }
0160
0161 out:
0162     *freq = child_freq;
0163
0164     return 0;
0165 }
0166
0167 static int devfreq_passive_get_target_freq(struct devfreq *devfreq,
0168                        unsigned long *freq)
0169 {
0170     struct devfreq_passive_data *p_data =
0171                 (struct devfreq_passive_data *)devfreq->data;
0172     int ret;
0173
0174     if (!p_data)
0175         return -EINVAL;
0176
0177     /*
0178      * If the devfreq device with passive governor has the specific method
0179      * to determine the next frequency, should use the get_target_freq()
0180      * of struct devfreq_passive_data.
0181      */
0182     if (p_data->get_target_freq)
0183         return p_data->get_target_freq(devfreq, freq);
0184
0185     switch (p_data->parent_type) {
0186     case DEVFREQ_PARENT_DEV:
0187         ret = get_target_freq_with_devfreq(devfreq, freq);
0188         break;
0189     case CPUFREQ_PARENT_DEV:
0190         ret = get_target_freq_with_cpufreq(devfreq, freq);
0191         break;
0192     default:
0193         ret = -EINVAL;
0194         dev_err(&devfreq->dev, "Invalid parent type\n");
0195         break;
0196     }
0197
0198     return ret;
0199 }
0200
0201 static int cpufreq_passive_notifier_call(struct notifier_block *nb,
0202                      unsigned long event, void *ptr)
0203 {
0204     struct devfreq_passive_data *p_data =
0205             container_of(nb, struct devfreq_passive_data, nb);
0206     struct devfreq *devfreq = (struct devfreq *)p_data->this;
0207     struct devfreq_cpu_data *parent_cpu_data;
0208     struct cpufreq_freqs *freqs = ptr;
0209     unsigned int cur_freq;
0210     int ret;
0211
0212     if (event != CPUFREQ_POSTCHANGE || !freqs)
0213         return 0;
0214
0215     parent_cpu_data = get_parent_cpu_data(p_data, freqs->policy);
0216     if (!parent_cpu_data || parent_cpu_data->cur_freq == freqs->new)
0217         return 0;
0218
0219     cur_freq = parent_cpu_data->cur_freq;
0220     parent_cpu_data->cur_freq = freqs->new;
0221
0222     mutex_lock(&devfreq->lock);
0223     ret = devfreq_update_target(devfreq, freqs->new);
0224     mutex_unlock(&devfreq->lock);
0225     if (ret) {
0226         parent_cpu_data->cur_freq = cur_freq;
0227         dev_err(&devfreq->dev, "failed to update the frequency.\n");
0228         return ret;
0229     }
0230
0231     return 0;
0232 }
0233
0234 static int cpufreq_passive_unregister_notifier(struct devfreq *devfreq)
0235 {
0236     struct devfreq_passive_data *p_data
0237             = (struct devfreq_passive_data *)devfreq->data;
0238     int ret;
0239
0240     if (p_data->nb.notifier_call) {
0241         ret = cpufreq_unregister_notifier(&p_data->nb,
0242                     CPUFREQ_TRANSITION_NOTIFIER);
0243         if (ret < 0)
0244             return ret;
0245     }
0246
0247     delete_parent_cpu_data(p_data);
0248
0249     return 0;
0250 }
0251
0252 static int cpufreq_passive_register_notifier(struct devfreq *devfreq)
0253 {
0254     struct devfreq_passive_data *p_data
0255             = (struct devfreq_passive_data *)devfreq->data;
0256     struct device *dev = devfreq->dev.parent;
0257     struct opp_table *opp_table = NULL;
0258     struct devfreq_cpu_data *parent_cpu_data;
0259     struct cpufreq_policy *policy;
0260     struct device *cpu_dev;
0261     unsigned int cpu;
0262     int ret;
0263
0264     p_data->cpu_data_list
0265         = (struct list_head)LIST_HEAD_INIT(p_data->cpu_data_list);
0266
0267     p_data->nb.notifier_call = cpufreq_passive_notifier_call;
0268     ret = cpufreq_register_notifier(&p_data->nb, CPUFREQ_TRANSITION_NOTIFIER);
0269     if (ret) {
0270         dev_err(dev, "failed to register cpufreq notifier\n");
0271         p_data->nb.notifier_call = NULL;
0272         goto err;
0273     }
0274
0275     for_each_possible_cpu(cpu) {
0276         policy = cpufreq_cpu_get(cpu);
0277         if (!policy) {
0278             ret = -EPROBE_DEFER;
0279             goto err;
0280         }
0281
0282         parent_cpu_data = get_parent_cpu_data(p_data, policy);
0283         if (parent_cpu_data) {
0284             cpufreq_cpu_put(policy);
0285             continue;
0286         }
0287
0288         parent_cpu_data = kzalloc(sizeof(*parent_cpu_data),
0289                         GFP_KERNEL);
0290         if (!parent_cpu_data) {
0291             ret = -ENOMEM;
0292             goto err_put_policy;
0293         }
0294
0295         cpu_dev = get_cpu_device(cpu);
0296         if (!cpu_dev) {
0297             dev_err(dev, "failed to get cpu device\n");
0298             ret = -ENODEV;
0299             goto err_free_cpu_data;
0300         }
0301
0302         opp_table = dev_pm_opp_get_opp_table(cpu_dev);
0303         if (IS_ERR(opp_table)) {
0304             dev_err(dev, "failed to get opp_table of cpu%d\n", cpu);
0305             ret = PTR_ERR(opp_table);
0306             goto err_free_cpu_data;
0307         }
0308
0309         parent_cpu_data->dev = cpu_dev;
0310         parent_cpu_data->opp_table = opp_table;
0311         parent_cpu_data->first_cpu = cpumask_first(policy->related_cpus);
0312         parent_cpu_data->cur_freq = policy->cur;
0313         parent_cpu_data->min_freq = policy->cpuinfo.min_freq;
0314         parent_cpu_data->max_freq = policy->cpuinfo.max_freq;
0315
0316         list_add_tail(&parent_cpu_data->node, &p_data->cpu_data_list);
0317         cpufreq_cpu_put(policy);
0318     }
0319
0320     mutex_lock(&devfreq->lock);
0321     ret = devfreq_update_target(devfreq, 0L);
0322     mutex_unlock(&devfreq->lock);
0323     if (ret)
0324         dev_err(dev, "failed to update the frequency\n");
0325
0326     return ret;
0327
0328 err_free_cpu_data:
0329     kfree(parent_cpu_data);
0330 err_put_policy:
0331     cpufreq_cpu_put(policy);
0332 err:
0333
0334     return ret;
0335 }
0336
0337 static int devfreq_passive_notifier_call(struct notifier_block *nb,
0338                 unsigned long event, void *ptr)
0339 {
0340     struct devfreq_passive_data *data
0341             = container_of(nb, struct devfreq_passive_data, nb);
0342     struct devfreq *devfreq = (struct devfreq *)data->this;
0343     struct devfreq *parent = (struct devfreq *)data->parent;
0344     struct devfreq_freqs *freqs = (struct devfreq_freqs *)ptr;
0345     unsigned long freq = freqs->new;
0346     int ret = 0;
0347
0348     mutex_lock_nested(&devfreq->lock, SINGLE_DEPTH_NESTING);
0349     switch (event) {
0350     case DEVFREQ_PRECHANGE:
0351         if (parent->previous_freq > freq)
0352             ret = devfreq_update_target(devfreq, freq);
0353
0354         break;
0355     case DEVFREQ_POSTCHANGE:
0356         if (parent->previous_freq < freq)
0357             ret = devfreq_update_target(devfreq, freq);
0358         break;
0359     }
0360     mutex_unlock(&devfreq->lock);
0361
0362     if (ret < 0)
0363         dev_warn(&devfreq->dev,
0364             "failed to update devfreq using passive governor\n");
0365
0366     return NOTIFY_DONE;
0367 }
0368
0369 static int devfreq_passive_unregister_notifier(struct devfreq *devfreq)
0370 {
0371     struct devfreq_passive_data *p_data
0372             = (struct devfreq_passive_data *)devfreq->data;
0373     struct devfreq *parent = (struct devfreq *)p_data->parent;
0374     struct notifier_block *nb = &p_data->nb;
0375
0376     return devfreq_unregister_notifier(parent, nb, DEVFREQ_TRANSITION_NOTIFIER);
0377 }
0378
0379 static int devfreq_passive_register_notifier(struct devfreq *devfreq)
0380 {
0381     struct devfreq_passive_data *p_data
0382             = (struct devfreq_passive_data *)devfreq->data;
0383     struct devfreq *parent = (struct devfreq *)p_data->parent;
0384     struct notifier_block *nb = &p_data->nb;
0385
0386     if (!parent)
0387         return -EPROBE_DEFER;
0388
0389     nb->notifier_call = devfreq_passive_notifier_call;
0390     return devfreq_register_notifier(parent, nb, DEVFREQ_TRANSITION_NOTIFIER);
0391 }
0392
0393 static int devfreq_passive_event_handler(struct devfreq *devfreq,
0394                 unsigned int event, void *data)
0395 {
0396     struct devfreq_passive_data *p_data
0397             = (struct devfreq_passive_data *)devfreq->data;
0398     int ret = 0;
0399
0400     if (!p_data)
0401         return -EINVAL;
0402
0403     p_data->this = devfreq;
0404
0405     switch (event) {
0406     case DEVFREQ_GOV_START:
0407         if (p_data->parent_type == DEVFREQ_PARENT_DEV)
0408             ret = devfreq_passive_register_notifier(devfreq);
0409         else if (p_data->parent_type == CPUFREQ_PARENT_DEV)
0410             ret = cpufreq_passive_register_notifier(devfreq);
0411         break;
0412     case DEVFREQ_GOV_STOP:
0413         if (p_data->parent_type == DEVFREQ_PARENT_DEV)
0414             WARN_ON(devfreq_passive_unregister_notifier(devfreq));
0415         else if (p_data->parent_type == CPUFREQ_PARENT_DEV)
0416             WARN_ON(cpufreq_passive_unregister_notifier(devfreq));
0417         break;
0418     default:
0419         break;
0420     }
0421
0422     return ret;
0423 }
0424
0425 static struct devfreq_governor devfreq_passive = {
0426     .name = DEVFREQ_GOV_PASSIVE,
0427     .flags = DEVFREQ_GOV_FLAG_IMMUTABLE,
0428     .get_target_freq = devfreq_passive_get_target_freq,
0429     .event_handler = devfreq_passive_event_handler,
0430 };
0431
0432 static int __init devfreq_passive_init(void)
0433 {
0434     return devfreq_add_governor(&devfreq_passive);
0435 }
0436 subsys_initcall(devfreq_passive_init);
0437
0438 static void __exit devfreq_passive_exit(void)
0439 {
0440     int ret;
0441
0442     ret = devfreq_remove_governor(&devfreq_passive);
0443     if (ret)
0444         pr_err("%s: failed remove governor %d\n", __func__, ret);
0445 }
0446 module_exit(devfreq_passive_exit);
0447
0448 MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>");
0449 MODULE_AUTHOR("MyungJoo Ham <myungjoo.ham@samsung.com>");
0450 MODULE_DESCRIPTION("DEVFREQ Passive governor");
0451 MODULE_LICENSE("GPL v2");