1 /*
2 * Arm SCP/MCP Software
3 * Copyright (c) 2015-2023, Arm Limited and Contributors. All rights reserved.
4 *
5 * SPDX-License-Identifier: BSD-3-Clause
6 *
7 * Description:
8 * SCMI Clock Management Protocol Support.
9 */
10
11 #include <internal/scmi_clock.h>
12
13 #include <mod_clock.h>
14 #include <mod_scmi.h>
15 #include <mod_scmi_clock.h>
16
17 #include <fwk_assert.h>
18 #include <fwk_attributes.h>
19 #include <fwk_core.h>
20 #include <fwk_event.h>
21 #include <fwk_id.h>
22 #include <fwk_log.h>
23 #include <fwk_macros.h>
24 #include <fwk_mm.h>
25 #include <fwk_module.h>
26 #include <fwk_module_idx.h>
27 #include <fwk_status.h>
28 #include <fwk_string.h>
29
30 #include <stdbool.h>
31
32 #ifdef BUILD_HAS_MOD_RESOURCE_PERMS
33 # include <mod_resource_perms.h>
34 #endif
35
36 struct clock_operations {
37 /*
38 * Service identifier currently requesting operation from this clock.
39 * A 'none' value indicates that there is no pending request.
40 */
41 fwk_id_t service_id;
42
43 /*
44 * The state to be set in this operation.
45 */
46 enum mod_clock_state state;
47
48 /*
49 * The SCMI clock index for this operation.
50 */
51 uint32_t scmi_clock_idx;
52
53 /*
54 * Request type for this operation.
55 */
56 enum scmi_clock_request_type request;
57 };
58
59 struct mod_scmi_clock_ctx {
60 /*! SCMI Clock Module Configuration */
61 const struct mod_scmi_clock_config *config;
62
63 /*! Maximum supported number of pending, asynchronous clock rate changes */
64 uint8_t max_pending_transactions;
65
66 /*!
67 * Pointer to the table of agent descriptors, used to provide per-agent
68 * views of clocks in the system.
69 */
70 const struct mod_scmi_clock_agent *agent_table;
71
72 /* SCMI module API */
73 const struct mod_scmi_from_protocol_api *scmi_api;
74
75 /* Clock module API */
76 const struct mod_clock_api *clock_api;
77
78 /* Number of clock devices */
79 int clock_devices;
80
81 /* Pointer to a table of clock operations */
82 struct clock_operations *clock_ops;
83
84 #ifdef BUILD_HAS_MOD_RESOURCE_PERMS
85 /* SCMI Resource Permissions API */
86 const struct mod_res_permissions_api *res_perms_api;
87 #endif
88 };
89
90 static const fwk_id_t mod_scmi_clock_event_id_get_state =
91 FWK_ID_EVENT_INIT(FWK_MODULE_IDX_SCMI_CLOCK,
92 SCMI_CLOCK_EVENT_IDX_GET_STATE);
93
94 static const fwk_id_t mod_scmi_clock_event_id_get_rate =
95 FWK_ID_EVENT_INIT(FWK_MODULE_IDX_SCMI_CLOCK,
96 SCMI_CLOCK_EVENT_IDX_GET_RATE);
97
98 static const fwk_id_t mod_scmi_clock_event_id_set_rate =
99 FWK_ID_EVENT_INIT(FWK_MODULE_IDX_SCMI_CLOCK,
100 SCMI_CLOCK_EVENT_IDX_SET_RATE);
101
102 static const fwk_id_t mod_scmi_clock_event_id_set_state =
103 FWK_ID_EVENT_INIT(FWK_MODULE_IDX_SCMI_CLOCK,
104 SCMI_CLOCK_EVENT_IDX_SET_STATE);
105
106 /*
107 * SCMI Clock Message Handlers
108 */
109 static int scmi_clock_protocol_version_handler(fwk_id_t service_id,
110 const uint32_t *payload);
111 static int scmi_clock_protocol_attributes_handler(fwk_id_t service_id,
112 const uint32_t *payload);
113 static int scmi_clock_protocol_message_attributes_handler(
114 fwk_id_t service_id, const uint32_t *payload);
115 static int scmi_clock_attributes_handler(fwk_id_t service_id,
116 const uint32_t *payload);
117 static int scmi_clock_rate_get_handler(fwk_id_t service_id,
118 const uint32_t *payload);
119 static int scmi_clock_rate_set_handler(fwk_id_t service_id,
120 const uint32_t *payload);
121 static int scmi_clock_config_set_handler(fwk_id_t service_id,
122 const uint32_t *payload);
123 static int scmi_clock_describe_rates_handler(fwk_id_t service_id,
124 const uint32_t *payload);
125
126 /*
127 * Internal variables.
128 */
129 static struct mod_scmi_clock_ctx scmi_clock_ctx;
130
131 static int (*const handler_table[MOD_SCMI_CLOCK_COMMAND_COUNT])(
132 fwk_id_t,
133 const uint32_t *) = {
134 [MOD_SCMI_PROTOCOL_VERSION] = scmi_clock_protocol_version_handler,
135 [MOD_SCMI_PROTOCOL_ATTRIBUTES] = scmi_clock_protocol_attributes_handler,
136 [MOD_SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] =
137 scmi_clock_protocol_message_attributes_handler,
138 [MOD_SCMI_CLOCK_ATTRIBUTES] = scmi_clock_attributes_handler,
139 [MOD_SCMI_CLOCK_RATE_GET] = scmi_clock_rate_get_handler,
140 [MOD_SCMI_CLOCK_RATE_SET] = scmi_clock_rate_set_handler,
141 [MOD_SCMI_CLOCK_CONFIG_SET] = scmi_clock_config_set_handler,
142 [MOD_SCMI_CLOCK_DESCRIBE_RATES] = scmi_clock_describe_rates_handler,
143 };
144
145 static const unsigned int payload_size_table[MOD_SCMI_CLOCK_COMMAND_COUNT] = {
146 [MOD_SCMI_PROTOCOL_VERSION] = 0,
147 [MOD_SCMI_PROTOCOL_ATTRIBUTES] = 0,
148 [MOD_SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] =
149 (unsigned int)sizeof(struct scmi_protocol_message_attributes_a2p),
150 [MOD_SCMI_CLOCK_ATTRIBUTES] =
151 (unsigned int)sizeof(struct scmi_clock_attributes_a2p),
152 [MOD_SCMI_CLOCK_RATE_GET] =
153 (unsigned int)sizeof(struct scmi_clock_rate_get_a2p),
154 [MOD_SCMI_CLOCK_RATE_SET] =
155 (unsigned int)sizeof(struct scmi_clock_rate_set_a2p),
156 [MOD_SCMI_CLOCK_CONFIG_SET] =
157 (unsigned int)sizeof(struct scmi_clock_config_set_a2p),
158 [MOD_SCMI_CLOCK_DESCRIBE_RATES] =
159 (unsigned int)sizeof(struct scmi_clock_describe_rates_a2p),
160 };
161
162 /*
163 * Given a service identifier, retrieve a pointer to its agent's
164 * \c mod_scmi_clock_agent structure within the agent table.
165 */
get_agent_entry(fwk_id_t service_id,const struct mod_scmi_clock_agent ** agent)166 static int get_agent_entry(
167 fwk_id_t service_id,
168 const struct mod_scmi_clock_agent **agent)
169 {
170 int status;
171 unsigned int agent_id;
172
173 if (agent == NULL) {
174 return FWK_E_PARAM;
175 }
176
177 status = scmi_clock_ctx.scmi_api->get_agent_id(service_id, &agent_id);
178 if (status != FWK_SUCCESS) {
179 return status;
180 }
181
182 if (agent_id >= scmi_clock_ctx.config->agent_count) {
183 return FWK_E_PARAM;
184 }
185
186 *agent =
187 (struct mod_scmi_clock_agent *)&scmi_clock_ctx.agent_table[agent_id];
188
189 return FWK_SUCCESS;
190 }
191
192 /*
193 * Given a service identifier and an SCMI clock index, retrieve a pointer to the
194 * clock's \c mod_scmi_clock_device structure within the agent's device table.
195 * Optionally, a pointer to the agent may be retrieved as well.
196 */
get_clock_device_entry(fwk_id_t service_id,unsigned int scmi_clock_idx,const struct mod_scmi_clock_device ** clock_device,const struct mod_scmi_clock_agent ** agent)197 static int get_clock_device_entry(
198 fwk_id_t service_id,
199 unsigned int scmi_clock_idx,
200 const struct mod_scmi_clock_device **clock_device,
201 const struct mod_scmi_clock_agent **agent)
202 {
203 int status;
204 const struct mod_scmi_clock_agent *agent_entry;
205
206 if (clock_device == NULL) {
207 return FWK_E_PARAM;
208 }
209
210 status = get_agent_entry(service_id, &agent_entry);
211 if (status != FWK_SUCCESS) {
212 return status;
213 }
214
215 if (scmi_clock_idx >= agent_entry->device_count) {
216 return FWK_E_RANGE;
217 }
218
219 *clock_device = &agent_entry->device_table[scmi_clock_idx];
220
221 fwk_assert(fwk_module_is_valid_element_id((*clock_device)->element_id));
222
223 if (agent != NULL) {
224 *agent = agent_entry;
225 }
226
227 return FWK_SUCCESS;
228 }
229
230 /*
231 * Given a service identifier and an SCMI clock index, retrieve the
232 * corresponding clock device index.
233 */
get_clock_device_idx(fwk_id_t service_id,unsigned int scmi_clock_idx,unsigned int * clock_idx)234 static inline int get_clock_device_idx(
235 fwk_id_t service_id,
236 unsigned int scmi_clock_idx,
237 unsigned int *clock_idx)
238 {
239 const struct mod_scmi_clock_device *clock_device;
240 int status;
241
242 status =
243 get_clock_device_entry(service_id, scmi_clock_idx, &clock_device, NULL);
244 if (status != FWK_SUCCESS) {
245 return status;
246 }
247
248 *clock_idx = fwk_id_get_element_idx(clock_device->element_id);
249 return FWK_SUCCESS;
250 }
251
252 /*
253 * Given a service identifier and an SCMI clock index, retrieve the state
254 * for that agent:clock
255 */
get_agent_clock_state(uint8_t * state,fwk_id_t service_id,unsigned int scmi_clock_idx,uint8_t * agent_clock_state)256 static int get_agent_clock_state(
257 uint8_t *state,
258 fwk_id_t service_id,
259 unsigned int scmi_clock_idx,
260 uint8_t *agent_clock_state)
261 {
262 unsigned int agent_id;
263 const struct mod_scmi_clock_agent *agent_entry;
264 int idx;
265 int status;
266
267 status = get_agent_entry(service_id, &agent_entry);
268 if (status != FWK_SUCCESS) {
269 return status;
270 }
271
272 if (scmi_clock_idx >= agent_entry->device_count) {
273 return FWK_E_RANGE;
274 }
275
276 status = scmi_clock_ctx.scmi_api->get_agent_id(service_id, &agent_id);
277 if (status != FWK_SUCCESS) {
278 return status;
279 }
280
281 idx = (int)((agent_id * scmi_clock_ctx.clock_devices) + scmi_clock_idx);
282
283 *state = agent_clock_state[idx];
284
285 return FWK_SUCCESS;
286 }
287
288 /*
289 * Given a service identifier and an SCMI clock index, set the state
290 * for that agent:clock
291 */
set_agent_clock_state(uint8_t * agent_clock_state,uint8_t state,fwk_id_t service_id,unsigned int scmi_clock_idx)292 static int set_agent_clock_state(
293 uint8_t *agent_clock_state,
294 uint8_t state,
295 fwk_id_t service_id,
296 unsigned int scmi_clock_idx)
297 {
298 unsigned int agent_id;
299 const struct mod_scmi_clock_agent *agent_entry;
300 int idx;
301 int status;
302
303 status = get_agent_entry(service_id, &agent_entry);
304 if (status != FWK_SUCCESS) {
305 return status;
306 }
307
308 if (scmi_clock_idx >= agent_entry->device_count) {
309 return FWK_E_RANGE;
310 }
311
312 status = scmi_clock_ctx.scmi_api->get_agent_id(service_id, &agent_id);
313 if (status != FWK_SUCCESS) {
314 return status;
315 }
316
317 idx = (int)((agent_id * scmi_clock_ctx.clock_devices) + scmi_clock_idx);
318
319 agent_clock_state[idx] = state;
320
321 return FWK_SUCCESS;
322 }
323
324 /*
325 * Helper for clock operations
326 */
clock_ops_set_busy(unsigned int clock_dev_idx,fwk_id_t service_id,uint32_t scmi_clock_idx,enum mod_clock_state state,enum scmi_clock_request_type request)327 static void clock_ops_set_busy(
328 unsigned int clock_dev_idx,
329 fwk_id_t service_id,
330 uint32_t scmi_clock_idx,
331 enum mod_clock_state state,
332 enum scmi_clock_request_type request)
333 {
334 scmi_clock_ctx.clock_ops[clock_dev_idx].service_id = service_id;
335 scmi_clock_ctx.clock_ops[clock_dev_idx].state = state;
336 scmi_clock_ctx.clock_ops[clock_dev_idx].scmi_clock_idx = scmi_clock_idx;
337 scmi_clock_ctx.clock_ops[clock_dev_idx].request = request;
338 }
339
clock_ops_update_state(unsigned int clock_dev_idx,int status)340 static void clock_ops_update_state(unsigned int clock_dev_idx, int status)
341 {
342 enum mod_scmi_clock_policy_status policy_status;
343 int set_policy_status;
344
345 if ((status == FWK_SUCCESS) &&
346 (scmi_clock_ctx.clock_ops[clock_dev_idx].request ==
347 SCMI_CLOCK_REQUEST_SET_STATE)) {
348 set_policy_status = mod_scmi_clock_config_set_policy(
349 &policy_status,
350 &scmi_clock_ctx.clock_ops[clock_dev_idx].state,
351 MOD_SCMI_CLOCK_POST_MESSAGE_HANDLER,
352 scmi_clock_ctx.clock_ops[clock_dev_idx].service_id,
353 scmi_clock_ctx.clock_ops[clock_dev_idx].scmi_clock_idx);
354 if (set_policy_status != FWK_SUCCESS) {
355 FWK_LOG_DEBUG("[SCMI-CLK] %s @%d", __func__, __LINE__);
356 }
357 }
358 }
359
clock_ops_set_available(unsigned int clock_dev_idx)360 static inline void clock_ops_set_available(unsigned int clock_dev_idx)
361 {
362 scmi_clock_ctx.clock_ops[clock_dev_idx].service_id = FWK_ID_NONE;
363 }
364
clock_ops_get_service(unsigned int clock_dev_idx)365 static inline fwk_id_t clock_ops_get_service(unsigned int clock_dev_idx)
366 {
367 return scmi_clock_ctx.clock_ops[clock_dev_idx].service_id;
368 }
369
clock_ops_is_available(unsigned int clock_dev_idx)370 static inline bool clock_ops_is_available(unsigned int clock_dev_idx)
371 {
372 return fwk_id_is_equal(scmi_clock_ctx.clock_ops[clock_dev_idx].service_id,
373 FWK_ID_NONE);
374 }
375
376 /*
377 * Helper for the 'get_state' response
378 */
get_state_respond(fwk_id_t clock_dev_id,fwk_id_t service_id,enum mod_clock_state * clock_state,int status)379 static void get_state_respond(fwk_id_t clock_dev_id,
380 fwk_id_t service_id,
381 enum mod_clock_state *clock_state,
382 int status)
383 {
384 int respond_status;
385 size_t response_size;
386 struct scmi_clock_attributes_p2a return_values = { 0 };
387
388 if (status == FWK_SUCCESS) {
389 return_values.attributes = SCMI_CLOCK_ATTRIBUTES(
390 (uint32_t)(*clock_state == MOD_CLOCK_STATE_RUNNING));
391
392 fwk_str_strncpy(
393 return_values.clock_name,
394 fwk_module_get_element_name(clock_dev_id),
395 sizeof(return_values.clock_name) - 1);
396
397 return_values.status = (int32_t)SCMI_SUCCESS;
398 response_size = sizeof(return_values);
399 } else {
400 return_values.status = (int32_t)SCMI_GENERIC_ERROR;
401
402 response_size = sizeof(return_values.status);
403 }
404
405 respond_status = scmi_clock_ctx.scmi_api->respond(
406 service_id, &return_values, response_size);
407 if (respond_status != FWK_SUCCESS) {
408 FWK_LOG_DEBUG("[SCMI-CLK] %s @%d", __func__, __LINE__);
409 }
410 }
411
412 /*
413 * Helper for the 'get_rate' response
414 */
get_rate_respond(fwk_id_t service_id,uint64_t * rate,int status)415 static void get_rate_respond(fwk_id_t service_id,
416 uint64_t *rate,
417 int status)
418 {
419 int respond_status;
420 size_t response_size;
421 struct scmi_clock_rate_get_p2a return_values = { 0 };
422
423 if (status == FWK_SUCCESS) {
424 return_values.rate[0] = (uint32_t)*rate;
425 return_values.rate[1] = (uint32_t)(*rate >> 32);
426
427 return_values.status = (int32_t)SCMI_SUCCESS;
428 response_size = sizeof(return_values);
429 } else {
430 return_values.status = (int32_t)SCMI_GENERIC_ERROR;
431
432 response_size = sizeof(return_values.status);
433 }
434
435 respond_status = scmi_clock_ctx.scmi_api->respond(
436 service_id, &return_values, response_size);
437 if (respond_status != FWK_SUCCESS) {
438 FWK_LOG_DEBUG("[SCMI-CLK] %s @%d", __func__, __LINE__);
439 }
440 }
441
request_response(int response_status,fwk_id_t service_id)442 static void request_response(int response_status,
443 fwk_id_t service_id)
444 {
445 int respond_status;
446 struct scmi_clock_generic_p2a return_values = {
447 .status = (int32_t)SCMI_GENERIC_ERROR
448 };
449
450 if (response_status == FWK_E_SUPPORT) {
451 return_values.status = (int32_t)SCMI_NOT_SUPPORTED;
452 }
453
454 if (response_status == FWK_E_RANGE) {
455 return_values.status = (int32_t)SCMI_INVALID_PARAMETERS;
456 }
457
458 respond_status = scmi_clock_ctx.scmi_api->respond(
459 service_id, &return_values, sizeof(return_values.status));
460 if (respond_status != FWK_SUCCESS) {
461 FWK_LOG_DEBUG("[SCMI-CLK] %s @%d", __func__, __LINE__);
462 }
463 }
464
465 /*
466 * Helper for the 'set_' responses
467 */
set_request_respond(fwk_id_t service_id,int status)468 static void set_request_respond(fwk_id_t service_id, int status)
469 {
470 int respond_status;
471 struct scmi_clock_generic_p2a return_values = { 0 };
472
473 if (status == FWK_E_RANGE || status == FWK_E_PARAM) {
474 return_values.status = (int32_t)SCMI_INVALID_PARAMETERS;
475 } else if (status == FWK_E_SUPPORT) {
476 return_values.status = (int32_t)SCMI_NOT_SUPPORTED;
477 } else if (status != FWK_SUCCESS) {
478 return_values.status = (int32_t)SCMI_GENERIC_ERROR;
479 } else {
480 return_values.status = (int32_t)SCMI_SUCCESS;
481 }
482
483 respond_status = scmi_clock_ctx.scmi_api->respond(
484 service_id, &return_values, sizeof(return_values.status));
485 if (respond_status != FWK_SUCCESS) {
486 FWK_LOG_DEBUG("[SCMI-CLK] %s @%d", __func__, __LINE__);
487 }
488 }
489
mod_scmi_clock_rate_set_policy(enum mod_scmi_clock_policy_status * policy_status,enum mod_clock_round_mode * round_mode,uint64_t * rate,enum mod_scmi_clock_policy_commit policy_commit,fwk_id_t service_id,uint32_t scmi_clock_idx)490 FWK_WEAK int mod_scmi_clock_rate_set_policy(
491 enum mod_scmi_clock_policy_status *policy_status,
492 enum mod_clock_round_mode *round_mode,
493 uint64_t *rate,
494 enum mod_scmi_clock_policy_commit policy_commit,
495 fwk_id_t service_id,
496 uint32_t scmi_clock_idx)
497 {
498 *policy_status = MOD_SCMI_CLOCK_EXECUTE_MESSAGE_HANDLER;
499
500 return FWK_SUCCESS;
501 }
502
mod_scmi_clock_config_set_policy(enum mod_scmi_clock_policy_status * policy_status,enum mod_clock_state * state,enum mod_scmi_clock_policy_commit policy_commit,fwk_id_t service_id,uint32_t scmi_clock_idx)503 FWK_WEAK int mod_scmi_clock_config_set_policy(
504 enum mod_scmi_clock_policy_status *policy_status,
505 enum mod_clock_state *state,
506 enum mod_scmi_clock_policy_commit policy_commit,
507 fwk_id_t service_id,
508 uint32_t scmi_clock_idx)
509 {
510 /* Pointer to a table of clock reference counts */
511 static uint8_t *clock_count = NULL;
512 /* Pointer to a table of agent:clock states */
513 static uint8_t *agent_clock_state = NULL;
514
515 const struct mod_scmi_clock_agent *agent;
516
517 unsigned int agent_id, ref_count_table_idx;
518 unsigned int clock_idx = 0;
519 int status = FWK_SUCCESS;
520 uint8_t clock_state;
521
522 *policy_status = MOD_SCMI_CLOCK_SKIP_MESSAGE_HANDLER;
523
524 if (agent_clock_state == NULL) {
525 /* Allocate table of agent:clock states */
526 agent_clock_state = fwk_mm_calloc(
527 (unsigned int)scmi_clock_ctx.config->agent_count *
528 (unsigned int)scmi_clock_ctx.clock_devices,
529 sizeof(uint8_t));
530
531 /* Allocate table of clock reference counts */
532 clock_count = fwk_mm_calloc(
533 (unsigned int)scmi_clock_ctx.clock_devices, sizeof(uint32_t));
534
535 /* Set all clocks as running if required */
536 for (agent_id = 0;
537 agent_id < (unsigned int)scmi_clock_ctx.config->agent_count;
538 agent_id++) {
539 agent = &scmi_clock_ctx.agent_table[agent_id];
540 for (clock_idx = 0; clock_idx < (unsigned int)agent->device_count;
541 clock_idx++) {
542 if (agent->device_table[clock_idx].starts_enabled) {
543 ref_count_table_idx =
544 (agent_id * scmi_clock_ctx.clock_devices) + clock_idx;
545 agent_clock_state[ref_count_table_idx] =
546 (uint8_t)MOD_CLOCK_STATE_RUNNING;
547
548 ref_count_table_idx = fwk_id_get_element_idx(
549 agent->device_table[clock_idx].element_id);
550 clock_count[ref_count_table_idx]++;
551 }
552 }
553 }
554 }
555
556 /*
557 * clock state is the last state this agent successfully set
558 * for this clock.
559 */
560 status = get_agent_clock_state(
561 &clock_state, service_id, scmi_clock_idx, agent_clock_state);
562 if (status != FWK_SUCCESS) {
563 return status;
564 }
565
566 status = get_clock_device_idx(service_id, scmi_clock_idx, &clock_idx);
567 if (status != FWK_SUCCESS) {
568 return status;
569 }
570
571 switch (*state) {
572 case MOD_CLOCK_STATE_RUNNING:
573 /* The agent has already requested to set state to RUNNING */
574 if (clock_state == MOD_CLOCK_STATE_RUNNING) {
575 return FWK_SUCCESS;
576 }
577
578 /* set the clock state for this agent to RUNNING */
579 if (policy_commit == MOD_SCMI_CLOCK_POST_MESSAGE_HANDLER) {
580 status = set_agent_clock_state(
581 agent_clock_state,
582 (uint8_t)MOD_CLOCK_STATE_RUNNING,
583 service_id,
584 scmi_clock_idx);
585 if (status != FWK_SUCCESS) {
586 return status;
587 }
588 }
589
590 /*
591 * Only allow the clock to be stopped if clock_count == 0
592 * before being updated for this call.
593 *
594 * This is the first agent to set the clock RUNNING.
595 */
596 if (clock_count[clock_idx] != 0) {
597 status = FWK_E_STATE;
598 }
599 if (policy_commit == MOD_SCMI_CLOCK_POST_MESSAGE_HANDLER) {
600 clock_count[clock_idx]++;
601 }
602 break;
603
604 case MOD_CLOCK_STATE_STOPPED:
605 /* The agent has already requested to set state to STOPPED */
606 if (clock_state == MOD_CLOCK_STATE_STOPPED) {
607 return FWK_SUCCESS;
608 }
609
610 /* error to try and stop a stopped clock */
611 if (clock_count[clock_idx] == 0) {
612 /*
613 * Here we ignore whether the agent_id will be correctly returned as
614 * we will return an error regardless
615 */
616 (void)scmi_clock_ctx.scmi_api->get_agent_id(service_id, &agent_id);
617
618 FWK_LOG_WARN(
619 "[SCMI-CLK] Invalid STOP request agent:"
620 " %d scmi_clock_id: %d state:%d\n",
621 (int)agent_id,
622 (int)scmi_clock_idx,
623 *state);
624 return FWK_E_STATE;
625 }
626
627 /* set the clock state for this agent to STOPPED */
628 if (policy_commit == MOD_SCMI_CLOCK_POST_MESSAGE_HANDLER) {
629 status = set_agent_clock_state(
630 agent_clock_state,
631 (uint8_t)MOD_CLOCK_STATE_STOPPED,
632 service_id,
633 scmi_clock_idx);
634 if (status != FWK_SUCCESS) {
635 return status;
636 }
637 }
638
639 /*
640 * Only allow the clock to be stopped if ref_clk_count == 0
641 * after being updated for this call.
642 *
643 * This is the last agent to set the clock STOPPED.
644 */
645 if (clock_count[clock_idx] != 1) {
646 status = FWK_E_STATE;
647 }
648 if (policy_commit == MOD_SCMI_CLOCK_POST_MESSAGE_HANDLER) {
649 clock_count[clock_idx]--;
650 }
651 break;
652
653 default:
654 return FWK_E_PARAM;
655 }
656
657 if (status == FWK_SUCCESS) {
658 *policy_status = MOD_SCMI_CLOCK_EXECUTE_MESSAGE_HANDLER;
659 }
660
661 return FWK_SUCCESS;
662 }
663
664 #ifdef BUILD_HAS_MOD_RESOURCE_PERMS
665
scmi_clock_permissions_handler(uint32_t scmi_clock_idx,fwk_id_t service_id,unsigned int message_id)666 static int scmi_clock_permissions_handler(
667 uint32_t scmi_clock_idx,
668 fwk_id_t service_id,
669 unsigned int message_id)
670 {
671 enum mod_res_perms_permissions perms;
672 unsigned int agent_id;
673 int status;
674
675 status = scmi_clock_ctx.scmi_api->get_agent_id(service_id, &agent_id);
676 if (status != FWK_SUCCESS) {
677 return FWK_E_ACCESS;
678 }
679
680 if (message_id < 3) {
681 perms = scmi_clock_ctx.res_perms_api->agent_has_protocol_permission(
682 agent_id, MOD_SCMI_PROTOCOL_ID_CLOCK);
683 if (perms == MOD_RES_PERMS_ACCESS_ALLOWED) {
684 return FWK_SUCCESS;
685 }
686 return FWK_E_ACCESS;
687 }
688
689 perms = scmi_clock_ctx.res_perms_api->agent_has_resource_permission(
690 agent_id, MOD_SCMI_PROTOCOL_ID_CLOCK, message_id, scmi_clock_idx);
691
692 if (perms == MOD_RES_PERMS_ACCESS_ALLOWED) {
693 return FWK_SUCCESS;
694 } else {
695 return FWK_E_ACCESS;
696 }
697 }
698
699 #endif
700
701 /*
702 * Helper to create events for processing pending requests
703 */
create_event_request(fwk_id_t clock_id,fwk_id_t service_id,enum scmi_clock_request_type request,void * data,uint32_t scmi_clock_idx)704 static int create_event_request(
705 fwk_id_t clock_id,
706 fwk_id_t service_id,
707 enum scmi_clock_request_type request,
708 void *data,
709 uint32_t scmi_clock_idx)
710 {
711 int status;
712 union event_request_data request_data;
713 unsigned int clock_dev_idx = fwk_id_get_element_idx(clock_id);
714 struct scmi_clock_event_request_params *params;
715 enum mod_clock_state state = MOD_CLOCK_STATE_COUNT;
716
717 if (!clock_ops_is_available(clock_dev_idx)) {
718 return FWK_E_BUSY;
719 }
720
721 struct fwk_event event = {
722 .target_id = fwk_module_id_scmi_clock,
723 };
724
725 params = (struct scmi_clock_event_request_params *)event.params;
726
727 switch (request) {
728 case SCMI_CLOCK_REQUEST_GET_STATE:
729 event.id = mod_scmi_clock_event_id_get_state;
730 break;
731
732 case SCMI_CLOCK_REQUEST_GET_RATE:
733 event.id = mod_scmi_clock_event_id_get_rate;
734 break;
735
736 case SCMI_CLOCK_REQUEST_SET_RATE:
737 {
738 struct event_set_rate_request_data *rate_data =
739 (struct event_set_rate_request_data *)data;
740
741 request_data.set_rate_data.rate[0] = rate_data->rate[0];
742 request_data.set_rate_data.rate[1] = rate_data->rate[1];
743 request_data.set_rate_data.round_mode = rate_data->round_mode;
744
745 params->request_data = request_data;
746 }
747
748 event.id = mod_scmi_clock_event_id_set_rate;
749 break;
750
751 case SCMI_CLOCK_REQUEST_SET_STATE:
752 {
753 struct event_set_state_request_data *state_data =
754 (struct event_set_state_request_data *)data;
755 request_data.set_state_data.state = state_data->state;
756 state = state_data->state;
757
758 params->request_data = request_data;
759 }
760
761 event.id = mod_scmi_clock_event_id_set_state;
762 break;
763
764 default:
765 return FWK_E_PARAM;
766 }
767
768 params->clock_dev_id = clock_id;
769
770 status = fwk_put_event(&event);
771 if (status != FWK_SUCCESS) {
772 return status;
773 }
774
775 clock_ops_set_busy(
776 clock_dev_idx, service_id, scmi_clock_idx, state, request);
777
778 return FWK_SUCCESS;
779 }
780
781 /*
782 * Protocol Version
783 */
scmi_clock_protocol_version_handler(fwk_id_t service_id,const uint32_t * payload)784 static int scmi_clock_protocol_version_handler(fwk_id_t service_id,
785 const uint32_t *payload)
786 {
787 struct scmi_protocol_version_p2a return_values = {
788 .status = (int32_t)SCMI_SUCCESS,
789 .version = SCMI_PROTOCOL_VERSION_CLOCK,
790 };
791
792 return scmi_clock_ctx.scmi_api->respond(
793 service_id, &return_values, sizeof(return_values));
794 }
795
796 /*
797 * Protocol Attributes
798 */
scmi_clock_protocol_attributes_handler(fwk_id_t service_id,const uint32_t * payload)799 static int scmi_clock_protocol_attributes_handler(fwk_id_t service_id,
800 const uint32_t *payload)
801 {
802 int status, respond_status;
803 const struct mod_scmi_clock_agent *agent;
804 struct scmi_protocol_attributes_p2a return_values = {
805 .status = (int32_t)SCMI_SUCCESS,
806 };
807
808 status = get_agent_entry(service_id, &agent);
809 if (status != FWK_SUCCESS) {
810 return_values.status = (int32_t)SCMI_GENERIC_ERROR;
811 goto exit;
812 }
813
814 return_values.attributes = SCMI_CLOCK_PROTOCOL_ATTRIBUTES(
815 scmi_clock_ctx.max_pending_transactions,
816 agent->device_count
817 );
818
819 exit:
820 respond_status = scmi_clock_ctx.scmi_api->respond(
821 service_id, &return_values, sizeof(return_values));
822 if (respond_status != FWK_SUCCESS) {
823 FWK_LOG_DEBUG("[SCMI-CLK] %s @%d", __func__, __LINE__);
824 }
825 return status;
826 }
827
828 /*
829 * Protocol Message Attributes
830 */
scmi_clock_protocol_message_attributes_handler(fwk_id_t service_id,const uint32_t * payload)831 static int scmi_clock_protocol_message_attributes_handler(fwk_id_t service_id,
832 const uint32_t *payload)
833 {
834 size_t response_size;
835 const struct scmi_protocol_message_attributes_a2p *parameters;
836 unsigned int message_id;
837 struct scmi_protocol_message_attributes_p2a return_values = {
838 .status = (int32_t)SCMI_SUCCESS,
839 .attributes = 0,
840 };
841
842 parameters = (const struct scmi_protocol_message_attributes_a2p*)
843 payload;
844 message_id = parameters->message_id;
845
846 if ((message_id >= FWK_ARRAY_SIZE(handler_table)) ||
847 (handler_table[message_id] == NULL)) {
848 return_values.status = (int32_t)SCMI_NOT_FOUND;
849 goto exit;
850 }
851
852 exit:
853 response_size = (return_values.status == SCMI_SUCCESS) ?
854 sizeof(return_values) : sizeof(return_values.status);
855 return scmi_clock_ctx.scmi_api->respond(
856 service_id, &return_values, response_size);
857 }
858
859 /*
860 * Clock Attributes
861 */
862
scmi_clock_attributes_handler(fwk_id_t service_id,const uint32_t * payload)863 static int scmi_clock_attributes_handler(fwk_id_t service_id,
864 const uint32_t *payload)
865 {
866 int status, respond_status;
867 const struct mod_scmi_clock_agent *agent;
868 const struct mod_scmi_clock_device *clock_device;
869 size_t response_size;
870 const struct scmi_clock_attributes_a2p *parameters;
871 struct scmi_clock_attributes_p2a return_values = {
872 .status = (int32_t)SCMI_GENERIC_ERROR
873 };
874
875 parameters = (const struct scmi_clock_attributes_a2p*)payload;
876
877 status = get_clock_device_entry(service_id,
878 parameters->clock_id,
879 &clock_device,
880 &agent);
881 if (status != FWK_SUCCESS) {
882 return_values.status = (int32_t)SCMI_NOT_FOUND;
883 goto exit;
884 }
885
886 #ifdef BUILD_HAS_MOD_RESOURCE_PERMS
887 status = scmi_clock_permissions_handler(
888 parameters->clock_id,
889 service_id,
890 (unsigned int)MOD_SCMI_CLOCK_ATTRIBUTES);
891 if (status != FWK_SUCCESS) {
892 return_values.status = (int32_t)SCMI_DENIED;
893 goto exit;
894 }
895 #endif
896
897 status = create_event_request(
898 clock_device->element_id,
899 service_id,
900 SCMI_CLOCK_REQUEST_GET_STATE,
901 NULL,
902 parameters->clock_id);
903 if (status == FWK_E_BUSY) {
904 return_values.status = (int32_t)SCMI_BUSY;
905 status = FWK_SUCCESS;
906 goto exit;
907 }
908
909 if (status != FWK_SUCCESS) {
910 goto exit;
911 }
912
913 return FWK_SUCCESS;
914
915 exit:
916 response_size = (return_values.status == SCMI_SUCCESS) ?
917 sizeof(return_values) : sizeof(return_values.status);
918 respond_status = scmi_clock_ctx.scmi_api->respond(
919 service_id, &return_values, response_size);
920 if (respond_status != FWK_SUCCESS) {
921 FWK_LOG_DEBUG("[SCMI-CLK] %s @%d", __func__, __LINE__);
922 }
923
924 return status;
925 }
926
927 /*
928 * Clock Rate Get
929 */
scmi_clock_rate_get_handler(fwk_id_t service_id,const uint32_t * payload)930 static int scmi_clock_rate_get_handler(fwk_id_t service_id,
931 const uint32_t *payload)
932 {
933 int status, respond_status;
934 const struct mod_scmi_clock_agent *agent;
935 const struct mod_scmi_clock_device *clock_device;
936 size_t response_size;
937 const struct scmi_clock_rate_get_a2p *parameters;
938 struct scmi_clock_rate_get_p2a return_values = {
939 .status = (int32_t)SCMI_GENERIC_ERROR
940 };
941
942 parameters = (const struct scmi_clock_rate_get_a2p*)payload;
943
944 status = get_clock_device_entry(service_id,
945 parameters->clock_id,
946 &clock_device,
947 &agent);
948 if (status != FWK_SUCCESS) {
949 return_values.status = (int32_t)SCMI_NOT_FOUND;
950 goto exit;
951 }
952
953 #ifdef BUILD_HAS_MOD_RESOURCE_PERMS
954 status = scmi_clock_permissions_handler(
955 parameters->clock_id,
956 service_id,
957 (unsigned int)MOD_SCMI_CLOCK_RATE_GET);
958 if (status != FWK_SUCCESS) {
959 return_values.status = (int32_t)SCMI_DENIED;
960 goto exit;
961 }
962 #endif
963
964 status = create_event_request(
965 clock_device->element_id,
966 service_id,
967 SCMI_CLOCK_REQUEST_GET_RATE,
968 NULL,
969 parameters->clock_id);
970 if (status == FWK_E_BUSY) {
971 return_values.status = (int32_t)SCMI_BUSY;
972 status = FWK_SUCCESS;
973 goto exit;
974 }
975
976 if (status != FWK_SUCCESS) {
977 goto exit;
978 }
979
980 return FWK_SUCCESS;
981
982 exit:
983 response_size = (return_values.status == SCMI_SUCCESS) ?
984 sizeof(return_values) : sizeof(return_values.status);
985 respond_status = scmi_clock_ctx.scmi_api->respond(
986 service_id, &return_values, response_size);
987 if (respond_status != FWK_SUCCESS) {
988 FWK_LOG_DEBUG("[SCMI-CLK] %s @%d", __func__, __LINE__);
989 }
990
991 return status;
992 }
993
994 /*
995 * Clock Rate Set (Synchronous Only)
996 */
scmi_clock_rate_set_handler(fwk_id_t service_id,const uint32_t * payload)997 static int scmi_clock_rate_set_handler(fwk_id_t service_id,
998 const uint32_t *payload)
999 {
1000 int status;
1001 const struct mod_scmi_clock_agent *agent;
1002 const struct mod_scmi_clock_device *clock_device;
1003 bool round_auto;
1004 bool round_up;
1005 bool asynchronous;
1006 size_t response_size;
1007 unsigned int agent_id;
1008 uint64_t rate;
1009 enum mod_clock_round_mode round_mode;
1010 const struct scmi_clock_rate_set_a2p *parameters;
1011 struct scmi_clock_rate_set_p2a return_values = {
1012 .status = (int32_t)SCMI_GENERIC_ERROR
1013 };
1014 enum mod_scmi_clock_policy_status policy_status;
1015
1016 parameters = (const struct scmi_clock_rate_set_a2p*)payload;
1017 round_up = (parameters->flags & SCMI_CLOCK_RATE_SET_ROUND_UP_MASK) != 0;
1018 round_auto = (parameters->flags & SCMI_CLOCK_RATE_SET_ROUND_AUTO_MASK) != 0;
1019 asynchronous = (parameters->flags & SCMI_CLOCK_RATE_SET_ASYNC_MASK) != 0;
1020
1021 if ((parameters->flags & ~SCMI_CLOCK_RATE_SET_FLAGS_MASK) != 0) {
1022 return_values.status = (int32_t)SCMI_INVALID_PARAMETERS;
1023 goto exit;
1024 }
1025
1026 status = get_clock_device_entry(service_id,
1027 parameters->clock_id,
1028 &clock_device,
1029 &agent);
1030 if (status != FWK_SUCCESS) {
1031 return_values.status = (int32_t)SCMI_NOT_FOUND;
1032 goto exit;
1033 }
1034
1035 #ifdef BUILD_HAS_MOD_RESOURCE_PERMS
1036 status = scmi_clock_permissions_handler(
1037 parameters->clock_id,
1038 service_id,
1039 (unsigned int)MOD_SCMI_CLOCK_RATE_SET);
1040 if (status != FWK_SUCCESS) {
1041 return_values.status = (int32_t)SCMI_DENIED;
1042 goto exit;
1043 }
1044 #endif
1045
1046 if (asynchronous) {
1047 /* Support for async clock set commands not yet implemented */
1048 return_values.status = (int32_t)SCMI_NOT_SUPPORTED;
1049 goto exit;
1050 }
1051
1052 status = scmi_clock_ctx.scmi_api->get_agent_id(service_id, &agent_id);
1053 if (status != FWK_SUCCESS) {
1054 goto exit;
1055 }
1056
1057 rate = ((uint64_t)(parameters->rate[1]) << 32) |
1058 (uint64_t)parameters->rate[0];
1059 round_mode = round_auto ?
1060 MOD_CLOCK_ROUND_MODE_NEAREST :
1061 (round_up ? MOD_CLOCK_ROUND_MODE_UP : MOD_CLOCK_ROUND_MODE_DOWN);
1062
1063 /*
1064 * Note that rate and round_mode may be modified by the policy handler.
1065 */
1066 status = mod_scmi_clock_rate_set_policy(
1067 &policy_status,
1068 &round_mode,
1069 &rate,
1070 MOD_SCMI_CLOCK_PRE_MESSAGE_HANDLER,
1071 service_id,
1072 parameters->clock_id);
1073
1074 if (status != FWK_SUCCESS) {
1075 return_values.status = (int32_t)SCMI_GENERIC_ERROR;
1076 goto exit;
1077 }
1078 if (policy_status == MOD_SCMI_CLOCK_SKIP_MESSAGE_HANDLER) {
1079 return_values.status = (int32_t)SCMI_SUCCESS;
1080 goto exit;
1081 }
1082
1083 struct event_set_rate_request_data data = {
1084 .rate = {
1085 [0] = (uint32_t)rate,
1086 [1] = (uint32_t)(rate >> 32),
1087 },
1088 .round_mode = round_mode,
1089 };
1090
1091 status = create_event_request(
1092 clock_device->element_id,
1093 service_id,
1094 SCMI_CLOCK_REQUEST_SET_RATE,
1095 &data,
1096 parameters->clock_id);
1097 if (status == FWK_E_BUSY) {
1098 return_values.status = (int32_t)SCMI_BUSY;
1099 status = FWK_SUCCESS;
1100 goto exit;
1101 }
1102
1103 if (status != FWK_SUCCESS) {
1104 goto exit;
1105 }
1106
1107 return FWK_SUCCESS;
1108
1109 exit:
1110 response_size = (return_values.status == SCMI_SUCCESS) ?
1111 sizeof(return_values) : sizeof(return_values.status);
1112 return scmi_clock_ctx.scmi_api->respond(
1113 service_id, &return_values, response_size);
1114 }
1115
1116 /*
1117 * Clock Config Set
1118 */
scmi_clock_config_set_handler(fwk_id_t service_id,const uint32_t * payload)1119 static int scmi_clock_config_set_handler(fwk_id_t service_id,
1120 const uint32_t *payload)
1121 {
1122 int status;
1123 bool enable;
1124 size_t response_size;
1125 const struct scmi_clock_config_set_a2p *parameters;
1126 const struct mod_scmi_clock_agent *agent;
1127 const struct mod_scmi_clock_device *clock_device;
1128 unsigned int agent_id;
1129 struct scmi_clock_rate_set_p2a return_values = {
1130 .status = (int32_t)SCMI_GENERIC_ERROR
1131 };
1132 enum mod_scmi_clock_policy_status policy_status;
1133
1134 parameters = (const struct scmi_clock_config_set_a2p*)payload;
1135 enable = (parameters->attributes & SCMI_CLOCK_CONFIG_SET_ENABLE_MASK) != 0;
1136
1137 status = get_clock_device_entry(service_id,
1138 parameters->clock_id,
1139 &clock_device,
1140 &agent);
1141 if (status != FWK_SUCCESS) {
1142 return_values.status = (int32_t)SCMI_NOT_FOUND;
1143 goto exit;
1144 }
1145
1146 if ((parameters->attributes & ~SCMI_CLOCK_CONFIG_SET_ENABLE_MASK) != 0) {
1147 return_values.status = (int32_t)SCMI_INVALID_PARAMETERS;
1148 goto exit;
1149 }
1150
1151 #ifdef BUILD_HAS_MOD_RESOURCE_PERMS
1152 status = scmi_clock_permissions_handler(
1153 parameters->clock_id,
1154 service_id,
1155 (unsigned int)MOD_SCMI_CLOCK_CONFIG_SET);
1156 if (status != FWK_SUCCESS) {
1157 return_values.status = (int32_t)SCMI_DENIED;
1158 goto exit;
1159 }
1160 #endif
1161
1162 struct event_set_state_request_data data = {
1163 .state = enable ? MOD_CLOCK_STATE_RUNNING : MOD_CLOCK_STATE_STOPPED
1164 };
1165
1166 status = scmi_clock_ctx.scmi_api->get_agent_id(service_id, &agent_id);
1167 if (status != FWK_SUCCESS) {
1168 goto exit;
1169 }
1170
1171 /*
1172 * Note that data.state may be modified by the policy handler.
1173 */
1174 status = mod_scmi_clock_config_set_policy(
1175 &policy_status,
1176 &data.state,
1177 MOD_SCMI_CLOCK_PRE_MESSAGE_HANDLER,
1178 service_id,
1179 parameters->clock_id);
1180 if (status != FWK_SUCCESS) {
1181 return_values.status = (int32_t)SCMI_GENERIC_ERROR;
1182 goto exit;
1183 }
1184 if (policy_status == MOD_SCMI_CLOCK_SKIP_MESSAGE_HANDLER) {
1185 return_values.status = (int32_t)SCMI_SUCCESS;
1186 goto exit;
1187 }
1188
1189 status = create_event_request(
1190 clock_device->element_id,
1191 service_id,
1192 SCMI_CLOCK_REQUEST_SET_STATE,
1193 &data,
1194 parameters->clock_id);
1195 if (status == FWK_E_BUSY) {
1196 return_values.status = (int32_t)SCMI_BUSY;
1197 status = FWK_SUCCESS;
1198 goto exit;
1199 }
1200
1201 if (status != FWK_SUCCESS) {
1202 goto exit;
1203 }
1204
1205 return FWK_SUCCESS;
1206
1207 exit:
1208 response_size = (return_values.status == SCMI_SUCCESS) ?
1209 sizeof(return_values) : sizeof(return_values.status);
1210 return scmi_clock_ctx.scmi_api->respond(
1211 service_id, &return_values, response_size);
1212 }
1213
1214 /*
1215 * Clock Describe Rates
1216 */
scmi_clock_describe_rates_handler(fwk_id_t service_id,const uint32_t * payload)1217 static int scmi_clock_describe_rates_handler(fwk_id_t service_id,
1218 const uint32_t *payload)
1219 {
1220 int status, respond_status;
1221 const struct mod_scmi_clock_agent *agent;
1222 const struct mod_scmi_clock_device *clock_device;
1223 unsigned int i;
1224 size_t max_payload_size;
1225 uint32_t payload_size;
1226 uint32_t index;
1227 unsigned int rate_count;
1228 unsigned int remaining_rates;
1229 uint64_t rate;
1230 struct scmi_clock_rate scmi_rate;
1231 struct scmi_clock_rate clock_range[3];
1232 struct mod_clock_info info;
1233 const struct scmi_clock_describe_rates_a2p *parameters;
1234 struct scmi_clock_describe_rates_p2a return_values = {
1235 .status = (int32_t)SCMI_GENERIC_ERROR
1236 };
1237
1238 parameters = (const struct scmi_clock_describe_rates_a2p*)payload;
1239 index = parameters->rate_index;
1240 payload_size = (uint32_t)sizeof(return_values);
1241
1242 status = get_clock_device_entry(service_id,
1243 parameters->clock_id,
1244 &clock_device,
1245 &agent);
1246 if (status != FWK_SUCCESS) {
1247 return_values.status = (int32_t)SCMI_NOT_FOUND;
1248 goto exit;
1249 }
1250
1251 #ifdef BUILD_HAS_MOD_RESOURCE_PERMS
1252 status = scmi_clock_permissions_handler(
1253 parameters->clock_id,
1254 service_id,
1255 (unsigned int)MOD_SCMI_CLOCK_DESCRIBE_RATES);
1256 if (status != FWK_SUCCESS) {
1257 return_values.status = (int32_t)SCMI_DENIED;
1258 goto exit;
1259 }
1260 #endif
1261
1262 /*
1263 * Get the maximum payload size to determine how many clock rate entries can
1264 * be returned in one response.
1265 */
1266 status = scmi_clock_ctx.scmi_api->get_max_payload_size(
1267 service_id, &max_payload_size);
1268 if (status != FWK_SUCCESS) {
1269 goto exit;
1270 }
1271
1272 status = scmi_clock_ctx.clock_api->get_info(clock_device->element_id,
1273 &info);
1274 if (status != FWK_SUCCESS) {
1275 goto exit;
1276 }
1277
1278 if (info.range.rate_type == MOD_CLOCK_RATE_TYPE_DISCRETE) {
1279 /* The clock has a discrete list of frequencies */
1280
1281 if (index >= info.range.rate_count) {
1282 return_values.status = (int32_t)SCMI_OUT_OF_RANGE;
1283 goto exit;
1284 }
1285
1286 /* Can at least one entry be returned? */
1287 if (SCMI_CLOCK_RATES_MAX(max_payload_size) == 0) {
1288 status = FWK_E_SIZE;
1289 goto exit;
1290 }
1291
1292 /* The number of rates being returned in this payload is defined as the
1293 * smaller of:
1294 * - The clock rates that are available between the index and the
1295 clock's maximum rate.
1296 - The number of rates that can be returned in each payload.
1297 */
1298 rate_count = (unsigned int)FWK_MIN(
1299 SCMI_CLOCK_RATES_MAX(max_payload_size),
1300 info.range.rate_count - index);
1301
1302 /*
1303 * Because the agent gives a starting index into the clock's rate list
1304 * the number of rates remaining is calculated as the number of rates
1305 * the clock supports minus the index, with the number of rates being
1306 * returned in this payload subtracted.
1307 */
1308 remaining_rates =
1309 (unsigned int)(info.range.rate_count - index) - rate_count;
1310
1311 /* Give the number of rates sent in the message payload */
1312 return_values.num_rates_flags =
1313 SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS(
1314 rate_count,
1315 SCMI_CLOCK_RATE_FORMAT_LIST,
1316 remaining_rates
1317 );
1318
1319 /* Set each rate entry in the payload to the associated frequency */
1320 for (i = 0; i < rate_count;
1321 i++, payload_size += (uint32_t)sizeof(struct scmi_clock_rate)) {
1322 status = scmi_clock_ctx.clock_api->get_rate_from_index(
1323 clock_device->element_id,
1324 index + i,
1325 &rate);
1326 if (status != FWK_SUCCESS) {
1327 goto exit;
1328 }
1329
1330 scmi_rate.low = (uint32_t)rate;
1331 scmi_rate.high = (uint32_t)(rate >> 32);
1332
1333 status = scmi_clock_ctx.scmi_api->write_payload(service_id,
1334 payload_size, &scmi_rate, sizeof(scmi_rate));
1335 if (status != FWK_SUCCESS) {
1336 goto exit;
1337 }
1338 }
1339 } else {
1340 /* The clock has a linear stepping */
1341
1342 /* Is the payload area large enough to return the complete triplet? */
1343 if (SCMI_CLOCK_RATES_MAX(max_payload_size) < 3) {
1344 status = FWK_E_SIZE;
1345 goto exit;
1346 }
1347
1348 return_values.num_rates_flags =
1349 SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS(
1350 SCMI_CLOCK_NUM_OF_RATES_RANGE,
1351 SCMI_CLOCK_RATE_FORMAT_RANGE,
1352 /* No further rates are available */
1353 0U);
1354
1355 /* Store the range data in the range entry in the payload */
1356 clock_range[0].low = (uint32_t)info.range.min;
1357 clock_range[0].high = (uint32_t)(info.range.min >> 32);
1358 clock_range[1].low = (uint32_t)info.range.max;
1359 clock_range[1].high = (uint32_t)(info.range.max >> 32);
1360 clock_range[2].low = (uint32_t)info.range.step;
1361 clock_range[2].high = (uint32_t)(info.range.step >> 32);
1362
1363 status = scmi_clock_ctx.scmi_api->write_payload(service_id,
1364 payload_size, &clock_range, sizeof(clock_range));
1365 if (status != FWK_SUCCESS) {
1366 goto exit;
1367 }
1368 payload_size += (uint32_t)sizeof(clock_range);
1369 }
1370
1371 return_values.status = (int32_t)SCMI_SUCCESS;
1372 status = scmi_clock_ctx.scmi_api->write_payload(service_id, 0,
1373 &return_values, sizeof(return_values));
1374
1375 exit:
1376 respond_status = scmi_clock_ctx.scmi_api->respond(
1377 service_id,
1378 (return_values.status == SCMI_SUCCESS) ? NULL : &return_values.status,
1379 (return_values.status == SCMI_SUCCESS) ? payload_size :
1380 sizeof(return_values.status));
1381 if (respond_status != FWK_SUCCESS) {
1382 FWK_LOG_DEBUG("[SCMI-CLK] %s @%d", __func__, __LINE__);
1383 }
1384
1385 return status;
1386 }
1387
1388 /*
1389 * SCMI module -> SCMI clock module interface
1390 */
scmi_clock_get_scmi_protocol_id(fwk_id_t protocol_id,uint8_t * scmi_protocol_id)1391 static int scmi_clock_get_scmi_protocol_id(fwk_id_t protocol_id,
1392 uint8_t *scmi_protocol_id)
1393 {
1394 *scmi_protocol_id = (uint8_t)MOD_SCMI_PROTOCOL_ID_CLOCK;
1395
1396 return FWK_SUCCESS;
1397 }
1398
scmi_clock_message_handler(fwk_id_t protocol_id,fwk_id_t service_id,const uint32_t * payload,size_t payload_size,unsigned int message_id)1399 static int scmi_clock_message_handler(fwk_id_t protocol_id, fwk_id_t service_id,
1400 const uint32_t *payload, size_t payload_size, unsigned int message_id)
1401 {
1402 int32_t return_value;
1403
1404 static_assert(FWK_ARRAY_SIZE(handler_table) ==
1405 FWK_ARRAY_SIZE(payload_size_table),
1406 "[SCMI] Clock management protocol table sizes not consistent");
1407 fwk_assert(payload != NULL);
1408
1409 if (message_id >= FWK_ARRAY_SIZE(handler_table)) {
1410 return_value = (int32_t)SCMI_NOT_FOUND;
1411 goto error;
1412 }
1413
1414 if (payload_size != payload_size_table[message_id]) {
1415 return_value = (int32_t)SCMI_PROTOCOL_ERROR;
1416 goto error;
1417 }
1418
1419 return handler_table[message_id](service_id, payload);
1420
1421 error:
1422 return scmi_clock_ctx.scmi_api->respond(
1423 service_id, &return_value, sizeof(return_value));
1424 }
1425
1426 static struct mod_scmi_to_protocol_api scmi_clock_mod_scmi_to_protocol_api = {
1427 .get_scmi_protocol_id = scmi_clock_get_scmi_protocol_id,
1428 .message_handler = scmi_clock_message_handler
1429 };
1430
1431 /*
1432 * Framework handlers
1433 */
1434
scmi_clock_init(fwk_id_t module_id,unsigned int element_count,const void * data)1435 static int scmi_clock_init(fwk_id_t module_id, unsigned int element_count,
1436 const void *data)
1437 {
1438 int clock_devices;
1439 const struct mod_scmi_clock_config *config =
1440 (const struct mod_scmi_clock_config *)data;
1441
1442 if ((config == NULL) || (config->agent_table == NULL)) {
1443 return FWK_E_PARAM;
1444 }
1445
1446 scmi_clock_ctx.config = config;
1447 scmi_clock_ctx.max_pending_transactions = config->max_pending_transactions;
1448 scmi_clock_ctx.agent_table = config->agent_table;
1449
1450 clock_devices = fwk_module_get_element_count(fwk_module_id_clock);
1451 if (clock_devices == FWK_E_PARAM) {
1452 return FWK_E_PANIC;
1453 }
1454
1455 scmi_clock_ctx.clock_devices = clock_devices;
1456
1457 /* Allocate a table of clock operations */
1458 scmi_clock_ctx.clock_ops =
1459 fwk_mm_calloc((unsigned int)clock_devices,
1460 sizeof(struct clock_operations));
1461
1462 /* Initialize table */
1463 for (unsigned int i = 0; i < (unsigned int)clock_devices; i++) {
1464 scmi_clock_ctx.clock_ops[i].service_id = FWK_ID_NONE;
1465 }
1466
1467 return FWK_SUCCESS;
1468 }
1469
scmi_clock_bind(fwk_id_t id,unsigned int round)1470 static int scmi_clock_bind(fwk_id_t id, unsigned int round)
1471 {
1472 int status;
1473
1474 if (round == 1) {
1475 return FWK_SUCCESS;
1476 }
1477
1478 status = fwk_module_bind(FWK_ID_MODULE(FWK_MODULE_IDX_SCMI),
1479 FWK_ID_API(FWK_MODULE_IDX_SCMI, MOD_SCMI_API_IDX_PROTOCOL),
1480 &scmi_clock_ctx.scmi_api);
1481 if (status != FWK_SUCCESS) {
1482 return status;
1483 }
1484
1485 #ifdef BUILD_HAS_MOD_RESOURCE_PERMS
1486 status = fwk_module_bind(
1487 FWK_ID_MODULE(FWK_MODULE_IDX_RESOURCE_PERMS),
1488 FWK_ID_API(FWK_MODULE_IDX_RESOURCE_PERMS, MOD_RES_PERM_RESOURCE_PERMS),
1489 &scmi_clock_ctx.res_perms_api);
1490 if (status != FWK_SUCCESS) {
1491 return status;
1492 }
1493 #endif
1494
1495 return fwk_module_bind(FWK_ID_MODULE(FWK_MODULE_IDX_CLOCK),
1496 FWK_ID_API(FWK_MODULE_IDX_CLOCK, 0), &scmi_clock_ctx.clock_api);
1497 }
1498
scmi_clock_process_bind_request(fwk_id_t source_id,fwk_id_t target_id,fwk_id_t api_id,const void ** api)1499 static int scmi_clock_process_bind_request(fwk_id_t source_id,
1500 fwk_id_t target_id, fwk_id_t api_id, const void **api)
1501 {
1502 if (!fwk_id_is_equal(source_id, FWK_ID_MODULE(FWK_MODULE_IDX_SCMI))) {
1503 return FWK_E_ACCESS;
1504 }
1505
1506 *api = &scmi_clock_mod_scmi_to_protocol_api;
1507
1508 return FWK_SUCCESS;
1509 }
1510
process_request_event(const struct fwk_event * event)1511 static int process_request_event(const struct fwk_event *event)
1512 {
1513 struct scmi_clock_event_request_params *params;
1514 unsigned int clock_dev_idx;
1515 int status;
1516 enum mod_clock_state clock_state;
1517 uint64_t rate;
1518 struct event_set_rate_request_data set_rate_data;
1519 struct event_set_state_request_data set_state_data;
1520 fwk_id_t service_id;
1521 enum scmi_clock_event_idx event_id_type;
1522
1523 params = (struct scmi_clock_event_request_params *)event->params;
1524 clock_dev_idx = fwk_id_get_element_idx(params->clock_dev_id);
1525 service_id = clock_ops_get_service(clock_dev_idx);
1526
1527 event_id_type = (enum scmi_clock_event_idx)fwk_id_get_event_idx(event->id);
1528
1529 switch (event_id_type) {
1530 case SCMI_CLOCK_EVENT_IDX_GET_STATE:
1531 status = scmi_clock_ctx.clock_api->get_state(params->clock_dev_id,
1532 &clock_state);
1533 if (status != FWK_PENDING) {
1534 /* Request completed */
1535 get_state_respond(params->clock_dev_id,
1536 service_id,
1537 &clock_state,
1538 status);
1539 }
1540 break;
1541
1542 case SCMI_CLOCK_EVENT_IDX_GET_RATE:
1543 status = scmi_clock_ctx.clock_api->get_rate(params->clock_dev_id,
1544 &rate);
1545 if (status != FWK_PENDING) {
1546 /* Request completed */
1547 get_rate_respond(service_id, &rate, status);
1548 }
1549 break;
1550
1551 case SCMI_CLOCK_EVENT_IDX_SET_RATE:
1552 set_rate_data = params->request_data.set_rate_data;
1553
1554 rate = (uint64_t)set_rate_data.rate[0] +
1555 (((uint64_t)set_rate_data.rate[1]) << 32);
1556
1557 status =
1558 scmi_clock_ctx.clock_api->set_rate(params->clock_dev_id,
1559 rate,
1560 set_rate_data.round_mode);
1561 if (status != FWK_PENDING) {
1562 /* Request completed */
1563 set_request_respond(service_id, status);
1564 status = FWK_SUCCESS;
1565 }
1566 break;
1567
1568 case SCMI_CLOCK_EVENT_IDX_SET_STATE:
1569 set_state_data = params->request_data.set_state_data;
1570
1571 status = scmi_clock_ctx.clock_api->set_state(params->clock_dev_id,
1572 set_state_data.state);
1573 if (status != FWK_PENDING) {
1574 /* Request completed */
1575 set_request_respond(service_id, status);
1576 clock_ops_update_state(clock_dev_idx, status);
1577 status = FWK_SUCCESS;
1578 }
1579 break;
1580
1581 default:
1582 return FWK_E_PARAM;
1583 }
1584
1585 if (status == FWK_PENDING) {
1586 return FWK_SUCCESS;
1587 }
1588
1589 if (status == FWK_SUCCESS) {
1590 clock_ops_set_available(clock_dev_idx);
1591 }
1592
1593 return status;
1594 }
1595
process_response_event(const struct fwk_event * event)1596 static int process_response_event(const struct fwk_event *event)
1597 {
1598 struct mod_clock_resp_params *params =
1599 (struct mod_clock_resp_params *)event->params;
1600 unsigned int clock_dev_idx;
1601 fwk_id_t service_id;
1602 enum mod_clock_state clock_state;
1603 uint64_t rate;
1604 enum mod_clock_event_idx event_id_type;
1605
1606 clock_dev_idx = fwk_id_get_element_idx(event->source_id);
1607 service_id = clock_ops_get_service(clock_dev_idx);
1608
1609 if (params->status != FWK_SUCCESS) {
1610 request_response(params->status, service_id);
1611 } else {
1612 event_id_type =
1613 (enum mod_clock_event_idx)fwk_id_get_event_idx(event->id);
1614
1615 switch (event_id_type) {
1616 case MOD_CLOCK_EVENT_IDX_GET_STATE_REQUEST:
1617 clock_state = params->value.state;
1618
1619 get_state_respond(event->source_id, service_id, &clock_state,
1620 FWK_SUCCESS);
1621
1622 break;
1623
1624 case MOD_CLOCK_EVENT_IDX_GET_RATE_REQUEST:
1625 rate = params->value.rate;
1626
1627 get_rate_respond(service_id, &rate, FWK_SUCCESS);
1628
1629 break;
1630
1631 case MOD_CLOCK_EVENT_IDX_SET_RATE_REQUEST:
1632 case MOD_CLOCK_EVENT_IDX_SET_STATE_REQUEST:
1633 set_request_respond(service_id, FWK_SUCCESS);
1634 clock_ops_update_state(clock_dev_idx, FWK_SUCCESS);
1635
1636 break;
1637
1638 default:
1639 return FWK_E_PARAM;
1640 }
1641 }
1642 clock_ops_set_available(clock_dev_idx);
1643
1644 return FWK_SUCCESS;
1645 }
1646
scmi_clock_process_event(const struct fwk_event * event,struct fwk_event * resp_event)1647 static int scmi_clock_process_event(const struct fwk_event *event,
1648 struct fwk_event *resp_event)
1649 {
1650 if (fwk_id_get_module_idx(event->source_id) ==
1651 fwk_id_get_module_idx(fwk_module_id_scmi)) {
1652 /* Request events */
1653 return process_request_event(event);
1654 }
1655
1656 if (fwk_id_get_module_idx(event->source_id) ==
1657 fwk_id_get_module_idx(fwk_module_id_clock)) {
1658 /* Responses from Clock HAL */
1659 return process_response_event(event);
1660 }
1661
1662 return FWK_E_PARAM;
1663 }
1664
1665 /* SCMI Clock Management Protocol Definition */
1666 const struct fwk_module module_scmi_clock = {
1667 .api_count = 1,
1668 .event_count = (unsigned int)SCMI_CLOCK_EVENT_IDX_COUNT,
1669 .type = FWK_MODULE_TYPE_PROTOCOL,
1670 .init = scmi_clock_init,
1671 .bind = scmi_clock_bind,
1672 .process_bind_request = scmi_clock_process_bind_request,
1673 .process_event = scmi_clock_process_event,
1674 };
1675