firmware: scmi: sandbox test for SCMI clocks

Add tests for SCMI clocks. A test device driver sandbox-scmi_devices.c
is used to get clock resources, allowing further clock manipulation.

Change sandbox-smci_agent to emulate 3 clocks exposed through 2 agents.
Add DM test scmi_clocks to test these 3 clocks.
Update DM test sandbox_scmi_agent with load/remove test sequences
factorized by {load|remove}_sandbox_scmi_test_devices() helper functions.

Signed-off-by: Etienne Carriere <etienne.carriere@linaro.org>
Cc: Simon Glass <sjg@chromium.org>
Cc: Peng Fan <peng.fan@nxp.com>
Cc: Sudeep Holla <sudeep.holla@arm.com>
Reviewed-by: Simon Glass <sjg@chromium.org>

3 files changed, 241 insertions, 5 deletions

diff --git a/drivers/firmware/scmi/Makefile b/drivers/firmware/scmi/Makefile
index 2f782bb..e1e0224 100644
--- a/drivers/firmware/scmi/Makefile
+++ b/drivers/firmware/scmi/Makefile
@@ -2,4 +2,4 @@ obj-y	+= scmi_agent-uclass.o
 obj-y	+= smt.o
 obj-$(CONFIG_ARM_SMCCC) 	+= smccc_agent.o
 obj-$(CONFIG_DM_MAILBOX)	+= mailbox_agent.o
-obj-$(CONFIG_SANDBOX)		+= sandbox-scmi_agent.o
+obj-$(CONFIG_SANDBOX)		+= sandbox-scmi_agent.o sandbox-scmi_devices.o
diff --git a/drivers/firmware/scmi/sandbox-scmi_agent.c b/drivers/firmware/scmi/sandbox-scmi_agent.c
index 3179438..ff59098 100644
--- a/drivers/firmware/scmi/sandbox-scmi_agent.c
+++ b/drivers/firmware/scmi/sandbox-scmi_agent.c
@@ -16,18 +16,34 @@
 /*
  * 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 none.
+ * SCMI protocols embedded in U-Boot. Currently:
+ * - SCMI clock protocol: emulate 2 agents each exposing few clocks
  *
- * This driver simulates 2 SCMI agents for test purpose.
+ * Agent #0 simulates 2 clocks.
+ * See IDs in scmi0_clk[] 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.
  *
  * 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, currently nothing.
+ * various uclass devices, currently only clock 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_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];
 
@@ -46,17 +62,158 @@ 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);
 };
 
+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;
+}
+
+/*
+ * 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_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_BASE:
 	case SCMI_PROTOCOL_ID_POWER_DOMAIN:
 	case SCMI_PROTOCOL_ID_SYSTEM:
 	case SCMI_PROTOCOL_ID_PERF:
-	case SCMI_PROTOCOL_ID_CLOCK:
 	case SCMI_PROTOCOL_ID_SENSOR:
 	case SCMI_PROTOCOL_ID_RESET_DOMAIN:
 		*(u32 *)msg->out_msg = SCMI_NOT_SUPPORTED;
@@ -101,11 +258,15 @@ static int sandbox_scmi_test_probe(struct udevice *dev)
 	case '0':
 		*agent = (struct sandbox_scmi_agent){
 			.idx = 0,
+			.clk = scmi0_clk,
+			.clk_count = ARRAY_SIZE(scmi0_clk),
 		};
 		break;
 	case '1':
 		*agent = (struct sandbox_scmi_agent){
 			.idx = 1,
+			.clk = scmi1_clk,
+			.clk_count = ARRAY_SIZE(scmi1_clk),
 		};
 		break;
 	default:
diff --git a/drivers/firmware/scmi/sandbox-scmi_devices.c b/drivers/firmware/scmi/sandbox-scmi_devices.c
new file mode 100644
index 0000000..b3e411c
--- /dev/null
+++ b/drivers/firmware/scmi/sandbox-scmi_devices.c
@@ -0,0 +1,75 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020, Linaro Limited
+ */
+
+#include <common.h>
+#include <clk.h>
+#include <dm.h>
+#include <malloc.h>
+#include <asm/io.h>
+#include <asm/scmi_test.h>
+#include <dm/device_compat.h>
+
+/*
+ * Simulate to some extent a SCMI exchange.
+ * This drivers gets SCMI resources and offers API function to the
+ * SCMI test sequence manipulate the resources, currently clocks.
+ */
+
+#define SCMI_TEST_DEVICES_CLK_COUNT		3
+
+/*
+ * struct sandbox_scmi_device_priv - Storage for device handles used by test
+ * @clk:		Array of clock instances used by tests
+ * @devices:		Resources exposed by sandbox_scmi_devices_ctx()
+ */
+struct sandbox_scmi_device_priv {
+	struct clk clk[SCMI_TEST_DEVICES_CLK_COUNT];
+	struct sandbox_scmi_devices devices;
+};
+
+struct sandbox_scmi_devices *sandbox_scmi_devices_ctx(struct udevice *dev)
+{
+	struct sandbox_scmi_device_priv *priv = dev_get_priv(dev);
+
+	if (priv)
+		return &priv->devices;
+
+	return NULL;
+}
+
+static int sandbox_scmi_devices_probe(struct udevice *dev)
+{
+	struct sandbox_scmi_device_priv *priv = dev_get_priv(dev);
+	int ret;
+	size_t n;
+
+	priv->devices = (struct sandbox_scmi_devices){
+		.clk = priv->clk,
+		.clk_count = SCMI_TEST_DEVICES_CLK_COUNT,
+	};
+
+	for (n = 0; n < SCMI_TEST_DEVICES_CLK_COUNT; n++) {
+		ret = clk_get_by_index(dev, n, priv->devices.clk + n);
+		if (ret) {
+			dev_err(dev, "%s: Failed on clk %zu\n", __func__, n);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static const struct udevice_id sandbox_scmi_devices_ids[] = {
+	{ .compatible = "sandbox,scmi-devices" },
+	{ }
+};
+
+U_BOOT_DRIVER(sandbox_scmi_devices) = {
+	.name = "sandbox-scmi_devices",
+	.id = UCLASS_MISC,
+	.of_match = sandbox_scmi_devices_ids,
+	.priv_auto_alloc_size = sizeof(struct sandbox_scmi_device_priv),
+	.probe = sandbox_scmi_devices_probe,
+};