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 
33 enum bmips_type {
34 	BMIPS5000,
35 	BMIPS5200,
36 };
37 
38 struct 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 
52 static struct cpufreq_compat bmips_cpufreq_compat[] = {
53 	BMIPS("brcm,bmips5000", BMIPS5000, 8, 4),
54 	BMIPS("brcm,bmips5200", BMIPS5200, 8, 4),
55 	{ }
56 };
57 
58 static struct cpufreq_compat *priv;
59 
htp_freq_to_cpu_freq(unsigned int clk_mult)60 static int htp_freq_to_cpu_freq(unsigned int clk_mult)
61 {
62 	return mips_hpt_frequency * clk_mult / 1000;
63 }
64 
65 static struct cpufreq_frequency_table *
bmips_cpufreq_get_freq_table(const struct cpufreq_policy * policy)66 bmips_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 
bmips_cpufreq_get(unsigned int cpu)87 static 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 
bmips_cpufreq_target_index(struct cpufreq_policy * policy,unsigned int index)105 static 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 
bmips_cpufreq_exit(struct cpufreq_policy * policy)124 static int bmips_cpufreq_exit(struct cpufreq_policy *policy)
125 {
126 	kfree(policy->freq_table);
127 
128 	return 0;
129 }
130 
bmips_cpufreq_init(struct cpufreq_policy * policy)131 static 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 
148 static 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 
bmips_cpufreq_driver_init(void)159 static 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 }
179 module_init(bmips_cpufreq_driver_init);
180 
bmips_cpufreq_driver_exit(void)181 static void __exit bmips_cpufreq_driver_exit(void)
182 {
183 	cpufreq_unregister_driver(&bmips_cpufreq_driver);
184 }
185 module_exit(bmips_cpufreq_driver_exit);
186 
187 MODULE_AUTHOR("Markus Mayer <mmayer@broadcom.com>");
188 MODULE_DESCRIPTION("CPUfreq driver for Broadcom BMIPS SoCs");
189 MODULE_LICENSE("GPL");
190