drivers/firmware/scmi/sandbox-scmi_agent.c

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