|
@@ -0,0 +1,410 @@
|
|
|
+// SPDX-License-Identifier: GPL-2.0+
|
|
|
+/*
|
|
|
+ * Copyright (C) 2020, Linaro Limited
|
|
|
+ */
|
|
|
+
|
|
|
+#include <common.h>
|
|
|
+#include <dm.h>
|
|
|
+#include <malloc.h>
|
|
|
+#include <scmi_agent.h>
|
|
|
+#include <scmi_agent-uclass.h>
|
|
|
+#include <scmi_protocols.h>
|
|
|
+#include <asm/io.h>
|
|
|
+#include <asm/scmi_test.h>
|
|
|
+#include <dm/device_compat.h>
|
|
|
+
|
|
|
+/*
|
|
|
+ * The sandbox SCMI agent driver simulates to some extend a SCMI message
|
|
|
+ * processing. It simulates few of the SCMI services for some of the
|
|
|
+ * SCMI protocols embedded in U-Boot. Currently:
|
|
|
+ * - SCMI clock protocol: emulate 2 agents each exposing few clocks
|
|
|
+ * - SCMI reset protocol: emulate 1 agents each exposing a reset
|
|
|
+ *
|
|
|
+ * Agent #0 simulates 2 clocks and 1 reset domain.
|
|
|
+ * See IDs in scmi0_clk[]/scmi0_reset[] and "sandbox-scmi-agent@0" in test.dts.
|
|
|
+ *
|
|
|
+ * Agent #1 simulates 1 clock.
|
|
|
+ * See IDs in scmi1_clk[] and "sandbox-scmi-agent@1" in test.dts.
|
|
|
+ *
|
|
|
+ * All clocks are default disabled and reset levels down.
|
|
|
+ *
|
|
|
+ * This Driver exports sandbox_scmi_service_ct() for the test sequence to
|
|
|
+ * get the state of the simulated services (clock state, rate, ...) and
|
|
|
+ * check back-end device state reflects the request send through the
|
|
|
+ * various uclass devices, as clocks and reset controllers.
|
|
|
+ */
|
|
|
+
|
|
|
+#define SANDBOX_SCMI_AGENT_COUNT 2
|
|
|
+
|
|
|
+static struct sandbox_scmi_clk scmi0_clk[] = {
|
|
|
+ { .id = 7, .rate = 1000 },
|
|
|
+ { .id = 3, .rate = 333 },
|
|
|
+};
|
|
|
+
|
|
|
+static struct sandbox_scmi_reset scmi0_reset[] = {
|
|
|
+ { .id = 3 },
|
|
|
+};
|
|
|
+
|
|
|
+static struct sandbox_scmi_clk scmi1_clk[] = {
|
|
|
+ { .id = 1, .rate = 44 },
|
|
|
+};
|
|
|
+
|
|
|
+/* The list saves to simulted end devices references for test purpose */
|
|
|
+struct sandbox_scmi_agent *sandbox_scmi_agent_list[SANDBOX_SCMI_AGENT_COUNT];
|
|
|
+
|
|
|
+static struct sandbox_scmi_service sandbox_scmi_service_state = {
|
|
|
+ .agent = sandbox_scmi_agent_list,
|
|
|
+ .agent_count = SANDBOX_SCMI_AGENT_COUNT,
|
|
|
+};
|
|
|
+
|
|
|
+struct sandbox_scmi_service *sandbox_scmi_service_ctx(void)
|
|
|
+{
|
|
|
+ return &sandbox_scmi_service_state;
|
|
|
+}
|
|
|
+
|
|
|
+static void debug_print_agent_state(struct udevice *dev, char *str)
|
|
|
+{
|
|
|
+ struct sandbox_scmi_agent *agent = dev_get_priv(dev);
|
|
|
+
|
|
|
+ dev_dbg(dev, "Dump sandbox_scmi_agent %u: %s\n", agent->idx, str);
|
|
|
+ dev_dbg(dev, " scmi%u_clk (%zu): %d/%ld, %d/%ld, %d/%ld, ...\n",
|
|
|
+ agent->idx,
|
|
|
+ agent->clk_count,
|
|
|
+ agent->clk_count ? agent->clk[0].enabled : -1,
|
|
|
+ agent->clk_count ? agent->clk[0].rate : -1,
|
|
|
+ agent->clk_count > 1 ? agent->clk[1].enabled : -1,
|
|
|
+ agent->clk_count > 1 ? agent->clk[1].rate : -1,
|
|
|
+ agent->clk_count > 2 ? agent->clk[2].enabled : -1,
|
|
|
+ agent->clk_count > 2 ? agent->clk[2].rate : -1);
|
|
|
+ dev_dbg(dev, " scmi%u_reset (%zu): %d, %d, ...\n",
|
|
|
+ agent->idx,
|
|
|
+ agent->reset_count,
|
|
|
+ agent->reset_count ? agent->reset[0].asserted : -1,
|
|
|
+ agent->reset_count > 1 ? agent->reset[1].asserted : -1);
|
|
|
+};
|
|
|
+
|
|
|
+static struct sandbox_scmi_clk *get_scmi_clk_state(uint agent_id, uint clock_id)
|
|
|
+{
|
|
|
+ struct sandbox_scmi_clk *target = NULL;
|
|
|
+ size_t target_count = 0;
|
|
|
+ size_t n;
|
|
|
+
|
|
|
+ switch (agent_id) {
|
|
|
+ case 0:
|
|
|
+ target = scmi0_clk;
|
|
|
+ target_count = ARRAY_SIZE(scmi0_clk);
|
|
|
+ break;
|
|
|
+ case 1:
|
|
|
+ target = scmi1_clk;
|
|
|
+ target_count = ARRAY_SIZE(scmi1_clk);
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ return NULL;
|
|
|
+ }
|
|
|
+
|
|
|
+ for (n = 0; n < target_count; n++)
|
|
|
+ if (target[n].id == clock_id)
|
|
|
+ return target + n;
|
|
|
+
|
|
|
+ return NULL;
|
|
|
+}
|
|
|
+
|
|
|
+static struct sandbox_scmi_reset *get_scmi_reset_state(uint agent_id,
|
|
|
+ uint reset_id)
|
|
|
+{
|
|
|
+ size_t n;
|
|
|
+
|
|
|
+ if (agent_id == 0) {
|
|
|
+ for (n = 0; n < ARRAY_SIZE(scmi0_reset); n++)
|
|
|
+ if (scmi0_reset[n].id == reset_id)
|
|
|
+ return scmi0_reset + n;
|
|
|
+ }
|
|
|
+
|
|
|
+ return NULL;
|
|
|
+}
|
|
|
+
|
|
|
+/*
|
|
|
+ * Sandbox SCMI agent ops
|
|
|
+ */
|
|
|
+
|
|
|
+static int sandbox_scmi_clock_rate_set(struct udevice *dev,
|
|
|
+ struct scmi_msg *msg)
|
|
|
+{
|
|
|
+ struct sandbox_scmi_agent *agent = dev_get_priv(dev);
|
|
|
+ struct scmi_clk_rate_set_in *in = NULL;
|
|
|
+ struct scmi_clk_rate_set_out *out = NULL;
|
|
|
+ struct sandbox_scmi_clk *clk_state = NULL;
|
|
|
+
|
|
|
+ if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
|
|
|
+ !msg->out_msg || msg->out_msg_sz < sizeof(*out))
|
|
|
+ return -EINVAL;
|
|
|
+
|
|
|
+ in = (struct scmi_clk_rate_set_in *)msg->in_msg;
|
|
|
+ out = (struct scmi_clk_rate_set_out *)msg->out_msg;
|
|
|
+
|
|
|
+ clk_state = get_scmi_clk_state(agent->idx, in->clock_id);
|
|
|
+ if (!clk_state) {
|
|
|
+ dev_err(dev, "Unexpected clock ID %u\n", in->clock_id);
|
|
|
+
|
|
|
+ out->status = SCMI_NOT_FOUND;
|
|
|
+ } else {
|
|
|
+ u64 rate = ((u64)in->rate_msb << 32) + in->rate_lsb;
|
|
|
+
|
|
|
+ clk_state->rate = (ulong)rate;
|
|
|
+
|
|
|
+ out->status = SCMI_SUCCESS;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+static int sandbox_scmi_clock_rate_get(struct udevice *dev,
|
|
|
+ struct scmi_msg *msg)
|
|
|
+{
|
|
|
+ struct sandbox_scmi_agent *agent = dev_get_priv(dev);
|
|
|
+ struct scmi_clk_rate_get_in *in = NULL;
|
|
|
+ struct scmi_clk_rate_get_out *out = NULL;
|
|
|
+ struct sandbox_scmi_clk *clk_state = NULL;
|
|
|
+
|
|
|
+ if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
|
|
|
+ !msg->out_msg || msg->out_msg_sz < sizeof(*out))
|
|
|
+ return -EINVAL;
|
|
|
+
|
|
|
+ in = (struct scmi_clk_rate_get_in *)msg->in_msg;
|
|
|
+ out = (struct scmi_clk_rate_get_out *)msg->out_msg;
|
|
|
+
|
|
|
+ clk_state = get_scmi_clk_state(agent->idx, in->clock_id);
|
|
|
+ if (!clk_state) {
|
|
|
+ dev_err(dev, "Unexpected clock ID %u\n", in->clock_id);
|
|
|
+
|
|
|
+ out->status = SCMI_NOT_FOUND;
|
|
|
+ } else {
|
|
|
+ out->rate_msb = (u32)((u64)clk_state->rate >> 32);
|
|
|
+ out->rate_lsb = (u32)clk_state->rate;
|
|
|
+
|
|
|
+ out->status = SCMI_SUCCESS;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+static int sandbox_scmi_clock_gate(struct udevice *dev, struct scmi_msg *msg)
|
|
|
+{
|
|
|
+ struct sandbox_scmi_agent *agent = dev_get_priv(dev);
|
|
|
+ struct scmi_clk_state_in *in = NULL;
|
|
|
+ struct scmi_clk_state_out *out = NULL;
|
|
|
+ struct sandbox_scmi_clk *clk_state = NULL;
|
|
|
+
|
|
|
+ if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
|
|
|
+ !msg->out_msg || msg->out_msg_sz < sizeof(*out))
|
|
|
+ return -EINVAL;
|
|
|
+
|
|
|
+ in = (struct scmi_clk_state_in *)msg->in_msg;
|
|
|
+ out = (struct scmi_clk_state_out *)msg->out_msg;
|
|
|
+
|
|
|
+ clk_state = get_scmi_clk_state(agent->idx, in->clock_id);
|
|
|
+ if (!clk_state) {
|
|
|
+ dev_err(dev, "Unexpected clock ID %u\n", in->clock_id);
|
|
|
+
|
|
|
+ out->status = SCMI_NOT_FOUND;
|
|
|
+ } else if (in->attributes > 1) {
|
|
|
+ out->status = SCMI_PROTOCOL_ERROR;
|
|
|
+ } else {
|
|
|
+ clk_state->enabled = in->attributes;
|
|
|
+
|
|
|
+ out->status = SCMI_SUCCESS;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+static int sandbox_scmi_rd_attribs(struct udevice *dev, struct scmi_msg *msg)
|
|
|
+{
|
|
|
+ struct sandbox_scmi_agent *agent = dev_get_priv(dev);
|
|
|
+ struct scmi_rd_attr_in *in = NULL;
|
|
|
+ struct scmi_rd_attr_out *out = NULL;
|
|
|
+ struct sandbox_scmi_reset *reset_state = NULL;
|
|
|
+
|
|
|
+ if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
|
|
|
+ !msg->out_msg || msg->out_msg_sz < sizeof(*out))
|
|
|
+ return -EINVAL;
|
|
|
+
|
|
|
+ in = (struct scmi_rd_attr_in *)msg->in_msg;
|
|
|
+ out = (struct scmi_rd_attr_out *)msg->out_msg;
|
|
|
+
|
|
|
+ reset_state = get_scmi_reset_state(agent->idx, in->domain_id);
|
|
|
+ if (!reset_state) {
|
|
|
+ dev_err(dev, "Unexpected reset domain ID %u\n", in->domain_id);
|
|
|
+
|
|
|
+ out->status = SCMI_NOT_FOUND;
|
|
|
+ } else {
|
|
|
+ memset(out, 0, sizeof(*out));
|
|
|
+ snprintf(out->name, sizeof(out->name), "rd%u", in->domain_id);
|
|
|
+
|
|
|
+ out->status = SCMI_SUCCESS;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+static int sandbox_scmi_rd_reset(struct udevice *dev, struct scmi_msg *msg)
|
|
|
+{
|
|
|
+ struct sandbox_scmi_agent *agent = dev_get_priv(dev);
|
|
|
+ struct scmi_rd_reset_in *in = NULL;
|
|
|
+ struct scmi_rd_reset_out *out = NULL;
|
|
|
+ struct sandbox_scmi_reset *reset_state = NULL;
|
|
|
+
|
|
|
+ if (!msg->in_msg || msg->in_msg_sz < sizeof(*in) ||
|
|
|
+ !msg->out_msg || msg->out_msg_sz < sizeof(*out))
|
|
|
+ return -EINVAL;
|
|
|
+
|
|
|
+ in = (struct scmi_rd_reset_in *)msg->in_msg;
|
|
|
+ out = (struct scmi_rd_reset_out *)msg->out_msg;
|
|
|
+
|
|
|
+ reset_state = get_scmi_reset_state(agent->idx, in->domain_id);
|
|
|
+ if (!reset_state) {
|
|
|
+ dev_err(dev, "Unexpected reset domain ID %u\n", in->domain_id);
|
|
|
+
|
|
|
+ out->status = SCMI_NOT_FOUND;
|
|
|
+ } else if (in->reset_state > 1) {
|
|
|
+ dev_err(dev, "Invalid reset domain input attribute value\n");
|
|
|
+
|
|
|
+ out->status = SCMI_INVALID_PARAMETERS;
|
|
|
+ } else {
|
|
|
+ if (in->flags & SCMI_RD_RESET_FLAG_CYCLE) {
|
|
|
+ if (in->flags & SCMI_RD_RESET_FLAG_ASYNC) {
|
|
|
+ out->status = SCMI_NOT_SUPPORTED;
|
|
|
+ } else {
|
|
|
+ /* Ends deasserted whatever current state */
|
|
|
+ reset_state->asserted = false;
|
|
|
+ out->status = SCMI_SUCCESS;
|
|
|
+ }
|
|
|
+ } else {
|
|
|
+ reset_state->asserted = in->flags &
|
|
|
+ SCMI_RD_RESET_FLAG_ASSERT;
|
|
|
+
|
|
|
+ out->status = SCMI_SUCCESS;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+static int sandbox_scmi_test_process_msg(struct udevice *dev,
|
|
|
+ struct scmi_msg *msg)
|
|
|
+{
|
|
|
+ switch (msg->protocol_id) {
|
|
|
+ case SCMI_PROTOCOL_ID_CLOCK:
|
|
|
+ switch (msg->message_id) {
|
|
|
+ case SCMI_CLOCK_RATE_SET:
|
|
|
+ return sandbox_scmi_clock_rate_set(dev, msg);
|
|
|
+ case SCMI_CLOCK_RATE_GET:
|
|
|
+ return sandbox_scmi_clock_rate_get(dev, msg);
|
|
|
+ case SCMI_CLOCK_CONFIG_SET:
|
|
|
+ return sandbox_scmi_clock_gate(dev, msg);
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ case SCMI_PROTOCOL_ID_RESET_DOMAIN:
|
|
|
+ switch (msg->message_id) {
|
|
|
+ case SCMI_RESET_DOMAIN_ATTRIBUTES:
|
|
|
+ return sandbox_scmi_rd_attribs(dev, msg);
|
|
|
+ case SCMI_RESET_DOMAIN_RESET:
|
|
|
+ return sandbox_scmi_rd_reset(dev, msg);
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ case SCMI_PROTOCOL_ID_BASE:
|
|
|
+ case SCMI_PROTOCOL_ID_POWER_DOMAIN:
|
|
|
+ case SCMI_PROTOCOL_ID_SYSTEM:
|
|
|
+ case SCMI_PROTOCOL_ID_PERF:
|
|
|
+ case SCMI_PROTOCOL_ID_SENSOR:
|
|
|
+ *(u32 *)msg->out_msg = SCMI_NOT_SUPPORTED;
|
|
|
+ return 0;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ dev_err(dev, "%s(%s): Unhandled protocol_id %#x/message_id %#x\n",
|
|
|
+ __func__, dev->name, msg->protocol_id, msg->message_id);
|
|
|
+
|
|
|
+ if (msg->out_msg_sz < sizeof(u32))
|
|
|
+ return -EINVAL;
|
|
|
+
|
|
|
+ /* Intentionnaly report unhandled IDs through the SCMI return code */
|
|
|
+ *(u32 *)msg->out_msg = SCMI_PROTOCOL_ERROR;
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+static int sandbox_scmi_test_remove(struct udevice *dev)
|
|
|
+{
|
|
|
+ struct sandbox_scmi_agent *agent = dev_get_priv(dev);
|
|
|
+
|
|
|
+ debug_print_agent_state(dev, "removed");
|
|
|
+
|
|
|
+ /* We only need to dereference the agent in the context */
|
|
|
+ sandbox_scmi_service_ctx()->agent[agent->idx] = NULL;
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+static int sandbox_scmi_test_probe(struct udevice *dev)
|
|
|
+{
|
|
|
+ static const char basename[] = "sandbox-scmi-agent@";
|
|
|
+ struct sandbox_scmi_agent *agent = dev_get_priv(dev);
|
|
|
+ const size_t basename_size = sizeof(basename) - 1;
|
|
|
+
|
|
|
+ if (strncmp(basename, dev->name, basename_size))
|
|
|
+ return -ENOENT;
|
|
|
+
|
|
|
+ switch (dev->name[basename_size]) {
|
|
|
+ case '0':
|
|
|
+ *agent = (struct sandbox_scmi_agent){
|
|
|
+ .idx = 0,
|
|
|
+ .clk = scmi0_clk,
|
|
|
+ .clk_count = ARRAY_SIZE(scmi0_clk),
|
|
|
+ .reset = scmi0_reset,
|
|
|
+ .reset_count = ARRAY_SIZE(scmi0_reset),
|
|
|
+ };
|
|
|
+ break;
|
|
|
+ case '1':
|
|
|
+ *agent = (struct sandbox_scmi_agent){
|
|
|
+ .idx = 1,
|
|
|
+ .clk = scmi1_clk,
|
|
|
+ .clk_count = ARRAY_SIZE(scmi1_clk),
|
|
|
+ };
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ dev_err(dev, "%s(): Unexpected agent ID %s\n",
|
|
|
+ __func__, dev->name + basename_size);
|
|
|
+ return -ENOENT;
|
|
|
+ }
|
|
|
+
|
|
|
+ debug_print_agent_state(dev, "probed");
|
|
|
+
|
|
|
+ /* Save reference for tests purpose */
|
|
|
+ sandbox_scmi_service_ctx()->agent[agent->idx] = agent;
|
|
|
+
|
|
|
+ return 0;
|
|
|
+};
|
|
|
+
|
|
|
+static const struct udevice_id sandbox_scmi_test_ids[] = {
|
|
|
+ { .compatible = "sandbox,scmi-agent" },
|
|
|
+ { }
|
|
|
+};
|
|
|
+
|
|
|
+struct scmi_agent_ops sandbox_scmi_test_ops = {
|
|
|
+ .process_msg = sandbox_scmi_test_process_msg,
|
|
|
+};
|
|
|
+
|
|
|
+U_BOOT_DRIVER(sandbox_scmi_agent) = {
|
|
|
+ .name = "sandbox-scmi_agent",
|
|
|
+ .id = UCLASS_SCMI_AGENT,
|
|
|
+ .of_match = sandbox_scmi_test_ids,
|
|
|
+ .priv_auto_alloc_size = sizeof(struct sandbox_scmi_agent),
|
|
|
+ .probe = sandbox_scmi_test_probe,
|
|
|
+ .remove = sandbox_scmi_test_remove,
|
|
|
+ .ops = &sandbox_scmi_test_ops,
|
|
|
+};
|