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 */
