1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2024, STMicroelectronics
4 */
5
6 #include <assert.h>
7 #include <drivers/clk.h>
8 #include <drivers/clk_dt.h>
9 #include <dt-bindings/scmi/scmi-clock.h>
10 #include <kernel/boot.h>
11 #include <kernel/panic.h>
12 #include <libfdt.h>
13 #include <malloc.h>
14 #include <scmi_agent_configuration.h>
15 #include <scmi_clock_consumer.h>
16 #include <tee_api_defines_extensions.h>
17 #include <trace.h>
18
19 /*
20 * struct scmi_server_clock: data for a SCMI clock in DT
21 *
22 * @domain_id: SCMI domain identifier
23 * @domain_name: SCMI domain name
24 * @clock: clock to control through SCMI protocol
25 * @flags: capabilities of the SCMI clock
26 */
27 struct scmi_server_clock {
28 uint32_t domain_id;
29 const char *domain_name;
30 struct clk *clock;
31 uint32_t flags;
32 };
33
34 /*
35 * struct scmi_clock_consumer: context of scmi_clock_consumer.c for one channel
36 *
37 * @clocks: platform clocks exposed with SCMI
38 * @scmi_flags: capabilities of an clocks
39 * @count: number of clocks and scmi_flags
40 * @link: link for list of scmi_clock_consumer
41 */
42 struct scmi_clock_consumer {
43 struct clk *clocks;
44 uint32_t *scmi_flags;
45 unsigned int count;
46
47 SLIST_ENTRY(scmi_clock_consumer) link;
48 };
49
50 static SLIST_HEAD(scmi_clock_consumer_head, scmi_clock_consumer) ctx =
51 SLIST_HEAD_INITIALIZER(&ctx);
52
scmi_clk_get_rates_array(struct clk * clk,size_t index,unsigned long * rates,size_t * nb_elts)53 static TEE_Result scmi_clk_get_rates_array(struct clk *clk, size_t index,
54 unsigned long *rates,
55 size_t *nb_elts)
56 {
57 uint32_t scmi_flags = *(uint32_t *)clk->priv;
58
59 if (!nb_elts)
60 return TEE_ERROR_BAD_PARAMETERS;
61
62 if (scmi_flags & SCMI_CLOCK_ALLOW_SET_RATE)
63 return clk_get_rates_array(clk->parent, index, rates, nb_elts);
64
65 if (!rates || !*nb_elts) {
66 *nb_elts = 1;
67 return TEE_SUCCESS;
68 }
69
70 if (index)
71 return TEE_ERROR_BAD_PARAMETERS;
72
73 /*
74 * Clocks not exposed have no effective parent/platform clock.
75 * Report a 0 Hz rate in this case.
76 */
77 if (clk->parent)
78 *rates = clk_get_rate(clk->parent);
79 else
80 *rates = 0;
81
82 *nb_elts = 1;
83
84 return TEE_SUCCESS;
85 }
86
scmi_clk_get_rates_steps(struct clk * clk,unsigned long * min,unsigned long * max,unsigned long * step)87 static TEE_Result scmi_clk_get_rates_steps(struct clk *clk,
88 unsigned long *min,
89 unsigned long *max,
90 unsigned long *step)
91 {
92 uint32_t scmi_flags = *(uint32_t *)clk->priv;
93 TEE_Result res = TEE_ERROR_GENERIC;
94
95 if (scmi_flags & SCMI_CLOCK_ALLOW_SET_RATE) {
96 res = clk_get_rates_steps(clk->parent, min, max, step);
97 } else {
98 *min = clk_get_rate(clk);
99 *max = *min;
100 *step = 1;
101
102 res = TEE_SUCCESS;
103 }
104
105 return res;
106 }
107
108 static const struct clk_ops scmi_clk_ops = {
109 .get_rates_array = scmi_clk_get_rates_array,
110 .get_rates_steps = scmi_clk_get_rates_steps,
111 };
112
optee_scmi_server_init_clocks(const void * fdt,int node,struct scpfw_agent_config * agent_cfg,struct scpfw_channel_config * channel_cfg)113 TEE_Result optee_scmi_server_init_clocks(const void *fdt, int node,
114 struct scpfw_agent_config *agent_cfg,
115 struct scpfw_channel_config
116 *channel_cfg)
117 {
118 TEE_Result res = TEE_ERROR_GENERIC;
119 struct scmi_clock_consumer *consumer = NULL;
120 struct scmi_server_clock *s_clocks = NULL;
121 size_t s_clocks_count = 0;
122 int item_node = 0;
123 int subnode = 0;
124 bool have_subnodes = false;
125 size_t n = 0;
126 size_t i = 0;
127
128 item_node = fdt_subnode_offset(fdt, node, "clocks");
129 if (item_node < 0)
130 return TEE_SUCCESS;
131
132 /* Compute the number of domains to allocate */
133 fdt_for_each_subnode(subnode, fdt, item_node) {
134 paddr_t reg = fdt_reg_base_address(fdt, subnode);
135
136 assert(reg != DT_INFO_INVALID_REG);
137 if (reg > s_clocks_count)
138 s_clocks_count = reg;
139
140 have_subnodes = true;
141 }
142
143 if (!have_subnodes)
144 return TEE_SUCCESS;
145
146 consumer = calloc(1, sizeof(*consumer));
147 if (!consumer)
148 return TEE_ERROR_OUT_OF_MEMORY;
149
150 SLIST_INSERT_HEAD(&ctx, consumer, link);
151
152 /* Number of SCMI clock domains is the max domain ID + 1 */
153 s_clocks_count++;
154 s_clocks = calloc(s_clocks_count, sizeof(*s_clocks));
155 if (!s_clocks)
156 panic();
157
158 fdt_for_each_subnode(subnode, fdt, item_node) {
159 struct scmi_server_clock *s_clock = NULL;
160 const fdt32_t *cuint = NULL;
161 struct clk *clock = NULL;
162 uint32_t domain_id = 0;
163
164 res = clk_dt_get_by_index(fdt, subnode, 0, &clock);
165 if (res == TEE_ERROR_DEFER_DRIVER_INIT) {
166 panic("Unexpected init deferral");
167 } else if (res) {
168 EMSG("Can't get clock %s (%#"PRIx32"), skipped",
169 fdt_get_name(fdt, subnode, NULL), res);
170 continue;
171 }
172
173 domain_id = fdt_reg_base_address(fdt, subnode);
174 s_clock = s_clocks + domain_id;
175 s_clock->domain_id = domain_id;
176
177 cuint = fdt_getprop(fdt, subnode, "domain-name", NULL);
178 if (cuint)
179 s_clock->domain_name = (char *)cuint;
180 else
181 s_clock->domain_name = fdt_get_name(fdt, subnode, NULL);
182
183 /* Check that the domain_id is not already used */
184 if (s_clock->clock) {
185 EMSG("Domain ID %"PRIu32" already used", domain_id);
186 panic();
187 }
188 s_clock->clock = clock;
189
190 /*
191 * Get clock flags
192 */
193 cuint = fdt_getprop(fdt, subnode, "flags", NULL);
194 if (cuint) {
195 s_clock->flags = fdt32_to_cpu(*cuint);
196 s_clock->flags &= SCMI_CLOCK_MASK;
197 }
198
199 DMSG("scmi clock shares %s on domain ID %"PRIu32,
200 s_clock->domain_name, domain_id);
201 }
202
203 for (n = 0; n < s_clocks_count; n++) {
204 /*
205 * Assign domain IDs to un-exposed clock as SCMI specification
206 * requires the resource is defined even if not accessible.
207 */
208 if (!s_clocks[n].clock) {
209 s_clocks[n].domain_id = n;
210 s_clocks[n].domain_name = "";
211 }
212
213 s_clocks[n].domain_name = strdup(s_clocks[n].domain_name);
214 if (!s_clocks[n].domain_name)
215 panic();
216 }
217
218 if (consumer->clocks || channel_cfg->clock) {
219 EMSG("Clock already loaded: agent %u, channel %u",
220 agent_cfg->agent_id, channel_cfg->channel_id);
221 panic();
222 }
223
224 consumer->count = s_clocks_count;
225 consumer->clocks = calloc(consumer->count, sizeof(*consumer->clocks));
226 assert(consumer->clocks);
227
228 consumer->scmi_flags = calloc(consumer->count,
229 sizeof(*consumer->scmi_flags));
230 assert(consumer->scmi_flags);
231
232 channel_cfg->clock_count = s_clocks_count;
233 channel_cfg->clock = calloc(channel_cfg->clock_count,
234 sizeof(*channel_cfg->clock));
235 assert(channel_cfg->clock);
236
237 for (i = 0; i < s_clocks_count; i++) {
238 unsigned int domain_id = s_clocks[i].domain_id;
239 struct clk *new_clock = consumer->clocks + domain_id;
240 uint32_t *scmi_flags = consumer->scmi_flags + domain_id;
241
242 *scmi_flags = s_clocks[i].flags;
243
244 new_clock->ops = &scmi_clk_ops;
245 new_clock->priv = scmi_flags;
246 new_clock->name = s_clocks[i].domain_name;
247 new_clock->parent = s_clocks[i].clock;
248
249 if (*scmi_flags & SCMI_CLOCK_ALLOW_SET_RATE)
250 new_clock->flags = CLK_SET_RATE_PARENT;
251
252 new_clock->flags |= CLK_DUTY_CYCLE_PARENT;
253
254 if (clk_register(new_clock))
255 panic();
256
257 channel_cfg->clock[domain_id] = (struct scmi_clock){
258 .name = clk_get_name(new_clock),
259 .clk = new_clock,
260 .enabled = *scmi_flags & SCMI_CLOCK_DEFAULT_ENABLED,
261 };
262 }
263
264 /*
265 * We can free s_clk_channel resources since content is now
266 * referenced from the SCMI server configuration data.
267 */
268 free(s_clocks);
269
270 return TEE_SUCCESS;
271 }
272