[linux-2.6-block.git] / drivers / cpufreq / armada-37xx-cpufreq.c

// SPDX-License-Identifier: GPL-2.0+
/*
 * CPU frequency scaling support for Armada 37xx platform.
 *
 * Copyright (C) 2017 Marvell
 *
 * Gregory CLEMENT <gregory.clement@free-electrons.com>
 */

#include <linux/clk.h>
#include <linux/cpu.h>
#include <linux/cpufreq.h>
#include <linux/err.h>
#include <linux/interrupt.h>
#include <linux/io.h>
#include <linux/mfd/syscon.h>
#include <linux/module.h>
#include <linux/of_address.h>
#include <linux/of_device.h>
#include <linux/of_irq.h>
#include <linux/platform_device.h>
#include <linux/pm_opp.h>
#include <linux/regmap.h>
#include <linux/slab.h>

/* Power management in North Bridge register set */
#define ARMADA_37XX_NB_L0L1	0x18
#define ARMADA_37XX_NB_L2L3	0x1C
#define  ARMADA_37XX_NB_TBG_DIV_OFF	13
#define  ARMADA_37XX_NB_TBG_DIV_MASK	0x7
#define  ARMADA_37XX_NB_CLK_SEL_OFF	11
#define  ARMADA_37XX_NB_CLK_SEL_MASK	0x1
#define  ARMADA_37XX_NB_CLK_SEL_TBG	0x1
#define  ARMADA_37XX_NB_TBG_SEL_OFF	9
#define  ARMADA_37XX_NB_TBG_SEL_MASK	0x3
#define  ARMADA_37XX_NB_VDD_SEL_OFF	6
#define  ARMADA_37XX_NB_VDD_SEL_MASK	0x3
#define  ARMADA_37XX_NB_CONFIG_SHIFT	16
#define ARMADA_37XX_NB_DYN_MOD	0x24
#define  ARMADA_37XX_NB_CLK_SEL_EN	BIT(26)
#define  ARMADA_37XX_NB_TBG_EN		BIT(28)
#define  ARMADA_37XX_NB_DIV_EN		BIT(29)
#define  ARMADA_37XX_NB_VDD_EN		BIT(30)
#define  ARMADA_37XX_NB_DFS_EN		BIT(31)
#define ARMADA_37XX_NB_CPU_LOAD 0x30
#define  ARMADA_37XX_NB_CPU_LOAD_MASK	0x3
#define  ARMADA_37XX_DVFS_LOAD_0	0
#define  ARMADA_37XX_DVFS_LOAD_1	1
#define  ARMADA_37XX_DVFS_LOAD_2	2
#define  ARMADA_37XX_DVFS_LOAD_3	3

/*
 * On Armada 37xx the Power management manages 4 level of CPU load,
 * each level can be associated with a CPU clock source, a CPU
 * divider, a VDD level, etc...
 */
#define LOAD_LEVEL_NR	4

struct armada_37xx_dvfs {
	u32 cpu_freq_max;
	u8 divider[LOAD_LEVEL_NR];
};

static struct armada_37xx_dvfs armada_37xx_dvfs[] = {
	{.cpu_freq_max = 1200*1000*1000, .divider = {1, 2, 4, 6} },
	{.cpu_freq_max = 1000*1000*1000, .divider = {1, 2, 4, 5} },
	{.cpu_freq_max = 800*1000*1000,  .divider = {1, 2, 3, 4} },
	{.cpu_freq_max = 600*1000*1000,  .divider = {2, 4, 5, 6} },
};

static struct armada_37xx_dvfs *armada_37xx_cpu_freq_info_get(u32 freq)
{
	int i;

	for (i = 0; i < ARRAY_SIZE(armada_37xx_dvfs); i++) {
		if (freq == armada_37xx_dvfs[i].cpu_freq_max)
			return &armada_37xx_dvfs[i];
	}

	pr_err("Unsupported CPU frequency %d MHz\n", freq/1000000);
	return NULL;
}

/*
 * Setup the four level managed by the hardware. Once the four level
 * will be configured then the DVFS will be enabled.
 */
static void __init armada37xx_cpufreq_dvfs_setup(struct regmap *base,
						 struct clk *clk, u8 *divider)
{
	int load_lvl;
	struct clk *parent;

	for (load_lvl = 0; load_lvl < LOAD_LEVEL_NR; load_lvl++) {
		unsigned int reg, mask, val, offset = 0;

		if (load_lvl <= ARMADA_37XX_DVFS_LOAD_1)
			reg = ARMADA_37XX_NB_L0L1;
		else
			reg = ARMADA_37XX_NB_L2L3;

		if (load_lvl == ARMADA_37XX_DVFS_LOAD_0 ||
		    load_lvl == ARMADA_37XX_DVFS_LOAD_2)
			offset += ARMADA_37XX_NB_CONFIG_SHIFT;

		/* Set cpu clock source, for all the level we use TBG */
		val = ARMADA_37XX_NB_CLK_SEL_TBG << ARMADA_37XX_NB_CLK_SEL_OFF;
		mask = (ARMADA_37XX_NB_CLK_SEL_MASK
			<< ARMADA_37XX_NB_CLK_SEL_OFF);

		/*
		 * Set cpu divider based on the pre-computed array in
		 * order to have balanced step.
		 */
		val |= divider[load_lvl] << ARMADA_37XX_NB_TBG_DIV_OFF;
		mask |= (ARMADA_37XX_NB_TBG_DIV_MASK
			<< ARMADA_37XX_NB_TBG_DIV_OFF);

		/* Set VDD divider which is actually the load level. */
		val |= load_lvl << ARMADA_37XX_NB_VDD_SEL_OFF;
		mask |= (ARMADA_37XX_NB_VDD_SEL_MASK
			<< ARMADA_37XX_NB_VDD_SEL_OFF);

		val <<= offset;
		mask <<= offset;

		regmap_update_bits(base, reg, mask, val);
	}

	/*
	 * Set cpu clock source, for all the level we keep the same
	 * clock source that the one already configured. For this one
	 * we need to use the clock framework
	 */
	parent = clk_get_parent(clk);
	clk_set_parent(clk, parent);
}

static void __init armada37xx_cpufreq_disable_dvfs(struct regmap *base)
{
	unsigned int reg = ARMADA_37XX_NB_DYN_MOD,
		mask = ARMADA_37XX_NB_DFS_EN;

	regmap_update_bits(base, reg, mask, 0);
}

static void __init armada37xx_cpufreq_enable_dvfs(struct regmap *base)
{
	unsigned int val, reg = ARMADA_37XX_NB_CPU_LOAD,
		mask = ARMADA_37XX_NB_CPU_LOAD_MASK;

	/* Start with the highest load (0) */
	val = ARMADA_37XX_DVFS_LOAD_0;
	regmap_update_bits(base, reg, mask, val);

	/* Now enable DVFS for the CPUs */
	reg = ARMADA_37XX_NB_DYN_MOD;
	mask =	ARMADA_37XX_NB_CLK_SEL_EN | ARMADA_37XX_NB_TBG_EN |
		ARMADA_37XX_NB_DIV_EN | ARMADA_37XX_NB_VDD_EN |
		ARMADA_37XX_NB_DFS_EN;

	regmap_update_bits(base, reg, mask, mask);
}

static int __init armada37xx_cpufreq_driver_init(void)
{
	struct armada_37xx_dvfs *dvfs;
	struct platform_device *pdev;
	unsigned long freq;
	unsigned int cur_frequency;
	struct regmap *nb_pm_base;
	struct device *cpu_dev;
	int load_lvl, ret;
	struct clk *clk;

	nb_pm_base =
		syscon_regmap_lookup_by_compatible("marvell,armada-3700-nb-pm");

	if (IS_ERR(nb_pm_base))
		return -ENODEV;

	/* Before doing any configuration on the DVFS first, disable it */
	armada37xx_cpufreq_disable_dvfs(nb_pm_base);

	/*
	 * On CPU 0 register the operating points supported (which are
	 * the nominal CPU frequency and full integer divisions of
	 * it).
	 */
	cpu_dev = get_cpu_device(0);
	if (!cpu_dev) {
		dev_err(cpu_dev, "Cannot get CPU\n");
		return -ENODEV;
	}

	clk = clk_get(cpu_dev, 0);
	if (IS_ERR(clk)) {
		dev_err(cpu_dev, "Cannot get clock for CPU0\n");
		return PTR_ERR(clk);
	}

	/* Get nominal (current) CPU frequency */
	cur_frequency = clk_get_rate(clk);
	if (!cur_frequency) {
		dev_err(cpu_dev, "Failed to get clock rate for CPU\n");
		clk_put(clk);
		return -EINVAL;
	}

	dvfs = armada_37xx_cpu_freq_info_get(cur_frequency);
	if (!dvfs) {
		clk_put(clk);
		return -EINVAL;
	}

	armada37xx_cpufreq_dvfs_setup(nb_pm_base, clk, dvfs->divider);
	clk_put(clk);

	for (load_lvl = ARMADA_37XX_DVFS_LOAD_0; load_lvl < LOAD_LEVEL_NR;
	     load_lvl++) {
		freq = cur_frequency / dvfs->divider[load_lvl];

		ret = dev_pm_opp_add(cpu_dev, freq, 0);
		if (ret)
			goto remove_opp;
	}

	/* Now that everything is setup, enable the DVFS at hardware level */
	armada37xx_cpufreq_enable_dvfs(nb_pm_base);

	pdev = platform_device_register_simple("cpufreq-dt", -1, NULL, 0);
	ret = PTR_ERR_OR_ZERO(pdev);
	if (ret)
		goto disable_dvfs;

	return 0;

disable_dvfs:
	armada37xx_cpufreq_disable_dvfs(nb_pm_base);
remove_opp:
	/* clean-up the already added opp before leaving */
	while (load_lvl-- > ARMADA_37XX_DVFS_LOAD_0) {
		freq = cur_frequency / dvfs->divider[load_lvl];
		dev_pm_opp_remove(cpu_dev, freq);
	}

	return ret;
}
/* late_initcall, to guarantee the driver is loaded after A37xx clock driver */
late_initcall(armada37xx_cpufreq_driver_init);

MODULE_AUTHOR("Gregory CLEMENT <gregory.clement@free-electrons.com>");
MODULE_DESCRIPTION("Armada 37xx cpufreq driver");
MODULE_LICENSE("GPL");
Commit	Line	Data
92ce45fb GC	1	// SPDX-License-Identifier: GPL-2.0+
	2	/*
	3	* CPU frequency scaling support for Armada 37xx platform.
	4	*
	5	* Copyright (C) 2017 Marvell
	6	*
	7	* Gregory CLEMENT <gregory.clement@free-electrons.com>
	8	*/
	9
	10	#include <linux/clk.h>
	11	#include <linux/cpu.h>
	12	#include <linux/cpufreq.h>
	13	#include <linux/err.h>
	14	#include <linux/interrupt.h>
	15	#include <linux/io.h>
	16	#include <linux/mfd/syscon.h>
	17	#include <linux/module.h>
	18	#include <linux/of_address.h>
	19	#include <linux/of_device.h>
	20	#include <linux/of_irq.h>
	21	#include <linux/platform_device.h>
	22	#include <linux/pm_opp.h>
	23	#include <linux/regmap.h>
	24	#include <linux/slab.h>
	25
	26	/* Power management in North Bridge register set */
	27	#define ARMADA_37XX_NB_L0L1 0x18
	28	#define ARMADA_37XX_NB_L2L3 0x1C
	29	#define ARMADA_37XX_NB_TBG_DIV_OFF 13
	30	#define ARMADA_37XX_NB_TBG_DIV_MASK 0x7
	31	#define ARMADA_37XX_NB_CLK_SEL_OFF 11
	32	#define ARMADA_37XX_NB_CLK_SEL_MASK 0x1
	33	#define ARMADA_37XX_NB_CLK_SEL_TBG 0x1
	34	#define ARMADA_37XX_NB_TBG_SEL_OFF 9
	35	#define ARMADA_37XX_NB_TBG_SEL_MASK 0x3
	36	#define ARMADA_37XX_NB_VDD_SEL_OFF 6
	37	#define ARMADA_37XX_NB_VDD_SEL_MASK 0x3
	38	#define ARMADA_37XX_NB_CONFIG_SHIFT 16
	39	#define ARMADA_37XX_NB_DYN_MOD 0x24
	40	#define ARMADA_37XX_NB_CLK_SEL_EN BIT(26)
	41	#define ARMADA_37XX_NB_TBG_EN BIT(28)
	42	#define ARMADA_37XX_NB_DIV_EN BIT(29)
	43	#define ARMADA_37XX_NB_VDD_EN BIT(30)
	44	#define ARMADA_37XX_NB_DFS_EN BIT(31)
	45	#define ARMADA_37XX_NB_CPU_LOAD 0x30
	46	#define ARMADA_37XX_NB_CPU_LOAD_MASK 0x3
	47	#define ARMADA_37XX_DVFS_LOAD_0 0
	48	#define ARMADA_37XX_DVFS_LOAD_1 1
	49	#define ARMADA_37XX_DVFS_LOAD_2 2
	50	#define ARMADA_37XX_DVFS_LOAD_3 3
	51
	52	/*
	53	* On Armada 37xx the Power management manages 4 level of CPU load,
	54	* each level can be associated with a CPU clock source, a CPU
	55	* divider, a VDD level, etc...
	56	*/
	57	#define LOAD_LEVEL_NR 4
	58
	59	struct armada_37xx_dvfs {
	60	u32 cpu_freq_max;
	61	u8 divider[LOAD_LEVEL_NR];
	62	};
	63
	64	static struct armada_37xx_dvfs armada_37xx_dvfs[] = {
65	{.cpu_freq_max = 120010001000, .divider = {1, 2, 4, 6} },
66	{.cpu_freq_max = 100010001000, .divider = {1, 2, 4, 5} },
67	{.cpu_freq_max = 80010001000, .divider = {1, 2, 3, 4} },
68	{.cpu_freq_max = 60010001000, .divider = {2, 4, 5, 6} },
69	};
70
71	static struct armada_37xx_dvfs *armada_37xx_cpu_freq_info_get(u32 freq)
72	{
73	int i;
74
75	for (i = 0; i < ARRAY_SIZE(armada_37xx_dvfs); i++) {
76	if (freq == armada_37xx_dvfs[i].cpu_freq_max)
77	return &armada_37xx_dvfs[i];
78	}
79
80	pr_err("Unsupported CPU frequency %d MHz\n", freq/1000000);
81	return NULL;
82	}
83
84	/*
85	* Setup the four level managed by the hardware. Once the four level
86	* will be configured then the DVFS will be enabled.
87	*/
88	static void __init armada37xx_cpufreq_dvfs_setup(struct regmap *base,
89	struct clk clk, u8 divider)
90	{
91	int load_lvl;
92	struct clk *parent;
93
94	for (load_lvl = 0; load_lvl < LOAD_LEVEL_NR; load_lvl++) {
95	unsigned int reg, mask, val, offset = 0;
96
97	if (load_lvl <= ARMADA_37XX_DVFS_LOAD_1)
98	reg = ARMADA_37XX_NB_L0L1;
99	else
100	reg = ARMADA_37XX_NB_L2L3;
101
102	if (load_lvl == ARMADA_37XX_DVFS_LOAD_0 \|\|
103	load_lvl == ARMADA_37XX_DVFS_LOAD_2)
104	offset += ARMADA_37XX_NB_CONFIG_SHIFT;
105
106	/* Set cpu clock source, for all the level we use TBG */
107	val = ARMADA_37XX_NB_CLK_SEL_TBG << ARMADA_37XX_NB_CLK_SEL_OFF;
108	mask = (ARMADA_37XX_NB_CLK_SEL_MASK
109	<< ARMADA_37XX_NB_CLK_SEL_OFF);
110
111	/*
112	* Set cpu divider based on the pre-computed array in
113	* order to have balanced step.
114	*/
115	val \|= divider[load_lvl] << ARMADA_37XX_NB_TBG_DIV_OFF;
116	mask \|= (ARMADA_37XX_NB_TBG_DIV_MASK
117	<< ARMADA_37XX_NB_TBG_DIV_OFF);
118
119	/* Set VDD divider which is actually the load level. */
120	val \|= load_lvl << ARMADA_37XX_NB_VDD_SEL_OFF;
121	mask \|= (ARMADA_37XX_NB_VDD_SEL_MASK
122	<< ARMADA_37XX_NB_VDD_SEL_OFF);
123
124	val <<= offset;
125	mask <<= offset;
126
127	regmap_update_bits(base, reg, mask, val);
128	}
129
130	/*
131	* Set cpu clock source, for all the level we keep the same
132	* clock source that the one already configured. For this one
133	* we need to use the clock framework
134	*/
135	parent = clk_get_parent(clk);
136	clk_set_parent(clk, parent);
137	}
138
139	static void __init armada37xx_cpufreq_disable_dvfs(struct regmap *base)
140	{
141	unsigned int reg = ARMADA_37XX_NB_DYN_MOD,
142	mask = ARMADA_37XX_NB_DFS_EN;
143
144	regmap_update_bits(base, reg, mask, 0);
145	}
146
147	static void __init armada37xx_cpufreq_enable_dvfs(struct regmap *base)
148	{
149	unsigned int val, reg = ARMADA_37XX_NB_CPU_LOAD,
150	mask = ARMADA_37XX_NB_CPU_LOAD_MASK;
151
152	/* Start with the highest load (0) */
153	val = ARMADA_37XX_DVFS_LOAD_0;
154	regmap_update_bits(base, reg, mask, val);
155
156	/* Now enable DVFS for the CPUs */
157	reg = ARMADA_37XX_NB_DYN_MOD;
158	mask = ARMADA_37XX_NB_CLK_SEL_EN \| ARMADA_37XX_NB_TBG_EN \|
159	ARMADA_37XX_NB_DIV_EN \| ARMADA_37XX_NB_VDD_EN \|
160	ARMADA_37XX_NB_DFS_EN;
161
162	regmap_update_bits(base, reg, mask, mask);
163	}
164
165	static int __init armada37xx_cpufreq_driver_init(void)
166	{
167	struct armada_37xx_dvfs *dvfs;
168	struct platform_device *pdev;
cfd84631	169	unsigned long freq;
92ce45fb GC	170	unsigned int cur_frequency;
	171	struct regmap *nb_pm_base;
	172	struct device *cpu_dev;
	173	int load_lvl, ret;
	174	struct clk *clk;
	175
	176	nb_pm_base =
	177	syscon_regmap_lookup_by_compatible("marvell,armada-3700-nb-pm");
	178
	179	if (IS_ERR(nb_pm_base))
	180	return -ENODEV;
	181
	182	/* Before doing any configuration on the DVFS first, disable it */
	183	armada37xx_cpufreq_disable_dvfs(nb_pm_base);
	184
	185	/*
	186	* On CPU 0 register the operating points supported (which are
	187	* the nominal CPU frequency and full integer divisions of
	188	* it).
	189	*/
	190	cpu_dev = get_cpu_device(0);
	191	if (!cpu_dev) {
	192	dev_err(cpu_dev, "Cannot get CPU\n");
	193	return -ENODEV;
	194	}
	195
	196	clk = clk_get(cpu_dev, 0);
	197	if (IS_ERR(clk)) {
	198	dev_err(cpu_dev, "Cannot get clock for CPU0\n");
	199	return PTR_ERR(clk);
	200	}
	201
	202	/* Get nominal (current) CPU frequency */
	203	cur_frequency = clk_get_rate(clk);
	204	if (!cur_frequency) {
	205	dev_err(cpu_dev, "Failed to get clock rate for CPU\n");
bbcc3285	206	clk_put(clk);
92ce45fb GC	207	return -EINVAL;
	208	}
	209
	210	dvfs = armada_37xx_cpu_freq_info_get(cur_frequency);
cfd84631 VK	211	if (!dvfs) {
cfd84631 VK	212	clk_put(clk);
92ce45fb	213	return -EINVAL;
cfd84631	214	}
92ce45fb GC	215
92ce45fb GC	216	armada37xx_cpufreq_dvfs_setup(nb_pm_base, clk, dvfs->divider);
bbcc3285	217	clk_put(clk);
92ce45fb GC	218
	219	for (load_lvl = ARMADA_37XX_DVFS_LOAD_0; load_lvl < LOAD_LEVEL_NR;
	220	load_lvl++) {
cfd84631	221	freq = cur_frequency / dvfs->divider[load_lvl];
92ce45fb GC	222
92ce45fb GC	223	ret = dev_pm_opp_add(cpu_dev, freq, 0);
cfd84631 VK	224	if (ret)
cfd84631 VK	225	goto remove_opp;
92ce45fb GC	226	}
	227
	228	/* Now that everything is setup, enable the DVFS at hardware level */
	229	armada37xx_cpufreq_enable_dvfs(nb_pm_base);
	230
	231	pdev = platform_device_register_simple("cpufreq-dt", -1, NULL, 0);
cfd84631 VK	232	ret = PTR_ERR_OR_ZERO(pdev);
	233	if (ret)
	234	goto disable_dvfs;
	235
	236	return 0;
	237
	238	disable_dvfs:
	239	armada37xx_cpufreq_disable_dvfs(nb_pm_base);
	240	remove_opp:
	241	/* clean-up the already added opp before leaving */
	242	while (load_lvl-- > ARMADA_37XX_DVFS_LOAD_0) {
	243	freq = cur_frequency / dvfs->divider[load_lvl];
	244	dev_pm_opp_remove(cpu_dev, freq);
	245	}
92ce45fb	246
cfd84631	247	return ret;
92ce45fb GC	248	}
	249	/* late_initcall, to guarantee the driver is loaded after A37xx clock driver */
	250	late_initcall(armada37xx_cpufreq_driver_init);
	251
	252	MODULE_AUTHOR("Gregory CLEMENT <gregory.clement@free-electrons.com>");
	253	MODULE_DESCRIPTION("Armada 37xx cpufreq driver");
	254	MODULE_LICENSE("GPL");