1 // SPDX-License-Identifier: BSD-3-Clause
2 /*
3 * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
4 * Copyright (c) 2020, Linaro Limited
5 */
6 #include <assert.h>
7 #include <config.h>
8 #include <confine_array_index.h>
9 #include <drivers/scmi-msg.h>
10 #include <drivers/scmi.h>
11 #include <string.h>
12 #include <util.h>
13
14 #include "common.h"
15 #include "voltage_domain.h"
16
17 static bool message_id_is_supported(unsigned int message_id);
18
plat_scmi_voltd_count(unsigned int channel_id __unused)19 size_t __weak plat_scmi_voltd_count(unsigned int channel_id __unused)
20 {
21 return 0;
22 }
23
plat_scmi_voltd_get_name(unsigned int channel_id __unused,unsigned int scmi_id __unused)24 const char __weak *plat_scmi_voltd_get_name(unsigned int channel_id __unused,
25 unsigned int scmi_id __unused)
26 {
27 return NULL;
28 }
29
plat_scmi_voltd_levels_array(unsigned int channel_id __unused,unsigned int scmi_id __unused,size_t start_index __unused,long * levels __unused,size_t * nb_elts __unused)30 int32_t __weak plat_scmi_voltd_levels_array(unsigned int channel_id __unused,
31 unsigned int scmi_id __unused,
32 size_t start_index __unused,
33 long *levels __unused,
34 size_t *nb_elts __unused)
35 {
36 return SCMI_NOT_SUPPORTED;
37 }
38
plat_scmi_voltd_levels_by_step(unsigned int channel_id __unused,unsigned int scmi_id __unused,long * steps __unused)39 int32_t __weak plat_scmi_voltd_levels_by_step(unsigned int channel_id __unused,
40 unsigned int scmi_id __unused,
41 long *steps __unused)
42 {
43 return SCMI_NOT_SUPPORTED;
44 }
45
plat_scmi_voltd_get_level(unsigned int channel_id __unused,unsigned int scmi_id __unused)46 long __weak plat_scmi_voltd_get_level(unsigned int channel_id __unused,
47 unsigned int scmi_id __unused)
48 {
49 return 0;
50 }
51
plat_scmi_voltd_set_level(unsigned int channel_id __unused,unsigned int scmi_id __unused,long microvolt __unused)52 int32_t __weak plat_scmi_voltd_set_level(unsigned int channel_id __unused,
53 unsigned int scmi_id __unused,
54 long microvolt __unused)
55 {
56 return SCMI_NOT_SUPPORTED;
57 }
58
plat_scmi_voltd_get_config(unsigned int channel_id __unused,unsigned int scmi_id __unused,uint32_t * config __unused)59 int32_t __weak plat_scmi_voltd_get_config(unsigned int channel_id __unused,
60 unsigned int scmi_id __unused,
61 uint32_t *config __unused)
62 {
63 return SCMI_NOT_SUPPORTED;
64 }
65
plat_scmi_voltd_set_config(unsigned int channel_id __unused,unsigned int scmi_id __unused,uint32_t config __unused)66 int32_t __weak plat_scmi_voltd_set_config(unsigned int channel_id __unused,
67 unsigned int scmi_id __unused,
68 uint32_t config __unused)
69 {
70 return SCMI_NOT_SUPPORTED;
71 }
72
report_version(struct scmi_msg * msg)73 static void report_version(struct scmi_msg *msg)
74 {
75 struct scmi_protocol_version_p2a out_args = {
76 .status = SCMI_SUCCESS,
77 .version = SCMI_PROTOCOL_VERSION_VOLTAGE_DOMAIN,
78 };
79
80 if (IS_ENABLED(CFG_SCMI_MSG_STRICT_ABI) && msg->in_size) {
81 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
82 return;
83 }
84
85 scmi_write_response(msg, &out_args, sizeof(out_args));
86 }
87
report_attributes(struct scmi_msg * msg)88 static void report_attributes(struct scmi_msg *msg)
89 {
90 size_t domain_count = plat_scmi_voltd_count(msg->channel_id);
91 struct scmi_protocol_attributes_p2a out_args = {
92 .status = SCMI_SUCCESS,
93 .attributes = domain_count,
94 };
95
96 assert(!(domain_count & ~SCMI_VOLTAGE_DOMAIN_COUNT_MASK));
97
98 if (IS_ENABLED(CFG_SCMI_MSG_STRICT_ABI) && msg->in_size) {
99 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
100 return;
101 }
102
103 scmi_write_response(msg, &out_args, sizeof(out_args));
104 }
105
report_message_attributes(struct scmi_msg * msg)106 static void report_message_attributes(struct scmi_msg *msg)
107 {
108 struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
109 struct scmi_protocol_message_attributes_p2a out_args = {
110 .status = SCMI_SUCCESS,
111 /* For this protocol, attributes shall be zero */
112 .attributes = 0,
113 };
114
115 if (msg->in_size != sizeof(*in_args)) {
116 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
117 return;
118 }
119
120 if (!message_id_is_supported(in_args->message_id)) {
121 scmi_status_response(msg, SCMI_NOT_FOUND);
122 return;
123 }
124
125 scmi_write_response(msg, &out_args, sizeof(out_args));
126 }
127
scmi_voltd_domain_attributes(struct scmi_msg * msg)128 static void scmi_voltd_domain_attributes(struct scmi_msg *msg)
129 {
130 const struct scmi_voltd_attributes_a2p *in_args = (void *)msg->in;
131 struct scmi_voltd_attributes_p2a out_args = {
132 .status = SCMI_SUCCESS,
133 };
134 const char *name = NULL;
135 unsigned int domain_id = 0;
136
137 if (msg->in_size != sizeof(*in_args)) {
138 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
139 return;
140 }
141
142 if (in_args->domain_id >= plat_scmi_voltd_count(msg->channel_id)) {
143 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
144 return;
145 }
146
147 domain_id = confine_array_index(in_args->domain_id,
148 plat_scmi_voltd_count(msg->channel_id));
149
150 name = plat_scmi_voltd_get_name(msg->channel_id, domain_id);
151 if (!name) {
152 scmi_status_response(msg, SCMI_NOT_FOUND);
153 return;
154 }
155
156 COPY_NAME_IDENTIFIER(out_args.name, name);
157
158 scmi_write_response(msg, &out_args, sizeof(out_args));
159 }
160
161 #define LEVELS_BY_ARRAY(_nb_rates, _rem_rates) \
162 SCMI_VOLTAGE_DOMAIN_LEVELS_FLAGS((_nb_rates), \
163 SCMI_VOLTD_LEVELS_FORMAT_LIST, \
164 (_rem_rates))
165
166 /* List levels array by small chunks fitting in SCMI message max payload size */
167 #define LEVELS_ARRAY_SIZE_MAX_2 \
168 ((SCMI_PLAYLOAD_MAX - sizeof(struct scmi_voltd_describe_levels_p2a)) / \
169 sizeof(int32_t))
170
171 #define LEVELS_ARRAY_SIZE_MAX 10
172
173 #define LEVELS_BY_STEP \
174 SCMI_VOLTAGE_DOMAIN_LEVELS_FLAGS(3, SCMI_VOLTD_LEVELS_FORMAT_RANGE, 0)
175
scmi_voltd_describe_levels(struct scmi_msg * msg)176 static void scmi_voltd_describe_levels(struct scmi_msg *msg)
177 {
178 const struct scmi_voltd_describe_levels_a2p *in_args = (void *)msg->in;
179 size_t nb_levels = 0;
180 unsigned int domain_id = 0;
181 int32_t status = SCMI_GENERIC_ERROR;
182
183 if (msg->in_size != sizeof(*in_args)) {
184 status = SCMI_PROTOCOL_ERROR;
185 goto out;
186 }
187
188 if (in_args->domain_id >= plat_scmi_voltd_count(msg->channel_id)) {
189 status = SCMI_INVALID_PARAMETERS;
190 goto out;
191 }
192
193 domain_id = confine_array_index(in_args->domain_id,
194 plat_scmi_voltd_count(msg->channel_id));
195
196 /* Platform may support array rate description */
197 status = plat_scmi_voltd_levels_array(msg->channel_id, domain_id, 0,
198 NULL, &nb_levels);
199 if (status == SCMI_SUCCESS) {
200 /* Use the stack to get the returned portion of levels array */
201 long plat_levels[LEVELS_ARRAY_SIZE_MAX];
202 size_t ret_nb = 0;
203 size_t rem_nb = 0;
204
205 if (in_args->level_index >= nb_levels) {
206 status = SCMI_INVALID_PARAMETERS;
207 goto out;
208 }
209
210 ret_nb = MIN(ARRAY_SIZE(plat_levels),
211 nb_levels - in_args->level_index);
212 rem_nb = nb_levels - in_args->level_index - ret_nb;
213
214 status = plat_scmi_voltd_levels_array(msg->channel_id,
215 domain_id,
216 in_args->level_index,
217 plat_levels,
218 &ret_nb);
219 if (status == SCMI_SUCCESS) {
220 struct scmi_voltd_describe_levels_p2a out_args = {
221 .flags = LEVELS_BY_ARRAY(ret_nb, rem_nb),
222 .status = SCMI_SUCCESS,
223 };
224 int32_t *voltage = NULL;
225 size_t i = 0;
226
227 /* By construction these values are 32bit aligned */
228 voltage = (int32_t *)(uintptr_t)(msg->out +
229 sizeof(out_args));
230
231 for (i = 0; i < ret_nb; i++)
232 voltage[i] = plat_levels[i];
233
234 memcpy(msg->out, &out_args, sizeof(out_args));
235
236 msg->out_size_out = sizeof(out_args) +
237 ret_nb * sizeof(uint32_t);
238 }
239 } else if (status == SCMI_NOT_SUPPORTED) {
240 long triplet[3] = { 0, 0, 0 };
241
242 /* Platform may support min/max/step triplet description */
243 status = plat_scmi_voltd_levels_by_step(msg->channel_id,
244 domain_id, triplet);
245 if (status == SCMI_SUCCESS) {
246 struct scmi_voltd_describe_levels_p2a out_args = {
247 .flags = LEVELS_BY_STEP,
248 .status = SCMI_SUCCESS,
249 };
250 int32_t *voltage = NULL;
251
252 /* By construction these values are 32bit aligned */
253 voltage = (int32_t *)(uintptr_t)(msg->out +
254 sizeof(out_args));
255 voltage[0] = triplet[0];
256 voltage[1] = triplet[1];
257 voltage[2] = triplet[2];
258
259 memcpy(msg->out, &out_args, sizeof(out_args));
260
261 msg->out_size_out = sizeof(out_args) +
262 3 * sizeof(int32_t);
263 }
264 }
265
266 out:
267 if (status)
268 scmi_status_response(msg, status);
269 }
270
scmi_voltd_config_set(struct scmi_msg * msg)271 static void scmi_voltd_config_set(struct scmi_msg *msg)
272 {
273 const struct scmi_voltd_config_set_a2p *in_args = (void *)msg->in;
274 unsigned int domain_id = 0;
275 unsigned long config = 0;
276 int32_t status = SCMI_GENERIC_ERROR;
277
278 if (msg->in_size != sizeof(*in_args)) {
279 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
280 return;
281 }
282
283 if (in_args->domain_id >= plat_scmi_voltd_count(msg->channel_id)) {
284 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
285 return;
286 }
287
288 domain_id = confine_array_index(in_args->domain_id,
289 plat_scmi_voltd_count(msg->channel_id));
290
291 config = in_args->config & SCMI_VOLTAGE_DOMAIN_CONFIG_MASK;
292
293 status = plat_scmi_voltd_set_config(msg->channel_id, domain_id, config);
294
295 scmi_status_response(msg, status);
296 }
297
scmi_voltd_config_get(struct scmi_msg * msg)298 static void scmi_voltd_config_get(struct scmi_msg *msg)
299 {
300 const struct scmi_voltd_config_get_a2p *in_args = (void *)msg->in;
301 struct scmi_voltd_config_get_p2a out_args = { };
302 unsigned int domain_id = 0;
303
304 if (msg->in_size != sizeof(*in_args)) {
305 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
306 return;
307 }
308
309 if (in_args->domain_id >= plat_scmi_voltd_count(msg->channel_id)) {
310 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
311 return;
312 }
313
314 domain_id = confine_array_index(in_args->domain_id,
315 plat_scmi_voltd_count(msg->channel_id));
316
317 if (plat_scmi_voltd_get_config(msg->channel_id, domain_id,
318 &out_args.config)) {
319 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
320 return;
321 }
322
323 scmi_write_response(msg, &out_args, sizeof(out_args));
324 }
325
scmi_voltd_level_set(struct scmi_msg * msg)326 static void scmi_voltd_level_set(struct scmi_msg *msg)
327 {
328 const struct scmi_voltd_level_set_a2p *in_args = (void *)msg->in;
329 int32_t status = SCMI_GENERIC_ERROR;
330 unsigned int domain_id = 0;
331
332 if (msg->in_size != sizeof(*in_args)) {
333 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
334 return;
335 }
336
337 if (in_args->domain_id >= plat_scmi_voltd_count(msg->channel_id)) {
338 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
339 return;
340 }
341
342 domain_id = confine_array_index(in_args->domain_id,
343 plat_scmi_voltd_count(msg->channel_id));
344
345 status = plat_scmi_voltd_set_level(msg->channel_id, domain_id,
346 in_args->voltage_level);
347
348 scmi_status_response(msg, status);
349 }
350
scmi_voltd_level_get(struct scmi_msg * msg)351 static void scmi_voltd_level_get(struct scmi_msg *msg)
352 {
353 const struct scmi_voltd_level_get_a2p *in_args = (void *)msg->in;
354 struct scmi_voltd_level_get_p2a out_args = {
355 .status = SCMI_SUCCESS,
356 };
357 unsigned int domain_id = 0;
358
359 if (msg->in_size != sizeof(*in_args)) {
360 scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
361 return;
362 }
363
364 if (in_args->domain_id >= plat_scmi_voltd_count(msg->channel_id)) {
365 scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
366 return;
367 }
368
369 domain_id = confine_array_index(in_args->domain_id,
370 plat_scmi_voltd_count(msg->channel_id));
371
372 out_args.voltage_level = plat_scmi_voltd_get_level(msg->channel_id,
373 domain_id);
374
375 scmi_write_response(msg, &out_args, sizeof(out_args));
376 }
377
378 static const scmi_msg_handler_t handler_array[] = {
379 [SCMI_PROTOCOL_VERSION] = report_version,
380 [SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
381 [SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
382 [SCMI_VOLTAGE_DOMAIN_ATTRIBUTES] = scmi_voltd_domain_attributes,
383 [SCMI_VOLTAGE_DESCRIBE_LEVELS] = scmi_voltd_describe_levels,
384 [SCMI_VOLTAGE_CONFIG_SET] = scmi_voltd_config_set,
385 [SCMI_VOLTAGE_CONFIG_GET] = scmi_voltd_config_get,
386 [SCMI_VOLTAGE_LEVEL_SET] = scmi_voltd_level_set,
387 [SCMI_VOLTAGE_LEVEL_GET] = scmi_voltd_level_get,
388 };
389
message_id_is_supported(size_t id)390 static bool message_id_is_supported(size_t id)
391 {
392 return id < ARRAY_SIZE(handler_array) && handler_array[id];
393 }
394
scmi_msg_get_voltd_handler(struct scmi_msg * msg)395 scmi_msg_handler_t scmi_msg_get_voltd_handler(struct scmi_msg *msg)
396 {
397 const size_t array_size = ARRAY_SIZE(handler_array);
398 unsigned int message_id = 0;
399
400 if (msg->message_id >= array_size) {
401 DMSG("Voltage domain handle not found %u", msg->message_id);
402 return NULL;
403 }
404
405 message_id = confine_array_index(msg->message_id, array_size);
406
407 return handler_array[message_id];
408 }
409