[v2 0/2] media: i2c: mt9v111 sensor driver

[v2 0/2] media: i2c: mt9v111 sensor driver

[Jacopo Mondi]

Hello,
   this is a sensor level driver for Aptina MT9V111.

Compared to v1 the biggest difference is that now auto-exposure and
auto-white-balancing are exposed through the v4l2-ctrl framework, and can be
activated/deactivated by the user.

For this reason, while auto-exposure algorithm still controls the frame rate in
low light conditions, the driver now enables it by default and let the user
disable it to more precisely control the framerate.

(Most of the) comments from Sakari and Kieran have been addressed, see changelog
for a more precise list.

Tested VGA, QVGA QQVGA resolutions at 5, 10, 15 and 30 frame per second.

v4l2-compliance fails on some tests, and I've not been able to identify what's
wrong with the driver. Copy of the failures reported here below:

fail: v4l2-test-subdevs.cpp(69): doioctl(node, VIDIOC_SUBDEV_ENUM_FRAME_INTERVAL, &fie) != EINVAL
fail: v4l2-test-subdevs.cpp(180): ret && ret != ENOTTY
fail: v4l2-test-subdevs.cpp(248): ret && ret != ENOTTY
test Active VIDIOC_SUBDEV_ENUM_MBUS_CODE/FRAME_SIZE/FRAME_INTERVAL: FAIL

I've found no mention of ENOTTY to be reported by enum_frame_interval in the
documentation, but apparently v4l2-compliance checks for that.

v1: https://lkml.org/lkml/2018/6/11/467

Thanks
   j

v1 -> v2:
- Addressed syntax mistakes pointed out by Kieran
- Drop spinlock around address space change
- Extend mutex to cover frame rate and format get/set operations
- Add controls for auto-exposure and auto-white-balancing
- Move MAINTAINERS modifications to bindings patch
- Fix resolution in bindings document
- Drop dependency on OF
- Drop subdev 'open' function in favour of pad operation init_cfg and use a
  default configuration to initialize the sensor
- Turn off power if sensor identification fails

Jacopo Mondi (2):
  dt-bindings: media: i2c: Document MT9V111 bindings
  media: i2c: Add driver for Aptina MT9V111

 .../bindings/media/i2c/aptina,mt9v111.txt          |   46 +
 MAINTAINERS                                        |    8 +
 drivers/media/i2c/Kconfig                          |   11 +
 drivers/media/i2c/Makefile                         |    1 +
 drivers/media/i2c/mt9v111.c                        | 1299 ++++++++++++++++++++
 5 files changed, 1365 insertions(+)
 create mode 100644 Documentation/devicetree/bindings/media/i2c/aptina,mt9v111.txt
 create mode 100644 drivers/media/i2c/mt9v111.c

--
2.7.4

[v2 1/2] dt-bindings: media: i2c: Document MT9V111 bindings

[Jacopo Mondi]

From: Jacopo Mondi <jacopo+renesas@jmondi.org>

Add documentation for Aptina MT9V111 image sensor.

Reviewed-by: Rob Herring <robh@kernel.org>
Signed-off-by: Jacopo Mondi <jacopo+renesas@jmondi.org>
---
 .../bindings/media/i2c/aptina,mt9v111.txt          | 46 ++++++++++++++++++++++
 MAINTAINERS                                        |  8 ++++
 2 files changed, 54 insertions(+)
 create mode 100644 Documentation/devicetree/bindings/media/i2c/aptina,mt9v111.txt

diff --git a/Documentation/devicetree/bindings/media/i2c/aptina,mt9v111.txt b/Documentation/devicetree/bindings/media/i2c/aptina,mt9v111.txt
new file mode 100644
index 0000000..bd896e9
--- /dev/null
+++ b/Documentation/devicetree/bindings/media/i2c/aptina,mt9v111.txt
@@ -0,0 +1,46 @@
+* Aptina MT9V111 CMOS sensor
+----------------------------
+
+The Aptina MT9V111 is a 1/4-Inch VGA-format digital image sensor with a core
+based on Aptina MT9V011 sensor and an integrated Image Flow Processor (IFP).
+
+The sensor has an active pixel array of 640x480 pixels and can output a number
+of image resolution and formats controllable through a simple two-wires
+interface.
+
+Required properties:
+--------------------
+
+- compatible: shall be "aptina,mt9v111".
+- clocks: reference to the system clock input provider.
+
+Optional properties:
+--------------------
+
+- enable-gpios: output enable signal, pin name "OE#". Active low.
+- standby-gpios: low power state control signal, pin name "STANDBY".
+  Active high.
+- reset-gpios: chip reset signal, pin name "RESET#". Active low.
+
+The device node must contain one 'port' child node with one 'endpoint' child
+sub-node for its digital output video port, in accordance with the video
+interface bindings defined in:
+Documentation/devicetree/bindings/media/video-interfaces.txt
+
+Example:
+--------
+
+        &i2c1 {
+                camera@48 {
+                        compatible = "aptina,mt9v111";
+                        reg = <0x48>;
+
+                        clocks = <&camera_clk>;
+
+                        port {
+                                mt9v111_out: endpoint {
+                                        remote-endpoint = <&ceu_in>;
+                                };
+                        };
+                };
+        };
diff --git a/MAINTAINERS b/MAINTAINERS
index bbd9b9b..4ab113c 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -9651,6 +9651,14 @@ F:	Documentation/devicetree/bindings/media/i2c/mt9v032.txt
 F:	drivers/media/i2c/mt9v032.c
 F:	include/media/i2c/mt9v032.h

+MT9V111 APTINA CAMERA SENSOR
+M:	Jacopo Mondi <jacopo@jmondi.org>
+L:	linux-media@vger.kernel.org
+T:	git git://linuxtv.org/media_tree.git
+S:	Maintained
+F:	Documentation/devicetree/bindings/media/i2c/aptina,mt9v111.txt
+F:	drivers/media/i2c/mt9v111.c
+
 MULTIFUNCTION DEVICES (MFD)
 M:	Lee Jones <lee.jones@linaro.org>
 T:	git git://git.kernel.org/pub/scm/linux/kernel/git/lee/mfd.git
--
2.7.4

[v2 2/2] media: i2c: Add driver for Aptina MT9V111

[Jacopo Mondi]

From: Jacopo Mondi <jacopo+renesas@jmondi.org>

Add V4L2 sensor driver for Aptina MT9V111 CMOS image sensor.

The MT9V111 is a 1/4-Inch CMOS image sensor based on MT9V011 with an
integrated Image Flow Processor.

Reviewed-by: Kieran Bingham <kieran.bingham+renesas@ideasonboard.com>
Signed-off-by: Jacopo Mondi <jacopo+renesas@jmondi.org>
---
 drivers/media/i2c/Kconfig   |   11 +
 drivers/media/i2c/Makefile  |    1 +
 drivers/media/i2c/mt9v111.c | 1298 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 1310 insertions(+)
 create mode 100644 drivers/media/i2c/mt9v111.c

diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index 8669853..b6fea81 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -861,6 +861,17 @@ config VIDEO_MT9V032
 	  This is a Video4Linux2 sensor-level driver for the Micron
 	  MT9V032 752x480 CMOS sensor.
 
+config VIDEO_MT9V111
+	tristate "Aptina MT9V111 sensor support"
+	depends on I2C && VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
+	help
+	  This is a Video4Linux2 sensor driver for the Aptina/Micron
+	  MT9V111 sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mt9v111.
+
 config VIDEO_SR030PC30
 	tristate "Siliconfile SR030PC30 sensor support"
 	depends on I2C && VIDEO_V4L2
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index 837c428..ba9a1e4 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -86,6 +86,7 @@ obj-$(CONFIG_VIDEO_MT9T001) += mt9t001.o
 obj-$(CONFIG_VIDEO_MT9T112) += mt9t112.o
 obj-$(CONFIG_VIDEO_MT9V011) += mt9v011.o
 obj-$(CONFIG_VIDEO_MT9V032) += mt9v032.o
+obj-$(CONFIG_VIDEO_MT9V111) += mt9v111.o
 obj-$(CONFIG_VIDEO_SR030PC30)	+= sr030pc30.o
 obj-$(CONFIG_VIDEO_NOON010PC30)	+= noon010pc30.o
 obj-$(CONFIG_VIDEO_RJ54N1)	+= rj54n1cb0c.o
diff --git a/drivers/media/i2c/mt9v111.c b/drivers/media/i2c/mt9v111.c
new file mode 100644
index 0000000..da8f6ab
--- /dev/null
+++ b/drivers/media/i2c/mt9v111.c
@@ -0,0 +1,1298 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * V4L2 sensor driver for Aptina MT9V111 image sensor
+ * Copyright (C) 2018 Jacopo Mondi <jacopo@jmondi.org>
+ *
+ * Based on mt9v032 driver
+ * Copyright (C) 2010, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * Based on mt9v011 driver
+ * Copyright (c) 2009 Mauro Carvalho Chehab <mchehab@kernel.org>
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/module.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/v4l2-subdev.h>
+
+/*
+ * MT9V111 is a 1/4-Inch CMOS digital image sensor with an integrated
+ * Image Flow Processing (IFP) engine and a sensor core loosely based on
+ * MT9V011.
+ *
+ * The IFP can produce several output image formats from the sensor core
+ * output. This driver currently supports only YUYV format permutations.
+ *
+ * The driver allows manual frame rate control through s_frame_interval subdev
+ * operation or V4L2_CID_V/HBLANK controls, but it is known that the
+ * auto-exposure algorithm might modify the programmed frame rate. While the
+ * driver initially programs the sensor with auto-exposure and
+ * auto-white-balancing enabled, it is possible to disable them and more
+ * precisely control the frame rate.
+ *
+ * While it seems possible to instruct the auto-exposure control algorithm to
+ * respect a programmed frame rate when adjusting the pixel integration time,
+ * registers controlling this feature are not documented in the public
+ * available sensor manual used to develop this driver (09005aef80e90084,
+ * MT9V111_1.fm - Rev. G 1/05 EN).
+ */
+
+#define MT9V111_CHIP_ID_HIGH				0x82
+#define MT9V111_CHIP_ID_LOW				0x3a
+
+#define MT9V111_R01_ADDR_SPACE				0x01
+#define MT9V111_R01_IFP					0x01
+#define MT9V111_R01_CORE				0x04
+
+#define MT9V111_IFP_R06_OPMODE_CTRL			0x06
+#define		MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN	BIT(1)
+#define		MT9V111_IFP_R06_OPMODE_CTRL_AE_EN	BIT(14)
+#define MT9V111_IFP_R07_IFP_RESET			0x07
+#define		MT9V111_IFP_R07_IFP_RESET_MASK		BIT(0)
+#define MT9V111_IFP_R08_OUTFMT_CTRL			0x08
+#define		MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER	BIT(11)
+#define		MT9V111_IFP_R08_OUTFMT_CTRL_PCLK	BIT(5)
+#define MT9V111_IFP_R3A_OUTFMT_CTRL2			0x3a
+#define		MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR	BIT(0)
+#define		MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC	BIT(1)
+#define		MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK	GENMASK(2, 0)
+#define MT9V111_IFP_RA5_HPAN				0xa5
+#define MT9V111_IFP_RA6_HZOOM				0xa6
+#define MT9V111_IFP_RA7_HOUT				0xa7
+#define MT9V111_IFP_RA8_VPAN				0xa8
+#define MT9V111_IFP_RA9_VZOOM				0xa9
+#define MT9V111_IFP_RAA_VOUT				0xaa
+#define MT9V111_IFP_DECIMATION_MASK			GENMASK(9, 0)
+#define MT9V111_IFP_DECIMATION_FREEZE			BIT(15)
+
+#define MT9V111_CORE_R03_WIN_HEIGHT			0x03
+#define		MT9V111_CORE_R03_WIN_V_OFFS		2
+#define MT9V111_CORE_R04_WIN_WIDTH			0x04
+#define		MT9V111_CORE_R04_WIN_H_OFFS		114
+#define MT9V111_CORE_R05_HBLANK				0x05
+#define		MT9V111_CORE_R05_MIN_HBLANK		0x09
+#define		MT9V111_CORE_R05_MAX_HBLANK		GENMASK(9, 0)
+#define		MT9V111_CORE_R05_DEF_HBLANK		0x26
+#define MT9V111_CORE_R06_VBLANK				0x06
+#define		MT9V111_CORE_R06_MIN_VBLANK		0x03
+#define		MT9V111_CORE_R06_MAX_VBLANK		GENMASK(11, 0)
+#define		MT9V111_CORE_R06_DEF_VBLANK		0x04
+#define MT9V111_CORE_R07_OUT_CTRL			0x07
+#define		MT9V111_CORE_R07_OUT_CTRL_SAMPLE	BIT(4)
+#define MT9V111_CORE_R09_PIXEL_INT			0x09
+#define		MT9V111_CORE_R09_PIXEL_INT_MASK		GENMASK(11, 0)
+#define MT9V111_CORE_R0D_CORE_RESET			0x0d
+#define		MT9V111_CORE_R0D_CORE_RESET_MASK	BIT(0)
+#define MT9V111_CORE_RFF_CHIP_VER			0xff
+
+#define MT9V111_PIXEL_ARRAY_WIDTH			640
+#define MT9V111_PIXEL_ARRAY_HEIGHT			480
+
+#define MT9V111_MAX_CLKIN				27000000
+
+/* The default sensor configuration at startup time. */
+static struct v4l2_mbus_framefmt mt9v111_def_fmt = {
+	.width		= 640,
+	.height		= 480,
+	.code		= MEDIA_BUS_FMT_UYVY8_2X8,
+	.field		= V4L2_FIELD_NONE,
+	.colorspace	= V4L2_COLORSPACE_SRGB,
+	.ycbcr_enc	= V4L2_YCBCR_ENC_601,
+	.quantization	= V4L2_QUANTIZATION_LIM_RANGE,
+	.xfer_func	= V4L2_XFER_FUNC_SRGB,
+};
+
+struct mt9v111_dev {
+	struct device *dev;
+	struct i2c_client *client;
+
+	u8 addr_space;
+
+	struct v4l2_subdev sd;
+#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+	struct media_pad pad;
+#endif
+
+	struct v4l2_ctrl *auto_awb;
+	struct v4l2_ctrl *auto_exp;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl_handler ctrls;
+
+	/* Output image format and sizes. */
+	struct v4l2_mbus_framefmt fmt;
+	unsigned int fps;
+
+	/* Protects power up/down sequences. */
+	struct mutex pwr_mutex;
+	int pwr_count;
+
+	/* Protects stream on/off sequences. */
+	struct mutex stream_mutex;
+	bool streaming;
+
+	/* Flags to mark HW settings as not yet applied. */
+	bool pending;
+
+	/* Clock provider and system clock frequency. */
+	struct clk *clk;
+	u32 sysclk;
+
+	struct gpio_desc *oe;
+	struct gpio_desc *standby;
+	struct gpio_desc *reset;
+};
+
+#define sd_to_mt9v111(__sd) container_of((__sd), struct mt9v111_dev, sd)
+
+/*
+ * mt9v111_mbus_fmt - List all media bus formats supported by the driver.
+ *
+ * Only list the media bus code here. The image sizes are freely configurable
+ * in the pixel array sizes range.
+ *
+ * The desired frame interval, in the supported frame interval range, is
+ * obtained by configuring blanking as the sensor does not have a PLL but
+ * only a fixed clock divider that generates the output pixel clock.
+ */
+static struct mt9v111_mbus_fmt {
+	u32	code;
+} mt9v111_formats[] = {
+	{
+		.code	= MEDIA_BUS_FMT_UYVY8_2X8,
+	},
+	{
+		.code	= MEDIA_BUS_FMT_YUYV8_2X8,
+	},
+	{
+		.code	= MEDIA_BUS_FMT_VYUY8_2X8,
+	},
+	{
+		.code	= MEDIA_BUS_FMT_YVYU8_2X8,
+	},
+};
+
+static u32 mt9v111_frame_intervals[] = {5, 10, 15, 20, 30};
+
+/*
+ * mt9v111_frame_sizes - List sensor's supported resolutions.
+ *
+ * Resolution generated through decimation in the IFP block from the
+ * full VGA pixel array.
+ */
+static struct v4l2_rect mt9v111_frame_sizes[] = {
+	{
+		.width	= 640,
+		.height	= 480,
+	},
+	{
+		.width	= 352,
+		.height	= 288
+	},
+	{
+		.width	= 320,
+		.height	= 240,
+	},
+	{
+		.width	= 176,
+		.height	= 144,
+	},
+	{
+		.width	= 160,
+		.height	= 120,
+	},
+};
+
+/* --- Device I/O access --- */
+
+static int __mt9v111_read(struct i2c_client *c, u8 reg, u16 *val)
+{
+	struct i2c_msg msg[2];
+	__be16 buf;
+	int ret;
+
+	msg[0].addr = c->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &reg;
+
+	msg[1].addr = c->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 2;
+	msg[1].buf = (char *)&buf;
+
+	ret = i2c_transfer(c->adapter, msg, 2);
+	if (ret < 0) {
+		dev_err(&c->dev, "i2c read transfer error: %d\n", ret);
+		return ret;
+	}
+
+	*val = be16_to_cpu(buf);
+
+	dev_dbg(&c->dev, "%s: %x=%x\n", __func__, reg, *val);
+
+	return 0;
+}
+
+static int __mt9v111_write(struct i2c_client *c, u8 reg, u16 val)
+{
+	struct i2c_msg msg;
+	u8 buf[3] = { 0 };
+	int ret;
+
+	buf[0] = reg;
+	buf[1] = val >> 8;
+	buf[2] = val & 0xff;
+
+	msg.addr = c->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = (char *)buf;
+
+	dev_dbg(&c->dev, "%s: %x = %x%x\n", __func__, reg, buf[1], buf[2]);
+
+	ret = i2c_transfer(c->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&c->dev, "i2c write transfer error: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __mt9v111_addr_space_select(struct i2c_client *c, u16 addr_space)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+	u16 val;
+	int ret;
+
+	if (mt9v111->addr_space == addr_space)
+		return 0;
+
+	ret = __mt9v111_write(c, MT9V111_R01_ADDR_SPACE, addr_space);
+	if (ret)
+		return ret;
+
+	/* Verify address space has been updated */
+	ret = __mt9v111_read(c, MT9V111_R01_ADDR_SPACE, &val);
+	if (ret)
+		return ret;
+
+	if (val != addr_space)
+		return -EINVAL;
+
+	mt9v111->addr_space = addr_space;
+
+	return 0;
+}
+
+static int mt9v111_read(struct i2c_client *c, u8 addr_space, u8 reg, u16 *val)
+{
+	int ret;
+
+	/* Select register address space first. */
+	ret = __mt9v111_addr_space_select(c, addr_space);
+	if (ret)
+		return ret;
+
+	ret = __mt9v111_read(c, reg, val);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int mt9v111_write(struct i2c_client *c, u8 addr_space, u8 reg, u16 val)
+{
+	int ret;
+
+	/* Select register address space first. */
+	ret = __mt9v111_addr_space_select(c, addr_space);
+	if (ret)
+		return ret;
+
+	ret = __mt9v111_write(c, reg, val);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int mt9v111_update(struct i2c_client *c, u8 addr_space, u8 reg,
+			  u16 mask, u16 val)
+{
+	u16 current_val;
+	int ret;
+
+	/* Select register address space first. */
+	ret = __mt9v111_addr_space_select(c, addr_space);
+	if (ret)
+		return ret;
+
+	/* Read the current register value, then update it. */
+	ret = __mt9v111_read(c, reg, &current_val);
+	if (ret)
+		return ret;
+
+	current_val &= ~mask;
+	current_val |= (val & mask);
+	ret = __mt9v111_write(c, reg, current_val);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+/* --- Sensor HW operations --- */
+
+static int __mt9v111_power_on(struct v4l2_subdev *sd)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+	int ret;
+
+	ret = clk_prepare_enable(mt9v111->clk);
+	if (ret)
+		return ret;
+
+	clk_set_rate(mt9v111->clk, mt9v111->sysclk);
+
+	gpiod_set_value(mt9v111->standby, 0);
+	usleep_range(500, 1000);
+
+	gpiod_set_value(mt9v111->oe, 1);
+	usleep_range(500, 1000);
+
+	return 0;
+}
+
+static int __mt9v111_power_off(struct v4l2_subdev *sd)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+
+	gpiod_set_value(mt9v111->oe, 0);
+	usleep_range(500, 1000);
+
+	gpiod_set_value(mt9v111->standby, 1);
+	usleep_range(500, 1000);
+
+	clk_disable_unprepare(mt9v111->clk);
+
+	return 0;
+}
+
+static int __mt9v111_hw_reset(struct mt9v111_dev *mt9v111)
+{
+	if (!mt9v111->reset)
+		return -EINVAL;
+
+	gpiod_set_value(mt9v111->reset, 1);
+	usleep_range(500, 1000);
+
+	gpiod_set_value(mt9v111->reset, 0);
+	usleep_range(500, 1000);
+
+	return 0;
+}
+
+static int __mt9v111_sw_reset(struct mt9v111_dev *mt9v111)
+{
+	struct i2c_client *c = mt9v111->client;
+	int ret;
+
+	/* Software reset core and IFP blocks. */
+
+	ret = mt9v111_update(c, MT9V111_R01_CORE,
+			     MT9V111_CORE_R0D_CORE_RESET,
+			     MT9V111_CORE_R0D_CORE_RESET_MASK, 1);
+	if (ret)
+		return ret;
+	usleep_range(500, 1000);
+
+	ret = mt9v111_update(c, MT9V111_R01_CORE,
+			     MT9V111_CORE_R0D_CORE_RESET,
+			     MT9V111_CORE_R0D_CORE_RESET_MASK, 0);
+	if (ret)
+		return ret;
+	usleep_range(500, 1000);
+
+	ret = mt9v111_update(c, MT9V111_R01_IFP,
+			     MT9V111_IFP_R07_IFP_RESET,
+			     MT9V111_IFP_R07_IFP_RESET_MASK, 1);
+	if (ret)
+		return ret;
+	usleep_range(500, 1000);
+
+	ret = mt9v111_update(c, MT9V111_R01_IFP,
+			     MT9V111_IFP_R07_IFP_RESET,
+			     MT9V111_IFP_R07_IFP_RESET_MASK, 0);
+	if (ret)
+		return ret;
+	usleep_range(500, 1000);
+
+	return 0;
+}
+
+static int mt9v111_calc_frame_rate(struct mt9v111_dev *mt9v111,
+				   struct v4l2_fract *tpf)
+{
+	unsigned int fps = tpf->numerator ?
+			   tpf->denominator / tpf->numerator :
+			   tpf->denominator;
+	unsigned int best_diff;
+	unsigned int frm_cols;
+	unsigned int row_pclk;
+	unsigned int best_fps;
+	unsigned int pclk;
+	unsigned int diff;
+	unsigned int idx;
+	unsigned int hb;
+	unsigned int vb;
+	unsigned int i;
+	int ret;
+
+	/* Approximate to the closest supported frame interval. */
+	best_diff = ~0L;
+	for (i = 0, idx = 0; i < ARRAY_SIZE(mt9v111_frame_intervals); i++) {
+		diff = abs(fps - mt9v111_frame_intervals[i]);
+		if (diff < best_diff) {
+			idx = i;
+			best_diff = diff;
+		}
+	}
+	fps = mt9v111_frame_intervals[idx];
+
+	/*
+	 * The sensor does not provide a PLL circuitry and pixel clock is
+	 * generated dividing the master clock source by two.
+	 *
+	 * Trow = (W + Hblank + 114) * 2 * (1 / SYSCLK)
+	 * TFrame = Trow * (H + Vblank + 2)
+	 *
+	 * FPS = (SYSCLK / 2) / (Trow * (H + Vblank + 2))
+	 *
+	 * This boils down to tune H and V blanks to best approximate the
+	 * above equation.
+	 *
+	 * Test all available H/V blank values, until we reach the
+	 * desired frame rate.
+	 */
+	best_fps = vb = hb = 0;
+	pclk = DIV_ROUND_CLOSEST(mt9v111->sysclk, 2);
+	row_pclk = MT9V111_PIXEL_ARRAY_WIDTH + 7 + MT9V111_CORE_R04_WIN_H_OFFS;
+	frm_cols = MT9V111_PIXEL_ARRAY_HEIGHT + 7 + MT9V111_CORE_R03_WIN_V_OFFS;
+
+	best_diff = ~0L;
+	for (vb = MT9V111_CORE_R06_MIN_VBLANK;
+	     vb < MT9V111_CORE_R06_MAX_VBLANK; vb++) {
+		for (hb = MT9V111_CORE_R05_MIN_HBLANK;
+		     hb < MT9V111_CORE_R05_MAX_HBLANK; hb += 10) {
+			unsigned int t_frame = (row_pclk + hb) *
+					       (frm_cols + vb);
+			unsigned int t_fps = DIV_ROUND_CLOSEST(pclk, t_frame);
+
+			diff = abs(fps - t_fps);
+			if (diff < best_diff) {
+				best_diff = diff;
+				best_fps = t_fps;
+
+				if (diff == 0)
+					break;
+			}
+		}
+
+		if (diff == 0)
+			break;
+	}
+
+	ret = v4l2_ctrl_s_ctrl_int64(mt9v111->hblank, hb);
+	if (ret)
+		return ret;
+
+	ret = v4l2_ctrl_s_ctrl_int64(mt9v111->vblank, vb);
+	if (ret)
+		return ret;
+
+	tpf->numerator = 1;
+	tpf->denominator = best_fps;
+
+	return 0;
+}
+
+static int mt9v111_hw_config(struct mt9v111_dev *mt9v111)
+{
+	struct i2c_client *c = mt9v111->client;
+	unsigned int ret;
+	u16 outfmtctrl2;
+
+	/* Force device reset. */
+	ret = __mt9v111_hw_reset(mt9v111);
+	if (ret == -EINVAL)
+		ret = __mt9v111_sw_reset(mt9v111);
+	if (ret)
+		return ret;
+
+	/* Configure internal clock sample rate. */
+	ret = mt9v111->sysclk < DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ?
+				mt9v111_update(c, MT9V111_R01_CORE,
+					MT9V111_CORE_R07_OUT_CTRL,
+					MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 1) :
+				mt9v111_update(c, MT9V111_R01_CORE,
+					MT9V111_CORE_R07_OUT_CTRL,
+					MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 0);
+	if (ret)
+		return ret;
+
+	/*
+	 * Configure output image format components ordering.
+	 *
+	 * TODO: IFP block can also output several RGB permutations, we only
+	 *	 support YUYV permutations at the moment.
+	 */
+	switch (mt9v111->fmt.code) {
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+			outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC;
+			break;
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+			outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR;
+			break;
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+			outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC |
+				      MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR;
+			break;
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+	default:
+			outfmtctrl2 = 0;
+			break;
+	}
+
+	ret = mt9v111_update(c, MT9V111_R01_IFP, MT9V111_IFP_R3A_OUTFMT_CTRL2,
+			     MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK,
+			     outfmtctrl2);
+	if (ret)
+		return ret;
+
+	/*
+	 * Do not change default sensor's core configuration:
+	 * output the whole 640x480 pixel array, skip 18 columns and 6 rows.
+	 *
+	 * Instead, control the output image size through IFP block.
+	 *
+	 * TODO: No zoom&pan support. Currently we control the output image
+	 *	 size only through decimation, with no zoom support.
+	 */
+	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA5_HPAN,
+			    MT9V111_IFP_DECIMATION_FREEZE);
+	if (ret)
+		return ret;
+
+	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA8_VPAN,
+			    MT9V111_IFP_DECIMATION_FREEZE);
+	if (ret)
+		return ret;
+
+	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA6_HZOOM,
+			    MT9V111_IFP_DECIMATION_FREEZE |
+			    MT9V111_PIXEL_ARRAY_WIDTH);
+	if (ret)
+		return ret;
+
+	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA9_VZOOM,
+			    MT9V111_IFP_DECIMATION_FREEZE |
+			    MT9V111_PIXEL_ARRAY_HEIGHT);
+	if (ret)
+		return ret;
+
+	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA7_HOUT,
+			    MT9V111_IFP_DECIMATION_FREEZE |
+			    mt9v111->fmt.width);
+	if (ret)
+		return ret;
+
+	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RAA_VOUT,
+			    mt9v111->fmt.height);
+	if (ret)
+		return ret;
+
+	/* Apply controls to set auto exp, auto awb and timings */
+	ret = v4l2_ctrl_handler_setup(&mt9v111->ctrls);
+	if (ret)
+		return ret;
+
+	/*
+	 * Set pixel integration time to the whole frame time.
+	 * This value controls the the shutter delay when running with AE
+	 * disabled. If longer than frame time, it affects the output
+	 * frame rate.
+	 */
+	return mt9v111_write(c, MT9V111_R01_CORE, MT9V111_CORE_R09_PIXEL_INT,
+			     MT9V111_PIXEL_ARRAY_HEIGHT);
+}
+
+/* ---  V4L2 subdev operations --- */
+
+static int mt9v111_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+	int pwr_count;
+	int ret = 0;
+
+	mutex_lock(&mt9v111->pwr_mutex);
+
+	/*
+	 * Make sure we're transitioning from 0 to 1, or viceversa,
+	 * before actually changing the power state.
+	 */
+	pwr_count = mt9v111->pwr_count;
+	pwr_count += on ? 1 : -1;
+	if (pwr_count == !!on) {
+		ret = on ? __mt9v111_power_on(sd) :
+			   __mt9v111_power_off(sd);
+		if (!ret)
+			/* All went well, updated power counter. */
+			mt9v111->pwr_count = pwr_count;
+
+		mutex_unlock(&mt9v111->pwr_mutex);
+
+		return ret;
+	}
+
+	/*
+	 * Update power counter to keep track of how many nested calls we
+	 * received.
+	 */
+	WARN_ON(pwr_count < 0 || pwr_count > 1);
+	mt9v111->pwr_count = pwr_count;
+
+	mutex_unlock(&mt9v111->pwr_mutex);
+
+	return ret;
+}
+
+static int mt9v111_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
+	int ret;
+
+	mutex_lock(&mt9v111->stream_mutex);
+
+	if (mt9v111->streaming == enable) {
+		mutex_unlock(&mt9v111->stream_mutex);
+		return 0;
+	}
+
+	ret = mt9v111_s_power(subdev, enable);
+	if (ret)
+		goto error_unlock;
+
+	if (enable && mt9v111->pending) {
+		ret = mt9v111_hw_config(mt9v111);
+		if (ret)
+			goto error_unlock;
+
+		/*
+		 * No need to update control here as far as only H/VBLANK are
+		 * supported and immediately programmed to registers in .s_ctrl
+		 */
+
+		mt9v111->pending = false;
+	}
+
+	mt9v111->streaming = enable ? true : false;
+	mutex_unlock(&mt9v111->stream_mutex);
+
+	return 0;
+
+error_unlock:
+	mutex_unlock(&mt9v111->stream_mutex);
+
+	return ret;
+}
+
+static int mt9v111_s_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_frame_interval *ival)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+	struct v4l2_fract *tpf = &ival->interval;
+	unsigned int fps = tpf->numerator ?
+			   tpf->denominator / tpf->numerator :
+			   tpf->denominator;
+	unsigned int max_fps;
+
+	if (!tpf->numerator)
+		tpf->numerator = 1;
+
+	mutex_lock(&mt9v111->stream_mutex);
+
+	if (mt9v111->streaming) {
+		mutex_unlock(&mt9v111->stream_mutex);
+		return -EBUSY;
+	}
+
+	if (mt9v111->fps == fps) {
+		mutex_unlock(&mt9v111->stream_mutex);
+		return 0;
+	}
+
+	/* Make sure frame rate/image sizes constraints are respected. */
+	if (mt9v111->fmt.width < QVGA_WIDTH &&
+	    mt9v111->fmt.height < QVGA_HEIGHT)
+		max_fps = 90;
+	else if (mt9v111->fmt.width < CIF_WIDTH &&
+		 mt9v111->fmt.height < CIF_HEIGHT)
+		max_fps = 60;
+	else
+		max_fps = mt9v111->sysclk <
+				DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ? 15 :
+									  30;
+
+	if (fps > max_fps) {
+		mutex_unlock(&mt9v111->stream_mutex);
+		return -EINVAL;
+	}
+
+	mt9v111_calc_frame_rate(mt9v111, tpf);
+
+	mt9v111->fps = fps;
+	mt9v111->pending = true;
+
+	mutex_unlock(&mt9v111->stream_mutex);
+
+	return 0;
+}
+
+static int mt9v111_g_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_frame_interval *ival)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+	struct v4l2_fract *tpf = &ival->interval;
+
+	mutex_lock(&mt9v111->stream_mutex);
+
+	tpf->numerator = 1;
+	tpf->denominator = mt9v111->fps;
+
+	mutex_unlock(&mt9v111->stream_mutex);
+
+	return 0;
+}
+
+static struct v4l2_mbus_framefmt *__mt9v111_get_pad_format(
+					struct mt9v111_dev *mt9v111,
+					struct v4l2_subdev_pad_config *cfg,
+					unsigned int pad,
+					enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+		return v4l2_subdev_get_try_format(&mt9v111->sd, cfg, pad);
+#else
+		return &cfg->try_fmt;
+#endif
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &mt9v111->fmt;
+	default:
+		return NULL;
+	}
+}
+
+static int mt9v111_enum_mbus_code(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index > ARRAY_SIZE(mt9v111_formats) - 1)
+		return -EINVAL;
+
+	code->code = mt9v111_formats[code->index].code;
+
+	return 0;
+}
+
+static int mt9v111_enum_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_frame_interval_enum *fie)
+{
+	unsigned int i;
+
+	if (fie->pad || fie->index >= ARRAY_SIZE(mt9v111_frame_intervals))
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++)
+		if (fie->width == mt9v111_frame_sizes[i].width &&
+		    fie->height == mt9v111_frame_sizes[i].height)
+			break;
+
+	if (i == ARRAY_SIZE(mt9v111_frame_sizes))
+		return -EINVAL;
+
+	fie->interval.numerator = 1;
+	fie->interval.denominator = mt9v111_frame_intervals[fie->index];
+
+	return 0;
+}
+
+static int mt9v111_enum_frame_size(struct v4l2_subdev *subdev,
+				   struct v4l2_subdev_pad_config *cfg,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->pad || fse->index > ARRAY_SIZE(mt9v111_frame_sizes))
+		return -EINVAL;
+
+	fse->min_width = mt9v111_frame_sizes[fse->index].width;
+	fse->max_width = mt9v111_frame_sizes[fse->index].width;
+	fse->min_height = mt9v111_frame_sizes[fse->index].height;
+	fse->max_height = mt9v111_frame_sizes[fse->index].height;
+
+	return 0;
+}
+
+static int mt9v111_get_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *format)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
+
+	if (format->pad)
+		return -EINVAL;
+
+	mutex_lock(&mt9v111->stream_mutex);
+	format->format = *__mt9v111_get_pad_format(mt9v111, cfg, format->pad,
+						   format->which);
+	mutex_unlock(&mt9v111->stream_mutex);
+
+	return 0;
+}
+
+static int mt9v111_set_format(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *format)
+{
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
+	struct v4l2_mbus_framefmt new_fmt;
+	struct v4l2_mbus_framefmt *__fmt;
+	unsigned int best_fit = ~0L;
+	unsigned int idx;
+	unsigned int i;
+
+	mutex_lock(&mt9v111->stream_mutex);
+	if (mt9v111->streaming) {
+		mutex_unlock(&mt9v111->stream_mutex);
+		return -EBUSY;
+	}
+
+	if (format->pad) {
+		mutex_unlock(&mt9v111->stream_mutex);
+		return -EINVAL;
+	}
+
+	/* Update mbus format code and sizes. */
+	for (i = 0; i < ARRAY_SIZE(mt9v111_formats); i++) {
+		if (format->format.code == mt9v111_formats[i].code) {
+			new_fmt.code = mt9v111_formats[i].code;
+			break;
+		}
+	}
+	if (i == ARRAY_SIZE(mt9v111_formats))
+		new_fmt.code = mt9v111_formats[0].code;
+
+	for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) {
+		unsigned int fit = abs(mt9v111_frame_sizes[i].width -
+				       format->format.width) +
+				   abs(mt9v111_frame_sizes[i].height -
+				       format->format.height);
+		if (fit < best_fit) {
+			best_fit = fit;
+			idx = i;
+
+			if (fit == 0)
+				break;
+		}
+	}
+	new_fmt.width = mt9v111_frame_sizes[idx].width;
+	new_fmt.height = mt9v111_frame_sizes[idx].height;
+
+	/* Update the device (or pad) format if it has changed. */
+	__fmt = __mt9v111_get_pad_format(mt9v111, cfg, format->pad,
+					 format->which);
+
+	/* Format hasn't changed, stop here. */
+	if (__fmt->code == new_fmt.code &&
+	    __fmt->width == new_fmt.width &&
+	    __fmt->height == new_fmt.height)
+		goto done;
+
+	/* Update the format and sizes, then  mark changes as pending. */
+	__fmt->code = new_fmt.code;
+	__fmt->width = new_fmt.width;
+	__fmt->height = new_fmt.height;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		mt9v111->pending = true;
+
+	dev_dbg(mt9v111->dev, "%s: mbus_code: %x - (%ux%u)\n",
+		__func__, __fmt->code, __fmt->width, __fmt->height);
+
+done:
+	format->format = *__fmt;
+
+	mutex_unlock(&mt9v111->stream_mutex);
+
+	return 0;
+}
+
+static int mt9v111_init_cfg(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_pad_config *cfg)
+{
+	cfg->try_fmt = mt9v111_def_fmt;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops mt9v111_core_ops = {
+	.s_power		= mt9v111_s_power,
+};
+
+static const struct v4l2_subdev_video_ops mt9v111_video_ops = {
+	.s_stream		= mt9v111_s_stream,
+	.s_frame_interval	= mt9v111_s_frame_interval,
+	.g_frame_interval	= mt9v111_g_frame_interval,
+};
+
+static const struct v4l2_subdev_pad_ops mt9v111_pad_ops = {
+	.init_cfg		= mt9v111_init_cfg,
+	.enum_mbus_code		= mt9v111_enum_mbus_code,
+	.enum_frame_size	= mt9v111_enum_frame_size,
+	.enum_frame_interval	= mt9v111_enum_frame_interval,
+	.get_fmt		= mt9v111_get_format,
+	.set_fmt		= mt9v111_set_format,
+};
+
+static const struct v4l2_subdev_ops mt9v111_ops = {
+	.core	= &mt9v111_core_ops,
+	.video	= &mt9v111_video_ops,
+	.pad	= &mt9v111_pad_ops,
+};
+
+#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+static const struct media_entity_operations mt9v111_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+#endif
+
+/* --- V4L2 ctrl --- */
+static int mt9v111_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9v111_dev *mt9v111 = container_of(ctrl->handler,
+						   struct mt9v111_dev,
+						   ctrls);
+	int ret;
+
+	mutex_lock(&mt9v111->pwr_mutex);
+	/*
+	 * If sensor is powered down, just cache new control values,
+	 * no actual register access.
+	 */
+	if (!mt9v111->pwr_count) {
+		mt9v111->pending = true;
+		mutex_unlock(&mt9v111->pwr_mutex);
+		return 0;
+	}
+	mutex_unlock(&mt9v111->pwr_mutex);
+
+	/*
+	 * Flickering control gets disabled if both auto exp and auto awb
+	 * are disabled too. If any of the two is enabled, enable it.
+	 *
+	 * Disabling flickering when ae and awb are off allows a more precise
+	 * control of the programmed frame rate.
+	 */
+	if (mt9v111->auto_exp->is_new || mt9v111->auto_awb->is_new) {
+		if (mt9v111->auto_exp->val == V4L2_EXPOSURE_MANUAL &&
+		    mt9v111->auto_awb->val == V4L2_WHITE_BALANCE_MANUAL)
+			ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
+					     MT9V111_IFP_R08_OUTFMT_CTRL,
+					     MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
+					     0);
+		else
+			ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
+					     MT9V111_IFP_R08_OUTFMT_CTRL,
+					     MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
+					     1);
+		if (ret)
+			return ret;
+	}
+
+	ret = -EINVAL;
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
+				     MT9V111_IFP_R06_OPMODE_CTRL,
+				     MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN,
+				     ctrl->val == V4L2_WHITE_BALANCE_AUTO ?
+				     MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN : 0);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
+				     MT9V111_IFP_R06_OPMODE_CTRL,
+				     MT9V111_IFP_R06_OPMODE_CTRL_AE_EN,
+				     ctrl->val == V4L2_EXPOSURE_AUTO ?
+				     MT9V111_IFP_R06_OPMODE_CTRL_AE_EN : 0);
+		break;
+	case V4L2_CID_HBLANK:
+		ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
+				     MT9V111_CORE_R05_HBLANK,
+				     MT9V111_CORE_R05_MAX_HBLANK,
+				     mt9v111->hblank->val);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
+				     MT9V111_CORE_R06_VBLANK,
+				     MT9V111_CORE_R06_MAX_VBLANK,
+				     mt9v111->vblank->val);
+		break;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = {
+	.s_ctrl = mt9v111_s_ctrl,
+};
+
+static int mt9v111_chip_probe(struct mt9v111_dev *mt9v111)
+{
+	int ret;
+	u16 val;
+
+	ret = __mt9v111_power_on(&mt9v111->sd);
+	if (ret)
+		return ret;
+
+	ret = mt9v111_read(mt9v111->client, MT9V111_R01_CORE,
+			   MT9V111_CORE_RFF_CHIP_VER, &val);
+	if (ret)
+		goto power_off;
+
+	if ((val >> 8) != MT9V111_CHIP_ID_HIGH &&
+	    (val & 0xff) != MT9V111_CHIP_ID_LOW) {
+		dev_err(mt9v111->dev,
+			"Unable to identify MT9V111 chip: 0x%2x%2x\n",
+			val >> 8, val & 0xff);
+		ret = -EIO;
+		goto power_off;
+	}
+
+	dev_dbg(mt9v111->dev, "Chip identified: 0x%2x%2x\n",
+		val >> 8, val & 0xff);
+
+power_off:
+	__mt9v111_power_off(&mt9v111->sd);
+
+	return ret;
+}
+
+static int mt9v111_probe(struct i2c_client *client)
+{
+	struct mt9v111_dev *mt9v111;
+	struct v4l2_fract tpf;
+	int ret;
+
+	mt9v111 = devm_kzalloc(&client->dev, sizeof(*mt9v111), GFP_KERNEL);
+	if (!mt9v111)
+		return -ENOMEM;
+
+	mt9v111->dev = &client->dev;
+	mt9v111->client = client;
+
+	mt9v111->clk = devm_clk_get(&client->dev, NULL);
+	if (IS_ERR(mt9v111->clk))
+		return PTR_ERR(mt9v111->clk);
+
+	mt9v111->sysclk = clk_get_rate(mt9v111->clk);
+	if (mt9v111->sysclk > MT9V111_MAX_CLKIN)
+		return -EINVAL;
+
+	mt9v111->oe = devm_gpiod_get_optional(&client->dev, "enable",
+					      GPIOD_OUT_LOW);
+	if (IS_ERR(mt9v111->oe)) {
+		dev_err(&client->dev, "Unable to get GPIO \"enable\": %ld\n",
+			PTR_ERR(mt9v111->oe));
+		return PTR_ERR(mt9v111->oe);
+	}
+
+	mt9v111->standby = devm_gpiod_get_optional(&client->dev, "standby",
+						   GPIOD_OUT_HIGH);
+	if (IS_ERR(mt9v111->standby)) {
+		dev_err(&client->dev, "Unable to get GPIO \"standby\": %ld\n",
+			PTR_ERR(mt9v111->standby));
+		return PTR_ERR(mt9v111->standby);
+	}
+
+	mt9v111->reset = devm_gpiod_get_optional(&client->dev, "reset",
+						 GPIOD_OUT_LOW);
+	if (IS_ERR(mt9v111->reset)) {
+		dev_err(&client->dev, "Unable to get GPIO \"reset\": %ld\n",
+			PTR_ERR(mt9v111->reset));
+		return PTR_ERR(mt9v111->reset);
+	}
+
+	mutex_init(&mt9v111->pwr_mutex);
+	mutex_init(&mt9v111->stream_mutex);
+
+	v4l2_ctrl_handler_init(&mt9v111->ctrls, 5);
+
+	mt9v111->auto_awb = v4l2_ctrl_new_std(&mt9v111->ctrls,
+					      &mt9v111_ctrl_ops,
+					      V4L2_CID_AUTO_WHITE_BALANCE,
+					      0, 1, 1,
+					      V4L2_WHITE_BALANCE_AUTO);
+	if (IS_ERR_OR_NULL(mt9v111->auto_awb)) {
+		ret = PTR_ERR(mt9v111->auto_awb);
+		goto error_free_ctrls;
+	}
+
+	mt9v111->auto_exp = v4l2_ctrl_new_std_menu(&mt9v111->ctrls,
+						   &mt9v111_ctrl_ops,
+						   V4L2_CID_EXPOSURE_AUTO,
+						   V4L2_EXPOSURE_MANUAL,
+						   0, V4L2_EXPOSURE_AUTO);
+	if (IS_ERR_OR_NULL(mt9v111->auto_exp)) {
+		ret = PTR_ERR(mt9v111->auto_exp);
+		goto error_free_ctrls;
+	}
+
+	/* Initialize timings */
+	mt9v111->hblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
+					    V4L2_CID_HBLANK,
+					    MT9V111_CORE_R05_MIN_HBLANK,
+					    MT9V111_CORE_R05_MAX_HBLANK, 1,
+					    MT9V111_CORE_R05_DEF_HBLANK);
+	if (IS_ERR_OR_NULL(mt9v111->hblank)) {
+		ret = PTR_ERR(mt9v111->hblank);
+		goto error_free_ctrls;
+	}
+
+	mt9v111->vblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
+					    V4L2_CID_VBLANK,
+					    MT9V111_CORE_R06_MIN_VBLANK,
+					    MT9V111_CORE_R06_MAX_VBLANK, 1,
+					    MT9V111_CORE_R06_DEF_VBLANK);
+	if (IS_ERR_OR_NULL(mt9v111->vblank)) {
+		ret = PTR_ERR(mt9v111->vblank);
+		goto error_free_ctrls;
+	}
+
+	/* PIXEL_RATE is fixed: just expose it to user space. */
+	v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
+			  V4L2_CID_PIXEL_RATE, 0,
+			  DIV_ROUND_CLOSEST(mt9v111->sysclk, 2), 1,
+			  DIV_ROUND_CLOSEST(mt9v111->sysclk, 2));
+
+	mt9v111->sd.ctrl_handler = &mt9v111->ctrls;
+
+	/* Start with default configuration: 640x480 UYVY. */
+	mt9v111->fmt	= mt9v111_def_fmt;
+
+	/* Re-calculate blankings for 640x480@15fps. */
+	mt9v111->fps		= 15;
+	tpf.numerator		= 1;
+	tpf.denominator		= mt9v111->fps;
+	mt9v111_calc_frame_rate(mt9v111, &tpf);
+
+	mt9v111->pwr_count	= 0;
+	mt9v111->addr_space	= MT9V111_R01_IFP;
+	mt9v111->pending	= true;
+
+	v4l2_i2c_subdev_init(&mt9v111->sd, client, &mt9v111_ops);
+
+#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+	mt9v111->sd.flags	|= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	mt9v111->sd.entity.ops	= &mt9v111_subdev_entity_ops;
+	mt9v111->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	mt9v111->pad.flags	= MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&mt9v111->sd.entity, 1, &mt9v111->pad);
+	if (ret)
+		goto error_free_ctrls;
+#endif
+
+	ret = mt9v111_chip_probe(mt9v111);
+	if (ret)
+		goto error_free_ctrls;
+
+	ret = v4l2_async_register_subdev(&mt9v111->sd);
+	if (ret)
+		goto error_free_ctrls;
+
+	return 0;
+
+error_free_ctrls:
+	v4l2_ctrl_handler_free(&mt9v111->ctrls);
+
+#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+	media_entity_cleanup(&mt9v111->sd.entity);
+#endif
+
+	mutex_destroy(&mt9v111->pwr_mutex);
+	mutex_destroy(&mt9v111->stream_mutex);
+
+	return ret;
+}
+
+static int mt9v111_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
+
+	v4l2_async_unregister_subdev(sd);
+
+	v4l2_ctrl_handler_free(&mt9v111->ctrls);
+
+#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+	media_entity_cleanup(&sd->entity);
+#endif
+
+	mutex_destroy(&mt9v111->pwr_mutex);
+	mutex_destroy(&mt9v111->stream_mutex);
+
+	devm_gpiod_put(mt9v111->dev, mt9v111->oe);
+	devm_gpiod_put(mt9v111->dev, mt9v111->standby);
+	devm_gpiod_put(mt9v111->dev, mt9v111->reset);
+
+	devm_clk_put(mt9v111->dev, mt9v111->clk);
+
+	return 0;
+}
+
+static const struct of_device_id mt9v111_of_match[] = {
+	{ .compatible = "aptina,mt9v111", },
+	{ /* sentinel */ },
+};
+
+static struct i2c_driver mt9v111_driver = {
+	.driver = {
+		.name = "mt9v111",
+		.of_match_table = mt9v111_of_match,
+	},
+	.probe_new	= mt9v111_probe,
+	.remove		= mt9v111_remove,
+};
+
+module_i2c_driver(mt9v111_driver);
+
+MODULE_DESCRIPTION("V4L2 sensor driver for Aptina MT9V111");
+MODULE_AUTHOR("Jacopo Mondi <jacopo@jmondi.org>");
+MODULE_LICENSE("GPL v2");
-- 
2.7.4