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