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