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 SCMI_RATES_BY_ARRAY(_nb_rates, _rem_rates) \ 244 SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS((_nb_rates), \ 245 SCMI_CLOCK_RATE_FORMAT_LIST, \ 246 (_rem_rates)) 247 #define SCMI_RATES_BY_STEP \ 248 SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS(3, \ 249 SCMI_CLOCK_RATE_FORMAT_RANGE, \ 250 0) 251 252 #define RATE_DESC_SIZE sizeof(struct scmi_clock_rate) 253 254 static void write_rate_desc_array_in_buffer(char *dest, unsigned long *rates, 255 size_t nb_elt) 256 { 257 uint32_t *out = NULL; 258 size_t n = 0; 259 260 assert(IS_ALIGNED_WITH_TYPE(dest, uint32_t)); 261 out = (uint32_t *)(uintptr_t)dest; 262 263 for (n = 0; n < nb_elt; n++) { 264 uint64_t rate = rates[n]; 265 266 reg_pair_from_64(rate, out + 2 * n + 1, out + 2 * n); 267 } 268 } 269 270 static void scmi_clock_describe_rates(struct scmi_msg *msg) 271 { 272 const struct scmi_clock_describe_rates_a2p *in_args = (void *)msg->in; 273 struct scmi_clock_describe_rates_p2a p2a = { }; 274 size_t nb_rates = 0; 275 int32_t status = SCMI_GENERIC_ERROR; 276 unsigned int clock_id = 0; 277 unsigned int out_count = 0; 278 279 if (msg->in_size != sizeof(*in_args)) { 280 scmi_status_response(msg, SCMI_PROTOCOL_ERROR); 281 return; 282 } 283 284 if (in_args->clock_id >= plat_scmi_clock_count(msg->channel_id)) { 285 scmi_status_response(msg, SCMI_INVALID_PARAMETERS); 286 return; 287 } 288 289 clock_id = confine_array_index(in_args->clock_id, 290 plat_scmi_clock_count(msg->channel_id)); 291 292 /* Platform may support array rate description */ 293 status = plat_scmi_clock_rates_array(msg->channel_id, clock_id, 0, NULL, 294 &nb_rates); 295 if (status == SCMI_SUCCESS) { 296 unsigned int rate_index = in_args->rate_index; 297 unsigned int remaining = 0; 298 size_t avail_sz = msg->out_size - sizeof(p2a); 299 char *out_rates = msg->out + sizeof(p2a); 300 301 if (avail_sz < RATE_DESC_SIZE && nb_rates) { 302 status = SCMI_PROTOCOL_ERROR; 303 goto out; 304 } 305 306 while (avail_sz >= RATE_DESC_SIZE && rate_index < nb_rates) { 307 unsigned long rate = 0; 308 size_t cnt = 1; 309 310 status = plat_scmi_clock_rates_array(msg->channel_id, 311 clock_id, 312 rate_index, 313 &rate, &cnt); 314 if (status) 315 goto out; 316 317 write_rate_desc_array_in_buffer(out_rates, &rate, cnt); 318 avail_sz -= RATE_DESC_SIZE; 319 out_rates += RATE_DESC_SIZE; 320 rate_index++; 321 } 322 323 out_count = rate_index - in_args->rate_index; 324 remaining = nb_rates - rate_index; 325 p2a.num_rates_flags = SCMI_RATES_BY_ARRAY(out_count, remaining); 326 } else if (status == SCMI_NOT_SUPPORTED) { 327 unsigned long triplet[3] = { 0, 0, 0 }; 328 329 if (msg->out_size < sizeof(p2a) + 3 * RATE_DESC_SIZE) { 330 status = SCMI_PROTOCOL_ERROR; 331 goto out; 332 } 333 334 /* Platform may support min/max/step triplet description */ 335 status = plat_scmi_clock_rates_by_step(msg->channel_id, 336 clock_id, triplet); 337 if (status) 338 goto out; 339 340 write_rate_desc_array_in_buffer(msg->out + sizeof(p2a), 341 triplet, 3); 342 343 out_count = 3; 344 p2a.num_rates_flags = SCMI_RATES_BY_STEP; 345 } else { 346 /* Fallthrough generic exit sequence below with error status */ 347 } 348 349 out: 350 if (status) { 351 scmi_status_response(msg, status); 352 } else { 353 p2a.status = SCMI_SUCCESS; 354 memcpy(msg->out, &p2a, sizeof(p2a)); 355 msg->out_size_out = sizeof(p2a) + out_count * RATE_DESC_SIZE; 356 } 357 } 358 359 static const scmi_msg_handler_t scmi_clock_handler_table[] = { 360 [SCMI_PROTOCOL_VERSION] = report_version, 361 [SCMI_PROTOCOL_ATTRIBUTES] = report_attributes, 362 [SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes, 363 [SCMI_CLOCK_ATTRIBUTES] = scmi_clock_attributes, 364 [SCMI_CLOCK_DESCRIBE_RATES] = scmi_clock_describe_rates, 365 [SCMI_CLOCK_RATE_SET] = scmi_clock_rate_set, 366 [SCMI_CLOCK_RATE_GET] = scmi_clock_rate_get, 367 [SCMI_CLOCK_CONFIG_SET] = scmi_clock_config_set, 368 }; 369 370 static bool message_id_is_supported(unsigned int message_id) 371 { 372 return message_id < ARRAY_SIZE(scmi_clock_handler_table) && 373 scmi_clock_handler_table[message_id]; 374 } 375 376 scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg) 377 { 378 const size_t array_size = ARRAY_SIZE(scmi_clock_handler_table); 379 unsigned int message_id = 0; 380 381 if (msg->message_id >= array_size) { 382 DMSG("Clock handle not found %u", msg->message_id); 383 return NULL; 384 } 385 386 message_id = confine_array_index(msg->message_id, array_size); 387 388 return scmi_clock_handler_table[message_id]; 389 } 390