From b848d8f4139e7c8cae1516c4300abdbd4e49cbd5 Mon Sep 17 00:00:00 2001
From: b19715 <b19715@freescale.com>
Date: Fri, 11 Jun 2021 17:13:50 +0800
Subject: [PATCH 2/3] Add ISL79987 support for MEK board. Support weaving mode
 de-interlace.

---
 arch/arm64/boot/dts/freescale/Makefile             |   1 +
 .../boot/dts/freescale/imx8qxp-mek-isl7998x.dts    | 132 +++
 drivers/staging/media/imx/Kconfig                  |   7 +
 drivers/staging/media/imx/Makefile                 |   1 +
 drivers/staging/media/imx/isl7998x.c               | 938 +++++++++++++++++++++
 5 files changed, 1079 insertions(+)
 create mode 100644 arch/arm64/boot/dts/freescale/imx8qxp-mek-isl7998x.dts
 create mode 100644 drivers/staging/media/imx/isl7998x.c

diff --git a/arch/arm64/boot/dts/freescale/Makefile b/arch/arm64/boot/dts/freescale/Makefile
index 27d3bbd..3c2d8fa 100644
--- a/arch/arm64/boot/dts/freescale/Makefile
+++ b/arch/arm64/boot/dts/freescale/Makefile
@@ -129,6 +129,7 @@ dtb-$(CONFIG_ARCH_MXC) += imx8qxp-mek.dtb imx8qxp-mek-dsp.dtb imx8qxp-mek-ov5640
 			  imx8qxp-mek-dpu-lcdif-rpmsg.dtb \
 			  imx8qxp-mek-ov5640-rpmsg.dtb \
 			  imx8qxp-mek-pcie-ep.dtb \
+			  imx8qxp-mek-isl7998x.dtb \
 			  imx8qm-mek-pcie-ep.dtb \
 			  imx8dx-mek.dtb imx8dx-mek-dsp.dtb imx8dx-mek-rpmsg.dtb \
 			  imx8dx-mek-enet2-tja1100.dtb \
diff --git a/arch/arm64/boot/dts/freescale/imx8qxp-mek-isl7998x.dts b/arch/arm64/boot/dts/freescale/imx8qxp-mek-isl7998x.dts
new file mode 100644
index 00000000..4c58b97
--- /dev/null
+++ b/arch/arm64/boot/dts/freescale/imx8qxp-mek-isl7998x.dts
@@ -0,0 +1,132 @@
+/*
+ * Copyright 2019 NXP
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * 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 "imx8qxp-mek.dts"
+
+&iomuxc {
+	pinctrl_isl7998x0: isl7998x0grp{
+		fsl,pins = <
+			IMX8QXP_MIPI_CSI0_GPIO0_01_LSIO_GPIO3_IO07		0xC0000041
+			IMX8QXP_MIPI_CSI0_GPIO0_00_LSIO_GPIO3_IO08		0xC0000041
+		>;
+	};
+};
+
+&cameradev {
+	/delete-property/parallel_csi;
+	status = "okay";
+};
+
+&parallel_csi {
+	status = "disabled";
+};
+
+&isi_0 {
+	interface = <2 0 2>;
+	deinterlace_mode = <2>;  /* 0 ~ 1: No de-interlacing,
+				    2: Odd even weave, 3: Even odd weave
+				    4: Odd even blending, 5: Even odd blending
+				    6 ~ 7: Line doubling */
+	ntsc_top = <0>;		/* 0 for ISL79987. */
+	status = "okay";
+};
+
+&isi_1 {
+	interface = <2 1 2>;
+	deinterlace_mode = <2>;  /* 0 ~ 1: No de-interlacing,
+				    2: Odd even weave, 3: Even odd weave
+				    4: Odd even blending, 5: Even odd blending
+				    6 ~ 7: Line doubling */
+	ntsc_top = <0>;		/* 0 for ISL79987. */
+	status = "okay";
+};
+
+&isi_2 {
+	interface = <2 2 2>;
+	deinterlace_mode = <2>;  /* 0 ~ 1: No de-interlacing,
+				    2: Odd even weave, 3: Even odd weave
+				    4: Odd even blending, 5: Even odd blending
+				    6 ~ 7: Line doubling */
+	ntsc_top = <0>;		/* 0 for ISL79987. */
+	status = "okay";
+};
+
+&isi_3 {
+	interface = <2 3 2>;
+	deinterlace_mode = <2>;  /* 0 ~ 1: No de-interlacing,
+				    2: Odd even weave, 3: Even odd weave
+				    4: Odd even blending, 5: Even odd blending
+				    6 ~ 7: Line doubling */
+	ntsc_top = <0>;		/* 0 for ISL79987. */
+	status = "okay";
+};
+
+&isi_4 {
+	status = "disabled";
+};
+
+&isi_5 {
+	status = "disabled";
+};
+
+&isi_6 {
+	status = "disabled";
+};
+
+&isi_7 {
+	status = "disabled";
+};
+
+&mipi_csi_0 {
+	virtual-channel;
+	status = "okay";
+
+	/delete-node/port@0;
+
+	port@0 {
+		reg = <0>;
+		mipi_csi0_ep: endpoint {
+			remote-endpoint = <&isl7998x_mipi_ep>;
+			data-lanes = <1 2>;
+		};
+	};
+};
+
+&i2c_mipi_csi0 {
+	clock-frequency = <100000>;
+	status = "okay";
+
+	isl7998x: isl7998x@44 {
+		compatible = "intersil,isl7998x";
+		reg = <0x44>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_isl7998x0>;
+		clocks = <&clk_dummy>;
+		clock-names = "capture_mclk";
+		mclk = <27000000>;
+		mclk_source = <0>;
+		pwn-gpios = <&lsio_gpio3 7 GPIO_ACTIVE_LOW>;
+		rst-gpios = <&lsio_gpio3 8 GPIO_ACTIVE_HIGH>;
+		virtual-channel;
+		status = "okay";
+		port {
+			isl7998x_mipi_ep: endpoint {
+				remote-endpoint = <&mipi_csi0_ep>;
+			};
+		};
+	};
+
+	/delete-node/max9286_mipi@6a;
+};
+
diff --git a/drivers/staging/media/imx/Kconfig b/drivers/staging/media/imx/Kconfig
index be3e854..dac9453 100644
--- a/drivers/staging/media/imx/Kconfig
+++ b/drivers/staging/media/imx/Kconfig
@@ -113,5 +113,12 @@ config GMSL_MAX9286
 	help
 	  Enable support for video4linux camera sensor driver for GMSL MAX9286
 
+config TVIN_ISL7998X
+	tristate "ISL7998x TVin"
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	default y
+	---help---
+	  Enable support for video4linux TVin driver for ISL7998x.
+
 endmenu
 endif  #VIDEO_IMX_CAPTURE
diff --git a/drivers/staging/media/imx/Makefile b/drivers/staging/media/imx/Makefile
index 90173fd..fd9bbbc 100644
--- a/drivers/staging/media/imx/Makefile
+++ b/drivers/staging/media/imx/Makefile
@@ -27,3 +27,4 @@ obj-$(CONFIG_IMX8_MIPI_CSI2) 	  += imx8-mipi-csi2.o
 obj-$(CONFIG_IMX8_MIPI_CSI2_SAM)  += imx8-mipi-csi2-sam.o
 obj-$(CONFIG_IMX8_PARALLEL_CSI)	  += imx8-parallel-csi.o
 obj-$(CONFIG_GMSL_MAX9286)	  += gmsl-max9286.o
+obj-$(CONFIG_TVIN_ISL7998X) += isl7998x.o
diff --git a/drivers/staging/media/imx/isl7998x.c b/drivers/staging/media/imx/isl7998x.c
new file mode 100644
index 00000000..2805e99
--- /dev/null
+++ b/drivers/staging/media/imx/isl7998x.c
@@ -0,0 +1,938 @@
+/*
+ * Copyright 2019 NXP
+ */
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/of_device.h>
+#include <linux/i2c.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/of_gpio.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-subdev.h>
+
+
+#define CHIP_ID_79985		0x85
+#define CHIP_ID_79987		0x87
+
+#define MIPI_CSI2_SENS_VC0_PAD_SOURCE	0
+#define MIPI_CSI2_SENS_VC1_PAD_SOURCE	1
+#define MIPI_CSI2_SENS_VC2_PAD_SOURCE	2
+#define MIPI_CSI2_SENS_VC3_PAD_SOURCE	3
+#define MIPI_CSI2_SENS_VCX_PADS_NUM		4
+
+#define MAX_FPS		30
+#define MIN_FPS		25
+#define DEFAULT_FPS	30
+
+/* List of input video formats supported. The video formats is corresponding
+ * with v4l2 id in video_fmt
+ */
+typedef enum {
+	NTSC = 0,	/* Locked on (M) NTSC video signal. */
+	PAL,		/* (B, G, H, I, N)PAL video signal. */
+} video_fmt_idx;
+
+/*! Video format structure. */
+typedef struct {
+	int v4l2_id;		/*!< Video for linux ID. */
+	char name[16];		/*!< Name (e.g., "NTSC", "PAL", etc.) */
+	u16 width;		/*!< Active width. */
+	u16 height;		/*!< Active height. */
+	int frame_rate;		/*!< Frame rate. */
+} video_fmt_t;
+
+/*! Description of video formats supported.
+ *
+ *  PAL: raw=720x625, active=720x576.
+ *  NTSC: raw=720x525, active=720x480.
+ */
+static video_fmt_t video_fmts[] = {
+	{			/*! NTSC */
+	 .v4l2_id = V4L2_STD_NTSC,
+	 .name = "NTSC",
+	 .width = 720,	/* ACT_FRM_WIDTH plus 1 */
+	 .height = 480,	/* ACT_FRM_WIDTH plus 1 */
+	 .frame_rate = 30,
+	 },
+	{			/*! (B, G, H, I, N) PAL */
+	 .v4l2_id = V4L2_STD_PAL,
+	 .name = "PAL",
+	 .width = 720,
+	 .height = 576,
+	 .frame_rate = 25,
+	 },
+};
+
+struct sensor_data {
+	struct v4l2_subdev	subdev;
+	struct media_pad pads[MIPI_CSI2_SENS_VCX_PADS_NUM];
+	struct i2c_client *i2c_client;
+	struct v4l2_mbus_framefmt format;
+	struct v4l2_captureparm streamcap;
+	char running;
+
+	u32 mclk;
+	u8 mclk_source;
+	struct clk *sensor_clk;
+	int v_channel;
+	bool is_mipi;
+	unsigned int chip_id;
+	int num_lanes;
+	video_fmt_idx video_id;
+	unsigned short mipi_bps;
+
+	int rst_gpio;
+	int pwn_gpio;
+};
+
+
+static inline struct sensor_data *subdev_to_sensor_data(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct sensor_data, subdev);
+}
+
+/*! Read one register from a ISL7998x i2c slave device.
+ *
+ *  @param *reg		register in the device we wish to access.
+ *
+ *  @return		       0 if success, an error code otherwise.
+ */
+static inline int isl7998x_read_reg(struct sensor_data *isl7998x_data, u8 reg)
+{
+	int val;
+
+	val = i2c_smbus_read_byte_data(isl7998x_data->i2c_client, reg);
+	if (val < 0) {
+		dev_info(&isl7998x_data->i2c_client->dev,
+			"%s:read reg error: reg=%2x\n", __func__, reg);
+		return -1;
+	}
+	return val;
+}
+
+/*! Write one register of a ISL7998x i2c slave device.
+ *
+ *  @param *reg		register in the device we wish to access.
+ *
+ *  @return		       0 if success, an error code otherwise.
+ */
+static int isl7998x_write_reg(struct sensor_data *isl7998x_data, u8 reg, u8 val)
+{
+	s32 ret;
+
+	ret = i2c_smbus_write_byte_data(isl7998x_data->i2c_client, reg, val);
+
+	dev_dbg(&isl7998x_data->i2c_client->dev,
+		"%s:  addr %02x reg %02x val %02x\n",
+		__func__, isl7998x_data->i2c_client->addr, reg, val);
+
+	if (ret < 0) {
+		dev_info(&isl7998x_data->i2c_client->dev,
+			"%s:write reg error:reg=%2x,val=%2x\n", __func__,
+			reg, val);
+		return -1;
+	}
+	return 0;
+}
+
+static void isl7998x_hw_reset(struct sensor_data *isl7998x_data)
+{
+	gpio_set_value(isl7998x_data->rst_gpio, 0);
+	gpio_set_value(isl7998x_data->pwn_gpio, 1);
+	msleep(1);
+	gpio_set_value(isl7998x_data->rst_gpio, 1);
+	msleep(1);
+}
+
+static int isl7998x_hardware_init(struct sensor_data *isl7998x_data)
+{
+	int retval = 0;
+	u8 data = 0;
+
+	dev_info(&isl7998x_data->i2c_client->dev, "In %s()\n", __func__);
+
+	// Init the isl7998x
+	if (isl7998x_data->chip_id == CHIP_ID_79985) {
+		// Page 0
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x03, 0x00);
+		if (isl7998x_data->num_lanes == 2)
+			isl7998x_write_reg(isl7998x_data, 0x0B, 0x41);
+		else
+			isl7998x_write_reg(isl7998x_data, 0x0B, 0x40);
+		isl7998x_write_reg(isl7998x_data, 0x0D, 0xC9);
+		isl7998x_write_reg(isl7998x_data, 0x0E, 0xC9);
+		isl7998x_write_reg(isl7998x_data, 0x10, 0x01);
+		isl7998x_write_reg(isl7998x_data, 0x11, 0x03);
+		isl7998x_write_reg(isl7998x_data, 0x12, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x13, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x14, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x00);
+
+		// Page 1
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x01);
+		isl7998x_write_reg(isl7998x_data, 0x0D, 0x40);
+		isl7998x_write_reg(isl7998x_data, 0x2F, 0xE6);
+		isl7998x_write_reg(isl7998x_data, 0x33, 0x85);
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x01);
+
+		// Page 2
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x02);
+		isl7998x_write_reg(isl7998x_data, 0x0D, 0x40);
+		isl7998x_write_reg(isl7998x_data, 0x2F, 0xE6);
+		isl7998x_write_reg(isl7998x_data, 0x33, 0x85);
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x02);
+
+		// Page 3
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x03);
+		isl7998x_write_reg(isl7998x_data, 0x0D, 0x40);
+		isl7998x_write_reg(isl7998x_data, 0x2F, 0xE6);
+		isl7998x_write_reg(isl7998x_data, 0x33, 0x85);
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x03);
+
+		// Page 4
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x04);
+		isl7998x_write_reg(isl7998x_data, 0x0D, 0x40);
+		isl7998x_write_reg(isl7998x_data, 0x2F, 0xE6);
+		isl7998x_write_reg(isl7998x_data, 0x33, 0x85);
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x04);
+
+		// Page 5
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x05);
+		isl7998x_write_reg(isl7998x_data, 0x01, 0x85);
+		isl7998x_write_reg(isl7998x_data, 0x02, 0xA0);
+		isl7998x_write_reg(isl7998x_data, 0x03, 0x08);
+		isl7998x_write_reg(isl7998x_data, 0x04, 0xE4);
+		isl7998x_write_reg(isl7998x_data, 0x05, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x06, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x07, 0x46);
+		isl7998x_write_reg(isl7998x_data, 0x08, 0x02);
+		isl7998x_write_reg(isl7998x_data, 0x09, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x0A, 0x68);
+		isl7998x_write_reg(isl7998x_data, 0x0B, 0x02);
+		isl7998x_write_reg(isl7998x_data, 0x0C, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x0D, 0x06);
+		isl7998x_write_reg(isl7998x_data, 0x0E, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x0F, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x10, 0x05);
+		isl7998x_write_reg(isl7998x_data, 0x11, 0xA0);
+		isl7998x_write_reg(isl7998x_data, 0x12, 0x76);
+		isl7998x_write_reg(isl7998x_data, 0x13, 0x2F);
+		isl7998x_write_reg(isl7998x_data, 0x14, 0x0E);
+		isl7998x_write_reg(isl7998x_data, 0x15, 0x36);
+		isl7998x_write_reg(isl7998x_data, 0x16, 0x12);
+		isl7998x_write_reg(isl7998x_data, 0x17, 0xF6);
+		isl7998x_write_reg(isl7998x_data, 0x18, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x19, 0x17);
+		isl7998x_write_reg(isl7998x_data, 0x1A, 0x0A);
+		isl7998x_write_reg(isl7998x_data, 0x1B, 0x61);
+		isl7998x_write_reg(isl7998x_data, 0x1C, 0x7A);
+		isl7998x_write_reg(isl7998x_data, 0x1D, 0x0F);
+		isl7998x_write_reg(isl7998x_data, 0x1E, 0x8C);
+		isl7998x_write_reg(isl7998x_data, 0x1F, 0x02);
+		isl7998x_write_reg(isl7998x_data, 0x20, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x21, 0x0C);
+		isl7998x_write_reg(isl7998x_data, 0x22, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x23, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x24, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x25, 0xF0);
+		isl7998x_write_reg(isl7998x_data, 0x26, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x27, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x28, 0x01);
+		isl7998x_write_reg(isl7998x_data, 0x29, 0x0E);
+		isl7998x_write_reg(isl7998x_data, 0x2A, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x2B, 0x19);
+		isl7998x_write_reg(isl7998x_data, 0x2C, 0x18);
+		isl7998x_write_reg(isl7998x_data, 0x2D, 0xF1);
+		isl7998x_write_reg(isl7998x_data, 0x2E, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x2F, 0xF1);
+		isl7998x_write_reg(isl7998x_data, 0x30, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x31, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x32, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x33, 0xC0);
+		isl7998x_write_reg(isl7998x_data, 0x34, 0x18);
+		isl7998x_write_reg(isl7998x_data, 0x35, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x36, 0x00);
+		if (isl7998x_data->num_lanes == 2)
+			isl7998x_write_reg(isl7998x_data, 0x00, 0x02);
+		else
+			isl7998x_write_reg(isl7998x_data, 0x00, 0x01);
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x05);
+	} else if (isl7998x_data->chip_id == CHIP_ID_79987) {
+		// Page 5
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x05);
+		data = isl7998x_read_reg(isl7998x_data, 0x00);
+		data &= ~0x80;
+		isl7998x_write_reg(isl7998x_data, 0x00, data);	/* clear PowerDown */
+
+		// Page 0
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x02, 0x1F);  /* set Reset */
+
+		// Default setting
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x03, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x0D, 0xC9);
+		isl7998x_write_reg(isl7998x_data, 0x0E, 0xC9);
+		isl7998x_write_reg(isl7998x_data, 0x10, 0x01);
+		isl7998x_write_reg(isl7998x_data, 0x11, 0x03);
+		isl7998x_write_reg(isl7998x_data, 0x12, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x13, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x14, 0x00);
+
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x05);
+		isl7998x_write_reg(isl7998x_data, 0x00, 0x02);
+		isl7998x_write_reg(isl7998x_data, 0x01, 0x85);
+		isl7998x_write_reg(isl7998x_data, 0x02, 0xA0);
+		isl7998x_write_reg(isl7998x_data, 0x03, 0x18);
+		isl7998x_write_reg(isl7998x_data, 0x05, 0x40);
+		isl7998x_write_reg(isl7998x_data, 0x06, 0x40);
+		isl7998x_write_reg(isl7998x_data, 0x10, 0x05);
+		isl7998x_write_reg(isl7998x_data, 0x11, 0xA0);
+		isl7998x_write_reg(isl7998x_data, 0x20, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x21, 0x0C);
+		isl7998x_write_reg(isl7998x_data, 0x22, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x23, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x24, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x25, 0xF0);
+		isl7998x_write_reg(isl7998x_data, 0x26, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x27, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x2A, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x2B, 0x19);
+		isl7998x_write_reg(isl7998x_data, 0x2C, 0x18);
+		isl7998x_write_reg(isl7998x_data, 0x2D, 0xF1);
+		isl7998x_write_reg(isl7998x_data, 0x2E, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x2F, 0xF1);
+		isl7998x_write_reg(isl7998x_data, 0x30, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x31, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x32, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x33, 0xC0);
+		isl7998x_write_reg(isl7998x_data, 0x34, 0x18);
+		isl7998x_write_reg(isl7998x_data, 0x36, 0x00);
+
+		// Page 0
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x02, 0x10);  /* clear CReset_CH1 ~ Reset_CH4 */
+
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x0F);
+		isl7998x_write_reg(isl7998x_data, 0x08, 0x14);
+		isl7998x_write_reg(isl7998x_data, 0x2F, 0xE6);
+		isl7998x_write_reg(isl7998x_data, 0x33, 0x85);
+		isl7998x_write_reg(isl7998x_data, 0x45, 0x11);
+		isl7998x_write_reg(isl7998x_data, 0xE7, 0x00);
+
+		// Page 0
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x07, 0x02);
+		isl7998x_write_reg(isl7998x_data, 0x08, 0x1F);
+		isl7998x_write_reg(isl7998x_data, 0x09, 0x43);
+		isl7998x_write_reg(isl7998x_data, 0x0A, 0x4F);
+		if (isl7998x_data->num_lanes == 2)
+			isl7998x_write_reg(isl7998x_data, 0x0B, 0x41);
+		else
+			isl7998x_write_reg(isl7998x_data, 0x0B, 0x40);
+/*
+		// Page 1
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x01);
+		isl7998x_write_reg(isl7998x_data, 0x0D, 0x40);
+
+		// Page 2
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x02);
+		isl7998x_write_reg(isl7998x_data, 0x0D, 0x40);
+
+		// Page 3
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x03);
+		isl7998x_write_reg(isl7998x_data, 0x0D, 0x40);
+
+		// Page 4
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x04);
+		isl7998x_write_reg(isl7998x_data, 0x0D, 0x40);
+*/
+/*
+		// Page 1
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x01);
+		isl7998x_write_reg(isl7998x_data, 0x3D, 0x09);
+
+		// Page 2
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x02);
+		isl7998x_write_reg(isl7998x_data, 0x3D, 0x09);
+
+		// Page 3
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x03);
+		isl7998x_write_reg(isl7998x_data, 0x3D, 0x09);
+
+		// Page 4
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x04);
+		isl7998x_write_reg(isl7998x_data, 0x3D, 0x09);
+*/
+		// Page 5
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x05);
+		if (isl7998x_data->num_lanes == 2)
+			isl7998x_write_reg(isl7998x_data, 0x00, 0x02);
+		else
+			isl7998x_write_reg(isl7998x_data, 0x00, 0x01);
+		isl7998x_write_reg(isl7998x_data, 0x01, 0x05);  //For field mode
+		//isl7998x_write_reg(isl7998x_data, 0x01, 0x25);  //For frame mode
+		isl7998x_write_reg(isl7998x_data, 0x02, 0xA0);
+		isl7998x_write_reg(isl7998x_data, 0x03, 0x10);
+		isl7998x_write_reg(isl7998x_data, 0x04, 0xE4);
+		isl7998x_write_reg(isl7998x_data, 0x05, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x06, 0x60);
+		isl7998x_write_reg(isl7998x_data, 0x07, 0x2B);
+		isl7998x_write_reg(isl7998x_data, 0x08, 0x02);
+		isl7998x_write_reg(isl7998x_data, 0x09, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x0A, 0x62);
+		isl7998x_write_reg(isl7998x_data, 0x0B, 0x02);
+		isl7998x_write_reg(isl7998x_data, 0x0C, 0x36);
+		isl7998x_write_reg(isl7998x_data, 0x0D, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x0E, 0x6C);
+		isl7998x_write_reg(isl7998x_data, 0x0F, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x10, 0x05);
+		isl7998x_write_reg(isl7998x_data, 0x11, 0xA0);
+		isl7998x_write_reg(isl7998x_data, 0x12, 0x77);
+		isl7998x_write_reg(isl7998x_data, 0x13, 0x17);
+		isl7998x_write_reg(isl7998x_data, 0x14, 0x08);
+		isl7998x_write_reg(isl7998x_data, 0x15, 0x38);
+		isl7998x_write_reg(isl7998x_data, 0x16, 0x14);
+		isl7998x_write_reg(isl7998x_data, 0x17, 0xF6);
+		isl7998x_write_reg(isl7998x_data, 0x18, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x19, 0x17);
+		isl7998x_write_reg(isl7998x_data, 0x1A, 0x0A);
+		isl7998x_write_reg(isl7998x_data, 0x1B, 0x71);
+		isl7998x_write_reg(isl7998x_data, 0x1C, 0x7A);
+		isl7998x_write_reg(isl7998x_data, 0x1D, 0x0F);
+		isl7998x_write_reg(isl7998x_data, 0x1E, 0x8C);
+		isl7998x_write_reg(isl7998x_data, 0x23, 0x0A);
+		isl7998x_write_reg(isl7998x_data, 0x26, 0x08);
+		isl7998x_write_reg(isl7998x_data, 0x28, 0x01);
+		isl7998x_write_reg(isl7998x_data, 0x29, 0x0E);
+		isl7998x_write_reg(isl7998x_data, 0x2A, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x2B, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x38, 0x03);
+		isl7998x_write_reg(isl7998x_data, 0x39, 0xC0);
+		isl7998x_write_reg(isl7998x_data, 0x3A, 0x06);
+		isl7998x_write_reg(isl7998x_data, 0x3B, 0xB3);
+		isl7998x_write_reg(isl7998x_data, 0x3C, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x3D, 0xF1);
+
+		/* change by BKang */
+		isl7998x_write_reg(isl7998x_data, 0x06, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x35, 0x00);
+
+		// Page 0
+		isl7998x_write_reg(isl7998x_data, 0xFF, 0x00);
+		isl7998x_write_reg(isl7998x_data, 0x02, 0x00);  /* clear Reset */
+	}
+
+	msleep(10);
+	return retval;
+}
+
+static int isl7998x_g_std(struct v4l2_subdev *sd, v4l2_std_id *norm)
+{
+	v4l2_std_id std;
+	struct sensor_data *isl7998x_data = subdev_to_sensor_data(sd);
+	u8 data;
+
+	// Page 1
+	isl7998x_write_reg(isl7998x_data, 0xFF, 0x01);
+	data = (u8)isl7998x_read_reg(isl7998x_data, 0x1C);
+	data &= 0x70;
+
+	if ((data == 0x10) || (data == 0x20) || (data == 0x50)) {
+		std = V4L2_STD_PAL;
+		 isl7998x_data->video_id = PAL;
+	} else {
+		std = V4L2_STD_NTSC;
+		isl7998x_data->video_id = NTSC;
+	}
+
+	if (norm != NULL)
+		*norm = std;
+
+	return 0;
+}
+
+static int isl7998x_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)
+{
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+	struct sensor_data *isl7998x_data = subdev_to_sensor_data(sd);
+	int ret = 0;
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		memset(a, 0, sizeof(*a));
+		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cparm->capability = isl7998x_data->streamcap.capability;
+		cparm->timeperframe = isl7998x_data->streamcap.timeperframe;
+		cparm->capturemode = isl7998x_data->streamcap.capturemode;
+		ret = 0;
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		ret = -EINVAL;
+		break;
+	default:
+		pr_debug("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_parm - V4L2 sensor interface handler for VIDIOC_S_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_S_PARM ioctl structure
+ *
+ * Configures the sensor to use the input parameters, if possible.  If
+ * not possible, reverts to the old parameters and returns the
+ * appropriate error code.
+ */
+static int isl7998x_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)
+{
+	int ret = 0;
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		pr_debug("   type is not "\
+				 "V4L2_BUF_TYPE_VIDEO_CAPTURE but %d\n",
+			a->type);
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_debug("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int isl7998x_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct sensor_data *isl7998x_data = subdev_to_sensor_data(sd);
+
+	code->code = isl7998x_data->format.code;
+	return 0;
+}
+
+/*!
+ * isl7998x_enum_framesizes - V4L2 sensor interface handler for
+ *			   VIDIOC_ENUM_FRAMESIZES ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @fsize: standard V4L2 VIDIOC_ENUM_FRAMESIZES ioctl structure
+ *
+ * Return 0 if successful, otherwise -EINVAL.
+ */
+static int isl7998x_enum_framesizes(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index > PAL)
+		return -EINVAL;
+
+	fse->max_width = max(video_fmts[fse->index].width, video_fmts[fse->index].width);
+	fse->min_width = fse->max_width;
+
+	fse->max_height = max(video_fmts[fse->index].height, video_fmts[fse->index].height);
+	fse->min_height = fse->max_height;
+
+	return 0;
+}
+static int isl7998x_enum_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_pad_config *cfg,
+				   struct v4l2_subdev_frame_interval_enum *fie)
+{
+	int i, count;
+
+	if (fie->index < 0 || fie->index > PAL)
+		return -EINVAL;
+
+	if (fie->width == 0 || fie->height == 0 ||
+	    fie->code == 0) {
+		pr_warning("Please assign pixel format, width and height.\n");
+		return -EINVAL;
+	}
+
+	fie->interval.numerator = 1;
+
+	 /* TODO Reserved to extension */
+	count = 0;
+	for (i = 0; i < PAL; i++) {
+		if (fie->width == video_fmts[i].width
+		 && fie->height == video_fmts[i].height) {
+			count++;
+		}
+		if (fie->index == (count - 1)) {
+			fie->interval.denominator = video_fmts[i].frame_rate;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int isl7998x_get_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct sensor_data *isl7998x_data = subdev_to_sensor_data(sd);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+
+	if (fmt->pad)
+		return -EINVAL;
+
+	mf->code = isl7998x_data->format.code;
+	mf->width =  isl7998x_data->format.width;
+	mf->height = isl7998x_data->format.height;
+	mf->colorspace = isl7998x_data->format.colorspace;
+	mf->field = isl7998x_data->format.field;
+	mf->reserved[0] = isl7998x_data->mipi_bps;
+
+	return 0;
+}
+
+static int isl7998x_set_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_subdev_pad_config *cfg,
+			 struct v4l2_subdev_format *fmt)
+{
+	struct sensor_data *isl7998x_data = subdev_to_sensor_data(sd);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+
+	if (fmt->pad)
+		return -EINVAL;
+
+	mf->code = isl7998x_data->format.code;
+	mf->colorspace = isl7998x_data->format.colorspace;
+	mf->field = V4L2_FIELD_INTERLACED;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	memcpy(&isl7998x_data->format, &fmt->format, sizeof(struct v4l2_mbus_framefmt));
+
+	return 0;
+}
+
+static int isl7998x_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+				  struct v4l2_mbus_frame_desc *fd)
+{
+	return 0;
+}
+
+static int isl7998x_set_frame_desc(struct v4l2_subdev *sd,
+					unsigned int pad,
+					struct v4l2_mbus_frame_desc *fd)
+{
+	return 0;
+}
+
+static int isl7998x_set_power(struct v4l2_subdev *sd, int on)
+{
+	return 0;
+}
+
+static int isl7998x_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct sensor_data *isl7998x_data = subdev_to_sensor_data(sd);
+	u8 data;
+
+	dev_dbg(sd->dev, "%s\n", __func__);
+	if (enable) {
+		if (!isl7998x_data->running) {
+			/* Enable CSI output */
+			isl7998x_write_reg(isl7998x_data, 0xFF, 0x05);
+			data = isl7998x_read_reg(isl7998x_data, 0x00);
+			data &= ~0x80;
+			isl7998x_write_reg(isl7998x_data, 0x00, data);
+		}
+		isl7998x_data->running++;
+
+	} else {
+
+		if (isl7998x_data->running) {
+			/* Disable CSI Output */
+			isl7998x_write_reg(isl7998x_data, 0xFF, 0x05);
+			data = isl7998x_read_reg(isl7998x_data, 0x00);
+			data |= 0x80;
+			isl7998x_write_reg(isl7998x_data, 0x00, data);
+		}
+		isl7998x_data->running--;
+	}
+
+	return 0;
+}
+
+static int isl7998x_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops isl7998x_pad_ops = {
+	.enum_mbus_code		= isl7998x_enum_mbus_code,
+	.enum_frame_size	= isl7998x_enum_framesizes,
+	.enum_frame_interval	= isl7998x_enum_frame_interval,
+	.get_fmt		= isl7998x_get_fmt,
+	.set_fmt		= isl7998x_set_fmt,
+	.get_frame_desc		= isl7998x_get_frame_desc,
+	.set_frame_desc		= isl7998x_set_frame_desc,
+};
+
+static const struct v4l2_subdev_core_ops isl7998x_core_ops = {
+	.s_power	= isl7998x_set_power,
+};
+
+static const struct v4l2_subdev_video_ops isl7998x_video_ops = {
+	.g_std = isl7998x_g_std,
+	.s_parm = isl7998x_s_parm,
+	.g_parm = isl7998x_g_parm,
+	.s_stream = isl7998x_s_stream,
+};
+
+static const struct v4l2_subdev_ops isl7998x_subdev_ops = {
+	.core	= &isl7998x_core_ops,
+	.pad	= &isl7998x_pad_ops,
+	.video	= &isl7998x_video_ops,
+};
+
+static const struct media_entity_operations isl7998x_sd_media_ops = {
+	.link_setup = isl7998x_link_setup,
+};
+
+/*!
+ * isl7998x I2C probe function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @return  Error code indicating success or failure
+ */
+static int isl7998x_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct sensor_data *isl7998x_data;
+	struct v4l2_subdev *sd;
+	u8 data;
+	int retval;
+
+	isl7998x_data = devm_kzalloc(dev, sizeof(*isl7998x_data), GFP_KERNEL);
+	if (!isl7998x_data)
+		return -ENOMEM;
+
+	/* Set initial values for the sensor struct. */
+	isl7998x_data->sensor_clk = devm_clk_get(dev, "capture_mclk");
+	if (IS_ERR(isl7998x_data->sensor_clk)) {
+		/* assuming clock enabled by default */
+		isl7998x_data->sensor_clk = NULL;
+		dev_err(dev, "clock-frequency missing or invalid\n");
+		return PTR_ERR(isl7998x_data->sensor_clk);
+	}
+
+	retval = of_property_read_u32(dev->of_node, "mclk",
+		&(isl7998x_data->mclk));
+	if (retval) {
+		dev_err(dev, "mclk missing or invalid\n");
+		return retval;
+	}
+
+	retval = of_property_read_u32(dev->of_node, "mclk_source",
+		(u32 *)&(isl7998x_data->mclk_source));
+	if (retval) {
+		dev_err(dev, "mclk_source missing or invalid\n");
+		return retval;
+	}
+
+	/* request reset pin */
+	isl7998x_data->rst_gpio = of_get_named_gpio(dev->of_node, "rst-gpios", 0);
+	if (!gpio_is_valid(isl7998x_data->rst_gpio)) {
+		dev_err(dev, "no sensor reset pin available\n");
+		return -ENODEV;
+	}
+	retval = devm_gpio_request_one(dev, isl7998x_data->rst_gpio, GPIOF_OUT_INIT_HIGH,
+					"isl7998x_rst");
+	if (retval < 0)
+		return retval;
+
+	/* request power down pin */
+	isl7998x_data->pwn_gpio = of_get_named_gpio(dev->of_node, "pwn-gpios", 0);
+	if (!gpio_is_valid(isl7998x_data->pwn_gpio)) {
+		dev_err(dev, "no sensor pwdn pin available\n");
+		return -ENODEV;
+	}
+	retval = devm_gpio_request_one(dev, isl7998x_data->pwn_gpio, GPIOF_OUT_INIT_HIGH,
+					"isl7998x_pwd");
+	if (retval < 0)
+		return retval;
+
+	isl7998x_data->num_lanes = 2;  /* Default 2 lanes */
+	of_property_read_u32(dev->of_node, "data-lanes", &isl7998x_data->num_lanes);
+
+	isl7998x_hw_reset(isl7998x_data);
+
+	clk_prepare_enable(isl7998x_data->sensor_clk);
+
+	isl7998x_data->i2c_client = client;
+	isl7998x_data->video_id = NTSC;
+	isl7998x_data->format.code = MEDIA_BUS_FMT_YUYV8_1X16;
+	isl7998x_data->format.width = video_fmts[isl7998x_data->video_id].width;
+	isl7998x_data->format.height = video_fmts[isl7998x_data->video_id].height;
+	isl7998x_data->format.colorspace = V4L2_COLORSPACE_JPEG;
+	/*****************************************
+	 * Pass mipi phy clock rate Mbps
+	 * fcsi2 = PCLk * WIDTH * CHANNELS / LANES
+	 * fsci2 = 27MPCLK * 8 bit * 4 channels / num_lanes
+	 ****************************************/
+	isl7998x_data->mipi_bps = 27 * 8 * 4 / isl7998x_data->num_lanes;
+	isl7998x_data->format.field = V4L2_FIELD_INTERLACED;
+	isl7998x_data->streamcap.capturemode = 0;
+	isl7998x_data->streamcap.timeperframe.denominator = video_fmts[isl7998x_data->video_id].frame_rate;
+	isl7998x_data->streamcap.timeperframe.numerator = 1;
+	isl7998x_data->is_mipi = 1;
+
+	isl7998x_write_reg(isl7998x_data, 0xFF, 0x00);
+	data = isl7998x_read_reg(isl7998x_data, 0x00);
+	if ((data != CHIP_ID_79985) && (data != CHIP_ID_79987)) {
+		pr_warning("isl7998x is not found, chip id reg 0x00 = 0x%x.\n", data);
+		clk_disable_unprepare(isl7998x_data->sensor_clk);
+		devm_gpio_free(dev, isl7998x_data->rst_gpio);
+		devm_gpio_free(dev, isl7998x_data->pwn_gpio);
+		return -ENODEV;
+	}
+	isl7998x_data->chip_id = data;
+
+	isl7998x_data->streamcap.capability = V4L2_CAP_TIMEPERFRAME;
+	isl7998x_data->streamcap.timeperframe.denominator = 30;
+	isl7998x_data->streamcap.timeperframe.numerator = 1;
+	isl7998x_data->v_channel = 0;
+
+	sd = &isl7998x_data->subdev;
+	v4l2_i2c_subdev_init(sd, client, &isl7998x_subdev_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	isl7998x_data->pads[MIPI_CSI2_SENS_VC0_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	isl7998x_data->pads[MIPI_CSI2_SENS_VC1_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	isl7998x_data->pads[MIPI_CSI2_SENS_VC2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	isl7998x_data->pads[MIPI_CSI2_SENS_VC3_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	retval = media_entity_pads_init(&sd->entity, MIPI_CSI2_SENS_VCX_PADS_NUM,
+							isl7998x_data->pads);
+	if (retval < 0)
+		return retval;
+
+	isl7998x_data->subdev.entity.ops = &isl7998x_sd_media_ops;
+	retval = v4l2_async_register_subdev(&isl7998x_data->subdev);
+	if (retval < 0) {
+		dev_err(&client->dev,
+					"%s--Async register failed, ret=%d\n", __func__, retval);
+		media_entity_cleanup(&sd->entity);
+	}
+
+	retval = isl7998x_hardware_init(isl7998x_data);
+	if (retval < 0) {
+		dev_err(&client->dev, "camera init failed\n");
+		clk_disable_unprepare(isl7998x_data->sensor_clk);
+		media_entity_cleanup(&sd->entity);
+		v4l2_async_unregister_subdev(sd);
+		return retval;
+	}
+
+	/* Disable CSI Output */
+	// Page 5
+	isl7998x_write_reg(isl7998x_data, 0xFF, 0x05);
+	data = isl7998x_read_reg(isl7998x_data, 0x00);
+	data |= 0x80;
+	isl7998x_write_reg(isl7998x_data, 0x00, data);
+
+	isl7998x_data->running = 0;
+
+	dev_info(&isl7998x_data->i2c_client->dev,
+			"isl7998x is found, id = 0x%x, name %s\n", isl7998x_data->chip_id, sd->name);
+	return retval;
+}
+
+/*!
+ * isl7998x I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  Error code indicating success or failure
+ */
+static int isl7998x_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct sensor_data *isl7998x_data = subdev_to_sensor_data(sd);
+
+	clk_disable_unprepare(isl7998x_data->sensor_clk);
+	media_entity_cleanup(&sd->entity);
+	v4l2_async_unregister_subdev(sd);
+
+	return 0;
+}
+
+static const struct i2c_device_id isl7998x_id[] = {
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, isl7998x_id);
+
+static const struct of_device_id isl7998x_of_match[] = {
+	{ .compatible = "intersil,isl7998x" },
+	{ /* sentinel */ }
+};
+
+static struct i2c_driver isl7998x_i2c_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name   = "isl7998x",
+		.of_match_table	= of_match_ptr(isl7998x_of_match),
+	},
+	.probe  = isl7998x_probe,
+	.remove = isl7998x_remove,
+	.id_table = isl7998x_id,
+};
+
+module_i2c_driver(isl7998x_i2c_driver);
+
+MODULE_AUTHOR("NXP Semiconductor, Inc.");
+MODULE_DESCRIPTION("ISL7998x Video Decoder Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.0");
+MODULE_ALIAS("CSI");
-- 
1.9.1

