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 SCMI_RATES_BY_ARRAY(_nb_rates, _rem_rates) \
244 SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS((_nb_rates), \
245 SCMI_CLOCK_RATE_FORMAT_LIST, \
246 (_rem_rates))
247 #define SCMI_RATES_BY_STEP \
248 SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS(3, \
249 SCMI_CLOCK_RATE_FORMAT_RANGE, \
250 0)
251
252 #define RATE_DESC_SIZE sizeof(struct scmi_clock_rate)
253
write_rate_desc_array_in_buffer(char * dest,unsigned long * rates,size_t nb_elt)254 static void write_rate_desc_array_in_buffer(char *dest, unsigned long *rates,
255 size_t nb_elt)
256 {
257 uint32_t *out = NULL;
258 size_t n = 0;
259
260 assert(IS_ALIGNED_WITH_TYPE(dest, uint32_t));
261 out = (uint32_t *)(uintptr_t)dest;
262
263 for (n = 0; n < nb_elt; n++) {
264 uint64_t rate = rates[n];
265
266 reg_pair_from_64(rate, out + 2 * n + 1, out + 2 * n);
267 }
268 }
269
scmi_clock_describe_rates(struct scmi_msg * msg)270 static void scmi_clock_describe_rates(struct scmi_msg *msg)
271 {
272 const struct scmi_clock_describe_rates_a2p *in_args = (void *)msg->in;
273 struct scmi_clock_describe_rates_p2a p2a = { };
274 size_t nb_rates = 0;
275 int32_t status = SCMI_GENERIC_ERROR;
276 unsigned int clock_id = 0;
277 unsigned int out_count = 0;
278
279 if (msg->in_size != sizeof(*in_args)) {
280 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
281 return;
282 }
283
284 if (in_args->clock_id >= plat_scmi_clock_count(msg->channel_id)) {
285 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
286 return;
287 }
288
289 clock_id = confine_array_index(in_args->clock_id,
290 plat_scmi_clock_count(msg->channel_id));
291
292 /* Platform may support array rate description */
293 status = plat_scmi_clock_rates_array(msg->channel_id, clock_id, 0, NULL,
294 &nb_rates);
295 if (status == SCMI_SUCCESS) {
296 unsigned int rate_index = in_args->rate_index;
297 unsigned int remaining = 0;
298 size_t avail_sz = msg->out_size - sizeof(p2a);
299 char *out_rates = msg->out + sizeof(p2a);
300
301 if (avail_sz < RATE_DESC_SIZE && nb_rates) {
302 status = SCMI_PROTOCOL_ERROR;
303 goto out;
304 }
305
306 while (avail_sz >= RATE_DESC_SIZE && rate_index < nb_rates) {
307 unsigned long rate = 0;
308 size_t cnt = 1;
309
310 status = plat_scmi_clock_rates_array(msg->channel_id,
311 clock_id,
312 rate_index,
313 &rate, &cnt);
314 if (status)
315 goto out;
316
317 write_rate_desc_array_in_buffer(out_rates, &rate, cnt);
318 avail_sz -= RATE_DESC_SIZE;
319 out_rates += RATE_DESC_SIZE;
320 rate_index++;
321 }
322
323 out_count = rate_index - in_args->rate_index;
324 remaining = nb_rates - in_args->rate_index;
325 p2a.num_rates_flags = SCMI_RATES_BY_ARRAY(out_count, remaining);
326 } else if (status == SCMI_NOT_SUPPORTED) {
327 unsigned long triplet[3] = { 0, 0, 0 };
328
329 if (msg->out_size < sizeof(p2a) + 3 * RATE_DESC_SIZE) {
330 status = SCMI_PROTOCOL_ERROR;
331 goto out;
332 }
333
334 /* Platform may support min/max/step triplet description */
335 status = plat_scmi_clock_rates_by_step(msg->channel_id,
336 clock_id, triplet);
337 if (status)
338 goto out;
339
340 write_rate_desc_array_in_buffer(msg->out + sizeof(p2a),
341 triplet, 3);
342
343 out_count = 3;
344 p2a.num_rates_flags = SCMI_RATES_BY_STEP;
345 } else {
346 /* Fallthrough generic exit sequence below with error status */
347 }
348
349 out:
350 if (status) {
351 scmi_status_response(msg, status);
352 } else {
353 p2a.status = SCMI_SUCCESS;
354 memcpy(msg->out, &p2a, sizeof(p2a));
355 msg->out_size_out = sizeof(p2a) + out_count * RATE_DESC_SIZE;
356 }
357 }
358
359 static const scmi_msg_handler_t scmi_clock_handler_table[] = {
360 [SCMI_PROTOCOL_VERSION] = report_version,
361 [SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
362 [SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
363 [SCMI_CLOCK_ATTRIBUTES] = scmi_clock_attributes,
364 [SCMI_CLOCK_DESCRIBE_RATES] = scmi_clock_describe_rates,
365 [SCMI_CLOCK_RATE_SET] = scmi_clock_rate_set,
366 [SCMI_CLOCK_RATE_GET] = scmi_clock_rate_get,
367 [SCMI_CLOCK_CONFIG_SET] = scmi_clock_config_set,
368 };
369
message_id_is_supported(unsigned int message_id)370 static bool message_id_is_supported(unsigned int message_id)
371 {
372 return message_id < ARRAY_SIZE(scmi_clock_handler_table) &&
373 scmi_clock_handler_table[message_id];
374 }
375
scmi_msg_get_clock_handler(struct scmi_msg * msg)376 scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg)
377 {
378 const size_t array_size = ARRAY_SIZE(scmi_clock_handler_table);
379 unsigned int message_id = 0;
380
381 if (msg->message_id >= array_size) {
382 DMSG("Clock handle not found %u", msg->message_id);
383 return NULL;
384 }
385
386 message_id = confine_array_index(msg->message_id, array_size);
387
388 return scmi_clock_handler_table[message_id];
389 }
390