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_rates_array
18 #pragma weak plat_scmi_clock_rates_by_step
19 #pragma weak plat_scmi_clock_get_rate
20 #pragma weak plat_scmi_clock_set_rate
21 #pragma weak plat_scmi_clock_get_state
22 #pragma weak plat_scmi_clock_set_state
23
24 static bool message_id_is_supported(unsigned int message_id);
25
plat_scmi_clock_count(unsigned int agent_id __unused)26 size_t plat_scmi_clock_count(unsigned int agent_id __unused)
27 {
28 return 0U;
29 }
30
plat_scmi_clock_get_name(unsigned int agent_id __unused,unsigned int scmi_id __unused)31 const char *plat_scmi_clock_get_name(unsigned int agent_id __unused,
32 unsigned int scmi_id __unused)
33 {
34 return NULL;
35 }
36
plat_scmi_clock_rates_array(unsigned int agent_id __unused,unsigned int scmi_id __unused,unsigned long * rates __unused,size_t * nb_elts __unused)37 int32_t plat_scmi_clock_rates_array(unsigned int agent_id __unused,
38 unsigned int scmi_id __unused,
39 unsigned long *rates __unused,
40 size_t *nb_elts __unused)
41 {
42 return SCMI_NOT_SUPPORTED;
43 }
44
plat_scmi_clock_rates_by_step(unsigned int agent_id __unused,unsigned int scmi_id __unused,unsigned long * steps __unused)45 int32_t plat_scmi_clock_rates_by_step(unsigned int agent_id __unused,
46 unsigned int scmi_id __unused,
47 unsigned long *steps __unused)
48 {
49 return SCMI_NOT_SUPPORTED;
50 }
51
plat_scmi_clock_get_rate(unsigned int agent_id __unused,unsigned int scmi_id __unused)52 unsigned long plat_scmi_clock_get_rate(unsigned int agent_id __unused,
53 unsigned int scmi_id __unused)
54 {
55 return 0U;
56 }
57
plat_scmi_clock_set_rate(unsigned int agent_id __unused,unsigned int scmi_id __unused,unsigned long rate __unused)58 int32_t plat_scmi_clock_set_rate(unsigned int agent_id __unused,
59 unsigned int scmi_id __unused,
60 unsigned long rate __unused)
61 {
62 return SCMI_NOT_SUPPORTED;
63 }
64
plat_scmi_clock_get_state(unsigned int agent_id __unused,unsigned int scmi_id __unused)65 int32_t plat_scmi_clock_get_state(unsigned int agent_id __unused,
66 unsigned int scmi_id __unused)
67 {
68 return SCMI_NOT_SUPPORTED;
69 }
70
plat_scmi_clock_set_state(unsigned int agent_id __unused,unsigned int scmi_id __unused,bool enable_not_disable __unused)71 int32_t plat_scmi_clock_set_state(unsigned int agent_id __unused,
72 unsigned int scmi_id __unused,
73 bool enable_not_disable __unused)
74 {
75 return SCMI_NOT_SUPPORTED;
76 }
77
report_version(struct scmi_msg * msg)78 static void report_version(struct scmi_msg *msg)
79 {
80 struct scmi_protocol_version_p2a return_values = {
81 .status = SCMI_SUCCESS,
82 .version = SCMI_PROTOCOL_VERSION_CLOCK,
83 };
84
85 if (msg->in_size != 0) {
86 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
87 return;
88 }
89
90 scmi_write_response(msg, &return_values, sizeof(return_values));
91 }
92
report_attributes(struct scmi_msg * msg)93 static void report_attributes(struct scmi_msg *msg)
94 {
95 size_t agent_count = plat_scmi_clock_count(msg->agent_id);
96 struct scmi_protocol_attributes_p2a return_values = {
97 .status = SCMI_SUCCESS,
98 .attributes = SCMI_CLOCK_PROTOCOL_ATTRIBUTES(1U, agent_count),
99 };
100
101 if (msg->in_size != 0) {
102 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
103 return;
104 }
105
106 scmi_write_response(msg, &return_values, sizeof(return_values));
107 }
108
report_message_attributes(struct scmi_msg * msg)109 static void report_message_attributes(struct scmi_msg *msg)
110 {
111 struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
112 struct scmi_protocol_message_attributes_p2a return_values = {
113 .status = SCMI_SUCCESS,
114 /* For this protocol, attributes shall be zero */
115 .attributes = 0U,
116 };
117
118 if (msg->in_size != sizeof(*in_args)) {
119 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
120 return;
121 }
122
123 if (!message_id_is_supported(in_args->message_id)) {
124 scmi_status_response(msg, SCMI_NOT_FOUND);
125 return;
126 }
127
128 scmi_write_response(msg, &return_values, sizeof(return_values));
129 }
130
scmi_clock_attributes(struct scmi_msg * msg)131 static void scmi_clock_attributes(struct scmi_msg *msg)
132 {
133 const struct scmi_clock_attributes_a2p *in_args = (void *)msg->in;
134 struct scmi_clock_attributes_p2a return_values = {
135 .status = SCMI_SUCCESS,
136 };
137 const char *name = NULL;
138 unsigned int clock_id = 0U;
139
140 if (msg->in_size != sizeof(*in_args)) {
141 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
142 return;
143 }
144
145 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
146
147 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
148 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
149 return;
150 }
151
152
153 name = plat_scmi_clock_get_name(msg->agent_id, clock_id);
154 if (name == NULL) {
155 scmi_status_response(msg, SCMI_NOT_FOUND);
156 return;
157 }
158
159 COPY_NAME_IDENTIFIER(return_values.clock_name, name);
160
161 return_values.attributes = plat_scmi_clock_get_state(msg->agent_id,
162 clock_id);
163
164 scmi_write_response(msg, &return_values, sizeof(return_values));
165 }
166
scmi_clock_rate_get(struct scmi_msg * msg)167 static void scmi_clock_rate_get(struct scmi_msg *msg)
168 {
169 const struct scmi_clock_rate_get_a2p *in_args = (void *)msg->in;
170 unsigned long rate = 0U;
171 struct scmi_clock_rate_get_p2a return_values = {
172 .status = SCMI_SUCCESS,
173 };
174 unsigned int clock_id = 0U;
175
176 if (msg->in_size != sizeof(*in_args)) {
177 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
178 return;
179 }
180
181 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
182
183 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
184 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
185 return;
186 }
187
188 rate = plat_scmi_clock_get_rate(msg->agent_id, clock_id);
189
190 return_values.rate[0] = (uint32_t)rate;
191 return_values.rate[1] = (uint32_t)((uint64_t)rate >> 32);
192
193 scmi_write_response(msg, &return_values, sizeof(return_values));
194 }
195
scmi_clock_rate_set(struct scmi_msg * msg)196 static void scmi_clock_rate_set(struct scmi_msg *msg)
197 {
198 const struct scmi_clock_rate_set_a2p *in_args = (void *)msg->in;
199 unsigned long rate = 0U;
200 int32_t status = 0;
201 unsigned int clock_id = 0U;
202
203 if (msg->in_size != sizeof(*in_args)) {
204 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
205 return;
206 }
207
208 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
209
210 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
211 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
212 return;
213 }
214
215 rate = (unsigned long)(((uint64_t)in_args->rate[1] << 32) |
216 in_args->rate[0]);
217
218 status = plat_scmi_clock_set_rate(msg->agent_id, clock_id, rate);
219
220 scmi_status_response(msg, status);
221 }
222
scmi_clock_config_set(struct scmi_msg * msg)223 static void scmi_clock_config_set(struct scmi_msg *msg)
224 {
225 const struct scmi_clock_config_set_a2p *in_args = (void *)msg->in;
226 int32_t status = SCMI_GENERIC_ERROR;
227 bool enable = false;
228 unsigned int clock_id = 0U;
229
230 if (msg->in_size != sizeof(*in_args)) {
231 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
232 return;
233 }
234
235 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
236
237 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
238 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
239 return;
240 }
241
242 enable = in_args->attributes & SCMI_CLOCK_CONFIG_SET_ENABLE_MASK;
243
244 status = plat_scmi_clock_set_state(msg->agent_id, clock_id, enable);
245
246 scmi_status_response(msg, status);
247 }
248
249 #define RATES_ARRAY_SIZE_MAX (SCMI_PLAYLOAD_MAX - \
250 sizeof(struct scmi_clock_describe_rates_p2a))
251
252 #define SCMI_RATES_BY_ARRAY(_nb_rates, _rem_rates) \
253 SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS((_nb_rates), \
254 SCMI_CLOCK_RATE_FORMAT_LIST, \
255 (_rem_rates))
256 #define SCMI_RATES_BY_STEP \
257 SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS(3U, \
258 SCMI_CLOCK_RATE_FORMAT_RANGE, \
259 0U)
260
261 #define RATE_DESC_SIZE sizeof(struct scmi_clock_rate)
262
write_rate_desc_array_in_buffer(char * dest,unsigned long * rates,size_t nb_elt)263 static void write_rate_desc_array_in_buffer(char *dest, unsigned long *rates,
264 size_t nb_elt)
265 {
266 uint32_t *out = (uint32_t *)(uintptr_t)dest;
267 size_t n;
268
269 ASSERT_SYM_PTR_ALIGN(out);
270
271 for (n = 0U; n < nb_elt; n++) {
272 out[2 * n] = (uint32_t)rates[n];
273 out[2 * n + 1] = (uint32_t)((uint64_t)rates[n] >> 32);
274 }
275 }
276
scmi_clock_describe_rates(struct scmi_msg * msg)277 static void scmi_clock_describe_rates(struct scmi_msg *msg)
278 {
279 const struct scmi_clock_describe_rates_a2p *in_args = (void *)msg->in;
280 struct scmi_clock_describe_rates_p2a p2a = {
281 .status = SCMI_SUCCESS,
282 };
283 size_t nb_rates;
284 int32_t status;
285 unsigned int clock_id;
286
287 if (msg->in_size != sizeof(*in_args)) {
288 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
289 return;
290 }
291
292 clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
293
294 if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
295 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
296 return;
297 }
298
299 /* Platform may support array rate description */
300 status = plat_scmi_clock_rates_array(msg->agent_id, clock_id, NULL,
301 &nb_rates);
302 if (status == SCMI_SUCCESS) {
303 /* Currently 12 cells mex, so it's affordable for the stack */
304 unsigned long plat_rates[RATES_ARRAY_SIZE_MAX / RATE_DESC_SIZE];
305 size_t max_nb = RATES_ARRAY_SIZE_MAX / RATE_DESC_SIZE;
306 size_t ret_nb = MIN(nb_rates - in_args->rate_index, max_nb);
307 size_t rem_nb = nb_rates - in_args->rate_index - ret_nb;
308
309 status = plat_scmi_clock_rates_array(msg->agent_id, clock_id,
310 plat_rates, &ret_nb);
311 if (status == SCMI_SUCCESS) {
312 write_rate_desc_array_in_buffer(msg->out + sizeof(p2a),
313 plat_rates, ret_nb);
314
315 p2a.num_rates_flags = SCMI_RATES_BY_ARRAY(ret_nb,
316 rem_nb);
317 p2a.status = SCMI_SUCCESS;
318
319 memcpy(msg->out, &p2a, sizeof(p2a));
320 msg->out_size_out = sizeof(p2a) +
321 ret_nb * RATE_DESC_SIZE;
322 }
323 } else if (status == SCMI_NOT_SUPPORTED) {
324 unsigned long triplet[3] = { 0U, 0U, 0U };
325
326 /* Platform may support min§max/step triplet description */
327 status = plat_scmi_clock_rates_by_step(msg->agent_id, clock_id,
328 triplet);
329 if (status == SCMI_SUCCESS) {
330 write_rate_desc_array_in_buffer(msg->out + sizeof(p2a),
331 triplet, 3U);
332
333 p2a.num_rates_flags = SCMI_RATES_BY_STEP;
334 p2a.status = SCMI_SUCCESS;
335
336 memcpy(msg->out, &p2a, sizeof(p2a));
337 msg->out_size_out = sizeof(p2a) + (3U * RATE_DESC_SIZE);
338 }
339 } else {
340 /* Fallthrough generic exit sequence below with error status */
341 }
342
343 if (status != SCMI_SUCCESS) {
344 scmi_status_response(msg, status);
345 } else {
346 /*
347 * Message payload is already writen to msg->out, and
348 * msg->out_size_out updated.
349 */
350 }
351 }
352
353 static const scmi_msg_handler_t scmi_clock_handler_table[] = {
354 [SCMI_PROTOCOL_VERSION] = report_version,
355 [SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
356 [SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
357 [SCMI_CLOCK_ATTRIBUTES] = scmi_clock_attributes,
358 [SCMI_CLOCK_DESCRIBE_RATES] = scmi_clock_describe_rates,
359 [SCMI_CLOCK_RATE_SET] = scmi_clock_rate_set,
360 [SCMI_CLOCK_RATE_GET] = scmi_clock_rate_get,
361 [SCMI_CLOCK_CONFIG_SET] = scmi_clock_config_set,
362 };
363
message_id_is_supported(size_t message_id)364 static bool message_id_is_supported(size_t message_id)
365 {
366 return (message_id < ARRAY_SIZE(scmi_clock_handler_table)) &&
367 (scmi_clock_handler_table[message_id] != NULL);
368 }
369
scmi_msg_get_clock_handler(struct scmi_msg * msg)370 scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg)
371 {
372 const size_t array_size = ARRAY_SIZE(scmi_clock_handler_table);
373 unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id);
374
375 if (message_id >= array_size) {
376 VERBOSE("Clock handle not found %u", msg->message_id);
377 return NULL;
378 }
379
380 return scmi_clock_handler_table[message_id];
381 }
382