diff --git a/drivers/media/i2c/imx219.c b/drivers/media/i2c/imx219.c
index 7a14688f8c22..e2258e88e626 100644
--- a/drivers/media/i2c/imx219.c
+++ b/drivers/media/i2c/imx219.c
@@ -1,1558 +1,1143 @@
-// SPDX-License-Identifier: GPL-2.0
 /*
- * A V4L2 driver for Sony IMX219 cameras.
- * Copyright (C) 2019, Raspberry Pi (Trading) Ltd
+ * Driver for IMX219 CMOS Image Sensor from Sony
  *
- * Based on Sony imx258 camera driver
- * Copyright (C) 2018 Intel Corporation
- *
- * DT / fwnode changes, and regulator / GPIO control taken from imx214 driver
- * Copyright 2018 Qtechnology A/S
- *
- * Flip handling taken from the Sony IMX319 driver.
- * Copyright (C) 2018 Intel Corporation
+ * Copyright (C) 2014, Andrew Chew <achew@nvidia.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.
+ * V0.0X01.0X01 add enum_frame_interval function.
  */
-
 #include <linux/clk.h>
 #include <linux/delay.h>
-#include <linux/gpio/consumer.h>
 #include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/io.h>
 #include <linux/module.h>
-#include <linux/pm_runtime.h>
-#include <linux/regulator/consumer.h>
+#include <linux/of_graph.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/version.h>
+#include <linux/rk-camera-module.h>
+#include <linux/compat.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
+#include <media/v4l2-image-sizes.h>
 #include <media/v4l2-mediabus.h>
-#include <asm/unaligned.h>
 
-#define IMX219_REG_VALUE_08BIT		1
-#define IMX219_REG_VALUE_16BIT		2
+#define DRIVER_VERSION			KERNEL_VERSION(0, 0x01, 0x1)
 
-#define IMX219_REG_MODE_SELECT		0x0100
-#define IMX219_MODE_STANDBY		0x00
-#define IMX219_MODE_STREAMING		0x01
+/* IMX219 supported geometry */
+#define IMX219_TABLE_END		0xffff
+#define IMX219_ANALOGUE_GAIN_MULTIPLIER	256
+#define IMX219_ANALOGUE_GAIN_MIN	(1 * IMX219_ANALOGUE_GAIN_MULTIPLIER)
+#define IMX219_ANALOGUE_GAIN_MAX	(11 * IMX219_ANALOGUE_GAIN_MULTIPLIER)
+#define IMX219_ANALOGUE_GAIN_DEFAULT	(2 * IMX219_ANALOGUE_GAIN_MULTIPLIER)
 
-/* Chip ID */
-#define IMX219_REG_CHIP_ID		0x0000
-#define IMX219_CHIP_ID			0x0219
+/* In dB*256 */
+#define IMX219_DIGITAL_GAIN_MIN		256
+#define IMX219_DIGITAL_GAIN_MAX		43663
+#define IMX219_DIGITAL_GAIN_DEFAULT	256
 
-/* External clock frequency is 24.0M */
-#define IMX219_XCLK_FREQ		24000000
+#define IMX219_DIGITAL_EXPOSURE_MIN	0
+#define IMX219_DIGITAL_EXPOSURE_MAX	4095
+#define IMX219_DIGITAL_EXPOSURE_DEFAULT	1575
 
-/* Pixel rate is fixed at 182.4M for all the modes */
-#define IMX219_PIXEL_RATE		182400000
+#define IMX219_EXP_LINES_MARGIN	4
 
-#define IMX219_DEFAULT_LINK_FREQ	456000000
+#define IMX219_NAME			"imx219"
+#define IMX219_LANES  			2
 
-/* V_TIMING internal */
-#define IMX219_REG_VTS			0x0160
-#define IMX219_VTS_15FPS		0x0dc6
-#define IMX219_VTS_30FPS_1080P		0x06e3
-#define IMX219_VTS_30FPS_BINNED		0x06e3
-#define IMX219_VTS_30FPS_640x480	0x06e3
-#define IMX219_VTS_MAX			0xffff
-
-#define IMX219_VBLANK_MIN		4
-
-/*Frame Length Line*/
-#define IMX219_FLL_MIN			0x08a6
-#define IMX219_FLL_MAX			0xffff
-#define IMX219_FLL_STEP			1
-#define IMX219_FLL_DEFAULT		0x0c98
-
-/* HBLANK control - read only */
-#define IMX219_PPL_DEFAULT		3448
-
-/* Exposure control */
-#define IMX219_REG_EXPOSURE		0x015a
-#define IMX219_EXPOSURE_MIN		4
-#define IMX219_EXPOSURE_STEP		1
-#define IMX219_EXPOSURE_DEFAULT		0x640
-#define IMX219_EXPOSURE_MAX		65535
-
-/* Analog gain control */
-#define IMX219_REG_ANALOG_GAIN		0x0157
-#define IMX219_ANA_GAIN_MIN		0
-#define IMX219_ANA_GAIN_MAX		232
-#define IMX219_ANA_GAIN_STEP		1
-#define IMX219_ANA_GAIN_DEFAULT		0x0
-
-/* Digital gain control */
-#define IMX219_REG_DIGITAL_GAIN		0x0158
-#define IMX219_DGTL_GAIN_MIN		0x0100
-#define IMX219_DGTL_GAIN_MAX		0x0fff
-#define IMX219_DGTL_GAIN_DEFAULT	0x0100
-#define IMX219_DGTL_GAIN_STEP		1
-
-#define IMX219_REG_ORIENTATION		0x0172
-
-/* Binning  Mode */
-#define IMX219_REG_BINNING_MODE		0x0174
-#define IMX219_BINNING_NONE		0x0000
-#define IMX219_BINNING_2X2		0x0101
-#define IMX219_BINNING_2X2_ANALOG	0x0303
-
-/* Test Pattern Control */
-#define IMX219_REG_TEST_PATTERN		0x0600
-#define IMX219_TEST_PATTERN_DISABLE	0
-#define IMX219_TEST_PATTERN_SOLID_COLOR	1
-#define IMX219_TEST_PATTERN_COLOR_BARS	2
-#define IMX219_TEST_PATTERN_GREY_COLOR	3
-#define IMX219_TEST_PATTERN_PN9		4
-
-/* Test pattern colour components */
-#define IMX219_REG_TESTP_RED		0x0602
-#define IMX219_REG_TESTP_GREENR		0x0604
-#define IMX219_REG_TESTP_BLUE		0x0606
-#define IMX219_REG_TESTP_GREENB		0x0608
-#define IMX219_TESTP_COLOUR_MIN		0
-#define IMX219_TESTP_COLOUR_MAX		0x03ff
-#define IMX219_TESTP_COLOUR_STEP	1
-#define IMX219_TESTP_RED_DEFAULT	IMX219_TESTP_COLOUR_MAX
-#define IMX219_TESTP_GREENR_DEFAULT	0
-#define IMX219_TESTP_BLUE_DEFAULT	0
-#define IMX219_TESTP_GREENB_DEFAULT	0
-
-/* IMX219 native and active pixel array size. */
-#define IMX219_NATIVE_WIDTH		3296U
-#define IMX219_NATIVE_HEIGHT		2480U
-#define IMX219_PIXEL_ARRAY_LEFT		8U
-#define IMX219_PIXEL_ARRAY_TOP		8U
-#define IMX219_PIXEL_ARRAY_WIDTH	3280U
-#define IMX219_PIXEL_ARRAY_HEIGHT	2464U
+static const s64 link_freq_menu_items[] = {
+	456000000,
+};
 
 struct imx219_reg {
-	u16 address;
+	u16 addr;
 	u8 val;
 };
 
-struct imx219_reg_list {
-	unsigned int num_of_regs;
-	const struct imx219_reg *regs;
-};
-
-/* Mode : resolution and related config&values */
 struct imx219_mode {
-	/* Frame width */
-	unsigned int width;
-	/* Frame height */
-	unsigned int height;
-
-	/* Analog crop rectangle. */
-	struct v4l2_rect crop;
-
-	/* V-timing */
-	unsigned int vts_def;
-
-	/* Default register values */
-	struct imx219_reg_list reg_list;
-
-	/* 2x2 binning is used */
-	bool binning;
+	u32 width;
+	u32 height;
+	struct v4l2_fract max_fps;
+	u32 hts_def;
+	u32 vts_def;
+	const struct imx219_reg *reg_list;
 };
 
-static const struct imx219_reg imx219_common_regs[] = {
-	{0x0100, 0x00},	/* Mode Select */
+/* MCLK:24MHz  3280x2464  21.2fps   MIPI LANE2 */
+static const struct imx219_reg imx219_init_tab_3280_2464_21fps[] = {
+	{0x30EB, 0x05},		/* Access Code for address over 0x3000 */
+	{0x30EB, 0x0C},		/* Access Code for address over 0x3000 */
+	{0x300A, 0xFF},		/* Access Code for address over 0x3000 */
+	{0x300B, 0xFF},		/* Access Code for address over 0x3000 */
+	{0x30EB, 0x05},		/* Access Code for address over 0x3000 */
+	{0x30EB, 0x09},		/* Access Code for address over 0x3000 */
+	{0x0114, 0x01},		/* CSI_LANE_MODE[1:0} */
+	{0x0128, 0x00},		/* DPHY_CNTRL */
+	{0x012A, 0x18},		/* EXCK_FREQ[15:8] */
+	{0x012B, 0x00},		/* EXCK_FREQ[7:0] */
+	{0x015A, 0x01},		/* INTEG TIME[15:8] */
+	{0x015B, 0xF4},		/* INTEG TIME[7:0] */
+	{0x0160, 0x09},		/* FRM_LENGTH_A[15:8] */
+	{0x0161, 0xC4},		/* FRM_LENGTH_A[7:0] */
+	{0x0162, 0x0D},		/* LINE_LENGTH_A[15:8] */
+	{0x0163, 0x78},		/* LINE_LENGTH_A[7:0] */
+	{0x0260, 0x09},		/* FRM_LENGTH_B[15:8] */
+	{0x0261, 0xC4},		/* FRM_LENGTH_B[7:0] */
+	{0x0262, 0x0D},		/* LINE_LENGTH_B[15:8] */
+	{0x0263, 0x78},		/* LINE_LENGTH_B[7:0] */
+	{0x0170, 0x01},		/* X_ODD_INC_A[2:0] */
+	{0x0171, 0x01},		/* Y_ODD_INC_A[2:0] */
+	{0x0270, 0x01},		/* X_ODD_INC_B[2:0] */
+	{0x0271, 0x01},		/* Y_ODD_INC_B[2:0] */
+	{0x0174, 0x00},		/* BINNING_MODE_H_A */
+	{0x0175, 0x00},		/* BINNING_MODE_V_A */
+	{0x0274, 0x00},		/* BINNING_MODE_H_B */
+	{0x0275, 0x00},		/* BINNING_MODE_V_B */
+	{0x018C, 0x0A},		/* CSI_DATA_FORMAT_A[15:8] */
+	{0x018D, 0x0A},		/* CSI_DATA_FORMAT_A[7:0] */
+	{0x028C, 0x0A},		/* CSI_DATA_FORMAT_B[15:8] */
+	{0x028D, 0x0A},		/* CSI_DATA_FORMAT_B[7:0] */
+	{0x0301, 0x05},		/* VTPXCK_DIV */
+	{0x0303, 0x01},		/* VTSYCK_DIV */
+	{0x0304, 0x03},		/* PREPLLCK_VT_DIV[3:0] */
+	{0x0305, 0x03},		/* PREPLLCK_OP_DIV[3:0] */
+	{0x0306, 0x00},		/* PLL_VT_MPY[10:8] */
+	{0x0307, 0x39},		/* PLL_VT_MPY[7:0] */
+	{0x0309, 0x0A},		/* OPPXCK_DIV[4:0] */
+	{0x030B, 0x01},		/* OPSYCK_DIV */
+	{0x030C, 0x00},		/* PLL_OP_MPY[10:8] */
+	{0x030D, 0x72},		/* PLL_OP_MPY[7:0] */
+	{0x455E, 0x00},		/* CIS Tuning */
+	{0x471E, 0x4B},		/* CIS Tuning */
+	{0x4767, 0x0F},		/* CIS Tuning */
+	{0x4750, 0x14},		/* CIS Tuning */
+	{0x47B4, 0x14},		/* CIS Tuning */
+	{IMX219_TABLE_END, 0x00}
+};
 
-	/* To Access Addresses 3000-5fff, send the following commands */
-	{0x30eb, 0x0c},
-	{0x30eb, 0x05},
-	{0x300a, 0xff},
-	{0x300b, 0xff},
-	{0x30eb, 0x05},
-	{0x30eb, 0x09},
-
-	/* PLL Clock Table */
-	{0x0301, 0x05},	/* VTPXCK_DIV */
-	{0x0303, 0x01},	/* VTSYSCK_DIV */
-	{0x0304, 0x03},	/* PREPLLCK_VT_DIV 0x03 = AUTO set */
-	{0x0305, 0x03}, /* PREPLLCK_OP_DIV 0x03 = AUTO set */
-	{0x0306, 0x00},	/* PLL_VT_MPY */
-	{0x0307, 0x39},
-	{0x030b, 0x01},	/* OP_SYS_CLK_DIV */
-	{0x030c, 0x00},	/* PLL_OP_MPY */
-	{0x030d, 0x72},
-
-	/* Undocumented registers */
-	{0x455e, 0x00},
-	{0x471e, 0x4b},
-	{0x4767, 0x0f},
-	{0x4750, 0x14},
-	{0x4540, 0x00},
-	{0x47b4, 0x14},
-	{0x4713, 0x30},
-	{0x478b, 0x10},
-	{0x478f, 0x10},
-	{0x4793, 0x10},
-	{0x4797, 0x0e},
-	{0x479b, 0x0e},
-
-	/* Frame Bank Register Group "A" */
-	{0x0162, 0x0d},	/* Line_Length_A */
+/* MCLK:24MHz  1920x1080  30fps   MIPI LANE2 */
+static const struct imx219_reg imx219_init_tab_1920_1080_30fps[] = {
+	{0x30EB, 0x05},
+	{0x30EB, 0x0C},
+	{0x300A, 0xFF},
+	{0x300B, 0xFF},
+	{0x30EB, 0x05},
+	{0x30EB, 0x09},
+	{0x0114, 0x01},
+	{0x0128, 0x00},
+	{0x012A, 0x18},
+	{0x012B, 0x00},
+	{0x0160, 0x06},
+	{0x0161, 0xE6},
+	{0x0162, 0x0D},
 	{0x0163, 0x78},
-	{0x0170, 0x01}, /* X_ODD_INC_A */
-	{0x0171, 0x01}, /* Y_ODD_INC_A */
-
-	/* Output setup registers */
-	{0x0114, 0x01},	/* CSI 2-Lane Mode */
-	{0x0128, 0x00},	/* DPHY Auto Mode */
-	{0x012a, 0x18},	/* EXCK_Freq */
-	{0x012b, 0x00},
-};
-
-/*
- * Register sets lifted off the i2C interface from the Raspberry Pi firmware
- * driver.
- * 3280x2464 = mode 2, 1920x1080 = mode 1, 1640x1232 = mode 4, 640x480 = mode 7.
- */
-static const struct imx219_reg mode_3280x2464_regs[] = {
-	{0x0164, 0x00},
-	{0x0165, 0x00},
-	{0x0166, 0x0c},
-	{0x0167, 0xcf},
-	{0x0168, 0x00},
-	{0x0169, 0x00},
-	{0x016a, 0x09},
-	{0x016b, 0x9f},
-	{0x016c, 0x0c},
-	{0x016d, 0xd0},
-	{0x016e, 0x09},
-	{0x016f, 0xa0},
-	{0x0624, 0x0c},
-	{0x0625, 0xd0},
-	{0x0626, 0x09},
-	{0x0627, 0xa0},
-};
-
-static const struct imx219_reg mode_1920_1080_regs[] = {
 	{0x0164, 0x02},
-	{0x0165, 0xa8},
-	{0x0166, 0x0a},
+	{0x0165, 0xA8},
+	{0x0166, 0x0A},
 	{0x0167, 0x27},
 	{0x0168, 0x02},
-	{0x0169, 0xb4},
-	{0x016a, 0x06},
-	{0x016b, 0xeb},
-	{0x016c, 0x07},
-	{0x016d, 0x80},
-	{0x016e, 0x04},
-	{0x016f, 0x38},
-	{0x0624, 0x07},
-	{0x0625, 0x80},
-	{0x0626, 0x04},
-	{0x0627, 0x38},
+	{0x0169, 0xB4},
+	{0x016A, 0x06},
+	{0x016B, 0xEB},
+	{0x016C, 0x07},
+	{0x016D, 0x80},
+	{0x016E, 0x04},
+	{0x016F, 0x38},
+	{0x0170, 0x01},
+	{0x0171, 0x01},
+	{0x0174, 0x00},
+	{0x0175, 0x00},
+	{0x018C, 0x0A},
+	{0x018D, 0x0A},
+	{0x0301, 0x05},
+	{0x0303, 0x01},
+	{0x0304, 0x03},
+	{0x0305, 0x03},
+	{0x0306, 0x00},
+	{0x0307, 0x39},
+	{0x0309, 0x0A},
+	{0x030B, 0x01},
+	{0x030C, 0x00},
+	{0x030D, 0x72},
+	{0x455E, 0x00},
+	{0x471E, 0x4B},
+	{0x4767, 0x0F},
+	{0x4750, 0x14},
+	{0x4540, 0x00},
+	{0x47B4, 0x14},
+	{IMX219_TABLE_END, 0x00}
 };
 
-static const struct imx219_reg mode_1640_1232_regs[] = {
-	{0x0164, 0x00},
-	{0x0165, 0x00},
-	{0x0166, 0x0c},
-	{0x0167, 0xcf},
-	{0x0168, 0x00},
-	{0x0169, 0x00},
-	{0x016a, 0x09},
-	{0x016b, 0x9f},
-	{0x016c, 0x06},
-	{0x016d, 0x68},
-	{0x016e, 0x04},
-	{0x016f, 0xd0},
-	{0x0624, 0x06},
-	{0x0625, 0x68},
-	{0x0626, 0x04},
-	{0x0627, 0xd0},
+static const struct imx219_reg start[] = {
+	{0x0100, 0x01},		/* mode select streaming on */
+	{IMX219_TABLE_END, 0x00}
 };
 
-static const struct imx219_reg mode_640_480_regs[] = {
-	{0x0164, 0x03},
-	{0x0165, 0xe8},
-	{0x0166, 0x08},
-	{0x0167, 0xe7},
-	{0x0168, 0x02},
-	{0x0169, 0xf0},
-	{0x016a, 0x06},
-	{0x016b, 0xaf},
-	{0x016c, 0x02},
-	{0x016d, 0x80},
-	{0x016e, 0x01},
-	{0x016f, 0xe0},
-	{0x0624, 0x06},
-	{0x0625, 0x68},
-	{0x0626, 0x04},
-	{0x0627, 0xd0},
+static const struct imx219_reg stop[] = {
+	{0x0100, 0x00},		/* mode select streaming off */
+	{IMX219_TABLE_END, 0x00}
 };
 
-static const struct imx219_reg raw8_framefmt_regs[] = {
-	{0x018c, 0x08},
-	{0x018d, 0x08},
-	{0x0309, 0x08},
+enum {
+	TEST_PATTERN_DISABLED,
+	TEST_PATTERN_SOLID_BLACK,
+	TEST_PATTERN_SOLID_WHITE,
+	TEST_PATTERN_SOLID_RED,
+	TEST_PATTERN_SOLID_GREEN,
+	TEST_PATTERN_SOLID_BLUE,
+	TEST_PATTERN_COLOR_BAR,
+	TEST_PATTERN_FADE_TO_GREY_COLOR_BAR,
+	TEST_PATTERN_PN9,
+	TEST_PATTERN_16_SPLIT_COLOR_BAR,
+	TEST_PATTERN_16_SPLIT_INVERTED_COLOR_BAR,
+	TEST_PATTERN_COLUMN_COUNTER,
+	TEST_PATTERN_INVERTED_COLUMN_COUNTER,
+	TEST_PATTERN_PN31,
+	TEST_PATTERN_MAX
 };
 
-static const struct imx219_reg raw10_framefmt_regs[] = {
-	{0x018c, 0x0a},
-	{0x018d, 0x0a},
-	{0x0309, 0x0a},
-};
-
-static const s64 imx219_link_freq_menu[] = {
-	IMX219_DEFAULT_LINK_FREQ,
-};
-
-static const char * const imx219_test_pattern_menu[] = {
+static const char *const tp_qmenu[] = {
 	"Disabled",
-	"Color Bars",
-	"Solid Color",
-	"Grey Color Bars",
-	"PN9"
+	"Solid Black",
+	"Solid White",
+	"Solid Red",
+	"Solid Green",
+	"Solid Blue",
+	"Color Bar",
+	"Fade to Grey Color Bar",
+	"PN9",
+	"16 Split Color Bar",
+	"16 Split Inverted Color Bar",
+	"Column Counter",
+	"Inverted Column Counter",
+	"PN31",
 };
 
-static const int imx219_test_pattern_val[] = {
-	IMX219_TEST_PATTERN_DISABLE,
-	IMX219_TEST_PATTERN_COLOR_BARS,
-	IMX219_TEST_PATTERN_SOLID_COLOR,
-	IMX219_TEST_PATTERN_GREY_COLOR,
-	IMX219_TEST_PATTERN_PN9,
-};
-
-/* regulator supplies */
-static const char * const imx219_supply_name[] = {
-	/* Supplies can be enabled in any order */
-	"VANA",  /* Analog (2.8V) supply */
-	"VDIG",  /* Digital Core (1.8V) supply */
-	"VDDL",  /* IF (1.2V) supply */
-};
-
-#define IMX219_NUM_SUPPLIES ARRAY_SIZE(imx219_supply_name)
-
-/*
- * The supported formats.
- * This table MUST contain 4 entries per format, to cover the various flip
- * combinations in the order
- * - no flip
- * - h flip
- * - v flip
- * - h&v flips
- */
-static const u32 codes[] = {
-	MEDIA_BUS_FMT_SRGGB10_1X10,
-	MEDIA_BUS_FMT_SGRBG10_1X10,
-	MEDIA_BUS_FMT_SGBRG10_1X10,
-	MEDIA_BUS_FMT_SBGGR10_1X10,
-
-	MEDIA_BUS_FMT_SRGGB8_1X8,
-	MEDIA_BUS_FMT_SGRBG8_1X8,
-	MEDIA_BUS_FMT_SGBRG8_1X8,
-	MEDIA_BUS_FMT_SBGGR8_1X8,
-};
-
-/*
- * Initialisation delay between XCLR low->high and the moment when the sensor
- * can start capture (i.e. can leave software stanby) must be not less than:
- *   t4 + max(t5, t6 + <time to initialize the sensor register over I2C>)
- * where
- *   t4 is fixed, and is max 200uS,
- *   t5 is fixed, and is 6000uS,
- *   t6 depends on the sensor external clock, and is max 32000 clock periods.
- * As per sensor datasheet, the external clock must be from 6MHz to 27MHz.
- * So for any acceptable external clock t6 is always within the range of
- * 1185 to 5333 uS, and is always less than t5.
- * For this reason this is always safe to wait (t4 + t5) = 6200 uS, then
- * initialize the sensor over I2C, and then exit the software standby.
- *
- * This start-up time can be optimized a bit more, if we start the writes
- * over I2C after (t4+t6), but before (t4+t5) expires. But then sensor
- * initialization over I2C may complete before (t4+t5) expires, and we must
- * ensure that capture is not started before (t4+t5).
- *
- * This delay doesn't account for the power supply startup time. If needed,
- * this should be taken care of via the regulator framework. E.g. in the
- * case of DT for regulator-fixed one should define the startup-delay-us
- * property.
- */
-#define IMX219_XCLR_MIN_DELAY_US	6200
-#define IMX219_XCLR_DELAY_RANGE_US	1000
-
-/* Mode configs */
-static const struct imx219_mode supported_modes[] = {
-	{
-		/* 8MPix 15fps mode */
-		.width = 3280,
-		.height = 2464,
-		.crop = {
-			.left = IMX219_PIXEL_ARRAY_LEFT,
-			.top = IMX219_PIXEL_ARRAY_TOP,
-			.width = 3280,
-			.height = 2464
-		},
-		.vts_def = IMX219_VTS_15FPS,
-		.reg_list = {
-			.num_of_regs = ARRAY_SIZE(mode_3280x2464_regs),
-			.regs = mode_3280x2464_regs,
-		},
-		.binning = false,
-	},
-	{
-		/* 1080P 30fps cropped */
-		.width = 1920,
-		.height = 1080,
-		.crop = {
-			.left = 688,
-			.top = 700,
-			.width = 1920,
-			.height = 1080
-		},
-		.vts_def = IMX219_VTS_30FPS_1080P,
-		.reg_list = {
-			.num_of_regs = ARRAY_SIZE(mode_1920_1080_regs),
-			.regs = mode_1920_1080_regs,
-		},
-		.binning = false,
-	},
-	{
-		/* 2x2 binned 30fps mode */
-		.width = 1640,
-		.height = 1232,
-		.crop = {
-			.left = IMX219_PIXEL_ARRAY_LEFT,
-			.top = IMX219_PIXEL_ARRAY_TOP,
-			.width = 3280,
-			.height = 2464
-		},
-		.vts_def = IMX219_VTS_30FPS_BINNED,
-		.reg_list = {
-			.num_of_regs = ARRAY_SIZE(mode_1640_1232_regs),
-			.regs = mode_1640_1232_regs,
-		},
-		.binning = true,
-	},
-	{
-		/* 640x480 30fps mode */
-		.width = 640,
-		.height = 480,
-		.crop = {
-			.left = 1008,
-			.top = 760,
-			.width = 1280,
-			.height = 960
-		},
-		.vts_def = IMX219_VTS_30FPS_640x480,
-		.reg_list = {
-			.num_of_regs = ARRAY_SIZE(mode_640_480_regs),
-			.regs = mode_640_480_regs,
-		},
-		.binning = true,
-	},
-};
+#define SIZEOF_I2C_TRANSBUF 32
 
 struct imx219 {
-	struct v4l2_subdev sd;
+	struct v4l2_subdev subdev;
 	struct media_pad pad;
-
-	struct v4l2_mbus_framefmt fmt;
-
-	struct clk *xclk; /* system clock to IMX219 */
-	u32 xclk_freq;
-
-	struct gpio_desc *reset_gpio;
-	struct regulator_bulk_data supplies[IMX219_NUM_SUPPLIES];
-
 	struct v4l2_ctrl_handler ctrl_handler;
-	/* V4L2 Controls */
-	struct v4l2_ctrl *pixel_rate;
-	struct v4l2_ctrl *link_freq;
-	struct v4l2_ctrl *exposure;
-	struct v4l2_ctrl *vflip;
-	struct v4l2_ctrl *hflip;
-	struct v4l2_ctrl *vblank;
+	struct clk *clk;
+	struct v4l2_rect crop_rect;
+	int hflip;
+	int vflip;
+	u8 analogue_gain;
+	u16 digital_gain;	/* bits 11:0 */
+	u16 exposure_time;
+	u16 test_pattern;
+	u16 test_pattern_solid_color_r;
+	u16 test_pattern_solid_color_gr;
+	u16 test_pattern_solid_color_b;
+	u16 test_pattern_solid_color_gb;
 	struct v4l2_ctrl *hblank;
-
-	/* Current mode */
-	const struct imx219_mode *mode;
-
-	/*
-	 * Mutex for serialized access:
-	 * Protect sensor module set pad format and start/stop streaming safely.
-	 */
-	struct mutex mutex;
-
-	/* Streaming on/off */
-	bool streaming;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *pixel_rate;
+	const struct imx219_mode *cur_mode;
+	u32 cfg_num;
+	u16 cur_vts;
+	u32 module_index;
+	const char *module_facing;
+	const char *module_name;
+	const char *len_name;
 };
 
-static inline struct imx219 *to_imx219(struct v4l2_subdev *_sd)
+static const struct imx219_mode supported_modes[] = {
+	{
+		.width = 1920,
+		.height = 1080,
+		.max_fps = {
+			.numerator = 10000,
+			.denominator = 300000,
+		},
+		.hts_def = 0x0d78 - IMX219_EXP_LINES_MARGIN,
+		.vts_def = 0x06E6,
+		.reg_list = imx219_init_tab_1920_1080_30fps,
+	},
+	{
+		.width = 3280,
+		.height = 2464,
+		.max_fps = {
+			.numerator = 10000,
+			.denominator = 210000,
+		},
+		.hts_def = 0x0d78 - IMX219_EXP_LINES_MARGIN,
+		.vts_def = 0x09c4,
+		.reg_list = imx219_init_tab_3280_2464_21fps,
+	},
+};
+
+static struct imx219 *to_imx219(const struct i2c_client *client)
 {
-	return container_of(_sd, struct imx219, sd);
+	return container_of(i2c_get_clientdata(client), struct imx219, subdev);
 }
 
-/* Read registers up to 2 at a time */
-static int imx219_read_reg(struct imx219 *imx219, u16 reg, u32 len, u32 *val)
+static int reg_write(struct i2c_client *client, const u16 addr, const u8 data)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
-	struct i2c_msg msgs[2];
-	u8 addr_buf[2] = { reg >> 8, reg & 0xff };
-	u8 data_buf[4] = { 0, };
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_msg msg;
+	u8 tx[3];
 	int ret;
 
-	if (len > 4)
-		return -EINVAL;
+	msg.addr = client->addr;
+	msg.buf = tx;
+	msg.len = 3;
+	msg.flags = 0;
+	tx[0] = addr >> 8;
+	tx[1] = addr & 0xff;
+	tx[2] = data;
+	ret = i2c_transfer(adap, &msg, 1);
+	mdelay(2);
 
-	/* Write register address */
-	msgs[0].addr = client->addr;
-	msgs[0].flags = 0;
-	msgs[0].len = ARRAY_SIZE(addr_buf);
-	msgs[0].buf = addr_buf;
+	return ret == 1 ? 0 : -EIO;
+}
 
-	/* Read data from register */
-	msgs[1].addr = client->addr;
-	msgs[1].flags = I2C_M_RD;
-	msgs[1].len = len;
-	msgs[1].buf = &data_buf[4 - len];
+static int reg_read(struct i2c_client *client, const u16 addr)
+{
+	u8 buf[2] = {addr >> 8, addr & 0xff};
+	int ret;
+	struct i2c_msg msgs[] = {
+		{
+			.addr  = client->addr,
+			.flags = 0,
+			.len   = 2,
+			.buf   = buf,
+		}, {
+			.addr  = client->addr,
+			.flags = I2C_M_RD,
+			.len   = 1,
+			.buf   = buf,
+		},
+	};
 
 	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
-	if (ret != ARRAY_SIZE(msgs))
-		return -EIO;
+	if (ret < 0) {
+		dev_warn(&client->dev, "Reading register %x from %x failed\n",
+			 addr, client->addr);
+		return ret;
+	}
 
-	*val = get_unaligned_be32(data_buf);
-
-	return 0;
+	return buf[0];
 }
 
-/* Write registers up to 2 at a time */
-static int imx219_write_reg(struct imx219 *imx219, u16 reg, u32 len, u32 val)
+static int reg_write_table(struct i2c_client *client,
+			   const struct imx219_reg table[])
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
-	u8 buf[6];
-
-	if (len > 4)
-		return -EINVAL;
-
-	put_unaligned_be16(reg, buf);
-	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
-	if (i2c_master_send(client, buf, len + 2) != len + 2)
-		return -EIO;
-
-	return 0;
-}
-
-/* Write a list of registers */
-static int imx219_write_regs(struct imx219 *imx219,
-			     const struct imx219_reg *regs, u32 len)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
-	unsigned int i;
+	const struct imx219_reg *reg;
 	int ret;
 
-	for (i = 0; i < len; i++) {
-		ret = imx219_write_reg(imx219, regs[i].address, 1, regs[i].val);
-		if (ret) {
-			dev_err_ratelimited(&client->dev,
-					    "Failed to write reg 0x%4.4x. error = %d\n",
-					    regs[i].address, ret);
-
+	for (reg = table; reg->addr != IMX219_TABLE_END; reg++) {
+		ret = reg_write(client, reg->addr, reg->val);
+		if (ret < 0)
 			return ret;
-		}
 	}
 
 	return 0;
 }
 
-/* Get bayer order based on flip setting. */
-static u32 imx219_get_format_code(struct imx219 *imx219, u32 code)
+/* V4L2 subdev video operations */
+static int imx219_s_stream(struct v4l2_subdev *sd, int enable)
 {
-	unsigned int i;
-
-	lockdep_assert_held(&imx219->mutex);
-
-	for (i = 0; i < ARRAY_SIZE(codes); i++)
-		if (codes[i] == code)
-			break;
-
-	if (i >= ARRAY_SIZE(codes))
-		i = 0;
-
-	i = (i & ~3) | (imx219->vflip->val ? 2 : 0) |
-	    (imx219->hflip->val ? 1 : 0);
-
-	return codes[i];
-}
-
-static void imx219_set_default_format(struct imx219 *imx219)
-{
-	struct v4l2_mbus_framefmt *fmt;
-
-	fmt = &imx219->fmt;
-	fmt->code = MEDIA_BUS_FMT_SRGGB10_1X10;
-	fmt->colorspace = V4L2_COLORSPACE_SRGB;
-	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
-	fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
-							  fmt->colorspace,
-							  fmt->ycbcr_enc);
-	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
-	fmt->width = supported_modes[0].width;
-	fmt->height = supported_modes[0].height;
-	fmt->field = V4L2_FIELD_NONE;
-}
-
-static int imx219_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
-{
-	struct imx219 *imx219 = to_imx219(sd);
-	struct v4l2_mbus_framefmt *try_fmt =
-		v4l2_subdev_get_try_format(sd, fh->state, 0);
-	struct v4l2_rect *try_crop;
-
-	mutex_lock(&imx219->mutex);
-
-	/* Initialize try_fmt */
-	try_fmt->width = supported_modes[0].width;
-	try_fmt->height = supported_modes[0].height;
-	try_fmt->code = imx219_get_format_code(imx219,
-					       MEDIA_BUS_FMT_SRGGB10_1X10);
-	try_fmt->field = V4L2_FIELD_NONE;
-
-	/* Initialize try_crop rectangle. */
-	try_crop = v4l2_subdev_get_try_crop(sd, fh->state, 0);
-	try_crop->top = IMX219_PIXEL_ARRAY_TOP;
-	try_crop->left = IMX219_PIXEL_ARRAY_LEFT;
-	try_crop->width = IMX219_PIXEL_ARRAY_WIDTH;
-	try_crop->height = IMX219_PIXEL_ARRAY_HEIGHT;
-
-	mutex_unlock(&imx219->mutex);
-
-	return 0;
-}
-
-static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct imx219 *imx219 =
-		container_of(ctrl->handler, struct imx219, ctrl_handler);
-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx219 *priv = to_imx219(client);
+	u8 reg = 0x00;
 	int ret;
 
-	if (ctrl->id == V4L2_CID_VBLANK) {
-		int exposure_max, exposure_def;
+	if (!enable)
+		return reg_write_table(client, stop);
 
-		/* Update max exposure while meeting expected vblanking */
-		exposure_max = imx219->mode->height + ctrl->val - 4;
-		exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
-			exposure_max : IMX219_EXPOSURE_DEFAULT;
-		__v4l2_ctrl_modify_range(imx219->exposure,
-					 imx219->exposure->minimum,
-					 exposure_max, imx219->exposure->step,
-					 exposure_def);
+	ret = reg_write_table(client, priv->cur_mode->reg_list);
+	if (ret)
+		return ret;
+
+	/* Handle crop */
+	ret = reg_write(client, 0x0164, priv->crop_rect.left >> 8);
+	ret |= reg_write(client, 0x0165, priv->crop_rect.left & 0xff);
+	ret |= reg_write(client, 0x0166, (priv->crop_rect.left + priv->crop_rect.width - 1) >> 8);
+	ret |= reg_write(client, 0x0167, (priv->crop_rect.left + priv->crop_rect.width - 1) & 0xff);
+	ret |= reg_write(client, 0x0168, priv->crop_rect.top >> 8);
+	ret |= reg_write(client, 0x0169, priv->crop_rect.top & 0xff);
+	ret |= reg_write(client, 0x016A, (priv->crop_rect.top + priv->crop_rect.height - 1) >> 8);
+	ret |= reg_write(client, 0x016B, (priv->crop_rect.top + priv->crop_rect.height - 1) & 0xff);
+	ret |= reg_write(client, 0x016C, priv->crop_rect.width >> 8);
+	ret |= reg_write(client, 0x016D, priv->crop_rect.width & 0xff);
+	ret |= reg_write(client, 0x016E, priv->crop_rect.height >> 8);
+	ret |= reg_write(client, 0x016F, priv->crop_rect.height & 0xff);
+
+	if (ret)
+		return ret;
+
+	/* Handle flip/mirror */
+	if (priv->hflip)
+		reg |= 0x1;
+	if (priv->vflip)
+		reg |= 0x2;
+
+	ret = reg_write(client, 0x0172, reg);
+	if (ret)
+		return ret;
+
+	/* Handle test pattern */
+	if (priv->test_pattern) {
+		ret = reg_write(client, 0x0600, priv->test_pattern >> 8);
+		ret |= reg_write(client, 0x0601, priv->test_pattern & 0xff);
+		ret |= reg_write(client, 0x0602,
+				 priv->test_pattern_solid_color_r >> 8);
+		ret |= reg_write(client, 0x0603,
+				 priv->test_pattern_solid_color_r & 0xff);
+		ret |= reg_write(client, 0x0604,
+				 priv->test_pattern_solid_color_gr >> 8);
+		ret |= reg_write(client, 0x0605,
+				 priv->test_pattern_solid_color_gr & 0xff);
+		ret |= reg_write(client, 0x0606,
+				 priv->test_pattern_solid_color_b >> 8);
+		ret |= reg_write(client, 0x0607,
+				 priv->test_pattern_solid_color_b & 0xff);
+		ret |= reg_write(client, 0x0608,
+				 priv->test_pattern_solid_color_gb >> 8);
+		ret |= reg_write(client, 0x0609,
+				 priv->test_pattern_solid_color_gb & 0xff);
+		ret |= reg_write(client, 0x0620, priv->crop_rect.left >> 8);
+		ret |= reg_write(client, 0x0621, priv->crop_rect.left & 0xff);
+		ret |= reg_write(client, 0x0622, priv->crop_rect.top >> 8);
+		ret |= reg_write(client, 0x0623, priv->crop_rect.top & 0xff);
+		ret |= reg_write(client, 0x0624, priv->crop_rect.width >> 8);
+		ret |= reg_write(client, 0x0625, priv->crop_rect.width & 0xff);
+		ret |= reg_write(client, 0x0626, priv->crop_rect.height >> 8);
+		ret |= reg_write(client, 0x0627, priv->crop_rect.height & 0xff);
+	} else {
+		ret = reg_write(client, 0x0600, 0x00);
+		ret |= reg_write(client, 0x0601, 0x00);
 	}
 
-	/*
-	 * Applying V4L2 control value only happens
-	 * when power is up for streaming
-	 */
-	if (pm_runtime_get_if_in_use(&client->dev) == 0)
-		return 0;
+	priv->cur_vts = priv->cur_mode->vts_def - IMX219_EXP_LINES_MARGIN;
+	if (ret)
+		return ret;
 
-	switch (ctrl->id) {
-	case V4L2_CID_ANALOGUE_GAIN:
-		ret = imx219_write_reg(imx219, IMX219_REG_ANALOG_GAIN,
-				       IMX219_REG_VALUE_08BIT, ctrl->val);
+	return reg_write_table(client, start);
+}
+
+/* V4L2 subdev core operations */
+static int imx219_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx219 *priv = to_imx219(client);
+
+	if (on)	{
+		dev_dbg(&client->dev, "imx219 power on\n");
+		clk_prepare_enable(priv->clk);
+	} else if (!on) {
+		dev_dbg(&client->dev, "imx219 power off\n");
+		clk_disable_unprepare(priv->clk);
+	}
+
+	return 0;
+}
+
+/* V4L2 ctrl operations */
+static int imx219_s_ctrl_test_pattern(struct v4l2_ctrl *ctrl)
+{
+	struct imx219 *priv =
+	    container_of(ctrl->handler, struct imx219, ctrl_handler);
+
+	switch (ctrl->val) {
+	case TEST_PATTERN_DISABLED:
+		priv->test_pattern = 0x0000;
 		break;
-	case V4L2_CID_EXPOSURE:
-		ret = imx219_write_reg(imx219, IMX219_REG_EXPOSURE,
-				       IMX219_REG_VALUE_16BIT, ctrl->val);
+	case TEST_PATTERN_SOLID_BLACK:
+		priv->test_pattern = 0x0001;
+		priv->test_pattern_solid_color_r = 0x0000;
+		priv->test_pattern_solid_color_gr = 0x0000;
+		priv->test_pattern_solid_color_b = 0x0000;
+		priv->test_pattern_solid_color_gb = 0x0000;
 		break;
-	case V4L2_CID_DIGITAL_GAIN:
-		ret = imx219_write_reg(imx219, IMX219_REG_DIGITAL_GAIN,
-				       IMX219_REG_VALUE_16BIT, ctrl->val);
+	case TEST_PATTERN_SOLID_WHITE:
+		priv->test_pattern = 0x0001;
+		priv->test_pattern_solid_color_r = 0x0fff;
+		priv->test_pattern_solid_color_gr = 0x0fff;
+		priv->test_pattern_solid_color_b = 0x0fff;
+		priv->test_pattern_solid_color_gb = 0x0fff;
 		break;
-	case V4L2_CID_TEST_PATTERN:
-		ret = imx219_write_reg(imx219, IMX219_REG_TEST_PATTERN,
-				       IMX219_REG_VALUE_16BIT,
-				       imx219_test_pattern_val[ctrl->val]);
+	case TEST_PATTERN_SOLID_RED:
+		priv->test_pattern = 0x0001;
+		priv->test_pattern_solid_color_r = 0x0fff;
+		priv->test_pattern_solid_color_gr = 0x0000;
+		priv->test_pattern_solid_color_b = 0x0000;
+		priv->test_pattern_solid_color_gb = 0x0000;
 		break;
-	case V4L2_CID_HFLIP:
-	case V4L2_CID_VFLIP:
-		ret = imx219_write_reg(imx219, IMX219_REG_ORIENTATION, 1,
-				       imx219->hflip->val |
-				       imx219->vflip->val << 1);
+	case TEST_PATTERN_SOLID_GREEN:
+		priv->test_pattern = 0x0001;
+		priv->test_pattern_solid_color_r = 0x0000;
+		priv->test_pattern_solid_color_gr = 0x0fff;
+		priv->test_pattern_solid_color_b = 0x0000;
+		priv->test_pattern_solid_color_gb = 0x0fff;
 		break;
-	case V4L2_CID_VBLANK:
-		ret = imx219_write_reg(imx219, IMX219_REG_VTS,
-				       IMX219_REG_VALUE_16BIT,
-				       imx219->mode->height + ctrl->val);
+	case TEST_PATTERN_SOLID_BLUE:
+		priv->test_pattern = 0x0001;
+		priv->test_pattern_solid_color_r = 0x0000;
+		priv->test_pattern_solid_color_gr = 0x0000;
+		priv->test_pattern_solid_color_b = 0x0fff;
+		priv->test_pattern_solid_color_gb = 0x0000;
 		break;
-	case V4L2_CID_TEST_PATTERN_RED:
-		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_RED,
-				       IMX219_REG_VALUE_16BIT, ctrl->val);
+	case TEST_PATTERN_COLOR_BAR:
+		priv->test_pattern = 0x0002;
 		break;
-	case V4L2_CID_TEST_PATTERN_GREENR:
-		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENR,
-				       IMX219_REG_VALUE_16BIT, ctrl->val);
+	case TEST_PATTERN_FADE_TO_GREY_COLOR_BAR:
+		priv->test_pattern = 0x0003;
 		break;
-	case V4L2_CID_TEST_PATTERN_BLUE:
-		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_BLUE,
-				       IMX219_REG_VALUE_16BIT, ctrl->val);
+	case TEST_PATTERN_PN9:
+		priv->test_pattern = 0x0004;
 		break;
-	case V4L2_CID_TEST_PATTERN_GREENB:
-		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENB,
-				       IMX219_REG_VALUE_16BIT, ctrl->val);
+	case TEST_PATTERN_16_SPLIT_COLOR_BAR:
+		priv->test_pattern = 0x0005;
+		break;
+	case TEST_PATTERN_16_SPLIT_INVERTED_COLOR_BAR:
+		priv->test_pattern = 0x0006;
+		break;
+	case TEST_PATTERN_COLUMN_COUNTER:
+		priv->test_pattern = 0x0007;
+		break;
+	case TEST_PATTERN_INVERTED_COLUMN_COUNTER:
+		priv->test_pattern = 0x0008;
+		break;
+	case TEST_PATTERN_PN31:
+		priv->test_pattern = 0x0009;
 		break;
 	default:
-		dev_info(&client->dev,
-			 "ctrl(id:0x%x,val:0x%x) is not handled\n",
-			 ctrl->id, ctrl->val);
-		ret = -EINVAL;
-		break;
+		return -EINVAL;
 	}
 
-	pm_runtime_put(&client->dev);
-
-	return ret;
+	return 0;
 }
 
-static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
-	.s_ctrl = imx219_set_ctrl,
-};
+static int imx219_g_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *fi)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx219 *priv = to_imx219(client);
+	const struct imx219_mode *mode = priv->cur_mode;
+
+	fi->interval = mode->max_fps;
+
+	return 0;
+}
+
+static int imx219_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx219 *priv =
+	    container_of(ctrl->handler, struct imx219, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&priv->subdev);
+	u8 reg;
+	int ret;
+	u16 gain = 256;
+	u16 a_gain = 256;
+	u16 d_gain = 1;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		priv->hflip = ctrl->val;
+		break;
+
+	case V4L2_CID_VFLIP:
+		priv->vflip = ctrl->val;
+		break;
+
+	case V4L2_CID_ANALOGUE_GAIN:
+	case V4L2_CID_GAIN:
+		/*
+		 * hal transfer (gain * 256)  to kernel
+		 * than divide into analog gain & digital gain in kernel
+		 */
+
+		gain = ctrl->val;
+		if (gain < 256)
+			gain = 256;
+		if (gain > 43663)
+			gain = 43663;
+		if (gain >= 256 && gain <= 2728) {
+			a_gain = gain;
+			d_gain = 1 * 256;
+		} else {
+			a_gain = 2728;
+			d_gain = (gain * 256) / a_gain;
+		}
+
+		/*
+		 * Analog gain, reg range[0, 232], gain value[1, 10.66]
+		 * reg = 256 - 256 / again
+		 * a_gain here is 256 multify
+		 * so the reg = 256 - 256 * 256 / a_gain
+		 */
+		priv->analogue_gain = (256 - (256 * 256) / a_gain);
+		if (a_gain < 256)
+			priv->analogue_gain = 0;
+		if (priv->analogue_gain > 232)
+			priv->analogue_gain = 232;
+
+		/*
+		 * Digital gain, reg range[256, 4095], gain rage[1, 16]
+		 * reg = dgain * 256
+		 */
+		priv->digital_gain = d_gain;
+		if (priv->digital_gain < 256)
+			priv->digital_gain = 256;
+		if (priv->digital_gain > 4095)
+			priv->digital_gain = 4095;
+
+		/*
+		 * for bank A and bank B switch
+		 * exposure time , gain, vts must change at the same time
+		 * so the exposure & gain can reflect at the same frame
+		 */
+
+		ret = reg_write(client, 0x0157, priv->analogue_gain);
+		ret |= reg_write(client, 0x0158, priv->digital_gain >> 8);
+		ret |= reg_write(client, 0x0159, priv->digital_gain & 0xff);
+
+		return ret;
+
+	case V4L2_CID_EXPOSURE:
+		priv->exposure_time = ctrl->val;
+
+		ret = reg_write(client, 0x015a, priv->exposure_time >> 8);
+		ret |= reg_write(client, 0x015b, priv->exposure_time & 0xff);
+		return ret;
+
+	case V4L2_CID_TEST_PATTERN:
+		return imx219_s_ctrl_test_pattern(ctrl);
+
+	case V4L2_CID_VBLANK:
+		if (ctrl->val < priv->cur_mode->vts_def)
+			ctrl->val = priv->cur_mode->vts_def;
+		if ((ctrl->val - IMX219_EXP_LINES_MARGIN) != priv->cur_vts)
+			priv->cur_vts = ctrl->val - IMX219_EXP_LINES_MARGIN;
+		ret = reg_write(client, 0x0160, ((priv->cur_vts >> 8) & 0xff));
+		ret |= reg_write(client, 0x0161, (priv->cur_vts & 0xff));
+		return ret;
+
+	default:
+		return -EINVAL;
+	}
+	/* If enabled, apply settings immediately */
+	reg = reg_read(client, 0x0100);
+	if ((reg & 0x1f) == 0x01)
+		imx219_s_stream(&priv->subdev, 1);
+
+	return 0;
+}
 
 static int imx219_enum_mbus_code(struct v4l2_subdev *sd,
 				 struct v4l2_subdev_state *sd_state,
 				 struct v4l2_subdev_mbus_code_enum *code)
 {
-	struct imx219 *imx219 = to_imx219(sd);
-
-	if (code->index >= (ARRAY_SIZE(codes) / 4))
+	if (code->index != 0)
 		return -EINVAL;
-
-	mutex_lock(&imx219->mutex);
-	code->code = imx219_get_format_code(imx219, codes[code->index * 4]);
-	mutex_unlock(&imx219->mutex);
+	code->code = MEDIA_BUS_FMT_SRGGB10_1X10;
 
 	return 0;
 }
 
-static int imx219_enum_frame_size(struct v4l2_subdev *sd,
-				  struct v4l2_subdev_state *sd_state,
-				  struct v4l2_subdev_frame_size_enum *fse)
+static int imx219_get_reso_dist(const struct imx219_mode *mode,
+				struct v4l2_mbus_framefmt *framefmt)
 {
-	struct imx219 *imx219 = to_imx219(sd);
-	u32 code;
-
-	if (fse->index >= ARRAY_SIZE(supported_modes))
-		return -EINVAL;
-
-	mutex_lock(&imx219->mutex);
-	code = imx219_get_format_code(imx219, fse->code);
-	mutex_unlock(&imx219->mutex);
-	if (fse->code != code)
-		return -EINVAL;
-
-	fse->min_width = supported_modes[fse->index].width;
-	fse->max_width = fse->min_width;
-	fse->min_height = supported_modes[fse->index].height;
-	fse->max_height = fse->min_height;
-
-	return 0;
+	return abs(mode->width - framefmt->width) +
+	       abs(mode->height - framefmt->height);
 }
 
-static void imx219_reset_colorspace(struct v4l2_mbus_framefmt *fmt)
+static const struct imx219_mode *imx219_find_best_fit(
+					struct v4l2_subdev_format *fmt)
 {
-	fmt->colorspace = V4L2_COLORSPACE_SRGB;
-	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
-	fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
-							  fmt->colorspace,
-							  fmt->ycbcr_enc);
-	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+	struct v4l2_mbus_framefmt *framefmt = &fmt->format;
+	int dist;
+	int cur_best_fit = 0;
+	int cur_best_fit_dist = -1;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(supported_modes); i++) {
+		dist = imx219_get_reso_dist(&supported_modes[i], framefmt);
+		if (cur_best_fit_dist == -1 || dist < cur_best_fit_dist) {
+			cur_best_fit_dist = dist;
+			cur_best_fit = i;
+		}
+	}
+
+	return &supported_modes[cur_best_fit];
 }
 
-static void imx219_update_pad_format(struct imx219 *imx219,
-				     const struct imx219_mode *mode,
-				     struct v4l2_subdev_format *fmt)
+static int imx219_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
 {
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx219 *priv = to_imx219(client);
+	const struct imx219_mode *mode;
+	s64 h_blank, v_blank, pixel_rate;
+	u32 fps = 0;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	mode = imx219_find_best_fit(fmt);
+	fmt->format.code = MEDIA_BUS_FMT_SRGGB10_1X10;
 	fmt->format.width = mode->width;
 	fmt->format.height = mode->height;
 	fmt->format.field = V4L2_FIELD_NONE;
-	imx219_reset_colorspace(&fmt->format);
-}
+	priv->cur_mode = mode;
+	h_blank = mode->hts_def - mode->width;
+	__v4l2_ctrl_modify_range(priv->hblank, h_blank,
+					h_blank, 1, h_blank);
+	v_blank = mode->vts_def - mode->height;
+	__v4l2_ctrl_modify_range(priv->vblank, v_blank,
+					v_blank,
+					1, v_blank);
+	fps = DIV_ROUND_CLOSEST(mode->max_fps.denominator,
+		mode->max_fps.numerator);
+	pixel_rate = mode->vts_def * mode->hts_def * fps;
+	__v4l2_ctrl_modify_range(priv->pixel_rate, pixel_rate,
+					pixel_rate, 1, pixel_rate);
 
-static int __imx219_get_pad_format(struct imx219 *imx219,
-				   struct v4l2_subdev_state *sd_state,
-				   struct v4l2_subdev_format *fmt)
-{
-	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
-		struct v4l2_mbus_framefmt *try_fmt =
-			v4l2_subdev_get_try_format(&imx219->sd, sd_state,
-						   fmt->pad);
-		/* update the code which could change due to vflip or hflip: */
-		try_fmt->code = imx219_get_format_code(imx219, try_fmt->code);
-		fmt->format = *try_fmt;
-	} else {
-		imx219_update_pad_format(imx219, imx219->mode, fmt);
-		fmt->format.code = imx219_get_format_code(imx219,
-							  imx219->fmt.code);
-	}
+	/* reset crop window */
+	priv->crop_rect.left = 1640 - (mode->width / 2);
+	if (priv->crop_rect.left < 0)
+		priv->crop_rect.left = 0;
+	priv->crop_rect.top = 1232 - (mode->height / 2);
+	if (priv->crop_rect.top < 0)
+		priv->crop_rect.top = 0;
+	priv->crop_rect.width = mode->width;
+	priv->crop_rect.height = mode->height;
 
 	return 0;
 }
 
-static int imx219_get_pad_format(struct v4l2_subdev *sd,
-				 struct v4l2_subdev_state *sd_state,
-				 struct v4l2_subdev_format *fmt)
+static int imx219_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
 {
-	struct imx219 *imx219 = to_imx219(sd);
-	int ret;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx219 *priv = to_imx219(client);
+	const struct imx219_mode *mode = priv->cur_mode;
 
-	mutex_lock(&imx219->mutex);
-	ret = __imx219_get_pad_format(imx219, sd_state, fmt);
-	mutex_unlock(&imx219->mutex);
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.code = MEDIA_BUS_FMT_SRGGB10_1X10;
+	fmt->format.field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static void imx219_get_module_inf(struct imx219 *imx219,
+				  struct rkmodule_inf *inf)
+{
+	memset(inf, 0, sizeof(*inf));
+	strlcpy(inf->base.sensor, IMX219_NAME, sizeof(inf->base.sensor));
+	strlcpy(inf->base.module, imx219->module_name,
+		sizeof(inf->base.module));
+	strlcpy(inf->base.lens, imx219->len_name, sizeof(inf->base.lens));
+}
+
+static long imx219_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx219 *imx219 = to_imx219(client);
+	long ret = 0;
+
+	switch (cmd) {
+	case RKMODULE_GET_MODULE_INFO:
+		imx219_get_module_inf(imx219, (struct rkmodule_inf *)arg);
+		break;
+	default:
+		ret = -ENOIOCTLCMD;
+		break;
+	}
 
 	return ret;
 }
 
-static int imx219_set_pad_format(struct v4l2_subdev *sd,
-				 struct v4l2_subdev_state *sd_state,
-				 struct v4l2_subdev_format *fmt)
+#ifdef CONFIG_COMPAT
+static long imx219_compat_ioctl32(struct v4l2_subdev *sd,
+				  unsigned int cmd, unsigned long arg)
 {
-	struct imx219 *imx219 = to_imx219(sd);
-	const struct imx219_mode *mode;
-	struct v4l2_mbus_framefmt *framefmt;
-	int exposure_max, exposure_def, hblank;
-	unsigned int i;
+	void __user *up = compat_ptr(arg);
+	struct rkmodule_inf *inf;
+	struct rkmodule_awb_cfg *cfg;
+	long ret;
 
-	mutex_lock(&imx219->mutex);
+	switch (cmd) {
+	case RKMODULE_GET_MODULE_INFO:
+		inf = kzalloc(sizeof(*inf), GFP_KERNEL);
+		if (!inf) {
+			ret = -ENOMEM;
+			return ret;
+		}
 
-	for (i = 0; i < ARRAY_SIZE(codes); i++)
-		if (codes[i] == fmt->format.code)
-			break;
-	if (i >= ARRAY_SIZE(codes))
-		i = 0;
+		ret = imx219_ioctl(sd, cmd, inf);
+		if (!ret)
+			ret = copy_to_user(up, inf, sizeof(*inf));
+		kfree(inf);
+		break;
+	case RKMODULE_AWB_CFG:
+		cfg = kzalloc(sizeof(*cfg), GFP_KERNEL);
+		if (!cfg) {
+			ret = -ENOMEM;
+			return ret;
+		}
 
-	/* Bayer order varies with flips */
-	fmt->format.code = imx219_get_format_code(imx219, codes[i]);
-
-	mode = v4l2_find_nearest_size(supported_modes,
-				      ARRAY_SIZE(supported_modes),
-				      width, height,
-				      fmt->format.width, fmt->format.height);
-	imx219_update_pad_format(imx219, mode, fmt);
-	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
-		framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
-		*framefmt = fmt->format;
-	} else if (imx219->mode != mode ||
-		   imx219->fmt.code != fmt->format.code) {
-		imx219->fmt = fmt->format;
-		imx219->mode = mode;
-		/* Update limits and set FPS to default */
-		__v4l2_ctrl_modify_range(imx219->vblank, IMX219_VBLANK_MIN,
-					 IMX219_VTS_MAX - mode->height, 1,
-					 mode->vts_def - mode->height);
-		__v4l2_ctrl_s_ctrl(imx219->vblank,
-				   mode->vts_def - mode->height);
-		/* Update max exposure while meeting expected vblanking */
-		exposure_max = mode->vts_def - 4;
-		exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
-			exposure_max : IMX219_EXPOSURE_DEFAULT;
-		__v4l2_ctrl_modify_range(imx219->exposure,
-					 imx219->exposure->minimum,
-					 exposure_max, imx219->exposure->step,
-					 exposure_def);
-		/*
-		 * Currently PPL is fixed to IMX219_PPL_DEFAULT, so hblank
-		 * depends on mode->width only, and is not changeble in any
-		 * way other than changing the mode.
-		 */
-		hblank = IMX219_PPL_DEFAULT - mode->width;
-		__v4l2_ctrl_modify_range(imx219->hblank, hblank, hblank, 1,
-					 hblank);
+		ret = copy_from_user(cfg, up, sizeof(*cfg));
+		if (!ret)
+			ret = imx219_ioctl(sd, cmd, cfg);
+		kfree(cfg);
+		break;
+	default:
+		ret = -ENOIOCTLCMD;
+		break;
 	}
 
-	mutex_unlock(&imx219->mutex);
+	return ret;
+}
+#endif
+
+static int imx219_g_mbus_config(struct v4l2_subdev *sd, unsigned int pad_id,
+				struct v4l2_mbus_config *config)
+{
+	//struct i2c_client *client = v4l2_get_subdevdata(sd);
+	//struct imx219 *imx219 = to_imx219(client);
+
+	config->type = V4L2_MBUS_CSI2_DPHY;
+	config->bus.mipi_csi2.num_data_lanes = IMX219_LANES;
 
 	return 0;
 }
 
-static int imx219_set_framefmt(struct imx219 *imx219)
+static int imx219_enum_frame_interval(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *sd_state,
+				       struct v4l2_subdev_frame_interval_enum *fie)
 {
-	switch (imx219->fmt.code) {
-	case MEDIA_BUS_FMT_SRGGB8_1X8:
-	case MEDIA_BUS_FMT_SGRBG8_1X8:
-	case MEDIA_BUS_FMT_SGBRG8_1X8:
-	case MEDIA_BUS_FMT_SBGGR8_1X8:
-		return imx219_write_regs(imx219, raw8_framefmt_regs,
-					ARRAY_SIZE(raw8_framefmt_regs));
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx219 *priv = to_imx219(client);
 
-	case MEDIA_BUS_FMT_SRGGB10_1X10:
-	case MEDIA_BUS_FMT_SGRBG10_1X10:
-	case MEDIA_BUS_FMT_SGBRG10_1X10:
-	case MEDIA_BUS_FMT_SBGGR10_1X10:
-		return imx219_write_regs(imx219, raw10_framefmt_regs,
-					ARRAY_SIZE(raw10_framefmt_regs));
-	}
+	if (fie->index >= priv->cfg_num)
+		return -EINVAL;
 
-	return -EINVAL;
+	if (fie->code != MEDIA_BUS_FMT_SRGGB10_1X10)
+		return -EINVAL;
+
+	fie->width = supported_modes[fie->index].width;
+	fie->height = supported_modes[fie->index].height;
+	fie->interval = supported_modes[fie->index].max_fps;
+	return 0;
 }
 
-static int imx219_set_binning(struct imx219 *imx219)
+/* Various V4L2 operations tables */
+static struct v4l2_subdev_video_ops imx219_subdev_video_ops = {
+	.s_stream = imx219_s_stream,
+	.g_frame_interval = imx219_g_frame_interval,
+};
+
+static struct v4l2_subdev_core_ops imx219_subdev_core_ops = {
+	.s_power = imx219_s_power,
+	.ioctl = imx219_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl32 = imx219_compat_ioctl32,
+#endif
+};
+
+static const struct v4l2_subdev_pad_ops imx219_subdev_pad_ops = {
+	.enum_mbus_code = imx219_enum_mbus_code,
+	.enum_frame_interval = imx219_enum_frame_interval,
+	.set_fmt = imx219_set_fmt,
+	.get_fmt = imx219_get_fmt,
+	.get_mbus_config = imx219_g_mbus_config,
+};
+
+static struct v4l2_subdev_ops imx219_subdev_ops = {
+	.core = &imx219_subdev_core_ops,
+	.video = &imx219_subdev_video_ops,
+	.pad = &imx219_subdev_pad_ops,
+};
+
+static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
+	.s_ctrl = imx219_s_ctrl,
+};
+
+static int imx219_video_probe(struct i2c_client *client)
 {
-	if (!imx219->mode->binning) {
-		return imx219_write_reg(imx219, IMX219_REG_BINNING_MODE,
-					IMX219_REG_VALUE_16BIT,
-					IMX219_BINNING_NONE);
-	}
-
-	switch (imx219->fmt.code) {
-	case MEDIA_BUS_FMT_SRGGB8_1X8:
-	case MEDIA_BUS_FMT_SGRBG8_1X8:
-	case MEDIA_BUS_FMT_SGBRG8_1X8:
-	case MEDIA_BUS_FMT_SBGGR8_1X8:
-		return imx219_write_reg(imx219, IMX219_REG_BINNING_MODE,
-					IMX219_REG_VALUE_16BIT,
-					IMX219_BINNING_2X2_ANALOG);
-
-	case MEDIA_BUS_FMT_SRGGB10_1X10:
-	case MEDIA_BUS_FMT_SGRBG10_1X10:
-	case MEDIA_BUS_FMT_SGBRG10_1X10:
-	case MEDIA_BUS_FMT_SBGGR10_1X10:
-		return imx219_write_reg(imx219, IMX219_REG_BINNING_MODE,
-					IMX219_REG_VALUE_16BIT,
-					IMX219_BINNING_2X2);
-	}
-
-	return -EINVAL;
-}
-
-static const struct v4l2_rect *
-__imx219_get_pad_crop(struct imx219 *imx219,
-		      struct v4l2_subdev_state *sd_state,
-		      unsigned int pad, enum v4l2_subdev_format_whence which)
-{
-	switch (which) {
-	case V4L2_SUBDEV_FORMAT_TRY:
-		return v4l2_subdev_get_try_crop(&imx219->sd, sd_state, pad);
-	case V4L2_SUBDEV_FORMAT_ACTIVE:
-		return &imx219->mode->crop;
-	}
-
-	return NULL;
-}
-
-static int imx219_get_selection(struct v4l2_subdev *sd,
-				struct v4l2_subdev_state *sd_state,
-				struct v4l2_subdev_selection *sel)
-{
-	switch (sel->target) {
-	case V4L2_SEL_TGT_CROP: {
-		struct imx219 *imx219 = to_imx219(sd);
-
-		mutex_lock(&imx219->mutex);
-		sel->r = *__imx219_get_pad_crop(imx219, sd_state, sel->pad,
-						sel->which);
-		mutex_unlock(&imx219->mutex);
-
-		return 0;
-	}
-
-	case V4L2_SEL_TGT_NATIVE_SIZE:
-		sel->r.top = 0;
-		sel->r.left = 0;
-		sel->r.width = IMX219_NATIVE_WIDTH;
-		sel->r.height = IMX219_NATIVE_HEIGHT;
-
-		return 0;
-
-	case V4L2_SEL_TGT_CROP_DEFAULT:
-	case V4L2_SEL_TGT_CROP_BOUNDS:
-		sel->r.top = IMX219_PIXEL_ARRAY_TOP;
-		sel->r.left = IMX219_PIXEL_ARRAY_LEFT;
-		sel->r.width = IMX219_PIXEL_ARRAY_WIDTH;
-		sel->r.height = IMX219_PIXEL_ARRAY_HEIGHT;
-
-		return 0;
-	}
-
-	return -EINVAL;
-}
-
-static int imx219_start_streaming(struct imx219 *imx219)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
-	const struct imx219_reg_list *reg_list;
+	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
+	u16 model_id;
+	u32 lot_id;
+	u16 chip_id;
 	int ret;
 
-	ret = pm_runtime_resume_and_get(&client->dev);
+	ret = imx219_s_power(subdev, 1);
 	if (ret < 0)
 		return ret;
 
-	/* Send all registers that are common to all modes */
-	ret = imx219_write_regs(imx219, imx219_common_regs, ARRAY_SIZE(imx219_common_regs));
-	if (ret) {
-		dev_err(&client->dev, "%s failed to send mfg header\n", __func__);
-		goto err_rpm_put;
+	/* Check and show model, lot, and chip ID. */
+	ret = reg_read(client, 0x0000);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failure to read Model ID (high byte)\n");
+		goto done;
 	}
+	model_id = ret << 8;
 
-	/* Apply default values of current mode */
-	reg_list = &imx219->mode->reg_list;
-	ret = imx219_write_regs(imx219, reg_list->regs, reg_list->num_of_regs);
-	if (ret) {
-		dev_err(&client->dev, "%s failed to set mode\n", __func__);
-		goto err_rpm_put;
+	ret = reg_read(client, 0x0001);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failure to read Model ID (low byte)\n");
+		goto done;
 	}
+	model_id |= ret;
 
-	ret = imx219_set_framefmt(imx219);
-	if (ret) {
-		dev_err(&client->dev, "%s failed to set frame format: %d\n",
-			__func__, ret);
-		goto err_rpm_put;
+	ret = reg_read(client, 0x0004);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failure to read Lot ID (high byte)\n");
+		goto done;
 	}
+	lot_id = ret << 16;
 
-	ret = imx219_set_binning(imx219);
-	if (ret) {
-		dev_err(&client->dev, "%s failed to set binning: %d\n",
-			__func__, ret);
-		goto err_rpm_put;
+	ret = reg_read(client, 0x0005);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failure to read Lot ID (mid byte)\n");
+		goto done;
 	}
+	lot_id |= ret << 8;
 
-	/* Apply customized values from user */
-	ret =  __v4l2_ctrl_handler_setup(imx219->sd.ctrl_handler);
-	if (ret)
-		goto err_rpm_put;
+	ret = reg_read(client, 0x0006);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failure to read Lot ID (low byte)\n");
+		goto done;
+	}
+	lot_id |= ret;
 
-	/* set stream on register */
-	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
-			       IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
-	if (ret)
-		goto err_rpm_put;
+	ret = reg_read(client, 0x000D);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failure to read Chip ID (high byte)\n");
+		goto done;
+	}
+	chip_id = ret << 8;
 
-	/* vflip and hflip cannot change during streaming */
-	__v4l2_ctrl_grab(imx219->vflip, true);
-	__v4l2_ctrl_grab(imx219->hflip, true);
+	ret = reg_read(client, 0x000E);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failure to read Chip ID (low byte)\n");
+		goto done;
+	}
+	chip_id |= ret;
 
-	return 0;
-
-err_rpm_put:
-	pm_runtime_put(&client->dev);
+	if (model_id != 0x0219) {
+		dev_err(&client->dev, "Model ID: %x not supported!\n",
+			model_id);
+		ret = -ENODEV;
+		goto done;
+	}
+	dev_info(&client->dev,
+		 "Model ID 0x%04x, Lot ID 0x%06x, Chip ID 0x%04x\n",
+		 model_id, lot_id, chip_id);
+done:
+	imx219_s_power(subdev, 0);
 	return ret;
 }
 
-static void imx219_stop_streaming(struct imx219 *imx219)
+static int imx219_ctrls_init(struct v4l2_subdev *sd)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx219 *priv = to_imx219(client);
+	const struct imx219_mode *mode = priv->cur_mode;
+	s64 pixel_rate, h_blank, v_blank;
 	int ret;
+	u32 fps = 0;
 
-	/* set stream off register */
-	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
-			       IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
-	if (ret)
-		dev_err(&client->dev, "%s failed to set stream\n", __func__);
+	v4l2_ctrl_handler_init(&priv->ctrl_handler, 10);
+	v4l2_ctrl_new_std(&priv->ctrl_handler, &imx219_ctrl_ops,
+			  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&priv->ctrl_handler, &imx219_ctrl_ops,
+			  V4L2_CID_VFLIP, 0, 1, 1, 0);
 
-	__v4l2_ctrl_grab(imx219->vflip, false);
-	__v4l2_ctrl_grab(imx219->hflip, false);
+	/* exposure */
+	v4l2_ctrl_new_std(&priv->ctrl_handler, &imx219_ctrl_ops,
+			  V4L2_CID_ANALOGUE_GAIN,
+			  IMX219_ANALOGUE_GAIN_MIN,
+			  IMX219_ANALOGUE_GAIN_MAX,
+			  1, IMX219_ANALOGUE_GAIN_DEFAULT);
+	v4l2_ctrl_new_std(&priv->ctrl_handler, &imx219_ctrl_ops,
+			  V4L2_CID_GAIN,
+			  IMX219_DIGITAL_GAIN_MIN,
+			  IMX219_DIGITAL_GAIN_MAX, 1,
+			  IMX219_DIGITAL_GAIN_DEFAULT);
+	v4l2_ctrl_new_std(&priv->ctrl_handler, &imx219_ctrl_ops,
+			  V4L2_CID_EXPOSURE,
+			  IMX219_DIGITAL_EXPOSURE_MIN,
+			  IMX219_DIGITAL_EXPOSURE_MAX, 1,
+			  IMX219_DIGITAL_EXPOSURE_DEFAULT);
 
-	pm_runtime_put(&client->dev);
-}
+	/* blank */
+	h_blank = mode->hts_def - mode->width;
+	priv->hblank = v4l2_ctrl_new_std(&priv->ctrl_handler, NULL, V4L2_CID_HBLANK,
+			  h_blank, h_blank, 1, h_blank);
+	v_blank = mode->vts_def - mode->height;
+	priv->vblank = v4l2_ctrl_new_std(&priv->ctrl_handler, NULL, V4L2_CID_VBLANK,
+			  v_blank, v_blank, 1, v_blank);
 
-static int imx219_set_stream(struct v4l2_subdev *sd, int enable)
-{
-	struct imx219 *imx219 = to_imx219(sd);
-	int ret = 0;
+	/* freq */
+	v4l2_ctrl_new_int_menu(&priv->ctrl_handler, NULL, V4L2_CID_LINK_FREQ,
+			       0, 0, link_freq_menu_items);
+	fps = DIV_ROUND_CLOSEST(mode->max_fps.denominator,
+		mode->max_fps.numerator);
+	pixel_rate = mode->vts_def * mode->hts_def * fps;
+	priv->pixel_rate = v4l2_ctrl_new_std(&priv->ctrl_handler, NULL, V4L2_CID_PIXEL_RATE,
+			  0, pixel_rate, 1, pixel_rate);
 
-	mutex_lock(&imx219->mutex);
-	if (imx219->streaming == enable) {
-		mutex_unlock(&imx219->mutex);
-		return 0;
+	v4l2_ctrl_new_std_menu_items(&priv->ctrl_handler, &imx219_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(tp_qmenu) - 1, 0, 0, tp_qmenu);
+
+	priv->subdev.ctrl_handler = &priv->ctrl_handler;
+	if (priv->ctrl_handler.error) {
+		dev_err(&client->dev, "Error %d adding controls\n",
+			priv->ctrl_handler.error);
+		ret = priv->ctrl_handler.error;
+		goto error;
 	}
 
-	if (enable) {
-		/*
-		 * Apply default & customized values
-		 * and then start streaming.
-		 */
-		ret = imx219_start_streaming(imx219);
-		if (ret)
-			goto err_unlock;
-	} else {
-		imx219_stop_streaming(imx219);
-	}
-
-	imx219->streaming = enable;
-
-	mutex_unlock(&imx219->mutex);
-
-	return ret;
-
-err_unlock:
-	mutex_unlock(&imx219->mutex);
-
-	return ret;
-}
-
-/* Power/clock management functions */
-static int imx219_power_on(struct device *dev)
-{
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
-	struct imx219 *imx219 = to_imx219(sd);
-	int ret;
-
-	ret = regulator_bulk_enable(IMX219_NUM_SUPPLIES,
-				    imx219->supplies);
-	if (ret) {
-		dev_err(dev, "%s: failed to enable regulators\n",
-			__func__);
-		return ret;
-	}
-
-	ret = clk_prepare_enable(imx219->xclk);
-	if (ret) {
-		dev_err(dev, "%s: failed to enable clock\n",
-			__func__);
-		goto reg_off;
-	}
-
-	gpiod_set_value_cansleep(imx219->reset_gpio, 1);
-	usleep_range(IMX219_XCLR_MIN_DELAY_US,
-		     IMX219_XCLR_MIN_DELAY_US + IMX219_XCLR_DELAY_RANGE_US);
-
-	return 0;
-
-reg_off:
-	regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
-
-	return ret;
-}
-
-static int imx219_power_off(struct device *dev)
-{
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
-	struct imx219 *imx219 = to_imx219(sd);
-
-	gpiod_set_value_cansleep(imx219->reset_gpio, 0);
-	regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
-	clk_disable_unprepare(imx219->xclk);
-
-	return 0;
-}
-
-static int __maybe_unused imx219_suspend(struct device *dev)
-{
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
-	struct imx219 *imx219 = to_imx219(sd);
-
-	if (imx219->streaming)
-		imx219_stop_streaming(imx219);
-
-	return 0;
-}
-
-static int __maybe_unused imx219_resume(struct device *dev)
-{
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
-	struct imx219 *imx219 = to_imx219(sd);
-	int ret;
-
-	if (imx219->streaming) {
-		ret = imx219_start_streaming(imx219);
-		if (ret)
-			goto error;
+	ret = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
+	if (ret < 0) {
+		dev_err(&client->dev, "Error %d setting default controls\n",
+			ret);
+		goto error;
 	}
 
 	return 0;
-
 error:
-	imx219_stop_streaming(imx219);
-	imx219->streaming = false;
-
+	v4l2_ctrl_handler_free(&priv->ctrl_handler);
 	return ret;
 }
 
-static int imx219_get_regulators(struct imx219 *imx219)
+static int imx219_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
-	unsigned int i;
-
-	for (i = 0; i < IMX219_NUM_SUPPLIES; i++)
-		imx219->supplies[i].supply = imx219_supply_name[i];
-
-	return devm_regulator_bulk_get(&client->dev,
-				       IMX219_NUM_SUPPLIES,
-				       imx219->supplies);
-}
-
-/* Verify chip ID */
-static int imx219_identify_module(struct imx219 *imx219)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct device *dev = &client->dev;
+	struct device_node *node = dev->of_node;
+	struct imx219 *priv;
+	struct v4l2_subdev *sd;
+	char facing[2];
 	int ret;
-	u32 val;
 
-	ret = imx219_read_reg(imx219, IMX219_REG_CHIP_ID,
-			      IMX219_REG_VALUE_16BIT, &val);
-	if (ret) {
-		dev_err(&client->dev, "failed to read chip id %x\n",
-			IMX219_CHIP_ID);
-		return ret;
-	}
+	dev_info(dev, "driver version: %02x.%02x.%02x",
+		DRIVER_VERSION >> 16,
+		(DRIVER_VERSION & 0xff00) >> 8,
+		DRIVER_VERSION & 0x00ff);
 
-	if (val != IMX219_CHIP_ID) {
-		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
-			IMX219_CHIP_ID, val);
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_warn(&adapter->dev,
+			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_BYTE\n");
 		return -EIO;
 	}
-
-	return 0;
-}
-
-static const struct v4l2_subdev_core_ops imx219_core_ops = {
-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
-};
-
-static const struct v4l2_subdev_video_ops imx219_video_ops = {
-	.s_stream = imx219_set_stream,
-};
-
-static const struct v4l2_subdev_pad_ops imx219_pad_ops = {
-	.enum_mbus_code = imx219_enum_mbus_code,
-	.get_fmt = imx219_get_pad_format,
-	.set_fmt = imx219_set_pad_format,
-	.get_selection = imx219_get_selection,
-	.enum_frame_size = imx219_enum_frame_size,
-};
-
-static const struct v4l2_subdev_ops imx219_subdev_ops = {
-	.core = &imx219_core_ops,
-	.video = &imx219_video_ops,
-	.pad = &imx219_pad_ops,
-};
-
-static const struct v4l2_subdev_internal_ops imx219_internal_ops = {
-	.open = imx219_open,
-};
-
-/* Initialize control handlers */
-static int imx219_init_controls(struct imx219 *imx219)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
-	struct v4l2_ctrl_handler *ctrl_hdlr;
-	unsigned int height = imx219->mode->height;
-	struct v4l2_fwnode_device_properties props;
-	int exposure_max, exposure_def, hblank;
-	int i, ret;
-
-	ctrl_hdlr = &imx219->ctrl_handler;
-	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 12);
-	if (ret)
-		return ret;
-
-	mutex_init(&imx219->mutex);
-	ctrl_hdlr->lock = &imx219->mutex;
-
-	/* By default, PIXEL_RATE is read only */
-	imx219->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
-					       V4L2_CID_PIXEL_RATE,
-					       IMX219_PIXEL_RATE,
-					       IMX219_PIXEL_RATE, 1,
-					       IMX219_PIXEL_RATE);
-
-	imx219->link_freq =
-		v4l2_ctrl_new_int_menu(ctrl_hdlr, &imx219_ctrl_ops,
-				       V4L2_CID_LINK_FREQ,
-				       ARRAY_SIZE(imx219_link_freq_menu) - 1, 0,
-				       imx219_link_freq_menu);
-	if (imx219->link_freq)
-		imx219->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
-
-	/* Initial vblank/hblank/exposure parameters based on current mode */
-	imx219->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
-					   V4L2_CID_VBLANK, IMX219_VBLANK_MIN,
-					   IMX219_VTS_MAX - height, 1,
-					   imx219->mode->vts_def - height);
-	hblank = IMX219_PPL_DEFAULT - imx219->mode->width;
-	imx219->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
-					   V4L2_CID_HBLANK, hblank, hblank,
-					   1, hblank);
-	if (imx219->hblank)
-		imx219->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
-	exposure_max = imx219->mode->vts_def - 4;
-	exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
-		exposure_max : IMX219_EXPOSURE_DEFAULT;
-	imx219->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
-					     V4L2_CID_EXPOSURE,
-					     IMX219_EXPOSURE_MIN, exposure_max,
-					     IMX219_EXPOSURE_STEP,
-					     exposure_def);
-
-	v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
-			  IMX219_ANA_GAIN_MIN, IMX219_ANA_GAIN_MAX,
-			  IMX219_ANA_GAIN_STEP, IMX219_ANA_GAIN_DEFAULT);
-
-	v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
-			  IMX219_DGTL_GAIN_MIN, IMX219_DGTL_GAIN_MAX,
-			  IMX219_DGTL_GAIN_STEP, IMX219_DGTL_GAIN_DEFAULT);
-
-	imx219->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
-					  V4L2_CID_HFLIP, 0, 1, 1, 0);
-	if (imx219->hflip)
-		imx219->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
-
-	imx219->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
-					  V4L2_CID_VFLIP, 0, 1, 1, 0);
-	if (imx219->vflip)
-		imx219->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
-
-	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx219_ctrl_ops,
-				     V4L2_CID_TEST_PATTERN,
-				     ARRAY_SIZE(imx219_test_pattern_menu) - 1,
-				     0, 0, imx219_test_pattern_menu);
-	for (i = 0; i < 4; i++) {
-		/*
-		 * The assumption is that
-		 * V4L2_CID_TEST_PATTERN_GREENR == V4L2_CID_TEST_PATTERN_RED + 1
-		 * V4L2_CID_TEST_PATTERN_BLUE   == V4L2_CID_TEST_PATTERN_RED + 2
-		 * V4L2_CID_TEST_PATTERN_GREENB == V4L2_CID_TEST_PATTERN_RED + 3
-		 */
-		v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
-				  V4L2_CID_TEST_PATTERN_RED + i,
-				  IMX219_TESTP_COLOUR_MIN,
-				  IMX219_TESTP_COLOUR_MAX,
-				  IMX219_TESTP_COLOUR_STEP,
-				  IMX219_TESTP_COLOUR_MAX);
-		/* The "Solid color" pattern is white by default */
-	}
-
-	if (ctrl_hdlr->error) {
-		ret = ctrl_hdlr->error;
-		dev_err(&client->dev, "%s control init failed (%d)\n",
-			__func__, ret);
-		goto error;
-	}
-
-	ret = v4l2_fwnode_device_parse(&client->dev, &props);
-	if (ret)
-		goto error;
-
-	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx219_ctrl_ops,
-					      &props);
-	if (ret)
-		goto error;
-
-	imx219->sd.ctrl_handler = ctrl_hdlr;
-
-	return 0;
-
-error:
-	v4l2_ctrl_handler_free(ctrl_hdlr);
-	mutex_destroy(&imx219->mutex);
-
-	return ret;
-}
-
-static void imx219_free_controls(struct imx219 *imx219)
-{
-	v4l2_ctrl_handler_free(imx219->sd.ctrl_handler);
-	mutex_destroy(&imx219->mutex);
-}
-
-static int imx219_check_hwcfg(struct device *dev)
-{
-	struct fwnode_handle *endpoint;
-	struct v4l2_fwnode_endpoint ep_cfg = {
-		.bus_type = V4L2_MBUS_CSI2_DPHY
-	};
-	int ret = -EINVAL;
-
-	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
-	if (!endpoint) {
-		dev_err(dev, "endpoint node not found\n");
-		return -EINVAL;
-	}
-
-	if (v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep_cfg)) {
-		dev_err(dev, "could not parse endpoint\n");
-		goto error_out;
-	}
-
-	/* Check the number of MIPI CSI2 data lanes */
-	if (ep_cfg.bus.mipi_csi2.num_data_lanes != 2) {
-		dev_err(dev, "only 2 data lanes are currently supported\n");
-		goto error_out;
-	}
-
-	/* Check the link frequency set in device tree */
-	if (!ep_cfg.nr_of_link_frequencies) {
-		dev_err(dev, "link-frequency property not found in DT\n");
-		goto error_out;
-	}
-
-	if (ep_cfg.nr_of_link_frequencies != 1 ||
-	    ep_cfg.link_frequencies[0] != IMX219_DEFAULT_LINK_FREQ) {
-		dev_err(dev, "Link frequency not supported: %lld\n",
-			ep_cfg.link_frequencies[0]);
-		goto error_out;
-	}
-
-	ret = 0;
-
-error_out:
-	v4l2_fwnode_endpoint_free(&ep_cfg);
-	fwnode_handle_put(endpoint);
-
-	return ret;
-}
-
-static int imx219_probe(struct i2c_client *client)
-{
-	struct device *dev = &client->dev;
-	struct imx219 *imx219;
-	int ret;
-
-	imx219 = devm_kzalloc(&client->dev, sizeof(*imx219), GFP_KERNEL);
-	if (!imx219)
+	priv = devm_kzalloc(&client->dev, sizeof(struct imx219), GFP_KERNEL);
+	if (!priv)
 		return -ENOMEM;
 
-	v4l2_i2c_subdev_init(&imx219->sd, client, &imx219_subdev_ops);
-
-	/* Check the hardware configuration in device tree */
-	if (imx219_check_hwcfg(dev))
-		return -EINVAL;
-
-	/* Get system clock (xclk) */
-	imx219->xclk = devm_clk_get(dev, NULL);
-	if (IS_ERR(imx219->xclk)) {
-		dev_err(dev, "failed to get xclk\n");
-		return PTR_ERR(imx219->xclk);
-	}
-
-	imx219->xclk_freq = clk_get_rate(imx219->xclk);
-	if (imx219->xclk_freq != IMX219_XCLK_FREQ) {
-		dev_err(dev, "xclk frequency not supported: %d Hz\n",
-			imx219->xclk_freq);
+	ret = of_property_read_u32(node, RKMODULE_CAMERA_MODULE_INDEX,
+				   &priv->module_index);
+	ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_FACING,
+				       &priv->module_facing);
+	ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_NAME,
+				       &priv->module_name);
+	ret |= of_property_read_string(node, RKMODULE_CAMERA_LENS_NAME,
+				       &priv->len_name);
+	if (ret) {
+		dev_err(dev, "could not get module information!\n");
 		return -EINVAL;
 	}
 
-	ret = imx219_get_regulators(imx219);
-	if (ret) {
-		dev_err(dev, "failed to get regulators\n");
-		return ret;
+	priv->clk = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(priv->clk)) {
+		dev_info(&client->dev, "Error %ld getting clock\n",
+			 PTR_ERR(priv->clk));
+		return -EPROBE_DEFER;
 	}
 
-	/* Request optional enable pin */
-	imx219->reset_gpio = devm_gpiod_get_optional(dev, "reset",
-						     GPIOD_OUT_HIGH);
+	/* 1920 * 1080 by default */
+	priv->cur_mode = &supported_modes[1];
+	priv->cfg_num = ARRAY_SIZE(supported_modes);
 
-	/*
-	 * The sensor must be powered for imx219_identify_module()
-	 * to be able to read the CHIP_ID register
-	 */
-	ret = imx219_power_on(dev);
-	if (ret)
+	// priv->crop_rect.left = 680;
+	// priv->crop_rect.top = 692;
+	priv->crop_rect.width = priv->cur_mode->width;
+	priv->crop_rect.height = priv->cur_mode->height;
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &imx219_subdev_ops);
+	ret = imx219_ctrls_init(&priv->subdev);
+	if (ret < 0)
+		return ret;
+	ret = imx219_video_probe(client);
+	if (ret < 0)
 		return ret;
 
-	ret = imx219_identify_module(imx219);
-	if (ret)
-		goto error_power_off;
+	priv->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+		     V4L2_SUBDEV_FL_HAS_EVENTS;
 
-	/* Set default mode to max resolution */
-	imx219->mode = &supported_modes[0];
-
-	/* sensor doesn't enter LP-11 state upon power up until and unless
-	 * streaming is started, so upon power up switch the modes to:
-	 * streaming -> standby
-	 */
-	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
-			       IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
+	priv->pad.flags = MEDIA_PAD_FL_SOURCE;
+	priv->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&priv->subdev.entity, 1, &priv->pad);
 	if (ret < 0)
-		goto error_power_off;
-	usleep_range(100, 110);
+		return ret;
 
-	/* put sensor back to standby mode */
-	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
-			       IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
+	sd = &priv->subdev;
+	memset(facing, 0, sizeof(facing));
+	if (strcmp(priv->module_facing, "back") == 0)
+		facing[0] = 'b';
+	else
+		facing[0] = 'f';
+
+	snprintf(sd->name, sizeof(sd->name), "m%02d_%s_%s %s",
+		 priv->module_index, facing,
+		 IMX219_NAME, dev_name(sd->dev));
+	ret = v4l2_async_register_subdev_sensor(sd);
 	if (ret < 0)
-		goto error_power_off;
-	usleep_range(100, 110);
-
-	ret = imx219_init_controls(imx219);
-	if (ret)
-		goto error_power_off;
-
-	/* Initialize subdev */
-	imx219->sd.internal_ops = &imx219_internal_ops;
-	imx219->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
-			    V4L2_SUBDEV_FL_HAS_EVENTS;
-	imx219->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
-
-	/* Initialize source pad */
-	imx219->pad.flags = MEDIA_PAD_FL_SOURCE;
-
-	/* Initialize default format */
-	imx219_set_default_format(imx219);
-
-	ret = media_entity_pads_init(&imx219->sd.entity, 1, &imx219->pad);
-	if (ret) {
-		dev_err(dev, "failed to init entity pads: %d\n", ret);
-		goto error_handler_free;
-	}
-
-	ret = v4l2_async_register_subdev_sensor(&imx219->sd);
-	if (ret < 0) {
-		dev_err(dev, "failed to register sensor sub-device: %d\n", ret);
-		goto error_media_entity;
-	}
-
-	/* Enable runtime PM and turn off the device */
-	pm_runtime_set_active(dev);
-	pm_runtime_enable(dev);
-	pm_runtime_idle(dev);
-
-	return 0;
-
-error_media_entity:
-	media_entity_cleanup(&imx219->sd.entity);
-
-error_handler_free:
-	imx219_free_controls(imx219);
-
-error_power_off:
-	imx219_power_off(dev);
-
+		return ret;
+	printk("==> imx219 ken: %d %s \n",__LINE__,__func__);
 	return ret;
 }
 
 static void imx219_remove(struct i2c_client *client)
 {
-	struct v4l2_subdev *sd = i2c_get_clientdata(client);
-	struct imx219 *imx219 = to_imx219(sd);
+	struct imx219 *priv = to_imx219(client);
 
-	v4l2_async_unregister_subdev(sd);
-	media_entity_cleanup(&sd->entity);
-	imx219_free_controls(imx219);
+	v4l2_async_unregister_subdev(&priv->subdev);
+	media_entity_cleanup(&priv->subdev.entity);
+	v4l2_ctrl_handler_free(&priv->ctrl_handler);
 
-	pm_runtime_disable(&client->dev);
-	if (!pm_runtime_status_suspended(&client->dev))
-		imx219_power_off(&client->dev);
-	pm_runtime_set_suspended(&client->dev);
 }
 
-static const struct of_device_id imx219_dt_ids[] = {
+static const struct i2c_device_id imx219_id[] = {
+	{"imx219", 0},
+	{}
+};
+
+static const struct of_device_id imx219_of_match[] = {
 	{ .compatible = "sony,imx219" },
-	{ /* sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, imx219_dt_ids);
-
-static const struct dev_pm_ops imx219_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(imx219_suspend, imx219_resume)
-	SET_RUNTIME_PM_OPS(imx219_power_off, imx219_power_on, NULL)
+	{ /* sentinel */ },
 };
+MODULE_DEVICE_TABLE(of, imx219_of_match);
 
+MODULE_DEVICE_TABLE(i2c, imx219_id);
 static struct i2c_driver imx219_i2c_driver = {
 	.driver = {
-		.name = "imx219",
-		.of_match_table	= imx219_dt_ids,
-		.pm = &imx219_pm_ops,
+		.of_match_table = of_match_ptr(imx219_of_match),
+		.name = IMX219_NAME,
 	},
-	.probe_new = imx219_probe,
+	.probe = imx219_probe,
 	.remove = imx219_remove,
+	.id_table = imx219_id,
 };
 
 module_i2c_driver(imx219_i2c_driver);
-
-MODULE_AUTHOR("Dave Stevenson <dave.stevenson@raspberrypi.com");
-MODULE_DESCRIPTION("Sony IMX219 sensor driver");
+MODULE_DESCRIPTION("Sony IMX219 Camera driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
 MODULE_LICENSE("GPL v2");