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