1 // SPDX-License-Identifier: BSD-3-Clause 2 /* 3 * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved. 4 * Copyright (c) 2019-2020, Linaro Limited 5 */ 6 #include <cdefs.h> 7 #include <string.h> 8 9 #include <drivers/scmi-msg.h> 10 #include <drivers/scmi.h> 11 #include <lib/utils_def.h> 12 13 #include "common.h" 14 15 #pragma weak plat_scmi_clock_count 16 #pragma weak plat_scmi_clock_get_name 17 #pragma weak plat_scmi_clock_rates_array 18 #pragma weak plat_scmi_clock_rates_by_step 19 #pragma weak plat_scmi_clock_get_rate 20 #pragma weak plat_scmi_clock_set_rate 21 #pragma weak plat_scmi_clock_get_state 22 #pragma weak plat_scmi_clock_set_state 23 24 static bool message_id_is_supported(unsigned int message_id); 25 26 size_t plat_scmi_clock_count(unsigned int agent_id __unused) 27 { 28 return 0U; 29 } 30 31 const char *plat_scmi_clock_get_name(unsigned int agent_id __unused, 32 unsigned int scmi_id __unused) 33 { 34 return NULL; 35 } 36 37 int32_t plat_scmi_clock_rates_array(unsigned int agent_id __unused, 38 unsigned int scmi_id __unused, 39 unsigned long *rates __unused, 40 size_t *nb_elts __unused, 41 uint32_t start_idx __unused) 42 { 43 return SCMI_NOT_SUPPORTED; 44 } 45 46 int32_t plat_scmi_clock_rates_by_step(unsigned int agent_id __unused, 47 unsigned int scmi_id __unused, 48 unsigned long *steps __unused) 49 { 50 return SCMI_NOT_SUPPORTED; 51 } 52 53 unsigned long plat_scmi_clock_get_rate(unsigned int agent_id __unused, 54 unsigned int scmi_id __unused) 55 { 56 return 0U; 57 } 58 59 int32_t plat_scmi_clock_set_rate(unsigned int agent_id __unused, 60 unsigned int scmi_id __unused, 61 unsigned long rate __unused) 62 { 63 return SCMI_NOT_SUPPORTED; 64 } 65 66 int32_t plat_scmi_clock_get_state(unsigned int agent_id __unused, 67 unsigned int scmi_id __unused) 68 { 69 return SCMI_NOT_SUPPORTED; 70 } 71 72 int32_t plat_scmi_clock_set_state(unsigned int agent_id __unused, 73 unsigned int scmi_id __unused, 74 bool enable_not_disable __unused) 75 { 76 return SCMI_NOT_SUPPORTED; 77 } 78 79 static void report_version(struct scmi_msg *msg) 80 { 81 struct scmi_protocol_version_p2a return_values = { 82 .status = SCMI_SUCCESS, 83 .version = SCMI_PROTOCOL_VERSION_CLOCK, 84 }; 85 86 if (msg->in_size != 0) { 87 scmi_status_response(msg, SCMI_PROTOCOL_ERROR); 88 return; 89 } 90 91 scmi_write_response(msg, &return_values, sizeof(return_values)); 92 } 93 94 static void report_attributes(struct scmi_msg *msg) 95 { 96 size_t agent_count = plat_scmi_clock_count(msg->agent_id); 97 struct scmi_protocol_attributes_p2a return_values = { 98 .status = SCMI_SUCCESS, 99 .attributes = SCMI_CLOCK_PROTOCOL_ATTRIBUTES(1U, agent_count), 100 }; 101 102 if (msg->in_size != 0) { 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 report_message_attributes(struct scmi_msg *msg) 111 { 112 struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in; 113 struct scmi_protocol_message_attributes_p2a return_values = { 114 .status = SCMI_SUCCESS, 115 /* For this protocol, attributes shall be zero */ 116 .attributes = 0U, 117 }; 118 119 if (msg->in_size != sizeof(*in_args)) { 120 scmi_status_response(msg, SCMI_PROTOCOL_ERROR); 121 return; 122 } 123 124 if (!message_id_is_supported(in_args->message_id)) { 125 scmi_status_response(msg, SCMI_NOT_FOUND); 126 return; 127 } 128 129 scmi_write_response(msg, &return_values, sizeof(return_values)); 130 } 131 132 static void scmi_clock_attributes(struct scmi_msg *msg) 133 { 134 const struct scmi_clock_attributes_a2p *in_args = (void *)msg->in; 135 struct scmi_clock_attributes_p2a return_values = { 136 .status = SCMI_SUCCESS, 137 }; 138 const char *name = NULL; 139 unsigned int clock_id = 0U; 140 141 if (msg->in_size != sizeof(*in_args)) { 142 scmi_status_response(msg, SCMI_PROTOCOL_ERROR); 143 return; 144 } 145 146 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id); 147 148 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) { 149 scmi_status_response(msg, SCMI_NOT_FOUND); 150 return; 151 } 152 153 154 name = plat_scmi_clock_get_name(msg->agent_id, clock_id); 155 if (name == NULL) { 156 scmi_status_response(msg, SCMI_NOT_FOUND); 157 return; 158 } 159 160 COPY_NAME_IDENTIFIER(return_values.clock_name, name); 161 162 return_values.attributes = plat_scmi_clock_get_state(msg->agent_id, 163 clock_id); 164 165 scmi_write_response(msg, &return_values, sizeof(return_values)); 166 } 167 168 static void scmi_clock_rate_get(struct scmi_msg *msg) 169 { 170 const struct scmi_clock_rate_get_a2p *in_args = (void *)msg->in; 171 unsigned long rate = 0U; 172 struct scmi_clock_rate_get_p2a return_values = { 173 .status = SCMI_SUCCESS, 174 }; 175 unsigned int clock_id = 0U; 176 177 if (msg->in_size != sizeof(*in_args)) { 178 scmi_status_response(msg, SCMI_PROTOCOL_ERROR); 179 return; 180 } 181 182 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id); 183 184 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) { 185 scmi_status_response(msg, SCMI_NOT_FOUND); 186 return; 187 } 188 189 rate = plat_scmi_clock_get_rate(msg->agent_id, clock_id); 190 191 return_values.rate[0] = (uint32_t)rate; 192 return_values.rate[1] = (uint32_t)((uint64_t)rate >> 32); 193 194 scmi_write_response(msg, &return_values, sizeof(return_values)); 195 } 196 197 static void scmi_clock_rate_set(struct scmi_msg *msg) 198 { 199 const struct scmi_clock_rate_set_a2p *in_args = (void *)msg->in; 200 unsigned long rate = 0U; 201 int32_t status = 0; 202 unsigned int clock_id = 0U; 203 204 if (msg->in_size != sizeof(*in_args)) { 205 scmi_status_response(msg, SCMI_PROTOCOL_ERROR); 206 return; 207 } 208 209 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id); 210 211 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) { 212 scmi_status_response(msg, SCMI_NOT_FOUND); 213 return; 214 } 215 216 rate = (unsigned long)(((uint64_t)in_args->rate[1] << 32) | 217 in_args->rate[0]); 218 219 status = plat_scmi_clock_set_rate(msg->agent_id, clock_id, rate); 220 221 scmi_status_response(msg, status); 222 } 223 224 static void scmi_clock_config_set(struct scmi_msg *msg) 225 { 226 const struct scmi_clock_config_set_a2p *in_args = (void *)msg->in; 227 int32_t status = SCMI_GENERIC_ERROR; 228 bool enable = false; 229 unsigned int clock_id = 0U; 230 231 if (msg->in_size != sizeof(*in_args)) { 232 scmi_status_response(msg, SCMI_PROTOCOL_ERROR); 233 return; 234 } 235 236 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id); 237 238 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) { 239 scmi_status_response(msg, SCMI_NOT_FOUND); 240 return; 241 } 242 243 enable = in_args->attributes & SCMI_CLOCK_CONFIG_SET_ENABLE_MASK; 244 245 status = plat_scmi_clock_set_state(msg->agent_id, clock_id, enable); 246 247 scmi_status_response(msg, status); 248 } 249 250 #define RATES_ARRAY_SIZE_MAX (SCMI_PLAYLOAD_MAX - \ 251 sizeof(struct scmi_clock_describe_rates_p2a)) 252 253 #define SCMI_RATES_BY_ARRAY(_nb_rates, _rem_rates) \ 254 SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS((_nb_rates), \ 255 SCMI_CLOCK_RATE_FORMAT_LIST, \ 256 (_rem_rates)) 257 #define SCMI_RATES_BY_STEP \ 258 SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS(3U, \ 259 SCMI_CLOCK_RATE_FORMAT_RANGE, \ 260 0U) 261 262 #define RATE_DESC_SIZE sizeof(struct scmi_clock_rate) 263 264 static void write_rate_desc_array_in_buffer(char *dest, unsigned long *rates, 265 size_t nb_elt) 266 { 267 uint32_t *out = (uint32_t *)(uintptr_t)dest; 268 size_t n; 269 270 ASSERT_SYM_PTR_ALIGN(out); 271 272 for (n = 0U; n < nb_elt; n++) { 273 out[2 * n] = (uint32_t)rates[n]; 274 out[2 * n + 1] = (uint32_t)((uint64_t)rates[n] >> 32); 275 } 276 } 277 278 static void scmi_clock_describe_rates(struct scmi_msg *msg) 279 { 280 const struct scmi_clock_describe_rates_a2p *in_args = (void *)msg->in; 281 struct scmi_clock_describe_rates_p2a p2a = { 282 .status = SCMI_SUCCESS, 283 }; 284 size_t nb_rates; 285 int32_t status; 286 unsigned int clock_id; 287 288 if (msg->in_size != sizeof(*in_args)) { 289 scmi_status_response(msg, SCMI_PROTOCOL_ERROR); 290 return; 291 } 292 293 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id); 294 295 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) { 296 scmi_status_response(msg, SCMI_NOT_FOUND); 297 return; 298 } 299 300 /* Platform may support array rate description */ 301 status = plat_scmi_clock_rates_array(msg->agent_id, clock_id, NULL, 302 &nb_rates, 0); 303 if (status == SCMI_SUCCESS) { 304 /* Currently 12 cells mex, so it's affordable for the stack */ 305 unsigned long plat_rates[RATES_ARRAY_SIZE_MAX / RATE_DESC_SIZE]; 306 size_t max_nb = RATES_ARRAY_SIZE_MAX / RATE_DESC_SIZE; 307 size_t ret_nb = MIN(nb_rates - in_args->rate_index, max_nb); 308 size_t rem_nb = nb_rates - in_args->rate_index - ret_nb; 309 310 status = plat_scmi_clock_rates_array(msg->agent_id, clock_id, 311 plat_rates, &ret_nb, 312 in_args->rate_index); 313 if (status == SCMI_SUCCESS) { 314 write_rate_desc_array_in_buffer(msg->out + sizeof(p2a), 315 plat_rates, ret_nb); 316 317 p2a.num_rates_flags = SCMI_RATES_BY_ARRAY(ret_nb, 318 rem_nb); 319 p2a.status = SCMI_SUCCESS; 320 321 memcpy(msg->out, &p2a, sizeof(p2a)); 322 msg->out_size_out = sizeof(p2a) + 323 ret_nb * RATE_DESC_SIZE; 324 } 325 } else if (status == SCMI_NOT_SUPPORTED) { 326 unsigned long triplet[3] = { 0U, 0U, 0U }; 327 328 /* Platform may support min§max/step triplet description */ 329 status = plat_scmi_clock_rates_by_step(msg->agent_id, clock_id, 330 triplet); 331 if (status == SCMI_SUCCESS) { 332 write_rate_desc_array_in_buffer(msg->out + sizeof(p2a), 333 triplet, 3U); 334 335 p2a.num_rates_flags = SCMI_RATES_BY_STEP; 336 p2a.status = SCMI_SUCCESS; 337 338 memcpy(msg->out, &p2a, sizeof(p2a)); 339 msg->out_size_out = sizeof(p2a) + (3U * RATE_DESC_SIZE); 340 } 341 } else { 342 /* Fallthrough generic exit sequence below with error status */ 343 } 344 345 if (status != SCMI_SUCCESS) { 346 scmi_status_response(msg, status); 347 } else { 348 /* 349 * Message payload is already written to msg->out, and 350 * msg->out_size_out updated. 351 */ 352 } 353 } 354 355 static const scmi_msg_handler_t scmi_clock_handler_table[] = { 356 [SCMI_PROTOCOL_VERSION] = report_version, 357 [SCMI_PROTOCOL_ATTRIBUTES] = report_attributes, 358 [SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes, 359 [SCMI_CLOCK_ATTRIBUTES] = scmi_clock_attributes, 360 [SCMI_CLOCK_DESCRIBE_RATES] = scmi_clock_describe_rates, 361 [SCMI_CLOCK_RATE_SET] = scmi_clock_rate_set, 362 [SCMI_CLOCK_RATE_GET] = scmi_clock_rate_get, 363 [SCMI_CLOCK_CONFIG_SET] = scmi_clock_config_set, 364 }; 365 366 static bool message_id_is_supported(unsigned int message_id) 367 { 368 return (message_id < ARRAY_SIZE(scmi_clock_handler_table)) && 369 (scmi_clock_handler_table[message_id] != NULL); 370 } 371 372 scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg) 373 { 374 const size_t array_size = ARRAY_SIZE(scmi_clock_handler_table); 375 unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id); 376 377 if (message_id >= array_size) { 378 VERBOSE("Clock handle not found %u", msg->message_id); 379 return NULL; 380 } 381 382 return scmi_clock_handler_table[message_id]; 383 } 384