xref: /linux/drivers/clk/zynqmp/clkc.c (revision a1c613ae4c322ddd58d5a8539dbfba2a0380a8c0)
1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Zynq UltraScale+ MPSoC clock controller
4  *
5  *  Copyright (C) 2016-2019 Xilinx
6  *
7  * Based on drivers/clk/zynq/clkc.c
8  */
9 
10 #include <linux/bitfield.h>
11 #include <linux/clk.h>
12 #include <linux/clk-provider.h>
13 #include <linux/module.h>
14 #include <linux/of.h>
15 #include <linux/platform_device.h>
16 #include <linux/slab.h>
17 #include <linux/string.h>
18 
19 #include "clk-zynqmp.h"
20 
21 #define MAX_PARENT			100
22 #define MAX_NODES			6
23 #define MAX_NAME_LEN			50
24 
25 /* Flags for parents */
26 #define PARENT_CLK_SELF			0
27 #define PARENT_CLK_NODE1		1
28 #define PARENT_CLK_NODE2		2
29 #define PARENT_CLK_NODE3		3
30 #define PARENT_CLK_NODE4		4
31 #define PARENT_CLK_EXTERNAL		5
32 
33 #define END_OF_CLK_NAME			"END_OF_CLK"
34 #define END_OF_TOPOLOGY_NODE		1
35 #define END_OF_PARENTS			1
36 #define RESERVED_CLK_NAME		""
37 
38 #define CLK_GET_NAME_RESP_LEN		16
39 #define CLK_GET_TOPOLOGY_RESP_WORDS	3
40 #define CLK_GET_PARENTS_RESP_WORDS	3
41 #define CLK_GET_ATTR_RESP_WORDS		1
42 
43 enum clk_type {
44 	CLK_TYPE_OUTPUT,
45 	CLK_TYPE_EXTERNAL,
46 };
47 
48 /**
49  * struct clock_parent - Clock parent
50  * @name:	Parent name
51  * @id:		Parent clock ID
52  * @flag:	Parent flags
53  */
54 struct clock_parent {
55 	char name[MAX_NAME_LEN];
56 	int id;
57 	u32 flag;
58 };
59 
60 /**
61  * struct zynqmp_clock - Clock
62  * @clk_name:		Clock name
63  * @valid:		Validity flag of clock
64  * @type:		Clock type (Output/External)
65  * @node:		Clock topology nodes
66  * @num_nodes:		Number of nodes present in topology
67  * @parent:		Parent of clock
68  * @num_parents:	Number of parents of clock
69  * @clk_id:		Clock id
70  */
71 struct zynqmp_clock {
72 	char clk_name[MAX_NAME_LEN];
73 	u32 valid;
74 	enum clk_type type;
75 	struct clock_topology node[MAX_NODES];
76 	u32 num_nodes;
77 	struct clock_parent parent[MAX_PARENT];
78 	u32 num_parents;
79 	u32 clk_id;
80 };
81 
82 struct name_resp {
83 	char name[CLK_GET_NAME_RESP_LEN];
84 };
85 
86 struct topology_resp {
87 #define CLK_TOPOLOGY_TYPE		GENMASK(3, 0)
88 #define CLK_TOPOLOGY_CUSTOM_TYPE_FLAGS	GENMASK(7, 4)
89 #define CLK_TOPOLOGY_FLAGS		GENMASK(23, 8)
90 #define CLK_TOPOLOGY_TYPE_FLAGS		GENMASK(31, 24)
91 	u32 topology[CLK_GET_TOPOLOGY_RESP_WORDS];
92 };
93 
94 struct parents_resp {
95 #define NA_PARENT			0xFFFFFFFF
96 #define DUMMY_PARENT			0xFFFFFFFE
97 #define CLK_PARENTS_ID			GENMASK(15, 0)
98 #define CLK_PARENTS_FLAGS		GENMASK(31, 16)
99 	u32 parents[CLK_GET_PARENTS_RESP_WORDS];
100 };
101 
102 struct attr_resp {
103 #define CLK_ATTR_VALID			BIT(0)
104 #define CLK_ATTR_TYPE			BIT(2)
105 #define CLK_ATTR_NODE_INDEX		GENMASK(13, 0)
106 #define CLK_ATTR_NODE_TYPE		GENMASK(19, 14)
107 #define CLK_ATTR_NODE_SUBCLASS		GENMASK(25, 20)
108 #define CLK_ATTR_NODE_CLASS		GENMASK(31, 26)
109 	u32 attr[CLK_GET_ATTR_RESP_WORDS];
110 };
111 
112 static const char clk_type_postfix[][10] = {
113 	[TYPE_INVALID] = "",
114 	[TYPE_MUX] = "_mux",
115 	[TYPE_GATE] = "",
116 	[TYPE_DIV1] = "_div1",
117 	[TYPE_DIV2] = "_div2",
118 	[TYPE_FIXEDFACTOR] = "_ff",
119 	[TYPE_PLL] = ""
120 };
121 
122 static struct clk_hw *(* const clk_topology[]) (const char *name, u32 clk_id,
123 					const char * const *parents,
124 					u8 num_parents,
125 					const struct clock_topology *nodes)
126 					= {
127 	[TYPE_INVALID] = NULL,
128 	[TYPE_MUX] = zynqmp_clk_register_mux,
129 	[TYPE_PLL] = zynqmp_clk_register_pll,
130 	[TYPE_FIXEDFACTOR] = zynqmp_clk_register_fixed_factor,
131 	[TYPE_DIV1] = zynqmp_clk_register_divider,
132 	[TYPE_DIV2] = zynqmp_clk_register_divider,
133 	[TYPE_GATE] = zynqmp_clk_register_gate
134 };
135 
136 static struct zynqmp_clock *clock;
137 static struct clk_hw_onecell_data *zynqmp_data;
138 static unsigned int clock_max_idx;
139 
140 /**
141  * zynqmp_is_valid_clock() - Check whether clock is valid or not
142  * @clk_id:	Clock index
143  *
144  * Return: 1 if clock is valid, 0 if clock is invalid else error code
145  */
zynqmp_is_valid_clock(u32 clk_id)146 static inline int zynqmp_is_valid_clock(u32 clk_id)
147 {
148 	if (clk_id >= clock_max_idx)
149 		return -ENODEV;
150 
151 	return clock[clk_id].valid;
152 }
153 
154 /**
155  * zynqmp_get_clock_name() - Get name of clock from Clock index
156  * @clk_id:	Clock index
157  * @clk_name:	Name of clock
158  *
159  * Return: 0 on success else error code
160  */
zynqmp_get_clock_name(u32 clk_id,char * clk_name)161 static int zynqmp_get_clock_name(u32 clk_id, char *clk_name)
162 {
163 	int ret;
164 
165 	ret = zynqmp_is_valid_clock(clk_id);
166 	if (ret == 1) {
167 		strscpy(clk_name, clock[clk_id].clk_name, MAX_NAME_LEN);
168 		return 0;
169 	}
170 
171 	return ret == 0 ? -EINVAL : ret;
172 }
173 
174 /**
175  * zynqmp_get_clock_type() - Get type of clock
176  * @clk_id:	Clock index
177  * @type:	Clock type: CLK_TYPE_OUTPUT or CLK_TYPE_EXTERNAL
178  *
179  * Return: 0 on success else error code
180  */
zynqmp_get_clock_type(u32 clk_id,u32 * type)181 static int zynqmp_get_clock_type(u32 clk_id, u32 *type)
182 {
183 	int ret;
184 
185 	ret = zynqmp_is_valid_clock(clk_id);
186 	if (ret == 1) {
187 		*type = clock[clk_id].type;
188 		return 0;
189 	}
190 
191 	return ret == 0 ? -EINVAL : ret;
192 }
193 
194 /**
195  * zynqmp_pm_clock_get_num_clocks() - Get number of clocks in system
196  * @nclocks:	Number of clocks in system/board.
197  *
198  * Call firmware API to get number of clocks.
199  *
200  * Return: 0 on success else error code.
201  */
zynqmp_pm_clock_get_num_clocks(u32 * nclocks)202 static int zynqmp_pm_clock_get_num_clocks(u32 *nclocks)
203 {
204 	struct zynqmp_pm_query_data qdata = {0};
205 	u32 ret_payload[PAYLOAD_ARG_CNT];
206 	int ret;
207 
208 	qdata.qid = PM_QID_CLOCK_GET_NUM_CLOCKS;
209 
210 	ret = zynqmp_pm_query_data(qdata, ret_payload);
211 	*nclocks = ret_payload[1];
212 
213 	return ret;
214 }
215 
216 /**
217  * zynqmp_pm_clock_get_name() - Get the name of clock for given id
218  * @clock_id:	ID of the clock to be queried
219  * @response:	Name of the clock with the given id
220  *
221  * This function is used to get name of clock specified by given
222  * clock ID.
223  *
224  * Return: 0 on success else error+reason
225  */
zynqmp_pm_clock_get_name(u32 clock_id,struct name_resp * response)226 static int zynqmp_pm_clock_get_name(u32 clock_id,
227 				    struct name_resp *response)
228 {
229 	struct zynqmp_pm_query_data qdata = {0};
230 	u32 ret_payload[PAYLOAD_ARG_CNT];
231 	int ret;
232 
233 	qdata.qid = PM_QID_CLOCK_GET_NAME;
234 	qdata.arg1 = clock_id;
235 
236 	ret = zynqmp_pm_query_data(qdata, ret_payload);
237 	if (ret)
238 		return ret;
239 
240 	memcpy(response, ret_payload, sizeof(*response));
241 
242 	return 0;
243 }
244 
245 /**
246  * zynqmp_pm_clock_get_topology() - Get the topology of clock for given id
247  * @clock_id:	ID of the clock to be queried
248  * @index:	Node index of clock topology
249  * @response:	Buffer used for the topology response
250  *
251  * This function is used to get topology information for the clock
252  * specified by given clock ID.
253  *
254  * This API will return 3 node of topology with a single response. To get
255  * other nodes, master should call same API in loop with new
256  * index till error is returned. E.g First call should have
257  * index 0 which will return nodes 0,1 and 2. Next call, index
258  * should be 3 which will return nodes 3,4 and 5 and so on.
259  *
260  * Return: 0 on success else error+reason
261  */
zynqmp_pm_clock_get_topology(u32 clock_id,u32 index,struct topology_resp * response)262 static int zynqmp_pm_clock_get_topology(u32 clock_id, u32 index,
263 					struct topology_resp *response)
264 {
265 	struct zynqmp_pm_query_data qdata = {0};
266 	u32 ret_payload[PAYLOAD_ARG_CNT];
267 	int ret;
268 
269 	qdata.qid = PM_QID_CLOCK_GET_TOPOLOGY;
270 	qdata.arg1 = clock_id;
271 	qdata.arg2 = index;
272 
273 	ret = zynqmp_pm_query_data(qdata, ret_payload);
274 	memcpy(response, &ret_payload[1], sizeof(*response));
275 
276 	return ret;
277 }
278 
zynqmp_clk_map_common_ccf_flags(const u32 zynqmp_flag)279 unsigned long zynqmp_clk_map_common_ccf_flags(const u32 zynqmp_flag)
280 {
281 	unsigned long ccf_flag = 0;
282 
283 	if (zynqmp_flag & ZYNQMP_CLK_SET_RATE_GATE)
284 		ccf_flag |= CLK_SET_RATE_GATE;
285 	if (zynqmp_flag & ZYNQMP_CLK_SET_PARENT_GATE)
286 		ccf_flag |= CLK_SET_PARENT_GATE;
287 	if (zynqmp_flag & ZYNQMP_CLK_SET_RATE_PARENT)
288 		ccf_flag |= CLK_SET_RATE_PARENT;
289 	if (zynqmp_flag & ZYNQMP_CLK_IGNORE_UNUSED)
290 		ccf_flag |= CLK_IGNORE_UNUSED;
291 	if (zynqmp_flag & ZYNQMP_CLK_SET_RATE_NO_REPARENT)
292 		ccf_flag |= CLK_SET_RATE_NO_REPARENT;
293 	if (zynqmp_flag & ZYNQMP_CLK_IS_CRITICAL)
294 		ccf_flag |= CLK_IS_CRITICAL;
295 
296 	return ccf_flag;
297 }
298 
299 /**
300  * zynqmp_clk_register_fixed_factor() - Register fixed factor with the
301  *					clock framework
302  * @name:		Name of this clock
303  * @clk_id:		Clock ID
304  * @parents:		Name of this clock's parents
305  * @num_parents:	Number of parents
306  * @nodes:		Clock topology node
307  *
308  * Return: clock hardware to the registered clock
309  */
zynqmp_clk_register_fixed_factor(const char * name,u32 clk_id,const char * const * parents,u8 num_parents,const struct clock_topology * nodes)310 struct clk_hw *zynqmp_clk_register_fixed_factor(const char *name, u32 clk_id,
311 					const char * const *parents,
312 					u8 num_parents,
313 					const struct clock_topology *nodes)
314 {
315 	u32 mult, div;
316 	struct clk_hw *hw;
317 	struct zynqmp_pm_query_data qdata = {0};
318 	u32 ret_payload[PAYLOAD_ARG_CNT];
319 	int ret;
320 	unsigned long flag;
321 
322 	qdata.qid = PM_QID_CLOCK_GET_FIXEDFACTOR_PARAMS;
323 	qdata.arg1 = clk_id;
324 
325 	ret = zynqmp_pm_query_data(qdata, ret_payload);
326 	if (ret)
327 		return ERR_PTR(ret);
328 
329 	mult = ret_payload[1];
330 	div = ret_payload[2];
331 
332 	flag = zynqmp_clk_map_common_ccf_flags(nodes->flag);
333 
334 	hw = clk_hw_register_fixed_factor(NULL, name,
335 					  parents[0],
336 					  flag, mult,
337 					  div);
338 
339 	return hw;
340 }
341 
342 /**
343  * zynqmp_pm_clock_get_parents() - Get the first 3 parents of clock for given id
344  * @clock_id:	Clock ID
345  * @index:	Parent index
346  * @response:	Parents of the given clock
347  *
348  * This function is used to get 3 parents for the clock specified by
349  * given clock ID.
350  *
351  * This API will return 3 parents with a single response. To get
352  * other parents, master should call same API in loop with new
353  * parent index till error is returned. E.g First call should have
354  * index 0 which will return parents 0,1 and 2. Next call, index
355  * should be 3 which will return parent 3,4 and 5 and so on.
356  *
357  * Return: 0 on success else error+reason
358  */
zynqmp_pm_clock_get_parents(u32 clock_id,u32 index,struct parents_resp * response)359 static int zynqmp_pm_clock_get_parents(u32 clock_id, u32 index,
360 				       struct parents_resp *response)
361 {
362 	struct zynqmp_pm_query_data qdata = {0};
363 	u32 ret_payload[PAYLOAD_ARG_CNT];
364 	int ret;
365 
366 	qdata.qid = PM_QID_CLOCK_GET_PARENTS;
367 	qdata.arg1 = clock_id;
368 	qdata.arg2 = index;
369 
370 	ret = zynqmp_pm_query_data(qdata, ret_payload);
371 	memcpy(response, &ret_payload[1], sizeof(*response));
372 
373 	return ret;
374 }
375 
376 /**
377  * zynqmp_pm_clock_get_attributes() - Get the attributes of clock for given id
378  * @clock_id:	Clock ID
379  * @response:	Clock attributes response
380  *
381  * This function is used to get clock's attributes(e.g. valid, clock type, etc).
382  *
383  * Return: 0 on success else error+reason
384  */
zynqmp_pm_clock_get_attributes(u32 clock_id,struct attr_resp * response)385 static int zynqmp_pm_clock_get_attributes(u32 clock_id,
386 					  struct attr_resp *response)
387 {
388 	struct zynqmp_pm_query_data qdata = {0};
389 	u32 ret_payload[PAYLOAD_ARG_CNT];
390 	int ret;
391 
392 	qdata.qid = PM_QID_CLOCK_GET_ATTRIBUTES;
393 	qdata.arg1 = clock_id;
394 
395 	ret = zynqmp_pm_query_data(qdata, ret_payload);
396 	memcpy(response, &ret_payload[1], sizeof(*response));
397 
398 	return ret;
399 }
400 
401 /**
402  * __zynqmp_clock_get_topology() - Get topology data of clock from firmware
403  *				   response data
404  * @topology:		Clock topology
405  * @response:		Clock topology data received from firmware
406  * @nnodes:		Number of nodes
407  *
408  * Return: 0 on success else error+reason
409  */
__zynqmp_clock_get_topology(struct clock_topology * topology,struct topology_resp * response,u32 * nnodes)410 static int __zynqmp_clock_get_topology(struct clock_topology *topology,
411 				       struct topology_resp *response,
412 				       u32 *nnodes)
413 {
414 	int i;
415 	u32 type;
416 
417 	for (i = 0; i < ARRAY_SIZE(response->topology); i++) {
418 		type = FIELD_GET(CLK_TOPOLOGY_TYPE, response->topology[i]);
419 		if (type == TYPE_INVALID)
420 			return END_OF_TOPOLOGY_NODE;
421 		topology[*nnodes].type = type;
422 		topology[*nnodes].flag = FIELD_GET(CLK_TOPOLOGY_FLAGS,
423 						   response->topology[i]);
424 		topology[*nnodes].type_flag =
425 				FIELD_GET(CLK_TOPOLOGY_TYPE_FLAGS,
426 					  response->topology[i]);
427 		topology[*nnodes].custom_type_flag =
428 			FIELD_GET(CLK_TOPOLOGY_CUSTOM_TYPE_FLAGS,
429 				  response->topology[i]);
430 		(*nnodes)++;
431 	}
432 
433 	return 0;
434 }
435 
436 /**
437  * zynqmp_clock_get_topology() - Get topology of clock from firmware using
438  *				 PM_API
439  * @clk_id:		Clock index
440  * @topology:		Clock topology
441  * @num_nodes:		Number of nodes
442  *
443  * Return: 0 on success else error+reason
444  */
zynqmp_clock_get_topology(u32 clk_id,struct clock_topology * topology,u32 * num_nodes)445 static int zynqmp_clock_get_topology(u32 clk_id,
446 				     struct clock_topology *topology,
447 				     u32 *num_nodes)
448 {
449 	int j, ret;
450 	struct topology_resp response = { };
451 
452 	*num_nodes = 0;
453 	for (j = 0; j <= MAX_NODES; j += ARRAY_SIZE(response.topology)) {
454 		ret = zynqmp_pm_clock_get_topology(clock[clk_id].clk_id, j,
455 						   &response);
456 		if (ret)
457 			return ret;
458 		ret = __zynqmp_clock_get_topology(topology, &response,
459 						  num_nodes);
460 		if (ret == END_OF_TOPOLOGY_NODE)
461 			return 0;
462 	}
463 
464 	return 0;
465 }
466 
467 /**
468  * __zynqmp_clock_get_parents() - Get parents info of clock from firmware
469  *				   response data
470  * @parents:		Clock parents
471  * @response:		Clock parents data received from firmware
472  * @nparent:		Number of parent
473  *
474  * Return: 0 on success else error+reason
475  */
__zynqmp_clock_get_parents(struct clock_parent * parents,struct parents_resp * response,u32 * nparent)476 static int __zynqmp_clock_get_parents(struct clock_parent *parents,
477 				      struct parents_resp *response,
478 				      u32 *nparent)
479 {
480 	int i;
481 	struct clock_parent *parent;
482 
483 	for (i = 0; i < ARRAY_SIZE(response->parents); i++) {
484 		if (response->parents[i] == NA_PARENT)
485 			return END_OF_PARENTS;
486 
487 		parent = &parents[i];
488 		parent->id = FIELD_GET(CLK_PARENTS_ID, response->parents[i]);
489 		if (response->parents[i] == DUMMY_PARENT) {
490 			strcpy(parent->name, "dummy_name");
491 			parent->flag = 0;
492 		} else {
493 			parent->flag = FIELD_GET(CLK_PARENTS_FLAGS,
494 						 response->parents[i]);
495 			if (zynqmp_get_clock_name(parent->id, parent->name))
496 				continue;
497 		}
498 		*nparent += 1;
499 	}
500 
501 	return 0;
502 }
503 
504 /**
505  * zynqmp_clock_get_parents() - Get parents info from firmware using PM_API
506  * @clk_id:		Clock index
507  * @parents:		Clock parents
508  * @num_parents:	Total number of parents
509  *
510  * Return: 0 on success else error+reason
511  */
zynqmp_clock_get_parents(u32 clk_id,struct clock_parent * parents,u32 * num_parents)512 static int zynqmp_clock_get_parents(u32 clk_id, struct clock_parent *parents,
513 				    u32 *num_parents)
514 {
515 	int j = 0, ret;
516 	struct parents_resp response = { };
517 
518 	*num_parents = 0;
519 	do {
520 		/* Get parents from firmware */
521 		ret = zynqmp_pm_clock_get_parents(clock[clk_id].clk_id, j,
522 						  &response);
523 		if (ret)
524 			return ret;
525 
526 		ret = __zynqmp_clock_get_parents(&parents[j], &response,
527 						 num_parents);
528 		if (ret == END_OF_PARENTS)
529 			return 0;
530 		j += ARRAY_SIZE(response.parents);
531 	} while (*num_parents <= MAX_PARENT);
532 
533 	return 0;
534 }
535 
536 /**
537  * zynqmp_get_parent_list() - Create list of parents name
538  * @np:			Device node
539  * @clk_id:		Clock index
540  * @parent_list:	List of parent's name
541  * @num_parents:	Total number of parents
542  *
543  * Return: 0 on success else error+reason
544  */
zynqmp_get_parent_list(struct device_node * np,u32 clk_id,const char ** parent_list,u32 * num_parents)545 static int zynqmp_get_parent_list(struct device_node *np, u32 clk_id,
546 				  const char **parent_list, u32 *num_parents)
547 {
548 	int i = 0, ret;
549 	u32 total_parents = clock[clk_id].num_parents;
550 	struct clock_topology *clk_nodes;
551 	struct clock_parent *parents;
552 
553 	clk_nodes = clock[clk_id].node;
554 	parents = clock[clk_id].parent;
555 
556 	for (i = 0; i < total_parents; i++) {
557 		if (!parents[i].flag) {
558 			parent_list[i] = parents[i].name;
559 		} else if (parents[i].flag == PARENT_CLK_EXTERNAL) {
560 			ret = of_property_match_string(np, "clock-names",
561 						       parents[i].name);
562 			if (ret < 0)
563 				strcpy(parents[i].name, "dummy_name");
564 			parent_list[i] = parents[i].name;
565 		} else {
566 			strcat(parents[i].name,
567 			       clk_type_postfix[clk_nodes[parents[i].flag - 1].
568 			       type]);
569 			parent_list[i] = parents[i].name;
570 		}
571 	}
572 
573 	*num_parents = total_parents;
574 	return 0;
575 }
576 
577 /**
578  * zynqmp_register_clk_topology() - Register clock topology
579  * @clk_id:		Clock index
580  * @clk_name:		Clock Name
581  * @num_parents:	Total number of parents
582  * @parent_names:	List of parents name
583  *
584  * Return: Returns either clock hardware or error+reason
585  */
zynqmp_register_clk_topology(int clk_id,char * clk_name,int num_parents,const char ** parent_names)586 static struct clk_hw *zynqmp_register_clk_topology(int clk_id, char *clk_name,
587 						   int num_parents,
588 						   const char **parent_names)
589 {
590 	int j;
591 	u32 num_nodes, clk_dev_id;
592 	char *clk_out[MAX_NODES];
593 	struct clock_topology *nodes;
594 	struct clk_hw *hw = NULL;
595 
596 	nodes = clock[clk_id].node;
597 	num_nodes = clock[clk_id].num_nodes;
598 	clk_dev_id = clock[clk_id].clk_id;
599 
600 	for (j = 0; j < num_nodes; j++) {
601 		/*
602 		 * Clock name received from firmware is output clock name.
603 		 * Intermediate clock names are postfixed with type of clock.
604 		 */
605 		if (j != (num_nodes - 1)) {
606 			clk_out[j] = kasprintf(GFP_KERNEL, "%s%s", clk_name,
607 					    clk_type_postfix[nodes[j].type]);
608 		} else {
609 			clk_out[j] = kasprintf(GFP_KERNEL, "%s", clk_name);
610 		}
611 
612 		if (!clk_topology[nodes[j].type])
613 			continue;
614 
615 		hw = (*clk_topology[nodes[j].type])(clk_out[j], clk_dev_id,
616 						    parent_names,
617 						    num_parents,
618 						    &nodes[j]);
619 		if (IS_ERR(hw))
620 			pr_warn_once("%s() 0x%x: %s register fail with %ld\n",
621 				     __func__,  clk_dev_id, clk_name,
622 				     PTR_ERR(hw));
623 
624 		parent_names[0] = clk_out[j];
625 	}
626 
627 	for (j = 0; j < num_nodes; j++)
628 		kfree(clk_out[j]);
629 
630 	return hw;
631 }
632 
633 /**
634  * zynqmp_register_clocks() - Register clocks
635  * @np:		Device node
636  *
637  * Return: 0 on success else error code
638  */
zynqmp_register_clocks(struct device_node * np)639 static int zynqmp_register_clocks(struct device_node *np)
640 {
641 	int ret;
642 	u32 i, total_parents = 0, type = 0;
643 	const char *parent_names[MAX_PARENT];
644 
645 	for (i = 0; i < clock_max_idx; i++) {
646 		char clk_name[MAX_NAME_LEN];
647 
648 		/* get clock name, continue to next clock if name not found */
649 		if (zynqmp_get_clock_name(i, clk_name))
650 			continue;
651 
652 		/* Check if clock is valid and output clock.
653 		 * Do not register invalid or external clock.
654 		 */
655 		ret = zynqmp_get_clock_type(i, &type);
656 		if (ret || type != CLK_TYPE_OUTPUT)
657 			continue;
658 
659 		/* Get parents of clock*/
660 		if (zynqmp_get_parent_list(np, i, parent_names,
661 					   &total_parents)) {
662 			WARN_ONCE(1, "No parents found for %s\n",
663 				  clock[i].clk_name);
664 			continue;
665 		}
666 
667 		zynqmp_data->hws[i] =
668 			zynqmp_register_clk_topology(i, clk_name,
669 						     total_parents,
670 						     parent_names);
671 	}
672 
673 	for (i = 0; i < clock_max_idx; i++) {
674 		if (IS_ERR(zynqmp_data->hws[i])) {
675 			pr_err("Zynq Ultrascale+ MPSoC clk %s: register failed with %ld\n",
676 			       clock[i].clk_name, PTR_ERR(zynqmp_data->hws[i]));
677 			WARN_ON(1);
678 		}
679 	}
680 	return 0;
681 }
682 
683 /**
684  * zynqmp_get_clock_info() - Get clock information from firmware using PM_API
685  */
zynqmp_get_clock_info(void)686 static void zynqmp_get_clock_info(void)
687 {
688 	int i, ret;
689 	u32 type = 0;
690 	u32 nodetype, subclass, class;
691 	struct attr_resp attr;
692 	struct name_resp name;
693 
694 	for (i = 0; i < clock_max_idx; i++) {
695 		ret = zynqmp_pm_clock_get_attributes(i, &attr);
696 		if (ret)
697 			continue;
698 
699 		clock[i].valid = FIELD_GET(CLK_ATTR_VALID, attr.attr[0]);
700 		/* skip query for Invalid clock */
701 		ret = zynqmp_is_valid_clock(i);
702 		if (ret != CLK_ATTR_VALID)
703 			continue;
704 
705 		clock[i].type = FIELD_GET(CLK_ATTR_TYPE, attr.attr[0]) ?
706 			CLK_TYPE_EXTERNAL : CLK_TYPE_OUTPUT;
707 
708 		nodetype = FIELD_GET(CLK_ATTR_NODE_TYPE, attr.attr[0]);
709 		subclass = FIELD_GET(CLK_ATTR_NODE_SUBCLASS, attr.attr[0]);
710 		class = FIELD_GET(CLK_ATTR_NODE_CLASS, attr.attr[0]);
711 
712 		clock[i].clk_id = FIELD_PREP(CLK_ATTR_NODE_CLASS, class) |
713 				  FIELD_PREP(CLK_ATTR_NODE_SUBCLASS, subclass) |
714 				  FIELD_PREP(CLK_ATTR_NODE_TYPE, nodetype) |
715 				  FIELD_PREP(CLK_ATTR_NODE_INDEX, i);
716 
717 		zynqmp_pm_clock_get_name(clock[i].clk_id, &name);
718 
719 		/*
720 		 * Terminate with NULL character in case name provided by firmware
721 		 * is longer and truncated due to size limit.
722 		 */
723 		name.name[sizeof(name.name) - 1] = '\0';
724 
725 		if (!strcmp(name.name, RESERVED_CLK_NAME))
726 			continue;
727 		strscpy(clock[i].clk_name, name.name, MAX_NAME_LEN);
728 	}
729 
730 	/* Get topology of all clock */
731 	for (i = 0; i < clock_max_idx; i++) {
732 		ret = zynqmp_get_clock_type(i, &type);
733 		if (ret || type != CLK_TYPE_OUTPUT)
734 			continue;
735 
736 		ret = zynqmp_clock_get_topology(i, clock[i].node,
737 						&clock[i].num_nodes);
738 		if (ret)
739 			continue;
740 
741 		ret = zynqmp_clock_get_parents(i, clock[i].parent,
742 					       &clock[i].num_parents);
743 		if (ret)
744 			continue;
745 	}
746 }
747 
748 /**
749  * zynqmp_clk_setup() - Setup the clock framework and register clocks
750  * @np:		Device node
751  *
752  * Return: 0 on success else error code
753  */
zynqmp_clk_setup(struct device_node * np)754 static int zynqmp_clk_setup(struct device_node *np)
755 {
756 	int ret;
757 
758 	ret = zynqmp_pm_clock_get_num_clocks(&clock_max_idx);
759 	if (ret)
760 		return ret;
761 
762 	zynqmp_data = kzalloc(struct_size(zynqmp_data, hws, clock_max_idx),
763 			      GFP_KERNEL);
764 	if (!zynqmp_data)
765 		return -ENOMEM;
766 
767 	clock = kcalloc(clock_max_idx, sizeof(*clock), GFP_KERNEL);
768 	if (!clock) {
769 		kfree(zynqmp_data);
770 		return -ENOMEM;
771 	}
772 
773 	zynqmp_get_clock_info();
774 	zynqmp_register_clocks(np);
775 
776 	zynqmp_data->num = clock_max_idx;
777 	return of_clk_add_hw_provider(np, of_clk_hw_onecell_get, zynqmp_data);
778 }
779 
zynqmp_clock_probe(struct platform_device * pdev)780 static int zynqmp_clock_probe(struct platform_device *pdev)
781 {
782 	int ret;
783 	struct device *dev = &pdev->dev;
784 
785 	ret = zynqmp_clk_setup(dev->of_node);
786 
787 	return ret;
788 }
789 
790 static const struct of_device_id zynqmp_clock_of_match[] = {
791 	{.compatible = "xlnx,zynqmp-clk"},
792 	{.compatible = "xlnx,versal-clk"},
793 	{},
794 };
795 MODULE_DEVICE_TABLE(of, zynqmp_clock_of_match);
796 
797 static struct platform_driver zynqmp_clock_driver = {
798 	.driver = {
799 		.name = "zynqmp_clock",
800 		.of_match_table = zynqmp_clock_of_match,
801 	},
802 	.probe = zynqmp_clock_probe,
803 };
804 module_platform_driver(zynqmp_clock_driver);
805