1/*
2 * CPU frequency scaling for Broadcom BMIPS SoCs
3 *
4 * Copyright (c) 2017 Broadcom
5 *
6 * This program is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License as
8 * published by the Free Software Foundation version 2.
9 *
10 * This program is distributed "as is" WITHOUT ANY WARRANTY of any
11 * kind, whether express or implied; without even the implied warranty
12 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 * GNU General Public License for more details.
14 */
15
16#include <linux/cpufreq.h>
17#include <linux/module.h>
18#include <linux/of_address.h>
19#include <linux/slab.h>
20
21/* for mips_hpt_frequency */
22#include <asm/time.h>
23
24#define BMIPS_CPUFREQ_PREFIX	"bmips"
25#define BMIPS_CPUFREQ_NAME	BMIPS_CPUFREQ_PREFIX "-cpufreq"
26
27#define TRANSITION_LATENCY	(25 * 1000)	/* 25 us */
28
29#define BMIPS5_CLK_DIV_SET_SHIFT	0x7
30#define BMIPS5_CLK_DIV_SHIFT		0x4
31#define BMIPS5_CLK_DIV_MASK		0xf
32
33enum bmips_type {
34	BMIPS5000,
35	BMIPS5200,
36};
37
38struct cpufreq_compat {
39	const char *compatible;
40	unsigned int bmips_type;
41	unsigned int clk_mult;
42	unsigned int max_freqs;
43};
44
45#define BMIPS(c, t, m, f) { \
46	.compatible = c, \
47	.bmips_type = (t), \
48	.clk_mult = (m), \
49	.max_freqs = (f), \
50}
51
52static struct cpufreq_compat bmips_cpufreq_compat[] = {
53	BMIPS("brcm,bmips5000", BMIPS5000, 8, 4),
54	BMIPS("brcm,bmips5200", BMIPS5200, 8, 4),
55	{ }
56};
57
58static struct cpufreq_compat *priv;
59
60static int htp_freq_to_cpu_freq(unsigned int clk_mult)
61{
62	return mips_hpt_frequency * clk_mult / 1000;
63}
64
65static struct cpufreq_frequency_table *
66bmips_cpufreq_get_freq_table(const struct cpufreq_policy *policy)
67{
68	struct cpufreq_frequency_table *table;
69	unsigned long cpu_freq;
70	int i;
71
72	cpu_freq = htp_freq_to_cpu_freq(priv->clk_mult);
73
74	table = kmalloc_array(priv->max_freqs + 1, sizeof(*table), GFP_KERNEL);
75	if (!table)
76		return ERR_PTR(-ENOMEM);
77
78	for (i = 0; i < priv->max_freqs; i++) {
79		table[i].frequency = cpu_freq / (1 << i);
80		table[i].driver_data = i;
81	}
82	table[i].frequency = CPUFREQ_TABLE_END;
83
84	return table;
85}
86
87static unsigned int bmips_cpufreq_get(unsigned int cpu)
88{
89	unsigned int div;
90	uint32_t mode;
91
92	switch (priv->bmips_type) {
93	case BMIPS5200:
94	case BMIPS5000:
95		mode = read_c0_brcm_mode();
96		div = ((mode >> BMIPS5_CLK_DIV_SHIFT) & BMIPS5_CLK_DIV_MASK);
97		break;
98	default:
99		div = 0;
100	}
101
102	return htp_freq_to_cpu_freq(priv->clk_mult) / (1 << div);
103}
104
105static int bmips_cpufreq_target_index(struct cpufreq_policy *policy,
106				      unsigned int index)
107{
108	unsigned int div = policy->freq_table[index].driver_data;
109
110	switch (priv->bmips_type) {
111	case BMIPS5200:
112	case BMIPS5000:
113		change_c0_brcm_mode(BMIPS5_CLK_DIV_MASK << BMIPS5_CLK_DIV_SHIFT,
114				    (1 << BMIPS5_CLK_DIV_SET_SHIFT) |
115				    (div << BMIPS5_CLK_DIV_SHIFT));
116		break;
117	default:
118		return -ENOTSUPP;
119	}
120
121	return 0;
122}
123
124static int bmips_cpufreq_exit(struct cpufreq_policy *policy)
125{
126	kfree(policy->freq_table);
127
128	return 0;
129}
130
131static int bmips_cpufreq_init(struct cpufreq_policy *policy)
132{
133	struct cpufreq_frequency_table *freq_table;
134
135	freq_table = bmips_cpufreq_get_freq_table(policy);
136	if (IS_ERR(freq_table)) {
137		pr_err("%s: couldn't determine frequency table (%ld).\n",
138			BMIPS_CPUFREQ_NAME, PTR_ERR(freq_table));
139		return PTR_ERR(freq_table);
140	}
141
142	cpufreq_generic_init(policy, freq_table, TRANSITION_LATENCY);
143	pr_info("%s: registered\n", BMIPS_CPUFREQ_NAME);
144
145	return 0;
146}
147
148static struct cpufreq_driver bmips_cpufreq_driver = {
149	.flags		= CPUFREQ_NEED_INITIAL_FREQ_CHECK,
150	.verify		= cpufreq_generic_frequency_table_verify,
151	.target_index	= bmips_cpufreq_target_index,
152	.get		= bmips_cpufreq_get,
153	.init		= bmips_cpufreq_init,
154	.exit		= bmips_cpufreq_exit,
155	.attr		= cpufreq_generic_attr,
156	.name		= BMIPS_CPUFREQ_PREFIX,
157};
158
159static int __init bmips_cpufreq_driver_init(void)
160{
161	struct cpufreq_compat *cc;
162	struct device_node *np;
163
164	for (cc = bmips_cpufreq_compat; cc->compatible; cc++) {
165		np = of_find_compatible_node(NULL, "cpu", cc->compatible);
166		if (np) {
167			of_node_put(np);
168			priv = cc;
169			break;
170		}
171	}
172
173	/* We hit the guard element of the array. No compatible CPU found. */
174	if (!cc->compatible)
175		return -ENODEV;
176
177	return cpufreq_register_driver(&bmips_cpufreq_driver);
178}
179module_init(bmips_cpufreq_driver_init);
180
181static void __exit bmips_cpufreq_driver_exit(void)
182{
183	cpufreq_unregister_driver(&bmips_cpufreq_driver);
184}
185module_exit(bmips_cpufreq_driver_exit);
186
187MODULE_AUTHOR("Markus Mayer <mmayer@broadcom.com>");
188MODULE_DESCRIPTION("CPUfreq driver for Broadcom BMIPS SoCs");
189MODULE_LICENSE("GPL");
190