// SPDX-License-Identifier: GPL-2.0-only /* * Copyright (c) 2023, NVIDIA Corporation. All rights reserved. */ #include #include #include #include #include #include #include #include #include #if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 19, 0) #include #else #include #endif /** * struct tegra_wmark_data - governor private data stored in struct devfreq * @load_target: Frequency scaling logic will try to keep the device * running at the specified load under specific frequency. * The average watermark thresholds of the actmon should be * calculated with this value: * $$ * avg_x_wmark = freq_x * load_target / 1000 * $$ * where 'x' can be 'upper' or 'lower' in the formula, and * the valid value of 'load_target' ranges from 0 to 1000. * @up_wmark_margin: Offset value that will be applied to load_target for the * consecutive upper watermark threshold, which means the * formula becomes: * $$ * consec_upper_wmark = * freq_upper * (load_target + up_wmark_margin) / 1000 * $$ * @down_wmark_margin: Offset value that will be applied to load_target for the * consecutive lower watermark threshold, which means the * formula becomes: * $$ * consec_lower_wmark = * freq_lower * (load_target - down_wmark_margin) / 1000 * $$ * @up_freq_margin: Number of frequency steps for scaling up the frequency * when consecutive upper watermark interrupt get triggered. * @down_freq_margin: Number of frequency steps for scaling down the frequency * when consecutive lower watermark interrupt get triggered. * @curr_freq_index: Index value of current frequency in the frequency table. * @df: The devfreq instance of own device. * @nb: Notifier block for DEVFREQ_TRANSITION_NOTIFIER list. */ struct tegra_wmark_data { unsigned int load_target; unsigned int up_wmark_margin; unsigned int down_wmark_margin; unsigned int up_freq_margin; unsigned int down_freq_margin; int curr_freq_index; struct devfreq *df; struct notifier_block nb; }; static int devfreq_get_freq_index(struct devfreq *df, unsigned long freq) { int i; for (i = 0; i < df->profile->max_state; i++) { if (df->profile->freq_table[i] >= freq) break; } return i; } static int devfreq_tegra_wmark_target_freq(struct devfreq *df, unsigned long *freq) { struct tegra_wmark_data *govdata = df->governor_data; struct devfreq_tegra_wmark_data *drvdata = df->data; int target_index = 0; switch (drvdata->event) { case DEVFREQ_TEGRA_AVG_WMARK_BELOW: target_index = max_t(int, 0, govdata->curr_freq_index-1); break; case DEVFREQ_TEGRA_AVG_WMARK_ABOVE: target_index = min_t(int, df->profile->max_state-1, govdata->curr_freq_index+1); break; case DEVFREQ_TEGRA_CONSEC_WMARK_BELOW: target_index = max_t(int, 0, govdata->curr_freq_index-govdata->down_freq_margin); break; case DEVFREQ_TEGRA_CONSEC_WMARK_ABOVE: target_index = min_t(int, df->profile->max_state-1, govdata->curr_freq_index + govdata->up_freq_margin); break; default: break; } *freq = df->profile->freq_table[target_index]; return 0; } #if LINUX_VERSION_CODE < KERNEL_VERSION(5, 19, 0) static s32 devfreq_pm_qos_read_value(struct devfreq *df, enum dev_pm_qos_req_type type) { struct device *dev = df->dev.parent; struct dev_pm_qos *pm_qos = dev->power.qos; s32 ret = 0; unsigned long flags; spin_lock_irqsave(&dev->power.lock, flags); switch (type) { case DEV_PM_QOS_MIN_FREQUENCY: ret = IS_ERR_OR_NULL(pm_qos) ? PM_QOS_MIN_FREQUENCY_DEFAULT_VALUE : READ_ONCE(pm_qos->freq.min_freq.target_value); break; case DEV_PM_QOS_MAX_FREQUENCY: ret = IS_ERR_OR_NULL(pm_qos) ? PM_QOS_MAX_FREQUENCY_DEFAULT_VALUE : READ_ONCE(pm_qos->freq.max_freq.target_value); break; default: break; } spin_unlock_irqrestore(&dev->power.lock, flags); return ret; } static void devfreq_get_freq_range(struct devfreq *df, unsigned long *min_freq, unsigned long *max_freq) { s32 qos_min_freq, qos_max_freq; *min_freq = df->profile->freq_table[0]; *max_freq = df->profile->freq_table[df->profile->max_state - 1]; qos_min_freq = devfreq_pm_qos_read_value(df, DEV_PM_QOS_MIN_FREQUENCY); qos_max_freq = devfreq_pm_qos_read_value(df, DEV_PM_QOS_MAX_FREQUENCY); /* Apply constraints from PM QoS */ *min_freq = max(*min_freq, (unsigned long)qos_min_freq * 1000); if (qos_max_freq != PM_QOS_MAX_FREQUENCY_DEFAULT_VALUE) *max_freq = min(*max_freq, (unsigned long)qos_max_freq * 1000); /* Apply constraints from OPP framework */ *min_freq = max(*min_freq, df->scaling_min_freq); *max_freq = min(*max_freq, df->scaling_max_freq); } #endif static void devfreq_update_wmark_threshold(struct devfreq *df) { struct tegra_wmark_data *govdata = df->governor_data; struct devfreq_tegra_wmark_data *drvdata = df->data; struct devfreq_tegra_wmark_config wmark_config; unsigned long curr_freq, prev_freq, min_freq, max_freq; int err; devfreq_get_freq_range(df, &min_freq, &max_freq); err = devfreq_update_stats(df); if (err) return; govdata->curr_freq_index = devfreq_get_freq_index(df, df->last_status.current_frequency); curr_freq = df->profile->freq_table[govdata->curr_freq_index]; if (curr_freq < max_freq) { wmark_config.avg_upper_wmark = curr_freq / 1000 * govdata->load_target; wmark_config.consec_upper_wmark = wmark_config.avg_upper_wmark + (curr_freq / 1000 * govdata->up_wmark_margin); wmark_config.upper_wmark_enabled = 1; } else wmark_config.upper_wmark_enabled = 0; if (curr_freq > min_freq) { prev_freq = max_t(unsigned long, df->profile->freq_table[max_t(int, govdata->curr_freq_index-1, 0)], min_freq); wmark_config.avg_lower_wmark = prev_freq / 1000 * govdata->load_target; wmark_config.consec_lower_wmark = wmark_config.avg_lower_wmark - (prev_freq / 1000 * govdata->down_wmark_margin); wmark_config.lower_wmark_enabled = 1; } else wmark_config.lower_wmark_enabled = 0; drvdata->update_wmark_threshold(df, &wmark_config); } static ssize_t up_freq_margin_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct devfreq *df = to_devfreq(dev); struct tegra_wmark_data *govdata; unsigned int freq_margin; int ret; ret = kstrtouint(buf, 0, &freq_margin); if (ret) return ret; freq_margin = min_t(unsigned int, freq_margin, df->profile->max_state); mutex_lock(&df->lock); govdata = df->governor_data; govdata->up_freq_margin = freq_margin; mutex_unlock(&df->lock); return count; } static ssize_t up_freq_margin_show(struct device *dev, struct device_attribute *attr, char *buf) { struct devfreq *df = to_devfreq(dev); struct tegra_wmark_data *govdata; int err; mutex_lock(&df->lock); govdata = df->governor_data; err = sprintf(buf, "%u\n", govdata->up_freq_margin); mutex_unlock(&df->lock); return err; } static DEVICE_ATTR_RW(up_freq_margin); static ssize_t down_freq_margin_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct devfreq *df = to_devfreq(dev); struct tegra_wmark_data *govdata; unsigned int freq_margin; int ret; ret = kstrtouint(buf, 0, &freq_margin); if (ret) return ret; freq_margin = min_t(unsigned int, freq_margin, df->profile->max_state); mutex_lock(&df->lock); govdata = df->governor_data; govdata->down_freq_margin = freq_margin; mutex_unlock(&df->lock); return count; } static ssize_t down_freq_margin_show(struct device *dev, struct device_attribute *attr, char *buf) { struct devfreq *df = to_devfreq(dev); struct tegra_wmark_data *govdata; int err; mutex_lock(&df->lock); govdata = df->governor_data; err = sprintf(buf, "%u\n", govdata->down_freq_margin); mutex_unlock(&df->lock); return err; } static DEVICE_ATTR_RW(down_freq_margin); static ssize_t up_wmark_margin_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct devfreq *df = to_devfreq(dev); struct tegra_wmark_data *govdata; unsigned int wmark_margin; int ret; ret = kstrtouint(buf, 0, &wmark_margin); if (ret) return ret; wmark_margin = min_t(unsigned int, wmark_margin, 1000); mutex_lock(&df->lock); govdata = df->governor_data; govdata->up_wmark_margin = wmark_margin; mutex_unlock(&df->lock); devfreq_update_wmark_threshold(df); return count; } static ssize_t up_wmark_margin_show(struct device *dev, struct device_attribute *attr, char *buf) { struct devfreq *df = to_devfreq(dev); struct tegra_wmark_data *govdata; int err; mutex_lock(&df->lock); govdata = df->governor_data; err = sprintf(buf, "%u\n", govdata->up_wmark_margin); mutex_unlock(&df->lock); return err; } static DEVICE_ATTR_RW(up_wmark_margin); static ssize_t down_wmark_margin_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct devfreq *df = to_devfreq(dev); struct tegra_wmark_data *govdata; unsigned int wmark_margin; int ret; ret = kstrtouint(buf, 0, &wmark_margin); if (ret) return ret; wmark_margin = min_t(unsigned int, wmark_margin, 1000); mutex_lock(&df->lock); govdata = df->governor_data; govdata->down_wmark_margin = wmark_margin; mutex_unlock(&df->lock); devfreq_update_wmark_threshold(df); return count; } static ssize_t down_wmark_margin_show(struct device *dev, struct device_attribute *attr, char *buf) { struct devfreq *df = to_devfreq(dev); struct tegra_wmark_data *govdata; int err; mutex_lock(&df->lock); govdata = df->governor_data; err = sprintf(buf, "%u\n", govdata->down_wmark_margin); mutex_unlock(&df->lock); return err; } static DEVICE_ATTR_RW(down_wmark_margin); static ssize_t load_target_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct devfreq *df = to_devfreq(dev); struct tegra_wmark_data *govdata; unsigned int load_target; int ret; ret = kstrtouint(buf, 0, &load_target); if (ret) return ret; load_target = min_t(unsigned int, load_target, 1000); mutex_lock(&df->lock); govdata = df->governor_data; govdata->load_target = load_target; mutex_unlock(&df->lock); devfreq_update_wmark_threshold(df); return count; } static ssize_t load_target_show(struct device *dev, struct device_attribute *attr, char *buf) { struct devfreq *df = to_devfreq(dev); struct tegra_wmark_data *govdata; int err; mutex_lock(&df->lock); govdata = df->governor_data; err = sprintf(buf, "%u\n", govdata->load_target); mutex_unlock(&df->lock); return err; } static DEVICE_ATTR_RW(load_target); static struct attribute *dev_entries[] = { &dev_attr_load_target.attr, &dev_attr_up_wmark_margin.attr, &dev_attr_down_wmark_margin.attr, &dev_attr_up_freq_margin.attr, &dev_attr_down_freq_margin.attr, NULL, }; static struct attribute_group dev_attr_group = { .name = DEVFREQ_GOV_TEGRA_WMARK, .attrs = dev_entries, }; static int devfreq_tegra_wmark_notifier_call(struct notifier_block *nb, unsigned long event, void *ptr) { struct tegra_wmark_data *govdata = container_of(nb, struct tegra_wmark_data, nb); switch (event) { case DEVFREQ_POSTCHANGE: devfreq_update_wmark_threshold(govdata->df); break; default: break; }; return NOTIFY_DONE; } static int tegra_wmark_init(struct devfreq *df) { struct tegra_wmark_data *govdata; struct devfreq_tegra_wmark_data *drvdata; int err; if (!df->data) return -EINVAL; drvdata = df->data; if (!drvdata->update_wmark_threshold) return -EINVAL; govdata = kzalloc(sizeof(*govdata), GFP_KERNEL); if (!govdata) return -ENOMEM; govdata->load_target = 800; govdata->up_wmark_margin = 100; govdata->down_wmark_margin = 100; govdata->up_freq_margin = 4; govdata->down_freq_margin = 1; govdata->curr_freq_index = 0; govdata->df = df; df->governor_data = govdata; govdata->nb.notifier_call = devfreq_tegra_wmark_notifier_call; err = devfreq_register_notifier(df, &govdata->nb, DEVFREQ_TRANSITION_NOTIFIER); if (err) goto out_register_notifier; err = sysfs_create_group(&df->dev.kobj, &dev_attr_group); if (err) goto out_create_sysfs; return err; out_create_sysfs: devfreq_unregister_notifier(df, &govdata->nb, DEVFREQ_TRANSITION_NOTIFIER); out_register_notifier: kfree(df->governor_data); df->governor_data = NULL; return err; } static void tegra_wmark_exit(struct devfreq *df) { struct tegra_wmark_data *govdata = df->governor_data; devfreq_unregister_notifier(df, &govdata->nb, DEVFREQ_TRANSITION_NOTIFIER); sysfs_remove_group(&df->dev.kobj, &dev_attr_group); kfree(df->governor_data); df->governor_data = NULL; } static int devfreq_tegra_wmark_event_handler(struct devfreq *df, unsigned int event, void *data) { struct devfreq_tegra_wmark_data *drvdata = df->data; struct devfreq_tegra_wmark_config wmark_config; int err; switch (event) { case DEVFREQ_GOV_START: err = tegra_wmark_init(df); if (err) return err; devfreq_update_wmark_threshold(df); break; case DEVFREQ_GOV_STOP: wmark_config.upper_wmark_enabled = 0; wmark_config.lower_wmark_enabled = 0; drvdata->update_wmark_threshold(df, &wmark_config); tegra_wmark_exit(df); break; case DEVFREQ_GOV_SUSPEND: wmark_config.upper_wmark_enabled = 0; wmark_config.lower_wmark_enabled = 0; drvdata->update_wmark_threshold(df, &wmark_config); break; case DEVFREQ_GOV_RESUME: devfreq_update_wmark_threshold(df); break; default: break; } return 0; } static struct devfreq_governor devfreq_tegra_wmark = { .name = DEVFREQ_GOV_TEGRA_WMARK, .flags = DEVFREQ_GOV_FLAG_IRQ_DRIVEN, .get_target_freq = devfreq_tegra_wmark_target_freq, .event_handler = devfreq_tegra_wmark_event_handler, }; static int __init devfreq_tegra_wmark_init(void) { return devfreq_add_governor(&devfreq_tegra_wmark); } subsys_initcall(devfreq_tegra_wmark_init); static void __exit devfreq_tegra_wmark_exit(void) { devfreq_remove_governor(&devfreq_tegra_wmark); } module_exit(devfreq_tegra_wmark_exit); MODULE_AUTHOR("Johnny Liu "); MODULE_LICENSE("GPL v2");