[linux-2.6-block.git] / drivers / cpufreq / sti-cpufreq.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * Match running platform with pre-defined OPP values for CPUFreq
 *
 * Author: Ajit Pal Singh <ajitpal.singh@st.com>
 *         Lee Jones <lee.jones@linaro.org>
 *
 * Copyright (C) 2015 STMicroelectronics (R&D) Limited
 */

#include <linux/cpu.h>
#include <linux/io.h>
#include <linux/mfd/syscon.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/of_platform.h>
#include <linux/pm_opp.h>
#include <linux/regmap.h>

#define VERSION_ELEMENTS	3
#define MAX_PCODE_NAME_LEN	7

#define VERSION_SHIFT		28
#define HW_INFO_INDEX		1
#define MAJOR_ID_INDEX		1
#define MINOR_ID_INDEX		2

/*
 * Only match on "suitable for ALL versions" entries
 *
 * This will be used with the BIT() macro.  It sets the
 * top bit of a 32bit value and is equal to 0x80000000.
 */
#define DEFAULT_VERSION		31

enum {
	PCODE = 0,
	SUBSTRATE,
	DVFS_MAX_REGFIELDS,
};

/**
 * ST CPUFreq Driver Data
 *
 * @cpu_node		CPU's OF node
 * @syscfg_eng		Engineering Syscon register map
 * @regmap		Syscon register map
 */
static struct sti_cpufreq_ddata {
	struct device *cpu;
	struct regmap *syscfg_eng;
	struct regmap *syscfg;
} ddata;

static int sti_cpufreq_fetch_major(void) {
	struct device_node *np = ddata.cpu->of_node;
	struct device *dev = ddata.cpu;
	unsigned int major_offset;
	unsigned int socid;
	int ret;

	ret = of_property_read_u32_index(np, "st,syscfg",
					 MAJOR_ID_INDEX, &major_offset);
	if (ret) {
		dev_err(dev, "No major number offset provided in %pOF [%d]\n",
			np, ret);
		return ret;
	}

	ret = regmap_read(ddata.syscfg, major_offset, &socid);
	if (ret) {
		dev_err(dev, "Failed to read major number from syscon [%d]\n",
			ret);
		return ret;
	}

	return ((socid >> VERSION_SHIFT) & 0xf) + 1;
}

static int sti_cpufreq_fetch_minor(void)
{
	struct device *dev = ddata.cpu;
	struct device_node *np = dev->of_node;
	unsigned int minor_offset;
	unsigned int minid;
	int ret;

	ret = of_property_read_u32_index(np, "st,syscfg-eng",
					 MINOR_ID_INDEX, &minor_offset);
	if (ret) {
		dev_err(dev,
			"No minor number offset provided %pOF [%d]\n",
			np, ret);
		return ret;
	}

	ret = regmap_read(ddata.syscfg_eng, minor_offset, &minid);
	if (ret) {
		dev_err(dev,
			"Failed to read the minor number from syscon [%d]\n",
			ret);
		return ret;
	}

	return minid & 0xf;
}

static int sti_cpufreq_fetch_regmap_field(const struct reg_field *reg_fields,
					  int hw_info_offset, int field)
{
	struct regmap_field *regmap_field;
	struct reg_field reg_field = reg_fields[field];
	struct device *dev = ddata.cpu;
	unsigned int value;
	int ret;

	reg_field.reg = hw_info_offset;
	regmap_field = devm_regmap_field_alloc(dev,
					       ddata.syscfg_eng,
					       reg_field);
	if (IS_ERR(regmap_field)) {
		dev_err(dev, "Failed to allocate reg field\n");
		return PTR_ERR(regmap_field);
	}

	ret = regmap_field_read(regmap_field, &value);
	if (ret) {
		dev_err(dev, "Failed to read %s code\n",
			field ? "SUBSTRATE" : "PCODE");
		return ret;
	}

	return value;
}

static const struct reg_field sti_stih407_dvfs_regfields[DVFS_MAX_REGFIELDS] = {
	[PCODE]		= REG_FIELD(0, 16, 19),
	[SUBSTRATE]	= REG_FIELD(0, 0, 2),
};

static const struct reg_field *sti_cpufreq_match(void)
{
	if (of_machine_is_compatible("st,stih407") ||
	    of_machine_is_compatible("st,stih410"))
		return sti_stih407_dvfs_regfields;

	return NULL;
}

static int sti_cpufreq_set_opp_info(void)
{
	struct device *dev = ddata.cpu;
	struct device_node *np = dev->of_node;
	const struct reg_field *reg_fields;
	unsigned int hw_info_offset;
	unsigned int version[VERSION_ELEMENTS];
	int pcode, substrate, major, minor;
	int ret;
	char name[MAX_PCODE_NAME_LEN];
	struct opp_table *opp_table;

	reg_fields = sti_cpufreq_match();
	if (!reg_fields) {
		dev_err(dev, "This SoC doesn't support voltage scaling\n");
		return -ENODEV;
	}

	ret = of_property_read_u32_index(np, "st,syscfg-eng",
					 HW_INFO_INDEX, &hw_info_offset);
	if (ret) {
		dev_warn(dev, "Failed to read HW info offset from DT\n");
		substrate = DEFAULT_VERSION;
		pcode = 0;
		goto use_defaults;
	}

	pcode = sti_cpufreq_fetch_regmap_field(reg_fields,
					       hw_info_offset,
					       PCODE);
	if (pcode < 0) {
		dev_warn(dev, "Failed to obtain process code\n");
		/* Use default pcode */
		pcode = 0;
	}

	substrate = sti_cpufreq_fetch_regmap_field(reg_fields,
						   hw_info_offset,
						   SUBSTRATE);
	if (substrate) {
		dev_warn(dev, "Failed to obtain substrate code\n");
		/* Use default substrate */
		substrate = DEFAULT_VERSION;
	}

use_defaults:
	major = sti_cpufreq_fetch_major();
	if (major < 0) {
		dev_err(dev, "Failed to obtain major version\n");
		/* Use default major number */
		major = DEFAULT_VERSION;
	}

	minor = sti_cpufreq_fetch_minor();
	if (minor < 0) {
		dev_err(dev, "Failed to obtain minor version\n");
		/* Use default minor number */
		minor = DEFAULT_VERSION;
	}

	snprintf(name, MAX_PCODE_NAME_LEN, "pcode%d", pcode);

	opp_table = dev_pm_opp_set_prop_name(dev, name);
	if (IS_ERR(opp_table)) {
		dev_err(dev, "Failed to set prop name\n");
		return PTR_ERR(opp_table);
	}

	version[0] = BIT(major);
	version[1] = BIT(minor);
	version[2] = BIT(substrate);

	opp_table = dev_pm_opp_set_supported_hw(dev, version, VERSION_ELEMENTS);
	if (IS_ERR(opp_table)) {
		dev_err(dev, "Failed to set supported hardware\n");
		return PTR_ERR(opp_table);
	}

	dev_dbg(dev, "pcode: %d major: %d minor: %d substrate: %d\n",
		pcode, major, minor, substrate);
	dev_dbg(dev, "version[0]: %x version[1]: %x version[2]: %x\n",
		version[0], version[1], version[2]);

	return 0;
}

static int sti_cpufreq_fetch_syscon_registers(void)
{
	struct device *dev = ddata.cpu;
	struct device_node *np = dev->of_node;

	ddata.syscfg = syscon_regmap_lookup_by_phandle(np, "st,syscfg");
	if (IS_ERR(ddata.syscfg)) {
		dev_err(dev,  "\"st,syscfg\" not supplied\n");
		return PTR_ERR(ddata.syscfg);
	}

	ddata.syscfg_eng = syscon_regmap_lookup_by_phandle(np, "st,syscfg-eng");
	if (IS_ERR(ddata.syscfg_eng)) {
		dev_err(dev, "\"st,syscfg-eng\" not supplied\n");
		return PTR_ERR(ddata.syscfg_eng);
	}

	return 0;
}

static int sti_cpufreq_init(void)
{
	int ret;

	if ((!of_machine_is_compatible("st,stih407")) &&
		(!of_machine_is_compatible("st,stih410")))
		return -ENODEV;

	ddata.cpu = get_cpu_device(0);
	if (!ddata.cpu) {
		dev_err(ddata.cpu, "Failed to get device for CPU0\n");
		goto skip_voltage_scaling;
	}

	if (!of_get_property(ddata.cpu->of_node, "operating-points-v2", NULL)) {
		dev_err(ddata.cpu, "OPP-v2 not supported\n");
		goto skip_voltage_scaling;
	}

	ret = sti_cpufreq_fetch_syscon_registers();
	if (ret)
		goto skip_voltage_scaling;

	ret = sti_cpufreq_set_opp_info();
	if (!ret)
		goto register_cpufreq_dt;

skip_voltage_scaling:
	dev_err(ddata.cpu, "Not doing voltage scaling\n");

register_cpufreq_dt:
	platform_device_register_simple("cpufreq-dt", -1, NULL, 0);

	return 0;
}
module_init(sti_cpufreq_init);

MODULE_DESCRIPTION("STMicroelectronics CPUFreq/OPP driver");
MODULE_AUTHOR("Ajitpal Singh <ajitpal.singh@st.com>");
MODULE_AUTHOR("Lee Jones <lee.jones@linaro.org>");
MODULE_LICENSE("GPL v2");
Commit	Line	Data
aecd8454	1	// SPDX-License-Identifier: GPL-2.0-only
ab0ea257 LJ	2	/*
	3	* Match running platform with pre-defined OPP values for CPUFreq
	4	*
	5	* Author: Ajit Pal Singh <ajitpal.singh@st.com>
	6	* Lee Jones <lee.jones@linaro.org>
	7	*
	8	* Copyright (C) 2015 STMicroelectronics (R&D) Limited
ab0ea257 LJ	9	*/
	10
	11	#include <linux/cpu.h>
	12	#include <linux/io.h>
	13	#include <linux/mfd/syscon.h>
	14	#include <linux/module.h>
	15	#include <linux/of.h>
	16	#include <linux/of_platform.h>
	17	#include <linux/pm_opp.h>
	18	#include <linux/regmap.h>
	19
	20	#define VERSION_ELEMENTS 3
	21	#define MAX_PCODE_NAME_LEN 7
	22
	23	#define VERSION_SHIFT 28
	24	#define HW_INFO_INDEX 1
	25	#define MAJOR_ID_INDEX 1
	26	#define MINOR_ID_INDEX 2
	27
	28	/*
	29	* Only match on "suitable for ALL versions" entries
	30	*
	31	* This will be used with the BIT() macro. It sets the
	32	* top bit of a 32bit value and is equal to 0x80000000.
	33	*/
	34	#define DEFAULT_VERSION 31
	35
	36	enum {
	37	PCODE = 0,
	38	SUBSTRATE,
	39	DVFS_MAX_REGFIELDS,
	40	};
	41
	42	/**
	43	* ST CPUFreq Driver Data
	44	*
	45	* @cpu_node CPU's OF node
	46	* @syscfg_eng Engineering Syscon register map
	47	* @regmap Syscon register map
	48	*/
	49	static struct sti_cpufreq_ddata {
	50	struct device *cpu;
	51	struct regmap *syscfg_eng;
	52	struct regmap *syscfg;
	53	} ddata;
	54
	55	static int sti_cpufreq_fetch_major(void) {
	56	struct device_node *np = ddata.cpu->of_node;
	57	struct device *dev = ddata.cpu;
	58	unsigned int major_offset;
	59	unsigned int socid;
	60	int ret;
	61
	62	ret = of_property_read_u32_index(np, "st,syscfg",
	63	MAJOR_ID_INDEX, &major_offset);
	64	if (ret) {
cc5a7a74 RH	65	dev_err(dev, "No major number offset provided in %pOF [%d]\n",
cc5a7a74 RH	66	np, ret);
ab0ea257 LJ	67	return ret;
	68	}
	69
	70	ret = regmap_read(ddata.syscfg, major_offset, &socid);
	71	if (ret) {
	72	dev_err(dev, "Failed to read major number from syscon [%d]\n",
	73	ret);
	74	return ret;
	75	}
	76
	77	return ((socid >> VERSION_SHIFT) & 0xf) + 1;
	78	}
	79
	80	static int sti_cpufreq_fetch_minor(void)
	81	{
	82	struct device *dev = ddata.cpu;
	83	struct device_node *np = dev->of_node;
	84	unsigned int minor_offset;
	85	unsigned int minid;
	86	int ret;
	87
	88	ret = of_property_read_u32_index(np, "st,syscfg-eng",
	89	MINOR_ID_INDEX, &minor_offset);
	90	if (ret) {
	91	dev_err(dev,
cc5a7a74 RH	92	"No minor number offset provided %pOF [%d]\n",
cc5a7a74 RH	93	np, ret);
ab0ea257 LJ	94	return ret;
	95	}
	96
	97	ret = regmap_read(ddata.syscfg_eng, minor_offset, &minid);
	98	if (ret) {
	99	dev_err(dev,
	100	"Failed to read the minor number from syscon [%d]\n",
	101	ret);
	102	return ret;
	103	}
	104
	105	return minid & 0xf;
	106	}
	107
	108	static int sti_cpufreq_fetch_regmap_field(const struct reg_field *reg_fields,
	109	int hw_info_offset, int field)
	110	{
	111	struct regmap_field *regmap_field;
	112	struct reg_field reg_field = reg_fields[field];
	113	struct device *dev = ddata.cpu;
	114	unsigned int value;
	115	int ret;
	116
	117	reg_field.reg = hw_info_offset;
	118	regmap_field = devm_regmap_field_alloc(dev,
	119	ddata.syscfg_eng,
	120	reg_field);
	121	if (IS_ERR(regmap_field)) {
	122	dev_err(dev, "Failed to allocate reg field\n");
	123	return PTR_ERR(regmap_field);
	124	}
	125
	126	ret = regmap_field_read(regmap_field, &value);
	127	if (ret) {
	128	dev_err(dev, "Failed to read %s code\n",
	129	field ? "SUBSTRATE" : "PCODE");
	130	return ret;
	131	}
	132
	133	return value;
	134	}
	135
	136	static const struct reg_field sti_stih407_dvfs_regfields[DVFS_MAX_REGFIELDS] = {
	137	[PCODE] = REG_FIELD(0, 16, 19),
	138	[SUBSTRATE] = REG_FIELD(0, 0, 2),
	139	};
	140
	141	static const struct reg_field *sti_cpufreq_match(void)
	142	{
	143	if (of_machine_is_compatible("st,stih407") \|\|
	144	of_machine_is_compatible("st,stih410"))
	145	return sti_stih407_dvfs_regfields;
	146
	147	return NULL;
	148	}
	149
	150	static int sti_cpufreq_set_opp_info(void)
	151	{
	152	struct device *dev = ddata.cpu;
	153	struct device_node *np = dev->of_node;
	154	const struct reg_field *reg_fields;
	155	unsigned int hw_info_offset;
	156	unsigned int version[VERSION_ELEMENTS];
	157	int pcode, substrate, major, minor;
158	int ret;
159	char name[MAX_PCODE_NAME_LEN];
fa30184d	160	struct opp_table *opp_table;
ab0ea257 LJ	161
	162	reg_fields = sti_cpufreq_match();
	163	if (!reg_fields) {
9ad0a1b6	164	dev_err(dev, "This SoC doesn't support voltage scaling\n");
ab0ea257 LJ	165	return -ENODEV;
	166	}
	167
	168	ret = of_property_read_u32_index(np, "st,syscfg-eng",
	169	HW_INFO_INDEX, &hw_info_offset);
	170	if (ret) {
	171	dev_warn(dev, "Failed to read HW info offset from DT\n");
	172	substrate = DEFAULT_VERSION;
	173	pcode = 0;
	174	goto use_defaults;
	175	}
	176
	177	pcode = sti_cpufreq_fetch_regmap_field(reg_fields,
	178	hw_info_offset,
	179	PCODE);
	180	if (pcode < 0) {
	181	dev_warn(dev, "Failed to obtain process code\n");
	182	/* Use default pcode */
	183	pcode = 0;
	184	}
	185
	186	substrate = sti_cpufreq_fetch_regmap_field(reg_fields,
	187	hw_info_offset,
	188	SUBSTRATE);
	189	if (substrate) {
	190	dev_warn(dev, "Failed to obtain substrate code\n");
	191	/* Use default substrate */
	192	substrate = DEFAULT_VERSION;
	193	}
	194
	195	use_defaults:
	196	major = sti_cpufreq_fetch_major();
	197	if (major < 0) {
	198	dev_err(dev, "Failed to obtain major version\n");
	199	/* Use default major number */
	200	major = DEFAULT_VERSION;
	201	}
	202
	203	minor = sti_cpufreq_fetch_minor();
	204	if (minor < 0) {
	205	dev_err(dev, "Failed to obtain minor version\n");
	206	/* Use default minor number */
	207	minor = DEFAULT_VERSION;
	208	}
	209
	210	snprintf(name, MAX_PCODE_NAME_LEN, "pcode%d", pcode);
	211
fa30184d VK	212	opp_table = dev_pm_opp_set_prop_name(dev, name);
fa30184d VK	213	if (IS_ERR(opp_table)) {
ab0ea257	214	dev_err(dev, "Failed to set prop name\n");
fa30184d	215	return PTR_ERR(opp_table);
ab0ea257 LJ	216	}
	217
	218	version[0] = BIT(major);
	219	version[1] = BIT(minor);
	220	version[2] = BIT(substrate);
	221
fa30184d VK	222	opp_table = dev_pm_opp_set_supported_hw(dev, version, VERSION_ELEMENTS);
fa30184d VK	223	if (IS_ERR(opp_table)) {
ab0ea257	224	dev_err(dev, "Failed to set supported hardware\n");
fa30184d	225	return PTR_ERR(opp_table);
ab0ea257 LJ	226	}
	227
	228	dev_dbg(dev, "pcode: %d major: %d minor: %d substrate: %d\n",
	229	pcode, major, minor, substrate);
	230	dev_dbg(dev, "version[0]: %x version[1]: %x version[2]: %x\n",
	231	version[0], version[1], version[2]);
	232
	233	return 0;
	234	}
	235
ad61dd30	236	static int sti_cpufreq_fetch_syscon_registers(void)
ab0ea257 LJ	237	{
	238	struct device *dev = ddata.cpu;
	239	struct device_node *np = dev->of_node;
	240
	241	ddata.syscfg = syscon_regmap_lookup_by_phandle(np, "st,syscfg");
	242	if (IS_ERR(ddata.syscfg)) {
	243	dev_err(dev, "\"st,syscfg\" not supplied\n");
	244	return PTR_ERR(ddata.syscfg);
	245	}
	246
	247	ddata.syscfg_eng = syscon_regmap_lookup_by_phandle(np, "st,syscfg-eng");
	248	if (IS_ERR(ddata.syscfg_eng)) {
	249	dev_err(dev, "\"st,syscfg-eng\" not supplied\n");
	250	return PTR_ERR(ddata.syscfg_eng);
	251	}
	252
	253	return 0;
	254	}
	255
	256	static int sti_cpufreq_init(void)
	257	{
	258	int ret;
	259
2482bc31 SH	260	if ((!of_machine_is_compatible("st,stih407")) &&
	261	(!of_machine_is_compatible("st,stih410")))
	262	return -ENODEV;
	263
ab0ea257 LJ	264	ddata.cpu = get_cpu_device(0);
	265	if (!ddata.cpu) {
	266	dev_err(ddata.cpu, "Failed to get device for CPU0\n");
	267	goto skip_voltage_scaling;
	268	}
	269
	270	if (!of_get_property(ddata.cpu->of_node, "operating-points-v2", NULL)) {
	271	dev_err(ddata.cpu, "OPP-v2 not supported\n");
	272	goto skip_voltage_scaling;
	273	}
	274
ad61dd30	275	ret = sti_cpufreq_fetch_syscon_registers();
ab0ea257 LJ	276	if (ret)
	277	goto skip_voltage_scaling;
	278
	279	ret = sti_cpufreq_set_opp_info();
	280	if (!ret)
	281	goto register_cpufreq_dt;
	282
	283	skip_voltage_scaling:
	284	dev_err(ddata.cpu, "Not doing voltage scaling\n");
	285
	286	register_cpufreq_dt:
	287	platform_device_register_simple("cpufreq-dt", -1, NULL, 0);
	288
	289	return 0;
	290	}
	291	module_init(sti_cpufreq_init);
	292
	293	MODULE_DESCRIPTION("STMicroelectronics CPUFreq/OPP driver");
	294	MODULE_AUTHOR("Ajitpal Singh <ajitpal.singh@st.com>");
	295	MODULE_AUTHOR("Lee Jones <lee.jones@linaro.org>");
	296	MODULE_LICENSE("GPL v2");