1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * Copyright (C) 2020, Linaro Limited
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 * The sandbox SCMI agent driver simulates to some extend a SCMI message
20 * processing. It simulates few of the SCMI services for some of the
21 * SCMI protocols embedded in U-Boot. Currently:
22 * - SCMI clock protocol: emulate 2 agents each exposing few clocks
23 * - SCMI reset protocol: emulate 1 agents each exposing a reset
24 *
25 * Agent #0 simulates 2 clocks and 1 reset domain.
26 * See IDs in scmi0_clk[]/scmi0_reset[] and "sandbox-scmi-agent@0" in test.dts.
27 *
28 * Agent #1 simulates 1 clock.
29 * See IDs in scmi1_clk[] and "sandbox-scmi-agent@1" in test.dts.
30 *
31 * All clocks are default disabled and reset levels down.
32 *
33 * This Driver exports sandbox_scmi_service_ct() for the test sequence to
34 * get the state of the simulated services (clock state, rate, ...) and
35 * check back-end device state reflects the request send through the
36 * various uclass devices, as clocks and reset controllers.
37 */
38
39 #define SANDBOX_SCMI_AGENT_COUNT 2
40
41 static struct sandbox_scmi_clk scmi0_clk[] = {
42 { .id = 7, .rate = 1000 },
43 { .id = 3, .rate = 333 },
44 };
45
46 static struct sandbox_scmi_reset scmi0_reset[] = {
47 { .id = 3 },
48 };
49
50 static struct sandbox_scmi_clk scmi1_clk[] = {
51 { .id = 1, .rate = 44 },
52 };
53
54 /* The list saves to simulted end devices references for test purpose */
55 struct sandbox_scmi_agent *sandbox_scmi_agent_list[SANDBOX_SCMI_AGENT_COUNT];
56
57 static struct sandbox_scmi_service sandbox_scmi_service_state = {
58 .agent = sandbox_scmi_agent_list,
59 .agent_count = SANDBOX_SCMI_AGENT_COUNT,
60 };
61
sandbox_scmi_service_ctx(void)62 struct sandbox_scmi_service *sandbox_scmi_service_ctx(void)
63 {
64 return &sandbox_scmi_service_state;
65 }
66
debug_print_agent_state(struct udevice * dev,char * str)67 static void debug_print_agent_state(struct udevice *dev, char *str)
68 {
69 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
70
71 dev_dbg(dev, "Dump sandbox_scmi_agent %u: %s\n", agent->idx, str);
72 dev_dbg(dev, " scmi%u_clk (%zu): %d/%ld, %d/%ld, %d/%ld, ...\n",
73 agent->idx,
74 agent->clk_count,
75 agent->clk_count ? agent->clk[0].enabled : -1,
76 agent->clk_count ? agent->clk[0].rate : -1,
77 agent->clk_count > 1 ? agent->clk[1].enabled : -1,
78 agent->clk_count > 1 ? agent->clk[1].rate : -1,
79 agent->clk_count > 2 ? agent->clk[2].enabled : -1,
80 agent->clk_count > 2 ? agent->clk[2].rate : -1);
81 dev_dbg(dev, " scmi%u_reset (%zu): %d, %d, ...\n",
82 agent->idx,
83 agent->reset_count,
84 agent->reset_count ? agent->reset[0].asserted : -1,
85 agent->reset_count > 1 ? agent->reset[1].asserted : -1);
86 };
87
get_scmi_clk_state(uint agent_id,uint clock_id)88 static struct sandbox_scmi_clk *get_scmi_clk_state(uint agent_id, uint clock_id)
89 {
90 struct sandbox_scmi_clk *target = NULL;
91 size_t target_count = 0;
92 size_t n;
93
94 switch (agent_id) {
95 case 0:
96 target = scmi0_clk;
97 target_count = ARRAY_SIZE(scmi0_clk);
98 break;
99 case 1:
100 target = scmi1_clk;
101 target_count = ARRAY_SIZE(scmi1_clk);
102 break;
103 default:
104 return NULL;
105 }
106
107 for (n = 0; n < target_count; n++)
108 if (target[n].id == clock_id)
109 return target + n;
110
111 return NULL;
112 }
113
get_scmi_reset_state(uint agent_id,uint reset_id)114 static struct sandbox_scmi_reset *get_scmi_reset_state(uint agent_id,
115 uint reset_id)
116 {
117 size_t n;
118
119 if (agent_id == 0) {
120 for (n = 0; n < ARRAY_SIZE(scmi0_reset); n++)
121 if (scmi0_reset[n].id == reset_id)
122 return scmi0_reset + n;
123 }
124
125 return NULL;
126 }
127
128 /*
129 * Sandbox SCMI agent ops
130 */
131
sandbox_scmi_clock_rate_set(struct udevice * dev,struct scmi_msg * msg)132 static int sandbox_scmi_clock_rate_set(struct udevice *dev,
133 struct scmi_msg *msg)
134 {
135 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
136 struct scmi_clk_rate_set_in *in = NULL;
137 struct scmi_clk_rate_set_out *out = NULL;
138 struct sandbox_scmi_clk *clk_state = NULL;
139
140 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
141 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
142 return -EINVAL;
143
144 in = (struct scmi_clk_rate_set_in *)msg->in_msg;
145 out = (struct scmi_clk_rate_set_out *)msg->out_msg;
146
147 clk_state = get_scmi_clk_state(agent->idx, in->clock_id);
148 if (!clk_state) {
149 dev_err(dev, "Unexpected clock ID %u\n", in->clock_id);
150
151 out->status = SCMI_NOT_FOUND;
152 } else {
153 u64 rate = ((u64)in->rate_msb << 32) + in->rate_lsb;
154
155 clk_state->rate = (ulong)rate;
156
157 out->status = SCMI_SUCCESS;
158 }
159
160 return 0;
161 }
162
sandbox_scmi_clock_rate_get(struct udevice * dev,struct scmi_msg * msg)163 static int sandbox_scmi_clock_rate_get(struct udevice *dev,
164 struct scmi_msg *msg)
165 {
166 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
167 struct scmi_clk_rate_get_in *in = NULL;
168 struct scmi_clk_rate_get_out *out = NULL;
169 struct sandbox_scmi_clk *clk_state = NULL;
170
171 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
172 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
173 return -EINVAL;
174
175 in = (struct scmi_clk_rate_get_in *)msg->in_msg;
176 out = (struct scmi_clk_rate_get_out *)msg->out_msg;
177
178 clk_state = get_scmi_clk_state(agent->idx, in->clock_id);
179 if (!clk_state) {
180 dev_err(dev, "Unexpected clock ID %u\n", in->clock_id);
181
182 out->status = SCMI_NOT_FOUND;
183 } else {
184 out->rate_msb = (u32)((u64)clk_state->rate >> 32);
185 out->rate_lsb = (u32)clk_state->rate;
186
187 out->status = SCMI_SUCCESS;
188 }
189
190 return 0;
191 }
192
sandbox_scmi_clock_gate(struct udevice * dev,struct scmi_msg * msg)193 static int sandbox_scmi_clock_gate(struct udevice *dev, struct scmi_msg *msg)
194 {
195 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
196 struct scmi_clk_state_in *in = NULL;
197 struct scmi_clk_state_out *out = NULL;
198 struct sandbox_scmi_clk *clk_state = NULL;
199
200 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
201 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
202 return -EINVAL;
203
204 in = (struct scmi_clk_state_in *)msg->in_msg;
205 out = (struct scmi_clk_state_out *)msg->out_msg;
206
207 clk_state = get_scmi_clk_state(agent->idx, in->clock_id);
208 if (!clk_state) {
209 dev_err(dev, "Unexpected clock ID %u\n", in->clock_id);
210
211 out->status = SCMI_NOT_FOUND;
212 } else if (in->attributes > 1) {
213 out->status = SCMI_PROTOCOL_ERROR;
214 } else {
215 clk_state->enabled = in->attributes;
216
217 out->status = SCMI_SUCCESS;
218 }
219
220 return 0;
221 }
222
sandbox_scmi_rd_attribs(struct udevice * dev,struct scmi_msg * msg)223 static int sandbox_scmi_rd_attribs(struct udevice *dev, struct scmi_msg *msg)
224 {
225 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
226 struct scmi_rd_attr_in *in = NULL;
227 struct scmi_rd_attr_out *out = NULL;
228 struct sandbox_scmi_reset *reset_state = NULL;
229
230 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
231 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
232 return -EINVAL;
233
234 in = (struct scmi_rd_attr_in *)msg->in_msg;
235 out = (struct scmi_rd_attr_out *)msg->out_msg;
236
237 reset_state = get_scmi_reset_state(agent->idx, in->domain_id);
238 if (!reset_state) {
239 dev_err(dev, "Unexpected reset domain ID %u\n", in->domain_id);
240
241 out->status = SCMI_NOT_FOUND;
242 } else {
243 memset(out, 0, sizeof(*out));
244 snprintf(out->name, sizeof(out->name), "rd%u", in->domain_id);
245
246 out->status = SCMI_SUCCESS;
247 }
248
249 return 0;
250 }
251
sandbox_scmi_rd_reset(struct udevice * dev,struct scmi_msg * msg)252 static int sandbox_scmi_rd_reset(struct udevice *dev, struct scmi_msg *msg)
253 {
254 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
255 struct scmi_rd_reset_in *in = NULL;
256 struct scmi_rd_reset_out *out = NULL;
257 struct sandbox_scmi_reset *reset_state = NULL;
258
259 if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
260 !msg->out_msg || msg->out_msg_sz < sizeof(*out))
261 return -EINVAL;
262
263 in = (struct scmi_rd_reset_in *)msg->in_msg;
264 out = (struct scmi_rd_reset_out *)msg->out_msg;
265
266 reset_state = get_scmi_reset_state(agent->idx, in->domain_id);
267 if (!reset_state) {
268 dev_err(dev, "Unexpected reset domain ID %u\n", in->domain_id);
269
270 out->status = SCMI_NOT_FOUND;
271 } else if (in->reset_state > 1) {
272 dev_err(dev, "Invalid reset domain input attribute value\n");
273
274 out->status = SCMI_INVALID_PARAMETERS;
275 } else {
276 if (in->flags & SCMI_RD_RESET_FLAG_CYCLE) {
277 if (in->flags & SCMI_RD_RESET_FLAG_ASYNC) {
278 out->status = SCMI_NOT_SUPPORTED;
279 } else {
280 /* Ends deasserted whatever current state */
281 reset_state->asserted = false;
282 out->status = SCMI_SUCCESS;
283 }
284 } else {
285 reset_state->asserted = in->flags &
286 SCMI_RD_RESET_FLAG_ASSERT;
287
288 out->status = SCMI_SUCCESS;
289 }
290 }
291
292 return 0;
293 }
294
sandbox_scmi_test_process_msg(struct udevice * dev,struct scmi_msg * msg)295 static int sandbox_scmi_test_process_msg(struct udevice *dev,
296 struct scmi_msg *msg)
297 {
298 switch (msg->protocol_id) {
299 case SCMI_PROTOCOL_ID_CLOCK:
300 switch (msg->message_id) {
301 case SCMI_CLOCK_RATE_SET:
302 return sandbox_scmi_clock_rate_set(dev, msg);
303 case SCMI_CLOCK_RATE_GET:
304 return sandbox_scmi_clock_rate_get(dev, msg);
305 case SCMI_CLOCK_CONFIG_SET:
306 return sandbox_scmi_clock_gate(dev, msg);
307 default:
308 break;
309 }
310 break;
311 case SCMI_PROTOCOL_ID_RESET_DOMAIN:
312 switch (msg->message_id) {
313 case SCMI_RESET_DOMAIN_ATTRIBUTES:
314 return sandbox_scmi_rd_attribs(dev, msg);
315 case SCMI_RESET_DOMAIN_RESET:
316 return sandbox_scmi_rd_reset(dev, msg);
317 default:
318 break;
319 }
320 break;
321 case SCMI_PROTOCOL_ID_BASE:
322 case SCMI_PROTOCOL_ID_POWER_DOMAIN:
323 case SCMI_PROTOCOL_ID_SYSTEM:
324 case SCMI_PROTOCOL_ID_PERF:
325 case SCMI_PROTOCOL_ID_SENSOR:
326 *(u32 *)msg->out_msg = SCMI_NOT_SUPPORTED;
327 return 0;
328 default:
329 break;
330 }
331
332 dev_err(dev, "%s(%s): Unhandled protocol_id %#x/message_id %#x\n",
333 __func__, dev->name, msg->protocol_id, msg->message_id);
334
335 if (msg->out_msg_sz < sizeof(u32))
336 return -EINVAL;
337
338 /* Intentionnaly report unhandled IDs through the SCMI return code */
339 *(u32 *)msg->out_msg = SCMI_PROTOCOL_ERROR;
340 return 0;
341 }
342
sandbox_scmi_test_remove(struct udevice * dev)343 static int sandbox_scmi_test_remove(struct udevice *dev)
344 {
345 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
346
347 debug_print_agent_state(dev, "removed");
348
349 /* We only need to dereference the agent in the context */
350 sandbox_scmi_service_ctx()->agent[agent->idx] = NULL;
351
352 return 0;
353 }
354
sandbox_scmi_test_probe(struct udevice * dev)355 static int sandbox_scmi_test_probe(struct udevice *dev)
356 {
357 static const char basename[] = "sandbox-scmi-agent@";
358 struct sandbox_scmi_agent *agent = dev_get_priv(dev);
359 const size_t basename_size = sizeof(basename) - 1;
360
361 if (strncmp(basename, dev->name, basename_size))
362 return -ENOENT;
363
364 switch (dev->name[basename_size]) {
365 case '0':
366 *agent = (struct sandbox_scmi_agent){
367 .idx = 0,
368 .clk = scmi0_clk,
369 .clk_count = ARRAY_SIZE(scmi0_clk),
370 .reset = scmi0_reset,
371 .reset_count = ARRAY_SIZE(scmi0_reset),
372 };
373 break;
374 case '1':
375 *agent = (struct sandbox_scmi_agent){
376 .idx = 1,
377 .clk = scmi1_clk,
378 .clk_count = ARRAY_SIZE(scmi1_clk),
379 };
380 break;
381 default:
382 dev_err(dev, "%s(): Unexpected agent ID %s\n",
383 __func__, dev->name + basename_size);
384 return -ENOENT;
385 }
386
387 debug_print_agent_state(dev, "probed");
388
389 /* Save reference for tests purpose */
390 sandbox_scmi_service_ctx()->agent[agent->idx] = agent;
391
392 return 0;
393 };
394
395 static const struct udevice_id sandbox_scmi_test_ids[] = {
396 { .compatible = "sandbox,scmi-agent" },
397 { }
398 };
399
400 struct scmi_agent_ops sandbox_scmi_test_ops = {
401 .process_msg = sandbox_scmi_test_process_msg,
402 };
403
404 U_BOOT_DRIVER(sandbox_scmi_agent) = {
405 .name = "sandbox-scmi_agent",
406 .id = UCLASS_SCMI_AGENT,
407 .of_match = sandbox_scmi_test_ids,
408 .priv_auto = sizeof(struct sandbox_scmi_agent),
409 .probe = sandbox_scmi_test_probe,
410 .remove = sandbox_scmi_test_remove,
411 .ops = &sandbox_scmi_test_ops,
412 };
413