1 files changed, 52 insertions, 1 deletions

diff --git a/drivers/firmware/scmi/smt.c b/drivers/firmware/scmi/smt.c
index e60c2aebc8..509ed618a9 100644
--- a/drivers/firmware/scmi/smt.c
+++ b/drivers/firmware/scmi/smt.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0
 /*
  * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
- * Copyright (C) 2019-2020 Linaro Limited.
+ * Copyright (C) 2019-2022 Linaro Limited.
  */
 
 #define LOG_CATEGORY UCLASS_SCMI_AGENT
@@ -137,3 +137,54 @@ void scmi_clear_smt_channel(struct scmi_smt *smt)
 
 	hdr->channel_status &= ~SCMI_SHMEM_CHAN_STAT_CHANNEL_ERROR;
 }
+
+/**
+ * Write SCMI message @msg into a SMT_MSG shared buffer @smt.
+ * Return 0 on success and with a negative errno in case of error.
+ */
+int scmi_msg_to_smt_msg(struct udevice *dev, struct scmi_smt *smt,
+			struct scmi_msg *msg, size_t *buf_size)
+{
+	struct scmi_smt_msg_header *hdr = (void *)smt->buf;
+
+	if ((!msg->in_msg && msg->in_msg_sz) ||
+	    (!msg->out_msg && msg->out_msg_sz))
+		return -EINVAL;
+
+	if (smt->size < (sizeof(*hdr) + msg->in_msg_sz) ||
+	    smt->size < (sizeof(*hdr) + msg->out_msg_sz)) {
+		dev_dbg(dev, "Buffer too small\n");
+		return -ETOOSMALL;
+	}
+
+	*buf_size = msg->in_msg_sz + sizeof(hdr->msg_header);
+
+	hdr->msg_header = SMT_HEADER_TOKEN(0) |
+			  SMT_HEADER_MESSAGE_TYPE(0) |
+			  SMT_HEADER_PROTOCOL_ID(msg->protocol_id) |
+			  SMT_HEADER_MESSAGE_ID(msg->message_id);
+
+	memcpy(hdr->msg_payload, msg->in_msg, msg->in_msg_sz);
+
+	return 0;
+}
+
+/**
+ * Read SCMI message from a SMT shared buffer @smt and copy it into @msg.
+ * Return 0 on success and with a negative errno in case of error.
+ */
+int scmi_msg_from_smt_msg(struct udevice *dev, struct scmi_smt *smt,
+			  struct scmi_msg *msg, size_t buf_size)
+{
+	struct scmi_smt_msg_header *hdr = (void *)smt->buf;
+
+	if (buf_size > msg->out_msg_sz + sizeof(hdr->msg_header)) {
+		dev_err(dev, "Buffer to small\n");
+		return -ETOOSMALL;
+	}
+
+	msg->out_msg_sz = buf_size - sizeof(hdr->msg_header);
+	memcpy(msg->out_msg, hdr->msg_payload, msg->out_msg_sz);
+
+	return 0;
+}