1 /*
2 * Copyright (C) 2013-2015 FUJITSU SEMICONDUCTOR LIMITED
3 * Copyright (C) 2015 Linaro Ltd.
4 *
5 * This program is free software: you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation, version 2 of the License.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 */
14
15 #include <linux/clkdev.h>
16 #include <linux/err.h>
17 #include <linux/io.h>
18 #include <linux/of.h>
19 #include <linux/cpu.h>
20 #include <linux/clk-provider.h>
21 #include <linux/spinlock.h>
22 #include <linux/module.h>
23 #include <linux/topology.h>
24 #include <linux/mailbox_client.h>
25 #include <linux/platform_device.h>
26
27 #include <soc/mb86s7x/scb_mhu.h>
28
29 #define to_crg_clk(p) container_of(p, struct crg_clk, hw)
30 #define to_clc_clk(p) container_of(p, struct cl_clk, hw)
31
32 struct mb86s7x_peri_clk {
33 u32 payload_size;
34 u32 cntrlr;
35 u32 domain;
36 u32 port;
37 u32 en;
38 u64 frequency;
39 } __packed __aligned(4);
40
41 struct hack_rate {
42 unsigned clk_id;
43 unsigned long rate;
44 int gated;
45 };
46
47 struct crg_clk {
48 struct clk_hw hw;
49 u8 cntrlr, domain, port;
50 };
51
crg_gate_control(struct clk_hw * hw,int en)52 static int crg_gate_control(struct clk_hw *hw, int en)
53 {
54 struct crg_clk *crgclk = to_crg_clk(hw);
55 struct mb86s7x_peri_clk cmd;
56 int ret;
57
58 cmd.payload_size = sizeof(cmd);
59 cmd.cntrlr = crgclk->cntrlr;
60 cmd.domain = crgclk->domain;
61 cmd.port = crgclk->port;
62 cmd.en = en;
63
64 /* Port is UngatedCLK */
65 if (cmd.port == 8)
66 return en ? 0 : -EINVAL;
67
68 pr_debug("%s:%d CMD Cntrlr-%u Dom-%u Port-%u En-%u}\n",
69 __func__, __LINE__, cmd.cntrlr,
70 cmd.domain, cmd.port, cmd.en);
71
72 ret = mb86s7x_send_packet(CMD_PERI_CLOCK_GATE_SET_REQ,
73 &cmd, sizeof(cmd));
74 if (ret < 0) {
75 pr_err("%s:%d failed!\n", __func__, __LINE__);
76 return ret;
77 }
78
79 pr_debug("%s:%d REP Cntrlr-%u Dom-%u Port-%u En-%u}\n",
80 __func__, __LINE__, cmd.cntrlr,
81 cmd.domain, cmd.port, cmd.en);
82
83 /* If the request was rejected */
84 if (cmd.en != en)
85 ret = -EINVAL;
86 else
87 ret = 0;
88
89 return ret;
90 }
91
crg_port_prepare(struct clk_hw * hw)92 static int crg_port_prepare(struct clk_hw *hw)
93 {
94 return crg_gate_control(hw, 1);
95 }
96
crg_port_unprepare(struct clk_hw * hw)97 static void crg_port_unprepare(struct clk_hw *hw)
98 {
99 crg_gate_control(hw, 0);
100 }
101
102 static int
crg_rate_control(struct clk_hw * hw,int set,unsigned long * rate)103 crg_rate_control(struct clk_hw *hw, int set, unsigned long *rate)
104 {
105 struct crg_clk *crgclk = to_crg_clk(hw);
106 struct mb86s7x_peri_clk cmd;
107 int code, ret;
108
109 cmd.payload_size = sizeof(cmd);
110 cmd.cntrlr = crgclk->cntrlr;
111 cmd.domain = crgclk->domain;
112 cmd.port = crgclk->port;
113 cmd.frequency = *rate;
114
115 if (set) {
116 code = CMD_PERI_CLOCK_RATE_SET_REQ;
117 pr_debug("%s:%d CMD Cntrlr-%u Dom-%u Port-%u Rate-SET %lluHz}\n",
118 __func__, __LINE__, cmd.cntrlr,
119 cmd.domain, cmd.port, cmd.frequency);
120 } else {
121 code = CMD_PERI_CLOCK_RATE_GET_REQ;
122 pr_debug("%s:%d CMD Cntrlr-%u Dom-%u Port-%u Rate-GET}\n",
123 __func__, __LINE__, cmd.cntrlr,
124 cmd.domain, cmd.port);
125 }
126
127 ret = mb86s7x_send_packet(code, &cmd, sizeof(cmd));
128 if (ret < 0) {
129 pr_err("%s:%d failed!\n", __func__, __LINE__);
130 return ret;
131 }
132
133 if (set)
134 pr_debug("%s:%d REP Cntrlr-%u Dom-%u Port-%u Rate-SET %lluHz}\n",
135 __func__, __LINE__, cmd.cntrlr,
136 cmd.domain, cmd.port, cmd.frequency);
137 else
138 pr_debug("%s:%d REP Cntrlr-%u Dom-%u Port-%u Rate-GOT %lluHz}\n",
139 __func__, __LINE__, cmd.cntrlr,
140 cmd.domain, cmd.port, cmd.frequency);
141
142 *rate = cmd.frequency;
143 return 0;
144 }
145
146 static unsigned long
crg_port_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)147 crg_port_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
148 {
149 unsigned long rate;
150
151 crg_rate_control(hw, 0, &rate);
152
153 return rate;
154 }
155
156 static long
crg_port_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * pr)157 crg_port_round_rate(struct clk_hw *hw,
158 unsigned long rate, unsigned long *pr)
159 {
160 return rate;
161 }
162
163 static int
crg_port_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)164 crg_port_set_rate(struct clk_hw *hw,
165 unsigned long rate, unsigned long parent_rate)
166 {
167 return crg_rate_control(hw, 1, &rate);
168 }
169
170 const struct clk_ops crg_port_ops = {
171 .prepare = crg_port_prepare,
172 .unprepare = crg_port_unprepare,
173 .recalc_rate = crg_port_recalc_rate,
174 .round_rate = crg_port_round_rate,
175 .set_rate = crg_port_set_rate,
176 };
177
178 struct mb86s70_crg11 {
179 struct mutex lock; /* protects CLK populating and searching */
180 };
181
crg11_get(struct of_phandle_args * clkspec,void * data)182 static struct clk *crg11_get(struct of_phandle_args *clkspec, void *data)
183 {
184 struct mb86s70_crg11 *crg11 = data;
185 struct clk_init_data init;
186 u32 cntrlr, domain, port;
187 struct crg_clk *crgclk;
188 struct clk *clk;
189 char clkp[20];
190
191 if (clkspec->args_count != 3)
192 return ERR_PTR(-EINVAL);
193
194 cntrlr = clkspec->args[0];
195 domain = clkspec->args[1];
196 port = clkspec->args[2];
197
198 if (port > 7)
199 snprintf(clkp, 20, "UngatedCLK%d_%X", cntrlr, domain);
200 else
201 snprintf(clkp, 20, "CLK%d_%X_%d", cntrlr, domain, port);
202
203 mutex_lock(&crg11->lock);
204
205 clk = __clk_lookup(clkp);
206 if (clk) {
207 mutex_unlock(&crg11->lock);
208 return clk;
209 }
210
211 crgclk = kzalloc(sizeof(*crgclk), GFP_KERNEL);
212 if (!crgclk) {
213 mutex_unlock(&crg11->lock);
214 return ERR_PTR(-ENOMEM);
215 }
216
217 init.name = clkp;
218 init.num_parents = 0;
219 init.ops = &crg_port_ops;
220 init.flags = CLK_IS_ROOT;
221 crgclk->hw.init = &init;
222 crgclk->cntrlr = cntrlr;
223 crgclk->domain = domain;
224 crgclk->port = port;
225 clk = clk_register(NULL, &crgclk->hw);
226 if (IS_ERR(clk))
227 pr_err("%s:%d Error!\n", __func__, __LINE__);
228 else
229 pr_debug("Registered %s\n", clkp);
230
231 clk_register_clkdev(clk, clkp, NULL);
232 mutex_unlock(&crg11->lock);
233 return clk;
234 }
235
crg_port_init(struct device_node * node)236 static void __init crg_port_init(struct device_node *node)
237 {
238 struct mb86s70_crg11 *crg11;
239
240 crg11 = kzalloc(sizeof(*crg11), GFP_KERNEL);
241 if (!crg11)
242 return;
243
244 mutex_init(&crg11->lock);
245
246 of_clk_add_provider(node, crg11_get, crg11);
247 }
248 CLK_OF_DECLARE(crg11_gate, "fujitsu,mb86s70-crg11", crg_port_init);
249
250 struct cl_clk {
251 struct clk_hw hw;
252 int cluster;
253 };
254
255 struct mb86s7x_cpu_freq {
256 u32 payload_size;
257 u32 cluster_class;
258 u32 cluster_id;
259 u32 cpu_id;
260 u64 frequency;
261 };
262
mhu_cluster_rate(struct clk_hw * hw,unsigned long * rate,int get)263 static void mhu_cluster_rate(struct clk_hw *hw, unsigned long *rate, int get)
264 {
265 struct cl_clk *clc = to_clc_clk(hw);
266 struct mb86s7x_cpu_freq cmd;
267 int code, ret;
268
269 cmd.payload_size = sizeof(cmd);
270 cmd.cluster_class = 0;
271 cmd.cluster_id = clc->cluster;
272 cmd.cpu_id = 0;
273 cmd.frequency = *rate;
274
275 if (get)
276 code = CMD_CPU_CLOCK_RATE_GET_REQ;
277 else
278 code = CMD_CPU_CLOCK_RATE_SET_REQ;
279
280 pr_debug("%s:%d CMD Cl_Class-%u CL_ID-%u CPU_ID-%u Freq-%llu}\n",
281 __func__, __LINE__, cmd.cluster_class,
282 cmd.cluster_id, cmd.cpu_id, cmd.frequency);
283
284 ret = mb86s7x_send_packet(code, &cmd, sizeof(cmd));
285 if (ret < 0) {
286 pr_err("%s:%d failed!\n", __func__, __LINE__);
287 return;
288 }
289
290 pr_debug("%s:%d REP Cl_Class-%u CL_ID-%u CPU_ID-%u Freq-%llu}\n",
291 __func__, __LINE__, cmd.cluster_class,
292 cmd.cluster_id, cmd.cpu_id, cmd.frequency);
293
294 *rate = cmd.frequency;
295 }
296
297 static unsigned long
clc_recalc_rate(struct clk_hw * hw,unsigned long unused)298 clc_recalc_rate(struct clk_hw *hw, unsigned long unused)
299 {
300 unsigned long rate;
301
302 mhu_cluster_rate(hw, &rate, 1);
303 return rate;
304 }
305
306 static long
clc_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * unused)307 clc_round_rate(struct clk_hw *hw, unsigned long rate,
308 unsigned long *unused)
309 {
310 return rate;
311 }
312
313 static int
clc_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long unused)314 clc_set_rate(struct clk_hw *hw, unsigned long rate,
315 unsigned long unused)
316 {
317 unsigned long res = rate;
318
319 mhu_cluster_rate(hw, &res, 0);
320
321 return (res == rate) ? 0 : -EINVAL;
322 }
323
324 static struct clk_ops clk_clc_ops = {
325 .recalc_rate = clc_recalc_rate,
326 .round_rate = clc_round_rate,
327 .set_rate = clc_set_rate,
328 };
329
mb86s7x_clclk_register(struct device * cpu_dev)330 struct clk *mb86s7x_clclk_register(struct device *cpu_dev)
331 {
332 struct clk_init_data init;
333 struct cl_clk *clc;
334
335 clc = kzalloc(sizeof(*clc), GFP_KERNEL);
336 if (!clc)
337 return ERR_PTR(-ENOMEM);
338
339 clc->hw.init = &init;
340 clc->cluster = topology_physical_package_id(cpu_dev->id);
341
342 init.name = dev_name(cpu_dev);
343 init.ops = &clk_clc_ops;
344 init.flags = CLK_IS_ROOT | CLK_GET_RATE_NOCACHE;
345 init.num_parents = 0;
346
347 return devm_clk_register(cpu_dev, &clc->hw);
348 }
349
mb86s7x_clclk_of_init(void)350 static int mb86s7x_clclk_of_init(void)
351 {
352 int cpu, ret = -ENODEV;
353 struct device_node *np;
354 struct clk *clk;
355
356 np = of_find_compatible_node(NULL, NULL, "fujitsu,mb86s70-scb-1.0");
357 if (!np || !of_device_is_available(np))
358 goto exit;
359
360 for_each_possible_cpu(cpu) {
361 struct device *cpu_dev = get_cpu_device(cpu);
362
363 if (!cpu_dev) {
364 pr_err("failed to get cpu%d device\n", cpu);
365 continue;
366 }
367
368 clk = mb86s7x_clclk_register(cpu_dev);
369 if (IS_ERR(clk)) {
370 pr_err("failed to register cpu%d clock\n", cpu);
371 continue;
372 }
373 if (clk_register_clkdev(clk, NULL, dev_name(cpu_dev))) {
374 pr_err("failed to register cpu%d clock lookup\n", cpu);
375 continue;
376 }
377 pr_debug("registered clk for %s\n", dev_name(cpu_dev));
378 }
379 ret = 0;
380
381 platform_device_register_simple("arm-bL-cpufreq-dt", -1, NULL, 0);
382 exit:
383 of_node_put(np);
384 return ret;
385 }
386 module_init(mb86s7x_clclk_of_init);
387