| // SPDX-License-Identifier: GPL-2.0-only |
| /* |
| * Copyright 2021 Linaro Limited |
| * |
| * Author: Daniel Lezcano <daniel.lezcano@linaro.org> |
| * |
| * The devfreq device combined with the energy model and the load can |
| * give an estimation of the power consumption as well as limiting the |
| * power. |
| * |
| */ |
| #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt |
| |
| #include <linux/cpumask.h> |
| #include <linux/devfreq.h> |
| #include <linux/dtpm.h> |
| #include <linux/energy_model.h> |
| #include <linux/of.h> |
| #include <linux/pm_qos.h> |
| #include <linux/slab.h> |
| #include <linux/units.h> |
| |
| struct dtpm_devfreq { |
| struct dtpm dtpm; |
| struct dev_pm_qos_request qos_req; |
| struct devfreq *devfreq; |
| }; |
| |
| static struct dtpm_devfreq *to_dtpm_devfreq(struct dtpm *dtpm) |
| { |
| return container_of(dtpm, struct dtpm_devfreq, dtpm); |
| } |
| |
| static int update_pd_power_uw(struct dtpm *dtpm) |
| { |
| struct dtpm_devfreq *dtpm_devfreq = to_dtpm_devfreq(dtpm); |
| struct devfreq *devfreq = dtpm_devfreq->devfreq; |
| struct device *dev = devfreq->dev.parent; |
| struct em_perf_domain *pd = em_pd_get(dev); |
| struct em_perf_state *table; |
| |
| rcu_read_lock(); |
| table = em_perf_state_from_pd(pd); |
| |
| dtpm->power_min = table[0].power; |
| |
| dtpm->power_max = table[pd->nr_perf_states - 1].power; |
| |
| rcu_read_unlock(); |
| return 0; |
| } |
| |
| static u64 set_pd_power_limit(struct dtpm *dtpm, u64 power_limit) |
| { |
| struct dtpm_devfreq *dtpm_devfreq = to_dtpm_devfreq(dtpm); |
| struct devfreq *devfreq = dtpm_devfreq->devfreq; |
| struct device *dev = devfreq->dev.parent; |
| struct em_perf_domain *pd = em_pd_get(dev); |
| struct em_perf_state *table; |
| unsigned long freq; |
| int i; |
| |
| rcu_read_lock(); |
| table = em_perf_state_from_pd(pd); |
| for (i = 0; i < pd->nr_perf_states; i++) { |
| if (table[i].power > power_limit) |
| break; |
| } |
| |
| freq = table[i - 1].frequency; |
| power_limit = table[i - 1].power; |
| rcu_read_unlock(); |
| |
| dev_pm_qos_update_request(&dtpm_devfreq->qos_req, freq); |
| |
| return power_limit; |
| } |
| |
| static void _normalize_load(struct devfreq_dev_status *status) |
| { |
| if (status->total_time > 0xfffff) { |
| status->total_time >>= 10; |
| status->busy_time >>= 10; |
| } |
| |
| status->busy_time <<= 10; |
| status->busy_time /= status->total_time ? : 1; |
| |
| status->busy_time = status->busy_time ? : 1; |
| status->total_time = 1024; |
| } |
| |
| static u64 get_pd_power_uw(struct dtpm *dtpm) |
| { |
| struct dtpm_devfreq *dtpm_devfreq = to_dtpm_devfreq(dtpm); |
| struct devfreq *devfreq = dtpm_devfreq->devfreq; |
| struct device *dev = devfreq->dev.parent; |
| struct em_perf_domain *pd = em_pd_get(dev); |
| struct devfreq_dev_status status; |
| struct em_perf_state *table; |
| unsigned long freq; |
| u64 power = 0; |
| int i; |
| |
| mutex_lock(&devfreq->lock); |
| status = devfreq->last_status; |
| mutex_unlock(&devfreq->lock); |
| |
| freq = DIV_ROUND_UP(status.current_frequency, HZ_PER_KHZ); |
| _normalize_load(&status); |
| |
| rcu_read_lock(); |
| table = em_perf_state_from_pd(pd); |
| for (i = 0; i < pd->nr_perf_states; i++) { |
| |
| if (table[i].frequency < freq) |
| continue; |
| |
| power = table[i].power; |
| power *= status.busy_time; |
| power >>= 10; |
| |
| break; |
| } |
| rcu_read_unlock(); |
| |
| return power; |
| } |
| |
| static void pd_release(struct dtpm *dtpm) |
| { |
| struct dtpm_devfreq *dtpm_devfreq = to_dtpm_devfreq(dtpm); |
| |
| if (dev_pm_qos_request_active(&dtpm_devfreq->qos_req)) |
| dev_pm_qos_remove_request(&dtpm_devfreq->qos_req); |
| |
| kfree(dtpm_devfreq); |
| } |
| |
| static struct dtpm_ops dtpm_ops = { |
| .set_power_uw = set_pd_power_limit, |
| .get_power_uw = get_pd_power_uw, |
| .update_power_uw = update_pd_power_uw, |
| .release = pd_release, |
| }; |
| |
| static int __dtpm_devfreq_setup(struct devfreq *devfreq, struct dtpm *parent) |
| { |
| struct device *dev = devfreq->dev.parent; |
| struct dtpm_devfreq *dtpm_devfreq; |
| struct em_perf_domain *pd; |
| int ret = -ENOMEM; |
| |
| pd = em_pd_get(dev); |
| if (!pd) { |
| ret = dev_pm_opp_of_register_em(dev, NULL); |
| if (ret) { |
| pr_err("No energy model available for '%s'\n", dev_name(dev)); |
| return -EINVAL; |
| } |
| } |
| |
| dtpm_devfreq = kzalloc(sizeof(*dtpm_devfreq), GFP_KERNEL); |
| if (!dtpm_devfreq) |
| return -ENOMEM; |
| |
| dtpm_init(&dtpm_devfreq->dtpm, &dtpm_ops); |
| |
| dtpm_devfreq->devfreq = devfreq; |
| |
| ret = dtpm_register(dev_name(dev), &dtpm_devfreq->dtpm, parent); |
| if (ret) { |
| pr_err("Failed to register '%s': %d\n", dev_name(dev), ret); |
| kfree(dtpm_devfreq); |
| return ret; |
| } |
| |
| ret = dev_pm_qos_add_request(dev, &dtpm_devfreq->qos_req, |
| DEV_PM_QOS_MAX_FREQUENCY, |
| PM_QOS_MAX_FREQUENCY_DEFAULT_VALUE); |
| if (ret < 0) { |
| pr_err("Failed to add QoS request: %d\n", ret); |
| goto out_dtpm_unregister; |
| } |
| |
| dtpm_update_power(&dtpm_devfreq->dtpm); |
| |
| return 0; |
| |
| out_dtpm_unregister: |
| dtpm_unregister(&dtpm_devfreq->dtpm); |
| |
| return ret; |
| } |
| |
| static int dtpm_devfreq_setup(struct dtpm *dtpm, struct device_node *np) |
| { |
| struct devfreq *devfreq; |
| |
| devfreq = devfreq_get_devfreq_by_node(np); |
| if (IS_ERR(devfreq)) |
| return 0; |
| |
| return __dtpm_devfreq_setup(devfreq, dtpm); |
| } |
| |
| struct dtpm_subsys_ops dtpm_devfreq_ops = { |
| .name = KBUILD_MODNAME, |
| .setup = dtpm_devfreq_setup, |
| }; |