1 // SPDX-License-Identifier: BSD-3-Clause
2 /*
3 * Copyright (c) 2015-2022, 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 #pragma weak scmi_msg_get_sensor_handler
19
scmi_msg_get_clock_handler(struct scmi_msg * msg __unused)20 scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg __unused)
21 {
22 return NULL;
23 }
24
scmi_msg_get_rstd_handler(struct scmi_msg * msg __unused)25 scmi_msg_handler_t scmi_msg_get_rstd_handler(struct scmi_msg *msg __unused)
26 {
27 return NULL;
28 }
29
scmi_msg_get_pd_handler(struct scmi_msg * msg __unused)30 scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg __unused)
31 {
32 return NULL;
33 }
34
scmi_msg_get_voltage_handler(struct scmi_msg * msg __unused)35 scmi_msg_handler_t scmi_msg_get_voltage_handler(struct scmi_msg *msg __unused)
36 {
37 return NULL;
38 }
39
scmi_msg_get_sensor_handler(struct scmi_msg * msg __unused)40 scmi_msg_handler_t scmi_msg_get_sensor_handler(struct scmi_msg *msg __unused)
41 {
42 return NULL;
43 }
44
scmi_status_response(struct scmi_msg * msg,int32_t status)45 void scmi_status_response(struct scmi_msg *msg, int32_t status)
46 {
47 assert(msg->out && msg->out_size >= sizeof(int32_t));
48
49 memcpy(msg->out, &status, sizeof(int32_t));
50 msg->out_size_out = sizeof(int32_t);
51 }
52
scmi_write_response(struct scmi_msg * msg,void * payload,size_t size)53 void scmi_write_response(struct scmi_msg *msg, void *payload, size_t size)
54 {
55 /*
56 * Output payload shall be at least the size of the status
57 * Output buffer shall be at least be the size of the status
58 * Output paylaod shall fit in output buffer
59 */
60 assert(payload && size >= sizeof(int32_t) && size <= msg->out_size &&
61 msg->out && msg->out_size >= sizeof(int32_t));
62
63 memcpy(msg->out, payload, size);
64 msg->out_size_out = size;
65 }
66
scmi_process_message(struct scmi_msg * msg)67 void scmi_process_message(struct scmi_msg *msg)
68 {
69 scmi_msg_handler_t handler = NULL;
70
71 switch (msg->protocol_id) {
72 case SCMI_PROTOCOL_ID_BASE:
73 handler = scmi_msg_get_base_handler(msg);
74 break;
75 case SCMI_PROTOCOL_ID_CLOCK:
76 handler = scmi_msg_get_clock_handler(msg);
77 break;
78 case SCMI_PROTOCOL_ID_RESET_DOMAIN:
79 handler = scmi_msg_get_rstd_handler(msg);
80 break;
81 case SCMI_PROTOCOL_ID_POWER_DOMAIN:
82 handler = scmi_msg_get_pd_handler(msg);
83 break;
84 case SCMI_PROTOCOL_ID_SENSOR:
85 handler = scmi_msg_get_sensor_handler(msg);
86 break;
87 default:
88 break;
89 }
90
91 if (handler) {
92 handler(msg);
93 return;
94 }
95
96 ERROR("Agent %u Protocol 0x%x Message 0x%x: not supported\n",
97 msg->agent_id, msg->protocol_id, msg->message_id);
98
99 scmi_status_response(msg, SCMI_NOT_SUPPORTED);
100 }
101