[RFC,V2,08/11] media: platform: Add Mediatek ISP P1 V4L2 functions

[Jungo Lin <jungo.lin@mediatek.com>]

Implement standard V4L2 video driver that utilizes V4L2
and media framework APIs. In this driver, supports one media
device, one sub-device and six video devices during
initialization. Moreover, it also connects with sensor and
senif drivers with V4L2 async APIs.

Signed-off-by: Jungo Lin <jungo.lin@mediatek.com>
---
This patch dependeds on "media: support Mediatek sensor interface driver"[1].

ISP P1 sub-device communicates with seninf sub-device with CIO.

[1]. media: support Mediatek sensor interface driver
https://patchwork.kernel.org/cover/10852957/
---
---
 .../platform/mtk-isp/isp_50/cam/Makefile      |   19 +
 .../platform/mtk-isp/isp_50/cam/mtk_cam-dev.c |  758 ++++++++++++
 .../platform/mtk-isp/isp_50/cam/mtk_cam-dev.h |  250 ++++
 .../mtk-isp/isp_50/cam/mtk_cam-v4l2-util.c    | 1086 +++++++++++++++++
 .../mtk-isp/isp_50/cam/mtk_cam-v4l2-util.h    |   43 +
 5 files changed, 2156 insertions(+)
 create mode 100644 drivers/media/platform/mtk-isp/isp_50/cam/Makefile
 create mode 100644 drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-dev.c
 create mode 100644 drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-dev.h
 create mode 100644 drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-v4l2-util.c
 create mode 100644 drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-v4l2-util.h

diff --git a/drivers/media/platform/mtk-isp/isp_50/cam/Makefile b/drivers/media/platform/mtk-isp/isp_50/cam/Makefile
new file mode 100644
index 000000000000..5a581ab65945
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/isp_50/cam/Makefile
@@ -0,0 +1,19 @@
+#
+# Copyright (C) 2018 MediaTek Inc.
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License version 2 as
+# published by the Free Software Foundation.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+
+mtk-cam-isp-objs += \
+	mtk_cam.o mtk_cam-dev.o \
+	mtk_cam-ctrl.o mtk_cam-scp.o \
+	mtk_cam-v4l2-util.o mtk_cam-smem-dev.o
+
+obj-$(CONFIG_VIDEO_MEDIATEK_ISP_PASS1_SUPPORT) += mtk-cam-isp.o
diff --git a/drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-dev.c b/drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-dev.c
new file mode 100644
index 000000000000..dda8a7b161ee
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-dev.c
@@ -0,0 +1,758 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 Mediatek Corporation.
+ * Copyright (c) 2017 Intel Corporation.
+ * Copyright (C) 2017 Google, Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * MTK_CAM-dev is highly based on Intel IPU3 ImgU driver.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "mtk_cam.h"
+#include "mtk_cam-dev.h"
+#include "mtk_cam-smem.h"
+#include "mtk_cam-v4l2-util.h"
+
+static const struct v4l2_ioctl_ops mtk_cam_v4l2_vcap_ioctl_ops = {
+	.vidioc_querycap = mtk_cam_videoc_querycap,
+	.vidioc_enum_framesizes = mtk_cam_enum_framesizes,
+	.vidioc_enum_fmt_vid_cap_mplane = mtk_cam_videoc_enum_fmt,
+	.vidioc_g_fmt_vid_cap_mplane = mtk_cam_videoc_g_fmt,
+	.vidioc_s_fmt_vid_cap_mplane = mtk_cam_videoc_s_fmt,
+	.vidioc_try_fmt_vid_cap_mplane = mtk_cam_videoc_try_fmt,
+	.vidioc_enum_input = mtk_cam_vidioc_enum_input,
+	.vidioc_g_input = mtk_cam_vidioc_g_input,
+	.vidioc_s_input = mtk_cam_vidioc_s_input,
+	/* buffer queue management */
+	.vidioc_reqbufs = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_qbuf = vb2_ioctl_qbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+	.vidioc_subscribe_event = mtk_cam_vidioc_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_ioctl_ops mtk_cam_v4l2_vout_ioctl_ops = {
+	.vidioc_querycap = mtk_cam_videoc_querycap,
+	.vidioc_enum_framesizes = mtk_cam_enum_framesizes,
+	.vidioc_enum_fmt_vid_out_mplane = mtk_cam_videoc_enum_fmt,
+	.vidioc_g_fmt_vid_out_mplane = mtk_cam_videoc_g_fmt,
+	.vidioc_s_fmt_vid_out_mplane = mtk_cam_videoc_s_fmt,
+	.vidioc_try_fmt_vid_out_mplane = mtk_cam_videoc_try_fmt,
+	.vidioc_enum_input = mtk_cam_vidioc_enum_input,
+	.vidioc_g_input = mtk_cam_vidioc_g_input,
+	.vidioc_s_input = mtk_cam_vidioc_s_input,
+	/* buffer queue management */
+	.vidioc_reqbufs = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_qbuf = vb2_ioctl_qbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+};
+
+static const struct v4l2_ioctl_ops mtk_cam_v4l2_meta_cap_ioctl_ops = {
+	.vidioc_querycap = mtk_cam_videoc_querycap,
+	.vidioc_enum_fmt_meta_cap = mtk_cam_meta_enum_format,
+	.vidioc_g_fmt_meta_cap = mtk_cam_videoc_g_meta_fmt,
+	.vidioc_s_fmt_meta_cap = mtk_cam_videoc_g_meta_fmt,
+	.vidioc_try_fmt_meta_cap = mtk_cam_videoc_g_meta_fmt,
+	.vidioc_reqbufs = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_qbuf = vb2_ioctl_qbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+};
+
+static const struct v4l2_ioctl_ops mtk_cam_v4l2_meta_out_ioctl_ops = {
+	.vidioc_querycap = mtk_cam_videoc_querycap,
+	.vidioc_enum_fmt_meta_out = mtk_cam_meta_enum_format,
+	.vidioc_g_fmt_meta_out = mtk_cam_videoc_g_meta_fmt,
+	.vidioc_s_fmt_meta_out = mtk_cam_videoc_g_meta_fmt,
+	.vidioc_try_fmt_meta_out = mtk_cam_videoc_g_meta_fmt,
+	.vidioc_reqbufs = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_qbuf = vb2_ioctl_qbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+};
+
+static struct v4l2_format meta_fmts[] = {
+	{
+		.fmt.meta = {
+			.dataformat = V4L2_META_FMT_MTISP_PARAMS,
+			.buffersize = 128 * PAGE_SIZE,
+		},
+	},
+	{
+		.fmt.meta = {
+			.dataformat = V4L2_META_FMT_MTISP_3A,
+			.buffersize = 300 * PAGE_SIZE,
+		},
+	},
+	{
+		.fmt.meta = {
+			.dataformat = V4L2_META_FMT_MTISP_AF,
+			.buffersize = 160 * PAGE_SIZE,
+		},
+	},
+	{
+		.fmt.meta = {
+			.dataformat = V4L2_META_FMT_MTISP_LCS,
+			.buffersize = 72 * PAGE_SIZE,
+		},
+	},
+	{
+		.fmt.meta = {
+			.dataformat = V4L2_META_FMT_MTISP_LMV,
+			.buffersize = 256,
+		},
+	},
+};
+
+/* Need to update mtk_cam_dev_fmt_set_img for default format configuration */
+static struct v4l2_format stream_out_fmts[] = {
+	{
+		.fmt.pix_mp = {
+			.width = IMG_MAX_WIDTH,
+			.height = IMG_MAX_HEIGHT,
+			.pixelformat = V4L2_PIX_FMT_MTISP_B8,
+			.field = V4L2_FIELD_NONE,
+			.colorspace = V4L2_COLORSPACE_SRGB,
+			.num_planes = 1,
+		},
+	},
+	{
+		.fmt.pix_mp = {
+			.width = IMG_MAX_WIDTH,
+			.height = IMG_MAX_HEIGHT,
+			.pixelformat = V4L2_PIX_FMT_MTISP_B10,
+			.field = V4L2_FIELD_NONE,
+			.colorspace = V4L2_COLORSPACE_SRGB,
+			.num_planes = 1,
+		},
+	},
+	{
+		.fmt.pix_mp = {
+			.width = IMG_MAX_WIDTH,
+			.height = IMG_MAX_HEIGHT,
+			.pixelformat = V4L2_PIX_FMT_MTISP_B12,
+			.field = V4L2_FIELD_NONE,
+			.colorspace = V4L2_COLORSPACE_SRGB,
+			.num_planes = 1,
+		},
+	},
+	{
+		.fmt.pix_mp = {
+			.width = IMG_MAX_WIDTH,
+			.height = IMG_MAX_HEIGHT,
+			.pixelformat = V4L2_PIX_FMT_MTISP_B14,
+			.field = V4L2_FIELD_NONE,
+			.colorspace = V4L2_COLORSPACE_SRGB,
+			.num_planes = 1,
+		},
+	},
+};
+
+static struct v4l2_format bin_out_fmts[] = {
+	{
+		.fmt.pix_mp = {
+			.width = RRZ_MAX_WIDTH,
+			.height = RRZ_MAX_HEIGHT,
+			.pixelformat = V4L2_PIX_FMT_MTISP_F8,
+			.field = V4L2_FIELD_NONE,
+			.colorspace = V4L2_COLORSPACE_RAW,
+			.num_planes = 1,
+		},
+	},
+	{
+		.fmt.pix_mp = {
+			.width = RRZ_MAX_WIDTH,
+			.height = RRZ_MAX_HEIGHT,
+			.pixelformat = V4L2_PIX_FMT_MTISP_F10,
+			.field = V4L2_FIELD_NONE,
+			.colorspace = V4L2_COLORSPACE_RAW,
+			.num_planes = 1,
+		},
+	},
+	{
+		.fmt.pix_mp = {
+			.width = RRZ_MAX_WIDTH,
+			.height = RRZ_MAX_HEIGHT,
+			.pixelformat = V4L2_PIX_FMT_MTISP_F12,
+			.field = V4L2_FIELD_NONE,
+			.colorspace = V4L2_COLORSPACE_RAW,
+			.num_planes = 1,
+		},
+	},
+	{
+		.fmt.pix_mp = {
+			.width = RRZ_MAX_WIDTH,
+			.height = RRZ_MAX_HEIGHT,
+			.pixelformat = V4L2_PIX_FMT_MTISP_F14,
+			.field = V4L2_FIELD_NONE,
+			.colorspace = V4L2_COLORSPACE_RAW,
+			.num_planes = 1,
+		},
+	},
+};
+
+static const struct
+mtk_cam_dev_node_desc output_queues[MTK_CAM_P1_TOTAL_OUTPUT] = {
+	{
+		.id = MTK_CAM_P1_META_IN_0,
+		.name = "meta input",
+		.description = "ISP tuning parameters",
+		.cap = V4L2_CAP_META_OUTPUT,
+		.buf_type = V4L2_BUF_TYPE_META_OUTPUT,
+		.link_flags = 0,
+		.capture = false,
+		.image = false,
+		.smem_alloc = true,
+		.fmts = meta_fmts,
+		.num_fmts = ARRAY_SIZE(meta_fmts),
+		.default_fmt_idx = 0,
+		.max_buf_count = 10,
+		.ioctl_ops = &mtk_cam_v4l2_meta_out_ioctl_ops,
+	},
+};
+
+static const struct
+mtk_cam_dev_node_desc capture_queues[MTK_CAM_P1_TOTAL_CAPTURE] = {
+	{
+		.id = MTK_CAM_P1_MAIN_STREAM_OUT,
+		.name = "main stream",
+		.cap = V4L2_CAP_VIDEO_CAPTURE_MPLANE,
+		.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+		.link_flags = 0,
+		.capture = true,
+		.image = true,
+		.smem_alloc = false,
+		.dma_port = R_IMGO,
+		.fmts = stream_out_fmts,
+		.num_fmts = ARRAY_SIZE(stream_out_fmts),
+		.default_fmt_idx = 0,
+		.ioctl_ops = &mtk_cam_v4l2_vcap_ioctl_ops,
+	},
+	{
+		.id = MTK_CAM_P1_PACKED_BIN_OUT,
+		.name = "packed out",
+		.cap = V4L2_CAP_VIDEO_CAPTURE_MPLANE,
+		.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+		.link_flags = 0,
+		.capture = true,
+		.image = true,
+		.smem_alloc = false,
+		.dma_port = R_RRZO,
+		.fmts = bin_out_fmts,
+		.num_fmts = ARRAY_SIZE(bin_out_fmts),
+		.default_fmt_idx = 1,
+		.ioctl_ops = &mtk_cam_v4l2_vcap_ioctl_ops,
+	},
+	{
+		.id = MTK_CAM_P1_META_OUT_0,
+		.name = "partial meta 0",
+		.description = "AE/AWB histogram",
+		.cap = V4L2_CAP_META_CAPTURE,
+		.buf_type = V4L2_BUF_TYPE_META_CAPTURE,
+		.link_flags = 0,
+		.capture = true,
+		.image = false,
+		.smem_alloc = false,
+		.dma_port = R_AAO | R_FLKO | R_PSO,
+		.fmts = meta_fmts,
+		.num_fmts = ARRAY_SIZE(meta_fmts),
+		.default_fmt_idx = 1,
+		.max_buf_count = 5,
+		.ioctl_ops = &mtk_cam_v4l2_meta_cap_ioctl_ops,
+	},
+	{
+		.id = MTK_CAM_P1_META_OUT_1,
+		.name = "partial meta 1",
+		.description = "AF histogram",
+		.cap = V4L2_CAP_META_CAPTURE,
+		.buf_type = V4L2_BUF_TYPE_META_CAPTURE,
+		.link_flags = 0,
+		.capture = true,
+		.image = false,
+		.smem_alloc = false,
+		.dma_port = R_AFO,
+		.fmts = meta_fmts,
+		.num_fmts = ARRAY_SIZE(meta_fmts),
+		.default_fmt_idx = 2,
+		.max_buf_count = 5,
+		.ioctl_ops = &mtk_cam_v4l2_meta_cap_ioctl_ops,
+	},
+	{
+		.id = MTK_CAM_P1_META_OUT_2,
+		.name = "partial meta 2",
+		.description = "Local contrast enhanced statistics",
+		.cap = V4L2_CAP_META_CAPTURE,
+		.buf_type = V4L2_BUF_TYPE_META_CAPTURE,
+		.link_flags = MEDIA_LNK_FL_DYNAMIC,
+		.capture = true,
+		.image = false,
+		.smem_alloc = false,
+		.dma_port = R_LCSO,
+		.fmts = meta_fmts,
+		.num_fmts = ARRAY_SIZE(meta_fmts),
+		.default_fmt_idx = 3,
+		.max_buf_count = 10,
+		.ioctl_ops = &mtk_cam_v4l2_meta_cap_ioctl_ops,
+	},
+	{
+		.id = MTK_CAM_P1_META_OUT_3,
+		.name = "partial meta 3",
+		.description = "Local motion vector histogram",
+		.cap = V4L2_CAP_META_CAPTURE,
+		.buf_type = V4L2_BUF_TYPE_META_CAPTURE,
+		.link_flags = MEDIA_LNK_FL_DYNAMIC,
+		.capture = true,
+		.image = false,
+		.smem_alloc = false,
+		.dma_port = R_LMVO,
+		.fmts = meta_fmts,
+		.num_fmts = ARRAY_SIZE(meta_fmts),
+		.default_fmt_idx = 4,
+		.max_buf_count = 10,
+		.ioctl_ops = &mtk_cam_v4l2_meta_cap_ioctl_ops,
+	},
+};
+
+static const struct mtk_cam_dev_queues_setting queues_setting = {
+	.output_node_descs = output_queues,
+	.total_output_nodes = MTK_CAM_P1_TOTAL_OUTPUT,
+	.capture_node_descs = capture_queues,
+	.total_capture_nodes = MTK_CAM_P1_TOTAL_CAPTURE,
+};
+
+static __u32 get_pixel_byte_by_fmt(__u32 pix_fmt)
+{
+	switch (pix_fmt) {
+	case V4L2_PIX_FMT_MTISP_B8:
+	case V4L2_PIX_FMT_MTISP_F8:
+		return 8;
+	case V4L2_PIX_FMT_MTISP_B10:
+	case V4L2_PIX_FMT_MTISP_F10:
+		return 10;
+	case V4L2_PIX_FMT_MTISP_B12:
+	case V4L2_PIX_FMT_MTISP_F12:
+		return 12;
+	case V4L2_PIX_FMT_MTISP_B14:
+	case V4L2_PIX_FMT_MTISP_F14:
+		return 14;
+	case V4L2_PIX_FMT_MTISP_U8:
+	case V4L2_PIX_FMT_MTISP_U10:
+	case V4L2_PIX_FMT_MTISP_U12:
+	case V4L2_PIX_FMT_MTISP_U14:
+		return 16;
+	default:
+		return 0;
+	}
+}
+
+static __u32 align_main_stream_size(__u32 size, unsigned int pix_mode)
+{
+	switch (pix_mode) {
+	case default_pixel_mode:
+	case four_pixel_mode:
+		return ALIGN(size, 8);
+	case two_pixel_mode:
+		return ALIGN(size, 4);
+	case one_pixel_mode:
+		return ALIGN(size, 2);
+	default:
+		break;
+	}
+	return 0;
+}
+
+static unsigned int align_packetd_out_size(__u32 size,
+					   unsigned int pix_mode,
+					   __u32 fmt)
+{
+	switch (pix_mode) {
+	case default_pixel_mode:
+	case four_pixel_mode:
+		return ALIGN(size, 16);
+	case two_pixel_mode:
+		return ALIGN(size, 8);
+	case one_pixel_mode:
+		if (fmt == V4L2_PIX_FMT_MTISP_F10)
+			return ALIGN(size, 4);
+		else
+			return ALIGN(size, 8);
+	default:
+		return ALIGN(size, 16);
+	}
+	return 0;
+}
+
+static __u32 cal_main_stream_stride(struct device *dev,
+				    __u32 width,
+				    __u32 pix_fmt,
+				    __u32 pix_mode)
+{
+	__u32 stride;
+	__u32 pixel_byte = get_pixel_byte_by_fmt(pix_fmt);
+
+	width = ALIGN(width, 4);
+	stride = ALIGN(DIV_ROUND_UP(width * pixel_byte, 8), 2);
+	/* expand stride, instead of shrink width */
+	stride = align_main_stream_size(stride, pix_mode);
+
+	dev_dbg(dev,
+		"main width:%d, pix_mode:%d, stride:%d\n",
+		width, pix_mode, stride);
+	return stride;
+}
+
+static __u32 cal_packed_out_stride(struct device *dev,
+				   __u32 width,
+				   __u32 pix_fmt,
+				   __u32 pix_mode)
+{
+	__u32 stride;
+	__u32 pixel_byte = get_pixel_byte_by_fmt(pix_fmt);
+
+	width = ALIGN(width, 4);
+	stride = DIV_ROUND_UP(width * 3, 2);
+	stride = DIV_ROUND_UP(stride * pixel_byte, 8);
+	/* expand stride, instead of shrink width */
+	stride = align_packetd_out_size(stride, pix_mode, pix_fmt);
+
+	dev_dbg(dev,
+		"packed width:%d, pix_mode:%d, stride:%d\n",
+		width, pix_mode, stride);
+
+	return stride;
+}
+
+static __u32 cal_img_stride(struct device *dev,
+			    int node_id,
+			    __u32 width,
+			    __u32 pix_fmt)
+{
+	__u32 bpl;
+
+	/* Currently, only support one_pixel_mode */
+	if (node_id == MTK_CAM_P1_MAIN_STREAM_OUT)
+		bpl = cal_main_stream_stride(dev, width, pix_fmt,
+					     one_pixel_mode);
+	else if (node_id == MTK_CAM_P1_PACKED_BIN_OUT)
+		bpl = cal_packed_out_stride(dev, width, pix_fmt,
+					    one_pixel_mode);
+
+	/* For DIP HW constrained, it needs 4 byte alignment */
+	bpl = ALIGN(bpl, 4);
+
+	return bpl;
+}
+
+struct v4l2_format *
+mtk_cam_dev_find_fmt(struct mtk_cam_dev_node_desc *desc, u32 format)
+{
+	unsigned int i;
+	struct v4l2_format *dev_fmt;
+
+	for (i = 0; i < desc->num_fmts; i++) {
+		dev_fmt = &desc->fmts[i];
+		if (dev_fmt->fmt.pix_mp.pixelformat == format)
+			return dev_fmt;
+	}
+
+	return NULL;
+}
+
+/* The helper to configure the device context */
+void mtk_cam_dev_queue_setup(struct mtk_cam_dev *cam_dev,
+			     const struct mtk_cam_dev_queues_setting *setting)
+{
+	unsigned int i, node_idx;
+
+	node_idx = 0;
+
+	/* Setup the output queue */
+	for (i = 0; i < setting->total_output_nodes; i++)
+		cam_dev->mem2mem2_nodes[node_idx++].desc =
+			setting->output_node_descs[i];
+
+	/* Setup the capture queue */
+	for (i = 0; i < setting->total_capture_nodes; i++)
+		cam_dev->mem2mem2_nodes[node_idx++].desc =
+			setting->capture_node_descs[i];
+
+	cam_dev->dev_node_num = node_idx;
+}
+
+int mtk_cam_dev_job_finish(struct mtk_cam_dev *cam_dev,
+			   struct mtk_cam_dev_finish_param *fram_param)
+{
+	struct mtk_cam_dev_buffer *buf, *b0;
+
+	if (!cam_dev->streaming)
+		return 0;
+
+	dev_dbg(&cam_dev->pdev->dev,
+		"job recvied request fd:%d, frame_seq:%d state:%d\n",
+		fram_param->request_fd,
+		fram_param->frame_seq_no,
+		fram_param->state);
+
+	/*
+	 * Set the buffer's VB2 status so that the user can dequeue
+	 * the buffer.
+	 */
+	list_for_each_entry_safe(buf, b0, fram_param->list_buf, list) {
+		list_del(&buf->list);
+		buf->vbb.vb2_buf.timestamp = ktime_get_ns();
+		buf->vbb.sequence = fram_param->frame_seq_no;
+		if (buf->vbb.vb2_buf.state == VB2_BUF_STATE_ACTIVE)
+			vb2_buffer_done(&buf->vbb.vb2_buf, fram_param->state);
+	}
+
+	return 0;
+}
+
+int mtk_cam_dev_event_frame_sync(struct mtk_cam_dev *cam_dev,
+				 __u32 frame_seq_no)
+{
+	struct v4l2_event event;
+
+	memset(&event, 0, sizeof(event));
+	event.type = V4L2_EVENT_FRAME_SYNC;
+	event.u.frame_sync.frame_sequence = frame_seq_no;
+	v4l2_event_queue(cam_dev->subdev.devnode, &event);
+
+	return 0;
+}
+
+/* Calcuate mplane pix format */
+void mtk_cam_dev_cal_mplane_pix_fmt(struct device *dev,
+				    struct v4l2_pix_format_mplane *dest_fmt,
+				    unsigned int node_id)
+{
+	unsigned int i;
+	__u32 bpl, sizeimage, imagsize;
+
+	imagsize = 0;
+	for (i = 0 ; i < dest_fmt->num_planes; ++i) {
+		bpl = cal_img_stride(dev,
+				     node_id,
+				     dest_fmt->width,
+				     dest_fmt->pixelformat);
+		sizeimage = bpl * dest_fmt->height;
+		imagsize += sizeimage;
+		dest_fmt->plane_fmt[i].bytesperline = bpl;
+		dest_fmt->plane_fmt[i].sizeimage = sizeimage;
+		memset(dest_fmt->plane_fmt[i].reserved,
+		       0, sizeof(dest_fmt->plane_fmt[i].reserved));
+		dev_dbg(dev, "plane:%d,bpl:%d,sizeimage:%u\n",
+			i,  bpl, dest_fmt->plane_fmt[i].sizeimage);
+	}
+
+	if (dest_fmt->num_planes == 1)
+		dest_fmt->plane_fmt[0].sizeimage = imagsize;
+}
+
+void mtk_cam_dev_fmt_set_img(struct device *dev,
+			     struct v4l2_pix_format_mplane *dest_fmt,
+			     struct v4l2_pix_format_mplane *src_fmt,
+			     unsigned int node_id)
+{
+	dest_fmt->width = src_fmt->width;
+	dest_fmt->height = src_fmt->height;
+	dest_fmt->pixelformat = src_fmt->pixelformat;
+	dest_fmt->field = src_fmt->field;
+	dest_fmt->colorspace = src_fmt->colorspace;
+	dest_fmt->num_planes = src_fmt->num_planes;
+	/* Use default */
+	dest_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	dest_fmt->quantization = V4L2_QUANTIZATION_DEFAULT;
+	dest_fmt->xfer_func =
+		V4L2_MAP_XFER_FUNC_DEFAULT(dest_fmt->colorspace);
+	memset(dest_fmt->reserved, 0, sizeof(dest_fmt->reserved));
+
+	dev_dbg(dev, "%s: Dest Fmt:%c%c%c%c, w*h:%d*%d\n",
+		__func__,
+		(dest_fmt->pixelformat & 0xFF),
+		(dest_fmt->pixelformat >> 8) & 0xFF,
+		(dest_fmt->pixelformat >> 16) & 0xFF,
+		(dest_fmt->pixelformat >> 24) & 0xFF,
+		dest_fmt->width,
+		dest_fmt->height);
+
+	mtk_cam_dev_cal_mplane_pix_fmt(dev, dest_fmt, node_id);
+}
+
+/* Get the default format setting */
+void mtk_cam_dev_load_default_fmt(struct device *dev,
+				  struct mtk_cam_dev_node_desc *queue_desc,
+				  struct v4l2_format *dest)
+{
+	struct v4l2_format *default_fmt =
+		&queue_desc->fmts[queue_desc->default_fmt_idx];
+
+	dest->type = queue_desc->buf_type;
+
+	/* Configure default format based on node type */
+	if (queue_desc->image) {
+		mtk_cam_dev_fmt_set_img(dev,
+					&dest->fmt.pix_mp,
+					&default_fmt->fmt.pix_mp,
+					queue_desc->id);
+	} else {
+		dest->fmt.meta.dataformat = default_fmt->fmt.meta.dataformat;
+		dest->fmt.meta.buffersize = default_fmt->fmt.meta.buffersize;
+	}
+}
+
+/* Get a free buffer from a video node */
+static struct mtk_cam_dev_buffer *
+mtk_cam_dev_get_pending_buf(struct mtk_cam_dev *cam_dev, int node)
+{
+	struct mtk_cam_dev_buffer *buf;
+	struct mtk_cam_video_device *vdev;
+
+	if (node > cam_dev->dev_node_num || node < 0) {
+		dev_err(&cam_dev->pdev->dev, "Invalid mtk_cam_dev node.\n");
+		return NULL;
+	}
+	vdev = &cam_dev->mem2mem2_nodes[node];
+
+	spin_lock(&vdev->slock);
+	buf = list_first_entry_or_null(&vdev->pending_list,
+				       struct mtk_cam_dev_buffer,
+				       list);
+	if (!buf) {
+		spin_unlock(&vdev->slock);
+		return NULL;
+	}
+	list_del(&buf->list);
+	spin_unlock(&vdev->slock);
+
+	return buf;
+}
+
+int mtk_cam_dev_queue_req_buffers(struct mtk_cam_dev *cam_dev)
+{
+	unsigned int node;
+	const int mtk_cam_dev_node_num = cam_dev->dev_node_num;
+	struct device *dev = &cam_dev->pdev->dev;
+	struct mtk_cam_dev_start_param s_param;
+	struct mtk_cam_dev_buffer *buf;
+
+	memset(&s_param, 0, sizeof(struct mtk_cam_dev_start_param));
+
+	if (!cam_dev->streaming) {
+		dev_dbg(dev, "%s: stream off, no enqueue\n", __func__);
+		return 0;
+	}
+
+	/* Check all enabled nodes to collect its buffer  */
+	for (node = 0; node < mtk_cam_dev_node_num; node++) {
+		if (!cam_dev->mem2mem2_nodes[node].enabled)
+			continue;
+		buf = mtk_cam_dev_get_pending_buf(cam_dev, node);
+		if (!buf)
+			continue;
+
+		/* TBD: use buf_init callback function */
+		buf->daddr =
+			vb2_dma_contig_plane_dma_addr(&buf->vbb.vb2_buf, 0);
+		if (cam_dev->mem2mem2_nodes[node].desc.smem_alloc) {
+			buf->scp_addr = mtk_cam_smem_iova_to_scp_addr(
+				cam_dev->smem_dev, buf->daddr);
+		} else {
+			buf->scp_addr = 0;
+		}
+
+		dev_dbg(dev,
+			"Node:%d fd:%d idx:%d state:%d daddr:%pad addr:%pad",
+			node,
+			buf->vbb.request_fd,
+			buf->vbb.vb2_buf.index,
+			buf->vbb.vb2_buf.state,
+			&buf->daddr,
+			&buf->scp_addr);
+
+		s_param.buffers[node] = buf;
+		s_param.request_fd = buf->vbb.request_fd;
+	}
+
+	/* Trigger en-queued job to driver */
+	mtk_isp_req_enqueue(dev, &s_param);
+
+	return 0;
+}
+
+int mtk_cam_dev_init(struct platform_device *pdev,
+		     struct mtk_cam_dev *cam_dev)
+{
+	int ret;
+
+	cam_dev->pdev = pdev;
+
+	mtk_cam_dev_queue_setup(cam_dev, &queues_setting);
+
+	/* v4l2 sub-device registration */
+	dev_dbg(&cam_dev->pdev->dev, "mem2mem2.name: %s\n",
+		MTK_CAM_DEV_P1_NAME);
+
+	ret = mtk_cam_mem2mem2_v4l2_register(cam_dev);
+	if (ret)
+		return ret;
+
+	ret = mtk_cam_v4l2_async_register(cam_dev);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int mtk_cam_dev_release(struct platform_device *pdev,
+			struct mtk_cam_dev *cam_dev)
+{
+	mtk_cam_v4l2_async_unregister(cam_dev);
+	mtk_cam_v4l2_unregister(cam_dev);
+
+	return 0;
+}
diff --git a/drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-dev.h b/drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-dev.h
new file mode 100644
index 000000000000..410460de44fa
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-dev.h
@@ -0,0 +1,250 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2018 Mediatek Corporation.
+ * Copyright (c) 2017 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * MTK_CAM-dev is highly based on Intel IPU3 ImgU driver.
+ *
+ */
+
+#ifndef __MTK_CAM_DEV_H__
+#define __MTK_CAM_DEV_H__
+
+#include <linux/device.h>
+#include <linux/types.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-v4l2.h>
+
+#define MTK_CAM_DEV_P1_NAME		"MTK-ISP-P1-V4L2"
+
+#define MTK_CAM_DEV_NODES		11
+
+#define MTK_CAM_P1_META_IN_0		0
+#define MTK_CAM_P1_TOTAL_OUTPUT		1
+
+#define MTK_CAM_P1_MAIN_STREAM_OUT	1
+#define MTK_CAM_P1_PACKED_BIN_OUT	2
+#define MTK_CAM_P1_META_OUT_0		3
+#define MTK_CAM_P1_META_OUT_1		4
+#define MTK_CAM_P1_META_OUT_2		5
+#define MTK_CAM_P1_META_OUT_3		6
+#define MTK_CAM_P1_TOTAL_CAPTURE	6
+
+struct mtk_cam_dev_buffer {
+	struct vb2_v4l2_buffer	vbb;
+	struct list_head	list;
+	/* Intenal part */
+	dma_addr_t		daddr;
+	dma_addr_t		scp_addr;
+};
+
+/* Attributes setup by device owner */
+struct mtk_cam_dev_queues_setting {
+	const struct mtk_cam_dev_node_desc *output_node_descs;
+	unsigned int total_output_nodes;
+	const struct mtk_cam_dev_node_desc *capture_node_descs;
+	unsigned int total_capture_nodes;
+};
+
+struct mtk_cam_dev_start_param {
+	int request_fd;
+	struct mtk_cam_dev_buffer *buffers[MTK_CAM_DEV_NODES];
+};
+
+struct mtk_cam_dev_finish_param {
+	int request_fd;
+	unsigned int frame_seq_no;
+	unsigned int state;
+	struct list_head *list_buf;
+};
+
+/*
+ * struct mtk_cam_dev_node_desc - node attributes
+ *
+ * @id:		 id of the context queue
+ * @name:	 media entity name
+ * @description: descritpion of node
+ * @cap:	 mapped to V4L2 capabilities
+ * @buf_type:	 mapped to V4L2 buffer type
+ * @dma_port:	 the dma port associated to the buffer
+ * @link_flags:	 default media link flags
+ * @smem_alloc:	 using the cam_smem_drv as alloc ctx or not
+ * @capture:	 true for capture queue (device to user)
+ *		 false for output queue (from user to device)
+ * @image:	 true for image node, false for meta node
+ * @num_fmts:	 the number of supported formats
+ * @default_fmt_idx: default format of this node
+ * @max_buf_count: maximum V4L2 buffer count
+ * @ioctl_ops:  mapped to v4l2_ioctl_ops
+ * @fmts:	supported format
+ *
+ */
+struct mtk_cam_dev_node_desc {
+	u8 id;
+	char *name;
+	char *description;
+	u32 cap;
+	u32 buf_type;
+	u32 dma_port;
+	u32 link_flags;
+	u8 smem_alloc:1;
+	u8 capture:1;
+	u8 image:1;
+	u8 num_fmts;
+	u8 default_fmt_idx;
+	u8 max_buf_count;
+	const struct v4l2_ioctl_ops *ioctl_ops;
+	struct v4l2_format *fmts;
+};
+
+/*
+ * struct mtk_cam_video_device - Mediatek video device structure.
+ *
+ * @id:		Id for mtk_cam_dev_node_desc or mem2mem2_nodes array
+ * @enabled:	Indicate the device is enabled or not
+ * @vdev_fmt:	The V4L2 format of video device
+ * @vdev_apd:	The media pad graph object of video device
+ * @vbq:	A videobuf queue of video device
+ * @desc:	The node attributes of video device
+ * @ctrl_handler:	The control handler of video device
+ * @pending_list:	List for pending buffers before enqueuing into driver
+ * @lock:	Serializes vb2 queue and video device operations.
+ * @slock:	Protect for pending_list.
+ *
+ */
+struct mtk_cam_video_device {
+	unsigned int id;
+	unsigned int enabled;
+	struct v4l2_format vdev_fmt;
+	struct video_device vdev;
+	struct media_pad vdev_pad;
+	struct vb2_queue vbq;
+	struct mtk_cam_dev_node_desc desc;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct list_head pending_list;
+	/* Used for vbq & vdev */
+	struct mutex lock;
+	/* protect for pending_list */
+	spinlock_t slock;
+};
+
+/*
+ * struct mtk_cam_dev - Mediatek camera device structure.
+ *
+ * @pdev:	Pointer to platform device
+ * @smem_pdev:	Pointer to shared memory platform device
+ * @pipeline:	Media pipeline information
+ * @media_dev:	Media device
+ * @subdev:	The V4L2 sub-device
+ * @v4l2_dev:	The V4L2 device driver
+ * @notifier:	The v4l2_device notifier data
+ * @subdev_pads: Pointer to the number of media pads of this sub-device
+ * @ctrl_handler: The control handler
+ * @mem2mem2_nodes: The array list of mtk_cam_video_device
+ * @seninf:	Pointer to the seninf sub-device
+ * @sensor:	Pointer to the active sensor V4L2 sub-device when streaming on
+ * @streaming:	Indicate the overall streaming status is on or off
+ * @dev_node_num: The number of supported V4L2 video device nodes
+ * @request_fd:	The file descriptor of request API
+ * @request_count: The buffer count of request API
+ *
+ * Below is the graph topology for Camera IO connection.
+ * sensor 1 (main) --> sensor IF --> P1 sub-device
+ * sensor 2 (sub)  -->
+ *
+ */
+struct mtk_cam_dev {
+	struct platform_device *pdev;
+	struct device *smem_dev;
+	struct media_pipeline pipeline;
+	struct media_device media_dev;
+	struct v4l2_subdev subdev;
+	struct v4l2_device v4l2_dev;
+	struct v4l2_async_notifier notifier;
+	struct media_pad *subdev_pads;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct mtk_cam_video_device mem2mem2_nodes[MTK_CAM_DEV_NODES];
+	struct v4l2_subdev *seninf;
+	struct v4l2_subdev *sensor;
+	unsigned int streaming;
+	unsigned int dev_node_num;
+	int request_fd;
+	unsigned int request_count;
+};
+
+int mtk_cam_dev_init(struct platform_device *pdev,
+		     struct mtk_cam_dev *cam_dev);
+int mtk_cam_v4l2_register(struct device *dev,
+			  struct media_device *media_dev,
+			  struct v4l2_device *v4l2_dev,
+			  struct v4l2_ctrl_handler *ctrl_handler);
+int mtk_cam_v4l2_unregister(struct mtk_cam_dev *cam_dev);
+int mtk_cam_mem2mem2_v4l2_register(struct mtk_cam_dev *cam_dev);
+int mtk_cam_v4l2_async_register(struct mtk_cam_dev *cam_dev);
+void mtk_cam_v4l2_async_unregister(struct mtk_cam_dev *cam_dev);
+int mtk_cam_dev_queue_req_buffers(struct mtk_cam_dev *cam_dev);
+int mtk_cam_dev_release(struct platform_device *pdev,
+			struct mtk_cam_dev *cam_dev);
+void mtk_cam_dev_queue_setup(struct mtk_cam_dev *cam_dev,
+			     const struct mtk_cam_dev_queues_setting *setting);
+int mtk_cam_dev_job_finish(struct mtk_cam_dev *cam_dev,
+			   struct mtk_cam_dev_finish_param *param);
+int mtk_cam_dev_event_frame_sync(struct mtk_cam_dev *cam_dev,
+				 __u32 frame_seq_no);
+void mtk_cam_dev_fmt_set_img(struct device *dev,
+			     struct v4l2_pix_format_mplane *dest_fmt,
+			     struct v4l2_pix_format_mplane *src_fmt,
+			     unsigned int node_id);
+struct v4l2_format *
+mtk_cam_dev_find_fmt(struct mtk_cam_dev_node_desc *queue_desc, u32 format);
+void mtk_cam_dev_load_default_fmt(struct device *dev,
+				  struct mtk_cam_dev_node_desc *queue,
+				  struct v4l2_format *dest_fmt);
+void mtk_cam_dev_cal_mplane_pix_fmt(struct device *dev,
+				    struct v4l2_pix_format_mplane *dest_fmt,
+				    unsigned int node_id);
+
+static inline struct mtk_cam_video_device *
+file_to_mtk_cam_node(struct file *__file)
+{
+	return container_of(video_devdata(__file),
+		struct mtk_cam_video_device, vdev);
+}
+
+static inline struct mtk_cam_dev *
+mtk_cam_subdev_to_dev(struct v4l2_subdev *__sd)
+{
+	return container_of(__sd,
+		struct mtk_cam_dev, subdev);
+}
+
+static inline struct mtk_cam_video_device *
+mtk_cam_vbq_to_vdev(struct vb2_queue *__vq)
+{
+	return container_of(__vq,
+		struct mtk_cam_video_device, vbq);
+}
+
+static inline struct mtk_cam_dev_buffer *
+mtk_cam_vb2_buf_to_dev_buf(struct vb2_buffer *__vb)
+{
+	return container_of(__vb,
+		struct mtk_cam_dev_buffer, vbb.vb2_buf);
+}
+
+#endif /* __MTK_CAM_DEV_H__ */
diff --git a/drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-v4l2-util.c b/drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-v4l2-util.c
new file mode 100644
index 000000000000..196aaef3d854
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-v4l2-util.c
@@ -0,0 +1,1086 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 Mediatek Corporation.
+ * Copyright (c) 2017 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * MTK_CAM-v4l2 is highly based on Intel IPU3 ImgU driver.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <media/v4l2-common.h>
+#include <media/media-entity.h>
+#include <media/v4l2-async.h>
+
+#include "mtk_cam.h"
+#include "mtk_cam-ctrl.h"
+#include "mtk_cam-dev.h"
+#include "mtk_cam-v4l2-util.h"
+
+#define MTK_CAM_SENINF_PAD_SRC			4
+#define MTK_CAM_P1_HUB_PAD_SINK			MTK_CAM_DEV_NODES
+
+static int mtk_cam_subdev_open(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_fh *fh)
+{
+	struct mtk_cam_dev *cam_dev = mtk_cam_subdev_to_dev(sd);
+
+	cam_dev->request_fd = -1;
+	cam_dev->request_count = 0;
+
+	return mtk_isp_open(&cam_dev->pdev->dev);
+}
+
+static int mtk_cam_subdev_close(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh)
+{
+	struct mtk_cam_dev *cam_dev = mtk_cam_subdev_to_dev(sd);
+
+	return mtk_isp_release(&cam_dev->pdev->dev);
+}
+
+static int mtk_cam_v4l2_get_active_sensor(struct mtk_cam_dev *cam_dev)
+{
+	struct media_device *mdev = cam_dev->seninf->entity.graph_obj.mdev;
+	struct media_entity *entity;
+	struct device *dev = &cam_dev->pdev->dev;
+
+	cam_dev->sensor = NULL;
+	media_device_for_each_entity(entity, mdev) {
+		dev_dbg(dev, "media entity: %s:0x%x\n",
+			entity->name, entity->function);
+		if (entity->function == MEDIA_ENT_F_CAM_SENSOR &&
+		    entity->stream_count > 0) {
+			cam_dev->sensor = media_entity_to_v4l2_subdev(entity);
+			dev_dbg(dev, "Sensor found: %s\n", entity->name);
+			break;
+		}
+	}
+
+	if (!cam_dev->sensor) {
+		dev_err(dev, "Sensor is not connected\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int mtk_cam_cio_stream_on(struct mtk_cam_dev *cam_dev)
+{
+	struct device *dev = &cam_dev->pdev->dev;
+	struct isp_p1_device *p1_dev = get_p1_device(dev);
+	struct mtk_isp_p1_ctx *isp_ctx = &p1_dev->isp_ctx;
+	int ret;
+
+	/* Align vb2_core_streamon design */
+	if (cam_dev->streaming) {
+		dev_warn(dev, "already streaming\n", dev);
+		return 0;
+	}
+
+	if (!cam_dev->seninf) {
+		dev_err(dev, "no seninf connected:%d\n", ret);
+		return -EPERM;
+	}
+
+	/* Get active sensor from graph topology */
+	ret = mtk_cam_v4l2_get_active_sensor(cam_dev);
+	if (ret)
+		return -EPERM;
+
+	ret = mtk_isp_config(dev);
+	if (ret)
+		return -EPERM;
+
+	/* Seninf must stream on first */
+	dev_dbg(dev, "streamed on: %s\n", cam_dev->seninf->entity.name);
+	ret = v4l2_subdev_call(cam_dev->seninf, video, s_stream, 1);
+	if (ret) {
+		dev_err(dev, "%s stream on failed:%d\n",
+			cam_dev->seninf->entity.name, ret);
+		return -EPERM;
+	}
+
+	dev_dbg(dev, "streamed on: %s\n", cam_dev->sensor->entity.name);
+	ret = v4l2_subdev_call(cam_dev->sensor, video, s_stream, 1);
+	if (ret) {
+		dev_err(dev, "%s stream on failed:%d\n",
+			cam_dev->sensor->entity.name, ret);
+		goto fail_sensor_on;
+	}
+
+	cam_dev->streaming = true;
+	mtk_cam_dev_queue_req_buffers(cam_dev);
+	isp_composer_stream(isp_ctx, 1);
+	dev_dbg(dev, "streamed on Pass 1\n");
+
+	return 0;
+
+fail_sensor_on:
+	v4l2_subdev_call(cam_dev->seninf, video, s_stream, 0);
+	return -EPERM;
+}
+
+static int mtk_cam_cio_stream_off(struct mtk_cam_dev *cam_dev)
+{
+	struct device *dev = &cam_dev->pdev->dev;
+	struct isp_p1_device *p1_dev = get_p1_device(dev);
+	struct mtk_isp_p1_ctx *isp_ctx = &p1_dev->isp_ctx;
+	int ret;
+
+	if (!cam_dev->streaming) {
+		dev_warn(dev, "already stream off");
+		return 0;
+	}
+
+	dev_dbg(dev, "stream off: %s\n", cam_dev->sensor->entity.name);
+	ret = v4l2_subdev_call(cam_dev->sensor, video, s_stream, 0);
+	if (ret) {
+		dev_err(dev, "%s stream off failed:%d\n",
+			cam_dev->sensor->entity.name, ret);
+		return -EPERM;
+	}
+
+	dev_dbg(dev, "stream off: %s\n", cam_dev->seninf->entity.name);
+	ret = v4l2_subdev_call(cam_dev->seninf, video, s_stream, 0);
+	if (ret) {
+		dev_err(dev, "%s stream off failed:%d\n",
+			cam_dev->seninf->entity.name, ret);
+		goto fail_sensor_off;
+	}
+
+	isp_composer_stream(isp_ctx, 0);
+	cam_dev->streaming = false;
+	dev_dbg(dev, "streamed off Pass 1\n");
+
+	return 0;
+
+fail_sensor_off:
+	v4l2_subdev_call(cam_dev->seninf, video, s_stream, 1);
+	return -EPERM;
+}
+
+static int mtk_cam_subdev_s_stream(struct v4l2_subdev *sd,
+				   int enable)
+{
+	struct mtk_cam_dev *cam_dev = mtk_cam_subdev_to_dev(sd);
+
+	if (enable)
+		return mtk_cam_cio_stream_on(cam_dev);
+	else
+		return mtk_cam_cio_stream_off(cam_dev);
+}
+
+static int mtk_cam_subdev_subscribe_event(struct v4l2_subdev *subdev,
+					  struct v4l2_fh *fh,
+					  struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_FRAME_SYNC:
+		return v4l2_event_subscribe(fh, sub, 0, NULL);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mtk_cam_link_setup(struct media_entity *entity,
+			      const struct media_pad *local,
+	const struct media_pad *remote, u32 flags)
+{
+	struct mtk_cam_dev *cam_dev =
+		container_of(entity, struct mtk_cam_dev, subdev.entity);
+	u32 pad = local->index;
+
+	dev_dbg(&cam_dev->pdev->dev, "link setup: %d -> %d\n",
+		pad, remote->index);
+
+	if (pad < cam_dev->dev_node_num)
+		cam_dev->mem2mem2_nodes[pad].enabled =
+			!!(flags & MEDIA_LNK_FL_ENABLED);
+
+	return 0;
+}
+
+static void mtk_cam_dev_queue_buffers(struct vb2_buffer *vb)
+{
+	struct mtk_cam_dev *cam_dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct mtk_cam_video_device *node = mtk_cam_vbq_to_vdev(vb->vb2_queue);
+	struct mtk_cam_dev_buffer *buf;
+
+	buf = mtk_cam_vb2_buf_to_dev_buf(vb);
+	buf->daddr = vb2_dma_contig_plane_dma_addr(&buf->vbb.vb2_buf, 0);
+	buf->scp_addr = 0;
+
+	dev_dbg(&cam_dev->pdev->dev, "%pad:%pad\n",
+		&buf->daddr, &buf->scp_addr);
+
+	mtk_isp_enqueue(&cam_dev->pdev->dev, node->desc.dma_port, buf);
+}
+
+static void mtk_cam_vb2_buf_queue(struct vb2_buffer *vb)
+{
+	struct mtk_cam_dev *mtk_cam_dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct mtk_cam_video_device *node = mtk_cam_vbq_to_vdev(vb->vb2_queue);
+	struct device *dev = &mtk_cam_dev->pdev->dev;
+	struct mtk_cam_dev_buffer *buf;
+	struct vb2_v4l2_buffer *v4l2_buf;
+
+	buf = mtk_cam_vb2_buf_to_dev_buf(vb);
+	v4l2_buf = to_vb2_v4l2_buffer(vb);
+
+	dev_dbg(dev, "%s: node:%d fd:%d idx:%d\n",
+		__func__,
+		node->id,
+		v4l2_buf->request_fd,
+		v4l2_buf->vb2_buf.index);
+
+	if (v4l2_buf->request_fd < 0) {
+		mtk_cam_dev_queue_buffers(vb);
+		return;
+	}
+
+	if (mtk_cam_dev->request_fd != v4l2_buf->request_fd) {
+		mtk_cam_dev->request_fd = v4l2_buf->request_fd;
+		mtk_cam_dev->request_count =
+			vb->req_obj.req->num_incomplete_objects;
+		dev_dbg(dev, "init  mtk_cam_dev_buf, fd(%d) count(%d)\n",
+			v4l2_buf->request_fd,
+			vb->req_obj.req->num_incomplete_objects);
+	}
+
+	/* Added the buffer into the tracking list */
+	spin_lock(&node->slock);
+	list_add_tail(&buf->list, &node->pending_list);
+	spin_unlock(&node->slock);
+
+	mtk_cam_dev->request_count--;
+
+	if (!mtk_cam_dev->request_count) {
+		mtk_cam_dev->request_fd = -1;
+		mtk_cam_dev_queue_req_buffers(mtk_cam_dev);
+	}
+}
+
+static int mtk_cam_vb2_queue_setup(struct vb2_queue *vq,
+				   unsigned int *num_buffers,
+				   unsigned int *num_planes,
+				   unsigned int sizes[],
+				   struct device *alloc_devs[])
+{
+	struct mtk_cam_dev *cam_dev = vb2_get_drv_priv(vq);
+	struct mtk_cam_video_device *node = mtk_cam_vbq_to_vdev(vq);
+	struct device *dev = &cam_dev->pdev->dev;
+	unsigned int max_buffer_count = node->desc.max_buf_count;
+	const struct v4l2_format *fmt = &node->vdev_fmt;
+	unsigned int size;
+
+	/* Check the limitation of buffer size */
+	if (max_buffer_count > 0)
+		*num_buffers = clamp_val(*num_buffers, 1, max_buffer_count);
+	else
+		*num_buffers = clamp_val(*num_buffers, 1, VB2_MAX_FRAME);
+
+	if (node->desc.smem_alloc) {
+		alloc_devs[0] = cam_dev->smem_dev;
+		dev_dbg(dev, "Select smem alloc_devs(0x%pK)\n", alloc_devs[0]);
+	} else {
+		alloc_devs[0] = &cam_dev->pdev->dev;
+		dev_dbg(dev, "Select default alloc_devs(0x%pK)\n",
+			alloc_devs[0]);
+	}
+
+	if (vq->type == V4L2_BUF_TYPE_META_OUTPUT ||
+	    vq->type == V4L2_BUF_TYPE_META_CAPTURE)
+		size = fmt->fmt.meta.buffersize;
+	else
+		size = fmt->fmt.pix_mp.plane_fmt[0].sizeimage;
+
+	/* Validate initialized num_planes & size[0] */
+	if (*num_planes) {
+		if (sizes[0] < size)
+			return -EINVAL;
+	} else {
+		*num_planes = 1;
+		sizes[0] = size;
+	}
+
+	/* Initialize buffer queue & locks */
+	INIT_LIST_HEAD(&node->pending_list);
+	mutex_init(&node->lock);
+	spin_lock_init(&node->slock);
+
+	return 0;
+}
+
+static bool
+mtk_cam_all_nodes_streaming(struct mtk_cam_dev *cam_dev,
+			    struct mtk_cam_video_device *except)
+{
+	unsigned int i;
+
+	for (i = 0; i < cam_dev->dev_node_num; i++) {
+		struct mtk_cam_video_device *node = &cam_dev->mem2mem2_nodes[i];
+
+		if (node == except)
+			continue;
+		if (node->enabled && !vb2_start_streaming_called(&node->vbq))
+			return false;
+	}
+
+	return true;
+}
+
+static void mtk_cam_return_all_buffers(struct mtk_cam_dev *cam_dev,
+				       struct mtk_cam_video_device *node,
+				       enum vb2_buffer_state state)
+{
+	struct mtk_cam_dev_buffer *b, *b0;
+	unsigned int i;
+
+	dev_dbg(&cam_dev->pdev->dev, "%s: node:%s", __func__, node->vdev.name);
+
+	/* Return all buffers */
+	spin_lock(&node->slock);
+	list_for_each_entry_safe(b, b0, &node->pending_list, list) {
+		list_del(&b->list);
+	}
+	spin_unlock(&node->slock);
+
+	for (i = 0; i < node->vbq.num_buffers; ++i)
+		if (node->vbq.bufs[i]->state == VB2_BUF_STATE_ACTIVE)
+			vb2_buffer_done(node->vbq.bufs[i], state);
+}
+
+static int mtk_cam_vb2_start_streaming(struct vb2_queue *vq,
+				       unsigned int count)
+{
+	struct mtk_cam_dev *cam_dev = vb2_get_drv_priv(vq);
+	struct mtk_cam_video_device *node = mtk_cam_vbq_to_vdev(vq);
+	int ret;
+
+	if (!node->enabled) {
+		dev_err(&cam_dev->pdev->dev, "Node:%d is not enable\n",
+			node->id);
+		ret = -ENOLINK;
+		goto fail_return_bufs;
+	}
+
+	ret = media_pipeline_start(&node->vdev.entity, &cam_dev->pipeline);
+	if (ret < 0) {
+		dev_err(&cam_dev->pdev->dev, "Node:%d %s failed\n",
+			node->id, __func__);
+		goto fail_return_bufs;
+	}
+
+	if (!mtk_cam_all_nodes_streaming(cam_dev, node))
+		return 0;
+
+	/* Start streaming of the whole pipeline now */
+	ret = v4l2_subdev_call(&cam_dev->subdev, video, s_stream, 1);
+	if (ret < 0) {
+		dev_err(&cam_dev->pdev->dev, "Node:%d s_stream failed\n",
+			node->id);
+		goto fail_stop_pipeline;
+	}
+	return 0;
+
+fail_stop_pipeline:
+	media_pipeline_stop(&node->vdev.entity);
+fail_return_bufs:
+	mtk_cam_return_all_buffers(cam_dev, node, VB2_BUF_STATE_QUEUED);
+	return ret;
+}
+
+static void mtk_cam_vb2_stop_streaming(struct vb2_queue *vq)
+{
+	struct mtk_cam_dev *cam_dev = vb2_get_drv_priv(vq);
+	struct mtk_cam_video_device *node = mtk_cam_vbq_to_vdev(vq);
+
+	/* Was this the first node with streaming disabled? */
+	if (mtk_cam_all_nodes_streaming(cam_dev, node)) {
+		/* Yes, really stop streaming now */
+		if (v4l2_subdev_call(&cam_dev->subdev, video, s_stream, 0))
+			dev_err(&cam_dev->pdev->dev,
+				"failed to stop streaming\n");
+	}
+	mtk_cam_return_all_buffers(cam_dev, node, VB2_BUF_STATE_ERROR);
+	media_pipeline_stop(&node->vdev.entity);
+}
+
+int mtk_cam_videoc_querycap(struct file *file, void *fh,
+			    struct v4l2_capability *cap)
+{
+	struct mtk_cam_dev *cam_dev = video_drvdata(file);
+
+	strscpy(cap->driver, MTK_CAM_DEV_P1_NAME, sizeof(cap->driver));
+	strscpy(cap->card, MTK_CAM_DEV_P1_NAME, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(cam_dev->media_dev.dev));
+
+	return 0;
+}
+
+int mtk_cam_videoc_enum_fmt(struct file *file, void *fh,
+			    struct v4l2_fmtdesc *f)
+{
+	struct mtk_cam_video_device *node = file_to_mtk_cam_node(file);
+
+	if (f->index >= node->desc.num_fmts || f->type != node->vbq.type)
+		return -EINVAL;
+
+	f->pixelformat = node->desc.fmts[f->index].fmt.pix_mp.pixelformat;
+	f->flags = 0;
+
+	return 0;
+}
+
+int mtk_cam_videoc_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct mtk_cam_video_device *node = file_to_mtk_cam_node(file);
+
+	if (f->type != node->vbq.type)
+		return -EINVAL;
+
+	f->fmt = node->vdev_fmt.fmt;
+
+	return 0;
+}
+
+int mtk_cam_videoc_try_fmt(struct file *file, void *fh,
+			   struct v4l2_format *in_fmt)
+{
+	struct mtk_cam_dev *cam_dev = video_drvdata(file);
+	struct mtk_cam_video_device *node = file_to_mtk_cam_node(file);
+	struct v4l2_format *dev_fmt;
+	__u32  width, height;
+
+	if (in_fmt->type != node->vbq.type)
+		return -EINVAL;
+
+	dev_dbg(&cam_dev->pdev->dev, "%s: fmt:%c%c%c%c, w*h:%u*%u\n",
+		__func__,
+		(in_fmt->fmt.pix_mp.pixelformat & 0xFF),
+		(in_fmt->fmt.pix_mp.pixelformat >> 8) & 0xFF,
+		(in_fmt->fmt.pix_mp.pixelformat >> 16) & 0xFF,
+		(in_fmt->fmt.pix_mp.pixelformat >> 24) & 0xFF,
+		in_fmt->fmt.pix_mp.width, in_fmt->fmt.pix_mp.height);
+
+	width = in_fmt->fmt.pix_mp.width;
+	height = in_fmt->fmt.pix_mp.height;
+
+	dev_fmt = mtk_cam_dev_find_fmt(&node->desc,
+				       in_fmt->fmt.pix_mp.pixelformat);
+	if (dev_fmt) {
+		mtk_cam_dev_fmt_set_img(&cam_dev->pdev->dev,
+					&in_fmt->fmt.pix_mp,
+					&dev_fmt->fmt.pix_mp,
+					node->id);
+	} else {
+		mtk_cam_dev_load_default_fmt(&cam_dev->pdev->dev,
+					     &node->desc,
+					     in_fmt);
+	}
+	in_fmt->fmt.pix_mp.width = clamp_t(u32,
+					   width,
+					   CAM_MIN_WIDTH,
+					   in_fmt->fmt.pix_mp.width);
+	in_fmt->fmt.pix_mp.height = clamp_t(u32,
+					    height,
+					    CAM_MIN_HEIGHT,
+					    in_fmt->fmt.pix_mp.height);
+	mtk_cam_dev_cal_mplane_pix_fmt(&cam_dev->pdev->dev,
+				       &in_fmt->fmt.pix_mp,
+				       node->id);
+
+	return 0;
+}
+
+int mtk_cam_videoc_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct mtk_cam_dev *cam_dev = video_drvdata(file);
+	struct mtk_cam_video_device *node = file_to_mtk_cam_node(file);
+
+	if (f->type != node->vbq.type)
+		return -EINVAL;
+
+	if (cam_dev->streaming)
+		return -EBUSY;
+
+	/* Get the valid format */
+	mtk_cam_videoc_try_fmt(file, fh, f);
+
+	/* Configure to video device */
+	mtk_cam_dev_fmt_set_img(&cam_dev->pdev->dev,
+				&node->vdev_fmt.fmt.pix_mp,
+				&f->fmt.pix_mp,
+				node->id);
+
+	return 0;
+}
+
+int mtk_cam_vidioc_enum_input(struct file *file, void *fh,
+			      struct v4l2_input *input)
+{
+	if (input->index > 0)
+		return -EINVAL;
+
+	strscpy(input->name, "camera", sizeof(input->name));
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+
+	return 0;
+}
+
+int mtk_cam_vidioc_g_input(struct file *file, void *fh, unsigned int *input)
+{
+	*input = 0;
+
+	return 0;
+}
+
+int mtk_cam_vidioc_s_input(struct file *file, void *fh, unsigned int input)
+{
+	return input == 0 ? 0 : -EINVAL;
+}
+
+int mtk_cam_vidioc_subscribe_event(struct v4l2_fh *fh,
+				   const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+int mtk_cam_enum_framesizes(struct file *filp, void *priv,
+			    struct v4l2_frmsizeenum *sizes)
+{
+	struct mtk_cam_video_device *node = file_to_mtk_cam_node(filp);
+	struct v4l2_format *dev_fmt;
+
+	dev_fmt = mtk_cam_dev_find_fmt(&node->desc, sizes->pixel_format);
+	if (!dev_fmt || sizes->index)
+		return -EINVAL;
+
+	if (node->id == MTK_CAM_P1_MAIN_STREAM_OUT) {
+		sizes->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+		sizes->stepwise.max_width = IMG_MAX_WIDTH;
+		sizes->stepwise.min_width = IMG_MIN_WIDTH;
+		sizes->stepwise.max_height = IMG_MAX_HEIGHT;
+		sizes->stepwise.min_height = IMG_MIN_HEIGHT;
+		sizes->stepwise.step_height = 1;
+		sizes->stepwise.step_width = 1;
+	} else if (node->id == MTK_CAM_P1_PACKED_BIN_OUT) {
+		sizes->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+		sizes->stepwise.max_width = RRZ_MAX_WIDTH;
+		sizes->stepwise.min_width = RRZ_MIN_WIDTH;
+		sizes->stepwise.max_height = RRZ_MAX_HEIGHT;
+		sizes->stepwise.min_height = RRZ_MIN_HEIGHT;
+		sizes->stepwise.step_height = 1;
+		sizes->stepwise.step_width = 1;
+	}
+
+	return 0;
+}
+
+int mtk_cam_meta_enum_format(struct file *file, void *fh,
+			     struct v4l2_fmtdesc *f)
+{
+	struct mtk_cam_video_device *node = file_to_mtk_cam_node(file);
+
+	/* Each node is dedicated to only one meta format */
+	if (f->index > 0 || f->type != node->vbq.type)
+		return -EINVAL;
+
+	strscpy(f->description, node->desc.description,
+		sizeof(node->desc.description));
+	f->pixelformat = node->vdev_fmt.fmt.meta.dataformat;
+
+	return 0;
+}
+
+int mtk_cam_videoc_g_meta_fmt(struct file *file, void *fh,
+			      struct v4l2_format *f)
+{
+	struct mtk_cam_video_device *node = file_to_mtk_cam_node(file);
+
+	/* Each node is dedicated to only one meta format */
+	if (f->type != node->vbq.type)
+		return -EINVAL;
+
+	f->fmt = node->vdev_fmt.fmt;
+
+	return 0;
+}
+
+/* subdev internal operations */
+static const struct v4l2_subdev_internal_ops mtk_cam_subdev_internal_ops = {
+	.open = mtk_cam_subdev_open,
+	.close = mtk_cam_subdev_close,
+};
+
+static const struct v4l2_subdev_core_ops mtk_cam_subdev_core_ops = {
+	.subscribe_event = mtk_cam_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops mtk_cam_subdev_video_ops = {
+	.s_stream = mtk_cam_subdev_s_stream,
+};
+
+static const struct v4l2_subdev_ops mtk_cam_subdev_ops = {
+	.core = &mtk_cam_subdev_core_ops,
+	.video = &mtk_cam_subdev_video_ops,
+};
+
+static const struct media_entity_operations mtk_cam_media_ops = {
+	.link_setup = mtk_cam_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static void mtk_cam_vb2_buf_request_complete(struct vb2_buffer *vb)
+{
+	struct mtk_cam_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+
+	v4l2_ctrl_request_complete(vb->req_obj.req,
+				   dev->v4l2_dev.ctrl_handler);
+}
+
+static const struct vb2_ops mtk_cam_vb2_ops = {
+	.buf_queue = mtk_cam_vb2_buf_queue,
+	.queue_setup = mtk_cam_vb2_queue_setup,
+	.start_streaming = mtk_cam_vb2_start_streaming,
+	.stop_streaming = mtk_cam_vb2_stop_streaming,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.buf_request_complete = mtk_cam_vb2_buf_request_complete,
+};
+
+static const struct v4l2_file_operations mtk_cam_v4l2_fops = {
+	.unlocked_ioctl = video_ioctl2,
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.poll = vb2_fop_poll,
+	.mmap = vb2_fop_mmap,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl32 = v4l2_compat_ioctl32,
+#endif
+};
+
+/*
+ * Config node's video properties
+ * according to the device context requirement
+ */
+static void mtk_cam_node_to_v4l2(struct mtk_cam_dev *cam_dev,
+				 unsigned int node,
+				 struct video_device *vdev,
+				 struct v4l2_format *f)
+{
+	struct mtk_cam_dev_node_desc *node_desc =
+		&cam_dev->mem2mem2_nodes[node].desc;
+
+	/* set cap/type/ioctl_ops of the video device */
+	vdev->device_caps = V4L2_CAP_STREAMING | node_desc->cap;
+	f->type = node_desc->buf_type;
+	vdev->ioctl_ops = node_desc->ioctl_ops;
+
+	mtk_cam_dev_load_default_fmt(&cam_dev->pdev->dev,
+				     node_desc,
+				     f);
+}
+
+static const struct media_device_ops mtk_cam_media_req_ops = {
+	.req_validate = vb2_request_validate,
+	.req_queue = vb2_request_queue,
+};
+
+static int mtk_cam_media_register(struct device *dev,
+				  struct media_device *media_dev)
+{
+	int ret;
+
+	media_dev->dev = dev;
+	strscpy(media_dev->model, MTK_CAM_DEV_P1_NAME,
+		sizeof(media_dev->model));
+	snprintf(media_dev->bus_info, sizeof(media_dev->bus_info),
+		 "platform:%s", dev_name(dev));
+	media_dev->hw_revision = 0;
+	media_device_init(media_dev);
+	media_dev->ops = &mtk_cam_media_req_ops;
+	dev_info(dev, "Register media device: %s, 0x%pK",
+		 MTK_CAM_DEV_P1_NAME, media_dev);
+
+	ret = media_device_register(media_dev);
+	if (ret) {
+		dev_err(dev, "failed to register media device (%d)\n", ret);
+		goto fail_v4l2_dev;
+	}
+
+	return 0;
+
+fail_v4l2_dev:
+	media_device_unregister(media_dev);
+	media_device_cleanup(media_dev);
+
+	return ret;
+}
+
+int mtk_cam_v4l2_register(struct device *dev,
+			  struct media_device *media_dev,
+			  struct v4l2_device *v4l2_dev,
+			  struct v4l2_ctrl_handler *ctrl_handler)
+{
+	int ret;
+
+	/* Set up v4l2 device */
+	v4l2_dev->ctrl_handler = ctrl_handler;
+	v4l2_dev->mdev = media_dev;
+	dev_info(dev, "Register v4l2 device: 0x%pK", v4l2_dev);
+	ret = v4l2_device_register(dev, v4l2_dev);
+	if (ret) {
+		dev_err(dev, "failed to register V4L2 device (%d)\n", ret);
+		goto fail_v4l2_dev;
+	}
+
+	return 0;
+
+fail_v4l2_dev:
+	media_device_unregister(media_dev);
+	media_device_cleanup(media_dev);
+
+	return ret;
+}
+
+int mtk_cam_mem2mem2_v4l2_register(struct mtk_cam_dev *cam_dev)
+{
+	struct device *dev = &cam_dev->pdev->dev;
+	unsigned int num_nodes = cam_dev->dev_node_num;
+	/* Total pad numbers is video devices + one seninf pad */
+	unsigned int num_subdev_pads = MTK_CAM_DEV_NODES + 1;
+	unsigned int i;
+	int ret;
+
+	ret = mtk_cam_media_register(dev,
+				     &cam_dev->media_dev);
+	if (ret) {
+		dev_err(dev, "failed to register media device:%d\n", ret);
+		goto fail_media_dev;
+	}
+
+	ret = mtk_cam_v4l2_register(dev,
+				    &cam_dev->media_dev,
+				    &cam_dev->v4l2_dev,
+				    NULL);
+	if (ret) {
+		dev_err(dev, "failed to register V4L2 device:%d\n", ret);
+		goto fail_v4l2_dev;
+	}
+
+	/* Initialize subdev media entity */
+	cam_dev->subdev_pads = devm_kcalloc(dev, num_subdev_pads,
+					    sizeof(*cam_dev->subdev_pads),
+					    GFP_KERNEL);
+	if (!cam_dev->subdev_pads) {
+		ret = -ENOMEM;
+		goto fail_subdev_pads;
+	}
+
+	ret = media_entity_pads_init(&cam_dev->subdev.entity,
+				     num_subdev_pads,
+				     cam_dev->subdev_pads);
+	if (ret) {
+		dev_err(dev, "failed initialize media pads:%d:\n", ret);
+		goto fail_subdev_pads;
+	}
+
+	/* Initialize all pads with MEDIA_PAD_FL_SOURCE */
+	for (i = 0; i < num_subdev_pads; i++)
+		cam_dev->subdev_pads[i].flags = MEDIA_PAD_FL_SOURCE;
+
+	/* Customize the last one pad as CIO sink pad. */
+	cam_dev->subdev_pads[MTK_CAM_DEV_NODES].flags = MEDIA_PAD_FL_SINK;
+
+	/* Initialize subdev */
+	v4l2_subdev_init(&cam_dev->subdev, &mtk_cam_subdev_ops);
+	cam_dev->subdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_STATISTICS;
+	cam_dev->subdev.entity.ops = &mtk_cam_media_ops;
+	cam_dev->subdev.flags = V4L2_SUBDEV_FL_HAS_DEVNODE |
+				V4L2_SUBDEV_FL_HAS_EVENTS;
+	snprintf(cam_dev->subdev.name, sizeof(cam_dev->subdev.name),
+		 "%s", MTK_CAM_DEV_P1_NAME);
+	v4l2_set_subdevdata(&cam_dev->subdev, cam_dev);
+	cam_dev->subdev.internal_ops = &mtk_cam_subdev_internal_ops;
+
+	dev_info(dev, "register subdev: %s\n", cam_dev->subdev.name);
+	ret = v4l2_device_register_subdev(&cam_dev->v4l2_dev, &cam_dev->subdev);
+	if (ret) {
+		dev_err(dev, "failed initialize subdev:%d\n", ret);
+		goto fail_subdev;
+	}
+
+	/* Create video nodes and links */
+	for (i = 0; i < num_nodes; i++) {
+		struct mtk_cam_video_device *node = &cam_dev->mem2mem2_nodes[i];
+		struct video_device *vdev = &node->vdev;
+		struct vb2_queue *vbq = &node->vbq;
+		u32 output = !cam_dev->mem2mem2_nodes[i].desc.capture;
+		u32 link_flags = cam_dev->mem2mem2_nodes[i].desc.link_flags;
+
+		cam_dev->subdev_pads[i].flags = output ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+
+		/* Initialize miscellaneous variables */
+		mutex_init(&node->lock);
+		spin_lock_init(&node->slock);
+		INIT_LIST_HEAD(&node->pending_list);
+
+		/* Initialize formats to default values */
+		mtk_cam_node_to_v4l2(cam_dev, i, vdev, &node->vdev_fmt);
+
+		/* Initialize media entities */
+		ret = media_entity_pads_init(&vdev->entity, 1, &node->vdev_pad);
+		if (ret) {
+			dev_err(dev, "failed initialize media pad:%d\n", ret);
+			goto fail_vdev_media_entity;
+		}
+		node->enabled = false;
+		node->id = i;
+		node->vdev_pad.flags = cam_dev->subdev_pads[i].flags;
+		vdev->entity.ops = NULL;
+
+		/* Initialize vbq */
+		vbq->type = node->vdev_fmt.type;
+		if (vbq->type == V4L2_BUF_TYPE_META_OUTPUT)
+			vbq->io_modes = VB2_MMAP;
+		else
+			vbq->io_modes = VB2_MMAP | VB2_DMABUF;
+		if (node->desc.smem_alloc)
+			vbq->bidirectional = 1;
+		if (vbq->type == V4L2_BUF_TYPE_META_CAPTURE)
+			vdev->entity.function =
+				MEDIA_ENT_F_PROC_VIDEO_STATISTICS;
+		vbq->ops = &mtk_cam_vb2_ops;
+		vbq->mem_ops = &vb2_dma_contig_memops;
+		vbq->buf_struct_size = sizeof(struct mtk_cam_dev_buffer);
+		vbq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		vbq->min_buffers_needed = 0;	/* Can streamon w/o buffers */
+		/* Put the process hub sub device in the vb2 private data */
+		vbq->drv_priv = cam_dev;
+		vbq->lock = &node->lock;
+		vbq->supports_requests = true;
+
+		ret = vb2_queue_init(vbq);
+		if (ret) {
+			dev_err(dev, "failed to init. vb2 queue:%d\n", ret);
+			goto fail_vdev;
+		}
+
+		/* Initialize vdev */
+		snprintf(vdev->name, sizeof(vdev->name), "%s %s",
+			 MTK_CAM_DEV_P1_NAME, node->desc.name);
+		vdev->release = video_device_release_empty;
+		vdev->fops = &mtk_cam_v4l2_fops;
+		vdev->lock = &node->lock;
+		vdev->v4l2_dev = &cam_dev->v4l2_dev;
+		vdev->queue = &node->vbq;
+		vdev->vfl_dir = output ? VFL_DIR_TX : VFL_DIR_RX;
+		/* Enable private control for image video devices */
+		if (node->desc.image) {
+			mtk_cam_ctrl_init(cam_dev, &node->ctrl_handler);
+			vdev->ctrl_handler = &node->ctrl_handler;
+		}
+		video_set_drvdata(vdev, cam_dev);
+		dev_dbg(dev, "register vdev:%d:%s\n", i, vdev->name);
+
+		ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+		if (ret) {
+			dev_err(dev, "failed to register vde:%d\n", ret);
+			goto fail_vdev;
+		}
+
+		/* Create link between video node and the subdev pad */
+		if (output) {
+			ret = media_create_pad_link(&vdev->entity, 0,
+						    &cam_dev->subdev.entity,
+						    i, link_flags);
+		} else {
+			ret = media_create_pad_link(&cam_dev->subdev.entity,
+						    i, &vdev->entity, 0,
+						    link_flags);
+		}
+		if (ret)
+			goto fail_link;
+	}
+
+	vb2_dma_contig_set_max_seg_size(dev, DMA_BIT_MASK(32));
+
+	return 0;
+
+	for (; i >= 0; i--) {
+fail_link:
+		video_unregister_device(&cam_dev->mem2mem2_nodes[i].vdev);
+fail_vdev:
+		media_entity_cleanup(&cam_dev->mem2mem2_nodes[i].vdev.entity);
+fail_vdev_media_entity:
+		mutex_destroy(&cam_dev->mem2mem2_nodes[i].lock);
+	}
+fail_subdev:
+	media_entity_cleanup(&cam_dev->subdev.entity);
+fail_subdev_pads:
+	v4l2_device_unregister(&cam_dev->v4l2_dev);
+fail_v4l2_dev:
+fail_media_dev:
+	dev_err(dev, "fail_v4l2_dev mdev: 0x%pK:%d", &cam_dev->media_dev, ret);
+	media_device_unregister(&cam_dev->media_dev);
+	media_device_cleanup(&cam_dev->media_dev);
+
+	return ret;
+}
+
+int mtk_cam_v4l2_unregister(struct mtk_cam_dev *cam_dev)
+{
+	unsigned int i;
+	struct mtk_cam_video_device *dev;
+
+	for (i = 0; i < cam_dev->dev_node_num; i++) {
+		dev = &cam_dev->mem2mem2_nodes[i];
+		video_unregister_device(&dev->vdev);
+		media_entity_cleanup(&dev->vdev.entity);
+		mutex_destroy(&dev->lock);
+		if (dev->desc.image)
+			v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	}
+
+	vb2_dma_contig_clear_max_seg_size(&cam_dev->pdev->dev);
+	v4l2_device_unregister_subdev(&cam_dev->subdev);
+	media_entity_cleanup(&cam_dev->subdev.entity);
+	kfree(cam_dev->subdev_pads);
+	v4l2_device_unregister(&cam_dev->v4l2_dev);
+	media_device_unregister(&cam_dev->media_dev);
+	media_device_cleanup(&cam_dev->media_dev);
+
+	return 0;
+}
+
+static int mtk_cam_dev_complete(struct v4l2_async_notifier *notifier)
+{
+	struct mtk_cam_dev *cam_dev =
+		container_of(notifier, struct mtk_cam_dev, notifier);
+	struct device *dev = &cam_dev->pdev->dev;
+	int ret;
+
+	ret = media_create_pad_link(&cam_dev->seninf->entity,
+				    MTK_CAM_SENINF_PAD_SRC,
+				    &cam_dev->subdev.entity,
+				    MTK_CAM_P1_HUB_PAD_SINK,
+				    0);
+	if (ret)
+		dev_err(dev, "fail to create pad link %s %s err:%d\n",
+			cam_dev->seninf->entity.name,
+			cam_dev->subdev.entity.name,
+			ret);
+
+	dev_info(dev, "Complete the v4l2 registration\n");
+
+	ret = v4l2_device_register_subdev_nodes(&cam_dev->v4l2_dev);
+	if (ret) {
+		dev_err(dev, "failed initialize subdev nodes:%d\n", ret);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int mtk_cam_dev_notifier_bound(struct v4l2_async_notifier *notifier,
+				      struct v4l2_subdev *sd,
+				      struct v4l2_async_subdev *asd)
+{
+	struct mtk_cam_dev *cam_dev =
+		container_of(notifier, struct mtk_cam_dev, notifier);
+
+	cam_dev->seninf = sd;
+	dev_info(&cam_dev->pdev->dev, "%s is bounded\n", sd->entity.name);
+	return 0;
+}
+
+static void mtk_cam_dev_notifier_unbind(struct v4l2_async_notifier *notifier,
+					struct v4l2_subdev *sd,
+					struct v4l2_async_subdev *asd)
+{
+	struct mtk_cam_dev *cam_dev =
+		container_of(notifier, struct mtk_cam_dev, notifier);
+
+	dev_dbg(&cam_dev->pdev->dev, "%s is unbounded\n", sd->entity.name);
+}
+
+static int mtk_cam_dev_notifier_complete(struct v4l2_async_notifier *notifier)
+{
+	return mtk_cam_dev_complete(notifier);
+}
+
+static const struct v4l2_async_notifier_operations mtk_cam_async_ops = {
+	.bound = mtk_cam_dev_notifier_bound,
+	.unbind = mtk_cam_dev_notifier_unbind,
+	.complete = mtk_cam_dev_notifier_complete,
+};
+
+static int mtk_cam_dev_fwnode_parse(struct device *dev,
+				    struct v4l2_fwnode_endpoint *vep,
+				    struct v4l2_async_subdev *asd)
+{
+	return 0;
+}
+
+int mtk_cam_v4l2_async_register(struct mtk_cam_dev *cam_dev)
+{
+	int ret;
+
+	ret = v4l2_async_notifier_parse_fwnode_endpoints(
+		&cam_dev->pdev->dev, &cam_dev->notifier,
+		sizeof(struct v4l2_async_subdev),
+		mtk_cam_dev_fwnode_parse);
+	if (ret < 0)
+		return ret;
+
+	if (!cam_dev->notifier.num_subdevs)
+		return -ENODEV;
+
+	cam_dev->notifier.ops = &mtk_cam_async_ops;
+	dev_info(&cam_dev->pdev->dev, "mtk_cam v4l2_async_notifier_register\n");
+	ret = v4l2_async_notifier_register(&cam_dev->v4l2_dev,
+					   &cam_dev->notifier);
+	if (ret) {
+		dev_err(&cam_dev->pdev->dev,
+			"failed to register async notifier : %d\n", ret);
+		v4l2_async_notifier_cleanup(&cam_dev->notifier);
+	}
+
+	return ret;
+}
+
+void mtk_cam_v4l2_async_unregister(struct mtk_cam_dev *cam_dev)
+{
+	v4l2_async_notifier_unregister(&cam_dev->notifier);
+	v4l2_async_notifier_cleanup(&cam_dev->notifier);
+}
diff --git a/drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-v4l2-util.h b/drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-v4l2-util.h
new file mode 100644
index 000000000000..73b36916da08
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/isp_50/cam/mtk_cam-v4l2-util.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Frederic Chen <frederic.chen@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MTK_CAM_DEV_V4L2_H__
+#define __MTK_CAM_DEV_V4L2_H__
+
+#include <media/v4l2-device.h>
+#include <media/videobuf2-v4l2.h>
+
+int mtk_cam_videoc_querycap(struct file *file, void *fh,
+			    struct v4l2_capability *cap);
+int mtk_cam_enum_framesizes(struct file *filp, void *priv,
+			    struct v4l2_frmsizeenum *sizes);
+int mtk_cam_videoc_enum_fmt(struct file *file, void *fh,
+			    struct v4l2_fmtdesc *f);
+int mtk_cam_videoc_g_fmt(struct file *file, void *fh, struct v4l2_format *f);
+int mtk_cam_videoc_s_fmt(struct file *file, void *fh, struct v4l2_format *f);
+int mtk_cam_videoc_try_fmt(struct file *file,
+			   void *fh, struct v4l2_format *in_fmt);
+int mtk_cam_vidioc_enum_input(struct file *file, void *fh,
+			      struct v4l2_input *input);
+int mtk_cam_vidioc_g_input(struct file *file, void *fh, unsigned int *input);
+int mtk_cam_vidioc_s_input(struct file *file, void *fh, unsigned int input);
+int mtk_cam_meta_enum_format(struct file *file, void *fh,
+			     struct v4l2_fmtdesc *f);
+int mtk_cam_videoc_g_meta_fmt(struct file *file, void *fh,
+			      struct v4l2_format *f);
+int mtk_cam_vidioc_subscribe_event(struct v4l2_fh *fh,
+				   const struct v4l2_event_subscription *sub);
+
+#endif /* __MTK_CAM_DEV_V4L2_H__ */
-- 
2.18.0


_______________________________________________
linux-arm-kernel mailing list
linux-arm-kernel@lists.infradead.org
http://lists.infradead.org/mailman/listinfo/linux-arm-kernel