Adding upstream version 6.1.76.upstream/6.1.76upstream

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

101 files changed, 28472 insertions, 0 deletions

diff --git a/drivers/media/platform/mediatek/Kconfig b/drivers/media/platform/mediatek/Kconfig
new file mode 100644
index 000000000..84104e2cd
--- /dev/null
+++ b/drivers/media/platform/mediatek/Kconfig
@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+comment "Mediatek media platform drivers"
+
+source "drivers/media/platform/mediatek/jpeg/Kconfig"
+source "drivers/media/platform/mediatek/mdp/Kconfig"
+source "drivers/media/platform/mediatek/vcodec/Kconfig"
+source "drivers/media/platform/mediatek/vpu/Kconfig"
+source "drivers/media/platform/mediatek/mdp3/Kconfig"
diff --git a/drivers/media/platform/mediatek/Makefile b/drivers/media/platform/mediatek/Makefile
new file mode 100644
index 000000000..38e6ba917
--- /dev/null
+++ b/drivers/media/platform/mediatek/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0-only
+obj-y += jpeg/
+obj-y += mdp/
+obj-y += vcodec/
+obj-y += vpu/
+obj-y += mdp3/
diff --git a/drivers/media/platform/mediatek/jpeg/Kconfig b/drivers/media/platform/mediatek/jpeg/Kconfig
new file mode 100644
index 000000000..39c4d1bc6
--- /dev/null
+++ b/drivers/media/platform/mediatek/jpeg/Kconfig
@@ -0,0 +1,16 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_MEDIATEK_JPEG
+	tristate "Mediatek JPEG Codec driver"
+	depends on V4L_MEM2MEM_DRIVERS
+	depends on MTK_IOMMU_V1 || MTK_IOMMU || COMPILE_TEST
+	depends on VIDEO_DEV
+	depends on ARCH_MEDIATEK || COMPILE_TEST
+	depends on MTK_SMI || (COMPILE_TEST && MTK_SMI=n)
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	help
+	  Mediatek jpeg codec driver provides HW capability to decode
+	  JPEG format
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mtk-jpeg
diff --git a/drivers/media/platform/mediatek/jpeg/Makefile b/drivers/media/platform/mediatek/jpeg/Makefile
new file mode 100644
index 000000000..76c33aad0
--- /dev/null
+++ b/drivers/media/platform/mediatek/jpeg/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0-only
+mtk_jpeg-objs := mtk_jpeg_core.o \
+		 mtk_jpeg_dec_hw.o \
+		 mtk_jpeg_dec_parse.o \
+		 mtk_jpeg_enc_hw.o
+obj-$(CONFIG_VIDEO_MEDIATEK_JPEG) += mtk_jpeg.o
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.c b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.c
new file mode 100644
index 000000000..f911d3c7d
--- /dev/null
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.c
@@ -0,0 +1,1528 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ *         Xia Jiang <xia.jiang@mediatek.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "mtk_jpeg_enc_hw.h"
+#include "mtk_jpeg_dec_hw.h"
+#include "mtk_jpeg_core.h"
+#include "mtk_jpeg_dec_parse.h"
+
+static struct mtk_jpeg_fmt mtk_jpeg_enc_formats[] = {
+	{
+		.fourcc		= V4L2_PIX_FMT_JPEG,
+		.colplanes	= 1,
+		.flags		= MTK_JPEG_FMT_FLAG_CAPTURE,
+	},
+	{
+		.fourcc		= V4L2_PIX_FMT_NV12M,
+		.hw_format	= JPEG_ENC_YUV_FORMAT_NV12,
+		.h_sample	= {4, 4},
+		.v_sample	= {4, 2},
+		.colplanes	= 2,
+		.h_align	= 4,
+		.v_align	= 4,
+		.flags		= MTK_JPEG_FMT_FLAG_OUTPUT,
+	},
+	{
+		.fourcc		= V4L2_PIX_FMT_NV21M,
+		.hw_format	= JEPG_ENC_YUV_FORMAT_NV21,
+		.h_sample	= {4, 4},
+		.v_sample	= {4, 2},
+		.colplanes	= 2,
+		.h_align	= 4,
+		.v_align	= 4,
+		.flags		= MTK_JPEG_FMT_FLAG_OUTPUT,
+	},
+	{
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.hw_format	= JPEG_ENC_YUV_FORMAT_YUYV,
+		.h_sample	= {8},
+		.v_sample	= {4},
+		.colplanes	= 1,
+		.h_align	= 5,
+		.v_align	= 3,
+		.flags		= MTK_JPEG_FMT_FLAG_OUTPUT,
+	},
+	{
+		.fourcc		= V4L2_PIX_FMT_YVYU,
+		.hw_format	= JPEG_ENC_YUV_FORMAT_YVYU,
+		.h_sample	= {8},
+		.v_sample	= {4},
+		.colplanes	= 1,
+		.h_align	= 5,
+		.v_align	= 3,
+		.flags		= MTK_JPEG_FMT_FLAG_OUTPUT,
+	},
+};
+
+static struct mtk_jpeg_fmt mtk_jpeg_dec_formats[] = {
+	{
+		.fourcc		= V4L2_PIX_FMT_JPEG,
+		.colplanes	= 1,
+		.flags		= MTK_JPEG_FMT_FLAG_OUTPUT,
+	},
+	{
+		.fourcc		= V4L2_PIX_FMT_YUV420M,
+		.h_sample	= {4, 2, 2},
+		.v_sample	= {4, 2, 2},
+		.colplanes	= 3,
+		.h_align	= 5,
+		.v_align	= 4,
+		.flags		= MTK_JPEG_FMT_FLAG_CAPTURE,
+	},
+	{
+		.fourcc		= V4L2_PIX_FMT_YUV422M,
+		.h_sample	= {4, 2, 2},
+		.v_sample	= {4, 4, 4},
+		.colplanes	= 3,
+		.h_align	= 5,
+		.v_align	= 3,
+		.flags		= MTK_JPEG_FMT_FLAG_CAPTURE,
+	},
+};
+
+#define MTK_JPEG_ENC_NUM_FORMATS ARRAY_SIZE(mtk_jpeg_enc_formats)
+#define MTK_JPEG_DEC_NUM_FORMATS ARRAY_SIZE(mtk_jpeg_dec_formats)
+
+struct mtk_jpeg_src_buf {
+	struct vb2_v4l2_buffer b;
+	struct list_head list;
+	struct mtk_jpeg_dec_param dec_param;
+};
+
+static int debug;
+module_param(debug, int, 0644);
+
+static inline struct mtk_jpeg_ctx *ctrl_to_ctx(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct mtk_jpeg_ctx, ctrl_hdl);
+}
+
+static inline struct mtk_jpeg_ctx *mtk_jpeg_fh_to_ctx(struct v4l2_fh *fh)
+{
+	return container_of(fh, struct mtk_jpeg_ctx, fh);
+}
+
+static inline struct mtk_jpeg_src_buf *mtk_jpeg_vb2_to_srcbuf(
+							struct vb2_buffer *vb)
+{
+	return container_of(to_vb2_v4l2_buffer(vb), struct mtk_jpeg_src_buf, b);
+}
+
+static int mtk_jpeg_querycap(struct file *file, void *priv,
+			     struct v4l2_capability *cap)
+{
+	struct mtk_jpeg_dev *jpeg = video_drvdata(file);
+
+	strscpy(cap->driver, jpeg->variant->dev_name, sizeof(cap->driver));
+	strscpy(cap->card, jpeg->variant->dev_name, sizeof(cap->card));
+
+	return 0;
+}
+
+static int vidioc_jpeg_enc_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mtk_jpeg_ctx *ctx = ctrl_to_ctx(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_JPEG_RESTART_INTERVAL:
+		ctx->restart_interval = ctrl->val;
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		ctx->enc_quality = ctrl->val;
+		break;
+	case V4L2_CID_JPEG_ACTIVE_MARKER:
+		ctx->enable_exif = ctrl->val & V4L2_JPEG_ACTIVE_MARKER_APP1;
+		break;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops mtk_jpeg_enc_ctrl_ops = {
+	.s_ctrl = vidioc_jpeg_enc_s_ctrl,
+};
+
+static int mtk_jpeg_enc_ctrls_setup(struct mtk_jpeg_ctx *ctx)
+{
+	const struct v4l2_ctrl_ops *ops = &mtk_jpeg_enc_ctrl_ops;
+	struct v4l2_ctrl_handler *handler = &ctx->ctrl_hdl;
+
+	v4l2_ctrl_handler_init(handler, 3);
+
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_JPEG_RESTART_INTERVAL, 0, 100,
+			  1, 0);
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_JPEG_COMPRESSION_QUALITY, 48,
+			  100, 1, 90);
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_JPEG_ACTIVE_MARKER, 0,
+			  V4L2_JPEG_ACTIVE_MARKER_APP1, 0, 0);
+
+	if (handler->error) {
+		v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+		return handler->error;
+	}
+
+	v4l2_ctrl_handler_setup(&ctx->ctrl_hdl);
+
+	return 0;
+}
+
+static int mtk_jpeg_enum_fmt(struct mtk_jpeg_fmt *mtk_jpeg_formats, int n,
+			     struct v4l2_fmtdesc *f, u32 type)
+{
+	int i, num = 0;
+
+	for (i = 0; i < n; ++i) {
+		if (mtk_jpeg_formats[i].flags & type) {
+			if (num == f->index)
+				break;
+			++num;
+		}
+	}
+
+	if (i >= n)
+		return -EINVAL;
+
+	f->pixelformat = mtk_jpeg_formats[i].fourcc;
+
+	return 0;
+}
+
+static int mtk_jpeg_enum_fmt_vid_cap(struct file *file, void *priv,
+				     struct v4l2_fmtdesc *f)
+{
+	struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+
+	return mtk_jpeg_enum_fmt(jpeg->variant->formats,
+				 jpeg->variant->num_formats, f,
+				 MTK_JPEG_FMT_FLAG_CAPTURE);
+}
+
+static int mtk_jpeg_enum_fmt_vid_out(struct file *file, void *priv,
+				     struct v4l2_fmtdesc *f)
+{
+	struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+
+	return mtk_jpeg_enum_fmt(jpeg->variant->formats,
+				 jpeg->variant->num_formats, f,
+				 MTK_JPEG_FMT_FLAG_OUTPUT);
+}
+
+static struct mtk_jpeg_q_data *mtk_jpeg_get_q_data(struct mtk_jpeg_ctx *ctx,
+						   enum v4l2_buf_type type)
+{
+	if (V4L2_TYPE_IS_OUTPUT(type))
+		return &ctx->out_q;
+	return &ctx->cap_q;
+}
+
+static struct mtk_jpeg_fmt *
+mtk_jpeg_find_format(struct mtk_jpeg_fmt *mtk_jpeg_formats, int num_formats,
+		     u32 pixelformat, unsigned int fmt_type)
+{
+	unsigned int k;
+	struct mtk_jpeg_fmt *fmt;
+
+	for (k = 0; k < num_formats; k++) {
+		fmt = &mtk_jpeg_formats[k];
+
+		if (fmt->fourcc == pixelformat && fmt->flags & fmt_type)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+static int mtk_jpeg_try_fmt_mplane(struct v4l2_pix_format_mplane *pix_mp,
+				   struct mtk_jpeg_fmt *fmt)
+{
+	int i;
+
+	pix_mp->field = V4L2_FIELD_NONE;
+
+	pix_mp->num_planes = fmt->colplanes;
+	pix_mp->pixelformat = fmt->fourcc;
+
+	if (fmt->fourcc == V4L2_PIX_FMT_JPEG) {
+		struct v4l2_plane_pix_format *pfmt = &pix_mp->plane_fmt[0];
+
+		pix_mp->height = clamp(pix_mp->height, MTK_JPEG_MIN_HEIGHT,
+				       MTK_JPEG_MAX_HEIGHT);
+		pix_mp->width = clamp(pix_mp->width, MTK_JPEG_MIN_WIDTH,
+				      MTK_JPEG_MAX_WIDTH);
+
+		pfmt->bytesperline = 0;
+		/* Source size must be aligned to 128 */
+		pfmt->sizeimage = round_up(pfmt->sizeimage, 128);
+		if (pfmt->sizeimage == 0)
+			pfmt->sizeimage = MTK_JPEG_DEFAULT_SIZEIMAGE;
+		return 0;
+	}
+
+	/* other fourcc */
+	pix_mp->height = clamp(round_up(pix_mp->height, fmt->v_align),
+			       MTK_JPEG_MIN_HEIGHT, MTK_JPEG_MAX_HEIGHT);
+	pix_mp->width = clamp(round_up(pix_mp->width, fmt->h_align),
+			      MTK_JPEG_MIN_WIDTH, MTK_JPEG_MAX_WIDTH);
+
+	for (i = 0; i < fmt->colplanes; i++) {
+		struct v4l2_plane_pix_format *pfmt = &pix_mp->plane_fmt[i];
+		u32 stride = pix_mp->width * fmt->h_sample[i] / 4;
+		u32 h = pix_mp->height * fmt->v_sample[i] / 4;
+
+		pfmt->bytesperline = stride;
+		pfmt->sizeimage = stride * h;
+	}
+	return 0;
+}
+
+static int mtk_jpeg_g_fmt_vid_mplane(struct file *file, void *priv,
+				     struct v4l2_format *f)
+{
+	struct vb2_queue *vq;
+	struct mtk_jpeg_q_data *q_data = NULL;
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+	int i;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = mtk_jpeg_get_q_data(ctx, f->type);
+
+	pix_mp->width = q_data->pix_mp.width;
+	pix_mp->height = q_data->pix_mp.height;
+	pix_mp->field = V4L2_FIELD_NONE;
+	pix_mp->pixelformat = q_data->fmt->fourcc;
+	pix_mp->num_planes = q_data->fmt->colplanes;
+	pix_mp->colorspace = q_data->pix_mp.colorspace;
+	pix_mp->ycbcr_enc = q_data->pix_mp.ycbcr_enc;
+	pix_mp->xfer_func = q_data->pix_mp.xfer_func;
+	pix_mp->quantization = q_data->pix_mp.quantization;
+
+	v4l2_dbg(1, debug, &jpeg->v4l2_dev, "(%d) g_fmt:%c%c%c%c wxh:%ux%u\n",
+		 f->type,
+		 (pix_mp->pixelformat & 0xff),
+		 (pix_mp->pixelformat >>  8 & 0xff),
+		 (pix_mp->pixelformat >> 16 & 0xff),
+		 (pix_mp->pixelformat >> 24 & 0xff),
+		 pix_mp->width, pix_mp->height);
+
+	for (i = 0; i < pix_mp->num_planes; i++) {
+		struct v4l2_plane_pix_format *pfmt = &pix_mp->plane_fmt[i];
+
+		pfmt->bytesperline = q_data->pix_mp.plane_fmt[i].bytesperline;
+		pfmt->sizeimage = q_data->pix_mp.plane_fmt[i].sizeimage;
+
+		v4l2_dbg(1, debug, &jpeg->v4l2_dev,
+			 "plane[%d] bpl=%u, size=%u\n",
+			 i,
+			 pfmt->bytesperline,
+			 pfmt->sizeimage);
+	}
+	return 0;
+}
+
+static int mtk_jpeg_try_fmt_vid_cap_mplane(struct file *file, void *priv,
+					   struct v4l2_format *f)
+{
+	struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+	struct mtk_jpeg_fmt *fmt;
+
+	fmt = mtk_jpeg_find_format(jpeg->variant->formats,
+				   jpeg->variant->num_formats,
+				   f->fmt.pix_mp.pixelformat,
+				   MTK_JPEG_FMT_FLAG_CAPTURE);
+	if (!fmt)
+		fmt = ctx->cap_q.fmt;
+
+	v4l2_dbg(2, debug, &ctx->jpeg->v4l2_dev, "(%d) try_fmt:%c%c%c%c\n",
+		 f->type,
+		 (fmt->fourcc & 0xff),
+		 (fmt->fourcc >>  8 & 0xff),
+		 (fmt->fourcc >> 16 & 0xff),
+		 (fmt->fourcc >> 24 & 0xff));
+
+	if (ctx->state != MTK_JPEG_INIT) {
+		mtk_jpeg_g_fmt_vid_mplane(file, priv, f);
+		return 0;
+	}
+
+	return mtk_jpeg_try_fmt_mplane(&f->fmt.pix_mp, fmt);
+}
+
+static int mtk_jpeg_try_fmt_vid_out_mplane(struct file *file, void *priv,
+					   struct v4l2_format *f)
+{
+	struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+	struct mtk_jpeg_fmt *fmt;
+
+	fmt = mtk_jpeg_find_format(jpeg->variant->formats,
+				   jpeg->variant->num_formats,
+				   f->fmt.pix_mp.pixelformat,
+				   MTK_JPEG_FMT_FLAG_OUTPUT);
+	if (!fmt)
+		fmt = ctx->out_q.fmt;
+
+	v4l2_dbg(2, debug, &ctx->jpeg->v4l2_dev, "(%d) try_fmt:%c%c%c%c\n",
+		 f->type,
+		 (fmt->fourcc & 0xff),
+		 (fmt->fourcc >>  8 & 0xff),
+		 (fmt->fourcc >> 16 & 0xff),
+		 (fmt->fourcc >> 24 & 0xff));
+
+	if (ctx->state != MTK_JPEG_INIT) {
+		mtk_jpeg_g_fmt_vid_mplane(file, priv, f);
+		return 0;
+	}
+
+	return mtk_jpeg_try_fmt_mplane(&f->fmt.pix_mp, fmt);
+}
+
+static int mtk_jpeg_s_fmt_mplane(struct mtk_jpeg_ctx *ctx,
+				 struct v4l2_format *f, unsigned int fmt_type)
+{
+	struct vb2_queue *vq;
+	struct mtk_jpeg_q_data *q_data = NULL;
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+	int i;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = mtk_jpeg_get_q_data(ctx, f->type);
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&jpeg->v4l2_dev, "queue busy\n");
+		return -EBUSY;
+	}
+
+	q_data->fmt = mtk_jpeg_find_format(jpeg->variant->formats,
+					   jpeg->variant->num_formats,
+					   pix_mp->pixelformat, fmt_type);
+	q_data->pix_mp.width = pix_mp->width;
+	q_data->pix_mp.height = pix_mp->height;
+	q_data->enc_crop_rect.width = pix_mp->width;
+	q_data->enc_crop_rect.height = pix_mp->height;
+	q_data->pix_mp.colorspace = V4L2_COLORSPACE_SRGB;
+	q_data->pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_601;
+	q_data->pix_mp.xfer_func = V4L2_XFER_FUNC_SRGB;
+	q_data->pix_mp.quantization = V4L2_QUANTIZATION_FULL_RANGE;
+
+	v4l2_dbg(1, debug, &jpeg->v4l2_dev, "(%d) s_fmt:%c%c%c%c wxh:%ux%u\n",
+		 f->type,
+		 (q_data->fmt->fourcc & 0xff),
+		 (q_data->fmt->fourcc >>  8 & 0xff),
+		 (q_data->fmt->fourcc >> 16 & 0xff),
+		 (q_data->fmt->fourcc >> 24 & 0xff),
+		 q_data->pix_mp.width, q_data->pix_mp.height);
+
+	for (i = 0; i < q_data->fmt->colplanes; i++) {
+		q_data->pix_mp.plane_fmt[i].bytesperline =
+					pix_mp->plane_fmt[i].bytesperline;
+		q_data->pix_mp.plane_fmt[i].sizeimage =
+					pix_mp->plane_fmt[i].sizeimage;
+
+		v4l2_dbg(1, debug, &jpeg->v4l2_dev,
+			 "plane[%d] bpl=%u, size=%u\n",
+			 i, q_data->pix_mp.plane_fmt[i].bytesperline,
+			 q_data->pix_mp.plane_fmt[i].sizeimage);
+	}
+
+	return 0;
+}
+
+static int mtk_jpeg_s_fmt_vid_out_mplane(struct file *file, void *priv,
+					 struct v4l2_format *f)
+{
+	int ret;
+
+	ret = mtk_jpeg_try_fmt_vid_out_mplane(file, priv, f);
+	if (ret)
+		return ret;
+
+	return mtk_jpeg_s_fmt_mplane(mtk_jpeg_fh_to_ctx(priv), f,
+				     MTK_JPEG_FMT_FLAG_OUTPUT);
+}
+
+static int mtk_jpeg_s_fmt_vid_cap_mplane(struct file *file, void *priv,
+					 struct v4l2_format *f)
+{
+	int ret;
+
+	ret = mtk_jpeg_try_fmt_vid_cap_mplane(file, priv, f);
+	if (ret)
+		return ret;
+
+	return mtk_jpeg_s_fmt_mplane(mtk_jpeg_fh_to_ctx(priv), f,
+				     MTK_JPEG_FMT_FLAG_CAPTURE);
+}
+
+static void mtk_jpeg_queue_src_chg_event(struct mtk_jpeg_ctx *ctx)
+{
+	static const struct v4l2_event ev_src_ch = {
+		.type = V4L2_EVENT_SOURCE_CHANGE,
+		.u.src_change.changes =
+		V4L2_EVENT_SRC_CH_RESOLUTION,
+	};
+
+	v4l2_event_queue_fh(&ctx->fh, &ev_src_ch);
+}
+
+static int mtk_jpeg_subscribe_event(struct v4l2_fh *fh,
+				    const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subscribe(fh, sub);
+	}
+
+	return v4l2_ctrl_subscribe_event(fh, sub);
+}
+
+static int mtk_jpeg_enc_g_selection(struct file *file, void *priv,
+				    struct v4l2_selection *s)
+{
+	struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		s->r = ctx->out_q.enc_crop_rect;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		s->r.width = ctx->out_q.pix_mp.width;
+		s->r.height = ctx->out_q.pix_mp.height;
+		s->r.left = 0;
+		s->r.top = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int mtk_jpeg_dec_g_selection(struct file *file, void *priv,
+				    struct v4l2_selection *s)
+{
+	struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		s->r.width = ctx->out_q.pix_mp.width;
+		s->r.height = ctx->out_q.pix_mp.height;
+		s->r.left = 0;
+		s->r.top = 0;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_PADDED:
+		s->r.width = ctx->cap_q.pix_mp.width;
+		s->r.height = ctx->cap_q.pix_mp.height;
+		s->r.left = 0;
+		s->r.top = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int mtk_jpeg_enc_s_selection(struct file *file, void *priv,
+				    struct v4l2_selection *s)
+{
+	struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = min(s->r.width, ctx->out_q.pix_mp.width);
+		s->r.height = min(s->r.height, ctx->out_q.pix_mp.height);
+		ctx->out_q.enc_crop_rect = s->r;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops mtk_jpeg_enc_ioctl_ops = {
+	.vidioc_querycap                = mtk_jpeg_querycap,
+	.vidioc_enum_fmt_vid_cap	= mtk_jpeg_enum_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_out	= mtk_jpeg_enum_fmt_vid_out,
+	.vidioc_try_fmt_vid_cap_mplane	= mtk_jpeg_try_fmt_vid_cap_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= mtk_jpeg_try_fmt_vid_out_mplane,
+	.vidioc_g_fmt_vid_cap_mplane    = mtk_jpeg_g_fmt_vid_mplane,
+	.vidioc_g_fmt_vid_out_mplane    = mtk_jpeg_g_fmt_vid_mplane,
+	.vidioc_s_fmt_vid_cap_mplane    = mtk_jpeg_s_fmt_vid_cap_mplane,
+	.vidioc_s_fmt_vid_out_mplane    = mtk_jpeg_s_fmt_vid_out_mplane,
+	.vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
+	.vidioc_subscribe_event         = mtk_jpeg_subscribe_event,
+	.vidioc_g_selection		= mtk_jpeg_enc_g_selection,
+	.vidioc_s_selection		= mtk_jpeg_enc_s_selection,
+
+	.vidioc_create_bufs		= v4l2_m2m_ioctl_create_bufs,
+	.vidioc_prepare_buf		= v4l2_m2m_ioctl_prepare_buf,
+	.vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
+	.vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
+	.vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
+	.vidioc_streamon                = v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
+
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_ioctl_ops mtk_jpeg_dec_ioctl_ops = {
+	.vidioc_querycap                = mtk_jpeg_querycap,
+	.vidioc_enum_fmt_vid_cap	= mtk_jpeg_enum_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_out	= mtk_jpeg_enum_fmt_vid_out,
+	.vidioc_try_fmt_vid_cap_mplane	= mtk_jpeg_try_fmt_vid_cap_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= mtk_jpeg_try_fmt_vid_out_mplane,
+	.vidioc_g_fmt_vid_cap_mplane    = mtk_jpeg_g_fmt_vid_mplane,
+	.vidioc_g_fmt_vid_out_mplane    = mtk_jpeg_g_fmt_vid_mplane,
+	.vidioc_s_fmt_vid_cap_mplane    = mtk_jpeg_s_fmt_vid_cap_mplane,
+	.vidioc_s_fmt_vid_out_mplane    = mtk_jpeg_s_fmt_vid_out_mplane,
+	.vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
+	.vidioc_subscribe_event         = mtk_jpeg_subscribe_event,
+	.vidioc_g_selection		= mtk_jpeg_dec_g_selection,
+
+	.vidioc_create_bufs		= v4l2_m2m_ioctl_create_bufs,
+	.vidioc_prepare_buf		= v4l2_m2m_ioctl_prepare_buf,
+	.vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
+	.vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
+	.vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
+	.vidioc_streamon                = v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
+
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+static int mtk_jpeg_queue_setup(struct vb2_queue *q,
+				unsigned int *num_buffers,
+				unsigned int *num_planes,
+				unsigned int sizes[],
+				struct device *alloc_ctxs[])
+{
+	struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(q);
+	struct mtk_jpeg_q_data *q_data = NULL;
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+	int i;
+
+	v4l2_dbg(1, debug, &jpeg->v4l2_dev, "(%d) buf_req count=%u\n",
+		 q->type, *num_buffers);
+
+	q_data = mtk_jpeg_get_q_data(ctx, q->type);
+	if (!q_data)
+		return -EINVAL;
+
+	if (*num_planes) {
+		for (i = 0; i < *num_planes; i++)
+			if (sizes[i] < q_data->pix_mp.plane_fmt[i].sizeimage)
+				return -EINVAL;
+		return 0;
+	}
+
+	*num_planes = q_data->fmt->colplanes;
+	for (i = 0; i < q_data->fmt->colplanes; i++) {
+		sizes[i] =  q_data->pix_mp.plane_fmt[i].sizeimage;
+		v4l2_dbg(1, debug, &jpeg->v4l2_dev, "sizeimage[%d]=%u\n",
+			 i, sizes[i]);
+	}
+
+	return 0;
+}
+
+static int mtk_jpeg_buf_prepare(struct vb2_buffer *vb)
+{
+	struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct mtk_jpeg_q_data *q_data = NULL;
+	struct v4l2_plane_pix_format plane_fmt;
+	int i;
+
+	q_data = mtk_jpeg_get_q_data(ctx, vb->vb2_queue->type);
+	if (!q_data)
+		return -EINVAL;
+
+	for (i = 0; i < q_data->fmt->colplanes; i++) {
+		plane_fmt = q_data->pix_mp.plane_fmt[i];
+		if (ctx->enable_exif &&
+		    q_data->fmt->fourcc == V4L2_PIX_FMT_JPEG)
+			vb2_set_plane_payload(vb, i, plane_fmt.sizeimage +
+					      MTK_JPEG_MAX_EXIF_SIZE);
+		else
+			vb2_set_plane_payload(vb, i,  plane_fmt.sizeimage);
+	}
+
+	return 0;
+}
+
+static bool mtk_jpeg_check_resolution_change(struct mtk_jpeg_ctx *ctx,
+					     struct mtk_jpeg_dec_param *param)
+{
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+	struct mtk_jpeg_q_data *q_data;
+
+	q_data = &ctx->out_q;
+	if (q_data->pix_mp.width != param->pic_w ||
+	    q_data->pix_mp.height != param->pic_h) {
+		v4l2_dbg(1, debug, &jpeg->v4l2_dev, "Picture size change\n");
+		return true;
+	}
+
+	q_data = &ctx->cap_q;
+	if (q_data->fmt !=
+	    mtk_jpeg_find_format(jpeg->variant->formats,
+				 jpeg->variant->num_formats, param->dst_fourcc,
+				 MTK_JPEG_FMT_FLAG_CAPTURE)) {
+		v4l2_dbg(1, debug, &jpeg->v4l2_dev, "format change\n");
+		return true;
+	}
+	return false;
+}
+
+static void mtk_jpeg_set_queue_data(struct mtk_jpeg_ctx *ctx,
+				    struct mtk_jpeg_dec_param *param)
+{
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+	struct mtk_jpeg_q_data *q_data;
+	int i;
+
+	q_data = &ctx->out_q;
+	q_data->pix_mp.width = param->pic_w;
+	q_data->pix_mp.height = param->pic_h;
+
+	q_data = &ctx->cap_q;
+	q_data->pix_mp.width = param->dec_w;
+	q_data->pix_mp.height = param->dec_h;
+	q_data->fmt = mtk_jpeg_find_format(jpeg->variant->formats,
+					   jpeg->variant->num_formats,
+					   param->dst_fourcc,
+					   MTK_JPEG_FMT_FLAG_CAPTURE);
+
+	for (i = 0; i < q_data->fmt->colplanes; i++) {
+		q_data->pix_mp.plane_fmt[i].bytesperline = param->mem_stride[i];
+		q_data->pix_mp.plane_fmt[i].sizeimage = param->comp_size[i];
+	}
+
+	v4l2_dbg(1, debug, &jpeg->v4l2_dev,
+		 "set_parse cap:%c%c%c%c pic(%u, %u), buf(%u, %u)\n",
+		 (param->dst_fourcc & 0xff),
+		 (param->dst_fourcc >>  8 & 0xff),
+		 (param->dst_fourcc >> 16 & 0xff),
+		 (param->dst_fourcc >> 24 & 0xff),
+		 param->pic_w, param->pic_h,
+		 param->dec_w, param->dec_h);
+}
+
+static void mtk_jpeg_enc_buf_queue(struct vb2_buffer *vb)
+{
+	struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+
+	v4l2_dbg(2, debug, &jpeg->v4l2_dev, "(%d) buf_q id=%d, vb=%p\n",
+		 vb->vb2_queue->type, vb->index, vb);
+
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, to_vb2_v4l2_buffer(vb));
+}
+
+static void mtk_jpeg_dec_buf_queue(struct vb2_buffer *vb)
+{
+	struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct mtk_jpeg_dec_param *param;
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+	struct mtk_jpeg_src_buf *jpeg_src_buf;
+	bool header_valid;
+
+	v4l2_dbg(2, debug, &jpeg->v4l2_dev, "(%d) buf_q id=%d, vb=%p\n",
+		 vb->vb2_queue->type, vb->index, vb);
+
+	if (vb->vb2_queue->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		goto end;
+
+	jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(vb);
+	param = &jpeg_src_buf->dec_param;
+	memset(param, 0, sizeof(*param));
+
+	header_valid = mtk_jpeg_parse(param, (u8 *)vb2_plane_vaddr(vb, 0),
+				      vb2_get_plane_payload(vb, 0));
+	if (!header_valid) {
+		v4l2_err(&jpeg->v4l2_dev, "Header invalid.\n");
+		vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+		return;
+	}
+
+	if (ctx->state == MTK_JPEG_INIT) {
+		struct vb2_queue *dst_vq = v4l2_m2m_get_vq(
+			ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+
+		mtk_jpeg_queue_src_chg_event(ctx);
+		mtk_jpeg_set_queue_data(ctx, param);
+		ctx->state = vb2_is_streaming(dst_vq) ?
+				MTK_JPEG_SOURCE_CHANGE : MTK_JPEG_RUNNING;
+	}
+end:
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, to_vb2_v4l2_buffer(vb));
+}
+
+static struct vb2_v4l2_buffer *mtk_jpeg_buf_remove(struct mtk_jpeg_ctx *ctx,
+				 enum v4l2_buf_type type)
+{
+	if (V4L2_TYPE_IS_OUTPUT(type))
+		return v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	else
+		return v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+}
+
+static void mtk_jpeg_enc_stop_streaming(struct vb2_queue *q)
+{
+	struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(q);
+	struct vb2_v4l2_buffer *vb;
+
+	while ((vb = mtk_jpeg_buf_remove(ctx, q->type)))
+		v4l2_m2m_buf_done(vb, VB2_BUF_STATE_ERROR);
+}
+
+static void mtk_jpeg_dec_stop_streaming(struct vb2_queue *q)
+{
+	struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(q);
+	struct vb2_v4l2_buffer *vb;
+
+	/*
+	 * STREAMOFF is an acknowledgment for source change event.
+	 * Before STREAMOFF, we still have to return the old resolution and
+	 * subsampling. Update capture queue when the stream is off.
+	 */
+	if (ctx->state == MTK_JPEG_SOURCE_CHANGE &&
+	    V4L2_TYPE_IS_CAPTURE(q->type)) {
+		struct mtk_jpeg_src_buf *src_buf;
+
+		vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+		src_buf = mtk_jpeg_vb2_to_srcbuf(&vb->vb2_buf);
+		mtk_jpeg_set_queue_data(ctx, &src_buf->dec_param);
+		ctx->state = MTK_JPEG_RUNNING;
+	} else if (V4L2_TYPE_IS_OUTPUT(q->type)) {
+		ctx->state = MTK_JPEG_INIT;
+	}
+
+	while ((vb = mtk_jpeg_buf_remove(ctx, q->type)))
+		v4l2_m2m_buf_done(vb, VB2_BUF_STATE_ERROR);
+}
+
+static const struct vb2_ops mtk_jpeg_dec_qops = {
+	.queue_setup        = mtk_jpeg_queue_setup,
+	.buf_prepare        = mtk_jpeg_buf_prepare,
+	.buf_queue          = mtk_jpeg_dec_buf_queue,
+	.wait_prepare       = vb2_ops_wait_prepare,
+	.wait_finish        = vb2_ops_wait_finish,
+	.stop_streaming     = mtk_jpeg_dec_stop_streaming,
+};
+
+static const struct vb2_ops mtk_jpeg_enc_qops = {
+	.queue_setup        = mtk_jpeg_queue_setup,
+	.buf_prepare        = mtk_jpeg_buf_prepare,
+	.buf_queue          = mtk_jpeg_enc_buf_queue,
+	.wait_prepare       = vb2_ops_wait_prepare,
+	.wait_finish        = vb2_ops_wait_finish,
+	.stop_streaming     = mtk_jpeg_enc_stop_streaming,
+};
+
+static void mtk_jpeg_set_dec_src(struct mtk_jpeg_ctx *ctx,
+				 struct vb2_buffer *src_buf,
+				 struct mtk_jpeg_bs *bs)
+{
+	bs->str_addr = vb2_dma_contig_plane_dma_addr(src_buf, 0);
+	bs->end_addr = bs->str_addr +
+		       round_up(vb2_get_plane_payload(src_buf, 0), 16);
+	bs->size = round_up(vb2_plane_size(src_buf, 0), 128);
+}
+
+static int mtk_jpeg_set_dec_dst(struct mtk_jpeg_ctx *ctx,
+				struct mtk_jpeg_dec_param *param,
+				struct vb2_buffer *dst_buf,
+				struct mtk_jpeg_fb *fb)
+{
+	int i;
+
+	if (param->comp_num != dst_buf->num_planes) {
+		dev_err(ctx->jpeg->dev, "plane number mismatch (%u != %u)\n",
+			param->comp_num, dst_buf->num_planes);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < dst_buf->num_planes; i++) {
+		if (vb2_plane_size(dst_buf, i) < param->comp_size[i]) {
+			dev_err(ctx->jpeg->dev,
+				"buffer size is underflow (%lu < %u)\n",
+				vb2_plane_size(dst_buf, 0),
+				param->comp_size[i]);
+			return -EINVAL;
+		}
+		fb->plane_addr[i] = vb2_dma_contig_plane_dma_addr(dst_buf, i);
+	}
+
+	return 0;
+}
+
+static void mtk_jpeg_enc_device_run(void *priv)
+{
+	struct mtk_jpeg_ctx *ctx = priv;
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
+	unsigned long flags;
+	int ret;
+
+	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+
+	ret = pm_runtime_resume_and_get(jpeg->dev);
+	if (ret < 0)
+		goto enc_end;
+
+	schedule_delayed_work(&jpeg->job_timeout_work,
+			      msecs_to_jiffies(MTK_JPEG_HW_TIMEOUT_MSEC));
+
+	spin_lock_irqsave(&jpeg->hw_lock, flags);
+
+	/*
+	 * Resetting the hardware every frame is to ensure that all the
+	 * registers are cleared. This is a hardware requirement.
+	 */
+	mtk_jpeg_enc_reset(jpeg->reg_base);
+
+	mtk_jpeg_set_enc_src(ctx, jpeg->reg_base, &src_buf->vb2_buf);
+	mtk_jpeg_set_enc_dst(ctx, jpeg->reg_base, &dst_buf->vb2_buf);
+	mtk_jpeg_set_enc_params(ctx, jpeg->reg_base);
+	mtk_jpeg_enc_start(jpeg->reg_base);
+	spin_unlock_irqrestore(&jpeg->hw_lock, flags);
+	return;
+
+enc_end:
+	v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+	v4l2_m2m_buf_done(src_buf, buf_state);
+	v4l2_m2m_buf_done(dst_buf, buf_state);
+	v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
+}
+
+static void mtk_jpeg_dec_device_run(void *priv)
+{
+	struct mtk_jpeg_ctx *ctx = priv;
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
+	unsigned long flags;
+	struct mtk_jpeg_src_buf *jpeg_src_buf;
+	struct mtk_jpeg_bs bs;
+	struct mtk_jpeg_fb fb;
+	int ret;
+
+	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(&src_buf->vb2_buf);
+
+	if (mtk_jpeg_check_resolution_change(ctx, &jpeg_src_buf->dec_param)) {
+		mtk_jpeg_queue_src_chg_event(ctx);
+		ctx->state = MTK_JPEG_SOURCE_CHANGE;
+		v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
+		return;
+	}
+
+	ret = pm_runtime_resume_and_get(jpeg->dev);
+	if (ret < 0)
+		goto dec_end;
+
+	mtk_jpeg_set_dec_src(ctx, &src_buf->vb2_buf, &bs);
+	if (mtk_jpeg_set_dec_dst(ctx, &jpeg_src_buf->dec_param, &dst_buf->vb2_buf, &fb))
+		goto dec_end;
+
+	schedule_delayed_work(&jpeg->job_timeout_work,
+			      msecs_to_jiffies(MTK_JPEG_HW_TIMEOUT_MSEC));
+
+	spin_lock_irqsave(&jpeg->hw_lock, flags);
+	mtk_jpeg_dec_reset(jpeg->reg_base);
+	mtk_jpeg_dec_set_config(jpeg->reg_base,
+				&jpeg_src_buf->dec_param, &bs, &fb);
+
+	mtk_jpeg_dec_start(jpeg->reg_base);
+	spin_unlock_irqrestore(&jpeg->hw_lock, flags);
+	return;
+
+dec_end:
+	v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+	v4l2_m2m_buf_done(src_buf, buf_state);
+	v4l2_m2m_buf_done(dst_buf, buf_state);
+	v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
+}
+
+static int mtk_jpeg_dec_job_ready(void *priv)
+{
+	struct mtk_jpeg_ctx *ctx = priv;
+
+	return (ctx->state == MTK_JPEG_RUNNING) ? 1 : 0;
+}
+
+static const struct v4l2_m2m_ops mtk_jpeg_enc_m2m_ops = {
+	.device_run = mtk_jpeg_enc_device_run,
+};
+
+static const struct v4l2_m2m_ops mtk_jpeg_dec_m2m_ops = {
+	.device_run = mtk_jpeg_dec_device_run,
+	.job_ready  = mtk_jpeg_dec_job_ready,
+};
+
+static int mtk_jpeg_queue_init(void *priv, struct vb2_queue *src_vq,
+			       struct vb2_queue *dst_vq)
+{
+	struct mtk_jpeg_ctx *ctx = priv;
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_DMABUF | VB2_MMAP;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct mtk_jpeg_src_buf);
+	src_vq->ops = jpeg->variant->qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->jpeg->lock;
+	src_vq->dev = ctx->jpeg->dev;
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_DMABUF | VB2_MMAP;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->ops = jpeg->variant->qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->jpeg->lock;
+	dst_vq->dev = ctx->jpeg->dev;
+	ret = vb2_queue_init(dst_vq);
+
+	return ret;
+}
+
+static void mtk_jpeg_clk_on(struct mtk_jpeg_dev *jpeg)
+{
+	int ret;
+
+	ret = clk_bulk_prepare_enable(jpeg->variant->num_clks,
+				      jpeg->variant->clks);
+	if (ret)
+		dev_err(jpeg->dev, "Failed to open jpeg clk: %d\n", ret);
+}
+
+static void mtk_jpeg_clk_off(struct mtk_jpeg_dev *jpeg)
+{
+	clk_bulk_disable_unprepare(jpeg->variant->num_clks,
+				   jpeg->variant->clks);
+}
+
+static irqreturn_t mtk_jpeg_enc_done(struct mtk_jpeg_dev *jpeg)
+{
+	struct mtk_jpeg_ctx *ctx;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
+	u32 result_size;
+
+	ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
+	if (!ctx) {
+		v4l2_err(&jpeg->v4l2_dev, "Context is NULL\n");
+		return IRQ_HANDLED;
+	}
+
+	src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+
+	result_size = mtk_jpeg_enc_get_file_size(jpeg->reg_base);
+	vb2_set_plane_payload(&dst_buf->vb2_buf, 0, result_size);
+
+	buf_state = VB2_BUF_STATE_DONE;
+
+	v4l2_m2m_buf_done(src_buf, buf_state);
+	v4l2_m2m_buf_done(dst_buf, buf_state);
+	v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
+	pm_runtime_put(ctx->jpeg->dev);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t mtk_jpeg_enc_irq(int irq, void *priv)
+{
+	struct mtk_jpeg_dev *jpeg = priv;
+	u32 irq_status;
+	irqreturn_t ret = IRQ_NONE;
+
+	cancel_delayed_work(&jpeg->job_timeout_work);
+
+	irq_status = readl(jpeg->reg_base + JPEG_ENC_INT_STS) &
+		     JPEG_ENC_INT_STATUS_MASK_ALLIRQ;
+	if (irq_status)
+		writel(0, jpeg->reg_base + JPEG_ENC_INT_STS);
+
+	if (!(irq_status & JPEG_ENC_INT_STATUS_DONE))
+		return ret;
+
+	ret = mtk_jpeg_enc_done(jpeg);
+	return ret;
+}
+
+static irqreturn_t mtk_jpeg_dec_irq(int irq, void *priv)
+{
+	struct mtk_jpeg_dev *jpeg = priv;
+	struct mtk_jpeg_ctx *ctx;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	struct mtk_jpeg_src_buf *jpeg_src_buf;
+	enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
+	u32	dec_irq_ret;
+	u32 dec_ret;
+	int i;
+
+	cancel_delayed_work(&jpeg->job_timeout_work);
+
+	dec_ret = mtk_jpeg_dec_get_int_status(jpeg->reg_base);
+	dec_irq_ret = mtk_jpeg_dec_enum_result(dec_ret);
+	ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
+	if (!ctx) {
+		v4l2_err(&jpeg->v4l2_dev, "Context is NULL\n");
+		return IRQ_HANDLED;
+	}
+
+	src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+	jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(&src_buf->vb2_buf);
+
+	if (dec_irq_ret >= MTK_JPEG_DEC_RESULT_UNDERFLOW)
+		mtk_jpeg_dec_reset(jpeg->reg_base);
+
+	if (dec_irq_ret != MTK_JPEG_DEC_RESULT_EOF_DONE) {
+		dev_err(jpeg->dev, "decode failed\n");
+		goto dec_end;
+	}
+
+	for (i = 0; i < dst_buf->vb2_buf.num_planes; i++)
+		vb2_set_plane_payload(&dst_buf->vb2_buf, i,
+				      jpeg_src_buf->dec_param.comp_size[i]);
+
+	buf_state = VB2_BUF_STATE_DONE;
+
+dec_end:
+	v4l2_m2m_buf_done(src_buf, buf_state);
+	v4l2_m2m_buf_done(dst_buf, buf_state);
+	v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
+	pm_runtime_put(ctx->jpeg->dev);
+	return IRQ_HANDLED;
+}
+
+static void mtk_jpeg_set_default_params(struct mtk_jpeg_ctx *ctx)
+{
+	struct mtk_jpeg_q_data *q = &ctx->out_q;
+	struct mtk_jpeg_dev *jpeg = ctx->jpeg;
+
+	ctx->fh.ctrl_handler = &ctx->ctrl_hdl;
+	q->pix_mp.colorspace = V4L2_COLORSPACE_SRGB;
+	q->pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_601;
+	q->pix_mp.quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	q->pix_mp.xfer_func = V4L2_XFER_FUNC_SRGB;
+
+	q->fmt = mtk_jpeg_find_format(jpeg->variant->formats,
+				      jpeg->variant->num_formats,
+				      jpeg->variant->out_q_default_fourcc,
+				      MTK_JPEG_FMT_FLAG_OUTPUT);
+	q->pix_mp.width = MTK_JPEG_MIN_WIDTH;
+	q->pix_mp.height = MTK_JPEG_MIN_HEIGHT;
+	mtk_jpeg_try_fmt_mplane(&q->pix_mp, q->fmt);
+
+	q = &ctx->cap_q;
+	q->fmt = mtk_jpeg_find_format(jpeg->variant->formats,
+				      jpeg->variant->num_formats,
+				      jpeg->variant->cap_q_default_fourcc,
+				      MTK_JPEG_FMT_FLAG_CAPTURE);
+	q->pix_mp.colorspace = V4L2_COLORSPACE_SRGB;
+	q->pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_601;
+	q->pix_mp.quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	q->pix_mp.xfer_func = V4L2_XFER_FUNC_SRGB;
+	q->pix_mp.width = MTK_JPEG_MIN_WIDTH;
+	q->pix_mp.height = MTK_JPEG_MIN_HEIGHT;
+
+	mtk_jpeg_try_fmt_mplane(&q->pix_mp, q->fmt);
+}
+
+static int mtk_jpeg_open(struct file *file)
+{
+	struct mtk_jpeg_dev *jpeg = video_drvdata(file);
+	struct video_device *vfd = video_devdata(file);
+	struct mtk_jpeg_ctx *ctx;
+	int ret = 0;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	if (mutex_lock_interruptible(&jpeg->lock)) {
+		ret = -ERESTARTSYS;
+		goto free;
+	}
+
+	v4l2_fh_init(&ctx->fh, vfd);
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	ctx->jpeg = jpeg;
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(jpeg->m2m_dev, ctx,
+					    mtk_jpeg_queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto error;
+	}
+
+	if (jpeg->variant->cap_q_default_fourcc == V4L2_PIX_FMT_JPEG) {
+		ret = mtk_jpeg_enc_ctrls_setup(ctx);
+		if (ret) {
+			v4l2_err(&jpeg->v4l2_dev, "Failed to setup jpeg enc controls\n");
+			goto error;
+		}
+	} else {
+		v4l2_ctrl_handler_init(&ctx->ctrl_hdl, 0);
+	}
+	mtk_jpeg_set_default_params(ctx);
+	mutex_unlock(&jpeg->lock);
+	return 0;
+
+error:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	mutex_unlock(&jpeg->lock);
+free:
+	kfree(ctx);
+	return ret;
+}
+
+static int mtk_jpeg_release(struct file *file)
+{
+	struct mtk_jpeg_dev *jpeg = video_drvdata(file);
+	struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(file->private_data);
+
+	mutex_lock(&jpeg->lock);
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+	mutex_unlock(&jpeg->lock);
+	return 0;
+}
+
+static const struct v4l2_file_operations mtk_jpeg_fops = {
+	.owner          = THIS_MODULE,
+	.open           = mtk_jpeg_open,
+	.release        = mtk_jpeg_release,
+	.poll           = v4l2_m2m_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap           = v4l2_m2m_fop_mmap,
+};
+
+static struct clk_bulk_data mt8173_jpeg_dec_clocks[] = {
+	{ .id = "jpgdec-smi" },
+	{ .id = "jpgdec" },
+};
+
+static struct clk_bulk_data mtk_jpeg_clocks[] = {
+	{ .id = "jpgenc" },
+};
+
+static void mtk_jpeg_job_timeout_work(struct work_struct *work)
+{
+	struct mtk_jpeg_dev *jpeg = container_of(work, struct mtk_jpeg_dev,
+						 job_timeout_work.work);
+	struct mtk_jpeg_ctx *ctx;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+
+	ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
+	src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+
+	jpeg->variant->hw_reset(jpeg->reg_base);
+
+	pm_runtime_put(jpeg->dev);
+
+	v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
+	v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
+	v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
+}
+
+static int mtk_jpeg_probe(struct platform_device *pdev)
+{
+	struct mtk_jpeg_dev *jpeg;
+	int jpeg_irq;
+	int ret;
+
+	jpeg = devm_kzalloc(&pdev->dev, sizeof(*jpeg), GFP_KERNEL);
+	if (!jpeg)
+		return -ENOMEM;
+
+	mutex_init(&jpeg->lock);
+	spin_lock_init(&jpeg->hw_lock);
+	jpeg->dev = &pdev->dev;
+	jpeg->variant = of_device_get_match_data(jpeg->dev);
+	INIT_DELAYED_WORK(&jpeg->job_timeout_work, mtk_jpeg_job_timeout_work);
+
+	jpeg->reg_base = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(jpeg->reg_base)) {
+		ret = PTR_ERR(jpeg->reg_base);
+		return ret;
+	}
+
+	jpeg_irq = platform_get_irq(pdev, 0);
+	if (jpeg_irq < 0)
+		return jpeg_irq;
+
+	ret = devm_request_irq(&pdev->dev, jpeg_irq,
+			       jpeg->variant->irq_handler, 0, pdev->name, jpeg);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to request jpeg_irq %d (%d)\n",
+			jpeg_irq, ret);
+		goto err_req_irq;
+	}
+
+	ret = devm_clk_bulk_get(jpeg->dev, jpeg->variant->num_clks,
+				jpeg->variant->clks);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to init clk, err %d\n", ret);
+		goto err_clk_init;
+	}
+
+	ret = v4l2_device_register(&pdev->dev, &jpeg->v4l2_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to register v4l2 device\n");
+		ret = -EINVAL;
+		goto err_dev_register;
+	}
+
+	jpeg->m2m_dev = v4l2_m2m_init(jpeg->variant->m2m_ops);
+
+	if (IS_ERR(jpeg->m2m_dev)) {
+		v4l2_err(&jpeg->v4l2_dev, "Failed to init mem2mem device\n");
+		ret = PTR_ERR(jpeg->m2m_dev);
+		goto err_m2m_init;
+	}
+
+	jpeg->vdev = video_device_alloc();
+	if (!jpeg->vdev) {
+		ret = -ENOMEM;
+		goto err_vfd_jpeg_alloc;
+	}
+	snprintf(jpeg->vdev->name, sizeof(jpeg->vdev->name),
+		 "%s", jpeg->variant->dev_name);
+	jpeg->vdev->fops = &mtk_jpeg_fops;
+	jpeg->vdev->ioctl_ops = jpeg->variant->ioctl_ops;
+	jpeg->vdev->minor = -1;
+	jpeg->vdev->release = video_device_release;
+	jpeg->vdev->lock = &jpeg->lock;
+	jpeg->vdev->v4l2_dev = &jpeg->v4l2_dev;
+	jpeg->vdev->vfl_dir = VFL_DIR_M2M;
+	jpeg->vdev->device_caps = V4L2_CAP_STREAMING |
+				  V4L2_CAP_VIDEO_M2M_MPLANE;
+
+	if (of_get_property(pdev->dev.of_node, "dma-ranges", NULL))
+		dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(34));
+
+	ret = video_register_device(jpeg->vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		v4l2_err(&jpeg->v4l2_dev, "Failed to register video device\n");
+		goto err_vfd_jpeg_register;
+	}
+
+	video_set_drvdata(jpeg->vdev, jpeg);
+	v4l2_info(&jpeg->v4l2_dev,
+		  "%s device registered as /dev/video%d (%d,%d)\n",
+		  jpeg->variant->dev_name, jpeg->vdev->num,
+		  VIDEO_MAJOR, jpeg->vdev->minor);
+
+	platform_set_drvdata(pdev, jpeg);
+
+	pm_runtime_enable(&pdev->dev);
+
+	return 0;
+
+err_vfd_jpeg_register:
+	video_device_release(jpeg->vdev);
+
+err_vfd_jpeg_alloc:
+	v4l2_m2m_release(jpeg->m2m_dev);
+
+err_m2m_init:
+	v4l2_device_unregister(&jpeg->v4l2_dev);
+
+err_dev_register:
+
+err_clk_init:
+
+err_req_irq:
+
+	return ret;
+}
+
+static int mtk_jpeg_remove(struct platform_device *pdev)
+{
+	struct mtk_jpeg_dev *jpeg = platform_get_drvdata(pdev);
+
+	pm_runtime_disable(&pdev->dev);
+	video_unregister_device(jpeg->vdev);
+	v4l2_m2m_release(jpeg->m2m_dev);
+	v4l2_device_unregister(&jpeg->v4l2_dev);
+
+	return 0;
+}
+
+static __maybe_unused int mtk_jpeg_pm_suspend(struct device *dev)
+{
+	struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
+
+	mtk_jpeg_clk_off(jpeg);
+
+	return 0;
+}
+
+static __maybe_unused int mtk_jpeg_pm_resume(struct device *dev)
+{
+	struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
+
+	mtk_jpeg_clk_on(jpeg);
+
+	return 0;
+}
+
+static __maybe_unused int mtk_jpeg_suspend(struct device *dev)
+{
+	struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
+
+	v4l2_m2m_suspend(jpeg->m2m_dev);
+	return pm_runtime_force_suspend(dev);
+}
+
+static __maybe_unused int mtk_jpeg_resume(struct device *dev)
+{
+	struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
+	int ret;
+
+	ret = pm_runtime_force_resume(dev);
+	if (ret < 0)
+		return ret;
+
+	v4l2_m2m_resume(jpeg->m2m_dev);
+	return ret;
+}
+
+static const struct dev_pm_ops mtk_jpeg_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(mtk_jpeg_suspend, mtk_jpeg_resume)
+	SET_RUNTIME_PM_OPS(mtk_jpeg_pm_suspend, mtk_jpeg_pm_resume, NULL)
+};
+
+static const struct mtk_jpeg_variant mt8173_jpeg_drvdata = {
+	.clks = mt8173_jpeg_dec_clocks,
+	.num_clks = ARRAY_SIZE(mt8173_jpeg_dec_clocks),
+	.formats = mtk_jpeg_dec_formats,
+	.num_formats = MTK_JPEG_DEC_NUM_FORMATS,
+	.qops = &mtk_jpeg_dec_qops,
+	.irq_handler = mtk_jpeg_dec_irq,
+	.hw_reset = mtk_jpeg_dec_reset,
+	.m2m_ops = &mtk_jpeg_dec_m2m_ops,
+	.dev_name = "mtk-jpeg-dec",
+	.ioctl_ops = &mtk_jpeg_dec_ioctl_ops,
+	.out_q_default_fourcc = V4L2_PIX_FMT_JPEG,
+	.cap_q_default_fourcc = V4L2_PIX_FMT_YUV420M,
+};
+
+static const struct mtk_jpeg_variant mtk_jpeg_drvdata = {
+	.clks = mtk_jpeg_clocks,
+	.num_clks = ARRAY_SIZE(mtk_jpeg_clocks),
+	.formats = mtk_jpeg_enc_formats,
+	.num_formats = MTK_JPEG_ENC_NUM_FORMATS,
+	.qops = &mtk_jpeg_enc_qops,
+	.irq_handler = mtk_jpeg_enc_irq,
+	.hw_reset = mtk_jpeg_enc_reset,
+	.m2m_ops = &mtk_jpeg_enc_m2m_ops,
+	.dev_name = "mtk-jpeg-enc",
+	.ioctl_ops = &mtk_jpeg_enc_ioctl_ops,
+	.out_q_default_fourcc = V4L2_PIX_FMT_YUYV,
+	.cap_q_default_fourcc = V4L2_PIX_FMT_JPEG,
+};
+
+static const struct of_device_id mtk_jpeg_match[] = {
+	{
+		.compatible = "mediatek,mt8173-jpgdec",
+		.data = &mt8173_jpeg_drvdata,
+	},
+	{
+		.compatible = "mediatek,mt2701-jpgdec",
+		.data = &mt8173_jpeg_drvdata,
+	},
+	{
+		.compatible = "mediatek,mtk-jpgenc",
+		.data = &mtk_jpeg_drvdata,
+	},
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, mtk_jpeg_match);
+
+static struct platform_driver mtk_jpeg_driver = {
+	.probe = mtk_jpeg_probe,
+	.remove = mtk_jpeg_remove,
+	.driver = {
+		.name           = MTK_JPEG_NAME,
+		.of_match_table = mtk_jpeg_match,
+		.pm             = &mtk_jpeg_pm_ops,
+	},
+};
+
+module_platform_driver(mtk_jpeg_driver);
+
+MODULE_DESCRIPTION("MediaTek JPEG codec driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.h b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.h
new file mode 100644
index 000000000..3e4811a41
--- /dev/null
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.h
@@ -0,0 +1,163 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ *         Xia Jiang <xia.jiang@mediatek.com>
+ */
+
+#ifndef _MTK_JPEG_CORE_H
+#define _MTK_JPEG_CORE_H
+
+#include <linux/interrupt.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+
+#define MTK_JPEG_NAME		"mtk-jpeg"
+
+#define MTK_JPEG_COMP_MAX		3
+
+#define MTK_JPEG_FMT_FLAG_OUTPUT	BIT(0)
+#define MTK_JPEG_FMT_FLAG_CAPTURE	BIT(1)
+
+#define MTK_JPEG_MIN_WIDTH	32U
+#define MTK_JPEG_MIN_HEIGHT	32U
+#define MTK_JPEG_MAX_WIDTH	65535U
+#define MTK_JPEG_MAX_HEIGHT	65535U
+
+#define MTK_JPEG_DEFAULT_SIZEIMAGE	(1 * 1024 * 1024)
+
+#define MTK_JPEG_HW_TIMEOUT_MSEC 1000
+
+#define MTK_JPEG_MAX_EXIF_SIZE	(64 * 1024)
+
+/**
+ * enum mtk_jpeg_ctx_state - states of the context state machine
+ * @MTK_JPEG_INIT:		current state is initialized
+ * @MTK_JPEG_RUNNING:		current state is running
+ * @MTK_JPEG_SOURCE_CHANGE:	current state is source resolution change
+ */
+enum mtk_jpeg_ctx_state {
+	MTK_JPEG_INIT = 0,
+	MTK_JPEG_RUNNING,
+	MTK_JPEG_SOURCE_CHANGE,
+};
+
+/**
+ * struct mtk_jpeg_variant - mtk jpeg driver variant
+ * @clks:			clock names
+ * @num_clks:			numbers of clock
+ * @formats:			jpeg driver's internal color format
+ * @num_formats:		number of formats
+ * @qops:			the callback of jpeg vb2_ops
+ * @irq_handler:		jpeg irq handler callback
+ * @hw_reset:			jpeg hardware reset callback
+ * @m2m_ops:			the callback of jpeg v4l2_m2m_ops
+ * @dev_name:			jpeg device name
+ * @ioctl_ops:			the callback of jpeg v4l2_ioctl_ops
+ * @out_q_default_fourcc:	output queue default fourcc
+ * @cap_q_default_fourcc:	capture queue default fourcc
+ */
+struct mtk_jpeg_variant {
+	struct clk_bulk_data *clks;
+	int num_clks;
+	struct mtk_jpeg_fmt *formats;
+	int num_formats;
+	const struct vb2_ops *qops;
+	irqreturn_t (*irq_handler)(int irq, void *priv);
+	void (*hw_reset)(void __iomem *base);
+	const struct v4l2_m2m_ops *m2m_ops;
+	const char *dev_name;
+	const struct v4l2_ioctl_ops *ioctl_ops;
+	u32 out_q_default_fourcc;
+	u32 cap_q_default_fourcc;
+};
+
+/**
+ * struct mtk_jpeg_dev - JPEG IP abstraction
+ * @lock:		the mutex protecting this structure
+ * @hw_lock:		spinlock protecting the hw device resource
+ * @workqueue:		decode work queue
+ * @dev:		JPEG device
+ * @v4l2_dev:		v4l2 device for mem2mem mode
+ * @m2m_dev:		v4l2 mem2mem device data
+ * @alloc_ctx:		videobuf2 memory allocator's context
+ * @vdev:		video device node for jpeg mem2mem mode
+ * @reg_base:		JPEG registers mapping
+ * @job_timeout_work:	IRQ timeout structure
+ * @variant:		driver variant to be used
+ */
+struct mtk_jpeg_dev {
+	struct mutex		lock;
+	spinlock_t		hw_lock;
+	struct workqueue_struct	*workqueue;
+	struct device		*dev;
+	struct v4l2_device	v4l2_dev;
+	struct v4l2_m2m_dev	*m2m_dev;
+	void			*alloc_ctx;
+	struct video_device	*vdev;
+	void __iomem		*reg_base;
+	struct delayed_work job_timeout_work;
+	const struct mtk_jpeg_variant *variant;
+};
+
+/**
+ * struct mtk_jpeg_fmt - driver's internal color format data
+ * @fourcc:	the fourcc code, 0 if not applicable
+ * @hw_format:	hardware format value
+ * @h_sample:	horizontal sample count of plane in 4 * 4 pixel image
+ * @v_sample:	vertical sample count of plane in 4 * 4 pixel image
+ * @colplanes:	number of color planes (1 for packed formats)
+ * @h_align:	horizontal alignment order (align to 2^h_align)
+ * @v_align:	vertical alignment order (align to 2^v_align)
+ * @flags:	flags describing format applicability
+ */
+struct mtk_jpeg_fmt {
+	u32	fourcc;
+	u32	hw_format;
+	int	h_sample[VIDEO_MAX_PLANES];
+	int	v_sample[VIDEO_MAX_PLANES];
+	int	colplanes;
+	int	h_align;
+	int	v_align;
+	u32	flags;
+};
+
+/**
+ * struct mtk_jpeg_q_data - parameters of one queue
+ * @fmt:	  driver-specific format of this queue
+ * @pix_mp:	  multiplanar format
+ * @enc_crop_rect:	jpeg encoder crop information
+ */
+struct mtk_jpeg_q_data {
+	struct mtk_jpeg_fmt	*fmt;
+	struct v4l2_pix_format_mplane pix_mp;
+	struct v4l2_rect enc_crop_rect;
+};
+
+/**
+ * struct mtk_jpeg_ctx - the device context data
+ * @jpeg:		JPEG IP device for this context
+ * @out_q:		source (output) queue information
+ * @cap_q:		destination (capture) queue queue information
+ * @fh:			V4L2 file handle
+ * @state:		state of the context
+ * @enable_exif:	enable exif mode of jpeg encoder
+ * @enc_quality:	jpeg encoder quality
+ * @restart_interval:	jpeg encoder restart interval
+ * @ctrl_hdl:		controls handler
+ */
+struct mtk_jpeg_ctx {
+	struct mtk_jpeg_dev		*jpeg;
+	struct mtk_jpeg_q_data		out_q;
+	struct mtk_jpeg_q_data		cap_q;
+	struct v4l2_fh			fh;
+	enum mtk_jpeg_ctx_state		state;
+	bool enable_exif;
+	u8 enc_quality;
+	u8 restart_interval;
+	struct v4l2_ctrl_handler ctrl_hdl;
+};
+
+#endif /* _MTK_JPEG_CORE_H */
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.c b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.c
new file mode 100644
index 000000000..6d200e237
--- /dev/null
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.c
@@ -0,0 +1,415 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ */
+
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <media/videobuf2-core.h>
+
+#include "mtk_jpeg_dec_hw.h"
+
+#define MTK_JPEG_DUNUM_MASK(val)	(((val) - 1) & 0x3)
+
+enum mtk_jpeg_color {
+	MTK_JPEG_COLOR_420		= 0x00221111,
+	MTK_JPEG_COLOR_422		= 0x00211111,
+	MTK_JPEG_COLOR_444		= 0x00111111,
+	MTK_JPEG_COLOR_422V		= 0x00121111,
+	MTK_JPEG_COLOR_422X2		= 0x00412121,
+	MTK_JPEG_COLOR_422VX2		= 0x00222121,
+	MTK_JPEG_COLOR_400		= 0x00110000
+};
+
+static inline int mtk_jpeg_verify_align(u32 val, int align, u32 reg)
+{
+	if (val & (align - 1)) {
+		pr_err("mtk-jpeg: write reg %x without %d align\n", reg, align);
+		return -1;
+	}
+
+	return 0;
+}
+
+static int mtk_jpeg_decide_format(struct mtk_jpeg_dec_param *param)
+{
+	param->src_color = (param->sampling_w[0] << 20) |
+			   (param->sampling_h[0] << 16) |
+			   (param->sampling_w[1] << 12) |
+			   (param->sampling_h[1] << 8) |
+			   (param->sampling_w[2] << 4) |
+			   (param->sampling_h[2]);
+
+	param->uv_brz_w = 0;
+	switch (param->src_color) {
+	case MTK_JPEG_COLOR_444:
+		param->uv_brz_w = 1;
+		param->dst_fourcc = V4L2_PIX_FMT_YUV422M;
+		break;
+	case MTK_JPEG_COLOR_422X2:
+	case MTK_JPEG_COLOR_422:
+		param->dst_fourcc = V4L2_PIX_FMT_YUV422M;
+		break;
+	case MTK_JPEG_COLOR_422V:
+	case MTK_JPEG_COLOR_422VX2:
+		param->uv_brz_w = 1;
+		param->dst_fourcc = V4L2_PIX_FMT_YUV420M;
+		break;
+	case MTK_JPEG_COLOR_420:
+		param->dst_fourcc = V4L2_PIX_FMT_YUV420M;
+		break;
+	case MTK_JPEG_COLOR_400:
+		param->dst_fourcc = V4L2_PIX_FMT_GREY;
+		break;
+	default:
+		param->dst_fourcc = 0;
+		return -1;
+	}
+
+	return 0;
+}
+
+static void mtk_jpeg_calc_mcu(struct mtk_jpeg_dec_param *param)
+{
+	u32 factor_w, factor_h;
+	u32 i, comp, blk;
+
+	factor_w = 2 + param->sampling_w[0];
+	factor_h = 2 + param->sampling_h[0];
+	param->mcu_w = (param->pic_w + (1 << factor_w) - 1) >> factor_w;
+	param->mcu_h = (param->pic_h + (1 << factor_h) - 1) >> factor_h;
+	param->total_mcu = param->mcu_w * param->mcu_h;
+	param->unit_num = ((param->pic_w + 7) >> 3) * ((param->pic_h + 7) >> 3);
+	param->blk_num = 0;
+	for (i = 0; i < MTK_JPEG_COMP_MAX; i++) {
+		param->blk_comp[i] = 0;
+		if (i >= param->comp_num)
+			continue;
+		param->blk_comp[i] = param->sampling_w[i] *
+				     param->sampling_h[i];
+		param->blk_num += param->blk_comp[i];
+	}
+
+	param->membership = 0;
+	for (i = 0, blk = 0, comp = 0; i < MTK_JPEG_BLOCK_MAX; i++) {
+		if (i < param->blk_num && comp < param->comp_num) {
+			u32 tmp;
+
+			tmp = (0x04 + (comp & 0x3));
+			param->membership |= tmp << (i * 3);
+			if (++blk == param->blk_comp[comp]) {
+				comp++;
+				blk = 0;
+			}
+		} else {
+			param->membership |=  7 << (i * 3);
+		}
+	}
+}
+
+static void mtk_jpeg_calc_dma_group(struct mtk_jpeg_dec_param *param)
+{
+	u32 factor_mcu = 3;
+
+	if (param->src_color == MTK_JPEG_COLOR_444 &&
+	    param->dst_fourcc == V4L2_PIX_FMT_YUV422M)
+		factor_mcu = 4;
+	else if (param->src_color == MTK_JPEG_COLOR_422V &&
+		 param->dst_fourcc == V4L2_PIX_FMT_YUV420M)
+		factor_mcu = 4;
+	else if (param->src_color == MTK_JPEG_COLOR_422X2 &&
+		 param->dst_fourcc == V4L2_PIX_FMT_YUV422M)
+		factor_mcu = 2;
+	else if (param->src_color == MTK_JPEG_COLOR_400 ||
+		 (param->src_color & 0x0FFFF) == 0)
+		factor_mcu = 4;
+
+	param->dma_mcu = 1 << factor_mcu;
+	param->dma_group = param->mcu_w / param->dma_mcu;
+	param->dma_last_mcu = param->mcu_w % param->dma_mcu;
+	if (param->dma_last_mcu)
+		param->dma_group++;
+	else
+		param->dma_last_mcu = param->dma_mcu;
+}
+
+static int mtk_jpeg_calc_dst_size(struct mtk_jpeg_dec_param *param)
+{
+	u32 i, padding_w;
+	u32 ds_row_h[3];
+	u32 brz_w[3];
+
+	brz_w[0] = 0;
+	brz_w[1] = param->uv_brz_w;
+	brz_w[2] = brz_w[1];
+
+	for (i = 0; i < param->comp_num; i++) {
+		if (brz_w[i] > 3)
+			return -1;
+
+		padding_w = param->mcu_w * MTK_JPEG_DCTSIZE *
+				param->sampling_w[i];
+		/* output format is 420/422 */
+		param->comp_w[i] = padding_w >> brz_w[i];
+		param->comp_w[i] = round_up(param->comp_w[i],
+					    MTK_JPEG_DCTSIZE);
+		param->img_stride[i] = i ? round_up(param->comp_w[i], 16)
+					: round_up(param->comp_w[i], 32);
+		ds_row_h[i] = (MTK_JPEG_DCTSIZE * param->sampling_h[i]);
+	}
+	param->dec_w = param->img_stride[0];
+	param->dec_h = ds_row_h[0] * param->mcu_h;
+
+	for (i = 0; i < MTK_JPEG_COMP_MAX; i++) {
+		/* They must be equal in frame mode. */
+		param->mem_stride[i] = param->img_stride[i];
+		param->comp_size[i] = param->mem_stride[i] * ds_row_h[i] *
+				      param->mcu_h;
+	}
+
+	param->y_size = param->comp_size[0];
+	param->uv_size = param->comp_size[1];
+	param->dec_size = param->y_size + (param->uv_size << 1);
+
+	return 0;
+}
+
+int mtk_jpeg_dec_fill_param(struct mtk_jpeg_dec_param *param)
+{
+	if (mtk_jpeg_decide_format(param))
+		return -1;
+
+	mtk_jpeg_calc_mcu(param);
+	mtk_jpeg_calc_dma_group(param);
+	if (mtk_jpeg_calc_dst_size(param))
+		return -2;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_jpeg_dec_fill_param);
+
+u32 mtk_jpeg_dec_get_int_status(void __iomem *base)
+{
+	u32 ret;
+
+	ret = readl(base + JPGDEC_REG_INTERRUPT_STATUS) & BIT_INQST_MASK_ALLIRQ;
+	if (ret)
+		writel(ret, base + JPGDEC_REG_INTERRUPT_STATUS);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mtk_jpeg_dec_get_int_status);
+
+u32 mtk_jpeg_dec_enum_result(u32 irq_result)
+{
+	if (irq_result & BIT_INQST_MASK_EOF)
+		return MTK_JPEG_DEC_RESULT_EOF_DONE;
+	if (irq_result & BIT_INQST_MASK_PAUSE)
+		return MTK_JPEG_DEC_RESULT_PAUSE;
+	if (irq_result & BIT_INQST_MASK_UNDERFLOW)
+		return MTK_JPEG_DEC_RESULT_UNDERFLOW;
+	if (irq_result & BIT_INQST_MASK_OVERFLOW)
+		return MTK_JPEG_DEC_RESULT_OVERFLOW;
+	if (irq_result & BIT_INQST_MASK_ERROR_BS)
+		return MTK_JPEG_DEC_RESULT_ERROR_BS;
+
+	return MTK_JPEG_DEC_RESULT_ERROR_UNKNOWN;
+}
+EXPORT_SYMBOL_GPL(mtk_jpeg_dec_enum_result);
+
+void mtk_jpeg_dec_start(void __iomem *base)
+{
+	writel(0, base + JPGDEC_REG_TRIG);
+}
+EXPORT_SYMBOL_GPL(mtk_jpeg_dec_start);
+
+static void mtk_jpeg_dec_soft_reset(void __iomem *base)
+{
+	writel(0x0000FFFF, base + JPGDEC_REG_INTERRUPT_STATUS);
+	writel(0x00, base + JPGDEC_REG_RESET);
+	writel(0x01, base + JPGDEC_REG_RESET);
+}
+
+static void mtk_jpeg_dec_hard_reset(void __iomem *base)
+{
+	writel(0x00, base + JPGDEC_REG_RESET);
+	writel(0x10, base + JPGDEC_REG_RESET);
+}
+
+void mtk_jpeg_dec_reset(void __iomem *base)
+{
+	mtk_jpeg_dec_soft_reset(base);
+	mtk_jpeg_dec_hard_reset(base);
+}
+EXPORT_SYMBOL_GPL(mtk_jpeg_dec_reset);
+
+static void mtk_jpeg_dec_set_brz_factor(void __iomem *base, u8 yscale_w,
+					u8 yscale_h, u8 uvscale_w, u8 uvscale_h)
+{
+	u32 val;
+
+	val = (uvscale_h << 12) | (uvscale_w << 8) |
+	      (yscale_h << 4) | yscale_w;
+	writel(val, base + JPGDEC_REG_BRZ_FACTOR);
+}
+
+static void mtk_jpeg_dec_set_dst_bank0(void __iomem *base, u32 addr_y,
+				       u32 addr_u, u32 addr_v)
+{
+	mtk_jpeg_verify_align(addr_y, 16, JPGDEC_REG_DEST_ADDR0_Y);
+	writel(addr_y, base + JPGDEC_REG_DEST_ADDR0_Y);
+	mtk_jpeg_verify_align(addr_u, 16, JPGDEC_REG_DEST_ADDR0_U);
+	writel(addr_u, base + JPGDEC_REG_DEST_ADDR0_U);
+	mtk_jpeg_verify_align(addr_v, 16, JPGDEC_REG_DEST_ADDR0_V);
+	writel(addr_v, base + JPGDEC_REG_DEST_ADDR0_V);
+}
+
+static void mtk_jpeg_dec_set_dst_bank1(void __iomem *base, u32 addr_y,
+				       u32 addr_u, u32 addr_v)
+{
+	writel(addr_y, base + JPGDEC_REG_DEST_ADDR1_Y);
+	writel(addr_u, base + JPGDEC_REG_DEST_ADDR1_U);
+	writel(addr_v, base + JPGDEC_REG_DEST_ADDR1_V);
+}
+
+static void mtk_jpeg_dec_set_mem_stride(void __iomem *base, u32 stride_y,
+					u32 stride_uv)
+{
+	writel((stride_y & 0xFFFF), base + JPGDEC_REG_STRIDE_Y);
+	writel((stride_uv & 0xFFFF), base + JPGDEC_REG_STRIDE_UV);
+}
+
+static void mtk_jpeg_dec_set_img_stride(void __iomem *base, u32 stride_y,
+					u32 stride_uv)
+{
+	writel((stride_y & 0xFFFF), base + JPGDEC_REG_IMG_STRIDE_Y);
+	writel((stride_uv & 0xFFFF), base + JPGDEC_REG_IMG_STRIDE_UV);
+}
+
+static void mtk_jpeg_dec_set_pause_mcu_idx(void __iomem *base, u32 idx)
+{
+	writel(idx & 0x0003FFFFFF, base + JPGDEC_REG_PAUSE_MCU_NUM);
+}
+
+static void mtk_jpeg_dec_set_dec_mode(void __iomem *base, u32 mode)
+{
+	writel(mode & 0x03, base + JPGDEC_REG_OPERATION_MODE);
+}
+
+static void mtk_jpeg_dec_set_bs_write_ptr(void __iomem *base, u32 ptr)
+{
+	mtk_jpeg_verify_align(ptr, 16, JPGDEC_REG_FILE_BRP);
+	writel(ptr, base + JPGDEC_REG_FILE_BRP);
+}
+
+static void mtk_jpeg_dec_set_bs_info(void __iomem *base, u32 addr, u32 size)
+{
+	mtk_jpeg_verify_align(addr, 16, JPGDEC_REG_FILE_ADDR);
+	mtk_jpeg_verify_align(size, 128, JPGDEC_REG_FILE_TOTAL_SIZE);
+	writel(addr, base + JPGDEC_REG_FILE_ADDR);
+	writel(size, base + JPGDEC_REG_FILE_TOTAL_SIZE);
+}
+
+static void mtk_jpeg_dec_set_comp_id(void __iomem *base, u32 id_y, u32 id_u,
+				     u32 id_v)
+{
+	u32 val;
+
+	val = ((id_y & 0x00FF) << 24) | ((id_u & 0x00FF) << 16) |
+	      ((id_v & 0x00FF) << 8);
+	writel(val, base + JPGDEC_REG_COMP_ID);
+}
+
+static void mtk_jpeg_dec_set_total_mcu(void __iomem *base, u32 num)
+{
+	writel(num - 1, base + JPGDEC_REG_TOTAL_MCU_NUM);
+}
+
+static void mtk_jpeg_dec_set_comp0_du(void __iomem *base, u32 num)
+{
+	writel(num - 1, base + JPGDEC_REG_COMP0_DATA_UNIT_NUM);
+}
+
+static void mtk_jpeg_dec_set_du_membership(void __iomem *base, u32 member,
+					   u32 gmc, u32 isgray)
+{
+	if (isgray)
+		member = 0x3FFFFFFC;
+	member |= (isgray << 31) | (gmc << 30);
+	writel(member, base + JPGDEC_REG_DU_CTRL);
+}
+
+static void mtk_jpeg_dec_set_q_table(void __iomem *base, u32 id0, u32 id1,
+				     u32 id2)
+{
+	u32 val;
+
+	val = ((id0 & 0x0f) << 8) | ((id1 & 0x0f) << 4) | ((id2 & 0x0f) << 0);
+	writel(val, base + JPGDEC_REG_QT_ID);
+}
+
+static void mtk_jpeg_dec_set_dma_group(void __iomem *base, u32 mcu_group,
+				       u32 group_num, u32 last_mcu)
+{
+	u32 val;
+
+	val = (((mcu_group - 1) & 0x00FF) << 16) |
+	      (((group_num - 1) & 0x007F) << 8) |
+	      ((last_mcu - 1) & 0x00FF);
+	writel(val, base + JPGDEC_REG_WDMA_CTRL);
+}
+
+static void mtk_jpeg_dec_set_sampling_factor(void __iomem *base, u32 comp_num,
+					     u32 y_w, u32 y_h, u32 u_w,
+					     u32 u_h, u32 v_w, u32 v_h)
+{
+	u32 val;
+	u32 y_wh = (MTK_JPEG_DUNUM_MASK(y_w) << 2) | MTK_JPEG_DUNUM_MASK(y_h);
+	u32 u_wh = (MTK_JPEG_DUNUM_MASK(u_w) << 2) | MTK_JPEG_DUNUM_MASK(u_h);
+	u32 v_wh = (MTK_JPEG_DUNUM_MASK(v_w) << 2) | MTK_JPEG_DUNUM_MASK(v_h);
+
+	if (comp_num == 1)
+		val = 0;
+	else
+		val = (y_wh << 8) | (u_wh << 4) | v_wh;
+	writel(val, base + JPGDEC_REG_DU_NUM);
+}
+
+void mtk_jpeg_dec_set_config(void __iomem *base,
+			     struct mtk_jpeg_dec_param *config,
+			     struct mtk_jpeg_bs *bs,
+			     struct mtk_jpeg_fb *fb)
+{
+	mtk_jpeg_dec_set_brz_factor(base, 0, 0, config->uv_brz_w, 0);
+	mtk_jpeg_dec_set_dec_mode(base, 0);
+	mtk_jpeg_dec_set_comp0_du(base, config->unit_num);
+	mtk_jpeg_dec_set_total_mcu(base, config->total_mcu);
+	mtk_jpeg_dec_set_bs_info(base, bs->str_addr, bs->size);
+	mtk_jpeg_dec_set_bs_write_ptr(base, bs->end_addr);
+	mtk_jpeg_dec_set_du_membership(base, config->membership, 1,
+				       (config->comp_num == 1) ? 1 : 0);
+	mtk_jpeg_dec_set_comp_id(base, config->comp_id[0], config->comp_id[1],
+				 config->comp_id[2]);
+	mtk_jpeg_dec_set_q_table(base, config->qtbl_num[0],
+				 config->qtbl_num[1], config->qtbl_num[2]);
+	mtk_jpeg_dec_set_sampling_factor(base, config->comp_num,
+					 config->sampling_w[0],
+					 config->sampling_h[0],
+					 config->sampling_w[1],
+					 config->sampling_h[1],
+					 config->sampling_w[2],
+					 config->sampling_h[2]);
+	mtk_jpeg_dec_set_mem_stride(base, config->mem_stride[0],
+				    config->mem_stride[1]);
+	mtk_jpeg_dec_set_img_stride(base, config->img_stride[0],
+				    config->img_stride[1]);
+	mtk_jpeg_dec_set_dst_bank0(base, fb->plane_addr[0],
+				   fb->plane_addr[1], fb->plane_addr[2]);
+	mtk_jpeg_dec_set_dst_bank1(base, 0, 0, 0);
+	mtk_jpeg_dec_set_dma_group(base, config->dma_mcu, config->dma_group,
+				   config->dma_last_mcu);
+	mtk_jpeg_dec_set_pause_mcu_idx(base, config->total_mcu);
+}
+EXPORT_SYMBOL_GPL(mtk_jpeg_dec_set_config);
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.h b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.h
new file mode 100644
index 000000000..fa0d45fd7
--- /dev/null
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.h
@@ -0,0 +1,79 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ *         Xia Jiang <xia.jiang@mediatek.com>
+ */
+
+#ifndef _MTK_JPEG_DEC_HW_H
+#define _MTK_JPEG_DEC_HW_H
+
+#include <media/videobuf2-core.h>
+
+#include "mtk_jpeg_core.h"
+#include "mtk_jpeg_dec_reg.h"
+
+enum {
+	MTK_JPEG_DEC_RESULT_EOF_DONE		= 0,
+	MTK_JPEG_DEC_RESULT_PAUSE		= 1,
+	MTK_JPEG_DEC_RESULT_UNDERFLOW		= 2,
+	MTK_JPEG_DEC_RESULT_OVERFLOW		= 3,
+	MTK_JPEG_DEC_RESULT_ERROR_BS		= 4,
+	MTK_JPEG_DEC_RESULT_ERROR_UNKNOWN	= 6
+};
+
+struct mtk_jpeg_dec_param {
+	u32 pic_w;
+	u32 pic_h;
+	u32 dec_w;
+	u32 dec_h;
+	u32 src_color;
+	u32 dst_fourcc;
+	u32 mcu_w;
+	u32 mcu_h;
+	u32 total_mcu;
+	u32 unit_num;
+	u32 comp_num;
+	u32 comp_id[MTK_JPEG_COMP_MAX];
+	u32 sampling_w[MTK_JPEG_COMP_MAX];
+	u32 sampling_h[MTK_JPEG_COMP_MAX];
+	u32 qtbl_num[MTK_JPEG_COMP_MAX];
+	u32 blk_num;
+	u32 blk_comp[MTK_JPEG_COMP_MAX];
+	u32 membership;
+	u32 dma_mcu;
+	u32 dma_group;
+	u32 dma_last_mcu;
+	u32 img_stride[MTK_JPEG_COMP_MAX];
+	u32 mem_stride[MTK_JPEG_COMP_MAX];
+	u32 comp_w[MTK_JPEG_COMP_MAX];
+	u32 comp_size[MTK_JPEG_COMP_MAX];
+	u32 y_size;
+	u32 uv_size;
+	u32 dec_size;
+	u8 uv_brz_w;
+};
+
+struct mtk_jpeg_bs {
+	dma_addr_t	str_addr;
+	dma_addr_t	end_addr;
+	size_t		size;
+};
+
+struct mtk_jpeg_fb {
+	dma_addr_t	plane_addr[MTK_JPEG_COMP_MAX];
+	size_t		size;
+};
+
+int mtk_jpeg_dec_fill_param(struct mtk_jpeg_dec_param *param);
+u32 mtk_jpeg_dec_get_int_status(void __iomem *dec_reg_base);
+u32 mtk_jpeg_dec_enum_result(u32 irq_result);
+void mtk_jpeg_dec_set_config(void __iomem *base,
+			     struct mtk_jpeg_dec_param *config,
+			     struct mtk_jpeg_bs *bs,
+			     struct mtk_jpeg_fb *fb);
+void mtk_jpeg_dec_reset(void __iomem *dec_reg_base);
+void mtk_jpeg_dec_start(void __iomem *dec_reg_base);
+
+#endif /* _MTK_JPEG_HW_H */
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_parse.c b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_parse.c
new file mode 100644
index 000000000..b95c45791
--- /dev/null
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_parse.c
@@ -0,0 +1,152 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+
+#include "mtk_jpeg_dec_parse.h"
+
+#define TEM	0x01
+#define SOF0	0xc0
+#define RST	0xd0
+#define SOI	0xd8
+#define EOI	0xd9
+
+struct mtk_jpeg_stream {
+	u8 *addr;
+	u32 size;
+	u32 curr;
+};
+
+static int read_byte(struct mtk_jpeg_stream *stream)
+{
+	if (stream->curr >= stream->size)
+		return -1;
+	return stream->addr[stream->curr++];
+}
+
+static int read_word_be(struct mtk_jpeg_stream *stream, u32 *word)
+{
+	u32 temp;
+	int byte;
+
+	byte = read_byte(stream);
+	if (byte == -1)
+		return -1;
+	temp = byte << 8;
+	byte = read_byte(stream);
+	if (byte == -1)
+		return -1;
+	*word = (u32)byte | temp;
+
+	return 0;
+}
+
+static void read_skip(struct mtk_jpeg_stream *stream, long len)
+{
+	if (len <= 0)
+		return;
+	while (len--)
+		read_byte(stream);
+}
+
+static bool mtk_jpeg_do_parse(struct mtk_jpeg_dec_param *param, u8 *src_addr_va,
+			      u32 src_size)
+{
+	bool notfound = true;
+	struct mtk_jpeg_stream stream;
+
+	stream.addr = src_addr_va;
+	stream.size = src_size;
+	stream.curr = 0;
+
+	while (notfound) {
+		int i, length, byte;
+		u32 word;
+
+		byte = read_byte(&stream);
+		if (byte == -1)
+			return false;
+		if (byte != 0xff)
+			continue;
+		do
+			byte = read_byte(&stream);
+		while (byte == 0xff);
+		if (byte == -1)
+			return false;
+		if (byte == 0)
+			continue;
+
+		length = 0;
+		switch (byte) {
+		case SOF0:
+			/* length */
+			if (read_word_be(&stream, &word))
+				break;
+
+			/* precision */
+			if (read_byte(&stream) == -1)
+				break;
+
+			if (read_word_be(&stream, &word))
+				break;
+			param->pic_h = word;
+
+			if (read_word_be(&stream, &word))
+				break;
+			param->pic_w = word;
+
+			param->comp_num = read_byte(&stream);
+			if (param->comp_num != 1 && param->comp_num != 3)
+				break;
+
+			for (i = 0; i < param->comp_num; i++) {
+				param->comp_id[i] = read_byte(&stream);
+				if (param->comp_id[i] == -1)
+					break;
+
+				/* sampling */
+				byte = read_byte(&stream);
+				if (byte == -1)
+					break;
+				param->sampling_w[i] = (byte >> 4) & 0x0F;
+				param->sampling_h[i] = byte & 0x0F;
+
+				param->qtbl_num[i] = read_byte(&stream);
+				if (param->qtbl_num[i] == -1)
+					break;
+			}
+
+			notfound = !(i == param->comp_num);
+			break;
+		case RST ... RST + 7:
+		case SOI:
+		case EOI:
+		case TEM:
+			break;
+		default:
+			if (read_word_be(&stream, &word))
+				break;
+			length = (long)word - 2;
+			read_skip(&stream, length);
+			break;
+		}
+	}
+
+	return !notfound;
+}
+
+bool mtk_jpeg_parse(struct mtk_jpeg_dec_param *param, u8 *src_addr_va,
+		    u32 src_size)
+{
+	if (!mtk_jpeg_do_parse(param, src_addr_va, src_size))
+		return false;
+	if (mtk_jpeg_dec_fill_param(param))
+		return false;
+
+	return true;
+}
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_parse.h b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_parse.h
new file mode 100644
index 000000000..2918f1581
--- /dev/null
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_parse.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ */
+
+#ifndef _MTK_JPEG_PARSE_H
+#define _MTK_JPEG_PARSE_H
+
+#include "mtk_jpeg_dec_hw.h"
+
+bool mtk_jpeg_parse(struct mtk_jpeg_dec_param *param, u8 *src_addr_va,
+		    u32 src_size);
+
+#endif /* _MTK_JPEG_PARSE_H */
+
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_reg.h b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_reg.h
new file mode 100644
index 000000000..21ec8f967
--- /dev/null
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_reg.h
@@ -0,0 +1,49 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ *         Rick Chang <rick.chang@mediatek.com>
+ */
+
+#ifndef _MTK_JPEG_REG_H
+#define _MTK_JPEG_REG_H
+
+#define MTK_JPEG_BLOCK_MAX		10
+#define MTK_JPEG_DCTSIZE		8
+
+#define BIT_INQST_MASK_ERROR_BS		0x20
+#define BIT_INQST_MASK_PAUSE		0x10
+#define BIT_INQST_MASK_OVERFLOW		0x04
+#define BIT_INQST_MASK_UNDERFLOW	0x02
+#define BIT_INQST_MASK_EOF		0x01
+#define BIT_INQST_MASK_ALLIRQ		0x37
+
+#define JPGDEC_REG_RESET		0x0090
+#define JPGDEC_REG_BRZ_FACTOR		0x00f8
+#define JPGDEC_REG_DU_NUM		0x00fc
+#define JPGDEC_REG_DEST_ADDR0_Y		0x0140
+#define JPGDEC_REG_DEST_ADDR0_U		0x0144
+#define JPGDEC_REG_DEST_ADDR0_V		0x0148
+#define JPGDEC_REG_DEST_ADDR1_Y		0x014c
+#define JPGDEC_REG_DEST_ADDR1_U		0x0150
+#define JPGDEC_REG_DEST_ADDR1_V		0x0154
+#define JPGDEC_REG_STRIDE_Y		0x0158
+#define JPGDEC_REG_STRIDE_UV		0x015c
+#define JPGDEC_REG_IMG_STRIDE_Y		0x0160
+#define JPGDEC_REG_IMG_STRIDE_UV	0x0164
+#define JPGDEC_REG_WDMA_CTRL		0x016c
+#define JPGDEC_REG_PAUSE_MCU_NUM	0x0170
+#define JPGDEC_REG_OPERATION_MODE	0x017c
+#define JPGDEC_REG_FILE_ADDR		0x0200
+#define JPGDEC_REG_COMP_ID		0x020c
+#define JPGDEC_REG_TOTAL_MCU_NUM	0x0210
+#define JPGDEC_REG_COMP0_DATA_UNIT_NUM	0x0224
+#define JPGDEC_REG_DU_CTRL		0x023c
+#define JPGDEC_REG_TRIG			0x0240
+#define JPGDEC_REG_FILE_BRP		0x0248
+#define JPGDEC_REG_FILE_TOTAL_SIZE	0x024c
+#define JPGDEC_REG_QT_ID		0x0270
+#define JPGDEC_REG_INTERRUPT_STATUS	0x0274
+#define JPGDEC_REG_STATUS		0x0278
+
+#endif /* _MTK_JPEG_REG_H */
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.c b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.c
new file mode 100644
index 000000000..8c271c38c
--- /dev/null
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.c
@@ -0,0 +1,155 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ * Author: Xia Jiang <xia.jiang@mediatek.com>
+ *
+ */
+
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "mtk_jpeg_enc_hw.h"
+
+static const struct mtk_jpeg_enc_qlt mtk_jpeg_enc_quality[] = {
+	{.quality_param = 34, .hardware_value = JPEG_ENC_QUALITY_Q34},
+	{.quality_param = 39, .hardware_value = JPEG_ENC_QUALITY_Q39},
+	{.quality_param = 48, .hardware_value = JPEG_ENC_QUALITY_Q48},
+	{.quality_param = 60, .hardware_value = JPEG_ENC_QUALITY_Q60},
+	{.quality_param = 64, .hardware_value = JPEG_ENC_QUALITY_Q64},
+	{.quality_param = 68, .hardware_value = JPEG_ENC_QUALITY_Q68},
+	{.quality_param = 74, .hardware_value = JPEG_ENC_QUALITY_Q74},
+	{.quality_param = 80, .hardware_value = JPEG_ENC_QUALITY_Q80},
+	{.quality_param = 82, .hardware_value = JPEG_ENC_QUALITY_Q82},
+	{.quality_param = 84, .hardware_value = JPEG_ENC_QUALITY_Q84},
+	{.quality_param = 87, .hardware_value = JPEG_ENC_QUALITY_Q87},
+	{.quality_param = 90, .hardware_value = JPEG_ENC_QUALITY_Q90},
+	{.quality_param = 92, .hardware_value = JPEG_ENC_QUALITY_Q92},
+	{.quality_param = 95, .hardware_value = JPEG_ENC_QUALITY_Q95},
+	{.quality_param = 97, .hardware_value = JPEG_ENC_QUALITY_Q97},
+};
+
+void mtk_jpeg_enc_reset(void __iomem *base)
+{
+	writel(0, base + JPEG_ENC_RSTB);
+	writel(JPEG_ENC_RESET_BIT, base + JPEG_ENC_RSTB);
+	writel(0, base + JPEG_ENC_CODEC_SEL);
+}
+
+u32 mtk_jpeg_enc_get_file_size(void __iomem *base)
+{
+	return readl(base + JPEG_ENC_DMA_ADDR0) -
+	       readl(base + JPEG_ENC_DST_ADDR0);
+}
+
+void mtk_jpeg_enc_start(void __iomem *base)
+{
+	u32 value;
+
+	value = readl(base + JPEG_ENC_CTRL);
+	value |= JPEG_ENC_CTRL_INT_EN_BIT | JPEG_ENC_CTRL_ENABLE_BIT;
+	writel(value, base + JPEG_ENC_CTRL);
+}
+
+void mtk_jpeg_set_enc_src(struct mtk_jpeg_ctx *ctx,  void __iomem *base,
+			  struct vb2_buffer *src_buf)
+{
+	int i;
+	dma_addr_t dma_addr;
+
+	for (i = 0; i < src_buf->num_planes; i++) {
+		dma_addr = vb2_dma_contig_plane_dma_addr(src_buf, i) +
+			   src_buf->planes[i].data_offset;
+		if (!i)
+			writel(dma_addr, base + JPEG_ENC_SRC_LUMA_ADDR);
+		else
+			writel(dma_addr, base + JPEG_ENC_SRC_CHROMA_ADDR);
+	}
+}
+
+void mtk_jpeg_set_enc_dst(struct mtk_jpeg_ctx *ctx, void __iomem *base,
+			  struct vb2_buffer *dst_buf)
+{
+	dma_addr_t dma_addr;
+	size_t size;
+	u32 dma_addr_offset;
+	u32 dma_addr_offsetmask;
+
+	dma_addr = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
+	dma_addr_offset = ctx->enable_exif ? MTK_JPEG_MAX_EXIF_SIZE : 0;
+	dma_addr_offsetmask = dma_addr & JPEG_ENC_DST_ADDR_OFFSET_MASK;
+	size = vb2_plane_size(dst_buf, 0);
+
+	writel(dma_addr_offset & ~0xf, base + JPEG_ENC_OFFSET_ADDR);
+	writel(dma_addr_offsetmask & 0xf, base + JPEG_ENC_BYTE_OFFSET_MASK);
+	writel(dma_addr & ~0xf, base + JPEG_ENC_DST_ADDR0);
+	writel((dma_addr + size) & ~0xf, base + JPEG_ENC_STALL_ADDR0);
+}
+
+void mtk_jpeg_set_enc_params(struct mtk_jpeg_ctx *ctx,  void __iomem *base)
+{
+	u32 value;
+	u32 width = ctx->out_q.enc_crop_rect.width;
+	u32 height = ctx->out_q.enc_crop_rect.height;
+	u32 enc_format = ctx->out_q.fmt->fourcc;
+	u32 bytesperline = ctx->out_q.pix_mp.plane_fmt[0].bytesperline;
+	u32 blk_num;
+	u32 img_stride;
+	u32 mem_stride;
+	u32 i, enc_quality;
+	u32 nr_enc_quality = ARRAY_SIZE(mtk_jpeg_enc_quality);
+
+	value = width << 16 | height;
+	writel(value, base + JPEG_ENC_IMG_SIZE);
+
+	if (enc_format == V4L2_PIX_FMT_NV12M ||
+	    enc_format == V4L2_PIX_FMT_NV21M)
+	    /*
+	     * Total 8 x 8 block number of luma and chroma.
+	     * The number of blocks is counted from 0.
+	     */
+		blk_num = DIV_ROUND_UP(width, 16) *
+			  DIV_ROUND_UP(height, 16) * 6 - 1;
+	else
+		blk_num = DIV_ROUND_UP(width, 16) *
+			  DIV_ROUND_UP(height, 8) * 4 - 1;
+	writel(blk_num, base + JPEG_ENC_BLK_NUM);
+
+	if (enc_format == V4L2_PIX_FMT_NV12M ||
+	    enc_format == V4L2_PIX_FMT_NV21M) {
+		/* 4:2:0 */
+		img_stride = round_up(width, 16);
+		mem_stride = bytesperline;
+	} else {
+		/* 4:2:2 */
+		img_stride = round_up(width * 2, 32);
+		mem_stride = img_stride;
+	}
+	writel(img_stride, base + JPEG_ENC_IMG_STRIDE);
+	writel(mem_stride, base + JPEG_ENC_STRIDE);
+
+	enc_quality = mtk_jpeg_enc_quality[nr_enc_quality - 1].hardware_value;
+	for (i = 0; i < nr_enc_quality; i++) {
+		if (ctx->enc_quality <= mtk_jpeg_enc_quality[i].quality_param) {
+			enc_quality = mtk_jpeg_enc_quality[i].hardware_value;
+			break;
+		}
+	}
+	writel(enc_quality, base + JPEG_ENC_QUALITY);
+
+	value = readl(base + JPEG_ENC_CTRL);
+	value &= ~JPEG_ENC_CTRL_YUV_FORMAT_MASK;
+	value |= (ctx->out_q.fmt->hw_format & 3) << 3;
+	if (ctx->enable_exif)
+		value |= JPEG_ENC_CTRL_FILE_FORMAT_BIT;
+	else
+		value &= ~JPEG_ENC_CTRL_FILE_FORMAT_BIT;
+	if (ctx->restart_interval)
+		value |= JPEG_ENC_CTRL_RESTART_EN_BIT;
+	else
+		value &= ~JPEG_ENC_CTRL_RESTART_EN_BIT;
+	writel(value, base + JPEG_ENC_CTRL);
+
+	writel(ctx->restart_interval, base + JPEG_ENC_RST_MCU_NUM);
+}
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.h b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.h
new file mode 100644
index 000000000..61c60e4e5
--- /dev/null
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.h
@@ -0,0 +1,91 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2019 MediaTek Inc.
+ * Author: Xia Jiang <xia.jiang@mediatek.com>
+ *
+ */
+
+#ifndef _MTK_JPEG_ENC_HW_H
+#define _MTK_JPEG_ENC_HW_H
+
+#include <media/videobuf2-core.h>
+
+#include "mtk_jpeg_core.h"
+
+#define JPEG_ENC_INT_STATUS_DONE	BIT(0)
+#define JPEG_ENC_INT_STATUS_MASK_ALLIRQ	0x13
+
+#define JPEG_ENC_DST_ADDR_OFFSET_MASK	GENMASK(3, 0)
+
+#define JPEG_ENC_CTRL_YUV_FORMAT_MASK	0x18
+#define JPEG_ENC_CTRL_RESTART_EN_BIT	BIT(10)
+#define JPEG_ENC_CTRL_FILE_FORMAT_BIT	BIT(5)
+#define JPEG_ENC_CTRL_INT_EN_BIT	BIT(2)
+#define JPEG_ENC_CTRL_ENABLE_BIT	BIT(0)
+#define JPEG_ENC_RESET_BIT		BIT(0)
+
+#define JPEG_ENC_YUV_FORMAT_YUYV	0
+#define JPEG_ENC_YUV_FORMAT_YVYU	1
+#define JPEG_ENC_YUV_FORMAT_NV12	2
+#define JEPG_ENC_YUV_FORMAT_NV21	3
+
+#define JPEG_ENC_QUALITY_Q60		0x0
+#define JPEG_ENC_QUALITY_Q80		0x1
+#define JPEG_ENC_QUALITY_Q90		0x2
+#define JPEG_ENC_QUALITY_Q95		0x3
+#define JPEG_ENC_QUALITY_Q39		0x4
+#define JPEG_ENC_QUALITY_Q68		0x5
+#define JPEG_ENC_QUALITY_Q84		0x6
+#define JPEG_ENC_QUALITY_Q92		0x7
+#define JPEG_ENC_QUALITY_Q48		0x8
+#define JPEG_ENC_QUALITY_Q74		0xa
+#define JPEG_ENC_QUALITY_Q87		0xb
+#define JPEG_ENC_QUALITY_Q34		0xc
+#define JPEG_ENC_QUALITY_Q64		0xe
+#define JPEG_ENC_QUALITY_Q82		0xf
+#define JPEG_ENC_QUALITY_Q97		0x10
+
+#define JPEG_ENC_RSTB			0x100
+#define JPEG_ENC_CTRL			0x104
+#define JPEG_ENC_QUALITY		0x108
+#define JPEG_ENC_BLK_NUM		0x10C
+#define JPEG_ENC_BLK_CNT		0x110
+#define JPEG_ENC_INT_STS		0x11c
+#define JPEG_ENC_DST_ADDR0		0x120
+#define JPEG_ENC_DMA_ADDR0		0x124
+#define JPEG_ENC_STALL_ADDR0		0x128
+#define JPEG_ENC_OFFSET_ADDR		0x138
+#define JPEG_ENC_RST_MCU_NUM		0x150
+#define JPEG_ENC_IMG_SIZE		0x154
+#define JPEG_ENC_DEBUG_INFO0		0x160
+#define JPEG_ENC_DEBUG_INFO1		0x164
+#define JPEG_ENC_TOTAL_CYCLE		0x168
+#define JPEG_ENC_BYTE_OFFSET_MASK	0x16c
+#define JPEG_ENC_SRC_LUMA_ADDR		0x170
+#define JPEG_ENC_SRC_CHROMA_ADDR	0x174
+#define JPEG_ENC_STRIDE			0x178
+#define JPEG_ENC_IMG_STRIDE		0x17c
+#define JPEG_ENC_DCM_CTRL		0x300
+#define JPEG_ENC_CODEC_SEL		0x314
+#define JPEG_ENC_ULTRA_THRES		0x318
+
+/**
+ * struct mtk_jpeg_enc_qlt - JPEG encoder quality data
+ * @quality_param:	quality value
+ * @hardware_value:	hardware value of quality
+ */
+struct mtk_jpeg_enc_qlt {
+	u8	quality_param;
+	u8	hardware_value;
+};
+
+void mtk_jpeg_enc_reset(void __iomem *base);
+u32 mtk_jpeg_enc_get_file_size(void __iomem *base);
+void mtk_jpeg_enc_start(void __iomem *enc_reg_base);
+void mtk_jpeg_set_enc_src(struct mtk_jpeg_ctx *ctx,  void __iomem *base,
+			  struct vb2_buffer *src_buf);
+void mtk_jpeg_set_enc_dst(struct mtk_jpeg_ctx *ctx, void __iomem *base,
+			  struct vb2_buffer *dst_buf);
+void mtk_jpeg_set_enc_params(struct mtk_jpeg_ctx *ctx,  void __iomem *base);
+
+#endif /* _MTK_JPEG_ENC_HW_H */
diff --git a/drivers/media/platform/mediatek/mdp/Kconfig b/drivers/media/platform/mediatek/mdp/Kconfig
new file mode 100644
index 000000000..9f13a4289
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/Kconfig
@@ -0,0 +1,17 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_MEDIATEK_MDP
+	tristate "Mediatek MDP driver"
+	depends on V4L_MEM2MEM_DRIVERS
+	depends on MTK_IOMMU || COMPILE_TEST
+	depends on VIDEO_DEV
+	depends on ARCH_MEDIATEK || COMPILE_TEST
+	depends on MTK_SMI || (COMPILE_TEST && MTK_SMI=n)
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	select VIDEO_MEDIATEK_VPU
+	help
+	    It is a v4l2 driver and present in Mediatek MT8173 SoCs.
+	    The driver supports for scaling and color space conversion.
+
+	    To compile this driver as a module, choose M here: the
+	    module will be called mtk-mdp.
diff --git a/drivers/media/platform/mediatek/mdp/Makefile b/drivers/media/platform/mediatek/mdp/Makefile
new file mode 100644
index 000000000..b7c16ebec
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/Makefile
@@ -0,0 +1,10 @@
+# SPDX-License-Identifier: GPL-2.0
+mtk-mdp-y += mtk_mdp_core.o
+mtk-mdp-y += mtk_mdp_comp.o
+mtk-mdp-y += mtk_mdp_m2m.o
+mtk-mdp-y += mtk_mdp_regs.o
+mtk-mdp-y += mtk_mdp_vpu.o
+
+obj-$(CONFIG_VIDEO_MEDIATEK_MDP) += mtk-mdp.o
+
+ccflags-y += -I$(srctree)/drivers/media/platform/mediatek/vpu
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_comp.c b/drivers/media/platform/mediatek/mdp/mtk_mdp_comp.c
new file mode 100644
index 000000000..1e3833f1c
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_comp.c
@@ -0,0 +1,77 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+
+#include "mtk_mdp_comp.h"
+
+
+void mtk_mdp_comp_clock_on(struct device *dev, struct mtk_mdp_comp *comp)
+{
+	int i, err;
+
+	for (i = 0; i < ARRAY_SIZE(comp->clk); i++) {
+		if (IS_ERR(comp->clk[i]))
+			continue;
+		err = clk_prepare_enable(comp->clk[i]);
+		if (err)
+			dev_err(dev,
+			"failed to enable clock, err %d. type:%d i:%d\n",
+				err, comp->type, i);
+	}
+}
+
+void mtk_mdp_comp_clock_off(struct device *dev, struct mtk_mdp_comp *comp)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(comp->clk); i++) {
+		if (IS_ERR(comp->clk[i]))
+			continue;
+		clk_disable_unprepare(comp->clk[i]);
+	}
+}
+
+int mtk_mdp_comp_init(struct device *dev, struct device_node *node,
+		      struct mtk_mdp_comp *comp,
+		      enum mtk_mdp_comp_type comp_type)
+{
+	int ret;
+	int i;
+
+	comp->dev_node = of_node_get(node);
+	comp->type = comp_type;
+
+	for (i = 0; i < ARRAY_SIZE(comp->clk); i++) {
+		comp->clk[i] = of_clk_get(node, i);
+		if (IS_ERR(comp->clk[i])) {
+			if (PTR_ERR(comp->clk[i]) != -EPROBE_DEFER)
+				dev_err(dev, "Failed to get clock\n");
+			ret = PTR_ERR(comp->clk[i]);
+			goto put_dev;
+		}
+
+		/* Only RDMA needs two clocks */
+		if (comp->type != MTK_MDP_RDMA)
+			break;
+	}
+
+	return 0;
+
+put_dev:
+	of_node_put(comp->dev_node);
+
+	return ret;
+}
+
+void mtk_mdp_comp_deinit(struct device *dev, struct mtk_mdp_comp *comp)
+{
+	of_node_put(comp->dev_node);
+}
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_comp.h b/drivers/media/platform/mediatek/mdp/mtk_mdp_comp.h
new file mode 100644
index 000000000..ae41dd3cd
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_comp.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ */
+
+#ifndef __MTK_MDP_COMP_H__
+#define __MTK_MDP_COMP_H__
+
+/**
+ * enum mtk_mdp_comp_type - the MDP component
+ * @MTK_MDP_RDMA:	Read DMA
+ * @MTK_MDP_RSZ:	Riszer
+ * @MTK_MDP_WDMA:	Write DMA
+ * @MTK_MDP_WROT:	Write DMA with rotation
+ */
+enum mtk_mdp_comp_type {
+	MTK_MDP_RDMA,
+	MTK_MDP_RSZ,
+	MTK_MDP_WDMA,
+	MTK_MDP_WROT,
+};
+
+/**
+ * struct mtk_mdp_comp - the MDP's function component data
+ * @node:	list node to track sibing MDP components
+ * @dev_node:	component device node
+ * @clk:	clocks required for component
+ * @type:	component type
+ */
+struct mtk_mdp_comp {
+	struct list_head	node;
+	struct device_node	*dev_node;
+	struct clk		*clk[2];
+	enum mtk_mdp_comp_type	type;
+};
+
+int mtk_mdp_comp_init(struct device *dev, struct device_node *node,
+		      struct mtk_mdp_comp *comp,
+		      enum mtk_mdp_comp_type comp_type);
+void mtk_mdp_comp_deinit(struct device *dev, struct mtk_mdp_comp *comp);
+void mtk_mdp_comp_clock_on(struct device *dev, struct mtk_mdp_comp *comp);
+void mtk_mdp_comp_clock_off(struct device *dev, struct mtk_mdp_comp *comp);
+
+
+#endif /* __MTK_MDP_COMP_H__ */
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_core.c b/drivers/media/platform/mediatek/mdp/mtk_mdp_core.c
new file mode 100644
index 000000000..d83c4964e
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_core.c
@@ -0,0 +1,314 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2015-2016 MediaTek Inc.
+ * Author: Houlong Wei <houlong.wei@mediatek.com>
+ *         Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/workqueue.h>
+
+#include "mtk_mdp_core.h"
+#include "mtk_mdp_m2m.h"
+#include "mtk_vpu.h"
+
+/* MDP debug log level (0-3). 3 shows all the logs. */
+int mtk_mdp_dbg_level;
+EXPORT_SYMBOL(mtk_mdp_dbg_level);
+
+module_param(mtk_mdp_dbg_level, int, 0644);
+
+static const struct of_device_id mtk_mdp_comp_dt_ids[] = {
+	{
+		.compatible = "mediatek,mt8173-mdp-rdma",
+		.data = (void *)MTK_MDP_RDMA
+	}, {
+		.compatible = "mediatek,mt8173-mdp-rsz",
+		.data = (void *)MTK_MDP_RSZ
+	}, {
+		.compatible = "mediatek,mt8173-mdp-wdma",
+		.data = (void *)MTK_MDP_WDMA
+	}, {
+		.compatible = "mediatek,mt8173-mdp-wrot",
+		.data = (void *)MTK_MDP_WROT
+	},
+	{ },
+};
+
+static const struct of_device_id mtk_mdp_of_ids[] = {
+	{ .compatible = "mediatek,mt8173-mdp", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, mtk_mdp_of_ids);
+
+static void mtk_mdp_clock_on(struct mtk_mdp_dev *mdp)
+{
+	struct device *dev = &mdp->pdev->dev;
+	struct mtk_mdp_comp *comp_node;
+
+	list_for_each_entry(comp_node, &mdp->comp_list, node)
+		mtk_mdp_comp_clock_on(dev, comp_node);
+}
+
+static void mtk_mdp_clock_off(struct mtk_mdp_dev *mdp)
+{
+	struct device *dev = &mdp->pdev->dev;
+	struct mtk_mdp_comp *comp_node;
+
+	list_for_each_entry(comp_node, &mdp->comp_list, node)
+		mtk_mdp_comp_clock_off(dev, comp_node);
+}
+
+static void mtk_mdp_wdt_worker(struct work_struct *work)
+{
+	struct mtk_mdp_dev *mdp =
+			container_of(work, struct mtk_mdp_dev, wdt_work);
+	struct mtk_mdp_ctx *ctx;
+
+	mtk_mdp_err("Watchdog timeout");
+
+	list_for_each_entry(ctx, &mdp->ctx_list, list) {
+		mtk_mdp_dbg(0, "[%d] Change as state error", ctx->id);
+		mtk_mdp_ctx_state_lock_set(ctx, MTK_MDP_CTX_ERROR);
+	}
+}
+
+static void mtk_mdp_reset_handler(void *priv)
+{
+	struct mtk_mdp_dev *mdp = priv;
+
+	queue_work(mdp->wdt_wq, &mdp->wdt_work);
+}
+
+void mtk_mdp_register_component(struct mtk_mdp_dev *mdp,
+				struct mtk_mdp_comp *comp)
+{
+	list_add(&comp->node, &mdp->comp_list);
+}
+
+void mtk_mdp_unregister_component(struct mtk_mdp_dev *mdp,
+				  struct mtk_mdp_comp *comp)
+{
+	list_del(&comp->node);
+}
+
+static int mtk_mdp_probe(struct platform_device *pdev)
+{
+	struct mtk_mdp_dev *mdp;
+	struct device *dev = &pdev->dev;
+	struct device_node *node, *parent;
+	struct mtk_mdp_comp *comp, *comp_temp;
+	int ret = 0;
+
+	mdp = devm_kzalloc(dev, sizeof(*mdp), GFP_KERNEL);
+	if (!mdp)
+		return -ENOMEM;
+
+	mdp->id = pdev->id;
+	mdp->pdev = pdev;
+	INIT_LIST_HEAD(&mdp->comp_list);
+	INIT_LIST_HEAD(&mdp->ctx_list);
+
+	mutex_init(&mdp->lock);
+	mutex_init(&mdp->vpulock);
+
+	/* Old dts had the components as child nodes */
+	node = of_get_next_child(dev->of_node, NULL);
+	if (node) {
+		of_node_put(node);
+		parent = dev->of_node;
+		dev_warn(dev, "device tree is out of date\n");
+	} else {
+		parent = dev->of_node->parent;
+	}
+
+	/* Iterate over sibling MDP function blocks */
+	for_each_child_of_node(parent, node) {
+		const struct of_device_id *of_id;
+		enum mtk_mdp_comp_type comp_type;
+
+		of_id = of_match_node(mtk_mdp_comp_dt_ids, node);
+		if (!of_id)
+			continue;
+
+		if (!of_device_is_available(node)) {
+			dev_err(dev, "Skipping disabled component %pOF\n",
+				node);
+			continue;
+		}
+
+		comp_type = (uintptr_t)of_id->data;
+
+		comp = devm_kzalloc(dev, sizeof(*comp), GFP_KERNEL);
+		if (!comp) {
+			ret = -ENOMEM;
+			of_node_put(node);
+			goto err_comp;
+		}
+
+		ret = mtk_mdp_comp_init(dev, node, comp, comp_type);
+		if (ret) {
+			of_node_put(node);
+			goto err_comp;
+		}
+
+		mtk_mdp_register_component(mdp, comp);
+	}
+
+	mdp->job_wq = create_singlethread_workqueue(MTK_MDP_MODULE_NAME);
+	if (!mdp->job_wq) {
+		dev_err(&pdev->dev, "unable to alloc job workqueue\n");
+		ret = -ENOMEM;
+		goto err_alloc_job_wq;
+	}
+
+	mdp->wdt_wq = create_singlethread_workqueue("mdp_wdt_wq");
+	if (!mdp->wdt_wq) {
+		dev_err(&pdev->dev, "unable to alloc wdt workqueue\n");
+		ret = -ENOMEM;
+		goto err_alloc_wdt_wq;
+	}
+	INIT_WORK(&mdp->wdt_work, mtk_mdp_wdt_worker);
+
+	ret = v4l2_device_register(dev, &mdp->v4l2_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to register v4l2 device\n");
+		ret = -EINVAL;
+		goto err_dev_register;
+	}
+
+	ret = mtk_mdp_register_m2m_device(mdp);
+	if (ret) {
+		v4l2_err(&mdp->v4l2_dev, "Failed to init mem2mem device\n");
+		goto err_m2m_register;
+	}
+
+	mdp->vpu_dev = vpu_get_plat_device(pdev);
+	ret = vpu_wdt_reg_handler(mdp->vpu_dev, mtk_mdp_reset_handler, mdp,
+				  VPU_RST_MDP);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to register reset handler\n");
+		goto err_m2m_register;
+	}
+
+	platform_set_drvdata(pdev, mdp);
+
+	ret = vb2_dma_contig_set_max_seg_size(&pdev->dev, DMA_BIT_MASK(32));
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to set vb2 dma mag seg size\n");
+		goto err_m2m_register;
+	}
+
+	pm_runtime_enable(dev);
+	dev_dbg(dev, "mdp-%d registered successfully\n", mdp->id);
+
+	return 0;
+
+err_m2m_register:
+	v4l2_device_unregister(&mdp->v4l2_dev);
+
+err_dev_register:
+	destroy_workqueue(mdp->wdt_wq);
+
+err_alloc_wdt_wq:
+	destroy_workqueue(mdp->job_wq);
+
+err_alloc_job_wq:
+
+err_comp:
+	list_for_each_entry_safe(comp, comp_temp, &mdp->comp_list, node) {
+		mtk_mdp_unregister_component(mdp, comp);
+		mtk_mdp_comp_deinit(dev, comp);
+	}
+
+	dev_dbg(dev, "err %d\n", ret);
+	return ret;
+}
+
+static int mtk_mdp_remove(struct platform_device *pdev)
+{
+	struct mtk_mdp_dev *mdp = platform_get_drvdata(pdev);
+	struct mtk_mdp_comp *comp, *comp_temp;
+
+	pm_runtime_disable(&pdev->dev);
+	vb2_dma_contig_clear_max_seg_size(&pdev->dev);
+	mtk_mdp_unregister_m2m_device(mdp);
+	v4l2_device_unregister(&mdp->v4l2_dev);
+
+	destroy_workqueue(mdp->wdt_wq);
+
+	destroy_workqueue(mdp->job_wq);
+
+	list_for_each_entry_safe(comp, comp_temp, &mdp->comp_list, node) {
+		mtk_mdp_unregister_component(mdp, comp);
+		mtk_mdp_comp_deinit(&pdev->dev, comp);
+	}
+
+	dev_dbg(&pdev->dev, "%s driver unloaded\n", pdev->name);
+	return 0;
+}
+
+static int __maybe_unused mtk_mdp_pm_suspend(struct device *dev)
+{
+	struct mtk_mdp_dev *mdp = dev_get_drvdata(dev);
+
+	mtk_mdp_clock_off(mdp);
+
+	return 0;
+}
+
+static int __maybe_unused mtk_mdp_pm_resume(struct device *dev)
+{
+	struct mtk_mdp_dev *mdp = dev_get_drvdata(dev);
+
+	mtk_mdp_clock_on(mdp);
+
+	return 0;
+}
+
+static int __maybe_unused mtk_mdp_suspend(struct device *dev)
+{
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	return mtk_mdp_pm_suspend(dev);
+}
+
+static int __maybe_unused mtk_mdp_resume(struct device *dev)
+{
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	return mtk_mdp_pm_resume(dev);
+}
+
+static const struct dev_pm_ops mtk_mdp_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(mtk_mdp_suspend, mtk_mdp_resume)
+	SET_RUNTIME_PM_OPS(mtk_mdp_pm_suspend, mtk_mdp_pm_resume, NULL)
+};
+
+static struct platform_driver mtk_mdp_driver = {
+	.probe		= mtk_mdp_probe,
+	.remove		= mtk_mdp_remove,
+	.driver = {
+		.name	= MTK_MDP_MODULE_NAME,
+		.pm	= &mtk_mdp_pm_ops,
+		.of_match_table = mtk_mdp_of_ids,
+	}
+};
+
+module_platform_driver(mtk_mdp_driver);
+
+MODULE_AUTHOR("Houlong Wei <houlong.wei@mediatek.com>");
+MODULE_DESCRIPTION("Mediatek image processor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_core.h b/drivers/media/platform/mediatek/mdp/mtk_mdp_core.h
new file mode 100644
index 000000000..a6e6dc363
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_core.h
@@ -0,0 +1,256 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2015-2016 MediaTek Inc.
+ * Author: Houlong Wei <houlong.wei@mediatek.com>
+ *         Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ */
+
+#ifndef __MTK_MDP_CORE_H__
+#define __MTK_MDP_CORE_H__
+
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "mtk_mdp_vpu.h"
+#include "mtk_mdp_comp.h"
+
+
+#define MTK_MDP_MODULE_NAME		"mtk-mdp"
+
+#define MTK_MDP_SHUTDOWN_TIMEOUT	((100*HZ)/1000) /* 100ms */
+#define MTK_MDP_MAX_CTRL_NUM		10
+
+#define MTK_MDP_FMT_FLAG_OUTPUT		BIT(0)
+#define MTK_MDP_FMT_FLAG_CAPTURE	BIT(1)
+
+#define MTK_MDP_VPU_INIT		BIT(0)
+#define MTK_MDP_CTX_ERROR		BIT(5)
+
+/**
+ *  struct mtk_mdp_pix_align - alignment of image
+ *  @org_w: source alignment of width
+ *  @org_h: source alignment of height
+ *  @target_w: dst alignment of width
+ *  @target_h: dst alignment of height
+ */
+struct mtk_mdp_pix_align {
+	u16 org_w;
+	u16 org_h;
+	u16 target_w;
+	u16 target_h;
+};
+
+/**
+ * struct mtk_mdp_fmt - the driver's internal color format data
+ * @pixelformat: the fourcc code for this format, 0 if not applicable
+ * @num_planes: number of physically non-contiguous data planes
+ * @num_comp: number of logical data planes
+ * @depth: per plane driver's private 'number of bits per pixel'
+ * @row_depth: per plane driver's private 'number of bits per pixel per row'
+ * @flags: flags indicating which operation mode format applies to
+ *	   MTK_MDP_FMT_FLAG_OUTPUT is used in OUTPUT stream
+ *	   MTK_MDP_FMT_FLAG_CAPTURE is used in CAPTURE stream
+ * @align: pointer to a pixel alignment struct, NULL if using default value
+ */
+struct mtk_mdp_fmt {
+	u32	pixelformat;
+	u16	num_planes;
+	u16	num_comp;
+	u8	depth[VIDEO_MAX_PLANES];
+	u8	row_depth[VIDEO_MAX_PLANES];
+	u32	flags;
+	struct mtk_mdp_pix_align *align;
+};
+
+/**
+ * struct mtk_mdp_addr - the image processor physical address set
+ * @addr:	address of planes
+ */
+struct mtk_mdp_addr {
+	dma_addr_t addr[MTK_MDP_MAX_NUM_PLANE];
+};
+
+/* struct mtk_mdp_ctrls - the image processor control set
+ * @rotate: rotation degree
+ * @hflip: horizontal flip
+ * @vflip: vertical flip
+ * @global_alpha: the alpha value of current frame
+ */
+struct mtk_mdp_ctrls {
+	struct v4l2_ctrl *rotate;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *global_alpha;
+};
+
+/**
+ * struct mtk_mdp_frame - source/target frame properties
+ * @width:	SRC : SRCIMG_WIDTH, DST : OUTPUTDMA_WHOLE_IMG_WIDTH
+ * @height:	SRC : SRCIMG_HEIGHT, DST : OUTPUTDMA_WHOLE_IMG_HEIGHT
+ * @crop:	cropped(source)/scaled(destination) size
+ * @payload:	image size in bytes (w x h x bpp)
+ * @pitch:	bytes per line of image in memory
+ * @addr:	image frame buffer physical addresses
+ * @fmt:	color format pointer
+ * @alpha:	frame's alpha value
+ */
+struct mtk_mdp_frame {
+	u32				width;
+	u32				height;
+	struct v4l2_rect		crop;
+	unsigned long			payload[VIDEO_MAX_PLANES];
+	unsigned int			pitch[VIDEO_MAX_PLANES];
+	struct mtk_mdp_addr		addr;
+	const struct mtk_mdp_fmt	*fmt;
+	u8				alpha;
+};
+
+/**
+ * struct mtk_mdp_variant - image processor variant information
+ * @pix_max:		maximum limit of image size
+ * @pix_min:		minimum limit of image size
+ * @pix_align:		alignment of image
+ * @h_scale_up_max:	maximum scale-up in horizontal
+ * @v_scale_up_max:	maximum scale-up in vertical
+ * @h_scale_down_max:	maximum scale-down in horizontal
+ * @v_scale_down_max:	maximum scale-down in vertical
+ */
+struct mtk_mdp_variant {
+	struct mtk_mdp_pix_limit	*pix_max;
+	struct mtk_mdp_pix_limit	*pix_min;
+	struct mtk_mdp_pix_align	*pix_align;
+	u16				h_scale_up_max;
+	u16				v_scale_up_max;
+	u16				h_scale_down_max;
+	u16				v_scale_down_max;
+};
+
+/**
+ * struct mtk_mdp_dev - abstraction for image processor entity
+ * @lock:	the mutex protecting this data structure
+ * @vpulock:	the mutex protecting the communication with VPU
+ * @pdev:	pointer to the image processor platform device
+ * @variant:	the IP variant information
+ * @id:		image processor device index (0..MTK_MDP_MAX_DEVS)
+ * @comp_list:	list of MDP function components
+ * @m2m_dev:	v4l2 memory-to-memory device data
+ * @ctx_list:	list of struct mtk_mdp_ctx
+ * @vdev:	video device for image processor driver
+ * @v4l2_dev:	V4L2 device to register video devices for.
+ * @job_wq:	processor work queue
+ * @vpu_dev:	VPU platform device
+ * @ctx_num:	counter of active MTK MDP context
+ * @id_counter:	An integer id given to the next opened context
+ * @wdt_wq:	work queue for VPU watchdog
+ * @wdt_work:	worker for VPU watchdog
+ */
+struct mtk_mdp_dev {
+	struct mutex			lock;
+	struct mutex			vpulock;
+	struct platform_device		*pdev;
+	struct mtk_mdp_variant		*variant;
+	u16				id;
+	struct list_head		comp_list;
+	struct v4l2_m2m_dev		*m2m_dev;
+	struct list_head		ctx_list;
+	struct video_device		*vdev;
+	struct v4l2_device		v4l2_dev;
+	struct workqueue_struct		*job_wq;
+	struct platform_device		*vpu_dev;
+	int				ctx_num;
+	unsigned long			id_counter;
+	struct workqueue_struct		*wdt_wq;
+	struct work_struct		wdt_work;
+};
+
+/**
+ * struct mtk_mdp_ctx - the device context data
+ * @list:		link to ctx_list of mtk_mdp_dev
+ * @s_frame:		source frame properties
+ * @d_frame:		destination frame properties
+ * @id:			index of the context that this structure describes
+ * @flags:		additional flags for image conversion
+ * @state:		flags to keep track of user configuration
+ *			Protected by slock
+ * @rotation:		rotates the image by specified angle
+ * @hflip:		mirror the picture horizontally
+ * @vflip:		mirror the picture vertically
+ * @mdp_dev:		the image processor device this context applies to
+ * @m2m_ctx:		memory-to-memory device context
+ * @fh:			v4l2 file handle
+ * @ctrl_handler:	v4l2 controls handler
+ * @ctrls:		image processor control set
+ * @ctrls_rdy:		true if the control handler is initialized
+ * @colorspace:		enum v4l2_colorspace; supplemental to pixelformat
+ * @ycbcr_enc:		enum v4l2_ycbcr_encoding, Y'CbCr encoding
+ * @xfer_func:		enum v4l2_xfer_func, colorspace transfer function
+ * @quant:		enum v4l2_quantization, colorspace quantization
+ * @vpu:		VPU instance
+ * @slock:		the mutex protecting mtp_mdp_ctx.state
+ * @work:		worker for image processing
+ */
+struct mtk_mdp_ctx {
+	struct list_head		list;
+	struct mtk_mdp_frame		s_frame;
+	struct mtk_mdp_frame		d_frame;
+	u32				flags;
+	u32				state;
+	int				id;
+	int				rotation;
+	u32				hflip:1;
+	u32				vflip:1;
+	struct mtk_mdp_dev		*mdp_dev;
+	struct v4l2_m2m_ctx		*m2m_ctx;
+	struct v4l2_fh			fh;
+	struct v4l2_ctrl_handler	ctrl_handler;
+	struct mtk_mdp_ctrls		ctrls;
+	bool				ctrls_rdy;
+	enum v4l2_colorspace		colorspace;
+	enum v4l2_ycbcr_encoding	ycbcr_enc;
+	enum v4l2_xfer_func		xfer_func;
+	enum v4l2_quantization		quant;
+
+	struct mtk_mdp_vpu		vpu;
+	struct mutex			slock;
+	struct work_struct		work;
+};
+
+extern int mtk_mdp_dbg_level;
+
+void mtk_mdp_register_component(struct mtk_mdp_dev *mdp,
+				struct mtk_mdp_comp *comp);
+
+void mtk_mdp_unregister_component(struct mtk_mdp_dev *mdp,
+				  struct mtk_mdp_comp *comp);
+
+#if defined(DEBUG)
+
+#define mtk_mdp_dbg(level, fmt, args...)				 \
+	do {								 \
+		if (mtk_mdp_dbg_level >= level)				 \
+			pr_info("[MTK_MDP] level=%d %s(),%d: " fmt "\n", \
+				level, __func__, __LINE__, ##args);	 \
+	} while (0)
+
+#define mtk_mdp_err(fmt, args...)					\
+	pr_err("[MTK_MDP][ERROR] %s:%d: " fmt "\n", __func__, __LINE__, \
+	       ##args)
+
+
+#define mtk_mdp_dbg_enter()  mtk_mdp_dbg(3, "+")
+#define mtk_mdp_dbg_leave()  mtk_mdp_dbg(3, "-")
+
+#else
+
+#define mtk_mdp_dbg(level, fmt, args...) {}
+#define mtk_mdp_err(fmt, args...)
+#define mtk_mdp_dbg_enter()
+#define mtk_mdp_dbg_leave()
+
+#endif
+
+#endif /* __MTK_MDP_CORE_H__ */
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_ipi.h b/drivers/media/platform/mediatek/mdp/mtk_mdp_ipi.h
new file mode 100644
index 000000000..b810c9669
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_ipi.h
@@ -0,0 +1,120 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2015-2016 MediaTek Inc.
+ * Author: Houlong Wei <houlong.wei@mediatek.com>
+ *         Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ */
+
+#ifndef __MTK_MDP_IPI_H__
+#define __MTK_MDP_IPI_H__
+
+#define MTK_MDP_MAX_NUM_PLANE		3
+
+enum mdp_ipi_msgid {
+	AP_MDP_INIT		= 0xd000,
+	AP_MDP_DEINIT		= 0xd001,
+	AP_MDP_PROCESS		= 0xd002,
+
+	VPU_MDP_INIT_ACK	= 0xe000,
+	VPU_MDP_DEINIT_ACK	= 0xe001,
+	VPU_MDP_PROCESS_ACK	= 0xe002
+};
+
+#pragma pack(push, 4)
+
+/**
+ * struct mdp_ipi_init - for AP_MDP_INIT
+ * @msg_id   : AP_MDP_INIT
+ * @ipi_id   : IPI_MDP
+ * @ap_inst  : AP mtk_mdp_vpu address
+ */
+struct mdp_ipi_init {
+	uint32_t msg_id;
+	uint32_t ipi_id;
+	uint64_t ap_inst;
+};
+
+/**
+ * struct mdp_ipi_comm - for AP_MDP_PROCESS, AP_MDP_DEINIT
+ * @msg_id        : AP_MDP_PROCESS, AP_MDP_DEINIT
+ * @ipi_id        : IPI_MDP
+ * @ap_inst       : AP mtk_mdp_vpu address
+ * @vpu_inst_addr : VPU MDP instance address
+ * @padding       : Alignment padding
+ */
+struct mdp_ipi_comm {
+	uint32_t msg_id;
+	uint32_t ipi_id;
+	uint64_t ap_inst;
+	uint32_t vpu_inst_addr;
+	uint32_t padding;
+};
+
+/**
+ * struct mdp_ipi_comm_ack - for VPU_MDP_DEINIT_ACK, VPU_MDP_PROCESS_ACK
+ * @msg_id        : VPU_MDP_DEINIT_ACK, VPU_MDP_PROCESS_ACK
+ * @ipi_id        : IPI_MDP
+ * @ap_inst       : AP mtk_mdp_vpu address
+ * @vpu_inst_addr : VPU MDP instance address
+ * @status        : VPU exeuction result
+ */
+struct mdp_ipi_comm_ack {
+	uint32_t msg_id;
+	uint32_t ipi_id;
+	uint64_t ap_inst;
+	uint32_t vpu_inst_addr;
+	int32_t status;
+};
+
+/**
+ * struct mdp_config - configured for source/destination image
+ * @x        : left
+ * @y        : top
+ * @w        : width
+ * @h        : height
+ * @w_stride : bytes in horizontal
+ * @h_stride : bytes in vertical
+ * @crop_x   : cropped left
+ * @crop_y   : cropped top
+ * @crop_w   : cropped width
+ * @crop_h   : cropped height
+ * @format   : color format
+ */
+struct mdp_config {
+	int32_t x;
+	int32_t y;
+	int32_t w;
+	int32_t h;
+	int32_t w_stride;
+	int32_t h_stride;
+	int32_t crop_x;
+	int32_t crop_y;
+	int32_t crop_w;
+	int32_t crop_h;
+	int32_t format;
+};
+
+struct mdp_buffer {
+	uint64_t addr_mva[MTK_MDP_MAX_NUM_PLANE];
+	int32_t plane_size[MTK_MDP_MAX_NUM_PLANE];
+	int32_t plane_num;
+};
+
+struct mdp_config_misc {
+	int32_t orientation; /* 0, 90, 180, 270 */
+	int32_t hflip; /* 1 will enable the flip */
+	int32_t vflip; /* 1 will enable the flip */
+	int32_t alpha; /* global alpha */
+};
+
+struct mdp_process_vsi {
+	struct mdp_config src_config;
+	struct mdp_buffer src_buffer;
+	struct mdp_config dst_config;
+	struct mdp_buffer dst_buffer;
+	struct mdp_config_misc misc;
+};
+
+#pragma pack(pop)
+
+#endif /* __MTK_MDP_IPI_H__ */
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_m2m.c b/drivers/media/platform/mediatek/mdp/mtk_mdp_m2m.c
new file mode 100644
index 000000000..f14779e75
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_m2m.c
@@ -0,0 +1,1229 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2015-2016 MediaTek Inc.
+ * Author: Houlong Wei <houlong.wei@mediatek.com>
+ *         Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ */
+
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+
+#include "mtk_mdp_core.h"
+#include "mtk_mdp_m2m.h"
+#include "mtk_mdp_regs.h"
+#include "mtk_vpu.h"
+
+
+/**
+ *  struct mtk_mdp_pix_limit - image pixel size limits
+ *  @org_w: source pixel width
+ *  @org_h: source pixel height
+ *  @target_rot_dis_w: pixel dst scaled width with the rotator is off
+ *  @target_rot_dis_h: pixel dst scaled height with the rotator is off
+ *  @target_rot_en_w: pixel dst scaled width with the rotator is on
+ *  @target_rot_en_h: pixel dst scaled height with the rotator is on
+ */
+struct mtk_mdp_pix_limit {
+	u16 org_w;
+	u16 org_h;
+	u16 target_rot_dis_w;
+	u16 target_rot_dis_h;
+	u16 target_rot_en_w;
+	u16 target_rot_en_h;
+};
+
+static struct mtk_mdp_pix_align mtk_mdp_size_align = {
+	.org_w			= 16,
+	.org_h			= 16,
+	.target_w		= 2,
+	.target_h		= 2,
+};
+
+static const struct mtk_mdp_fmt mtk_mdp_formats[] = {
+	{
+		.pixelformat	= V4L2_PIX_FMT_MT21C,
+		.depth		= { 8, 4 },
+		.row_depth	= { 8, 8 },
+		.num_planes	= 2,
+		.num_comp	= 2,
+		.align		= &mtk_mdp_size_align,
+		.flags		= MTK_MDP_FMT_FLAG_OUTPUT,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_NV12M,
+		.depth		= { 8, 4 },
+		.row_depth	= { 8, 8 },
+		.num_planes	= 2,
+		.num_comp	= 2,
+		.flags		= MTK_MDP_FMT_FLAG_OUTPUT |
+				  MTK_MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_YUV420M,
+		.depth		= { 8, 2, 2 },
+		.row_depth	= { 8, 4, 4 },
+		.num_planes	= 3,
+		.num_comp	= 3,
+		.flags		= MTK_MDP_FMT_FLAG_OUTPUT |
+				  MTK_MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_YVU420,
+		.depth		= { 12 },
+		.row_depth	= { 8 },
+		.num_planes	= 1,
+		.num_comp	= 3,
+		.flags		= MTK_MDP_FMT_FLAG_OUTPUT |
+				  MTK_MDP_FMT_FLAG_CAPTURE,
+	}
+};
+
+static struct mtk_mdp_pix_limit mtk_mdp_size_max = {
+	.target_rot_dis_w	= 4096,
+	.target_rot_dis_h	= 4096,
+	.target_rot_en_w	= 4096,
+	.target_rot_en_h	= 4096,
+};
+
+static struct mtk_mdp_pix_limit mtk_mdp_size_min = {
+	.org_w			= 16,
+	.org_h			= 16,
+	.target_rot_dis_w	= 16,
+	.target_rot_dis_h	= 16,
+	.target_rot_en_w	= 16,
+	.target_rot_en_h	= 16,
+};
+
+/* align size for normal raster scan pixel format */
+static struct mtk_mdp_pix_align mtk_mdp_rs_align = {
+	.org_w			= 2,
+	.org_h			= 2,
+	.target_w		= 2,
+	.target_h		= 2,
+};
+
+static struct mtk_mdp_variant mtk_mdp_default_variant = {
+	.pix_max		= &mtk_mdp_size_max,
+	.pix_min		= &mtk_mdp_size_min,
+	.pix_align		= &mtk_mdp_rs_align,
+	.h_scale_up_max		= 32,
+	.v_scale_up_max		= 32,
+	.h_scale_down_max	= 32,
+	.v_scale_down_max	= 128,
+};
+
+static const struct mtk_mdp_fmt *mtk_mdp_find_fmt(u32 pixelformat, u32 type)
+{
+	u32 i, flag;
+
+	flag = V4L2_TYPE_IS_OUTPUT(type) ? MTK_MDP_FMT_FLAG_OUTPUT :
+					   MTK_MDP_FMT_FLAG_CAPTURE;
+
+	for (i = 0; i < ARRAY_SIZE(mtk_mdp_formats); ++i) {
+		if (!(mtk_mdp_formats[i].flags & flag))
+			continue;
+		if (mtk_mdp_formats[i].pixelformat == pixelformat)
+			return &mtk_mdp_formats[i];
+	}
+	return NULL;
+}
+
+static const struct mtk_mdp_fmt *mtk_mdp_find_fmt_by_index(u32 index, u32 type)
+{
+	u32 i, flag, num = 0;
+
+	flag = V4L2_TYPE_IS_OUTPUT(type) ? MTK_MDP_FMT_FLAG_OUTPUT :
+					   MTK_MDP_FMT_FLAG_CAPTURE;
+
+	for (i = 0; i < ARRAY_SIZE(mtk_mdp_formats); ++i) {
+		if (!(mtk_mdp_formats[i].flags & flag))
+			continue;
+		if (index == num)
+			return &mtk_mdp_formats[i];
+		num++;
+	}
+	return NULL;
+}
+
+static void mtk_mdp_bound_align_image(u32 *w, unsigned int wmin,
+				      unsigned int wmax, unsigned int align_w,
+				      u32 *h, unsigned int hmin,
+				      unsigned int hmax, unsigned int align_h)
+{
+	int org_w, org_h, step_w, step_h;
+	int walign, halign;
+
+	org_w = *w;
+	org_h = *h;
+	walign = ffs(align_w) - 1;
+	halign = ffs(align_h) - 1;
+	v4l_bound_align_image(w, wmin, wmax, walign, h, hmin, hmax, halign, 0);
+
+	step_w = 1 << walign;
+	step_h = 1 << halign;
+	if (*w < org_w && (*w + step_w) <= wmax)
+		*w += step_w;
+	if (*h < org_h && (*h + step_h) <= hmax)
+		*h += step_h;
+}
+
+static const struct mtk_mdp_fmt *mtk_mdp_try_fmt_mplane(struct mtk_mdp_ctx *ctx,
+							struct v4l2_format *f)
+{
+	struct mtk_mdp_dev *mdp = ctx->mdp_dev;
+	struct mtk_mdp_variant *variant = mdp->variant;
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	const struct mtk_mdp_fmt *fmt;
+	u32 max_w, max_h, align_w, align_h;
+	u32 min_w, min_h, org_w, org_h;
+	int i;
+
+	fmt = mtk_mdp_find_fmt(pix_mp->pixelformat, f->type);
+	if (!fmt)
+		fmt = mtk_mdp_find_fmt_by_index(0, f->type);
+	if (!fmt) {
+		dev_dbg(&ctx->mdp_dev->pdev->dev,
+			"pixelformat format 0x%X invalid\n",
+			pix_mp->pixelformat);
+		return NULL;
+	}
+
+	pix_mp->field = V4L2_FIELD_NONE;
+	pix_mp->pixelformat = fmt->pixelformat;
+	if (V4L2_TYPE_IS_CAPTURE(f->type)) {
+		pix_mp->colorspace = ctx->colorspace;
+		pix_mp->xfer_func = ctx->xfer_func;
+		pix_mp->ycbcr_enc = ctx->ycbcr_enc;
+		pix_mp->quantization = ctx->quant;
+	}
+
+	max_w = variant->pix_max->target_rot_dis_w;
+	max_h = variant->pix_max->target_rot_dis_h;
+
+	if (fmt->align == NULL) {
+		/* use default alignment */
+		align_w = variant->pix_align->org_w;
+		align_h = variant->pix_align->org_h;
+	} else {
+		align_w = fmt->align->org_w;
+		align_h = fmt->align->org_h;
+	}
+
+	if (V4L2_TYPE_IS_OUTPUT(f->type)) {
+		min_w = variant->pix_min->org_w;
+		min_h = variant->pix_min->org_h;
+	} else {
+		min_w = variant->pix_min->target_rot_dis_w;
+		min_h = variant->pix_min->target_rot_dis_h;
+	}
+
+	mtk_mdp_dbg(2, "[%d] type:%d, wxh:%ux%u, align:%ux%u, max:%ux%u",
+		    ctx->id, f->type, pix_mp->width, pix_mp->height,
+		    align_w, align_h, max_w, max_h);
+	/*
+	 * To check if image size is modified to adjust parameter against
+	 * hardware abilities
+	 */
+	org_w = pix_mp->width;
+	org_h = pix_mp->height;
+
+	mtk_mdp_bound_align_image(&pix_mp->width, min_w, max_w, align_w,
+				  &pix_mp->height, min_h, max_h, align_h);
+
+	if (org_w != pix_mp->width || org_h != pix_mp->height)
+		mtk_mdp_dbg(1, "[%d] size change:%ux%u to %ux%u", ctx->id,
+			    org_w, org_h, pix_mp->width, pix_mp->height);
+	pix_mp->num_planes = fmt->num_planes;
+
+	for (i = 0; i < pix_mp->num_planes; ++i) {
+		int bpl = (pix_mp->width * fmt->row_depth[i]) / 8;
+		int sizeimage = (pix_mp->width * pix_mp->height *
+			fmt->depth[i]) / 8;
+
+		pix_mp->plane_fmt[i].bytesperline = bpl;
+		if (pix_mp->plane_fmt[i].sizeimage < sizeimage)
+			pix_mp->plane_fmt[i].sizeimage = sizeimage;
+		mtk_mdp_dbg(2, "[%d] p%d, bpl:%d, sizeimage:%u (%u)", ctx->id,
+			    i, bpl, pix_mp->plane_fmt[i].sizeimage, sizeimage);
+	}
+
+	return fmt;
+}
+
+static struct mtk_mdp_frame *mtk_mdp_ctx_get_frame(struct mtk_mdp_ctx *ctx,
+					    enum v4l2_buf_type type)
+{
+	if (V4L2_TYPE_IS_OUTPUT(type))
+		return &ctx->s_frame;
+	return &ctx->d_frame;
+}
+
+static void mtk_mdp_check_crop_change(u32 new_w, u32 new_h, u32 *w, u32 *h)
+{
+	if (new_w != *w || new_h != *h) {
+		mtk_mdp_dbg(1, "size change:%dx%d to %dx%d",
+			    *w, *h, new_w, new_h);
+
+		*w = new_w;
+		*h = new_h;
+	}
+}
+
+static int mtk_mdp_try_crop(struct mtk_mdp_ctx *ctx, u32 type,
+			    struct v4l2_rect *r)
+{
+	struct mtk_mdp_frame *frame;
+	struct mtk_mdp_dev *mdp = ctx->mdp_dev;
+	struct mtk_mdp_variant *variant = mdp->variant;
+	u32 align_w, align_h, new_w, new_h;
+	u32 min_w, min_h, max_w, max_h;
+
+	if (r->top < 0 || r->left < 0) {
+		dev_err(&ctx->mdp_dev->pdev->dev,
+			"doesn't support negative values for top & left\n");
+		return -EINVAL;
+	}
+
+	mtk_mdp_dbg(2, "[%d] type:%d, set wxh:%dx%d", ctx->id, type,
+		    r->width, r->height);
+
+	frame = mtk_mdp_ctx_get_frame(ctx, type);
+	max_w = frame->width;
+	max_h = frame->height;
+	new_w = r->width;
+	new_h = r->height;
+
+	if (V4L2_TYPE_IS_OUTPUT(type)) {
+		align_w = 1;
+		align_h = 1;
+		min_w = 64;
+		min_h = 32;
+	} else {
+		align_w = variant->pix_align->target_w;
+		align_h = variant->pix_align->target_h;
+		if (ctx->ctrls.rotate->val == 90 ||
+		    ctx->ctrls.rotate->val == 270) {
+			max_w = frame->height;
+			max_h = frame->width;
+			min_w = variant->pix_min->target_rot_en_w;
+			min_h = variant->pix_min->target_rot_en_h;
+			new_w = r->height;
+			new_h = r->width;
+		} else {
+			min_w = variant->pix_min->target_rot_dis_w;
+			min_h = variant->pix_min->target_rot_dis_h;
+		}
+	}
+
+	mtk_mdp_dbg(2, "[%d] align:%dx%d, min:%dx%d, new:%dx%d", ctx->id,
+		    align_w, align_h, min_w, min_h, new_w, new_h);
+
+	mtk_mdp_bound_align_image(&new_w, min_w, max_w, align_w,
+				  &new_h, min_h, max_h, align_h);
+
+	if (V4L2_TYPE_IS_CAPTURE(type) &&
+	    (ctx->ctrls.rotate->val == 90 || ctx->ctrls.rotate->val == 270))
+		mtk_mdp_check_crop_change(new_h, new_w,
+					  &r->width, &r->height);
+	else
+		mtk_mdp_check_crop_change(new_w, new_h,
+					  &r->width, &r->height);
+
+	/* adjust left/top if cropping rectangle is out of bounds */
+	/* Need to add code to algin left value with 2's multiple */
+	if (r->left + new_w > max_w)
+		r->left = max_w - new_w;
+	if (r->top + new_h > max_h)
+		r->top = max_h - new_h;
+
+	if (r->left & 1)
+		r->left -= 1;
+
+	mtk_mdp_dbg(2, "[%d] crop l,t,w,h:%d,%d,%d,%d, max:%dx%d", ctx->id,
+		    r->left, r->top, r->width,
+		    r->height, max_w, max_h);
+	return 0;
+}
+
+static inline struct mtk_mdp_ctx *fh_to_ctx(struct v4l2_fh *fh)
+{
+	return container_of(fh, struct mtk_mdp_ctx, fh);
+}
+
+static inline struct mtk_mdp_ctx *ctrl_to_ctx(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct mtk_mdp_ctx, ctrl_handler);
+}
+
+void mtk_mdp_ctx_state_lock_set(struct mtk_mdp_ctx *ctx, u32 state)
+{
+	mutex_lock(&ctx->slock);
+	ctx->state |= state;
+	mutex_unlock(&ctx->slock);
+}
+
+static bool mtk_mdp_ctx_state_is_set(struct mtk_mdp_ctx *ctx, u32 mask)
+{
+	bool ret;
+
+	mutex_lock(&ctx->slock);
+	ret = (ctx->state & mask) == mask;
+	mutex_unlock(&ctx->slock);
+	return ret;
+}
+
+static void mtk_mdp_set_frame_size(struct mtk_mdp_frame *frame, int width,
+				   int height)
+{
+	frame->width = width;
+	frame->height = height;
+	frame->crop.width = width;
+	frame->crop.height = height;
+	frame->crop.left = 0;
+	frame->crop.top = 0;
+}
+
+static int mtk_mdp_m2m_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct mtk_mdp_ctx *ctx = q->drv_priv;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(&ctx->mdp_dev->pdev->dev);
+	if (ret < 0)
+		mtk_mdp_dbg(1, "[%d] pm_runtime_resume_and_get failed:%d",
+			    ctx->id, ret);
+
+	return ret;
+}
+
+static void *mtk_mdp_m2m_buf_remove(struct mtk_mdp_ctx *ctx,
+				    enum v4l2_buf_type type)
+{
+	if (V4L2_TYPE_IS_OUTPUT(type))
+		return v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+	else
+		return v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+}
+
+static void mtk_mdp_m2m_stop_streaming(struct vb2_queue *q)
+{
+	struct mtk_mdp_ctx *ctx = q->drv_priv;
+	struct vb2_buffer *vb;
+
+	vb = mtk_mdp_m2m_buf_remove(ctx, q->type);
+	while (vb != NULL) {
+		v4l2_m2m_buf_done(to_vb2_v4l2_buffer(vb), VB2_BUF_STATE_ERROR);
+		vb = mtk_mdp_m2m_buf_remove(ctx, q->type);
+	}
+
+	pm_runtime_put(&ctx->mdp_dev->pdev->dev);
+}
+
+/* The color format (num_planes) must be already configured. */
+static void mtk_mdp_prepare_addr(struct mtk_mdp_ctx *ctx,
+				 struct vb2_buffer *vb,
+				 struct mtk_mdp_frame *frame,
+				 struct mtk_mdp_addr *addr)
+{
+	u32 pix_size, planes, i;
+
+	pix_size = frame->width * frame->height;
+	planes = min_t(u32, frame->fmt->num_planes, ARRAY_SIZE(addr->addr));
+	for (i = 0; i < planes; i++)
+		addr->addr[i] = vb2_dma_contig_plane_dma_addr(vb, i);
+
+	if (planes == 1) {
+		if (frame->fmt->pixelformat == V4L2_PIX_FMT_YVU420) {
+			addr->addr[1] = (dma_addr_t)(addr->addr[0] + pix_size);
+			addr->addr[2] = (dma_addr_t)(addr->addr[1] +
+					(pix_size >> 2));
+		} else {
+			dev_err(&ctx->mdp_dev->pdev->dev,
+				"Invalid pixelformat:0x%x\n",
+				frame->fmt->pixelformat);
+		}
+	}
+	mtk_mdp_dbg(3, "[%d] planes:%d, size:%d, addr:%p,%p,%p",
+		    ctx->id, planes, pix_size, (void *)addr->addr[0],
+		    (void *)addr->addr[1], (void *)addr->addr[2]);
+}
+
+static void mtk_mdp_m2m_get_bufs(struct mtk_mdp_ctx *ctx)
+{
+	struct mtk_mdp_frame *s_frame, *d_frame;
+	struct vb2_v4l2_buffer *src_vbuf, *dst_vbuf;
+
+	s_frame = &ctx->s_frame;
+	d_frame = &ctx->d_frame;
+
+	src_vbuf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	mtk_mdp_prepare_addr(ctx, &src_vbuf->vb2_buf, s_frame, &s_frame->addr);
+
+	dst_vbuf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	mtk_mdp_prepare_addr(ctx, &dst_vbuf->vb2_buf, d_frame, &d_frame->addr);
+
+	dst_vbuf->vb2_buf.timestamp = src_vbuf->vb2_buf.timestamp;
+}
+
+static void mtk_mdp_process_done(void *priv, int vb_state)
+{
+	struct mtk_mdp_dev *mdp = priv;
+	struct mtk_mdp_ctx *ctx;
+	struct vb2_v4l2_buffer *src_vbuf, *dst_vbuf;
+
+	ctx = v4l2_m2m_get_curr_priv(mdp->m2m_dev);
+	if (!ctx)
+		return;
+
+	src_vbuf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+	dst_vbuf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+
+	dst_vbuf->vb2_buf.timestamp = src_vbuf->vb2_buf.timestamp;
+	dst_vbuf->timecode = src_vbuf->timecode;
+	dst_vbuf->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	dst_vbuf->flags |= src_vbuf->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+
+	v4l2_m2m_buf_done(src_vbuf, vb_state);
+	v4l2_m2m_buf_done(dst_vbuf, vb_state);
+	v4l2_m2m_job_finish(ctx->mdp_dev->m2m_dev, ctx->m2m_ctx);
+}
+
+static void mtk_mdp_m2m_worker(struct work_struct *work)
+{
+	struct mtk_mdp_ctx *ctx =
+				container_of(work, struct mtk_mdp_ctx, work);
+	struct mtk_mdp_dev *mdp = ctx->mdp_dev;
+	enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
+	int ret;
+
+	if (mtk_mdp_ctx_state_is_set(ctx, MTK_MDP_CTX_ERROR)) {
+		dev_err(&mdp->pdev->dev, "ctx is in error state");
+		goto worker_end;
+	}
+
+	mtk_mdp_m2m_get_bufs(ctx);
+
+	mtk_mdp_hw_set_input_addr(ctx, &ctx->s_frame.addr);
+	mtk_mdp_hw_set_output_addr(ctx, &ctx->d_frame.addr);
+
+	mtk_mdp_hw_set_in_size(ctx);
+	mtk_mdp_hw_set_in_image_format(ctx);
+
+	mtk_mdp_hw_set_out_size(ctx);
+	mtk_mdp_hw_set_out_image_format(ctx);
+
+	mtk_mdp_hw_set_rotation(ctx);
+	mtk_mdp_hw_set_global_alpha(ctx);
+
+	ret = mtk_mdp_vpu_process(&ctx->vpu);
+	if (ret) {
+		dev_err(&mdp->pdev->dev, "processing failed: %d", ret);
+		goto worker_end;
+	}
+
+	buf_state = VB2_BUF_STATE_DONE;
+
+worker_end:
+	mtk_mdp_process_done(mdp, buf_state);
+}
+
+static void mtk_mdp_m2m_device_run(void *priv)
+{
+	struct mtk_mdp_ctx *ctx = priv;
+
+	queue_work(ctx->mdp_dev->job_wq, &ctx->work);
+}
+
+static int mtk_mdp_m2m_queue_setup(struct vb2_queue *vq,
+			unsigned int *num_buffers, unsigned int *num_planes,
+			unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct mtk_mdp_ctx *ctx = vb2_get_drv_priv(vq);
+	struct mtk_mdp_frame *frame;
+	int i;
+
+	frame = mtk_mdp_ctx_get_frame(ctx, vq->type);
+	*num_planes = frame->fmt->num_planes;
+	for (i = 0; i < frame->fmt->num_planes; i++)
+		sizes[i] = frame->payload[i];
+	mtk_mdp_dbg(2, "[%d] type:%d, planes:%d, buffers:%d, size:%u,%u",
+		    ctx->id, vq->type, *num_planes, *num_buffers,
+		    sizes[0], sizes[1]);
+	return 0;
+}
+
+static int mtk_mdp_m2m_buf_prepare(struct vb2_buffer *vb)
+{
+	struct mtk_mdp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct mtk_mdp_frame *frame;
+	int i;
+
+	frame = mtk_mdp_ctx_get_frame(ctx, vb->vb2_queue->type);
+
+	if (!V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
+		for (i = 0; i < frame->fmt->num_planes; i++)
+			vb2_set_plane_payload(vb, i, frame->payload[i]);
+	}
+
+	return 0;
+}
+
+static void mtk_mdp_m2m_buf_queue(struct vb2_buffer *vb)
+{
+	struct mtk_mdp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	v4l2_m2m_buf_queue(ctx->m2m_ctx, to_vb2_v4l2_buffer(vb));
+}
+
+static const struct vb2_ops mtk_mdp_m2m_qops = {
+	.queue_setup	 = mtk_mdp_m2m_queue_setup,
+	.buf_prepare	 = mtk_mdp_m2m_buf_prepare,
+	.buf_queue	 = mtk_mdp_m2m_buf_queue,
+	.stop_streaming	 = mtk_mdp_m2m_stop_streaming,
+	.start_streaming = mtk_mdp_m2m_start_streaming,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
+};
+
+static int mtk_mdp_m2m_querycap(struct file *file, void *fh,
+				struct v4l2_capability *cap)
+{
+	struct mtk_mdp_ctx *ctx = fh_to_ctx(fh);
+	struct mtk_mdp_dev *mdp = ctx->mdp_dev;
+
+	strscpy(cap->driver, MTK_MDP_MODULE_NAME, sizeof(cap->driver));
+	strscpy(cap->card, mdp->pdev->name, sizeof(cap->card));
+	strscpy(cap->bus_info, "platform:mt8173", sizeof(cap->bus_info));
+
+	return 0;
+}
+
+static int mtk_mdp_enum_fmt(struct v4l2_fmtdesc *f, u32 type)
+{
+	const struct mtk_mdp_fmt *fmt;
+
+	fmt = mtk_mdp_find_fmt_by_index(f->index, type);
+	if (!fmt)
+		return -EINVAL;
+
+	f->pixelformat = fmt->pixelformat;
+
+	return 0;
+}
+
+static int mtk_mdp_m2m_enum_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_fmtdesc *f)
+{
+	return mtk_mdp_enum_fmt(f, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+}
+
+static int mtk_mdp_m2m_enum_fmt_vid_out(struct file *file, void *priv,
+					struct v4l2_fmtdesc *f)
+{
+	return mtk_mdp_enum_fmt(f, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+}
+
+static int mtk_mdp_m2m_g_fmt_mplane(struct file *file, void *fh,
+				    struct v4l2_format *f)
+{
+	struct mtk_mdp_ctx *ctx = fh_to_ctx(fh);
+	struct mtk_mdp_frame *frame;
+	struct v4l2_pix_format_mplane *pix_mp;
+	int i;
+
+	mtk_mdp_dbg(2, "[%d] type:%d", ctx->id, f->type);
+
+	frame = mtk_mdp_ctx_get_frame(ctx, f->type);
+	pix_mp = &f->fmt.pix_mp;
+
+	pix_mp->width = frame->width;
+	pix_mp->height = frame->height;
+	pix_mp->field = V4L2_FIELD_NONE;
+	pix_mp->pixelformat = frame->fmt->pixelformat;
+	pix_mp->num_planes = frame->fmt->num_planes;
+	pix_mp->colorspace = ctx->colorspace;
+	pix_mp->xfer_func = ctx->xfer_func;
+	pix_mp->ycbcr_enc = ctx->ycbcr_enc;
+	pix_mp->quantization = ctx->quant;
+	mtk_mdp_dbg(2, "[%d] wxh:%dx%d", ctx->id,
+		    pix_mp->width, pix_mp->height);
+
+	for (i = 0; i < pix_mp->num_planes; ++i) {
+		pix_mp->plane_fmt[i].bytesperline = (frame->width *
+			frame->fmt->row_depth[i]) / 8;
+		pix_mp->plane_fmt[i].sizeimage = (frame->width *
+			frame->height * frame->fmt->depth[i]) / 8;
+
+		mtk_mdp_dbg(2, "[%d] p%d, bpl:%d, sizeimage:%d", ctx->id, i,
+			    pix_mp->plane_fmt[i].bytesperline,
+			    pix_mp->plane_fmt[i].sizeimage);
+	}
+
+	return 0;
+}
+
+static int mtk_mdp_m2m_try_fmt_mplane(struct file *file, void *fh,
+				      struct v4l2_format *f)
+{
+	struct mtk_mdp_ctx *ctx = fh_to_ctx(fh);
+
+	if (!mtk_mdp_try_fmt_mplane(ctx, f))
+		return -EINVAL;
+	return 0;
+}
+
+static int mtk_mdp_m2m_s_fmt_mplane(struct file *file, void *fh,
+				    struct v4l2_format *f)
+{
+	struct mtk_mdp_ctx *ctx = fh_to_ctx(fh);
+	struct vb2_queue *vq;
+	struct mtk_mdp_frame *frame;
+	struct v4l2_pix_format_mplane *pix_mp;
+	const struct mtk_mdp_fmt *fmt;
+	int i;
+
+	mtk_mdp_dbg(2, "[%d] type:%d", ctx->id, f->type);
+
+	frame = mtk_mdp_ctx_get_frame(ctx, f->type);
+	fmt = mtk_mdp_try_fmt_mplane(ctx, f);
+	if (!fmt) {
+		mtk_mdp_err("[%d] try_fmt failed, type:%d", ctx->id, f->type);
+		return -EINVAL;
+	}
+	frame->fmt = fmt;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	if (vb2_is_streaming(vq)) {
+		dev_info(&ctx->mdp_dev->pdev->dev, "queue %d busy", f->type);
+		return -EBUSY;
+	}
+
+	pix_mp = &f->fmt.pix_mp;
+	for (i = 0; i < frame->fmt->num_planes; i++) {
+		frame->payload[i] = pix_mp->plane_fmt[i].sizeimage;
+		frame->pitch[i] = pix_mp->plane_fmt[i].bytesperline;
+	}
+
+	mtk_mdp_set_frame_size(frame, pix_mp->width, pix_mp->height);
+	if (V4L2_TYPE_IS_OUTPUT(f->type)) {
+		ctx->colorspace = pix_mp->colorspace;
+		ctx->xfer_func = pix_mp->xfer_func;
+		ctx->ycbcr_enc = pix_mp->ycbcr_enc;
+		ctx->quant = pix_mp->quantization;
+	}
+
+	mtk_mdp_dbg(2, "[%d] type:%d, frame:%dx%d", ctx->id, f->type,
+		    frame->width, frame->height);
+
+	return 0;
+}
+
+static int mtk_mdp_m2m_reqbufs(struct file *file, void *fh,
+			       struct v4l2_requestbuffers *reqbufs)
+{
+	struct mtk_mdp_ctx *ctx = fh_to_ctx(fh);
+
+	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
+}
+
+static int mtk_mdp_m2m_streamon(struct file *file, void *fh,
+				enum v4l2_buf_type type)
+{
+	struct mtk_mdp_ctx *ctx = fh_to_ctx(fh);
+	int ret;
+
+	if (!mtk_mdp_ctx_state_is_set(ctx, MTK_MDP_VPU_INIT)) {
+		ret = mtk_mdp_vpu_init(&ctx->vpu);
+		if (ret < 0) {
+			dev_err(&ctx->mdp_dev->pdev->dev,
+				"vpu init failed %d\n",
+				ret);
+			return -EINVAL;
+		}
+		mtk_mdp_ctx_state_lock_set(ctx, MTK_MDP_VPU_INIT);
+	}
+
+	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
+}
+
+static inline bool mtk_mdp_is_target_compose(u32 target)
+{
+	if (target == V4L2_SEL_TGT_COMPOSE_DEFAULT
+	    || target == V4L2_SEL_TGT_COMPOSE_BOUNDS
+	    || target == V4L2_SEL_TGT_COMPOSE)
+		return true;
+	return false;
+}
+
+static inline bool mtk_mdp_is_target_crop(u32 target)
+{
+	if (target == V4L2_SEL_TGT_CROP_DEFAULT
+	    || target == V4L2_SEL_TGT_CROP_BOUNDS
+	    || target == V4L2_SEL_TGT_CROP)
+		return true;
+	return false;
+}
+
+static int mtk_mdp_m2m_g_selection(struct file *file, void *fh,
+				       struct v4l2_selection *s)
+{
+	struct mtk_mdp_frame *frame;
+	struct mtk_mdp_ctx *ctx = fh_to_ctx(fh);
+	bool valid = false;
+
+	if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		if (mtk_mdp_is_target_compose(s->target))
+			valid = true;
+	} else if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		if (mtk_mdp_is_target_crop(s->target))
+			valid = true;
+	}
+	if (!valid) {
+		mtk_mdp_dbg(1, "[%d] invalid type:%d,%u", ctx->id, s->type,
+			    s->target);
+		return -EINVAL;
+	}
+
+	frame = mtk_mdp_ctx_get_frame(ctx, s->type);
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = frame->width;
+		s->r.height = frame->height;
+		return 0;
+
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_CROP:
+		s->r.left = frame->crop.left;
+		s->r.top = frame->crop.top;
+		s->r.width = frame->crop.width;
+		s->r.height = frame->crop.height;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int mtk_mdp_check_scaler_ratio(struct mtk_mdp_variant *var, int src_w,
+				      int src_h, int dst_w, int dst_h, int rot)
+{
+	int tmp_w, tmp_h;
+
+	if (rot == 90 || rot == 270) {
+		tmp_w = dst_h;
+		tmp_h = dst_w;
+	} else {
+		tmp_w = dst_w;
+		tmp_h = dst_h;
+	}
+
+	if ((src_w / tmp_w) > var->h_scale_down_max ||
+	    (src_h / tmp_h) > var->v_scale_down_max ||
+	    (tmp_w / src_w) > var->h_scale_up_max ||
+	    (tmp_h / src_h) > var->v_scale_up_max)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int mtk_mdp_m2m_s_selection(struct file *file, void *fh,
+				   struct v4l2_selection *s)
+{
+	struct mtk_mdp_frame *frame;
+	struct mtk_mdp_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_rect new_r;
+	struct mtk_mdp_variant *variant = ctx->mdp_dev->variant;
+	int ret;
+	bool valid = false;
+
+	if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		if (s->target == V4L2_SEL_TGT_COMPOSE)
+			valid = true;
+	} else if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		if (s->target == V4L2_SEL_TGT_CROP)
+			valid = true;
+	}
+	if (!valid) {
+		mtk_mdp_dbg(1, "[%d] invalid type:%d,%u", ctx->id, s->type,
+			    s->target);
+		return -EINVAL;
+	}
+
+	new_r = s->r;
+	ret = mtk_mdp_try_crop(ctx, s->type, &new_r);
+	if (ret)
+		return ret;
+
+	if (mtk_mdp_is_target_crop(s->target))
+		frame = &ctx->s_frame;
+	else
+		frame = &ctx->d_frame;
+
+	/* Check to see if scaling ratio is within supported range */
+	if (V4L2_TYPE_IS_OUTPUT(s->type))
+		ret = mtk_mdp_check_scaler_ratio(variant, new_r.width,
+			new_r.height, ctx->d_frame.crop.width,
+			ctx->d_frame.crop.height,
+			ctx->ctrls.rotate->val);
+	else
+		ret = mtk_mdp_check_scaler_ratio(variant,
+			ctx->s_frame.crop.width,
+			ctx->s_frame.crop.height, new_r.width,
+			new_r.height, ctx->ctrls.rotate->val);
+
+	if (ret) {
+		dev_info(&ctx->mdp_dev->pdev->dev,
+			"Out of scaler range");
+		return -EINVAL;
+	}
+
+	s->r = new_r;
+	frame->crop = new_r;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops mtk_mdp_m2m_ioctl_ops = {
+	.vidioc_querycap		= mtk_mdp_m2m_querycap,
+	.vidioc_enum_fmt_vid_cap	= mtk_mdp_m2m_enum_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_out	= mtk_mdp_m2m_enum_fmt_vid_out,
+	.vidioc_g_fmt_vid_cap_mplane	= mtk_mdp_m2m_g_fmt_mplane,
+	.vidioc_g_fmt_vid_out_mplane	= mtk_mdp_m2m_g_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= mtk_mdp_m2m_try_fmt_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= mtk_mdp_m2m_try_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= mtk_mdp_m2m_s_fmt_mplane,
+	.vidioc_s_fmt_vid_out_mplane	= mtk_mdp_m2m_s_fmt_mplane,
+	.vidioc_reqbufs			= mtk_mdp_m2m_reqbufs,
+	.vidioc_create_bufs		= v4l2_m2m_ioctl_create_bufs,
+	.vidioc_expbuf			= v4l2_m2m_ioctl_expbuf,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
+	.vidioc_streamon		= mtk_mdp_m2m_streamon,
+	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
+	.vidioc_g_selection		= mtk_mdp_m2m_g_selection,
+	.vidioc_s_selection		= mtk_mdp_m2m_s_selection
+};
+
+static int mtk_mdp_m2m_queue_init(void *priv, struct vb2_queue *src_vq,
+				  struct vb2_queue *dst_vq)
+{
+	struct mtk_mdp_ctx *ctx = priv;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->ops = &mtk_mdp_m2m_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->dev = &ctx->mdp_dev->pdev->dev;
+	src_vq->lock = &ctx->mdp_dev->lock;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->ops = &mtk_mdp_m2m_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->dev = &ctx->mdp_dev->pdev->dev;
+	dst_vq->lock = &ctx->mdp_dev->lock;
+
+	return vb2_queue_init(dst_vq);
+}
+
+static int mtk_mdp_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mtk_mdp_ctx *ctx = ctrl_to_ctx(ctrl);
+	struct mtk_mdp_dev *mdp = ctx->mdp_dev;
+	struct mtk_mdp_variant *variant = mdp->variant;
+	int ret = 0;
+
+	if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		ctx->hflip = ctrl->val;
+		break;
+	case V4L2_CID_VFLIP:
+		ctx->vflip = ctrl->val;
+		break;
+	case V4L2_CID_ROTATE:
+		ret = mtk_mdp_check_scaler_ratio(variant,
+				ctx->s_frame.crop.width,
+				ctx->s_frame.crop.height,
+				ctx->d_frame.crop.width,
+				ctx->d_frame.crop.height,
+				ctx->ctrls.rotate->val);
+
+		if (ret)
+			return -EINVAL;
+
+		ctx->rotation = ctrl->val;
+		break;
+	case V4L2_CID_ALPHA_COMPONENT:
+		ctx->d_frame.alpha = ctrl->val;
+		break;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops mtk_mdp_ctrl_ops = {
+	.s_ctrl = mtk_mdp_s_ctrl,
+};
+
+static int mtk_mdp_ctrls_create(struct mtk_mdp_ctx *ctx)
+{
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, MTK_MDP_MAX_CTRL_NUM);
+
+	ctx->ctrls.rotate = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+			&mtk_mdp_ctrl_ops, V4L2_CID_ROTATE, 0, 270, 90, 0);
+	ctx->ctrls.hflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+					     &mtk_mdp_ctrl_ops,
+					     V4L2_CID_HFLIP,
+					     0, 1, 1, 0);
+	ctx->ctrls.vflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+					     &mtk_mdp_ctrl_ops,
+					     V4L2_CID_VFLIP,
+					     0, 1, 1, 0);
+	ctx->ctrls.global_alpha = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+						    &mtk_mdp_ctrl_ops,
+						    V4L2_CID_ALPHA_COMPONENT,
+						    0, 255, 1, 0);
+	ctx->ctrls_rdy = ctx->ctrl_handler.error == 0;
+
+	if (ctx->ctrl_handler.error) {
+		int err = ctx->ctrl_handler.error;
+
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+		dev_err(&ctx->mdp_dev->pdev->dev,
+			"Failed to create control handlers\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static void mtk_mdp_set_default_params(struct mtk_mdp_ctx *ctx)
+{
+	struct mtk_mdp_dev *mdp = ctx->mdp_dev;
+	struct mtk_mdp_frame *frame;
+
+	frame = mtk_mdp_ctx_get_frame(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	frame->fmt = mtk_mdp_find_fmt_by_index(0,
+					V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	frame->width = mdp->variant->pix_min->org_w;
+	frame->height = mdp->variant->pix_min->org_h;
+	frame->payload[0] = frame->width * frame->height;
+	frame->payload[1] = frame->payload[0] / 2;
+
+	frame = mtk_mdp_ctx_get_frame(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	frame->fmt = mtk_mdp_find_fmt_by_index(0,
+					V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	frame->width = mdp->variant->pix_min->target_rot_dis_w;
+	frame->height = mdp->variant->pix_min->target_rot_dis_h;
+	frame->payload[0] = frame->width * frame->height;
+	frame->payload[1] = frame->payload[0] / 2;
+
+}
+
+static int mtk_mdp_m2m_open(struct file *file)
+{
+	struct mtk_mdp_dev *mdp = video_drvdata(file);
+	struct video_device *vfd = video_devdata(file);
+	struct mtk_mdp_ctx *ctx = NULL;
+	int ret;
+	struct v4l2_format default_format;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	if (mutex_lock_interruptible(&mdp->lock)) {
+		ret = -ERESTARTSYS;
+		goto err_lock;
+	}
+
+	mutex_init(&ctx->slock);
+	ctx->id = mdp->id_counter++;
+	v4l2_fh_init(&ctx->fh, vfd);
+	file->private_data = &ctx->fh;
+	ret = mtk_mdp_ctrls_create(ctx);
+	if (ret)
+		goto error_ctrls;
+
+	/* Use separate control handler per file handle */
+	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+	v4l2_fh_add(&ctx->fh);
+	INIT_LIST_HEAD(&ctx->list);
+
+	ctx->mdp_dev = mdp;
+	mtk_mdp_set_default_params(ctx);
+
+	INIT_WORK(&ctx->work, mtk_mdp_m2m_worker);
+	ctx->m2m_ctx = v4l2_m2m_ctx_init(mdp->m2m_dev, ctx,
+					 mtk_mdp_m2m_queue_init);
+	if (IS_ERR(ctx->m2m_ctx)) {
+		dev_err(&mdp->pdev->dev, "Failed to initialize m2m context");
+		ret = PTR_ERR(ctx->m2m_ctx);
+		goto error_m2m_ctx;
+	}
+	ctx->fh.m2m_ctx = ctx->m2m_ctx;
+	if (mdp->ctx_num++ == 0) {
+		ret = vpu_load_firmware(mdp->vpu_dev);
+		if (ret < 0) {
+			dev_err(&mdp->pdev->dev,
+				"vpu_load_firmware failed %d\n", ret);
+			goto err_load_vpu;
+		}
+
+		ret = mtk_mdp_vpu_register(mdp->pdev);
+		if (ret < 0) {
+			dev_err(&mdp->pdev->dev,
+				"mdp_vpu register failed %d\n", ret);
+			goto err_load_vpu;
+		}
+	}
+
+	list_add(&ctx->list, &mdp->ctx_list);
+	mutex_unlock(&mdp->lock);
+
+	/* Default format */
+	memset(&default_format, 0, sizeof(default_format));
+	default_format.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	default_format.fmt.pix_mp.width = 32;
+	default_format.fmt.pix_mp.height = 32;
+	default_format.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_YUV420M;
+	mtk_mdp_m2m_s_fmt_mplane(file, &ctx->fh, &default_format);
+	default_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	mtk_mdp_m2m_s_fmt_mplane(file, &ctx->fh, &default_format);
+
+	mtk_mdp_dbg(0, "%s [%d]", dev_name(&mdp->pdev->dev), ctx->id);
+
+	return 0;
+
+err_load_vpu:
+	mdp->ctx_num--;
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+error_m2m_ctx:
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+error_ctrls:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	mutex_unlock(&mdp->lock);
+err_lock:
+	kfree(ctx);
+
+	return ret;
+}
+
+static int mtk_mdp_m2m_release(struct file *file)
+{
+	struct mtk_mdp_ctx *ctx = fh_to_ctx(file->private_data);
+	struct mtk_mdp_dev *mdp = ctx->mdp_dev;
+
+	flush_workqueue(mdp->job_wq);
+	mutex_lock(&mdp->lock);
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	mtk_mdp_vpu_deinit(&ctx->vpu);
+	mdp->ctx_num--;
+	list_del_init(&ctx->list);
+
+	mtk_mdp_dbg(0, "%s [%d]", dev_name(&mdp->pdev->dev), ctx->id);
+
+	mutex_unlock(&mdp->lock);
+	kfree(ctx);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations mtk_mdp_m2m_fops = {
+	.owner		= THIS_MODULE,
+	.open		= mtk_mdp_m2m_open,
+	.release	= mtk_mdp_m2m_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static const struct v4l2_m2m_ops mtk_mdp_m2m_ops = {
+	.device_run	= mtk_mdp_m2m_device_run,
+};
+
+int mtk_mdp_register_m2m_device(struct mtk_mdp_dev *mdp)
+{
+	struct device *dev = &mdp->pdev->dev;
+	int ret;
+
+	mdp->variant = &mtk_mdp_default_variant;
+	mdp->vdev = video_device_alloc();
+	if (!mdp->vdev) {
+		dev_err(dev, "failed to allocate video device\n");
+		ret = -ENOMEM;
+		goto err_video_alloc;
+	}
+	mdp->vdev->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING;
+	mdp->vdev->fops = &mtk_mdp_m2m_fops;
+	mdp->vdev->ioctl_ops = &mtk_mdp_m2m_ioctl_ops;
+	mdp->vdev->release = video_device_release;
+	mdp->vdev->lock = &mdp->lock;
+	mdp->vdev->vfl_dir = VFL_DIR_M2M;
+	mdp->vdev->v4l2_dev = &mdp->v4l2_dev;
+	snprintf(mdp->vdev->name, sizeof(mdp->vdev->name), "%s:m2m",
+		 MTK_MDP_MODULE_NAME);
+	video_set_drvdata(mdp->vdev, mdp);
+
+	mdp->m2m_dev = v4l2_m2m_init(&mtk_mdp_m2m_ops);
+	if (IS_ERR(mdp->m2m_dev)) {
+		dev_err(dev, "failed to initialize v4l2-m2m device\n");
+		ret = PTR_ERR(mdp->m2m_dev);
+		goto err_m2m_init;
+	}
+
+	ret = video_register_device(mdp->vdev, VFL_TYPE_VIDEO, 2);
+	if (ret) {
+		dev_err(dev, "failed to register video device\n");
+		goto err_vdev_register;
+	}
+
+	v4l2_info(&mdp->v4l2_dev, "driver registered as /dev/video%d",
+		  mdp->vdev->num);
+	return 0;
+
+err_vdev_register:
+	v4l2_m2m_release(mdp->m2m_dev);
+err_m2m_init:
+	video_device_release(mdp->vdev);
+err_video_alloc:
+
+	return ret;
+}
+
+void mtk_mdp_unregister_m2m_device(struct mtk_mdp_dev *mdp)
+{
+	video_unregister_device(mdp->vdev);
+	v4l2_m2m_release(mdp->m2m_dev);
+}
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_m2m.h b/drivers/media/platform/mediatek/mdp/mtk_mdp_m2m.h
new file mode 100644
index 000000000..485dbdbbf
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_m2m.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ */
+
+#ifndef __MTK_MDP_M2M_H__
+#define __MTK_MDP_M2M_H__
+
+void mtk_mdp_ctx_state_lock_set(struct mtk_mdp_ctx *ctx, u32 state);
+int mtk_mdp_register_m2m_device(struct mtk_mdp_dev *mdp);
+void mtk_mdp_unregister_m2m_device(struct mtk_mdp_dev *mdp);
+
+#endif /* __MTK_MDP_M2M_H__ */
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_regs.c b/drivers/media/platform/mediatek/mdp/mtk_mdp_regs.c
new file mode 100644
index 000000000..ba476d50a
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_regs.c
@@ -0,0 +1,148 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2015-2016 MediaTek Inc.
+ * Author: Houlong Wei <houlong.wei@mediatek.com>
+ *         Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ */
+
+#include <linux/platform_device.h>
+
+#include "mtk_mdp_core.h"
+#include "mtk_mdp_regs.h"
+
+
+#define MDP_COLORFMT_PACK(VIDEO, PLANE, COPLANE, HF, VF, BITS, GROUP, SWAP, ID)\
+	(((VIDEO) << 27) | ((PLANE) << 24) | ((COPLANE) << 22) |\
+	((HF) << 20) | ((VF) << 18) | ((BITS) << 8) | ((GROUP) << 6) |\
+	((SWAP) << 5) | ((ID) << 0))
+
+enum MDP_COLOR_ENUM {
+	MDP_COLOR_UNKNOWN = 0,
+	MDP_COLOR_NV12 = MDP_COLORFMT_PACK(0, 2, 1, 1, 1, 8, 1, 0, 12),
+	MDP_COLOR_I420 = MDP_COLORFMT_PACK(0, 3, 0, 1, 1, 8, 1, 0, 8),
+	MDP_COLOR_YV12 = MDP_COLORFMT_PACK(0, 3, 0, 1, 1, 8, 1, 1, 8),
+	/* Mediatek proprietary format */
+	MDP_COLOR_420_MT21 = MDP_COLORFMT_PACK(5, 2, 1, 1, 1, 256, 1, 0, 12),
+};
+
+static int32_t mtk_mdp_map_color_format(int v4l2_format)
+{
+	switch (v4l2_format) {
+	case V4L2_PIX_FMT_NV12M:
+	case V4L2_PIX_FMT_NV12:
+		return MDP_COLOR_NV12;
+	case V4L2_PIX_FMT_MT21C:
+		return MDP_COLOR_420_MT21;
+	case V4L2_PIX_FMT_YUV420M:
+	case V4L2_PIX_FMT_YUV420:
+		return MDP_COLOR_I420;
+	case V4L2_PIX_FMT_YVU420:
+		return MDP_COLOR_YV12;
+	}
+
+	mtk_mdp_err("Unknown format 0x%x", v4l2_format);
+
+	return MDP_COLOR_UNKNOWN;
+}
+
+void mtk_mdp_hw_set_input_addr(struct mtk_mdp_ctx *ctx,
+			       struct mtk_mdp_addr *addr)
+{
+	struct mdp_buffer *src_buf = &ctx->vpu.vsi->src_buffer;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(addr->addr); i++)
+		src_buf->addr_mva[i] = (uint64_t)addr->addr[i];
+}
+
+void mtk_mdp_hw_set_output_addr(struct mtk_mdp_ctx *ctx,
+				struct mtk_mdp_addr *addr)
+{
+	struct mdp_buffer *dst_buf = &ctx->vpu.vsi->dst_buffer;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(addr->addr); i++)
+		dst_buf->addr_mva[i] = (uint64_t)addr->addr[i];
+}
+
+void mtk_mdp_hw_set_in_size(struct mtk_mdp_ctx *ctx)
+{
+	struct mtk_mdp_frame *frame = &ctx->s_frame;
+	struct mdp_config *config = &ctx->vpu.vsi->src_config;
+
+	/* Set input pixel offset */
+	config->crop_x = frame->crop.left;
+	config->crop_y = frame->crop.top;
+
+	/* Set input cropped size */
+	config->crop_w = frame->crop.width;
+	config->crop_h = frame->crop.height;
+
+	/* Set input original size */
+	config->x = 0;
+	config->y = 0;
+	config->w = frame->width;
+	config->h = frame->height;
+}
+
+void mtk_mdp_hw_set_in_image_format(struct mtk_mdp_ctx *ctx)
+{
+	unsigned int i;
+	struct mtk_mdp_frame *frame = &ctx->s_frame;
+	struct mdp_config *config = &ctx->vpu.vsi->src_config;
+	struct mdp_buffer *src_buf = &ctx->vpu.vsi->src_buffer;
+
+	src_buf->plane_num = frame->fmt->num_comp;
+	config->format = mtk_mdp_map_color_format(frame->fmt->pixelformat);
+	config->w_stride = 0; /* MDP will calculate it by color format. */
+	config->h_stride = 0; /* MDP will calculate it by color format. */
+
+	for (i = 0; i < src_buf->plane_num; i++)
+		src_buf->plane_size[i] = frame->payload[i];
+}
+
+void mtk_mdp_hw_set_out_size(struct mtk_mdp_ctx *ctx)
+{
+	struct mtk_mdp_frame *frame = &ctx->d_frame;
+	struct mdp_config *config = &ctx->vpu.vsi->dst_config;
+
+	config->crop_x = frame->crop.left;
+	config->crop_y = frame->crop.top;
+	config->crop_w = frame->crop.width;
+	config->crop_h = frame->crop.height;
+	config->x = 0;
+	config->y = 0;
+	config->w = frame->width;
+	config->h = frame->height;
+}
+
+void mtk_mdp_hw_set_out_image_format(struct mtk_mdp_ctx *ctx)
+{
+	unsigned int i;
+	struct mtk_mdp_frame *frame = &ctx->d_frame;
+	struct mdp_config *config = &ctx->vpu.vsi->dst_config;
+	struct mdp_buffer *dst_buf = &ctx->vpu.vsi->dst_buffer;
+
+	dst_buf->plane_num = frame->fmt->num_comp;
+	config->format = mtk_mdp_map_color_format(frame->fmt->pixelformat);
+	config->w_stride = 0; /* MDP will calculate it by color format. */
+	config->h_stride = 0; /* MDP will calculate it by color format. */
+	for (i = 0; i < dst_buf->plane_num; i++)
+		dst_buf->plane_size[i] = frame->payload[i];
+}
+
+void mtk_mdp_hw_set_rotation(struct mtk_mdp_ctx *ctx)
+{
+	struct mdp_config_misc *misc = &ctx->vpu.vsi->misc;
+
+	misc->orientation = ctx->ctrls.rotate->val;
+	misc->hflip = ctx->ctrls.hflip->val;
+	misc->vflip = ctx->ctrls.vflip->val;
+}
+
+void mtk_mdp_hw_set_global_alpha(struct mtk_mdp_ctx *ctx)
+{
+	struct mdp_config_misc *misc = &ctx->vpu.vsi->misc;
+
+	misc->alpha = ctx->ctrls.global_alpha->val;
+}
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_regs.h b/drivers/media/platform/mediatek/mdp/mtk_mdp_regs.h
new file mode 100644
index 000000000..32cf202f2
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_regs.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ */
+
+#ifndef __MTK_MDP_REGS_H__
+#define __MTK_MDP_REGS_H__
+
+
+void mtk_mdp_hw_set_input_addr(struct mtk_mdp_ctx *ctx,
+			       struct mtk_mdp_addr *addr);
+void mtk_mdp_hw_set_output_addr(struct mtk_mdp_ctx *ctx,
+				struct mtk_mdp_addr *addr);
+void mtk_mdp_hw_set_in_size(struct mtk_mdp_ctx *ctx);
+void mtk_mdp_hw_set_in_image_format(struct mtk_mdp_ctx *ctx);
+void mtk_mdp_hw_set_out_size(struct mtk_mdp_ctx *ctx);
+void mtk_mdp_hw_set_out_image_format(struct mtk_mdp_ctx *ctx);
+void mtk_mdp_hw_set_rotation(struct mtk_mdp_ctx *ctx);
+void mtk_mdp_hw_set_global_alpha(struct mtk_mdp_ctx *ctx);
+
+
+#endif /* __MTK_MDP_REGS_H__ */
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_vpu.c b/drivers/media/platform/mediatek/mdp/mtk_mdp_vpu.c
new file mode 100644
index 000000000..b065ccd06
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_vpu.c
@@ -0,0 +1,138 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2015-2016 MediaTek Inc.
+ * Author: Houlong Wei <houlong.wei@mediatek.com>
+ *         Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ */
+
+#include "mtk_mdp_core.h"
+#include "mtk_mdp_vpu.h"
+#include "mtk_vpu.h"
+
+
+static inline struct mtk_mdp_ctx *vpu_to_ctx(struct mtk_mdp_vpu *vpu)
+{
+	return container_of(vpu, struct mtk_mdp_ctx, vpu);
+}
+
+static void mtk_mdp_vpu_handle_init_ack(const struct mdp_ipi_comm_ack *msg)
+{
+	struct mtk_mdp_vpu *vpu = (struct mtk_mdp_vpu *)
+					(unsigned long)msg->ap_inst;
+
+	/* mapping VPU address to kernel virtual address */
+	vpu->vsi = (struct mdp_process_vsi *)
+			vpu_mapping_dm_addr(vpu->pdev, msg->vpu_inst_addr);
+	vpu->inst_addr = msg->vpu_inst_addr;
+}
+
+static void mtk_mdp_vpu_ipi_handler(const void *data, unsigned int len,
+				    void *priv)
+{
+	const struct mdp_ipi_comm_ack *msg = data;
+	unsigned int msg_id = msg->msg_id;
+	struct mtk_mdp_vpu *vpu = (struct mtk_mdp_vpu *)
+					(unsigned long)msg->ap_inst;
+	struct mtk_mdp_ctx *ctx;
+
+	vpu->failure = msg->status;
+	if (!vpu->failure) {
+		switch (msg_id) {
+		case VPU_MDP_INIT_ACK:
+			mtk_mdp_vpu_handle_init_ack(data);
+			break;
+		case VPU_MDP_DEINIT_ACK:
+		case VPU_MDP_PROCESS_ACK:
+			break;
+		default:
+			ctx = vpu_to_ctx(vpu);
+			dev_err(&ctx->mdp_dev->pdev->dev,
+				"handle unknown ipi msg:0x%x\n",
+				msg_id);
+			break;
+		}
+	} else {
+		ctx = vpu_to_ctx(vpu);
+		mtk_mdp_dbg(0, "[%d]:msg 0x%x, failure:%d", ctx->id,
+			    msg_id, vpu->failure);
+	}
+}
+
+int mtk_mdp_vpu_register(struct platform_device *pdev)
+{
+	struct mtk_mdp_dev *mdp = platform_get_drvdata(pdev);
+	int err;
+
+	err = vpu_ipi_register(mdp->vpu_dev, IPI_MDP,
+			       mtk_mdp_vpu_ipi_handler, "mdp_vpu", NULL);
+	if (err)
+		dev_err(&mdp->pdev->dev,
+			"vpu_ipi_registration fail status=%d\n", err);
+
+	return err;
+}
+
+static int mtk_mdp_vpu_send_msg(void *msg, int len, struct mtk_mdp_vpu *vpu,
+				int id)
+{
+	struct mtk_mdp_ctx *ctx = vpu_to_ctx(vpu);
+	int err;
+
+	if (!vpu->pdev) {
+		mtk_mdp_dbg(1, "[%d]:vpu pdev is NULL", ctx->id);
+		return -EINVAL;
+	}
+
+	mutex_lock(&ctx->mdp_dev->vpulock);
+	err = vpu_ipi_send(vpu->pdev, (enum ipi_id)id, msg, len);
+	if (err)
+		dev_err(&ctx->mdp_dev->pdev->dev,
+			"vpu_ipi_send fail status %d\n", err);
+	mutex_unlock(&ctx->mdp_dev->vpulock);
+
+	return err;
+}
+
+static int mtk_mdp_vpu_send_ap_ipi(struct mtk_mdp_vpu *vpu, uint32_t msg_id)
+{
+	int err;
+	struct mdp_ipi_comm msg;
+
+	msg.msg_id = msg_id;
+	msg.ipi_id = IPI_MDP;
+	msg.vpu_inst_addr = vpu->inst_addr;
+	msg.ap_inst = (unsigned long)vpu;
+	err = mtk_mdp_vpu_send_msg((void *)&msg, sizeof(msg), vpu, IPI_MDP);
+	if (!err && vpu->failure)
+		err = -EINVAL;
+
+	return err;
+}
+
+int mtk_mdp_vpu_init(struct mtk_mdp_vpu *vpu)
+{
+	int err;
+	struct mdp_ipi_init msg;
+	struct mtk_mdp_ctx *ctx = vpu_to_ctx(vpu);
+
+	vpu->pdev = ctx->mdp_dev->vpu_dev;
+
+	msg.msg_id = AP_MDP_INIT;
+	msg.ipi_id = IPI_MDP;
+	msg.ap_inst = (unsigned long)vpu;
+	err = mtk_mdp_vpu_send_msg((void *)&msg, sizeof(msg), vpu, IPI_MDP);
+	if (!err && vpu->failure)
+		err = -EINVAL;
+
+	return err;
+}
+
+int mtk_mdp_vpu_deinit(struct mtk_mdp_vpu *vpu)
+{
+	return mtk_mdp_vpu_send_ap_ipi(vpu, AP_MDP_DEINIT);
+}
+
+int mtk_mdp_vpu_process(struct mtk_mdp_vpu *vpu)
+{
+	return mtk_mdp_vpu_send_ap_ipi(vpu, AP_MDP_PROCESS);
+}
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_vpu.h b/drivers/media/platform/mediatek/mdp/mtk_mdp_vpu.h
new file mode 100644
index 000000000..5a1020508
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_vpu.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2015-2016 MediaTek Inc.
+ * Author: Houlong Wei <houlong.wei@mediatek.com>
+ *         Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
+ */
+
+#ifndef __MTK_MDP_VPU_H__
+#define __MTK_MDP_VPU_H__
+
+#include "mtk_mdp_ipi.h"
+
+
+/**
+ * struct mtk_mdp_vpu - VPU instance for MDP
+ * @pdev	: pointer to the VPU platform device
+ * @inst_addr	: VPU MDP instance address
+ * @failure	: VPU execution result status
+ * @vsi		: VPU shared information
+ */
+struct mtk_mdp_vpu {
+	struct platform_device	*pdev;
+	uint32_t		inst_addr;
+	int32_t			failure;
+	struct mdp_process_vsi	*vsi;
+};
+
+int mtk_mdp_vpu_register(struct platform_device *pdev);
+int mtk_mdp_vpu_init(struct mtk_mdp_vpu *vpu);
+int mtk_mdp_vpu_deinit(struct mtk_mdp_vpu *vpu);
+int mtk_mdp_vpu_process(struct mtk_mdp_vpu *vpu);
+
+#endif /* __MTK_MDP_VPU_H__ */
diff --git a/drivers/media/platform/mediatek/mdp3/Kconfig b/drivers/media/platform/mediatek/mdp3/Kconfig
new file mode 100644
index 000000000..602329c44
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/Kconfig
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_MEDIATEK_MDP3
+	tristate "MediaTek MDP v3 driver"
+	depends on MTK_IOMMU || COMPILE_TEST
+	depends on VIDEO_DEV
+	depends on HAS_DMA
+	depends on REMOTEPROC
+	depends on MTK_MMSYS
+	depends on MTK_CMDQ
+	depends on MTK_SCP
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	default n
+	help
+	    It is a v4l2 driver and present in MediaTek MT8183 SoC.
+	    The driver supports scaling and color space conversion.
+
+	    To compile this driver as a module, choose M here: the
+	    module will be called mtk-mdp3.
diff --git a/drivers/media/platform/mediatek/mdp3/Makefile b/drivers/media/platform/mediatek/mdp3/Makefile
new file mode 100644
index 000000000..63e6c87e4
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0-only
+mtk-mdp3-y += mtk-mdp3-core.o mtk-mdp3-vpu.o mtk-mdp3-regs.o
+mtk-mdp3-y += mtk-mdp3-m2m.o
+mtk-mdp3-y += mtk-mdp3-comp.o mtk-mdp3-cmdq.o
+
+obj-$(CONFIG_VIDEO_MEDIATEK_MDP3) += mtk-mdp3.o
diff --git a/drivers/media/platform/mediatek/mdp3/mdp_reg_ccorr.h b/drivers/media/platform/mediatek/mdp3/mdp_reg_ccorr.h
new file mode 100644
index 000000000..3b2c6531c
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mdp_reg_ccorr.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#ifndef __MDP_REG_CCORR_H__
+#define __MDP_REG_CCORR_H__
+
+#define MDP_CCORR_EN                0x000
+#define MDP_CCORR_CFG               0x020
+#define MDP_CCORR_SIZE              0x030
+
+/* MASK */
+#define MDP_CCORR_EN_MASK           0x00000001
+#define MDP_CCORR_CFG_MASK          0x70001317
+#define MDP_CCORR_SIZE_MASK         0x1fff1fff
+
+#endif  // __MDP_REG_CCORR_H__
diff --git a/drivers/media/platform/mediatek/mdp3/mdp_reg_rdma.h b/drivers/media/platform/mediatek/mdp3/mdp_reg_rdma.h
new file mode 100644
index 000000000..be4065e25
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mdp_reg_rdma.h
@@ -0,0 +1,65 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#ifndef __MDP_REG_RDMA_H__
+#define __MDP_REG_RDMA_H__
+
+#define MDP_RDMA_EN                     0x000
+#define MDP_RDMA_RESET                  0x008
+#define MDP_RDMA_CON                    0x020
+#define MDP_RDMA_GMCIF_CON              0x028
+#define MDP_RDMA_SRC_CON                0x030
+#define MDP_RDMA_MF_BKGD_SIZE_IN_BYTE   0x060
+#define MDP_RDMA_MF_BKGD_SIZE_IN_PXL    0x068
+#define MDP_RDMA_MF_SRC_SIZE            0x070
+#define MDP_RDMA_MF_CLIP_SIZE           0x078
+#define MDP_RDMA_MF_OFFSET_1            0x080
+#define MDP_RDMA_SF_BKGD_SIZE_IN_BYTE   0x090
+#define MDP_RDMA_SRC_END_0              0x100
+#define MDP_RDMA_SRC_END_1              0x108
+#define MDP_RDMA_SRC_END_2              0x110
+#define MDP_RDMA_SRC_OFFSET_0           0x118
+#define MDP_RDMA_SRC_OFFSET_1           0x120
+#define MDP_RDMA_SRC_OFFSET_2           0x128
+#define MDP_RDMA_SRC_OFFSET_0_P         0x148
+#define MDP_RDMA_TRANSFORM_0            0x200
+#define MDP_RDMA_RESV_DUMMY_0           0x2a0
+#define MDP_RDMA_MON_STA_1              0x408
+#define MDP_RDMA_SRC_BASE_0             0xf00
+#define MDP_RDMA_SRC_BASE_1             0xf08
+#define MDP_RDMA_SRC_BASE_2             0xf10
+#define MDP_RDMA_UFO_DEC_LENGTH_BASE_Y  0xf20
+#define MDP_RDMA_UFO_DEC_LENGTH_BASE_C  0xf28
+
+/* MASK */
+#define MDP_RDMA_EN_MASK                    0x00000001
+#define MDP_RDMA_RESET_MASK                 0x00000001
+#define MDP_RDMA_CON_MASK                   0x00001110
+#define MDP_RDMA_GMCIF_CON_MASK             0xfffb3771
+#define MDP_RDMA_SRC_CON_MASK               0xf3ffffff
+#define MDP_RDMA_MF_BKGD_SIZE_IN_BYTE_MASK  0x001fffff
+#define MDP_RDMA_MF_BKGD_SIZE_IN_PXL_MASK   0x001fffff
+#define MDP_RDMA_MF_SRC_SIZE_MASK           0x1fff1fff
+#define MDP_RDMA_MF_CLIP_SIZE_MASK          0x1fff1fff
+#define MDP_RDMA_MF_OFFSET_1_MASK           0x003f001f
+#define MDP_RDMA_SF_BKGD_SIZE_IN_BYTE_MASK  0x001fffff
+#define MDP_RDMA_SRC_END_0_MASK             0xffffffff
+#define MDP_RDMA_SRC_END_1_MASK             0xffffffff
+#define MDP_RDMA_SRC_END_2_MASK             0xffffffff
+#define MDP_RDMA_SRC_OFFSET_0_MASK          0xffffffff
+#define MDP_RDMA_SRC_OFFSET_1_MASK          0xffffffff
+#define MDP_RDMA_SRC_OFFSET_2_MASK          0xffffffff
+#define MDP_RDMA_SRC_OFFSET_0_P_MASK        0xffffffff
+#define MDP_RDMA_TRANSFORM_0_MASK           0xff110777
+#define MDP_RDMA_RESV_DUMMY_0_MASK          0xffffffff
+#define MDP_RDMA_MON_STA_1_MASK             0xffffffff
+#define MDP_RDMA_SRC_BASE_0_MASK            0xffffffff
+#define MDP_RDMA_SRC_BASE_1_MASK            0xffffffff
+#define MDP_RDMA_SRC_BASE_2_MASK            0xffffffff
+#define MDP_RDMA_UFO_DEC_LENGTH_BASE_Y_MASK 0xffffffff
+#define MDP_RDMA_UFO_DEC_LENGTH_BASE_C_MASK 0xffffffff
+
+#endif  // __MDP_REG_RDMA_H__
diff --git a/drivers/media/platform/mediatek/mdp3/mdp_reg_rsz.h b/drivers/media/platform/mediatek/mdp3/mdp_reg_rsz.h
new file mode 100644
index 000000000..484f6d606
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mdp_reg_rsz.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#ifndef __MDP_REG_RSZ_H__
+#define __MDP_REG_RSZ_H__
+
+#define PRZ_ENABLE                                        0x000
+#define PRZ_CONTROL_1                                     0x004
+#define PRZ_CONTROL_2                                     0x008
+#define PRZ_INPUT_IMAGE                                   0x010
+#define PRZ_OUTPUT_IMAGE                                  0x014
+#define PRZ_HORIZONTAL_COEFF_STEP                         0x018
+#define PRZ_VERTICAL_COEFF_STEP                           0x01c
+#define PRZ_LUMA_HORIZONTAL_INTEGER_OFFSET                0x020
+#define PRZ_LUMA_HORIZONTAL_SUBPIXEL_OFFSET               0x024
+#define PRZ_LUMA_VERTICAL_INTEGER_OFFSET                  0x028
+#define PRZ_LUMA_VERTICAL_SUBPIXEL_OFFSET                 0x02c
+#define PRZ_CHROMA_HORIZONTAL_INTEGER_OFFSET              0x030
+#define PRZ_CHROMA_HORIZONTAL_SUBPIXEL_OFFSET             0x034
+
+/* MASK */
+#define PRZ_ENABLE_MASK                                   0x00010001
+#define PRZ_CONTROL_1_MASK                                0xfffffff3
+#define PRZ_CONTROL_2_MASK                                0x0ffffaff
+#define PRZ_INPUT_IMAGE_MASK                              0xffffffff
+#define PRZ_OUTPUT_IMAGE_MASK                             0xffffffff
+#define PRZ_HORIZONTAL_COEFF_STEP_MASK                    0x007fffff
+#define PRZ_VERTICAL_COEFF_STEP_MASK                      0x007fffff
+#define PRZ_LUMA_HORIZONTAL_INTEGER_OFFSET_MASK           0x0000ffff
+#define PRZ_LUMA_HORIZONTAL_SUBPIXEL_OFFSET_MASK          0x001fffff
+#define PRZ_LUMA_VERTICAL_INTEGER_OFFSET_MASK             0x0000ffff
+#define PRZ_LUMA_VERTICAL_SUBPIXEL_OFFSET_MASK            0x001fffff
+#define PRZ_CHROMA_HORIZONTAL_INTEGER_OFFSET_MASK         0x0000ffff
+#define PRZ_CHROMA_HORIZONTAL_SUBPIXEL_OFFSET_MASK        0x001fffff
+
+#endif // __MDP_REG_RSZ_H__
diff --git a/drivers/media/platform/mediatek/mdp3/mdp_reg_wdma.h b/drivers/media/platform/mediatek/mdp3/mdp_reg_wdma.h
new file mode 100644
index 000000000..0280e91c0
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mdp_reg_wdma.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#ifndef __MDP_REG_WDMA_H__
+#define __MDP_REG_WDMA_H__
+
+#define WDMA_EN                 0x008
+#define WDMA_RST                0x00c
+#define WDMA_CFG                0x014
+#define WDMA_SRC_SIZE           0x018
+#define WDMA_CLIP_SIZE          0x01c
+#define WDMA_CLIP_COORD         0x020
+#define WDMA_DST_W_IN_BYTE      0x028
+#define WDMA_ALPHA              0x02c
+#define WDMA_BUF_CON2           0x03c
+#define WDMA_DST_UV_PITCH       0x078
+#define WDMA_DST_ADDR_OFFSET    0x080
+#define WDMA_DST_U_ADDR_OFFSET  0x084
+#define WDMA_DST_V_ADDR_OFFSET  0x088
+#define WDMA_FLOW_CTRL_DBG      0x0a0
+#define WDMA_DST_ADDR           0xf00
+#define WDMA_DST_U_ADDR         0xf04
+#define WDMA_DST_V_ADDR         0xf08
+
+/* MASK */
+#define WDMA_EN_MASK                0x00000001
+#define WDMA_RST_MASK               0x00000001
+#define WDMA_CFG_MASK               0xff03bff0
+#define WDMA_SRC_SIZE_MASK          0x3fff3fff
+#define WDMA_CLIP_SIZE_MASK         0x3fff3fff
+#define WDMA_CLIP_COORD_MASK        0x3fff3fff
+#define WDMA_DST_W_IN_BYTE_MASK     0x0000ffff
+#define WDMA_ALPHA_MASK             0x800000ff
+#define WDMA_BUF_CON2_MASK          0xffffffff
+#define WDMA_DST_UV_PITCH_MASK      0x0000ffff
+#define WDMA_DST_ADDR_OFFSET_MASK   0x0fffffff
+#define WDMA_DST_U_ADDR_OFFSET_MASK 0x0fffffff
+#define WDMA_DST_V_ADDR_OFFSET_MASK 0x0fffffff
+#define WDMA_FLOW_CTRL_DBG_MASK     0x0000f3ff
+#define WDMA_DST_ADDR_MASK          0xffffffff
+#define WDMA_DST_U_ADDR_MASK        0xffffffff
+#define WDMA_DST_V_ADDR_MASK        0xffffffff
+
+#endif  // __MDP_REG_WDMA_H__
diff --git a/drivers/media/platform/mediatek/mdp3/mdp_reg_wrot.h b/drivers/media/platform/mediatek/mdp3/mdp_reg_wrot.h
new file mode 100644
index 000000000..6d3ff0e2b
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mdp_reg_wrot.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#ifndef __MDP_REG_WROT_H__
+#define __MDP_REG_WROT_H__
+
+#define VIDO_CTRL                   0x000
+#define VIDO_MAIN_BUF_SIZE          0x008
+#define VIDO_SOFT_RST               0x010
+#define VIDO_SOFT_RST_STAT          0x014
+#define VIDO_CROP_OFST              0x020
+#define VIDO_TAR_SIZE               0x024
+#define VIDO_OFST_ADDR              0x02c
+#define VIDO_STRIDE                 0x030
+#define VIDO_OFST_ADDR_C            0x038
+#define VIDO_STRIDE_C               0x03c
+#define VIDO_DITHER                 0x054
+#define VIDO_STRIDE_V               0x06c
+#define VIDO_OFST_ADDR_V            0x068
+#define VIDO_RSV_1                  0x070
+#define VIDO_IN_SIZE                0x078
+#define VIDO_ROT_EN                 0x07c
+#define VIDO_FIFO_TEST              0x080
+#define VIDO_MAT_CTRL               0x084
+#define VIDO_BASE_ADDR              0xf00
+#define VIDO_BASE_ADDR_C            0xf04
+#define VIDO_BASE_ADDR_V            0xf08
+
+/* MASK */
+#define VIDO_CTRL_MASK                  0xf530711f
+#define VIDO_MAIN_BUF_SIZE_MASK         0x1fff7f77
+#define VIDO_SOFT_RST_MASK              0x00000001
+#define VIDO_SOFT_RST_STAT_MASK         0x00000001
+#define VIDO_TAR_SIZE_MASK              0x1fff1fff
+#define VIDO_CROP_OFST_MASK             0x1fff1fff
+#define VIDO_OFST_ADDR_MASK             0x0fffffff
+#define VIDO_STRIDE_MASK                0x0000ffff
+#define VIDO_OFST_ADDR_C_MASK           0x0fffffff
+#define VIDO_STRIDE_C_MASK              0x0000ffff
+#define VIDO_DITHER_MASK                0xff000001
+#define VIDO_STRIDE_V_MASK              0x0000ffff
+#define VIDO_OFST_ADDR_V_MASK           0x0fffffff
+#define VIDO_RSV_1_MASK                 0xffffffff
+#define VIDO_IN_SIZE_MASK               0x1fff1fff
+#define VIDO_ROT_EN_MASK                0x00000001
+#define VIDO_FIFO_TEST_MASK             0x00000fff
+#define VIDO_MAT_CTRL_MASK              0x000000f3
+#define VIDO_BASE_ADDR_MASK             0xffffffff
+#define VIDO_BASE_ADDR_C_MASK           0xffffffff
+#define VIDO_BASE_ADDR_V_MASK           0xffffffff
+
+#endif  // __MDP_REG_WROT_H__
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-img-ipi.h b/drivers/media/platform/mediatek/mdp3/mtk-img-ipi.h
new file mode 100644
index 000000000..3e66ebaee
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-img-ipi.h
@@ -0,0 +1,290 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Holmes Chiou <holmes.chiou@mediatek.com>
+ *         Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#ifndef __MTK_IMG_IPI_H__
+#define __MTK_IMG_IPI_H__
+
+#include <linux/types.h>
+
+/*
+ * ISP-MDP generic input information
+ * MD5 of the target SCP blob:
+ *     6da52bdcf4bf76a0983b313e1d4745d6
+ */
+
+#define IMG_MAX_HW_INPUTS	3
+
+#define IMG_MAX_HW_OUTPUTS	4
+
+#define IMG_MAX_PLANES		3
+
+#define IMG_IPI_INIT    1
+#define IMG_IPI_DEINIT  2
+#define IMG_IPI_FRAME   3
+#define IMG_IPI_DEBUG   4
+
+struct img_timeval {
+	u32 tv_sec;
+	u32 tv_usec;
+} __packed;
+
+struct img_addr {
+	u64 va; /* Used for Linux OS access */
+	u32 pa; /* Used for CM4 access */
+	u32 iova; /* Used for IOMMU HW access */
+} __packed;
+
+struct tuning_addr {
+	u64	present;
+	u32	pa;	/* Used for CM4 access */
+	u32	iova;	/* Used for IOMMU HW access */
+} __packed;
+
+struct img_sw_addr {
+	u64 va; /* Used for APMCU access */
+	u32 pa; /* Used for CM4 access */
+} __packed;
+
+struct img_plane_format {
+	u32 size;
+	u16 stride;
+} __packed;
+
+struct img_pix_format {
+	u16 width;
+	u16 height;
+	u32 colorformat; /* enum mdp_color */
+	u16 ycbcr_prof; /* enum mdp_ycbcr_profile */
+	struct img_plane_format plane_fmt[IMG_MAX_PLANES];
+} __packed;
+
+struct img_image_buffer {
+	struct img_pix_format format;
+	u32 iova[IMG_MAX_PLANES];
+	/* enum mdp_buffer_usage, FD or advanced ISP usages */
+	u32 usage;
+} __packed;
+
+#define IMG_SUBPIXEL_SHIFT	20
+
+struct img_crop {
+	s16 left;
+	s16 top;
+	u16 width;
+	u16 height;
+	u32 left_subpix;
+	u32 top_subpix;
+	u32 width_subpix;
+	u32 height_subpix;
+} __packed;
+
+#define IMG_CTRL_FLAG_HFLIP	BIT(0)
+#define IMG_CTRL_FLAG_DITHER	BIT(1)
+#define IMG_CTRL_FLAG_SHARPNESS	BIT(4)
+#define IMG_CTRL_FLAG_HDR	BIT(5)
+#define IMG_CTRL_FLAG_DRE	BIT(6)
+
+struct img_input {
+	struct img_image_buffer buffer;
+	u16 flags; /* HDR, DRE, dither */
+} __packed;
+
+struct img_output {
+	struct img_image_buffer buffer;
+	struct img_crop crop;
+	s16 rotation;
+	u16 flags; /* H-flip, sharpness, dither */
+} __packed;
+
+struct img_ipi_frameparam {
+	u32 index;
+	u32 frame_no;
+	struct img_timeval timestamp;
+	u8 type; /* enum mdp_stream_type */
+	u8 state;
+	u8 num_inputs;
+	u8 num_outputs;
+	u64 drv_data;
+	struct img_input inputs[IMG_MAX_HW_INPUTS];
+	struct img_output outputs[IMG_MAX_HW_OUTPUTS];
+	struct tuning_addr tuning_data;
+	struct img_addr subfrm_data;
+	struct img_sw_addr config_data;
+	struct img_sw_addr self_data;
+} __packed;
+
+struct img_sw_buffer {
+	u64	handle;		/* Used for APMCU access */
+	u32	scp_addr;	/* Used for CM4 access */
+} __packed;
+
+struct img_ipi_param {
+	u8 usage;
+	struct img_sw_buffer frm_param;
+} __packed;
+
+struct img_frameparam {
+	struct list_head list_entry;
+	struct img_ipi_frameparam frameparam;
+};
+
+/* ISP-MDP generic output information */
+
+struct img_comp_frame {
+	u32 output_disable:1;
+	u32 bypass:1;
+	u16 in_width;
+	u16 in_height;
+	u16 out_width;
+	u16 out_height;
+	struct img_crop crop;
+	u16 in_total_width;
+	u16 out_total_width;
+} __packed;
+
+struct img_region {
+	s16 left;
+	s16 right;
+	s16 top;
+	s16 bottom;
+} __packed;
+
+struct img_offset {
+	s16 left;
+	s16 top;
+	u32 left_subpix;
+	u32 top_subpix;
+} __packed;
+
+struct img_comp_subfrm {
+	u32 tile_disable:1;
+	struct img_region in;
+	struct img_region out;
+	struct img_offset luma;
+	struct img_offset chroma;
+	s16 out_vertical; /* Output vertical index */
+	s16 out_horizontal; /* Output horizontal index */
+} __packed;
+
+#define IMG_MAX_SUBFRAMES	14
+
+struct mdp_rdma_subfrm {
+	u32 offset[IMG_MAX_PLANES];
+	u32 offset_0_p;
+	u32 src;
+	u32 clip;
+	u32 clip_ofst;
+} __packed;
+
+struct mdp_rdma_data {
+	u32 src_ctrl;
+	u32 control;
+	u32 iova[IMG_MAX_PLANES];
+	u32 iova_end[IMG_MAX_PLANES];
+	u32 mf_bkgd;
+	u32 mf_bkgd_in_pxl;
+	u32 sf_bkgd;
+	u32 ufo_dec_y;
+	u32 ufo_dec_c;
+	u32 transform;
+	struct mdp_rdma_subfrm subfrms[IMG_MAX_SUBFRAMES];
+} __packed;
+
+struct mdp_rsz_subfrm {
+	u32 control2;
+	u32 src;
+	u32 clip;
+} __packed;
+
+struct mdp_rsz_data {
+	u32 coeff_step_x;
+	u32 coeff_step_y;
+	u32 control1;
+	u32 control2;
+	struct mdp_rsz_subfrm subfrms[IMG_MAX_SUBFRAMES];
+} __packed;
+
+struct mdp_wrot_subfrm {
+	u32 offset[IMG_MAX_PLANES];
+	u32 src;
+	u32 clip;
+	u32 clip_ofst;
+	u32 main_buf;
+} __packed;
+
+struct mdp_wrot_data {
+	u32 iova[IMG_MAX_PLANES];
+	u32 control;
+	u32 stride[IMG_MAX_PLANES];
+	u32 mat_ctrl;
+	u32 fifo_test;
+	u32 filter;
+	struct mdp_wrot_subfrm subfrms[IMG_MAX_SUBFRAMES];
+} __packed;
+
+struct mdp_wdma_subfrm {
+	u32 offset[IMG_MAX_PLANES];
+	u32 src;
+	u32 clip;
+	u32 clip_ofst;
+} __packed;
+
+struct mdp_wdma_data {
+	u32 wdma_cfg;
+	u32 iova[IMG_MAX_PLANES];
+	u32 w_in_byte;
+	u32 uv_stride;
+	struct mdp_wdma_subfrm subfrms[IMG_MAX_SUBFRAMES];
+} __packed;
+
+struct isp_data {
+	u64 dl_flags; /* 1 << (enum mdp_comp_type) */
+	u32 smxi_iova[4];
+	u32 cq_idx;
+	u32 cq_iova;
+	u32 tpipe_iova[IMG_MAX_SUBFRAMES];
+} __packed;
+
+struct img_compparam {
+	u16 type; /* enum mdp_comp_type */
+	u16 id; /* enum mtk_mdp_comp_id */
+	u32 input;
+	u32 outputs[IMG_MAX_HW_OUTPUTS];
+	u32 num_outputs;
+	struct img_comp_frame frame;
+	struct img_comp_subfrm subfrms[IMG_MAX_SUBFRAMES];
+	u32 num_subfrms;
+	union {
+		struct mdp_rdma_data rdma;
+		struct mdp_rsz_data rsz;
+		struct mdp_wrot_data wrot;
+		struct mdp_wdma_data wdma;
+		struct isp_data isp;
+	};
+} __packed;
+
+#define IMG_MAX_COMPONENTS	20
+
+struct img_mux {
+	u32 reg;
+	u32 value;
+	u32 subsys_id;
+};
+
+struct img_mmsys_ctrl {
+	struct img_mux sets[IMG_MAX_COMPONENTS * 2];
+	u32 num_sets;
+};
+
+struct img_config {
+	struct img_compparam components[IMG_MAX_COMPONENTS];
+	u32 num_components;
+	struct img_mmsys_ctrl ctrls[IMG_MAX_SUBFRAMES];
+	u32 num_subfrms;
+} __packed;
+
+#endif  /* __MTK_IMG_IPI_H__ */
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.c b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.c
new file mode 100644
index 000000000..124c1b96e
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.c
@@ -0,0 +1,469 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#include <linux/mailbox_controller.h>
+#include <linux/platform_device.h>
+#include "mtk-mdp3-cmdq.h"
+#include "mtk-mdp3-comp.h"
+#include "mtk-mdp3-core.h"
+#include "mtk-mdp3-m2m.h"
+
+#define MDP_PATH_MAX_COMPS	IMG_MAX_COMPONENTS
+
+struct mdp_path {
+	struct mdp_dev		*mdp_dev;
+	struct mdp_comp_ctx	comps[MDP_PATH_MAX_COMPS];
+	u32			num_comps;
+	const struct img_config	*config;
+	const struct img_ipi_frameparam *param;
+	const struct v4l2_rect	*composes[IMG_MAX_HW_OUTPUTS];
+	struct v4l2_rect	bounds[IMG_MAX_HW_OUTPUTS];
+};
+
+#define has_op(ctx, op) \
+	((ctx)->comp->ops && (ctx)->comp->ops->op)
+ #define call_op(ctx, op, ...) \
+	(has_op(ctx, op) ? (ctx)->comp->ops->op(ctx, ##__VA_ARGS__) : 0)
+
+static bool is_output_disabled(const struct img_compparam *param, u32 count)
+{
+	return (count < param->num_subfrms) ?
+		(param->frame.output_disable ||
+		param->subfrms[count].tile_disable) :
+		true;
+}
+
+static int mdp_path_subfrm_require(const struct mdp_path *path,
+				   struct mdp_cmdq_cmd *cmd,
+				   s32 *mutex_id, u32 count)
+{
+	const struct img_config *config = path->config;
+	const struct mdp_comp_ctx *ctx;
+	const struct mtk_mdp_driver_data *data = path->mdp_dev->mdp_data;
+	struct device *dev = &path->mdp_dev->pdev->dev;
+	struct mtk_mutex **mutex = path->mdp_dev->mdp_mutex;
+	int id, index;
+
+	/* Decide which mutex to use based on the current pipeline */
+	switch (path->comps[0].comp->id) {
+	case MDP_COMP_RDMA0:
+		*mutex_id = MDP_PIPE_RDMA0;
+		break;
+	case MDP_COMP_ISP_IMGI:
+		*mutex_id = MDP_PIPE_IMGI;
+		break;
+	case MDP_COMP_WPEI:
+		*mutex_id = MDP_PIPE_WPEI;
+		break;
+	case MDP_COMP_WPEI2:
+		*mutex_id = MDP_PIPE_WPEI2;
+		break;
+	default:
+		dev_err(dev, "Unknown pipeline and no mutex is assigned");
+		return -EINVAL;
+	}
+
+	/* Set mutex mod */
+	for (index = 0; index < config->num_components; index++) {
+		ctx = &path->comps[index];
+		if (is_output_disabled(ctx->param, count))
+			continue;
+		id = ctx->comp->id;
+		mtk_mutex_write_mod(mutex[*mutex_id],
+				    data->mdp_mutex_table_idx[id], false);
+	}
+
+	mtk_mutex_write_sof(mutex[*mutex_id],
+			    MUTEX_SOF_IDX_SINGLE_MODE);
+
+	return 0;
+}
+
+static int mdp_path_subfrm_run(const struct mdp_path *path,
+			       struct mdp_cmdq_cmd *cmd,
+			       s32 *mutex_id, u32 count)
+{
+	const struct img_config *config = path->config;
+	const struct mdp_comp_ctx *ctx;
+	struct device *dev = &path->mdp_dev->pdev->dev;
+	struct mtk_mutex **mutex = path->mdp_dev->mdp_mutex;
+	int index;
+	s32 event;
+
+	if (-1 == *mutex_id) {
+		dev_err(dev, "Incorrect mutex id");
+		return -EINVAL;
+	}
+
+	/* Wait WROT SRAM shared to DISP RDMA */
+	/* Clear SOF event for each engine */
+	for (index = 0; index < config->num_components; index++) {
+		ctx = &path->comps[index];
+		if (is_output_disabled(ctx->param, count))
+			continue;
+		event = ctx->comp->gce_event[MDP_GCE_EVENT_SOF];
+		if (event != MDP_GCE_NO_EVENT)
+			MM_REG_CLEAR(cmd, event);
+	}
+
+	/* Enable the mutex */
+	mtk_mutex_enable_by_cmdq(mutex[*mutex_id], (void *)&cmd->pkt);
+
+	/* Wait SOF events and clear mutex modules (optional) */
+	for (index = 0; index < config->num_components; index++) {
+		ctx = &path->comps[index];
+		if (is_output_disabled(ctx->param, count))
+			continue;
+		event = ctx->comp->gce_event[MDP_GCE_EVENT_SOF];
+		if (event != MDP_GCE_NO_EVENT)
+			MM_REG_WAIT(cmd, event);
+	}
+
+	return 0;
+}
+
+static int mdp_path_ctx_init(struct mdp_dev *mdp, struct mdp_path *path)
+{
+	const struct img_config *config = path->config;
+	int index, ret;
+
+	if (config->num_components < 1)
+		return -EINVAL;
+
+	for (index = 0; index < config->num_components; index++) {
+		ret = mdp_comp_ctx_config(mdp, &path->comps[index],
+					  &config->components[index],
+					  path->param);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int mdp_path_config_subfrm(struct mdp_cmdq_cmd *cmd,
+				  struct mdp_path *path, u32 count)
+{
+	const struct img_config *config = path->config;
+	const struct img_mmsys_ctrl *ctrl = &config->ctrls[count];
+	const struct img_mux *set;
+	struct mdp_comp_ctx *ctx;
+	s32 mutex_id;
+	int index, ret;
+
+	/* Acquire components */
+	ret = mdp_path_subfrm_require(path, cmd, &mutex_id, count);
+	if (ret)
+		return ret;
+	/* Enable mux settings */
+	for (index = 0; index < ctrl->num_sets; index++) {
+		set = &ctrl->sets[index];
+		cmdq_pkt_write_mask(&cmd->pkt, set->subsys_id, set->reg,
+				    set->value, 0xFFFFFFFF);
+	}
+	/* Config sub-frame information */
+	for (index = (config->num_components - 1); index >= 0; index--) {
+		ctx = &path->comps[index];
+		if (is_output_disabled(ctx->param, count))
+			continue;
+		ret = call_op(ctx, config_subfrm, cmd, count);
+		if (ret)
+			return ret;
+	}
+	/* Run components */
+	ret = mdp_path_subfrm_run(path, cmd, &mutex_id, count);
+	if (ret)
+		return ret;
+	/* Wait components done */
+	for (index = 0; index < config->num_components; index++) {
+		ctx = &path->comps[index];
+		if (is_output_disabled(ctx->param, count))
+			continue;
+		ret = call_op(ctx, wait_comp_event, cmd);
+		if (ret)
+			return ret;
+	}
+	/* Advance to the next sub-frame */
+	for (index = 0; index < config->num_components; index++) {
+		ctx = &path->comps[index];
+		ret = call_op(ctx, advance_subfrm, cmd, count);
+		if (ret)
+			return ret;
+	}
+	/* Disable mux settings */
+	for (index = 0; index < ctrl->num_sets; index++) {
+		set = &ctrl->sets[index];
+		cmdq_pkt_write_mask(&cmd->pkt, set->subsys_id, set->reg,
+				    0, 0xFFFFFFFF);
+	}
+
+	return 0;
+}
+
+static int mdp_path_config(struct mdp_dev *mdp, struct mdp_cmdq_cmd *cmd,
+			   struct mdp_path *path)
+{
+	const struct img_config *config = path->config;
+	struct mdp_comp_ctx *ctx;
+	int index, count, ret;
+
+	/* Config path frame */
+	/* Reset components */
+	for (index = 0; index < config->num_components; index++) {
+		ctx = &path->comps[index];
+		ret = call_op(ctx, init_comp, cmd);
+		if (ret)
+			return ret;
+	}
+	/* Config frame mode */
+	for (index = 0; index < config->num_components; index++) {
+		const struct v4l2_rect *compose =
+			path->composes[ctx->param->outputs[0]];
+
+		ctx = &path->comps[index];
+		ret = call_op(ctx, config_frame, cmd, compose);
+		if (ret)
+			return ret;
+	}
+
+	/* Config path sub-frames */
+	for (count = 0; count < config->num_subfrms; count++) {
+		ret = mdp_path_config_subfrm(cmd, path, count);
+		if (ret)
+			return ret;
+	}
+	/* Post processing information */
+	for (index = 0; index < config->num_components; index++) {
+		ctx = &path->comps[index];
+		ret = call_op(ctx, post_process, cmd);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
+static int mdp_cmdq_pkt_create(struct cmdq_client *client, struct cmdq_pkt *pkt,
+			       size_t size)
+{
+	struct device *dev;
+	dma_addr_t dma_addr;
+
+	pkt->va_base = kzalloc(size, GFP_KERNEL);
+	if (!pkt->va_base)
+		return -ENOMEM;
+
+	pkt->buf_size = size;
+	pkt->cl = (void *)client;
+
+	dev = client->chan->mbox->dev;
+	dma_addr = dma_map_single(dev, pkt->va_base, pkt->buf_size,
+				  DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, dma_addr)) {
+		dev_err(dev, "dma map failed, size=%u\n", (u32)(u64)size);
+		kfree(pkt->va_base);
+		return -ENOMEM;
+	}
+
+	pkt->pa_base = dma_addr;
+
+	return 0;
+}
+
+static void mdp_cmdq_pkt_destroy(struct cmdq_pkt *pkt)
+{
+	struct cmdq_client *client = (struct cmdq_client *)pkt->cl;
+
+	dma_unmap_single(client->chan->mbox->dev, pkt->pa_base, pkt->buf_size,
+			 DMA_TO_DEVICE);
+	kfree(pkt->va_base);
+	pkt->va_base = NULL;
+}
+
+static void mdp_auto_release_work(struct work_struct *work)
+{
+	struct mdp_cmdq_cmd *cmd;
+	struct mdp_dev *mdp;
+
+	cmd = container_of(work, struct mdp_cmdq_cmd, auto_release_work);
+	mdp = cmd->mdp;
+
+	mtk_mutex_unprepare(mdp->mdp_mutex[MDP_PIPE_RDMA0]);
+	mdp_comp_clocks_off(&mdp->pdev->dev, cmd->comps,
+			    cmd->num_comps);
+
+	atomic_dec(&mdp->job_count);
+	wake_up(&mdp->callback_wq);
+
+	mdp_cmdq_pkt_destroy(&cmd->pkt);
+	kfree(cmd->comps);
+	cmd->comps = NULL;
+	kfree(cmd);
+	cmd = NULL;
+}
+
+static void mdp_handle_cmdq_callback(struct mbox_client *cl, void *mssg)
+{
+	struct mdp_cmdq_cmd *cmd;
+	struct cmdq_cb_data *data;
+	struct mdp_dev *mdp;
+	struct device *dev;
+
+	if (!mssg) {
+		pr_info("%s:no callback data\n", __func__);
+		return;
+	}
+
+	data = (struct cmdq_cb_data *)mssg;
+	cmd = container_of(data->pkt, struct mdp_cmdq_cmd, pkt);
+	mdp = cmd->mdp;
+	dev = &mdp->pdev->dev;
+
+	if (cmd->mdp_ctx)
+		mdp_m2m_job_finish(cmd->mdp_ctx);
+
+	if (cmd->user_cmdq_cb) {
+		struct cmdq_cb_data user_cb_data;
+
+		user_cb_data.sta = data->sta;
+		user_cb_data.pkt = data->pkt;
+		cmd->user_cmdq_cb(user_cb_data);
+	}
+
+	INIT_WORK(&cmd->auto_release_work, mdp_auto_release_work);
+	if (!queue_work(mdp->clock_wq, &cmd->auto_release_work)) {
+		dev_err(dev, "%s:queue_work fail!\n", __func__);
+		mtk_mutex_unprepare(mdp->mdp_mutex[MDP_PIPE_RDMA0]);
+		mdp_comp_clocks_off(&mdp->pdev->dev, cmd->comps,
+				    cmd->num_comps);
+
+		atomic_dec(&mdp->job_count);
+		wake_up(&mdp->callback_wq);
+
+		mdp_cmdq_pkt_destroy(&cmd->pkt);
+		kfree(cmd->comps);
+		cmd->comps = NULL;
+		kfree(cmd);
+		cmd = NULL;
+	}
+}
+
+int mdp_cmdq_send(struct mdp_dev *mdp, struct mdp_cmdq_param *param)
+{
+	struct mdp_path *path = NULL;
+	struct mdp_cmdq_cmd *cmd = NULL;
+	struct mdp_comp *comps = NULL;
+	struct device *dev = &mdp->pdev->dev;
+	int i, ret;
+
+	atomic_inc(&mdp->job_count);
+	if (atomic_read(&mdp->suspended)) {
+		atomic_dec(&mdp->job_count);
+		return -ECANCELED;
+	}
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd) {
+		ret = -ENOMEM;
+		goto err_cancel_job;
+	}
+
+	ret = mdp_cmdq_pkt_create(mdp->cmdq_clt, &cmd->pkt, SZ_16K);
+	if (ret)
+		goto err_free_cmd;
+
+	comps = kcalloc(param->config->num_components, sizeof(*comps),
+			GFP_KERNEL);
+	if (!comps) {
+		ret = -ENOMEM;
+		goto err_destroy_pkt;
+	}
+
+	path = kzalloc(sizeof(*path), GFP_KERNEL);
+	if (!path) {
+		ret = -ENOMEM;
+		goto err_free_comps;
+	}
+
+	ret = mtk_mutex_prepare(mdp->mdp_mutex[MDP_PIPE_RDMA0]);
+	if (ret) {
+		dev_err(dev, "Fail to enable mutex clk\n");
+		goto err_free_path;
+	}
+
+	path->mdp_dev = mdp;
+	path->config = param->config;
+	path->param = param->param;
+	for (i = 0; i < param->param->num_outputs; i++) {
+		path->bounds[i].left = 0;
+		path->bounds[i].top = 0;
+		path->bounds[i].width =
+			param->param->outputs[i].buffer.format.width;
+		path->bounds[i].height =
+			param->param->outputs[i].buffer.format.height;
+		path->composes[i] = param->composes[i] ?
+			param->composes[i] : &path->bounds[i];
+	}
+
+	ret = mdp_path_ctx_init(mdp, path);
+	if (ret) {
+		dev_err(dev, "mdp_path_ctx_init error\n");
+		goto err_free_path;
+	}
+
+	ret = mdp_path_config(mdp, cmd, path);
+	if (ret) {
+		dev_err(dev, "mdp_path_config error\n");
+		goto err_free_path;
+	}
+	cmdq_pkt_finalize(&cmd->pkt);
+
+	for (i = 0; i < param->config->num_components; i++)
+		memcpy(&comps[i], path->comps[i].comp,
+		       sizeof(struct mdp_comp));
+
+	mdp->cmdq_clt->client.rx_callback = mdp_handle_cmdq_callback;
+	cmd->mdp = mdp;
+	cmd->user_cmdq_cb = param->cmdq_cb;
+	cmd->user_cb_data = param->cb_data;
+	cmd->comps = comps;
+	cmd->num_comps = param->config->num_components;
+	cmd->mdp_ctx = param->mdp_ctx;
+
+	ret = mdp_comp_clocks_on(&mdp->pdev->dev, cmd->comps, cmd->num_comps);
+	if (ret)
+		goto err_free_path;
+
+	dma_sync_single_for_device(mdp->cmdq_clt->chan->mbox->dev,
+				   cmd->pkt.pa_base, cmd->pkt.cmd_buf_size,
+				   DMA_TO_DEVICE);
+	ret = mbox_send_message(mdp->cmdq_clt->chan, &cmd->pkt);
+	if (ret < 0) {
+		dev_err(dev, "mbox send message fail %d!\n", ret);
+		goto err_clock_off;
+	}
+	mbox_client_txdone(mdp->cmdq_clt->chan, 0);
+
+	kfree(path);
+	return 0;
+
+err_clock_off:
+	mdp_comp_clocks_off(&mdp->pdev->dev, cmd->comps,
+			    cmd->num_comps);
+err_free_path:
+	mtk_mutex_unprepare(mdp->mdp_mutex[MDP_PIPE_RDMA0]);
+	kfree(path);
+err_free_comps:
+	kfree(comps);
+err_destroy_pkt:
+	mdp_cmdq_pkt_destroy(&cmd->pkt);
+err_free_cmd:
+	kfree(cmd);
+err_cancel_job:
+	atomic_dec(&mdp->job_count);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mdp_cmdq_send);
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.h b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.h
new file mode 100644
index 000000000..43475b862
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#ifndef __MTK_MDP3_CMDQ_H__
+#define __MTK_MDP3_CMDQ_H__
+
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <linux/soc/mediatek/mtk-cmdq.h>
+#include "mtk-img-ipi.h"
+
+struct platform_device *mdp_get_plat_device(struct platform_device *pdev);
+
+struct mdp_cmdq_param {
+	struct img_config *config;
+	struct img_ipi_frameparam *param;
+	const struct v4l2_rect *composes[IMG_MAX_HW_OUTPUTS];
+
+	void (*cmdq_cb)(struct cmdq_cb_data data);
+	void *cb_data;
+	void *mdp_ctx;
+};
+
+struct mdp_cmdq_cmd {
+	struct work_struct auto_release_work;
+	struct cmdq_pkt pkt;
+	s32 *event;
+	struct mdp_dev *mdp;
+	void (*user_cmdq_cb)(struct cmdq_cb_data data);
+	void *user_cb_data;
+	struct mdp_comp *comps;
+	void *mdp_ctx;
+	u8 num_comps;
+};
+
+struct mdp_dev;
+
+int mdp_cmdq_send(struct mdp_dev *mdp, struct mdp_cmdq_param *param);
+
+#endif  /* __MTK_MDP3_CMDQ_H__ */
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.c b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.c
new file mode 100644
index 000000000..9a3f46c1f
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.c
@@ -0,0 +1,1048 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/of_platform.h>
+#include <linux/of_address.h>
+#include <linux/pm_runtime.h>
+#include "mtk-mdp3-comp.h"
+#include "mtk-mdp3-core.h"
+#include "mtk-mdp3-regs.h"
+
+#include "mdp_reg_rdma.h"
+#include "mdp_reg_ccorr.h"
+#include "mdp_reg_rsz.h"
+#include "mdp_reg_wrot.h"
+#include "mdp_reg_wdma.h"
+
+static u32 mdp_comp_alias_id[MDP_COMP_TYPE_COUNT];
+
+static inline const struct mdp_platform_config *
+__get_plat_cfg(const struct mdp_comp_ctx *ctx)
+{
+	if (!ctx)
+		return NULL;
+
+	return ctx->comp->mdp_dev->mdp_data->mdp_cfg;
+}
+
+static s64 get_comp_flag(const struct mdp_comp_ctx *ctx)
+{
+	const struct mdp_platform_config *mdp_cfg = __get_plat_cfg(ctx);
+
+	if (mdp_cfg && mdp_cfg->rdma_rsz1_sram_sharing)
+		if (ctx->comp->id == MDP_COMP_RDMA0)
+			return BIT(MDP_COMP_RDMA0) | BIT(MDP_COMP_RSZ1);
+
+	return BIT(ctx->comp->id);
+}
+
+static int init_rdma(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
+{
+	const struct mdp_platform_config *mdp_cfg = __get_plat_cfg(ctx);
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	if (mdp_cfg && mdp_cfg->rdma_support_10bit) {
+		struct mdp_comp *prz1 = ctx->comp->mdp_dev->comp[MDP_COMP_RSZ1];
+
+		/* Disable RSZ1 */
+		if (ctx->comp->id == MDP_COMP_RDMA0 && prz1)
+			MM_REG_WRITE(cmd, subsys_id, prz1->reg_base, PRZ_ENABLE,
+				     0x0, BIT(0));
+	}
+
+	/* Reset RDMA */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_RESET, BIT(0), BIT(0));
+	MM_REG_POLL(cmd, subsys_id, base, MDP_RDMA_MON_STA_1, BIT(8), BIT(8));
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_RESET, 0x0, BIT(0));
+	return 0;
+}
+
+static int config_rdma_frame(struct mdp_comp_ctx *ctx,
+			     struct mdp_cmdq_cmd *cmd,
+			     const struct v4l2_rect *compose)
+{
+	const struct mdp_rdma_data *rdma = &ctx->param->rdma;
+	const struct mdp_platform_config *mdp_cfg = __get_plat_cfg(ctx);
+	u32 colorformat = ctx->input->buffer.format.colorformat;
+	bool block10bit = MDP_COLOR_IS_10BIT_PACKED(colorformat);
+	bool en_ufo = MDP_COLOR_IS_UFP(colorformat);
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	if (mdp_cfg && mdp_cfg->rdma_support_10bit) {
+		if (block10bit)
+			MM_REG_WRITE(cmd, subsys_id, base,
+				     MDP_RDMA_RESV_DUMMY_0, 0x7, 0x7);
+		else
+			MM_REG_WRITE(cmd, subsys_id, base,
+				     MDP_RDMA_RESV_DUMMY_0, 0x0, 0x7);
+	}
+
+	/* Setup smi control */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_GMCIF_CON,
+		     (7 <<  4) + //burst type to 8
+		     (1 << 16),  //enable pre-ultra
+		     0x00030071);
+
+	/* Setup source frame info */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_CON, rdma->src_ctrl,
+		     0x03C8FE0F);
+
+	if (mdp_cfg)
+		if (mdp_cfg->rdma_support_10bit && en_ufo) {
+			/* Setup source buffer base */
+			MM_REG_WRITE(cmd, subsys_id,
+				     base, MDP_RDMA_UFO_DEC_LENGTH_BASE_Y,
+				     rdma->ufo_dec_y, 0xFFFFFFFF);
+			MM_REG_WRITE(cmd, subsys_id,
+				     base, MDP_RDMA_UFO_DEC_LENGTH_BASE_C,
+				     rdma->ufo_dec_c, 0xFFFFFFFF);
+			/* Set 10bit source frame pitch */
+			if (block10bit)
+				MM_REG_WRITE(cmd, subsys_id,
+					     base, MDP_RDMA_MF_BKGD_SIZE_IN_PXL,
+					     rdma->mf_bkgd_in_pxl, 0x001FFFFF);
+		}
+
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_CON, rdma->control,
+		     0x1110);
+	/* Setup source buffer base */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_BASE_0, rdma->iova[0],
+		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_BASE_1, rdma->iova[1],
+		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_BASE_2, rdma->iova[2],
+		     0xFFFFFFFF);
+	/* Setup source buffer end */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_END_0,
+		     rdma->iova_end[0], 0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_END_1,
+		     rdma->iova_end[1], 0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_END_2,
+		     rdma->iova_end[2], 0xFFFFFFFF);
+	/* Setup source frame pitch */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_MF_BKGD_SIZE_IN_BYTE,
+		     rdma->mf_bkgd, 0x001FFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SF_BKGD_SIZE_IN_BYTE,
+		     rdma->sf_bkgd, 0x001FFFFF);
+	/* Setup color transform */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_TRANSFORM_0,
+		     rdma->transform, 0x0F110000);
+
+	return 0;
+}
+
+static int config_rdma_subfrm(struct mdp_comp_ctx *ctx,
+			      struct mdp_cmdq_cmd *cmd, u32 index)
+{
+	const struct mdp_rdma_subfrm *subfrm = &ctx->param->rdma.subfrms[index];
+	const struct img_comp_subfrm *csf = &ctx->param->subfrms[index];
+	const struct mdp_platform_config *mdp_cfg = __get_plat_cfg(ctx);
+	u32 colorformat = ctx->input->buffer.format.colorformat;
+	bool block10bit = MDP_COLOR_IS_10BIT_PACKED(colorformat);
+	bool en_ufo = MDP_COLOR_IS_UFP(colorformat);
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	/* Enable RDMA */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_EN, BIT(0), BIT(0));
+
+	/* Set Y pixel offset */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_OFFSET_0,
+		     subfrm->offset[0], 0xFFFFFFFF);
+
+	/* Set 10bit UFO mode */
+	if (mdp_cfg)
+		if (mdp_cfg->rdma_support_10bit && block10bit && en_ufo)
+			MM_REG_WRITE(cmd, subsys_id, base,
+				     MDP_RDMA_SRC_OFFSET_0_P,
+				     subfrm->offset_0_p, 0xFFFFFFFF);
+
+	/* Set U pixel offset */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_OFFSET_1,
+		     subfrm->offset[1], 0xFFFFFFFF);
+	/* Set V pixel offset */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_OFFSET_2,
+		     subfrm->offset[2], 0xFFFFFFFF);
+	/* Set source size */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_MF_SRC_SIZE, subfrm->src,
+		     0x1FFF1FFF);
+	/* Set target size */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_MF_CLIP_SIZE,
+		     subfrm->clip, 0x1FFF1FFF);
+	/* Set crop offset */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_MF_OFFSET_1,
+		     subfrm->clip_ofst, 0x003F001F);
+
+	if (mdp_cfg && mdp_cfg->rdma_upsample_repeat_only)
+		if ((csf->in.right - csf->in.left + 1) > 320)
+			MM_REG_WRITE(cmd, subsys_id, base,
+				     MDP_RDMA_RESV_DUMMY_0, BIT(2), BIT(2));
+
+	return 0;
+}
+
+static int wait_rdma_event(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
+{
+	struct device *dev = &ctx->comp->mdp_dev->pdev->dev;
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	if (ctx->comp->alias_id == 0)
+		MM_REG_WAIT(cmd, ctx->comp->gce_event[MDP_GCE_EVENT_EOF]);
+	else
+		dev_err(dev, "Do not support RDMA1_DONE event\n");
+
+	/* Disable RDMA */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_EN, 0x0, BIT(0));
+	return 0;
+}
+
+static const struct mdp_comp_ops rdma_ops = {
+	.get_comp_flag = get_comp_flag,
+	.init_comp = init_rdma,
+	.config_frame = config_rdma_frame,
+	.config_subfrm = config_rdma_subfrm,
+	.wait_comp_event = wait_rdma_event,
+};
+
+static int init_rsz(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
+{
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	/* Reset RSZ */
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_ENABLE, 0x10000, BIT(16));
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_ENABLE, 0x0, BIT(16));
+	/* Enable RSZ */
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_ENABLE, BIT(0), BIT(0));
+	return 0;
+}
+
+static int config_rsz_frame(struct mdp_comp_ctx *ctx,
+			    struct mdp_cmdq_cmd *cmd,
+			    const struct v4l2_rect *compose)
+{
+	const struct mdp_rsz_data *rsz = &ctx->param->rsz;
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	if (ctx->param->frame.bypass) {
+		/* Disable RSZ */
+		MM_REG_WRITE(cmd, subsys_id, base, PRZ_ENABLE, 0x0, BIT(0));
+		return 0;
+	}
+
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_CONTROL_1, rsz->control1,
+		     0x03FFFDF3);
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_CONTROL_2, rsz->control2,
+		     0x0FFFC290);
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_HORIZONTAL_COEFF_STEP,
+		     rsz->coeff_step_x, 0x007FFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_VERTICAL_COEFF_STEP,
+		     rsz->coeff_step_y, 0x007FFFFF);
+	return 0;
+}
+
+static int config_rsz_subfrm(struct mdp_comp_ctx *ctx,
+			     struct mdp_cmdq_cmd *cmd, u32 index)
+{
+	const struct mdp_rsz_subfrm *subfrm = &ctx->param->rsz.subfrms[index];
+	const struct img_comp_subfrm *csf = &ctx->param->subfrms[index];
+	const struct mdp_platform_config *mdp_cfg = __get_plat_cfg(ctx);
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_CONTROL_2, subfrm->control2,
+		     0x00003800);
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_INPUT_IMAGE, subfrm->src,
+		     0xFFFFFFFF);
+
+	if (mdp_cfg && mdp_cfg->rsz_disable_dcm_small_sample)
+		if ((csf->in.right - csf->in.left + 1) <= 16)
+			MM_REG_WRITE(cmd, subsys_id, base, PRZ_CONTROL_1,
+				     BIT(27), BIT(27));
+
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_LUMA_HORIZONTAL_INTEGER_OFFSET,
+		     csf->luma.left, 0xFFFF);
+	MM_REG_WRITE(cmd, subsys_id,
+		     base, PRZ_LUMA_HORIZONTAL_SUBPIXEL_OFFSET,
+		     csf->luma.left_subpix, 0x1FFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_LUMA_VERTICAL_INTEGER_OFFSET,
+		     csf->luma.top, 0xFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_LUMA_VERTICAL_SUBPIXEL_OFFSET,
+		     csf->luma.top_subpix, 0x1FFFFF);
+	MM_REG_WRITE(cmd, subsys_id,
+		     base, PRZ_CHROMA_HORIZONTAL_INTEGER_OFFSET,
+		     csf->chroma.left, 0xFFFF);
+	MM_REG_WRITE(cmd, subsys_id,
+		     base, PRZ_CHROMA_HORIZONTAL_SUBPIXEL_OFFSET,
+		     csf->chroma.left_subpix, 0x1FFFFF);
+
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_OUTPUT_IMAGE, subfrm->clip,
+		     0xFFFFFFFF);
+
+	return 0;
+}
+
+static int advance_rsz_subfrm(struct mdp_comp_ctx *ctx,
+			      struct mdp_cmdq_cmd *cmd, u32 index)
+{
+	const struct mdp_platform_config *mdp_cfg = __get_plat_cfg(ctx);
+
+	if (mdp_cfg && mdp_cfg->rsz_disable_dcm_small_sample) {
+		const struct img_comp_subfrm *csf = &ctx->param->subfrms[index];
+		phys_addr_t base = ctx->comp->reg_base;
+		u8 subsys_id = ctx->comp->subsys_id;
+
+		if ((csf->in.right - csf->in.left + 1) <= 16)
+			MM_REG_WRITE(cmd, subsys_id, base, PRZ_CONTROL_1, 0x0,
+				     BIT(27));
+	}
+
+	return 0;
+}
+
+static const struct mdp_comp_ops rsz_ops = {
+	.get_comp_flag = get_comp_flag,
+	.init_comp = init_rsz,
+	.config_frame = config_rsz_frame,
+	.config_subfrm = config_rsz_subfrm,
+	.advance_subfrm = advance_rsz_subfrm,
+};
+
+static int init_wrot(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
+{
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	/* Reset WROT */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_SOFT_RST, BIT(0), BIT(0));
+	MM_REG_POLL(cmd, subsys_id, base, VIDO_SOFT_RST_STAT, BIT(0), BIT(0));
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_SOFT_RST, 0x0, BIT(0));
+	MM_REG_POLL(cmd, subsys_id, base, VIDO_SOFT_RST_STAT, 0x0, BIT(0));
+	return 0;
+}
+
+static int config_wrot_frame(struct mdp_comp_ctx *ctx,
+			     struct mdp_cmdq_cmd *cmd,
+			     const struct v4l2_rect *compose)
+{
+	const struct mdp_wrot_data *wrot = &ctx->param->wrot;
+	const struct mdp_platform_config *mdp_cfg = __get_plat_cfg(ctx);
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	/* Write frame base address */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_BASE_ADDR, wrot->iova[0],
+		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_BASE_ADDR_C, wrot->iova[1],
+		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_BASE_ADDR_V, wrot->iova[2],
+		     0xFFFFFFFF);
+	/* Write frame related registers */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_CTRL, wrot->control,
+		     0xF131510F);
+	/* Write frame Y pitch */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_STRIDE, wrot->stride[0],
+		     0x0000FFFF);
+	/* Write frame UV pitch */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_STRIDE_C, wrot->stride[1],
+		     0xFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_STRIDE_V, wrot->stride[2],
+		     0xFFFF);
+	/* Write matrix control */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_MAT_CTRL, wrot->mat_ctrl, 0xF3);
+
+	/* Set the fixed ALPHA as 0xFF */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_DITHER, 0xFF000000,
+		     0xFF000000);
+	/* Set VIDO_EOL_SEL */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_RSV_1, BIT(31), BIT(31));
+	/* Set VIDO_FIFO_TEST */
+	if (wrot->fifo_test != 0)
+		MM_REG_WRITE(cmd, subsys_id, base, VIDO_FIFO_TEST,
+			     wrot->fifo_test, 0xFFF);
+	/* Filter enable */
+	if (mdp_cfg && mdp_cfg->wrot_filter_constraint)
+		MM_REG_WRITE(cmd, subsys_id, base, VIDO_MAIN_BUF_SIZE,
+			     wrot->filter, 0x77);
+
+	return 0;
+}
+
+static int config_wrot_subfrm(struct mdp_comp_ctx *ctx,
+			      struct mdp_cmdq_cmd *cmd, u32 index)
+{
+	const struct mdp_wrot_subfrm *subfrm = &ctx->param->wrot.subfrms[index];
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	/* Write Y pixel offset */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_OFST_ADDR,
+		     subfrm->offset[0], 0x0FFFFFFF);
+	/* Write U pixel offset */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_OFST_ADDR_C,
+		     subfrm->offset[1], 0x0FFFFFFF);
+	/* Write V pixel offset */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_OFST_ADDR_V,
+		     subfrm->offset[2], 0x0FFFFFFF);
+	/* Write source size */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_IN_SIZE, subfrm->src,
+		     0x1FFF1FFF);
+	/* Write target size */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_TAR_SIZE, subfrm->clip,
+		     0x1FFF1FFF);
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_CROP_OFST, subfrm->clip_ofst,
+		     0x1FFF1FFF);
+
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_MAIN_BUF_SIZE,
+		     subfrm->main_buf, 0x1FFF7F00);
+
+	/* Enable WROT */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_ROT_EN, BIT(0), BIT(0));
+
+	return 0;
+}
+
+static int wait_wrot_event(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
+{
+	const struct mdp_platform_config *mdp_cfg = __get_plat_cfg(ctx);
+	struct device *dev = &ctx->comp->mdp_dev->pdev->dev;
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	if (ctx->comp->alias_id == 0)
+		MM_REG_WAIT(cmd, ctx->comp->gce_event[MDP_GCE_EVENT_EOF]);
+	else
+		dev_err(dev, "Do not support WROT1_DONE event\n");
+
+	if (mdp_cfg && mdp_cfg->wrot_filter_constraint)
+		MM_REG_WRITE(cmd, subsys_id, base, VIDO_MAIN_BUF_SIZE, 0x0,
+			     0x77);
+
+	/* Disable WROT */
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_ROT_EN, 0x0, BIT(0));
+
+	return 0;
+}
+
+static const struct mdp_comp_ops wrot_ops = {
+	.get_comp_flag = get_comp_flag,
+	.init_comp = init_wrot,
+	.config_frame = config_wrot_frame,
+	.config_subfrm = config_wrot_subfrm,
+	.wait_comp_event = wait_wrot_event,
+};
+
+static int init_wdma(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
+{
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	/* Reset WDMA */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_RST, BIT(0), BIT(0));
+	MM_REG_POLL(cmd, subsys_id, base, WDMA_FLOW_CTRL_DBG, BIT(0), BIT(0));
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_RST, 0x0, BIT(0));
+	return 0;
+}
+
+static int config_wdma_frame(struct mdp_comp_ctx *ctx,
+			     struct mdp_cmdq_cmd *cmd,
+			     const struct v4l2_rect *compose)
+{
+	const struct mdp_wdma_data *wdma = &ctx->param->wdma;
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_BUF_CON2, 0x10101050,
+		     0xFFFFFFFF);
+
+	/* Setup frame information */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_CFG, wdma->wdma_cfg,
+		     0x0F01B8F0);
+	/* Setup frame base address */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_ADDR,   wdma->iova[0],
+		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_U_ADDR, wdma->iova[1],
+		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_V_ADDR, wdma->iova[2],
+		     0xFFFFFFFF);
+	/* Setup Y pitch */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_W_IN_BYTE,
+		     wdma->w_in_byte, 0x0000FFFF);
+	/* Setup UV pitch */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_UV_PITCH,
+		     wdma->uv_stride, 0x0000FFFF);
+	/* Set the fixed ALPHA as 0xFF */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_ALPHA, 0x800000FF,
+		     0x800000FF);
+
+	return 0;
+}
+
+static int config_wdma_subfrm(struct mdp_comp_ctx *ctx,
+			      struct mdp_cmdq_cmd *cmd, u32 index)
+{
+	const struct mdp_wdma_subfrm *subfrm = &ctx->param->wdma.subfrms[index];
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	/* Write Y pixel offset */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_ADDR_OFFSET,
+		     subfrm->offset[0], 0x0FFFFFFF);
+	/* Write U pixel offset */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_U_ADDR_OFFSET,
+		     subfrm->offset[1], 0x0FFFFFFF);
+	/* Write V pixel offset */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_V_ADDR_OFFSET,
+		     subfrm->offset[2], 0x0FFFFFFF);
+	/* Write source size */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_SRC_SIZE, subfrm->src,
+		     0x3FFF3FFF);
+	/* Write target size */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_CLIP_SIZE, subfrm->clip,
+		     0x3FFF3FFF);
+	/* Write clip offset */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_CLIP_COORD, subfrm->clip_ofst,
+		     0x3FFF3FFF);
+
+	/* Enable WDMA */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_EN, BIT(0), BIT(0));
+
+	return 0;
+}
+
+static int wait_wdma_event(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
+{
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	MM_REG_WAIT(cmd, ctx->comp->gce_event[MDP_GCE_EVENT_EOF]);
+	/* Disable WDMA */
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_EN, 0x0, BIT(0));
+	return 0;
+}
+
+static const struct mdp_comp_ops wdma_ops = {
+	.get_comp_flag = get_comp_flag,
+	.init_comp = init_wdma,
+	.config_frame = config_wdma_frame,
+	.config_subfrm = config_wdma_subfrm,
+	.wait_comp_event = wait_wdma_event,
+};
+
+static int init_ccorr(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
+{
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+
+	/* CCORR enable */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_CCORR_EN, BIT(0), BIT(0));
+	/* Relay mode */
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_CCORR_CFG, BIT(0), BIT(0));
+	return 0;
+}
+
+static int config_ccorr_subfrm(struct mdp_comp_ctx *ctx,
+			       struct mdp_cmdq_cmd *cmd, u32 index)
+{
+	const struct img_comp_subfrm *csf = &ctx->param->subfrms[index];
+	phys_addr_t base = ctx->comp->reg_base;
+	u8 subsys_id = ctx->comp->subsys_id;
+	u32 hsize, vsize;
+
+	hsize = csf->in.right - csf->in.left + 1;
+	vsize = csf->in.bottom - csf->in.top + 1;
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_CCORR_SIZE,
+		     (hsize << 16) + (vsize <<  0), 0x1FFF1FFF);
+	return 0;
+}
+
+static const struct mdp_comp_ops ccorr_ops = {
+	.get_comp_flag = get_comp_flag,
+	.init_comp = init_ccorr,
+	.config_subfrm = config_ccorr_subfrm,
+};
+
+static const struct mdp_comp_ops *mdp_comp_ops[MDP_COMP_TYPE_COUNT] = {
+	[MDP_COMP_TYPE_RDMA] =		&rdma_ops,
+	[MDP_COMP_TYPE_RSZ] =		&rsz_ops,
+	[MDP_COMP_TYPE_WROT] =		&wrot_ops,
+	[MDP_COMP_TYPE_WDMA] =		&wdma_ops,
+	[MDP_COMP_TYPE_CCORR] =		&ccorr_ops,
+};
+
+struct mdp_comp_match {
+	enum mdp_comp_type	type;
+	u32			alias_id;
+};
+
+static const struct mdp_comp_match mdp_comp_matches[MDP_MAX_COMP_COUNT] = {
+	[MDP_COMP_WPEI] =	{ MDP_COMP_TYPE_WPEI, 0 },
+	[MDP_COMP_WPEO] =	{ MDP_COMP_TYPE_EXTO, 2 },
+	[MDP_COMP_WPEI2] =	{ MDP_COMP_TYPE_WPEI, 1 },
+	[MDP_COMP_WPEO2] =	{ MDP_COMP_TYPE_EXTO, 3 },
+	[MDP_COMP_ISP_IMGI] =	{ MDP_COMP_TYPE_IMGI, 0 },
+	[MDP_COMP_ISP_IMGO] =	{ MDP_COMP_TYPE_EXTO, 0 },
+	[MDP_COMP_ISP_IMG2O] =	{ MDP_COMP_TYPE_EXTO, 1 },
+
+	[MDP_COMP_CAMIN] =	{ MDP_COMP_TYPE_DL_PATH, 0 },
+	[MDP_COMP_CAMIN2] =	{ MDP_COMP_TYPE_DL_PATH, 1 },
+	[MDP_COMP_RDMA0] =	{ MDP_COMP_TYPE_RDMA, 0 },
+	[MDP_COMP_CCORR0] =	{ MDP_COMP_TYPE_CCORR, 0 },
+	[MDP_COMP_RSZ0] =	{ MDP_COMP_TYPE_RSZ, 0 },
+	[MDP_COMP_RSZ1] =	{ MDP_COMP_TYPE_RSZ, 1 },
+	[MDP_COMP_PATH0_SOUT] =	{ MDP_COMP_TYPE_PATH, 0 },
+	[MDP_COMP_PATH1_SOUT] =	{ MDP_COMP_TYPE_PATH, 1 },
+	[MDP_COMP_WROT0] =	{ MDP_COMP_TYPE_WROT, 0 },
+	[MDP_COMP_WDMA] =	{ MDP_COMP_TYPE_WDMA, 0 },
+};
+
+static const struct of_device_id mdp_comp_dt_ids[] = {
+	{
+		.compatible = "mediatek,mt8183-mdp3-rdma",
+		.data = (void *)MDP_COMP_TYPE_RDMA,
+	}, {
+		.compatible = "mediatek,mt8183-mdp3-ccorr",
+		.data = (void *)MDP_COMP_TYPE_CCORR,
+	}, {
+		.compatible = "mediatek,mt8183-mdp3-rsz",
+		.data = (void *)MDP_COMP_TYPE_RSZ,
+	}, {
+		.compatible = "mediatek,mt8183-mdp3-wrot",
+		.data = (void *)MDP_COMP_TYPE_WROT,
+	}, {
+		.compatible = "mediatek,mt8183-mdp3-wdma",
+		.data = (void *)MDP_COMP_TYPE_WDMA,
+	},
+	{}
+};
+
+static const struct of_device_id mdp_sub_comp_dt_ids[] = {
+	{
+		.compatible = "mediatek,mt8183-mdp3-wdma",
+		.data = (void *)MDP_COMP_TYPE_PATH,
+	}, {
+		.compatible = "mediatek,mt8183-mdp3-wrot",
+		.data = (void *)MDP_COMP_TYPE_PATH,
+	},
+	{}
+};
+
+/* Used to describe the item order in MDP property */
+struct mdp_comp_info {
+	u32	clk_num;
+	u32	clk_ofst;
+	u32	dts_reg_ofst;
+};
+
+static const struct mdp_comp_info mdp_comp_dt_info[MDP_MAX_COMP_COUNT] = {
+	[MDP_COMP_RDMA0]	= {2, 0, 0},
+	[MDP_COMP_RSZ0]		= {1, 0, 0},
+	[MDP_COMP_WROT0]	= {1, 0, 0},
+	[MDP_COMP_WDMA]		= {1, 0, 0},
+	[MDP_COMP_CCORR0]	= {1, 0, 0},
+};
+
+static inline bool is_dma_capable(const enum mdp_comp_type type)
+{
+	return (type == MDP_COMP_TYPE_RDMA ||
+		type == MDP_COMP_TYPE_WROT ||
+		type == MDP_COMP_TYPE_WDMA);
+}
+
+static inline bool is_bypass_gce_event(const enum mdp_comp_type type)
+{
+	/*
+	 * Subcomponent PATH is only used for the direction of data flow and
+	 * dose not need to wait for GCE event.
+	 */
+	return (type == MDP_COMP_TYPE_PATH);
+}
+
+static int mdp_comp_get_id(enum mdp_comp_type type, int alias_id)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mdp_comp_matches); i++)
+		if (mdp_comp_matches[i].type == type &&
+		    mdp_comp_matches[i].alias_id == alias_id)
+			return i;
+	return -ENODEV;
+}
+
+int mdp_comp_clock_on(struct device *dev, struct mdp_comp *comp)
+{
+	int i, ret;
+
+	if (comp->comp_dev) {
+		ret = pm_runtime_resume_and_get(comp->comp_dev);
+		if (ret < 0) {
+			dev_err(dev,
+				"Failed to get power, err %d. type:%d id:%d\n",
+				ret, comp->type, comp->id);
+			return ret;
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(comp->clks); i++) {
+		if (IS_ERR_OR_NULL(comp->clks[i]))
+			continue;
+		ret = clk_prepare_enable(comp->clks[i]);
+		if (ret) {
+			dev_err(dev,
+				"Failed to enable clk %d. type:%d id:%d\n",
+				i, comp->type, comp->id);
+			goto err_revert;
+		}
+	}
+
+	return 0;
+
+err_revert:
+	while (--i >= 0) {
+		if (IS_ERR_OR_NULL(comp->clks[i]))
+			continue;
+		clk_disable_unprepare(comp->clks[i]);
+	}
+	if (comp->comp_dev)
+		pm_runtime_put_sync(comp->comp_dev);
+
+	return ret;
+}
+
+void mdp_comp_clock_off(struct device *dev, struct mdp_comp *comp)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(comp->clks); i++) {
+		if (IS_ERR_OR_NULL(comp->clks[i]))
+			continue;
+		clk_disable_unprepare(comp->clks[i]);
+	}
+
+	if (comp->comp_dev)
+		pm_runtime_put(comp->comp_dev);
+}
+
+int mdp_comp_clocks_on(struct device *dev, struct mdp_comp *comps, int num)
+{
+	int i, ret;
+
+	for (i = 0; i < num; i++) {
+		ret = mdp_comp_clock_on(dev, &comps[i]);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+void mdp_comp_clocks_off(struct device *dev, struct mdp_comp *comps, int num)
+{
+	int i;
+
+	for (i = 0; i < num; i++)
+		mdp_comp_clock_off(dev, &comps[i]);
+}
+
+static int mdp_get_subsys_id(struct device *dev, struct device_node *node,
+			     struct mdp_comp *comp)
+{
+	struct platform_device *comp_pdev;
+	struct cmdq_client_reg  cmdq_reg;
+	int ret = 0;
+	int index = 0;
+
+	if (!dev || !node || !comp)
+		return -EINVAL;
+
+	comp_pdev = of_find_device_by_node(node);
+
+	if (!comp_pdev) {
+		dev_err(dev, "get comp_pdev fail! comp id=%d type=%d\n",
+			comp->id, comp->type);
+		return -ENODEV;
+	}
+
+	index = mdp_comp_dt_info[comp->id].dts_reg_ofst;
+	ret = cmdq_dev_get_client_reg(&comp_pdev->dev, &cmdq_reg, index);
+	if (ret != 0) {
+		dev_err(&comp_pdev->dev, "cmdq_dev_get_subsys fail!\n");
+		put_device(&comp_pdev->dev);
+		return -EINVAL;
+	}
+
+	comp->subsys_id = cmdq_reg.subsys;
+	dev_dbg(&comp_pdev->dev, "subsys id=%d\n", cmdq_reg.subsys);
+	put_device(&comp_pdev->dev);
+
+	return 0;
+}
+
+static void __mdp_comp_init(struct mdp_dev *mdp, struct device_node *node,
+			    struct mdp_comp *comp)
+{
+	struct resource res;
+	phys_addr_t base;
+	int index = mdp_comp_dt_info[comp->id].dts_reg_ofst;
+
+	if (of_address_to_resource(node, index, &res) < 0)
+		base = 0L;
+	else
+		base = res.start;
+
+	comp->mdp_dev = mdp;
+	comp->regs = of_iomap(node, 0);
+	comp->reg_base = base;
+}
+
+static int mdp_comp_init(struct mdp_dev *mdp, struct device_node *node,
+			 struct mdp_comp *comp, enum mtk_mdp_comp_id id)
+{
+	struct device *dev = &mdp->pdev->dev;
+	int clk_num;
+	int clk_ofst;
+	int i;
+	s32 event;
+
+	if (id < 0 || id >= MDP_MAX_COMP_COUNT) {
+		dev_err(dev, "Invalid component id %d\n", id);
+		return -EINVAL;
+	}
+
+	comp->id = id;
+	comp->type = mdp_comp_matches[id].type;
+	comp->alias_id = mdp_comp_matches[id].alias_id;
+	comp->ops = mdp_comp_ops[comp->type];
+	__mdp_comp_init(mdp, node, comp);
+
+	clk_num = mdp_comp_dt_info[id].clk_num;
+	clk_ofst = mdp_comp_dt_info[id].clk_ofst;
+
+	for (i = 0; i < clk_num; i++) {
+		comp->clks[i] = of_clk_get(node, i + clk_ofst);
+		if (IS_ERR(comp->clks[i]))
+			break;
+	}
+
+	mdp_get_subsys_id(dev, node, comp);
+
+	/* Set GCE SOF event */
+	if (is_bypass_gce_event(comp->type) ||
+	    of_property_read_u32_index(node, "mediatek,gce-events",
+				       MDP_GCE_EVENT_SOF, &event))
+		event = MDP_GCE_NO_EVENT;
+
+	comp->gce_event[MDP_GCE_EVENT_SOF] = event;
+
+	/* Set GCE EOF event */
+	if (is_dma_capable(comp->type)) {
+		if (of_property_read_u32_index(node, "mediatek,gce-events",
+					       MDP_GCE_EVENT_EOF, &event)) {
+			dev_err(dev, "Component id %d has no EOF\n", id);
+			return -EINVAL;
+		}
+	} else {
+		event = MDP_GCE_NO_EVENT;
+	}
+
+	comp->gce_event[MDP_GCE_EVENT_EOF] = event;
+
+	return 0;
+}
+
+static void mdp_comp_deinit(struct mdp_comp *comp)
+{
+	if (!comp)
+		return;
+
+	if (comp->regs)
+		iounmap(comp->regs);
+}
+
+static struct mdp_comp *mdp_comp_create(struct mdp_dev *mdp,
+					struct device_node *node,
+					enum mtk_mdp_comp_id id)
+{
+	struct device *dev = &mdp->pdev->dev;
+	struct mdp_comp *comp;
+	int ret;
+
+	if (mdp->comp[id])
+		return ERR_PTR(-EEXIST);
+
+	comp = devm_kzalloc(dev, sizeof(*comp), GFP_KERNEL);
+	if (!comp)
+		return ERR_PTR(-ENOMEM);
+
+	ret = mdp_comp_init(mdp, node, comp, id);
+	if (ret) {
+		devm_kfree(dev, comp);
+		return ERR_PTR(ret);
+	}
+	mdp->comp[id] = comp;
+	mdp->comp[id]->mdp_dev = mdp;
+
+	dev_dbg(dev, "%s type:%d alias:%d id:%d base:%#x regs:%p\n",
+		dev->of_node->name, comp->type, comp->alias_id, id,
+		(u32)comp->reg_base, comp->regs);
+	return comp;
+}
+
+static int mdp_comp_sub_create(struct mdp_dev *mdp)
+{
+	struct device *dev = &mdp->pdev->dev;
+	struct device_node *node, *parent;
+
+	parent = dev->of_node->parent;
+
+	for_each_child_of_node(parent, node) {
+		const struct of_device_id *of_id;
+		enum mdp_comp_type type;
+		int id, alias_id;
+		struct mdp_comp *comp;
+
+		of_id = of_match_node(mdp_sub_comp_dt_ids, node);
+		if (!of_id)
+			continue;
+		if (!of_device_is_available(node)) {
+			dev_dbg(dev, "Skipping disabled sub comp. %pOF\n",
+				node);
+			continue;
+		}
+
+		type = (enum mdp_comp_type)(uintptr_t)of_id->data;
+		alias_id = mdp_comp_alias_id[type];
+		id = mdp_comp_get_id(type, alias_id);
+		if (id < 0) {
+			dev_err(dev,
+				"Fail to get sub comp. id: type %d alias %d\n",
+				type, alias_id);
+			return -EINVAL;
+		}
+		mdp_comp_alias_id[type]++;
+
+		comp = mdp_comp_create(mdp, node, id);
+		if (IS_ERR(comp))
+			return PTR_ERR(comp);
+	}
+
+	return 0;
+}
+
+void mdp_comp_destroy(struct mdp_dev *mdp)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mdp->comp); i++) {
+		if (mdp->comp[i]) {
+			pm_runtime_disable(mdp->comp[i]->comp_dev);
+			mdp_comp_deinit(mdp->comp[i]);
+			devm_kfree(mdp->comp[i]->comp_dev, mdp->comp[i]);
+			mdp->comp[i] = NULL;
+		}
+	}
+}
+
+int mdp_comp_config(struct mdp_dev *mdp)
+{
+	struct device *dev = &mdp->pdev->dev;
+	struct device_node *node, *parent;
+	struct platform_device *pdev;
+	int ret;
+
+	memset(mdp_comp_alias_id, 0, sizeof(mdp_comp_alias_id));
+
+	parent = dev->of_node->parent;
+	/* Iterate over sibling MDP function blocks */
+	for_each_child_of_node(parent, node) {
+		const struct of_device_id *of_id;
+		enum mdp_comp_type type;
+		int id, alias_id;
+		struct mdp_comp *comp;
+
+		of_id = of_match_node(mdp_comp_dt_ids, node);
+		if (!of_id)
+			continue;
+
+		if (!of_device_is_available(node)) {
+			dev_dbg(dev, "Skipping disabled component %pOF\n",
+				node);
+			continue;
+		}
+
+		type = (enum mdp_comp_type)(uintptr_t)of_id->data;
+		alias_id = mdp_comp_alias_id[type];
+		id = mdp_comp_get_id(type, alias_id);
+		if (id < 0) {
+			dev_err(dev,
+				"Fail to get component id: type %d alias %d\n",
+				type, alias_id);
+			continue;
+		}
+		mdp_comp_alias_id[type]++;
+
+		comp = mdp_comp_create(mdp, node, id);
+		if (IS_ERR(comp)) {
+			ret = PTR_ERR(comp);
+			goto err_init_comps;
+		}
+
+		/* Only DMA capable components need the pm control */
+		comp->comp_dev = NULL;
+		if (!is_dma_capable(comp->type))
+			continue;
+
+		pdev = of_find_device_by_node(node);
+		if (!pdev) {
+			dev_warn(dev, "can't find platform device of node:%s\n",
+				 node->name);
+			return -ENODEV;
+		}
+
+		comp->comp_dev = &pdev->dev;
+		pm_runtime_enable(comp->comp_dev);
+	}
+
+	ret = mdp_comp_sub_create(mdp);
+	if (ret)
+		goto err_init_comps;
+
+	return 0;
+
+err_init_comps:
+	mdp_comp_destroy(mdp);
+	return ret;
+}
+
+int mdp_comp_ctx_config(struct mdp_dev *mdp, struct mdp_comp_ctx *ctx,
+			const struct img_compparam *param,
+			const struct img_ipi_frameparam *frame)
+{
+	struct device *dev = &mdp->pdev->dev;
+	int i;
+
+	if (param->type < 0 || param->type >= MDP_MAX_COMP_COUNT) {
+		dev_err(dev, "Invalid component id %d", param->type);
+		return -EINVAL;
+	}
+
+	ctx->comp = mdp->comp[param->type];
+	if (!ctx->comp) {
+		dev_err(dev, "Uninit component id %d", param->type);
+		return -EINVAL;
+	}
+
+	ctx->param = param;
+	ctx->input = &frame->inputs[param->input];
+	for (i = 0; i < param->num_outputs; i++)
+		ctx->outputs[i] = &frame->outputs[param->outputs[i]];
+	return 0;
+}
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.h b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.h
new file mode 100644
index 000000000..dc48f55ac
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.h
@@ -0,0 +1,186 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#ifndef __MTK_MDP3_COMP_H__
+#define __MTK_MDP3_COMP_H__
+
+#include "mtk-mdp3-cmdq.h"
+
+#define MM_REG_WRITE_MASK(cmd, id, base, ofst, val, mask, ...)	\
+	cmdq_pkt_write_mask(&((cmd)->pkt), id,			\
+		(base) + (ofst), (val), (mask), ##__VA_ARGS__)
+
+#define MM_REG_WRITE(cmd, id, base, ofst, val, mask, ...)	\
+do {								\
+	typeof(mask) (m) = (mask);				\
+	MM_REG_WRITE_MASK(cmd, id, base, ofst, val,		\
+		(((m) & (ofst##_MASK)) == (ofst##_MASK)) ?	\
+			(0xffffffff) : (m), ##__VA_ARGS__);	\
+} while (0)
+
+#define MM_REG_WAIT(cmd, evt)					\
+do {								\
+	typeof(cmd) (c) = (cmd);				\
+	typeof(evt) (e) = (evt);				\
+	cmdq_pkt_wfe(&((c)->pkt), (e), true);			\
+} while (0)
+
+#define MM_REG_WAIT_NO_CLEAR(cmd, evt)				\
+do {								\
+	typeof(cmd) (c) = (cmd);				\
+	typeof(evt) (e) = (evt);				\
+	cmdq_pkt_wfe(&((c)->pkt), (e), false);			\
+} while (0)
+
+#define MM_REG_CLEAR(cmd, evt)					\
+do {								\
+	typeof(cmd) (c) = (cmd);				\
+	typeof(evt) (e) = (evt);				\
+	cmdq_pkt_clear_event(&((c)->pkt), (e));			\
+} while (0)
+
+#define MM_REG_SET_EVENT(cmd, evt)				\
+do {								\
+	typeof(cmd) (c) = (cmd);				\
+	typeof(evt) (e) = (evt);				\
+	cmdq_pkt_set_event(&((c)->pkt), (e));			\
+} while (0)
+
+#define MM_REG_POLL_MASK(cmd, id, base, ofst, val, _mask, ...)	\
+do {								\
+	typeof(_mask) (_m) = (_mask);				\
+	cmdq_pkt_poll_mask(&((cmd)->pkt), id,			\
+		(base) + (ofst), (val), (_m), ##__VA_ARGS__);	\
+} while (0)
+
+#define MM_REG_POLL(cmd, id, base, ofst, val, mask, ...)	\
+do {								\
+	typeof(mask) (m) = (mask);				\
+	MM_REG_POLL_MASK((cmd), id, base, ofst, val,		\
+		(((m) & (ofst##_MASK)) == (ofst##_MASK)) ?	\
+			(0xffffffff) : (m), ##__VA_ARGS__);	\
+} while (0)
+
+enum mtk_mdp_comp_id {
+	MDP_COMP_NONE = -1,	/* Invalid engine */
+
+	/* ISP */
+	MDP_COMP_WPEI = 0,
+	MDP_COMP_WPEO,		/* 1 */
+	MDP_COMP_WPEI2,		/* 2 */
+	MDP_COMP_WPEO2,		/* 3 */
+	MDP_COMP_ISP_IMGI,	/* 4 */
+	MDP_COMP_ISP_IMGO,	/* 5 */
+	MDP_COMP_ISP_IMG2O,	/* 6 */
+
+	/* IPU */
+	MDP_COMP_IPUI,		/* 7 */
+	MDP_COMP_IPUO,		/* 8 */
+
+	/* MDP */
+	MDP_COMP_CAMIN,		/* 9 */
+	MDP_COMP_CAMIN2,	/* 10 */
+	MDP_COMP_RDMA0,		/* 11 */
+	MDP_COMP_AAL0,		/* 12 */
+	MDP_COMP_CCORR0,	/* 13 */
+	MDP_COMP_RSZ0,		/* 14 */
+	MDP_COMP_RSZ1,		/* 15 */
+	MDP_COMP_TDSHP0,	/* 16 */
+	MDP_COMP_COLOR0,	/* 17 */
+	MDP_COMP_PATH0_SOUT,	/* 18 */
+	MDP_COMP_PATH1_SOUT,	/* 19 */
+	MDP_COMP_WROT0,		/* 20 */
+	MDP_COMP_WDMA,		/* 21 */
+
+	/* Dummy Engine */
+	MDP_COMP_RDMA1,		/* 22 */
+	MDP_COMP_RSZ2,		/* 23 */
+	MDP_COMP_TDSHP1,	/* 24 */
+	MDP_COMP_WROT1,		/* 25 */
+
+	MDP_MAX_COMP_COUNT	/* ALWAYS keep at the end */
+};
+
+enum mdp_comp_type {
+	MDP_COMP_TYPE_INVALID = 0,
+
+	MDP_COMP_TYPE_RDMA,
+	MDP_COMP_TYPE_RSZ,
+	MDP_COMP_TYPE_WROT,
+	MDP_COMP_TYPE_WDMA,
+	MDP_COMP_TYPE_PATH,
+
+	MDP_COMP_TYPE_TDSHP,
+	MDP_COMP_TYPE_COLOR,
+	MDP_COMP_TYPE_DRE,
+	MDP_COMP_TYPE_CCORR,
+	MDP_COMP_TYPE_HDR,
+
+	MDP_COMP_TYPE_IMGI,
+	MDP_COMP_TYPE_WPEI,
+	MDP_COMP_TYPE_EXTO,	/* External path */
+	MDP_COMP_TYPE_DL_PATH,	/* Direct-link path */
+
+	MDP_COMP_TYPE_COUNT	/* ALWAYS keep at the end */
+};
+
+#define MDP_GCE_NO_EVENT (-1)
+enum {
+	MDP_GCE_EVENT_SOF = 0,
+	MDP_GCE_EVENT_EOF = 1,
+	MDP_GCE_EVENT_MAX,
+};
+
+struct mdp_comp_ops;
+
+struct mdp_comp {
+	struct mdp_dev			*mdp_dev;
+	void __iomem			*regs;
+	phys_addr_t			reg_base;
+	u8				subsys_id;
+	struct clk			*clks[6];
+	struct device			*comp_dev;
+	enum mdp_comp_type		type;
+	enum mtk_mdp_comp_id		id;
+	u32				alias_id;
+	s32				gce_event[MDP_GCE_EVENT_MAX];
+	const struct mdp_comp_ops	*ops;
+};
+
+struct mdp_comp_ctx {
+	struct mdp_comp			*comp;
+	const struct img_compparam	*param;
+	const struct img_input		*input;
+	const struct img_output		*outputs[IMG_MAX_HW_OUTPUTS];
+};
+
+struct mdp_comp_ops {
+	s64 (*get_comp_flag)(const struct mdp_comp_ctx *ctx);
+	int (*init_comp)(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd);
+	int (*config_frame)(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd,
+			    const struct v4l2_rect *compose);
+	int (*config_subfrm)(struct mdp_comp_ctx *ctx,
+			     struct mdp_cmdq_cmd *cmd, u32 index);
+	int (*wait_comp_event)(struct mdp_comp_ctx *ctx,
+			       struct mdp_cmdq_cmd *cmd);
+	int (*advance_subfrm)(struct mdp_comp_ctx *ctx,
+			      struct mdp_cmdq_cmd *cmd, u32 index);
+	int (*post_process)(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd);
+};
+
+struct mdp_dev;
+
+int mdp_comp_config(struct mdp_dev *mdp);
+void mdp_comp_destroy(struct mdp_dev *mdp);
+int mdp_comp_clock_on(struct device *dev, struct mdp_comp *comp);
+void mdp_comp_clock_off(struct device *dev, struct mdp_comp *comp);
+int mdp_comp_clocks_on(struct device *dev, struct mdp_comp *comps, int num);
+void mdp_comp_clocks_off(struct device *dev, struct mdp_comp *comps, int num);
+int mdp_comp_ctx_config(struct mdp_dev *mdp, struct mdp_comp_ctx *ctx,
+			const struct img_compparam *param,
+			const struct img_ipi_frameparam *frame);
+
+#endif  /* __MTK_MDP3_COMP_H__ */
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.c b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.c
new file mode 100644
index 000000000..97edcd9d1
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.c
@@ -0,0 +1,360 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/remoteproc.h>
+#include <linux/remoteproc/mtk_scp.h>
+#include <media/videobuf2-dma-contig.h>
+#include "mtk-mdp3-core.h"
+#include "mtk-mdp3-m2m.h"
+
+static const struct mdp_platform_config mt8183_plat_cfg = {
+	.rdma_support_10bit		= true,
+	.rdma_rsz1_sram_sharing		= true,
+	.rdma_upsample_repeat_only	= true,
+	.rsz_disable_dcm_small_sample	= false,
+	.wrot_filter_constraint		= false,
+};
+
+static const struct of_device_id mt8183_mdp_probe_infra[MDP_INFRA_MAX] = {
+	[MDP_INFRA_MMSYS] = { .compatible = "mediatek,mt8183-mmsys" },
+	[MDP_INFRA_MUTEX] = { .compatible = "mediatek,mt8183-disp-mutex" },
+	[MDP_INFRA_SCP] = { .compatible = "mediatek,mt8183-scp" }
+};
+
+static const u32 mt8183_mutex_idx[MDP_MAX_COMP_COUNT] = {
+	[MDP_COMP_RDMA0] = MUTEX_MOD_IDX_MDP_RDMA0,
+	[MDP_COMP_RSZ0] = MUTEX_MOD_IDX_MDP_RSZ0,
+	[MDP_COMP_RSZ1] = MUTEX_MOD_IDX_MDP_RSZ1,
+	[MDP_COMP_TDSHP0] = MUTEX_MOD_IDX_MDP_TDSHP0,
+	[MDP_COMP_WROT0] = MUTEX_MOD_IDX_MDP_WROT0,
+	[MDP_COMP_WDMA] = MUTEX_MOD_IDX_MDP_WDMA,
+	[MDP_COMP_AAL0] = MUTEX_MOD_IDX_MDP_AAL0,
+	[MDP_COMP_CCORR0] = MUTEX_MOD_IDX_MDP_CCORR0,
+};
+
+static const struct mtk_mdp_driver_data mt8183_mdp_driver_data = {
+	.mdp_probe_infra = mt8183_mdp_probe_infra,
+	.mdp_cfg = &mt8183_plat_cfg,
+	.mdp_mutex_table_idx = mt8183_mutex_idx,
+};
+
+static const struct of_device_id mdp_of_ids[] = {
+	{ .compatible = "mediatek,mt8183-mdp3-rdma",
+	  .data = &mt8183_mdp_driver_data,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, mdp_of_ids);
+
+static struct platform_device *__get_pdev_by_id(struct platform_device *pdev,
+						enum mdp_infra_id id)
+{
+	struct device_node *node;
+	struct platform_device *mdp_pdev = NULL;
+	const struct mtk_mdp_driver_data *mdp_data;
+	const char *compat;
+
+	if (!pdev)
+		return NULL;
+
+	if (id < MDP_INFRA_MMSYS || id >= MDP_INFRA_MAX) {
+		dev_err(&pdev->dev, "Illegal infra id %d\n", id);
+		return NULL;
+	}
+
+	mdp_data = of_device_get_match_data(&pdev->dev);
+	if (!mdp_data) {
+		dev_err(&pdev->dev, "have no driver data to find node\n");
+		return NULL;
+	}
+	compat = mdp_data->mdp_probe_infra[id].compatible;
+
+	node = of_find_compatible_node(NULL, NULL, compat);
+	if (WARN_ON(!node)) {
+		dev_err(&pdev->dev, "find node from id %d failed\n", id);
+		return NULL;
+	}
+
+	mdp_pdev = of_find_device_by_node(node);
+	of_node_put(node);
+	if (WARN_ON(!mdp_pdev)) {
+		dev_err(&pdev->dev, "find pdev from id %d failed\n", id);
+		return NULL;
+	}
+
+	return mdp_pdev;
+}
+
+struct platform_device *mdp_get_plat_device(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *mdp_node;
+	struct platform_device *mdp_pdev;
+
+	mdp_node = of_parse_phandle(dev->of_node, MDP_PHANDLE_NAME, 0);
+	if (!mdp_node) {
+		dev_err(dev, "can't get node %s\n", MDP_PHANDLE_NAME);
+		return NULL;
+	}
+
+	mdp_pdev = of_find_device_by_node(mdp_node);
+	of_node_put(mdp_node);
+
+	return mdp_pdev;
+}
+EXPORT_SYMBOL_GPL(mdp_get_plat_device);
+
+int mdp_vpu_get_locked(struct mdp_dev *mdp)
+{
+	int ret = 0;
+
+	if (mdp->vpu_count++ == 0) {
+		ret = rproc_boot(mdp->rproc_handle);
+		if (ret) {
+			dev_err(&mdp->pdev->dev,
+				"vpu_load_firmware failed %d\n", ret);
+			goto err_load_vpu;
+		}
+		ret = mdp_vpu_register(mdp);
+		if (ret) {
+			dev_err(&mdp->pdev->dev,
+				"mdp_vpu register failed %d\n", ret);
+			goto err_reg_vpu;
+		}
+		ret = mdp_vpu_dev_init(&mdp->vpu, mdp->scp, &mdp->vpu_lock);
+		if (ret) {
+			dev_err(&mdp->pdev->dev,
+				"mdp_vpu device init failed %d\n", ret);
+			goto err_init_vpu;
+		}
+	}
+	return 0;
+
+err_init_vpu:
+	mdp_vpu_unregister(mdp);
+err_reg_vpu:
+err_load_vpu:
+	mdp->vpu_count--;
+	return ret;
+}
+
+void mdp_vpu_put_locked(struct mdp_dev *mdp)
+{
+	if (--mdp->vpu_count == 0) {
+		mdp_vpu_dev_deinit(&mdp->vpu);
+		mdp_vpu_unregister(mdp);
+	}
+}
+
+void mdp_video_device_release(struct video_device *vdev)
+{
+	struct mdp_dev *mdp = (struct mdp_dev *)video_get_drvdata(vdev);
+	int i;
+
+	scp_put(mdp->scp);
+
+	destroy_workqueue(mdp->job_wq);
+	destroy_workqueue(mdp->clock_wq);
+
+	pm_runtime_disable(&mdp->pdev->dev);
+
+	vb2_dma_contig_clear_max_seg_size(&mdp->pdev->dev);
+
+	mdp_comp_destroy(mdp);
+	for (i = 0; i < MDP_PIPE_MAX; i++)
+		mtk_mutex_put(mdp->mdp_mutex[i]);
+
+	mdp_vpu_shared_mem_free(&mdp->vpu);
+	v4l2_m2m_release(mdp->m2m_dev);
+	kfree(mdp);
+}
+
+static int mdp_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct mdp_dev *mdp;
+	struct platform_device *mm_pdev;
+	int ret, i;
+
+	mdp = kzalloc(sizeof(*mdp), GFP_KERNEL);
+	if (!mdp) {
+		ret = -ENOMEM;
+		goto err_return;
+	}
+
+	mdp->pdev = pdev;
+	mdp->mdp_data = of_device_get_match_data(&pdev->dev);
+
+	mm_pdev = __get_pdev_by_id(pdev, MDP_INFRA_MMSYS);
+	if (!mm_pdev) {
+		ret = -ENODEV;
+		goto err_destroy_device;
+	}
+	mdp->mdp_mmsys = &mm_pdev->dev;
+
+	mm_pdev = __get_pdev_by_id(pdev, MDP_INFRA_MUTEX);
+	if (WARN_ON(!mm_pdev)) {
+		ret = -ENODEV;
+		goto err_destroy_device;
+	}
+	for (i = 0; i < MDP_PIPE_MAX; i++) {
+		mdp->mdp_mutex[i] = mtk_mutex_get(&mm_pdev->dev);
+		if (IS_ERR(mdp->mdp_mutex[i])) {
+			ret = PTR_ERR(mdp->mdp_mutex[i]);
+			goto err_free_mutex;
+		}
+	}
+
+	ret = mdp_comp_config(mdp);
+	if (ret) {
+		dev_err(dev, "Failed to config mdp components\n");
+		goto err_free_mutex;
+	}
+
+	mdp->job_wq = alloc_workqueue(MDP_MODULE_NAME, WQ_FREEZABLE, 0);
+	if (!mdp->job_wq) {
+		dev_err(dev, "Unable to create job workqueue\n");
+		ret = -ENOMEM;
+		goto err_deinit_comp;
+	}
+
+	mdp->clock_wq = alloc_workqueue(MDP_MODULE_NAME "-clock", WQ_FREEZABLE,
+					0);
+	if (!mdp->clock_wq) {
+		dev_err(dev, "Unable to create clock workqueue\n");
+		ret = -ENOMEM;
+		goto err_destroy_job_wq;
+	}
+
+	mm_pdev = __get_pdev_by_id(pdev, MDP_INFRA_SCP);
+	if (WARN_ON(!mm_pdev)) {
+		dev_err(&pdev->dev, "Could not get scp device\n");
+		ret = -ENODEV;
+		goto err_destroy_clock_wq;
+	}
+	mdp->scp = platform_get_drvdata(mm_pdev);
+	mdp->rproc_handle = scp_get_rproc(mdp->scp);
+	dev_dbg(&pdev->dev, "MDP rproc_handle: %pK", mdp->rproc_handle);
+
+	mutex_init(&mdp->vpu_lock);
+	mutex_init(&mdp->m2m_lock);
+
+	mdp->cmdq_clt = cmdq_mbox_create(dev, 0);
+	if (IS_ERR(mdp->cmdq_clt)) {
+		ret = PTR_ERR(mdp->cmdq_clt);
+		goto err_put_scp;
+	}
+
+	init_waitqueue_head(&mdp->callback_wq);
+	ida_init(&mdp->mdp_ida);
+	platform_set_drvdata(pdev, mdp);
+
+	vb2_dma_contig_set_max_seg_size(&pdev->dev, DMA_BIT_MASK(32));
+
+	ret = v4l2_device_register(dev, &mdp->v4l2_dev);
+	if (ret) {
+		dev_err(dev, "Failed to register v4l2 device\n");
+		ret = -EINVAL;
+		goto err_mbox_destroy;
+	}
+
+	ret = mdp_m2m_device_register(mdp);
+	if (ret) {
+		v4l2_err(&mdp->v4l2_dev, "Failed to register m2m device\n");
+		goto err_unregister_device;
+	}
+
+	dev_dbg(dev, "mdp-%d registered successfully\n", pdev->id);
+	return 0;
+
+err_unregister_device:
+	v4l2_device_unregister(&mdp->v4l2_dev);
+err_mbox_destroy:
+	cmdq_mbox_destroy(mdp->cmdq_clt);
+err_put_scp:
+	scp_put(mdp->scp);
+err_destroy_clock_wq:
+	destroy_workqueue(mdp->clock_wq);
+err_destroy_job_wq:
+	destroy_workqueue(mdp->job_wq);
+err_deinit_comp:
+	mdp_comp_destroy(mdp);
+err_free_mutex:
+	for (i = 0; i < MDP_PIPE_MAX; i++)
+		if (!IS_ERR_OR_NULL(mdp->mdp_mutex[i]))
+			mtk_mutex_put(mdp->mdp_mutex[i]);
+err_destroy_device:
+	kfree(mdp);
+err_return:
+	dev_dbg(dev, "Errno %d\n", ret);
+	return ret;
+}
+
+static int mdp_remove(struct platform_device *pdev)
+{
+	struct mdp_dev *mdp = platform_get_drvdata(pdev);
+
+	v4l2_device_unregister(&mdp->v4l2_dev);
+
+	dev_dbg(&pdev->dev, "%s driver unloaded\n", pdev->name);
+	return 0;
+}
+
+static int __maybe_unused mdp_suspend(struct device *dev)
+{
+	struct mdp_dev *mdp = dev_get_drvdata(dev);
+	int ret;
+
+	atomic_set(&mdp->suspended, 1);
+
+	if (atomic_read(&mdp->job_count)) {
+		ret = wait_event_timeout(mdp->callback_wq,
+					 !atomic_read(&mdp->job_count),
+					 2 * HZ);
+		if (ret == 0) {
+			dev_err(dev,
+				"%s:flushed cmdq task incomplete, count=%d\n",
+				__func__, atomic_read(&mdp->job_count));
+			return -EBUSY;
+		}
+	}
+
+	return 0;
+}
+
+static int __maybe_unused mdp_resume(struct device *dev)
+{
+	struct mdp_dev *mdp = dev_get_drvdata(dev);
+
+	atomic_set(&mdp->suspended, 0);
+
+	return 0;
+}
+
+static const struct dev_pm_ops mdp_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(mdp_suspend, mdp_resume)
+};
+
+static struct platform_driver mdp_driver = {
+	.probe		= mdp_probe,
+	.remove		= mdp_remove,
+	.driver = {
+		.name	= MDP_MODULE_NAME,
+		.pm	= &mdp_pm_ops,
+		.of_match_table = of_match_ptr(mdp_of_ids),
+	},
+};
+
+module_platform_driver(mdp_driver);
+
+MODULE_AUTHOR("Ping-Hsun Wu <ping-hsun.wu@mediatek.com>");
+MODULE_DESCRIPTION("MediaTek image processor 3 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.h b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.h
new file mode 100644
index 000000000..2ef5fbc4f
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.h
@@ -0,0 +1,94 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#ifndef __MTK_MDP3_CORE_H__
+#define __MTK_MDP3_CORE_H__
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-mem2mem.h>
+#include <linux/soc/mediatek/mtk-mmsys.h>
+#include <linux/soc/mediatek/mtk-mutex.h>
+#include "mtk-mdp3-comp.h"
+#include "mtk-mdp3-vpu.h"
+
+#define MDP_MODULE_NAME		"mtk-mdp3"
+#define MDP_DEVICE_NAME		"MediaTek MDP3"
+#define MDP_PHANDLE_NAME	"mediatek,mdp3"
+
+enum mdp_infra_id {
+	MDP_INFRA_MMSYS,
+	MDP_INFRA_MUTEX,
+	MDP_INFRA_SCP,
+	MDP_INFRA_MAX
+};
+
+enum mdp_buffer_usage {
+	MDP_BUFFER_USAGE_HW_READ,
+	MDP_BUFFER_USAGE_MDP,
+	MDP_BUFFER_USAGE_MDP2,
+	MDP_BUFFER_USAGE_ISP,
+	MDP_BUFFER_USAGE_WPE,
+};
+
+struct mdp_platform_config {
+	bool	rdma_support_10bit;
+	bool	rdma_rsz1_sram_sharing;
+	bool	rdma_upsample_repeat_only;
+	bool	rsz_disable_dcm_small_sample;
+	bool	wrot_filter_constraint;
+};
+
+/* indicate which mutex is used by each pipepline */
+enum mdp_pipe_id {
+	MDP_PIPE_RDMA0,
+	MDP_PIPE_IMGI,
+	MDP_PIPE_WPEI,
+	MDP_PIPE_WPEI2,
+	MDP_PIPE_MAX
+};
+
+struct mtk_mdp_driver_data {
+	const struct of_device_id *mdp_probe_infra;
+	const struct mdp_platform_config *mdp_cfg;
+	const u32 *mdp_mutex_table_idx;
+};
+
+struct mdp_dev {
+	struct platform_device			*pdev;
+	struct device				*mdp_mmsys;
+	struct mtk_mutex			*mdp_mutex[MDP_PIPE_MAX];
+	struct mdp_comp				*comp[MDP_MAX_COMP_COUNT];
+	const struct mtk_mdp_driver_data	*mdp_data;
+
+	struct workqueue_struct			*job_wq;
+	struct workqueue_struct			*clock_wq;
+	struct mdp_vpu_dev			vpu;
+	struct mtk_scp				*scp;
+	struct rproc				*rproc_handle;
+	/* synchronization protect for accessing vpu working buffer info */
+	struct mutex				vpu_lock;
+	s32					vpu_count;
+	u32					id_count;
+	struct ida				mdp_ida;
+	struct cmdq_client			*cmdq_clt;
+	wait_queue_head_t			callback_wq;
+
+	struct v4l2_device			v4l2_dev;
+	struct video_device			*m2m_vdev;
+	struct v4l2_m2m_dev			*m2m_dev;
+	/* synchronization protect for m2m device operation */
+	struct mutex				m2m_lock;
+	atomic_t				suspended;
+	atomic_t				job_count;
+};
+
+int mdp_vpu_get_locked(struct mdp_dev *mdp);
+void mdp_vpu_put_locked(struct mdp_dev *mdp);
+int mdp_vpu_register(struct mdp_dev *mdp);
+void mdp_vpu_unregister(struct mdp_dev *mdp);
+void mdp_video_device_release(struct video_device *vdev);
+
+#endif  /* __MTK_MDP3_CORE_H__ */
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-m2m.c b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-m2m.c
new file mode 100644
index 000000000..8612a48bd
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-m2m.c
@@ -0,0 +1,730 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#include <linux/platform_device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-dma-contig.h>
+#include "mtk-mdp3-m2m.h"
+
+static inline struct mdp_m2m_ctx *fh_to_ctx(struct v4l2_fh *fh)
+{
+	return container_of(fh, struct mdp_m2m_ctx, fh);
+}
+
+static inline struct mdp_m2m_ctx *ctrl_to_ctx(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct mdp_m2m_ctx, ctrl_handler);
+}
+
+static inline struct mdp_frame *ctx_get_frame(struct mdp_m2m_ctx *ctx,
+					      enum v4l2_buf_type type)
+{
+	if (V4L2_TYPE_IS_OUTPUT(type))
+		return &ctx->curr_param.output;
+	else
+		return &ctx->curr_param.captures[0];
+}
+
+static inline void mdp_m2m_ctx_set_state(struct mdp_m2m_ctx *ctx, u32 state)
+{
+	atomic_or(state, &ctx->curr_param.state);
+}
+
+static inline bool mdp_m2m_ctx_is_state_set(struct mdp_m2m_ctx *ctx, u32 mask)
+{
+	return ((atomic_read(&ctx->curr_param.state) & mask) == mask);
+}
+
+static void mdp_m2m_process_done(void *priv, int vb_state)
+{
+	struct mdp_m2m_ctx *ctx = priv;
+	struct vb2_v4l2_buffer *src_vbuf, *dst_vbuf;
+
+	src_vbuf = (struct vb2_v4l2_buffer *)
+			v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+	dst_vbuf = (struct vb2_v4l2_buffer *)
+			v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+	ctx->curr_param.frame_no = ctx->frame_count[MDP_M2M_SRC];
+	src_vbuf->sequence = ctx->frame_count[MDP_M2M_SRC]++;
+	dst_vbuf->sequence = ctx->frame_count[MDP_M2M_DST]++;
+	v4l2_m2m_buf_copy_metadata(src_vbuf, dst_vbuf, true);
+
+	v4l2_m2m_buf_done(src_vbuf, vb_state);
+	v4l2_m2m_buf_done(dst_vbuf, vb_state);
+	v4l2_m2m_job_finish(ctx->mdp_dev->m2m_dev, ctx->m2m_ctx);
+}
+
+static void mdp_m2m_device_run(void *priv)
+{
+	struct mdp_m2m_ctx *ctx = priv;
+	struct mdp_frame *frame;
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+	struct img_ipi_frameparam param = {};
+	struct mdp_cmdq_param task = {};
+	enum vb2_buffer_state vb_state = VB2_BUF_STATE_ERROR;
+	int ret;
+
+	if (mdp_m2m_ctx_is_state_set(ctx, MDP_M2M_CTX_ERROR)) {
+		dev_err(&ctx->mdp_dev->pdev->dev,
+			"mdp_m2m_ctx is in error state\n");
+		goto worker_end;
+	}
+
+	param.frame_no = ctx->curr_param.frame_no;
+	param.type = ctx->curr_param.type;
+	param.num_inputs = 1;
+	param.num_outputs = 1;
+
+	frame = ctx_get_frame(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	src_vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	mdp_set_src_config(&param.inputs[0], frame, &src_vb->vb2_buf);
+
+	frame = ctx_get_frame(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	dst_vb = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	mdp_set_dst_config(&param.outputs[0], frame, &dst_vb->vb2_buf);
+
+	ret = mdp_vpu_process(&ctx->vpu, &param);
+	if (ret) {
+		dev_err(&ctx->mdp_dev->pdev->dev,
+			"VPU MDP process failed: %d\n", ret);
+		goto worker_end;
+	}
+
+	task.config = ctx->vpu.config;
+	task.param = &param;
+	task.composes[0] = &frame->compose;
+	task.cmdq_cb = NULL;
+	task.cb_data = NULL;
+	task.mdp_ctx = ctx;
+
+	ret = mdp_cmdq_send(ctx->mdp_dev, &task);
+	if (ret) {
+		dev_err(&ctx->mdp_dev->pdev->dev,
+			"CMDQ sendtask failed: %d\n", ret);
+		goto worker_end;
+	}
+
+	return;
+
+worker_end:
+	mdp_m2m_process_done(ctx, vb_state);
+}
+
+static int mdp_m2m_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct mdp_m2m_ctx *ctx = vb2_get_drv_priv(q);
+	struct mdp_frame *capture;
+	struct vb2_queue *vq;
+	int ret;
+	bool out_streaming, cap_streaming;
+
+	if (V4L2_TYPE_IS_OUTPUT(q->type))
+		ctx->frame_count[MDP_M2M_SRC] = 0;
+
+	if (V4L2_TYPE_IS_CAPTURE(q->type))
+		ctx->frame_count[MDP_M2M_DST] = 0;
+
+	capture = ctx_get_frame(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	vq = v4l2_m2m_get_src_vq(ctx->m2m_ctx);
+	out_streaming = vb2_is_streaming(vq);
+	vq = v4l2_m2m_get_dst_vq(ctx->m2m_ctx);
+	cap_streaming = vb2_is_streaming(vq);
+
+	/* Check to see if scaling ratio is within supported range */
+	if ((V4L2_TYPE_IS_OUTPUT(q->type) && cap_streaming) ||
+	    (V4L2_TYPE_IS_CAPTURE(q->type) && out_streaming)) {
+		ret = mdp_check_scaling_ratio(&capture->crop.c,
+					      &capture->compose,
+					      capture->rotation,
+					      ctx->curr_param.limit);
+		if (ret) {
+			dev_err(&ctx->mdp_dev->pdev->dev,
+				"Out of scaling range\n");
+			return ret;
+		}
+	}
+
+	if (!mdp_m2m_ctx_is_state_set(ctx, MDP_VPU_INIT)) {
+		ret = mdp_vpu_get_locked(ctx->mdp_dev);
+		if (ret)
+			return ret;
+
+		ret = mdp_vpu_ctx_init(&ctx->vpu, &ctx->mdp_dev->vpu,
+				       MDP_DEV_M2M);
+		if (ret) {
+			dev_err(&ctx->mdp_dev->pdev->dev,
+				"VPU init failed %d\n", ret);
+			return -EINVAL;
+		}
+		mdp_m2m_ctx_set_state(ctx, MDP_VPU_INIT);
+	}
+
+	return 0;
+}
+
+static struct vb2_v4l2_buffer *mdp_m2m_buf_remove(struct mdp_m2m_ctx *ctx,
+						  unsigned int type)
+{
+	if (V4L2_TYPE_IS_OUTPUT(type))
+		return (struct vb2_v4l2_buffer *)
+			v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+	else
+		return (struct vb2_v4l2_buffer *)
+			v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+}
+
+static void mdp_m2m_stop_streaming(struct vb2_queue *q)
+{
+	struct mdp_m2m_ctx *ctx = vb2_get_drv_priv(q);
+	struct vb2_v4l2_buffer *vb;
+
+	vb = mdp_m2m_buf_remove(ctx, q->type);
+	while (vb) {
+		v4l2_m2m_buf_done(vb, VB2_BUF_STATE_ERROR);
+		vb = mdp_m2m_buf_remove(ctx, q->type);
+	}
+}
+
+static int mdp_m2m_queue_setup(struct vb2_queue *q,
+			       unsigned int *num_buffers,
+			       unsigned int *num_planes, unsigned int sizes[],
+			       struct device *alloc_devs[])
+{
+	struct mdp_m2m_ctx *ctx = vb2_get_drv_priv(q);
+	struct v4l2_pix_format_mplane *pix_mp;
+	u32 i;
+
+	pix_mp = &ctx_get_frame(ctx, q->type)->format.fmt.pix_mp;
+
+	/* from VIDIOC_CREATE_BUFS */
+	if (*num_planes) {
+		if (*num_planes != pix_mp->num_planes)
+			return -EINVAL;
+		for (i = 0; i < pix_mp->num_planes; ++i)
+			if (sizes[i] < pix_mp->plane_fmt[i].sizeimage)
+				return -EINVAL;
+	} else {/* from VIDIOC_REQBUFS */
+		*num_planes = pix_mp->num_planes;
+		for (i = 0; i < pix_mp->num_planes; ++i)
+			sizes[i] = pix_mp->plane_fmt[i].sizeimage;
+	}
+
+	return 0;
+}
+
+static int mdp_m2m_buf_prepare(struct vb2_buffer *vb)
+{
+	struct mdp_m2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct v4l2_pix_format_mplane *pix_mp;
+	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
+	u32 i;
+
+	v4l2_buf->field = V4L2_FIELD_NONE;
+
+	if (V4L2_TYPE_IS_CAPTURE(vb->type)) {
+		pix_mp = &ctx_get_frame(ctx, vb->type)->format.fmt.pix_mp;
+		for (i = 0; i < pix_mp->num_planes; ++i) {
+			vb2_set_plane_payload(vb, i,
+					      pix_mp->plane_fmt[i].sizeimage);
+		}
+	}
+	return 0;
+}
+
+static int mdp_m2m_buf_out_validate(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
+
+	v4l2_buf->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static void mdp_m2m_buf_queue(struct vb2_buffer *vb)
+{
+	struct mdp_m2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
+
+	v4l2_buf->field = V4L2_FIELD_NONE;
+
+	v4l2_m2m_buf_queue(ctx->m2m_ctx, to_vb2_v4l2_buffer(vb));
+}
+
+static const struct vb2_ops mdp_m2m_qops = {
+	.queue_setup	= mdp_m2m_queue_setup,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+	.buf_prepare	= mdp_m2m_buf_prepare,
+	.start_streaming = mdp_m2m_start_streaming,
+	.stop_streaming	= mdp_m2m_stop_streaming,
+	.buf_queue	= mdp_m2m_buf_queue,
+	.buf_out_validate = mdp_m2m_buf_out_validate,
+};
+
+static int mdp_m2m_querycap(struct file *file, void *fh,
+			    struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, MDP_MODULE_NAME, sizeof(cap->driver));
+	strscpy(cap->card, MDP_DEVICE_NAME, sizeof(cap->card));
+
+	return 0;
+}
+
+static int mdp_m2m_enum_fmt_mplane(struct file *file, void *fh,
+				   struct v4l2_fmtdesc *f)
+{
+	return mdp_enum_fmt_mplane(f);
+}
+
+static int mdp_m2m_g_fmt_mplane(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	struct mdp_m2m_ctx *ctx = fh_to_ctx(fh);
+	struct mdp_frame *frame;
+	struct v4l2_pix_format_mplane *pix_mp;
+
+	frame = ctx_get_frame(ctx, f->type);
+	*f = frame->format;
+	pix_mp = &f->fmt.pix_mp;
+	pix_mp->colorspace = ctx->curr_param.colorspace;
+	pix_mp->xfer_func = ctx->curr_param.xfer_func;
+	pix_mp->ycbcr_enc = ctx->curr_param.ycbcr_enc;
+	pix_mp->quantization = ctx->curr_param.quant;
+
+	return 0;
+}
+
+static int mdp_m2m_s_fmt_mplane(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	struct mdp_m2m_ctx *ctx = fh_to_ctx(fh);
+	struct mdp_frame *frame = ctx_get_frame(ctx, f->type);
+	struct mdp_frame *capture;
+	const struct mdp_format *fmt;
+	struct vb2_queue *vq;
+
+	fmt = mdp_try_fmt_mplane(f, &ctx->curr_param, ctx->id);
+	if (!fmt)
+		return -EINVAL;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	if (vb2_is_busy(vq))
+		return -EBUSY;
+
+	frame->format = *f;
+	frame->mdp_fmt = fmt;
+	frame->ycbcr_prof = mdp_map_ycbcr_prof_mplane(f, fmt->mdp_color);
+	frame->usage = V4L2_TYPE_IS_OUTPUT(f->type) ?
+		MDP_BUFFER_USAGE_HW_READ : MDP_BUFFER_USAGE_MDP;
+
+	capture = ctx_get_frame(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (V4L2_TYPE_IS_OUTPUT(f->type)) {
+		capture->crop.c.left = 0;
+		capture->crop.c.top = 0;
+		capture->crop.c.width = f->fmt.pix_mp.width;
+		capture->crop.c.height = f->fmt.pix_mp.height;
+		ctx->curr_param.colorspace = f->fmt.pix_mp.colorspace;
+		ctx->curr_param.ycbcr_enc = f->fmt.pix_mp.ycbcr_enc;
+		ctx->curr_param.quant = f->fmt.pix_mp.quantization;
+		ctx->curr_param.xfer_func = f->fmt.pix_mp.xfer_func;
+	} else {
+		capture->compose.left = 0;
+		capture->compose.top = 0;
+		capture->compose.width = f->fmt.pix_mp.width;
+		capture->compose.height = f->fmt.pix_mp.height;
+	}
+
+	return 0;
+}
+
+static int mdp_m2m_try_fmt_mplane(struct file *file, void *fh,
+				  struct v4l2_format *f)
+{
+	struct mdp_m2m_ctx *ctx = fh_to_ctx(fh);
+
+	if (!mdp_try_fmt_mplane(f, &ctx->curr_param, ctx->id))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int mdp_m2m_g_selection(struct file *file, void *fh,
+			       struct v4l2_selection *s)
+{
+	struct mdp_m2m_ctx *ctx = fh_to_ctx(fh);
+	struct mdp_frame *frame;
+	bool valid = false;
+
+	if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		valid = mdp_target_is_crop(s->target);
+	else if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		valid = mdp_target_is_compose(s->target);
+
+	if (!valid)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		frame = ctx_get_frame(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+		s->r = frame->crop.c;
+		return 0;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		frame = ctx_get_frame(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+		s->r = frame->compose;
+		return 0;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		frame = ctx_get_frame(ctx, s->type);
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = frame->format.fmt.pix_mp.width;
+		s->r.height = frame->format.fmt.pix_mp.height;
+		return 0;
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		frame = ctx_get_frame(ctx, s->type);
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = frame->format.fmt.pix_mp.width;
+		s->r.height = frame->format.fmt.pix_mp.height;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int mdp_m2m_s_selection(struct file *file, void *fh,
+			       struct v4l2_selection *s)
+{
+	struct mdp_m2m_ctx *ctx = fh_to_ctx(fh);
+	struct mdp_frame *frame = ctx_get_frame(ctx, s->type);
+	struct mdp_frame *capture;
+	struct v4l2_rect r;
+	struct device *dev = &ctx->mdp_dev->pdev->dev;
+	bool valid = false;
+	int ret;
+
+	if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		valid = (s->target == V4L2_SEL_TGT_CROP);
+	else if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		valid = (s->target == V4L2_SEL_TGT_COMPOSE);
+
+	if (!valid) {
+		dev_dbg(dev, "[%s:%d] invalid type:%u target:%u", __func__,
+			ctx->id, s->type, s->target);
+		return -EINVAL;
+	}
+
+	ret = mdp_try_crop(ctx, &r, s, frame);
+	if (ret)
+		return ret;
+	capture = ctx_get_frame(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+
+	if (mdp_target_is_crop(s->target))
+		capture->crop.c = r;
+	else
+		capture->compose = r;
+
+	s->r = r;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops mdp_m2m_ioctl_ops = {
+	.vidioc_querycap		= mdp_m2m_querycap,
+	.vidioc_enum_fmt_vid_cap	= mdp_m2m_enum_fmt_mplane,
+	.vidioc_enum_fmt_vid_out	= mdp_m2m_enum_fmt_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= mdp_m2m_g_fmt_mplane,
+	.vidioc_g_fmt_vid_out_mplane	= mdp_m2m_g_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= mdp_m2m_s_fmt_mplane,
+	.vidioc_s_fmt_vid_out_mplane	= mdp_m2m_s_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= mdp_m2m_try_fmt_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= mdp_m2m_try_fmt_mplane,
+	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
+	.vidioc_expbuf			= v4l2_m2m_ioctl_expbuf,
+	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
+	.vidioc_create_bufs		= v4l2_m2m_ioctl_create_bufs,
+	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
+	.vidioc_g_selection		= mdp_m2m_g_selection,
+	.vidioc_s_selection		= mdp_m2m_s_selection,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+static int mdp_m2m_queue_init(void *priv,
+			      struct vb2_queue *src_vq,
+			      struct vb2_queue *dst_vq)
+{
+	struct mdp_m2m_ctx *ctx = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->ops = &mdp_m2m_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->dev = &ctx->mdp_dev->pdev->dev;
+	src_vq->lock = &ctx->ctx_lock;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->ops = &mdp_m2m_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->dev = &ctx->mdp_dev->pdev->dev;
+	dst_vq->lock = &ctx->ctx_lock;
+
+	return vb2_queue_init(dst_vq);
+}
+
+static int mdp_m2m_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mdp_m2m_ctx *ctx = ctrl_to_ctx(ctrl);
+	struct mdp_frame *capture;
+
+	capture = ctx_get_frame(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		capture->hflip = ctrl->val;
+		break;
+	case V4L2_CID_VFLIP:
+		capture->vflip = ctrl->val;
+		break;
+	case V4L2_CID_ROTATE:
+		capture->rotation = ctrl->val;
+		break;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops mdp_m2m_ctrl_ops = {
+	.s_ctrl	= mdp_m2m_s_ctrl,
+};
+
+static int mdp_m2m_ctrls_create(struct mdp_m2m_ctx *ctx)
+{
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, MDP_MAX_CTRLS);
+	ctx->ctrls.hflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+					     &mdp_m2m_ctrl_ops, V4L2_CID_HFLIP,
+					     0, 1, 1, 0);
+	ctx->ctrls.vflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+					     &mdp_m2m_ctrl_ops, V4L2_CID_VFLIP,
+					     0, 1, 1, 0);
+	ctx->ctrls.rotate = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+					      &mdp_m2m_ctrl_ops,
+					      V4L2_CID_ROTATE, 0, 270, 90, 0);
+
+	if (ctx->ctrl_handler.error) {
+		int err = ctx->ctrl_handler.error;
+
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+		dev_err(&ctx->mdp_dev->pdev->dev,
+			"Failed to register controls\n");
+		return err;
+	}
+	return 0;
+}
+
+static int mdp_m2m_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct mdp_dev *mdp = video_get_drvdata(vdev);
+	struct mdp_m2m_ctx *ctx;
+	struct device *dev = &mdp->pdev->dev;
+	int ret;
+	struct v4l2_format default_format = {};
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	if (mutex_lock_interruptible(&mdp->m2m_lock)) {
+		ret = -ERESTARTSYS;
+		goto err_free_ctx;
+	}
+
+	ret = ida_alloc(&mdp->mdp_ida, GFP_KERNEL);
+	if (ret < 0)
+		goto err_unlock_mutex;
+	ctx->id = ret;
+
+	ctx->mdp_dev = mdp;
+
+	v4l2_fh_init(&ctx->fh, vdev);
+	file->private_data = &ctx->fh;
+	ret = mdp_m2m_ctrls_create(ctx);
+	if (ret)
+		goto err_exit_fh;
+
+	/* Use separate control handler per file handle */
+	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+	v4l2_fh_add(&ctx->fh);
+
+	mutex_init(&ctx->ctx_lock);
+	ctx->m2m_ctx = v4l2_m2m_ctx_init(mdp->m2m_dev, ctx, mdp_m2m_queue_init);
+	if (IS_ERR(ctx->m2m_ctx)) {
+		dev_err(dev, "Failed to initialize m2m context\n");
+		ret = PTR_ERR(ctx->m2m_ctx);
+		goto err_release_handler;
+	}
+	ctx->fh.m2m_ctx = ctx->m2m_ctx;
+
+	ctx->curr_param.ctx = ctx;
+	ret = mdp_frameparam_init(&ctx->curr_param);
+	if (ret) {
+		dev_err(dev, "Failed to initialize mdp parameter\n");
+		goto err_release_m2m_ctx;
+	}
+
+	mutex_unlock(&mdp->m2m_lock);
+
+	/* Default format */
+	default_format.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	default_format.fmt.pix_mp.width = 32;
+	default_format.fmt.pix_mp.height = 32;
+	default_format.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_YUV420M;
+	mdp_m2m_s_fmt_mplane(file, &ctx->fh, &default_format);
+	default_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	mdp_m2m_s_fmt_mplane(file, &ctx->fh, &default_format);
+
+	dev_dbg(dev, "%s:[%d]", __func__, ctx->id);
+
+	return 0;
+
+err_release_m2m_ctx:
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+err_release_handler:
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	v4l2_fh_del(&ctx->fh);
+err_exit_fh:
+	v4l2_fh_exit(&ctx->fh);
+	ida_free(&mdp->mdp_ida, ctx->id);
+err_unlock_mutex:
+	mutex_unlock(&mdp->m2m_lock);
+err_free_ctx:
+	kfree(ctx);
+
+	return ret;
+}
+
+static int mdp_m2m_release(struct file *file)
+{
+	struct mdp_m2m_ctx *ctx = fh_to_ctx(file->private_data);
+	struct mdp_dev *mdp = video_drvdata(file);
+	struct device *dev = &mdp->pdev->dev;
+
+	mutex_lock(&mdp->m2m_lock);
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+	if (mdp_m2m_ctx_is_state_set(ctx, MDP_VPU_INIT)) {
+		mdp_vpu_ctx_deinit(&ctx->vpu);
+		mdp_vpu_put_locked(mdp);
+	}
+
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	ida_free(&mdp->mdp_ida, ctx->id);
+	mutex_unlock(&mdp->m2m_lock);
+
+	dev_dbg(dev, "%s:[%d]", __func__, ctx->id);
+	kfree(ctx);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations mdp_m2m_fops = {
+	.owner		= THIS_MODULE,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+	.open		= mdp_m2m_open,
+	.release	= mdp_m2m_release,
+};
+
+static const struct v4l2_m2m_ops mdp_m2m_ops = {
+	.device_run	= mdp_m2m_device_run,
+};
+
+int mdp_m2m_device_register(struct mdp_dev *mdp)
+{
+	struct device *dev = &mdp->pdev->dev;
+	int ret = 0;
+
+	mdp->m2m_vdev = video_device_alloc();
+	if (!mdp->m2m_vdev) {
+		dev_err(dev, "Failed to allocate video device\n");
+		ret = -ENOMEM;
+		goto err_video_alloc;
+	}
+	mdp->m2m_vdev->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE |
+		V4L2_CAP_STREAMING;
+	mdp->m2m_vdev->fops = &mdp_m2m_fops;
+	mdp->m2m_vdev->ioctl_ops = &mdp_m2m_ioctl_ops;
+	mdp->m2m_vdev->release = mdp_video_device_release;
+	mdp->m2m_vdev->lock = &mdp->m2m_lock;
+	mdp->m2m_vdev->vfl_dir = VFL_DIR_M2M;
+	mdp->m2m_vdev->v4l2_dev = &mdp->v4l2_dev;
+	snprintf(mdp->m2m_vdev->name, sizeof(mdp->m2m_vdev->name), "%s:m2m",
+		 MDP_MODULE_NAME);
+	video_set_drvdata(mdp->m2m_vdev, mdp);
+
+	mdp->m2m_dev = v4l2_m2m_init(&mdp_m2m_ops);
+	if (IS_ERR(mdp->m2m_dev)) {
+		dev_err(dev, "Failed to initialize v4l2-m2m device\n");
+		ret = PTR_ERR(mdp->m2m_dev);
+		goto err_m2m_init;
+	}
+
+	ret = video_register_device(mdp->m2m_vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		dev_err(dev, "Failed to register video device\n");
+		goto err_video_register;
+	}
+
+	v4l2_info(&mdp->v4l2_dev, "Driver registered as /dev/video%d",
+		  mdp->m2m_vdev->num);
+	return 0;
+
+err_video_register:
+	v4l2_m2m_release(mdp->m2m_dev);
+err_m2m_init:
+	video_device_release(mdp->m2m_vdev);
+err_video_alloc:
+
+	return ret;
+}
+
+void mdp_m2m_device_unregister(struct mdp_dev *mdp)
+{
+	video_unregister_device(mdp->m2m_vdev);
+}
+
+void mdp_m2m_job_finish(struct mdp_m2m_ctx *ctx)
+{
+	enum vb2_buffer_state vb_state = VB2_BUF_STATE_DONE;
+
+	mdp_m2m_process_done(ctx, vb_state);
+}
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-m2m.h b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-m2m.h
new file mode 100644
index 000000000..61ddbaf1b
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-m2m.h
@@ -0,0 +1,48 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#ifndef __MTK_MDP3_M2M_H__
+#define __MTK_MDP3_M2M_H__
+
+#include <media/v4l2-ctrls.h>
+#include "mtk-mdp3-core.h"
+#include "mtk-mdp3-vpu.h"
+#include "mtk-mdp3-regs.h"
+
+#define MDP_MAX_CTRLS	10
+
+enum {
+	MDP_M2M_SRC = 0,
+	MDP_M2M_DST = 1,
+	MDP_M2M_MAX,
+};
+
+struct mdp_m2m_ctrls {
+	struct v4l2_ctrl	*hflip;
+	struct v4l2_ctrl	*vflip;
+	struct v4l2_ctrl	*rotate;
+};
+
+struct mdp_m2m_ctx {
+	u32				id;
+	struct mdp_dev			*mdp_dev;
+	struct v4l2_fh			fh;
+	struct v4l2_ctrl_handler	ctrl_handler;
+	struct mdp_m2m_ctrls		ctrls;
+	struct v4l2_m2m_ctx		*m2m_ctx;
+	struct mdp_vpu_ctx		vpu;
+	u32				frame_count[MDP_M2M_MAX];
+
+	struct mdp_frameparam		curr_param;
+	/* synchronization protect for mdp m2m context */
+	struct mutex			ctx_lock;
+};
+
+int mdp_m2m_device_register(struct mdp_dev *mdp);
+void mdp_m2m_device_unregister(struct mdp_dev *mdp);
+void mdp_m2m_job_finish(struct mdp_m2m_ctx *ctx);
+
+#endif  /* __MTK_MDP3_M2M_H__ */
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-regs.c b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-regs.c
new file mode 100644
index 000000000..36336d169
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-regs.c
@@ -0,0 +1,737 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#include <linux/math64.h>
+#include <media/v4l2-common.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+#include "mtk-mdp3-core.h"
+#include "mtk-mdp3-regs.h"
+#include "mtk-mdp3-m2m.h"
+
+/*
+ * All 10-bit related formats are not added in the basic format list,
+ * please add the corresponding format settings before use.
+ */
+static const struct mdp_format mdp_formats[] = {
+	{
+		.pixelformat	= V4L2_PIX_FMT_GREY,
+		.mdp_color	= MDP_COLOR_GREY,
+		.depth		= { 8 },
+		.row_depth	= { 8 },
+		.num_planes	= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_RGB565X,
+		.mdp_color	= MDP_COLOR_BGR565,
+		.depth		= { 16 },
+		.row_depth	= { 16 },
+		.num_planes	= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_RGB565,
+		.mdp_color	= MDP_COLOR_RGB565,
+		.depth		= { 16 },
+		.row_depth	= { 16 },
+		.num_planes	= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_RGB24,
+		.mdp_color	= MDP_COLOR_RGB888,
+		.depth		= { 24 },
+		.row_depth	= { 24 },
+		.num_planes	= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_BGR24,
+		.mdp_color	= MDP_COLOR_BGR888,
+		.depth		= { 24 },
+		.row_depth	= { 24 },
+		.num_planes	= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_ABGR32,
+		.mdp_color	= MDP_COLOR_BGRA8888,
+		.depth		= { 32 },
+		.row_depth	= { 32 },
+		.num_planes	= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_ARGB32,
+		.mdp_color	= MDP_COLOR_ARGB8888,
+		.depth		= { 32 },
+		.row_depth	= { 32 },
+		.num_planes	= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_UYVY,
+		.mdp_color	= MDP_COLOR_UYVY,
+		.depth		= { 16 },
+		.row_depth	= { 16 },
+		.num_planes	= 1,
+		.walign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_VYUY,
+		.mdp_color	= MDP_COLOR_VYUY,
+		.depth		= { 16 },
+		.row_depth	= { 16 },
+		.num_planes	= 1,
+		.walign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_YUYV,
+		.mdp_color	= MDP_COLOR_YUYV,
+		.depth		= { 16 },
+		.row_depth	= { 16 },
+		.num_planes	= 1,
+		.walign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_YVYU,
+		.mdp_color	= MDP_COLOR_YVYU,
+		.depth		= { 16 },
+		.row_depth	= { 16 },
+		.num_planes	= 1,
+		.walign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_YUV420,
+		.mdp_color	= MDP_COLOR_I420,
+		.depth		= { 12 },
+		.row_depth	= { 8 },
+		.num_planes	= 1,
+		.walign		= 1,
+		.halign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_YVU420,
+		.mdp_color	= MDP_COLOR_YV12,
+		.depth		= { 12 },
+		.row_depth	= { 8 },
+		.num_planes	= 1,
+		.walign		= 1,
+		.halign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_NV12,
+		.mdp_color	= MDP_COLOR_NV12,
+		.depth		= { 12 },
+		.row_depth	= { 8 },
+		.num_planes	= 1,
+		.walign		= 1,
+		.halign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_NV21,
+		.mdp_color	= MDP_COLOR_NV21,
+		.depth		= { 12 },
+		.row_depth	= { 8 },
+		.num_planes	= 1,
+		.walign		= 1,
+		.halign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_NV16,
+		.mdp_color	= MDP_COLOR_NV16,
+		.depth		= { 16 },
+		.row_depth	= { 8 },
+		.num_planes	= 1,
+		.walign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_NV61,
+		.mdp_color	= MDP_COLOR_NV61,
+		.depth		= { 16 },
+		.row_depth	= { 8 },
+		.num_planes	= 1,
+		.walign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_NV24,
+		.mdp_color	= MDP_COLOR_NV24,
+		.depth		= { 24 },
+		.row_depth	= { 8 },
+		.num_planes	= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_NV42,
+		.mdp_color	= MDP_COLOR_NV42,
+		.depth		= { 24 },
+		.row_depth	= { 8 },
+		.num_planes	= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_MT21C,
+		.mdp_color	= MDP_COLOR_420_BLK_UFO,
+		.depth		= { 8, 4 },
+		.row_depth	= { 8, 8 },
+		.num_planes	= 2,
+		.walign		= 4,
+		.halign		= 5,
+		.flags		= MDP_FMT_FLAG_OUTPUT,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_MM21,
+		.mdp_color	= MDP_COLOR_420_BLK,
+		.depth		= { 8, 4 },
+		.row_depth	= { 8, 8 },
+		.num_planes	= 2,
+		.walign		= 4,
+		.halign		= 5,
+		.flags		= MDP_FMT_FLAG_OUTPUT,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_NV12M,
+		.mdp_color	= MDP_COLOR_NV12,
+		.depth		= { 8, 4 },
+		.row_depth	= { 8, 8 },
+		.num_planes	= 2,
+		.walign		= 1,
+		.halign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_NV21M,
+		.mdp_color	= MDP_COLOR_NV21,
+		.depth		= { 8, 4 },
+		.row_depth	= { 8, 8 },
+		.num_planes	= 2,
+		.walign		= 1,
+		.halign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_NV16M,
+		.mdp_color	= MDP_COLOR_NV16,
+		.depth		= { 8, 8 },
+		.row_depth	= { 8, 8 },
+		.num_planes	= 2,
+		.walign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_NV61M,
+		.mdp_color	= MDP_COLOR_NV61,
+		.depth		= { 8, 8 },
+		.row_depth	= { 8, 8 },
+		.num_planes	= 2,
+		.walign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_YUV420M,
+		.mdp_color	= MDP_COLOR_I420,
+		.depth		= { 8, 2, 2 },
+		.row_depth	= { 8, 4, 4 },
+		.num_planes	= 3,
+		.walign		= 1,
+		.halign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}, {
+		.pixelformat	= V4L2_PIX_FMT_YVU420M,
+		.mdp_color	= MDP_COLOR_YV12,
+		.depth		= { 8, 2, 2 },
+		.row_depth	= { 8, 4, 4 },
+		.num_planes	= 3,
+		.walign		= 1,
+		.halign		= 1,
+		.flags		= MDP_FMT_FLAG_OUTPUT | MDP_FMT_FLAG_CAPTURE,
+	}
+};
+
+static const struct mdp_limit mdp_def_limit = {
+	.out_limit = {
+		.wmin	= 16,
+		.hmin	= 16,
+		.wmax	= 8176,
+		.hmax	= 8176,
+	},
+	.cap_limit = {
+		.wmin	= 2,
+		.hmin	= 2,
+		.wmax	= 8176,
+		.hmax	= 8176,
+	},
+	.h_scale_up_max = 32,
+	.v_scale_up_max = 32,
+	.h_scale_down_max = 20,
+	.v_scale_down_max = 128,
+};
+
+static const struct mdp_format *mdp_find_fmt(u32 pixelformat, u32 type)
+{
+	u32 i, flag;
+
+	flag = V4L2_TYPE_IS_OUTPUT(type) ? MDP_FMT_FLAG_OUTPUT :
+					MDP_FMT_FLAG_CAPTURE;
+	for (i = 0; i < ARRAY_SIZE(mdp_formats); ++i) {
+		if (!(mdp_formats[i].flags & flag))
+			continue;
+		if (mdp_formats[i].pixelformat == pixelformat)
+			return &mdp_formats[i];
+	}
+	return NULL;
+}
+
+static const struct mdp_format *mdp_find_fmt_by_index(u32 index, u32 type)
+{
+	u32 i, flag, num = 0;
+
+	flag = V4L2_TYPE_IS_OUTPUT(type) ? MDP_FMT_FLAG_OUTPUT :
+					MDP_FMT_FLAG_CAPTURE;
+	for (i = 0; i < ARRAY_SIZE(mdp_formats); ++i) {
+		if (!(mdp_formats[i].flags & flag))
+			continue;
+		if (index == num)
+			return &mdp_formats[i];
+		num++;
+	}
+	return NULL;
+}
+
+enum mdp_ycbcr_profile mdp_map_ycbcr_prof_mplane(struct v4l2_format *f,
+						 u32 mdp_color)
+{
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+
+	if (MDP_COLOR_IS_RGB(mdp_color))
+		return MDP_YCBCR_PROFILE_FULL_BT601;
+
+	switch (pix_mp->colorspace) {
+	case V4L2_COLORSPACE_JPEG:
+		return MDP_YCBCR_PROFILE_JPEG;
+	case V4L2_COLORSPACE_REC709:
+	case V4L2_COLORSPACE_DCI_P3:
+		if (pix_mp->quantization == V4L2_QUANTIZATION_FULL_RANGE)
+			return MDP_YCBCR_PROFILE_FULL_BT709;
+		return MDP_YCBCR_PROFILE_BT709;
+	case V4L2_COLORSPACE_BT2020:
+		if (pix_mp->quantization == V4L2_QUANTIZATION_FULL_RANGE)
+			return MDP_YCBCR_PROFILE_FULL_BT2020;
+		return MDP_YCBCR_PROFILE_BT2020;
+	default:
+		if (pix_mp->quantization == V4L2_QUANTIZATION_FULL_RANGE)
+			return MDP_YCBCR_PROFILE_FULL_BT601;
+		return MDP_YCBCR_PROFILE_BT601;
+	}
+}
+
+static void mdp_bound_align_image(u32 *w, u32 *h,
+				  struct v4l2_frmsize_stepwise *s,
+				  unsigned int salign)
+{
+	unsigned int org_w, org_h;
+
+	org_w = *w;
+	org_h = *h;
+	v4l_bound_align_image(w, s->min_width, s->max_width, s->step_width,
+			      h, s->min_height, s->max_height, s->step_height,
+			      salign);
+
+	s->min_width = org_w;
+	s->min_height = org_h;
+	v4l2_apply_frmsize_constraints(w, h, s);
+}
+
+static int mdp_clamp_align(s32 *x, int min, int max, unsigned int align)
+{
+	unsigned int mask;
+
+	if (min < 0 || max < 0)
+		return -ERANGE;
+
+	/* Bits that must be zero to be aligned */
+	mask = ~((1 << align) - 1);
+
+	min = 0 ? 0 : ((min + ~mask) & mask);
+	max = max & mask;
+	if ((unsigned int)min > (unsigned int)max)
+		return -ERANGE;
+
+	/* Clamp to aligned min and max */
+	*x = clamp(*x, min, max);
+
+	/* Round to nearest aligned value */
+	if (align)
+		*x = (*x + (1 << (align - 1))) & mask;
+	return 0;
+}
+
+int mdp_enum_fmt_mplane(struct v4l2_fmtdesc *f)
+{
+	const struct mdp_format *fmt;
+
+	fmt = mdp_find_fmt_by_index(f->index, f->type);
+	if (!fmt)
+		return -EINVAL;
+
+	f->pixelformat = fmt->pixelformat;
+	return 0;
+}
+
+const struct mdp_format *mdp_try_fmt_mplane(struct v4l2_format *f,
+					    struct mdp_frameparam *param,
+					    u32 ctx_id)
+{
+	struct device *dev = &param->ctx->mdp_dev->pdev->dev;
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	const struct mdp_format *fmt;
+	const struct mdp_pix_limit *pix_limit;
+	struct v4l2_frmsize_stepwise s;
+	u32 org_w, org_h;
+	unsigned int i;
+
+	fmt = mdp_find_fmt(pix_mp->pixelformat, f->type);
+	if (!fmt) {
+		fmt = mdp_find_fmt_by_index(0, f->type);
+		if (!fmt) {
+			dev_dbg(dev, "%d: pixelformat %c%c%c%c invalid", ctx_id,
+				(pix_mp->pixelformat & 0xff),
+				(pix_mp->pixelformat >>  8) & 0xff,
+				(pix_mp->pixelformat >> 16) & 0xff,
+				(pix_mp->pixelformat >> 24) & 0xff);
+			return NULL;
+		}
+	}
+
+	pix_mp->field = V4L2_FIELD_NONE;
+	pix_mp->flags = 0;
+	pix_mp->pixelformat = fmt->pixelformat;
+	if (V4L2_TYPE_IS_CAPTURE(f->type)) {
+		pix_mp->colorspace = param->colorspace;
+		pix_mp->xfer_func = param->xfer_func;
+		pix_mp->ycbcr_enc = param->ycbcr_enc;
+		pix_mp->quantization = param->quant;
+	}
+
+	pix_limit = V4L2_TYPE_IS_OUTPUT(f->type) ? &param->limit->out_limit :
+						&param->limit->cap_limit;
+	s.min_width = pix_limit->wmin;
+	s.max_width = pix_limit->wmax;
+	s.step_width = fmt->walign;
+	s.min_height = pix_limit->hmin;
+	s.max_height = pix_limit->hmax;
+	s.step_height = fmt->halign;
+	org_w = pix_mp->width;
+	org_h = pix_mp->height;
+
+	mdp_bound_align_image(&pix_mp->width, &pix_mp->height, &s, fmt->salign);
+	if (org_w != pix_mp->width || org_h != pix_mp->height)
+		dev_dbg(dev, "%d: size change: %ux%u to %ux%u", ctx_id,
+			org_w, org_h, pix_mp->width, pix_mp->height);
+
+	if (pix_mp->num_planes && pix_mp->num_planes != fmt->num_planes)
+		dev_dbg(dev, "%d num of planes change: %u to %u", ctx_id,
+			pix_mp->num_planes, fmt->num_planes);
+	pix_mp->num_planes = fmt->num_planes;
+
+	for (i = 0; i < pix_mp->num_planes; ++i) {
+		u32 min_bpl = (pix_mp->width * fmt->row_depth[i]) >> 3;
+		u32 max_bpl = (pix_limit->wmax * fmt->row_depth[i]) >> 3;
+		u32 bpl = pix_mp->plane_fmt[i].bytesperline;
+		u32 min_si, max_si;
+		u32 si = pix_mp->plane_fmt[i].sizeimage;
+		u64 di;
+
+		bpl = clamp(bpl, min_bpl, max_bpl);
+		pix_mp->plane_fmt[i].bytesperline = bpl;
+
+		di = (u64)bpl * pix_mp->height * fmt->depth[i];
+		min_si = (u32)div_u64(di, fmt->row_depth[i]);
+		di = (u64)bpl * s.max_height * fmt->depth[i];
+		max_si = (u32)div_u64(di, fmt->row_depth[i]);
+
+		si = clamp(si, min_si, max_si);
+		pix_mp->plane_fmt[i].sizeimage = si;
+
+		dev_dbg(dev, "%d: p%u, bpl:%u [%u, %u], sizeimage:%u [%u, %u]",
+			ctx_id, i, bpl, min_bpl, max_bpl, si, min_si, max_si);
+	}
+
+	return fmt;
+}
+
+static int mdp_clamp_start(s32 *x, int min, int max, unsigned int align,
+			   u32 flags)
+{
+	if (flags & V4L2_SEL_FLAG_GE)
+		max = *x;
+	if (flags & V4L2_SEL_FLAG_LE)
+		min = *x;
+	return mdp_clamp_align(x, min, max, align);
+}
+
+static int mdp_clamp_end(s32 *x, int min, int max, unsigned int align,
+			 u32 flags)
+{
+	if (flags & V4L2_SEL_FLAG_GE)
+		min = *x;
+	if (flags & V4L2_SEL_FLAG_LE)
+		max = *x;
+	return mdp_clamp_align(x, min, max, align);
+}
+
+int mdp_try_crop(struct mdp_m2m_ctx *ctx, struct v4l2_rect *r,
+		 const struct v4l2_selection *s, struct mdp_frame *frame)
+{
+	struct device *dev = &ctx->mdp_dev->pdev->dev;
+	s32 left, top, right, bottom;
+	u32 framew, frameh, walign, halign;
+	int ret;
+
+	dev_dbg(dev, "%d target:%d, set:(%d,%d) %ux%u", ctx->id,
+		s->target, s->r.left, s->r.top, s->r.width, s->r.height);
+
+	left = s->r.left;
+	top = s->r.top;
+	right = s->r.left + s->r.width;
+	bottom = s->r.top + s->r.height;
+	framew = frame->format.fmt.pix_mp.width;
+	frameh = frame->format.fmt.pix_mp.height;
+
+	if (mdp_target_is_crop(s->target)) {
+		walign = 1;
+		halign = 1;
+	} else {
+		walign = frame->mdp_fmt->walign;
+		halign = frame->mdp_fmt->halign;
+	}
+
+	dev_dbg(dev, "%d align:%u,%u, bound:%ux%u", ctx->id,
+		walign, halign, framew, frameh);
+
+	ret = mdp_clamp_start(&left, 0, right, walign, s->flags);
+	if (ret)
+		return ret;
+	ret = mdp_clamp_start(&top, 0, bottom, halign, s->flags);
+	if (ret)
+		return ret;
+	ret = mdp_clamp_end(&right, left, framew, walign, s->flags);
+	if (ret)
+		return ret;
+	ret = mdp_clamp_end(&bottom, top, frameh, halign, s->flags);
+	if (ret)
+		return ret;
+
+	r->left = left;
+	r->top = top;
+	r->width = right - left;
+	r->height = bottom - top;
+
+	dev_dbg(dev, "%d crop:(%d,%d) %ux%u", ctx->id,
+		r->left, r->top, r->width, r->height);
+	return 0;
+}
+
+int mdp_check_scaling_ratio(const struct v4l2_rect *crop,
+			    const struct v4l2_rect *compose, s32 rotation,
+	const struct mdp_limit *limit)
+{
+	u32 crop_w, crop_h, comp_w, comp_h;
+
+	crop_w = crop->width;
+	crop_h = crop->height;
+	if (90 == rotation || 270 == rotation) {
+		comp_w = compose->height;
+		comp_h = compose->width;
+	} else {
+		comp_w = compose->width;
+		comp_h = compose->height;
+	}
+
+	if ((crop_w / comp_w) > limit->h_scale_down_max ||
+	    (crop_h / comp_h) > limit->v_scale_down_max ||
+	    (comp_w / crop_w) > limit->h_scale_up_max ||
+	    (comp_h / crop_h) > limit->v_scale_up_max)
+		return -ERANGE;
+	return 0;
+}
+
+/* Stride that is accepted by MDP HW */
+static u32 mdp_fmt_get_stride(const struct mdp_format *fmt,
+			      u32 bytesperline, unsigned int plane)
+{
+	enum mdp_color c = fmt->mdp_color;
+	u32 stride;
+
+	stride = (bytesperline * MDP_COLOR_BITS_PER_PIXEL(c))
+		/ fmt->row_depth[0];
+	if (plane == 0)
+		return stride;
+	if (plane < MDP_COLOR_GET_PLANE_COUNT(c)) {
+		if (MDP_COLOR_IS_BLOCK_MODE(c))
+			stride = stride / 2;
+		return stride;
+	}
+	return 0;
+}
+
+/* Stride that is accepted by MDP HW of format with contiguous planes */
+static u32 mdp_fmt_get_stride_contig(const struct mdp_format *fmt,
+				     u32 pix_stride, unsigned int plane)
+{
+	enum mdp_color c = fmt->mdp_color;
+	u32 stride = pix_stride;
+
+	if (plane == 0)
+		return stride;
+	if (plane < MDP_COLOR_GET_PLANE_COUNT(c)) {
+		stride = stride >> MDP_COLOR_GET_H_SUBSAMPLE(c);
+		if (MDP_COLOR_IS_UV_COPLANE(c) && !MDP_COLOR_IS_BLOCK_MODE(c))
+			stride = stride * 2;
+		return stride;
+	}
+	return 0;
+}
+
+/* Plane size that is accepted by MDP HW */
+static u32 mdp_fmt_get_plane_size(const struct mdp_format *fmt,
+				  u32 stride, u32 height, unsigned int plane)
+{
+	enum mdp_color c = fmt->mdp_color;
+	u32 bytesperline;
+
+	bytesperline = (stride * fmt->row_depth[0])
+		/ MDP_COLOR_BITS_PER_PIXEL(c);
+	if (plane == 0)
+		return bytesperline * height;
+	if (plane < MDP_COLOR_GET_PLANE_COUNT(c)) {
+		height = height >> MDP_COLOR_GET_V_SUBSAMPLE(c);
+		if (MDP_COLOR_IS_BLOCK_MODE(c))
+			bytesperline = bytesperline * 2;
+		return bytesperline * height;
+	}
+	return 0;
+}
+
+static void mdp_prepare_buffer(struct img_image_buffer *b,
+			       struct mdp_frame *frame, struct vb2_buffer *vb)
+{
+	struct v4l2_pix_format_mplane *pix_mp = &frame->format.fmt.pix_mp;
+	unsigned int i;
+
+	b->format.colorformat = frame->mdp_fmt->mdp_color;
+	b->format.ycbcr_prof = frame->ycbcr_prof;
+	for (i = 0; i < pix_mp->num_planes; ++i) {
+		u32 stride = mdp_fmt_get_stride(frame->mdp_fmt,
+			pix_mp->plane_fmt[i].bytesperline, i);
+
+		b->format.plane_fmt[i].stride = stride;
+		b->format.plane_fmt[i].size =
+			mdp_fmt_get_plane_size(frame->mdp_fmt, stride,
+					       pix_mp->height, i);
+		b->iova[i] = vb2_dma_contig_plane_dma_addr(vb, i);
+	}
+	for (; i < MDP_COLOR_GET_PLANE_COUNT(b->format.colorformat); ++i) {
+		u32 stride = mdp_fmt_get_stride_contig(frame->mdp_fmt,
+			b->format.plane_fmt[0].stride, i);
+
+		b->format.plane_fmt[i].stride = stride;
+		b->format.plane_fmt[i].size =
+			mdp_fmt_get_plane_size(frame->mdp_fmt, stride,
+					       pix_mp->height, i);
+		b->iova[i] = b->iova[i - 1] + b->format.plane_fmt[i - 1].size;
+	}
+	b->usage = frame->usage;
+}
+
+void mdp_set_src_config(struct img_input *in,
+			struct mdp_frame *frame, struct vb2_buffer *vb)
+{
+	in->buffer.format.width = frame->format.fmt.pix_mp.width;
+	in->buffer.format.height = frame->format.fmt.pix_mp.height;
+	mdp_prepare_buffer(&in->buffer, frame, vb);
+}
+
+static u32 mdp_to_fixed(u32 *r, struct v4l2_fract *f)
+{
+	u32 q;
+
+	if (f->denominator == 0) {
+		*r = 0;
+		return 0;
+	}
+
+	q = f->numerator / f->denominator;
+	*r = div_u64(((u64)f->numerator - q * f->denominator) <<
+		     IMG_SUBPIXEL_SHIFT, f->denominator);
+	return q;
+}
+
+static void mdp_set_src_crop(struct img_crop *c, struct mdp_crop *crop)
+{
+	c->left = crop->c.left
+		+ mdp_to_fixed(&c->left_subpix, &crop->left_subpix);
+	c->top = crop->c.top
+		+ mdp_to_fixed(&c->top_subpix, &crop->top_subpix);
+	c->width = crop->c.width
+		+ mdp_to_fixed(&c->width_subpix, &crop->width_subpix);
+	c->height = crop->c.height
+		+ mdp_to_fixed(&c->height_subpix, &crop->height_subpix);
+}
+
+static void mdp_set_orientation(struct img_output *out,
+				s32 rotation, bool hflip, bool vflip)
+{
+	u8 flip = 0;
+
+	if (hflip)
+		flip ^= 1;
+	if (vflip) {
+		/*
+		 * A vertical flip is equivalent to
+		 * a 180-degree rotation with a horizontal flip
+		 */
+		rotation += 180;
+		flip ^= 1;
+	}
+
+	out->rotation = rotation % 360;
+	if (flip != 0)
+		out->flags |= IMG_CTRL_FLAG_HFLIP;
+	else
+		out->flags &= ~IMG_CTRL_FLAG_HFLIP;
+}
+
+void mdp_set_dst_config(struct img_output *out,
+			struct mdp_frame *frame, struct vb2_buffer *vb)
+{
+	out->buffer.format.width = frame->compose.width;
+	out->buffer.format.height = frame->compose.height;
+	mdp_prepare_buffer(&out->buffer, frame, vb);
+	mdp_set_src_crop(&out->crop, &frame->crop);
+	mdp_set_orientation(out, frame->rotation, frame->hflip, frame->vflip);
+}
+
+int mdp_frameparam_init(struct mdp_frameparam *param)
+{
+	struct mdp_frame *frame;
+
+	if (!param)
+		return -EINVAL;
+
+	INIT_LIST_HEAD(&param->list);
+	param->limit = &mdp_def_limit;
+	param->type = MDP_STREAM_TYPE_BITBLT;
+
+	frame = &param->output;
+	frame->format.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	frame->mdp_fmt = mdp_try_fmt_mplane(&frame->format, param, 0);
+	frame->ycbcr_prof =
+		mdp_map_ycbcr_prof_mplane(&frame->format,
+					  frame->mdp_fmt->mdp_color);
+	frame->usage = MDP_BUFFER_USAGE_HW_READ;
+
+	param->num_captures = 1;
+	frame = &param->captures[0];
+	frame->format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	frame->mdp_fmt = mdp_try_fmt_mplane(&frame->format, param, 0);
+	frame->ycbcr_prof =
+		mdp_map_ycbcr_prof_mplane(&frame->format,
+					  frame->mdp_fmt->mdp_color);
+	frame->usage = MDP_BUFFER_USAGE_MDP;
+	frame->crop.c.width = param->output.format.fmt.pix_mp.width;
+	frame->crop.c.height = param->output.format.fmt.pix_mp.height;
+	frame->compose.width = frame->format.fmt.pix_mp.width;
+	frame->compose.height = frame->format.fmt.pix_mp.height;
+
+	return 0;
+}
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-regs.h b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-regs.h
new file mode 100644
index 000000000..f995e536d
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-regs.h
@@ -0,0 +1,373 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#ifndef __MTK_MDP3_REGS_H__
+#define __MTK_MDP3_REGS_H__
+
+#include <linux/videodev2.h>
+#include <media/videobuf2-core.h>
+#include "mtk-img-ipi.h"
+
+/*
+ * MDP native color code
+ * Plane count: 1, 2, 3
+ * H-subsample: 0, 1, 2
+ * V-subsample: 0, 1
+ * Color group: 0-RGB, 1-YUV, 2-raw
+ */
+#define MDP_COLOR(PACKED, LOOSE, VIDEO, PLANE, HF, VF, BITS, GROUP, SWAP, ID)\
+	(((PACKED) << 27) | ((LOOSE) << 26) | ((VIDEO) << 23) |\
+	 ((PLANE) << 21) | ((HF) << 19) | ((VF) << 18) | ((BITS) << 8) |\
+	 ((GROUP) << 6) | ((SWAP) << 5) | ((ID) << 0))
+
+#define MDP_COLOR_IS_10BIT_PACKED(c)	((0x08000000 & (c)) >> 27)
+#define MDP_COLOR_IS_10BIT_LOOSE(c)	(((0x0c000000 & (c)) >> 26) == 1)
+#define MDP_COLOR_IS_10BIT_TILE(c)	(((0x0c000000 & (c)) >> 26) == 3)
+#define MDP_COLOR_IS_UFP(c)		((0x02000000 & (c)) >> 25)
+#define MDP_COLOR_IS_INTERLACED(c)	((0x01000000 & (c)) >> 24)
+#define MDP_COLOR_IS_BLOCK_MODE(c)	((0x00800000 & (c)) >> 23)
+#define MDP_COLOR_GET_PLANE_COUNT(c)	((0x00600000 & (c)) >> 21)
+#define MDP_COLOR_GET_H_SUBSAMPLE(c)	((0x00180000 & (c)) >> 19)
+#define MDP_COLOR_GET_V_SUBSAMPLE(c)	((0x00040000 & (c)) >> 18)
+#define MDP_COLOR_BITS_PER_PIXEL(c)	((0x0003ff00 & (c)) >>  8)
+#define MDP_COLOR_GET_GROUP(c)		((0x000000c0 & (c)) >>  6)
+#define MDP_COLOR_IS_SWAPPED(c)		((0x00000020 & (c)) >>  5)
+#define MDP_COLOR_GET_UNIQUE_ID(c)	((0x0000001f & (c)) >>  0)
+#define MDP_COLOR_GET_HW_FORMAT(c)	((0x0000001f & (c)) >>  0)
+
+#define MDP_COLOR_IS_RGB(c)		(MDP_COLOR_GET_GROUP(c) == 0)
+#define MDP_COLOR_IS_YUV(c)		(MDP_COLOR_GET_GROUP(c) == 1)
+
+enum mdp_color {
+	MDP_COLOR_UNKNOWN	= 0,
+
+	//MDP_COLOR_FULLG8,
+	MDP_COLOR_FULLG8_RGGB	= MDP_COLOR(0, 0, 0, 1, 0, 0,  8, 2,  0, 21),
+	MDP_COLOR_FULLG8_GRBG	= MDP_COLOR(0, 0, 0, 1, 0, 1,  8, 2,  0, 21),
+	MDP_COLOR_FULLG8_GBRG	= MDP_COLOR(0, 0, 0, 1, 1, 0,  8, 2,  0, 21),
+	MDP_COLOR_FULLG8_BGGR	= MDP_COLOR(0, 0, 0, 1, 1, 1,  8, 2,  0, 21),
+	MDP_COLOR_FULLG8	= MDP_COLOR_FULLG8_BGGR,
+
+	//MDP_COLOR_FULLG10,
+	MDP_COLOR_FULLG10_RGGB	= MDP_COLOR(0, 0, 0, 1, 0, 0, 10, 2,  0, 21),
+	MDP_COLOR_FULLG10_GRBG	= MDP_COLOR(0, 0, 0, 1, 0, 1, 10, 2,  0, 21),
+	MDP_COLOR_FULLG10_GBRG	= MDP_COLOR(0, 0, 0, 1, 1, 0, 10, 2,  0, 21),
+	MDP_COLOR_FULLG10_BGGR	= MDP_COLOR(0, 0, 0, 1, 1, 1, 10, 2,  0, 21),
+	MDP_COLOR_FULLG10	= MDP_COLOR_FULLG10_BGGR,
+
+	//MDP_COLOR_FULLG12,
+	MDP_COLOR_FULLG12_RGGB	= MDP_COLOR(0, 0, 0, 1, 0, 0, 12, 2,  0, 21),
+	MDP_COLOR_FULLG12_GRBG	= MDP_COLOR(0, 0, 0, 1, 0, 1, 12, 2,  0, 21),
+	MDP_COLOR_FULLG12_GBRG	= MDP_COLOR(0, 0, 0, 1, 1, 0, 12, 2,  0, 21),
+	MDP_COLOR_FULLG12_BGGR	= MDP_COLOR(0, 0, 0, 1, 1, 1, 12, 2,  0, 21),
+	MDP_COLOR_FULLG12	= MDP_COLOR_FULLG12_BGGR,
+
+	//MDP_COLOR_FULLG14,
+	MDP_COLOR_FULLG14_RGGB	= MDP_COLOR(0, 0, 0, 1, 0, 0, 14, 2,  0, 21),
+	MDP_COLOR_FULLG14_GRBG	= MDP_COLOR(0, 0, 0, 1, 0, 1, 14, 2,  0, 21),
+	MDP_COLOR_FULLG14_GBRG	= MDP_COLOR(0, 0, 0, 1, 1, 0, 14, 2,  0, 21),
+	MDP_COLOR_FULLG14_BGGR	= MDP_COLOR(0, 0, 0, 1, 1, 1, 14, 2,  0, 21),
+	MDP_COLOR_FULLG14	= MDP_COLOR_FULLG14_BGGR,
+
+	MDP_COLOR_UFO10		= MDP_COLOR(0, 0, 0, 1, 0, 0, 10, 2,  0, 24),
+
+	//MDP_COLOR_BAYER8,
+	MDP_COLOR_BAYER8_RGGB	= MDP_COLOR(0, 0, 0, 1, 0, 0,  8, 2,  0, 20),
+	MDP_COLOR_BAYER8_GRBG	= MDP_COLOR(0, 0, 0, 1, 0, 1,  8, 2,  0, 20),
+	MDP_COLOR_BAYER8_GBRG	= MDP_COLOR(0, 0, 0, 1, 1, 0,  8, 2,  0, 20),
+	MDP_COLOR_BAYER8_BGGR	= MDP_COLOR(0, 0, 0, 1, 1, 1,  8, 2,  0, 20),
+	MDP_COLOR_BAYER8	= MDP_COLOR_BAYER8_BGGR,
+
+	//MDP_COLOR_BAYER10,
+	MDP_COLOR_BAYER10_RGGB	= MDP_COLOR(0, 0, 0, 1, 0, 0, 10, 2,  0, 20),
+	MDP_COLOR_BAYER10_GRBG	= MDP_COLOR(0, 0, 0, 1, 0, 1, 10, 2,  0, 20),
+	MDP_COLOR_BAYER10_GBRG	= MDP_COLOR(0, 0, 0, 1, 1, 0, 10, 2,  0, 20),
+	MDP_COLOR_BAYER10_BGGR	= MDP_COLOR(0, 0, 0, 1, 1, 1, 10, 2,  0, 20),
+	MDP_COLOR_BAYER10	= MDP_COLOR_BAYER10_BGGR,
+
+	//MDP_COLOR_BAYER12,
+	MDP_COLOR_BAYER12_RGGB	= MDP_COLOR(0, 0, 0, 1, 0, 0, 12, 2,  0, 20),
+	MDP_COLOR_BAYER12_GRBG	= MDP_COLOR(0, 0, 0, 1, 0, 1, 12, 2,  0, 20),
+	MDP_COLOR_BAYER12_GBRG	= MDP_COLOR(0, 0, 0, 1, 1, 0, 12, 2,  0, 20),
+	MDP_COLOR_BAYER12_BGGR	= MDP_COLOR(0, 0, 0, 1, 1, 1, 12, 2,  0, 20),
+	MDP_COLOR_BAYER12	= MDP_COLOR_BAYER12_BGGR,
+
+	//MDP_COLOR_BAYER14,
+	MDP_COLOR_BAYER14_RGGB	= MDP_COLOR(0, 0, 0, 1, 0, 0, 14, 2,  0, 20),
+	MDP_COLOR_BAYER14_GRBG	= MDP_COLOR(0, 0, 0, 1, 0, 1, 14, 2,  0, 20),
+	MDP_COLOR_BAYER14_GBRG	= MDP_COLOR(0, 0, 0, 1, 1, 0, 14, 2,  0, 20),
+	MDP_COLOR_BAYER14_BGGR	= MDP_COLOR(0, 0, 0, 1, 1, 1, 14, 2,  0, 20),
+	MDP_COLOR_BAYER14	= MDP_COLOR_BAYER14_BGGR,
+
+	MDP_COLOR_RGB48		= MDP_COLOR(0, 0, 0, 1, 0, 0, 48, 0,  0, 23),
+	/* For bayer+mono raw-16 */
+	MDP_COLOR_RGB565_RAW	= MDP_COLOR(0, 0, 0, 1, 0, 0, 16, 2,  0, 0),
+
+	MDP_COLOR_BAYER8_UNPAK	= MDP_COLOR(0, 0, 0, 1, 0, 0,  8, 2,  0, 22),
+	MDP_COLOR_BAYER10_UNPAK	= MDP_COLOR(0, 0, 0, 1, 0, 0, 10, 2,  0, 22),
+	MDP_COLOR_BAYER12_UNPAK	= MDP_COLOR(0, 0, 0, 1, 0, 0, 12, 2,  0, 22),
+	MDP_COLOR_BAYER14_UNPAK	= MDP_COLOR(0, 0, 0, 1, 0, 0, 14, 2,  0, 22),
+
+	/* Unified formats */
+	MDP_COLOR_GREY		= MDP_COLOR(0, 0, 0, 1, 0, 0,  8, 1,  0, 7),
+
+	MDP_COLOR_RGB565	= MDP_COLOR(0, 0, 0, 1, 0, 0, 16, 0,  0, 0),
+	MDP_COLOR_BGR565	= MDP_COLOR(0, 0, 0, 1, 0, 0, 16, 0,  1, 0),
+	MDP_COLOR_RGB888	= MDP_COLOR(0, 0, 0, 1, 0, 0, 24, 0,  1, 1),
+	MDP_COLOR_BGR888	= MDP_COLOR(0, 0, 0, 1, 0, 0, 24, 0,  0, 1),
+	MDP_COLOR_RGBA8888	= MDP_COLOR(0, 0, 0, 1, 0, 0, 32, 0,  1, 2),
+	MDP_COLOR_BGRA8888	= MDP_COLOR(0, 0, 0, 1, 0, 0, 32, 0,  0, 2),
+	MDP_COLOR_ARGB8888	= MDP_COLOR(0, 0, 0, 1, 0, 0, 32, 0,  1, 3),
+	MDP_COLOR_ABGR8888	= MDP_COLOR(0, 0, 0, 1, 0, 0, 32, 0,  0, 3),
+
+	MDP_COLOR_UYVY		= MDP_COLOR(0, 0, 0, 1, 1, 0, 16, 1,  0, 4),
+	MDP_COLOR_VYUY		= MDP_COLOR(0, 0, 0, 1, 1, 0, 16, 1,  1, 4),
+	MDP_COLOR_YUYV		= MDP_COLOR(0, 0, 0, 1, 1, 0, 16, 1,  0, 5),
+	MDP_COLOR_YVYU		= MDP_COLOR(0, 0, 0, 1, 1, 0, 16, 1,  1, 5),
+
+	MDP_COLOR_I420		= MDP_COLOR(0, 0, 0, 3, 1, 1,  8, 1,  0, 8),
+	MDP_COLOR_YV12		= MDP_COLOR(0, 0, 0, 3, 1, 1,  8, 1,  1, 8),
+	MDP_COLOR_I422		= MDP_COLOR(0, 0, 0, 3, 1, 0,  8, 1,  0, 9),
+	MDP_COLOR_YV16		= MDP_COLOR(0, 0, 0, 3, 1, 0,  8, 1,  1, 9),
+	MDP_COLOR_I444		= MDP_COLOR(0, 0, 0, 3, 0, 0,  8, 1,  0, 10),
+	MDP_COLOR_YV24		= MDP_COLOR(0, 0, 0, 3, 0, 0,  8, 1,  1, 10),
+
+	MDP_COLOR_NV12		= MDP_COLOR(0, 0, 0, 2, 1, 1,  8, 1,  0, 12),
+	MDP_COLOR_NV21		= MDP_COLOR(0, 0, 0, 2, 1, 1,  8, 1,  1, 12),
+	MDP_COLOR_NV16		= MDP_COLOR(0, 0, 0, 2, 1, 0,  8, 1,  0, 13),
+	MDP_COLOR_NV61		= MDP_COLOR(0, 0, 0, 2, 1, 0,  8, 1,  1, 13),
+	MDP_COLOR_NV24		= MDP_COLOR(0, 0, 0, 2, 0, 0,  8, 1,  0, 14),
+	MDP_COLOR_NV42		= MDP_COLOR(0, 0, 0, 2, 0, 0,  8, 1,  1, 14),
+
+	/* MediaTek proprietary formats */
+	/* UFO encoded block mode */
+	MDP_COLOR_420_BLK_UFO	= MDP_COLOR(0, 0, 5, 2, 1, 1, 256, 1, 0, 12),
+	/* Block mode */
+	MDP_COLOR_420_BLK	= MDP_COLOR(0, 0, 1, 2, 1, 1, 256, 1, 0, 12),
+	/* Block mode + field mode */
+	MDP_COLOR_420_BLKI	= MDP_COLOR(0, 0, 3, 2, 1, 1, 256, 1, 0, 12),
+	/* Block mode */
+	MDP_COLOR_422_BLK	= MDP_COLOR(0, 0, 1, 1, 1, 0, 512, 1, 0, 4),
+
+	MDP_COLOR_IYU2		= MDP_COLOR(0, 0, 0, 1, 0, 0, 24,  1, 0, 25),
+	MDP_COLOR_YUV444	= MDP_COLOR(0, 0, 0, 1, 0, 0, 24,  1, 0, 30),
+
+	/* Packed 10-bit formats */
+	MDP_COLOR_RGBA1010102	= MDP_COLOR(1, 0, 0, 1, 0, 0, 32,  0, 1, 2),
+	MDP_COLOR_BGRA1010102	= MDP_COLOR(1, 0, 0, 1, 0, 0, 32,  0, 0, 2),
+	/* Packed 10-bit UYVY */
+	MDP_COLOR_UYVY_10P	= MDP_COLOR(1, 0, 0, 1, 1, 0, 20,  1, 0, 4),
+	/* Packed 10-bit NV21 */
+	MDP_COLOR_NV21_10P	= MDP_COLOR(1, 0, 0, 2, 1, 1, 10,  1, 1, 12),
+	/* 10-bit block mode */
+	MDP_COLOR_420_BLK_10_H	= MDP_COLOR(1, 0, 1, 2, 1, 1, 320, 1, 0, 12),
+	/* 10-bit HEVC tile mode */
+	MDP_COLOR_420_BLK_10_V	= MDP_COLOR(1, 1, 1, 2, 1, 1, 320, 1, 0, 12),
+	/* UFO encoded 10-bit block mode */
+	MDP_COLOR_420_BLK_U10_H	= MDP_COLOR(1, 0, 5, 2, 1, 1, 320, 1, 0, 12),
+	/* UFO encoded 10-bit HEVC tile mode */
+	MDP_COLOR_420_BLK_U10_V	= MDP_COLOR(1, 1, 5, 2, 1, 1, 320, 1, 0, 12),
+
+	/* Loose 10-bit formats */
+	MDP_COLOR_UYVY_10L	= MDP_COLOR(0, 1, 0, 1, 1, 0, 20,  1, 0, 4),
+	MDP_COLOR_VYUY_10L	= MDP_COLOR(0, 1, 0, 1, 1, 0, 20,  1, 1, 4),
+	MDP_COLOR_YUYV_10L	= MDP_COLOR(0, 1, 0, 1, 1, 0, 20,  1, 0, 5),
+	MDP_COLOR_YVYU_10L	= MDP_COLOR(0, 1, 0, 1, 1, 0, 20,  1, 1, 5),
+	MDP_COLOR_NV12_10L	= MDP_COLOR(0, 1, 0, 2, 1, 1, 10,  1, 0, 12),
+	MDP_COLOR_NV21_10L	= MDP_COLOR(0, 1, 0, 2, 1, 1, 10,  1, 1, 12),
+	MDP_COLOR_NV16_10L	= MDP_COLOR(0, 1, 0, 2, 1, 0, 10,  1, 0, 13),
+	MDP_COLOR_NV61_10L	= MDP_COLOR(0, 1, 0, 2, 1, 0, 10,  1, 1, 13),
+	MDP_COLOR_YV12_10L	= MDP_COLOR(0, 1, 0, 3, 1, 1, 10,  1, 1, 8),
+	MDP_COLOR_I420_10L	= MDP_COLOR(0, 1, 0, 3, 1, 1, 10,  1, 0, 8),
+};
+
+static inline bool MDP_COLOR_IS_UV_COPLANE(enum mdp_color c)
+{
+	return (MDP_COLOR_GET_PLANE_COUNT(c) == 2 && MDP_COLOR_IS_YUV(c));
+}
+
+/* Minimum Y stride that is accepted by MDP HW */
+static inline u32 mdp_color_get_min_y_stride(enum mdp_color c, u32 width)
+{
+	return ((MDP_COLOR_BITS_PER_PIXEL(c) * width) + 4) >> 3;
+}
+
+/* Minimum UV stride that is accepted by MDP HW */
+static inline u32 mdp_color_get_min_uv_stride(enum mdp_color c, u32 width)
+{
+	u32 min_stride;
+
+	if (MDP_COLOR_GET_PLANE_COUNT(c) == 1)
+		return 0;
+	min_stride = mdp_color_get_min_y_stride(c, width)
+		>> MDP_COLOR_GET_H_SUBSAMPLE(c);
+	if (MDP_COLOR_IS_UV_COPLANE(c) && !MDP_COLOR_IS_BLOCK_MODE(c))
+		min_stride = min_stride * 2;
+	return min_stride;
+}
+
+/* Minimum Y plane size that is necessary in buffer */
+static inline u32 mdp_color_get_min_y_size(enum mdp_color c,
+					   u32 width, u32 height)
+{
+	if (MDP_COLOR_IS_BLOCK_MODE(c))
+		return ((MDP_COLOR_BITS_PER_PIXEL(c) * width) >> 8) * height;
+	return mdp_color_get_min_y_stride(c, width) * height;
+}
+
+/* Minimum UV plane size that is necessary in buffer */
+static inline u32 mdp_color_get_min_uv_size(enum mdp_color c,
+					    u32 width, u32 height)
+{
+	height = height >> MDP_COLOR_GET_V_SUBSAMPLE(c);
+	if (MDP_COLOR_IS_BLOCK_MODE(c) && (MDP_COLOR_GET_PLANE_COUNT(c) > 1))
+		return ((MDP_COLOR_BITS_PER_PIXEL(c) * width) >> 8) * height;
+	return mdp_color_get_min_uv_stride(c, width) * height;
+}
+
+/* Combine colorspace, xfer_func, ycbcr_encoding, and quantization */
+enum mdp_ycbcr_profile {
+	/* V4L2_YCBCR_ENC_601 and V4L2_QUANTIZATION_LIM_RANGE */
+	MDP_YCBCR_PROFILE_BT601,
+	/* V4L2_YCBCR_ENC_709 and V4L2_QUANTIZATION_LIM_RANGE */
+	MDP_YCBCR_PROFILE_BT709,
+	/* V4L2_YCBCR_ENC_601 and V4L2_QUANTIZATION_FULL_RANGE */
+	MDP_YCBCR_PROFILE_JPEG,
+	MDP_YCBCR_PROFILE_FULL_BT601 = MDP_YCBCR_PROFILE_JPEG,
+
+	/* Colorspaces not support for capture */
+	/* V4L2_YCBCR_ENC_BT2020 and V4L2_QUANTIZATION_LIM_RANGE */
+	MDP_YCBCR_PROFILE_BT2020,
+	/* V4L2_YCBCR_ENC_709 and V4L2_QUANTIZATION_FULL_RANGE */
+	MDP_YCBCR_PROFILE_FULL_BT709,
+	/* V4L2_YCBCR_ENC_BT2020 and V4L2_QUANTIZATION_FULL_RANGE */
+	MDP_YCBCR_PROFILE_FULL_BT2020,
+};
+
+#define MDP_FMT_FLAG_OUTPUT	BIT(0)
+#define MDP_FMT_FLAG_CAPTURE	BIT(1)
+
+struct mdp_format {
+	u32	pixelformat;
+	u32	mdp_color;
+	u8	depth[VIDEO_MAX_PLANES];
+	u8	row_depth[VIDEO_MAX_PLANES];
+	u8	num_planes;
+	u8	walign;
+	u8	halign;
+	u8	salign;
+	u32	flags;
+};
+
+struct mdp_pix_limit {
+	u32	wmin;
+	u32	hmin;
+	u32	wmax;
+	u32	hmax;
+};
+
+struct mdp_limit {
+	struct mdp_pix_limit	out_limit;
+	struct mdp_pix_limit	cap_limit;
+	u32			h_scale_up_max;
+	u32			v_scale_up_max;
+	u32			h_scale_down_max;
+	u32			v_scale_down_max;
+};
+
+enum mdp_stream_type {
+	MDP_STREAM_TYPE_UNKNOWN,
+	MDP_STREAM_TYPE_BITBLT,
+	MDP_STREAM_TYPE_GPU_BITBLT,
+	MDP_STREAM_TYPE_DUAL_BITBLT,
+	MDP_STREAM_TYPE_2ND_BITBLT,
+	MDP_STREAM_TYPE_ISP_IC,
+	MDP_STREAM_TYPE_ISP_VR,
+	MDP_STREAM_TYPE_ISP_ZSD,
+	MDP_STREAM_TYPE_ISP_IP,
+	MDP_STREAM_TYPE_ISP_VSS,
+	MDP_STREAM_TYPE_ISP_ZSD_SLOW,
+	MDP_STREAM_TYPE_WPE,
+	MDP_STREAM_TYPE_WPE2,
+};
+
+struct mdp_crop {
+	struct v4l2_rect	c;
+	struct v4l2_fract	left_subpix;
+	struct v4l2_fract	top_subpix;
+	struct v4l2_fract	width_subpix;
+	struct v4l2_fract	height_subpix;
+};
+
+struct mdp_frame {
+	struct v4l2_format	format;
+	const struct mdp_format	*mdp_fmt;
+	u32			ycbcr_prof;	/* enum mdp_ycbcr_profile */
+	u32			usage;		/* enum mdp_buffer_usage */
+	struct mdp_crop		crop;
+	struct v4l2_rect	compose;
+	s32			rotation;
+	u32			hflip:1;
+	u32			vflip:1;
+	u32			hdr:1;
+	u32			dre:1;
+	u32			sharpness:1;
+	u32			dither:1;
+};
+
+static inline bool mdp_target_is_crop(u32 target)
+{
+	return (target == V4L2_SEL_TGT_CROP) ||
+		(target == V4L2_SEL_TGT_CROP_DEFAULT) ||
+		(target == V4L2_SEL_TGT_CROP_BOUNDS);
+}
+
+static inline bool mdp_target_is_compose(u32 target)
+{
+	return (target == V4L2_SEL_TGT_COMPOSE) ||
+		(target == V4L2_SEL_TGT_COMPOSE_DEFAULT) ||
+		(target == V4L2_SEL_TGT_COMPOSE_BOUNDS);
+}
+
+#define MDP_MAX_CAPTURES	IMG_MAX_HW_OUTPUTS
+
+#define MDP_VPU_INIT		BIT(0)
+#define MDP_M2M_CTX_ERROR	BIT(1)
+
+struct mdp_frameparam {
+	struct list_head	list;
+	struct mdp_m2m_ctx	*ctx;
+	atomic_t		state;
+	const struct mdp_limit	*limit;
+	u32			type;	/* enum mdp_stream_type */
+	u32			frame_no;
+	struct mdp_frame	output;
+	struct mdp_frame	captures[MDP_MAX_CAPTURES];
+	u32			num_captures;
+	enum v4l2_colorspace		colorspace;
+	enum v4l2_ycbcr_encoding	ycbcr_enc;
+	enum v4l2_xfer_func		xfer_func;
+	enum v4l2_quantization		quant;
+};
+
+int mdp_enum_fmt_mplane(struct v4l2_fmtdesc *f);
+const struct mdp_format *mdp_try_fmt_mplane(struct v4l2_format *f,
+					    struct mdp_frameparam *param,
+					    u32 ctx_id);
+enum mdp_ycbcr_profile mdp_map_ycbcr_prof_mplane(struct v4l2_format *f,
+						 u32 mdp_color);
+int mdp_try_crop(struct mdp_m2m_ctx *ctx, struct v4l2_rect *r,
+		 const struct v4l2_selection *s, struct mdp_frame *frame);
+int mdp_check_scaling_ratio(const struct v4l2_rect *crop,
+			    const struct v4l2_rect *compose, s32 rotation,
+	const struct mdp_limit *limit);
+void mdp_set_src_config(struct img_input *in,
+			struct mdp_frame *frame, struct vb2_buffer *vb);
+void mdp_set_dst_config(struct img_output *out,
+			struct mdp_frame *frame, struct vb2_buffer *vb);
+int mdp_frameparam_init(struct mdp_frameparam *param);
+
+#endif  /* __MTK_MDP3_REGS_H__ */
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-vpu.c b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-vpu.c
new file mode 100644
index 000000000..a72bed927
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-vpu.c
@@ -0,0 +1,314 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#include <linux/remoteproc.h>
+#include <linux/remoteproc/mtk_scp.h>
+#include "mtk-mdp3-vpu.h"
+#include "mtk-mdp3-core.h"
+
+#define MDP_VPU_MESSAGE_TIMEOUT 500U
+#define vpu_alloc_size		0x600000
+
+static inline struct mdp_dev *vpu_to_mdp(struct mdp_vpu_dev *vpu)
+{
+	return container_of(vpu, struct mdp_dev, vpu);
+}
+
+static int mdp_vpu_shared_mem_alloc(struct mdp_vpu_dev *vpu)
+{
+	if (vpu->work && vpu->work_addr)
+		return 0;
+
+	vpu->work = dma_alloc_coherent(scp_get_device(vpu->scp), vpu_alloc_size,
+				       &vpu->work_addr, GFP_KERNEL);
+
+	if (!vpu->work)
+		return -ENOMEM;
+	else
+		return 0;
+}
+
+void mdp_vpu_shared_mem_free(struct mdp_vpu_dev *vpu)
+{
+	if (vpu->work && vpu->work_addr)
+		dma_free_coherent(scp_get_device(vpu->scp), vpu_alloc_size,
+				  vpu->work, vpu->work_addr);
+}
+
+static void mdp_vpu_ipi_handle_init_ack(void *data, unsigned int len,
+					void *priv)
+{
+	struct mdp_ipi_init_msg *msg = (struct mdp_ipi_init_msg *)data;
+	struct mdp_vpu_dev *vpu =
+		(struct mdp_vpu_dev *)(unsigned long)msg->drv_data;
+
+	if (!vpu->work_size)
+		vpu->work_size = msg->work_size;
+
+	vpu->status = msg->status;
+	complete(&vpu->ipi_acked);
+}
+
+static void mdp_vpu_ipi_handle_deinit_ack(void *data, unsigned int len,
+					  void *priv)
+{
+	struct mdp_ipi_deinit_msg *msg = (struct mdp_ipi_deinit_msg *)data;
+	struct mdp_vpu_dev *vpu =
+		(struct mdp_vpu_dev *)(unsigned long)msg->drv_data;
+
+	vpu->status = msg->status;
+	complete(&vpu->ipi_acked);
+}
+
+static void mdp_vpu_ipi_handle_frame_ack(void *data, unsigned int len,
+					 void *priv)
+{
+	struct img_sw_addr *addr = (struct img_sw_addr *)data;
+	struct img_ipi_frameparam *param =
+		(struct img_ipi_frameparam *)(unsigned long)addr->va;
+	struct mdp_vpu_ctx *ctx =
+		(struct mdp_vpu_ctx *)(unsigned long)param->drv_data;
+
+	if (param->state) {
+		struct mdp_dev *mdp = vpu_to_mdp(ctx->vpu_dev);
+
+		dev_err(&mdp->pdev->dev, "VPU MDP failure:%d\n", param->state);
+	}
+	ctx->vpu_dev->status = param->state;
+	complete(&ctx->vpu_dev->ipi_acked);
+}
+
+int mdp_vpu_register(struct mdp_dev *mdp)
+{
+	int err;
+	struct mtk_scp *scp = mdp->scp;
+	struct device *dev = &mdp->pdev->dev;
+
+	err = scp_ipi_register(scp, SCP_IPI_MDP_INIT,
+			       mdp_vpu_ipi_handle_init_ack, NULL);
+	if (err) {
+		dev_err(dev, "scp_ipi_register failed %d\n", err);
+		goto err_ipi_init;
+	}
+	err = scp_ipi_register(scp, SCP_IPI_MDP_DEINIT,
+			       mdp_vpu_ipi_handle_deinit_ack, NULL);
+	if (err) {
+		dev_err(dev, "scp_ipi_register failed %d\n", err);
+		goto err_ipi_deinit;
+	}
+	err = scp_ipi_register(scp, SCP_IPI_MDP_FRAME,
+			       mdp_vpu_ipi_handle_frame_ack, NULL);
+	if (err) {
+		dev_err(dev, "scp_ipi_register failed %d\n", err);
+		goto err_ipi_frame;
+	}
+	return 0;
+
+err_ipi_frame:
+	scp_ipi_unregister(scp, SCP_IPI_MDP_DEINIT);
+err_ipi_deinit:
+	scp_ipi_unregister(scp, SCP_IPI_MDP_INIT);
+err_ipi_init:
+
+	return err;
+}
+
+void mdp_vpu_unregister(struct mdp_dev *mdp)
+{
+	scp_ipi_unregister(mdp->scp, SCP_IPI_MDP_INIT);
+	scp_ipi_unregister(mdp->scp, SCP_IPI_MDP_DEINIT);
+	scp_ipi_unregister(mdp->scp, SCP_IPI_MDP_FRAME);
+}
+
+static int mdp_vpu_sendmsg(struct mdp_vpu_dev *vpu, enum scp_ipi_id id,
+			   void *buf, unsigned int len)
+{
+	struct mdp_dev *mdp = vpu_to_mdp(vpu);
+	unsigned int t = MDP_VPU_MESSAGE_TIMEOUT;
+	int ret;
+
+	if (!vpu->scp) {
+		dev_dbg(&mdp->pdev->dev, "vpu scp is NULL");
+		return -EINVAL;
+	}
+	ret = scp_ipi_send(vpu->scp, id, buf, len, 2000);
+
+	if (ret) {
+		dev_err(&mdp->pdev->dev, "scp_ipi_send failed %d\n", ret);
+		return -EPERM;
+	}
+	ret = wait_for_completion_timeout(&vpu->ipi_acked,
+					  msecs_to_jiffies(t));
+	if (!ret)
+		ret = -ETIME;
+	else if (vpu->status)
+		ret = -EINVAL;
+	else
+		ret = 0;
+	return ret;
+}
+
+int mdp_vpu_dev_init(struct mdp_vpu_dev *vpu, struct mtk_scp *scp,
+		     struct mutex *lock)
+{
+	struct mdp_ipi_init_msg msg = {
+		.drv_data = (unsigned long)vpu,
+	};
+	size_t mem_size;
+	phys_addr_t pool;
+	const size_t pool_size = sizeof(struct mdp_config_pool);
+	struct mdp_dev *mdp = vpu_to_mdp(vpu);
+	int err;
+
+	init_completion(&vpu->ipi_acked);
+	vpu->scp = scp;
+	vpu->lock = lock;
+	vpu->work_size = 0;
+	err = mdp_vpu_sendmsg(vpu, SCP_IPI_MDP_INIT, &msg, sizeof(msg));
+	if (err)
+		goto err_work_size;
+	/* vpu work_size was set in mdp_vpu_ipi_handle_init_ack */
+
+	mem_size = vpu_alloc_size;
+	err = mdp_vpu_shared_mem_alloc(vpu);
+	if (err) {
+		dev_err(&mdp->pdev->dev, "VPU memory alloc fail!");
+		goto err_mem_alloc;
+	}
+
+	pool = ALIGN((uintptr_t)vpu->work + vpu->work_size, 8);
+	if (pool + pool_size - (uintptr_t)vpu->work > mem_size) {
+		dev_err(&mdp->pdev->dev,
+			"VPU memory insufficient: %zx + %zx > %zx",
+			vpu->work_size, pool_size, mem_size);
+		err = -ENOMEM;
+		goto err_mem_size;
+	}
+
+	dev_dbg(&mdp->pdev->dev,
+		"VPU work:%pK pa:%pad sz:%zx pool:%pa sz:%zx (mem sz:%zx)",
+		vpu->work, &vpu->work_addr, vpu->work_size,
+		&pool, pool_size, mem_size);
+	vpu->pool = (struct mdp_config_pool *)(uintptr_t)pool;
+	msg.work_addr = vpu->work_addr;
+	msg.work_size = vpu->work_size;
+	err = mdp_vpu_sendmsg(vpu, SCP_IPI_MDP_INIT, &msg, sizeof(msg));
+	if (err)
+		goto err_work_size;
+
+	memset(vpu->pool, 0, sizeof(*vpu->pool));
+	return 0;
+
+err_work_size:
+	switch (vpu->status) {
+	case -MDP_IPI_EBUSY:
+		err = -EBUSY;
+		break;
+	case -MDP_IPI_ENOMEM:
+		err = -ENOSPC;	/* -ENOMEM */
+		break;
+	}
+	return err;
+err_mem_size:
+err_mem_alloc:
+	return err;
+}
+
+int mdp_vpu_dev_deinit(struct mdp_vpu_dev *vpu)
+{
+	struct mdp_ipi_deinit_msg msg = {
+		.drv_data = (unsigned long)vpu,
+		.work_addr = vpu->work_addr,
+	};
+
+	return mdp_vpu_sendmsg(vpu, SCP_IPI_MDP_DEINIT, &msg, sizeof(msg));
+}
+
+static struct img_config *mdp_config_get(struct mdp_vpu_dev *vpu,
+					 enum mdp_config_id id, uint32_t *addr)
+{
+	struct img_config *config;
+
+	if (id < 0 || id >= MDP_CONFIG_POOL_SIZE)
+		return ERR_PTR(-EINVAL);
+
+	mutex_lock(vpu->lock);
+	vpu->pool->cfg_count[id]++;
+	config = &vpu->pool->configs[id];
+	*addr = vpu->work_addr + ((uintptr_t)config - (uintptr_t)vpu->work);
+	mutex_unlock(vpu->lock);
+
+	return config;
+}
+
+static int mdp_config_put(struct mdp_vpu_dev *vpu,
+			  enum mdp_config_id id,
+			  const struct img_config *config)
+{
+	int err = 0;
+
+	if (id < 0 || id >= MDP_CONFIG_POOL_SIZE)
+		return -EINVAL;
+	if (vpu->lock)
+		mutex_lock(vpu->lock);
+	if (!vpu->pool->cfg_count[id] || config != &vpu->pool->configs[id])
+		err = -EINVAL;
+	else
+		vpu->pool->cfg_count[id]--;
+	if (vpu->lock)
+		mutex_unlock(vpu->lock);
+	return err;
+}
+
+int mdp_vpu_ctx_init(struct mdp_vpu_ctx *ctx, struct mdp_vpu_dev *vpu,
+		     enum mdp_config_id id)
+{
+	ctx->config = mdp_config_get(vpu, id, &ctx->inst_addr);
+	if (IS_ERR(ctx->config)) {
+		int err = PTR_ERR(ctx->config);
+
+		ctx->config = NULL;
+		return err;
+	}
+	ctx->config_id = id;
+	ctx->vpu_dev = vpu;
+	return 0;
+}
+
+int mdp_vpu_ctx_deinit(struct mdp_vpu_ctx *ctx)
+{
+	int err = mdp_config_put(ctx->vpu_dev, ctx->config_id, ctx->config);
+
+	ctx->config_id = 0;
+	ctx->config = NULL;
+	ctx->inst_addr = 0;
+	return err;
+}
+
+int mdp_vpu_process(struct mdp_vpu_ctx *ctx, struct img_ipi_frameparam *param)
+{
+	struct mdp_vpu_dev *vpu = ctx->vpu_dev;
+	struct mdp_dev *mdp = vpu_to_mdp(vpu);
+	struct img_sw_addr addr;
+
+	if (!ctx->vpu_dev->work || !ctx->vpu_dev->work_addr) {
+		if (mdp_vpu_shared_mem_alloc(vpu)) {
+			dev_err(&mdp->pdev->dev, "VPU memory alloc fail!");
+			return -ENOMEM;
+		}
+	}
+	memset((void *)ctx->vpu_dev->work, 0, ctx->vpu_dev->work_size);
+	memset(ctx->config, 0, sizeof(*ctx->config));
+	param->config_data.va = (unsigned long)ctx->config;
+	param->config_data.pa = ctx->inst_addr;
+	param->drv_data = (unsigned long)ctx;
+
+	memcpy((void *)ctx->vpu_dev->work, param, sizeof(*param));
+	addr.pa = ctx->vpu_dev->work_addr;
+	addr.va = (uintptr_t)ctx->vpu_dev->work;
+	return mdp_vpu_sendmsg(ctx->vpu_dev, SCP_IPI_MDP_FRAME,
+		&addr, sizeof(addr));
+}
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-vpu.h b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-vpu.h
new file mode 100644
index 000000000..244b3a32d
--- /dev/null
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-vpu.h
@@ -0,0 +1,78 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
+ */
+
+#ifndef __MTK_MDP3_VPU_H__
+#define __MTK_MDP3_VPU_H__
+
+#include <linux/platform_device.h>
+#include "mtk-img-ipi.h"
+
+enum mdp_ipi_result {
+	MDP_IPI_SUCCESS	= 0,
+	MDP_IPI_ENOMEM	= 12,
+	MDP_IPI_EBUSY	= 16,
+	MDP_IPI_EINVAL	= 22,
+	MDP_IPI_EMINST	= 24,
+	MDP_IPI_ERANGE	= 34,
+	MDP_IPI_NR_ERRNO,
+
+	MDP_IPI_EOTHER	= MDP_IPI_NR_ERRNO,
+	MDP_IPI_PATH_CANT_MERGE,
+	MDP_IPI_OP_FAIL,
+};
+
+struct mdp_ipi_init_msg {
+	u32	status;
+	u64	drv_data;
+	u32	work_addr;	/* [in] working buffer address */
+	u32	work_size;	/* [in] working buffer size */
+} __packed;
+
+struct mdp_ipi_deinit_msg {
+	u32	status;
+	u64	drv_data;
+	u32	work_addr;
+} __packed;
+
+enum mdp_config_id {
+	MDP_DEV_M2M = 0,
+	MDP_CONFIG_POOL_SIZE	/* ALWAYS keep at the end */
+};
+
+struct mdp_config_pool {
+	u64			cfg_count[MDP_CONFIG_POOL_SIZE];
+	struct img_config	configs[MDP_CONFIG_POOL_SIZE];
+};
+
+struct mdp_vpu_dev {
+	/* synchronization protect for accessing vpu working buffer info */
+	struct mutex		*lock;
+	struct mtk_scp		*scp;
+	struct completion	ipi_acked;
+	void			*work;
+	dma_addr_t		work_addr;
+	size_t			work_size;
+	struct mdp_config_pool	*pool;
+	u32			status;
+};
+
+struct mdp_vpu_ctx {
+	struct mdp_vpu_dev	*vpu_dev;
+	u32			config_id;
+	struct img_config	*config;
+	u32			inst_addr;
+};
+
+void mdp_vpu_shared_mem_free(struct mdp_vpu_dev *vpu);
+int mdp_vpu_dev_init(struct mdp_vpu_dev *vpu, struct mtk_scp *scp,
+		     struct mutex *lock /* for sync */);
+int mdp_vpu_dev_deinit(struct mdp_vpu_dev *vpu);
+int mdp_vpu_ctx_init(struct mdp_vpu_ctx *ctx, struct mdp_vpu_dev *vpu,
+		     enum mdp_config_id id);
+int mdp_vpu_ctx_deinit(struct mdp_vpu_ctx *ctx);
+int mdp_vpu_process(struct mdp_vpu_ctx *vpu, struct img_ipi_frameparam *param);
+
+#endif  /* __MTK_MDP3_VPU_H__ */
diff --git a/drivers/media/platform/mediatek/vcodec/Kconfig b/drivers/media/platform/mediatek/vcodec/Kconfig
new file mode 100644
index 000000000..74b00eb1b
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/Kconfig
@@ -0,0 +1,37 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_MEDIATEK_VCODEC_SCP
+	bool
+
+config VIDEO_MEDIATEK_VCODEC_VPU
+	bool
+
+config VIDEO_MEDIATEK_VCODEC
+	tristate "Mediatek Video Codec driver"
+	depends on V4L_MEM2MEM_DRIVERS
+	depends on MTK_IOMMU || COMPILE_TEST
+	depends on VIDEO_DEV
+	depends on ARCH_MEDIATEK || COMPILE_TEST
+	depends on VIDEO_MEDIATEK_VPU || MTK_SCP
+	# The two following lines ensure we have the same state ("m" or "y") as
+	# our dependencies, to avoid missing symbols during link.
+	depends on VIDEO_MEDIATEK_VPU || !VIDEO_MEDIATEK_VPU
+	depends on MTK_SCP || !MTK_SCP
+	depends on MTK_SMI || (COMPILE_TEST && MTK_SMI=n)
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	select VIDEO_MEDIATEK_VCODEC_VPU if VIDEO_MEDIATEK_VPU
+	select VIDEO_MEDIATEK_VCODEC_SCP if MTK_SCP
+	select V4L2_H264
+	select V4L2_VP9
+	select MEDIA_CONTROLLER
+	select MEDIA_CONTROLLER_REQUEST_API
+	help
+	  Mediatek video codec driver provides HW capability to
+	  encode and decode in a range of video formats on MT8173
+	  and MT8183.
+
+	  Note that support for MT8173 requires VIDEO_MEDIATEK_VPU to
+	  also be selected. Support for MT8183 depends on MTK_SCP.
+
+	  To compile this driver as modules, choose M here: the
+	  modules will be called mtk-vcodec-dec and mtk-vcodec-enc.
diff --git a/drivers/media/platform/mediatek/vcodec/Makefile b/drivers/media/platform/mediatek/vcodec/Makefile
new file mode 100644
index 000000000..93e7a343b
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/Makefile
@@ -0,0 +1,46 @@
+# SPDX-License-Identifier: GPL-2.0
+
+obj-$(CONFIG_VIDEO_MEDIATEK_VCODEC) += mtk-vcodec-dec.o \
+				       mtk-vcodec-enc.o \
+				       mtk-vcodec-common.o \
+				       mtk-vcodec-dec-hw.o
+
+mtk-vcodec-dec-y := vdec/vdec_h264_if.o \
+		vdec/vdec_vp8_if.o \
+		vdec/vdec_vp8_req_if.o \
+		vdec/vdec_vp9_if.o \
+		vdec/vdec_vp9_req_lat_if.o \
+		vdec/vdec_h264_req_if.o \
+		vdec/vdec_h264_req_common.o \
+		vdec/vdec_h264_req_multi_if.o \
+		mtk_vcodec_dec_drv.o \
+		vdec_drv_if.o \
+		vdec_vpu_if.o \
+		vdec_msg_queue.o \
+		mtk_vcodec_dec.o \
+		mtk_vcodec_dec_stateful.o \
+		mtk_vcodec_dec_stateless.o \
+		mtk_vcodec_dec_pm.o \
+
+mtk-vcodec-dec-hw-y := mtk_vcodec_dec_hw.o
+
+mtk-vcodec-enc-y := venc/venc_vp8_if.o \
+		venc/venc_h264_if.o \
+		mtk_vcodec_enc.o \
+		mtk_vcodec_enc_drv.o \
+		mtk_vcodec_enc_pm.o \
+		venc_drv_if.o \
+		venc_vpu_if.o \
+
+
+mtk-vcodec-common-y := mtk_vcodec_intr.o \
+		mtk_vcodec_util.o \
+		mtk_vcodec_fw.o \
+
+ifneq ($(CONFIG_VIDEO_MEDIATEK_VCODEC_VPU),)
+mtk-vcodec-common-y += mtk_vcodec_fw_vpu.o
+endif
+
+ifneq ($(CONFIG_VIDEO_MEDIATEK_VCODEC_SCP),)
+mtk-vcodec-common-y += mtk_vcodec_fw_scp.o
+endif
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec.c
new file mode 100644
index 000000000..93fcea821
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec.c
@@ -0,0 +1,1010 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: PC Chen <pc.chen@mediatek.com>
+ *         Tiffany Lin <tiffany.lin@mediatek.com>
+ */
+
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_dec.h"
+#include "mtk_vcodec_intr.h"
+#include "mtk_vcodec_util.h"
+#include "vdec_drv_if.h"
+#include "mtk_vcodec_dec_pm.h"
+
+#define DFT_CFG_WIDTH	MTK_VDEC_MIN_W
+#define DFT_CFG_HEIGHT	MTK_VDEC_MIN_H
+
+static const struct mtk_video_fmt *
+mtk_vdec_find_format(struct v4l2_format *f,
+		     const struct mtk_vcodec_dec_pdata *dec_pdata)
+{
+	const struct mtk_video_fmt *fmt;
+	unsigned int k;
+
+	for (k = 0; k < *dec_pdata->num_formats; k++) {
+		fmt = &dec_pdata->vdec_formats[k];
+		if (fmt->fourcc == f->fmt.pix_mp.pixelformat)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+static bool mtk_vdec_get_cap_fmt(struct mtk_vcodec_ctx *ctx, int format_index)
+{
+	const struct mtk_vcodec_dec_pdata *dec_pdata = ctx->dev->vdec_pdata;
+	const struct mtk_video_fmt *fmt;
+	int num_frame_count = 0, i;
+
+	fmt = &dec_pdata->vdec_formats[format_index];
+	for (i = 0; i < *dec_pdata->num_formats; i++) {
+		if (dec_pdata->vdec_formats[i].type != MTK_FMT_FRAME)
+			continue;
+
+		num_frame_count++;
+	}
+
+	if (num_frame_count == 1 || fmt->fourcc == V4L2_PIX_FMT_MM21)
+		return true;
+
+	return false;
+}
+
+static struct mtk_q_data *mtk_vdec_get_q_data(struct mtk_vcodec_ctx *ctx,
+					      enum v4l2_buf_type type)
+{
+	if (V4L2_TYPE_IS_OUTPUT(type))
+		return &ctx->q_data[MTK_Q_DATA_SRC];
+
+	return &ctx->q_data[MTK_Q_DATA_DST];
+}
+
+static int vidioc_try_decoder_cmd(struct file *file, void *priv,
+				struct v4l2_decoder_cmd *cmd)
+{
+	return v4l2_m2m_ioctl_try_decoder_cmd(file, priv, cmd);
+}
+
+
+static int vidioc_decoder_cmd(struct file *file, void *priv,
+				struct v4l2_decoder_cmd *cmd)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct vb2_queue *src_vq, *dst_vq;
+	int ret;
+
+	ret = vidioc_try_decoder_cmd(file, priv, cmd);
+	if (ret)
+		return ret;
+
+	mtk_v4l2_debug(1, "decoder cmd=%u", cmd->cmd);
+	dst_vq = v4l2_m2m_get_vq(ctx->m2m_ctx,
+				V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	switch (cmd->cmd) {
+	case V4L2_DEC_CMD_STOP:
+		src_vq = v4l2_m2m_get_vq(ctx->m2m_ctx,
+				V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+		if (!vb2_is_streaming(src_vq)) {
+			mtk_v4l2_debug(1, "Output stream is off. No need to flush.");
+			return 0;
+		}
+		if (!vb2_is_streaming(dst_vq)) {
+			mtk_v4l2_debug(1, "Capture stream is off. No need to flush.");
+			return 0;
+		}
+		v4l2_m2m_buf_queue(ctx->m2m_ctx, &ctx->empty_flush_buf.vb);
+		v4l2_m2m_try_schedule(ctx->m2m_ctx);
+		break;
+
+	case V4L2_DEC_CMD_START:
+		vb2_clear_last_buffer_dequeued(dst_vq);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+void mtk_vdec_unlock(struct mtk_vcodec_ctx *ctx)
+{
+	mutex_unlock(&ctx->dev->dec_mutex[ctx->hw_id]);
+}
+
+void mtk_vdec_lock(struct mtk_vcodec_ctx *ctx)
+{
+	mutex_lock(&ctx->dev->dec_mutex[ctx->hw_id]);
+}
+
+void mtk_vcodec_dec_release(struct mtk_vcodec_ctx *ctx)
+{
+	vdec_if_deinit(ctx);
+	ctx->state = MTK_STATE_FREE;
+}
+
+void mtk_vcodec_dec_set_default_params(struct mtk_vcodec_ctx *ctx)
+{
+	struct mtk_q_data *q_data;
+
+	ctx->m2m_ctx->q_lock = &ctx->dev->dev_mutex;
+	ctx->fh.m2m_ctx = ctx->m2m_ctx;
+	ctx->fh.ctrl_handler = &ctx->ctrl_hdl;
+	INIT_WORK(&ctx->decode_work, ctx->dev->vdec_pdata->worker);
+	ctx->colorspace = V4L2_COLORSPACE_REC709;
+	ctx->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	ctx->quantization = V4L2_QUANTIZATION_DEFAULT;
+	ctx->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+
+	q_data = &ctx->q_data[MTK_Q_DATA_SRC];
+	memset(q_data, 0, sizeof(struct mtk_q_data));
+	q_data->visible_width = DFT_CFG_WIDTH;
+	q_data->visible_height = DFT_CFG_HEIGHT;
+	q_data->fmt = ctx->dev->vdec_pdata->default_out_fmt;
+	q_data->field = V4L2_FIELD_NONE;
+
+	q_data->sizeimage[0] = DFT_CFG_WIDTH * DFT_CFG_HEIGHT;
+	q_data->bytesperline[0] = 0;
+
+	q_data = &ctx->q_data[MTK_Q_DATA_DST];
+	memset(q_data, 0, sizeof(struct mtk_q_data));
+	q_data->visible_width = DFT_CFG_WIDTH;
+	q_data->visible_height = DFT_CFG_HEIGHT;
+	q_data->coded_width = DFT_CFG_WIDTH;
+	q_data->coded_height = DFT_CFG_HEIGHT;
+	q_data->fmt = ctx->dev->vdec_pdata->default_cap_fmt;
+	q_data->field = V4L2_FIELD_NONE;
+
+	q_data->sizeimage[0] = q_data->coded_width * q_data->coded_height;
+	q_data->bytesperline[0] = q_data->coded_width;
+	q_data->sizeimage[1] = q_data->sizeimage[0] / 2;
+	q_data->bytesperline[1] = q_data->coded_width;
+}
+
+static int vidioc_vdec_qbuf(struct file *file, void *priv,
+			    struct v4l2_buffer *buf)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+
+	if (ctx->state == MTK_STATE_ABORT) {
+		mtk_v4l2_err("[%d] Call on QBUF after unrecoverable error",
+				ctx->id);
+		return -EIO;
+	}
+
+	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int vidioc_vdec_dqbuf(struct file *file, void *priv,
+			     struct v4l2_buffer *buf)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+
+	if (ctx->state == MTK_STATE_ABORT) {
+		mtk_v4l2_err("[%d] Call on DQBUF after unrecoverable error",
+				ctx->id);
+		return -EIO;
+	}
+
+	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int mtk_vcodec_dec_get_chip_name(void *priv)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct device *dev = &ctx->dev->plat_dev->dev;
+
+	if (of_device_is_compatible(dev->of_node, "mediatek,mt8173-vcodec-dec"))
+		return 8173;
+	else if (of_device_is_compatible(dev->of_node, "mediatek,mt8183-vcodec-dec"))
+		return 8183;
+	else if (of_device_is_compatible(dev->of_node, "mediatek,mt8192-vcodec-dec"))
+		return 8192;
+	else if (of_device_is_compatible(dev->of_node, "mediatek,mt8195-vcodec-dec"))
+		return 8195;
+	else if (of_device_is_compatible(dev->of_node, "mediatek,mt8186-vcodec-dec"))
+		return 8186;
+	else if (of_device_is_compatible(dev->of_node, "mediatek,mt8188-vcodec-dec"))
+		return 8188;
+	else
+		return 8173;
+}
+
+static int vidioc_vdec_querycap(struct file *file, void *priv,
+				struct v4l2_capability *cap)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct device *dev = &ctx->dev->plat_dev->dev;
+	int platform_name = mtk_vcodec_dec_get_chip_name(priv);
+
+	strscpy(cap->driver, dev->driver->name, sizeof(cap->driver));
+	snprintf(cap->card, sizeof(cap->card), "MT%d video decoder", platform_name);
+
+	return 0;
+}
+
+static int vidioc_vdec_subscribe_evt(struct v4l2_fh *fh,
+				     const struct v4l2_event_subscription *sub)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(fh);
+
+	if (ctx->dev->vdec_pdata->uses_stateless_api)
+		return v4l2_ctrl_subscribe_event(fh, sub);
+
+	switch (sub->type) {
+	case V4L2_EVENT_EOS:
+		return v4l2_event_subscribe(fh, sub, 2, NULL);
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subscribe(fh, sub);
+	default:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	}
+}
+
+static int vidioc_try_fmt(struct mtk_vcodec_ctx *ctx, struct v4l2_format *f,
+			  const struct mtk_video_fmt *fmt)
+{
+	struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
+	const struct v4l2_frmsize_stepwise *frmsize;
+
+	pix_fmt_mp->field = V4L2_FIELD_NONE;
+
+	/* Always apply frame size constraints from the coded side */
+	if (V4L2_TYPE_IS_OUTPUT(f->type))
+		frmsize = &fmt->frmsize;
+	else
+		frmsize = &ctx->q_data[MTK_Q_DATA_SRC].fmt->frmsize;
+
+	pix_fmt_mp->width = clamp(pix_fmt_mp->width, MTK_VDEC_MIN_W, frmsize->max_width);
+	pix_fmt_mp->height = clamp(pix_fmt_mp->height, MTK_VDEC_MIN_H, frmsize->max_height);
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		pix_fmt_mp->num_planes = 1;
+		pix_fmt_mp->plane_fmt[0].bytesperline = 0;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		int tmp_w, tmp_h;
+
+		/*
+		 * Find next closer width align 64, heign align 64, size align
+		 * 64 rectangle
+		 * Note: This only get default value, the real HW needed value
+		 *       only available when ctx in MTK_STATE_HEADER state
+		 */
+		tmp_w = pix_fmt_mp->width;
+		tmp_h = pix_fmt_mp->height;
+		v4l_bound_align_image(&pix_fmt_mp->width, MTK_VDEC_MIN_W, frmsize->max_width, 6,
+				      &pix_fmt_mp->height, MTK_VDEC_MIN_H, frmsize->max_height, 6,
+				      9);
+
+		if (pix_fmt_mp->width < tmp_w &&
+		    (pix_fmt_mp->width + 64) <= frmsize->max_width)
+			pix_fmt_mp->width += 64;
+		if (pix_fmt_mp->height < tmp_h &&
+		    (pix_fmt_mp->height + 64) <= frmsize->max_height)
+			pix_fmt_mp->height += 64;
+
+		mtk_v4l2_debug(0,
+			"before resize width=%d, height=%d, after resize width=%d, height=%d, sizeimage=%d",
+			tmp_w, tmp_h, pix_fmt_mp->width,
+			pix_fmt_mp->height,
+			pix_fmt_mp->width * pix_fmt_mp->height);
+
+		pix_fmt_mp->num_planes = fmt->num_planes;
+		pix_fmt_mp->plane_fmt[0].sizeimage =
+				pix_fmt_mp->width * pix_fmt_mp->height;
+		pix_fmt_mp->plane_fmt[0].bytesperline = pix_fmt_mp->width;
+
+		if (pix_fmt_mp->num_planes == 2) {
+			pix_fmt_mp->plane_fmt[1].sizeimage =
+				(pix_fmt_mp->width * pix_fmt_mp->height) / 2;
+			pix_fmt_mp->plane_fmt[1].bytesperline =
+				pix_fmt_mp->width;
+		}
+	}
+
+	pix_fmt_mp->flags = 0;
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_cap_mplane(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	const struct mtk_video_fmt *fmt;
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	const struct mtk_vcodec_dec_pdata *dec_pdata = ctx->dev->vdec_pdata;
+
+	fmt = mtk_vdec_find_format(f, dec_pdata);
+	if (!fmt) {
+		f->fmt.pix.pixelformat =
+			ctx->q_data[MTK_Q_DATA_DST].fmt->fourcc;
+		fmt = mtk_vdec_find_format(f, dec_pdata);
+	}
+
+	return vidioc_try_fmt(ctx, f, fmt);
+}
+
+static int vidioc_try_fmt_vid_out_mplane(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
+	const struct mtk_video_fmt *fmt;
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	const struct mtk_vcodec_dec_pdata *dec_pdata = ctx->dev->vdec_pdata;
+
+	fmt = mtk_vdec_find_format(f, dec_pdata);
+	if (!fmt) {
+		f->fmt.pix.pixelformat =
+			ctx->q_data[MTK_Q_DATA_SRC].fmt->fourcc;
+		fmt = mtk_vdec_find_format(f, dec_pdata);
+	}
+
+	if (pix_fmt_mp->plane_fmt[0].sizeimage == 0) {
+		mtk_v4l2_err("sizeimage of output format must be given");
+		return -EINVAL;
+	}
+
+	return vidioc_try_fmt(ctx, f, fmt);
+}
+
+static int vidioc_vdec_g_selection(struct file *file, void *priv,
+			struct v4l2_selection *s)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct mtk_q_data *q_data;
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	q_data = &ctx->q_data[MTK_Q_DATA_DST];
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = ctx->picinfo.pic_w;
+		s->r.height = ctx->picinfo.pic_h;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = ctx->picinfo.buf_w;
+		s->r.height = ctx->picinfo.buf_h;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (vdec_if_get_param(ctx, GET_PARAM_CROP_INFO, &(s->r))) {
+			/* set to default value if header info not ready yet*/
+			s->r.left = 0;
+			s->r.top = 0;
+			s->r.width = q_data->visible_width;
+			s->r.height = q_data->visible_height;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (ctx->state < MTK_STATE_HEADER) {
+		/* set to default value if header info not ready yet*/
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = q_data->visible_width;
+		s->r.height = q_data->visible_height;
+		return 0;
+	}
+
+	return 0;
+}
+
+static int vidioc_vdec_s_selection(struct file *file, void *priv,
+				struct v4l2_selection *s)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE:
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = ctx->picinfo.pic_w;
+		s->r.height = ctx->picinfo.pic_h;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vidioc_vdec_s_fmt(struct file *file, void *priv,
+			     struct v4l2_format *f)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct v4l2_pix_format_mplane *pix_mp;
+	struct mtk_q_data *q_data;
+	int ret = 0;
+	const struct mtk_video_fmt *fmt;
+	const struct mtk_vcodec_dec_pdata *dec_pdata = ctx->dev->vdec_pdata;
+
+	mtk_v4l2_debug(3, "[%d]", ctx->id);
+
+	q_data = mtk_vdec_get_q_data(ctx, f->type);
+	if (!q_data)
+		return -EINVAL;
+
+	pix_mp = &f->fmt.pix_mp;
+	/*
+	 * Setting OUTPUT format after OUTPUT buffers are allocated is invalid
+	 * if using the stateful API.
+	 */
+	if (!dec_pdata->uses_stateless_api &&
+	    f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
+	    vb2_is_busy(&ctx->m2m_ctx->out_q_ctx.q)) {
+		mtk_v4l2_err("out_q_ctx buffers already requested");
+		ret = -EBUSY;
+	}
+
+	/*
+	 * Setting CAPTURE format after CAPTURE buffers are allocated is
+	 * invalid.
+	 */
+	if ((f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) &&
+	    vb2_is_busy(&ctx->m2m_ctx->cap_q_ctx.q)) {
+		mtk_v4l2_err("cap_q_ctx buffers already requested");
+		ret = -EBUSY;
+	}
+
+	fmt = mtk_vdec_find_format(f, dec_pdata);
+	if (fmt == NULL) {
+		if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+			f->fmt.pix.pixelformat =
+				dec_pdata->default_out_fmt->fourcc;
+			fmt = mtk_vdec_find_format(f, dec_pdata);
+		} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+			f->fmt.pix.pixelformat =
+				dec_pdata->default_cap_fmt->fourcc;
+			fmt = mtk_vdec_find_format(f, dec_pdata);
+		}
+	}
+	if (fmt == NULL)
+		return -EINVAL;
+
+	q_data->fmt = fmt;
+	vidioc_try_fmt(ctx, f, q_data->fmt);
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		q_data->sizeimage[0] = pix_mp->plane_fmt[0].sizeimage;
+		q_data->coded_width = pix_mp->width;
+		q_data->coded_height = pix_mp->height;
+
+		ctx->colorspace = pix_mp->colorspace;
+		ctx->ycbcr_enc = pix_mp->ycbcr_enc;
+		ctx->quantization = pix_mp->quantization;
+		ctx->xfer_func = pix_mp->xfer_func;
+
+		ctx->current_codec = fmt->fourcc;
+		if (ctx->state == MTK_STATE_FREE) {
+			ret = vdec_if_init(ctx, q_data->fmt->fourcc);
+			if (ret) {
+				mtk_v4l2_err("[%d]: vdec_if_init() fail ret=%d",
+					ctx->id, ret);
+				return -EINVAL;
+			}
+			ctx->state = MTK_STATE_INIT;
+		}
+	} else {
+		ctx->capture_fourcc = fmt->fourcc;
+	}
+
+	/*
+	 * If using the stateless API, S_FMT should have the effect of setting
+	 * the CAPTURE queue resolution no matter which queue it was called on.
+	 */
+	if (dec_pdata->uses_stateless_api) {
+		ctx->picinfo.pic_w = pix_mp->width;
+		ctx->picinfo.pic_h = pix_mp->height;
+
+		/*
+		 * If get pic info fail, need to use the default pic info params, or
+		 * v4l2-compliance will fail
+		 */
+		ret = vdec_if_get_param(ctx, GET_PARAM_PIC_INFO, &ctx->picinfo);
+		if (ret) {
+			mtk_v4l2_err("[%d]Error!! Get GET_PARAM_PICTURE_INFO Fail",
+				     ctx->id);
+		}
+
+		ctx->last_decoded_picinfo = ctx->picinfo;
+
+		if (ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 1) {
+			ctx->q_data[MTK_Q_DATA_DST].sizeimage[0] =
+				ctx->picinfo.fb_sz[0] +
+				ctx->picinfo.fb_sz[1];
+			ctx->q_data[MTK_Q_DATA_DST].bytesperline[0] =
+				ctx->picinfo.buf_w;
+		} else {
+			ctx->q_data[MTK_Q_DATA_DST].sizeimage[0] =
+				ctx->picinfo.fb_sz[0];
+			ctx->q_data[MTK_Q_DATA_DST].bytesperline[0] =
+				ctx->picinfo.buf_w;
+			ctx->q_data[MTK_Q_DATA_DST].sizeimage[1] =
+				ctx->picinfo.fb_sz[1];
+			ctx->q_data[MTK_Q_DATA_DST].bytesperline[1] =
+				ctx->picinfo.buf_w;
+		}
+
+		ctx->q_data[MTK_Q_DATA_DST].coded_width = ctx->picinfo.buf_w;
+		ctx->q_data[MTK_Q_DATA_DST].coded_height = ctx->picinfo.buf_h;
+		mtk_v4l2_debug(2, "[%d] vdec_if_init() num_plane = %d wxh=%dx%d pic wxh=%dx%d sz[0]=0x%x sz[1]=0x%x",
+			       ctx->id, pix_mp->num_planes, ctx->picinfo.buf_w, ctx->picinfo.buf_h,
+			       ctx->picinfo.pic_w, ctx->picinfo.pic_h,
+			       ctx->q_data[MTK_Q_DATA_DST].sizeimage[0],
+			       ctx->q_data[MTK_Q_DATA_DST].sizeimage[1]);
+	}
+	return 0;
+}
+
+static int vidioc_enum_framesizes(struct file *file, void *priv,
+				struct v4l2_frmsizeenum *fsize)
+{
+	int i = 0;
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	const struct mtk_vcodec_dec_pdata *dec_pdata = ctx->dev->vdec_pdata;
+
+	if (fsize->index != 0)
+		return -EINVAL;
+
+	for (i = 0; i < *dec_pdata->num_formats; i++) {
+		if (fsize->pixel_format != dec_pdata->vdec_formats[i].fourcc)
+			continue;
+
+		/* Only coded formats have frame sizes set */
+		if (!dec_pdata->vdec_formats[i].frmsize.max_width)
+			return -ENOTTY;
+
+		fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+		fsize->stepwise = dec_pdata->vdec_formats[i].frmsize;
+
+		mtk_v4l2_debug(1, "%x, %d %d %d %d %d %d",
+				ctx->dev->dec_capability,
+				fsize->stepwise.min_width,
+				fsize->stepwise.max_width,
+				fsize->stepwise.step_width,
+				fsize->stepwise.min_height,
+				fsize->stepwise.max_height,
+				fsize->stepwise.step_height);
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int vidioc_enum_fmt(struct v4l2_fmtdesc *f, void *priv,
+			   bool output_queue)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	const struct mtk_vcodec_dec_pdata *dec_pdata = ctx->dev->vdec_pdata;
+	const struct mtk_video_fmt *fmt;
+	int i, j = 0;
+
+	for (i = 0; i < *dec_pdata->num_formats; i++) {
+		if (output_queue &&
+		    dec_pdata->vdec_formats[i].type != MTK_FMT_DEC)
+			continue;
+		if (!output_queue &&
+		    dec_pdata->vdec_formats[i].type != MTK_FMT_FRAME)
+			continue;
+
+		if (!output_queue && !mtk_vdec_get_cap_fmt(ctx, i))
+			continue;
+
+		if (j == f->index)
+			break;
+		++j;
+	}
+
+	if (i == *dec_pdata->num_formats)
+		return -EINVAL;
+
+	fmt = &dec_pdata->vdec_formats[i];
+	f->pixelformat = fmt->fourcc;
+	f->flags = fmt->flags;
+
+	return 0;
+}
+
+static int vidioc_vdec_enum_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_fmtdesc *f)
+{
+	return vidioc_enum_fmt(f, priv, false);
+}
+
+static int vidioc_vdec_enum_fmt_vid_out(struct file *file, void *priv,
+					struct v4l2_fmtdesc *f)
+{
+	return vidioc_enum_fmt(f, priv, true);
+}
+
+static int vidioc_vdec_g_fmt(struct file *file, void *priv,
+			     struct v4l2_format *f)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct vb2_queue *vq;
+	struct mtk_q_data *q_data;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	if (!vq) {
+		mtk_v4l2_err("no vb2 queue for type=%d", f->type);
+		return -EINVAL;
+	}
+
+	q_data = mtk_vdec_get_q_data(ctx, f->type);
+
+	pix_mp->field = V4L2_FIELD_NONE;
+	pix_mp->colorspace = ctx->colorspace;
+	pix_mp->ycbcr_enc = ctx->ycbcr_enc;
+	pix_mp->quantization = ctx->quantization;
+	pix_mp->xfer_func = ctx->xfer_func;
+
+	if ((f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) &&
+	    (ctx->state >= MTK_STATE_HEADER)) {
+		/* Until STREAMOFF is called on the CAPTURE queue
+		 * (acknowledging the event), the driver operates as if
+		 * the resolution hasn't changed yet.
+		 * So we just return picinfo yet, and update picinfo in
+		 * stop_streaming hook function
+		 */
+		q_data->sizeimage[0] = ctx->picinfo.fb_sz[0];
+		q_data->sizeimage[1] = ctx->picinfo.fb_sz[1];
+		q_data->bytesperline[0] = ctx->last_decoded_picinfo.buf_w;
+		q_data->bytesperline[1] = ctx->last_decoded_picinfo.buf_w;
+		q_data->coded_width = ctx->picinfo.buf_w;
+		q_data->coded_height = ctx->picinfo.buf_h;
+		ctx->last_decoded_picinfo.cap_fourcc = q_data->fmt->fourcc;
+
+		/*
+		 * Width and height are set to the dimensions
+		 * of the movie, the buffer is bigger and
+		 * further processing stages should crop to this
+		 * rectangle.
+		 */
+		pix_mp->width = q_data->coded_width;
+		pix_mp->height = q_data->coded_height;
+
+		/*
+		 * Set pixelformat to the format in which mt vcodec
+		 * outputs the decoded frame
+		 */
+		pix_mp->num_planes = q_data->fmt->num_planes;
+		pix_mp->pixelformat = q_data->fmt->fourcc;
+		pix_mp->plane_fmt[0].bytesperline = q_data->bytesperline[0];
+		pix_mp->plane_fmt[0].sizeimage = q_data->sizeimage[0];
+		pix_mp->plane_fmt[1].bytesperline = q_data->bytesperline[1];
+		pix_mp->plane_fmt[1].sizeimage = q_data->sizeimage[1];
+
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		/*
+		 * This is run on OUTPUT
+		 * The buffer contains compressed image
+		 * so width and height have no meaning.
+		 * Assign value here to pass v4l2-compliance test
+		 */
+		pix_mp->width = q_data->visible_width;
+		pix_mp->height = q_data->visible_height;
+		pix_mp->plane_fmt[0].bytesperline = q_data->bytesperline[0];
+		pix_mp->plane_fmt[0].sizeimage = q_data->sizeimage[0];
+		pix_mp->pixelformat = q_data->fmt->fourcc;
+		pix_mp->num_planes = q_data->fmt->num_planes;
+	} else {
+		pix_mp->width = q_data->coded_width;
+		pix_mp->height = q_data->coded_height;
+		pix_mp->num_planes = q_data->fmt->num_planes;
+		pix_mp->pixelformat = q_data->fmt->fourcc;
+		pix_mp->plane_fmt[0].bytesperline = q_data->bytesperline[0];
+		pix_mp->plane_fmt[0].sizeimage = q_data->sizeimage[0];
+		pix_mp->plane_fmt[1].bytesperline = q_data->bytesperline[1];
+		pix_mp->plane_fmt[1].sizeimage = q_data->sizeimage[1];
+
+		mtk_v4l2_debug(1, "[%d] type=%d state=%d Format information could not be read, not ready yet!",
+				ctx->id, f->type, ctx->state);
+	}
+
+	return 0;
+}
+
+int vb2ops_vdec_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+			    unsigned int *nplanes, unsigned int sizes[],
+			    struct device *alloc_devs[])
+{
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vq);
+	struct mtk_q_data *q_data;
+	unsigned int i;
+
+	q_data = mtk_vdec_get_q_data(ctx, vq->type);
+
+	if (q_data == NULL) {
+		mtk_v4l2_err("vq->type=%d err\n", vq->type);
+		return -EINVAL;
+	}
+
+	if (*nplanes) {
+		if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+			if (*nplanes != q_data->fmt->num_planes)
+				return -EINVAL;
+		} else {
+			if (*nplanes != 1)
+				return -EINVAL;
+		}
+		for (i = 0; i < *nplanes; i++) {
+			if (sizes[i] < q_data->sizeimage[i])
+				return -EINVAL;
+		}
+	} else {
+		if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+			*nplanes = q_data->fmt->num_planes;
+		else
+			*nplanes = 1;
+
+		for (i = 0; i < *nplanes; i++)
+			sizes[i] = q_data->sizeimage[i];
+	}
+
+	mtk_v4l2_debug(1,
+			"[%d]\t type = %d, get %d plane(s), %d buffer(s) of size 0x%x 0x%x ",
+			ctx->id, vq->type, *nplanes, *nbuffers,
+			sizes[0], sizes[1]);
+
+	return 0;
+}
+
+int vb2ops_vdec_buf_prepare(struct vb2_buffer *vb)
+{
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct mtk_q_data *q_data;
+	int i;
+
+	mtk_v4l2_debug(3, "[%d] (%d) id=%d",
+			ctx->id, vb->vb2_queue->type, vb->index);
+
+	q_data = mtk_vdec_get_q_data(ctx, vb->vb2_queue->type);
+
+	for (i = 0; i < q_data->fmt->num_planes; i++) {
+		if (vb2_plane_size(vb, i) < q_data->sizeimage[i]) {
+			mtk_v4l2_err("data will not fit into plane %d (%lu < %d)",
+				i, vb2_plane_size(vb, i),
+				q_data->sizeimage[i]);
+			return -EINVAL;
+		}
+		if (!V4L2_TYPE_IS_OUTPUT(vb->type))
+			vb2_set_plane_payload(vb, i, q_data->sizeimage[i]);
+	}
+
+	return 0;
+}
+
+void vb2ops_vdec_buf_finish(struct vb2_buffer *vb)
+{
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vb2_v4l2;
+	struct mtk_video_dec_buf *buf;
+	bool buf_error;
+
+	vb2_v4l2 = container_of(vb, struct vb2_v4l2_buffer, vb2_buf);
+	buf = container_of(vb2_v4l2, struct mtk_video_dec_buf, m2m_buf.vb);
+	mutex_lock(&ctx->lock);
+	if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		buf->queued_in_v4l2 = false;
+		buf->queued_in_vb2 = false;
+	}
+	buf_error = buf->error;
+	mutex_unlock(&ctx->lock);
+
+	if (buf_error) {
+		mtk_v4l2_err("Unrecoverable error on buffer.");
+		ctx->state = MTK_STATE_ABORT;
+	}
+}
+
+int vb2ops_vdec_buf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vb2_v4l2 = container_of(vb,
+					struct vb2_v4l2_buffer, vb2_buf);
+	struct mtk_video_dec_buf *buf = container_of(vb2_v4l2,
+					struct mtk_video_dec_buf, m2m_buf.vb);
+
+	if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		buf->used = false;
+		buf->queued_in_v4l2 = false;
+	}
+
+	return 0;
+}
+
+int vb2ops_vdec_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(q);
+
+	if (ctx->state == MTK_STATE_FLUSH)
+		ctx->state = MTK_STATE_HEADER;
+
+	return 0;
+}
+
+void vb2ops_vdec_stop_streaming(struct vb2_queue *q)
+{
+	struct vb2_v4l2_buffer *src_buf = NULL, *dst_buf = NULL;
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(q);
+	int ret;
+
+	mtk_v4l2_debug(3, "[%d] (%d) state=(%x) ctx->decoded_frame_cnt=%d",
+			ctx->id, q->type, ctx->state, ctx->decoded_frame_cnt);
+
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		while ((src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx))) {
+			if (src_buf != &ctx->empty_flush_buf.vb) {
+				struct media_request *req =
+					src_buf->vb2_buf.req_obj.req;
+				v4l2_m2m_buf_done(src_buf,
+						VB2_BUF_STATE_ERROR);
+				if (req)
+					v4l2_ctrl_request_complete(req, &ctx->ctrl_hdl);
+			}
+		}
+		return;
+	}
+
+	if (ctx->state >= MTK_STATE_HEADER) {
+
+		/* Until STREAMOFF is called on the CAPTURE queue
+		 * (acknowledging the event), the driver operates
+		 * as if the resolution hasn't changed yet, i.e.
+		 * VIDIOC_G_FMT< etc. return previous resolution.
+		 * So we update picinfo here
+		 */
+		ctx->picinfo = ctx->last_decoded_picinfo;
+
+		mtk_v4l2_debug(2,
+				"[%d]-> new(%d,%d), old(%d,%d), real(%d,%d)",
+				ctx->id, ctx->last_decoded_picinfo.pic_w,
+				ctx->last_decoded_picinfo.pic_h,
+				ctx->picinfo.pic_w, ctx->picinfo.pic_h,
+				ctx->last_decoded_picinfo.buf_w,
+				ctx->last_decoded_picinfo.buf_h);
+
+		ret = ctx->dev->vdec_pdata->flush_decoder(ctx);
+		if (ret)
+			mtk_v4l2_err("DecodeFinal failed, ret=%d", ret);
+	}
+	ctx->state = MTK_STATE_FLUSH;
+
+	while ((dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx))) {
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, 0);
+		if (ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 2)
+			vb2_set_plane_payload(&dst_buf->vb2_buf, 1, 0);
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
+	}
+
+}
+
+static void m2mops_vdec_device_run(void *priv)
+{
+	struct mtk_vcodec_ctx *ctx = priv;
+	struct mtk_vcodec_dev *dev = ctx->dev;
+
+	queue_work(dev->decode_workqueue, &ctx->decode_work);
+}
+
+static int m2mops_vdec_job_ready(void *m2m_priv)
+{
+	struct mtk_vcodec_ctx *ctx = m2m_priv;
+
+	mtk_v4l2_debug(3, "[%d]", ctx->id);
+
+	if (ctx->state == MTK_STATE_ABORT)
+		return 0;
+
+	if ((ctx->last_decoded_picinfo.pic_w != ctx->picinfo.pic_w) ||
+	    (ctx->last_decoded_picinfo.pic_h != ctx->picinfo.pic_h))
+		return 0;
+
+	if (ctx->state != MTK_STATE_HEADER)
+		return 0;
+
+	return 1;
+}
+
+static void m2mops_vdec_job_abort(void *priv)
+{
+	struct mtk_vcodec_ctx *ctx = priv;
+
+	ctx->state = MTK_STATE_ABORT;
+}
+
+const struct v4l2_m2m_ops mtk_vdec_m2m_ops = {
+	.device_run	= m2mops_vdec_device_run,
+	.job_ready	= m2mops_vdec_job_ready,
+	.job_abort	= m2mops_vdec_job_abort,
+};
+
+const struct v4l2_ioctl_ops mtk_vdec_ioctl_ops = {
+	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff	= v4l2_m2m_ioctl_streamoff,
+	.vidioc_reqbufs		= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf	= v4l2_m2m_ioctl_querybuf,
+	.vidioc_expbuf		= v4l2_m2m_ioctl_expbuf,
+
+	.vidioc_qbuf		= vidioc_vdec_qbuf,
+	.vidioc_dqbuf		= vidioc_vdec_dqbuf,
+
+	.vidioc_try_fmt_vid_cap_mplane	= vidioc_try_fmt_vid_cap_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= vidioc_try_fmt_vid_out_mplane,
+
+	.vidioc_s_fmt_vid_cap_mplane	= vidioc_vdec_s_fmt,
+	.vidioc_s_fmt_vid_out_mplane	= vidioc_vdec_s_fmt,
+	.vidioc_g_fmt_vid_cap_mplane	= vidioc_vdec_g_fmt,
+	.vidioc_g_fmt_vid_out_mplane	= vidioc_vdec_g_fmt,
+
+	.vidioc_create_bufs		= v4l2_m2m_ioctl_create_bufs,
+
+	.vidioc_enum_fmt_vid_cap	= vidioc_vdec_enum_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_out	= vidioc_vdec_enum_fmt_vid_out,
+	.vidioc_enum_framesizes	= vidioc_enum_framesizes,
+
+	.vidioc_querycap		= vidioc_vdec_querycap,
+	.vidioc_subscribe_event		= vidioc_vdec_subscribe_evt,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+	.vidioc_g_selection             = vidioc_vdec_g_selection,
+	.vidioc_s_selection             = vidioc_vdec_s_selection,
+
+	.vidioc_decoder_cmd = vidioc_decoder_cmd,
+	.vidioc_try_decoder_cmd = vidioc_try_decoder_cmd,
+};
+
+int mtk_vcodec_dec_queue_init(void *priv, struct vb2_queue *src_vq,
+			   struct vb2_queue *dst_vq)
+{
+	struct mtk_vcodec_ctx *ctx = priv;
+	int ret = 0;
+
+	mtk_v4l2_debug(3, "[%d]", ctx->id);
+
+	src_vq->type		= V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes	= VB2_DMABUF | VB2_MMAP;
+	src_vq->drv_priv	= ctx;
+	src_vq->buf_struct_size = sizeof(struct mtk_video_dec_buf);
+	src_vq->ops		= ctx->dev->vdec_pdata->vdec_vb2_ops;
+	src_vq->mem_ops		= &vb2_dma_contig_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock		= &ctx->dev->dev_mutex;
+	src_vq->dev             = &ctx->dev->plat_dev->dev;
+	src_vq->allow_cache_hints = 1;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret) {
+		mtk_v4l2_err("Failed to initialize videobuf2 queue(output)");
+		return ret;
+	}
+	dst_vq->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes	= VB2_DMABUF | VB2_MMAP;
+	dst_vq->drv_priv	= ctx;
+	dst_vq->buf_struct_size = sizeof(struct mtk_video_dec_buf);
+	dst_vq->ops		= ctx->dev->vdec_pdata->vdec_vb2_ops;
+	dst_vq->mem_ops		= &vb2_dma_contig_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock		= &ctx->dev->dev_mutex;
+	dst_vq->dev             = &ctx->dev->plat_dev->dev;
+	dst_vq->allow_cache_hints = 1;
+
+	ret = vb2_queue_init(dst_vq);
+	if (ret)
+		mtk_v4l2_err("Failed to initialize videobuf2 queue(capture)");
+
+	return ret;
+}
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec.h b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec.h
new file mode 100644
index 000000000..4572f9282
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec.h
@@ -0,0 +1,101 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: PC Chen <pc.chen@mediatek.com>
+ *         Tiffany Lin <tiffany.lin@mediatek.com>
+ */
+
+#ifndef _MTK_VCODEC_DEC_H_
+#define _MTK_VCODEC_DEC_H_
+
+#include <media/videobuf2-core.h>
+#include <media/v4l2-mem2mem.h>
+
+#define VCODEC_DEC_ALIGNED_64 64
+#define VCODEC_CAPABILITY_4K_DISABLED	0x10
+#define VCODEC_DEC_4K_CODED_WIDTH	4096U
+#define VCODEC_DEC_4K_CODED_HEIGHT	2304U
+#define MTK_VDEC_MAX_W	2048U
+#define MTK_VDEC_MAX_H	1088U
+#define MTK_VDEC_MIN_W	64U
+#define MTK_VDEC_MIN_H	64U
+
+#define MTK_VDEC_IRQ_STATUS_DEC_SUCCESS        0x10000
+
+/**
+ * struct vdec_fb  - decoder frame buffer
+ * @base_y	: Y plane memory info
+ * @base_c	: C plane memory info
+ * @status      : frame buffer status (vdec_fb_status)
+ */
+struct vdec_fb {
+	struct mtk_vcodec_mem	base_y;
+	struct mtk_vcodec_mem	base_c;
+	unsigned int	status;
+};
+
+/**
+ * struct mtk_video_dec_buf - Private data related to each VB2 buffer.
+ * @m2m_buf:	M2M buffer
+ * @list:	link list
+ * @used:	Capture buffer contain decoded frame data and keep in
+ *			codec data structure
+ * @queued_in_vb2:	Capture buffer is queue in vb2
+ * @queued_in_v4l2:	Capture buffer is in v4l2 driver, but not in vb2
+ *			queue yet
+ * @error:		An unrecoverable error occurs on this buffer.
+ * @frame_buffer:	Decode status, and buffer information of Capture buffer
+ * @bs_buffer:	Output buffer info
+ *
+ * Note : These status information help us track and debug buffer state
+ */
+struct mtk_video_dec_buf {
+	struct v4l2_m2m_buffer	m2m_buf;
+
+	bool	used;
+	bool	queued_in_vb2;
+	bool	queued_in_v4l2;
+	bool	error;
+
+	union {
+		struct vdec_fb	frame_buffer;
+		struct mtk_vcodec_mem	bs_buffer;
+	};
+};
+
+extern const struct v4l2_ioctl_ops mtk_vdec_ioctl_ops;
+extern const struct v4l2_m2m_ops mtk_vdec_m2m_ops;
+extern const struct media_device_ops mtk_vcodec_media_ops;
+extern const struct mtk_vcodec_dec_pdata mtk_vdec_8173_pdata;
+extern const struct mtk_vcodec_dec_pdata mtk_vdec_8183_pdata;
+extern const struct mtk_vcodec_dec_pdata mtk_lat_sig_core_pdata;
+extern const struct mtk_vcodec_dec_pdata mtk_vdec_single_core_pdata;
+
+
+/*
+ * mtk_vdec_lock/mtk_vdec_unlock are for ctx instance to
+ * get/release lock before/after access decoder hw.
+ * mtk_vdec_lock get decoder hw lock and set curr_ctx
+ * to ctx instance that get lock
+ */
+void mtk_vdec_unlock(struct mtk_vcodec_ctx *ctx);
+void mtk_vdec_lock(struct mtk_vcodec_ctx *ctx);
+int mtk_vcodec_dec_queue_init(void *priv, struct vb2_queue *src_vq,
+			   struct vb2_queue *dst_vq);
+void mtk_vcodec_dec_set_default_params(struct mtk_vcodec_ctx *ctx);
+void mtk_vcodec_dec_release(struct mtk_vcodec_ctx *ctx);
+
+/*
+ * VB2 ops
+ */
+int vb2ops_vdec_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+			    unsigned int *nplanes, unsigned int sizes[],
+			    struct device *alloc_devs[]);
+int vb2ops_vdec_buf_prepare(struct vb2_buffer *vb);
+void vb2ops_vdec_buf_finish(struct vb2_buffer *vb);
+int vb2ops_vdec_buf_init(struct vb2_buffer *vb);
+int vb2ops_vdec_start_streaming(struct vb2_queue *q, unsigned int count);
+void vb2ops_vdec_stop_streaming(struct vb2_queue *q);
+
+
+#endif /* _MTK_VCODEC_DEC_H_ */
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_drv.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_drv.c
new file mode 100644
index 000000000..42df901e8
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_drv.c
@@ -0,0 +1,528 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: PC Chen <pc.chen@mediatek.com>
+ *         Tiffany Lin <tiffany.lin@mediatek.com>
+ */
+
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-device.h>
+
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_dec.h"
+#include "mtk_vcodec_dec_hw.h"
+#include "mtk_vcodec_dec_pm.h"
+#include "mtk_vcodec_intr.h"
+#include "mtk_vcodec_util.h"
+#include "mtk_vcodec_fw.h"
+
+static int mtk_vcodec_get_hw_count(struct mtk_vcodec_dev *dev)
+{
+	switch (dev->vdec_pdata->hw_arch) {
+	case MTK_VDEC_PURE_SINGLE_CORE:
+		return MTK_VDEC_ONE_CORE;
+	case MTK_VDEC_LAT_SINGLE_CORE:
+		return MTK_VDEC_ONE_LAT_ONE_CORE;
+	default:
+		mtk_v4l2_err("hw arch %d not supported", dev->vdec_pdata->hw_arch);
+		return MTK_VDEC_NO_HW;
+	}
+}
+
+static irqreturn_t mtk_vcodec_dec_irq_handler(int irq, void *priv)
+{
+	struct mtk_vcodec_dev *dev = priv;
+	struct mtk_vcodec_ctx *ctx;
+	u32 cg_status = 0;
+	unsigned int dec_done_status = 0;
+	void __iomem *vdec_misc_addr = dev->reg_base[VDEC_MISC] +
+					VDEC_IRQ_CFG_REG;
+
+	ctx = mtk_vcodec_get_curr_ctx(dev, MTK_VDEC_CORE);
+
+	/* check if HW active or not */
+	cg_status = readl(dev->reg_base[0]);
+	if ((cg_status & VDEC_HW_ACTIVE) != 0) {
+		mtk_v4l2_err("DEC ISR, VDEC active is not 0x0 (0x%08x)",
+			     cg_status);
+		return IRQ_HANDLED;
+	}
+
+	dec_done_status = readl(vdec_misc_addr);
+	ctx->irq_status = dec_done_status;
+	if ((dec_done_status & MTK_VDEC_IRQ_STATUS_DEC_SUCCESS) !=
+		MTK_VDEC_IRQ_STATUS_DEC_SUCCESS)
+		return IRQ_HANDLED;
+
+	/* clear interrupt */
+	writel((readl(vdec_misc_addr) | VDEC_IRQ_CFG),
+		dev->reg_base[VDEC_MISC] + VDEC_IRQ_CFG_REG);
+	writel((readl(vdec_misc_addr) & ~VDEC_IRQ_CLR),
+		dev->reg_base[VDEC_MISC] + VDEC_IRQ_CFG_REG);
+
+	wake_up_ctx(ctx, MTK_INST_IRQ_RECEIVED, 0);
+
+	mtk_v4l2_debug(3,
+			"mtk_vcodec_dec_irq_handler :wake up ctx %d, dec_done_status=%x",
+			ctx->id, dec_done_status);
+
+	return IRQ_HANDLED;
+}
+
+static int mtk_vcodec_get_reg_bases(struct mtk_vcodec_dev *dev)
+{
+	struct platform_device *pdev = dev->plat_dev;
+	int reg_num, i;
+
+	/* Sizeof(u32) * 4 bytes for each register base. */
+	reg_num = of_property_count_elems_of_size(pdev->dev.of_node, "reg",
+						  sizeof(u32) * 4);
+	if (reg_num <= 0 || reg_num > NUM_MAX_VDEC_REG_BASE) {
+		dev_err(&pdev->dev, "Invalid register property size: %d\n", reg_num);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < reg_num; i++) {
+		dev->reg_base[i] = devm_platform_ioremap_resource(pdev, i);
+		if (IS_ERR(dev->reg_base[i]))
+			return PTR_ERR(dev->reg_base[i]);
+
+		mtk_v4l2_debug(2, "reg[%d] base=%p", i, dev->reg_base[i]);
+	}
+
+	return 0;
+}
+
+static int mtk_vcodec_init_dec_resources(struct mtk_vcodec_dev *dev)
+{
+	struct platform_device *pdev = dev->plat_dev;
+	int ret;
+
+	ret = mtk_vcodec_get_reg_bases(dev);
+	if (ret)
+		return ret;
+
+	if (dev->vdec_pdata->is_subdev_supported)
+		return 0;
+
+	dev->dec_irq = platform_get_irq(pdev, 0);
+	if (dev->dec_irq < 0)
+		return dev->dec_irq;
+
+	irq_set_status_flags(dev->dec_irq, IRQ_NOAUTOEN);
+	ret = devm_request_irq(&pdev->dev, dev->dec_irq,
+			       mtk_vcodec_dec_irq_handler, 0, pdev->name, dev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to install dev->dec_irq %d (%d)",
+			dev->dec_irq, ret);
+		return ret;
+	}
+
+	ret = mtk_vcodec_init_dec_clk(pdev, &dev->pm);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to get mt vcodec clock source");
+		return ret;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+	return 0;
+}
+
+static int fops_vcodec_open(struct file *file)
+{
+	struct mtk_vcodec_dev *dev = video_drvdata(file);
+	struct mtk_vcodec_ctx *ctx = NULL;
+	int ret = 0, i, hw_count;
+	struct vb2_queue *src_vq;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	mutex_lock(&dev->dev_mutex);
+	ctx->id = dev->id_counter++;
+	v4l2_fh_init(&ctx->fh, video_devdata(file));
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+	INIT_LIST_HEAD(&ctx->list);
+	ctx->dev = dev;
+	if (ctx->dev->vdec_pdata->is_subdev_supported) {
+		hw_count = mtk_vcodec_get_hw_count(dev);
+		if (!hw_count || !dev->subdev_prob_done) {
+			ret = -EINVAL;
+			goto err_ctrls_setup;
+		}
+
+		ret = dev->subdev_prob_done(dev);
+		if (ret)
+			goto err_ctrls_setup;
+
+		for (i = 0; i < hw_count; i++)
+			init_waitqueue_head(&ctx->queue[i]);
+	} else {
+		init_waitqueue_head(&ctx->queue[0]);
+	}
+	mutex_init(&ctx->lock);
+
+	ctx->type = MTK_INST_DECODER;
+	ret = dev->vdec_pdata->ctrls_setup(ctx);
+	if (ret) {
+		mtk_v4l2_err("Failed to setup mt vcodec controls");
+		goto err_ctrls_setup;
+	}
+	ctx->m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev_dec, ctx,
+		&mtk_vcodec_dec_queue_init);
+	if (IS_ERR((__force void *)ctx->m2m_ctx)) {
+		ret = PTR_ERR((__force void *)ctx->m2m_ctx);
+		mtk_v4l2_err("Failed to v4l2_m2m_ctx_init() (%d)",
+			ret);
+		goto err_m2m_ctx_init;
+	}
+	src_vq = v4l2_m2m_get_vq(ctx->m2m_ctx,
+				V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	ctx->empty_flush_buf.vb.vb2_buf.vb2_queue = src_vq;
+	mtk_vcodec_dec_set_default_params(ctx);
+
+	if (v4l2_fh_is_singular(&ctx->fh)) {
+		/*
+		 * Does nothing if firmware was already loaded.
+		 */
+		ret = mtk_vcodec_fw_load_firmware(dev->fw_handler);
+		if (ret < 0) {
+			/*
+			 * Return 0 if downloading firmware successfully,
+			 * otherwise it is failed
+			 */
+			mtk_v4l2_err("failed to load firmware!");
+			goto err_load_fw;
+		}
+
+		dev->dec_capability =
+			mtk_vcodec_fw_get_vdec_capa(dev->fw_handler);
+
+		mtk_v4l2_debug(0, "decoder capability %x", dev->dec_capability);
+	}
+
+	ctx->dev->vdec_pdata->init_vdec_params(ctx);
+
+	list_add(&ctx->list, &dev->ctx_list);
+
+	mutex_unlock(&dev->dev_mutex);
+	mtk_v4l2_debug(0, "%s decoder [%d]", dev_name(&dev->plat_dev->dev),
+			ctx->id);
+	return ret;
+
+	/* Deinit when failure occurred */
+err_load_fw:
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+err_m2m_ctx_init:
+	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+err_ctrls_setup:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+	mutex_unlock(&dev->dev_mutex);
+
+	return ret;
+}
+
+static int fops_vcodec_release(struct file *file)
+{
+	struct mtk_vcodec_dev *dev = video_drvdata(file);
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(file->private_data);
+
+	mtk_v4l2_debug(0, "[%d] decoder", ctx->id);
+	mutex_lock(&dev->dev_mutex);
+
+	/*
+	 * Call v4l2_m2m_ctx_release before mtk_vcodec_dec_release. First, it
+	 * makes sure the worker thread is not running after vdec_if_deinit.
+	 * Second, the decoder will be flushed and all the buffers will be
+	 * returned in stop_streaming.
+	 */
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+	mtk_vcodec_dec_release(ctx);
+
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+
+	list_del_init(&ctx->list);
+	kfree(ctx);
+	mutex_unlock(&dev->dev_mutex);
+	return 0;
+}
+
+static const struct v4l2_file_operations mtk_vcodec_fops = {
+	.owner		= THIS_MODULE,
+	.open		= fops_vcodec_open,
+	.release	= fops_vcodec_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static int mtk_vcodec_probe(struct platform_device *pdev)
+{
+	struct mtk_vcodec_dev *dev;
+	struct video_device *vfd_dec;
+	phandle rproc_phandle;
+	enum mtk_vcodec_fw_type fw_type;
+	int i, ret;
+
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&dev->ctx_list);
+	dev->plat_dev = pdev;
+
+	dev->vdec_pdata = of_device_get_match_data(&pdev->dev);
+	if (!of_property_read_u32(pdev->dev.of_node, "mediatek,vpu",
+				  &rproc_phandle)) {
+		fw_type = VPU;
+	} else if (!of_property_read_u32(pdev->dev.of_node, "mediatek,scp",
+					 &rproc_phandle)) {
+		fw_type = SCP;
+	} else {
+		mtk_v4l2_err("Could not get vdec IPI device");
+		return -ENODEV;
+	}
+	dma_set_max_seg_size(&pdev->dev, UINT_MAX);
+
+	dev->fw_handler = mtk_vcodec_fw_select(dev, fw_type, DECODER);
+	if (IS_ERR(dev->fw_handler))
+		return PTR_ERR(dev->fw_handler);
+
+	ret = mtk_vcodec_init_dec_resources(dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to init dec resources");
+		goto err_dec_pm;
+	}
+
+	if (IS_VDEC_LAT_ARCH(dev->vdec_pdata->hw_arch)) {
+		vdec_msg_queue_init_ctx(&dev->msg_queue_core_ctx, MTK_VDEC_CORE);
+		dev->core_workqueue =
+			alloc_ordered_workqueue("core-decoder",
+						WQ_MEM_RECLAIM | WQ_FREEZABLE);
+		if (!dev->core_workqueue) {
+			mtk_v4l2_err("Failed to create core workqueue");
+			ret = -EINVAL;
+			goto err_res;
+		}
+	}
+
+	if (of_get_property(pdev->dev.of_node, "dma-ranges", NULL)) {
+		ret = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(34));
+		if (ret) {
+			mtk_v4l2_err("Failed to set mask");
+			goto err_core_workq;
+		}
+	}
+
+	for (i = 0; i < MTK_VDEC_HW_MAX; i++)
+		mutex_init(&dev->dec_mutex[i]);
+	mutex_init(&dev->dev_mutex);
+	spin_lock_init(&dev->irqlock);
+
+	snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name), "%s",
+		"[/MTK_V4L2_VDEC]");
+
+	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+	if (ret) {
+		mtk_v4l2_err("v4l2_device_register err=%d", ret);
+		goto err_core_workq;
+	}
+
+	init_waitqueue_head(&dev->queue);
+
+	vfd_dec = video_device_alloc();
+	if (!vfd_dec) {
+		mtk_v4l2_err("Failed to allocate video device");
+		ret = -ENOMEM;
+		goto err_dec_alloc;
+	}
+	vfd_dec->fops		= &mtk_vcodec_fops;
+	vfd_dec->ioctl_ops	= &mtk_vdec_ioctl_ops;
+	vfd_dec->release	= video_device_release;
+	vfd_dec->lock		= &dev->dev_mutex;
+	vfd_dec->v4l2_dev	= &dev->v4l2_dev;
+	vfd_dec->vfl_dir	= VFL_DIR_M2M;
+	vfd_dec->device_caps	= V4L2_CAP_VIDEO_M2M_MPLANE |
+			V4L2_CAP_STREAMING;
+
+	snprintf(vfd_dec->name, sizeof(vfd_dec->name), "%s",
+		MTK_VCODEC_DEC_NAME);
+	video_set_drvdata(vfd_dec, dev);
+	dev->vfd_dec = vfd_dec;
+	platform_set_drvdata(pdev, dev);
+
+	dev->m2m_dev_dec = v4l2_m2m_init(&mtk_vdec_m2m_ops);
+	if (IS_ERR((__force void *)dev->m2m_dev_dec)) {
+		mtk_v4l2_err("Failed to init mem2mem dec device");
+		ret = PTR_ERR((__force void *)dev->m2m_dev_dec);
+		goto err_dec_alloc;
+	}
+
+	dev->decode_workqueue =
+		alloc_ordered_workqueue(MTK_VCODEC_DEC_NAME,
+			WQ_MEM_RECLAIM | WQ_FREEZABLE);
+	if (!dev->decode_workqueue) {
+		mtk_v4l2_err("Failed to create decode workqueue");
+		ret = -EINVAL;
+		goto err_event_workq;
+	}
+
+	if (dev->vdec_pdata->is_subdev_supported) {
+		ret = of_platform_populate(pdev->dev.of_node, NULL, NULL,
+					   &pdev->dev);
+		if (ret) {
+			mtk_v4l2_err("Main device of_platform_populate failed.");
+			goto err_reg_cont;
+		}
+	} else {
+		set_bit(MTK_VDEC_CORE, dev->subdev_bitmap);
+	}
+
+	atomic_set(&dev->dec_active_cnt, 0);
+	memset(dev->vdec_racing_info, 0, sizeof(dev->vdec_racing_info));
+	mutex_init(&dev->dec_racing_info_mutex);
+
+	ret = video_register_device(vfd_dec, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		mtk_v4l2_err("Failed to register video device");
+		goto err_reg_cont;
+	}
+
+	if (dev->vdec_pdata->uses_stateless_api) {
+		v4l2_disable_ioctl(vfd_dec, VIDIOC_DECODER_CMD);
+		v4l2_disable_ioctl(vfd_dec, VIDIOC_TRY_DECODER_CMD);
+
+		dev->mdev_dec.dev = &pdev->dev;
+		strscpy(dev->mdev_dec.model, MTK_VCODEC_DEC_NAME,
+			sizeof(dev->mdev_dec.model));
+
+		media_device_init(&dev->mdev_dec);
+		dev->mdev_dec.ops = &mtk_vcodec_media_ops;
+		dev->v4l2_dev.mdev = &dev->mdev_dec;
+
+		ret = v4l2_m2m_register_media_controller(dev->m2m_dev_dec, dev->vfd_dec,
+							 MEDIA_ENT_F_PROC_VIDEO_DECODER);
+		if (ret) {
+			mtk_v4l2_err("Failed to register media controller");
+			goto err_dec_mem_init;
+		}
+
+		ret = media_device_register(&dev->mdev_dec);
+		if (ret) {
+			mtk_v4l2_err("Failed to register media device");
+			goto err_media_reg;
+		}
+
+		mtk_v4l2_debug(0, "media registered as /dev/media%d", vfd_dec->minor);
+	}
+
+	mtk_v4l2_debug(0, "decoder registered as /dev/video%d", vfd_dec->minor);
+
+	return 0;
+
+err_media_reg:
+	v4l2_m2m_unregister_media_controller(dev->m2m_dev_dec);
+err_dec_mem_init:
+	video_unregister_device(vfd_dec);
+err_reg_cont:
+	if (dev->vdec_pdata->uses_stateless_api)
+		media_device_cleanup(&dev->mdev_dec);
+	destroy_workqueue(dev->decode_workqueue);
+err_event_workq:
+	v4l2_m2m_release(dev->m2m_dev_dec);
+err_dec_alloc:
+	v4l2_device_unregister(&dev->v4l2_dev);
+err_core_workq:
+	if (IS_VDEC_LAT_ARCH(dev->vdec_pdata->hw_arch))
+		destroy_workqueue(dev->core_workqueue);
+err_res:
+	if (!dev->vdec_pdata->is_subdev_supported)
+		pm_runtime_disable(dev->pm.dev);
+err_dec_pm:
+	mtk_vcodec_fw_release(dev->fw_handler);
+	return ret;
+}
+
+static const struct of_device_id mtk_vcodec_match[] = {
+	{
+		.compatible = "mediatek,mt8173-vcodec-dec",
+		.data = &mtk_vdec_8173_pdata,
+	},
+	{
+		.compatible = "mediatek,mt8183-vcodec-dec",
+		.data = &mtk_vdec_8183_pdata,
+	},
+	{
+		.compatible = "mediatek,mt8192-vcodec-dec",
+		.data = &mtk_lat_sig_core_pdata,
+	},
+	{
+		.compatible = "mediatek,mt8186-vcodec-dec",
+		.data = &mtk_vdec_single_core_pdata,
+	},
+	{
+		.compatible = "mediatek,mt8195-vcodec-dec",
+		.data = &mtk_lat_sig_core_pdata,
+	},
+	{
+		.compatible = "mediatek,mt8188-vcodec-dec",
+		.data = &mtk_lat_sig_core_pdata,
+	},
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, mtk_vcodec_match);
+
+static int mtk_vcodec_dec_remove(struct platform_device *pdev)
+{
+	struct mtk_vcodec_dev *dev = platform_get_drvdata(pdev);
+
+	destroy_workqueue(dev->decode_workqueue);
+
+	if (media_devnode_is_registered(dev->mdev_dec.devnode)) {
+		media_device_unregister(&dev->mdev_dec);
+		v4l2_m2m_unregister_media_controller(dev->m2m_dev_dec);
+		media_device_cleanup(&dev->mdev_dec);
+	}
+
+	if (dev->m2m_dev_dec)
+		v4l2_m2m_release(dev->m2m_dev_dec);
+
+	if (dev->vfd_dec)
+		video_unregister_device(dev->vfd_dec);
+
+	v4l2_device_unregister(&dev->v4l2_dev);
+	if (!dev->vdec_pdata->is_subdev_supported)
+		pm_runtime_disable(dev->pm.dev);
+	mtk_vcodec_fw_release(dev->fw_handler);
+	return 0;
+}
+
+static struct platform_driver mtk_vcodec_dec_driver = {
+	.probe	= mtk_vcodec_probe,
+	.remove	= mtk_vcodec_dec_remove,
+	.driver	= {
+		.name	= MTK_VCODEC_DEC_NAME,
+		.of_match_table = mtk_vcodec_match,
+	},
+};
+
+module_platform_driver(mtk_vcodec_dec_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Mediatek video codec V4L2 decoder driver");
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_hw.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_hw.c
new file mode 100644
index 000000000..b753bf54e
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_hw.c
@@ -0,0 +1,214 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2021 MediaTek Inc.
+ * Author: Yunfei Dong <yunfei.dong@mediatek.com>
+ */
+
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_dec.h"
+#include "mtk_vcodec_dec_hw.h"
+#include "mtk_vcodec_dec_pm.h"
+#include "mtk_vcodec_intr.h"
+#include "mtk_vcodec_util.h"
+
+static const struct of_device_id mtk_vdec_hw_match[] = {
+	{
+		.compatible = "mediatek,mtk-vcodec-lat",
+		.data = (void *)MTK_VDEC_LAT0,
+	},
+	{
+		.compatible = "mediatek,mtk-vcodec-core",
+		.data = (void *)MTK_VDEC_CORE,
+	},
+	{
+		.compatible = "mediatek,mtk-vcodec-lat-soc",
+		.data = (void *)MTK_VDEC_LAT_SOC,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, mtk_vdec_hw_match);
+
+static int mtk_vdec_hw_prob_done(struct mtk_vcodec_dev *vdec_dev)
+{
+	struct platform_device *pdev = vdec_dev->plat_dev;
+	struct device_node *subdev_node;
+	enum mtk_vdec_hw_id hw_idx;
+	const struct of_device_id *of_id;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mtk_vdec_hw_match); i++) {
+		of_id = &mtk_vdec_hw_match[i];
+		subdev_node = of_find_compatible_node(NULL, NULL,
+						      of_id->compatible);
+		if (!subdev_node)
+			continue;
+
+		of_node_put(subdev_node);
+
+		hw_idx = (enum mtk_vdec_hw_id)(uintptr_t)of_id->data;
+		if (!test_bit(hw_idx, vdec_dev->subdev_bitmap)) {
+			dev_err(&pdev->dev, "vdec %d is not ready", hw_idx);
+			return -EAGAIN;
+		}
+	}
+
+	return 0;
+}
+
+static irqreturn_t mtk_vdec_hw_irq_handler(int irq, void *priv)
+{
+	struct mtk_vdec_hw_dev *dev = priv;
+	struct mtk_vcodec_ctx *ctx;
+	u32 cg_status;
+	unsigned int dec_done_status;
+	void __iomem *vdec_misc_addr = dev->reg_base[VDEC_HW_MISC] +
+					VDEC_IRQ_CFG_REG;
+
+	ctx = mtk_vcodec_get_curr_ctx(dev->main_dev, dev->hw_idx);
+
+	/* check if HW active or not */
+	cg_status = readl(dev->reg_base[VDEC_HW_SYS]);
+	if (cg_status & VDEC_HW_ACTIVE) {
+		mtk_v4l2_err("vdec active is not 0x0 (0x%08x)",
+			     cg_status);
+		return IRQ_HANDLED;
+	}
+
+	dec_done_status = readl(vdec_misc_addr);
+	if ((dec_done_status & MTK_VDEC_IRQ_STATUS_DEC_SUCCESS) !=
+	    MTK_VDEC_IRQ_STATUS_DEC_SUCCESS)
+		return IRQ_HANDLED;
+
+	/* clear interrupt */
+	writel(dec_done_status | VDEC_IRQ_CFG, vdec_misc_addr);
+	writel(dec_done_status & ~VDEC_IRQ_CLR, vdec_misc_addr);
+
+	wake_up_ctx(ctx, MTK_INST_IRQ_RECEIVED, dev->hw_idx);
+
+	mtk_v4l2_debug(3, "wake up ctx %d, dec_done_status=%x",
+		       ctx->id, dec_done_status);
+
+	return IRQ_HANDLED;
+}
+
+static int mtk_vdec_hw_init_irq(struct mtk_vdec_hw_dev *dev)
+{
+	struct platform_device *pdev = dev->plat_dev;
+	int ret;
+
+	dev->dec_irq = platform_get_irq(pdev, 0);
+	if (dev->dec_irq < 0)
+		return dev->dec_irq;
+
+	irq_set_status_flags(dev->dec_irq, IRQ_NOAUTOEN);
+	ret = devm_request_irq(&pdev->dev, dev->dec_irq,
+			       mtk_vdec_hw_irq_handler, 0, pdev->name, dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to install dev->dec_irq %d (%d)",
+			dev->dec_irq, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int mtk_vdec_hw_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct mtk_vdec_hw_dev *subdev_dev;
+	struct mtk_vcodec_dev *main_dev;
+	const struct of_device_id *of_id;
+	int hw_idx;
+	int ret;
+
+	if (!dev->parent) {
+		dev_err(dev, "no parent for hardware devices.\n");
+		return -ENODEV;
+	}
+
+	main_dev = dev_get_drvdata(dev->parent);
+	if (!main_dev) {
+		dev_err(dev, "failed to get parent driver data");
+		return -EINVAL;
+	}
+
+	subdev_dev = devm_kzalloc(dev, sizeof(*subdev_dev), GFP_KERNEL);
+	if (!subdev_dev)
+		return -ENOMEM;
+
+	subdev_dev->plat_dev = pdev;
+	ret = mtk_vcodec_init_dec_clk(pdev, &subdev_dev->pm);
+	if (ret)
+		return ret;
+	pm_runtime_enable(&pdev->dev);
+
+	of_id = of_match_device(mtk_vdec_hw_match, dev);
+	if (!of_id) {
+		dev_err(dev, "Can't get vdec subdev id.\n");
+		ret = -EINVAL;
+		goto err;
+	}
+
+	hw_idx = (enum mtk_vdec_hw_id)(uintptr_t)of_id->data;
+	if (hw_idx >= MTK_VDEC_HW_MAX) {
+		dev_err(dev, "Hardware index %d not correct.\n", hw_idx);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	main_dev->subdev_dev[hw_idx] = subdev_dev;
+	subdev_dev->hw_idx = hw_idx;
+	subdev_dev->main_dev = main_dev;
+	subdev_dev->reg_base[VDEC_HW_SYS] = main_dev->reg_base[VDEC_HW_SYS];
+	set_bit(subdev_dev->hw_idx, main_dev->subdev_bitmap);
+
+	if (IS_SUPPORT_VDEC_HW_IRQ(hw_idx)) {
+		ret = mtk_vdec_hw_init_irq(subdev_dev);
+		if (ret)
+			goto err;
+	}
+
+	subdev_dev->reg_base[VDEC_HW_MISC] =
+		devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR((__force void *)subdev_dev->reg_base[VDEC_HW_MISC])) {
+		ret = PTR_ERR((__force void *)subdev_dev->reg_base[VDEC_HW_MISC]);
+		goto err;
+	}
+
+	if (!main_dev->subdev_prob_done)
+		main_dev->subdev_prob_done = mtk_vdec_hw_prob_done;
+
+	platform_set_drvdata(pdev, subdev_dev);
+	return 0;
+err:
+	pm_runtime_disable(subdev_dev->pm.dev);
+	return ret;
+}
+
+static int mtk_vdec_hw_remove(struct platform_device *pdev)
+{
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+static struct platform_driver mtk_vdec_driver = {
+	.probe	= mtk_vdec_hw_probe,
+	.remove = mtk_vdec_hw_remove,
+	.driver	= {
+		.name	= "mtk-vdec-comp",
+		.of_match_table = mtk_vdec_hw_match,
+	},
+};
+module_platform_driver(mtk_vdec_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Mediatek video decoder hardware driver");
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_hw.h b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_hw.h
new file mode 100644
index 000000000..36faa8d9d
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_hw.h
@@ -0,0 +1,58 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2021 MediaTek Inc.
+ * Author: Yunfei Dong <yunfei.dong@mediatek.com>
+ */
+
+#ifndef _MTK_VCODEC_DEC_HW_H_
+#define _MTK_VCODEC_DEC_HW_H_
+
+#include <linux/io.h>
+#include <linux/platform_device.h>
+
+#include "mtk_vcodec_drv.h"
+
+#define VDEC_HW_ACTIVE 0x10
+#define VDEC_IRQ_CFG 0x11
+#define VDEC_IRQ_CLR 0x10
+#define VDEC_IRQ_CFG_REG 0xa4
+
+#define IS_SUPPORT_VDEC_HW_IRQ(hw_idx) ((hw_idx) != MTK_VDEC_LAT_SOC)
+
+/**
+ * enum mtk_vdec_hw_reg_idx - subdev hardware register base index
+ * @VDEC_HW_SYS : vdec soc register index
+ * @VDEC_HW_MISC: vdec misc register index
+ * @VDEC_HW_MAX : vdec supported max register index
+ */
+enum mtk_vdec_hw_reg_idx {
+	VDEC_HW_SYS,
+	VDEC_HW_MISC,
+	VDEC_HW_MAX
+};
+
+/**
+ * struct mtk_vdec_hw_dev - vdec hardware driver data
+ * @plat_dev: platform device
+ * @main_dev: main device
+ * @reg_base: mapped address of MTK Vcodec registers.
+ *
+ * @curr_ctx: the context that is waiting for codec hardware
+ *
+ * @dec_irq : decoder irq resource
+ * @pm      : power management control
+ * @hw_idx  : each hardware index
+ */
+struct mtk_vdec_hw_dev {
+	struct platform_device *plat_dev;
+	struct mtk_vcodec_dev *main_dev;
+	void __iomem *reg_base[VDEC_HW_MAX];
+
+	struct mtk_vcodec_ctx *curr_ctx;
+
+	int dec_irq;
+	struct mtk_vcodec_pm pm;
+	int hw_idx;
+};
+
+#endif /* _MTK_VCODEC_DEC_HW_H_ */
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_pm.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_pm.c
new file mode 100644
index 000000000..4305e4eb9
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_pm.c
@@ -0,0 +1,265 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Tiffany Lin <tiffany.lin@mediatek.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/pm_runtime.h>
+
+#include "mtk_vcodec_dec_hw.h"
+#include "mtk_vcodec_dec_pm.h"
+#include "mtk_vcodec_util.h"
+
+int mtk_vcodec_init_dec_clk(struct platform_device *pdev, struct mtk_vcodec_pm *pm)
+{
+	struct mtk_vcodec_clk *dec_clk;
+	struct mtk_vcodec_clk_info *clk_info;
+	int i = 0, ret;
+
+	dec_clk = &pm->vdec_clk;
+	pm->dev = &pdev->dev;
+
+	dec_clk->clk_num =
+		of_property_count_strings(pdev->dev.of_node, "clock-names");
+	if (dec_clk->clk_num > 0) {
+		dec_clk->clk_info = devm_kcalloc(&pdev->dev,
+			dec_clk->clk_num, sizeof(*clk_info),
+			GFP_KERNEL);
+		if (!dec_clk->clk_info)
+			return -ENOMEM;
+	} else {
+		mtk_v4l2_err("Failed to get vdec clock count");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < dec_clk->clk_num; i++) {
+		clk_info = &dec_clk->clk_info[i];
+		ret = of_property_read_string_index(pdev->dev.of_node,
+			"clock-names", i, &clk_info->clk_name);
+		if (ret) {
+			mtk_v4l2_err("Failed to get clock name id = %d", i);
+			return ret;
+		}
+		clk_info->vcodec_clk = devm_clk_get(&pdev->dev,
+			clk_info->clk_name);
+		if (IS_ERR(clk_info->vcodec_clk)) {
+			mtk_v4l2_err("devm_clk_get (%d)%s fail", i,
+				clk_info->clk_name);
+			return PTR_ERR(clk_info->vcodec_clk);
+		}
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_vcodec_init_dec_clk);
+
+static int mtk_vcodec_dec_pw_on(struct mtk_vcodec_pm *pm)
+{
+	int ret;
+
+	ret = pm_runtime_resume_and_get(pm->dev);
+	if (ret)
+		mtk_v4l2_err("pm_runtime_resume_and_get fail %d", ret);
+
+	return ret;
+}
+
+static void mtk_vcodec_dec_pw_off(struct mtk_vcodec_pm *pm)
+{
+	int ret;
+
+	ret = pm_runtime_put_sync(pm->dev);
+	if (ret)
+		mtk_v4l2_err("pm_runtime_put_sync fail %d", ret);
+}
+
+static void mtk_vcodec_dec_clock_on(struct mtk_vcodec_pm *pm)
+{
+	struct mtk_vcodec_clk *dec_clk;
+	int ret, i;
+
+	dec_clk = &pm->vdec_clk;
+	for (i = 0; i < dec_clk->clk_num; i++) {
+		ret = clk_prepare_enable(dec_clk->clk_info[i].vcodec_clk);
+		if (ret) {
+			mtk_v4l2_err("clk_prepare_enable %d %s fail %d", i,
+				dec_clk->clk_info[i].clk_name, ret);
+			goto error;
+		}
+	}
+
+	return;
+error:
+	for (i -= 1; i >= 0; i--)
+		clk_disable_unprepare(dec_clk->clk_info[i].vcodec_clk);
+}
+
+static void mtk_vcodec_dec_clock_off(struct mtk_vcodec_pm *pm)
+{
+	struct mtk_vcodec_clk *dec_clk;
+	int i;
+
+	dec_clk = &pm->vdec_clk;
+	for (i = dec_clk->clk_num - 1; i >= 0; i--)
+		clk_disable_unprepare(dec_clk->clk_info[i].vcodec_clk);
+}
+
+static void mtk_vcodec_dec_enable_irq(struct mtk_vcodec_dev *vdec_dev, int hw_idx)
+{
+	struct mtk_vdec_hw_dev *subdev_dev;
+
+	if (!test_bit(hw_idx, vdec_dev->subdev_bitmap))
+		return;
+
+	if (vdec_dev->vdec_pdata->is_subdev_supported) {
+		subdev_dev = mtk_vcodec_get_hw_dev(vdec_dev, hw_idx);
+		if (subdev_dev)
+			enable_irq(subdev_dev->dec_irq);
+		else
+			mtk_v4l2_err("Failed to get hw dev\n");
+	} else {
+		enable_irq(vdec_dev->dec_irq);
+	}
+}
+
+static void mtk_vcodec_dec_disable_irq(struct mtk_vcodec_dev *vdec_dev, int hw_idx)
+{
+	struct mtk_vdec_hw_dev *subdev_dev;
+
+	if (!test_bit(hw_idx, vdec_dev->subdev_bitmap))
+		return;
+
+	if (vdec_dev->vdec_pdata->is_subdev_supported) {
+		subdev_dev = mtk_vcodec_get_hw_dev(vdec_dev, hw_idx);
+		if (subdev_dev)
+			disable_irq(subdev_dev->dec_irq);
+		else
+			mtk_v4l2_err("Failed to get hw dev\n");
+	} else {
+		disable_irq(vdec_dev->dec_irq);
+	}
+}
+
+static void mtk_vcodec_load_racing_info(struct mtk_vcodec_ctx *ctx)
+{
+	void __iomem *vdec_racing_addr;
+	int j;
+
+	mutex_lock(&ctx->dev->dec_racing_info_mutex);
+	if (atomic_inc_return(&ctx->dev->dec_active_cnt) == 1) {
+		vdec_racing_addr = ctx->dev->reg_base[VDEC_MISC] + 0x100;
+		for (j = 0; j < 132; j++)
+			writel(ctx->dev->vdec_racing_info[j], vdec_racing_addr + j * 4);
+	}
+	mutex_unlock(&ctx->dev->dec_racing_info_mutex);
+}
+
+static void mtk_vcodec_record_racing_info(struct mtk_vcodec_ctx *ctx)
+{
+	void __iomem *vdec_racing_addr;
+	int j;
+
+	mutex_lock(&ctx->dev->dec_racing_info_mutex);
+	if (atomic_dec_and_test(&ctx->dev->dec_active_cnt)) {
+		vdec_racing_addr = ctx->dev->reg_base[VDEC_MISC] + 0x100;
+		for (j = 0; j < 132; j++)
+			ctx->dev->vdec_racing_info[j] = readl(vdec_racing_addr + j * 4);
+	}
+	mutex_unlock(&ctx->dev->dec_racing_info_mutex);
+}
+
+static struct mtk_vcodec_pm *mtk_vcodec_dec_get_pm(struct mtk_vcodec_dev *vdec_dev,
+						   int hw_idx)
+{
+	struct mtk_vdec_hw_dev *subdev_dev;
+
+	if (!test_bit(hw_idx, vdec_dev->subdev_bitmap))
+		return NULL;
+
+	if (vdec_dev->vdec_pdata->is_subdev_supported) {
+		subdev_dev = mtk_vcodec_get_hw_dev(vdec_dev, hw_idx);
+		if (subdev_dev)
+			return &subdev_dev->pm;
+
+		mtk_v4l2_err("Failed to get hw dev\n");
+		return NULL;
+	}
+
+	return &vdec_dev->pm;
+}
+
+static void mtk_vcodec_dec_child_dev_on(struct mtk_vcodec_dev *vdec_dev,
+					int hw_idx)
+{
+	struct mtk_vcodec_pm *pm;
+
+	pm = mtk_vcodec_dec_get_pm(vdec_dev, hw_idx);
+	if (pm) {
+		mtk_vcodec_dec_pw_on(pm);
+		mtk_vcodec_dec_clock_on(pm);
+	}
+
+	if (hw_idx == MTK_VDEC_LAT0) {
+		pm = mtk_vcodec_dec_get_pm(vdec_dev, MTK_VDEC_LAT_SOC);
+		if (pm) {
+			mtk_vcodec_dec_pw_on(pm);
+			mtk_vcodec_dec_clock_on(pm);
+		}
+	}
+}
+
+static void mtk_vcodec_dec_child_dev_off(struct mtk_vcodec_dev *vdec_dev,
+					 int hw_idx)
+{
+	struct mtk_vcodec_pm *pm;
+
+	pm = mtk_vcodec_dec_get_pm(vdec_dev, hw_idx);
+	if (pm) {
+		mtk_vcodec_dec_clock_off(pm);
+		mtk_vcodec_dec_pw_off(pm);
+	}
+
+	if (hw_idx == MTK_VDEC_LAT0) {
+		pm = mtk_vcodec_dec_get_pm(vdec_dev, MTK_VDEC_LAT_SOC);
+		if (pm) {
+			mtk_vcodec_dec_clock_off(pm);
+			mtk_vcodec_dec_pw_off(pm);
+		}
+	}
+}
+
+void mtk_vcodec_dec_enable_hardware(struct mtk_vcodec_ctx *ctx, int hw_idx)
+{
+	mutex_lock(&ctx->dev->dec_mutex[hw_idx]);
+
+	if (IS_VDEC_LAT_ARCH(ctx->dev->vdec_pdata->hw_arch) &&
+	    hw_idx == MTK_VDEC_CORE)
+		mtk_vcodec_dec_child_dev_on(ctx->dev, MTK_VDEC_LAT0);
+	mtk_vcodec_dec_child_dev_on(ctx->dev, hw_idx);
+
+	mtk_vcodec_dec_enable_irq(ctx->dev, hw_idx);
+
+	if (IS_VDEC_INNER_RACING(ctx->dev->dec_capability))
+		mtk_vcodec_load_racing_info(ctx);
+}
+EXPORT_SYMBOL_GPL(mtk_vcodec_dec_enable_hardware);
+
+void mtk_vcodec_dec_disable_hardware(struct mtk_vcodec_ctx *ctx, int hw_idx)
+{
+	if (IS_VDEC_INNER_RACING(ctx->dev->dec_capability))
+		mtk_vcodec_record_racing_info(ctx);
+
+	mtk_vcodec_dec_disable_irq(ctx->dev, hw_idx);
+
+	mtk_vcodec_dec_child_dev_off(ctx->dev, hw_idx);
+	if (IS_VDEC_LAT_ARCH(ctx->dev->vdec_pdata->hw_arch) &&
+	    hw_idx == MTK_VDEC_CORE)
+		mtk_vcodec_dec_child_dev_off(ctx->dev, MTK_VDEC_LAT0);
+
+	mutex_unlock(&ctx->dev->dec_mutex[hw_idx]);
+}
+EXPORT_SYMBOL_GPL(mtk_vcodec_dec_disable_hardware);
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_pm.h b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_pm.h
new file mode 100644
index 000000000..dbcf3cabe
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_pm.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Tiffany Lin <tiffany.lin@mediatek.com>
+ */
+
+#ifndef _MTK_VCODEC_DEC_PM_H_
+#define _MTK_VCODEC_DEC_PM_H_
+
+#include "mtk_vcodec_drv.h"
+
+int mtk_vcodec_init_dec_clk(struct platform_device *pdev, struct mtk_vcodec_pm *pm);
+
+void mtk_vcodec_dec_enable_hardware(struct mtk_vcodec_ctx *ctx, int hw_idx);
+void mtk_vcodec_dec_disable_hardware(struct mtk_vcodec_ctx *ctx, int hw_idx);
+
+#endif /* _MTK_VCODEC_DEC_PM_H_ */
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_stateful.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_stateful.c
new file mode 100644
index 000000000..0fbd03002
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_stateful.c
@@ -0,0 +1,623 @@
+// SPDX-License-Identifier: GPL-2.0
+
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_dec.h"
+#include "mtk_vcodec_intr.h"
+#include "mtk_vcodec_util.h"
+#include "mtk_vcodec_dec_pm.h"
+#include "vdec_drv_if.h"
+
+static struct mtk_video_fmt mtk_video_formats[] = {
+	{
+		.fourcc = V4L2_PIX_FMT_H264,
+		.type = MTK_FMT_DEC,
+		.num_planes = 1,
+		.flags = V4L2_FMT_FLAG_DYN_RESOLUTION,
+		.frmsize = { MTK_VDEC_MIN_W, MTK_VDEC_MAX_W, 16,
+			     MTK_VDEC_MIN_H, MTK_VDEC_MAX_H, 16 },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_VP8,
+		.type = MTK_FMT_DEC,
+		.num_planes = 1,
+		.flags = V4L2_FMT_FLAG_DYN_RESOLUTION,
+		.frmsize = { MTK_VDEC_MIN_W, MTK_VDEC_MAX_W, 16,
+			     MTK_VDEC_MIN_H, MTK_VDEC_MAX_H, 16 },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_VP9,
+		.type = MTK_FMT_DEC,
+		.num_planes = 1,
+		.flags = V4L2_FMT_FLAG_DYN_RESOLUTION,
+		.frmsize = { MTK_VDEC_MIN_W, MTK_VDEC_MAX_W, 16,
+			     MTK_VDEC_MIN_H, MTK_VDEC_MAX_H, 16 },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_MT21C,
+		.type = MTK_FMT_FRAME,
+		.num_planes = 2,
+	},
+};
+
+static const unsigned int num_supported_formats =
+	ARRAY_SIZE(mtk_video_formats);
+
+#define DEFAULT_OUT_FMT_IDX 0
+#define DEFAULT_CAP_FMT_IDX 3
+
+/*
+ * This function tries to clean all display buffers, the buffers will return
+ * in display order.
+ * Note the buffers returned from codec driver may still be in driver's
+ * reference list.
+ */
+static struct vb2_buffer *get_display_buffer(struct mtk_vcodec_ctx *ctx)
+{
+	struct vdec_fb *disp_frame_buffer = NULL;
+	struct mtk_video_dec_buf *dstbuf;
+	struct vb2_v4l2_buffer *vb;
+
+	mtk_v4l2_debug(3, "[%d]", ctx->id);
+	if (vdec_if_get_param(ctx, GET_PARAM_DISP_FRAME_BUFFER,
+			      &disp_frame_buffer)) {
+		mtk_v4l2_err("[%d]Cannot get param : GET_PARAM_DISP_FRAME_BUFFER", ctx->id);
+		return NULL;
+	}
+
+	if (!disp_frame_buffer) {
+		mtk_v4l2_debug(3, "No display frame buffer");
+		return NULL;
+	}
+
+	dstbuf = container_of(disp_frame_buffer, struct mtk_video_dec_buf,
+			      frame_buffer);
+	vb = &dstbuf->m2m_buf.vb;
+	mutex_lock(&ctx->lock);
+	if (dstbuf->used) {
+		mtk_v4l2_debug(2, "[%d]status=%x queue id=%d to done_list %d",
+			       ctx->id, disp_frame_buffer->status,
+			       vb->vb2_buf.index, dstbuf->queued_in_vb2);
+
+		v4l2_m2m_buf_done(vb, VB2_BUF_STATE_DONE);
+		ctx->decoded_frame_cnt++;
+	}
+	mutex_unlock(&ctx->lock);
+	return &vb->vb2_buf;
+}
+
+/*
+ * This function tries to clean all capture buffers that are not used as
+ * reference buffers by codec driver any more
+ * In this case, we need re-queue buffer to vb2 buffer if user space
+ * already returns this buffer to v4l2 or this buffer is just the output of
+ * previous sps/pps/resolution change decode, or do nothing if user
+ * space still owns this buffer
+ */
+static struct vb2_buffer *get_free_buffer(struct mtk_vcodec_ctx *ctx)
+{
+	struct mtk_video_dec_buf *dstbuf;
+	struct vdec_fb *free_frame_buffer = NULL;
+	struct vb2_v4l2_buffer *vb;
+
+	if (vdec_if_get_param(ctx, GET_PARAM_FREE_FRAME_BUFFER,
+			      &free_frame_buffer)) {
+		mtk_v4l2_err("[%d] Error!! Cannot get param", ctx->id);
+		return NULL;
+	}
+	if (!free_frame_buffer) {
+		mtk_v4l2_debug(3, " No free frame buffer");
+		return NULL;
+	}
+
+	mtk_v4l2_debug(3, "[%d] tmp_frame_addr = 0x%p", ctx->id,
+		       free_frame_buffer);
+
+	dstbuf = container_of(free_frame_buffer, struct mtk_video_dec_buf,
+			      frame_buffer);
+	vb = &dstbuf->m2m_buf.vb;
+
+	mutex_lock(&ctx->lock);
+	if (dstbuf->used) {
+		if (dstbuf->queued_in_vb2 && dstbuf->queued_in_v4l2 &&
+		    free_frame_buffer->status == FB_ST_FREE) {
+			/*
+			 * After decode sps/pps or non-display buffer, we don't
+			 * need to return capture buffer to user space, but
+			 * just re-queue this capture buffer to vb2 queue.
+			 * This reduce overheads that dq/q unused capture
+			 * buffer. In this case, queued_in_vb2 = true.
+			 */
+			mtk_v4l2_debug(2, "[%d]status=%x queue id=%d to rdy_queue %d",
+				       ctx->id, free_frame_buffer->status,
+				       vb->vb2_buf.index, dstbuf->queued_in_vb2);
+			v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
+		} else if (!dstbuf->queued_in_vb2 && dstbuf->queued_in_v4l2) {
+			/*
+			 * If buffer in v4l2 driver but not in vb2 queue yet,
+			 * and we get this buffer from free_list, it means
+			 * that codec driver do not use this buffer as
+			 * reference buffer anymore. We should q buffer to vb2
+			 * queue, so later work thread could get this buffer
+			 * for decode. In this case, queued_in_vb2 = false
+			 * means this buffer is not from previous decode
+			 * output.
+			 */
+			mtk_v4l2_debug(2,
+				       "[%d]status=%x queue id=%d to rdy_queue",
+				       ctx->id, free_frame_buffer->status,
+				       vb->vb2_buf.index);
+			v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
+			dstbuf->queued_in_vb2 = true;
+		} else {
+			/*
+			 * Codec driver do not need to reference this capture
+			 * buffer and this buffer is not in v4l2 driver.
+			 * Then we don't need to do any thing, just add log when
+			 * we need to debug buffer flow.
+			 * When this buffer q from user space, it could
+			 * directly q to vb2 buffer
+			 */
+			mtk_v4l2_debug(3, "[%d]status=%x err queue id=%d %d %d",
+				       ctx->id, free_frame_buffer->status,
+				       vb->vb2_buf.index, dstbuf->queued_in_vb2,
+				       dstbuf->queued_in_v4l2);
+		}
+		dstbuf->used = false;
+	}
+	mutex_unlock(&ctx->lock);
+	return &vb->vb2_buf;
+}
+
+static void clean_display_buffer(struct mtk_vcodec_ctx *ctx)
+{
+	while (get_display_buffer(ctx))
+		;
+}
+
+static void clean_free_buffer(struct mtk_vcodec_ctx *ctx)
+{
+	while (get_free_buffer(ctx))
+		;
+}
+
+static void mtk_vdec_queue_res_chg_event(struct mtk_vcodec_ctx *ctx)
+{
+	static const struct v4l2_event ev_src_ch = {
+		.type = V4L2_EVENT_SOURCE_CHANGE,
+		.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+	};
+
+	mtk_v4l2_debug(1, "[%d]", ctx->id);
+	v4l2_event_queue_fh(&ctx->fh, &ev_src_ch);
+}
+
+static int mtk_vdec_flush_decoder(struct mtk_vcodec_ctx *ctx)
+{
+	bool res_chg;
+	int ret;
+
+	ret = vdec_if_decode(ctx, NULL, NULL, &res_chg);
+	if (ret)
+		mtk_v4l2_err("DecodeFinal failed, ret=%d", ret);
+
+	clean_display_buffer(ctx);
+	clean_free_buffer(ctx);
+
+	return 0;
+}
+
+static void mtk_vdec_update_fmt(struct mtk_vcodec_ctx *ctx,
+				unsigned int pixelformat)
+{
+	const struct mtk_video_fmt *fmt;
+	struct mtk_q_data *dst_q_data;
+	unsigned int k;
+
+	dst_q_data = &ctx->q_data[MTK_Q_DATA_DST];
+	for (k = 0; k < num_supported_formats; k++) {
+		fmt = &mtk_video_formats[k];
+		if (fmt->fourcc == pixelformat) {
+			mtk_v4l2_debug(1, "Update cap fourcc(%d -> %d)",
+				       dst_q_data->fmt->fourcc, pixelformat);
+			dst_q_data->fmt = fmt;
+			return;
+		}
+	}
+
+	mtk_v4l2_err("Cannot get fourcc(%d), using init value", pixelformat);
+}
+
+static int mtk_vdec_pic_info_update(struct mtk_vcodec_ctx *ctx)
+{
+	unsigned int dpbsize = 0;
+	int ret;
+
+	if (vdec_if_get_param(ctx, GET_PARAM_PIC_INFO,
+			      &ctx->last_decoded_picinfo)) {
+		mtk_v4l2_err("[%d]Error!! Cannot get param : GET_PARAM_PICTURE_INFO ERR", ctx->id);
+		return -EINVAL;
+	}
+
+	if (ctx->last_decoded_picinfo.pic_w == 0 ||
+	    ctx->last_decoded_picinfo.pic_h == 0 ||
+	    ctx->last_decoded_picinfo.buf_w == 0 ||
+	    ctx->last_decoded_picinfo.buf_h == 0) {
+		mtk_v4l2_err("Cannot get correct pic info");
+		return -EINVAL;
+	}
+
+	if (ctx->last_decoded_picinfo.cap_fourcc != ctx->picinfo.cap_fourcc &&
+	    ctx->picinfo.cap_fourcc != 0)
+		mtk_vdec_update_fmt(ctx, ctx->picinfo.cap_fourcc);
+
+	if (ctx->last_decoded_picinfo.pic_w == ctx->picinfo.pic_w ||
+	    ctx->last_decoded_picinfo.pic_h == ctx->picinfo.pic_h)
+		return 0;
+
+	mtk_v4l2_debug(1, "[%d]-> new(%d,%d), old(%d,%d), real(%d,%d)", ctx->id,
+		       ctx->last_decoded_picinfo.pic_w,
+		       ctx->last_decoded_picinfo.pic_h, ctx->picinfo.pic_w,
+		       ctx->picinfo.pic_h, ctx->last_decoded_picinfo.buf_w,
+		       ctx->last_decoded_picinfo.buf_h);
+
+	ret = vdec_if_get_param(ctx, GET_PARAM_DPB_SIZE, &dpbsize);
+	if (dpbsize == 0)
+		mtk_v4l2_err("Incorrect dpb size, ret=%d", ret);
+
+	ctx->dpb_size = dpbsize;
+
+	return ret;
+}
+
+static void mtk_vdec_worker(struct work_struct *work)
+{
+	struct mtk_vcodec_ctx *ctx =
+		container_of(work, struct mtk_vcodec_ctx, decode_work);
+	struct mtk_vcodec_dev *dev = ctx->dev;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	struct mtk_vcodec_mem buf;
+	struct vdec_fb *pfb;
+	bool res_chg = false;
+	int ret;
+	struct mtk_video_dec_buf *dst_buf_info, *src_buf_info;
+
+	src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	if (!src_buf) {
+		v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
+		mtk_v4l2_debug(1, "[%d] src_buf empty!!", ctx->id);
+		return;
+	}
+
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	if (!dst_buf) {
+		v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
+		mtk_v4l2_debug(1, "[%d] dst_buf empty!!", ctx->id);
+		return;
+	}
+
+	dst_buf_info =
+		container_of(dst_buf, struct mtk_video_dec_buf, m2m_buf.vb);
+
+	pfb = &dst_buf_info->frame_buffer;
+	pfb->base_y.va = vb2_plane_vaddr(&dst_buf->vb2_buf, 0);
+	pfb->base_y.dma_addr =
+		vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
+	pfb->base_y.size = ctx->picinfo.fb_sz[0];
+
+	pfb->base_c.va = vb2_plane_vaddr(&dst_buf->vb2_buf, 1);
+	pfb->base_c.dma_addr =
+		vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 1);
+	pfb->base_c.size = ctx->picinfo.fb_sz[1];
+	pfb->status = 0;
+	mtk_v4l2_debug(3, "===>[%d] vdec_if_decode() ===>", ctx->id);
+
+	mtk_v4l2_debug(3,
+		       "id=%d Framebuf  pfb=%p VA=%p Y_DMA=%pad C_DMA=%pad Size=%zx",
+		       dst_buf->vb2_buf.index, pfb, pfb->base_y.va,
+		       &pfb->base_y.dma_addr, &pfb->base_c.dma_addr, pfb->base_y.size);
+
+	if (src_buf == &ctx->empty_flush_buf.vb) {
+		mtk_v4l2_debug(1, "Got empty flush input buffer.");
+		src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+
+		/* update dst buf status */
+		dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+		mutex_lock(&ctx->lock);
+		dst_buf_info->used = false;
+		mutex_unlock(&ctx->lock);
+
+		vdec_if_decode(ctx, NULL, NULL, &res_chg);
+		clean_display_buffer(ctx);
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, 0);
+		if (ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 2)
+			vb2_set_plane_payload(&dst_buf->vb2_buf, 1, 0);
+		dst_buf->flags |= V4L2_BUF_FLAG_LAST;
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
+		clean_free_buffer(ctx);
+		v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
+		return;
+	}
+
+	src_buf_info =
+		container_of(src_buf, struct mtk_video_dec_buf, m2m_buf.vb);
+
+	buf.va = vb2_plane_vaddr(&src_buf->vb2_buf, 0);
+	buf.dma_addr = vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
+	buf.size = (size_t)src_buf->vb2_buf.planes[0].bytesused;
+	if (!buf.va) {
+		v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
+		mtk_v4l2_err("[%d] id=%d src_addr is NULL!!", ctx->id,
+			     src_buf->vb2_buf.index);
+		return;
+	}
+	mtk_v4l2_debug(3, "[%d] Bitstream VA=%p DMA=%pad Size=%zx vb=%p",
+		       ctx->id, buf.va, &buf.dma_addr, buf.size, src_buf);
+	dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp;
+	dst_buf->timecode = src_buf->timecode;
+	mutex_lock(&ctx->lock);
+	dst_buf_info->used = true;
+	mutex_unlock(&ctx->lock);
+	src_buf_info->used = true;
+
+	ret = vdec_if_decode(ctx, &buf, pfb, &res_chg);
+
+	if (ret) {
+		mtk_v4l2_err(" <===[%d], src_buf[%d] sz=0x%zx pts=%llu dst_buf[%d] vdec_if_decode() ret=%d res_chg=%d===>",
+			     ctx->id, src_buf->vb2_buf.index, buf.size,
+			     src_buf->vb2_buf.timestamp, dst_buf->vb2_buf.index, ret, res_chg);
+		src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+		if (ret == -EIO) {
+			mutex_lock(&ctx->lock);
+			src_buf_info->error = true;
+			mutex_unlock(&ctx->lock);
+		}
+		v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
+	} else if (!res_chg) {
+		/*
+		 * we only return src buffer with VB2_BUF_STATE_DONE
+		 * when decode success without resolution change
+		 */
+		src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+		v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+	}
+
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+	clean_display_buffer(ctx);
+	clean_free_buffer(ctx);
+
+	if (!ret && res_chg) {
+		mtk_vdec_pic_info_update(ctx);
+		/*
+		 * On encountering a resolution change in the stream.
+		 * The driver must first process and decode all
+		 * remaining buffers from before the resolution change
+		 * point, so call flush decode here
+		 */
+		mtk_vdec_flush_decoder(ctx);
+		/*
+		 * After all buffers containing decoded frames from
+		 * before the resolution change point ready to be
+		 * dequeued on the CAPTURE queue, the driver sends a
+		 * V4L2_EVENT_SOURCE_CHANGE event for source change
+		 * type V4L2_EVENT_SRC_CH_RESOLUTION
+		 */
+		mtk_vdec_queue_res_chg_event(ctx);
+	}
+	v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
+}
+
+static void vb2ops_vdec_stateful_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *src_buf;
+	struct mtk_vcodec_mem src_mem;
+	bool res_chg = false;
+	int ret;
+	unsigned int dpbsize = 1, i;
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vb2_v4l2;
+	struct mtk_q_data *dst_q_data;
+
+	mtk_v4l2_debug(3, "[%d] (%d) id=%d, vb=%p", ctx->id,
+		       vb->vb2_queue->type, vb->index, vb);
+	/*
+	 * check if this buffer is ready to be used after decode
+	 */
+	if (vb->vb2_queue->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		struct mtk_video_dec_buf *buf;
+
+		vb2_v4l2 = to_vb2_v4l2_buffer(vb);
+		buf = container_of(vb2_v4l2, struct mtk_video_dec_buf,
+				   m2m_buf.vb);
+		mutex_lock(&ctx->lock);
+		if (!buf->used) {
+			v4l2_m2m_buf_queue(ctx->m2m_ctx, vb2_v4l2);
+			buf->queued_in_vb2 = true;
+			buf->queued_in_v4l2 = true;
+		} else {
+			buf->queued_in_vb2 = false;
+			buf->queued_in_v4l2 = true;
+		}
+		mutex_unlock(&ctx->lock);
+		return;
+	}
+
+	v4l2_m2m_buf_queue(ctx->m2m_ctx, to_vb2_v4l2_buffer(vb));
+
+	if (ctx->state != MTK_STATE_INIT) {
+		mtk_v4l2_debug(3, "[%d] already init driver %d", ctx->id,
+			       ctx->state);
+		return;
+	}
+
+	src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	if (!src_buf) {
+		mtk_v4l2_err("No src buffer");
+		return;
+	}
+
+	if (src_buf == &ctx->empty_flush_buf.vb) {
+		/* This shouldn't happen. Just in case. */
+		mtk_v4l2_err("Invalid flush buffer.");
+		v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+		return;
+	}
+
+	src_mem.va = vb2_plane_vaddr(&src_buf->vb2_buf, 0);
+	src_mem.dma_addr = vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
+	src_mem.size = (size_t)src_buf->vb2_buf.planes[0].bytesused;
+	mtk_v4l2_debug(2, "[%d] buf id=%d va=%p dma=%pad size=%zx", ctx->id,
+		       src_buf->vb2_buf.index, src_mem.va, &src_mem.dma_addr,
+		       src_mem.size);
+
+	ret = vdec_if_decode(ctx, &src_mem, NULL, &res_chg);
+	if (ret || !res_chg) {
+		/*
+		 * fb == NULL means to parse SPS/PPS header or
+		 * resolution info in src_mem. Decode can fail
+		 * if there is no SPS header or picture info
+		 * in bs
+		 */
+
+		src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+		if (ret == -EIO) {
+			mtk_v4l2_err("[%d] Unrecoverable error in vdec_if_decode.", ctx->id);
+			ctx->state = MTK_STATE_ABORT;
+			v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
+		} else {
+			v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+		}
+		mtk_v4l2_debug(ret ? 0 : 1,
+			       "[%d] vdec_if_decode() src_buf=%d, size=%zu, fail=%d, res_chg=%d",
+			       ctx->id, src_buf->vb2_buf.index, src_mem.size, ret, res_chg);
+		return;
+	}
+
+	if (vdec_if_get_param(ctx, GET_PARAM_PIC_INFO, &ctx->picinfo)) {
+		mtk_v4l2_err("[%d]Error!! Cannot get param : GET_PARAM_PICTURE_INFO ERR", ctx->id);
+		return;
+	}
+
+	ctx->last_decoded_picinfo = ctx->picinfo;
+	dst_q_data = &ctx->q_data[MTK_Q_DATA_DST];
+	for (i = 0; i < dst_q_data->fmt->num_planes; i++) {
+		dst_q_data->sizeimage[i] = ctx->picinfo.fb_sz[i];
+		dst_q_data->bytesperline[i] = ctx->picinfo.buf_w;
+	}
+
+	mtk_v4l2_debug(2, "[%d] vdec_if_init() OK wxh=%dx%d pic wxh=%dx%d sz[0]=0x%x sz[1]=0x%x",
+		       ctx->id, ctx->picinfo.buf_w, ctx->picinfo.buf_h, ctx->picinfo.pic_w,
+		       ctx->picinfo.pic_h, dst_q_data->sizeimage[0], dst_q_data->sizeimage[1]);
+
+	ret = vdec_if_get_param(ctx, GET_PARAM_DPB_SIZE, &dpbsize);
+	if (dpbsize == 0)
+		mtk_v4l2_err("[%d] GET_PARAM_DPB_SIZE fail=%d", ctx->id, ret);
+
+	ctx->dpb_size = dpbsize;
+	ctx->state = MTK_STATE_HEADER;
+	mtk_v4l2_debug(1, "[%d] dpbsize=%d", ctx->id, ctx->dpb_size);
+
+	mtk_vdec_queue_res_chg_event(ctx);
+}
+
+static int mtk_vdec_g_v_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mtk_vcodec_ctx *ctx = ctrl_to_ctx(ctrl);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_MIN_BUFFERS_FOR_CAPTURE:
+		if (ctx->state >= MTK_STATE_HEADER) {
+			ctrl->val = ctx->dpb_size;
+		} else {
+			mtk_v4l2_debug(0, "Seqinfo not ready");
+			ctrl->val = 0;
+		}
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops mtk_vcodec_dec_ctrl_ops = {
+	.g_volatile_ctrl = mtk_vdec_g_v_ctrl,
+};
+
+static int mtk_vcodec_dec_ctrls_setup(struct mtk_vcodec_ctx *ctx)
+{
+	struct v4l2_ctrl *ctrl;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_hdl, 1);
+
+	ctrl = v4l2_ctrl_new_std(&ctx->ctrl_hdl, &mtk_vcodec_dec_ctrl_ops,
+				 V4L2_CID_MIN_BUFFERS_FOR_CAPTURE, 0, 32, 1, 1);
+	ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	v4l2_ctrl_new_std_menu(&ctx->ctrl_hdl, &mtk_vcodec_dec_ctrl_ops,
+			       V4L2_CID_MPEG_VIDEO_VP9_PROFILE,
+			       V4L2_MPEG_VIDEO_VP9_PROFILE_0, 0,
+			       V4L2_MPEG_VIDEO_VP9_PROFILE_0);
+	/*
+	 * H264. Baseline / Extended decoding is not supported.
+	 */
+	v4l2_ctrl_new_std_menu(&ctx->ctrl_hdl, &mtk_vcodec_dec_ctrl_ops,
+			       V4L2_CID_MPEG_VIDEO_H264_PROFILE, V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
+			       BIT(V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
+			       BIT(V4L2_MPEG_VIDEO_H264_PROFILE_EXTENDED),
+			       V4L2_MPEG_VIDEO_H264_PROFILE_MAIN);
+
+	if (ctx->ctrl_hdl.error) {
+		mtk_v4l2_err("Adding control failed %d", ctx->ctrl_hdl.error);
+		return ctx->ctrl_hdl.error;
+	}
+
+	v4l2_ctrl_handler_setup(&ctx->ctrl_hdl);
+	return 0;
+}
+
+static void mtk_init_vdec_params(struct mtk_vcodec_ctx *ctx)
+{
+	unsigned int i;
+
+	if (!(ctx->dev->dec_capability & VCODEC_CAPABILITY_4K_DISABLED)) {
+		for (i = 0; i < num_supported_formats; i++) {
+			if (mtk_video_formats[i].type != MTK_FMT_DEC)
+				continue;
+
+			mtk_video_formats[i].frmsize.max_width =
+				VCODEC_DEC_4K_CODED_WIDTH;
+			mtk_video_formats[i].frmsize.max_height =
+				VCODEC_DEC_4K_CODED_HEIGHT;
+		}
+	}
+}
+
+static struct vb2_ops mtk_vdec_frame_vb2_ops = {
+	.queue_setup = vb2ops_vdec_queue_setup,
+	.buf_prepare = vb2ops_vdec_buf_prepare,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.start_streaming = vb2ops_vdec_start_streaming,
+
+	.buf_queue = vb2ops_vdec_stateful_buf_queue,
+	.buf_init = vb2ops_vdec_buf_init,
+	.buf_finish = vb2ops_vdec_buf_finish,
+	.stop_streaming = vb2ops_vdec_stop_streaming,
+};
+
+const struct mtk_vcodec_dec_pdata mtk_vdec_8173_pdata = {
+	.init_vdec_params = mtk_init_vdec_params,
+	.ctrls_setup = mtk_vcodec_dec_ctrls_setup,
+	.vdec_vb2_ops = &mtk_vdec_frame_vb2_ops,
+	.vdec_formats = mtk_video_formats,
+	.num_formats = &num_supported_formats,
+	.default_out_fmt = &mtk_video_formats[DEFAULT_OUT_FMT_IDX],
+	.default_cap_fmt = &mtk_video_formats[DEFAULT_CAP_FMT_IDX],
+	.worker = mtk_vdec_worker,
+	.flush_decoder = mtk_vdec_flush_decoder,
+	.is_subdev_supported = false,
+	.hw_arch = MTK_VDEC_PURE_SINGLE_CORE,
+};
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_stateless.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_stateless.c
new file mode 100644
index 000000000..3000db975
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_dec_stateless.c
@@ -0,0 +1,513 @@
+// SPDX-License-Identifier: GPL-2.0
+
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+#include <linux/module.h>
+
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_dec.h"
+#include "mtk_vcodec_intr.h"
+#include "mtk_vcodec_util.h"
+#include "mtk_vcodec_dec_pm.h"
+#include "vdec_drv_if.h"
+
+/**
+ * struct mtk_stateless_control  - CID control type
+ * @cfg: control configuration
+ * @codec_type: codec type (V4L2 pixel format) for CID control type
+ */
+struct mtk_stateless_control {
+	struct v4l2_ctrl_config cfg;
+	int codec_type;
+};
+
+static const struct mtk_stateless_control mtk_stateless_controls[] = {
+	{
+		.cfg = {
+			.id = V4L2_CID_STATELESS_H264_SPS,
+		},
+		.codec_type = V4L2_PIX_FMT_H264_SLICE,
+	},
+	{
+		.cfg = {
+			.id = V4L2_CID_STATELESS_H264_PPS,
+		},
+		.codec_type = V4L2_PIX_FMT_H264_SLICE,
+	},
+	{
+		.cfg = {
+			.id = V4L2_CID_STATELESS_H264_SCALING_MATRIX,
+		},
+		.codec_type = V4L2_PIX_FMT_H264_SLICE,
+	},
+	{
+		.cfg = {
+			.id = V4L2_CID_STATELESS_H264_DECODE_PARAMS,
+		},
+		.codec_type = V4L2_PIX_FMT_H264_SLICE,
+	},
+	{
+		.cfg = {
+			.id = V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+			.def = V4L2_MPEG_VIDEO_H264_PROFILE_MAIN,
+			.max = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
+			.menu_skip_mask =
+				BIT(V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
+				BIT(V4L2_MPEG_VIDEO_H264_PROFILE_EXTENDED),
+		},
+		.codec_type = V4L2_PIX_FMT_H264_SLICE,
+	},
+	{
+		.cfg = {
+			.id = V4L2_CID_STATELESS_H264_DECODE_MODE,
+			.min = V4L2_STATELESS_H264_DECODE_MODE_FRAME_BASED,
+			.def = V4L2_STATELESS_H264_DECODE_MODE_FRAME_BASED,
+			.max = V4L2_STATELESS_H264_DECODE_MODE_FRAME_BASED,
+		},
+		.codec_type = V4L2_PIX_FMT_H264_SLICE,
+	},
+	{
+		.cfg = {
+			.id = V4L2_CID_STATELESS_H264_START_CODE,
+			.min = V4L2_STATELESS_H264_START_CODE_ANNEX_B,
+			.def = V4L2_STATELESS_H264_START_CODE_ANNEX_B,
+			.max = V4L2_STATELESS_H264_START_CODE_ANNEX_B,
+		},
+		.codec_type = V4L2_PIX_FMT_H264_SLICE,
+	},
+	{
+		.cfg = {
+			.id = V4L2_CID_STATELESS_VP8_FRAME,
+		},
+		.codec_type = V4L2_PIX_FMT_VP8_FRAME,
+	},
+	{
+		.cfg = {
+			.id = V4L2_CID_MPEG_VIDEO_VP8_PROFILE,
+			.min = V4L2_MPEG_VIDEO_VP8_PROFILE_0,
+			.def = V4L2_MPEG_VIDEO_VP8_PROFILE_0,
+			.max = V4L2_MPEG_VIDEO_VP8_PROFILE_3,
+		},
+		.codec_type = V4L2_PIX_FMT_VP8_FRAME,
+	},
+	{
+		.cfg = {
+			.id = V4L2_CID_STATELESS_VP9_FRAME,
+		},
+		.codec_type = V4L2_PIX_FMT_VP9_FRAME,
+	},
+	{
+		.cfg = {
+			.id = V4L2_CID_MPEG_VIDEO_VP9_PROFILE,
+			.min = V4L2_MPEG_VIDEO_VP9_PROFILE_0,
+			.def = V4L2_MPEG_VIDEO_VP9_PROFILE_0,
+			.max = V4L2_MPEG_VIDEO_VP9_PROFILE_3,
+		},
+		.codec_type = V4L2_PIX_FMT_VP9_FRAME,
+	},
+};
+
+#define NUM_CTRLS ARRAY_SIZE(mtk_stateless_controls)
+
+static struct mtk_video_fmt mtk_video_formats[5];
+
+static struct mtk_video_fmt default_out_format;
+static struct mtk_video_fmt default_cap_format;
+static unsigned int num_formats;
+
+static const struct v4l2_frmsize_stepwise stepwise_fhd = {
+	.min_width = MTK_VDEC_MIN_W,
+	.max_width = MTK_VDEC_MAX_W,
+	.step_width = 16,
+	.min_height = MTK_VDEC_MIN_H,
+	.max_height = MTK_VDEC_MAX_H,
+	.step_height = 16
+};
+
+static void mtk_vdec_stateless_cap_to_disp(struct mtk_vcodec_ctx *ctx, int error,
+					   struct media_request *src_buf_req)
+{
+	struct vb2_v4l2_buffer *vb2_dst;
+	enum vb2_buffer_state state;
+
+	if (error)
+		state = VB2_BUF_STATE_ERROR;
+	else
+		state = VB2_BUF_STATE_DONE;
+
+	vb2_dst = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+	if (vb2_dst) {
+		v4l2_m2m_buf_done(vb2_dst, state);
+		mtk_v4l2_debug(2, "free frame buffer id:%d to done list",
+			       vb2_dst->vb2_buf.index);
+	} else {
+		mtk_v4l2_err("dst buffer is NULL");
+	}
+
+	if (src_buf_req)
+		v4l2_ctrl_request_complete(src_buf_req, &ctx->ctrl_hdl);
+}
+
+static struct vdec_fb *vdec_get_cap_buffer(struct mtk_vcodec_ctx *ctx)
+{
+	struct mtk_video_dec_buf *framebuf;
+	struct vb2_v4l2_buffer *vb2_v4l2;
+	struct vb2_buffer *dst_buf;
+	struct vdec_fb *pfb;
+
+	vb2_v4l2 = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	if (!vb2_v4l2) {
+		mtk_v4l2_debug(1, "[%d] dst_buf empty!!", ctx->id);
+		return NULL;
+	}
+
+	dst_buf = &vb2_v4l2->vb2_buf;
+	framebuf = container_of(vb2_v4l2, struct mtk_video_dec_buf, m2m_buf.vb);
+
+	pfb = &framebuf->frame_buffer;
+	pfb->base_y.va = vb2_plane_vaddr(dst_buf, 0);
+	pfb->base_y.dma_addr = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
+	pfb->base_y.size = ctx->q_data[MTK_Q_DATA_DST].sizeimage[0];
+
+	if (ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 2) {
+		pfb->base_c.va = vb2_plane_vaddr(dst_buf, 1);
+		pfb->base_c.dma_addr =
+			vb2_dma_contig_plane_dma_addr(dst_buf, 1);
+		pfb->base_c.size = ctx->q_data[MTK_Q_DATA_DST].sizeimage[1];
+	}
+	mtk_v4l2_debug(1, "id=%d Framebuf  pfb=%p VA=%p Y_DMA=%pad C_DMA=%pad Size=%zx frame_count = %d",
+		       dst_buf->index, pfb, pfb->base_y.va, &pfb->base_y.dma_addr,
+		       &pfb->base_c.dma_addr, pfb->base_y.size, ctx->decoded_frame_cnt);
+
+	return pfb;
+}
+
+static void vb2ops_vdec_buf_request_complete(struct vb2_buffer *vb)
+{
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	v4l2_ctrl_request_complete(vb->req_obj.req, &ctx->ctrl_hdl);
+}
+
+static void mtk_vdec_worker(struct work_struct *work)
+{
+	struct mtk_vcodec_ctx *ctx =
+		container_of(work, struct mtk_vcodec_ctx, decode_work);
+	struct mtk_vcodec_dev *dev = ctx->dev;
+	struct vb2_v4l2_buffer *vb2_v4l2_src;
+	struct vb2_buffer *vb2_src;
+	struct mtk_vcodec_mem *bs_src;
+	struct mtk_video_dec_buf *dec_buf_src;
+	struct media_request *src_buf_req;
+	enum vb2_buffer_state state;
+	bool res_chg = false;
+	int ret;
+
+	vb2_v4l2_src = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	if (!vb2_v4l2_src) {
+		v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
+		mtk_v4l2_debug(1, "[%d] no available source buffer", ctx->id);
+		return;
+	}
+
+	vb2_src = &vb2_v4l2_src->vb2_buf;
+	dec_buf_src = container_of(vb2_v4l2_src, struct mtk_video_dec_buf,
+				   m2m_buf.vb);
+	bs_src = &dec_buf_src->bs_buffer;
+
+	mtk_v4l2_debug(3, "[%d] (%d) id=%d, vb=%p", ctx->id,
+		       vb2_src->vb2_queue->type, vb2_src->index, vb2_src);
+
+	bs_src->va = vb2_plane_vaddr(vb2_src, 0);
+	bs_src->dma_addr = vb2_dma_contig_plane_dma_addr(vb2_src, 0);
+	bs_src->size = (size_t)vb2_src->planes[0].bytesused;
+	if (!bs_src->va) {
+		v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
+		mtk_v4l2_err("[%d] id=%d source buffer is NULL", ctx->id,
+			     vb2_src->index);
+		return;
+	}
+
+	mtk_v4l2_debug(3, "[%d] Bitstream VA=%p DMA=%pad Size=%zx vb=%p",
+		       ctx->id, bs_src->va, &bs_src->dma_addr, bs_src->size, vb2_src);
+	/* Apply request controls. */
+	src_buf_req = vb2_src->req_obj.req;
+	if (src_buf_req)
+		v4l2_ctrl_request_setup(src_buf_req, &ctx->ctrl_hdl);
+	else
+		mtk_v4l2_err("vb2 buffer media request is NULL");
+
+	ret = vdec_if_decode(ctx, bs_src, NULL, &res_chg);
+	if (ret) {
+		mtk_v4l2_err(" <===[%d], src_buf[%d] sz=0x%zx pts=%llu vdec_if_decode() ret=%d res_chg=%d===>",
+			     ctx->id, vb2_src->index, bs_src->size,
+			     vb2_src->timestamp, ret, res_chg);
+		if (ret == -EIO) {
+			mutex_lock(&ctx->lock);
+			dec_buf_src->error = true;
+			mutex_unlock(&ctx->lock);
+		}
+	}
+
+	state = ret ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE;
+	if (!IS_VDEC_LAT_ARCH(dev->vdec_pdata->hw_arch) ||
+	    ctx->current_codec == V4L2_PIX_FMT_VP8_FRAME) {
+		v4l2_m2m_buf_done_and_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx, state);
+		if (src_buf_req)
+			v4l2_ctrl_request_complete(src_buf_req, &ctx->ctrl_hdl);
+	} else {
+		if (ret != -EAGAIN) {
+			v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+			v4l2_m2m_buf_done(vb2_v4l2_src, state);
+		}
+		v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
+	}
+}
+
+static void vb2ops_vdec_stateless_buf_queue(struct vb2_buffer *vb)
+{
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vb2_v4l2 = to_vb2_v4l2_buffer(vb);
+
+	mtk_v4l2_debug(3, "[%d] (%d) id=%d, vb=%p", ctx->id, vb->vb2_queue->type, vb->index, vb);
+
+	mutex_lock(&ctx->lock);
+	v4l2_m2m_buf_queue(ctx->m2m_ctx, vb2_v4l2);
+	mutex_unlock(&ctx->lock);
+	if (vb->vb2_queue->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return;
+
+	/* If an OUTPUT buffer, we may need to update the state */
+	if (ctx->state == MTK_STATE_INIT) {
+		ctx->state = MTK_STATE_HEADER;
+		mtk_v4l2_debug(1, "Init driver from init to header.");
+	} else {
+		mtk_v4l2_debug(3, "[%d] already init driver %d", ctx->id, ctx->state);
+	}
+}
+
+static int mtk_vdec_flush_decoder(struct mtk_vcodec_ctx *ctx)
+{
+	bool res_chg;
+
+	return vdec_if_decode(ctx, NULL, NULL, &res_chg);
+}
+
+static int mtk_vcodec_dec_ctrls_setup(struct mtk_vcodec_ctx *ctx)
+{
+	unsigned int i;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_hdl, NUM_CTRLS);
+	if (ctx->ctrl_hdl.error) {
+		mtk_v4l2_err("v4l2_ctrl_handler_init failed\n");
+		return ctx->ctrl_hdl.error;
+	}
+
+	for (i = 0; i < NUM_CTRLS; i++) {
+		struct v4l2_ctrl_config cfg = mtk_stateless_controls[i].cfg;
+
+		v4l2_ctrl_new_custom(&ctx->ctrl_hdl, &cfg, NULL);
+		if (ctx->ctrl_hdl.error) {
+			mtk_v4l2_err("Adding control %d failed %d", i, ctx->ctrl_hdl.error);
+			return ctx->ctrl_hdl.error;
+		}
+	}
+
+	v4l2_ctrl_handler_setup(&ctx->ctrl_hdl);
+
+	return 0;
+}
+
+static int fops_media_request_validate(struct media_request *mreq)
+{
+	const unsigned int buffer_cnt = vb2_request_buffer_cnt(mreq);
+
+	switch (buffer_cnt) {
+	case 1:
+		/* We expect exactly one buffer with the request */
+		break;
+	case 0:
+		mtk_v4l2_debug(1, "No buffer provided with the request");
+		return -ENOENT;
+	default:
+		mtk_v4l2_debug(1, "Too many buffers (%d) provided with the request",
+			       buffer_cnt);
+		return -EINVAL;
+	}
+
+	return vb2_request_validate(mreq);
+}
+
+const struct media_device_ops mtk_vcodec_media_ops = {
+	.req_validate	= fops_media_request_validate,
+	.req_queue	= v4l2_m2m_request_queue,
+};
+
+static void mtk_vcodec_add_formats(unsigned int fourcc,
+				   struct mtk_vcodec_ctx *ctx)
+{
+	struct mtk_vcodec_dev *dev = ctx->dev;
+	const struct mtk_vcodec_dec_pdata *pdata = dev->vdec_pdata;
+	int count_formats = *pdata->num_formats;
+
+	switch (fourcc) {
+	case V4L2_PIX_FMT_H264_SLICE:
+	case V4L2_PIX_FMT_VP8_FRAME:
+	case V4L2_PIX_FMT_VP9_FRAME:
+		mtk_video_formats[count_formats].fourcc = fourcc;
+		mtk_video_formats[count_formats].type = MTK_FMT_DEC;
+		mtk_video_formats[count_formats].num_planes = 1;
+		mtk_video_formats[count_formats].frmsize = stepwise_fhd;
+
+		if (!(ctx->dev->dec_capability & VCODEC_CAPABILITY_4K_DISABLED) &&
+		    fourcc != V4L2_PIX_FMT_VP8_FRAME) {
+			mtk_video_formats[count_formats].frmsize.max_width =
+				VCODEC_DEC_4K_CODED_WIDTH;
+			mtk_video_formats[count_formats].frmsize.max_height =
+				VCODEC_DEC_4K_CODED_HEIGHT;
+		}
+		break;
+	case V4L2_PIX_FMT_MM21:
+	case V4L2_PIX_FMT_MT21C:
+		mtk_video_formats[count_formats].fourcc = fourcc;
+		mtk_video_formats[count_formats].type = MTK_FMT_FRAME;
+		mtk_video_formats[count_formats].num_planes = 2;
+		break;
+	default:
+		mtk_v4l2_err("Can not add unsupported format type");
+		return;
+	}
+
+	num_formats++;
+	mtk_v4l2_debug(3, "num_formats: %d dec_capability: 0x%x",
+		       count_formats, ctx->dev->dec_capability);
+}
+
+static void mtk_vcodec_get_supported_formats(struct mtk_vcodec_ctx *ctx)
+{
+	int cap_format_count = 0, out_format_count = 0;
+
+	if (num_formats)
+		return;
+
+	if (ctx->dev->dec_capability & MTK_VDEC_FORMAT_MT21C) {
+		mtk_vcodec_add_formats(V4L2_PIX_FMT_MT21C, ctx);
+		cap_format_count++;
+	}
+	if (ctx->dev->dec_capability & MTK_VDEC_FORMAT_MM21) {
+		mtk_vcodec_add_formats(V4L2_PIX_FMT_MM21, ctx);
+		cap_format_count++;
+	}
+	if (ctx->dev->dec_capability & MTK_VDEC_FORMAT_H264_SLICE) {
+		mtk_vcodec_add_formats(V4L2_PIX_FMT_H264_SLICE, ctx);
+		out_format_count++;
+	}
+	if (ctx->dev->dec_capability & MTK_VDEC_FORMAT_VP8_FRAME) {
+		mtk_vcodec_add_formats(V4L2_PIX_FMT_VP8_FRAME, ctx);
+		out_format_count++;
+	}
+	if (ctx->dev->dec_capability & MTK_VDEC_FORMAT_VP9_FRAME) {
+		mtk_vcodec_add_formats(V4L2_PIX_FMT_VP9_FRAME, ctx);
+		out_format_count++;
+	}
+
+	if (cap_format_count)
+		default_cap_format = mtk_video_formats[cap_format_count - 1];
+	if (out_format_count)
+		default_out_format =
+			mtk_video_formats[cap_format_count + out_format_count - 1];
+}
+
+static void mtk_init_vdec_params(struct mtk_vcodec_ctx *ctx)
+{
+	struct vb2_queue *src_vq;
+
+	src_vq = v4l2_m2m_get_vq(ctx->m2m_ctx,
+				 V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+
+	if (!ctx->dev->vdec_pdata->is_subdev_supported)
+		ctx->dev->dec_capability |=
+			MTK_VDEC_FORMAT_H264_SLICE | MTK_VDEC_FORMAT_MM21;
+	mtk_vcodec_get_supported_formats(ctx);
+
+	/* Support request api for output plane */
+	src_vq->supports_requests = true;
+	src_vq->requires_requests = true;
+}
+
+static int vb2ops_vdec_out_buf_validate(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	vbuf->field = V4L2_FIELD_NONE;
+	return 0;
+}
+
+static struct vb2_ops mtk_vdec_request_vb2_ops = {
+	.queue_setup	= vb2ops_vdec_queue_setup,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+	.start_streaming	= vb2ops_vdec_start_streaming,
+	.stop_streaming	= vb2ops_vdec_stop_streaming,
+
+	.buf_queue	= vb2ops_vdec_stateless_buf_queue,
+	.buf_out_validate = vb2ops_vdec_out_buf_validate,
+	.buf_init	= vb2ops_vdec_buf_init,
+	.buf_prepare	= vb2ops_vdec_buf_prepare,
+	.buf_finish	= vb2ops_vdec_buf_finish,
+	.buf_request_complete = vb2ops_vdec_buf_request_complete,
+};
+
+const struct mtk_vcodec_dec_pdata mtk_vdec_8183_pdata = {
+	.init_vdec_params = mtk_init_vdec_params,
+	.ctrls_setup = mtk_vcodec_dec_ctrls_setup,
+	.vdec_vb2_ops = &mtk_vdec_request_vb2_ops,
+	.vdec_formats = mtk_video_formats,
+	.num_formats = &num_formats,
+	.default_out_fmt = &default_out_format,
+	.default_cap_fmt = &default_cap_format,
+	.uses_stateless_api = true,
+	.worker = mtk_vdec_worker,
+	.flush_decoder = mtk_vdec_flush_decoder,
+	.cap_to_disp = mtk_vdec_stateless_cap_to_disp,
+	.get_cap_buffer = vdec_get_cap_buffer,
+	.is_subdev_supported = false,
+	.hw_arch = MTK_VDEC_PURE_SINGLE_CORE,
+};
+
+/* This platform data is used for one lat and one core architecture. */
+const struct mtk_vcodec_dec_pdata mtk_lat_sig_core_pdata = {
+	.init_vdec_params = mtk_init_vdec_params,
+	.ctrls_setup = mtk_vcodec_dec_ctrls_setup,
+	.vdec_vb2_ops = &mtk_vdec_request_vb2_ops,
+	.vdec_formats = mtk_video_formats,
+	.num_formats = &num_formats,
+	.default_out_fmt = &default_out_format,
+	.default_cap_fmt = &default_cap_format,
+	.uses_stateless_api = true,
+	.worker = mtk_vdec_worker,
+	.flush_decoder = mtk_vdec_flush_decoder,
+	.cap_to_disp = mtk_vdec_stateless_cap_to_disp,
+	.get_cap_buffer = vdec_get_cap_buffer,
+	.is_subdev_supported = true,
+	.hw_arch = MTK_VDEC_LAT_SINGLE_CORE,
+};
+
+const struct mtk_vcodec_dec_pdata mtk_vdec_single_core_pdata = {
+	.init_vdec_params = mtk_init_vdec_params,
+	.ctrls_setup = mtk_vcodec_dec_ctrls_setup,
+	.vdec_vb2_ops = &mtk_vdec_request_vb2_ops,
+	.vdec_formats = mtk_video_formats,
+	.num_formats = &num_formats,
+	.default_out_fmt = &default_out_format,
+	.default_cap_fmt = &default_cap_format,
+	.uses_stateless_api = true,
+	.worker = mtk_vdec_worker,
+	.flush_decoder = mtk_vdec_flush_decoder,
+	.cap_to_disp = mtk_vdec_stateless_cap_to_disp,
+	.get_cap_buffer = vdec_get_cap_buffer,
+	.is_subdev_supported = true,
+	.hw_arch = MTK_VDEC_PURE_SINGLE_CORE,
+};
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_drv.h b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_drv.h
new file mode 100644
index 000000000..9acab54fd
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_drv.h
@@ -0,0 +1,544 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+* Copyright (c) 2016 MediaTek Inc.
+* Author: PC Chen <pc.chen@mediatek.com>
+*         Tiffany Lin <tiffany.lin@mediatek.com>
+*/
+
+#ifndef _MTK_VCODEC_DRV_H_
+#define _MTK_VCODEC_DRV_H_
+
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-core.h>
+
+#include "mtk_vcodec_util.h"
+#include "vdec_msg_queue.h"
+
+#define MTK_VCODEC_DEC_NAME	"mtk-vcodec-dec"
+#define MTK_VCODEC_ENC_NAME	"mtk-vcodec-enc"
+
+#define MTK_VCODEC_MAX_PLANES	3
+#define MTK_V4L2_BENCHMARK	0
+#define WAIT_INTR_TIMEOUT_MS	1000
+#define IS_VDEC_LAT_ARCH(hw_arch) ((hw_arch) >= MTK_VDEC_LAT_SINGLE_CORE)
+#define IS_VDEC_INNER_RACING(capability) ((capability) & MTK_VCODEC_INNER_RACING)
+
+/*
+ * enum mtk_hw_reg_idx - MTK hw register base index
+ */
+enum mtk_hw_reg_idx {
+	VDEC_SYS,
+	VDEC_MISC,
+	VDEC_LD,
+	VDEC_TOP,
+	VDEC_CM,
+	VDEC_AD,
+	VDEC_AV,
+	VDEC_PP,
+	VDEC_HWD,
+	VDEC_HWQ,
+	VDEC_HWB,
+	VDEC_HWG,
+	NUM_MAX_VDEC_REG_BASE,
+	/* h264 encoder */
+	VENC_SYS = NUM_MAX_VDEC_REG_BASE,
+	/* vp8 encoder */
+	VENC_LT_SYS,
+	NUM_MAX_VCODEC_REG_BASE
+};
+
+/*
+ * enum mtk_instance_type - The type of an MTK Vcodec instance.
+ */
+enum mtk_instance_type {
+	MTK_INST_DECODER		= 0,
+	MTK_INST_ENCODER		= 1,
+};
+
+/**
+ * enum mtk_instance_state - The state of an MTK Vcodec instance.
+ * @MTK_STATE_FREE: default state when instance is created
+ * @MTK_STATE_INIT: vcodec instance is initialized
+ * @MTK_STATE_HEADER: vdec had sps/pps header parsed or venc
+ *			had sps/pps header encoded
+ * @MTK_STATE_FLUSH: vdec is flushing. Only used by decoder
+ * @MTK_STATE_ABORT: vcodec should be aborted
+ */
+enum mtk_instance_state {
+	MTK_STATE_FREE = 0,
+	MTK_STATE_INIT = 1,
+	MTK_STATE_HEADER = 2,
+	MTK_STATE_FLUSH = 3,
+	MTK_STATE_ABORT = 4,
+};
+
+/*
+ * enum mtk_encode_param - General encoding parameters type
+ */
+enum mtk_encode_param {
+	MTK_ENCODE_PARAM_NONE = 0,
+	MTK_ENCODE_PARAM_BITRATE = (1 << 0),
+	MTK_ENCODE_PARAM_FRAMERATE = (1 << 1),
+	MTK_ENCODE_PARAM_INTRA_PERIOD = (1 << 2),
+	MTK_ENCODE_PARAM_FORCE_INTRA = (1 << 3),
+	MTK_ENCODE_PARAM_GOP_SIZE = (1 << 4),
+};
+
+enum mtk_fmt_type {
+	MTK_FMT_DEC = 0,
+	MTK_FMT_ENC = 1,
+	MTK_FMT_FRAME = 2,
+};
+
+/*
+ * enum mtk_vdec_hw_id - Hardware index used to separate
+ *                         different hardware
+ */
+enum mtk_vdec_hw_id {
+	MTK_VDEC_CORE,
+	MTK_VDEC_LAT0,
+	MTK_VDEC_LAT1,
+	MTK_VDEC_LAT_SOC,
+	MTK_VDEC_HW_MAX,
+};
+
+/*
+ * enum mtk_vdec_hw_count - Supported hardware count
+ */
+enum mtk_vdec_hw_count {
+	MTK_VDEC_NO_HW = 0,
+	MTK_VDEC_ONE_CORE,
+	MTK_VDEC_ONE_LAT_ONE_CORE,
+	MTK_VDEC_MAX_HW_COUNT,
+};
+
+/*
+ * struct mtk_video_fmt - Structure used to store information about pixelformats
+ */
+struct mtk_video_fmt {
+	u32	fourcc;
+	enum mtk_fmt_type	type;
+	u32	num_planes;
+	u32	flags;
+	struct v4l2_frmsize_stepwise frmsize;
+};
+
+/*
+ * enum mtk_q_type - Type of queue
+ */
+enum mtk_q_type {
+	MTK_Q_DATA_SRC = 0,
+	MTK_Q_DATA_DST = 1,
+};
+
+/*
+ * struct mtk_q_data - Structure used to store information about queue
+ */
+struct mtk_q_data {
+	unsigned int	visible_width;
+	unsigned int	visible_height;
+	unsigned int	coded_width;
+	unsigned int	coded_height;
+	enum v4l2_field	field;
+	unsigned int	bytesperline[MTK_VCODEC_MAX_PLANES];
+	unsigned int	sizeimage[MTK_VCODEC_MAX_PLANES];
+	const struct mtk_video_fmt	*fmt;
+};
+
+/**
+ * struct mtk_enc_params - General encoding parameters
+ * @bitrate: target bitrate in bits per second
+ * @num_b_frame: number of b frames between p-frame
+ * @rc_frame: frame based rate control
+ * @rc_mb: macroblock based rate control
+ * @seq_hdr_mode: H.264 sequence header is encoded separately or joined
+ *		  with the first frame
+ * @intra_period: I frame period
+ * @gop_size: group of picture size, it's used as the intra frame period
+ * @framerate_num: frame rate numerator. ex: framerate_num=30 and
+ *		   framerate_denom=1 means FPS is 30
+ * @framerate_denom: frame rate denominator. ex: framerate_num=30 and
+ *		     framerate_denom=1 means FPS is 30
+ * @h264_max_qp: Max value for H.264 quantization parameter
+ * @h264_profile: V4L2 defined H.264 profile
+ * @h264_level: V4L2 defined H.264 level
+ * @force_intra: force/insert intra frame
+ */
+struct mtk_enc_params {
+	unsigned int	bitrate;
+	unsigned int	num_b_frame;
+	unsigned int	rc_frame;
+	unsigned int	rc_mb;
+	unsigned int	seq_hdr_mode;
+	unsigned int	intra_period;
+	unsigned int	gop_size;
+	unsigned int	framerate_num;
+	unsigned int	framerate_denom;
+	unsigned int	h264_max_qp;
+	unsigned int	h264_profile;
+	unsigned int	h264_level;
+	unsigned int	force_intra;
+};
+
+/*
+ * struct mtk_vcodec_clk_info - Structure used to store clock name
+ */
+struct mtk_vcodec_clk_info {
+	const char	*clk_name;
+	struct clk	*vcodec_clk;
+};
+
+/*
+ * struct mtk_vcodec_clk - Structure used to store vcodec clock information
+ */
+struct mtk_vcodec_clk {
+	struct mtk_vcodec_clk_info	*clk_info;
+	int	clk_num;
+};
+
+/*
+ * struct mtk_vcodec_pm - Power management data structure
+ */
+struct mtk_vcodec_pm {
+	struct mtk_vcodec_clk	vdec_clk;
+	struct mtk_vcodec_clk	venc_clk;
+	struct device	*dev;
+};
+
+/**
+ * struct vdec_pic_info  - picture size information
+ * @pic_w: picture width
+ * @pic_h: picture height
+ * @buf_w: picture buffer width (64 aligned up from pic_w)
+ * @buf_h: picture buffer heiht (64 aligned up from pic_h)
+ * @fb_sz: bitstream size of each plane
+ * E.g. suppose picture size is 176x144,
+ *      buffer size will be aligned to 176x160.
+ * @cap_fourcc: fourcc number(may changed when resolution change)
+ * @reserved: align struct to 64-bit in order to adjust 32-bit and 64-bit os.
+ */
+struct vdec_pic_info {
+	unsigned int pic_w;
+	unsigned int pic_h;
+	unsigned int buf_w;
+	unsigned int buf_h;
+	unsigned int fb_sz[VIDEO_MAX_PLANES];
+	unsigned int cap_fourcc;
+	unsigned int reserved;
+};
+
+/**
+ * struct mtk_vcodec_ctx - Context (instance) private data.
+ *
+ * @type: type of the instance - decoder or encoder
+ * @dev: pointer to the mtk_vcodec_dev of the device
+ * @list: link to ctx_list of mtk_vcodec_dev
+ * @fh: struct v4l2_fh
+ * @m2m_ctx: pointer to the v4l2_m2m_ctx of the context
+ * @q_data: store information of input and output queue
+ *	    of the context
+ * @id: index of the context that this structure describes
+ * @state: state of the context
+ * @param_change: indicate encode parameter type
+ * @enc_params: encoding parameters
+ * @dec_if: hooked decoder driver interface
+ * @enc_if: hooked encoder driver interface
+ * @drv_handle: driver handle for specific decode/encode instance
+ *
+ * @picinfo: store picture info after header parsing
+ * @dpb_size: store dpb count after header parsing
+ * @int_cond: variable used by the waitqueue
+ * @int_type: type of the last interrupt
+ * @queue: waitqueue that can be used to wait for this context to
+ *	   finish
+ * @irq_status: irq status
+ *
+ * @ctrl_hdl: handler for v4l2 framework
+ * @decode_work: worker for the decoding
+ * @encode_work: worker for the encoding
+ * @last_decoded_picinfo: pic information get from latest decode
+ * @empty_flush_buf: a fake size-0 capture buffer that indicates flush. Only
+ *		     to be used with encoder and stateful decoder.
+ * @is_flushing: set to true if flushing is in progress.
+ * @current_codec: current set input codec, in V4L2 pixel format
+ * @capture_fourcc: capture queue type in V4L2 pixel format
+ *
+ * @colorspace: enum v4l2_colorspace; supplemental to pixelformat
+ * @ycbcr_enc: enum v4l2_ycbcr_encoding, Y'CbCr encoding
+ * @quantization: enum v4l2_quantization, colorspace quantization
+ * @xfer_func: enum v4l2_xfer_func, colorspace transfer function
+ * @decoded_frame_cnt: number of decoded frames
+ * @lock: protect variables accessed by V4L2 threads and worker thread such as
+ *	  mtk_video_dec_buf.
+ * @hw_id: hardware index used to identify different hardware.
+ *
+ * @msg_queue: msg queue used to store lat buffer information.
+ * @q_mutex: vb2_queue mutex.
+ */
+struct mtk_vcodec_ctx {
+	enum mtk_instance_type type;
+	struct mtk_vcodec_dev *dev;
+	struct list_head list;
+
+	struct v4l2_fh fh;
+	struct v4l2_m2m_ctx *m2m_ctx;
+	struct mtk_q_data q_data[2];
+	int id;
+	enum mtk_instance_state state;
+	enum mtk_encode_param param_change;
+	struct mtk_enc_params enc_params;
+
+	const struct vdec_common_if *dec_if;
+	const struct venc_common_if *enc_if;
+	void *drv_handle;
+
+	struct vdec_pic_info picinfo;
+	int dpb_size;
+
+	int int_cond[MTK_VDEC_HW_MAX];
+	int int_type[MTK_VDEC_HW_MAX];
+	wait_queue_head_t queue[MTK_VDEC_HW_MAX];
+	unsigned int irq_status;
+
+	struct v4l2_ctrl_handler ctrl_hdl;
+	struct work_struct decode_work;
+	struct work_struct encode_work;
+	struct vdec_pic_info last_decoded_picinfo;
+	struct v4l2_m2m_buffer empty_flush_buf;
+	bool is_flushing;
+
+	u32 current_codec;
+	u32 capture_fourcc;
+
+	enum v4l2_colorspace colorspace;
+	enum v4l2_ycbcr_encoding ycbcr_enc;
+	enum v4l2_quantization quantization;
+	enum v4l2_xfer_func xfer_func;
+
+	int decoded_frame_cnt;
+	struct mutex lock;
+	int hw_id;
+
+	struct vdec_msg_queue msg_queue;
+
+	struct mutex q_mutex;
+};
+
+/*
+ * enum mtk_vdec_hw_arch - Used to separate different hardware architecture
+ */
+enum mtk_vdec_hw_arch {
+	MTK_VDEC_PURE_SINGLE_CORE,
+	MTK_VDEC_LAT_SINGLE_CORE,
+};
+
+/*
+ * struct mtk_vdec_format_types - Structure used to get supported
+ *		  format types according to decoder capability
+ */
+enum mtk_vdec_format_types {
+	MTK_VDEC_FORMAT_MM21 = 0x20,
+	MTK_VDEC_FORMAT_MT21C = 0x40,
+	MTK_VDEC_FORMAT_H264_SLICE = 0x100,
+	MTK_VDEC_FORMAT_VP8_FRAME = 0x200,
+	MTK_VDEC_FORMAT_VP9_FRAME = 0x400,
+	MTK_VCODEC_INNER_RACING = 0x20000,
+};
+
+/**
+ * struct mtk_vcodec_dec_pdata - compatible data for each IC
+ * @init_vdec_params: init vdec params
+ * @ctrls_setup: init vcodec dec ctrls
+ * @worker: worker to start a decode job
+ * @flush_decoder: function that flushes the decoder
+ * @get_cap_buffer: get capture buffer from capture queue
+ * @cap_to_disp: put capture buffer to disp list for lat and core arch
+ * @vdec_vb2_ops: struct vb2_ops
+ *
+ * @vdec_formats: supported video decoder formats
+ * @num_formats: count of video decoder formats
+ * @default_out_fmt: default output buffer format
+ * @default_cap_fmt: default capture buffer format
+ *
+ * @hw_arch: hardware arch is used to separate pure_sin_core and lat_sin_core
+ *
+ * @is_subdev_supported: whether support parent-node architecture(subdev)
+ * @uses_stateless_api: whether the decoder uses the stateless API with requests
+ */
+
+struct mtk_vcodec_dec_pdata {
+	void (*init_vdec_params)(struct mtk_vcodec_ctx *ctx);
+	int (*ctrls_setup)(struct mtk_vcodec_ctx *ctx);
+	void (*worker)(struct work_struct *work);
+	int (*flush_decoder)(struct mtk_vcodec_ctx *ctx);
+	struct vdec_fb *(*get_cap_buffer)(struct mtk_vcodec_ctx *ctx);
+	void (*cap_to_disp)(struct mtk_vcodec_ctx *ctx, int error,
+			    struct media_request *src_buf_req);
+
+	struct vb2_ops *vdec_vb2_ops;
+
+	const struct mtk_video_fmt *vdec_formats;
+	const int *num_formats;
+	const struct mtk_video_fmt *default_out_fmt;
+	const struct mtk_video_fmt *default_cap_fmt;
+
+	enum mtk_vdec_hw_arch hw_arch;
+
+	bool is_subdev_supported;
+	bool uses_stateless_api;
+};
+
+/**
+ * struct mtk_vcodec_enc_pdata - compatible data for each IC
+ *
+ * @uses_ext: whether the encoder uses the extended firmware messaging format
+ * @min_bitrate: minimum supported encoding bitrate
+ * @max_bitrate: maximum supported encoding bitrate
+ * @capture_formats: array of supported capture formats
+ * @num_capture_formats: number of entries in capture_formats
+ * @output_formats: array of supported output formats
+ * @num_output_formats: number of entries in output_formats
+ * @core_id: stand for h264 or vp8 encode index
+ * @uses_34bit: whether the encoder uses 34-bit iova
+ */
+struct mtk_vcodec_enc_pdata {
+	bool uses_ext;
+	unsigned long min_bitrate;
+	unsigned long max_bitrate;
+	const struct mtk_video_fmt *capture_formats;
+	size_t num_capture_formats;
+	const struct mtk_video_fmt *output_formats;
+	size_t num_output_formats;
+	int core_id;
+	bool uses_34bit;
+};
+
+#define MTK_ENC_CTX_IS_EXT(ctx) ((ctx)->dev->venc_pdata->uses_ext)
+#define MTK_ENC_IOVA_IS_34BIT(ctx) ((ctx)->dev->venc_pdata->uses_34bit)
+
+/**
+ * struct mtk_vcodec_dev - driver data
+ * @v4l2_dev: V4L2 device to register video devices for.
+ * @vfd_dec: Video device for decoder
+ * @mdev_dec: Media device for decoder
+ * @vfd_enc: Video device for encoder.
+ *
+ * @m2m_dev_dec: m2m device for decoder
+ * @m2m_dev_enc: m2m device for encoder.
+ * @plat_dev: platform device
+ * @ctx_list: list of struct mtk_vcodec_ctx
+ * @irqlock: protect data access by irq handler and work thread
+ * @curr_ctx: The context that is waiting for codec hardware
+ *
+ * @reg_base: Mapped address of MTK Vcodec registers.
+ * @vdec_pdata: decoder IC-specific data
+ * @venc_pdata: encoder IC-specific data
+ *
+ * @fw_handler: used to communicate with the firmware.
+ * @id_counter: used to identify current opened instance
+ *
+ * @decode_workqueue: decode work queue
+ * @encode_workqueue: encode work queue
+ *
+ * @int_cond: used to identify interrupt condition happen
+ * @int_type: used to identify what kind of interrupt condition happen
+ * @dev_mutex: video_device lock
+ * @queue: waitqueue for waiting for completion of device commands
+ *
+ * @dec_irq: decoder irq resource
+ * @enc_irq: h264 encoder irq resource
+ *
+ * @dec_mutex: decoder hardware lock
+ * @enc_mutex: encoder hardware lock.
+ *
+ * @pm: power management control
+ * @dec_capability: used to identify decode capability, ex: 4k
+ * @enc_capability: used to identify encode capability
+ *
+ * @core_workqueue: queue used for core hardware decode
+ * @msg_queue_core_ctx: msg queue context used for core workqueue
+ *
+ * @subdev_dev: subdev hardware device
+ * @subdev_prob_done: check whether all used hw device is prob done
+ * @subdev_bitmap: used to record hardware is ready or not
+ *
+ * @dec_active_cnt: used to mark whether need to record register value
+ * @vdec_racing_info: record register value
+ * @dec_racing_info_mutex: mutex lock used for inner racing mode
+ */
+struct mtk_vcodec_dev {
+	struct v4l2_device v4l2_dev;
+	struct video_device *vfd_dec;
+	struct media_device mdev_dec;
+	struct video_device *vfd_enc;
+
+	struct v4l2_m2m_dev *m2m_dev_dec;
+	struct v4l2_m2m_dev *m2m_dev_enc;
+	struct platform_device *plat_dev;
+	struct list_head ctx_list;
+	spinlock_t irqlock;
+	struct mtk_vcodec_ctx *curr_ctx;
+	void __iomem *reg_base[NUM_MAX_VCODEC_REG_BASE];
+	const struct mtk_vcodec_dec_pdata *vdec_pdata;
+	const struct mtk_vcodec_enc_pdata *venc_pdata;
+
+	struct mtk_vcodec_fw *fw_handler;
+
+	unsigned long id_counter;
+
+	struct workqueue_struct *decode_workqueue;
+	struct workqueue_struct *encode_workqueue;
+	int int_cond;
+	int int_type;
+	struct mutex dev_mutex;
+	wait_queue_head_t queue;
+
+	int dec_irq;
+	int enc_irq;
+
+	/* decoder hardware mutex lock */
+	struct mutex dec_mutex[MTK_VDEC_HW_MAX];
+	struct mutex enc_mutex;
+
+	struct mtk_vcodec_pm pm;
+	unsigned int dec_capability;
+	unsigned int enc_capability;
+
+	struct workqueue_struct *core_workqueue;
+	struct vdec_msg_queue_ctx msg_queue_core_ctx;
+
+	void *subdev_dev[MTK_VDEC_HW_MAX];
+	int (*subdev_prob_done)(struct mtk_vcodec_dev *vdec_dev);
+	DECLARE_BITMAP(subdev_bitmap, MTK_VDEC_HW_MAX);
+
+	atomic_t dec_active_cnt;
+	u32 vdec_racing_info[132];
+	/* Protects access to vdec_racing_info data */
+	struct mutex dec_racing_info_mutex;
+};
+
+static inline struct mtk_vcodec_ctx *fh_to_ctx(struct v4l2_fh *fh)
+{
+	return container_of(fh, struct mtk_vcodec_ctx, fh);
+}
+
+static inline struct mtk_vcodec_ctx *ctrl_to_ctx(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct mtk_vcodec_ctx, ctrl_hdl);
+}
+
+/* Wake up context wait_queue */
+static inline void
+wake_up_ctx(struct mtk_vcodec_ctx *ctx, unsigned int reason, unsigned int hw_id)
+{
+	ctx->int_cond[hw_id] = 1;
+	ctx->int_type[hw_id] = reason;
+	wake_up_interruptible(&ctx->queue[hw_id]);
+}
+
+#endif /* _MTK_VCODEC_DRV_H_ */
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc.c
new file mode 100644
index 000000000..31e3c3766
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc.c
@@ -0,0 +1,1487 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+* Copyright (c) 2016 MediaTek Inc.
+* Author: PC Chen <pc.chen@mediatek.com>
+*         Tiffany Lin <tiffany.lin@mediatek.com>
+*/
+
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+#include <linux/pm_runtime.h>
+
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_enc.h"
+#include "mtk_vcodec_intr.h"
+#include "mtk_vcodec_util.h"
+#include "venc_drv_if.h"
+
+#define MTK_VENC_MIN_W	160U
+#define MTK_VENC_MIN_H	128U
+#define MTK_VENC_HD_MAX_W	1920U
+#define MTK_VENC_HD_MAX_H	1088U
+#define MTK_VENC_4K_MAX_W	3840U
+#define MTK_VENC_4K_MAX_H	2176U
+
+#define DFT_CFG_WIDTH	MTK_VENC_MIN_W
+#define DFT_CFG_HEIGHT	MTK_VENC_MIN_H
+#define MTK_MAX_CTRLS_HINT	20
+
+#define MTK_DEFAULT_FRAMERATE_NUM 1001
+#define MTK_DEFAULT_FRAMERATE_DENOM 30000
+#define MTK_VENC_4K_CAPABILITY_ENABLE BIT(0)
+
+static void mtk_venc_worker(struct work_struct *work);
+
+static const struct v4l2_frmsize_stepwise mtk_venc_hd_framesizes = {
+	MTK_VENC_MIN_W, MTK_VENC_HD_MAX_W, 16,
+	MTK_VENC_MIN_H, MTK_VENC_HD_MAX_H, 16,
+};
+
+static const struct v4l2_frmsize_stepwise mtk_venc_4k_framesizes = {
+	MTK_VENC_MIN_W, MTK_VENC_4K_MAX_W, 16,
+	MTK_VENC_MIN_H, MTK_VENC_4K_MAX_H, 16,
+};
+
+static int vidioc_venc_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mtk_vcodec_ctx *ctx = ctrl_to_ctx(ctrl);
+	struct mtk_enc_params *p = &ctx->enc_params;
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_BITRATE_MODE val= %d",
+			       ctrl->val);
+		if (ctrl->val != V4L2_MPEG_VIDEO_BITRATE_MODE_CBR) {
+			mtk_v4l2_err("Unsupported bitrate mode =%d", ctrl->val);
+			ret = -EINVAL;
+		}
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_BITRATE val = %d",
+			       ctrl->val);
+		p->bitrate = ctrl->val;
+		ctx->param_change |= MTK_ENCODE_PARAM_BITRATE;
+		break;
+	case V4L2_CID_MPEG_VIDEO_B_FRAMES:
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_B_FRAMES val = %d",
+			       ctrl->val);
+		p->num_b_frame = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE:
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE val = %d",
+			       ctrl->val);
+		p->rc_frame = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_MAX_QP:
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_H264_MAX_QP val = %d",
+			       ctrl->val);
+		p->h264_max_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_HEADER_MODE:
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_HEADER_MODE val = %d",
+			       ctrl->val);
+		p->seq_hdr_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE:
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE val = %d",
+			       ctrl->val);
+		p->rc_mb = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_H264_PROFILE val = %d",
+			       ctrl->val);
+		p->h264_profile = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_H264_LEVEL val = %d",
+			       ctrl->val);
+		p->h264_level = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_I_PERIOD:
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_H264_I_PERIOD val = %d",
+			       ctrl->val);
+		p->intra_period = ctrl->val;
+		ctx->param_change |= MTK_ENCODE_PARAM_INTRA_PERIOD;
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_GOP_SIZE val = %d",
+			       ctrl->val);
+		p->gop_size = ctrl->val;
+		ctx->param_change |= MTK_ENCODE_PARAM_GOP_SIZE;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VP8_PROFILE:
+		/*
+		 * FIXME - what vp8 profiles are actually supported?
+		 * The ctrl is added (with only profile 0 supported) for now.
+		 */
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_VP8_PROFILE val = %d", ctrl->val);
+		break;
+	case V4L2_CID_MPEG_VIDEO_FORCE_KEY_FRAME:
+		mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_FORCE_KEY_FRAME");
+		p->force_intra = 1;
+		ctx->param_change |= MTK_ENCODE_PARAM_FORCE_INTRA;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops mtk_vcodec_enc_ctrl_ops = {
+	.s_ctrl = vidioc_venc_s_ctrl,
+};
+
+static int vidioc_enum_fmt(struct v4l2_fmtdesc *f,
+			   const struct mtk_video_fmt *formats,
+			   size_t num_formats)
+{
+	if (f->index >= num_formats)
+		return -EINVAL;
+
+	f->pixelformat = formats[f->index].fourcc;
+
+	return 0;
+}
+
+static const struct mtk_video_fmt *
+mtk_venc_find_format(u32 fourcc, const struct mtk_vcodec_enc_pdata *pdata)
+{
+	const struct mtk_video_fmt *fmt;
+	unsigned int k;
+
+	for (k = 0; k < pdata->num_capture_formats; k++) {
+		fmt = &pdata->capture_formats[k];
+		if (fmt->fourcc == fourcc)
+			return fmt;
+	}
+
+	for (k = 0; k < pdata->num_output_formats; k++) {
+		fmt = &pdata->output_formats[k];
+		if (fmt->fourcc == fourcc)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+static int vidioc_enum_framesizes(struct file *file, void *fh,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	const struct mtk_video_fmt *fmt;
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(fh);
+
+	if (fsize->index != 0)
+		return -EINVAL;
+
+	fmt = mtk_venc_find_format(fsize->pixel_format,
+				   ctx->dev->venc_pdata);
+	if (!fmt)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+
+	if (ctx->dev->enc_capability & MTK_VENC_4K_CAPABILITY_ENABLE)
+		fsize->stepwise = mtk_venc_4k_framesizes;
+	else
+		fsize->stepwise = mtk_venc_hd_framesizes;
+
+	return 0;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	const struct mtk_vcodec_enc_pdata *pdata =
+		fh_to_ctx(priv)->dev->venc_pdata;
+
+	return vidioc_enum_fmt(f, pdata->capture_formats,
+			       pdata->num_capture_formats);
+}
+
+static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	const struct mtk_vcodec_enc_pdata *pdata =
+		fh_to_ctx(priv)->dev->venc_pdata;
+
+	return vidioc_enum_fmt(f, pdata->output_formats,
+			       pdata->num_output_formats);
+}
+
+static int mtk_vcodec_enc_get_chip_name(void *priv)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct device *dev = &ctx->dev->plat_dev->dev;
+
+	if (of_device_is_compatible(dev->of_node, "mediatek,mt8173-vcodec-enc"))
+		return 8173;
+	else if (of_device_is_compatible(dev->of_node, "mediatek,mt8183-vcodec-enc"))
+		return 8183;
+	else if (of_device_is_compatible(dev->of_node, "mediatek,mt8192-vcodec-enc"))
+		return 8192;
+	else if (of_device_is_compatible(dev->of_node, "mediatek,mt8195-vcodec-enc"))
+		return 8195;
+	else if (of_device_is_compatible(dev->of_node, "mediatek,mt8188-vcodec-enc"))
+		return 8188;
+	else
+		return 8173;
+}
+
+static int vidioc_venc_querycap(struct file *file, void *priv,
+				struct v4l2_capability *cap)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct device *dev = &ctx->dev->plat_dev->dev;
+	int platform_name = mtk_vcodec_enc_get_chip_name(priv);
+
+	strscpy(cap->driver, dev->driver->name, sizeof(cap->driver));
+	snprintf(cap->card, sizeof(cap->card), "MT%d video encoder", platform_name);
+
+	return 0;
+}
+
+static int vidioc_venc_s_parm(struct file *file, void *priv,
+			      struct v4l2_streamparm *a)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct v4l2_fract *timeperframe = &a->parm.output.timeperframe;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return -EINVAL;
+
+	if (timeperframe->numerator == 0 || timeperframe->denominator == 0) {
+		timeperframe->numerator = MTK_DEFAULT_FRAMERATE_NUM;
+		timeperframe->denominator = MTK_DEFAULT_FRAMERATE_DENOM;
+	}
+
+	ctx->enc_params.framerate_num = timeperframe->denominator;
+	ctx->enc_params.framerate_denom = timeperframe->numerator;
+	ctx->param_change |= MTK_ENCODE_PARAM_FRAMERATE;
+
+	a->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
+
+	return 0;
+}
+
+static int vidioc_venc_g_parm(struct file *file, void *priv,
+			      struct v4l2_streamparm *a)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return -EINVAL;
+
+	a->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
+	a->parm.output.timeperframe.denominator =
+			ctx->enc_params.framerate_num;
+	a->parm.output.timeperframe.numerator =
+			ctx->enc_params.framerate_denom;
+
+	return 0;
+}
+
+static struct mtk_q_data *mtk_venc_get_q_data(struct mtk_vcodec_ctx *ctx,
+					      enum v4l2_buf_type type)
+{
+	if (V4L2_TYPE_IS_OUTPUT(type))
+		return &ctx->q_data[MTK_Q_DATA_SRC];
+
+	return &ctx->q_data[MTK_Q_DATA_DST];
+}
+
+static void vidioc_try_fmt_cap(struct v4l2_format *f)
+{
+	f->fmt.pix_mp.field = V4L2_FIELD_NONE;
+	f->fmt.pix_mp.num_planes = 1;
+	f->fmt.pix_mp.plane_fmt[0].bytesperline = 0;
+	f->fmt.pix_mp.flags = 0;
+}
+
+/* V4L2 specification suggests the driver corrects the format struct if any of
+ * the dimensions is unsupported
+ */
+static int vidioc_try_fmt_out(struct mtk_vcodec_ctx *ctx, struct v4l2_format *f,
+			      const struct mtk_video_fmt *fmt)
+{
+	struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
+	int tmp_w, tmp_h;
+	unsigned int max_width, max_height;
+
+	pix_fmt_mp->field = V4L2_FIELD_NONE;
+
+	if (ctx->dev->enc_capability & MTK_VENC_4K_CAPABILITY_ENABLE) {
+		max_width = MTK_VENC_4K_MAX_W;
+		max_height = MTK_VENC_4K_MAX_H;
+	} else {
+		max_width = MTK_VENC_HD_MAX_W;
+		max_height = MTK_VENC_HD_MAX_H;
+	}
+
+	pix_fmt_mp->height = clamp(pix_fmt_mp->height, MTK_VENC_MIN_H, max_height);
+	pix_fmt_mp->width = clamp(pix_fmt_mp->width, MTK_VENC_MIN_W, max_width);
+
+	/* find next closer width align 16, heign align 32, size align
+	 * 64 rectangle
+	 */
+	tmp_w = pix_fmt_mp->width;
+	tmp_h = pix_fmt_mp->height;
+	v4l_bound_align_image(&pix_fmt_mp->width,
+			      MTK_VENC_MIN_W,
+			      max_width, 4,
+			      &pix_fmt_mp->height,
+			      MTK_VENC_MIN_H,
+			      max_height, 5, 6);
+
+	if (pix_fmt_mp->width < tmp_w && (pix_fmt_mp->width + 16) <= max_width)
+		pix_fmt_mp->width += 16;
+	if (pix_fmt_mp->height < tmp_h && (pix_fmt_mp->height + 32) <= max_height)
+		pix_fmt_mp->height += 32;
+
+	mtk_v4l2_debug(0, "before resize w=%d, h=%d, after resize w=%d, h=%d, sizeimage=%d %d",
+		       tmp_w, tmp_h, pix_fmt_mp->width,
+		       pix_fmt_mp->height,
+		       pix_fmt_mp->plane_fmt[0].sizeimage,
+		       pix_fmt_mp->plane_fmt[1].sizeimage);
+
+	pix_fmt_mp->num_planes = fmt->num_planes;
+	pix_fmt_mp->plane_fmt[0].sizeimage =
+			pix_fmt_mp->width * pix_fmt_mp->height +
+			((ALIGN(pix_fmt_mp->width, 16) * 2) * 16);
+	pix_fmt_mp->plane_fmt[0].bytesperline = pix_fmt_mp->width;
+
+	if (pix_fmt_mp->num_planes == 2) {
+		pix_fmt_mp->plane_fmt[1].sizeimage =
+			(pix_fmt_mp->width * pix_fmt_mp->height) / 2 +
+			(ALIGN(pix_fmt_mp->width, 16) * 16);
+		pix_fmt_mp->plane_fmt[2].sizeimage = 0;
+		pix_fmt_mp->plane_fmt[1].bytesperline =
+						pix_fmt_mp->width;
+		pix_fmt_mp->plane_fmt[2].bytesperline = 0;
+	} else if (pix_fmt_mp->num_planes == 3) {
+		pix_fmt_mp->plane_fmt[1].sizeimage =
+		pix_fmt_mp->plane_fmt[2].sizeimage =
+			(pix_fmt_mp->width * pix_fmt_mp->height) / 4 +
+			((ALIGN(pix_fmt_mp->width, 16) / 2) * 16);
+		pix_fmt_mp->plane_fmt[1].bytesperline =
+			pix_fmt_mp->plane_fmt[2].bytesperline =
+			pix_fmt_mp->width / 2;
+	}
+
+	pix_fmt_mp->flags = 0;
+
+	return 0;
+}
+
+static void mtk_venc_set_param(struct mtk_vcodec_ctx *ctx,
+				struct venc_enc_param *param)
+{
+	struct mtk_q_data *q_data_src = &ctx->q_data[MTK_Q_DATA_SRC];
+	struct mtk_enc_params *enc_params = &ctx->enc_params;
+
+	switch (q_data_src->fmt->fourcc) {
+	case V4L2_PIX_FMT_YUV420M:
+		param->input_yuv_fmt = VENC_YUV_FORMAT_I420;
+		break;
+	case V4L2_PIX_FMT_YVU420M:
+		param->input_yuv_fmt = VENC_YUV_FORMAT_YV12;
+		break;
+	case V4L2_PIX_FMT_NV12M:
+		param->input_yuv_fmt = VENC_YUV_FORMAT_NV12;
+		break;
+	case V4L2_PIX_FMT_NV21M:
+		param->input_yuv_fmt = VENC_YUV_FORMAT_NV21;
+		break;
+	default:
+		mtk_v4l2_err("Unsupported fourcc =%d", q_data_src->fmt->fourcc);
+		break;
+	}
+	param->h264_profile = enc_params->h264_profile;
+	param->h264_level = enc_params->h264_level;
+
+	/* Config visible resolution */
+	param->width = q_data_src->visible_width;
+	param->height = q_data_src->visible_height;
+	/* Config coded resolution */
+	param->buf_width = q_data_src->coded_width;
+	param->buf_height = q_data_src->coded_height;
+	param->frm_rate = enc_params->framerate_num /
+			enc_params->framerate_denom;
+	param->intra_period = enc_params->intra_period;
+	param->gop_size = enc_params->gop_size;
+	param->bitrate = enc_params->bitrate;
+
+	mtk_v4l2_debug(0,
+		"fmt 0x%x, P/L %d/%d, w/h %d/%d, buf %d/%d, fps/bps %d/%d, gop %d, i_period %d",
+		param->input_yuv_fmt, param->h264_profile,
+		param->h264_level, param->width, param->height,
+		param->buf_width, param->buf_height,
+		param->frm_rate, param->bitrate,
+		param->gop_size, param->intra_period);
+}
+
+static int vidioc_venc_s_fmt_cap(struct file *file, void *priv,
+			     struct v4l2_format *f)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	const struct mtk_vcodec_enc_pdata *pdata = ctx->dev->venc_pdata;
+	struct vb2_queue *vq;
+	struct mtk_q_data *q_data = mtk_venc_get_q_data(ctx, f->type);
+	int i, ret;
+	const struct mtk_video_fmt *fmt;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	if (!vq) {
+		mtk_v4l2_err("fail to get vq");
+		return -EINVAL;
+	}
+
+	if (vb2_is_busy(vq)) {
+		mtk_v4l2_err("queue busy");
+		return -EBUSY;
+	}
+
+	fmt = mtk_venc_find_format(f->fmt.pix.pixelformat, pdata);
+	if (!fmt) {
+		fmt = &ctx->dev->venc_pdata->capture_formats[0];
+		f->fmt.pix.pixelformat = fmt->fourcc;
+	}
+
+	q_data->fmt = fmt;
+	vidioc_try_fmt_cap(f);
+
+	q_data->coded_width = f->fmt.pix_mp.width;
+	q_data->coded_height = f->fmt.pix_mp.height;
+	q_data->field = f->fmt.pix_mp.field;
+
+	for (i = 0; i < f->fmt.pix_mp.num_planes; i++) {
+		struct v4l2_plane_pix_format	*plane_fmt;
+
+		plane_fmt = &f->fmt.pix_mp.plane_fmt[i];
+		q_data->bytesperline[i]	= plane_fmt->bytesperline;
+		q_data->sizeimage[i] = plane_fmt->sizeimage;
+	}
+
+	if (ctx->state == MTK_STATE_FREE) {
+		ret = venc_if_init(ctx, q_data->fmt->fourcc);
+		if (ret) {
+			mtk_v4l2_err("venc_if_init failed=%d, codec type=%x",
+					ret, q_data->fmt->fourcc);
+			return -EBUSY;
+		}
+		ctx->state = MTK_STATE_INIT;
+	}
+
+	return 0;
+}
+
+static int vidioc_venc_s_fmt_out(struct file *file, void *priv,
+			     struct v4l2_format *f)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	const struct mtk_vcodec_enc_pdata *pdata = ctx->dev->venc_pdata;
+	struct vb2_queue *vq;
+	struct mtk_q_data *q_data = mtk_venc_get_q_data(ctx, f->type);
+	int ret, i;
+	const struct mtk_video_fmt *fmt;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	if (!vq) {
+		mtk_v4l2_err("fail to get vq");
+		return -EINVAL;
+	}
+
+	if (vb2_is_busy(vq)) {
+		mtk_v4l2_err("queue busy");
+		return -EBUSY;
+	}
+
+	fmt = mtk_venc_find_format(f->fmt.pix.pixelformat, pdata);
+	if (!fmt) {
+		fmt = &ctx->dev->venc_pdata->output_formats[0];
+		f->fmt.pix.pixelformat = fmt->fourcc;
+	}
+
+	q_data->visible_width = f->fmt.pix_mp.width;
+	q_data->visible_height = f->fmt.pix_mp.height;
+	q_data->fmt = fmt;
+	ret = vidioc_try_fmt_out(ctx, f, q_data->fmt);
+	if (ret)
+		return ret;
+
+	q_data->coded_width = f->fmt.pix_mp.width;
+	q_data->coded_height = f->fmt.pix_mp.height;
+
+	q_data->field = f->fmt.pix_mp.field;
+	ctx->colorspace = f->fmt.pix_mp.colorspace;
+	ctx->ycbcr_enc = f->fmt.pix_mp.ycbcr_enc;
+	ctx->quantization = f->fmt.pix_mp.quantization;
+	ctx->xfer_func = f->fmt.pix_mp.xfer_func;
+
+	for (i = 0; i < f->fmt.pix_mp.num_planes; i++) {
+		struct v4l2_plane_pix_format *plane_fmt;
+
+		plane_fmt = &f->fmt.pix_mp.plane_fmt[i];
+		q_data->bytesperline[i] = plane_fmt->bytesperline;
+		q_data->sizeimage[i] = plane_fmt->sizeimage;
+	}
+
+	return 0;
+}
+
+static int vidioc_venc_g_fmt(struct file *file, void *priv,
+			     struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct vb2_queue *vq;
+	struct mtk_q_data *q_data = mtk_venc_get_q_data(ctx, f->type);
+	int i;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+
+	pix->width = q_data->coded_width;
+	pix->height = q_data->coded_height;
+	pix->pixelformat = q_data->fmt->fourcc;
+	pix->field = q_data->field;
+	pix->num_planes = q_data->fmt->num_planes;
+	for (i = 0; i < pix->num_planes; i++) {
+		pix->plane_fmt[i].bytesperline = q_data->bytesperline[i];
+		pix->plane_fmt[i].sizeimage = q_data->sizeimage[i];
+	}
+
+	pix->flags = 0;
+	pix->colorspace = ctx->colorspace;
+	pix->ycbcr_enc = ctx->ycbcr_enc;
+	pix->quantization = ctx->quantization;
+	pix->xfer_func = ctx->xfer_func;
+
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_cap_mplane(struct file *file, void *priv,
+					 struct v4l2_format *f)
+{
+	const struct mtk_video_fmt *fmt;
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	const struct mtk_vcodec_enc_pdata *pdata = ctx->dev->venc_pdata;
+
+	fmt = mtk_venc_find_format(f->fmt.pix.pixelformat, pdata);
+	if (!fmt) {
+		fmt = &ctx->dev->venc_pdata->capture_formats[0];
+		f->fmt.pix.pixelformat = fmt->fourcc;
+	}
+	f->fmt.pix_mp.colorspace = ctx->colorspace;
+	f->fmt.pix_mp.ycbcr_enc = ctx->ycbcr_enc;
+	f->fmt.pix_mp.quantization = ctx->quantization;
+	f->fmt.pix_mp.xfer_func = ctx->xfer_func;
+
+	vidioc_try_fmt_cap(f);
+
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_out_mplane(struct file *file, void *priv,
+					 struct v4l2_format *f)
+{
+	const struct mtk_video_fmt *fmt;
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	const struct mtk_vcodec_enc_pdata *pdata = ctx->dev->venc_pdata;
+
+	fmt = mtk_venc_find_format(f->fmt.pix.pixelformat, pdata);
+	if (!fmt) {
+		fmt = &ctx->dev->venc_pdata->output_formats[0];
+		f->fmt.pix.pixelformat = fmt->fourcc;
+	}
+	if (!f->fmt.pix_mp.colorspace) {
+		f->fmt.pix_mp.colorspace = V4L2_COLORSPACE_REC709;
+		f->fmt.pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+		f->fmt.pix_mp.quantization = V4L2_QUANTIZATION_DEFAULT;
+		f->fmt.pix_mp.xfer_func = V4L2_XFER_FUNC_DEFAULT;
+	}
+
+	return vidioc_try_fmt_out(ctx, f, fmt);
+}
+
+static int vidioc_venc_g_selection(struct file *file, void *priv,
+				     struct v4l2_selection *s)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct mtk_q_data *q_data = mtk_venc_get_q_data(ctx, s->type);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		s->r.top = 0;
+		s->r.left = 0;
+		s->r.width = q_data->coded_width;
+		s->r.height = q_data->coded_height;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		s->r.top = 0;
+		s->r.left = 0;
+		s->r.width = q_data->visible_width;
+		s->r.height = q_data->visible_height;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vidioc_venc_s_selection(struct file *file, void *priv,
+				     struct v4l2_selection *s)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct mtk_q_data *q_data = mtk_venc_get_q_data(ctx, s->type);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		/* Only support crop from (0,0) */
+		s->r.top = 0;
+		s->r.left = 0;
+		s->r.width = min(s->r.width, q_data->coded_width);
+		s->r.height = min(s->r.height, q_data->coded_height);
+		q_data->visible_width = s->r.width;
+		q_data->visible_height = s->r.height;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int vidioc_venc_qbuf(struct file *file, void *priv,
+			    struct v4l2_buffer *buf)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+
+	if (ctx->state == MTK_STATE_ABORT) {
+		mtk_v4l2_err("[%d] Call on QBUF after unrecoverable error",
+				ctx->id);
+		return -EIO;
+	}
+
+	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int vidioc_venc_dqbuf(struct file *file, void *priv,
+			     struct v4l2_buffer *buf)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	int ret;
+
+	if (ctx->state == MTK_STATE_ABORT) {
+		mtk_v4l2_err("[%d] Call on QBUF after unrecoverable error",
+				ctx->id);
+		return -EIO;
+	}
+
+	ret = v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
+	if (ret)
+		return ret;
+
+	/*
+	 * Complete flush if the user dequeued the 0-payload LAST buffer.
+	 * We check the payload because a buffer with the LAST flag can also
+	 * be seen during resolution changes. If we happen to be flushing at
+	 * that time, the last buffer before the resolution changes could be
+	 * misinterpreted for the buffer generated by the flush and terminate
+	 * it earlier than we want.
+	 */
+	if (!V4L2_TYPE_IS_OUTPUT(buf->type) &&
+	    buf->flags & V4L2_BUF_FLAG_LAST &&
+	    buf->m.planes[0].bytesused == 0 &&
+	    ctx->is_flushing) {
+		/*
+		 * Last CAPTURE buffer is dequeued, we can allow another flush
+		 * to take place.
+		 */
+		ctx->is_flushing = false;
+	}
+
+	return 0;
+}
+
+static int vidioc_encoder_cmd(struct file *file, void *priv,
+			      struct v4l2_encoder_cmd *cmd)
+{
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
+	struct vb2_queue *src_vq, *dst_vq;
+	int ret;
+
+	if (ctx->state == MTK_STATE_ABORT) {
+		mtk_v4l2_err("[%d] Call to CMD after unrecoverable error",
+			     ctx->id);
+		return -EIO;
+	}
+
+	ret = v4l2_m2m_ioctl_try_encoder_cmd(file, priv, cmd);
+	if (ret)
+		return ret;
+
+	/* Calling START or STOP is invalid if a flush is in progress */
+	if (ctx->is_flushing)
+		return -EBUSY;
+
+	mtk_v4l2_debug(1, "encoder cmd=%u", cmd->cmd);
+
+	dst_vq = v4l2_m2m_get_vq(ctx->m2m_ctx,
+				 V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	switch (cmd->cmd) {
+	case V4L2_ENC_CMD_STOP:
+		src_vq = v4l2_m2m_get_vq(ctx->m2m_ctx,
+					 V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+		if (!vb2_is_streaming(src_vq)) {
+			mtk_v4l2_debug(1, "Output stream is off. No need to flush.");
+			return 0;
+		}
+		if (!vb2_is_streaming(dst_vq)) {
+			mtk_v4l2_debug(1, "Capture stream is off. No need to flush.");
+			return 0;
+		}
+		ctx->is_flushing = true;
+		v4l2_m2m_buf_queue(ctx->m2m_ctx, &ctx->empty_flush_buf.vb);
+		v4l2_m2m_try_schedule(ctx->m2m_ctx);
+		break;
+
+	case V4L2_ENC_CMD_START:
+		vb2_clear_last_buffer_dequeued(dst_vq);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+const struct v4l2_ioctl_ops mtk_venc_ioctl_ops = {
+	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
+
+	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf			= vidioc_venc_qbuf,
+	.vidioc_dqbuf			= vidioc_venc_dqbuf,
+
+	.vidioc_querycap		= vidioc_venc_querycap,
+	.vidioc_enum_fmt_vid_cap	= vidioc_enum_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_out	= vidioc_enum_fmt_vid_out,
+	.vidioc_enum_framesizes		= vidioc_enum_framesizes,
+
+	.vidioc_try_fmt_vid_cap_mplane	= vidioc_try_fmt_vid_cap_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= vidioc_try_fmt_vid_out_mplane,
+	.vidioc_expbuf			= v4l2_m2m_ioctl_expbuf,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+
+	.vidioc_s_parm			= vidioc_venc_s_parm,
+	.vidioc_g_parm			= vidioc_venc_g_parm,
+	.vidioc_s_fmt_vid_cap_mplane	= vidioc_venc_s_fmt_cap,
+	.vidioc_s_fmt_vid_out_mplane	= vidioc_venc_s_fmt_out,
+
+	.vidioc_g_fmt_vid_cap_mplane	= vidioc_venc_g_fmt,
+	.vidioc_g_fmt_vid_out_mplane	= vidioc_venc_g_fmt,
+
+	.vidioc_create_bufs		= v4l2_m2m_ioctl_create_bufs,
+	.vidioc_prepare_buf		= v4l2_m2m_ioctl_prepare_buf,
+
+	.vidioc_g_selection		= vidioc_venc_g_selection,
+	.vidioc_s_selection		= vidioc_venc_s_selection,
+
+	.vidioc_encoder_cmd		= vidioc_encoder_cmd,
+	.vidioc_try_encoder_cmd		= v4l2_m2m_ioctl_try_encoder_cmd,
+};
+
+static int vb2ops_venc_queue_setup(struct vb2_queue *vq,
+				   unsigned int *nbuffers,
+				   unsigned int *nplanes,
+				   unsigned int sizes[],
+				   struct device *alloc_devs[])
+{
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vq);
+	struct mtk_q_data *q_data = mtk_venc_get_q_data(ctx, vq->type);
+	unsigned int i;
+
+	if (q_data == NULL)
+		return -EINVAL;
+
+	if (*nplanes) {
+		if (*nplanes != q_data->fmt->num_planes)
+			return -EINVAL;
+		for (i = 0; i < *nplanes; i++)
+			if (sizes[i] < q_data->sizeimage[i])
+				return -EINVAL;
+	} else {
+		*nplanes = q_data->fmt->num_planes;
+		for (i = 0; i < *nplanes; i++)
+			sizes[i] = q_data->sizeimage[i];
+	}
+
+	return 0;
+}
+
+static int vb2ops_venc_buf_prepare(struct vb2_buffer *vb)
+{
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct mtk_q_data *q_data = mtk_venc_get_q_data(ctx, vb->vb2_queue->type);
+	int i;
+
+	for (i = 0; i < q_data->fmt->num_planes; i++) {
+		if (vb2_plane_size(vb, i) < q_data->sizeimage[i]) {
+			mtk_v4l2_err("data will not fit into plane %d (%lu < %d)",
+				i, vb2_plane_size(vb, i),
+				q_data->sizeimage[i]);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static void vb2ops_venc_buf_queue(struct vb2_buffer *vb)
+{
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vb2_v4l2 =
+			container_of(vb, struct vb2_v4l2_buffer, vb2_buf);
+
+	struct mtk_video_enc_buf *mtk_buf =
+			container_of(vb2_v4l2, struct mtk_video_enc_buf,
+				     m2m_buf.vb);
+
+	if ((vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) &&
+	    (ctx->param_change != MTK_ENCODE_PARAM_NONE)) {
+		mtk_v4l2_debug(1, "[%d] Before id=%d encode parameter change %x",
+			       ctx->id,
+			       vb2_v4l2->vb2_buf.index,
+			       ctx->param_change);
+		mtk_buf->param_change = ctx->param_change;
+		mtk_buf->enc_params = ctx->enc_params;
+		ctx->param_change = MTK_ENCODE_PARAM_NONE;
+	}
+
+	v4l2_m2m_buf_queue(ctx->m2m_ctx, to_vb2_v4l2_buffer(vb));
+}
+
+static int vb2ops_venc_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(q);
+	struct venc_enc_param param;
+	int ret, pm_ret;
+	int i;
+
+	/* Once state turn into MTK_STATE_ABORT, we need stop_streaming
+	  * to clear it
+	  */
+	if ((ctx->state == MTK_STATE_ABORT) || (ctx->state == MTK_STATE_FREE)) {
+		ret = -EIO;
+		goto err_start_stream;
+	}
+
+	/* Do the initialization when both start_streaming have been called */
+	if (V4L2_TYPE_IS_OUTPUT(q->type)) {
+		if (!vb2_start_streaming_called(&ctx->m2m_ctx->cap_q_ctx.q))
+			return 0;
+	} else {
+		if (!vb2_start_streaming_called(&ctx->m2m_ctx->out_q_ctx.q))
+			return 0;
+	}
+
+	ret = pm_runtime_resume_and_get(&ctx->dev->plat_dev->dev);
+	if (ret < 0) {
+		mtk_v4l2_err("pm_runtime_resume_and_get fail %d", ret);
+		goto err_start_stream;
+	}
+
+	mtk_venc_set_param(ctx, &param);
+	ret = venc_if_set_param(ctx, VENC_SET_PARAM_ENC, &param);
+	if (ret) {
+		mtk_v4l2_err("venc_if_set_param failed=%d", ret);
+		ctx->state = MTK_STATE_ABORT;
+		goto err_set_param;
+	}
+	ctx->param_change = MTK_ENCODE_PARAM_NONE;
+
+	if ((ctx->q_data[MTK_Q_DATA_DST].fmt->fourcc == V4L2_PIX_FMT_H264) &&
+	    (ctx->enc_params.seq_hdr_mode !=
+				V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE)) {
+		ret = venc_if_set_param(ctx,
+					VENC_SET_PARAM_PREPEND_HEADER,
+					NULL);
+		if (ret) {
+			mtk_v4l2_err("venc_if_set_param failed=%d", ret);
+			ctx->state = MTK_STATE_ABORT;
+			goto err_set_param;
+		}
+		ctx->state = MTK_STATE_HEADER;
+	}
+
+	return 0;
+
+err_set_param:
+	pm_ret = pm_runtime_put(&ctx->dev->plat_dev->dev);
+	if (pm_ret < 0)
+		mtk_v4l2_err("pm_runtime_put fail %d", pm_ret);
+
+err_start_stream:
+	for (i = 0; i < q->num_buffers; ++i) {
+		struct vb2_buffer *buf = vb2_get_buffer(q, i);
+
+		/*
+		 * FIXME: This check is not needed as only active buffers
+		 * can be marked as done.
+		 */
+		if (buf->state == VB2_BUF_STATE_ACTIVE) {
+			mtk_v4l2_debug(0, "[%d] id=%d, type=%d, %d -> VB2_BUF_STATE_QUEUED",
+					ctx->id, i, q->type,
+					(int)buf->state);
+			v4l2_m2m_buf_done(to_vb2_v4l2_buffer(buf),
+					  VB2_BUF_STATE_QUEUED);
+		}
+	}
+
+	return ret;
+}
+
+static void vb2ops_venc_stop_streaming(struct vb2_queue *q)
+{
+	struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(q);
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	int ret;
+
+	mtk_v4l2_debug(2, "[%d]-> type=%d", ctx->id, q->type);
+
+	if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		while ((dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx))) {
+			vb2_set_plane_payload(&dst_buf->vb2_buf, 0, 0);
+			v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
+		}
+		/* STREAMOFF on the CAPTURE queue completes any ongoing flush */
+		if (ctx->is_flushing) {
+			struct v4l2_m2m_buffer *b, *n;
+
+			mtk_v4l2_debug(1, "STREAMOFF called while flushing");
+			/*
+			 * STREAMOFF could be called before the flush buffer is
+			 * dequeued. Check whether empty flush buf is still in
+			 * queue before removing it.
+			 */
+			v4l2_m2m_for_each_src_buf_safe(ctx->m2m_ctx, b, n) {
+				if (b == &ctx->empty_flush_buf) {
+					v4l2_m2m_src_buf_remove_by_buf(ctx->m2m_ctx, &b->vb);
+					break;
+				}
+			}
+			ctx->is_flushing = false;
+		}
+	} else {
+		while ((src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx))) {
+			if (src_buf != &ctx->empty_flush_buf.vb)
+				v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
+		}
+		if (ctx->is_flushing) {
+			/*
+			 * If we are in the middle of a flush, put the flush
+			 * buffer back into the queue so the next CAPTURE
+			 * buffer gets returned with the LAST flag set.
+			 */
+			v4l2_m2m_buf_queue(ctx->m2m_ctx,
+					   &ctx->empty_flush_buf.vb);
+		}
+	}
+
+	if ((q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+	     vb2_is_streaming(&ctx->m2m_ctx->out_q_ctx.q)) ||
+	    (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
+	     vb2_is_streaming(&ctx->m2m_ctx->cap_q_ctx.q))) {
+		mtk_v4l2_debug(1, "[%d]-> q type %d out=%d cap=%d",
+			       ctx->id, q->type,
+			       vb2_is_streaming(&ctx->m2m_ctx->out_q_ctx.q),
+			       vb2_is_streaming(&ctx->m2m_ctx->cap_q_ctx.q));
+		return;
+	}
+
+	/* Release the encoder if both streams are stopped. */
+	ret = venc_if_deinit(ctx);
+	if (ret)
+		mtk_v4l2_err("venc_if_deinit failed=%d", ret);
+
+	ret = pm_runtime_put(&ctx->dev->plat_dev->dev);
+	if (ret < 0)
+		mtk_v4l2_err("pm_runtime_put fail %d", ret);
+
+	ctx->state = MTK_STATE_FREE;
+}
+
+static int vb2ops_venc_buf_out_validate(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	vbuf->field = V4L2_FIELD_NONE;
+	return 0;
+}
+
+static const struct vb2_ops mtk_venc_vb2_ops = {
+	.queue_setup		= vb2ops_venc_queue_setup,
+	.buf_out_validate	= vb2ops_venc_buf_out_validate,
+	.buf_prepare		= vb2ops_venc_buf_prepare,
+	.buf_queue		= vb2ops_venc_buf_queue,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+	.start_streaming	= vb2ops_venc_start_streaming,
+	.stop_streaming		= vb2ops_venc_stop_streaming,
+};
+
+static int mtk_venc_encode_header(void *priv)
+{
+	struct mtk_vcodec_ctx *ctx = priv;
+	int ret;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	struct mtk_vcodec_mem bs_buf;
+	struct venc_done_result enc_result;
+
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+	if (!dst_buf) {
+		mtk_v4l2_debug(1, "No dst buffer");
+		return -EINVAL;
+	}
+
+	bs_buf.va = vb2_plane_vaddr(&dst_buf->vb2_buf, 0);
+	bs_buf.dma_addr = vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
+	bs_buf.size = (size_t)dst_buf->vb2_buf.planes[0].length;
+
+	mtk_v4l2_debug(1,
+			"[%d] buf id=%d va=0x%p dma_addr=0x%llx size=%zu",
+			ctx->id,
+			dst_buf->vb2_buf.index, bs_buf.va,
+			(u64)bs_buf.dma_addr,
+			bs_buf.size);
+
+	ret = venc_if_encode(ctx,
+			VENC_START_OPT_ENCODE_SEQUENCE_HEADER,
+			NULL, &bs_buf, &enc_result);
+
+	if (ret) {
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, 0);
+		ctx->state = MTK_STATE_ABORT;
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
+		mtk_v4l2_err("venc_if_encode failed=%d", ret);
+		return -EINVAL;
+	}
+	src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	if (src_buf) {
+		dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp;
+		dst_buf->timecode = src_buf->timecode;
+	} else {
+		mtk_v4l2_err("No timestamp for the header buffer.");
+	}
+
+	ctx->state = MTK_STATE_HEADER;
+	vb2_set_plane_payload(&dst_buf->vb2_buf, 0, enc_result.bs_size);
+	v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
+
+	return 0;
+}
+
+static int mtk_venc_param_change(struct mtk_vcodec_ctx *ctx)
+{
+	struct venc_enc_param enc_prm;
+	struct vb2_v4l2_buffer *vb2_v4l2 = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	struct mtk_video_enc_buf *mtk_buf;
+	int ret = 0;
+
+	/* Don't upcast the empty flush buffer */
+	if (vb2_v4l2 == &ctx->empty_flush_buf.vb)
+		return 0;
+
+	mtk_buf = container_of(vb2_v4l2, struct mtk_video_enc_buf, m2m_buf.vb);
+
+	memset(&enc_prm, 0, sizeof(enc_prm));
+	if (mtk_buf->param_change == MTK_ENCODE_PARAM_NONE)
+		return 0;
+
+	if (mtk_buf->param_change & MTK_ENCODE_PARAM_BITRATE) {
+		enc_prm.bitrate = mtk_buf->enc_params.bitrate;
+		mtk_v4l2_debug(1, "[%d] id=%d, change param br=%d",
+				ctx->id,
+				vb2_v4l2->vb2_buf.index,
+				enc_prm.bitrate);
+		ret |= venc_if_set_param(ctx,
+					 VENC_SET_PARAM_ADJUST_BITRATE,
+					 &enc_prm);
+	}
+	if (!ret && mtk_buf->param_change & MTK_ENCODE_PARAM_FRAMERATE) {
+		enc_prm.frm_rate = mtk_buf->enc_params.framerate_num /
+				   mtk_buf->enc_params.framerate_denom;
+		mtk_v4l2_debug(1, "[%d] id=%d, change param fr=%d",
+			       ctx->id,
+			       vb2_v4l2->vb2_buf.index,
+			       enc_prm.frm_rate);
+		ret |= venc_if_set_param(ctx,
+					 VENC_SET_PARAM_ADJUST_FRAMERATE,
+					 &enc_prm);
+	}
+	if (!ret && mtk_buf->param_change & MTK_ENCODE_PARAM_GOP_SIZE) {
+		enc_prm.gop_size = mtk_buf->enc_params.gop_size;
+		mtk_v4l2_debug(1, "change param intra period=%d",
+			       enc_prm.gop_size);
+		ret |= venc_if_set_param(ctx,
+					 VENC_SET_PARAM_GOP_SIZE,
+					 &enc_prm);
+	}
+	if (!ret && mtk_buf->param_change & MTK_ENCODE_PARAM_FORCE_INTRA) {
+		mtk_v4l2_debug(1, "[%d] id=%d, change param force I=%d",
+				ctx->id,
+				vb2_v4l2->vb2_buf.index,
+				mtk_buf->enc_params.force_intra);
+		if (mtk_buf->enc_params.force_intra)
+			ret |= venc_if_set_param(ctx,
+						 VENC_SET_PARAM_FORCE_INTRA,
+						 NULL);
+	}
+
+	mtk_buf->param_change = MTK_ENCODE_PARAM_NONE;
+
+	if (ret) {
+		ctx->state = MTK_STATE_ABORT;
+		mtk_v4l2_err("venc_if_set_param %d failed=%d",
+				mtk_buf->param_change, ret);
+		return -1;
+	}
+
+	return 0;
+}
+
+/*
+ * v4l2_m2m_streamoff() holds dev_mutex and waits mtk_venc_worker()
+ * to call v4l2_m2m_job_finish().
+ * If mtk_venc_worker() tries to acquire dev_mutex, it will deadlock.
+ * So this function must not try to acquire dev->dev_mutex.
+ * This means v4l2 ioctls and mtk_venc_worker() can run at the same time.
+ * mtk_venc_worker() should be carefully implemented to avoid bugs.
+ */
+static void mtk_venc_worker(struct work_struct *work)
+{
+	struct mtk_vcodec_ctx *ctx = container_of(work, struct mtk_vcodec_ctx,
+				    encode_work);
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	struct venc_frm_buf frm_buf;
+	struct mtk_vcodec_mem bs_buf;
+	struct venc_done_result enc_result;
+	int ret, i;
+
+	/* check dst_buf, dst_buf may be removed in device_run
+	 * to stored encdoe header so we need check dst_buf and
+	 * call job_finish here to prevent recursion
+	 */
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+	if (!dst_buf) {
+		v4l2_m2m_job_finish(ctx->dev->m2m_dev_enc, ctx->m2m_ctx);
+		return;
+	}
+
+	src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+
+	/*
+	 * If we see the flush buffer, send an empty buffer with the LAST flag
+	 * to the client. is_flushing will be reset at the time the buffer
+	 * is dequeued.
+	 */
+	if (src_buf == &ctx->empty_flush_buf.vb) {
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, 0);
+		dst_buf->flags |= V4L2_BUF_FLAG_LAST;
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
+		v4l2_m2m_job_finish(ctx->dev->m2m_dev_enc, ctx->m2m_ctx);
+		return;
+	}
+
+	memset(&frm_buf, 0, sizeof(frm_buf));
+	for (i = 0; i < src_buf->vb2_buf.num_planes ; i++) {
+		frm_buf.fb_addr[i].dma_addr =
+				vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, i);
+		frm_buf.fb_addr[i].size =
+				(size_t)src_buf->vb2_buf.planes[i].length;
+	}
+	bs_buf.va = vb2_plane_vaddr(&dst_buf->vb2_buf, 0);
+	bs_buf.dma_addr = vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
+	bs_buf.size = (size_t)dst_buf->vb2_buf.planes[0].length;
+
+	mtk_v4l2_debug(2,
+			"Framebuf PA=%llx Size=0x%zx;PA=0x%llx Size=0x%zx;PA=0x%llx Size=%zu",
+			(u64)frm_buf.fb_addr[0].dma_addr,
+			frm_buf.fb_addr[0].size,
+			(u64)frm_buf.fb_addr[1].dma_addr,
+			frm_buf.fb_addr[1].size,
+			(u64)frm_buf.fb_addr[2].dma_addr,
+			frm_buf.fb_addr[2].size);
+
+	ret = venc_if_encode(ctx, VENC_START_OPT_ENCODE_FRAME,
+			     &frm_buf, &bs_buf, &enc_result);
+
+	dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp;
+	dst_buf->timecode = src_buf->timecode;
+
+	if (enc_result.is_key_frm)
+		dst_buf->flags |= V4L2_BUF_FLAG_KEYFRAME;
+
+	if (ret) {
+		v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, 0);
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
+		mtk_v4l2_err("venc_if_encode failed=%d", ret);
+	} else {
+		v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, enc_result.bs_size);
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
+		mtk_v4l2_debug(2, "venc_if_encode bs size=%d",
+				 enc_result.bs_size);
+	}
+
+	v4l2_m2m_job_finish(ctx->dev->m2m_dev_enc, ctx->m2m_ctx);
+
+	mtk_v4l2_debug(1, "<=== src_buf[%d] dst_buf[%d] venc_if_encode ret=%d Size=%u===>",
+			src_buf->vb2_buf.index, dst_buf->vb2_buf.index, ret,
+			enc_result.bs_size);
+}
+
+static void m2mops_venc_device_run(void *priv)
+{
+	struct mtk_vcodec_ctx *ctx = priv;
+
+	if ((ctx->q_data[MTK_Q_DATA_DST].fmt->fourcc == V4L2_PIX_FMT_H264) &&
+	    (ctx->state != MTK_STATE_HEADER)) {
+		/* encode h264 sps/pps header */
+		mtk_venc_encode_header(ctx);
+		queue_work(ctx->dev->encode_workqueue, &ctx->encode_work);
+		return;
+	}
+
+	mtk_venc_param_change(ctx);
+	queue_work(ctx->dev->encode_workqueue, &ctx->encode_work);
+}
+
+static int m2mops_venc_job_ready(void *m2m_priv)
+{
+	struct mtk_vcodec_ctx *ctx = m2m_priv;
+
+	if (ctx->state == MTK_STATE_ABORT || ctx->state == MTK_STATE_FREE) {
+		mtk_v4l2_debug(3, "[%d]Not ready: state=0x%x.",
+			       ctx->id, ctx->state);
+		return 0;
+	}
+
+	return 1;
+}
+
+static void m2mops_venc_job_abort(void *priv)
+{
+	struct mtk_vcodec_ctx *ctx = priv;
+
+	ctx->state = MTK_STATE_ABORT;
+}
+
+const struct v4l2_m2m_ops mtk_venc_m2m_ops = {
+	.device_run	= m2mops_venc_device_run,
+	.job_ready	= m2mops_venc_job_ready,
+	.job_abort	= m2mops_venc_job_abort,
+};
+
+void mtk_vcodec_enc_set_default_params(struct mtk_vcodec_ctx *ctx)
+{
+	struct mtk_q_data *q_data;
+
+	ctx->m2m_ctx->q_lock = &ctx->q_mutex;
+	ctx->fh.m2m_ctx = ctx->m2m_ctx;
+	ctx->fh.ctrl_handler = &ctx->ctrl_hdl;
+	INIT_WORK(&ctx->encode_work, mtk_venc_worker);
+
+	ctx->colorspace = V4L2_COLORSPACE_REC709;
+	ctx->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	ctx->quantization = V4L2_QUANTIZATION_DEFAULT;
+	ctx->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+
+	q_data = &ctx->q_data[MTK_Q_DATA_SRC];
+	memset(q_data, 0, sizeof(struct mtk_q_data));
+	q_data->visible_width = DFT_CFG_WIDTH;
+	q_data->visible_height = DFT_CFG_HEIGHT;
+	q_data->coded_width = DFT_CFG_WIDTH;
+	q_data->coded_height = DFT_CFG_HEIGHT;
+	q_data->field = V4L2_FIELD_NONE;
+
+	q_data->fmt = &ctx->dev->venc_pdata->output_formats[0];
+
+	v4l_bound_align_image(&q_data->coded_width,
+				MTK_VENC_MIN_W,
+				MTK_VENC_HD_MAX_W, 4,
+				&q_data->coded_height,
+				MTK_VENC_MIN_H,
+				MTK_VENC_HD_MAX_H, 5, 6);
+
+	if (q_data->coded_width < DFT_CFG_WIDTH &&
+		(q_data->coded_width + 16) <= MTK_VENC_HD_MAX_W)
+		q_data->coded_width += 16;
+	if (q_data->coded_height < DFT_CFG_HEIGHT &&
+		(q_data->coded_height + 32) <= MTK_VENC_HD_MAX_H)
+		q_data->coded_height += 32;
+
+	q_data->sizeimage[0] =
+		q_data->coded_width * q_data->coded_height+
+		((ALIGN(q_data->coded_width, 16) * 2) * 16);
+	q_data->bytesperline[0] = q_data->coded_width;
+	q_data->sizeimage[1] =
+		(q_data->coded_width * q_data->coded_height) / 2 +
+		(ALIGN(q_data->coded_width, 16) * 16);
+	q_data->bytesperline[1] = q_data->coded_width;
+
+	q_data = &ctx->q_data[MTK_Q_DATA_DST];
+	memset(q_data, 0, sizeof(struct mtk_q_data));
+	q_data->coded_width = DFT_CFG_WIDTH;
+	q_data->coded_height = DFT_CFG_HEIGHT;
+	q_data->fmt = &ctx->dev->venc_pdata->capture_formats[0];
+	q_data->field = V4L2_FIELD_NONE;
+	ctx->q_data[MTK_Q_DATA_DST].sizeimage[0] =
+		DFT_CFG_WIDTH * DFT_CFG_HEIGHT;
+	ctx->q_data[MTK_Q_DATA_DST].bytesperline[0] = 0;
+
+	ctx->enc_params.framerate_num = MTK_DEFAULT_FRAMERATE_NUM;
+	ctx->enc_params.framerate_denom = MTK_DEFAULT_FRAMERATE_DENOM;
+}
+
+int mtk_vcodec_enc_ctrls_setup(struct mtk_vcodec_ctx *ctx)
+{
+	const struct v4l2_ctrl_ops *ops = &mtk_vcodec_enc_ctrl_ops;
+	struct v4l2_ctrl_handler *handler = &ctx->ctrl_hdl;
+	u8 h264_max_level;
+
+	if (ctx->dev->enc_capability & MTK_VENC_4K_CAPABILITY_ENABLE)
+		h264_max_level = V4L2_MPEG_VIDEO_H264_LEVEL_5_1;
+	else
+		h264_max_level = V4L2_MPEG_VIDEO_H264_LEVEL_4_2;
+
+	v4l2_ctrl_handler_init(handler, MTK_MAX_CTRLS_HINT);
+
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_MIN_BUFFERS_FOR_OUTPUT,
+			  1, 1, 1, 1);
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_BITRATE,
+			  ctx->dev->venc_pdata->min_bitrate,
+			  ctx->dev->venc_pdata->max_bitrate, 1, 4000000);
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_B_FRAMES,
+			0, 2, 1, 0);
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE,
+			0, 1, 1, 1);
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_H264_MAX_QP,
+			0, 51, 1, 51);
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_H264_I_PERIOD,
+			0, 65535, 1, 0);
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_GOP_SIZE,
+			0, 65535, 1, 0);
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE,
+			0, 1, 1, 0);
+	v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_FORCE_KEY_FRAME,
+			0, 0, 0, 0);
+	v4l2_ctrl_new_std_menu(handler, ops,
+			V4L2_CID_MPEG_VIDEO_HEADER_MODE,
+			V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME,
+			0, V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE);
+	v4l2_ctrl_new_std_menu(handler, ops, V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+			V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
+			0, V4L2_MPEG_VIDEO_H264_PROFILE_HIGH);
+	v4l2_ctrl_new_std_menu(handler, ops, V4L2_CID_MPEG_VIDEO_H264_LEVEL,
+			       h264_max_level,
+			       0, V4L2_MPEG_VIDEO_H264_LEVEL_4_0);
+	v4l2_ctrl_new_std_menu(handler, ops, V4L2_CID_MPEG_VIDEO_VP8_PROFILE,
+			       V4L2_MPEG_VIDEO_VP8_PROFILE_0, 0, V4L2_MPEG_VIDEO_VP8_PROFILE_0);
+	v4l2_ctrl_new_std_menu(handler, ops, V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
+			       V4L2_MPEG_VIDEO_BITRATE_MODE_CBR,
+			       ~(1 << V4L2_MPEG_VIDEO_BITRATE_MODE_CBR),
+			       V4L2_MPEG_VIDEO_BITRATE_MODE_CBR);
+
+
+	if (handler->error) {
+		mtk_v4l2_err("Init control handler fail %d",
+				handler->error);
+		return handler->error;
+	}
+
+	v4l2_ctrl_handler_setup(&ctx->ctrl_hdl);
+
+	return 0;
+}
+
+int mtk_vcodec_enc_queue_init(void *priv, struct vb2_queue *src_vq,
+			      struct vb2_queue *dst_vq)
+{
+	struct mtk_vcodec_ctx *ctx = priv;
+	int ret;
+
+	/* Note: VB2_USERPTR works with dma-contig because mt8173
+	 * support iommu
+	 * https://patchwork.kernel.org/patch/8335461/
+	 * https://patchwork.kernel.org/patch/7596181/
+	 */
+	src_vq->type		= V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes	= VB2_DMABUF | VB2_MMAP | VB2_USERPTR;
+	src_vq->drv_priv	= ctx;
+	src_vq->buf_struct_size = sizeof(struct mtk_video_enc_buf);
+	src_vq->ops		= &mtk_venc_vb2_ops;
+	src_vq->mem_ops		= &vb2_dma_contig_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock		= &ctx->q_mutex;
+	src_vq->dev		= &ctx->dev->plat_dev->dev;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes	= VB2_DMABUF | VB2_MMAP | VB2_USERPTR;
+	dst_vq->drv_priv	= ctx;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->ops		= &mtk_venc_vb2_ops;
+	dst_vq->mem_ops		= &vb2_dma_contig_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock		= &ctx->q_mutex;
+	dst_vq->dev		= &ctx->dev->plat_dev->dev;
+
+	return vb2_queue_init(dst_vq);
+}
+
+int mtk_venc_unlock(struct mtk_vcodec_ctx *ctx)
+{
+	struct mtk_vcodec_dev *dev = ctx->dev;
+
+	mutex_unlock(&dev->enc_mutex);
+	return 0;
+}
+
+int mtk_venc_lock(struct mtk_vcodec_ctx *ctx)
+{
+	struct mtk_vcodec_dev *dev = ctx->dev;
+
+	mutex_lock(&dev->enc_mutex);
+	return 0;
+}
+
+void mtk_vcodec_enc_release(struct mtk_vcodec_ctx *ctx)
+{
+	int ret = venc_if_deinit(ctx);
+
+	if (ret)
+		mtk_v4l2_err("venc_if_deinit failed=%d", ret);
+
+	ctx->state = MTK_STATE_FREE;
+}
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc.h b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc.h
new file mode 100644
index 000000000..513ee7993
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc.h
@@ -0,0 +1,50 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+* Copyright (c) 2016 MediaTek Inc.
+* Author: PC Chen <pc.chen@mediatek.com>
+*         Tiffany Lin <tiffany.lin@mediatek.com>
+*/
+
+#ifndef _MTK_VCODEC_ENC_H_
+#define _MTK_VCODEC_ENC_H_
+
+#include <media/videobuf2-core.h>
+#include <media/v4l2-mem2mem.h>
+
+#define MTK_VENC_IRQ_STATUS_SPS	0x1
+#define MTK_VENC_IRQ_STATUS_PPS	0x2
+#define MTK_VENC_IRQ_STATUS_FRM	0x4
+#define MTK_VENC_IRQ_STATUS_DRAM	0x8
+#define MTK_VENC_IRQ_STATUS_PAUSE	0x10
+#define MTK_VENC_IRQ_STATUS_SWITCH	0x20
+
+#define MTK_VENC_IRQ_STATUS_OFFSET	0x05C
+#define MTK_VENC_IRQ_ACK_OFFSET	0x060
+
+/**
+ * struct mtk_video_enc_buf - Private data related to each VB2 buffer.
+ * @m2m_buf:	M2M buffer
+ * @list:	list that buffer link to
+ * @param_change: Types of encode parameter change before encoding this
+ *				buffer
+ * @enc_params: Encode parameters changed before encode this buffer
+ */
+struct mtk_video_enc_buf {
+	struct v4l2_m2m_buffer m2m_buf;
+
+	u32 param_change;
+	struct mtk_enc_params enc_params;
+};
+
+extern const struct v4l2_ioctl_ops mtk_venc_ioctl_ops;
+extern const struct v4l2_m2m_ops mtk_venc_m2m_ops;
+
+int mtk_venc_unlock(struct mtk_vcodec_ctx *ctx);
+int mtk_venc_lock(struct mtk_vcodec_ctx *ctx);
+int mtk_vcodec_enc_queue_init(void *priv, struct vb2_queue *src_vq,
+			      struct vb2_queue *dst_vq);
+void mtk_vcodec_enc_release(struct mtk_vcodec_ctx *ctx);
+int mtk_vcodec_enc_ctrls_setup(struct mtk_vcodec_ctx *ctx);
+void mtk_vcodec_enc_set_default_params(struct mtk_vcodec_ctx *ctx);
+
+#endif /* _MTK_VCODEC_ENC_H_ */
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc_drv.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc_drv.c
new file mode 100644
index 000000000..9095186d5
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc_drv.c
@@ -0,0 +1,485 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+* Copyright (c) 2016 MediaTek Inc.
+* Author: PC Chen <pc.chen@mediatek.com>
+*	Tiffany Lin <tiffany.lin@mediatek.com>
+*/
+
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_enc.h"
+#include "mtk_vcodec_enc_pm.h"
+#include "mtk_vcodec_intr.h"
+#include "mtk_vcodec_util.h"
+#include "mtk_vcodec_fw.h"
+
+static const struct mtk_video_fmt mtk_video_formats_output[] = {
+	{
+		.fourcc = V4L2_PIX_FMT_NV12M,
+		.type = MTK_FMT_FRAME,
+		.num_planes = 2,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_NV21M,
+		.type = MTK_FMT_FRAME,
+		.num_planes = 2,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_YUV420M,
+		.type = MTK_FMT_FRAME,
+		.num_planes = 3,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_YVU420M,
+		.type = MTK_FMT_FRAME,
+		.num_planes = 3,
+	},
+};
+
+static const struct mtk_video_fmt mtk_video_formats_capture_h264[] =  {
+	{
+		.fourcc = V4L2_PIX_FMT_H264,
+		.type = MTK_FMT_ENC,
+		.num_planes = 1,
+	},
+};
+
+static const struct mtk_video_fmt mtk_video_formats_capture_vp8[] =  {
+	{
+		.fourcc = V4L2_PIX_FMT_VP8,
+		.type = MTK_FMT_ENC,
+		.num_planes = 1,
+	},
+};
+
+static void clean_irq_status(unsigned int irq_status, void __iomem *addr)
+{
+	if (irq_status & MTK_VENC_IRQ_STATUS_PAUSE)
+		writel(MTK_VENC_IRQ_STATUS_PAUSE, addr);
+
+	if (irq_status & MTK_VENC_IRQ_STATUS_SWITCH)
+		writel(MTK_VENC_IRQ_STATUS_SWITCH, addr);
+
+	if (irq_status & MTK_VENC_IRQ_STATUS_DRAM)
+		writel(MTK_VENC_IRQ_STATUS_DRAM, addr);
+
+	if (irq_status & MTK_VENC_IRQ_STATUS_SPS)
+		writel(MTK_VENC_IRQ_STATUS_SPS, addr);
+
+	if (irq_status & MTK_VENC_IRQ_STATUS_PPS)
+		writel(MTK_VENC_IRQ_STATUS_PPS, addr);
+
+	if (irq_status & MTK_VENC_IRQ_STATUS_FRM)
+		writel(MTK_VENC_IRQ_STATUS_FRM, addr);
+
+}
+static irqreturn_t mtk_vcodec_enc_irq_handler(int irq, void *priv)
+{
+	struct mtk_vcodec_dev *dev = priv;
+	struct mtk_vcodec_ctx *ctx;
+	unsigned long flags;
+	void __iomem *addr;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+	ctx = dev->curr_ctx;
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+
+	mtk_v4l2_debug(1, "id=%d coreid:%d", ctx->id, dev->venc_pdata->core_id);
+	addr = dev->reg_base[dev->venc_pdata->core_id] +
+				MTK_VENC_IRQ_ACK_OFFSET;
+
+	ctx->irq_status = readl(dev->reg_base[dev->venc_pdata->core_id] +
+				(MTK_VENC_IRQ_STATUS_OFFSET));
+
+	clean_irq_status(ctx->irq_status, addr);
+
+	wake_up_ctx(ctx, MTK_INST_IRQ_RECEIVED, 0);
+	return IRQ_HANDLED;
+}
+
+static int fops_vcodec_open(struct file *file)
+{
+	struct mtk_vcodec_dev *dev = video_drvdata(file);
+	struct mtk_vcodec_ctx *ctx = NULL;
+	int ret = 0;
+	struct vb2_queue *src_vq;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	mutex_lock(&dev->dev_mutex);
+	/*
+	 * Use simple counter to uniquely identify this context. Only
+	 * used for logging.
+	 */
+	ctx->id = dev->id_counter++;
+	v4l2_fh_init(&ctx->fh, video_devdata(file));
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+	INIT_LIST_HEAD(&ctx->list);
+	ctx->dev = dev;
+	init_waitqueue_head(&ctx->queue[0]);
+	mutex_init(&ctx->q_mutex);
+
+	ctx->type = MTK_INST_ENCODER;
+	ret = mtk_vcodec_enc_ctrls_setup(ctx);
+	if (ret) {
+		mtk_v4l2_err("Failed to setup controls() (%d)",
+				ret);
+		goto err_ctrls_setup;
+	}
+	ctx->m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev_enc, ctx,
+					 &mtk_vcodec_enc_queue_init);
+	if (IS_ERR((__force void *)ctx->m2m_ctx)) {
+		ret = PTR_ERR((__force void *)ctx->m2m_ctx);
+		mtk_v4l2_err("Failed to v4l2_m2m_ctx_init() (%d)",
+				ret);
+		goto err_m2m_ctx_init;
+	}
+	src_vq = v4l2_m2m_get_vq(ctx->m2m_ctx,
+				 V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	ctx->empty_flush_buf.vb.vb2_buf.vb2_queue = src_vq;
+	mtk_vcodec_enc_set_default_params(ctx);
+
+	if (v4l2_fh_is_singular(&ctx->fh)) {
+		/*
+		 * load fireware to checks if it was loaded already and
+		 * does nothing in that case
+		 */
+		ret = mtk_vcodec_fw_load_firmware(dev->fw_handler);
+		if (ret < 0) {
+			/*
+			 * Return 0 if downloading firmware successfully,
+			 * otherwise it is failed
+			 */
+			mtk_v4l2_err("vpu_load_firmware failed!");
+			goto err_load_fw;
+		}
+
+		dev->enc_capability =
+			mtk_vcodec_fw_get_venc_capa(dev->fw_handler);
+		mtk_v4l2_debug(0, "encoder capability %x", dev->enc_capability);
+	}
+
+	mtk_v4l2_debug(2, "Create instance [%d]@%p m2m_ctx=%p ",
+			ctx->id, ctx, ctx->m2m_ctx);
+
+	list_add(&ctx->list, &dev->ctx_list);
+
+	mutex_unlock(&dev->dev_mutex);
+	mtk_v4l2_debug(0, "%s encoder [%d]", dev_name(&dev->plat_dev->dev),
+			ctx->id);
+	return ret;
+
+	/* Deinit when failure occurred */
+err_load_fw:
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+err_m2m_ctx_init:
+	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+err_ctrls_setup:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+	mutex_unlock(&dev->dev_mutex);
+
+	return ret;
+}
+
+static int fops_vcodec_release(struct file *file)
+{
+	struct mtk_vcodec_dev *dev = video_drvdata(file);
+	struct mtk_vcodec_ctx *ctx = fh_to_ctx(file->private_data);
+
+	mtk_v4l2_debug(1, "[%d] encoder", ctx->id);
+	mutex_lock(&dev->dev_mutex);
+
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+	mtk_vcodec_enc_release(ctx);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
+
+	list_del_init(&ctx->list);
+	kfree(ctx);
+	mutex_unlock(&dev->dev_mutex);
+	return 0;
+}
+
+static const struct v4l2_file_operations mtk_vcodec_fops = {
+	.owner		= THIS_MODULE,
+	.open		= fops_vcodec_open,
+	.release	= fops_vcodec_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static int mtk_vcodec_probe(struct platform_device *pdev)
+{
+	struct mtk_vcodec_dev *dev;
+	struct video_device *vfd_enc;
+	phandle rproc_phandle;
+	enum mtk_vcodec_fw_type fw_type;
+	int ret;
+
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&dev->ctx_list);
+	dev->plat_dev = pdev;
+
+	if (!of_property_read_u32(pdev->dev.of_node, "mediatek,vpu",
+				  &rproc_phandle)) {
+		fw_type = VPU;
+	} else if (!of_property_read_u32(pdev->dev.of_node, "mediatek,scp",
+					 &rproc_phandle)) {
+		fw_type = SCP;
+	} else {
+		mtk_v4l2_err("Could not get venc IPI device");
+		return -ENODEV;
+	}
+	dma_set_max_seg_size(&pdev->dev, UINT_MAX);
+
+	dev->fw_handler = mtk_vcodec_fw_select(dev, fw_type, ENCODER);
+	if (IS_ERR(dev->fw_handler))
+		return PTR_ERR(dev->fw_handler);
+
+	dev->venc_pdata = of_device_get_match_data(&pdev->dev);
+	ret = mtk_vcodec_init_enc_clk(dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to get mtk vcodec clock source!");
+		goto err_enc_pm;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+
+	dev->reg_base[dev->venc_pdata->core_id] =
+		devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(dev->reg_base[dev->venc_pdata->core_id])) {
+		ret = PTR_ERR(dev->reg_base[dev->venc_pdata->core_id]);
+		goto err_res;
+	}
+
+	dev->enc_irq = platform_get_irq(pdev, 0);
+	if (dev->enc_irq < 0) {
+		ret = dev->enc_irq;
+		goto err_res;
+	}
+
+	irq_set_status_flags(dev->enc_irq, IRQ_NOAUTOEN);
+	ret = devm_request_irq(&pdev->dev, dev->enc_irq,
+			       mtk_vcodec_enc_irq_handler,
+			       0, pdev->name, dev);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Failed to install dev->enc_irq %d (%d) core_id (%d)",
+			dev->enc_irq, ret, dev->venc_pdata->core_id);
+		ret = -EINVAL;
+		goto err_res;
+	}
+
+	mutex_init(&dev->enc_mutex);
+	mutex_init(&dev->dev_mutex);
+	spin_lock_init(&dev->irqlock);
+
+	snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name), "%s",
+		 "[MTK_V4L2_VENC]");
+
+	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+	if (ret) {
+		mtk_v4l2_err("v4l2_device_register err=%d", ret);
+		goto err_res;
+	}
+
+	init_waitqueue_head(&dev->queue);
+
+	/* allocate video device for encoder and register it */
+	vfd_enc = video_device_alloc();
+	if (!vfd_enc) {
+		mtk_v4l2_err("Failed to allocate video device");
+		ret = -ENOMEM;
+		goto err_enc_alloc;
+	}
+	vfd_enc->fops           = &mtk_vcodec_fops;
+	vfd_enc->ioctl_ops      = &mtk_venc_ioctl_ops;
+	vfd_enc->release        = video_device_release;
+	vfd_enc->lock           = &dev->dev_mutex;
+	vfd_enc->v4l2_dev       = &dev->v4l2_dev;
+	vfd_enc->vfl_dir        = VFL_DIR_M2M;
+	vfd_enc->device_caps	= V4L2_CAP_VIDEO_M2M_MPLANE |
+					V4L2_CAP_STREAMING;
+
+	snprintf(vfd_enc->name, sizeof(vfd_enc->name), "%s",
+		 MTK_VCODEC_ENC_NAME);
+	video_set_drvdata(vfd_enc, dev);
+	dev->vfd_enc = vfd_enc;
+	platform_set_drvdata(pdev, dev);
+
+	dev->m2m_dev_enc = v4l2_m2m_init(&mtk_venc_m2m_ops);
+	if (IS_ERR((__force void *)dev->m2m_dev_enc)) {
+		mtk_v4l2_err("Failed to init mem2mem enc device");
+		ret = PTR_ERR((__force void *)dev->m2m_dev_enc);
+		goto err_enc_mem_init;
+	}
+
+	dev->encode_workqueue =
+			alloc_ordered_workqueue(MTK_VCODEC_ENC_NAME,
+						WQ_MEM_RECLAIM |
+						WQ_FREEZABLE);
+	if (!dev->encode_workqueue) {
+		mtk_v4l2_err("Failed to create encode workqueue");
+		ret = -EINVAL;
+		goto err_event_workq;
+	}
+
+	if (of_get_property(pdev->dev.of_node, "dma-ranges", NULL))
+		dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(34));
+
+	ret = video_register_device(vfd_enc, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		mtk_v4l2_err("Failed to register video device");
+		goto err_enc_reg;
+	}
+
+	mtk_v4l2_debug(0, "encoder %d registered as /dev/video%d",
+		       dev->venc_pdata->core_id, vfd_enc->num);
+
+	return 0;
+
+err_enc_reg:
+	destroy_workqueue(dev->encode_workqueue);
+err_event_workq:
+	v4l2_m2m_release(dev->m2m_dev_enc);
+err_enc_mem_init:
+	video_unregister_device(vfd_enc);
+err_enc_alloc:
+	v4l2_device_unregister(&dev->v4l2_dev);
+err_res:
+	pm_runtime_disable(dev->pm.dev);
+err_enc_pm:
+	mtk_vcodec_fw_release(dev->fw_handler);
+	return ret;
+}
+
+static const struct mtk_vcodec_enc_pdata mt8173_avc_pdata = {
+	.capture_formats = mtk_video_formats_capture_h264,
+	.num_capture_formats = ARRAY_SIZE(mtk_video_formats_capture_h264),
+	.output_formats = mtk_video_formats_output,
+	.num_output_formats = ARRAY_SIZE(mtk_video_formats_output),
+	.min_bitrate = 64,
+	.max_bitrate = 60000000,
+	.core_id = VENC_SYS,
+};
+
+static const struct mtk_vcodec_enc_pdata mt8173_vp8_pdata = {
+	.capture_formats = mtk_video_formats_capture_vp8,
+	.num_capture_formats = ARRAY_SIZE(mtk_video_formats_capture_vp8),
+	.output_formats = mtk_video_formats_output,
+	.num_output_formats = ARRAY_SIZE(mtk_video_formats_output),
+	.min_bitrate = 64,
+	.max_bitrate = 9000000,
+	.core_id = VENC_LT_SYS,
+};
+
+static const struct mtk_vcodec_enc_pdata mt8183_pdata = {
+	.uses_ext = true,
+	.capture_formats = mtk_video_formats_capture_h264,
+	.num_capture_formats = ARRAY_SIZE(mtk_video_formats_capture_h264),
+	.output_formats = mtk_video_formats_output,
+	.num_output_formats = ARRAY_SIZE(mtk_video_formats_output),
+	.min_bitrate = 64,
+	.max_bitrate = 40000000,
+	.core_id = VENC_SYS,
+};
+
+static const struct mtk_vcodec_enc_pdata mt8188_pdata = {
+	.uses_ext = true,
+	.capture_formats = mtk_video_formats_capture_h264,
+	.num_capture_formats = ARRAY_SIZE(mtk_video_formats_capture_h264),
+	.output_formats = mtk_video_formats_output,
+	.num_output_formats = ARRAY_SIZE(mtk_video_formats_output),
+	.min_bitrate = 64,
+	.max_bitrate = 50000000,
+	.core_id = VENC_SYS,
+	.uses_34bit = true,
+};
+
+static const struct mtk_vcodec_enc_pdata mt8192_pdata = {
+	.uses_ext = true,
+	.capture_formats = mtk_video_formats_capture_h264,
+	.num_capture_formats = ARRAY_SIZE(mtk_video_formats_capture_h264),
+	.output_formats = mtk_video_formats_output,
+	.num_output_formats = ARRAY_SIZE(mtk_video_formats_output),
+	.min_bitrate = 64,
+	.max_bitrate = 100000000,
+	.core_id = VENC_SYS,
+};
+
+static const struct mtk_vcodec_enc_pdata mt8195_pdata = {
+	.uses_ext = true,
+	.capture_formats = mtk_video_formats_capture_h264,
+	.num_capture_formats = ARRAY_SIZE(mtk_video_formats_capture_h264),
+	.output_formats = mtk_video_formats_output,
+	.num_output_formats = ARRAY_SIZE(mtk_video_formats_output),
+	.min_bitrate = 64,
+	.max_bitrate = 100000000,
+	.core_id = VENC_SYS,
+};
+
+static const struct of_device_id mtk_vcodec_enc_match[] = {
+	{.compatible = "mediatek,mt8173-vcodec-enc",
+			.data = &mt8173_avc_pdata},
+	{.compatible = "mediatek,mt8173-vcodec-enc-vp8",
+			.data = &mt8173_vp8_pdata},
+	{.compatible = "mediatek,mt8183-vcodec-enc", .data = &mt8183_pdata},
+	{.compatible = "mediatek,mt8188-vcodec-enc", .data = &mt8188_pdata},
+	{.compatible = "mediatek,mt8192-vcodec-enc", .data = &mt8192_pdata},
+	{.compatible = "mediatek,mt8195-vcodec-enc", .data = &mt8195_pdata},
+	{},
+};
+MODULE_DEVICE_TABLE(of, mtk_vcodec_enc_match);
+
+static int mtk_vcodec_enc_remove(struct platform_device *pdev)
+{
+	struct mtk_vcodec_dev *dev = platform_get_drvdata(pdev);
+
+	mtk_v4l2_debug_enter();
+	destroy_workqueue(dev->encode_workqueue);
+	if (dev->m2m_dev_enc)
+		v4l2_m2m_release(dev->m2m_dev_enc);
+
+	if (dev->vfd_enc)
+		video_unregister_device(dev->vfd_enc);
+
+	v4l2_device_unregister(&dev->v4l2_dev);
+	pm_runtime_disable(dev->pm.dev);
+	mtk_vcodec_fw_release(dev->fw_handler);
+	return 0;
+}
+
+static struct platform_driver mtk_vcodec_enc_driver = {
+	.probe	= mtk_vcodec_probe,
+	.remove	= mtk_vcodec_enc_remove,
+	.driver	= {
+		.name	= MTK_VCODEC_ENC_NAME,
+		.of_match_table = mtk_vcodec_enc_match,
+	},
+};
+
+module_platform_driver(mtk_vcodec_enc_driver);
+
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Mediatek video codec V4L2 encoder driver");
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc_pm.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc_pm.c
new file mode 100644
index 000000000..7055954eb
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc_pm.c
@@ -0,0 +1,90 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+* Copyright (c) 2016 MediaTek Inc.
+* Author: Tiffany Lin <tiffany.lin@mediatek.com>
+*/
+
+#include <linux/clk.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/pm_runtime.h>
+
+#include "mtk_vcodec_enc_pm.h"
+#include "mtk_vcodec_util.h"
+
+int mtk_vcodec_init_enc_clk(struct mtk_vcodec_dev *mtkdev)
+{
+	struct platform_device *pdev;
+	struct mtk_vcodec_pm *pm;
+	struct mtk_vcodec_clk *enc_clk;
+	struct mtk_vcodec_clk_info *clk_info;
+	int ret, i;
+
+	pdev = mtkdev->plat_dev;
+	pm = &mtkdev->pm;
+	memset(pm, 0, sizeof(struct mtk_vcodec_pm));
+	pm->dev = &pdev->dev;
+	enc_clk = &pm->venc_clk;
+
+	enc_clk->clk_num = of_property_count_strings(pdev->dev.of_node,
+		"clock-names");
+	if (enc_clk->clk_num > 0) {
+		enc_clk->clk_info = devm_kcalloc(&pdev->dev,
+			enc_clk->clk_num, sizeof(*clk_info),
+			GFP_KERNEL);
+		if (!enc_clk->clk_info)
+			return -ENOMEM;
+	} else {
+		mtk_v4l2_err("Failed to get venc clock count");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < enc_clk->clk_num; i++) {
+		clk_info = &enc_clk->clk_info[i];
+		ret = of_property_read_string_index(pdev->dev.of_node,
+			"clock-names", i, &clk_info->clk_name);
+		if (ret) {
+			mtk_v4l2_err("venc failed to get clk name %d", i);
+			return ret;
+		}
+		clk_info->vcodec_clk = devm_clk_get(&pdev->dev,
+			clk_info->clk_name);
+		if (IS_ERR(clk_info->vcodec_clk)) {
+			mtk_v4l2_err("venc devm_clk_get (%d)%s fail", i,
+				clk_info->clk_name);
+			return PTR_ERR(clk_info->vcodec_clk);
+		}
+	}
+
+	return 0;
+}
+
+void mtk_vcodec_enc_clock_on(struct mtk_vcodec_pm *pm)
+{
+	struct mtk_vcodec_clk *enc_clk = &pm->venc_clk;
+	int ret, i = 0;
+
+	for (i = 0; i < enc_clk->clk_num; i++) {
+		ret = clk_prepare_enable(enc_clk->clk_info[i].vcodec_clk);
+		if (ret) {
+			mtk_v4l2_err("venc clk_prepare_enable %d %s fail %d", i,
+				enc_clk->clk_info[i].clk_name, ret);
+			goto clkerr;
+		}
+	}
+
+	return;
+
+clkerr:
+	for (i -= 1; i >= 0; i--)
+		clk_disable_unprepare(enc_clk->clk_info[i].vcodec_clk);
+}
+
+void mtk_vcodec_enc_clock_off(struct mtk_vcodec_pm *pm)
+{
+	struct mtk_vcodec_clk *enc_clk = &pm->venc_clk;
+	int i = 0;
+
+	for (i = enc_clk->clk_num - 1; i >= 0; i--)
+		clk_disable_unprepare(enc_clk->clk_info[i].vcodec_clk);
+}
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc_pm.h b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc_pm.h
new file mode 100644
index 000000000..bc455cefc
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_enc_pm.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+* Copyright (c) 2016 MediaTek Inc.
+* Author: Tiffany Lin <tiffany.lin@mediatek.com>
+*/
+
+#ifndef _MTK_VCODEC_ENC_PM_H_
+#define _MTK_VCODEC_ENC_PM_H_
+
+#include "mtk_vcodec_drv.h"
+
+int mtk_vcodec_init_enc_clk(struct mtk_vcodec_dev *dev);
+
+void mtk_vcodec_enc_clock_on(struct mtk_vcodec_pm *pm);
+void mtk_vcodec_enc_clock_off(struct mtk_vcodec_pm *pm);
+
+#endif /* _MTK_VCODEC_ENC_PM_H_ */
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw.c
new file mode 100644
index 000000000..556e54aad
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw.c
@@ -0,0 +1,73 @@
+// SPDX-License-Identifier: GPL-2.0
+
+#include "mtk_vcodec_fw.h"
+#include "mtk_vcodec_fw_priv.h"
+#include "mtk_vcodec_util.h"
+#include "mtk_vcodec_drv.h"
+
+struct mtk_vcodec_fw *mtk_vcodec_fw_select(struct mtk_vcodec_dev *dev,
+					   enum mtk_vcodec_fw_type type,
+					   enum mtk_vcodec_fw_use fw_use)
+{
+	switch (type) {
+	case VPU:
+		return mtk_vcodec_fw_vpu_init(dev, fw_use);
+	case SCP:
+		return mtk_vcodec_fw_scp_init(dev);
+	default:
+		mtk_v4l2_err("invalid vcodec fw type");
+		return ERR_PTR(-EINVAL);
+	}
+}
+EXPORT_SYMBOL_GPL(mtk_vcodec_fw_select);
+
+void mtk_vcodec_fw_release(struct mtk_vcodec_fw *fw)
+{
+	fw->ops->release(fw);
+}
+EXPORT_SYMBOL_GPL(mtk_vcodec_fw_release);
+
+int mtk_vcodec_fw_load_firmware(struct mtk_vcodec_fw *fw)
+{
+	return fw->ops->load_firmware(fw);
+}
+EXPORT_SYMBOL_GPL(mtk_vcodec_fw_load_firmware);
+
+unsigned int mtk_vcodec_fw_get_vdec_capa(struct mtk_vcodec_fw *fw)
+{
+	return fw->ops->get_vdec_capa(fw);
+}
+EXPORT_SYMBOL_GPL(mtk_vcodec_fw_get_vdec_capa);
+
+unsigned int mtk_vcodec_fw_get_venc_capa(struct mtk_vcodec_fw *fw)
+{
+	return fw->ops->get_venc_capa(fw);
+}
+EXPORT_SYMBOL_GPL(mtk_vcodec_fw_get_venc_capa);
+
+void *mtk_vcodec_fw_map_dm_addr(struct mtk_vcodec_fw *fw, u32 mem_addr)
+{
+	return fw->ops->map_dm_addr(fw, mem_addr);
+}
+EXPORT_SYMBOL_GPL(mtk_vcodec_fw_map_dm_addr);
+
+int mtk_vcodec_fw_ipi_register(struct mtk_vcodec_fw *fw, int id,
+			       mtk_vcodec_ipi_handler handler,
+			       const char *name, void *priv)
+{
+	return fw->ops->ipi_register(fw, id, handler, name, priv);
+}
+EXPORT_SYMBOL_GPL(mtk_vcodec_fw_ipi_register);
+
+int mtk_vcodec_fw_ipi_send(struct mtk_vcodec_fw *fw, int id, void *buf,
+			   unsigned int len, unsigned int wait)
+{
+	return fw->ops->ipi_send(fw, id, buf, len, wait);
+}
+EXPORT_SYMBOL_GPL(mtk_vcodec_fw_ipi_send);
+
+int mtk_vcodec_fw_get_type(struct mtk_vcodec_fw *fw)
+{
+	return fw->type;
+}
+EXPORT_SYMBOL_GPL(mtk_vcodec_fw_get_type);
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw.h b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw.h
new file mode 100644
index 000000000..168241146
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+
+#ifndef _MTK_VCODEC_FW_H_
+#define _MTK_VCODEC_FW_H_
+
+#include <linux/remoteproc.h>
+#include <linux/remoteproc/mtk_scp.h>
+
+#include "../vpu/mtk_vpu.h"
+
+struct mtk_vcodec_dev;
+
+enum mtk_vcodec_fw_type {
+	VPU,
+	SCP,
+};
+
+enum mtk_vcodec_fw_use {
+	DECODER,
+	ENCODER,
+};
+
+struct mtk_vcodec_fw;
+
+typedef void (*mtk_vcodec_ipi_handler) (void *data,
+	unsigned int len, void *priv);
+
+struct mtk_vcodec_fw *mtk_vcodec_fw_select(struct mtk_vcodec_dev *dev,
+					   enum mtk_vcodec_fw_type type,
+					   enum mtk_vcodec_fw_use fw_use);
+void mtk_vcodec_fw_release(struct mtk_vcodec_fw *fw);
+
+int mtk_vcodec_fw_load_firmware(struct mtk_vcodec_fw *fw);
+unsigned int mtk_vcodec_fw_get_vdec_capa(struct mtk_vcodec_fw *fw);
+unsigned int mtk_vcodec_fw_get_venc_capa(struct mtk_vcodec_fw *fw);
+void *mtk_vcodec_fw_map_dm_addr(struct mtk_vcodec_fw *fw, u32 mem_addr);
+int mtk_vcodec_fw_ipi_register(struct mtk_vcodec_fw *fw, int id,
+			       mtk_vcodec_ipi_handler handler,
+			       const char *name, void *priv);
+int mtk_vcodec_fw_ipi_send(struct mtk_vcodec_fw *fw, int id,
+			   void *buf, unsigned int len, unsigned int wait);
+int mtk_vcodec_fw_get_type(struct mtk_vcodec_fw *fw);
+
+#endif /* _MTK_VCODEC_FW_H_ */
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw_priv.h b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw_priv.h
new file mode 100644
index 000000000..b41e66185
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw_priv.h
@@ -0,0 +1,52 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+
+#ifndef _MTK_VCODEC_FW_PRIV_H_
+#define _MTK_VCODEC_FW_PRIV_H_
+
+#include "mtk_vcodec_fw.h"
+
+struct mtk_vcodec_dev;
+
+struct mtk_vcodec_fw {
+	enum mtk_vcodec_fw_type type;
+	const struct mtk_vcodec_fw_ops *ops;
+	struct platform_device *pdev;
+	struct mtk_scp *scp;
+};
+
+struct mtk_vcodec_fw_ops {
+	int (*load_firmware)(struct mtk_vcodec_fw *fw);
+	unsigned int (*get_vdec_capa)(struct mtk_vcodec_fw *fw);
+	unsigned int (*get_venc_capa)(struct mtk_vcodec_fw *fw);
+	void *(*map_dm_addr)(struct mtk_vcodec_fw *fw, u32 dtcm_dmem_addr);
+	int (*ipi_register)(struct mtk_vcodec_fw *fw, int id,
+			    mtk_vcodec_ipi_handler handler, const char *name,
+			    void *priv);
+	int (*ipi_send)(struct mtk_vcodec_fw *fw, int id, void *buf,
+			unsigned int len, unsigned int wait);
+	void (*release)(struct mtk_vcodec_fw *fw);
+};
+
+#if IS_ENABLED(CONFIG_VIDEO_MEDIATEK_VCODEC_VPU)
+struct mtk_vcodec_fw *mtk_vcodec_fw_vpu_init(struct mtk_vcodec_dev *dev,
+					     enum mtk_vcodec_fw_use fw_use);
+#else
+static inline struct mtk_vcodec_fw *
+mtk_vcodec_fw_vpu_init(struct mtk_vcodec_dev *dev,
+		       enum mtk_vcodec_fw_use fw_use)
+{
+	return ERR_PTR(-ENODEV);
+}
+#endif /* CONFIG_VIDEO_MEDIATEK_VCODEC_VPU */
+
+#if IS_ENABLED(CONFIG_VIDEO_MEDIATEK_VCODEC_SCP)
+struct mtk_vcodec_fw *mtk_vcodec_fw_scp_init(struct mtk_vcodec_dev *dev);
+#else
+static inline struct mtk_vcodec_fw *
+mtk_vcodec_fw_scp_init(struct mtk_vcodec_dev *dev)
+{
+	return ERR_PTR(-ENODEV);
+}
+#endif /* CONFIG_VIDEO_MEDIATEK_VCODEC_SCP */
+
+#endif /* _MTK_VCODEC_FW_PRIV_H_ */
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw_scp.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw_scp.c
new file mode 100644
index 000000000..d8e66b645
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw_scp.c
@@ -0,0 +1,73 @@
+// SPDX-License-Identifier: GPL-2.0
+
+#include "mtk_vcodec_fw_priv.h"
+#include "mtk_vcodec_util.h"
+#include "mtk_vcodec_drv.h"
+
+static int mtk_vcodec_scp_load_firmware(struct mtk_vcodec_fw *fw)
+{
+	return rproc_boot(scp_get_rproc(fw->scp));
+}
+
+static unsigned int mtk_vcodec_scp_get_vdec_capa(struct mtk_vcodec_fw *fw)
+{
+	return scp_get_vdec_hw_capa(fw->scp);
+}
+
+static unsigned int mtk_vcodec_scp_get_venc_capa(struct mtk_vcodec_fw *fw)
+{
+	return scp_get_venc_hw_capa(fw->scp);
+}
+
+static void *mtk_vcodec_vpu_scp_dm_addr(struct mtk_vcodec_fw *fw,
+					u32 dtcm_dmem_addr)
+{
+	return scp_mapping_dm_addr(fw->scp, dtcm_dmem_addr);
+}
+
+static int mtk_vcodec_scp_set_ipi_register(struct mtk_vcodec_fw *fw, int id,
+					   mtk_vcodec_ipi_handler handler,
+					   const char *name, void *priv)
+{
+	return scp_ipi_register(fw->scp, id, handler, priv);
+}
+
+static int mtk_vcodec_scp_ipi_send(struct mtk_vcodec_fw *fw, int id, void *buf,
+				   unsigned int len, unsigned int wait)
+{
+	return scp_ipi_send(fw->scp, id, buf, len, wait);
+}
+
+static void mtk_vcodec_scp_release(struct mtk_vcodec_fw *fw)
+{
+	scp_put(fw->scp);
+}
+
+static const struct mtk_vcodec_fw_ops mtk_vcodec_rproc_msg = {
+	.load_firmware = mtk_vcodec_scp_load_firmware,
+	.get_vdec_capa = mtk_vcodec_scp_get_vdec_capa,
+	.get_venc_capa = mtk_vcodec_scp_get_venc_capa,
+	.map_dm_addr = mtk_vcodec_vpu_scp_dm_addr,
+	.ipi_register = mtk_vcodec_scp_set_ipi_register,
+	.ipi_send = mtk_vcodec_scp_ipi_send,
+	.release = mtk_vcodec_scp_release,
+};
+
+struct mtk_vcodec_fw *mtk_vcodec_fw_scp_init(struct mtk_vcodec_dev *dev)
+{
+	struct mtk_vcodec_fw *fw;
+	struct mtk_scp *scp;
+
+	scp = scp_get(dev->plat_dev);
+	if (!scp) {
+		mtk_v4l2_err("could not get vdec scp handle");
+		return ERR_PTR(-EPROBE_DEFER);
+	}
+
+	fw = devm_kzalloc(&dev->plat_dev->dev, sizeof(*fw), GFP_KERNEL);
+	fw->type = SCP;
+	fw->ops = &mtk_vcodec_rproc_msg;
+	fw->scp = scp;
+
+	return fw;
+}
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw_vpu.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw_vpu.c
new file mode 100644
index 000000000..cfc7ebed8
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_fw_vpu.c
@@ -0,0 +1,112 @@
+// SPDX-License-Identifier: GPL-2.0
+
+#include "mtk_vcodec_fw_priv.h"
+#include "mtk_vcodec_util.h"
+#include "mtk_vcodec_drv.h"
+
+static int mtk_vcodec_vpu_load_firmware(struct mtk_vcodec_fw *fw)
+{
+	return vpu_load_firmware(fw->pdev);
+}
+
+static unsigned int mtk_vcodec_vpu_get_vdec_capa(struct mtk_vcodec_fw *fw)
+{
+	return vpu_get_vdec_hw_capa(fw->pdev);
+}
+
+static unsigned int mtk_vcodec_vpu_get_venc_capa(struct mtk_vcodec_fw *fw)
+{
+	return vpu_get_venc_hw_capa(fw->pdev);
+}
+
+static void *mtk_vcodec_vpu_map_dm_addr(struct mtk_vcodec_fw *fw,
+					u32 dtcm_dmem_addr)
+{
+	return vpu_mapping_dm_addr(fw->pdev, dtcm_dmem_addr);
+}
+
+static int mtk_vcodec_vpu_set_ipi_register(struct mtk_vcodec_fw *fw, int id,
+					   mtk_vcodec_ipi_handler handler,
+					   const char *name, void *priv)
+{
+	/*
+	 * The handler we receive takes a void * as its first argument. We
+	 * cannot change this because it needs to be passed down to the rproc
+	 * subsystem when SCP is used. VPU takes a const argument, which is
+	 * more constrained, so the conversion below is safe.
+	 */
+	ipi_handler_t handler_const = (ipi_handler_t)handler;
+
+	return vpu_ipi_register(fw->pdev, id, handler_const, name, priv);
+}
+
+static int mtk_vcodec_vpu_ipi_send(struct mtk_vcodec_fw *fw, int id, void *buf,
+				   unsigned int len, unsigned int wait)
+{
+	return vpu_ipi_send(fw->pdev, id, buf, len);
+}
+
+static void mtk_vcodec_vpu_release(struct mtk_vcodec_fw *fw)
+{
+	put_device(&fw->pdev->dev);
+}
+
+static void mtk_vcodec_vpu_reset_handler(void *priv)
+{
+	struct mtk_vcodec_dev *dev = priv;
+	struct mtk_vcodec_ctx *ctx;
+
+	mtk_v4l2_err("Watchdog timeout!!");
+
+	mutex_lock(&dev->dev_mutex);
+	list_for_each_entry(ctx, &dev->ctx_list, list) {
+		ctx->state = MTK_STATE_ABORT;
+		mtk_v4l2_debug(0, "[%d] Change to state MTK_STATE_ABORT",
+			       ctx->id);
+	}
+	mutex_unlock(&dev->dev_mutex);
+}
+
+static const struct mtk_vcodec_fw_ops mtk_vcodec_vpu_msg = {
+	.load_firmware = mtk_vcodec_vpu_load_firmware,
+	.get_vdec_capa = mtk_vcodec_vpu_get_vdec_capa,
+	.get_venc_capa = mtk_vcodec_vpu_get_venc_capa,
+	.map_dm_addr = mtk_vcodec_vpu_map_dm_addr,
+	.ipi_register = mtk_vcodec_vpu_set_ipi_register,
+	.ipi_send = mtk_vcodec_vpu_ipi_send,
+	.release = mtk_vcodec_vpu_release,
+};
+
+struct mtk_vcodec_fw *mtk_vcodec_fw_vpu_init(struct mtk_vcodec_dev *dev,
+					     enum mtk_vcodec_fw_use fw_use)
+{
+	struct platform_device *fw_pdev;
+	struct mtk_vcodec_fw *fw;
+	enum rst_id rst_id;
+
+	switch (fw_use) {
+	case ENCODER:
+		rst_id = VPU_RST_ENC;
+		break;
+	case DECODER:
+	default:
+		rst_id = VPU_RST_DEC;
+		break;
+	}
+
+	fw_pdev = vpu_get_plat_device(dev->plat_dev);
+	if (!fw_pdev) {
+		mtk_v4l2_err("firmware device is not ready");
+		return ERR_PTR(-EINVAL);
+	}
+	vpu_wdt_reg_handler(fw_pdev, mtk_vcodec_vpu_reset_handler, dev, rst_id);
+
+	fw = devm_kzalloc(&dev->plat_dev->dev, sizeof(*fw), GFP_KERNEL);
+	if (!fw)
+		return ERR_PTR(-ENOMEM);
+	fw->type = VPU;
+	fw->ops = &mtk_vcodec_vpu_msg;
+	fw->pdev = fw_pdev;
+
+	return fw;
+}
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_intr.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_intr.c
new file mode 100644
index 000000000..552b4c93d
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_intr.c
@@ -0,0 +1,43 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+* Copyright (c) 2016 MediaTek Inc.
+* Author: Tiffany Lin <tiffany.lin@mediatek.com>
+*/
+
+#include <linux/errno.h>
+#include <linux/wait.h>
+
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_intr.h"
+#include "mtk_vcodec_util.h"
+
+int mtk_vcodec_wait_for_done_ctx(struct mtk_vcodec_ctx *ctx,
+				 int command, unsigned int timeout_ms,
+				 unsigned int hw_id)
+{
+	long timeout_jiff, ret;
+	int status = 0;
+
+	timeout_jiff = msecs_to_jiffies(timeout_ms);
+	ret = wait_event_interruptible_timeout(ctx->queue[hw_id],
+					       ctx->int_cond[hw_id],
+					       timeout_jiff);
+
+	if (!ret) {
+		status = -1;	/* timeout */
+		mtk_v4l2_err("[%d] cmd=%d, type=%d, dec timeout=%ums (%d %d)",
+			     ctx->id, command, ctx->type, timeout_ms,
+			     ctx->int_cond[hw_id], ctx->int_type[hw_id]);
+	} else if (-ERESTARTSYS == ret) {
+		status = -1;
+		mtk_v4l2_err("[%d] cmd=%d, type=%d, dec inter fail (%d %d)",
+			     ctx->id, command, ctx->type,
+			     ctx->int_cond[hw_id], ctx->int_type[hw_id]);
+	}
+
+	ctx->int_cond[hw_id] = 0;
+	ctx->int_type[hw_id] = 0;
+
+	return status;
+}
+EXPORT_SYMBOL(mtk_vcodec_wait_for_done_ctx);
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_intr.h b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_intr.h
new file mode 100644
index 000000000..9681f4928
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_intr.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+* Copyright (c) 2016 MediaTek Inc.
+* Author: Tiffany Lin <tiffany.lin@mediatek.com>
+*/
+
+#ifndef _MTK_VCODEC_INTR_H_
+#define _MTK_VCODEC_INTR_H_
+
+#define MTK_INST_IRQ_RECEIVED		0x1
+
+struct mtk_vcodec_ctx;
+
+/* timeout is ms */
+int mtk_vcodec_wait_for_done_ctx(struct mtk_vcodec_ctx *ctx,
+				 int command, unsigned int timeout_ms,
+				 unsigned int hw_id);
+
+#endif /* _MTK_VCODEC_INTR_H_ */
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_util.c b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_util.c
new file mode 100644
index 000000000..ace78c4b5
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_util.c
@@ -0,0 +1,135 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+* Copyright (c) 2016 MediaTek Inc.
+* Author: PC Chen <pc.chen@mediatek.com>
+*	Tiffany Lin <tiffany.lin@mediatek.com>
+*/
+
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+
+#include "mtk_vcodec_dec_hw.h"
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_util.h"
+
+void __iomem *mtk_vcodec_get_reg_addr(struct mtk_vcodec_ctx *data,
+					unsigned int reg_idx)
+{
+	struct mtk_vcodec_ctx *ctx = (struct mtk_vcodec_ctx *)data;
+
+	if (!data || reg_idx >= NUM_MAX_VCODEC_REG_BASE) {
+		mtk_v4l2_err("Invalid arguments, reg_idx=%d", reg_idx);
+		return NULL;
+	}
+	return ctx->dev->reg_base[reg_idx];
+}
+EXPORT_SYMBOL(mtk_vcodec_get_reg_addr);
+
+int mtk_vcodec_mem_alloc(struct mtk_vcodec_ctx *data,
+			struct mtk_vcodec_mem *mem)
+{
+	unsigned long size = mem->size;
+	struct mtk_vcodec_ctx *ctx = (struct mtk_vcodec_ctx *)data;
+	struct device *dev = &ctx->dev->plat_dev->dev;
+
+	mem->va = dma_alloc_coherent(dev, size, &mem->dma_addr, GFP_KERNEL);
+	if (!mem->va) {
+		mtk_v4l2_err("%s dma_alloc size=%ld failed!", dev_name(dev),
+			     size);
+		return -ENOMEM;
+	}
+
+	mtk_v4l2_debug(3, "[%d]  - va      = %p", ctx->id, mem->va);
+	mtk_v4l2_debug(3, "[%d]  - dma     = 0x%lx", ctx->id,
+		       (unsigned long)mem->dma_addr);
+	mtk_v4l2_debug(3, "[%d]    size = 0x%lx", ctx->id, size);
+
+	return 0;
+}
+EXPORT_SYMBOL(mtk_vcodec_mem_alloc);
+
+void mtk_vcodec_mem_free(struct mtk_vcodec_ctx *data,
+			struct mtk_vcodec_mem *mem)
+{
+	unsigned long size = mem->size;
+	struct mtk_vcodec_ctx *ctx = (struct mtk_vcodec_ctx *)data;
+	struct device *dev = &ctx->dev->plat_dev->dev;
+
+	if (!mem->va) {
+		mtk_v4l2_err("%s dma_free size=%ld failed!", dev_name(dev),
+			     size);
+		return;
+	}
+
+	mtk_v4l2_debug(3, "[%d]  - va      = %p", ctx->id, mem->va);
+	mtk_v4l2_debug(3, "[%d]  - dma     = 0x%lx", ctx->id,
+		       (unsigned long)mem->dma_addr);
+	mtk_v4l2_debug(3, "[%d]    size = 0x%lx", ctx->id, size);
+
+	dma_free_coherent(dev, size, mem->va, mem->dma_addr);
+	mem->va = NULL;
+	mem->dma_addr = 0;
+	mem->size = 0;
+}
+EXPORT_SYMBOL(mtk_vcodec_mem_free);
+
+void *mtk_vcodec_get_hw_dev(struct mtk_vcodec_dev *dev, int hw_idx)
+{
+	if (hw_idx >= MTK_VDEC_HW_MAX || hw_idx < 0 || !dev->subdev_dev[hw_idx]) {
+		mtk_v4l2_err("hw idx is out of range:%d", hw_idx);
+		return NULL;
+	}
+
+	return dev->subdev_dev[hw_idx];
+}
+EXPORT_SYMBOL(mtk_vcodec_get_hw_dev);
+
+void mtk_vcodec_set_curr_ctx(struct mtk_vcodec_dev *vdec_dev,
+			     struct mtk_vcodec_ctx *ctx, int hw_idx)
+{
+	unsigned long flags;
+	struct mtk_vdec_hw_dev *subdev_dev;
+
+	spin_lock_irqsave(&vdec_dev->irqlock, flags);
+	if (vdec_dev->vdec_pdata->is_subdev_supported) {
+		subdev_dev = mtk_vcodec_get_hw_dev(vdec_dev, hw_idx);
+		if (!subdev_dev) {
+			mtk_v4l2_err("Failed to get hw dev");
+			spin_unlock_irqrestore(&vdec_dev->irqlock, flags);
+			return;
+		}
+		subdev_dev->curr_ctx = ctx;
+	} else {
+		vdec_dev->curr_ctx = ctx;
+	}
+	spin_unlock_irqrestore(&vdec_dev->irqlock, flags);
+}
+EXPORT_SYMBOL(mtk_vcodec_set_curr_ctx);
+
+struct mtk_vcodec_ctx *mtk_vcodec_get_curr_ctx(struct mtk_vcodec_dev *vdec_dev,
+					       unsigned int hw_idx)
+{
+	unsigned long flags;
+	struct mtk_vcodec_ctx *ctx;
+	struct mtk_vdec_hw_dev *subdev_dev;
+
+	spin_lock_irqsave(&vdec_dev->irqlock, flags);
+	if (vdec_dev->vdec_pdata->is_subdev_supported) {
+		subdev_dev = mtk_vcodec_get_hw_dev(vdec_dev, hw_idx);
+		if (!subdev_dev) {
+			mtk_v4l2_err("Failed to get hw dev");
+			spin_unlock_irqrestore(&vdec_dev->irqlock, flags);
+			return NULL;
+		}
+		ctx = subdev_dev->curr_ctx;
+	} else {
+		ctx = vdec_dev->curr_ctx;
+	}
+	spin_unlock_irqrestore(&vdec_dev->irqlock, flags);
+	return ctx;
+}
+EXPORT_SYMBOL(mtk_vcodec_get_curr_ctx);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Mediatek video codec driver");
diff --git a/drivers/media/platform/mediatek/vcodec/mtk_vcodec_util.h b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_util.h
new file mode 100644
index 000000000..71956627a
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/mtk_vcodec_util.h
@@ -0,0 +1,63 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+* Copyright (c) 2016 MediaTek Inc.
+* Author: PC Chen <pc.chen@mediatek.com>
+*	Tiffany Lin <tiffany.lin@mediatek.com>
+*/
+
+#ifndef _MTK_VCODEC_UTIL_H_
+#define _MTK_VCODEC_UTIL_H_
+
+#include <linux/types.h>
+#include <linux/dma-direction.h>
+
+struct mtk_vcodec_mem {
+	size_t size;
+	void *va;
+	dma_addr_t dma_addr;
+};
+
+struct mtk_vcodec_fb {
+	size_t size;
+	dma_addr_t dma_addr;
+};
+
+struct mtk_vcodec_ctx;
+struct mtk_vcodec_dev;
+
+#undef pr_fmt
+#define pr_fmt(fmt) "%s(),%d: " fmt, __func__, __LINE__
+
+#define mtk_v4l2_err(fmt, args...)                \
+	pr_err("[MTK_V4L2][ERROR] " fmt "\n", ##args)
+
+#define mtk_vcodec_err(h, fmt, args...)				\
+	pr_err("[MTK_VCODEC][ERROR][%d]: " fmt "\n",		\
+	       ((struct mtk_vcodec_ctx *)(h)->ctx)->id, ##args)
+
+
+#define mtk_v4l2_debug(level, fmt, args...) pr_debug(fmt, ##args)
+
+#define mtk_v4l2_debug_enter()  mtk_v4l2_debug(3, "+")
+#define mtk_v4l2_debug_leave()  mtk_v4l2_debug(3, "-")
+
+#define mtk_vcodec_debug(h, fmt, args...)			\
+	pr_debug("[MTK_VCODEC][%d]: " fmt "\n",			\
+		((struct mtk_vcodec_ctx *)(h)->ctx)->id, ##args)
+
+#define mtk_vcodec_debug_enter(h)  mtk_vcodec_debug(h, "+")
+#define mtk_vcodec_debug_leave(h)  mtk_vcodec_debug(h, "-")
+
+void __iomem *mtk_vcodec_get_reg_addr(struct mtk_vcodec_ctx *data,
+				unsigned int reg_idx);
+int mtk_vcodec_mem_alloc(struct mtk_vcodec_ctx *data,
+				struct mtk_vcodec_mem *mem);
+void mtk_vcodec_mem_free(struct mtk_vcodec_ctx *data,
+				struct mtk_vcodec_mem *mem);
+void mtk_vcodec_set_curr_ctx(struct mtk_vcodec_dev *vdec_dev,
+			     struct mtk_vcodec_ctx *ctx, int hw_idx);
+struct mtk_vcodec_ctx *mtk_vcodec_get_curr_ctx(struct mtk_vcodec_dev *vdec_dev,
+					       unsigned int hw_idx);
+void *mtk_vcodec_get_hw_dev(struct mtk_vcodec_dev *dev, int hw_idx);
+
+#endif /* _MTK_VCODEC_UTIL_H_ */
diff --git a/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_if.c b/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_if.c
new file mode 100644
index 000000000..481655bb6
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_if.c
@@ -0,0 +1,503 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: PC Chen <pc.chen@mediatek.com>
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#include "../vdec_drv_if.h"
+#include "../mtk_vcodec_util.h"
+#include "../mtk_vcodec_dec.h"
+#include "../mtk_vcodec_intr.h"
+#include "../vdec_vpu_if.h"
+#include "../vdec_drv_base.h"
+
+#define NAL_NON_IDR_SLICE			0x01
+#define NAL_IDR_SLICE				0x05
+#define NAL_H264_PPS				0x08
+#define NAL_TYPE(value)				((value) & 0x1F)
+
+#define BUF_PREDICTION_SZ			(32 * 1024)
+
+#define MB_UNIT_LEN				16
+
+/* motion vector size (bytes) for every macro block */
+#define HW_MB_STORE_SZ				64
+
+#define H264_MAX_FB_NUM				17
+#define HDR_PARSING_BUF_SZ			1024
+
+#define DEC_ERR_RET(ret)			((ret) >> 16)
+#define H264_ERR_NOT_VALID			3
+
+/**
+ * struct h264_fb - h264 decode frame buffer information
+ * @vdec_fb_va  : virtual address of struct vdec_fb
+ * @y_fb_dma    : dma address of Y frame buffer (luma)
+ * @c_fb_dma    : dma address of C frame buffer (chroma)
+ * @poc         : picture order count of frame buffer
+ * @reserved    : for 8 bytes alignment
+ */
+struct h264_fb {
+	uint64_t vdec_fb_va;
+	uint64_t y_fb_dma;
+	uint64_t c_fb_dma;
+	int32_t poc;
+	uint32_t reserved;
+};
+
+/**
+ * struct h264_ring_fb_list - ring frame buffer list
+ * @fb_list   : frame buffer array
+ * @read_idx  : read index
+ * @write_idx : write index
+ * @count     : buffer count in list
+ * @reserved  : for 8 bytes alignment
+ */
+struct h264_ring_fb_list {
+	struct h264_fb fb_list[H264_MAX_FB_NUM];
+	unsigned int read_idx;
+	unsigned int write_idx;
+	unsigned int count;
+	unsigned int reserved;
+};
+
+/**
+ * struct vdec_h264_dec_info - decode information
+ * @dpb_sz		: decoding picture buffer size
+ * @resolution_changed  : resolution change happen
+ * @realloc_mv_buf	: flag to notify driver to re-allocate mv buffer
+ * @reserved		: for 8 bytes alignment
+ * @bs_dma		: Input bit-stream buffer dma address
+ * @y_fb_dma		: Y frame buffer dma address
+ * @c_fb_dma		: C frame buffer dma address
+ * @vdec_fb_va		: VDEC frame buffer struct virtual address
+ */
+struct vdec_h264_dec_info {
+	uint32_t dpb_sz;
+	uint32_t resolution_changed;
+	uint32_t realloc_mv_buf;
+	uint32_t reserved;
+	uint64_t bs_dma;
+	uint64_t y_fb_dma;
+	uint64_t c_fb_dma;
+	uint64_t vdec_fb_va;
+};
+
+/**
+ * struct vdec_h264_vsi - shared memory for decode information exchange
+ *                        between VPU and Host.
+ *                        The memory is allocated by VPU then mapping to Host
+ *                        in vpu_dec_init() and freed in vpu_dec_deinit()
+ *                        by VPU.
+ *                        AP-W/R : AP is writer/reader on this item
+ *                        VPU-W/R: VPU is write/reader on this item
+ * @hdr_buf      : Header parsing buffer (AP-W, VPU-R)
+ * @pred_buf_dma : HW working predication buffer dma address (AP-W, VPU-R)
+ * @mv_buf_dma   : HW working motion vector buffer dma address (AP-W, VPU-R)
+ * @list_free    : free frame buffer ring list (AP-W/R, VPU-W)
+ * @list_disp    : display frame buffer ring list (AP-R, VPU-W)
+ * @dec          : decode information (AP-R, VPU-W)
+ * @pic          : picture information (AP-R, VPU-W)
+ * @crop         : crop information (AP-R, VPU-W)
+ */
+struct vdec_h264_vsi {
+	unsigned char hdr_buf[HDR_PARSING_BUF_SZ];
+	uint64_t pred_buf_dma;
+	uint64_t mv_buf_dma[H264_MAX_FB_NUM];
+	struct h264_ring_fb_list list_free;
+	struct h264_ring_fb_list list_disp;
+	struct vdec_h264_dec_info dec;
+	struct vdec_pic_info pic;
+	struct v4l2_rect crop;
+};
+
+/**
+ * struct vdec_h264_inst - h264 decoder instance
+ * @num_nalu : how many nalus be decoded
+ * @ctx      : point to mtk_vcodec_ctx
+ * @pred_buf : HW working predication buffer
+ * @mv_buf   : HW working motion vector buffer
+ * @vpu      : VPU instance
+ * @vsi      : VPU shared information
+ */
+struct vdec_h264_inst {
+	unsigned int num_nalu;
+	struct mtk_vcodec_ctx *ctx;
+	struct mtk_vcodec_mem pred_buf;
+	struct mtk_vcodec_mem mv_buf[H264_MAX_FB_NUM];
+	struct vdec_vpu_inst vpu;
+	struct vdec_h264_vsi *vsi;
+};
+
+static unsigned int get_mv_buf_size(unsigned int width, unsigned int height)
+{
+	return HW_MB_STORE_SZ * (width/MB_UNIT_LEN) * (height/MB_UNIT_LEN);
+}
+
+static int allocate_predication_buf(struct vdec_h264_inst *inst)
+{
+	int err = 0;
+
+	inst->pred_buf.size = BUF_PREDICTION_SZ;
+	err = mtk_vcodec_mem_alloc(inst->ctx, &inst->pred_buf);
+	if (err) {
+		mtk_vcodec_err(inst, "failed to allocate ppl buf");
+		return err;
+	}
+
+	inst->vsi->pred_buf_dma = inst->pred_buf.dma_addr;
+	return 0;
+}
+
+static void free_predication_buf(struct vdec_h264_inst *inst)
+{
+	struct mtk_vcodec_mem *mem = NULL;
+
+	mtk_vcodec_debug_enter(inst);
+
+	inst->vsi->pred_buf_dma = 0;
+	mem = &inst->pred_buf;
+	if (mem->va)
+		mtk_vcodec_mem_free(inst->ctx, mem);
+}
+
+static int alloc_mv_buf(struct vdec_h264_inst *inst, struct vdec_pic_info *pic)
+{
+	int i;
+	int err;
+	struct mtk_vcodec_mem *mem = NULL;
+	unsigned int buf_sz = get_mv_buf_size(pic->buf_w, pic->buf_h);
+
+	for (i = 0; i < H264_MAX_FB_NUM; i++) {
+		mem = &inst->mv_buf[i];
+		if (mem->va)
+			mtk_vcodec_mem_free(inst->ctx, mem);
+		mem->size = buf_sz;
+		err = mtk_vcodec_mem_alloc(inst->ctx, mem);
+		if (err) {
+			mtk_vcodec_err(inst, "failed to allocate mv buf");
+			return err;
+		}
+		inst->vsi->mv_buf_dma[i] = mem->dma_addr;
+	}
+
+	return 0;
+}
+
+static void free_mv_buf(struct vdec_h264_inst *inst)
+{
+	int i;
+	struct mtk_vcodec_mem *mem = NULL;
+
+	for (i = 0; i < H264_MAX_FB_NUM; i++) {
+		inst->vsi->mv_buf_dma[i] = 0;
+		mem = &inst->mv_buf[i];
+		if (mem->va)
+			mtk_vcodec_mem_free(inst->ctx, mem);
+	}
+}
+
+static int check_list_validity(struct vdec_h264_inst *inst, bool disp_list)
+{
+	struct h264_ring_fb_list *list;
+
+	list = disp_list ? &inst->vsi->list_disp : &inst->vsi->list_free;
+
+	if (list->count > H264_MAX_FB_NUM ||
+	    list->read_idx >= H264_MAX_FB_NUM ||
+	    list->write_idx >= H264_MAX_FB_NUM) {
+		mtk_vcodec_err(inst, "%s list err: cnt=%d r_idx=%d w_idx=%d",
+			       disp_list ? "disp" : "free", list->count,
+			       list->read_idx, list->write_idx);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void put_fb_to_free(struct vdec_h264_inst *inst, struct vdec_fb *fb)
+{
+	struct h264_ring_fb_list *list;
+
+	if (fb) {
+		if (check_list_validity(inst, false))
+			return;
+
+		list = &inst->vsi->list_free;
+		if (list->count == H264_MAX_FB_NUM) {
+			mtk_vcodec_err(inst, "[FB] put fb free_list full");
+			return;
+		}
+
+		mtk_vcodec_debug(inst, "[FB] put fb into free_list @(%p, %llx)",
+				 fb->base_y.va, (u64)fb->base_y.dma_addr);
+
+		list->fb_list[list->write_idx].vdec_fb_va = (u64)(uintptr_t)fb;
+		list->write_idx = (list->write_idx == H264_MAX_FB_NUM - 1) ?
+				  0 : list->write_idx + 1;
+		list->count++;
+	}
+}
+
+static void get_pic_info(struct vdec_h264_inst *inst,
+			 struct vdec_pic_info *pic)
+{
+	*pic = inst->vsi->pic;
+	mtk_vcodec_debug(inst, "pic(%d, %d), buf(%d, %d)",
+			 pic->pic_w, pic->pic_h, pic->buf_w, pic->buf_h);
+	mtk_vcodec_debug(inst, "fb size: Y(%d), C(%d)",
+			 pic->fb_sz[0], pic->fb_sz[1]);
+}
+
+static void get_crop_info(struct vdec_h264_inst *inst, struct v4l2_rect *cr)
+{
+	cr->left = inst->vsi->crop.left;
+	cr->top = inst->vsi->crop.top;
+	cr->width = inst->vsi->crop.width;
+	cr->height = inst->vsi->crop.height;
+
+	mtk_vcodec_debug(inst, "l=%d, t=%d, w=%d, h=%d",
+			 cr->left, cr->top, cr->width, cr->height);
+}
+
+static void get_dpb_size(struct vdec_h264_inst *inst, unsigned int *dpb_sz)
+{
+	*dpb_sz = inst->vsi->dec.dpb_sz;
+	mtk_vcodec_debug(inst, "sz=%d", *dpb_sz);
+}
+
+static int vdec_h264_init(struct mtk_vcodec_ctx *ctx)
+{
+	struct vdec_h264_inst *inst = NULL;
+	int err;
+
+	inst = kzalloc(sizeof(*inst), GFP_KERNEL);
+	if (!inst)
+		return -ENOMEM;
+
+	inst->ctx = ctx;
+
+	inst->vpu.id = IPI_VDEC_H264;
+	inst->vpu.ctx = ctx;
+
+	err = vpu_dec_init(&inst->vpu);
+	if (err) {
+		mtk_vcodec_err(inst, "vdec_h264 init err=%d", err);
+		goto error_free_inst;
+	}
+
+	inst->vsi = (struct vdec_h264_vsi *)inst->vpu.vsi;
+	err = allocate_predication_buf(inst);
+	if (err)
+		goto error_deinit;
+
+	mtk_vcodec_debug(inst, "H264 Instance >> %p", inst);
+
+	ctx->drv_handle = inst;
+	return 0;
+
+error_deinit:
+	vpu_dec_deinit(&inst->vpu);
+
+error_free_inst:
+	kfree(inst);
+	return err;
+}
+
+static void vdec_h264_deinit(void *h_vdec)
+{
+	struct vdec_h264_inst *inst = (struct vdec_h264_inst *)h_vdec;
+
+	mtk_vcodec_debug_enter(inst);
+
+	vpu_dec_deinit(&inst->vpu);
+	free_predication_buf(inst);
+	free_mv_buf(inst);
+
+	kfree(inst);
+}
+
+static int find_start_code(unsigned char *data, unsigned int data_sz)
+{
+	if (data_sz > 3 && data[0] == 0 && data[1] == 0 && data[2] == 1)
+		return 3;
+
+	if (data_sz > 4 && data[0] == 0 && data[1] == 0 && data[2] == 0 &&
+	    data[3] == 1)
+		return 4;
+
+	return -1;
+}
+
+static int vdec_h264_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
+			    struct vdec_fb *fb, bool *res_chg)
+{
+	struct vdec_h264_inst *inst = (struct vdec_h264_inst *)h_vdec;
+	struct vdec_vpu_inst *vpu = &inst->vpu;
+	int nal_start_idx = 0;
+	int err = 0;
+	unsigned int nal_start;
+	unsigned int nal_type;
+	unsigned char *buf;
+	unsigned int buf_sz;
+	unsigned int data[2];
+	uint64_t vdec_fb_va = (u64)(uintptr_t)fb;
+	uint64_t y_fb_dma = fb ? (u64)fb->base_y.dma_addr : 0;
+	uint64_t c_fb_dma = fb ? (u64)fb->base_c.dma_addr : 0;
+
+	mtk_vcodec_debug(inst, "+ [%d] FB y_dma=%llx c_dma=%llx va=%p",
+			 ++inst->num_nalu, y_fb_dma, c_fb_dma, fb);
+
+	/* bs NULL means flush decoder */
+	if (bs == NULL)
+		return vpu_dec_reset(vpu);
+
+	buf = (unsigned char *)bs->va;
+	buf_sz = bs->size;
+	nal_start_idx = find_start_code(buf, buf_sz);
+	if (nal_start_idx < 0) {
+		mtk_vcodec_err(inst, "invalid nal start code");
+		err = -EIO;
+		goto err_free_fb_out;
+	}
+
+	nal_start = buf[nal_start_idx];
+	nal_type = NAL_TYPE(buf[nal_start_idx]);
+	mtk_vcodec_debug(inst, "\n + NALU[%d] type %d +\n", inst->num_nalu,
+			 nal_type);
+
+	if (nal_type == NAL_H264_PPS) {
+		buf_sz -= nal_start_idx;
+		if (buf_sz > HDR_PARSING_BUF_SZ) {
+			err = -EILSEQ;
+			goto err_free_fb_out;
+		}
+		memcpy(inst->vsi->hdr_buf, buf + nal_start_idx, buf_sz);
+	}
+
+	inst->vsi->dec.bs_dma = (uint64_t)bs->dma_addr;
+	inst->vsi->dec.y_fb_dma = y_fb_dma;
+	inst->vsi->dec.c_fb_dma = c_fb_dma;
+	inst->vsi->dec.vdec_fb_va = vdec_fb_va;
+
+	data[0] = buf_sz;
+	data[1] = nal_start;
+	err = vpu_dec_start(vpu, data, 2);
+	if (err) {
+		if (err > 0 && (DEC_ERR_RET(err) == H264_ERR_NOT_VALID)) {
+			mtk_vcodec_err(inst, "- error bitstream - err = %d -",
+				       err);
+			err = -EIO;
+		}
+		goto err_free_fb_out;
+	}
+
+	*res_chg = inst->vsi->dec.resolution_changed;
+	if (*res_chg) {
+		struct vdec_pic_info pic;
+
+		mtk_vcodec_debug(inst, "- resolution changed -");
+		get_pic_info(inst, &pic);
+
+		if (inst->vsi->dec.realloc_mv_buf) {
+			err = alloc_mv_buf(inst, &pic);
+			if (err)
+				goto err_free_fb_out;
+		}
+	}
+
+	if (nal_type == NAL_NON_IDR_SLICE || nal_type == NAL_IDR_SLICE) {
+		/* wait decoder done interrupt */
+		err = mtk_vcodec_wait_for_done_ctx(inst->ctx,
+						   MTK_INST_IRQ_RECEIVED,
+						   WAIT_INTR_TIMEOUT_MS, 0);
+		if (err)
+			goto err_free_fb_out;
+
+		vpu_dec_end(vpu);
+	}
+
+	mtk_vcodec_debug(inst, "\n - NALU[%d] type=%d -\n", inst->num_nalu,
+			 nal_type);
+	return 0;
+
+err_free_fb_out:
+	put_fb_to_free(inst, fb);
+	mtk_vcodec_err(inst, "\n - NALU[%d] err=%d -\n", inst->num_nalu, err);
+	return err;
+}
+
+static void vdec_h264_get_fb(struct vdec_h264_inst *inst,
+			     struct h264_ring_fb_list *list,
+			     bool disp_list, struct vdec_fb **out_fb)
+{
+	struct vdec_fb *fb;
+
+	if (check_list_validity(inst, disp_list))
+		return;
+
+	if (list->count == 0) {
+		mtk_vcodec_debug(inst, "[FB] there is no %s fb",
+				 disp_list ? "disp" : "free");
+		*out_fb = NULL;
+		return;
+	}
+
+	fb = (struct vdec_fb *)
+		(uintptr_t)list->fb_list[list->read_idx].vdec_fb_va;
+	fb->status |= (disp_list ? FB_ST_DISPLAY : FB_ST_FREE);
+
+	*out_fb = fb;
+	mtk_vcodec_debug(inst, "[FB] get %s fb st=%d poc=%d %llx",
+			 disp_list ? "disp" : "free",
+			 fb->status, list->fb_list[list->read_idx].poc,
+			 list->fb_list[list->read_idx].vdec_fb_va);
+
+	list->read_idx = (list->read_idx == H264_MAX_FB_NUM - 1) ?
+			 0 : list->read_idx + 1;
+	list->count--;
+}
+
+static int vdec_h264_get_param(void *h_vdec, enum vdec_get_param_type type,
+			       void *out)
+{
+	struct vdec_h264_inst *inst = (struct vdec_h264_inst *)h_vdec;
+
+	switch (type) {
+	case GET_PARAM_DISP_FRAME_BUFFER:
+		vdec_h264_get_fb(inst, &inst->vsi->list_disp, true, out);
+		break;
+
+	case GET_PARAM_FREE_FRAME_BUFFER:
+		vdec_h264_get_fb(inst, &inst->vsi->list_free, false, out);
+		break;
+
+	case GET_PARAM_PIC_INFO:
+		get_pic_info(inst, out);
+		break;
+
+	case GET_PARAM_DPB_SIZE:
+		get_dpb_size(inst, out);
+		break;
+
+	case GET_PARAM_CROP_INFO:
+		get_crop_info(inst, out);
+		break;
+
+	default:
+		mtk_vcodec_err(inst, "invalid get parameter type=%d", type);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+const struct vdec_common_if vdec_h264_if = {
+	.init		= vdec_h264_init,
+	.decode		= vdec_h264_decode,
+	.get_param	= vdec_h264_get_param,
+	.deinit		= vdec_h264_deinit,
+};
diff --git a/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_req_common.c b/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_req_common.c
new file mode 100644
index 000000000..580ce979e
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_req_common.c
@@ -0,0 +1,322 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Yunfei Dong <yunfei.dong@mediatek.com>
+ */
+
+#include "vdec_h264_req_common.h"
+
+/* get used parameters for sps/pps */
+#define GET_MTK_VDEC_FLAG(cond, flag) \
+	{ dst_param->cond = ((src_param->flags & flag) ? (1) : (0)); }
+#define GET_MTK_VDEC_PARAM(param) \
+	{ dst_param->param = src_param->param; }
+
+void mtk_vdec_h264_get_ref_list(u8 *ref_list,
+				const struct v4l2_h264_reference *v4l2_ref_list,
+				int num_valid)
+{
+	u32 i;
+
+	/*
+	 * TODO The firmware does not support field decoding. Future
+	 * implementation must use v4l2_ref_list[i].fields to obtain
+	 * the reference field parity.
+	 */
+
+	for (i = 0; i < num_valid; i++)
+		ref_list[i] = v4l2_ref_list[i].index;
+
+	/*
+	 * The firmware expects unused reflist entries to have the value 0x20.
+	 */
+	memset(&ref_list[num_valid], 0x20, 32 - num_valid);
+}
+
+void *mtk_vdec_h264_get_ctrl_ptr(struct mtk_vcodec_ctx *ctx, int id)
+{
+	struct v4l2_ctrl *ctrl = v4l2_ctrl_find(&ctx->ctrl_hdl, id);
+
+	if (!ctrl)
+		return ERR_PTR(-EINVAL);
+
+	return ctrl->p_cur.p;
+}
+
+void mtk_vdec_h264_fill_dpb_info(struct mtk_vcodec_ctx *ctx,
+				 struct slice_api_h264_decode_param *decode_params,
+				 struct mtk_h264_dpb_info *h264_dpb_info)
+{
+	const struct slice_h264_dpb_entry *dpb;
+	struct vb2_queue *vq;
+	struct vb2_buffer *vb;
+	struct vb2_v4l2_buffer *vb2_v4l2;
+	int index;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+
+	for (index = 0; index < V4L2_H264_NUM_DPB_ENTRIES; index++) {
+		dpb = &decode_params->dpb[index];
+		if (!(dpb->flags & V4L2_H264_DPB_ENTRY_FLAG_ACTIVE)) {
+			h264_dpb_info[index].reference_flag = 0;
+			continue;
+		}
+
+		vb = vb2_find_buffer(vq, dpb->reference_ts);
+		if (!vb) {
+			dev_err(&ctx->dev->plat_dev->dev,
+				"Reference invalid: dpb_index(%d) reference_ts(%lld)",
+				index, dpb->reference_ts);
+			continue;
+		}
+
+		/* 1 for short term reference, 2 for long term reference */
+		if (!(dpb->flags & V4L2_H264_DPB_ENTRY_FLAG_LONG_TERM))
+			h264_dpb_info[index].reference_flag = 1;
+		else
+			h264_dpb_info[index].reference_flag = 2;
+
+		vb2_v4l2 = container_of(vb, struct vb2_v4l2_buffer, vb2_buf);
+		h264_dpb_info[index].field = vb2_v4l2->field;
+
+		h264_dpb_info[index].y_dma_addr =
+			vb2_dma_contig_plane_dma_addr(vb, 0);
+		if (ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 2)
+			h264_dpb_info[index].c_dma_addr =
+				vb2_dma_contig_plane_dma_addr(vb, 1);
+		else
+			h264_dpb_info[index].c_dma_addr =
+				h264_dpb_info[index].y_dma_addr +
+				ctx->picinfo.fb_sz[0];
+	}
+}
+
+void mtk_vdec_h264_copy_sps_params(struct mtk_h264_sps_param *dst_param,
+				   const struct v4l2_ctrl_h264_sps *src_param)
+{
+	GET_MTK_VDEC_PARAM(chroma_format_idc);
+	GET_MTK_VDEC_PARAM(bit_depth_luma_minus8);
+	GET_MTK_VDEC_PARAM(bit_depth_chroma_minus8);
+	GET_MTK_VDEC_PARAM(log2_max_frame_num_minus4);
+	GET_MTK_VDEC_PARAM(pic_order_cnt_type);
+	GET_MTK_VDEC_PARAM(log2_max_pic_order_cnt_lsb_minus4);
+	GET_MTK_VDEC_PARAM(max_num_ref_frames);
+	GET_MTK_VDEC_PARAM(pic_width_in_mbs_minus1);
+	GET_MTK_VDEC_PARAM(pic_height_in_map_units_minus1);
+
+	GET_MTK_VDEC_FLAG(separate_colour_plane_flag,
+			  V4L2_H264_SPS_FLAG_SEPARATE_COLOUR_PLANE);
+	GET_MTK_VDEC_FLAG(qpprime_y_zero_transform_bypass_flag,
+			  V4L2_H264_SPS_FLAG_QPPRIME_Y_ZERO_TRANSFORM_BYPASS);
+	GET_MTK_VDEC_FLAG(delta_pic_order_always_zero_flag,
+			  V4L2_H264_SPS_FLAG_DELTA_PIC_ORDER_ALWAYS_ZERO);
+	GET_MTK_VDEC_FLAG(frame_mbs_only_flag,
+			  V4L2_H264_SPS_FLAG_FRAME_MBS_ONLY);
+	GET_MTK_VDEC_FLAG(mb_adaptive_frame_field_flag,
+			  V4L2_H264_SPS_FLAG_MB_ADAPTIVE_FRAME_FIELD);
+	GET_MTK_VDEC_FLAG(direct_8x8_inference_flag,
+			  V4L2_H264_SPS_FLAG_DIRECT_8X8_INFERENCE);
+}
+
+void mtk_vdec_h264_copy_pps_params(struct mtk_h264_pps_param *dst_param,
+				   const struct v4l2_ctrl_h264_pps *src_param)
+{
+	GET_MTK_VDEC_PARAM(num_ref_idx_l0_default_active_minus1);
+	GET_MTK_VDEC_PARAM(num_ref_idx_l1_default_active_minus1);
+	GET_MTK_VDEC_PARAM(weighted_bipred_idc);
+	GET_MTK_VDEC_PARAM(pic_init_qp_minus26);
+	GET_MTK_VDEC_PARAM(chroma_qp_index_offset);
+	GET_MTK_VDEC_PARAM(second_chroma_qp_index_offset);
+
+	GET_MTK_VDEC_FLAG(entropy_coding_mode_flag,
+			  V4L2_H264_PPS_FLAG_ENTROPY_CODING_MODE);
+	GET_MTK_VDEC_FLAG(pic_order_present_flag,
+			  V4L2_H264_PPS_FLAG_BOTTOM_FIELD_PIC_ORDER_IN_FRAME_PRESENT);
+	GET_MTK_VDEC_FLAG(weighted_pred_flag,
+			  V4L2_H264_PPS_FLAG_WEIGHTED_PRED);
+	GET_MTK_VDEC_FLAG(deblocking_filter_control_present_flag,
+			  V4L2_H264_PPS_FLAG_DEBLOCKING_FILTER_CONTROL_PRESENT);
+	GET_MTK_VDEC_FLAG(constrained_intra_pred_flag,
+			  V4L2_H264_PPS_FLAG_CONSTRAINED_INTRA_PRED);
+	GET_MTK_VDEC_FLAG(redundant_pic_cnt_present_flag,
+			  V4L2_H264_PPS_FLAG_REDUNDANT_PIC_CNT_PRESENT);
+	GET_MTK_VDEC_FLAG(transform_8x8_mode_flag,
+			  V4L2_H264_PPS_FLAG_TRANSFORM_8X8_MODE);
+	GET_MTK_VDEC_FLAG(scaling_matrix_present_flag,
+			  V4L2_H264_PPS_FLAG_SCALING_MATRIX_PRESENT);
+}
+
+void mtk_vdec_h264_copy_slice_hd_params(struct mtk_h264_slice_hd_param *dst_param,
+					const struct v4l2_ctrl_h264_slice_params *src_param,
+					const struct v4l2_ctrl_h264_decode_params *dec_param)
+{
+	int temp;
+
+	GET_MTK_VDEC_PARAM(first_mb_in_slice);
+	GET_MTK_VDEC_PARAM(slice_type);
+	GET_MTK_VDEC_PARAM(cabac_init_idc);
+	GET_MTK_VDEC_PARAM(slice_qp_delta);
+	GET_MTK_VDEC_PARAM(disable_deblocking_filter_idc);
+	GET_MTK_VDEC_PARAM(slice_alpha_c0_offset_div2);
+	GET_MTK_VDEC_PARAM(slice_beta_offset_div2);
+	GET_MTK_VDEC_PARAM(num_ref_idx_l0_active_minus1);
+	GET_MTK_VDEC_PARAM(num_ref_idx_l1_active_minus1);
+
+	dst_param->frame_num = dec_param->frame_num;
+	dst_param->pic_order_cnt_lsb = dec_param->pic_order_cnt_lsb;
+
+	dst_param->delta_pic_order_cnt_bottom =
+		dec_param->delta_pic_order_cnt_bottom;
+	dst_param->delta_pic_order_cnt0 =
+		dec_param->delta_pic_order_cnt0;
+	dst_param->delta_pic_order_cnt1 =
+		dec_param->delta_pic_order_cnt1;
+
+	temp = dec_param->flags & V4L2_H264_DECODE_PARAM_FLAG_FIELD_PIC;
+	dst_param->field_pic_flag = temp ? 1 : 0;
+
+	temp = dec_param->flags & V4L2_H264_DECODE_PARAM_FLAG_BOTTOM_FIELD;
+	dst_param->bottom_field_flag = temp ? 1 : 0;
+
+	GET_MTK_VDEC_FLAG(direct_spatial_mv_pred_flag,
+			  V4L2_H264_SLICE_FLAG_DIRECT_SPATIAL_MV_PRED);
+}
+
+void mtk_vdec_h264_copy_scaling_matrix(struct slice_api_h264_scaling_matrix *dst_matrix,
+				       const struct v4l2_ctrl_h264_scaling_matrix *src_matrix)
+{
+	memcpy(dst_matrix->scaling_list_4x4, src_matrix->scaling_list_4x4,
+	       sizeof(dst_matrix->scaling_list_4x4));
+
+	memcpy(dst_matrix->scaling_list_8x8, src_matrix->scaling_list_8x8,
+	       sizeof(dst_matrix->scaling_list_8x8));
+}
+
+void
+mtk_vdec_h264_copy_decode_params(struct slice_api_h264_decode_param *dst_params,
+				 const struct v4l2_ctrl_h264_decode_params *src_params,
+				 const struct v4l2_h264_dpb_entry dpb[V4L2_H264_NUM_DPB_ENTRIES])
+{
+	struct slice_h264_dpb_entry *dst_entry;
+	const struct v4l2_h264_dpb_entry *src_entry;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dst_params->dpb); i++) {
+		dst_entry = &dst_params->dpb[i];
+		src_entry = &dpb[i];
+
+		dst_entry->reference_ts = src_entry->reference_ts;
+		dst_entry->frame_num = src_entry->frame_num;
+		dst_entry->pic_num = src_entry->pic_num;
+		dst_entry->top_field_order_cnt = src_entry->top_field_order_cnt;
+		dst_entry->bottom_field_order_cnt =
+			src_entry->bottom_field_order_cnt;
+		dst_entry->flags = src_entry->flags;
+	}
+
+	/* num_slices is a leftover from the old H.264 support and is ignored
+	 * by the firmware.
+	 */
+	dst_params->num_slices = 0;
+	dst_params->nal_ref_idc = src_params->nal_ref_idc;
+	dst_params->top_field_order_cnt = src_params->top_field_order_cnt;
+	dst_params->bottom_field_order_cnt = src_params->bottom_field_order_cnt;
+	dst_params->flags = src_params->flags;
+}
+
+static bool mtk_vdec_h264_dpb_entry_match(const struct v4l2_h264_dpb_entry *a,
+					  const struct v4l2_h264_dpb_entry *b)
+{
+	return a->top_field_order_cnt == b->top_field_order_cnt &&
+	       a->bottom_field_order_cnt == b->bottom_field_order_cnt;
+}
+
+/*
+ * Move DPB entries of dec_param that refer to a frame already existing in dpb
+ * into the already existing slot in dpb, and move other entries into new slots.
+ *
+ * This function is an adaptation of the similarly-named function in
+ * hantro_h264.c.
+ */
+void mtk_vdec_h264_update_dpb(const struct v4l2_ctrl_h264_decode_params *dec_param,
+			      struct v4l2_h264_dpb_entry *dpb)
+{
+	DECLARE_BITMAP(new, ARRAY_SIZE(dec_param->dpb)) = { 0, };
+	DECLARE_BITMAP(in_use, ARRAY_SIZE(dec_param->dpb)) = { 0, };
+	DECLARE_BITMAP(used, ARRAY_SIZE(dec_param->dpb)) = { 0, };
+	unsigned int i, j;
+
+	/* Disable all entries by default, and mark the ones in use. */
+	for (i = 0; i < ARRAY_SIZE(dec_param->dpb); i++) {
+		if (dpb[i].flags & V4L2_H264_DPB_ENTRY_FLAG_ACTIVE)
+			set_bit(i, in_use);
+		dpb[i].flags &= ~V4L2_H264_DPB_ENTRY_FLAG_ACTIVE;
+	}
+
+	/* Try to match new DPB entries with existing ones by their POCs. */
+	for (i = 0; i < ARRAY_SIZE(dec_param->dpb); i++) {
+		const struct v4l2_h264_dpb_entry *ndpb = &dec_param->dpb[i];
+
+		if (!(ndpb->flags & V4L2_H264_DPB_ENTRY_FLAG_ACTIVE))
+			continue;
+
+		/*
+		 * To cut off some comparisons, iterate only on target DPB
+		 * entries were already used.
+		 */
+		for_each_set_bit(j, in_use, ARRAY_SIZE(dec_param->dpb)) {
+			struct v4l2_h264_dpb_entry *cdpb;
+
+			cdpb = &dpb[j];
+			if (!mtk_vdec_h264_dpb_entry_match(cdpb, ndpb))
+				continue;
+
+			*cdpb = *ndpb;
+			set_bit(j, used);
+			/* Don't reiterate on this one. */
+			clear_bit(j, in_use);
+			break;
+		}
+
+		if (j == ARRAY_SIZE(dec_param->dpb))
+			set_bit(i, new);
+	}
+
+	/* For entries that could not be matched, use remaining free slots. */
+	for_each_set_bit(i, new, ARRAY_SIZE(dec_param->dpb)) {
+		const struct v4l2_h264_dpb_entry *ndpb = &dec_param->dpb[i];
+		struct v4l2_h264_dpb_entry *cdpb;
+
+		/*
+		 * Both arrays are of the same sizes, so there is no way
+		 * we can end up with no space in target array, unless
+		 * something is buggy.
+		 */
+		j = find_first_zero_bit(used, ARRAY_SIZE(dec_param->dpb));
+		if (WARN_ON(j >= ARRAY_SIZE(dec_param->dpb)))
+			return;
+
+		cdpb = &dpb[j];
+		*cdpb = *ndpb;
+		set_bit(j, used);
+	}
+}
+
+unsigned int mtk_vdec_h264_get_mv_buf_size(unsigned int width, unsigned int height)
+{
+	int unit_size = (width / MB_UNIT_LEN) * (height / MB_UNIT_LEN) + 8;
+
+	return HW_MB_STORE_SZ * unit_size;
+}
+
+int mtk_vdec_h264_find_start_code(unsigned char *data, unsigned int data_sz)
+{
+	if (data_sz > 3 && data[0] == 0 && data[1] == 0 && data[2] == 1)
+		return 3;
+
+	if (data_sz > 4 && data[0] == 0 && data[1] == 0 && data[2] == 0 &&
+	    data[3] == 1)
+		return 4;
+
+	return -1;
+}
diff --git a/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_req_common.h b/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_req_common.h
new file mode 100644
index 000000000..53d0a7c96
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_req_common.h
@@ -0,0 +1,277 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Yunfei Dong <yunfei.dong@mediatek.com>
+ */
+
+#ifndef _VDEC_H264_REQ_COMMON_H_
+#define _VDEC_H264_REQ_COMMON_H_
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <media/v4l2-h264.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "../mtk_vcodec_drv.h"
+
+#define NAL_NON_IDR_SLICE			0x01
+#define NAL_IDR_SLICE				0x05
+#define NAL_TYPE(value)				((value) & 0x1F)
+
+#define BUF_PREDICTION_SZ			(64 * 4096)
+#define MB_UNIT_LEN				16
+
+/* motion vector size (bytes) for every macro block */
+#define HW_MB_STORE_SZ				64
+
+#define H264_MAX_MV_NUM				32
+
+/**
+ * struct mtk_h264_dpb_info  - h264 dpb information
+ *
+ * @y_dma_addr:	Y bitstream physical address
+ * @c_dma_addr:	CbCr bitstream physical address
+ * @reference_flag:	reference picture flag (short/long term reference picture)
+ * @field:		field picture flag
+ */
+struct mtk_h264_dpb_info {
+	dma_addr_t y_dma_addr;
+	dma_addr_t c_dma_addr;
+	int reference_flag;
+	int field;
+};
+
+/*
+ * struct mtk_h264_sps_param  - parameters for sps
+ */
+struct mtk_h264_sps_param {
+	unsigned char chroma_format_idc;
+	unsigned char bit_depth_luma_minus8;
+	unsigned char bit_depth_chroma_minus8;
+	unsigned char log2_max_frame_num_minus4;
+	unsigned char pic_order_cnt_type;
+	unsigned char log2_max_pic_order_cnt_lsb_minus4;
+	unsigned char max_num_ref_frames;
+	unsigned char separate_colour_plane_flag;
+	unsigned short pic_width_in_mbs_minus1;
+	unsigned short pic_height_in_map_units_minus1;
+	unsigned int max_frame_nums;
+	unsigned char qpprime_y_zero_transform_bypass_flag;
+	unsigned char delta_pic_order_always_zero_flag;
+	unsigned char frame_mbs_only_flag;
+	unsigned char mb_adaptive_frame_field_flag;
+	unsigned char direct_8x8_inference_flag;
+	unsigned char reserved[3];
+};
+
+/*
+ * struct mtk_h264_pps_param  - parameters for pps
+ */
+struct mtk_h264_pps_param {
+	unsigned char num_ref_idx_l0_default_active_minus1;
+	unsigned char num_ref_idx_l1_default_active_minus1;
+	unsigned char weighted_bipred_idc;
+	char pic_init_qp_minus26;
+	char chroma_qp_index_offset;
+	char second_chroma_qp_index_offset;
+	unsigned char entropy_coding_mode_flag;
+	unsigned char pic_order_present_flag;
+	unsigned char deblocking_filter_control_present_flag;
+	unsigned char constrained_intra_pred_flag;
+	unsigned char weighted_pred_flag;
+	unsigned char redundant_pic_cnt_present_flag;
+	unsigned char transform_8x8_mode_flag;
+	unsigned char scaling_matrix_present_flag;
+	unsigned char reserved[2];
+};
+
+/*
+ * struct mtk_h264_slice_hd_param  - parameters for slice header
+ */
+struct mtk_h264_slice_hd_param {
+	unsigned int first_mb_in_slice;
+	unsigned int field_pic_flag;
+	unsigned int slice_type;
+	unsigned int frame_num;
+	int pic_order_cnt_lsb;
+	int delta_pic_order_cnt_bottom;
+	unsigned int bottom_field_flag;
+	unsigned int direct_spatial_mv_pred_flag;
+	int delta_pic_order_cnt0;
+	int delta_pic_order_cnt1;
+	unsigned int cabac_init_idc;
+	int slice_qp_delta;
+	unsigned int disable_deblocking_filter_idc;
+	int slice_alpha_c0_offset_div2;
+	int slice_beta_offset_div2;
+	unsigned int num_ref_idx_l0_active_minus1;
+	unsigned int num_ref_idx_l1_active_minus1;
+	unsigned int reserved;
+};
+
+/*
+ * struct slice_api_h264_scaling_matrix  - parameters for scaling list
+ */
+struct slice_api_h264_scaling_matrix {
+	unsigned char scaling_list_4x4[6][16];
+	unsigned char scaling_list_8x8[6][64];
+};
+
+/*
+ * struct slice_h264_dpb_entry  - each dpb information
+ */
+struct slice_h264_dpb_entry {
+	unsigned long long reference_ts;
+	unsigned short frame_num;
+	unsigned short pic_num;
+	/* Note that field is indicated by v4l2_buffer.field */
+	int top_field_order_cnt;
+	int bottom_field_order_cnt;
+	unsigned int flags;
+};
+
+/*
+ * struct slice_api_h264_decode_param - parameters for decode.
+ */
+struct slice_api_h264_decode_param {
+	struct slice_h264_dpb_entry dpb[V4L2_H264_NUM_DPB_ENTRIES];
+	unsigned short num_slices;
+	unsigned short nal_ref_idc;
+	unsigned char ref_pic_list_p0[32];
+	unsigned char ref_pic_list_b0[32];
+	unsigned char ref_pic_list_b1[32];
+	int top_field_order_cnt;
+	int bottom_field_order_cnt;
+	unsigned int flags;
+};
+
+/**
+ * struct h264_fb - h264 decode frame buffer information
+ *
+ * @vdec_fb_va:	virtual address of struct vdec_fb
+ * @y_fb_dma:		dma address of Y frame buffer (luma)
+ * @c_fb_dma:		dma address of C frame buffer (chroma)
+ * @poc:		picture order count of frame buffer
+ * @reserved:		for 8 bytes alignment
+ */
+struct h264_fb {
+	u64 vdec_fb_va;
+	u64 y_fb_dma;
+	u64 c_fb_dma;
+	s32 poc;
+	u32 reserved;
+};
+
+/**
+ * mtk_vdec_h264_get_ref_list - translate V4L2 reference list
+ *
+ * @ref_list:		Mediatek reference picture list
+ * @v4l2_ref_list:	V4L2 reference picture list
+ * @num_valid:		used reference number
+ */
+void mtk_vdec_h264_get_ref_list(u8 *ref_list,
+				const struct v4l2_h264_reference *v4l2_ref_list,
+				int num_valid);
+
+/**
+ * mtk_vdec_h264_get_ctrl_ptr - get each CID contrl address.
+ *
+ * @ctx:	v4l2 ctx
+ * @id:	CID control ID
+ *
+ * Return: returns CID ctrl address.
+ */
+void *mtk_vdec_h264_get_ctrl_ptr(struct mtk_vcodec_ctx *ctx, int id);
+
+/**
+ * mtk_vdec_h264_fill_dpb_info - get each CID contrl address.
+ *
+ * @ctx:		v4l2 ctx
+ * @decode_params:	slice decode params
+ * @h264_dpb_info:	dpb buffer information
+ */
+void mtk_vdec_h264_fill_dpb_info(struct mtk_vcodec_ctx *ctx,
+				 struct slice_api_h264_decode_param *decode_params,
+				 struct mtk_h264_dpb_info *h264_dpb_info);
+
+/**
+ * mtk_vdec_h264_copy_sps_params - get sps params.
+ *
+ * @dst_param:	sps params for hw decoder
+ * @src_param:	sps params from user driver
+ */
+void mtk_vdec_h264_copy_sps_params(struct mtk_h264_sps_param *dst_param,
+				   const struct v4l2_ctrl_h264_sps *src_param);
+
+/**
+ * mtk_vdec_h264_copy_pps_params - get pps params.
+ *
+ * @dst_param:	pps params for hw decoder
+ * @src_param:	pps params from user driver
+ */
+void mtk_vdec_h264_copy_pps_params(struct mtk_h264_pps_param *dst_param,
+				   const struct v4l2_ctrl_h264_pps *src_param);
+
+/**
+ * mtk_vdec_h264_copy_slice_hd_params - get slice header params.
+ *
+ * @dst_param:	slice params for hw decoder
+ * @src_param:	slice params from user driver
+ * @dec_param:	decode params from user driver
+ */
+void mtk_vdec_h264_copy_slice_hd_params(struct mtk_h264_slice_hd_param *dst_param,
+					const struct v4l2_ctrl_h264_slice_params *src_param,
+					const struct v4l2_ctrl_h264_decode_params *dec_param);
+
+/**
+ * mtk_vdec_h264_copy_scaling_matrix - get each CID contrl address.
+ *
+ * @dst_matrix:	scaling list params for hw decoder
+ * @src_matrix:	scaling list params from user driver
+ */
+void mtk_vdec_h264_copy_scaling_matrix(struct slice_api_h264_scaling_matrix *dst_matrix,
+				       const struct v4l2_ctrl_h264_scaling_matrix *src_matrix);
+
+/**
+ * mtk_vdec_h264_copy_decode_params - get decode params.
+ *
+ * @dst_params:	dst params for hw decoder
+ * @src_params:	decode params from user driver
+ * @dpb:		dpb information
+ */
+void
+mtk_vdec_h264_copy_decode_params(struct slice_api_h264_decode_param *dst_params,
+				 const struct v4l2_ctrl_h264_decode_params *src_params,
+				 const struct v4l2_h264_dpb_entry dpb[V4L2_H264_NUM_DPB_ENTRIES]);
+
+/**
+ * mtk_vdec_h264_update_dpb - updata dpb list.
+ *
+ * @dec_param:	v4l2 control decode params
+ * @dpb:	dpb entry informaton
+ */
+void mtk_vdec_h264_update_dpb(const struct v4l2_ctrl_h264_decode_params *dec_param,
+			      struct v4l2_h264_dpb_entry *dpb);
+
+/**
+ * mtk_vdec_h264_find_start_code - find h264 start code using sofeware.
+ *
+ * @data:	input buffer address
+ * @data_sz:	input buffer size
+ *
+ * Return: returns start code position.
+ */
+int mtk_vdec_h264_find_start_code(unsigned char *data, unsigned int data_sz);
+
+/**
+ * mtk_vdec_h264_get_mv_buf_size - get mv buffer size.
+ *
+ * @width:	picture width
+ * @height:	picture height
+ *
+ * Return: returns mv buffer size.
+ */
+unsigned int mtk_vdec_h264_get_mv_buf_size(unsigned int width, unsigned int height);
+
+#endif
diff --git a/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_req_if.c b/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_req_if.c
new file mode 100644
index 000000000..4bc05ab5a
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_req_if.c
@@ -0,0 +1,449 @@
+// SPDX-License-Identifier: GPL-2.0
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-h264.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "../mtk_vcodec_util.h"
+#include "../mtk_vcodec_dec.h"
+#include "../mtk_vcodec_intr.h"
+#include "../vdec_drv_base.h"
+#include "../vdec_drv_if.h"
+#include "../vdec_vpu_if.h"
+#include "vdec_h264_req_common.h"
+
+/*
+ * struct mtk_h264_dec_slice_param  - parameters for decode current frame
+ */
+struct mtk_h264_dec_slice_param {
+	struct mtk_h264_sps_param			sps;
+	struct mtk_h264_pps_param			pps;
+	struct slice_api_h264_scaling_matrix		scaling_matrix;
+	struct slice_api_h264_decode_param		decode_params;
+	struct mtk_h264_dpb_info h264_dpb_info[16];
+};
+
+/**
+ * struct vdec_h264_dec_info - decode information
+ * @dpb_sz		: decoding picture buffer size
+ * @resolution_changed  : resoltion change happen
+ * @realloc_mv_buf	: flag to notify driver to re-allocate mv buffer
+ * @cap_num_planes	: number planes of capture buffer
+ * @bs_dma		: Input bit-stream buffer dma address
+ * @y_fb_dma		: Y frame buffer dma address
+ * @c_fb_dma		: C frame buffer dma address
+ * @vdec_fb_va		: VDEC frame buffer struct virtual address
+ */
+struct vdec_h264_dec_info {
+	u32 dpb_sz;
+	u32 resolution_changed;
+	u32 realloc_mv_buf;
+	u32 cap_num_planes;
+	u64 bs_dma;
+	u64 y_fb_dma;
+	u64 c_fb_dma;
+	u64 vdec_fb_va;
+};
+
+/**
+ * struct vdec_h264_vsi - shared memory for decode information exchange
+ *                        between VPU and Host.
+ *                        The memory is allocated by VPU then mapping to Host
+ *                        in vpu_dec_init() and freed in vpu_dec_deinit()
+ *                        by VPU.
+ *                        AP-W/R : AP is writer/reader on this item
+ *                        VPU-W/R: VPU is write/reader on this item
+ * @pred_buf_dma : HW working predication buffer dma address (AP-W, VPU-R)
+ * @mv_buf_dma   : HW working motion vector buffer dma address (AP-W, VPU-R)
+ * @dec          : decode information (AP-R, VPU-W)
+ * @pic          : picture information (AP-R, VPU-W)
+ * @crop         : crop information (AP-R, VPU-W)
+ * @h264_slice_params : the parameters that hardware use to decode
+ */
+struct vdec_h264_vsi {
+	u64 pred_buf_dma;
+	u64 mv_buf_dma[H264_MAX_MV_NUM];
+	struct vdec_h264_dec_info dec;
+	struct vdec_pic_info pic;
+	struct v4l2_rect crop;
+	struct mtk_h264_dec_slice_param h264_slice_params;
+};
+
+/**
+ * struct vdec_h264_slice_inst - h264 decoder instance
+ * @num_nalu : how many nalus be decoded
+ * @ctx      : point to mtk_vcodec_ctx
+ * @pred_buf : HW working predication buffer
+ * @mv_buf   : HW working motion vector buffer
+ * @vpu      : VPU instance
+ * @vsi_ctx  : Local VSI data for this decoding context
+ * @h264_slice_param : the parameters that hardware use to decode
+ * @dpb : decoded picture buffer used to store reference buffer information
+ */
+struct vdec_h264_slice_inst {
+	unsigned int num_nalu;
+	struct mtk_vcodec_ctx *ctx;
+	struct mtk_vcodec_mem pred_buf;
+	struct mtk_vcodec_mem mv_buf[H264_MAX_MV_NUM];
+	struct vdec_vpu_inst vpu;
+	struct vdec_h264_vsi vsi_ctx;
+	struct mtk_h264_dec_slice_param h264_slice_param;
+
+	struct v4l2_h264_dpb_entry dpb[16];
+};
+
+static int get_vdec_decode_parameters(struct vdec_h264_slice_inst *inst)
+{
+	const struct v4l2_ctrl_h264_decode_params *dec_params;
+	const struct v4l2_ctrl_h264_sps *sps;
+	const struct v4l2_ctrl_h264_pps *pps;
+	const struct v4l2_ctrl_h264_scaling_matrix *scaling_matrix;
+	struct mtk_h264_dec_slice_param *slice_param = &inst->h264_slice_param;
+	struct v4l2_h264_reflist_builder reflist_builder;
+	struct v4l2_h264_reference v4l2_p0_reflist[V4L2_H264_REF_LIST_LEN];
+	struct v4l2_h264_reference v4l2_b0_reflist[V4L2_H264_REF_LIST_LEN];
+	struct v4l2_h264_reference v4l2_b1_reflist[V4L2_H264_REF_LIST_LEN];
+	u8 *p0_reflist = slice_param->decode_params.ref_pic_list_p0;
+	u8 *b0_reflist = slice_param->decode_params.ref_pic_list_b0;
+	u8 *b1_reflist = slice_param->decode_params.ref_pic_list_b1;
+
+	dec_params =
+		mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_DECODE_PARAMS);
+	if (IS_ERR(dec_params))
+		return PTR_ERR(dec_params);
+
+	sps = mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_SPS);
+	if (IS_ERR(sps))
+		return PTR_ERR(sps);
+
+	pps = mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_PPS);
+	if (IS_ERR(pps))
+		return PTR_ERR(pps);
+
+	scaling_matrix =
+		mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_SCALING_MATRIX);
+	if (IS_ERR(scaling_matrix))
+		return PTR_ERR(scaling_matrix);
+
+	mtk_vdec_h264_update_dpb(dec_params, inst->dpb);
+
+	mtk_vdec_h264_copy_sps_params(&slice_param->sps, sps);
+	mtk_vdec_h264_copy_pps_params(&slice_param->pps, pps);
+	mtk_vdec_h264_copy_scaling_matrix(&slice_param->scaling_matrix, scaling_matrix);
+	mtk_vdec_h264_copy_decode_params(&slice_param->decode_params,
+					 dec_params, inst->dpb);
+	mtk_vdec_h264_fill_dpb_info(inst->ctx, &slice_param->decode_params,
+				    slice_param->h264_dpb_info);
+
+	/* Build the reference lists */
+	v4l2_h264_init_reflist_builder(&reflist_builder, dec_params, sps,
+				       inst->dpb);
+	v4l2_h264_build_p_ref_list(&reflist_builder, v4l2_p0_reflist);
+	v4l2_h264_build_b_ref_lists(&reflist_builder, v4l2_b0_reflist,
+				    v4l2_b1_reflist);
+
+	/* Adapt the built lists to the firmware's expectations */
+	mtk_vdec_h264_get_ref_list(p0_reflist, v4l2_p0_reflist, reflist_builder.num_valid);
+	mtk_vdec_h264_get_ref_list(b0_reflist, v4l2_b0_reflist, reflist_builder.num_valid);
+	mtk_vdec_h264_get_ref_list(b1_reflist, v4l2_b1_reflist, reflist_builder.num_valid);
+
+	memcpy(&inst->vsi_ctx.h264_slice_params, slice_param,
+	       sizeof(inst->vsi_ctx.h264_slice_params));
+
+	return 0;
+}
+
+static int allocate_predication_buf(struct vdec_h264_slice_inst *inst)
+{
+	int err;
+
+	inst->pred_buf.size = BUF_PREDICTION_SZ;
+	err = mtk_vcodec_mem_alloc(inst->ctx, &inst->pred_buf);
+	if (err) {
+		mtk_vcodec_err(inst, "failed to allocate ppl buf");
+		return err;
+	}
+
+	inst->vsi_ctx.pred_buf_dma = inst->pred_buf.dma_addr;
+	return 0;
+}
+
+static void free_predication_buf(struct vdec_h264_slice_inst *inst)
+{
+	struct mtk_vcodec_mem *mem = &inst->pred_buf;
+
+	mtk_vcodec_debug_enter(inst);
+
+	inst->vsi_ctx.pred_buf_dma = 0;
+	if (mem->va)
+		mtk_vcodec_mem_free(inst->ctx, mem);
+}
+
+static int alloc_mv_buf(struct vdec_h264_slice_inst *inst,
+			struct vdec_pic_info *pic)
+{
+	int i;
+	int err;
+	struct mtk_vcodec_mem *mem = NULL;
+	unsigned int buf_sz = mtk_vdec_h264_get_mv_buf_size(pic->buf_w, pic->buf_h);
+
+	mtk_v4l2_debug(3, "size = 0x%x", buf_sz);
+	for (i = 0; i < H264_MAX_MV_NUM; i++) {
+		mem = &inst->mv_buf[i];
+		if (mem->va)
+			mtk_vcodec_mem_free(inst->ctx, mem);
+		mem->size = buf_sz;
+		err = mtk_vcodec_mem_alloc(inst->ctx, mem);
+		if (err) {
+			mtk_vcodec_err(inst, "failed to allocate mv buf");
+			return err;
+		}
+		inst->vsi_ctx.mv_buf_dma[i] = mem->dma_addr;
+	}
+
+	return 0;
+}
+
+static void free_mv_buf(struct vdec_h264_slice_inst *inst)
+{
+	int i;
+	struct mtk_vcodec_mem *mem;
+
+	for (i = 0; i < H264_MAX_MV_NUM; i++) {
+		inst->vsi_ctx.mv_buf_dma[i] = 0;
+		mem = &inst->mv_buf[i];
+		if (mem->va)
+			mtk_vcodec_mem_free(inst->ctx, mem);
+	}
+}
+
+static void get_pic_info(struct vdec_h264_slice_inst *inst,
+			 struct vdec_pic_info *pic)
+{
+	struct mtk_vcodec_ctx *ctx = inst->ctx;
+
+	ctx->picinfo.buf_w = ALIGN(ctx->picinfo.pic_w, VCODEC_DEC_ALIGNED_64);
+	ctx->picinfo.buf_h = ALIGN(ctx->picinfo.pic_h, VCODEC_DEC_ALIGNED_64);
+	ctx->picinfo.fb_sz[0] = ctx->picinfo.buf_w * ctx->picinfo.buf_h;
+	ctx->picinfo.fb_sz[1] = ctx->picinfo.fb_sz[0] >> 1;
+	inst->vsi_ctx.dec.cap_num_planes =
+		ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes;
+
+	*pic = ctx->picinfo;
+	mtk_vcodec_debug(inst, "pic(%d, %d), buf(%d, %d)",
+			 ctx->picinfo.pic_w, ctx->picinfo.pic_h,
+			 ctx->picinfo.buf_w, ctx->picinfo.buf_h);
+	mtk_vcodec_debug(inst, "Y/C(%d, %d)", ctx->picinfo.fb_sz[0],
+			 ctx->picinfo.fb_sz[1]);
+
+	if (ctx->last_decoded_picinfo.pic_w != ctx->picinfo.pic_w ||
+	    ctx->last_decoded_picinfo.pic_h != ctx->picinfo.pic_h) {
+		inst->vsi_ctx.dec.resolution_changed = true;
+		if (ctx->last_decoded_picinfo.buf_w != ctx->picinfo.buf_w ||
+		    ctx->last_decoded_picinfo.buf_h != ctx->picinfo.buf_h)
+			inst->vsi_ctx.dec.realloc_mv_buf = true;
+
+		mtk_v4l2_debug(1, "ResChg: (%d %d) : old(%d, %d) -> new(%d, %d)",
+			       inst->vsi_ctx.dec.resolution_changed,
+			       inst->vsi_ctx.dec.realloc_mv_buf,
+			       ctx->last_decoded_picinfo.pic_w,
+			       ctx->last_decoded_picinfo.pic_h,
+			       ctx->picinfo.pic_w, ctx->picinfo.pic_h);
+	}
+}
+
+static void get_crop_info(struct vdec_h264_slice_inst *inst, struct v4l2_rect *cr)
+{
+	cr->left = inst->vsi_ctx.crop.left;
+	cr->top = inst->vsi_ctx.crop.top;
+	cr->width = inst->vsi_ctx.crop.width;
+	cr->height = inst->vsi_ctx.crop.height;
+
+	mtk_vcodec_debug(inst, "l=%d, t=%d, w=%d, h=%d",
+			 cr->left, cr->top, cr->width, cr->height);
+}
+
+static void get_dpb_size(struct vdec_h264_slice_inst *inst, unsigned int *dpb_sz)
+{
+	*dpb_sz = inst->vsi_ctx.dec.dpb_sz;
+	mtk_vcodec_debug(inst, "sz=%d", *dpb_sz);
+}
+
+static int vdec_h264_slice_init(struct mtk_vcodec_ctx *ctx)
+{
+	struct vdec_h264_slice_inst *inst;
+	int err;
+
+	inst = kzalloc(sizeof(*inst), GFP_KERNEL);
+	if (!inst)
+		return -ENOMEM;
+
+	inst->ctx = ctx;
+
+	inst->vpu.id = SCP_IPI_VDEC_H264;
+	inst->vpu.ctx = ctx;
+
+	err = vpu_dec_init(&inst->vpu);
+	if (err) {
+		mtk_vcodec_err(inst, "vdec_h264 init err=%d", err);
+		goto error_free_inst;
+	}
+
+	memcpy(&inst->vsi_ctx, inst->vpu.vsi, sizeof(inst->vsi_ctx));
+	inst->vsi_ctx.dec.resolution_changed = true;
+	inst->vsi_ctx.dec.realloc_mv_buf = true;
+
+	err = allocate_predication_buf(inst);
+	if (err)
+		goto error_deinit;
+
+	mtk_vcodec_debug(inst, "struct size = %zu,%zu,%zu,%zu\n",
+			 sizeof(struct mtk_h264_sps_param),
+			 sizeof(struct mtk_h264_pps_param),
+			 sizeof(struct mtk_h264_dec_slice_param),
+			 sizeof(struct mtk_h264_dpb_info));
+
+	mtk_vcodec_debug(inst, "H264 Instance >> %p", inst);
+
+	ctx->drv_handle = inst;
+	return 0;
+
+error_deinit:
+	vpu_dec_deinit(&inst->vpu);
+
+error_free_inst:
+	kfree(inst);
+	return err;
+}
+
+static void vdec_h264_slice_deinit(void *h_vdec)
+{
+	struct vdec_h264_slice_inst *inst = h_vdec;
+
+	mtk_vcodec_debug_enter(inst);
+
+	vpu_dec_deinit(&inst->vpu);
+	free_predication_buf(inst);
+	free_mv_buf(inst);
+
+	kfree(inst);
+}
+
+static int vdec_h264_slice_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
+				  struct vdec_fb *unused, bool *res_chg)
+{
+	struct vdec_h264_slice_inst *inst = h_vdec;
+	const struct v4l2_ctrl_h264_decode_params *dec_params =
+		mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_DECODE_PARAMS);
+	struct vdec_vpu_inst *vpu = &inst->vpu;
+	struct mtk_video_dec_buf *src_buf_info;
+	struct mtk_video_dec_buf *dst_buf_info;
+	struct vdec_fb *fb;
+	u32 data[2];
+	u64 y_fb_dma;
+	u64 c_fb_dma;
+	int err;
+
+	inst->num_nalu++;
+	/* bs NULL means flush decoder */
+	if (!bs)
+		return vpu_dec_reset(vpu);
+
+	fb = inst->ctx->dev->vdec_pdata->get_cap_buffer(inst->ctx);
+	src_buf_info = container_of(bs, struct mtk_video_dec_buf, bs_buffer);
+	dst_buf_info = container_of(fb, struct mtk_video_dec_buf, frame_buffer);
+
+	y_fb_dma = fb ? (u64)fb->base_y.dma_addr : 0;
+	c_fb_dma = fb ? (u64)fb->base_c.dma_addr : 0;
+
+	mtk_vcodec_debug(inst, "+ [%d] FB y_dma=%llx c_dma=%llx va=%p",
+			 inst->num_nalu, y_fb_dma, c_fb_dma, fb);
+
+	inst->vsi_ctx.dec.bs_dma = (uint64_t)bs->dma_addr;
+	inst->vsi_ctx.dec.y_fb_dma = y_fb_dma;
+	inst->vsi_ctx.dec.c_fb_dma = c_fb_dma;
+	inst->vsi_ctx.dec.vdec_fb_va = (u64)(uintptr_t)fb;
+
+	v4l2_m2m_buf_copy_metadata(&src_buf_info->m2m_buf.vb,
+				   &dst_buf_info->m2m_buf.vb, true);
+	err = get_vdec_decode_parameters(inst);
+	if (err)
+		goto err_free_fb_out;
+
+	data[0] = bs->size;
+	/*
+	 * Reconstruct the first byte of the NAL unit, as the firmware requests
+	 * that information to be passed even though it is present in the stream
+	 * itself...
+	 */
+	data[1] = (dec_params->nal_ref_idc << 5) |
+		  ((dec_params->flags & V4L2_H264_DECODE_PARAM_FLAG_IDR_PIC)
+			? 0x5 : 0x1);
+
+	*res_chg = inst->vsi_ctx.dec.resolution_changed;
+	if (*res_chg) {
+		mtk_vcodec_debug(inst, "- resolution changed -");
+		if (inst->vsi_ctx.dec.realloc_mv_buf) {
+			err = alloc_mv_buf(inst, &inst->ctx->picinfo);
+			inst->vsi_ctx.dec.realloc_mv_buf = false;
+			if (err)
+				goto err_free_fb_out;
+		}
+		*res_chg = false;
+	}
+
+	memcpy(inst->vpu.vsi, &inst->vsi_ctx, sizeof(inst->vsi_ctx));
+	err = vpu_dec_start(vpu, data, 2);
+	if (err)
+		goto err_free_fb_out;
+
+	/* wait decoder done interrupt */
+	err = mtk_vcodec_wait_for_done_ctx(inst->ctx,
+					   MTK_INST_IRQ_RECEIVED,
+					   WAIT_INTR_TIMEOUT_MS, 0);
+	if (err)
+		goto err_free_fb_out;
+	vpu_dec_end(vpu);
+
+	memcpy(&inst->vsi_ctx, inst->vpu.vsi, sizeof(inst->vsi_ctx));
+	mtk_vcodec_debug(inst, "\n - NALU[%d]", inst->num_nalu);
+	return 0;
+
+err_free_fb_out:
+	mtk_vcodec_err(inst, "\n - NALU[%d] err=%d -\n", inst->num_nalu, err);
+	return err;
+}
+
+static int vdec_h264_slice_get_param(void *h_vdec, enum vdec_get_param_type type, void *out)
+{
+	struct vdec_h264_slice_inst *inst = h_vdec;
+
+	switch (type) {
+	case GET_PARAM_PIC_INFO:
+		get_pic_info(inst, out);
+		break;
+
+	case GET_PARAM_DPB_SIZE:
+		get_dpb_size(inst, out);
+		break;
+
+	case GET_PARAM_CROP_INFO:
+		get_crop_info(inst, out);
+		break;
+
+	default:
+		mtk_vcodec_err(inst, "invalid get parameter type=%d", type);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+const struct vdec_common_if vdec_h264_slice_if = {
+	.init		= vdec_h264_slice_init,
+	.decode		= vdec_h264_slice_decode,
+	.get_param	= vdec_h264_slice_get_param,
+	.deinit		= vdec_h264_slice_deinit,
+};
diff --git a/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_req_multi_if.c b/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_req_multi_if.c
new file mode 100644
index 000000000..999ce7ee5
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec/vdec_h264_req_multi_if.c
@@ -0,0 +1,855 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2022 MediaTek Inc.
+ * Author: Yunfei Dong <yunfei.dong@mediatek.com>
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <media/v4l2-h264.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "../mtk_vcodec_util.h"
+#include "../mtk_vcodec_dec.h"
+#include "../mtk_vcodec_intr.h"
+#include "../vdec_drv_base.h"
+#include "../vdec_drv_if.h"
+#include "../vdec_vpu_if.h"
+#include "vdec_h264_req_common.h"
+
+/**
+ * enum vdec_h264_core_dec_err_type  - core decode error type
+ *
+ * @TRANS_BUFFER_FULL: trans buffer is full
+ * @SLICE_HEADER_FULL: slice header buffer is full
+ */
+enum vdec_h264_core_dec_err_type {
+	TRANS_BUFFER_FULL = 1,
+	SLICE_HEADER_FULL,
+};
+
+/**
+ * struct vdec_h264_slice_lat_dec_param  - parameters for decode current frame
+ *
+ * @sps:		h264 sps syntax parameters
+ * @pps:		h264 pps syntax parameters
+ * @slice_header:	h264 slice header syntax parameters
+ * @scaling_matrix:	h264 scaling list parameters
+ * @decode_params:	decoder parameters of each frame used for hardware decode
+ * @h264_dpb_info:	dpb reference list
+ */
+struct vdec_h264_slice_lat_dec_param {
+	struct mtk_h264_sps_param sps;
+	struct mtk_h264_pps_param pps;
+	struct mtk_h264_slice_hd_param slice_header;
+	struct slice_api_h264_scaling_matrix scaling_matrix;
+	struct slice_api_h264_decode_param decode_params;
+	struct mtk_h264_dpb_info h264_dpb_info[V4L2_H264_NUM_DPB_ENTRIES];
+};
+
+/**
+ * struct vdec_h264_slice_info - decode information
+ *
+ * @nal_info:		nal info of current picture
+ * @timeout:		Decode timeout: 1 timeout, 0 no timeount
+ * @bs_buf_size:	bitstream size
+ * @bs_buf_addr:	bitstream buffer dma address
+ * @y_fb_dma:		Y frame buffer dma address
+ * @c_fb_dma:		C frame buffer dma address
+ * @vdec_fb_va:	VDEC frame buffer struct virtual address
+ * @crc:		Used to check whether hardware's status is right
+ */
+struct vdec_h264_slice_info {
+	u16 nal_info;
+	u16 timeout;
+	u32 bs_buf_size;
+	u64 bs_buf_addr;
+	u64 y_fb_dma;
+	u64 c_fb_dma;
+	u64 vdec_fb_va;
+	u32 crc[8];
+};
+
+/**
+ * struct vdec_h264_slice_vsi - shared memory for decode information exchange
+ *        between SCP and Host.
+ *
+ * @wdma_err_addr:		wdma error dma address
+ * @wdma_start_addr:		wdma start dma address
+ * @wdma_end_addr:		wdma end dma address
+ * @slice_bc_start_addr:	slice bc start dma address
+ * @slice_bc_end_addr:		slice bc end dma address
+ * @row_info_start_addr:	row info start dma address
+ * @row_info_end_addr:		row info end dma address
+ * @trans_start:		trans start dma address
+ * @trans_end:			trans end dma address
+ * @wdma_end_addr_offset:	wdma end address offset
+ *
+ * @mv_buf_dma:		HW working motion vector buffer
+ *				dma address (AP-W, VPU-R)
+ * @dec:			decode information (AP-R, VPU-W)
+ * @h264_slice_params:		decode parameters for hw used
+ */
+struct vdec_h264_slice_vsi {
+	/* LAT dec addr */
+	u64 wdma_err_addr;
+	u64 wdma_start_addr;
+	u64 wdma_end_addr;
+	u64 slice_bc_start_addr;
+	u64 slice_bc_end_addr;
+	u64 row_info_start_addr;
+	u64 row_info_end_addr;
+	u64 trans_start;
+	u64 trans_end;
+	u64 wdma_end_addr_offset;
+
+	u64 mv_buf_dma[H264_MAX_MV_NUM];
+	struct vdec_h264_slice_info dec;
+	struct vdec_h264_slice_lat_dec_param h264_slice_params;
+};
+
+/**
+ * struct vdec_h264_slice_share_info - shared information used to exchange
+ *                                     message between lat and core
+ *
+ * @sps:		sequence header information from user space
+ * @dec_params:	decoder params from user space
+ * @h264_slice_params:	decoder params used for hardware
+ * @trans_start:	trans start dma address
+ * @trans_end:		trans end dma address
+ * @nal_info:		nal info of current picture
+ */
+struct vdec_h264_slice_share_info {
+	struct v4l2_ctrl_h264_sps sps;
+	struct v4l2_ctrl_h264_decode_params dec_params;
+	struct vdec_h264_slice_lat_dec_param h264_slice_params;
+	u64 trans_start;
+	u64 trans_end;
+	u16 nal_info;
+};
+
+/**
+ * struct vdec_h264_slice_inst - h264 decoder instance
+ *
+ * @slice_dec_num:	how many picture be decoded
+ * @ctx:		point to mtk_vcodec_ctx
+ * @pred_buf:		HW working predication buffer
+ * @mv_buf:		HW working motion vector buffer
+ * @vpu:		VPU instance
+ * @vsi:		vsi used for lat
+ * @vsi_core:		vsi used for core
+ *
+ * @vsi_ctx:		Local VSI data for this decoding context
+ * @h264_slice_param:	the parameters that hardware use to decode
+ *
+ * @resolution_changed:resolution changed
+ * @realloc_mv_buf:	reallocate mv buffer
+ * @cap_num_planes:	number of capture queue plane
+ *
+ * @dpb:		decoded picture buffer used to store reference
+ *			buffer information
+ *@is_field_bitstream:	is field bitstream
+ */
+struct vdec_h264_slice_inst {
+	unsigned int slice_dec_num;
+	struct mtk_vcodec_ctx *ctx;
+	struct mtk_vcodec_mem pred_buf;
+	struct mtk_vcodec_mem mv_buf[H264_MAX_MV_NUM];
+	struct vdec_vpu_inst vpu;
+	struct vdec_h264_slice_vsi *vsi;
+	struct vdec_h264_slice_vsi *vsi_core;
+
+	struct vdec_h264_slice_vsi vsi_ctx;
+	struct vdec_h264_slice_lat_dec_param h264_slice_param;
+
+	unsigned int resolution_changed;
+	unsigned int realloc_mv_buf;
+	unsigned int cap_num_planes;
+
+	struct v4l2_h264_dpb_entry dpb[16];
+	bool is_field_bitstream;
+};
+
+static int vdec_h264_slice_fill_decode_parameters(struct vdec_h264_slice_inst *inst,
+						  struct vdec_h264_slice_share_info *share_info)
+{
+	struct vdec_h264_slice_lat_dec_param *slice_param = &inst->vsi->h264_slice_params;
+	const struct v4l2_ctrl_h264_decode_params *dec_params;
+	const struct v4l2_ctrl_h264_scaling_matrix *src_matrix;
+	const struct v4l2_ctrl_h264_sps *sps;
+	const struct v4l2_ctrl_h264_pps *pps;
+
+	dec_params =
+		mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_DECODE_PARAMS);
+	if (IS_ERR(dec_params))
+		return PTR_ERR(dec_params);
+
+	src_matrix =
+		mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_SCALING_MATRIX);
+	if (IS_ERR(src_matrix))
+		return PTR_ERR(src_matrix);
+
+	sps = mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_SPS);
+	if (IS_ERR(sps))
+		return PTR_ERR(sps);
+
+	pps = mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_PPS);
+	if (IS_ERR(pps))
+		return PTR_ERR(pps);
+
+	if (dec_params->flags & V4L2_H264_DECODE_PARAM_FLAG_FIELD_PIC) {
+		mtk_vcodec_err(inst, "No support for H.264 field decoding.");
+		inst->is_field_bitstream = true;
+		return -EINVAL;
+	}
+
+	mtk_vdec_h264_copy_sps_params(&slice_param->sps, sps);
+	mtk_vdec_h264_copy_pps_params(&slice_param->pps, pps);
+	mtk_vdec_h264_copy_scaling_matrix(&slice_param->scaling_matrix, src_matrix);
+
+	memcpy(&share_info->sps, sps, sizeof(*sps));
+	memcpy(&share_info->dec_params, dec_params, sizeof(*dec_params));
+
+	return 0;
+}
+
+static int get_vdec_sig_decode_parameters(struct vdec_h264_slice_inst *inst)
+{
+	const struct v4l2_ctrl_h264_decode_params *dec_params;
+	const struct v4l2_ctrl_h264_sps *sps;
+	const struct v4l2_ctrl_h264_pps *pps;
+	const struct v4l2_ctrl_h264_scaling_matrix *scaling_matrix;
+	struct vdec_h264_slice_lat_dec_param *slice_param = &inst->h264_slice_param;
+	struct v4l2_h264_reflist_builder reflist_builder;
+	struct v4l2_h264_reference v4l2_p0_reflist[V4L2_H264_REF_LIST_LEN];
+	struct v4l2_h264_reference v4l2_b0_reflist[V4L2_H264_REF_LIST_LEN];
+	struct v4l2_h264_reference v4l2_b1_reflist[V4L2_H264_REF_LIST_LEN];
+	u8 *p0_reflist = slice_param->decode_params.ref_pic_list_p0;
+	u8 *b0_reflist = slice_param->decode_params.ref_pic_list_b0;
+	u8 *b1_reflist = slice_param->decode_params.ref_pic_list_b1;
+
+	dec_params =
+		mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_DECODE_PARAMS);
+	if (IS_ERR(dec_params))
+		return PTR_ERR(dec_params);
+
+	sps = mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_SPS);
+	if (IS_ERR(sps))
+		return PTR_ERR(sps);
+
+	pps = mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_PPS);
+	if (IS_ERR(pps))
+		return PTR_ERR(pps);
+
+	scaling_matrix =
+		mtk_vdec_h264_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_H264_SCALING_MATRIX);
+	if (IS_ERR(scaling_matrix))
+		return PTR_ERR(scaling_matrix);
+
+	mtk_vdec_h264_update_dpb(dec_params, inst->dpb);
+
+	mtk_vdec_h264_copy_sps_params(&slice_param->sps, sps);
+	mtk_vdec_h264_copy_pps_params(&slice_param->pps, pps);
+	mtk_vdec_h264_copy_scaling_matrix(&slice_param->scaling_matrix, scaling_matrix);
+
+	mtk_vdec_h264_copy_decode_params(&slice_param->decode_params, dec_params, inst->dpb);
+	mtk_vdec_h264_fill_dpb_info(inst->ctx, &slice_param->decode_params,
+				    slice_param->h264_dpb_info);
+
+	/* Build the reference lists */
+	v4l2_h264_init_reflist_builder(&reflist_builder, dec_params, sps, inst->dpb);
+	v4l2_h264_build_p_ref_list(&reflist_builder, v4l2_p0_reflist);
+	v4l2_h264_build_b_ref_lists(&reflist_builder, v4l2_b0_reflist, v4l2_b1_reflist);
+
+	/* Adapt the built lists to the firmware's expectations */
+	mtk_vdec_h264_get_ref_list(p0_reflist, v4l2_p0_reflist, reflist_builder.num_valid);
+	mtk_vdec_h264_get_ref_list(b0_reflist, v4l2_b0_reflist, reflist_builder.num_valid);
+	mtk_vdec_h264_get_ref_list(b1_reflist, v4l2_b1_reflist, reflist_builder.num_valid);
+
+	memcpy(&inst->vsi_ctx.h264_slice_params, slice_param,
+	       sizeof(inst->vsi_ctx.h264_slice_params));
+
+	return 0;
+}
+
+static void vdec_h264_slice_fill_decode_reflist(struct vdec_h264_slice_inst *inst,
+						struct vdec_h264_slice_lat_dec_param *slice_param,
+						struct vdec_h264_slice_share_info *share_info)
+{
+	struct v4l2_ctrl_h264_decode_params *dec_params = &share_info->dec_params;
+	struct v4l2_ctrl_h264_sps *sps = &share_info->sps;
+	struct v4l2_h264_reflist_builder reflist_builder;
+	struct v4l2_h264_reference v4l2_p0_reflist[V4L2_H264_REF_LIST_LEN];
+	struct v4l2_h264_reference v4l2_b0_reflist[V4L2_H264_REF_LIST_LEN];
+	struct v4l2_h264_reference v4l2_b1_reflist[V4L2_H264_REF_LIST_LEN];
+	u8 *p0_reflist = slice_param->decode_params.ref_pic_list_p0;
+	u8 *b0_reflist = slice_param->decode_params.ref_pic_list_b0;
+	u8 *b1_reflist = slice_param->decode_params.ref_pic_list_b1;
+
+	mtk_vdec_h264_update_dpb(dec_params, inst->dpb);
+
+	mtk_vdec_h264_copy_decode_params(&slice_param->decode_params, dec_params,
+					 inst->dpb);
+	mtk_vdec_h264_fill_dpb_info(inst->ctx, &slice_param->decode_params,
+				    slice_param->h264_dpb_info);
+
+	mtk_v4l2_debug(3, "cur poc = %d\n", dec_params->bottom_field_order_cnt);
+	/* Build the reference lists */
+	v4l2_h264_init_reflist_builder(&reflist_builder, dec_params, sps,
+				       inst->dpb);
+	v4l2_h264_build_p_ref_list(&reflist_builder, v4l2_p0_reflist);
+	v4l2_h264_build_b_ref_lists(&reflist_builder, v4l2_b0_reflist, v4l2_b1_reflist);
+
+	/* Adapt the built lists to the firmware's expectations */
+	mtk_vdec_h264_get_ref_list(p0_reflist, v4l2_p0_reflist, reflist_builder.num_valid);
+	mtk_vdec_h264_get_ref_list(b0_reflist, v4l2_b0_reflist, reflist_builder.num_valid);
+	mtk_vdec_h264_get_ref_list(b1_reflist, v4l2_b1_reflist, reflist_builder.num_valid);
+}
+
+static int vdec_h264_slice_alloc_mv_buf(struct vdec_h264_slice_inst *inst,
+					struct vdec_pic_info *pic)
+{
+	unsigned int buf_sz = mtk_vdec_h264_get_mv_buf_size(pic->buf_w, pic->buf_h);
+	struct mtk_vcodec_mem *mem;
+	int i, err;
+
+	mtk_v4l2_debug(3, "size = 0x%x", buf_sz);
+	for (i = 0; i < H264_MAX_MV_NUM; i++) {
+		mem = &inst->mv_buf[i];
+		if (mem->va)
+			mtk_vcodec_mem_free(inst->ctx, mem);
+		mem->size = buf_sz;
+		err = mtk_vcodec_mem_alloc(inst->ctx, mem);
+		if (err) {
+			mtk_vcodec_err(inst, "failed to allocate mv buf");
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+static void vdec_h264_slice_free_mv_buf(struct vdec_h264_slice_inst *inst)
+{
+	int i;
+	struct mtk_vcodec_mem *mem;
+
+	for (i = 0; i < H264_MAX_MV_NUM; i++) {
+		mem = &inst->mv_buf[i];
+		if (mem->va)
+			mtk_vcodec_mem_free(inst->ctx, mem);
+	}
+}
+
+static void vdec_h264_slice_get_pic_info(struct vdec_h264_slice_inst *inst)
+{
+	struct mtk_vcodec_ctx *ctx = inst->ctx;
+	u32 data[3];
+
+	data[0] = ctx->picinfo.pic_w;
+	data[1] = ctx->picinfo.pic_h;
+	data[2] = ctx->capture_fourcc;
+	vpu_dec_get_param(&inst->vpu, data, 3, GET_PARAM_PIC_INFO);
+
+	ctx->picinfo.buf_w = ALIGN(ctx->picinfo.pic_w, VCODEC_DEC_ALIGNED_64);
+	ctx->picinfo.buf_h = ALIGN(ctx->picinfo.pic_h, VCODEC_DEC_ALIGNED_64);
+	ctx->picinfo.fb_sz[0] = inst->vpu.fb_sz[0];
+	ctx->picinfo.fb_sz[1] = inst->vpu.fb_sz[1];
+	inst->cap_num_planes =
+		ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes;
+
+	mtk_vcodec_debug(inst, "pic(%d, %d), buf(%d, %d)",
+			 ctx->picinfo.pic_w, ctx->picinfo.pic_h,
+			 ctx->picinfo.buf_w, ctx->picinfo.buf_h);
+	mtk_vcodec_debug(inst, "Y/C(%d, %d)", ctx->picinfo.fb_sz[0],
+			 ctx->picinfo.fb_sz[1]);
+
+	if (ctx->last_decoded_picinfo.pic_w != ctx->picinfo.pic_w ||
+	    ctx->last_decoded_picinfo.pic_h != ctx->picinfo.pic_h) {
+		inst->resolution_changed = true;
+		if (ctx->last_decoded_picinfo.buf_w != ctx->picinfo.buf_w ||
+		    ctx->last_decoded_picinfo.buf_h != ctx->picinfo.buf_h)
+			inst->realloc_mv_buf = true;
+
+		mtk_v4l2_debug(1, "resChg: (%d %d) : old(%d, %d) -> new(%d, %d)",
+			       inst->resolution_changed,
+			       inst->realloc_mv_buf,
+			       ctx->last_decoded_picinfo.pic_w,
+			       ctx->last_decoded_picinfo.pic_h,
+			       ctx->picinfo.pic_w, ctx->picinfo.pic_h);
+	}
+}
+
+static void vdec_h264_slice_get_crop_info(struct vdec_h264_slice_inst *inst,
+					  struct v4l2_rect *cr)
+{
+	cr->left = 0;
+	cr->top = 0;
+	cr->width = inst->ctx->picinfo.pic_w;
+	cr->height = inst->ctx->picinfo.pic_h;
+
+	mtk_vcodec_debug(inst, "l=%d, t=%d, w=%d, h=%d",
+			 cr->left, cr->top, cr->width, cr->height);
+}
+
+static int vdec_h264_slice_init(struct mtk_vcodec_ctx *ctx)
+{
+	struct vdec_h264_slice_inst *inst;
+	int err, vsi_size;
+
+	inst = kzalloc(sizeof(*inst), GFP_KERNEL);
+	if (!inst)
+		return -ENOMEM;
+
+	inst->ctx = ctx;
+
+	inst->vpu.id = SCP_IPI_VDEC_LAT;
+	inst->vpu.core_id = SCP_IPI_VDEC_CORE;
+	inst->vpu.ctx = ctx;
+	inst->vpu.codec_type = ctx->current_codec;
+	inst->vpu.capture_type = ctx->capture_fourcc;
+
+	err = vpu_dec_init(&inst->vpu);
+	if (err) {
+		mtk_vcodec_err(inst, "vdec_h264 init err=%d", err);
+		goto error_free_inst;
+	}
+
+	vsi_size = round_up(sizeof(struct vdec_h264_slice_vsi), VCODEC_DEC_ALIGNED_64);
+	inst->vsi = inst->vpu.vsi;
+	inst->vsi_core =
+		(struct vdec_h264_slice_vsi *)(((char *)inst->vpu.vsi) + vsi_size);
+	inst->resolution_changed = true;
+	inst->realloc_mv_buf = true;
+
+	mtk_vcodec_debug(inst, "lat struct size = %d,%d,%d,%d vsi: %d\n",
+			 (int)sizeof(struct mtk_h264_sps_param),
+			 (int)sizeof(struct mtk_h264_pps_param),
+			 (int)sizeof(struct vdec_h264_slice_lat_dec_param),
+			 (int)sizeof(struct mtk_h264_dpb_info),
+			 vsi_size);
+	mtk_vcodec_debug(inst, "lat H264 instance >> %p, codec_type = 0x%x",
+			 inst, inst->vpu.codec_type);
+
+	ctx->drv_handle = inst;
+	return 0;
+
+error_free_inst:
+	kfree(inst);
+	return err;
+}
+
+static void vdec_h264_slice_deinit(void *h_vdec)
+{
+	struct vdec_h264_slice_inst *inst = h_vdec;
+
+	mtk_vcodec_debug_enter(inst);
+
+	vpu_dec_deinit(&inst->vpu);
+	vdec_h264_slice_free_mv_buf(inst);
+	vdec_msg_queue_deinit(&inst->ctx->msg_queue, inst->ctx);
+
+	kfree(inst);
+}
+
+static int vdec_h264_slice_core_decode(struct vdec_lat_buf *lat_buf)
+{
+	struct vdec_fb *fb;
+	u64 vdec_fb_va;
+	u64 y_fb_dma, c_fb_dma;
+	int err, timeout, i;
+	struct mtk_vcodec_ctx *ctx = lat_buf->ctx;
+	struct vdec_h264_slice_inst *inst = ctx->drv_handle;
+	struct vb2_v4l2_buffer *vb2_v4l2;
+	struct vdec_h264_slice_share_info *share_info = lat_buf->private_data;
+	struct mtk_vcodec_mem *mem;
+	struct vdec_vpu_inst *vpu = &inst->vpu;
+
+	mtk_vcodec_debug(inst, "[h264-core] vdec_h264 core decode");
+	memcpy(&inst->vsi_core->h264_slice_params, &share_info->h264_slice_params,
+	       sizeof(share_info->h264_slice_params));
+
+	fb = ctx->dev->vdec_pdata->get_cap_buffer(ctx);
+	if (!fb) {
+		err = -EBUSY;
+		mtk_vcodec_err(inst, "fb buffer is NULL");
+		goto vdec_dec_end;
+	}
+
+	vdec_fb_va = (unsigned long)fb;
+	y_fb_dma = (u64)fb->base_y.dma_addr;
+	if (ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 1)
+		c_fb_dma =
+			y_fb_dma + inst->ctx->picinfo.buf_w * inst->ctx->picinfo.buf_h;
+	else
+		c_fb_dma = (u64)fb->base_c.dma_addr;
+
+	mtk_vcodec_debug(inst, "[h264-core] y/c addr = 0x%llx 0x%llx", y_fb_dma,
+			 c_fb_dma);
+
+	inst->vsi_core->dec.y_fb_dma = y_fb_dma;
+	inst->vsi_core->dec.c_fb_dma = c_fb_dma;
+	inst->vsi_core->dec.vdec_fb_va = vdec_fb_va;
+	inst->vsi_core->dec.nal_info = share_info->nal_info;
+	inst->vsi_core->wdma_start_addr =
+		lat_buf->ctx->msg_queue.wdma_addr.dma_addr;
+	inst->vsi_core->wdma_end_addr =
+		lat_buf->ctx->msg_queue.wdma_addr.dma_addr +
+		lat_buf->ctx->msg_queue.wdma_addr.size;
+	inst->vsi_core->wdma_err_addr = lat_buf->wdma_err_addr.dma_addr;
+	inst->vsi_core->slice_bc_start_addr = lat_buf->slice_bc_addr.dma_addr;
+	inst->vsi_core->slice_bc_end_addr = lat_buf->slice_bc_addr.dma_addr +
+		lat_buf->slice_bc_addr.size;
+	inst->vsi_core->trans_start = share_info->trans_start;
+	inst->vsi_core->trans_end = share_info->trans_end;
+	for (i = 0; i < H264_MAX_MV_NUM; i++) {
+		mem = &inst->mv_buf[i];
+		inst->vsi_core->mv_buf_dma[i] = mem->dma_addr;
+	}
+
+	vb2_v4l2 = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	v4l2_m2m_buf_copy_metadata(&lat_buf->ts_info, vb2_v4l2, true);
+
+	vdec_h264_slice_fill_decode_reflist(inst, &inst->vsi_core->h264_slice_params,
+					    share_info);
+
+	err = vpu_dec_core(vpu);
+	if (err) {
+		mtk_vcodec_err(inst, "core decode err=%d", err);
+		goto vdec_dec_end;
+	}
+
+	/* wait decoder done interrupt */
+	timeout = mtk_vcodec_wait_for_done_ctx(inst->ctx, MTK_INST_IRQ_RECEIVED,
+					       WAIT_INTR_TIMEOUT_MS, MTK_VDEC_CORE);
+	if (timeout)
+		mtk_vcodec_err(inst, "core decode timeout: pic_%d",
+			       ctx->decoded_frame_cnt);
+	inst->vsi_core->dec.timeout = !!timeout;
+
+	vpu_dec_core_end(vpu);
+	mtk_vcodec_debug(inst, "pic[%d] crc: 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x",
+			 ctx->decoded_frame_cnt,
+			 inst->vsi_core->dec.crc[0], inst->vsi_core->dec.crc[1],
+			 inst->vsi_core->dec.crc[2], inst->vsi_core->dec.crc[3],
+			 inst->vsi_core->dec.crc[4], inst->vsi_core->dec.crc[5],
+			 inst->vsi_core->dec.crc[6], inst->vsi_core->dec.crc[7]);
+
+vdec_dec_end:
+	vdec_msg_queue_update_ube_rptr(&lat_buf->ctx->msg_queue, share_info->trans_end);
+	ctx->dev->vdec_pdata->cap_to_disp(ctx, !!err, lat_buf->src_buf_req);
+	mtk_vcodec_debug(inst, "core decode done err=%d", err);
+	ctx->decoded_frame_cnt++;
+	return 0;
+}
+
+static void vdec_h264_insert_startcode(struct mtk_vcodec_dev *vcodec_dev, unsigned char *buf,
+				       size_t *bs_size, struct mtk_h264_pps_param *pps)
+{
+	struct device *dev = &vcodec_dev->plat_dev->dev;
+
+	/* Need to add pending data at the end of bitstream when bs_sz is small than
+	 * 20 bytes for cavlc bitstream, or lat will decode fail. This pending data is
+	 * useful for mt8192 and mt8195 platform.
+	 *
+	 * cavlc bitstream when entropy_coding_mode_flag is false.
+	 */
+	if (pps->entropy_coding_mode_flag || *bs_size > 20 ||
+	    !(of_device_is_compatible(dev->of_node, "mediatek,mt8192-vcodec-dec") ||
+	    of_device_is_compatible(dev->of_node, "mediatek,mt8195-vcodec-dec")))
+		return;
+
+	buf[*bs_size] = 0;
+	buf[*bs_size + 1] = 0;
+	buf[*bs_size + 2] = 1;
+	buf[*bs_size + 3] = 0xff;
+	(*bs_size) += 4;
+}
+
+static int vdec_h264_slice_lat_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
+				      struct vdec_fb *fb, bool *res_chg)
+{
+	struct vdec_h264_slice_inst *inst = h_vdec;
+	struct vdec_vpu_inst *vpu = &inst->vpu;
+	struct mtk_video_dec_buf *src_buf_info;
+	int nal_start_idx, err, timeout = 0, i;
+	unsigned int data[2];
+	struct vdec_lat_buf *lat_buf;
+	struct vdec_h264_slice_share_info *share_info;
+	unsigned char *buf;
+	struct mtk_vcodec_mem *mem;
+
+	if (vdec_msg_queue_init(&inst->ctx->msg_queue, inst->ctx,
+				vdec_h264_slice_core_decode,
+				sizeof(*share_info)))
+		return -ENOMEM;
+
+	/* bs NULL means flush decoder */
+	if (!bs) {
+		vdec_msg_queue_wait_lat_buf_full(&inst->ctx->msg_queue);
+		return vpu_dec_reset(vpu);
+	}
+
+	if (inst->is_field_bitstream)
+		return -EINVAL;
+
+	lat_buf = vdec_msg_queue_dqbuf(&inst->ctx->msg_queue.lat_ctx);
+	if (!lat_buf) {
+		mtk_vcodec_err(inst, "failed to get lat buffer");
+		return -EAGAIN;
+	}
+	share_info = lat_buf->private_data;
+	src_buf_info = container_of(bs, struct mtk_video_dec_buf, bs_buffer);
+
+	buf = (unsigned char *)bs->va;
+	nal_start_idx = mtk_vdec_h264_find_start_code(buf, bs->size);
+	if (nal_start_idx < 0) {
+		err = -EINVAL;
+		goto err_free_fb_out;
+	}
+
+	inst->vsi->dec.nal_info = buf[nal_start_idx];
+	lat_buf->src_buf_req = src_buf_info->m2m_buf.vb.vb2_buf.req_obj.req;
+	v4l2_m2m_buf_copy_metadata(&src_buf_info->m2m_buf.vb, &lat_buf->ts_info, true);
+
+	err = vdec_h264_slice_fill_decode_parameters(inst, share_info);
+	if (err)
+		goto err_free_fb_out;
+
+	vdec_h264_insert_startcode(inst->ctx->dev, buf, &bs->size,
+				   &share_info->h264_slice_params.pps);
+
+	inst->vsi->dec.bs_buf_addr = (uint64_t)bs->dma_addr;
+	inst->vsi->dec.bs_buf_size = bs->size;
+
+	*res_chg = inst->resolution_changed;
+	if (inst->resolution_changed) {
+		mtk_vcodec_debug(inst, "- resolution changed -");
+		if (inst->realloc_mv_buf) {
+			err = vdec_h264_slice_alloc_mv_buf(inst, &inst->ctx->picinfo);
+			inst->realloc_mv_buf = false;
+			if (err)
+				goto err_free_fb_out;
+		}
+		inst->resolution_changed = false;
+	}
+	for (i = 0; i < H264_MAX_MV_NUM; i++) {
+		mem = &inst->mv_buf[i];
+		inst->vsi->mv_buf_dma[i] = mem->dma_addr;
+	}
+	inst->vsi->wdma_start_addr = lat_buf->ctx->msg_queue.wdma_addr.dma_addr;
+	inst->vsi->wdma_end_addr = lat_buf->ctx->msg_queue.wdma_addr.dma_addr +
+		lat_buf->ctx->msg_queue.wdma_addr.size;
+	inst->vsi->wdma_err_addr = lat_buf->wdma_err_addr.dma_addr;
+	inst->vsi->slice_bc_start_addr = lat_buf->slice_bc_addr.dma_addr;
+	inst->vsi->slice_bc_end_addr = lat_buf->slice_bc_addr.dma_addr +
+		lat_buf->slice_bc_addr.size;
+
+	inst->vsi->trans_end = inst->ctx->msg_queue.wdma_rptr_addr;
+	inst->vsi->trans_start = inst->ctx->msg_queue.wdma_wptr_addr;
+	mtk_vcodec_debug(inst, "lat:trans(0x%llx 0x%llx) err:0x%llx",
+			 inst->vsi->wdma_start_addr,
+			 inst->vsi->wdma_end_addr,
+			 inst->vsi->wdma_err_addr);
+
+	mtk_vcodec_debug(inst, "slice(0x%llx 0x%llx) rprt((0x%llx 0x%llx))",
+			 inst->vsi->slice_bc_start_addr,
+			 inst->vsi->slice_bc_end_addr,
+			 inst->vsi->trans_start,
+			 inst->vsi->trans_end);
+	err = vpu_dec_start(vpu, data, 2);
+	if (err) {
+		mtk_vcodec_debug(inst, "lat decode err: %d", err);
+		goto err_free_fb_out;
+	}
+
+	share_info->trans_end = inst->ctx->msg_queue.wdma_addr.dma_addr +
+		inst->vsi->wdma_end_addr_offset;
+	share_info->trans_start = inst->ctx->msg_queue.wdma_wptr_addr;
+	share_info->nal_info = inst->vsi->dec.nal_info;
+
+	if (IS_VDEC_INNER_RACING(inst->ctx->dev->dec_capability)) {
+		memcpy(&share_info->h264_slice_params, &inst->vsi->h264_slice_params,
+		       sizeof(share_info->h264_slice_params));
+		vdec_msg_queue_qbuf(&inst->ctx->dev->msg_queue_core_ctx, lat_buf);
+	}
+
+	/* wait decoder done interrupt */
+	timeout = mtk_vcodec_wait_for_done_ctx(inst->ctx, MTK_INST_IRQ_RECEIVED,
+					       WAIT_INTR_TIMEOUT_MS, MTK_VDEC_LAT0);
+	if (timeout)
+		mtk_vcodec_err(inst, "lat decode timeout: pic_%d", inst->slice_dec_num);
+	inst->vsi->dec.timeout = !!timeout;
+
+	err = vpu_dec_end(vpu);
+	if (err == SLICE_HEADER_FULL || err == TRANS_BUFFER_FULL) {
+		if (!IS_VDEC_INNER_RACING(inst->ctx->dev->dec_capability))
+			vdec_msg_queue_qbuf(&inst->ctx->msg_queue.lat_ctx, lat_buf);
+		inst->slice_dec_num++;
+		mtk_vcodec_err(inst, "lat dec fail: pic_%d err:%d", inst->slice_dec_num, err);
+		return -EINVAL;
+	}
+
+	share_info->trans_end = inst->ctx->msg_queue.wdma_addr.dma_addr +
+		inst->vsi->wdma_end_addr_offset;
+	vdec_msg_queue_update_ube_wptr(&lat_buf->ctx->msg_queue, share_info->trans_end);
+
+	if (!IS_VDEC_INNER_RACING(inst->ctx->dev->dec_capability)) {
+		memcpy(&share_info->h264_slice_params, &inst->vsi->h264_slice_params,
+		       sizeof(share_info->h264_slice_params));
+		vdec_msg_queue_qbuf(&inst->ctx->dev->msg_queue_core_ctx, lat_buf);
+	}
+	mtk_vcodec_debug(inst, "dec num: %d lat crc: 0x%x 0x%x 0x%x", inst->slice_dec_num,
+			 inst->vsi->dec.crc[0], inst->vsi->dec.crc[1], inst->vsi->dec.crc[2]);
+
+	inst->slice_dec_num++;
+	return 0;
+err_free_fb_out:
+	vdec_msg_queue_qbuf(&inst->ctx->msg_queue.lat_ctx, lat_buf);
+	mtk_vcodec_err(inst, "slice dec number: %d err: %d", inst->slice_dec_num, err);
+	return err;
+}
+
+static int vdec_h264_slice_single_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
+					 struct vdec_fb *unused, bool *res_chg)
+{
+	struct vdec_h264_slice_inst *inst = h_vdec;
+	struct vdec_vpu_inst *vpu = &inst->vpu;
+	struct mtk_video_dec_buf *src_buf_info, *dst_buf_info;
+	struct vdec_fb *fb;
+	unsigned char *buf;
+	unsigned int data[2], i;
+	u64 y_fb_dma, c_fb_dma;
+	struct mtk_vcodec_mem *mem;
+	int err, nal_start_idx;
+
+	/* bs NULL means flush decoder */
+	if (!bs)
+		return vpu_dec_reset(vpu);
+
+	fb = inst->ctx->dev->vdec_pdata->get_cap_buffer(inst->ctx);
+	src_buf_info = container_of(bs, struct mtk_video_dec_buf, bs_buffer);
+	dst_buf_info = container_of(fb, struct mtk_video_dec_buf, frame_buffer);
+
+	y_fb_dma = fb ? (u64)fb->base_y.dma_addr : 0;
+	c_fb_dma = fb ? (u64)fb->base_c.dma_addr : 0;
+	mtk_vcodec_debug(inst, "[h264-dec] [%d] y_dma=%llx c_dma=%llx",
+			 inst->ctx->decoded_frame_cnt, y_fb_dma, c_fb_dma);
+
+	inst->vsi_ctx.dec.bs_buf_addr = (u64)bs->dma_addr;
+	inst->vsi_ctx.dec.bs_buf_size = bs->size;
+	inst->vsi_ctx.dec.y_fb_dma = y_fb_dma;
+	inst->vsi_ctx.dec.c_fb_dma = c_fb_dma;
+	inst->vsi_ctx.dec.vdec_fb_va = (u64)(uintptr_t)fb;
+
+	v4l2_m2m_buf_copy_metadata(&src_buf_info->m2m_buf.vb,
+				   &dst_buf_info->m2m_buf.vb, true);
+	err = get_vdec_sig_decode_parameters(inst);
+	if (err)
+		goto err_free_fb_out;
+
+	buf = (unsigned char *)bs->va;
+	nal_start_idx = mtk_vdec_h264_find_start_code(buf, bs->size);
+	if (nal_start_idx < 0) {
+		err = -EINVAL;
+		goto err_free_fb_out;
+	}
+	inst->vsi_ctx.dec.nal_info = buf[nal_start_idx];
+
+	*res_chg = inst->resolution_changed;
+	if (inst->resolution_changed) {
+		mtk_vcodec_debug(inst, "- resolution changed -");
+		if (inst->realloc_mv_buf) {
+			err = vdec_h264_slice_alloc_mv_buf(inst, &inst->ctx->picinfo);
+			inst->realloc_mv_buf = false;
+			if (err)
+				goto err_free_fb_out;
+		}
+		inst->resolution_changed = false;
+
+		for (i = 0; i < H264_MAX_MV_NUM; i++) {
+			mem = &inst->mv_buf[i];
+			inst->vsi_ctx.mv_buf_dma[i] = mem->dma_addr;
+		}
+	}
+
+	memcpy(inst->vpu.vsi, &inst->vsi_ctx, sizeof(inst->vsi_ctx));
+	err = vpu_dec_start(vpu, data, 2);
+	if (err)
+		goto err_free_fb_out;
+
+	/* wait decoder done interrupt */
+	err = mtk_vcodec_wait_for_done_ctx(inst->ctx, MTK_INST_IRQ_RECEIVED,
+					   WAIT_INTR_TIMEOUT_MS, MTK_VDEC_CORE);
+	if (err)
+		mtk_vcodec_err(inst, "decode timeout: pic_%d",
+			       inst->ctx->decoded_frame_cnt);
+
+	inst->vsi->dec.timeout = !!err;
+	err = vpu_dec_end(vpu);
+	if (err)
+		goto err_free_fb_out;
+
+	memcpy(&inst->vsi_ctx, inst->vpu.vsi, sizeof(inst->vsi_ctx));
+	mtk_vcodec_debug(inst, "pic[%d] crc: 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x",
+			 inst->ctx->decoded_frame_cnt,
+			 inst->vsi_ctx.dec.crc[0], inst->vsi_ctx.dec.crc[1],
+			 inst->vsi_ctx.dec.crc[2], inst->vsi_ctx.dec.crc[3],
+			 inst->vsi_ctx.dec.crc[4], inst->vsi_ctx.dec.crc[5],
+			 inst->vsi_ctx.dec.crc[6], inst->vsi_ctx.dec.crc[7]);
+
+	inst->ctx->decoded_frame_cnt++;
+	return 0;
+
+err_free_fb_out:
+	mtk_vcodec_err(inst, "dec frame number: %d err: %d",
+		       inst->ctx->decoded_frame_cnt, err);
+	return err;
+}
+
+static int vdec_h264_slice_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
+				  struct vdec_fb *unused, bool *res_chg)
+{
+	struct vdec_h264_slice_inst *inst = h_vdec;
+	int ret;
+
+	if (!h_vdec)
+		return -EINVAL;
+
+	if (inst->ctx->dev->vdec_pdata->hw_arch == MTK_VDEC_PURE_SINGLE_CORE)
+		ret = vdec_h264_slice_single_decode(h_vdec, bs, unused, res_chg);
+	else
+		ret = vdec_h264_slice_lat_decode(h_vdec, bs, unused, res_chg);
+
+	return ret;
+}
+
+static int vdec_h264_slice_get_param(void *h_vdec, enum vdec_get_param_type type,
+				     void *out)
+{
+	struct vdec_h264_slice_inst *inst = h_vdec;
+
+	switch (type) {
+	case GET_PARAM_PIC_INFO:
+		vdec_h264_slice_get_pic_info(inst);
+		break;
+	case GET_PARAM_DPB_SIZE:
+		*(unsigned int *)out = 6;
+		break;
+	case GET_PARAM_CROP_INFO:
+		vdec_h264_slice_get_crop_info(inst, out);
+		break;
+	default:
+		mtk_vcodec_err(inst, "invalid get parameter type=%d", type);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+const struct vdec_common_if vdec_h264_slice_multi_if = {
+	.init		= vdec_h264_slice_init,
+	.decode		= vdec_h264_slice_decode,
+	.get_param	= vdec_h264_slice_get_param,
+	.deinit		= vdec_h264_slice_deinit,
+};
diff --git a/drivers/media/platform/mediatek/vcodec/vdec/vdec_vp8_if.c b/drivers/media/platform/mediatek/vcodec/vdec/vdec_vp8_if.c
new file mode 100644
index 000000000..88c046731
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec/vdec_vp8_if.c
@@ -0,0 +1,616 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Jungchang Tsao <jungchang.tsao@mediatek.com>
+ *	   PC Chen <pc.chen@mediatek.com>
+ */
+
+#include <linux/slab.h>
+#include "../vdec_drv_if.h"
+#include "../mtk_vcodec_util.h"
+#include "../mtk_vcodec_dec.h"
+#include "../mtk_vcodec_intr.h"
+#include "../vdec_vpu_if.h"
+#include "../vdec_drv_base.h"
+
+/* Decoding picture buffer size (3 reference frames plus current frame) */
+#define VP8_DPB_SIZE			4
+
+/* HW working buffer size (bytes) */
+#define VP8_WORKING_BUF_SZ		(45 * 4096)
+
+/* HW control register address */
+#define VP8_SEGID_DRAM_ADDR		0x3c
+#define VP8_HW_VLD_ADDR			0x93C
+#define VP8_HW_VLD_VALUE		0x940
+#define VP8_BSASET			0x100
+#define VP8_BSDSET			0x104
+#define VP8_RW_CKEN_SET			0x0
+#define VP8_RW_DCM_CON			0x18
+#define VP8_WO_VLD_SRST			0x108
+#define VP8_RW_MISC_SYS_SEL		0x84
+#define VP8_RW_MISC_SPEC_CON		0xC8
+#define VP8_WO_VLD_SRST			0x108
+#define VP8_RW_VP8_CTRL			0xA4
+#define VP8_RW_MISC_DCM_CON		0xEC
+#define VP8_RW_MISC_SRST		0xF4
+#define VP8_RW_MISC_FUNC_CON		0xCC
+
+#define VP8_MAX_FRM_BUF_NUM		5
+#define VP8_MAX_FRM_BUF_NODE_NUM	(VP8_MAX_FRM_BUF_NUM * 2)
+
+/* required buffer size (bytes) to store decode information */
+#define VP8_HW_SEGMENT_DATA_SZ		272
+#define VP8_HW_SEGMENT_UINT		4
+
+#define VP8_DEC_TABLE_PROC_LOOP		96
+#define VP8_DEC_TABLE_UNIT		3
+#define VP8_DEC_TABLE_SZ		300
+#define VP8_DEC_TABLE_OFFSET		2
+#define VP8_DEC_TABLE_RW_UNIT		4
+
+/**
+ * struct vdec_vp8_dec_info - decode misc information
+ * @working_buf_dma   : working buffer dma address
+ * @prev_y_dma        : previous decoded frame buffer Y plane address
+ * @cur_y_fb_dma      : current plane Y frame buffer dma address
+ * @cur_c_fb_dma      : current plane C frame buffer dma address
+ * @bs_dma	      : bitstream dma address
+ * @bs_sz	      : bitstream size
+ * @resolution_changed: resolution change flag 1 - changed,  0 - not change
+ * @show_frame	      : display this frame or not
+ * @wait_key_frame    : wait key frame coming
+ */
+struct vdec_vp8_dec_info {
+	uint64_t working_buf_dma;
+	uint64_t prev_y_dma;
+	uint64_t cur_y_fb_dma;
+	uint64_t cur_c_fb_dma;
+	uint64_t bs_dma;
+	uint32_t bs_sz;
+	uint32_t resolution_changed;
+	uint32_t show_frame;
+	uint32_t wait_key_frame;
+};
+
+/**
+ * struct vdec_vp8_vsi - VPU shared information
+ * @dec			: decoding information
+ * @pic			: picture information
+ * @dec_table		: decoder coefficient table
+ * @segment_buf		: segmentation buffer
+ * @load_data		: flag to indicate reload decode data
+ */
+struct vdec_vp8_vsi {
+	struct vdec_vp8_dec_info dec;
+	struct vdec_pic_info pic;
+	uint32_t dec_table[VP8_DEC_TABLE_SZ];
+	uint32_t segment_buf[VP8_HW_SEGMENT_DATA_SZ][VP8_HW_SEGMENT_UINT];
+	uint32_t load_data;
+};
+
+/**
+ * struct vdec_vp8_hw_reg_base - HW register base
+ * @sys		: base address for sys
+ * @misc	: base address for misc
+ * @ld		: base address for ld
+ * @top		: base address for top
+ * @cm		: base address for cm
+ * @hwd		: base address for hwd
+ * @hwb		: base address for hwb
+ */
+struct vdec_vp8_hw_reg_base {
+	void __iomem *sys;
+	void __iomem *misc;
+	void __iomem *ld;
+	void __iomem *top;
+	void __iomem *cm;
+	void __iomem *hwd;
+	void __iomem *hwb;
+};
+
+/**
+ * struct vdec_vp8_vpu_inst - VPU instance for VP8 decode
+ * @wq_hd	: Wait queue to wait VPU message ack
+ * @signaled	: 1 - Host has received ack message from VPU, 0 - not receive
+ * @failure	: VPU execution result status 0 - success, others - fail
+ * @inst_addr	: VPU decoder instance address
+ */
+struct vdec_vp8_vpu_inst {
+	wait_queue_head_t wq_hd;
+	int signaled;
+	int failure;
+	uint32_t inst_addr;
+};
+
+/* frame buffer (fb) list
+ * [available_fb_node_list]  - decode fb are initialized to 0 and populated in
+ * [fb_use_list]  - fb is set after decode and is moved to this list
+ * [fb_free_list] - fb is not needed for reference will be moved from
+ *		     [fb_use_list] to [fb_free_list] and
+ *		     once user remove fb from [fb_free_list],
+ *		     it is circulated back to [available_fb_node_list]
+ * [fb_disp_list] - fb is set after decode and is moved to this list
+ *                   once user remove fb from [fb_disp_list] it is
+ *                   circulated back to [available_fb_node_list]
+ */
+
+/**
+ * struct vdec_vp8_inst - VP8 decoder instance
+ * @cur_fb		   : current frame buffer
+ * @dec_fb		   : decode frame buffer node
+ * @available_fb_node_list : list to store available frame buffer node
+ * @fb_use_list		   : list to store frame buffer in use
+ * @fb_free_list	   : list to store free frame buffer
+ * @fb_disp_list	   : list to store display ready frame buffer
+ * @working_buf		   : HW decoder working buffer
+ * @reg_base		   : HW register base address
+ * @frm_cnt		   : decode frame count
+ * @ctx			   : V4L2 context
+ * @vpu			   : VPU instance for decoder
+ * @vsi			   : VPU share information
+ */
+struct vdec_vp8_inst {
+	struct vdec_fb *cur_fb;
+	struct vdec_fb_node dec_fb[VP8_MAX_FRM_BUF_NODE_NUM];
+	struct list_head available_fb_node_list;
+	struct list_head fb_use_list;
+	struct list_head fb_free_list;
+	struct list_head fb_disp_list;
+	struct mtk_vcodec_mem working_buf;
+	struct vdec_vp8_hw_reg_base reg_base;
+	unsigned int frm_cnt;
+	struct mtk_vcodec_ctx *ctx;
+	struct vdec_vpu_inst vpu;
+	struct vdec_vp8_vsi *vsi;
+};
+
+static void get_hw_reg_base(struct vdec_vp8_inst *inst)
+{
+	inst->reg_base.top = mtk_vcodec_get_reg_addr(inst->ctx, VDEC_TOP);
+	inst->reg_base.cm = mtk_vcodec_get_reg_addr(inst->ctx, VDEC_CM);
+	inst->reg_base.hwd = mtk_vcodec_get_reg_addr(inst->ctx, VDEC_HWD);
+	inst->reg_base.sys = mtk_vcodec_get_reg_addr(inst->ctx, VDEC_SYS);
+	inst->reg_base.misc = mtk_vcodec_get_reg_addr(inst->ctx, VDEC_MISC);
+	inst->reg_base.ld = mtk_vcodec_get_reg_addr(inst->ctx, VDEC_LD);
+	inst->reg_base.hwb = mtk_vcodec_get_reg_addr(inst->ctx, VDEC_HWB);
+}
+
+static void write_hw_segmentation_data(struct vdec_vp8_inst *inst)
+{
+	int i, j;
+	u32 seg_id_addr;
+	u32 val;
+	void __iomem *cm = inst->reg_base.cm;
+	struct vdec_vp8_vsi *vsi = inst->vsi;
+
+	seg_id_addr = readl(inst->reg_base.top + VP8_SEGID_DRAM_ADDR) >> 4;
+
+	for (i = 0; i < ARRAY_SIZE(vsi->segment_buf); i++) {
+		for (j = ARRAY_SIZE(vsi->segment_buf[i]) - 1; j >= 0; j--) {
+			val = (1 << 16) + ((seg_id_addr + i) << 2) + j;
+			writel(val, cm + VP8_HW_VLD_ADDR);
+
+			val = vsi->segment_buf[i][j];
+			writel(val, cm + VP8_HW_VLD_VALUE);
+		}
+	}
+}
+
+static void read_hw_segmentation_data(struct vdec_vp8_inst *inst)
+{
+	int i, j;
+	u32 seg_id_addr;
+	u32 val;
+	void __iomem *cm = inst->reg_base.cm;
+	struct vdec_vp8_vsi *vsi = inst->vsi;
+
+	seg_id_addr = readl(inst->reg_base.top + VP8_SEGID_DRAM_ADDR) >> 4;
+
+	for (i = 0; i < ARRAY_SIZE(vsi->segment_buf); i++) {
+		for (j = ARRAY_SIZE(vsi->segment_buf[i]) - 1; j >= 0; j--) {
+			val = ((seg_id_addr + i) << 2) + j;
+			writel(val, cm + VP8_HW_VLD_ADDR);
+
+			val = readl(cm + VP8_HW_VLD_VALUE);
+			vsi->segment_buf[i][j] = val;
+		}
+	}
+}
+
+/* reset HW and enable HW read/write data function */
+static void enable_hw_rw_function(struct vdec_vp8_inst *inst)
+{
+	u32 val = 0;
+	void __iomem *sys = inst->reg_base.sys;
+	void __iomem *misc = inst->reg_base.misc;
+	void __iomem *ld = inst->reg_base.ld;
+	void __iomem *hwb = inst->reg_base.hwb;
+	void __iomem *hwd = inst->reg_base.hwd;
+
+	writel(0x1, sys + VP8_RW_CKEN_SET);
+	writel(0x101, ld + VP8_WO_VLD_SRST);
+	writel(0x101, hwb + VP8_WO_VLD_SRST);
+
+	writel(1, sys);
+	val = readl(misc + VP8_RW_MISC_SRST);
+	writel((val & 0xFFFFFFFE), misc + VP8_RW_MISC_SRST);
+
+	writel(0x1, misc + VP8_RW_MISC_SYS_SEL);
+	writel(0x17F, misc + VP8_RW_MISC_SPEC_CON);
+	writel(0x71201100, misc + VP8_RW_MISC_FUNC_CON);
+	writel(0x0, ld + VP8_WO_VLD_SRST);
+	writel(0x0, hwb + VP8_WO_VLD_SRST);
+	writel(0x1, sys + VP8_RW_DCM_CON);
+	writel(0x1, misc + VP8_RW_MISC_DCM_CON);
+	writel(0x1, hwd + VP8_RW_VP8_CTRL);
+}
+
+static void store_dec_table(struct vdec_vp8_inst *inst)
+{
+	int i, j;
+	u32 addr = 0, val = 0;
+	void __iomem *hwd = inst->reg_base.hwd;
+	u32 *p = &inst->vsi->dec_table[VP8_DEC_TABLE_OFFSET];
+
+	for (i = 0; i < VP8_DEC_TABLE_PROC_LOOP; i++) {
+		writel(addr, hwd + VP8_BSASET);
+		for (j = 0; j < VP8_DEC_TABLE_UNIT ; j++) {
+			val = *p++;
+			writel(val, hwd + VP8_BSDSET);
+		}
+		addr += VP8_DEC_TABLE_RW_UNIT;
+	}
+}
+
+static void load_dec_table(struct vdec_vp8_inst *inst)
+{
+	int i;
+	u32 addr = 0;
+	u32 *p = &inst->vsi->dec_table[VP8_DEC_TABLE_OFFSET];
+	void __iomem *hwd = inst->reg_base.hwd;
+
+	for (i = 0; i < VP8_DEC_TABLE_PROC_LOOP; i++) {
+		writel(addr, hwd + VP8_BSASET);
+		/* read total 11 bytes */
+		*p++ = readl(hwd + VP8_BSDSET);
+		*p++ = readl(hwd + VP8_BSDSET);
+		*p++ = readl(hwd + VP8_BSDSET) & 0xFFFFFF;
+		addr += VP8_DEC_TABLE_RW_UNIT;
+	}
+}
+
+static void get_pic_info(struct vdec_vp8_inst *inst, struct vdec_pic_info *pic)
+{
+	*pic = inst->vsi->pic;
+
+	mtk_vcodec_debug(inst, "pic(%d, %d), buf(%d, %d)",
+			 pic->pic_w, pic->pic_h, pic->buf_w, pic->buf_h);
+	mtk_vcodec_debug(inst, "fb size: Y(%d), C(%d)",
+			 pic->fb_sz[0], pic->fb_sz[1]);
+}
+
+static void vp8_dec_finish(struct vdec_vp8_inst *inst)
+{
+	struct vdec_fb_node *node;
+	uint64_t prev_y_dma = inst->vsi->dec.prev_y_dma;
+
+	mtk_vcodec_debug(inst, "prev fb base dma=%llx", prev_y_dma);
+
+	/* put last decode ok frame to fb_free_list */
+	if (prev_y_dma != 0) {
+		list_for_each_entry(node, &inst->fb_use_list, list) {
+			struct vdec_fb *fb = (struct vdec_fb *)node->fb;
+
+			if (prev_y_dma == (uint64_t)fb->base_y.dma_addr) {
+				list_move_tail(&node->list,
+					       &inst->fb_free_list);
+				break;
+			}
+		}
+	}
+
+	/* available_fb_node_list -> fb_use_list */
+	node = list_first_entry(&inst->available_fb_node_list,
+				struct vdec_fb_node, list);
+	node->fb = inst->cur_fb;
+	list_move_tail(&node->list, &inst->fb_use_list);
+
+	/* available_fb_node_list -> fb_disp_list */
+	if (inst->vsi->dec.show_frame) {
+		node = list_first_entry(&inst->available_fb_node_list,
+					struct vdec_fb_node, list);
+		node->fb = inst->cur_fb;
+		list_move_tail(&node->list, &inst->fb_disp_list);
+	}
+}
+
+static void move_fb_list_use_to_free(struct vdec_vp8_inst *inst)
+{
+	struct vdec_fb_node *node, *tmp;
+
+	list_for_each_entry_safe(node, tmp, &inst->fb_use_list, list)
+		list_move_tail(&node->list, &inst->fb_free_list);
+}
+
+static void init_list(struct vdec_vp8_inst *inst)
+{
+	int i;
+
+	INIT_LIST_HEAD(&inst->available_fb_node_list);
+	INIT_LIST_HEAD(&inst->fb_use_list);
+	INIT_LIST_HEAD(&inst->fb_free_list);
+	INIT_LIST_HEAD(&inst->fb_disp_list);
+
+	for (i = 0; i < ARRAY_SIZE(inst->dec_fb); i++) {
+		INIT_LIST_HEAD(&inst->dec_fb[i].list);
+		inst->dec_fb[i].fb = NULL;
+		list_add_tail(&inst->dec_fb[i].list,
+			      &inst->available_fb_node_list);
+	}
+}
+
+static void add_fb_to_free_list(struct vdec_vp8_inst *inst, void *fb)
+{
+	struct vdec_fb_node *node;
+
+	if (fb) {
+		node = list_first_entry(&inst->available_fb_node_list,
+					struct vdec_fb_node, list);
+		node->fb = fb;
+		list_move_tail(&node->list, &inst->fb_free_list);
+	}
+}
+
+static int alloc_working_buf(struct vdec_vp8_inst *inst)
+{
+	int err;
+	struct mtk_vcodec_mem *mem = &inst->working_buf;
+
+	mem->size = VP8_WORKING_BUF_SZ;
+	err = mtk_vcodec_mem_alloc(inst->ctx, mem);
+	if (err) {
+		mtk_vcodec_err(inst, "Cannot allocate working buffer");
+		return err;
+	}
+
+	inst->vsi->dec.working_buf_dma = (uint64_t)mem->dma_addr;
+	return 0;
+}
+
+static void free_working_buf(struct vdec_vp8_inst *inst)
+{
+	struct mtk_vcodec_mem *mem = &inst->working_buf;
+
+	if (mem->va)
+		mtk_vcodec_mem_free(inst->ctx, mem);
+
+	inst->vsi->dec.working_buf_dma = 0;
+}
+
+static int vdec_vp8_init(struct mtk_vcodec_ctx *ctx)
+{
+	struct vdec_vp8_inst *inst;
+	int err;
+
+	inst = kzalloc(sizeof(*inst), GFP_KERNEL);
+	if (!inst)
+		return  -ENOMEM;
+
+	inst->ctx = ctx;
+
+	inst->vpu.id = IPI_VDEC_VP8;
+	inst->vpu.ctx = ctx;
+
+	err = vpu_dec_init(&inst->vpu);
+	if (err) {
+		mtk_vcodec_err(inst, "vdec_vp8 init err=%d", err);
+		goto error_free_inst;
+	}
+
+	inst->vsi = (struct vdec_vp8_vsi *)inst->vpu.vsi;
+	init_list(inst);
+	err = alloc_working_buf(inst);
+	if (err)
+		goto error_deinit;
+
+	get_hw_reg_base(inst);
+	mtk_vcodec_debug(inst, "VP8 Instance >> %p", inst);
+
+	ctx->drv_handle = inst;
+	return 0;
+
+error_deinit:
+	vpu_dec_deinit(&inst->vpu);
+error_free_inst:
+	kfree(inst);
+	return err;
+}
+
+static int vdec_vp8_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
+			   struct vdec_fb *fb, bool *res_chg)
+{
+	struct vdec_vp8_inst *inst = (struct vdec_vp8_inst *)h_vdec;
+	struct vdec_vp8_dec_info *dec = &inst->vsi->dec;
+	struct vdec_vpu_inst *vpu = &inst->vpu;
+	unsigned char *bs_va;
+	unsigned int data;
+	int err = 0;
+	uint64_t y_fb_dma;
+	uint64_t c_fb_dma;
+
+	/* bs NULL means flush decoder */
+	if (bs == NULL) {
+		move_fb_list_use_to_free(inst);
+		return vpu_dec_reset(vpu);
+	}
+
+	y_fb_dma = fb ? (u64)fb->base_y.dma_addr : 0;
+	c_fb_dma = fb ? (u64)fb->base_c.dma_addr : 0;
+
+	mtk_vcodec_debug(inst, "+ [%d] FB y_dma=%llx c_dma=%llx fb=%p",
+			 inst->frm_cnt, y_fb_dma, c_fb_dma, fb);
+
+	inst->cur_fb = fb;
+	dec->bs_dma = (unsigned long)bs->dma_addr;
+	dec->bs_sz = bs->size;
+	dec->cur_y_fb_dma = y_fb_dma;
+	dec->cur_c_fb_dma = c_fb_dma;
+
+	mtk_vcodec_debug(inst, "\n + FRAME[%d] +\n", inst->frm_cnt);
+
+	write_hw_segmentation_data(inst);
+	enable_hw_rw_function(inst);
+	store_dec_table(inst);
+
+	bs_va = (unsigned char *)bs->va;
+
+	/* retrieve width/hight and scale info from header */
+	data = (*(bs_va + 9) << 24) | (*(bs_va + 8) << 16) |
+	       (*(bs_va + 7) << 8) | *(bs_va + 6);
+	err = vpu_dec_start(vpu, &data, 1);
+	if (err) {
+		add_fb_to_free_list(inst, fb);
+		if (dec->wait_key_frame) {
+			mtk_vcodec_debug(inst, "wait key frame !");
+			return 0;
+		}
+
+		goto error;
+	}
+
+	if (dec->resolution_changed) {
+		mtk_vcodec_debug(inst, "- resolution_changed -");
+		*res_chg = true;
+		add_fb_to_free_list(inst, fb);
+		return 0;
+	}
+
+	/* wait decoder done interrupt */
+	mtk_vcodec_wait_for_done_ctx(inst->ctx, MTK_INST_IRQ_RECEIVED,
+				     WAIT_INTR_TIMEOUT_MS, 0);
+
+	if (inst->vsi->load_data)
+		load_dec_table(inst);
+
+	vp8_dec_finish(inst);
+	read_hw_segmentation_data(inst);
+
+	err = vpu_dec_end(vpu);
+	if (err)
+		goto error;
+
+	mtk_vcodec_debug(inst, "\n - FRAME[%d] - show=%d\n", inst->frm_cnt,
+			 dec->show_frame);
+	inst->frm_cnt++;
+	*res_chg = false;
+	return 0;
+
+error:
+	mtk_vcodec_err(inst, "\n - FRAME[%d] - err=%d\n", inst->frm_cnt, err);
+	return err;
+}
+
+static void get_disp_fb(struct vdec_vp8_inst *inst, struct vdec_fb **out_fb)
+{
+	struct vdec_fb_node *node;
+	struct vdec_fb *fb;
+
+	node = list_first_entry_or_null(&inst->fb_disp_list,
+					struct vdec_fb_node, list);
+	if (node) {
+		list_move_tail(&node->list, &inst->available_fb_node_list);
+		fb = (struct vdec_fb *)node->fb;
+		fb->status |= FB_ST_DISPLAY;
+		mtk_vcodec_debug(inst, "[FB] get disp fb %p st=%d",
+				 node->fb, fb->status);
+	} else {
+		fb = NULL;
+		mtk_vcodec_debug(inst, "[FB] there is no disp fb");
+	}
+
+	*out_fb = fb;
+}
+
+static void get_free_fb(struct vdec_vp8_inst *inst, struct vdec_fb **out_fb)
+{
+	struct vdec_fb_node *node;
+	struct vdec_fb *fb;
+
+	node = list_first_entry_or_null(&inst->fb_free_list,
+					struct vdec_fb_node, list);
+	if (node) {
+		list_move_tail(&node->list, &inst->available_fb_node_list);
+		fb = (struct vdec_fb *)node->fb;
+		fb->status |= FB_ST_FREE;
+		mtk_vcodec_debug(inst, "[FB] get free fb %p st=%d",
+				 node->fb, fb->status);
+	} else {
+		fb = NULL;
+		mtk_vcodec_debug(inst, "[FB] there is no free fb");
+	}
+
+	*out_fb = fb;
+}
+
+static void get_crop_info(struct vdec_vp8_inst *inst, struct v4l2_rect *cr)
+{
+	cr->left = 0;
+	cr->top = 0;
+	cr->width = inst->vsi->pic.pic_w;
+	cr->height = inst->vsi->pic.pic_h;
+	mtk_vcodec_debug(inst, "get crop info l=%d, t=%d, w=%d, h=%d",
+			 cr->left, cr->top, cr->width, cr->height);
+}
+
+static int vdec_vp8_get_param(void *h_vdec, enum vdec_get_param_type type,
+			      void *out)
+{
+	struct vdec_vp8_inst *inst = (struct vdec_vp8_inst *)h_vdec;
+
+	switch (type) {
+	case GET_PARAM_DISP_FRAME_BUFFER:
+		get_disp_fb(inst, out);
+		break;
+
+	case GET_PARAM_FREE_FRAME_BUFFER:
+		get_free_fb(inst, out);
+		break;
+
+	case GET_PARAM_PIC_INFO:
+		get_pic_info(inst, out);
+		break;
+
+	case GET_PARAM_CROP_INFO:
+		get_crop_info(inst, out);
+		break;
+
+	case GET_PARAM_DPB_SIZE:
+		*((unsigned int *)out) = VP8_DPB_SIZE;
+		break;
+
+	default:
+		mtk_vcodec_err(inst, "invalid get parameter type=%d", type);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void vdec_vp8_deinit(void *h_vdec)
+{
+	struct vdec_vp8_inst *inst = (struct vdec_vp8_inst *)h_vdec;
+
+	mtk_vcodec_debug_enter(inst);
+
+	vpu_dec_deinit(&inst->vpu);
+	free_working_buf(inst);
+	kfree(inst);
+}
+
+const struct vdec_common_if vdec_vp8_if = {
+	.init		= vdec_vp8_init,
+	.decode		= vdec_vp8_decode,
+	.get_param	= vdec_vp8_get_param,
+	.deinit		= vdec_vp8_deinit,
+};
diff --git a/drivers/media/platform/mediatek/vcodec/vdec/vdec_vp8_req_if.c b/drivers/media/platform/mediatek/vcodec/vdec/vdec_vp8_req_if.c
new file mode 100644
index 000000000..e1fe2603e
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec/vdec_vp8_req_if.c
@@ -0,0 +1,436 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2021 MediaTek Inc.
+ * Author: Yunfei Dong <yunfei.dong@mediatek.com>
+ */
+
+#include <linux/slab.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+#include <uapi/linux/v4l2-controls.h>
+
+#include "../mtk_vcodec_util.h"
+#include "../mtk_vcodec_dec.h"
+#include "../mtk_vcodec_intr.h"
+#include "../vdec_drv_base.h"
+#include "../vdec_drv_if.h"
+#include "../vdec_vpu_if.h"
+
+/* Decoding picture buffer size (3 reference frames plus current frame) */
+#define VP8_DPB_SIZE 4
+
+/* HW working buffer size (bytes) */
+#define VP8_SEG_ID_SZ   SZ_256K
+#define VP8_PP_WRAPY_SZ SZ_64K
+#define VP8_PP_WRAPC_SZ SZ_64K
+#define VP8_VLD_PRED_SZ SZ_64K
+
+/**
+ * struct vdec_vp8_slice_info - decode misc information
+ *
+ * @vld_wrapper_dma:	vld wrapper dma address
+ * @seg_id_buf_dma:	seg id dma address
+ * @wrap_y_dma:	wrap y dma address
+ * @wrap_c_dma:	wrap y dma address
+ * @cur_y_fb_dma:	current plane Y frame buffer dma address
+ * @cur_c_fb_dma:	current plane C frame buffer dma address
+ * @bs_dma:		bitstream dma address
+ * @bs_sz:		bitstream size
+ * @resolution_changed:resolution change flag 1 - changed,  0 - not change
+ * @frame_header_type:	current frame header type
+ * @wait_key_frame:	wait key frame coming
+ * @crc:		used to check whether hardware's status is right
+ * @reserved:		reserved, currently unused
+ */
+struct vdec_vp8_slice_info {
+	u64 vld_wrapper_dma;
+	u64 seg_id_buf_dma;
+	u64 wrap_y_dma;
+	u64 wrap_c_dma;
+	u64 cur_y_fb_dma;
+	u64 cur_c_fb_dma;
+	u64 bs_dma;
+	u32 bs_sz;
+	u32 resolution_changed;
+	u32 frame_header_type;
+	u32 crc[8];
+	u32 reserved;
+};
+
+/**
+ * struct vdec_vp8_slice_dpb_info  - vp8 reference information
+ *
+ * @y_dma_addr:	Y bitstream physical address
+ * @c_dma_addr:	CbCr bitstream physical address
+ * @reference_flag:	reference picture flag
+ * @reserved:		64bit align
+ */
+struct vdec_vp8_slice_dpb_info {
+	dma_addr_t y_dma_addr;
+	dma_addr_t c_dma_addr;
+	int reference_flag;
+	int reserved;
+};
+
+/**
+ * struct vdec_vp8_slice_vsi - VPU shared information
+ *
+ * @dec:		decoding information
+ * @pic:		picture information
+ * @vp8_dpb_info:	reference buffer information
+ */
+struct vdec_vp8_slice_vsi {
+	struct vdec_vp8_slice_info dec;
+	struct vdec_pic_info pic;
+	struct vdec_vp8_slice_dpb_info vp8_dpb_info[3];
+};
+
+/**
+ * struct vdec_vp8_slice_inst - VP8 decoder instance
+ *
+ * @seg_id_buf:	seg buffer
+ * @wrap_y_buf:	wrapper y buffer
+ * @wrap_c_buf:	wrapper c buffer
+ * @vld_wrapper_buf:	vld wrapper buffer
+ * @ctx:		V4L2 context
+ * @vpu:		VPU instance for decoder
+ * @vsi:		VPU share information
+ */
+struct vdec_vp8_slice_inst {
+	struct mtk_vcodec_mem seg_id_buf;
+	struct mtk_vcodec_mem wrap_y_buf;
+	struct mtk_vcodec_mem wrap_c_buf;
+	struct mtk_vcodec_mem vld_wrapper_buf;
+	struct mtk_vcodec_ctx *ctx;
+	struct vdec_vpu_inst vpu;
+	struct vdec_vp8_slice_vsi *vsi;
+};
+
+static void *vdec_vp8_slice_get_ctrl_ptr(struct mtk_vcodec_ctx *ctx, int id)
+{
+	struct v4l2_ctrl *ctrl = v4l2_ctrl_find(&ctx->ctrl_hdl, id);
+
+	if (!ctrl)
+		return ERR_PTR(-EINVAL);
+
+	return ctrl->p_cur.p;
+}
+
+static void vdec_vp8_slice_get_pic_info(struct vdec_vp8_slice_inst *inst)
+{
+	struct mtk_vcodec_ctx *ctx = inst->ctx;
+	unsigned int data[3];
+
+	data[0] = ctx->picinfo.pic_w;
+	data[1] = ctx->picinfo.pic_h;
+	data[2] = ctx->capture_fourcc;
+	vpu_dec_get_param(&inst->vpu, data, 3, GET_PARAM_PIC_INFO);
+
+	ctx->picinfo.buf_w = ALIGN(ctx->picinfo.pic_w, 64);
+	ctx->picinfo.buf_h = ALIGN(ctx->picinfo.pic_h, 64);
+	ctx->picinfo.fb_sz[0] = inst->vpu.fb_sz[0];
+	ctx->picinfo.fb_sz[1] = inst->vpu.fb_sz[1];
+
+	inst->vsi->pic.pic_w = ctx->picinfo.pic_w;
+	inst->vsi->pic.pic_h = ctx->picinfo.pic_h;
+	inst->vsi->pic.buf_w = ctx->picinfo.buf_w;
+	inst->vsi->pic.buf_h = ctx->picinfo.buf_h;
+	inst->vsi->pic.fb_sz[0] = ctx->picinfo.fb_sz[0];
+	inst->vsi->pic.fb_sz[1] = ctx->picinfo.fb_sz[1];
+	mtk_vcodec_debug(inst, "pic(%d, %d), buf(%d, %d)",
+			 ctx->picinfo.pic_w, ctx->picinfo.pic_h,
+			 ctx->picinfo.buf_w, ctx->picinfo.buf_h);
+	mtk_vcodec_debug(inst, "fb size: Y(%d), C(%d)",
+			 ctx->picinfo.fb_sz[0], ctx->picinfo.fb_sz[1]);
+}
+
+static int vdec_vp8_slice_alloc_working_buf(struct vdec_vp8_slice_inst *inst)
+{
+	int err;
+	struct mtk_vcodec_mem *mem;
+
+	mem = &inst->seg_id_buf;
+	mem->size = VP8_SEG_ID_SZ;
+	err = mtk_vcodec_mem_alloc(inst->ctx, mem);
+	if (err) {
+		mtk_vcodec_err(inst, "Cannot allocate working buffer");
+		return err;
+	}
+	inst->vsi->dec.seg_id_buf_dma = (u64)mem->dma_addr;
+
+	mem = &inst->wrap_y_buf;
+	mem->size = VP8_PP_WRAPY_SZ;
+	err = mtk_vcodec_mem_alloc(inst->ctx, mem);
+	if (err) {
+		mtk_vcodec_err(inst, "cannot allocate WRAP Y buffer");
+		return err;
+	}
+	inst->vsi->dec.wrap_y_dma = (u64)mem->dma_addr;
+
+	mem = &inst->wrap_c_buf;
+	mem->size = VP8_PP_WRAPC_SZ;
+	err = mtk_vcodec_mem_alloc(inst->ctx, mem);
+	if (err) {
+		mtk_vcodec_err(inst, "cannot allocate WRAP C buffer");
+		return err;
+	}
+	inst->vsi->dec.wrap_c_dma = (u64)mem->dma_addr;
+
+	mem = &inst->vld_wrapper_buf;
+	mem->size = VP8_VLD_PRED_SZ;
+	err = mtk_vcodec_mem_alloc(inst->ctx, mem);
+	if (err) {
+		mtk_vcodec_err(inst, "cannot allocate vld wrapper buffer");
+		return err;
+	}
+	inst->vsi->dec.vld_wrapper_dma = (u64)mem->dma_addr;
+
+	return 0;
+}
+
+static void vdec_vp8_slice_free_working_buf(struct vdec_vp8_slice_inst *inst)
+{
+	struct mtk_vcodec_mem *mem;
+
+	mem = &inst->seg_id_buf;
+	if (mem->va)
+		mtk_vcodec_mem_free(inst->ctx, mem);
+	inst->vsi->dec.seg_id_buf_dma = 0;
+
+	mem = &inst->wrap_y_buf;
+	if (mem->va)
+		mtk_vcodec_mem_free(inst->ctx, mem);
+	inst->vsi->dec.wrap_y_dma = 0;
+
+	mem = &inst->wrap_c_buf;
+	if (mem->va)
+		mtk_vcodec_mem_free(inst->ctx, mem);
+	inst->vsi->dec.wrap_c_dma = 0;
+
+	mem = &inst->vld_wrapper_buf;
+	if (mem->va)
+		mtk_vcodec_mem_free(inst->ctx, mem);
+	inst->vsi->dec.vld_wrapper_dma = 0;
+}
+
+static u64 vdec_vp8_slice_get_ref_by_ts(const struct v4l2_ctrl_vp8_frame *frame_header,
+					int index)
+{
+	switch (index) {
+	case 0:
+		return frame_header->last_frame_ts;
+	case 1:
+		return frame_header->golden_frame_ts;
+	case 2:
+		return frame_header->alt_frame_ts;
+	default:
+		break;
+	}
+
+	return -1;
+}
+
+static int vdec_vp8_slice_get_decode_parameters(struct vdec_vp8_slice_inst *inst)
+{
+	const struct v4l2_ctrl_vp8_frame *frame_header;
+	struct mtk_vcodec_ctx *ctx = inst->ctx;
+	struct vb2_queue *vq;
+	struct vb2_buffer *vb;
+	u64 referenct_ts;
+	int index;
+
+	frame_header = vdec_vp8_slice_get_ctrl_ptr(inst->ctx, V4L2_CID_STATELESS_VP8_FRAME);
+	if (IS_ERR(frame_header))
+		return PTR_ERR(frame_header);
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	for (index = 0; index < 3; index++) {
+		referenct_ts = vdec_vp8_slice_get_ref_by_ts(frame_header, index);
+		vb = vb2_find_buffer(vq, referenct_ts);
+		if (!vb) {
+			if (!V4L2_VP8_FRAME_IS_KEY_FRAME(frame_header))
+				mtk_vcodec_err(inst, "reference invalid: index(%d) ts(%lld)",
+					       index, referenct_ts);
+			inst->vsi->vp8_dpb_info[index].reference_flag = 0;
+			continue;
+		}
+		inst->vsi->vp8_dpb_info[index].reference_flag = 1;
+
+		inst->vsi->vp8_dpb_info[index].y_dma_addr =
+			vb2_dma_contig_plane_dma_addr(vb, 0);
+		if (ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 2)
+			inst->vsi->vp8_dpb_info[index].c_dma_addr =
+				vb2_dma_contig_plane_dma_addr(vb, 1);
+		else
+			inst->vsi->vp8_dpb_info[index].c_dma_addr =
+				inst->vsi->vp8_dpb_info[index].y_dma_addr +
+				ctx->picinfo.fb_sz[0];
+	}
+
+	inst->vsi->dec.frame_header_type = frame_header->flags >> 1;
+
+	return 0;
+}
+
+static int vdec_vp8_slice_init(struct mtk_vcodec_ctx *ctx)
+{
+	struct vdec_vp8_slice_inst *inst;
+	int err;
+
+	inst = kzalloc(sizeof(*inst), GFP_KERNEL);
+	if (!inst)
+		return -ENOMEM;
+
+	inst->ctx = ctx;
+
+	inst->vpu.id = SCP_IPI_VDEC_LAT;
+	inst->vpu.core_id = SCP_IPI_VDEC_CORE;
+	inst->vpu.ctx = ctx;
+	inst->vpu.codec_type = ctx->current_codec;
+	inst->vpu.capture_type = ctx->capture_fourcc;
+
+	err = vpu_dec_init(&inst->vpu);
+	if (err) {
+		mtk_vcodec_err(inst, "vdec_vp8 init err=%d", err);
+		goto error_free_inst;
+	}
+
+	inst->vsi = inst->vpu.vsi;
+	err = vdec_vp8_slice_alloc_working_buf(inst);
+	if (err)
+		goto error_deinit;
+
+	mtk_vcodec_debug(inst, "vp8 struct size = %d vsi: %d\n",
+			 (int)sizeof(struct v4l2_ctrl_vp8_frame),
+			 (int)sizeof(struct vdec_vp8_slice_vsi));
+	mtk_vcodec_debug(inst, "vp8:%p, codec_type = 0x%x vsi: 0x%p",
+			 inst, inst->vpu.codec_type, inst->vpu.vsi);
+
+	ctx->drv_handle = inst;
+	return 0;
+
+error_deinit:
+	vpu_dec_deinit(&inst->vpu);
+error_free_inst:
+	kfree(inst);
+	return err;
+}
+
+static int vdec_vp8_slice_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
+				 struct vdec_fb *fb, bool *res_chg)
+{
+	struct vdec_vp8_slice_inst *inst = h_vdec;
+	struct vdec_vpu_inst *vpu = &inst->vpu;
+	struct mtk_video_dec_buf *src_buf_info, *dst_buf_info;
+	unsigned int data;
+	u64 y_fb_dma, c_fb_dma;
+	int err, timeout;
+
+	/* Resolution changes are never initiated by us */
+	*res_chg = false;
+
+	/* bs NULL means flush decoder */
+	if (!bs)
+		return vpu_dec_reset(vpu);
+
+	src_buf_info = container_of(bs, struct mtk_video_dec_buf, bs_buffer);
+
+	fb = inst->ctx->dev->vdec_pdata->get_cap_buffer(inst->ctx);
+	dst_buf_info = container_of(fb, struct mtk_video_dec_buf, frame_buffer);
+
+	y_fb_dma = fb ? (u64)fb->base_y.dma_addr : 0;
+	if (inst->ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 1)
+		c_fb_dma = y_fb_dma +
+			inst->ctx->picinfo.buf_w * inst->ctx->picinfo.buf_h;
+	else
+		c_fb_dma = fb ? (u64)fb->base_c.dma_addr : 0;
+
+	inst->vsi->dec.bs_dma = (u64)bs->dma_addr;
+	inst->vsi->dec.bs_sz = bs->size;
+	inst->vsi->dec.cur_y_fb_dma = y_fb_dma;
+	inst->vsi->dec.cur_c_fb_dma = c_fb_dma;
+
+	mtk_vcodec_debug(inst, "frame[%d] bs(%zu 0x%llx) y/c(0x%llx 0x%llx)",
+			 inst->ctx->decoded_frame_cnt,
+			 bs->size, (u64)bs->dma_addr,
+			 y_fb_dma, c_fb_dma);
+
+	v4l2_m2m_buf_copy_metadata(&src_buf_info->m2m_buf.vb,
+				   &dst_buf_info->m2m_buf.vb, true);
+
+	err = vdec_vp8_slice_get_decode_parameters(inst);
+	if (err)
+		goto error;
+
+	err = vpu_dec_start(vpu, &data, 1);
+	if (err) {
+		mtk_vcodec_debug(inst, "vp8 dec start err!");
+		goto error;
+	}
+
+	if (inst->vsi->dec.resolution_changed) {
+		mtk_vcodec_debug(inst, "- resolution_changed -");
+		*res_chg = true;
+		return 0;
+	}
+
+	/* wait decode done interrupt */
+	timeout = mtk_vcodec_wait_for_done_ctx(inst->ctx, MTK_INST_IRQ_RECEIVED,
+					       50, MTK_VDEC_CORE);
+
+	err = vpu_dec_end(vpu);
+	if (err || timeout)
+		mtk_vcodec_debug(inst, "vp8 dec error timeout:%d err: %d pic_%d",
+				 timeout, err, inst->ctx->decoded_frame_cnt);
+
+	mtk_vcodec_debug(inst, "pic[%d] crc: 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x",
+			 inst->ctx->decoded_frame_cnt,
+			 inst->vsi->dec.crc[0], inst->vsi->dec.crc[1],
+			 inst->vsi->dec.crc[2], inst->vsi->dec.crc[3],
+			 inst->vsi->dec.crc[4], inst->vsi->dec.crc[5],
+			 inst->vsi->dec.crc[6], inst->vsi->dec.crc[7]);
+
+	inst->ctx->decoded_frame_cnt++;
+error:
+	return err;
+}
+
+static int vdec_vp8_slice_get_param(void *h_vdec, enum vdec_get_param_type type, void *out)
+{
+	struct vdec_vp8_slice_inst *inst = h_vdec;
+
+	switch (type) {
+	case GET_PARAM_PIC_INFO:
+		vdec_vp8_slice_get_pic_info(inst);
+		break;
+	case GET_PARAM_CROP_INFO:
+		mtk_vcodec_debug(inst, "No need to get vp8 crop information.");
+		break;
+	case GET_PARAM_DPB_SIZE:
+		*((unsigned int *)out) = VP8_DPB_SIZE;
+		break;
+	default:
+		mtk_vcodec_err(inst, "invalid get parameter type=%d", type);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void vdec_vp8_slice_deinit(void *h_vdec)
+{
+	struct vdec_vp8_slice_inst *inst = h_vdec;
+
+	mtk_vcodec_debug_enter(inst);
+
+	vpu_dec_deinit(&inst->vpu);
+	vdec_vp8_slice_free_working_buf(inst);
+	kfree(inst);
+}
+
+const struct vdec_common_if vdec_vp8_slice_if = {
+	.init		= vdec_vp8_slice_init,
+	.decode		= vdec_vp8_slice_decode,
+	.get_param	= vdec_vp8_slice_get_param,
+	.deinit		= vdec_vp8_slice_deinit,
+};
diff --git a/drivers/media/platform/mediatek/vcodec/vdec/vdec_vp9_if.c b/drivers/media/platform/mediatek/vcodec/vdec/vdec_vp9_if.c
new file mode 100644
index 000000000..a27a109d8
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec/vdec_vp9_if.c
@@ -0,0 +1,1029 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Daniel Hsiao <daniel.hsiao@mediatek.com>
+ *	Kai-Sean Yang <kai-sean.yang@mediatek.com>
+ *	Tiffany Lin <tiffany.lin@mediatek.com>
+ */
+
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/syscalls.h>
+#include <linux/delay.h>
+#include <linux/time.h>
+
+#include "../mtk_vcodec_intr.h"
+#include "../vdec_drv_base.h"
+#include "../vdec_vpu_if.h"
+
+#define VP9_SUPER_FRAME_BS_SZ 64
+#define MAX_VP9_DPB_SIZE	9
+
+#define REFS_PER_FRAME 3
+#define MAX_NUM_REF_FRAMES 8
+#define VP9_MAX_FRM_BUF_NUM 9
+#define VP9_MAX_FRM_BUF_NODE_NUM (VP9_MAX_FRM_BUF_NUM * 2)
+#define VP9_SEG_ID_SZ 0x12000
+
+/**
+ * struct vp9_dram_buf - contains buffer info for vpu
+ * @va : cpu address
+ * @pa : iova address
+ * @sz : buffer size
+ * @padding : for 64 bytes alignment
+ */
+struct vp9_dram_buf {
+	unsigned long va;
+	unsigned long pa;
+	unsigned int sz;
+	unsigned int padding;
+};
+
+/**
+ * struct vp9_fb_info - contains frame buffer info
+ * @fb : frmae buffer
+ * @reserved : reserved field used by vpu
+ */
+struct vp9_fb_info {
+	struct vdec_fb *fb;
+	unsigned int reserved[32];
+};
+
+/**
+ * struct vp9_ref_cnt_buf - contains reference buffer information
+ * @buf : referenced frame buffer
+ * @ref_cnt : referenced frame buffer's reference count.
+ *	When reference count=0, remove it from reference list
+ */
+struct vp9_ref_cnt_buf {
+	struct vp9_fb_info buf;
+	unsigned int ref_cnt;
+};
+
+/**
+ * struct vp9_ref_buf - contains current frame's reference buffer information
+ * @buf : reference buffer
+ * @idx : reference buffer index to frm_bufs
+ * @reserved : reserved field used by vpu
+ */
+struct vp9_ref_buf {
+	struct vp9_fb_info *buf;
+	unsigned int idx;
+	unsigned int reserved[6];
+};
+
+/**
+ * struct vp9_sf_ref_fb - contains frame buffer info
+ * @fb : super frame reference frame buffer
+ * @used : this reference frame info entry is used
+ * @padding : for 64 bytes size align
+ */
+struct vp9_sf_ref_fb {
+	struct vdec_fb fb;
+	int used;
+	int padding;
+};
+
+/*
+ * struct vdec_vp9_vsi - shared buffer between host and VPU firmware
+ *	AP-W/R : AP is writer/reader on this item
+ *	VPU-W/R: VPU is write/reader on this item
+ * @sf_bs_buf : super frame backup buffer (AP-W, VPU-R)
+ * @sf_ref_fb : record supoer frame reference buffer information
+ *	(AP-R/W, VPU-R/W)
+ * @sf_next_ref_fb_idx : next available super frame (AP-W, VPU-R)
+ * @sf_frm_cnt : super frame count, filled by vpu (AP-R, VPU-W)
+ * @sf_frm_offset : super frame offset, filled by vpu (AP-R, VPU-W)
+ * @sf_frm_sz : super frame size, filled by vpu (AP-R, VPU-W)
+ * @sf_frm_idx : current super frame (AP-R, VPU-W)
+ * @sf_init : inform super frame info already parsed by vpu (AP-R, VPU-W)
+ * @fb : capture buffer (AP-W, VPU-R)
+ * @bs : bs buffer (AP-W, VPU-R)
+ * @cur_fb : current show capture buffer (AP-R/W, VPU-R/W)
+ * @pic_w : picture width (AP-R, VPU-W)
+ * @pic_h : picture height (AP-R, VPU-W)
+ * @buf_w : codec width (AP-R, VPU-W)
+ * @buf_h : coded height (AP-R, VPU-W)
+ * @buf_sz_y_bs : ufo compressed y plane size (AP-R, VPU-W)
+ * @buf_sz_c_bs : ufo compressed cbcr plane size (AP-R, VPU-W)
+ * @buf_len_sz_y : size used to store y plane ufo info (AP-R, VPU-W)
+ * @buf_len_sz_c : size used to store cbcr plane ufo info (AP-R, VPU-W)
+
+ * @profile : profile sparsed from vpu (AP-R, VPU-W)
+ * @show_frame : [BIT(0)] display this frame or not (AP-R, VPU-W)
+ *	[BIT(1)] reset segment data or not (AP-R, VPU-W)
+ *	[BIT(2)] trig decoder hardware or not (AP-R, VPU-W)
+ *	[BIT(3)] ask VPU to set bits(0~4) accordingly (AP-W, VPU-R)
+ *	[BIT(4)] do not reset segment data before every frame (AP-R, VPU-W)
+ * @show_existing_frame : inform this frame is show existing frame
+ *	(AP-R, VPU-W)
+ * @frm_to_show_idx : index to show frame (AP-R, VPU-W)
+
+ * @refresh_frm_flags : indicate when frame need to refine reference count
+ *	(AP-R, VPU-W)
+ * @resolution_changed : resolution change in this frame (AP-R, VPU-W)
+
+ * @frm_bufs : maintain reference buffer info (AP-R/W, VPU-R/W)
+ * @ref_frm_map : maintain reference buffer map info (AP-R/W, VPU-R/W)
+ * @new_fb_idx : index to frm_bufs array (AP-R, VPU-W)
+ * @frm_num : decoded frame number, include sub-frame count (AP-R, VPU-W)
+ * @mv_buf : motion vector working buffer (AP-W, VPU-R)
+ * @frm_refs : maintain three reference buffer info (AP-R/W, VPU-R/W)
+ * @seg_id_buf : segmentation map working buffer (AP-W, VPU-R)
+ */
+struct vdec_vp9_vsi {
+	unsigned char sf_bs_buf[VP9_SUPER_FRAME_BS_SZ];
+	struct vp9_sf_ref_fb sf_ref_fb[VP9_MAX_FRM_BUF_NUM-1];
+	int sf_next_ref_fb_idx;
+	unsigned int sf_frm_cnt;
+	unsigned int sf_frm_offset[VP9_MAX_FRM_BUF_NUM-1];
+	unsigned int sf_frm_sz[VP9_MAX_FRM_BUF_NUM-1];
+	unsigned int sf_frm_idx;
+	unsigned int sf_init;
+	struct vdec_fb fb;
+	struct mtk_vcodec_mem bs;
+	struct vdec_fb cur_fb;
+	unsigned int pic_w;
+	unsigned int pic_h;
+	unsigned int buf_w;
+	unsigned int buf_h;
+	unsigned int buf_sz_y_bs;
+	unsigned int buf_sz_c_bs;
+	unsigned int buf_len_sz_y;
+	unsigned int buf_len_sz_c;
+	unsigned int profile;
+	unsigned int show_frame;
+	unsigned int show_existing_frame;
+	unsigned int frm_to_show_idx;
+	unsigned int refresh_frm_flags;
+	unsigned int resolution_changed;
+
+	struct vp9_ref_cnt_buf frm_bufs[VP9_MAX_FRM_BUF_NUM];
+	int ref_frm_map[MAX_NUM_REF_FRAMES];
+	unsigned int new_fb_idx;
+	unsigned int frm_num;
+	struct vp9_dram_buf mv_buf;
+
+	struct vp9_ref_buf frm_refs[REFS_PER_FRAME];
+	struct vp9_dram_buf seg_id_buf;
+
+};
+
+/*
+ * struct vdec_vp9_inst - vp9 decode instance
+ * @mv_buf : working buffer for mv
+ * @seg_id_buf : working buffer for segmentation map
+ * @dec_fb : vdec_fb node to link fb to different fb_xxx_list
+ * @available_fb_node_list : current available vdec_fb node
+ * @fb_use_list : current used or referenced vdec_fb
+ * @fb_free_list : current available to free vdec_fb
+ * @fb_disp_list : current available to display vdec_fb
+ * @cur_fb : current frame buffer
+ * @ctx : current decode context
+ * @vpu : vpu instance information
+ * @vsi : shared buffer between host and VPU firmware
+ * @total_frm_cnt : total frame count, it do not include sub-frames in super
+ *	    frame
+ * @mem : instance memory information
+ */
+struct vdec_vp9_inst {
+	struct mtk_vcodec_mem mv_buf;
+	struct mtk_vcodec_mem seg_id_buf;
+
+	struct vdec_fb_node dec_fb[VP9_MAX_FRM_BUF_NODE_NUM];
+	struct list_head available_fb_node_list;
+	struct list_head fb_use_list;
+	struct list_head fb_free_list;
+	struct list_head fb_disp_list;
+	struct vdec_fb *cur_fb;
+	struct mtk_vcodec_ctx *ctx;
+	struct vdec_vpu_inst vpu;
+	struct vdec_vp9_vsi *vsi;
+	unsigned int total_frm_cnt;
+	struct mtk_vcodec_mem mem;
+};
+
+static bool vp9_is_sf_ref_fb(struct vdec_vp9_inst *inst, struct vdec_fb *fb)
+{
+	int i;
+	struct vdec_vp9_vsi *vsi = inst->vsi;
+
+	for (i = 0; i < ARRAY_SIZE(vsi->sf_ref_fb); i++) {
+		if (fb == &vsi->sf_ref_fb[i].fb)
+			return true;
+	}
+	return false;
+}
+
+static struct vdec_fb *vp9_rm_from_fb_use_list(struct vdec_vp9_inst
+					*inst, void *addr)
+{
+	struct vdec_fb *fb = NULL;
+	struct vdec_fb_node *node;
+
+	list_for_each_entry(node, &inst->fb_use_list, list) {
+		fb = (struct vdec_fb *)node->fb;
+		if (fb->base_y.va == addr) {
+			list_move_tail(&node->list,
+				       &inst->available_fb_node_list);
+			return fb;
+		}
+	}
+
+	return NULL;
+}
+
+static void vp9_add_to_fb_free_list(struct vdec_vp9_inst *inst,
+			     struct vdec_fb *fb)
+{
+	struct vdec_fb_node *node;
+
+	if (fb) {
+		node = list_first_entry_or_null(&inst->available_fb_node_list,
+					struct vdec_fb_node, list);
+
+		if (node) {
+			node->fb = fb;
+			list_move_tail(&node->list, &inst->fb_free_list);
+		}
+	} else {
+		mtk_vcodec_debug(inst, "No free fb node");
+	}
+}
+
+static void vp9_free_sf_ref_fb(struct vdec_fb *fb)
+{
+	struct vp9_sf_ref_fb *sf_ref_fb =
+		container_of(fb, struct vp9_sf_ref_fb, fb);
+
+	sf_ref_fb->used = 0;
+}
+
+static void vp9_ref_cnt_fb(struct vdec_vp9_inst *inst, int *idx,
+			   int new_idx)
+{
+	struct vdec_vp9_vsi *vsi = inst->vsi;
+	int ref_idx = *idx;
+
+	if (ref_idx >= 0 && vsi->frm_bufs[ref_idx].ref_cnt > 0) {
+		vsi->frm_bufs[ref_idx].ref_cnt--;
+
+		if (vsi->frm_bufs[ref_idx].ref_cnt == 0) {
+			if (!vp9_is_sf_ref_fb(inst,
+					      vsi->frm_bufs[ref_idx].buf.fb)) {
+				struct vdec_fb *fb;
+
+				fb = vp9_rm_from_fb_use_list(inst,
+				     vsi->frm_bufs[ref_idx].buf.fb->base_y.va);
+				vp9_add_to_fb_free_list(inst, fb);
+			} else
+				vp9_free_sf_ref_fb(
+					vsi->frm_bufs[ref_idx].buf.fb);
+		}
+	}
+
+	*idx = new_idx;
+	vsi->frm_bufs[new_idx].ref_cnt++;
+}
+
+static void vp9_free_all_sf_ref_fb(struct vdec_vp9_inst *inst)
+{
+	int i;
+	struct vdec_vp9_vsi *vsi = inst->vsi;
+
+	for (i = 0; i < ARRAY_SIZE(vsi->sf_ref_fb); i++) {
+		if (vsi->sf_ref_fb[i].fb.base_y.va) {
+			mtk_vcodec_mem_free(inst->ctx,
+				&vsi->sf_ref_fb[i].fb.base_y);
+			mtk_vcodec_mem_free(inst->ctx,
+				&vsi->sf_ref_fb[i].fb.base_c);
+			vsi->sf_ref_fb[i].used = 0;
+		}
+	}
+}
+
+/* For each sub-frame except the last one, the driver will dynamically
+ * allocate reference buffer by calling vp9_get_sf_ref_fb()
+ * The last sub-frame will use the original fb provided by the
+ * vp9_dec_decode() interface
+ */
+static int vp9_get_sf_ref_fb(struct vdec_vp9_inst *inst)
+{
+	int idx;
+	struct mtk_vcodec_mem *mem_basy_y;
+	struct mtk_vcodec_mem *mem_basy_c;
+	struct vdec_vp9_vsi *vsi = inst->vsi;
+
+	for (idx = 0;
+		idx < ARRAY_SIZE(vsi->sf_ref_fb);
+		idx++) {
+		if (vsi->sf_ref_fb[idx].fb.base_y.va &&
+		    vsi->sf_ref_fb[idx].used == 0) {
+			return idx;
+		}
+	}
+
+	for (idx = 0;
+		idx < ARRAY_SIZE(vsi->sf_ref_fb);
+		idx++) {
+		if (vsi->sf_ref_fb[idx].fb.base_y.va == NULL)
+			break;
+	}
+
+	if (idx == ARRAY_SIZE(vsi->sf_ref_fb)) {
+		mtk_vcodec_err(inst, "List Full");
+		return -1;
+	}
+
+	mem_basy_y = &vsi->sf_ref_fb[idx].fb.base_y;
+	mem_basy_y->size = vsi->buf_sz_y_bs +
+		vsi->buf_len_sz_y;
+
+	if (mtk_vcodec_mem_alloc(inst->ctx, mem_basy_y)) {
+		mtk_vcodec_err(inst, "Cannot allocate sf_ref_buf y_buf");
+		return -1;
+	}
+
+	mem_basy_c = &vsi->sf_ref_fb[idx].fb.base_c;
+	mem_basy_c->size = vsi->buf_sz_c_bs +
+		vsi->buf_len_sz_c;
+
+	if (mtk_vcodec_mem_alloc(inst->ctx, mem_basy_c)) {
+		mtk_vcodec_err(inst, "Cannot allocate sf_ref_fb c_buf");
+		return -1;
+	}
+	vsi->sf_ref_fb[idx].used = 0;
+
+	return idx;
+}
+
+static bool vp9_alloc_work_buf(struct vdec_vp9_inst *inst)
+{
+	struct vdec_vp9_vsi *vsi = inst->vsi;
+	int result;
+	struct mtk_vcodec_mem *mem;
+
+	unsigned int max_pic_w;
+	unsigned int max_pic_h;
+
+
+	if (!(inst->ctx->dev->dec_capability &
+		VCODEC_CAPABILITY_4K_DISABLED)) {
+		max_pic_w = VCODEC_DEC_4K_CODED_WIDTH;
+		max_pic_h = VCODEC_DEC_4K_CODED_HEIGHT;
+	} else {
+		max_pic_w = MTK_VDEC_MAX_W;
+		max_pic_h = MTK_VDEC_MAX_H;
+	}
+
+	if ((vsi->pic_w > max_pic_w) ||
+		(vsi->pic_h > max_pic_h)) {
+		mtk_vcodec_err(inst, "Invalid w/h %d/%d",
+				vsi->pic_w, vsi->pic_h);
+		return false;
+	}
+
+	mtk_vcodec_debug(inst, "BUF CHG(%d): w/h/sb_w/sb_h=%d/%d/%d/%d",
+			vsi->resolution_changed,
+			vsi->pic_w,
+			vsi->pic_h,
+			vsi->buf_w,
+			vsi->buf_h);
+
+	mem = &inst->mv_buf;
+	if (mem->va)
+		mtk_vcodec_mem_free(inst->ctx, mem);
+
+	mem->size = ((vsi->buf_w / 64) *
+		    (vsi->buf_h / 64) + 2) * 36 * 16;
+	result = mtk_vcodec_mem_alloc(inst->ctx, mem);
+	if (result) {
+		mem->size = 0;
+		mtk_vcodec_err(inst, "Cannot allocate mv_buf");
+		return false;
+	}
+	/* Set the va again */
+	vsi->mv_buf.va = (unsigned long)mem->va;
+	vsi->mv_buf.pa = (unsigned long)mem->dma_addr;
+	vsi->mv_buf.sz = (unsigned int)mem->size;
+
+
+	mem = &inst->seg_id_buf;
+	if (mem->va)
+		mtk_vcodec_mem_free(inst->ctx, mem);
+
+	mem->size = VP9_SEG_ID_SZ;
+	result = mtk_vcodec_mem_alloc(inst->ctx, mem);
+	if (result) {
+		mem->size = 0;
+		mtk_vcodec_err(inst, "Cannot allocate seg_id_buf");
+		return false;
+	}
+	/* Set the va again */
+	vsi->seg_id_buf.va = (unsigned long)mem->va;
+	vsi->seg_id_buf.pa = (unsigned long)mem->dma_addr;
+	vsi->seg_id_buf.sz = (unsigned int)mem->size;
+
+
+	vp9_free_all_sf_ref_fb(inst);
+	vsi->sf_next_ref_fb_idx = vp9_get_sf_ref_fb(inst);
+
+	return true;
+}
+
+static bool vp9_add_to_fb_disp_list(struct vdec_vp9_inst *inst,
+			     struct vdec_fb *fb)
+{
+	struct vdec_fb_node *node;
+
+	if (!fb) {
+		mtk_vcodec_err(inst, "fb == NULL");
+		return false;
+	}
+
+	node = list_first_entry_or_null(&inst->available_fb_node_list,
+					struct vdec_fb_node, list);
+	if (node) {
+		node->fb = fb;
+		list_move_tail(&node->list, &inst->fb_disp_list);
+	} else {
+		mtk_vcodec_err(inst, "No available fb node");
+		return false;
+	}
+
+	return true;
+}
+
+/* If any buffer updating is signaled it should be done here. */
+static void vp9_swap_frm_bufs(struct vdec_vp9_inst *inst)
+{
+	struct vdec_vp9_vsi *vsi = inst->vsi;
+	struct vp9_fb_info *frm_to_show;
+	int ref_index = 0, mask;
+
+	for (mask = vsi->refresh_frm_flags; mask; mask >>= 1) {
+		if (mask & 1)
+			vp9_ref_cnt_fb(inst, &vsi->ref_frm_map[ref_index],
+				       vsi->new_fb_idx);
+		++ref_index;
+	}
+
+	frm_to_show = &vsi->frm_bufs[vsi->new_fb_idx].buf;
+	vsi->frm_bufs[vsi->new_fb_idx].ref_cnt--;
+
+	if (frm_to_show->fb != inst->cur_fb) {
+		/* This frame is show exist frame and no decode output
+		 * copy frame data from frm_to_show to current CAPTURE
+		 * buffer
+		 */
+		if ((frm_to_show->fb != NULL) &&
+			(inst->cur_fb->base_y.size >=
+			frm_to_show->fb->base_y.size) &&
+			(inst->cur_fb->base_c.size >=
+			frm_to_show->fb->base_c.size)) {
+			memcpy((void *)inst->cur_fb->base_y.va,
+				(void *)frm_to_show->fb->base_y.va,
+				frm_to_show->fb->base_y.size);
+			memcpy((void *)inst->cur_fb->base_c.va,
+				(void *)frm_to_show->fb->base_c.va,
+				frm_to_show->fb->base_c.size);
+		} else {
+			/* After resolution change case, current CAPTURE buffer
+			 * may have less buffer size than frm_to_show buffer
+			 * size
+			 */
+			if (frm_to_show->fb != NULL)
+				mtk_vcodec_err(inst,
+					"inst->cur_fb->base_y.size=%zu, frm_to_show->fb.base_y.size=%zu",
+					inst->cur_fb->base_y.size,
+					frm_to_show->fb->base_y.size);
+		}
+		if (!vp9_is_sf_ref_fb(inst, inst->cur_fb)) {
+			if (vsi->show_frame & BIT(0))
+				vp9_add_to_fb_disp_list(inst, inst->cur_fb);
+		}
+	} else {
+		if (!vp9_is_sf_ref_fb(inst, inst->cur_fb)) {
+			if (vsi->show_frame & BIT(0))
+				vp9_add_to_fb_disp_list(inst, frm_to_show->fb);
+		}
+	}
+
+	/* when ref_cnt ==0, move this fb to fb_free_list. v4l2 driver will
+	 * clean fb_free_list
+	 */
+	if (vsi->frm_bufs[vsi->new_fb_idx].ref_cnt == 0) {
+		if (!vp9_is_sf_ref_fb(
+			inst, vsi->frm_bufs[vsi->new_fb_idx].buf.fb)) {
+			struct vdec_fb *fb;
+
+			fb = vp9_rm_from_fb_use_list(inst,
+			vsi->frm_bufs[vsi->new_fb_idx].buf.fb->base_y.va);
+
+			vp9_add_to_fb_free_list(inst, fb);
+		} else {
+			vp9_free_sf_ref_fb(
+				vsi->frm_bufs[vsi->new_fb_idx].buf.fb);
+		}
+	}
+
+	/* if this super frame and it is not last sub-frame, get next fb for
+	 * sub-frame decode
+	 */
+	if (vsi->sf_frm_cnt > 0 && vsi->sf_frm_idx != vsi->sf_frm_cnt - 1)
+		vsi->sf_next_ref_fb_idx = vp9_get_sf_ref_fb(inst);
+}
+
+static bool vp9_wait_dec_end(struct vdec_vp9_inst *inst)
+{
+	struct mtk_vcodec_ctx *ctx = inst->ctx;
+
+	mtk_vcodec_wait_for_done_ctx(inst->ctx,
+			MTK_INST_IRQ_RECEIVED,
+			WAIT_INTR_TIMEOUT_MS, 0);
+
+	if (ctx->irq_status & MTK_VDEC_IRQ_STATUS_DEC_SUCCESS)
+		return true;
+	else
+		return false;
+}
+
+static struct vdec_vp9_inst *vp9_alloc_inst(struct mtk_vcodec_ctx *ctx)
+{
+	int result;
+	struct mtk_vcodec_mem mem;
+	struct vdec_vp9_inst *inst;
+
+	memset(&mem, 0, sizeof(mem));
+	mem.size = sizeof(struct vdec_vp9_inst);
+	result = mtk_vcodec_mem_alloc(ctx, &mem);
+	if (result)
+		return NULL;
+
+	inst = mem.va;
+	inst->mem = mem;
+
+	return inst;
+}
+
+static void vp9_free_inst(struct vdec_vp9_inst *inst)
+{
+	struct mtk_vcodec_mem mem;
+
+	mem = inst->mem;
+	if (mem.va)
+		mtk_vcodec_mem_free(inst->ctx, &mem);
+}
+
+static bool vp9_decode_end_proc(struct vdec_vp9_inst *inst)
+{
+	struct vdec_vp9_vsi *vsi = inst->vsi;
+	bool ret = false;
+
+	if (!vsi->show_existing_frame) {
+		ret = vp9_wait_dec_end(inst);
+		if (!ret) {
+			mtk_vcodec_err(inst, "Decode failed, Decode Timeout @[%d]",
+				vsi->frm_num);
+			return false;
+		}
+
+		if (vpu_dec_end(&inst->vpu)) {
+			mtk_vcodec_err(inst, "vp9_dec_vpu_end failed");
+			return false;
+		}
+		mtk_vcodec_debug(inst, "Decode Ok @%d (%d/%d)", vsi->frm_num,
+				vsi->pic_w, vsi->pic_h);
+	} else {
+		mtk_vcodec_debug(inst, "Decode Ok @%d (show_existing_frame)",
+				vsi->frm_num);
+	}
+
+	vp9_swap_frm_bufs(inst);
+	vsi->frm_num++;
+	return true;
+}
+
+static bool vp9_is_last_sub_frm(struct vdec_vp9_inst *inst)
+{
+	struct vdec_vp9_vsi *vsi = inst->vsi;
+
+	if (vsi->sf_frm_cnt <= 0 || vsi->sf_frm_idx == vsi->sf_frm_cnt)
+		return true;
+
+	return false;
+}
+
+static struct vdec_fb *vp9_rm_from_fb_disp_list(struct vdec_vp9_inst *inst)
+{
+	struct vdec_fb_node *node;
+	struct vdec_fb *fb = NULL;
+
+	node = list_first_entry_or_null(&inst->fb_disp_list,
+					struct vdec_fb_node, list);
+	if (node) {
+		fb = (struct vdec_fb *)node->fb;
+		fb->status |= FB_ST_DISPLAY;
+		list_move_tail(&node->list, &inst->available_fb_node_list);
+		mtk_vcodec_debug(inst, "[FB] get disp fb %p st=%d",
+				 node->fb, fb->status);
+	} else
+		mtk_vcodec_debug(inst, "[FB] there is no disp fb");
+
+	return fb;
+}
+
+static bool vp9_add_to_fb_use_list(struct vdec_vp9_inst *inst,
+			    struct vdec_fb *fb)
+{
+	struct vdec_fb_node *node;
+
+	if (!fb) {
+		mtk_vcodec_debug(inst, "fb == NULL");
+		return false;
+	}
+
+	node = list_first_entry_or_null(&inst->available_fb_node_list,
+					struct vdec_fb_node, list);
+	if (node) {
+		node->fb = fb;
+		list_move_tail(&node->list, &inst->fb_use_list);
+	} else {
+		mtk_vcodec_err(inst, "No free fb node");
+		return false;
+	}
+	return true;
+}
+
+static void vp9_reset(struct vdec_vp9_inst *inst)
+{
+	struct vdec_fb_node *node, *tmp;
+
+	list_for_each_entry_safe(node, tmp, &inst->fb_use_list, list)
+		list_move_tail(&node->list, &inst->fb_free_list);
+
+	vp9_free_all_sf_ref_fb(inst);
+	inst->vsi->sf_next_ref_fb_idx = vp9_get_sf_ref_fb(inst);
+
+	if (vpu_dec_reset(&inst->vpu))
+		mtk_vcodec_err(inst, "vp9_dec_vpu_reset failed");
+
+	/* Set the va again, since vpu_dec_reset will clear mv_buf in vpu */
+	inst->vsi->mv_buf.va = (unsigned long)inst->mv_buf.va;
+	inst->vsi->mv_buf.pa = (unsigned long)inst->mv_buf.dma_addr;
+	inst->vsi->mv_buf.sz = (unsigned long)inst->mv_buf.size;
+
+	/* Set the va again, since vpu_dec_reset will clear seg_id_buf in vpu */
+	inst->vsi->seg_id_buf.va = (unsigned long)inst->seg_id_buf.va;
+	inst->vsi->seg_id_buf.pa = (unsigned long)inst->seg_id_buf.dma_addr;
+	inst->vsi->seg_id_buf.sz = (unsigned long)inst->seg_id_buf.size;
+
+}
+
+static void init_all_fb_lists(struct vdec_vp9_inst *inst)
+{
+	int i;
+
+	INIT_LIST_HEAD(&inst->available_fb_node_list);
+	INIT_LIST_HEAD(&inst->fb_use_list);
+	INIT_LIST_HEAD(&inst->fb_free_list);
+	INIT_LIST_HEAD(&inst->fb_disp_list);
+
+	for (i = 0; i < ARRAY_SIZE(inst->dec_fb); i++) {
+		INIT_LIST_HEAD(&inst->dec_fb[i].list);
+		inst->dec_fb[i].fb = NULL;
+		list_add_tail(&inst->dec_fb[i].list,
+			      &inst->available_fb_node_list);
+	}
+}
+
+static void get_pic_info(struct vdec_vp9_inst *inst, struct vdec_pic_info *pic)
+{
+	pic->fb_sz[0] = inst->vsi->buf_sz_y_bs + inst->vsi->buf_len_sz_y;
+	pic->fb_sz[1] = inst->vsi->buf_sz_c_bs + inst->vsi->buf_len_sz_c;
+
+	pic->pic_w = inst->vsi->pic_w;
+	pic->pic_h = inst->vsi->pic_h;
+	pic->buf_w = inst->vsi->buf_w;
+	pic->buf_h = inst->vsi->buf_h;
+
+	mtk_vcodec_debug(inst, "pic(%d, %d), buf(%d, %d)",
+		 pic->pic_w, pic->pic_h, pic->buf_w, pic->buf_h);
+	mtk_vcodec_debug(inst, "fb size: Y(%d), C(%d)",
+		pic->fb_sz[0],
+		pic->fb_sz[1]);
+}
+
+static void get_disp_fb(struct vdec_vp9_inst *inst, struct vdec_fb **out_fb)
+{
+
+	*out_fb = vp9_rm_from_fb_disp_list(inst);
+	if (*out_fb)
+		(*out_fb)->status |= FB_ST_DISPLAY;
+}
+
+static void get_free_fb(struct vdec_vp9_inst *inst, struct vdec_fb **out_fb)
+{
+	struct vdec_fb_node *node;
+	struct vdec_fb *fb = NULL;
+
+	node = list_first_entry_or_null(&inst->fb_free_list,
+					struct vdec_fb_node, list);
+	if (node) {
+		list_move_tail(&node->list, &inst->available_fb_node_list);
+		fb = (struct vdec_fb *)node->fb;
+		fb->status |= FB_ST_FREE;
+		mtk_vcodec_debug(inst, "[FB] get free fb %p st=%d",
+				 node->fb, fb->status);
+	} else {
+		mtk_vcodec_debug(inst, "[FB] there is no free fb");
+	}
+
+	*out_fb = fb;
+}
+
+static int validate_vsi_array_indexes(struct vdec_vp9_inst *inst,
+		struct vdec_vp9_vsi *vsi) {
+	if (vsi->sf_frm_idx >= VP9_MAX_FRM_BUF_NUM - 1) {
+		mtk_vcodec_err(inst, "Invalid vsi->sf_frm_idx=%u.",
+				vsi->sf_frm_idx);
+		return -EIO;
+	}
+	if (vsi->frm_to_show_idx >= VP9_MAX_FRM_BUF_NUM) {
+		mtk_vcodec_err(inst, "Invalid vsi->frm_to_show_idx=%u.",
+				vsi->frm_to_show_idx);
+		return -EIO;
+	}
+	if (vsi->new_fb_idx >= VP9_MAX_FRM_BUF_NUM) {
+		mtk_vcodec_err(inst, "Invalid vsi->new_fb_idx=%u.",
+				vsi->new_fb_idx);
+		return -EIO;
+	}
+	return 0;
+}
+
+static void vdec_vp9_deinit(void *h_vdec)
+{
+	struct vdec_vp9_inst *inst = (struct vdec_vp9_inst *)h_vdec;
+	struct mtk_vcodec_mem *mem;
+	int ret = 0;
+
+	ret = vpu_dec_deinit(&inst->vpu);
+	if (ret)
+		mtk_vcodec_err(inst, "vpu_dec_deinit failed");
+
+	mem = &inst->mv_buf;
+	if (mem->va)
+		mtk_vcodec_mem_free(inst->ctx, mem);
+
+	mem = &inst->seg_id_buf;
+	if (mem->va)
+		mtk_vcodec_mem_free(inst->ctx, mem);
+
+	vp9_free_all_sf_ref_fb(inst);
+	vp9_free_inst(inst);
+}
+
+static int vdec_vp9_init(struct mtk_vcodec_ctx *ctx)
+{
+	struct vdec_vp9_inst *inst;
+
+	inst = vp9_alloc_inst(ctx);
+	if (!inst)
+		return -ENOMEM;
+
+	inst->total_frm_cnt = 0;
+	inst->ctx = ctx;
+
+	inst->vpu.id = IPI_VDEC_VP9;
+	inst->vpu.ctx = ctx;
+
+	if (vpu_dec_init(&inst->vpu)) {
+		mtk_vcodec_err(inst, "vp9_dec_vpu_init failed");
+		goto err_deinit_inst;
+	}
+
+	inst->vsi = (struct vdec_vp9_vsi *)inst->vpu.vsi;
+
+	inst->vsi->show_frame |= BIT(3);
+
+	init_all_fb_lists(inst);
+
+	ctx->drv_handle = inst;
+	return 0;
+
+err_deinit_inst:
+	vp9_free_inst(inst);
+
+	return -EINVAL;
+}
+
+static int vdec_vp9_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
+			   struct vdec_fb *fb, bool *res_chg)
+{
+	int ret = 0;
+	struct vdec_vp9_inst *inst = (struct vdec_vp9_inst *)h_vdec;
+	struct vdec_vp9_vsi *vsi = inst->vsi;
+	u32 data[3];
+	int i;
+
+	*res_chg = false;
+
+	if ((bs == NULL) && (fb == NULL)) {
+		mtk_vcodec_debug(inst, "[EOS]");
+		vp9_reset(inst);
+		return ret;
+	}
+
+	if (bs == NULL) {
+		mtk_vcodec_err(inst, "bs == NULL");
+		return -EINVAL;
+	}
+
+	mtk_vcodec_debug(inst, "Input BS Size = %zu", bs->size);
+
+	while (1) {
+		struct vdec_fb *cur_fb = NULL;
+
+		data[0] = *((unsigned int *)bs->va);
+		data[1] = *((unsigned int *)(bs->va + 4));
+		data[2] = *((unsigned int *)(bs->va + 8));
+
+		vsi->bs = *bs;
+
+		if (fb)
+			vsi->fb = *fb;
+
+		if (!vsi->sf_init) {
+			unsigned int sf_bs_sz;
+			unsigned int sf_bs_off;
+			unsigned char *sf_bs_src;
+			unsigned char *sf_bs_dst;
+
+			sf_bs_sz = bs->size > VP9_SUPER_FRAME_BS_SZ ?
+					VP9_SUPER_FRAME_BS_SZ : bs->size;
+			sf_bs_off = VP9_SUPER_FRAME_BS_SZ - sf_bs_sz;
+			sf_bs_src = bs->va + bs->size - sf_bs_sz;
+			sf_bs_dst = vsi->sf_bs_buf + sf_bs_off;
+			memcpy(sf_bs_dst, sf_bs_src, sf_bs_sz);
+		} else {
+			if ((vsi->sf_frm_cnt > 0) &&
+				(vsi->sf_frm_idx < vsi->sf_frm_cnt)) {
+				unsigned int idx = vsi->sf_frm_idx;
+
+				memcpy((void *)bs->va,
+					(void *)(bs->va +
+					vsi->sf_frm_offset[idx]),
+					vsi->sf_frm_sz[idx]);
+			}
+		}
+
+		if (!(vsi->show_frame & BIT(4)))
+			memset(inst->seg_id_buf.va, 0, inst->seg_id_buf.size);
+
+		ret = vpu_dec_start(&inst->vpu, data, 3);
+		if (ret) {
+			mtk_vcodec_err(inst, "vpu_dec_start failed");
+			goto DECODE_ERROR;
+		}
+
+		if (vsi->show_frame & BIT(1)) {
+			memset(inst->seg_id_buf.va, 0, inst->seg_id_buf.size);
+
+			if (vsi->show_frame & BIT(2)) {
+				ret = vpu_dec_start(&inst->vpu, NULL, 0);
+				if (ret) {
+					mtk_vcodec_err(inst, "vpu trig decoder failed");
+					goto DECODE_ERROR;
+				}
+			}
+		}
+
+		ret = validate_vsi_array_indexes(inst, vsi);
+		if (ret) {
+			mtk_vcodec_err(inst, "Invalid values from VPU.");
+			goto DECODE_ERROR;
+		}
+
+		if (vsi->resolution_changed) {
+			if (!vp9_alloc_work_buf(inst)) {
+				ret = -EIO;
+				goto DECODE_ERROR;
+			}
+		}
+
+		if (vsi->sf_frm_cnt > 0) {
+			cur_fb = &vsi->sf_ref_fb[vsi->sf_next_ref_fb_idx].fb;
+
+			if (vsi->sf_frm_idx < vsi->sf_frm_cnt)
+				inst->cur_fb = cur_fb;
+			else
+				inst->cur_fb = fb;
+		} else {
+			inst->cur_fb = fb;
+		}
+
+		vsi->frm_bufs[vsi->new_fb_idx].buf.fb = inst->cur_fb;
+		if (!vp9_is_sf_ref_fb(inst, inst->cur_fb))
+			vp9_add_to_fb_use_list(inst, inst->cur_fb);
+
+		mtk_vcodec_debug(inst, "[#pic %d]", vsi->frm_num);
+
+		if (vsi->show_existing_frame)
+			mtk_vcodec_debug(inst,
+				"drv->new_fb_idx=%d, drv->frm_to_show_idx=%d",
+				vsi->new_fb_idx, vsi->frm_to_show_idx);
+
+		if (vsi->show_existing_frame && (vsi->frm_to_show_idx <
+					VP9_MAX_FRM_BUF_NUM)) {
+			mtk_vcodec_debug(inst,
+				"Skip Decode drv->new_fb_idx=%d, drv->frm_to_show_idx=%d",
+				vsi->new_fb_idx, vsi->frm_to_show_idx);
+
+			vp9_ref_cnt_fb(inst, &vsi->new_fb_idx,
+					vsi->frm_to_show_idx);
+		}
+
+		/* VPU assign the buffer pointer in its address space,
+		 * reassign here
+		 */
+		for (i = 0; i < ARRAY_SIZE(vsi->frm_refs); i++) {
+			unsigned int idx = vsi->frm_refs[i].idx;
+
+			vsi->frm_refs[i].buf = &vsi->frm_bufs[idx].buf;
+		}
+
+		if (vsi->resolution_changed) {
+			*res_chg = true;
+			mtk_vcodec_debug(inst, "VDEC_ST_RESOLUTION_CHANGED");
+
+			ret = 0;
+			goto DECODE_ERROR;
+		}
+
+		if (!vp9_decode_end_proc(inst)) {
+			mtk_vcodec_err(inst, "vp9_decode_end_proc");
+			ret = -EINVAL;
+			goto DECODE_ERROR;
+		}
+
+		if (vp9_is_last_sub_frm(inst))
+			break;
+
+	}
+	inst->total_frm_cnt++;
+
+DECODE_ERROR:
+	if (ret < 0)
+		vp9_add_to_fb_free_list(inst, fb);
+
+	return ret;
+}
+
+static void get_crop_info(struct vdec_vp9_inst *inst, struct v4l2_rect *cr)
+{
+	cr->left = 0;
+	cr->top = 0;
+	cr->width = inst->vsi->pic_w;
+	cr->height = inst->vsi->pic_h;
+	mtk_vcodec_debug(inst, "get crop info l=%d, t=%d, w=%d, h=%d\n",
+			 cr->left, cr->top, cr->width, cr->height);
+}
+
+static int vdec_vp9_get_param(void *h_vdec, enum vdec_get_param_type type,
+			      void *out)
+{
+	struct vdec_vp9_inst *inst = (struct vdec_vp9_inst *)h_vdec;
+	int ret = 0;
+
+	switch (type) {
+	case GET_PARAM_DISP_FRAME_BUFFER:
+		get_disp_fb(inst, out);
+		break;
+	case GET_PARAM_FREE_FRAME_BUFFER:
+		get_free_fb(inst, out);
+		break;
+	case GET_PARAM_PIC_INFO:
+		get_pic_info(inst, out);
+		break;
+	case GET_PARAM_DPB_SIZE:
+		*((unsigned int *)out) = MAX_VP9_DPB_SIZE;
+		break;
+	case GET_PARAM_CROP_INFO:
+		get_crop_info(inst, out);
+		break;
+	default:
+		mtk_vcodec_err(inst, "not supported param type %d", type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+const struct vdec_common_if vdec_vp9_if = {
+	.init		= vdec_vp9_init,
+	.decode		= vdec_vp9_decode,
+	.get_param	= vdec_vp9_get_param,
+	.deinit		= vdec_vp9_deinit,
+};
diff --git a/drivers/media/platform/mediatek/vcodec/vdec/vdec_vp9_req_lat_if.c b/drivers/media/platform/mediatek/vcodec/vdec/vdec_vp9_req_lat_if.c
new file mode 100644
index 000000000..cf16cf280
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec/vdec_vp9_req_lat_if.c
@@ -0,0 +1,2225 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2021 MediaTek Inc.
+ * Author: George Sun <george.sun@mediatek.com>
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-vp9.h>
+
+#include "../mtk_vcodec_util.h"
+#include "../mtk_vcodec_dec.h"
+#include "../mtk_vcodec_intr.h"
+#include "../vdec_drv_base.h"
+#include "../vdec_drv_if.h"
+#include "../vdec_vpu_if.h"
+
+/* reset_frame_context defined in VP9 spec */
+#define VP9_RESET_FRAME_CONTEXT_NONE0 0
+#define VP9_RESET_FRAME_CONTEXT_NONE1 1
+#define VP9_RESET_FRAME_CONTEXT_SPEC 2
+#define VP9_RESET_FRAME_CONTEXT_ALL 3
+
+#define VP9_TILE_BUF_SIZE 4096
+#define VP9_PROB_BUF_SIZE 2560
+#define VP9_COUNTS_BUF_SIZE 16384
+
+#define HDR_FLAG(x) (!!((hdr)->flags & V4L2_VP9_FRAME_FLAG_##x))
+#define LF_FLAG(x) (!!((lf)->flags & V4L2_VP9_LOOP_FILTER_FLAG_##x))
+#define SEG_FLAG(x) (!!((seg)->flags & V4L2_VP9_SEGMENTATION_FLAG_##x))
+#define VP9_BAND_6(band) ((band) == 0 ? 3 : 6)
+
+/*
+ * struct vdec_vp9_slice_frame_ctx - vp9 prob tables footprint
+ */
+struct vdec_vp9_slice_frame_ctx {
+	struct {
+		u8 probs[6][3];
+		u8 padding[2];
+	} coef_probs[4][2][2][6];
+
+	u8 y_mode_prob[4][16];
+	u8 switch_interp_prob[4][16];
+	u8 seg[32];  /* ignore */
+	u8 comp_inter_prob[16];
+	u8 comp_ref_prob[16];
+	u8 single_ref_prob[5][2];
+	u8 single_ref_prob_padding[6];
+
+	u8 joint[3];
+	u8 joint_padding[13];
+	struct {
+		u8 sign;
+		u8 classes[10];
+		u8 padding[5];
+	} sign_classes[2];
+	struct {
+		u8 class0[1];
+		u8 bits[10];
+		u8 padding[5];
+	} class0_bits[2];
+	struct {
+		u8 class0_fp[2][3];
+		u8 fp[3];
+		u8 class0_hp;
+		u8 hp;
+		u8 padding[5];
+	} class0_fp_hp[2];
+
+	u8 uv_mode_prob[10][16];
+	u8 uv_mode_prob_padding[2][16];
+
+	u8 partition_prob[16][4];
+
+	u8 inter_mode_probs[7][4];
+	u8 skip_probs[4];
+
+	u8 tx_p8x8[2][4];
+	u8 tx_p16x16[2][4];
+	u8 tx_p32x32[2][4];
+	u8 intra_inter_prob[8];
+};
+
+/*
+ * struct vdec_vp9_slice_frame_counts - vp9 counts tables footprint
+ */
+struct vdec_vp9_slice_frame_counts {
+	union {
+		struct {
+			u32 band_0[3];
+			u32 padding0[1];
+			u32 band_1_5[5][6];
+			u32 padding1[2];
+		} eob_branch[4][2][2];
+		u32 eob_branch_space[256 * 4];
+	};
+
+	struct {
+		u32 band_0[3][4];
+		u32 band_1_5[5][6][4];
+	} coef_probs[4][2][2];
+
+	u32 intra_inter[4][2];
+	u32 comp_inter[5][2];
+	u32 comp_inter_padding[2];
+	u32 comp_ref[5][2];
+	u32 comp_ref_padding[2];
+	u32 single_ref[5][2][2];
+	u32 inter_mode[7][4];
+	u32 y_mode[4][12];
+	u32 uv_mode[10][10];
+	u32 partition[16][4];
+	u32 switchable_interp[4][4];
+
+	u32 tx_p8x8[2][2];
+	u32 tx_p16x16[2][4];
+	u32 tx_p32x32[2][4];
+
+	u32 skip[3][4];
+
+	u32 joint[4];
+
+	struct {
+		u32 sign[2];
+		u32 class0[2];
+		u32 classes[12];
+		u32 bits[10][2];
+		u32 padding[4];
+		u32 class0_fp[2][4];
+		u32 fp[4];
+		u32 class0_hp[2];
+		u32 hp[2];
+	} mvcomp[2];
+
+	u32 reserved[126][4];
+};
+
+/**
+ * struct vdec_vp9_slice_counts_map - vp9 counts tables to map
+ *                                    v4l2_vp9_frame_symbol_counts
+ * @skip:	skip counts.
+ * @y_mode:	Y prediction mode counts.
+ * @filter:	interpolation filter counts.
+ * @mv_joint:	motion vector joint counts.
+ * @sign:	motion vector sign counts.
+ * @classes:	motion vector class counts.
+ * @class0:	motion vector class0 bit counts.
+ * @bits:	motion vector bits counts.
+ * @class0_fp:	motion vector class0 fractional bit counts.
+ * @fp:	motion vector fractional bit counts.
+ * @class0_hp:	motion vector class0 high precision fractional bit counts.
+ * @hp:	motion vector high precision fractional bit counts.
+ */
+struct vdec_vp9_slice_counts_map {
+	u32 skip[3][2];
+	u32 y_mode[4][10];
+	u32 filter[4][3];
+	u32 sign[2][2];
+	u32 classes[2][11];
+	u32 class0[2][2];
+	u32 bits[2][10][2];
+	u32 class0_fp[2][2][4];
+	u32 fp[2][4];
+	u32 class0_hp[2][2];
+	u32 hp[2][2];
+};
+
+/*
+ * struct vdec_vp9_slice_uncompressed_header - vp9 uncompressed header syntax
+ *                                             used for decoding
+ */
+struct vdec_vp9_slice_uncompressed_header {
+	u8 profile;
+	u8 last_frame_type;
+	u8 frame_type;
+
+	u8 last_show_frame;
+	u8 show_frame;
+	u8 error_resilient_mode;
+
+	u8 bit_depth;
+	u8 padding0[1];
+	u16 last_frame_width;
+	u16 last_frame_height;
+	u16 frame_width;
+	u16 frame_height;
+
+	u8 intra_only;
+	u8 reset_frame_context;
+	u8 ref_frame_sign_bias[4];
+	u8 allow_high_precision_mv;
+	u8 interpolation_filter;
+
+	u8 refresh_frame_context;
+	u8 frame_parallel_decoding_mode;
+	u8 frame_context_idx;
+
+	/* loop_filter_params */
+	u8 loop_filter_level;
+	u8 loop_filter_sharpness;
+	u8 loop_filter_delta_enabled;
+	s8 loop_filter_ref_deltas[4];
+	s8 loop_filter_mode_deltas[2];
+
+	/* quantization_params */
+	u8 base_q_idx;
+	s8 delta_q_y_dc;
+	s8 delta_q_uv_dc;
+	s8 delta_q_uv_ac;
+
+	/* segmentation_params */
+	u8 segmentation_enabled;
+	u8 segmentation_update_map;
+	u8 segmentation_tree_probs[7];
+	u8 padding1[1];
+	u8 segmentation_temporal_udpate;
+	u8 segmentation_pred_prob[3];
+	u8 segmentation_update_data;
+	u8 segmentation_abs_or_delta_update;
+	u8 feature_enabled[8];
+	s16 feature_value[8][4];
+
+	/* tile_info */
+	u8 tile_cols_log2;
+	u8 tile_rows_log2;
+	u8 padding2[2];
+
+	u16 uncompressed_header_size;
+	u16 header_size_in_bytes;
+
+	/* LAT OUT, CORE IN */
+	u32 dequant[8][4];
+};
+
+/*
+ * struct vdec_vp9_slice_compressed_header - vp9 compressed header syntax
+ *                                           used for decoding.
+ */
+struct vdec_vp9_slice_compressed_header {
+	u8 tx_mode;
+	u8 ref_mode;
+	u8 comp_fixed_ref;
+	u8 comp_var_ref[2];
+	u8 padding[3];
+};
+
+/*
+ * struct vdec_vp9_slice_tiles - vp9 tile syntax
+ */
+struct vdec_vp9_slice_tiles {
+	u32 size[4][64];
+	u32 mi_rows[4];
+	u32 mi_cols[64];
+	u8 actual_rows;
+	u8 padding[7];
+};
+
+/*
+ * struct vdec_vp9_slice_reference - vp9 reference frame information
+ */
+struct vdec_vp9_slice_reference {
+	u16 frame_width;
+	u16 frame_height;
+	u8 bit_depth;
+	u8 subsampling_x;
+	u8 subsampling_y;
+	u8 padding;
+};
+
+/*
+ * struct vdec_vp9_slice_frame - vp9 syntax used for decoding
+ */
+struct vdec_vp9_slice_frame {
+	struct vdec_vp9_slice_uncompressed_header uh;
+	struct vdec_vp9_slice_compressed_header ch;
+	struct vdec_vp9_slice_tiles tiles;
+	struct vdec_vp9_slice_reference ref[3];
+};
+
+/*
+ * struct vdec_vp9_slice_init_vsi - VSI used to initialize instance
+ */
+struct vdec_vp9_slice_init_vsi {
+	unsigned int architecture;
+	unsigned int reserved;
+	u64 core_vsi;
+	/* default frame context's position in MicroP */
+	u64 default_frame_ctx;
+};
+
+/*
+ * struct vdec_vp9_slice_mem - memory address and size
+ */
+struct vdec_vp9_slice_mem {
+	union {
+		u64 buf;
+		dma_addr_t dma_addr;
+	};
+	union {
+		size_t size;
+		dma_addr_t dma_addr_end;
+		u64 padding;
+	};
+};
+
+/*
+ * struct vdec_vp9_slice_bs - input buffer for decoding
+ */
+struct vdec_vp9_slice_bs {
+	struct vdec_vp9_slice_mem buf;
+	struct vdec_vp9_slice_mem frame;
+};
+
+/*
+ * struct vdec_vp9_slice_fb - frame buffer for decoding
+ */
+struct vdec_vp9_slice_fb {
+	struct vdec_vp9_slice_mem y;
+	struct vdec_vp9_slice_mem c;
+};
+
+/*
+ * struct vdec_vp9_slice_state - decoding state
+ */
+struct vdec_vp9_slice_state {
+	int err;
+	unsigned int full;
+	unsigned int timeout;
+	unsigned int perf;
+
+	unsigned int crc[12];
+};
+
+/**
+ * struct vdec_vp9_slice_vsi - exchange decoding information
+ *                             between Main CPU and MicroP
+ *
+ * @bs:	input buffer
+ * @fb:	output buffer
+ * @ref:	3 reference buffers
+ * @mv:	mv working buffer
+ * @seg:	segmentation working buffer
+ * @tile:	tile buffer
+ * @prob:	prob table buffer, used to set/update prob table
+ * @counts:	counts table buffer, used to update prob table
+ * @ube:	general buffer
+ * @trans:	trans buffer position in general buffer
+ * @err_map:	error buffer
+ * @row_info:	row info buffer
+ * @frame:	decoding syntax
+ * @state:	decoding state
+ */
+struct vdec_vp9_slice_vsi {
+	/* used in LAT stage */
+	struct vdec_vp9_slice_bs bs;
+	/* used in Core stage */
+	struct vdec_vp9_slice_fb fb;
+	struct vdec_vp9_slice_fb ref[3];
+
+	struct vdec_vp9_slice_mem mv[2];
+	struct vdec_vp9_slice_mem seg[2];
+	struct vdec_vp9_slice_mem tile;
+	struct vdec_vp9_slice_mem prob;
+	struct vdec_vp9_slice_mem counts;
+
+	/* LAT stage's output, Core stage's input */
+	struct vdec_vp9_slice_mem ube;
+	struct vdec_vp9_slice_mem trans;
+	struct vdec_vp9_slice_mem err_map;
+	struct vdec_vp9_slice_mem row_info;
+
+	/* decoding parameters */
+	struct vdec_vp9_slice_frame frame;
+
+	struct vdec_vp9_slice_state state;
+};
+
+/**
+ * struct vdec_vp9_slice_pfc - per-frame context that contains a local vsi.
+ *                             pass it from lat to core
+ *
+ * @vsi:	local vsi. copy to/from remote vsi before/after decoding
+ * @ref_idx:	reference buffer index
+ * @seq:	picture sequence
+ * @state:	decoding state
+ */
+struct vdec_vp9_slice_pfc {
+	struct vdec_vp9_slice_vsi vsi;
+
+	u64 ref_idx[3];
+
+	int seq;
+
+	/* LAT/Core CRC */
+	struct vdec_vp9_slice_state state[2];
+};
+
+/*
+ * enum vdec_vp9_slice_resolution_level
+ */
+enum vdec_vp9_slice_resolution_level {
+	VP9_RES_NONE,
+	VP9_RES_FHD,
+	VP9_RES_4K,
+	VP9_RES_8K,
+};
+
+/*
+ * struct vdec_vp9_slice_ref - picture's width & height should kept
+ *                             for later decoding as reference picture
+ */
+struct vdec_vp9_slice_ref {
+	unsigned int width;
+	unsigned int height;
+};
+
+/**
+ * struct vdec_vp9_slice_instance - represent one vp9 instance
+ *
+ * @ctx:		pointer to codec's context
+ * @vpu:		VPU instance
+ * @seq:		global picture sequence
+ * @level:		level of current resolution
+ * @width:		width of last picture
+ * @height:		height of last picture
+ * @frame_type:	frame_type of last picture
+ * @irq:		irq to Main CPU or MicroP
+ * @show_frame:	show_frame of last picture
+ * @dpb:		picture information (width/height) for reference
+ * @mv:		mv working buffer
+ * @seg:		segmentation working buffer
+ * @tile:		tile buffer
+ * @prob:		prob table buffer, used to set/update prob table
+ * @counts:		counts table buffer, used to update prob table
+ * @frame_ctx:		4 frame context according to VP9 Spec
+ * @frame_ctx_helper:	4 frame context according to newest kernel spec
+ * @dirty:		state of each frame context
+ * @init_vsi:		vsi used for initialized VP9 instance
+ * @vsi:		vsi used for decoding/flush ...
+ * @core_vsi:		vsi used for Core stage
+ *
+ * @sc_pfc:		per frame context single core
+ * @counts_map:	used map to counts_helper
+ * @counts_helper:	counts table according to newest kernel spec
+ */
+struct vdec_vp9_slice_instance {
+	struct mtk_vcodec_ctx *ctx;
+	struct vdec_vpu_inst vpu;
+
+	int seq;
+
+	enum vdec_vp9_slice_resolution_level level;
+
+	/* for resolution change and get_pic_info */
+	unsigned int width;
+	unsigned int height;
+
+	/* for last_frame_type */
+	unsigned int frame_type;
+	unsigned int irq;
+
+	unsigned int show_frame;
+
+	/* maintain vp9 reference frame state */
+	struct vdec_vp9_slice_ref dpb[VB2_MAX_FRAME];
+
+	/*
+	 * normal working buffers
+	 * mv[0]/seg[0]/tile/prob/counts is used for LAT
+	 * mv[1]/seg[1] is used for CORE
+	 */
+	struct mtk_vcodec_mem mv[2];
+	struct mtk_vcodec_mem seg[2];
+	struct mtk_vcodec_mem tile;
+	struct mtk_vcodec_mem prob;
+	struct mtk_vcodec_mem counts;
+
+	/* 4 prob tables */
+	struct vdec_vp9_slice_frame_ctx frame_ctx[4];
+	/*4 helper tables */
+	struct v4l2_vp9_frame_context frame_ctx_helper;
+	unsigned char dirty[4];
+
+	/* MicroP vsi */
+	union {
+		struct vdec_vp9_slice_init_vsi *init_vsi;
+		struct vdec_vp9_slice_vsi *vsi;
+	};
+	struct vdec_vp9_slice_vsi *core_vsi;
+
+	struct vdec_vp9_slice_pfc sc_pfc;
+	struct vdec_vp9_slice_counts_map counts_map;
+	struct v4l2_vp9_frame_symbol_counts counts_helper;
+};
+
+/*
+ * all VP9 instances could share this default frame context.
+ */
+static struct vdec_vp9_slice_frame_ctx *vdec_vp9_slice_default_frame_ctx;
+static DEFINE_MUTEX(vdec_vp9_slice_frame_ctx_lock);
+
+static int vdec_vp9_slice_core_decode(struct vdec_lat_buf *lat_buf);
+
+static int vdec_vp9_slice_init_default_frame_ctx(struct vdec_vp9_slice_instance *instance)
+{
+	struct vdec_vp9_slice_frame_ctx *remote_frame_ctx;
+	struct vdec_vp9_slice_frame_ctx *frame_ctx;
+	struct mtk_vcodec_ctx *ctx;
+	struct vdec_vp9_slice_init_vsi *vsi;
+	int ret = 0;
+
+	ctx = instance->ctx;
+	vsi = instance->vpu.vsi;
+	if (!ctx || !vsi)
+		return -EINVAL;
+
+	remote_frame_ctx = mtk_vcodec_fw_map_dm_addr(ctx->dev->fw_handler,
+						     (u32)vsi->default_frame_ctx);
+	if (!remote_frame_ctx) {
+		mtk_vcodec_err(instance, "failed to map default frame ctx\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&vdec_vp9_slice_frame_ctx_lock);
+	if (vdec_vp9_slice_default_frame_ctx)
+		goto out;
+
+	frame_ctx = kmemdup(remote_frame_ctx, sizeof(*frame_ctx), GFP_KERNEL);
+	if (!frame_ctx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	vdec_vp9_slice_default_frame_ctx = frame_ctx;
+
+out:
+	mutex_unlock(&vdec_vp9_slice_frame_ctx_lock);
+
+	return ret;
+}
+
+static int vdec_vp9_slice_alloc_working_buffer(struct vdec_vp9_slice_instance *instance,
+					       struct vdec_vp9_slice_vsi *vsi)
+{
+	struct mtk_vcodec_ctx *ctx = instance->ctx;
+	enum vdec_vp9_slice_resolution_level level;
+	/* super blocks */
+	unsigned int max_sb_w;
+	unsigned int max_sb_h;
+	unsigned int max_w;
+	unsigned int max_h;
+	unsigned int w;
+	unsigned int h;
+	size_t size;
+	int ret;
+	int i;
+
+	w = vsi->frame.uh.frame_width;
+	h = vsi->frame.uh.frame_height;
+
+	if (w > VCODEC_DEC_4K_CODED_WIDTH ||
+	    h > VCODEC_DEC_4K_CODED_HEIGHT) {
+		return -EINVAL;
+	} else if (w > MTK_VDEC_MAX_W || h > MTK_VDEC_MAX_H) {
+		/* 4K */
+		level = VP9_RES_4K;
+		max_w = VCODEC_DEC_4K_CODED_WIDTH;
+		max_h = VCODEC_DEC_4K_CODED_HEIGHT;
+	} else {
+		/* FHD */
+		level = VP9_RES_FHD;
+		max_w = MTK_VDEC_MAX_W;
+		max_h = MTK_VDEC_MAX_H;
+	}
+
+	if (level == instance->level)
+		return 0;
+
+	mtk_vcodec_debug(instance, "resolution level changed, from %u to %u, %ux%u",
+			 instance->level, level, w, h);
+
+	max_sb_w = DIV_ROUND_UP(max_w, 64);
+	max_sb_h = DIV_ROUND_UP(max_h, 64);
+	ret = -ENOMEM;
+
+	/*
+	 * Lat-flush must wait core idle, otherwise core will
+	 * use released buffers
+	 */
+
+	size = (max_sb_w * max_sb_h + 2) * 576;
+	for (i = 0; i < 2; i++) {
+		if (instance->mv[i].va)
+			mtk_vcodec_mem_free(ctx, &instance->mv[i]);
+		instance->mv[i].size = size;
+		if (mtk_vcodec_mem_alloc(ctx, &instance->mv[i]))
+			goto err;
+	}
+
+	size = (max_sb_w * max_sb_h * 32) + 256;
+	for (i = 0; i < 2; i++) {
+		if (instance->seg[i].va)
+			mtk_vcodec_mem_free(ctx, &instance->seg[i]);
+		instance->seg[i].size = size;
+		if (mtk_vcodec_mem_alloc(ctx, &instance->seg[i]))
+			goto err;
+	}
+
+	if (!instance->tile.va) {
+		instance->tile.size = VP9_TILE_BUF_SIZE;
+		if (mtk_vcodec_mem_alloc(ctx, &instance->tile))
+			goto err;
+	}
+
+	if (!instance->prob.va) {
+		instance->prob.size = VP9_PROB_BUF_SIZE;
+		if (mtk_vcodec_mem_alloc(ctx, &instance->prob))
+			goto err;
+	}
+
+	if (!instance->counts.va) {
+		instance->counts.size = VP9_COUNTS_BUF_SIZE;
+		if (mtk_vcodec_mem_alloc(ctx, &instance->counts))
+			goto err;
+	}
+
+	instance->level = level;
+	return 0;
+
+err:
+	instance->level = VP9_RES_NONE;
+	return ret;
+}
+
+static void vdec_vp9_slice_free_working_buffer(struct vdec_vp9_slice_instance *instance)
+{
+	struct mtk_vcodec_ctx *ctx = instance->ctx;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(instance->mv); i++) {
+		if (instance->mv[i].va)
+			mtk_vcodec_mem_free(ctx, &instance->mv[i]);
+	}
+	for (i = 0; i < ARRAY_SIZE(instance->seg); i++) {
+		if (instance->seg[i].va)
+			mtk_vcodec_mem_free(ctx, &instance->seg[i]);
+	}
+	if (instance->tile.va)
+		mtk_vcodec_mem_free(ctx, &instance->tile);
+	if (instance->prob.va)
+		mtk_vcodec_mem_free(ctx, &instance->prob);
+	if (instance->counts.va)
+		mtk_vcodec_mem_free(ctx, &instance->counts);
+
+	instance->level = VP9_RES_NONE;
+}
+
+static void vdec_vp9_slice_vsi_from_remote(struct vdec_vp9_slice_vsi *vsi,
+					   struct vdec_vp9_slice_vsi *remote_vsi,
+					   int skip)
+{
+	struct vdec_vp9_slice_frame *rf;
+	struct vdec_vp9_slice_frame *f;
+
+	/*
+	 * compressed header
+	 * dequant
+	 * buffer position
+	 * decode state
+	 */
+	if (!skip) {
+		rf = &remote_vsi->frame;
+		f = &vsi->frame;
+		memcpy(&f->ch, &rf->ch, sizeof(f->ch));
+		memcpy(&f->uh.dequant, &rf->uh.dequant, sizeof(f->uh.dequant));
+		memcpy(&vsi->trans, &remote_vsi->trans, sizeof(vsi->trans));
+	}
+
+	memcpy(&vsi->state, &remote_vsi->state, sizeof(vsi->state));
+}
+
+static void vdec_vp9_slice_vsi_to_remote(struct vdec_vp9_slice_vsi *vsi,
+					 struct vdec_vp9_slice_vsi *remote_vsi)
+{
+	memcpy(remote_vsi, vsi, sizeof(*vsi));
+}
+
+static int vdec_vp9_slice_tile_offset(int idx, int mi_num, int tile_log2)
+{
+	int sbs = (mi_num + 7) >> 3;
+	int offset = ((idx * sbs) >> tile_log2) << 3;
+
+	return min(offset, mi_num);
+}
+
+static
+int vdec_vp9_slice_setup_single_from_src_to_dst(struct vdec_vp9_slice_instance *instance)
+{
+	struct vb2_v4l2_buffer *src;
+	struct vb2_v4l2_buffer *dst;
+
+	src = v4l2_m2m_next_src_buf(instance->ctx->m2m_ctx);
+	if (!src)
+		return -EINVAL;
+
+	dst = v4l2_m2m_next_dst_buf(instance->ctx->m2m_ctx);
+	if (!dst)
+		return -EINVAL;
+
+	v4l2_m2m_buf_copy_metadata(src, dst, true);
+
+	return 0;
+}
+
+static int vdec_vp9_slice_setup_lat_from_src_buf(struct vdec_vp9_slice_instance *instance,
+						 struct vdec_lat_buf *lat_buf)
+{
+	struct vb2_v4l2_buffer *src;
+	struct vb2_v4l2_buffer *dst;
+
+	src = v4l2_m2m_next_src_buf(instance->ctx->m2m_ctx);
+	if (!src)
+		return -EINVAL;
+
+	lat_buf->src_buf_req = src->vb2_buf.req_obj.req;
+
+	dst = &lat_buf->ts_info;
+	v4l2_m2m_buf_copy_metadata(src, dst, true);
+	return 0;
+}
+
+static void vdec_vp9_slice_setup_hdr(struct vdec_vp9_slice_instance *instance,
+				     struct vdec_vp9_slice_uncompressed_header *uh,
+				     struct v4l2_ctrl_vp9_frame *hdr)
+{
+	int i;
+
+	uh->profile = hdr->profile;
+	uh->last_frame_type = instance->frame_type;
+	uh->frame_type = !HDR_FLAG(KEY_FRAME);
+	uh->last_show_frame = instance->show_frame;
+	uh->show_frame = HDR_FLAG(SHOW_FRAME);
+	uh->error_resilient_mode = HDR_FLAG(ERROR_RESILIENT);
+	uh->bit_depth = hdr->bit_depth;
+	uh->last_frame_width = instance->width;
+	uh->last_frame_height = instance->height;
+	uh->frame_width = hdr->frame_width_minus_1 + 1;
+	uh->frame_height = hdr->frame_height_minus_1 + 1;
+	uh->intra_only = HDR_FLAG(INTRA_ONLY);
+	/* map v4l2 enum to values defined in VP9 spec for firmware */
+	switch (hdr->reset_frame_context) {
+	case V4L2_VP9_RESET_FRAME_CTX_NONE:
+		uh->reset_frame_context = VP9_RESET_FRAME_CONTEXT_NONE0;
+		break;
+	case V4L2_VP9_RESET_FRAME_CTX_SPEC:
+		uh->reset_frame_context = VP9_RESET_FRAME_CONTEXT_SPEC;
+		break;
+	case V4L2_VP9_RESET_FRAME_CTX_ALL:
+		uh->reset_frame_context = VP9_RESET_FRAME_CONTEXT_ALL;
+		break;
+	default:
+		uh->reset_frame_context = VP9_RESET_FRAME_CONTEXT_NONE0;
+		break;
+	}
+	/*
+	 * ref_frame_sign_bias specifies the intended direction
+	 * of the motion vector in time for each reference frame.
+	 * - INTRA_FRAME = 0,
+	 * - LAST_FRAME = 1,
+	 * - GOLDEN_FRAME = 2,
+	 * - ALTREF_FRAME = 3,
+	 * ref_frame_sign_bias[INTRA_FRAME] is always 0
+	 * and VDA only passes another 3 directions
+	 */
+	uh->ref_frame_sign_bias[0] = 0;
+	for (i = 0; i < 3; i++)
+		uh->ref_frame_sign_bias[i + 1] =
+			!!(hdr->ref_frame_sign_bias & (1 << i));
+	uh->allow_high_precision_mv = HDR_FLAG(ALLOW_HIGH_PREC_MV);
+	uh->interpolation_filter = hdr->interpolation_filter;
+	uh->refresh_frame_context = HDR_FLAG(REFRESH_FRAME_CTX);
+	uh->frame_parallel_decoding_mode = HDR_FLAG(PARALLEL_DEC_MODE);
+	uh->frame_context_idx = hdr->frame_context_idx;
+
+	/* tile info */
+	uh->tile_cols_log2 = hdr->tile_cols_log2;
+	uh->tile_rows_log2 = hdr->tile_rows_log2;
+
+	uh->uncompressed_header_size = hdr->uncompressed_header_size;
+	uh->header_size_in_bytes = hdr->compressed_header_size;
+}
+
+static void vdec_vp9_slice_setup_frame_ctx(struct vdec_vp9_slice_instance *instance,
+					   struct vdec_vp9_slice_uncompressed_header *uh,
+					   struct v4l2_ctrl_vp9_frame *hdr)
+{
+	int error_resilient_mode;
+	int reset_frame_context;
+	int key_frame;
+	int intra_only;
+	int i;
+
+	key_frame = HDR_FLAG(KEY_FRAME);
+	intra_only = HDR_FLAG(INTRA_ONLY);
+	error_resilient_mode = HDR_FLAG(ERROR_RESILIENT);
+	reset_frame_context = uh->reset_frame_context;
+
+	/*
+	 * according to "6.2 Uncompressed header syntax" in
+	 * "VP9 Bitstream & Decoding Process Specification",
+	 * reset @frame_context_idx when (FrameIsIntra || error_resilient_mode)
+	 */
+	if (key_frame || intra_only || error_resilient_mode) {
+		/*
+		 * @reset_frame_context specifies
+		 * whether the frame context should be
+		 * reset to default values:
+		 * 0 or 1 means do not reset any frame context
+		 * 2 resets just the context specified in the frame header
+		 * 3 resets all contexts
+		 */
+		if (key_frame || error_resilient_mode ||
+		    reset_frame_context == 3) {
+			/* use default table */
+			for (i = 0; i < 4; i++)
+				instance->dirty[i] = 0;
+		} else if (reset_frame_context == 2) {
+			instance->dirty[uh->frame_context_idx] = 0;
+		}
+		uh->frame_context_idx = 0;
+	}
+}
+
+static void vdec_vp9_slice_setup_loop_filter(struct vdec_vp9_slice_uncompressed_header *uh,
+					     struct v4l2_vp9_loop_filter *lf)
+{
+	int i;
+
+	uh->loop_filter_level = lf->level;
+	uh->loop_filter_sharpness = lf->sharpness;
+	uh->loop_filter_delta_enabled = LF_FLAG(DELTA_ENABLED);
+	for (i = 0; i < 4; i++)
+		uh->loop_filter_ref_deltas[i] = lf->ref_deltas[i];
+	for (i = 0; i < 2; i++)
+		uh->loop_filter_mode_deltas[i] = lf->mode_deltas[i];
+}
+
+static void vdec_vp9_slice_setup_quantization(struct vdec_vp9_slice_uncompressed_header *uh,
+					      struct v4l2_vp9_quantization *quant)
+{
+	uh->base_q_idx = quant->base_q_idx;
+	uh->delta_q_y_dc = quant->delta_q_y_dc;
+	uh->delta_q_uv_dc = quant->delta_q_uv_dc;
+	uh->delta_q_uv_ac = quant->delta_q_uv_ac;
+}
+
+static void vdec_vp9_slice_setup_segmentation(struct vdec_vp9_slice_uncompressed_header *uh,
+					      struct v4l2_vp9_segmentation *seg)
+{
+	int i;
+	int j;
+
+	uh->segmentation_enabled = SEG_FLAG(ENABLED);
+	uh->segmentation_update_map = SEG_FLAG(UPDATE_MAP);
+	for (i = 0; i < 7; i++)
+		uh->segmentation_tree_probs[i] = seg->tree_probs[i];
+	uh->segmentation_temporal_udpate = SEG_FLAG(TEMPORAL_UPDATE);
+	for (i = 0; i < 3; i++)
+		uh->segmentation_pred_prob[i] = seg->pred_probs[i];
+	uh->segmentation_update_data = SEG_FLAG(UPDATE_DATA);
+	uh->segmentation_abs_or_delta_update = SEG_FLAG(ABS_OR_DELTA_UPDATE);
+	for (i = 0; i < 8; i++) {
+		uh->feature_enabled[i] = seg->feature_enabled[i];
+		for (j = 0; j < 4; j++)
+			uh->feature_value[i][j] = seg->feature_data[i][j];
+	}
+}
+
+static int vdec_vp9_slice_setup_tile(struct vdec_vp9_slice_vsi *vsi,
+				     struct v4l2_ctrl_vp9_frame *hdr)
+{
+	unsigned int rows_log2;
+	unsigned int cols_log2;
+	unsigned int rows;
+	unsigned int cols;
+	unsigned int mi_rows;
+	unsigned int mi_cols;
+	struct vdec_vp9_slice_tiles *tiles;
+	int offset;
+	int start;
+	int end;
+	int i;
+
+	rows_log2 = hdr->tile_rows_log2;
+	cols_log2 = hdr->tile_cols_log2;
+	rows = 1 << rows_log2;
+	cols = 1 << cols_log2;
+	tiles = &vsi->frame.tiles;
+	tiles->actual_rows = 0;
+
+	if (rows > 4 || cols > 64)
+		return -EINVAL;
+
+	/* setup mi rows/cols information */
+	mi_rows = (hdr->frame_height_minus_1 + 1 + 7) >> 3;
+	mi_cols = (hdr->frame_width_minus_1 + 1 + 7) >> 3;
+
+	for (i = 0; i < rows; i++) {
+		start = vdec_vp9_slice_tile_offset(i, mi_rows, rows_log2);
+		end = vdec_vp9_slice_tile_offset(i + 1, mi_rows, rows_log2);
+		offset = end - start;
+		tiles->mi_rows[i] = (offset + 7) >> 3;
+		if (tiles->mi_rows[i])
+			tiles->actual_rows++;
+	}
+
+	for (i = 0; i < cols; i++) {
+		start = vdec_vp9_slice_tile_offset(i, mi_cols, cols_log2);
+		end = vdec_vp9_slice_tile_offset(i + 1, mi_cols, cols_log2);
+		offset = end - start;
+		tiles->mi_cols[i] = (offset + 7) >> 3;
+	}
+
+	return 0;
+}
+
+static void vdec_vp9_slice_setup_state(struct vdec_vp9_slice_vsi *vsi)
+{
+	memset(&vsi->state, 0, sizeof(vsi->state));
+}
+
+static void vdec_vp9_slice_setup_ref_idx(struct vdec_vp9_slice_pfc *pfc,
+					 struct v4l2_ctrl_vp9_frame *hdr)
+{
+	pfc->ref_idx[0] = hdr->last_frame_ts;
+	pfc->ref_idx[1] = hdr->golden_frame_ts;
+	pfc->ref_idx[2] = hdr->alt_frame_ts;
+}
+
+static int vdec_vp9_slice_setup_pfc(struct vdec_vp9_slice_instance *instance,
+				    struct vdec_vp9_slice_pfc *pfc)
+{
+	struct v4l2_ctrl_vp9_frame *hdr;
+	struct vdec_vp9_slice_uncompressed_header *uh;
+	struct v4l2_ctrl *hdr_ctrl;
+	struct vdec_vp9_slice_vsi *vsi;
+	int ret;
+
+	/* frame header */
+	hdr_ctrl = v4l2_ctrl_find(&instance->ctx->ctrl_hdl, V4L2_CID_STATELESS_VP9_FRAME);
+	if (!hdr_ctrl || !hdr_ctrl->p_cur.p)
+		return -EINVAL;
+
+	hdr = hdr_ctrl->p_cur.p;
+	vsi = &pfc->vsi;
+	uh = &vsi->frame.uh;
+
+	/* setup vsi information */
+	vdec_vp9_slice_setup_hdr(instance, uh, hdr);
+	vdec_vp9_slice_setup_frame_ctx(instance, uh, hdr);
+	vdec_vp9_slice_setup_loop_filter(uh, &hdr->lf);
+	vdec_vp9_slice_setup_quantization(uh, &hdr->quant);
+	vdec_vp9_slice_setup_segmentation(uh, &hdr->seg);
+	ret = vdec_vp9_slice_setup_tile(vsi, hdr);
+	if (ret)
+		return ret;
+	vdec_vp9_slice_setup_state(vsi);
+
+	/* core stage needs buffer index to get ref y/c ... */
+	vdec_vp9_slice_setup_ref_idx(pfc, hdr);
+
+	pfc->seq = instance->seq;
+	instance->seq++;
+
+	return 0;
+}
+
+static int vdec_vp9_slice_setup_lat_buffer(struct vdec_vp9_slice_instance *instance,
+					   struct vdec_vp9_slice_vsi *vsi,
+					   struct mtk_vcodec_mem *bs,
+					   struct vdec_lat_buf *lat_buf)
+{
+	int i;
+
+	vsi->bs.buf.dma_addr = bs->dma_addr;
+	vsi->bs.buf.size = bs->size;
+	vsi->bs.frame.dma_addr = bs->dma_addr;
+	vsi->bs.frame.size = bs->size;
+
+	for (i = 0; i < 2; i++) {
+		vsi->mv[i].dma_addr = instance->mv[i].dma_addr;
+		vsi->mv[i].size = instance->mv[i].size;
+	}
+	for (i = 0; i < 2; i++) {
+		vsi->seg[i].dma_addr = instance->seg[i].dma_addr;
+		vsi->seg[i].size = instance->seg[i].size;
+	}
+	vsi->tile.dma_addr = instance->tile.dma_addr;
+	vsi->tile.size = instance->tile.size;
+	vsi->prob.dma_addr = instance->prob.dma_addr;
+	vsi->prob.size = instance->prob.size;
+	vsi->counts.dma_addr = instance->counts.dma_addr;
+	vsi->counts.size = instance->counts.size;
+
+	vsi->ube.dma_addr = lat_buf->ctx->msg_queue.wdma_addr.dma_addr;
+	vsi->ube.size = lat_buf->ctx->msg_queue.wdma_addr.size;
+	vsi->trans.dma_addr = lat_buf->ctx->msg_queue.wdma_wptr_addr;
+	/* used to store trans end */
+	vsi->trans.dma_addr_end = lat_buf->ctx->msg_queue.wdma_rptr_addr;
+	vsi->err_map.dma_addr = lat_buf->wdma_err_addr.dma_addr;
+	vsi->err_map.size = lat_buf->wdma_err_addr.size;
+
+	vsi->row_info.buf = 0;
+	vsi->row_info.size = 0;
+
+	return 0;
+}
+
+static int vdec_vp9_slice_setup_prob_buffer(struct vdec_vp9_slice_instance *instance,
+					    struct vdec_vp9_slice_vsi *vsi)
+{
+	struct vdec_vp9_slice_frame_ctx *frame_ctx;
+	struct vdec_vp9_slice_uncompressed_header *uh;
+
+	uh = &vsi->frame.uh;
+
+	mtk_vcodec_debug(instance, "ctx dirty %u idx %d\n",
+			 instance->dirty[uh->frame_context_idx],
+			 uh->frame_context_idx);
+
+	if (instance->dirty[uh->frame_context_idx])
+		frame_ctx = &instance->frame_ctx[uh->frame_context_idx];
+	else
+		frame_ctx = vdec_vp9_slice_default_frame_ctx;
+	memcpy(instance->prob.va, frame_ctx, sizeof(*frame_ctx));
+
+	return 0;
+}
+
+static void vdec_vp9_slice_setup_seg_buffer(struct vdec_vp9_slice_instance *instance,
+					    struct vdec_vp9_slice_vsi *vsi,
+					    struct mtk_vcodec_mem *buf)
+{
+	struct vdec_vp9_slice_uncompressed_header *uh;
+
+	/* reset segment buffer */
+	uh = &vsi->frame.uh;
+	if (uh->frame_type == 0 ||
+	    uh->intra_only ||
+	    uh->error_resilient_mode ||
+	    uh->frame_width != instance->width ||
+	    uh->frame_height != instance->height) {
+		mtk_vcodec_debug(instance, "reset seg\n");
+		memset(buf->va, 0, buf->size);
+	}
+}
+
+/*
+ * parse tiles according to `6.4 Decode tiles syntax`
+ * in "vp9-bitstream-specification"
+ *
+ * frame contains uncompress header, compressed header and several tiles.
+ * this function parses tiles' position and size, stores them to tile buffer
+ * for decoding.
+ */
+static int vdec_vp9_slice_setup_tile_buffer(struct vdec_vp9_slice_instance *instance,
+					    struct vdec_vp9_slice_vsi *vsi,
+					    struct mtk_vcodec_mem *bs)
+{
+	struct vdec_vp9_slice_uncompressed_header *uh;
+	unsigned int rows_log2;
+	unsigned int cols_log2;
+	unsigned int rows;
+	unsigned int cols;
+	unsigned int mi_row;
+	unsigned int mi_col;
+	unsigned int offset;
+	unsigned int pa;
+	unsigned int size;
+	struct vdec_vp9_slice_tiles *tiles;
+	unsigned char *pos;
+	unsigned char *end;
+	unsigned char *va;
+	unsigned int *tb;
+	int i;
+	int j;
+
+	uh = &vsi->frame.uh;
+	rows_log2 = uh->tile_rows_log2;
+	cols_log2 = uh->tile_cols_log2;
+	rows = 1 << rows_log2;
+	cols = 1 << cols_log2;
+
+	if (rows > 4 || cols > 64) {
+		mtk_vcodec_err(instance, "tile_rows %u tile_cols %u\n",
+			       rows, cols);
+		return -EINVAL;
+	}
+
+	offset = uh->uncompressed_header_size +
+		uh->header_size_in_bytes;
+	if (bs->size <= offset) {
+		mtk_vcodec_err(instance, "bs size %zu tile offset %u\n",
+			       bs->size, offset);
+		return -EINVAL;
+	}
+
+	tiles = &vsi->frame.tiles;
+	/* setup tile buffer */
+
+	va = (unsigned char *)bs->va;
+	pos = va + offset;
+	end = va + bs->size;
+	/* truncated */
+	pa = (unsigned int)bs->dma_addr + offset;
+	tb = instance->tile.va;
+	for (i = 0; i < rows; i++) {
+		for (j = 0; j < cols; j++) {
+			if (i == rows - 1 &&
+			    j == cols - 1) {
+				size = (unsigned int)(end - pos);
+			} else {
+				if (end - pos < 4)
+					return -EINVAL;
+
+				size = (pos[0] << 24) | (pos[1] << 16) |
+					(pos[2] << 8) | pos[3];
+				pos += 4;
+				pa += 4;
+				offset += 4;
+				if (end - pos < size)
+					return -EINVAL;
+			}
+			tiles->size[i][j] = size;
+			if (tiles->mi_rows[i]) {
+				*tb++ = (size << 3) + ((offset << 3) & 0x7f);
+				*tb++ = pa & ~0xf;
+				*tb++ = (pa << 3) & 0x7f;
+				mi_row = (tiles->mi_rows[i] - 1) & 0x1ff;
+				mi_col = (tiles->mi_cols[j] - 1) & 0x3f;
+				*tb++ = (mi_row << 6) + mi_col;
+			}
+			pos += size;
+			pa += size;
+			offset += size;
+		}
+	}
+
+	return 0;
+}
+
+static int vdec_vp9_slice_setup_lat(struct vdec_vp9_slice_instance *instance,
+				    struct mtk_vcodec_mem *bs,
+				    struct vdec_lat_buf *lat_buf,
+				    struct vdec_vp9_slice_pfc *pfc)
+{
+	struct vdec_vp9_slice_vsi *vsi = &pfc->vsi;
+	int ret;
+
+	ret = vdec_vp9_slice_setup_lat_from_src_buf(instance, lat_buf);
+	if (ret)
+		goto err;
+
+	ret = vdec_vp9_slice_setup_pfc(instance, pfc);
+	if (ret)
+		goto err;
+
+	ret = vdec_vp9_slice_alloc_working_buffer(instance, vsi);
+	if (ret)
+		goto err;
+
+	ret = vdec_vp9_slice_setup_lat_buffer(instance, vsi, bs, lat_buf);
+	if (ret)
+		goto err;
+
+	vdec_vp9_slice_setup_seg_buffer(instance, vsi, &instance->seg[0]);
+
+	/* setup prob/tile buffers for LAT */
+
+	ret = vdec_vp9_slice_setup_prob_buffer(instance, vsi);
+	if (ret)
+		goto err;
+
+	ret = vdec_vp9_slice_setup_tile_buffer(instance, vsi, bs);
+	if (ret)
+		goto err;
+
+	return 0;
+
+err:
+	return ret;
+}
+
+static
+void vdec_vp9_slice_map_counts_eob_coef(unsigned int i, unsigned int j, unsigned int k,
+					struct vdec_vp9_slice_frame_counts *counts,
+					struct v4l2_vp9_frame_symbol_counts *counts_helper)
+{
+	u32 l = 0, m;
+
+	/*
+	 * helper eo -> mtk eo
+	 * helpre e1 -> mtk c3
+	 * helper c0 -> c0
+	 * helper c1 -> c1
+	 * helper c2 -> c2
+	 */
+	for (m = 0; m < 3; m++) {
+		counts_helper->coeff[i][j][k][l][m] =
+			(u32 (*)[3]) & counts->coef_probs[i][j][k].band_0[m];
+		counts_helper->eob[i][j][k][l][m][0] =
+			&counts->eob_branch[i][j][k].band_0[m];
+		counts_helper->eob[i][j][k][l][m][1] =
+			&counts->coef_probs[i][j][k].band_0[m][3];
+	}
+
+	for (l = 1; l < 6; l++) {
+		for (m = 0; m < 6; m++) {
+			counts_helper->coeff[i][j][k][l][m] =
+				(u32 (*)[3]) & counts->coef_probs[i][j][k].band_1_5[l - 1][m];
+			counts_helper->eob[i][j][k][l][m][0] =
+				&counts->eob_branch[i][j][k].band_1_5[l - 1][m];
+			counts_helper->eob[i][j][k][l][m][1] =
+				&counts->coef_probs[i][j][k].band_1_5[l - 1][m][3];
+		}
+	}
+}
+
+static void vdec_vp9_slice_counts_map_helper(struct vdec_vp9_slice_counts_map *counts_map,
+					     struct vdec_vp9_slice_frame_counts *counts,
+					     struct v4l2_vp9_frame_symbol_counts *counts_helper)
+{
+	int i, j, k;
+
+	counts_helper->partition = &counts->partition;
+	counts_helper->intra_inter = &counts->intra_inter;
+	counts_helper->tx32p = &counts->tx_p32x32;
+	counts_helper->tx16p = &counts->tx_p16x16;
+	counts_helper->tx8p = &counts->tx_p8x8;
+	counts_helper->uv_mode = &counts->uv_mode;
+
+	counts_helper->comp = &counts->comp_inter;
+	counts_helper->comp_ref = &counts->comp_ref;
+	counts_helper->single_ref = &counts->single_ref;
+	counts_helper->mv_mode = &counts->inter_mode;
+	counts_helper->mv_joint = &counts->joint;
+
+	for (i = 0; i < ARRAY_SIZE(counts_map->skip); i++)
+		memcpy(counts_map->skip[i], counts->skip[i],
+		       sizeof(counts_map->skip[0]));
+	counts_helper->skip = &counts_map->skip;
+
+	for (i = 0; i < ARRAY_SIZE(counts_map->y_mode); i++)
+		memcpy(counts_map->y_mode[i], counts->y_mode[i],
+		       sizeof(counts_map->y_mode[0]));
+	counts_helper->y_mode = &counts_map->y_mode;
+
+	for (i = 0; i < ARRAY_SIZE(counts_map->filter); i++)
+		memcpy(counts_map->filter[i], counts->switchable_interp[i],
+		       sizeof(counts_map->filter[0]));
+	counts_helper->filter = &counts_map->filter;
+
+	for (i = 0; i < ARRAY_SIZE(counts_map->sign); i++)
+		memcpy(counts_map->sign[i], counts->mvcomp[i].sign,
+		       sizeof(counts_map->sign[0]));
+	counts_helper->sign = &counts_map->sign;
+
+	for (i = 0; i < ARRAY_SIZE(counts_map->classes); i++)
+		memcpy(counts_map->classes[i], counts->mvcomp[i].classes,
+		       sizeof(counts_map->classes[0]));
+	counts_helper->classes = &counts_map->classes;
+
+	for (i = 0; i < ARRAY_SIZE(counts_map->class0); i++)
+		memcpy(counts_map->class0[i], counts->mvcomp[i].class0,
+		       sizeof(counts_map->class0[0]));
+	counts_helper->class0 = &counts_map->class0;
+
+	for (i = 0; i < ARRAY_SIZE(counts_map->bits); i++)
+		for (j = 0; j < ARRAY_SIZE(counts_map->bits[0]); j++)
+			memcpy(counts_map->bits[i][j], counts->mvcomp[i].bits[j],
+			       sizeof(counts_map->bits[0][0]));
+	counts_helper->bits = &counts_map->bits;
+
+	for (i = 0; i < ARRAY_SIZE(counts_map->class0_fp); i++)
+		for (j = 0; j < ARRAY_SIZE(counts_map->class0_fp[0]); j++)
+			memcpy(counts_map->class0_fp[i][j], counts->mvcomp[i].class0_fp[j],
+			       sizeof(counts_map->class0_fp[0][0]));
+	counts_helper->class0_fp = &counts_map->class0_fp;
+
+	for (i = 0; i < ARRAY_SIZE(counts_map->fp); i++)
+		memcpy(counts_map->fp[i], counts->mvcomp[i].fp,
+		       sizeof(counts_map->fp[0]));
+	counts_helper->fp = &counts_map->fp;
+
+	for (i = 0; i < ARRAY_SIZE(counts_map->class0_hp); i++)
+		memcpy(counts_map->class0_hp[i], counts->mvcomp[i].class0_hp,
+		       sizeof(counts_map->class0_hp[0]));
+	counts_helper->class0_hp = &counts_map->class0_hp;
+
+	for (i = 0; i < ARRAY_SIZE(counts_map->hp); i++)
+		memcpy(counts_map->hp[i], counts->mvcomp[i].hp, sizeof(counts_map->hp[0]));
+
+	counts_helper->hp = &counts_map->hp;
+
+	for (i = 0; i < 4; i++)
+		for (j = 0; j < 2; j++)
+			for (k = 0; k < 2; k++)
+				vdec_vp9_slice_map_counts_eob_coef(i, j, k, counts, counts_helper);
+}
+
+static void vdec_vp9_slice_map_to_coef(unsigned int i, unsigned int j, unsigned int k,
+				       struct vdec_vp9_slice_frame_ctx *frame_ctx,
+				       struct v4l2_vp9_frame_context *frame_ctx_helper)
+{
+	u32 l, m;
+
+	for (l = 0; l < ARRAY_SIZE(frame_ctx_helper->coef[0][0][0]); l++) {
+		for (m = 0; m < VP9_BAND_6(l); m++) {
+			memcpy(frame_ctx_helper->coef[i][j][k][l][m],
+			       frame_ctx->coef_probs[i][j][k][l].probs[m],
+			       sizeof(frame_ctx_helper->coef[i][j][k][l][0]));
+		}
+	}
+}
+
+static void vdec_vp9_slice_map_from_coef(unsigned int i, unsigned int j, unsigned int k,
+					 struct vdec_vp9_slice_frame_ctx *frame_ctx,
+					 struct v4l2_vp9_frame_context *frame_ctx_helper)
+{
+	u32 l, m;
+
+	for (l = 0; l < ARRAY_SIZE(frame_ctx_helper->coef[0][0][0]); l++) {
+		for (m = 0; m < VP9_BAND_6(l); m++) {
+			memcpy(frame_ctx->coef_probs[i][j][k][l].probs[m],
+			       frame_ctx_helper->coef[i][j][k][l][m],
+			       sizeof(frame_ctx_helper->coef[i][j][k][l][0]));
+		}
+	}
+}
+
+static
+void vdec_vp9_slice_framectx_map_helper(bool frame_is_intra,
+					struct vdec_vp9_slice_frame_ctx *pre_frame_ctx,
+					struct vdec_vp9_slice_frame_ctx *frame_ctx,
+					struct v4l2_vp9_frame_context *frame_ctx_helper)
+{
+	struct v4l2_vp9_frame_mv_context *mv = &frame_ctx_helper->mv;
+	u32 i, j, k;
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->coef); i++)
+		for (j = 0; j < ARRAY_SIZE(frame_ctx_helper->coef[0]); j++)
+			for (k = 0; k < ARRAY_SIZE(frame_ctx_helper->coef[0][0]); k++)
+				vdec_vp9_slice_map_to_coef(i, j, k, pre_frame_ctx,
+							   frame_ctx_helper);
+
+	/*
+	 * use previous prob when frame is not intra or
+	 * we should use the prob updated by the compressed header parse
+	 */
+	if (!frame_is_intra)
+		frame_ctx = pre_frame_ctx;
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->tx8); i++)
+		memcpy(frame_ctx_helper->tx8[i], frame_ctx->tx_p8x8[i],
+		       sizeof(frame_ctx_helper->tx8[0]));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->tx16); i++)
+		memcpy(frame_ctx_helper->tx16[i], frame_ctx->tx_p16x16[i],
+		       sizeof(frame_ctx_helper->tx16[0]));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->tx32); i++)
+		memcpy(frame_ctx_helper->tx32[i], frame_ctx->tx_p32x32[i],
+		       sizeof(frame_ctx_helper->tx32[0]));
+
+	memcpy(frame_ctx_helper->skip, frame_ctx->skip_probs, sizeof(frame_ctx_helper->skip));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->inter_mode); i++)
+		memcpy(frame_ctx_helper->inter_mode[i], frame_ctx->inter_mode_probs[i],
+		       sizeof(frame_ctx_helper->inter_mode[0]));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->interp_filter); i++)
+		memcpy(frame_ctx_helper->interp_filter[i], frame_ctx->switch_interp_prob[i],
+		       sizeof(frame_ctx_helper->interp_filter[0]));
+
+	memcpy(frame_ctx_helper->is_inter, frame_ctx->intra_inter_prob,
+	       sizeof(frame_ctx_helper->is_inter));
+
+	memcpy(frame_ctx_helper->comp_mode, frame_ctx->comp_inter_prob,
+	       sizeof(frame_ctx_helper->comp_mode));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->single_ref); i++)
+		memcpy(frame_ctx_helper->single_ref[i], frame_ctx->single_ref_prob[i],
+		       sizeof(frame_ctx_helper->single_ref[0]));
+
+	memcpy(frame_ctx_helper->comp_ref, frame_ctx->comp_ref_prob,
+	       sizeof(frame_ctx_helper->comp_ref));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->y_mode); i++)
+		memcpy(frame_ctx_helper->y_mode[i], frame_ctx->y_mode_prob[i],
+		       sizeof(frame_ctx_helper->y_mode[0]));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->uv_mode); i++)
+		memcpy(frame_ctx_helper->uv_mode[i], frame_ctx->uv_mode_prob[i],
+		       sizeof(frame_ctx_helper->uv_mode[0]));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->partition); i++)
+		memcpy(frame_ctx_helper->partition[i], frame_ctx->partition_prob[i],
+		       sizeof(frame_ctx_helper->partition[0]));
+
+	memcpy(mv->joint, frame_ctx->joint, sizeof(mv->joint));
+
+	for (i = 0; i < ARRAY_SIZE(mv->sign); i++)
+		mv->sign[i] = frame_ctx->sign_classes[i].sign;
+
+	for (i = 0; i < ARRAY_SIZE(mv->classes); i++)
+		memcpy(mv->classes[i], frame_ctx->sign_classes[i].classes,
+		       sizeof(mv->classes[i]));
+
+	for (i = 0; i < ARRAY_SIZE(mv->class0_bit); i++)
+		mv->class0_bit[i] = frame_ctx->class0_bits[i].class0[0];
+
+	for (i = 0; i < ARRAY_SIZE(mv->bits); i++)
+		memcpy(mv->bits[i], frame_ctx->class0_bits[i].bits, sizeof(mv->bits[0]));
+
+	for (i = 0; i < ARRAY_SIZE(mv->class0_fr); i++)
+		for (j = 0; j < ARRAY_SIZE(mv->class0_fr[0]); j++)
+			memcpy(mv->class0_fr[i][j], frame_ctx->class0_fp_hp[i].class0_fp[j],
+			       sizeof(mv->class0_fr[0][0]));
+
+	for (i = 0; i < ARRAY_SIZE(mv->fr); i++)
+		memcpy(mv->fr[i], frame_ctx->class0_fp_hp[i].fp, sizeof(mv->fr[0]));
+
+	for (i = 0; i < ARRAY_SIZE(mv->class0_hp); i++)
+		mv->class0_hp[i] = frame_ctx->class0_fp_hp[i].class0_hp;
+
+	for (i = 0; i < ARRAY_SIZE(mv->hp); i++)
+		mv->hp[i] = frame_ctx->class0_fp_hp[i].hp;
+}
+
+static void vdec_vp9_slice_helper_map_framectx(struct v4l2_vp9_frame_context *frame_ctx_helper,
+					       struct vdec_vp9_slice_frame_ctx *frame_ctx)
+{
+	struct v4l2_vp9_frame_mv_context *mv = &frame_ctx_helper->mv;
+	u32 i, j, k;
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->tx8); i++)
+		memcpy(frame_ctx->tx_p8x8[i], frame_ctx_helper->tx8[i],
+		       sizeof(frame_ctx_helper->tx8[0]));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->tx16); i++)
+		memcpy(frame_ctx->tx_p16x16[i], frame_ctx_helper->tx16[i],
+		       sizeof(frame_ctx_helper->tx16[0]));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->tx32); i++)
+		memcpy(frame_ctx->tx_p32x32[i], frame_ctx_helper->tx32[i],
+		       sizeof(frame_ctx_helper->tx32[0]));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->coef); i++)
+		for (j = 0; j < ARRAY_SIZE(frame_ctx_helper->coef[0]); j++)
+			for (k = 0; k < ARRAY_SIZE(frame_ctx_helper->coef[0][0]); k++)
+				vdec_vp9_slice_map_from_coef(i, j, k, frame_ctx,
+							     frame_ctx_helper);
+
+	memcpy(frame_ctx->skip_probs, frame_ctx_helper->skip, sizeof(frame_ctx_helper->skip));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->inter_mode); i++)
+		memcpy(frame_ctx->inter_mode_probs[i], frame_ctx_helper->inter_mode[i],
+		       sizeof(frame_ctx_helper->inter_mode[0]));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->interp_filter); i++)
+		memcpy(frame_ctx->switch_interp_prob[i], frame_ctx_helper->interp_filter[i],
+		       sizeof(frame_ctx_helper->interp_filter[0]));
+
+	memcpy(frame_ctx->intra_inter_prob, frame_ctx_helper->is_inter,
+	       sizeof(frame_ctx_helper->is_inter));
+
+	memcpy(frame_ctx->comp_inter_prob, frame_ctx_helper->comp_mode,
+	       sizeof(frame_ctx_helper->comp_mode));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->single_ref); i++)
+		memcpy(frame_ctx->single_ref_prob[i], frame_ctx_helper->single_ref[i],
+		       sizeof(frame_ctx_helper->single_ref[0]));
+
+	memcpy(frame_ctx->comp_ref_prob, frame_ctx_helper->comp_ref,
+	       sizeof(frame_ctx_helper->comp_ref));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->y_mode); i++)
+		memcpy(frame_ctx->y_mode_prob[i], frame_ctx_helper->y_mode[i],
+		       sizeof(frame_ctx_helper->y_mode[0]));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->uv_mode); i++)
+		memcpy(frame_ctx->uv_mode_prob[i], frame_ctx_helper->uv_mode[i],
+		       sizeof(frame_ctx_helper->uv_mode[0]));
+
+	for (i = 0; i < ARRAY_SIZE(frame_ctx_helper->partition); i++)
+		memcpy(frame_ctx->partition_prob[i], frame_ctx_helper->partition[i],
+		       sizeof(frame_ctx_helper->partition[0]));
+
+	memcpy(frame_ctx->joint, mv->joint, sizeof(mv->joint));
+
+	for (i = 0; i < ARRAY_SIZE(mv->sign); i++)
+		frame_ctx->sign_classes[i].sign = mv->sign[i];
+
+	for (i = 0; i < ARRAY_SIZE(mv->classes); i++)
+		memcpy(frame_ctx->sign_classes[i].classes, mv->classes[i],
+		       sizeof(mv->classes[i]));
+
+	for (i = 0; i < ARRAY_SIZE(mv->class0_bit); i++)
+		frame_ctx->class0_bits[i].class0[0] = mv->class0_bit[i];
+
+	for (i = 0; i < ARRAY_SIZE(mv->bits); i++)
+		memcpy(frame_ctx->class0_bits[i].bits, mv->bits[i], sizeof(mv->bits[0]));
+
+	for (i = 0; i < ARRAY_SIZE(mv->class0_fr); i++)
+		for (j = 0; j < ARRAY_SIZE(mv->class0_fr[0]); j++)
+			memcpy(frame_ctx->class0_fp_hp[i].class0_fp[j], mv->class0_fr[i][j],
+			       sizeof(mv->class0_fr[0][0]));
+
+	for (i = 0; i < ARRAY_SIZE(mv->fr); i++)
+		memcpy(frame_ctx->class0_fp_hp[i].fp, mv->fr[i], sizeof(mv->fr[0]));
+
+	for (i = 0; i < ARRAY_SIZE(mv->class0_hp); i++)
+		frame_ctx->class0_fp_hp[i].class0_hp = mv->class0_hp[i];
+
+	for (i = 0; i < ARRAY_SIZE(mv->hp); i++)
+		frame_ctx->class0_fp_hp[i].hp = mv->hp[i];
+}
+
+static int vdec_vp9_slice_update_prob(struct vdec_vp9_slice_instance *instance,
+				      struct vdec_vp9_slice_vsi *vsi)
+{
+	struct vdec_vp9_slice_frame_ctx *pre_frame_ctx;
+	struct v4l2_vp9_frame_context *pre_frame_ctx_helper;
+	struct vdec_vp9_slice_frame_ctx *frame_ctx;
+	struct vdec_vp9_slice_frame_counts *counts;
+	struct v4l2_vp9_frame_symbol_counts *counts_helper;
+	struct vdec_vp9_slice_uncompressed_header *uh;
+	bool frame_is_intra;
+	bool use_128;
+
+	uh = &vsi->frame.uh;
+	pre_frame_ctx = &instance->frame_ctx[uh->frame_context_idx];
+	pre_frame_ctx_helper = &instance->frame_ctx_helper;
+	frame_ctx = (struct vdec_vp9_slice_frame_ctx *)instance->prob.va;
+	counts = (struct vdec_vp9_slice_frame_counts *)instance->counts.va;
+	counts_helper = &instance->counts_helper;
+
+	if (!uh->refresh_frame_context)
+		return 0;
+
+	if (!uh->frame_parallel_decoding_mode) {
+		vdec_vp9_slice_counts_map_helper(&instance->counts_map, counts, counts_helper);
+
+		frame_is_intra = !vsi->frame.uh.frame_type || vsi->frame.uh.intra_only;
+		/* check default prob */
+		if (!instance->dirty[uh->frame_context_idx])
+			vdec_vp9_slice_framectx_map_helper(frame_is_intra,
+							   vdec_vp9_slice_default_frame_ctx,
+							   frame_ctx,
+							   pre_frame_ctx_helper);
+		else
+			vdec_vp9_slice_framectx_map_helper(frame_is_intra,
+							   pre_frame_ctx,
+							   frame_ctx,
+							   pre_frame_ctx_helper);
+
+		use_128 = !frame_is_intra && !vsi->frame.uh.last_frame_type;
+		v4l2_vp9_adapt_coef_probs(pre_frame_ctx_helper,
+					  counts_helper,
+					  use_128,
+					  frame_is_intra);
+		if (!frame_is_intra)
+			v4l2_vp9_adapt_noncoef_probs(pre_frame_ctx_helper,
+						     counts_helper,
+						     V4L2_VP9_REFERENCE_MODE_SINGLE_REFERENCE,
+						     vsi->frame.uh.interpolation_filter,
+						     vsi->frame.ch.tx_mode,
+						     vsi->frame.uh.allow_high_precision_mv ?
+						     V4L2_VP9_FRAME_FLAG_ALLOW_HIGH_PREC_MV : 0);
+		vdec_vp9_slice_helper_map_framectx(pre_frame_ctx_helper, pre_frame_ctx);
+	} else {
+		memcpy(pre_frame_ctx, frame_ctx, sizeof(*frame_ctx));
+	}
+
+	instance->dirty[uh->frame_context_idx] = 1;
+
+	return 0;
+}
+
+static int vdec_vp9_slice_update_single(struct vdec_vp9_slice_instance *instance,
+					struct vdec_vp9_slice_pfc *pfc)
+{
+	struct vdec_vp9_slice_vsi *vsi;
+
+	vsi = &pfc->vsi;
+	memcpy(&pfc->state[0], &vsi->state, sizeof(vsi->state));
+
+	mtk_vcodec_debug(instance, "Frame %u Y_CRC %08x %08x %08x %08x\n",
+			 pfc->seq,
+			 vsi->state.crc[0], vsi->state.crc[1],
+			 vsi->state.crc[2], vsi->state.crc[3]);
+	mtk_vcodec_debug(instance, "Frame %u C_CRC %08x %08x %08x %08x\n",
+			 pfc->seq,
+			 vsi->state.crc[4], vsi->state.crc[5],
+			 vsi->state.crc[6], vsi->state.crc[7]);
+
+	vdec_vp9_slice_update_prob(instance, vsi);
+
+	instance->width = vsi->frame.uh.frame_width;
+	instance->height = vsi->frame.uh.frame_height;
+	instance->frame_type = vsi->frame.uh.frame_type;
+	instance->show_frame = vsi->frame.uh.show_frame;
+
+	return 0;
+}
+
+static int vdec_vp9_slice_update_lat(struct vdec_vp9_slice_instance *instance,
+				     struct vdec_lat_buf *lat_buf,
+				     struct vdec_vp9_slice_pfc *pfc)
+{
+	struct vdec_vp9_slice_vsi *vsi;
+
+	vsi = &pfc->vsi;
+	memcpy(&pfc->state[0], &vsi->state, sizeof(vsi->state));
+
+	mtk_vcodec_debug(instance, "Frame %u LAT CRC 0x%08x %lx %lx\n",
+			 pfc->seq, vsi->state.crc[0],
+			 (unsigned long)vsi->trans.dma_addr,
+			 (unsigned long)vsi->trans.dma_addr_end);
+
+	/* buffer full, need to re-decode */
+	if (vsi->state.full) {
+		/* buffer not enough */
+		if (vsi->trans.dma_addr_end - vsi->trans.dma_addr ==
+			vsi->ube.size)
+			return -ENOMEM;
+		return -EAGAIN;
+	}
+
+	vdec_vp9_slice_update_prob(instance, vsi);
+
+	instance->width = vsi->frame.uh.frame_width;
+	instance->height = vsi->frame.uh.frame_height;
+	instance->frame_type = vsi->frame.uh.frame_type;
+	instance->show_frame = vsi->frame.uh.show_frame;
+
+	return 0;
+}
+
+static int vdec_vp9_slice_setup_core_to_dst_buf(struct vdec_vp9_slice_instance *instance,
+						struct vdec_lat_buf *lat_buf)
+{
+	struct vb2_v4l2_buffer *dst;
+
+	dst = v4l2_m2m_next_dst_buf(instance->ctx->m2m_ctx);
+	if (!dst)
+		return -EINVAL;
+
+	v4l2_m2m_buf_copy_metadata(&lat_buf->ts_info, dst, true);
+	return 0;
+}
+
+static int vdec_vp9_slice_setup_core_buffer(struct vdec_vp9_slice_instance *instance,
+					    struct vdec_vp9_slice_pfc *pfc,
+					    struct vdec_vp9_slice_vsi *vsi,
+					    struct vdec_fb *fb,
+					    struct vdec_lat_buf *lat_buf)
+{
+	struct vb2_buffer *vb;
+	struct vb2_queue *vq;
+	struct vdec_vp9_slice_reference *ref;
+	int plane;
+	int size;
+	int w;
+	int h;
+	int i;
+
+	plane = instance->ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes;
+	w = vsi->frame.uh.frame_width;
+	h = vsi->frame.uh.frame_height;
+	size = ALIGN(w, 64) * ALIGN(h, 64);
+
+	/* frame buffer */
+	vsi->fb.y.dma_addr = fb->base_y.dma_addr;
+	if (plane == 1)
+		vsi->fb.c.dma_addr = fb->base_y.dma_addr + size;
+	else
+		vsi->fb.c.dma_addr = fb->base_c.dma_addr;
+
+	/* reference buffers */
+	vq = v4l2_m2m_get_vq(instance->ctx->m2m_ctx,
+			     V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (!vq)
+		return -EINVAL;
+
+	/* get current output buffer */
+	vb = &v4l2_m2m_next_dst_buf(instance->ctx->m2m_ctx)->vb2_buf;
+	if (!vb)
+		return -EINVAL;
+
+	/* update internal buffer's width/height */
+	for (i = 0; i < vq->num_buffers; i++) {
+		if (vb == vq->bufs[i]) {
+			instance->dpb[i].width = w;
+			instance->dpb[i].height = h;
+			break;
+		}
+	}
+
+	/*
+	 * get buffer's width/height from instance
+	 * get buffer address from vb2buf
+	 */
+	for (i = 0; i < 3; i++) {
+		ref = &vsi->frame.ref[i];
+		vb = vb2_find_buffer(vq, pfc->ref_idx[i]);
+		if (!vb) {
+			ref->frame_width = w;
+			ref->frame_height = h;
+			memset(&vsi->ref[i], 0, sizeof(vsi->ref[i]));
+		} else {
+			int idx = vb->index;
+
+			ref->frame_width = instance->dpb[idx].width;
+			ref->frame_height = instance->dpb[idx].height;
+			vsi->ref[i].y.dma_addr =
+				vb2_dma_contig_plane_dma_addr(vb, 0);
+			if (plane == 1)
+				vsi->ref[i].c.dma_addr =
+					vsi->ref[i].y.dma_addr + size;
+			else
+				vsi->ref[i].c.dma_addr =
+					vb2_dma_contig_plane_dma_addr(vb, 1);
+		}
+	}
+
+	return 0;
+}
+
+static void vdec_vp9_slice_setup_single_buffer(struct vdec_vp9_slice_instance *instance,
+					       struct vdec_vp9_slice_pfc *pfc,
+					       struct vdec_vp9_slice_vsi *vsi,
+					       struct mtk_vcodec_mem *bs,
+					       struct vdec_fb *fb)
+{
+	int i;
+
+	vsi->bs.buf.dma_addr = bs->dma_addr;
+	vsi->bs.buf.size = bs->size;
+	vsi->bs.frame.dma_addr = bs->dma_addr;
+	vsi->bs.frame.size = bs->size;
+
+	for (i = 0; i < 2; i++) {
+		vsi->mv[i].dma_addr = instance->mv[i].dma_addr;
+		vsi->mv[i].size = instance->mv[i].size;
+	}
+	for (i = 0; i < 2; i++) {
+		vsi->seg[i].dma_addr = instance->seg[i].dma_addr;
+		vsi->seg[i].size = instance->seg[i].size;
+	}
+	vsi->tile.dma_addr = instance->tile.dma_addr;
+	vsi->tile.size = instance->tile.size;
+	vsi->prob.dma_addr = instance->prob.dma_addr;
+	vsi->prob.size = instance->prob.size;
+	vsi->counts.dma_addr = instance->counts.dma_addr;
+	vsi->counts.size = instance->counts.size;
+
+	vsi->row_info.buf = 0;
+	vsi->row_info.size = 0;
+
+	vdec_vp9_slice_setup_core_buffer(instance, pfc, vsi, fb, NULL);
+}
+
+static int vdec_vp9_slice_setup_core(struct vdec_vp9_slice_instance *instance,
+				     struct vdec_fb *fb,
+				     struct vdec_lat_buf *lat_buf,
+				     struct vdec_vp9_slice_pfc *pfc)
+{
+	struct vdec_vp9_slice_vsi *vsi = &pfc->vsi;
+	int ret;
+
+	vdec_vp9_slice_setup_state(vsi);
+
+	ret = vdec_vp9_slice_setup_core_to_dst_buf(instance, lat_buf);
+	if (ret)
+		goto err;
+
+	ret = vdec_vp9_slice_setup_core_buffer(instance, pfc, vsi, fb, lat_buf);
+	if (ret)
+		goto err;
+
+	vdec_vp9_slice_setup_seg_buffer(instance, vsi, &instance->seg[1]);
+
+	return 0;
+
+err:
+	return ret;
+}
+
+static int vdec_vp9_slice_setup_single(struct vdec_vp9_slice_instance *instance,
+				       struct mtk_vcodec_mem *bs,
+				       struct vdec_fb *fb,
+				       struct vdec_vp9_slice_pfc *pfc)
+{
+	struct vdec_vp9_slice_vsi *vsi = &pfc->vsi;
+	int ret;
+
+	ret = vdec_vp9_slice_setup_single_from_src_to_dst(instance);
+	if (ret)
+		goto err;
+
+	ret = vdec_vp9_slice_setup_pfc(instance, pfc);
+	if (ret)
+		goto err;
+
+	ret = vdec_vp9_slice_alloc_working_buffer(instance, vsi);
+	if (ret)
+		goto err;
+
+	vdec_vp9_slice_setup_single_buffer(instance, pfc, vsi, bs, fb);
+	vdec_vp9_slice_setup_seg_buffer(instance, vsi, &instance->seg[0]);
+
+	ret = vdec_vp9_slice_setup_prob_buffer(instance, vsi);
+	if (ret)
+		goto err;
+
+	ret = vdec_vp9_slice_setup_tile_buffer(instance, vsi, bs);
+	if (ret)
+		goto err;
+
+	return 0;
+
+err:
+	return ret;
+}
+
+static int vdec_vp9_slice_update_core(struct vdec_vp9_slice_instance *instance,
+				      struct vdec_lat_buf *lat_buf,
+				      struct vdec_vp9_slice_pfc *pfc)
+{
+	struct vdec_vp9_slice_vsi *vsi;
+
+	vsi = &pfc->vsi;
+	memcpy(&pfc->state[1], &vsi->state, sizeof(vsi->state));
+
+	mtk_vcodec_debug(instance, "Frame %u Y_CRC %08x %08x %08x %08x\n",
+			 pfc->seq,
+			 vsi->state.crc[0], vsi->state.crc[1],
+			 vsi->state.crc[2], vsi->state.crc[3]);
+	mtk_vcodec_debug(instance, "Frame %u C_CRC %08x %08x %08x %08x\n",
+			 pfc->seq,
+			 vsi->state.crc[4], vsi->state.crc[5],
+			 vsi->state.crc[6], vsi->state.crc[7]);
+
+	return 0;
+}
+
+static int vdec_vp9_slice_init(struct mtk_vcodec_ctx *ctx)
+{
+	struct vdec_vp9_slice_instance *instance;
+	struct vdec_vp9_slice_init_vsi *vsi;
+	int ret;
+
+	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
+	if (!instance)
+		return -ENOMEM;
+
+	instance->ctx = ctx;
+	instance->vpu.id = SCP_IPI_VDEC_LAT;
+	instance->vpu.core_id = SCP_IPI_VDEC_CORE;
+	instance->vpu.ctx = ctx;
+	instance->vpu.codec_type = ctx->current_codec;
+
+	ret = vpu_dec_init(&instance->vpu);
+	if (ret) {
+		mtk_vcodec_err(instance, "failed to init vpu dec, ret %d\n", ret);
+		goto error_vpu_init;
+	}
+
+	/* init vsi and global flags */
+
+	vsi = instance->vpu.vsi;
+	if (!vsi) {
+		mtk_vcodec_err(instance, "failed to get VP9 vsi\n");
+		ret = -EINVAL;
+		goto error_vsi;
+	}
+	instance->init_vsi = vsi;
+	instance->core_vsi = mtk_vcodec_fw_map_dm_addr(ctx->dev->fw_handler,
+						       (u32)vsi->core_vsi);
+	if (!instance->core_vsi) {
+		mtk_vcodec_err(instance, "failed to get VP9 core vsi\n");
+		ret = -EINVAL;
+		goto error_vsi;
+	}
+
+	instance->irq = 1;
+
+	ret = vdec_vp9_slice_init_default_frame_ctx(instance);
+	if (ret)
+		goto error_default_frame_ctx;
+
+	ctx->drv_handle = instance;
+
+	return 0;
+
+error_default_frame_ctx:
+error_vsi:
+	vpu_dec_deinit(&instance->vpu);
+error_vpu_init:
+	kfree(instance);
+	return ret;
+}
+
+static void vdec_vp9_slice_deinit(void *h_vdec)
+{
+	struct vdec_vp9_slice_instance *instance = h_vdec;
+
+	if (!instance)
+		return;
+
+	vpu_dec_deinit(&instance->vpu);
+	vdec_vp9_slice_free_working_buffer(instance);
+	vdec_msg_queue_deinit(&instance->ctx->msg_queue, instance->ctx);
+	kfree(instance);
+}
+
+static int vdec_vp9_slice_flush(void *h_vdec, struct mtk_vcodec_mem *bs,
+				struct vdec_fb *fb, bool *res_chg)
+{
+	struct vdec_vp9_slice_instance *instance = h_vdec;
+
+	mtk_vcodec_debug(instance, "flush ...\n");
+	if (instance->ctx->dev->vdec_pdata->hw_arch != MTK_VDEC_PURE_SINGLE_CORE)
+		vdec_msg_queue_wait_lat_buf_full(&instance->ctx->msg_queue);
+	return vpu_dec_reset(&instance->vpu);
+}
+
+static void vdec_vp9_slice_get_pic_info(struct vdec_vp9_slice_instance *instance)
+{
+	struct mtk_vcodec_ctx *ctx = instance->ctx;
+	unsigned int data[3];
+
+	mtk_vcodec_debug(instance, "w %u h %u\n",
+			 ctx->picinfo.pic_w, ctx->picinfo.pic_h);
+
+	data[0] = ctx->picinfo.pic_w;
+	data[1] = ctx->picinfo.pic_h;
+	data[2] = ctx->capture_fourcc;
+	vpu_dec_get_param(&instance->vpu, data, 3, GET_PARAM_PIC_INFO);
+
+	ctx->picinfo.buf_w = ALIGN(ctx->picinfo.pic_w, 64);
+	ctx->picinfo.buf_h = ALIGN(ctx->picinfo.pic_h, 64);
+	ctx->picinfo.fb_sz[0] = instance->vpu.fb_sz[0];
+	ctx->picinfo.fb_sz[1] = instance->vpu.fb_sz[1];
+}
+
+static void vdec_vp9_slice_get_dpb_size(struct vdec_vp9_slice_instance *instance,
+					unsigned int *dpb_sz)
+{
+	/* refer VP9 specification */
+	*dpb_sz = 9;
+}
+
+static int vdec_vp9_slice_get_param(void *h_vdec, enum vdec_get_param_type type, void *out)
+{
+	struct vdec_vp9_slice_instance *instance = h_vdec;
+
+	switch (type) {
+	case GET_PARAM_PIC_INFO:
+		vdec_vp9_slice_get_pic_info(instance);
+		break;
+	case GET_PARAM_DPB_SIZE:
+		vdec_vp9_slice_get_dpb_size(instance, out);
+		break;
+	case GET_PARAM_CROP_INFO:
+		mtk_vcodec_debug(instance, "No need to get vp9 crop information.");
+		break;
+	default:
+		mtk_vcodec_err(instance, "invalid get parameter type=%d\n",
+			       type);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vdec_vp9_slice_single_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
+					struct vdec_fb *fb, bool *res_chg)
+{
+	struct vdec_vp9_slice_instance *instance = h_vdec;
+	struct vdec_vp9_slice_pfc *pfc = &instance->sc_pfc;
+	struct vdec_vp9_slice_vsi *vsi;
+	struct mtk_vcodec_ctx *ctx;
+	int ret;
+
+	if (!instance || !instance->ctx)
+		return -EINVAL;
+	ctx = instance->ctx;
+
+	/* bs NULL means flush decoder */
+	if (!bs)
+		return vdec_vp9_slice_flush(h_vdec, bs, fb, res_chg);
+
+	fb = ctx->dev->vdec_pdata->get_cap_buffer(ctx);
+	if (!fb)
+		return -EBUSY;
+
+	vsi = &pfc->vsi;
+
+	ret = vdec_vp9_slice_setup_single(instance, bs, fb, pfc);
+	if (ret) {
+		mtk_vcodec_err(instance, "Failed to setup VP9 single ret %d\n", ret);
+		return ret;
+	}
+	vdec_vp9_slice_vsi_to_remote(vsi, instance->vsi);
+
+	ret = vpu_dec_start(&instance->vpu, NULL, 0);
+	if (ret) {
+		mtk_vcodec_err(instance, "Failed to dec VP9 ret %d\n", ret);
+		return ret;
+	}
+
+	ret = mtk_vcodec_wait_for_done_ctx(ctx,	MTK_INST_IRQ_RECEIVED,
+					   WAIT_INTR_TIMEOUT_MS, MTK_VDEC_CORE);
+	/* update remote vsi if decode timeout */
+	if (ret) {
+		mtk_vcodec_err(instance, "VP9 decode timeout %d\n", ret);
+		WRITE_ONCE(instance->vsi->state.timeout, 1);
+	}
+
+	vpu_dec_end(&instance->vpu);
+
+	vdec_vp9_slice_vsi_from_remote(vsi, instance->vsi, 0);
+	ret = vdec_vp9_slice_update_single(instance, pfc);
+	if (ret) {
+		mtk_vcodec_err(instance, "VP9 decode error: %d\n", ret);
+		return ret;
+	}
+
+	instance->ctx->decoded_frame_cnt++;
+	return 0;
+}
+
+static int vdec_vp9_slice_lat_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
+				     struct vdec_fb *fb, bool *res_chg)
+{
+	struct vdec_vp9_slice_instance *instance = h_vdec;
+	struct vdec_lat_buf *lat_buf;
+	struct vdec_vp9_slice_pfc *pfc;
+	struct vdec_vp9_slice_vsi *vsi;
+	struct mtk_vcodec_ctx *ctx;
+	int ret;
+
+	if (!instance || !instance->ctx)
+		return -EINVAL;
+	ctx = instance->ctx;
+
+	/* init msgQ for the first time */
+	if (vdec_msg_queue_init(&ctx->msg_queue, ctx,
+				vdec_vp9_slice_core_decode,
+				sizeof(*pfc)))
+		return -ENOMEM;
+
+	/* bs NULL means flush decoder */
+	if (!bs)
+		return vdec_vp9_slice_flush(h_vdec, bs, fb, res_chg);
+
+	lat_buf = vdec_msg_queue_dqbuf(&instance->ctx->msg_queue.lat_ctx);
+	if (!lat_buf) {
+		mtk_vcodec_err(instance, "Failed to get VP9 lat buf\n");
+		return -EAGAIN;
+	}
+	pfc = (struct vdec_vp9_slice_pfc *)lat_buf->private_data;
+	if (!pfc) {
+		ret = -EINVAL;
+		goto err_free_fb_out;
+	}
+	vsi = &pfc->vsi;
+
+	ret = vdec_vp9_slice_setup_lat(instance, bs, lat_buf, pfc);
+	if (ret) {
+		mtk_vcodec_err(instance, "Failed to setup VP9 lat ret %d\n", ret);
+		goto err_free_fb_out;
+	}
+	vdec_vp9_slice_vsi_to_remote(vsi, instance->vsi);
+
+	ret = vpu_dec_start(&instance->vpu, NULL, 0);
+	if (ret) {
+		mtk_vcodec_err(instance, "Failed to dec VP9 ret %d\n", ret);
+		goto err_free_fb_out;
+	}
+
+	if (instance->irq) {
+		ret = mtk_vcodec_wait_for_done_ctx(ctx,	MTK_INST_IRQ_RECEIVED,
+						   WAIT_INTR_TIMEOUT_MS, MTK_VDEC_LAT0);
+		/* update remote vsi if decode timeout */
+		if (ret) {
+			mtk_vcodec_err(instance, "VP9 decode timeout %d pic %d\n", ret, pfc->seq);
+			WRITE_ONCE(instance->vsi->state.timeout, 1);
+		}
+		vpu_dec_end(&instance->vpu);
+	}
+
+	vdec_vp9_slice_vsi_from_remote(vsi, instance->vsi, 0);
+	ret = vdec_vp9_slice_update_lat(instance, lat_buf, pfc);
+
+	/* LAT trans full, no more UBE or decode timeout */
+	if (ret) {
+		mtk_vcodec_err(instance, "VP9 decode error: %d\n", ret);
+		goto err_free_fb_out;
+	}
+
+	mtk_vcodec_debug(instance, "lat dma addr: 0x%lx 0x%lx\n",
+			 (unsigned long)pfc->vsi.trans.dma_addr,
+			 (unsigned long)pfc->vsi.trans.dma_addr_end);
+
+	vdec_msg_queue_update_ube_wptr(&ctx->msg_queue,
+				       vsi->trans.dma_addr_end +
+				       ctx->msg_queue.wdma_addr.dma_addr);
+	vdec_msg_queue_qbuf(&ctx->dev->msg_queue_core_ctx, lat_buf);
+
+	return 0;
+err_free_fb_out:
+	vdec_msg_queue_qbuf(&ctx->msg_queue.lat_ctx, lat_buf);
+	return ret;
+}
+
+static int vdec_vp9_slice_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
+				 struct vdec_fb *fb, bool *res_chg)
+{
+	struct vdec_vp9_slice_instance *instance = h_vdec;
+	int ret;
+
+	if (instance->ctx->dev->vdec_pdata->hw_arch == MTK_VDEC_PURE_SINGLE_CORE)
+		ret = vdec_vp9_slice_single_decode(h_vdec, bs, fb, res_chg);
+	else
+		ret = vdec_vp9_slice_lat_decode(h_vdec, bs, fb, res_chg);
+
+	return ret;
+}
+
+static int vdec_vp9_slice_core_decode(struct vdec_lat_buf *lat_buf)
+{
+	struct vdec_vp9_slice_instance *instance;
+	struct vdec_vp9_slice_pfc *pfc;
+	struct mtk_vcodec_ctx *ctx = NULL;
+	struct vdec_fb *fb = NULL;
+	int ret = -EINVAL;
+
+	if (!lat_buf)
+		goto err;
+
+	pfc = lat_buf->private_data;
+	ctx = lat_buf->ctx;
+	if (!pfc || !ctx)
+		goto err;
+
+	instance = ctx->drv_handle;
+	if (!instance)
+		goto err;
+
+	fb = ctx->dev->vdec_pdata->get_cap_buffer(ctx);
+	if (!fb) {
+		ret = -EBUSY;
+		goto err;
+	}
+
+	ret = vdec_vp9_slice_setup_core(instance, fb, lat_buf, pfc);
+	if (ret) {
+		mtk_vcodec_err(instance, "vdec_vp9_slice_setup_core\n");
+		goto err;
+	}
+	vdec_vp9_slice_vsi_to_remote(&pfc->vsi, instance->core_vsi);
+
+	ret = vpu_dec_core(&instance->vpu);
+	if (ret) {
+		mtk_vcodec_err(instance, "vpu_dec_core\n");
+		goto err;
+	}
+
+	if (instance->irq) {
+		ret = mtk_vcodec_wait_for_done_ctx(ctx, MTK_INST_IRQ_RECEIVED,
+						   WAIT_INTR_TIMEOUT_MS, MTK_VDEC_CORE);
+		/* update remote vsi if decode timeout */
+		if (ret) {
+			mtk_vcodec_err(instance, "VP9 core timeout pic %d\n", pfc->seq);
+			WRITE_ONCE(instance->core_vsi->state.timeout, 1);
+		}
+		vpu_dec_core_end(&instance->vpu);
+	}
+
+	vdec_vp9_slice_vsi_from_remote(&pfc->vsi, instance->core_vsi, 1);
+	ret = vdec_vp9_slice_update_core(instance, lat_buf, pfc);
+	if (ret) {
+		mtk_vcodec_err(instance, "vdec_vp9_slice_update_core\n");
+		goto err;
+	}
+
+	pfc->vsi.trans.dma_addr_end += ctx->msg_queue.wdma_addr.dma_addr;
+	mtk_vcodec_debug(instance, "core dma_addr_end 0x%lx\n",
+			 (unsigned long)pfc->vsi.trans.dma_addr_end);
+	vdec_msg_queue_update_ube_rptr(&ctx->msg_queue, pfc->vsi.trans.dma_addr_end);
+	ctx->dev->vdec_pdata->cap_to_disp(ctx, 0, lat_buf->src_buf_req);
+
+	return 0;
+
+err:
+	if (ctx && pfc) {
+		/* always update read pointer */
+		vdec_msg_queue_update_ube_rptr(&ctx->msg_queue, pfc->vsi.trans.dma_addr_end);
+
+		if (fb)
+			ctx->dev->vdec_pdata->cap_to_disp(ctx, 1, lat_buf->src_buf_req);
+	}
+	return ret;
+}
+
+const struct vdec_common_if vdec_vp9_slice_lat_if = {
+	.init		= vdec_vp9_slice_init,
+	.decode		= vdec_vp9_slice_decode,
+	.get_param	= vdec_vp9_slice_get_param,
+	.deinit		= vdec_vp9_slice_deinit,
+};
diff --git a/drivers/media/platform/mediatek/vcodec/vdec_drv_base.h b/drivers/media/platform/mediatek/vcodec/vdec_drv_base.h
new file mode 100644
index 000000000..e913f963b
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec_drv_base.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: PC Chen <pc.chen@mediatek.com>
+ */
+
+#ifndef _VDEC_DRV_BASE_
+#define _VDEC_DRV_BASE_
+
+#include "vdec_drv_if.h"
+
+struct vdec_common_if {
+	/**
+	 * (*init)() - initialize decode driver
+	 * @ctx     : [in] mtk v4l2 context
+	 * @h_vdec  : [out] driver handle
+	 */
+	int (*init)(struct mtk_vcodec_ctx *ctx);
+
+	/**
+	 * (*decode)() - trigger decode
+	 * @h_vdec  : [in] driver handle
+	 * @bs      : [in] input bitstream
+	 * @fb      : [in] frame buffer to store decoded frame
+	 * @res_chg : [out] resolution change happen
+	 */
+	int (*decode)(void *h_vdec, struct mtk_vcodec_mem *bs,
+		      struct vdec_fb *fb, bool *res_chg);
+
+	/**
+	 * (*get_param)() - get driver's parameter
+	 * @h_vdec : [in] driver handle
+	 * @type   : [in] input parameter type
+	 * @out    : [out] buffer to store query result
+	 */
+	int (*get_param)(void *h_vdec, enum vdec_get_param_type type,
+			 void *out);
+
+	/**
+	 * (*deinit)() - deinitialize driver.
+	 * @h_vdec : [in] driver handle to be deinit
+	 */
+	void (*deinit)(void *h_vdec);
+};
+
+#endif
diff --git a/drivers/media/platform/mediatek/vcodec/vdec_drv_if.c b/drivers/media/platform/mediatek/vcodec/vdec_drv_if.c
new file mode 100644
index 000000000..f3807f03d
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec_drv_if.c
@@ -0,0 +1,120 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: PC Chen <pc.chen@mediatek.com>
+ *         Tiffany Lin <tiffany.lin@mediatek.com>
+ */
+
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+
+#include "vdec_drv_if.h"
+#include "mtk_vcodec_dec.h"
+#include "vdec_drv_base.h"
+#include "mtk_vcodec_dec_pm.h"
+
+int vdec_if_init(struct mtk_vcodec_ctx *ctx, unsigned int fourcc)
+{
+	enum mtk_vdec_hw_arch hw_arch = ctx->dev->vdec_pdata->hw_arch;
+	int ret = 0;
+
+	switch (fourcc) {
+	case V4L2_PIX_FMT_H264_SLICE:
+		if (!ctx->dev->vdec_pdata->is_subdev_supported) {
+			ctx->dec_if = &vdec_h264_slice_if;
+			ctx->hw_id = MTK_VDEC_CORE;
+		} else {
+			ctx->dec_if = &vdec_h264_slice_multi_if;
+			ctx->hw_id = IS_VDEC_LAT_ARCH(hw_arch) ? MTK_VDEC_LAT0 : MTK_VDEC_CORE;
+		}
+		break;
+	case V4L2_PIX_FMT_H264:
+		ctx->dec_if = &vdec_h264_if;
+		ctx->hw_id = MTK_VDEC_CORE;
+		break;
+	case V4L2_PIX_FMT_VP8_FRAME:
+		ctx->dec_if = &vdec_vp8_slice_if;
+		ctx->hw_id = MTK_VDEC_CORE;
+		break;
+	case V4L2_PIX_FMT_VP8:
+		ctx->dec_if = &vdec_vp8_if;
+		ctx->hw_id = MTK_VDEC_CORE;
+		break;
+	case V4L2_PIX_FMT_VP9:
+		ctx->dec_if = &vdec_vp9_if;
+		ctx->hw_id = MTK_VDEC_CORE;
+		break;
+	case V4L2_PIX_FMT_VP9_FRAME:
+		ctx->dec_if = &vdec_vp9_slice_lat_if;
+		ctx->hw_id = IS_VDEC_LAT_ARCH(hw_arch) ? MTK_VDEC_LAT0 : MTK_VDEC_CORE;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	mtk_vcodec_dec_enable_hardware(ctx, ctx->hw_id);
+	ret = ctx->dec_if->init(ctx);
+	mtk_vcodec_dec_disable_hardware(ctx, ctx->hw_id);
+
+	return ret;
+}
+
+int vdec_if_decode(struct mtk_vcodec_ctx *ctx, struct mtk_vcodec_mem *bs,
+		   struct vdec_fb *fb, bool *res_chg)
+{
+	int ret = 0;
+
+	if (bs) {
+		if ((bs->dma_addr & 63) != 0) {
+			mtk_v4l2_err("bs dma_addr should 64 byte align");
+			return -EINVAL;
+		}
+	}
+
+	if (fb) {
+		if (((fb->base_y.dma_addr & 511) != 0) ||
+		    ((fb->base_c.dma_addr & 511) != 0)) {
+			mtk_v4l2_err("frame buffer dma_addr should 512 byte align");
+			return -EINVAL;
+		}
+	}
+
+	if (!ctx->drv_handle)
+		return -EIO;
+
+	mtk_vcodec_dec_enable_hardware(ctx, ctx->hw_id);
+	mtk_vcodec_set_curr_ctx(ctx->dev, ctx, ctx->hw_id);
+	ret = ctx->dec_if->decode(ctx->drv_handle, bs, fb, res_chg);
+	mtk_vcodec_set_curr_ctx(ctx->dev, NULL, ctx->hw_id);
+	mtk_vcodec_dec_disable_hardware(ctx, ctx->hw_id);
+
+	return ret;
+}
+
+int vdec_if_get_param(struct mtk_vcodec_ctx *ctx, enum vdec_get_param_type type,
+		      void *out)
+{
+	int ret = 0;
+
+	if (!ctx->drv_handle)
+		return -EIO;
+
+	mtk_vdec_lock(ctx);
+	ret = ctx->dec_if->get_param(ctx->drv_handle, type, out);
+	mtk_vdec_unlock(ctx);
+
+	return ret;
+}
+
+void vdec_if_deinit(struct mtk_vcodec_ctx *ctx)
+{
+	if (!ctx->drv_handle)
+		return;
+
+	mtk_vcodec_dec_enable_hardware(ctx, ctx->hw_id);
+	ctx->dec_if->deinit(ctx->drv_handle);
+	mtk_vcodec_dec_disable_hardware(ctx, ctx->hw_id);
+
+	ctx->drv_handle = NULL;
+}
diff --git a/drivers/media/platform/mediatek/vcodec/vdec_drv_if.h b/drivers/media/platform/mediatek/vcodec/vdec_drv_if.h
new file mode 100644
index 000000000..076306ff2
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec_drv_if.h
@@ -0,0 +1,103 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: PC Chen <pc.chen@mediatek.com>
+ *		   Tiffany Lin <tiffany.lin@mediatek.com>
+ */
+
+#ifndef _VDEC_DRV_IF_H_
+#define _VDEC_DRV_IF_H_
+
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_dec.h"
+#include "mtk_vcodec_util.h"
+
+
+/**
+ * enum vdec_fb_status  - decoder frame buffer status
+ * @FB_ST_NORMAL: initial state
+ * @FB_ST_DISPLAY: frame buffer is ready to be displayed
+ * @FB_ST_FREE: frame buffer is not used by decoder any more
+ */
+enum vdec_fb_status {
+	FB_ST_NORMAL		= 0,
+	FB_ST_DISPLAY		= (1 << 0),
+	FB_ST_FREE		= (1 << 1)
+};
+
+/* For GET_PARAM_DISP_FRAME_BUFFER and GET_PARAM_FREE_FRAME_BUFFER,
+ * the caller does not own the returned buffer. The buffer will not be
+ *				released before vdec_if_deinit.
+ * GET_PARAM_DISP_FRAME_BUFFER	: get next displayable frame buffer,
+ *				struct vdec_fb**
+ * GET_PARAM_FREE_FRAME_BUFFER	: get non-referenced framebuffer, vdec_fb**
+ * GET_PARAM_PIC_INFO		: get picture info, struct vdec_pic_info*
+ * GET_PARAM_CROP_INFO		: get crop info, struct v4l2_crop*
+ * GET_PARAM_DPB_SIZE		: get dpb size, unsigned int*
+ */
+enum vdec_get_param_type {
+	GET_PARAM_DISP_FRAME_BUFFER,
+	GET_PARAM_FREE_FRAME_BUFFER,
+	GET_PARAM_PIC_INFO,
+	GET_PARAM_CROP_INFO,
+	GET_PARAM_DPB_SIZE
+};
+
+/**
+ * struct vdec_fb_node  - decoder frame buffer node
+ * @list	: list to hold this node
+ * @fb	: point to frame buffer (vdec_fb), fb could point to frame buffer and
+ *	working buffer this is for maintain buffers in different state
+ */
+struct vdec_fb_node {
+	struct list_head list;
+	struct vdec_fb *fb;
+};
+
+extern const struct vdec_common_if vdec_h264_if;
+extern const struct vdec_common_if vdec_h264_slice_if;
+extern const struct vdec_common_if vdec_h264_slice_multi_if;
+extern const struct vdec_common_if vdec_vp8_if;
+extern const struct vdec_common_if vdec_vp8_slice_if;
+extern const struct vdec_common_if vdec_vp9_if;
+extern const struct vdec_common_if vdec_vp9_slice_lat_if;
+
+/**
+ * vdec_if_init() - initialize decode driver
+ * @ctx	: [in] v4l2 context
+ * @fourcc	: [in] video format fourcc, V4L2_PIX_FMT_H264/VP8/VP9..
+ */
+int vdec_if_init(struct mtk_vcodec_ctx *ctx, unsigned int fourcc);
+
+/**
+ * vdec_if_deinit() - deinitialize decode driver
+ * @ctx	: [in] v4l2 context
+ *
+ */
+void vdec_if_deinit(struct mtk_vcodec_ctx *ctx);
+
+/**
+ * vdec_if_decode() - trigger decode
+ * @ctx	: [in] v4l2 context
+ * @bs	: [in] input bitstream
+ * @fb	: [in] frame buffer to store decoded frame, when null means parse
+ *	header only
+ * @res_chg	: [out] resolution change happens if current bs have different
+ *	picture width/height
+ * Note: To flush the decoder when reaching EOF, set input bitstream as NULL.
+ *
+ * Return: 0 on success. -EIO on unrecoverable error.
+ */
+int vdec_if_decode(struct mtk_vcodec_ctx *ctx, struct mtk_vcodec_mem *bs,
+		   struct vdec_fb *fb, bool *res_chg);
+
+/**
+ * vdec_if_get_param() - get driver's parameter
+ * @ctx	: [in] v4l2 context
+ * @type	: [in] input parameter type
+ * @out	: [out] buffer to store query result
+ */
+int vdec_if_get_param(struct mtk_vcodec_ctx *ctx, enum vdec_get_param_type type,
+		      void *out);
+
+#endif
diff --git a/drivers/media/platform/mediatek/vcodec/vdec_ipi_msg.h b/drivers/media/platform/mediatek/vcodec/vdec_ipi_msg.h
new file mode 100644
index 000000000..47070be2a
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec_ipi_msg.h
@@ -0,0 +1,153 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: PC Chen <pc.chen@mediatek.com>
+ */
+
+#ifndef _VDEC_IPI_MSG_H_
+#define _VDEC_IPI_MSG_H_
+
+/*
+ * enum vdec_ipi_msgid - message id between AP and VPU
+ * @AP_IPIMSG_XXX	: AP to VPU cmd message id
+ * @VPU_IPIMSG_XXX_ACK	: VPU ack AP cmd message id
+ */
+enum vdec_ipi_msgid {
+	AP_IPIMSG_DEC_INIT = 0xA000,
+	AP_IPIMSG_DEC_START = 0xA001,
+	AP_IPIMSG_DEC_END = 0xA002,
+	AP_IPIMSG_DEC_DEINIT = 0xA003,
+	AP_IPIMSG_DEC_RESET = 0xA004,
+	AP_IPIMSG_DEC_CORE = 0xA005,
+	AP_IPIMSG_DEC_CORE_END = 0xA006,
+	AP_IPIMSG_DEC_GET_PARAM = 0xA007,
+
+	VPU_IPIMSG_DEC_INIT_ACK = 0xB000,
+	VPU_IPIMSG_DEC_START_ACK = 0xB001,
+	VPU_IPIMSG_DEC_END_ACK = 0xB002,
+	VPU_IPIMSG_DEC_DEINIT_ACK = 0xB003,
+	VPU_IPIMSG_DEC_RESET_ACK = 0xB004,
+	VPU_IPIMSG_DEC_CORE_ACK = 0xB005,
+	VPU_IPIMSG_DEC_CORE_END_ACK = 0xB006,
+	VPU_IPIMSG_DEC_GET_PARAM_ACK = 0xB007,
+};
+
+/**
+ * struct vdec_ap_ipi_cmd - generic AP to VPU ipi command format
+ * @msg_id	: vdec_ipi_msgid
+ * @vpu_inst_addr : VPU decoder instance address. Used if ABI version < 2.
+ * @inst_id     : instance ID. Used if the ABI version >= 2.
+ * @codec_type	: codec fourcc
+ * @reserved	: reserved param
+ */
+struct vdec_ap_ipi_cmd {
+	uint32_t msg_id;
+	union {
+		uint32_t vpu_inst_addr;
+		uint32_t inst_id;
+	};
+	u32 codec_type;
+	u32 reserved;
+};
+
+/**
+ * struct vdec_vpu_ipi_ack - generic VPU to AP ipi command format
+ * @msg_id	: vdec_ipi_msgid
+ * @status	: VPU exeuction result
+ * @ap_inst_addr	: AP video decoder instance address
+ */
+struct vdec_vpu_ipi_ack {
+	uint32_t msg_id;
+	int32_t status;
+	uint64_t ap_inst_addr;
+};
+
+/**
+ * struct vdec_ap_ipi_init - for AP_IPIMSG_DEC_INIT
+ * @msg_id	: AP_IPIMSG_DEC_INIT
+ * @codec_type	: codec fourcc
+ * @ap_inst_addr	: AP video decoder instance address
+ */
+struct vdec_ap_ipi_init {
+	uint32_t msg_id;
+	u32 codec_type;
+	uint64_t ap_inst_addr;
+};
+
+/**
+ * struct vdec_ap_ipi_dec_start - for AP_IPIMSG_DEC_START
+ * @msg_id	: AP_IPIMSG_DEC_START
+ * @vpu_inst_addr : VPU decoder instance address. Used if ABI version < 2.
+ * @inst_id     : instance ID. Used if the ABI version >= 2.
+ * @data	: Header info
+ *	H264 decoder [0]:buf_sz [1]:nal_start
+ *	VP8 decoder  [0]:width/height
+ *	VP9 decoder  [0]:profile, [1][2] width/height
+ * @codec_type	: codec fourcc
+ */
+struct vdec_ap_ipi_dec_start {
+	uint32_t msg_id;
+	union {
+		uint32_t vpu_inst_addr;
+		uint32_t inst_id;
+	};
+	uint32_t data[3];
+	u32 codec_type;
+};
+
+/**
+ * struct vdec_vpu_ipi_init_ack - for VPU_IPIMSG_DEC_INIT_ACK
+ * @msg_id	: VPU_IPIMSG_DEC_INIT_ACK
+ * @status	: VPU exeuction result
+ * @ap_inst_addr	: AP vcodec_vpu_inst instance address
+ * @vpu_inst_addr	: VPU decoder instance address
+ * @vdec_abi_version:	ABI version of the firmware. Kernel can use it to
+ *			ensure that it is compatible with the firmware.
+ *			This field is not valid for MT8173 and must not be
+ *			accessed for this chip.
+ * @inst_id     : instance ID. Valid only if the ABI version >= 2.
+ */
+struct vdec_vpu_ipi_init_ack {
+	uint32_t msg_id;
+	int32_t status;
+	uint64_t ap_inst_addr;
+	uint32_t vpu_inst_addr;
+	uint32_t vdec_abi_version;
+	uint32_t inst_id;
+};
+
+/**
+ * struct vdec_ap_ipi_get_param - for AP_IPIMSG_DEC_GET_PARAM
+ * @msg_id	: AP_IPIMSG_DEC_GET_PARAM
+ * @inst_id     : instance ID. Used if the ABI version >= 2.
+ * @data	: picture information
+ * @param_type	: get param type
+ * @codec_type	: Codec fourcc
+ */
+struct vdec_ap_ipi_get_param {
+	u32 msg_id;
+	u32 inst_id;
+	u32 data[4];
+	u32 param_type;
+	u32 codec_type;
+};
+
+/**
+ * struct vdec_vpu_ipi_get_param_ack - for VPU_IPIMSG_DEC_GET_PARAM_ACK
+ * @msg_id	: VPU_IPIMSG_DEC_GET_PARAM_ACK
+ * @status	: VPU execution result
+ * @ap_inst_addr	: AP vcodec_vpu_inst instance address
+ * @data     : picture information from SCP.
+ * @param_type	: get param type
+ * @reserved : reserved param
+ */
+struct vdec_vpu_ipi_get_param_ack {
+	u32 msg_id;
+	s32 status;
+	u64 ap_inst_addr;
+	u32 data[4];
+	u32 param_type;
+	u32 reserved;
+};
+
+#endif
diff --git a/drivers/media/platform/mediatek/vcodec/vdec_msg_queue.c b/drivers/media/platform/mediatek/vcodec/vdec_msg_queue.c
new file mode 100644
index 000000000..a81212c0a
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec_msg_queue.c
@@ -0,0 +1,358 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2021 MediaTek Inc.
+ * Author: Yunfei Dong <yunfei.dong@mediatek.com>
+ */
+
+#include <linux/freezer.h>
+#include <linux/interrupt.h>
+#include <linux/kthread.h>
+
+#include "mtk_vcodec_dec_pm.h"
+#include "mtk_vcodec_drv.h"
+#include "vdec_msg_queue.h"
+
+#define VDEC_MSG_QUEUE_TIMEOUT_MS 1500
+
+/* the size used to store lat slice header information */
+#define VDEC_LAT_SLICE_HEADER_SZ    (640 * SZ_1K)
+
+/* the size used to store avc error information */
+#define VDEC_ERR_MAP_SZ_AVC         (17 * SZ_1K)
+
+/* core will read the trans buffer which decoded by lat to decode again.
+ * The trans buffer size of FHD and 4K bitstreams are different.
+ */
+static int vde_msg_queue_get_trans_size(int width, int height)
+{
+	if (width > 1920 || height > 1088)
+		return 30 * SZ_1M;
+	else
+		return 6 * SZ_1M;
+}
+
+void vdec_msg_queue_init_ctx(struct vdec_msg_queue_ctx *ctx, int hardware_index)
+{
+	init_waitqueue_head(&ctx->ready_to_use);
+	INIT_LIST_HEAD(&ctx->ready_queue);
+	spin_lock_init(&ctx->ready_lock);
+	ctx->ready_num = 0;
+	ctx->hardware_index = hardware_index;
+}
+
+static struct list_head *vdec_get_buf_list(int hardware_index, struct vdec_lat_buf *buf)
+{
+	switch (hardware_index) {
+	case MTK_VDEC_CORE:
+		return &buf->core_list;
+	case MTK_VDEC_LAT0:
+		return &buf->lat_list;
+	default:
+		return NULL;
+	}
+}
+
+static void vdec_msg_queue_inc(struct vdec_msg_queue *msg_queue, int hardware_index)
+{
+	if (hardware_index == MTK_VDEC_CORE)
+		atomic_inc(&msg_queue->core_list_cnt);
+	else
+		atomic_inc(&msg_queue->lat_list_cnt);
+}
+
+static void vdec_msg_queue_dec(struct vdec_msg_queue *msg_queue, int hardware_index)
+{
+	if (hardware_index == MTK_VDEC_CORE)
+		atomic_dec(&msg_queue->core_list_cnt);
+	else
+		atomic_dec(&msg_queue->lat_list_cnt);
+}
+
+int vdec_msg_queue_qbuf(struct vdec_msg_queue_ctx *msg_ctx, struct vdec_lat_buf *buf)
+{
+	struct list_head *head;
+
+	head = vdec_get_buf_list(msg_ctx->hardware_index, buf);
+	if (!head) {
+		mtk_v4l2_err("fail to qbuf: %d", msg_ctx->hardware_index);
+		return -EINVAL;
+	}
+
+	spin_lock(&msg_ctx->ready_lock);
+	list_add_tail(head, &msg_ctx->ready_queue);
+	msg_ctx->ready_num++;
+
+	vdec_msg_queue_inc(&buf->ctx->msg_queue, msg_ctx->hardware_index);
+	if (msg_ctx->hardware_index != MTK_VDEC_CORE) {
+		wake_up_all(&msg_ctx->ready_to_use);
+	} else {
+		if (!(buf->ctx->msg_queue.status & CONTEXT_LIST_QUEUED)) {
+			queue_work(buf->ctx->dev->core_workqueue, &buf->ctx->msg_queue.core_work);
+			buf->ctx->msg_queue.status |= CONTEXT_LIST_QUEUED;
+		}
+	}
+
+	mtk_v4l2_debug(3, "enqueue buf type: %d addr: 0x%p num: %d",
+		       msg_ctx->hardware_index, buf, msg_ctx->ready_num);
+	spin_unlock(&msg_ctx->ready_lock);
+
+	return 0;
+}
+
+static bool vdec_msg_queue_wait_event(struct vdec_msg_queue_ctx *msg_ctx)
+{
+	int ret;
+
+	ret = wait_event_timeout(msg_ctx->ready_to_use,
+				 !list_empty(&msg_ctx->ready_queue),
+				 msecs_to_jiffies(VDEC_MSG_QUEUE_TIMEOUT_MS));
+	if (!ret)
+		return false;
+
+	return true;
+}
+
+struct vdec_lat_buf *vdec_msg_queue_dqbuf(struct vdec_msg_queue_ctx *msg_ctx)
+{
+	struct vdec_lat_buf *buf;
+	struct list_head *head;
+	int ret;
+
+	spin_lock(&msg_ctx->ready_lock);
+	if (list_empty(&msg_ctx->ready_queue)) {
+		mtk_v4l2_debug(3, "queue is NULL, type:%d num: %d",
+			       msg_ctx->hardware_index, msg_ctx->ready_num);
+		spin_unlock(&msg_ctx->ready_lock);
+
+		if (msg_ctx->hardware_index == MTK_VDEC_CORE)
+			return NULL;
+
+		ret = vdec_msg_queue_wait_event(msg_ctx);
+		if (!ret)
+			return NULL;
+		spin_lock(&msg_ctx->ready_lock);
+	}
+
+	if (msg_ctx->hardware_index == MTK_VDEC_CORE)
+		buf = list_first_entry(&msg_ctx->ready_queue,
+				       struct vdec_lat_buf, core_list);
+	else
+		buf = list_first_entry(&msg_ctx->ready_queue,
+				       struct vdec_lat_buf, lat_list);
+
+	head = vdec_get_buf_list(msg_ctx->hardware_index, buf);
+	if (!head) {
+		spin_unlock(&msg_ctx->ready_lock);
+		mtk_v4l2_err("fail to dqbuf: %d", msg_ctx->hardware_index);
+		return NULL;
+	}
+	list_del(head);
+	vdec_msg_queue_dec(&buf->ctx->msg_queue, msg_ctx->hardware_index);
+
+	msg_ctx->ready_num--;
+	mtk_v4l2_debug(3, "dqueue buf type:%d addr: 0x%p num: %d",
+		       msg_ctx->hardware_index, buf, msg_ctx->ready_num);
+	spin_unlock(&msg_ctx->ready_lock);
+
+	return buf;
+}
+
+void vdec_msg_queue_update_ube_rptr(struct vdec_msg_queue *msg_queue, uint64_t ube_rptr)
+{
+	spin_lock(&msg_queue->lat_ctx.ready_lock);
+	msg_queue->wdma_rptr_addr = ube_rptr;
+	mtk_v4l2_debug(3, "update ube rprt (0x%llx)", ube_rptr);
+	spin_unlock(&msg_queue->lat_ctx.ready_lock);
+}
+
+void vdec_msg_queue_update_ube_wptr(struct vdec_msg_queue *msg_queue, uint64_t ube_wptr)
+{
+	spin_lock(&msg_queue->lat_ctx.ready_lock);
+	msg_queue->wdma_wptr_addr = ube_wptr;
+	mtk_v4l2_debug(3, "update ube wprt: (0x%llx 0x%llx) offset: 0x%llx",
+		       msg_queue->wdma_rptr_addr, msg_queue->wdma_wptr_addr,
+		       ube_wptr);
+	spin_unlock(&msg_queue->lat_ctx.ready_lock);
+}
+
+bool vdec_msg_queue_wait_lat_buf_full(struct vdec_msg_queue *msg_queue)
+{
+	struct vdec_lat_buf *buf, *tmp;
+	struct list_head *list_core[3];
+	struct vdec_msg_queue_ctx *core_ctx;
+	int ret, i, in_core_count = 0, count = 0;
+	long timeout_jiff;
+
+	core_ctx = &msg_queue->ctx->dev->msg_queue_core_ctx;
+	spin_lock(&core_ctx->ready_lock);
+	list_for_each_entry_safe(buf, tmp, &core_ctx->ready_queue, core_list) {
+		if (buf && buf->ctx == msg_queue->ctx) {
+			list_core[in_core_count++] = &buf->core_list;
+			list_del(&buf->core_list);
+		}
+	}
+
+	for (i = 0; i < in_core_count; i++) {
+		list_add(list_core[in_core_count - (1 + i)], &core_ctx->ready_queue);
+		queue_work(msg_queue->ctx->dev->core_workqueue, &msg_queue->core_work);
+	}
+	spin_unlock(&core_ctx->ready_lock);
+
+	timeout_jiff = msecs_to_jiffies(1000 * (NUM_BUFFER_COUNT + 2));
+	ret = wait_event_timeout(msg_queue->ctx->msg_queue.core_dec_done,
+				 msg_queue->lat_ctx.ready_num == NUM_BUFFER_COUNT,
+				 timeout_jiff);
+	if (ret) {
+		mtk_v4l2_debug(3, "success to get lat buf: %d",
+			       msg_queue->lat_ctx.ready_num);
+		return true;
+	}
+
+	spin_lock(&core_ctx->ready_lock);
+	list_for_each_entry_safe(buf, tmp, &core_ctx->ready_queue, core_list) {
+		if (buf && buf->ctx == msg_queue->ctx) {
+			count++;
+			list_del(&buf->core_list);
+		}
+	}
+	spin_unlock(&core_ctx->ready_lock);
+
+	mtk_v4l2_err("failed with lat buf isn't full: list(%d %d) count:%d",
+		     atomic_read(&msg_queue->lat_list_cnt),
+		     atomic_read(&msg_queue->core_list_cnt), count);
+
+	return false;
+}
+
+void vdec_msg_queue_deinit(struct vdec_msg_queue *msg_queue,
+			   struct mtk_vcodec_ctx *ctx)
+{
+	struct vdec_lat_buf *lat_buf;
+	struct mtk_vcodec_mem *mem;
+	int i;
+
+	mem = &msg_queue->wdma_addr;
+	if (mem->va)
+		mtk_vcodec_mem_free(ctx, mem);
+	for (i = 0; i < NUM_BUFFER_COUNT; i++) {
+		lat_buf = &msg_queue->lat_buf[i];
+
+		mem = &lat_buf->wdma_err_addr;
+		if (mem->va)
+			mtk_vcodec_mem_free(ctx, mem);
+
+		mem = &lat_buf->slice_bc_addr;
+		if (mem->va)
+			mtk_vcodec_mem_free(ctx, mem);
+
+		kfree(lat_buf->private_data);
+		lat_buf->private_data = NULL;
+	}
+}
+
+static void vdec_msg_queue_core_work(struct work_struct *work)
+{
+	struct vdec_msg_queue *msg_queue =
+		container_of(work, struct vdec_msg_queue, core_work);
+	struct mtk_vcodec_ctx *ctx =
+		container_of(msg_queue, struct mtk_vcodec_ctx, msg_queue);
+	struct mtk_vcodec_dev *dev = ctx->dev;
+	struct vdec_lat_buf *lat_buf;
+
+	spin_lock(&ctx->dev->msg_queue_core_ctx.ready_lock);
+	ctx->msg_queue.status &= ~CONTEXT_LIST_QUEUED;
+	spin_unlock(&ctx->dev->msg_queue_core_ctx.ready_lock);
+
+	lat_buf = vdec_msg_queue_dqbuf(&dev->msg_queue_core_ctx);
+	if (!lat_buf)
+		return;
+
+	ctx = lat_buf->ctx;
+	mtk_vcodec_dec_enable_hardware(ctx, MTK_VDEC_CORE);
+	mtk_vcodec_set_curr_ctx(dev, ctx, MTK_VDEC_CORE);
+
+	lat_buf->core_decode(lat_buf);
+
+	mtk_vcodec_set_curr_ctx(dev, NULL, MTK_VDEC_CORE);
+	mtk_vcodec_dec_disable_hardware(ctx, MTK_VDEC_CORE);
+	vdec_msg_queue_qbuf(&ctx->msg_queue.lat_ctx, lat_buf);
+
+	wake_up_all(&ctx->msg_queue.core_dec_done);
+	if (!(ctx->msg_queue.status & CONTEXT_LIST_QUEUED) &&
+	    atomic_read(&msg_queue->core_list_cnt)) {
+		spin_lock(&ctx->dev->msg_queue_core_ctx.ready_lock);
+		ctx->msg_queue.status |= CONTEXT_LIST_QUEUED;
+		spin_unlock(&ctx->dev->msg_queue_core_ctx.ready_lock);
+		queue_work(ctx->dev->core_workqueue, &msg_queue->core_work);
+	}
+}
+
+int vdec_msg_queue_init(struct vdec_msg_queue *msg_queue,
+			struct mtk_vcodec_ctx *ctx, core_decode_cb_t core_decode,
+			int private_size)
+{
+	struct vdec_lat_buf *lat_buf;
+	int i, err;
+
+	/* already init msg queue */
+	if (msg_queue->wdma_addr.size)
+		return 0;
+
+	msg_queue->ctx = ctx;
+	vdec_msg_queue_init_ctx(&msg_queue->lat_ctx, MTK_VDEC_LAT0);
+	INIT_WORK(&msg_queue->core_work, vdec_msg_queue_core_work);
+
+	atomic_set(&msg_queue->lat_list_cnt, 0);
+	atomic_set(&msg_queue->core_list_cnt, 0);
+	init_waitqueue_head(&msg_queue->core_dec_done);
+	msg_queue->status = CONTEXT_LIST_EMPTY;
+
+	msg_queue->wdma_addr.size =
+		vde_msg_queue_get_trans_size(ctx->picinfo.buf_w,
+					     ctx->picinfo.buf_h);
+	err = mtk_vcodec_mem_alloc(ctx, &msg_queue->wdma_addr);
+	if (err) {
+		mtk_v4l2_err("failed to allocate wdma_addr buf");
+		msg_queue->wdma_addr.size = 0;
+		return -ENOMEM;
+	}
+	msg_queue->wdma_rptr_addr = msg_queue->wdma_addr.dma_addr;
+	msg_queue->wdma_wptr_addr = msg_queue->wdma_addr.dma_addr;
+
+	for (i = 0; i < NUM_BUFFER_COUNT; i++) {
+		lat_buf = &msg_queue->lat_buf[i];
+
+		lat_buf->wdma_err_addr.size = VDEC_ERR_MAP_SZ_AVC;
+		err = mtk_vcodec_mem_alloc(ctx, &lat_buf->wdma_err_addr);
+		if (err) {
+			mtk_v4l2_err("failed to allocate wdma_err_addr buf[%d]", i);
+			goto mem_alloc_err;
+		}
+
+		lat_buf->slice_bc_addr.size = VDEC_LAT_SLICE_HEADER_SZ;
+		err = mtk_vcodec_mem_alloc(ctx, &lat_buf->slice_bc_addr);
+		if (err) {
+			mtk_v4l2_err("failed to allocate wdma_addr buf[%d]", i);
+			goto mem_alloc_err;
+		}
+
+		lat_buf->private_data = kzalloc(private_size, GFP_KERNEL);
+		if (!lat_buf->private_data) {
+			err = -ENOMEM;
+			goto mem_alloc_err;
+		}
+
+		lat_buf->ctx = ctx;
+		lat_buf->core_decode = core_decode;
+		err = vdec_msg_queue_qbuf(&msg_queue->lat_ctx, lat_buf);
+		if (err) {
+			mtk_v4l2_err("failed to qbuf buf[%d]", i);
+			goto mem_alloc_err;
+		}
+	}
+	return 0;
+
+mem_alloc_err:
+	vdec_msg_queue_deinit(msg_queue, ctx);
+	return err;
+}
diff --git a/drivers/media/platform/mediatek/vcodec/vdec_msg_queue.h b/drivers/media/platform/mediatek/vcodec/vdec_msg_queue.h
new file mode 100644
index 000000000..8f82d1484
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec_msg_queue.h
@@ -0,0 +1,179 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2021 MediaTek Inc.
+ * Author: Yunfei Dong <yunfei.dong@mediatek.com>
+ */
+
+#ifndef _VDEC_MSG_QUEUE_H_
+#define _VDEC_MSG_QUEUE_H_
+
+#include <linux/sched.h>
+#include <linux/semaphore.h>
+#include <linux/slab.h>
+#include <media/videobuf2-v4l2.h>
+
+#include "mtk_vcodec_util.h"
+
+#define NUM_BUFFER_COUNT 3
+
+struct vdec_lat_buf;
+struct mtk_vcodec_ctx;
+struct mtk_vcodec_dev;
+typedef int (*core_decode_cb_t)(struct vdec_lat_buf *lat_buf);
+
+/**
+ * enum core_ctx_status - Context decode status for core hardwre.
+ * @CONTEXT_LIST_EMPTY: No buffer queued on core hardware(must always be 0)
+ * @CONTEXT_LIST_QUEUED: Buffer queued to core work list
+ * @CONTEXT_LIST_DEC_DONE: context decode done
+ */
+enum core_ctx_status {
+	CONTEXT_LIST_EMPTY = 0,
+	CONTEXT_LIST_QUEUED,
+	CONTEXT_LIST_DEC_DONE,
+};
+
+/**
+ * struct vdec_msg_queue_ctx - represents a queue for buffers ready to be processed
+ * @ready_to_use: ready used queue used to signalize when get a job queue
+ * @ready_queue: list of ready lat buffer queues
+ * @ready_lock: spin lock to protect the lat buffer usage
+ * @ready_num: number of buffers ready to be processed
+ * @hardware_index: hardware id that this queue is used for
+ */
+struct vdec_msg_queue_ctx {
+	wait_queue_head_t ready_to_use;
+	struct list_head ready_queue;
+	/* protect lat buffer */
+	spinlock_t ready_lock;
+	int ready_num;
+	int hardware_index;
+};
+
+/**
+ * struct vdec_lat_buf - lat buffer message used to store lat info for core decode
+ * @wdma_err_addr: wdma error address used for lat hardware
+ * @slice_bc_addr: slice bc address used for lat hardware
+ * @ts_info: need to set timestamp from output to capture
+ * @src_buf_req: output buffer media request object
+ *
+ * @private_data: shared information used to lat and core hardware
+ * @ctx: mtk vcodec context information
+ * @core_decode: different codec use different decode callback function
+ * @lat_list: add lat buffer to lat head list
+ * @core_list: add lat buffer to core head list
+ */
+struct vdec_lat_buf {
+	struct mtk_vcodec_mem wdma_err_addr;
+	struct mtk_vcodec_mem slice_bc_addr;
+	struct vb2_v4l2_buffer ts_info;
+	struct media_request *src_buf_req;
+
+	void *private_data;
+	struct mtk_vcodec_ctx *ctx;
+	core_decode_cb_t core_decode;
+	struct list_head lat_list;
+	struct list_head core_list;
+};
+
+/**
+ * struct vdec_msg_queue - used to store lat buffer message
+ * @lat_buf: lat buffer used to store lat buffer information
+ * @wdma_addr: wdma address used for ube
+ * @wdma_rptr_addr: ube read point
+ * @wdma_wptr_addr: ube write point
+ * @core_work: core hardware work
+ * @lat_ctx: used to store lat buffer list
+ * @ctx: point to mtk_vcodec_ctx
+ *
+ * @lat_list_cnt: used to record each instance lat list count
+ * @core_list_cnt: used to record each instance core list count
+ * @core_dec_done: core work queue decode done event
+ * @status: current context decode status for core hardware
+ */
+struct vdec_msg_queue {
+	struct vdec_lat_buf lat_buf[NUM_BUFFER_COUNT];
+
+	struct mtk_vcodec_mem wdma_addr;
+	u64 wdma_rptr_addr;
+	u64 wdma_wptr_addr;
+
+	struct work_struct core_work;
+	struct vdec_msg_queue_ctx lat_ctx;
+	struct mtk_vcodec_ctx *ctx;
+
+	atomic_t lat_list_cnt;
+	atomic_t core_list_cnt;
+	wait_queue_head_t core_dec_done;
+	int status;
+};
+
+/**
+ * vdec_msg_queue_init - init lat buffer information.
+ * @msg_queue: used to store the lat buffer information
+ * @ctx: v4l2 ctx
+ * @core_decode: core decode callback for each codec
+ * @private_size: the private data size used to share with core
+ *
+ * Return: returns 0 if init successfully, or fail.
+ */
+int vdec_msg_queue_init(struct vdec_msg_queue *msg_queue,
+			struct mtk_vcodec_ctx *ctx, core_decode_cb_t core_decode,
+			int private_size);
+
+/**
+ * vdec_msg_queue_init_ctx - used to init msg queue context information.
+ * @ctx: message queue context
+ * @hardware_index: hardware index
+ */
+void vdec_msg_queue_init_ctx(struct vdec_msg_queue_ctx *ctx, int hardware_index);
+
+/**
+ * vdec_msg_queue_qbuf - enqueue lat buffer to queue list.
+ * @ctx: message queue context
+ * @buf: current lat buffer
+ *
+ * Return: returns 0 if qbuf successfully, or fail.
+ */
+int vdec_msg_queue_qbuf(struct vdec_msg_queue_ctx *ctx, struct vdec_lat_buf *buf);
+
+/**
+ * vdec_msg_queue_dqbuf - dequeue lat buffer from queue list.
+ * @ctx: message queue context
+ *
+ * Return: returns not null if dq successfully, or fail.
+ */
+struct vdec_lat_buf *vdec_msg_queue_dqbuf(struct vdec_msg_queue_ctx *ctx);
+
+/**
+ * vdec_msg_queue_update_ube_rptr - used to updata the ube read point.
+ * @msg_queue: used to store the lat buffer information
+ * @ube_rptr: current ube read point
+ */
+void vdec_msg_queue_update_ube_rptr(struct vdec_msg_queue *msg_queue, uint64_t ube_rptr);
+
+/**
+ * vdec_msg_queue_update_ube_wptr - used to updata the ube write point.
+ * @msg_queue: used to store the lat buffer information
+ * @ube_wptr: current ube write point
+ */
+void vdec_msg_queue_update_ube_wptr(struct vdec_msg_queue *msg_queue, uint64_t ube_wptr);
+
+/**
+ * vdec_msg_queue_wait_lat_buf_full - used to check whether all lat buffer
+ *                                    in lat list.
+ * @msg_queue: used to store the lat buffer information
+ *
+ * Return: returns true if successfully, or fail.
+ */
+bool vdec_msg_queue_wait_lat_buf_full(struct vdec_msg_queue *msg_queue);
+
+/**
+ * vdec_msg_queue_deinit - deinit lat buffer information.
+ * @msg_queue: used to store the lat buffer information
+ * @ctx: v4l2 ctx
+ */
+void vdec_msg_queue_deinit(struct vdec_msg_queue *msg_queue,
+			   struct mtk_vcodec_ctx *ctx);
+
+#endif
diff --git a/drivers/media/platform/mediatek/vcodec/vdec_vpu_if.c b/drivers/media/platform/mediatek/vcodec/vdec_vpu_if.c
new file mode 100644
index 000000000..df309e8e9
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec_vpu_if.c
@@ -0,0 +1,297 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: PC Chen <pc.chen@mediatek.com>
+ */
+
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_util.h"
+#include "vdec_drv_if.h"
+#include "vdec_ipi_msg.h"
+#include "vdec_vpu_if.h"
+#include "mtk_vcodec_fw.h"
+
+static void handle_init_ack_msg(const struct vdec_vpu_ipi_init_ack *msg)
+{
+	struct vdec_vpu_inst *vpu = (struct vdec_vpu_inst *)
+					(unsigned long)msg->ap_inst_addr;
+
+	mtk_vcodec_debug(vpu, "+ ap_inst_addr = 0x%llx", msg->ap_inst_addr);
+
+	/* mapping VPU address to kernel virtual address */
+	/* the content in vsi is initialized to 0 in VPU */
+	vpu->vsi = mtk_vcodec_fw_map_dm_addr(vpu->ctx->dev->fw_handler,
+					     msg->vpu_inst_addr);
+	vpu->inst_addr = msg->vpu_inst_addr;
+
+	mtk_vcodec_debug(vpu, "- vpu_inst_addr = 0x%x", vpu->inst_addr);
+
+	/* Set default ABI version if dealing with unversioned firmware. */
+	vpu->fw_abi_version = 0;
+	/*
+	 * Instance ID is only used if ABI version >= 2. Initialize it with
+	 * garbage by default.
+	 */
+	vpu->inst_id = 0xdeadbeef;
+
+	/* VPU firmware does not contain a version field. */
+	if (mtk_vcodec_fw_get_type(vpu->ctx->dev->fw_handler) == VPU)
+		return;
+
+	/* Check firmware version. */
+	vpu->fw_abi_version = msg->vdec_abi_version;
+	mtk_vcodec_debug(vpu, "firmware version 0x%x\n", vpu->fw_abi_version);
+	switch (vpu->fw_abi_version) {
+	case 1:
+		break;
+	case 2:
+		vpu->inst_id = msg->inst_id;
+		break;
+	default:
+		mtk_vcodec_err(vpu, "unhandled firmware version 0x%x\n",
+			       vpu->fw_abi_version);
+		vpu->failure = 1;
+		break;
+	}
+}
+
+static void handle_get_param_msg_ack(const struct vdec_vpu_ipi_get_param_ack *msg)
+{
+	struct vdec_vpu_inst *vpu = (struct vdec_vpu_inst *)
+					(unsigned long)msg->ap_inst_addr;
+
+	mtk_vcodec_debug(vpu, "+ ap_inst_addr = 0x%llx", msg->ap_inst_addr);
+
+	/* param_type is enum vdec_get_param_type */
+	switch (msg->param_type) {
+	case GET_PARAM_PIC_INFO:
+		vpu->fb_sz[0] = msg->data[0];
+		vpu->fb_sz[1] = msg->data[1];
+		break;
+	default:
+		mtk_vcodec_err(vpu, "invalid get param type=%d", msg->param_type);
+		vpu->failure = 1;
+		break;
+	}
+}
+
+/*
+ * vpu_dec_ipi_handler - Handler for VPU ipi message.
+ *
+ * @data: ipi message
+ * @len : length of ipi message
+ * @priv: callback private data which is passed by decoder when register.
+ *
+ * This function runs in interrupt context and it means there's an IPI MSG
+ * from VPU.
+ */
+static void vpu_dec_ipi_handler(void *data, unsigned int len, void *priv)
+{
+	const struct vdec_vpu_ipi_ack *msg = data;
+	struct vdec_vpu_inst *vpu = (struct vdec_vpu_inst *)
+					(unsigned long)msg->ap_inst_addr;
+
+	if (!vpu) {
+		mtk_v4l2_err("ap_inst_addr is NULL, did the SCP hang or crash?");
+		return;
+	}
+
+	mtk_vcodec_debug(vpu, "+ id=%X", msg->msg_id);
+
+	vpu->failure = msg->status;
+	vpu->signaled = 1;
+
+	if (msg->status == 0) {
+		switch (msg->msg_id) {
+		case VPU_IPIMSG_DEC_INIT_ACK:
+			handle_init_ack_msg(data);
+			break;
+
+		case VPU_IPIMSG_DEC_START_ACK:
+		case VPU_IPIMSG_DEC_END_ACK:
+		case VPU_IPIMSG_DEC_DEINIT_ACK:
+		case VPU_IPIMSG_DEC_RESET_ACK:
+		case VPU_IPIMSG_DEC_CORE_ACK:
+		case VPU_IPIMSG_DEC_CORE_END_ACK:
+			break;
+
+		case VPU_IPIMSG_DEC_GET_PARAM_ACK:
+			handle_get_param_msg_ack(data);
+			break;
+		default:
+			mtk_vcodec_err(vpu, "invalid msg=%X", msg->msg_id);
+			break;
+		}
+	}
+
+	mtk_vcodec_debug(vpu, "- id=%X", msg->msg_id);
+}
+
+static int vcodec_vpu_send_msg(struct vdec_vpu_inst *vpu, void *msg, int len)
+{
+	int err, id, msgid;
+
+	msgid = *(uint32_t *)msg;
+	mtk_vcodec_debug(vpu, "id=%X", msgid);
+
+	vpu->failure = 0;
+	vpu->signaled = 0;
+
+	if (vpu->ctx->dev->vdec_pdata->hw_arch == MTK_VDEC_LAT_SINGLE_CORE) {
+		if (msgid == AP_IPIMSG_DEC_CORE ||
+		    msgid == AP_IPIMSG_DEC_CORE_END)
+			id = vpu->core_id;
+		else
+			id = vpu->id;
+	} else {
+		id = vpu->id;
+	}
+
+	err = mtk_vcodec_fw_ipi_send(vpu->ctx->dev->fw_handler, id, msg,
+				     len, 2000);
+	if (err) {
+		mtk_vcodec_err(vpu, "send fail vpu_id=%d msg_id=%X status=%d",
+			       id, msgid, err);
+		return err;
+	}
+
+	return vpu->failure;
+}
+
+static int vcodec_send_ap_ipi(struct vdec_vpu_inst *vpu, unsigned int msg_id)
+{
+	struct vdec_ap_ipi_cmd msg;
+	int err = 0;
+
+	mtk_vcodec_debug(vpu, "+ id=%X", msg_id);
+
+	memset(&msg, 0, sizeof(msg));
+	msg.msg_id = msg_id;
+	if (vpu->fw_abi_version < 2)
+		msg.vpu_inst_addr = vpu->inst_addr;
+	else
+		msg.inst_id = vpu->inst_id;
+	msg.codec_type = vpu->codec_type;
+
+	err = vcodec_vpu_send_msg(vpu, &msg, sizeof(msg));
+	mtk_vcodec_debug(vpu, "- id=%X ret=%d", msg_id, err);
+	return err;
+}
+
+int vpu_dec_init(struct vdec_vpu_inst *vpu)
+{
+	struct vdec_ap_ipi_init msg;
+	int err;
+
+	mtk_vcodec_debug_enter(vpu);
+
+	init_waitqueue_head(&vpu->wq);
+	vpu->handler = vpu_dec_ipi_handler;
+
+	err = mtk_vcodec_fw_ipi_register(vpu->ctx->dev->fw_handler, vpu->id,
+					 vpu->handler, "vdec", NULL);
+	if (err) {
+		mtk_vcodec_err(vpu, "vpu_ipi_register fail status=%d", err);
+		return err;
+	}
+
+	if (vpu->ctx->dev->vdec_pdata->hw_arch == MTK_VDEC_LAT_SINGLE_CORE) {
+		err = mtk_vcodec_fw_ipi_register(vpu->ctx->dev->fw_handler,
+						 vpu->core_id, vpu->handler,
+						 "vdec", NULL);
+		if (err) {
+			mtk_vcodec_err(vpu, "vpu_ipi_register core fail status=%d", err);
+			return err;
+		}
+	}
+
+	memset(&msg, 0, sizeof(msg));
+	msg.msg_id = AP_IPIMSG_DEC_INIT;
+	msg.ap_inst_addr = (unsigned long)vpu;
+	msg.codec_type = vpu->codec_type;
+
+	mtk_vcodec_debug(vpu, "vdec_inst=%p", vpu);
+
+	err = vcodec_vpu_send_msg(vpu, (void *)&msg, sizeof(msg));
+	mtk_vcodec_debug(vpu, "- ret=%d", err);
+	return err;
+}
+
+int vpu_dec_start(struct vdec_vpu_inst *vpu, uint32_t *data, unsigned int len)
+{
+	struct vdec_ap_ipi_dec_start msg;
+	int i;
+	int err = 0;
+
+	mtk_vcodec_debug_enter(vpu);
+
+	if (len > ARRAY_SIZE(msg.data)) {
+		mtk_vcodec_err(vpu, "invalid len = %d\n", len);
+		return -EINVAL;
+	}
+
+	memset(&msg, 0, sizeof(msg));
+	msg.msg_id = AP_IPIMSG_DEC_START;
+	if (vpu->fw_abi_version < 2)
+		msg.vpu_inst_addr = vpu->inst_addr;
+	else
+		msg.inst_id = vpu->inst_id;
+
+	for (i = 0; i < len; i++)
+		msg.data[i] = data[i];
+	msg.codec_type = vpu->codec_type;
+
+	err = vcodec_vpu_send_msg(vpu, (void *)&msg, sizeof(msg));
+	mtk_vcodec_debug(vpu, "- ret=%d", err);
+	return err;
+}
+
+int vpu_dec_get_param(struct vdec_vpu_inst *vpu, uint32_t *data,
+		      unsigned int len, unsigned int param_type)
+{
+	struct vdec_ap_ipi_get_param msg;
+	int err;
+
+	mtk_vcodec_debug_enter(vpu);
+
+	if (len > ARRAY_SIZE(msg.data)) {
+		mtk_vcodec_err(vpu, "invalid len = %d\n", len);
+		return -EINVAL;
+	}
+
+	memset(&msg, 0, sizeof(msg));
+	msg.msg_id = AP_IPIMSG_DEC_GET_PARAM;
+	msg.inst_id = vpu->inst_id;
+	memcpy(msg.data, data, sizeof(unsigned int) * len);
+	msg.param_type = param_type;
+	msg.codec_type = vpu->codec_type;
+
+	err = vcodec_vpu_send_msg(vpu, (void *)&msg, sizeof(msg));
+	mtk_vcodec_debug(vpu, "- ret=%d", err);
+	return err;
+}
+
+int vpu_dec_core(struct vdec_vpu_inst *vpu)
+{
+	return vcodec_send_ap_ipi(vpu, AP_IPIMSG_DEC_CORE);
+}
+
+int vpu_dec_end(struct vdec_vpu_inst *vpu)
+{
+	return vcodec_send_ap_ipi(vpu, AP_IPIMSG_DEC_END);
+}
+
+int vpu_dec_core_end(struct vdec_vpu_inst *vpu)
+{
+	return vcodec_send_ap_ipi(vpu, AP_IPIMSG_DEC_CORE_END);
+}
+
+int vpu_dec_deinit(struct vdec_vpu_inst *vpu)
+{
+	return vcodec_send_ap_ipi(vpu, AP_IPIMSG_DEC_DEINIT);
+}
+
+int vpu_dec_reset(struct vdec_vpu_inst *vpu)
+{
+	return vcodec_send_ap_ipi(vpu, AP_IPIMSG_DEC_RESET);
+}
diff --git a/drivers/media/platform/mediatek/vcodec/vdec_vpu_if.h b/drivers/media/platform/mediatek/vcodec/vdec_vpu_if.h
new file mode 100644
index 000000000..0436bba91
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/vdec_vpu_if.h
@@ -0,0 +1,122 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: PC Chen <pc.chen@mediatek.com>
+ */
+
+#ifndef _VDEC_VPU_IF_H_
+#define _VDEC_VPU_IF_H_
+
+#include "mtk_vcodec_fw.h"
+
+struct mtk_vcodec_ctx;
+
+/**
+ * struct vdec_vpu_inst - VPU instance for video codec
+ * @id          : ipi msg id for each decoder
+ * @core_id     : core id used to separate different hardware
+ * @vsi         : driver structure allocated by VPU side and shared to AP side
+ *                for control and info share
+ * @failure     : VPU execution result status, 0: success, others: fail
+ * @inst_addr	: VPU decoder instance address
+ * @fw_abi_version : ABI version of the firmware.
+ * @inst_id	: if fw_abi_version >= 2, contains the instance ID to be given
+ *                in place of inst_addr in messages.
+ * @signaled    : 1 - Host has received ack message from VPU, 0 - not received
+ * @ctx         : context for v4l2 layer integration
+ * @dev		: platform device of VPU
+ * @wq          : wait queue to wait VPU message ack
+ * @handler     : ipi handler for each decoder
+ * @codec_type     : use codec type to separate different codecs
+ * @capture_type:	used capture type to separate different capture format
+ * @fb_sz  : frame buffer size of each plane
+ */
+struct vdec_vpu_inst {
+	int id;
+	int core_id;
+	void *vsi;
+	int32_t failure;
+	uint32_t inst_addr;
+	uint32_t fw_abi_version;
+	uint32_t inst_id;
+	unsigned int signaled;
+	struct mtk_vcodec_ctx *ctx;
+	wait_queue_head_t wq;
+	mtk_vcodec_ipi_handler handler;
+	unsigned int codec_type;
+	unsigned int capture_type;
+	unsigned int fb_sz[2];
+};
+
+/**
+ * vpu_dec_init - init decoder instance and allocate required resource in VPU.
+ *
+ * @vpu: instance for vdec_vpu_inst
+ */
+int vpu_dec_init(struct vdec_vpu_inst *vpu);
+
+/**
+ * vpu_dec_start - start decoding, basically the function will be invoked once
+ *                 every frame.
+ *
+ * @vpu : instance for vdec_vpu_inst
+ * @data: meta data to pass bitstream info to VPU decoder
+ * @len : meta data length
+ */
+int vpu_dec_start(struct vdec_vpu_inst *vpu, uint32_t *data, unsigned int len);
+
+/**
+ * vpu_dec_end - end decoding, basically the function will be invoked once
+ *               when HW decoding done interrupt received successfully. The
+ *               decoder in VPU will continue to do reference frame management
+ *               and check if there is a new decoded frame available to display.
+ *
+ * @vpu : instance for vdec_vpu_inst
+ */
+int vpu_dec_end(struct vdec_vpu_inst *vpu);
+
+/**
+ * vpu_dec_deinit - deinit decoder instance and resource freed in VPU.
+ *
+ * @vpu: instance for vdec_vpu_inst
+ */
+int vpu_dec_deinit(struct vdec_vpu_inst *vpu);
+
+/**
+ * vpu_dec_reset - reset decoder, use for flush decoder when end of stream or
+ *                 seek. Remainig non displayed frame will be pushed to display.
+ *
+ * @vpu: instance for vdec_vpu_inst
+ */
+int vpu_dec_reset(struct vdec_vpu_inst *vpu);
+
+/**
+ * vpu_dec_core - core start decoding, basically the function will be invoked once
+ *                 every frame.
+ *
+ * @vpu : instance for vdec_vpu_inst
+ */
+int vpu_dec_core(struct vdec_vpu_inst *vpu);
+
+/**
+ * vpu_dec_core_end - core end decoding, basically the function will be invoked once
+ *               when core HW decoding done and receive interrupt successfully. The
+ *               decoder in VPU will updata hardware information and deinit hardware
+ *               and check if there is a new decoded frame available to display.
+ *
+ * @vpu : instance for vdec_vpu_inst
+ */
+int vpu_dec_core_end(struct vdec_vpu_inst *vpu);
+
+/**
+ * vpu_dec_get_param - get param from scp
+ *
+ * @vpu : instance for vdec_vpu_inst
+ * @data: meta data to pass bitstream info to VPU decoder
+ * @len : meta data length
+ * @param_type : get param type
+ */
+int vpu_dec_get_param(struct vdec_vpu_inst *vpu, uint32_t *data,
+		      unsigned int len, unsigned int param_type);
+
+#endif
diff --git a/drivers/media/platform/mediatek/vcodec/venc/venc_h264_if.c b/drivers/media/platform/mediatek/vcodec/venc/venc_h264_if.c
new file mode 100644
index 000000000..13c4f860f
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/venc/venc_h264_if.c
@@ -0,0 +1,842 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Jungchang Tsao <jungchang.tsao@mediatek.com>
+ *         Daniel Hsiao <daniel.hsiao@mediatek.com>
+ *         PoChun Lin <pochun.lin@mediatek.com>
+ */
+
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+
+#include "../mtk_vcodec_drv.h"
+#include "../mtk_vcodec_util.h"
+#include "../mtk_vcodec_intr.h"
+#include "../mtk_vcodec_enc.h"
+#include "../mtk_vcodec_enc_pm.h"
+#include "../venc_drv_base.h"
+#include "../venc_ipi_msg.h"
+#include "../venc_vpu_if.h"
+
+static const char h264_filler_marker[] = {0x0, 0x0, 0x0, 0x1, 0xc};
+
+#define H264_FILLER_MARKER_SIZE ARRAY_SIZE(h264_filler_marker)
+#define VENC_PIC_BITSTREAM_BYTE_CNT 0x0098
+
+/*
+ * enum venc_h264_frame_type - h264 encoder output bitstream frame type
+ */
+enum venc_h264_frame_type {
+	VENC_H264_IDR_FRM,
+	VENC_H264_I_FRM,
+	VENC_H264_P_FRM,
+	VENC_H264_B_FRM,
+};
+
+/*
+ * enum venc_h264_vpu_work_buf - h264 encoder buffer index
+ */
+enum venc_h264_vpu_work_buf {
+	VENC_H264_VPU_WORK_BUF_RC_INFO,
+	VENC_H264_VPU_WORK_BUF_RC_CODE,
+	VENC_H264_VPU_WORK_BUF_REC_LUMA,
+	VENC_H264_VPU_WORK_BUF_REC_CHROMA,
+	VENC_H264_VPU_WORK_BUF_REF_LUMA,
+	VENC_H264_VPU_WORK_BUF_REF_CHROMA,
+	VENC_H264_VPU_WORK_BUF_MV_INFO_1,
+	VENC_H264_VPU_WORK_BUF_MV_INFO_2,
+	VENC_H264_VPU_WORK_BUF_SKIP_FRAME,
+	VENC_H264_VPU_WORK_BUF_MAX,
+};
+
+/*
+ * enum venc_h264_bs_mode - for bs_mode argument in h264_enc_vpu_encode
+ */
+enum venc_h264_bs_mode {
+	H264_BS_MODE_SPS,
+	H264_BS_MODE_PPS,
+	H264_BS_MODE_FRAME,
+};
+
+/*
+ * struct venc_h264_vpu_config - Structure for h264 encoder configuration
+ *                               AP-W/R : AP is writer/reader on this item
+ *                               VPU-W/R: VPU is write/reader on this item
+ * @input_fourcc: input fourcc
+ * @bitrate: target bitrate (in bps)
+ * @pic_w: picture width. Picture size is visible stream resolution, in pixels,
+ *         to be used for display purposes; must be smaller or equal to buffer
+ *         size.
+ * @pic_h: picture height
+ * @buf_w: buffer width. Buffer size is stream resolution in pixels aligned to
+ *         hardware requirements.
+ * @buf_h: buffer height
+ * @gop_size: group of picture size (idr frame)
+ * @intra_period: intra frame period
+ * @framerate: frame rate in fps
+ * @profile: as specified in standard
+ * @level: as specified in standard
+ * @wfd: WFD mode 1:on, 0:off
+ */
+struct venc_h264_vpu_config {
+	u32 input_fourcc;
+	u32 bitrate;
+	u32 pic_w;
+	u32 pic_h;
+	u32 buf_w;
+	u32 buf_h;
+	u32 gop_size;
+	u32 intra_period;
+	u32 framerate;
+	u32 profile;
+	u32 level;
+	u32 wfd;
+};
+
+/*
+ * struct venc_h264_vpu_buf - Structure for buffer information
+ *                            AP-W/R : AP is writer/reader on this item
+ *                            VPU-W/R: VPU is write/reader on this item
+ * @iova: IO virtual address
+ * @vpua: VPU side memory addr which is used by RC_CODE
+ * @size: buffer size (in bytes)
+ */
+struct venc_h264_vpu_buf {
+	u32 iova;
+	u32 vpua;
+	u32 size;
+};
+
+/*
+ * struct venc_h264_vsi - Structure for VPU driver control and info share
+ *                        AP-W/R : AP is writer/reader on this item
+ *                        VPU-W/R: VPU is write/reader on this item
+ * This structure is allocated in VPU side and shared to AP side.
+ * @config: h264 encoder configuration
+ * @work_bufs: working buffer information in VPU side
+ * The work_bufs here is for storing the 'size' info shared to AP side.
+ * The similar item in struct venc_h264_inst is for memory allocation
+ * in AP side. The AP driver will copy the 'size' from here to the one in
+ * struct mtk_vcodec_mem, then invoke mtk_vcodec_mem_alloc to allocate
+ * the buffer. After that, bypass the 'dma_addr' to the 'iova' field here for
+ * register setting in VPU side.
+ */
+struct venc_h264_vsi {
+	struct venc_h264_vpu_config config;
+	struct venc_h264_vpu_buf work_bufs[VENC_H264_VPU_WORK_BUF_MAX];
+};
+
+/**
+ * struct venc_h264_vpu_config_ext - Structure for h264 encoder configuration
+ *                                   AP-W/R : AP is writer/reader on this item
+ *                                   VPU-W/R: VPU is write/reader on this item
+ * @input_fourcc: input fourcc
+ * @bitrate: target bitrate (in bps)
+ * @pic_w: picture width. Picture size is visible stream resolution, in pixels,
+ *         to be used for display purposes; must be smaller or equal to buffer
+ *         size.
+ * @pic_h: picture height
+ * @buf_w: buffer width. Buffer size is stream resolution in pixels aligned to
+ *         hardware requirements.
+ * @buf_h: buffer height
+ * @gop_size: group of picture size (idr frame)
+ * @intra_period: intra frame period
+ * @framerate: frame rate in fps
+ * @profile: as specified in standard
+ * @level: as specified in standard
+ * @wfd: WFD mode 1:on, 0:off
+ * @max_qp: max quant parameter
+ * @min_qp: min quant parameter
+ * @reserved: reserved configs
+ */
+struct venc_h264_vpu_config_ext {
+	u32 input_fourcc;
+	u32 bitrate;
+	u32 pic_w;
+	u32 pic_h;
+	u32 buf_w;
+	u32 buf_h;
+	u32 gop_size;
+	u32 intra_period;
+	u32 framerate;
+	u32 profile;
+	u32 level;
+	u32 wfd;
+	u32 max_qp;
+	u32 min_qp;
+	u32 reserved[8];
+};
+
+/**
+ * struct venc_h264_vpu_buf_34 - Structure for 34-bit buffer information
+ *                               AP-W/R : AP is writer/reader on this item
+ *                               VPU-W/R: VPU is write/reader on this item
+ * @iova: 34-bit IO virtual address
+ * @vpua: VPU side memory addr which is used by RC_CODE
+ * @size: buffer size (in bytes)
+ */
+struct venc_h264_vpu_buf_34 {
+	u64 iova;
+	u32 vpua;
+	u32 size;
+};
+
+/**
+ * struct venc_h264_vsi_34 - Structure for VPU driver control and info share
+ *                           Used for 34-bit iova sharing
+ * @config: h264 encoder configuration
+ * @work_bufs: working buffer information in VPU side
+ */
+struct venc_h264_vsi_34 {
+	struct venc_h264_vpu_config_ext config;
+	struct venc_h264_vpu_buf_34 work_bufs[VENC_H264_VPU_WORK_BUF_MAX];
+};
+
+/*
+ * struct venc_h264_inst - h264 encoder AP driver instance
+ * @hw_base: h264 encoder hardware register base
+ * @work_bufs: working buffer
+ * @pps_buf: buffer to store the pps bitstream
+ * @work_buf_allocated: working buffer allocated flag
+ * @frm_cnt: encoded frame count
+ * @prepend_hdr: when the v4l2 layer send VENC_SET_PARAM_PREPEND_HEADER cmd
+ *  through h264_enc_set_param interface, it will set this flag and prepend the
+ *  sps/pps in h264_enc_encode function.
+ * @vpu_inst: VPU instance to exchange information between AP and VPU
+ * @vsi: driver structure allocated by VPU side and shared to AP side for
+ *	 control and info share
+ * @vsi_34: driver structure allocated by VPU side and shared to AP side for
+ *	 control and info share, used for 34-bit iova sharing.
+ * @ctx: context for v4l2 layer integration
+ */
+struct venc_h264_inst {
+	void __iomem *hw_base;
+	struct mtk_vcodec_mem work_bufs[VENC_H264_VPU_WORK_BUF_MAX];
+	struct mtk_vcodec_mem pps_buf;
+	bool work_buf_allocated;
+	unsigned int frm_cnt;
+	unsigned int skip_frm_cnt;
+	unsigned int prepend_hdr;
+	struct venc_vpu_inst vpu_inst;
+	struct venc_h264_vsi *vsi;
+	struct venc_h264_vsi_34 *vsi_34;
+	struct mtk_vcodec_ctx *ctx;
+};
+
+static inline u32 h264_read_reg(struct venc_h264_inst *inst, u32 addr)
+{
+	return readl(inst->hw_base + addr);
+}
+
+static unsigned int h264_get_profile(struct venc_h264_inst *inst,
+				     unsigned int profile)
+{
+	switch (profile) {
+	case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
+		return 66;
+	case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
+		return 77;
+	case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
+		return 100;
+	case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
+		mtk_vcodec_err(inst, "unsupported CONSTRAINED_BASELINE");
+		return 0;
+	case V4L2_MPEG_VIDEO_H264_PROFILE_EXTENDED:
+		mtk_vcodec_err(inst, "unsupported EXTENDED");
+		return 0;
+	default:
+		mtk_vcodec_debug(inst, "unsupported profile %d", profile);
+		return 100;
+	}
+}
+
+static unsigned int h264_get_level(struct venc_h264_inst *inst,
+				   unsigned int level)
+{
+	switch (level) {
+	case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
+		mtk_vcodec_err(inst, "unsupported 1B");
+		return 0;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
+		return 10;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
+		return 11;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
+		return 12;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
+		return 13;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
+		return 20;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
+		return 21;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
+		return 22;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
+		return 30;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
+		return 31;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
+		return 32;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
+		return 40;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_4_1:
+		return 41;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_4_2:
+		return 42;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_5_0:
+		return 50;
+	case V4L2_MPEG_VIDEO_H264_LEVEL_5_1:
+		return 51;
+	default:
+		mtk_vcodec_debug(inst, "unsupported level %d", level);
+		return 31;
+	}
+}
+
+static void h264_enc_free_work_buf(struct venc_h264_inst *inst)
+{
+	int i;
+
+	mtk_vcodec_debug_enter(inst);
+
+	/* Except the SKIP_FRAME buffers,
+	 * other buffers need to be freed by AP.
+	 */
+	for (i = 0; i < VENC_H264_VPU_WORK_BUF_MAX; i++) {
+		if (i != VENC_H264_VPU_WORK_BUF_SKIP_FRAME)
+			mtk_vcodec_mem_free(inst->ctx, &inst->work_bufs[i]);
+	}
+
+	mtk_vcodec_mem_free(inst->ctx, &inst->pps_buf);
+
+	mtk_vcodec_debug_leave(inst);
+}
+
+static int h264_enc_alloc_work_buf(struct venc_h264_inst *inst, bool is_34bit)
+{
+	struct venc_h264_vpu_buf *wb = NULL;
+	struct venc_h264_vpu_buf_34 *wb_34 = NULL;
+	int i;
+	u32 vpua, wb_size;
+	int ret = 0;
+
+	mtk_vcodec_debug_enter(inst);
+
+	if (is_34bit)
+		wb_34 = inst->vsi_34->work_bufs;
+	else
+		wb = inst->vsi->work_bufs;
+
+	for (i = 0; i < VENC_H264_VPU_WORK_BUF_MAX; i++) {
+		/*
+		 * This 'wb' structure is set by VPU side and shared to AP for
+		 * buffer allocation and IO virtual addr mapping. For most of
+		 * the buffers, AP will allocate the buffer according to 'size'
+		 * field and store the IO virtual addr in 'iova' field. There
+		 * are two exceptions:
+		 * (1) RC_CODE buffer, it's pre-allocated in the VPU side, and
+		 * save the VPU addr in the 'vpua' field. The AP will translate
+		 * the VPU addr to the corresponding IO virtual addr and store
+		 * in 'iova' field for reg setting in VPU side.
+		 * (2) SKIP_FRAME buffer, it's pre-allocated in the VPU side,
+		 * and save the VPU addr in the 'vpua' field. The AP will
+		 * translate the VPU addr to the corresponding AP side virtual
+		 * address and do some memcpy access to move to bitstream buffer
+		 * assigned by v4l2 layer.
+		 */
+		if (is_34bit) {
+			inst->work_bufs[i].size = wb_34[i].size;
+			vpua = wb_34[i].vpua;
+			wb_size = wb_34[i].size;
+		} else {
+			inst->work_bufs[i].size = wb[i].size;
+			vpua = wb[i].vpua;
+			wb_size = wb[i].size;
+		}
+
+		if (i == VENC_H264_VPU_WORK_BUF_SKIP_FRAME) {
+			struct mtk_vcodec_fw *handler;
+
+			handler = inst->vpu_inst.ctx->dev->fw_handler;
+			inst->work_bufs[i].va =
+				mtk_vcodec_fw_map_dm_addr(handler, vpua);
+			inst->work_bufs[i].dma_addr = 0;
+		} else {
+			ret = mtk_vcodec_mem_alloc(inst->ctx,
+						   &inst->work_bufs[i]);
+			if (ret) {
+				mtk_vcodec_err(inst,
+					       "cannot allocate buf %d", i);
+				goto err_alloc;
+			}
+			/*
+			 * This RC_CODE is pre-allocated by VPU and saved in VPU
+			 * addr. So we need use memcpy to copy RC_CODE from VPU
+			 * addr into IO virtual addr in 'iova' field for reg
+			 * setting in VPU side.
+			 */
+			if (i == VENC_H264_VPU_WORK_BUF_RC_CODE) {
+				struct mtk_vcodec_fw *handler;
+				void *tmp_va;
+
+				handler = inst->vpu_inst.ctx->dev->fw_handler;
+				tmp_va = mtk_vcodec_fw_map_dm_addr(handler,
+								   vpua);
+				memcpy(inst->work_bufs[i].va, tmp_va, wb_size);
+			}
+		}
+		if (is_34bit)
+			wb_34[i].iova = inst->work_bufs[i].dma_addr;
+		else
+			wb[i].iova = inst->work_bufs[i].dma_addr;
+
+		mtk_vcodec_debug(inst,
+				 "work_buf[%d] va=0x%p iova=%pad size=%zu",
+				 i, inst->work_bufs[i].va,
+				 &inst->work_bufs[i].dma_addr,
+				 inst->work_bufs[i].size);
+	}
+
+	/* the pps_buf is used by AP side only */
+	inst->pps_buf.size = 128;
+	ret = mtk_vcodec_mem_alloc(inst->ctx, &inst->pps_buf);
+	if (ret) {
+		mtk_vcodec_err(inst, "cannot allocate pps_buf");
+		goto err_alloc;
+	}
+
+	mtk_vcodec_debug_leave(inst);
+
+	return ret;
+
+err_alloc:
+	h264_enc_free_work_buf(inst);
+
+	return ret;
+}
+
+static unsigned int h264_enc_wait_venc_done(struct venc_h264_inst *inst)
+{
+	unsigned int irq_status = 0;
+	struct mtk_vcodec_ctx *ctx = (struct mtk_vcodec_ctx *)inst->ctx;
+
+	if (!mtk_vcodec_wait_for_done_ctx(ctx, MTK_INST_IRQ_RECEIVED,
+					  WAIT_INTR_TIMEOUT_MS, 0)) {
+		irq_status = ctx->irq_status;
+		mtk_vcodec_debug(inst, "irq_status %x <-", irq_status);
+	}
+	return irq_status;
+}
+
+static int h264_frame_type(unsigned int frm_cnt, unsigned int gop_size,
+			   unsigned int intra_period)
+{
+	if ((gop_size != 0 && (frm_cnt % gop_size) == 0) ||
+	    (frm_cnt == 0 && gop_size == 0)) {
+		/* IDR frame */
+		return VENC_H264_IDR_FRM;
+	} else if ((intra_period != 0 && (frm_cnt % intra_period) == 0) ||
+		   (frm_cnt == 0 && intra_period == 0)) {
+		/* I frame */
+		return VENC_H264_I_FRM;
+	} else {
+		return VENC_H264_P_FRM;  /* Note: B frames are not supported */
+	}
+}
+
+static int h264_encode_sps(struct venc_h264_inst *inst,
+			   struct mtk_vcodec_mem *bs_buf,
+			   unsigned int *bs_size)
+{
+	int ret = 0;
+	unsigned int irq_status;
+
+	mtk_vcodec_debug_enter(inst);
+
+	ret = vpu_enc_encode(&inst->vpu_inst, H264_BS_MODE_SPS, NULL, bs_buf, NULL);
+	if (ret)
+		return ret;
+
+	irq_status = h264_enc_wait_venc_done(inst);
+	if (irq_status != MTK_VENC_IRQ_STATUS_SPS) {
+		mtk_vcodec_err(inst, "expect irq status %d",
+			       MTK_VENC_IRQ_STATUS_SPS);
+		return -EINVAL;
+	}
+
+	*bs_size = h264_read_reg(inst, VENC_PIC_BITSTREAM_BYTE_CNT);
+	mtk_vcodec_debug(inst, "bs size %d <-", *bs_size);
+
+	return ret;
+}
+
+static int h264_encode_pps(struct venc_h264_inst *inst,
+			   struct mtk_vcodec_mem *bs_buf,
+			   unsigned int *bs_size)
+{
+	int ret = 0;
+	unsigned int irq_status;
+
+	mtk_vcodec_debug_enter(inst);
+
+	ret = vpu_enc_encode(&inst->vpu_inst, H264_BS_MODE_PPS, NULL, bs_buf, NULL);
+	if (ret)
+		return ret;
+
+	irq_status = h264_enc_wait_venc_done(inst);
+	if (irq_status != MTK_VENC_IRQ_STATUS_PPS) {
+		mtk_vcodec_err(inst, "expect irq status %d",
+			       MTK_VENC_IRQ_STATUS_PPS);
+		return -EINVAL;
+	}
+
+	*bs_size = h264_read_reg(inst, VENC_PIC_BITSTREAM_BYTE_CNT);
+	mtk_vcodec_debug(inst, "bs size %d <-", *bs_size);
+
+	return ret;
+}
+
+static int h264_encode_header(struct venc_h264_inst *inst,
+			      struct mtk_vcodec_mem *bs_buf,
+			      unsigned int *bs_size)
+{
+	int ret = 0;
+	unsigned int bs_size_sps;
+	unsigned int bs_size_pps;
+
+	ret = h264_encode_sps(inst, bs_buf, &bs_size_sps);
+	if (ret)
+		return ret;
+
+	ret = h264_encode_pps(inst, &inst->pps_buf, &bs_size_pps);
+	if (ret)
+		return ret;
+
+	memcpy(bs_buf->va + bs_size_sps, inst->pps_buf.va, bs_size_pps);
+	*bs_size = bs_size_sps + bs_size_pps;
+
+	return ret;
+}
+
+static int h264_encode_frame(struct venc_h264_inst *inst,
+			     struct venc_frm_buf *frm_buf,
+			     struct mtk_vcodec_mem *bs_buf,
+			     unsigned int *bs_size)
+{
+	int ret = 0;
+	unsigned int gop_size;
+	unsigned int intra_period;
+	unsigned int irq_status;
+	struct venc_frame_info frame_info;
+	struct mtk_vcodec_ctx *ctx = inst->ctx;
+
+	mtk_vcodec_debug_enter(inst);
+	mtk_vcodec_debug(inst, "frm_cnt = %d\n ", inst->frm_cnt);
+
+	if (MTK_ENC_IOVA_IS_34BIT(ctx)) {
+		gop_size = inst->vsi_34->config.gop_size;
+		intra_period = inst->vsi_34->config.intra_period;
+	} else {
+		gop_size = inst->vsi->config.gop_size;
+		intra_period = inst->vsi->config.intra_period;
+	}
+	frame_info.frm_count = inst->frm_cnt;
+	frame_info.skip_frm_count = inst->skip_frm_cnt;
+	frame_info.frm_type = h264_frame_type(inst->frm_cnt, gop_size,
+					      intra_period);
+	mtk_vcodec_debug(inst, "frm_count = %d,skip_frm_count =%d,frm_type=%d.\n",
+			 frame_info.frm_count, frame_info.skip_frm_count,
+			 frame_info.frm_type);
+
+	ret = vpu_enc_encode(&inst->vpu_inst, H264_BS_MODE_FRAME,
+			     frm_buf, bs_buf, &frame_info);
+	if (ret)
+		return ret;
+
+	/*
+	 * skip frame case: The skip frame buffer is composed by vpu side only,
+	 * it does not trigger the hw, so skip the wait interrupt operation.
+	 */
+	if (inst->vpu_inst.state == VEN_IPI_MSG_ENC_STATE_SKIP) {
+		*bs_size = inst->vpu_inst.bs_size;
+		memcpy(bs_buf->va,
+		       inst->work_bufs[VENC_H264_VPU_WORK_BUF_SKIP_FRAME].va,
+		       *bs_size);
+		++inst->frm_cnt;
+		++inst->skip_frm_cnt;
+		return ret;
+	}
+
+	irq_status = h264_enc_wait_venc_done(inst);
+	if (irq_status != MTK_VENC_IRQ_STATUS_FRM) {
+		mtk_vcodec_err(inst, "irq_status=%d failed", irq_status);
+		return -EIO;
+	}
+
+	*bs_size = h264_read_reg(inst, VENC_PIC_BITSTREAM_BYTE_CNT);
+
+	++inst->frm_cnt;
+	mtk_vcodec_debug(inst, "frm %d bs_size %d key_frm %d <-",
+			 inst->frm_cnt, *bs_size, inst->vpu_inst.is_key_frm);
+
+	return ret;
+}
+
+static void h264_encode_filler(struct venc_h264_inst *inst, void *buf,
+			       int size)
+{
+	unsigned char *p = buf;
+
+	if (size < H264_FILLER_MARKER_SIZE) {
+		mtk_vcodec_err(inst, "filler size too small %d", size);
+		return;
+	}
+
+	memcpy(p, h264_filler_marker, ARRAY_SIZE(h264_filler_marker));
+	size -= H264_FILLER_MARKER_SIZE;
+	p += H264_FILLER_MARKER_SIZE;
+	memset(p, 0xff, size);
+}
+
+static int h264_enc_init(struct mtk_vcodec_ctx *ctx)
+{
+	const bool is_ext = MTK_ENC_CTX_IS_EXT(ctx);
+	int ret = 0;
+	struct venc_h264_inst *inst;
+
+	inst = kzalloc(sizeof(*inst), GFP_KERNEL);
+	if (!inst)
+		return -ENOMEM;
+
+	inst->ctx = ctx;
+	inst->vpu_inst.ctx = ctx;
+	inst->vpu_inst.id = is_ext ? SCP_IPI_VENC_H264 : IPI_VENC_H264;
+	inst->hw_base = mtk_vcodec_get_reg_addr(inst->ctx, VENC_SYS);
+
+	mtk_vcodec_debug_enter(inst);
+
+	ret = vpu_enc_init(&inst->vpu_inst);
+
+	if (MTK_ENC_IOVA_IS_34BIT(ctx))
+		inst->vsi_34 = (struct venc_h264_vsi_34 *)inst->vpu_inst.vsi;
+	else
+		inst->vsi = (struct venc_h264_vsi *)inst->vpu_inst.vsi;
+
+	mtk_vcodec_debug_leave(inst);
+
+	if (ret)
+		kfree(inst);
+	else
+		ctx->drv_handle = inst;
+
+	return ret;
+}
+
+static int h264_enc_encode(void *handle,
+			   enum venc_start_opt opt,
+			   struct venc_frm_buf *frm_buf,
+			   struct mtk_vcodec_mem *bs_buf,
+			   struct venc_done_result *result)
+{
+	int ret = 0;
+	struct venc_h264_inst *inst = (struct venc_h264_inst *)handle;
+	struct mtk_vcodec_ctx *ctx = inst->ctx;
+
+	mtk_vcodec_debug(inst, "opt %d ->", opt);
+
+	enable_irq(ctx->dev->enc_irq);
+
+	switch (opt) {
+	case VENC_START_OPT_ENCODE_SEQUENCE_HEADER: {
+		unsigned int bs_size_hdr;
+
+		ret = h264_encode_header(inst, bs_buf, &bs_size_hdr);
+		if (ret)
+			goto encode_err;
+
+		result->bs_size = bs_size_hdr;
+		result->is_key_frm = false;
+		break;
+	}
+
+	case VENC_START_OPT_ENCODE_FRAME: {
+		int hdr_sz;
+		int hdr_sz_ext;
+		int filler_sz = 0;
+		const int bs_alignment = 128;
+		struct mtk_vcodec_mem tmp_bs_buf;
+		unsigned int bs_size_hdr;
+		unsigned int bs_size_frm;
+
+		if (!inst->prepend_hdr) {
+			ret = h264_encode_frame(inst, frm_buf, bs_buf,
+						&result->bs_size);
+			if (ret)
+				goto encode_err;
+			result->is_key_frm = inst->vpu_inst.is_key_frm;
+			break;
+		}
+
+		mtk_vcodec_debug(inst, "h264_encode_frame prepend SPS/PPS");
+
+		ret = h264_encode_header(inst, bs_buf, &bs_size_hdr);
+		if (ret)
+			goto encode_err;
+
+		hdr_sz = bs_size_hdr;
+		hdr_sz_ext = (hdr_sz & (bs_alignment - 1));
+		if (hdr_sz_ext) {
+			filler_sz = bs_alignment - hdr_sz_ext;
+			if (hdr_sz_ext + H264_FILLER_MARKER_SIZE > bs_alignment)
+				filler_sz += bs_alignment;
+			h264_encode_filler(inst, bs_buf->va + hdr_sz,
+					   filler_sz);
+		}
+
+		tmp_bs_buf.va = bs_buf->va + hdr_sz + filler_sz;
+		tmp_bs_buf.dma_addr = bs_buf->dma_addr + hdr_sz + filler_sz;
+		tmp_bs_buf.size = bs_buf->size - (hdr_sz + filler_sz);
+
+		ret = h264_encode_frame(inst, frm_buf, &tmp_bs_buf,
+					&bs_size_frm);
+		if (ret)
+			goto encode_err;
+
+		result->bs_size = hdr_sz + filler_sz + bs_size_frm;
+
+		mtk_vcodec_debug(inst, "hdr %d filler %d frame %d bs %d",
+				 hdr_sz, filler_sz, bs_size_frm,
+				 result->bs_size);
+
+		inst->prepend_hdr = 0;
+		result->is_key_frm = inst->vpu_inst.is_key_frm;
+		break;
+	}
+
+	default:
+		mtk_vcodec_err(inst, "venc_start_opt %d not supported", opt);
+		ret = -EINVAL;
+		break;
+	}
+
+encode_err:
+
+	disable_irq(ctx->dev->enc_irq);
+	mtk_vcodec_debug(inst, "opt %d <-", opt);
+
+	return ret;
+}
+
+static void h264_enc_set_vsi_configs(struct venc_h264_inst *inst,
+				     struct venc_enc_param *enc_prm)
+{
+	inst->vsi->config.input_fourcc = enc_prm->input_yuv_fmt;
+	inst->vsi->config.bitrate = enc_prm->bitrate;
+	inst->vsi->config.pic_w = enc_prm->width;
+	inst->vsi->config.pic_h = enc_prm->height;
+	inst->vsi->config.buf_w = enc_prm->buf_width;
+	inst->vsi->config.buf_h = enc_prm->buf_height;
+	inst->vsi->config.gop_size = enc_prm->gop_size;
+	inst->vsi->config.framerate = enc_prm->frm_rate;
+	inst->vsi->config.intra_period = enc_prm->intra_period;
+	inst->vsi->config.profile =
+		h264_get_profile(inst, enc_prm->h264_profile);
+	inst->vsi->config.level =
+		h264_get_level(inst, enc_prm->h264_level);
+	inst->vsi->config.wfd = 0;
+}
+
+static void h264_enc_set_vsi_34_configs(struct venc_h264_inst *inst,
+					struct venc_enc_param *enc_prm)
+{
+	inst->vsi_34->config.input_fourcc = enc_prm->input_yuv_fmt;
+	inst->vsi_34->config.bitrate = enc_prm->bitrate;
+	inst->vsi_34->config.pic_w = enc_prm->width;
+	inst->vsi_34->config.pic_h = enc_prm->height;
+	inst->vsi_34->config.buf_w = enc_prm->buf_width;
+	inst->vsi_34->config.buf_h = enc_prm->buf_height;
+	inst->vsi_34->config.gop_size = enc_prm->gop_size;
+	inst->vsi_34->config.framerate = enc_prm->frm_rate;
+	inst->vsi_34->config.intra_period = enc_prm->intra_period;
+	inst->vsi_34->config.profile =
+		h264_get_profile(inst, enc_prm->h264_profile);
+	inst->vsi_34->config.level =
+		h264_get_level(inst, enc_prm->h264_level);
+	inst->vsi_34->config.wfd = 0;
+}
+
+static int h264_enc_set_param(void *handle,
+			      enum venc_set_param_type type,
+			      struct venc_enc_param *enc_prm)
+{
+	int ret = 0;
+	struct venc_h264_inst *inst = (struct venc_h264_inst *)handle;
+	struct mtk_vcodec_ctx *ctx = inst->ctx;
+	const bool is_34bit = MTK_ENC_IOVA_IS_34BIT(ctx);
+
+	mtk_vcodec_debug(inst, "->type=%d", type);
+
+	switch (type) {
+	case VENC_SET_PARAM_ENC:
+		if (is_34bit)
+			h264_enc_set_vsi_34_configs(inst, enc_prm);
+		else
+			h264_enc_set_vsi_configs(inst, enc_prm);
+		ret = vpu_enc_set_param(&inst->vpu_inst, type, enc_prm);
+		if (ret)
+			break;
+		if (inst->work_buf_allocated) {
+			h264_enc_free_work_buf(inst);
+			inst->work_buf_allocated = false;
+		}
+		ret = h264_enc_alloc_work_buf(inst, is_34bit);
+		if (ret)
+			break;
+		inst->work_buf_allocated = true;
+		break;
+
+	case VENC_SET_PARAM_PREPEND_HEADER:
+		inst->prepend_hdr = 1;
+		mtk_vcodec_debug(inst, "set prepend header mode");
+		break;
+	case VENC_SET_PARAM_FORCE_INTRA:
+	case VENC_SET_PARAM_GOP_SIZE:
+	case VENC_SET_PARAM_INTRA_PERIOD:
+		inst->frm_cnt = 0;
+		inst->skip_frm_cnt = 0;
+		fallthrough;
+	default:
+		ret = vpu_enc_set_param(&inst->vpu_inst, type, enc_prm);
+		break;
+	}
+
+	mtk_vcodec_debug_leave(inst);
+
+	return ret;
+}
+
+static int h264_enc_deinit(void *handle)
+{
+	int ret = 0;
+	struct venc_h264_inst *inst = (struct venc_h264_inst *)handle;
+
+	mtk_vcodec_debug_enter(inst);
+
+	ret = vpu_enc_deinit(&inst->vpu_inst);
+
+	if (inst->work_buf_allocated)
+		h264_enc_free_work_buf(inst);
+
+	mtk_vcodec_debug_leave(inst);
+	kfree(inst);
+
+	return ret;
+}
+
+const struct venc_common_if venc_h264_if = {
+	.init = h264_enc_init,
+	.encode = h264_enc_encode,
+	.set_param = h264_enc_set_param,
+	.deinit = h264_enc_deinit,
+};
diff --git a/drivers/media/platform/mediatek/vcodec/venc/venc_vp8_if.c b/drivers/media/platform/mediatek/vcodec/venc/venc_vp8_if.c
new file mode 100644
index 000000000..56ce58f76
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/venc/venc_vp8_if.c
@@ -0,0 +1,468 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Daniel Hsiao <daniel.hsiao@mediatek.com>
+ *         PoChun Lin <pochun.lin@mediatek.com>
+ */
+
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+
+#include "../mtk_vcodec_drv.h"
+#include "../mtk_vcodec_util.h"
+#include "../mtk_vcodec_intr.h"
+#include "../mtk_vcodec_enc.h"
+#include "../mtk_vcodec_enc_pm.h"
+#include "../venc_drv_base.h"
+#include "../venc_ipi_msg.h"
+#include "../venc_vpu_if.h"
+
+#define VENC_BITSTREAM_FRAME_SIZE 0x0098
+#define VENC_BITSTREAM_HEADER_LEN 0x00e8
+
+/* This ac_tag is vp8 frame tag. */
+#define MAX_AC_TAG_SIZE 10
+
+/*
+ * enum venc_vp8_vpu_work_buf - vp8 encoder buffer index
+ */
+enum venc_vp8_vpu_work_buf {
+	VENC_VP8_VPU_WORK_BUF_LUMA,
+	VENC_VP8_VPU_WORK_BUF_LUMA2,
+	VENC_VP8_VPU_WORK_BUF_LUMA3,
+	VENC_VP8_VPU_WORK_BUF_CHROMA,
+	VENC_VP8_VPU_WORK_BUF_CHROMA2,
+	VENC_VP8_VPU_WORK_BUF_CHROMA3,
+	VENC_VP8_VPU_WORK_BUF_MV_INFO,
+	VENC_VP8_VPU_WORK_BUF_BS_HEADER,
+	VENC_VP8_VPU_WORK_BUF_PROB_BUF,
+	VENC_VP8_VPU_WORK_BUF_RC_INFO,
+	VENC_VP8_VPU_WORK_BUF_RC_CODE,
+	VENC_VP8_VPU_WORK_BUF_RC_CODE2,
+	VENC_VP8_VPU_WORK_BUF_RC_CODE3,
+	VENC_VP8_VPU_WORK_BUF_MAX,
+};
+
+/*
+ * struct venc_vp8_vpu_config - Structure for vp8 encoder configuration
+ *                              AP-W/R : AP is writer/reader on this item
+ *                              VPU-W/R: VPU is write/reader on this item
+ * @input_fourcc: input fourcc
+ * @bitrate: target bitrate (in bps)
+ * @pic_w: picture width. Picture size is visible stream resolution, in pixels,
+ *         to be used for display purposes; must be smaller or equal to buffer
+ *         size.
+ * @pic_h: picture height
+ * @buf_w: buffer width (with 16 alignment). Buffer size is stream resolution
+ *         in pixels aligned to hardware requirements.
+ * @buf_h: buffer height (with 16 alignment)
+ * @gop_size: group of picture size (key frame)
+ * @framerate: frame rate in fps
+ * @ts_mode: temporal scalability mode (0: disable, 1: enable)
+ *	     support three temporal layers - 0: 7.5fps 1: 7.5fps 2: 15fps.
+ */
+struct venc_vp8_vpu_config {
+	u32 input_fourcc;
+	u32 bitrate;
+	u32 pic_w;
+	u32 pic_h;
+	u32 buf_w;
+	u32 buf_h;
+	u32 gop_size;
+	u32 framerate;
+	u32 ts_mode;
+};
+
+/*
+ * struct venc_vp8_vpu_buf - Structure for buffer information
+ *                           AP-W/R : AP is writer/reader on this item
+ *                           VPU-W/R: VPU is write/reader on this item
+ * @iova: IO virtual address
+ * @vpua: VPU side memory addr which is used by RC_CODE
+ * @size: buffer size (in bytes)
+ */
+struct venc_vp8_vpu_buf {
+	u32 iova;
+	u32 vpua;
+	u32 size;
+};
+
+/*
+ * struct venc_vp8_vsi - Structure for VPU driver control and info share
+ *                       AP-W/R : AP is writer/reader on this item
+ *                       VPU-W/R: VPU is write/reader on this item
+ * This structure is allocated in VPU side and shared to AP side.
+ * @config: vp8 encoder configuration
+ * @work_bufs: working buffer information in VPU side
+ * The work_bufs here is for storing the 'size' info shared to AP side.
+ * The similar item in struct venc_vp8_inst is for memory allocation
+ * in AP side. The AP driver will copy the 'size' from here to the one in
+ * struct mtk_vcodec_mem, then invoke mtk_vcodec_mem_alloc to allocate
+ * the buffer. After that, bypass the 'dma_addr' to the 'iova' field here for
+ * register setting in VPU side.
+ */
+struct venc_vp8_vsi {
+	struct venc_vp8_vpu_config config;
+	struct venc_vp8_vpu_buf work_bufs[VENC_VP8_VPU_WORK_BUF_MAX];
+};
+
+/*
+ * struct venc_vp8_inst - vp8 encoder AP driver instance
+ * @hw_base: vp8 encoder hardware register base
+ * @work_bufs: working buffer
+ * @work_buf_allocated: working buffer allocated flag
+ * @frm_cnt: encoded frame count, it's used for I-frame judgement and
+ *	     reset when force intra cmd received.
+ * @ts_mode: temporal scalability mode (0: disable, 1: enable)
+ *	     support three temporal layers - 0: 7.5fps 1: 7.5fps 2: 15fps.
+ * @vpu_inst: VPU instance to exchange information between AP and VPU
+ * @vsi: driver structure allocated by VPU side and shared to AP side for
+ *	 control and info share
+ * @ctx: context for v4l2 layer integration
+ */
+struct venc_vp8_inst {
+	void __iomem *hw_base;
+	struct mtk_vcodec_mem work_bufs[VENC_VP8_VPU_WORK_BUF_MAX];
+	bool work_buf_allocated;
+	unsigned int frm_cnt;
+	unsigned int ts_mode;
+	struct venc_vpu_inst vpu_inst;
+	struct venc_vp8_vsi *vsi;
+	struct mtk_vcodec_ctx *ctx;
+};
+
+static inline u32 vp8_enc_read_reg(struct venc_vp8_inst *inst, u32 addr)
+{
+	return readl(inst->hw_base + addr);
+}
+
+static void vp8_enc_free_work_buf(struct venc_vp8_inst *inst)
+{
+	int i;
+
+	mtk_vcodec_debug_enter(inst);
+
+	/* Buffers need to be freed by AP. */
+	for (i = 0; i < VENC_VP8_VPU_WORK_BUF_MAX; i++) {
+		if (inst->work_bufs[i].size == 0)
+			continue;
+		mtk_vcodec_mem_free(inst->ctx, &inst->work_bufs[i]);
+	}
+
+	mtk_vcodec_debug_leave(inst);
+}
+
+static int vp8_enc_alloc_work_buf(struct venc_vp8_inst *inst)
+{
+	int i;
+	int ret = 0;
+	struct venc_vp8_vpu_buf *wb = inst->vsi->work_bufs;
+
+	mtk_vcodec_debug_enter(inst);
+
+	for (i = 0; i < VENC_VP8_VPU_WORK_BUF_MAX; i++) {
+		if (wb[i].size == 0)
+			continue;
+		/*
+		 * This 'wb' structure is set by VPU side and shared to AP for
+		 * buffer allocation and IO virtual addr mapping. For most of
+		 * the buffers, AP will allocate the buffer according to 'size'
+		 * field and store the IO virtual addr in 'iova' field. For the
+		 * RC_CODEx buffers, they are pre-allocated in the VPU side
+		 * because they are inside VPU SRAM, and save the VPU addr in
+		 * the 'vpua' field. The AP will translate the VPU addr to the
+		 * corresponding IO virtual addr and store in 'iova' field.
+		 */
+		inst->work_bufs[i].size = wb[i].size;
+		ret = mtk_vcodec_mem_alloc(inst->ctx, &inst->work_bufs[i]);
+		if (ret) {
+			mtk_vcodec_err(inst,
+				       "cannot alloc work_bufs[%d]", i);
+			goto err_alloc;
+		}
+		/*
+		 * This RC_CODEx is pre-allocated by VPU and saved in VPU addr.
+		 * So we need use memcpy to copy RC_CODEx from VPU addr into IO
+		 * virtual addr in 'iova' field for reg setting in VPU side.
+		 */
+		if (i == VENC_VP8_VPU_WORK_BUF_RC_CODE ||
+		    i == VENC_VP8_VPU_WORK_BUF_RC_CODE2 ||
+		    i == VENC_VP8_VPU_WORK_BUF_RC_CODE3) {
+			struct mtk_vcodec_fw *handler;
+			void *tmp_va;
+
+			handler = inst->vpu_inst.ctx->dev->fw_handler;
+			tmp_va = mtk_vcodec_fw_map_dm_addr(handler,
+							   wb[i].vpua);
+			memcpy(inst->work_bufs[i].va, tmp_va, wb[i].size);
+		}
+		wb[i].iova = inst->work_bufs[i].dma_addr;
+
+		mtk_vcodec_debug(inst,
+				 "work_bufs[%d] va=0x%p,iova=%pad,size=%zu",
+				 i, inst->work_bufs[i].va,
+				 &inst->work_bufs[i].dma_addr,
+				 inst->work_bufs[i].size);
+	}
+
+	mtk_vcodec_debug_leave(inst);
+
+	return ret;
+
+err_alloc:
+	vp8_enc_free_work_buf(inst);
+
+	return ret;
+}
+
+static unsigned int vp8_enc_wait_venc_done(struct venc_vp8_inst *inst)
+{
+	unsigned int irq_status = 0;
+	struct mtk_vcodec_ctx *ctx = (struct mtk_vcodec_ctx *)inst->ctx;
+
+	if (!mtk_vcodec_wait_for_done_ctx(ctx, MTK_INST_IRQ_RECEIVED,
+					  WAIT_INTR_TIMEOUT_MS, 0)) {
+		irq_status = ctx->irq_status;
+		mtk_vcodec_debug(inst, "isr return %x", irq_status);
+	}
+	return irq_status;
+}
+
+/*
+ * Compose ac_tag, bitstream header and bitstream payload into
+ * one bitstream buffer.
+ */
+static int vp8_enc_compose_one_frame(struct venc_vp8_inst *inst,
+				     struct mtk_vcodec_mem *bs_buf,
+				     unsigned int *bs_size)
+{
+	unsigned int not_key;
+	u32 bs_frm_size;
+	u32 bs_hdr_len;
+	unsigned int ac_tag_size;
+	u8 ac_tag[MAX_AC_TAG_SIZE];
+	u32 tag;
+
+	bs_frm_size = vp8_enc_read_reg(inst, VENC_BITSTREAM_FRAME_SIZE);
+	bs_hdr_len = vp8_enc_read_reg(inst, VENC_BITSTREAM_HEADER_LEN);
+
+	/* if a frame is key frame, not_key is 0 */
+	not_key = !inst->vpu_inst.is_key_frm;
+	tag = (bs_hdr_len << 5) | 0x10 | not_key;
+	ac_tag[0] = tag & 0xff;
+	ac_tag[1] = (tag >> 8) & 0xff;
+	ac_tag[2] = (tag >> 16) & 0xff;
+
+	/* key frame */
+	if (not_key == 0) {
+		ac_tag_size = MAX_AC_TAG_SIZE;
+		ac_tag[3] = 0x9d;
+		ac_tag[4] = 0x01;
+		ac_tag[5] = 0x2a;
+		ac_tag[6] = inst->vsi->config.pic_w;
+		ac_tag[7] = inst->vsi->config.pic_w >> 8;
+		ac_tag[8] = inst->vsi->config.pic_h;
+		ac_tag[9] = inst->vsi->config.pic_h >> 8;
+	} else {
+		ac_tag_size = 3;
+	}
+
+	if (bs_buf->size < bs_hdr_len + bs_frm_size + ac_tag_size) {
+		mtk_vcodec_err(inst, "bitstream buf size is too small(%zu)",
+			       bs_buf->size);
+		return -EINVAL;
+	}
+
+	/*
+	* (1) The vp8 bitstream header and body are generated by the HW vp8
+	* encoder separately at the same time. We cannot know the bitstream
+	* header length in advance.
+	* (2) From the vp8 spec, there is no stuffing byte allowed between the
+	* ac tag, bitstream header and bitstream body.
+	*/
+	memmove(bs_buf->va + bs_hdr_len + ac_tag_size,
+		bs_buf->va, bs_frm_size);
+	memcpy(bs_buf->va + ac_tag_size,
+	       inst->work_bufs[VENC_VP8_VPU_WORK_BUF_BS_HEADER].va,
+	       bs_hdr_len);
+	memcpy(bs_buf->va, ac_tag, ac_tag_size);
+	*bs_size = bs_frm_size + bs_hdr_len + ac_tag_size;
+
+	return 0;
+}
+
+static int vp8_enc_encode_frame(struct venc_vp8_inst *inst,
+				struct venc_frm_buf *frm_buf,
+				struct mtk_vcodec_mem *bs_buf,
+				unsigned int *bs_size)
+{
+	int ret = 0;
+	unsigned int irq_status;
+
+	mtk_vcodec_debug(inst, "->frm_cnt=%d", inst->frm_cnt);
+
+	ret = vpu_enc_encode(&inst->vpu_inst, 0, frm_buf, bs_buf, NULL);
+	if (ret)
+		return ret;
+
+	irq_status = vp8_enc_wait_venc_done(inst);
+	if (irq_status != MTK_VENC_IRQ_STATUS_FRM) {
+		mtk_vcodec_err(inst, "irq_status=%d failed", irq_status);
+		return -EIO;
+	}
+
+	if (vp8_enc_compose_one_frame(inst, bs_buf, bs_size)) {
+		mtk_vcodec_err(inst, "vp8_enc_compose_one_frame failed");
+		return -EINVAL;
+	}
+
+	inst->frm_cnt++;
+	mtk_vcodec_debug(inst, "<-size=%d key_frm=%d", *bs_size,
+			 inst->vpu_inst.is_key_frm);
+
+	return ret;
+}
+
+static int vp8_enc_init(struct mtk_vcodec_ctx *ctx)
+{
+	int ret = 0;
+	struct venc_vp8_inst *inst;
+
+	inst = kzalloc(sizeof(*inst), GFP_KERNEL);
+	if (!inst)
+		return -ENOMEM;
+
+	inst->ctx = ctx;
+	inst->vpu_inst.ctx = ctx;
+	inst->vpu_inst.id = IPI_VENC_VP8;
+	inst->hw_base = mtk_vcodec_get_reg_addr(inst->ctx, VENC_LT_SYS);
+
+	mtk_vcodec_debug_enter(inst);
+
+	ret = vpu_enc_init(&inst->vpu_inst);
+
+	inst->vsi = (struct venc_vp8_vsi *)inst->vpu_inst.vsi;
+
+	mtk_vcodec_debug_leave(inst);
+
+	if (ret)
+		kfree(inst);
+	else
+		ctx->drv_handle = inst;
+
+	return ret;
+}
+
+static int vp8_enc_encode(void *handle,
+			  enum venc_start_opt opt,
+			  struct venc_frm_buf *frm_buf,
+			  struct mtk_vcodec_mem *bs_buf,
+			  struct venc_done_result *result)
+{
+	int ret = 0;
+	struct venc_vp8_inst *inst = (struct venc_vp8_inst *)handle;
+	struct mtk_vcodec_ctx *ctx = inst->ctx;
+
+	mtk_vcodec_debug_enter(inst);
+
+	enable_irq(ctx->dev->enc_irq);
+
+	switch (opt) {
+	case VENC_START_OPT_ENCODE_FRAME:
+		ret = vp8_enc_encode_frame(inst, frm_buf, bs_buf,
+					   &result->bs_size);
+		if (ret)
+			goto encode_err;
+		result->is_key_frm = inst->vpu_inst.is_key_frm;
+		break;
+
+	default:
+		mtk_vcodec_err(inst, "opt not support:%d", opt);
+		ret = -EINVAL;
+		break;
+	}
+
+encode_err:
+
+	disable_irq(ctx->dev->enc_irq);
+	mtk_vcodec_debug_leave(inst);
+
+	return ret;
+}
+
+static int vp8_enc_set_param(void *handle,
+			     enum venc_set_param_type type,
+			     struct venc_enc_param *enc_prm)
+{
+	int ret = 0;
+	struct venc_vp8_inst *inst = (struct venc_vp8_inst *)handle;
+
+	mtk_vcodec_debug(inst, "->type=%d", type);
+
+	switch (type) {
+	case VENC_SET_PARAM_ENC:
+		inst->vsi->config.input_fourcc = enc_prm->input_yuv_fmt;
+		inst->vsi->config.bitrate = enc_prm->bitrate;
+		inst->vsi->config.pic_w = enc_prm->width;
+		inst->vsi->config.pic_h = enc_prm->height;
+		inst->vsi->config.buf_w = enc_prm->buf_width;
+		inst->vsi->config.buf_h = enc_prm->buf_height;
+		inst->vsi->config.gop_size = enc_prm->gop_size;
+		inst->vsi->config.framerate = enc_prm->frm_rate;
+		inst->vsi->config.ts_mode = inst->ts_mode;
+		ret = vpu_enc_set_param(&inst->vpu_inst, type, enc_prm);
+		if (ret)
+			break;
+		if (inst->work_buf_allocated) {
+			vp8_enc_free_work_buf(inst);
+			inst->work_buf_allocated = false;
+		}
+		ret = vp8_enc_alloc_work_buf(inst);
+		if (ret)
+			break;
+		inst->work_buf_allocated = true;
+		break;
+
+	/*
+	 * VENC_SET_PARAM_TS_MODE must be called before VENC_SET_PARAM_ENC
+	 */
+	case VENC_SET_PARAM_TS_MODE:
+		inst->ts_mode = 1;
+		mtk_vcodec_debug(inst, "set ts_mode");
+		break;
+
+	default:
+		ret = vpu_enc_set_param(&inst->vpu_inst, type, enc_prm);
+		break;
+	}
+
+	mtk_vcodec_debug_leave(inst);
+
+	return ret;
+}
+
+static int vp8_enc_deinit(void *handle)
+{
+	int ret = 0;
+	struct venc_vp8_inst *inst = (struct venc_vp8_inst *)handle;
+
+	mtk_vcodec_debug_enter(inst);
+
+	ret = vpu_enc_deinit(&inst->vpu_inst);
+
+	if (inst->work_buf_allocated)
+		vp8_enc_free_work_buf(inst);
+
+	mtk_vcodec_debug_leave(inst);
+	kfree(inst);
+
+	return ret;
+}
+
+const struct venc_common_if venc_vp8_if = {
+	.init = vp8_enc_init,
+	.encode = vp8_enc_encode,
+	.set_param = vp8_enc_set_param,
+	.deinit = vp8_enc_deinit,
+};
diff --git a/drivers/media/platform/mediatek/vcodec/venc_drv_base.h b/drivers/media/platform/mediatek/vcodec/venc_drv_base.h
new file mode 100644
index 000000000..3d718411d
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/venc_drv_base.h
@@ -0,0 +1,53 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Daniel Hsiao <daniel.hsiao@mediatek.com>
+ *	Jungchang Tsao <jungchang.tsao@mediatek.com>
+ *	Tiffany Lin <tiffany.lin@mediatek.com>
+ */
+
+#ifndef _VENC_DRV_BASE_
+#define _VENC_DRV_BASE_
+
+#include "mtk_vcodec_drv.h"
+
+#include "venc_drv_if.h"
+
+struct venc_common_if {
+	/**
+	 * (*init)() - initialize driver
+	 * @ctx:	[in] mtk v4l2 context
+	 * @handle: [out] driver handle
+	 */
+	int (*init)(struct mtk_vcodec_ctx *ctx);
+
+	/**
+	 * (*encode)() - trigger encode
+	 * @handle: [in] driver handle
+	 * @opt: [in] encode option
+	 * @frm_buf: [in] frame buffer to store input frame
+	 * @bs_buf: [in] bitstream buffer to store output bitstream
+	 * @result: [out] encode result
+	 */
+	int (*encode)(void *handle, enum venc_start_opt opt,
+		      struct venc_frm_buf *frm_buf,
+		      struct mtk_vcodec_mem *bs_buf,
+		      struct venc_done_result *result);
+
+	/**
+	 * (*set_param)() - set driver's parameter
+	 * @handle: [in] driver handle
+	 * @type: [in] parameter type
+	 * @in: [in] buffer to store the parameter
+	 */
+	int (*set_param)(void *handle, enum venc_set_param_type type,
+			 struct venc_enc_param *in);
+
+	/**
+	 * (*deinit)() - deinitialize driver.
+	 * @handle: [in] driver handle
+	 */
+	int (*deinit)(void *handle);
+};
+
+#endif
diff --git a/drivers/media/platform/mediatek/vcodec/venc_drv_if.c b/drivers/media/platform/mediatek/vcodec/venc_drv_if.c
new file mode 100644
index 000000000..ce0bce811
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/venc_drv_if.c
@@ -0,0 +1,100 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Daniel Hsiao <daniel.hsiao@mediatek.com>
+ *	Jungchang Tsao <jungchang.tsao@mediatek.com>
+ *	Tiffany Lin <tiffany.lin@mediatek.com>
+ */
+
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+
+#include "venc_drv_base.h"
+#include "venc_drv_if.h"
+
+#include "mtk_vcodec_enc.h"
+#include "mtk_vcodec_enc_pm.h"
+
+int venc_if_init(struct mtk_vcodec_ctx *ctx, unsigned int fourcc)
+{
+	int ret = 0;
+
+	switch (fourcc) {
+	case V4L2_PIX_FMT_VP8:
+		ctx->enc_if = &venc_vp8_if;
+		break;
+	case V4L2_PIX_FMT_H264:
+		ctx->enc_if = &venc_h264_if;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	mtk_venc_lock(ctx);
+	mtk_vcodec_enc_clock_on(&ctx->dev->pm);
+	ret = ctx->enc_if->init(ctx);
+	mtk_vcodec_enc_clock_off(&ctx->dev->pm);
+	mtk_venc_unlock(ctx);
+
+	return ret;
+}
+
+int venc_if_set_param(struct mtk_vcodec_ctx *ctx,
+		enum venc_set_param_type type, struct venc_enc_param *in)
+{
+	int ret = 0;
+
+	mtk_venc_lock(ctx);
+	mtk_vcodec_enc_clock_on(&ctx->dev->pm);
+	ret = ctx->enc_if->set_param(ctx->drv_handle, type, in);
+	mtk_vcodec_enc_clock_off(&ctx->dev->pm);
+	mtk_venc_unlock(ctx);
+
+	return ret;
+}
+
+int venc_if_encode(struct mtk_vcodec_ctx *ctx,
+		   enum venc_start_opt opt, struct venc_frm_buf *frm_buf,
+		   struct mtk_vcodec_mem *bs_buf,
+		   struct venc_done_result *result)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	mtk_venc_lock(ctx);
+
+	spin_lock_irqsave(&ctx->dev->irqlock, flags);
+	ctx->dev->curr_ctx = ctx;
+	spin_unlock_irqrestore(&ctx->dev->irqlock, flags);
+
+	mtk_vcodec_enc_clock_on(&ctx->dev->pm);
+	ret = ctx->enc_if->encode(ctx->drv_handle, opt, frm_buf,
+				  bs_buf, result);
+	mtk_vcodec_enc_clock_off(&ctx->dev->pm);
+
+	spin_lock_irqsave(&ctx->dev->irqlock, flags);
+	ctx->dev->curr_ctx = NULL;
+	spin_unlock_irqrestore(&ctx->dev->irqlock, flags);
+
+	mtk_venc_unlock(ctx);
+	return ret;
+}
+
+int venc_if_deinit(struct mtk_vcodec_ctx *ctx)
+{
+	int ret = 0;
+
+	if (!ctx->drv_handle)
+		return 0;
+
+	mtk_venc_lock(ctx);
+	mtk_vcodec_enc_clock_on(&ctx->dev->pm);
+	ret = ctx->enc_if->deinit(ctx->drv_handle);
+	mtk_vcodec_enc_clock_off(&ctx->dev->pm);
+	mtk_venc_unlock(ctx);
+
+	ctx->drv_handle = NULL;
+
+	return ret;
+}
diff --git a/drivers/media/platform/mediatek/vcodec/venc_drv_if.h b/drivers/media/platform/mediatek/vcodec/venc_drv_if.h
new file mode 100644
index 000000000..0b04a1020
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/venc_drv_if.h
@@ -0,0 +1,170 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Daniel Hsiao <daniel.hsiao@mediatek.com>
+ *		Jungchang Tsao <jungchang.tsao@mediatek.com>
+ *		Tiffany Lin <tiffany.lin@mediatek.com>
+ */
+
+#ifndef _VENC_DRV_IF_H_
+#define _VENC_DRV_IF_H_
+
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_util.h"
+
+/*
+ * enum venc_yuv_fmt - The type of input yuv format
+ * (VPU related: If you change the order, you must also update the VPU codes.)
+ * @VENC_YUV_FORMAT_I420: I420 YUV format
+ * @VENC_YUV_FORMAT_YV12: YV12 YUV format
+ * @VENC_YUV_FORMAT_NV12: NV12 YUV format
+ * @VENC_YUV_FORMAT_NV21: NV21 YUV format
+ */
+enum venc_yuv_fmt {
+	VENC_YUV_FORMAT_I420 = 3,
+	VENC_YUV_FORMAT_YV12 = 5,
+	VENC_YUV_FORMAT_NV12 = 6,
+	VENC_YUV_FORMAT_NV21 = 7,
+};
+
+/*
+ * enum venc_start_opt - encode frame option used in venc_if_encode()
+ * @VENC_START_OPT_ENCODE_SEQUENCE_HEADER: encode SPS/PPS for H264
+ * @VENC_START_OPT_ENCODE_FRAME: encode normal frame
+ */
+enum venc_start_opt {
+	VENC_START_OPT_ENCODE_SEQUENCE_HEADER,
+	VENC_START_OPT_ENCODE_FRAME,
+};
+
+/*
+ * enum venc_set_param_type - The type of set parameter used in
+ *						      venc_if_set_param()
+ * (VPU related: If you change the order, you must also update the VPU codes.)
+ * @VENC_SET_PARAM_ENC: set encoder parameters
+ * @VENC_SET_PARAM_FORCE_INTRA: force an intra frame
+ * @VENC_SET_PARAM_ADJUST_BITRATE: adjust bitrate (in bps)
+ * @VENC_SET_PARAM_ADJUST_FRAMERATE: set frame rate
+ * @VENC_SET_PARAM_GOP_SIZE: set IDR interval
+ * @VENC_SET_PARAM_INTRA_PERIOD: set I frame interval
+ * @VENC_SET_PARAM_SKIP_FRAME: set H264 skip one frame
+ * @VENC_SET_PARAM_PREPEND_HEADER: set H264 prepend SPS/PPS before IDR
+ * @VENC_SET_PARAM_TS_MODE: set VP8 temporal scalability mode
+ */
+enum venc_set_param_type {
+	VENC_SET_PARAM_ENC,
+	VENC_SET_PARAM_FORCE_INTRA,
+	VENC_SET_PARAM_ADJUST_BITRATE,
+	VENC_SET_PARAM_ADJUST_FRAMERATE,
+	VENC_SET_PARAM_GOP_SIZE,
+	VENC_SET_PARAM_INTRA_PERIOD,
+	VENC_SET_PARAM_SKIP_FRAME,
+	VENC_SET_PARAM_PREPEND_HEADER,
+	VENC_SET_PARAM_TS_MODE,
+};
+
+/*
+ * struct venc_enc_prm - encoder settings for VENC_SET_PARAM_ENC used in
+ *					  venc_if_set_param()
+ * @input_fourcc: input yuv format
+ * @h264_profile: V4L2 defined H.264 profile
+ * @h264_level: V4L2 defined H.264 level
+ * @width: image width
+ * @height: image height
+ * @buf_width: buffer width
+ * @buf_height: buffer height
+ * @frm_rate: frame rate in fps
+ * @intra_period: intra frame period
+ * @bitrate: target bitrate in bps
+ * @gop_size: group of picture size
+ */
+struct venc_enc_param {
+	enum venc_yuv_fmt input_yuv_fmt;
+	unsigned int h264_profile;
+	unsigned int h264_level;
+	unsigned int width;
+	unsigned int height;
+	unsigned int buf_width;
+	unsigned int buf_height;
+	unsigned int frm_rate;
+	unsigned int intra_period;
+	unsigned int bitrate;
+	unsigned int gop_size;
+};
+
+/**
+ * struct venc_frame_info - per-frame information to pass to the firmware.
+ *
+ * @frm_count:		sequential number for this frame
+ * @skip_frm_count:	number of frames skipped so far while decoding
+ * @frm_type:		type of the frame, from enum venc_h264_frame_type
+ */
+struct venc_frame_info {
+	unsigned int frm_count;		/* per frame update */
+	unsigned int skip_frm_count;	/* per frame update */
+	unsigned int frm_type;		/* per frame update */
+};
+
+/*
+ * struct venc_frm_buf - frame buffer information used in venc_if_encode()
+ * @fb_addr: plane frame buffer addresses
+ */
+struct venc_frm_buf {
+	struct mtk_vcodec_fb fb_addr[MTK_VCODEC_MAX_PLANES];
+};
+
+/*
+ * struct venc_done_result - This is return information used in venc_if_encode()
+ * @bs_size: output bitstream size
+ * @is_key_frm: output is key frame or not
+ */
+struct venc_done_result {
+	unsigned int bs_size;
+	bool is_key_frm;
+};
+
+extern const struct venc_common_if venc_h264_if;
+extern const struct venc_common_if venc_vp8_if;
+
+/*
+ * venc_if_init - Create the driver handle
+ * @ctx: device context
+ * @fourcc: encoder input format
+ * Return: 0 if creating handle successfully, otherwise it is failed.
+ */
+int venc_if_init(struct mtk_vcodec_ctx *ctx, unsigned int fourcc);
+
+/*
+ * venc_if_deinit - Release the driver handle
+ * @ctx: device context
+ * Return: 0 if releasing handle successfully, otherwise it is failed.
+ */
+int venc_if_deinit(struct mtk_vcodec_ctx *ctx);
+
+/*
+ * venc_if_set_param - Set parameter to driver
+ * @ctx: device context
+ * @type: parameter type
+ * @in: input parameter
+ * Return: 0 if setting param successfully, otherwise it is failed.
+ */
+int venc_if_set_param(struct mtk_vcodec_ctx *ctx,
+		      enum venc_set_param_type type,
+		      struct venc_enc_param *in);
+
+/*
+ * venc_if_encode - Encode one frame
+ * @ctx: device context
+ * @opt: encode frame option
+ * @frm_buf: input frame buffer information
+ * @bs_buf: output bitstream buffer infomraiton
+ * @result: encode result
+ * Return: 0 if encoding frame successfully, otherwise it is failed.
+ */
+int venc_if_encode(struct mtk_vcodec_ctx *ctx,
+		   enum venc_start_opt opt,
+		   struct venc_frm_buf *frm_buf,
+		   struct mtk_vcodec_mem *bs_buf,
+		   struct venc_done_result *result);
+
+#endif /* _VENC_DRV_IF_H_ */
diff --git a/drivers/media/platform/mediatek/vcodec/venc_ipi_msg.h b/drivers/media/platform/mediatek/vcodec/venc_ipi_msg.h
new file mode 100644
index 000000000..bb16d96a7
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/venc_ipi_msg.h
@@ -0,0 +1,244 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: Jungchang Tsao <jungchang.tsao@mediatek.com>
+ *	   Daniel Hsiao <daniel.hsiao@mediatek.com>
+ *	   Tiffany Lin <tiffany.lin@mediatek.com>
+ */
+
+#ifndef _VENC_IPI_MSG_H_
+#define _VENC_IPI_MSG_H_
+
+#define AP_IPIMSG_VENC_BASE 0xC000
+#define VPU_IPIMSG_VENC_BASE 0xD000
+
+/*
+ * enum venc_ipi_msg_id - message id between AP and VPU
+ * (ipi stands for inter-processor interrupt)
+ * @AP_IPIMSG_ENC_XXX:		AP to VPU cmd message id
+ * @VPU_IPIMSG_ENC_XXX_DONE:	VPU ack AP cmd message id
+ */
+enum venc_ipi_msg_id {
+	AP_IPIMSG_ENC_INIT = AP_IPIMSG_VENC_BASE,
+	AP_IPIMSG_ENC_SET_PARAM,
+	AP_IPIMSG_ENC_ENCODE,
+	AP_IPIMSG_ENC_DEINIT,
+
+	VPU_IPIMSG_ENC_INIT_DONE = VPU_IPIMSG_VENC_BASE,
+	VPU_IPIMSG_ENC_SET_PARAM_DONE,
+	VPU_IPIMSG_ENC_ENCODE_DONE,
+	VPU_IPIMSG_ENC_DEINIT_DONE,
+};
+
+/**
+ * struct venc_ap_ipi_msg_init - AP to VPU init cmd structure
+ * @msg_id:	message id (AP_IPIMSG_XXX_ENC_INIT)
+ * @reserved:	reserved for future use. vpu is running in 32bit. Without
+ *		this reserved field, if kernel run in 64bit. this struct size
+ *		will be different between kernel and vpu
+ * @venc_inst:	AP encoder instance
+ *		(struct venc_vp8_inst/venc_h264_inst *)
+ */
+struct venc_ap_ipi_msg_init {
+	uint32_t msg_id;
+	uint32_t reserved;
+	uint64_t venc_inst;
+};
+
+/**
+ * struct venc_ap_ipi_msg_set_param - AP to VPU set_param cmd structure
+ * @msg_id:	message id (AP_IPIMSG_XXX_ENC_SET_PARAM)
+ * @vpu_inst_addr:	VPU encoder instance addr
+ *			(struct venc_vp8_vsi/venc_h264_vsi *)
+ * @param_id:	parameter id (venc_set_param_type)
+ * @data_item:	number of items in the data array
+ * @data:	data array to store the set parameters
+ */
+struct venc_ap_ipi_msg_set_param {
+	uint32_t msg_id;
+	uint32_t vpu_inst_addr;
+	uint32_t param_id;
+	uint32_t data_item;
+	uint32_t data[8];
+};
+
+struct venc_ap_ipi_msg_set_param_ext {
+	struct venc_ap_ipi_msg_set_param base;
+	uint32_t data_ext[24];
+};
+
+/**
+ * struct venc_ap_ipi_msg_enc - AP to VPU enc cmd structure
+ * @msg_id:	message id (AP_IPIMSG_XXX_ENC_ENCODE)
+ * @vpu_inst_addr:	VPU encoder instance addr
+ *			(struct venc_vp8_vsi/venc_h264_vsi *)
+ * @bs_mode:	bitstream mode for h264
+ *		(H264_BS_MODE_SPS/H264_BS_MODE_PPS/H264_BS_MODE_FRAME)
+ * @input_addr:	pointer to input image buffer plane
+ * @bs_addr:	pointer to output bit stream buffer
+ * @bs_size:	bit stream buffer size
+ */
+struct venc_ap_ipi_msg_enc {
+	uint32_t msg_id;
+	uint32_t vpu_inst_addr;
+	uint32_t bs_mode;
+	uint32_t input_addr[3];
+	uint32_t bs_addr;
+	uint32_t bs_size;
+};
+
+/**
+ * struct venc_ap_ipi_msg_enc_ext - AP to SCP extended enc cmd structure
+ *
+ * @base:	base msg structure
+ * @data_item:	number of items in the data array
+ * @data:	data array to store the set parameters
+ */
+struct venc_ap_ipi_msg_enc_ext {
+	struct venc_ap_ipi_msg_enc base;
+	uint32_t data_item;
+	uint32_t data[32];
+};
+
+/**
+ * struct venc_ap_ipi_msg_enc_ext_34 - AP to SCP extended enc cmd structure
+ * @msg_id:		message id (AP_IPIMSG_XXX_ENC_ENCODE)
+ * @vpu_inst_addr:	VPU encoder instance addr
+ * @bs_mode:		bitstream mode for h264
+ * @reserved:		for struct padding
+ * @input_addr:		input frame buffer 34 bit address
+ * @bs_addr:		output bitstream buffer 34 bit address
+ * @bs_size:		bitstream buffer size
+ * @data_item:		number of items in the data array
+ * @data:		data array to store the set parameters
+ */
+struct venc_ap_ipi_msg_enc_ext_34 {
+	u32 msg_id;
+	u32 vpu_inst_addr;
+	u32 bs_mode;
+	u32 reserved;
+	u64 input_addr[3];
+	u64 bs_addr;
+	u32 bs_size;
+	u32 data_item;
+	u32 data[32];
+};
+
+/**
+ * struct venc_ap_ipi_msg_deinit - AP to VPU deinit cmd structure
+ * @msg_id:	message id (AP_IPIMSG_XXX_ENC_DEINIT)
+ * @vpu_inst_addr:	VPU encoder instance addr
+ *			(struct venc_vp8_vsi/venc_h264_vsi *)
+ */
+struct venc_ap_ipi_msg_deinit {
+	uint32_t msg_id;
+	uint32_t vpu_inst_addr;
+};
+
+/*
+ * enum venc_ipi_msg_status - VPU ack AP cmd status
+ */
+enum venc_ipi_msg_status {
+	VENC_IPI_MSG_STATUS_OK,
+	VENC_IPI_MSG_STATUS_FAIL,
+};
+
+/**
+ * struct venc_vpu_ipi_msg_common - VPU ack AP cmd common structure
+ * @msg_id:	message id (VPU_IPIMSG_XXX_DONE)
+ * @status:	cmd status (venc_ipi_msg_status)
+ * @venc_inst:	AP encoder instance (struct venc_vp8_inst/venc_h264_inst *)
+ */
+struct venc_vpu_ipi_msg_common {
+	uint32_t msg_id;
+	uint32_t status;
+	uint64_t venc_inst;
+};
+
+/**
+ * struct venc_vpu_ipi_msg_init - VPU ack AP init cmd structure
+ * @msg_id:	message id (VPU_IPIMSG_XXX_ENC_SET_PARAM_DONE)
+ * @status:	cmd status (venc_ipi_msg_status)
+ * @venc_inst:	AP encoder instance (struct venc_vp8_inst/venc_h264_inst *)
+ * @vpu_inst_addr:	VPU encoder instance addr
+ *			(struct venc_vp8_vsi/venc_h264_vsi *)
+ * @venc_abi_version:	ABI version of the firmware. Kernel can use it to
+ *			ensure that it is compatible with the firmware.
+ *			For MT8173 the value of this field is undefined and
+ *			should not be used.
+ */
+struct venc_vpu_ipi_msg_init {
+	uint32_t msg_id;
+	uint32_t status;
+	uint64_t venc_inst;
+	uint32_t vpu_inst_addr;
+	uint32_t venc_abi_version;
+};
+
+/**
+ * struct venc_vpu_ipi_msg_set_param - VPU ack AP set_param cmd structure
+ * @msg_id:	message id (VPU_IPIMSG_XXX_ENC_SET_PARAM_DONE)
+ * @status:	cmd status (venc_ipi_msg_status)
+ * @venc_inst:	AP encoder instance (struct venc_vp8_inst/venc_h264_inst *)
+ * @param_id:	parameter id (venc_set_param_type)
+ * @data_item:	number of items in the data array
+ * @data:	data array to store the return result
+ */
+struct venc_vpu_ipi_msg_set_param {
+	uint32_t msg_id;
+	uint32_t status;
+	uint64_t venc_inst;
+	uint32_t param_id;
+	uint32_t data_item;
+	uint32_t data[6];
+};
+
+/**
+ * enum venc_ipi_msg_enc_state - Type of encode state
+ * @VEN_IPI_MSG_ENC_STATE_FRAME:	one frame being encoded
+ * @VEN_IPI_MSG_ENC_STATE_PART:		bit stream buffer full
+ * @VEN_IPI_MSG_ENC_STATE_SKIP:		encoded skip frame
+ * @VEN_IPI_MSG_ENC_STATE_ERROR:	encounter error
+ */
+enum venc_ipi_msg_enc_state {
+	VEN_IPI_MSG_ENC_STATE_FRAME,
+	VEN_IPI_MSG_ENC_STATE_PART,
+	VEN_IPI_MSG_ENC_STATE_SKIP,
+	VEN_IPI_MSG_ENC_STATE_ERROR,
+};
+
+/**
+ * struct venc_vpu_ipi_msg_enc - VPU ack AP enc cmd structure
+ * @msg_id:	message id (VPU_IPIMSG_XXX_ENC_ENCODE_DONE)
+ * @status:	cmd status (venc_ipi_msg_status)
+ * @venc_inst:	AP encoder instance (struct venc_vp8_inst/venc_h264_inst *)
+ * @state:	encode state (venc_ipi_msg_enc_state)
+ * @is_key_frm:	whether the encoded frame is key frame
+ * @bs_size:	encoded bitstream size
+ * @reserved:	reserved for future use. vpu is running in 32bit. Without
+ *		this reserved field, if kernel run in 64bit. this struct size
+ *		will be different between kernel and vpu
+ */
+struct venc_vpu_ipi_msg_enc {
+	uint32_t msg_id;
+	uint32_t status;
+	uint64_t venc_inst;
+	uint32_t state;
+	uint32_t is_key_frm;
+	uint32_t bs_size;
+	uint32_t reserved;
+};
+
+/**
+ * struct venc_vpu_ipi_msg_deinit - VPU ack AP deinit cmd structure
+ * @msg_id:   message id (VPU_IPIMSG_XXX_ENC_DEINIT_DONE)
+ * @status:   cmd status (venc_ipi_msg_status)
+ * @venc_inst:	AP encoder instance (struct venc_vp8_inst/venc_h264_inst *)
+ */
+struct venc_vpu_ipi_msg_deinit {
+	uint32_t msg_id;
+	uint32_t status;
+	uint64_t venc_inst;
+};
+
+#endif /* _VENC_IPI_MSG_H_ */
diff --git a/drivers/media/platform/mediatek/vcodec/venc_vpu_if.c b/drivers/media/platform/mediatek/vcodec/venc_vpu_if.c
new file mode 100644
index 000000000..09e7eaa25
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/venc_vpu_if.c
@@ -0,0 +1,361 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: PoChun Lin <pochun.lin@mediatek.com>
+ */
+
+#include "mtk_vcodec_drv.h"
+#include "mtk_vcodec_fw.h"
+#include "venc_ipi_msg.h"
+#include "venc_vpu_if.h"
+
+static void handle_enc_init_msg(struct venc_vpu_inst *vpu, const void *data)
+{
+	const struct venc_vpu_ipi_msg_init *msg = data;
+
+	vpu->inst_addr = msg->vpu_inst_addr;
+	vpu->vsi = mtk_vcodec_fw_map_dm_addr(vpu->ctx->dev->fw_handler,
+					     msg->vpu_inst_addr);
+
+	/* Firmware version field value is unspecified on MT8173. */
+	if (mtk_vcodec_fw_get_type(vpu->ctx->dev->fw_handler) == VPU)
+		return;
+
+	/* Check firmware version. */
+	mtk_vcodec_debug(vpu, "firmware version: 0x%x\n",
+			 msg->venc_abi_version);
+	switch (msg->venc_abi_version) {
+	case 1:
+		break;
+	default:
+		mtk_vcodec_err(vpu, "unhandled firmware version 0x%x\n",
+			       msg->venc_abi_version);
+		vpu->failure = 1;
+		break;
+	}
+}
+
+static void handle_enc_encode_msg(struct venc_vpu_inst *vpu, const void *data)
+{
+	const struct venc_vpu_ipi_msg_enc *msg = data;
+
+	vpu->state = msg->state;
+	vpu->bs_size = msg->bs_size;
+	vpu->is_key_frm = msg->is_key_frm;
+}
+
+static void vpu_enc_ipi_handler(void *data, unsigned int len, void *priv)
+{
+	const struct venc_vpu_ipi_msg_common *msg = data;
+	struct venc_vpu_inst *vpu =
+		(struct venc_vpu_inst *)(unsigned long)msg->venc_inst;
+
+	mtk_vcodec_debug(vpu, "msg_id %x inst %p status %d",
+			 msg->msg_id, vpu, msg->status);
+
+	vpu->signaled = 1;
+	vpu->failure = (msg->status != VENC_IPI_MSG_STATUS_OK);
+	if (vpu->failure)
+		goto failure;
+
+	switch (msg->msg_id) {
+	case VPU_IPIMSG_ENC_INIT_DONE:
+		handle_enc_init_msg(vpu, data);
+		break;
+	case VPU_IPIMSG_ENC_SET_PARAM_DONE:
+		break;
+	case VPU_IPIMSG_ENC_ENCODE_DONE:
+		handle_enc_encode_msg(vpu, data);
+		break;
+	case VPU_IPIMSG_ENC_DEINIT_DONE:
+		break;
+	default:
+		mtk_vcodec_err(vpu, "unknown msg id %x", msg->msg_id);
+		break;
+	}
+
+failure:
+	mtk_vcodec_debug_leave(vpu);
+}
+
+static int vpu_enc_send_msg(struct venc_vpu_inst *vpu, void *msg,
+			    int len)
+{
+	int status;
+
+	mtk_vcodec_debug_enter(vpu);
+
+	if (!vpu->ctx->dev->fw_handler) {
+		mtk_vcodec_err(vpu, "inst dev is NULL");
+		return -EINVAL;
+	}
+
+	status = mtk_vcodec_fw_ipi_send(vpu->ctx->dev->fw_handler, vpu->id, msg,
+					len, 2000);
+	if (status) {
+		mtk_vcodec_err(vpu, "vpu_ipi_send msg_id %x len %d fail %d",
+			       *(uint32_t *)msg, len, status);
+		return -EINVAL;
+	}
+	if (vpu->failure)
+		return -EINVAL;
+
+	mtk_vcodec_debug_leave(vpu);
+
+	return 0;
+}
+
+int vpu_enc_init(struct venc_vpu_inst *vpu)
+{
+	int status;
+	struct venc_ap_ipi_msg_init out;
+
+	mtk_vcodec_debug_enter(vpu);
+
+	init_waitqueue_head(&vpu->wq_hd);
+	vpu->signaled = 0;
+	vpu->failure = 0;
+
+	status = mtk_vcodec_fw_ipi_register(vpu->ctx->dev->fw_handler, vpu->id,
+					    vpu_enc_ipi_handler, "venc", NULL);
+
+	if (status) {
+		mtk_vcodec_err(vpu, "vpu_ipi_register fail %d", status);
+		return -EINVAL;
+	}
+
+	memset(&out, 0, sizeof(out));
+	out.msg_id = AP_IPIMSG_ENC_INIT;
+	out.venc_inst = (unsigned long)vpu;
+	if (vpu_enc_send_msg(vpu, &out, sizeof(out))) {
+		mtk_vcodec_err(vpu, "AP_IPIMSG_ENC_INIT fail");
+		return -EINVAL;
+	}
+
+	mtk_vcodec_debug_leave(vpu);
+
+	return 0;
+}
+
+static unsigned int venc_enc_param_crop_right(struct venc_vpu_inst *vpu,
+					      struct venc_enc_param *enc_prm)
+{
+	unsigned int img_crop_right = enc_prm->buf_width - enc_prm->width;
+
+	return img_crop_right % 16;
+}
+
+static unsigned int venc_enc_param_crop_bottom(struct venc_enc_param *enc_prm)
+{
+	return round_up(enc_prm->height, 16) - enc_prm->height;
+}
+
+static unsigned int venc_enc_param_num_mb(struct venc_enc_param *enc_prm)
+{
+	return DIV_ROUND_UP(enc_prm->width, 16) *
+	       DIV_ROUND_UP(enc_prm->height, 16);
+}
+
+int vpu_enc_set_param(struct venc_vpu_inst *vpu,
+		      enum venc_set_param_type id,
+		      struct venc_enc_param *enc_param)
+{
+	const bool is_ext = MTK_ENC_CTX_IS_EXT(vpu->ctx);
+	size_t msg_size = is_ext ?
+		sizeof(struct venc_ap_ipi_msg_set_param_ext) :
+		sizeof(struct venc_ap_ipi_msg_set_param);
+	struct venc_ap_ipi_msg_set_param_ext out;
+
+	mtk_vcodec_debug(vpu, "id %d ->", id);
+
+	memset(&out, 0, sizeof(out));
+	out.base.msg_id = AP_IPIMSG_ENC_SET_PARAM;
+	out.base.vpu_inst_addr = vpu->inst_addr;
+	out.base.param_id = id;
+	switch (id) {
+	case VENC_SET_PARAM_ENC:
+		if (is_ext) {
+			out.base.data_item = 3;
+			out.base.data[0] =
+				venc_enc_param_crop_right(vpu, enc_param);
+			out.base.data[1] =
+				venc_enc_param_crop_bottom(enc_param);
+			out.base.data[2] = venc_enc_param_num_mb(enc_param);
+		} else {
+			out.base.data_item = 0;
+		}
+		break;
+	case VENC_SET_PARAM_FORCE_INTRA:
+		out.base.data_item = 0;
+		break;
+	case VENC_SET_PARAM_ADJUST_BITRATE:
+		out.base.data_item = 1;
+		out.base.data[0] = enc_param->bitrate;
+		break;
+	case VENC_SET_PARAM_ADJUST_FRAMERATE:
+		out.base.data_item = 1;
+		out.base.data[0] = enc_param->frm_rate;
+		break;
+	case VENC_SET_PARAM_GOP_SIZE:
+		out.base.data_item = 1;
+		out.base.data[0] = enc_param->gop_size;
+		break;
+	case VENC_SET_PARAM_INTRA_PERIOD:
+		out.base.data_item = 1;
+		out.base.data[0] = enc_param->intra_period;
+		break;
+	case VENC_SET_PARAM_SKIP_FRAME:
+		out.base.data_item = 0;
+		break;
+	default:
+		mtk_vcodec_err(vpu, "id %d not supported", id);
+		return -EINVAL;
+	}
+	if (vpu_enc_send_msg(vpu, &out, msg_size)) {
+		mtk_vcodec_err(vpu,
+			       "AP_IPIMSG_ENC_SET_PARAM %d fail", id);
+		return -EINVAL;
+	}
+
+	mtk_vcodec_debug(vpu, "id %d <-", id);
+
+	return 0;
+}
+
+static int vpu_enc_encode_32bits(struct venc_vpu_inst *vpu,
+				 unsigned int bs_mode,
+				 struct venc_frm_buf *frm_buf,
+				 struct mtk_vcodec_mem *bs_buf,
+				 struct venc_frame_info *frame_info)
+{
+	const bool is_ext = MTK_ENC_CTX_IS_EXT(vpu->ctx);
+	size_t msg_size = is_ext ?
+		sizeof(struct venc_ap_ipi_msg_enc_ext) :
+		sizeof(struct venc_ap_ipi_msg_enc);
+	struct venc_ap_ipi_msg_enc_ext out;
+
+	mtk_vcodec_debug(vpu, "bs_mode %d ->", bs_mode);
+
+	memset(&out, 0, sizeof(out));
+	out.base.msg_id = AP_IPIMSG_ENC_ENCODE;
+	out.base.vpu_inst_addr = vpu->inst_addr;
+	out.base.bs_mode = bs_mode;
+	if (frm_buf) {
+		if ((frm_buf->fb_addr[0].dma_addr % 16 == 0) &&
+		    (frm_buf->fb_addr[1].dma_addr % 16 == 0) &&
+		    (frm_buf->fb_addr[2].dma_addr % 16 == 0)) {
+			out.base.input_addr[0] = frm_buf->fb_addr[0].dma_addr;
+			out.base.input_addr[1] = frm_buf->fb_addr[1].dma_addr;
+			out.base.input_addr[2] = frm_buf->fb_addr[2].dma_addr;
+		} else {
+			mtk_vcodec_err(vpu, "dma_addr not align to 16");
+			return -EINVAL;
+		}
+	}
+	if (bs_buf) {
+		out.base.bs_addr = bs_buf->dma_addr;
+		out.base.bs_size = bs_buf->size;
+	}
+	if (is_ext && frame_info) {
+		out.data_item = 3;
+		out.data[0] = frame_info->frm_count;
+		out.data[1] = frame_info->skip_frm_count;
+		out.data[2] = frame_info->frm_type;
+	}
+	if (vpu_enc_send_msg(vpu, &out, msg_size)) {
+		mtk_vcodec_err(vpu, "AP_IPIMSG_ENC_ENCODE %d fail",
+			       bs_mode);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vpu_enc_encode_34bits(struct venc_vpu_inst *vpu,
+				 unsigned int bs_mode,
+				 struct venc_frm_buf *frm_buf,
+				 struct mtk_vcodec_mem *bs_buf,
+				 struct venc_frame_info *frame_info)
+{
+	struct venc_ap_ipi_msg_enc_ext_34 out;
+	size_t msg_size = sizeof(struct venc_ap_ipi_msg_enc_ext_34);
+
+	mtk_vcodec_debug(vpu, "bs_mode %d ->", bs_mode);
+
+	memset(&out, 0, sizeof(out));
+	out.msg_id = AP_IPIMSG_ENC_ENCODE;
+	out.vpu_inst_addr = vpu->inst_addr;
+	out.bs_mode = bs_mode;
+
+	if (frm_buf) {
+		if ((frm_buf->fb_addr[0].dma_addr % 16 == 0) &&
+		    (frm_buf->fb_addr[1].dma_addr % 16 == 0) &&
+		    (frm_buf->fb_addr[2].dma_addr % 16 == 0)) {
+			out.input_addr[0] = frm_buf->fb_addr[0].dma_addr;
+			out.input_addr[1] = frm_buf->fb_addr[1].dma_addr;
+			out.input_addr[2] = frm_buf->fb_addr[2].dma_addr;
+		} else {
+			mtk_vcodec_err(vpu, "dma_addr not align to 16");
+			return -EINVAL;
+		}
+	}
+	if (bs_buf) {
+		out.bs_addr = bs_buf->dma_addr;
+		out.bs_size = bs_buf->size;
+	}
+	if (frame_info) {
+		out.data_item = 3;
+		out.data[0] = frame_info->frm_count;
+		out.data[1] = frame_info->skip_frm_count;
+		out.data[2] = frame_info->frm_type;
+	}
+	if (vpu_enc_send_msg(vpu, &out, msg_size)) {
+		mtk_vcodec_err(vpu, "AP_IPIMSG_ENC_ENCODE %d fail",
+			       bs_mode);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int vpu_enc_encode(struct venc_vpu_inst *vpu, unsigned int bs_mode,
+		   struct venc_frm_buf *frm_buf,
+		   struct mtk_vcodec_mem *bs_buf,
+		   struct venc_frame_info *frame_info)
+{
+	int ret;
+
+	if (MTK_ENC_IOVA_IS_34BIT(vpu->ctx))
+		ret = vpu_enc_encode_34bits(vpu, bs_mode,
+					    frm_buf, bs_buf, frame_info);
+	else
+		ret = vpu_enc_encode_32bits(vpu, bs_mode,
+					    frm_buf, bs_buf, frame_info);
+
+	if (ret)
+		return ret;
+
+	mtk_vcodec_debug(vpu, "bs_mode %d state %d size %d key_frm %d <-",
+			 bs_mode, vpu->state, vpu->bs_size, vpu->is_key_frm);
+
+	return 0;
+}
+
+int vpu_enc_deinit(struct venc_vpu_inst *vpu)
+{
+	struct venc_ap_ipi_msg_deinit out;
+
+	mtk_vcodec_debug_enter(vpu);
+
+	memset(&out, 0, sizeof(out));
+	out.msg_id = AP_IPIMSG_ENC_DEINIT;
+	out.vpu_inst_addr = vpu->inst_addr;
+	if (vpu_enc_send_msg(vpu, &out, sizeof(out))) {
+		mtk_vcodec_err(vpu, "AP_IPIMSG_ENC_DEINIT fail");
+		return -EINVAL;
+	}
+
+	mtk_vcodec_debug_leave(vpu);
+
+	return 0;
+}
diff --git a/drivers/media/platform/mediatek/vcodec/venc_vpu_if.h b/drivers/media/platform/mediatek/vcodec/venc_vpu_if.h
new file mode 100644
index 000000000..f83bc1b3f
--- /dev/null
+++ b/drivers/media/platform/mediatek/vcodec/venc_vpu_if.h
@@ -0,0 +1,51 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ * Author: PoChun Lin <pochun.lin@mediatek.com>
+ */
+
+#ifndef _VENC_VPU_IF_H_
+#define _VENC_VPU_IF_H_
+
+#include "mtk_vcodec_fw.h"
+#include "venc_drv_if.h"
+
+/*
+ * struct venc_vpu_inst - encoder VPU driver instance
+ * @wq_hd: wait queue used for vpu cmd trigger then wait vpu interrupt done
+ * @signaled: flag used for checking vpu interrupt done
+ * @failure: flag to show vpu cmd succeeds or not
+ * @state: enum venc_ipi_msg_enc_state
+ * @bs_size: bitstream size for skip frame case usage
+ * @is_key_frm: key frame flag
+ * @inst_addr: VPU instance addr
+ * @vsi: driver structure allocated by VPU side and shared to AP side for
+ *	 control and info share
+ * @id: the id of inter-processor interrupt
+ * @ctx: context for v4l2 layer integration
+ * @dev: device for v4l2 layer integration
+ */
+struct venc_vpu_inst {
+	wait_queue_head_t wq_hd;
+	int signaled;
+	int failure;
+	int state;
+	int bs_size;
+	int is_key_frm;
+	unsigned int inst_addr;
+	void *vsi;
+	int id;
+	struct mtk_vcodec_ctx *ctx;
+};
+
+int vpu_enc_init(struct venc_vpu_inst *vpu);
+int vpu_enc_set_param(struct venc_vpu_inst *vpu,
+		      enum venc_set_param_type id,
+		      struct venc_enc_param *param);
+int vpu_enc_encode(struct venc_vpu_inst *vpu, unsigned int bs_mode,
+		   struct venc_frm_buf *frm_buf,
+		   struct mtk_vcodec_mem *bs_buf,
+		   struct venc_frame_info *frame_info);
+int vpu_enc_deinit(struct venc_vpu_inst *vpu);
+
+#endif
diff --git a/drivers/media/platform/mediatek/vpu/Kconfig b/drivers/media/platform/mediatek/vpu/Kconfig
new file mode 100644
index 000000000..2a8443a93
--- /dev/null
+++ b/drivers/media/platform/mediatek/vpu/Kconfig
@@ -0,0 +1,15 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+config VIDEO_MEDIATEK_VPU
+	tristate "Mediatek Video Processor Unit"
+	depends on V4L_MEM2MEM_DRIVERS
+	depends on VIDEO_DEV
+	depends on ARCH_MEDIATEK || COMPILE_TEST
+	help
+	    This driver provides downloading VPU firmware and
+	    communicating with VPU. This driver for hw video
+	    codec embedded in Mediatek's MT8173 SOCs. It is able
+	    to handle video decoding/encoding in a range of formats.
+
+	    To compile this driver as a module, choose M here: the
+	    module will be called mtk-vpu.
diff --git a/drivers/media/platform/mediatek/vpu/Makefile b/drivers/media/platform/mediatek/vpu/Makefile
new file mode 100644
index 000000000..ecd2d392b
--- /dev/null
+++ b/drivers/media/platform/mediatek/vpu/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0-only
+mtk-vpu-y += mtk_vpu.o
+
+obj-$(CONFIG_VIDEO_MEDIATEK_VPU) += mtk-vpu.o
diff --git a/drivers/media/platform/mediatek/vpu/mtk_vpu.c b/drivers/media/platform/mediatek/vpu/mtk_vpu.c
new file mode 100644
index 000000000..6beab9e86
--- /dev/null
+++ b/drivers/media/platform/mediatek/vpu/mtk_vpu.c
@@ -0,0 +1,1056 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+* Copyright (c) 2016 MediaTek Inc.
+* Author: Andrew-CT Chen <andrew-ct.chen@mediatek.com>
+*/
+#include <linux/clk.h>
+#include <linux/debugfs.h>
+#include <linux/firmware.h>
+#include <linux/interrupt.h>
+#include <linux/iommu.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/of_reserved_mem.h>
+#include <linux/sched.h>
+#include <linux/sizes.h>
+#include <linux/dma-mapping.h>
+
+#include "mtk_vpu.h"
+
+/*
+ * VPU (video processor unit) is a tiny processor controlling video hardware
+ * related to video codec, scaling and color format converting.
+ * VPU interfaces with other blocks by share memory and interrupt.
+ */
+
+#define INIT_TIMEOUT_MS		2000U
+#define IPI_TIMEOUT_MS		2000U
+#define VPU_IDLE_TIMEOUT_MS	1000U
+#define VPU_FW_VER_LEN		16
+
+/* maximum program/data TCM (Tightly-Coupled Memory) size */
+#define VPU_PTCM_SIZE		(96 * SZ_1K)
+#define VPU_DTCM_SIZE		(32 * SZ_1K)
+/* the offset to get data tcm address */
+#define VPU_DTCM_OFFSET		0x18000UL
+/* daynamic allocated maximum extended memory size */
+#define VPU_EXT_P_SIZE		SZ_1M
+#define VPU_EXT_D_SIZE		SZ_4M
+/* maximum binary firmware size */
+#define VPU_P_FW_SIZE		(VPU_PTCM_SIZE + VPU_EXT_P_SIZE)
+#define VPU_D_FW_SIZE		(VPU_DTCM_SIZE + VPU_EXT_D_SIZE)
+/* the size of share buffer between Host and  VPU */
+#define SHARE_BUF_SIZE		48
+
+/* binary firmware name */
+#define VPU_P_FW		"vpu_p.bin"
+#define VPU_D_FW		"vpu_d.bin"
+#define VPU_P_FW_NEW		"mediatek/mt8173/vpu_p.bin"
+#define VPU_D_FW_NEW		"mediatek/mt8173/vpu_d.bin"
+
+#define VPU_RESET		0x0
+#define VPU_TCM_CFG		0x0008
+#define VPU_PMEM_EXT0_ADDR	0x000C
+#define VPU_PMEM_EXT1_ADDR	0x0010
+#define VPU_TO_HOST		0x001C
+#define VPU_DMEM_EXT0_ADDR	0x0014
+#define VPU_DMEM_EXT1_ADDR	0x0018
+#define HOST_TO_VPU		0x0024
+#define VPU_IDLE_REG		0x002C
+#define VPU_INT_STATUS		0x0034
+#define VPU_PC_REG		0x0060
+#define VPU_SP_REG		0x0064
+#define VPU_RA_REG		0x0068
+#define VPU_WDT_REG		0x0084
+
+/* vpu inter-processor communication interrupt */
+#define VPU_IPC_INT		BIT(8)
+/* vpu idle state */
+#define VPU_IDLE_STATE		BIT(23)
+
+/**
+ * enum vpu_fw_type - VPU firmware type
+ *
+ * @P_FW: program firmware
+ * @D_FW: data firmware
+ *
+ */
+enum vpu_fw_type {
+	P_FW,
+	D_FW,
+};
+
+/**
+ * struct vpu_mem - VPU extended program/data memory information
+ *
+ * @va:		the kernel virtual memory address of VPU extended memory
+ * @pa:		the physical memory address of VPU extended memory
+ *
+ */
+struct vpu_mem {
+	void *va;
+	dma_addr_t pa;
+};
+
+/**
+ * struct vpu_regs - VPU TCM and configuration registers
+ *
+ * @tcm:	the register for VPU Tightly-Coupled Memory
+ * @cfg:	the register for VPU configuration
+ * @irq:	the irq number for VPU interrupt
+ */
+struct vpu_regs {
+	void __iomem *tcm;
+	void __iomem *cfg;
+	int irq;
+};
+
+/**
+ * struct vpu_wdt_handler - VPU watchdog reset handler
+ *
+ * @reset_func:	reset handler
+ * @priv:	private data
+ */
+struct vpu_wdt_handler {
+	void (*reset_func)(void *);
+	void *priv;
+};
+
+/**
+ * struct vpu_wdt - VPU watchdog workqueue
+ *
+ * @handler:	VPU watchdog reset handler
+ * @ws:		workstruct for VPU watchdog
+ * @wq:		workqueue for VPU watchdog
+ */
+struct vpu_wdt {
+	struct vpu_wdt_handler handler[VPU_RST_MAX];
+	struct work_struct ws;
+	struct workqueue_struct *wq;
+};
+
+/**
+ * struct vpu_run - VPU initialization status
+ *
+ * @signaled:		the signal of vpu initialization completed
+ * @fw_ver:		VPU firmware version
+ * @dec_capability:	decoder capability which is not used for now and
+ *			the value is reserved for future use
+ * @enc_capability:	encoder capability which is not used for now and
+ *			the value is reserved for future use
+ * @wq:			wait queue for VPU initialization status
+ */
+struct vpu_run {
+	u32 signaled;
+	char fw_ver[VPU_FW_VER_LEN];
+	unsigned int	dec_capability;
+	unsigned int	enc_capability;
+	wait_queue_head_t wq;
+};
+
+/**
+ * struct vpu_ipi_desc - VPU IPI descriptor
+ *
+ * @handler:	IPI handler
+ * @name:	the name of IPI handler
+ * @priv:	the private data of IPI handler
+ */
+struct vpu_ipi_desc {
+	ipi_handler_t handler;
+	const char *name;
+	void *priv;
+};
+
+/**
+ * struct share_obj - DTCM (Data Tightly-Coupled Memory) buffer shared with
+ *		      AP and VPU
+ *
+ * @id:		IPI id
+ * @len:	share buffer length
+ * @share_buf:	share buffer data
+ */
+struct share_obj {
+	s32 id;
+	u32 len;
+	unsigned char share_buf[SHARE_BUF_SIZE];
+};
+
+/**
+ * struct mtk_vpu - vpu driver data
+ * @extmem:		VPU extended memory information
+ * @reg:		VPU TCM and configuration registers
+ * @run:		VPU initialization status
+ * @wdt:		VPU watchdog workqueue
+ * @ipi_desc:		VPU IPI descriptor
+ * @recv_buf:		VPU DTCM share buffer for receiving. The
+ *			receive buffer is only accessed in interrupt context.
+ * @send_buf:		VPU DTCM share buffer for sending
+ * @dev:		VPU struct device
+ * @clk:		VPU clock on/off
+ * @fw_loaded:		indicate VPU firmware loaded
+ * @enable_4GB:		VPU 4GB mode on/off
+ * @vpu_mutex:		protect mtk_vpu (except recv_buf) and ensure only
+ *			one client to use VPU service at a time. For example,
+ *			suppose a client is using VPU to decode VP8.
+ *			If the other client wants to encode VP8,
+ *			it has to wait until VP8 decode completes.
+ * @wdt_refcnt:		WDT reference count to make sure the watchdog can be
+ *			disabled if no other client is using VPU service
+ * @ack_wq:		The wait queue for each codec and mdp. When sleeping
+ *			processes wake up, they will check the condition
+ *			"ipi_id_ack" to run the corresponding action or
+ *			go back to sleep.
+ * @ipi_id_ack:		The ACKs for registered IPI function sending
+ *			interrupt to VPU
+ *
+ */
+struct mtk_vpu {
+	struct vpu_mem extmem[2];
+	struct vpu_regs reg;
+	struct vpu_run run;
+	struct vpu_wdt wdt;
+	struct vpu_ipi_desc ipi_desc[IPI_MAX];
+	struct share_obj __iomem *recv_buf;
+	struct share_obj __iomem *send_buf;
+	struct device *dev;
+	struct clk *clk;
+	bool fw_loaded;
+	bool enable_4GB;
+	struct mutex vpu_mutex; /* for protecting vpu data data structure */
+	u32 wdt_refcnt;
+	wait_queue_head_t ack_wq;
+	bool ipi_id_ack[IPI_MAX];
+};
+
+static inline void vpu_cfg_writel(struct mtk_vpu *vpu, u32 val, u32 offset)
+{
+	writel(val, vpu->reg.cfg + offset);
+}
+
+static inline u32 vpu_cfg_readl(struct mtk_vpu *vpu, u32 offset)
+{
+	return readl(vpu->reg.cfg + offset);
+}
+
+static inline bool vpu_running(struct mtk_vpu *vpu)
+{
+	return vpu_cfg_readl(vpu, VPU_RESET) & BIT(0);
+}
+
+static void vpu_clock_disable(struct mtk_vpu *vpu)
+{
+	/* Disable VPU watchdog */
+	mutex_lock(&vpu->vpu_mutex);
+	if (!--vpu->wdt_refcnt)
+		vpu_cfg_writel(vpu,
+			       vpu_cfg_readl(vpu, VPU_WDT_REG) & ~(1L << 31),
+			       VPU_WDT_REG);
+	mutex_unlock(&vpu->vpu_mutex);
+
+	clk_disable(vpu->clk);
+}
+
+static int vpu_clock_enable(struct mtk_vpu *vpu)
+{
+	int ret;
+
+	ret = clk_enable(vpu->clk);
+	if (ret)
+		return ret;
+	/* Enable VPU watchdog */
+	mutex_lock(&vpu->vpu_mutex);
+	if (!vpu->wdt_refcnt++)
+		vpu_cfg_writel(vpu,
+			       vpu_cfg_readl(vpu, VPU_WDT_REG) | (1L << 31),
+			       VPU_WDT_REG);
+	mutex_unlock(&vpu->vpu_mutex);
+
+	return ret;
+}
+
+static void vpu_dump_status(struct mtk_vpu *vpu)
+{
+	dev_info(vpu->dev,
+		 "vpu: run %x, pc = 0x%x, ra = 0x%x, sp = 0x%x, idle = 0x%x\n"
+		 "vpu: int %x, hv = 0x%x, vh = 0x%x, wdt = 0x%x\n",
+		 vpu_running(vpu), vpu_cfg_readl(vpu, VPU_PC_REG),
+		 vpu_cfg_readl(vpu, VPU_RA_REG), vpu_cfg_readl(vpu, VPU_SP_REG),
+		 vpu_cfg_readl(vpu, VPU_IDLE_REG),
+		 vpu_cfg_readl(vpu, VPU_INT_STATUS),
+		 vpu_cfg_readl(vpu, HOST_TO_VPU),
+		 vpu_cfg_readl(vpu, VPU_TO_HOST),
+		 vpu_cfg_readl(vpu, VPU_WDT_REG));
+}
+
+int vpu_ipi_register(struct platform_device *pdev,
+		     enum ipi_id id, ipi_handler_t handler,
+		     const char *name, void *priv)
+{
+	struct mtk_vpu *vpu = platform_get_drvdata(pdev);
+	struct vpu_ipi_desc *ipi_desc;
+
+	if (!vpu) {
+		dev_err(&pdev->dev, "vpu device in not ready\n");
+		return -EPROBE_DEFER;
+	}
+
+	if (id < IPI_MAX && handler) {
+		ipi_desc = vpu->ipi_desc;
+		ipi_desc[id].name = name;
+		ipi_desc[id].handler = handler;
+		ipi_desc[id].priv = priv;
+		return 0;
+	}
+
+	dev_err(&pdev->dev, "register vpu ipi id %d with invalid arguments\n",
+		id);
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(vpu_ipi_register);
+
+int vpu_ipi_send(struct platform_device *pdev,
+		 enum ipi_id id, void *buf,
+		 unsigned int len)
+{
+	struct mtk_vpu *vpu = platform_get_drvdata(pdev);
+	struct share_obj __iomem *send_obj = vpu->send_buf;
+	unsigned long timeout;
+	int ret = 0;
+
+	if (id <= IPI_VPU_INIT || id >= IPI_MAX ||
+	    len > sizeof(send_obj->share_buf) || !buf) {
+		dev_err(vpu->dev, "failed to send ipi message\n");
+		return -EINVAL;
+	}
+
+	ret = vpu_clock_enable(vpu);
+	if (ret) {
+		dev_err(vpu->dev, "failed to enable vpu clock\n");
+		return ret;
+	}
+	if (!vpu_running(vpu)) {
+		dev_err(vpu->dev, "vpu_ipi_send: VPU is not running\n");
+		ret = -EINVAL;
+		goto clock_disable;
+	}
+
+	mutex_lock(&vpu->vpu_mutex);
+
+	 /* Wait until VPU receives the last command */
+	timeout = jiffies + msecs_to_jiffies(IPI_TIMEOUT_MS);
+	do {
+		if (time_after(jiffies, timeout)) {
+			dev_err(vpu->dev, "vpu_ipi_send: IPI timeout!\n");
+			ret = -EIO;
+			vpu_dump_status(vpu);
+			goto mut_unlock;
+		}
+	} while (vpu_cfg_readl(vpu, HOST_TO_VPU));
+
+	memcpy_toio(send_obj->share_buf, buf, len);
+	writel(len, &send_obj->len);
+	writel(id, &send_obj->id);
+
+	vpu->ipi_id_ack[id] = false;
+	/* send the command to VPU */
+	vpu_cfg_writel(vpu, 0x1, HOST_TO_VPU);
+
+	mutex_unlock(&vpu->vpu_mutex);
+
+	/* wait for VPU's ACK */
+	timeout = msecs_to_jiffies(IPI_TIMEOUT_MS);
+	ret = wait_event_timeout(vpu->ack_wq, vpu->ipi_id_ack[id], timeout);
+	vpu->ipi_id_ack[id] = false;
+	if (ret == 0) {
+		dev_err(vpu->dev, "vpu ipi %d ack time out !\n", id);
+		ret = -EIO;
+		vpu_dump_status(vpu);
+		goto clock_disable;
+	}
+	vpu_clock_disable(vpu);
+
+	return 0;
+
+mut_unlock:
+	mutex_unlock(&vpu->vpu_mutex);
+clock_disable:
+	vpu_clock_disable(vpu);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vpu_ipi_send);
+
+static void vpu_wdt_reset_func(struct work_struct *ws)
+{
+	struct vpu_wdt *wdt = container_of(ws, struct vpu_wdt, ws);
+	struct mtk_vpu *vpu = container_of(wdt, struct mtk_vpu, wdt);
+	struct vpu_wdt_handler *handler = wdt->handler;
+	int index, ret;
+
+	dev_info(vpu->dev, "vpu reset\n");
+	ret = vpu_clock_enable(vpu);
+	if (ret) {
+		dev_err(vpu->dev, "[VPU] wdt enables clock failed %d\n", ret);
+		return;
+	}
+	mutex_lock(&vpu->vpu_mutex);
+	vpu_cfg_writel(vpu, 0x0, VPU_RESET);
+	vpu->fw_loaded = false;
+	mutex_unlock(&vpu->vpu_mutex);
+	vpu_clock_disable(vpu);
+
+	for (index = 0; index < VPU_RST_MAX; index++) {
+		if (handler[index].reset_func) {
+			handler[index].reset_func(handler[index].priv);
+			dev_dbg(vpu->dev, "wdt handler func %d\n", index);
+		}
+	}
+}
+
+int vpu_wdt_reg_handler(struct platform_device *pdev,
+			void wdt_reset(void *),
+			void *priv, enum rst_id id)
+{
+	struct mtk_vpu *vpu = platform_get_drvdata(pdev);
+	struct vpu_wdt_handler *handler;
+
+	if (!vpu) {
+		dev_err(&pdev->dev, "vpu device in not ready\n");
+		return -EPROBE_DEFER;
+	}
+
+	handler = vpu->wdt.handler;
+
+	if (id < VPU_RST_MAX && wdt_reset) {
+		dev_dbg(vpu->dev, "wdt register id %d\n", id);
+		mutex_lock(&vpu->vpu_mutex);
+		handler[id].reset_func = wdt_reset;
+		handler[id].priv = priv;
+		mutex_unlock(&vpu->vpu_mutex);
+		return 0;
+	}
+
+	dev_err(vpu->dev, "register vpu wdt handler failed\n");
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(vpu_wdt_reg_handler);
+
+unsigned int vpu_get_vdec_hw_capa(struct platform_device *pdev)
+{
+	struct mtk_vpu *vpu = platform_get_drvdata(pdev);
+
+	return vpu->run.dec_capability;
+}
+EXPORT_SYMBOL_GPL(vpu_get_vdec_hw_capa);
+
+unsigned int vpu_get_venc_hw_capa(struct platform_device *pdev)
+{
+	struct mtk_vpu *vpu = platform_get_drvdata(pdev);
+
+	return vpu->run.enc_capability;
+}
+EXPORT_SYMBOL_GPL(vpu_get_venc_hw_capa);
+
+void *vpu_mapping_dm_addr(struct platform_device *pdev,
+			  u32 dtcm_dmem_addr)
+{
+	struct mtk_vpu *vpu = platform_get_drvdata(pdev);
+
+	if (!dtcm_dmem_addr ||
+	    (dtcm_dmem_addr > (VPU_DTCM_SIZE + VPU_EXT_D_SIZE))) {
+		dev_err(vpu->dev, "invalid virtual data memory address\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (dtcm_dmem_addr < VPU_DTCM_SIZE)
+		return (__force void *)(dtcm_dmem_addr + vpu->reg.tcm +
+					VPU_DTCM_OFFSET);
+
+	return vpu->extmem[D_FW].va + (dtcm_dmem_addr - VPU_DTCM_SIZE);
+}
+EXPORT_SYMBOL_GPL(vpu_mapping_dm_addr);
+
+struct platform_device *vpu_get_plat_device(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *vpu_node;
+	struct platform_device *vpu_pdev;
+
+	vpu_node = of_parse_phandle(dev->of_node, "mediatek,vpu", 0);
+	if (!vpu_node) {
+		dev_err(dev, "can't get vpu node\n");
+		return NULL;
+	}
+
+	vpu_pdev = of_find_device_by_node(vpu_node);
+	of_node_put(vpu_node);
+	if (WARN_ON(!vpu_pdev)) {
+		dev_err(dev, "vpu pdev failed\n");
+		return NULL;
+	}
+
+	return vpu_pdev;
+}
+EXPORT_SYMBOL_GPL(vpu_get_plat_device);
+
+/* load vpu program/data memory */
+static int load_requested_vpu(struct mtk_vpu *vpu,
+			      u8 fw_type)
+{
+	size_t tcm_size = fw_type ? VPU_DTCM_SIZE : VPU_PTCM_SIZE;
+	size_t fw_size = fw_type ? VPU_D_FW_SIZE : VPU_P_FW_SIZE;
+	char *fw_name = fw_type ? VPU_D_FW : VPU_P_FW;
+	char *fw_new_name = fw_type ? VPU_D_FW_NEW : VPU_P_FW_NEW;
+	const struct firmware *vpu_fw;
+	size_t dl_size = 0;
+	size_t extra_fw_size = 0;
+	void *dest;
+	int ret;
+
+	ret = request_firmware(&vpu_fw, fw_new_name, vpu->dev);
+	if (ret < 0) {
+		dev_info(vpu->dev, "Failed to load %s, %d, retry\n",
+			 fw_new_name, ret);
+
+		ret = request_firmware(&vpu_fw, fw_name, vpu->dev);
+		if (ret < 0) {
+			dev_err(vpu->dev, "Failed to load %s, %d\n", fw_name,
+				ret);
+			return ret;
+		}
+	}
+	dl_size = vpu_fw->size;
+	if (dl_size > fw_size) {
+		dev_err(vpu->dev, "fw %s size %zu is abnormal\n", fw_name,
+			dl_size);
+		release_firmware(vpu_fw);
+		return  -EFBIG;
+	}
+	dev_dbg(vpu->dev, "Downloaded fw %s size: %zu.\n",
+		fw_name,
+		dl_size);
+	/* reset VPU */
+	vpu_cfg_writel(vpu, 0x0, VPU_RESET);
+
+	/* handle extended firmware size */
+	if (dl_size > tcm_size) {
+		dev_dbg(vpu->dev, "fw size %zu > limited fw size %zu\n",
+			dl_size, tcm_size);
+		extra_fw_size = dl_size - tcm_size;
+		dev_dbg(vpu->dev, "extra_fw_size %zu\n", extra_fw_size);
+		dl_size = tcm_size;
+	}
+	dest = (__force void *)vpu->reg.tcm;
+	if (fw_type == D_FW)
+		dest += VPU_DTCM_OFFSET;
+	memcpy(dest, vpu_fw->data, dl_size);
+	/* download to extended memory if need */
+	if (extra_fw_size > 0) {
+		dest = vpu->extmem[fw_type].va;
+		dev_dbg(vpu->dev, "download extended memory type %x\n",
+			fw_type);
+		memcpy(dest, vpu_fw->data + tcm_size, extra_fw_size);
+	}
+
+	release_firmware(vpu_fw);
+
+	return 0;
+}
+
+int vpu_load_firmware(struct platform_device *pdev)
+{
+	struct mtk_vpu *vpu;
+	struct device *dev;
+	struct vpu_run *run;
+	int ret;
+
+	if (!pdev) {
+		pr_err("VPU platform device is invalid\n");
+		return -EINVAL;
+	}
+
+	dev = &pdev->dev;
+
+	vpu = platform_get_drvdata(pdev);
+	run = &vpu->run;
+
+	mutex_lock(&vpu->vpu_mutex);
+	if (vpu->fw_loaded) {
+		mutex_unlock(&vpu->vpu_mutex);
+		return 0;
+	}
+	mutex_unlock(&vpu->vpu_mutex);
+
+	ret = vpu_clock_enable(vpu);
+	if (ret) {
+		dev_err(dev, "enable clock failed %d\n", ret);
+		return ret;
+	}
+
+	mutex_lock(&vpu->vpu_mutex);
+
+	run->signaled = false;
+	dev_dbg(vpu->dev, "firmware request\n");
+	/* Downloading program firmware to device*/
+	ret = load_requested_vpu(vpu, P_FW);
+	if (ret < 0) {
+		dev_err(dev, "Failed to request %s, %d\n", VPU_P_FW, ret);
+		goto OUT_LOAD_FW;
+	}
+
+	/* Downloading data firmware to device */
+	ret = load_requested_vpu(vpu, D_FW);
+	if (ret < 0) {
+		dev_err(dev, "Failed to request %s, %d\n", VPU_D_FW, ret);
+		goto OUT_LOAD_FW;
+	}
+
+	vpu->fw_loaded = true;
+	/* boot up vpu */
+	vpu_cfg_writel(vpu, 0x1, VPU_RESET);
+
+	ret = wait_event_interruptible_timeout(run->wq,
+					       run->signaled,
+					       msecs_to_jiffies(INIT_TIMEOUT_MS)
+					       );
+	if (ret == 0) {
+		ret = -ETIME;
+		dev_err(dev, "wait vpu initialization timeout!\n");
+		goto OUT_LOAD_FW;
+	} else if (-ERESTARTSYS == ret) {
+		dev_err(dev, "wait vpu interrupted by a signal!\n");
+		goto OUT_LOAD_FW;
+	}
+
+	ret = 0;
+	dev_info(dev, "vpu is ready. Fw version %s\n", run->fw_ver);
+
+OUT_LOAD_FW:
+	mutex_unlock(&vpu->vpu_mutex);
+	vpu_clock_disable(vpu);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vpu_load_firmware);
+
+static void vpu_init_ipi_handler(const void *data, unsigned int len, void *priv)
+{
+	struct mtk_vpu *vpu = priv;
+	const struct vpu_run *run = data;
+
+	vpu->run.signaled = run->signaled;
+	strscpy(vpu->run.fw_ver, run->fw_ver, sizeof(vpu->run.fw_ver));
+	vpu->run.dec_capability = run->dec_capability;
+	vpu->run.enc_capability = run->enc_capability;
+	wake_up_interruptible(&vpu->run.wq);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static ssize_t vpu_debug_read(struct file *file, char __user *user_buf,
+			      size_t count, loff_t *ppos)
+{
+	char buf[256];
+	unsigned int len;
+	unsigned int running, pc, vpu_to_host, host_to_vpu, wdt, idle, ra, sp;
+	int ret;
+	struct device *dev = file->private_data;
+	struct mtk_vpu *vpu = dev_get_drvdata(dev);
+
+	ret = vpu_clock_enable(vpu);
+	if (ret) {
+		dev_err(vpu->dev, "[VPU] enable clock failed %d\n", ret);
+		return 0;
+	}
+
+	/* vpu register status */
+	running = vpu_running(vpu);
+	pc = vpu_cfg_readl(vpu, VPU_PC_REG);
+	wdt = vpu_cfg_readl(vpu, VPU_WDT_REG);
+	host_to_vpu = vpu_cfg_readl(vpu, HOST_TO_VPU);
+	vpu_to_host = vpu_cfg_readl(vpu, VPU_TO_HOST);
+	ra = vpu_cfg_readl(vpu, VPU_RA_REG);
+	sp = vpu_cfg_readl(vpu, VPU_SP_REG);
+	idle = vpu_cfg_readl(vpu, VPU_IDLE_REG);
+
+	vpu_clock_disable(vpu);
+
+	if (running) {
+		len = snprintf(buf, sizeof(buf), "VPU is running\n\n"
+		"FW Version: %s\n"
+		"PC: 0x%x\n"
+		"WDT: 0x%x\n"
+		"Host to VPU: 0x%x\n"
+		"VPU to Host: 0x%x\n"
+		"SP: 0x%x\n"
+		"RA: 0x%x\n"
+		"idle: 0x%x\n",
+		vpu->run.fw_ver, pc, wdt,
+		host_to_vpu, vpu_to_host, sp, ra, idle);
+	} else {
+		len = snprintf(buf, sizeof(buf), "VPU not running\n");
+	}
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+}
+
+static const struct file_operations vpu_debug_fops = {
+	.open = simple_open,
+	.read = vpu_debug_read,
+};
+#endif /* CONFIG_DEBUG_FS */
+
+static void vpu_free_ext_mem(struct mtk_vpu *vpu, u8 fw_type)
+{
+	struct device *dev = vpu->dev;
+	size_t fw_ext_size = fw_type ? VPU_EXT_D_SIZE : VPU_EXT_P_SIZE;
+
+	dma_free_coherent(dev, fw_ext_size, vpu->extmem[fw_type].va,
+			  vpu->extmem[fw_type].pa);
+}
+
+static int vpu_alloc_ext_mem(struct mtk_vpu *vpu, u32 fw_type)
+{
+	struct device *dev = vpu->dev;
+	size_t fw_ext_size = fw_type ? VPU_EXT_D_SIZE : VPU_EXT_P_SIZE;
+	u32 vpu_ext_mem0 = fw_type ? VPU_DMEM_EXT0_ADDR : VPU_PMEM_EXT0_ADDR;
+	u32 vpu_ext_mem1 = fw_type ? VPU_DMEM_EXT1_ADDR : VPU_PMEM_EXT1_ADDR;
+	u32 offset_4gb = vpu->enable_4GB ? 0x40000000 : 0;
+
+	vpu->extmem[fw_type].va = dma_alloc_coherent(dev,
+					       fw_ext_size,
+					       &vpu->extmem[fw_type].pa,
+					       GFP_KERNEL);
+	if (!vpu->extmem[fw_type].va) {
+		dev_err(dev, "Failed to allocate the extended program memory\n");
+		return -ENOMEM;
+	}
+
+	/* Disable extend0. Enable extend1 */
+	vpu_cfg_writel(vpu, 0x1, vpu_ext_mem0);
+	vpu_cfg_writel(vpu, (vpu->extmem[fw_type].pa & 0xFFFFF000) + offset_4gb,
+		       vpu_ext_mem1);
+
+	dev_info(dev, "%s extend memory phy=0x%llx virt=0x%p\n",
+		 fw_type ? "Data" : "Program",
+		 (unsigned long long)vpu->extmem[fw_type].pa,
+		 vpu->extmem[fw_type].va);
+
+	return 0;
+}
+
+static void vpu_ipi_handler(struct mtk_vpu *vpu)
+{
+	struct share_obj __iomem *rcv_obj = vpu->recv_buf;
+	struct vpu_ipi_desc *ipi_desc = vpu->ipi_desc;
+	unsigned char data[SHARE_BUF_SIZE];
+	s32 id = readl(&rcv_obj->id);
+
+	memcpy_fromio(data, rcv_obj->share_buf, sizeof(data));
+	if (id < IPI_MAX && ipi_desc[id].handler) {
+		ipi_desc[id].handler(data, readl(&rcv_obj->len),
+				     ipi_desc[id].priv);
+		if (id > IPI_VPU_INIT) {
+			vpu->ipi_id_ack[id] = true;
+			wake_up(&vpu->ack_wq);
+		}
+	} else {
+		dev_err(vpu->dev, "No such ipi id = %d\n", id);
+	}
+}
+
+static int vpu_ipi_init(struct mtk_vpu *vpu)
+{
+	/* Disable VPU to host interrupt */
+	vpu_cfg_writel(vpu, 0x0, VPU_TO_HOST);
+
+	/* shared buffer initialization */
+	vpu->recv_buf = vpu->reg.tcm + VPU_DTCM_OFFSET;
+	vpu->send_buf = vpu->recv_buf + 1;
+	memset_io(vpu->recv_buf, 0, sizeof(struct share_obj));
+	memset_io(vpu->send_buf, 0, sizeof(struct share_obj));
+
+	return 0;
+}
+
+static irqreturn_t vpu_irq_handler(int irq, void *priv)
+{
+	struct mtk_vpu *vpu = priv;
+	u32 vpu_to_host;
+	int ret;
+
+	/*
+	 * Clock should have been enabled already.
+	 * Enable again in case vpu_ipi_send times out
+	 * and has disabled the clock.
+	 */
+	ret = clk_enable(vpu->clk);
+	if (ret) {
+		dev_err(vpu->dev, "[VPU] enable clock failed %d\n", ret);
+		return IRQ_NONE;
+	}
+	vpu_to_host = vpu_cfg_readl(vpu, VPU_TO_HOST);
+	if (vpu_to_host & VPU_IPC_INT) {
+		vpu_ipi_handler(vpu);
+	} else {
+		dev_err(vpu->dev, "vpu watchdog timeout! 0x%x", vpu_to_host);
+		queue_work(vpu->wdt.wq, &vpu->wdt.ws);
+	}
+
+	/* VPU won't send another interrupt until we set VPU_TO_HOST to 0. */
+	vpu_cfg_writel(vpu, 0x0, VPU_TO_HOST);
+	clk_disable(vpu->clk);
+
+	return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_DEBUG_FS
+static struct dentry *vpu_debugfs;
+#endif
+static int mtk_vpu_probe(struct platform_device *pdev)
+{
+	struct mtk_vpu *vpu;
+	struct device *dev;
+	int ret = 0;
+
+	dev_dbg(&pdev->dev, "initialization\n");
+
+	dev = &pdev->dev;
+	vpu = devm_kzalloc(dev, sizeof(*vpu), GFP_KERNEL);
+	if (!vpu)
+		return -ENOMEM;
+
+	vpu->dev = &pdev->dev;
+	vpu->reg.tcm = devm_platform_ioremap_resource_byname(pdev, "tcm");
+	if (IS_ERR((__force void *)vpu->reg.tcm))
+		return PTR_ERR((__force void *)vpu->reg.tcm);
+
+	vpu->reg.cfg = devm_platform_ioremap_resource_byname(pdev, "cfg_reg");
+	if (IS_ERR((__force void *)vpu->reg.cfg))
+		return PTR_ERR((__force void *)vpu->reg.cfg);
+
+	/* Get VPU clock */
+	vpu->clk = devm_clk_get(dev, "main");
+	if (IS_ERR(vpu->clk)) {
+		dev_err(dev, "get vpu clock failed\n");
+		return PTR_ERR(vpu->clk);
+	}
+
+	platform_set_drvdata(pdev, vpu);
+
+	ret = clk_prepare(vpu->clk);
+	if (ret) {
+		dev_err(dev, "prepare vpu clock failed\n");
+		return ret;
+	}
+
+	/* VPU watchdog */
+	vpu->wdt.wq = create_singlethread_workqueue("vpu_wdt");
+	if (!vpu->wdt.wq) {
+		dev_err(dev, "initialize wdt workqueue failed\n");
+		ret = -ENOMEM;
+		goto clk_unprepare;
+	}
+	INIT_WORK(&vpu->wdt.ws, vpu_wdt_reset_func);
+	mutex_init(&vpu->vpu_mutex);
+
+	ret = vpu_clock_enable(vpu);
+	if (ret) {
+		dev_err(dev, "enable vpu clock failed\n");
+		goto workqueue_destroy;
+	}
+
+	dev_dbg(dev, "vpu ipi init\n");
+	ret = vpu_ipi_init(vpu);
+	if (ret) {
+		dev_err(dev, "Failed to init ipi\n");
+		goto disable_vpu_clk;
+	}
+
+	/* register vpu initialization IPI */
+	ret = vpu_ipi_register(pdev, IPI_VPU_INIT, vpu_init_ipi_handler,
+			       "vpu_init", vpu);
+	if (ret) {
+		dev_err(dev, "Failed to register IPI_VPU_INIT\n");
+		goto vpu_mutex_destroy;
+	}
+
+#ifdef CONFIG_DEBUG_FS
+	vpu_debugfs = debugfs_create_file("mtk_vpu", S_IRUGO, NULL, (void *)dev,
+					  &vpu_debug_fops);
+#endif
+
+	/* Set PTCM to 96K and DTCM to 32K */
+	vpu_cfg_writel(vpu, 0x2, VPU_TCM_CFG);
+
+	vpu->enable_4GB = !!(totalram_pages() > (SZ_2G >> PAGE_SHIFT));
+	dev_info(dev, "4GB mode %u\n", vpu->enable_4GB);
+
+	if (vpu->enable_4GB) {
+		ret = of_reserved_mem_device_init(dev);
+		if (ret)
+			dev_info(dev, "init reserved memory failed\n");
+			/* continue to use dynamic allocation if failed */
+	}
+
+	ret = vpu_alloc_ext_mem(vpu, D_FW);
+	if (ret) {
+		dev_err(dev, "Allocate DM failed\n");
+		goto remove_debugfs;
+	}
+
+	ret = vpu_alloc_ext_mem(vpu, P_FW);
+	if (ret) {
+		dev_err(dev, "Allocate PM failed\n");
+		goto free_d_mem;
+	}
+
+	init_waitqueue_head(&vpu->run.wq);
+	init_waitqueue_head(&vpu->ack_wq);
+
+	ret = platform_get_irq(pdev, 0);
+	if (ret < 0)
+		goto free_p_mem;
+	vpu->reg.irq = ret;
+	ret = devm_request_irq(dev, vpu->reg.irq, vpu_irq_handler, 0,
+			       pdev->name, vpu);
+	if (ret) {
+		dev_err(dev, "failed to request irq\n");
+		goto free_p_mem;
+	}
+
+	vpu_clock_disable(vpu);
+	dev_dbg(dev, "initialization completed\n");
+
+	return 0;
+
+free_p_mem:
+	vpu_free_ext_mem(vpu, P_FW);
+free_d_mem:
+	vpu_free_ext_mem(vpu, D_FW);
+remove_debugfs:
+	of_reserved_mem_device_release(dev);
+#ifdef CONFIG_DEBUG_FS
+	debugfs_remove(vpu_debugfs);
+#endif
+	memset(vpu->ipi_desc, 0, sizeof(struct vpu_ipi_desc) * IPI_MAX);
+vpu_mutex_destroy:
+	mutex_destroy(&vpu->vpu_mutex);
+disable_vpu_clk:
+	vpu_clock_disable(vpu);
+workqueue_destroy:
+	destroy_workqueue(vpu->wdt.wq);
+clk_unprepare:
+	clk_unprepare(vpu->clk);
+
+	return ret;
+}
+
+static const struct of_device_id mtk_vpu_match[] = {
+	{
+		.compatible = "mediatek,mt8173-vpu",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, mtk_vpu_match);
+
+static int mtk_vpu_remove(struct platform_device *pdev)
+{
+	struct mtk_vpu *vpu = platform_get_drvdata(pdev);
+
+#ifdef CONFIG_DEBUG_FS
+	debugfs_remove(vpu_debugfs);
+#endif
+	if (vpu->wdt.wq)
+		destroy_workqueue(vpu->wdt.wq);
+	vpu_free_ext_mem(vpu, P_FW);
+	vpu_free_ext_mem(vpu, D_FW);
+	mutex_destroy(&vpu->vpu_mutex);
+	clk_unprepare(vpu->clk);
+
+	return 0;
+}
+
+static int mtk_vpu_suspend(struct device *dev)
+{
+	struct mtk_vpu *vpu = dev_get_drvdata(dev);
+	unsigned long timeout;
+	int ret;
+
+	ret = vpu_clock_enable(vpu);
+	if (ret) {
+		dev_err(dev, "failed to enable vpu clock\n");
+		return ret;
+	}
+
+	if (!vpu_running(vpu)) {
+		vpu_clock_disable(vpu);
+		clk_unprepare(vpu->clk);
+		return 0;
+	}
+
+	mutex_lock(&vpu->vpu_mutex);
+	/* disable vpu timer interrupt */
+	vpu_cfg_writel(vpu, vpu_cfg_readl(vpu, VPU_INT_STATUS) | VPU_IDLE_STATE,
+		       VPU_INT_STATUS);
+	/* check if vpu is idle for system suspend */
+	timeout = jiffies + msecs_to_jiffies(VPU_IDLE_TIMEOUT_MS);
+	do {
+		if (time_after(jiffies, timeout)) {
+			dev_err(dev, "vpu idle timeout\n");
+			mutex_unlock(&vpu->vpu_mutex);
+			vpu_clock_disable(vpu);
+			return -EIO;
+		}
+	} while (!vpu_cfg_readl(vpu, VPU_IDLE_REG));
+
+	mutex_unlock(&vpu->vpu_mutex);
+	vpu_clock_disable(vpu);
+	clk_unprepare(vpu->clk);
+
+	return 0;
+}
+
+static int mtk_vpu_resume(struct device *dev)
+{
+	struct mtk_vpu *vpu = dev_get_drvdata(dev);
+	int ret;
+
+	clk_prepare(vpu->clk);
+	ret = vpu_clock_enable(vpu);
+	if (ret) {
+		dev_err(dev, "failed to enable vpu clock\n");
+		return ret;
+	}
+
+	mutex_lock(&vpu->vpu_mutex);
+	/* enable vpu timer interrupt */
+	vpu_cfg_writel(vpu,
+		       vpu_cfg_readl(vpu, VPU_INT_STATUS) & ~(VPU_IDLE_STATE),
+		       VPU_INT_STATUS);
+	mutex_unlock(&vpu->vpu_mutex);
+	vpu_clock_disable(vpu);
+
+	return 0;
+}
+
+static const struct dev_pm_ops mtk_vpu_pm = {
+	.suspend = mtk_vpu_suspend,
+	.resume = mtk_vpu_resume,
+};
+
+static struct platform_driver mtk_vpu_driver = {
+	.probe	= mtk_vpu_probe,
+	.remove	= mtk_vpu_remove,
+	.driver	= {
+		.name	= "mtk_vpu",
+		.pm = &mtk_vpu_pm,
+		.of_match_table = mtk_vpu_match,
+	},
+};
+
+module_platform_driver(mtk_vpu_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Mediatek Video Processor Unit driver");
diff --git a/drivers/media/platform/mediatek/vpu/mtk_vpu.h b/drivers/media/platform/mediatek/vpu/mtk_vpu.h
new file mode 100644
index 000000000..a56053ff1
--- /dev/null
+++ b/drivers/media/platform/mediatek/vpu/mtk_vpu.h
@@ -0,0 +1,188 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+* Copyright (c) 2016 MediaTek Inc.
+* Author: Andrew-CT Chen <andrew-ct.chen@mediatek.com>
+*/
+
+#ifndef _MTK_VPU_H
+#define _MTK_VPU_H
+
+#include <linux/platform_device.h>
+
+/**
+ * DOC: VPU
+ *
+ * VPU (video processor unit) is a tiny processor controlling video hardware
+ * related to video codec, scaling and color format converting.
+ * VPU interfaces with other blocks by share memory and interrupt.
+ */
+
+typedef void (*ipi_handler_t) (const void *data,
+			       unsigned int len,
+			       void *priv);
+
+/**
+ * enum ipi_id - the id of inter-processor interrupt
+ *
+ * @IPI_VPU_INIT:	 The interrupt from vpu is to notfiy kernel
+ *			 VPU initialization completed.
+ *			 IPI_VPU_INIT is sent from VPU when firmware is
+ *			 loaded. AP doesn't need to send IPI_VPU_INIT
+ *			 command to VPU.
+ *			 For other IPI below, AP should send the request
+ *			 to VPU to trigger the interrupt.
+ * @IPI_VDEC_H264:	 The interrupt from vpu is to notify kernel to
+ *			 handle H264 vidoe decoder job, and vice versa.
+ *			 Decode output format is always MT21 no matter what
+ *			 the input format is.
+ * @IPI_VDEC_VP8:	 The interrupt from is to notify kernel to
+ *			 handle VP8 video decoder job, and vice versa.
+ *			 Decode output format is always MT21 no matter what
+ *			 the input format is.
+ * @IPI_VDEC_VP9:	 The interrupt from vpu is to notify kernel to
+ *			 handle VP9 video decoder job, and vice versa.
+ *			 Decode output format is always MT21 no matter what
+ *			 the input format is.
+ * @IPI_VENC_H264:	 The interrupt from vpu is to notify kernel to
+ *			 handle H264 video encoder job, and vice versa.
+ * @IPI_VENC_VP8:	 The interrupt fro vpu is to notify kernel to
+ *			 handle VP8 video encoder job,, and vice versa.
+ * @IPI_MDP:		 The interrupt from vpu is to notify kernel to
+ *			 handle MDP (Media Data Path) job, and vice versa.
+ * @IPI_MAX:		 The maximum IPI number
+ */
+
+enum ipi_id {
+	IPI_VPU_INIT = 0,
+	IPI_VDEC_H264,
+	IPI_VDEC_VP8,
+	IPI_VDEC_VP9,
+	IPI_VENC_H264,
+	IPI_VENC_VP8,
+	IPI_MDP,
+	IPI_MAX,
+};
+
+/**
+ * enum rst_id - reset id to register reset function for VPU watchdog timeout
+ *
+ * @VPU_RST_ENC: encoder reset id
+ * @VPU_RST_DEC: decoder reset id
+ * @VPU_RST_MDP: MDP (Media Data Path) reset id
+ * @VPU_RST_MAX: maximum reset id
+ */
+enum rst_id {
+	VPU_RST_ENC,
+	VPU_RST_DEC,
+	VPU_RST_MDP,
+	VPU_RST_MAX,
+};
+
+/**
+ * vpu_ipi_register - register an ipi function
+ *
+ * @pdev:	VPU platform device
+ * @id:		IPI ID
+ * @handler:	IPI handler
+ * @name:	IPI name
+ * @priv:	private data for IPI handler
+ *
+ * Register an ipi function to receive ipi interrupt from VPU.
+ *
+ * Return: Return 0 if ipi registers successfully, otherwise it is failed.
+ */
+int vpu_ipi_register(struct platform_device *pdev, enum ipi_id id,
+		     ipi_handler_t handler, const char *name, void *priv);
+
+/**
+ * vpu_ipi_send - send data from AP to vpu.
+ *
+ * @pdev:	VPU platform device
+ * @id:		IPI ID
+ * @buf:	the data buffer
+ * @len:	the data buffer length
+ *
+ * This function is thread-safe. When this function returns,
+ * VPU has received the data and starts the processing.
+ * When the processing completes, IPI handler registered
+ * by vpu_ipi_register will be called in interrupt context.
+ *
+ * Return: Return 0 if sending data successfully, otherwise it is failed.
+ **/
+int vpu_ipi_send(struct platform_device *pdev,
+		 enum ipi_id id, void *buf,
+		 unsigned int len);
+
+/**
+ * vpu_get_plat_device - get VPU's platform device
+ *
+ * @pdev:	the platform device of the module requesting VPU platform
+ *		device for using VPU API.
+ *
+ * Return: Return NULL if it is failed.
+ * otherwise it is VPU's platform device
+ **/
+struct platform_device *vpu_get_plat_device(struct platform_device *pdev);
+
+/**
+ * vpu_wdt_reg_handler - register a VPU watchdog handler
+ *
+ * @pdev:               VPU platform device
+ * @vpu_wdt_reset_func():	the callback reset function
+ *	@priv: the private data for reset function
+ * @priv:		the private data for reset function
+ * @id:			reset id
+ *
+ * Register a handler performing own tasks when vpu reset by watchdog
+ *
+ * Return: Return 0 if the handler is added successfully,
+ * otherwise it is failed.
+ **/
+int vpu_wdt_reg_handler(struct platform_device *pdev,
+			void vpu_wdt_reset_func(void *priv),
+			void *priv, enum rst_id id);
+
+/**
+ * vpu_get_vdec_hw_capa - get video decoder hardware capability
+ *
+ * @pdev:	VPU platform device
+ *
+ * Return: video decoder hardware capability
+ **/
+unsigned int vpu_get_vdec_hw_capa(struct platform_device *pdev);
+
+/**
+ * vpu_get_venc_hw_capa - get video encoder hardware capability
+ *
+ * @pdev:	VPU platform device
+ *
+ * Return: video encoder hardware capability
+ **/
+unsigned int vpu_get_venc_hw_capa(struct platform_device *pdev);
+
+/**
+ * vpu_load_firmware - download VPU firmware and boot it
+ *
+ * @pdev:	VPU platform device
+ *
+ * Return: Return 0 if downloading firmware successfully,
+ * otherwise it is failed
+ **/
+int vpu_load_firmware(struct platform_device *pdev);
+
+/**
+ * vpu_mapping_dm_addr - Mapping DTCM/DMEM to kernel virtual address
+ *
+ * @pdev:		VPU platform device
+ * @dtcm_dmem_addr:	VPU's data memory address
+ *
+ * Mapping the VPU's DTCM (Data Tightly-Coupled Memory) /
+ * DMEM (Data Extended Memory) memory address to
+ * kernel virtual address.
+ *
+ * Return: Return ERR_PTR(-EINVAL) if mapping failed,
+ * otherwise the mapped kernel virtual address
+ **/
+void *vpu_mapping_dm_addr(struct platform_device *pdev,
+			  u32 dtcm_dmem_addr);
+#endif /* _MTK_VPU_H */