From 1543e317f1da31b75942316931e8f491a8920811 Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Thu, 04 Jan 2024 10:08:02 +0000
Subject: [PATCH] disable FB

---
 kernel/drivers/media/i2c/imx219.c | 2267 +++++++++++++++++++++++++++++++++++------------------------
 1 files changed, 1,332 insertions(+), 935 deletions(-)

diff --git a/kernel/drivers/media/i2c/imx219.c b/kernel/drivers/media/i2c/imx219.c
index d5254dc..b975636 100644
--- a/kernel/drivers/media/i2c/imx219.c
+++ b/kernel/drivers/media/i2c/imx219.c
@@ -1,1147 +1,1544 @@
+// SPDX-License-Identifier: GPL-2.0
 /*
- * Driver for IMX219 CMOS Image Sensor from Sony
+ * A V4L2 driver for Sony IMX219 cameras.
+ * Copyright (C) 2019, Raspberry Pi (Trading) Ltd
  *
- * Copyright (C) 2014, Andrew Chew <achew@nvidia.com>
+ * Based on Sony imx258 camera driver
+ * Copyright (C) 2018 Intel Corporation
  *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- * V0.0X01.0X01 add enum_frame_interval function.
- * V0.0X01.0X02 add function g_mbus_config.
+ * 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
+ *
  */
+
 #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/of_graph.h>
-#include <linux/slab.h>
-#include <linux/videodev2.h>
-#include <linux/version.h>
-#include <linux/rk-camera-module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.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 DRIVER_VERSION			KERNEL_VERSION(0, 0x01, 0x2)
+#define IMX219_REG_VALUE_08BIT		1
+#define IMX219_REG_VALUE_16BIT		2
 
-/* 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)
+#define IMX219_REG_MODE_SELECT		0x0100
+#define IMX219_MODE_STANDBY		0x00
+#define IMX219_MODE_STREAMING		0x01
 
-/* In dB*256 */
-#define IMX219_DIGITAL_GAIN_MIN		256
-#define IMX219_DIGITAL_GAIN_MAX		43663
-#define IMX219_DIGITAL_GAIN_DEFAULT	256
+/* Chip ID */
+#define IMX219_REG_CHIP_ID		0x0000
+#define IMX219_CHIP_ID			0x0219
 
-#define IMX219_DIGITAL_EXPOSURE_MIN	0
-#define IMX219_DIGITAL_EXPOSURE_MAX	4095
-#define IMX219_DIGITAL_EXPOSURE_DEFAULT	1575
+/* External clock frequency is 24.0M */
+#define IMX219_XCLK_FREQ		24000000
 
-#define IMX219_EXP_LINES_MARGIN	4
+/* Pixel rate is fixed at 182.4M for all the modes */
+#define IMX219_PIXEL_RATE		182400000
 
-#define IMX219_NAME			"imx219"
+#define IMX219_DEFAULT_LINK_FREQ	456000000
 
-#define IMX219_LANES			2
+/* V_TIMING internal */
+#define IMX219_REG_VTS			0x0160
+#define IMX219_VTS_15FPS		0x0dc6
+#define IMX219_VTS_30FPS_1080P		0x06e3
+#define IMX219_VTS_30FPS_BINNED		0x06e3
+#define IMX219_VTS_30FPS_640x480	0x06e3
+#define IMX219_VTS_MAX			0xffff
 
-static const s64 link_freq_menu_items[] = {
-	456000000,
-};
+#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 addr;
+	u16 address;
 	u8 val;
 };
 
+struct imx219_reg_list {
+	unsigned int num_of_regs;
+	const struct imx219_reg *regs;
+};
+
+/* Mode : resolution and related config&values */
 struct imx219_mode {
-	u32 width;
-	u32 height;
-	struct v4l2_fract max_fps;
-	u32 hts_def;
-	u32 vts_def;
-	const struct imx219_reg *reg_list;
+	/* Frame width */
+	unsigned int width;
+	/* Frame height */
+	unsigned int height;
+
+	/* Analog crop rectangle. */
+	struct v4l2_rect crop;
+
+	/* V-timing */
+	unsigned int vts_def;
+
+	/* Default register values */
+	struct imx219_reg_list reg_list;
+
+	/* 2x2 binning is used */
+	bool binning;
 };
 
-/* 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 imx219_common_regs[] = {
+	{0x0100, 0x00},	/* Mode Select */
 
-/* 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, 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},
+	/* 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},
-	{0x0309, 0x0A},
-	{0x030B, 0x01},
-	{0x030C, 0x00},
-	{0x030D, 0x72},
-	{0x455E, 0x00},
-	{0x471E, 0x4B},
-	{0x4767, 0x0F},
+	{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},
-	{IMX219_TABLE_END, 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 struct imx219_reg start[] = {
-	{0x0100, 0x01},		/* mode select streaming on */
-	{IMX219_TABLE_END, 0x00}
+/*
+ * Register sets lifted off the i2C interface from the Raspberry Pi firmware
+ * driver.
+ * 3280x2464 = mode 2, 1920x1080 = mode 1, 1640x1232 = mode 4, 640x480 = mode 7.
+ */
+static const struct imx219_reg mode_3280x2464_regs[] = {
+	{0x0164, 0x00},
+	{0x0165, 0x00},
+	{0x0166, 0x0c},
+	{0x0167, 0xcf},
+	{0x0168, 0x00},
+	{0x0169, 0x00},
+	{0x016a, 0x09},
+	{0x016b, 0x9f},
+	{0x016c, 0x0c},
+	{0x016d, 0xd0},
+	{0x016e, 0x09},
+	{0x016f, 0xa0},
+	{0x0624, 0x0c},
+	{0x0625, 0xd0},
+	{0x0626, 0x09},
+	{0x0627, 0xa0},
 };
 
-static const struct imx219_reg stop[] = {
-	{0x0100, 0x00},		/* mode select streaming off */
-	{IMX219_TABLE_END, 0x00}
+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},
 };
 
-enum {
-	TEST_PATTERN_DISABLED,
-	TEST_PATTERN_SOLID_BLACK,
-	TEST_PATTERN_SOLID_WHITE,
-	TEST_PATTERN_SOLID_RED,
-	TEST_PATTERN_SOLID_GREEN,
-	TEST_PATTERN_SOLID_BLUE,
-	TEST_PATTERN_COLOR_BAR,
-	TEST_PATTERN_FADE_TO_GREY_COLOR_BAR,
-	TEST_PATTERN_PN9,
-	TEST_PATTERN_16_SPLIT_COLOR_BAR,
-	TEST_PATTERN_16_SPLIT_INVERTED_COLOR_BAR,
-	TEST_PATTERN_COLUMN_COUNTER,
-	TEST_PATTERN_INVERTED_COLUMN_COUNTER,
-	TEST_PATTERN_PN31,
-	TEST_PATTERN_MAX
+static const struct imx219_reg mode_1640_1232_regs[] = {
+	{0x0164, 0x00},
+	{0x0165, 0x00},
+	{0x0166, 0x0c},
+	{0x0167, 0xcf},
+	{0x0168, 0x00},
+	{0x0169, 0x00},
+	{0x016a, 0x09},
+	{0x016b, 0x9f},
+	{0x016c, 0x06},
+	{0x016d, 0x68},
+	{0x016e, 0x04},
+	{0x016f, 0xd0},
+	{0x0624, 0x06},
+	{0x0625, 0x68},
+	{0x0626, 0x04},
+	{0x0627, 0xd0},
 };
 
-static const char *const tp_qmenu[] = {
+static const struct imx219_reg mode_640_480_regs[] = {
+	{0x0164, 0x03},
+	{0x0165, 0xe8},
+	{0x0166, 0x08},
+	{0x0167, 0xe7},
+	{0x0168, 0x02},
+	{0x0169, 0xf0},
+	{0x016a, 0x06},
+	{0x016b, 0xaf},
+	{0x016c, 0x02},
+	{0x016d, 0x80},
+	{0x016e, 0x01},
+	{0x016f, 0xe0},
+	{0x0624, 0x06},
+	{0x0625, 0x68},
+	{0x0626, 0x04},
+	{0x0627, 0xd0},
+};
+
+static const struct imx219_reg raw8_framefmt_regs[] = {
+	{0x018c, 0x08},
+	{0x018d, 0x08},
+	{0x0309, 0x08},
+};
+
+static const struct imx219_reg raw10_framefmt_regs[] = {
+	{0x018c, 0x0a},
+	{0x018d, 0x0a},
+	{0x0309, 0x0a},
+};
+
+static const char * const imx219_test_pattern_menu[] = {
 	"Disabled",
-	"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",
+	"Color Bars",
+	"Solid Color",
+	"Grey Color Bars",
+	"PN9"
 };
 
-#define SIZEOF_I2C_TRANSBUF 32
-
-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;
+static const int imx219_test_pattern_val[] = {
+	IMX219_TEST_PATTERN_DISABLE,
+	IMX219_TEST_PATTERN_COLOR_BARS,
+	IMX219_TEST_PATTERN_SOLID_COLOR,
+	IMX219_TEST_PATTERN_GREY_COLOR,
+	IMX219_TEST_PATTERN_PN9,
 };
 
+/* regulator supplies */
+static const char * const imx219_supply_name[] = {
+	/* Supplies can be enabled in any order */
+	"VANA",  /* Analog (2.8V) supply */
+	"VDIG",  /* Digital Core (1.8V) supply */
+	"VDDL",  /* IF (1.2V) supply */
+};
+
+#define IMX219_NUM_SUPPLIES ARRAY_SIZE(imx219_supply_name)
+
+/*
+ * The supported formats.
+ * This table MUST contain 4 entries per format, to cover the various flip
+ * combinations in the order
+ * - no flip
+ * - h flip
+ * - v flip
+ * - h&v flips
+ */
+static const u32 codes[] = {
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+
+	MEDIA_BUS_FMT_SRGGB8_1X8,
+	MEDIA_BUS_FMT_SGRBG8_1X8,
+	MEDIA_BUS_FMT_SGBRG8_1X8,
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+};
+
+/*
+ * Initialisation delay between XCLR low->high and the moment when the sensor
+ * can start capture (i.e. can leave software stanby) must be not less than:
+ *   t4 + max(t5, t6 + <time to initialize the sensor register over I2C>)
+ * where
+ *   t4 is fixed, and is max 200uS,
+ *   t5 is fixed, and is 6000uS,
+ *   t6 depends on the sensor external clock, and is max 32000 clock periods.
+ * As per sensor datasheet, the external clock must be from 6MHz to 27MHz.
+ * So for any acceptable external clock t6 is always within the range of
+ * 1185 to 5333 uS, and is always less than t5.
+ * For this reason this is always safe to wait (t4 + t5) = 6200 uS, then
+ * initialize the sensor over I2C, and then exit the software standby.
+ *
+ * This start-up time can be optimized a bit more, if we start the writes
+ * over I2C after (t4+t6), but before (t4+t5) expires. But then sensor
+ * initialization over I2C may complete before (t4+t5) expires, and we must
+ * ensure that capture is not started before (t4+t5).
+ *
+ * This delay doesn't account for the power supply startup time. If needed,
+ * this should be taken care of via the regulator framework. E.g. in the
+ * case of DT for regulator-fixed one should define the startup-delay-us
+ * property.
+ */
+#define IMX219_XCLR_MIN_DELAY_US	6200
+#define IMX219_XCLR_DELAY_RANGE_US	1000
+
+/* Mode configs */
 static const struct imx219_mode supported_modes[] = {
 	{
-		.width = 1920,
-		.height = 1080,
-		.max_fps = {
-			.numerator = 10000,
-			.denominator = 300000,
-		},
-		.hts_def = 0x0d78 - IMX219_EXP_LINES_MARGIN,
-		.vts_def = 0x06E6,
-		.reg_list = imx219_init_tab_1920_1080_30fps,
-	},
-	{
+		/* 8MPix 15fps mode */
 		.width = 3280,
 		.height = 2464,
-		.max_fps = {
-			.numerator = 10000,
-			.denominator = 210000,
+		.crop = {
+			.left = IMX219_PIXEL_ARRAY_LEFT,
+			.top = IMX219_PIXEL_ARRAY_TOP,
+			.width = 3280,
+			.height = 2464
 		},
-		.hts_def = 0x0d78 - IMX219_EXP_LINES_MARGIN,
-		.vts_def = 0x09c4,
-		.reg_list = imx219_init_tab_3280_2464_21fps,
+		.vts_def = IMX219_VTS_15FPS,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3280x2464_regs),
+			.regs = mode_3280x2464_regs,
+		},
+		.binning = false,
+	},
+	{
+		/* 1080P 30fps cropped */
+		.width = 1920,
+		.height = 1080,
+		.crop = {
+			.left = 688,
+			.top = 700,
+			.width = 1920,
+			.height = 1080
+		},
+		.vts_def = IMX219_VTS_30FPS_1080P,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1920_1080_regs),
+			.regs = mode_1920_1080_regs,
+		},
+		.binning = false,
+	},
+	{
+		/* 2x2 binned 30fps mode */
+		.width = 1640,
+		.height = 1232,
+		.crop = {
+			.left = IMX219_PIXEL_ARRAY_LEFT,
+			.top = IMX219_PIXEL_ARRAY_TOP,
+			.width = 3280,
+			.height = 2464
+		},
+		.vts_def = IMX219_VTS_30FPS_BINNED,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1640_1232_regs),
+			.regs = mode_1640_1232_regs,
+		},
+		.binning = true,
+	},
+	{
+		/* 640x480 30fps mode */
+		.width = 640,
+		.height = 480,
+		.crop = {
+			.left = 1008,
+			.top = 760,
+			.width = 1280,
+			.height = 960
+		},
+		.vts_def = IMX219_VTS_30FPS_640x480,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_640_480_regs),
+			.regs = mode_640_480_regs,
+		},
+		.binning = true,
 	},
 };
 
-static struct imx219 *to_imx219(const struct i2c_client *client)
+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 *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)
 {
-	return container_of(i2c_get_clientdata(client), struct imx219, subdev);
+	return container_of(_sd, struct imx219, sd);
 }
 
-static int reg_write(struct i2c_client *client, const u16 addr, const u8 data)
+/* Read registers up to 2 at a time */
+static int imx219_read_reg(struct imx219 *imx219, u16 reg, u32 len, u32 *val)
 {
-	struct i2c_adapter *adap = client->adapter;
-	struct i2c_msg msg;
-	u8 tx[3];
+	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, };
 	int ret;
 
-	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);
+	if (len > 4)
+		return -EINVAL;
 
-	return ret == 1 ? 0 : -EIO;
-}
+	/* Write register address */
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len = ARRAY_SIZE(addr_buf);
+	msgs[0].buf = addr_buf;
 
-static int reg_read(struct i2c_client *client, const u16 addr)
-{
-	u8 buf[2] = {addr >> 8, addr & 0xff};
-	int ret;
-	struct i2c_msg msgs[] = {
-		{
-			.addr  = client->addr,
-			.flags = 0,
-			.len   = 2,
-			.buf   = buf,
-		}, {
-			.addr  = client->addr,
-			.flags = I2C_M_RD,
-			.len   = 1,
-			.buf   = buf,
-		},
-	};
+	/* 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 < 0) {
-		dev_warn(&client->dev, "Reading register %x from %x failed\n",
-			 addr, client->addr);
-		return ret;
-	}
+	if (ret != ARRAY_SIZE(msgs))
+		return -EIO;
 
-	return buf[0];
-}
-
-static int reg_write_table(struct i2c_client *client,
-			   const struct imx219_reg table[])
-{
-	const struct imx219_reg *reg;
-	int ret;
-
-	for (reg = table; reg->addr != IMX219_TABLE_END; reg++) {
-		ret = reg_write(client, reg->addr, reg->val);
-		if (ret < 0)
-			return ret;
-	}
+	*val = get_unaligned_be32(data_buf);
 
 	return 0;
 }
 
-/* V4L2 subdev video operations */
-static int imx219_s_stream(struct v4l2_subdev *sd, int enable)
+/* Write registers up to 2 at a time */
+static int imx219_write_reg(struct imx219 *imx219, u16 reg, u32 len, u32 val)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct imx219 *priv = to_imx219(client);
-	u8 reg = 0x00;
-	int ret;
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	u8 buf[6];
 
-	if (!enable)
-		return reg_write_table(client, stop);
-
-	ret = reg_write_table(client, priv->cur_mode->reg_list);
-	if (ret)
-		return ret;
-
-	/* Handle crop */
-	ret = reg_write(client, 0x0164, priv->crop_rect.left >> 8);
-	ret |= reg_write(client, 0x0165, priv->crop_rect.left & 0xff);
-	ret |= reg_write(client, 0x0166, (priv->crop_rect.left + priv->crop_rect.width - 1) >> 8);
-	ret |= reg_write(client, 0x0167, (priv->crop_rect.left + priv->crop_rect.width - 1) & 0xff);
-	ret |= reg_write(client, 0x0168, priv->crop_rect.top >> 8);
-	ret |= reg_write(client, 0x0169, priv->crop_rect.top & 0xff);
-	ret |= reg_write(client, 0x016A, (priv->crop_rect.top + priv->crop_rect.height - 1) >> 8);
-	ret |= reg_write(client, 0x016B, (priv->crop_rect.top + priv->crop_rect.height - 1) & 0xff);
-	ret |= reg_write(client, 0x016C, priv->crop_rect.width >> 8);
-	ret |= reg_write(client, 0x016D, priv->crop_rect.width & 0xff);
-	ret |= reg_write(client, 0x016E, priv->crop_rect.height >> 8);
-	ret |= reg_write(client, 0x016F, priv->crop_rect.height & 0xff);
-
-	if (ret)
-		return ret;
-
-	/* Handle flip/mirror */
-	if (priv->hflip)
-		reg |= 0x1;
-	if (priv->vflip)
-		reg |= 0x2;
-
-	ret = reg_write(client, 0x0172, reg);
-	if (ret)
-		return ret;
-
-	/* Handle test pattern */
-	if (priv->test_pattern) {
-		ret = reg_write(client, 0x0600, priv->test_pattern >> 8);
-		ret |= reg_write(client, 0x0601, priv->test_pattern & 0xff);
-		ret |= reg_write(client, 0x0602,
-				 priv->test_pattern_solid_color_r >> 8);
-		ret |= reg_write(client, 0x0603,
-				 priv->test_pattern_solid_color_r & 0xff);
-		ret |= reg_write(client, 0x0604,
-				 priv->test_pattern_solid_color_gr >> 8);
-		ret |= reg_write(client, 0x0605,
-				 priv->test_pattern_solid_color_gr & 0xff);
-		ret |= reg_write(client, 0x0606,
-				 priv->test_pattern_solid_color_b >> 8);
-		ret |= reg_write(client, 0x0607,
-				 priv->test_pattern_solid_color_b & 0xff);
-		ret |= reg_write(client, 0x0608,
-				 priv->test_pattern_solid_color_gb >> 8);
-		ret |= reg_write(client, 0x0609,
-				 priv->test_pattern_solid_color_gb & 0xff);
-		ret |= reg_write(client, 0x0620, priv->crop_rect.left >> 8);
-		ret |= reg_write(client, 0x0621, priv->crop_rect.left & 0xff);
-		ret |= reg_write(client, 0x0622, priv->crop_rect.top >> 8);
-		ret |= reg_write(client, 0x0623, priv->crop_rect.top & 0xff);
-		ret |= reg_write(client, 0x0624, priv->crop_rect.width >> 8);
-		ret |= reg_write(client, 0x0625, priv->crop_rect.width & 0xff);
-		ret |= reg_write(client, 0x0626, priv->crop_rect.height >> 8);
-		ret |= reg_write(client, 0x0627, priv->crop_rect.height & 0xff);
-	} else {
-		ret = reg_write(client, 0x0600, 0x00);
-		ret |= reg_write(client, 0x0601, 0x00);
-	}
-
-	priv->cur_vts = priv->cur_mode->vts_def - IMX219_EXP_LINES_MARGIN;
-	if (ret)
-		return ret;
-
-	return reg_write_table(client, start);
-}
-
-/* V4L2 subdev core operations */
-static int imx219_s_power(struct v4l2_subdev *sd, int on)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct imx219 *priv = to_imx219(client);
-
-	if (on)	{
-		dev_dbg(&client->dev, "imx219 power on\n");
-		clk_prepare_enable(priv->clk);
-	} else if (!on) {
-		dev_dbg(&client->dev, "imx219 power off\n");
-		clk_disable_unprepare(priv->clk);
-	}
-
-	return 0;
-}
-
-/* V4L2 ctrl operations */
-static int imx219_s_ctrl_test_pattern(struct v4l2_ctrl *ctrl)
-{
-	struct imx219 *priv =
-	    container_of(ctrl->handler, struct imx219, ctrl_handler);
-
-	switch (ctrl->val) {
-	case TEST_PATTERN_DISABLED:
-		priv->test_pattern = 0x0000;
-		break;
-	case 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 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 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 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 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 TEST_PATTERN_COLOR_BAR:
-		priv->test_pattern = 0x0002;
-		break;
-	case TEST_PATTERN_FADE_TO_GREY_COLOR_BAR:
-		priv->test_pattern = 0x0003;
-		break;
-	case TEST_PATTERN_PN9:
-		priv->test_pattern = 0x0004;
-		break;
-	case TEST_PATTERN_16_SPLIT_COLOR_BAR:
-		priv->test_pattern = 0x0005;
-		break;
-	case TEST_PATTERN_16_SPLIT_INVERTED_COLOR_BAR:
-		priv->test_pattern = 0x0006;
-		break;
-	case TEST_PATTERN_COLUMN_COUNTER:
-		priv->test_pattern = 0x0007;
-		break;
-	case TEST_PATTERN_INVERTED_COLUMN_COUNTER:
-		priv->test_pattern = 0x0008;
-		break;
-	case TEST_PATTERN_PN31:
-		priv->test_pattern = 0x0009;
-		break;
-	default:
+	if (len > 4)
 		return -EINVAL;
+
+	put_unaligned_be16(reg, buf);
+	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
+		return -EIO;
+
+	return 0;
+}
+
+/* Write a list of registers */
+static int imx219_write_regs(struct imx219 *imx219,
+			     const struct imx219_reg *regs, u32 len)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	unsigned int i;
+	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);
+
+			return ret;
+		}
 	}
 
 	return 0;
 }
 
-static int imx219_g_frame_interval(struct v4l2_subdev *sd,
-				   struct v4l2_subdev_frame_interval *fi)
+/* Get bayer order based on flip setting. */
+static u32 imx219_get_format_code(struct imx219 *imx219, u32 code)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct imx219 *priv = to_imx219(client);
-	const struct imx219_mode *mode = priv->cur_mode;
+	unsigned int i;
 
-	fi->interval = mode->max_fps;
+	lockdep_assert_held(&imx219->mutex);
+
+	for (i = 0; i < ARRAY_SIZE(codes); i++)
+		if (codes[i] == code)
+			break;
+
+	if (i >= ARRAY_SIZE(codes))
+		i = 0;
+
+	i = (i & ~3) | (imx219->vflip->val ? 2 : 0) |
+	    (imx219->hflip->val ? 1 : 0);
+
+	return codes[i];
+}
+
+static void imx219_set_default_format(struct imx219 *imx219)
+{
+	struct v4l2_mbus_framefmt *fmt;
+
+	fmt = &imx219->fmt;
+	fmt->code = MEDIA_BUS_FMT_SRGGB10_1X10;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+							  fmt->colorspace,
+							  fmt->ycbcr_enc);
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+	fmt->width = supported_modes[0].width;
+	fmt->height = supported_modes[0].height;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int imx219_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+	struct v4l2_mbus_framefmt *try_fmt =
+		v4l2_subdev_get_try_format(sd, fh->pad, 0);
+	struct v4l2_rect *try_crop;
+
+	mutex_lock(&imx219->mutex);
+
+	/* Initialize try_fmt */
+	try_fmt->width = supported_modes[0].width;
+	try_fmt->height = supported_modes[0].height;
+	try_fmt->code = imx219_get_format_code(imx219,
+					       MEDIA_BUS_FMT_SRGGB10_1X10);
+	try_fmt->field = V4L2_FIELD_NONE;
+
+	/* Initialize try_crop rectangle. */
+	try_crop = v4l2_subdev_get_try_crop(sd, fh->pad, 0);
+	try_crop->top = IMX219_PIXEL_ARRAY_TOP;
+	try_crop->left = IMX219_PIXEL_ARRAY_LEFT;
+	try_crop->width = IMX219_PIXEL_ARRAY_WIDTH;
+	try_crop->height = IMX219_PIXEL_ARRAY_HEIGHT;
+
+	mutex_unlock(&imx219->mutex);
 
 	return 0;
 }
 
-static int imx219_s_ctrl(struct v4l2_ctrl *ctrl)
+static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct imx219 *priv =
-	    container_of(ctrl->handler, struct imx219, ctrl_handler);
-	struct i2c_client *client = v4l2_get_subdevdata(&priv->subdev);
-	u8 reg;
+	struct imx219 *imx219 =
+		container_of(ctrl->handler, struct imx219, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 	int ret;
-	u16 gain = 256;
-	u16 a_gain = 256;
-	u16 d_gain = 1;
+
+	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;
 
 	switch (ctrl->id) {
-	case V4L2_CID_HFLIP:
-		priv->hflip = ctrl->val;
-		break;
-
-	case V4L2_CID_VFLIP:
-		priv->vflip = ctrl->val;
-		break;
-
 	case V4L2_CID_ANALOGUE_GAIN:
-	case V4L2_CID_GAIN:
-		/*
-		 * hal transfer (gain * 256)  to kernel
-		 * than divide into analog gain & digital gain in kernel
-		 */
-
-		gain = ctrl->val;
-		if (gain < 256)
-			gain = 256;
-		if (gain > 43663)
-			gain = 43663;
-		if (gain >= 256 && gain <= 2728) {
-			a_gain = gain;
-			d_gain = 1 * 256;
-		} else {
-			a_gain = 2728;
-			d_gain = (gain * 256) / a_gain;
-		}
-
-		/*
-		 * Analog gain, reg range[0, 232], gain value[1, 10.66]
-		 * reg = 256 - 256 / again
-		 * a_gain here is 256 multify
-		 * so the reg = 256 - 256 * 256 / a_gain
-		 */
-		priv->analogue_gain = (256 - (256 * 256) / a_gain);
-		if (a_gain < 256)
-			priv->analogue_gain = 0;
-		if (priv->analogue_gain > 232)
-			priv->analogue_gain = 232;
-
-		/*
-		 * Digital gain, reg range[256, 4095], gain rage[1, 16]
-		 * reg = dgain * 256
-		 */
-		priv->digital_gain = d_gain;
-		if (priv->digital_gain < 256)
-			priv->digital_gain = 256;
-		if (priv->digital_gain > 4095)
-			priv->digital_gain = 4095;
-
-		/*
-		 * for bank A and bank B switch
-		 * exposure time , gain, vts must change at the same time
-		 * so the exposure & gain can reflect at the same frame
-		 */
-
-		ret = reg_write(client, 0x0157, priv->analogue_gain);
-		ret |= reg_write(client, 0x0158, priv->digital_gain >> 8);
-		ret |= reg_write(client, 0x0159, priv->digital_gain & 0xff);
-
-		return ret;
-
+		ret = imx219_write_reg(imx219, IMX219_REG_ANALOG_GAIN,
+				       IMX219_REG_VALUE_08BIT, ctrl->val);
+		break;
 	case V4L2_CID_EXPOSURE:
-		priv->exposure_time = ctrl->val;
-
-		ret = reg_write(client, 0x015a, priv->exposure_time >> 8);
-		ret |= reg_write(client, 0x015b, priv->exposure_time & 0xff);
-		return ret;
-
+		ret = imx219_write_reg(imx219, IMX219_REG_EXPOSURE,
+				       IMX219_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = imx219_write_reg(imx219, IMX219_REG_DIGITAL_GAIN,
+				       IMX219_REG_VALUE_16BIT, ctrl->val);
+		break;
 	case V4L2_CID_TEST_PATTERN:
-		return imx219_s_ctrl_test_pattern(ctrl);
-
+		ret = imx219_write_reg(imx219, IMX219_REG_TEST_PATTERN,
+				       IMX219_REG_VALUE_16BIT,
+				       imx219_test_pattern_val[ctrl->val]);
+		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);
+		break;
 	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;
-
+		ret = imx219_write_reg(imx219, IMX219_REG_VTS,
+				       IMX219_REG_VALUE_16BIT,
+				       imx219->mode->height + ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN_RED:
+		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_RED,
+				       IMX219_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN_GREENR:
+		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENR,
+				       IMX219_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN_BLUE:
+		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_BLUE,
+				       IMX219_REG_VALUE_16BIT, ctrl->val);
+		break;
+	case V4L2_CID_TEST_PATTERN_GREENB:
+		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENB,
+				       IMX219_REG_VALUE_16BIT, ctrl->val);
+		break;
 	default:
-		return -EINVAL;
+		dev_info(&client->dev,
+			 "ctrl(id:0x%x,val:0x%x) is not handled\n",
+			 ctrl->id, ctrl->val);
+		ret = -EINVAL;
+		break;
 	}
-	/* If enabled, apply settings immediately */
-	reg = reg_read(client, 0x0100);
-	if ((reg & 0x1f) == 0x01)
-		imx219_s_stream(&priv->subdev, 1);
 
-	return 0;
+	pm_runtime_put(&client->dev);
+
+	return ret;
 }
+
+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_pad_config *cfg,
 				 struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (code->index != 0)
+	struct imx219 *imx219 = to_imx219(sd);
+
+	if (code->index >= (ARRAY_SIZE(codes) / 4))
 		return -EINVAL;
-	code->code = MEDIA_BUS_FMT_SRGGB10_1X10;
+
+	code->code = imx219_get_format_code(imx219, codes[code->index * 4]);
 
 	return 0;
 }
 
-static int imx219_get_reso_dist(const struct imx219_mode *mode,
-				struct v4l2_mbus_framefmt *framefmt)
+static int imx219_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_frame_size_enum *fse)
 {
-	return abs(mode->width - framefmt->width) +
-	       abs(mode->height - framefmt->height);
+	struct imx219 *imx219 = to_imx219(sd);
+
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != imx219_get_format_code(imx219, fse->code))
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
 }
 
-static const struct imx219_mode *imx219_find_best_fit(
-					struct v4l2_subdev_format *fmt)
+static void imx219_reset_colorspace(struct v4l2_mbus_framefmt *fmt)
 {
-	struct v4l2_mbus_framefmt *framefmt = &fmt->format;
-	int dist;
-	int cur_best_fit = 0;
-	int cur_best_fit_dist = -1;
-	int i;
+	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);
+}
 
-	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;
-		}
+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);
+}
+
+static int __imx219_get_pad_format(struct imx219 *imx219,
+				   struct v4l2_subdev_pad_config *cfg,
+				   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, cfg, 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);
 	}
 
-	return &supported_modes[cur_best_fit];
+	return 0;
 }
 
-static int imx219_set_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_subdev_pad_config *cfg,
-			  struct v4l2_subdev_format *fmt)
+static int imx219_get_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_format *fmt)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct imx219 *priv = to_imx219(client);
+	struct imx219 *imx219 = to_imx219(sd);
+	int ret;
+
+	mutex_lock(&imx219->mutex);
+	ret = __imx219_get_pad_format(imx219, cfg, fmt);
+	mutex_unlock(&imx219->mutex);
+
+	return ret;
+}
+
+static int imx219_set_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct imx219 *imx219 = to_imx219(sd);
 	const struct imx219_mode *mode;
-	s64 h_blank, v_blank, pixel_rate;
-	u32 fps = 0;
+	struct v4l2_mbus_framefmt *framefmt;
+	int exposure_max, exposure_def, hblank;
+	unsigned int i;
 
-	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
-		return 0;
+	mutex_lock(&imx219->mutex);
 
-	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);
+	for (i = 0; i < ARRAY_SIZE(codes); i++)
+		if (codes[i] == fmt->format.code)
+			break;
+	if (i >= ARRAY_SIZE(codes))
+		i = 0;
 
-	/* 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;
+	/* Bayer order varies with flips */
+	fmt->format.code = imx219_get_format_code(imx219, codes[i]);
 
-	return 0;
-}
-
-static int imx219_get_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_subdev_pad_config *cfg,
-			  struct v4l2_subdev_format *fmt)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct imx219 *priv = to_imx219(client);
-	const struct imx219_mode *mode = priv->cur_mode;
-
-	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
-		return 0;
-
-	fmt->format.width = mode->width;
-	fmt->format.height = mode->height;
-	fmt->format.code = MEDIA_BUS_FMT_SRGGB10_1X10;
-	fmt->format.field = V4L2_FIELD_NONE;
-
-	return 0;
-}
-
-static void imx219_get_module_inf(struct imx219 *imx219,
-				  struct rkmodule_inf *inf)
-{
-	memset(inf, 0, sizeof(*inf));
-	strlcpy(inf->base.sensor, IMX219_NAME, sizeof(inf->base.sensor));
-	strlcpy(inf->base.module, imx219->module_name,
-		sizeof(inf->base.module));
-	strlcpy(inf->base.lens, imx219->len_name, sizeof(inf->base.lens));
-}
-
-static long imx219_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct imx219 *imx219 = to_imx219(client);
-	long ret = 0;
-
-	switch (cmd) {
-	case RKMODULE_GET_MODULE_INFO:
-		imx219_get_module_inf(imx219, (struct rkmodule_inf *)arg);
-		break;
-	default:
-		ret = -ENOIOCTLCMD;
-		break;
+	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, cfg, 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);
 	}
 
-	return ret;
+	mutex_unlock(&imx219->mutex);
+
+	return 0;
 }
 
-#ifdef CONFIG_COMPAT
-static long imx219_compat_ioctl32(struct v4l2_subdev *sd,
-				  unsigned int cmd, unsigned long arg)
+static int imx219_set_framefmt(struct imx219 *imx219)
 {
-	void __user *up = compat_ptr(arg);
-	struct rkmodule_inf *inf;
-	struct rkmodule_awb_cfg *cfg;
-	long ret;
+	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));
 
-	switch (cmd) {
-	case RKMODULE_GET_MODULE_INFO:
-		inf = kzalloc(sizeof(*inf), GFP_KERNEL);
-		if (!inf) {
-			ret = -ENOMEM;
-			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 = copy_from_user(cfg, up, sizeof(*cfg));
-		if (!ret)
-			ret = imx219_ioctl(sd, cmd, cfg);
-		kfree(cfg);
-		break;
-	default:
-		ret = -ENOIOCTLCMD;
-		break;
+	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));
 	}
 
-	return ret;
-}
-#endif
-
-static int imx219_enum_frame_interval(struct v4l2_subdev *sd,
-				       struct v4l2_subdev_pad_config *cfg,
-				       struct v4l2_subdev_frame_interval_enum *fie)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct imx219 *priv = to_imx219(client);
-
-	if (fie->index >= priv->cfg_num)
-		return -EINVAL;
-
-	if (fie->code != MEDIA_BUS_FMT_SRGGB10_1X10)
-		return -EINVAL;
-
-	fie->width = supported_modes[fie->index].width;
-	fie->height = supported_modes[fie->index].height;
-	fie->interval = supported_modes[fie->index].max_fps;
-	return 0;
+	return -EINVAL;
 }
 
-static int imx219_g_mbus_config(struct v4l2_subdev *sd,
-				struct v4l2_mbus_config *config)
+static int imx219_set_binning(struct imx219 *imx219)
 {
-	u32 val = 0;
+	if (!imx219->mode->binning) {
+		return imx219_write_reg(imx219, IMX219_REG_BINNING_MODE,
+					IMX219_REG_VALUE_16BIT,
+					IMX219_BINNING_NONE);
+	}
 
-	val = 1 << (IMX219_LANES - 1) |
-	      V4L2_MBUS_CSI2_CHANNEL_0 |
-	      V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
-	config->type = V4L2_MBUS_CSI2;
-	config->flags = val;
+	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);
 
-	return 0;
+	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;
 }
 
-/* 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,
-	.g_mbus_config = imx219_g_mbus_config,
-};
-
-static struct v4l2_subdev_core_ops imx219_subdev_core_ops = {
-	.s_power = imx219_s_power,
-	.ioctl = imx219_ioctl,
-#ifdef CONFIG_COMPAT
-	.compat_ioctl32 = imx219_compat_ioctl32,
-#endif
-};
-
-static const struct v4l2_subdev_pad_ops imx219_subdev_pad_ops = {
-	.enum_mbus_code = imx219_enum_mbus_code,
-	.enum_frame_interval = imx219_enum_frame_interval,
-	.set_fmt = imx219_set_fmt,
-	.get_fmt = imx219_get_fmt,
-};
-
-static struct v4l2_subdev_ops imx219_subdev_ops = {
-	.core = &imx219_subdev_core_ops,
-	.video = &imx219_subdev_video_ops,
-	.pad = &imx219_subdev_pad_ops,
-};
-
-static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
-	.s_ctrl = imx219_s_ctrl,
-};
-
-static int imx219_video_probe(struct i2c_client *client)
+static const struct v4l2_rect *
+__imx219_get_pad_crop(struct imx219 *imx219, struct v4l2_subdev_pad_config *cfg,
+		      unsigned int pad, enum v4l2_subdev_format_whence which)
 {
-	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
-	u16 model_id;
-	u32 lot_id;
-	u16 chip_id;
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&imx219->sd, cfg, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &imx219->mode->crop;
+	}
+
+	return NULL;
+}
+
+static int imx219_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP: {
+		struct imx219 *imx219 = to_imx219(sd);
+
+		mutex_lock(&imx219->mutex);
+		sel->r = *__imx219_get_pad_crop(imx219, cfg, sel->pad,
+						sel->which);
+		mutex_unlock(&imx219->mutex);
+
+		return 0;
+	}
+
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = IMX219_NATIVE_WIDTH;
+		sel->r.height = IMX219_NATIVE_HEIGHT;
+
+		return 0;
+
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = IMX219_PIXEL_ARRAY_TOP;
+		sel->r.left = IMX219_PIXEL_ARRAY_LEFT;
+		sel->r.width = IMX219_PIXEL_ARRAY_WIDTH;
+		sel->r.height = IMX219_PIXEL_ARRAY_HEIGHT;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int imx219_start_streaming(struct imx219 *imx219)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	const struct imx219_reg_list *reg_list;
 	int ret;
 
-	ret = imx219_s_power(subdev, 1);
-	if (ret < 0)
+	ret = pm_runtime_get_sync(&client->dev);
+	if (ret < 0) {
+		pm_runtime_put_noidle(&client->dev);
 		return ret;
-
-	/* 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;
 
-	ret = reg_read(client, 0x0001);
-	if (ret < 0) {
-		dev_err(&client->dev, "Failure to read Model ID (low byte)\n");
-		goto done;
+	/* 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;
 	}
-	model_id |= ret;
 
-	ret = reg_read(client, 0x0004);
-	if (ret < 0) {
-		dev_err(&client->dev, "Failure to read Lot ID (high byte)\n");
-		goto done;
+	/* 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;
 	}
-	lot_id = ret << 16;
 
-	ret = reg_read(client, 0x0005);
-	if (ret < 0) {
-		dev_err(&client->dev, "Failure to read Lot ID (mid byte)\n");
-		goto done;
+	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;
 	}
-	lot_id |= ret << 8;
 
-	ret = reg_read(client, 0x0006);
-	if (ret < 0) {
-		dev_err(&client->dev, "Failure to read Lot ID (low byte)\n");
-		goto done;
+	ret = imx219_set_binning(imx219);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set binning: %d\n",
+			__func__, ret);
+		goto err_rpm_put;
 	}
-	lot_id |= ret;
 
-	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;
+	/* Apply customized values from user */
+	ret =  __v4l2_ctrl_handler_setup(imx219->sd.ctrl_handler);
+	if (ret)
+		goto err_rpm_put;
 
-	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;
+	/* 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 (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);
+	/* vflip and hflip cannot change during streaming */
+	__v4l2_ctrl_grab(imx219->vflip, true);
+	__v4l2_ctrl_grab(imx219->hflip, true);
+
+	return 0;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
 	return ret;
 }
 
-static int imx219_ctrls_init(struct v4l2_subdev *sd)
+static void imx219_stop_streaming(struct imx219 *imx219)
 {
-	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;
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 	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);
+	/* 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__);
 
-	/* 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);
+	__v4l2_ctrl_grab(imx219->vflip, false);
+	__v4l2_ctrl_grab(imx219->hflip, false);
 
-	/* 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);
+	pm_runtime_put(&client->dev);
+}
 
-	/* 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);
+static int imx219_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct imx219 *imx219 = to_imx219(sd);
+	int ret = 0;
 
-	v4l2_ctrl_new_std_menu_items(&priv->ctrl_handler, &imx219_ctrl_ops,
-				     V4L2_CID_TEST_PATTERN,
-				     ARRAY_SIZE(tp_qmenu) - 1, 0, 0, tp_qmenu);
-
-	priv->subdev.ctrl_handler = &priv->ctrl_handler;
-	if (priv->ctrl_handler.error) {
-		dev_err(&client->dev, "Error %d adding controls\n",
-			priv->ctrl_handler.error);
-		ret = priv->ctrl_handler.error;
-		goto error;
+	mutex_lock(&imx219->mutex);
+	if (imx219->streaming == enable) {
+		mutex_unlock(&imx219->mutex);
+		return 0;
 	}
 
-	ret = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
-	if (ret < 0) {
-		dev_err(&client->dev, "Error %d setting default controls\n",
-			ret);
-		goto error;
+	if (enable) {
+		/*
+		 * Apply default & customized values
+		 * and then start streaming.
+		 */
+		ret = imx219_start_streaming(imx219);
+		if (ret)
+			goto err_unlock;
+	} else {
+		imx219_stop_streaming(imx219);
+	}
+
+	imx219->streaming = enable;
+
+	mutex_unlock(&imx219->mutex);
+
+	return ret;
+
+err_unlock:
+	mutex_unlock(&imx219->mutex);
+
+	return ret;
+}
+
+/* Power/clock management functions */
+static int imx219_power_on(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx219 *imx219 = to_imx219(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(IMX219_NUM_SUPPLIES,
+				    imx219->supplies);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable regulators\n",
+			__func__);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(imx219->xclk);
+	if (ret) {
+		dev_err(&client->dev, "%s: failed to enable clock\n",
+			__func__);
+		goto reg_off;
+	}
+
+	gpiod_set_value_cansleep(imx219->reset_gpio, 1);
+	usleep_range(IMX219_XCLR_MIN_DELAY_US,
+		     IMX219_XCLR_MIN_DELAY_US + IMX219_XCLR_DELAY_RANGE_US);
+
+	return 0;
+
+reg_off:
+	regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
+
+	return ret;
+}
+
+static int imx219_power_off(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx219 *imx219 = to_imx219(sd);
+
+	gpiod_set_value_cansleep(imx219->reset_gpio, 0);
+	regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
+	clk_disable_unprepare(imx219->xclk);
+
+	return 0;
+}
+
+static int __maybe_unused imx219_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx219 *imx219 = to_imx219(sd);
+
+	if (imx219->streaming)
+		imx219_stop_streaming(imx219);
+
+	return 0;
+}
+
+static int __maybe_unused imx219_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx219 *imx219 = to_imx219(sd);
+	int ret;
+
+	if (imx219->streaming) {
+		ret = imx219_start_streaming(imx219);
+		if (ret)
+			goto error;
 	}
 
 	return 0;
+
 error:
-	v4l2_ctrl_handler_free(&priv->ctrl_handler);
+	imx219_stop_streaming(imx219);
+	imx219->streaming = false;
+
 	return ret;
 }
 
-static int imx219_probe(struct i2c_client *client,
-			const struct i2c_device_id *did)
+static int imx219_get_regulators(struct imx219 *imx219)
 {
-	struct imx219 *priv;
-	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
-	struct device *dev = &client->dev;
-	struct device_node *node = dev->of_node;
-	struct v4l2_subdev *sd;
-	char facing[2];
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	unsigned int i;
+
+	for (i = 0; i < IMX219_NUM_SUPPLIES; i++)
+		imx219->supplies[i].supply = imx219_supply_name[i];
+
+	return devm_regulator_bulk_get(&client->dev,
+				       IMX219_NUM_SUPPLIES,
+				       imx219->supplies);
+}
+
+/* Verify chip ID */
+static int imx219_identify_module(struct imx219 *imx219)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 	int ret;
+	u32 val;
 
-	dev_info(dev, "driver version: %02x.%02x.%02x",
-		DRIVER_VERSION >> 16,
-		(DRIVER_VERSION & 0xff00) >> 8,
-		DRIVER_VERSION & 0x00ff);
+	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 (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
-		dev_warn(&adapter->dev,
-			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_BYTE\n");
+	if (val != IMX219_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+			IMX219_CHIP_ID, val);
 		return -EIO;
 	}
-	priv = devm_kzalloc(&client->dev, sizeof(struct imx219), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
 
-	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 0;
+}
+
+static const struct v4l2_subdev_core_ops imx219_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops imx219_video_ops = {
+	.s_stream = imx219_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops imx219_pad_ops = {
+	.enum_mbus_code = imx219_enum_mbus_code,
+	.get_fmt = imx219_get_pad_format,
+	.set_fmt = imx219_set_pad_format,
+	.get_selection = imx219_get_selection,
+	.enum_frame_size = imx219_enum_frame_size,
+};
+
+static const struct v4l2_subdev_ops imx219_subdev_ops = {
+	.core = &imx219_core_ops,
+	.video = &imx219_video_ops,
+	.pad = &imx219_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops imx219_internal_ops = {
+	.open = imx219_open,
+};
+
+/* Initialize control handlers */
+static int imx219_init_controls(struct imx219 *imx219)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	unsigned int height = imx219->mode->height;
+	struct v4l2_fwnode_device_properties props;
+	int exposure_max, exposure_def, hblank;
+	int i, ret;
+
+	ctrl_hdlr = &imx219->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 11);
+	if (ret)
+		return ret;
+
+	mutex_init(&imx219->mutex);
+	ctrl_hdlr->lock = &imx219->mutex;
+
+	/* By default, PIXEL_RATE is read only */
+	imx219->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					       V4L2_CID_PIXEL_RATE,
+					       IMX219_PIXEL_RATE,
+					       IMX219_PIXEL_RATE, 1,
+					       IMX219_PIXEL_RATE);
+
+	/* Initial vblank/hblank/exposure parameters based on current mode */
+	imx219->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					   V4L2_CID_VBLANK, IMX219_VBLANK_MIN,
+					   IMX219_VTS_MAX - height, 1,
+					   imx219->mode->vts_def - height);
+	hblank = IMX219_PPL_DEFAULT - imx219->mode->width;
+	imx219->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					   V4L2_CID_HBLANK, hblank, hblank,
+					   1, hblank);
+	if (imx219->hblank)
+		imx219->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	exposure_max = imx219->mode->vts_def - 4;
+	exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
+		exposure_max : IMX219_EXPOSURE_DEFAULT;
+	imx219->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     IMX219_EXPOSURE_MIN, exposure_max,
+					     IMX219_EXPOSURE_STEP,
+					     exposure_def);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  IMX219_ANA_GAIN_MIN, IMX219_ANA_GAIN_MAX,
+			  IMX219_ANA_GAIN_STEP, IMX219_ANA_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  IMX219_DGTL_GAIN_MIN, IMX219_DGTL_GAIN_MAX,
+			  IMX219_DGTL_GAIN_STEP, IMX219_DGTL_GAIN_DEFAULT);
+
+	imx219->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (imx219->hflip)
+		imx219->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	imx219->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (imx219->vflip)
+		imx219->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx219_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(imx219_test_pattern_menu) - 1,
+				     0, 0, imx219_test_pattern_menu);
+	for (i = 0; i < 4; i++) {
+		/*
+		 * The assumption is that
+		 * V4L2_CID_TEST_PATTERN_GREENR == V4L2_CID_TEST_PATTERN_RED + 1
+		 * V4L2_CID_TEST_PATTERN_BLUE   == V4L2_CID_TEST_PATTERN_RED + 2
+		 * V4L2_CID_TEST_PATTERN_GREENB == V4L2_CID_TEST_PATTERN_RED + 3
+		 */
+		v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
+				  V4L2_CID_TEST_PATTERN_RED + i,
+				  IMX219_TESTP_COLOUR_MIN,
+				  IMX219_TESTP_COLOUR_MAX,
+				  IMX219_TESTP_COLOUR_STEP,
+				  IMX219_TESTP_COLOUR_MAX);
+		/* The "Solid color" pattern is white by default */
+	}
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(&client->dev, "%s control init failed (%d)\n",
+			__func__, ret);
+		goto error;
+	}
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx219_ctrl_ops,
+					      &props);
+	if (ret)
+		goto error;
+
+	imx219->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+	mutex_destroy(&imx219->mutex);
+
+	return ret;
+}
+
+static void imx219_free_controls(struct imx219 *imx219)
+{
+	v4l2_ctrl_handler_free(imx219->sd.ctrl_handler);
+	mutex_destroy(&imx219->mutex);
+}
+
+static int imx219_check_hwcfg(struct device *dev)
+{
+	struct fwnode_handle *endpoint;
+	struct v4l2_fwnode_endpoint ep_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	int ret = -EINVAL;
+
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
 		return -EINVAL;
 	}
 
-	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;
+	if (v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep_cfg)) {
+		dev_err(dev, "could not parse endpoint\n");
+		goto error_out;
 	}
 
-	/* 1920 * 1080 by default */
-	priv->cur_mode = &supported_modes[0];
-	priv->cfg_num = ARRAY_SIZE(supported_modes);
+	/* 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;
+	}
 
-	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;
+	/* 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;
+	}
 
-	v4l2_i2c_subdev_init(&priv->subdev, client, &imx219_subdev_ops);
-	ret = imx219_ctrls_init(&priv->subdev);
-	if (ret < 0)
+	if (ep_cfg.nr_of_link_frequencies != 1 ||
+	    ep_cfg.link_frequencies[0] != IMX219_DEFAULT_LINK_FREQ) {
+		dev_err(dev, "Link frequency not supported: %lld\n",
+			ep_cfg.link_frequencies[0]);
+		goto error_out;
+	}
+
+	ret = 0;
+
+error_out:
+	v4l2_fwnode_endpoint_free(&ep_cfg);
+	fwnode_handle_put(endpoint);
+
+	return ret;
+}
+
+static int imx219_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct imx219 *imx219;
+	int ret;
+
+	imx219 = devm_kzalloc(&client->dev, sizeof(*imx219), GFP_KERNEL);
+	if (!imx219)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&imx219->sd, client, &imx219_subdev_ops);
+
+	/* Check the hardware configuration in device tree */
+	if (imx219_check_hwcfg(dev))
+		return -EINVAL;
+
+	/* Get system clock (xclk) */
+	imx219->xclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(imx219->xclk)) {
+		dev_err(dev, "failed to get xclk\n");
+		return PTR_ERR(imx219->xclk);
+	}
+
+	imx219->xclk_freq = clk_get_rate(imx219->xclk);
+	if (imx219->xclk_freq != IMX219_XCLK_FREQ) {
+		dev_err(dev, "xclk frequency not supported: %d Hz\n",
+			imx219->xclk_freq);
+		return -EINVAL;
+	}
+
+	ret = imx219_get_regulators(imx219);
+	if (ret) {
+		dev_err(dev, "failed to get regulators\n");
 		return ret;
-	ret = imx219_video_probe(client);
-	if (ret < 0)
+	}
+
+	/* 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;
 
-	priv->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
-		     V4L2_SUBDEV_FL_HAS_EVENTS;
+	ret = imx219_identify_module(imx219);
+	if (ret)
+		goto error_power_off;
 
-	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);
+	/* Set default mode to max resolution */
+	imx219->mode = &supported_modes[0];
+
+	/* sensor doesn't enter LP-11 state upon power up until and unless
+	 * streaming is started, so upon power up switch the modes to:
+	 * streaming -> standby
+	 */
+	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
+			       IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
 	if (ret < 0)
-		return ret;
+		goto error_power_off;
+	usleep_range(100, 110);
 
-	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_common(sd);
+	/* put sensor back to standby mode */
+	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
+			       IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
 	if (ret < 0)
-		return ret;
+		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;
+	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_common(&imx219->sd);
+	if (ret < 0) {
+		dev_err(dev, "failed to register sensor sub-device: %d\n", ret);
+		goto error_media_entity;
+	}
+
+	/* Enable runtime PM and turn off the device */
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_idle(dev);
+
+	return 0;
+
+error_media_entity:
+	media_entity_cleanup(&imx219->sd.entity);
+
+error_handler_free:
+	imx219_free_controls(imx219);
+
+error_power_off:
+	imx219_power_off(dev);
 
 	return ret;
 }
 
 static int imx219_remove(struct i2c_client *client)
 {
-	struct imx219 *priv = to_imx219(client);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx219 *imx219 = to_imx219(sd);
 
-	v4l2_async_unregister_subdev(&priv->subdev);
-	media_entity_cleanup(&priv->subdev.entity);
-	v4l2_ctrl_handler_free(&priv->ctrl_handler);
+	v4l2_async_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	imx219_free_controls(imx219);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		imx219_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
 
 	return 0;
 }
 
-static const struct i2c_device_id imx219_id[] = {
-	{"imx219", 0},
-	{}
-};
-
-static const struct of_device_id imx219_of_match[] = {
+static const struct of_device_id imx219_dt_ids[] = {
 	{ .compatible = "sony,imx219" },
-	{ /* sentinel */ },
+	{ /* sentinel */ }
 };
-MODULE_DEVICE_TABLE(of, imx219_of_match);
+MODULE_DEVICE_TABLE(of, imx219_dt_ids);
 
-MODULE_DEVICE_TABLE(i2c, imx219_id);
+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 struct i2c_driver imx219_i2c_driver = {
 	.driver = {
-		.of_match_table = of_match_ptr(imx219_of_match),
-		.name = IMX219_NAME,
+		.name = "imx219",
+		.of_match_table	= imx219_dt_ids,
+		.pm = &imx219_pm_ops,
 	},
-	.probe = imx219_probe,
+	.probe_new = imx219_probe,
 	.remove = imx219_remove,
-	.id_table = imx219_id,
 };
 
 module_i2c_driver(imx219_i2c_driver);
-MODULE_DESCRIPTION("Sony IMX219 Camera driver");
-MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
+
+MODULE_AUTHOR("Dave Stevenson <dave.stevenson@raspberrypi.com");
+MODULE_DESCRIPTION("Sony IMX219 sensor driver");
 MODULE_LICENSE("GPL v2");

--
Gitblit v1.6.2