1// SPDX-License-Identifier: GPL-2.0-only
  2/*
  3 * Copyright 2021 Linaro Limited
  4 *
  5 * Author: Daniel Lezcano <daniel.lezcano@linaro.org>
  6 *
  7 * The devfreq device combined with the energy model and the load can
  8 * give an estimation of the power consumption as well as limiting the
  9 * power.
 10 *
 11 */
 12#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 13
 14#include <linux/cpumask.h>
 15#include <linux/devfreq.h>
 16#include <linux/dtpm.h>
 17#include <linux/energy_model.h>
 18#include <linux/of.h>
 19#include <linux/pm_qos.h>
 20#include <linux/slab.h>
 21#include <linux/units.h>
 22
 23struct dtpm_devfreq {
 24	struct dtpm dtpm;
 25	struct dev_pm_qos_request qos_req;
 26	struct devfreq *devfreq;
 27};
 28
 29static struct dtpm_devfreq *to_dtpm_devfreq(struct dtpm *dtpm)
 30{
 31	return container_of(dtpm, struct dtpm_devfreq, dtpm);
 32}
 33
 34static int update_pd_power_uw(struct dtpm *dtpm)
 35{
 36	struct dtpm_devfreq *dtpm_devfreq = to_dtpm_devfreq(dtpm);
 37	struct devfreq *devfreq = dtpm_devfreq->devfreq;
 38	struct device *dev = devfreq->dev.parent;
 39	struct em_perf_domain *pd = em_pd_get(dev);
 40
 41	dtpm->power_min = pd->table[0].power;
 42
 43	dtpm->power_max = pd->table[pd->nr_perf_states - 1].power;
 44
 45	return 0;
 46}
 47
 48static u64 set_pd_power_limit(struct dtpm *dtpm, u64 power_limit)
 49{
 50	struct dtpm_devfreq *dtpm_devfreq = to_dtpm_devfreq(dtpm);
 51	struct devfreq *devfreq = dtpm_devfreq->devfreq;
 52	struct device *dev = devfreq->dev.parent;
 53	struct em_perf_domain *pd = em_pd_get(dev);
 54	unsigned long freq;
 55	int i;
 56
 57	for (i = 0; i < pd->nr_perf_states; i++) {
 58		if (pd->table[i].power > power_limit)
 59			break;
 60	}
 61
 62	freq = pd->table[i - 1].frequency;
 63
 64	dev_pm_qos_update_request(&dtpm_devfreq->qos_req, freq);
 65
 66	power_limit = pd->table[i - 1].power;
 67
 68	return power_limit;
 69}
 70
 71static void _normalize_load(struct devfreq_dev_status *status)
 72{
 73	if (status->total_time > 0xfffff) {
 74		status->total_time >>= 10;
 75		status->busy_time >>= 10;
 76	}
 77
 78	status->busy_time <<= 10;
 79	status->busy_time /= status->total_time ? : 1;
 80
 81	status->busy_time = status->busy_time ? : 1;
 82	status->total_time = 1024;
 83}
 84
 85static u64 get_pd_power_uw(struct dtpm *dtpm)
 86{
 87	struct dtpm_devfreq *dtpm_devfreq = to_dtpm_devfreq(dtpm);
 88	struct devfreq *devfreq = dtpm_devfreq->devfreq;
 89	struct device *dev = devfreq->dev.parent;
 90	struct em_perf_domain *pd = em_pd_get(dev);
 91	struct devfreq_dev_status status;
 92	unsigned long freq;
 93	u64 power;
 94	int i;
 95
 96	mutex_lock(&devfreq->lock);
 97	status = devfreq->last_status;
 98	mutex_unlock(&devfreq->lock);
 99
100	freq = DIV_ROUND_UP(status.current_frequency, HZ_PER_KHZ);
101	_normalize_load(&status);
102
103	for (i = 0; i < pd->nr_perf_states; i++) {
104
105		if (pd->table[i].frequency < freq)
106			continue;
107
108		power = pd->table[i].power;
109		power *= status.busy_time;
110		power >>= 10;
111
112		return power;
113	}
114
115	return 0;
116}
117
118static void pd_release(struct dtpm *dtpm)
119{
120	struct dtpm_devfreq *dtpm_devfreq = to_dtpm_devfreq(dtpm);
121
122	if (dev_pm_qos_request_active(&dtpm_devfreq->qos_req))
123		dev_pm_qos_remove_request(&dtpm_devfreq->qos_req);
124
125	kfree(dtpm_devfreq);
126}
127
128static struct dtpm_ops dtpm_ops = {
129	.set_power_uw = set_pd_power_limit,
130	.get_power_uw = get_pd_power_uw,
131	.update_power_uw = update_pd_power_uw,
132	.release = pd_release,
133};
134
135static int __dtpm_devfreq_setup(struct devfreq *devfreq, struct dtpm *parent)
136{
137	struct device *dev = devfreq->dev.parent;
138	struct dtpm_devfreq *dtpm_devfreq;
139	struct em_perf_domain *pd;
140	int ret = -ENOMEM;
141
142	pd = em_pd_get(dev);
143	if (!pd) {
144		ret = dev_pm_opp_of_register_em(dev, NULL);
145		if (ret) {
146			pr_err("No energy model available for '%s'\n", dev_name(dev));
147			return -EINVAL;
148		}
149	}
150
151	dtpm_devfreq = kzalloc(sizeof(*dtpm_devfreq), GFP_KERNEL);
152	if (!dtpm_devfreq)
153		return -ENOMEM;
154
155	dtpm_init(&dtpm_devfreq->dtpm, &dtpm_ops);
156
157	dtpm_devfreq->devfreq = devfreq;
158
159	ret = dtpm_register(dev_name(dev), &dtpm_devfreq->dtpm, parent);
160	if (ret) {
161		pr_err("Failed to register '%s': %d\n", dev_name(dev), ret);
162		kfree(dtpm_devfreq);
163		return ret;
164	}
165
166	ret = dev_pm_qos_add_request(dev, &dtpm_devfreq->qos_req,
167				     DEV_PM_QOS_MAX_FREQUENCY,
168				     PM_QOS_MAX_FREQUENCY_DEFAULT_VALUE);
169	if (ret) {
170		pr_err("Failed to add QoS request: %d\n", ret);
171		goto out_dtpm_unregister;
172	}
173
174	dtpm_update_power(&dtpm_devfreq->dtpm);
175
176	return 0;
177
178out_dtpm_unregister:
179	dtpm_unregister(&dtpm_devfreq->dtpm);
180
181	return ret;
182}
183
184static int dtpm_devfreq_setup(struct dtpm *dtpm, struct device_node *np)
185{
186	struct devfreq *devfreq;
187
188	devfreq = devfreq_get_devfreq_by_node(np);
189	if (IS_ERR(devfreq))
190		return 0;
191
192	return __dtpm_devfreq_setup(devfreq, dtpm);
193}
194
195struct dtpm_subsys_ops dtpm_devfreq_ops = {
196	.name = KBUILD_MODNAME,
197	.setup = dtpm_devfreq_setup,
198};