drivers/devfreq/governor_staticmap.c - kernel/msm.git - Git at Google

 // SPDX-License-Identifier: GPL-2.0-only
 /*
  * Copyright (c) 2019, The Linux Foundation. All rights reserved.
  */

 #define pr_fmt(fmt) "governor-static-map: " fmt

 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/err.h>
 #include <linux/errno.h>
 #include <linux/platform_device.h>
 #include <linux/of.h>
 #include <linux/clk.h>
 #include <linux/devfreq.h>
 #include "governor.h"

 struct core_dev_map {
 	unsigned int core_mhz;
 	unsigned int target_freq;
 };

 struct static_map_gov {
 	struct device		*dev;
 	struct device_node	*of_node;
 	struct clk		*dev_clk;
 	unsigned long		dev_clk_cur_freq;
 	struct notifier_block	clock_change_nb;
 	struct core_dev_map	*freq_map;
 	struct devfreq_governor	*gov;
 	struct devfreq		*df;
 	bool			mon_started;
 	struct list_head	list;
 	void			*orig_data;
 	unsigned long		resume_freq;
 };

 static LIST_HEAD(static_map_list);
 static DEFINE_MUTEX(static_map_lock);
 static DEFINE_MUTEX(state_lock);
 static int static_use_cnt;

 static struct static_map_gov *find_static_map_node(struct devfreq *df)
 {
 	struct static_map_gov *node, *found = NULL;

 	mutex_lock(&static_map_lock);
 	list_for_each_entry(node, &static_map_list, list)
 		if (node->of_node == df->dev.parent->of_node) {
 			found = node;
 			break;
 		}
 	mutex_unlock(&static_map_lock);

 	return found;
 }

 static unsigned long core_to_dev_freq(struct static_map_gov *d,
 		unsigned long coref)
 {
 	struct core_dev_map *map = d->freq_map;
 	unsigned long freq = 0;

 	if (!map || !coref)
 		goto out;

 	/* Start with the first non-zero freq map entry */
 	map++;
 	while (map->core_mhz && map->core_mhz != coref)
 		map++;
 	if (!map->core_mhz)
 		map--;
 	freq = map->target_freq;

 out:
 	pr_debug("core freq: %lu -> target: %lu\n", coref, freq);
 	return freq;
 }

 #define NUM_COLS	2
 static struct core_dev_map *init_core_dev_map(struct device *dev,
 					struct device_node *of_node,
 					char *prop_name)
 {
 	int len, nf, i, j;
 	u32 data;
 	struct core_dev_map *tbl;
 	int ret;

 	if (!of_node)
 		of_node = dev->of_node;

 	if (!of_find_property(of_node, prop_name, &len))
 		return NULL;
 	len /= sizeof(data);

 	if (len % NUM_COLS || len == 0)
 		return NULL;
 	nf = len / NUM_COLS;

 	tbl = devm_kzalloc(dev, (nf + 1) * sizeof(struct core_dev_map),
 			GFP_KERNEL);
 	if (!tbl)
 		return NULL;

 	for (i = 0, j = 0; i < nf; i++, j += 2) {
 		ret = of_property_read_u32_index(of_node, prop_name, j,
 				&data);
 		if (ret) {
 			dev_err(dev,
 				"Couldn't read the core-dev freq table %d\n",
 									ret);
 			return NULL;
 		}
 		tbl[i].core_mhz = data;

 		ret = of_property_read_u32_index(of_node, prop_name, j + 1,
 				&data);
 		if (ret) {
 			dev_err(dev,
 				"Couldn't read the core-dev freq table %d\n",
 									ret);
 			return NULL;
 		}
 		tbl[i].target_freq = data;
 		pr_debug("Entry%d DEV:%u, Target:%u\n", i, tbl[i].core_mhz,
 				tbl[i].target_freq);
 	}
 	tbl[i].core_mhz = 0;

 	return tbl;
 }
 static int devfreq_static_map_get_freq(struct devfreq *df,
 					unsigned long *freq)
 {
 	struct static_map_gov *gov_node = df->data;

 	*freq = core_to_dev_freq(gov_node, gov_node->dev_clk_cur_freq);

 	return 0;
 }
 static int devfreq_clock_change_notify_cb(struct notifier_block *nb,
 				       unsigned long action, void *ptr)
 {
 	struct clk_notifier_data *data = ptr;
 	struct static_map_gov *d;
 	int ret;

 	if (action != POST_RATE_CHANGE)
 		return NOTIFY_OK;

 	mutex_lock(&state_lock);
 	d = container_of(nb, struct static_map_gov, clock_change_nb);

 	mutex_lock(&d->df->lock);
 	d->dev_clk_cur_freq = data->new_rate;
 	if (IS_ERR_VALUE(d->dev_clk_cur_freq)) {
 		mutex_unlock(&d->df->lock);
 		mutex_unlock(&state_lock);
 		return d->dev_clk_cur_freq;
 	}
 	d->dev_clk_cur_freq = d->dev_clk_cur_freq / 1000;

 	ret = update_devfreq(d->df);
 	if (ret)
 		dev_err(d->dev,
 			"Unable to update freq on request %d\n", ret);
 	mutex_unlock(&d->df->lock);
 	mutex_unlock(&state_lock);

 	return 0;
 }

 static int devfreq_static_map_ev_handler(struct devfreq *df,
 					unsigned int event, void *data)
 {
 	int ret = 0;
 	struct static_map_gov *gov_node;

 	mutex_lock(&state_lock);
 	gov_node = find_static_map_node(df);
 	if (!gov_node) {
 		mutex_unlock(&state_lock);
 		dev_err(df->dev.parent,
 				"Unable to find static map governor!\n");
 		return -ENODEV;
 	}

 	switch (event) {
 	case DEVFREQ_GOV_START:
 		gov_node->clock_change_nb.notifier_call =
 						devfreq_clock_change_notify_cb;
 		gov_node->orig_data = df->data;
 		gov_node->df = df;
 		df->data = gov_node;
 		ret = clk_notifier_register(gov_node->dev_clk,
 					&gov_node->clock_change_nb);
 		if (ret) {
 			dev_err(df->dev.parent,
 				"Failed to register clock change notifier %d\n",
 									ret);
 		}
 		break;
 	case DEVFREQ_GOV_STOP:
 		ret = clk_notifier_unregister(gov_node->dev_clk,
 						&gov_node->clock_change_nb);
 		if (ret) {
 			dev_err(df->dev.parent,
 				"Failed to register clock change notifier %d\n",
 									ret);
 		}
 		df->data = gov_node->orig_data;
 		gov_node->orig_data = NULL;
 		break;
 	case DEVFREQ_GOV_SUSPEND:
 		ret = clk_notifier_unregister(gov_node->dev_clk,
 						&gov_node->clock_change_nb);
 		if (ret) {
 			dev_err(df->dev.parent,
 				"Failed to unregister clk notifier %d\n", ret);
 		}
 		mutex_lock(&df->lock);
 		gov_node->resume_freq = gov_node->dev_clk_cur_freq;
 		gov_node->dev_clk_cur_freq = 0;
 		update_devfreq(df);
 		mutex_unlock(&df->lock);
 		break;
 	case DEVFREQ_GOV_RESUME:
 		ret = clk_notifier_register(gov_node->dev_clk,
 						&gov_node->clock_change_nb);
 		if (ret) {
 			dev_err(df->dev.parent,
 				"Failed to register clock change notifier %d\n",
 									ret);
 		}
 		mutex_lock(&df->lock);
 		gov_node->dev_clk_cur_freq = gov_node->resume_freq;
 		update_devfreq(df);
 		gov_node->resume_freq = 0;
 		mutex_unlock(&df->lock);
 		break;
 	default:
 		break;
 	}
 	mutex_unlock(&state_lock);
 	return ret;
 }
 static struct devfreq_governor devfreq_gov_static_map = {
 	.name = "static_map",
 	.get_target_freq = devfreq_static_map_get_freq,
 	.event_handler = devfreq_static_map_ev_handler,
 };

 static int gov_static_map_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct static_map_gov *d;
 	int ret;
 	const char *dev_clk_name;

 	d = devm_kzalloc(dev, sizeof(*d), GFP_KERNEL);
 	if (!d)
 		return -ENOMEM;
 	d->dev = dev;

 	ret = of_property_read_string(dev->of_node, "qcom,dev_clk",
 							&dev_clk_name);
 	if (ret) {
 		dev_err(&pdev->dev,
 			"Failed to read device clock name %d\n", ret);
 		return ret;
 	}
 	d->dev_clk = devm_clk_get(dev, dev_clk_name);
 	if (IS_ERR(d->dev_clk))
 		return PTR_ERR(d->dev_clk);

 	d->of_node = of_parse_phandle(dev->of_node, "qcom,target-dev", 0);
 	if (!d->of_node) {
 		dev_err(dev, "Couldn't find a target device.\n");
 		ret = -ENODEV;
 		return ret;
 	}

 	d->freq_map = init_core_dev_map(dev, NULL, "qcom,core-dev-table");
 	if (!d->freq_map) {
 		dev_err(dev, "Couldn't find the core-dev freq table!\n");
 		return -EINVAL;
 	}
 	mutex_lock(&static_map_lock);
 	list_add_tail(&d->list, &static_map_list);
 	mutex_unlock(&static_map_lock);

 	mutex_lock(&state_lock);
 	d->gov = &devfreq_gov_static_map;
 	if (!static_use_cnt)
 		ret = devfreq_add_governor(&devfreq_gov_static_map);
 	if (ret)
 		dev_err(dev, "Failed to add governor %d\n", ret);
 	if (!ret)
 		static_use_cnt++;
 	mutex_unlock(&state_lock);

 	return ret;
 }

 static const struct of_device_id static_map_match_table[] = {
 	{ .compatible = "qcom,static-map"},
 	{}
 };

 static struct platform_driver gov_static_map_driver = {
 	.probe = gov_static_map_probe,
 	.driver = {
 		.name = "static-map",
 		.of_match_table = static_map_match_table,
 		.suppress_bind_attrs = true,
 	},
 };

 module_platform_driver(gov_static_map_driver);
 MODULE_DESCRIPTION("STATIC MAP GOVERNOR FOR DDR");
 MODULE_LICENSE("GPL v2");
	// SPDX-License-Identifier: GPL-2.0-only
	/*
	* Copyright (c) 2019, The Linux Foundation. All rights reserved.
	*/

	#define pr_fmt(fmt) "governor-static-map: " fmt

	#include <linux/kernel.h>
	#include <linux/module.h>
	#include <linux/init.h>
	#include <linux/io.h>
	#include <linux/err.h>
	#include <linux/errno.h>
	#include <linux/platform_device.h>
	#include <linux/of.h>
	#include <linux/clk.h>
	#include <linux/devfreq.h>
	#include "governor.h"

	struct core_dev_map {
	unsigned int core_mhz;
	unsigned int target_freq;
	};

	struct static_map_gov {
	struct device *dev;
	struct device_node *of_node;
	struct clk *dev_clk;
	unsigned long dev_clk_cur_freq;
	struct notifier_block clock_change_nb;
	struct core_dev_map *freq_map;
	struct devfreq_governor *gov;
	struct devfreq *df;
	bool mon_started;
	struct list_head list;
	void *orig_data;
	unsigned long resume_freq;
	};

	static LIST_HEAD(static_map_list);
	static DEFINE_MUTEX(static_map_lock);
	static DEFINE_MUTEX(state_lock);
	static int static_use_cnt;

	static struct static_map_gov find_static_map_node(struct devfreq df)
	{
	struct static_map_gov node, found = NULL;

	mutex_lock(&static_map_lock);
	list_for_each_entry(node, &static_map_list, list)
	if (node->of_node == df->dev.parent->of_node) {
	found = node;
	break;
	}
	mutex_unlock(&static_map_lock);

	return found;
	}

	static unsigned long core_to_dev_freq(struct static_map_gov *d,
	unsigned long coref)
	{
	struct core_dev_map *map = d->freq_map;
	unsigned long freq = 0;

	if (!map \|\| !coref)
	goto out;

	/* Start with the first non-zero freq map entry */
	map++;
	while (map->core_mhz && map->core_mhz != coref)
	map++;
	if (!map->core_mhz)
	map--;
	freq = map->target_freq;

	out:
	pr_debug("core freq: %lu -> target: %lu\n", coref, freq);
	return freq;
	}

	#define NUM_COLS 2
	static struct core_dev_map init_core_dev_map(struct device dev,
	struct device_node *of_node,
	char *prop_name)
	{
	int len, nf, i, j;
	u32 data;
	struct core_dev_map *tbl;
	int ret;

	if (!of_node)
	of_node = dev->of_node;

	if (!of_find_property(of_node, prop_name, &len))
	return NULL;
	len /= sizeof(data);

	if (len % NUM_COLS \|\| len == 0)
	return NULL;
	nf = len / NUM_COLS;

	tbl = devm_kzalloc(dev, (nf + 1) * sizeof(struct core_dev_map),
	GFP_KERNEL);
	if (!tbl)
	return NULL;

	for (i = 0, j = 0; i < nf; i++, j += 2) {
	ret = of_property_read_u32_index(of_node, prop_name, j,
	&data);
	if (ret) {
	dev_err(dev,
	"Couldn't read the core-dev freq table %d\n",
	ret);
	return NULL;
	}
	tbl[i].core_mhz = data;

	ret = of_property_read_u32_index(of_node, prop_name, j + 1,
	&data);
	if (ret) {
	dev_err(dev,
	"Couldn't read the core-dev freq table %d\n",
	ret);
	return NULL;
	}
	tbl[i].target_freq = data;
	pr_debug("Entry%d DEV:%u, Target:%u\n", i, tbl[i].core_mhz,
	tbl[i].target_freq);
	}
	tbl[i].core_mhz = 0;

	return tbl;
	}
	static int devfreq_static_map_get_freq(struct devfreq *df,
	unsigned long *freq)
	{
	struct static_map_gov *gov_node = df->data;

	*freq = core_to_dev_freq(gov_node, gov_node->dev_clk_cur_freq);

	return 0;
	}
	static int devfreq_clock_change_notify_cb(struct notifier_block *nb,
	unsigned long action, void *ptr)
	{
	struct clk_notifier_data *data = ptr;
	struct static_map_gov *d;
	int ret;

	if (action != POST_RATE_CHANGE)
	return NOTIFY_OK;

	mutex_lock(&state_lock);
	d = container_of(nb, struct static_map_gov, clock_change_nb);

	mutex_lock(&d->df->lock);
	d->dev_clk_cur_freq = data->new_rate;
	if (IS_ERR_VALUE(d->dev_clk_cur_freq)) {
	mutex_unlock(&d->df->lock);
	mutex_unlock(&state_lock);
	return d->dev_clk_cur_freq;
	}
	d->dev_clk_cur_freq = d->dev_clk_cur_freq / 1000;

	ret = update_devfreq(d->df);
	if (ret)
	dev_err(d->dev,
	"Unable to update freq on request %d\n", ret);
	mutex_unlock(&d->df->lock);
	mutex_unlock(&state_lock);

	return 0;
	}

	static int devfreq_static_map_ev_handler(struct devfreq *df,
	unsigned int event, void *data)
	{
	int ret = 0;
	struct static_map_gov *gov_node;

	mutex_lock(&state_lock);
	gov_node = find_static_map_node(df);
	if (!gov_node) {
	mutex_unlock(&state_lock);
	dev_err(df->dev.parent,
	"Unable to find static map governor!\n");
	return -ENODEV;
	}

	switch (event) {
	case DEVFREQ_GOV_START:
	gov_node->clock_change_nb.notifier_call =
	devfreq_clock_change_notify_cb;
	gov_node->orig_data = df->data;
	gov_node->df = df;
	df->data = gov_node;
	ret = clk_notifier_register(gov_node->dev_clk,
	&gov_node->clock_change_nb);
	if (ret) {
	dev_err(df->dev.parent,
	"Failed to register clock change notifier %d\n",
	ret);
	}
	break;
	case DEVFREQ_GOV_STOP:
	ret = clk_notifier_unregister(gov_node->dev_clk,
	&gov_node->clock_change_nb);
	if (ret) {
	dev_err(df->dev.parent,
	"Failed to register clock change notifier %d\n",
	ret);
	}
	df->data = gov_node->orig_data;
	gov_node->orig_data = NULL;
	break;
	case DEVFREQ_GOV_SUSPEND:
	ret = clk_notifier_unregister(gov_node->dev_clk,
	&gov_node->clock_change_nb);
	if (ret) {
	dev_err(df->dev.parent,
	"Failed to unregister clk notifier %d\n", ret);
	}
	mutex_lock(&df->lock);
	gov_node->resume_freq = gov_node->dev_clk_cur_freq;
	gov_node->dev_clk_cur_freq = 0;
	update_devfreq(df);
	mutex_unlock(&df->lock);
	break;
	case DEVFREQ_GOV_RESUME:
	ret = clk_notifier_register(gov_node->dev_clk,
	&gov_node->clock_change_nb);
	if (ret) {
	dev_err(df->dev.parent,
	"Failed to register clock change notifier %d\n",
	ret);
	}
	mutex_lock(&df->lock);
	gov_node->dev_clk_cur_freq = gov_node->resume_freq;
	update_devfreq(df);
	gov_node->resume_freq = 0;
	mutex_unlock(&df->lock);
	break;
	default:
	break;
	}
	mutex_unlock(&state_lock);
	return ret;
	}
	static struct devfreq_governor devfreq_gov_static_map = {
	.name = "static_map",
	.get_target_freq = devfreq_static_map_get_freq,
	.event_handler = devfreq_static_map_ev_handler,
	};

	static int gov_static_map_probe(struct platform_device *pdev)
	{
	struct device *dev = &pdev->dev;
	struct static_map_gov *d;
	int ret;
	const char *dev_clk_name;

	d = devm_kzalloc(dev, sizeof(*d), GFP_KERNEL);
	if (!d)
	return -ENOMEM;
	d->dev = dev;

	ret = of_property_read_string(dev->of_node, "qcom,dev_clk",
	&dev_clk_name);
	if (ret) {
	dev_err(&pdev->dev,
	"Failed to read device clock name %d\n", ret);
	return ret;
	}
	d->dev_clk = devm_clk_get(dev, dev_clk_name);
	if (IS_ERR(d->dev_clk))
	return PTR_ERR(d->dev_clk);

	d->of_node = of_parse_phandle(dev->of_node, "qcom,target-dev", 0);
	if (!d->of_node) {
	dev_err(dev, "Couldn't find a target device.\n");
	ret = -ENODEV;
	return ret;
	}

	d->freq_map = init_core_dev_map(dev, NULL, "qcom,core-dev-table");
	if (!d->freq_map) {
	dev_err(dev, "Couldn't find the core-dev freq table!\n");
	return -EINVAL;
	}
	mutex_lock(&static_map_lock);
	list_add_tail(&d->list, &static_map_list);
	mutex_unlock(&static_map_lock);

	mutex_lock(&state_lock);
	d->gov = &devfreq_gov_static_map;
	if (!static_use_cnt)
	ret = devfreq_add_governor(&devfreq_gov_static_map);
	if (ret)
	dev_err(dev, "Failed to add governor %d\n", ret);
	if (!ret)
	static_use_cnt++;
	mutex_unlock(&state_lock);

	return ret;
	}

	static const struct of_device_id static_map_match_table[] = {
	{ .compatible = "qcom,static-map"},
	{}
	};

	static struct platform_driver gov_static_map_driver = {
	.probe = gov_static_map_probe,
	.driver = {
	.name = "static-map",
	.of_match_table = static_map_match_table,
	.suppress_bind_attrs = true,
	},
	};

	module_platform_driver(gov_static_map_driver);
	MODULE_DESCRIPTION("STATIC MAP GOVERNOR FOR DDR");
	MODULE_LICENSE("GPL v2");