[linux-block.git] / drivers / cpufreq / mediatek-cpufreq-hw.c

// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (c) 2020 MediaTek Inc.
 */

#include <linux/bitfield.h>
#include <linux/cpufreq.h>
#include <linux/energy_model.h>
#include <linux/init.h>
#include <linux/iopoll.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/of_address.h>
#include <linux/of_platform.h>
#include <linux/slab.h>

#define LUT_MAX_ENTRIES			32U
#define LUT_FREQ			GENMASK(11, 0)
#define LUT_ROW_SIZE			0x4
#define CPUFREQ_HW_STATUS		BIT(0)
#define SVS_HW_STATUS			BIT(1)
#define POLL_USEC			1000
#define TIMEOUT_USEC			300000

enum {
	REG_FREQ_LUT_TABLE,
	REG_FREQ_ENABLE,
	REG_FREQ_PERF_STATE,
	REG_FREQ_HW_STATE,
	REG_EM_POWER_TBL,
	REG_FREQ_LATENCY,

	REG_ARRAY_SIZE,
};

struct mtk_cpufreq_data {
	struct cpufreq_frequency_table *table;
	void __iomem *reg_bases[REG_ARRAY_SIZE];
	struct resource *res;
	void __iomem *base;
	int nr_opp;
};

static const u16 cpufreq_mtk_offsets[REG_ARRAY_SIZE] = {
	[REG_FREQ_LUT_TABLE]	= 0x0,
	[REG_FREQ_ENABLE]	= 0x84,
	[REG_FREQ_PERF_STATE]	= 0x88,
	[REG_FREQ_HW_STATE]	= 0x8c,
	[REG_EM_POWER_TBL]	= 0x90,
	[REG_FREQ_LATENCY]	= 0x110,
};

static int __maybe_unused
mtk_cpufreq_get_cpu_power(struct device *cpu_dev, unsigned long *uW,
			  unsigned long *KHz)
{
	struct mtk_cpufreq_data *data;
	struct cpufreq_policy *policy;
	int i;

	policy = cpufreq_cpu_get_raw(cpu_dev->id);
	if (!policy)
		return 0;

	data = policy->driver_data;

	for (i = 0; i < data->nr_opp; i++) {
		if (data->table[i].frequency < *KHz)
			break;
	}
	i--;

	*KHz = data->table[i].frequency;
	/* Provide micro-Watts value to the Energy Model */
	*uW = readl_relaxed(data->reg_bases[REG_EM_POWER_TBL] +
			    i * LUT_ROW_SIZE);

	return 0;
}

static int mtk_cpufreq_hw_target_index(struct cpufreq_policy *policy,
				       unsigned int index)
{
	struct mtk_cpufreq_data *data = policy->driver_data;

	writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]);

	return 0;
}

static unsigned int mtk_cpufreq_hw_get(unsigned int cpu)
{
	struct mtk_cpufreq_data *data;
	struct cpufreq_policy *policy;
	unsigned int index;

	policy = cpufreq_cpu_get_raw(cpu);
	if (!policy)
		return 0;

	data = policy->driver_data;

	index = readl_relaxed(data->reg_bases[REG_FREQ_PERF_STATE]);
	index = min(index, LUT_MAX_ENTRIES - 1);

	return data->table[index].frequency;
}

static unsigned int mtk_cpufreq_hw_fast_switch(struct cpufreq_policy *policy,
					       unsigned int target_freq)
{
	struct mtk_cpufreq_data *data = policy->driver_data;
	unsigned int index;

	index = cpufreq_table_find_index_dl(policy, target_freq, false);

	writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]);

	return policy->freq_table[index].frequency;
}

static int mtk_cpu_create_freq_table(struct platform_device *pdev,
				     struct mtk_cpufreq_data *data)
{
	struct device *dev = &pdev->dev;
	u32 temp, i, freq, prev_freq = 0;
	void __iomem *base_table;

	data->table = devm_kcalloc(dev, LUT_MAX_ENTRIES + 1,
				   sizeof(*data->table), GFP_KERNEL);
	if (!data->table)
		return -ENOMEM;

	base_table = data->reg_bases[REG_FREQ_LUT_TABLE];

	for (i = 0; i < LUT_MAX_ENTRIES; i++) {
		temp = readl_relaxed(base_table + (i * LUT_ROW_SIZE));
		freq = FIELD_GET(LUT_FREQ, temp) * 1000;

		if (freq == prev_freq)
			break;

		data->table[i].frequency = freq;

		dev_dbg(dev, "index=%d freq=%d\n", i, data->table[i].frequency);

		prev_freq = freq;
	}

	data->table[i].frequency = CPUFREQ_TABLE_END;
	data->nr_opp = i;

	return 0;
}

static int mtk_cpu_resources_init(struct platform_device *pdev,
				  struct cpufreq_policy *policy,
				  const u16 *offsets)
{
	struct mtk_cpufreq_data *data;
	struct device *dev = &pdev->dev;
	struct resource *res;
	void __iomem *base;
	int ret, i;
	int index;

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

	index = of_perf_domain_get_sharing_cpumask(policy->cpu, "performance-domains",
						   "#performance-domain-cells",
						   policy->cpus);
	if (index < 0)
		return index;

	res = platform_get_resource(pdev, IORESOURCE_MEM, index);
	if (!res) {
		dev_err(dev, "failed to get mem resource %d\n", index);
		return -ENODEV;
	}

	if (!request_mem_region(res->start, resource_size(res), res->name)) {
		dev_err(dev, "failed to request resource %pR\n", res);
		return -EBUSY;
	}

	base = ioremap(res->start, resource_size(res));
	if (!base) {
		dev_err(dev, "failed to map resource %pR\n", res);
		ret = -ENOMEM;
		goto release_region;
	}

	data->base = base;
	data->res = res;

	for (i = REG_FREQ_LUT_TABLE; i < REG_ARRAY_SIZE; i++)
		data->reg_bases[i] = base + offsets[i];

	ret = mtk_cpu_create_freq_table(pdev, data);
	if (ret) {
		dev_info(dev, "Domain-%d failed to create freq table\n", index);
		return ret;
	}

	policy->freq_table = data->table;
	policy->driver_data = data;

	return 0;
release_region:
	release_mem_region(res->start, resource_size(res));
	return ret;
}

static int mtk_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
{
	struct platform_device *pdev = cpufreq_get_driver_data();
	int sig, pwr_hw = CPUFREQ_HW_STATUS | SVS_HW_STATUS;
	struct mtk_cpufreq_data *data;
	unsigned int latency;
	int ret;

	/* Get the bases of cpufreq for domains */
	ret = mtk_cpu_resources_init(pdev, policy, platform_get_drvdata(pdev));
	if (ret) {
		dev_info(&pdev->dev, "CPUFreq resource init failed\n");
		return ret;
	}

	data = policy->driver_data;

	latency = readl_relaxed(data->reg_bases[REG_FREQ_LATENCY]) * 1000;
	if (!latency)
		latency = CPUFREQ_ETERNAL;

	policy->cpuinfo.transition_latency = latency;
	policy->fast_switch_possible = true;

	/* HW should be in enabled state to proceed now */
	writel_relaxed(0x1, data->reg_bases[REG_FREQ_ENABLE]);
	if (readl_poll_timeout(data->reg_bases[REG_FREQ_HW_STATE], sig,
			       (sig & pwr_hw) == pwr_hw, POLL_USEC,
			       TIMEOUT_USEC)) {
		if (!(sig & CPUFREQ_HW_STATUS)) {
			pr_info("cpufreq hardware of CPU%d is not enabled\n",
				policy->cpu);
			return -ENODEV;
		}

		pr_info("SVS of CPU%d is not enabled\n", policy->cpu);
	}

	return 0;
}

static int mtk_cpufreq_hw_cpu_exit(struct cpufreq_policy *policy)
{
	struct mtk_cpufreq_data *data = policy->driver_data;
	struct resource *res = data->res;
	void __iomem *base = data->base;

	/* HW should be in paused state now */
	writel_relaxed(0x0, data->reg_bases[REG_FREQ_ENABLE]);
	iounmap(base);
	release_mem_region(res->start, resource_size(res));

	return 0;
}

static void mtk_cpufreq_register_em(struct cpufreq_policy *policy)
{
	struct em_data_callback em_cb = EM_DATA_CB(mtk_cpufreq_get_cpu_power);
	struct mtk_cpufreq_data *data = policy->driver_data;

	em_dev_register_perf_domain(get_cpu_device(policy->cpu), data->nr_opp,
				    &em_cb, policy->cpus, true);
}

static struct cpufreq_driver cpufreq_mtk_hw_driver = {
	.flags		= CPUFREQ_NEED_INITIAL_FREQ_CHECK |
			  CPUFREQ_HAVE_GOVERNOR_PER_POLICY |
			  CPUFREQ_IS_COOLING_DEV,
	.verify		= cpufreq_generic_frequency_table_verify,
	.target_index	= mtk_cpufreq_hw_target_index,
	.get		= mtk_cpufreq_hw_get,
	.init		= mtk_cpufreq_hw_cpu_init,
	.exit		= mtk_cpufreq_hw_cpu_exit,
	.register_em	= mtk_cpufreq_register_em,
	.fast_switch	= mtk_cpufreq_hw_fast_switch,
	.name		= "mtk-cpufreq-hw",
	.attr		= cpufreq_generic_attr,
};

static int mtk_cpufreq_hw_driver_probe(struct platform_device *pdev)
{
	const void *data;
	int ret;

	data = of_device_get_match_data(&pdev->dev);
	if (!data)
		return -EINVAL;

	platform_set_drvdata(pdev, (void *) data);
	cpufreq_mtk_hw_driver.driver_data = pdev;

	ret = cpufreq_register_driver(&cpufreq_mtk_hw_driver);
	if (ret)
		dev_err(&pdev->dev, "CPUFreq HW driver failed to register\n");

	return ret;
}

static int mtk_cpufreq_hw_driver_remove(struct platform_device *pdev)
{
	return cpufreq_unregister_driver(&cpufreq_mtk_hw_driver);
}

static const struct of_device_id mtk_cpufreq_hw_match[] = {
	{ .compatible = "mediatek,cpufreq-hw", .data = &cpufreq_mtk_offsets },
	{}
};

static struct platform_driver mtk_cpufreq_hw_driver = {
	.probe = mtk_cpufreq_hw_driver_probe,
	.remove = mtk_cpufreq_hw_driver_remove,
	.driver = {
		.name = "mtk-cpufreq-hw",
		.of_match_table = mtk_cpufreq_hw_match,
	},
};
module_platform_driver(mtk_cpufreq_hw_driver);

MODULE_AUTHOR("Hector Yuan <hector.yuan@mediatek.com>");
MODULE_DESCRIPTION("Mediatek cpufreq-hw driver");
MODULE_LICENSE("GPL v2");
Commit	Line	Data
4855e26b HY	1	// SPDX-License-Identifier: GPL-2.0
	2	/*
	3	* Copyright (c) 2020 MediaTek Inc.
	4	*/
	5
	6	#include <linux/bitfield.h>
	7	#include <linux/cpufreq.h>
	8	#include <linux/energy_model.h>
	9	#include <linux/init.h>
	10	#include <linux/iopoll.h>
	11	#include <linux/kernel.h>
	12	#include <linux/module.h>
	13	#include <linux/of_address.h>
	14	#include <linux/of_platform.h>
	15	#include <linux/slab.h>
	16
	17	#define LUT_MAX_ENTRIES 32U
	18	#define LUT_FREQ GENMASK(11, 0)
	19	#define LUT_ROW_SIZE 0x4
	20	#define CPUFREQ_HW_STATUS BIT(0)
	21	#define SVS_HW_STATUS BIT(1)
	22	#define POLL_USEC 1000
	23	#define TIMEOUT_USEC 300000
	24
	25	enum {
	26	REG_FREQ_LUT_TABLE,
	27	REG_FREQ_ENABLE,
	28	REG_FREQ_PERF_STATE,
	29	REG_FREQ_HW_STATE,
	30	REG_EM_POWER_TBL,
	31	REG_FREQ_LATENCY,
	32
	33	REG_ARRAY_SIZE,
	34	};
	35
	36	struct mtk_cpufreq_data {
	37	struct cpufreq_frequency_table *table;
	38	void __iomem *reg_bases[REG_ARRAY_SIZE];
d776790a HY	39	struct resource *res;
d776790a HY	40	void __iomem *base;
4855e26b HY	41	int nr_opp;
	42	};
	43
	44	static const u16 cpufreq_mtk_offsets[REG_ARRAY_SIZE] = {
	45	[REG_FREQ_LUT_TABLE] = 0x0,
	46	[REG_FREQ_ENABLE] = 0x84,
	47	[REG_FREQ_PERF_STATE] = 0x88,
	48	[REG_FREQ_HW_STATE] = 0x8c,
	49	[REG_EM_POWER_TBL] = 0x90,
	50	[REG_FREQ_LATENCY] = 0x110,
	51	};
	52
	53	static int __maybe_unused
ae6ccaa6	54	mtk_cpufreq_get_cpu_power(struct device cpu_dev, unsigned long uW,
75a3a99a	55	unsigned long *KHz)
4855e26b HY	56	{
	57	struct mtk_cpufreq_data *data;
	58	struct cpufreq_policy *policy;
	59	int i;
	60
	61	policy = cpufreq_cpu_get_raw(cpu_dev->id);
	62	if (!policy)
	63	return 0;
	64
	65	data = policy->driver_data;
	66
	67	for (i = 0; i < data->nr_opp; i++) {
	68	if (data->table[i].frequency < *KHz)
	69	break;
	70	}
	71	i--;
	72
	73	*KHz = data->table[i].frequency;
ae6ccaa6 LL	74	/* Provide micro-Watts value to the Energy Model */
	75	*uW = readl_relaxed(data->reg_bases[REG_EM_POWER_TBL] +
	76	i * LUT_ROW_SIZE);
4855e26b HY	77
	78	return 0;
	79	}
	80
	81	static int mtk_cpufreq_hw_target_index(struct cpufreq_policy *policy,
	82	unsigned int index)
	83	{
	84	struct mtk_cpufreq_data *data = policy->driver_data;
	85
	86	writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]);
	87
	88	return 0;
	89	}
	90
	91	static unsigned int mtk_cpufreq_hw_get(unsigned int cpu)
	92	{
	93	struct mtk_cpufreq_data *data;
	94	struct cpufreq_policy *policy;
	95	unsigned int index;
	96
	97	policy = cpufreq_cpu_get_raw(cpu);
	98	if (!policy)
	99	return 0;
	100
	101	data = policy->driver_data;
	102
	103	index = readl_relaxed(data->reg_bases[REG_FREQ_PERF_STATE]);
	104	index = min(index, LUT_MAX_ENTRIES - 1);
	105
	106	return data->table[index].frequency;
	107	}
	108
	109	static unsigned int mtk_cpufreq_hw_fast_switch(struct cpufreq_policy *policy,
	110	unsigned int target_freq)
	111	{
	112	struct mtk_cpufreq_data *data = policy->driver_data;
	113	unsigned int index;
	114
6215a5de	115	index = cpufreq_table_find_index_dl(policy, target_freq, false);
4855e26b HY	116
	117	writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]);
	118
	119	return policy->freq_table[index].frequency;
	120	}
	121
	122	static int mtk_cpu_create_freq_table(struct platform_device *pdev,
	123	struct mtk_cpufreq_data *data)
	124	{
	125	struct device *dev = &pdev->dev;
	126	u32 temp, i, freq, prev_freq = 0;
	127	void __iomem *base_table;
	128
	129	data->table = devm_kcalloc(dev, LUT_MAX_ENTRIES + 1,
	130	sizeof(*data->table), GFP_KERNEL);
	131	if (!data->table)
	132	return -ENOMEM;
	133
	134	base_table = data->reg_bases[REG_FREQ_LUT_TABLE];
	135
	136	for (i = 0; i < LUT_MAX_ENTRIES; i++) {
	137	temp = readl_relaxed(base_table + (i * LUT_ROW_SIZE));
	138	freq = FIELD_GET(LUT_FREQ, temp) * 1000;
	139
	140	if (freq == prev_freq)
	141	break;
	142
	143	data->table[i].frequency = freq;
	144
	145	dev_dbg(dev, "index=%d freq=%d\n", i, data->table[i].frequency);
	146
	147	prev_freq = freq;
	148	}
	149
	150	data->table[i].frequency = CPUFREQ_TABLE_END;
	151	data->nr_opp = i;
	152
	153	return 0;
	154	}
	155
	156	static int mtk_cpu_resources_init(struct platform_device *pdev,
	157	struct cpufreq_policy *policy,
	158	const u16 *offsets)
	159	{
	160	struct mtk_cpufreq_data *data;
	161	struct device *dev = &pdev->dev;
d776790a	162	struct resource *res;
4855e26b HY	163	void __iomem *base;
	164	int ret, i;
	165	int index;
	166
	167	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
	168	if (!data)
	169	return -ENOMEM;
	170
	171	index = of_perf_domain_get_sharing_cpumask(policy->cpu, "performance-domains",
	172	"#performance-domain-cells",
	173	policy->cpus);
	174	if (index < 0)
	175	return index;
	176
d776790a HY	177	res = platform_get_resource(pdev, IORESOURCE_MEM, index);
	178	if (!res) {
	179	dev_err(dev, "failed to get mem resource %d\n", index);
	180	return -ENODEV;
	181	}
	182
	183	if (!request_mem_region(res->start, resource_size(res), res->name)) {
	184	dev_err(dev, "failed to request resource %pR\n", res);
	185	return -EBUSY;
	186	}
	187
	188	base = ioremap(res->start, resource_size(res));
	189	if (!base) {
	190	dev_err(dev, "failed to map resource %pR\n", res);
	191	ret = -ENOMEM;
	192	goto release_region;
	193	}
	194
	195	data->base = base;
	196	data->res = res;
4855e26b HY	197
	198	for (i = REG_FREQ_LUT_TABLE; i < REG_ARRAY_SIZE; i++)
	199	data->reg_bases[i] = base + offsets[i];
	200
	201	ret = mtk_cpu_create_freq_table(pdev, data);
	202	if (ret) {
	203	dev_info(dev, "Domain-%d failed to create freq table\n", index);
	204	return ret;
	205	}
	206
	207	policy->freq_table = data->table;
	208	policy->driver_data = data;
	209
	210	return 0;
d776790a HY	211	release_region:
	212	release_mem_region(res->start, resource_size(res));
	213	return ret;
4855e26b HY	214	}
	215
	216	static int mtk_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
	217	{
	218	struct platform_device *pdev = cpufreq_get_driver_data();
	219	int sig, pwr_hw = CPUFREQ_HW_STATUS \| SVS_HW_STATUS;
	220	struct mtk_cpufreq_data *data;
	221	unsigned int latency;
	222	int ret;
	223
	224	/* Get the bases of cpufreq for domains */
	225	ret = mtk_cpu_resources_init(pdev, policy, platform_get_drvdata(pdev));
	226	if (ret) {
	227	dev_info(&pdev->dev, "CPUFreq resource init failed\n");
	228	return ret;
	229	}
	230
	231	data = policy->driver_data;
	232
	233	latency = readl_relaxed(data->reg_bases[REG_FREQ_LATENCY]) * 1000;
	234	if (!latency)
	235	latency = CPUFREQ_ETERNAL;
	236
	237	policy->cpuinfo.transition_latency = latency;
	238	policy->fast_switch_possible = true;
	239
	240	/* HW should be in enabled state to proceed now */
	241	writel_relaxed(0x1, data->reg_bases[REG_FREQ_ENABLE]);
	242	if (readl_poll_timeout(data->reg_bases[REG_FREQ_HW_STATE], sig,
	243	(sig & pwr_hw) == pwr_hw, POLL_USEC,
	244	TIMEOUT_USEC)) {
	245	if (!(sig & CPUFREQ_HW_STATUS)) {
	246	pr_info("cpufreq hardware of CPU%d is not enabled\n",
	247	policy->cpu);
	248	return -ENODEV;
	249	}
	250
	251	pr_info("SVS of CPU%d is not enabled\n", policy->cpu);
	252	}
	253
	254	return 0;
	255	}
	256
	257	static int mtk_cpufreq_hw_cpu_exit(struct cpufreq_policy *policy)
	258	{
	259	struct mtk_cpufreq_data *data = policy->driver_data;
d776790a HY	260	struct resource *res = data->res;
d776790a HY	261	void __iomem *base = data->base;
4855e26b HY	262
	263	/* HW should be in paused state now */
	264	writel_relaxed(0x0, data->reg_bases[REG_FREQ_ENABLE]);
d776790a HY	265	iounmap(base);
d776790a HY	266	release_mem_region(res->start, resource_size(res));
4855e26b HY	267
	268	return 0;
	269	}
	270
	271	static void mtk_cpufreq_register_em(struct cpufreq_policy *policy)
	272	{
	273	struct em_data_callback em_cb = EM_DATA_CB(mtk_cpufreq_get_cpu_power);
	274	struct mtk_cpufreq_data *data = policy->driver_data;
	275
	276	em_dev_register_perf_domain(get_cpu_device(policy->cpu), data->nr_opp,
	277	&em_cb, policy->cpus, true);
	278	}
	279
	280	static struct cpufreq_driver cpufreq_mtk_hw_driver = {
	281	.flags = CPUFREQ_NEED_INITIAL_FREQ_CHECK \|
	282	CPUFREQ_HAVE_GOVERNOR_PER_POLICY \|
	283	CPUFREQ_IS_COOLING_DEV,
	284	.verify = cpufreq_generic_frequency_table_verify,
	285	.target_index = mtk_cpufreq_hw_target_index,
	286	.get = mtk_cpufreq_hw_get,
	287	.init = mtk_cpufreq_hw_cpu_init,
	288	.exit = mtk_cpufreq_hw_cpu_exit,
	289	.register_em = mtk_cpufreq_register_em,
	290	.fast_switch = mtk_cpufreq_hw_fast_switch,
	291	.name = "mtk-cpufreq-hw",
	292	.attr = cpufreq_generic_attr,
	293	};
	294
	295	static int mtk_cpufreq_hw_driver_probe(struct platform_device *pdev)
	296	{
	297	const void *data;
	298	int ret;
	299
	300	data = of_device_get_match_data(&pdev->dev);
	301	if (!data)
	302	return -EINVAL;
	303
	304	platform_set_drvdata(pdev, (void *) data);
	305	cpufreq_mtk_hw_driver.driver_data = pdev;
	306
	307	ret = cpufreq_register_driver(&cpufreq_mtk_hw_driver);
	308	if (ret)
	309	dev_err(&pdev->dev, "CPUFreq HW driver failed to register\n");
	310
	311	return ret;
	312	}
	313
	314	static int mtk_cpufreq_hw_driver_remove(struct platform_device *pdev)
	315	{
	316	return cpufreq_unregister_driver(&cpufreq_mtk_hw_driver);
	317	}
	318
	319	static const struct of_device_id mtk_cpufreq_hw_match[] = {
	320	{ .compatible = "mediatek,cpufreq-hw", .data = &cpufreq_mtk_offsets },
	321	{}
	322	};
	323
	324	static struct platform_driver mtk_cpufreq_hw_driver = {
	325	.probe = mtk_cpufreq_hw_driver_probe,
	326	.remove = mtk_cpufreq_hw_driver_remove,
	327	.driver = {
	328	.name = "mtk-cpufreq-hw",
	329	.of_match_table = mtk_cpufreq_hw_match,
	330	},
331	};
332	module_platform_driver(mtk_cpufreq_hw_driver);
333
334	MODULE_AUTHOR("Hector Yuan <hector.yuan@mediatek.com>");
335	MODULE_DESCRIPTION("Mediatek cpufreq-hw driver");
336	MODULE_LICENSE("GPL v2");