1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2021, Bootlin
4 */
5
6 #include <assert.h>
7 #include <drivers/clk.h>
8 #include <drivers/clk_dt.h>
9 #include <initcall.h>
10 #include <kernel/boot.h>
11 #include <kernel/dt_driver.h>
12 #include <kernel/panic.h>
13 #include <libfdt.h>
14 #include <stddef.h>
15
clk_dt_get_by_name(const void * fdt,int nodeoffset,const char * name,struct clk ** clk)16 TEE_Result clk_dt_get_by_name(const void *fdt, int nodeoffset,
17 const char *name, struct clk **clk)
18 {
19 int clk_id = 0;
20
21 clk_id = fdt_stringlist_search(fdt, nodeoffset, "clock-names", name);
22 if (clk_id < 0) {
23 *clk = NULL;
24 return TEE_ERROR_GENERIC;
25 }
26
27 return clk_dt_get_by_index(fdt, nodeoffset, clk_id, clk);
28 }
29
clk_dt_get_by_idx_prop(const char * prop_name,const void * fdt,int nodeoffset,unsigned int clk_idx,struct clk ** clk)30 static TEE_Result clk_dt_get_by_idx_prop(const char *prop_name, const void *fdt,
31 int nodeoffset, unsigned int clk_idx,
32 struct clk **clk)
33 {
34 TEE_Result res = TEE_ERROR_GENERIC;
35
36 *clk = dt_driver_device_from_node_idx_prop(prop_name, fdt, nodeoffset,
37 clk_idx, DT_DRIVER_CLK,
38 &res);
39 return res;
40 }
41
clk_dt_get_by_index(const void * fdt,int nodeoffset,unsigned int clk_idx,struct clk ** clk)42 TEE_Result clk_dt_get_by_index(const void *fdt, int nodeoffset,
43 unsigned int clk_idx, struct clk **clk)
44 {
45 return clk_dt_get_by_idx_prop("clocks", fdt, nodeoffset, clk_idx, clk);
46 }
47
48 #ifdef CFG_DRIVERS_CLK_EARLY_PROBE
49 /* Recursively called from parse_clock_property() */
50 static TEE_Result clk_probe_clock_provider_node(const void *fdt, int node);
51
parse_clock_property(const void * fdt,int node)52 static TEE_Result parse_clock_property(const void *fdt, int node)
53 {
54 int len = 0;
55 int idx = 0;
56 int parent_node = 0;
57 int clock_cells = 0;
58 uint32_t phandle = 0;
59 const uint32_t *prop = NULL;
60 TEE_Result res = TEE_ERROR_GENERIC;
61
62 prop = fdt_getprop(fdt, node, "clocks", &len);
63 if (!prop)
64 return TEE_SUCCESS;
65
66 len /= sizeof(uint32_t);
67 while (idx < len) {
68 phandle = fdt32_to_cpu(prop[idx]);
69
70 parent_node = fdt_node_offset_by_phandle(fdt, phandle);
71 if (parent_node < 0)
72 return TEE_ERROR_GENERIC;
73
74 /* Parent probe should not fail or clock won't be available */
75 res = clk_probe_clock_provider_node(fdt, parent_node);
76 if (res) {
77 EMSG("Probe parent clock node %s on node %s: %#"PRIx32,
78 fdt_get_name(fdt, parent_node, NULL),
79 fdt_get_name(fdt, node, NULL), res);
80 panic();
81 }
82
83 clock_cells = fdt_get_dt_driver_cells(fdt, parent_node,
84 DT_DRIVER_CLK);
85 if (clock_cells < 0)
86 return TEE_ERROR_GENERIC;
87
88 idx += 1 + clock_cells;
89 }
90
91 return TEE_SUCCESS;
92 }
93
clk_probe_clock_provider_node(const void * fdt,int node)94 static TEE_Result clk_probe_clock_provider_node(const void *fdt, int node)
95 {
96 int len = 0;
97 int status = 0;
98 TEE_Result res = TEE_ERROR_GENERIC;
99
100 status = _fdt_get_status(fdt, node);
101 if (!(status & DT_STATUS_OK_SEC))
102 return TEE_ERROR_ITEM_NOT_FOUND;
103
104 /* Check if the node is a clock provider */
105 if (!fdt_getprop(fdt, node, "#clock-cells", &len))
106 return TEE_ERROR_ITEM_NOT_FOUND;
107
108 /* Check if node has already been probed */
109 if (dt_driver_get_provider_by_node(node, DT_DRIVER_CLK))
110 return TEE_SUCCESS;
111
112 /* Check if the node has a clock property first to probe parent */
113 res = parse_clock_property(fdt, node);
114 if (res)
115 return res;
116
117 return dt_driver_probe_device_by_node(fdt, node, DT_DRIVER_CLK);
118 }
119
clk_probe_node(const void * fdt,int parent_node)120 static void clk_probe_node(const void *fdt, int parent_node)
121 {
122 int child = 0;
123 int status = 0;
124 __maybe_unused TEE_Result res = TEE_ERROR_GENERIC;
125
126 fdt_for_each_subnode(child, fdt, parent_node) {
127 status = _fdt_get_status(fdt, child);
128 if (status == DT_STATUS_DISABLED)
129 continue;
130
131 res = clk_probe_clock_provider_node(fdt, child);
132 assert(res == TEE_SUCCESS || res == TEE_ERROR_ITEM_NOT_FOUND);
133
134 clk_probe_node(fdt, child);
135 }
136 }
137
parse_assigned_clock(const void * fdt,int nodeoffset)138 static void parse_assigned_clock(const void *fdt, int nodeoffset)
139 {
140 int rate_len = 0;
141 int clock_idx = 0;
142 struct clk *clk = NULL;
143 unsigned long rate = 0;
144 struct clk *parent = NULL;
145 const uint32_t *rate_prop = NULL;
146 TEE_Result __maybe_unused res = TEE_ERROR_GENERIC;
147
148 rate_prop = fdt_getprop(fdt, nodeoffset, "assigned-clock-rates",
149 &rate_len);
150 rate_len /= sizeof(uint32_t);
151
152 while (true) {
153 res = clk_dt_get_by_idx_prop("assigned-clocks", fdt, nodeoffset,
154 clock_idx, &clk);
155 if (!clk)
156 return;
157 assert(!res);
158
159 res = clk_dt_get_by_idx_prop("assigned-clock-parents", fdt,
160 nodeoffset, clock_idx, &parent);
161 if (parent) {
162 assert(!res);
163 if (clk_set_parent(clk, parent)) {
164 EMSG("Could not set clk %s parent to clock %s",
165 clk->name, parent->name);
166 panic();
167 }
168 }
169
170 if (rate_prop && clock_idx < rate_len) {
171 rate = fdt32_to_cpu(rate_prop[clock_idx]);
172 if (rate && clk_set_rate(clk, rate) != TEE_SUCCESS)
173 panic();
174 }
175
176 clock_idx++;
177 }
178 }
179
clk_probe_assigned(const void * fdt,int parent_node)180 static void clk_probe_assigned(const void *fdt, int parent_node)
181 {
182 int len = 0;
183 int child = 0;
184 int status = 0;
185
186 fdt_for_each_subnode(child, fdt, parent_node) {
187 clk_probe_assigned(fdt, child);
188
189 status = _fdt_get_status(fdt, child);
190 if (status == DT_STATUS_DISABLED)
191 continue;
192
193 if (fdt_getprop(fdt, child, "assigned-clocks", &len))
194 parse_assigned_clock(fdt, child);
195 }
196 }
197
clk_dt_probe(void)198 static TEE_Result clk_dt_probe(void)
199 {
200 const void *fdt = get_secure_dt();
201
202 DMSG("Probing clocks from devicetree");
203 if (!fdt)
204 panic();
205
206 clk_probe_node(fdt, -1);
207
208 clk_probe_assigned(fdt, -1);
209
210 return TEE_SUCCESS;
211 }
212 early_init(clk_dt_probe);
213 #endif
214