1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2024, STMicroelectronics International N.V.
4 */
5
6 #include <assert.h>
7 #include <drivers/scmi.h>
8 #include <kernel/boot.h>
9 #include <kernel/dt.h>
10 #include <kernel/panic.h>
11 #include <libfdt.h>
12 #include <scmi_agent_configuration.h>
13 #include <scmi_clock_consumer.h>
14 #include <scmi_reset_consumer.h>
15 #include <stdlib.h>
16 #include <sys/queue.h>
17 #include <util.h>
18
19 /*
20 * struct optee_scmi_server - Data of scmi_server_scpfw
21 *
22 * @dt_name: SCMI node name
23 * @agent_list: list of optee_scmi_server_agent
24 */
25 struct optee_scmi_server {
26 const char *dt_name;
27 SIMPLEQ_HEAD(, optee_scmi_server_agent) agent_list;
28 };
29
30 /*
31 * @struct optee_scmi_server_agent - Data of an SCMI agent
32 *
33 * @dt_name: SCMI agent node name
34 * @agent_id: SCMI agent identifier
35 * @channel_id: SCMI channel identifier
36 * @protocol_list: list of optee_scmi_server_protocol
37 * @link: link for optee_scmi_server:agent_list
38 */
39 struct optee_scmi_server_agent {
40 const char *dt_name;
41 unsigned int agent_id;
42 unsigned int channel_id;
43 SIMPLEQ_HEAD(, optee_scmi_server_protocol) protocol_list;
44 SIMPLEQ_ENTRY(optee_scmi_server_agent) link;
45 };
46
47 /*
48 * struct optee_scmi_server_protocol - Data of an SCMI protocol
49 *
50 * @dt_name: SCMI protocol node name
51 * @dt_node: SCMI protocol node
52 * @protocol_id: SCMI protocol identifier
53 * @link: list for optee_scmi_server_agent:protocol_list
54 */
55 struct optee_scmi_server_protocol {
56 const char *dt_name;
57 int dt_node;
58 unsigned int protocol_id;
59 SIMPLEQ_ENTRY(optee_scmi_server_protocol) link;
60 };
61
62 /* scmi_agent_configuration API */
63 static struct scpfw_config scpfw_cfg;
64
scmi_scpfw_free_agent(struct scpfw_agent_config * agent_cfg)65 static void scmi_scpfw_free_agent(struct scpfw_agent_config *agent_cfg)
66 {
67 unsigned int j = 0;
68
69 for (j = 0; j < agent_cfg->channel_count; j++) {
70 struct scpfw_channel_config *channel_cfg =
71 agent_cfg->channel_config + j;
72
73 free(channel_cfg->reset);
74 free(channel_cfg->clock);
75 }
76 free(agent_cfg->channel_config);
77 }
78
scmi_scpfw_get_configuration(void)79 struct scpfw_config *scmi_scpfw_get_configuration(void)
80 {
81 struct scpfw_agent_config *old_agent_config = scpfw_cfg.agent_config;
82
83 assert(scpfw_cfg.agent_count >= 1);
84 assert(!old_agent_config[0].channel_count);
85
86 /*
87 * The agents config data passed to SCP-firmware do not consider
88 * agent ID 0 that is reserved for the SCMI server itself in
89 * SCP-firmware and therefore has no configuration data.
90 */
91 scpfw_cfg.agent_count--;
92 scpfw_cfg.agent_config = calloc(scpfw_cfg.agent_count,
93 sizeof(*scpfw_cfg.agent_config));
94
95 memcpy(scpfw_cfg.agent_config, old_agent_config + 1,
96 sizeof(*scpfw_cfg.agent_config) * scpfw_cfg.agent_count);
97
98 free(old_agent_config);
99
100 return &scpfw_cfg;
101 }
102
scmi_scpfw_release_configuration(void)103 void scmi_scpfw_release_configuration(void)
104 {
105 unsigned int i = 0;
106
107 for (i = 0; i < scpfw_cfg.agent_count; i++)
108 scmi_scpfw_free_agent(scpfw_cfg.agent_config + i);
109
110 free(scpfw_cfg.agent_config);
111 }
112
optee_scmi_server_probe_agent(const void * fdt,int agent_node,struct optee_scmi_server_agent * agent_ctx)113 static TEE_Result optee_scmi_server_probe_agent(const void *fdt, int agent_node,
114 struct optee_scmi_server_agent
115 *agent_ctx)
116 {
117 struct optee_scmi_server_protocol *protocol_ctx = NULL;
118 int protocol_node = 0;
119 const fdt32_t *cuint = NULL;
120
121 SIMPLEQ_INIT(&agent_ctx->protocol_list);
122
123 /*
124 * Get agent ID from reg property, implicitly a single
125 * 32bit cell value. (required)
126 */
127 cuint = fdt_getprop(fdt, agent_node, "reg", NULL);
128 if (!cuint) {
129 EMSG("%s Missing property reg", agent_ctx->dt_name);
130 panic();
131 }
132 agent_ctx->agent_id = fdt32_to_cpu(*cuint);
133
134 /* Agent ID 0 is strictly reserved to SCMI server. */
135 assert(agent_ctx->agent_id > 0);
136
137 if (!fdt_node_check_compatible(fdt, agent_node, "linaro,scmi-optee")) {
138 cuint = fdt_getprop(fdt, agent_node, "scmi-channel-id", NULL);
139 if (!cuint) {
140 EMSG("%s scmi-channel-id property not found",
141 agent_ctx->dt_name);
142 panic();
143 }
144 agent_ctx->channel_id = fdt32_to_cpu(*cuint);
145 } else {
146 EMSG("%s Incorrect compatible", agent_ctx->dt_name);
147 panic();
148 }
149
150 fdt_for_each_subnode(protocol_node, fdt, agent_node) {
151 const char *node_name = fdt_get_name(fdt, protocol_node, NULL);
152 struct optee_scmi_server_protocol *p = NULL;
153
154 if (!strstr(node_name, "protocol@"))
155 continue;
156
157 protocol_ctx = calloc(1, sizeof(*protocol_ctx));
158 if (!protocol_ctx)
159 return TEE_ERROR_OUT_OF_MEMORY;
160
161 protocol_ctx->dt_name = node_name;
162 protocol_ctx->dt_node = protocol_node;
163
164 /*
165 * Get protocol ID from reg property, implicitly a single
166 * 32bit cell value. (required)
167 */
168 cuint = fdt_getprop(fdt, protocol_node, "reg", NULL);
169 if (!cuint) {
170 EMSG("%s Missing property reg", protocol_ctx->dt_name);
171 panic();
172 }
173 protocol_ctx->protocol_id = fdt32_to_cpu(*cuint);
174
175 SIMPLEQ_FOREACH(p, &agent_ctx->protocol_list, link)
176 assert(p->protocol_id != protocol_ctx->protocol_id);
177
178 SIMPLEQ_INSERT_TAIL(&agent_ctx->protocol_list, protocol_ctx,
179 link);
180 }
181
182 return TEE_SUCCESS;
183 }
184
185 static void
optee_scmi_server_init_protocol(const void * fdt,struct optee_scmi_server_protocol * protocol_ctx,struct scpfw_agent_config * agent_cfg,struct scpfw_channel_config * channel_cfg)186 optee_scmi_server_init_protocol(const void *fdt,
187 struct optee_scmi_server_protocol *protocol_ctx,
188 struct scpfw_agent_config *agent_cfg,
189 struct scpfw_channel_config *channel_cfg)
190 {
191 switch (protocol_ctx->protocol_id) {
192 case SCMI_PROTOCOL_ID_CLOCK:
193 if (optee_scmi_server_init_clocks(fdt, protocol_ctx->dt_node,
194 agent_cfg, channel_cfg))
195 panic("Error during clocks init");
196 break;
197 case SCMI_PROTOCOL_ID_RESET_DOMAIN:
198 if (optee_scmi_server_init_resets(fdt, protocol_ctx->dt_node,
199 agent_cfg, channel_cfg))
200 panic("Error during resets init");
201 break;
202 default:
203 EMSG("%s Unknown protocol ID: %#x", protocol_ctx->dt_name,
204 protocol_ctx->protocol_id);
205 panic();
206 }
207 }
208
optee_scmi_server_probe(const void * fdt,int parent_node,const void * compat_data __unused)209 static TEE_Result optee_scmi_server_probe(const void *fdt, int parent_node,
210 const void *compat_data __unused)
211 {
212 struct optee_scmi_server *ctx = NULL;
213 struct optee_scmi_server_agent *agent_ctx = NULL;
214 struct optee_scmi_server_agent *a = NULL;
215 TEE_Result res = TEE_SUCCESS;
216 unsigned int agent_cfg_count = 0;
217 unsigned int i = 0;
218 int agent_node = 0;
219
220 ctx = calloc(1, sizeof(*ctx));
221 if (!ctx)
222 return TEE_ERROR_OUT_OF_MEMORY;
223
224 ctx->dt_name = fdt_get_name(fdt, parent_node, NULL);
225
226 /* Read device tree */
227 SIMPLEQ_INIT(&ctx->agent_list);
228
229 fdt_for_each_subnode(agent_node, fdt, parent_node) {
230 const char *node_name = fdt_get_name(fdt, agent_node, NULL);
231
232 if (!strstr(node_name, "agent@"))
233 continue;
234
235 agent_ctx = calloc(1, sizeof(*agent_ctx));
236 if (!agent_ctx) {
237 res = TEE_ERROR_OUT_OF_MEMORY;
238 goto fail_agent;
239 }
240 agent_ctx->dt_name = node_name;
241
242 res = optee_scmi_server_probe_agent(fdt, agent_node, agent_ctx);
243 if (res)
244 goto fail_agent;
245
246 SIMPLEQ_FOREACH(a, &ctx->agent_list, link)
247 assert(a->agent_id != agent_ctx->agent_id);
248
249 SIMPLEQ_INSERT_TAIL(&ctx->agent_list, agent_ctx, link);
250 agent_cfg_count = MAX(agent_cfg_count, agent_ctx->agent_id);
251 }
252
253 agent_cfg_count++;
254
255 /* Create SCMI config structures */
256 scpfw_cfg.agent_count = agent_cfg_count;
257 scpfw_cfg.agent_config = calloc(scpfw_cfg.agent_count,
258 sizeof(*scpfw_cfg.agent_config));
259 if (!scpfw_cfg.agent_config) {
260 res = TEE_ERROR_OUT_OF_MEMORY;
261 scpfw_cfg.agent_count = 0;
262 goto fail_agent;
263 }
264
265 SIMPLEQ_FOREACH(agent_ctx, &ctx->agent_list, link) {
266 struct scpfw_agent_config *agent_cfg =
267 scpfw_cfg.agent_config + agent_ctx->agent_id;
268
269 agent_cfg->name = (const char *)strdup(agent_ctx->dt_name);
270 if (!agent_cfg->name) {
271 res = TEE_ERROR_OUT_OF_MEMORY;
272 goto fail_scpfw_cfg;
273 }
274
275 agent_cfg->agent_id = agent_ctx->agent_id;
276
277 /*
278 * Right now this driver can handle one channel per agent only.
279 */
280 assert(agent_ctx->channel_id == 0);
281 agent_cfg->channel_count = 1;
282 agent_cfg->channel_config =
283 calloc(agent_cfg->channel_count,
284 sizeof(*agent_cfg->channel_config));
285 if (!agent_cfg->channel_config) {
286 res = TEE_ERROR_OUT_OF_MEMORY;
287 agent_cfg->channel_count = 0;
288 goto fail_scpfw_cfg;
289 }
290
291 for (i = 0; i < agent_cfg->channel_count; i++) {
292 struct scpfw_channel_config *channel_cfg =
293 agent_cfg->channel_config + i;
294
295 channel_cfg->name = "channel";
296 channel_cfg->channel_id = agent_ctx->channel_id;
297 }
298 }
299
300 /* Parse protocols and fill channels config */
301 SIMPLEQ_FOREACH(agent_ctx, &ctx->agent_list, link) {
302 struct optee_scmi_server_protocol *protocol_ctx = NULL;
303 struct scpfw_agent_config *agent_cfg =
304 scpfw_cfg.agent_config + agent_ctx->agent_id;
305 struct scpfw_channel_config *channel_cfg =
306 agent_cfg->channel_config + agent_ctx->channel_id;
307
308 SIMPLEQ_FOREACH(protocol_ctx, &agent_ctx->protocol_list, link)
309 optee_scmi_server_init_protocol(fdt, protocol_ctx,
310 agent_cfg, channel_cfg);
311 }
312
313 return TEE_SUCCESS;
314
315 fail_scpfw_cfg:
316 scmi_scpfw_release_configuration();
317
318 for (i = 0; i < scpfw_cfg.agent_count; i++)
319 free((void *)scpfw_cfg.agent_config[i].name);
320
321 fail_agent:
322 while (!SIMPLEQ_EMPTY(&ctx->agent_list)) {
323 agent_ctx = SIMPLEQ_FIRST(&ctx->agent_list);
324
325 while (!SIMPLEQ_EMPTY(&agent_ctx->protocol_list)) {
326 struct optee_scmi_server_protocol *protocol_ctx =
327 SIMPLEQ_FIRST(&agent_ctx->protocol_list);
328
329 SIMPLEQ_REMOVE_HEAD(&agent_ctx->protocol_list, link);
330 free(protocol_ctx);
331 }
332
333 SIMPLEQ_REMOVE_HEAD(&ctx->agent_list, link);
334 free(agent_ctx);
335 }
336
337 free(ctx);
338
339 return res;
340 }
341
optee_scmi_server_init(void)342 static TEE_Result optee_scmi_server_init(void)
343 {
344 const void *fdt = get_embedded_dt();
345 int node = -1;
346
347 if (!fdt)
348 panic();
349
350 node = fdt_node_offset_by_compatible(fdt, node, "optee,scmi-server");
351 if (node < 0)
352 panic();
353
354 return optee_scmi_server_probe(fdt, node, NULL);
355 }
356
357 driver_init_late(optee_scmi_server_init);
358