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 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 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 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 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 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 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 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