[deliverable/linux.git] / drivers / cpufreq / sti-cpufreq.c

/*
 * 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
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the version 2 of the GNU General Public License as
 * published by the Free Software Foundation
 */

#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 %s [%d]\n",
			np->full_name, 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 %s [%d]\n",
			np->full_name, 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];

	reg_fields = sti_cpufreq_match();
	if (!reg_fields) {
		dev_err(dev, "This SoC doesn't support voltage scaling");
		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);

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

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

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

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