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