From 68a7b8b70fe4a2ef57b3d28a988c0dbf07b83d62 Mon Sep 17 00:00:00 2001
From: "Qiang.li" <qiang.li@nxp.com>
Date: Tue, 24 Aug 2021 11:11:21 +0800
Subject: [PATCH] iMX8QM, iMX8QX, ISI: auto merge the continued buffers to one
 memory region, it can support VB2_MEMORY_USERPTR and VB2_MEMORY_DMABUF, when
 the buffers are allocated as continued.

---
 drivers/media/platform/imx8/mxc-isi-cap.c | 103 ++++++++++++++++++++++++------
 1 file changed, 84 insertions(+), 19 deletions(-)

diff --git a/drivers/media/platform/imx8/mxc-isi-cap.c b/drivers/media/platform/imx8/mxc-isi-cap.c
index 291bf8f..8b6fe18 100644
--- a/drivers/media/platform/imx8/mxc-isi-cap.c
+++ b/drivers/media/platform/imx8/mxc-isi-cap.c
@@ -395,7 +395,11 @@ static int mxc_isi_create_parition(struct mxc_isi_dev *mxc_isi)
 	sc_rsrc_t resource;
 	sc_rm_mr_t mr;
 	sc_faddr_t addr_start, addr_end;
-	struct vb2_queue *q = &mxc_isi->isi_cap.vb2_q;
+	bool can_merge = false;
+	sc_faddr_t merged_addr_start, merged_length, length;
+	struct mxc_isi_buffer *buf;
+	struct vb2_buffer *vb2;
+	int i;
 
 	if (mxc_isi->isi_partition != 0) {
 		dev_err(&mxc_isi->pdev->dev, "%s: isi_partition hasn't been released.\n", __func__);
@@ -444,28 +448,89 @@ static int mxc_isi_create_parition(struct mxc_isi_dev *mxc_isi)
 	}
 
 	/* Alloc and assign memory regions for out_pending buffers */
-	addr_start = q->all_buf_phy_addr & ~0xFFF;
-	addr_end = (q->all_buf_phy_addr + q->all_buf_size - 1) | 0xFFF;
+	can_merge = true;
+	merged_addr_start = 0;
+	merged_length = 0;
+	list_for_each_entry(buf, &mxc_isi->isi_cap.out_pending, list) {
+		vb2 = &buf->v4l2_buf.vb2_buf;
 
-	mr = 0;
-	sciErr = sc_rm_memreg_alloc(ipc_handle, &mr, addr_start, addr_end);
-	if (sciErr != SC_ERR_NONE) {
-		dev_err(&mxc_isi->pdev->dev, "%s: can't alloc memory region for out_pending buffer: %d\n", __func__, sciErr);
-		if(mr != 0)
-			sc_rm_memreg_free(ipc_handle, mr);
-		goto EXIT_3;
-	}
+		for (i = 0; i < vb2->num_planes; i++) {
+			addr_start = vb2_dma_contig_plane_dma_addr(vb2, i);
+			length = vb2->planes[i].length;
+
+			if ((merged_addr_start == 0) && (merged_length == 0)) {
+				merged_addr_start = addr_start;
+				merged_length = length;
+				continue;
+			} else if (merged_addr_start + merged_length == addr_start) {
+				merged_length += length;
+			} else if (addr_start + length == merged_addr_start) {
+				merged_addr_start = addr_start;
+				merged_length += length;
+			} else {
+				can_merge = false;
+				break;
+			}
+		}
 
-	sciErr = sc_rm_assign_memreg(ipc_handle, mxc_isi->isi_partition, mr);
-	if (sciErr != SC_ERR_NONE) {
-		dev_err(&mxc_isi->pdev->dev, "%s: can't assign memory region for out_pending buffer: %d\n", __func__, sciErr);
-		goto EXIT_3;
+		if (!can_merge)
+			break;
 	}
 
-	sciErr = sc_rm_set_memreg_permissions(ipc_handle, mr, pt, SC_RM_PERM_FULL);
-	if (sciErr != SC_ERR_NONE) {
-		dev_err(&mxc_isi->pdev->dev, "%s: can't set memory region permission for out_pending buffer: %d\n", __func__, sciErr);
-		goto EXIT_3;
+	if (can_merge) {
+		addr_start = merged_addr_start & ~0xFFF;
+		addr_end = (merged_addr_start + merged_length - 1) | 0xFFF;
+
+		mr = 0;
+		sciErr = sc_rm_memreg_alloc(ipc_handle, &mr, addr_start, addr_end);
+		if (sciErr != SC_ERR_NONE) {
+			dev_err(&mxc_isi->pdev->dev, "%s: can't alloc memory region for out_pending buffer: %d\n", __func__, sciErr);
+			if(mr != 0)
+				sc_rm_memreg_free(ipc_handle, mr);
+			goto EXIT_3;
+		}
+
+		sciErr = sc_rm_assign_memreg(ipc_handle, mxc_isi->isi_partition, mr);
+		if (sciErr != SC_ERR_NONE) {
+			dev_err(&mxc_isi->pdev->dev, "%s: can't assign memory region for out_pending buffer: %d\n", __func__, sciErr);
+			goto EXIT_3;
+		}
+
+		sciErr = sc_rm_set_memreg_permissions(ipc_handle, mr, pt, SC_RM_PERM_FULL);
+		if (sciErr != SC_ERR_NONE) {
+			dev_err(&mxc_isi->pdev->dev, "%s: can't set memory region permission for out_pending buffer: %d\n", __func__, sciErr);
+			goto EXIT_3;
+		}
+	} else {
+		list_for_each_entry(buf, &mxc_isi->isi_cap.out_pending, list) {
+			vb2 = &buf->v4l2_buf.vb2_buf;
+
+			for (i = 0; i < vb2->num_planes; i++) {
+				addr_start = vb2_dma_contig_plane_dma_addr(vb2, i) & ~0xFFF;
+				addr_end = (vb2_dma_contig_plane_dma_addr(vb2, i) + vb2->planes[i].length - 1) | 0xFFF;
+
+				mr = 0;
+				sciErr = sc_rm_memreg_alloc(ipc_handle, &mr, addr_start, addr_end);
+				if (sciErr != SC_ERR_NONE) {
+					dev_err(&mxc_isi->pdev->dev, "%s: can't alloc memory region for out_pending buffer[%d]: %d\n", __func__, i, sciErr);
+					if(mr != 0)
+						sc_rm_memreg_free(ipc_handle, mr);
+					goto EXIT_3;
+				}
+
+				sciErr = sc_rm_assign_memreg(ipc_handle, mxc_isi->isi_partition, mr);
+				if (sciErr != SC_ERR_NONE) {
+					dev_err(&mxc_isi->pdev->dev, "%s: can't assign memory region for out_pending buffer[%d]: %d\n", __func__, i, sciErr);
+					goto EXIT_3;
+				}
+
+				sciErr = sc_rm_set_memreg_permissions(ipc_handle, mr, pt, SC_RM_PERM_FULL);
+				if (sciErr != SC_ERR_NONE) {
+					dev_err(&mxc_isi->pdev->dev, "%s: can't set memory region permission for out_pending buffer[%d]: %d\n", __func__, i, sciErr);
+					goto EXIT_3;
+				}
+			}
+		}
 	}
 
 	sc_ipc_close(ipc_handle);
-- 
1.9.1

