[RFC PATCH V0 0/7] media: platform: Add support for Face Detection (FD) on mt8183 SoC

[RFC PATCH V0 0/7] media: platform: Add support for Face Detection (FD) on mt8183 SoC

[Jerry-ch Chen]

Hello,

This is the first version of the RFC patch series adding Face Detection
(FD) driver on Mediatek mt8183 SoC, which will be used in camera features
on CrOS application. It belongs to the first Mediatek's camera driver
series based on V4L2 and media controller framework. I posted the main part
of the FD driver as RFC to discuss first and would like some review
comments on the overall structure of the driver.

Face Detection (FD) unit provide hardware accelerated face detection
feature. It can detect different sizes of faces in a given image.
Furthermore, it has the capability to detect the faces of Rotation-in-Plane
from -180 to +180 degrees and Rotation-off-Plane from -90 to +90 degrees.

The driver is implemented with V4L2 and media controller framework. We have
the following entities describing the FD path.

1. Meta input (output video device): connects to FD sub device. It accepts
   the input parameter buffer from userspace. The metadata interface used
   currently is only a temporary solution to kick off driver development
   and is not ready for reviewed yet.

2. RAW (output video device): connects to FD sub device. It accepts input
   image buffer from userspace.

3. FD (sub device): connects to Meta output. When processing an image,
   FD hardware only returns the statistics of detected faces so it needs
   only one capture video devices to return the streaming data to the user.

4. Meta output (capture video device): Return the result of detected faces
   as metadata output.

   The overall file structure of the FD driver is as following:

* mtk_fd-dev-ctx-core.c: Implements common software flow of FD driver.
* mtk_fd-v4l2.c: Static FD contexts configuration.
* mtk_fd.c: Controls the hardware flow.
* mtk_fd-dev.c: Implements context-independent flow.
* mtk_fd-ctrl.c: Handles the HW ctrl request from userspace.
* mtk_fd-smem-drv.c: Provides the shared memory management required
operation. We reserved a memory region for the co-processor and FD to
exchange the hardware configuration data.
* mtk_fd-v4l2-util.c: Implements V4L2 and vb2 ops.

Jerry-ch Chen (7):
  dt-bindings: mt8183: Add binding for FD shared memory
  dts: arm64: mt8183: Add FD shared memory node
  dt-bindings: mt8183: Added FD-SMEM dt-bindings
  dt-bindings: mt8183: Added FD dt-bindings
  dts: arm64: mt8183: Add FD nodes
  media: platform: Add Mediatek FD driver KConfig
  platform: mtk-isp: Add Mediatek FD driver

 .../devicetree/bindings/media/mediatek,fd_smem.txt |   28 +
 .../bindings/media/mediatek,mt8183-fd.txt          |   30 +
 .../mediatek,reserve-memory-fd_smem.txt            |   44 +
 arch/arm64/boot/dts/mediatek/mt8183.dtsi           |   28 +
 drivers/media/platform/Kconfig                     |    2 +
 drivers/media/platform/mtk-isp/Kconfig             |   10 +
 drivers/media/platform/mtk-isp/Makefile            |   16 +
 drivers/media/platform/mtk-isp/fd/Makefile         |   38 +
 drivers/media/platform/mtk-isp/fd/mtk_fd-core.h    |  157 +++
 drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h     |  299 ++++++
 .../platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c      |  917 +++++++++++++++++
 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c     |  355 +++++++
 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h     |  198 ++++
 .../media/platform/mtk-isp/fd/mtk_fd-smem-drv.c    |  452 +++++++++
 drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h    |   25 +
 .../media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c   | 1046 ++++++++++++++++++++
 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c    |  115 +++
 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h    |   36 +
 drivers/media/platform/mtk-isp/fd/mtk_fd.c         |  730 ++++++++++++++
 drivers/media/platform/mtk-isp/fd/mtk_fd.h         |  127 +++
 20 files changed, 4653 insertions(+)
 create mode 100644 Documentation/devicetree/bindings/media/mediatek,fd_smem.txt
 create mode 100644 Documentation/devicetree/bindings/media/mediatek,mt8183-fd.txt
 create mode 100644 Documentation/devicetree/bindings/reserved-memory/mediatek,reserve-memory-fd_smem.txt
 create mode 100644 drivers/media/platform/mtk-isp/Kconfig
 create mode 100644 drivers/media/platform/mtk-isp/Makefile
 create mode 100644 drivers/media/platform/mtk-isp/fd/Makefile
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-core.h
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-smem-drv.c
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd.c
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd.h

-- 
1.9.1

[RFC PATCH V0 1/7] dt-bindings: mt8183: Add binding for FD shared memory

[Jerry-ch Chen]

This patch adds the binding for describing the shared memory
used to exchange meta data between the co-processor and Face
Detection (FD) unit of the camera system on Mediatek SoCs.

Signed-off-by: Jerry-ch Chen <jerry-ch.chen@mediatek.com>
---
 .../mediatek,reserve-memory-fd_smem.txt            | 44 ++++++++++++++++++++++
 1 file changed, 44 insertions(+)
 create mode 100644 Documentation/devicetree/bindings/reserved-memory/mediatek,reserve-memory-fd_smem.txt

diff --git a/Documentation/devicetree/bindings/reserved-memory/mediatek,reserve-memory-fd_smem.txt b/Documentation/devicetree/bindings/reserved-memory/mediatek,reserve-memory-fd_smem.txt
new file mode 100644
index 0000000..52ae507
--- /dev/null
+++ b/Documentation/devicetree/bindings/reserved-memory/mediatek,reserve-memory-fd_smem.txt
@@ -0,0 +1,44 @@
+Mediatek FD Shared Memory binding
+
+This binding describes the shared memory, which serves the purpose of
+describing the shared memory region used to exchange data between Face
+Detection hardware (FD) and co-processor in Mediatek SoCs.
+
+The co-processor doesn't have the iommu so we need to use the physical
+address to access the shared buffer in the firmware.
+
+The Face Detection hardware (FD) can access memory through mt8183 IOMMU so
+it can use dma address to access the memory region.
+(See iommu/mediatek,iommu.txt for the detailed description of Mediatek IOMMU)
+
+
+Required properties:
+
+- compatible: must be "mediatek,reserve-memory-fd_smem"
+
+- reg: required for static allocation (see reserved-memory.txt for
+  the detailed usage)
+
+- alloc-range: required for dynamic allocation. The range must
+  between 0x00000400 and 0x100000000 due to the co-processer's
+  addressing limitation
+
+- size: required for dynamic allocation. The unit is bytes.
+  for Face Detection Unit, you need 1 MB at least.
+
+
+Example:
+
+The following example shows the FD shared memory setup for MT8183.
+
+	reserved-memory {
+		#address-cells = <2>;
+		#size-cells = <2>;
+		ranges;
+		reserve-memory-fd_smem {
+			compatible = "mediatek,reserve-memory-fd_smem";
+			size = <0 0x00100000>;
+			alignment = <0 0x1000>;
+			alloc-ranges = <0 0x40000000 0 0x100000000>;
+		};
+	};
\ No newline at end of file
-- 
1.9.1

[RFC PATCH V0 2/7] dts: arm64: mt8183: Add FD shared memory node

[Jerry-ch Chen]

This patch adds a shared memory region used on mt8183 for exchanging
meta data between co-processor and Face Detection (FD) unit.

Signed-off-by: Jerry-ch Chen <jerry-ch.chen@mediatek.com>
---
 arch/arm64/boot/dts/mediatek/mt8183.dtsi | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/arch/arm64/boot/dts/mediatek/mt8183.dtsi b/arch/arm64/boot/dts/mediatek/mt8183.dtsi
index c3a516e..b3d8dfd 100644
--- a/arch/arm64/boot/dts/mediatek/mt8183.dtsi
+++ b/arch/arm64/boot/dts/mediatek/mt8183.dtsi
@@ -134,6 +134,14 @@
 		clock-output-names = "clk26m";
 	};
 
+	reserve-memory-fd_smem {
+		compatible = "mediatek,reserve-memory-fd_smem";
+		no-map;
+		size = <0 0x00100000>;  /*1 MB share mem size */
+		alignment = <0 0x1000>;
+		alloc-ranges = <0 0x40000000 0 0x10000000>;
+	};
+
 	timer {
 		compatible = "arm,armv8-timer";
 		interrupt-parent = <&gic>;
-- 
1.9.1

[RFC PATCH V0 3/7] [media] dt-bindings: mt8183: Added FD-SMEM dt-bindings

[Jerry-ch Chen]

This patch adds the DT binding documentation for the shared memory
between Face Detection unit of the camera system and the co-processor
in Mediatek SoCs.

Signed-off-by: Jerry-ch Chen <jerry-ch.chen@mediatek.com>
---
 .../devicetree/bindings/media/mediatek,fd_smem.txt | 28 ++++++++++++++++++++++
 1 file changed, 28 insertions(+)
 create mode 100644 Documentation/devicetree/bindings/media/mediatek,fd_smem.txt

diff --git a/Documentation/devicetree/bindings/media/mediatek,fd_smem.txt b/Documentation/devicetree/bindings/media/mediatek,fd_smem.txt
new file mode 100644
index 0000000..6f7c836
--- /dev/null
+++ b/Documentation/devicetree/bindings/media/mediatek,fd_smem.txt
@@ -0,0 +1,28 @@
+Mediatek FD Shared Memory Device
+
+Mediatek FD Shared Memory Device is used to manage shared memory
+among CPU, FD and coprocessor. It is associated with a reserved
+memory region (Please see Documentation/devicetree/bindings/
+reserved-memory/mediatek,reserve-memory-fd_smem.txt) and
+provide the context to allocate memory with dma addresses.
+
+Required properties:
+- compatible: Shall be "mediatek,fd_smem"
+
+- iommus: Shall point to the respective IOMMU block with master port
+  as argument. Please set the ports which may be accessed
+  through the common path. You can see
+  Documentation/devicetree/bindings/iommu/mediatek,iommu.txt
+  for the detail.
+
+- mediatek,larb: must contain the local arbiters in the current SoCs.
+  Please set the larb of imgsys for FD if you are using FD function.
+  You can see Documentation/devicetree/bindings/memory-controllers/
+  mediatek,smi-larb.txt for the detail.
+
+Example:
+	fd_smem: fd_smem {
+		compatible = "mediatek,fd_smem";
+		mediatek,larb = <&larb5>;
+		iommus = <&iommu M4U_PORT_CAM_IMGI>;
+	};
-- 
1.9.1

[RFC PATCH V0 4/7] [media] dt-bindings: mt8183: Added FD dt-bindings

[Jerry-ch Chen]

This patch adds DT binding documentation for the Face Detection (FD)
unit of the camera system on Mediatek's SoCs.

Signed-off-by: Jerry-ch Chen <jerry-ch.chen@mediatek.com>
---
 .../bindings/media/mediatek,mt8183-fd.txt          | 30 ++++++++++++++++++++++
 1 file changed, 30 insertions(+)
 create mode 100644 Documentation/devicetree/bindings/media/mediatek,mt8183-fd.txt

diff --git a/Documentation/devicetree/bindings/media/mediatek,mt8183-fd.txt b/Documentation/devicetree/bindings/media/mediatek,mt8183-fd.txt
new file mode 100644
index 0000000..d8dbb89
--- /dev/null
+++ b/Documentation/devicetree/bindings/media/mediatek,mt8183-fd.txt
@@ -0,0 +1,30 @@
+* Mediatek Face Detection Unit (FD)
+
+Face Detection (FD) unit is a typical memory-to-memory HW device.
+It provides hardware accelerated face detection function, and it
+is able to detect different poses of faces. FD will writre result
+of detected face into memory as output.
+
+Required properties:
+- compatible: "mediatek,fd"
+- reg: Must contain an entry for each entry in reg-names.
+- reg-names: Must include the following entries:
+- interrupts: interrupt number to the cpu.
+- clocks : clock name from clock manager
+- clock-names: must be main. It is the main clock of FD
+
+Example:
+	fd:fd@1502b000 {
+		compatible = "mediatek,fd";
+		mediatek,larb = <&larb5>;
+		mediatek,vpu = <&vpu>;
+		iommus = <&iommu M4U_PORT_CAM_FDVT_RP>,
+			 <&iommu M4U_PORT_CAM_FDVT_WR>,
+			 <&iommu M4U_PORT_CAM_FDVT_RB>;
+		reg = <0 0x1502b000 0 0x1000>;
+		interrupts = <GIC_SPI 269 IRQ_TYPE_LEVEL_LOW>;
+		clocks = <&imgsys CLK_IMG_FDVT>;
+		clock-names = "FD_CLK_IMG_FDVT";
+		smem_device = <&fd_smem>;
+	};
+
-- 
1.9.1

[RFC PATCH V0 5/7] dts: arm64: mt8183: Add FD nodes

[Jerry-ch Chen]

This patch adds nodes for Face Detection (FD) unit. FD is embedded
in Mediatek SoCs and works with the co-processor to perform face
detection on the input data and image and output detected face result.

Signed-off-by: Jerry-ch Chen <jerry-ch.chen@mediatek.com>
---
 arch/arm64/boot/dts/mediatek/mt8183.dtsi | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/arch/arm64/boot/dts/mediatek/mt8183.dtsi b/arch/arm64/boot/dts/mediatek/mt8183.dtsi
index b3d8dfd..45c7e2f 100644
--- a/arch/arm64/boot/dts/mediatek/mt8183.dtsi
+++ b/arch/arm64/boot/dts/mediatek/mt8183.dtsi
@@ -440,6 +440,26 @@
 			#clock-cells = <1>;
 		};
 
+		fd_smem: fd_smem {
+			compatible = "mediatek,fd_smem";
+			mediatek,larb = <&larb5>;
+			iommus = <&iommu M4U_PORT_CAM_IMGI>;
+		};
+
+		fd:fd@1502b000 {
+			compatible = "mediatek,fd";
+			mediatek,larb = <&larb5>;
+			mediatek,vpu = <&vpu>;
+			iommus = <&iommu M4U_PORT_CAM_FDVT_RP>,
+				 <&iommu M4U_PORT_CAM_FDVT_WR>,
+				 <&iommu M4U_PORT_CAM_FDVT_RB>;
+			reg = <0 0x1502b000 0 0x1000>;
+			interrupts = <GIC_SPI 269 IRQ_TYPE_LEVEL_LOW>;
+			clocks = <&imgsys CLK_IMG_FDVT>;
+			clock-names = "FD_CLK_IMG_FD";
+			smem_device = <&fd_smem>;
+		};
+
 		vdecsys: syscon@16000000 {
 			compatible = "mediatek,mt8183-vdecsys", "syscon";
 			reg = <0 0x16000000 0 0x1000>;
-- 
1.9.1

[RFC PATCH V0 6/7] media: platform: Add Mediatek FD driver KConfig

[Jerry-ch Chen]

This patch adds KConfig for Mediatek Face Detection driver (FD).
FD is embedded in Mediatek SoCs. It can provide hardware
accelerated face detection function.

Signed-off-by: Jerry-ch Chen <jerry-ch.chen@mediatek.com>
---
 drivers/media/platform/Kconfig         |  2 ++
 drivers/media/platform/mtk-isp/Kconfig | 10 ++++++++++
 2 files changed, 12 insertions(+)
 create mode 100644 drivers/media/platform/mtk-isp/Kconfig

diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index a505e9f..ef08d48 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -32,6 +32,8 @@ source "drivers/media/platform/davinci/Kconfig"
 
 source "drivers/media/platform/omap/Kconfig"
 
+source "drivers/media/platform/mtk-isp/Kconfig"
+
 config VIDEO_ASPEED
 	tristate "Aspeed AST2400 and AST2500 Video Engine driver"
 	depends on VIDEO_V4L2
diff --git a/drivers/media/platform/mtk-isp/Kconfig b/drivers/media/platform/mtk-isp/Kconfig
new file mode 100644
index 0000000..096d2cb
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/Kconfig
@@ -0,0 +1,10 @@
+config VIDEO_MEDIATEK_FD_SUPPORT
+	bool "Mediatek face detection processing function"
+
+	default n
+	help
+		Support the basic hardware accelerated face detectioin feature.
+
+		FD driver provide face detection function, it can detect
+		faces of Rotation-in-Plane from -180 degrees to +180 degrees
+		and Rotation-off-Plane from -90 degrees to +90 degrees.
-- 
1.9.1

[RFC PATCH V0 7/7] [media] platform: mtk-isp: Add Mediatek FD driver

[Jerry-ch Chen]

This patch adds the driver of Face Detection (FD) unit in
Mediatek camera system, providing face detection function.

The mtk-isp directory will contain drivers for multiple IP
blocks found in Mediatek ISP system. It will include ISP Pass 1
driver (CAM), sensor interface driver, DIP driver and face
detection driver.

Signed-off-by: Jerry-ch Chen <jerry-ch.chen@mediatek.com>
---
 drivers/media/platform/mtk-isp/Makefile            |   16 +
 drivers/media/platform/mtk-isp/fd/Makefile         |   38 +
 drivers/media/platform/mtk-isp/fd/mtk_fd-core.h    |  157 +++
 drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h     |  299 ++++++
 .../platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c      |  912 +++++++++++++++++
 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c     |  355 +++++++
 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h     |  198 ++++
 .../media/platform/mtk-isp/fd/mtk_fd-smem-drv.c    |  452 +++++++++
 drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h    |   25 +
 .../media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c   | 1046 ++++++++++++++++++++
 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c    |  114 +++
 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h    |   36 +
 drivers/media/platform/mtk-isp/fd/mtk_fd.c         |  730 ++++++++++++++
 drivers/media/platform/mtk-isp/fd/mtk_fd.h         |  127 +++
 14 files changed, 4505 insertions(+)
 create mode 100644 drivers/media/platform/mtk-isp/Makefile
 create mode 100644 drivers/media/platform/mtk-isp/fd/Makefile
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-core.h
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-smem-drv.c
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd.c
 create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd.h

diff --git a/drivers/media/platform/mtk-isp/Makefile b/drivers/media/platform/mtk-isp/Makefile
new file mode 100644
index 0000000..5e3a9aa
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/Makefile
@@ -0,0 +1,16 @@
+#
+# Copyright (C) 2018 MediaTek Inc.
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License version 2 as
+# published by the Free Software Foundation.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+
+ifeq ($(CONFIG_VIDEO_MEDIATEK_FD_SUPPORT),y)
+obj-y += fd/
+endif
diff --git a/drivers/media/platform/mtk-isp/fd/Makefile b/drivers/media/platform/mtk-isp/fd/Makefile
new file mode 100644
index 0000000..ac168c1
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/Makefile
@@ -0,0 +1,38 @@
+#
+# Copyright (C) 2018 MediaTek Inc.
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License version 2 as
+# published by the Free Software Foundation.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+
+$(info "FD: makefile start srctree: $(srctree)")
+ccflags-y += -I$(srctree)/drivers/media/platform/mtk-vpu
+ccflags-y += -I$(srctree)/drivers/media/platform/mtk-isp/fd
+#ccflags-y += -I$(srctree)/drivers/media/platform/mtk-isp/common
+
+obj-y += mtk_fd.o
+obj-y += mtk_fd-v4l2.o
+
+# To provide alloc context managing memory shared
+# between CPU and FD coprocessor
+mtk_fd_smem-objs := \
+mtk_fd-smem-drv.o
+
+obj-y += mtk_fd_smem.o
+
+# Utilits to provide frame-based streaming model
+# with v4l2 user interfaces
+mtk_fd_util-objs := \
+mtk_fd-dev.o \
+mtk_fd-v4l2-util.o \
+mtk_fd-dev-ctx-core.o
+
+obj-y += mtk_fd_util.o
+
+$(info "FD: makefile end")
diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-core.h b/drivers/media/platform/mtk-isp/fd/mtk_fd-core.h
new file mode 100644
index 0000000..a7c1e1d
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-core.h
@@ -0,0 +1,157 @@
+/* SPDX-License-Identifier: GPL-2.0
+ * Copyright (C) 2015 MediaTek Inc.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MTK_FD_CORE_H__
+#define __MTK_FD_CORE_H__
+
+#define SIG_ERESTARTSYS 512
+
+#ifndef CONFIG_MTK_CLKMGR
+#include <linux/clk.h>
+#endif
+
+#ifdef CONFIG_MTK_CLKMGR
+#include <mach/mt_clkmgr.h>
+#endif
+
+#include "mtk_fd-dev.h"
+#include "mtk_fd-ctx.h"
+
+#include <linux/io.h>
+
+#define FD_WR32(v, a) \
+do { \
+	__raw_writel((v), (void __force __iomem *)((a))); \
+	mb(); /* ensure written */ \
+} while (0)
+
+#define FD_RD32(addr)                  ioread32((void *)addr)
+
+#define	FD_INT_EN	(0x15c)
+#define	FD_INT		(0x168)
+#define	FD_RESULT	(0x178)
+#define FD_IRQ_MASK	(0x001)
+
+#define RS_BUF_SIZE_MAX	(2288788)
+#define VA_OFFSET	(0xffff000000000000)
+
+enum fd_irq {
+	FD_IRQ_IDX = 0,
+	FD_IRQ_IDX_NUM
+};
+
+enum fd_state {
+	FD_INI,
+	FD_ENQ,
+	FD_CBD,
+};
+
+enum stream_stat {
+	STREAM_OFF,
+	STREAM_ON,
+};
+
+struct ipi_fd_enq_param {
+	u8 source_img_fmt;
+	struct fd_buffer output_addr;
+	struct fd_buffer src_y;
+	struct fd_buffer src_uv;
+	struct fd_buffer config_addr;
+} __packed;
+
+struct fd_manager_ctx {
+	struct fd_buffer learn_data_buf[2][LEARNDATA_NUM];
+	struct fd_buffer fd_config;
+	struct fd_buffer rs_config;
+	struct fd_buffer fd_result;
+	struct fd_buffer rs_result;
+	struct fd_buffer src_img;
+} __packed;
+
+struct mtk_fd_drv_ctx {
+	struct sg_table sgtable;
+	u32 frame_id;
+	struct fd_buffer rs_result;
+	struct fd_buffer scp_mem;
+	struct platform_device *vpu_pdev;
+
+	atomic_t fd_enque_cnt;
+	atomic_t fd_stream_cnt;
+	atomic_t fd_user_cnt;
+};
+
+struct mtk_fd_drv_dev {
+	struct platform_device *pdev;
+
+	dev_t fd_devno;
+	struct cdev   fd_cdev;
+	struct class *fd_class;
+	struct mtk_fd_drv_ctx fd_ctx;
+
+	struct device *larb_dev;
+	struct clk *fd_clk;
+	enum fd_state state;
+	enum stream_stat streaming;
+
+	wait_queue_head_t wq;
+	u32 fd_irq_result;
+
+	void __iomem *fd_base;
+};
+
+struct mtk_isp_fd_drv_data {
+	struct mtk_fd_dev fd_dev;
+	struct mtk_fd_drv_dev fd_hw_dev;
+} __packed;
+
+static inline struct mtk_fd_drv_dev *get_fd_hw_device(struct device *dev)
+{
+	struct mtk_isp_fd_drv_data *drv_data =
+		dev_get_drvdata(dev);
+	if (drv_data)
+		return &drv_data->fd_hw_dev;
+	else
+		return NULL;
+}
+
+#define mtk_fd_us_to_jiffies(us) \
+	((((unsigned long)(us) / 1000) * HZ + 512) >> 10)
+
+#define mtk_fd_hw_dev_to_drv(__fd_hw_dev) \
+	container_of(__fd_hw_dev, \
+	struct mtk_isp_fd_drv_data, fd_hw_dev)
+
+#define mtk_fd_ctx_to_drv(__fd_ctx) \
+	container_of(__fd_ctx, \
+	struct mtk_isp_fd_drv_data, fd_hw_dev.fd_ctx)
+
+enum fd_scp_cmd {
+	FD_CMD_INIT,
+	FD_CMD_ENQ,
+	FD_CMD_EXIT,
+};
+
+enum fd_clk {
+	clock_off = 0,
+	clock_on,
+};
+
+struct ipi_message {
+	u8 cmd_id;
+	union {
+		struct fd_buffer fd_manager;
+		struct v4l2_fd_param fd_param;
+	};
+} __packed;
+
+#endif/*__MTK_FD_CORE_H__*/
diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h b/drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h
new file mode 100644
index 0000000..d78a3af
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h
@@ -0,0 +1,299 @@
+/* SPDX-License-Identifier: GPL-2.0
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Frederic Chen <frederic.chen@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MTK_FD_CTX_H__
+#define __MTK_FD_CTX_H__
+
+#include <linux/types.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/videobuf2-core.h>
+#include <media/v4l2-subdev.h>
+
+#define MTK_FD_CTX_QUEUES (16)
+#define MTK_FD_CTX_FRAME_BUNDLE_BUFFER_MAX (MTK_FD_CTX_QUEUES)
+#define MTK_FD_CTX_DESC_MAX (MTK_FD_CTX_QUEUES)
+
+#define MTK_FD_CTX_MODE_DEBUG_OFF (0)
+#define MTK_FD_CTX_MODE_DEBUG_BYPASS_JOB_TRIGGER (1)
+#define MTK_FD_CTX_MODE_DEBUG_BYPASS_ALL (2)
+
+#define MTK_FD_GET_CTX_ID_FROM_SEQUENCE(sequence) \
+	((sequence) >> 16 & 0x0000FFFF)
+
+#define MTK_FD_CTX_META_BUF_DEFAULT_SIZE (1110 * 1024)
+
+struct mtk_fd_ctx;
+struct mtk_fd_ctx_finish_param;
+
+/**
+ * Attributes setup by device context owner
+ */
+struct mtk_fd_ctx_queue_desc {
+	int id;
+	/* id of the context queue */
+	char *name;
+	/* Will be exported to media entity name */
+	int capture;
+	/**
+	 * 1 for capture queue (device to user),
+	 * 0 for output queue (from user to device)
+	 */
+	int image;
+	/* 1 for image, 0 for meta data */
+	unsigned int dma_port;
+	/*The dma port associated to the buffer*/
+	struct mtk_fd_ctx_format *fmts;
+	int num_fmts;
+	/* Default format of this queue */
+	int default_fmt_idx;
+};
+
+/**
+ * Supported format and the information used for
+ * size calculation
+ */
+struct mtk_fd_ctx_meta_format {
+	u32 dataformat;
+	u32 max_buffer_size;
+	u8 flags;
+};
+
+/**
+ * MDP module's private format definitation
+ * (the same as struct mdp_format)
+ * It will be removed and changed to MDP's external interface
+ * after the integration with MDP module.
+ */
+struct mtk_fd_ctx_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 mtk_fd_ctx_format {
+	union {
+		struct mtk_fd_ctx_meta_format meta;
+		struct mtk_fd_ctx_mdp_format img;
+	} fmt;
+};
+
+union mtk_v4l2_fmt {
+	struct v4l2_pix_format_mplane pix_mp;
+	struct v4l2_meta_format	meta;
+};
+
+/* Attributes setup by device context owner */
+struct mtk_fd_ctx_queues_setting {
+	int master;
+	/* The master input node to trigger the frame data enqueue */
+	struct mtk_fd_ctx_queue_desc *output_queue_descs;
+	int total_output_queues;
+	struct mtk_fd_ctx_queue_desc *capture_queue_descs;
+	int total_capture_queues;
+};
+
+struct mtk_fd_ctx_queue_attr {
+	int master;
+	int input_offset;
+	int total_num;
+};
+
+/**
+ * Video node context. Since we use
+ * mtk_fd_ctx_frame_bundle to manage enqueued
+ * buffers by frame now, we don't use bufs filed of
+ * mtk_fd_ctx_queue now
+ */
+struct mtk_fd_ctx_queue {
+	union mtk_v4l2_fmt fmt;
+	struct mtk_fd_ctx_format *ctx_fmt;
+
+	unsigned int width_pad;
+	/* bytesperline, reserved */
+	struct mtk_fd_ctx_queue_desc desc;
+	unsigned int buffer_usage;
+	/* Current buffer usage of the queue */
+	int rotation;
+	struct list_head bufs;
+	/* Reserved, not used now */
+};
+
+enum mtk_fd_ctx_frame_bundle_state {
+	MTK_FD_CTX_FRAME_NEW,
+	/* Not allocated */
+	MTK_FD_CTX_FRAME_PREPARED,
+	/* Allocated but has not be processed */
+	MTK_FD_CTX_FRAME_PROCESSING,
+	/* Queued, waiting to be filled */
+};
+
+/**
+ * The definiation is compatible with FD driver's state definiation
+ * currently and will be decoupled after further integration
+ */
+enum mtk_fd_ctx_frame_data_state {
+	MTK_FD_CTX_FRAME_DATA_EMPTY = 0, /* FRAME_STATE_INIT */
+	MTK_FD_CTX_FRAME_DATA_DONE = 3, /* FRAME_STATE_DONE */
+	MTK_FD_CTX_FRAME_DATA_STREAMOFF_DONE = 4, /*FRAME_STATE_STREAMOFF*/
+	MTK_FD_CTX_FRAME_DATA_ERROR = 5, /*FRAME_STATE_ERROR*/
+};
+
+struct mtk_fd_ctx_frame_bundle {
+	struct mtk_fd_ctx_buffer*
+		buffers[MTK_FD_CTX_FRAME_BUNDLE_BUFFER_MAX];
+	int id;
+	int num_img_capture_bufs;
+	int num_img_output_bufs;
+	int num_meta_capture_bufs;
+	int num_meta_output_bufs;
+	int last_index;
+	int state;
+	struct list_head list;
+};
+
+struct mtk_fd_ctx_frame_bundle_list {
+	struct list_head list;
+};
+
+struct mtk_fd_ctx {
+	struct platform_device *pdev;
+	struct platform_device *smem_device;
+	unsigned short ctx_id;
+	char device_name[12];
+	const struct mtk_fd_ctx_ops *ops;
+	struct mtk_fd_dev_node_mapping *mtk_fd_dev_node_map;
+	unsigned int dev_node_num;
+	struct mtk_fd_ctx_queue queue[MTK_FD_CTX_QUEUES];
+	struct mtk_fd_ctx_queue_attr queues_attr;
+	atomic_t frame_param_sequence;
+	int streaming;
+	void *img_vb2_alloc_ctx;
+	void *smem_vb2_alloc_ctx;
+	struct v4l2_subdev_fh *fh;
+	struct mtk_fd_ctx_frame_bundle frame_bundles[VB2_MAX_FRAME];
+	struct mtk_fd_ctx_frame_bundle_list processing_frames;
+	struct mtk_fd_ctx_frame_bundle_list free_frames;
+	int enabled_dma_ports;
+	int num_frame_bundle;
+	int mode; /* Reserved for debug */
+	spinlock_t qlock;
+};
+
+enum mtk_fd_ctx_buffer_state {
+	MTK_FD_CTX_BUFFER_NEW,
+	MTK_FD_CTX_BUFFER_PROCESSING,
+	MTK_FD_CTX_BUFFER_DONE,
+	MTK_FD_CTX_BUFFER_FAILED,
+};
+
+struct mtk_fd_ctx_buffer {
+	union mtk_v4l2_fmt fmt;
+	struct mtk_fd_ctx_format *ctx_fmt;
+	int capture;
+	int image;
+	int frame_id;
+	int user_sequence; /* Sequence number assigned by user */
+	dma_addr_t daddr;
+	void *vaddr;
+	phys_addr_t paddr;
+	unsigned int queue;
+	unsigned int buffer_usage;
+	enum mtk_fd_ctx_buffer_state state;
+	int rotation;
+	struct list_head list;
+};
+
+struct mtk_fd_ctx_desc {
+	char *proc_dev_phandle;
+	/* The context device's compatble string name in device tree*/
+	int (*init)(struct mtk_fd_ctx *ctx);
+	/* configure the core functions of the device context */
+};
+
+struct mtk_fd_ctx_init_table {
+	int total_dev_ctx;
+	struct mtk_fd_ctx_desc *ctx_desc_tbl;
+};
+
+struct mtk_fd_ctx_finish_param {
+	unsigned int frame_id;
+	u64 timestamp;
+	unsigned int state;
+};
+
+bool mtk_fd_ctx_is_streaming(struct mtk_fd_ctx *ctx);
+
+int mtk_fd_ctx_core_job_finish(struct mtk_fd_ctx *ctx,
+			       struct mtk_fd_ctx_finish_param *param);
+
+int mtk_fd_ctx_core_init(struct mtk_fd_ctx *ctx,
+			 struct platform_device *pdev, int ctx_id,
+			 struct mtk_fd_ctx_desc *ctx_desc,
+			 struct platform_device *proc_pdev,
+			 struct platform_device *smem_pdev);
+
+int mtk_fd_ctx_core_exit(struct mtk_fd_ctx *ctx);
+
+void mtk_fd_ctx_buf_init(struct mtk_fd_ctx_buffer *b, unsigned int queue,
+			 dma_addr_t daddr);
+
+enum mtk_fd_ctx_buffer_state
+	mtk_fd_ctx_get_buffer_state(struct mtk_fd_ctx_buffer *b);
+
+int mtk_fd_ctx_next_global_frame_sequence(struct mtk_fd_ctx *ctx, int locked);
+
+int mtk_fd_ctx_core_queue_setup
+	(struct mtk_fd_ctx *ctx,
+	 struct mtk_fd_ctx_queues_setting *queues_setting);
+
+int mtk_fd_ctx_core_finish_param_init(void *param, int frame_id, int state);
+
+int mtk_fd_ctx_finish_frame(struct mtk_fd_ctx *dev_ctx,
+			    struct mtk_fd_ctx_frame_bundle *frame_bundle,
+			    int done);
+
+int mtk_fd_ctx_frame_bundle_init(struct mtk_fd_ctx_frame_bundle *frame_bundle);
+
+void mtk_fd_ctx_frame_bundle_add(struct mtk_fd_ctx *ctx,
+				 struct mtk_fd_ctx_frame_bundle *bundle,
+				 struct mtk_fd_ctx_buffer *ctx_buf);
+
+int mtk_fd_ctx_trigger_job(struct mtk_fd_ctx *dev_ctx,
+			   struct mtk_fd_ctx_frame_bundle *bundle_data);
+
+int mtk_fd_ctx_fmt_set_img(struct mtk_fd_ctx *dev_ctx, int queue_id,
+			   struct v4l2_pix_format_mplane *user_fmt,
+			   struct v4l2_pix_format_mplane *node_fmt);
+
+int mtk_fd_ctx_fmt_set_meta(struct mtk_fd_ctx *dev_ctx, int queue_id,
+			    struct v4l2_meta_format *user_fmt,
+			    struct v4l2_meta_format *node_fmt);
+
+int mtk_fd_ctx_format_load_default_fmt(struct mtk_fd_ctx_queue *queue,
+				       struct v4l2_format *fmt_to_fill);
+
+int mtk_fd_ctx_streamon(struct mtk_fd_ctx *dev_ctx);
+int mtk_fd_ctx_streamoff(struct mtk_fd_ctx *dev_ctx);
+int mtk_fd_ctx_release(struct mtk_fd_ctx *dev_ctx);
+int mtk_fd_ctx_open(struct mtk_fd_ctx *dev_ctx);
+int mtk_fd_ctx_fd_init(struct mtk_fd_ctx *ctx);
+
+#endif /*__MTK_FD_CTX_H__*/
diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c b/drivers/media/platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c
new file mode 100644
index 0000000..7314436
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c
@@ -0,0 +1,912 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Frederic Chen <frederic.chen@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <media/videobuf2-dma-contig.h>
+#include <linux/dma-mapping.h>
+
+#include "mtk_fd.h"
+#include "mtk_fd-dev.h"
+#include "mtk_fd-smem.h"
+#include "mtk_fd-v4l2.h"
+
+#if KERNEL_VERSION(4, 8, 0) >= MTK_FD_KERNEL_BASE_VERSION
+#include <linux/dma-attrs.h>
+#endif
+
+struct vb2_v4l2_buffer *mtk_fd_ctx_buffer_get_vb2_v4l2_buffer
+(struct mtk_fd_ctx_buffer *ctx_buf)
+{
+	struct mtk_fd_dev_buffer *dev_buf = NULL;
+
+	if (!ctx_buf) {
+		pr_err("Failed to convert ctx_buf to dev_buf: Null pointer\n");
+		return NULL;
+	}
+
+	dev_buf	= mtk_fd_ctx_buf_to_dev_buf(ctx_buf);
+
+	return &dev_buf->m2m2_buf.vbb;
+}
+
+int mtk_fd_ctx_core_queue_setup(struct mtk_fd_ctx *ctx,
+				struct mtk_fd_ctx_queues_setting *
+				queues_setting)
+{
+	int queue_idx = 0;
+	int i = 0;
+
+	for (i = 0; i < queues_setting->total_output_queues; i++) {
+		struct mtk_fd_ctx_queue_desc *queue_desc =
+			&queues_setting->output_queue_descs[i];
+		ctx->queue[queue_idx].desc = *queue_desc;
+		queue_idx++;
+	}
+
+	/* Setup the capture queue */
+	for (i = 0; i < queues_setting->total_capture_queues; i++) {
+		struct mtk_fd_ctx_queue_desc *queue_desc =
+			&queues_setting->capture_queue_descs[i];
+		ctx->queue[queue_idx].desc = *queue_desc;
+		queue_idx++;
+	}
+
+	ctx->queues_attr.master = queues_setting->master;
+	ctx->queues_attr.total_num = queue_idx;
+	ctx->dev_node_num = ctx->queues_attr.total_num;
+	strcpy(ctx->device_name, MTK_FD_DEV_NAME);
+	return 0;
+}
+
+/* Mediatek FD context core initialization */
+int mtk_fd_ctx_core_init(struct mtk_fd_ctx *ctx,
+			 struct platform_device *pdev, int ctx_id,
+			 struct mtk_fd_ctx_desc *ctx_desc,
+			 struct platform_device *proc_pdev,
+			 struct platform_device *smem_pdev)
+{
+	/* Initialize main data structure */
+	int r = 0;
+
+#if KERNEL_VERSION(4, 8, 0) >= MTK_FD_KERNEL_BASE_VERSION
+	ctx->smem_vb2_alloc_ctx =
+		vb2_dma_contig_init_ctx(&smem_pdev->dev);
+	ctx->img_vb2_alloc_ctx =
+		vb2_dma_contig_init_ctx(&pdev->dev);
+#else
+	ctx->smem_vb2_alloc_ctx = &smem_pdev->dev;
+	ctx->img_vb2_alloc_ctx = &pdev->dev;
+#endif
+	if (IS_ERR((__force void *)ctx->smem_vb2_alloc_ctx))
+		pr_err("Failed to alloc vb2 dma context: smem_vb2_alloc_ctx");
+
+	if (IS_ERR((__force void *)ctx->img_vb2_alloc_ctx))
+		pr_err("Failed to alloc vb2 dma context: img_vb2_alloc_ctx");
+
+	ctx->pdev = pdev;
+	ctx->ctx_id = ctx_id;
+	/* keep th smem pdev to use related iommu functions */
+	ctx->smem_device = smem_pdev;
+
+	/* Will set default enabled after passing the unit test */
+	ctx->mode = MTK_FD_CTX_MODE_DEBUG_OFF;
+
+	/* initialized the global frame index of the device context */
+	atomic_set(&ctx->frame_param_sequence, 0);
+	spin_lock_init(&ctx->qlock);
+
+	/* setup the core operation of the device context */
+	if (ctx_desc && ctx_desc->init)
+		r = ctx_desc->init(ctx);
+
+	return r;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_core_init);
+
+int mtk_fd_ctx_core_exit(struct mtk_fd_ctx *ctx)
+{
+#if KERNEL_VERSION(4, 8, 0) >= MTK_FD_KERNEL_BASE_VERSION
+	vb2_dma_contig_cleanup_ctx(ctx->smem_vb2_alloc_ctx);
+	vb2_dma_contig_cleanup_ctx(ctx->img_vb2_alloc_ctx);
+#else
+	ctx->smem_vb2_alloc_ctx = NULL;
+	ctx->img_vb2_alloc_ctx = NULL;
+#endif
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_core_exit);
+
+int mtk_fd_ctx_next_global_frame_sequence(struct mtk_fd_ctx *ctx, int locked)
+{
+	int global_frame_sequence =
+		atomic_inc_return(&ctx->frame_param_sequence);
+
+	if (!locked)
+		spin_lock(&ctx->qlock);
+
+	global_frame_sequence =
+		(global_frame_sequence & 0x0000FFFF) | (ctx->ctx_id << 16);
+
+	if (!locked)
+		spin_unlock(&ctx->qlock);
+
+	return global_frame_sequence;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_next_global_frame_sequence);
+
+static void mtk_fd_ctx_buffer_done
+	(struct mtk_fd_ctx_buffer *ctx_buf, int state)
+{
+		if (!ctx_buf || state != MTK_FD_CTX_BUFFER_DONE ||
+		    state != MTK_FD_CTX_BUFFER_FAILED)
+			return;
+
+		ctx_buf->state = state;
+}
+
+struct mtk_fd_ctx_frame_bundle *mtk_fd_ctx_get_processing_frame
+(struct mtk_fd_ctx *dev_ctx, int frame_id)
+{
+	struct mtk_fd_ctx_frame_bundle *frame_bundle = NULL;
+
+	spin_lock(&dev_ctx->qlock);
+
+	list_for_each_entry(frame_bundle,
+			    &dev_ctx->processing_frames.list, list) {
+		if (frame_bundle->id == frame_id) {
+			spin_unlock(&dev_ctx->qlock);
+			return frame_bundle;
+		}
+	}
+
+	spin_unlock(&dev_ctx->qlock);
+
+	return NULL;
+}
+
+/**
+ * structure mtk_fd_ctx_finish_param must be the first elemt of param
+ * So that the buffer can be return to vb2 queue successfully
+ */
+int mtk_fd_ctx_core_finish_param_init(void *param, int frame_id, int state)
+{
+	struct mtk_fd_ctx_finish_param *fram_param =
+		(struct mtk_fd_ctx_finish_param *)param;
+	fram_param->frame_id = frame_id;
+	fram_param->state = state;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_core_finish_param_init);
+
+void mtk_fd_ctx_frame_bundle_add(struct mtk_fd_ctx *ctx,
+				 struct mtk_fd_ctx_frame_bundle *bundle,
+				 struct mtk_fd_ctx_buffer *ctx_buf)
+{
+	int queue_id = 0;
+	struct mtk_fd_ctx_queue *ctx_queue = NULL;
+
+	if (!bundle || !ctx_buf) {
+		pr_warn("Add buffer to frame bundle failed, bundle(%llx),buf(%llx)\n",
+			(long long)bundle, (long long)ctx_buf);
+		return;
+	}
+
+	queue_id = ctx_buf->queue;
+
+	if (bundle->buffers[queue_id])
+		pr_warn("Queue(%d) buffer has alreay in this bundle, overwrite happen\n",
+			queue_id);
+
+	pr_debug("Add queue(%d) buffer%llx\n",
+		 queue_id, (unsigned long long)ctx_buf);
+		 bundle->buffers[queue_id] = ctx_buf;
+
+	/* Fill context queue related information */
+	ctx_queue = &ctx->queue[queue_id];
+
+	if (!ctx_queue) {
+		pr_err("Can't find ctx queue (%d)\n", queue_id);
+		return;
+	}
+
+	if (ctx->queue[ctx_buf->queue].desc.image) {
+		if (ctx->queue[ctx_buf->queue].desc.capture)
+			bundle->num_img_capture_bufs++;
+		else
+			bundle->num_img_output_bufs++;
+	} else {
+		if (ctx->queue[ctx_buf->queue].desc.capture)
+			bundle->num_meta_capture_bufs++;
+		else
+			bundle->num_meta_output_bufs++;
+	}
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_frame_bundle_add);
+
+void mtk_fd_ctx_buf_init(struct mtk_fd_ctx_buffer *b,
+			 unsigned int queue, dma_addr_t daddr)
+{
+	b->state = MTK_FD_CTX_BUFFER_NEW;
+	b->queue = queue;
+	b->daddr = daddr;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_buf_init);
+
+enum mtk_fd_ctx_buffer_state
+	mtk_fd_ctx_get_buffer_state(struct mtk_fd_ctx_buffer *b)
+{
+	return b->state;
+}
+
+bool mtk_fd_ctx_is_streaming(struct mtk_fd_ctx *ctx)
+{
+	return ctx->streaming;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_is_streaming);
+
+static int mtk_fd_ctx_free_frame(struct mtk_fd_ctx *dev_ctx,
+				 struct mtk_fd_ctx_frame_bundle *frame_bundle)
+{
+	spin_lock(&dev_ctx->qlock);
+
+	frame_bundle->state = MTK_FD_CTX_FRAME_NEW;
+	list_del(&frame_bundle->list);
+	list_add_tail(&frame_bundle->list, &dev_ctx->free_frames.list);
+
+	spin_unlock(&dev_ctx->qlock);
+
+	return 0;
+}
+
+int mtk_fd_ctx_core_job_finish(struct mtk_fd_ctx *dev_ctx,
+			       struct mtk_fd_ctx_finish_param *param)
+{
+	int i = 0;
+	struct platform_device *pdev = dev_ctx->pdev;
+	struct mtk_fd_ctx_finish_param *fram_param =
+		(struct mtk_fd_ctx_finish_param *)param;
+	struct mtk_fd_dev *fd_dev = NULL;
+	struct mtk_fd_ctx_frame_bundle *frame = NULL;
+	enum vb2_buffer_state vbf_state = VB2_BUF_STATE_DONE;
+	enum mtk_fd_ctx_buffer_state ctxf_state =
+		MTK_FD_CTX_BUFFER_DONE;
+	int user_sequence = 0;
+
+	const int ctx_id =
+		MTK_FD_GET_CTX_ID_FROM_SEQUENCE(fram_param->frame_id);
+	u64 timestamp = 0;
+
+	pr_debug("mtk_fd_ctx_core_job_finish_cb: param (%llx), platform_device(%llx)\n",
+		 (unsigned long long)param, (unsigned long long)pdev);
+
+	if (!dev_ctx)
+		pr_err("dev_ctx can't be null, can't release the frame\n");
+
+	fd_dev = mtk_fd_ctx_to_dev(dev_ctx);
+
+	if (fram_param) {
+		dev_info(&fd_dev->pdev->dev,
+			 "CB recvied from ctx(%d), frame(%d), state(%d), fd_dev(%llx)\n",
+			 ctx_id, fram_param->frame_id,
+			 fram_param->state, (long long)fd_dev);
+	} else {
+		dev_err(&fd_dev->pdev->dev,
+			"CB recvied from ctx(%d), frame param is NULL\n",
+			ctx_id);
+			return -EINVAL;
+	}
+
+	/* Get the buffers of the processed frame */
+	frame = mtk_fd_ctx_get_processing_frame(&fd_dev->ctx,
+						fram_param->frame_id);
+
+	if (!frame) {
+		pr_err("Can't find the frame boundle, Frame(%d)\n",
+		       fram_param->frame_id);
+		return -EINVAL;
+	}
+
+	if (fram_param->state == MTK_FD_CTX_FRAME_DATA_ERROR) {
+		vbf_state = VB2_BUF_STATE_ERROR;
+		ctxf_state = MTK_FD_CTX_BUFFER_FAILED;
+	}
+
+	/**
+	 * Set the buffer's VB2 status so that
+	 * the user can dequeue the buffer
+	 */
+	timestamp = ktime_get_ns();
+	for (i = 0; i <= frame->last_index; i++) {
+		struct mtk_fd_ctx_buffer *ctx_buf = frame->buffers[i];
+
+		if (!ctx_buf) {
+			dev_dbg(&fd_dev->pdev->dev,
+				"ctx_buf(queue id= %d) of frame(%d)is NULL\n",
+				i, fram_param->frame_id);
+			continue;
+		} else {
+			struct vb2_v4l2_buffer *b =
+				mtk_fd_ctx_buffer_get_vb2_v4l2_buffer(ctx_buf);
+			b->vb2_buf.timestamp = ktime_get_ns();
+			user_sequence = ctx_buf->user_sequence;
+			mtk_fd_ctx_buffer_done(ctx_buf, ctxf_state);
+			mtk_fd_v4l2_buffer_done(&b->vb2_buf, vbf_state);
+		}
+	}
+
+	mtk_fd_ctx_free_frame(&fd_dev->ctx, frame);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_core_job_finish);
+
+int mtk_fd_ctx_finish_frame(struct mtk_fd_ctx *dev_ctx,
+			    struct mtk_fd_ctx_frame_bundle *frame_bundle,
+			    int done)
+{
+	spin_lock(&dev_ctx->qlock);
+	frame_bundle->state = MTK_FD_CTX_FRAME_PROCESSING;
+	list_add_tail(&frame_bundle->list, &dev_ctx->processing_frames.list);
+	spin_unlock(&dev_ctx->qlock);
+	return 0;
+}
+
+static void set_img_fmt(struct v4l2_pix_format_mplane *mfmt_to_fill,
+			struct mtk_fd_ctx_format *ctx_fmt)
+{
+	int i = 0;
+
+	mfmt_to_fill->pixelformat = ctx_fmt->fmt.img.pixelformat;
+	mfmt_to_fill->num_planes = ctx_fmt->fmt.img.num_planes;
+
+	pr_info("%s: Fmt(%d),w(%d),h(%d)\n",
+		__func__,
+		mfmt_to_fill->pixelformat,
+		mfmt_to_fill->width,
+		mfmt_to_fill->height);
+
+	/**
+	 * The implementation wil be adjust after integrating MDP module
+	 * since it provides the common format suppporting function
+	 */
+	for (i = 0 ; i < mfmt_to_fill->num_planes; ++i) {
+		int bpl = (mfmt_to_fill->width *
+			   ctx_fmt->fmt.img.row_depth[i]) / 8;
+		int sizeimage = (mfmt_to_fill->width * mfmt_to_fill->height *
+				 ctx_fmt->fmt.img.depth[i]) / 8;
+
+		mfmt_to_fill->plane_fmt[i].bytesperline = bpl;
+		mfmt_to_fill->plane_fmt[i].sizeimage = sizeimage;
+		pr_info("plane(%d):bpl(%d),sizeimage(%u)\n",
+			i, bpl, mfmt_to_fill->plane_fmt[i].sizeimage);
+	}
+}
+
+static void set_meta_fmt(struct v4l2_meta_format *metafmt_to_fill,
+			 struct mtk_fd_ctx_format *ctx_fmt)
+{
+	metafmt_to_fill->dataformat = ctx_fmt->fmt.meta.dataformat;
+
+	if (ctx_fmt->fmt.meta.max_buffer_size <= 0 ||
+	    ctx_fmt->fmt.meta.max_buffer_size >
+	    MTK_FD_CTX_META_BUF_DEFAULT_SIZE) {
+		pr_warn("buf size of meta(%u) can't be 0, use default %u\n",
+			ctx_fmt->fmt.meta.dataformat,
+			MTK_FD_CTX_META_BUF_DEFAULT_SIZE);
+		metafmt_to_fill->buffersize = MTK_FD_CTX_META_BUF_DEFAULT_SIZE;
+	} else {
+		pr_info("Load the meta size setting %u\n",
+			ctx_fmt->fmt.meta.max_buffer_size);
+		metafmt_to_fill->buffersize = ctx_fmt->fmt.meta.max_buffer_size;
+	}
+}
+
+/* Get the default format setting */
+int mtk_fd_ctx_format_load_default_fmt(struct mtk_fd_ctx_queue *queue,
+				       struct v4l2_format *fmt_to_fill)
+{
+	struct mtk_fd_ctx_format *ctx_fmt = NULL;
+
+	if (queue->desc.num_fmts == 0)
+		return 0; /* no format support list associated to this queue */
+
+	if (queue->desc.default_fmt_idx >= queue->desc.num_fmts) {
+		pr_warn("Queue(%s) err: default idx(%d) must < num_fmts(%d)\n",
+			queue->desc.name, queue->desc.default_fmt_idx,
+			queue->desc.num_fmts);
+		queue->desc.default_fmt_idx = 0;
+		pr_warn("Queue(%s) : reset default idx(%d)\n",
+			queue->desc.name, queue->desc.default_fmt_idx);
+	}
+
+	ctx_fmt	= &queue->desc.fmts[queue->desc.default_fmt_idx];
+
+	/* Check the type of the buffer */
+	if (queue->desc.image) {
+		struct v4l2_pix_format_mplane *node_fmt =
+			&fmt_to_fill->fmt.pix_mp;
+
+		if (queue->desc.capture) {
+			fmt_to_fill->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+			node_fmt->width = MTK_FD_OUTPUT_MAX_WIDTH;
+			node_fmt->height = MTK_FD_OUTPUT_MAX_HEIGHT;
+		} else {
+			fmt_to_fill->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+			node_fmt->width = MTK_FD_INPUT_MAX_WIDTH;
+			node_fmt->height = MTK_FD_INPUT_MAX_HEIGHT;
+		}
+		set_img_fmt(node_fmt, ctx_fmt);
+	}	else {
+		/* meta buffer type */
+		struct v4l2_meta_format *node_fmt = &fmt_to_fill->fmt.meta;
+
+		if (queue->desc.capture)
+			fmt_to_fill->type = V4L2_BUF_TYPE_META_CAPTURE;
+		else
+			fmt_to_fill->type = V4L2_BUF_TYPE_META_OUTPUT;
+
+		set_meta_fmt(node_fmt, ctx_fmt);
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_format_load_default_fmt);
+
+static struct mtk_fd_ctx_format *mtk_fd_ctx_find_fmt
+	(struct mtk_fd_ctx_queue *queue, u32 format)
+{
+	int i;
+	struct mtk_fd_ctx_format *ctx_fmt;
+
+	for (i = 0; i < queue->desc.num_fmts; i++) {
+		ctx_fmt = &queue->desc.fmts[i];
+		if (queue->desc.image) {
+			pr_debug("idx(%d), pixelformat(%x), fmt(%x)\n",
+				 i, ctx_fmt->fmt.img.pixelformat, format);
+			if (ctx_fmt->fmt.img.pixelformat == format)
+				return ctx_fmt;
+		} else {
+			if (ctx_fmt->fmt.meta.dataformat == format)
+				return ctx_fmt;
+		}
+	}
+	return NULL;
+}
+
+int mtk_fd_ctx_fmt_set_meta(struct mtk_fd_ctx *dev_ctx, int queue_id,
+			    struct v4l2_meta_format *user_fmt,
+			    struct v4l2_meta_format *node_fmt)
+{
+	struct mtk_fd_ctx_queue *queue = NULL;
+	struct mtk_fd_ctx_format *ctx_fmt;
+
+	if (queue_id >= dev_ctx->queues_attr.total_num) {
+		pr_err("Invalid queue id:%d\n", queue_id);
+		return -EINVAL;
+	}
+
+	queue = &dev_ctx->queue[queue_id];
+	if (!user_fmt || !node_fmt)
+		return -EINVAL;
+
+	ctx_fmt = mtk_fd_ctx_find_fmt(queue, user_fmt->dataformat);
+	if (!ctx_fmt)
+		return -EINVAL;
+
+	queue->ctx_fmt = ctx_fmt;
+	set_meta_fmt(node_fmt, ctx_fmt);
+	*user_fmt = *node_fmt;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_fmt_set_meta);
+
+int mtk_fd_ctx_fmt_set_img(struct mtk_fd_ctx *dev_ctx, int queue_id,
+			   struct v4l2_pix_format_mplane *user_fmt,
+			   struct v4l2_pix_format_mplane *node_fmt)
+{
+	struct mtk_fd_ctx_queue *queue = NULL;
+	struct mtk_fd_ctx_format *ctx_fmt;
+
+	if (queue_id >= dev_ctx->queues_attr.total_num) {
+		pr_err("Invalid queue id:%d\n", queue_id);
+		return -EINVAL;
+	}
+
+	queue = &dev_ctx->queue[queue_id];
+	if (!user_fmt || !node_fmt)
+		return -EINVAL;
+
+	ctx_fmt = mtk_fd_ctx_find_fmt(queue, user_fmt->pixelformat);
+	if (!ctx_fmt)
+		return -EINVAL;
+
+	queue->ctx_fmt = ctx_fmt;
+	node_fmt->width = user_fmt->width;
+	node_fmt->height = user_fmt->height;
+
+	set_img_fmt(node_fmt, ctx_fmt);
+
+	*user_fmt = *node_fmt;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_fmt_set_img);
+
+int mtk_fd_ctx_streamon(struct mtk_fd_ctx *dev_ctx)
+{
+	int ret = 0;
+
+	if (dev_ctx->streaming) {
+		pr_warn("stream on failed, pdev(%llx), ctx(%d) already stream on\n",
+			(long long)dev_ctx->pdev, dev_ctx->ctx_id);
+		return -EBUSY;
+	}
+
+	ret = mtk_fd_streamon(dev_ctx->pdev, dev_ctx->ctx_id);
+	if (ret) {
+		pr_err("streamon: ctx(%d) failed, notified by context\n",
+		       dev_ctx->ctx_id);
+		return -EBUSY;
+	}
+
+	dev_ctx->streaming = true;
+	ret = mtk_fd_dev_queue_buffers(mtk_fd_ctx_to_dev(dev_ctx), true);
+	if (ret)
+		pr_err("failed to queue initial buffers (%d)", ret);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_streamon);
+
+int mtk_fd_ctx_streamoff(struct mtk_fd_ctx *dev_ctx)
+{
+	int ret = 0;
+
+	if (!dev_ctx->streaming) {
+		pr_warn("Do nothing, pdev(%llx), ctx(%d) is already stream off\n",
+			(long long)dev_ctx->pdev, dev_ctx->ctx_id);
+		return -EBUSY;
+	}
+
+	ret = mtk_fd_streamoff(dev_ctx->pdev, dev_ctx->ctx_id);
+	if (ret) {
+		pr_warn("streamoff: ctx(%d) failed, notified by context\n",
+			dev_ctx->ctx_id);
+		return -EBUSY;
+	}
+
+	dev_ctx->streaming = false;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_streamoff);
+
+int mtk_fd_ctx_init_frame_bundles(struct mtk_fd_ctx *dev_ctx)
+{
+	int i = 0;
+
+	dev_ctx->num_frame_bundle = VB2_MAX_FRAME;
+
+	spin_lock(&dev_ctx->qlock);
+
+	/* Reset the queue*/
+	INIT_LIST_HEAD(&dev_ctx->processing_frames.list);
+	INIT_LIST_HEAD(&dev_ctx->free_frames.list);
+
+	for (i = 0; i < dev_ctx->num_frame_bundle; i++) {
+		struct mtk_fd_ctx_frame_bundle *frame_bundle =
+			&dev_ctx->frame_bundles[i];
+		frame_bundle->state = MTK_FD_CTX_FRAME_NEW;
+		list_add_tail(&frame_bundle->list, &dev_ctx->free_frames.list);
+	}
+
+	spin_unlock(&dev_ctx->qlock);
+
+	return 0;
+}
+
+int mtk_fd_ctx_open(struct mtk_fd_ctx *dev_ctx)
+{
+	struct mtk_fd_dev *fd_dev = mtk_fd_ctx_to_dev(dev_ctx);
+	unsigned int enabled_dma_ports = 0;
+	int i = 0;
+
+	if (!dev_ctx)
+		return -EINVAL;
+
+	/* Get the enabled DMA ports */
+	for (i = 0; i < fd_dev->mem2mem2.num_nodes; i++) {
+		if (fd_dev->mem2mem2.nodes[i].enabled)
+			enabled_dma_ports |= dev_ctx->queue[i].desc.dma_port;
+	}
+
+	dev_ctx->enabled_dma_ports = enabled_dma_ports;
+
+	dev_dbg(&fd_dev->pdev->dev, "open device: (%llx)\n",
+		(long long)&fd_dev->pdev->dev);
+
+	/* Workaround for SCP EMI access */
+	mtk_fd_smem_enable_mpu(&dev_ctx->smem_device->dev);
+
+	/* Init the frame bundle pool */
+	mtk_fd_ctx_init_frame_bundles(dev_ctx);
+
+	return mtk_fd_open(dev_ctx->pdev);
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_open);
+
+int mtk_fd_ctx_release(struct mtk_fd_ctx *dev_ctx)
+{
+	return mtk_fd_release(dev_ctx->pdev);
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_release);
+
+static int mtk_fd_ctx_core_job_start(struct mtk_fd_ctx *dev_ctx,
+				     struct mtk_fd_ctx_frame_bundle *bundle)
+{
+	struct platform_device *pdev = dev_ctx->pdev;
+	int ret = 0;
+	struct v4l2_fd_param fd_param;
+	struct mtk_fd_ctx_buffer *ctx_buf_yuv_in = NULL;
+	struct mtk_fd_ctx_buffer *ctx_buf_meta_in = NULL;
+	struct mtk_fd_ctx_buffer *ctx_buf_meta_out = NULL;
+
+	if (!pdev || !bundle) {
+		dev_err(&pdev->dev,
+			"pdev(%llx) and param(%llx) in start can't be NULL\n",
+			(long long)pdev, (long long)bundle);
+		return -EINVAL;
+	}
+	memset(&fd_param, 0, sizeof(struct v4l2_fd_param));
+	fd_param.frame_id = bundle->id;
+
+	/* Img-in buffer */
+	ctx_buf_yuv_in = bundle->buffers[MTK_FD_CTX_FD_YUV_IN];
+	if (ctx_buf_yuv_in) {
+		fd_param.src_img.iova =	(uint32_t)ctx_buf_yuv_in->daddr;
+		fd_param.src_img.va = (uint64_t)ctx_buf_yuv_in->vaddr;
+		fd_param.src_img_h =
+			(uint16_t)ctx_buf_yuv_in->fmt.pix_mp.height;
+		fd_param.src_img_w =
+			(uint16_t)ctx_buf_yuv_in->fmt.pix_mp.width;
+	}
+
+	/* Meta-in buffer */
+	ctx_buf_meta_in = bundle->buffers[MTK_FD_CTX_FD_CONFIG_IN];
+	if (ctx_buf_meta_in) {
+		fd_param.fd_user_param.va = (uint64_t)ctx_buf_meta_in->vaddr;
+		fd_param.fd_user_param.pa = (uint32_t)ctx_buf_meta_in->paddr;
+		fd_param.fd_user_param.iova = (uint32_t)ctx_buf_meta_in->daddr;
+	} else {
+		dev_err(&pdev->dev, "meta in is null!\n");
+		fd_param.fd_user_param.pa = 0;
+		fd_param.fd_user_param.va = 0;
+		fd_param.fd_user_param.iova = 0;
+	}
+
+	/* Meta-out buffer */
+	ctx_buf_meta_out = bundle->buffers[MTK_FD_CTX_FD_OUT];
+	if (ctx_buf_meta_out) {
+		fd_param.fd_user_result.va = (uint64_t)ctx_buf_meta_out->vaddr;
+		fd_param.fd_user_result.pa = (uint32_t)ctx_buf_meta_out->paddr;
+		fd_param.fd_user_result.iova =
+					(uint32_t)ctx_buf_meta_out->daddr;
+	} else {
+		dev_err(&pdev->dev, "meta out is null!\n");
+		fd_param.fd_user_result.pa = 0;
+		fd_param.fd_user_result.va = 0;
+		fd_param.fd_user_result.iova = 0;
+	}
+
+	dev_info(&pdev->dev,
+		 "Delegate job to mtk_fd_enqueue: pdev(0x%llx), frame(%d)\n",
+		 (long long)pdev, bundle->id);
+	ret = mtk_fd_enqueue(pdev, &fd_param);
+
+	if (ret) {
+		dev_warn(&pdev->dev, "mtk_fd_enqueue failed: %d\n", ret);
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+static void debug_bundle(struct mtk_fd_ctx_frame_bundle *bundle_data)
+{
+	int i = 0;
+
+	if (!bundle_data) {
+		pr_warn("bundle_data is NULL\n");
+		return;
+	}
+
+	pr_debug("bundle buf nums (%d, %d,%d,%d)\n",
+		 bundle_data->num_img_capture_bufs,
+		 bundle_data->num_img_output_bufs,
+		 bundle_data->num_meta_capture_bufs,
+		 bundle_data->num_meta_output_bufs);
+
+	for (i = 0; i < 16 ; i++) {
+		pr_debug("Bundle, buf[%d] = %llx\n",
+			 i,
+			 (unsigned long long)bundle_data->buffers[i]);
+	}
+
+	pr_debug("Bundle last idx: %d\n", bundle_data->last_index);
+}
+
+static struct mtk_fd_ctx_frame_bundle *mtk_fd_ctx_get_free_frame
+	(struct mtk_fd_ctx *dev_ctx)
+{
+	struct mtk_fd_ctx_frame_bundle *frame_bundle = NULL;
+
+	spin_lock(&dev_ctx->qlock);
+	list_for_each_entry(frame_bundle,
+			    &dev_ctx->free_frames.list, list){
+		pr_debug("Check frame: state %d, new should be %d\n",
+			 frame_bundle->state, MTK_FD_CTX_FRAME_NEW);
+		if (frame_bundle->state == MTK_FD_CTX_FRAME_NEW) {
+			frame_bundle->state = MTK_FD_CTX_FRAME_PREPARED;
+			pr_debug("Found free frame\n");
+			spin_unlock(&dev_ctx->qlock);
+			return frame_bundle;
+		}
+	}
+	spin_unlock(&dev_ctx->qlock);
+	pr_err("Can't found any bundle is MTK_FD_CTX_FRAME_NEW\n");
+	return NULL;
+}
+
+static int mtk_fd_ctx_process_frame
+	(struct mtk_fd_ctx *dev_ctx,
+	 struct mtk_fd_ctx_frame_bundle *frame_bundle)
+{
+	spin_lock(&dev_ctx->qlock);
+
+	frame_bundle->state = MTK_FD_CTX_FRAME_PROCESSING;
+	list_del(&frame_bundle->list);
+	list_add_tail(&frame_bundle->list, &dev_ctx->processing_frames.list);
+
+	spin_unlock(&dev_ctx->qlock);
+	return 0;
+}
+
+int mtk_fd_ctx_trigger_job(struct mtk_fd_ctx  *dev_ctx,
+			   struct mtk_fd_ctx_frame_bundle *bundle_data)
+{
+	/* Scan all buffers and filled the ipi frame data*/
+	int i = 0;
+	struct mtk_fd_ctx_finish_param fram_param;
+
+	struct mtk_fd_ctx_frame_bundle *bundle =
+		mtk_fd_ctx_get_free_frame(dev_ctx);
+
+	pr_debug("Bundle data: , ctx id:%d\n",
+		 dev_ctx->ctx_id);
+
+	debug_bundle(bundle_data);
+
+	if (!bundle) {
+		pr_err("bundle can't be NULL\n");
+		goto FAILE_JOB_NOT_TRIGGER;
+	}
+	if (!bundle_data) {
+		pr_err("bundle_data can't be NULL\n");
+		goto FAILE_JOB_NOT_TRIGGER;
+	}
+
+	pr_debug("Copy bundle_data->buffers to bundle->buffers\n");
+	memcpy(bundle->buffers, bundle_data->buffers,
+	       sizeof(struct mtk_fd_ctx_buffer *) *
+	       MTK_FD_CTX_FRAME_BUNDLE_BUFFER_MAX);
+
+	pr_debug("bundle setup (%d, %d,%d,%d)\n",
+		 bundle_data->num_img_capture_bufs,
+		 bundle_data->num_img_output_bufs,
+		 bundle_data->num_meta_capture_bufs,
+		 bundle_data->num_meta_output_bufs);
+
+	bundle->num_img_capture_bufs = bundle_data->num_img_capture_bufs;
+	bundle->num_img_output_bufs = bundle_data->num_img_output_bufs;
+	bundle->num_meta_capture_bufs = bundle_data->num_meta_capture_bufs;
+	bundle->num_meta_output_bufs = bundle_data->num_meta_output_bufs;
+	bundle->id = mtk_fd_ctx_next_global_frame_sequence(dev_ctx,
+							   dev_ctx->ctx_id);
+	bundle->last_index = dev_ctx->queues_attr.total_num - 1;
+
+	debug_bundle(bundle);
+
+	pr_debug("Fill Address data\n");
+	for (i = 0; i <= bundle->last_index; i++) {
+		struct mtk_fd_ctx_buffer *ctx_buf = bundle->buffers[i];
+		struct vb2_v4l2_buffer *b = NULL;
+
+		pr_debug("Process queue[%d], ctx_buf:(%llx)\n",
+			 i, (unsigned long long)ctx_buf);
+
+		if (!ctx_buf) {
+			pr_warn("queue[%d], ctx_buf is NULL!!\n", i);
+			continue;
+		}
+
+		pr_debug("Get VB2 V4L2 buffer\n");
+		b = mtk_fd_ctx_buffer_get_vb2_v4l2_buffer(ctx_buf);
+
+		ctx_buf->image = dev_ctx->queue[ctx_buf->queue].desc.image;
+		ctx_buf->capture = dev_ctx->queue[ctx_buf->queue].desc.capture;
+		/* copy the fmt setting for queue's fmt*/
+		ctx_buf->fmt = dev_ctx->queue[ctx_buf->queue].fmt;
+		ctx_buf->ctx_fmt = dev_ctx->queue[ctx_buf->queue].ctx_fmt;
+			ctx_buf->frame_id = bundle->id;
+		ctx_buf->daddr =
+			vb2_dma_contig_plane_dma_addr(&b->vb2_buf, 0);
+		pr_debug("%s:vb2_buf: type(%d),idx(%d),mem(%d)\n",
+			 __func__,
+			 b->vb2_buf.type,
+			 b->vb2_buf.index,
+			 b->vb2_buf.memory);
+		ctx_buf->vaddr = vb2_plane_vaddr(&b->vb2_buf, 0);
+		ctx_buf->buffer_usage = dev_ctx->queue[i].buffer_usage;
+		ctx_buf->rotation = dev_ctx->queue[i].rotation;
+		pr_debug("Buf: queue(%d), vaddr(%llx), daddr(%llx)",
+			 ctx_buf->queue, (unsigned long long)ctx_buf->vaddr,
+			 (unsigned long long)ctx_buf->daddr);
+
+		if (!ctx_buf->image) {
+			ctx_buf->paddr =
+				mtk_fd_smem_iova_to_phys
+					(&dev_ctx->smem_device->dev,
+					 ctx_buf->daddr);
+		} else {
+			pr_debug("No pa: it is a image buffer\n");
+			ctx_buf->paddr = 0;
+		}
+		ctx_buf->state = MTK_FD_CTX_BUFFER_PROCESSING;
+	}
+
+	if (mtk_fd_ctx_process_frame(dev_ctx, bundle)) {
+		pr_err("mtk_fd_ctx_process_frame failed: frame(%d)\n",
+		       bundle->id);
+		goto FAILE_JOB_NOT_TRIGGER;
+	}
+
+	if (dev_ctx->mode == MTK_FD_CTX_MODE_DEBUG_BYPASS_JOB_TRIGGER) {
+		memset(&fram_param, 0, sizeof(struct mtk_fd_ctx_finish_param));
+		fram_param.frame_id = bundle->id;
+		fram_param.state = MTK_FD_CTX_FRAME_DATA_DONE;
+		pr_debug("Ctx(%d) in HW bypass mode, will not trigger hw\n",
+			 dev_ctx->ctx_id);
+
+		mtk_fd_ctx_core_job_finish(dev_ctx,	(void *)&fram_param);
+		return 0;
+	}
+
+	if (mtk_fd_ctx_core_job_start(dev_ctx, bundle))
+		goto FAILE_JOB_NOT_TRIGGER;
+	return 0;
+
+FAILE_JOB_NOT_TRIGGER:
+	pr_debug("FAILE_JOB_NOT_TRIGGER: init fram_param: %llx\n",
+		 (unsigned long long)&fram_param);
+	memset(&fram_param, 0, sizeof(struct mtk_fd_ctx_finish_param));
+	fram_param.frame_id = bundle->id;
+	fram_param.state = MTK_FD_CTX_FRAME_DATA_ERROR;
+	pr_debug("Call mtk_fd_ctx_core_job_finish_cb: fram_param: %llx",
+		 (unsigned long long)&fram_param);
+	mtk_fd_ctx_core_job_finish(dev_ctx, (void *)&fram_param);
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_trigger_job);
diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c b/drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c
new file mode 100644
index 0000000..7e3acf7
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c
@@ -0,0 +1,355 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 Mediatek Corporation.
+ * Copyright (c) 2017 Intel Corporation.
+ * Copyright (C) 2017 Google, Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * MTK_FD-dev is highly based on Intel IPU 3 chrome driver
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <media/videobuf2-dma-contig.h>
+#include "mtk_fd-dev.h"
+
+static struct platform_device *mtk_fd_dev_of_find_smem_dev
+	(struct platform_device *pdev);
+
+/* Initliaze a mtk_fd_dev representing a completed HW FD */
+/* device */
+int mtk_fd_dev_init(struct mtk_fd_dev *fd_dev,
+		    struct platform_device *pdev,
+		    struct media_device *media_dev,
+		    struct v4l2_device *v4l2_dev)
+{
+	int r = 0;
+
+	fd_dev->pdev = pdev;
+
+	mutex_init(&fd_dev->lock);
+	atomic_set(&fd_dev->qbuf_barrier, 0);
+	init_waitqueue_head(&fd_dev->buf_drain_wq);
+
+	/* v4l2 sub-device registration */
+	r = mtk_fd_dev_mem2mem2_init(fd_dev, media_dev, v4l2_dev);
+
+	if (r) {
+		dev_err(&fd_dev->pdev->dev,
+			"failed to create V4L2 devices (%d)\n", r);
+		goto failed_mem2mem2;
+	}
+
+	return 0;
+
+failed_mem2mem2:
+	mutex_destroy(&fd_dev->lock);
+	return r;
+}
+
+int mtk_fd_dev_get_total_node(struct mtk_fd_dev *mtk_fd_dev)
+{
+	return mtk_fd_dev->ctx.queues_attr.total_num;
+}
+
+int mtk_fd_dev_mem2mem2_init(struct mtk_fd_dev *fd_dev,
+			     struct media_device *media_dev,
+			     struct v4l2_device *v4l2_dev)
+{
+	int r, i;
+	const int queue_master = fd_dev->ctx.queues_attr.master;
+
+	pr_info("mem2mem2.name: %s\n", fd_dev->ctx.device_name);
+	fd_dev->mem2mem2.name = fd_dev->ctx.device_name;
+	fd_dev->mem2mem2.model = fd_dev->ctx.device_name;
+	fd_dev->mem2mem2.num_nodes =
+		mtk_fd_dev_get_total_node(fd_dev);
+	fd_dev->mem2mem2.vb2_mem_ops = &vb2_dma_contig_memops;
+	fd_dev->mem2mem2.buf_struct_size =
+		sizeof(struct mtk_fd_dev_buffer);
+
+	fd_dev->mem2mem2.nodes = fd_dev->mem2mem2_nodes;
+	fd_dev->mem2mem2.dev = &fd_dev->pdev->dev;
+
+	for (i = 0; i < fd_dev->ctx.dev_node_num; i++) {
+		fd_dev->mem2mem2.nodes[i].name =
+			mtk_fd_dev_get_node_name(fd_dev, i);
+		fd_dev->mem2mem2.nodes[i].output =
+				(!fd_dev->ctx.queue[i].desc.capture);
+		fd_dev->mem2mem2.nodes[i].immutable = false;
+		fd_dev->mem2mem2.nodes[i].enabled = false;
+		atomic_set(&fd_dev->mem2mem2.nodes[i].sequence, 0);
+	}
+
+	/* Master queue is always enabled */
+	fd_dev->mem2mem2.nodes[queue_master].immutable = true;
+	fd_dev->mem2mem2.nodes[queue_master].enabled = true;
+
+	pr_info("register v4l2 for %llx\n",
+		(unsigned long long)fd_dev);
+	r = mtk_fd_mem2mem2_v4l2_register(fd_dev, media_dev, v4l2_dev);
+
+	if (r) {
+		pr_err("v4l2 init failed, dev(ctx:%d)\n", fd_dev->ctx.ctx_id);
+		return r;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_dev_mem2mem2_init);
+
+void mtk_fd_dev_mem2mem2_exit(struct mtk_fd_dev *fd_dev)
+{
+	mtk_fd_v4l2_unregister(fd_dev);
+}
+EXPORT_SYMBOL_GPL(mtk_fd_dev_mem2mem2_exit);
+
+char *mtk_fd_dev_get_node_name
+	(struct mtk_fd_dev *fd_dev, int node)
+{
+	struct mtk_fd_ctx_queue_desc *mapped_queue_desc =
+		&fd_dev->ctx.queue[node].desc;
+
+	return mapped_queue_desc->name;
+}
+
+/* Get a free buffer from a video node */
+static struct mtk_fd_ctx_buffer __maybe_unused *mtk_fd_dev_queue_getbuf
+	(struct mtk_fd_dev *fd_dev, int node)
+{
+	struct mtk_fd_dev_buffer *buf;
+	int queue = -1;
+
+	if (node > fd_dev->ctx.dev_node_num || node < 0) {
+		dev_err(&fd_dev->pdev->dev, "Invalid mtk_fd_dev node.\n");
+		return NULL;
+	}
+
+	/* Get the corrosponding queue id of the video node */
+	/* Currently the queue id is the same as the node number */
+	queue = node;
+
+	if (queue < 0) {
+		dev_err(&fd_dev->pdev->dev, "Invalid mtk_fd_dev node.\n");
+		return NULL;
+	}
+
+	/* Find first free buffer from the node */
+	list_for_each_entry(buf, &fd_dev->mem2mem2.nodes[node].buffers,
+			    m2m2_buf.list) {
+		if (mtk_fd_ctx_get_buffer_state(&buf->ctx_buf)
+			== MTK_FD_CTX_BUFFER_NEW)
+			return &buf->ctx_buf;
+	}
+
+	/* There were no free buffers*/
+	return NULL;
+}
+
+int mtk_fd_dev_get_queue_id_of_dev_node(struct mtk_fd_dev *fd_dev,
+					struct mtk_fd_dev_video_device *node)
+{
+	return (node - fd_dev->mem2mem2.nodes);
+}
+EXPORT_SYMBOL_GPL(mtk_fd_dev_get_queue_id_of_dev_node);
+
+int mtk_fd_dev_queue_buffers(struct mtk_fd_dev *fd_dev,
+			     bool initial)
+{
+	unsigned int node;
+	int r = 0;
+	struct mtk_fd_dev_buffer *ibuf;
+	struct mtk_fd_ctx_frame_bundle bundle;
+	const int mtk_fd_dev_node_num = mtk_fd_dev_get_total_node(fd_dev);
+	const int queue_master = fd_dev->ctx.queues_attr.master;
+
+	memset(&bundle, 0, sizeof(struct mtk_fd_ctx_frame_bundle));
+
+	pr_info("%s, init(%d)\n", __func__, initial);
+
+	if (!mtk_fd_ctx_is_streaming(&fd_dev->ctx)) {
+		pr_info("%s: stream off, no hw enqueue triggered\n", __func__);
+		return 0;
+	}
+
+	mutex_lock(&fd_dev->lock);
+
+	/* Buffer set is queued to background driver (e.g. DIP, FD, and P1) */
+	/* only when master input buffer is ready */
+	if (!mtk_fd_dev_queue_getbuf(fd_dev, queue_master)) {
+		mutex_unlock(&fd_dev->lock);
+		return 0;
+	}
+
+	/* Check all node from the node after the master node */
+	for (node = (queue_master + 1) % mtk_fd_dev_node_num;
+		1; node = (node + 1) % mtk_fd_dev_node_num) {
+		pr_info("Check node(%d), queue enabled(%d), node enabled(%d)\n",
+			node, fd_dev->queue_enabled[node],
+			fd_dev->mem2mem2.nodes[node].enabled);
+
+		/* May skip some node according the scenario in the future */
+		if (fd_dev->queue_enabled[node] ||
+		    fd_dev->mem2mem2.nodes[node].enabled) {
+			struct mtk_fd_ctx_buffer *buf =
+				mtk_fd_dev_queue_getbuf(fd_dev, node);
+			char *node_name =
+				mtk_fd_dev_get_node_name(fd_dev, node);
+
+			if (!buf) {
+				dev_dbg(&fd_dev->pdev->dev,
+					"No free buffer of enabled node %s\n",
+					node_name);
+				break;
+			}
+
+			/* To show the debug message */
+			ibuf = container_of(buf,
+					    struct mtk_fd_dev_buffer, ctx_buf);
+			dev_dbg(&fd_dev->pdev->dev,
+				"may queue user %s buffer idx(%d) to ctx\n",
+				node_name,
+				ibuf->m2m2_buf.vbb.vb2_buf.index);
+			mtk_fd_ctx_frame_bundle_add(&fd_dev->ctx,
+						    &bundle, buf);
+		}
+
+		/* Stop if there is no free buffer in master input node */
+		if (node == queue_master) {
+			if (mtk_fd_dev_queue_getbuf(fd_dev, queue_master)) {
+				/* Has collected all buffer required */
+				mtk_fd_ctx_trigger_job(&fd_dev->ctx, &bundle);
+			} else {
+				pr_debug("no new buffer found in master node, not trigger job\n");
+				break;
+			}
+		}
+	}
+	mutex_unlock(&fd_dev->lock);
+
+	if (r && r != -EBUSY)
+		goto failed;
+
+	return 0;
+
+failed:
+	/*
+	 * On error, mark all buffers as failed which are not
+	 * yet queued to CSS
+	 */
+	dev_err(&fd_dev->pdev->dev,
+		"failed to queue buffer to ctx on queue %i (%d)\n",
+		node, r);
+
+	if (initial)
+		/* If we were called from streamon(), no need to finish bufs */
+		return r;
+
+	for (node = 0; node < mtk_fd_dev_node_num; node++) {
+		struct mtk_fd_dev_buffer *buf, *buf0;
+
+		if (!fd_dev->queue_enabled[node])
+			continue;	/* Skip disabled queues */
+
+		mutex_lock(&fd_dev->lock);
+		list_for_each_entry_safe
+			(buf, buf0,
+			 &fd_dev->mem2mem2.nodes[node].buffers,
+			 m2m2_buf.list) {
+			if (mtk_fd_ctx_get_buffer_state(&buf->ctx_buf) ==
+				MTK_FD_CTX_BUFFER_PROCESSING)
+				continue;	/* Was already queued, skip */
+
+			mtk_fd_v4l2_buffer_done(&buf->m2m2_buf.vbb.vb2_buf,
+						VB2_BUF_STATE_ERROR);
+		}
+		mutex_unlock(&fd_dev->lock);
+	}
+
+	return r;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_dev_queue_buffers);
+
+int mtk_fd_dev_core_init(struct platform_device *pdev,
+			 struct mtk_fd_dev *fd_dev,
+			 struct mtk_fd_ctx_desc *ctx_desc)
+{
+	return mtk_fd_dev_core_init_ext(pdev,
+		fd_dev, ctx_desc, NULL, NULL);
+}
+EXPORT_SYMBOL_GPL(mtk_fd_dev_core_init);
+
+int mtk_fd_dev_core_init_ext(struct platform_device *pdev,
+			     struct mtk_fd_dev *fd_dev,
+			     struct mtk_fd_ctx_desc *ctx_desc,
+			     struct media_device *media_dev,
+			     struct v4l2_device *v4l2_dev)
+{
+	int r;
+	struct platform_device *smem_dev = NULL;
+
+	smem_dev = mtk_fd_dev_of_find_smem_dev(pdev);
+
+	if (!smem_dev)
+		dev_err(&pdev->dev, "failed to find smem_dev\n");
+
+	/* Device context must be initialized before device instance */
+	r = mtk_fd_ctx_core_init(&fd_dev->ctx, pdev,
+				 0, ctx_desc, pdev, smem_dev);
+
+	dev_info(&pdev->dev, "init fd_dev: %llx\n",
+		 (unsigned long long)fd_dev);
+	/* init other device level members */
+	mtk_fd_dev_init(fd_dev, pdev, media_dev, v4l2_dev);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_dev_core_init_ext);
+
+int mtk_fd_dev_core_release(struct platform_device *pdev,
+			    struct mtk_fd_dev *fd_dev)
+{
+	mtk_fd_dev_mem2mem2_exit(fd_dev);
+	mtk_fd_ctx_core_exit(&fd_dev->ctx);
+	mutex_destroy(&fd_dev->lock);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_dev_core_release);
+
+static struct platform_device *mtk_fd_dev_of_find_smem_dev
+	(struct platform_device *pdev)
+{
+	struct device_node *smem_dev_node = NULL;
+
+	if (!pdev) {
+		pr_err("Find_smem_dev failed, pdev can't be NULL\n");
+		return NULL;
+	}
+
+	smem_dev_node = of_parse_phandle(pdev->dev.of_node,
+					 "smem_device", 0);
+
+	if (!smem_dev_node) {
+		dev_err(&pdev->dev,
+			"failed to find isp smem device for (%s)\n",
+			pdev->name);
+		return NULL;
+	}
+
+	dev_dbg(&pdev->dev, "smem of node found, try to discovery device\n");
+	return of_find_device_by_node(smem_dev_node);
+}
+
diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h b/drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h
new file mode 100644
index 0000000..d2b7d77
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h
@@ -0,0 +1,198 @@
+/* SPDX-License-Identifier: GPL-2.0
+ * Copyright (c) 2018 Mediatek Corporation.
+ * Copyright (c) 2017 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * MTK_FD-dev is highly based on Intel IPU 3 chrome driver
+ *
+ */
+
+#ifndef __MTK_FD_DEV_H__
+#define __MTK_FD_DEV_H__
+
+#include <linux/platform_device.h>
+#include <linux/version.h>
+#include <media/v4l2-device.h>
+#include <media/videobuf2-v4l2.h>
+#include "mtk_fd-ctx.h"
+
+/* Added the macro for early stage verification */
+/* based on kernel 4.4 environment. */
+/* I will remove the version check after getting */
+/* the devlopment platform based on 4.14 */
+#define MTK_FD_KERNEL_BASE_VERSION KERNEL_VERSION(4, 14, 0)
+
+#define MTK_FD_DEV_NODE_MAX			(MTK_FD_CTX_QUEUES)
+
+#define MTK_FD_INPUT_MIN_WIDTH		0U
+#define MTK_FD_INPUT_MIN_HEIGHT		0U
+#define MTK_FD_INPUT_MAX_WIDTH		480U
+#define MTK_FD_INPUT_MAX_HEIGHT		640U
+#define MTK_FD_OUTPUT_MIN_WIDTH		2U
+#define MTK_FD_OUTPUT_MIN_HEIGHT		2U
+#define MTK_FD_OUTPUT_MAX_WIDTH		480U
+#define MTK_FD_OUTPUT_MAX_HEIGHT		640U
+
+#define file_to_mtk_fd_node(__file) \
+	container_of(video_devdata(__file),\
+	struct mtk_fd_dev_video_device, vdev)
+
+#define mtk_fd_ctx_to_dev(__ctx) \
+	container_of(__ctx,\
+	struct mtk_fd_dev, ctx)
+
+#define mtk_fd_m2m_to_dev(__m2m) \
+	container_of(__m2m,\
+	struct mtk_fd_dev, mem2mem2)
+
+#define mtk_fd_subdev_to_dev(__sd) \
+	container_of(__sd, \
+	struct mtk_fd_dev, mem2mem2.subdev)
+
+#define mtk_fd_vbq_to_isp_node(__vq) \
+	container_of(__vq, \
+	struct mtk_fd_dev_video_device, vbq)
+
+#define mtk_fd_ctx_buf_to_dev_buf(__ctx_buf) \
+	container_of(__ctx_buf, \
+	struct mtk_fd_dev_buffer, ctx_buf)
+
+#define mtk_fd_vb2_buf_to_dev_buf(__vb) \
+	container_of(vb, \
+	struct mtk_fd_dev_buffer, \
+	m2m2_buf.vbb.vb2_buf)
+
+#define mtk_fd_vb2_buf_to_m2m_buf(__vb) \
+	container_of(__vb, \
+	struct mtk_fd_mem2mem2_buffer, \
+	vbb.vb2_buf)
+
+#define mtk_fd_subdev_to_m2m(__sd) \
+	container_of(__sd, \
+	struct mtk_fd_mem2mem2_device, subdev)
+
+struct mtk_fd_mem2mem2_device;
+
+struct mtk_fd_mem2mem2_buffer {
+	struct vb2_v4l2_buffer vbb;
+	struct list_head list;
+};
+
+struct mtk_fd_dev_buffer {
+	struct mtk_fd_mem2mem2_buffer m2m2_buf;
+	/* Intenal part */
+	struct mtk_fd_ctx_buffer ctx_buf;
+};
+
+struct mtk_fd_dev_video_device {
+	const char *name;
+	int output;
+	int immutable;
+	int enabled;
+	int queued;
+	struct v4l2_format vdev_fmt;
+	struct video_device vdev;
+	struct media_pad vdev_pad;
+	struct v4l2_mbus_framefmt pad_fmt;
+	struct vb2_queue vbq;
+	struct list_head buffers;
+	struct mutex lock; /* Protect node data */
+	atomic_t sequence;
+};
+
+struct mtk_fd_mem2mem2_device {
+	const char *name;
+	const char *model;
+	struct device *dev;
+	int num_nodes;
+	struct mtk_fd_dev_video_device *nodes;
+	const struct vb2_mem_ops *vb2_mem_ops;
+	unsigned int buf_struct_size;
+	int streaming;
+	struct v4l2_device *v4l2_dev;
+	struct media_device *media_dev;
+	struct media_pipeline pipeline;
+	struct v4l2_subdev subdev;
+	struct media_pad *subdev_pads;
+	struct v4l2_file_operations v4l2_file_ops;
+	const struct file_operations fops;
+};
+
+struct mtk_fd_dev {
+	struct platform_device *pdev;
+	struct mtk_fd_dev_video_device mem2mem2_nodes[MTK_FD_DEV_NODE_MAX];
+	int queue_enabled[MTK_FD_DEV_NODE_MAX];
+	struct mtk_fd_mem2mem2_device mem2mem2;
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+	struct mtk_fd_ctx ctx;
+	struct mutex lock; /* queue protection */
+	atomic_t qbuf_barrier;
+	struct {
+		struct v4l2_rect eff;
+		struct v4l2_rect bds;
+		struct v4l2_rect gdc;
+	} rect;
+	int suspend_in_stream;
+	wait_queue_head_t buf_drain_wq;
+};
+
+int mtk_fd_media_register(struct device *dev,
+			  struct media_device *media_dev,
+			  const char *model);
+
+int mtk_fd_v4l2_register(struct device *dev,
+			 struct media_device *media_dev,
+			 struct v4l2_device *v4l2_dev);
+
+int mtk_fd_v4l2_unregister(struct mtk_fd_dev *dev);
+
+int mtk_fd_mem2mem2_v4l2_register(struct mtk_fd_dev *dev,
+				  struct media_device *media_dev,
+				  struct v4l2_device *v4l2_dev);
+
+void mtk_fd_v4l2_buffer_done(struct vb2_buffer *vb,
+			     enum vb2_buffer_state state);
+
+int mtk_fd_dev_queue_buffers(struct mtk_fd_dev *dev, bool initial);
+
+int mtk_fd_dev_get_total_node(struct mtk_fd_dev *mtk_fd_dev);
+
+char *mtk_fd_dev_get_node_name(struct mtk_fd_dev *mtk_fd_dev_obj, int node);
+
+int mtk_fd_dev_init(struct mtk_fd_dev *fd_dev,
+		    struct platform_device *pdev,
+		    struct media_device *media_dev,
+		    struct v4l2_device *v4l2_dev);
+
+void mtk_fd_dev_mem2mem2_exit(struct mtk_fd_dev *mtk_fd_dev_obj);
+
+int mtk_fd_dev_mem2mem2_init(struct mtk_fd_dev *fd_dev,
+			     struct media_device *media_dev,
+			     struct v4l2_device *v4l2_dev);
+
+int mtk_fd_dev_get_queue_id_of_dev_node(struct mtk_fd_dev *mtk_fd_dev_obj,
+					struct mtk_fd_dev_video_device *node);
+
+int mtk_fd_dev_core_init(struct platform_device *pdev,
+			 struct mtk_fd_dev *fd_dev,
+			 struct mtk_fd_ctx_desc *ctx_desc);
+
+int mtk_fd_dev_core_init_ext(struct platform_device *pdev,
+			     struct mtk_fd_dev *fd_dev,
+			     struct mtk_fd_ctx_desc *ctx_desc,
+			     struct media_device *media_dev,
+			     struct v4l2_device *v4l2_dev);
+
+int mtk_fd_dev_core_release(struct platform_device *pdev,
+			    struct mtk_fd_dev *fd_dev);
+
+#endif /* __MTK_FD_DEV_H__ */
diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-smem-drv.c b/drivers/media/platform/mtk-isp/fd/mtk_fd-smem-drv.c
new file mode 100644
index 0000000..99a852d
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-smem-drv.c
@@ -0,0 +1,452 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Frederic Chen <frederic.chen@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/of_fdt.h>
+#include <linux/of_reserved_mem.h>
+#include <linux/dma-contiguous.h>
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/iommu.h>
+#include <asm/cacheflush.h>
+
+#define MTK_FD_SMEM_DEV_NAME "MTK-FD-SMEM"
+
+struct mtk_fd_smem_drv {
+	struct platform_device *pdev;
+	struct sg_table sgt;
+	struct page **smem_pages;
+	int num_smem_pages;
+	phys_addr_t smem_base;
+	dma_addr_t smem_dma_base;
+	int smem_size;
+};
+
+static struct reserved_mem *isp_reserved_smem;
+
+static int mtk_fd_smem_setup_dma_ops(struct device *smem_dev,
+				     const struct dma_map_ops *smem_ops);
+
+static int mtk_fd_smem_get_sgtable(struct device *dev,
+				   struct sg_table *sgt,
+				   void *cpu_addr, dma_addr_t dma_addr,
+				   size_t size, unsigned long attrs);
+
+static const struct dma_map_ops smem_dma_ops = {
+	.get_sgtable = mtk_fd_smem_get_sgtable,
+};
+
+static int mtk_fd_smem_init(struct mtk_fd_smem_drv **mtk_fd_smem_drv_out,
+			    struct platform_device *pdev)
+{
+	struct mtk_fd_smem_drv *isp_sys = NULL;
+	struct device *dev = &pdev->dev;
+
+	isp_sys = devm_kzalloc(dev,
+			       sizeof(*isp_sys), GFP_KERNEL);
+
+	isp_sys->pdev = pdev;
+
+	*mtk_fd_smem_drv_out = isp_sys;
+
+	return 0;
+}
+
+static int mtk_fd_smem_drv_probe(struct platform_device *pdev)
+{
+	struct mtk_fd_smem_drv *smem_drv = NULL;
+	int r = 0;
+	struct device *dev = &pdev->dev;
+
+	dev_dbg(dev, "probe mtk_fd_smem_drv\n");
+
+	r = mtk_fd_smem_init(&smem_drv, pdev);
+
+	if (!smem_drv)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, smem_drv);
+
+	if (isp_reserved_smem) {
+		dma_addr_t dma_addr;
+		phys_addr_t addr;
+		struct iommu_domain *smem_dom;
+		int i = 0;
+		int size_align = 0;
+		struct page **pages = NULL;
+		int n_pages = 0;
+		struct sg_table *sgt = &smem_drv->sgt;
+
+		size_align = round_down(isp_reserved_smem->size,
+					PAGE_SIZE);
+		n_pages = size_align >> PAGE_SHIFT;
+
+		pages = kmalloc_array(n_pages, sizeof(struct page *),
+				      GFP_KERNEL);
+
+		if (!pages)
+			return -ENOMEM;
+
+		for (i = 0; i < n_pages; i++)
+			pages[i] = phys_to_page(isp_reserved_smem->base
+						+ i * PAGE_SIZE);
+
+		r = sg_alloc_table_from_pages(sgt, pages, n_pages, 0,
+					      size_align, GFP_KERNEL);
+
+		if (r) {
+			dev_err(dev, "failed to get alloca sg table\n");
+			return -ENOMEM;
+		}
+
+		dma_map_sg_attrs(dev, sgt->sgl, sgt->nents,
+				 DMA_BIDIRECTIONAL,
+				 DMA_ATTR_SKIP_CPU_SYNC);
+
+		dma_addr = sg_dma_address(sgt->sgl);
+		smem_dom = iommu_get_domain_for_dev(dev);
+		addr = iommu_iova_to_phys(smem_dom, dma_addr);
+
+		if (addr != isp_reserved_smem->base)
+			dev_err(dev,
+				"incorrect pa(%llx) from iommu_iova_to_phys, should be %llx\n",
+			(unsigned long long)addr,
+			(unsigned long long)isp_reserved_smem->base);
+
+		r = dma_declare_coherent_memory(dev,
+						isp_reserved_smem->base,
+			dma_addr, size_align, DMA_MEMORY_EXCLUSIVE);
+
+		dev_dbg(dev,
+			"Coherent mem base(%llx,%llx),size(%lx),ret(%d)\n",
+			isp_reserved_smem->base,
+			dma_addr, size_align, r);
+
+		smem_drv->smem_base = isp_reserved_smem->base;
+		smem_drv->smem_size = size_align;
+		smem_drv->smem_pages = pages;
+		smem_drv->num_smem_pages = n_pages;
+		smem_drv->smem_dma_base = dma_addr;
+
+		dev_dbg(dev, "smem_drv setting (%llx,%lx,%llx,%d)\n",
+			smem_drv->smem_base, smem_drv->smem_size,
+			(unsigned long long)smem_drv->smem_pages,
+			smem_drv->num_smem_pages);
+	}
+
+	r = mtk_fd_smem_setup_dma_ops(dev, &smem_dma_ops);
+
+	return r;
+}
+
+phys_addr_t mtk_fd_smem_iova_to_phys(struct device *dev,
+				     dma_addr_t iova)
+{
+		struct iommu_domain *smem_dom;
+		phys_addr_t addr;
+		phys_addr_t limit;
+		struct mtk_fd_smem_drv *smem_dev =
+			dev_get_drvdata(dev);
+
+		if (!smem_dev)
+			return 0;
+
+		smem_dom = iommu_get_domain_for_dev(dev);
+
+		if (!smem_dom)
+			return 0;
+
+		addr = iommu_iova_to_phys(smem_dom, iova);
+
+		limit = smem_dev->smem_base + smem_dev->smem_size;
+
+		if (addr < smem_dev->smem_base || addr >= limit) {
+			dev_err(dev,
+				"Unexpected paddr %pa (must >= %pa and <%pa)\n",
+				&addr, &smem_dev->smem_base, &limit);
+			return 0;
+		}
+		dev_dbg(dev, "Pa verifcation pass: %pa(>=%pa, <%pa)\n",
+			&addr, &smem_dev->smem_base, &limit);
+		return addr;
+}
+
+static int mtk_fd_smem_drv_remove(struct platform_device *pdev)
+{
+	struct mtk_fd_smem_drv *smem_drv =
+		dev_get_drvdata(&pdev->dev);
+
+	kfree(smem_drv->smem_pages);
+	return 0;
+}
+
+static int mtk_fd_smem_drv_suspend(struct device *dev)
+{
+	return 0;
+}
+
+static int mtk_fd_smem_drv_resume(struct device *dev)
+{
+	return 0;
+}
+
+static int mtk_fd_smem_drv_dummy_cb(struct device *dev)
+{
+	return 0;
+}
+
+static const struct dev_pm_ops mtk_fd_smem_drv_pm_ops = {
+	SET_RUNTIME_PM_OPS(&mtk_fd_smem_drv_dummy_cb,
+			   &mtk_fd_smem_drv_dummy_cb, NULL)
+	SET_SYSTEM_SLEEP_PM_OPS
+		(&mtk_fd_smem_drv_suspend, &mtk_fd_smem_drv_resume)
+};
+
+static const struct of_device_id mtk_fd_smem_drv_of_match[] = {
+	{
+		.compatible = "mediatek,fd_smem",
+	},
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, mtk_fd_smem_drv_of_match);
+
+static struct platform_driver mtk_fd_smem_driver = {
+	.probe = mtk_fd_smem_drv_probe,
+	.remove = mtk_fd_smem_drv_remove,
+	.driver = {
+		.name = MTK_FD_SMEM_DEV_NAME,
+		.of_match_table =
+			of_match_ptr(mtk_fd_smem_drv_of_match),
+		.pm = &mtk_fd_smem_drv_pm_ops,
+	},
+};
+
+static int __init mtk_fd_smem_dma_setup(struct reserved_mem
+					*rmem)
+{
+	unsigned long node = rmem->fdt_node;
+
+	if (of_get_flat_dt_prop(node, "reusable", NULL))
+		return -EINVAL;
+
+	if (!of_get_flat_dt_prop(node, "no-map", NULL)) {
+		pr_err("Reserved memory: regions without no-map are not yet supported\n");
+		return -EINVAL;
+	}
+
+	isp_reserved_smem = rmem;
+
+	pr_debug("Reserved memory: created DMA memory pool at %pa, size %ld MiB\n",
+		 &rmem->base, (unsigned long)rmem->size / SZ_1M);
+	return 0;
+}
+
+RESERVEDMEM_OF_DECLARE(mtk_fd_smem,
+		       "mediatek,reserve-memory-fd_smem",
+		       mtk_fd_smem_dma_setup);
+
+int __init mtk_fd_smem_drv_init(void)
+{
+	int ret = 0;
+
+	pr_debug("platform_driver_register: mtk_fd_smem_driver\n");
+	ret = platform_driver_register(&mtk_fd_smem_driver);
+
+	if (ret)
+		pr_warn("fd smem drv init failed, driver didn't probe\n");
+
+	return ret;
+}
+subsys_initcall(mtk_fd_smem_drv_init);
+
+void __exit mtk_fd_smem_drv_ext(void)
+{
+	platform_driver_unregister(&mtk_fd_smem_driver);
+}
+module_exit(mtk_fd_smem_drv_ext);
+
+/********************************************
+ * MTK FD SMEM DMA ops *
+ ********************************************/
+
+struct dma_coherent_mem {
+	void		*virt_base;
+	dma_addr_t	device_base;
+	unsigned long	pfn_base;
+	int		size;
+	int		flags;
+	unsigned long	*bitmap;
+	spinlock_t	spinlock; /* protect the members in dma_coherent_mem */
+	bool		use_dev_dma_pfn_offset;
+};
+
+static struct dma_coherent_mem *dev_get_coherent_memory(struct device *dev)
+{
+	if (dev && dev->dma_mem)
+		return dev->dma_mem;
+	return NULL;
+}
+
+static int mtk_fd_smem_get_sgtable(struct device *dev,
+				   struct sg_table *sgt,
+	void *cpu_addr, dma_addr_t dma_addr,
+	size_t size, unsigned long attrs)
+{
+	struct mtk_fd_smem_drv *smem_dev = dev_get_drvdata(dev);
+	int n_pages_align = 0;
+	int size_align = 0;
+	int page_start = 0;
+	unsigned long long offset_p = 0;
+	unsigned long long offset_d = 0;
+
+	phys_addr_t paddr = mtk_fd_smem_iova_to_phys(dev, dma_addr);
+
+	offset_d = (unsigned long long)dma_addr -
+		(unsigned long long)smem_dev->smem_dma_base;
+
+	offset_p = (unsigned long long)paddr -
+		(unsigned long long)smem_dev->smem_base;
+
+	dev_dbg(dev, "%s:dma_addr:%llx,cpu_addr:%llx,pa:%llx,size:%d\n",
+		__func__,
+		(unsigned long long)dma_addr,
+		(unsigned long long)cpu_addr,
+		(unsigned long long)paddr,
+		size
+		);
+
+	dev_dbg(dev, "%s:offset p:%llx,offset d:%llx\n",
+		__func__,
+		(unsigned long long)offset_p,
+		(unsigned long long)offset_d
+		);
+
+	size_align = round_up(size, PAGE_SIZE);
+	n_pages_align = size_align >> PAGE_SHIFT;
+	page_start = offset_p >> PAGE_SHIFT;
+
+	dev_dbg(dev,
+		"%s:page idx:%d,page pa:%llx,pa:%llx, aligned size:%d\n",
+		__func__,
+		page_start,
+		(unsigned long long)page_to_phys(*(smem_dev->smem_pages
+			+ page_start)),
+		(unsigned long long)paddr,
+		size_align
+		);
+
+	if (!smem_dev) {
+		dev_err(dev, "can't get sgtable from smem_dev\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(dev, "get sgt of the smem: %d pages\n", n_pages_align);
+
+	return sg_alloc_table_from_pages(sgt,
+		smem_dev->smem_pages + page_start,
+		n_pages_align,
+		0, size_align, GFP_KERNEL);
+}
+
+static void *mtk_fd_smem_get_cpu_addr(struct mtk_fd_smem_drv *smem_dev,
+				      struct scatterlist *sg)
+{
+	struct device *dev = &smem_dev->pdev->dev;
+	struct dma_coherent_mem *dma_mem =
+		dev_get_coherent_memory(dev);
+
+	phys_addr_t addr = (phys_addr_t)sg_phys(sg);
+
+	if (addr < smem_dev->smem_base ||
+	    addr > smem_dev->smem_base + smem_dev->smem_size) {
+		dev_err(dev, "Invalid paddr 0x%llx from sg\n", addr);
+		return NULL;
+	}
+
+	return dma_mem->virt_base + (addr - smem_dev->smem_base);
+}
+
+static void mtk_fd_smem_sync_sg_for_cpu(struct device *dev,
+					struct scatterlist *sgl, int nelems,
+					enum dma_data_direction dir)
+{
+	struct mtk_fd_smem_drv *smem_dev =
+		dev_get_drvdata(dev);
+	void *cpu_addr;
+
+	cpu_addr = mtk_fd_smem_get_cpu_addr(smem_dev, sgl);
+
+	dev_dbg(dev,
+		"__dma_unmap_area:paddr(0x%llx),vaddr(0x%llx),size(%d)\n",
+		(unsigned long long)sg_phys(sgl),
+		(unsigned long long)cpu_addr,
+		sgl->length);
+
+	__dma_unmap_area(cpu_addr, sgl->length, dir);
+}
+
+static void mtk_fd_smem_sync_sg_for_device(struct device *dev,
+					   struct scatterlist *sgl, int nelems,
+					   enum dma_data_direction dir)
+{
+	struct mtk_fd_smem_drv *smem_dev =
+			dev_get_drvdata(dev);
+	void *cpu_addr;
+
+	cpu_addr = mtk_fd_smem_get_cpu_addr(smem_dev, sgl);
+
+	dev_dbg(dev,
+		"__dma_map_area:paddr(0x%llx),vaddr(0x%llx),size(%d)\n",
+		(unsigned long long)sg_phys(sgl),
+		(unsigned long long)cpu_addr,
+		sgl->length);
+
+	__dma_map_area(cpu_addr, sgl->length, dir);
+}
+
+static int mtk_fd_smem_setup_dma_ops(struct device *dev,
+				     const struct dma_map_ops *smem_ops)
+{
+	if (!dev->dma_ops)
+		return -EINVAL;
+
+	memcpy((void *)smem_ops, dev->dma_ops, sizeof(*smem_ops));
+
+	((struct dma_map_ops *)smem_ops)->get_sgtable =
+		mtk_fd_smem_get_sgtable;
+	((struct dma_map_ops *)smem_ops)->sync_sg_for_device =
+		mtk_fd_smem_sync_sg_for_device;
+	((struct dma_map_ops *)smem_ops)->sync_sg_for_cpu =
+		mtk_fd_smem_sync_sg_for_cpu;
+
+	dev->dma_ops = smem_ops;
+
+	return 0;
+}
+
+void mtk_fd_smem_enable_mpu(struct device *dev)
+{
+	dev_warn(dev, "MPU enabling func is not ready now\n");
+}
+
+MODULE_AUTHOR("Frederic Chen <frederic.chen@mediatek.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Mediatek FD shared memory driver");
diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h b/drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h
new file mode 100644
index 0000000..a19fc37
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Frederic Chen <frederic.chen@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MTK_FD_SMEM_H__
+#define __MTK_FD_SMEM_H__
+
+#include <linux/dma-mapping.h>
+
+phys_addr_t mtk_fd_smem_iova_to_phys(struct device *smem_dev,
+				     dma_addr_t iova);
+void mtk_fd_smem_enable_mpu(struct device *smem_dev);
+
+#endif /*__MTK_FD_SMEM_H__*/
diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c b/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c
new file mode 100644
index 0000000..ab85aea
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c
@@ -0,0 +1,1046 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 Mediatek Corporation.
+ * Copyright (c) 2017 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * MTK_FD-v4l2 is highly based on Intel IPU 3 chrome driver
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-event.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+
+#include "mtk_fd-dev.h"
+
+static u32 mtk_fd_node_get_v4l2_cap
+	(struct mtk_fd_ctx_queue *node_ctx);
+
+static int mtk_fd_videoc_s_meta_fmt(struct file *file, void *fh,
+				    struct v4l2_format *f);
+
+static int mtk_fd_subdev_open(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_fh *fh)
+{
+	struct mtk_fd_dev *fd_dev = mtk_fd_subdev_to_dev(sd);
+
+	fd_dev->ctx.fh = fh;
+
+	return mtk_fd_ctx_open(&fd_dev->ctx);
+}
+
+static int mtk_fd_subdev_close(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_fh *fh)
+{
+	struct mtk_fd_dev *fd_dev = mtk_fd_subdev_to_dev(sd);
+
+	return mtk_fd_ctx_release(&fd_dev->ctx);
+}
+
+static int mtk_fd_subdev_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	int ret = 0;
+
+	struct mtk_fd_dev *fd_dev = mtk_fd_subdev_to_dev(sd);
+
+	if (enable) {
+		ret = mtk_fd_ctx_streamon(&fd_dev->ctx);
+
+		if (!ret)
+			ret = mtk_fd_dev_queue_buffers
+				(mtk_fd_ctx_to_dev(&fd_dev->ctx), true);
+		if (ret)
+			pr_err("failed to queue initial buffers (%d)", ret);
+	}	else {
+		ret = mtk_fd_ctx_streamoff(&fd_dev->ctx);
+	}
+
+	if (!ret)
+		fd_dev->mem2mem2.streaming = enable;
+
+	return ret;
+}
+
+static int mtk_fd_link_setup(struct media_entity *entity,
+			     const struct media_pad *local,
+			     const struct media_pad *remote, u32 flags)
+{
+	struct mtk_fd_mem2mem2_device *m2m2 =
+		container_of(entity, struct mtk_fd_mem2mem2_device,
+			     subdev.entity);
+	struct mtk_fd_dev *fd_dev =
+		container_of(m2m2, struct mtk_fd_dev, mem2mem2);
+
+	u32 pad = local->index;
+
+	pr_info("link setup: %d --> %d\n", pad, remote->index);
+
+#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
+	WARN_ON(entity->type != MEDIA_ENT_T_V4L2_SUBDEV);
+#else
+	WARN_ON(entity->obj_type != MEDIA_ENTITY_TYPE_V4L2_SUBDEV);
+#endif
+
+	WARN_ON(pad >= m2m2->num_nodes);
+
+	m2m2->nodes[pad].enabled = !!(flags & MEDIA_LNK_FL_ENABLED);
+
+	/**
+	 * queue_enable can be phase out in the future since
+	 * we don't have internal queue of each node in
+	 * v4l2 common module
+	 */
+	fd_dev->queue_enabled[pad] = m2m2->nodes[pad].enabled;
+
+	return 0;
+}
+
+static void mtk_fd_vb2_buf_queue(struct vb2_buffer *vb)
+{
+	struct mtk_fd_mem2mem2_device *m2m2 = vb2_get_drv_priv(vb->vb2_queue);
+	struct mtk_fd_dev *mtk_fd_dev = mtk_fd_m2m_to_dev(m2m2);
+	struct device *dev = &mtk_fd_dev->pdev->dev;
+	struct mtk_fd_dev_buffer *buf = NULL;
+	struct vb2_v4l2_buffer *v4l2_buf = NULL;
+	struct mtk_fd_dev_video_device *node =
+		mtk_fd_vbq_to_isp_node(vb->vb2_queue);
+	int queue = mtk_fd_dev_get_queue_id_of_dev_node(mtk_fd_dev, node);
+
+	dev_dbg(dev,
+		"queue vb2_buf: Node(%s) queue id(%d)\n",
+		node->name,
+		queue);
+
+	if (queue < 0) {
+		dev_err(m2m2->dev, "Invalid mtk_fd_dev node.\n");
+		return;
+	}
+
+	if (mtk_fd_dev->ctx.mode == MTK_FD_CTX_MODE_DEBUG_BYPASS_ALL) {
+		dev_dbg(m2m2->dev, "By pass mode, just loop back the buffer\n");
+		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+		return;
+	}
+
+	if (!vb)
+		pr_err("VB can't be null\n");
+
+	buf = mtk_fd_vb2_buf_to_dev_buf(vb);
+
+	if (!buf)
+		pr_err("buf can't be null\n");
+
+	v4l2_buf = to_vb2_v4l2_buffer(vb);
+
+	if (!v4l2_buf)
+		pr_err("v4l2_buf can't be null\n");
+
+	mutex_lock(&mtk_fd_dev->lock);
+
+	pr_err("init  mtk_fd_ctx_buf_init, sequence(%d)\n", v4l2_buf->sequence);
+
+	/* the dma address will be filled in later frame buffer handling*/
+	mtk_fd_ctx_buf_init(&buf->ctx_buf, queue, (dma_addr_t)0);
+	pr_info("set mtk_fd_ctx_buf_init: user seq=%d\n",
+		buf->ctx_buf.user_sequence);
+
+	/* Added the buffer into the tracking list */
+	list_add_tail(&buf->m2m2_buf.list,
+		      &m2m2->nodes[node - m2m2->nodes].buffers);
+	mutex_unlock(&mtk_fd_dev->lock);
+
+	/* Enqueue the buffer */
+	if (mtk_fd_dev->mem2mem2.streaming) {
+		pr_info("%s: mtk_fd_dev_queue_buffers\n", node->name);
+		mtk_fd_dev_queue_buffers(mtk_fd_dev, false);
+	}
+}
+
+#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
+static int mtk_fd_vb2_queue_setup(struct vb2_queue *vq,
+				  const void *parg,
+				  unsigned int *num_buffers,
+				  unsigned int *num_planes,
+				  unsigned int sizes[], void *alloc_ctxs[])
+#else
+static int mtk_fd_vb2_queue_setup(struct vb2_queue *vq,
+				  unsigned int *num_buffers,
+				  unsigned int *num_planes,
+				  unsigned int sizes[],
+				  struct device *alloc_devs[])
+#endif
+{
+	struct mtk_fd_mem2mem2_device *m2m2 = vb2_get_drv_priv(vq);
+	struct mtk_fd_dev_video_device *node = mtk_fd_vbq_to_isp_node(vq);
+	struct mtk_fd_dev *fd_dev = mtk_fd_m2m_to_dev(m2m2);
+	void *buf_alloc_ctx = NULL;
+	int queue = mtk_fd_dev_get_queue_id_of_dev_node(fd_dev, node);
+	/* Get V4L2 format with the following method */
+	const struct v4l2_format *fmt = &node->vdev_fmt;
+
+	*num_planes = 1;
+	*num_buffers = clamp_val(*num_buffers, 1, VB2_MAX_FRAME);
+
+	vq->dma_attrs |= DMA_ATTR_NON_CONSISTENT;
+	pr_info("queue(%d): cached mmap enabled\n", queue);
+
+	if (vq->type == V4L2_BUF_TYPE_META_CAPTURE ||
+	    vq->type == V4L2_BUF_TYPE_META_OUTPUT) {
+		sizes[0] = fmt->fmt.meta.buffersize;
+		buf_alloc_ctx = fd_dev->ctx.smem_vb2_alloc_ctx;
+		pr_info("Select smem_vb2_alloc_ctx(%llx)\n",
+			(unsigned long long)buf_alloc_ctx);
+	} else {
+		sizes[0] = fmt->fmt.pix_mp.plane_fmt[0].sizeimage;
+		buf_alloc_ctx = fd_dev->ctx.img_vb2_alloc_ctx;
+		pr_info("Select img_vb2_alloc_ctx(%llx)\n",
+			(unsigned long long)buf_alloc_ctx);
+	}
+
+#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
+	alloc_ctxs[0] = buf_alloc_ctx;
+#else
+	alloc_devs[0] = (struct device *)buf_alloc_ctx;
+#endif
+
+	pr_info("mtk_fd_vb2_queue_setup:type(%d),size(%d),ctx(%llx)\n",
+		vq->type, sizes[0], (unsigned long long)buf_alloc_ctx);
+
+	/* Initialize buffer queue */
+	INIT_LIST_HEAD(&node->buffers);
+
+	return 0;
+}
+
+static bool mtk_fd_all_nodes_streaming(struct mtk_fd_mem2mem2_device *m2m2,
+				       struct mtk_fd_dev_video_device *except)
+{
+	int i;
+
+	for (i = 0; i < m2m2->num_nodes; i++) {
+		struct mtk_fd_dev_video_device *node = &m2m2->nodes[i];
+
+		if (node == except)
+			continue;
+		if (node->enabled && !vb2_start_streaming_called(&node->vbq))
+			return false;
+	}
+
+	return true;
+}
+
+static void mtk_fd_return_all_buffers(struct mtk_fd_mem2mem2_device *m2m2,
+				      struct mtk_fd_dev_video_device *node,
+				      enum vb2_buffer_state state)
+{
+	struct mtk_fd_dev *mtk_fd_dev = mtk_fd_m2m_to_dev(m2m2);
+	struct mtk_fd_mem2mem2_buffer *b, *b0;
+
+	/* Return all buffers */
+	mutex_lock(&mtk_fd_dev->lock);
+	list_for_each_entry_safe(b, b0, &node->buffers, list) {
+		list_del(&b->list);
+		vb2_buffer_done(&b->vbb.vb2_buf, state);
+	}
+	mutex_unlock(&mtk_fd_dev->lock);
+}
+
+static int mtk_fd_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct mtk_fd_mem2mem2_device *m2m2 = vb2_get_drv_priv(vq);
+	struct mtk_fd_dev_video_device *node =
+		mtk_fd_vbq_to_isp_node(vq);
+	int r;
+
+	if (m2m2->streaming) {
+		r = -EBUSY;
+		goto fail_return_bufs;
+	}
+
+	if (!node->enabled) {
+		pr_err("Node (%ld) is not enable\n", node - m2m2->nodes);
+		r = -EINVAL;
+		goto fail_return_bufs;
+	}
+#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
+	r = media_entity_pipeline_start(&node->vdev.entity, &m2m2->pipeline);
+#else
+	r = media_pipeline_start(&node->vdev.entity, &m2m2->pipeline);
+#endif
+	if (r < 0) {
+		pr_err("Node (%ld) media_pipeline_start failed\n",
+		       node - m2m2->nodes);
+		goto fail_return_bufs;
+	}
+
+	if (!mtk_fd_all_nodes_streaming(m2m2, node))
+		return 0;
+
+	/* Start streaming of the whole pipeline now */
+
+	r = v4l2_subdev_call(&m2m2->subdev, video, s_stream, 1);
+	if (r < 0) {
+		pr_err("Node (%ld) v4l2_subdev_call s_stream failed\n",
+		       node - m2m2->nodes);
+		goto fail_stop_pipeline;
+	}
+	return 0;
+
+fail_stop_pipeline:
+#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
+	media_entity_pipeline_stop(&node->vdev.entity);
+#else
+	media_pipeline_stop(&node->vdev.entity);
+#endif
+fail_return_bufs:
+	mtk_fd_return_all_buffers(m2m2, node, VB2_BUF_STATE_QUEUED);
+
+	return r;
+}
+
+static void mtk_fd_vb2_stop_streaming(struct vb2_queue *vq)
+{
+	struct mtk_fd_mem2mem2_device *m2m2 = vb2_get_drv_priv(vq);
+	struct mtk_fd_dev_video_device *node =
+		mtk_fd_vbq_to_isp_node(vq);
+	int r;
+
+	WARN_ON(!node->enabled);
+
+	/* Was this the first node with streaming disabled? */
+	if (mtk_fd_all_nodes_streaming(m2m2, node)) {
+		/* Yes, really stop streaming now */
+		r = v4l2_subdev_call(&m2m2->subdev, video, s_stream, 0);
+		if (r)
+			dev_err(m2m2->dev, "failed to stop streaming\n");
+	}
+
+	mtk_fd_return_all_buffers(m2m2, node, VB2_BUF_STATE_ERROR);
+#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
+	media_entity_pipeline_stop(&node->vdev.entity);
+#else
+	media_pipeline_stop(&node->vdev.entity);
+#endif
+}
+
+static int mtk_fd_videoc_querycap(struct file *file, void *fh,
+				  struct v4l2_capability *cap)
+{
+	struct mtk_fd_mem2mem2_device *m2m2 = video_drvdata(file);
+	struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
+	struct mtk_fd_dev *fd_dev = mtk_fd_m2m_to_dev(m2m2);
+	int queue_id =
+		mtk_fd_dev_get_queue_id_of_dev_node(fd_dev, node);
+	struct mtk_fd_ctx_queue *node_ctx = &fd_dev->ctx.queue[queue_id];
+
+	strlcpy(cap->driver, m2m2->name, sizeof(cap->driver));
+	strlcpy(cap->card, m2m2->model, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+		 "platform:%s", node->name);
+
+	cap->device_caps =
+		mtk_fd_node_get_v4l2_cap(node_ctx) | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+/* Propagate forward always the format from the CIO2 subdev */
+static int mtk_fd_videoc_g_fmt(struct file *file, void *fh,
+			       struct v4l2_format *f)
+{
+	struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
+
+	f->fmt = node->vdev_fmt.fmt;
+
+	return 0;
+}
+
+static int mtk_fd_videoc_try_fmt(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct mtk_fd_mem2mem2_device *m2m2 = video_drvdata(file);
+	struct mtk_fd_dev *fd_dev = mtk_fd_m2m_to_dev(m2m2);
+	struct mtk_fd_ctx *dev_ctx = &fd_dev->ctx;
+	struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
+	int queue_id =
+		mtk_fd_dev_get_queue_id_of_dev_node(fd_dev, node);
+	int ret = 0;
+
+	ret = mtk_fd_ctx_fmt_set_img(dev_ctx, queue_id, &f->fmt.pix_mp,
+				     &node->vdev_fmt.fmt.pix_mp);
+
+	/* Simply set the format to the node context in the initial version */
+	if (ret) {
+		pr_warn("Fmt(%d) not support for queue(%d), load default fmt\n",
+			f->fmt.pix_mp.pixelformat, queue_id);
+
+		ret = mtk_fd_ctx_format_load_default_fmt
+			(&dev_ctx->queue[queue_id], f);
+	}
+
+	if (!ret) {
+		node->vdev_fmt.fmt.pix_mp = f->fmt.pix_mp;
+		dev_ctx->queue[queue_id].fmt.pix_mp = node->vdev_fmt.fmt.pix_mp;
+	}
+
+	return ret;
+}
+
+static int mtk_fd_videoc_s_fmt(struct file *file, void *fh,
+			       struct v4l2_format *f)
+{
+	struct mtk_fd_mem2mem2_device *m2m2 = video_drvdata(file);
+	struct mtk_fd_dev *fd_dev = mtk_fd_m2m_to_dev(m2m2);
+	struct mtk_fd_ctx *dev_ctx = &fd_dev->ctx;
+	struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
+	int queue_id = mtk_fd_dev_get_queue_id_of_dev_node(fd_dev, node);
+	int ret = 0;
+
+	ret = mtk_fd_ctx_fmt_set_img(dev_ctx, queue_id, &f->fmt.pix_mp,
+				     &node->vdev_fmt.fmt.pix_mp);
+
+	/* Simply set the format to the node context in the initial version */
+	if (!ret)
+		dev_ctx->queue[queue_id].fmt.pix_mp = node->vdev_fmt.fmt.pix_mp;
+	else
+		dev_warn(&fd_dev->pdev->dev, "s_fmt, format not support\n");
+
+	return ret;
+}
+
+static int mtk_fd_meta_enum_format(struct file *file, void *fh,
+				   struct v4l2_fmtdesc *f)
+{
+	struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
+
+	if (f->index > 0 || f->type != node->vbq.type)
+		return -EINVAL;
+
+	f->pixelformat = node->vdev_fmt.fmt.meta.dataformat;
+
+	return 0;
+}
+
+static int mtk_fd_videoc_s_meta_fmt(struct file *file, void *fh,
+				    struct v4l2_format *f)
+{
+	struct mtk_fd_mem2mem2_device *m2m2 = video_drvdata(file);
+	struct mtk_fd_dev *fd_dev = mtk_fd_m2m_to_dev(m2m2);
+	struct mtk_fd_ctx *dev_ctx = &fd_dev->ctx;
+	struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
+	int queue_id = mtk_fd_dev_get_queue_id_of_dev_node(fd_dev, node);
+
+	int ret = 0;
+
+	if (f->type != node->vbq.type)
+		return -EINVAL;
+
+	ret = mtk_fd_ctx_format_load_default_fmt(&dev_ctx->queue[queue_id], f);
+
+	if (!ret) {
+		node->vdev_fmt.fmt.meta = f->fmt.meta;
+		dev_ctx->queue[queue_id].fmt.meta = node->vdev_fmt.fmt.meta;
+	} else {
+		dev_warn(&fd_dev->pdev->dev,
+			 "s_meta_fm failed, format not support\n");
+	}
+
+	return ret;
+}
+
+static int mtk_fd_videoc_g_meta_fmt(struct file *file, void *fh,
+				    struct v4l2_format *f)
+{
+	struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
+
+	if (f->type != node->vbq.type)
+		return -EINVAL;
+
+	f->fmt = node->vdev_fmt.fmt;
+
+	return 0;
+}
+
+int mtk_fd_videoc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct vb2_buffer *vb;
+	struct mtk_fd_dev_buffer *db;
+	int r = 0;
+
+	/* check if vb2 queue is busy */
+	if (vdev->queue->owner && vdev->queue->owner != file->private_data)
+		return -EBUSY;
+
+	/**
+	 * Keep the value of sequence in v4l2_buffer
+	 * in ctx buf since vb2_qbuf will set it to 0
+	 */
+	vb = vdev->queue->bufs[p->index];
+
+	if (vb) {
+		db = mtk_fd_vb2_buf_to_dev_buf(vb);
+		pr_info("qbuf: p:%llx,vb:%llx, db:%llx\n",
+			(unsigned long long)p,
+			(unsigned long long)vb,
+			(unsigned long long)db);
+		db->ctx_buf.user_sequence = p->sequence;
+	}
+	r = vb2_qbuf(vdev->queue, vdev->v4l2_dev->mdev, p);
+	if (r)
+		pr_err("vb2_qbuf failed(err=%d): buf idx(%d)\n", r, p->index);
+	return r;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_videoc_qbuf);
+
+/******************** function pointers ********************/
+
+/* subdev internal operations */
+static const struct v4l2_subdev_internal_ops mtk_fd_subdev_internal_ops = {
+	.open = mtk_fd_subdev_open,
+	.close = mtk_fd_subdev_close,
+};
+
+static const struct v4l2_subdev_core_ops mtk_fd_subdev_core_ops = {
+#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
+	.g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
+	.try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
+	.s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+#endif
+};
+
+static const struct v4l2_subdev_video_ops mtk_fd_subdev_video_ops = {
+	.s_stream = mtk_fd_subdev_s_stream,
+};
+
+static const struct v4l2_subdev_ops mtk_fd_subdev_ops = {
+	.core = &mtk_fd_subdev_core_ops,
+	.video = &mtk_fd_subdev_video_ops,
+};
+
+static const struct media_entity_operations mtk_fd_media_ops = {
+	.link_setup = mtk_fd_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct vb2_ops mtk_fd_vb2_ops = {
+	.buf_queue = mtk_fd_vb2_buf_queue,
+	.queue_setup = mtk_fd_vb2_queue_setup,
+	.start_streaming = mtk_fd_vb2_start_streaming,
+	.stop_streaming = mtk_fd_vb2_stop_streaming,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+};
+
+static const struct v4l2_file_operations mtk_fd_v4l2_fops = {
+	.unlocked_ioctl = video_ioctl2,
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.poll = vb2_fop_poll,
+	.mmap = vb2_fop_mmap,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl32 = v4l2_compat_ioctl32,
+#endif
+};
+
+static const struct v4l2_ioctl_ops mtk_fd_v4l2_ioctl_ops = {
+	.vidioc_querycap = mtk_fd_videoc_querycap,
+
+	.vidioc_g_fmt_vid_cap_mplane = mtk_fd_videoc_g_fmt,
+	.vidioc_s_fmt_vid_cap_mplane = mtk_fd_videoc_s_fmt,
+	.vidioc_try_fmt_vid_cap_mplane = mtk_fd_videoc_try_fmt,
+
+	.vidioc_g_fmt_vid_out_mplane = mtk_fd_videoc_g_fmt,
+	.vidioc_s_fmt_vid_out_mplane = mtk_fd_videoc_s_fmt,
+	.vidioc_try_fmt_vid_out_mplane = mtk_fd_videoc_try_fmt,
+
+	/* buffer queue management */
+	.vidioc_reqbufs = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_qbuf = mtk_fd_videoc_qbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+};
+
+static const struct v4l2_ioctl_ops mtk_fd_v4l2_meta_ioctl_ops = {
+	.vidioc_querycap = mtk_fd_videoc_querycap,
+
+	.vidioc_enum_fmt_meta_cap = mtk_fd_meta_enum_format,
+	.vidioc_g_fmt_meta_cap = mtk_fd_videoc_g_meta_fmt,
+	.vidioc_s_fmt_meta_cap = mtk_fd_videoc_s_meta_fmt,
+	.vidioc_try_fmt_meta_cap = mtk_fd_videoc_g_meta_fmt,
+
+	.vidioc_enum_fmt_meta_out = mtk_fd_meta_enum_format,
+	.vidioc_g_fmt_meta_out = mtk_fd_videoc_g_meta_fmt,
+	.vidioc_s_fmt_meta_out = mtk_fd_videoc_s_meta_fmt,
+	.vidioc_try_fmt_meta_out = mtk_fd_videoc_g_meta_fmt,
+
+	.vidioc_reqbufs = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_qbuf = mtk_fd_videoc_qbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+};
+
+static u32 mtk_fd_node_get_v4l2_cap(struct mtk_fd_ctx_queue *node_ctx)
+{
+	u32 cap = 0;
+
+	if (node_ctx->desc.capture)
+		if (node_ctx->desc.image)
+			cap = V4L2_CAP_VIDEO_CAPTURE_MPLANE;
+		else
+			cap = V4L2_CAP_META_CAPTURE;
+	else
+		if (node_ctx->desc.image)
+			cap = V4L2_CAP_VIDEO_OUTPUT_MPLANE;
+		else
+			cap = V4L2_CAP_META_OUTPUT;
+
+	return cap;
+}
+
+static u32 mtk_fd_node_get_format_type(struct mtk_fd_ctx_queue *node_ctx)
+{
+	u32 type;
+
+	if (node_ctx->desc.capture)
+		if (node_ctx->desc.image)
+			type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+		else
+			type = V4L2_BUF_TYPE_META_CAPTURE;
+	else
+		if (node_ctx->desc.image)
+			type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+		else
+			type = V4L2_BUF_TYPE_META_OUTPUT;
+
+	return type;
+}
+
+static const struct v4l2_ioctl_ops *mtk_fd_node_get_ioctl_ops
+	(struct mtk_fd_ctx_queue *node_ctx)
+{
+	const struct v4l2_ioctl_ops *ops = NULL;
+
+	if (node_ctx->desc.image)
+		ops = &mtk_fd_v4l2_ioctl_ops;
+	else
+		ops = &mtk_fd_v4l2_meta_ioctl_ops;
+	return ops;
+}
+
+/**
+ * Config node's video properties
+ * according to the device context requirement
+ */
+static void mtk_fd_node_to_v4l2(struct mtk_fd_dev *fd_dev, u32 node,
+				struct video_device *vdev,
+				struct v4l2_format *f)
+{
+	u32 cap;
+	struct mtk_fd_ctx *device_ctx = &fd_dev->ctx;
+	struct mtk_fd_ctx_queue *node_ctx = &device_ctx->queue[node];
+
+	WARN_ON(node >= mtk_fd_dev_get_total_node(fd_dev));
+	WARN_ON(!node_ctx);
+
+	/* set cap of the node */
+	cap = mtk_fd_node_get_v4l2_cap(node_ctx);
+	f->type = mtk_fd_node_get_format_type(node_ctx);
+	vdev->ioctl_ops = mtk_fd_node_get_ioctl_ops(node_ctx);
+
+	if (mtk_fd_ctx_format_load_default_fmt(&device_ctx->queue[node], f)) {
+		dev_err(&fd_dev->pdev->dev,
+			"Can't load default for node (%d): (%s)",
+			node, device_ctx->queue[node].desc.name);
+	} else {
+		if (device_ctx->queue[node].desc.image) {
+			dev_dbg(&fd_dev->pdev->dev,
+				"Node (%d): (%s), dfmt (f:0x%x w:%d: h:%d s:%d)\n",
+				node, device_ctx->queue[node].desc.name,
+				f->fmt.pix_mp.pixelformat,
+				f->fmt.pix_mp.width,
+				f->fmt.pix_mp.height,
+				f->fmt.pix_mp.plane_fmt[0].sizeimage);
+			node_ctx->fmt.pix_mp = f->fmt.pix_mp;
+		} else {
+			dev_info(&fd_dev->pdev->dev,
+				 "Node (%d): (%s), dfmt (f:0x%x s:%u)\n",
+				 node, device_ctx->queue[node].desc.name,
+				 f->fmt.meta.dataformat,
+				 f->fmt.meta.buffersize);
+			node_ctx->fmt.meta = f->fmt.meta;
+		}
+	}
+
+#if KERNEL_VERSION(4, 7, 0) < MTK_FD_KERNEL_BASE_VERSION
+	/* device_caps was supported after 4.7 */
+	vdev->device_caps = V4L2_CAP_STREAMING | cap;
+#endif
+}
+
+int mtk_fd_media_register(struct device *dev, struct media_device *media_dev,
+			  const char *model)
+{
+	int r = 0;
+
+	media_dev->dev = dev;
+	dev_info(dev, "setup media_dev.dev: %llx\n",
+		 (unsigned long long)media_dev->dev);
+
+	strlcpy(media_dev->model, model, sizeof(media_dev->model));
+	dev_info(dev, "setup media_dev.model: %s\n",
+		 media_dev->model);
+
+	snprintf(media_dev->bus_info, sizeof(media_dev->bus_info),
+		 "%s", dev_name(dev));
+	dev_info(dev, "setup media_dev.bus_info: %s\n",
+		 media_dev->bus_info);
+
+	media_dev->hw_revision = 0;
+	dev_info(dev, "setup media_dev.hw_revision: %d\n",
+		 media_dev->hw_revision);
+
+#if KERNEL_VERSION(4, 5, 0) <= MTK_FD_KERNEL_BASE_VERSION
+	dev_info(dev, "media_device_init: media_dev:%llx\n",
+		 (unsigned long long)media_dev);
+	media_device_init(media_dev);
+#endif
+
+	pr_info("Register media device: %s, %llx",
+		media_dev->model,
+		(unsigned long long)media_dev);
+
+	r = media_device_register(media_dev);
+
+	if (r) {
+		dev_err(dev, "failed to register media device (%d)\n", r);
+		goto fail_v4l2_dev;
+	}
+	return 0;
+
+fail_v4l2_dev:
+	media_device_unregister(media_dev);
+#if KERNEL_VERSION(4, 5, 0) <= MTK_FD_KERNEL_BASE_VERSION
+	media_device_cleanup(media_dev);
+#endif
+
+	return r;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_media_register);
+
+int mtk_fd_v4l2_register(struct device *dev,
+			 struct media_device *media_dev,
+			 struct v4l2_device *v4l2_dev)
+{
+	int r = 0;
+	/* Set up v4l2 device */
+	v4l2_dev->mdev = media_dev;
+	dev_info(dev, "setup v4l2_dev->mdev: %llx",
+		 (unsigned long long)v4l2_dev->mdev);
+
+	dev_info(dev, "Register v4l2 device: %llx",
+		 (unsigned long long)v4l2_dev);
+
+	r = v4l2_device_register(dev, v4l2_dev);
+
+	if (r) {
+		dev_err(dev, "failed to register V4L2 device (%d)\n", r);
+		goto fail_v4l2_dev;
+	}
+
+	return 0;
+
+fail_v4l2_dev:
+	media_device_unregister(media_dev);
+#if KERNEL_VERSION(4, 5, 0) <= MTK_FD_KERNEL_BASE_VERSION
+	media_device_cleanup(media_dev);
+#endif
+
+	return r;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_v4l2_register);
+
+int mtk_fd_mem2mem2_v4l2_register(struct mtk_fd_dev *dev,
+				  struct media_device *media_dev,
+				  struct v4l2_device *v4l2_dev)
+{
+	struct mtk_fd_mem2mem2_device *m2m2 = &dev->mem2mem2;
+
+	int i, r;
+
+	/**
+	 * If media_dev or v4l2_dev is not set,
+	 * use the default one in mtk_fd_dev
+	 */
+	if (!media_dev) {
+		m2m2->media_dev = &dev->media_dev;
+		r = mtk_fd_media_register(&dev->pdev->dev, m2m2->media_dev,
+					  m2m2->model);
+
+	if (r) {
+		dev_err(m2m2->dev, "failed to register media device (%d)\n", r);
+		goto fail_media_dev;
+	}
+	} else {
+		m2m2->media_dev = media_dev;
+	}
+
+	if (!v4l2_dev) {
+		m2m2->v4l2_dev = &dev->v4l2_dev;
+		r = mtk_fd_v4l2_register(&dev->pdev->dev,
+					 m2m2->media_dev,
+					 m2m2->v4l2_dev);
+	if (r) {
+		dev_err(m2m2->dev, "failed to register V4L2 device (%d)\n", r);
+		goto fail_v4l2_dev;
+	}
+	} else {
+		m2m2->v4l2_dev = v4l2_dev;
+	}
+
+	/* Initialize miscellaneous variables */
+	m2m2->streaming = false;
+	m2m2->v4l2_file_ops = mtk_fd_v4l2_fops;
+
+	/* Initialize subdev media entity */
+	m2m2->subdev_pads = kcalloc(m2m2->num_nodes, sizeof(*m2m2->subdev_pads),
+				    GFP_KERNEL);
+	if (!m2m2->subdev_pads) {
+		r = -ENOMEM;
+		goto fail_subdev_pads;
+	}
+#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
+	r = media_entity_init(&m2m2->subdev.entity, m2m2->num_nodes,
+			      m2m2->subdev_pads, 0);
+#else
+	r = media_entity_pads_init(&m2m2->subdev.entity, m2m2->num_nodes,
+				   m2m2->subdev_pads);
+#endif
+	if (r) {
+		dev_err(m2m2->dev,
+			"failed initialize subdev media entity (%d)\n", r);
+		goto fail_media_entity;
+	}
+
+	/* Initialize subdev */
+	v4l2_subdev_init(&m2m2->subdev, &mtk_fd_subdev_ops);
+
+	m2m2->subdev.entity.function =
+		MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+
+	m2m2->subdev.entity.ops = &mtk_fd_media_ops;
+
+	for (i = 0; i < m2m2->num_nodes; i++) {
+		m2m2->subdev_pads[i].flags = m2m2->nodes[i].output ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+	}
+
+	m2m2->subdev.flags =
+		V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	snprintf(m2m2->subdev.name, sizeof(m2m2->subdev.name),
+		 "%s", m2m2->name);
+	v4l2_set_subdevdata(&m2m2->subdev, m2m2);
+	m2m2->subdev.internal_ops = &mtk_fd_subdev_internal_ops;
+
+	pr_info("register subdev: %s\n", m2m2->subdev.name);
+	r = v4l2_device_register_subdev(m2m2->v4l2_dev, &m2m2->subdev);
+	if (r) {
+		dev_err(m2m2->dev, "failed initialize subdev (%d)\n", r);
+		goto fail_subdev;
+	}
+	r = v4l2_device_register_subdev_nodes(m2m2->v4l2_dev);
+	if (r) {
+		dev_err(m2m2->dev, "failed to register subdevs (%d)\n", r);
+		goto fail_subdevs;
+	}
+
+	/* Create video nodes and links */
+	for (i = 0; i < m2m2->num_nodes; i++) {
+		struct mtk_fd_dev_video_device *node = &m2m2->nodes[i];
+		struct video_device *vdev = &node->vdev;
+		struct vb2_queue *vbq = &node->vbq;
+		u32 flags;
+
+		/* Initialize miscellaneous variables */
+		mutex_init(&node->lock);
+		INIT_LIST_HEAD(&node->buffers);
+
+		/* Initialize formats to default values */
+		mtk_fd_node_to_v4l2(dev, i, vdev, &node->vdev_fmt);
+
+		/* Initialize media entities */
+#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
+		r = media_entity_init(&vdev->entity, 1, &node->vdev_pad, 0);
+#else
+		r = media_entity_pads_init(&vdev->entity, 1, &node->vdev_pad);
+#endif
+		if (r) {
+			dev_err(m2m2->dev,
+				"failed initialize media entity (%d)\n", r);
+			goto fail_vdev_media_entity;
+		}
+		node->vdev_pad.flags = node->output ?
+			MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
+		vdev->entity.ops = NULL;
+
+		/* Initialize vbq */
+		vbq->type = node->vdev_fmt.type;
+		vbq->io_modes = VB2_MMAP | VB2_DMABUF;
+		vbq->ops = &mtk_fd_vb2_ops;
+		vbq->mem_ops = m2m2->vb2_mem_ops;
+		m2m2->buf_struct_size = sizeof(struct mtk_fd_dev_buffer);
+		vbq->buf_struct_size = m2m2->buf_struct_size;
+		vbq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		vbq->min_buffers_needed = 0;	/* Can streamon w/o buffers */
+		/* Put the process hub sub device in the vb2 private data*/
+		vbq->drv_priv = m2m2;
+		vbq->lock = &node->lock;
+		r = vb2_queue_init(vbq);
+		if (r) {
+			dev_err(m2m2->dev,
+				"failed to initialize video queue (%d)\n", r);
+			goto fail_vdev;
+		}
+
+		/* Initialize vdev */
+		snprintf(vdev->name, sizeof(vdev->name), "%s %s",
+			 m2m2->name, node->name);
+		vdev->release = video_device_release_empty;
+		vdev->fops = &m2m2->v4l2_file_ops;
+		vdev->lock = &node->lock;
+		vdev->v4l2_dev = m2m2->v4l2_dev;
+		vdev->queue = &node->vbq;
+		vdev->vfl_dir = node->output ? VFL_DIR_TX : VFL_DIR_RX;
+		video_set_drvdata(vdev, m2m2);
+		pr_info("register vdev: %s\n", vdev->name);
+		r = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+		if (r) {
+			dev_err(m2m2->dev,
+				"failed to register video device (%d)\n", r);
+			goto fail_vdev;
+		}
+
+		/* Create link between video node and the subdev pad */
+		flags = 0;
+		if (node->enabled)
+			flags |= MEDIA_LNK_FL_ENABLED;
+		if (node->immutable)
+			flags |= MEDIA_LNK_FL_IMMUTABLE;
+		if (node->output) {
+#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
+			r = media_entity_create_link
+#else
+			r = media_create_pad_link
+#endif
+						(&vdev->entity, 0,
+						 &m2m2->subdev.entity,
+						 i, flags);
+		} else {
+#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
+			r = media_entity_create_link
+#else
+			r = media_create_pad_link
+#endif
+						(&m2m2->subdev.entity,
+						 i, &vdev->entity, 0,
+						 flags);
+		}
+		if (r)
+			goto fail_link;
+	}
+
+	return 0;
+
+	for (; i >= 0; i--) {
+fail_link:
+		video_unregister_device(&m2m2->nodes[i].vdev);
+fail_vdev:
+		media_entity_cleanup(&m2m2->nodes[i].vdev.entity);
+fail_vdev_media_entity:
+		mutex_destroy(&m2m2->nodes[i].lock);
+	}
+fail_subdevs:
+	v4l2_device_unregister_subdev(&m2m2->subdev);
+fail_subdev:
+	media_entity_cleanup(&m2m2->subdev.entity);
+fail_media_entity:
+	kfree(m2m2->subdev_pads);
+fail_subdev_pads:
+	v4l2_device_unregister(m2m2->v4l2_dev);
+fail_v4l2_dev:
+fail_media_dev:
+	pr_err("fail_v4l2_dev: media_device_unregister and clenaup:%llx",
+	       (unsigned long long)m2m2->media_dev);
+	media_device_unregister(m2m2->media_dev);
+#if KERNEL_VERSION(4, 5, 0) <= MTK_FD_KERNEL_BASE_VERSION
+	media_device_cleanup(m2m2->media_dev);
+#endif
+
+	return r;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_mem2mem2_v4l2_register);
+
+int mtk_fd_v4l2_unregister(struct mtk_fd_dev *dev)
+{
+	struct mtk_fd_mem2mem2_device *m2m2 = &dev->mem2mem2;
+	unsigned int i;
+
+	for (i = 0; i < m2m2->num_nodes; i++) {
+		video_unregister_device(&m2m2->nodes[i].vdev);
+		media_entity_cleanup(&m2m2->nodes[i].vdev.entity);
+		mutex_destroy(&m2m2->nodes[i].lock);
+	}
+
+	v4l2_device_unregister_subdev(&m2m2->subdev);
+	media_entity_cleanup(&m2m2->subdev.entity);
+	kfree(m2m2->subdev_pads);
+	v4l2_device_unregister(m2m2->v4l2_dev);
+	media_device_unregister(m2m2->media_dev);
+#if KERNEL_VERSION(4, 5, 0) <= MTK_FD_KERNEL_BASE_VERSION
+	media_device_cleanup(m2m2->media_dev);
+#endif
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mtk_fd_v4l2_unregister);
+
+void mtk_fd_v4l2_buffer_done(struct vb2_buffer *vb,
+			     enum vb2_buffer_state state)
+{
+	struct mtk_fd_mem2mem2_buffer *b =
+		container_of(vb, struct mtk_fd_mem2mem2_buffer, vbb.vb2_buf);
+
+	list_del(&b->list);
+	vb2_buffer_done(&b->vbb.vb2_buf, state);
+}
+EXPORT_SYMBOL_GPL(mtk_fd_v4l2_buffer_done);
diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c b/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c
new file mode 100644
index 0000000..bd447b7
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c
@@ -0,0 +1,114 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Frederic Chen <frederic.chen@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include "mtk_fd.h"
+#include "mtk_fd-ctx.h"
+#include "mtk_fd-v4l2.h"
+
+static struct mtk_fd_ctx_format fd_param_fmts[] = {
+	{
+		.fmt.meta = {
+		.dataformat = V4L2_META_FMT_MTISP_PARAMS,
+		.max_buffer_size = 1024 * 30,
+		},
+	},
+};
+
+static struct mtk_fd_ctx_format in_fmts[] = {
+	{
+		.fmt.img = {
+			.pixelformat  = V4L2_PIX_FMT_VYUY,
+			.depth    = { 16 },
+			.row_depth  = { 16 },
+			.num_planes = 1,
+		},
+	},
+	{
+		.fmt.img = {
+			.pixelformat  = V4L2_PIX_FMT_YUYV,
+			.depth    = { 16 },
+			.row_depth  = { 16 },
+			.num_planes = 1,
+		},
+	},
+	{
+		.fmt.img = {
+			.pixelformat  = V4L2_PIX_FMT_YVYU,
+			.depth	  = { 16 },
+			.row_depth  = { 16 },
+			.num_planes = 1,
+		},
+	},
+	{
+		.fmt.img = {
+			.pixelformat  = V4L2_PIX_FMT_UYVY,
+			.depth    = { 16 },
+			.row_depth  = { 16 },
+			.num_planes = 1,
+		},
+	},
+};
+
+static struct mtk_fd_ctx_queue_desc
+output_queues[MTK_FD_CTX_FD_TOTAL_OUTPUT] = {
+	{
+		.id = MTK_FD_CTX_FD_YUV_IN,
+		.name = "FDInput",
+		.capture = 0,
+		.image = 1,
+		.fmts = in_fmts,
+		.num_fmts = ARRAY_SIZE(in_fmts),
+		.default_fmt_idx = 1,
+	},
+	{
+		.id = MTK_FD_CTX_FD_CONFIG_IN,
+		.name = "FDConfig",
+		.capture = 0,
+		.image = 0,
+		.fmts = fd_param_fmts,
+		.num_fmts = 1,
+		.default_fmt_idx = 0,
+	},
+};
+
+static struct mtk_fd_ctx_queue_desc
+capture_queues[MTK_FD_CTX_FD_TOTAL_CAPTURE] = {
+	{
+		.id = MTK_FD_CTX_FD_OUT,
+		.name = "FDOutput",
+		.capture = 1,
+		.image = 0,
+		.fmts = fd_param_fmts,
+		.num_fmts = 1,
+		.default_fmt_idx = 0,
+	},
+};
+
+static struct mtk_fd_ctx_queues_setting queues_setting = {
+	.master = MTK_FD_CTX_FD_OUT,
+	.output_queue_descs = output_queues,
+	.total_output_queues = MTK_FD_CTX_FD_TOTAL_OUTPUT,
+	.capture_queue_descs = capture_queues,
+	.total_capture_queues = MTK_FD_CTX_FD_TOTAL_CAPTURE,
+};
+
+int mtk_fd_ctx_fd_init(struct mtk_fd_ctx *ctx)
+{
+	/* Initialize main data structure */
+	return mtk_fd_ctx_core_queue_setup(ctx, &queues_setting);
+}
+EXPORT_SYMBOL_GPL(mtk_fd_ctx_fd_init);
diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h b/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h
new file mode 100644
index 0000000..0702abc
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Frederic Chen <frederic.chen@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MTK_FD_V4L2__
+#define __MTK_FD_V4L2__
+
+#include <linux/types.h>
+#include "mtk_fd-ctx.h"
+
+#define MTK_FD_DEV_NAME     "MTK-FD-V4L2"
+
+/* Input: YUV image */
+#define MTK_FD_CTX_FD_YUV_IN		0
+/* Input: FD Configuration */
+#define MTK_FD_CTX_FD_CONFIG_IN		1
+#define MTK_FD_CTX_FD_TOTAL_OUTPUT	2
+
+/* OUT: FD output devices*/
+#define MTK_FD_CTX_FD_OUT		2
+#define MTK_FD_CTX_FD_TOTAL_CAPTURE	1
+
+int mtk_fd_ctx_fd_init(struct mtk_fd_ctx *ctx);
+
+#endif /*__MTK_FD_V4L2__*/
diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd.c b/drivers/media/platform/mtk-isp/fd/mtk_fd.c
new file mode 100644
index 0000000..7e2fd00
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/mtk_fd.c
@@ -0,0 +1,730 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2015 MediaTek Inc.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/kdev_t.h>
+
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/mm_types.h>
+#include <linux/mm.h>
+#include <linux/jiffies.h>
+#include <linux/sched.h>
+#include <linux/uaccess.h>
+#include <asm/page.h>
+#include <linux/vmalloc.h>
+#include <linux/interrupt.h>
+#include <linux/wait.h>
+
+#include <linux/of_platform.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+
+#include "mtk_fd.h"
+#include "mtk_fd-core.h"
+
+#include "mtk_vpu.h"
+
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+
+#ifdef CONFIG_PM_WAKELOCKS
+#include <linux/pm_wakeup.h>
+#else
+#include <linux/wakelock.h>
+#endif
+
+#define FD_DRVNAME	"mtk-fd"
+
+static const struct of_device_id mtk_fd_of_ids[] = {
+	/* Remider: Add this device node manually in .dtsi */
+	{ .compatible = "mediatek,fd", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, mtk_fd_of_ids);
+
+static void mtk_fd_prepare_enable_ccf_clock(struct mtk_fd_drv_dev *fd_hw_dev)
+{
+	int ret;
+
+	pm_runtime_get_sync(fd_hw_dev->larb_dev);
+	ret = clk_prepare_enable(fd_hw_dev->fd_clk);
+	if (ret)
+		dev_err(&fd_hw_dev->pdev->dev,
+			"cannot prepare and enable CG_IMGSYS_FD clock\n");
+}
+
+static void mtk_fd_disable_unprepare_ccf_clock(struct mtk_fd_drv_dev *fd_hw_dev)
+{
+	clk_disable_unprepare(fd_hw_dev->fd_clk);
+}
+
+static int mtk_fd_clk_ctrl(struct mtk_fd_drv_dev *fd_hw_dev, int en)
+{
+	if (en)
+		mtk_fd_prepare_enable_ccf_clock(fd_hw_dev);
+	else
+		mtk_fd_disable_unprepare_ccf_clock(fd_hw_dev);
+
+	dev_dbg(&fd_hw_dev->pdev->dev, "clock en: %d\n", en);
+
+	return 0;
+}
+
+static int mtk_fd_wait_irq(struct mtk_fd_drv_dev *fd_hw_dev)
+{
+	int timeout;
+
+	timeout = wait_event_interruptible_timeout
+		(fd_hw_dev->wq,
+		 (fd_hw_dev->fd_irq_result & FD_IRQ_MASK),
+		 mtk_fd_us_to_jiffies(1 * 1000000));
+
+	if (timeout == 0) {
+		dev_err(&fd_hw_dev->pdev->dev,
+			"%s timeout, %d\n",
+			__func__, fd_hw_dev->fd_irq_result);
+		return -EAGAIN;
+	}
+
+	dev_dbg(&fd_hw_dev->pdev->dev, "irq_res: 0x%8x\n",
+		fd_hw_dev->fd_irq_result);
+
+	if (timeout != 0 && !(fd_hw_dev->fd_irq_result & FD_IRQ_MASK)) {
+		dev_err(&fd_hw_dev->pdev->dev,
+			"%s interrupted by system signal, return value(%d)\n",
+			__func__, timeout);
+		return -ERESTARTSYS;
+	}
+
+	if (!(fd_hw_dev->fd_irq_result & FD_IRQ_MASK)) {
+		dev_err(&fd_hw_dev->pdev->dev,
+			"%s Not FD, %d\n",
+			__func__, fd_hw_dev->fd_irq_result);
+		return -1;
+	}
+
+	fd_hw_dev->fd_irq_result = 0;
+
+	return 0;
+}
+
+static int mtk_fd_send_ipi_init(struct platform_device *pdev,
+				struct fd_buffer *scp_mem)
+{
+	struct ipi_message fd_init_msg;
+
+	fd_init_msg.cmd_id = FD_CMD_INIT;
+	fd_init_msg.fd_manager = *scp_mem;
+
+	vpu_ipi_send(pdev, IPI_FD_CMD, &fd_init_msg, sizeof(fd_init_msg));
+
+	return 0;
+}
+
+static int mtk_fd_send_ipi_cmd(struct platform_device *pdev,
+			       struct v4l2_fd_param *fd_param)
+{
+	struct ipi_message fd_ipi_msg;
+
+	fd_ipi_msg.cmd_id = FD_CMD_ENQ;
+	fd_ipi_msg.fd_param = *fd_param;
+
+	vpu_ipi_send_sync_async(pdev, IPI_FD_CMD, &fd_ipi_msg,
+				sizeof(fd_ipi_msg), 0);
+	return 0;
+}
+
+static irqreturn_t mtk_fd_irq(int irq, void *dev_addr)
+{
+	struct mtk_fd_drv_dev *fd_hw_dev;
+
+	fd_hw_dev = (struct mtk_fd_drv_dev *)dev_addr;
+	fd_hw_dev->fd_irq_result = FD_RD32(fd_hw_dev->fd_base + FD_INT);
+	wake_up_interruptible(&fd_hw_dev->wq);
+
+	return IRQ_HANDLED;
+}
+
+static int mtk_fd_do_callback(struct mtk_fd_drv_dev *fd_hw_dev,
+			      unsigned int frame_state)
+{
+	struct mtk_fd_ctx_finish_param fparam;
+	struct mtk_isp_fd_drv_data *drv_data;
+	struct device *dev;
+	int ret = 0;
+
+	drv_data = mtk_fd_hw_dev_to_drv(fd_hw_dev);
+	dev = &drv_data->fd_hw_dev.pdev->dev;
+
+	fparam.state = frame_state;
+	fparam.frame_id = fd_hw_dev->fd_ctx.frame_id;
+	dev_dbg(dev, "frame_id(%d)\n", fparam.frame_id);
+
+	ret = mtk_fd_ctx_core_job_finish(&drv_data->fd_dev.ctx, &fparam);
+	if (ret)
+		dev_err(dev, "frame_id(%d), finish op failed: %d\n",
+			fparam.frame_id, ret);
+	else
+		fd_hw_dev->state = FD_CBD;
+
+	return ret;
+}
+
+static int mtk_fd_dequeue(struct mtk_fd_drv_dev *fd_hw_dev,
+			  struct v4l2_fd_param *fd_param)
+{
+	struct mtk_isp_fd_drv_data *drv_data;
+	struct fd_user_output *fd_output;
+	u32 num = 0, i = 0;
+
+	dev_dbg(&fd_hw_dev->pdev->dev, "-E. %s.\n", __func__);
+
+	if ((uint64_t)fd_hw_dev->fd_base < VA_OFFSET) {
+		dev_info(&fd_hw_dev->pdev->dev,
+			 "-X. %s. fd_base_error: %p\n",
+			 __func__, fd_hw_dev->fd_base);
+		return -1;
+	}
+
+	num = FD_RD32(fd_hw_dev->fd_base + FD_RESULT);
+	FD_WR32(0x0, fd_hw_dev->fd_base + FD_INT_EN);
+	fd_output = (struct fd_user_output *)fd_param->fd_user_result.va;
+	fd_output->face_number = num;
+
+	drv_data = mtk_fd_hw_dev_to_drv(fd_hw_dev);
+	if ((uint64_t)drv_data < VA_OFFSET) {
+		dev_dbg(&fd_hw_dev->pdev->dev,
+			"-X. %s. fd_base_error\n", __func__);
+		return -1;
+	}
+	mtk_fd_do_callback(fd_hw_dev, MTK_FD_CTX_FRAME_DATA_DONE);
+
+	for (i = 0; i < num; i++) {
+		dev_dbg(&fd_hw_dev->pdev->dev,
+			"id:%d, typ:%d, x0:%d, y0:%d, x1:%d, y1:%d, fcv:0x%x\n",
+			fd_output->face[i].face_idx,
+			fd_output->face[i].type,
+			fd_output->face[i].x0,
+			fd_output->face[i].y0,
+			fd_output->face[i].x1,
+			fd_output->face[i].y1,
+			fd_output->face[i].fcv);
+	}
+	dev_dbg(&fd_hw_dev->pdev->dev, "-X. %s.\n", __func__);
+	return 0;
+}
+
+static int mtk_fd_manager_buf_init(struct mtk_fd_drv_dev *fd_hw_dev)
+{
+	struct fd_manager_ctx *manager_ctx;
+
+	manager_ctx = (struct fd_manager_ctx *)fd_hw_dev->fd_ctx.scp_mem.va;
+	manager_ctx->rs_result = fd_hw_dev->fd_ctx.rs_result;
+
+	return 0;
+}
+
+static int mtk_fd_alloc_rs_buf(struct mtk_fd_drv_dev *fd_hw_dev)
+{
+	u64 va = 0;
+	dma_addr_t dma_handle = 0;
+	u32 size = RS_BUF_SIZE_MAX;
+
+	va = (uint64_t)dma_alloc_coherent(&fd_hw_dev->pdev->dev, size,
+	&dma_handle, GFP_KERNEL);
+
+	dev_dbg(&fd_hw_dev->pdev->dev, "rsbuffer size: %u\n", size);
+	dev_dbg(&fd_hw_dev->pdev->dev, "va = 0x%llx, iova = 0x%x\n", va,
+		dma_handle);
+
+	if (va == 0) {
+		dev_err(&fd_hw_dev->pdev->dev, "dma_alloc null va!\n");
+		return -1;
+	}
+
+	memset((uint8_t *)va, 0, size);
+
+	fd_hw_dev->fd_ctx.rs_result.va = va;
+	fd_hw_dev->fd_ctx.rs_result.iova = dma_handle;
+
+	return 0;
+}
+
+static int mtk_fd_get_reserve_mem(struct mtk_fd_drv_dev *fd_hw_dev)
+{
+	phys_addr_t scp_mem_pa;
+	u64 scp_mem_va;
+	u32 scp_mem_iova;
+	u32 size = 0, size_align = 0;
+	struct sg_table *sgt;
+	int n_pages = 0, i = 0, ret = 0;
+	struct page **pages = NULL;
+	struct mtk_fd_drv_ctx *fd_ctx;
+	struct platform_device *pdev = fd_hw_dev->pdev;
+
+	fd_ctx = &fd_hw_dev->fd_ctx;
+
+	scp_mem_iova = 0;
+	scp_mem_va = vpu_get_reserve_mem_virt(FD_MEM_ID);
+	scp_mem_pa = vpu_get_reserve_mem_phys(FD_MEM_ID);
+	size = (u32)vpu_get_reserve_mem_size(FD_MEM_ID);
+
+	dev_dbg(&pdev->dev, "fd_scp_mem va: 0x%llx, pa: 0x%llx, sz:0x%x\n",
+		scp_mem_va, (u64)scp_mem_pa, size);
+
+	if (scp_mem_va != 0 && size > 0)
+		memset((void *)scp_mem_va, 0, size);
+
+	/* get iova */
+	sgt = &fd_ctx->sgtable;
+	sg_alloc_table(sgt, 1, GFP_KERNEL);
+
+	size_align = round_up(size, PAGE_SIZE);
+	n_pages = size_align >> PAGE_SHIFT;
+
+	pages = kmalloc_array(n_pages, sizeof(struct page *), GFP_KERNEL);
+
+	for (i = 0; i < n_pages; i++)
+		pages[i] = phys_to_page(scp_mem_pa + i * PAGE_SIZE);
+	ret = sg_alloc_table_from_pages(sgt, pages, n_pages,
+					0, size_align, GFP_KERNEL);
+
+	if (ret) {
+		dev_err(&pdev->dev, "failed to get allocate sg table\n");
+		kfree(pages);
+		return ret;
+	}
+
+	dma_map_sg_attrs(&pdev->dev, sgt->sgl, sgt->nents,
+			 DMA_BIDIRECTIONAL, DMA_ATTR_SKIP_CPU_SYNC);
+	scp_mem_iova = sg_dma_address(sgt->sgl);
+	kfree(pages);
+
+	dev_dbg(&fd_hw_dev->pdev->dev, "scpmem size:%u,0x%X\n", size, size);
+	dev_dbg(&fd_hw_dev->pdev->dev, "pa:0x%08X\n", (u32)scp_mem_pa);
+	dev_dbg(&fd_hw_dev->pdev->dev, "va:0x%16llX\n", (u64)scp_mem_va);
+	dev_dbg(&fd_hw_dev->pdev->dev, "iova:0x%08X\n", (u32)scp_mem_iova);
+
+	fd_ctx->scp_mem.pa = scp_mem_pa;
+	fd_ctx->scp_mem.va = scp_mem_va;
+	fd_ctx->scp_mem.iova = scp_mem_iova;
+
+	return 0;
+}
+
+static int mtk_fd_load_vpu(struct mtk_fd_drv_dev *fd_hw_dev)
+{
+	struct mtk_fd_drv_ctx *fd_ctx;
+	int ret = 0;
+
+	fd_ctx = &fd_hw_dev->fd_ctx;
+
+	/* init vpu */
+	fd_ctx->vpu_pdev = vpu_get_plat_device(fd_hw_dev->pdev);
+
+	if (!fd_ctx->vpu_pdev) {
+		dev_err(&fd_hw_dev->pdev->dev,
+			"Failed to get VPU device\n");
+		return -EINVAL;
+	}
+
+	ret = vpu_load_firmware(fd_ctx->vpu_pdev);
+	if (ret < 0) {
+		/**
+		 * Return 0 if downloading firmware successfully,
+		 * otherwise it is failed
+		 */
+		dev_err(&fd_hw_dev->pdev->dev,
+			"vpu_load_firmware failed!");
+		return -EINVAL;
+	}
+	return ret;
+}
+
+static int mtk_fd_open_context(struct mtk_fd_drv_dev *fd_hw_dev)
+{
+	struct mtk_fd_drv_ctx *fd_ctx;
+
+	fd_ctx = &fd_hw_dev->fd_ctx;
+
+	mtk_fd_load_vpu(fd_hw_dev);
+
+	mtk_fd_get_reserve_mem(fd_hw_dev);
+	mtk_fd_alloc_rs_buf(fd_hw_dev);
+	mtk_fd_manager_buf_init(fd_hw_dev);
+
+	mtk_fd_send_ipi_init(fd_ctx->vpu_pdev, &fd_ctx->scp_mem);
+	return 0;
+}
+
+static int mtk_fd_release_context(struct mtk_fd_drv_dev *fd_hw_dev)
+{
+	struct mtk_fd_drv_ctx *fd_ctx;
+
+	fd_ctx = &fd_hw_dev->fd_ctx;
+
+	atomic_set(&fd_ctx->fd_user_cnt, 0);
+	atomic_set(&fd_ctx->fd_stream_cnt, 0);
+	atomic_set(&fd_ctx->fd_enque_cnt, 0);
+	sg_free_table(&fd_ctx->sgtable);
+
+	return 0;
+}
+
+int mtk_fd_open(struct platform_device *pdev)
+{
+	int ret = 0;
+	s32 usercount;
+	struct mtk_fd_drv_dev *fd_hw_dev;
+	struct mtk_isp_fd_drv_data *fd_drv;
+	struct mtk_fd_drv_ctx *fd_ctx;
+
+	dev_dbg(&pdev->dev, "- E. %s.\n", __func__);
+
+	if (!pdev) {
+		dev_err(&fd_hw_dev->pdev->dev, "platform device is NULL\n");
+		return -EINVAL;
+	}
+
+	fd_drv = dev_get_drvdata(&pdev->dev);
+	fd_hw_dev = &fd_drv->fd_hw_dev;
+	fd_ctx = &fd_hw_dev->fd_ctx;
+	dev_dbg(&fd_hw_dev->pdev->dev, "open fd_hw_dev = 0x%p\n", fd_hw_dev);
+	dev_dbg(&fd_hw_dev->pdev->dev, "open fd_drv = 0x%p\n", fd_drv);
+
+	usercount = atomic_inc_return(&fd_hw_dev->fd_ctx.fd_user_cnt);
+
+	if (usercount == 1) {
+		/* Enable clock */
+		pm_runtime_get_sync(&fd_hw_dev->pdev->dev);
+
+		mtk_fd_open_context(fd_hw_dev);
+		fd_hw_dev->state = FD_INI;
+		fd_hw_dev->streaming = STREAM_OFF;
+	}
+
+	dev_dbg(&fd_hw_dev->pdev->dev, "usercount = %d",
+		atomic_read(&fd_ctx->fd_user_cnt));
+
+	dev_dbg(&fd_hw_dev->pdev->dev, "X. %s\n", __func__);
+
+	return ret;
+}
+EXPORT_SYMBOL(mtk_fd_open);
+
+int mtk_fd_streamon(struct platform_device *pdev, u16 id)
+{
+	int ret = 0;
+	struct mtk_fd_drv_dev *fd_hw_dev;
+	struct mtk_isp_fd_drv_data *fd_drv;
+
+	fd_drv = dev_get_drvdata(&pdev->dev);
+	fd_hw_dev = &fd_drv->fd_hw_dev;
+
+	dev_dbg(&pdev->dev, "- E. %s\n", __func__);
+	fd_hw_dev->streaming = STREAM_ON;
+	atomic_inc_return(&fd_hw_dev->fd_ctx.fd_stream_cnt);
+
+	dev_dbg(&pdev->dev, "- X. %s\n", __func__);
+	return ret;
+}
+EXPORT_SYMBOL(mtk_fd_streamon);
+
+int mtk_fd_enqueue(struct platform_device *pdev,
+		   struct v4l2_fd_param *fd_param)
+{
+	struct mtk_fd_drv_dev *fd_hw_dev;
+	struct mtk_fd_drv_ctx *fd_ctx;
+	struct mtk_isp_fd_drv_data *fd_drv;
+
+	dev_dbg(&pdev->dev, "- E. %s\n", __func__);
+	fd_drv = dev_get_drvdata(&pdev->dev);
+	fd_hw_dev = &fd_drv->fd_hw_dev;
+	fd_ctx = &fd_hw_dev->fd_ctx;
+
+	fd_ctx->frame_id = fd_param->frame_id;
+
+	if (fd_hw_dev->streaming == STREAM_OFF || fd_hw_dev->state == FD_ENQ) {
+		dev_err(&fd_hw_dev->pdev->dev, "enqueue before stream on!\n");
+		return mtk_fd_do_callback(fd_hw_dev,
+						MTK_FD_CTX_FRAME_DATA_ERROR);
+	}
+
+	fd_hw_dev->state = FD_ENQ;
+	atomic_inc_return(&fd_ctx->fd_enque_cnt);
+
+	if (mtk_fd_send_ipi_cmd(fd_ctx->vpu_pdev, fd_param))
+		return -2;
+
+	if (mtk_fd_wait_irq(fd_hw_dev))
+		return mtk_fd_do_callback(fd_hw_dev,
+						MTK_FD_CTX_FRAME_DATA_ERROR);
+
+	mtk_fd_dequeue(fd_hw_dev, fd_param);
+
+	dev_dbg(&pdev->dev, "- E. %s\n", __func__);
+	return 0;
+}
+EXPORT_SYMBOL(mtk_fd_enqueue);
+
+int mtk_fd_release(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct mtk_fd_drv_dev *fd_hw_dev;
+	struct mtk_isp_fd_drv_data *fd_drv;
+
+	fd_drv = dev_get_drvdata(&pdev->dev);
+	fd_hw_dev = &fd_drv->fd_hw_dev;
+	dev_dbg(&fd_hw_dev->pdev->dev, "- E. %s\n", __func__);
+
+	if (!pdev) {
+		dev_err(&fd_hw_dev->pdev->dev, "platform device is NULL\n");
+		return -EINVAL;
+	}
+
+	dev_info(&fd_hw_dev->pdev->dev, "release fd_hw_dev: 0x%p\n", fd_hw_dev);
+
+	if (atomic_dec_and_test(&fd_hw_dev->fd_ctx.fd_user_cnt)) {
+		if (fd_hw_dev->state == FD_ENQ)
+			mtk_fd_wait_irq(fd_hw_dev);
+
+		mtk_fd_release_context(fd_hw_dev);
+
+		pm_runtime_put_sync(&fd_hw_dev->pdev->dev);
+	}
+	dev_info(&fd_hw_dev->pdev->dev, "usercount = %d\n",
+		 atomic_read(&fd_hw_dev->fd_ctx.fd_user_cnt));
+
+	dev_dbg(&fd_hw_dev->pdev->dev, "- X. %s\n", __func__);
+	return ret;
+}
+EXPORT_SYMBOL(mtk_fd_release);
+
+int mtk_fd_streamoff(struct platform_device *pdev, u16 id)
+{
+	int ret = 0;
+	struct mtk_fd_drv_dev *fd_hw_dev;
+	struct mtk_isp_fd_drv_data *fd_drv;
+
+	fd_drv = dev_get_drvdata(&pdev->dev);
+	fd_hw_dev = &fd_drv->fd_hw_dev;
+
+	dev_dbg(&pdev->dev, "- E. %s\n", __func__);
+
+	if (fd_hw_dev->state == FD_ENQ)
+		mtk_fd_wait_irq(fd_hw_dev);
+
+	fd_hw_dev->streaming = STREAM_OFF;
+	atomic_dec_return(&fd_hw_dev->fd_ctx.fd_stream_cnt);
+
+	dev_dbg(&pdev->dev, "- X. %s\n", __func__);
+	return ret;
+}
+EXPORT_SYMBOL(mtk_fd_streamoff);
+
+static struct mtk_fd_ctx_desc mtk_isp_ctx_desc_fd = {
+	"proc_device_fd", mtk_fd_ctx_fd_init,};
+
+static int mtk_fd_probe(struct platform_device *pdev)
+{
+	struct mtk_isp_fd_drv_data *fd_drv;
+	struct mtk_fd_drv_dev *fd_hw_dev;
+	struct mtk_fd_drv_ctx *fd_ctx;
+	struct device_node *node;
+	struct platform_device *larb_pdev;
+	int irq_num = 0;
+	int ret = 0;
+
+	dev_info(&pdev->dev, "E. %s.\n", __func__);
+
+	fd_drv = devm_kzalloc(&pdev->dev, sizeof(*fd_drv), GFP_KERNEL);
+
+	if (!fd_drv)
+		return -ENOMEM;
+
+	dev_set_drvdata(&pdev->dev, fd_drv);
+	fd_hw_dev = &fd_drv->fd_hw_dev;
+
+	if (!fd_hw_dev) {
+		dev_err(&pdev->dev, "Unable to allocate fd_hw_dev\n");
+		return -ENOMEM;
+	}
+
+	dev_info(&pdev->dev, "open fd_hw_dev = 0x%p\n", fd_hw_dev);
+	dev_info(&pdev->dev, "open fd_drv = 0x%p\n", fd_drv);
+
+	fd_hw_dev->pdev = pdev;
+	fd_ctx = &fd_hw_dev->fd_ctx;
+
+	irq_num = irq_of_parse_and_map(pdev->dev.of_node, FD_IRQ_IDX);
+	ret = request_irq(irq_num, (irq_handler_t)mtk_fd_irq,
+			  IRQF_TRIGGER_NONE, FD_DRVNAME, fd_hw_dev);
+	if (ret) {
+		dev_info(&pdev->dev, "%s request_irq fail, irq=%d\n",
+			 __func__, irq_num);
+		return ret;
+	}
+	dev_info(&pdev->dev, "irq_num=%d\n", irq_num);
+
+	node = of_parse_phandle(pdev->dev.of_node, "mediatek,larb", 0);
+	if (!node) {
+		dev_err(&pdev->dev, "no mediatek, larb found");
+		return -EINVAL;
+	}
+	larb_pdev = of_find_device_by_node(node);
+	if (!larb_pdev) {
+		dev_err(&pdev->dev, "no mediatek, larb device found");
+		return -EINVAL;
+	}
+	fd_hw_dev->larb_dev = &larb_pdev->dev;
+
+	node = of_find_compatible_node(NULL, NULL, "mediatek,fd");
+	if (!node) {
+		dev_err(&pdev->dev, "find fd node failed!!!\n");
+		return -ENODEV;
+	}
+	fd_hw_dev->fd_base = of_iomap(node, 0);
+	if (!fd_hw_dev->fd_base) {
+		dev_err(&pdev->dev, "unable to map fd node!!!\n");
+		return -ENODEV;
+	}
+	dev_info(&pdev->dev, "fd_hw_dev->fd_base: %lx\n",
+		 (unsigned long)fd_hw_dev->fd_base);
+
+	/* CCF: Grab clock pointer (struct clk*) */
+	fd_hw_dev->fd_clk = devm_clk_get(&pdev->dev, "FD_CLK_IMG_FD");
+	if (IS_ERR(fd_hw_dev->fd_clk)) {
+		dev_err(&pdev->dev, "cannot get FD_CLK_IMG_FD clock\n");
+		return PTR_ERR(fd_hw_dev->fd_clk);
+	}
+
+	pm_runtime_enable(&pdev->dev);
+
+	atomic_set(&fd_ctx->fd_user_cnt, 0);
+	atomic_set(&fd_ctx->fd_stream_cnt, 0);
+	atomic_set(&fd_ctx->fd_enque_cnt, 0);
+
+	init_waitqueue_head(&fd_hw_dev->wq);
+
+	fd_hw_dev->fd_irq_result = 0;
+	fd_hw_dev->streaming = STREAM_OFF;
+
+	ret = mtk_fd_dev_core_init(pdev, &fd_drv->fd_dev, &mtk_isp_ctx_desc_fd);
+
+	if (ret)
+		dev_err(&pdev->dev, "v4l2 init failed: %d\n", ret);
+
+	dev_info(&pdev->dev, "X. FD driver probe.\n");
+
+	return 0;
+}
+
+static int mtk_fd_remove(struct platform_device *pdev)
+{
+	int i4IRQ = 0;
+	struct mtk_fd_drv_dev *fd_hw_dev = NULL;
+	struct mtk_isp_fd_drv_data *drv_data = dev_get_drvdata(&pdev->dev);
+
+	dev_info(&fd_hw_dev->pdev->dev, "-E. %s\n", __func__);
+	if (drv_data) {
+		mtk_fd_dev_core_release(pdev, &drv_data->fd_dev);
+		fd_hw_dev = &drv_data->fd_hw_dev;
+	} else {
+		dev_err(&pdev->dev, "Can't find fd driver data\n");
+		return -EINVAL;
+	}
+
+	mtk_fd_clk_ctrl(fd_hw_dev, clock_off);
+	pm_runtime_disable(&pdev->dev);
+
+	i4IRQ = platform_get_irq(pdev, 0);
+	free_irq(i4IRQ, NULL);
+	kfree(fd_hw_dev);
+
+	dev_info(&fd_hw_dev->pdev->dev, "-X. %s\n", __func__);
+	return 0;
+}
+
+static int mtk_fd_suspend(struct device *dev)
+{
+	struct mtk_isp_fd_drv_data *fd_drv;
+	struct platform_device *pdev;
+	struct mtk_fd_drv_dev *fd_hw_dev;
+
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	fd_drv = dev_get_drvdata(dev);
+	fd_hw_dev = &fd_drv->fd_hw_dev;
+	pdev = fd_hw_dev->pdev;
+
+	dev_info(&pdev->dev, "E.%s\n", __func__);
+
+	if (atomic_read(&fd_hw_dev->fd_ctx.fd_user_cnt) > 0) {
+		mtk_fd_clk_ctrl(fd_hw_dev, clock_off);
+		dev_info(&pdev->dev, "Disable clock\n");
+	}
+
+	dev_info(&pdev->dev, "X.%s\n", __func__);
+	return 0;
+}
+
+static int mtk_fd_resume(struct device *dev)
+{
+	struct mtk_isp_fd_drv_data *fd_drv;
+	struct platform_device *pdev;
+	struct mtk_fd_drv_dev *fd_hw_dev;
+
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	fd_drv = dev_get_drvdata(dev);
+	fd_hw_dev = &fd_drv->fd_hw_dev;
+	pdev = fd_hw_dev->pdev;
+
+	dev_info(&pdev->dev, "E.%s\n", __func__);
+
+	if (atomic_read(&fd_hw_dev->fd_ctx.fd_user_cnt) > 0) {
+		mtk_fd_clk_ctrl(fd_hw_dev, clock_on);
+		dev_info(&pdev->dev, "Enable clock\n");
+	}
+
+	dev_info(&pdev->dev, "X.%s\n", __func__);
+	return 0;
+}
+
+static const struct dev_pm_ops mtk_fd_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(mtk_fd_suspend, mtk_fd_resume)
+	SET_RUNTIME_PM_OPS(mtk_fd_suspend, mtk_fd_resume, NULL)
+};
+
+static struct platform_driver mtk_fd_driver = {
+	.probe   = mtk_fd_probe,
+	.remove  = mtk_fd_remove,
+	.driver  = {
+		.name  = FD_DRVNAME,
+		.of_match_table = mtk_fd_of_ids,
+		.pm = &mtk_fd_pm_ops,
+	}
+};
+module_platform_driver(mtk_fd_driver);
+
+MODULE_DESCRIPTION("Mediatek FD driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd.h b/drivers/media/platform/mtk-isp/fd/mtk_fd.h
new file mode 100644
index 0000000..6cae440
--- /dev/null
+++ b/drivers/media/platform/mtk-isp/fd/mtk_fd.h
@@ -0,0 +1,127 @@
+/* SPDX-License-Identifier: GPL-2.0
+ * Copyright (C) 2015 MediaTek Inc.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MTK_FD_H__
+#define __MTK_FD_H__
+
+#define MTK_FD_MAX_NO		(1024)
+#define MAX_FACE_SEL_NUM	(MTK_FD_MAX_NO + 2)
+
+/* The max number of face sizes could be detected, for feature scaling */
+#define FACE_SIZE_NUM_MAX	(14)
+
+/* FACE_SIZE_NUM_MAX + 1, first scale for input image W/H */
+#define FD_SCALE_NUM		(15)
+
+/* Number of Learning data sets */
+#define LEARNDATA_NUM		(18)
+
+struct fd_buffer {
+	__u64 va;	/* used by APMCU access */
+	__u32 pa;	/* used by CM4 access */
+	__u32 iova;	/* used by HW access */
+} __packed;
+
+enum fd_img_format {
+	FMT_VYUY = 2,
+	FMT_UYVY,
+	FMT_YVYU,
+	FMT_YUYV,
+};
+
+enum fd_mode {
+	FDMODE,
+	SDMODE,
+	VFB,
+	CFB,
+};
+
+struct fd_face_result {
+	__u64 face_idx:12, type:1, x0:10, y0:10, x1:10, y1:10,
+		fcv:18, rip_dir:4, rop_dir:3, det_size:5;
+};
+
+struct fd_user_output {
+	struct fd_face_result face[MAX_FACE_SEL_NUM];
+	__u16 face_number;
+};
+
+struct v4l2_fd_param {
+	u32 frame_id;
+	u16 src_img_h;
+	u16 src_img_w;
+	struct fd_buffer src_img;
+	struct fd_buffer fd_user_param;
+	struct fd_buffer fd_user_result;
+} __packed;
+
+/**
+ * mtk_fd_enqueue - enqueue to fd driver
+ *
+ * @pdev: FD platform device
+ * @fdvtconfig: frame parameters from V4L2 common Framework
+ *
+ * Enqueue a frame to fd driver.
+ *
+ * Return: Return 0 if successfully, otherwise it is failed.
+ */
+int mtk_fd_enqueue(struct platform_device *pdev,
+		   struct v4l2_fd_param *fd_param);
+
+/**
+ * mtk_fd_open -
+ *
+ * @pdev: FD platform device
+ *
+ * Open the FD device driver
+ *
+ * Return: Return 0 if success, otherwise it is failed.
+ */
+int mtk_fd_open(struct platform_device *pdev);
+
+/**
+ * mtk_fd_release -
+ *
+ * @pdev: FD platform device
+ *
+ * Enqueue a frame to FD driver.
+ *
+ * Return: Return 0 if success, otherwise it is failed.
+ */
+int mtk_fd_release(struct platform_device *pdev);
+
+/**
+ * mtk_fd_streamon -
+ *
+ * @pdev: FD platform device
+ * @id: device context id
+ *
+ * Stream on
+ *
+ * Return: Return 0 if success, otherwise it is failed.
+ */
+int mtk_fd_streamon(struct platform_device *pdev, u16 id);
+
+/**
+ * mtk_fd_streamoff -
+ *
+ * @pdev: FD platform device
+ * @id: device context id
+ *
+ * Stream off
+ *
+ * Return: Return 0 if success, otherwise it is failed.
+ */
+int mtk_fd_streamoff(struct platform_device *pdev, u16 id);
+
+#endif/*__MTK_FD_H__*/
-- 
1.9.1

Re: [RFC PATCH V0 7/7] [media] platform: mtk-isp: Add Mediatek FD driver

[Tomasz Figa]

+CC Shik and Keiichi

On Wed, Feb 20, 2019 at 4:53 PM Jerry-ch Chen
<Jerry-Ch.chen@mediatek.com> wrote:
>
> This patch adds the driver of Face Detection (FD) unit in
> Mediatek camera system, providing face detection function.
>
> The mtk-isp directory will contain drivers for multiple IP
> blocks found in Mediatek ISP system. It will include ISP Pass 1
> driver (CAM), sensor interface driver, DIP driver and face
> detection driver.
>
> Signed-off-by: Jerry-ch Chen <jerry-ch.chen@mediatek.com>
> ---
>  drivers/media/platform/mtk-isp/Makefile            |   16 +
>  drivers/media/platform/mtk-isp/fd/Makefile         |   38 +
>  drivers/media/platform/mtk-isp/fd/mtk_fd-core.h    |  157 +++
>  drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h     |  299 ++++++
>  .../platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c      |  912 +++++++++++++++++
>  drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c     |  355 +++++++
>  drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h     |  198 ++++
>  .../media/platform/mtk-isp/fd/mtk_fd-smem-drv.c    |  452 +++++++++
>  drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h    |   25 +
>  .../media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c   | 1046 ++++++++++++++++++++
>  drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c    |  114 +++
>  drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h    |   36 +
>  drivers/media/platform/mtk-isp/fd/mtk_fd.c         |  730 ++++++++++++++
>  drivers/media/platform/mtk-isp/fd/mtk_fd.h         |  127 +++
>  14 files changed, 4505 insertions(+)
>  create mode 100644 drivers/media/platform/mtk-isp/Makefile
>  create mode 100644 drivers/media/platform/mtk-isp/fd/Makefile
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-core.h
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-smem-drv.c
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd.c
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd.h
>
> diff --git a/drivers/media/platform/mtk-isp/Makefile b/drivers/media/platform/mtk-isp/Makefile
> new file mode 100644
> index 0000000..5e3a9aa
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/Makefile
> @@ -0,0 +1,16 @@
> +#
> +# Copyright (C) 2018 MediaTek Inc.
> +#
> +# This program is free software: you can redistribute it and/or modify
> +# it under the terms of the GNU General Public License version 2 as
> +# published by the Free Software Foundation.
> +#
> +# This program is distributed in the hope that it will be useful,
> +# but WITHOUT ANY WARRANTY; without even the implied warranty of
> +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
> +# GNU General Public License for more details.
> +#
> +
> +ifeq ($(CONFIG_VIDEO_MEDIATEK_FD_SUPPORT),y)
> +obj-y += fd/
> +endif
> diff --git a/drivers/media/platform/mtk-isp/fd/Makefile b/drivers/media/platform/mtk-isp/fd/Makefile
> new file mode 100644
> index 0000000..ac168c1
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/Makefile
> @@ -0,0 +1,38 @@
> +#
> +# Copyright (C) 2018 MediaTek Inc.
> +#
> +# This program is free software: you can redistribute it and/or modify
> +# it under the terms of the GNU General Public License version 2 as
> +# published by the Free Software Foundation.
> +#
> +# This program is distributed in the hope that it will be useful,
> +# but WITHOUT ANY WARRANTY; without even the implied warranty of
> +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
> +# GNU General Public License for more details.
> +#
> +
> +$(info "FD: makefile start srctree: $(srctree)")
> +ccflags-y += -I$(srctree)/drivers/media/platform/mtk-vpu
> +ccflags-y += -I$(srctree)/drivers/media/platform/mtk-isp/fd
> +#ccflags-y += -I$(srctree)/drivers/media/platform/mtk-isp/common
> +
> +obj-y += mtk_fd.o
> +obj-y += mtk_fd-v4l2.o
> +
> +# To provide alloc context managing memory shared
> +# between CPU and FD coprocessor
> +mtk_fd_smem-objs := \
> +mtk_fd-smem-drv.o
> +
> +obj-y += mtk_fd_smem.o
> +
> +# Utilits to provide frame-based streaming model
> +# with v4l2 user interfaces
> +mtk_fd_util-objs := \
> +mtk_fd-dev.o \
> +mtk_fd-v4l2-util.o \
> +mtk_fd-dev-ctx-core.o
> +
> +obj-y += mtk_fd_util.o
> +
> +$(info "FD: makefile end")
> diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-core.h b/drivers/media/platform/mtk-isp/fd/mtk_fd-core.h
> new file mode 100644
> index 0000000..a7c1e1d
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-core.h
> @@ -0,0 +1,157 @@
> +/* SPDX-License-Identifier: GPL-2.0
> + * Copyright (C) 2015 MediaTek Inc.
> + *
> + * This program is free software: you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
> + * GNU General Public License for more details.
> + */
> +
> +#ifndef __MTK_FD_CORE_H__
> +#define __MTK_FD_CORE_H__
> +
> +#define SIG_ERESTARTSYS 512
> +
> +#ifndef CONFIG_MTK_CLKMGR
> +#include <linux/clk.h>
> +#endif
> +
> +#ifdef CONFIG_MTK_CLKMGR
> +#include <mach/mt_clkmgr.h>
> +#endif
> +
> +#include "mtk_fd-dev.h"
> +#include "mtk_fd-ctx.h"
> +
> +#include <linux/io.h>
> +
> +#define FD_WR32(v, a) \
> +do { \
> +       __raw_writel((v), (void __force __iomem *)((a))); \
> +       mb(); /* ensure written */ \
> +} while (0)
> +
> +#define FD_RD32(addr)                  ioread32((void *)addr)
> +
> +#define        FD_INT_EN       (0x15c)
> +#define        FD_INT          (0x168)
> +#define        FD_RESULT       (0x178)
> +#define FD_IRQ_MASK    (0x001)
> +
> +#define RS_BUF_SIZE_MAX        (2288788)
> +#define VA_OFFSET      (0xffff000000000000)
> +
> +enum fd_irq {
> +       FD_IRQ_IDX = 0,
> +       FD_IRQ_IDX_NUM
> +};
> +
> +enum fd_state {
> +       FD_INI,
> +       FD_ENQ,
> +       FD_CBD,
> +};
> +
> +enum stream_stat {
> +       STREAM_OFF,
> +       STREAM_ON,
> +};
> +
> +struct ipi_fd_enq_param {
> +       u8 source_img_fmt;
> +       struct fd_buffer output_addr;
> +       struct fd_buffer src_y;
> +       struct fd_buffer src_uv;
> +       struct fd_buffer config_addr;
> +} __packed;
> +
> +struct fd_manager_ctx {
> +       struct fd_buffer learn_data_buf[2][LEARNDATA_NUM];
> +       struct fd_buffer fd_config;
> +       struct fd_buffer rs_config;
> +       struct fd_buffer fd_result;
> +       struct fd_buffer rs_result;
> +       struct fd_buffer src_img;
> +} __packed;
> +
> +struct mtk_fd_drv_ctx {
> +       struct sg_table sgtable;
> +       u32 frame_id;
> +       struct fd_buffer rs_result;
> +       struct fd_buffer scp_mem;
> +       struct platform_device *vpu_pdev;
> +
> +       atomic_t fd_enque_cnt;
> +       atomic_t fd_stream_cnt;
> +       atomic_t fd_user_cnt;
> +};
> +
> +struct mtk_fd_drv_dev {
> +       struct platform_device *pdev;
> +
> +       dev_t fd_devno;
> +       struct cdev   fd_cdev;
> +       struct class *fd_class;
> +       struct mtk_fd_drv_ctx fd_ctx;
> +
> +       struct device *larb_dev;
> +       struct clk *fd_clk;
> +       enum fd_state state;
> +       enum stream_stat streaming;
> +
> +       wait_queue_head_t wq;
> +       u32 fd_irq_result;
> +
> +       void __iomem *fd_base;
> +};
> +
> +struct mtk_isp_fd_drv_data {
> +       struct mtk_fd_dev fd_dev;
> +       struct mtk_fd_drv_dev fd_hw_dev;
> +} __packed;
> +
> +static inline struct mtk_fd_drv_dev *get_fd_hw_device(struct device *dev)
> +{
> +       struct mtk_isp_fd_drv_data *drv_data =
> +               dev_get_drvdata(dev);
> +       if (drv_data)
> +               return &drv_data->fd_hw_dev;
> +       else
> +               return NULL;
> +}
> +
> +#define mtk_fd_us_to_jiffies(us) \
> +       ((((unsigned long)(us) / 1000) * HZ + 512) >> 10)
> +
> +#define mtk_fd_hw_dev_to_drv(__fd_hw_dev) \
> +       container_of(__fd_hw_dev, \
> +       struct mtk_isp_fd_drv_data, fd_hw_dev)
> +
> +#define mtk_fd_ctx_to_drv(__fd_ctx) \
> +       container_of(__fd_ctx, \
> +       struct mtk_isp_fd_drv_data, fd_hw_dev.fd_ctx)
> +
> +enum fd_scp_cmd {
> +       FD_CMD_INIT,
> +       FD_CMD_ENQ,
> +       FD_CMD_EXIT,
> +};
> +
> +enum fd_clk {
> +       clock_off = 0,
> +       clock_on,
> +};
> +
> +struct ipi_message {
> +       u8 cmd_id;
> +       union {
> +               struct fd_buffer fd_manager;
> +               struct v4l2_fd_param fd_param;
> +       };
> +} __packed;
> +
> +#endif/*__MTK_FD_CORE_H__*/
> diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h b/drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h
> new file mode 100644
> index 0000000..d78a3af
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h
> @@ -0,0 +1,299 @@
> +/* SPDX-License-Identifier: GPL-2.0
> + * Copyright (c) 2018 MediaTek Inc.
> + * Author: Frederic Chen <frederic.chen@mediatek.com>
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> + * GNU General Public License for more details.
> + */
> +
> +#ifndef __MTK_FD_CTX_H__
> +#define __MTK_FD_CTX_H__
> +
> +#include <linux/types.h>
> +#include <linux/videodev2.h>
> +#include <media/v4l2-ctrls.h>
> +#include <media/videobuf2-core.h>
> +#include <media/v4l2-subdev.h>
> +
> +#define MTK_FD_CTX_QUEUES (16)
> +#define MTK_FD_CTX_FRAME_BUNDLE_BUFFER_MAX (MTK_FD_CTX_QUEUES)
> +#define MTK_FD_CTX_DESC_MAX (MTK_FD_CTX_QUEUES)
> +
> +#define MTK_FD_CTX_MODE_DEBUG_OFF (0)
> +#define MTK_FD_CTX_MODE_DEBUG_BYPASS_JOB_TRIGGER (1)
> +#define MTK_FD_CTX_MODE_DEBUG_BYPASS_ALL (2)
> +
> +#define MTK_FD_GET_CTX_ID_FROM_SEQUENCE(sequence) \
> +       ((sequence) >> 16 & 0x0000FFFF)
> +
> +#define MTK_FD_CTX_META_BUF_DEFAULT_SIZE (1110 * 1024)
> +
> +struct mtk_fd_ctx;
> +struct mtk_fd_ctx_finish_param;
> +
> +/**
> + * Attributes setup by device context owner
> + */
> +struct mtk_fd_ctx_queue_desc {
> +       int id;
> +       /* id of the context queue */
> +       char *name;
> +       /* Will be exported to media entity name */
> +       int capture;
> +       /**
> +        * 1 for capture queue (device to user),
> +        * 0 for output queue (from user to device)
> +        */
> +       int image;
> +       /* 1 for image, 0 for meta data */
> +       unsigned int dma_port;
> +       /*The dma port associated to the buffer*/
> +       struct mtk_fd_ctx_format *fmts;
> +       int num_fmts;
> +       /* Default format of this queue */
> +       int default_fmt_idx;
> +};
> +
> +/**
> + * Supported format and the information used for
> + * size calculation
> + */
> +struct mtk_fd_ctx_meta_format {
> +       u32 dataformat;
> +       u32 max_buffer_size;
> +       u8 flags;
> +};
> +
> +/**
> + * MDP module's private format definitation
> + * (the same as struct mdp_format)
> + * It will be removed and changed to MDP's external interface
> + * after the integration with MDP module.
> + */
> +struct mtk_fd_ctx_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 mtk_fd_ctx_format {
> +       union {
> +               struct mtk_fd_ctx_meta_format meta;
> +               struct mtk_fd_ctx_mdp_format img;
> +       } fmt;
> +};
> +
> +union mtk_v4l2_fmt {
> +       struct v4l2_pix_format_mplane pix_mp;
> +       struct v4l2_meta_format meta;
> +};
> +
> +/* Attributes setup by device context owner */
> +struct mtk_fd_ctx_queues_setting {
> +       int master;
> +       /* The master input node to trigger the frame data enqueue */
> +       struct mtk_fd_ctx_queue_desc *output_queue_descs;
> +       int total_output_queues;
> +       struct mtk_fd_ctx_queue_desc *capture_queue_descs;
> +       int total_capture_queues;
> +};
> +
> +struct mtk_fd_ctx_queue_attr {
> +       int master;
> +       int input_offset;
> +       int total_num;
> +};
> +
> +/**
> + * Video node context. Since we use
> + * mtk_fd_ctx_frame_bundle to manage enqueued
> + * buffers by frame now, we don't use bufs filed of
> + * mtk_fd_ctx_queue now
> + */
> +struct mtk_fd_ctx_queue {
> +       union mtk_v4l2_fmt fmt;
> +       struct mtk_fd_ctx_format *ctx_fmt;
> +
> +       unsigned int width_pad;
> +       /* bytesperline, reserved */
> +       struct mtk_fd_ctx_queue_desc desc;
> +       unsigned int buffer_usage;
> +       /* Current buffer usage of the queue */
> +       int rotation;
> +       struct list_head bufs;
> +       /* Reserved, not used now */
> +};
> +
> +enum mtk_fd_ctx_frame_bundle_state {
> +       MTK_FD_CTX_FRAME_NEW,
> +       /* Not allocated */
> +       MTK_FD_CTX_FRAME_PREPARED,
> +       /* Allocated but has not be processed */
> +       MTK_FD_CTX_FRAME_PROCESSING,
> +       /* Queued, waiting to be filled */
> +};
> +
> +/**
> + * The definiation is compatible with FD driver's state definiation
> + * currently and will be decoupled after further integration
> + */
> +enum mtk_fd_ctx_frame_data_state {
> +       MTK_FD_CTX_FRAME_DATA_EMPTY = 0, /* FRAME_STATE_INIT */
> +       MTK_FD_CTX_FRAME_DATA_DONE = 3, /* FRAME_STATE_DONE */
> +       MTK_FD_CTX_FRAME_DATA_STREAMOFF_DONE = 4, /*FRAME_STATE_STREAMOFF*/
> +       MTK_FD_CTX_FRAME_DATA_ERROR = 5, /*FRAME_STATE_ERROR*/
> +};
> +
> +struct mtk_fd_ctx_frame_bundle {
> +       struct mtk_fd_ctx_buffer*
> +               buffers[MTK_FD_CTX_FRAME_BUNDLE_BUFFER_MAX];
> +       int id;
> +       int num_img_capture_bufs;
> +       int num_img_output_bufs;
> +       int num_meta_capture_bufs;
> +       int num_meta_output_bufs;
> +       int last_index;
> +       int state;
> +       struct list_head list;
> +};
> +
> +struct mtk_fd_ctx_frame_bundle_list {
> +       struct list_head list;
> +};
> +
> +struct mtk_fd_ctx {
> +       struct platform_device *pdev;
> +       struct platform_device *smem_device;
> +       unsigned short ctx_id;
> +       char device_name[12];
> +       const struct mtk_fd_ctx_ops *ops;
> +       struct mtk_fd_dev_node_mapping *mtk_fd_dev_node_map;
> +       unsigned int dev_node_num;
> +       struct mtk_fd_ctx_queue queue[MTK_FD_CTX_QUEUES];
> +       struct mtk_fd_ctx_queue_attr queues_attr;
> +       atomic_t frame_param_sequence;
> +       int streaming;
> +       void *img_vb2_alloc_ctx;
> +       void *smem_vb2_alloc_ctx;
> +       struct v4l2_subdev_fh *fh;
> +       struct mtk_fd_ctx_frame_bundle frame_bundles[VB2_MAX_FRAME];
> +       struct mtk_fd_ctx_frame_bundle_list processing_frames;
> +       struct mtk_fd_ctx_frame_bundle_list free_frames;
> +       int enabled_dma_ports;
> +       int num_frame_bundle;
> +       int mode; /* Reserved for debug */
> +       spinlock_t qlock;
> +};
> +
> +enum mtk_fd_ctx_buffer_state {
> +       MTK_FD_CTX_BUFFER_NEW,
> +       MTK_FD_CTX_BUFFER_PROCESSING,
> +       MTK_FD_CTX_BUFFER_DONE,
> +       MTK_FD_CTX_BUFFER_FAILED,
> +};
> +
> +struct mtk_fd_ctx_buffer {
> +       union mtk_v4l2_fmt fmt;
> +       struct mtk_fd_ctx_format *ctx_fmt;
> +       int capture;
> +       int image;
> +       int frame_id;
> +       int user_sequence; /* Sequence number assigned by user */
> +       dma_addr_t daddr;
> +       void *vaddr;
> +       phys_addr_t paddr;
> +       unsigned int queue;
> +       unsigned int buffer_usage;
> +       enum mtk_fd_ctx_buffer_state state;
> +       int rotation;
> +       struct list_head list;
> +};
> +
> +struct mtk_fd_ctx_desc {
> +       char *proc_dev_phandle;
> +       /* The context device's compatble string name in device tree*/
> +       int (*init)(struct mtk_fd_ctx *ctx);
> +       /* configure the core functions of the device context */
> +};
> +
> +struct mtk_fd_ctx_init_table {
> +       int total_dev_ctx;
> +       struct mtk_fd_ctx_desc *ctx_desc_tbl;
> +};
> +
> +struct mtk_fd_ctx_finish_param {
> +       unsigned int frame_id;
> +       u64 timestamp;
> +       unsigned int state;
> +};
> +
> +bool mtk_fd_ctx_is_streaming(struct mtk_fd_ctx *ctx);
> +
> +int mtk_fd_ctx_core_job_finish(struct mtk_fd_ctx *ctx,
> +                              struct mtk_fd_ctx_finish_param *param);
> +
> +int mtk_fd_ctx_core_init(struct mtk_fd_ctx *ctx,
> +                        struct platform_device *pdev, int ctx_id,
> +                        struct mtk_fd_ctx_desc *ctx_desc,
> +                        struct platform_device *proc_pdev,
> +                        struct platform_device *smem_pdev);
> +
> +int mtk_fd_ctx_core_exit(struct mtk_fd_ctx *ctx);
> +
> +void mtk_fd_ctx_buf_init(struct mtk_fd_ctx_buffer *b, unsigned int queue,
> +                        dma_addr_t daddr);
> +
> +enum mtk_fd_ctx_buffer_state
> +       mtk_fd_ctx_get_buffer_state(struct mtk_fd_ctx_buffer *b);
> +
> +int mtk_fd_ctx_next_global_frame_sequence(struct mtk_fd_ctx *ctx, int locked);
> +
> +int mtk_fd_ctx_core_queue_setup
> +       (struct mtk_fd_ctx *ctx,
> +        struct mtk_fd_ctx_queues_setting *queues_setting);
> +
> +int mtk_fd_ctx_core_finish_param_init(void *param, int frame_id, int state);
> +
> +int mtk_fd_ctx_finish_frame(struct mtk_fd_ctx *dev_ctx,
> +                           struct mtk_fd_ctx_frame_bundle *frame_bundle,
> +                           int done);
> +
> +int mtk_fd_ctx_frame_bundle_init(struct mtk_fd_ctx_frame_bundle *frame_bundle);
> +
> +void mtk_fd_ctx_frame_bundle_add(struct mtk_fd_ctx *ctx,
> +                                struct mtk_fd_ctx_frame_bundle *bundle,
> +                                struct mtk_fd_ctx_buffer *ctx_buf);
> +
> +int mtk_fd_ctx_trigger_job(struct mtk_fd_ctx *dev_ctx,
> +                          struct mtk_fd_ctx_frame_bundle *bundle_data);
> +
> +int mtk_fd_ctx_fmt_set_img(struct mtk_fd_ctx *dev_ctx, int queue_id,
> +                          struct v4l2_pix_format_mplane *user_fmt,
> +                          struct v4l2_pix_format_mplane *node_fmt);
> +
> +int mtk_fd_ctx_fmt_set_meta(struct mtk_fd_ctx *dev_ctx, int queue_id,
> +                           struct v4l2_meta_format *user_fmt,
> +                           struct v4l2_meta_format *node_fmt);
> +
> +int mtk_fd_ctx_format_load_default_fmt(struct mtk_fd_ctx_queue *queue,
> +                                      struct v4l2_format *fmt_to_fill);
> +
> +int mtk_fd_ctx_streamon(struct mtk_fd_ctx *dev_ctx);
> +int mtk_fd_ctx_streamoff(struct mtk_fd_ctx *dev_ctx);
> +int mtk_fd_ctx_release(struct mtk_fd_ctx *dev_ctx);
> +int mtk_fd_ctx_open(struct mtk_fd_ctx *dev_ctx);
> +int mtk_fd_ctx_fd_init(struct mtk_fd_ctx *ctx);
> +
> +#endif /*__MTK_FD_CTX_H__*/
> diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c b/drivers/media/platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c
> new file mode 100644
> index 0000000..7314436
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c
> @@ -0,0 +1,912 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * Copyright (c) 2018 MediaTek Inc.
> + * Author: Frederic Chen <frederic.chen@mediatek.com>
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> + * GNU General Public License for more details.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/platform_device.h>
> +#include <media/videobuf2-dma-contig.h>
> +#include <linux/dma-mapping.h>
> +
> +#include "mtk_fd.h"
> +#include "mtk_fd-dev.h"
> +#include "mtk_fd-smem.h"
> +#include "mtk_fd-v4l2.h"
> +
> +#if KERNEL_VERSION(4, 8, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +#include <linux/dma-attrs.h>
> +#endif
> +
> +struct vb2_v4l2_buffer *mtk_fd_ctx_buffer_get_vb2_v4l2_buffer
> +(struct mtk_fd_ctx_buffer *ctx_buf)
> +{
> +       struct mtk_fd_dev_buffer *dev_buf = NULL;
> +
> +       if (!ctx_buf) {
> +               pr_err("Failed to convert ctx_buf to dev_buf: Null pointer\n");
> +               return NULL;
> +       }
> +
> +       dev_buf = mtk_fd_ctx_buf_to_dev_buf(ctx_buf);
> +
> +       return &dev_buf->m2m2_buf.vbb;
> +}
> +
> +int mtk_fd_ctx_core_queue_setup(struct mtk_fd_ctx *ctx,
> +                               struct mtk_fd_ctx_queues_setting *
> +                               queues_setting)
> +{
> +       int queue_idx = 0;
> +       int i = 0;
> +
> +       for (i = 0; i < queues_setting->total_output_queues; i++) {
> +               struct mtk_fd_ctx_queue_desc *queue_desc =
> +                       &queues_setting->output_queue_descs[i];
> +               ctx->queue[queue_idx].desc = *queue_desc;
> +               queue_idx++;
> +       }
> +
> +       /* Setup the capture queue */
> +       for (i = 0; i < queues_setting->total_capture_queues; i++) {
> +               struct mtk_fd_ctx_queue_desc *queue_desc =
> +                       &queues_setting->capture_queue_descs[i];
> +               ctx->queue[queue_idx].desc = *queue_desc;
> +               queue_idx++;
> +       }
> +
> +       ctx->queues_attr.master = queues_setting->master;
> +       ctx->queues_attr.total_num = queue_idx;
> +       ctx->dev_node_num = ctx->queues_attr.total_num;
> +       strcpy(ctx->device_name, MTK_FD_DEV_NAME);
> +       return 0;
> +}
> +
> +/* Mediatek FD context core initialization */
> +int mtk_fd_ctx_core_init(struct mtk_fd_ctx *ctx,
> +                        struct platform_device *pdev, int ctx_id,
> +                        struct mtk_fd_ctx_desc *ctx_desc,
> +                        struct platform_device *proc_pdev,
> +                        struct platform_device *smem_pdev)
> +{
> +       /* Initialize main data structure */
> +       int r = 0;
> +
> +#if KERNEL_VERSION(4, 8, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +       ctx->smem_vb2_alloc_ctx =
> +               vb2_dma_contig_init_ctx(&smem_pdev->dev);
> +       ctx->img_vb2_alloc_ctx =
> +               vb2_dma_contig_init_ctx(&pdev->dev);
> +#else
> +       ctx->smem_vb2_alloc_ctx = &smem_pdev->dev;
> +       ctx->img_vb2_alloc_ctx = &pdev->dev;
> +#endif
> +       if (IS_ERR((__force void *)ctx->smem_vb2_alloc_ctx))
> +               pr_err("Failed to alloc vb2 dma context: smem_vb2_alloc_ctx");
> +
> +       if (IS_ERR((__force void *)ctx->img_vb2_alloc_ctx))
> +               pr_err("Failed to alloc vb2 dma context: img_vb2_alloc_ctx");
> +
> +       ctx->pdev = pdev;
> +       ctx->ctx_id = ctx_id;
> +       /* keep th smem pdev to use related iommu functions */
> +       ctx->smem_device = smem_pdev;
> +
> +       /* Will set default enabled after passing the unit test */
> +       ctx->mode = MTK_FD_CTX_MODE_DEBUG_OFF;
> +
> +       /* initialized the global frame index of the device context */
> +       atomic_set(&ctx->frame_param_sequence, 0);
> +       spin_lock_init(&ctx->qlock);
> +
> +       /* setup the core operation of the device context */
> +       if (ctx_desc && ctx_desc->init)
> +               r = ctx_desc->init(ctx);
> +
> +       return r;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_core_init);
> +
> +int mtk_fd_ctx_core_exit(struct mtk_fd_ctx *ctx)
> +{
> +#if KERNEL_VERSION(4, 8, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +       vb2_dma_contig_cleanup_ctx(ctx->smem_vb2_alloc_ctx);
> +       vb2_dma_contig_cleanup_ctx(ctx->img_vb2_alloc_ctx);
> +#else
> +       ctx->smem_vb2_alloc_ctx = NULL;
> +       ctx->img_vb2_alloc_ctx = NULL;
> +#endif
> +       return 0;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_core_exit);
> +
> +int mtk_fd_ctx_next_global_frame_sequence(struct mtk_fd_ctx *ctx, int locked)
> +{
> +       int global_frame_sequence =
> +               atomic_inc_return(&ctx->frame_param_sequence);
> +
> +       if (!locked)
> +               spin_lock(&ctx->qlock);
> +
> +       global_frame_sequence =
> +               (global_frame_sequence & 0x0000FFFF) | (ctx->ctx_id << 16);
> +
> +       if (!locked)
> +               spin_unlock(&ctx->qlock);
> +
> +       return global_frame_sequence;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_next_global_frame_sequence);
> +
> +static void mtk_fd_ctx_buffer_done
> +       (struct mtk_fd_ctx_buffer *ctx_buf, int state)
> +{
> +               if (!ctx_buf || state != MTK_FD_CTX_BUFFER_DONE ||
> +                   state != MTK_FD_CTX_BUFFER_FAILED)
> +                       return;
> +
> +               ctx_buf->state = state;
> +}
> +
> +struct mtk_fd_ctx_frame_bundle *mtk_fd_ctx_get_processing_frame
> +(struct mtk_fd_ctx *dev_ctx, int frame_id)
> +{
> +       struct mtk_fd_ctx_frame_bundle *frame_bundle = NULL;
> +
> +       spin_lock(&dev_ctx->qlock);
> +
> +       list_for_each_entry(frame_bundle,
> +                           &dev_ctx->processing_frames.list, list) {
> +               if (frame_bundle->id == frame_id) {
> +                       spin_unlock(&dev_ctx->qlock);
> +                       return frame_bundle;
> +               }
> +       }
> +
> +       spin_unlock(&dev_ctx->qlock);
> +
> +       return NULL;
> +}
> +
> +/**
> + * structure mtk_fd_ctx_finish_param must be the first elemt of param
> + * So that the buffer can be return to vb2 queue successfully
> + */
> +int mtk_fd_ctx_core_finish_param_init(void *param, int frame_id, int state)
> +{
> +       struct mtk_fd_ctx_finish_param *fram_param =
> +               (struct mtk_fd_ctx_finish_param *)param;
> +       fram_param->frame_id = frame_id;
> +       fram_param->state = state;
> +       return 0;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_core_finish_param_init);
> +
> +void mtk_fd_ctx_frame_bundle_add(struct mtk_fd_ctx *ctx,
> +                                struct mtk_fd_ctx_frame_bundle *bundle,
> +                                struct mtk_fd_ctx_buffer *ctx_buf)
> +{
> +       int queue_id = 0;
> +       struct mtk_fd_ctx_queue *ctx_queue = NULL;
> +
> +       if (!bundle || !ctx_buf) {
> +               pr_warn("Add buffer to frame bundle failed, bundle(%llx),buf(%llx)\n",
> +                       (long long)bundle, (long long)ctx_buf);
> +               return;
> +       }
> +
> +       queue_id = ctx_buf->queue;
> +
> +       if (bundle->buffers[queue_id])
> +               pr_warn("Queue(%d) buffer has alreay in this bundle, overwrite happen\n",
> +                       queue_id);
> +
> +       pr_debug("Add queue(%d) buffer%llx\n",
> +                queue_id, (unsigned long long)ctx_buf);
> +                bundle->buffers[queue_id] = ctx_buf;
> +
> +       /* Fill context queue related information */
> +       ctx_queue = &ctx->queue[queue_id];
> +
> +       if (!ctx_queue) {
> +               pr_err("Can't find ctx queue (%d)\n", queue_id);
> +               return;
> +       }
> +
> +       if (ctx->queue[ctx_buf->queue].desc.image) {
> +               if (ctx->queue[ctx_buf->queue].desc.capture)
> +                       bundle->num_img_capture_bufs++;
> +               else
> +                       bundle->num_img_output_bufs++;
> +       } else {
> +               if (ctx->queue[ctx_buf->queue].desc.capture)
> +                       bundle->num_meta_capture_bufs++;
> +               else
> +                       bundle->num_meta_output_bufs++;
> +       }
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_frame_bundle_add);
> +
> +void mtk_fd_ctx_buf_init(struct mtk_fd_ctx_buffer *b,
> +                        unsigned int queue, dma_addr_t daddr)
> +{
> +       b->state = MTK_FD_CTX_BUFFER_NEW;
> +       b->queue = queue;
> +       b->daddr = daddr;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_buf_init);
> +
> +enum mtk_fd_ctx_buffer_state
> +       mtk_fd_ctx_get_buffer_state(struct mtk_fd_ctx_buffer *b)
> +{
> +       return b->state;
> +}
> +
> +bool mtk_fd_ctx_is_streaming(struct mtk_fd_ctx *ctx)
> +{
> +       return ctx->streaming;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_is_streaming);
> +
> +static int mtk_fd_ctx_free_frame(struct mtk_fd_ctx *dev_ctx,
> +                                struct mtk_fd_ctx_frame_bundle *frame_bundle)
> +{
> +       spin_lock(&dev_ctx->qlock);
> +
> +       frame_bundle->state = MTK_FD_CTX_FRAME_NEW;
> +       list_del(&frame_bundle->list);
> +       list_add_tail(&frame_bundle->list, &dev_ctx->free_frames.list);
> +
> +       spin_unlock(&dev_ctx->qlock);
> +
> +       return 0;
> +}
> +
> +int mtk_fd_ctx_core_job_finish(struct mtk_fd_ctx *dev_ctx,
> +                              struct mtk_fd_ctx_finish_param *param)
> +{
> +       int i = 0;
> +       struct platform_device *pdev = dev_ctx->pdev;
> +       struct mtk_fd_ctx_finish_param *fram_param =
> +               (struct mtk_fd_ctx_finish_param *)param;
> +       struct mtk_fd_dev *fd_dev = NULL;
> +       struct mtk_fd_ctx_frame_bundle *frame = NULL;
> +       enum vb2_buffer_state vbf_state = VB2_BUF_STATE_DONE;
> +       enum mtk_fd_ctx_buffer_state ctxf_state =
> +               MTK_FD_CTX_BUFFER_DONE;
> +       int user_sequence = 0;
> +
> +       const int ctx_id =
> +               MTK_FD_GET_CTX_ID_FROM_SEQUENCE(fram_param->frame_id);
> +       u64 timestamp = 0;
> +
> +       pr_debug("mtk_fd_ctx_core_job_finish_cb: param (%llx), platform_device(%llx)\n",
> +                (unsigned long long)param, (unsigned long long)pdev);
> +
> +       if (!dev_ctx)
> +               pr_err("dev_ctx can't be null, can't release the frame\n");
> +
> +       fd_dev = mtk_fd_ctx_to_dev(dev_ctx);
> +
> +       if (fram_param) {
> +               dev_info(&fd_dev->pdev->dev,
> +                        "CB recvied from ctx(%d), frame(%d), state(%d), fd_dev(%llx)\n",
> +                        ctx_id, fram_param->frame_id,
> +                        fram_param->state, (long long)fd_dev);
> +       } else {
> +               dev_err(&fd_dev->pdev->dev,
> +                       "CB recvied from ctx(%d), frame param is NULL\n",
> +                       ctx_id);
> +                       return -EINVAL;
> +       }
> +
> +       /* Get the buffers of the processed frame */
> +       frame = mtk_fd_ctx_get_processing_frame(&fd_dev->ctx,
> +                                               fram_param->frame_id);
> +
> +       if (!frame) {
> +               pr_err("Can't find the frame boundle, Frame(%d)\n",
> +                      fram_param->frame_id);
> +               return -EINVAL;
> +       }
> +
> +       if (fram_param->state == MTK_FD_CTX_FRAME_DATA_ERROR) {
> +               vbf_state = VB2_BUF_STATE_ERROR;
> +               ctxf_state = MTK_FD_CTX_BUFFER_FAILED;
> +       }
> +
> +       /**
> +        * Set the buffer's VB2 status so that
> +        * the user can dequeue the buffer
> +        */
> +       timestamp = ktime_get_ns();
> +       for (i = 0; i <= frame->last_index; i++) {
> +               struct mtk_fd_ctx_buffer *ctx_buf = frame->buffers[i];
> +
> +               if (!ctx_buf) {
> +                       dev_dbg(&fd_dev->pdev->dev,
> +                               "ctx_buf(queue id= %d) of frame(%d)is NULL\n",
> +                               i, fram_param->frame_id);
> +                       continue;
> +               } else {
> +                       struct vb2_v4l2_buffer *b =
> +                               mtk_fd_ctx_buffer_get_vb2_v4l2_buffer(ctx_buf);
> +                       b->vb2_buf.timestamp = ktime_get_ns();
> +                       user_sequence = ctx_buf->user_sequence;
> +                       mtk_fd_ctx_buffer_done(ctx_buf, ctxf_state);
> +                       mtk_fd_v4l2_buffer_done(&b->vb2_buf, vbf_state);
> +               }
> +       }
> +
> +       mtk_fd_ctx_free_frame(&fd_dev->ctx, frame);
> +       return 0;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_core_job_finish);
> +
> +int mtk_fd_ctx_finish_frame(struct mtk_fd_ctx *dev_ctx,
> +                           struct mtk_fd_ctx_frame_bundle *frame_bundle,
> +                           int done)
> +{
> +       spin_lock(&dev_ctx->qlock);
> +       frame_bundle->state = MTK_FD_CTX_FRAME_PROCESSING;
> +       list_add_tail(&frame_bundle->list, &dev_ctx->processing_frames.list);
> +       spin_unlock(&dev_ctx->qlock);
> +       return 0;
> +}
> +
> +static void set_img_fmt(struct v4l2_pix_format_mplane *mfmt_to_fill,
> +                       struct mtk_fd_ctx_format *ctx_fmt)
> +{
> +       int i = 0;
> +
> +       mfmt_to_fill->pixelformat = ctx_fmt->fmt.img.pixelformat;
> +       mfmt_to_fill->num_planes = ctx_fmt->fmt.img.num_planes;
> +
> +       pr_info("%s: Fmt(%d),w(%d),h(%d)\n",
> +               __func__,
> +               mfmt_to_fill->pixelformat,
> +               mfmt_to_fill->width,
> +               mfmt_to_fill->height);
> +
> +       /**
> +        * The implementation wil be adjust after integrating MDP module
> +        * since it provides the common format suppporting function
> +        */
> +       for (i = 0 ; i < mfmt_to_fill->num_planes; ++i) {
> +               int bpl = (mfmt_to_fill->width *
> +                          ctx_fmt->fmt.img.row_depth[i]) / 8;
> +               int sizeimage = (mfmt_to_fill->width * mfmt_to_fill->height *
> +                                ctx_fmt->fmt.img.depth[i]) / 8;
> +
> +               mfmt_to_fill->plane_fmt[i].bytesperline = bpl;
> +               mfmt_to_fill->plane_fmt[i].sizeimage = sizeimage;
> +               pr_info("plane(%d):bpl(%d),sizeimage(%u)\n",
> +                       i, bpl, mfmt_to_fill->plane_fmt[i].sizeimage);
> +       }
> +}
> +
> +static void set_meta_fmt(struct v4l2_meta_format *metafmt_to_fill,
> +                        struct mtk_fd_ctx_format *ctx_fmt)
> +{
> +       metafmt_to_fill->dataformat = ctx_fmt->fmt.meta.dataformat;
> +
> +       if (ctx_fmt->fmt.meta.max_buffer_size <= 0 ||
> +           ctx_fmt->fmt.meta.max_buffer_size >
> +           MTK_FD_CTX_META_BUF_DEFAULT_SIZE) {
> +               pr_warn("buf size of meta(%u) can't be 0, use default %u\n",
> +                       ctx_fmt->fmt.meta.dataformat,
> +                       MTK_FD_CTX_META_BUF_DEFAULT_SIZE);
> +               metafmt_to_fill->buffersize = MTK_FD_CTX_META_BUF_DEFAULT_SIZE;
> +       } else {
> +               pr_info("Load the meta size setting %u\n",
> +                       ctx_fmt->fmt.meta.max_buffer_size);
> +               metafmt_to_fill->buffersize = ctx_fmt->fmt.meta.max_buffer_size;
> +       }
> +}
> +
> +/* Get the default format setting */
> +int mtk_fd_ctx_format_load_default_fmt(struct mtk_fd_ctx_queue *queue,
> +                                      struct v4l2_format *fmt_to_fill)
> +{
> +       struct mtk_fd_ctx_format *ctx_fmt = NULL;
> +
> +       if (queue->desc.num_fmts == 0)
> +               return 0; /* no format support list associated to this queue */
> +
> +       if (queue->desc.default_fmt_idx >= queue->desc.num_fmts) {
> +               pr_warn("Queue(%s) err: default idx(%d) must < num_fmts(%d)\n",
> +                       queue->desc.name, queue->desc.default_fmt_idx,
> +                       queue->desc.num_fmts);
> +               queue->desc.default_fmt_idx = 0;
> +               pr_warn("Queue(%s) : reset default idx(%d)\n",
> +                       queue->desc.name, queue->desc.default_fmt_idx);
> +       }
> +
> +       ctx_fmt = &queue->desc.fmts[queue->desc.default_fmt_idx];
> +
> +       /* Check the type of the buffer */
> +       if (queue->desc.image) {
> +               struct v4l2_pix_format_mplane *node_fmt =
> +                       &fmt_to_fill->fmt.pix_mp;
> +
> +               if (queue->desc.capture) {
> +                       fmt_to_fill->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
> +                       node_fmt->width = MTK_FD_OUTPUT_MAX_WIDTH;
> +                       node_fmt->height = MTK_FD_OUTPUT_MAX_HEIGHT;
> +               } else {
> +                       fmt_to_fill->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
> +                       node_fmt->width = MTK_FD_INPUT_MAX_WIDTH;
> +                       node_fmt->height = MTK_FD_INPUT_MAX_HEIGHT;
> +               }
> +               set_img_fmt(node_fmt, ctx_fmt);
> +       }       else {
> +               /* meta buffer type */
> +               struct v4l2_meta_format *node_fmt = &fmt_to_fill->fmt.meta;
> +
> +               if (queue->desc.capture)
> +                       fmt_to_fill->type = V4L2_BUF_TYPE_META_CAPTURE;
> +               else
> +                       fmt_to_fill->type = V4L2_BUF_TYPE_META_OUTPUT;
> +
> +               set_meta_fmt(node_fmt, ctx_fmt);
> +       }
> +       return 0;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_format_load_default_fmt);
> +
> +static struct mtk_fd_ctx_format *mtk_fd_ctx_find_fmt
> +       (struct mtk_fd_ctx_queue *queue, u32 format)
> +{
> +       int i;
> +       struct mtk_fd_ctx_format *ctx_fmt;
> +
> +       for (i = 0; i < queue->desc.num_fmts; i++) {
> +               ctx_fmt = &queue->desc.fmts[i];
> +               if (queue->desc.image) {
> +                       pr_debug("idx(%d), pixelformat(%x), fmt(%x)\n",
> +                                i, ctx_fmt->fmt.img.pixelformat, format);
> +                       if (ctx_fmt->fmt.img.pixelformat == format)
> +                               return ctx_fmt;
> +               } else {
> +                       if (ctx_fmt->fmt.meta.dataformat == format)
> +                               return ctx_fmt;
> +               }
> +       }
> +       return NULL;
> +}
> +
> +int mtk_fd_ctx_fmt_set_meta(struct mtk_fd_ctx *dev_ctx, int queue_id,
> +                           struct v4l2_meta_format *user_fmt,
> +                           struct v4l2_meta_format *node_fmt)
> +{
> +       struct mtk_fd_ctx_queue *queue = NULL;
> +       struct mtk_fd_ctx_format *ctx_fmt;
> +
> +       if (queue_id >= dev_ctx->queues_attr.total_num) {
> +               pr_err("Invalid queue id:%d\n", queue_id);
> +               return -EINVAL;
> +       }
> +
> +       queue = &dev_ctx->queue[queue_id];
> +       if (!user_fmt || !node_fmt)
> +               return -EINVAL;
> +
> +       ctx_fmt = mtk_fd_ctx_find_fmt(queue, user_fmt->dataformat);
> +       if (!ctx_fmt)
> +               return -EINVAL;
> +
> +       queue->ctx_fmt = ctx_fmt;
> +       set_meta_fmt(node_fmt, ctx_fmt);
> +       *user_fmt = *node_fmt;
> +
> +       return 0;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_fmt_set_meta);
> +
> +int mtk_fd_ctx_fmt_set_img(struct mtk_fd_ctx *dev_ctx, int queue_id,
> +                          struct v4l2_pix_format_mplane *user_fmt,
> +                          struct v4l2_pix_format_mplane *node_fmt)
> +{
> +       struct mtk_fd_ctx_queue *queue = NULL;
> +       struct mtk_fd_ctx_format *ctx_fmt;
> +
> +       if (queue_id >= dev_ctx->queues_attr.total_num) {
> +               pr_err("Invalid queue id:%d\n", queue_id);
> +               return -EINVAL;
> +       }
> +
> +       queue = &dev_ctx->queue[queue_id];
> +       if (!user_fmt || !node_fmt)
> +               return -EINVAL;
> +
> +       ctx_fmt = mtk_fd_ctx_find_fmt(queue, user_fmt->pixelformat);
> +       if (!ctx_fmt)
> +               return -EINVAL;
> +
> +       queue->ctx_fmt = ctx_fmt;
> +       node_fmt->width = user_fmt->width;
> +       node_fmt->height = user_fmt->height;
> +
> +       set_img_fmt(node_fmt, ctx_fmt);
> +
> +       *user_fmt = *node_fmt;
> +
> +       return 0;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_fmt_set_img);
> +
> +int mtk_fd_ctx_streamon(struct mtk_fd_ctx *dev_ctx)
> +{
> +       int ret = 0;
> +
> +       if (dev_ctx->streaming) {
> +               pr_warn("stream on failed, pdev(%llx), ctx(%d) already stream on\n",
> +                       (long long)dev_ctx->pdev, dev_ctx->ctx_id);
> +               return -EBUSY;
> +       }
> +
> +       ret = mtk_fd_streamon(dev_ctx->pdev, dev_ctx->ctx_id);
> +       if (ret) {
> +               pr_err("streamon: ctx(%d) failed, notified by context\n",
> +                      dev_ctx->ctx_id);
> +               return -EBUSY;
> +       }
> +
> +       dev_ctx->streaming = true;
> +       ret = mtk_fd_dev_queue_buffers(mtk_fd_ctx_to_dev(dev_ctx), true);
> +       if (ret)
> +               pr_err("failed to queue initial buffers (%d)", ret);
> +
> +       return ret;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_streamon);
> +
> +int mtk_fd_ctx_streamoff(struct mtk_fd_ctx *dev_ctx)
> +{
> +       int ret = 0;
> +
> +       if (!dev_ctx->streaming) {
> +               pr_warn("Do nothing, pdev(%llx), ctx(%d) is already stream off\n",
> +                       (long long)dev_ctx->pdev, dev_ctx->ctx_id);
> +               return -EBUSY;
> +       }
> +
> +       ret = mtk_fd_streamoff(dev_ctx->pdev, dev_ctx->ctx_id);
> +       if (ret) {
> +               pr_warn("streamoff: ctx(%d) failed, notified by context\n",
> +                       dev_ctx->ctx_id);
> +               return -EBUSY;
> +       }
> +
> +       dev_ctx->streaming = false;
> +
> +       return 0;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_streamoff);
> +
> +int mtk_fd_ctx_init_frame_bundles(struct mtk_fd_ctx *dev_ctx)
> +{
> +       int i = 0;
> +
> +       dev_ctx->num_frame_bundle = VB2_MAX_FRAME;
> +
> +       spin_lock(&dev_ctx->qlock);
> +
> +       /* Reset the queue*/
> +       INIT_LIST_HEAD(&dev_ctx->processing_frames.list);
> +       INIT_LIST_HEAD(&dev_ctx->free_frames.list);
> +
> +       for (i = 0; i < dev_ctx->num_frame_bundle; i++) {
> +               struct mtk_fd_ctx_frame_bundle *frame_bundle =
> +                       &dev_ctx->frame_bundles[i];
> +               frame_bundle->state = MTK_FD_CTX_FRAME_NEW;
> +               list_add_tail(&frame_bundle->list, &dev_ctx->free_frames.list);
> +       }
> +
> +       spin_unlock(&dev_ctx->qlock);
> +
> +       return 0;
> +}
> +
> +int mtk_fd_ctx_open(struct mtk_fd_ctx *dev_ctx)
> +{
> +       struct mtk_fd_dev *fd_dev = mtk_fd_ctx_to_dev(dev_ctx);
> +       unsigned int enabled_dma_ports = 0;
> +       int i = 0;
> +
> +       if (!dev_ctx)
> +               return -EINVAL;
> +
> +       /* Get the enabled DMA ports */
> +       for (i = 0; i < fd_dev->mem2mem2.num_nodes; i++) {
> +               if (fd_dev->mem2mem2.nodes[i].enabled)
> +                       enabled_dma_ports |= dev_ctx->queue[i].desc.dma_port;
> +       }
> +
> +       dev_ctx->enabled_dma_ports = enabled_dma_ports;
> +
> +       dev_dbg(&fd_dev->pdev->dev, "open device: (%llx)\n",
> +               (long long)&fd_dev->pdev->dev);
> +
> +       /* Workaround for SCP EMI access */
> +       mtk_fd_smem_enable_mpu(&dev_ctx->smem_device->dev);
> +
> +       /* Init the frame bundle pool */
> +       mtk_fd_ctx_init_frame_bundles(dev_ctx);
> +
> +       return mtk_fd_open(dev_ctx->pdev);
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_open);
> +
> +int mtk_fd_ctx_release(struct mtk_fd_ctx *dev_ctx)
> +{
> +       return mtk_fd_release(dev_ctx->pdev);
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_release);
> +
> +static int mtk_fd_ctx_core_job_start(struct mtk_fd_ctx *dev_ctx,
> +                                    struct mtk_fd_ctx_frame_bundle *bundle)
> +{
> +       struct platform_device *pdev = dev_ctx->pdev;
> +       int ret = 0;
> +       struct v4l2_fd_param fd_param;
> +       struct mtk_fd_ctx_buffer *ctx_buf_yuv_in = NULL;
> +       struct mtk_fd_ctx_buffer *ctx_buf_meta_in = NULL;
> +       struct mtk_fd_ctx_buffer *ctx_buf_meta_out = NULL;
> +
> +       if (!pdev || !bundle) {
> +               dev_err(&pdev->dev,
> +                       "pdev(%llx) and param(%llx) in start can't be NULL\n",
> +                       (long long)pdev, (long long)bundle);
> +               return -EINVAL;
> +       }
> +       memset(&fd_param, 0, sizeof(struct v4l2_fd_param));
> +       fd_param.frame_id = bundle->id;
> +
> +       /* Img-in buffer */
> +       ctx_buf_yuv_in = bundle->buffers[MTK_FD_CTX_FD_YUV_IN];
> +       if (ctx_buf_yuv_in) {
> +               fd_param.src_img.iova = (uint32_t)ctx_buf_yuv_in->daddr;
> +               fd_param.src_img.va = (uint64_t)ctx_buf_yuv_in->vaddr;
> +               fd_param.src_img_h =
> +                       (uint16_t)ctx_buf_yuv_in->fmt.pix_mp.height;
> +               fd_param.src_img_w =
> +                       (uint16_t)ctx_buf_yuv_in->fmt.pix_mp.width;
> +       }
> +
> +       /* Meta-in buffer */
> +       ctx_buf_meta_in = bundle->buffers[MTK_FD_CTX_FD_CONFIG_IN];
> +       if (ctx_buf_meta_in) {
> +               fd_param.fd_user_param.va = (uint64_t)ctx_buf_meta_in->vaddr;
> +               fd_param.fd_user_param.pa = (uint32_t)ctx_buf_meta_in->paddr;
> +               fd_param.fd_user_param.iova = (uint32_t)ctx_buf_meta_in->daddr;
> +       } else {
> +               dev_err(&pdev->dev, "meta in is null!\n");
> +               fd_param.fd_user_param.pa = 0;
> +               fd_param.fd_user_param.va = 0;
> +               fd_param.fd_user_param.iova = 0;
> +       }
> +
> +       /* Meta-out buffer */
> +       ctx_buf_meta_out = bundle->buffers[MTK_FD_CTX_FD_OUT];
> +       if (ctx_buf_meta_out) {
> +               fd_param.fd_user_result.va = (uint64_t)ctx_buf_meta_out->vaddr;
> +               fd_param.fd_user_result.pa = (uint32_t)ctx_buf_meta_out->paddr;
> +               fd_param.fd_user_result.iova =
> +                                       (uint32_t)ctx_buf_meta_out->daddr;
> +       } else {
> +               dev_err(&pdev->dev, "meta out is null!\n");
> +               fd_param.fd_user_result.pa = 0;
> +               fd_param.fd_user_result.va = 0;
> +               fd_param.fd_user_result.iova = 0;
> +       }
> +
> +       dev_info(&pdev->dev,
> +                "Delegate job to mtk_fd_enqueue: pdev(0x%llx), frame(%d)\n",
> +                (long long)pdev, bundle->id);
> +       ret = mtk_fd_enqueue(pdev, &fd_param);
> +
> +       if (ret) {
> +               dev_warn(&pdev->dev, "mtk_fd_enqueue failed: %d\n", ret);
> +               return -EBUSY;
> +       }
> +
> +       return 0;
> +}
> +
> +static void debug_bundle(struct mtk_fd_ctx_frame_bundle *bundle_data)
> +{
> +       int i = 0;
> +
> +       if (!bundle_data) {
> +               pr_warn("bundle_data is NULL\n");
> +               return;
> +       }
> +
> +       pr_debug("bundle buf nums (%d, %d,%d,%d)\n",
> +                bundle_data->num_img_capture_bufs,
> +                bundle_data->num_img_output_bufs,
> +                bundle_data->num_meta_capture_bufs,
> +                bundle_data->num_meta_output_bufs);
> +
> +       for (i = 0; i < 16 ; i++) {
> +               pr_debug("Bundle, buf[%d] = %llx\n",
> +                        i,
> +                        (unsigned long long)bundle_data->buffers[i]);
> +       }
> +
> +       pr_debug("Bundle last idx: %d\n", bundle_data->last_index);
> +}
> +
> +static struct mtk_fd_ctx_frame_bundle *mtk_fd_ctx_get_free_frame
> +       (struct mtk_fd_ctx *dev_ctx)
> +{
> +       struct mtk_fd_ctx_frame_bundle *frame_bundle = NULL;
> +
> +       spin_lock(&dev_ctx->qlock);
> +       list_for_each_entry(frame_bundle,
> +                           &dev_ctx->free_frames.list, list){
> +               pr_debug("Check frame: state %d, new should be %d\n",
> +                        frame_bundle->state, MTK_FD_CTX_FRAME_NEW);
> +               if (frame_bundle->state == MTK_FD_CTX_FRAME_NEW) {
> +                       frame_bundle->state = MTK_FD_CTX_FRAME_PREPARED;
> +                       pr_debug("Found free frame\n");
> +                       spin_unlock(&dev_ctx->qlock);
> +                       return frame_bundle;
> +               }
> +       }
> +       spin_unlock(&dev_ctx->qlock);
> +       pr_err("Can't found any bundle is MTK_FD_CTX_FRAME_NEW\n");
> +       return NULL;
> +}
> +
> +static int mtk_fd_ctx_process_frame
> +       (struct mtk_fd_ctx *dev_ctx,
> +        struct mtk_fd_ctx_frame_bundle *frame_bundle)
> +{
> +       spin_lock(&dev_ctx->qlock);
> +
> +       frame_bundle->state = MTK_FD_CTX_FRAME_PROCESSING;
> +       list_del(&frame_bundle->list);
> +       list_add_tail(&frame_bundle->list, &dev_ctx->processing_frames.list);
> +
> +       spin_unlock(&dev_ctx->qlock);
> +       return 0;
> +}
> +
> +int mtk_fd_ctx_trigger_job(struct mtk_fd_ctx  *dev_ctx,
> +                          struct mtk_fd_ctx_frame_bundle *bundle_data)
> +{
> +       /* Scan all buffers and filled the ipi frame data*/
> +       int i = 0;
> +       struct mtk_fd_ctx_finish_param fram_param;
> +
> +       struct mtk_fd_ctx_frame_bundle *bundle =
> +               mtk_fd_ctx_get_free_frame(dev_ctx);
> +
> +       pr_debug("Bundle data: , ctx id:%d\n",
> +                dev_ctx->ctx_id);
> +
> +       debug_bundle(bundle_data);
> +
> +       if (!bundle) {
> +               pr_err("bundle can't be NULL\n");
> +               goto FAILE_JOB_NOT_TRIGGER;
> +       }
> +       if (!bundle_data) {
> +               pr_err("bundle_data can't be NULL\n");
> +               goto FAILE_JOB_NOT_TRIGGER;
> +       }
> +
> +       pr_debug("Copy bundle_data->buffers to bundle->buffers\n");
> +       memcpy(bundle->buffers, bundle_data->buffers,
> +              sizeof(struct mtk_fd_ctx_buffer *) *
> +              MTK_FD_CTX_FRAME_BUNDLE_BUFFER_MAX);
> +
> +       pr_debug("bundle setup (%d, %d,%d,%d)\n",
> +                bundle_data->num_img_capture_bufs,
> +                bundle_data->num_img_output_bufs,
> +                bundle_data->num_meta_capture_bufs,
> +                bundle_data->num_meta_output_bufs);
> +
> +       bundle->num_img_capture_bufs = bundle_data->num_img_capture_bufs;
> +       bundle->num_img_output_bufs = bundle_data->num_img_output_bufs;
> +       bundle->num_meta_capture_bufs = bundle_data->num_meta_capture_bufs;
> +       bundle->num_meta_output_bufs = bundle_data->num_meta_output_bufs;
> +       bundle->id = mtk_fd_ctx_next_global_frame_sequence(dev_ctx,
> +                                                          dev_ctx->ctx_id);
> +       bundle->last_index = dev_ctx->queues_attr.total_num - 1;
> +
> +       debug_bundle(bundle);
> +
> +       pr_debug("Fill Address data\n");
> +       for (i = 0; i <= bundle->last_index; i++) {
> +               struct mtk_fd_ctx_buffer *ctx_buf = bundle->buffers[i];
> +               struct vb2_v4l2_buffer *b = NULL;
> +
> +               pr_debug("Process queue[%d], ctx_buf:(%llx)\n",
> +                        i, (unsigned long long)ctx_buf);
> +
> +               if (!ctx_buf) {
> +                       pr_warn("queue[%d], ctx_buf is NULL!!\n", i);
> +                       continue;
> +               }
> +
> +               pr_debug("Get VB2 V4L2 buffer\n");
> +               b = mtk_fd_ctx_buffer_get_vb2_v4l2_buffer(ctx_buf);
> +
> +               ctx_buf->image = dev_ctx->queue[ctx_buf->queue].desc.image;
> +               ctx_buf->capture = dev_ctx->queue[ctx_buf->queue].desc.capture;
> +               /* copy the fmt setting for queue's fmt*/
> +               ctx_buf->fmt = dev_ctx->queue[ctx_buf->queue].fmt;
> +               ctx_buf->ctx_fmt = dev_ctx->queue[ctx_buf->queue].ctx_fmt;
> +                       ctx_buf->frame_id = bundle->id;
> +               ctx_buf->daddr =
> +                       vb2_dma_contig_plane_dma_addr(&b->vb2_buf, 0);
> +               pr_debug("%s:vb2_buf: type(%d),idx(%d),mem(%d)\n",
> +                        __func__,
> +                        b->vb2_buf.type,
> +                        b->vb2_buf.index,
> +                        b->vb2_buf.memory);
> +               ctx_buf->vaddr = vb2_plane_vaddr(&b->vb2_buf, 0);
> +               ctx_buf->buffer_usage = dev_ctx->queue[i].buffer_usage;
> +               ctx_buf->rotation = dev_ctx->queue[i].rotation;
> +               pr_debug("Buf: queue(%d), vaddr(%llx), daddr(%llx)",
> +                        ctx_buf->queue, (unsigned long long)ctx_buf->vaddr,
> +                        (unsigned long long)ctx_buf->daddr);
> +
> +               if (!ctx_buf->image) {
> +                       ctx_buf->paddr =
> +                               mtk_fd_smem_iova_to_phys
> +                                       (&dev_ctx->smem_device->dev,
> +                                        ctx_buf->daddr);
> +               } else {
> +                       pr_debug("No pa: it is a image buffer\n");
> +                       ctx_buf->paddr = 0;
> +               }
> +               ctx_buf->state = MTK_FD_CTX_BUFFER_PROCESSING;
> +       }
> +
> +       if (mtk_fd_ctx_process_frame(dev_ctx, bundle)) {
> +               pr_err("mtk_fd_ctx_process_frame failed: frame(%d)\n",
> +                      bundle->id);
> +               goto FAILE_JOB_NOT_TRIGGER;
> +       }
> +
> +       if (dev_ctx->mode == MTK_FD_CTX_MODE_DEBUG_BYPASS_JOB_TRIGGER) {
> +               memset(&fram_param, 0, sizeof(struct mtk_fd_ctx_finish_param));
> +               fram_param.frame_id = bundle->id;
> +               fram_param.state = MTK_FD_CTX_FRAME_DATA_DONE;
> +               pr_debug("Ctx(%d) in HW bypass mode, will not trigger hw\n",
> +                        dev_ctx->ctx_id);
> +
> +               mtk_fd_ctx_core_job_finish(dev_ctx,     (void *)&fram_param);
> +               return 0;
> +       }
> +
> +       if (mtk_fd_ctx_core_job_start(dev_ctx, bundle))
> +               goto FAILE_JOB_NOT_TRIGGER;
> +       return 0;
> +
> +FAILE_JOB_NOT_TRIGGER:
> +       pr_debug("FAILE_JOB_NOT_TRIGGER: init fram_param: %llx\n",
> +                (unsigned long long)&fram_param);
> +       memset(&fram_param, 0, sizeof(struct mtk_fd_ctx_finish_param));
> +       fram_param.frame_id = bundle->id;
> +       fram_param.state = MTK_FD_CTX_FRAME_DATA_ERROR;
> +       pr_debug("Call mtk_fd_ctx_core_job_finish_cb: fram_param: %llx",
> +                (unsigned long long)&fram_param);
> +       mtk_fd_ctx_core_job_finish(dev_ctx, (void *)&fram_param);
> +
> +       return -EINVAL;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_trigger_job);
> diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c b/drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c
> new file mode 100644
> index 0000000..7e3acf7
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c
> @@ -0,0 +1,355 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * Copyright (c) 2018 Mediatek Corporation.
> + * Copyright (c) 2017 Intel Corporation.
> + * Copyright (C) 2017 Google, Inc.
> + *
> + * This program is free software; you can redistribute it and/or
> + * modify it under the terms of the GNU General Public License version
> + * 2 as published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> + * GNU General Public License for more details.
> + *
> + * MTK_FD-dev is highly based on Intel IPU 3 chrome driver
> + *
> + */
> +
> +#include <linux/module.h>
> +#include <linux/device.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/of.h>
> +#include <linux/of_platform.h>
> +#include <linux/platform_device.h>
> +#include <media/videobuf2-dma-contig.h>
> +#include "mtk_fd-dev.h"
> +
> +static struct platform_device *mtk_fd_dev_of_find_smem_dev
> +       (struct platform_device *pdev);
> +
> +/* Initliaze a mtk_fd_dev representing a completed HW FD */
> +/* device */
> +int mtk_fd_dev_init(struct mtk_fd_dev *fd_dev,
> +                   struct platform_device *pdev,
> +                   struct media_device *media_dev,
> +                   struct v4l2_device *v4l2_dev)
> +{
> +       int r = 0;
> +
> +       fd_dev->pdev = pdev;
> +
> +       mutex_init(&fd_dev->lock);
> +       atomic_set(&fd_dev->qbuf_barrier, 0);
> +       init_waitqueue_head(&fd_dev->buf_drain_wq);
> +
> +       /* v4l2 sub-device registration */
> +       r = mtk_fd_dev_mem2mem2_init(fd_dev, media_dev, v4l2_dev);
> +
> +       if (r) {
> +               dev_err(&fd_dev->pdev->dev,
> +                       "failed to create V4L2 devices (%d)\n", r);
> +               goto failed_mem2mem2;
> +       }
> +
> +       return 0;
> +
> +failed_mem2mem2:
> +       mutex_destroy(&fd_dev->lock);
> +       return r;
> +}
> +
> +int mtk_fd_dev_get_total_node(struct mtk_fd_dev *mtk_fd_dev)
> +{
> +       return mtk_fd_dev->ctx.queues_attr.total_num;
> +}
> +
> +int mtk_fd_dev_mem2mem2_init(struct mtk_fd_dev *fd_dev,
> +                            struct media_device *media_dev,
> +                            struct v4l2_device *v4l2_dev)
> +{
> +       int r, i;
> +       const int queue_master = fd_dev->ctx.queues_attr.master;
> +
> +       pr_info("mem2mem2.name: %s\n", fd_dev->ctx.device_name);
> +       fd_dev->mem2mem2.name = fd_dev->ctx.device_name;
> +       fd_dev->mem2mem2.model = fd_dev->ctx.device_name;
> +       fd_dev->mem2mem2.num_nodes =
> +               mtk_fd_dev_get_total_node(fd_dev);
> +       fd_dev->mem2mem2.vb2_mem_ops = &vb2_dma_contig_memops;
> +       fd_dev->mem2mem2.buf_struct_size =
> +               sizeof(struct mtk_fd_dev_buffer);
> +
> +       fd_dev->mem2mem2.nodes = fd_dev->mem2mem2_nodes;
> +       fd_dev->mem2mem2.dev = &fd_dev->pdev->dev;
> +
> +       for (i = 0; i < fd_dev->ctx.dev_node_num; i++) {
> +               fd_dev->mem2mem2.nodes[i].name =
> +                       mtk_fd_dev_get_node_name(fd_dev, i);
> +               fd_dev->mem2mem2.nodes[i].output =
> +                               (!fd_dev->ctx.queue[i].desc.capture);
> +               fd_dev->mem2mem2.nodes[i].immutable = false;
> +               fd_dev->mem2mem2.nodes[i].enabled = false;
> +               atomic_set(&fd_dev->mem2mem2.nodes[i].sequence, 0);
> +       }
> +
> +       /* Master queue is always enabled */
> +       fd_dev->mem2mem2.nodes[queue_master].immutable = true;
> +       fd_dev->mem2mem2.nodes[queue_master].enabled = true;
> +
> +       pr_info("register v4l2 for %llx\n",
> +               (unsigned long long)fd_dev);
> +       r = mtk_fd_mem2mem2_v4l2_register(fd_dev, media_dev, v4l2_dev);
> +
> +       if (r) {
> +               pr_err("v4l2 init failed, dev(ctx:%d)\n", fd_dev->ctx.ctx_id);
> +               return r;
> +       }
> +
> +       return 0;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_dev_mem2mem2_init);
> +
> +void mtk_fd_dev_mem2mem2_exit(struct mtk_fd_dev *fd_dev)
> +{
> +       mtk_fd_v4l2_unregister(fd_dev);
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_dev_mem2mem2_exit);
> +
> +char *mtk_fd_dev_get_node_name
> +       (struct mtk_fd_dev *fd_dev, int node)
> +{
> +       struct mtk_fd_ctx_queue_desc *mapped_queue_desc =
> +               &fd_dev->ctx.queue[node].desc;
> +
> +       return mapped_queue_desc->name;
> +}
> +
> +/* Get a free buffer from a video node */
> +static struct mtk_fd_ctx_buffer __maybe_unused *mtk_fd_dev_queue_getbuf
> +       (struct mtk_fd_dev *fd_dev, int node)
> +{
> +       struct mtk_fd_dev_buffer *buf;
> +       int queue = -1;
> +
> +       if (node > fd_dev->ctx.dev_node_num || node < 0) {
> +               dev_err(&fd_dev->pdev->dev, "Invalid mtk_fd_dev node.\n");
> +               return NULL;
> +       }
> +
> +       /* Get the corrosponding queue id of the video node */
> +       /* Currently the queue id is the same as the node number */
> +       queue = node;
> +
> +       if (queue < 0) {
> +               dev_err(&fd_dev->pdev->dev, "Invalid mtk_fd_dev node.\n");
> +               return NULL;
> +       }
> +
> +       /* Find first free buffer from the node */
> +       list_for_each_entry(buf, &fd_dev->mem2mem2.nodes[node].buffers,
> +                           m2m2_buf.list) {
> +               if (mtk_fd_ctx_get_buffer_state(&buf->ctx_buf)
> +                       == MTK_FD_CTX_BUFFER_NEW)
> +                       return &buf->ctx_buf;
> +       }
> +
> +       /* There were no free buffers*/
> +       return NULL;
> +}
> +
> +int mtk_fd_dev_get_queue_id_of_dev_node(struct mtk_fd_dev *fd_dev,
> +                                       struct mtk_fd_dev_video_device *node)
> +{
> +       return (node - fd_dev->mem2mem2.nodes);
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_dev_get_queue_id_of_dev_node);
> +
> +int mtk_fd_dev_queue_buffers(struct mtk_fd_dev *fd_dev,
> +                            bool initial)
> +{
> +       unsigned int node;
> +       int r = 0;
> +       struct mtk_fd_dev_buffer *ibuf;
> +       struct mtk_fd_ctx_frame_bundle bundle;
> +       const int mtk_fd_dev_node_num = mtk_fd_dev_get_total_node(fd_dev);
> +       const int queue_master = fd_dev->ctx.queues_attr.master;
> +
> +       memset(&bundle, 0, sizeof(struct mtk_fd_ctx_frame_bundle));
> +
> +       pr_info("%s, init(%d)\n", __func__, initial);
> +
> +       if (!mtk_fd_ctx_is_streaming(&fd_dev->ctx)) {
> +               pr_info("%s: stream off, no hw enqueue triggered\n", __func__);
> +               return 0;
> +       }
> +
> +       mutex_lock(&fd_dev->lock);
> +
> +       /* Buffer set is queued to background driver (e.g. DIP, FD, and P1) */
> +       /* only when master input buffer is ready */
> +       if (!mtk_fd_dev_queue_getbuf(fd_dev, queue_master)) {
> +               mutex_unlock(&fd_dev->lock);
> +               return 0;
> +       }
> +
> +       /* Check all node from the node after the master node */
> +       for (node = (queue_master + 1) % mtk_fd_dev_node_num;
> +               1; node = (node + 1) % mtk_fd_dev_node_num) {
> +               pr_info("Check node(%d), queue enabled(%d), node enabled(%d)\n",
> +                       node, fd_dev->queue_enabled[node],
> +                       fd_dev->mem2mem2.nodes[node].enabled);
> +
> +               /* May skip some node according the scenario in the future */
> +               if (fd_dev->queue_enabled[node] ||
> +                   fd_dev->mem2mem2.nodes[node].enabled) {
> +                       struct mtk_fd_ctx_buffer *buf =
> +                               mtk_fd_dev_queue_getbuf(fd_dev, node);
> +                       char *node_name =
> +                               mtk_fd_dev_get_node_name(fd_dev, node);
> +
> +                       if (!buf) {
> +                               dev_dbg(&fd_dev->pdev->dev,
> +                                       "No free buffer of enabled node %s\n",
> +                                       node_name);
> +                               break;
> +                       }
> +
> +                       /* To show the debug message */
> +                       ibuf = container_of(buf,
> +                                           struct mtk_fd_dev_buffer, ctx_buf);
> +                       dev_dbg(&fd_dev->pdev->dev,
> +                               "may queue user %s buffer idx(%d) to ctx\n",
> +                               node_name,
> +                               ibuf->m2m2_buf.vbb.vb2_buf.index);
> +                       mtk_fd_ctx_frame_bundle_add(&fd_dev->ctx,
> +                                                   &bundle, buf);
> +               }
> +
> +               /* Stop if there is no free buffer in master input node */
> +               if (node == queue_master) {
> +                       if (mtk_fd_dev_queue_getbuf(fd_dev, queue_master)) {
> +                               /* Has collected all buffer required */
> +                               mtk_fd_ctx_trigger_job(&fd_dev->ctx, &bundle);
> +                       } else {
> +                               pr_debug("no new buffer found in master node, not trigger job\n");
> +                               break;
> +                       }
> +               }
> +       }
> +       mutex_unlock(&fd_dev->lock);
> +
> +       if (r && r != -EBUSY)
> +               goto failed;
> +
> +       return 0;
> +
> +failed:
> +       /*
> +        * On error, mark all buffers as failed which are not
> +        * yet queued to CSS
> +        */
> +       dev_err(&fd_dev->pdev->dev,
> +               "failed to queue buffer to ctx on queue %i (%d)\n",
> +               node, r);
> +
> +       if (initial)
> +               /* If we were called from streamon(), no need to finish bufs */
> +               return r;
> +
> +       for (node = 0; node < mtk_fd_dev_node_num; node++) {
> +               struct mtk_fd_dev_buffer *buf, *buf0;
> +
> +               if (!fd_dev->queue_enabled[node])
> +                       continue;       /* Skip disabled queues */
> +
> +               mutex_lock(&fd_dev->lock);
> +               list_for_each_entry_safe
> +                       (buf, buf0,
> +                        &fd_dev->mem2mem2.nodes[node].buffers,
> +                        m2m2_buf.list) {
> +                       if (mtk_fd_ctx_get_buffer_state(&buf->ctx_buf) ==
> +                               MTK_FD_CTX_BUFFER_PROCESSING)
> +                               continue;       /* Was already queued, skip */
> +
> +                       mtk_fd_v4l2_buffer_done(&buf->m2m2_buf.vbb.vb2_buf,
> +                                               VB2_BUF_STATE_ERROR);
> +               }
> +               mutex_unlock(&fd_dev->lock);
> +       }
> +
> +       return r;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_dev_queue_buffers);
> +
> +int mtk_fd_dev_core_init(struct platform_device *pdev,
> +                        struct mtk_fd_dev *fd_dev,
> +                        struct mtk_fd_ctx_desc *ctx_desc)
> +{
> +       return mtk_fd_dev_core_init_ext(pdev,
> +               fd_dev, ctx_desc, NULL, NULL);
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_dev_core_init);
> +
> +int mtk_fd_dev_core_init_ext(struct platform_device *pdev,
> +                            struct mtk_fd_dev *fd_dev,
> +                            struct mtk_fd_ctx_desc *ctx_desc,
> +                            struct media_device *media_dev,
> +                            struct v4l2_device *v4l2_dev)
> +{
> +       int r;
> +       struct platform_device *smem_dev = NULL;
> +
> +       smem_dev = mtk_fd_dev_of_find_smem_dev(pdev);
> +
> +       if (!smem_dev)
> +               dev_err(&pdev->dev, "failed to find smem_dev\n");
> +
> +       /* Device context must be initialized before device instance */
> +       r = mtk_fd_ctx_core_init(&fd_dev->ctx, pdev,
> +                                0, ctx_desc, pdev, smem_dev);
> +
> +       dev_info(&pdev->dev, "init fd_dev: %llx\n",
> +                (unsigned long long)fd_dev);
> +       /* init other device level members */
> +       mtk_fd_dev_init(fd_dev, pdev, media_dev, v4l2_dev);
> +
> +       return 0;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_dev_core_init_ext);
> +
> +int mtk_fd_dev_core_release(struct platform_device *pdev,
> +                           struct mtk_fd_dev *fd_dev)
> +{
> +       mtk_fd_dev_mem2mem2_exit(fd_dev);
> +       mtk_fd_ctx_core_exit(&fd_dev->ctx);
> +       mutex_destroy(&fd_dev->lock);
> +       return 0;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_dev_core_release);
> +
> +static struct platform_device *mtk_fd_dev_of_find_smem_dev
> +       (struct platform_device *pdev)
> +{
> +       struct device_node *smem_dev_node = NULL;
> +
> +       if (!pdev) {
> +               pr_err("Find_smem_dev failed, pdev can't be NULL\n");
> +               return NULL;
> +       }
> +
> +       smem_dev_node = of_parse_phandle(pdev->dev.of_node,
> +                                        "smem_device", 0);
> +
> +       if (!smem_dev_node) {
> +               dev_err(&pdev->dev,
> +                       "failed to find isp smem device for (%s)\n",
> +                       pdev->name);
> +               return NULL;
> +       }
> +
> +       dev_dbg(&pdev->dev, "smem of node found, try to discovery device\n");
> +       return of_find_device_by_node(smem_dev_node);
> +}
> +
> diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h b/drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h
> new file mode 100644
> index 0000000..d2b7d77
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h
> @@ -0,0 +1,198 @@
> +/* SPDX-License-Identifier: GPL-2.0
> + * Copyright (c) 2018 Mediatek Corporation.
> + * Copyright (c) 2017 Intel Corporation.
> + *
> + * This program is free software; you can redistribute it and/or
> + * modify it under the terms of the GNU General Public License version
> + * 2 as published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> + * GNU General Public License for more details.
> + *
> + * MTK_FD-dev is highly based on Intel IPU 3 chrome driver
> + *
> + */
> +
> +#ifndef __MTK_FD_DEV_H__
> +#define __MTK_FD_DEV_H__
> +
> +#include <linux/platform_device.h>
> +#include <linux/version.h>
> +#include <media/v4l2-device.h>
> +#include <media/videobuf2-v4l2.h>
> +#include "mtk_fd-ctx.h"
> +
> +/* Added the macro for early stage verification */
> +/* based on kernel 4.4 environment. */
> +/* I will remove the version check after getting */
> +/* the devlopment platform based on 4.14 */
> +#define MTK_FD_KERNEL_BASE_VERSION KERNEL_VERSION(4, 14, 0)
> +
> +#define MTK_FD_DEV_NODE_MAX                    (MTK_FD_CTX_QUEUES)
> +
> +#define MTK_FD_INPUT_MIN_WIDTH         0U
> +#define MTK_FD_INPUT_MIN_HEIGHT                0U
> +#define MTK_FD_INPUT_MAX_WIDTH         480U
> +#define MTK_FD_INPUT_MAX_HEIGHT                640U
> +#define MTK_FD_OUTPUT_MIN_WIDTH                2U
> +#define MTK_FD_OUTPUT_MIN_HEIGHT               2U
> +#define MTK_FD_OUTPUT_MAX_WIDTH                480U
> +#define MTK_FD_OUTPUT_MAX_HEIGHT               640U
> +
> +#define file_to_mtk_fd_node(__file) \
> +       container_of(video_devdata(__file),\
> +       struct mtk_fd_dev_video_device, vdev)
> +
> +#define mtk_fd_ctx_to_dev(__ctx) \
> +       container_of(__ctx,\
> +       struct mtk_fd_dev, ctx)
> +
> +#define mtk_fd_m2m_to_dev(__m2m) \
> +       container_of(__m2m,\
> +       struct mtk_fd_dev, mem2mem2)
> +
> +#define mtk_fd_subdev_to_dev(__sd) \
> +       container_of(__sd, \
> +       struct mtk_fd_dev, mem2mem2.subdev)
> +
> +#define mtk_fd_vbq_to_isp_node(__vq) \
> +       container_of(__vq, \
> +       struct mtk_fd_dev_video_device, vbq)
> +
> +#define mtk_fd_ctx_buf_to_dev_buf(__ctx_buf) \
> +       container_of(__ctx_buf, \
> +       struct mtk_fd_dev_buffer, ctx_buf)
> +
> +#define mtk_fd_vb2_buf_to_dev_buf(__vb) \
> +       container_of(vb, \
> +       struct mtk_fd_dev_buffer, \
> +       m2m2_buf.vbb.vb2_buf)
> +
> +#define mtk_fd_vb2_buf_to_m2m_buf(__vb) \
> +       container_of(__vb, \
> +       struct mtk_fd_mem2mem2_buffer, \
> +       vbb.vb2_buf)
> +
> +#define mtk_fd_subdev_to_m2m(__sd) \
> +       container_of(__sd, \
> +       struct mtk_fd_mem2mem2_device, subdev)
> +
> +struct mtk_fd_mem2mem2_device;
> +
> +struct mtk_fd_mem2mem2_buffer {
> +       struct vb2_v4l2_buffer vbb;
> +       struct list_head list;
> +};
> +
> +struct mtk_fd_dev_buffer {
> +       struct mtk_fd_mem2mem2_buffer m2m2_buf;
> +       /* Intenal part */
> +       struct mtk_fd_ctx_buffer ctx_buf;
> +};
> +
> +struct mtk_fd_dev_video_device {
> +       const char *name;
> +       int output;
> +       int immutable;
> +       int enabled;
> +       int queued;
> +       struct v4l2_format vdev_fmt;
> +       struct video_device vdev;
> +       struct media_pad vdev_pad;
> +       struct v4l2_mbus_framefmt pad_fmt;
> +       struct vb2_queue vbq;
> +       struct list_head buffers;
> +       struct mutex lock; /* Protect node data */
> +       atomic_t sequence;
> +};
> +
> +struct mtk_fd_mem2mem2_device {
> +       const char *name;
> +       const char *model;
> +       struct device *dev;
> +       int num_nodes;
> +       struct mtk_fd_dev_video_device *nodes;
> +       const struct vb2_mem_ops *vb2_mem_ops;
> +       unsigned int buf_struct_size;
> +       int streaming;
> +       struct v4l2_device *v4l2_dev;
> +       struct media_device *media_dev;
> +       struct media_pipeline pipeline;
> +       struct v4l2_subdev subdev;
> +       struct media_pad *subdev_pads;
> +       struct v4l2_file_operations v4l2_file_ops;
> +       const struct file_operations fops;
> +};
> +
> +struct mtk_fd_dev {
> +       struct platform_device *pdev;
> +       struct mtk_fd_dev_video_device mem2mem2_nodes[MTK_FD_DEV_NODE_MAX];
> +       int queue_enabled[MTK_FD_DEV_NODE_MAX];
> +       struct mtk_fd_mem2mem2_device mem2mem2;
> +       struct v4l2_device v4l2_dev;
> +       struct media_device media_dev;
> +       struct mtk_fd_ctx ctx;
> +       struct mutex lock; /* queue protection */
> +       atomic_t qbuf_barrier;
> +       struct {
> +               struct v4l2_rect eff;
> +               struct v4l2_rect bds;
> +               struct v4l2_rect gdc;
> +       } rect;
> +       int suspend_in_stream;
> +       wait_queue_head_t buf_drain_wq;
> +};
> +
> +int mtk_fd_media_register(struct device *dev,
> +                         struct media_device *media_dev,
> +                         const char *model);
> +
> +int mtk_fd_v4l2_register(struct device *dev,
> +                        struct media_device *media_dev,
> +                        struct v4l2_device *v4l2_dev);
> +
> +int mtk_fd_v4l2_unregister(struct mtk_fd_dev *dev);
> +
> +int mtk_fd_mem2mem2_v4l2_register(struct mtk_fd_dev *dev,
> +                                 struct media_device *media_dev,
> +                                 struct v4l2_device *v4l2_dev);
> +
> +void mtk_fd_v4l2_buffer_done(struct vb2_buffer *vb,
> +                            enum vb2_buffer_state state);
> +
> +int mtk_fd_dev_queue_buffers(struct mtk_fd_dev *dev, bool initial);
> +
> +int mtk_fd_dev_get_total_node(struct mtk_fd_dev *mtk_fd_dev);
> +
> +char *mtk_fd_dev_get_node_name(struct mtk_fd_dev *mtk_fd_dev_obj, int node);
> +
> +int mtk_fd_dev_init(struct mtk_fd_dev *fd_dev,
> +                   struct platform_device *pdev,
> +                   struct media_device *media_dev,
> +                   struct v4l2_device *v4l2_dev);
> +
> +void mtk_fd_dev_mem2mem2_exit(struct mtk_fd_dev *mtk_fd_dev_obj);
> +
> +int mtk_fd_dev_mem2mem2_init(struct mtk_fd_dev *fd_dev,
> +                            struct media_device *media_dev,
> +                            struct v4l2_device *v4l2_dev);
> +
> +int mtk_fd_dev_get_queue_id_of_dev_node(struct mtk_fd_dev *mtk_fd_dev_obj,
> +                                       struct mtk_fd_dev_video_device *node);
> +
> +int mtk_fd_dev_core_init(struct platform_device *pdev,
> +                        struct mtk_fd_dev *fd_dev,
> +                        struct mtk_fd_ctx_desc *ctx_desc);
> +
> +int mtk_fd_dev_core_init_ext(struct platform_device *pdev,
> +                            struct mtk_fd_dev *fd_dev,
> +                            struct mtk_fd_ctx_desc *ctx_desc,
> +                            struct media_device *media_dev,
> +                            struct v4l2_device *v4l2_dev);
> +
> +int mtk_fd_dev_core_release(struct platform_device *pdev,
> +                           struct mtk_fd_dev *fd_dev);
> +
> +#endif /* __MTK_FD_DEV_H__ */
> diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-smem-drv.c b/drivers/media/platform/mtk-isp/fd/mtk_fd-smem-drv.c
> new file mode 100644
> index 0000000..99a852d
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-smem-drv.c
> @@ -0,0 +1,452 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * Copyright (c) 2018 MediaTek Inc.
> + * Author: Frederic Chen <frederic.chen@mediatek.com>
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> + * GNU General Public License for more details.
> + */
> +
> +#include <linux/module.h>
> +#include <linux/device.h>
> +#include <linux/platform_device.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/of.h>
> +#include <linux/of_fdt.h>
> +#include <linux/of_reserved_mem.h>
> +#include <linux/dma-contiguous.h>
> +#include <linux/dma-mapping.h>
> +#include <linux/slab.h>
> +#include <linux/err.h>
> +#include <linux/iommu.h>
> +#include <asm/cacheflush.h>
> +
> +#define MTK_FD_SMEM_DEV_NAME "MTK-FD-SMEM"
> +
> +struct mtk_fd_smem_drv {
> +       struct platform_device *pdev;
> +       struct sg_table sgt;
> +       struct page **smem_pages;
> +       int num_smem_pages;
> +       phys_addr_t smem_base;
> +       dma_addr_t smem_dma_base;
> +       int smem_size;
> +};
> +
> +static struct reserved_mem *isp_reserved_smem;
> +
> +static int mtk_fd_smem_setup_dma_ops(struct device *smem_dev,
> +                                    const struct dma_map_ops *smem_ops);
> +
> +static int mtk_fd_smem_get_sgtable(struct device *dev,
> +                                  struct sg_table *sgt,
> +                                  void *cpu_addr, dma_addr_t dma_addr,
> +                                  size_t size, unsigned long attrs);
> +
> +static const struct dma_map_ops smem_dma_ops = {
> +       .get_sgtable = mtk_fd_smem_get_sgtable,
> +};
> +
> +static int mtk_fd_smem_init(struct mtk_fd_smem_drv **mtk_fd_smem_drv_out,
> +                           struct platform_device *pdev)
> +{
> +       struct mtk_fd_smem_drv *isp_sys = NULL;
> +       struct device *dev = &pdev->dev;
> +
> +       isp_sys = devm_kzalloc(dev,
> +                              sizeof(*isp_sys), GFP_KERNEL);
> +
> +       isp_sys->pdev = pdev;
> +
> +       *mtk_fd_smem_drv_out = isp_sys;
> +
> +       return 0;
> +}
> +
> +static int mtk_fd_smem_drv_probe(struct platform_device *pdev)
> +{
> +       struct mtk_fd_smem_drv *smem_drv = NULL;
> +       int r = 0;
> +       struct device *dev = &pdev->dev;
> +
> +       dev_dbg(dev, "probe mtk_fd_smem_drv\n");
> +
> +       r = mtk_fd_smem_init(&smem_drv, pdev);
> +
> +       if (!smem_drv)
> +               return -ENOMEM;
> +
> +       dev_set_drvdata(dev, smem_drv);
> +
> +       if (isp_reserved_smem) {
> +               dma_addr_t dma_addr;
> +               phys_addr_t addr;
> +               struct iommu_domain *smem_dom;
> +               int i = 0;
> +               int size_align = 0;
> +               struct page **pages = NULL;
> +               int n_pages = 0;
> +               struct sg_table *sgt = &smem_drv->sgt;
> +
> +               size_align = round_down(isp_reserved_smem->size,
> +                                       PAGE_SIZE);
> +               n_pages = size_align >> PAGE_SHIFT;
> +
> +               pages = kmalloc_array(n_pages, sizeof(struct page *),
> +                                     GFP_KERNEL);
> +
> +               if (!pages)
> +                       return -ENOMEM;
> +
> +               for (i = 0; i < n_pages; i++)
> +                       pages[i] = phys_to_page(isp_reserved_smem->base
> +                                               + i * PAGE_SIZE);
> +
> +               r = sg_alloc_table_from_pages(sgt, pages, n_pages, 0,
> +                                             size_align, GFP_KERNEL);
> +
> +               if (r) {
> +                       dev_err(dev, "failed to get alloca sg table\n");
> +                       return -ENOMEM;
> +               }
> +
> +               dma_map_sg_attrs(dev, sgt->sgl, sgt->nents,
> +                                DMA_BIDIRECTIONAL,
> +                                DMA_ATTR_SKIP_CPU_SYNC);
> +
> +               dma_addr = sg_dma_address(sgt->sgl);
> +               smem_dom = iommu_get_domain_for_dev(dev);
> +               addr = iommu_iova_to_phys(smem_dom, dma_addr);
> +
> +               if (addr != isp_reserved_smem->base)
> +                       dev_err(dev,
> +                               "incorrect pa(%llx) from iommu_iova_to_phys, should be %llx\n",
> +                       (unsigned long long)addr,
> +                       (unsigned long long)isp_reserved_smem->base);
> +
> +               r = dma_declare_coherent_memory(dev,
> +                                               isp_reserved_smem->base,
> +                       dma_addr, size_align, DMA_MEMORY_EXCLUSIVE);
> +
> +               dev_dbg(dev,
> +                       "Coherent mem base(%llx,%llx),size(%lx),ret(%d)\n",
> +                       isp_reserved_smem->base,
> +                       dma_addr, size_align, r);
> +
> +               smem_drv->smem_base = isp_reserved_smem->base;
> +               smem_drv->smem_size = size_align;
> +               smem_drv->smem_pages = pages;
> +               smem_drv->num_smem_pages = n_pages;
> +               smem_drv->smem_dma_base = dma_addr;
> +
> +               dev_dbg(dev, "smem_drv setting (%llx,%lx,%llx,%d)\n",
> +                       smem_drv->smem_base, smem_drv->smem_size,
> +                       (unsigned long long)smem_drv->smem_pages,
> +                       smem_drv->num_smem_pages);
> +       }
> +
> +       r = mtk_fd_smem_setup_dma_ops(dev, &smem_dma_ops);
> +
> +       return r;
> +}
> +
> +phys_addr_t mtk_fd_smem_iova_to_phys(struct device *dev,
> +                                    dma_addr_t iova)
> +{
> +               struct iommu_domain *smem_dom;
> +               phys_addr_t addr;
> +               phys_addr_t limit;
> +               struct mtk_fd_smem_drv *smem_dev =
> +                       dev_get_drvdata(dev);
> +
> +               if (!smem_dev)
> +                       return 0;
> +
> +               smem_dom = iommu_get_domain_for_dev(dev);
> +
> +               if (!smem_dom)
> +                       return 0;
> +
> +               addr = iommu_iova_to_phys(smem_dom, iova);
> +
> +               limit = smem_dev->smem_base + smem_dev->smem_size;
> +
> +               if (addr < smem_dev->smem_base || addr >= limit) {
> +                       dev_err(dev,
> +                               "Unexpected paddr %pa (must >= %pa and <%pa)\n",
> +                               &addr, &smem_dev->smem_base, &limit);
> +                       return 0;
> +               }
> +               dev_dbg(dev, "Pa verifcation pass: %pa(>=%pa, <%pa)\n",
> +                       &addr, &smem_dev->smem_base, &limit);
> +               return addr;
> +}
> +
> +static int mtk_fd_smem_drv_remove(struct platform_device *pdev)
> +{
> +       struct mtk_fd_smem_drv *smem_drv =
> +               dev_get_drvdata(&pdev->dev);
> +
> +       kfree(smem_drv->smem_pages);
> +       return 0;
> +}
> +
> +static int mtk_fd_smem_drv_suspend(struct device *dev)
> +{
> +       return 0;
> +}
> +
> +static int mtk_fd_smem_drv_resume(struct device *dev)
> +{
> +       return 0;
> +}
> +
> +static int mtk_fd_smem_drv_dummy_cb(struct device *dev)
> +{
> +       return 0;
> +}
> +
> +static const struct dev_pm_ops mtk_fd_smem_drv_pm_ops = {
> +       SET_RUNTIME_PM_OPS(&mtk_fd_smem_drv_dummy_cb,
> +                          &mtk_fd_smem_drv_dummy_cb, NULL)
> +       SET_SYSTEM_SLEEP_PM_OPS
> +               (&mtk_fd_smem_drv_suspend, &mtk_fd_smem_drv_resume)
> +};
> +
> +static const struct of_device_id mtk_fd_smem_drv_of_match[] = {
> +       {
> +               .compatible = "mediatek,fd_smem",
> +       },
> +       {},
> +};
> +
> +MODULE_DEVICE_TABLE(of, mtk_fd_smem_drv_of_match);
> +
> +static struct platform_driver mtk_fd_smem_driver = {
> +       .probe = mtk_fd_smem_drv_probe,
> +       .remove = mtk_fd_smem_drv_remove,
> +       .driver = {
> +               .name = MTK_FD_SMEM_DEV_NAME,
> +               .of_match_table =
> +                       of_match_ptr(mtk_fd_smem_drv_of_match),
> +               .pm = &mtk_fd_smem_drv_pm_ops,
> +       },
> +};
> +
> +static int __init mtk_fd_smem_dma_setup(struct reserved_mem
> +                                       *rmem)
> +{
> +       unsigned long node = rmem->fdt_node;
> +
> +       if (of_get_flat_dt_prop(node, "reusable", NULL))
> +               return -EINVAL;
> +
> +       if (!of_get_flat_dt_prop(node, "no-map", NULL)) {
> +               pr_err("Reserved memory: regions without no-map are not yet supported\n");
> +               return -EINVAL;
> +       }
> +
> +       isp_reserved_smem = rmem;
> +
> +       pr_debug("Reserved memory: created DMA memory pool at %pa, size %ld MiB\n",
> +                &rmem->base, (unsigned long)rmem->size / SZ_1M);
> +       return 0;
> +}
> +
> +RESERVEDMEM_OF_DECLARE(mtk_fd_smem,
> +                      "mediatek,reserve-memory-fd_smem",
> +                      mtk_fd_smem_dma_setup);
> +
> +int __init mtk_fd_smem_drv_init(void)
> +{
> +       int ret = 0;
> +
> +       pr_debug("platform_driver_register: mtk_fd_smem_driver\n");
> +       ret = platform_driver_register(&mtk_fd_smem_driver);
> +
> +       if (ret)
> +               pr_warn("fd smem drv init failed, driver didn't probe\n");
> +
> +       return ret;
> +}
> +subsys_initcall(mtk_fd_smem_drv_init);
> +
> +void __exit mtk_fd_smem_drv_ext(void)
> +{
> +       platform_driver_unregister(&mtk_fd_smem_driver);
> +}
> +module_exit(mtk_fd_smem_drv_ext);
> +
> +/********************************************
> + * MTK FD SMEM DMA ops *
> + ********************************************/
> +
> +struct dma_coherent_mem {
> +       void            *virt_base;
> +       dma_addr_t      device_base;
> +       unsigned long   pfn_base;
> +       int             size;
> +       int             flags;
> +       unsigned long   *bitmap;
> +       spinlock_t      spinlock; /* protect the members in dma_coherent_mem */
> +       bool            use_dev_dma_pfn_offset;
> +};
> +
> +static struct dma_coherent_mem *dev_get_coherent_memory(struct device *dev)
> +{
> +       if (dev && dev->dma_mem)
> +               return dev->dma_mem;
> +       return NULL;
> +}
> +
> +static int mtk_fd_smem_get_sgtable(struct device *dev,
> +                                  struct sg_table *sgt,
> +       void *cpu_addr, dma_addr_t dma_addr,
> +       size_t size, unsigned long attrs)
> +{
> +       struct mtk_fd_smem_drv *smem_dev = dev_get_drvdata(dev);
> +       int n_pages_align = 0;
> +       int size_align = 0;
> +       int page_start = 0;
> +       unsigned long long offset_p = 0;
> +       unsigned long long offset_d = 0;
> +
> +       phys_addr_t paddr = mtk_fd_smem_iova_to_phys(dev, dma_addr);
> +
> +       offset_d = (unsigned long long)dma_addr -
> +               (unsigned long long)smem_dev->smem_dma_base;
> +
> +       offset_p = (unsigned long long)paddr -
> +               (unsigned long long)smem_dev->smem_base;
> +
> +       dev_dbg(dev, "%s:dma_addr:%llx,cpu_addr:%llx,pa:%llx,size:%d\n",
> +               __func__,
> +               (unsigned long long)dma_addr,
> +               (unsigned long long)cpu_addr,
> +               (unsigned long long)paddr,
> +               size
> +               );
> +
> +       dev_dbg(dev, "%s:offset p:%llx,offset d:%llx\n",
> +               __func__,
> +               (unsigned long long)offset_p,
> +               (unsigned long long)offset_d
> +               );
> +
> +       size_align = round_up(size, PAGE_SIZE);
> +       n_pages_align = size_align >> PAGE_SHIFT;
> +       page_start = offset_p >> PAGE_SHIFT;
> +
> +       dev_dbg(dev,
> +               "%s:page idx:%d,page pa:%llx,pa:%llx, aligned size:%d\n",
> +               __func__,
> +               page_start,
> +               (unsigned long long)page_to_phys(*(smem_dev->smem_pages
> +                       + page_start)),
> +               (unsigned long long)paddr,
> +               size_align
> +               );
> +
> +       if (!smem_dev) {
> +               dev_err(dev, "can't get sgtable from smem_dev\n");
> +               return -EINVAL;
> +       }
> +
> +       dev_dbg(dev, "get sgt of the smem: %d pages\n", n_pages_align);
> +
> +       return sg_alloc_table_from_pages(sgt,
> +               smem_dev->smem_pages + page_start,
> +               n_pages_align,
> +               0, size_align, GFP_KERNEL);
> +}
> +
> +static void *mtk_fd_smem_get_cpu_addr(struct mtk_fd_smem_drv *smem_dev,
> +                                     struct scatterlist *sg)
> +{
> +       struct device *dev = &smem_dev->pdev->dev;
> +       struct dma_coherent_mem *dma_mem =
> +               dev_get_coherent_memory(dev);
> +
> +       phys_addr_t addr = (phys_addr_t)sg_phys(sg);
> +
> +       if (addr < smem_dev->smem_base ||
> +           addr > smem_dev->smem_base + smem_dev->smem_size) {
> +               dev_err(dev, "Invalid paddr 0x%llx from sg\n", addr);
> +               return NULL;
> +       }
> +
> +       return dma_mem->virt_base + (addr - smem_dev->smem_base);
> +}
> +
> +static void mtk_fd_smem_sync_sg_for_cpu(struct device *dev,
> +                                       struct scatterlist *sgl, int nelems,
> +                                       enum dma_data_direction dir)
> +{
> +       struct mtk_fd_smem_drv *smem_dev =
> +               dev_get_drvdata(dev);
> +       void *cpu_addr;
> +
> +       cpu_addr = mtk_fd_smem_get_cpu_addr(smem_dev, sgl);
> +
> +       dev_dbg(dev,
> +               "__dma_unmap_area:paddr(0x%llx),vaddr(0x%llx),size(%d)\n",
> +               (unsigned long long)sg_phys(sgl),
> +               (unsigned long long)cpu_addr,
> +               sgl->length);
> +
> +       __dma_unmap_area(cpu_addr, sgl->length, dir);
> +}
> +
> +static void mtk_fd_smem_sync_sg_for_device(struct device *dev,
> +                                          struct scatterlist *sgl, int nelems,
> +                                          enum dma_data_direction dir)
> +{
> +       struct mtk_fd_smem_drv *smem_dev =
> +                       dev_get_drvdata(dev);
> +       void *cpu_addr;
> +
> +       cpu_addr = mtk_fd_smem_get_cpu_addr(smem_dev, sgl);
> +
> +       dev_dbg(dev,
> +               "__dma_map_area:paddr(0x%llx),vaddr(0x%llx),size(%d)\n",
> +               (unsigned long long)sg_phys(sgl),
> +               (unsigned long long)cpu_addr,
> +               sgl->length);
> +
> +       __dma_map_area(cpu_addr, sgl->length, dir);
> +}
> +
> +static int mtk_fd_smem_setup_dma_ops(struct device *dev,
> +                                    const struct dma_map_ops *smem_ops)
> +{
> +       if (!dev->dma_ops)
> +               return -EINVAL;
> +
> +       memcpy((void *)smem_ops, dev->dma_ops, sizeof(*smem_ops));
> +
> +       ((struct dma_map_ops *)smem_ops)->get_sgtable =
> +               mtk_fd_smem_get_sgtable;
> +       ((struct dma_map_ops *)smem_ops)->sync_sg_for_device =
> +               mtk_fd_smem_sync_sg_for_device;
> +       ((struct dma_map_ops *)smem_ops)->sync_sg_for_cpu =
> +               mtk_fd_smem_sync_sg_for_cpu;
> +
> +       dev->dma_ops = smem_ops;
> +
> +       return 0;
> +}
> +
> +void mtk_fd_smem_enable_mpu(struct device *dev)
> +{
> +       dev_warn(dev, "MPU enabling func is not ready now\n");
> +}
> +
> +MODULE_AUTHOR("Frederic Chen <frederic.chen@mediatek.com>");
> +MODULE_LICENSE("GPL v2");
> +MODULE_DESCRIPTION("Mediatek FD shared memory driver");
> diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h b/drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h
> new file mode 100644
> index 0000000..a19fc37
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h
> @@ -0,0 +1,25 @@
> +/* SPDX-License-Identifier: GPL-2.0 */
> +/*
> + * Copyright (c) 2018 MediaTek Inc.
> + * Author: Frederic Chen <frederic.chen@mediatek.com>
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> + * GNU General Public License for more details.
> + */
> +
> +#ifndef __MTK_FD_SMEM_H__
> +#define __MTK_FD_SMEM_H__
> +
> +#include <linux/dma-mapping.h>
> +
> +phys_addr_t mtk_fd_smem_iova_to_phys(struct device *smem_dev,
> +                                    dma_addr_t iova);
> +void mtk_fd_smem_enable_mpu(struct device *smem_dev);
> +
> +#endif /*__MTK_FD_SMEM_H__*/
> diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c b/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c
> new file mode 100644
> index 0000000..ab85aea
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c
> @@ -0,0 +1,1046 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * Copyright (c) 2018 Mediatek Corporation.
> + * Copyright (c) 2017 Intel Corporation.
> + *
> + * This program is free software; you can redistribute it and/or
> + * modify it under the terms of the GNU General Public License version
> + * 2 as published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> + * GNU General Public License for more details.
> + *
> + * MTK_FD-v4l2 is highly based on Intel IPU 3 chrome driver
> + *
> + */
> +
> +#include <linux/module.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/videodev2.h>
> +#include <media/v4l2-ioctl.h>
> +#include <media/videobuf2-dma-contig.h>
> +#include <media/v4l2-subdev.h>
> +#include <media/v4l2-event.h>
> +#include <linux/device.h>
> +#include <linux/platform_device.h>
> +
> +#include "mtk_fd-dev.h"
> +
> +static u32 mtk_fd_node_get_v4l2_cap
> +       (struct mtk_fd_ctx_queue *node_ctx);
> +
> +static int mtk_fd_videoc_s_meta_fmt(struct file *file, void *fh,
> +                                   struct v4l2_format *f);
> +
> +static int mtk_fd_subdev_open(struct v4l2_subdev *sd,
> +                             struct v4l2_subdev_fh *fh)
> +{
> +       struct mtk_fd_dev *fd_dev = mtk_fd_subdev_to_dev(sd);
> +
> +       fd_dev->ctx.fh = fh;
> +
> +       return mtk_fd_ctx_open(&fd_dev->ctx);
> +}
> +
> +static int mtk_fd_subdev_close(struct v4l2_subdev *sd,
> +                              struct v4l2_subdev_fh *fh)
> +{
> +       struct mtk_fd_dev *fd_dev = mtk_fd_subdev_to_dev(sd);
> +
> +       return mtk_fd_ctx_release(&fd_dev->ctx);
> +}
> +
> +static int mtk_fd_subdev_s_stream(struct v4l2_subdev *sd, int enable)
> +{
> +       int ret = 0;
> +
> +       struct mtk_fd_dev *fd_dev = mtk_fd_subdev_to_dev(sd);
> +
> +       if (enable) {
> +               ret = mtk_fd_ctx_streamon(&fd_dev->ctx);
> +
> +               if (!ret)
> +                       ret = mtk_fd_dev_queue_buffers
> +                               (mtk_fd_ctx_to_dev(&fd_dev->ctx), true);
> +               if (ret)
> +                       pr_err("failed to queue initial buffers (%d)", ret);
> +       }       else {
> +               ret = mtk_fd_ctx_streamoff(&fd_dev->ctx);
> +       }
> +
> +       if (!ret)
> +               fd_dev->mem2mem2.streaming = enable;
> +
> +       return ret;
> +}
> +
> +static int mtk_fd_link_setup(struct media_entity *entity,
> +                            const struct media_pad *local,
> +                            const struct media_pad *remote, u32 flags)
> +{
> +       struct mtk_fd_mem2mem2_device *m2m2 =
> +               container_of(entity, struct mtk_fd_mem2mem2_device,
> +                            subdev.entity);
> +       struct mtk_fd_dev *fd_dev =
> +               container_of(m2m2, struct mtk_fd_dev, mem2mem2);
> +
> +       u32 pad = local->index;
> +
> +       pr_info("link setup: %d --> %d\n", pad, remote->index);
> +
> +#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +       WARN_ON(entity->type != MEDIA_ENT_T_V4L2_SUBDEV);
> +#else
> +       WARN_ON(entity->obj_type != MEDIA_ENTITY_TYPE_V4L2_SUBDEV);
> +#endif
> +
> +       WARN_ON(pad >= m2m2->num_nodes);
> +
> +       m2m2->nodes[pad].enabled = !!(flags & MEDIA_LNK_FL_ENABLED);
> +
> +       /**
> +        * queue_enable can be phase out in the future since
> +        * we don't have internal queue of each node in
> +        * v4l2 common module
> +        */
> +       fd_dev->queue_enabled[pad] = m2m2->nodes[pad].enabled;
> +
> +       return 0;
> +}
> +
> +static void mtk_fd_vb2_buf_queue(struct vb2_buffer *vb)
> +{
> +       struct mtk_fd_mem2mem2_device *m2m2 = vb2_get_drv_priv(vb->vb2_queue);
> +       struct mtk_fd_dev *mtk_fd_dev = mtk_fd_m2m_to_dev(m2m2);
> +       struct device *dev = &mtk_fd_dev->pdev->dev;
> +       struct mtk_fd_dev_buffer *buf = NULL;
> +       struct vb2_v4l2_buffer *v4l2_buf = NULL;
> +       struct mtk_fd_dev_video_device *node =
> +               mtk_fd_vbq_to_isp_node(vb->vb2_queue);
> +       int queue = mtk_fd_dev_get_queue_id_of_dev_node(mtk_fd_dev, node);
> +
> +       dev_dbg(dev,
> +               "queue vb2_buf: Node(%s) queue id(%d)\n",
> +               node->name,
> +               queue);
> +
> +       if (queue < 0) {
> +               dev_err(m2m2->dev, "Invalid mtk_fd_dev node.\n");
> +               return;
> +       }
> +
> +       if (mtk_fd_dev->ctx.mode == MTK_FD_CTX_MODE_DEBUG_BYPASS_ALL) {
> +               dev_dbg(m2m2->dev, "By pass mode, just loop back the buffer\n");
> +               vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
> +               return;
> +       }
> +
> +       if (!vb)
> +               pr_err("VB can't be null\n");
> +
> +       buf = mtk_fd_vb2_buf_to_dev_buf(vb);
> +
> +       if (!buf)
> +               pr_err("buf can't be null\n");
> +
> +       v4l2_buf = to_vb2_v4l2_buffer(vb);
> +
> +       if (!v4l2_buf)
> +               pr_err("v4l2_buf can't be null\n");
> +
> +       mutex_lock(&mtk_fd_dev->lock);
> +
> +       pr_err("init  mtk_fd_ctx_buf_init, sequence(%d)\n", v4l2_buf->sequence);
> +
> +       /* the dma address will be filled in later frame buffer handling*/
> +       mtk_fd_ctx_buf_init(&buf->ctx_buf, queue, (dma_addr_t)0);
> +       pr_info("set mtk_fd_ctx_buf_init: user seq=%d\n",
> +               buf->ctx_buf.user_sequence);
> +
> +       /* Added the buffer into the tracking list */
> +       list_add_tail(&buf->m2m2_buf.list,
> +                     &m2m2->nodes[node - m2m2->nodes].buffers);
> +       mutex_unlock(&mtk_fd_dev->lock);
> +
> +       /* Enqueue the buffer */
> +       if (mtk_fd_dev->mem2mem2.streaming) {
> +               pr_info("%s: mtk_fd_dev_queue_buffers\n", node->name);
> +               mtk_fd_dev_queue_buffers(mtk_fd_dev, false);
> +       }
> +}
> +
> +#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +static int mtk_fd_vb2_queue_setup(struct vb2_queue *vq,
> +                                 const void *parg,
> +                                 unsigned int *num_buffers,
> +                                 unsigned int *num_planes,
> +                                 unsigned int sizes[], void *alloc_ctxs[])
> +#else
> +static int mtk_fd_vb2_queue_setup(struct vb2_queue *vq,
> +                                 unsigned int *num_buffers,
> +                                 unsigned int *num_planes,
> +                                 unsigned int sizes[],
> +                                 struct device *alloc_devs[])
> +#endif
> +{
> +       struct mtk_fd_mem2mem2_device *m2m2 = vb2_get_drv_priv(vq);
> +       struct mtk_fd_dev_video_device *node = mtk_fd_vbq_to_isp_node(vq);
> +       struct mtk_fd_dev *fd_dev = mtk_fd_m2m_to_dev(m2m2);
> +       void *buf_alloc_ctx = NULL;
> +       int queue = mtk_fd_dev_get_queue_id_of_dev_node(fd_dev, node);
> +       /* Get V4L2 format with the following method */
> +       const struct v4l2_format *fmt = &node->vdev_fmt;
> +
> +       *num_planes = 1;
> +       *num_buffers = clamp_val(*num_buffers, 1, VB2_MAX_FRAME);
> +
> +       vq->dma_attrs |= DMA_ATTR_NON_CONSISTENT;
> +       pr_info("queue(%d): cached mmap enabled\n", queue);
> +
> +       if (vq->type == V4L2_BUF_TYPE_META_CAPTURE ||
> +           vq->type == V4L2_BUF_TYPE_META_OUTPUT) {
> +               sizes[0] = fmt->fmt.meta.buffersize;
> +               buf_alloc_ctx = fd_dev->ctx.smem_vb2_alloc_ctx;
> +               pr_info("Select smem_vb2_alloc_ctx(%llx)\n",
> +                       (unsigned long long)buf_alloc_ctx);
> +       } else {
> +               sizes[0] = fmt->fmt.pix_mp.plane_fmt[0].sizeimage;
> +               buf_alloc_ctx = fd_dev->ctx.img_vb2_alloc_ctx;
> +               pr_info("Select img_vb2_alloc_ctx(%llx)\n",
> +                       (unsigned long long)buf_alloc_ctx);
> +       }
> +
> +#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +       alloc_ctxs[0] = buf_alloc_ctx;
> +#else
> +       alloc_devs[0] = (struct device *)buf_alloc_ctx;
> +#endif
> +
> +       pr_info("mtk_fd_vb2_queue_setup:type(%d),size(%d),ctx(%llx)\n",
> +               vq->type, sizes[0], (unsigned long long)buf_alloc_ctx);
> +
> +       /* Initialize buffer queue */
> +       INIT_LIST_HEAD(&node->buffers);
> +
> +       return 0;
> +}
> +
> +static bool mtk_fd_all_nodes_streaming(struct mtk_fd_mem2mem2_device *m2m2,
> +                                      struct mtk_fd_dev_video_device *except)
> +{
> +       int i;
> +
> +       for (i = 0; i < m2m2->num_nodes; i++) {
> +               struct mtk_fd_dev_video_device *node = &m2m2->nodes[i];
> +
> +               if (node == except)
> +                       continue;
> +               if (node->enabled && !vb2_start_streaming_called(&node->vbq))
> +                       return false;
> +       }
> +
> +       return true;
> +}
> +
> +static void mtk_fd_return_all_buffers(struct mtk_fd_mem2mem2_device *m2m2,
> +                                     struct mtk_fd_dev_video_device *node,
> +                                     enum vb2_buffer_state state)
> +{
> +       struct mtk_fd_dev *mtk_fd_dev = mtk_fd_m2m_to_dev(m2m2);
> +       struct mtk_fd_mem2mem2_buffer *b, *b0;
> +
> +       /* Return all buffers */
> +       mutex_lock(&mtk_fd_dev->lock);
> +       list_for_each_entry_safe(b, b0, &node->buffers, list) {
> +               list_del(&b->list);
> +               vb2_buffer_done(&b->vbb.vb2_buf, state);
> +       }
> +       mutex_unlock(&mtk_fd_dev->lock);
> +}
> +
> +static int mtk_fd_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
> +{
> +       struct mtk_fd_mem2mem2_device *m2m2 = vb2_get_drv_priv(vq);
> +       struct mtk_fd_dev_video_device *node =
> +               mtk_fd_vbq_to_isp_node(vq);
> +       int r;
> +
> +       if (m2m2->streaming) {
> +               r = -EBUSY;
> +               goto fail_return_bufs;
> +       }
> +
> +       if (!node->enabled) {
> +               pr_err("Node (%ld) is not enable\n", node - m2m2->nodes);
> +               r = -EINVAL;
> +               goto fail_return_bufs;
> +       }
> +#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +       r = media_entity_pipeline_start(&node->vdev.entity, &m2m2->pipeline);
> +#else
> +       r = media_pipeline_start(&node->vdev.entity, &m2m2->pipeline);
> +#endif
> +       if (r < 0) {
> +               pr_err("Node (%ld) media_pipeline_start failed\n",
> +                      node - m2m2->nodes);
> +               goto fail_return_bufs;
> +       }
> +
> +       if (!mtk_fd_all_nodes_streaming(m2m2, node))
> +               return 0;
> +
> +       /* Start streaming of the whole pipeline now */
> +
> +       r = v4l2_subdev_call(&m2m2->subdev, video, s_stream, 1);
> +       if (r < 0) {
> +               pr_err("Node (%ld) v4l2_subdev_call s_stream failed\n",
> +                      node - m2m2->nodes);
> +               goto fail_stop_pipeline;
> +       }
> +       return 0;
> +
> +fail_stop_pipeline:
> +#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +       media_entity_pipeline_stop(&node->vdev.entity);
> +#else
> +       media_pipeline_stop(&node->vdev.entity);
> +#endif
> +fail_return_bufs:
> +       mtk_fd_return_all_buffers(m2m2, node, VB2_BUF_STATE_QUEUED);
> +
> +       return r;
> +}
> +
> +static void mtk_fd_vb2_stop_streaming(struct vb2_queue *vq)
> +{
> +       struct mtk_fd_mem2mem2_device *m2m2 = vb2_get_drv_priv(vq);
> +       struct mtk_fd_dev_video_device *node =
> +               mtk_fd_vbq_to_isp_node(vq);
> +       int r;
> +
> +       WARN_ON(!node->enabled);
> +
> +       /* Was this the first node with streaming disabled? */
> +       if (mtk_fd_all_nodes_streaming(m2m2, node)) {
> +               /* Yes, really stop streaming now */
> +               r = v4l2_subdev_call(&m2m2->subdev, video, s_stream, 0);
> +               if (r)
> +                       dev_err(m2m2->dev, "failed to stop streaming\n");
> +       }
> +
> +       mtk_fd_return_all_buffers(m2m2, node, VB2_BUF_STATE_ERROR);
> +#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +       media_entity_pipeline_stop(&node->vdev.entity);
> +#else
> +       media_pipeline_stop(&node->vdev.entity);
> +#endif
> +}
> +
> +static int mtk_fd_videoc_querycap(struct file *file, void *fh,
> +                                 struct v4l2_capability *cap)
> +{
> +       struct mtk_fd_mem2mem2_device *m2m2 = video_drvdata(file);
> +       struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
> +       struct mtk_fd_dev *fd_dev = mtk_fd_m2m_to_dev(m2m2);
> +       int queue_id =
> +               mtk_fd_dev_get_queue_id_of_dev_node(fd_dev, node);
> +       struct mtk_fd_ctx_queue *node_ctx = &fd_dev->ctx.queue[queue_id];
> +
> +       strlcpy(cap->driver, m2m2->name, sizeof(cap->driver));
> +       strlcpy(cap->card, m2m2->model, sizeof(cap->card));
> +       snprintf(cap->bus_info, sizeof(cap->bus_info),
> +                "platform:%s", node->name);
> +
> +       cap->device_caps =
> +               mtk_fd_node_get_v4l2_cap(node_ctx) | V4L2_CAP_STREAMING;
> +       cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
> +
> +       return 0;
> +}
> +
> +/* Propagate forward always the format from the CIO2 subdev */
> +static int mtk_fd_videoc_g_fmt(struct file *file, void *fh,
> +                              struct v4l2_format *f)
> +{
> +       struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
> +
> +       f->fmt = node->vdev_fmt.fmt;
> +
> +       return 0;
> +}
> +
> +static int mtk_fd_videoc_try_fmt(struct file *file, void *fh,
> +                                struct v4l2_format *f)
> +{
> +       struct mtk_fd_mem2mem2_device *m2m2 = video_drvdata(file);
> +       struct mtk_fd_dev *fd_dev = mtk_fd_m2m_to_dev(m2m2);
> +       struct mtk_fd_ctx *dev_ctx = &fd_dev->ctx;
> +       struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
> +       int queue_id =
> +               mtk_fd_dev_get_queue_id_of_dev_node(fd_dev, node);
> +       int ret = 0;
> +
> +       ret = mtk_fd_ctx_fmt_set_img(dev_ctx, queue_id, &f->fmt.pix_mp,
> +                                    &node->vdev_fmt.fmt.pix_mp);
> +
> +       /* Simply set the format to the node context in the initial version */
> +       if (ret) {
> +               pr_warn("Fmt(%d) not support for queue(%d), load default fmt\n",
> +                       f->fmt.pix_mp.pixelformat, queue_id);
> +
> +               ret = mtk_fd_ctx_format_load_default_fmt
> +                       (&dev_ctx->queue[queue_id], f);
> +       }
> +
> +       if (!ret) {
> +               node->vdev_fmt.fmt.pix_mp = f->fmt.pix_mp;
> +               dev_ctx->queue[queue_id].fmt.pix_mp = node->vdev_fmt.fmt.pix_mp;
> +       }
> +
> +       return ret;
> +}
> +
> +static int mtk_fd_videoc_s_fmt(struct file *file, void *fh,
> +                              struct v4l2_format *f)
> +{
> +       struct mtk_fd_mem2mem2_device *m2m2 = video_drvdata(file);
> +       struct mtk_fd_dev *fd_dev = mtk_fd_m2m_to_dev(m2m2);
> +       struct mtk_fd_ctx *dev_ctx = &fd_dev->ctx;
> +       struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
> +       int queue_id = mtk_fd_dev_get_queue_id_of_dev_node(fd_dev, node);
> +       int ret = 0;
> +
> +       ret = mtk_fd_ctx_fmt_set_img(dev_ctx, queue_id, &f->fmt.pix_mp,
> +                                    &node->vdev_fmt.fmt.pix_mp);
> +
> +       /* Simply set the format to the node context in the initial version */
> +       if (!ret)
> +               dev_ctx->queue[queue_id].fmt.pix_mp = node->vdev_fmt.fmt.pix_mp;
> +       else
> +               dev_warn(&fd_dev->pdev->dev, "s_fmt, format not support\n");
> +
> +       return ret;
> +}
> +
> +static int mtk_fd_meta_enum_format(struct file *file, void *fh,
> +                                  struct v4l2_fmtdesc *f)
> +{
> +       struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
> +
> +       if (f->index > 0 || f->type != node->vbq.type)
> +               return -EINVAL;
> +
> +       f->pixelformat = node->vdev_fmt.fmt.meta.dataformat;
> +
> +       return 0;
> +}
> +
> +static int mtk_fd_videoc_s_meta_fmt(struct file *file, void *fh,
> +                                   struct v4l2_format *f)
> +{
> +       struct mtk_fd_mem2mem2_device *m2m2 = video_drvdata(file);
> +       struct mtk_fd_dev *fd_dev = mtk_fd_m2m_to_dev(m2m2);
> +       struct mtk_fd_ctx *dev_ctx = &fd_dev->ctx;
> +       struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
> +       int queue_id = mtk_fd_dev_get_queue_id_of_dev_node(fd_dev, node);
> +
> +       int ret = 0;
> +
> +       if (f->type != node->vbq.type)
> +               return -EINVAL;
> +
> +       ret = mtk_fd_ctx_format_load_default_fmt(&dev_ctx->queue[queue_id], f);
> +
> +       if (!ret) {
> +               node->vdev_fmt.fmt.meta = f->fmt.meta;
> +               dev_ctx->queue[queue_id].fmt.meta = node->vdev_fmt.fmt.meta;
> +       } else {
> +               dev_warn(&fd_dev->pdev->dev,
> +                        "s_meta_fm failed, format not support\n");
> +       }
> +
> +       return ret;
> +}
> +
> +static int mtk_fd_videoc_g_meta_fmt(struct file *file, void *fh,
> +                                   struct v4l2_format *f)
> +{
> +       struct mtk_fd_dev_video_device *node = file_to_mtk_fd_node(file);
> +
> +       if (f->type != node->vbq.type)
> +               return -EINVAL;
> +
> +       f->fmt = node->vdev_fmt.fmt;
> +
> +       return 0;
> +}
> +
> +int mtk_fd_videoc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
> +{
> +       struct video_device *vdev = video_devdata(file);
> +       struct vb2_buffer *vb;
> +       struct mtk_fd_dev_buffer *db;
> +       int r = 0;
> +
> +       /* check if vb2 queue is busy */
> +       if (vdev->queue->owner && vdev->queue->owner != file->private_data)
> +               return -EBUSY;
> +
> +       /**
> +        * Keep the value of sequence in v4l2_buffer
> +        * in ctx buf since vb2_qbuf will set it to 0
> +        */
> +       vb = vdev->queue->bufs[p->index];
> +
> +       if (vb) {
> +               db = mtk_fd_vb2_buf_to_dev_buf(vb);
> +               pr_info("qbuf: p:%llx,vb:%llx, db:%llx\n",
> +                       (unsigned long long)p,
> +                       (unsigned long long)vb,
> +                       (unsigned long long)db);
> +               db->ctx_buf.user_sequence = p->sequence;
> +       }
> +       r = vb2_qbuf(vdev->queue, vdev->v4l2_dev->mdev, p);
> +       if (r)
> +               pr_err("vb2_qbuf failed(err=%d): buf idx(%d)\n", r, p->index);
> +       return r;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_videoc_qbuf);
> +
> +/******************** function pointers ********************/
> +
> +/* subdev internal operations */
> +static const struct v4l2_subdev_internal_ops mtk_fd_subdev_internal_ops = {
> +       .open = mtk_fd_subdev_open,
> +       .close = mtk_fd_subdev_close,
> +};
> +
> +static const struct v4l2_subdev_core_ops mtk_fd_subdev_core_ops = {
> +#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +       .g_ext_ctrls = v4l2_subdev_g_ext_ctrls,
> +       .try_ext_ctrls = v4l2_subdev_try_ext_ctrls,
> +       .s_ext_ctrls = v4l2_subdev_s_ext_ctrls,
> +       .g_ctrl = v4l2_subdev_g_ctrl,
> +       .s_ctrl = v4l2_subdev_s_ctrl,
> +       .queryctrl = v4l2_subdev_queryctrl,
> +       .querymenu = v4l2_subdev_querymenu,
> +#endif
> +};
> +
> +static const struct v4l2_subdev_video_ops mtk_fd_subdev_video_ops = {
> +       .s_stream = mtk_fd_subdev_s_stream,
> +};
> +
> +static const struct v4l2_subdev_ops mtk_fd_subdev_ops = {
> +       .core = &mtk_fd_subdev_core_ops,
> +       .video = &mtk_fd_subdev_video_ops,
> +};
> +
> +static const struct media_entity_operations mtk_fd_media_ops = {
> +       .link_setup = mtk_fd_link_setup,
> +       .link_validate = v4l2_subdev_link_validate,
> +};
> +
> +static const struct vb2_ops mtk_fd_vb2_ops = {
> +       .buf_queue = mtk_fd_vb2_buf_queue,
> +       .queue_setup = mtk_fd_vb2_queue_setup,
> +       .start_streaming = mtk_fd_vb2_start_streaming,
> +       .stop_streaming = mtk_fd_vb2_stop_streaming,
> +       .wait_prepare = vb2_ops_wait_prepare,
> +       .wait_finish = vb2_ops_wait_finish,
> +};
> +
> +static const struct v4l2_file_operations mtk_fd_v4l2_fops = {
> +       .unlocked_ioctl = video_ioctl2,
> +       .open = v4l2_fh_open,
> +       .release = vb2_fop_release,
> +       .poll = vb2_fop_poll,
> +       .mmap = vb2_fop_mmap,
> +#ifdef CONFIG_COMPAT
> +       .compat_ioctl32 = v4l2_compat_ioctl32,
> +#endif
> +};
> +
> +static const struct v4l2_ioctl_ops mtk_fd_v4l2_ioctl_ops = {
> +       .vidioc_querycap = mtk_fd_videoc_querycap,
> +
> +       .vidioc_g_fmt_vid_cap_mplane = mtk_fd_videoc_g_fmt,
> +       .vidioc_s_fmt_vid_cap_mplane = mtk_fd_videoc_s_fmt,
> +       .vidioc_try_fmt_vid_cap_mplane = mtk_fd_videoc_try_fmt,
> +
> +       .vidioc_g_fmt_vid_out_mplane = mtk_fd_videoc_g_fmt,
> +       .vidioc_s_fmt_vid_out_mplane = mtk_fd_videoc_s_fmt,
> +       .vidioc_try_fmt_vid_out_mplane = mtk_fd_videoc_try_fmt,
> +
> +       /* buffer queue management */
> +       .vidioc_reqbufs = vb2_ioctl_reqbufs,
> +       .vidioc_create_bufs = vb2_ioctl_create_bufs,
> +       .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
> +       .vidioc_querybuf = vb2_ioctl_querybuf,
> +       .vidioc_qbuf = mtk_fd_videoc_qbuf,
> +       .vidioc_dqbuf = vb2_ioctl_dqbuf,
> +       .vidioc_streamon = vb2_ioctl_streamon,
> +       .vidioc_streamoff = vb2_ioctl_streamoff,
> +       .vidioc_expbuf = vb2_ioctl_expbuf,
> +};
> +
> +static const struct v4l2_ioctl_ops mtk_fd_v4l2_meta_ioctl_ops = {
> +       .vidioc_querycap = mtk_fd_videoc_querycap,
> +
> +       .vidioc_enum_fmt_meta_cap = mtk_fd_meta_enum_format,
> +       .vidioc_g_fmt_meta_cap = mtk_fd_videoc_g_meta_fmt,
> +       .vidioc_s_fmt_meta_cap = mtk_fd_videoc_s_meta_fmt,
> +       .vidioc_try_fmt_meta_cap = mtk_fd_videoc_g_meta_fmt,
> +
> +       .vidioc_enum_fmt_meta_out = mtk_fd_meta_enum_format,
> +       .vidioc_g_fmt_meta_out = mtk_fd_videoc_g_meta_fmt,
> +       .vidioc_s_fmt_meta_out = mtk_fd_videoc_s_meta_fmt,
> +       .vidioc_try_fmt_meta_out = mtk_fd_videoc_g_meta_fmt,
> +
> +       .vidioc_reqbufs = vb2_ioctl_reqbufs,
> +       .vidioc_create_bufs = vb2_ioctl_create_bufs,
> +       .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
> +       .vidioc_querybuf = vb2_ioctl_querybuf,
> +       .vidioc_qbuf = mtk_fd_videoc_qbuf,
> +       .vidioc_dqbuf = vb2_ioctl_dqbuf,
> +       .vidioc_streamon = vb2_ioctl_streamon,
> +       .vidioc_streamoff = vb2_ioctl_streamoff,
> +       .vidioc_expbuf = vb2_ioctl_expbuf,
> +};
> +
> +static u32 mtk_fd_node_get_v4l2_cap(struct mtk_fd_ctx_queue *node_ctx)
> +{
> +       u32 cap = 0;
> +
> +       if (node_ctx->desc.capture)
> +               if (node_ctx->desc.image)
> +                       cap = V4L2_CAP_VIDEO_CAPTURE_MPLANE;
> +               else
> +                       cap = V4L2_CAP_META_CAPTURE;
> +       else
> +               if (node_ctx->desc.image)
> +                       cap = V4L2_CAP_VIDEO_OUTPUT_MPLANE;
> +               else
> +                       cap = V4L2_CAP_META_OUTPUT;
> +
> +       return cap;
> +}
> +
> +static u32 mtk_fd_node_get_format_type(struct mtk_fd_ctx_queue *node_ctx)
> +{
> +       u32 type;
> +
> +       if (node_ctx->desc.capture)
> +               if (node_ctx->desc.image)
> +                       type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
> +               else
> +                       type = V4L2_BUF_TYPE_META_CAPTURE;
> +       else
> +               if (node_ctx->desc.image)
> +                       type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
> +               else
> +                       type = V4L2_BUF_TYPE_META_OUTPUT;
> +
> +       return type;
> +}
> +
> +static const struct v4l2_ioctl_ops *mtk_fd_node_get_ioctl_ops
> +       (struct mtk_fd_ctx_queue *node_ctx)
> +{
> +       const struct v4l2_ioctl_ops *ops = NULL;
> +
> +       if (node_ctx->desc.image)
> +               ops = &mtk_fd_v4l2_ioctl_ops;
> +       else
> +               ops = &mtk_fd_v4l2_meta_ioctl_ops;
> +       return ops;
> +}
> +
> +/**
> + * Config node's video properties
> + * according to the device context requirement
> + */
> +static void mtk_fd_node_to_v4l2(struct mtk_fd_dev *fd_dev, u32 node,
> +                               struct video_device *vdev,
> +                               struct v4l2_format *f)
> +{
> +       u32 cap;
> +       struct mtk_fd_ctx *device_ctx = &fd_dev->ctx;
> +       struct mtk_fd_ctx_queue *node_ctx = &device_ctx->queue[node];
> +
> +       WARN_ON(node >= mtk_fd_dev_get_total_node(fd_dev));
> +       WARN_ON(!node_ctx);
> +
> +       /* set cap of the node */
> +       cap = mtk_fd_node_get_v4l2_cap(node_ctx);
> +       f->type = mtk_fd_node_get_format_type(node_ctx);
> +       vdev->ioctl_ops = mtk_fd_node_get_ioctl_ops(node_ctx);
> +
> +       if (mtk_fd_ctx_format_load_default_fmt(&device_ctx->queue[node], f)) {
> +               dev_err(&fd_dev->pdev->dev,
> +                       "Can't load default for node (%d): (%s)",
> +                       node, device_ctx->queue[node].desc.name);
> +       } else {
> +               if (device_ctx->queue[node].desc.image) {
> +                       dev_dbg(&fd_dev->pdev->dev,
> +                               "Node (%d): (%s), dfmt (f:0x%x w:%d: h:%d s:%d)\n",
> +                               node, device_ctx->queue[node].desc.name,
> +                               f->fmt.pix_mp.pixelformat,
> +                               f->fmt.pix_mp.width,
> +                               f->fmt.pix_mp.height,
> +                               f->fmt.pix_mp.plane_fmt[0].sizeimage);
> +                       node_ctx->fmt.pix_mp = f->fmt.pix_mp;
> +               } else {
> +                       dev_info(&fd_dev->pdev->dev,
> +                                "Node (%d): (%s), dfmt (f:0x%x s:%u)\n",
> +                                node, device_ctx->queue[node].desc.name,
> +                                f->fmt.meta.dataformat,
> +                                f->fmt.meta.buffersize);
> +                       node_ctx->fmt.meta = f->fmt.meta;
> +               }
> +       }
> +
> +#if KERNEL_VERSION(4, 7, 0) < MTK_FD_KERNEL_BASE_VERSION
> +       /* device_caps was supported after 4.7 */
> +       vdev->device_caps = V4L2_CAP_STREAMING | cap;
> +#endif
> +}
> +
> +int mtk_fd_media_register(struct device *dev, struct media_device *media_dev,
> +                         const char *model)
> +{
> +       int r = 0;
> +
> +       media_dev->dev = dev;
> +       dev_info(dev, "setup media_dev.dev: %llx\n",
> +                (unsigned long long)media_dev->dev);
> +
> +       strlcpy(media_dev->model, model, sizeof(media_dev->model));
> +       dev_info(dev, "setup media_dev.model: %s\n",
> +                media_dev->model);
> +
> +       snprintf(media_dev->bus_info, sizeof(media_dev->bus_info),
> +                "%s", dev_name(dev));
> +       dev_info(dev, "setup media_dev.bus_info: %s\n",
> +                media_dev->bus_info);
> +
> +       media_dev->hw_revision = 0;
> +       dev_info(dev, "setup media_dev.hw_revision: %d\n",
> +                media_dev->hw_revision);
> +
> +#if KERNEL_VERSION(4, 5, 0) <= MTK_FD_KERNEL_BASE_VERSION
> +       dev_info(dev, "media_device_init: media_dev:%llx\n",
> +                (unsigned long long)media_dev);
> +       media_device_init(media_dev);
> +#endif
> +
> +       pr_info("Register media device: %s, %llx",
> +               media_dev->model,
> +               (unsigned long long)media_dev);
> +
> +       r = media_device_register(media_dev);
> +
> +       if (r) {
> +               dev_err(dev, "failed to register media device (%d)\n", r);
> +               goto fail_v4l2_dev;
> +       }
> +       return 0;
> +
> +fail_v4l2_dev:
> +       media_device_unregister(media_dev);
> +#if KERNEL_VERSION(4, 5, 0) <= MTK_FD_KERNEL_BASE_VERSION
> +       media_device_cleanup(media_dev);
> +#endif
> +
> +       return r;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_media_register);
> +
> +int mtk_fd_v4l2_register(struct device *dev,
> +                        struct media_device *media_dev,
> +                        struct v4l2_device *v4l2_dev)
> +{
> +       int r = 0;
> +       /* Set up v4l2 device */
> +       v4l2_dev->mdev = media_dev;
> +       dev_info(dev, "setup v4l2_dev->mdev: %llx",
> +                (unsigned long long)v4l2_dev->mdev);
> +
> +       dev_info(dev, "Register v4l2 device: %llx",
> +                (unsigned long long)v4l2_dev);
> +
> +       r = v4l2_device_register(dev, v4l2_dev);
> +
> +       if (r) {
> +               dev_err(dev, "failed to register V4L2 device (%d)\n", r);
> +               goto fail_v4l2_dev;
> +       }
> +
> +       return 0;
> +
> +fail_v4l2_dev:
> +       media_device_unregister(media_dev);
> +#if KERNEL_VERSION(4, 5, 0) <= MTK_FD_KERNEL_BASE_VERSION
> +       media_device_cleanup(media_dev);
> +#endif
> +
> +       return r;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_v4l2_register);
> +
> +int mtk_fd_mem2mem2_v4l2_register(struct mtk_fd_dev *dev,
> +                                 struct media_device *media_dev,
> +                                 struct v4l2_device *v4l2_dev)
> +{
> +       struct mtk_fd_mem2mem2_device *m2m2 = &dev->mem2mem2;
> +
> +       int i, r;
> +
> +       /**
> +        * If media_dev or v4l2_dev is not set,
> +        * use the default one in mtk_fd_dev
> +        */
> +       if (!media_dev) {
> +               m2m2->media_dev = &dev->media_dev;
> +               r = mtk_fd_media_register(&dev->pdev->dev, m2m2->media_dev,
> +                                         m2m2->model);
> +
> +       if (r) {
> +               dev_err(m2m2->dev, "failed to register media device (%d)\n", r);
> +               goto fail_media_dev;
> +       }
> +       } else {
> +               m2m2->media_dev = media_dev;
> +       }
> +
> +       if (!v4l2_dev) {
> +               m2m2->v4l2_dev = &dev->v4l2_dev;
> +               r = mtk_fd_v4l2_register(&dev->pdev->dev,
> +                                        m2m2->media_dev,
> +                                        m2m2->v4l2_dev);
> +       if (r) {
> +               dev_err(m2m2->dev, "failed to register V4L2 device (%d)\n", r);
> +               goto fail_v4l2_dev;
> +       }
> +       } else {
> +               m2m2->v4l2_dev = v4l2_dev;
> +       }
> +
> +       /* Initialize miscellaneous variables */
> +       m2m2->streaming = false;
> +       m2m2->v4l2_file_ops = mtk_fd_v4l2_fops;
> +
> +       /* Initialize subdev media entity */
> +       m2m2->subdev_pads = kcalloc(m2m2->num_nodes, sizeof(*m2m2->subdev_pads),
> +                                   GFP_KERNEL);
> +       if (!m2m2->subdev_pads) {
> +               r = -ENOMEM;
> +               goto fail_subdev_pads;
> +       }
> +#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +       r = media_entity_init(&m2m2->subdev.entity, m2m2->num_nodes,
> +                             m2m2->subdev_pads, 0);
> +#else
> +       r = media_entity_pads_init(&m2m2->subdev.entity, m2m2->num_nodes,
> +                                  m2m2->subdev_pads);
> +#endif
> +       if (r) {
> +               dev_err(m2m2->dev,
> +                       "failed initialize subdev media entity (%d)\n", r);
> +               goto fail_media_entity;
> +       }
> +
> +       /* Initialize subdev */
> +       v4l2_subdev_init(&m2m2->subdev, &mtk_fd_subdev_ops);
> +
> +       m2m2->subdev.entity.function =
> +               MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
> +
> +       m2m2->subdev.entity.ops = &mtk_fd_media_ops;
> +
> +       for (i = 0; i < m2m2->num_nodes; i++) {
> +               m2m2->subdev_pads[i].flags = m2m2->nodes[i].output ?
> +                       MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
> +       }
> +
> +       m2m2->subdev.flags =
> +               V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
> +       snprintf(m2m2->subdev.name, sizeof(m2m2->subdev.name),
> +                "%s", m2m2->name);
> +       v4l2_set_subdevdata(&m2m2->subdev, m2m2);
> +       m2m2->subdev.internal_ops = &mtk_fd_subdev_internal_ops;
> +
> +       pr_info("register subdev: %s\n", m2m2->subdev.name);
> +       r = v4l2_device_register_subdev(m2m2->v4l2_dev, &m2m2->subdev);
> +       if (r) {
> +               dev_err(m2m2->dev, "failed initialize subdev (%d)\n", r);
> +               goto fail_subdev;
> +       }
> +       r = v4l2_device_register_subdev_nodes(m2m2->v4l2_dev);
> +       if (r) {
> +               dev_err(m2m2->dev, "failed to register subdevs (%d)\n", r);
> +               goto fail_subdevs;
> +       }
> +
> +       /* Create video nodes and links */
> +       for (i = 0; i < m2m2->num_nodes; i++) {
> +               struct mtk_fd_dev_video_device *node = &m2m2->nodes[i];
> +               struct video_device *vdev = &node->vdev;
> +               struct vb2_queue *vbq = &node->vbq;
> +               u32 flags;
> +
> +               /* Initialize miscellaneous variables */
> +               mutex_init(&node->lock);
> +               INIT_LIST_HEAD(&node->buffers);
> +
> +               /* Initialize formats to default values */
> +               mtk_fd_node_to_v4l2(dev, i, vdev, &node->vdev_fmt);
> +
> +               /* Initialize media entities */
> +#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +               r = media_entity_init(&vdev->entity, 1, &node->vdev_pad, 0);
> +#else
> +               r = media_entity_pads_init(&vdev->entity, 1, &node->vdev_pad);
> +#endif
> +               if (r) {
> +                       dev_err(m2m2->dev,
> +                               "failed initialize media entity (%d)\n", r);
> +                       goto fail_vdev_media_entity;
> +               }
> +               node->vdev_pad.flags = node->output ?
> +                       MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
> +               vdev->entity.ops = NULL;
> +
> +               /* Initialize vbq */
> +               vbq->type = node->vdev_fmt.type;
> +               vbq->io_modes = VB2_MMAP | VB2_DMABUF;
> +               vbq->ops = &mtk_fd_vb2_ops;
> +               vbq->mem_ops = m2m2->vb2_mem_ops;
> +               m2m2->buf_struct_size = sizeof(struct mtk_fd_dev_buffer);
> +               vbq->buf_struct_size = m2m2->buf_struct_size;
> +               vbq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
> +               vbq->min_buffers_needed = 0;    /* Can streamon w/o buffers */
> +               /* Put the process hub sub device in the vb2 private data*/
> +               vbq->drv_priv = m2m2;
> +               vbq->lock = &node->lock;
> +               r = vb2_queue_init(vbq);
> +               if (r) {
> +                       dev_err(m2m2->dev,
> +                               "failed to initialize video queue (%d)\n", r);
> +                       goto fail_vdev;
> +               }
> +
> +               /* Initialize vdev */
> +               snprintf(vdev->name, sizeof(vdev->name), "%s %s",
> +                        m2m2->name, node->name);
> +               vdev->release = video_device_release_empty;
> +               vdev->fops = &m2m2->v4l2_file_ops;
> +               vdev->lock = &node->lock;
> +               vdev->v4l2_dev = m2m2->v4l2_dev;
> +               vdev->queue = &node->vbq;
> +               vdev->vfl_dir = node->output ? VFL_DIR_TX : VFL_DIR_RX;
> +               video_set_drvdata(vdev, m2m2);
> +               pr_info("register vdev: %s\n", vdev->name);
> +               r = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
> +               if (r) {
> +                       dev_err(m2m2->dev,
> +                               "failed to register video device (%d)\n", r);
> +                       goto fail_vdev;
> +               }
> +
> +               /* Create link between video node and the subdev pad */
> +               flags = 0;
> +               if (node->enabled)
> +                       flags |= MEDIA_LNK_FL_ENABLED;
> +               if (node->immutable)
> +                       flags |= MEDIA_LNK_FL_IMMUTABLE;
> +               if (node->output) {
> +#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +                       r = media_entity_create_link
> +#else
> +                       r = media_create_pad_link
> +#endif
> +                                               (&vdev->entity, 0,
> +                                                &m2m2->subdev.entity,
> +                                                i, flags);
> +               } else {
> +#if KERNEL_VERSION(4, 5, 0) >= MTK_FD_KERNEL_BASE_VERSION
> +                       r = media_entity_create_link
> +#else
> +                       r = media_create_pad_link
> +#endif
> +                                               (&m2m2->subdev.entity,
> +                                                i, &vdev->entity, 0,
> +                                                flags);
> +               }
> +               if (r)
> +                       goto fail_link;
> +       }
> +
> +       return 0;
> +
> +       for (; i >= 0; i--) {
> +fail_link:
> +               video_unregister_device(&m2m2->nodes[i].vdev);
> +fail_vdev:
> +               media_entity_cleanup(&m2m2->nodes[i].vdev.entity);
> +fail_vdev_media_entity:
> +               mutex_destroy(&m2m2->nodes[i].lock);
> +       }
> +fail_subdevs:
> +       v4l2_device_unregister_subdev(&m2m2->subdev);
> +fail_subdev:
> +       media_entity_cleanup(&m2m2->subdev.entity);
> +fail_media_entity:
> +       kfree(m2m2->subdev_pads);
> +fail_subdev_pads:
> +       v4l2_device_unregister(m2m2->v4l2_dev);
> +fail_v4l2_dev:
> +fail_media_dev:
> +       pr_err("fail_v4l2_dev: media_device_unregister and clenaup:%llx",
> +              (unsigned long long)m2m2->media_dev);
> +       media_device_unregister(m2m2->media_dev);
> +#if KERNEL_VERSION(4, 5, 0) <= MTK_FD_KERNEL_BASE_VERSION
> +       media_device_cleanup(m2m2->media_dev);
> +#endif
> +
> +       return r;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_mem2mem2_v4l2_register);
> +
> +int mtk_fd_v4l2_unregister(struct mtk_fd_dev *dev)
> +{
> +       struct mtk_fd_mem2mem2_device *m2m2 = &dev->mem2mem2;
> +       unsigned int i;
> +
> +       for (i = 0; i < m2m2->num_nodes; i++) {
> +               video_unregister_device(&m2m2->nodes[i].vdev);
> +               media_entity_cleanup(&m2m2->nodes[i].vdev.entity);
> +               mutex_destroy(&m2m2->nodes[i].lock);
> +       }
> +
> +       v4l2_device_unregister_subdev(&m2m2->subdev);
> +       media_entity_cleanup(&m2m2->subdev.entity);
> +       kfree(m2m2->subdev_pads);
> +       v4l2_device_unregister(m2m2->v4l2_dev);
> +       media_device_unregister(m2m2->media_dev);
> +#if KERNEL_VERSION(4, 5, 0) <= MTK_FD_KERNEL_BASE_VERSION
> +       media_device_cleanup(m2m2->media_dev);
> +#endif
> +
> +       return 0;
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_v4l2_unregister);
> +
> +void mtk_fd_v4l2_buffer_done(struct vb2_buffer *vb,
> +                            enum vb2_buffer_state state)
> +{
> +       struct mtk_fd_mem2mem2_buffer *b =
> +               container_of(vb, struct mtk_fd_mem2mem2_buffer, vbb.vb2_buf);
> +
> +       list_del(&b->list);
> +       vb2_buffer_done(&b->vbb.vb2_buf, state);
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_v4l2_buffer_done);
> diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c b/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c
> new file mode 100644
> index 0000000..bd447b7
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c
> @@ -0,0 +1,114 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * Copyright (c) 2018 MediaTek Inc.
> + * Author: Frederic Chen <frederic.chen@mediatek.com>
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> + * GNU General Public License for more details.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/platform_device.h>
> +#include "mtk_fd.h"
> +#include "mtk_fd-ctx.h"
> +#include "mtk_fd-v4l2.h"
> +
> +static struct mtk_fd_ctx_format fd_param_fmts[] = {
> +       {
> +               .fmt.meta = {
> +               .dataformat = V4L2_META_FMT_MTISP_PARAMS,
> +               .max_buffer_size = 1024 * 30,
> +               },
> +       },
> +};
> +
> +static struct mtk_fd_ctx_format in_fmts[] = {
> +       {
> +               .fmt.img = {
> +                       .pixelformat  = V4L2_PIX_FMT_VYUY,
> +                       .depth    = { 16 },
> +                       .row_depth  = { 16 },
> +                       .num_planes = 1,
> +               },
> +       },
> +       {
> +               .fmt.img = {
> +                       .pixelformat  = V4L2_PIX_FMT_YUYV,
> +                       .depth    = { 16 },
> +                       .row_depth  = { 16 },
> +                       .num_planes = 1,
> +               },
> +       },
> +       {
> +               .fmt.img = {
> +                       .pixelformat  = V4L2_PIX_FMT_YVYU,
> +                       .depth    = { 16 },
> +                       .row_depth  = { 16 },
> +                       .num_planes = 1,
> +               },
> +       },
> +       {
> +               .fmt.img = {
> +                       .pixelformat  = V4L2_PIX_FMT_UYVY,
> +                       .depth    = { 16 },
> +                       .row_depth  = { 16 },
> +                       .num_planes = 1,
> +               },
> +       },
> +};
> +
> +static struct mtk_fd_ctx_queue_desc
> +output_queues[MTK_FD_CTX_FD_TOTAL_OUTPUT] = {
> +       {
> +               .id = MTK_FD_CTX_FD_YUV_IN,
> +               .name = "FDInput",
> +               .capture = 0,
> +               .image = 1,
> +               .fmts = in_fmts,
> +               .num_fmts = ARRAY_SIZE(in_fmts),
> +               .default_fmt_idx = 1,
> +       },
> +       {
> +               .id = MTK_FD_CTX_FD_CONFIG_IN,
> +               .name = "FDConfig",
> +               .capture = 0,
> +               .image = 0,
> +               .fmts = fd_param_fmts,
> +               .num_fmts = 1,
> +               .default_fmt_idx = 0,
> +       },
> +};
> +
> +static struct mtk_fd_ctx_queue_desc
> +capture_queues[MTK_FD_CTX_FD_TOTAL_CAPTURE] = {
> +       {
> +               .id = MTK_FD_CTX_FD_OUT,
> +               .name = "FDOutput",
> +               .capture = 1,
> +               .image = 0,
> +               .fmts = fd_param_fmts,
> +               .num_fmts = 1,
> +               .default_fmt_idx = 0,
> +       },
> +};
> +
> +static struct mtk_fd_ctx_queues_setting queues_setting = {
> +       .master = MTK_FD_CTX_FD_OUT,
> +       .output_queue_descs = output_queues,
> +       .total_output_queues = MTK_FD_CTX_FD_TOTAL_OUTPUT,
> +       .capture_queue_descs = capture_queues,
> +       .total_capture_queues = MTK_FD_CTX_FD_TOTAL_CAPTURE,
> +};
> +
> +int mtk_fd_ctx_fd_init(struct mtk_fd_ctx *ctx)
> +{
> +       /* Initialize main data structure */
> +       return mtk_fd_ctx_core_queue_setup(ctx, &queues_setting);
> +}
> +EXPORT_SYMBOL_GPL(mtk_fd_ctx_fd_init);
> diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h b/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h
> new file mode 100644
> index 0000000..0702abc
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h
> @@ -0,0 +1,36 @@
> +/* SPDX-License-Identifier: GPL-2.0 */
> +/*
> + * Copyright (c) 2018 MediaTek Inc.
> + * Author: Frederic Chen <frederic.chen@mediatek.com>
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> + * GNU General Public License for more details.
> + */
> +
> +#ifndef __MTK_FD_V4L2__
> +#define __MTK_FD_V4L2__
> +
> +#include <linux/types.h>
> +#include "mtk_fd-ctx.h"
> +
> +#define MTK_FD_DEV_NAME     "MTK-FD-V4L2"
> +
> +/* Input: YUV image */
> +#define MTK_FD_CTX_FD_YUV_IN           0
> +/* Input: FD Configuration */
> +#define MTK_FD_CTX_FD_CONFIG_IN                1
> +#define MTK_FD_CTX_FD_TOTAL_OUTPUT     2
> +
> +/* OUT: FD output devices*/
> +#define MTK_FD_CTX_FD_OUT              2
> +#define MTK_FD_CTX_FD_TOTAL_CAPTURE    1
> +
> +int mtk_fd_ctx_fd_init(struct mtk_fd_ctx *ctx);
> +
> +#endif /*__MTK_FD_V4L2__*/
> diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd.c b/drivers/media/platform/mtk-isp/fd/mtk_fd.c
> new file mode 100644
> index 0000000..7e2fd00
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/mtk_fd.c
> @@ -0,0 +1,730 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * Copyright (C) 2015 MediaTek Inc.
> + *
> + * This program is free software: you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
> + * GNU General Public License for more details.
> + */
> +
> +#include <linux/init.h>
> +#include <linux/module.h>
> +#include <linux/kernel.h>
> +#include <linux/types.h>
> +#include <linux/device.h>
> +#include <linux/kdev_t.h>
> +
> +#include <linux/platform_device.h>
> +#include <linux/dma-mapping.h>
> +#include <linux/mm_types.h>
> +#include <linux/mm.h>
> +#include <linux/jiffies.h>
> +#include <linux/sched.h>
> +#include <linux/uaccess.h>
> +#include <asm/page.h>
> +#include <linux/vmalloc.h>
> +#include <linux/interrupt.h>
> +#include <linux/wait.h>
> +
> +#include <linux/of_platform.h>
> +#include <linux/of_irq.h>
> +#include <linux/of_address.h>
> +#include <linux/of_device.h>
> +
> +#include "mtk_fd.h"
> +#include "mtk_fd-core.h"
> +
> +#include "mtk_vpu.h"
> +
> +#include <linux/pm_runtime.h>
> +#include <linux/clk.h>
> +
> +#ifdef CONFIG_PM_WAKELOCKS
> +#include <linux/pm_wakeup.h>
> +#else
> +#include <linux/wakelock.h>
> +#endif
> +
> +#define FD_DRVNAME     "mtk-fd"
> +
> +static const struct of_device_id mtk_fd_of_ids[] = {
> +       /* Remider: Add this device node manually in .dtsi */
> +       { .compatible = "mediatek,fd", },
> +       {}
> +};
> +MODULE_DEVICE_TABLE(of, mtk_fd_of_ids);
> +
> +static void mtk_fd_prepare_enable_ccf_clock(struct mtk_fd_drv_dev *fd_hw_dev)
> +{
> +       int ret;
> +
> +       pm_runtime_get_sync(fd_hw_dev->larb_dev);
> +       ret = clk_prepare_enable(fd_hw_dev->fd_clk);
> +       if (ret)
> +               dev_err(&fd_hw_dev->pdev->dev,
> +                       "cannot prepare and enable CG_IMGSYS_FD clock\n");
> +}
> +
> +static void mtk_fd_disable_unprepare_ccf_clock(struct mtk_fd_drv_dev *fd_hw_dev)
> +{
> +       clk_disable_unprepare(fd_hw_dev->fd_clk);
> +}
> +
> +static int mtk_fd_clk_ctrl(struct mtk_fd_drv_dev *fd_hw_dev, int en)
> +{
> +       if (en)
> +               mtk_fd_prepare_enable_ccf_clock(fd_hw_dev);
> +       else
> +               mtk_fd_disable_unprepare_ccf_clock(fd_hw_dev);
> +
> +       dev_dbg(&fd_hw_dev->pdev->dev, "clock en: %d\n", en);
> +
> +       return 0;
> +}
> +
> +static int mtk_fd_wait_irq(struct mtk_fd_drv_dev *fd_hw_dev)
> +{
> +       int timeout;
> +
> +       timeout = wait_event_interruptible_timeout
> +               (fd_hw_dev->wq,
> +                (fd_hw_dev->fd_irq_result & FD_IRQ_MASK),
> +                mtk_fd_us_to_jiffies(1 * 1000000));
> +
> +       if (timeout == 0) {
> +               dev_err(&fd_hw_dev->pdev->dev,
> +                       "%s timeout, %d\n",
> +                       __func__, fd_hw_dev->fd_irq_result);
> +               return -EAGAIN;
> +       }
> +
> +       dev_dbg(&fd_hw_dev->pdev->dev, "irq_res: 0x%8x\n",
> +               fd_hw_dev->fd_irq_result);
> +
> +       if (timeout != 0 && !(fd_hw_dev->fd_irq_result & FD_IRQ_MASK)) {
> +               dev_err(&fd_hw_dev->pdev->dev,
> +                       "%s interrupted by system signal, return value(%d)\n",
> +                       __func__, timeout);
> +               return -ERESTARTSYS;
> +       }
> +
> +       if (!(fd_hw_dev->fd_irq_result & FD_IRQ_MASK)) {
> +               dev_err(&fd_hw_dev->pdev->dev,
> +                       "%s Not FD, %d\n",
> +                       __func__, fd_hw_dev->fd_irq_result);
> +               return -1;
> +       }
> +
> +       fd_hw_dev->fd_irq_result = 0;
> +
> +       return 0;
> +}
> +
> +static int mtk_fd_send_ipi_init(struct platform_device *pdev,
> +                               struct fd_buffer *scp_mem)
> +{
> +       struct ipi_message fd_init_msg;
> +
> +       fd_init_msg.cmd_id = FD_CMD_INIT;
> +       fd_init_msg.fd_manager = *scp_mem;
> +
> +       vpu_ipi_send(pdev, IPI_FD_CMD, &fd_init_msg, sizeof(fd_init_msg));
> +
> +       return 0;
> +}
> +
> +static int mtk_fd_send_ipi_cmd(struct platform_device *pdev,
> +                              struct v4l2_fd_param *fd_param)
> +{
> +       struct ipi_message fd_ipi_msg;
> +
> +       fd_ipi_msg.cmd_id = FD_CMD_ENQ;
> +       fd_ipi_msg.fd_param = *fd_param;
> +
> +       vpu_ipi_send_sync_async(pdev, IPI_FD_CMD, &fd_ipi_msg,
> +                               sizeof(fd_ipi_msg), 0);
> +       return 0;
> +}
> +
> +static irqreturn_t mtk_fd_irq(int irq, void *dev_addr)
> +{
> +       struct mtk_fd_drv_dev *fd_hw_dev;
> +
> +       fd_hw_dev = (struct mtk_fd_drv_dev *)dev_addr;
> +       fd_hw_dev->fd_irq_result = FD_RD32(fd_hw_dev->fd_base + FD_INT);
> +       wake_up_interruptible(&fd_hw_dev->wq);
> +
> +       return IRQ_HANDLED;
> +}
> +
> +static int mtk_fd_do_callback(struct mtk_fd_drv_dev *fd_hw_dev,
> +                             unsigned int frame_state)
> +{
> +       struct mtk_fd_ctx_finish_param fparam;
> +       struct mtk_isp_fd_drv_data *drv_data;
> +       struct device *dev;
> +       int ret = 0;
> +
> +       drv_data = mtk_fd_hw_dev_to_drv(fd_hw_dev);
> +       dev = &drv_data->fd_hw_dev.pdev->dev;
> +
> +       fparam.state = frame_state;
> +       fparam.frame_id = fd_hw_dev->fd_ctx.frame_id;
> +       dev_dbg(dev, "frame_id(%d)\n", fparam.frame_id);
> +
> +       ret = mtk_fd_ctx_core_job_finish(&drv_data->fd_dev.ctx, &fparam);
> +       if (ret)
> +               dev_err(dev, "frame_id(%d), finish op failed: %d\n",
> +                       fparam.frame_id, ret);
> +       else
> +               fd_hw_dev->state = FD_CBD;
> +
> +       return ret;
> +}
> +
> +static int mtk_fd_dequeue(struct mtk_fd_drv_dev *fd_hw_dev,
> +                         struct v4l2_fd_param *fd_param)
> +{
> +       struct mtk_isp_fd_drv_data *drv_data;
> +       struct fd_user_output *fd_output;
> +       u32 num = 0, i = 0;
> +
> +       dev_dbg(&fd_hw_dev->pdev->dev, "-E. %s.\n", __func__);
> +
> +       if ((uint64_t)fd_hw_dev->fd_base < VA_OFFSET) {
> +               dev_info(&fd_hw_dev->pdev->dev,
> +                        "-X. %s. fd_base_error: %p\n",
> +                        __func__, fd_hw_dev->fd_base);
> +               return -1;
> +       }
> +
> +       num = FD_RD32(fd_hw_dev->fd_base + FD_RESULT);
> +       FD_WR32(0x0, fd_hw_dev->fd_base + FD_INT_EN);
> +       fd_output = (struct fd_user_output *)fd_param->fd_user_result.va;
> +       fd_output->face_number = num;
> +
> +       drv_data = mtk_fd_hw_dev_to_drv(fd_hw_dev);
> +       if ((uint64_t)drv_data < VA_OFFSET) {
> +               dev_dbg(&fd_hw_dev->pdev->dev,
> +                       "-X. %s. fd_base_error\n", __func__);
> +               return -1;
> +       }
> +       mtk_fd_do_callback(fd_hw_dev, MTK_FD_CTX_FRAME_DATA_DONE);
> +
> +       for (i = 0; i < num; i++) {
> +               dev_dbg(&fd_hw_dev->pdev->dev,
> +                       "id:%d, typ:%d, x0:%d, y0:%d, x1:%d, y1:%d, fcv:0x%x\n",
> +                       fd_output->face[i].face_idx,
> +                       fd_output->face[i].type,
> +                       fd_output->face[i].x0,
> +                       fd_output->face[i].y0,
> +                       fd_output->face[i].x1,
> +                       fd_output->face[i].y1,
> +                       fd_output->face[i].fcv);
> +       }
> +       dev_dbg(&fd_hw_dev->pdev->dev, "-X. %s.\n", __func__);
> +       return 0;
> +}
> +
> +static int mtk_fd_manager_buf_init(struct mtk_fd_drv_dev *fd_hw_dev)
> +{
> +       struct fd_manager_ctx *manager_ctx;
> +
> +       manager_ctx = (struct fd_manager_ctx *)fd_hw_dev->fd_ctx.scp_mem.va;
> +       manager_ctx->rs_result = fd_hw_dev->fd_ctx.rs_result;
> +
> +       return 0;
> +}
> +
> +static int mtk_fd_alloc_rs_buf(struct mtk_fd_drv_dev *fd_hw_dev)
> +{
> +       u64 va = 0;
> +       dma_addr_t dma_handle = 0;
> +       u32 size = RS_BUF_SIZE_MAX;
> +
> +       va = (uint64_t)dma_alloc_coherent(&fd_hw_dev->pdev->dev, size,
> +       &dma_handle, GFP_KERNEL);
> +
> +       dev_dbg(&fd_hw_dev->pdev->dev, "rsbuffer size: %u\n", size);
> +       dev_dbg(&fd_hw_dev->pdev->dev, "va = 0x%llx, iova = 0x%x\n", va,
> +               dma_handle);
> +
> +       if (va == 0) {
> +               dev_err(&fd_hw_dev->pdev->dev, "dma_alloc null va!\n");
> +               return -1;
> +       }
> +
> +       memset((uint8_t *)va, 0, size);
> +
> +       fd_hw_dev->fd_ctx.rs_result.va = va;
> +       fd_hw_dev->fd_ctx.rs_result.iova = dma_handle;
> +
> +       return 0;
> +}
> +
> +static int mtk_fd_get_reserve_mem(struct mtk_fd_drv_dev *fd_hw_dev)
> +{
> +       phys_addr_t scp_mem_pa;
> +       u64 scp_mem_va;
> +       u32 scp_mem_iova;
> +       u32 size = 0, size_align = 0;
> +       struct sg_table *sgt;
> +       int n_pages = 0, i = 0, ret = 0;
> +       struct page **pages = NULL;
> +       struct mtk_fd_drv_ctx *fd_ctx;
> +       struct platform_device *pdev = fd_hw_dev->pdev;
> +
> +       fd_ctx = &fd_hw_dev->fd_ctx;
> +
> +       scp_mem_iova = 0;
> +       scp_mem_va = vpu_get_reserve_mem_virt(FD_MEM_ID);
> +       scp_mem_pa = vpu_get_reserve_mem_phys(FD_MEM_ID);
> +       size = (u32)vpu_get_reserve_mem_size(FD_MEM_ID);
> +
> +       dev_dbg(&pdev->dev, "fd_scp_mem va: 0x%llx, pa: 0x%llx, sz:0x%x\n",
> +               scp_mem_va, (u64)scp_mem_pa, size);
> +
> +       if (scp_mem_va != 0 && size > 0)
> +               memset((void *)scp_mem_va, 0, size);
> +
> +       /* get iova */
> +       sgt = &fd_ctx->sgtable;
> +       sg_alloc_table(sgt, 1, GFP_KERNEL);
> +
> +       size_align = round_up(size, PAGE_SIZE);
> +       n_pages = size_align >> PAGE_SHIFT;
> +
> +       pages = kmalloc_array(n_pages, sizeof(struct page *), GFP_KERNEL);
> +
> +       for (i = 0; i < n_pages; i++)
> +               pages[i] = phys_to_page(scp_mem_pa + i * PAGE_SIZE);
> +       ret = sg_alloc_table_from_pages(sgt, pages, n_pages,
> +                                       0, size_align, GFP_KERNEL);
> +
> +       if (ret) {
> +               dev_err(&pdev->dev, "failed to get allocate sg table\n");
> +               kfree(pages);
> +               return ret;
> +       }
> +
> +       dma_map_sg_attrs(&pdev->dev, sgt->sgl, sgt->nents,
> +                        DMA_BIDIRECTIONAL, DMA_ATTR_SKIP_CPU_SYNC);
> +       scp_mem_iova = sg_dma_address(sgt->sgl);
> +       kfree(pages);
> +
> +       dev_dbg(&fd_hw_dev->pdev->dev, "scpmem size:%u,0x%X\n", size, size);
> +       dev_dbg(&fd_hw_dev->pdev->dev, "pa:0x%08X\n", (u32)scp_mem_pa);
> +       dev_dbg(&fd_hw_dev->pdev->dev, "va:0x%16llX\n", (u64)scp_mem_va);
> +       dev_dbg(&fd_hw_dev->pdev->dev, "iova:0x%08X\n", (u32)scp_mem_iova);
> +
> +       fd_ctx->scp_mem.pa = scp_mem_pa;
> +       fd_ctx->scp_mem.va = scp_mem_va;
> +       fd_ctx->scp_mem.iova = scp_mem_iova;
> +
> +       return 0;
> +}
> +
> +static int mtk_fd_load_vpu(struct mtk_fd_drv_dev *fd_hw_dev)
> +{
> +       struct mtk_fd_drv_ctx *fd_ctx;
> +       int ret = 0;
> +
> +       fd_ctx = &fd_hw_dev->fd_ctx;
> +
> +       /* init vpu */
> +       fd_ctx->vpu_pdev = vpu_get_plat_device(fd_hw_dev->pdev);
> +
> +       if (!fd_ctx->vpu_pdev) {
> +               dev_err(&fd_hw_dev->pdev->dev,
> +                       "Failed to get VPU device\n");
> +               return -EINVAL;
> +       }
> +
> +       ret = vpu_load_firmware(fd_ctx->vpu_pdev);
> +       if (ret < 0) {
> +               /**
> +                * Return 0 if downloading firmware successfully,
> +                * otherwise it is failed
> +                */
> +               dev_err(&fd_hw_dev->pdev->dev,
> +                       "vpu_load_firmware failed!");
> +               return -EINVAL;
> +       }
> +       return ret;
> +}
> +
> +static int mtk_fd_open_context(struct mtk_fd_drv_dev *fd_hw_dev)
> +{
> +       struct mtk_fd_drv_ctx *fd_ctx;
> +
> +       fd_ctx = &fd_hw_dev->fd_ctx;
> +
> +       mtk_fd_load_vpu(fd_hw_dev);
> +
> +       mtk_fd_get_reserve_mem(fd_hw_dev);
> +       mtk_fd_alloc_rs_buf(fd_hw_dev);
> +       mtk_fd_manager_buf_init(fd_hw_dev);
> +
> +       mtk_fd_send_ipi_init(fd_ctx->vpu_pdev, &fd_ctx->scp_mem);
> +       return 0;
> +}
> +
> +static int mtk_fd_release_context(struct mtk_fd_drv_dev *fd_hw_dev)
> +{
> +       struct mtk_fd_drv_ctx *fd_ctx;
> +
> +       fd_ctx = &fd_hw_dev->fd_ctx;
> +
> +       atomic_set(&fd_ctx->fd_user_cnt, 0);
> +       atomic_set(&fd_ctx->fd_stream_cnt, 0);
> +       atomic_set(&fd_ctx->fd_enque_cnt, 0);
> +       sg_free_table(&fd_ctx->sgtable);
> +
> +       return 0;
> +}
> +
> +int mtk_fd_open(struct platform_device *pdev)
> +{
> +       int ret = 0;
> +       s32 usercount;
> +       struct mtk_fd_drv_dev *fd_hw_dev;
> +       struct mtk_isp_fd_drv_data *fd_drv;
> +       struct mtk_fd_drv_ctx *fd_ctx;
> +
> +       dev_dbg(&pdev->dev, "- E. %s.\n", __func__);
> +
> +       if (!pdev) {
> +               dev_err(&fd_hw_dev->pdev->dev, "platform device is NULL\n");
> +               return -EINVAL;
> +       }
> +
> +       fd_drv = dev_get_drvdata(&pdev->dev);
> +       fd_hw_dev = &fd_drv->fd_hw_dev;
> +       fd_ctx = &fd_hw_dev->fd_ctx;
> +       dev_dbg(&fd_hw_dev->pdev->dev, "open fd_hw_dev = 0x%p\n", fd_hw_dev);
> +       dev_dbg(&fd_hw_dev->pdev->dev, "open fd_drv = 0x%p\n", fd_drv);
> +
> +       usercount = atomic_inc_return(&fd_hw_dev->fd_ctx.fd_user_cnt);
> +
> +       if (usercount == 1) {
> +               /* Enable clock */
> +               pm_runtime_get_sync(&fd_hw_dev->pdev->dev);
> +
> +               mtk_fd_open_context(fd_hw_dev);
> +               fd_hw_dev->state = FD_INI;
> +               fd_hw_dev->streaming = STREAM_OFF;
> +       }
> +
> +       dev_dbg(&fd_hw_dev->pdev->dev, "usercount = %d",
> +               atomic_read(&fd_ctx->fd_user_cnt));
> +
> +       dev_dbg(&fd_hw_dev->pdev->dev, "X. %s\n", __func__);
> +
> +       return ret;
> +}
> +EXPORT_SYMBOL(mtk_fd_open);
> +
> +int mtk_fd_streamon(struct platform_device *pdev, u16 id)
> +{
> +       int ret = 0;
> +       struct mtk_fd_drv_dev *fd_hw_dev;
> +       struct mtk_isp_fd_drv_data *fd_drv;
> +
> +       fd_drv = dev_get_drvdata(&pdev->dev);
> +       fd_hw_dev = &fd_drv->fd_hw_dev;
> +
> +       dev_dbg(&pdev->dev, "- E. %s\n", __func__);
> +       fd_hw_dev->streaming = STREAM_ON;
> +       atomic_inc_return(&fd_hw_dev->fd_ctx.fd_stream_cnt);
> +
> +       dev_dbg(&pdev->dev, "- X. %s\n", __func__);
> +       return ret;
> +}
> +EXPORT_SYMBOL(mtk_fd_streamon);
> +
> +int mtk_fd_enqueue(struct platform_device *pdev,
> +                  struct v4l2_fd_param *fd_param)
> +{
> +       struct mtk_fd_drv_dev *fd_hw_dev;
> +       struct mtk_fd_drv_ctx *fd_ctx;
> +       struct mtk_isp_fd_drv_data *fd_drv;
> +
> +       dev_dbg(&pdev->dev, "- E. %s\n", __func__);
> +       fd_drv = dev_get_drvdata(&pdev->dev);
> +       fd_hw_dev = &fd_drv->fd_hw_dev;
> +       fd_ctx = &fd_hw_dev->fd_ctx;
> +
> +       fd_ctx->frame_id = fd_param->frame_id;
> +
> +       if (fd_hw_dev->streaming == STREAM_OFF || fd_hw_dev->state == FD_ENQ) {
> +               dev_err(&fd_hw_dev->pdev->dev, "enqueue before stream on!\n");
> +               return mtk_fd_do_callback(fd_hw_dev,
> +                                               MTK_FD_CTX_FRAME_DATA_ERROR);
> +       }
> +
> +       fd_hw_dev->state = FD_ENQ;
> +       atomic_inc_return(&fd_ctx->fd_enque_cnt);
> +
> +       if (mtk_fd_send_ipi_cmd(fd_ctx->vpu_pdev, fd_param))
> +               return -2;
> +
> +       if (mtk_fd_wait_irq(fd_hw_dev))
> +               return mtk_fd_do_callback(fd_hw_dev,
> +                                               MTK_FD_CTX_FRAME_DATA_ERROR);
> +
> +       mtk_fd_dequeue(fd_hw_dev, fd_param);
> +
> +       dev_dbg(&pdev->dev, "- E. %s\n", __func__);
> +       return 0;
> +}
> +EXPORT_SYMBOL(mtk_fd_enqueue);
> +
> +int mtk_fd_release(struct platform_device *pdev)
> +{
> +       int ret = 0;
> +       struct mtk_fd_drv_dev *fd_hw_dev;
> +       struct mtk_isp_fd_drv_data *fd_drv;
> +
> +       fd_drv = dev_get_drvdata(&pdev->dev);
> +       fd_hw_dev = &fd_drv->fd_hw_dev;
> +       dev_dbg(&fd_hw_dev->pdev->dev, "- E. %s\n", __func__);
> +
> +       if (!pdev) {
> +               dev_err(&fd_hw_dev->pdev->dev, "platform device is NULL\n");
> +               return -EINVAL;
> +       }
> +
> +       dev_info(&fd_hw_dev->pdev->dev, "release fd_hw_dev: 0x%p\n", fd_hw_dev);
> +
> +       if (atomic_dec_and_test(&fd_hw_dev->fd_ctx.fd_user_cnt)) {
> +               if (fd_hw_dev->state == FD_ENQ)
> +                       mtk_fd_wait_irq(fd_hw_dev);
> +
> +               mtk_fd_release_context(fd_hw_dev);
> +
> +               pm_runtime_put_sync(&fd_hw_dev->pdev->dev);
> +       }
> +       dev_info(&fd_hw_dev->pdev->dev, "usercount = %d\n",
> +                atomic_read(&fd_hw_dev->fd_ctx.fd_user_cnt));
> +
> +       dev_dbg(&fd_hw_dev->pdev->dev, "- X. %s\n", __func__);
> +       return ret;
> +}
> +EXPORT_SYMBOL(mtk_fd_release);
> +
> +int mtk_fd_streamoff(struct platform_device *pdev, u16 id)
> +{
> +       int ret = 0;
> +       struct mtk_fd_drv_dev *fd_hw_dev;
> +       struct mtk_isp_fd_drv_data *fd_drv;
> +
> +       fd_drv = dev_get_drvdata(&pdev->dev);
> +       fd_hw_dev = &fd_drv->fd_hw_dev;
> +
> +       dev_dbg(&pdev->dev, "- E. %s\n", __func__);
> +
> +       if (fd_hw_dev->state == FD_ENQ)
> +               mtk_fd_wait_irq(fd_hw_dev);
> +
> +       fd_hw_dev->streaming = STREAM_OFF;
> +       atomic_dec_return(&fd_hw_dev->fd_ctx.fd_stream_cnt);
> +
> +       dev_dbg(&pdev->dev, "- X. %s\n", __func__);
> +       return ret;
> +}
> +EXPORT_SYMBOL(mtk_fd_streamoff);
> +
> +static struct mtk_fd_ctx_desc mtk_isp_ctx_desc_fd = {
> +       "proc_device_fd", mtk_fd_ctx_fd_init,};
> +
> +static int mtk_fd_probe(struct platform_device *pdev)
> +{
> +       struct mtk_isp_fd_drv_data *fd_drv;
> +       struct mtk_fd_drv_dev *fd_hw_dev;
> +       struct mtk_fd_drv_ctx *fd_ctx;
> +       struct device_node *node;
> +       struct platform_device *larb_pdev;
> +       int irq_num = 0;
> +       int ret = 0;
> +
> +       dev_info(&pdev->dev, "E. %s.\n", __func__);
> +
> +       fd_drv = devm_kzalloc(&pdev->dev, sizeof(*fd_drv), GFP_KERNEL);
> +
> +       if (!fd_drv)
> +               return -ENOMEM;
> +
> +       dev_set_drvdata(&pdev->dev, fd_drv);
> +       fd_hw_dev = &fd_drv->fd_hw_dev;
> +
> +       if (!fd_hw_dev) {
> +               dev_err(&pdev->dev, "Unable to allocate fd_hw_dev\n");
> +               return -ENOMEM;
> +       }
> +
> +       dev_info(&pdev->dev, "open fd_hw_dev = 0x%p\n", fd_hw_dev);
> +       dev_info(&pdev->dev, "open fd_drv = 0x%p\n", fd_drv);
> +
> +       fd_hw_dev->pdev = pdev;
> +       fd_ctx = &fd_hw_dev->fd_ctx;
> +
> +       irq_num = irq_of_parse_and_map(pdev->dev.of_node, FD_IRQ_IDX);
> +       ret = request_irq(irq_num, (irq_handler_t)mtk_fd_irq,
> +                         IRQF_TRIGGER_NONE, FD_DRVNAME, fd_hw_dev);
> +       if (ret) {
> +               dev_info(&pdev->dev, "%s request_irq fail, irq=%d\n",
> +                        __func__, irq_num);
> +               return ret;
> +       }
> +       dev_info(&pdev->dev, "irq_num=%d\n", irq_num);
> +
> +       node = of_parse_phandle(pdev->dev.of_node, "mediatek,larb", 0);
> +       if (!node) {
> +               dev_err(&pdev->dev, "no mediatek, larb found");
> +               return -EINVAL;
> +       }
> +       larb_pdev = of_find_device_by_node(node);
> +       if (!larb_pdev) {
> +               dev_err(&pdev->dev, "no mediatek, larb device found");
> +               return -EINVAL;
> +       }
> +       fd_hw_dev->larb_dev = &larb_pdev->dev;
> +
> +       node = of_find_compatible_node(NULL, NULL, "mediatek,fd");
> +       if (!node) {
> +               dev_err(&pdev->dev, "find fd node failed!!!\n");
> +               return -ENODEV;
> +       }
> +       fd_hw_dev->fd_base = of_iomap(node, 0);
> +       if (!fd_hw_dev->fd_base) {
> +               dev_err(&pdev->dev, "unable to map fd node!!!\n");
> +               return -ENODEV;
> +       }
> +       dev_info(&pdev->dev, "fd_hw_dev->fd_base: %lx\n",
> +                (unsigned long)fd_hw_dev->fd_base);
> +
> +       /* CCF: Grab clock pointer (struct clk*) */
> +       fd_hw_dev->fd_clk = devm_clk_get(&pdev->dev, "FD_CLK_IMG_FD");
> +       if (IS_ERR(fd_hw_dev->fd_clk)) {
> +               dev_err(&pdev->dev, "cannot get FD_CLK_IMG_FD clock\n");
> +               return PTR_ERR(fd_hw_dev->fd_clk);
> +       }
> +
> +       pm_runtime_enable(&pdev->dev);
> +
> +       atomic_set(&fd_ctx->fd_user_cnt, 0);
> +       atomic_set(&fd_ctx->fd_stream_cnt, 0);
> +       atomic_set(&fd_ctx->fd_enque_cnt, 0);
> +
> +       init_waitqueue_head(&fd_hw_dev->wq);
> +
> +       fd_hw_dev->fd_irq_result = 0;
> +       fd_hw_dev->streaming = STREAM_OFF;
> +
> +       ret = mtk_fd_dev_core_init(pdev, &fd_drv->fd_dev, &mtk_isp_ctx_desc_fd);
> +
> +       if (ret)
> +               dev_err(&pdev->dev, "v4l2 init failed: %d\n", ret);
> +
> +       dev_info(&pdev->dev, "X. FD driver probe.\n");
> +
> +       return 0;
> +}
> +
> +static int mtk_fd_remove(struct platform_device *pdev)
> +{
> +       int i4IRQ = 0;
> +       struct mtk_fd_drv_dev *fd_hw_dev = NULL;
> +       struct mtk_isp_fd_drv_data *drv_data = dev_get_drvdata(&pdev->dev);
> +
> +       dev_info(&fd_hw_dev->pdev->dev, "-E. %s\n", __func__);
> +       if (drv_data) {
> +               mtk_fd_dev_core_release(pdev, &drv_data->fd_dev);
> +               fd_hw_dev = &drv_data->fd_hw_dev;
> +       } else {
> +               dev_err(&pdev->dev, "Can't find fd driver data\n");
> +               return -EINVAL;
> +       }
> +
> +       mtk_fd_clk_ctrl(fd_hw_dev, clock_off);
> +       pm_runtime_disable(&pdev->dev);
> +
> +       i4IRQ = platform_get_irq(pdev, 0);
> +       free_irq(i4IRQ, NULL);
> +       kfree(fd_hw_dev);
> +
> +       dev_info(&fd_hw_dev->pdev->dev, "-X. %s\n", __func__);
> +       return 0;
> +}
> +
> +static int mtk_fd_suspend(struct device *dev)
> +{
> +       struct mtk_isp_fd_drv_data *fd_drv;
> +       struct platform_device *pdev;
> +       struct mtk_fd_drv_dev *fd_hw_dev;
> +
> +       if (pm_runtime_suspended(dev))
> +               return 0;
> +
> +       fd_drv = dev_get_drvdata(dev);
> +       fd_hw_dev = &fd_drv->fd_hw_dev;
> +       pdev = fd_hw_dev->pdev;
> +
> +       dev_info(&pdev->dev, "E.%s\n", __func__);
> +
> +       if (atomic_read(&fd_hw_dev->fd_ctx.fd_user_cnt) > 0) {
> +               mtk_fd_clk_ctrl(fd_hw_dev, clock_off);
> +               dev_info(&pdev->dev, "Disable clock\n");
> +       }
> +
> +       dev_info(&pdev->dev, "X.%s\n", __func__);
> +       return 0;
> +}
> +
> +static int mtk_fd_resume(struct device *dev)
> +{
> +       struct mtk_isp_fd_drv_data *fd_drv;
> +       struct platform_device *pdev;
> +       struct mtk_fd_drv_dev *fd_hw_dev;
> +
> +       if (pm_runtime_suspended(dev))
> +               return 0;
> +
> +       fd_drv = dev_get_drvdata(dev);
> +       fd_hw_dev = &fd_drv->fd_hw_dev;
> +       pdev = fd_hw_dev->pdev;
> +
> +       dev_info(&pdev->dev, "E.%s\n", __func__);
> +
> +       if (atomic_read(&fd_hw_dev->fd_ctx.fd_user_cnt) > 0) {
> +               mtk_fd_clk_ctrl(fd_hw_dev, clock_on);
> +               dev_info(&pdev->dev, "Enable clock\n");
> +       }
> +
> +       dev_info(&pdev->dev, "X.%s\n", __func__);
> +       return 0;
> +}
> +
> +static const struct dev_pm_ops mtk_fd_pm_ops = {
> +       SET_SYSTEM_SLEEP_PM_OPS(mtk_fd_suspend, mtk_fd_resume)
> +       SET_RUNTIME_PM_OPS(mtk_fd_suspend, mtk_fd_resume, NULL)
> +};
> +
> +static struct platform_driver mtk_fd_driver = {
> +       .probe   = mtk_fd_probe,
> +       .remove  = mtk_fd_remove,
> +       .driver  = {
> +               .name  = FD_DRVNAME,
> +               .of_match_table = mtk_fd_of_ids,
> +               .pm = &mtk_fd_pm_ops,
> +       }
> +};
> +module_platform_driver(mtk_fd_driver);
> +
> +MODULE_DESCRIPTION("Mediatek FD driver");
> +MODULE_LICENSE("GPL");
> diff --git a/drivers/media/platform/mtk-isp/fd/mtk_fd.h b/drivers/media/platform/mtk-isp/fd/mtk_fd.h
> new file mode 100644
> index 0000000..6cae440
> --- /dev/null
> +++ b/drivers/media/platform/mtk-isp/fd/mtk_fd.h
> @@ -0,0 +1,127 @@
> +/* SPDX-License-Identifier: GPL-2.0
> + * Copyright (C) 2015 MediaTek Inc.
> + *
> + * This program is free software: you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License version 2 as
> + * published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful,
> + * but WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
> + * GNU General Public License for more details.
> + */
> +
> +#ifndef __MTK_FD_H__
> +#define __MTK_FD_H__
> +
> +#define MTK_FD_MAX_NO          (1024)
> +#define MAX_FACE_SEL_NUM       (MTK_FD_MAX_NO + 2)
> +
> +/* The max number of face sizes could be detected, for feature scaling */
> +#define FACE_SIZE_NUM_MAX      (14)
> +
> +/* FACE_SIZE_NUM_MAX + 1, first scale for input image W/H */
> +#define FD_SCALE_NUM           (15)
> +
> +/* Number of Learning data sets */
> +#define LEARNDATA_NUM          (18)
> +
> +struct fd_buffer {
> +       __u64 va;       /* used by APMCU access */
> +       __u32 pa;       /* used by CM4 access */
> +       __u32 iova;     /* used by HW access */
> +} __packed;
> +
> +enum fd_img_format {
> +       FMT_VYUY = 2,
> +       FMT_UYVY,
> +       FMT_YVYU,
> +       FMT_YUYV,
> +};
> +
> +enum fd_mode {
> +       FDMODE,
> +       SDMODE,
> +       VFB,
> +       CFB,
> +};
> +
> +struct fd_face_result {
> +       __u64 face_idx:12, type:1, x0:10, y0:10, x1:10, y1:10,
> +               fcv:18, rip_dir:4, rop_dir:3, det_size:5;
> +};
> +
> +struct fd_user_output {
> +       struct fd_face_result face[MAX_FACE_SEL_NUM];
> +       __u16 face_number;
> +};
> +
> +struct v4l2_fd_param {
> +       u32 frame_id;
> +       u16 src_img_h;
> +       u16 src_img_w;
> +       struct fd_buffer src_img;
> +       struct fd_buffer fd_user_param;
> +       struct fd_buffer fd_user_result;
> +} __packed;
> +
> +/**
> + * mtk_fd_enqueue - enqueue to fd driver
> + *
> + * @pdev: FD platform device
> + * @fdvtconfig: frame parameters from V4L2 common Framework
> + *
> + * Enqueue a frame to fd driver.
> + *
> + * Return: Return 0 if successfully, otherwise it is failed.
> + */
> +int mtk_fd_enqueue(struct platform_device *pdev,
> +                  struct v4l2_fd_param *fd_param);
> +
> +/**
> + * mtk_fd_open -
> + *
> + * @pdev: FD platform device
> + *
> + * Open the FD device driver
> + *
> + * Return: Return 0 if success, otherwise it is failed.
> + */
> +int mtk_fd_open(struct platform_device *pdev);
> +
> +/**
> + * mtk_fd_release -
> + *
> + * @pdev: FD platform device
> + *
> + * Enqueue a frame to FD driver.
> + *
> + * Return: Return 0 if success, otherwise it is failed.
> + */
> +int mtk_fd_release(struct platform_device *pdev);
> +
> +/**
> + * mtk_fd_streamon -
> + *
> + * @pdev: FD platform device
> + * @id: device context id
> + *
> + * Stream on
> + *
> + * Return: Return 0 if success, otherwise it is failed.
> + */
> +int mtk_fd_streamon(struct platform_device *pdev, u16 id);
> +
> +/**
> + * mtk_fd_streamoff -
> + *
> + * @pdev: FD platform device
> + * @id: device context id
> + *
> + * Stream off
> + *
> + * Return: Return 0 if success, otherwise it is failed.
> + */
> +int mtk_fd_streamoff(struct platform_device *pdev, u16 id);
> +
> +#endif/*__MTK_FD_H__*/
> --
> 1.9.1
>

Re: [RFC PATCH V0 0/7] media: platform: Add support for Face Detection (FD) on mt8183 SoC

[Hans Verkuil]

Hi Jerry-ch Chen,

On 2/20/19 8:48 AM, Jerry-ch Chen wrote:
> Hello,
> 
> This is the first version of the RFC patch series adding Face Detection
> (FD) driver on Mediatek mt8183 SoC, which will be used in camera features
> on CrOS application. It belongs to the first Mediatek's camera driver
> series based on V4L2 and media controller framework. I posted the main part
> of the FD driver as RFC to discuss first and would like some review
> comments on the overall structure of the driver.
> 
> Face Detection (FD) unit provide hardware accelerated face detection
> feature. It can detect different sizes of faces in a given image.
> Furthermore, it has the capability to detect the faces of Rotation-in-Plane
> from -180 to +180 degrees and Rotation-off-Plane from -90 to +90 degrees.
> 
> The driver is implemented with V4L2 and media controller framework. We have
> the following entities describing the FD path.

Just a high-level comment before you post the next version of this series:

Please compile the latest version of v4l2-compliance (part of
git://linuxtv.org/v4l-utils.git) and run it against your driver:

v4l2-compliance -m /dev/mediaX

Whenever you post a new version of this series, please do a 'git pull' of
the v4l-utils repo, recompile and retest with v4l2-compliance and post the
test results in the cover letter.

Obviously, there should be no FAILs and probably no warnings.

I suspect that streaming (e.g. adding the -s10 option to v4l2-compliance)
probably won't work since v4l2-compliance doesn't know about the meta data
formats.

Regards,

	Hans

> 
> 1. Meta input (output video device): connects to FD sub device. It accepts
>    the input parameter buffer from userspace. The metadata interface used
>    currently is only a temporary solution to kick off driver development
>    and is not ready for reviewed yet.
> 
> 2. RAW (output video device): connects to FD sub device. It accepts input
>    image buffer from userspace.
> 
> 3. FD (sub device): connects to Meta output. When processing an image,
>    FD hardware only returns the statistics of detected faces so it needs
>    only one capture video devices to return the streaming data to the user.
> 
> 4. Meta output (capture video device): Return the result of detected faces
>    as metadata output.
> 
>    The overall file structure of the FD driver is as following:
> 
> * mtk_fd-dev-ctx-core.c: Implements common software flow of FD driver.
> * mtk_fd-v4l2.c: Static FD contexts configuration.
> * mtk_fd.c: Controls the hardware flow.
> * mtk_fd-dev.c: Implements context-independent flow.
> * mtk_fd-ctrl.c: Handles the HW ctrl request from userspace.
> * mtk_fd-smem-drv.c: Provides the shared memory management required
> operation. We reserved a memory region for the co-processor and FD to
> exchange the hardware configuration data.
> * mtk_fd-v4l2-util.c: Implements V4L2 and vb2 ops.
> 
> Jerry-ch Chen (7):
>   dt-bindings: mt8183: Add binding for FD shared memory
>   dts: arm64: mt8183: Add FD shared memory node
>   dt-bindings: mt8183: Added FD-SMEM dt-bindings
>   dt-bindings: mt8183: Added FD dt-bindings
>   dts: arm64: mt8183: Add FD nodes
>   media: platform: Add Mediatek FD driver KConfig
>   platform: mtk-isp: Add Mediatek FD driver
> 
>  .../devicetree/bindings/media/mediatek,fd_smem.txt |   28 +
>  .../bindings/media/mediatek,mt8183-fd.txt          |   30 +
>  .../mediatek,reserve-memory-fd_smem.txt            |   44 +
>  arch/arm64/boot/dts/mediatek/mt8183.dtsi           |   28 +
>  drivers/media/platform/Kconfig                     |    2 +
>  drivers/media/platform/mtk-isp/Kconfig             |   10 +
>  drivers/media/platform/mtk-isp/Makefile            |   16 +
>  drivers/media/platform/mtk-isp/fd/Makefile         |   38 +
>  drivers/media/platform/mtk-isp/fd/mtk_fd-core.h    |  157 +++
>  drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h     |  299 ++++++
>  .../platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c      |  917 +++++++++++++++++
>  drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c     |  355 +++++++
>  drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h     |  198 ++++
>  .../media/platform/mtk-isp/fd/mtk_fd-smem-drv.c    |  452 +++++++++
>  drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h    |   25 +
>  .../media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c   | 1046 ++++++++++++++++++++
>  drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c    |  115 +++
>  drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h    |   36 +
>  drivers/media/platform/mtk-isp/fd/mtk_fd.c         |  730 ++++++++++++++
>  drivers/media/platform/mtk-isp/fd/mtk_fd.h         |  127 +++
>  20 files changed, 4653 insertions(+)
>  create mode 100644 Documentation/devicetree/bindings/media/mediatek,fd_smem.txt
>  create mode 100644 Documentation/devicetree/bindings/media/mediatek,mt8183-fd.txt
>  create mode 100644 Documentation/devicetree/bindings/reserved-memory/mediatek,reserve-memory-fd_smem.txt
>  create mode 100644 drivers/media/platform/mtk-isp/Kconfig
>  create mode 100644 drivers/media/platform/mtk-isp/Makefile
>  create mode 100644 drivers/media/platform/mtk-isp/fd/Makefile
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-core.h
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-smem-drv.c
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd.c
>  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd.h
> 

Re: [RFC PATCH V0 0/7] media: platform: Add support for Face Detection (FD) on mt8183 SoC

[Jerry-ch Chen]

Hi Hans, Tomasz,

On Thu, 2019-03-14 at 16:40 +0800, Hans Verkuil wrote:
> Hi Jerry-ch Chen,
> 
> On 2/20/19 8:48 AM, Jerry-ch Chen wrote:
> > Hello,
> > 
> > This is the first version of the RFC patch series adding Face Detection
> > (FD) driver on Mediatek mt8183 SoC, which will be used in camera features
> > on CrOS application. It belongs to the first Mediatek's camera driver
> > series based on V4L2 and media controller framework. I posted the main part
> > of the FD driver as RFC to discuss first and would like some review
> > comments on the overall structure of the driver.
> > 
> > Face Detection (FD) unit provide hardware accelerated face detection
> > feature. It can detect different sizes of faces in a given image.
> > Furthermore, it has the capability to detect the faces of Rotation-in-Plane
> > from -180 to +180 degrees and Rotation-off-Plane from -90 to +90 degrees.
> > 
> > The driver is implemented with V4L2 and media controller framework. We have
> > the following entities describing the FD path.
> 
> Just a high-level comment before you post the next version of this series:
> 
> Please compile the latest version of v4l2-compliance (part of
> git://linuxtv.org/v4l-utils.git) and run it against your driver:
> 
> v4l2-compliance -m /dev/mediaX
> 
> Whenever you post a new version of this series, please do a 'git pull' of
> the v4l-utils repo, recompile and retest with v4l2-compliance and post the
> test results in the cover letter.
> 
> Obviously, there should be no FAILs and probably no warnings.
> 
> I suspect that streaming (e.g. adding the -s10 option to v4l2-compliance)
> probably won't work since v4l2-compliance doesn't know about the meta data
> formats.
> 
> Regards,
> 
> 	Hans
> 

Thanks for comments,
I am reworking FD driver based on general comments of P1 and DIP driver.
After that, I will upload the RFC V1 patch with the results of
v4l2-compliance in the cover-letter.

Best Regards,

	Jerry

> > 
> > 1. Meta input (output video device): connects to FD sub device. It accepts
> >    the input parameter buffer from userspace. The metadata interface used
> >    currently is only a temporary solution to kick off driver development
> >    and is not ready for reviewed yet.
> > 
> > 2. RAW (output video device): connects to FD sub device. It accepts input
> >    image buffer from userspace.
> > 
> > 3. FD (sub device): connects to Meta output. When processing an image,
> >    FD hardware only returns the statistics of detected faces so it needs
> >    only one capture video devices to return the streaming data to the user.
> > 
> > 4. Meta output (capture video device): Return the result of detected faces
> >    as metadata output.
> > 
> >    The overall file structure of the FD driver is as following:
> > 
> > * mtk_fd-dev-ctx-core.c: Implements common software flow of FD driver.
> > * mtk_fd-v4l2.c: Static FD contexts configuration.
> > * mtk_fd.c: Controls the hardware flow.
> > * mtk_fd-dev.c: Implements context-independent flow.
> > * mtk_fd-ctrl.c: Handles the HW ctrl request from userspace.
> > * mtk_fd-smem-drv.c: Provides the shared memory management required
> > operation. We reserved a memory region for the co-processor and FD to
> > exchange the hardware configuration data.
> > * mtk_fd-v4l2-util.c: Implements V4L2 and vb2 ops.
> > 
> > Jerry-ch Chen (7):
> >   dt-bindings: mt8183: Add binding for FD shared memory
> >   dts: arm64: mt8183: Add FD shared memory node
> >   dt-bindings: mt8183: Added FD-SMEM dt-bindings
> >   dt-bindings: mt8183: Added FD dt-bindings
> >   dts: arm64: mt8183: Add FD nodes
> >   media: platform: Add Mediatek FD driver KConfig
> >   platform: mtk-isp: Add Mediatek FD driver
> > 
> >  .../devicetree/bindings/media/mediatek,fd_smem.txt |   28 +
> >  .../bindings/media/mediatek,mt8183-fd.txt          |   30 +
> >  .../mediatek,reserve-memory-fd_smem.txt            |   44 +
> >  arch/arm64/boot/dts/mediatek/mt8183.dtsi           |   28 +
> >  drivers/media/platform/Kconfig                     |    2 +
> >  drivers/media/platform/mtk-isp/Kconfig             |   10 +
> >  drivers/media/platform/mtk-isp/Makefile            |   16 +
> >  drivers/media/platform/mtk-isp/fd/Makefile         |   38 +
> >  drivers/media/platform/mtk-isp/fd/mtk_fd-core.h    |  157 +++
> >  drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h     |  299 ++++++
> >  .../platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c      |  917 +++++++++++++++++
> >  drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c     |  355 +++++++
> >  drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h     |  198 ++++
> >  .../media/platform/mtk-isp/fd/mtk_fd-smem-drv.c    |  452 +++++++++
> >  drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h    |   25 +
> >  .../media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c   | 1046 ++++++++++++++++++++
> >  drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c    |  115 +++
> >  drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h    |   36 +
> >  drivers/media/platform/mtk-isp/fd/mtk_fd.c         |  730 ++++++++++++++
> >  drivers/media/platform/mtk-isp/fd/mtk_fd.h         |  127 +++
> >  20 files changed, 4653 insertions(+)
> >  create mode 100644 Documentation/devicetree/bindings/media/mediatek,fd_smem.txt
> >  create mode 100644 Documentation/devicetree/bindings/media/mediatek,mt8183-fd.txt
> >  create mode 100644 Documentation/devicetree/bindings/reserved-memory/mediatek,reserve-memory-fd_smem.txt
> >  create mode 100644 drivers/media/platform/mtk-isp/Kconfig
> >  create mode 100644 drivers/media/platform/mtk-isp/Makefile
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/Makefile
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-core.h
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-ctx.h
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev-ctx-core.c
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.c
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-dev.h
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-smem-drv.c
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-smem.h
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2-util.c
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.c
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd-v4l2.h
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd.c
> >  create mode 100644 drivers/media/platform/mtk-isp/fd/mtk_fd.h
> > 
> 

Re: [RFC PATCH V0 5/7] dts: arm64: mt8183: Add FD nodes

[Rob Herring]

On Wed, Feb 20, 2019 at 03:48:11PM +0800, Jerry-ch Chen wrote:
> This patch adds nodes for Face Detection (FD) unit. FD is embedded
> in Mediatek SoCs and works with the co-processor to perform face
> detection on the input data and image and output detected face result.
> 
> Signed-off-by: Jerry-ch Chen <jerry-ch.chen@mediatek.com>
> ---
>  arch/arm64/boot/dts/mediatek/mt8183.dtsi | 20 ++++++++++++++++++++
>  1 file changed, 20 insertions(+)
> 
> diff --git a/arch/arm64/boot/dts/mediatek/mt8183.dtsi b/arch/arm64/boot/dts/mediatek/mt8183.dtsi
> index b3d8dfd..45c7e2f 100644
> --- a/arch/arm64/boot/dts/mediatek/mt8183.dtsi
> +++ b/arch/arm64/boot/dts/mediatek/mt8183.dtsi
> @@ -440,6 +440,26 @@
>  			#clock-cells = <1>;
>  		};
>  
> +		fd_smem: fd_smem {
> +			compatible = "mediatek,fd_smem";
> +			mediatek,larb = <&larb5>;
> +			iommus = <&iommu M4U_PORT_CAM_IMGI>;

This doesn't look like an actual h/w device...

> +		};
> +
> +		fd:fd@1502b000 {

space after the :  ^
> +			compatible = "mediatek,fd";

Should be SoC specific.

> +			mediatek,larb = <&larb5>;
> +			mediatek,vpu = <&vpu>;
> +			iommus = <&iommu M4U_PORT_CAM_FDVT_RP>,
> +				 <&iommu M4U_PORT_CAM_FDVT_WR>,
> +				 <&iommu M4U_PORT_CAM_FDVT_RB>;
> +			reg = <0 0x1502b000 0 0x1000>;
> +			interrupts = <GIC_SPI 269 IRQ_TYPE_LEVEL_LOW>;
> +			clocks = <&imgsys CLK_IMG_FDVT>;
> +			clock-names = "FD_CLK_IMG_FD";
> +			smem_device = <&fd_smem>;
> +		};
> +
>  		vdecsys: syscon@16000000 {
>  			compatible = "mediatek,mt8183-vdecsys", "syscon";
>  			reg = <0 0x16000000 0 0x1000>;
> -- 
> 1.9.1
> 

Re: [RFC PATCH V0 5/7] dts: arm64: mt8183: Add FD nodes

[Jerry-ch Chen]

On Tue, 2019-03-26 at 05:57 +0800, Rob Herring wrote:
> On Wed, Feb 20, 2019 at 03:48:11PM +0800, Jerry-ch Chen wrote:
> > This patch adds nodes for Face Detection (FD) unit. FD is embedded
> > in Mediatek SoCs and works with the co-processor to perform face
> > detection on the input data and image and output detected face result.
> > 
> > Signed-off-by: Jerry-ch Chen <jerry-ch.chen@mediatek.com>
> > ---
> >  arch/arm64/boot/dts/mediatek/mt8183.dtsi | 20 ++++++++++++++++++++
> >  1 file changed, 20 insertions(+)
> > 
> > diff --git a/arch/arm64/boot/dts/mediatek/mt8183.dtsi b/arch/arm64/boot/dts/mediatek/mt8183.dtsi
> > index b3d8dfd..45c7e2f 100644
> > --- a/arch/arm64/boot/dts/mediatek/mt8183.dtsi
> > +++ b/arch/arm64/boot/dts/mediatek/mt8183.dtsi
> > @@ -440,6 +440,26 @@
> >  			#clock-cells = <1>;
> >  		};
> >  
> > +		fd_smem: fd_smem {
> > +			compatible = "mediatek,fd_smem";
> > +			mediatek,larb = <&larb5>;
> > +			iommus = <&iommu M4U_PORT_CAM_IMGI>;
> 
> This doesn't look like an actual h/w device...
> 
Thank you for your comments.

fd_smem is not an actual h/w device and it will not be used in the
future. We are going to remove the fd smem driver and fd_smem node.

> > +		};
> > +
> > +		fd:fd@1502b000 {
> 
> space after the :  ^
will be fixed.

> > +			compatible = "mediatek,fd";
> 
> Should be SoC specific.
> 
will be fixed as "mediatek, mt8183-fd".

Thanks and Best Regards
Jerry
> > +			mediatek,larb = <&larb5>;
> > +			mediatek,vpu = <&vpu>;
> > +			iommus = <&iommu M4U_PORT_CAM_FDVT_RP>,
> > +				 <&iommu M4U_PORT_CAM_FDVT_WR>,
> > +				 <&iommu M4U_PORT_CAM_FDVT_RB>;
> > +			reg = <0 0x1502b000 0 0x1000>;
> > +			interrupts = <GIC_SPI 269 IRQ_TYPE_LEVEL_LOW>;
> > +			clocks = <&imgsys CLK_IMG_FDVT>;
> > +			clock-names = "FD_CLK_IMG_FD";
> > +			smem_device = <&fd_smem>;
> > +		};
> > +
> >  		vdecsys: syscon@16000000 {
> >  			compatible = "mediatek,mt8183-vdecsys", "syscon";
> >  			reg = <0 0x16000000 0 0x1000>;
> > -- 
> > 1.9.1
> >