1
2
3
4
5
6#define LOG_CATEGORY UCLASS_SCMI_AGENT
7
8#include <common.h>
9#include <dm.h>
10#include <malloc.h>
11#include <scmi_agent.h>
12#include <scmi_agent-uclass.h>
13#include <scmi_protocols.h>
14#include <asm/io.h>
15#include <asm/scmi_test.h>
16#include <dm/device_compat.h>
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40#define SANDBOX_SCMI_AGENT_COUNT 2
41
42static struct sandbox_scmi_clk scmi0_clk[] = {
43 { .id = 7, .rate = 1000 },
44 { .id = 3, .rate = 333 },
45};
46
47static struct sandbox_scmi_reset scmi0_reset[] = {
48 { .id = 3 },
49};
50
51static struct sandbox_scmi_voltd scmi0_voltd[] = {
52 { .id = 0, .voltage_uv = 3300000 },
53 { .id = 1, .voltage_uv = 1800000 },
54};
55
56static struct sandbox_scmi_clk scmi1_clk[] = {
57 { .id = 1, .rate = 44 },
58};
59
60
61struct sandbox_scmi_agent *sandbox_scmi_agent_list[SANDBOX_SCMI_AGENT_COUNT];
62
63static struct sandbox_scmi_service sandbox_scmi_service_state = {
64 .agent = sandbox_scmi_agent_list,
65 .agent_count = SANDBOX_SCMI_AGENT_COUNT,
66};
67
68struct sandbox_scmi_service *sandbox_scmi_service_ctx(void)
69{
70 return &sandbox_scmi_service_state;
71}
72
73static void debug_print_agent_state(struct udevice *dev, char *str)
74{
75 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
76
77 dev_dbg(dev, "Dump sandbox_scmi_agent %u: %s\n", agent->idx, str);
78 dev_dbg(dev, " scmi%u_clk (%zu): %d/%ld, %d/%ld, %d/%ld, ...\n",
79 agent->idx,
80 agent->clk_count,
81 agent->clk_count ? agent->clk[0].enabled : -1,
82 agent->clk_count ? agent->clk[0].rate : -1,
83 agent->clk_count > 1 ? agent->clk[1].enabled : -1,
84 agent->clk_count > 1 ? agent->clk[1].rate : -1,
85 agent->clk_count > 2 ? agent->clk[2].enabled : -1,
86 agent->clk_count > 2 ? agent->clk[2].rate : -1);
87 dev_dbg(dev, " scmi%u_reset (%zu): %d, %d, ...\n",
88 agent->idx,
89 agent->reset_count,
90 agent->reset_count ? agent->reset[0].asserted : -1,
91 agent->reset_count > 1 ? agent->reset[1].asserted : -1);
92 dev_dbg(dev, " scmi%u_voltd (%zu): %u/%d, %u/%d, ...\n",
93 agent->idx,
94 agent->voltd_count,
95 agent->voltd_count ? agent->voltd[0].enabled : -1,
96 agent->voltd_count ? agent->voltd[0].voltage_uv : -1,
97 agent->voltd_count ? agent->voltd[1].enabled : -1,
98 agent->voltd_count ? agent->voltd[1].voltage_uv : -1);
99};
100
101static struct sandbox_scmi_clk *get_scmi_clk_state(uint agent_id, uint clock_id)
102{
103 struct sandbox_scmi_clk *target = NULL;
104 size_t target_count = 0;
105 size_t n;
106
107 switch (agent_id) {
108 case 0:
109 target = scmi0_clk;
110 target_count = ARRAY_SIZE(scmi0_clk);
111 break;
112 case 1:
113 target = scmi1_clk;
114 target_count = ARRAY_SIZE(scmi1_clk);
115 break;
116 default:
117 return NULL;
118 }
119
120 for (n = 0; n < target_count; n++)
121 if (target[n].id == clock_id)
122 return target + n;
123
124 return NULL;
125}
126
127static struct sandbox_scmi_reset *get_scmi_reset_state(uint agent_id,
128 uint reset_id)
129{
130 size_t n;
131
132 if (agent_id == 0) {
133 for (n = 0; n < ARRAY_SIZE(scmi0_reset); n++)
134 if (scmi0_reset[n].id == reset_id)
135 return scmi0_reset + n;
136 }
137
138 return NULL;
139}
140
141static struct sandbox_scmi_voltd *get_scmi_voltd_state(uint agent_id,
142 uint domain_id)
143{
144 size_t n;
145
146 if (agent_id == 0) {
147 for (n = 0; n < ARRAY_SIZE(scmi0_voltd); n++)
148 if (scmi0_voltd[n].id == domain_id)
149 return scmi0_voltd + n;
150 }
151
152 return NULL;
153}
154
155
156
157
158
159static int sandbox_scmi_clock_rate_set(struct udevice *dev,
160 struct scmi_msg *msg)
161{
162 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
163 struct scmi_clk_rate_set_in *in = NULL;
164 struct scmi_clk_rate_set_out *out = NULL;
165 struct sandbox_scmi_clk *clk_state = NULL;
166
167 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
168 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
169 return -EINVAL;
170
171 in = (struct scmi_clk_rate_set_in *)msg->in_msg;
172 out = (struct scmi_clk_rate_set_out *)msg->out_msg;
173
174 clk_state = get_scmi_clk_state(agent->idx, in->clock_id);
175 if (!clk_state) {
176 dev_err(dev, "Unexpected clock ID %u\n", in->clock_id);
177
178 out->status = SCMI_NOT_FOUND;
179 } else {
180 u64 rate = ((u64)in->rate_msb << 32) + in->rate_lsb;
181
182 clk_state->rate = (ulong)rate;
183
184 out->status = SCMI_SUCCESS;
185 }
186
187 return 0;
188}
189
190static int sandbox_scmi_clock_rate_get(struct udevice *dev,
191 struct scmi_msg *msg)
192{
193 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
194 struct scmi_clk_rate_get_in *in = NULL;
195 struct scmi_clk_rate_get_out *out = NULL;
196 struct sandbox_scmi_clk *clk_state = NULL;
197
198 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
199 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
200 return -EINVAL;
201
202 in = (struct scmi_clk_rate_get_in *)msg->in_msg;
203 out = (struct scmi_clk_rate_get_out *)msg->out_msg;
204
205 clk_state = get_scmi_clk_state(agent->idx, in->clock_id);
206 if (!clk_state) {
207 dev_err(dev, "Unexpected clock ID %u\n", in->clock_id);
208
209 out->status = SCMI_NOT_FOUND;
210 } else {
211 out->rate_msb = (u32)((u64)clk_state->rate >> 32);
212 out->rate_lsb = (u32)clk_state->rate;
213
214 out->status = SCMI_SUCCESS;
215 }
216
217 return 0;
218}
219
220static int sandbox_scmi_clock_gate(struct udevice *dev, struct scmi_msg *msg)
221{
222 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
223 struct scmi_clk_state_in *in = NULL;
224 struct scmi_clk_state_out *out = NULL;
225 struct sandbox_scmi_clk *clk_state = NULL;
226
227 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
228 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
229 return -EINVAL;
230
231 in = (struct scmi_clk_state_in *)msg->in_msg;
232 out = (struct scmi_clk_state_out *)msg->out_msg;
233
234 clk_state = get_scmi_clk_state(agent->idx, in->clock_id);
235 if (!clk_state) {
236 dev_err(dev, "Unexpected clock ID %u\n", in->clock_id);
237
238 out->status = SCMI_NOT_FOUND;
239 } else if (in->attributes > 1) {
240 out->status = SCMI_PROTOCOL_ERROR;
241 } else {
242 clk_state->enabled = in->attributes;
243
244 out->status = SCMI_SUCCESS;
245 }
246
247 return 0;
248}
249
250static int sandbox_scmi_rd_attribs(struct udevice *dev, struct scmi_msg *msg)
251{
252 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
253 struct scmi_rd_attr_in *in = NULL;
254 struct scmi_rd_attr_out *out = NULL;
255 struct sandbox_scmi_reset *reset_state = NULL;
256
257 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
258 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
259 return -EINVAL;
260
261 in = (struct scmi_rd_attr_in *)msg->in_msg;
262 out = (struct scmi_rd_attr_out *)msg->out_msg;
263
264 reset_state = get_scmi_reset_state(agent->idx, in->domain_id);
265 if (!reset_state) {
266 dev_err(dev, "Unexpected reset domain ID %u\n", in->domain_id);
267
268 out->status = SCMI_NOT_FOUND;
269 } else {
270 memset(out, 0, sizeof(*out));
271 snprintf(out->name, sizeof(out->name), "rd%u", in->domain_id);
272
273 out->status = SCMI_SUCCESS;
274 }
275
276 return 0;
277}
278
279static int sandbox_scmi_rd_reset(struct udevice *dev, struct scmi_msg *msg)
280{
281 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
282 struct scmi_rd_reset_in *in = NULL;
283 struct scmi_rd_reset_out *out = NULL;
284 struct sandbox_scmi_reset *reset_state = NULL;
285
286 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
287 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
288 return -EINVAL;
289
290 in = (struct scmi_rd_reset_in *)msg->in_msg;
291 out = (struct scmi_rd_reset_out *)msg->out_msg;
292
293 reset_state = get_scmi_reset_state(agent->idx, in->domain_id);
294 if (!reset_state) {
295 dev_err(dev, "Unexpected reset domain ID %u\n", in->domain_id);
296
297 out->status = SCMI_NOT_FOUND;
298 } else if (in->reset_state > 1) {
299 dev_err(dev, "Invalid reset domain input attribute value\n");
300
301 out->status = SCMI_INVALID_PARAMETERS;
302 } else {
303 if (in->flags & SCMI_RD_RESET_FLAG_CYCLE) {
304 if (in->flags & SCMI_RD_RESET_FLAG_ASYNC) {
305 out->status = SCMI_NOT_SUPPORTED;
306 } else {
307
308 reset_state->asserted = false;
309 out->status = SCMI_SUCCESS;
310 }
311 } else {
312 reset_state->asserted = in->flags &
313 SCMI_RD_RESET_FLAG_ASSERT;
314
315 out->status = SCMI_SUCCESS;
316 }
317 }
318
319 return 0;
320}
321
322static int sandbox_scmi_voltd_attribs(struct udevice *dev, struct scmi_msg *msg)
323{
324 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
325 struct scmi_voltd_attr_in *in = NULL;
326 struct scmi_voltd_attr_out *out = NULL;
327 struct sandbox_scmi_voltd *voltd_state = NULL;
328
329 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
330 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
331 return -EINVAL;
332
333 in = (struct scmi_voltd_attr_in *)msg->in_msg;
334 out = (struct scmi_voltd_attr_out *)msg->out_msg;
335
336 voltd_state = get_scmi_voltd_state(agent->idx, in->domain_id);
337 if (!voltd_state) {
338 dev_err(dev, "Unexpected domain ID %u\n", in->domain_id);
339
340 out->status = SCMI_NOT_FOUND;
341 } else {
342 memset(out, 0, sizeof(*out));
343 snprintf(out->name, sizeof(out->name), "regu%u", in->domain_id);
344
345 out->status = SCMI_SUCCESS;
346 }
347
348 return 0;
349}
350
351static int sandbox_scmi_voltd_config_set(struct udevice *dev,
352 struct scmi_msg *msg)
353{
354 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
355 struct scmi_voltd_config_set_in *in = NULL;
356 struct scmi_voltd_config_set_out *out = NULL;
357 struct sandbox_scmi_voltd *voltd_state = NULL;
358
359 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
360 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
361 return -EINVAL;
362
363 in = (struct scmi_voltd_config_set_in *)msg->in_msg;
364 out = (struct scmi_voltd_config_set_out *)msg->out_msg;
365
366 voltd_state = get_scmi_voltd_state(agent->idx, in->domain_id);
367 if (!voltd_state) {
368 dev_err(dev, "Unexpected domain ID %u\n", in->domain_id);
369
370 out->status = SCMI_NOT_FOUND;
371 } else if (in->config & ~SCMI_VOLTD_CONFIG_MASK) {
372 dev_err(dev, "Invalid config value 0x%x\n", in->config);
373
374 out->status = SCMI_INVALID_PARAMETERS;
375 } else if (in->config != SCMI_VOLTD_CONFIG_ON &&
376 in->config != SCMI_VOLTD_CONFIG_OFF) {
377 dev_err(dev, "Unexpected custom value 0x%x\n", in->config);
378
379 out->status = SCMI_INVALID_PARAMETERS;
380 } else {
381 voltd_state->enabled = in->config == SCMI_VOLTD_CONFIG_ON;
382 out->status = SCMI_SUCCESS;
383 }
384
385 return 0;
386}
387
388static int sandbox_scmi_voltd_config_get(struct udevice *dev,
389 struct scmi_msg *msg)
390{
391 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
392 struct scmi_voltd_config_get_in *in = NULL;
393 struct scmi_voltd_config_get_out *out = NULL;
394 struct sandbox_scmi_voltd *voltd_state = NULL;
395
396 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
397 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
398 return -EINVAL;
399
400 in = (struct scmi_voltd_config_get_in *)msg->in_msg;
401 out = (struct scmi_voltd_config_get_out *)msg->out_msg;
402
403 voltd_state = get_scmi_voltd_state(agent->idx, in->domain_id);
404 if (!voltd_state) {
405 dev_err(dev, "Unexpected domain ID %u\n", in->domain_id);
406
407 out->status = SCMI_NOT_FOUND;
408 } else {
409 if (voltd_state->enabled)
410 out->config = SCMI_VOLTD_CONFIG_ON;
411 else
412 out->config = SCMI_VOLTD_CONFIG_OFF;
413
414 out->status = SCMI_SUCCESS;
415 }
416
417 return 0;
418}
419
420static int sandbox_scmi_voltd_level_set(struct udevice *dev,
421 struct scmi_msg *msg)
422{
423 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
424 struct scmi_voltd_level_set_in *in = NULL;
425 struct scmi_voltd_level_set_out *out = NULL;
426 struct sandbox_scmi_voltd *voltd_state = NULL;
427
428 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
429 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
430 return -EINVAL;
431
432 in = (struct scmi_voltd_level_set_in *)msg->in_msg;
433 out = (struct scmi_voltd_level_set_out *)msg->out_msg;
434
435 voltd_state = get_scmi_voltd_state(agent->idx, in->domain_id);
436 if (!voltd_state) {
437 dev_err(dev, "Unexpected domain ID %u\n", in->domain_id);
438
439 out->status = SCMI_NOT_FOUND;
440 } else {
441 voltd_state->voltage_uv = in->voltage_level;
442 out->status = SCMI_SUCCESS;
443 }
444
445 return 0;
446}
447
448static int sandbox_scmi_voltd_level_get(struct udevice *dev,
449 struct scmi_msg *msg)
450{
451 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
452 struct scmi_voltd_level_get_in *in = NULL;
453 struct scmi_voltd_level_get_out *out = NULL;
454 struct sandbox_scmi_voltd *voltd_state = NULL;
455
456 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
457 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
458 return -EINVAL;
459
460 in = (struct scmi_voltd_level_get_in *)msg->in_msg;
461 out = (struct scmi_voltd_level_get_out *)msg->out_msg;
462
463 voltd_state = get_scmi_voltd_state(agent->idx, in->domain_id);
464 if (!voltd_state) {
465 dev_err(dev, "Unexpected domain ID %u\n", in->domain_id);
466
467 out->status = SCMI_NOT_FOUND;
468 } else {
469 out->voltage_level = voltd_state->voltage_uv;
470 out->status = SCMI_SUCCESS;
471 }
472
473 return 0;
474}
475
476static int sandbox_scmi_test_process_msg(struct udevice *dev,
477 struct scmi_msg *msg)
478{
479 switch (msg->protocol_id) {
480 case SCMI_PROTOCOL_ID_CLOCK:
481 switch (msg->message_id) {
482 case SCMI_CLOCK_RATE_SET:
483 return sandbox_scmi_clock_rate_set(dev, msg);
484 case SCMI_CLOCK_RATE_GET:
485 return sandbox_scmi_clock_rate_get(dev, msg);
486 case SCMI_CLOCK_CONFIG_SET:
487 return sandbox_scmi_clock_gate(dev, msg);
488 default:
489 break;
490 }
491 break;
492 case SCMI_PROTOCOL_ID_RESET_DOMAIN:
493 switch (msg->message_id) {
494 case SCMI_RESET_DOMAIN_ATTRIBUTES:
495 return sandbox_scmi_rd_attribs(dev, msg);
496 case SCMI_RESET_DOMAIN_RESET:
497 return sandbox_scmi_rd_reset(dev, msg);
498 default:
499 break;
500 }
501 break;
502 case SCMI_PROTOCOL_ID_VOLTAGE_DOMAIN:
503 switch (msg->message_id) {
504 case SCMI_VOLTAGE_DOMAIN_ATTRIBUTES:
505 return sandbox_scmi_voltd_attribs(dev, msg);
506 case SCMI_VOLTAGE_DOMAIN_CONFIG_SET:
507 return sandbox_scmi_voltd_config_set(dev, msg);
508 case SCMI_VOLTAGE_DOMAIN_CONFIG_GET:
509 return sandbox_scmi_voltd_config_get(dev, msg);
510 case SCMI_VOLTAGE_DOMAIN_LEVEL_SET:
511 return sandbox_scmi_voltd_level_set(dev, msg);
512 case SCMI_VOLTAGE_DOMAIN_LEVEL_GET:
513 return sandbox_scmi_voltd_level_get(dev, msg);
514 default:
515 break;
516 }
517 break;
518 case SCMI_PROTOCOL_ID_BASE:
519 case SCMI_PROTOCOL_ID_POWER_DOMAIN:
520 case SCMI_PROTOCOL_ID_SYSTEM:
521 case SCMI_PROTOCOL_ID_PERF:
522 case SCMI_PROTOCOL_ID_SENSOR:
523 *(u32 *)msg->out_msg = SCMI_NOT_SUPPORTED;
524 return 0;
525 default:
526 break;
527 }
528
529 dev_err(dev, "%s(%s): Unhandled protocol_id %#x/message_id %#x\n",
530 __func__, dev->name, msg->protocol_id, msg->message_id);
531
532 if (msg->out_msg_sz < sizeof(u32))
533 return -EINVAL;
534
535
536 *(u32 *)msg->out_msg = SCMI_PROTOCOL_ERROR;
537 return 0;
538}
539
540static int sandbox_scmi_test_remove(struct udevice *dev)
541{
542 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
543
544 debug_print_agent_state(dev, "removed");
545
546
547 sandbox_scmi_service_ctx()->agent[agent->idx] = NULL;
548
549 return 0;
550}
551
552static int sandbox_scmi_test_probe(struct udevice *dev)
553{
554 static const char basename[] = "sandbox-scmi-agent@";
555 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
556 const size_t basename_size = sizeof(basename) - 1;
557
558 if (strncmp(basename, dev->name, basename_size))
559 return -ENOENT;
560
561 switch (dev->name[basename_size]) {
562 case '0':
563 *agent = (struct sandbox_scmi_agent){
564 .idx = 0,
565 .clk = scmi0_clk,
566 .clk_count = ARRAY_SIZE(scmi0_clk),
567 .reset = scmi0_reset,
568 .reset_count = ARRAY_SIZE(scmi0_reset),
569 .voltd = scmi0_voltd,
570 .voltd_count = ARRAY_SIZE(scmi0_voltd),
571 };
572 break;
573 case '1':
574 *agent = (struct sandbox_scmi_agent){
575 .idx = 1,
576 .clk = scmi1_clk,
577 .clk_count = ARRAY_SIZE(scmi1_clk),
578 };
579 break;
580 default:
581 dev_err(dev, "%s(): Unexpected agent ID %s\n",
582 __func__, dev->name + basename_size);
583 return -ENOENT;
584 }
585
586 debug_print_agent_state(dev, "probed");
587
588
589 sandbox_scmi_service_ctx()->agent[agent->idx] = agent;
590
591 return 0;
592};
593
594static const struct udevice_id sandbox_scmi_test_ids[] = {
595 { .compatible = "sandbox,scmi-agent" },
596 { }
597};
598
599struct scmi_agent_ops sandbox_scmi_test_ops = {
600 .process_msg = sandbox_scmi_test_process_msg,
601};
602
603U_BOOT_DRIVER(sandbox_scmi_agent) = {
604 .name = "sandbox-scmi_agent",
605 .id = UCLASS_SCMI_AGENT,
606 .of_match = sandbox_scmi_test_ids,
607 .priv_auto = sizeof(struct sandbox_scmi_agent),
608 .probe = sandbox_scmi_test_probe,
609 .remove = sandbox_scmi_test_remove,
610 .ops = &sandbox_scmi_test_ops,
611};
612
