From 996e6bae70ec2ce04061daab23f20e9c353c4227 Mon Sep 17 00:00:00 2001
From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
Date: Tue, 18 Jan 2022 15:11:46 +0000
Subject: [PATCH 34/40] ANDROID: trusty-ffa: Add support for FFA memory
 operations

Initializes Trusty mem_ops with FFA memory operations for share,
lend, reclaim.

Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
Change-Id: Id3a1eb5ae8e4721cb983c624773d39bafe25a77d
Upstream-Status: Pending [Not submitted to upstream yet]
Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com
---
 drivers/trusty/trusty-ffa.c | 102 ++++++++++++++++++++++++++++++++++++
 drivers/trusty/trusty.c     |   5 ++
 2 files changed, 107 insertions(+)

diff --git a/drivers/trusty/trusty-ffa.c b/drivers/trusty/trusty-ffa.c
index c8c16a1fc700..0655b3887b52 100644
--- a/drivers/trusty/trusty-ffa.c
+++ b/drivers/trusty/trusty-ffa.c
@@ -15,8 +15,110 @@
 #include "trusty-ffa.h"
 #include "trusty-private.h"
 
+static int __trusty_ffa_share_memory(struct device *dev, u64 *id,
+				     struct scatterlist *sglist,
+				     unsigned int nents, pgprot_t pgprot,
+				     u64 tag, bool share)
+{
+	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
+	int ret;
+	struct scatterlist *sg;
+	size_t count;
+	struct ffa_device *ffa_dev = to_ffa_dev(s->ffa->dev);
+	const struct ffa_dev_ops *ffa_ops = s->ffa->ops;
+	struct ffa_mem_region_attributes ffa_mem_attr;
+	struct ffa_mem_ops_args ffa_mem_args;
+
+	if (WARN_ON(nents < 1))
+		return -EINVAL;
+
+	count = dma_map_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
+	if (count != nents) {
+		dev_err(s->dev, "failed to dma map sg_table\n");
+		return -EINVAL;
+	}
+
+	sg = sglist;
+
+	mutex_lock(&s->ffa->share_memory_msg_lock);
+
+	ffa_mem_attr.receiver = ffa_dev->vm_id;
+	ffa_mem_attr.attrs = FFA_MEM_RW;
+
+	ffa_mem_args.use_txbuf = 1;
+	ffa_mem_args.tag = tag;
+	ffa_mem_args.attrs = &ffa_mem_attr;
+	ffa_mem_args.nattrs = 1;
+	ffa_mem_args.sg = sg;
+	ffa_mem_args.flags = 0;
+
+	if (share) {
+		ret = ffa_ops->memory_share(ffa_dev, &ffa_mem_args);
+	} else {
+		ret = ffa_ops->memory_lend(ffa_dev, &ffa_mem_args);
+	}
+
+	mutex_unlock(&s->ffa->share_memory_msg_lock);
+
+	if (!ret) {
+		*id = ffa_mem_args.g_handle;
+		dev_dbg(s->dev, "%s: done\n", __func__);
+		return 0;
+	}
+
+	dev_err(s->dev, "%s: failed %d", __func__, ret);
+
+	dma_unmap_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
+	return ret;
+}
+
+static int trusty_ffa_share_memory(struct device *dev, u64 *id,
+				   struct scatterlist *sglist,
+				   unsigned int nents, pgprot_t pgprot, u64 tag)
+{
+	return __trusty_ffa_share_memory(dev, id, sglist, nents, pgprot, tag,
+					 true);
+}
+
+static int trusty_ffa_lend_memory(struct device *dev, u64 *id,
+				  struct scatterlist *sglist,
+				  unsigned int nents, pgprot_t pgprot, u64 tag)
+{
+	return __trusty_ffa_share_memory(dev, id, sglist, nents, pgprot, tag,
+					 false);
+}
+
+static int trusty_ffa_reclaim_memory(struct device *dev, u64 id,
+				     struct scatterlist *sglist,
+				     unsigned int nents)
+{
+	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
+	int ret = 0;
+	const struct ffa_dev_ops *ffa_ops = s->ffa->ops;
+
+	if (WARN_ON(nents < 1))
+		return -EINVAL;
+
+	mutex_lock(&s->ffa->share_memory_msg_lock);
+
+	ret = ffa_ops->memory_reclaim(id, 0);
+
+	mutex_unlock(&s->ffa->share_memory_msg_lock);
+
+	if (ret != 0)
+		return ret;
+
+	dma_unmap_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
+
+	dev_dbg(s->dev, "%s: done\n", __func__);
+	return 0;
+}
+
 static const struct trusty_mem_ops trusty_ffa_mem_ops = {
 	.desc = &trusty_ffa_transport,
+	.trusty_share_memory = &trusty_ffa_share_memory,
+	.trusty_lend_memory = &trusty_ffa_lend_memory,
+	.trusty_reclaim_memory = &trusty_ffa_reclaim_memory,
 };
 
 static const struct ffa_device_id trusty_ffa_device_id[] = {
diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
index 4686b0d34f61..f91c255c9897 100644
--- a/drivers/trusty/trusty.c
+++ b/drivers/trusty/trusty.c
@@ -674,6 +674,11 @@ static int trusty_remove(struct platform_device *pdev)
  *
  * For Trusty API version < TRUSTY_API_VERSION_MEM_OBJ:
  *     trusty_smc_transport used for messaging.
+ *
+ * For Trusty API version == TRUSTY_API_VERSION_MEM_OBJ:
+ *     trusty_smc_transport used for messaging.
+ *     trusty_ffa_transport used for memory sharing.
+ *
  */
 static const trusty_transports_t trusty_transports[] = {
 	&trusty_smc_transport,
-- 
2.34.1