From 96660aeb5ae22d287d5c8fe827a340f1ca72193f Mon Sep 17 00:00:00 2001
From: "Qiang.li" <qiang.li@nxp.com>
Date: Thu, 12 Aug 2021 11:36:03 +0800
Subject: [PATCH 2/2] iMX8QM, iMX8QX, ISI: update the workaround for multiple
 camera use case. The XRDC hardware can only support up to 16 memory region,
 for multiple camera use case, merge all buffers to one region.
 VB2_MEMORY_MMAP mode is supported.

---
 drivers/media/platform/imx8/mxc-isi-cap.c | 184 +++++++++++++++++++++---------
 drivers/media/v4l2-core/videobuf2-core.c  |  32 ++++++
 include/media/videobuf2-core.h            |  10 ++
 3 files changed, 169 insertions(+), 57 deletions(-)

diff --git a/drivers/media/platform/imx8/mxc-isi-cap.c b/drivers/media/platform/imx8/mxc-isi-cap.c
index b979c37..291bf8f 100644
--- a/drivers/media/platform/imx8/mxc-isi-cap.c
+++ b/drivers/media/platform/imx8/mxc-isi-cap.c
@@ -388,8 +388,6 @@ static void cap_vb2_buffer_queue(struct vb2_buffer *vb2)
 #ifdef CONFIG_IMX8_ISI_OVERRUN_FIX
 static int mxc_isi_create_parition(struct mxc_isi_dev *mxc_isi)
 {
-	struct mxc_isi_buffer *buf;
-	struct vb2_buffer *vb2;
 	sc_rm_pt_t pt;
 	sc_ipc_t ipc_handle;
 	uint32_t mu_id;
@@ -397,7 +395,7 @@ 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;
-	int i;
+	struct vb2_queue *q = &mxc_isi->isi_cap.vb2_q;
 
 	if (mxc_isi->isi_partition != 0) {
 		dev_err(&mxc_isi->pdev->dev, "%s: isi_partition hasn't been released.\n", __func__);
@@ -445,64 +443,29 @@ static int mxc_isi_create_parition(struct mxc_isi_dev *mxc_isi)
 		goto EXIT_3;
 	}
 
-	/* Alloc and assign memory regions for discard buffers */
-/*
-	for (i = 0; i < mxc_isi->pix.num_planes; i++) {
-		addr_start = mxc_isi->discard_buffer_dma[i] & ~0xFFF;
-		addr_end = (mxc_isi->discard_buffer_dma[i] + mxc_isi->discard_size[i] - 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 discard 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 discard 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 discard buffer[%d]: %d\n", __func__, i, sciErr);
-			goto EXIT_3;
-		}
-	}
-*/  //Discard buffer is not needed for memory region.
-
 	/* Alloc and assign memory regions for out_pending buffers */
-	list_for_each_entry(buf, &mxc_isi->isi_cap.out_pending, list) {
-		vb2 = &buf->v4l2_buf.vb2_buf;
+	addr_start = q->all_buf_phy_addr & ~0xFFF;
+	addr_end = (q->all_buf_phy_addr + q->all_buf_size - 1) | 0xFFF;
 
-		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;
-			}
+	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]: %d\n", __func__, i, sciErr);
-				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]: %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\n", __func__, sciErr);
+		goto EXIT_3;
 	}
 
 	sc_ipc_close(ipc_handle);
@@ -546,6 +509,109 @@ static void mxc_isi_free_parition(struct mxc_isi_dev *mxc_isi)
 
 	sc_ipc_close(ipc_handle);
 }
+
+#include <media/videobuf2-memops.h>
+
+// Copy from videobuf2-dma-contig.c
+struct vb2_dc_buf {
+	struct device			*dev;
+	void				*vaddr;
+	unsigned long			size;
+	void				*cookie;
+	dma_addr_t			dma_addr;
+	unsigned long			attrs;
+	enum dma_data_direction		dma_dir;
+	struct sg_table			*dma_sgt;
+	struct frame_vector		*vec;
+
+	/* MMAP related */
+	struct vb2_vmarea_handler	handler;
+	refcount_t			refcount;
+	struct sg_table			*sgt_base;
+
+	/* DMABUF related */
+	struct dma_buf_attachment	*db_attach;
+};
+
+int mxc_isi_cap_reqbufs(struct file *file, void *priv,
+			  struct v4l2_requestbuffers *p)
+{
+	struct mxc_isi_dev *mxc_isi = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+	int res = vb2_verify_memory_type(vdev->queue, p->memory, p->type);
+	struct vb2_queue *queue = vdev->queue;
+	struct vb2_buffer *buf = NULL;
+	struct vb2_plane *plane = NULL;
+	struct vb2_dc_buf *dc_buf = NULL;
+	int i, j;
+
+	if (res)
+		return res;
+	if (vdev->queue->owner && vdev->queue->owner != file->private_data)
+		return -EBUSY;
+
+	if ((p->count == 0) && (queue->all_buf_size != 0)) {
+		// release buffers, restore the address and size
+		for(i=0; i < queue->num_buffers; i++) {
+			buf = queue->bufs[i];
+			for(j=0; j < buf->num_planes; j++) {
+				plane = &buf->planes[j];
+				dc_buf = (struct vb2_dc_buf *)plane->mem_priv;
+				if (dc_buf) {
+					dc_buf->size = queue->saved_buf_size[i][j];
+					dc_buf->dma_addr = queue->saved_buf_phy_addr[i][j];
+					dc_buf->vaddr = (void *)(queue->saved_buf_vaddr[i][j]);
+				}
+				if ((i == 0) && (j == 0))
+					plane->length = queue->all_buf_size;
+				else
+					plane->length = PAGE_SIZE;
+			}
+		}
+	}
+
+	vdev->queue->imx8_isi_workaround = 1;
+	res = vb2_core_reqbufs(vdev->queue, p->memory, &p->count);
+	/* If count == 0, then the owner has released all buffers and he
+	   is no longer owner of the queue. Otherwise we have a new owner. */
+	if (res == 0)
+		vdev->queue->owner = p->count ? file->private_data : NULL;
+
+	if ((p->count != 0) && (queue->all_buf_size != 0)) {
+
+		for(i=0; i < queue->num_buffers; i++) {
+			buf = queue->bufs[i];
+
+			for(j=0; j < buf->num_planes; j++) {
+				plane = &buf->planes[j];
+				dc_buf = (struct vb2_dc_buf *)plane->mem_priv;
+
+				if (dc_buf) {
+					// save original buffer information
+					if ((i == 0) && (j == 0)) {
+						queue->all_buf_phy_addr = (unsigned long)(dc_buf->dma_addr);
+						queue->all_buf_vaddr = (unsigned long)(dc_buf->vaddr);
+					}
+					queue->saved_buf_size[i][j] = dc_buf->size;
+					queue->saved_buf_phy_addr[i][j] = (unsigned long)(dc_buf->dma_addr);
+					queue->saved_buf_vaddr[i][j] = (unsigned long)(dc_buf->vaddr);
+
+					// set new buffer information
+					dc_buf->size = queue->single_buf_size;
+					dc_buf->dma_addr = queue->all_buf_phy_addr + (i * buf->num_planes + j) * queue->single_buf_size;
+					dc_buf->vaddr = (void *)(queue->all_buf_vaddr + (i * buf->num_planes + j) * queue->single_buf_size);
+					plane->m.offset = (i * buf->num_planes + j) * queue->single_buf_size;
+					plane->length = queue->single_buf_size;
+				}
+			}
+		}
+
+		dev_info(&mxc_isi->pdev->dev, "mxc_isi_cap_reqbufs: all_buf_phy_addr = 0x%llx.\r\n", queue->all_buf_phy_addr);
+		dev_info(&mxc_isi->pdev->dev, "mxc_isi_cap_reqbufs: all_buf_size = 0x%x.\r\n", queue->all_buf_size);
+	}
+
+	return res;
+}
 #endif
 
 static int cap_vb2_start_streaming(struct vb2_queue *q, unsigned int count)
@@ -1501,7 +1567,11 @@ static int mxc_isi_cap_enum_frameintervals(struct file *file, void *fh,
 	.vidioc_s_fmt_vid_cap_mplane	= mxc_isi_cap_s_fmt_mplane,
 	.vidioc_g_fmt_vid_cap_mplane	= mxc_isi_cap_g_fmt_mplane,
 
+#ifdef CONFIG_IMX8_ISI_OVERRUN_FIX
+	.vidioc_reqbufs			= mxc_isi_cap_reqbufs,
+#else
 	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+#endif
 	.vidioc_querybuf		= vb2_ioctl_querybuf,
 	.vidioc_qbuf			= vb2_ioctl_qbuf,
 	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index f1725da..8a08122e 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -336,6 +336,26 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum vb2_memory memory,
 	num_buffers = min_t(unsigned int, num_buffers,
 			    VB2_MAX_FRAME - q->num_buffers);
 
+#ifdef CONFIG_IMX8_ISI_OVERRUN_FIX
+	if ((memory == VB2_MEMORY_MMAP) && (q->imx8_isi_workaround)) {
+		unsigned int max_plane_length = 0;
+		unsigned int total_planes = 0;
+		for (buffer = 0; buffer < num_buffers; ++buffer) {
+			for (plane = 0; plane < num_planes; ++plane) {
+				if (plane_sizes[plane] > max_plane_length)
+					max_plane_length =plane_sizes[plane];
+
+				total_planes += 1;
+			}
+		}
+		max_plane_length = PAGE_ALIGN(max_plane_length);
+		q->single_buf_size = max_plane_length;
+		q->all_buf_size = max_plane_length * total_planes;
+	} else {
+		q->all_buf_size = 0;
+	}
+#endif
+
 	for (buffer = 0; buffer < num_buffers; ++buffer) {
 		/* Allocate videobuf buffer structures */
 		vb = kzalloc(q->buf_struct_size, GFP_KERNEL);
@@ -358,6 +378,18 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum vb2_memory memory,
 
 		/* Allocate video buffer memory for the MMAP type */
 		if (memory == VB2_MEMORY_MMAP) {
+#ifdef CONFIG_IMX8_ISI_OVERRUN_FIX
+			if (q->imx8_isi_workaround) {
+				for (plane = 0; plane < num_planes; ++plane) {
+					vb->planes[plane].length = PAGE_SIZE;
+					vb->planes[plane].min_length = PAGE_SIZE;
+				}
+				if (buffer == 0) {
+					vb->planes[0].length = q->all_buf_size;
+					vb->planes[0].min_length = q->all_buf_size;
+				}
+			}
+#endif
 			ret = __vb2_buf_mem_alloc(vb);
 			if (ret) {
 				dprintk(1, "failed allocating memory for buffer %d\n",
diff --git a/include/media/videobuf2-core.h b/include/media/videobuf2-core.h
index ef9b643..8c9e20e 100644
--- a/include/media/videobuf2-core.h
+++ b/include/media/videobuf2-core.h
@@ -565,6 +565,16 @@ struct vb2_queue {
 	u32				cnt_start_streaming;
 	u32				cnt_stop_streaming;
 #endif
+#ifdef CONFIG_IMX8_ISI_OVERRUN_FIX
+	u32 			imx8_isi_workaround;
+	u32				single_buf_size;
+	u32				all_buf_size;
+	u64				all_buf_phy_addr;
+	u64				all_buf_vaddr;
+	u32				saved_buf_size[VB2_MAX_FRAME][VB2_MAX_PLANES];
+	u64				saved_buf_phy_addr[VB2_MAX_FRAME][VB2_MAX_PLANES];
+	u64				saved_buf_vaddr[VB2_MAX_FRAME][VB2_MAX_PLANES];
+#endif
 };
 
 /**
-- 
1.9.1

