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