From b283956191f6f30b27611cdd00531617f047be9e Mon Sep 17 00:00:00 2001
From: Maxim Paymushkin <bmx666@users.noreply.github.com>
Date: Tue, 3 Dec 2024 09:47:06 +0100
Subject: [PATCH] patch: kernel: rk35xx-vendor-6.1: port patch for camera

https://github.com/Joshua-Riek/linux-rockchip/commit/1b4e0edcaad78ed2b298318f8482f14838ac993b
---
 patch/kernel/rk35xx-vendor-6.1/camera.patch | 3246 +++++++++++++++++++
 1 file changed, 3246 insertions(+)
 create mode 100644 patch/kernel/rk35xx-vendor-6.1/camera.patch

diff --git a/patch/kernel/rk35xx-vendor-6.1/camera.patch b/patch/kernel/rk35xx-vendor-6.1/camera.patch
new file mode 100644
index 000000000000..24f5d2cf066f
--- /dev/null
+++ b/patch/kernel/rk35xx-vendor-6.1/camera.patch
@@ -0,0 +1,3246 @@
+From 1b4e0edcaad78ed2b298318f8482f14838ac993b Mon Sep 17 00:00:00 2001
+From: Peyton <peyton.howe@bellsouth.net>
+Date: Mon, 1 Jul 2024 00:35:50 -0400
+Subject: [PATCH] Add support for raspberry pi V1, V2, and V3 cameras and
+ change naming convention for OPI5 camera overlays
+
+---
+
+diff --git a/drivers/media/i2c/imx219.c b/drivers/media/i2c/imx219.c
+index 7a14688f8c228..a59f301c65334 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 IMX219_REG_MODE_SELECT		0x0100
+-#define IMX219_MODE_STANDBY		0x00
+-#define IMX219_MODE_STREAMING		0x01
+-
+-/* Chip ID */
+-#define IMX219_REG_CHIP_ID		0x0000
+-#define IMX219_CHIP_ID			0x0219
+-
+-/* External clock frequency is 24.0M */
+-#define IMX219_XCLK_FREQ		24000000
+-
+-/* Pixel rate is fixed at 182.4M for all the modes */
+-#define IMX219_PIXEL_RATE		182400000
+-
+-#define IMX219_DEFAULT_LINK_FREQ	456000000
+-
+-/* 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
+ 
+-struct imx219_reg {
+-	u16 address;
+-	u8 val;
+-};
++#define DRIVER_VERSION			KERNEL_VERSION(0, 0x01, 0x1)
+ 
+-struct imx219_reg_list {
+-	unsigned int num_of_regs;
+-	const struct imx219_reg *regs;
+-};
++/* 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)
+ 
+-/* 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;
++/* In dB*256 */
++#define IMX219_DIGITAL_GAIN_MIN		256
++#define IMX219_DIGITAL_GAIN_MAX		43663
++#define IMX219_DIGITAL_GAIN_DEFAULT	256
+ 
+-	/* V-timing */
+-	unsigned int vts_def;
++#define IMX219_DIGITAL_EXPOSURE_MIN	0
++#define IMX219_DIGITAL_EXPOSURE_MAX	4095
++#define IMX219_DIGITAL_EXPOSURE_DEFAULT	1575
+ 
+-	/* Default register values */
+-	struct imx219_reg_list reg_list;
++#define IMX219_EXP_LINES_MARGIN	4
+ 
+-	/* 2x2 binning is used */
+-	bool binning;
+-};
++#define IMX219_NAME			"imx219"
++#define IMX219_LANES  			2
+ 
+-static const struct imx219_reg imx219_common_regs[] = {
+-	{0x0100, 0x00},	/* Mode Select */
+-
+-	/* 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 */
+-	{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},
++static const s64 link_freq_menu_items[] = {
++	456000000,
+ };
+ 
+-/*
+- * 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},
++struct imx219_reg {
++	u16 addr;
++	u8 val;
+ };
+ 
+-static const struct imx219_reg mode_1920_1080_regs[] = {
+-	{0x0164, 0x02},
+-	{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},
++struct imx219_mode {
++	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 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},
++/* 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}
+ };
+ 
+-static const struct imx219_reg mode_640_480_regs[] = {
+-	{0x0164, 0x03},
+-	{0x0165, 0xe8},
+-	{0x0166, 0x08},
+-	{0x0167, 0xe7},
++/* 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},
++	{0x0164, 0x02},
++	{0x0165, 0xA8},
++	{0x0166, 0x0A},
++	{0x0167, 0x27},
+ 	{0x0168, 0x02},
+-	{0x0169, 0xf0},
+-	{0x016a, 0x06},
+-	{0x016b, 0xaf},
+-	{0x016c, 0x02},
+-	{0x016d, 0x80},
+-	{0x016e, 0x01},
+-	{0x016f, 0xe0},
+-	{0x0624, 0x06},
+-	{0x0625, 0x68},
+-	{0x0626, 0x04},
+-	{0x0627, 0xd0},
++	{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 raw8_framefmt_regs[] = {
+-	{0x018c, 0x08},
+-	{0x018d, 0x08},
+-	{0x0309, 0x08},
++static const struct imx219_reg start[] = {
++	{0x0100, 0x01},		/* mode select streaming on */
++	{IMX219_TABLE_END, 0x00}
+ };
+ 
+-static const struct imx219_reg raw10_framefmt_regs[] = {
+-	{0x018c, 0x0a},
+-	{0x018d, 0x0a},
+-	{0x0309, 0x0a},
++static const struct imx219_reg stop[] = {
++	{0x0100, 0x00},		/* mode select streaming off */
++	{IMX219_TABLE_END, 0x00}
+ };
+ 
+-static const s64 imx219_link_freq_menu[] = {
+-	IMX219_DEFAULT_LINK_FREQ,
++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 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,
+-};
++#define SIZEOF_I2C_TRANSBUF 32
+ 
+-/* 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,
++struct imx219 {
++	struct v4l2_subdev subdev;
++	struct media_pad pad;
++	struct v4l2_ctrl_handler ctrl_handler;
++	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;
++	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;
+ };
+ 
+-/*
+- * 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
++		.max_fps = {
++			.numerator = 10000,
++			.denominator = 300000,
+ 		},
+-		.vts_def = IMX219_VTS_30FPS_1080P,
+-		.reg_list = {
+-			.num_of_regs = ARRAY_SIZE(mode_1920_1080_regs),
+-			.regs = mode_1920_1080_regs,
+-		},
+-		.binning = false,
++		.hts_def = 0x0d78 - IMX219_EXP_LINES_MARGIN,
++		.vts_def = 0x06E6,
++		.reg_list = imx219_init_tab_1920_1080_30fps,
+ 	},
+ 	{
+-		/* 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,
++		.width = 3280,
++		.height = 2464,
++		.max_fps = {
++			.numerator = 10000,
++			.denominator = 210000,
+ 		},
+-		.binning = true,
++		.hts_def = 0x0d78 - IMX219_EXP_LINES_MARGIN,
++		.vts_def = 0x09c4,
++		.reg_list = imx219_init_tab_3280_2464_21fps,
+ 	},
+ };
+ 
+-struct imx219 {
+-	struct v4l2_subdev sd;
+-	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 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;
+-};
+-
+-static inline struct imx219 *to_imx219(struct v4l2_subdev *_sd)
++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;
+-
+-	/* Write register address */
+-	msgs[0].addr = client->addr;
+-	msgs[0].flags = 0;
+-	msgs[0].len = ARRAY_SIZE(addr_buf);
+-	msgs[0].buf = addr_buf;
+-
+-	/* 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];
+-
+-	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+-	if (ret != ARRAY_SIZE(msgs))
+-		return -EIO;
+-
+-	*val = get_unaligned_be32(data_buf);
+-
+-	return 0;
++	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);
++
++	return ret == 1 ? 0 : -EIO;
+ }
+ 
+-/* 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_read(struct i2c_client *client, const u16 addr)
+ {
+-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+-	u8 buf[6];
+-
+-	if (len > 4)
+-		return -EINVAL;
++	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,
++		},
++	};
+ 
+-	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;
++	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
++	if (ret < 0) {
++		dev_warn(&client->dev, "Reading register %x from %x failed\n",
++			 addr, client->addr);
++		return ret;
++	}
+ 
+-	return 0;
++	return buf[0];
+ }
+ 
+-/* Write a list of registers */
+-static int imx219_write_regs(struct imx219 *imx219,
+-			     const struct imx219_reg *regs, u32 len)
++static int reg_write_table(struct i2c_client *client,
++			   const struct imx219_reg table[])
+ {
+-	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);
++	struct i2c_client *client = v4l2_get_subdevdata(sd);
++	struct imx219 *priv = to_imx219(client);
++	u8 reg = 0x00;
++	int ret;
+ 
+-	for (i = 0; i < ARRAY_SIZE(codes); i++)
+-		if (codes[i] == code)
+-			break;
++	if (!enable)
++		return reg_write_table(client, stop);
+ 
+-	if (i >= ARRAY_SIZE(codes))
+-		i = 0;
++	ret = reg_write_table(client, priv->cur_mode->reg_list);
++	if (ret)
++		return ret;
+ 
+-	i = (i & ~3) | (imx219->vflip->val ? 2 : 0) |
+-	    (imx219->hflip->val ? 1 : 0);
++	/* 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);
+ 
+-	return codes[i];
+-}
++	if (ret)
++		return ret;
+ 
+-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;
+-}
++	/* Handle flip/mirror */
++	if (priv->hflip)
++		reg |= 0x1;
++	if (priv->vflip)
++		reg |= 0x2;
+ 
+-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;
++	ret = reg_write(client, 0x0172, reg);
++	if (ret)
++		return ret;
+ 
+-	mutex_lock(&imx219->mutex);
++	/* 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);
++	}
+ 
+-	/* 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;
++	priv->cur_vts = priv->cur_mode->vts_def - IMX219_EXP_LINES_MARGIN;
++	if (ret)
++		return ret;
+ 
+-	/* 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;
++	return reg_write_table(client, start);
++}
+ 
+-	mutex_unlock(&imx219->mutex);
++/* 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;
+ }
+ 
+-static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
++/* V4L2 ctrl operations */
++static int imx219_s_ctrl_test_pattern(struct v4l2_ctrl *ctrl)
+ {
+-	struct imx219 *imx219 =
+-		container_of(ctrl->handler, struct imx219, ctrl_handler);
+-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+-	int ret;
+-
+-	if (ctrl->id == V4L2_CID_VBLANK) {
+-		int exposure_max, exposure_def;
+-
+-		/* 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);
+-	}
+-
+-	/*
+-	 * Applying V4L2 control value only happens
+-	 * when power is up for streaming
+-	 */
+-	if (pm_runtime_get_if_in_use(&client->dev) == 0)
+-		return 0;
++	struct imx219 *priv =
++	    container_of(ctrl->handler, struct imx219, ctrl_handler);
+ 
+-	switch (ctrl->id) {
+-	case V4L2_CID_ANALOGUE_GAIN:
+-		ret = imx219_write_reg(imx219, IMX219_REG_ANALOG_GAIN,
+-				       IMX219_REG_VALUE_08BIT, ctrl->val);
++	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;
+-	default:
+-		dev_info(&client->dev,
+-			 "ctrl(id:0x%x,val:0x%x) is not handled\n",
+-			 ctrl->id, ctrl->val);
+-		ret = -EINVAL;
++	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:
++		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_enum_mbus_code(struct v4l2_subdev *sd,
+-				 struct v4l2_subdev_state *sd_state,
+-				 struct v4l2_subdev_mbus_code_enum *code)
++static int imx219_g_frame_interval(struct v4l2_subdev *sd,
++				   struct v4l2_subdev_frame_interval *fi)
+ {
+-	struct imx219 *imx219 = to_imx219(sd);
+-
+-	if (code->index >= (ARRAY_SIZE(codes) / 4))
+-		return -EINVAL;
++	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);
+-	code->code = imx219_get_format_code(imx219, codes[code->index * 4]);
+-	mutex_unlock(&imx219->mutex);
++	fi->interval = mode->max_fps;
+ 
+ 	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_s_ctrl(struct v4l2_ctrl *ctrl)
+ {
+-	struct imx219 *imx219 = to_imx219(sd);
+-	u32 code;
++	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;
+ 
+-	if (fse->index >= ARRAY_SIZE(supported_modes))
+-		return -EINVAL;
++	switch (ctrl->id) {
++	case V4L2_CID_HFLIP:
++		priv->hflip = ctrl->val;
++		break;
+ 
+-	mutex_lock(&imx219->mutex);
+-	code = imx219_get_format_code(imx219, fse->code);
+-	mutex_unlock(&imx219->mutex);
+-	if (fse->code != code)
+-		return -EINVAL;
++	case V4L2_CID_VFLIP:
++		priv->vflip = ctrl->val;
++		break;
+ 
+-	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;
++	case V4L2_CID_ANALOGUE_GAIN:
++	case V4L2_CID_GAIN:
++		/*
++		 * hal transfer (gain * 256)  to kernel
++		 * than divide into analog gain & digital gain in kernel
++		 */
+ 
+-	return 0;
+-}
++		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;
++		}
+ 
+-static void imx219_reset_colorspace(struct v4l2_mbus_framefmt *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);
+-}
++		/*
++		 * 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;
+ 
+-static void imx219_update_pad_format(struct imx219 *imx219,
+-				     const struct imx219_mode *mode,
+-				     struct v4l2_subdev_format *fmt)
+-{
+-	fmt->format.width = mode->width;
+-	fmt->format.height = mode->height;
+-	fmt->format.field = V4L2_FIELD_NONE;
+-	imx219_reset_colorspace(&fmt->format);
+-}
++		/*
++		 * 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;
+ 
+-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);
+-	}
++		/*
++		 * 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
++		 */
+ 
+-	return 0;
+-}
++		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);
+ 
+-static int imx219_get_pad_format(struct v4l2_subdev *sd,
+-				 struct v4l2_subdev_state *sd_state,
+-				 struct v4l2_subdev_format *fmt)
+-{
+-	struct imx219 *imx219 = to_imx219(sd);
+-	int ret;
++		return ret;
+ 
+-	mutex_lock(&imx219->mutex);
+-	ret = __imx219_get_pad_format(imx219, sd_state, fmt);
+-	mutex_unlock(&imx219->mutex);
++	case V4L2_CID_EXPOSURE:
++		priv->exposure_time = ctrl->val;
+ 
+-	return ret;
+-}
++		ret = reg_write(client, 0x015a, priv->exposure_time >> 8);
++		ret |= reg_write(client, 0x015b, priv->exposure_time & 0xff);
++		return ret;
+ 
+-static int imx219_set_pad_format(struct v4l2_subdev *sd,
+-				 struct v4l2_subdev_state *sd_state,
+-				 struct v4l2_subdev_format *fmt)
+-{
+-	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;
+-
+-	mutex_lock(&imx219->mutex);
+-
+-	for (i = 0; i < ARRAY_SIZE(codes); i++)
+-		if (codes[i] == fmt->format.code)
+-			break;
+-	if (i >= ARRAY_SIZE(codes))
+-		i = 0;
+-
+-	/* 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);
+-	}
++	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;
+ 
+-	mutex_unlock(&imx219->mutex);
++	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_set_framefmt(struct imx219 *imx219)
++static int imx219_enum_mbus_code(struct v4l2_subdev *sd,
++				 struct v4l2_subdev_state *sd_state,
++				 struct v4l2_subdev_mbus_code_enum *code)
+ {
+-	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));
+-
+-	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 (code->index != 0)
++		return -EINVAL;
++	code->code = MEDIA_BUS_FMT_SRGGB10_1X10;
+ 
+-	return -EINVAL;
++	return 0;
+ }
+ 
+-static int imx219_set_binning(struct imx219 *imx219)
++static int imx219_get_reso_dist(const struct imx219_mode *mode,
++				struct v4l2_mbus_framefmt *framefmt)
+ {
+-	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;
++	return abs(mode->width - framefmt->width) +
++	       abs(mode->height - framefmt->height);
+ }
+ 
+-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)
++static const struct imx219_mode *imx219_find_best_fit(
++					struct v4l2_subdev_format *fmt)
+ {
+-	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;
++	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 NULL;
++	return &supported_modes[cur_best_fit];
+ }
+ 
+-static int imx219_get_selection(struct v4l2_subdev *sd,
+-				struct v4l2_subdev_state *sd_state,
+-				struct v4l2_subdev_selection *sel)
++static int imx219_set_fmt(struct v4l2_subdev *sd,
++			  struct v4l2_subdev_state *sd_state,
++			  struct v4l2_subdev_format *fmt)
+ {
+-	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;
++	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;
+ 
+-	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;
+-	}
++	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;
++	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);
++
++	/* 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 -EINVAL;
++	return 0;
+ }
+ 
+-static int imx219_start_streaming(struct imx219 *imx219)
++static int imx219_get_fmt(struct v4l2_subdev *sd,
++			  struct v4l2_subdev_state *sd_state,
++			  struct v4l2_subdev_format *fmt)
+ {
+-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+-	const struct imx219_reg_list *reg_list;
+-	int ret;
+-
+-	ret = pm_runtime_resume_and_get(&client->dev);
+-	if (ret < 0)
+-		return ret;
++	struct i2c_client *client = v4l2_get_subdevdata(sd);
++	struct imx219 *priv = to_imx219(client);
++	const struct imx219_mode *mode = priv->cur_mode;
+ 
+-	/* 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;
+-	}
+-
+-	/* 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 = 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 = imx219_set_binning(imx219);
+-	if (ret) {
+-		dev_err(&client->dev, "%s failed to set binning: %d\n",
+-			__func__, ret);
+-		goto err_rpm_put;
+-	}
+-
+-	/* Apply customized values from user */
+-	ret =  __v4l2_ctrl_handler_setup(imx219->sd.ctrl_handler);
+-	if (ret)
+-		goto err_rpm_put;
+-
+-	/* 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;
++	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
++		return 0;
+ 
+-	/* vflip and hflip cannot change during streaming */
+-	__v4l2_ctrl_grab(imx219->vflip, true);
+-	__v4l2_ctrl_grab(imx219->hflip, true);
++	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;
+-
+-err_rpm_put:
+-	pm_runtime_put(&client->dev);
+-	return ret;
+ }
+ 
+-static void imx219_stop_streaming(struct imx219 *imx219)
++static void imx219_get_module_inf(struct imx219 *imx219,
++				  struct rkmodule_inf *inf)
+ {
+-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+-	int ret;
+-
+-	/* 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_grab(imx219->vflip, false);
+-	__v4l2_ctrl_grab(imx219->hflip, false);
+-
+-	pm_runtime_put(&client->dev);
++	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 int imx219_set_stream(struct v4l2_subdev *sd, int enable)
++static long imx219_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+ {
+-	struct imx219 *imx219 = to_imx219(sd);
+-	int ret = 0;
+-
+-	mutex_lock(&imx219->mutex);
+-	if (imx219->streaming == enable) {
+-		mutex_unlock(&imx219->mutex);
+-		return 0;
+-	}
++	struct i2c_client *client = v4l2_get_subdevdata(sd);
++	struct imx219 *imx219 = to_imx219(client);
++	long ret = 0;
+ 
+-	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);
++	switch (cmd) {
++	case RKMODULE_GET_MODULE_INFO:
++		imx219_get_module_inf(imx219, (struct rkmodule_inf *)arg);
++		break;
++	default:
++		ret = -ENOIOCTLCMD;
++		break;
+ 	}
+ 
+-	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)
++#ifdef CONFIG_COMPAT
++static long imx219_compat_ioctl32(struct v4l2_subdev *sd,
++				  unsigned int cmd, unsigned long arg)
+ {
+-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+-	struct imx219 *imx219 = to_imx219(sd);
+-	int ret;
++	void __user *up = compat_ptr(arg);
++	struct rkmodule_inf *inf;
++	struct rkmodule_awb_cfg *cfg;
++	long ret;
++
++	switch (cmd) {
++	case RKMODULE_GET_MODULE_INFO:
++		inf = kzalloc(sizeof(*inf), GFP_KERNEL);
++		if (!inf) {
++			ret = -ENOMEM;
++			return 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 = 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;
++		}
+ 
+-	ret = clk_prepare_enable(imx219->xclk);
+-	if (ret) {
+-		dev_err(dev, "%s: failed to enable clock\n",
+-			__func__);
+-		goto reg_off;
++		ret = copy_from_user(cfg, up, sizeof(*cfg));
++		if (!ret)
++			ret = imx219_ioctl(sd, cmd, cfg);
++		kfree(cfg);
++		break;
++	default:
++		ret = -ENOIOCTLCMD;
++		break;
+ 	}
+ 
+-	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;
+ }
++#endif
+ 
+-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)
++static int imx219_g_mbus_config(struct v4l2_subdev *sd, unsigned int pad_id,
++				struct v4l2_mbus_config *config)
+ {
+-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+-	struct imx219 *imx219 = to_imx219(sd);
++	//struct i2c_client *client = v4l2_get_subdevdata(sd);
++	//struct imx219 *imx219 = to_imx219(client);
+ 
+-	if (imx219->streaming)
+-		imx219_stop_streaming(imx219);
++	config->type = V4L2_MBUS_CSI2_DPHY;
++	config->bus.mipi_csi2.num_data_lanes = IMX219_LANES;
+ 
+ 	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;
+-	}
+-
+-	return 0;
+-
+-error:
+-	imx219_stop_streaming(imx219);
+-	imx219->streaming = false;
+-
+-	return ret;
+-}
+-
+-static int imx219_get_regulators(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)
+ {
+-	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+-	unsigned int i;
++	struct i2c_client *client = v4l2_get_subdevdata(sd);
++	struct imx219 *priv = to_imx219(client);
+ 
+-	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);
+-	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;
+-	}
++	if (fie->index >= priv->cfg_num)
++		return -EINVAL;
+ 
+-	if (val != IMX219_CHIP_ID) {
+-		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+-			IMX219_CHIP_ID, val);
+-		return -EIO;
+-	}
++	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 const struct v4l2_subdev_core_ops imx219_core_ops = {
+-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
++/* 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 const struct v4l2_subdev_video_ops imx219_video_ops = {
+-	.s_stream = imx219_set_stream,
++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_pad_ops = {
++static const struct v4l2_subdev_pad_ops imx219_subdev_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,
++	.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 const struct v4l2_subdev_ops imx219_subdev_ops = {
+-	.core = &imx219_core_ops,
+-	.video = &imx219_video_ops,
+-	.pad = &imx219_pad_ops,
++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_subdev_internal_ops imx219_internal_ops = {
+-	.open = imx219_open,
++static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
++	.s_ctrl = imx219_s_ctrl,
+ };
+ 
+-/* Initialize control handlers */
+-static int imx219_init_controls(struct imx219 *imx219)
++static int imx219_video_probe(struct i2c_client *client)
+ {
+-	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)
++	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
++	u16 model_id;
++	u32 lot_id;
++	u16 chip_id;
++	int ret;
++
++	ret = imx219_s_power(subdev, 1);
++	if (ret < 0)
+ 		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 */
++	/* 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;
+ 
+-	if (ctrl_hdlr->error) {
+-		ret = ctrl_hdlr->error;
+-		dev_err(&client->dev, "%s control init failed (%d)\n",
+-			__func__, ret);
+-		goto error;
++	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 = v4l2_fwnode_device_parse(&client->dev, &props);
+-	if (ret)
+-		goto error;
++	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 = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx219_ctrl_ops,
+-					      &props);
+-	if (ret)
+-		goto error;
++	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;
+ 
+-	imx219->sd.ctrl_handler = ctrl_hdlr;
++	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;
+ 
+-	return 0;
++	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;
+ 
+-error:
+-	v4l2_ctrl_handler_free(ctrl_hdlr);
+-	mutex_destroy(&imx219->mutex);
++	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;
+ 
++	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_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)
++static int imx219_ctrls_init(struct v4l2_subdev *sd)
+ {
+-	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;
+-	}
++	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;
++
++	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);
++
++	/* 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);
++
++	/* 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);
++
++	/* 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);
++
++	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);
+ 
+-	/* 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;
++	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 (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 = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
++	if (ret < 0) {
++		dev_err(&client->dev, "Error %d setting default controls\n",
++			ret);
++		goto error;
+ 	}
+ 
+-	ret = 0;
+-
+-error_out:
+-	v4l2_fwnode_endpoint_free(&ep_cfg);
+-	fwnode_handle_put(endpoint);
+-
++	return 0;
++error:
++	v4l2_ctrl_handler_free(&priv->ctrl_handler);
+ 	return ret;
+ }
+ 
+-static int imx219_probe(struct i2c_client *client)
++static int imx219_probe(struct i2c_client *client,
++			const struct i2c_device_id *did)
+ {
++	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+ 	struct device *dev = &client->dev;
+-	struct imx219 *imx219;
++	struct device_node *node = dev->of_node;
++	struct imx219 *priv;
++	struct v4l2_subdev *sd;
++	char facing[2];
+ 	int ret;
+ 
+-	imx219 = devm_kzalloc(&client->dev, sizeof(*imx219), GFP_KERNEL);
+-	if (!imx219)
+-		return -ENOMEM;
+-
+-	v4l2_i2c_subdev_init(&imx219->sd, client, &imx219_subdev_ops);
++	dev_info(dev, "driver version: %02x.%02x.%02x",
++		DRIVER_VERSION >> 16,
++		(DRIVER_VERSION & 0xff00) >> 8,
++		DRIVER_VERSION & 0x00ff);
+ 
+-	/* 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);
++	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;
+ 	}
++	priv = devm_kzalloc(&client->dev, sizeof(struct imx219), GFP_KERNEL);
++	if (!priv)
++		return -ENOMEM;
+ 
+-	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);
+-
+-	/*
+-	 * 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)
+-		return ret;
+-
+-	ret = imx219_identify_module(imx219);
+-	if (ret)
+-		goto error_power_off;
++	/* 1920 * 1080 by default */
++	priv->cur_mode = &supported_modes[1];
++	priv->cfg_num = ARRAY_SIZE(supported_modes);
+ 
+-	/* Set default mode to max resolution */
+-	imx219->mode = &supported_modes[0];
++	// 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;
+ 
+-	/* 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);
++	v4l2_i2c_subdev_init(&priv->subdev, client, &imx219_subdev_ops);
++	ret = imx219_ctrls_init(&priv->subdev);
+ 	if (ret < 0)
+-		goto error_power_off;
+-	usleep_range(100, 110);
+-
+-	/* put sensor back to standby mode */
+-	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
+-			       IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
++		return ret;
++	ret = imx219_video_probe(client);
+ 	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);
++		return ret;
+ 
+-error_handler_free:
+-	imx219_free_controls(imx219);
++	priv->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
++		     V4L2_SUBDEV_FL_HAS_EVENTS;
+ 
+-error_power_off:
+-	imx219_power_off(dev);
++	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)
++		return ret;
+ 
++	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)
++		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[] = {
+-	{ .compatible = "sony,imx219" },
+-	{ /* sentinel */ }
++static const struct i2c_device_id imx219_id[] = {
++	{"imx219", 0},
++	{}
+ };
+-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)
++static const struct of_device_id imx219_of_match[] = {
++	{ .compatible = "sony,imx219" },
++	{ /* 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_LICENSE("GPL v2");
++MODULE_DESCRIPTION("Sony IMX219 Camera driver");
++MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
++MODULE_LICENSE("GPL v2");
+\ No newline at end of file
+diff --git a/drivers/media/i2c/ov5647.c b/drivers/media/i2c/ov5647.c
+index 847a7bbb69c54..8b6870b838f38 100644
+--- a/drivers/media/i2c/ov5647.c
++++ b/drivers/media/i2c/ov5647.c
+@@ -20,8 +20,11 @@
+ #include <linux/module.h>
+ #include <linux/of_graph.h>
+ #include <linux/pm_runtime.h>
++#include <linux/regulator/consumer.h>
+ #include <linux/slab.h>
+ #include <linux/videodev2.h>
++#include <linux/version.h>
++#include <linux/rk-camera-module.h>
+ #include <media/v4l2-ctrls.h>
+ #include <media/v4l2-device.h>
+ #include <media/v4l2-event.h>
+@@ -29,12 +32,15 @@
+ #include <media/v4l2-image-sizes.h>
+ #include <media/v4l2-mediabus.h>
+ 
++#define DRIVER_VERSION			KERNEL_VERSION(0, 0x01, 0x1)
++
+ /*
+  * From the datasheet, "20ms after PWDN goes low or 20ms after RESETB goes
+  * high if reset is inserted after PWDN goes high, host can access sensor's
+  * SCCB to initialize sensor."
+  */
+-#define PWDN_ACTIVE_DELAY_MS	20
++#define PWDN_ACTIVE_DELAY_MS	500
++#define OV5647_LANES  2
+ 
+ #define MIPI_CTRL00_CLOCK_LANE_GATE		BIT(5)
+ #define MIPI_CTRL00_LINE_SYNC_ENABLE		BIT(4)
+@@ -54,6 +60,8 @@
+ #define OV5647_REG_GAIN_LO		0x350b
+ #define OV5647_REG_VTS_HI		0x380e
+ #define OV5647_REG_VTS_LO		0x380f
++#define OV5647_REG_VFLIP		0x3820
++#define OV5647_REG_HFLIP		0x3821
+ #define OV5647_REG_FRAME_OFF_NUMBER	0x4202
+ #define OV5647_REG_MIPI_CTRL00		0x4800
+ #define OV5647_REG_MIPI_CTRL14		0x4814
+@@ -68,11 +76,11 @@
+ #define OV5647_NATIVE_HEIGHT		1956U
+ 
+ #define OV5647_PIXEL_ARRAY_LEFT		16U
+-#define OV5647_PIXEL_ARRAY_TOP		16U
++#define OV5647_PIXEL_ARRAY_TOP		6U
+ #define OV5647_PIXEL_ARRAY_WIDTH	2592U
+ #define OV5647_PIXEL_ARRAY_HEIGHT	1944U
+ 
+-#define OV5647_VBLANK_MIN		4
++#define OV5647_VBLANK_MIN		24
+ #define OV5647_VTS_MAX			32767
+ 
+ #define OV5647_EXPOSURE_MIN		4
+@@ -80,6 +88,8 @@
+ #define OV5647_EXPOSURE_DEFAULT		1000
+ #define OV5647_EXPOSURE_MAX		65535
+ 
++#define OV5647_NAME			"ov5647"
++
+ struct regval_list {
+ 	u16 addr;
+ 	u8 data;
+@@ -87,10 +97,12 @@ struct regval_list {
+ 
+ struct ov5647_mode {
+ 	struct v4l2_mbus_framefmt	format;
++	struct v4l2_fract max_fps;
+ 	struct v4l2_rect		crop;
+ 	u64				pixel_rate;
+ 	int				hts;
+ 	int				vts;
++	int				link_freq_index;
+ 	const struct regval_list	*reg_list;
+ 	unsigned int			num_regs;
+ };
+@@ -101,6 +113,7 @@ struct ov5647 {
+ 	struct mutex			lock;
+ 	struct clk			*xclk;
+ 	struct gpio_desc		*pwdn;
++	struct gpio_desc		*reset;
+ 	bool				clock_ncont;
+ 	struct v4l2_ctrl_handler	ctrls;
+ 	const struct ov5647_mode	*mode;
+@@ -108,7 +121,14 @@ struct ov5647 {
+ 	struct v4l2_ctrl		*hblank;
+ 	struct v4l2_ctrl		*vblank;
+ 	struct v4l2_ctrl		*exposure;
++	struct v4l2_ctrl		*hflip;
++	struct v4l2_ctrl		*vflip;
++	struct v4l2_ctrl		*link_freq;
+ 	bool				streaming;
++	u32			module_index;
++	const char		*module_facing;
++	const char		*module_name;
++	const char		*len_name;
+ };
+ 
+ static inline struct ov5647 *to_sensor(struct v4l2_subdev *sd)
+@@ -136,7 +156,7 @@ static struct regval_list ov5647_2592x1944_10bpp[] = {
+ 	{0x3036, 0x69},
+ 	{0x303c, 0x11},
+ 	{0x3106, 0xf5},
+-	{0x3821, 0x06},
++	{0x3821, 0x00},
+ 	{0x3820, 0x00},
+ 	{0x3827, 0xec},
+ 	{0x370c, 0x03},
+@@ -225,7 +245,7 @@ static struct regval_list ov5647_1080p30_10bpp[] = {
+ 	{0x3036, 0x62},
+ 	{0x303c, 0x11},
+ 	{0x3106, 0xf5},
+-	{0x3821, 0x06},
++	{0x3821, 0x00},
+ 	{0x3820, 0x00},
+ 	{0x3827, 0xec},
+ 	{0x370c, 0x03},
+@@ -389,7 +409,7 @@ static struct regval_list ov5647_2x2binned_10bpp[] = {
+ 	{0x4800, 0x24},
+ 	{0x3503, 0x03},
+ 	{0x3820, 0x41},
+-	{0x3821, 0x07},
++	{0x3821, 0x01},
+ 	{0x350a, 0x00},
+ 	{0x350b, 0x10},
+ 	{0x3500, 0x00},
+@@ -405,7 +425,7 @@ static struct regval_list ov5647_640x480_10bpp[] = {
+ 	{0x3035, 0x11},
+ 	{0x3036, 0x46},
+ 	{0x303c, 0x11},
+-	{0x3821, 0x07},
++	{0x3821, 0x01},
+ 	{0x3820, 0x41},
+ 	{0x370c, 0x03},
+ 	{0x3612, 0x59},
+@@ -489,16 +509,28 @@ static struct regval_list ov5647_640x480_10bpp[] = {
+ 	{0x0100, 0x01},
+ };
+ 
++
++static const s64 link_freq_menu_items[] = {
++	175000000,
++	163333400,
++	163333400,
++	110000000,
++};
++
+ static const struct ov5647_mode ov5647_modes[] = {
+ 	/* 2592x1944 full resolution full FOV 10-bit mode. */
+ 	{
+ 		.format = {
+ 			.code		= MEDIA_BUS_FMT_SBGGR10_1X10,
+-			.colorspace	= V4L2_COLORSPACE_SRGB,
++			.colorspace	= V4L2_COLORSPACE_RAW,
+ 			.field		= V4L2_FIELD_NONE,
+ 			.width		= 2592,
+ 			.height		= 1944
+ 		},
++		.max_fps = {
++			.numerator = 10000,
++			.denominator = 150000,
++		},
+ 		.crop = {
+ 			.left		= OV5647_PIXEL_ARRAY_LEFT,
+ 			.top		= OV5647_PIXEL_ARRAY_TOP,
+@@ -508,6 +540,7 @@ static const struct ov5647_mode ov5647_modes[] = {
+ 		.pixel_rate	= 87500000,
+ 		.hts		= 2844,
+ 		.vts		= 0x7b0,
++		.link_freq_index = 0,
+ 		.reg_list	= ov5647_2592x1944_10bpp,
+ 		.num_regs	= ARRAY_SIZE(ov5647_2592x1944_10bpp)
+ 	},
+@@ -515,11 +548,15 @@ static const struct ov5647_mode ov5647_modes[] = {
+ 	{
+ 		.format = {
+ 			.code		= MEDIA_BUS_FMT_SBGGR10_1X10,
+-			.colorspace	= V4L2_COLORSPACE_SRGB,
++			.colorspace	= V4L2_COLORSPACE_RAW,
+ 			.field		= V4L2_FIELD_NONE,
+ 			.width		= 1920,
+ 			.height		= 1080
+ 		},
++		.max_fps = {
++			.numerator = 10000,
++			.denominator = 300000,
++		},
+ 		.crop = {
+ 			.left		= 348 + OV5647_PIXEL_ARRAY_LEFT,
+ 			.top		= 434 + OV5647_PIXEL_ARRAY_TOP,
+@@ -529,6 +566,7 @@ static const struct ov5647_mode ov5647_modes[] = {
+ 		.pixel_rate	= 81666700,
+ 		.hts		= 2416,
+ 		.vts		= 0x450,
++		.link_freq_index = 1,
+ 		.reg_list	= ov5647_1080p30_10bpp,
+ 		.num_regs	= ARRAY_SIZE(ov5647_1080p30_10bpp)
+ 	},
+@@ -536,11 +574,15 @@ static const struct ov5647_mode ov5647_modes[] = {
+ 	{
+ 		.format = {
+ 			.code		= MEDIA_BUS_FMT_SBGGR10_1X10,
+-			.colorspace	= V4L2_COLORSPACE_SRGB,
++			.colorspace	= V4L2_COLORSPACE_RAW,
+ 			.field		= V4L2_FIELD_NONE,
+ 			.width		= 1296,
+ 			.height		= 972
+ 		},
++		.max_fps = {
++			.numerator = 10000,
++			.denominator = 450000,
++		},
+ 		.crop = {
+ 			.left		= OV5647_PIXEL_ARRAY_LEFT,
+ 			.top		= OV5647_PIXEL_ARRAY_TOP,
+@@ -550,6 +592,7 @@ static const struct ov5647_mode ov5647_modes[] = {
+ 		.pixel_rate	= 81666700,
+ 		.hts		= 1896,
+ 		.vts		= 0x59b,
++		.link_freq_index = 2,
+ 		.reg_list	= ov5647_2x2binned_10bpp,
+ 		.num_regs	= ARRAY_SIZE(ov5647_2x2binned_10bpp)
+ 	},
+@@ -557,11 +600,15 @@ static const struct ov5647_mode ov5647_modes[] = {
+ 	{
+ 		.format = {
+ 			.code		= MEDIA_BUS_FMT_SBGGR10_1X10,
+-			.colorspace	= V4L2_COLORSPACE_SRGB,
++			.colorspace	= V4L2_COLORSPACE_RAW,
+ 			.field		= V4L2_FIELD_NONE,
+ 			.width		= 640,
+ 			.height		= 480
+ 		},
++		.max_fps = {
++			.numerator = 10000,
++			.denominator = 900000,
++		},
+ 		.crop = {
+ 			.left		= 16 + OV5647_PIXEL_ARRAY_LEFT,
+ 			.top		= OV5647_PIXEL_ARRAY_TOP,
+@@ -571,14 +618,15 @@ static const struct ov5647_mode ov5647_modes[] = {
+ 		.pixel_rate	= 55000000,
+ 		.hts		= 1852,
+ 		.vts		= 0x1f8,
++		.link_freq_index = 3,
+ 		.reg_list	= ov5647_640x480_10bpp,
+ 		.num_regs	= ARRAY_SIZE(ov5647_640x480_10bpp)
+ 	},
+ };
+ 
+ /* Default sensor mode is 2x2 binned 640x480 SBGGR10_1X10. */
+-#define OV5647_DEFAULT_MODE	(&ov5647_modes[3])
+-#define OV5647_DEFAULT_FORMAT	(ov5647_modes[3].format)
++#define OV5647_DEFAULT_MODE	(&ov5647_modes[0])
++#define OV5647_DEFAULT_FORMAT	(ov5647_modes[0].format)
+ 
+ static int ov5647_write16(struct v4l2_subdev *sd, u16 reg, u16 val)
+ {
+@@ -587,7 +635,13 @@ static int ov5647_write16(struct v4l2_subdev *sd, u16 reg, u16 val)
+ 	int ret;
+ 
+ 	ret = i2c_master_send(client, data, 4);
+-	if (ret < 0) {
++	/*
++	 * Writing the wrong number of bytes also needs to be flagged as an
++	 * error. Success needs to produce a 0 return code.
++	 */
++	if (ret == 4) {
++		ret = 0;
++	} else {
+ 		dev_dbg(&client->dev, "%s: i2c write error, reg: %x\n",
+ 			__func__, reg);
+ 		return ret;
+@@ -603,10 +657,17 @@ static int ov5647_write(struct v4l2_subdev *sd, u16 reg, u8 val)
+ 	int ret;
+ 
+ 	ret = i2c_master_send(client, data, 3);
+-	if (ret < 0) {
++	/*
++	 * Writing the wrong number of bytes also needs to be flagged as an
++	 * error. Success needs to produce a 0 return code.
++	 */
++	if (ret == 3) {
++		ret = 0;
++	} else {
+ 		dev_dbg(&client->dev, "%s: i2c write error, reg: %x\n",
+ 				__func__, reg);
+-		return ret;
++		if (ret >= 0)
++			ret = -EINVAL;
+ 	}
+ 
+ 	return 0;
+@@ -619,17 +680,30 @@ static int ov5647_read(struct v4l2_subdev *sd, u16 reg, u8 *val)
+ 	int ret;
+ 
+ 	ret = i2c_master_send(client, data_w, 2);
+-	if (ret < 0) {
++	/*
++	 * A negative return code, or sending the wrong number of bytes, both
++	 * count as an error.
++	 */
++	if (ret != 2) {
+ 		dev_dbg(&client->dev, "%s: i2c write error, reg: %x\n",
+ 			__func__, reg);
++		if (ret >= 0)
++			ret = -EINVAL;
+ 		return ret;
+ 	}
+ 
+ 	ret = i2c_master_recv(client, val, 1);
+-	if (ret < 0) {
++	/*
++	 * The only return value indicating success is 1. Anything else, even
++	 * a non-negative value, indicates something went wrong.
++	 */
++	if (ret == 1) {
++		ret = 0;
++	} else {
+ 		dev_dbg(&client->dev, "%s: i2c read error, reg: %x\n",
+ 				__func__, reg);
+-		return ret;
++		if (ret >= 0)
++			ret = -EINVAL;
+ 	}
+ 
+ 	return 0;
+@@ -755,7 +829,7 @@ static int ov5647_power_on(struct device *dev)
+ 	struct ov5647 *sensor = dev_get_drvdata(dev);
+ 	int ret;
+ 
+-	dev_dbg(dev, "OV5647 power on\n");
++	dev_info(dev, "OV5647 power on\n");
+ 
+ 	if (sensor->pwdn) {
+ 		gpiod_set_value_cansleep(sensor->pwdn, 0);
+@@ -798,7 +872,7 @@ static int ov5647_power_off(struct device *dev)
+ 	u8 rdval;
+ 	int ret;
+ 
+-	dev_dbg(dev, "OV5647 power off\n");
++	dev_info(dev, "OV5647 power off\n");
+ 
+ 	ret = ov5647_write_array(&sensor->sd, sensor_oe_disable_regs,
+ 				 ARRAY_SIZE(sensor_oe_disable_regs));
+@@ -845,6 +919,33 @@ static int ov5647_sensor_set_register(struct v4l2_subdev *sd,
+ }
+ #endif
+ 
++static void ov5647_get_module_inf(struct ov5647 *sensor,
++				  struct rkmodule_inf *inf)
++{
++	memset(inf, 0, sizeof(*inf));
++	strscpy(inf->base.sensor, OV5647_NAME, sizeof(inf->base.sensor));
++	strscpy(inf->base.module, sensor->module_name,
++		sizeof(inf->base.module));
++	strscpy(inf->base.lens, sensor->len_name, sizeof(inf->base.lens));
++}
++
++static long ov5647_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
++{
++	struct ov5647 *sensor = to_sensor(sd);
++	long ret = 0;
++
++	switch (cmd) {
++	case RKMODULE_GET_MODULE_INFO:
++		ov5647_get_module_inf(sensor, (struct rkmodule_inf *)arg);
++		break;
++	default:
++		ret = -ENOIOCTLCMD;
++		break;
++	}
++
++	return ret;
++}
++
+ /* Subdev core operations registration */
+ static const struct v4l2_subdev_core_ops ov5647_subdev_core_ops = {
+ 	.subscribe_event	= v4l2_ctrl_subdev_subscribe_event,
+@@ -853,23 +954,9 @@ static const struct v4l2_subdev_core_ops ov5647_subdev_core_ops = {
+ 	.g_register		= ov5647_sensor_get_register,
+ 	.s_register		= ov5647_sensor_set_register,
+ #endif
++	.ioctl	= ov5647_ioctl,
+ };
+ 
+-static const struct v4l2_rect *
+-__ov5647_get_pad_crop(struct ov5647 *ov5647,
+-		      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(&ov5647->sd, sd_state, pad);
+-	case V4L2_SUBDEV_FORMAT_ACTIVE:
+-		return &ov5647->mode->crop;
+-	}
+-
+-	return NULL;
+-}
+-
+ static int ov5647_s_stream(struct v4l2_subdev *sd, int enable)
+ {
+ 	struct i2c_client *client = v4l2_get_subdevdata(sd);
+@@ -883,16 +970,26 @@ static int ov5647_s_stream(struct v4l2_subdev *sd, int enable)
+ 	}
+ 
+ 	if (enable) {
+-		ret = pm_runtime_resume_and_get(&client->dev);
++		dev_info(&client->dev, "stream start\n");
++		ret = pm_runtime_resume(&client->dev);
+ 		if (ret < 0)
++		{
++			dev_err(&client->dev, "pm_runtime_resume failed: %d\n", ret);
+ 			goto error_unlock;
+-
++		}
++		ret = pm_runtime_get(&client->dev);
++		if (ret < 0)
++		{
++			dev_err(&client->dev, "pm_runtime_get failed: %d\n", ret);
++			goto error_unlock;
++		}
+ 		ret = ov5647_stream_on(sd);
+ 		if (ret < 0) {
+ 			dev_err(&client->dev, "stream start failed: %d\n", ret);
+ 			goto error_pm;
+ 		}
+ 	} else {
++		dev_info(&client->dev, "stream stop\n");
+ 		ret = ov5647_stream_off(sd);
+ 		if (ret < 0) {
+ 			dev_err(&client->dev, "stream stop failed: %d\n", ret);
+@@ -908,16 +1005,68 @@ static int ov5647_s_stream(struct v4l2_subdev *sd, int enable)
+ 
+ error_pm:
+ 	pm_runtime_put(&client->dev);
+-error_unlock:
++error_unlock:    
+ 	mutex_unlock(&sensor->lock);
+ 
+ 	return ret;
+ }
+ 
++static int ov5647_g_frame_interval(struct v4l2_subdev *sd,
++				   struct v4l2_subdev_frame_interval *fi)
++{
++	struct ov5647 *sensor = to_sensor(sd);
++	const struct ov5647_mode *mode = sensor->mode;
++
++	mutex_lock(&sensor->lock);
++	fi->interval = mode->max_fps;
++	mutex_unlock(&sensor->lock);
++
++	return 0;
++}
++
++static int ov5647_g_mbus_config(struct v4l2_subdev *sd, unsigned int pad_id,
++				struct v4l2_mbus_config *config)
++{
++	config->type = V4L2_MBUS_CSI2_DPHY;
++	config->bus.mipi_csi2.num_data_lanes = OV5647_LANES;
++
++	return 0;
++}
++
++
++static int ov5647_g_input_status(struct v4l2_subdev *sd, u32 *status)
++{
++	*status = 0;
++	return 0;
++}
++
++
+ static const struct v4l2_subdev_video_ops ov5647_subdev_video_ops = {
+ 	.s_stream =		ov5647_s_stream,
++	.g_input_status = ov5647_g_input_status,
++	.g_frame_interval = ov5647_g_frame_interval,
++
+ };
+ 
++/* This function returns the mbus code for the current settings of the
++   HFLIP and VFLIP controls. */
++
++static u32 ov5647_get_mbus_code(struct v4l2_subdev *sd)
++{
++	struct ov5647 *sensor = to_sensor(sd);
++	/* The control values are only 0 or 1. */
++	int index =  sensor->hflip->val | (sensor->vflip->val << 1);
++
++	static const u32 codes[4] = {
++		MEDIA_BUS_FMT_SGBRG10_1X10,
++		MEDIA_BUS_FMT_SBGGR10_1X10,
++		MEDIA_BUS_FMT_SRGGB10_1X10,
++		MEDIA_BUS_FMT_SGRBG10_1X10
++	};
++
++	return codes[index];
++}
++
+ static int ov5647_enum_mbus_code(struct v4l2_subdev *sd,
+ 				 struct v4l2_subdev_state *sd_state,
+ 				 struct v4l2_subdev_mbus_code_enum *code)
+@@ -925,18 +1074,33 @@ static int ov5647_enum_mbus_code(struct v4l2_subdev *sd,
+ 	if (code->index > 0)
+ 		return -EINVAL;
+ 
+-	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
++	code->code = ov5647_get_mbus_code(sd);
+ 
+ 	return 0;
+ }
+ 
++static int ov5647_enum_frame_interval(struct v4l2_subdev *sd,
++				       struct v4l2_subdev_state *sd_state,
++				       struct v4l2_subdev_frame_interval_enum *fie)
++{
++	if (fie->index >= ARRAY_SIZE(ov5647_modes))
++		return -EINVAL;
++
++	fie->code = MEDIA_BUS_FMT_SBGGR10_1X10;
++
++	fie->width = ov5647_modes[fie->index].format.width;
++	fie->height = ov5647_modes[fie->index].format.height;
++	fie->interval = ov5647_modes[fie->index].max_fps;
++	return 0;
++}
++
+ static int ov5647_enum_frame_size(struct v4l2_subdev *sd,
+ 				  struct v4l2_subdev_state *sd_state,
+ 				  struct v4l2_subdev_frame_size_enum *fse)
+ {
+ 	const struct v4l2_mbus_framefmt *fmt;
+ 
+-	if (fse->code != MEDIA_BUS_FMT_SBGGR10_1X10 ||
++	if (fse->code != ov5647_get_mbus_code(sd) ||
+ 	    fse->index >= ARRAY_SIZE(ov5647_modes))
+ 		return -EINVAL;
+ 
+@@ -969,6 +1133,8 @@ static int ov5647_get_pad_fmt(struct v4l2_subdev *sd,
+ 	}
+ 
+ 	*fmt = *sensor_format;
++	/* The code we pass back must reflect the current h/vflips. */
++	fmt->code = ov5647_get_mbus_code(sd);
+ 	mutex_unlock(&sensor->lock);
+ 
+ 	return 0;
+@@ -1016,54 +1182,24 @@ static int ov5647_set_pad_fmt(struct v4l2_subdev *sd,
+ 					 exposure_def);
+ 	}
+ 	*fmt = mode->format;
++	/* The code we pass back must reflect the current h/vflips. */
++	fmt->code = ov5647_get_mbus_code(sd);
++
++	__v4l2_ctrl_s_ctrl(sensor->link_freq, mode->link_freq_index);
++	v4l2_info(sd, "%s res wxh:%dx%d, link freq:%llu", __func__,
++			fmt->width, fmt->height, link_freq_menu_items[mode->link_freq_index]);
+ 	mutex_unlock(&sensor->lock);
+ 
+ 	return 0;
+ }
+ 
+-static int ov5647_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 ov5647 *sensor = to_sensor(sd);
+-
+-		mutex_lock(&sensor->lock);
+-		sel->r = *__ov5647_get_pad_crop(sensor, sd_state, sel->pad,
+-						sel->which);
+-		mutex_unlock(&sensor->lock);
+-
+-		return 0;
+-	}
+-
+-	case V4L2_SEL_TGT_NATIVE_SIZE:
+-		sel->r.top = 0;
+-		sel->r.left = 0;
+-		sel->r.width = OV5647_NATIVE_WIDTH;
+-		sel->r.height = OV5647_NATIVE_HEIGHT;
+-
+-		return 0;
+-
+-	case V4L2_SEL_TGT_CROP_DEFAULT:
+-	case V4L2_SEL_TGT_CROP_BOUNDS:
+-		sel->r.top = OV5647_PIXEL_ARRAY_TOP;
+-		sel->r.left = OV5647_PIXEL_ARRAY_LEFT;
+-		sel->r.width = OV5647_PIXEL_ARRAY_WIDTH;
+-		sel->r.height = OV5647_PIXEL_ARRAY_HEIGHT;
+-
+-		return 0;
+-	}
+-
+-	return -EINVAL;
+-}
+-
+ static const struct v4l2_subdev_pad_ops ov5647_subdev_pad_ops = {
+ 	.enum_mbus_code		= ov5647_enum_mbus_code,
++	.enum_frame_interval	= ov5647_enum_frame_interval,
+ 	.enum_frame_size	= ov5647_enum_frame_size,
+ 	.set_fmt		= ov5647_set_pad_fmt,
+ 	.get_fmt		= ov5647_get_pad_fmt,
+-	.get_selection		= ov5647_get_selection,
++	.get_mbus_config	= ov5647_g_mbus_config,
+ };
+ 
+ static const struct v4l2_subdev_ops ov5647_subdev_ops = {
+@@ -1191,6 +1327,25 @@ static int ov5647_s_exposure(struct v4l2_subdev *sd, u32 val)
+ 	return ov5647_write(sd, OV5647_REG_EXP_LO, (val & 0xf) << 4);
+ }
+ 
++static int ov5647_s_flip( struct v4l2_subdev *sd, u16 reg, u32 ctrl_val)
++{
++	int ret;
++	u8 reg_val;
++
++	/* Set or clear bit 1 and leave everything else alone. */
++	ret = ov5647_read(sd, reg, &reg_val);
++	if (ret == 0) {
++		if (ctrl_val)
++			reg_val |= 2;
++		else
++			reg_val &= ~2;
++
++		ret = ov5647_write(sd, reg, reg_val);
++	}
++
++	return ret;
++}
++
+ static int ov5647_s_ctrl(struct v4l2_ctrl *ctrl)
+ {
+ 	struct ov5647 *sensor = container_of(ctrl->handler,
+@@ -1249,6 +1404,14 @@ static int ov5647_s_ctrl(struct v4l2_ctrl *ctrl)
+ 		/* Read-only, but we adjust it based on mode. */
+ 		break;
+ 
++	case V4L2_CID_HFLIP:
++		/* There's an in-built hflip in the sensor, so account for that here. */
++		ov5647_s_flip(sd, OV5647_REG_HFLIP, !ctrl->val);
++		break;
++	case V4L2_CID_VFLIP:
++		ov5647_s_flip(sd, OV5647_REG_VFLIP, ctrl->val);
++		break;
++
+ 	default:
+ 		dev_info(&client->dev,
+ 			 "Control (id:0x%x, val:0x%x) not supported\n",
+@@ -1265,13 +1428,18 @@ static const struct v4l2_ctrl_ops ov5647_ctrl_ops = {
+ 	.s_ctrl = ov5647_s_ctrl,
+ };
+ 
+-static int ov5647_init_controls(struct ov5647 *sensor)
++
++static int ov5647_init_controls(struct ov5647 *sensor, struct device *dev)
+ {
+ 	struct i2c_client *client = v4l2_get_subdevdata(&sensor->sd);
+ 	int hblank, exposure_max, exposure_def;
++	//struct v4l2_fwnode_device_properties props;
+ 
+ 	v4l2_ctrl_handler_init(&sensor->ctrls, 8);
+ 
++	sensor->link_freq = v4l2_ctrl_new_int_menu(&sensor->ctrls, NULL, V4L2_CID_LINK_FREQ,
++				      ARRAY_SIZE(link_freq_menu_items) - 1, 0, link_freq_menu_items);
++
+ 	v4l2_ctrl_new_std(&sensor->ctrls, &ov5647_ctrl_ops,
+ 			  V4L2_CID_AUTOGAIN, 0, 1, 1, 0);
+ 
+@@ -1314,6 +1482,16 @@ static int ov5647_init_controls(struct ov5647 *sensor)
+ 					   sensor->mode->vts -
+ 					   sensor->mode->format.height);
+ 
++	sensor->hflip = v4l2_ctrl_new_std(&sensor->ctrls, &ov5647_ctrl_ops,
++					  V4L2_CID_HFLIP, 0, 1, 1, 0);
++	if (sensor->hflip)
++		sensor->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
++
++	sensor->vflip = v4l2_ctrl_new_std(&sensor->ctrls, &ov5647_ctrl_ops,
++					  V4L2_CID_VFLIP, 0, 1, 1, 0);
++	if (sensor->vflip)
++		sensor->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
++
+ 	if (sensor->ctrls.error)
+ 		goto handler_free;
+ 
+@@ -1356,15 +1534,22 @@ static int ov5647_parse_dt(struct ov5647 *sensor, struct device_node *np)
+ 	return ret;
+ }
+ 
+-static int ov5647_probe(struct i2c_client *client)
++static int ov5647_probe(struct i2c_client *client,
++			const struct i2c_device_id *did)
+ {
+-	struct device_node *np = client->dev.of_node;
+ 	struct device *dev = &client->dev;
++	struct device_node *np = dev->of_node;
+ 	struct ov5647 *sensor;
+ 	struct v4l2_subdev *sd;
+ 	u32 xclk_freq;
++	char facing[2];
+ 	int ret;
+ 
++	dev_info(dev, "driver version: %02x.%02x.%02x",
++		DRIVER_VERSION >> 16,
++		(DRIVER_VERSION & 0xff00) >> 8,
++		DRIVER_VERSION & 0x00ff);
++
+ 	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+ 	if (!sensor)
+ 		return -ENOMEM;
+@@ -1377,6 +1562,19 @@ static int ov5647_probe(struct i2c_client *client)
+ 		}
+ 	}
+ 
++	ret = of_property_read_u32(np, RKMODULE_CAMERA_MODULE_INDEX,
++				   &sensor->module_index);
++	ret |= of_property_read_string(np, RKMODULE_CAMERA_MODULE_FACING,
++				       &sensor->module_facing);
++	ret |= of_property_read_string(np, RKMODULE_CAMERA_MODULE_NAME,
++				       &sensor->module_name);
++	ret |= of_property_read_string(np, RKMODULE_CAMERA_LENS_NAME,
++				       &sensor->len_name);
++	if (ret) {
++		dev_err(dev, "could not get module information!\n");
++		return -EINVAL;
++	}
++
+ 	sensor->xclk = devm_clk_get(dev, NULL);
+ 	if (IS_ERR(sensor->xclk)) {
+ 		dev_err(dev, "could not get xclk");
+@@ -1400,7 +1598,7 @@ static int ov5647_probe(struct i2c_client *client)
+ 
+ 	sensor->mode = OV5647_DEFAULT_MODE;
+ 
+-	ret = ov5647_init_controls(sensor);
++	ret = ov5647_init_controls(sensor, dev);
+ 	if (ret)
+ 		goto mutex_destroy;
+ 
+@@ -1423,7 +1621,16 @@ static int ov5647_probe(struct i2c_client *client)
+ 	if (ret < 0)
+ 		goto power_off;
+ 
+-	ret = v4l2_async_register_subdev(sd);
++	memset(facing, 0, sizeof(facing));
++	if (strcmp(sensor->module_facing, "back") == 0)
++		facing[0] = 'b';
++	else
++		facing[0] = 'f';
++
++	snprintf(sd->name, sizeof(sd->name), "m%02d_%s_%s %s",
++		 sensor->module_index, facing,
++		 OV5647_NAME, dev_name(sd->dev));
++	ret = v4l2_async_register_subdev_sensor(sd);
+ 	if (ret < 0)
+ 		goto power_off;
+ 
+@@ -1432,7 +1639,7 @@ static int ov5647_probe(struct i2c_client *client)
+ 	pm_runtime_enable(dev);
+ 	pm_runtime_idle(dev);
+ 
+-	dev_dbg(dev, "OmniVision OV5647 camera driver probed\n");
++	dev_info(dev, "OmniVision OV5647 camera driver probed\n");
+ 
+ 	return 0;
+ 
+@@ -1482,10 +1689,10 @@ MODULE_DEVICE_TABLE(of, ov5647_of_match);
+ static struct i2c_driver ov5647_driver = {
+ 	.driver = {
+ 		.of_match_table = of_match_ptr(ov5647_of_match),
+-		.name	= "ov5647",
++		.name	= OV5647_NAME,
+ 		.pm	= &ov5647_pm_ops,
+ 	},
+-	.probe_new	= ov5647_probe,
++	.probe		= ov5647_probe,
+ 	.remove		= ov5647_remove,
+ 	.id_table	= ov5647_id,
+ };
+@@ -1493,5 +1700,7 @@ static struct i2c_driver ov5647_driver = {
+ module_i2c_driver(ov5647_driver);
+ 
+ MODULE_AUTHOR("Ramiro Oliveira <roliveir@synopsys.com>");
++MODULE_AUTHOR("Modify by abel <guilin1985@gmail.com>");
++MODULE_AUTHOR("Stephen Chen <stephen@radxa.com>");
+ MODULE_DESCRIPTION("A low-level driver for OmniVision ov5647 sensors");
+ MODULE_LICENSE("GPL v2");