xref: /linux/drivers/clk/tegra/clk-bpmp.c (revision 0ea5c948cb64bab5bc7a5516774eb8536f05aa0d)
1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * Copyright (C) 2016-2022 NVIDIA Corporation
4  */
5 
6 #include <linux/clk-provider.h>
7 #include <linux/device.h>
8 #include <linux/seq_buf.h>
9 #include <linux/slab.h>
10 
11 #include <soc/tegra/bpmp.h>
12 #include <soc/tegra/bpmp-abi.h>
13 
14 #define TEGRA_BPMP_DUMP_CLOCK_INFO	0
15 
16 #define TEGRA_BPMP_CLK_HAS_MUX		BIT(0)
17 #define TEGRA_BPMP_CLK_HAS_SET_RATE	BIT(1)
18 #define TEGRA_BPMP_CLK_IS_ROOT		BIT(2)
19 
20 struct tegra_bpmp_clk_info {
21 	unsigned int id;
22 	char name[MRQ_CLK_NAME_MAXLEN];
23 	unsigned int parents[MRQ_CLK_MAX_PARENTS];
24 	unsigned int num_parents;
25 	unsigned long flags;
26 };
27 
28 struct tegra_bpmp_clk {
29 	struct clk_hw hw;
30 
31 	struct tegra_bpmp *bpmp;
32 	unsigned int id;
33 
34 	unsigned int num_parents;
35 	unsigned int *parents;
36 };
37 
to_tegra_bpmp_clk(struct clk_hw * hw)38 static inline struct tegra_bpmp_clk *to_tegra_bpmp_clk(struct clk_hw *hw)
39 {
40 	return container_of(hw, struct tegra_bpmp_clk, hw);
41 }
42 
43 struct tegra_bpmp_clk_message {
44 	unsigned int cmd;
45 	unsigned int id;
46 
47 	struct {
48 		const void *data;
49 		size_t size;
50 	} tx;
51 
52 	struct {
53 		void *data;
54 		size_t size;
55 		int ret;
56 	} rx;
57 };
58 
tegra_bpmp_clk_transfer(struct tegra_bpmp * bpmp,const struct tegra_bpmp_clk_message * clk)59 static int tegra_bpmp_clk_transfer(struct tegra_bpmp *bpmp,
60 				   const struct tegra_bpmp_clk_message *clk)
61 {
62 	struct mrq_clk_request request;
63 	struct tegra_bpmp_message msg;
64 	void *req = &request;
65 	int err;
66 
67 	memset(&request, 0, sizeof(request));
68 	request.cmd_and_id = (clk->cmd << 24) | clk->id;
69 
70 	/*
71 	 * The mrq_clk_request structure has an anonymous union at offset 4
72 	 * that contains all possible sub-command structures. Copy the data
73 	 * to that union. Ideally we'd be able to refer to it by name, but
74 	 * doing so would require changing the ABI header and increase the
75 	 * maintenance burden.
76 	 */
77 	memcpy(req + 4, clk->tx.data, clk->tx.size);
78 
79 	memset(&msg, 0, sizeof(msg));
80 	msg.mrq = MRQ_CLK;
81 	msg.tx.data = &request;
82 	msg.tx.size = sizeof(request);
83 	msg.rx.data = clk->rx.data;
84 	msg.rx.size = clk->rx.size;
85 
86 	err = tegra_bpmp_transfer(bpmp, &msg);
87 	if (err < 0)
88 		return err;
89 	else if (msg.rx.ret < 0)
90 		return -EINVAL;
91 
92 	return 0;
93 }
94 
tegra_bpmp_clk_prepare(struct clk_hw * hw)95 static int tegra_bpmp_clk_prepare(struct clk_hw *hw)
96 {
97 	struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
98 	struct tegra_bpmp_clk_message msg;
99 
100 	memset(&msg, 0, sizeof(msg));
101 	msg.cmd = CMD_CLK_ENABLE;
102 	msg.id = clk->id;
103 
104 	return tegra_bpmp_clk_transfer(clk->bpmp, &msg);
105 }
106 
tegra_bpmp_clk_unprepare(struct clk_hw * hw)107 static void tegra_bpmp_clk_unprepare(struct clk_hw *hw)
108 {
109 	struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
110 	struct tegra_bpmp_clk_message msg;
111 	int err;
112 
113 	memset(&msg, 0, sizeof(msg));
114 	msg.cmd = CMD_CLK_DISABLE;
115 	msg.id = clk->id;
116 
117 	err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
118 	if (err < 0)
119 		dev_err(clk->bpmp->dev, "failed to disable clock %s: %d\n",
120 			clk_hw_get_name(hw), err);
121 }
122 
tegra_bpmp_clk_is_prepared(struct clk_hw * hw)123 static int tegra_bpmp_clk_is_prepared(struct clk_hw *hw)
124 {
125 	struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
126 	struct cmd_clk_is_enabled_response response;
127 	struct tegra_bpmp_clk_message msg;
128 	int err;
129 
130 	memset(&msg, 0, sizeof(msg));
131 	msg.cmd = CMD_CLK_IS_ENABLED;
132 	msg.id = clk->id;
133 	msg.rx.data = &response;
134 	msg.rx.size = sizeof(response);
135 
136 	err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
137 	if (err < 0)
138 		return err;
139 
140 	return response.state;
141 }
142 
tegra_bpmp_clk_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)143 static unsigned long tegra_bpmp_clk_recalc_rate(struct clk_hw *hw,
144 						unsigned long parent_rate)
145 {
146 	struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
147 	struct cmd_clk_get_rate_response response;
148 	struct cmd_clk_get_rate_request request;
149 	struct tegra_bpmp_clk_message msg;
150 	int err;
151 
152 	memset(&msg, 0, sizeof(msg));
153 	msg.cmd = CMD_CLK_GET_RATE;
154 	msg.id = clk->id;
155 	msg.tx.data = &request;
156 	msg.tx.size = sizeof(request);
157 	msg.rx.data = &response;
158 	msg.rx.size = sizeof(response);
159 
160 	err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
161 	if (err < 0)
162 		return 0;
163 
164 	return response.rate;
165 }
166 
tegra_bpmp_clk_determine_rate(struct clk_hw * hw,struct clk_rate_request * rate_req)167 static int tegra_bpmp_clk_determine_rate(struct clk_hw *hw,
168 					 struct clk_rate_request *rate_req)
169 {
170 	struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
171 	struct cmd_clk_round_rate_response response;
172 	struct cmd_clk_round_rate_request request;
173 	struct tegra_bpmp_clk_message msg;
174 	unsigned long rate;
175 	int err;
176 
177 	rate = min(max(rate_req->rate, rate_req->min_rate), rate_req->max_rate);
178 
179 	memset(&request, 0, sizeof(request));
180 	request.rate = min_t(u64, rate, S64_MAX);
181 
182 	memset(&msg, 0, sizeof(msg));
183 	msg.cmd = CMD_CLK_ROUND_RATE;
184 	msg.id = clk->id;
185 	msg.tx.data = &request;
186 	msg.tx.size = sizeof(request);
187 	msg.rx.data = &response;
188 	msg.rx.size = sizeof(response);
189 
190 	err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
191 	if (err < 0)
192 		return err;
193 
194 	rate_req->rate = (unsigned long)response.rate;
195 
196 	return 0;
197 }
198 
tegra_bpmp_clk_set_parent(struct clk_hw * hw,u8 index)199 static int tegra_bpmp_clk_set_parent(struct clk_hw *hw, u8 index)
200 {
201 	struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
202 	struct cmd_clk_set_parent_response response;
203 	struct cmd_clk_set_parent_request request;
204 	struct tegra_bpmp_clk_message msg;
205 	int err;
206 
207 	memset(&request, 0, sizeof(request));
208 	request.parent_id = clk->parents[index];
209 
210 	memset(&msg, 0, sizeof(msg));
211 	msg.cmd = CMD_CLK_SET_PARENT;
212 	msg.id = clk->id;
213 	msg.tx.data = &request;
214 	msg.tx.size = sizeof(request);
215 	msg.rx.data = &response;
216 	msg.rx.size = sizeof(response);
217 
218 	err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
219 	if (err < 0)
220 		return err;
221 
222 	/* XXX check parent ID in response */
223 
224 	return 0;
225 }
226 
tegra_bpmp_clk_get_parent(struct clk_hw * hw)227 static u8 tegra_bpmp_clk_get_parent(struct clk_hw *hw)
228 {
229 	struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
230 	struct cmd_clk_get_parent_response response;
231 	struct tegra_bpmp_clk_message msg;
232 	unsigned int i;
233 	int err;
234 
235 	memset(&msg, 0, sizeof(msg));
236 	msg.cmd = CMD_CLK_GET_PARENT;
237 	msg.id = clk->id;
238 	msg.rx.data = &response;
239 	msg.rx.size = sizeof(response);
240 
241 	err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
242 	if (err < 0) {
243 		dev_err(clk->bpmp->dev, "failed to get parent for %s: %d\n",
244 			clk_hw_get_name(hw), err);
245 		return U8_MAX;
246 	}
247 
248 	for (i = 0; i < clk->num_parents; i++)
249 		if (clk->parents[i] == response.parent_id)
250 			return i;
251 
252 	return U8_MAX;
253 }
254 
tegra_bpmp_clk_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)255 static int tegra_bpmp_clk_set_rate(struct clk_hw *hw, unsigned long rate,
256 				   unsigned long parent_rate)
257 {
258 	struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
259 	struct cmd_clk_set_rate_response response;
260 	struct cmd_clk_set_rate_request request;
261 	struct tegra_bpmp_clk_message msg;
262 
263 	memset(&request, 0, sizeof(request));
264 	request.rate = min_t(u64, rate, S64_MAX);
265 
266 	memset(&msg, 0, sizeof(msg));
267 	msg.cmd = CMD_CLK_SET_RATE;
268 	msg.id = clk->id;
269 	msg.tx.data = &request;
270 	msg.tx.size = sizeof(request);
271 	msg.rx.data = &response;
272 	msg.rx.size = sizeof(response);
273 
274 	return tegra_bpmp_clk_transfer(clk->bpmp, &msg);
275 }
276 
277 static const struct clk_ops tegra_bpmp_clk_gate_ops = {
278 	.prepare = tegra_bpmp_clk_prepare,
279 	.unprepare = tegra_bpmp_clk_unprepare,
280 	.is_prepared = tegra_bpmp_clk_is_prepared,
281 	.recalc_rate = tegra_bpmp_clk_recalc_rate,
282 };
283 
284 static const struct clk_ops tegra_bpmp_clk_mux_ops = {
285 	.prepare = tegra_bpmp_clk_prepare,
286 	.unprepare = tegra_bpmp_clk_unprepare,
287 	.is_prepared = tegra_bpmp_clk_is_prepared,
288 	.recalc_rate = tegra_bpmp_clk_recalc_rate,
289 	.determine_rate = clk_hw_determine_rate_no_reparent,
290 	.set_parent = tegra_bpmp_clk_set_parent,
291 	.get_parent = tegra_bpmp_clk_get_parent,
292 };
293 
294 static const struct clk_ops tegra_bpmp_clk_rate_ops = {
295 	.prepare = tegra_bpmp_clk_prepare,
296 	.unprepare = tegra_bpmp_clk_unprepare,
297 	.is_prepared = tegra_bpmp_clk_is_prepared,
298 	.recalc_rate = tegra_bpmp_clk_recalc_rate,
299 	.determine_rate = tegra_bpmp_clk_determine_rate,
300 	.set_rate = tegra_bpmp_clk_set_rate,
301 };
302 
303 static const struct clk_ops tegra_bpmp_clk_mux_rate_ops = {
304 	.prepare = tegra_bpmp_clk_prepare,
305 	.unprepare = tegra_bpmp_clk_unprepare,
306 	.is_prepared = tegra_bpmp_clk_is_prepared,
307 	.recalc_rate = tegra_bpmp_clk_recalc_rate,
308 	.determine_rate = tegra_bpmp_clk_determine_rate,
309 	.set_parent = tegra_bpmp_clk_set_parent,
310 	.get_parent = tegra_bpmp_clk_get_parent,
311 	.set_rate = tegra_bpmp_clk_set_rate,
312 };
313 
314 static const struct clk_ops tegra_bpmp_clk_mux_read_only_ops = {
315 	.get_parent = tegra_bpmp_clk_get_parent,
316 	.recalc_rate = tegra_bpmp_clk_recalc_rate,
317 };
318 
319 static const struct clk_ops tegra_bpmp_clk_read_only_ops = {
320 	.recalc_rate = tegra_bpmp_clk_recalc_rate,
321 };
322 
323 static const struct clk_ops tegra_bpmp_clk_gate_mux_read_only_ops = {
324 	.prepare = tegra_bpmp_clk_prepare,
325 	.unprepare = tegra_bpmp_clk_unprepare,
326 	.is_prepared = tegra_bpmp_clk_is_prepared,
327 	.recalc_rate = tegra_bpmp_clk_recalc_rate,
328 	.get_parent = tegra_bpmp_clk_get_parent,
329 };
330 
tegra_bpmp_clk_get_max_id(struct tegra_bpmp * bpmp)331 static int tegra_bpmp_clk_get_max_id(struct tegra_bpmp *bpmp)
332 {
333 	struct cmd_clk_get_max_clk_id_response response;
334 	struct tegra_bpmp_clk_message msg;
335 	int err;
336 
337 	memset(&msg, 0, sizeof(msg));
338 	msg.cmd = CMD_CLK_GET_MAX_CLK_ID;
339 	msg.rx.data = &response;
340 	msg.rx.size = sizeof(response);
341 
342 	err = tegra_bpmp_clk_transfer(bpmp, &msg);
343 	if (err < 0)
344 		return err;
345 
346 	if (response.max_id > INT_MAX)
347 		return -E2BIG;
348 
349 	return response.max_id;
350 }
351 
tegra_bpmp_clk_get_info(struct tegra_bpmp * bpmp,unsigned int id,struct tegra_bpmp_clk_info * info)352 static int tegra_bpmp_clk_get_info(struct tegra_bpmp *bpmp, unsigned int id,
353 				   struct tegra_bpmp_clk_info *info)
354 {
355 	struct cmd_clk_get_all_info_response response;
356 	struct tegra_bpmp_clk_message msg;
357 	unsigned int i;
358 	int err;
359 
360 	memset(&msg, 0, sizeof(msg));
361 	msg.cmd = CMD_CLK_GET_ALL_INFO;
362 	msg.id = id;
363 	msg.rx.data = &response;
364 	msg.rx.size = sizeof(response);
365 
366 	err = tegra_bpmp_clk_transfer(bpmp, &msg);
367 	if (err < 0)
368 		return err;
369 
370 	strscpy(info->name, response.name, MRQ_CLK_NAME_MAXLEN);
371 	info->num_parents = response.num_parents;
372 
373 	for (i = 0; i < info->num_parents; i++)
374 		info->parents[i] = response.parents[i];
375 
376 	info->flags = response.flags;
377 
378 	return 0;
379 }
380 
tegra_bpmp_clk_info_dump(struct tegra_bpmp * bpmp,const char * level,const struct tegra_bpmp_clk_info * info)381 static void tegra_bpmp_clk_info_dump(struct tegra_bpmp *bpmp,
382 				     const char *level,
383 				     const struct tegra_bpmp_clk_info *info)
384 {
385 	const char *prefix = "";
386 	struct seq_buf buf;
387 	unsigned int i;
388 	char flags[64];
389 
390 	seq_buf_init(&buf, flags, sizeof(flags));
391 
392 	if (info->flags)
393 		seq_buf_printf(&buf, "(");
394 
395 	if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) {
396 		seq_buf_printf(&buf, "%smux", prefix);
397 		prefix = ", ";
398 	}
399 
400 	if ((info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE) == 0) {
401 		seq_buf_printf(&buf, "%sfixed", prefix);
402 		prefix = ", ";
403 	}
404 
405 	if (info->flags & TEGRA_BPMP_CLK_IS_ROOT) {
406 		seq_buf_printf(&buf, "%sroot", prefix);
407 		prefix = ", ";
408 	}
409 
410 	if (info->flags)
411 		seq_buf_printf(&buf, ")");
412 
413 	dev_printk(level, bpmp->dev, "%03u: %s\n", info->id, info->name);
414 	dev_printk(level, bpmp->dev, "  flags: %lx %s\n", info->flags, flags);
415 	dev_printk(level, bpmp->dev, "  parents: %u\n", info->num_parents);
416 
417 	for (i = 0; i < info->num_parents; i++)
418 		dev_printk(level, bpmp->dev, "    %03u\n", info->parents[i]);
419 }
420 
tegra_bpmp_probe_clocks(struct tegra_bpmp * bpmp,struct tegra_bpmp_clk_info ** clocksp)421 static int tegra_bpmp_probe_clocks(struct tegra_bpmp *bpmp,
422 				   struct tegra_bpmp_clk_info **clocksp)
423 {
424 	struct tegra_bpmp_clk_info *clocks;
425 	unsigned int max_id, id, count = 0;
426 	unsigned int holes = 0;
427 	int err;
428 
429 	err = tegra_bpmp_clk_get_max_id(bpmp);
430 	if (err < 0)
431 		return err;
432 
433 	max_id = err;
434 
435 	dev_dbg(bpmp->dev, "maximum clock ID: %u\n", max_id);
436 
437 	clocks = kcalloc(max_id + 1, sizeof(*clocks), GFP_KERNEL);
438 	if (!clocks)
439 		return -ENOMEM;
440 
441 	for (id = 0; id <= max_id; id++) {
442 		struct tegra_bpmp_clk_info *info = &clocks[count];
443 
444 		err = tegra_bpmp_clk_get_info(bpmp, id, info);
445 		if (err < 0)
446 			continue;
447 
448 		if (info->num_parents >= U8_MAX) {
449 			dev_err(bpmp->dev,
450 				"clock %u has too many parents (%u, max: %u)\n",
451 				id, info->num_parents, U8_MAX);
452 			continue;
453 		}
454 
455 		/* clock not exposed by BPMP */
456 		if (info->name[0] == '\0') {
457 			holes++;
458 			continue;
459 		}
460 
461 		info->id = id;
462 		count++;
463 
464 		if (TEGRA_BPMP_DUMP_CLOCK_INFO)
465 			tegra_bpmp_clk_info_dump(bpmp, KERN_DEBUG, info);
466 	}
467 
468 	dev_dbg(bpmp->dev, "holes: %u\n", holes);
469 	*clocksp = clocks;
470 
471 	return count;
472 }
473 
474 static unsigned int
tegra_bpmp_clk_id_to_index(const struct tegra_bpmp_clk_info * clocks,unsigned int num_clocks,unsigned int id)475 tegra_bpmp_clk_id_to_index(const struct tegra_bpmp_clk_info *clocks,
476 			   unsigned int num_clocks, unsigned int id)
477 {
478 	unsigned int i;
479 
480 	for (i = 0; i < num_clocks; i++)
481 		if (clocks[i].id == id)
482 			return i;
483 
484 	return UINT_MAX;
485 }
486 
487 static const struct tegra_bpmp_clk_info *
tegra_bpmp_clk_find(const struct tegra_bpmp_clk_info * clocks,unsigned int num_clocks,unsigned int id)488 tegra_bpmp_clk_find(const struct tegra_bpmp_clk_info *clocks,
489 		    unsigned int num_clocks, unsigned int id)
490 {
491 	unsigned int i;
492 
493 	i = tegra_bpmp_clk_id_to_index(clocks, num_clocks, id);
494 
495 	if (i < num_clocks)
496 		return &clocks[i];
497 
498 	return NULL;
499 }
500 
501 static struct tegra_bpmp_clk *
tegra_bpmp_clk_register(struct tegra_bpmp * bpmp,const struct tegra_bpmp_clk_info * info,const struct tegra_bpmp_clk_info * clocks,unsigned int num_clocks)502 tegra_bpmp_clk_register(struct tegra_bpmp *bpmp,
503 			const struct tegra_bpmp_clk_info *info,
504 			const struct tegra_bpmp_clk_info *clocks,
505 			unsigned int num_clocks)
506 {
507 	struct tegra_bpmp_clk *clk;
508 	struct clk_init_data init;
509 	const char **parents;
510 	unsigned int i;
511 	int err;
512 
513 	clk = devm_kzalloc(bpmp->dev, sizeof(*clk), GFP_KERNEL);
514 	if (!clk)
515 		return ERR_PTR(-ENOMEM);
516 
517 	clk->id = info->id;
518 	clk->bpmp = bpmp;
519 
520 	clk->parents = devm_kcalloc(bpmp->dev, info->num_parents,
521 				    sizeof(*clk->parents), GFP_KERNEL);
522 	if (!clk->parents)
523 		return ERR_PTR(-ENOMEM);
524 
525 	clk->num_parents = info->num_parents;
526 
527 	/* hardware clock initialization */
528 	memset(&init, 0, sizeof(init));
529 	init.name = info->name;
530 	clk->hw.init = &init;
531 	if (info->flags & BPMP_CLK_STATE_CHANGE_DENIED) {
532 		if ((info->flags & BPMP_CLK_RATE_PARENT_CHANGE_DENIED) == 0) {
533 			dev_WARN(bpmp->dev,
534 				"Firmware bug! Inconsistent permission bits for clock %s. State and parent/rate changes disabled.",
535 				 init.name);
536 		}
537 		if (info->flags & TEGRA_BPMP_CLK_HAS_MUX)
538 			init.ops = &tegra_bpmp_clk_mux_read_only_ops;
539 		else
540 			init.ops = &tegra_bpmp_clk_read_only_ops;
541 	} else if (info->flags & BPMP_CLK_RATE_PARENT_CHANGE_DENIED) {
542 		if (info->flags & TEGRA_BPMP_CLK_HAS_MUX)
543 			init.ops = &tegra_bpmp_clk_gate_mux_read_only_ops;
544 		else
545 			init.ops = &tegra_bpmp_clk_gate_ops;
546 	} else if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) {
547 		if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE)
548 			init.ops = &tegra_bpmp_clk_mux_rate_ops;
549 		else
550 			init.ops = &tegra_bpmp_clk_mux_ops;
551 	} else {
552 		if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE)
553 			init.ops = &tegra_bpmp_clk_rate_ops;
554 		else
555 			init.ops = &tegra_bpmp_clk_gate_ops;
556 	}
557 
558 	init.num_parents = info->num_parents;
559 
560 	parents = kcalloc(info->num_parents, sizeof(*parents), GFP_KERNEL);
561 	if (!parents)
562 		return ERR_PTR(-ENOMEM);
563 
564 	for (i = 0; i < info->num_parents; i++) {
565 		const struct tegra_bpmp_clk_info *parent;
566 
567 		/* keep a private copy of the ID to parent index map */
568 		clk->parents[i] = info->parents[i];
569 
570 		parent = tegra_bpmp_clk_find(clocks, num_clocks,
571 					     info->parents[i]);
572 		if (!parent) {
573 			dev_err(bpmp->dev, "no parent %u found for %u\n",
574 				info->parents[i], info->id);
575 			continue;
576 		}
577 
578 		parents[i] = parent->name;
579 	}
580 
581 	init.parent_names = parents;
582 
583 	err = devm_clk_hw_register(bpmp->dev, &clk->hw);
584 
585 	kfree(parents);
586 
587 	if (err < 0)
588 		return ERR_PTR(err);
589 
590 	return clk;
591 }
592 
tegra_bpmp_register_clocks_one(struct tegra_bpmp * bpmp,struct tegra_bpmp_clk_info * infos,unsigned int i,unsigned int count)593 static void tegra_bpmp_register_clocks_one(struct tegra_bpmp *bpmp,
594 					   struct tegra_bpmp_clk_info *infos,
595 					   unsigned int i,
596 					   unsigned int count)
597 {
598 	unsigned int j;
599 	struct tegra_bpmp_clk_info *info;
600 	struct tegra_bpmp_clk *clk;
601 
602 	if (bpmp->clocks[i]) {
603 		/* already registered */
604 		return;
605 	}
606 
607 	info = &infos[i];
608 	for (j = 0; j < info->num_parents; ++j) {
609 		unsigned int p_id = info->parents[j];
610 		unsigned int p_i = tegra_bpmp_clk_id_to_index(infos, count,
611 							      p_id);
612 		if (p_i < count)
613 			tegra_bpmp_register_clocks_one(bpmp, infos, p_i, count);
614 	}
615 
616 	clk = tegra_bpmp_clk_register(bpmp, info, infos, count);
617 	if (IS_ERR(clk)) {
618 		dev_err(bpmp->dev,
619 			"failed to register clock %u (%s): %ld\n",
620 			info->id, info->name, PTR_ERR(clk));
621 		/* intentionally store the error pointer to
622 		 * bpmp->clocks[i] to avoid re-attempting the
623 		 * registration later
624 		 */
625 	}
626 
627 	bpmp->clocks[i] = clk;
628 }
629 
tegra_bpmp_register_clocks(struct tegra_bpmp * bpmp,struct tegra_bpmp_clk_info * infos,unsigned int count)630 static int tegra_bpmp_register_clocks(struct tegra_bpmp *bpmp,
631 				      struct tegra_bpmp_clk_info *infos,
632 				      unsigned int count)
633 {
634 	unsigned int i;
635 
636 	bpmp->num_clocks = count;
637 
638 	bpmp->clocks = devm_kcalloc(bpmp->dev, count, sizeof(struct tegra_bpmp_clk), GFP_KERNEL);
639 	if (!bpmp->clocks)
640 		return -ENOMEM;
641 
642 	for (i = 0; i < count; i++) {
643 		tegra_bpmp_register_clocks_one(bpmp, infos, i, count);
644 	}
645 
646 	return 0;
647 }
648 
tegra_bpmp_unregister_clocks(struct tegra_bpmp * bpmp)649 static void tegra_bpmp_unregister_clocks(struct tegra_bpmp *bpmp)
650 {
651 	unsigned int i;
652 
653 	for (i = 0; i < bpmp->num_clocks; i++)
654 		clk_hw_unregister(&bpmp->clocks[i]->hw);
655 }
656 
tegra_bpmp_clk_of_xlate(struct of_phandle_args * clkspec,void * data)657 static struct clk_hw *tegra_bpmp_clk_of_xlate(struct of_phandle_args *clkspec,
658 					      void *data)
659 {
660 	unsigned int id = clkspec->args[0], i;
661 	struct tegra_bpmp *bpmp = data;
662 
663 	for (i = 0; i < bpmp->num_clocks; i++) {
664 		struct tegra_bpmp_clk *clk = bpmp->clocks[i];
665 
666 		if (!clk)
667 			continue;
668 
669 		if (clk->id == id)
670 			return &clk->hw;
671 	}
672 
673 	return NULL;
674 }
675 
tegra_bpmp_init_clocks(struct tegra_bpmp * bpmp)676 int tegra_bpmp_init_clocks(struct tegra_bpmp *bpmp)
677 {
678 	struct tegra_bpmp_clk_info *clocks;
679 	unsigned int count;
680 	int err;
681 
682 	err = tegra_bpmp_probe_clocks(bpmp, &clocks);
683 	if (err < 0)
684 		return err;
685 
686 	count = err;
687 
688 	dev_dbg(bpmp->dev, "%u clocks probed\n", count);
689 
690 	err = tegra_bpmp_register_clocks(bpmp, clocks, count);
691 	if (err < 0)
692 		goto free;
693 
694 	err = of_clk_add_hw_provider(bpmp->dev->of_node,
695 				     tegra_bpmp_clk_of_xlate,
696 				     bpmp);
697 	if (err < 0) {
698 		tegra_bpmp_unregister_clocks(bpmp);
699 		goto free;
700 	}
701 
702 free:
703 	kfree(clocks);
704 	return err;
705 }
706