firmware: scmi: optee: use TEE shared memory for SCMI messages

Changes implementation when using TEE dynamically allocated shared
memory to synchronize with the Linux implementation where the legacy
SMT protocol cannot be used with such memory since it is expected from
device mapped memory whereas OP-TEE shared memory is cached and
hence should not be accessed using memcpy_toio()/memcpy_fromio().

This change implements the MSG shared memory protocol introduced
in Linux [1]. The protocol uses a simplified SMT header of 32bit
named MSG_SMT to carry SCMI protocol information and uses side channel
means to carry exchanged buffer size information, as TEE invocation API
parameters when used in the SCMI OP-TEE transport.

Link: [1] https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/commit/?id=f301bba0ca7392d16a6ea4f1d264a91f1fadea1a
Signed-off-by: Etienne Carriere <etienne.carriere@linaro.org>

3 files changed, 149 insertions, 17 deletions

diff --git a/drivers/firmware/scmi/optee_agent.c b/drivers/firmware/scmi/optee_agent.c
index 1f265922343..e76f738bbaf 100644
--- a/drivers/firmware/scmi/optee_agent.c
+++ b/drivers/firmware/scmi/optee_agent.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: GPL-2.0+
 /*
- * Copyright (C) 2020-2021 Linaro Limited.
+ * Copyright (C) 2020-2022 Linaro Limited.
  */
 
 #define LOG_CATEGORY UCLASS_SCMI_AGENT
@@ -98,6 +98,22 @@ enum optee_smci_pta_cmd {
 	 * [in]     value[0].b: Requested capabilities mask (enum pta_scmi_caps)
 	 */
 	PTA_SCMI_CMD_GET_CHANNEL = 3,
+
+	/*
+	 * PTA_SCMI_CMD_PROCESS_MSG_CHANNEL - Process SCMI message in MSG
+	 * buffers pointed by memref parameters
+	 *
+	 * [in]     value[0].a: Channel handle
+	 * [in]     memref[1]: Message buffer (MSG header and SCMI payload)
+	 * [out]    memref[2]: Response buffer (MSG header and SCMI payload)
+	 *
+	 * Shared memories used for SCMI message/response are MSG buffers
+	 * referenced by param[1] and param[2]. MSG transport protocol
+	 * uses a 32bit header to carry SCMI meta-data (protocol ID and
+	 * protocol message ID) followed by the effective SCMI message
+	 * payload.
+	 */
+	PTA_SCMI_CMD_PROCESS_MSG_CHANNEL = 4,
 };
 
 /*
@@ -106,9 +122,17 @@ enum optee_smci_pta_cmd {
  * PTA_SCMI_CAPS_SMT_HEADER
  * When set, OP-TEE supports command using SMT header protocol (SCMI shmem) in
  * shared memory buffers to carry SCMI protocol synchronisation information.
+ *
+ * PTA_SCMI_CAPS_MSG_HEADER
+ * When set, OP-TEE supports command using MSG header protocol in an OP-TEE
+ * shared memory to carry SCMI protocol synchronisation information and SCMI
+ * message payload.
  */
 #define PTA_SCMI_CAPS_NONE		0
 #define PTA_SCMI_CAPS_SMT_HEADER	BIT(0)
+#define PTA_SCMI_CAPS_MSG_HEADER	BIT(1)
+#define PTA_SCMI_CAPS_MASK		(PTA_SCMI_CAPS_SMT_HEADER | \
+					 PTA_SCMI_CAPS_MSG_HEADER)
 
 static int open_channel(struct udevice *dev, struct channel_session *sess)
 {
@@ -139,7 +163,10 @@ static int open_channel(struct udevice *dev, struct channel_session *sess)
 
 	param[0].attr = TEE_PARAM_ATTR_TYPE_VALUE_INOUT;
 	param[0].u.value.a = chan->channel_id;
-	param[0].u.value.b = PTA_SCMI_CAPS_SMT_HEADER;
+	if (chan->dyn_shm)
+		param[0].u.value.b = PTA_SCMI_CAPS_MSG_HEADER;
+	else
+		param[0].u.value.b = PTA_SCMI_CAPS_SMT_HEADER;
 
 	ret = tee_invoke_func(sess->tee, &cmd_arg, ARRAY_SIZE(param), param);
 	if (ret || cmd_arg.ret) {
@@ -167,34 +194,48 @@ static int invoke_cmd(struct udevice *dev, struct channel_session *sess,
 {
 	struct scmi_optee_channel *chan = dev_get_plat(dev);
 	struct tee_invoke_arg arg = { };
-	struct tee_param param[2] = { };
+	struct tee_param param[3] = { };
 	int ret;
 
-	scmi_write_msg_to_smt(dev, &chan->smt, msg);
-
 	arg.session = sess->tee_session;
 	param[0].attr = TEE_PARAM_ATTR_TYPE_VALUE_INPUT;
 	param[0].u.value.a = sess->channel_hdl;
 
-	if (chan->dyn_shm) {
-		arg.func = PTA_SCMI_CMD_PROCESS_SMT_CHANNEL_MESSAGE;
-		param[1].attr = TEE_PARAM_ATTR_TYPE_MEMREF_INOUT;
+	if (sess->tee_shm) {
+		size_t in_size;
+
+		ret = scmi_msg_to_smt_msg(dev, &chan->smt, msg, &in_size);
+		if (ret < 0)
+			return ret;
+
+		arg.func = PTA_SCMI_CMD_PROCESS_MSG_CHANNEL;
+		param[1].attr = TEE_PARAM_ATTR_TYPE_MEMREF_INPUT;
 		param[1].u.memref.shm = sess->tee_shm;
-		param[1].u.memref.size = SCMI_SHM_SIZE;
+		param[1].u.memref.size = in_size;
+		param[2].attr = TEE_PARAM_ATTR_TYPE_MEMREF_OUTPUT;
+		param[2].u.memref.shm = sess->tee_shm;
+		param[2].u.memref.size = sess->tee_shm->size;
 	} else {
 		arg.func = PTA_SCMI_CMD_PROCESS_SMT_CHANNEL;
+		scmi_write_msg_to_smt(dev, &chan->smt, msg);
 	}
 
 	ret = tee_invoke_func(sess->tee, &arg, ARRAY_SIZE(param), param);
 	if (ret || arg.ret) {
 		if (!ret)
 			ret = -EPROTO;
+
+		return ret;
+	}
+
+	if (sess->tee_shm) {
+		ret = scmi_msg_from_smt_msg(dev, &chan->smt, msg,
+					    param[2].u.memref.size);
 	} else {
 		ret = scmi_read_resp_from_smt(dev, &chan->smt, msg);
+		scmi_clear_smt_channel(&chan->smt);
 	}
 
-	scmi_clear_smt_channel(&chan->smt);
-
 	return ret;
 }
 
@@ -217,9 +258,6 @@ static int prepare_shm(struct udevice *dev, struct channel_session *sess)
 
 	chan->smt.buf = sess->tee_shm->addr;
 
-	/* Initialize shm buffer for message exchanges */
-	scmi_clear_smt_channel(&chan->smt);
-
 	return 0;
 }
 
@@ -233,7 +271,7 @@ static void release_shm(struct udevice *dev, struct channel_session *sess)
 
 static int scmi_optee_process_msg(struct udevice *dev, struct scmi_msg *msg)
 {
-	struct channel_session sess;
+	struct channel_session sess = { };
 	int ret;
 
 	ret = open_channel(dev, &sess);
diff --git a/drivers/firmware/scmi/smt.c b/drivers/firmware/scmi/smt.c
index e60c2aebc89..509ed618a99 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;
+}
diff --git a/drivers/firmware/scmi/smt.h b/drivers/firmware/scmi/smt.h
index a8c0987bd30..9d669a6c922 100644
--- a/drivers/firmware/scmi/smt.h
+++ b/drivers/firmware/scmi/smt.h
@@ -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.
  */
 #ifndef SCMI_SMT_H
 #define SCMI_SMT_H
@@ -29,6 +29,17 @@ struct scmi_smt_header {
 	u8 msg_payload[0];
 };
 
+/**
+ * struct scmi_msg_header - Description of a MSG shared memory message buffer
+ *
+ * MSG communication protocol uses a 32bit header memory cell to store SCMI
+ * protocol data followed by the exchange SCMI message payload.
+ */
+struct scmi_smt_msg_header {
+	__le32 msg_header;
+	u8 msg_payload[0];
+};
+
 #define SMT_HEADER_TOKEN(token)		(((token) << 18) & GENMASK(31, 18))
 #define SMT_HEADER_PROTOCOL_ID(proto)	(((proto) << 10) & GENMASK(17, 10))
 #define SMT_HEADER_MESSAGE_TYPE(type)	(((type) << 18) & GENMASK(9, 8))
@@ -75,12 +86,44 @@ static inline void scmi_smt_put_channel(struct scmi_smt *smt)
 
 int scmi_dt_get_smt_buffer(struct udevice *dev, struct scmi_smt *smt);
 
+/*
+ * Write SCMI message to a SMT shared memory
+ * @dev: SCMI device
+ * @smt: Reference to shared memory using SMT header
+ * @msg: Input SCMI message transmitted
+ */
 int scmi_write_msg_to_smt(struct udevice *dev, struct scmi_smt *smt,
 			  struct scmi_msg *msg);
 
+/*
+ * Read SCMI message from a SMT shared memory
+ * @dev: SCMI device
+ * @smt: Reference to shared memory using SMT header
+ * @msg: Output SCMI message received
+ */
 int scmi_read_resp_from_smt(struct udevice *dev, struct scmi_smt *smt,
 			    struct scmi_msg *msg);
 
 void scmi_clear_smt_channel(struct scmi_smt *smt);
 
+/*
+ * Write SCMI message to SMT_MSG shared memory
+ * @dev: SCMI device
+ * @smt: Reference to shared memory using SMT_MSG header
+ * @msg: Input SCMI message transmitted
+ * @buf_size: Size of the full SMT_MSG buffer transmitted
+ */
+int scmi_msg_to_smt_msg(struct udevice *dev, struct scmi_smt *smt,
+			struct scmi_msg *msg, size_t *buf_size);
+
+/*
+ * Read SCMI message from SMT_MSG shared memory
+ * @dev: SCMI device
+ * @smt: Reference to shared memory using SMT_MSG header
+ * @msg: Output SCMI message received
+ * @buf_size: Size of the full SMT_MSG buffer received
+ */
+int scmi_msg_from_smt_msg(struct udevice *dev, struct scmi_smt *smt,
+			  struct scmi_msg *msg, size_t buf_size);
+
 #endif /* SCMI_SMT_H */