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