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 int ret;
135
136 freq_table = bmips_cpufreq_get_freq_table(policy);
137 if (IS_ERR(freq_table)) {
138 ret = PTR_ERR(freq_table);
139 pr_err("%s: couldn't determine frequency table (%d).\n",
140 BMIPS_CPUFREQ_NAME, ret);
141 return ret;
142 }
143
144 ret = cpufreq_generic_init(policy, freq_table, TRANSITION_LATENCY);
145 if (ret)
146 bmips_cpufreq_exit(policy);
147 else
148 pr_info("%s: registered\n", BMIPS_CPUFREQ_NAME);
149
150 return ret;
151 }
152
153 static struct cpufreq_driver bmips_cpufreq_driver = {
154 .flags = CPUFREQ_NEED_INITIAL_FREQ_CHECK,
155 .verify = cpufreq_generic_frequency_table_verify,
156 .target_index = bmips_cpufreq_target_index,
157 .get = bmips_cpufreq_get,
158 .init = bmips_cpufreq_init,
159 .exit = bmips_cpufreq_exit,
160 .attr = cpufreq_generic_attr,
161 .name = BMIPS_CPUFREQ_PREFIX,
162 };
163
bmips_cpufreq_probe(void)164 static int __init bmips_cpufreq_probe(void)
165 {
166 struct cpufreq_compat *cc;
167 struct device_node *np;
168
169 for (cc = bmips_cpufreq_compat; cc->compatible; cc++) {
170 np = of_find_compatible_node(NULL, "cpu", cc->compatible);
171 if (np) {
172 of_node_put(np);
173 priv = cc;
174 break;
175 }
176 }
177
178 /* We hit the guard element of the array. No compatible CPU found. */
179 if (!cc->compatible)
180 return -ENODEV;
181
182 return cpufreq_register_driver(&bmips_cpufreq_driver);
183 }
184 device_initcall(bmips_cpufreq_probe);
185
186 MODULE_AUTHOR("Markus Mayer <mmayer@broadcom.com>");
187 MODULE_DESCRIPTION("CPUfreq driver for Broadcom BMIPS SoCs");
188 MODULE_LICENSE("GPL");
189