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_get_enable_delay
18 #pragma weak plat_scmi_clock_rates_array
19 #pragma weak plat_scmi_clock_rates_by_step
20 #pragma weak plat_scmi_clock_get_possible_parents
21 #pragma weak plat_scmi_clock_get_parent
22 #pragma weak plat_scmi_clock_set_parent
23 #pragma weak plat_scmi_clock_get_rate
24 #pragma weak plat_scmi_clock_set_rate
25 #pragma weak plat_scmi_clock_get_state
26 #pragma weak plat_scmi_clock_set_state
27 #pragma weak plat_scmi_clock_get_extended_config
28 #pragma weak plat_scmi_clock_set_extended_config
29
30 static bool message_id_is_supported(unsigned int message_id);
31
plat_scmi_clock_count(unsigned int agent_id __unused)32 size_t plat_scmi_clock_count(unsigned int agent_id __unused)
33 {
34 return 0U;
35 }
36
plat_scmi_clock_get_name(unsigned int agent_id __unused,unsigned int scmi_id __unused)37 const char *plat_scmi_clock_get_name(unsigned int agent_id __unused,
38 unsigned int scmi_id __unused)
39 {
40 return NULL;
41 }
42
plat_scmi_clock_get_enable_delay(unsigned int agent_id __unused,unsigned int scmi_id __unused)43 uint32_t plat_scmi_clock_get_enable_delay(unsigned int agent_id __unused,
44 unsigned int scmi_id __unused)
45 {
46 return 0U;
47 }
48
plat_scmi_clock_rates_array(unsigned int agent_id __unused,unsigned int scmi_id __unused,unsigned long * rates __unused,size_t * nb_elts __unused,uint32_t start_idx __unused)49 int32_t plat_scmi_clock_rates_array(unsigned int agent_id __unused,
50 unsigned int scmi_id __unused,
51 unsigned long *rates __unused,
52 size_t *nb_elts __unused,
53 uint32_t start_idx __unused)
54 {
55 return SCMI_NOT_SUPPORTED;
56 }
57
plat_scmi_clock_rates_by_step(unsigned int agent_id __unused,unsigned int scmi_id __unused,unsigned long * steps __unused)58 int32_t plat_scmi_clock_rates_by_step(unsigned int agent_id __unused,
59 unsigned int scmi_id __unused,
60 unsigned long *steps __unused)
61 {
62 return SCMI_NOT_SUPPORTED;
63 }
64
plat_scmi_clock_get_possible_parents(unsigned int agent_id,unsigned int scmi_id,unsigned int * plat_possible_parents,size_t * nb_elts,unsigned int skip_parents)65 int32_t plat_scmi_clock_get_possible_parents(unsigned int agent_id,
66 unsigned int scmi_id,
67 unsigned int *plat_possible_parents,
68 size_t *nb_elts,
69 unsigned int skip_parents)
70 {
71 return SCMI_NOT_SUPPORTED;
72 }
73
plat_scmi_clock_get_parent(unsigned int agent_id,unsigned int scmi_id,unsigned int * parent_id)74 int32_t plat_scmi_clock_get_parent(unsigned int agent_id,
75 unsigned int scmi_id,
76 unsigned int *parent_id)
77 {
78 return SCMI_NOT_SUPPORTED;
79 }
80
plat_scmi_clock_set_parent(unsigned int agent_id,unsigned int scmi_id,unsigned int parent_id)81 int32_t plat_scmi_clock_set_parent(unsigned int agent_id,
82 unsigned int scmi_id,
83 unsigned int parent_id)
84 {
85 return SCMI_NOT_SUPPORTED;
86 }
87
plat_scmi_clock_get_rate(unsigned int agent_id __unused,unsigned int scmi_id __unused)88 unsigned long plat_scmi_clock_get_rate(unsigned int agent_id __unused,
89 unsigned int scmi_id __unused)
90 {
91 return 0U;
92 }
93
plat_scmi_clock_set_rate(unsigned int agent_id __unused,unsigned int scmi_id __unused,unsigned long rate __unused)94 int32_t plat_scmi_clock_set_rate(unsigned int agent_id __unused,
95 unsigned int scmi_id __unused,
96 unsigned long rate __unused)
97 {
98 return SCMI_NOT_SUPPORTED;
99 }
100
plat_scmi_clock_get_state(unsigned int agent_id __unused,unsigned int scmi_id __unused)101 int32_t plat_scmi_clock_get_state(unsigned int agent_id __unused,
102 unsigned int scmi_id __unused)
103 {
104 return SCMI_NOT_SUPPORTED;
105 }
106
plat_scmi_clock_set_state(unsigned int agent_id __unused,unsigned int scmi_id __unused,bool enable_not_disable __unused)107 int32_t plat_scmi_clock_set_state(unsigned int agent_id __unused,
108 unsigned int scmi_id __unused,
109 bool enable_not_disable __unused)
110 {
111 return SCMI_NOT_SUPPORTED;
112 }
113
plat_scmi_clock_get_extended_config(unsigned int agent_id __unused,unsigned int scmi_id __unused,unsigned char extended_config_type __unused,unsigned int * extended_config_val __unused)114 int32_t plat_scmi_clock_get_extended_config(unsigned int agent_id __unused,
115 unsigned int scmi_id __unused,
116 unsigned char extended_config_type __unused,
117 unsigned int *extended_config_val __unused)
118 {
119 return SCMI_NOT_SUPPORTED;
120 }
121
plat_scmi_clock_set_extended_config(unsigned int agent_id __unused,unsigned int scmi_id __unused,unsigned char extended_config_type __unused,unsigned int extended_config_val __unused)122 int32_t plat_scmi_clock_set_extended_config(unsigned int agent_id __unused,
123 unsigned int scmi_id __unused,
124 unsigned char extended_config_type __unused,
125 unsigned int extended_config_val __unused)
126 {
127 return SCMI_NOT_SUPPORTED;
128 }
129
report_version(struct scmi_msg * msg)130 static void report_version(struct scmi_msg *msg)
131 {
132 struct scmi_protocol_version_p2a return_values = {
133 .status = SCMI_SUCCESS,
134 .version = SCMI_PROTOCOL_VERSION_CLOCK,
135 };
136
137 if (msg->in_size != 0) {
138 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
139 return;
140 }
141
142 scmi_write_response(msg, &return_values, sizeof(return_values));
143 }
144
report_attributes(struct scmi_msg * msg)145 static void report_attributes(struct scmi_msg *msg)
146 {
147 size_t agent_count = plat_scmi_clock_count(msg->agent_id);
148 struct scmi_protocol_attributes_p2a return_values = {
149 .status = SCMI_SUCCESS,
150 .attributes = SCMI_CLOCK_PROTOCOL_ATTRIBUTES(1U, agent_count),
151 };
152
153 if (msg->in_size != 0) {
154 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
155 return;
156 }
157
158 scmi_write_response(msg, &return_values, sizeof(return_values));
159 }
160
report_message_attributes(struct scmi_msg * msg)161 static void report_message_attributes(struct scmi_msg *msg)
162 {
163 struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
164 struct scmi_protocol_message_attributes_p2a return_values = {
165 .status = SCMI_SUCCESS,
166 /* For this protocol, attributes shall be zero */
167 .attributes = 0U,
168 };
169
170 if (msg->in_size != sizeof(*in_args)) {
171 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
172 return;
173 }
174
175 if (!message_id_is_supported(in_args->message_id)) {
176 scmi_status_response(msg, SCMI_NOT_FOUND);
177 return;
178 }
179
180 scmi_write_response(msg, &return_values, sizeof(return_values));
181 }
182
scmi_clock_attributes(struct scmi_msg * msg)183 static void scmi_clock_attributes(struct scmi_msg *msg)
184 {
185 const struct scmi_clock_attributes_a2p *in_args = (void *)msg->in;
186 struct scmi_clock_attributes_p2a return_values = {
187 .status = SCMI_SUCCESS,
188 };
189 const char *name = NULL;
190 unsigned int clock_id = 0U;
191
192 if (msg->in_size != sizeof(*in_args)) {
193 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
194 return;
195 }
196
197 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
198
199 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
200 scmi_status_response(msg, SCMI_NOT_FOUND);
201 return;
202 }
203
204
205 name = plat_scmi_clock_get_name(msg->agent_id, clock_id);
206 if (name == NULL) {
207 scmi_status_response(msg, SCMI_NOT_FOUND);
208 return;
209 }
210
211 COPY_NAME_IDENTIFIER(return_values.clock_name, name);
212
213 return_values.attributes = plat_scmi_clock_get_state(msg->agent_id,
214 clock_id);
215
216 return_values.attributes |= BIT(SCMI_CLOCK_EXTENDED_CONFIG_SUPPORT_POS);
217 return_values.attributes |= BIT(SCMI_CLOCK_PARENT_IDENTIFIER_SUPPORT_POS);
218
219 return_values.clock_enable_delay = plat_scmi_clock_get_enable_delay(msg->agent_id,
220 clock_id);
221
222 scmi_write_response(msg, &return_values, sizeof(return_values));
223 }
224
225
226
227 #define PARENTS_ARRAY_SIZE_MAX (SCMI_PLAYLOAD_MAX - \
228 sizeof(struct scmi_clock_possible_parents_get_p2a))
229
230 /* Protocol limits the maximum number of parent clock identifiers that are
231 * remaining or returned by this call
232 */
233 #define PARENT_CLOCK_MAX_COUNT 255U
234
235 #define PARENT_CLOCK_IDENTIFIER_SIZE sizeof(uint32_t)
236
237 #define PARENTS_ARRAY_ELEMENTS_NUMBER_MAX ((uint32_t)(PARENTS_ARRAY_SIZE_MAX / \
238 PARENT_CLOCK_IDENTIFIER_SIZE))
239
write_possible_parents_array_in_buffer(char * dest,unsigned int * possible_parents,size_t nb_elt)240 static void write_possible_parents_array_in_buffer(char *dest, unsigned int *possible_parents,
241 size_t nb_elt)
242 {
243 uint32_t *out = (uint32_t *)(uintptr_t)dest;
244 size_t n;
245
246 ASSERT_SYM_PTR_ALIGN(out);
247
248 for (n = 0U; n < nb_elt; n++) {
249 out[n] = (uint32_t)possible_parents[n];
250 }
251 }
252
scmi_clock_possible_parents_get(struct scmi_msg * msg)253 static void scmi_clock_possible_parents_get(struct scmi_msg *msg)
254 {
255 const struct scmi_clock_possible_parents_get_a2p *in_args = (void *)msg->in;
256 struct scmi_clock_possible_parents_get_p2a p2a = {
257 .status = SCMI_SUCCESS,
258 .flags = 0,
259 };
260
261 unsigned int clock_id = 0U;
262 size_t nb_possible_parents;
263 int32_t status;
264
265 if (msg->in_size != sizeof(*in_args)) {
266 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
267 return;
268 }
269
270 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
271
272 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
273 scmi_status_response(msg, SCMI_NOT_FOUND);
274 return;
275 }
276 /* Get number of possible parents */
277 status = plat_scmi_clock_get_possible_parents(msg->agent_id, clock_id, NULL,
278 &nb_possible_parents, 0);
279 if (status == SCMI_SUCCESS) {
280
281 if (in_args->skip_parents >= nb_possible_parents) {
282 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
283 return;
284 }
285
286 /* Currently 20 cells max, so it's affordable for the stack */
287 unsigned int plat_possible_parents[PARENTS_ARRAY_ELEMENTS_NUMBER_MAX];
288 size_t max_nb = MIN(PARENTS_ARRAY_ELEMENTS_NUMBER_MAX,
289 PARENT_CLOCK_MAX_COUNT);
290 size_t ret_nb = MIN(nb_possible_parents - in_args->skip_parents, max_nb);
291 size_t rem_nb = nb_possible_parents - in_args->skip_parents - ret_nb;
292
293 rem_nb = MIN((uint32_t)rem_nb, PARENT_CLOCK_MAX_COUNT);
294
295 status = plat_scmi_clock_get_possible_parents(msg->agent_id, clock_id,
296 plat_possible_parents, &ret_nb,
297 in_args->skip_parents);
298 if (status == SCMI_SUCCESS) {
299 ret_nb = MIN(ret_nb, max_nb);
300 write_possible_parents_array_in_buffer(msg->out + sizeof(p2a),
301 plat_possible_parents, ret_nb);
302
303 p2a.flags = ((uint8_t)rem_nb) << 24;
304 p2a.flags |= (uint8_t)ret_nb;
305 p2a.status = SCMI_SUCCESS;
306
307 memcpy(msg->out, &p2a, sizeof(p2a));
308 msg->out_size_out = sizeof(p2a) +
309 ret_nb * sizeof(unsigned int);
310 }
311
312 } else {
313 /* Fallthrough generic exit sequence below with error status */
314 }
315
316 if (status != SCMI_SUCCESS) {
317 scmi_status_response(msg, status);
318 } else {
319 /*
320 * Message payload is already written to msg->out, and
321 * msg->out_size_out updated.
322 */
323 }
324 }
325
scmi_clock_parent_get(struct scmi_msg * msg)326 static void scmi_clock_parent_get(struct scmi_msg *msg)
327 {
328 const struct scmi_clock_parent_get_a2p *in_args = (void *)msg->in;
329
330 struct scmi_clock_parent_get_p2a return_values = {
331 .status = SCMI_SUCCESS,
332 };
333 unsigned int clock_id = 0U;
334
335 if (msg->in_size != sizeof(*in_args)) {
336 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
337 return;
338 }
339
340 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
341
342 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
343 scmi_status_response(msg, SCMI_NOT_FOUND);
344 return;
345 }
346
347 return_values.status = plat_scmi_clock_get_parent(msg->agent_id, clock_id,
348 &return_values.parent_id);
349
350 scmi_write_response(msg, &return_values, sizeof(return_values));
351 }
352
scmi_clock_parent_set(struct scmi_msg * msg)353 static void scmi_clock_parent_set(struct scmi_msg *msg)
354 {
355 const struct scmi_clock_parent_set_a2p *in_args = (void *)msg->in;
356
357 int32_t status = 0;
358 unsigned int clock_id = 0U;
359 unsigned int parent_id = 0U;
360 unsigned int clock_count = 0U;
361
362 if (msg->in_size != sizeof(*in_args)) {
363 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
364 return;
365 }
366
367 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
368 parent_id = SPECULATION_SAFE_VALUE(in_args->parent_id);
369
370 clock_count = plat_scmi_clock_count(msg->agent_id);
371 if (clock_id >= clock_count || parent_id >= clock_count) {
372 scmi_status_response(msg, SCMI_NOT_FOUND);
373 return;
374 }
375
376 status = plat_scmi_clock_set_parent(msg->agent_id, clock_id, parent_id);
377
378 scmi_status_response(msg, status);
379 }
380
scmi_clock_rate_get(struct scmi_msg * msg)381 static void scmi_clock_rate_get(struct scmi_msg *msg)
382 {
383 const struct scmi_clock_rate_get_a2p *in_args = (void *)msg->in;
384 unsigned long rate = 0U;
385 struct scmi_clock_rate_get_p2a return_values = {
386 .status = SCMI_SUCCESS,
387 };
388 unsigned int clock_id = 0U;
389
390 if (msg->in_size != sizeof(*in_args)) {
391 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
392 return;
393 }
394
395 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
396
397 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
398 scmi_status_response(msg, SCMI_NOT_FOUND);
399 return;
400 }
401
402 rate = plat_scmi_clock_get_rate(msg->agent_id, clock_id);
403
404 return_values.rate[0] = (uint32_t)rate;
405 return_values.rate[1] = (uint32_t)((uint64_t)rate >> 32);
406
407 scmi_write_response(msg, &return_values, sizeof(return_values));
408 }
409
scmi_clock_rate_set(struct scmi_msg * msg)410 static void scmi_clock_rate_set(struct scmi_msg *msg)
411 {
412 const struct scmi_clock_rate_set_a2p *in_args = (void *)msg->in;
413 unsigned long rate = 0U;
414 int32_t status = 0;
415 unsigned int clock_id = 0U;
416
417 if (msg->in_size != sizeof(*in_args)) {
418 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
419 return;
420 }
421
422 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
423
424 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
425 scmi_status_response(msg, SCMI_NOT_FOUND);
426 return;
427 }
428
429 rate = (unsigned long)(((uint64_t)in_args->rate[1] << 32) |
430 in_args->rate[0]);
431
432 status = plat_scmi_clock_set_rate(msg->agent_id, clock_id, rate);
433
434 scmi_status_response(msg, status);
435 }
436
scmi_clock_config_get(struct scmi_msg * msg)437 static void scmi_clock_config_get(struct scmi_msg *msg)
438 {
439 const struct scmi_clock_config_get_a2p *in_args = (void *)msg->in;
440 struct scmi_clock_config_get_p2a p2a = {
441 .status = SCMI_SUCCESS,
442 };
443 unsigned int extended_config_type = 0U;
444 unsigned int clock_id = 0U;
445
446 if (msg->in_size != sizeof(*in_args)) {
447 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
448 return;
449 }
450
451 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
452
453 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
454 scmi_status_response(msg, SCMI_NOT_FOUND);
455 return;
456 }
457
458 p2a.config = plat_scmi_clock_get_state(msg->agent_id, clock_id);
459
460 extended_config_type = in_args->flags & SCMI_CLOCK_EXTENDED_CONFIG_GET_TYPE_MASK;
461 if (extended_config_type != 0U) {
462 p2a.status = plat_scmi_clock_get_extended_config(msg->agent_id,
463 clock_id,
464 extended_config_type,
465 &p2a.extended_config_val);
466 }
467
468 scmi_write_response(msg, &p2a, sizeof(p2a));
469 }
470
scmi_clock_config_set(struct scmi_msg * msg)471 static void scmi_clock_config_set(struct scmi_msg *msg)
472 {
473 const struct scmi_clock_config_set_a2p *in_args = (void *)msg->in;
474 int32_t status = SCMI_GENERIC_ERROR;
475 uint8_t enable = 0U;
476 unsigned int clock_id = 0U;
477 uint8_t extended_config_type = 0U;
478
479 if (msg->in_size != sizeof(*in_args)) {
480 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
481 return;
482 }
483
484 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
485
486 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
487 scmi_status_response(msg, SCMI_NOT_FOUND);
488 return;
489 }
490
491 enable = in_args->attributes & SCMI_CLOCK_CONFIG_SET_ENABLE_MASK;
492
493 extended_config_type = in_args->attributes & SCMI_CLOCK_EXTENDED_CONFIG_SET_TYPE_MASK;
494
495 if ((extended_config_type == 0U)
496 && (enable == SCMI_CLOCK_CONFIG_SET_UNCHANGED_STATE)) {
497 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
498 return;
499 }
500
501 if (enable == SCMI_CLOCK_CONFIG_SET_RESERVED_STATE) {
502 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
503 return;
504 }
505
506 if (enable != SCMI_CLOCK_CONFIG_SET_UNCHANGED_STATE) {
507 status = plat_scmi_clock_set_state(msg->agent_id, clock_id, (bool)enable);
508 if (status != SCMI_SUCCESS) {
509 scmi_status_response(msg, status);
510 return;
511 }
512 }
513
514 if (extended_config_type != 0U) {
515 status = plat_scmi_clock_set_extended_config(msg->agent_id, clock_id,
516 extended_config_type,
517 in_args->extended_config_val);
518 }
519
520 scmi_status_response(msg, status);
521 }
522
523 #define RATES_ARRAY_SIZE_MAX (SCMI_PLAYLOAD_MAX - \
524 sizeof(struct scmi_clock_describe_rates_p2a))
525
526 #define SCMI_RATES_BY_ARRAY(_nb_rates, _rem_rates) \
527 SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS((_nb_rates), \
528 SCMI_CLOCK_RATE_FORMAT_LIST, \
529 (_rem_rates))
530 #define SCMI_RATES_BY_STEP \
531 SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS(3U, \
532 SCMI_CLOCK_RATE_FORMAT_RANGE, \
533 0U)
534
535 #define RATE_DESC_SIZE sizeof(struct scmi_clock_rate)
536
write_rate_desc_array_in_buffer(char * dest,unsigned long * rates,size_t nb_elt)537 static void write_rate_desc_array_in_buffer(char *dest, unsigned long *rates,
538 size_t nb_elt)
539 {
540 uint32_t *out = (uint32_t *)(uintptr_t)dest;
541 size_t n;
542
543 ASSERT_SYM_PTR_ALIGN(out);
544
545 for (n = 0U; n < nb_elt; n++) {
546 out[2 * n] = (uint32_t)rates[n];
547 out[2 * n + 1] = (uint32_t)((uint64_t)rates[n] >> 32);
548 }
549 }
550
scmi_clock_describe_rates(struct scmi_msg * msg)551 static void scmi_clock_describe_rates(struct scmi_msg *msg)
552 {
553 const struct scmi_clock_describe_rates_a2p *in_args = (void *)msg->in;
554 struct scmi_clock_describe_rates_p2a p2a = {
555 .status = SCMI_SUCCESS,
556 };
557 size_t nb_rates;
558 int32_t status;
559 unsigned int clock_id;
560
561 if (msg->in_size != sizeof(*in_args)) {
562 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
563 return;
564 }
565
566 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
567
568 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
569 scmi_status_response(msg, SCMI_NOT_FOUND);
570 return;
571 }
572
573 /* Platform may support array rate description */
574 status = plat_scmi_clock_rates_array(msg->agent_id, clock_id, NULL,
575 &nb_rates, 0);
576 if (status == SCMI_SUCCESS) {
577 /* Currently 12 cells mex, so it's affordable for the stack */
578 unsigned long plat_rates[RATES_ARRAY_SIZE_MAX / RATE_DESC_SIZE];
579 size_t max_nb = RATES_ARRAY_SIZE_MAX / RATE_DESC_SIZE;
580 size_t ret_nb = MIN(nb_rates - in_args->rate_index, max_nb);
581 size_t rem_nb = nb_rates - in_args->rate_index - ret_nb;
582
583 status = plat_scmi_clock_rates_array(msg->agent_id, clock_id,
584 plat_rates, &ret_nb,
585 in_args->rate_index);
586 if (status == SCMI_SUCCESS) {
587 write_rate_desc_array_in_buffer(msg->out + sizeof(p2a),
588 plat_rates, ret_nb);
589
590 p2a.num_rates_flags = SCMI_RATES_BY_ARRAY(ret_nb,
591 rem_nb);
592 p2a.status = SCMI_SUCCESS;
593
594 memcpy(msg->out, &p2a, sizeof(p2a));
595 msg->out_size_out = sizeof(p2a) +
596 ret_nb * RATE_DESC_SIZE;
597 }
598 } else if (status == SCMI_NOT_SUPPORTED) {
599 unsigned long triplet[3] = { 0U, 0U, 0U };
600
601 /* Platform may support min§max/step triplet description */
602 status = plat_scmi_clock_rates_by_step(msg->agent_id, clock_id,
603 triplet);
604 if (status == SCMI_SUCCESS) {
605 write_rate_desc_array_in_buffer(msg->out + sizeof(p2a),
606 triplet, 3U);
607
608 p2a.num_rates_flags = SCMI_RATES_BY_STEP;
609 p2a.status = SCMI_SUCCESS;
610
611 memcpy(msg->out, &p2a, sizeof(p2a));
612 msg->out_size_out = sizeof(p2a) + (3U * RATE_DESC_SIZE);
613 }
614 } else {
615 /* Fallthrough generic exit sequence below with error status */
616 }
617
618 if (status != SCMI_SUCCESS) {
619 scmi_status_response(msg, status);
620 } else {
621 /*
622 * Message payload is already written to msg->out, and
623 * msg->out_size_out updated.
624 */
625 }
626 }
627
628 static const scmi_msg_handler_t scmi_clock_handler_table[] = {
629 [SCMI_PROTOCOL_VERSION] = report_version,
630 [SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
631 [SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
632 [SCMI_CLOCK_ATTRIBUTES] = scmi_clock_attributes,
633 [SCMI_CLOCK_DESCRIBE_RATES] = scmi_clock_describe_rates,
634 [SCMI_CLOCK_RATE_SET] = scmi_clock_rate_set,
635 [SCMI_CLOCK_RATE_GET] = scmi_clock_rate_get,
636 [SCMI_CLOCK_CONFIG_SET] = scmi_clock_config_set,
637 [SCMI_CLOCK_CONFIG_GET] = scmi_clock_config_get,
638 [SCMI_CLOCK_POSSIBLE_PARENTS_GET] = scmi_clock_possible_parents_get,
639 [SCMI_CLOCK_PARENT_SET] = scmi_clock_parent_set,
640 [SCMI_CLOCK_PARENT_GET] = scmi_clock_parent_get,
641 };
642
message_id_is_supported(unsigned int message_id)643 static bool message_id_is_supported(unsigned int message_id)
644 {
645 return (message_id < ARRAY_SIZE(scmi_clock_handler_table)) &&
646 (scmi_clock_handler_table[message_id] != NULL);
647 }
648
scmi_msg_get_clock_handler(struct scmi_msg * msg)649 scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg)
650 {
651 const size_t array_size = ARRAY_SIZE(scmi_clock_handler_table);
652 unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id);
653
654 if (message_id >= array_size) {
655 VERBOSE("Clock handle not found %u", msg->message_id);
656 return NULL;
657 }
658
659 return scmi_clock_handler_table[message_id];
660 }
661