xref: /rk3399_ARM-atf/drivers/scmi-msg/clock.c (revision ba3668f1865b44635e8c7aa3a38d0d315850cec3) 
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 
32 size_t plat_scmi_clock_count(unsigned int agent_id __unused)
33 {
34 	return 0U;
35 }
36 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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