Added mt9p031 camera sensor support

Signed-off-by: Christian Hemp <c.hemp@phytec.de>

8 files changed, 968 insertions, 10 deletions

diff --git a/arch/arm/mach-mx6/board-mx6q_phyflex.c b/arch/arm/mach-mx6/board-mx6q_phyflex.c
index 5c2e5dc2b3e8..3bb7e9534deb 100644
--- a/arch/arm/mach-mx6/board-mx6q_phyflex.c
+++ b/arch/arm/mach-mx6/board-mx6q_phyflex.c
@@ -1077,7 +1077,7 @@ static struct platform_device mxc_ipu_csi_devices[] = {
 
 static struct i2c_board_info phyflex_cameras[] = {
 	[0] = {
-		I2C_BOARD_INFO("mt9v022", 0x48), /* CTRL1 = 0 */
+		I2C_BOARD_INFO("mt9p031", 0x48), /* CTRL1 = 0 */
 	},
 	[1] = {
 		I2C_BOARD_INFO("mt9v022", 0x4c), /* CTRL1 = 1 */
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index 1c49b8fcea1a..d0c552e9c2d2 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -855,6 +855,12 @@ config SOC_CAMERA_MT9M111
 	  This driver supports MT9M111, MT9M112 and MT9M131 cameras from
 	  Micron/Aptina
 
+config SOC_CAMERA_MT9P031
+	tristate "mt9p031 support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This driver supports MT9P031 camera from Micron/Aptina
+
 config SOC_CAMERA_MT9T031
 	tristate "mt9t031 support"
 	depends on SOC_CAMERA && I2C
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index 32f1c572eaa7..bdb561528c1b 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -74,6 +74,7 @@ obj-$(CONFIG_VIDEO_M5MOLS)	+= m5mols/
 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_MT9P031) 	+= mt9p031.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/video/mt9p031.c b/drivers/media/video/mt9p031.c
new file mode 100644
index 000000000000..d0b288c205e4
--- /dev/null
+++ b/drivers/media/video/mt9p031.c
@@ -0,0 +1,917 @@
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/log2.h>
+
+#include <media/mt9p031.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/soc_camera.h>
+#include "mxc_camera.h"
+
+#define MT9P031_PIXEL_ARRAY_WIDTH			2752
+#define MT9P031_PIXEL_ARRAY_HEIGHT			2004
+
+#define MT9P031_CHIP_VERSION				0x00
+#define		MT9P031_CHIP_VERSION_VALUE		0x1801
+#define MT9P031_ROW_START				0x01
+#define		MT9P031_ROW_START_MIN			0
+#define		MT9P031_ROW_START_MAX			2004
+#define		MT9P031_ROW_START_DEF			54
+#define MT9P031_COLUMN_START				0x02
+#define		MT9P031_COLUMN_START_MIN		0
+#define		MT9P031_COLUMN_START_MAX		2750
+#define		MT9P031_COLUMN_START_DEF		16
+#define MT9P031_WINDOW_HEIGHT				0x03
+#define		MT9P031_WINDOW_HEIGHT_MIN		2
+#define		MT9P031_WINDOW_HEIGHT_MAX		2006
+#define		MT9P031_WINDOW_HEIGHT_DEF		1944
+#define MT9P031_WINDOW_WIDTH				0x04
+#define		MT9P031_WINDOW_WIDTH_MIN		2
+#define		MT9P031_WINDOW_WIDTH_MAX		2752
+#define		MT9P031_WINDOW_WIDTH_DEF		2592
+#define MT9P031_HORIZONTAL_BLANK			0x05
+#define		MT9P031_HORIZONTAL_BLANK_MIN		0
+#define		MT9P031_HORIZONTAL_BLANK_MAX		4095
+#define MT9P031_VERTICAL_BLANK				0x06
+#define		MT9P031_VERTICAL_BLANK_MIN		0
+#define		MT9P031_VERTICAL_BLANK_MAX		4095
+#define		MT9P031_VERTICAL_BLANK_DEF		25
+#define MT9P031_OUTPUT_CONTROL				0x07
+#define		MT9P031_OUTPUT_CONTROL_CEN		2
+#define		MT9P031_OUTPUT_CONTROL_SYN		1
+#define		MT9P031_OUTPUT_CONTROL_DEF		0x1f82
+#define MT9P031_SHUTTER_WIDTH_UPPER			0x08
+#define MT9P031_SHUTTER_WIDTH_LOWER			0x09
+#define		MT9P031_SHUTTER_WIDTH_MIN		1
+#define		MT9P031_SHUTTER_WIDTH_MAX		1048575
+#define		MT9P031_SHUTTER_WIDTH_DEF		1943
+#define	MT9P031_PLL_CONTROL				0x10
+#define		MT9P031_PLL_CONTROL_DEF			0x0050
+#define		MT9P031_PLL_CONTROL_PWROFF		0x0050
+#define		MT9P031_PLL_CONTROL_PWRON		0x0051
+#define		MT9P031_PLL_CONTROL_USEPLL		0x0052
+#define	MT9P031_PLL_CONFIG_1				0x11
+#define	MT9P031_PLL_CONFIG_2				0x12
+#define MT9P031_PIXEL_CLOCK_CONTROL			0x0a
+#define 	MT9P031_PIXEL_CLOCK_INVERT 		(1u << 15)
+#define MT9P031_FRAME_RESTART				0x0b
+#define MT9P031_SHUTTER_DELAY				0x0c
+#define MT9P031_RST					0x0d
+#define		MT9P031_RST_ENABLE			1
+#define		MT9P031_RST_DISABLE			0
+#define MT9P031_READ_MODE_1				0x1e
+#define		MT9P031_READ_MODE_1_DEF				0x4006
+#define MT9P031_READ_MODE_2				0x20
+#define		MT9P031_READ_MODE_2_ROW_MIR		(1 << 15)
+#define		MT9P031_READ_MODE_2_COL_MIR		(1 << 14)
+#define		MT9P031_READ_MODE_2_ROW_BLC		(1 << 6)
+#define MT9P031_ROW_ADDRESS_MODE			0x22
+#define MT9P031_COLUMN_ADDRESS_MODE			0x23
+#define MT9P031_GLOBAL_GAIN				0x35
+#define		MT9P031_GLOBAL_GAIN_MIN			8
+#define		MT9P031_GLOBAL_GAIN_MAX			1024
+#define		MT9P031_GLOBAL_GAIN_DEF			8
+#define		MT9P031_GLOBAL_GAIN_MULT		(1 << 6)
+#define MT9P031_ROW_BLACK_DEF_OFFSET			0x4b
+#define MT9P031_TEST_PATTERN				0xa0
+#define		MT9P031_TEST_PATTERN_SHIFT		3
+#define		MT9P031_TEST_PATTERN_ENABLE		(1 << 0)
+#define		MT9P031_TEST_PATTERN_DISABLE		(0 << 0)
+#define MT9P031_TEST_PATTERN_GREEN			0xa1
+#define MT9P031_TEST_PATTERN_RED			0xa2
+#define MT9P031_TEST_PATTERN_BLUE			0xa3
+
+#define MT9P031_TARGET_FREQ_DEF	96000000
+
+static bool		g_is_mono;
+module_param_named(mono, g_is_mono, bool, 0444);
+
+/* MT9P031 has only one fixed colorspace per pixelcode */
+struct mt9p031_datafmt {
+	enum v4l2_mbus_pixelcode	code;
+	enum v4l2_colorspace		colorspace;
+};
+
+/* Find a data format by a pixel code in an array */
+static const struct mt9p031_datafmt *mt9p031_find_datafmt(
+	enum v4l2_mbus_pixelcode code, const struct mt9p031_datafmt *fmt,
+	int n)
+{
+	int i;
+	for (i = 0; i < n; i++)
+		if (fmt[i].code == code)
+			return fmt + i;
+
+	return NULL;
+}
+
+//TODO: 12bit data has to be supported
+static const struct mt9p031_datafmt mt9p031_colour_fmts[] = {
+	/*
+	 * Order important: first natively supported,
+	 * second supported with a GPIO extender
+	 */
+	{V4L2_MBUS_FMT_SBGGR10_1X10, V4L2_COLORSPACE_SRGB},
+//	{V4L2_MBUS_FMT_SGRBG12_1X12, V4L2_COLORSPACE_SRGB},
+	{V4L2_MBUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
+};
+
+static const struct mt9p031_datafmt mt9p031_monochrome_fmts[] = {
+	/* Order important - see above */
+	{V4L2_MBUS_FMT_Y10_1X10, V4L2_COLORSPACE_JPEG},
+//	{V4L2_MBUS_FMT_Y12_1X12, V4L2_COLORSPACE_JPEG},
+	{V4L2_MBUS_FMT_Y8_1X8, V4L2_COLORSPACE_JPEG},
+};
+
+struct mt9p031_pll_divs {
+	u32 ext_freq;
+	u32 target_freq;
+	u8 m;
+	u8 n;
+	u8 p1;
+};
+
+struct mt9p031 {
+	struct v4l2_subdev subdev;
+	struct v4l2_rect rect;	/* Sensor window */
+	int model;	/* V4L2_IDENT_MT9V02x codes from v4l2-chip-ident.h */
+	u16 xskip;
+	u16 yskip;
+
+	const struct mt9p031_datafmt *fmt;
+	const struct mt9p031_datafmt *fmts;
+	int num_fmts;
+
+	bool use_pll;
+	const struct mt9p031_pll_divs *pll;
+
+	/* Register cache */
+	u16 output_control;
+	u16 mode2;
+};
+
+static struct mt9p031 *to_mt9p031(const struct i2c_client *client)
+{
+	return container_of(i2c_get_clientdata(client), struct mt9p031, subdev);
+}
+
+static int mt9p031_read(struct i2c_client *client, const u8 reg)
+{
+	s32 data = i2c_smbus_read_word_data(client, reg);
+	return data < 0 ? data : be16_to_cpu(data);
+}
+
+static int mt9p031_write(struct i2c_client *client, const u8 reg,
+		     const u16 data)
+{
+	return i2c_smbus_write_word_data(client, reg, cpu_to_be16(data));
+}
+
+static int mt9p031_set_output_control(struct mt9p031 *mt9p031, u16 clear,
+				      u16 set)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	u16 value = (mt9p031->output_control & ~clear) | set;
+	int ret;
+
+	ret = mt9p031_write(client, MT9P031_OUTPUT_CONTROL, value);
+	if (ret < 0)
+		return ret;
+
+	mt9p031->output_control = value;
+	return 0;
+}
+
+static int mt9p031_set_mode2(struct mt9p031 *mt9p031, u16 clear, u16 set)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	u16 value = (mt9p031->mode2 & ~clear) | set;
+	int ret;
+
+	ret = mt9p031_write(client, MT9P031_READ_MODE_2, value);
+	if (ret < 0)
+		return ret;
+
+	mt9p031->mode2 = value;
+	return 0;
+}
+
+static void mt9p031_video_remove(struct soc_camera_device *icd)
+{
+	struct soc_camera_link *icl = to_soc_camera_link(icd);
+
+	dev_dbg(&icd->dev, "Video removed: %p, %p\n",
+		icd->dev.parent, icd->vdev);
+	if (icl->free_bus)
+		icl->free_bus(icl);
+}
+
+/*TODO: add new dividers */
+static const struct mt9p031_pll_divs mt9p031_divs[] = {
+	/* ext_freq	target_freq	m	n	p1 */
+	{27000000,	MT9P031_TARGET_FREQ_DEF,	90,	4,	6}
+};
+
+static int mt9p031_pll_get_divs(struct mt9p031 *mt9p031)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	struct soc_camera_device *icd = client->dev.platform_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
+	struct mxc_camera_dev *mxc_cam = ici->priv;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mt9p031_divs); i++) {
+		if (mt9p031_divs[i].ext_freq == mxc_cam->pdata->mclk_default_rate) {
+			mt9p031->pll = &mt9p031_divs[i];
+			mt9p031->use_pll = true;
+			return 0;
+		}
+	}
+
+	if (mxc_cam->pdata->mclk_default_rate == MT9P031_TARGET_FREQ_DEF) {
+		mt9p031->use_pll = false;
+		return 0;
+	}
+
+	dev_err(&client->dev, "Couldn't find PLL dividers for ext_freq = %ld\n", mxc_cam->pdata->mclk_default_rate);
+	return -EINVAL;
+}
+
+static int mt9p031_pll_enable(struct mt9p031 *mt9p031)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	int ret;
+
+	ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
+			    MT9P031_PLL_CONTROL_PWRON);
+	if (ret < 0)
+		return ret;
+
+	ret = mt9p031_write(client, MT9P031_PLL_CONFIG_1,
+			    (mt9p031->pll->m << 8) | (mt9p031->pll->n - 1));
+	if (ret < 0)
+		return ret;
+
+	ret = mt9p031_write(client, MT9P031_PLL_CONFIG_2, mt9p031->pll->p1 - 1);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(1000, 2000);
+	ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
+			    MT9P031_PLL_CONTROL_PWRON |
+			    MT9P031_PLL_CONTROL_USEPLL);
+
+	return ret;
+}
+
+static inline int mt9p031_pll_disable(struct mt9p031 *mt9p031)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+
+	return mt9p031_write(client, MT9P031_PLL_CONTROL,
+			     MT9P031_PLL_CONTROL_PWROFF);
+}
+
+/* -----------------------------------------------------------------------------
+ * Soc camera operations
+ */
+
+static int mt9p031_set_bus_param(struct soc_camera_device *icd,
+				 unsigned long flags)
+{
+	struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
+	struct soc_camera_link *icl = to_soc_camera_link(icd);
+	unsigned int width_flag = flags & SOCAM_DATAWIDTH_MASK;
+	int ret;
+
+	if (!is_power_of_2(width_flag))
+		return -EINVAL;
+
+	if (icl->set_bus_param) {
+		ret = icl->set_bus_param(icl, width_flag);
+		if (ret)
+			return ret;
+	} else {
+		if (width_flag != SOCAM_DATAWIDTH_10)
+			return -EINVAL;
+	}
+
+	flags = soc_camera_apply_sensor_flags(icl, flags);
+
+	if (flags & SOCAM_SENSOR_INVERT_PCLK)
+		ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL, MT9P031_PIXEL_CLOCK_INVERT);
+	if (ret < 0)
+		return ret;
+	
+	return 0;
+}
+
+static unsigned long mt9p031_query_bus_param(struct soc_camera_device *icd)
+{
+	struct soc_camera_link *icl = to_soc_camera_link(icd);
+	unsigned int flags = SOCAM_MASTER | SOCAM_SLAVE |
+		SOCAM_PCLK_SAMPLE_RISING | SOCAM_PCLK_SAMPLE_FALLING |
+		SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_LOW |
+		SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_LOW |
+		SOCAM_DATA_ACTIVE_HIGH;
+
+	if (icl->query_bus_param)
+		flags |= icl->query_bus_param(icl) & SOCAM_DATAWIDTH_MASK;
+	else 
+		flags |= SOCAM_DATAWIDTH_10;
+
+	return soc_camera_apply_sensor_flags(icl, flags);
+}
+
+static struct soc_camera_ops mt9p031_ops = {
+	.set_bus_param		= mt9p031_set_bus_param,
+	.query_bus_param	= mt9p031_query_bus_param,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev video operations
+ */
+
+static int mt9p031_set_params(struct mt9p031 *mt9p031)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
+	const struct v4l2_rect *crop = &mt9p031->rect;
+	unsigned int hblank;
+	unsigned int vblank;
+	unsigned int xskip;
+	unsigned int yskip;
+	unsigned int xbin;
+	unsigned int ybin;
+	int ret;
+
+	/* Windows position and size.
+	 *
+	 * TODO: Make sure the start coordinates and window size match the
+	 * skipping, binning and mirroring (see description of registers 2 and 4
+	 * in table 13, and Binning section on page 41).
+	 */
+	ret = mt9p031_write(client, MT9P031_COLUMN_START, crop->left);
+	if (ret < 0)
+		return ret;
+	ret = mt9p031_write(client, MT9P031_ROW_START, crop->top);
+	if (ret < 0)
+		return ret;
+	ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, crop->width - 1);
+	if (ret < 0)
+		return ret;
+	ret = mt9p031_write(client, MT9P031_WINDOW_HEIGHT, crop->height - 1);
+	if (ret < 0)
+		return ret;
+
+
+	/* Row and column binning and skipping. Use the maximum binning value
+	 * compatible with the skipping settings.
+	 */
+	xskip = DIV_ROUND_CLOSEST(crop->width, MT9P031_WINDOW_WIDTH_DEF);
+	yskip = DIV_ROUND_CLOSEST(crop->height, MT9P031_WINDOW_HEIGHT_DEF);
+	xbin = 1 << (ffs(xskip) - 1);
+	ybin = 1 << (ffs(yskip) - 1);
+
+	ret = mt9p031_write(client, MT9P031_COLUMN_ADDRESS_MODE,
+			    ((xbin - 1) << 4) | (xskip - 1));
+	if (ret < 0)
+		return ret;
+	ret = mt9p031_write(client, MT9P031_ROW_ADDRESS_MODE,
+			    ((ybin - 1) << 4) | (yskip - 1));
+	if (ret < 0)
+		return ret;
+
+	/* Blanking - use minimum value for horizontal blanking and default
+	 * value for vertical blanking.
+	 */
+	hblank = 346 * ybin + 64 + (80 >> max_t(unsigned int, xbin, 3));
+	vblank = MT9P031_VERTICAL_BLANK_DEF;
+
+	ret = mt9p031_write(client, MT9P031_HORIZONTAL_BLANK, hblank);
+	if (ret < 0)
+		return ret;
+	ret = mt9p031_write(client, MT9P031_VERTICAL_BLANK, vblank);
+	if (ret < 0)
+		return ret;
+
+	return ret;
+}
+
+static int mt9p031_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9p031 *mt9p031 = to_mt9p031(client);
+	int ret;
+
+	if (!enable) {
+		ret = mt9p031_set_output_control(mt9p031, MT9P031_OUTPUT_CONTROL_CEN, 0);
+
+		if (ret < 0)
+			return ret;
+		if (mt9p031->use_pll)
+			return mt9p031_pll_disable(mt9p031);
+		else
+			return 0;
+	}
+
+	ret = mt9p031_set_params(mt9p031);
+	if (ret < 0)
+		return ret;
+
+	/* Switch to master "normal" mode */
+	ret = mt9p031_set_output_control(mt9p031, 0, MT9P031_OUTPUT_CONTROL_CEN);
+	if (ret < 0)
+		return ret;
+	if (mt9p031->use_pll)
+		return mt9p031_pll_enable(mt9p031);
+	else
+		return 0;
+}
+
+static int mt9p031_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9p031 *mt9p031 = to_mt9p031(client);
+	struct v4l2_rect rect = a->c;
+
+	if (mt9p031->fmts == mt9p031_colour_fmts) {
+		rect.width	= ALIGN(rect.width, 2);
+		rect.height	= ALIGN(rect.height, 2);
+	}
+
+	soc_camera_limit_side(&rect.left, &rect.width,
+		     MT9P031_COLUMN_START_DEF, MT9P031_WINDOW_WIDTH_MIN, MT9P031_WINDOW_WIDTH_MAX);
+
+	soc_camera_limit_side(&rect.top, &rect.height,
+		     MT9P031_ROW_START_DEF, MT9P031_WINDOW_HEIGHT_MIN, MT9P031_WINDOW_HEIGHT_MAX);
+
+	dev_dbg(&client->dev, "Frame %dx%d pixel\n", rect.width, rect.height);
+
+	mt9p031->rect = rect;
+
+	return 0;
+}
+
+static int mt9p031_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9p031 *mt9p031 = to_mt9p031(client);
+
+	a->c	= mt9p031->rect;
+	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int mt9p031_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left			= MT9P031_COLUMN_START_DEF;
+	a->bounds.top			= MT9P031_ROW_START_DEF;
+	a->bounds.width			= MT9P031_WINDOW_WIDTH_DEF;
+	a->bounds.height		= MT9P031_WINDOW_HEIGHT_DEF;
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int mt9p031_s_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9p031 *mt9p031 = to_mt9p031(client);
+	struct v4l2_crop a = {
+		.c = {
+			.left	= mt9p031->rect.left,
+			.top	= mt9p031->rect.top,
+			.width	= mf->width,
+			.height	= mf->height,
+		},
+	};
+	int ret;
+
+	ret = mt9p031_s_crop(sd, &a);
+	if (!ret) {
+		mf->width	= mt9p031->rect.width;
+		mf->height	= mt9p031->rect.height;
+		mt9p031->fmt	= mt9p031_find_datafmt(mf->code,
+					mt9p031->fmts, mt9p031->num_fmts);
+		mf->colorspace	= mt9p031->fmt->colorspace;
+	}
+
+	return ret;
+}
+
+static int mt9p031_try_fmt(struct v4l2_subdev *sd,
+			   struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9p031 *mt9p031 = to_mt9p031(client);
+	const struct mt9p031_datafmt *fmt;
+	int align = mf->code == V4L2_MBUS_FMT_SBGGR8_1X8 ||
+		mf->code == V4L2_MBUS_FMT_SBGGR10_1X10;
+
+	v4l_bound_align_image(&mf->width, MT9P031_WINDOW_WIDTH_MIN,
+		MT9P031_WINDOW_WIDTH_MAX, align,
+		&mf->height, MT9P031_WINDOW_HEIGHT_MIN,
+		MT9P031_WINDOW_HEIGHT_MAX, align, 0);
+
+	fmt = mt9p031_find_datafmt(mf->code, mt9p031->fmts,
+				   mt9p031->num_fmts);
+	if (!fmt) {
+		fmt = mt9p031->fmt;
+		mf->code = fmt->code;
+	}
+
+	mf->colorspace	= fmt->colorspace;
+
+	return 0;
+}
+
+static int mt9p031_g_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9p031 *mt9p031 = to_mt9p031(client);
+
+	mf->width	= mt9p031->rect.width;
+	mf->height	= mt9p031->rect.height;
+	mf->code	= mt9p031->fmt->code;
+	mf->colorspace	= mt9p031->fmt->colorspace;
+	mf->field	= V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int mt9p031_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			    enum v4l2_mbus_pixelcode *code)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9p031 *mt9p031 = to_mt9p031(client);
+
+	if (index >= mt9p031->num_fmts) {
+		return -EINVAL;
+	}
+
+	*code = mt9p031->fmts[index].code;
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev core operations
+ */
+
+#define V4L2_CID_TEST_PATTERN		(V4L2_CID_USER_BASE | 0x1001)
+
+static int mt9p031_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9p031 *mt9p031 = to_mt9p031(client);
+	u16 data;
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
+				    (ctrl->value >> 16) & 0xffff);
+		if (ret < 0)
+			return ret;
+
+		return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
+				     ctrl->value & 0xffff);
+
+	case V4L2_CID_GAIN:
+		/* Gain is controlled by 2 analog stages and a digital stage.
+		 * Valid values for the 3 stages are
+		 *
+		 * Stage                Min     Max     Step
+		 * ------------------------------------------
+		 * First analog stage   x1      x2      1
+		 * Second analog stage  x1      x4      0.125
+		 * Digital stage        x1      x16     0.125
+		 *
+		 * To minimize noise, the gain stages should be used in the
+		 * second analog stage, first analog stage, digital stage order.
+		 * Gain from a previous stage should be pushed to its maximum
+		 * value before the next stage is used.
+		 */
+		if (ctrl->value <= 32) {
+			data = ctrl->value;
+		} else if (ctrl->value <= 64) {
+			ctrl->value &= ~1;
+			data = (1 << 6) | (ctrl->value >> 1);
+		} else {
+			ctrl->value &= ~7;
+			data = ((ctrl->value - 64) << 5) | (1 << 6) | 32;
+		}
+
+		return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
+
+	case V4L2_CID_HFLIP:
+		if (ctrl->value)
+			return mt9p031_set_mode2(mt9p031,
+					0, MT9P031_READ_MODE_2_COL_MIR);
+		else
+			return mt9p031_set_mode2(mt9p031,
+					MT9P031_READ_MODE_2_COL_MIR, 0);
+
+	case V4L2_CID_VFLIP:
+		if (ctrl->value)
+			return mt9p031_set_mode2(mt9p031,
+					0, MT9P031_READ_MODE_2_ROW_MIR);
+		else
+			return mt9p031_set_mode2(mt9p031,
+					MT9P031_READ_MODE_2_ROW_MIR, 0);
+
+	case V4L2_CID_TEST_PATTERN:
+		if (!ctrl->value) {
+			ret = mt9p031_set_mode2(mt9p031,
+					0, MT9P031_READ_MODE_2_ROW_BLC);
+			if (ret < 0)
+				return ret;
+
+			return mt9p031_write(client, MT9P031_TEST_PATTERN,
+					     MT9P031_TEST_PATTERN_DISABLE);
+		}
+
+		ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
+		if (ret < 0)
+			return ret;
+		ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
+		if (ret < 0)
+			return ret;
+		ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
+		if (ret < 0)
+			return ret;
+
+		ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
+					0);
+		if (ret < 0)
+			return ret;
+		ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
+		if (ret < 0)
+			return ret;
+
+		return mt9p031_write(client, MT9P031_TEST_PATTERN,
+				((ctrl->value - 1) << MT9P031_TEST_PATTERN_SHIFT)
+				| MT9P031_TEST_PATTERN_ENABLE);
+	}
+	return 0;
+}
+
+static int mt9p031_g_chip_ident(struct v4l2_subdev *sd,
+				struct v4l2_dbg_chip_ident *id)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct mt9p031 *mt9p031 = to_mt9p031(client);
+
+	if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR);
+		return -EINVAL;
+
+	if (id->match.addr != client->addr)
+		return -EINVAL;
+
+	id->ident = mt9p031->model;
+	id->revision = 0;
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mt9p031_g_register(struct v4l2_subdev *sd,
+                              struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	reg->size = 2;
+	reg->val = mt9p031_read(client, reg->reg);
+
+	if (reg->val > 0xffff)
+		return -EIO;
+
+	return 0;
+}
+
+static int mt9p031_s_register(struct v4l2_subdev *sd,
+			      struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
+		return -EINVAL;
+
+	if (reg->match.addr != client->addr)
+		return -ENODEV;
+
+	if (mt9p031_write(client, reg->reg, reg->val) < 0)
+		return -EIO;
+
+	return 0;
+}
+#endif
+
+static struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
+	.s_ctrl		= mt9p031_s_ctrl,
+	.g_chip_ident	= mt9p031_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register     = mt9p031_g_register,
+	.s_register     = mt9p031_s_register,
+#endif
+};
+
+static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
+	.s_stream	= mt9p031_s_stream,
+	.s_mbus_fmt	= mt9p031_s_fmt,
+	.try_mbus_fmt	= mt9p031_try_fmt,
+	.s_crop		= mt9p031_s_crop,
+	.g_crop		= mt9p031_g_crop,
+	.cropcap	= mt9p031_cropcap,
+	.g_mbus_fmt	= mt9p031_g_fmt,
+	.enum_mbus_fmt	= mt9p031_enum_fmt,
+};
+
+static struct v4l2_subdev_ops mt9p031_subdev_ops = {
+	.core	= &mt9p031_subdev_core_ops,
+	.video	= &mt9p031_subdev_video_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Driver initialization and probing
+ */
+
+static int mt9p031_init(struct i2c_client *client)
+{
+	int ret;
+
+	ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, MT9P031_WINDOW_WIDTH_DEF);
+	if (!ret)
+		mt9p031_write(client, MT9P031_WINDOW_HEIGHT, MT9P031_WINDOW_HEIGHT_DEF);
+	if (!ret)
+		mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER, MT9P031_SHUTTER_WIDTH_DEF);
+
+	return ret;
+}
+
+static int mt9p031_video_probe(struct soc_camera_device *icd,
+					struct i2c_client *client)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(client);
+	struct soc_camera_link *icl = to_soc_camera_link(icd);
+	s32 data;
+	int ret;
+	unsigned long flags;
+
+	if (!icd->dev.parent ||
+	    to_soc_camera_host(icd->dev.parent)->nr != icd->iface) {
+		return -ENODEV;
+	}
+
+	data = mt9p031_read(client, MT9P031_CHIP_VERSION);
+	switch (data) {
+		case MT9P031_CHIP_VERSION_VALUE:
+			printk("%s: MT9P031 camera found, chip ID: 0x%02x\n", __func__, data);
+			mt9p031->model = V4L2_IDENT_MT9P031;
+			break;
+		default:
+			printk("%s: No MT9P031 camera found\n", __func__);
+			return -EINVAL;
+	}
+
+	if (g_is_mono)
+		mt9p031->fmts = mt9p031_monochrome_fmts;
+	else
+		mt9p031->fmts = mt9p031_colour_fmts;
+
+	mt9p031->num_fmts = 0;
+
+	/*
+	 * This is a 12bit sensor, but by default we only allow 10bit.
+	 * The platform may support different bus widths due to
+	 * different routing of the data lines.
+	 */
+	if (icl->query_bus_param) {
+		flags = icl->query_bus_param(icl);
+	} else {
+		flags = SOCAM_DATAWIDTH_10;
+	}
+	if (flags & SOCAM_DATAWIDTH_10)
+		mt9p031->num_fmts++;
+	else {
+		mt9p031->fmts++;
+	}
+
+	mt9p031->fmt = &mt9p031->fmts[0];
+
+	ret = mt9p031_init(client);
+	if (ret < 0)
+		dev_err(&client->dev, "Failed to initialise the camera\n");
+
+	return ret;
+}
+
+static int mt9p031_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct mt9p031 *mt9p031;
+	struct soc_camera_device *icd = client->dev.platform_data;
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct soc_camera_link *icl;
+	int ret = 0;
+
+	if (!icd) {
+		dev_err(&client->dev, "MT9P031: missing soc-camera data!\n");
+		return -EINVAL;
+	}
+
+	icl = to_soc_camera_link(icd);
+	if (!icl) {
+		dev_err(&client->dev, "MT9P031 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");
+		return -EIO;
+	}
+
+	mt9p031 = kzalloc(sizeof(struct mt9p031), GFP_KERNEL);
+	if (!mt9p031)
+		return -ENOMEM;
+
+	mt9p031->output_control	= MT9P031_OUTPUT_CONTROL_DEF;
+	mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
+
+	v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
+
+	icd->ops = &mt9p031_ops;
+
+	mt9p031->rect.width = MT9P031_WINDOW_WIDTH_DEF;
+	mt9p031->rect.height = MT9P031_WINDOW_HEIGHT_DEF;
+	mt9p031->rect.left = MT9P031_COLUMN_START_DEF;
+	mt9p031->rect.top = MT9P031_ROW_START_DEF;
+
+	ret = mt9p031_pll_get_divs(mt9p031);
+
+	ret = mt9p031_video_probe(icd, client);
+	if (ret) {
+		icd->ops = NULL;
+		kfree(mt9p031);
+	}
+
+	return ret;
+}
+
+static int mt9p031_remove(struct i2c_client *client)
+{
+	struct mt9p031 *mt9p031 = to_mt9p031(client);
+	struct soc_camera_device *icd = client->dev.platform_data;
+
+	icd->ops = NULL;
+	mt9p031_video_remove(icd);
+	kfree(mt9p031);
+
+	return 0;
+}
+
+static const struct i2c_device_id mt9p031_id[] = {
+	{ "mt9p031", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mt9p031_id);
+
+static struct i2c_driver mt9p031_i2c_driver = {
+	.driver = {
+		.name = "mt9p031",
+	},
+	.probe		= mt9p031_probe,
+	.remove		= mt9p031_remove,
+	.id_table	= mt9p031_id,
+};
+
+static int __init mt9p031_mod_init(void)
+{
+	return i2c_add_driver(&mt9p031_i2c_driver);
+}
+
+static void __exit mt9p031_mod_exit(void)
+{
+	i2c_del_driver(&mt9p031_i2c_driver);
+}
+
+module_init(mt9p031_mod_init);
+module_exit(mt9p031_mod_exit);
+
+MODULE_DESCRIPTION("Micron MT9P031 Camera driver");
+MODULE_AUTHOR("Raman Tunik <r.tunik@sam-solutions.net>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc_camera.c b/drivers/media/video/mxc_camera.c
index 11390f5829aa..ff8e3a9a468c 100644
--- a/drivers/media/video/mxc_camera.c
+++ b/drivers/media/video/mxc_camera.c
@@ -1,4 +1,4 @@
-
+#define DEBUG
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/version.h>
@@ -692,9 +692,13 @@ static int test_platform_param(struct mxc_camera_dev *mxc_cam,
 	 * If requested data width is supported by the platform, use it or any
 	 * possible lower value - i.MXC1 is smart enough to schift bits
 	 */
+
 	if (mxc_cam->platform_flags & MXC_CAMERA_DATAWIDTH_15)
 		*flags |= SOCAM_DATAWIDTH_15 | SOCAM_DATAWIDTH_10 |
 			SOCAM_DATAWIDTH_8 | SOCAM_DATAWIDTH_4;
+	else if (mxc_cam->platform_flags & MXC_CAMERA_DATAWIDTH_12) 
+		*flags |= SOCAM_DATAWIDTH_12 | SOCAM_DATAWIDTH_10 |
+			SOCAM_DATAWIDTH_8 | SOCAM_DATAWIDTH_4;
 	else if (mxc_cam->platform_flags & MXC_CAMERA_DATAWIDTH_10)
 		*flags |= SOCAM_DATAWIDTH_10 | SOCAM_DATAWIDTH_8 |
 			SOCAM_DATAWIDTH_4;
@@ -708,6 +712,11 @@ static int test_platform_param(struct mxc_camera_dev *mxc_cam,
 		if (!(*flags & SOCAM_DATAWIDTH_15))
 			return -EINVAL;
 		break;
+	case 12:
+		if (!(*flags & SOCAM_DATAWIDTH_12)) {
+			return -EINVAL;
+		}
+		break;
 	case 10:
 		if (!(*flags & SOCAM_DATAWIDTH_10))
 			return -EINVAL;
@@ -998,9 +1007,13 @@ static int mxc_camera_set_bus_param(struct soc_camera_device *icd, __u32 pixfmt)
 	 * Make the camera work in widest common mode, we'll take care of
 	 * the rest
 	 */
+
 	if (common_flags & SOCAM_DATAWIDTH_15)
 		common_flags = (common_flags & ~SOCAM_DATAWIDTH_MASK) |
 			SOCAM_DATAWIDTH_15;
+	else if (common_flags & SOCAM_DATAWIDTH_12)
+		common_flags = (common_flags & ~SOCAM_DATAWIDTH_MASK) |
+			SOCAM_DATAWIDTH_12;
 	else if (common_flags & SOCAM_DATAWIDTH_10)
 		common_flags = (common_flags & ~SOCAM_DATAWIDTH_MASK) |
 			SOCAM_DATAWIDTH_10;
@@ -1125,7 +1138,7 @@ static int __devinit mxc_camera_probe(struct platform_device *pdev)
 	/* TODO: add other data widths */
 	if (!(mxc_cam->platform_flags & (MXC_CAMERA_DATAWIDTH_4 |
 			MXC_CAMERA_DATAWIDTH_8 | MXC_CAMERA_DATAWIDTH_10 |
-			MXC_CAMERA_DATAWIDTH_15))) {
+			MXC_CAMERA_DATAWIDTH_12 | MXC_CAMERA_DATAWIDTH_15))) {
 		/*
 		 * Platform hasn't set available data widths. This is bad.
 		 * Warn and use a default.
diff --git a/include/media/mt9p031.h b/include/media/mt9p031.h
new file mode 100644
index 000000000000..12ead5e54238
--- /dev/null
+++ b/include/media/mt9p031.h
@@ -0,0 +1,19 @@
+#ifndef _MEDIA_MT9P031_H
+#define _MEDIA_MT9P031_H
+
+struct v4l2_subdev;
+
+enum {
+	MT9P031_COLOR_VERSION,
+	MT9P031_MONOCHROME_VERSION,
+};
+
+struct mt9p031_platform_data {
+	int (*set_clock)(struct v4l2_subdev *subdev, unsigned int rate);
+	int (*reset)(struct v4l2_subdev *subdev, int active);
+	u32 ext_freq; /* input frequency to the mt9p031 for PLL dividers */
+	u32 target_freq; /* frequency target for the PLL */
+	int version; /* MT9P031_COLOR_VERSION or MT9P031_MONOCHROME_VERSION */
+};
+
+#endif
diff --git a/include/media/soc_camera.h b/include/media/soc_camera.h
index 238bd334fd83..68520c6611eb 100644
--- a/include/media/soc_camera.h
+++ b/include/media/soc_camera.h
@@ -264,16 +264,17 @@ static inline struct v4l2_queryctrl const *soc_camera_find_qctrl(
 #define SOCAM_DATAWIDTH_8		(1 << 7)
 #define SOCAM_DATAWIDTH_9		(1 << 8)
 #define SOCAM_DATAWIDTH_10		(1 << 9)
-#define SOCAM_DATAWIDTH_15		(1 << 10)
-#define SOCAM_DATAWIDTH_16		(1 << 11)
-#define SOCAM_PCLK_SAMPLE_RISING	(1 << 12)
-#define SOCAM_PCLK_SAMPLE_FALLING	(1 << 13)
-#define SOCAM_DATA_ACTIVE_HIGH		(1 << 14)
-#define SOCAM_DATA_ACTIVE_LOW		(1 << 15)
+#define SOCAM_DATAWIDTH_12		(1 << 10)
+#define SOCAM_DATAWIDTH_15		(1 << 11)
+#define SOCAM_DATAWIDTH_16		(1 << 12)
+#define SOCAM_PCLK_SAMPLE_RISING	(1 << 13)
+#define SOCAM_PCLK_SAMPLE_FALLING	(1 << 14)
+#define SOCAM_DATA_ACTIVE_HIGH		(1 << 15)
+#define SOCAM_DATA_ACTIVE_LOW		(1 << 16)
 
 #define SOCAM_DATAWIDTH_MASK (SOCAM_DATAWIDTH_4 | SOCAM_DATAWIDTH_8 | \
 			      SOCAM_DATAWIDTH_9 | SOCAM_DATAWIDTH_10 | \
-			      SOCAM_DATAWIDTH_15 | SOCAM_DATAWIDTH_16)
+			      SOCAM_DATAWIDTH_12 | SOCAM_DATAWIDTH_15 | SOCAM_DATAWIDTH_16)
 
 static inline unsigned long soc_camera_bus_param_compatible(
 			unsigned long camera_flags, unsigned long bus_flags)
diff --git a/include/media/v4l2-chip-ident.h b/include/media/v4l2-chip-ident.h
index 4c3a89cbb491..f0f0d5b8ea37 100644
--- a/include/media/v4l2-chip-ident.h
+++ b/include/media/v4l2-chip-ident.h
@@ -298,6 +298,7 @@ enum {
 	V4L2_IDENT_MT9T112		= 45022,
 	V4L2_IDENT_MT9V111		= 45031,
 	V4L2_IDENT_MT9V112		= 45032,
+	V4L2_IDENT_MT9P031		= 45033,
 
 	/* HV7131R CMOS sensor: just ident 46000 */
 	V4L2_IDENT_HV7131R		= 46000,