1 // SPDX-License-Identifier: BSD-3-Clause 2 /* 3 * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved. 4 * Copyright (c) 2021, Linaro Limited 5 * Copyright (c) 2024, STMicroelectronics 6 */ 7 #include <assert.h> 8 #include <confine_array_index.h> 9 #include <drivers/scmi-msg.h> 10 #include <drivers/scmi.h> 11 #include <mm/core_memprot.h> 12 #include <string.h> 13 #include <util.h> 14 15 #include "common.h" 16 #include "perf_domain.h" 17 18 #define VERBOSE_MSG(...) FMSG(__VA_ARGS__) 19 20 static bool message_id_is_supported(unsigned int message_id); 21 22 /* Weak handlers platform shall override on purpose */ 23 size_t __weak plat_scmi_perf_count(unsigned int channel_id __unused) 24 { 25 return 0; 26 } 27 28 void __weak *plat_scmi_perf_statistics_buf(unsigned int channel_id __unused, 29 size_t *stats_len) 30 { 31 *stats_len = 0; 32 33 return NULL; 34 } 35 36 const char __weak *plat_scmi_perf_domain_name(unsigned int channel_id __unused, 37 unsigned int domain_id __unused) 38 { 39 return NULL; 40 } 41 42 int32_t __weak plat_scmi_perf_sustained_freq(unsigned int channel_id __unused, 43 unsigned int domain_id __unused, 44 unsigned int *freq __unused) 45 { 46 return SCMI_NOT_SUPPORTED; 47 } 48 49 int32_t __weak plat_scmi_perf_levels_array(unsigned int channel_id __unused, 50 unsigned int domain_id __unused, 51 size_t start_index __unused, 52 unsigned int *elt __unused, 53 size_t *nb_elts __unused) 54 { 55 return SCMI_NOT_SUPPORTED; 56 } 57 58 int32_t __weak plat_scmi_perf_level_latency(unsigned int channel_id __unused, 59 unsigned int domain_id __unused, 60 unsigned int level __unused, 61 unsigned int *latency) 62 { 63 /* Use 1 microsecond because the Linux kernel treats 0 as eternal */ 64 *latency = 1; 65 66 return SCMI_SUCCESS; 67 } 68 69 int32_t __weak plat_scmi_perf_level_power_cost(unsigned int channel_id __unused, 70 unsigned int domain_id __unused, 71 unsigned int level __unused, 72 unsigned int *cost __unused) 73 { 74 *cost = 0; 75 76 return SCMI_SUCCESS; 77 } 78 79 int32_t __weak plat_scmi_perf_level_get(unsigned int channel_id __unused, 80 unsigned int domain_id __unused, 81 unsigned int *level __unused) 82 { 83 return SCMI_NOT_SUPPORTED; 84 } 85 86 int32_t __weak plat_scmi_perf_level_set(unsigned int channel_id __unused, 87 unsigned int domain_id __unused, 88 unsigned int level __unused) 89 { 90 return SCMI_NOT_SUPPORTED; 91 } 92 93 static void protocol_version(struct scmi_msg *msg) 94 { 95 struct scmi_protocol_version_p2a return_values = { 96 .status = SCMI_SUCCESS, 97 .version = SCMI_PROTOCOL_VERSION_PERF_DOMAIN, 98 }; 99 100 VERBOSE_MSG("SCMI perf %#"PRIx32, SCMI_PROTOCOL_VERSION_PERF_DOMAIN); 101 102 if (msg->in_size) { 103 scmi_status_response(msg, SCMI_PROTOCOL_ERROR); 104 return; 105 } 106 107 scmi_write_response(msg, &return_values, sizeof(return_values)); 108 } 109 110 static void protocol_attributes(struct scmi_msg *msg) 111 { 112 unsigned int channel_id = msg->channel_id; 113 size_t count = plat_scmi_perf_count(channel_id); 114 uint32_t power_in_mw = 0; 115 struct scmi_perf_protocol_attributes_p2a return_values = { 116 .status = SCMI_SUCCESS, 117 .attributes = SCMI_PERF_PROTOCOL_ATTRIBUTES(power_in_mw, count), 118 }; 119 void *stats_buf = NULL; 120 size_t stats_len = 0; 121 122 VERBOSE_MSG("channel %u: %zu performance domains", channel_id, count); 123 124 if (msg->in_size) { 125 scmi_status_response(msg, SCMI_PROTOCOL_ERROR); 126 return; 127 } 128 129 stats_buf = plat_scmi_perf_statistics_buf(channel_id, &stats_len); 130 if (stats_len) { 131 paddr_t stats_pa = virt_to_phys(stats_buf); 132 133 if (stats_pa && tee_vbuf_is_non_sec(stats_buf, stats_len)) { 134 return_values.statistics_len = stats_len; 135 reg_pair_from_64((uint64_t)stats_pa, 136 &return_values.statistics_address_high, 137 &return_values.statistics_address_low); 138 } else { 139 IMSG("Disable SCMI perf statistics: invalid buffer"); 140 DMSG("Stats buffer va %p, pa %#"PRIxPA", size %zu", 141 stats_buf, stats_pa, stats_len); 142 } 143 } 144 145 scmi_write_response(msg, &return_values, sizeof(return_values)); 146 } 147 148 static void protocol_message_attributes(struct scmi_msg *msg) 149 { 150 struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in; 151 struct scmi_protocol_message_attributes_p2a return_values = { 152 .status = SCMI_SUCCESS, 153 /* For this protocol, attributes shall be zero */ 154 .attributes = 0, 155 }; 156 157 if (msg->in_size != sizeof(*in_args)) { 158 scmi_status_response(msg, SCMI_PROTOCOL_ERROR); 159 return; 160 } 161 162 if (!message_id_is_supported(in_args->message_id)) { 163 scmi_status_response(msg, SCMI_NOT_FOUND); 164 return; 165 } 166 167 scmi_write_response(msg, &return_values, sizeof(return_values)); 168 } 169 170 static int32_t sanitize_message(struct scmi_msg *msg, unsigned int *domain_id, 171 size_t exp_in_size) 172 { 173 size_t domain_count = plat_scmi_perf_count(msg->channel_id); 174 175 *domain_id = confine_array_index(*domain_id, domain_count); 176 177 if (msg->in_size != exp_in_size) 178 return SCMI_PROTOCOL_ERROR; 179 180 if (*domain_id >= domain_count) 181 return SCMI_INVALID_PARAMETERS; 182 183 return SCMI_SUCCESS; 184 } 185 186 static void scmi_perf_domain_attributes(struct scmi_msg *msg) 187 { 188 const struct scmi_perf_attributes_a2p *in_args = (void *)msg->in; 189 /* It is safe to read in_args->domain_id before sanitize_message() */ 190 unsigned int domain_id = in_args->domain_id; 191 int32_t res = SCMI_GENERIC_ERROR; 192 struct scmi_perf_attributes_p2a return_values = { 193 .attributes = SCMI_PERF_DOMAIN_ATTRIBUTES_CAN_SET_LEVEL, 194 .status = SCMI_SUCCESS, 195 }; 196 const char *name = NULL; 197 198 FMSG("channel %u: domain %u", msg->channel_id, domain_id); 199 200 res = sanitize_message(msg, &domain_id, sizeof(*in_args)); 201 if (res) { 202 scmi_status_response(msg, res); 203 return; 204 } 205 206 name = plat_scmi_perf_domain_name(msg->channel_id, domain_id); 207 if (!name) { 208 scmi_status_response(msg, SCMI_NOT_FOUND); 209 return; 210 } 211 212 res = plat_scmi_perf_sustained_freq(msg->channel_id, domain_id, 213 &return_values.sustained_freq); 214 if (res) { 215 scmi_status_response(msg, res); 216 return; 217 } 218 219 COPY_NAME_IDENTIFIER(return_values.name, name); 220 221 /* 222 * .rate_limit and .sustained_perf_level are 223 * implicitly set to 0. 224 */ 225 226 VERBOSE_MSG("channel %u: domain %u: name \"%s\"", msg->channel_id, 227 domain_id, name); 228 229 scmi_write_response(msg, &return_values, sizeof(return_values)); 230 } 231 232 static void scmi_perf_level_get(struct scmi_msg *msg) 233 { 234 const struct scmi_perf_level_get_a2p *in_args = (void *)msg->in; 235 /* It is safe to read in_args->domain_id before sanitize_message() */ 236 unsigned int domain_id = in_args->domain_id; 237 int32_t res = SCMI_GENERIC_ERROR; 238 unsigned int level = 0; 239 struct scmi_perf_level_get_p2a return_values = { 240 .status = SCMI_SUCCESS, 241 }; 242 243 VERBOSE_MSG("channel %u, domain %u", msg->channel_id, domain_id); 244 245 res = sanitize_message(msg, &domain_id, sizeof(*in_args)); 246 if (res) { 247 scmi_status_response(msg, res); 248 return; 249 } 250 251 res = plat_scmi_perf_level_get(msg->channel_id, domain_id, &level); 252 if (res) { 253 scmi_status_response(msg, res); 254 return; 255 } 256 257 assert(level <= UINT32_MAX); 258 return_values.performance_level = level; 259 260 VERBOSE_MSG("channel %u, domain %u: level %u", msg->channel_id, 261 domain_id, level); 262 263 scmi_write_response(msg, &return_values, sizeof(return_values)); 264 } 265 266 static void scmi_perf_level_set(struct scmi_msg *msg) 267 { 268 const struct scmi_perf_level_set_a2p *in_args = (void *)msg->in; 269 unsigned int channel_id = msg->channel_id; 270 /* It is safe to read in in_args before sanitize_message() */ 271 unsigned int domain_id = in_args->domain_id; 272 unsigned int level = in_args->performance_level; 273 int32_t res = SCMI_GENERIC_ERROR; 274 275 VERBOSE_MSG("channel %u, domain %u: set level %u", channel_id, 276 domain_id, level); 277 278 res = sanitize_message(msg, &domain_id, sizeof(*in_args)); 279 if (res == SCMI_SUCCESS) 280 res = plat_scmi_perf_level_set(channel_id, domain_id, level); 281 282 scmi_status_response(msg, res); 283 } 284 285 /* List levels array by small chunks fitting in SCMI message max payload size */ 286 #define LEVELS_ARRAY_SIZE \ 287 ((SCMI_SEC_PAYLOAD_SIZE - \ 288 sizeof(struct scmi_perf_describe_levels_a2p)) / \ 289 sizeof(struct scmi_perf_level)) 290 291 static void scmi_perf_describe_levels(struct scmi_msg *msg) 292 { 293 const struct scmi_perf_describe_levels_a2p *in_args = (void *)msg->in; 294 size_t nb_levels = 0; 295 /* It is safe to read in_args->domain_id before sanitize_message() */ 296 unsigned int domain_id = in_args->domain_id; 297 int32_t res = SCMI_GENERIC_ERROR; 298 /* Use the stack to get the returned a portion of the level array */ 299 unsigned int plat_levels[LEVELS_ARRAY_SIZE] = { 0 }; 300 size_t ret_nb = 0; 301 size_t rem_nb = 0; 302 303 VERBOSE_MSG("channel %u, domain %u", msg->channel_id, domain_id); 304 305 res = sanitize_message(msg, &domain_id, sizeof(*in_args)); 306 if (res) 307 goto err; 308 309 res = plat_scmi_perf_levels_array(msg->channel_id, domain_id, 0, 310 NULL, &nb_levels); 311 if (res) 312 goto err; 313 314 if (in_args->level_index >= nb_levels) { 315 res = SCMI_INVALID_PARAMETERS; 316 goto err; 317 } 318 319 ret_nb = MIN(ARRAY_SIZE(plat_levels), nb_levels - in_args->level_index); 320 rem_nb = nb_levels - in_args->level_index - ret_nb; 321 322 res = plat_scmi_perf_levels_array(msg->channel_id, domain_id, 323 in_args->level_index, plat_levels, 324 &ret_nb); 325 326 if (res == SCMI_SUCCESS) { 327 struct scmi_perf_describe_levels_p2a p2a = { 328 .status = SCMI_SUCCESS, 329 .num_levels = SCMI_PERF_NUM_LEVELS(ret_nb, rem_nb), 330 }; 331 struct scmi_perf_level *levels = NULL; 332 size_t n = 0; 333 334 memcpy(msg->out, &p2a, sizeof(p2a)); 335 336 /* By construction these values are 32bit aligned */ 337 levels = (void *)(uintptr_t)(msg->out + sizeof(p2a)); 338 339 for (n = 0; n < ret_nb; n++) { 340 unsigned int latency = 0; 341 unsigned int power_cost = 0; 342 343 res = plat_scmi_perf_level_latency(msg->channel_id, 344 domain_id, 345 plat_levels[n], 346 &latency); 347 if (res != SCMI_SUCCESS) 348 goto err; 349 350 assert(latency <= UINT16_MAX); 351 latency &= SCMI_PERF_LEVEL_ATTRIBUTES_LATENCY_US_MASK; 352 353 res = plat_scmi_perf_level_power_cost(msg->channel_id, 354 domain_id, 355 plat_levels[n], 356 &power_cost); 357 if (res != SCMI_SUCCESS) 358 goto err; 359 360 levels[n] = (struct scmi_perf_level){ 361 .performance_level = plat_levels[n], 362 .power_cost = power_cost, 363 .attributes = latency, 364 }; 365 } 366 367 msg->out_size_out = 368 sizeof(p2a) + ret_nb * sizeof(struct scmi_perf_level); 369 370 return; 371 } 372 373 err: 374 assert(res); 375 scmi_status_response(msg, res); 376 } 377 378 static const scmi_msg_handler_t scmi_perf_handler_table[] = { 379 [SCMI_PROTOCOL_VERSION] = protocol_version, 380 [SCMI_PROTOCOL_ATTRIBUTES] = protocol_attributes, 381 [SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = protocol_message_attributes, 382 [SCMI_PERF_DOMAIN_ATTRIBUTES] = scmi_perf_domain_attributes, 383 [SCMI_PERF_DESCRIBE_LEVELS] = scmi_perf_describe_levels, 384 [SCMI_PERF_LEVEL_SET] = scmi_perf_level_set, 385 [SCMI_PERF_LEVEL_GET] = scmi_perf_level_get, 386 }; 387 388 static bool message_id_is_supported(size_t message_id) 389 { 390 return message_id < ARRAY_SIZE(scmi_perf_handler_table) && 391 scmi_perf_handler_table[message_id]; 392 } 393 394 scmi_msg_handler_t scmi_msg_get_perf_handler(struct scmi_msg *msg) 395 { 396 const size_t array_size = ARRAY_SIZE(scmi_perf_handler_table); 397 unsigned int message_id = 0; 398 399 if (msg->message_id >= array_size) { 400 DMSG("handle not found %u", msg->message_id); 401 return NULL; 402 } 403 404 message_id = confine_array_index(msg->message_id, array_size); 405 406 return scmi_perf_handler_table[message_id]; 407 } 408