[PATCH v5 00/13] pxa_camera transition to v4l2 standalone device

[PATCH v5 00/13] pxa_camera transition to v4l2 standalone device

[Robert Jarzmik]

There is no change between v4 and v5, ie. the global diff is empty, only one
line was shifted to prevent breaking bisectablility.

All the text in https://lkml.org/lkml/2016/8/15/609 is still applicable.

Cheers.

--
Robert

Robert Jarzmik (13):
  media: mt9m111: make a standalone v4l2 subdevice
  media: mt9m111: use only the SRGB colorspace
  media: mt9m111: move mt9m111 out of soc_camera
  media: platform: pxa_camera: convert to vb2
  media: platform: pxa_camera: trivial move of functions
  media: platform: pxa_camera: introduce sensor_call
  media: platform: pxa_camera: make printk consistent
  media: platform: pxa_camera: add buffer sequencing
  media: platform: pxa_camera: remove set_crop
  media: platform: pxa_camera: make a standalone v4l2 device
  media: platform: pxa_camera: add debug register access
  media: platform: pxa_camera: change stop_streaming semantics
  media: platform: pxa_camera: move pxa_camera out of soc_camera

 drivers/media/i2c/Kconfig                      |    7 +
 drivers/media/i2c/Makefile                     |    1 +
 drivers/media/i2c/mt9m111.c                    | 1033 ++++++++++++
 drivers/media/i2c/soc_camera/Kconfig           |    7 +-
 drivers/media/i2c/soc_camera/Makefile          |    1 -
 drivers/media/i2c/soc_camera/mt9m111.c         | 1054 ------------
 drivers/media/platform/Kconfig                 |    8 +
 drivers/media/platform/Makefile                |    1 +
 drivers/media/platform/pxa_camera.c            | 2096 ++++++++++++++++++++++++
 drivers/media/platform/soc_camera/Kconfig      |    8 -
 drivers/media/platform/soc_camera/Makefile     |    1 -
 drivers/media/platform/soc_camera/pxa_camera.c | 1866 ---------------------
 include/linux/platform_data/media/camera-pxa.h |    2 +
 13 files changed, 3153 insertions(+), 2932 deletions(-)
 create mode 100644 drivers/media/i2c/mt9m111.c
 delete mode 100644 drivers/media/i2c/soc_camera/mt9m111.c
 create mode 100644 drivers/media/platform/pxa_camera.c
 delete mode 100644 drivers/media/platform/soc_camera/pxa_camera.c

-- 
2.1.4

[PATCH v5 01/13] media: mt9m111: make a standalone v4l2 subdevice

[Robert Jarzmik]

Remove the soc_camera adherence. Mostly the change removes the power
manipulation provided by soc_camera, and instead :
 - powers on the sensor when the s_power control is activated
 - powers on the sensor in initial probe
 - enables and disables the MCLK provided to it in power on/off

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
 drivers/media/i2c/soc_camera/mt9m111.c | 51 ++++++++++------------------------
 1 file changed, 15 insertions(+), 36 deletions(-)

diff --git a/drivers/media/i2c/soc_camera/mt9m111.c b/drivers/media/i2c/soc_camera/mt9m111.c
index 6dfaead6aaa8..a7efaa5964d1 100644
--- a/drivers/media/i2c/soc_camera/mt9m111.c
+++ b/drivers/media/i2c/soc_camera/mt9m111.c
@@ -16,10 +16,11 @@
 #include <linux/v4l2-mediabus.h>
 #include <linux/module.h>
 
-#include <media/soc_camera.h>
+#include <media/v4l2-async.h>
 #include <media/v4l2-clk.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
 
 /*
  * MT9M111, MT9M112 and MT9M131:
@@ -388,7 +389,7 @@ static int mt9m111_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	struct v4l2_rect rect = a->c;
 	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 	int width, height;
-	int ret;
+	int ret, align = 0;
 
 	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
@@ -396,17 +397,19 @@ static int mt9m111_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	if (mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
 	    mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
 		/* Bayer format - even size lengths */
-		rect.width	= ALIGN(rect.width, 2);
-		rect.height	= ALIGN(rect.height, 2);
+		align = 1;
 		/* Let the user play with the starting pixel */
 	}
 
 	/* FIXME: the datasheet doesn't specify minimum sizes */
-	soc_camera_limit_side(&rect.left, &rect.width,
-		     MT9M111_MIN_DARK_COLS, 2, MT9M111_MAX_WIDTH);
-
-	soc_camera_limit_side(&rect.top, &rect.height,
-		     MT9M111_MIN_DARK_ROWS, 2, MT9M111_MAX_HEIGHT);
+	v4l_bound_align_image(&rect.width, 2, MT9M111_MAX_WIDTH, align,
+			      &rect.height, 2, MT9M111_MAX_HEIGHT, align, 0);
+	rect.left = clamp(rect.left, MT9M111_MIN_DARK_COLS,
+			  MT9M111_MIN_DARK_COLS + MT9M111_MAX_WIDTH -
+			  (__s32)rect.width);
+	rect.top = clamp(rect.top, MT9M111_MIN_DARK_ROWS,
+			 MT9M111_MIN_DARK_ROWS + MT9M111_MAX_HEIGHT -
+			 (__s32)rect.height);
 
 	width = min(mt9m111->width, rect.width);
 	height = min(mt9m111->height, rect.height);
@@ -775,17 +778,16 @@ static int mt9m111_init(struct mt9m111 *mt9m111)
 static int mt9m111_power_on(struct mt9m111 *mt9m111)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
 	int ret;
 
-	ret = soc_camera_power_on(&client->dev, ssdd, mt9m111->clk);
+	ret = v4l2_clk_enable(mt9m111->clk);
 	if (ret < 0)
 		return ret;
 
 	ret = mt9m111_resume(mt9m111);
 	if (ret < 0) {
 		dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
-		soc_camera_power_off(&client->dev, ssdd, mt9m111->clk);
+		v4l2_clk_disable(mt9m111->clk);
 	}
 
 	return ret;
@@ -793,11 +795,8 @@ static int mt9m111_power_on(struct mt9m111 *mt9m111)
 
 static void mt9m111_power_off(struct mt9m111 *mt9m111)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
-
 	mt9m111_suspend(mt9m111);
-	soc_camera_power_off(&client->dev, ssdd, mt9m111->clk);
+	v4l2_clk_disable(mt9m111->clk);
 }
 
 static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
@@ -854,14 +853,10 @@ static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
 static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
 				struct v4l2_mbus_config *cfg)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
-
 	cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
 		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
 		V4L2_MBUS_DATA_ACTIVE_HIGH;
 	cfg->type = V4L2_MBUS_PARALLEL;
-	cfg->flags = soc_camera_apply_board_flags(ssdd, cfg);
 
 	return 0;
 }
@@ -933,20 +928,8 @@ static int mt9m111_probe(struct i2c_client *client,
 {
 	struct mt9m111 *mt9m111;
 	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
-	struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
 	int ret;
 
-	if (client->dev.of_node) {
-		ssdd = devm_kzalloc(&client->dev, sizeof(*ssdd), GFP_KERNEL);
-		if (!ssdd)
-			return -ENOMEM;
-		client->dev.platform_data = ssdd;
-	}
-	if (!ssdd) {
-		dev_err(&client->dev, "mt9m111: driver needs platform data\n");
-		return -EINVAL;
-	}
-
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
 		dev_warn(&adapter->dev,
 			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
@@ -992,10 +975,6 @@ static int mt9m111_probe(struct i2c_client *client,
 	mt9m111->lastpage	= -1;
 	mutex_init(&mt9m111->power_lock);
 
-	ret = soc_camera_power_init(&client->dev, ssdd);
-	if (ret < 0)
-		goto out_hdlfree;
-
 	ret = mt9m111_video_probe(client);
 	if (ret < 0)
 		goto out_hdlfree;
-- 
2.1.4

[PATCH v5 02/13] media: mt9m111: use only the SRGB colorspace

[Robert Jarzmik]

mt9m111 being a camera sensor, its colorspace should always be SRGB, for
both RGB based formats or YCbCr based ones.

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
 drivers/media/i2c/soc_camera/mt9m111.c | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/drivers/media/i2c/soc_camera/mt9m111.c b/drivers/media/i2c/soc_camera/mt9m111.c
index a7efaa5964d1..b7c4f371bae1 100644
--- a/drivers/media/i2c/soc_camera/mt9m111.c
+++ b/drivers/media/i2c/soc_camera/mt9m111.c
@@ -188,10 +188,10 @@ struct mt9m111_datafmt {
 };
 
 static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
-	{MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG},
-	{MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_JPEG},
-	{MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG},
-	{MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_JPEG},
+	{MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_SRGB},
 	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
 	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
 	{MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
-- 
2.1.4

[PATCH v5 03/13] media: mt9m111: move mt9m111 out of soc_camera

[Robert Jarzmik]

As the mt9m111 is now working as a standalone v4l2 subdevice sensor,
move it out of soc_camera directory and severe its dependency on
soc_camera.

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
 drivers/media/i2c/Kconfig              |    7 +
 drivers/media/i2c/Makefile             |    1 +
 drivers/media/i2c/mt9m111.c            | 1033 ++++++++++++++++++++++++++++++++
 drivers/media/i2c/soc_camera/Kconfig   |    7 +-
 drivers/media/i2c/soc_camera/Makefile  |    1 -
 drivers/media/i2c/soc_camera/mt9m111.c | 1033 --------------------------------
 6 files changed, 1046 insertions(+), 1036 deletions(-)
 create mode 100644 drivers/media/i2c/mt9m111.c
 delete mode 100644 drivers/media/i2c/soc_camera/mt9m111.c

diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index ce9006e10a30..7f8790507660 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -571,6 +571,13 @@ config VIDEO_MT9M032
 	  This driver supports MT9M032 camera sensors from Aptina, monochrome
 	  models only.
 
+config VIDEO_MT9M111
+	tristate "mt9m111, mt9m112 and mt9m131 support"
+	depends on I2C && VIDEO_V4L2
+	help
+	  This driver supports MT9M111, MT9M112 and MT9M131 cameras from
+	  Micron/Aptina
+
 config VIDEO_MT9P031
 	tristate "Aptina MT9P031 support"
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index 94f2c99e890d..a1a82331bebc 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -59,6 +59,7 @@ obj-$(CONFIG_VIDEO_OV7640) += ov7640.o
 obj-$(CONFIG_VIDEO_OV7670) += ov7670.o
 obj-$(CONFIG_VIDEO_OV9650) += ov9650.o
 obj-$(CONFIG_VIDEO_MT9M032) += mt9m032.o
+obj-$(CONFIG_VIDEO_MT9M111) += mt9m111.o
 obj-$(CONFIG_VIDEO_MT9P031) += mt9p031.o
 obj-$(CONFIG_VIDEO_MT9T001) += mt9t001.o
 obj-$(CONFIG_VIDEO_MT9V011) += mt9v011.o
diff --git a/drivers/media/i2c/mt9m111.c b/drivers/media/i2c/mt9m111.c
new file mode 100644
index 000000000000..b7c4f371bae1
--- /dev/null
+++ b/drivers/media/i2c/mt9m111.c
@@ -0,0 +1,1033 @@
+/*
+ * Driver for MT9M111/MT9M112/MT9M131 CMOS Image Sensor from Micron/Aptina
+ *
+ * Copyright (C) 2008, Robert Jarzmik <robert.jarzmik@free.fr>
+ *
+ * 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.
+ */
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/module.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-clk.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+/*
+ * MT9M111, MT9M112 and MT9M131:
+ * i2c address is 0x48 or 0x5d (depending on SADDR pin)
+ * The platform has to define struct i2c_board_info objects and link to them
+ * from struct soc_camera_host_desc
+ */
+
+/*
+ * Sensor core register addresses (0x000..0x0ff)
+ */
+#define MT9M111_CHIP_VERSION		0x000
+#define MT9M111_ROW_START		0x001
+#define MT9M111_COLUMN_START		0x002
+#define MT9M111_WINDOW_HEIGHT		0x003
+#define MT9M111_WINDOW_WIDTH		0x004
+#define MT9M111_HORIZONTAL_BLANKING_B	0x005
+#define MT9M111_VERTICAL_BLANKING_B	0x006
+#define MT9M111_HORIZONTAL_BLANKING_A	0x007
+#define MT9M111_VERTICAL_BLANKING_A	0x008
+#define MT9M111_SHUTTER_WIDTH		0x009
+#define MT9M111_ROW_SPEED		0x00a
+#define MT9M111_EXTRA_DELAY		0x00b
+#define MT9M111_SHUTTER_DELAY		0x00c
+#define MT9M111_RESET			0x00d
+#define MT9M111_READ_MODE_B		0x020
+#define MT9M111_READ_MODE_A		0x021
+#define MT9M111_FLASH_CONTROL		0x023
+#define MT9M111_GREEN1_GAIN		0x02b
+#define MT9M111_BLUE_GAIN		0x02c
+#define MT9M111_RED_GAIN		0x02d
+#define MT9M111_GREEN2_GAIN		0x02e
+#define MT9M111_GLOBAL_GAIN		0x02f
+#define MT9M111_CONTEXT_CONTROL		0x0c8
+#define MT9M111_PAGE_MAP		0x0f0
+#define MT9M111_BYTE_WISE_ADDR		0x0f1
+
+#define MT9M111_RESET_SYNC_CHANGES	(1 << 15)
+#define MT9M111_RESET_RESTART_BAD_FRAME	(1 << 9)
+#define MT9M111_RESET_SHOW_BAD_FRAMES	(1 << 8)
+#define MT9M111_RESET_RESET_SOC		(1 << 5)
+#define MT9M111_RESET_OUTPUT_DISABLE	(1 << 4)
+#define MT9M111_RESET_CHIP_ENABLE	(1 << 3)
+#define MT9M111_RESET_ANALOG_STANDBY	(1 << 2)
+#define MT9M111_RESET_RESTART_FRAME	(1 << 1)
+#define MT9M111_RESET_RESET_MODE	(1 << 0)
+
+#define MT9M111_RM_FULL_POWER_RD	(0 << 10)
+#define MT9M111_RM_LOW_POWER_RD		(1 << 10)
+#define MT9M111_RM_COL_SKIP_4X		(1 << 5)
+#define MT9M111_RM_ROW_SKIP_4X		(1 << 4)
+#define MT9M111_RM_COL_SKIP_2X		(1 << 3)
+#define MT9M111_RM_ROW_SKIP_2X		(1 << 2)
+#define MT9M111_RMB_MIRROR_COLS		(1 << 1)
+#define MT9M111_RMB_MIRROR_ROWS		(1 << 0)
+#define MT9M111_CTXT_CTRL_RESTART	(1 << 15)
+#define MT9M111_CTXT_CTRL_DEFECTCOR_B	(1 << 12)
+#define MT9M111_CTXT_CTRL_RESIZE_B	(1 << 10)
+#define MT9M111_CTXT_CTRL_CTRL2_B	(1 << 9)
+#define MT9M111_CTXT_CTRL_GAMMA_B	(1 << 8)
+#define MT9M111_CTXT_CTRL_XENON_EN	(1 << 7)
+#define MT9M111_CTXT_CTRL_READ_MODE_B	(1 << 3)
+#define MT9M111_CTXT_CTRL_LED_FLASH_EN	(1 << 2)
+#define MT9M111_CTXT_CTRL_VBLANK_SEL_B	(1 << 1)
+#define MT9M111_CTXT_CTRL_HBLANK_SEL_B	(1 << 0)
+
+/*
+ * Colorpipe register addresses (0x100..0x1ff)
+ */
+#define MT9M111_OPER_MODE_CTRL		0x106
+#define MT9M111_OUTPUT_FORMAT_CTRL	0x108
+#define MT9M111_REDUCER_XZOOM_B		0x1a0
+#define MT9M111_REDUCER_XSIZE_B		0x1a1
+#define MT9M111_REDUCER_YZOOM_B		0x1a3
+#define MT9M111_REDUCER_YSIZE_B		0x1a4
+#define MT9M111_REDUCER_XZOOM_A		0x1a6
+#define MT9M111_REDUCER_XSIZE_A		0x1a7
+#define MT9M111_REDUCER_YZOOM_A		0x1a9
+#define MT9M111_REDUCER_YSIZE_A		0x1aa
+
+#define MT9M111_OUTPUT_FORMAT_CTRL2_A	0x13a
+#define MT9M111_OUTPUT_FORMAT_CTRL2_B	0x19b
+
+#define MT9M111_OPMODE_AUTOEXPO_EN	(1 << 14)
+#define MT9M111_OPMODE_AUTOWHITEBAL_EN	(1 << 1)
+#define MT9M111_OUTFMT_FLIP_BAYER_COL	(1 << 9)
+#define MT9M111_OUTFMT_FLIP_BAYER_ROW	(1 << 8)
+#define MT9M111_OUTFMT_PROCESSED_BAYER	(1 << 14)
+#define MT9M111_OUTFMT_BYPASS_IFP	(1 << 10)
+#define MT9M111_OUTFMT_INV_PIX_CLOCK	(1 << 9)
+#define MT9M111_OUTFMT_RGB		(1 << 8)
+#define MT9M111_OUTFMT_RGB565		(0 << 6)
+#define MT9M111_OUTFMT_RGB555		(1 << 6)
+#define MT9M111_OUTFMT_RGB444x		(2 << 6)
+#define MT9M111_OUTFMT_RGBx444		(3 << 6)
+#define MT9M111_OUTFMT_TST_RAMP_OFF	(0 << 4)
+#define MT9M111_OUTFMT_TST_RAMP_COL	(1 << 4)
+#define MT9M111_OUTFMT_TST_RAMP_ROW	(2 << 4)
+#define MT9M111_OUTFMT_TST_RAMP_FRAME	(3 << 4)
+#define MT9M111_OUTFMT_SHIFT_3_UP	(1 << 3)
+#define MT9M111_OUTFMT_AVG_CHROMA	(1 << 2)
+#define MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN	(1 << 1)
+#define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B	(1 << 0)
+
+/*
+ * Camera control register addresses (0x200..0x2ff not implemented)
+ */
+
+#define reg_read(reg) mt9m111_reg_read(client, MT9M111_##reg)
+#define reg_write(reg, val) mt9m111_reg_write(client, MT9M111_##reg, (val))
+#define reg_set(reg, val) mt9m111_reg_set(client, MT9M111_##reg, (val))
+#define reg_clear(reg, val) mt9m111_reg_clear(client, MT9M111_##reg, (val))
+#define reg_mask(reg, val, mask) mt9m111_reg_mask(client, MT9M111_##reg, \
+		(val), (mask))
+
+#define MT9M111_MIN_DARK_ROWS	8
+#define MT9M111_MIN_DARK_COLS	26
+#define MT9M111_MAX_HEIGHT	1024
+#define MT9M111_MAX_WIDTH	1280
+
+struct mt9m111_context {
+	u16 read_mode;
+	u16 blanking_h;
+	u16 blanking_v;
+	u16 reducer_xzoom;
+	u16 reducer_yzoom;
+	u16 reducer_xsize;
+	u16 reducer_ysize;
+	u16 output_fmt_ctrl2;
+	u16 control;
+};
+
+static struct mt9m111_context context_a = {
+	.read_mode		= MT9M111_READ_MODE_A,
+	.blanking_h		= MT9M111_HORIZONTAL_BLANKING_A,
+	.blanking_v		= MT9M111_VERTICAL_BLANKING_A,
+	.reducer_xzoom		= MT9M111_REDUCER_XZOOM_A,
+	.reducer_yzoom		= MT9M111_REDUCER_YZOOM_A,
+	.reducer_xsize		= MT9M111_REDUCER_XSIZE_A,
+	.reducer_ysize		= MT9M111_REDUCER_YSIZE_A,
+	.output_fmt_ctrl2	= MT9M111_OUTPUT_FORMAT_CTRL2_A,
+	.control		= MT9M111_CTXT_CTRL_RESTART,
+};
+
+static struct mt9m111_context context_b = {
+	.read_mode		= MT9M111_READ_MODE_B,
+	.blanking_h		= MT9M111_HORIZONTAL_BLANKING_B,
+	.blanking_v		= MT9M111_VERTICAL_BLANKING_B,
+	.reducer_xzoom		= MT9M111_REDUCER_XZOOM_B,
+	.reducer_yzoom		= MT9M111_REDUCER_YZOOM_B,
+	.reducer_xsize		= MT9M111_REDUCER_XSIZE_B,
+	.reducer_ysize		= MT9M111_REDUCER_YSIZE_B,
+	.output_fmt_ctrl2	= MT9M111_OUTPUT_FORMAT_CTRL2_B,
+	.control		= MT9M111_CTXT_CTRL_RESTART |
+		MT9M111_CTXT_CTRL_DEFECTCOR_B | MT9M111_CTXT_CTRL_RESIZE_B |
+		MT9M111_CTXT_CTRL_CTRL2_B | MT9M111_CTXT_CTRL_GAMMA_B |
+		MT9M111_CTXT_CTRL_READ_MODE_B | MT9M111_CTXT_CTRL_VBLANK_SEL_B |
+		MT9M111_CTXT_CTRL_HBLANK_SEL_B,
+};
+
+/* MT9M111 has only one fixed colorspace per pixelcode */
+struct mt9m111_datafmt {
+	u32	code;
+	enum v4l2_colorspace		colorspace;
+};
+
+static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
+	{MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
+	{MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
+};
+
+struct mt9m111 {
+	struct v4l2_subdev subdev;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl *gain;
+	struct mt9m111_context *ctx;
+	struct v4l2_rect rect;	/* cropping rectangle */
+	struct v4l2_clk *clk;
+	unsigned int width;	/* output */
+	unsigned int height;	/* sizes */
+	struct mutex power_lock; /* lock to protect power_count */
+	int power_count;
+	const struct mt9m111_datafmt *fmt;
+	int lastpage;	/* PageMap cache value */
+};
+
+/* Find a data format by a pixel code */
+static const struct mt9m111_datafmt *mt9m111_find_datafmt(struct mt9m111 *mt9m111,
+						u32 code)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(mt9m111_colour_fmts); i++)
+		if (mt9m111_colour_fmts[i].code == code)
+			return mt9m111_colour_fmts + i;
+
+	return mt9m111->fmt;
+}
+
+static struct mt9m111 *to_mt9m111(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct mt9m111, subdev);
+}
+
+static int reg_page_map_set(struct i2c_client *client, const u16 reg)
+{
+	int ret;
+	u16 page;
+	struct mt9m111 *mt9m111 = to_mt9m111(client);
+
+	page = (reg >> 8);
+	if (page == mt9m111->lastpage)
+		return 0;
+	if (page > 2)
+		return -EINVAL;
+
+	ret = i2c_smbus_write_word_swapped(client, MT9M111_PAGE_MAP, page);
+	if (!ret)
+		mt9m111->lastpage = page;
+	return ret;
+}
+
+static int mt9m111_reg_read(struct i2c_client *client, const u16 reg)
+{
+	int ret;
+
+	ret = reg_page_map_set(client, reg);
+	if (!ret)
+		ret = i2c_smbus_read_word_swapped(client, reg & 0xff);
+
+	dev_dbg(&client->dev, "read  reg.%03x -> %04x\n", reg, ret);
+	return ret;
+}
+
+static int mt9m111_reg_write(struct i2c_client *client, const u16 reg,
+			     const u16 data)
+{
+	int ret;
+
+	ret = reg_page_map_set(client, reg);
+	if (!ret)
+		ret = i2c_smbus_write_word_swapped(client, reg & 0xff, data);
+	dev_dbg(&client->dev, "write reg.%03x = %04x -> %d\n", reg, data, ret);
+	return ret;
+}
+
+static int mt9m111_reg_set(struct i2c_client *client, const u16 reg,
+			   const u16 data)
+{
+	int ret;
+
+	ret = mt9m111_reg_read(client, reg);
+	if (ret >= 0)
+		ret = mt9m111_reg_write(client, reg, ret | data);
+	return ret;
+}
+
+static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg,
+			     const u16 data)
+{
+	int ret;
+
+	ret = mt9m111_reg_read(client, reg);
+	if (ret >= 0)
+		ret = mt9m111_reg_write(client, reg, ret & ~data);
+	return ret;
+}
+
+static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg,
+			    const u16 data, const u16 mask)
+{
+	int ret;
+
+	ret = mt9m111_reg_read(client, reg);
+	if (ret >= 0)
+		ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data);
+	return ret;
+}
+
+static int mt9m111_set_context(struct mt9m111 *mt9m111,
+			       struct mt9m111_context *ctx)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	return reg_write(CONTEXT_CONTROL, ctx->control);
+}
+
+static int mt9m111_setup_rect_ctx(struct mt9m111 *mt9m111,
+			struct mt9m111_context *ctx, struct v4l2_rect *rect,
+			unsigned int width, unsigned int height)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret = mt9m111_reg_write(client, ctx->reducer_xzoom, rect->width);
+	if (!ret)
+		ret = mt9m111_reg_write(client, ctx->reducer_yzoom, rect->height);
+	if (!ret)
+		ret = mt9m111_reg_write(client, ctx->reducer_xsize, width);
+	if (!ret)
+		ret = mt9m111_reg_write(client, ctx->reducer_ysize, height);
+	return ret;
+}
+
+static int mt9m111_setup_geometry(struct mt9m111 *mt9m111, struct v4l2_rect *rect,
+			int width, int height, u32 code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret;
+
+	ret = reg_write(COLUMN_START, rect->left);
+	if (!ret)
+		ret = reg_write(ROW_START, rect->top);
+
+	if (!ret)
+		ret = reg_write(WINDOW_WIDTH, rect->width);
+	if (!ret)
+		ret = reg_write(WINDOW_HEIGHT, rect->height);
+
+	if (code != MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
+		/* IFP in use, down-scaling possible */
+		if (!ret)
+			ret = mt9m111_setup_rect_ctx(mt9m111, &context_b,
+						     rect, width, height);
+		if (!ret)
+			ret = mt9m111_setup_rect_ctx(mt9m111, &context_a,
+						     rect, width, height);
+	}
+
+	dev_dbg(&client->dev, "%s(%x): %ux%u@%u:%u -> %ux%u = %d\n",
+		__func__, code, rect->width, rect->height, rect->left, rect->top,
+		width, height, ret);
+
+	return ret;
+}
+
+static int mt9m111_enable(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	return reg_write(RESET, MT9M111_RESET_CHIP_ENABLE);
+}
+
+static int mt9m111_reset(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret;
+
+	ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
+	if (!ret)
+		ret = reg_set(RESET, MT9M111_RESET_RESET_SOC);
+	if (!ret)
+		ret = reg_clear(RESET, MT9M111_RESET_RESET_MODE
+				| MT9M111_RESET_RESET_SOC);
+
+	return ret;
+}
+
+static int mt9m111_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
+{
+	struct v4l2_rect rect = a->c;
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+	int width, height;
+	int ret, align = 0;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
+	    mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
+		/* Bayer format - even size lengths */
+		align = 1;
+		/* Let the user play with the starting pixel */
+	}
+
+	/* FIXME: the datasheet doesn't specify minimum sizes */
+	v4l_bound_align_image(&rect.width, 2, MT9M111_MAX_WIDTH, align,
+			      &rect.height, 2, MT9M111_MAX_HEIGHT, align, 0);
+	rect.left = clamp(rect.left, MT9M111_MIN_DARK_COLS,
+			  MT9M111_MIN_DARK_COLS + MT9M111_MAX_WIDTH -
+			  (__s32)rect.width);
+	rect.top = clamp(rect.top, MT9M111_MIN_DARK_ROWS,
+			 MT9M111_MIN_DARK_ROWS + MT9M111_MAX_HEIGHT -
+			 (__s32)rect.height);
+
+	width = min(mt9m111->width, rect.width);
+	height = min(mt9m111->height, rect.height);
+
+	ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
+	if (!ret) {
+		mt9m111->rect = rect;
+		mt9m111->width = width;
+		mt9m111->height = height;
+	}
+
+	return ret;
+}
+
+static int mt9m111_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+
+	a->c	= mt9m111->rect;
+	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int mt9m111_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	a->bounds.left			= MT9M111_MIN_DARK_COLS;
+	a->bounds.top			= MT9M111_MIN_DARK_ROWS;
+	a->bounds.width			= MT9M111_MAX_WIDTH;
+	a->bounds.height		= MT9M111_MAX_HEIGHT;
+	a->defrect			= a->bounds;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int mt9m111_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+
+	if (format->pad)
+		return -EINVAL;
+
+	mf->width	= mt9m111->width;
+	mf->height	= mt9m111->height;
+	mf->code	= mt9m111->fmt->code;
+	mf->colorspace	= mt9m111->fmt->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
+			      u32 code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
+		MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB |
+		MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
+		MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 |
+		MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
+		MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+	int ret;
+
+	switch (code) {
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
+			MT9M111_OUTFMT_RGB;
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE:
+		data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
+		break;
+	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
+			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+		break;
+	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
+		break;
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+		break;
+	case MEDIA_BUS_FMT_RGB565_2X8_BE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
+		break;
+	case MEDIA_BUS_FMT_BGR565_2X8_BE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+		break;
+	case MEDIA_BUS_FMT_BGR565_2X8_LE:
+		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
+			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
+			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+		break;
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		data_outfmt2 = 0;
+		break;
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+		break;
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
+		break;
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
+			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
+		break;
+	default:
+		dev_err(&client->dev, "Pixel format not handled: %x\n", code);
+		return -EINVAL;
+	}
+
+	ret = mt9m111_reg_mask(client, context_a.output_fmt_ctrl2,
+			       data_outfmt2, mask_outfmt2);
+	if (!ret)
+		ret = mt9m111_reg_mask(client, context_b.output_fmt_ctrl2,
+				       data_outfmt2, mask_outfmt2);
+
+	return ret;
+}
+
+static int mt9m111_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+	const struct mt9m111_datafmt *fmt;
+	struct v4l2_rect *rect = &mt9m111->rect;
+	bool bayer;
+	int ret;
+
+	if (format->pad)
+		return -EINVAL;
+
+	fmt = mt9m111_find_datafmt(mt9m111, mf->code);
+
+	bayer = fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
+		fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE;
+
+	/*
+	 * With Bayer format enforce even side lengths, but let the user play
+	 * with the starting pixel
+	 */
+	if (bayer) {
+		rect->width = ALIGN(rect->width, 2);
+		rect->height = ALIGN(rect->height, 2);
+	}
+
+	if (fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
+		/* IFP bypass mode, no scaling */
+		mf->width = rect->width;
+		mf->height = rect->height;
+	} else {
+		/* No upscaling */
+		if (mf->width > rect->width)
+			mf->width = rect->width;
+		if (mf->height > rect->height)
+			mf->height = rect->height;
+	}
+
+	dev_dbg(&client->dev, "%s(): %ux%u, code=%x\n", __func__,
+		mf->width, mf->height, fmt->code);
+
+	mf->code = fmt->code;
+	mf->colorspace = fmt->colorspace;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *mf;
+		return 0;
+	}
+
+	ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
+	if (!ret)
+		ret = mt9m111_set_pixfmt(mt9m111, mf->code);
+	if (!ret) {
+		mt9m111->width	= mf->width;
+		mt9m111->height	= mf->height;
+		mt9m111->fmt	= fmt;
+	}
+
+	return ret;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mt9m111_g_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int val;
+
+	if (reg->reg > 0x2ff)
+		return -EINVAL;
+
+	val = mt9m111_reg_read(client, reg->reg);
+	reg->size = 2;
+	reg->val = (u64)val;
+
+	if (reg->val > 0xffff)
+		return -EIO;
+
+	return 0;
+}
+
+static int mt9m111_s_register(struct v4l2_subdev *sd,
+			      const struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg > 0x2ff)
+		return -EINVAL;
+
+	if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
+		return -EIO;
+
+	return 0;
+}
+#endif
+
+static int mt9m111_set_flip(struct mt9m111 *mt9m111, int flip, int mask)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret;
+
+	if (flip)
+		ret = mt9m111_reg_set(client, mt9m111->ctx->read_mode, mask);
+	else
+		ret = mt9m111_reg_clear(client, mt9m111->ctx->read_mode, mask);
+
+	return ret;
+}
+
+static int mt9m111_get_global_gain(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int data;
+
+	data = reg_read(GLOBAL_GAIN);
+	if (data >= 0)
+		return (data & 0x2f) * (1 << ((data >> 10) & 1)) *
+			(1 << ((data >> 9) & 1));
+	return data;
+}
+
+static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	u16 val;
+
+	if (gain > 63 * 2 * 2)
+		return -EINVAL;
+
+	if ((gain >= 64 * 2) && (gain < 63 * 2 * 2))
+		val = (1 << 10) | (1 << 9) | (gain / 4);
+	else if ((gain >= 64) && (gain < 64 * 2))
+		val = (1 << 9) | (gain / 2);
+	else
+		val = gain;
+
+	return reg_write(GLOBAL_GAIN, val);
+}
+
+static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+
+	if (val == V4L2_EXPOSURE_AUTO)
+		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
+	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
+}
+
+static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+
+	if (on)
+		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
+	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
+}
+
+static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct mt9m111 *mt9m111 = container_of(ctrl->handler,
+					       struct mt9m111, hdl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_VFLIP:
+		return mt9m111_set_flip(mt9m111, ctrl->val,
+					MT9M111_RMB_MIRROR_ROWS);
+	case V4L2_CID_HFLIP:
+		return mt9m111_set_flip(mt9m111, ctrl->val,
+					MT9M111_RMB_MIRROR_COLS);
+	case V4L2_CID_GAIN:
+		return mt9m111_set_global_gain(mt9m111, ctrl->val);
+	case V4L2_CID_EXPOSURE_AUTO:
+		return mt9m111_set_autoexposure(mt9m111, ctrl->val);
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		return mt9m111_set_autowhitebalance(mt9m111, ctrl->val);
+	}
+
+	return -EINVAL;
+}
+
+static int mt9m111_suspend(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret;
+
+	v4l2_ctrl_s_ctrl(mt9m111->gain, mt9m111_get_global_gain(mt9m111));
+
+	ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
+	if (!ret)
+		ret = reg_set(RESET, MT9M111_RESET_RESET_SOC |
+			      MT9M111_RESET_OUTPUT_DISABLE |
+			      MT9M111_RESET_ANALOG_STANDBY);
+	if (!ret)
+		ret = reg_clear(RESET, MT9M111_RESET_CHIP_ENABLE);
+
+	return ret;
+}
+
+static void mt9m111_restore_state(struct mt9m111 *mt9m111)
+{
+	mt9m111_set_context(mt9m111, mt9m111->ctx);
+	mt9m111_set_pixfmt(mt9m111, mt9m111->fmt->code);
+	mt9m111_setup_geometry(mt9m111, &mt9m111->rect,
+			mt9m111->width, mt9m111->height, mt9m111->fmt->code);
+	v4l2_ctrl_handler_setup(&mt9m111->hdl);
+}
+
+static int mt9m111_resume(struct mt9m111 *mt9m111)
+{
+	int ret = mt9m111_enable(mt9m111);
+	if (!ret)
+		ret = mt9m111_reset(mt9m111);
+	if (!ret)
+		mt9m111_restore_state(mt9m111);
+
+	return ret;
+}
+
+static int mt9m111_init(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret;
+
+	ret = mt9m111_enable(mt9m111);
+	if (!ret)
+		ret = mt9m111_reset(mt9m111);
+	if (!ret)
+		ret = mt9m111_set_context(mt9m111, mt9m111->ctx);
+	if (ret)
+		dev_err(&client->dev, "mt9m111 init failed: %d\n", ret);
+	return ret;
+}
+
+static int mt9m111_power_on(struct mt9m111 *mt9m111)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
+	int ret;
+
+	ret = v4l2_clk_enable(mt9m111->clk);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9m111_resume(mt9m111);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
+		v4l2_clk_disable(mt9m111->clk);
+	}
+
+	return ret;
+}
+
+static void mt9m111_power_off(struct mt9m111 *mt9m111)
+{
+	mt9m111_suspend(mt9m111);
+	v4l2_clk_disable(mt9m111->clk);
+}
+
+static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
+	int ret = 0;
+
+	mutex_lock(&mt9m111->power_lock);
+
+	/*
+	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
+	 * update the power state.
+	 */
+	if (mt9m111->power_count == !on) {
+		if (on)
+			ret = mt9m111_power_on(mt9m111);
+		else
+			mt9m111_power_off(mt9m111);
+	}
+
+	if (!ret) {
+		/* Update the power count. */
+		mt9m111->power_count += on ? 1 : -1;
+		WARN_ON(mt9m111->power_count < 0);
+	}
+
+	mutex_unlock(&mt9m111->power_lock);
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
+	.s_ctrl = mt9m111_s_ctrl,
+};
+
+static struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
+	.s_power	= mt9m111_s_power,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register	= mt9m111_g_register,
+	.s_register	= mt9m111_s_register,
+#endif
+};
+
+static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->pad || code->index >= ARRAY_SIZE(mt9m111_colour_fmts))
+		return -EINVAL;
+
+	code->code = mt9m111_colour_fmts[code->index].code;
+	return 0;
+}
+
+static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
+				struct v4l2_mbus_config *cfg)
+{
+	cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_DATA_ACTIVE_HIGH;
+	cfg->type = V4L2_MBUS_PARALLEL;
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
+	.s_crop		= mt9m111_s_crop,
+	.g_crop		= mt9m111_g_crop,
+	.cropcap	= mt9m111_cropcap,
+	.g_mbus_config	= mt9m111_g_mbus_config,
+};
+
+static const struct v4l2_subdev_pad_ops mt9m111_subdev_pad_ops = {
+	.enum_mbus_code = mt9m111_enum_mbus_code,
+	.get_fmt	= mt9m111_get_fmt,
+	.set_fmt	= mt9m111_set_fmt,
+};
+
+static struct v4l2_subdev_ops mt9m111_subdev_ops = {
+	.core	= &mt9m111_subdev_core_ops,
+	.video	= &mt9m111_subdev_video_ops,
+	.pad	= &mt9m111_subdev_pad_ops,
+};
+
+/*
+ * Interface active, can use i2c. If it fails, it can indeed mean, that
+ * this wasn't our capture interface, so, we wait for the right one
+ */
+static int mt9m111_video_probe(struct i2c_client *client)
+{
+	struct mt9m111 *mt9m111 = to_mt9m111(client);
+	s32 data;
+	int ret;
+
+	ret = mt9m111_s_power(&mt9m111->subdev, 1);
+	if (ret < 0)
+		return ret;
+
+	data = reg_read(CHIP_VERSION);
+
+	switch (data) {
+	case 0x143a: /* MT9M111 or MT9M131 */
+		dev_info(&client->dev,
+			"Detected a MT9M111/MT9M131 chip ID %x\n", data);
+		break;
+	case 0x148c: /* MT9M112 */
+		dev_info(&client->dev, "Detected a MT9M112 chip ID %x\n", data);
+		break;
+	default:
+		dev_err(&client->dev,
+			"No MT9M111/MT9M112/MT9M131 chip detected register read %x\n",
+			data);
+		ret = -ENODEV;
+		goto done;
+	}
+
+	ret = mt9m111_init(mt9m111);
+	if (ret)
+		goto done;
+
+	ret = v4l2_ctrl_handler_setup(&mt9m111->hdl);
+
+done:
+	mt9m111_s_power(&mt9m111->subdev, 0);
+	return ret;
+}
+
+static int mt9m111_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct mt9m111 *mt9m111;
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	int ret;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
+		dev_warn(&adapter->dev,
+			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
+		return -EIO;
+	}
+
+	mt9m111 = devm_kzalloc(&client->dev, sizeof(struct mt9m111), GFP_KERNEL);
+	if (!mt9m111)
+		return -ENOMEM;
+
+	mt9m111->clk = v4l2_clk_get(&client->dev, "mclk");
+	if (IS_ERR(mt9m111->clk))
+		return -EPROBE_DEFER;
+
+	/* Default HIGHPOWER context */
+	mt9m111->ctx = &context_b;
+
+	v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
+	v4l2_ctrl_handler_init(&mt9m111->hdl, 5);
+	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
+			V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+	mt9m111->gain = v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
+			V4L2_CID_GAIN, 0, 63 * 2 * 2, 1, 32);
+	v4l2_ctrl_new_std_menu(&mt9m111->hdl,
+			&mt9m111_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
+			V4L2_EXPOSURE_AUTO);
+	mt9m111->subdev.ctrl_handler = &mt9m111->hdl;
+	if (mt9m111->hdl.error) {
+		ret = mt9m111->hdl.error;
+		goto out_clkput;
+	}
+
+	/* Second stage probe - when a capture adapter is there */
+	mt9m111->rect.left	= MT9M111_MIN_DARK_COLS;
+	mt9m111->rect.top	= MT9M111_MIN_DARK_ROWS;
+	mt9m111->rect.width	= MT9M111_MAX_WIDTH;
+	mt9m111->rect.height	= MT9M111_MAX_HEIGHT;
+	mt9m111->fmt		= &mt9m111_colour_fmts[0];
+	mt9m111->lastpage	= -1;
+	mutex_init(&mt9m111->power_lock);
+
+	ret = mt9m111_video_probe(client);
+	if (ret < 0)
+		goto out_hdlfree;
+
+	mt9m111->subdev.dev = &client->dev;
+	ret = v4l2_async_register_subdev(&mt9m111->subdev);
+	if (ret < 0)
+		goto out_hdlfree;
+
+	return 0;
+
+out_hdlfree:
+	v4l2_ctrl_handler_free(&mt9m111->hdl);
+out_clkput:
+	v4l2_clk_put(mt9m111->clk);
+
+	return ret;
+}
+
+static int mt9m111_remove(struct i2c_client *client)
+{
+	struct mt9m111 *mt9m111 = to_mt9m111(client);
+
+	v4l2_async_unregister_subdev(&mt9m111->subdev);
+	v4l2_clk_put(mt9m111->clk);
+	v4l2_ctrl_handler_free(&mt9m111->hdl);
+
+	return 0;
+}
+static const struct of_device_id mt9m111_of_match[] = {
+	{ .compatible = "micron,mt9m111", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, mt9m111_of_match);
+
+static const struct i2c_device_id mt9m111_id[] = {
+	{ "mt9m111", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9m111_id);
+
+static struct i2c_driver mt9m111_i2c_driver = {
+	.driver = {
+		.name = "mt9m111",
+		.of_match_table = of_match_ptr(mt9m111_of_match),
+	},
+	.probe		= mt9m111_probe,
+	.remove		= mt9m111_remove,
+	.id_table	= mt9m111_id,
+};
+
+module_i2c_driver(mt9m111_i2c_driver);
+
+MODULE_DESCRIPTION("Micron/Aptina MT9M111/MT9M112/MT9M131 Camera driver");
+MODULE_AUTHOR("Robert Jarzmik");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/soc_camera/Kconfig b/drivers/media/i2c/soc_camera/Kconfig
index 23d352f0adf0..7704bcf5cc25 100644
--- a/drivers/media/i2c/soc_camera/Kconfig
+++ b/drivers/media/i2c/soc_camera/Kconfig
@@ -14,11 +14,14 @@ config SOC_CAMERA_MT9M001
 	  and colour models.
 
 config SOC_CAMERA_MT9M111
-	tristate "mt9m111, mt9m112 and mt9m131 support"
+	tristate "legacy soc_camera mt9m111, mt9m112 and mt9m131 support"
 	depends on SOC_CAMERA && I2C
+	select VIDEO_MT9M111
 	help
 	  This driver supports MT9M111, MT9M112 and MT9M131 cameras from
-	  Micron/Aptina
+	  Micron/Aptina.
+	  This is the legacy configuration which shouldn't be used anymore,
+	  while VIDEO_MT9M111 should be used instead.
 
 config SOC_CAMERA_MT9T031
 	tristate "mt9t031 support"
diff --git a/drivers/media/i2c/soc_camera/Makefile b/drivers/media/i2c/soc_camera/Makefile
index d0421feaa796..6f994f9353a0 100644
--- a/drivers/media/i2c/soc_camera/Makefile
+++ b/drivers/media/i2c/soc_camera/Makefile
@@ -1,6 +1,5 @@
 obj-$(CONFIG_SOC_CAMERA_IMX074)		+= imx074.o
 obj-$(CONFIG_SOC_CAMERA_MT9M001)	+= mt9m001.o
-obj-$(CONFIG_SOC_CAMERA_MT9M111)	+= mt9m111.o
 obj-$(CONFIG_SOC_CAMERA_MT9T031)	+= mt9t031.o
 obj-$(CONFIG_SOC_CAMERA_MT9T112)	+= mt9t112.o
 obj-$(CONFIG_SOC_CAMERA_MT9V022)	+= mt9v022.o
diff --git a/drivers/media/i2c/soc_camera/mt9m111.c b/drivers/media/i2c/soc_camera/mt9m111.c
deleted file mode 100644
index b7c4f371bae1..000000000000
--- a/drivers/media/i2c/soc_camera/mt9m111.c
+++ /dev/null
@@ -1,1033 +0,0 @@
-/*
- * Driver for MT9M111/MT9M112/MT9M131 CMOS Image Sensor from Micron/Aptina
- *
- * Copyright (C) 2008, Robert Jarzmik <robert.jarzmik@free.fr>
- *
- * 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.
- */
-#include <linux/videodev2.h>
-#include <linux/slab.h>
-#include <linux/i2c.h>
-#include <linux/log2.h>
-#include <linux/gpio.h>
-#include <linux/delay.h>
-#include <linux/v4l2-mediabus.h>
-#include <linux/module.h>
-
-#include <media/v4l2-async.h>
-#include <media/v4l2-clk.h>
-#include <media/v4l2-common.h>
-#include <media/v4l2-ctrls.h>
-#include <media/v4l2-device.h>
-
-/*
- * MT9M111, MT9M112 and MT9M131:
- * i2c address is 0x48 or 0x5d (depending on SADDR pin)
- * The platform has to define struct i2c_board_info objects and link to them
- * from struct soc_camera_host_desc
- */
-
-/*
- * Sensor core register addresses (0x000..0x0ff)
- */
-#define MT9M111_CHIP_VERSION		0x000
-#define MT9M111_ROW_START		0x001
-#define MT9M111_COLUMN_START		0x002
-#define MT9M111_WINDOW_HEIGHT		0x003
-#define MT9M111_WINDOW_WIDTH		0x004
-#define MT9M111_HORIZONTAL_BLANKING_B	0x005
-#define MT9M111_VERTICAL_BLANKING_B	0x006
-#define MT9M111_HORIZONTAL_BLANKING_A	0x007
-#define MT9M111_VERTICAL_BLANKING_A	0x008
-#define MT9M111_SHUTTER_WIDTH		0x009
-#define MT9M111_ROW_SPEED		0x00a
-#define MT9M111_EXTRA_DELAY		0x00b
-#define MT9M111_SHUTTER_DELAY		0x00c
-#define MT9M111_RESET			0x00d
-#define MT9M111_READ_MODE_B		0x020
-#define MT9M111_READ_MODE_A		0x021
-#define MT9M111_FLASH_CONTROL		0x023
-#define MT9M111_GREEN1_GAIN		0x02b
-#define MT9M111_BLUE_GAIN		0x02c
-#define MT9M111_RED_GAIN		0x02d
-#define MT9M111_GREEN2_GAIN		0x02e
-#define MT9M111_GLOBAL_GAIN		0x02f
-#define MT9M111_CONTEXT_CONTROL		0x0c8
-#define MT9M111_PAGE_MAP		0x0f0
-#define MT9M111_BYTE_WISE_ADDR		0x0f1
-
-#define MT9M111_RESET_SYNC_CHANGES	(1 << 15)
-#define MT9M111_RESET_RESTART_BAD_FRAME	(1 << 9)
-#define MT9M111_RESET_SHOW_BAD_FRAMES	(1 << 8)
-#define MT9M111_RESET_RESET_SOC		(1 << 5)
-#define MT9M111_RESET_OUTPUT_DISABLE	(1 << 4)
-#define MT9M111_RESET_CHIP_ENABLE	(1 << 3)
-#define MT9M111_RESET_ANALOG_STANDBY	(1 << 2)
-#define MT9M111_RESET_RESTART_FRAME	(1 << 1)
-#define MT9M111_RESET_RESET_MODE	(1 << 0)
-
-#define MT9M111_RM_FULL_POWER_RD	(0 << 10)
-#define MT9M111_RM_LOW_POWER_RD		(1 << 10)
-#define MT9M111_RM_COL_SKIP_4X		(1 << 5)
-#define MT9M111_RM_ROW_SKIP_4X		(1 << 4)
-#define MT9M111_RM_COL_SKIP_2X		(1 << 3)
-#define MT9M111_RM_ROW_SKIP_2X		(1 << 2)
-#define MT9M111_RMB_MIRROR_COLS		(1 << 1)
-#define MT9M111_RMB_MIRROR_ROWS		(1 << 0)
-#define MT9M111_CTXT_CTRL_RESTART	(1 << 15)
-#define MT9M111_CTXT_CTRL_DEFECTCOR_B	(1 << 12)
-#define MT9M111_CTXT_CTRL_RESIZE_B	(1 << 10)
-#define MT9M111_CTXT_CTRL_CTRL2_B	(1 << 9)
-#define MT9M111_CTXT_CTRL_GAMMA_B	(1 << 8)
-#define MT9M111_CTXT_CTRL_XENON_EN	(1 << 7)
-#define MT9M111_CTXT_CTRL_READ_MODE_B	(1 << 3)
-#define MT9M111_CTXT_CTRL_LED_FLASH_EN	(1 << 2)
-#define MT9M111_CTXT_CTRL_VBLANK_SEL_B	(1 << 1)
-#define MT9M111_CTXT_CTRL_HBLANK_SEL_B	(1 << 0)
-
-/*
- * Colorpipe register addresses (0x100..0x1ff)
- */
-#define MT9M111_OPER_MODE_CTRL		0x106
-#define MT9M111_OUTPUT_FORMAT_CTRL	0x108
-#define MT9M111_REDUCER_XZOOM_B		0x1a0
-#define MT9M111_REDUCER_XSIZE_B		0x1a1
-#define MT9M111_REDUCER_YZOOM_B		0x1a3
-#define MT9M111_REDUCER_YSIZE_B		0x1a4
-#define MT9M111_REDUCER_XZOOM_A		0x1a6
-#define MT9M111_REDUCER_XSIZE_A		0x1a7
-#define MT9M111_REDUCER_YZOOM_A		0x1a9
-#define MT9M111_REDUCER_YSIZE_A		0x1aa
-
-#define MT9M111_OUTPUT_FORMAT_CTRL2_A	0x13a
-#define MT9M111_OUTPUT_FORMAT_CTRL2_B	0x19b
-
-#define MT9M111_OPMODE_AUTOEXPO_EN	(1 << 14)
-#define MT9M111_OPMODE_AUTOWHITEBAL_EN	(1 << 1)
-#define MT9M111_OUTFMT_FLIP_BAYER_COL	(1 << 9)
-#define MT9M111_OUTFMT_FLIP_BAYER_ROW	(1 << 8)
-#define MT9M111_OUTFMT_PROCESSED_BAYER	(1 << 14)
-#define MT9M111_OUTFMT_BYPASS_IFP	(1 << 10)
-#define MT9M111_OUTFMT_INV_PIX_CLOCK	(1 << 9)
-#define MT9M111_OUTFMT_RGB		(1 << 8)
-#define MT9M111_OUTFMT_RGB565		(0 << 6)
-#define MT9M111_OUTFMT_RGB555		(1 << 6)
-#define MT9M111_OUTFMT_RGB444x		(2 << 6)
-#define MT9M111_OUTFMT_RGBx444		(3 << 6)
-#define MT9M111_OUTFMT_TST_RAMP_OFF	(0 << 4)
-#define MT9M111_OUTFMT_TST_RAMP_COL	(1 << 4)
-#define MT9M111_OUTFMT_TST_RAMP_ROW	(2 << 4)
-#define MT9M111_OUTFMT_TST_RAMP_FRAME	(3 << 4)
-#define MT9M111_OUTFMT_SHIFT_3_UP	(1 << 3)
-#define MT9M111_OUTFMT_AVG_CHROMA	(1 << 2)
-#define MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN	(1 << 1)
-#define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B	(1 << 0)
-
-/*
- * Camera control register addresses (0x200..0x2ff not implemented)
- */
-
-#define reg_read(reg) mt9m111_reg_read(client, MT9M111_##reg)
-#define reg_write(reg, val) mt9m111_reg_write(client, MT9M111_##reg, (val))
-#define reg_set(reg, val) mt9m111_reg_set(client, MT9M111_##reg, (val))
-#define reg_clear(reg, val) mt9m111_reg_clear(client, MT9M111_##reg, (val))
-#define reg_mask(reg, val, mask) mt9m111_reg_mask(client, MT9M111_##reg, \
-		(val), (mask))
-
-#define MT9M111_MIN_DARK_ROWS	8
-#define MT9M111_MIN_DARK_COLS	26
-#define MT9M111_MAX_HEIGHT	1024
-#define MT9M111_MAX_WIDTH	1280
-
-struct mt9m111_context {
-	u16 read_mode;
-	u16 blanking_h;
-	u16 blanking_v;
-	u16 reducer_xzoom;
-	u16 reducer_yzoom;
-	u16 reducer_xsize;
-	u16 reducer_ysize;
-	u16 output_fmt_ctrl2;
-	u16 control;
-};
-
-static struct mt9m111_context context_a = {
-	.read_mode		= MT9M111_READ_MODE_A,
-	.blanking_h		= MT9M111_HORIZONTAL_BLANKING_A,
-	.blanking_v		= MT9M111_VERTICAL_BLANKING_A,
-	.reducer_xzoom		= MT9M111_REDUCER_XZOOM_A,
-	.reducer_yzoom		= MT9M111_REDUCER_YZOOM_A,
-	.reducer_xsize		= MT9M111_REDUCER_XSIZE_A,
-	.reducer_ysize		= MT9M111_REDUCER_YSIZE_A,
-	.output_fmt_ctrl2	= MT9M111_OUTPUT_FORMAT_CTRL2_A,
-	.control		= MT9M111_CTXT_CTRL_RESTART,
-};
-
-static struct mt9m111_context context_b = {
-	.read_mode		= MT9M111_READ_MODE_B,
-	.blanking_h		= MT9M111_HORIZONTAL_BLANKING_B,
-	.blanking_v		= MT9M111_VERTICAL_BLANKING_B,
-	.reducer_xzoom		= MT9M111_REDUCER_XZOOM_B,
-	.reducer_yzoom		= MT9M111_REDUCER_YZOOM_B,
-	.reducer_xsize		= MT9M111_REDUCER_XSIZE_B,
-	.reducer_ysize		= MT9M111_REDUCER_YSIZE_B,
-	.output_fmt_ctrl2	= MT9M111_OUTPUT_FORMAT_CTRL2_B,
-	.control		= MT9M111_CTXT_CTRL_RESTART |
-		MT9M111_CTXT_CTRL_DEFECTCOR_B | MT9M111_CTXT_CTRL_RESIZE_B |
-		MT9M111_CTXT_CTRL_CTRL2_B | MT9M111_CTXT_CTRL_GAMMA_B |
-		MT9M111_CTXT_CTRL_READ_MODE_B | MT9M111_CTXT_CTRL_VBLANK_SEL_B |
-		MT9M111_CTXT_CTRL_HBLANK_SEL_B,
-};
-
-/* MT9M111 has only one fixed colorspace per pixelcode */
-struct mt9m111_datafmt {
-	u32	code;
-	enum v4l2_colorspace		colorspace;
-};
-
-static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
-	{MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB},
-	{MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_SRGB},
-	{MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB},
-	{MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_SRGB},
-	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
-	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
-	{MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
-	{MEDIA_BUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
-	{MEDIA_BUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB},
-	{MEDIA_BUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB},
-	{MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
-	{MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
-};
-
-struct mt9m111 {
-	struct v4l2_subdev subdev;
-	struct v4l2_ctrl_handler hdl;
-	struct v4l2_ctrl *gain;
-	struct mt9m111_context *ctx;
-	struct v4l2_rect rect;	/* cropping rectangle */
-	struct v4l2_clk *clk;
-	unsigned int width;	/* output */
-	unsigned int height;	/* sizes */
-	struct mutex power_lock; /* lock to protect power_count */
-	int power_count;
-	const struct mt9m111_datafmt *fmt;
-	int lastpage;	/* PageMap cache value */
-};
-
-/* Find a data format by a pixel code */
-static const struct mt9m111_datafmt *mt9m111_find_datafmt(struct mt9m111 *mt9m111,
-						u32 code)
-{
-	int i;
-	for (i = 0; i < ARRAY_SIZE(mt9m111_colour_fmts); i++)
-		if (mt9m111_colour_fmts[i].code == code)
-			return mt9m111_colour_fmts + i;
-
-	return mt9m111->fmt;
-}
-
-static struct mt9m111 *to_mt9m111(const struct i2c_client *client)
-{
-	return container_of(i2c_get_clientdata(client), struct mt9m111, subdev);
-}
-
-static int reg_page_map_set(struct i2c_client *client, const u16 reg)
-{
-	int ret;
-	u16 page;
-	struct mt9m111 *mt9m111 = to_mt9m111(client);
-
-	page = (reg >> 8);
-	if (page == mt9m111->lastpage)
-		return 0;
-	if (page > 2)
-		return -EINVAL;
-
-	ret = i2c_smbus_write_word_swapped(client, MT9M111_PAGE_MAP, page);
-	if (!ret)
-		mt9m111->lastpage = page;
-	return ret;
-}
-
-static int mt9m111_reg_read(struct i2c_client *client, const u16 reg)
-{
-	int ret;
-
-	ret = reg_page_map_set(client, reg);
-	if (!ret)
-		ret = i2c_smbus_read_word_swapped(client, reg & 0xff);
-
-	dev_dbg(&client->dev, "read  reg.%03x -> %04x\n", reg, ret);
-	return ret;
-}
-
-static int mt9m111_reg_write(struct i2c_client *client, const u16 reg,
-			     const u16 data)
-{
-	int ret;
-
-	ret = reg_page_map_set(client, reg);
-	if (!ret)
-		ret = i2c_smbus_write_word_swapped(client, reg & 0xff, data);
-	dev_dbg(&client->dev, "write reg.%03x = %04x -> %d\n", reg, data, ret);
-	return ret;
-}
-
-static int mt9m111_reg_set(struct i2c_client *client, const u16 reg,
-			   const u16 data)
-{
-	int ret;
-
-	ret = mt9m111_reg_read(client, reg);
-	if (ret >= 0)
-		ret = mt9m111_reg_write(client, reg, ret | data);
-	return ret;
-}
-
-static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg,
-			     const u16 data)
-{
-	int ret;
-
-	ret = mt9m111_reg_read(client, reg);
-	if (ret >= 0)
-		ret = mt9m111_reg_write(client, reg, ret & ~data);
-	return ret;
-}
-
-static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg,
-			    const u16 data, const u16 mask)
-{
-	int ret;
-
-	ret = mt9m111_reg_read(client, reg);
-	if (ret >= 0)
-		ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data);
-	return ret;
-}
-
-static int mt9m111_set_context(struct mt9m111 *mt9m111,
-			       struct mt9m111_context *ctx)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	return reg_write(CONTEXT_CONTROL, ctx->control);
-}
-
-static int mt9m111_setup_rect_ctx(struct mt9m111 *mt9m111,
-			struct mt9m111_context *ctx, struct v4l2_rect *rect,
-			unsigned int width, unsigned int height)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	int ret = mt9m111_reg_write(client, ctx->reducer_xzoom, rect->width);
-	if (!ret)
-		ret = mt9m111_reg_write(client, ctx->reducer_yzoom, rect->height);
-	if (!ret)
-		ret = mt9m111_reg_write(client, ctx->reducer_xsize, width);
-	if (!ret)
-		ret = mt9m111_reg_write(client, ctx->reducer_ysize, height);
-	return ret;
-}
-
-static int mt9m111_setup_geometry(struct mt9m111 *mt9m111, struct v4l2_rect *rect,
-			int width, int height, u32 code)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	int ret;
-
-	ret = reg_write(COLUMN_START, rect->left);
-	if (!ret)
-		ret = reg_write(ROW_START, rect->top);
-
-	if (!ret)
-		ret = reg_write(WINDOW_WIDTH, rect->width);
-	if (!ret)
-		ret = reg_write(WINDOW_HEIGHT, rect->height);
-
-	if (code != MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
-		/* IFP in use, down-scaling possible */
-		if (!ret)
-			ret = mt9m111_setup_rect_ctx(mt9m111, &context_b,
-						     rect, width, height);
-		if (!ret)
-			ret = mt9m111_setup_rect_ctx(mt9m111, &context_a,
-						     rect, width, height);
-	}
-
-	dev_dbg(&client->dev, "%s(%x): %ux%u@%u:%u -> %ux%u = %d\n",
-		__func__, code, rect->width, rect->height, rect->left, rect->top,
-		width, height, ret);
-
-	return ret;
-}
-
-static int mt9m111_enable(struct mt9m111 *mt9m111)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	return reg_write(RESET, MT9M111_RESET_CHIP_ENABLE);
-}
-
-static int mt9m111_reset(struct mt9m111 *mt9m111)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	int ret;
-
-	ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
-	if (!ret)
-		ret = reg_set(RESET, MT9M111_RESET_RESET_SOC);
-	if (!ret)
-		ret = reg_clear(RESET, MT9M111_RESET_RESET_MODE
-				| MT9M111_RESET_RESET_SOC);
-
-	return ret;
-}
-
-static int mt9m111_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
-{
-	struct v4l2_rect rect = a->c;
-	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
-	int width, height;
-	int ret, align = 0;
-
-	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	if (mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
-	    mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
-		/* Bayer format - even size lengths */
-		align = 1;
-		/* Let the user play with the starting pixel */
-	}
-
-	/* FIXME: the datasheet doesn't specify minimum sizes */
-	v4l_bound_align_image(&rect.width, 2, MT9M111_MAX_WIDTH, align,
-			      &rect.height, 2, MT9M111_MAX_HEIGHT, align, 0);
-	rect.left = clamp(rect.left, MT9M111_MIN_DARK_COLS,
-			  MT9M111_MIN_DARK_COLS + MT9M111_MAX_WIDTH -
-			  (__s32)rect.width);
-	rect.top = clamp(rect.top, MT9M111_MIN_DARK_ROWS,
-			 MT9M111_MIN_DARK_ROWS + MT9M111_MAX_HEIGHT -
-			 (__s32)rect.height);
-
-	width = min(mt9m111->width, rect.width);
-	height = min(mt9m111->height, rect.height);
-
-	ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
-	if (!ret) {
-		mt9m111->rect = rect;
-		mt9m111->width = width;
-		mt9m111->height = height;
-	}
-
-	return ret;
-}
-
-static int mt9m111_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
-{
-	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
-
-	a->c	= mt9m111->rect;
-	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	return 0;
-}
-
-static int mt9m111_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
-{
-	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	a->bounds.left			= MT9M111_MIN_DARK_COLS;
-	a->bounds.top			= MT9M111_MIN_DARK_ROWS;
-	a->bounds.width			= MT9M111_MAX_WIDTH;
-	a->bounds.height		= MT9M111_MAX_HEIGHT;
-	a->defrect			= a->bounds;
-	a->pixelaspect.numerator	= 1;
-	a->pixelaspect.denominator	= 1;
-
-	return 0;
-}
-
-static int mt9m111_get_fmt(struct v4l2_subdev *sd,
-		struct v4l2_subdev_pad_config *cfg,
-		struct v4l2_subdev_format *format)
-{
-	struct v4l2_mbus_framefmt *mf = &format->format;
-	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
-
-	if (format->pad)
-		return -EINVAL;
-
-	mf->width	= mt9m111->width;
-	mf->height	= mt9m111->height;
-	mf->code	= mt9m111->fmt->code;
-	mf->colorspace	= mt9m111->fmt->colorspace;
-	mf->field	= V4L2_FIELD_NONE;
-
-	return 0;
-}
-
-static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
-			      u32 code)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
-		MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB |
-		MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
-		MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 |
-		MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
-		MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
-	int ret;
-
-	switch (code) {
-	case MEDIA_BUS_FMT_SBGGR8_1X8:
-		data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
-			MT9M111_OUTFMT_RGB;
-		break;
-	case MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE:
-		data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
-		break;
-	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
-		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
-			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
-		break;
-	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
-		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
-		break;
-	case MEDIA_BUS_FMT_RGB565_2X8_LE:
-		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
-			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
-		break;
-	case MEDIA_BUS_FMT_RGB565_2X8_BE:
-		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
-		break;
-	case MEDIA_BUS_FMT_BGR565_2X8_BE:
-		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
-			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
-		break;
-	case MEDIA_BUS_FMT_BGR565_2X8_LE:
-		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
-			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
-			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
-		break;
-	case MEDIA_BUS_FMT_UYVY8_2X8:
-		data_outfmt2 = 0;
-		break;
-	case MEDIA_BUS_FMT_VYUY8_2X8:
-		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
-		break;
-	case MEDIA_BUS_FMT_YUYV8_2X8:
-		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
-		break;
-	case MEDIA_BUS_FMT_YVYU8_2X8:
-		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
-			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
-		break;
-	default:
-		dev_err(&client->dev, "Pixel format not handled: %x\n", code);
-		return -EINVAL;
-	}
-
-	ret = mt9m111_reg_mask(client, context_a.output_fmt_ctrl2,
-			       data_outfmt2, mask_outfmt2);
-	if (!ret)
-		ret = mt9m111_reg_mask(client, context_b.output_fmt_ctrl2,
-				       data_outfmt2, mask_outfmt2);
-
-	return ret;
-}
-
-static int mt9m111_set_fmt(struct v4l2_subdev *sd,
-		struct v4l2_subdev_pad_config *cfg,
-		struct v4l2_subdev_format *format)
-{
-	struct v4l2_mbus_framefmt *mf = &format->format;
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
-	const struct mt9m111_datafmt *fmt;
-	struct v4l2_rect *rect = &mt9m111->rect;
-	bool bayer;
-	int ret;
-
-	if (format->pad)
-		return -EINVAL;
-
-	fmt = mt9m111_find_datafmt(mt9m111, mf->code);
-
-	bayer = fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
-		fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE;
-
-	/*
-	 * With Bayer format enforce even side lengths, but let the user play
-	 * with the starting pixel
-	 */
-	if (bayer) {
-		rect->width = ALIGN(rect->width, 2);
-		rect->height = ALIGN(rect->height, 2);
-	}
-
-	if (fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
-		/* IFP bypass mode, no scaling */
-		mf->width = rect->width;
-		mf->height = rect->height;
-	} else {
-		/* No upscaling */
-		if (mf->width > rect->width)
-			mf->width = rect->width;
-		if (mf->height > rect->height)
-			mf->height = rect->height;
-	}
-
-	dev_dbg(&client->dev, "%s(): %ux%u, code=%x\n", __func__,
-		mf->width, mf->height, fmt->code);
-
-	mf->code = fmt->code;
-	mf->colorspace = fmt->colorspace;
-
-	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
-		cfg->try_fmt = *mf;
-		return 0;
-	}
-
-	ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
-	if (!ret)
-		ret = mt9m111_set_pixfmt(mt9m111, mf->code);
-	if (!ret) {
-		mt9m111->width	= mf->width;
-		mt9m111->height	= mf->height;
-		mt9m111->fmt	= fmt;
-	}
-
-	return ret;
-}
-
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-static int mt9m111_g_register(struct v4l2_subdev *sd,
-			      struct v4l2_dbg_register *reg)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	int val;
-
-	if (reg->reg > 0x2ff)
-		return -EINVAL;
-
-	val = mt9m111_reg_read(client, reg->reg);
-	reg->size = 2;
-	reg->val = (u64)val;
-
-	if (reg->val > 0xffff)
-		return -EIO;
-
-	return 0;
-}
-
-static int mt9m111_s_register(struct v4l2_subdev *sd,
-			      const struct v4l2_dbg_register *reg)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-
-	if (reg->reg > 0x2ff)
-		return -EINVAL;
-
-	if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
-		return -EIO;
-
-	return 0;
-}
-#endif
-
-static int mt9m111_set_flip(struct mt9m111 *mt9m111, int flip, int mask)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	int ret;
-
-	if (flip)
-		ret = mt9m111_reg_set(client, mt9m111->ctx->read_mode, mask);
-	else
-		ret = mt9m111_reg_clear(client, mt9m111->ctx->read_mode, mask);
-
-	return ret;
-}
-
-static int mt9m111_get_global_gain(struct mt9m111 *mt9m111)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	int data;
-
-	data = reg_read(GLOBAL_GAIN);
-	if (data >= 0)
-		return (data & 0x2f) * (1 << ((data >> 10) & 1)) *
-			(1 << ((data >> 9) & 1));
-	return data;
-}
-
-static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	u16 val;
-
-	if (gain > 63 * 2 * 2)
-		return -EINVAL;
-
-	if ((gain >= 64 * 2) && (gain < 63 * 2 * 2))
-		val = (1 << 10) | (1 << 9) | (gain / 4);
-	else if ((gain >= 64) && (gain < 64 * 2))
-		val = (1 << 9) | (gain / 2);
-	else
-		val = gain;
-
-	return reg_write(GLOBAL_GAIN, val);
-}
-
-static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int val)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-
-	if (val == V4L2_EXPOSURE_AUTO)
-		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
-	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
-}
-
-static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-
-	if (on)
-		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
-	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
-}
-
-static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct mt9m111 *mt9m111 = container_of(ctrl->handler,
-					       struct mt9m111, hdl);
-
-	switch (ctrl->id) {
-	case V4L2_CID_VFLIP:
-		return mt9m111_set_flip(mt9m111, ctrl->val,
-					MT9M111_RMB_MIRROR_ROWS);
-	case V4L2_CID_HFLIP:
-		return mt9m111_set_flip(mt9m111, ctrl->val,
-					MT9M111_RMB_MIRROR_COLS);
-	case V4L2_CID_GAIN:
-		return mt9m111_set_global_gain(mt9m111, ctrl->val);
-	case V4L2_CID_EXPOSURE_AUTO:
-		return mt9m111_set_autoexposure(mt9m111, ctrl->val);
-	case V4L2_CID_AUTO_WHITE_BALANCE:
-		return mt9m111_set_autowhitebalance(mt9m111, ctrl->val);
-	}
-
-	return -EINVAL;
-}
-
-static int mt9m111_suspend(struct mt9m111 *mt9m111)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	int ret;
-
-	v4l2_ctrl_s_ctrl(mt9m111->gain, mt9m111_get_global_gain(mt9m111));
-
-	ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
-	if (!ret)
-		ret = reg_set(RESET, MT9M111_RESET_RESET_SOC |
-			      MT9M111_RESET_OUTPUT_DISABLE |
-			      MT9M111_RESET_ANALOG_STANDBY);
-	if (!ret)
-		ret = reg_clear(RESET, MT9M111_RESET_CHIP_ENABLE);
-
-	return ret;
-}
-
-static void mt9m111_restore_state(struct mt9m111 *mt9m111)
-{
-	mt9m111_set_context(mt9m111, mt9m111->ctx);
-	mt9m111_set_pixfmt(mt9m111, mt9m111->fmt->code);
-	mt9m111_setup_geometry(mt9m111, &mt9m111->rect,
-			mt9m111->width, mt9m111->height, mt9m111->fmt->code);
-	v4l2_ctrl_handler_setup(&mt9m111->hdl);
-}
-
-static int mt9m111_resume(struct mt9m111 *mt9m111)
-{
-	int ret = mt9m111_enable(mt9m111);
-	if (!ret)
-		ret = mt9m111_reset(mt9m111);
-	if (!ret)
-		mt9m111_restore_state(mt9m111);
-
-	return ret;
-}
-
-static int mt9m111_init(struct mt9m111 *mt9m111)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	int ret;
-
-	ret = mt9m111_enable(mt9m111);
-	if (!ret)
-		ret = mt9m111_reset(mt9m111);
-	if (!ret)
-		ret = mt9m111_set_context(mt9m111, mt9m111->ctx);
-	if (ret)
-		dev_err(&client->dev, "mt9m111 init failed: %d\n", ret);
-	return ret;
-}
-
-static int mt9m111_power_on(struct mt9m111 *mt9m111)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
-	int ret;
-
-	ret = v4l2_clk_enable(mt9m111->clk);
-	if (ret < 0)
-		return ret;
-
-	ret = mt9m111_resume(mt9m111);
-	if (ret < 0) {
-		dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
-		v4l2_clk_disable(mt9m111->clk);
-	}
-
-	return ret;
-}
-
-static void mt9m111_power_off(struct mt9m111 *mt9m111)
-{
-	mt9m111_suspend(mt9m111);
-	v4l2_clk_disable(mt9m111->clk);
-}
-
-static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
-{
-	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
-	int ret = 0;
-
-	mutex_lock(&mt9m111->power_lock);
-
-	/*
-	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
-	 * update the power state.
-	 */
-	if (mt9m111->power_count == !on) {
-		if (on)
-			ret = mt9m111_power_on(mt9m111);
-		else
-			mt9m111_power_off(mt9m111);
-	}
-
-	if (!ret) {
-		/* Update the power count. */
-		mt9m111->power_count += on ? 1 : -1;
-		WARN_ON(mt9m111->power_count < 0);
-	}
-
-	mutex_unlock(&mt9m111->power_lock);
-	return ret;
-}
-
-static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
-	.s_ctrl = mt9m111_s_ctrl,
-};
-
-static struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
-	.s_power	= mt9m111_s_power,
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-	.g_register	= mt9m111_g_register,
-	.s_register	= mt9m111_s_register,
-#endif
-};
-
-static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
-		struct v4l2_subdev_pad_config *cfg,
-		struct v4l2_subdev_mbus_code_enum *code)
-{
-	if (code->pad || code->index >= ARRAY_SIZE(mt9m111_colour_fmts))
-		return -EINVAL;
-
-	code->code = mt9m111_colour_fmts[code->index].code;
-	return 0;
-}
-
-static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
-				struct v4l2_mbus_config *cfg)
-{
-	cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
-		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
-		V4L2_MBUS_DATA_ACTIVE_HIGH;
-	cfg->type = V4L2_MBUS_PARALLEL;
-
-	return 0;
-}
-
-static struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
-	.s_crop		= mt9m111_s_crop,
-	.g_crop		= mt9m111_g_crop,
-	.cropcap	= mt9m111_cropcap,
-	.g_mbus_config	= mt9m111_g_mbus_config,
-};
-
-static const struct v4l2_subdev_pad_ops mt9m111_subdev_pad_ops = {
-	.enum_mbus_code = mt9m111_enum_mbus_code,
-	.get_fmt	= mt9m111_get_fmt,
-	.set_fmt	= mt9m111_set_fmt,
-};
-
-static struct v4l2_subdev_ops mt9m111_subdev_ops = {
-	.core	= &mt9m111_subdev_core_ops,
-	.video	= &mt9m111_subdev_video_ops,
-	.pad	= &mt9m111_subdev_pad_ops,
-};
-
-/*
- * Interface active, can use i2c. If it fails, it can indeed mean, that
- * this wasn't our capture interface, so, we wait for the right one
- */
-static int mt9m111_video_probe(struct i2c_client *client)
-{
-	struct mt9m111 *mt9m111 = to_mt9m111(client);
-	s32 data;
-	int ret;
-
-	ret = mt9m111_s_power(&mt9m111->subdev, 1);
-	if (ret < 0)
-		return ret;
-
-	data = reg_read(CHIP_VERSION);
-
-	switch (data) {
-	case 0x143a: /* MT9M111 or MT9M131 */
-		dev_info(&client->dev,
-			"Detected a MT9M111/MT9M131 chip ID %x\n", data);
-		break;
-	case 0x148c: /* MT9M112 */
-		dev_info(&client->dev, "Detected a MT9M112 chip ID %x\n", data);
-		break;
-	default:
-		dev_err(&client->dev,
-			"No MT9M111/MT9M112/MT9M131 chip detected register read %x\n",
-			data);
-		ret = -ENODEV;
-		goto done;
-	}
-
-	ret = mt9m111_init(mt9m111);
-	if (ret)
-		goto done;
-
-	ret = v4l2_ctrl_handler_setup(&mt9m111->hdl);
-
-done:
-	mt9m111_s_power(&mt9m111->subdev, 0);
-	return ret;
-}
-
-static int mt9m111_probe(struct i2c_client *client,
-			 const struct i2c_device_id *did)
-{
-	struct mt9m111 *mt9m111;
-	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
-	int ret;
-
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
-		dev_warn(&adapter->dev,
-			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
-		return -EIO;
-	}
-
-	mt9m111 = devm_kzalloc(&client->dev, sizeof(struct mt9m111), GFP_KERNEL);
-	if (!mt9m111)
-		return -ENOMEM;
-
-	mt9m111->clk = v4l2_clk_get(&client->dev, "mclk");
-	if (IS_ERR(mt9m111->clk))
-		return -EPROBE_DEFER;
-
-	/* Default HIGHPOWER context */
-	mt9m111->ctx = &context_b;
-
-	v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
-	v4l2_ctrl_handler_init(&mt9m111->hdl, 5);
-	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
-			V4L2_CID_VFLIP, 0, 1, 1, 0);
-	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
-			V4L2_CID_HFLIP, 0, 1, 1, 0);
-	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
-			V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
-	mt9m111->gain = v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
-			V4L2_CID_GAIN, 0, 63 * 2 * 2, 1, 32);
-	v4l2_ctrl_new_std_menu(&mt9m111->hdl,
-			&mt9m111_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
-			V4L2_EXPOSURE_AUTO);
-	mt9m111->subdev.ctrl_handler = &mt9m111->hdl;
-	if (mt9m111->hdl.error) {
-		ret = mt9m111->hdl.error;
-		goto out_clkput;
-	}
-
-	/* Second stage probe - when a capture adapter is there */
-	mt9m111->rect.left	= MT9M111_MIN_DARK_COLS;
-	mt9m111->rect.top	= MT9M111_MIN_DARK_ROWS;
-	mt9m111->rect.width	= MT9M111_MAX_WIDTH;
-	mt9m111->rect.height	= MT9M111_MAX_HEIGHT;
-	mt9m111->fmt		= &mt9m111_colour_fmts[0];
-	mt9m111->lastpage	= -1;
-	mutex_init(&mt9m111->power_lock);
-
-	ret = mt9m111_video_probe(client);
-	if (ret < 0)
-		goto out_hdlfree;
-
-	mt9m111->subdev.dev = &client->dev;
-	ret = v4l2_async_register_subdev(&mt9m111->subdev);
-	if (ret < 0)
-		goto out_hdlfree;
-
-	return 0;
-
-out_hdlfree:
-	v4l2_ctrl_handler_free(&mt9m111->hdl);
-out_clkput:
-	v4l2_clk_put(mt9m111->clk);
-
-	return ret;
-}
-
-static int mt9m111_remove(struct i2c_client *client)
-{
-	struct mt9m111 *mt9m111 = to_mt9m111(client);
-
-	v4l2_async_unregister_subdev(&mt9m111->subdev);
-	v4l2_clk_put(mt9m111->clk);
-	v4l2_ctrl_handler_free(&mt9m111->hdl);
-
-	return 0;
-}
-static const struct of_device_id mt9m111_of_match[] = {
-	{ .compatible = "micron,mt9m111", },
-	{},
-};
-MODULE_DEVICE_TABLE(of, mt9m111_of_match);
-
-static const struct i2c_device_id mt9m111_id[] = {
-	{ "mt9m111", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, mt9m111_id);
-
-static struct i2c_driver mt9m111_i2c_driver = {
-	.driver = {
-		.name = "mt9m111",
-		.of_match_table = of_match_ptr(mt9m111_of_match),
-	},
-	.probe		= mt9m111_probe,
-	.remove		= mt9m111_remove,
-	.id_table	= mt9m111_id,
-};
-
-module_i2c_driver(mt9m111_i2c_driver);
-
-MODULE_DESCRIPTION("Micron/Aptina MT9M111/MT9M112/MT9M131 Camera driver");
-MODULE_AUTHOR("Robert Jarzmik");
-MODULE_LICENSE("GPL");
-- 
2.1.4

[PATCH v5 04/13] media: platform: pxa_camera: convert to vb2

[Robert Jarzmik]

Convert pxa_camera from videobuf to videobuf2.

As the soc_camera was already compatible with videobuf2, the port is
quite straightforward.

The special case of this code in which the vb2 to prepare is "too
big" in terms of size for the new capture format, the pxa_camera will
fail.

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
Since v1: took into account Hans's comments
	  renamed all vb2 functions to pxac_vb2_*()
Since v2: spit queue_buffer() and start_streaming()
Since v3: replace void *alloc_ctxs by struct device *alloc_devs
Since v4: move the queue device initialization to v4l2 standalone patch
---
 drivers/media/platform/soc_camera/Kconfig      |   4 +-
 drivers/media/platform/soc_camera/pxa_camera.c | 579 ++++++++++++-------------
 2 files changed, 269 insertions(+), 314 deletions(-)

diff --git a/drivers/media/platform/soc_camera/Kconfig b/drivers/media/platform/soc_camera/Kconfig
index 39f66414f621..3f927f96763a 100644
--- a/drivers/media/platform/soc_camera/Kconfig
+++ b/drivers/media/platform/soc_camera/Kconfig
@@ -19,8 +19,8 @@ config SOC_CAMERA_PLATFORM
 
 config VIDEO_PXA27x
 	tristate "PXA27x Quick Capture Interface driver"
-	depends on VIDEO_DEV && PXA27x && SOC_CAMERA
-	select VIDEOBUF_DMA_SG
+	depends on VIDEO_DEV && PXA27x && SOC_CAMERA && HAS_DMA
+	select VIDEOBUF2_DMA_SG
 	select SG_SPLIT
 	---help---
 	  This is a v4l2 driver for the PXA27x Quick Capture Interface
diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index 2aaf4a8f71a0..d1881d35d81d 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -34,7 +34,7 @@
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-dev.h>
-#include <media/videobuf-dma-sg.h>
+#include <media/videobuf2-dma-sg.h>
 #include <media/soc_camera.h>
 #include <media/drv-intf/soc_mediabus.h>
 #include <media/v4l2-of.h>
@@ -180,13 +180,16 @@ enum pxa_camera_active_dma {
 /* buffer for one video frame */
 struct pxa_buffer {
 	/* common v4l buffer stuff -- must be first */
-	struct videobuf_buffer		vb;
+	struct vb2_v4l2_buffer		vbuf;
+	struct list_head		queue;
 	u32	code;
+	int				nb_planes;
 	/* our descriptor lists for Y, U and V channels */
 	struct dma_async_tx_descriptor	*descs[3];
 	dma_cookie_t			cookie[3];
 	struct scatterlist		*sg[3];
 	int				sg_len[3];
+	size_t				plane_sizes[3];
 	int				inwork;
 	enum pxa_camera_active_dma	active_dma;
 };
@@ -230,59 +233,19 @@ struct pxa_cam {
 
 static const char *pxa_cam_driver_description = "PXA_Camera";
 
-static unsigned int vid_limit = 16;	/* Video memory limit, in Mb */
-
 /*
  *  Videobuf operations
  */
-static int pxa_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
-			      unsigned int *size)
+static struct pxa_buffer *vb2_to_pxa_buffer(struct vb2_buffer *vb)
 {
-	struct soc_camera_device *icd = vq->priv_data;
-
-	dev_dbg(icd->parent, "count=%d, size=%d\n", *count, *size);
-
-	*size = icd->sizeimage;
-
-	if (0 == *count)
-		*count = 32;
-	if (*size * *count > vid_limit * 1024 * 1024)
-		*count = (vid_limit * 1024 * 1024) / *size;
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 
-	return 0;
+	return container_of(vbuf, struct pxa_buffer, vbuf);
 }
 
-static void free_buffer(struct videobuf_queue *vq, struct pxa_buffer *buf)
+static struct device *pcdev_to_dev(struct pxa_camera_dev *pcdev)
 {
-	struct soc_camera_device *icd = vq->priv_data;
-	struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
-	int i;
-
-	BUG_ON(in_interrupt());
-
-	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
-		&buf->vb, buf->vb.baddr, buf->vb.bsize);
-
-	/*
-	 * This waits until this buffer is out of danger, i.e., until it is no
-	 * longer in STATE_QUEUED or STATE_ACTIVE
-	 */
-	videobuf_waiton(vq, &buf->vb, 0, 0);
-
-	for (i = 0; i < 3 && buf->descs[i]; i++) {
-		dmaengine_desc_free(buf->descs[i]);
-		kfree(buf->sg[i]);
-		buf->descs[i] = NULL;
-		buf->sg[i] = NULL;
-		buf->sg_len[i] = 0;
-	}
-	videobuf_dma_unmap(vq->dev, dma);
-	videobuf_dma_free(dma);
-
-	buf->vb.state = VIDEOBUF_NEEDS_INIT;
-
-	dev_dbg(icd->parent, "%s end (vb=0x%p) 0x%08lx %d\n", __func__,
-		&buf->vb, buf->vb.baddr, buf->vb.bsize);
+	return pcdev->soc_host.v4l2_dev.dev;
 }
 
 static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
@@ -312,31 +275,26 @@ static void pxa_camera_dma_irq_v(void *data)
 /**
  * pxa_init_dma_channel - init dma descriptors
  * @pcdev: pxa camera device
- * @buf: pxa buffer to find pxa dma channel
+ * @vb: videobuffer2 buffer
  * @dma: dma video buffer
  * @channel: dma channel (0 => 'Y', 1 => 'U', 2 => 'V')
  * @cibr: camera Receive Buffer Register
- * @size: bytes to transfer
- * @offset: offset in videobuffer of the first byte to transfer
  *
  * Prepares the pxa dma descriptors to transfer one camera channel.
  *
  * Returns 0 if success or -ENOMEM if no memory is available
  */
 static int pxa_init_dma_channel(struct pxa_camera_dev *pcdev,
-				struct pxa_buffer *buf,
-				struct videobuf_dmabuf *dma, int channel,
-				int cibr, int size, int offset)
+				struct pxa_buffer *buf, int channel,
+				struct scatterlist *sg, int sglen)
 {
 	struct dma_chan *dma_chan = pcdev->dma_chans[channel];
-	struct scatterlist *sg = buf->sg[channel];
-	int sglen = buf->sg_len[channel];
 	struct dma_async_tx_descriptor *tx;
 
 	tx = dmaengine_prep_slave_sg(dma_chan, sg, sglen, DMA_DEV_TO_MEM,
 				     DMA_PREP_INTERRUPT | DMA_CTRL_REUSE);
 	if (!tx) {
-		dev_err(pcdev->soc_host.v4l2_dev.dev,
+		dev_err(pcdev_to_dev(pcdev),
 			"dmaengine_prep_slave_sg failed\n");
 		goto fail;
 	}
@@ -357,11 +315,9 @@ static int pxa_init_dma_channel(struct pxa_camera_dev *pcdev,
 	buf->descs[channel] = tx;
 	return 0;
 fail:
-	kfree(sg);
-
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev,
-		"%s (vb=0x%p) dma_tx=%p\n",
-		__func__, &buf->vb, tx);
+	dev_dbg(pcdev_to_dev(pcdev),
+		"%s (vb=%p) dma_tx=%p\n",
+		__func__, buf, tx);
 
 	return -ENOMEM;
 }
@@ -374,129 +330,6 @@ static void pxa_videobuf_set_actdma(struct pxa_camera_dev *pcdev,
 		buf->active_dma |= DMA_U | DMA_V;
 }
 
-/*
- * Please check the DMA prepared buffer structure in :
- *   Documentation/video4linux/pxa_camera.txt
- * Please check also in pxa_camera_check_link_miss() to understand why DMA chain
- * modification while DMA chain is running will work anyway.
- */
-static int pxa_videobuf_prepare(struct videobuf_queue *vq,
-		struct videobuf_buffer *vb, enum v4l2_field field)
-{
-	struct soc_camera_device *icd = vq->priv_data;
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
-	struct device *dev = pcdev->soc_host.v4l2_dev.dev;
-	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
-	int ret;
-	int size_y, size_u = 0, size_v = 0;
-	size_t sizes[3];
-
-	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
-		vb, vb->baddr, vb->bsize);
-
-	/* Added list head initialization on alloc */
-	WARN_ON(!list_empty(&vb->queue));
-
-#ifdef DEBUG
-	/*
-	 * This can be useful if you want to see if we actually fill
-	 * the buffer with something
-	 */
-	memset((void *)vb->baddr, 0xaa, vb->bsize);
-#endif
-
-	BUG_ON(NULL == icd->current_fmt);
-
-	/*
-	 * I think, in buf_prepare you only have to protect global data,
-	 * the actual buffer is yours
-	 */
-	buf->inwork = 1;
-
-	if (buf->code	!= icd->current_fmt->code ||
-	    vb->width	!= icd->user_width ||
-	    vb->height	!= icd->user_height ||
-	    vb->field	!= field) {
-		buf->code	= icd->current_fmt->code;
-		vb->width	= icd->user_width;
-		vb->height	= icd->user_height;
-		vb->field	= field;
-		vb->state	= VIDEOBUF_NEEDS_INIT;
-	}
-
-	vb->size = icd->sizeimage;
-	if (0 != vb->baddr && vb->bsize < vb->size) {
-		ret = -EINVAL;
-		goto out;
-	}
-
-	if (vb->state == VIDEOBUF_NEEDS_INIT) {
-		int size = vb->size;
-		struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
-
-		ret = videobuf_iolock(vq, vb, NULL);
-		if (ret)
-			goto out;
-
-		if (pcdev->channels == 3) {
-			size_y = size / 2;
-			size_u = size_v = size / 4;
-		} else {
-			size_y = size;
-		}
-
-		sizes[0] = size_y;
-		sizes[1] = size_u;
-		sizes[2] = size_v;
-		ret = sg_split(dma->sglist, dma->sglen, 0, pcdev->channels,
-			       sizes, buf->sg, buf->sg_len, GFP_KERNEL);
-		if (ret < 0) {
-			dev_err(dev, "sg_split failed: %d\n", ret);
-			goto fail;
-		}
-
-		/* init DMA for Y channel */
-		ret = pxa_init_dma_channel(pcdev, buf, dma, 0, CIBR0,
-					   size_y, 0);
-		if (ret) {
-			dev_err(dev, "DMA initialization for Y/RGB failed\n");
-			goto fail;
-		}
-
-		/* init DMA for U channel */
-		if (size_u)
-			ret = pxa_init_dma_channel(pcdev, buf, dma, 1, CIBR1,
-						   size_u, size_y);
-		if (ret) {
-			dev_err(dev, "DMA initialization for U failed\n");
-			goto fail;
-		}
-
-		/* init DMA for V channel */
-		if (size_v)
-			ret = pxa_init_dma_channel(pcdev, buf, dma, 2, CIBR2,
-						   size_v, size_y + size_u);
-		if (ret) {
-			dev_err(dev, "DMA initialization for V failed\n");
-			goto fail;
-		}
-
-		vb->state = VIDEOBUF_PREPARED;
-	}
-
-	buf->inwork = 0;
-	pxa_videobuf_set_actdma(pcdev, buf);
-
-	return 0;
-
-fail:
-	free_buffer(vq, buf);
-out:
-	buf->inwork = 0;
-	return ret;
-}
-
 /**
  * pxa_dma_start_channels - start DMA channel for active buffer
  * @pcdev: pxa camera device
@@ -512,7 +345,7 @@ static void pxa_dma_start_channels(struct pxa_camera_dev *pcdev)
 	active = pcdev->active;
 
 	for (i = 0; i < pcdev->channels; i++) {
-		dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+		dev_dbg(pcdev_to_dev(pcdev),
 			"%s (channel=%d)\n", __func__, i);
 		dma_async_issue_pending(pcdev->dma_chans[i]);
 	}
@@ -523,7 +356,7 @@ static void pxa_dma_stop_channels(struct pxa_camera_dev *pcdev)
 	int i;
 
 	for (i = 0; i < pcdev->channels; i++) {
-		dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+		dev_dbg(pcdev_to_dev(pcdev),
 			"%s (channel=%d)\n", __func__, i);
 		dmaengine_terminate_all(pcdev->dma_chans[i]);
 	}
@@ -536,7 +369,7 @@ static void pxa_dma_add_tail_buf(struct pxa_camera_dev *pcdev,
 
 	for (i = 0; i < pcdev->channels; i++) {
 		buf->cookie[i] = dmaengine_submit(buf->descs[i]);
-		dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+		dev_dbg(pcdev_to_dev(pcdev),
 			"%s (channel=%d) : submit vb=%p cookie=%d\n",
 			__func__, i, buf, buf->descs[i]->cookie);
 	}
@@ -554,7 +387,7 @@ static void pxa_camera_start_capture(struct pxa_camera_dev *pcdev)
 {
 	unsigned long cicr0;
 
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev, "%s\n", __func__);
+	dev_dbg(pcdev_to_dev(pcdev), "%s\n", __func__);
 	__raw_writel(__raw_readl(pcdev->base + CISR), pcdev->base + CISR);
 	/* Enable End-Of-Frame Interrupt */
 	cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_ENB;
@@ -572,72 +405,20 @@ static void pxa_camera_stop_capture(struct pxa_camera_dev *pcdev)
 	__raw_writel(cicr0, pcdev->base + CICR0);
 
 	pcdev->active = NULL;
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev, "%s\n", __func__);
-}
-
-/* Called under spinlock_irqsave(&pcdev->lock, ...) */
-static void pxa_videobuf_queue(struct videobuf_queue *vq,
-			       struct videobuf_buffer *vb)
-{
-	struct soc_camera_device *icd = vq->priv_data;
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
-	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
-
-	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d active=%p\n",
-		__func__, vb, vb->baddr, vb->bsize, pcdev->active);
-
-	list_add_tail(&vb->queue, &pcdev->capture);
-
-	vb->state = VIDEOBUF_ACTIVE;
-	pxa_dma_add_tail_buf(pcdev, buf);
-
-	if (!pcdev->active)
-		pxa_camera_start_capture(pcdev);
-}
-
-static void pxa_videobuf_release(struct videobuf_queue *vq,
-				 struct videobuf_buffer *vb)
-{
-	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
-#ifdef DEBUG
-	struct soc_camera_device *icd = vq->priv_data;
-	struct device *dev = icd->parent;
-
-	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
-		vb, vb->baddr, vb->bsize);
-
-	switch (vb->state) {
-	case VIDEOBUF_ACTIVE:
-		dev_dbg(dev, "%s (active)\n", __func__);
-		break;
-	case VIDEOBUF_QUEUED:
-		dev_dbg(dev, "%s (queued)\n", __func__);
-		break;
-	case VIDEOBUF_PREPARED:
-		dev_dbg(dev, "%s (prepared)\n", __func__);
-		break;
-	default:
-		dev_dbg(dev, "%s (unknown)\n", __func__);
-		break;
-	}
-#endif
-
-	free_buffer(vq, buf);
+	dev_dbg(pcdev_to_dev(pcdev), "%s\n", __func__);
 }
 
 static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
-			      struct videobuf_buffer *vb,
 			      struct pxa_buffer *buf)
 {
+	struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+
 	/* _init is used to debug races, see comment in pxa_camera_reqbufs() */
-	list_del_init(&vb->queue);
-	vb->state = VIDEOBUF_DONE;
-	v4l2_get_timestamp(&vb->ts);
-	vb->field_count++;
-	wake_up(&vb->done);
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev, "%s dequeud buffer (vb=0x%p)\n",
-		__func__, vb);
+	list_del_init(&buf->queue);
+	vb->timestamp = ktime_get_ns();
+	vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+	dev_dbg(pcdev_to_dev(pcdev), "%s dequeud buffer (buf=0x%p)\n",
+		__func__, buf);
 
 	if (list_empty(&pcdev->capture)) {
 		pxa_camera_stop_capture(pcdev);
@@ -645,7 +426,7 @@ static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
 	}
 
 	pcdev->active = list_entry(pcdev->capture.next,
-				   struct pxa_buffer, vb.queue);
+				   struct pxa_buffer, queue);
 }
 
 /**
@@ -670,7 +451,7 @@ static void pxa_camera_check_link_miss(struct pxa_camera_dev *pcdev,
 {
 	bool is_dma_stopped = last_submitted != last_issued;
 
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+	dev_dbg(pcdev_to_dev(pcdev),
 		"%s : top queued buffer=%p, is_dma_stopped=%d\n",
 		__func__, pcdev->active, is_dma_stopped);
 
@@ -681,12 +462,11 @@ static void pxa_camera_check_link_miss(struct pxa_camera_dev *pcdev,
 static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 			       enum pxa_camera_active_dma act_dma)
 {
-	struct device *dev = pcdev->soc_host.v4l2_dev.dev;
+	struct device *dev = pcdev_to_dev(pcdev);
 	struct pxa_buffer *buf, *last_buf;
 	unsigned long flags;
 	u32 camera_status, overrun;
 	int chan;
-	struct videobuf_buffer *vb;
 	enum dma_status last_status;
 	dma_cookie_t last_issued;
 
@@ -714,9 +494,8 @@ static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 	if (!pcdev->active)
 		goto out;
 
-	vb = &pcdev->active->vb;
-	buf = container_of(vb, struct pxa_buffer, vb);
-	WARN_ON(buf->inwork || list_empty(&vb->queue));
+	buf = pcdev->active;
+	WARN_ON(buf->inwork || list_empty(&buf->queue));
 
 	/*
 	 * It's normal if the last frame creates an overrun, as there
@@ -734,7 +513,7 @@ static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 		break;
 	}
 	last_buf = list_entry(pcdev->capture.prev,
-			      struct pxa_buffer, vb.queue);
+			      struct pxa_buffer, queue);
 	last_status = dma_async_is_tx_complete(pcdev->dma_chans[chan],
 					       last_buf->cookie[chan],
 					       NULL, &last_issued);
@@ -743,14 +522,14 @@ static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 		dev_dbg(dev, "FIFO overrun! CISR: %x\n",
 			camera_status);
 		pxa_camera_stop_capture(pcdev);
-		list_for_each_entry(buf, &pcdev->capture, vb.queue)
+		list_for_each_entry(buf, &pcdev->capture, queue)
 			pxa_dma_add_tail_buf(pcdev, buf);
 		pxa_camera_start_capture(pcdev);
 		goto out;
 	}
 	buf->active_dma &= ~act_dma;
 	if (!buf->active_dma) {
-		pxa_camera_wakeup(pcdev, vb, buf);
+		pxa_camera_wakeup(pcdev, buf);
 		pxa_camera_check_link_miss(pcdev, last_buf->cookie[chan],
 					   last_issued);
 	}
@@ -759,26 +538,236 @@ out:
 	spin_unlock_irqrestore(&pcdev->lock, flags);
 }
 
-static struct videobuf_queue_ops pxa_videobuf_ops = {
-	.buf_setup      = pxa_videobuf_setup,
-	.buf_prepare    = pxa_videobuf_prepare,
-	.buf_queue      = pxa_videobuf_queue,
-	.buf_release    = pxa_videobuf_release,
+static void pxa_buffer_cleanup(struct pxa_buffer *buf)
+{
+	int i;
+
+	for (i = 0; i < 3 && buf->descs[i]; i++) {
+		dmaengine_desc_free(buf->descs[i]);
+		kfree(buf->sg[i]);
+		buf->descs[i] = NULL;
+		buf->sg[i] = NULL;
+		buf->sg_len[i] = 0;
+		buf->plane_sizes[i] = 0;
+	}
+	buf->nb_planes = 0;
+}
+
+static int pxa_buffer_init(struct pxa_camera_dev *pcdev,
+			   struct pxa_buffer *buf)
+{
+	struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
+	int nb_channels = pcdev->channels;
+	int i, ret = 0;
+	unsigned long size = vb2_plane_size(vb, 0);
+
+	switch (nb_channels) {
+	case 1:
+		buf->plane_sizes[0] = size;
+		break;
+	case 3:
+		buf->plane_sizes[0] = size / 2;
+		buf->plane_sizes[1] = size / 4;
+		buf->plane_sizes[2] = size / 4;
+		break;
+	default:
+		return -EINVAL;
+	};
+	buf->nb_planes = nb_channels;
+
+	ret = sg_split(sgt->sgl, sgt->nents, 0, nb_channels,
+		       buf->plane_sizes, buf->sg, buf->sg_len, GFP_KERNEL);
+	if (ret < 0) {
+		dev_err(pcdev_to_dev(pcdev),
+			"sg_split failed: %d\n", ret);
+		return ret;
+	}
+	for (i = 0; i < nb_channels; i++) {
+		ret = pxa_init_dma_channel(pcdev, buf, i,
+					   buf->sg[i], buf->sg_len[i]);
+		if (ret) {
+			pxa_buffer_cleanup(buf);
+			return ret;
+		}
+	}
+	INIT_LIST_HEAD(&buf->queue);
+
+	return ret;
+}
+
+static void pxac_vb2_cleanup(struct vb2_buffer *vb)
+{
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(vb=%p)\n", __func__, vb);
+	pxa_buffer_cleanup(buf);
+}
+
+static void pxac_vb2_queue(struct vb2_buffer *vb)
+{
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(vb=%p) nb_channels=%d size=%lu active=%p\n",
+		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0),
+		pcdev->active);
+
+	list_add_tail(&buf->queue, &pcdev->capture);
+
+	pxa_dma_add_tail_buf(pcdev, buf);
+}
+
+/*
+ * Please check the DMA prepared buffer structure in :
+ *   Documentation/video4linux/pxa_camera.txt
+ * Please check also in pxa_camera_check_link_miss() to understand why DMA chain
+ * modification while DMA chain is running will work anyway.
+ */
+static int pxac_vb2_prepare(struct vb2_buffer *vb)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+	int ret = 0;
+
+	switch (pcdev->channels) {
+	case 1:
+	case 3:
+		vb2_set_plane_payload(vb, 0, icd->sizeimage);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s (vb=%p) nb_channels=%d size=%lu\n",
+		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0));
+
+	WARN_ON(!icd->current_fmt);
+
+#ifdef DEBUG
+	/*
+	 * This can be useful if you want to see if we actually fill
+	 * the buffer with something
+	 */
+	for (i = 0; i < vb->num_planes; i++)
+		memset((void *)vb2_plane_vaddr(vb, i),
+		       0xaa, vb2_get_plane_payload(vb, i));
+#endif
+
+	/*
+	 * I think, in buf_prepare you only have to protect global data,
+	 * the actual buffer is yours
+	 */
+	buf->inwork = 0;
+	pxa_videobuf_set_actdma(pcdev, buf);
+
+	return ret;
+}
+
+static int pxac_vb2_init(struct vb2_buffer *vb)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(nb_channels=%d)\n",
+		__func__, pcdev->channels);
+
+	return pxa_buffer_init(pcdev, buf);
+}
+
+static int pxac_vb2_queue_setup(struct vb2_queue *vq,
+				unsigned int *nbufs,
+				unsigned int *num_planes, unsigned int sizes[],
+				struct device *alloc_devs[])
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
+	int size = icd->sizeimage;
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(vq=%p nbufs=%d num_planes=%d size=%d)\n",
+		__func__, vq, *nbufs, *num_planes, size);
+	/*
+	 * Called from VIDIOC_REQBUFS or in compatibility mode For YUV422P
+	 * format, even if there are 3 planes Y, U and V, we reply there is only
+	 * one plane, containing Y, U and V data, one after the other.
+	 */
+	if (*num_planes)
+		return sizes[0] < size ? -EINVAL : 0;
+
+	*num_planes = 1;
+	switch (pcdev->channels) {
+	case 1:
+	case 3:
+		sizes[0] = size;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (!*nbufs)
+		*nbufs = 1;
+
+	return 0;
+}
+
+static int pxac_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
+
+	dev_dbg(pcdev_to_dev(pcdev), "%s(count=%d) active=%p\n",
+		__func__, count, pcdev->active);
+
+	if (!pcdev->active)
+		pxa_camera_start_capture(pcdev);
+
+	return 0;
+}
+
+static void pxac_vb2_stop_streaming(struct vb2_queue *vq)
+{
+	vb2_wait_for_all_buffers(vq);
+}
+
+static struct vb2_ops pxac_vb2_ops = {
+	.queue_setup		= pxac_vb2_queue_setup,
+	.buf_init		= pxac_vb2_init,
+	.buf_prepare		= pxac_vb2_prepare,
+	.buf_queue		= pxac_vb2_queue,
+	.buf_cleanup		= pxac_vb2_cleanup,
+	.start_streaming	= pxac_vb2_start_streaming,
+	.stop_streaming		= pxac_vb2_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
 };
 
-static void pxa_camera_init_videobuf(struct videobuf_queue *q,
-			      struct soc_camera_device *icd)
+static int pxa_camera_init_videobuf2(struct vb2_queue *vq,
+				     struct soc_camera_device *icd)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct pxa_camera_dev *pcdev = ici->priv;
+	int ret;
 
-	/*
-	 * We must pass NULL as dev pointer, then all pci_* dma operations
-	 * transform to normal dma_* ones.
-	 */
-	videobuf_queue_sg_init(q, &pxa_videobuf_ops, NULL, &pcdev->lock,
-				V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
-				sizeof(struct pxa_buffer), icd, &ici->host_lock);
+	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	vq->drv_priv = pcdev;
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vq->buf_struct_size = sizeof(struct pxa_buffer);
+
+	vq->ops = &pxac_vb2_ops;
+	vq->mem_ops = &vb2_dma_sg_memops;
+
+	ret = vb2_queue_init(vq);
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "vb2_queue_init(vq=%p): %d\n", vq, ret);
+
+	return ret;
 }
 
 static u32 mclk_get_divisor(struct platform_device *pdev,
@@ -860,9 +849,8 @@ static void pxa_camera_eof(unsigned long arg)
 	struct pxa_camera_dev *pcdev = (struct pxa_camera_dev *)arg;
 	unsigned long cifr;
 	struct pxa_buffer *buf;
-	struct videobuf_buffer *vb;
 
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+	dev_dbg(pcdev_to_dev(pcdev),
 		"Camera interrupt status 0x%x\n",
 		__raw_readl(pcdev->base + CISR));
 
@@ -871,9 +859,8 @@ static void pxa_camera_eof(unsigned long arg)
 	__raw_writel(cifr, pcdev->base + CIFR);
 
 	pcdev->active = list_first_entry(&pcdev->capture,
-					 struct pxa_buffer, vb.queue);
-	vb = &pcdev->active->vb;
-	buf = container_of(vb, struct pxa_buffer, vb);
+					 struct pxa_buffer, queue);
+	buf = pcdev->active;
 	pxa_videobuf_set_actdma(pcdev, buf);
 
 	pxa_dma_start_channels(pcdev);
@@ -885,7 +872,7 @@ static irqreturn_t pxa_camera_irq(int irq, void *data)
 	unsigned long status, cicr0;
 
 	status = __raw_readl(pcdev->base + CISR);
-	dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+	dev_dbg(pcdev_to_dev(pcdev),
 		"Camera interrupt status 0x%lx\n", status);
 
 	if (!status)
@@ -1489,42 +1476,11 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 	return ret;
 }
 
-static int pxa_camera_reqbufs(struct soc_camera_device *icd,
-			      struct v4l2_requestbuffers *p)
-{
-	int i;
-
-	/*
-	 * This is for locking debugging only. I removed spinlocks and now I
-	 * check whether .prepare is ever called on a linked buffer, or whether
-	 * a dma IRQ can occur for an in-work or unlinked buffer. Until now
-	 * it hadn't triggered
-	 */
-	for (i = 0; i < p->count; i++) {
-		struct pxa_buffer *buf = container_of(icd->vb_vidq.bufs[i],
-						      struct pxa_buffer, vb);
-		buf->inwork = 0;
-		INIT_LIST_HEAD(&buf->vb.queue);
-	}
-
-	return 0;
-}
-
 static unsigned int pxa_camera_poll(struct file *file, poll_table *pt)
 {
 	struct soc_camera_device *icd = file->private_data;
-	struct pxa_buffer *buf;
-
-	buf = list_entry(icd->vb_vidq.stream.next, struct pxa_buffer,
-			 vb.stream);
 
-	poll_wait(file, &buf->vb.done, pt);
-
-	if (buf->vb.state == VIDEOBUF_DONE ||
-	    buf->vb.state == VIDEOBUF_ERROR)
-		return POLLIN|POLLRDNORM;
-
-	return 0;
+	return vb2_poll(&icd->vb2_vidq, file, pt);
 }
 
 static int pxa_camera_querycap(struct soc_camera_host *ici,
@@ -1597,8 +1553,7 @@ static struct soc_camera_host_ops pxa_soc_camera_host_ops = {
 	.put_formats	= pxa_camera_put_formats,
 	.set_fmt	= pxa_camera_set_fmt,
 	.try_fmt	= pxa_camera_try_fmt,
-	.init_videobuf	= pxa_camera_init_videobuf,
-	.reqbufs	= pxa_camera_reqbufs,
+	.init_videobuf2	= pxa_camera_init_videobuf2,
 	.poll		= pxa_camera_poll,
 	.querycap	= pxa_camera_querycap,
 	.set_bus_param	= pxa_camera_set_bus_param,
-- 
2.1.4

[PATCH v5 05/13] media: platform: pxa_camera: trivial move of functions

[Robert Jarzmik]

Move the functions in the file to be regrouped into meaningful blocks :
 1. pxa camera core handling functions, manipulating the herdware
 2. videobuf2 functions, dealing with video buffers
 3. video ioctl (vidioc) related functions
 4. driver probing, removal, suspend and resume

This patch doesn't modify a single line of code.

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
Since v3: replace void *alloc_ctxt by struct device *alloc_devs impact
Since v4: videobuf2 device init change impact
---
 drivers/media/platform/soc_camera/pxa_camera.c | 473 +++++++++++++------------
 1 file changed, 241 insertions(+), 232 deletions(-)

diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index d1881d35d81d..9d7c30cb1463 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -538,238 +538,6 @@ out:
 	spin_unlock_irqrestore(&pcdev->lock, flags);
 }
 
-static void pxa_buffer_cleanup(struct pxa_buffer *buf)
-{
-	int i;
-
-	for (i = 0; i < 3 && buf->descs[i]; i++) {
-		dmaengine_desc_free(buf->descs[i]);
-		kfree(buf->sg[i]);
-		buf->descs[i] = NULL;
-		buf->sg[i] = NULL;
-		buf->sg_len[i] = 0;
-		buf->plane_sizes[i] = 0;
-	}
-	buf->nb_planes = 0;
-}
-
-static int pxa_buffer_init(struct pxa_camera_dev *pcdev,
-			   struct pxa_buffer *buf)
-{
-	struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
-	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
-	int nb_channels = pcdev->channels;
-	int i, ret = 0;
-	unsigned long size = vb2_plane_size(vb, 0);
-
-	switch (nb_channels) {
-	case 1:
-		buf->plane_sizes[0] = size;
-		break;
-	case 3:
-		buf->plane_sizes[0] = size / 2;
-		buf->plane_sizes[1] = size / 4;
-		buf->plane_sizes[2] = size / 4;
-		break;
-	default:
-		return -EINVAL;
-	};
-	buf->nb_planes = nb_channels;
-
-	ret = sg_split(sgt->sgl, sgt->nents, 0, nb_channels,
-		       buf->plane_sizes, buf->sg, buf->sg_len, GFP_KERNEL);
-	if (ret < 0) {
-		dev_err(pcdev_to_dev(pcdev),
-			"sg_split failed: %d\n", ret);
-		return ret;
-	}
-	for (i = 0; i < nb_channels; i++) {
-		ret = pxa_init_dma_channel(pcdev, buf, i,
-					   buf->sg[i], buf->sg_len[i]);
-		if (ret) {
-			pxa_buffer_cleanup(buf);
-			return ret;
-		}
-	}
-	INIT_LIST_HEAD(&buf->queue);
-
-	return ret;
-}
-
-static void pxac_vb2_cleanup(struct vb2_buffer *vb)
-{
-	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		 "%s(vb=%p)\n", __func__, vb);
-	pxa_buffer_cleanup(buf);
-}
-
-static void pxac_vb2_queue(struct vb2_buffer *vb)
-{
-	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		 "%s(vb=%p) nb_channels=%d size=%lu active=%p\n",
-		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0),
-		pcdev->active);
-
-	list_add_tail(&buf->queue, &pcdev->capture);
-
-	pxa_dma_add_tail_buf(pcdev, buf);
-}
-
-/*
- * Please check the DMA prepared buffer structure in :
- *   Documentation/video4linux/pxa_camera.txt
- * Please check also in pxa_camera_check_link_miss() to understand why DMA chain
- * modification while DMA chain is running will work anyway.
- */
-static int pxac_vb2_prepare(struct vb2_buffer *vb)
-{
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
-	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
-	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
-	int ret = 0;
-
-	switch (pcdev->channels) {
-	case 1:
-	case 3:
-		vb2_set_plane_payload(vb, 0, icd->sizeimage);
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		 "%s (vb=%p) nb_channels=%d size=%lu\n",
-		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0));
-
-	WARN_ON(!icd->current_fmt);
-
-#ifdef DEBUG
-	/*
-	 * This can be useful if you want to see if we actually fill
-	 * the buffer with something
-	 */
-	for (i = 0; i < vb->num_planes; i++)
-		memset((void *)vb2_plane_vaddr(vb, i),
-		       0xaa, vb2_get_plane_payload(vb, i));
-#endif
-
-	/*
-	 * I think, in buf_prepare you only have to protect global data,
-	 * the actual buffer is yours
-	 */
-	buf->inwork = 0;
-	pxa_videobuf_set_actdma(pcdev, buf);
-
-	return ret;
-}
-
-static int pxac_vb2_init(struct vb2_buffer *vb)
-{
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
-	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		 "%s(nb_channels=%d)\n",
-		__func__, pcdev->channels);
-
-	return pxa_buffer_init(pcdev, buf);
-}
-
-static int pxac_vb2_queue_setup(struct vb2_queue *vq,
-				unsigned int *nbufs,
-				unsigned int *num_planes, unsigned int sizes[],
-				struct device *alloc_devs[])
-{
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
-	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
-	int size = icd->sizeimage;
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		 "%s(vq=%p nbufs=%d num_planes=%d size=%d)\n",
-		__func__, vq, *nbufs, *num_planes, size);
-	/*
-	 * Called from VIDIOC_REQBUFS or in compatibility mode For YUV422P
-	 * format, even if there are 3 planes Y, U and V, we reply there is only
-	 * one plane, containing Y, U and V data, one after the other.
-	 */
-	if (*num_planes)
-		return sizes[0] < size ? -EINVAL : 0;
-
-	*num_planes = 1;
-	switch (pcdev->channels) {
-	case 1:
-	case 3:
-		sizes[0] = size;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	if (!*nbufs)
-		*nbufs = 1;
-
-	return 0;
-}
-
-static int pxac_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
-{
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
-
-	dev_dbg(pcdev_to_dev(pcdev), "%s(count=%d) active=%p\n",
-		__func__, count, pcdev->active);
-
-	if (!pcdev->active)
-		pxa_camera_start_capture(pcdev);
-
-	return 0;
-}
-
-static void pxac_vb2_stop_streaming(struct vb2_queue *vq)
-{
-	vb2_wait_for_all_buffers(vq);
-}
-
-static struct vb2_ops pxac_vb2_ops = {
-	.queue_setup		= pxac_vb2_queue_setup,
-	.buf_init		= pxac_vb2_init,
-	.buf_prepare		= pxac_vb2_prepare,
-	.buf_queue		= pxac_vb2_queue,
-	.buf_cleanup		= pxac_vb2_cleanup,
-	.start_streaming	= pxac_vb2_start_streaming,
-	.stop_streaming		= pxac_vb2_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
-};
-
-static int pxa_camera_init_videobuf2(struct vb2_queue *vq,
-				     struct soc_camera_device *icd)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
-	int ret;
-
-	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
-	vq->drv_priv = pcdev;
-	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-	vq->buf_struct_size = sizeof(struct pxa_buffer);
-
-	vq->ops = &pxac_vb2_ops;
-	vq->mem_ops = &vb2_dma_sg_memops;
-
-	ret = vb2_queue_init(vq);
-	dev_dbg(pcdev_to_dev(pcdev),
-		 "vb2_queue_init(vq=%p): %d\n", vq, ret);
-
-	return ret;
-}
-
 static u32 mclk_get_divisor(struct platform_device *pdev,
 			    struct pxa_camera_dev *pcdev)
 {
@@ -1051,6 +819,244 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
 	__raw_writel(cicr0, pcdev->base + CICR0);
 }
 
+/*
+ * Videobuf2 section
+ */
+static void pxa_buffer_cleanup(struct pxa_buffer *buf)
+{
+	int i;
+
+	for (i = 0; i < 3 && buf->descs[i]; i++) {
+		dmaengine_desc_free(buf->descs[i]);
+		kfree(buf->sg[i]);
+		buf->descs[i] = NULL;
+		buf->sg[i] = NULL;
+		buf->sg_len[i] = 0;
+		buf->plane_sizes[i] = 0;
+	}
+	buf->nb_planes = 0;
+}
+
+static int pxa_buffer_init(struct pxa_camera_dev *pcdev,
+			   struct pxa_buffer *buf)
+{
+	struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
+	int nb_channels = pcdev->channels;
+	int i, ret = 0;
+	unsigned long size = vb2_plane_size(vb, 0);
+
+	switch (nb_channels) {
+	case 1:
+		buf->plane_sizes[0] = size;
+		break;
+	case 3:
+		buf->plane_sizes[0] = size / 2;
+		buf->plane_sizes[1] = size / 4;
+		buf->plane_sizes[2] = size / 4;
+		break;
+	default:
+		return -EINVAL;
+	};
+	buf->nb_planes = nb_channels;
+
+	ret = sg_split(sgt->sgl, sgt->nents, 0, nb_channels,
+		       buf->plane_sizes, buf->sg, buf->sg_len, GFP_KERNEL);
+	if (ret < 0) {
+		dev_err(pcdev_to_dev(pcdev),
+			"sg_split failed: %d\n", ret);
+		return ret;
+	}
+	for (i = 0; i < nb_channels; i++) {
+		ret = pxa_init_dma_channel(pcdev, buf, i,
+					   buf->sg[i], buf->sg_len[i]);
+		if (ret) {
+			pxa_buffer_cleanup(buf);
+			return ret;
+		}
+	}
+	INIT_LIST_HEAD(&buf->queue);
+
+	return ret;
+}
+
+static void pxac_vb2_cleanup(struct vb2_buffer *vb)
+{
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(vb=%p)\n", __func__, vb);
+	pxa_buffer_cleanup(buf);
+}
+
+static void pxac_vb2_queue(struct vb2_buffer *vb)
+{
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(vb=%p) nb_channels=%d size=%lu active=%p\n",
+		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0),
+		pcdev->active);
+
+	list_add_tail(&buf->queue, &pcdev->capture);
+
+	pxa_dma_add_tail_buf(pcdev, buf);
+}
+
+/*
+ * Please check the DMA prepared buffer structure in :
+ *   Documentation/video4linux/pxa_camera.txt
+ * Please check also in pxa_camera_check_link_miss() to understand why DMA chain
+ * modification while DMA chain is running will work anyway.
+ */
+static int pxac_vb2_prepare(struct vb2_buffer *vb)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+	int ret = 0;
+
+	switch (pcdev->channels) {
+	case 1:
+	case 3:
+		vb2_set_plane_payload(vb, 0, icd->sizeimage);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s (vb=%p) nb_channels=%d size=%lu\n",
+		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0));
+
+	WARN_ON(!icd->current_fmt);
+
+#ifdef DEBUG
+	/*
+	 * This can be useful if you want to see if we actually fill
+	 * the buffer with something
+	 */
+	for (i = 0; i < vb->num_planes; i++)
+		memset((void *)vb2_plane_vaddr(vb, i),
+		       0xaa, vb2_get_plane_payload(vb, i));
+#endif
+
+	/*
+	 * I think, in buf_prepare you only have to protect global data,
+	 * the actual buffer is yours
+	 */
+	buf->inwork = 0;
+	pxa_videobuf_set_actdma(pcdev, buf);
+
+	return ret;
+}
+
+static int pxac_vb2_init(struct vb2_buffer *vb)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(nb_channels=%d)\n",
+		__func__, pcdev->channels);
+
+	return pxa_buffer_init(pcdev, buf);
+}
+
+static int pxac_vb2_queue_setup(struct vb2_queue *vq,
+				unsigned int *nbufs,
+				unsigned int *num_planes, unsigned int sizes[],
+				struct device *alloc_devs[])
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
+	int size = icd->sizeimage;
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(vq=%p nbufs=%d num_planes=%d size=%d)\n",
+		__func__, vq, *nbufs, *num_planes, size);
+	/*
+	 * Called from VIDIOC_REQBUFS or in compatibility mode For YUV422P
+	 * format, even if there are 3 planes Y, U and V, we reply there is only
+	 * one plane, containing Y, U and V data, one after the other.
+	 */
+	if (*num_planes)
+		return sizes[0] < size ? -EINVAL : 0;
+
+	*num_planes = 1;
+	switch (pcdev->channels) {
+	case 1:
+	case 3:
+		sizes[0] = size;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (!*nbufs)
+		*nbufs = 1;
+
+	return 0;
+}
+
+static int pxac_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
+
+	dev_dbg(pcdev_to_dev(pcdev), "%s(count=%d) active=%p\n",
+		__func__, count, pcdev->active);
+
+	if (!pcdev->active)
+		pxa_camera_start_capture(pcdev);
+
+	return 0;
+}
+
+static void pxac_vb2_stop_streaming(struct vb2_queue *vq)
+{
+	vb2_wait_for_all_buffers(vq);
+}
+
+static struct vb2_ops pxac_vb2_ops = {
+	.queue_setup		= pxac_vb2_queue_setup,
+	.buf_init		= pxac_vb2_init,
+	.buf_prepare		= pxac_vb2_prepare,
+	.buf_queue		= pxac_vb2_queue,
+	.buf_cleanup		= pxac_vb2_cleanup,
+	.start_streaming	= pxac_vb2_start_streaming,
+	.stop_streaming		= pxac_vb2_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+static int pxa_camera_init_videobuf2(struct vb2_queue *vq,
+				     struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct pxa_camera_dev *pcdev = ici->priv;
+	int ret;
+
+	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	vq->drv_priv = pcdev;
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vq->buf_struct_size = sizeof(struct pxa_buffer);
+
+	vq->ops = &pxac_vb2_ops;
+	vq->mem_ops = &vb2_dma_sg_memops;
+
+	ret = vb2_queue_init(vq);
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "vb2_queue_init(vq=%p): %d\n", vq, ret);
+
+	return ret;
+}
+
+/*
+ * Video ioctls section
+ */
 static int pxa_camera_set_bus_param(struct soc_camera_device *icd)
 {
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
@@ -1494,6 +1500,9 @@ static int pxa_camera_querycap(struct soc_camera_host *ici,
 	return 0;
 }
 
+/*
+ * Driver probe, remove, suspend and resume operations
+ */
 static int pxa_camera_suspend(struct device *dev)
 {
 	struct soc_camera_host *ici = to_soc_camera_host(dev);
-- 
2.1.4

[PATCH v5 06/13] media: platform: pxa_camera: introduce sensor_call

[Robert Jarzmik]

Introduce sensor_call(), which will be used for all sensor invocations.
This is a preparation move to v4l2 device conversion, ie. soc_camera
adherence removal.

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
 drivers/media/platform/soc_camera/pxa_camera.c | 27 ++++++++++++++------------
 1 file changed, 15 insertions(+), 12 deletions(-)

diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index 9d7c30cb1463..9870c53e0ec2 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -168,6 +168,9 @@
 			CICR0_PERRM | CICR0_QDM | CICR0_CDM | CICR0_SOFM | \
 			CICR0_EOFM | CICR0_FOM)
 
+#define sensor_call(cam, o, f, args...) \
+	v4l2_subdev_call(sd, o, f, ##args)
+
 /*
  * Structures
  */
@@ -731,7 +734,7 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	unsigned long dw, bpp;
 	u32 cicr0, cicr1, cicr2, cicr3, cicr4 = 0, y_skip_top;
-	int ret = v4l2_subdev_call(sd, sensor, g_skip_top_lines, &y_skip_top);
+	int ret = sensor_call(pcdev, sensor, g_skip_top_lines, &y_skip_top);
 
 	if (ret < 0)
 		y_skip_top = 0;
@@ -1073,7 +1076,7 @@ static int pxa_camera_set_bus_param(struct soc_camera_device *icd)
 	if (ret < 0)
 		return ret;
 
-	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	ret = sensor_call(pcdev, video, g_mbus_config, &cfg);
 	if (!ret) {
 		common_flags = soc_mbus_config_compatible(&cfg,
 							  bus_flags);
@@ -1117,7 +1120,7 @@ static int pxa_camera_set_bus_param(struct soc_camera_device *icd)
 	}
 
 	cfg.flags = common_flags;
-	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+	ret = sensor_call(pcdev, video, s_mbus_config, &cfg);
 	if (ret < 0 && ret != -ENOIOCTLCMD) {
 		dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
 			common_flags, ret);
@@ -1144,7 +1147,7 @@ static int pxa_camera_try_bus_param(struct soc_camera_device *icd,
 	if (ret < 0)
 		return ret;
 
-	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	ret = sensor_call(pcdev, video, g_mbus_config, &cfg);
 	if (!ret) {
 		common_flags = soc_mbus_config_compatible(&cfg,
 							  bus_flags);
@@ -1195,7 +1198,7 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 	};
 	const struct soc_mbus_pixelfmt *fmt;
 
-	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
+	ret = sensor_call(pcdev, pad, enum_mbus_code, NULL, &code);
 	if (ret < 0)
 		/* No more formats */
 		return 0;
@@ -1297,7 +1300,7 @@ static int pxa_camera_set_crop(struct soc_camera_device *icd,
 	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
 		icd->sense = &sense;
 
-	ret = v4l2_subdev_call(sd, video, s_crop, a);
+	ret = sensor_call(pcdev, video, s_crop, a);
 
 	icd->sense = NULL;
 
@@ -1307,7 +1310,7 @@ static int pxa_camera_set_crop(struct soc_camera_device *icd,
 		return ret;
 	}
 
-	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
+	ret = sensor_call(pcdev, pad, get_fmt, NULL, &fmt);
 	if (ret < 0)
 		return ret;
 
@@ -1319,7 +1322,7 @@ static int pxa_camera_set_crop(struct soc_camera_device *icd,
 		v4l_bound_align_image(&mf->width, 48, 2048, 1,
 			&mf->height, 32, 2048, 0,
 			fourcc == V4L2_PIX_FMT_YUV422P ? 4 : 0);
-		ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &fmt);
+		ret = sensor_call(pcdev, pad, set_fmt, NULL, &fmt);
 		if (ret < 0)
 			return ret;
 
@@ -1384,7 +1387,7 @@ static int pxa_camera_set_fmt(struct soc_camera_device *icd,
 	mf->colorspace	= pix->colorspace;
 	mf->code	= xlate->code;
 
-	ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format);
+	ret = sensor_call(pcdev, pad, set_fmt, NULL, &format);
 
 	if (mf->code != xlate->code)
 		return -EINVAL;
@@ -1459,7 +1462,7 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 	mf->colorspace	= pix->colorspace;
 	mf->code	= xlate->code;
 
-	ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
+	ret = sensor_call(pcdev, pad, set_fmt, &pad_cfg, &format);
 	if (ret < 0)
 		return ret;
 
@@ -1517,7 +1520,7 @@ static int pxa_camera_suspend(struct device *dev)
 
 	if (pcdev->soc_host.icd) {
 		struct v4l2_subdev *sd = soc_camera_to_subdev(pcdev->soc_host.icd);
-		ret = v4l2_subdev_call(sd, core, s_power, 0);
+		ret = sensor_call(pcdev, core, s_power, 0);
 		if (ret == -ENOIOCTLCMD)
 			ret = 0;
 	}
@@ -1539,7 +1542,7 @@ static int pxa_camera_resume(struct device *dev)
 
 	if (pcdev->soc_host.icd) {
 		struct v4l2_subdev *sd = soc_camera_to_subdev(pcdev->soc_host.icd);
-		ret = v4l2_subdev_call(sd, core, s_power, 1);
+		ret = sensor_call(pcdev, core, s_power, 1);
 		if (ret == -ENOIOCTLCMD)
 			ret = 0;
 	}
-- 
2.1.4

[PATCH v5 07/13] media: platform: pxa_camera: make printk consistent

[Robert Jarzmik]

Make all print consistent by always using :
 - dev_xxx(pcdev_to_dev(pcdev), ....)

This prepares the soc_camera adherence removal by making these call rely
on only pcdev, and not the soc_camera icd structure.

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
 drivers/media/platform/soc_camera/pxa_camera.c | 70 ++++++++++++++++----------
 1 file changed, 43 insertions(+), 27 deletions(-)

diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index 9870c53e0ec2..45583a40a4bd 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -236,6 +236,14 @@ struct pxa_cam {
 
 static const char *pxa_cam_driver_description = "PXA_Camera";
 
+static struct pxa_camera_dev *icd_to_pcdev(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct pxa_camera_dev *pcdev = ici->priv;
+
+	return pcdev;
+}
+
 /*
  *  Videobuf operations
  */
@@ -465,7 +473,6 @@ static void pxa_camera_check_link_miss(struct pxa_camera_dev *pcdev,
 static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 			       enum pxa_camera_active_dma act_dma)
 {
-	struct device *dev = pcdev_to_dev(pcdev);
 	struct pxa_buffer *buf, *last_buf;
 	unsigned long flags;
 	u32 camera_status, overrun;
@@ -476,7 +483,7 @@ static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 	spin_lock_irqsave(&pcdev->lock, flags);
 
 	camera_status = __raw_readl(pcdev->base + CISR);
-	dev_dbg(dev, "camera dma irq, cisr=0x%x dma=%d\n",
+	dev_dbg(pcdev_to_dev(pcdev), "camera dma irq, cisr=0x%x dma=%d\n",
 		camera_status, act_dma);
 	overrun = CISR_IFO_0;
 	if (pcdev->channels == 3)
@@ -522,7 +529,7 @@ static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 					       NULL, &last_issued);
 	if (camera_status & overrun &&
 	    last_status != DMA_COMPLETE) {
-		dev_dbg(dev, "FIFO overrun! CISR: %x\n",
+		dev_dbg(pcdev_to_dev(pcdev), "FIFO overrun! CISR: %x\n",
 			camera_status);
 		pxa_camera_stop_capture(pcdev);
 		list_for_each_entry(buf, &pcdev->capture, queue)
@@ -545,7 +552,6 @@ static u32 mclk_get_divisor(struct platform_device *pdev,
 			    struct pxa_camera_dev *pcdev)
 {
 	unsigned long mclk = pcdev->mclk;
-	struct device *dev = &pdev->dev;
 	u32 div;
 	unsigned long lcdclk;
 
@@ -555,7 +561,8 @@ static u32 mclk_get_divisor(struct platform_device *pdev,
 	/* mclk <= ciclk / 4 (27.4.2) */
 	if (mclk > lcdclk / 4) {
 		mclk = lcdclk / 4;
-		dev_warn(dev, "Limiting master clock to %lu\n", mclk);
+		dev_warn(pcdev_to_dev(pcdev),
+			 "Limiting master clock to %lu\n", mclk);
 	}
 
 	/* We verify mclk != 0, so if anyone breaks it, here comes their Oops */
@@ -565,7 +572,7 @@ static u32 mclk_get_divisor(struct platform_device *pdev,
 	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
 		pcdev->mclk = lcdclk / (2 * (div + 1));
 
-	dev_dbg(dev, "LCD clock %luHz, target freq %luHz, divisor %u\n",
+	dev_dbg(pcdev_to_dev(pcdev), "LCD clock %luHz, target freq %luHz, divisor %u\n",
 		lcdclk, mclk, div);
 
 	return div;
@@ -662,7 +669,9 @@ static irqreturn_t pxa_camera_irq(int irq, void *data)
 
 static int pxa_camera_add_device(struct soc_camera_device *icd)
 {
-	dev_info(icd->parent, "PXA Camera driver attached to camera %d\n",
+	struct pxa_camera_dev *pcdev = icd_to_pcdev(icd);
+
+	dev_info(pcdev_to_dev(pcdev), "PXA Camera driver attached to camera %d\n",
 		 icd->devnum);
 
 	return 0;
@@ -670,7 +679,9 @@ static int pxa_camera_add_device(struct soc_camera_device *icd)
 
 static void pxa_camera_remove_device(struct soc_camera_device *icd)
 {
-	dev_info(icd->parent, "PXA Camera driver detached from camera %d\n",
+	struct pxa_camera_dev *pcdev = icd_to_pcdev(icd);
+
+	dev_info(pcdev_to_dev(pcdev), "PXA Camera driver detached from camera %d\n",
 		 icd->devnum);
 }
 
@@ -1081,7 +1092,7 @@ static int pxa_camera_set_bus_param(struct soc_camera_device *icd)
 		common_flags = soc_mbus_config_compatible(&cfg,
 							  bus_flags);
 		if (!common_flags) {
-			dev_warn(icd->parent,
+			dev_warn(pcdev_to_dev(pcdev),
 				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
 				 cfg.flags, bus_flags);
 			return -EINVAL;
@@ -1122,7 +1133,7 @@ static int pxa_camera_set_bus_param(struct soc_camera_device *icd)
 	cfg.flags = common_flags;
 	ret = sensor_call(pcdev, video, s_mbus_config, &cfg);
 	if (ret < 0 && ret != -ENOIOCTLCMD) {
-		dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
+		dev_dbg(pcdev_to_dev(pcdev), "camera s_mbus_config(0x%lx) returned %d\n",
 			common_flags, ret);
 		return ret;
 	}
@@ -1152,7 +1163,7 @@ static int pxa_camera_try_bus_param(struct soc_camera_device *icd,
 		common_flags = soc_mbus_config_compatible(&cfg,
 							  bus_flags);
 		if (!common_flags) {
-			dev_warn(icd->parent,
+			dev_warn(pcdev_to_dev(pcdev),
 				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
 				 cfg.flags, bus_flags);
 			return -EINVAL;
@@ -1189,7 +1200,7 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 				  struct soc_camera_format_xlate *xlate)
 {
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct device *dev = icd->parent;
+	struct pxa_camera_dev *pcdev = icd_to_pcdev(icd);
 	int formats = 0, ret;
 	struct pxa_cam *cam;
 	struct v4l2_subdev_mbus_code_enum code = {
@@ -1205,7 +1216,7 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 
 	fmt = soc_mbus_get_fmtdesc(code.code);
 	if (!fmt) {
-		dev_err(dev, "Invalid format code #%u: %d\n", idx, code.code);
+		dev_err(pcdev_to_dev(pcdev), "Invalid format code #%u: %d\n", idx, code.code);
 		return 0;
 	}
 
@@ -1231,7 +1242,8 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 			xlate->host_fmt	= &pxa_camera_formats[0];
 			xlate->code	= code.code;
 			xlate++;
-			dev_dbg(dev, "Providing format %s using code %d\n",
+			dev_dbg(pcdev_to_dev(pcdev),
+				"Providing format %s using code %d\n",
 				pxa_camera_formats[0].name, code.code);
 		}
 	case MEDIA_BUS_FMT_VYUY8_2X8:
@@ -1240,14 +1252,15 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 	case MEDIA_BUS_FMT_RGB565_2X8_LE:
 	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
 		if (xlate)
-			dev_dbg(dev, "Providing format %s packed\n",
+			dev_dbg(pcdev_to_dev(pcdev),
+				"Providing format %s packed\n",
 				fmt->name);
 		break;
 	default:
 		if (!pxa_camera_packing_supported(fmt))
 			return 0;
 		if (xlate)
-			dev_dbg(dev,
+			dev_dbg(pcdev_to_dev(pcdev),
 				"Providing format %s in pass-through mode\n",
 				fmt->name);
 	}
@@ -1305,7 +1318,7 @@ static int pxa_camera_set_crop(struct soc_camera_device *icd,
 	icd->sense = NULL;
 
 	if (ret < 0) {
-		dev_warn(dev, "Failed to crop to %ux%u@%u:%u\n",
+		dev_warn(pcdev_to_dev(pcdev), "Failed to crop to %ux%u@%u:%u\n",
 			 rect->width, rect->height, rect->left, rect->top);
 		return ret;
 	}
@@ -1327,7 +1340,7 @@ static int pxa_camera_set_crop(struct soc_camera_device *icd,
 			return ret;
 
 		if (pxa_camera_check_frame(mf->width, mf->height)) {
-			dev_warn(icd->parent,
+			dev_warn(pcdev_to_dev(pcdev),
 				 "Inconsistent state. Use S_FMT to repair\n");
 			return -EINVAL;
 		}
@@ -1335,7 +1348,7 @@ static int pxa_camera_set_crop(struct soc_camera_device *icd,
 
 	if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {
 		if (sense.pixel_clock > sense.pixel_clock_max) {
-			dev_err(dev,
+			dev_err(pcdev_to_dev(pcdev),
 				"pixel clock %lu set by the camera too high!",
 				sense.pixel_clock);
 			return -EIO;
@@ -1372,7 +1385,8 @@ static int pxa_camera_set_fmt(struct soc_camera_device *icd,
 
 	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
 	if (!xlate) {
-		dev_warn(dev, "Format %x not found\n", pix->pixelformat);
+		dev_warn(pcdev_to_dev(pcdev),
+			 "Format %x not found\n", pix->pixelformat);
 		return -EINVAL;
 	}
 
@@ -1395,16 +1409,17 @@ static int pxa_camera_set_fmt(struct soc_camera_device *icd,
 	icd->sense = NULL;
 
 	if (ret < 0) {
-		dev_warn(dev, "Failed to configure for format %x\n",
+		dev_warn(pcdev_to_dev(pcdev),
+			 "Failed to configure for format %x\n",
 			 pix->pixelformat);
 	} else if (pxa_camera_check_frame(mf->width, mf->height)) {
-		dev_warn(dev,
+		dev_warn(pcdev_to_dev(pcdev),
 			 "Camera driver produced an unsupported frame %dx%d\n",
 			 mf->width, mf->height);
 		ret = -EINVAL;
 	} else if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {
 		if (sense.pixel_clock > sense.pixel_clock_max) {
-			dev_err(dev,
+			dev_err(pcdev_to_dev(pcdev),
 				"pixel clock %lu set by the camera too high!",
 				sense.pixel_clock);
 			return -EIO;
@@ -1428,6 +1443,7 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 			      struct v4l2_format *f)
 {
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct pxa_camera_dev *pcdev = icd_to_pcdev(icd);
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
 	struct v4l2_subdev_pad_config pad_cfg;
@@ -1440,7 +1456,7 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 
 	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
 	if (!xlate) {
-		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
+		dev_warn(pcdev_to_dev(pcdev), "Format %x not found\n", pixfmt);
 		return -EINVAL;
 	}
 
@@ -1477,7 +1493,7 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 		break;
 	default:
 		/* TODO: support interlaced at least in pass-through mode */
-		dev_err(icd->parent, "Field type %d unsupported.\n",
+		dev_err(pcdev_to_dev(pcdev), "Field type %d unsupported.\n",
 			mf->field);
 		return -EINVAL;
 	}
@@ -1586,13 +1602,13 @@ static int pxa_camera_pdata_from_dt(struct device *dev,
 
 	np = of_graph_get_next_endpoint(np, NULL);
 	if (!np) {
-		dev_err(dev, "could not find endpoint\n");
+		dev_err(pcdev_to_dev(pcdev), "could not find endpoint\n");
 		return -EINVAL;
 	}
 
 	err = v4l2_of_parse_endpoint(np, &ep);
 	if (err) {
-		dev_err(dev, "could not parse endpoint\n");
+		dev_err(pcdev_to_dev(pcdev), "could not parse endpoint\n");
 		goto out;
 	}
 
-- 
2.1.4

[PATCH v5 08/13] media: platform: pxa_camera: add buffer sequencing

[Robert Jarzmik]

Add sequence numbers to completed buffers.

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
Since v3: reset buffer sequence number in start_streaming()
---
 drivers/media/platform/soc_camera/pxa_camera.c | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index 45583a40a4bd..9b294a14fa2e 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -223,6 +223,7 @@ struct pxa_camera_dev {
 	struct list_head	capture;
 
 	spinlock_t		lock;
+	unsigned int		buf_sequence;
 
 	struct pxa_buffer	*active;
 	struct tasklet_struct	task_eof;
@@ -423,10 +424,13 @@ static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
 			      struct pxa_buffer *buf)
 {
 	struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 
 	/* _init is used to debug races, see comment in pxa_camera_reqbufs() */
 	list_del_init(&buf->queue);
 	vb->timestamp = ktime_get_ns();
+	vbuf->sequence = pcdev->buf_sequence++;
+	vbuf->field = V4L2_FIELD_NONE;
 	vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
 	dev_dbg(pcdev_to_dev(pcdev), "%s dequeud buffer (buf=0x%p)\n",
 		__func__, buf);
@@ -1022,6 +1026,7 @@ static int pxac_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
 	dev_dbg(pcdev_to_dev(pcdev), "%s(count=%d) active=%p\n",
 		__func__, count, pcdev->active);
 
+	pcdev->buf_sequence = 0;
 	if (!pcdev->active)
 		pxa_camera_start_capture(pcdev);
 
-- 
2.1.4

[PATCH v5 09/13] media: platform: pxa_camera: remove set_crop

[Robert Jarzmik]

This is to be seen as a regression as the set_crop function is
removed. This is a temporary situation in the v4l2 porting, and will
have to be added later.

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
 drivers/media/platform/soc_camera/pxa_camera.c | 76 --------------------------
 1 file changed, 76 deletions(-)

diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index 9b294a14fa2e..8f329d0b2cda 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -1294,81 +1294,6 @@ static int pxa_camera_check_frame(u32 width, u32 height)
 		(width & 0x01);
 }
 
-static int pxa_camera_set_crop(struct soc_camera_device *icd,
-			       const struct v4l2_crop *a)
-{
-	const struct v4l2_rect *rect = &a->c;
-	struct device *dev = icd->parent;
-	struct soc_camera_host *ici = to_soc_camera_host(dev);
-	struct pxa_camera_dev *pcdev = ici->priv;
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct soc_camera_sense sense = {
-		.master_clock = pcdev->mclk,
-		.pixel_clock_max = pcdev->ciclk / 4,
-	};
-	struct v4l2_subdev_format fmt = {
-		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-	};
-	struct v4l2_mbus_framefmt *mf = &fmt.format;
-	struct pxa_cam *cam = icd->host_priv;
-	u32 fourcc = icd->current_fmt->host_fmt->fourcc;
-	int ret;
-
-	/* If PCLK is used to latch data from the sensor, check sense */
-	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
-		icd->sense = &sense;
-
-	ret = sensor_call(pcdev, video, s_crop, a);
-
-	icd->sense = NULL;
-
-	if (ret < 0) {
-		dev_warn(pcdev_to_dev(pcdev), "Failed to crop to %ux%u@%u:%u\n",
-			 rect->width, rect->height, rect->left, rect->top);
-		return ret;
-	}
-
-	ret = sensor_call(pcdev, pad, get_fmt, NULL, &fmt);
-	if (ret < 0)
-		return ret;
-
-	if (pxa_camera_check_frame(mf->width, mf->height)) {
-		/*
-		 * Camera cropping produced a frame beyond our capabilities.
-		 * FIXME: just extract a subframe, that we can process.
-		 */
-		v4l_bound_align_image(&mf->width, 48, 2048, 1,
-			&mf->height, 32, 2048, 0,
-			fourcc == V4L2_PIX_FMT_YUV422P ? 4 : 0);
-		ret = sensor_call(pcdev, pad, set_fmt, NULL, &fmt);
-		if (ret < 0)
-			return ret;
-
-		if (pxa_camera_check_frame(mf->width, mf->height)) {
-			dev_warn(pcdev_to_dev(pcdev),
-				 "Inconsistent state. Use S_FMT to repair\n");
-			return -EINVAL;
-		}
-	}
-
-	if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {
-		if (sense.pixel_clock > sense.pixel_clock_max) {
-			dev_err(pcdev_to_dev(pcdev),
-				"pixel clock %lu set by the camera too high!",
-				sense.pixel_clock);
-			return -EIO;
-		}
-		recalculate_fifo_timeout(pcdev, sense.pixel_clock);
-	}
-
-	icd->user_width		= mf->width;
-	icd->user_height	= mf->height;
-
-	pxa_camera_setup_cicr(icd, cam->flags, fourcc);
-
-	return ret;
-}
-
 static int pxa_camera_set_fmt(struct soc_camera_device *icd,
 			      struct v4l2_format *f)
 {
@@ -1581,7 +1506,6 @@ static struct soc_camera_host_ops pxa_soc_camera_host_ops = {
 	.remove		= pxa_camera_remove_device,
 	.clock_start	= pxa_camera_clock_start,
 	.clock_stop	= pxa_camera_clock_stop,
-	.set_crop	= pxa_camera_set_crop,
 	.get_formats	= pxa_camera_get_formats,
 	.put_formats	= pxa_camera_put_formats,
 	.set_fmt	= pxa_camera_set_fmt,
-- 
2.1.4

[PATCH v5 10/13] media: platform: pxa_camera: make a standalone v4l2 device

[Robert Jarzmik]

This patch removes the soc_camera API dependency from pxa_camera.
In the current status :
 - all previously captures are working the same on pxa270
 - the s_crop() call was removed, judged not working
   (see what happens soc_camera_s_crop() when get_crop() == NULL)
 - if the pixel clock is provided by then sensor, ie. not MCLK, the dual
   stage change is not handled yet.
   => there is no in-tree user of this, so I'll let it that way

 - the MCLK is not yet finished, it's as in the legacy way,
   ie. activated at video device opening and closed at video device
   closing.
   In a subsequence patch pxa_camera_mclk_ops should be used, and
   platform data MCLK ignored. It will be the sensor's duty to request
   the clock and enable it, which will end in pxa_camera_mclk_ops.

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
Since v1:
  - function namings were cleaned into pxac_XXX_YYYY()
  - function were regrouped in the 3 big categories :
    - device probing/removal : pxa_camera_*()
    - videobuf2 : pxac_vb2_*()
    - v42l file operations : pxac_vidioc_*()
    - internal driver functions : pxa_camera_*() : to be found a cute
      pattern for RFC v3
Since v2:
  - split functions
  - start_streaming() implemented
Since v3:
  - conflict in void *alloc_ctxt by struct device *alloc_devs change
  - ctrl_handler for video device added
  - 2 ioctrl disables removed
  - disable sensor module removal, it will be loaded forever ...
Since v4:
 - videobuf2 device initialization moved
---
 drivers/media/platform/soc_camera/Kconfig      |   2 +-
 drivers/media/platform/soc_camera/pxa_camera.c | 753 +++++++++++++++++--------
 include/linux/platform_data/media/camera-pxa.h |   2 +
 3 files changed, 518 insertions(+), 239 deletions(-)

diff --git a/drivers/media/platform/soc_camera/Kconfig b/drivers/media/platform/soc_camera/Kconfig
index 3f927f96763a..0bf33ccf9a1e 100644
--- a/drivers/media/platform/soc_camera/Kconfig
+++ b/drivers/media/platform/soc_camera/Kconfig
@@ -19,7 +19,7 @@ config SOC_CAMERA_PLATFORM
 
 config VIDEO_PXA27x
 	tristate "PXA27x Quick Capture Interface driver"
-	depends on VIDEO_DEV && PXA27x && SOC_CAMERA && HAS_DMA
+	depends on VIDEO_DEV && PXA27x && HAS_DMA
 	select VIDEOBUF2_DMA_SG
 	select SG_SPLIT
 	---help---
diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index 8f329d0b2cda..395cd398c32b 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -3,6 +3,7 @@
  *
  * Copyright (C) 2006, Sascha Hauer, Pengutronix
  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ * Copyright (C) 2016, Robert Jarzmik <robert.jarzmik@free.fr>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -14,6 +15,7 @@
 #include <linux/module.h>
 #include <linux/io.h>
 #include <linux/delay.h>
+#include <linux/device.h>
 #include <linux/dma-mapping.h>
 #include <linux/err.h>
 #include <linux/errno.h>
@@ -22,6 +24,7 @@
 #include <linux/kernel.h>
 #include <linux/mm.h>
 #include <linux/moduleparam.h>
+#include <linux/of.h>
 #include <linux/time.h>
 #include <linux/device.h>
 #include <linux/platform_device.h>
@@ -32,13 +35,16 @@
 #include <linux/dma-mapping.h>
 #include <linux/dma/pxa-dma.h>
 
+#include <media/v4l2-async.h>
+#include <media/v4l2-clk.h>
 #include <media/v4l2-common.h>
-#include <media/v4l2-dev.h>
-#include <media/videobuf2-dma-sg.h>
-#include <media/soc_camera.h>
-#include <media/drv-intf/soc_mediabus.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
 #include <media/v4l2-of.h>
 
+#include <media/drv-intf/soc_mediabus.h>
+#include <media/videobuf2-dma-sg.h>
+
 #include <linux/videodev2.h>
 
 #include <linux/platform_data/media/camera-pxa.h>
@@ -46,6 +52,9 @@
 #define PXA_CAM_VERSION "0.0.6"
 #define PXA_CAM_DRV_NAME "pxa27x-camera"
 
+#define DEFAULT_WIDTH	640
+#define DEFAULT_HEIGHT	480
+
 /* Camera Interface */
 #define CICR0		0x0000
 #define CICR1		0x0004
@@ -169,7 +178,25 @@
 			CICR0_EOFM | CICR0_FOM)
 
 #define sensor_call(cam, o, f, args...) \
-	v4l2_subdev_call(sd, o, f, ##args)
+	v4l2_subdev_call(cam->sensor, o, f, ##args)
+
+/*
+ * Format handling
+ */
+/**
+ * struct soc_camera_format_xlate - match between host and sensor formats
+ * @code: code of a sensor provided format
+ * @host_fmt: host format after host translation from code
+ *
+ * Host and sensor translation structure. Used in table of host and sensor
+ * formats matchings in soc_camera_device. A host can override the generic list
+ * generation by implementing get_formats(), and use it for format checks and
+ * format setup.
+ */
+struct soc_camera_format_xlate {
+	u32 code;
+	const struct soc_mbus_pixelfmt *host_fmt;
+};
 
 /*
  * Structures
@@ -198,7 +225,18 @@ struct pxa_buffer {
 };
 
 struct pxa_camera_dev {
-	struct soc_camera_host	soc_host;
+	struct v4l2_device	v4l2_dev;
+	struct video_device	vdev;
+	struct v4l2_async_notifier notifier;
+	struct vb2_queue	vb2_vq;
+	struct v4l2_subdev	*sensor;
+	struct soc_camera_format_xlate *user_formats;
+	const struct soc_camera_format_xlate *current_fmt;
+	struct v4l2_pix_format	current_pix;
+
+	struct v4l2_async_subdev asd;
+	struct v4l2_async_subdev *asds[1];
+
 	/*
 	 * PXA27x is only supposed to handle one camera on its Quick Capture
 	 * interface. If anyone ever builds hardware to enable more than
@@ -218,11 +256,13 @@ struct pxa_camera_dev {
 	unsigned long		ciclk;
 	unsigned long		mclk;
 	u32			mclk_divisor;
+	struct v4l2_clk		*mclk_clk;
 	u16			width_flags;	/* max 10 bits */
 
 	struct list_head	capture;
 
 	spinlock_t		lock;
+	struct mutex		mlock;
 	unsigned int		buf_sequence;
 
 	struct pxa_buffer	*active;
@@ -237,12 +277,69 @@ struct pxa_cam {
 
 static const char *pxa_cam_driver_description = "PXA_Camera";
 
-static struct pxa_camera_dev *icd_to_pcdev(struct soc_camera_device *icd)
+/*
+ * Format translation functions
+ */
+const struct soc_camera_format_xlate *soc_mbus_xlate_by_fourcc(
+	struct soc_camera_format_xlate *user_formats, unsigned int fourcc)
+{
+	unsigned int i;
+
+	for (i = 0; user_formats[i].code; i++)
+		if (user_formats[i].host_fmt->fourcc == fourcc)
+			return user_formats + i;
+	return NULL;
+}
+
+static struct soc_camera_format_xlate *soc_mbus_build_fmts_xlate(
+	struct v4l2_device *v4l2_dev, struct v4l2_subdev *subdev,
+	int (*get_formats)(struct v4l2_device *, unsigned int,
+			   struct soc_camera_format_xlate *xlate))
 {
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
+	unsigned int i, fmts = 0, raw_fmts = 0;
+	int ret;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct soc_camera_format_xlate *user_formats;
+
+	while (!v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code)) {
+		raw_fmts++;
+		code.index++;
+	}
+
+	/*
+	 * First pass - only count formats this host-sensor
+	 * configuration can provide
+	 */
+	for (i = 0; i < raw_fmts; i++) {
+		ret = get_formats(v4l2_dev, i, NULL);
+		if (ret < 0)
+			return ERR_PTR(ret);
+		fmts += ret;
+	}
+
+	if (!fmts)
+		return ERR_PTR(-ENXIO);
 
-	return pcdev;
+	user_formats = kcalloc(fmts + 1, sizeof(*user_formats), GFP_KERNEL);
+	if (!user_formats)
+		return ERR_PTR(-ENOMEM);
+
+	/* Second pass - actually fill data formats */
+	fmts = 0;
+	for (i = 0; i < raw_fmts; i++) {
+		ret = get_formats(v4l2_dev, i, user_formats + fmts);
+		if (ret < 0)
+			goto egfmt;
+		fmts += ret;
+	}
+	user_formats[fmts].code = 0;
+
+	return user_formats;
+egfmt:
+	kfree(user_formats);
+	return ERR_PTR(ret);
 }
 
 /*
@@ -257,7 +354,12 @@ static struct pxa_buffer *vb2_to_pxa_buffer(struct vb2_buffer *vb)
 
 static struct device *pcdev_to_dev(struct pxa_camera_dev *pcdev)
 {
-	return pcdev->soc_host.v4l2_dev.dev;
+	return pcdev->v4l2_dev.dev;
+}
+
+static struct pxa_camera_dev *v4l2_dev_to_pcdev(struct v4l2_device *v4l2_dev)
+{
+	return container_of(v4l2_dev, struct pxa_camera_dev, v4l2_dev);
 }
 
 static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
@@ -338,7 +440,7 @@ static void pxa_videobuf_set_actdma(struct pxa_camera_dev *pcdev,
 				    struct pxa_buffer *buf)
 {
 	buf->active_dma = DMA_Y;
-	if (pcdev->channels == 3)
+	if (buf->nb_planes == 3)
 		buf->active_dma |= DMA_U | DMA_V;
 }
 
@@ -671,51 +773,6 @@ static irqreturn_t pxa_camera_irq(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static int pxa_camera_add_device(struct soc_camera_device *icd)
-{
-	struct pxa_camera_dev *pcdev = icd_to_pcdev(icd);
-
-	dev_info(pcdev_to_dev(pcdev), "PXA Camera driver attached to camera %d\n",
-		 icd->devnum);
-
-	return 0;
-}
-
-static void pxa_camera_remove_device(struct soc_camera_device *icd)
-{
-	struct pxa_camera_dev *pcdev = icd_to_pcdev(icd);
-
-	dev_info(pcdev_to_dev(pcdev), "PXA Camera driver detached from camera %d\n",
-		 icd->devnum);
-}
-
-/*
- * The following two functions absolutely depend on the fact, that
- * there can be only one camera on PXA quick capture interface
- * Called with .host_lock held
- */
-static int pxa_camera_clock_start(struct soc_camera_host *ici)
-{
-	struct pxa_camera_dev *pcdev = ici->priv;
-
-	pxa_camera_activate(pcdev);
-
-	return 0;
-}
-
-/* Called with .host_lock held */
-static void pxa_camera_clock_stop(struct soc_camera_host *ici)
-{
-	struct pxa_camera_dev *pcdev = ici->priv;
-
-	/* disable capture, disable interrupts */
-	__raw_writel(0x3ff, pcdev->base + CICR0);
-
-	/* Stop DMA engine */
-	pxa_dma_stop_channels(pcdev);
-	pxa_camera_deactivate(pcdev);
-}
-
 static int test_platform_param(struct pxa_camera_dev *pcdev,
 			       unsigned char buswidth, unsigned long *flags)
 {
@@ -741,12 +798,9 @@ static int test_platform_param(struct pxa_camera_dev *pcdev,
 	return -EINVAL;
 }
 
-static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
+static void pxa_camera_setup_cicr(struct pxa_camera_dev *pcdev,
 				  unsigned long flags, __u32 pixfmt)
 {
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	unsigned long dw, bpp;
 	u32 cicr0, cicr1, cicr2, cicr3, cicr4 = 0, y_skip_top;
 	int ret = sensor_call(pcdev, sensor, g_skip_top_lines, &y_skip_top);
@@ -758,7 +812,7 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
 	 * Datawidth is now guaranteed to be equal to one of the three values.
 	 * We fix bit-per-pixel equal to data-width...
 	 */
-	switch (icd->current_fmt->host_fmt->bits_per_sample) {
+	switch (pcdev->current_fmt->host_fmt->bits_per_sample) {
 	case 10:
 		dw = 4;
 		bpp = 0x40;
@@ -792,7 +846,7 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
 	if (cicr0 & CICR0_ENB)
 		__raw_writel(cicr0 & ~CICR0_ENB, pcdev->base + CICR0);
 
-	cicr1 = CICR1_PPL_VAL(icd->user_width - 1) | bpp | dw;
+	cicr1 = CICR1_PPL_VAL(pcdev->current_pix.width - 1) | bpp | dw;
 
 	switch (pixfmt) {
 	case V4L2_PIX_FMT_YUV422P:
@@ -821,7 +875,7 @@ static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
 	}
 
 	cicr2 = 0;
-	cicr3 = CICR3_LPF_VAL(icd->user_height - 1) |
+	cicr3 = CICR3_LPF_VAL(pcdev->current_pix.height - 1) |
 		CICR3_BFW_VAL(min((u32)255, y_skip_top));
 	cicr4 |= pcdev->mclk_divisor;
 
@@ -933,13 +987,12 @@ static int pxac_vb2_prepare(struct vb2_buffer *vb)
 {
 	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
 	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
-	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
 	int ret = 0;
 
 	switch (pcdev->channels) {
 	case 1:
 	case 3:
-		vb2_set_plane_payload(vb, 0, icd->sizeimage);
+		vb2_set_plane_payload(vb, 0, pcdev->current_pix.sizeimage);
 		break;
 	default:
 		return -EINVAL;
@@ -949,7 +1002,7 @@ static int pxac_vb2_prepare(struct vb2_buffer *vb)
 		 "%s (vb=%p) nb_channels=%d size=%lu\n",
 		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0));
 
-	WARN_ON(!icd->current_fmt);
+	WARN_ON(!pcdev->current_fmt);
 
 #ifdef DEBUG
 	/*
@@ -989,8 +1042,7 @@ static int pxac_vb2_queue_setup(struct vb2_queue *vq,
 				struct device *alloc_devs[])
 {
 	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
-	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
-	int size = icd->sizeimage;
+	int size = pcdev->current_pix.sizeimage;
 
 	dev_dbg(pcdev_to_dev(pcdev),
 		 "%s(vq=%p nbufs=%d num_planes=%d size=%d)\n",
@@ -1050,21 +1102,22 @@ static struct vb2_ops pxac_vb2_ops = {
 	.wait_finish		= vb2_ops_wait_finish,
 };
 
-static int pxa_camera_init_videobuf2(struct vb2_queue *vq,
-				     struct soc_camera_device *icd)
+static int pxa_camera_init_videobuf2(struct pxa_camera_dev *pcdev)
 {
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
 	int ret;
+	struct vb2_queue *vq = &pcdev->vb2_vq;
 
+	memset(vq, 0, sizeof(*vq));
 	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 	vq->drv_priv = pcdev;
 	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	vq->buf_struct_size = sizeof(struct pxa_buffer);
+	vq->dev = pcdev->v4l2_dev.dev;
 
 	vq->ops = &pxac_vb2_ops;
 	vq->mem_ops = &vb2_dma_sg_memops;
+	vq->lock = &pcdev->mlock;
 
 	ret = vb2_queue_init(vq);
 	dev_dbg(pcdev_to_dev(pcdev),
@@ -1076,18 +1129,15 @@ static int pxa_camera_init_videobuf2(struct vb2_queue *vq,
 /*
  * Video ioctls section
  */
-static int pxa_camera_set_bus_param(struct soc_camera_device *icd)
+static int pxa_camera_set_bus_param(struct pxa_camera_dev *pcdev)
 {
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
 	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
-	u32 pixfmt = icd->current_fmt->host_fmt->fourcc;
+	u32 pixfmt = pcdev->current_fmt->host_fmt->fourcc;
 	unsigned long bus_flags, common_flags;
 	int ret;
-	struct pxa_cam *cam = icd->host_priv;
 
-	ret = test_platform_param(pcdev, icd->current_fmt->host_fmt->bits_per_sample,
+	ret = test_platform_param(pcdev,
+				  pcdev->current_fmt->host_fmt->bits_per_sample,
 				  &bus_flags);
 	if (ret < 0)
 		return ret;
@@ -1138,24 +1188,20 @@ static int pxa_camera_set_bus_param(struct soc_camera_device *icd)
 	cfg.flags = common_flags;
 	ret = sensor_call(pcdev, video, s_mbus_config, &cfg);
 	if (ret < 0 && ret != -ENOIOCTLCMD) {
-		dev_dbg(pcdev_to_dev(pcdev), "camera s_mbus_config(0x%lx) returned %d\n",
+		dev_dbg(pcdev_to_dev(pcdev),
+			"camera s_mbus_config(0x%lx) returned %d\n",
 			common_flags, ret);
 		return ret;
 	}
 
-	cam->flags = common_flags;
-
-	pxa_camera_setup_cicr(icd, common_flags, pixfmt);
+	pxa_camera_setup_cicr(pcdev, common_flags, pixfmt);
 
 	return 0;
 }
 
-static int pxa_camera_try_bus_param(struct soc_camera_device *icd,
+static int pxa_camera_try_bus_param(struct pxa_camera_dev *pcdev,
 				    unsigned char buswidth)
 {
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct pxa_camera_dev *pcdev = ici->priv;
 	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 	unsigned long bus_flags, common_flags;
 	int ret = test_platform_param(pcdev, buswidth, &bus_flags);
@@ -1201,13 +1247,12 @@ static bool pxa_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
 		 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
 }
 
-static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int idx,
+static int pxa_camera_get_formats(struct v4l2_device *v4l2_dev,
+				  unsigned int idx,
 				  struct soc_camera_format_xlate *xlate)
 {
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct pxa_camera_dev *pcdev = icd_to_pcdev(icd);
+	struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(v4l2_dev);
 	int formats = 0, ret;
-	struct pxa_cam *cam;
 	struct v4l2_subdev_mbus_code_enum code = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 		.index = idx,
@@ -1221,25 +1266,16 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 
 	fmt = soc_mbus_get_fmtdesc(code.code);
 	if (!fmt) {
-		dev_err(pcdev_to_dev(pcdev), "Invalid format code #%u: %d\n", idx, code.code);
+		dev_err(pcdev_to_dev(pcdev),
+			"Invalid format code #%u: %d\n", idx, code.code);
 		return 0;
 	}
 
 	/* This also checks support for the requested bits-per-sample */
-	ret = pxa_camera_try_bus_param(icd, fmt->bits_per_sample);
+	ret = pxa_camera_try_bus_param(pcdev, fmt->bits_per_sample);
 	if (ret < 0)
 		return 0;
 
-	if (!icd->host_priv) {
-		cam = kzalloc(sizeof(*cam), GFP_KERNEL);
-		if (!cam)
-			return -ENOMEM;
-
-		icd->host_priv = cam;
-	} else {
-		cam = icd->host_priv;
-	}
-
 	switch (code.code) {
 	case MEDIA_BUS_FMT_UYVY8_2X8:
 		formats++;
@@ -1281,10 +1317,22 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 	return formats;
 }
 
-static void pxa_camera_put_formats(struct soc_camera_device *icd)
+static int pxa_camera_build_formats(struct pxa_camera_dev *pcdev)
+{
+	struct soc_camera_format_xlate *xlate;
+
+	xlate = soc_mbus_build_fmts_xlate(&pcdev->v4l2_dev, pcdev->sensor,
+					  pxa_camera_get_formats);
+	if (IS_ERR(xlate))
+		return PTR_ERR(xlate);
+
+	pcdev->user_formats = xlate;
+	return 0;
+}
+
+static void pxa_camera_destroy_formats(struct pxa_camera_dev *pcdev)
 {
-	kfree(icd->host_priv);
-	icd->host_priv = NULL;
+	kfree(pcdev->user_formats);
 }
 
 static int pxa_camera_check_frame(u32 width, u32 height)
@@ -1294,86 +1342,44 @@ static int pxa_camera_check_frame(u32 width, u32 height)
 		(width & 0x01);
 }
 
-static int pxa_camera_set_fmt(struct soc_camera_device *icd,
-			      struct v4l2_format *f)
+static int pxac_vidioc_enum_fmt_vid_cap(struct file *filp, void  *priv,
+					struct v4l2_fmtdesc *f)
 {
-	struct device *dev = icd->parent;
-	struct soc_camera_host *ici = to_soc_camera_host(dev);
-	struct pxa_camera_dev *pcdev = ici->priv;
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	const struct soc_camera_format_xlate *xlate = NULL;
-	struct soc_camera_sense sense = {
-		.master_clock = pcdev->mclk,
-		.pixel_clock_max = pcdev->ciclk / 4,
-	};
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_subdev_format format = {
-		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-	};
-	struct v4l2_mbus_framefmt *mf = &format.format;
-	int ret;
-
-	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
-	if (!xlate) {
-		dev_warn(pcdev_to_dev(pcdev),
-			 "Format %x not found\n", pix->pixelformat);
-		return -EINVAL;
-	}
-
-	/* If PCLK is used to latch data from the sensor, check sense */
-	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
-		/* The caller holds a mutex. */
-		icd->sense = &sense;
-
-	mf->width	= pix->width;
-	mf->height	= pix->height;
-	mf->field	= pix->field;
-	mf->colorspace	= pix->colorspace;
-	mf->code	= xlate->code;
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	const struct soc_mbus_pixelfmt *format;
+	unsigned int idx;
 
-	ret = sensor_call(pcdev, pad, set_fmt, NULL, &format);
-
-	if (mf->code != xlate->code)
+	for (idx = 0; pcdev->user_formats[idx].code; idx++);
+	if (f->index >= idx)
 		return -EINVAL;
 
-	icd->sense = NULL;
-
-	if (ret < 0) {
-		dev_warn(pcdev_to_dev(pcdev),
-			 "Failed to configure for format %x\n",
-			 pix->pixelformat);
-	} else if (pxa_camera_check_frame(mf->width, mf->height)) {
-		dev_warn(pcdev_to_dev(pcdev),
-			 "Camera driver produced an unsupported frame %dx%d\n",
-			 mf->width, mf->height);
-		ret = -EINVAL;
-	} else if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {
-		if (sense.pixel_clock > sense.pixel_clock_max) {
-			dev_err(pcdev_to_dev(pcdev),
-				"pixel clock %lu set by the camera too high!",
-				sense.pixel_clock);
-			return -EIO;
-		}
-		recalculate_fifo_timeout(pcdev, sense.pixel_clock);
-	}
-
-	if (ret < 0)
-		return ret;
+	format = pcdev->user_formats[f->index].host_fmt;
+	f->pixelformat = format->fourcc;
+	return 0;
+}
 
-	pix->width		= mf->width;
-	pix->height		= mf->height;
-	pix->field		= mf->field;
-	pix->colorspace		= mf->colorspace;
-	icd->current_fmt	= xlate;
+static int pxac_vidioc_g_fmt_vid_cap(struct file *filp, void *priv,
+				    struct v4l2_format *f)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
 
-	return ret;
+	pix->width		= pcdev->current_pix.width;
+	pix->height		= pcdev->current_pix.height;
+	pix->bytesperline	= pcdev->current_pix.bytesperline;
+	pix->sizeimage		= pcdev->current_pix.sizeimage;
+	pix->field		= pcdev->current_pix.field;
+	pix->pixelformat	= pcdev->current_fmt->host_fmt->fourcc;
+	pix->colorspace		= pcdev->current_pix.colorspace;
+	dev_dbg(pcdev_to_dev(pcdev), "current_fmt->fourcc: 0x%08x\n",
+		pcdev->current_fmt->host_fmt->fourcc);
+	return 0;
 }
 
-static int pxa_camera_try_fmt(struct soc_camera_device *icd,
-			      struct v4l2_format *f)
+static int pxac_vidioc_try_fmt_vid_cap(struct file *filp, void *priv,
+				      struct v4l2_format *f)
 {
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct pxa_camera_dev *pcdev = icd_to_pcdev(icd);
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
 	struct v4l2_subdev_pad_config pad_cfg;
@@ -1384,7 +1390,7 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 	__u32 pixfmt = pix->pixelformat;
 	int ret;
 
-	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	xlate = soc_mbus_xlate_by_fourcc(pcdev->user_formats, pixfmt);
 	if (!xlate) {
 		dev_warn(pcdev_to_dev(pcdev), "Format %x not found\n", pixfmt);
 		return -EINVAL;
@@ -1400,26 +1406,18 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 			      &pix->height, 32, 2048, 0,
 			      pixfmt == V4L2_PIX_FMT_YUV422P ? 4 : 0);
 
-	/* limit to sensor capabilities */
-	mf->width	= pix->width;
-	mf->height	= pix->height;
-	/* Only progressive video supported so far */
-	mf->field	= V4L2_FIELD_NONE;
-	mf->colorspace	= pix->colorspace;
-	mf->code	= xlate->code;
-
+	v4l2_fill_mbus_format(mf, pix, xlate->code);
 	ret = sensor_call(pcdev, pad, set_fmt, &pad_cfg, &format);
 	if (ret < 0)
 		return ret;
 
-	pix->width	= mf->width;
-	pix->height	= mf->height;
-	pix->colorspace	= mf->colorspace;
+	v4l2_fill_pix_format(pix, mf);
 
+	/* Only progressive video supported so far */
 	switch (mf->field) {
 	case V4L2_FIELD_ANY:
 	case V4L2_FIELD_NONE:
-		pix->field	= V4L2_FIELD_NONE;
+		pix->field = V4L2_FIELD_NONE;
 		break;
 	default:
 		/* TODO: support interlaced at least in pass-through mode */
@@ -1428,20 +1426,74 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 		return -EINVAL;
 	}
 
-	return ret;
+	ret = soc_mbus_bytes_per_line(pix->width, xlate->host_fmt);
+	if (ret < 0)
+		return ret;
+
+	pix->bytesperline = ret;
+	ret = soc_mbus_image_size(xlate->host_fmt, pix->bytesperline,
+				  pix->height);
+	if (ret < 0)
+		return ret;
+
+	pix->sizeimage = ret;
+	return 0;
 }
 
-static unsigned int pxa_camera_poll(struct file *file, poll_table *pt)
+static int pxac_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
+				    struct v4l2_format *f)
 {
-	struct soc_camera_device *icd = file->private_data;
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	unsigned long flags;
+	int ret, is_busy;
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		"s_fmt_vid_cap(pix=%dx%d:%x)\n",
+		pix->width, pix->height, pix->pixelformat);
+
+	spin_lock_irqsave(&pcdev->lock, flags);
+	is_busy = pcdev->active || vb2_is_busy(&pcdev->vb2_vq);
+	spin_unlock_irqrestore(&pcdev->lock, flags);
+
+	if (is_busy)
+		return -EBUSY;
+
+	ret = pxac_vidioc_try_fmt_vid_cap(filp, priv, f);
+	if (ret)
+		return ret;
+
+	xlate = soc_mbus_xlate_by_fourcc(pcdev->user_formats,
+					 pix->pixelformat);
+	v4l2_fill_mbus_format(&format.format, pix, xlate->code);
+	ret = sensor_call(pcdev, pad, set_fmt, NULL, &format);
+	if (ret < 0) {
+		dev_warn(pcdev_to_dev(pcdev),
+			 "Failed to configure for format %x\n",
+			 pix->pixelformat);
+	} else if (pxa_camera_check_frame(pix->width, pix->height)) {
+		dev_warn(pcdev_to_dev(pcdev),
+			 "Camera driver produced an unsupported frame %dx%d\n",
+			 pix->width, pix->height);
+		return -EINVAL;
+	}
+
+	pcdev->current_fmt = xlate;
+	pcdev->current_pix = *pix;
 
-	return vb2_poll(&icd->vb2_vidq, file, pt);
+	ret = pxa_camera_set_bus_param(pcdev);
+	return ret;
 }
 
-static int pxa_camera_querycap(struct soc_camera_host *ici,
-			       struct v4l2_capability *cap)
+static int pxac_vidioc_querycap(struct file *file, void *priv,
+				struct v4l2_capability *cap)
 {
-	/* cap->name is set by the firendly caller:-> */
+	strlcpy(cap->bus_info, "platform:pxa-camera", sizeof(cap->bus_info));
+	strlcpy(cap->driver, PXA_CAM_DRV_NAME, sizeof(cap->driver));
 	strlcpy(cap->card, pxa_cam_driver_description, sizeof(cap->card));
 	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
@@ -1449,13 +1501,212 @@ static int pxa_camera_querycap(struct soc_camera_host *ici,
 	return 0;
 }
 
+static int pxac_vidioc_enum_input(struct file *file, void *priv,
+				  struct v4l2_input *i)
+{
+	if (i->index > 0)
+		return -EINVAL;
+
+	i->type = V4L2_INPUT_TYPE_CAMERA;
+	strlcpy(i->name, "Camera", sizeof(i->name));
+
+	return 0;
+}
+
+static int pxac_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+
+	return 0;
+}
+
+static int pxac_vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	if (i > 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int pxac_fops_camera_open(struct file *filp)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	int ret;
+
+	mutex_lock(&pcdev->mlock);
+	ret = v4l2_fh_open(filp);
+	if (ret < 0)
+		goto out;
+
+	ret = sensor_call(pcdev, core, s_power, 1);
+	if (ret)
+		v4l2_fh_release(filp);
+out:
+	mutex_unlock(&pcdev->mlock);
+	return ret;
+}
+
+static int pxac_fops_camera_release(struct file *filp)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	int ret;
+
+	ret = vb2_fop_release(filp);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&pcdev->mlock);
+	ret = sensor_call(pcdev, core, s_power, 0);
+	mutex_unlock(&pcdev->mlock);
+
+	return ret;
+}
+
+static const struct v4l2_file_operations pxa_camera_fops = {
+	.owner		= THIS_MODULE,
+	.open		= pxac_fops_camera_open,
+	.release	= pxac_fops_camera_release,
+	.read		= vb2_fop_read,
+	.poll		= vb2_fop_poll,
+	.mmap		= vb2_fop_mmap,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops pxa_camera_ioctl_ops = {
+	.vidioc_querycap		= pxac_vidioc_querycap,
+
+	.vidioc_enum_input		= pxac_vidioc_enum_input,
+	.vidioc_g_input			= pxac_vidioc_g_input,
+	.vidioc_s_input			= pxac_vidioc_s_input,
+
+	.vidioc_enum_fmt_vid_cap	= pxac_vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		= pxac_vidioc_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= pxac_vidioc_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= pxac_vidioc_try_fmt_vid_cap,
+
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+};
+
+static struct v4l2_clk_ops pxa_camera_mclk_ops = {
+};
+
+static const struct video_device pxa_camera_videodev_template = {
+	.name = "pxa-camera",
+	.minor = -1,
+	.fops = &pxa_camera_fops,
+	.ioctl_ops = &pxa_camera_ioctl_ops,
+	.release = video_device_release_empty,
+	.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING,
+};
+
+static int pxa_camera_sensor_bound(struct v4l2_async_notifier *notifier,
+		     struct v4l2_subdev *subdev,
+		     struct v4l2_async_subdev *asd)
+{
+	int err;
+	struct v4l2_device *v4l2_dev = notifier->v4l2_dev;
+	struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(v4l2_dev);
+	struct video_device *vdev = &pcdev->vdev;
+	struct v4l2_pix_format *pix = &pcdev->current_pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+
+	dev_info(pcdev_to_dev(pcdev), "%s(): trying to bind a device\n",
+		 __func__);
+	mutex_lock(&pcdev->mlock);
+	*vdev = pxa_camera_videodev_template;
+	vdev->v4l2_dev = v4l2_dev;
+	vdev->lock = &pcdev->mlock;
+	pcdev->sensor = subdev;
+	pcdev->vdev.queue = &pcdev->vb2_vq;
+	pcdev->vdev.v4l2_dev = &pcdev->v4l2_dev;
+	pcdev->vdev.ctrl_handler = subdev->ctrl_handler;
+	video_set_drvdata(&pcdev->vdev, pcdev);
+
+	err = pxa_camera_build_formats(pcdev);
+	if (err) {
+		dev_err(pcdev_to_dev(pcdev), "building formats failed: %d\n",
+			err);
+		goto out;
+	}
+
+	pcdev->current_fmt = pcdev->user_formats;
+	pix->field = V4L2_FIELD_NONE;
+	pix->width = DEFAULT_WIDTH;
+	pix->height = DEFAULT_HEIGHT;
+	pix->bytesperline =
+		soc_mbus_bytes_per_line(pix->width,
+					pcdev->current_fmt->host_fmt);
+	pix->sizeimage =
+		soc_mbus_image_size(pcdev->current_fmt->host_fmt,
+				    pix->bytesperline, pix->height);
+	pix->pixelformat = pcdev->current_fmt->host_fmt->fourcc;
+	v4l2_fill_mbus_format(mf, pix, pcdev->current_fmt->code);
+	err = sensor_call(pcdev, pad, set_fmt, NULL, &format);
+	if (err)
+		goto out;
+
+	v4l2_fill_pix_format(pix, mf);
+	pr_info("%s(): colorspace=0x%x pixfmt=0x%x\n",
+		__func__, pix->colorspace, pix->pixelformat);
+
+	err = pxa_camera_init_videobuf2(pcdev);
+	if (err)
+		goto out;
+
+	err = video_register_device(&pcdev->vdev, VFL_TYPE_GRABBER, -1);
+	if (err) {
+		v4l2_err(v4l2_dev, "register video device failed: %d\n", err);
+		pcdev->sensor = NULL;
+	} else {
+		dev_info(pcdev_to_dev(pcdev),
+			 "PXA Camera driver attached to camera %s\n",
+			 subdev->name);
+	}
+out:
+	mutex_unlock(&pcdev->mlock);
+	return err;
+}
+
+static void pxa_camera_sensor_unbind(struct v4l2_async_notifier *notifier,
+		     struct v4l2_subdev *subdev,
+		     struct v4l2_async_subdev *asd)
+{
+	struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(notifier->v4l2_dev);
+
+	mutex_lock(&pcdev->mlock);
+	dev_info(pcdev_to_dev(pcdev),
+		 "PXA Camera driver detached from camera %s\n",
+		 subdev->name);
+
+	/* disable capture, disable interrupts */
+	__raw_writel(0x3ff, pcdev->base + CICR0);
+
+	/* Stop DMA engine */
+	pxa_dma_stop_channels(pcdev);
+
+	pxa_camera_destroy_formats(pcdev);
+	video_unregister_device(&pcdev->vdev);
+	pcdev->sensor = NULL;
+
+	mutex_unlock(&pcdev->mlock);
+}
+
 /*
  * Driver probe, remove, suspend and resume operations
  */
 static int pxa_camera_suspend(struct device *dev)
 {
-	struct soc_camera_host *ici = to_soc_camera_host(dev);
-	struct pxa_camera_dev *pcdev = ici->priv;
+	struct pxa_camera_dev *pcdev = dev_get_drvdata(dev);
 	int i = 0, ret = 0;
 
 	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR0);
@@ -1464,8 +1715,7 @@ static int pxa_camera_suspend(struct device *dev)
 	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR3);
 	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR4);
 
-	if (pcdev->soc_host.icd) {
-		struct v4l2_subdev *sd = soc_camera_to_subdev(pcdev->soc_host.icd);
+	if (pcdev->sensor) {
 		ret = sensor_call(pcdev, core, s_power, 0);
 		if (ret == -ENOIOCTLCMD)
 			ret = 0;
@@ -1476,8 +1726,7 @@ static int pxa_camera_suspend(struct device *dev)
 
 static int pxa_camera_resume(struct device *dev)
 {
-	struct soc_camera_host *ici = to_soc_camera_host(dev);
-	struct pxa_camera_dev *pcdev = ici->priv;
+	struct pxa_camera_dev *pcdev = dev_get_drvdata(dev);
 	int i = 0, ret = 0;
 
 	__raw_writel(pcdev->save_cicr[i++] & ~CICR0_ENB, pcdev->base + CICR0);
@@ -1486,8 +1735,7 @@ static int pxa_camera_resume(struct device *dev)
 	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR3);
 	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR4);
 
-	if (pcdev->soc_host.icd) {
-		struct v4l2_subdev *sd = soc_camera_to_subdev(pcdev->soc_host.icd);
+	if (pcdev->sensor) {
 		ret = sensor_call(pcdev, core, s_power, 1);
 		if (ret == -ENOIOCTLCMD)
 			ret = 0;
@@ -1500,27 +1748,12 @@ static int pxa_camera_resume(struct device *dev)
 	return ret;
 }
 
-static struct soc_camera_host_ops pxa_soc_camera_host_ops = {
-	.owner		= THIS_MODULE,
-	.add		= pxa_camera_add_device,
-	.remove		= pxa_camera_remove_device,
-	.clock_start	= pxa_camera_clock_start,
-	.clock_stop	= pxa_camera_clock_stop,
-	.get_formats	= pxa_camera_get_formats,
-	.put_formats	= pxa_camera_put_formats,
-	.set_fmt	= pxa_camera_set_fmt,
-	.try_fmt	= pxa_camera_try_fmt,
-	.init_videobuf2	= pxa_camera_init_videobuf2,
-	.poll		= pxa_camera_poll,
-	.querycap	= pxa_camera_querycap,
-	.set_bus_param	= pxa_camera_set_bus_param,
-};
-
 static int pxa_camera_pdata_from_dt(struct device *dev,
-				    struct pxa_camera_dev *pcdev)
+				    struct pxa_camera_dev *pcdev,
+				    struct v4l2_async_subdev *asd)
 {
 	u32 mclk_rate;
-	struct device_node *np = dev->of_node;
+	struct device_node *remote, *np = dev->of_node;
 	struct v4l2_of_endpoint ep;
 	int err = of_property_read_u32(np, "clock-frequency",
 				       &mclk_rate);
@@ -1531,13 +1764,13 @@ static int pxa_camera_pdata_from_dt(struct device *dev,
 
 	np = of_graph_get_next_endpoint(np, NULL);
 	if (!np) {
-		dev_err(pcdev_to_dev(pcdev), "could not find endpoint\n");
+		dev_err(dev, "could not find endpoint\n");
 		return -EINVAL;
 	}
 
 	err = v4l2_of_parse_endpoint(np, &ep);
 	if (err) {
-		dev_err(pcdev_to_dev(pcdev), "could not parse endpoint\n");
+		dev_err(dev, "could not parse endpoint\n");
 		goto out;
 	}
 
@@ -1572,6 +1805,15 @@ static int pxa_camera_pdata_from_dt(struct device *dev,
 	if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
 		pcdev->platform_flags |= PXA_CAMERA_PCLK_EN;
 
+	asd->match_type = V4L2_ASYNC_MATCH_OF;
+	remote = of_graph_get_remote_port(np);
+	if (remote) {
+		asd->match.of.node = remote;
+		of_node_put(remote);
+	} else {
+		dev_notice(dev, "no remote for %s\n", of_node_full_name(np));
+	}
+
 out:
 	of_node_put(np);
 
@@ -1590,6 +1832,7 @@ static int pxa_camera_probe(struct platform_device *pdev)
 	};
 	dma_cap_mask_t mask;
 	struct pxad_param params;
+	char clk_name[V4L2_CLK_NAME_SIZE];
 	int irq;
 	int err = 0, i;
 
@@ -1612,10 +1855,14 @@ static int pxa_camera_probe(struct platform_device *pdev)
 
 	pcdev->pdata = pdev->dev.platform_data;
 	if (&pdev->dev.of_node && !pcdev->pdata) {
-		err = pxa_camera_pdata_from_dt(&pdev->dev, pcdev);
+		err = pxa_camera_pdata_from_dt(&pdev->dev, pcdev, &pcdev->asd);
 	} else {
 		pcdev->platform_flags = pcdev->pdata->flags;
 		pcdev->mclk = pcdev->pdata->mclk_10khz * 10000;
+		pcdev->asd.match_type = V4L2_ASYNC_MATCH_I2C;
+		pcdev->asd.match.i2c.adapter_id =
+			pcdev->pdata->sensor_i2c_adapter_id;
+		pcdev->asd.match.i2c.address = pcdev->pdata->sensor_i2c_address;
 	}
 	if (err < 0)
 		return err;
@@ -1647,6 +1894,7 @@ static int pxa_camera_probe(struct platform_device *pdev)
 
 	INIT_LIST_HEAD(&pcdev->capture);
 	spin_lock_init(&pcdev->lock);
+	mutex_init(&pcdev->mlock);
 
 	/*
 	 * Request the regions.
@@ -1709,19 +1957,48 @@ static int pxa_camera_probe(struct platform_device *pdev)
 		goto exit_free_dma;
 	}
 
-	pcdev->soc_host.drv_name	= PXA_CAM_DRV_NAME;
-	pcdev->soc_host.ops		= &pxa_soc_camera_host_ops;
-	pcdev->soc_host.priv		= pcdev;
-	pcdev->soc_host.v4l2_dev.dev	= &pdev->dev;
-	pcdev->soc_host.nr		= pdev->id;
 	tasklet_init(&pcdev->task_eof, pxa_camera_eof, (unsigned long)pcdev);
 
-	err = soc_camera_host_register(&pcdev->soc_host);
+	pxa_camera_activate(pcdev);
+
+	dev_set_drvdata(&pdev->dev, pcdev);
+	err = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
 	if (err)
 		goto exit_free_dma;
 
-	return 0;
+	pcdev->asds[0] = &pcdev->asd;
+	pcdev->notifier.subdevs = pcdev->asds;
+	pcdev->notifier.num_subdevs = 1;
+	pcdev->notifier.bound = pxa_camera_sensor_bound;
+	pcdev->notifier.unbind = pxa_camera_sensor_unbind;
+
+	if (!of_have_populated_dt())
+		pcdev->asd.match_type = V4L2_ASYNC_MATCH_I2C;
+
+	err = pxa_camera_init_videobuf2(pcdev);
+	if (err)
+		goto exit_free_v4l2dev;
 
+	if (pcdev->mclk) {
+		v4l2_clk_name_i2c(clk_name, sizeof(clk_name),
+				  pcdev->asd.match.i2c.adapter_id,
+				  pcdev->asd.match.i2c.address);
+
+		pcdev->mclk_clk = v4l2_clk_register(&pxa_camera_mclk_ops,
+						    clk_name, NULL);
+		if (IS_ERR(pcdev->mclk_clk))
+			return PTR_ERR(pcdev->mclk_clk);
+	}
+
+	err = v4l2_async_notifier_register(&pcdev->v4l2_dev, &pcdev->notifier);
+	if (err)
+		goto exit_free_clk;
+
+	return 0;
+exit_free_clk:
+	v4l2_clk_unregister(pcdev->mclk_clk);
+exit_free_v4l2dev:
+	v4l2_device_unregister(&pcdev->v4l2_dev);
 exit_free_dma:
 	dma_release_channel(pcdev->dma_chans[2]);
 exit_free_dma_u:
@@ -1733,15 +2010,15 @@ exit_free_dma_y:
 
 static int pxa_camera_remove(struct platform_device *pdev)
 {
-	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
-	struct pxa_camera_dev *pcdev = container_of(soc_host,
-					struct pxa_camera_dev, soc_host);
+	struct pxa_camera_dev *pcdev = dev_get_drvdata(&pdev->dev);
 
+	pxa_camera_deactivate(pcdev);
 	dma_release_channel(pcdev->dma_chans[0]);
 	dma_release_channel(pcdev->dma_chans[1]);
 	dma_release_channel(pcdev->dma_chans[2]);
 
-	soc_camera_host_unregister(soc_host);
+	v4l2_clk_unregister(pcdev->mclk_clk);
+	v4l2_device_unregister(&pcdev->v4l2_dev);
 
 	dev_info(&pdev->dev, "PXA Camera driver unloaded\n");
 
diff --git a/include/linux/platform_data/media/camera-pxa.h b/include/linux/platform_data/media/camera-pxa.h
index 6709b1cd7c77..ce5d90e1a6e4 100644
--- a/include/linux/platform_data/media/camera-pxa.h
+++ b/include/linux/platform_data/media/camera-pxa.h
@@ -37,6 +37,8 @@
 struct pxacamera_platform_data {
 	unsigned long flags;
 	unsigned long mclk_10khz;
+	int sensor_i2c_adapter_id;
+	int sensor_i2c_address;
 };
 
 extern void pxa_set_camera_info(struct pxacamera_platform_data *);
-- 
2.1.4

[PATCH v5 11/13] media: platform: pxa_camera: add debug register access

[Robert Jarzmik]

Add pxa_camera registers access through advanced video debugging.

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
 drivers/media/platform/soc_camera/pxa_camera.c | 32 ++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index 395cd398c32b..fb89b85f59ab 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -1342,6 +1342,34 @@ static int pxa_camera_check_frame(u32 width, u32 height)
 		(width & 0x01);
 }
 
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int pxac_vidioc_g_register(struct file *file, void *priv,
+				  struct v4l2_dbg_register *reg)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(file);
+
+	if (reg->reg > CIBR2)
+		return -ERANGE;
+
+	reg->val = __raw_readl(pcdev->base + reg->reg);
+	reg->size = sizeof(__u32);
+	return 0;
+}
+
+static int pxac_vidioc_s_register(struct file *file, void *priv,
+				  const struct v4l2_dbg_register *reg)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(file);
+
+	if (reg->reg > CIBR2)
+		return -ERANGE;
+	if (reg->size != sizeof(__u32))
+		return -EINVAL;
+	__raw_writel(reg->val, pcdev->base + reg->reg);
+	return 0;
+}
+#endif
+
 static int pxac_vidioc_enum_fmt_vid_cap(struct file *filp, void  *priv,
 					struct v4l2_fmtdesc *f)
 {
@@ -1592,6 +1620,10 @@ static const struct v4l2_ioctl_ops pxa_camera_ioctl_ops = {
 	.vidioc_expbuf			= vb2_ioctl_expbuf,
 	.vidioc_streamon		= vb2_ioctl_streamon,
 	.vidioc_streamoff		= vb2_ioctl_streamoff,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register		= pxac_vidioc_g_register,
+	.vidioc_s_register		= pxac_vidioc_s_register,
+#endif
 };
 
 static struct v4l2_clk_ops pxa_camera_mclk_ops = {
-- 
2.1.4

[PATCH v5 12/13] media: platform: pxa_camera: change stop_streaming semantics

[Robert Jarzmik]

Instead of the legacy behavior where it was required to wait for all
video buffers to be finished by the hardware, use a cancel like strategy
: as soon as the stop_streaming() call is done, abort all DMA transfers,
report the already buffers as failed and return.

This makes stop_streaming() more a "cancel capture" than a "wait for end
of capture" semantic.

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
 drivers/media/platform/soc_camera/pxa_camera.c | 15 ++++++++++++---
 1 file changed, 12 insertions(+), 3 deletions(-)

diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index fb89b85f59ab..868c6ad4784c 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -523,7 +523,8 @@ static void pxa_camera_stop_capture(struct pxa_camera_dev *pcdev)
 }
 
 static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
-			      struct pxa_buffer *buf)
+			      struct pxa_buffer *buf,
+			      enum vb2_buffer_state state)
 {
 	struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
@@ -645,7 +646,7 @@ static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 	}
 	buf->active_dma &= ~act_dma;
 	if (!buf->active_dma) {
-		pxa_camera_wakeup(pcdev, buf);
+		pxa_camera_wakeup(pcdev, buf, VB2_BUF_STATE_DONE);
 		pxa_camera_check_link_miss(pcdev, last_buf->cookie[chan],
 					   last_issued);
 	}
@@ -1087,7 +1088,15 @@ static int pxac_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
 
 static void pxac_vb2_stop_streaming(struct vb2_queue *vq)
 {
-	vb2_wait_for_all_buffers(vq);
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
+	struct pxa_buffer *buf, *tmp;
+
+	dev_dbg(pcdev_to_dev(pcdev), "%s active=%p\n",
+		__func__, pcdev->active);
+	pxa_camera_stop_capture(pcdev);
+
+	list_for_each_entry_safe(buf, tmp, &pcdev->capture, queue)
+		pxa_camera_wakeup(pcdev, buf, VB2_BUF_STATE_ERROR);
 }
 
 static struct vb2_ops pxac_vb2_ops = {
-- 
2.1.4

[PATCH v5 13/13] media: platform: pxa_camera: move pxa_camera out of soc_camera

[Robert Jarzmik]

As the conversion to a v4l2 standalone device is finished, move
pxa_camera one directory up and finish severing any dependency to
soc_camera.

Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
---
 drivers/media/platform/Kconfig                 |    8 +
 drivers/media/platform/Makefile                |    1 +
 drivers/media/platform/pxa_camera.c            | 2096 ++++++++++++++++++++++++
 drivers/media/platform/soc_camera/Kconfig      |    8 -
 drivers/media/platform/soc_camera/Makefile     |    1 -
 drivers/media/platform/soc_camera/pxa_camera.c | 2096 ------------------------
 6 files changed, 2105 insertions(+), 2105 deletions(-)
 create mode 100644 drivers/media/platform/pxa_camera.c
 delete mode 100644 drivers/media/platform/soc_camera/pxa_camera.c

diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index f25344bc7912..299af1977d61 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -91,6 +91,14 @@ config VIDEO_OMAP3_DEBUG
 	---help---
 	  Enable debug messages on OMAP 3 camera controller driver.
 
+config VIDEO_PXA27x
+	tristate "PXA27x Quick Capture Interface driver"
+	depends on VIDEO_DEV && PXA27x && HAS_DMA
+	select VIDEOBUF2_DMA_SG
+	select SG_SPLIT
+	---help---
+	  This is a v4l2 driver for the PXA27x Quick Capture Interface
+
 config VIDEO_S3C_CAMIF
 	tristate "Samsung S3C24XX/S3C64XX SoC Camera Interface driver"
 	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
index 21771c1a13fb..e38918bbe0d0 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_VIDEO_CAFE_CCIC) += marvell-ccic/
 obj-$(CONFIG_VIDEO_MMP_CAMERA) += marvell-ccic/
 
 obj-$(CONFIG_VIDEO_OMAP3)	+= omap3isp/
+obj-$(CONFIG_VIDEO_PXA27x)	+= pxa_camera.o soc_camera/soc_mediabus.o
 
 obj-$(CONFIG_VIDEO_VIU) += fsl-viu.o
 
diff --git a/drivers/media/platform/pxa_camera.c b/drivers/media/platform/pxa_camera.c
new file mode 100644
index 000000000000..868c6ad4784c
--- /dev/null
+++ b/drivers/media/platform/pxa_camera.c
@@ -0,0 +1,2096 @@
+/*
+ * V4L2 Driver for PXA camera host
+ *
+ * Copyright (C) 2006, Sascha Hauer, Pengutronix
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ * Copyright (C) 2016, Robert Jarzmik <robert.jarzmik@free.fr>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/moduleparam.h>
+#include <linux/of.h>
+#include <linux/time.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/dma/pxa-dma.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-clk.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-of.h>
+
+#include <media/drv-intf/soc_mediabus.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include <linux/videodev2.h>
+
+#include <linux/platform_data/media/camera-pxa.h>
+
+#define PXA_CAM_VERSION "0.0.6"
+#define PXA_CAM_DRV_NAME "pxa27x-camera"
+
+#define DEFAULT_WIDTH	640
+#define DEFAULT_HEIGHT	480
+
+/* Camera Interface */
+#define CICR0		0x0000
+#define CICR1		0x0004
+#define CICR2		0x0008
+#define CICR3		0x000C
+#define CICR4		0x0010
+#define CISR		0x0014
+#define CIFR		0x0018
+#define CITOR		0x001C
+#define CIBR0		0x0028
+#define CIBR1		0x0030
+#define CIBR2		0x0038
+
+#define CICR0_DMAEN	(1 << 31)	/* DMA request enable */
+#define CICR0_PAR_EN	(1 << 30)	/* Parity enable */
+#define CICR0_SL_CAP_EN	(1 << 29)	/* Capture enable for slave mode */
+#define CICR0_ENB	(1 << 28)	/* Camera interface enable */
+#define CICR0_DIS	(1 << 27)	/* Camera interface disable */
+#define CICR0_SIM	(0x7 << 24)	/* Sensor interface mode mask */
+#define CICR0_TOM	(1 << 9)	/* Time-out mask */
+#define CICR0_RDAVM	(1 << 8)	/* Receive-data-available mask */
+#define CICR0_FEM	(1 << 7)	/* FIFO-empty mask */
+#define CICR0_EOLM	(1 << 6)	/* End-of-line mask */
+#define CICR0_PERRM	(1 << 5)	/* Parity-error mask */
+#define CICR0_QDM	(1 << 4)	/* Quick-disable mask */
+#define CICR0_CDM	(1 << 3)	/* Disable-done mask */
+#define CICR0_SOFM	(1 << 2)	/* Start-of-frame mask */
+#define CICR0_EOFM	(1 << 1)	/* End-of-frame mask */
+#define CICR0_FOM	(1 << 0)	/* FIFO-overrun mask */
+
+#define CICR1_TBIT	(1 << 31)	/* Transparency bit */
+#define CICR1_RGBT_CONV	(0x3 << 29)	/* RGBT conversion mask */
+#define CICR1_PPL	(0x7ff << 15)	/* Pixels per line mask */
+#define CICR1_RGB_CONV	(0x7 << 12)	/* RGB conversion mask */
+#define CICR1_RGB_F	(1 << 11)	/* RGB format */
+#define CICR1_YCBCR_F	(1 << 10)	/* YCbCr format */
+#define CICR1_RGB_BPP	(0x7 << 7)	/* RGB bis per pixel mask */
+#define CICR1_RAW_BPP	(0x3 << 5)	/* Raw bis per pixel mask */
+#define CICR1_COLOR_SP	(0x3 << 3)	/* Color space mask */
+#define CICR1_DW	(0x7 << 0)	/* Data width mask */
+
+#define CICR2_BLW	(0xff << 24)	/* Beginning-of-line pixel clock
+					   wait count mask */
+#define CICR2_ELW	(0xff << 16)	/* End-of-line pixel clock
+					   wait count mask */
+#define CICR2_HSW	(0x3f << 10)	/* Horizontal sync pulse width mask */
+#define CICR2_BFPW	(0x3f << 3)	/* Beginning-of-frame pixel clock
+					   wait count mask */
+#define CICR2_FSW	(0x7 << 0)	/* Frame stabilization
+					   wait count mask */
+
+#define CICR3_BFW	(0xff << 24)	/* Beginning-of-frame line clock
+					   wait count mask */
+#define CICR3_EFW	(0xff << 16)	/* End-of-frame line clock
+					   wait count mask */
+#define CICR3_VSW	(0x3f << 10)	/* Vertical sync pulse width mask */
+#define CICR3_BFPW	(0x3f << 3)	/* Beginning-of-frame pixel clock
+					   wait count mask */
+#define CICR3_LPF	(0x7ff << 0)	/* Lines per frame mask */
+
+#define CICR4_MCLK_DLY	(0x3 << 24)	/* MCLK Data Capture Delay mask */
+#define CICR4_PCLK_EN	(1 << 23)	/* Pixel clock enable */
+#define CICR4_PCP	(1 << 22)	/* Pixel clock polarity */
+#define CICR4_HSP	(1 << 21)	/* Horizontal sync polarity */
+#define CICR4_VSP	(1 << 20)	/* Vertical sync polarity */
+#define CICR4_MCLK_EN	(1 << 19)	/* MCLK enable */
+#define CICR4_FR_RATE	(0x7 << 8)	/* Frame rate mask */
+#define CICR4_DIV	(0xff << 0)	/* Clock divisor mask */
+
+#define CISR_FTO	(1 << 15)	/* FIFO time-out */
+#define CISR_RDAV_2	(1 << 14)	/* Channel 2 receive data available */
+#define CISR_RDAV_1	(1 << 13)	/* Channel 1 receive data available */
+#define CISR_RDAV_0	(1 << 12)	/* Channel 0 receive data available */
+#define CISR_FEMPTY_2	(1 << 11)	/* Channel 2 FIFO empty */
+#define CISR_FEMPTY_1	(1 << 10)	/* Channel 1 FIFO empty */
+#define CISR_FEMPTY_0	(1 << 9)	/* Channel 0 FIFO empty */
+#define CISR_EOL	(1 << 8)	/* End of line */
+#define CISR_PAR_ERR	(1 << 7)	/* Parity error */
+#define CISR_CQD	(1 << 6)	/* Camera interface quick disable */
+#define CISR_CDD	(1 << 5)	/* Camera interface disable done */
+#define CISR_SOF	(1 << 4)	/* Start of frame */
+#define CISR_EOF	(1 << 3)	/* End of frame */
+#define CISR_IFO_2	(1 << 2)	/* FIFO overrun for Channel 2 */
+#define CISR_IFO_1	(1 << 1)	/* FIFO overrun for Channel 1 */
+#define CISR_IFO_0	(1 << 0)	/* FIFO overrun for Channel 0 */
+
+#define CIFR_FLVL2	(0x7f << 23)	/* FIFO 2 level mask */
+#define CIFR_FLVL1	(0x7f << 16)	/* FIFO 1 level mask */
+#define CIFR_FLVL0	(0xff << 8)	/* FIFO 0 level mask */
+#define CIFR_THL_0	(0x3 << 4)	/* Threshold Level for Channel 0 FIFO */
+#define CIFR_RESET_F	(1 << 3)	/* Reset input FIFOs */
+#define CIFR_FEN2	(1 << 2)	/* FIFO enable for channel 2 */
+#define CIFR_FEN1	(1 << 1)	/* FIFO enable for channel 1 */
+#define CIFR_FEN0	(1 << 0)	/* FIFO enable for channel 0 */
+
+#define CICR0_SIM_MP	(0 << 24)
+#define CICR0_SIM_SP	(1 << 24)
+#define CICR0_SIM_MS	(2 << 24)
+#define CICR0_SIM_EP	(3 << 24)
+#define CICR0_SIM_ES	(4 << 24)
+
+#define CICR1_DW_VAL(x)   ((x) & CICR1_DW)	    /* Data bus width */
+#define CICR1_PPL_VAL(x)  (((x) << 15) & CICR1_PPL) /* Pixels per line */
+#define CICR1_COLOR_SP_VAL(x)	(((x) << 3) & CICR1_COLOR_SP)	/* color space */
+#define CICR1_RGB_BPP_VAL(x)	(((x) << 7) & CICR1_RGB_BPP)	/* bpp for rgb */
+#define CICR1_RGBT_CONV_VAL(x)	(((x) << 29) & CICR1_RGBT_CONV)	/* rgbt conv */
+
+#define CICR2_BLW_VAL(x)  (((x) << 24) & CICR2_BLW) /* Beginning-of-line pixel clock wait count */
+#define CICR2_ELW_VAL(x)  (((x) << 16) & CICR2_ELW) /* End-of-line pixel clock wait count */
+#define CICR2_HSW_VAL(x)  (((x) << 10) & CICR2_HSW) /* Horizontal sync pulse width */
+#define CICR2_BFPW_VAL(x) (((x) << 3) & CICR2_BFPW) /* Beginning-of-frame pixel clock wait count */
+#define CICR2_FSW_VAL(x)  (((x) << 0) & CICR2_FSW)  /* Frame stabilization wait count */
+
+#define CICR3_BFW_VAL(x)  (((x) << 24) & CICR3_BFW) /* Beginning-of-frame line clock wait count  */
+#define CICR3_EFW_VAL(x)  (((x) << 16) & CICR3_EFW) /* End-of-frame line clock wait count */
+#define CICR3_VSW_VAL(x)  (((x) << 11) & CICR3_VSW) /* Vertical sync pulse width */
+#define CICR3_LPF_VAL(x)  (((x) << 0) & CICR3_LPF)  /* Lines per frame */
+
+#define CICR0_IRQ_MASK (CICR0_TOM | CICR0_RDAVM | CICR0_FEM | CICR0_EOLM | \
+			CICR0_PERRM | CICR0_QDM | CICR0_CDM | CICR0_SOFM | \
+			CICR0_EOFM | CICR0_FOM)
+
+#define sensor_call(cam, o, f, args...) \
+	v4l2_subdev_call(cam->sensor, o, f, ##args)
+
+/*
+ * Format handling
+ */
+/**
+ * struct soc_camera_format_xlate - match between host and sensor formats
+ * @code: code of a sensor provided format
+ * @host_fmt: host format after host translation from code
+ *
+ * Host and sensor translation structure. Used in table of host and sensor
+ * formats matchings in soc_camera_device. A host can override the generic list
+ * generation by implementing get_formats(), and use it for format checks and
+ * format setup.
+ */
+struct soc_camera_format_xlate {
+	u32 code;
+	const struct soc_mbus_pixelfmt *host_fmt;
+};
+
+/*
+ * Structures
+ */
+enum pxa_camera_active_dma {
+	DMA_Y = 0x1,
+	DMA_U = 0x2,
+	DMA_V = 0x4,
+};
+
+/* buffer for one video frame */
+struct pxa_buffer {
+	/* common v4l buffer stuff -- must be first */
+	struct vb2_v4l2_buffer		vbuf;
+	struct list_head		queue;
+	u32	code;
+	int				nb_planes;
+	/* our descriptor lists for Y, U and V channels */
+	struct dma_async_tx_descriptor	*descs[3];
+	dma_cookie_t			cookie[3];
+	struct scatterlist		*sg[3];
+	int				sg_len[3];
+	size_t				plane_sizes[3];
+	int				inwork;
+	enum pxa_camera_active_dma	active_dma;
+};
+
+struct pxa_camera_dev {
+	struct v4l2_device	v4l2_dev;
+	struct video_device	vdev;
+	struct v4l2_async_notifier notifier;
+	struct vb2_queue	vb2_vq;
+	struct v4l2_subdev	*sensor;
+	struct soc_camera_format_xlate *user_formats;
+	const struct soc_camera_format_xlate *current_fmt;
+	struct v4l2_pix_format	current_pix;
+
+	struct v4l2_async_subdev asd;
+	struct v4l2_async_subdev *asds[1];
+
+	/*
+	 * PXA27x is only supposed to handle one camera on its Quick Capture
+	 * interface. If anyone ever builds hardware to enable more than
+	 * one camera, they will have to modify this driver too
+	 */
+	struct clk		*clk;
+
+	unsigned int		irq;
+	void __iomem		*base;
+
+	int			channels;
+	struct dma_chan		*dma_chans[3];
+
+	struct pxacamera_platform_data *pdata;
+	struct resource		*res;
+	unsigned long		platform_flags;
+	unsigned long		ciclk;
+	unsigned long		mclk;
+	u32			mclk_divisor;
+	struct v4l2_clk		*mclk_clk;
+	u16			width_flags;	/* max 10 bits */
+
+	struct list_head	capture;
+
+	spinlock_t		lock;
+	struct mutex		mlock;
+	unsigned int		buf_sequence;
+
+	struct pxa_buffer	*active;
+	struct tasklet_struct	task_eof;
+
+	u32			save_cicr[5];
+};
+
+struct pxa_cam {
+	unsigned long flags;
+};
+
+static const char *pxa_cam_driver_description = "PXA_Camera";
+
+/*
+ * Format translation functions
+ */
+const struct soc_camera_format_xlate *soc_mbus_xlate_by_fourcc(
+	struct soc_camera_format_xlate *user_formats, unsigned int fourcc)
+{
+	unsigned int i;
+
+	for (i = 0; user_formats[i].code; i++)
+		if (user_formats[i].host_fmt->fourcc == fourcc)
+			return user_formats + i;
+	return NULL;
+}
+
+static struct soc_camera_format_xlate *soc_mbus_build_fmts_xlate(
+	struct v4l2_device *v4l2_dev, struct v4l2_subdev *subdev,
+	int (*get_formats)(struct v4l2_device *, unsigned int,
+			   struct soc_camera_format_xlate *xlate))
+{
+	unsigned int i, fmts = 0, raw_fmts = 0;
+	int ret;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct soc_camera_format_xlate *user_formats;
+
+	while (!v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code)) {
+		raw_fmts++;
+		code.index++;
+	}
+
+	/*
+	 * First pass - only count formats this host-sensor
+	 * configuration can provide
+	 */
+	for (i = 0; i < raw_fmts; i++) {
+		ret = get_formats(v4l2_dev, i, NULL);
+		if (ret < 0)
+			return ERR_PTR(ret);
+		fmts += ret;
+	}
+
+	if (!fmts)
+		return ERR_PTR(-ENXIO);
+
+	user_formats = kcalloc(fmts + 1, sizeof(*user_formats), GFP_KERNEL);
+	if (!user_formats)
+		return ERR_PTR(-ENOMEM);
+
+	/* Second pass - actually fill data formats */
+	fmts = 0;
+	for (i = 0; i < raw_fmts; i++) {
+		ret = get_formats(v4l2_dev, i, user_formats + fmts);
+		if (ret < 0)
+			goto egfmt;
+		fmts += ret;
+	}
+	user_formats[fmts].code = 0;
+
+	return user_formats;
+egfmt:
+	kfree(user_formats);
+	return ERR_PTR(ret);
+}
+
+/*
+ *  Videobuf operations
+ */
+static struct pxa_buffer *vb2_to_pxa_buffer(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	return container_of(vbuf, struct pxa_buffer, vbuf);
+}
+
+static struct device *pcdev_to_dev(struct pxa_camera_dev *pcdev)
+{
+	return pcdev->v4l2_dev.dev;
+}
+
+static struct pxa_camera_dev *v4l2_dev_to_pcdev(struct v4l2_device *v4l2_dev)
+{
+	return container_of(v4l2_dev, struct pxa_camera_dev, v4l2_dev);
+}
+
+static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
+			       enum pxa_camera_active_dma act_dma);
+
+static void pxa_camera_dma_irq_y(void *data)
+{
+	struct pxa_camera_dev *pcdev = data;
+
+	pxa_camera_dma_irq(pcdev, DMA_Y);
+}
+
+static void pxa_camera_dma_irq_u(void *data)
+{
+	struct pxa_camera_dev *pcdev = data;
+
+	pxa_camera_dma_irq(pcdev, DMA_U);
+}
+
+static void pxa_camera_dma_irq_v(void *data)
+{
+	struct pxa_camera_dev *pcdev = data;
+
+	pxa_camera_dma_irq(pcdev, DMA_V);
+}
+
+/**
+ * pxa_init_dma_channel - init dma descriptors
+ * @pcdev: pxa camera device
+ * @vb: videobuffer2 buffer
+ * @dma: dma video buffer
+ * @channel: dma channel (0 => 'Y', 1 => 'U', 2 => 'V')
+ * @cibr: camera Receive Buffer Register
+ *
+ * Prepares the pxa dma descriptors to transfer one camera channel.
+ *
+ * Returns 0 if success or -ENOMEM if no memory is available
+ */
+static int pxa_init_dma_channel(struct pxa_camera_dev *pcdev,
+				struct pxa_buffer *buf, int channel,
+				struct scatterlist *sg, int sglen)
+{
+	struct dma_chan *dma_chan = pcdev->dma_chans[channel];
+	struct dma_async_tx_descriptor *tx;
+
+	tx = dmaengine_prep_slave_sg(dma_chan, sg, sglen, DMA_DEV_TO_MEM,
+				     DMA_PREP_INTERRUPT | DMA_CTRL_REUSE);
+	if (!tx) {
+		dev_err(pcdev_to_dev(pcdev),
+			"dmaengine_prep_slave_sg failed\n");
+		goto fail;
+	}
+
+	tx->callback_param = pcdev;
+	switch (channel) {
+	case 0:
+		tx->callback = pxa_camera_dma_irq_y;
+		break;
+	case 1:
+		tx->callback = pxa_camera_dma_irq_u;
+		break;
+	case 2:
+		tx->callback = pxa_camera_dma_irq_v;
+		break;
+	}
+
+	buf->descs[channel] = tx;
+	return 0;
+fail:
+	dev_dbg(pcdev_to_dev(pcdev),
+		"%s (vb=%p) dma_tx=%p\n",
+		__func__, buf, tx);
+
+	return -ENOMEM;
+}
+
+static void pxa_videobuf_set_actdma(struct pxa_camera_dev *pcdev,
+				    struct pxa_buffer *buf)
+{
+	buf->active_dma = DMA_Y;
+	if (buf->nb_planes == 3)
+		buf->active_dma |= DMA_U | DMA_V;
+}
+
+/**
+ * pxa_dma_start_channels - start DMA channel for active buffer
+ * @pcdev: pxa camera device
+ *
+ * Initialize DMA channels to the beginning of the active video buffer, and
+ * start these channels.
+ */
+static void pxa_dma_start_channels(struct pxa_camera_dev *pcdev)
+{
+	int i;
+	struct pxa_buffer *active;
+
+	active = pcdev->active;
+
+	for (i = 0; i < pcdev->channels; i++) {
+		dev_dbg(pcdev_to_dev(pcdev),
+			"%s (channel=%d)\n", __func__, i);
+		dma_async_issue_pending(pcdev->dma_chans[i]);
+	}
+}
+
+static void pxa_dma_stop_channels(struct pxa_camera_dev *pcdev)
+{
+	int i;
+
+	for (i = 0; i < pcdev->channels; i++) {
+		dev_dbg(pcdev_to_dev(pcdev),
+			"%s (channel=%d)\n", __func__, i);
+		dmaengine_terminate_all(pcdev->dma_chans[i]);
+	}
+}
+
+static void pxa_dma_add_tail_buf(struct pxa_camera_dev *pcdev,
+				 struct pxa_buffer *buf)
+{
+	int i;
+
+	for (i = 0; i < pcdev->channels; i++) {
+		buf->cookie[i] = dmaengine_submit(buf->descs[i]);
+		dev_dbg(pcdev_to_dev(pcdev),
+			"%s (channel=%d) : submit vb=%p cookie=%d\n",
+			__func__, i, buf, buf->descs[i]->cookie);
+	}
+}
+
+/**
+ * pxa_camera_start_capture - start video capturing
+ * @pcdev: camera device
+ *
+ * Launch capturing. DMA channels should not be active yet. They should get
+ * activated at the end of frame interrupt, to capture only whole frames, and
+ * never begin the capture of a partial frame.
+ */
+static void pxa_camera_start_capture(struct pxa_camera_dev *pcdev)
+{
+	unsigned long cicr0;
+
+	dev_dbg(pcdev_to_dev(pcdev), "%s\n", __func__);
+	__raw_writel(__raw_readl(pcdev->base + CISR), pcdev->base + CISR);
+	/* Enable End-Of-Frame Interrupt */
+	cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_ENB;
+	cicr0 &= ~CICR0_EOFM;
+	__raw_writel(cicr0, pcdev->base + CICR0);
+}
+
+static void pxa_camera_stop_capture(struct pxa_camera_dev *pcdev)
+{
+	unsigned long cicr0;
+
+	pxa_dma_stop_channels(pcdev);
+
+	cicr0 = __raw_readl(pcdev->base + CICR0) & ~CICR0_ENB;
+	__raw_writel(cicr0, pcdev->base + CICR0);
+
+	pcdev->active = NULL;
+	dev_dbg(pcdev_to_dev(pcdev), "%s\n", __func__);
+}
+
+static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
+			      struct pxa_buffer *buf,
+			      enum vb2_buffer_state state)
+{
+	struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	/* _init is used to debug races, see comment in pxa_camera_reqbufs() */
+	list_del_init(&buf->queue);
+	vb->timestamp = ktime_get_ns();
+	vbuf->sequence = pcdev->buf_sequence++;
+	vbuf->field = V4L2_FIELD_NONE;
+	vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+	dev_dbg(pcdev_to_dev(pcdev), "%s dequeud buffer (buf=0x%p)\n",
+		__func__, buf);
+
+	if (list_empty(&pcdev->capture)) {
+		pxa_camera_stop_capture(pcdev);
+		return;
+	}
+
+	pcdev->active = list_entry(pcdev->capture.next,
+				   struct pxa_buffer, queue);
+}
+
+/**
+ * pxa_camera_check_link_miss - check missed DMA linking
+ * @pcdev: camera device
+ *
+ * The DMA chaining is done with DMA running. This means a tiny temporal window
+ * remains, where a buffer is queued on the chain, while the chain is already
+ * stopped. This means the tailed buffer would never be transferred by DMA.
+ * This function restarts the capture for this corner case, where :
+ *  - DADR() == DADDR_STOP
+ *  - a videobuffer is queued on the pcdev->capture list
+ *
+ * Please check the "DMA hot chaining timeslice issue" in
+ *   Documentation/video4linux/pxa_camera.txt
+ *
+ * Context: should only be called within the dma irq handler
+ */
+static void pxa_camera_check_link_miss(struct pxa_camera_dev *pcdev,
+				       dma_cookie_t last_submitted,
+				       dma_cookie_t last_issued)
+{
+	bool is_dma_stopped = last_submitted != last_issued;
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		"%s : top queued buffer=%p, is_dma_stopped=%d\n",
+		__func__, pcdev->active, is_dma_stopped);
+
+	if (pcdev->active && is_dma_stopped)
+		pxa_camera_start_capture(pcdev);
+}
+
+static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
+			       enum pxa_camera_active_dma act_dma)
+{
+	struct pxa_buffer *buf, *last_buf;
+	unsigned long flags;
+	u32 camera_status, overrun;
+	int chan;
+	enum dma_status last_status;
+	dma_cookie_t last_issued;
+
+	spin_lock_irqsave(&pcdev->lock, flags);
+
+	camera_status = __raw_readl(pcdev->base + CISR);
+	dev_dbg(pcdev_to_dev(pcdev), "camera dma irq, cisr=0x%x dma=%d\n",
+		camera_status, act_dma);
+	overrun = CISR_IFO_0;
+	if (pcdev->channels == 3)
+		overrun |= CISR_IFO_1 | CISR_IFO_2;
+
+	/*
+	 * pcdev->active should not be NULL in DMA irq handler.
+	 *
+	 * But there is one corner case : if capture was stopped due to an
+	 * overrun of channel 1, and at that same channel 2 was completed.
+	 *
+	 * When handling the overrun in DMA irq for channel 1, we'll stop the
+	 * capture and restart it (and thus set pcdev->active to NULL). But the
+	 * DMA irq handler will already be pending for channel 2. So on entering
+	 * the DMA irq handler for channel 2 there will be no active buffer, yet
+	 * that is normal.
+	 */
+	if (!pcdev->active)
+		goto out;
+
+	buf = pcdev->active;
+	WARN_ON(buf->inwork || list_empty(&buf->queue));
+
+	/*
+	 * It's normal if the last frame creates an overrun, as there
+	 * are no more DMA descriptors to fetch from QCI fifos
+	 */
+	switch (act_dma) {
+	case DMA_U:
+		chan = 1;
+		break;
+	case DMA_V:
+		chan = 2;
+		break;
+	default:
+		chan = 0;
+		break;
+	}
+	last_buf = list_entry(pcdev->capture.prev,
+			      struct pxa_buffer, queue);
+	last_status = dma_async_is_tx_complete(pcdev->dma_chans[chan],
+					       last_buf->cookie[chan],
+					       NULL, &last_issued);
+	if (camera_status & overrun &&
+	    last_status != DMA_COMPLETE) {
+		dev_dbg(pcdev_to_dev(pcdev), "FIFO overrun! CISR: %x\n",
+			camera_status);
+		pxa_camera_stop_capture(pcdev);
+		list_for_each_entry(buf, &pcdev->capture, queue)
+			pxa_dma_add_tail_buf(pcdev, buf);
+		pxa_camera_start_capture(pcdev);
+		goto out;
+	}
+	buf->active_dma &= ~act_dma;
+	if (!buf->active_dma) {
+		pxa_camera_wakeup(pcdev, buf, VB2_BUF_STATE_DONE);
+		pxa_camera_check_link_miss(pcdev, last_buf->cookie[chan],
+					   last_issued);
+	}
+
+out:
+	spin_unlock_irqrestore(&pcdev->lock, flags);
+}
+
+static u32 mclk_get_divisor(struct platform_device *pdev,
+			    struct pxa_camera_dev *pcdev)
+{
+	unsigned long mclk = pcdev->mclk;
+	u32 div;
+	unsigned long lcdclk;
+
+	lcdclk = clk_get_rate(pcdev->clk);
+	pcdev->ciclk = lcdclk;
+
+	/* mclk <= ciclk / 4 (27.4.2) */
+	if (mclk > lcdclk / 4) {
+		mclk = lcdclk / 4;
+		dev_warn(pcdev_to_dev(pcdev),
+			 "Limiting master clock to %lu\n", mclk);
+	}
+
+	/* We verify mclk != 0, so if anyone breaks it, here comes their Oops */
+	div = (lcdclk + 2 * mclk - 1) / (2 * mclk) - 1;
+
+	/* If we're not supplying MCLK, leave it at 0 */
+	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
+		pcdev->mclk = lcdclk / (2 * (div + 1));
+
+	dev_dbg(pcdev_to_dev(pcdev), "LCD clock %luHz, target freq %luHz, divisor %u\n",
+		lcdclk, mclk, div);
+
+	return div;
+}
+
+static void recalculate_fifo_timeout(struct pxa_camera_dev *pcdev,
+				     unsigned long pclk)
+{
+	/* We want a timeout > 1 pixel time, not ">=" */
+	u32 ciclk_per_pixel = pcdev->ciclk / pclk + 1;
+
+	__raw_writel(ciclk_per_pixel, pcdev->base + CITOR);
+}
+
+static void pxa_camera_activate(struct pxa_camera_dev *pcdev)
+{
+	u32 cicr4 = 0;
+
+	/* disable all interrupts */
+	__raw_writel(0x3ff, pcdev->base + CICR0);
+
+	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
+		cicr4 |= CICR4_PCLK_EN;
+	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
+		cicr4 |= CICR4_MCLK_EN;
+	if (pcdev->platform_flags & PXA_CAMERA_PCP)
+		cicr4 |= CICR4_PCP;
+	if (pcdev->platform_flags & PXA_CAMERA_HSP)
+		cicr4 |= CICR4_HSP;
+	if (pcdev->platform_flags & PXA_CAMERA_VSP)
+		cicr4 |= CICR4_VSP;
+
+	__raw_writel(pcdev->mclk_divisor | cicr4, pcdev->base + CICR4);
+
+	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
+		/* Initialise the timeout under the assumption pclk = mclk */
+		recalculate_fifo_timeout(pcdev, pcdev->mclk);
+	else
+		/* "Safe default" - 13MHz */
+		recalculate_fifo_timeout(pcdev, 13000000);
+
+	clk_prepare_enable(pcdev->clk);
+}
+
+static void pxa_camera_deactivate(struct pxa_camera_dev *pcdev)
+{
+	clk_disable_unprepare(pcdev->clk);
+}
+
+static void pxa_camera_eof(unsigned long arg)
+{
+	struct pxa_camera_dev *pcdev = (struct pxa_camera_dev *)arg;
+	unsigned long cifr;
+	struct pxa_buffer *buf;
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		"Camera interrupt status 0x%x\n",
+		__raw_readl(pcdev->base + CISR));
+
+	/* Reset the FIFOs */
+	cifr = __raw_readl(pcdev->base + CIFR) | CIFR_RESET_F;
+	__raw_writel(cifr, pcdev->base + CIFR);
+
+	pcdev->active = list_first_entry(&pcdev->capture,
+					 struct pxa_buffer, queue);
+	buf = pcdev->active;
+	pxa_videobuf_set_actdma(pcdev, buf);
+
+	pxa_dma_start_channels(pcdev);
+}
+
+static irqreturn_t pxa_camera_irq(int irq, void *data)
+{
+	struct pxa_camera_dev *pcdev = data;
+	unsigned long status, cicr0;
+
+	status = __raw_readl(pcdev->base + CISR);
+	dev_dbg(pcdev_to_dev(pcdev),
+		"Camera interrupt status 0x%lx\n", status);
+
+	if (!status)
+		return IRQ_NONE;
+
+	__raw_writel(status, pcdev->base + CISR);
+
+	if (status & CISR_EOF) {
+		cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_EOFM;
+		__raw_writel(cicr0, pcdev->base + CICR0);
+		tasklet_schedule(&pcdev->task_eof);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int test_platform_param(struct pxa_camera_dev *pcdev,
+			       unsigned char buswidth, unsigned long *flags)
+{
+	/*
+	 * Platform specified synchronization and pixel clock polarities are
+	 * only a recommendation and are only used during probing. The PXA270
+	 * quick capture interface supports both.
+	 */
+	*flags = (pcdev->platform_flags & PXA_CAMERA_MASTER ?
+		  V4L2_MBUS_MASTER : V4L2_MBUS_SLAVE) |
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_HSYNC_ACTIVE_LOW |
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_VSYNC_ACTIVE_LOW |
+		V4L2_MBUS_DATA_ACTIVE_HIGH |
+		V4L2_MBUS_PCLK_SAMPLE_RISING |
+		V4L2_MBUS_PCLK_SAMPLE_FALLING;
+
+	/* If requested data width is supported by the platform, use it */
+	if ((1 << (buswidth - 1)) & pcdev->width_flags)
+		return 0;
+
+	return -EINVAL;
+}
+
+static void pxa_camera_setup_cicr(struct pxa_camera_dev *pcdev,
+				  unsigned long flags, __u32 pixfmt)
+{
+	unsigned long dw, bpp;
+	u32 cicr0, cicr1, cicr2, cicr3, cicr4 = 0, y_skip_top;
+	int ret = sensor_call(pcdev, sensor, g_skip_top_lines, &y_skip_top);
+
+	if (ret < 0)
+		y_skip_top = 0;
+
+	/*
+	 * Datawidth is now guaranteed to be equal to one of the three values.
+	 * We fix bit-per-pixel equal to data-width...
+	 */
+	switch (pcdev->current_fmt->host_fmt->bits_per_sample) {
+	case 10:
+		dw = 4;
+		bpp = 0x40;
+		break;
+	case 9:
+		dw = 3;
+		bpp = 0x20;
+		break;
+	default:
+		/*
+		 * Actually it can only be 8 now,
+		 * default is just to silence compiler warnings
+		 */
+	case 8:
+		dw = 2;
+		bpp = 0;
+	}
+
+	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
+		cicr4 |= CICR4_PCLK_EN;
+	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
+		cicr4 |= CICR4_MCLK_EN;
+	if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+		cicr4 |= CICR4_PCP;
+	if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+		cicr4 |= CICR4_HSP;
+	if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+		cicr4 |= CICR4_VSP;
+
+	cicr0 = __raw_readl(pcdev->base + CICR0);
+	if (cicr0 & CICR0_ENB)
+		__raw_writel(cicr0 & ~CICR0_ENB, pcdev->base + CICR0);
+
+	cicr1 = CICR1_PPL_VAL(pcdev->current_pix.width - 1) | bpp | dw;
+
+	switch (pixfmt) {
+	case V4L2_PIX_FMT_YUV422P:
+		pcdev->channels = 3;
+		cicr1 |= CICR1_YCBCR_F;
+		/*
+		 * Normally, pxa bus wants as input UYVY format. We allow all
+		 * reorderings of the YUV422 format, as no processing is done,
+		 * and the YUV stream is just passed through without any
+		 * transformation. Note that UYVY is the only format that
+		 * should be used if pxa framebuffer Overlay2 is used.
+		 */
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_VYUY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+		cicr1 |= CICR1_COLOR_SP_VAL(2);
+		break;
+	case V4L2_PIX_FMT_RGB555:
+		cicr1 |= CICR1_RGB_BPP_VAL(1) | CICR1_RGBT_CONV_VAL(2) |
+			CICR1_TBIT | CICR1_COLOR_SP_VAL(1);
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		cicr1 |= CICR1_COLOR_SP_VAL(1) | CICR1_RGB_BPP_VAL(2);
+		break;
+	}
+
+	cicr2 = 0;
+	cicr3 = CICR3_LPF_VAL(pcdev->current_pix.height - 1) |
+		CICR3_BFW_VAL(min((u32)255, y_skip_top));
+	cicr4 |= pcdev->mclk_divisor;
+
+	__raw_writel(cicr1, pcdev->base + CICR1);
+	__raw_writel(cicr2, pcdev->base + CICR2);
+	__raw_writel(cicr3, pcdev->base + CICR3);
+	__raw_writel(cicr4, pcdev->base + CICR4);
+
+	/* CIF interrupts are not used, only DMA */
+	cicr0 = (cicr0 & CICR0_ENB) | (pcdev->platform_flags & PXA_CAMERA_MASTER ?
+		CICR0_SIM_MP : (CICR0_SL_CAP_EN | CICR0_SIM_SP));
+	cicr0 |= CICR0_DMAEN | CICR0_IRQ_MASK;
+	__raw_writel(cicr0, pcdev->base + CICR0);
+}
+
+/*
+ * Videobuf2 section
+ */
+static void pxa_buffer_cleanup(struct pxa_buffer *buf)
+{
+	int i;
+
+	for (i = 0; i < 3 && buf->descs[i]; i++) {
+		dmaengine_desc_free(buf->descs[i]);
+		kfree(buf->sg[i]);
+		buf->descs[i] = NULL;
+		buf->sg[i] = NULL;
+		buf->sg_len[i] = 0;
+		buf->plane_sizes[i] = 0;
+	}
+	buf->nb_planes = 0;
+}
+
+static int pxa_buffer_init(struct pxa_camera_dev *pcdev,
+			   struct pxa_buffer *buf)
+{
+	struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
+	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
+	int nb_channels = pcdev->channels;
+	int i, ret = 0;
+	unsigned long size = vb2_plane_size(vb, 0);
+
+	switch (nb_channels) {
+	case 1:
+		buf->plane_sizes[0] = size;
+		break;
+	case 3:
+		buf->plane_sizes[0] = size / 2;
+		buf->plane_sizes[1] = size / 4;
+		buf->plane_sizes[2] = size / 4;
+		break;
+	default:
+		return -EINVAL;
+	};
+	buf->nb_planes = nb_channels;
+
+	ret = sg_split(sgt->sgl, sgt->nents, 0, nb_channels,
+		       buf->plane_sizes, buf->sg, buf->sg_len, GFP_KERNEL);
+	if (ret < 0) {
+		dev_err(pcdev_to_dev(pcdev),
+			"sg_split failed: %d\n", ret);
+		return ret;
+	}
+	for (i = 0; i < nb_channels; i++) {
+		ret = pxa_init_dma_channel(pcdev, buf, i,
+					   buf->sg[i], buf->sg_len[i]);
+		if (ret) {
+			pxa_buffer_cleanup(buf);
+			return ret;
+		}
+	}
+	INIT_LIST_HEAD(&buf->queue);
+
+	return ret;
+}
+
+static void pxac_vb2_cleanup(struct vb2_buffer *vb)
+{
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(vb=%p)\n", __func__, vb);
+	pxa_buffer_cleanup(buf);
+}
+
+static void pxac_vb2_queue(struct vb2_buffer *vb)
+{
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(vb=%p) nb_channels=%d size=%lu active=%p\n",
+		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0),
+		pcdev->active);
+
+	list_add_tail(&buf->queue, &pcdev->capture);
+
+	pxa_dma_add_tail_buf(pcdev, buf);
+}
+
+/*
+ * Please check the DMA prepared buffer structure in :
+ *   Documentation/video4linux/pxa_camera.txt
+ * Please check also in pxa_camera_check_link_miss() to understand why DMA chain
+ * modification while DMA chain is running will work anyway.
+ */
+static int pxac_vb2_prepare(struct vb2_buffer *vb)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+	int ret = 0;
+
+	switch (pcdev->channels) {
+	case 1:
+	case 3:
+		vb2_set_plane_payload(vb, 0, pcdev->current_pix.sizeimage);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s (vb=%p) nb_channels=%d size=%lu\n",
+		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0));
+
+	WARN_ON(!pcdev->current_fmt);
+
+#ifdef DEBUG
+	/*
+	 * This can be useful if you want to see if we actually fill
+	 * the buffer with something
+	 */
+	for (i = 0; i < vb->num_planes; i++)
+		memset((void *)vb2_plane_vaddr(vb, i),
+		       0xaa, vb2_get_plane_payload(vb, i));
+#endif
+
+	/*
+	 * I think, in buf_prepare you only have to protect global data,
+	 * the actual buffer is yours
+	 */
+	buf->inwork = 0;
+	pxa_videobuf_set_actdma(pcdev, buf);
+
+	return ret;
+}
+
+static int pxac_vb2_init(struct vb2_buffer *vb)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
+	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(nb_channels=%d)\n",
+		__func__, pcdev->channels);
+
+	return pxa_buffer_init(pcdev, buf);
+}
+
+static int pxac_vb2_queue_setup(struct vb2_queue *vq,
+				unsigned int *nbufs,
+				unsigned int *num_planes, unsigned int sizes[],
+				struct device *alloc_devs[])
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
+	int size = pcdev->current_pix.sizeimage;
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "%s(vq=%p nbufs=%d num_planes=%d size=%d)\n",
+		__func__, vq, *nbufs, *num_planes, size);
+	/*
+	 * Called from VIDIOC_REQBUFS or in compatibility mode For YUV422P
+	 * format, even if there are 3 planes Y, U and V, we reply there is only
+	 * one plane, containing Y, U and V data, one after the other.
+	 */
+	if (*num_planes)
+		return sizes[0] < size ? -EINVAL : 0;
+
+	*num_planes = 1;
+	switch (pcdev->channels) {
+	case 1:
+	case 3:
+		sizes[0] = size;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (!*nbufs)
+		*nbufs = 1;
+
+	return 0;
+}
+
+static int pxac_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
+
+	dev_dbg(pcdev_to_dev(pcdev), "%s(count=%d) active=%p\n",
+		__func__, count, pcdev->active);
+
+	pcdev->buf_sequence = 0;
+	if (!pcdev->active)
+		pxa_camera_start_capture(pcdev);
+
+	return 0;
+}
+
+static void pxac_vb2_stop_streaming(struct vb2_queue *vq)
+{
+	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
+	struct pxa_buffer *buf, *tmp;
+
+	dev_dbg(pcdev_to_dev(pcdev), "%s active=%p\n",
+		__func__, pcdev->active);
+	pxa_camera_stop_capture(pcdev);
+
+	list_for_each_entry_safe(buf, tmp, &pcdev->capture, queue)
+		pxa_camera_wakeup(pcdev, buf, VB2_BUF_STATE_ERROR);
+}
+
+static struct vb2_ops pxac_vb2_ops = {
+	.queue_setup		= pxac_vb2_queue_setup,
+	.buf_init		= pxac_vb2_init,
+	.buf_prepare		= pxac_vb2_prepare,
+	.buf_queue		= pxac_vb2_queue,
+	.buf_cleanup		= pxac_vb2_cleanup,
+	.start_streaming	= pxac_vb2_start_streaming,
+	.stop_streaming		= pxac_vb2_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+static int pxa_camera_init_videobuf2(struct pxa_camera_dev *pcdev)
+{
+	int ret;
+	struct vb2_queue *vq = &pcdev->vb2_vq;
+
+	memset(vq, 0, sizeof(*vq));
+	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	vq->drv_priv = pcdev;
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vq->buf_struct_size = sizeof(struct pxa_buffer);
+	vq->dev = pcdev->v4l2_dev.dev;
+
+	vq->ops = &pxac_vb2_ops;
+	vq->mem_ops = &vb2_dma_sg_memops;
+	vq->lock = &pcdev->mlock;
+
+	ret = vb2_queue_init(vq);
+	dev_dbg(pcdev_to_dev(pcdev),
+		 "vb2_queue_init(vq=%p): %d\n", vq, ret);
+
+	return ret;
+}
+
+/*
+ * Video ioctls section
+ */
+static int pxa_camera_set_bus_param(struct pxa_camera_dev *pcdev)
+{
+	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	u32 pixfmt = pcdev->current_fmt->host_fmt->fourcc;
+	unsigned long bus_flags, common_flags;
+	int ret;
+
+	ret = test_platform_param(pcdev,
+				  pcdev->current_fmt->host_fmt->bits_per_sample,
+				  &bus_flags);
+	if (ret < 0)
+		return ret;
+
+	ret = sensor_call(pcdev, video, g_mbus_config, &cfg);
+	if (!ret) {
+		common_flags = soc_mbus_config_compatible(&cfg,
+							  bus_flags);
+		if (!common_flags) {
+			dev_warn(pcdev_to_dev(pcdev),
+				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
+				 cfg.flags, bus_flags);
+			return -EINVAL;
+		}
+	} else if (ret != -ENOIOCTLCMD) {
+		return ret;
+	} else {
+		common_flags = bus_flags;
+	}
+
+	pcdev->channels = 1;
+
+	/* Make choises, based on platform preferences */
+	if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
+		if (pcdev->platform_flags & PXA_CAMERA_HSP)
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
+	}
+
+	if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
+		if (pcdev->platform_flags & PXA_CAMERA_VSP)
+			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
+	}
+
+	if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
+	    (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
+		if (pcdev->platform_flags & PXA_CAMERA_PCP)
+			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
+		else
+			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
+	}
+
+	cfg.flags = common_flags;
+	ret = sensor_call(pcdev, video, s_mbus_config, &cfg);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		dev_dbg(pcdev_to_dev(pcdev),
+			"camera s_mbus_config(0x%lx) returned %d\n",
+			common_flags, ret);
+		return ret;
+	}
+
+	pxa_camera_setup_cicr(pcdev, common_flags, pixfmt);
+
+	return 0;
+}
+
+static int pxa_camera_try_bus_param(struct pxa_camera_dev *pcdev,
+				    unsigned char buswidth)
+{
+	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	unsigned long bus_flags, common_flags;
+	int ret = test_platform_param(pcdev, buswidth, &bus_flags);
+
+	if (ret < 0)
+		return ret;
+
+	ret = sensor_call(pcdev, video, g_mbus_config, &cfg);
+	if (!ret) {
+		common_flags = soc_mbus_config_compatible(&cfg,
+							  bus_flags);
+		if (!common_flags) {
+			dev_warn(pcdev_to_dev(pcdev),
+				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
+				 cfg.flags, bus_flags);
+			return -EINVAL;
+		}
+	} else if (ret == -ENOIOCTLCMD) {
+		ret = 0;
+	}
+
+	return ret;
+}
+
+static const struct soc_mbus_pixelfmt pxa_camera_formats[] = {
+	{
+		.fourcc			= V4L2_PIX_FMT_YUV422P,
+		.name			= "Planar YUV422 16 bit",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_U_V,
+	},
+};
+
+/* This will be corrected as we get more formats */
+static bool pxa_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
+{
+	return	fmt->packing == SOC_MBUS_PACKING_NONE ||
+		(fmt->bits_per_sample == 8 &&
+		 fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
+		(fmt->bits_per_sample > 8 &&
+		 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
+}
+
+static int pxa_camera_get_formats(struct v4l2_device *v4l2_dev,
+				  unsigned int idx,
+				  struct soc_camera_format_xlate *xlate)
+{
+	struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(v4l2_dev);
+	int formats = 0, ret;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
+	const struct soc_mbus_pixelfmt *fmt;
+
+	ret = sensor_call(pcdev, pad, enum_mbus_code, NULL, &code);
+	if (ret < 0)
+		/* No more formats */
+		return 0;
+
+	fmt = soc_mbus_get_fmtdesc(code.code);
+	if (!fmt) {
+		dev_err(pcdev_to_dev(pcdev),
+			"Invalid format code #%u: %d\n", idx, code.code);
+		return 0;
+	}
+
+	/* This also checks support for the requested bits-per-sample */
+	ret = pxa_camera_try_bus_param(pcdev, fmt->bits_per_sample);
+	if (ret < 0)
+		return 0;
+
+	switch (code.code) {
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		formats++;
+		if (xlate) {
+			xlate->host_fmt	= &pxa_camera_formats[0];
+			xlate->code	= code.code;
+			xlate++;
+			dev_dbg(pcdev_to_dev(pcdev),
+				"Providing format %s using code %d\n",
+				pxa_camera_formats[0].name, code.code);
+		}
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
+		if (xlate)
+			dev_dbg(pcdev_to_dev(pcdev),
+				"Providing format %s packed\n",
+				fmt->name);
+		break;
+	default:
+		if (!pxa_camera_packing_supported(fmt))
+			return 0;
+		if (xlate)
+			dev_dbg(pcdev_to_dev(pcdev),
+				"Providing format %s in pass-through mode\n",
+				fmt->name);
+	}
+
+	/* Generic pass-through */
+	formats++;
+	if (xlate) {
+		xlate->host_fmt	= fmt;
+		xlate->code	= code.code;
+		xlate++;
+	}
+
+	return formats;
+}
+
+static int pxa_camera_build_formats(struct pxa_camera_dev *pcdev)
+{
+	struct soc_camera_format_xlate *xlate;
+
+	xlate = soc_mbus_build_fmts_xlate(&pcdev->v4l2_dev, pcdev->sensor,
+					  pxa_camera_get_formats);
+	if (IS_ERR(xlate))
+		return PTR_ERR(xlate);
+
+	pcdev->user_formats = xlate;
+	return 0;
+}
+
+static void pxa_camera_destroy_formats(struct pxa_camera_dev *pcdev)
+{
+	kfree(pcdev->user_formats);
+}
+
+static int pxa_camera_check_frame(u32 width, u32 height)
+{
+	/* limit to pxa hardware capabilities */
+	return height < 32 || height > 2048 || width < 48 || width > 2048 ||
+		(width & 0x01);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int pxac_vidioc_g_register(struct file *file, void *priv,
+				  struct v4l2_dbg_register *reg)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(file);
+
+	if (reg->reg > CIBR2)
+		return -ERANGE;
+
+	reg->val = __raw_readl(pcdev->base + reg->reg);
+	reg->size = sizeof(__u32);
+	return 0;
+}
+
+static int pxac_vidioc_s_register(struct file *file, void *priv,
+				  const struct v4l2_dbg_register *reg)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(file);
+
+	if (reg->reg > CIBR2)
+		return -ERANGE;
+	if (reg->size != sizeof(__u32))
+		return -EINVAL;
+	__raw_writel(reg->val, pcdev->base + reg->reg);
+	return 0;
+}
+#endif
+
+static int pxac_vidioc_enum_fmt_vid_cap(struct file *filp, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	const struct soc_mbus_pixelfmt *format;
+	unsigned int idx;
+
+	for (idx = 0; pcdev->user_formats[idx].code; idx++);
+	if (f->index >= idx)
+		return -EINVAL;
+
+	format = pcdev->user_formats[f->index].host_fmt;
+	f->pixelformat = format->fourcc;
+	return 0;
+}
+
+static int pxac_vidioc_g_fmt_vid_cap(struct file *filp, void *priv,
+				    struct v4l2_format *f)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	pix->width		= pcdev->current_pix.width;
+	pix->height		= pcdev->current_pix.height;
+	pix->bytesperline	= pcdev->current_pix.bytesperline;
+	pix->sizeimage		= pcdev->current_pix.sizeimage;
+	pix->field		= pcdev->current_pix.field;
+	pix->pixelformat	= pcdev->current_fmt->host_fmt->fourcc;
+	pix->colorspace		= pcdev->current_pix.colorspace;
+	dev_dbg(pcdev_to_dev(pcdev), "current_fmt->fourcc: 0x%08x\n",
+		pcdev->current_fmt->host_fmt->fourcc);
+	return 0;
+}
+
+static int pxac_vidioc_try_fmt_vid_cap(struct file *filp, void *priv,
+				      struct v4l2_format *f)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	__u32 pixfmt = pix->pixelformat;
+	int ret;
+
+	xlate = soc_mbus_xlate_by_fourcc(pcdev->user_formats, pixfmt);
+	if (!xlate) {
+		dev_warn(pcdev_to_dev(pcdev), "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+
+	/*
+	 * Limit to pxa hardware capabilities.  YUV422P planar format requires
+	 * images size to be a multiple of 16 bytes.  If not, zeros will be
+	 * inserted between Y and U planes, and U and V planes, which violates
+	 * the YUV422P standard.
+	 */
+	v4l_bound_align_image(&pix->width, 48, 2048, 1,
+			      &pix->height, 32, 2048, 0,
+			      pixfmt == V4L2_PIX_FMT_YUV422P ? 4 : 0);
+
+	v4l2_fill_mbus_format(mf, pix, xlate->code);
+	ret = sensor_call(pcdev, pad, set_fmt, &pad_cfg, &format);
+	if (ret < 0)
+		return ret;
+
+	v4l2_fill_pix_format(pix, mf);
+
+	/* Only progressive video supported so far */
+	switch (mf->field) {
+	case V4L2_FIELD_ANY:
+	case V4L2_FIELD_NONE:
+		pix->field = V4L2_FIELD_NONE;
+		break;
+	default:
+		/* TODO: support interlaced at least in pass-through mode */
+		dev_err(pcdev_to_dev(pcdev), "Field type %d unsupported.\n",
+			mf->field);
+		return -EINVAL;
+	}
+
+	ret = soc_mbus_bytes_per_line(pix->width, xlate->host_fmt);
+	if (ret < 0)
+		return ret;
+
+	pix->bytesperline = ret;
+	ret = soc_mbus_image_size(xlate->host_fmt, pix->bytesperline,
+				  pix->height);
+	if (ret < 0)
+		return ret;
+
+	pix->sizeimage = ret;
+	return 0;
+}
+
+static int pxac_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
+				    struct v4l2_format *f)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	unsigned long flags;
+	int ret, is_busy;
+
+	dev_dbg(pcdev_to_dev(pcdev),
+		"s_fmt_vid_cap(pix=%dx%d:%x)\n",
+		pix->width, pix->height, pix->pixelformat);
+
+	spin_lock_irqsave(&pcdev->lock, flags);
+	is_busy = pcdev->active || vb2_is_busy(&pcdev->vb2_vq);
+	spin_unlock_irqrestore(&pcdev->lock, flags);
+
+	if (is_busy)
+		return -EBUSY;
+
+	ret = pxac_vidioc_try_fmt_vid_cap(filp, priv, f);
+	if (ret)
+		return ret;
+
+	xlate = soc_mbus_xlate_by_fourcc(pcdev->user_formats,
+					 pix->pixelformat);
+	v4l2_fill_mbus_format(&format.format, pix, xlate->code);
+	ret = sensor_call(pcdev, pad, set_fmt, NULL, &format);
+	if (ret < 0) {
+		dev_warn(pcdev_to_dev(pcdev),
+			 "Failed to configure for format %x\n",
+			 pix->pixelformat);
+	} else if (pxa_camera_check_frame(pix->width, pix->height)) {
+		dev_warn(pcdev_to_dev(pcdev),
+			 "Camera driver produced an unsupported frame %dx%d\n",
+			 pix->width, pix->height);
+		return -EINVAL;
+	}
+
+	pcdev->current_fmt = xlate;
+	pcdev->current_pix = *pix;
+
+	ret = pxa_camera_set_bus_param(pcdev);
+	return ret;
+}
+
+static int pxac_vidioc_querycap(struct file *file, void *priv,
+				struct v4l2_capability *cap)
+{
+	strlcpy(cap->bus_info, "platform:pxa-camera", sizeof(cap->bus_info));
+	strlcpy(cap->driver, PXA_CAM_DRV_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, pxa_cam_driver_description, sizeof(cap->card));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int pxac_vidioc_enum_input(struct file *file, void *priv,
+				  struct v4l2_input *i)
+{
+	if (i->index > 0)
+		return -EINVAL;
+
+	i->type = V4L2_INPUT_TYPE_CAMERA;
+	strlcpy(i->name, "Camera", sizeof(i->name));
+
+	return 0;
+}
+
+static int pxac_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+
+	return 0;
+}
+
+static int pxac_vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	if (i > 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int pxac_fops_camera_open(struct file *filp)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	int ret;
+
+	mutex_lock(&pcdev->mlock);
+	ret = v4l2_fh_open(filp);
+	if (ret < 0)
+		goto out;
+
+	ret = sensor_call(pcdev, core, s_power, 1);
+	if (ret)
+		v4l2_fh_release(filp);
+out:
+	mutex_unlock(&pcdev->mlock);
+	return ret;
+}
+
+static int pxac_fops_camera_release(struct file *filp)
+{
+	struct pxa_camera_dev *pcdev = video_drvdata(filp);
+	int ret;
+
+	ret = vb2_fop_release(filp);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&pcdev->mlock);
+	ret = sensor_call(pcdev, core, s_power, 0);
+	mutex_unlock(&pcdev->mlock);
+
+	return ret;
+}
+
+static const struct v4l2_file_operations pxa_camera_fops = {
+	.owner		= THIS_MODULE,
+	.open		= pxac_fops_camera_open,
+	.release	= pxac_fops_camera_release,
+	.read		= vb2_fop_read,
+	.poll		= vb2_fop_poll,
+	.mmap		= vb2_fop_mmap,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops pxa_camera_ioctl_ops = {
+	.vidioc_querycap		= pxac_vidioc_querycap,
+
+	.vidioc_enum_input		= pxac_vidioc_enum_input,
+	.vidioc_g_input			= pxac_vidioc_g_input,
+	.vidioc_s_input			= pxac_vidioc_s_input,
+
+	.vidioc_enum_fmt_vid_cap	= pxac_vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		= pxac_vidioc_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= pxac_vidioc_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= pxac_vidioc_try_fmt_vid_cap,
+
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register		= pxac_vidioc_g_register,
+	.vidioc_s_register		= pxac_vidioc_s_register,
+#endif
+};
+
+static struct v4l2_clk_ops pxa_camera_mclk_ops = {
+};
+
+static const struct video_device pxa_camera_videodev_template = {
+	.name = "pxa-camera",
+	.minor = -1,
+	.fops = &pxa_camera_fops,
+	.ioctl_ops = &pxa_camera_ioctl_ops,
+	.release = video_device_release_empty,
+	.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING,
+};
+
+static int pxa_camera_sensor_bound(struct v4l2_async_notifier *notifier,
+		     struct v4l2_subdev *subdev,
+		     struct v4l2_async_subdev *asd)
+{
+	int err;
+	struct v4l2_device *v4l2_dev = notifier->v4l2_dev;
+	struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(v4l2_dev);
+	struct video_device *vdev = &pcdev->vdev;
+	struct v4l2_pix_format *pix = &pcdev->current_pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+
+	dev_info(pcdev_to_dev(pcdev), "%s(): trying to bind a device\n",
+		 __func__);
+	mutex_lock(&pcdev->mlock);
+	*vdev = pxa_camera_videodev_template;
+	vdev->v4l2_dev = v4l2_dev;
+	vdev->lock = &pcdev->mlock;
+	pcdev->sensor = subdev;
+	pcdev->vdev.queue = &pcdev->vb2_vq;
+	pcdev->vdev.v4l2_dev = &pcdev->v4l2_dev;
+	pcdev->vdev.ctrl_handler = subdev->ctrl_handler;
+	video_set_drvdata(&pcdev->vdev, pcdev);
+
+	err = pxa_camera_build_formats(pcdev);
+	if (err) {
+		dev_err(pcdev_to_dev(pcdev), "building formats failed: %d\n",
+			err);
+		goto out;
+	}
+
+	pcdev->current_fmt = pcdev->user_formats;
+	pix->field = V4L2_FIELD_NONE;
+	pix->width = DEFAULT_WIDTH;
+	pix->height = DEFAULT_HEIGHT;
+	pix->bytesperline =
+		soc_mbus_bytes_per_line(pix->width,
+					pcdev->current_fmt->host_fmt);
+	pix->sizeimage =
+		soc_mbus_image_size(pcdev->current_fmt->host_fmt,
+				    pix->bytesperline, pix->height);
+	pix->pixelformat = pcdev->current_fmt->host_fmt->fourcc;
+	v4l2_fill_mbus_format(mf, pix, pcdev->current_fmt->code);
+	err = sensor_call(pcdev, pad, set_fmt, NULL, &format);
+	if (err)
+		goto out;
+
+	v4l2_fill_pix_format(pix, mf);
+	pr_info("%s(): colorspace=0x%x pixfmt=0x%x\n",
+		__func__, pix->colorspace, pix->pixelformat);
+
+	err = pxa_camera_init_videobuf2(pcdev);
+	if (err)
+		goto out;
+
+	err = video_register_device(&pcdev->vdev, VFL_TYPE_GRABBER, -1);
+	if (err) {
+		v4l2_err(v4l2_dev, "register video device failed: %d\n", err);
+		pcdev->sensor = NULL;
+	} else {
+		dev_info(pcdev_to_dev(pcdev),
+			 "PXA Camera driver attached to camera %s\n",
+			 subdev->name);
+	}
+out:
+	mutex_unlock(&pcdev->mlock);
+	return err;
+}
+
+static void pxa_camera_sensor_unbind(struct v4l2_async_notifier *notifier,
+		     struct v4l2_subdev *subdev,
+		     struct v4l2_async_subdev *asd)
+{
+	struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(notifier->v4l2_dev);
+
+	mutex_lock(&pcdev->mlock);
+	dev_info(pcdev_to_dev(pcdev),
+		 "PXA Camera driver detached from camera %s\n",
+		 subdev->name);
+
+	/* disable capture, disable interrupts */
+	__raw_writel(0x3ff, pcdev->base + CICR0);
+
+	/* Stop DMA engine */
+	pxa_dma_stop_channels(pcdev);
+
+	pxa_camera_destroy_formats(pcdev);
+	video_unregister_device(&pcdev->vdev);
+	pcdev->sensor = NULL;
+
+	mutex_unlock(&pcdev->mlock);
+}
+
+/*
+ * Driver probe, remove, suspend and resume operations
+ */
+static int pxa_camera_suspend(struct device *dev)
+{
+	struct pxa_camera_dev *pcdev = dev_get_drvdata(dev);
+	int i = 0, ret = 0;
+
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR0);
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR1);
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR2);
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR3);
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR4);
+
+	if (pcdev->sensor) {
+		ret = sensor_call(pcdev, core, s_power, 0);
+		if (ret == -ENOIOCTLCMD)
+			ret = 0;
+	}
+
+	return ret;
+}
+
+static int pxa_camera_resume(struct device *dev)
+{
+	struct pxa_camera_dev *pcdev = dev_get_drvdata(dev);
+	int i = 0, ret = 0;
+
+	__raw_writel(pcdev->save_cicr[i++] & ~CICR0_ENB, pcdev->base + CICR0);
+	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR1);
+	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR2);
+	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR3);
+	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR4);
+
+	if (pcdev->sensor) {
+		ret = sensor_call(pcdev, core, s_power, 1);
+		if (ret == -ENOIOCTLCMD)
+			ret = 0;
+	}
+
+	/* Restart frame capture if active buffer exists */
+	if (!ret && pcdev->active)
+		pxa_camera_start_capture(pcdev);
+
+	return ret;
+}
+
+static int pxa_camera_pdata_from_dt(struct device *dev,
+				    struct pxa_camera_dev *pcdev,
+				    struct v4l2_async_subdev *asd)
+{
+	u32 mclk_rate;
+	struct device_node *remote, *np = dev->of_node;
+	struct v4l2_of_endpoint ep;
+	int err = of_property_read_u32(np, "clock-frequency",
+				       &mclk_rate);
+	if (!err) {
+		pcdev->platform_flags |= PXA_CAMERA_MCLK_EN;
+		pcdev->mclk = mclk_rate;
+	}
+
+	np = of_graph_get_next_endpoint(np, NULL);
+	if (!np) {
+		dev_err(dev, "could not find endpoint\n");
+		return -EINVAL;
+	}
+
+	err = v4l2_of_parse_endpoint(np, &ep);
+	if (err) {
+		dev_err(dev, "could not parse endpoint\n");
+		goto out;
+	}
+
+	switch (ep.bus.parallel.bus_width) {
+	case 4:
+		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_4;
+		break;
+	case 5:
+		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_5;
+		break;
+	case 8:
+		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_8;
+		break;
+	case 9:
+		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_9;
+		break;
+	case 10:
+		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_10;
+		break;
+	default:
+		break;
+	}
+
+	if (ep.bus.parallel.flags & V4L2_MBUS_MASTER)
+		pcdev->platform_flags |= PXA_CAMERA_MASTER;
+	if (ep.bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		pcdev->platform_flags |= PXA_CAMERA_HSP;
+	if (ep.bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+		pcdev->platform_flags |= PXA_CAMERA_VSP;
+	if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		pcdev->platform_flags |= PXA_CAMERA_PCLK_EN | PXA_CAMERA_PCP;
+	if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+		pcdev->platform_flags |= PXA_CAMERA_PCLK_EN;
+
+	asd->match_type = V4L2_ASYNC_MATCH_OF;
+	remote = of_graph_get_remote_port(np);
+	if (remote) {
+		asd->match.of.node = remote;
+		of_node_put(remote);
+	} else {
+		dev_notice(dev, "no remote for %s\n", of_node_full_name(np));
+	}
+
+out:
+	of_node_put(np);
+
+	return err;
+}
+
+static int pxa_camera_probe(struct platform_device *pdev)
+{
+	struct pxa_camera_dev *pcdev;
+	struct resource *res;
+	void __iomem *base;
+	struct dma_slave_config config = {
+		.src_addr_width = 0,
+		.src_maxburst = 8,
+		.direction = DMA_DEV_TO_MEM,
+	};
+	dma_cap_mask_t mask;
+	struct pxad_param params;
+	char clk_name[V4L2_CLK_NAME_SIZE];
+	int irq;
+	int err = 0, i;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_irq(pdev, 0);
+	if (!res || irq < 0)
+		return -ENODEV;
+
+	pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
+	if (!pcdev) {
+		dev_err(&pdev->dev, "Could not allocate pcdev\n");
+		return -ENOMEM;
+	}
+
+	pcdev->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(pcdev->clk))
+		return PTR_ERR(pcdev->clk);
+
+	pcdev->res = res;
+
+	pcdev->pdata = pdev->dev.platform_data;
+	if (&pdev->dev.of_node && !pcdev->pdata) {
+		err = pxa_camera_pdata_from_dt(&pdev->dev, pcdev, &pcdev->asd);
+	} else {
+		pcdev->platform_flags = pcdev->pdata->flags;
+		pcdev->mclk = pcdev->pdata->mclk_10khz * 10000;
+		pcdev->asd.match_type = V4L2_ASYNC_MATCH_I2C;
+		pcdev->asd.match.i2c.adapter_id =
+			pcdev->pdata->sensor_i2c_adapter_id;
+		pcdev->asd.match.i2c.address = pcdev->pdata->sensor_i2c_address;
+	}
+	if (err < 0)
+		return err;
+
+	if (!(pcdev->platform_flags & (PXA_CAMERA_DATAWIDTH_8 |
+			PXA_CAMERA_DATAWIDTH_9 | PXA_CAMERA_DATAWIDTH_10))) {
+		/*
+		 * Platform hasn't set available data widths. This is bad.
+		 * Warn and use a default.
+		 */
+		dev_warn(&pdev->dev, "WARNING! Platform hasn't set available "
+			 "data widths, using default 10 bit\n");
+		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_10;
+	}
+	if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_8)
+		pcdev->width_flags = 1 << 7;
+	if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_9)
+		pcdev->width_flags |= 1 << 8;
+	if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_10)
+		pcdev->width_flags |= 1 << 9;
+	if (!pcdev->mclk) {
+		dev_warn(&pdev->dev,
+			 "mclk == 0! Please, fix your platform data. "
+			 "Using default 20MHz\n");
+		pcdev->mclk = 20000000;
+	}
+
+	pcdev->mclk_divisor = mclk_get_divisor(pdev, pcdev);
+
+	INIT_LIST_HEAD(&pcdev->capture);
+	spin_lock_init(&pcdev->lock);
+	mutex_init(&pcdev->mlock);
+
+	/*
+	 * Request the regions.
+	 */
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	pcdev->irq = irq;
+	pcdev->base = base;
+
+	/* request dma */
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+	dma_cap_set(DMA_PRIVATE, mask);
+
+	params.prio = 0;
+	params.drcmr = 68;
+	pcdev->dma_chans[0] =
+		dma_request_slave_channel_compat(mask, pxad_filter_fn,
+						 &params, &pdev->dev, "CI_Y");
+	if (!pcdev->dma_chans[0]) {
+		dev_err(&pdev->dev, "Can't request DMA for Y\n");
+		return -ENODEV;
+	}
+
+	params.drcmr = 69;
+	pcdev->dma_chans[1] =
+		dma_request_slave_channel_compat(mask, pxad_filter_fn,
+						 &params, &pdev->dev, "CI_U");
+	if (!pcdev->dma_chans[1]) {
+		dev_err(&pdev->dev, "Can't request DMA for Y\n");
+		goto exit_free_dma_y;
+	}
+
+	params.drcmr = 70;
+	pcdev->dma_chans[2] =
+		dma_request_slave_channel_compat(mask, pxad_filter_fn,
+						 &params, &pdev->dev, "CI_V");
+	if (!pcdev->dma_chans[2]) {
+		dev_err(&pdev->dev, "Can't request DMA for V\n");
+		goto exit_free_dma_u;
+	}
+
+	for (i = 0; i < 3; i++) {
+		config.src_addr = pcdev->res->start + CIBR0 + i * 8;
+		err = dmaengine_slave_config(pcdev->dma_chans[i], &config);
+		if (err < 0) {
+			dev_err(&pdev->dev, "dma slave config failed: %d\n",
+				err);
+			goto exit_free_dma;
+		}
+	}
+
+	/* request irq */
+	err = devm_request_irq(&pdev->dev, pcdev->irq, pxa_camera_irq, 0,
+			       PXA_CAM_DRV_NAME, pcdev);
+	if (err) {
+		dev_err(&pdev->dev, "Camera interrupt register failed\n");
+		goto exit_free_dma;
+	}
+
+	tasklet_init(&pcdev->task_eof, pxa_camera_eof, (unsigned long)pcdev);
+
+	pxa_camera_activate(pcdev);
+
+	dev_set_drvdata(&pdev->dev, pcdev);
+	err = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
+	if (err)
+		goto exit_free_dma;
+
+	pcdev->asds[0] = &pcdev->asd;
+	pcdev->notifier.subdevs = pcdev->asds;
+	pcdev->notifier.num_subdevs = 1;
+	pcdev->notifier.bound = pxa_camera_sensor_bound;
+	pcdev->notifier.unbind = pxa_camera_sensor_unbind;
+
+	if (!of_have_populated_dt())
+		pcdev->asd.match_type = V4L2_ASYNC_MATCH_I2C;
+
+	err = pxa_camera_init_videobuf2(pcdev);
+	if (err)
+		goto exit_free_v4l2dev;
+
+	if (pcdev->mclk) {
+		v4l2_clk_name_i2c(clk_name, sizeof(clk_name),
+				  pcdev->asd.match.i2c.adapter_id,
+				  pcdev->asd.match.i2c.address);
+
+		pcdev->mclk_clk = v4l2_clk_register(&pxa_camera_mclk_ops,
+						    clk_name, NULL);
+		if (IS_ERR(pcdev->mclk_clk))
+			return PTR_ERR(pcdev->mclk_clk);
+	}
+
+	err = v4l2_async_notifier_register(&pcdev->v4l2_dev, &pcdev->notifier);
+	if (err)
+		goto exit_free_clk;
+
+	return 0;
+exit_free_clk:
+	v4l2_clk_unregister(pcdev->mclk_clk);
+exit_free_v4l2dev:
+	v4l2_device_unregister(&pcdev->v4l2_dev);
+exit_free_dma:
+	dma_release_channel(pcdev->dma_chans[2]);
+exit_free_dma_u:
+	dma_release_channel(pcdev->dma_chans[1]);
+exit_free_dma_y:
+	dma_release_channel(pcdev->dma_chans[0]);
+	return err;
+}
+
+static int pxa_camera_remove(struct platform_device *pdev)
+{
+	struct pxa_camera_dev *pcdev = dev_get_drvdata(&pdev->dev);
+
+	pxa_camera_deactivate(pcdev);
+	dma_release_channel(pcdev->dma_chans[0]);
+	dma_release_channel(pcdev->dma_chans[1]);
+	dma_release_channel(pcdev->dma_chans[2]);
+
+	v4l2_clk_unregister(pcdev->mclk_clk);
+	v4l2_device_unregister(&pcdev->v4l2_dev);
+
+	dev_info(&pdev->dev, "PXA Camera driver unloaded\n");
+
+	return 0;
+}
+
+static const struct dev_pm_ops pxa_camera_pm = {
+	.suspend	= pxa_camera_suspend,
+	.resume		= pxa_camera_resume,
+};
+
+static const struct of_device_id pxa_camera_of_match[] = {
+	{ .compatible = "marvell,pxa270-qci", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, pxa_camera_of_match);
+
+static struct platform_driver pxa_camera_driver = {
+	.driver		= {
+		.name	= PXA_CAM_DRV_NAME,
+		.pm	= &pxa_camera_pm,
+		.of_match_table = of_match_ptr(pxa_camera_of_match),
+	},
+	.probe		= pxa_camera_probe,
+	.remove		= pxa_camera_remove,
+};
+
+module_platform_driver(pxa_camera_driver);
+
+MODULE_DESCRIPTION("PXA27x SoC Camera Host driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(PXA_CAM_VERSION);
+MODULE_ALIAS("platform:" PXA_CAM_DRV_NAME);
diff --git a/drivers/media/platform/soc_camera/Kconfig b/drivers/media/platform/soc_camera/Kconfig
index 0bf33ccf9a1e..ede2b5e2f3b2 100644
--- a/drivers/media/platform/soc_camera/Kconfig
+++ b/drivers/media/platform/soc_camera/Kconfig
@@ -17,14 +17,6 @@ config SOC_CAMERA_PLATFORM
 	help
 	  This is a generic SoC camera platform driver, useful for testing
 
-config VIDEO_PXA27x
-	tristate "PXA27x Quick Capture Interface driver"
-	depends on VIDEO_DEV && PXA27x && HAS_DMA
-	select VIDEOBUF2_DMA_SG
-	select SG_SPLIT
-	---help---
-	  This is a v4l2 driver for the PXA27x Quick Capture Interface
-
 config VIDEO_RCAR_VIN_OLD
 	tristate "R-Car Video Input (VIN) support (DEPRECATED)"
 	depends on VIDEO_DEV && SOC_CAMERA
diff --git a/drivers/media/platform/soc_camera/Makefile b/drivers/media/platform/soc_camera/Makefile
index 7703cb7ce456..0109a65a2775 100644
--- a/drivers/media/platform/soc_camera/Makefile
+++ b/drivers/media/platform/soc_camera/Makefile
@@ -7,7 +7,6 @@ obj-$(CONFIG_SOC_CAMERA_PLATFORM)	+= soc_camera_platform.o
 
 # soc-camera host drivers have to be linked after camera drivers
 obj-$(CONFIG_VIDEO_ATMEL_ISI)		+= atmel-isi.o
-obj-$(CONFIG_VIDEO_PXA27x)		+= pxa_camera.o
 obj-$(CONFIG_VIDEO_SH_MOBILE_CEU)	+= sh_mobile_ceu_camera.o
 obj-$(CONFIG_VIDEO_SH_MOBILE_CSI2)	+= sh_mobile_csi2.o
 obj-$(CONFIG_VIDEO_RCAR_VIN_OLD)	+= rcar_vin.o
diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
deleted file mode 100644
index 868c6ad4784c..000000000000
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ /dev/null
@@ -1,2096 +0,0 @@
-/*
- * V4L2 Driver for PXA camera host
- *
- * Copyright (C) 2006, Sascha Hauer, Pengutronix
- * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
- * Copyright (C) 2016, Robert Jarzmik <robert.jarzmik@free.fr>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/io.h>
-#include <linux/delay.h>
-#include <linux/device.h>
-#include <linux/dma-mapping.h>
-#include <linux/err.h>
-#include <linux/errno.h>
-#include <linux/fs.h>
-#include <linux/interrupt.h>
-#include <linux/kernel.h>
-#include <linux/mm.h>
-#include <linux/moduleparam.h>
-#include <linux/of.h>
-#include <linux/time.h>
-#include <linux/device.h>
-#include <linux/platform_device.h>
-#include <linux/clk.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/dmaengine.h>
-#include <linux/dma-mapping.h>
-#include <linux/dma/pxa-dma.h>
-
-#include <media/v4l2-async.h>
-#include <media/v4l2-clk.h>
-#include <media/v4l2-common.h>
-#include <media/v4l2-device.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-of.h>
-
-#include <media/drv-intf/soc_mediabus.h>
-#include <media/videobuf2-dma-sg.h>
-
-#include <linux/videodev2.h>
-
-#include <linux/platform_data/media/camera-pxa.h>
-
-#define PXA_CAM_VERSION "0.0.6"
-#define PXA_CAM_DRV_NAME "pxa27x-camera"
-
-#define DEFAULT_WIDTH	640
-#define DEFAULT_HEIGHT	480
-
-/* Camera Interface */
-#define CICR0		0x0000
-#define CICR1		0x0004
-#define CICR2		0x0008
-#define CICR3		0x000C
-#define CICR4		0x0010
-#define CISR		0x0014
-#define CIFR		0x0018
-#define CITOR		0x001C
-#define CIBR0		0x0028
-#define CIBR1		0x0030
-#define CIBR2		0x0038
-
-#define CICR0_DMAEN	(1 << 31)	/* DMA request enable */
-#define CICR0_PAR_EN	(1 << 30)	/* Parity enable */
-#define CICR0_SL_CAP_EN	(1 << 29)	/* Capture enable for slave mode */
-#define CICR0_ENB	(1 << 28)	/* Camera interface enable */
-#define CICR0_DIS	(1 << 27)	/* Camera interface disable */
-#define CICR0_SIM	(0x7 << 24)	/* Sensor interface mode mask */
-#define CICR0_TOM	(1 << 9)	/* Time-out mask */
-#define CICR0_RDAVM	(1 << 8)	/* Receive-data-available mask */
-#define CICR0_FEM	(1 << 7)	/* FIFO-empty mask */
-#define CICR0_EOLM	(1 << 6)	/* End-of-line mask */
-#define CICR0_PERRM	(1 << 5)	/* Parity-error mask */
-#define CICR0_QDM	(1 << 4)	/* Quick-disable mask */
-#define CICR0_CDM	(1 << 3)	/* Disable-done mask */
-#define CICR0_SOFM	(1 << 2)	/* Start-of-frame mask */
-#define CICR0_EOFM	(1 << 1)	/* End-of-frame mask */
-#define CICR0_FOM	(1 << 0)	/* FIFO-overrun mask */
-
-#define CICR1_TBIT	(1 << 31)	/* Transparency bit */
-#define CICR1_RGBT_CONV	(0x3 << 29)	/* RGBT conversion mask */
-#define CICR1_PPL	(0x7ff << 15)	/* Pixels per line mask */
-#define CICR1_RGB_CONV	(0x7 << 12)	/* RGB conversion mask */
-#define CICR1_RGB_F	(1 << 11)	/* RGB format */
-#define CICR1_YCBCR_F	(1 << 10)	/* YCbCr format */
-#define CICR1_RGB_BPP	(0x7 << 7)	/* RGB bis per pixel mask */
-#define CICR1_RAW_BPP	(0x3 << 5)	/* Raw bis per pixel mask */
-#define CICR1_COLOR_SP	(0x3 << 3)	/* Color space mask */
-#define CICR1_DW	(0x7 << 0)	/* Data width mask */
-
-#define CICR2_BLW	(0xff << 24)	/* Beginning-of-line pixel clock
-					   wait count mask */
-#define CICR2_ELW	(0xff << 16)	/* End-of-line pixel clock
-					   wait count mask */
-#define CICR2_HSW	(0x3f << 10)	/* Horizontal sync pulse width mask */
-#define CICR2_BFPW	(0x3f << 3)	/* Beginning-of-frame pixel clock
-					   wait count mask */
-#define CICR2_FSW	(0x7 << 0)	/* Frame stabilization
-					   wait count mask */
-
-#define CICR3_BFW	(0xff << 24)	/* Beginning-of-frame line clock
-					   wait count mask */
-#define CICR3_EFW	(0xff << 16)	/* End-of-frame line clock
-					   wait count mask */
-#define CICR3_VSW	(0x3f << 10)	/* Vertical sync pulse width mask */
-#define CICR3_BFPW	(0x3f << 3)	/* Beginning-of-frame pixel clock
-					   wait count mask */
-#define CICR3_LPF	(0x7ff << 0)	/* Lines per frame mask */
-
-#define CICR4_MCLK_DLY	(0x3 << 24)	/* MCLK Data Capture Delay mask */
-#define CICR4_PCLK_EN	(1 << 23)	/* Pixel clock enable */
-#define CICR4_PCP	(1 << 22)	/* Pixel clock polarity */
-#define CICR4_HSP	(1 << 21)	/* Horizontal sync polarity */
-#define CICR4_VSP	(1 << 20)	/* Vertical sync polarity */
-#define CICR4_MCLK_EN	(1 << 19)	/* MCLK enable */
-#define CICR4_FR_RATE	(0x7 << 8)	/* Frame rate mask */
-#define CICR4_DIV	(0xff << 0)	/* Clock divisor mask */
-
-#define CISR_FTO	(1 << 15)	/* FIFO time-out */
-#define CISR_RDAV_2	(1 << 14)	/* Channel 2 receive data available */
-#define CISR_RDAV_1	(1 << 13)	/* Channel 1 receive data available */
-#define CISR_RDAV_0	(1 << 12)	/* Channel 0 receive data available */
-#define CISR_FEMPTY_2	(1 << 11)	/* Channel 2 FIFO empty */
-#define CISR_FEMPTY_1	(1 << 10)	/* Channel 1 FIFO empty */
-#define CISR_FEMPTY_0	(1 << 9)	/* Channel 0 FIFO empty */
-#define CISR_EOL	(1 << 8)	/* End of line */
-#define CISR_PAR_ERR	(1 << 7)	/* Parity error */
-#define CISR_CQD	(1 << 6)	/* Camera interface quick disable */
-#define CISR_CDD	(1 << 5)	/* Camera interface disable done */
-#define CISR_SOF	(1 << 4)	/* Start of frame */
-#define CISR_EOF	(1 << 3)	/* End of frame */
-#define CISR_IFO_2	(1 << 2)	/* FIFO overrun for Channel 2 */
-#define CISR_IFO_1	(1 << 1)	/* FIFO overrun for Channel 1 */
-#define CISR_IFO_0	(1 << 0)	/* FIFO overrun for Channel 0 */
-
-#define CIFR_FLVL2	(0x7f << 23)	/* FIFO 2 level mask */
-#define CIFR_FLVL1	(0x7f << 16)	/* FIFO 1 level mask */
-#define CIFR_FLVL0	(0xff << 8)	/* FIFO 0 level mask */
-#define CIFR_THL_0	(0x3 << 4)	/* Threshold Level for Channel 0 FIFO */
-#define CIFR_RESET_F	(1 << 3)	/* Reset input FIFOs */
-#define CIFR_FEN2	(1 << 2)	/* FIFO enable for channel 2 */
-#define CIFR_FEN1	(1 << 1)	/* FIFO enable for channel 1 */
-#define CIFR_FEN0	(1 << 0)	/* FIFO enable for channel 0 */
-
-#define CICR0_SIM_MP	(0 << 24)
-#define CICR0_SIM_SP	(1 << 24)
-#define CICR0_SIM_MS	(2 << 24)
-#define CICR0_SIM_EP	(3 << 24)
-#define CICR0_SIM_ES	(4 << 24)
-
-#define CICR1_DW_VAL(x)   ((x) & CICR1_DW)	    /* Data bus width */
-#define CICR1_PPL_VAL(x)  (((x) << 15) & CICR1_PPL) /* Pixels per line */
-#define CICR1_COLOR_SP_VAL(x)	(((x) << 3) & CICR1_COLOR_SP)	/* color space */
-#define CICR1_RGB_BPP_VAL(x)	(((x) << 7) & CICR1_RGB_BPP)	/* bpp for rgb */
-#define CICR1_RGBT_CONV_VAL(x)	(((x) << 29) & CICR1_RGBT_CONV)	/* rgbt conv */
-
-#define CICR2_BLW_VAL(x)  (((x) << 24) & CICR2_BLW) /* Beginning-of-line pixel clock wait count */
-#define CICR2_ELW_VAL(x)  (((x) << 16) & CICR2_ELW) /* End-of-line pixel clock wait count */
-#define CICR2_HSW_VAL(x)  (((x) << 10) & CICR2_HSW) /* Horizontal sync pulse width */
-#define CICR2_BFPW_VAL(x) (((x) << 3) & CICR2_BFPW) /* Beginning-of-frame pixel clock wait count */
-#define CICR2_FSW_VAL(x)  (((x) << 0) & CICR2_FSW)  /* Frame stabilization wait count */
-
-#define CICR3_BFW_VAL(x)  (((x) << 24) & CICR3_BFW) /* Beginning-of-frame line clock wait count  */
-#define CICR3_EFW_VAL(x)  (((x) << 16) & CICR3_EFW) /* End-of-frame line clock wait count */
-#define CICR3_VSW_VAL(x)  (((x) << 11) & CICR3_VSW) /* Vertical sync pulse width */
-#define CICR3_LPF_VAL(x)  (((x) << 0) & CICR3_LPF)  /* Lines per frame */
-
-#define CICR0_IRQ_MASK (CICR0_TOM | CICR0_RDAVM | CICR0_FEM | CICR0_EOLM | \
-			CICR0_PERRM | CICR0_QDM | CICR0_CDM | CICR0_SOFM | \
-			CICR0_EOFM | CICR0_FOM)
-
-#define sensor_call(cam, o, f, args...) \
-	v4l2_subdev_call(cam->sensor, o, f, ##args)
-
-/*
- * Format handling
- */
-/**
- * struct soc_camera_format_xlate - match between host and sensor formats
- * @code: code of a sensor provided format
- * @host_fmt: host format after host translation from code
- *
- * Host and sensor translation structure. Used in table of host and sensor
- * formats matchings in soc_camera_device. A host can override the generic list
- * generation by implementing get_formats(), and use it for format checks and
- * format setup.
- */
-struct soc_camera_format_xlate {
-	u32 code;
-	const struct soc_mbus_pixelfmt *host_fmt;
-};
-
-/*
- * Structures
- */
-enum pxa_camera_active_dma {
-	DMA_Y = 0x1,
-	DMA_U = 0x2,
-	DMA_V = 0x4,
-};
-
-/* buffer for one video frame */
-struct pxa_buffer {
-	/* common v4l buffer stuff -- must be first */
-	struct vb2_v4l2_buffer		vbuf;
-	struct list_head		queue;
-	u32	code;
-	int				nb_planes;
-	/* our descriptor lists for Y, U and V channels */
-	struct dma_async_tx_descriptor	*descs[3];
-	dma_cookie_t			cookie[3];
-	struct scatterlist		*sg[3];
-	int				sg_len[3];
-	size_t				plane_sizes[3];
-	int				inwork;
-	enum pxa_camera_active_dma	active_dma;
-};
-
-struct pxa_camera_dev {
-	struct v4l2_device	v4l2_dev;
-	struct video_device	vdev;
-	struct v4l2_async_notifier notifier;
-	struct vb2_queue	vb2_vq;
-	struct v4l2_subdev	*sensor;
-	struct soc_camera_format_xlate *user_formats;
-	const struct soc_camera_format_xlate *current_fmt;
-	struct v4l2_pix_format	current_pix;
-
-	struct v4l2_async_subdev asd;
-	struct v4l2_async_subdev *asds[1];
-
-	/*
-	 * PXA27x is only supposed to handle one camera on its Quick Capture
-	 * interface. If anyone ever builds hardware to enable more than
-	 * one camera, they will have to modify this driver too
-	 */
-	struct clk		*clk;
-
-	unsigned int		irq;
-	void __iomem		*base;
-
-	int			channels;
-	struct dma_chan		*dma_chans[3];
-
-	struct pxacamera_platform_data *pdata;
-	struct resource		*res;
-	unsigned long		platform_flags;
-	unsigned long		ciclk;
-	unsigned long		mclk;
-	u32			mclk_divisor;
-	struct v4l2_clk		*mclk_clk;
-	u16			width_flags;	/* max 10 bits */
-
-	struct list_head	capture;
-
-	spinlock_t		lock;
-	struct mutex		mlock;
-	unsigned int		buf_sequence;
-
-	struct pxa_buffer	*active;
-	struct tasklet_struct	task_eof;
-
-	u32			save_cicr[5];
-};
-
-struct pxa_cam {
-	unsigned long flags;
-};
-
-static const char *pxa_cam_driver_description = "PXA_Camera";
-
-/*
- * Format translation functions
- */
-const struct soc_camera_format_xlate *soc_mbus_xlate_by_fourcc(
-	struct soc_camera_format_xlate *user_formats, unsigned int fourcc)
-{
-	unsigned int i;
-
-	for (i = 0; user_formats[i].code; i++)
-		if (user_formats[i].host_fmt->fourcc == fourcc)
-			return user_formats + i;
-	return NULL;
-}
-
-static struct soc_camera_format_xlate *soc_mbus_build_fmts_xlate(
-	struct v4l2_device *v4l2_dev, struct v4l2_subdev *subdev,
-	int (*get_formats)(struct v4l2_device *, unsigned int,
-			   struct soc_camera_format_xlate *xlate))
-{
-	unsigned int i, fmts = 0, raw_fmts = 0;
-	int ret;
-	struct v4l2_subdev_mbus_code_enum code = {
-		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-	};
-	struct soc_camera_format_xlate *user_formats;
-
-	while (!v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code)) {
-		raw_fmts++;
-		code.index++;
-	}
-
-	/*
-	 * First pass - only count formats this host-sensor
-	 * configuration can provide
-	 */
-	for (i = 0; i < raw_fmts; i++) {
-		ret = get_formats(v4l2_dev, i, NULL);
-		if (ret < 0)
-			return ERR_PTR(ret);
-		fmts += ret;
-	}
-
-	if (!fmts)
-		return ERR_PTR(-ENXIO);
-
-	user_formats = kcalloc(fmts + 1, sizeof(*user_formats), GFP_KERNEL);
-	if (!user_formats)
-		return ERR_PTR(-ENOMEM);
-
-	/* Second pass - actually fill data formats */
-	fmts = 0;
-	for (i = 0; i < raw_fmts; i++) {
-		ret = get_formats(v4l2_dev, i, user_formats + fmts);
-		if (ret < 0)
-			goto egfmt;
-		fmts += ret;
-	}
-	user_formats[fmts].code = 0;
-
-	return user_formats;
-egfmt:
-	kfree(user_formats);
-	return ERR_PTR(ret);
-}
-
-/*
- *  Videobuf operations
- */
-static struct pxa_buffer *vb2_to_pxa_buffer(struct vb2_buffer *vb)
-{
-	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-
-	return container_of(vbuf, struct pxa_buffer, vbuf);
-}
-
-static struct device *pcdev_to_dev(struct pxa_camera_dev *pcdev)
-{
-	return pcdev->v4l2_dev.dev;
-}
-
-static struct pxa_camera_dev *v4l2_dev_to_pcdev(struct v4l2_device *v4l2_dev)
-{
-	return container_of(v4l2_dev, struct pxa_camera_dev, v4l2_dev);
-}
-
-static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
-			       enum pxa_camera_active_dma act_dma);
-
-static void pxa_camera_dma_irq_y(void *data)
-{
-	struct pxa_camera_dev *pcdev = data;
-
-	pxa_camera_dma_irq(pcdev, DMA_Y);
-}
-
-static void pxa_camera_dma_irq_u(void *data)
-{
-	struct pxa_camera_dev *pcdev = data;
-
-	pxa_camera_dma_irq(pcdev, DMA_U);
-}
-
-static void pxa_camera_dma_irq_v(void *data)
-{
-	struct pxa_camera_dev *pcdev = data;
-
-	pxa_camera_dma_irq(pcdev, DMA_V);
-}
-
-/**
- * pxa_init_dma_channel - init dma descriptors
- * @pcdev: pxa camera device
- * @vb: videobuffer2 buffer
- * @dma: dma video buffer
- * @channel: dma channel (0 => 'Y', 1 => 'U', 2 => 'V')
- * @cibr: camera Receive Buffer Register
- *
- * Prepares the pxa dma descriptors to transfer one camera channel.
- *
- * Returns 0 if success or -ENOMEM if no memory is available
- */
-static int pxa_init_dma_channel(struct pxa_camera_dev *pcdev,
-				struct pxa_buffer *buf, int channel,
-				struct scatterlist *sg, int sglen)
-{
-	struct dma_chan *dma_chan = pcdev->dma_chans[channel];
-	struct dma_async_tx_descriptor *tx;
-
-	tx = dmaengine_prep_slave_sg(dma_chan, sg, sglen, DMA_DEV_TO_MEM,
-				     DMA_PREP_INTERRUPT | DMA_CTRL_REUSE);
-	if (!tx) {
-		dev_err(pcdev_to_dev(pcdev),
-			"dmaengine_prep_slave_sg failed\n");
-		goto fail;
-	}
-
-	tx->callback_param = pcdev;
-	switch (channel) {
-	case 0:
-		tx->callback = pxa_camera_dma_irq_y;
-		break;
-	case 1:
-		tx->callback = pxa_camera_dma_irq_u;
-		break;
-	case 2:
-		tx->callback = pxa_camera_dma_irq_v;
-		break;
-	}
-
-	buf->descs[channel] = tx;
-	return 0;
-fail:
-	dev_dbg(pcdev_to_dev(pcdev),
-		"%s (vb=%p) dma_tx=%p\n",
-		__func__, buf, tx);
-
-	return -ENOMEM;
-}
-
-static void pxa_videobuf_set_actdma(struct pxa_camera_dev *pcdev,
-				    struct pxa_buffer *buf)
-{
-	buf->active_dma = DMA_Y;
-	if (buf->nb_planes == 3)
-		buf->active_dma |= DMA_U | DMA_V;
-}
-
-/**
- * pxa_dma_start_channels - start DMA channel for active buffer
- * @pcdev: pxa camera device
- *
- * Initialize DMA channels to the beginning of the active video buffer, and
- * start these channels.
- */
-static void pxa_dma_start_channels(struct pxa_camera_dev *pcdev)
-{
-	int i;
-	struct pxa_buffer *active;
-
-	active = pcdev->active;
-
-	for (i = 0; i < pcdev->channels; i++) {
-		dev_dbg(pcdev_to_dev(pcdev),
-			"%s (channel=%d)\n", __func__, i);
-		dma_async_issue_pending(pcdev->dma_chans[i]);
-	}
-}
-
-static void pxa_dma_stop_channels(struct pxa_camera_dev *pcdev)
-{
-	int i;
-
-	for (i = 0; i < pcdev->channels; i++) {
-		dev_dbg(pcdev_to_dev(pcdev),
-			"%s (channel=%d)\n", __func__, i);
-		dmaengine_terminate_all(pcdev->dma_chans[i]);
-	}
-}
-
-static void pxa_dma_add_tail_buf(struct pxa_camera_dev *pcdev,
-				 struct pxa_buffer *buf)
-{
-	int i;
-
-	for (i = 0; i < pcdev->channels; i++) {
-		buf->cookie[i] = dmaengine_submit(buf->descs[i]);
-		dev_dbg(pcdev_to_dev(pcdev),
-			"%s (channel=%d) : submit vb=%p cookie=%d\n",
-			__func__, i, buf, buf->descs[i]->cookie);
-	}
-}
-
-/**
- * pxa_camera_start_capture - start video capturing
- * @pcdev: camera device
- *
- * Launch capturing. DMA channels should not be active yet. They should get
- * activated at the end of frame interrupt, to capture only whole frames, and
- * never begin the capture of a partial frame.
- */
-static void pxa_camera_start_capture(struct pxa_camera_dev *pcdev)
-{
-	unsigned long cicr0;
-
-	dev_dbg(pcdev_to_dev(pcdev), "%s\n", __func__);
-	__raw_writel(__raw_readl(pcdev->base + CISR), pcdev->base + CISR);
-	/* Enable End-Of-Frame Interrupt */
-	cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_ENB;
-	cicr0 &= ~CICR0_EOFM;
-	__raw_writel(cicr0, pcdev->base + CICR0);
-}
-
-static void pxa_camera_stop_capture(struct pxa_camera_dev *pcdev)
-{
-	unsigned long cicr0;
-
-	pxa_dma_stop_channels(pcdev);
-
-	cicr0 = __raw_readl(pcdev->base + CICR0) & ~CICR0_ENB;
-	__raw_writel(cicr0, pcdev->base + CICR0);
-
-	pcdev->active = NULL;
-	dev_dbg(pcdev_to_dev(pcdev), "%s\n", __func__);
-}
-
-static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
-			      struct pxa_buffer *buf,
-			      enum vb2_buffer_state state)
-{
-	struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
-	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-
-	/* _init is used to debug races, see comment in pxa_camera_reqbufs() */
-	list_del_init(&buf->queue);
-	vb->timestamp = ktime_get_ns();
-	vbuf->sequence = pcdev->buf_sequence++;
-	vbuf->field = V4L2_FIELD_NONE;
-	vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
-	dev_dbg(pcdev_to_dev(pcdev), "%s dequeud buffer (buf=0x%p)\n",
-		__func__, buf);
-
-	if (list_empty(&pcdev->capture)) {
-		pxa_camera_stop_capture(pcdev);
-		return;
-	}
-
-	pcdev->active = list_entry(pcdev->capture.next,
-				   struct pxa_buffer, queue);
-}
-
-/**
- * pxa_camera_check_link_miss - check missed DMA linking
- * @pcdev: camera device
- *
- * The DMA chaining is done with DMA running. This means a tiny temporal window
- * remains, where a buffer is queued on the chain, while the chain is already
- * stopped. This means the tailed buffer would never be transferred by DMA.
- * This function restarts the capture for this corner case, where :
- *  - DADR() == DADDR_STOP
- *  - a videobuffer is queued on the pcdev->capture list
- *
- * Please check the "DMA hot chaining timeslice issue" in
- *   Documentation/video4linux/pxa_camera.txt
- *
- * Context: should only be called within the dma irq handler
- */
-static void pxa_camera_check_link_miss(struct pxa_camera_dev *pcdev,
-				       dma_cookie_t last_submitted,
-				       dma_cookie_t last_issued)
-{
-	bool is_dma_stopped = last_submitted != last_issued;
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		"%s : top queued buffer=%p, is_dma_stopped=%d\n",
-		__func__, pcdev->active, is_dma_stopped);
-
-	if (pcdev->active && is_dma_stopped)
-		pxa_camera_start_capture(pcdev);
-}
-
-static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
-			       enum pxa_camera_active_dma act_dma)
-{
-	struct pxa_buffer *buf, *last_buf;
-	unsigned long flags;
-	u32 camera_status, overrun;
-	int chan;
-	enum dma_status last_status;
-	dma_cookie_t last_issued;
-
-	spin_lock_irqsave(&pcdev->lock, flags);
-
-	camera_status = __raw_readl(pcdev->base + CISR);
-	dev_dbg(pcdev_to_dev(pcdev), "camera dma irq, cisr=0x%x dma=%d\n",
-		camera_status, act_dma);
-	overrun = CISR_IFO_0;
-	if (pcdev->channels == 3)
-		overrun |= CISR_IFO_1 | CISR_IFO_2;
-
-	/*
-	 * pcdev->active should not be NULL in DMA irq handler.
-	 *
-	 * But there is one corner case : if capture was stopped due to an
-	 * overrun of channel 1, and at that same channel 2 was completed.
-	 *
-	 * When handling the overrun in DMA irq for channel 1, we'll stop the
-	 * capture and restart it (and thus set pcdev->active to NULL). But the
-	 * DMA irq handler will already be pending for channel 2. So on entering
-	 * the DMA irq handler for channel 2 there will be no active buffer, yet
-	 * that is normal.
-	 */
-	if (!pcdev->active)
-		goto out;
-
-	buf = pcdev->active;
-	WARN_ON(buf->inwork || list_empty(&buf->queue));
-
-	/*
-	 * It's normal if the last frame creates an overrun, as there
-	 * are no more DMA descriptors to fetch from QCI fifos
-	 */
-	switch (act_dma) {
-	case DMA_U:
-		chan = 1;
-		break;
-	case DMA_V:
-		chan = 2;
-		break;
-	default:
-		chan = 0;
-		break;
-	}
-	last_buf = list_entry(pcdev->capture.prev,
-			      struct pxa_buffer, queue);
-	last_status = dma_async_is_tx_complete(pcdev->dma_chans[chan],
-					       last_buf->cookie[chan],
-					       NULL, &last_issued);
-	if (camera_status & overrun &&
-	    last_status != DMA_COMPLETE) {
-		dev_dbg(pcdev_to_dev(pcdev), "FIFO overrun! CISR: %x\n",
-			camera_status);
-		pxa_camera_stop_capture(pcdev);
-		list_for_each_entry(buf, &pcdev->capture, queue)
-			pxa_dma_add_tail_buf(pcdev, buf);
-		pxa_camera_start_capture(pcdev);
-		goto out;
-	}
-	buf->active_dma &= ~act_dma;
-	if (!buf->active_dma) {
-		pxa_camera_wakeup(pcdev, buf, VB2_BUF_STATE_DONE);
-		pxa_camera_check_link_miss(pcdev, last_buf->cookie[chan],
-					   last_issued);
-	}
-
-out:
-	spin_unlock_irqrestore(&pcdev->lock, flags);
-}
-
-static u32 mclk_get_divisor(struct platform_device *pdev,
-			    struct pxa_camera_dev *pcdev)
-{
-	unsigned long mclk = pcdev->mclk;
-	u32 div;
-	unsigned long lcdclk;
-
-	lcdclk = clk_get_rate(pcdev->clk);
-	pcdev->ciclk = lcdclk;
-
-	/* mclk <= ciclk / 4 (27.4.2) */
-	if (mclk > lcdclk / 4) {
-		mclk = lcdclk / 4;
-		dev_warn(pcdev_to_dev(pcdev),
-			 "Limiting master clock to %lu\n", mclk);
-	}
-
-	/* We verify mclk != 0, so if anyone breaks it, here comes their Oops */
-	div = (lcdclk + 2 * mclk - 1) / (2 * mclk) - 1;
-
-	/* If we're not supplying MCLK, leave it at 0 */
-	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
-		pcdev->mclk = lcdclk / (2 * (div + 1));
-
-	dev_dbg(pcdev_to_dev(pcdev), "LCD clock %luHz, target freq %luHz, divisor %u\n",
-		lcdclk, mclk, div);
-
-	return div;
-}
-
-static void recalculate_fifo_timeout(struct pxa_camera_dev *pcdev,
-				     unsigned long pclk)
-{
-	/* We want a timeout > 1 pixel time, not ">=" */
-	u32 ciclk_per_pixel = pcdev->ciclk / pclk + 1;
-
-	__raw_writel(ciclk_per_pixel, pcdev->base + CITOR);
-}
-
-static void pxa_camera_activate(struct pxa_camera_dev *pcdev)
-{
-	u32 cicr4 = 0;
-
-	/* disable all interrupts */
-	__raw_writel(0x3ff, pcdev->base + CICR0);
-
-	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
-		cicr4 |= CICR4_PCLK_EN;
-	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
-		cicr4 |= CICR4_MCLK_EN;
-	if (pcdev->platform_flags & PXA_CAMERA_PCP)
-		cicr4 |= CICR4_PCP;
-	if (pcdev->platform_flags & PXA_CAMERA_HSP)
-		cicr4 |= CICR4_HSP;
-	if (pcdev->platform_flags & PXA_CAMERA_VSP)
-		cicr4 |= CICR4_VSP;
-
-	__raw_writel(pcdev->mclk_divisor | cicr4, pcdev->base + CICR4);
-
-	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
-		/* Initialise the timeout under the assumption pclk = mclk */
-		recalculate_fifo_timeout(pcdev, pcdev->mclk);
-	else
-		/* "Safe default" - 13MHz */
-		recalculate_fifo_timeout(pcdev, 13000000);
-
-	clk_prepare_enable(pcdev->clk);
-}
-
-static void pxa_camera_deactivate(struct pxa_camera_dev *pcdev)
-{
-	clk_disable_unprepare(pcdev->clk);
-}
-
-static void pxa_camera_eof(unsigned long arg)
-{
-	struct pxa_camera_dev *pcdev = (struct pxa_camera_dev *)arg;
-	unsigned long cifr;
-	struct pxa_buffer *buf;
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		"Camera interrupt status 0x%x\n",
-		__raw_readl(pcdev->base + CISR));
-
-	/* Reset the FIFOs */
-	cifr = __raw_readl(pcdev->base + CIFR) | CIFR_RESET_F;
-	__raw_writel(cifr, pcdev->base + CIFR);
-
-	pcdev->active = list_first_entry(&pcdev->capture,
-					 struct pxa_buffer, queue);
-	buf = pcdev->active;
-	pxa_videobuf_set_actdma(pcdev, buf);
-
-	pxa_dma_start_channels(pcdev);
-}
-
-static irqreturn_t pxa_camera_irq(int irq, void *data)
-{
-	struct pxa_camera_dev *pcdev = data;
-	unsigned long status, cicr0;
-
-	status = __raw_readl(pcdev->base + CISR);
-	dev_dbg(pcdev_to_dev(pcdev),
-		"Camera interrupt status 0x%lx\n", status);
-
-	if (!status)
-		return IRQ_NONE;
-
-	__raw_writel(status, pcdev->base + CISR);
-
-	if (status & CISR_EOF) {
-		cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_EOFM;
-		__raw_writel(cicr0, pcdev->base + CICR0);
-		tasklet_schedule(&pcdev->task_eof);
-	}
-
-	return IRQ_HANDLED;
-}
-
-static int test_platform_param(struct pxa_camera_dev *pcdev,
-			       unsigned char buswidth, unsigned long *flags)
-{
-	/*
-	 * Platform specified synchronization and pixel clock polarities are
-	 * only a recommendation and are only used during probing. The PXA270
-	 * quick capture interface supports both.
-	 */
-	*flags = (pcdev->platform_flags & PXA_CAMERA_MASTER ?
-		  V4L2_MBUS_MASTER : V4L2_MBUS_SLAVE) |
-		V4L2_MBUS_HSYNC_ACTIVE_HIGH |
-		V4L2_MBUS_HSYNC_ACTIVE_LOW |
-		V4L2_MBUS_VSYNC_ACTIVE_HIGH |
-		V4L2_MBUS_VSYNC_ACTIVE_LOW |
-		V4L2_MBUS_DATA_ACTIVE_HIGH |
-		V4L2_MBUS_PCLK_SAMPLE_RISING |
-		V4L2_MBUS_PCLK_SAMPLE_FALLING;
-
-	/* If requested data width is supported by the platform, use it */
-	if ((1 << (buswidth - 1)) & pcdev->width_flags)
-		return 0;
-
-	return -EINVAL;
-}
-
-static void pxa_camera_setup_cicr(struct pxa_camera_dev *pcdev,
-				  unsigned long flags, __u32 pixfmt)
-{
-	unsigned long dw, bpp;
-	u32 cicr0, cicr1, cicr2, cicr3, cicr4 = 0, y_skip_top;
-	int ret = sensor_call(pcdev, sensor, g_skip_top_lines, &y_skip_top);
-
-	if (ret < 0)
-		y_skip_top = 0;
-
-	/*
-	 * Datawidth is now guaranteed to be equal to one of the three values.
-	 * We fix bit-per-pixel equal to data-width...
-	 */
-	switch (pcdev->current_fmt->host_fmt->bits_per_sample) {
-	case 10:
-		dw = 4;
-		bpp = 0x40;
-		break;
-	case 9:
-		dw = 3;
-		bpp = 0x20;
-		break;
-	default:
-		/*
-		 * Actually it can only be 8 now,
-		 * default is just to silence compiler warnings
-		 */
-	case 8:
-		dw = 2;
-		bpp = 0;
-	}
-
-	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
-		cicr4 |= CICR4_PCLK_EN;
-	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
-		cicr4 |= CICR4_MCLK_EN;
-	if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
-		cicr4 |= CICR4_PCP;
-	if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
-		cicr4 |= CICR4_HSP;
-	if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
-		cicr4 |= CICR4_VSP;
-
-	cicr0 = __raw_readl(pcdev->base + CICR0);
-	if (cicr0 & CICR0_ENB)
-		__raw_writel(cicr0 & ~CICR0_ENB, pcdev->base + CICR0);
-
-	cicr1 = CICR1_PPL_VAL(pcdev->current_pix.width - 1) | bpp | dw;
-
-	switch (pixfmt) {
-	case V4L2_PIX_FMT_YUV422P:
-		pcdev->channels = 3;
-		cicr1 |= CICR1_YCBCR_F;
-		/*
-		 * Normally, pxa bus wants as input UYVY format. We allow all
-		 * reorderings of the YUV422 format, as no processing is done,
-		 * and the YUV stream is just passed through without any
-		 * transformation. Note that UYVY is the only format that
-		 * should be used if pxa framebuffer Overlay2 is used.
-		 */
-	case V4L2_PIX_FMT_UYVY:
-	case V4L2_PIX_FMT_VYUY:
-	case V4L2_PIX_FMT_YUYV:
-	case V4L2_PIX_FMT_YVYU:
-		cicr1 |= CICR1_COLOR_SP_VAL(2);
-		break;
-	case V4L2_PIX_FMT_RGB555:
-		cicr1 |= CICR1_RGB_BPP_VAL(1) | CICR1_RGBT_CONV_VAL(2) |
-			CICR1_TBIT | CICR1_COLOR_SP_VAL(1);
-		break;
-	case V4L2_PIX_FMT_RGB565:
-		cicr1 |= CICR1_COLOR_SP_VAL(1) | CICR1_RGB_BPP_VAL(2);
-		break;
-	}
-
-	cicr2 = 0;
-	cicr3 = CICR3_LPF_VAL(pcdev->current_pix.height - 1) |
-		CICR3_BFW_VAL(min((u32)255, y_skip_top));
-	cicr4 |= pcdev->mclk_divisor;
-
-	__raw_writel(cicr1, pcdev->base + CICR1);
-	__raw_writel(cicr2, pcdev->base + CICR2);
-	__raw_writel(cicr3, pcdev->base + CICR3);
-	__raw_writel(cicr4, pcdev->base + CICR4);
-
-	/* CIF interrupts are not used, only DMA */
-	cicr0 = (cicr0 & CICR0_ENB) | (pcdev->platform_flags & PXA_CAMERA_MASTER ?
-		CICR0_SIM_MP : (CICR0_SL_CAP_EN | CICR0_SIM_SP));
-	cicr0 |= CICR0_DMAEN | CICR0_IRQ_MASK;
-	__raw_writel(cicr0, pcdev->base + CICR0);
-}
-
-/*
- * Videobuf2 section
- */
-static void pxa_buffer_cleanup(struct pxa_buffer *buf)
-{
-	int i;
-
-	for (i = 0; i < 3 && buf->descs[i]; i++) {
-		dmaengine_desc_free(buf->descs[i]);
-		kfree(buf->sg[i]);
-		buf->descs[i] = NULL;
-		buf->sg[i] = NULL;
-		buf->sg_len[i] = 0;
-		buf->plane_sizes[i] = 0;
-	}
-	buf->nb_planes = 0;
-}
-
-static int pxa_buffer_init(struct pxa_camera_dev *pcdev,
-			   struct pxa_buffer *buf)
-{
-	struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
-	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
-	int nb_channels = pcdev->channels;
-	int i, ret = 0;
-	unsigned long size = vb2_plane_size(vb, 0);
-
-	switch (nb_channels) {
-	case 1:
-		buf->plane_sizes[0] = size;
-		break;
-	case 3:
-		buf->plane_sizes[0] = size / 2;
-		buf->plane_sizes[1] = size / 4;
-		buf->plane_sizes[2] = size / 4;
-		break;
-	default:
-		return -EINVAL;
-	};
-	buf->nb_planes = nb_channels;
-
-	ret = sg_split(sgt->sgl, sgt->nents, 0, nb_channels,
-		       buf->plane_sizes, buf->sg, buf->sg_len, GFP_KERNEL);
-	if (ret < 0) {
-		dev_err(pcdev_to_dev(pcdev),
-			"sg_split failed: %d\n", ret);
-		return ret;
-	}
-	for (i = 0; i < nb_channels; i++) {
-		ret = pxa_init_dma_channel(pcdev, buf, i,
-					   buf->sg[i], buf->sg_len[i]);
-		if (ret) {
-			pxa_buffer_cleanup(buf);
-			return ret;
-		}
-	}
-	INIT_LIST_HEAD(&buf->queue);
-
-	return ret;
-}
-
-static void pxac_vb2_cleanup(struct vb2_buffer *vb)
-{
-	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		 "%s(vb=%p)\n", __func__, vb);
-	pxa_buffer_cleanup(buf);
-}
-
-static void pxac_vb2_queue(struct vb2_buffer *vb)
-{
-	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		 "%s(vb=%p) nb_channels=%d size=%lu active=%p\n",
-		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0),
-		pcdev->active);
-
-	list_add_tail(&buf->queue, &pcdev->capture);
-
-	pxa_dma_add_tail_buf(pcdev, buf);
-}
-
-/*
- * Please check the DMA prepared buffer structure in :
- *   Documentation/video4linux/pxa_camera.txt
- * Please check also in pxa_camera_check_link_miss() to understand why DMA chain
- * modification while DMA chain is running will work anyway.
- */
-static int pxac_vb2_prepare(struct vb2_buffer *vb)
-{
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
-	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
-	int ret = 0;
-
-	switch (pcdev->channels) {
-	case 1:
-	case 3:
-		vb2_set_plane_payload(vb, 0, pcdev->current_pix.sizeimage);
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		 "%s (vb=%p) nb_channels=%d size=%lu\n",
-		__func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0));
-
-	WARN_ON(!pcdev->current_fmt);
-
-#ifdef DEBUG
-	/*
-	 * This can be useful if you want to see if we actually fill
-	 * the buffer with something
-	 */
-	for (i = 0; i < vb->num_planes; i++)
-		memset((void *)vb2_plane_vaddr(vb, i),
-		       0xaa, vb2_get_plane_payload(vb, i));
-#endif
-
-	/*
-	 * I think, in buf_prepare you only have to protect global data,
-	 * the actual buffer is yours
-	 */
-	buf->inwork = 0;
-	pxa_videobuf_set_actdma(pcdev, buf);
-
-	return ret;
-}
-
-static int pxac_vb2_init(struct vb2_buffer *vb)
-{
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
-	struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		 "%s(nb_channels=%d)\n",
-		__func__, pcdev->channels);
-
-	return pxa_buffer_init(pcdev, buf);
-}
-
-static int pxac_vb2_queue_setup(struct vb2_queue *vq,
-				unsigned int *nbufs,
-				unsigned int *num_planes, unsigned int sizes[],
-				struct device *alloc_devs[])
-{
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
-	int size = pcdev->current_pix.sizeimage;
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		 "%s(vq=%p nbufs=%d num_planes=%d size=%d)\n",
-		__func__, vq, *nbufs, *num_planes, size);
-	/*
-	 * Called from VIDIOC_REQBUFS or in compatibility mode For YUV422P
-	 * format, even if there are 3 planes Y, U and V, we reply there is only
-	 * one plane, containing Y, U and V data, one after the other.
-	 */
-	if (*num_planes)
-		return sizes[0] < size ? -EINVAL : 0;
-
-	*num_planes = 1;
-	switch (pcdev->channels) {
-	case 1:
-	case 3:
-		sizes[0] = size;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	if (!*nbufs)
-		*nbufs = 1;
-
-	return 0;
-}
-
-static int pxac_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
-{
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
-
-	dev_dbg(pcdev_to_dev(pcdev), "%s(count=%d) active=%p\n",
-		__func__, count, pcdev->active);
-
-	pcdev->buf_sequence = 0;
-	if (!pcdev->active)
-		pxa_camera_start_capture(pcdev);
-
-	return 0;
-}
-
-static void pxac_vb2_stop_streaming(struct vb2_queue *vq)
-{
-	struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
-	struct pxa_buffer *buf, *tmp;
-
-	dev_dbg(pcdev_to_dev(pcdev), "%s active=%p\n",
-		__func__, pcdev->active);
-	pxa_camera_stop_capture(pcdev);
-
-	list_for_each_entry_safe(buf, tmp, &pcdev->capture, queue)
-		pxa_camera_wakeup(pcdev, buf, VB2_BUF_STATE_ERROR);
-}
-
-static struct vb2_ops pxac_vb2_ops = {
-	.queue_setup		= pxac_vb2_queue_setup,
-	.buf_init		= pxac_vb2_init,
-	.buf_prepare		= pxac_vb2_prepare,
-	.buf_queue		= pxac_vb2_queue,
-	.buf_cleanup		= pxac_vb2_cleanup,
-	.start_streaming	= pxac_vb2_start_streaming,
-	.stop_streaming		= pxac_vb2_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
-};
-
-static int pxa_camera_init_videobuf2(struct pxa_camera_dev *pcdev)
-{
-	int ret;
-	struct vb2_queue *vq = &pcdev->vb2_vq;
-
-	memset(vq, 0, sizeof(*vq));
-	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
-	vq->drv_priv = pcdev;
-	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-	vq->buf_struct_size = sizeof(struct pxa_buffer);
-	vq->dev = pcdev->v4l2_dev.dev;
-
-	vq->ops = &pxac_vb2_ops;
-	vq->mem_ops = &vb2_dma_sg_memops;
-	vq->lock = &pcdev->mlock;
-
-	ret = vb2_queue_init(vq);
-	dev_dbg(pcdev_to_dev(pcdev),
-		 "vb2_queue_init(vq=%p): %d\n", vq, ret);
-
-	return ret;
-}
-
-/*
- * Video ioctls section
- */
-static int pxa_camera_set_bus_param(struct pxa_camera_dev *pcdev)
-{
-	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
-	u32 pixfmt = pcdev->current_fmt->host_fmt->fourcc;
-	unsigned long bus_flags, common_flags;
-	int ret;
-
-	ret = test_platform_param(pcdev,
-				  pcdev->current_fmt->host_fmt->bits_per_sample,
-				  &bus_flags);
-	if (ret < 0)
-		return ret;
-
-	ret = sensor_call(pcdev, video, g_mbus_config, &cfg);
-	if (!ret) {
-		common_flags = soc_mbus_config_compatible(&cfg,
-							  bus_flags);
-		if (!common_flags) {
-			dev_warn(pcdev_to_dev(pcdev),
-				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
-				 cfg.flags, bus_flags);
-			return -EINVAL;
-		}
-	} else if (ret != -ENOIOCTLCMD) {
-		return ret;
-	} else {
-		common_flags = bus_flags;
-	}
-
-	pcdev->channels = 1;
-
-	/* Make choises, based on platform preferences */
-	if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
-	    (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
-		if (pcdev->platform_flags & PXA_CAMERA_HSP)
-			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
-		else
-			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
-	}
-
-	if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
-	    (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
-		if (pcdev->platform_flags & PXA_CAMERA_VSP)
-			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
-		else
-			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
-	}
-
-	if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
-	    (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
-		if (pcdev->platform_flags & PXA_CAMERA_PCP)
-			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
-		else
-			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
-	}
-
-	cfg.flags = common_flags;
-	ret = sensor_call(pcdev, video, s_mbus_config, &cfg);
-	if (ret < 0 && ret != -ENOIOCTLCMD) {
-		dev_dbg(pcdev_to_dev(pcdev),
-			"camera s_mbus_config(0x%lx) returned %d\n",
-			common_flags, ret);
-		return ret;
-	}
-
-	pxa_camera_setup_cicr(pcdev, common_flags, pixfmt);
-
-	return 0;
-}
-
-static int pxa_camera_try_bus_param(struct pxa_camera_dev *pcdev,
-				    unsigned char buswidth)
-{
-	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
-	unsigned long bus_flags, common_flags;
-	int ret = test_platform_param(pcdev, buswidth, &bus_flags);
-
-	if (ret < 0)
-		return ret;
-
-	ret = sensor_call(pcdev, video, g_mbus_config, &cfg);
-	if (!ret) {
-		common_flags = soc_mbus_config_compatible(&cfg,
-							  bus_flags);
-		if (!common_flags) {
-			dev_warn(pcdev_to_dev(pcdev),
-				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
-				 cfg.flags, bus_flags);
-			return -EINVAL;
-		}
-	} else if (ret == -ENOIOCTLCMD) {
-		ret = 0;
-	}
-
-	return ret;
-}
-
-static const struct soc_mbus_pixelfmt pxa_camera_formats[] = {
-	{
-		.fourcc			= V4L2_PIX_FMT_YUV422P,
-		.name			= "Planar YUV422 16 bit",
-		.bits_per_sample	= 8,
-		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
-		.order			= SOC_MBUS_ORDER_LE,
-		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_U_V,
-	},
-};
-
-/* This will be corrected as we get more formats */
-static bool pxa_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
-{
-	return	fmt->packing == SOC_MBUS_PACKING_NONE ||
-		(fmt->bits_per_sample == 8 &&
-		 fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
-		(fmt->bits_per_sample > 8 &&
-		 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
-}
-
-static int pxa_camera_get_formats(struct v4l2_device *v4l2_dev,
-				  unsigned int idx,
-				  struct soc_camera_format_xlate *xlate)
-{
-	struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(v4l2_dev);
-	int formats = 0, ret;
-	struct v4l2_subdev_mbus_code_enum code = {
-		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-		.index = idx,
-	};
-	const struct soc_mbus_pixelfmt *fmt;
-
-	ret = sensor_call(pcdev, pad, enum_mbus_code, NULL, &code);
-	if (ret < 0)
-		/* No more formats */
-		return 0;
-
-	fmt = soc_mbus_get_fmtdesc(code.code);
-	if (!fmt) {
-		dev_err(pcdev_to_dev(pcdev),
-			"Invalid format code #%u: %d\n", idx, code.code);
-		return 0;
-	}
-
-	/* This also checks support for the requested bits-per-sample */
-	ret = pxa_camera_try_bus_param(pcdev, fmt->bits_per_sample);
-	if (ret < 0)
-		return 0;
-
-	switch (code.code) {
-	case MEDIA_BUS_FMT_UYVY8_2X8:
-		formats++;
-		if (xlate) {
-			xlate->host_fmt	= &pxa_camera_formats[0];
-			xlate->code	= code.code;
-			xlate++;
-			dev_dbg(pcdev_to_dev(pcdev),
-				"Providing format %s using code %d\n",
-				pxa_camera_formats[0].name, code.code);
-		}
-	case MEDIA_BUS_FMT_VYUY8_2X8:
-	case MEDIA_BUS_FMT_YUYV8_2X8:
-	case MEDIA_BUS_FMT_YVYU8_2X8:
-	case MEDIA_BUS_FMT_RGB565_2X8_LE:
-	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
-		if (xlate)
-			dev_dbg(pcdev_to_dev(pcdev),
-				"Providing format %s packed\n",
-				fmt->name);
-		break;
-	default:
-		if (!pxa_camera_packing_supported(fmt))
-			return 0;
-		if (xlate)
-			dev_dbg(pcdev_to_dev(pcdev),
-				"Providing format %s in pass-through mode\n",
-				fmt->name);
-	}
-
-	/* Generic pass-through */
-	formats++;
-	if (xlate) {
-		xlate->host_fmt	= fmt;
-		xlate->code	= code.code;
-		xlate++;
-	}
-
-	return formats;
-}
-
-static int pxa_camera_build_formats(struct pxa_camera_dev *pcdev)
-{
-	struct soc_camera_format_xlate *xlate;
-
-	xlate = soc_mbus_build_fmts_xlate(&pcdev->v4l2_dev, pcdev->sensor,
-					  pxa_camera_get_formats);
-	if (IS_ERR(xlate))
-		return PTR_ERR(xlate);
-
-	pcdev->user_formats = xlate;
-	return 0;
-}
-
-static void pxa_camera_destroy_formats(struct pxa_camera_dev *pcdev)
-{
-	kfree(pcdev->user_formats);
-}
-
-static int pxa_camera_check_frame(u32 width, u32 height)
-{
-	/* limit to pxa hardware capabilities */
-	return height < 32 || height > 2048 || width < 48 || width > 2048 ||
-		(width & 0x01);
-}
-
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-static int pxac_vidioc_g_register(struct file *file, void *priv,
-				  struct v4l2_dbg_register *reg)
-{
-	struct pxa_camera_dev *pcdev = video_drvdata(file);
-
-	if (reg->reg > CIBR2)
-		return -ERANGE;
-
-	reg->val = __raw_readl(pcdev->base + reg->reg);
-	reg->size = sizeof(__u32);
-	return 0;
-}
-
-static int pxac_vidioc_s_register(struct file *file, void *priv,
-				  const struct v4l2_dbg_register *reg)
-{
-	struct pxa_camera_dev *pcdev = video_drvdata(file);
-
-	if (reg->reg > CIBR2)
-		return -ERANGE;
-	if (reg->size != sizeof(__u32))
-		return -EINVAL;
-	__raw_writel(reg->val, pcdev->base + reg->reg);
-	return 0;
-}
-#endif
-
-static int pxac_vidioc_enum_fmt_vid_cap(struct file *filp, void  *priv,
-					struct v4l2_fmtdesc *f)
-{
-	struct pxa_camera_dev *pcdev = video_drvdata(filp);
-	const struct soc_mbus_pixelfmt *format;
-	unsigned int idx;
-
-	for (idx = 0; pcdev->user_formats[idx].code; idx++);
-	if (f->index >= idx)
-		return -EINVAL;
-
-	format = pcdev->user_formats[f->index].host_fmt;
-	f->pixelformat = format->fourcc;
-	return 0;
-}
-
-static int pxac_vidioc_g_fmt_vid_cap(struct file *filp, void *priv,
-				    struct v4l2_format *f)
-{
-	struct pxa_camera_dev *pcdev = video_drvdata(filp);
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-
-	pix->width		= pcdev->current_pix.width;
-	pix->height		= pcdev->current_pix.height;
-	pix->bytesperline	= pcdev->current_pix.bytesperline;
-	pix->sizeimage		= pcdev->current_pix.sizeimage;
-	pix->field		= pcdev->current_pix.field;
-	pix->pixelformat	= pcdev->current_fmt->host_fmt->fourcc;
-	pix->colorspace		= pcdev->current_pix.colorspace;
-	dev_dbg(pcdev_to_dev(pcdev), "current_fmt->fourcc: 0x%08x\n",
-		pcdev->current_fmt->host_fmt->fourcc);
-	return 0;
-}
-
-static int pxac_vidioc_try_fmt_vid_cap(struct file *filp, void *priv,
-				      struct v4l2_format *f)
-{
-	struct pxa_camera_dev *pcdev = video_drvdata(filp);
-	const struct soc_camera_format_xlate *xlate;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_subdev_pad_config pad_cfg;
-	struct v4l2_subdev_format format = {
-		.which = V4L2_SUBDEV_FORMAT_TRY,
-	};
-	struct v4l2_mbus_framefmt *mf = &format.format;
-	__u32 pixfmt = pix->pixelformat;
-	int ret;
-
-	xlate = soc_mbus_xlate_by_fourcc(pcdev->user_formats, pixfmt);
-	if (!xlate) {
-		dev_warn(pcdev_to_dev(pcdev), "Format %x not found\n", pixfmt);
-		return -EINVAL;
-	}
-
-	/*
-	 * Limit to pxa hardware capabilities.  YUV422P planar format requires
-	 * images size to be a multiple of 16 bytes.  If not, zeros will be
-	 * inserted between Y and U planes, and U and V planes, which violates
-	 * the YUV422P standard.
-	 */
-	v4l_bound_align_image(&pix->width, 48, 2048, 1,
-			      &pix->height, 32, 2048, 0,
-			      pixfmt == V4L2_PIX_FMT_YUV422P ? 4 : 0);
-
-	v4l2_fill_mbus_format(mf, pix, xlate->code);
-	ret = sensor_call(pcdev, pad, set_fmt, &pad_cfg, &format);
-	if (ret < 0)
-		return ret;
-
-	v4l2_fill_pix_format(pix, mf);
-
-	/* Only progressive video supported so far */
-	switch (mf->field) {
-	case V4L2_FIELD_ANY:
-	case V4L2_FIELD_NONE:
-		pix->field = V4L2_FIELD_NONE;
-		break;
-	default:
-		/* TODO: support interlaced at least in pass-through mode */
-		dev_err(pcdev_to_dev(pcdev), "Field type %d unsupported.\n",
-			mf->field);
-		return -EINVAL;
-	}
-
-	ret = soc_mbus_bytes_per_line(pix->width, xlate->host_fmt);
-	if (ret < 0)
-		return ret;
-
-	pix->bytesperline = ret;
-	ret = soc_mbus_image_size(xlate->host_fmt, pix->bytesperline,
-				  pix->height);
-	if (ret < 0)
-		return ret;
-
-	pix->sizeimage = ret;
-	return 0;
-}
-
-static int pxac_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
-				    struct v4l2_format *f)
-{
-	struct pxa_camera_dev *pcdev = video_drvdata(filp);
-	const struct soc_camera_format_xlate *xlate;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_subdev_format format = {
-		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-	};
-	unsigned long flags;
-	int ret, is_busy;
-
-	dev_dbg(pcdev_to_dev(pcdev),
-		"s_fmt_vid_cap(pix=%dx%d:%x)\n",
-		pix->width, pix->height, pix->pixelformat);
-
-	spin_lock_irqsave(&pcdev->lock, flags);
-	is_busy = pcdev->active || vb2_is_busy(&pcdev->vb2_vq);
-	spin_unlock_irqrestore(&pcdev->lock, flags);
-
-	if (is_busy)
-		return -EBUSY;
-
-	ret = pxac_vidioc_try_fmt_vid_cap(filp, priv, f);
-	if (ret)
-		return ret;
-
-	xlate = soc_mbus_xlate_by_fourcc(pcdev->user_formats,
-					 pix->pixelformat);
-	v4l2_fill_mbus_format(&format.format, pix, xlate->code);
-	ret = sensor_call(pcdev, pad, set_fmt, NULL, &format);
-	if (ret < 0) {
-		dev_warn(pcdev_to_dev(pcdev),
-			 "Failed to configure for format %x\n",
-			 pix->pixelformat);
-	} else if (pxa_camera_check_frame(pix->width, pix->height)) {
-		dev_warn(pcdev_to_dev(pcdev),
-			 "Camera driver produced an unsupported frame %dx%d\n",
-			 pix->width, pix->height);
-		return -EINVAL;
-	}
-
-	pcdev->current_fmt = xlate;
-	pcdev->current_pix = *pix;
-
-	ret = pxa_camera_set_bus_param(pcdev);
-	return ret;
-}
-
-static int pxac_vidioc_querycap(struct file *file, void *priv,
-				struct v4l2_capability *cap)
-{
-	strlcpy(cap->bus_info, "platform:pxa-camera", sizeof(cap->bus_info));
-	strlcpy(cap->driver, PXA_CAM_DRV_NAME, sizeof(cap->driver));
-	strlcpy(cap->card, pxa_cam_driver_description, sizeof(cap->card));
-	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
-	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
-
-	return 0;
-}
-
-static int pxac_vidioc_enum_input(struct file *file, void *priv,
-				  struct v4l2_input *i)
-{
-	if (i->index > 0)
-		return -EINVAL;
-
-	i->type = V4L2_INPUT_TYPE_CAMERA;
-	strlcpy(i->name, "Camera", sizeof(i->name));
-
-	return 0;
-}
-
-static int pxac_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
-{
-	*i = 0;
-
-	return 0;
-}
-
-static int pxac_vidioc_s_input(struct file *file, void *priv, unsigned int i)
-{
-	if (i > 0)
-		return -EINVAL;
-
-	return 0;
-}
-
-static int pxac_fops_camera_open(struct file *filp)
-{
-	struct pxa_camera_dev *pcdev = video_drvdata(filp);
-	int ret;
-
-	mutex_lock(&pcdev->mlock);
-	ret = v4l2_fh_open(filp);
-	if (ret < 0)
-		goto out;
-
-	ret = sensor_call(pcdev, core, s_power, 1);
-	if (ret)
-		v4l2_fh_release(filp);
-out:
-	mutex_unlock(&pcdev->mlock);
-	return ret;
-}
-
-static int pxac_fops_camera_release(struct file *filp)
-{
-	struct pxa_camera_dev *pcdev = video_drvdata(filp);
-	int ret;
-
-	ret = vb2_fop_release(filp);
-	if (ret < 0)
-		return ret;
-
-	mutex_lock(&pcdev->mlock);
-	ret = sensor_call(pcdev, core, s_power, 0);
-	mutex_unlock(&pcdev->mlock);
-
-	return ret;
-}
-
-static const struct v4l2_file_operations pxa_camera_fops = {
-	.owner		= THIS_MODULE,
-	.open		= pxac_fops_camera_open,
-	.release	= pxac_fops_camera_release,
-	.read		= vb2_fop_read,
-	.poll		= vb2_fop_poll,
-	.mmap		= vb2_fop_mmap,
-	.unlocked_ioctl = video_ioctl2,
-};
-
-static const struct v4l2_ioctl_ops pxa_camera_ioctl_ops = {
-	.vidioc_querycap		= pxac_vidioc_querycap,
-
-	.vidioc_enum_input		= pxac_vidioc_enum_input,
-	.vidioc_g_input			= pxac_vidioc_g_input,
-	.vidioc_s_input			= pxac_vidioc_s_input,
-
-	.vidioc_enum_fmt_vid_cap	= pxac_vidioc_enum_fmt_vid_cap,
-	.vidioc_g_fmt_vid_cap		= pxac_vidioc_g_fmt_vid_cap,
-	.vidioc_s_fmt_vid_cap		= pxac_vidioc_s_fmt_vid_cap,
-	.vidioc_try_fmt_vid_cap		= pxac_vidioc_try_fmt_vid_cap,
-
-	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
-	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
-	.vidioc_querybuf		= vb2_ioctl_querybuf,
-	.vidioc_qbuf			= vb2_ioctl_qbuf,
-	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
-	.vidioc_expbuf			= vb2_ioctl_expbuf,
-	.vidioc_streamon		= vb2_ioctl_streamon,
-	.vidioc_streamoff		= vb2_ioctl_streamoff,
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-	.vidioc_g_register		= pxac_vidioc_g_register,
-	.vidioc_s_register		= pxac_vidioc_s_register,
-#endif
-};
-
-static struct v4l2_clk_ops pxa_camera_mclk_ops = {
-};
-
-static const struct video_device pxa_camera_videodev_template = {
-	.name = "pxa-camera",
-	.minor = -1,
-	.fops = &pxa_camera_fops,
-	.ioctl_ops = &pxa_camera_ioctl_ops,
-	.release = video_device_release_empty,
-	.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING,
-};
-
-static int pxa_camera_sensor_bound(struct v4l2_async_notifier *notifier,
-		     struct v4l2_subdev *subdev,
-		     struct v4l2_async_subdev *asd)
-{
-	int err;
-	struct v4l2_device *v4l2_dev = notifier->v4l2_dev;
-	struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(v4l2_dev);
-	struct video_device *vdev = &pcdev->vdev;
-	struct v4l2_pix_format *pix = &pcdev->current_pix;
-	struct v4l2_subdev_format format = {
-		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-	};
-	struct v4l2_mbus_framefmt *mf = &format.format;
-
-	dev_info(pcdev_to_dev(pcdev), "%s(): trying to bind a device\n",
-		 __func__);
-	mutex_lock(&pcdev->mlock);
-	*vdev = pxa_camera_videodev_template;
-	vdev->v4l2_dev = v4l2_dev;
-	vdev->lock = &pcdev->mlock;
-	pcdev->sensor = subdev;
-	pcdev->vdev.queue = &pcdev->vb2_vq;
-	pcdev->vdev.v4l2_dev = &pcdev->v4l2_dev;
-	pcdev->vdev.ctrl_handler = subdev->ctrl_handler;
-	video_set_drvdata(&pcdev->vdev, pcdev);
-
-	err = pxa_camera_build_formats(pcdev);
-	if (err) {
-		dev_err(pcdev_to_dev(pcdev), "building formats failed: %d\n",
-			err);
-		goto out;
-	}
-
-	pcdev->current_fmt = pcdev->user_formats;
-	pix->field = V4L2_FIELD_NONE;
-	pix->width = DEFAULT_WIDTH;
-	pix->height = DEFAULT_HEIGHT;
-	pix->bytesperline =
-		soc_mbus_bytes_per_line(pix->width,
-					pcdev->current_fmt->host_fmt);
-	pix->sizeimage =
-		soc_mbus_image_size(pcdev->current_fmt->host_fmt,
-				    pix->bytesperline, pix->height);
-	pix->pixelformat = pcdev->current_fmt->host_fmt->fourcc;
-	v4l2_fill_mbus_format(mf, pix, pcdev->current_fmt->code);
-	err = sensor_call(pcdev, pad, set_fmt, NULL, &format);
-	if (err)
-		goto out;
-
-	v4l2_fill_pix_format(pix, mf);
-	pr_info("%s(): colorspace=0x%x pixfmt=0x%x\n",
-		__func__, pix->colorspace, pix->pixelformat);
-
-	err = pxa_camera_init_videobuf2(pcdev);
-	if (err)
-		goto out;
-
-	err = video_register_device(&pcdev->vdev, VFL_TYPE_GRABBER, -1);
-	if (err) {
-		v4l2_err(v4l2_dev, "register video device failed: %d\n", err);
-		pcdev->sensor = NULL;
-	} else {
-		dev_info(pcdev_to_dev(pcdev),
-			 "PXA Camera driver attached to camera %s\n",
-			 subdev->name);
-	}
-out:
-	mutex_unlock(&pcdev->mlock);
-	return err;
-}
-
-static void pxa_camera_sensor_unbind(struct v4l2_async_notifier *notifier,
-		     struct v4l2_subdev *subdev,
-		     struct v4l2_async_subdev *asd)
-{
-	struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(notifier->v4l2_dev);
-
-	mutex_lock(&pcdev->mlock);
-	dev_info(pcdev_to_dev(pcdev),
-		 "PXA Camera driver detached from camera %s\n",
-		 subdev->name);
-
-	/* disable capture, disable interrupts */
-	__raw_writel(0x3ff, pcdev->base + CICR0);
-
-	/* Stop DMA engine */
-	pxa_dma_stop_channels(pcdev);
-
-	pxa_camera_destroy_formats(pcdev);
-	video_unregister_device(&pcdev->vdev);
-	pcdev->sensor = NULL;
-
-	mutex_unlock(&pcdev->mlock);
-}
-
-/*
- * Driver probe, remove, suspend and resume operations
- */
-static int pxa_camera_suspend(struct device *dev)
-{
-	struct pxa_camera_dev *pcdev = dev_get_drvdata(dev);
-	int i = 0, ret = 0;
-
-	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR0);
-	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR1);
-	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR2);
-	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR3);
-	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR4);
-
-	if (pcdev->sensor) {
-		ret = sensor_call(pcdev, core, s_power, 0);
-		if (ret == -ENOIOCTLCMD)
-			ret = 0;
-	}
-
-	return ret;
-}
-
-static int pxa_camera_resume(struct device *dev)
-{
-	struct pxa_camera_dev *pcdev = dev_get_drvdata(dev);
-	int i = 0, ret = 0;
-
-	__raw_writel(pcdev->save_cicr[i++] & ~CICR0_ENB, pcdev->base + CICR0);
-	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR1);
-	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR2);
-	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR3);
-	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR4);
-
-	if (pcdev->sensor) {
-		ret = sensor_call(pcdev, core, s_power, 1);
-		if (ret == -ENOIOCTLCMD)
-			ret = 0;
-	}
-
-	/* Restart frame capture if active buffer exists */
-	if (!ret && pcdev->active)
-		pxa_camera_start_capture(pcdev);
-
-	return ret;
-}
-
-static int pxa_camera_pdata_from_dt(struct device *dev,
-				    struct pxa_camera_dev *pcdev,
-				    struct v4l2_async_subdev *asd)
-{
-	u32 mclk_rate;
-	struct device_node *remote, *np = dev->of_node;
-	struct v4l2_of_endpoint ep;
-	int err = of_property_read_u32(np, "clock-frequency",
-				       &mclk_rate);
-	if (!err) {
-		pcdev->platform_flags |= PXA_CAMERA_MCLK_EN;
-		pcdev->mclk = mclk_rate;
-	}
-
-	np = of_graph_get_next_endpoint(np, NULL);
-	if (!np) {
-		dev_err(dev, "could not find endpoint\n");
-		return -EINVAL;
-	}
-
-	err = v4l2_of_parse_endpoint(np, &ep);
-	if (err) {
-		dev_err(dev, "could not parse endpoint\n");
-		goto out;
-	}
-
-	switch (ep.bus.parallel.bus_width) {
-	case 4:
-		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_4;
-		break;
-	case 5:
-		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_5;
-		break;
-	case 8:
-		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_8;
-		break;
-	case 9:
-		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_9;
-		break;
-	case 10:
-		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_10;
-		break;
-	default:
-		break;
-	}
-
-	if (ep.bus.parallel.flags & V4L2_MBUS_MASTER)
-		pcdev->platform_flags |= PXA_CAMERA_MASTER;
-	if (ep.bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
-		pcdev->platform_flags |= PXA_CAMERA_HSP;
-	if (ep.bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
-		pcdev->platform_flags |= PXA_CAMERA_VSP;
-	if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
-		pcdev->platform_flags |= PXA_CAMERA_PCLK_EN | PXA_CAMERA_PCP;
-	if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
-		pcdev->platform_flags |= PXA_CAMERA_PCLK_EN;
-
-	asd->match_type = V4L2_ASYNC_MATCH_OF;
-	remote = of_graph_get_remote_port(np);
-	if (remote) {
-		asd->match.of.node = remote;
-		of_node_put(remote);
-	} else {
-		dev_notice(dev, "no remote for %s\n", of_node_full_name(np));
-	}
-
-out:
-	of_node_put(np);
-
-	return err;
-}
-
-static int pxa_camera_probe(struct platform_device *pdev)
-{
-	struct pxa_camera_dev *pcdev;
-	struct resource *res;
-	void __iomem *base;
-	struct dma_slave_config config = {
-		.src_addr_width = 0,
-		.src_maxburst = 8,
-		.direction = DMA_DEV_TO_MEM,
-	};
-	dma_cap_mask_t mask;
-	struct pxad_param params;
-	char clk_name[V4L2_CLK_NAME_SIZE];
-	int irq;
-	int err = 0, i;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	irq = platform_get_irq(pdev, 0);
-	if (!res || irq < 0)
-		return -ENODEV;
-
-	pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
-	if (!pcdev) {
-		dev_err(&pdev->dev, "Could not allocate pcdev\n");
-		return -ENOMEM;
-	}
-
-	pcdev->clk = devm_clk_get(&pdev->dev, NULL);
-	if (IS_ERR(pcdev->clk))
-		return PTR_ERR(pcdev->clk);
-
-	pcdev->res = res;
-
-	pcdev->pdata = pdev->dev.platform_data;
-	if (&pdev->dev.of_node && !pcdev->pdata) {
-		err = pxa_camera_pdata_from_dt(&pdev->dev, pcdev, &pcdev->asd);
-	} else {
-		pcdev->platform_flags = pcdev->pdata->flags;
-		pcdev->mclk = pcdev->pdata->mclk_10khz * 10000;
-		pcdev->asd.match_type = V4L2_ASYNC_MATCH_I2C;
-		pcdev->asd.match.i2c.adapter_id =
-			pcdev->pdata->sensor_i2c_adapter_id;
-		pcdev->asd.match.i2c.address = pcdev->pdata->sensor_i2c_address;
-	}
-	if (err < 0)
-		return err;
-
-	if (!(pcdev->platform_flags & (PXA_CAMERA_DATAWIDTH_8 |
-			PXA_CAMERA_DATAWIDTH_9 | PXA_CAMERA_DATAWIDTH_10))) {
-		/*
-		 * Platform hasn't set available data widths. This is bad.
-		 * Warn and use a default.
-		 */
-		dev_warn(&pdev->dev, "WARNING! Platform hasn't set available "
-			 "data widths, using default 10 bit\n");
-		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_10;
-	}
-	if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_8)
-		pcdev->width_flags = 1 << 7;
-	if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_9)
-		pcdev->width_flags |= 1 << 8;
-	if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_10)
-		pcdev->width_flags |= 1 << 9;
-	if (!pcdev->mclk) {
-		dev_warn(&pdev->dev,
-			 "mclk == 0! Please, fix your platform data. "
-			 "Using default 20MHz\n");
-		pcdev->mclk = 20000000;
-	}
-
-	pcdev->mclk_divisor = mclk_get_divisor(pdev, pcdev);
-
-	INIT_LIST_HEAD(&pcdev->capture);
-	spin_lock_init(&pcdev->lock);
-	mutex_init(&pcdev->mlock);
-
-	/*
-	 * Request the regions.
-	 */
-	base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(base))
-		return PTR_ERR(base);
-
-	pcdev->irq = irq;
-	pcdev->base = base;
-
-	/* request dma */
-	dma_cap_zero(mask);
-	dma_cap_set(DMA_SLAVE, mask);
-	dma_cap_set(DMA_PRIVATE, mask);
-
-	params.prio = 0;
-	params.drcmr = 68;
-	pcdev->dma_chans[0] =
-		dma_request_slave_channel_compat(mask, pxad_filter_fn,
-						 &params, &pdev->dev, "CI_Y");
-	if (!pcdev->dma_chans[0]) {
-		dev_err(&pdev->dev, "Can't request DMA for Y\n");
-		return -ENODEV;
-	}
-
-	params.drcmr = 69;
-	pcdev->dma_chans[1] =
-		dma_request_slave_channel_compat(mask, pxad_filter_fn,
-						 &params, &pdev->dev, "CI_U");
-	if (!pcdev->dma_chans[1]) {
-		dev_err(&pdev->dev, "Can't request DMA for Y\n");
-		goto exit_free_dma_y;
-	}
-
-	params.drcmr = 70;
-	pcdev->dma_chans[2] =
-		dma_request_slave_channel_compat(mask, pxad_filter_fn,
-						 &params, &pdev->dev, "CI_V");
-	if (!pcdev->dma_chans[2]) {
-		dev_err(&pdev->dev, "Can't request DMA for V\n");
-		goto exit_free_dma_u;
-	}
-
-	for (i = 0; i < 3; i++) {
-		config.src_addr = pcdev->res->start + CIBR0 + i * 8;
-		err = dmaengine_slave_config(pcdev->dma_chans[i], &config);
-		if (err < 0) {
-			dev_err(&pdev->dev, "dma slave config failed: %d\n",
-				err);
-			goto exit_free_dma;
-		}
-	}
-
-	/* request irq */
-	err = devm_request_irq(&pdev->dev, pcdev->irq, pxa_camera_irq, 0,
-			       PXA_CAM_DRV_NAME, pcdev);
-	if (err) {
-		dev_err(&pdev->dev, "Camera interrupt register failed\n");
-		goto exit_free_dma;
-	}
-
-	tasklet_init(&pcdev->task_eof, pxa_camera_eof, (unsigned long)pcdev);
-
-	pxa_camera_activate(pcdev);
-
-	dev_set_drvdata(&pdev->dev, pcdev);
-	err = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
-	if (err)
-		goto exit_free_dma;
-
-	pcdev->asds[0] = &pcdev->asd;
-	pcdev->notifier.subdevs = pcdev->asds;
-	pcdev->notifier.num_subdevs = 1;
-	pcdev->notifier.bound = pxa_camera_sensor_bound;
-	pcdev->notifier.unbind = pxa_camera_sensor_unbind;
-
-	if (!of_have_populated_dt())
-		pcdev->asd.match_type = V4L2_ASYNC_MATCH_I2C;
-
-	err = pxa_camera_init_videobuf2(pcdev);
-	if (err)
-		goto exit_free_v4l2dev;
-
-	if (pcdev->mclk) {
-		v4l2_clk_name_i2c(clk_name, sizeof(clk_name),
-				  pcdev->asd.match.i2c.adapter_id,
-				  pcdev->asd.match.i2c.address);
-
-		pcdev->mclk_clk = v4l2_clk_register(&pxa_camera_mclk_ops,
-						    clk_name, NULL);
-		if (IS_ERR(pcdev->mclk_clk))
-			return PTR_ERR(pcdev->mclk_clk);
-	}
-
-	err = v4l2_async_notifier_register(&pcdev->v4l2_dev, &pcdev->notifier);
-	if (err)
-		goto exit_free_clk;
-
-	return 0;
-exit_free_clk:
-	v4l2_clk_unregister(pcdev->mclk_clk);
-exit_free_v4l2dev:
-	v4l2_device_unregister(&pcdev->v4l2_dev);
-exit_free_dma:
-	dma_release_channel(pcdev->dma_chans[2]);
-exit_free_dma_u:
-	dma_release_channel(pcdev->dma_chans[1]);
-exit_free_dma_y:
-	dma_release_channel(pcdev->dma_chans[0]);
-	return err;
-}
-
-static int pxa_camera_remove(struct platform_device *pdev)
-{
-	struct pxa_camera_dev *pcdev = dev_get_drvdata(&pdev->dev);
-
-	pxa_camera_deactivate(pcdev);
-	dma_release_channel(pcdev->dma_chans[0]);
-	dma_release_channel(pcdev->dma_chans[1]);
-	dma_release_channel(pcdev->dma_chans[2]);
-
-	v4l2_clk_unregister(pcdev->mclk_clk);
-	v4l2_device_unregister(&pcdev->v4l2_dev);
-
-	dev_info(&pdev->dev, "PXA Camera driver unloaded\n");
-
-	return 0;
-}
-
-static const struct dev_pm_ops pxa_camera_pm = {
-	.suspend	= pxa_camera_suspend,
-	.resume		= pxa_camera_resume,
-};
-
-static const struct of_device_id pxa_camera_of_match[] = {
-	{ .compatible = "marvell,pxa270-qci", },
-	{},
-};
-MODULE_DEVICE_TABLE(of, pxa_camera_of_match);
-
-static struct platform_driver pxa_camera_driver = {
-	.driver		= {
-		.name	= PXA_CAM_DRV_NAME,
-		.pm	= &pxa_camera_pm,
-		.of_match_table = of_match_ptr(pxa_camera_of_match),
-	},
-	.probe		= pxa_camera_probe,
-	.remove		= pxa_camera_remove,
-};
-
-module_platform_driver(pxa_camera_driver);
-
-MODULE_DESCRIPTION("PXA27x SoC Camera Host driver");
-MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
-MODULE_LICENSE("GPL");
-MODULE_VERSION(PXA_CAM_VERSION);
-MODULE_ALIAS("platform:" PXA_CAM_DRV_NAME);
-- 
2.1.4

Re: [PATCH v5 03/13] media: mt9m111: move mt9m111 out of soc_camera

[Guennadi Liakhovetski]

Hi Robert,

You could use "git format-patch -M" to make this patch much smaller and to 
make it simple to verify, what actually changed in mt9m111.c, if anything. 
Actually it might even be good to merge this patch with patch #1.

Thanks
Guennadi

On Mon, 29 Aug 2016, Robert Jarzmik wrote:

> As the mt9m111 is now working as a standalone v4l2 subdevice sensor,
> move it out of soc_camera directory and severe its dependency on
> soc_camera.
> 
> Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>
> ---
>  drivers/media/i2c/Kconfig              |    7 +
>  drivers/media/i2c/Makefile             |    1 +
>  drivers/media/i2c/mt9m111.c            | 1033 ++++++++++++++++++++++++++++++++
>  drivers/media/i2c/soc_camera/Kconfig   |    7 +-
>  drivers/media/i2c/soc_camera/Makefile  |    1 -
>  drivers/media/i2c/soc_camera/mt9m111.c | 1033 --------------------------------
>  6 files changed, 1046 insertions(+), 1036 deletions(-)
>  create mode 100644 drivers/media/i2c/mt9m111.c
>  delete mode 100644 drivers/media/i2c/soc_camera/mt9m111.c
> 
> diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
> index ce9006e10a30..7f8790507660 100644
> --- a/drivers/media/i2c/Kconfig
> +++ b/drivers/media/i2c/Kconfig
> @@ -571,6 +571,13 @@ config VIDEO_MT9M032
>  	  This driver supports MT9M032 camera sensors from Aptina, monochrome
>  	  models only.
>  
> +config VIDEO_MT9M111
> +	tristate "mt9m111, mt9m112 and mt9m131 support"
> +	depends on I2C && VIDEO_V4L2
> +	help
> +	  This driver supports MT9M111, MT9M112 and MT9M131 cameras from
> +	  Micron/Aptina
> +
>  config VIDEO_MT9P031
>  	tristate "Aptina MT9P031 support"
>  	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
> diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
> index 94f2c99e890d..a1a82331bebc 100644
> --- a/drivers/media/i2c/Makefile
> +++ b/drivers/media/i2c/Makefile
> @@ -59,6 +59,7 @@ obj-$(CONFIG_VIDEO_OV7640) += ov7640.o
>  obj-$(CONFIG_VIDEO_OV7670) += ov7670.o
>  obj-$(CONFIG_VIDEO_OV9650) += ov9650.o
>  obj-$(CONFIG_VIDEO_MT9M032) += mt9m032.o
> +obj-$(CONFIG_VIDEO_MT9M111) += mt9m111.o
>  obj-$(CONFIG_VIDEO_MT9P031) += mt9p031.o
>  obj-$(CONFIG_VIDEO_MT9T001) += mt9t001.o
>  obj-$(CONFIG_VIDEO_MT9V011) += mt9v011.o
> diff --git a/drivers/media/i2c/mt9m111.c b/drivers/media/i2c/mt9m111.c
> new file mode 100644
> index 000000000000..b7c4f371bae1
> --- /dev/null
> +++ b/drivers/media/i2c/mt9m111.c
> @@ -0,0 +1,1033 @@
> +/*
> + * Driver for MT9M111/MT9M112/MT9M131 CMOS Image Sensor from Micron/Aptina
> + *
> + * Copyright (C) 2008, Robert Jarzmik <robert.jarzmik@free.fr>
> + *
> + * 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.
> + */
> +#include <linux/videodev2.h>
> +#include <linux/slab.h>
> +#include <linux/i2c.h>
> +#include <linux/log2.h>
> +#include <linux/gpio.h>
> +#include <linux/delay.h>
> +#include <linux/v4l2-mediabus.h>
> +#include <linux/module.h>
> +
> +#include <media/v4l2-async.h>
> +#include <media/v4l2-clk.h>
> +#include <media/v4l2-common.h>
> +#include <media/v4l2-ctrls.h>
> +#include <media/v4l2-device.h>
> +
> +/*
> + * MT9M111, MT9M112 and MT9M131:
> + * i2c address is 0x48 or 0x5d (depending on SADDR pin)
> + * The platform has to define struct i2c_board_info objects and link to them
> + * from struct soc_camera_host_desc
> + */
> +
> +/*
> + * Sensor core register addresses (0x000..0x0ff)
> + */
> +#define MT9M111_CHIP_VERSION		0x000
> +#define MT9M111_ROW_START		0x001
> +#define MT9M111_COLUMN_START		0x002
> +#define MT9M111_WINDOW_HEIGHT		0x003
> +#define MT9M111_WINDOW_WIDTH		0x004
> +#define MT9M111_HORIZONTAL_BLANKING_B	0x005
> +#define MT9M111_VERTICAL_BLANKING_B	0x006
> +#define MT9M111_HORIZONTAL_BLANKING_A	0x007
> +#define MT9M111_VERTICAL_BLANKING_A	0x008
> +#define MT9M111_SHUTTER_WIDTH		0x009
> +#define MT9M111_ROW_SPEED		0x00a
> +#define MT9M111_EXTRA_DELAY		0x00b
> +#define MT9M111_SHUTTER_DELAY		0x00c
> +#define MT9M111_RESET			0x00d
> +#define MT9M111_READ_MODE_B		0x020
> +#define MT9M111_READ_MODE_A		0x021
> +#define MT9M111_FLASH_CONTROL		0x023
> +#define MT9M111_GREEN1_GAIN		0x02b
> +#define MT9M111_BLUE_GAIN		0x02c
> +#define MT9M111_RED_GAIN		0x02d
> +#define MT9M111_GREEN2_GAIN		0x02e
> +#define MT9M111_GLOBAL_GAIN		0x02f
> +#define MT9M111_CONTEXT_CONTROL		0x0c8
> +#define MT9M111_PAGE_MAP		0x0f0
> +#define MT9M111_BYTE_WISE_ADDR		0x0f1
> +
> +#define MT9M111_RESET_SYNC_CHANGES	(1 << 15)
> +#define MT9M111_RESET_RESTART_BAD_FRAME	(1 << 9)
> +#define MT9M111_RESET_SHOW_BAD_FRAMES	(1 << 8)
> +#define MT9M111_RESET_RESET_SOC		(1 << 5)
> +#define MT9M111_RESET_OUTPUT_DISABLE	(1 << 4)
> +#define MT9M111_RESET_CHIP_ENABLE	(1 << 3)
> +#define MT9M111_RESET_ANALOG_STANDBY	(1 << 2)
> +#define MT9M111_RESET_RESTART_FRAME	(1 << 1)
> +#define MT9M111_RESET_RESET_MODE	(1 << 0)
> +
> +#define MT9M111_RM_FULL_POWER_RD	(0 << 10)
> +#define MT9M111_RM_LOW_POWER_RD		(1 << 10)
> +#define MT9M111_RM_COL_SKIP_4X		(1 << 5)
> +#define MT9M111_RM_ROW_SKIP_4X		(1 << 4)
> +#define MT9M111_RM_COL_SKIP_2X		(1 << 3)
> +#define MT9M111_RM_ROW_SKIP_2X		(1 << 2)
> +#define MT9M111_RMB_MIRROR_COLS		(1 << 1)
> +#define MT9M111_RMB_MIRROR_ROWS		(1 << 0)
> +#define MT9M111_CTXT_CTRL_RESTART	(1 << 15)
> +#define MT9M111_CTXT_CTRL_DEFECTCOR_B	(1 << 12)
> +#define MT9M111_CTXT_CTRL_RESIZE_B	(1 << 10)
> +#define MT9M111_CTXT_CTRL_CTRL2_B	(1 << 9)
> +#define MT9M111_CTXT_CTRL_GAMMA_B	(1 << 8)
> +#define MT9M111_CTXT_CTRL_XENON_EN	(1 << 7)
> +#define MT9M111_CTXT_CTRL_READ_MODE_B	(1 << 3)
> +#define MT9M111_CTXT_CTRL_LED_FLASH_EN	(1 << 2)
> +#define MT9M111_CTXT_CTRL_VBLANK_SEL_B	(1 << 1)
> +#define MT9M111_CTXT_CTRL_HBLANK_SEL_B	(1 << 0)
> +
> +/*
> + * Colorpipe register addresses (0x100..0x1ff)
> + */
> +#define MT9M111_OPER_MODE_CTRL		0x106
> +#define MT9M111_OUTPUT_FORMAT_CTRL	0x108
> +#define MT9M111_REDUCER_XZOOM_B		0x1a0
> +#define MT9M111_REDUCER_XSIZE_B		0x1a1
> +#define MT9M111_REDUCER_YZOOM_B		0x1a3
> +#define MT9M111_REDUCER_YSIZE_B		0x1a4
> +#define MT9M111_REDUCER_XZOOM_A		0x1a6
> +#define MT9M111_REDUCER_XSIZE_A		0x1a7
> +#define MT9M111_REDUCER_YZOOM_A		0x1a9
> +#define MT9M111_REDUCER_YSIZE_A		0x1aa
> +
> +#define MT9M111_OUTPUT_FORMAT_CTRL2_A	0x13a
> +#define MT9M111_OUTPUT_FORMAT_CTRL2_B	0x19b
> +
> +#define MT9M111_OPMODE_AUTOEXPO_EN	(1 << 14)
> +#define MT9M111_OPMODE_AUTOWHITEBAL_EN	(1 << 1)
> +#define MT9M111_OUTFMT_FLIP_BAYER_COL	(1 << 9)
> +#define MT9M111_OUTFMT_FLIP_BAYER_ROW	(1 << 8)
> +#define MT9M111_OUTFMT_PROCESSED_BAYER	(1 << 14)
> +#define MT9M111_OUTFMT_BYPASS_IFP	(1 << 10)
> +#define MT9M111_OUTFMT_INV_PIX_CLOCK	(1 << 9)
> +#define MT9M111_OUTFMT_RGB		(1 << 8)
> +#define MT9M111_OUTFMT_RGB565		(0 << 6)
> +#define MT9M111_OUTFMT_RGB555		(1 << 6)
> +#define MT9M111_OUTFMT_RGB444x		(2 << 6)
> +#define MT9M111_OUTFMT_RGBx444		(3 << 6)
> +#define MT9M111_OUTFMT_TST_RAMP_OFF	(0 << 4)
> +#define MT9M111_OUTFMT_TST_RAMP_COL	(1 << 4)
> +#define MT9M111_OUTFMT_TST_RAMP_ROW	(2 << 4)
> +#define MT9M111_OUTFMT_TST_RAMP_FRAME	(3 << 4)
> +#define MT9M111_OUTFMT_SHIFT_3_UP	(1 << 3)
> +#define MT9M111_OUTFMT_AVG_CHROMA	(1 << 2)
> +#define MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN	(1 << 1)
> +#define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B	(1 << 0)
> +
> +/*
> + * Camera control register addresses (0x200..0x2ff not implemented)
> + */
> +
> +#define reg_read(reg) mt9m111_reg_read(client, MT9M111_##reg)
> +#define reg_write(reg, val) mt9m111_reg_write(client, MT9M111_##reg, (val))
> +#define reg_set(reg, val) mt9m111_reg_set(client, MT9M111_##reg, (val))
> +#define reg_clear(reg, val) mt9m111_reg_clear(client, MT9M111_##reg, (val))
> +#define reg_mask(reg, val, mask) mt9m111_reg_mask(client, MT9M111_##reg, \
> +		(val), (mask))
> +
> +#define MT9M111_MIN_DARK_ROWS	8
> +#define MT9M111_MIN_DARK_COLS	26
> +#define MT9M111_MAX_HEIGHT	1024
> +#define MT9M111_MAX_WIDTH	1280
> +
> +struct mt9m111_context {
> +	u16 read_mode;
> +	u16 blanking_h;
> +	u16 blanking_v;
> +	u16 reducer_xzoom;
> +	u16 reducer_yzoom;
> +	u16 reducer_xsize;
> +	u16 reducer_ysize;
> +	u16 output_fmt_ctrl2;
> +	u16 control;
> +};
> +
> +static struct mt9m111_context context_a = {
> +	.read_mode		= MT9M111_READ_MODE_A,
> +	.blanking_h		= MT9M111_HORIZONTAL_BLANKING_A,
> +	.blanking_v		= MT9M111_VERTICAL_BLANKING_A,
> +	.reducer_xzoom		= MT9M111_REDUCER_XZOOM_A,
> +	.reducer_yzoom		= MT9M111_REDUCER_YZOOM_A,
> +	.reducer_xsize		= MT9M111_REDUCER_XSIZE_A,
> +	.reducer_ysize		= MT9M111_REDUCER_YSIZE_A,
> +	.output_fmt_ctrl2	= MT9M111_OUTPUT_FORMAT_CTRL2_A,
> +	.control		= MT9M111_CTXT_CTRL_RESTART,
> +};
> +
> +static struct mt9m111_context context_b = {
> +	.read_mode		= MT9M111_READ_MODE_B,
> +	.blanking_h		= MT9M111_HORIZONTAL_BLANKING_B,
> +	.blanking_v		= MT9M111_VERTICAL_BLANKING_B,
> +	.reducer_xzoom		= MT9M111_REDUCER_XZOOM_B,
> +	.reducer_yzoom		= MT9M111_REDUCER_YZOOM_B,
> +	.reducer_xsize		= MT9M111_REDUCER_XSIZE_B,
> +	.reducer_ysize		= MT9M111_REDUCER_YSIZE_B,
> +	.output_fmt_ctrl2	= MT9M111_OUTPUT_FORMAT_CTRL2_B,
> +	.control		= MT9M111_CTXT_CTRL_RESTART |
> +		MT9M111_CTXT_CTRL_DEFECTCOR_B | MT9M111_CTXT_CTRL_RESIZE_B |
> +		MT9M111_CTXT_CTRL_CTRL2_B | MT9M111_CTXT_CTRL_GAMMA_B |
> +		MT9M111_CTXT_CTRL_READ_MODE_B | MT9M111_CTXT_CTRL_VBLANK_SEL_B |
> +		MT9M111_CTXT_CTRL_HBLANK_SEL_B,
> +};
> +
> +/* MT9M111 has only one fixed colorspace per pixelcode */
> +struct mt9m111_datafmt {
> +	u32	code;
> +	enum v4l2_colorspace		colorspace;
> +};
> +
> +static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
> +	{MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB},
> +	{MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_SRGB},
> +	{MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB},
> +	{MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_SRGB},
> +	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
> +	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
> +	{MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
> +	{MEDIA_BUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
> +	{MEDIA_BUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB},
> +	{MEDIA_BUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB},
> +	{MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
> +	{MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
> +};
> +
> +struct mt9m111 {
> +	struct v4l2_subdev subdev;
> +	struct v4l2_ctrl_handler hdl;
> +	struct v4l2_ctrl *gain;
> +	struct mt9m111_context *ctx;
> +	struct v4l2_rect rect;	/* cropping rectangle */
> +	struct v4l2_clk *clk;
> +	unsigned int width;	/* output */
> +	unsigned int height;	/* sizes */
> +	struct mutex power_lock; /* lock to protect power_count */
> +	int power_count;
> +	const struct mt9m111_datafmt *fmt;
> +	int lastpage;	/* PageMap cache value */
> +};
> +
> +/* Find a data format by a pixel code */
> +static const struct mt9m111_datafmt *mt9m111_find_datafmt(struct mt9m111 *mt9m111,
> +						u32 code)
> +{
> +	int i;
> +	for (i = 0; i < ARRAY_SIZE(mt9m111_colour_fmts); i++)
> +		if (mt9m111_colour_fmts[i].code == code)
> +			return mt9m111_colour_fmts + i;
> +
> +	return mt9m111->fmt;
> +}
> +
> +static struct mt9m111 *to_mt9m111(const struct i2c_client *client)
> +{
> +	return container_of(i2c_get_clientdata(client), struct mt9m111, subdev);
> +}
> +
> +static int reg_page_map_set(struct i2c_client *client, const u16 reg)
> +{
> +	int ret;
> +	u16 page;
> +	struct mt9m111 *mt9m111 = to_mt9m111(client);
> +
> +	page = (reg >> 8);
> +	if (page == mt9m111->lastpage)
> +		return 0;
> +	if (page > 2)
> +		return -EINVAL;
> +
> +	ret = i2c_smbus_write_word_swapped(client, MT9M111_PAGE_MAP, page);
> +	if (!ret)
> +		mt9m111->lastpage = page;
> +	return ret;
> +}
> +
> +static int mt9m111_reg_read(struct i2c_client *client, const u16 reg)
> +{
> +	int ret;
> +
> +	ret = reg_page_map_set(client, reg);
> +	if (!ret)
> +		ret = i2c_smbus_read_word_swapped(client, reg & 0xff);
> +
> +	dev_dbg(&client->dev, "read  reg.%03x -> %04x\n", reg, ret);
> +	return ret;
> +}
> +
> +static int mt9m111_reg_write(struct i2c_client *client, const u16 reg,
> +			     const u16 data)
> +{
> +	int ret;
> +
> +	ret = reg_page_map_set(client, reg);
> +	if (!ret)
> +		ret = i2c_smbus_write_word_swapped(client, reg & 0xff, data);
> +	dev_dbg(&client->dev, "write reg.%03x = %04x -> %d\n", reg, data, ret);
> +	return ret;
> +}
> +
> +static int mt9m111_reg_set(struct i2c_client *client, const u16 reg,
> +			   const u16 data)
> +{
> +	int ret;
> +
> +	ret = mt9m111_reg_read(client, reg);
> +	if (ret >= 0)
> +		ret = mt9m111_reg_write(client, reg, ret | data);
> +	return ret;
> +}
> +
> +static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg,
> +			     const u16 data)
> +{
> +	int ret;
> +
> +	ret = mt9m111_reg_read(client, reg);
> +	if (ret >= 0)
> +		ret = mt9m111_reg_write(client, reg, ret & ~data);
> +	return ret;
> +}
> +
> +static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg,
> +			    const u16 data, const u16 mask)
> +{
> +	int ret;
> +
> +	ret = mt9m111_reg_read(client, reg);
> +	if (ret >= 0)
> +		ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data);
> +	return ret;
> +}
> +
> +static int mt9m111_set_context(struct mt9m111 *mt9m111,
> +			       struct mt9m111_context *ctx)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +	return reg_write(CONTEXT_CONTROL, ctx->control);
> +}
> +
> +static int mt9m111_setup_rect_ctx(struct mt9m111 *mt9m111,
> +			struct mt9m111_context *ctx, struct v4l2_rect *rect,
> +			unsigned int width, unsigned int height)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +	int ret = mt9m111_reg_write(client, ctx->reducer_xzoom, rect->width);
> +	if (!ret)
> +		ret = mt9m111_reg_write(client, ctx->reducer_yzoom, rect->height);
> +	if (!ret)
> +		ret = mt9m111_reg_write(client, ctx->reducer_xsize, width);
> +	if (!ret)
> +		ret = mt9m111_reg_write(client, ctx->reducer_ysize, height);
> +	return ret;
> +}
> +
> +static int mt9m111_setup_geometry(struct mt9m111 *mt9m111, struct v4l2_rect *rect,
> +			int width, int height, u32 code)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +	int ret;
> +
> +	ret = reg_write(COLUMN_START, rect->left);
> +	if (!ret)
> +		ret = reg_write(ROW_START, rect->top);
> +
> +	if (!ret)
> +		ret = reg_write(WINDOW_WIDTH, rect->width);
> +	if (!ret)
> +		ret = reg_write(WINDOW_HEIGHT, rect->height);
> +
> +	if (code != MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
> +		/* IFP in use, down-scaling possible */
> +		if (!ret)
> +			ret = mt9m111_setup_rect_ctx(mt9m111, &context_b,
> +						     rect, width, height);
> +		if (!ret)
> +			ret = mt9m111_setup_rect_ctx(mt9m111, &context_a,
> +						     rect, width, height);
> +	}
> +
> +	dev_dbg(&client->dev, "%s(%x): %ux%u@%u:%u -> %ux%u = %d\n",
> +		__func__, code, rect->width, rect->height, rect->left, rect->top,
> +		width, height, ret);
> +
> +	return ret;
> +}
> +
> +static int mt9m111_enable(struct mt9m111 *mt9m111)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +	return reg_write(RESET, MT9M111_RESET_CHIP_ENABLE);
> +}
> +
> +static int mt9m111_reset(struct mt9m111 *mt9m111)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +	int ret;
> +
> +	ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
> +	if (!ret)
> +		ret = reg_set(RESET, MT9M111_RESET_RESET_SOC);
> +	if (!ret)
> +		ret = reg_clear(RESET, MT9M111_RESET_RESET_MODE
> +				| MT9M111_RESET_RESET_SOC);
> +
> +	return ret;
> +}
> +
> +static int mt9m111_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
> +{
> +	struct v4l2_rect rect = a->c;
> +	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
> +	int width, height;
> +	int ret, align = 0;
> +
> +	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
> +		return -EINVAL;
> +
> +	if (mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
> +	    mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
> +		/* Bayer format - even size lengths */
> +		align = 1;
> +		/* Let the user play with the starting pixel */
> +	}
> +
> +	/* FIXME: the datasheet doesn't specify minimum sizes */
> +	v4l_bound_align_image(&rect.width, 2, MT9M111_MAX_WIDTH, align,
> +			      &rect.height, 2, MT9M111_MAX_HEIGHT, align, 0);
> +	rect.left = clamp(rect.left, MT9M111_MIN_DARK_COLS,
> +			  MT9M111_MIN_DARK_COLS + MT9M111_MAX_WIDTH -
> +			  (__s32)rect.width);
> +	rect.top = clamp(rect.top, MT9M111_MIN_DARK_ROWS,
> +			 MT9M111_MIN_DARK_ROWS + MT9M111_MAX_HEIGHT -
> +			 (__s32)rect.height);
> +
> +	width = min(mt9m111->width, rect.width);
> +	height = min(mt9m111->height, rect.height);
> +
> +	ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
> +	if (!ret) {
> +		mt9m111->rect = rect;
> +		mt9m111->width = width;
> +		mt9m111->height = height;
> +	}
> +
> +	return ret;
> +}
> +
> +static int mt9m111_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
> +{
> +	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
> +
> +	a->c	= mt9m111->rect;
> +	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
> +
> +	return 0;
> +}
> +
> +static int mt9m111_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
> +{
> +	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
> +		return -EINVAL;
> +
> +	a->bounds.left			= MT9M111_MIN_DARK_COLS;
> +	a->bounds.top			= MT9M111_MIN_DARK_ROWS;
> +	a->bounds.width			= MT9M111_MAX_WIDTH;
> +	a->bounds.height		= MT9M111_MAX_HEIGHT;
> +	a->defrect			= a->bounds;
> +	a->pixelaspect.numerator	= 1;
> +	a->pixelaspect.denominator	= 1;
> +
> +	return 0;
> +}
> +
> +static int mt9m111_get_fmt(struct v4l2_subdev *sd,
> +		struct v4l2_subdev_pad_config *cfg,
> +		struct v4l2_subdev_format *format)
> +{
> +	struct v4l2_mbus_framefmt *mf = &format->format;
> +	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
> +
> +	if (format->pad)
> +		return -EINVAL;
> +
> +	mf->width	= mt9m111->width;
> +	mf->height	= mt9m111->height;
> +	mf->code	= mt9m111->fmt->code;
> +	mf->colorspace	= mt9m111->fmt->colorspace;
> +	mf->field	= V4L2_FIELD_NONE;
> +
> +	return 0;
> +}
> +
> +static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
> +			      u32 code)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +	u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
> +		MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB |
> +		MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
> +		MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 |
> +		MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
> +		MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
> +	int ret;
> +
> +	switch (code) {
> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
> +		data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
> +			MT9M111_OUTFMT_RGB;
> +		break;
> +	case MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE:
> +		data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
> +		break;
> +	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
> +		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
> +			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
> +		break;
> +	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
> +		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
> +		break;
> +	case MEDIA_BUS_FMT_RGB565_2X8_LE:
> +		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
> +			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
> +		break;
> +	case MEDIA_BUS_FMT_RGB565_2X8_BE:
> +		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
> +		break;
> +	case MEDIA_BUS_FMT_BGR565_2X8_BE:
> +		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
> +			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
> +		break;
> +	case MEDIA_BUS_FMT_BGR565_2X8_LE:
> +		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
> +			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
> +			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
> +		break;
> +	case MEDIA_BUS_FMT_UYVY8_2X8:
> +		data_outfmt2 = 0;
> +		break;
> +	case MEDIA_BUS_FMT_VYUY8_2X8:
> +		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
> +		break;
> +	case MEDIA_BUS_FMT_YUYV8_2X8:
> +		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
> +		break;
> +	case MEDIA_BUS_FMT_YVYU8_2X8:
> +		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
> +			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
> +		break;
> +	default:
> +		dev_err(&client->dev, "Pixel format not handled: %x\n", code);
> +		return -EINVAL;
> +	}
> +
> +	ret = mt9m111_reg_mask(client, context_a.output_fmt_ctrl2,
> +			       data_outfmt2, mask_outfmt2);
> +	if (!ret)
> +		ret = mt9m111_reg_mask(client, context_b.output_fmt_ctrl2,
> +				       data_outfmt2, mask_outfmt2);
> +
> +	return ret;
> +}
> +
> +static int mt9m111_set_fmt(struct v4l2_subdev *sd,
> +		struct v4l2_subdev_pad_config *cfg,
> +		struct v4l2_subdev_format *format)
> +{
> +	struct v4l2_mbus_framefmt *mf = &format->format;
> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
> +	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
> +	const struct mt9m111_datafmt *fmt;
> +	struct v4l2_rect *rect = &mt9m111->rect;
> +	bool bayer;
> +	int ret;
> +
> +	if (format->pad)
> +		return -EINVAL;
> +
> +	fmt = mt9m111_find_datafmt(mt9m111, mf->code);
> +
> +	bayer = fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
> +		fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE;
> +
> +	/*
> +	 * With Bayer format enforce even side lengths, but let the user play
> +	 * with the starting pixel
> +	 */
> +	if (bayer) {
> +		rect->width = ALIGN(rect->width, 2);
> +		rect->height = ALIGN(rect->height, 2);
> +	}
> +
> +	if (fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
> +		/* IFP bypass mode, no scaling */
> +		mf->width = rect->width;
> +		mf->height = rect->height;
> +	} else {
> +		/* No upscaling */
> +		if (mf->width > rect->width)
> +			mf->width = rect->width;
> +		if (mf->height > rect->height)
> +			mf->height = rect->height;
> +	}
> +
> +	dev_dbg(&client->dev, "%s(): %ux%u, code=%x\n", __func__,
> +		mf->width, mf->height, fmt->code);
> +
> +	mf->code = fmt->code;
> +	mf->colorspace = fmt->colorspace;
> +
> +	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
> +		cfg->try_fmt = *mf;
> +		return 0;
> +	}
> +
> +	ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
> +	if (!ret)
> +		ret = mt9m111_set_pixfmt(mt9m111, mf->code);
> +	if (!ret) {
> +		mt9m111->width	= mf->width;
> +		mt9m111->height	= mf->height;
> +		mt9m111->fmt	= fmt;
> +	}
> +
> +	return ret;
> +}
> +
> +#ifdef CONFIG_VIDEO_ADV_DEBUG
> +static int mt9m111_g_register(struct v4l2_subdev *sd,
> +			      struct v4l2_dbg_register *reg)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
> +	int val;
> +
> +	if (reg->reg > 0x2ff)
> +		return -EINVAL;
> +
> +	val = mt9m111_reg_read(client, reg->reg);
> +	reg->size = 2;
> +	reg->val = (u64)val;
> +
> +	if (reg->val > 0xffff)
> +		return -EIO;
> +
> +	return 0;
> +}
> +
> +static int mt9m111_s_register(struct v4l2_subdev *sd,
> +			      const struct v4l2_dbg_register *reg)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
> +
> +	if (reg->reg > 0x2ff)
> +		return -EINVAL;
> +
> +	if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
> +		return -EIO;
> +
> +	return 0;
> +}
> +#endif
> +
> +static int mt9m111_set_flip(struct mt9m111 *mt9m111, int flip, int mask)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +	int ret;
> +
> +	if (flip)
> +		ret = mt9m111_reg_set(client, mt9m111->ctx->read_mode, mask);
> +	else
> +		ret = mt9m111_reg_clear(client, mt9m111->ctx->read_mode, mask);
> +
> +	return ret;
> +}
> +
> +static int mt9m111_get_global_gain(struct mt9m111 *mt9m111)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +	int data;
> +
> +	data = reg_read(GLOBAL_GAIN);
> +	if (data >= 0)
> +		return (data & 0x2f) * (1 << ((data >> 10) & 1)) *
> +			(1 << ((data >> 9) & 1));
> +	return data;
> +}
> +
> +static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +	u16 val;
> +
> +	if (gain > 63 * 2 * 2)
> +		return -EINVAL;
> +
> +	if ((gain >= 64 * 2) && (gain < 63 * 2 * 2))
> +		val = (1 << 10) | (1 << 9) | (gain / 4);
> +	else if ((gain >= 64) && (gain < 64 * 2))
> +		val = (1 << 9) | (gain / 2);
> +	else
> +		val = gain;
> +
> +	return reg_write(GLOBAL_GAIN, val);
> +}
> +
> +static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int val)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +
> +	if (val == V4L2_EXPOSURE_AUTO)
> +		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
> +	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
> +}
> +
> +static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +
> +	if (on)
> +		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
> +	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
> +}
> +
> +static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
> +{
> +	struct mt9m111 *mt9m111 = container_of(ctrl->handler,
> +					       struct mt9m111, hdl);
> +
> +	switch (ctrl->id) {
> +	case V4L2_CID_VFLIP:
> +		return mt9m111_set_flip(mt9m111, ctrl->val,
> +					MT9M111_RMB_MIRROR_ROWS);
> +	case V4L2_CID_HFLIP:
> +		return mt9m111_set_flip(mt9m111, ctrl->val,
> +					MT9M111_RMB_MIRROR_COLS);
> +	case V4L2_CID_GAIN:
> +		return mt9m111_set_global_gain(mt9m111, ctrl->val);
> +	case V4L2_CID_EXPOSURE_AUTO:
> +		return mt9m111_set_autoexposure(mt9m111, ctrl->val);
> +	case V4L2_CID_AUTO_WHITE_BALANCE:
> +		return mt9m111_set_autowhitebalance(mt9m111, ctrl->val);
> +	}
> +
> +	return -EINVAL;
> +}
> +
> +static int mt9m111_suspend(struct mt9m111 *mt9m111)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +	int ret;
> +
> +	v4l2_ctrl_s_ctrl(mt9m111->gain, mt9m111_get_global_gain(mt9m111));
> +
> +	ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
> +	if (!ret)
> +		ret = reg_set(RESET, MT9M111_RESET_RESET_SOC |
> +			      MT9M111_RESET_OUTPUT_DISABLE |
> +			      MT9M111_RESET_ANALOG_STANDBY);
> +	if (!ret)
> +		ret = reg_clear(RESET, MT9M111_RESET_CHIP_ENABLE);
> +
> +	return ret;
> +}
> +
> +static void mt9m111_restore_state(struct mt9m111 *mt9m111)
> +{
> +	mt9m111_set_context(mt9m111, mt9m111->ctx);
> +	mt9m111_set_pixfmt(mt9m111, mt9m111->fmt->code);
> +	mt9m111_setup_geometry(mt9m111, &mt9m111->rect,
> +			mt9m111->width, mt9m111->height, mt9m111->fmt->code);
> +	v4l2_ctrl_handler_setup(&mt9m111->hdl);
> +}
> +
> +static int mt9m111_resume(struct mt9m111 *mt9m111)
> +{
> +	int ret = mt9m111_enable(mt9m111);
> +	if (!ret)
> +		ret = mt9m111_reset(mt9m111);
> +	if (!ret)
> +		mt9m111_restore_state(mt9m111);
> +
> +	return ret;
> +}
> +
> +static int mt9m111_init(struct mt9m111 *mt9m111)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +	int ret;
> +
> +	ret = mt9m111_enable(mt9m111);
> +	if (!ret)
> +		ret = mt9m111_reset(mt9m111);
> +	if (!ret)
> +		ret = mt9m111_set_context(mt9m111, mt9m111->ctx);
> +	if (ret)
> +		dev_err(&client->dev, "mt9m111 init failed: %d\n", ret);
> +	return ret;
> +}
> +
> +static int mt9m111_power_on(struct mt9m111 *mt9m111)
> +{
> +	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> +	int ret;
> +
> +	ret = v4l2_clk_enable(mt9m111->clk);
> +	if (ret < 0)
> +		return ret;
> +
> +	ret = mt9m111_resume(mt9m111);
> +	if (ret < 0) {
> +		dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
> +		v4l2_clk_disable(mt9m111->clk);
> +	}
> +
> +	return ret;
> +}
> +
> +static void mt9m111_power_off(struct mt9m111 *mt9m111)
> +{
> +	mt9m111_suspend(mt9m111);
> +	v4l2_clk_disable(mt9m111->clk);
> +}
> +
> +static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
> +{
> +	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
> +	int ret = 0;
> +
> +	mutex_lock(&mt9m111->power_lock);
> +
> +	/*
> +	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
> +	 * update the power state.
> +	 */
> +	if (mt9m111->power_count == !on) {
> +		if (on)
> +			ret = mt9m111_power_on(mt9m111);
> +		else
> +			mt9m111_power_off(mt9m111);
> +	}
> +
> +	if (!ret) {
> +		/* Update the power count. */
> +		mt9m111->power_count += on ? 1 : -1;
> +		WARN_ON(mt9m111->power_count < 0);
> +	}
> +
> +	mutex_unlock(&mt9m111->power_lock);
> +	return ret;
> +}
> +
> +static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
> +	.s_ctrl = mt9m111_s_ctrl,
> +};
> +
> +static struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
> +	.s_power	= mt9m111_s_power,
> +#ifdef CONFIG_VIDEO_ADV_DEBUG
> +	.g_register	= mt9m111_g_register,
> +	.s_register	= mt9m111_s_register,
> +#endif
> +};
> +
> +static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
> +		struct v4l2_subdev_pad_config *cfg,
> +		struct v4l2_subdev_mbus_code_enum *code)
> +{
> +	if (code->pad || code->index >= ARRAY_SIZE(mt9m111_colour_fmts))
> +		return -EINVAL;
> +
> +	code->code = mt9m111_colour_fmts[code->index].code;
> +	return 0;
> +}
> +
> +static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
> +				struct v4l2_mbus_config *cfg)
> +{
> +	cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
> +		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
> +		V4L2_MBUS_DATA_ACTIVE_HIGH;
> +	cfg->type = V4L2_MBUS_PARALLEL;
> +
> +	return 0;
> +}
> +
> +static struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
> +	.s_crop		= mt9m111_s_crop,
> +	.g_crop		= mt9m111_g_crop,
> +	.cropcap	= mt9m111_cropcap,
> +	.g_mbus_config	= mt9m111_g_mbus_config,
> +};
> +
> +static const struct v4l2_subdev_pad_ops mt9m111_subdev_pad_ops = {
> +	.enum_mbus_code = mt9m111_enum_mbus_code,
> +	.get_fmt	= mt9m111_get_fmt,
> +	.set_fmt	= mt9m111_set_fmt,
> +};
> +
> +static struct v4l2_subdev_ops mt9m111_subdev_ops = {
> +	.core	= &mt9m111_subdev_core_ops,
> +	.video	= &mt9m111_subdev_video_ops,
> +	.pad	= &mt9m111_subdev_pad_ops,
> +};
> +
> +/*
> + * Interface active, can use i2c. If it fails, it can indeed mean, that
> + * this wasn't our capture interface, so, we wait for the right one
> + */
> +static int mt9m111_video_probe(struct i2c_client *client)
> +{
> +	struct mt9m111 *mt9m111 = to_mt9m111(client);
> +	s32 data;
> +	int ret;
> +
> +	ret = mt9m111_s_power(&mt9m111->subdev, 1);
> +	if (ret < 0)
> +		return ret;
> +
> +	data = reg_read(CHIP_VERSION);
> +
> +	switch (data) {
> +	case 0x143a: /* MT9M111 or MT9M131 */
> +		dev_info(&client->dev,
> +			"Detected a MT9M111/MT9M131 chip ID %x\n", data);
> +		break;
> +	case 0x148c: /* MT9M112 */
> +		dev_info(&client->dev, "Detected a MT9M112 chip ID %x\n", data);
> +		break;
> +	default:
> +		dev_err(&client->dev,
> +			"No MT9M111/MT9M112/MT9M131 chip detected register read %x\n",
> +			data);
> +		ret = -ENODEV;
> +		goto done;
> +	}
> +
> +	ret = mt9m111_init(mt9m111);
> +	if (ret)
> +		goto done;
> +
> +	ret = v4l2_ctrl_handler_setup(&mt9m111->hdl);
> +
> +done:
> +	mt9m111_s_power(&mt9m111->subdev, 0);
> +	return ret;
> +}
> +
> +static int mt9m111_probe(struct i2c_client *client,
> +			 const struct i2c_device_id *did)
> +{
> +	struct mt9m111 *mt9m111;
> +	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
> +	int ret;
> +
> +	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
> +		dev_warn(&adapter->dev,
> +			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
> +		return -EIO;
> +	}
> +
> +	mt9m111 = devm_kzalloc(&client->dev, sizeof(struct mt9m111), GFP_KERNEL);
> +	if (!mt9m111)
> +		return -ENOMEM;
> +
> +	mt9m111->clk = v4l2_clk_get(&client->dev, "mclk");
> +	if (IS_ERR(mt9m111->clk))
> +		return -EPROBE_DEFER;
> +
> +	/* Default HIGHPOWER context */
> +	mt9m111->ctx = &context_b;
> +
> +	v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
> +	v4l2_ctrl_handler_init(&mt9m111->hdl, 5);
> +	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
> +			V4L2_CID_VFLIP, 0, 1, 1, 0);
> +	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
> +			V4L2_CID_HFLIP, 0, 1, 1, 0);
> +	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
> +			V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
> +	mt9m111->gain = v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
> +			V4L2_CID_GAIN, 0, 63 * 2 * 2, 1, 32);
> +	v4l2_ctrl_new_std_menu(&mt9m111->hdl,
> +			&mt9m111_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
> +			V4L2_EXPOSURE_AUTO);
> +	mt9m111->subdev.ctrl_handler = &mt9m111->hdl;
> +	if (mt9m111->hdl.error) {
> +		ret = mt9m111->hdl.error;
> +		goto out_clkput;
> +	}
> +
> +	/* Second stage probe - when a capture adapter is there */
> +	mt9m111->rect.left	= MT9M111_MIN_DARK_COLS;
> +	mt9m111->rect.top	= MT9M111_MIN_DARK_ROWS;
> +	mt9m111->rect.width	= MT9M111_MAX_WIDTH;
> +	mt9m111->rect.height	= MT9M111_MAX_HEIGHT;
> +	mt9m111->fmt		= &mt9m111_colour_fmts[0];
> +	mt9m111->lastpage	= -1;
> +	mutex_init(&mt9m111->power_lock);
> +
> +	ret = mt9m111_video_probe(client);
> +	if (ret < 0)
> +		goto out_hdlfree;
> +
> +	mt9m111->subdev.dev = &client->dev;
> +	ret = v4l2_async_register_subdev(&mt9m111->subdev);
> +	if (ret < 0)
> +		goto out_hdlfree;
> +
> +	return 0;
> +
> +out_hdlfree:
> +	v4l2_ctrl_handler_free(&mt9m111->hdl);
> +out_clkput:
> +	v4l2_clk_put(mt9m111->clk);
> +
> +	return ret;
> +}
> +
> +static int mt9m111_remove(struct i2c_client *client)
> +{
> +	struct mt9m111 *mt9m111 = to_mt9m111(client);
> +
> +	v4l2_async_unregister_subdev(&mt9m111->subdev);
> +	v4l2_clk_put(mt9m111->clk);
> +	v4l2_ctrl_handler_free(&mt9m111->hdl);
> +
> +	return 0;
> +}
> +static const struct of_device_id mt9m111_of_match[] = {
> +	{ .compatible = "micron,mt9m111", },
> +	{},
> +};
> +MODULE_DEVICE_TABLE(of, mt9m111_of_match);
> +
> +static const struct i2c_device_id mt9m111_id[] = {
> +	{ "mt9m111", 0 },
> +	{ }
> +};
> +MODULE_DEVICE_TABLE(i2c, mt9m111_id);
> +
> +static struct i2c_driver mt9m111_i2c_driver = {
> +	.driver = {
> +		.name = "mt9m111",
> +		.of_match_table = of_match_ptr(mt9m111_of_match),
> +	},
> +	.probe		= mt9m111_probe,
> +	.remove		= mt9m111_remove,
> +	.id_table	= mt9m111_id,
> +};
> +
> +module_i2c_driver(mt9m111_i2c_driver);
> +
> +MODULE_DESCRIPTION("Micron/Aptina MT9M111/MT9M112/MT9M131 Camera driver");
> +MODULE_AUTHOR("Robert Jarzmik");
> +MODULE_LICENSE("GPL");
> diff --git a/drivers/media/i2c/soc_camera/Kconfig b/drivers/media/i2c/soc_camera/Kconfig
> index 23d352f0adf0..7704bcf5cc25 100644
> --- a/drivers/media/i2c/soc_camera/Kconfig
> +++ b/drivers/media/i2c/soc_camera/Kconfig
> @@ -14,11 +14,14 @@ config SOC_CAMERA_MT9M001
>  	  and colour models.
>  
>  config SOC_CAMERA_MT9M111
> -	tristate "mt9m111, mt9m112 and mt9m131 support"
> +	tristate "legacy soc_camera mt9m111, mt9m112 and mt9m131 support"
>  	depends on SOC_CAMERA && I2C
> +	select VIDEO_MT9M111
>  	help
>  	  This driver supports MT9M111, MT9M112 and MT9M131 cameras from
> -	  Micron/Aptina
> +	  Micron/Aptina.
> +	  This is the legacy configuration which shouldn't be used anymore,
> +	  while VIDEO_MT9M111 should be used instead.
>  
>  config SOC_CAMERA_MT9T031
>  	tristate "mt9t031 support"
> diff --git a/drivers/media/i2c/soc_camera/Makefile b/drivers/media/i2c/soc_camera/Makefile
> index d0421feaa796..6f994f9353a0 100644
> --- a/drivers/media/i2c/soc_camera/Makefile
> +++ b/drivers/media/i2c/soc_camera/Makefile
> @@ -1,6 +1,5 @@
>  obj-$(CONFIG_SOC_CAMERA_IMX074)		+= imx074.o
>  obj-$(CONFIG_SOC_CAMERA_MT9M001)	+= mt9m001.o
> -obj-$(CONFIG_SOC_CAMERA_MT9M111)	+= mt9m111.o
>  obj-$(CONFIG_SOC_CAMERA_MT9T031)	+= mt9t031.o
>  obj-$(CONFIG_SOC_CAMERA_MT9T112)	+= mt9t112.o
>  obj-$(CONFIG_SOC_CAMERA_MT9V022)	+= mt9v022.o
> diff --git a/drivers/media/i2c/soc_camera/mt9m111.c b/drivers/media/i2c/soc_camera/mt9m111.c
> deleted file mode 100644
> index b7c4f371bae1..000000000000
> --- a/drivers/media/i2c/soc_camera/mt9m111.c
> +++ /dev/null
> @@ -1,1033 +0,0 @@
> -/*
> - * Driver for MT9M111/MT9M112/MT9M131 CMOS Image Sensor from Micron/Aptina
> - *
> - * Copyright (C) 2008, Robert Jarzmik <robert.jarzmik@free.fr>
> - *
> - * 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.
> - */
> -#include <linux/videodev2.h>
> -#include <linux/slab.h>
> -#include <linux/i2c.h>
> -#include <linux/log2.h>
> -#include <linux/gpio.h>
> -#include <linux/delay.h>
> -#include <linux/v4l2-mediabus.h>
> -#include <linux/module.h>
> -
> -#include <media/v4l2-async.h>
> -#include <media/v4l2-clk.h>
> -#include <media/v4l2-common.h>
> -#include <media/v4l2-ctrls.h>
> -#include <media/v4l2-device.h>
> -
> -/*
> - * MT9M111, MT9M112 and MT9M131:
> - * i2c address is 0x48 or 0x5d (depending on SADDR pin)
> - * The platform has to define struct i2c_board_info objects and link to them
> - * from struct soc_camera_host_desc
> - */
> -
> -/*
> - * Sensor core register addresses (0x000..0x0ff)
> - */
> -#define MT9M111_CHIP_VERSION		0x000
> -#define MT9M111_ROW_START		0x001
> -#define MT9M111_COLUMN_START		0x002
> -#define MT9M111_WINDOW_HEIGHT		0x003
> -#define MT9M111_WINDOW_WIDTH		0x004
> -#define MT9M111_HORIZONTAL_BLANKING_B	0x005
> -#define MT9M111_VERTICAL_BLANKING_B	0x006
> -#define MT9M111_HORIZONTAL_BLANKING_A	0x007
> -#define MT9M111_VERTICAL_BLANKING_A	0x008
> -#define MT9M111_SHUTTER_WIDTH		0x009
> -#define MT9M111_ROW_SPEED		0x00a
> -#define MT9M111_EXTRA_DELAY		0x00b
> -#define MT9M111_SHUTTER_DELAY		0x00c
> -#define MT9M111_RESET			0x00d
> -#define MT9M111_READ_MODE_B		0x020
> -#define MT9M111_READ_MODE_A		0x021
> -#define MT9M111_FLASH_CONTROL		0x023
> -#define MT9M111_GREEN1_GAIN		0x02b
> -#define MT9M111_BLUE_GAIN		0x02c
> -#define MT9M111_RED_GAIN		0x02d
> -#define MT9M111_GREEN2_GAIN		0x02e
> -#define MT9M111_GLOBAL_GAIN		0x02f
> -#define MT9M111_CONTEXT_CONTROL		0x0c8
> -#define MT9M111_PAGE_MAP		0x0f0
> -#define MT9M111_BYTE_WISE_ADDR		0x0f1
> -
> -#define MT9M111_RESET_SYNC_CHANGES	(1 << 15)
> -#define MT9M111_RESET_RESTART_BAD_FRAME	(1 << 9)
> -#define MT9M111_RESET_SHOW_BAD_FRAMES	(1 << 8)
> -#define MT9M111_RESET_RESET_SOC		(1 << 5)
> -#define MT9M111_RESET_OUTPUT_DISABLE	(1 << 4)
> -#define MT9M111_RESET_CHIP_ENABLE	(1 << 3)
> -#define MT9M111_RESET_ANALOG_STANDBY	(1 << 2)
> -#define MT9M111_RESET_RESTART_FRAME	(1 << 1)
> -#define MT9M111_RESET_RESET_MODE	(1 << 0)
> -
> -#define MT9M111_RM_FULL_POWER_RD	(0 << 10)
> -#define MT9M111_RM_LOW_POWER_RD		(1 << 10)
> -#define MT9M111_RM_COL_SKIP_4X		(1 << 5)
> -#define MT9M111_RM_ROW_SKIP_4X		(1 << 4)
> -#define MT9M111_RM_COL_SKIP_2X		(1 << 3)
> -#define MT9M111_RM_ROW_SKIP_2X		(1 << 2)
> -#define MT9M111_RMB_MIRROR_COLS		(1 << 1)
> -#define MT9M111_RMB_MIRROR_ROWS		(1 << 0)
> -#define MT9M111_CTXT_CTRL_RESTART	(1 << 15)
> -#define MT9M111_CTXT_CTRL_DEFECTCOR_B	(1 << 12)
> -#define MT9M111_CTXT_CTRL_RESIZE_B	(1 << 10)
> -#define MT9M111_CTXT_CTRL_CTRL2_B	(1 << 9)
> -#define MT9M111_CTXT_CTRL_GAMMA_B	(1 << 8)
> -#define MT9M111_CTXT_CTRL_XENON_EN	(1 << 7)
> -#define MT9M111_CTXT_CTRL_READ_MODE_B	(1 << 3)
> -#define MT9M111_CTXT_CTRL_LED_FLASH_EN	(1 << 2)
> -#define MT9M111_CTXT_CTRL_VBLANK_SEL_B	(1 << 1)
> -#define MT9M111_CTXT_CTRL_HBLANK_SEL_B	(1 << 0)
> -
> -/*
> - * Colorpipe register addresses (0x100..0x1ff)
> - */
> -#define MT9M111_OPER_MODE_CTRL		0x106
> -#define MT9M111_OUTPUT_FORMAT_CTRL	0x108
> -#define MT9M111_REDUCER_XZOOM_B		0x1a0
> -#define MT9M111_REDUCER_XSIZE_B		0x1a1
> -#define MT9M111_REDUCER_YZOOM_B		0x1a3
> -#define MT9M111_REDUCER_YSIZE_B		0x1a4
> -#define MT9M111_REDUCER_XZOOM_A		0x1a6
> -#define MT9M111_REDUCER_XSIZE_A		0x1a7
> -#define MT9M111_REDUCER_YZOOM_A		0x1a9
> -#define MT9M111_REDUCER_YSIZE_A		0x1aa
> -
> -#define MT9M111_OUTPUT_FORMAT_CTRL2_A	0x13a
> -#define MT9M111_OUTPUT_FORMAT_CTRL2_B	0x19b
> -
> -#define MT9M111_OPMODE_AUTOEXPO_EN	(1 << 14)
> -#define MT9M111_OPMODE_AUTOWHITEBAL_EN	(1 << 1)
> -#define MT9M111_OUTFMT_FLIP_BAYER_COL	(1 << 9)
> -#define MT9M111_OUTFMT_FLIP_BAYER_ROW	(1 << 8)
> -#define MT9M111_OUTFMT_PROCESSED_BAYER	(1 << 14)
> -#define MT9M111_OUTFMT_BYPASS_IFP	(1 << 10)
> -#define MT9M111_OUTFMT_INV_PIX_CLOCK	(1 << 9)
> -#define MT9M111_OUTFMT_RGB		(1 << 8)
> -#define MT9M111_OUTFMT_RGB565		(0 << 6)
> -#define MT9M111_OUTFMT_RGB555		(1 << 6)
> -#define MT9M111_OUTFMT_RGB444x		(2 << 6)
> -#define MT9M111_OUTFMT_RGBx444		(3 << 6)
> -#define MT9M111_OUTFMT_TST_RAMP_OFF	(0 << 4)
> -#define MT9M111_OUTFMT_TST_RAMP_COL	(1 << 4)
> -#define MT9M111_OUTFMT_TST_RAMP_ROW	(2 << 4)
> -#define MT9M111_OUTFMT_TST_RAMP_FRAME	(3 << 4)
> -#define MT9M111_OUTFMT_SHIFT_3_UP	(1 << 3)
> -#define MT9M111_OUTFMT_AVG_CHROMA	(1 << 2)
> -#define MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN	(1 << 1)
> -#define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B	(1 << 0)
> -
> -/*
> - * Camera control register addresses (0x200..0x2ff not implemented)
> - */
> -
> -#define reg_read(reg) mt9m111_reg_read(client, MT9M111_##reg)
> -#define reg_write(reg, val) mt9m111_reg_write(client, MT9M111_##reg, (val))
> -#define reg_set(reg, val) mt9m111_reg_set(client, MT9M111_##reg, (val))
> -#define reg_clear(reg, val) mt9m111_reg_clear(client, MT9M111_##reg, (val))
> -#define reg_mask(reg, val, mask) mt9m111_reg_mask(client, MT9M111_##reg, \
> -		(val), (mask))
> -
> -#define MT9M111_MIN_DARK_ROWS	8
> -#define MT9M111_MIN_DARK_COLS	26
> -#define MT9M111_MAX_HEIGHT	1024
> -#define MT9M111_MAX_WIDTH	1280
> -
> -struct mt9m111_context {
> -	u16 read_mode;
> -	u16 blanking_h;
> -	u16 blanking_v;
> -	u16 reducer_xzoom;
> -	u16 reducer_yzoom;
> -	u16 reducer_xsize;
> -	u16 reducer_ysize;
> -	u16 output_fmt_ctrl2;
> -	u16 control;
> -};
> -
> -static struct mt9m111_context context_a = {
> -	.read_mode		= MT9M111_READ_MODE_A,
> -	.blanking_h		= MT9M111_HORIZONTAL_BLANKING_A,
> -	.blanking_v		= MT9M111_VERTICAL_BLANKING_A,
> -	.reducer_xzoom		= MT9M111_REDUCER_XZOOM_A,
> -	.reducer_yzoom		= MT9M111_REDUCER_YZOOM_A,
> -	.reducer_xsize		= MT9M111_REDUCER_XSIZE_A,
> -	.reducer_ysize		= MT9M111_REDUCER_YSIZE_A,
> -	.output_fmt_ctrl2	= MT9M111_OUTPUT_FORMAT_CTRL2_A,
> -	.control		= MT9M111_CTXT_CTRL_RESTART,
> -};
> -
> -static struct mt9m111_context context_b = {
> -	.read_mode		= MT9M111_READ_MODE_B,
> -	.blanking_h		= MT9M111_HORIZONTAL_BLANKING_B,
> -	.blanking_v		= MT9M111_VERTICAL_BLANKING_B,
> -	.reducer_xzoom		= MT9M111_REDUCER_XZOOM_B,
> -	.reducer_yzoom		= MT9M111_REDUCER_YZOOM_B,
> -	.reducer_xsize		= MT9M111_REDUCER_XSIZE_B,
> -	.reducer_ysize		= MT9M111_REDUCER_YSIZE_B,
> -	.output_fmt_ctrl2	= MT9M111_OUTPUT_FORMAT_CTRL2_B,
> -	.control		= MT9M111_CTXT_CTRL_RESTART |
> -		MT9M111_CTXT_CTRL_DEFECTCOR_B | MT9M111_CTXT_CTRL_RESIZE_B |
> -		MT9M111_CTXT_CTRL_CTRL2_B | MT9M111_CTXT_CTRL_GAMMA_B |
> -		MT9M111_CTXT_CTRL_READ_MODE_B | MT9M111_CTXT_CTRL_VBLANK_SEL_B |
> -		MT9M111_CTXT_CTRL_HBLANK_SEL_B,
> -};
> -
> -/* MT9M111 has only one fixed colorspace per pixelcode */
> -struct mt9m111_datafmt {
> -	u32	code;
> -	enum v4l2_colorspace		colorspace;
> -};
> -
> -static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
> -	{MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB},
> -	{MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_SRGB},
> -	{MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB},
> -	{MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_SRGB},
> -	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
> -	{MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
> -	{MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
> -	{MEDIA_BUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
> -	{MEDIA_BUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB},
> -	{MEDIA_BUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB},
> -	{MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
> -	{MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
> -};
> -
> -struct mt9m111 {
> -	struct v4l2_subdev subdev;
> -	struct v4l2_ctrl_handler hdl;
> -	struct v4l2_ctrl *gain;
> -	struct mt9m111_context *ctx;
> -	struct v4l2_rect rect;	/* cropping rectangle */
> -	struct v4l2_clk *clk;
> -	unsigned int width;	/* output */
> -	unsigned int height;	/* sizes */
> -	struct mutex power_lock; /* lock to protect power_count */
> -	int power_count;
> -	const struct mt9m111_datafmt *fmt;
> -	int lastpage;	/* PageMap cache value */
> -};
> -
> -/* Find a data format by a pixel code */
> -static const struct mt9m111_datafmt *mt9m111_find_datafmt(struct mt9m111 *mt9m111,
> -						u32 code)
> -{
> -	int i;
> -	for (i = 0; i < ARRAY_SIZE(mt9m111_colour_fmts); i++)
> -		if (mt9m111_colour_fmts[i].code == code)
> -			return mt9m111_colour_fmts + i;
> -
> -	return mt9m111->fmt;
> -}
> -
> -static struct mt9m111 *to_mt9m111(const struct i2c_client *client)
> -{
> -	return container_of(i2c_get_clientdata(client), struct mt9m111, subdev);
> -}
> -
> -static int reg_page_map_set(struct i2c_client *client, const u16 reg)
> -{
> -	int ret;
> -	u16 page;
> -	struct mt9m111 *mt9m111 = to_mt9m111(client);
> -
> -	page = (reg >> 8);
> -	if (page == mt9m111->lastpage)
> -		return 0;
> -	if (page > 2)
> -		return -EINVAL;
> -
> -	ret = i2c_smbus_write_word_swapped(client, MT9M111_PAGE_MAP, page);
> -	if (!ret)
> -		mt9m111->lastpage = page;
> -	return ret;
> -}
> -
> -static int mt9m111_reg_read(struct i2c_client *client, const u16 reg)
> -{
> -	int ret;
> -
> -	ret = reg_page_map_set(client, reg);
> -	if (!ret)
> -		ret = i2c_smbus_read_word_swapped(client, reg & 0xff);
> -
> -	dev_dbg(&client->dev, "read  reg.%03x -> %04x\n", reg, ret);
> -	return ret;
> -}
> -
> -static int mt9m111_reg_write(struct i2c_client *client, const u16 reg,
> -			     const u16 data)
> -{
> -	int ret;
> -
> -	ret = reg_page_map_set(client, reg);
> -	if (!ret)
> -		ret = i2c_smbus_write_word_swapped(client, reg & 0xff, data);
> -	dev_dbg(&client->dev, "write reg.%03x = %04x -> %d\n", reg, data, ret);
> -	return ret;
> -}
> -
> -static int mt9m111_reg_set(struct i2c_client *client, const u16 reg,
> -			   const u16 data)
> -{
> -	int ret;
> -
> -	ret = mt9m111_reg_read(client, reg);
> -	if (ret >= 0)
> -		ret = mt9m111_reg_write(client, reg, ret | data);
> -	return ret;
> -}
> -
> -static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg,
> -			     const u16 data)
> -{
> -	int ret;
> -
> -	ret = mt9m111_reg_read(client, reg);
> -	if (ret >= 0)
> -		ret = mt9m111_reg_write(client, reg, ret & ~data);
> -	return ret;
> -}
> -
> -static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg,
> -			    const u16 data, const u16 mask)
> -{
> -	int ret;
> -
> -	ret = mt9m111_reg_read(client, reg);
> -	if (ret >= 0)
> -		ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data);
> -	return ret;
> -}
> -
> -static int mt9m111_set_context(struct mt9m111 *mt9m111,
> -			       struct mt9m111_context *ctx)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	return reg_write(CONTEXT_CONTROL, ctx->control);
> -}
> -
> -static int mt9m111_setup_rect_ctx(struct mt9m111 *mt9m111,
> -			struct mt9m111_context *ctx, struct v4l2_rect *rect,
> -			unsigned int width, unsigned int height)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	int ret = mt9m111_reg_write(client, ctx->reducer_xzoom, rect->width);
> -	if (!ret)
> -		ret = mt9m111_reg_write(client, ctx->reducer_yzoom, rect->height);
> -	if (!ret)
> -		ret = mt9m111_reg_write(client, ctx->reducer_xsize, width);
> -	if (!ret)
> -		ret = mt9m111_reg_write(client, ctx->reducer_ysize, height);
> -	return ret;
> -}
> -
> -static int mt9m111_setup_geometry(struct mt9m111 *mt9m111, struct v4l2_rect *rect,
> -			int width, int height, u32 code)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	int ret;
> -
> -	ret = reg_write(COLUMN_START, rect->left);
> -	if (!ret)
> -		ret = reg_write(ROW_START, rect->top);
> -
> -	if (!ret)
> -		ret = reg_write(WINDOW_WIDTH, rect->width);
> -	if (!ret)
> -		ret = reg_write(WINDOW_HEIGHT, rect->height);
> -
> -	if (code != MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
> -		/* IFP in use, down-scaling possible */
> -		if (!ret)
> -			ret = mt9m111_setup_rect_ctx(mt9m111, &context_b,
> -						     rect, width, height);
> -		if (!ret)
> -			ret = mt9m111_setup_rect_ctx(mt9m111, &context_a,
> -						     rect, width, height);
> -	}
> -
> -	dev_dbg(&client->dev, "%s(%x): %ux%u@%u:%u -> %ux%u = %d\n",
> -		__func__, code, rect->width, rect->height, rect->left, rect->top,
> -		width, height, ret);
> -
> -	return ret;
> -}
> -
> -static int mt9m111_enable(struct mt9m111 *mt9m111)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	return reg_write(RESET, MT9M111_RESET_CHIP_ENABLE);
> -}
> -
> -static int mt9m111_reset(struct mt9m111 *mt9m111)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	int ret;
> -
> -	ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
> -	if (!ret)
> -		ret = reg_set(RESET, MT9M111_RESET_RESET_SOC);
> -	if (!ret)
> -		ret = reg_clear(RESET, MT9M111_RESET_RESET_MODE
> -				| MT9M111_RESET_RESET_SOC);
> -
> -	return ret;
> -}
> -
> -static int mt9m111_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
> -{
> -	struct v4l2_rect rect = a->c;
> -	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
> -	int width, height;
> -	int ret, align = 0;
> -
> -	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
> -		return -EINVAL;
> -
> -	if (mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
> -	    mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
> -		/* Bayer format - even size lengths */
> -		align = 1;
> -		/* Let the user play with the starting pixel */
> -	}
> -
> -	/* FIXME: the datasheet doesn't specify minimum sizes */
> -	v4l_bound_align_image(&rect.width, 2, MT9M111_MAX_WIDTH, align,
> -			      &rect.height, 2, MT9M111_MAX_HEIGHT, align, 0);
> -	rect.left = clamp(rect.left, MT9M111_MIN_DARK_COLS,
> -			  MT9M111_MIN_DARK_COLS + MT9M111_MAX_WIDTH -
> -			  (__s32)rect.width);
> -	rect.top = clamp(rect.top, MT9M111_MIN_DARK_ROWS,
> -			 MT9M111_MIN_DARK_ROWS + MT9M111_MAX_HEIGHT -
> -			 (__s32)rect.height);
> -
> -	width = min(mt9m111->width, rect.width);
> -	height = min(mt9m111->height, rect.height);
> -
> -	ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
> -	if (!ret) {
> -		mt9m111->rect = rect;
> -		mt9m111->width = width;
> -		mt9m111->height = height;
> -	}
> -
> -	return ret;
> -}
> -
> -static int mt9m111_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
> -{
> -	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
> -
> -	a->c	= mt9m111->rect;
> -	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
> -
> -	return 0;
> -}
> -
> -static int mt9m111_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
> -{
> -	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
> -		return -EINVAL;
> -
> -	a->bounds.left			= MT9M111_MIN_DARK_COLS;
> -	a->bounds.top			= MT9M111_MIN_DARK_ROWS;
> -	a->bounds.width			= MT9M111_MAX_WIDTH;
> -	a->bounds.height		= MT9M111_MAX_HEIGHT;
> -	a->defrect			= a->bounds;
> -	a->pixelaspect.numerator	= 1;
> -	a->pixelaspect.denominator	= 1;
> -
> -	return 0;
> -}
> -
> -static int mt9m111_get_fmt(struct v4l2_subdev *sd,
> -		struct v4l2_subdev_pad_config *cfg,
> -		struct v4l2_subdev_format *format)
> -{
> -	struct v4l2_mbus_framefmt *mf = &format->format;
> -	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
> -
> -	if (format->pad)
> -		return -EINVAL;
> -
> -	mf->width	= mt9m111->width;
> -	mf->height	= mt9m111->height;
> -	mf->code	= mt9m111->fmt->code;
> -	mf->colorspace	= mt9m111->fmt->colorspace;
> -	mf->field	= V4L2_FIELD_NONE;
> -
> -	return 0;
> -}
> -
> -static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
> -			      u32 code)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
> -		MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB |
> -		MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
> -		MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 |
> -		MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
> -		MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
> -	int ret;
> -
> -	switch (code) {
> -	case MEDIA_BUS_FMT_SBGGR8_1X8:
> -		data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
> -			MT9M111_OUTFMT_RGB;
> -		break;
> -	case MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE:
> -		data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
> -		break;
> -	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
> -		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
> -			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
> -		break;
> -	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
> -		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
> -		break;
> -	case MEDIA_BUS_FMT_RGB565_2X8_LE:
> -		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
> -			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
> -		break;
> -	case MEDIA_BUS_FMT_RGB565_2X8_BE:
> -		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
> -		break;
> -	case MEDIA_BUS_FMT_BGR565_2X8_BE:
> -		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
> -			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
> -		break;
> -	case MEDIA_BUS_FMT_BGR565_2X8_LE:
> -		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
> -			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
> -			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
> -		break;
> -	case MEDIA_BUS_FMT_UYVY8_2X8:
> -		data_outfmt2 = 0;
> -		break;
> -	case MEDIA_BUS_FMT_VYUY8_2X8:
> -		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
> -		break;
> -	case MEDIA_BUS_FMT_YUYV8_2X8:
> -		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
> -		break;
> -	case MEDIA_BUS_FMT_YVYU8_2X8:
> -		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
> -			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
> -		break;
> -	default:
> -		dev_err(&client->dev, "Pixel format not handled: %x\n", code);
> -		return -EINVAL;
> -	}
> -
> -	ret = mt9m111_reg_mask(client, context_a.output_fmt_ctrl2,
> -			       data_outfmt2, mask_outfmt2);
> -	if (!ret)
> -		ret = mt9m111_reg_mask(client, context_b.output_fmt_ctrl2,
> -				       data_outfmt2, mask_outfmt2);
> -
> -	return ret;
> -}
> -
> -static int mt9m111_set_fmt(struct v4l2_subdev *sd,
> -		struct v4l2_subdev_pad_config *cfg,
> -		struct v4l2_subdev_format *format)
> -{
> -	struct v4l2_mbus_framefmt *mf = &format->format;
> -	struct i2c_client *client = v4l2_get_subdevdata(sd);
> -	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
> -	const struct mt9m111_datafmt *fmt;
> -	struct v4l2_rect *rect = &mt9m111->rect;
> -	bool bayer;
> -	int ret;
> -
> -	if (format->pad)
> -		return -EINVAL;
> -
> -	fmt = mt9m111_find_datafmt(mt9m111, mf->code);
> -
> -	bayer = fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
> -		fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE;
> -
> -	/*
> -	 * With Bayer format enforce even side lengths, but let the user play
> -	 * with the starting pixel
> -	 */
> -	if (bayer) {
> -		rect->width = ALIGN(rect->width, 2);
> -		rect->height = ALIGN(rect->height, 2);
> -	}
> -
> -	if (fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
> -		/* IFP bypass mode, no scaling */
> -		mf->width = rect->width;
> -		mf->height = rect->height;
> -	} else {
> -		/* No upscaling */
> -		if (mf->width > rect->width)
> -			mf->width = rect->width;
> -		if (mf->height > rect->height)
> -			mf->height = rect->height;
> -	}
> -
> -	dev_dbg(&client->dev, "%s(): %ux%u, code=%x\n", __func__,
> -		mf->width, mf->height, fmt->code);
> -
> -	mf->code = fmt->code;
> -	mf->colorspace = fmt->colorspace;
> -
> -	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
> -		cfg->try_fmt = *mf;
> -		return 0;
> -	}
> -
> -	ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
> -	if (!ret)
> -		ret = mt9m111_set_pixfmt(mt9m111, mf->code);
> -	if (!ret) {
> -		mt9m111->width	= mf->width;
> -		mt9m111->height	= mf->height;
> -		mt9m111->fmt	= fmt;
> -	}
> -
> -	return ret;
> -}
> -
> -#ifdef CONFIG_VIDEO_ADV_DEBUG
> -static int mt9m111_g_register(struct v4l2_subdev *sd,
> -			      struct v4l2_dbg_register *reg)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(sd);
> -	int val;
> -
> -	if (reg->reg > 0x2ff)
> -		return -EINVAL;
> -
> -	val = mt9m111_reg_read(client, reg->reg);
> -	reg->size = 2;
> -	reg->val = (u64)val;
> -
> -	if (reg->val > 0xffff)
> -		return -EIO;
> -
> -	return 0;
> -}
> -
> -static int mt9m111_s_register(struct v4l2_subdev *sd,
> -			      const struct v4l2_dbg_register *reg)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(sd);
> -
> -	if (reg->reg > 0x2ff)
> -		return -EINVAL;
> -
> -	if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
> -		return -EIO;
> -
> -	return 0;
> -}
> -#endif
> -
> -static int mt9m111_set_flip(struct mt9m111 *mt9m111, int flip, int mask)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	int ret;
> -
> -	if (flip)
> -		ret = mt9m111_reg_set(client, mt9m111->ctx->read_mode, mask);
> -	else
> -		ret = mt9m111_reg_clear(client, mt9m111->ctx->read_mode, mask);
> -
> -	return ret;
> -}
> -
> -static int mt9m111_get_global_gain(struct mt9m111 *mt9m111)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	int data;
> -
> -	data = reg_read(GLOBAL_GAIN);
> -	if (data >= 0)
> -		return (data & 0x2f) * (1 << ((data >> 10) & 1)) *
> -			(1 << ((data >> 9) & 1));
> -	return data;
> -}
> -
> -static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	u16 val;
> -
> -	if (gain > 63 * 2 * 2)
> -		return -EINVAL;
> -
> -	if ((gain >= 64 * 2) && (gain < 63 * 2 * 2))
> -		val = (1 << 10) | (1 << 9) | (gain / 4);
> -	else if ((gain >= 64) && (gain < 64 * 2))
> -		val = (1 << 9) | (gain / 2);
> -	else
> -		val = gain;
> -
> -	return reg_write(GLOBAL_GAIN, val);
> -}
> -
> -static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int val)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -
> -	if (val == V4L2_EXPOSURE_AUTO)
> -		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
> -	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
> -}
> -
> -static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -
> -	if (on)
> -		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
> -	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
> -}
> -
> -static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
> -{
> -	struct mt9m111 *mt9m111 = container_of(ctrl->handler,
> -					       struct mt9m111, hdl);
> -
> -	switch (ctrl->id) {
> -	case V4L2_CID_VFLIP:
> -		return mt9m111_set_flip(mt9m111, ctrl->val,
> -					MT9M111_RMB_MIRROR_ROWS);
> -	case V4L2_CID_HFLIP:
> -		return mt9m111_set_flip(mt9m111, ctrl->val,
> -					MT9M111_RMB_MIRROR_COLS);
> -	case V4L2_CID_GAIN:
> -		return mt9m111_set_global_gain(mt9m111, ctrl->val);
> -	case V4L2_CID_EXPOSURE_AUTO:
> -		return mt9m111_set_autoexposure(mt9m111, ctrl->val);
> -	case V4L2_CID_AUTO_WHITE_BALANCE:
> -		return mt9m111_set_autowhitebalance(mt9m111, ctrl->val);
> -	}
> -
> -	return -EINVAL;
> -}
> -
> -static int mt9m111_suspend(struct mt9m111 *mt9m111)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	int ret;
> -
> -	v4l2_ctrl_s_ctrl(mt9m111->gain, mt9m111_get_global_gain(mt9m111));
> -
> -	ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
> -	if (!ret)
> -		ret = reg_set(RESET, MT9M111_RESET_RESET_SOC |
> -			      MT9M111_RESET_OUTPUT_DISABLE |
> -			      MT9M111_RESET_ANALOG_STANDBY);
> -	if (!ret)
> -		ret = reg_clear(RESET, MT9M111_RESET_CHIP_ENABLE);
> -
> -	return ret;
> -}
> -
> -static void mt9m111_restore_state(struct mt9m111 *mt9m111)
> -{
> -	mt9m111_set_context(mt9m111, mt9m111->ctx);
> -	mt9m111_set_pixfmt(mt9m111, mt9m111->fmt->code);
> -	mt9m111_setup_geometry(mt9m111, &mt9m111->rect,
> -			mt9m111->width, mt9m111->height, mt9m111->fmt->code);
> -	v4l2_ctrl_handler_setup(&mt9m111->hdl);
> -}
> -
> -static int mt9m111_resume(struct mt9m111 *mt9m111)
> -{
> -	int ret = mt9m111_enable(mt9m111);
> -	if (!ret)
> -		ret = mt9m111_reset(mt9m111);
> -	if (!ret)
> -		mt9m111_restore_state(mt9m111);
> -
> -	return ret;
> -}
> -
> -static int mt9m111_init(struct mt9m111 *mt9m111)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	int ret;
> -
> -	ret = mt9m111_enable(mt9m111);
> -	if (!ret)
> -		ret = mt9m111_reset(mt9m111);
> -	if (!ret)
> -		ret = mt9m111_set_context(mt9m111, mt9m111->ctx);
> -	if (ret)
> -		dev_err(&client->dev, "mt9m111 init failed: %d\n", ret);
> -	return ret;
> -}
> -
> -static int mt9m111_power_on(struct mt9m111 *mt9m111)
> -{
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	int ret;
> -
> -	ret = v4l2_clk_enable(mt9m111->clk);
> -	if (ret < 0)
> -		return ret;
> -
> -	ret = mt9m111_resume(mt9m111);
> -	if (ret < 0) {
> -		dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
> -		v4l2_clk_disable(mt9m111->clk);
> -	}
> -
> -	return ret;
> -}
> -
> -static void mt9m111_power_off(struct mt9m111 *mt9m111)
> -{
> -	mt9m111_suspend(mt9m111);
> -	v4l2_clk_disable(mt9m111->clk);
> -}
> -
> -static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
> -{
> -	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
> -	int ret = 0;
> -
> -	mutex_lock(&mt9m111->power_lock);
> -
> -	/*
> -	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
> -	 * update the power state.
> -	 */
> -	if (mt9m111->power_count == !on) {
> -		if (on)
> -			ret = mt9m111_power_on(mt9m111);
> -		else
> -			mt9m111_power_off(mt9m111);
> -	}
> -
> -	if (!ret) {
> -		/* Update the power count. */
> -		mt9m111->power_count += on ? 1 : -1;
> -		WARN_ON(mt9m111->power_count < 0);
> -	}
> -
> -	mutex_unlock(&mt9m111->power_lock);
> -	return ret;
> -}
> -
> -static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
> -	.s_ctrl = mt9m111_s_ctrl,
> -};
> -
> -static struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
> -	.s_power	= mt9m111_s_power,
> -#ifdef CONFIG_VIDEO_ADV_DEBUG
> -	.g_register	= mt9m111_g_register,
> -	.s_register	= mt9m111_s_register,
> -#endif
> -};
> -
> -static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
> -		struct v4l2_subdev_pad_config *cfg,
> -		struct v4l2_subdev_mbus_code_enum *code)
> -{
> -	if (code->pad || code->index >= ARRAY_SIZE(mt9m111_colour_fmts))
> -		return -EINVAL;
> -
> -	code->code = mt9m111_colour_fmts[code->index].code;
> -	return 0;
> -}
> -
> -static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
> -				struct v4l2_mbus_config *cfg)
> -{
> -	cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
> -		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
> -		V4L2_MBUS_DATA_ACTIVE_HIGH;
> -	cfg->type = V4L2_MBUS_PARALLEL;
> -
> -	return 0;
> -}
> -
> -static struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
> -	.s_crop		= mt9m111_s_crop,
> -	.g_crop		= mt9m111_g_crop,
> -	.cropcap	= mt9m111_cropcap,
> -	.g_mbus_config	= mt9m111_g_mbus_config,
> -};
> -
> -static const struct v4l2_subdev_pad_ops mt9m111_subdev_pad_ops = {
> -	.enum_mbus_code = mt9m111_enum_mbus_code,
> -	.get_fmt	= mt9m111_get_fmt,
> -	.set_fmt	= mt9m111_set_fmt,
> -};
> -
> -static struct v4l2_subdev_ops mt9m111_subdev_ops = {
> -	.core	= &mt9m111_subdev_core_ops,
> -	.video	= &mt9m111_subdev_video_ops,
> -	.pad	= &mt9m111_subdev_pad_ops,
> -};
> -
> -/*
> - * Interface active, can use i2c. If it fails, it can indeed mean, that
> - * this wasn't our capture interface, so, we wait for the right one
> - */
> -static int mt9m111_video_probe(struct i2c_client *client)
> -{
> -	struct mt9m111 *mt9m111 = to_mt9m111(client);
> -	s32 data;
> -	int ret;
> -
> -	ret = mt9m111_s_power(&mt9m111->subdev, 1);
> -	if (ret < 0)
> -		return ret;
> -
> -	data = reg_read(CHIP_VERSION);
> -
> -	switch (data) {
> -	case 0x143a: /* MT9M111 or MT9M131 */
> -		dev_info(&client->dev,
> -			"Detected a MT9M111/MT9M131 chip ID %x\n", data);
> -		break;
> -	case 0x148c: /* MT9M112 */
> -		dev_info(&client->dev, "Detected a MT9M112 chip ID %x\n", data);
> -		break;
> -	default:
> -		dev_err(&client->dev,
> -			"No MT9M111/MT9M112/MT9M131 chip detected register read %x\n",
> -			data);
> -		ret = -ENODEV;
> -		goto done;
> -	}
> -
> -	ret = mt9m111_init(mt9m111);
> -	if (ret)
> -		goto done;
> -
> -	ret = v4l2_ctrl_handler_setup(&mt9m111->hdl);
> -
> -done:
> -	mt9m111_s_power(&mt9m111->subdev, 0);
> -	return ret;
> -}
> -
> -static int mt9m111_probe(struct i2c_client *client,
> -			 const struct i2c_device_id *did)
> -{
> -	struct mt9m111 *mt9m111;
> -	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
> -	int ret;
> -
> -	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
> -		dev_warn(&adapter->dev,
> -			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
> -		return -EIO;
> -	}
> -
> -	mt9m111 = devm_kzalloc(&client->dev, sizeof(struct mt9m111), GFP_KERNEL);
> -	if (!mt9m111)
> -		return -ENOMEM;
> -
> -	mt9m111->clk = v4l2_clk_get(&client->dev, "mclk");
> -	if (IS_ERR(mt9m111->clk))
> -		return -EPROBE_DEFER;
> -
> -	/* Default HIGHPOWER context */
> -	mt9m111->ctx = &context_b;
> -
> -	v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
> -	v4l2_ctrl_handler_init(&mt9m111->hdl, 5);
> -	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
> -			V4L2_CID_VFLIP, 0, 1, 1, 0);
> -	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
> -			V4L2_CID_HFLIP, 0, 1, 1, 0);
> -	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
> -			V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
> -	mt9m111->gain = v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
> -			V4L2_CID_GAIN, 0, 63 * 2 * 2, 1, 32);
> -	v4l2_ctrl_new_std_menu(&mt9m111->hdl,
> -			&mt9m111_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
> -			V4L2_EXPOSURE_AUTO);
> -	mt9m111->subdev.ctrl_handler = &mt9m111->hdl;
> -	if (mt9m111->hdl.error) {
> -		ret = mt9m111->hdl.error;
> -		goto out_clkput;
> -	}
> -
> -	/* Second stage probe - when a capture adapter is there */
> -	mt9m111->rect.left	= MT9M111_MIN_DARK_COLS;
> -	mt9m111->rect.top	= MT9M111_MIN_DARK_ROWS;
> -	mt9m111->rect.width	= MT9M111_MAX_WIDTH;
> -	mt9m111->rect.height	= MT9M111_MAX_HEIGHT;
> -	mt9m111->fmt		= &mt9m111_colour_fmts[0];
> -	mt9m111->lastpage	= -1;
> -	mutex_init(&mt9m111->power_lock);
> -
> -	ret = mt9m111_video_probe(client);
> -	if (ret < 0)
> -		goto out_hdlfree;
> -
> -	mt9m111->subdev.dev = &client->dev;
> -	ret = v4l2_async_register_subdev(&mt9m111->subdev);
> -	if (ret < 0)
> -		goto out_hdlfree;
> -
> -	return 0;
> -
> -out_hdlfree:
> -	v4l2_ctrl_handler_free(&mt9m111->hdl);
> -out_clkput:
> -	v4l2_clk_put(mt9m111->clk);
> -
> -	return ret;
> -}
> -
> -static int mt9m111_remove(struct i2c_client *client)
> -{
> -	struct mt9m111 *mt9m111 = to_mt9m111(client);
> -
> -	v4l2_async_unregister_subdev(&mt9m111->subdev);
> -	v4l2_clk_put(mt9m111->clk);
> -	v4l2_ctrl_handler_free(&mt9m111->hdl);
> -
> -	return 0;
> -}
> -static const struct of_device_id mt9m111_of_match[] = {
> -	{ .compatible = "micron,mt9m111", },
> -	{},
> -};
> -MODULE_DEVICE_TABLE(of, mt9m111_of_match);
> -
> -static const struct i2c_device_id mt9m111_id[] = {
> -	{ "mt9m111", 0 },
> -	{ }
> -};
> -MODULE_DEVICE_TABLE(i2c, mt9m111_id);
> -
> -static struct i2c_driver mt9m111_i2c_driver = {
> -	.driver = {
> -		.name = "mt9m111",
> -		.of_match_table = of_match_ptr(mt9m111_of_match),
> -	},
> -	.probe		= mt9m111_probe,
> -	.remove		= mt9m111_remove,
> -	.id_table	= mt9m111_id,
> -};
> -
> -module_i2c_driver(mt9m111_i2c_driver);
> -
> -MODULE_DESCRIPTION("Micron/Aptina MT9M111/MT9M112/MT9M131 Camera driver");
> -MODULE_AUTHOR("Robert Jarzmik");
> -MODULE_LICENSE("GPL");
> -- 
> 2.1.4
> 

Re: [PATCH v5 01/13] media: mt9m111: make a standalone v4l2 subdevice

[Guennadi Liakhovetski]

Hi Robert,

On Mon, 29 Aug 2016, Robert Jarzmik wrote:

> Remove the soc_camera adherence. Mostly the change removes the power
> manipulation provided by soc_camera, and instead :
>  - powers on the sensor when the s_power control is activated
>  - powers on the sensor in initial probe
>  - enables and disables the MCLK provided to it in power on/off

Your patch also drops support for inverters on synchronisation and clock 
lines, I guess, your board doesn't use any. I assume, if any board ever 
needs such inverters, support for them can be added in the future. Also, 
as I mentioned in my reply to your other patch, maybe good to join this 
with #3. Otherwise and with that in mind

> Signed-off-by: Robert Jarzmik <robert.jarzmik@free.fr>

Acked-by: Guennadi Liakhovetski <g.liakhovetski@gmx.de>

Thanks
Guennadi

> ---
>  drivers/media/i2c/soc_camera/mt9m111.c | 51 ++++++++++------------------------
>  1 file changed, 15 insertions(+), 36 deletions(-)
> 
> diff --git a/drivers/media/i2c/soc_camera/mt9m111.c b/drivers/media/i2c/soc_camera/mt9m111.c
> index 6dfaead6aaa8..a7efaa5964d1 100644
> --- a/drivers/media/i2c/soc_camera/mt9m111.c
> +++ b/drivers/media/i2c/soc_camera/mt9m111.c
> @@ -16,10 +16,11 @@
>  #include <linux/v4l2-mediabus.h>
>  #include <linux/module.h>
>  
> -#include <media/soc_camera.h>
> +#include <media/v4l2-async.h>
>  #include <media/v4l2-clk.h>
>  #include <media/v4l2-common.h>
>  #include <media/v4l2-ctrls.h>
> +#include <media/v4l2-device.h>
>  
>  /*
>   * MT9M111, MT9M112 and MT9M131:
> @@ -388,7 +389,7 @@ static int mt9m111_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
>  	struct v4l2_rect rect = a->c;
>  	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
>  	int width, height;
> -	int ret;
> +	int ret, align = 0;
>  
>  	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
>  		return -EINVAL;
> @@ -396,17 +397,19 @@ static int mt9m111_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
>  	if (mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
>  	    mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
>  		/* Bayer format - even size lengths */
> -		rect.width	= ALIGN(rect.width, 2);
> -		rect.height	= ALIGN(rect.height, 2);
> +		align = 1;
>  		/* Let the user play with the starting pixel */
>  	}
>  
>  	/* FIXME: the datasheet doesn't specify minimum sizes */
> -	soc_camera_limit_side(&rect.left, &rect.width,
> -		     MT9M111_MIN_DARK_COLS, 2, MT9M111_MAX_WIDTH);
> -
> -	soc_camera_limit_side(&rect.top, &rect.height,
> -		     MT9M111_MIN_DARK_ROWS, 2, MT9M111_MAX_HEIGHT);
> +	v4l_bound_align_image(&rect.width, 2, MT9M111_MAX_WIDTH, align,
> +			      &rect.height, 2, MT9M111_MAX_HEIGHT, align, 0);
> +	rect.left = clamp(rect.left, MT9M111_MIN_DARK_COLS,
> +			  MT9M111_MIN_DARK_COLS + MT9M111_MAX_WIDTH -
> +			  (__s32)rect.width);
> +	rect.top = clamp(rect.top, MT9M111_MIN_DARK_ROWS,
> +			 MT9M111_MIN_DARK_ROWS + MT9M111_MAX_HEIGHT -
> +			 (__s32)rect.height);
>  
>  	width = min(mt9m111->width, rect.width);
>  	height = min(mt9m111->height, rect.height);
> @@ -775,17 +778,16 @@ static int mt9m111_init(struct mt9m111 *mt9m111)
>  static int mt9m111_power_on(struct mt9m111 *mt9m111)
>  {
>  	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
>  	int ret;
>  
> -	ret = soc_camera_power_on(&client->dev, ssdd, mt9m111->clk);
> +	ret = v4l2_clk_enable(mt9m111->clk);
>  	if (ret < 0)
>  		return ret;
>  
>  	ret = mt9m111_resume(mt9m111);
>  	if (ret < 0) {
>  		dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
> -		soc_camera_power_off(&client->dev, ssdd, mt9m111->clk);
> +		v4l2_clk_disable(mt9m111->clk);
>  	}
>  
>  	return ret;
> @@ -793,11 +795,8 @@ static int mt9m111_power_on(struct mt9m111 *mt9m111)
>  
>  static void mt9m111_power_off(struct mt9m111 *mt9m111)
>  {
> -	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
> -	struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
> -
>  	mt9m111_suspend(mt9m111);
> -	soc_camera_power_off(&client->dev, ssdd, mt9m111->clk);
> +	v4l2_clk_disable(mt9m111->clk);
>  }
>  
>  static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
> @@ -854,14 +853,10 @@ static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
>  static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
>  				struct v4l2_mbus_config *cfg)
>  {
> -	struct i2c_client *client = v4l2_get_subdevdata(sd);
> -	struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
> -
>  	cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
>  		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
>  		V4L2_MBUS_DATA_ACTIVE_HIGH;
>  	cfg->type = V4L2_MBUS_PARALLEL;
> -	cfg->flags = soc_camera_apply_board_flags(ssdd, cfg);
>  
>  	return 0;
>  }
> @@ -933,20 +928,8 @@ static int mt9m111_probe(struct i2c_client *client,
>  {
>  	struct mt9m111 *mt9m111;
>  	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
> -	struct soc_camera_subdev_desc *ssdd = soc_camera_i2c_to_desc(client);
>  	int ret;
>  
> -	if (client->dev.of_node) {
> -		ssdd = devm_kzalloc(&client->dev, sizeof(*ssdd), GFP_KERNEL);
> -		if (!ssdd)
> -			return -ENOMEM;
> -		client->dev.platform_data = ssdd;
> -	}
> -	if (!ssdd) {
> -		dev_err(&client->dev, "mt9m111: driver needs platform data\n");
> -		return -EINVAL;
> -	}
> -
>  	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
>  		dev_warn(&adapter->dev,
>  			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
> @@ -992,10 +975,6 @@ static int mt9m111_probe(struct i2c_client *client,
>  	mt9m111->lastpage	= -1;
>  	mutex_init(&mt9m111->power_lock);
>  
> -	ret = soc_camera_power_init(&client->dev, ssdd);
> -	if (ret < 0)
> -		goto out_hdlfree;
> -
>  	ret = mt9m111_video_probe(client);
>  	if (ret < 0)
>  		goto out_hdlfree;
> -- 
> 2.1.4
> 

Re: [PATCH v5 01/13] media: mt9m111: make a standalone v4l2 subdevice

[Robert Jarzmik]

Guennadi Liakhovetski <g.liakhovetski@gmx.de> writes:

> Hi Robert,
>
> On Mon, 29 Aug 2016, Robert Jarzmik wrote:
>
>> Remove the soc_camera adherence. Mostly the change removes the power
>> manipulation provided by soc_camera, and instead :
>>  - powers on the sensor when the s_power control is activated
>>  - powers on the sensor in initial probe
>>  - enables and disables the MCLK provided to it in power on/off
>
> Your patch also drops support for inverters on synchronisation and clock 
> lines, I guess, your board doesn't use any. I assume, if any board ever 
> needs such inverters, support for them can be added in the future.
Ah yeah, that would deserve a notice in the commit message.

It's a bit a pity to respin the whole serie for it, but you've got a fair
point. Maybe Hans would agree to apply 4/13 to 13/13 (if there is no comment
remaining), and let me respin the 3 patches around mtm9111.

> Also, as I mentioned in my reply to your other patch, maybe good to join this
> with #3. Otherwise and with that in mind
I'd like to keep the move separate from the remaining part. And yes, I should
have used the "-M" option ... I added it to my submission script so that I don't
forget it again :)

Thanks for the Ack, and cheers.

--
Robert

Re: [PATCH v5 00/13] pxa_camera transition to v4l2 standalone device

[Hans Verkuil]

On 08/29/2016 07:55 PM, Robert Jarzmik wrote:
> There is no change between v4 and v5, ie. the global diff is empty, only one
> line was shifted to prevent breaking bisectablility.

Against which tree do you develop? Unfortunately this patch series doesn't apply
to the media_tree master branch anymore due to conflicts with a merged patch that
converts s/g_crop to s/g_selection in all subdev drivers.

When you make the new patch series, please use the -M option with git send-email so
patches that move files around are handled cleanly. That makes it much easier to review.

BTW, checkpatch reported issues in a switch statement in function pxa_camera_get_formats():


        switch (code.code) {
        case MEDIA_BUS_FMT_UYVY8_2X8:
                formats++;
                if (xlate) {
                        xlate->host_fmt = &pxa_camera_formats[0];
                        xlate->code     = code.code;
                        xlate++;
                        dev_dbg(dev, "Providing format %s using code %d\n",
                                pxa_camera_formats[0].name, code.code);
                }
        case MEDIA_BUS_FMT_VYUY8_2X8:
        case MEDIA_BUS_FMT_YUYV8_2X8:
        case MEDIA_BUS_FMT_YVYU8_2X8:
        case MEDIA_BUS_FMT_RGB565_2X8_LE:
        case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
                if (xlate)
                        dev_dbg(dev, "Providing format %s packed\n",
                                fmt->name);
                break;
        default:
                if (!pxa_camera_packing_supported(fmt))
                        return 0;
                if (xlate)
                        dev_dbg(dev,
                                "Providing format %s in pass-through mode\n",
                                fmt->name);
        }

Before 'case MEDIA_BUS_FMT_VYUY8_2X8' should there be a break? If not, then
there should be a '/* fall through */' comment.

At the end of the default case there should also be a break statement.

This is already in the existing code, so just make a separate patch fixing
this.

I'm ready to make a pull request as soon as I have a v6 that applies cleanly.

Regards,

	Hans

> 
> All the text in https://lkml.org/lkml/2016/8/15/609 is still applicable.
> 
> Cheers.
> 
> --
> Robert
> 
> Robert Jarzmik (13):
>   media: mt9m111: make a standalone v4l2 subdevice
>   media: mt9m111: use only the SRGB colorspace
>   media: mt9m111: move mt9m111 out of soc_camera
>   media: platform: pxa_camera: convert to vb2
>   media: platform: pxa_camera: trivial move of functions
>   media: platform: pxa_camera: introduce sensor_call
>   media: platform: pxa_camera: make printk consistent
>   media: platform: pxa_camera: add buffer sequencing
>   media: platform: pxa_camera: remove set_crop
>   media: platform: pxa_camera: make a standalone v4l2 device
>   media: platform: pxa_camera: add debug register access
>   media: platform: pxa_camera: change stop_streaming semantics
>   media: platform: pxa_camera: move pxa_camera out of soc_camera
> 
>  drivers/media/i2c/Kconfig                      |    7 +
>  drivers/media/i2c/Makefile                     |    1 +
>  drivers/media/i2c/mt9m111.c                    | 1033 ++++++++++++
>  drivers/media/i2c/soc_camera/Kconfig           |    7 +-
>  drivers/media/i2c/soc_camera/Makefile          |    1 -
>  drivers/media/i2c/soc_camera/mt9m111.c         | 1054 ------------
>  drivers/media/platform/Kconfig                 |    8 +
>  drivers/media/platform/Makefile                |    1 +
>  drivers/media/platform/pxa_camera.c            | 2096 ++++++++++++++++++++++++
>  drivers/media/platform/soc_camera/Kconfig      |    8 -
>  drivers/media/platform/soc_camera/Makefile     |    1 -
>  drivers/media/platform/soc_camera/pxa_camera.c | 1866 ---------------------
>  include/linux/platform_data/media/camera-pxa.h |    2 +
>  13 files changed, 3153 insertions(+), 2932 deletions(-)
>  create mode 100644 drivers/media/i2c/mt9m111.c
>  delete mode 100644 drivers/media/i2c/soc_camera/mt9m111.c
>  create mode 100644 drivers/media/platform/pxa_camera.c
>  delete mode 100644 drivers/media/platform/soc_camera/pxa_camera.c
> 

Re: [PATCH v5 00/13] pxa_camera transition to v4l2 standalone device

[Robert Jarzmik]

Hans Verkuil <hverkuil@xs4all.nl> writes:

> On 08/29/2016 07:55 PM, Robert Jarzmik wrote:
>> There is no change between v4 and v5, ie. the global diff is empty, only one
>> line was shifted to prevent breaking bisectablility.
>
> Against which tree do you develop? Unfortunately this patch series doesn't apply
> to the media_tree master branch anymore due to conflicts with a merged patch that
> converts s/g_crop to s/g_selection in all subdev drivers.
v4.8-rc1 is their base, so Linus's master.

> When you make the new patch series, please use the -M option with git send-email so
> patches that move files around are handled cleanly. That makes it much easier
> to review.
Ok.

> BTW, checkpatch reported issues in a switch statement in function
> pxa_camera_get_formats():
Yep, I noticed.

>         switch (code.code) {
>         case MEDIA_BUS_FMT_UYVY8_2X8:
>                 formats++;
>                 if (xlate) {
>                         xlate->host_fmt = &pxa_camera_formats[0];
>                         xlate->code     = code.code;
>                         xlate++;
>                         dev_dbg(dev, "Providing format %s using code %d\n",
>                                 pxa_camera_formats[0].name, code.code);
>                 }
>         case MEDIA_BUS_FMT_VYUY8_2X8:
>         case MEDIA_BUS_FMT_YUYV8_2X8:
>         case MEDIA_BUS_FMT_YVYU8_2X8:
>         case MEDIA_BUS_FMT_RGB565_2X8_LE:
>         case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
>                 if (xlate)
>                         dev_dbg(dev, "Providing format %s packed\n",
>                                 fmt->name);
>                 break;
>         default:
>                 if (!pxa_camera_packing_supported(fmt))
>                         return 0;
>                 if (xlate)
>                         dev_dbg(dev,
>                                 "Providing format %s in pass-through mode\n",
>                                 fmt->name);
>         }
>
> Before 'case MEDIA_BUS_FMT_VYUY8_2X8' should there be a break? If not, then
> there should be a '/* fall through */' comment.
There should have been a '/* fall through */' in the original code, even if that
was not strictly "required" at the time of writing.
>
> At the end of the default case there should also be a break statement.
>
> This is already in the existing code, so just make a separate patch fixing
> this.
Ok, will do.

Cheers.

--
Robert