+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;
+}
+