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