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
7 #include <assert.h>
8
9 #include <drivers/scmi-msg.h>
10 #include <drivers/scmi.h>
11
12 #include "common.h"
13
14 #pragma weak scmi_msg_get_clock_handler
15 #pragma weak scmi_msg_get_rstd_handler
16 #pragma weak scmi_msg_get_pd_handler
17 #pragma weak scmi_msg_get_voltage_handler
18
scmi_msg_get_clock_handler(struct scmi_msg * msg __unused)19 scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg __unused)
20 {
21 return NULL;
22 }
23
scmi_msg_get_rstd_handler(struct scmi_msg * msg __unused)24 scmi_msg_handler_t scmi_msg_get_rstd_handler(struct scmi_msg *msg __unused)
25 {
26 return NULL;
27 }
28
scmi_msg_get_pd_handler(struct scmi_msg * msg __unused)29 scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg __unused)
30 {
31 return NULL;
32 }
33
scmi_msg_get_voltage_handler(struct scmi_msg * msg __unused)34 scmi_msg_handler_t scmi_msg_get_voltage_handler(struct scmi_msg *msg __unused)
35 {
36 return NULL;
37 }
38
scmi_status_response(struct scmi_msg * msg,int32_t status)39 void scmi_status_response(struct scmi_msg *msg, int32_t status)
40 {
41 assert(msg->out && msg->out_size >= sizeof(int32_t));
42
43 memcpy(msg->out, &status, sizeof(int32_t));
44 msg->out_size_out = sizeof(int32_t);
45 }
46
scmi_write_response(struct scmi_msg * msg,void * payload,size_t size)47 void scmi_write_response(struct scmi_msg *msg, void *payload, size_t size)
48 {
49 /*
50 * Output payload shall be at least the size of the status
51 * Output buffer shall be at least be the size of the status
52 * Output paylaod shall fit in output buffer
53 */
54 assert(payload && size >= sizeof(int32_t) && size <= msg->out_size &&
55 msg->out && msg->out_size >= sizeof(int32_t));
56
57 memcpy(msg->out, payload, size);
58 msg->out_size_out = size;
59 }
60
scmi_process_message(struct scmi_msg * msg)61 void scmi_process_message(struct scmi_msg *msg)
62 {
63 scmi_msg_handler_t handler = NULL;
64
65 switch (msg->protocol_id) {
66 case SCMI_PROTOCOL_ID_BASE:
67 handler = scmi_msg_get_base_handler(msg);
68 break;
69 case SCMI_PROTOCOL_ID_CLOCK:
70 handler = scmi_msg_get_clock_handler(msg);
71 break;
72 case SCMI_PROTOCOL_ID_RESET_DOMAIN:
73 handler = scmi_msg_get_rstd_handler(msg);
74 break;
75 case SCMI_PROTOCOL_ID_POWER_DOMAIN:
76 handler = scmi_msg_get_pd_handler(msg);
77 break;
78 default:
79 break;
80 }
81
82 if (handler) {
83 handler(msg);
84 return;
85 }
86
87 ERROR("Agent %u Protocol 0x%x Message 0x%x: not supported",
88 msg->agent_id, msg->protocol_id, msg->message_id);
89
90 scmi_status_response(msg, SCMI_NOT_SUPPORTED);
91 }
92