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"); 452