Index: linux-2.6.29-rc5/drivers/media/video/Kconfig
===================================================================
--- linux-2.6.29-rc5.orig/drivers/media/video/Kconfig
+++ linux-2.6.29-rc5/drivers/media/video/Kconfig
@@ -779,6 +779,13 @@ config SOC_CAMERA_OV772X
 	help
 	  This is a ov772x camera driver
 
+config SOC_CAMERA_OV7670
+	tristate "ov7670 support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a driver for OmniVision OV7670 VGA camera.
+	  It currently only works with SoC Camera interface.
+
 config VIDEO_PXA27x
 	tristate "PXA27x Quick Capture Interface driver"
 	depends on VIDEO_DEV && PXA27x && SOC_CAMERA
Index: linux-2.6.29-rc5/drivers/media/video/Makefile
===================================================================
--- linux-2.6.29-rc5.orig/drivers/media/video/Makefile
+++ linux-2.6.29-rc5/drivers/media/video/Makefile
@@ -104,7 +104,7 @@ obj-$(CONFIG_VIDEO_UPD64083) += upd64083
 obj-$(CONFIG_VIDEO_CX2341X) += cx2341x.o
 
 obj-$(CONFIG_VIDEO_CAFE_CCIC) += cafe_ccic.o
-obj-$(CONFIG_VIDEO_OV7670) 	+= ov7670.o
+obj-$(CONFIG_VIDEO_OV7670)	+= ov7670.o
 
 obj-$(CONFIG_VIDEO_TCM825X) += tcm825x.o
 
@@ -144,6 +144,7 @@ obj-$(CONFIG_SOC_CAMERA_MT9M111)	+= mt9m
 obj-$(CONFIG_SOC_CAMERA_MT9T031)	+= mt9t031.o
 obj-$(CONFIG_SOC_CAMERA_MT9V022)	+= mt9v022.o
 obj-$(CONFIG_SOC_CAMERA_OV772X)		+= ov772x.o
+obj-$(CONFIG_SOC_CAMERA_OV7670)		+= ov7670_soc.o
 obj-$(CONFIG_SOC_CAMERA_PLATFORM)	+= soc_camera_platform.o
 obj-$(CONFIG_SOC_CAMERA_TW9910)		+= tw9910.o
 
Index: linux-2.6.29-rc5/drivers/media/video/ov7670_soc.c
===================================================================
--- /dev/null
+++ linux-2.6.29-rc5/drivers/media/video/ov7670_soc.c
@@ -0,0 +1,1411 @@
+/*
+ * Driver for OV7670 CMOS Image Sensor from OmniVision
+ *
+ * Copyright (C) 2008, Darius Augulis <darius.augulis@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+/* Remove comment to compile in debug messages */
+//#define DEBUG
+
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/log2.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/soc_camera.h>
+
+/* OV7670 has slave I2C address 0x42 */
+#define OV7670_I2C_ADDR 0x42
+
+/* OV7670 registers */
+#define	OV7670_PID			0x0A	/*	76	R	*/
+#define	OV7670_VER			0x0B	/*	73	R	*/
+#define	OV7670_COM7			0x12	/*	0	RW	*/
+#define	OV7670_COM10			0x15	/*	0	RW	*/
+#define	OV7670_MIDH			0x1C	/*	7F	R	*/
+#define	OV7670_MIDL			0x1D	/*	A2	R	*/
+#define	OV7670_MVFP			0x1E	/*	1	RW	*/
+#define	OV7670_BRIGHT			0x55	/*	0	RW	*/
+#define	OV7670_CONTRAS			0x56	/*	40	RW	*/
+
+static const struct soc_camera_data_format ov7670_formats[] = {
+	{
+		.name		= "YUV 4:2:2",
+		.depth		= 16,
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+	}
+};
+
+struct ov7670 {
+	struct i2c_client *client;
+	struct soc_camera_device icd;
+	int model;
+};
+
+/* Camera sensor register access functions */
+static u8 reg_read(struct i2c_client *client, const u8 reg)
+{
+	u8 i2c_buf[2];
+	i2c_buf[0] = reg;
+	i2c_master_send(client, i2c_buf, 1);
+	i2c_master_recv(client, i2c_buf, 1);
+	return i2c_buf[0];
+}
+
+static u8 reg_write(struct i2c_client *client, const u8 reg, u8 data)
+{
+	u8 i2c_buf[2];
+	i2c_buf[0] = reg;
+	i2c_buf[1] = data;
+	return i2c_master_send(client, i2c_buf, 2);
+}
+
+/* YCbCr, VGA
 * 15fps @ 24MHz input clock
+ */
+u8 init_array_vga[][2] = {
+	{ 0x11, 0x03 },
+	{ 0x3a, 0x04 },
+	{ 0x12, 0x00 },
+	{ 0x17, 0x13 },
+	{ 0x18, 0x01 },
+	{ 0x32, 0xb6 },
+	{ 0x19, 0x02 },
+	{ 0x1a, 0x7a },
+	{ 0x03, 0x0a },
+	{ 0x0c, 0x00 },
+	{ 0x3e, 0x00 },
+	{ 0x70, 0x3a },
+	{ 0x71, 0x35 },
+	{ 0x72, 0x11 },
+	{ 0x73, 0xf0 },
+	{ 0xa2, 0x02 },
+
+	{ 0x7a, 0x20 },
+	{ 0x7b, 0x10 },
+	{ 0x7c, 0x1e },
+	{ 0x7d, 0x35 },
+	{ 0x7e, 0x5a },
+	{ 0x7f, 0x69 },
+	{ 0x80, 0x76 },
+	{ 0x81, 0x80 },
+	{ 0x82, 0x88 },
+	{ 0x83, 0x8f },
+	{ 0x84, 0x96 },
+	{ 0x85, 0xa3 },
+	{ 0x86, 0xaf },
+	{ 0x87, 0xc4 },
+	{ 0x88, 0xd7 },
+	{ 0x89, 0xe8 },
+
+	{ 0x13, 0xe0 },
+	{ 0x00, 0x00 },
+	{ 0x10, 0x00 },
+	{ 0x0d, 0x40 },
+	{ 0x14, 0x18 },
+	{ 0xa5, 0x05 },
+	{ 0xab, 0x07 },
+	{ 0x24, 0x95 },
+	{ 0x25, 0x33 },
+	{ 0x26, 0xe3 },
+	{ 0x9f, 0x78 },
+	{ 0xa0, 0x68 },
+	{ 0xa1, 0x03 },
+	{ 0xa6, 0xd8 },
+	{ 0xa7, 0xd8 },
+	{ 0xa8, 0xf0 },
+	{ 0xa9, 0x90 },
+	{ 0xaa, 0x94 },
+	{ 0x13, 0xe5 },
+
+	{ 0x0e, 0x61 },
+	{ 0x0f, 0x4b },
+	{ 0x16, 0x02 },
+	{ 0x1e, 0x07 },
+	{ 0x21, 0x02 },
+	{ 0x22, 0x91 },
+	{ 0x29, 0x07 },
+	{ 0x33, 0x0b },
+	{ 0x35, 0x0b },
+	{ 0x37, 0x1d },
+	{ 0x38, 0x71 },
+	{ 0x39, 0x2a },
+	{ 0x3c, 0x78 },
+	{ 0x4d, 0x40 },
+	{ 0x4e, 0x20 },
+	{ 0x69, 0x00 },
+	{ 0x6b, 0x4a },
+	{ 0x74, 0x10 },
+	{ 0x8d, 0x4f },
+	{ 0x8e, 0x00 },
+	{ 0x8f, 0x00 },
+	{ 0x90, 0x00 },
+	{ 0x91, 0x00 },
+	{ 0x96, 0x00 },
+	{ 0x9a, 0x80 },
+	{ 0xb0, 0x84 },
+	{ 0xb1, 0x0c },
+	{ 0xb2, 0x0e },
+	{ 0xb3, 0x82 },
+	{ 0xb8, 0x0a },
+
+	{ 0x43, 0x0a },
+	{ 0x44, 0xf0 },
+	{ 0x45, 0x34 },
+	{ 0x46, 0x58 },
+	{ 0x47, 0x28 },
+	{ 0x48, 0x3a },
+	{ 0x59, 0x88 },
+	{ 0x5a, 0x88 },
+	{ 0x5b, 0x44 },
+	{ 0x5c, 0x67 },
+	{ 0x5d, 0x49 },
+	{ 0x5e, 0x0e },
+	{ 0x6c, 0x0a },
+	{ 0x6d, 0x55 },
+	{ 0x6e, 0x11 },
+	{ 0x6f, 0x9f }, /* 9e */
+	{ 0x6a, 0x40 },
+	{ 0x01, 0x40 },
+	{ 0x02, 0x40 },
+	{ 0x13, 0xe7 },
+
+	{ 0x4f, 0x80 },
+	{ 0x50, 0x80 },
+	{ 0x51, 0x00 },
+	{ 0x52, 0x22 },
+	{ 0x53, 0x5e },
+	{ 0x54, 0x80 },
+	{ 0x58, 0x9e },
+
+	{ 0x41, 0x08 },
+	{ 0x3f, 0x00 },
+	{ 0x75, 0x05 },
+	{ 0x76, 0xe1 },
+	{ 0x4c, 0x00 },
+	{ 0x77, 0x01 },
+	{ 0x3d, 0xc2 },
+	{ 0x4b, 0x09 },
+	{ 0xc9, 0x60 },
+	{ 0x41, 0x38 },
+	{ 0x56, 0x40 },
+
+	{ 0x34, 0x11 },
+	{ 0x3b, 0x02 },
+	{ 0xa4, 0x88 },
+	{ 0x96, 0x00 },
+	{ 0x97, 0x30 },
+	{ 0x98, 0x20 },
+	{ 0x99, 0x30 },
+	{ 0x9a, 0x84 },
+	{ 0x9b, 0x29 },
+	{ 0x9c, 0x03 },
+	{ 0x9d, 0x4c },
+	{ 0x9e, 0x3f },
+	{ 0x78, 0x04 },
+};
+
+/* YCbCr, QVGA
+ * 15fps @ 24MHz input clock
+ */
+u8 init_array_qvga[][2] = {
+	{ 0x11, 0x03 },
+	{ 0x3a, 0x04 },
+	{ 0x12, 0x00 },
+	{ 0x32, 0x80 },
+	{ 0x17, 0x16 },
+	{ 0x18, 0x04 },
+	{ 0x19, 0x02 },
+	{ 0x1a, 0x7a },
+	{ 0x03, 0x0a },
+	{ 0x0c, 0x04 },
+	{ 0x3e, 0x19 },
+	{ 0x70, 0x3a },
+	{ 0x71, 0x35 },
+	{ 0x72, 0x11 },
+	{ 0x73, 0xf1 },
+	{ 0xa2, 0x02 },
+
+	{ 0x7a, 0x20 },
+	{ 0x7b, 0x10 },
+	{ 0x7c, 0x1e },
+	{ 0x7d, 0x35 },
+	{ 0x7e, 0x5a },
+	{ 0x7f, 0x69 },
+	{ 0x80, 0x76 },
+	{ 0x81, 0x80 },
+	{ 0x82, 0x88 },
+	{ 0x83, 0x8f },
+	{ 0x84, 0x96 },
+	{ 0x85, 0xa3 },
+	{ 0x86, 0xaf },
+	{ 0x87, 0xc4 },
+	{ 0x88, 0xd7 },
+	{ 0x89, 0xe8 },
+
+	{ 0x13, 0xe0 },
+	{ 0x00, 0x00 },
+	{ 0x10, 0x00 },
+	{ 0x0d, 0x40 },
+	{ 0x14, 0x18 },
+	{ 0xa5, 0x05 },
+	{ 0xab, 0x07 },
+	{ 0x24, 0x95 },
+	{ 0x25, 0x33 },
+	{ 0x26, 0xe3 },
+	{ 0x9f, 0x78 },
+	{ 0xa0, 0x68 },
+	{ 0xa1, 0x03 },
+	{ 0xa6, 0xd8 },
+	{ 0xa7, 0xd8 },
+	{ 0xa8, 0xf0 },
+	{ 0xa9, 0x90 },
+	{ 0xaa, 0x94 },
+	{ 0x13, 0xe5 },
+
+	{ 0x0e, 0x61 },
+	{ 0x0f, 0x4b },
+	{ 0x16, 0x02 },
+	{ 0x1e, 0x07 },
+	{ 0x21, 0x02 },
+	{ 0x22, 0x91 },
+	{ 0x29, 0x07 },
+	{ 0x33, 0x0b },
+	{ 0x35, 0x0b },
+	{ 0x37, 0x1d },
+	{ 0x38, 0x71 },
+	{ 0x39, 0x2a },
+	{ 0x3c, 0x78 },
+	{ 0x4d, 0x40 },
+	{ 0x4e, 0x20 },
+	{ 0x69, 0x00 },
+	{ 0x6b, 0x4a },
+	{ 0x74, 0x10 },
+	{ 0x8d, 0x4f },
+	{ 0x8e, 0x00 },
+	{ 0x8f, 0x00 },
+	{ 0x90, 0x00 },
+	{ 0x91, 0x00 },
+	{ 0x96, 0x00 },
+	{ 0x9a, 0x80 },
+	{ 0xb0, 0x84 },
+	{ 0xb1, 0x0c },
+	{ 0xb2, 0x0e },
+	{ 0xb3, 0x82 },
+	{ 0xb8, 0x0a },
+
+	{ 0x43, 0x0a },
+	{ 0x44, 0xf0 },
+	{ 0x45, 0x34 },
+	{ 0x46, 0x58 },
+	{ 0x47, 0x28 },
+	{ 0x48, 0x3a },
+	{ 0x59, 0x88 },
+	{ 0x5a, 0x88 },
+	{ 0x5b, 0x44 },
+	{ 0x5c, 0x67 },
+	{ 0x5d, 0x49 },
+	{ 0x5e, 0x0e },
+	{ 0x6c, 0x0a },
+	{ 0x6d, 0x55 },
+	{ 0x6e, 0x11 },
+	{ 0x6f, 0x9f }, /*9e*/
+	{ 0x6a, 0x40 },
+	{ 0x01, 0x40 },
+	{ 0x02, 0x40 },
+	{ 0x13, 0xe7 },
+
+	{ 0x4f, 0x80 },
+	{ 0x50, 0x80 },
+	{ 0x51, 0x00 },
+	{ 0x52, 0x22 },
+	{ 0x53, 0x5e },
+	{ 0x54, 0x80 },
+	{ 0x58, 0x9e },
+
+	{ 0x41, 0x08 },
+	{ 0x3f, 0x00 },
+	{ 0x75, 0x05 },
+	{ 0x76, 0xe1 },
+	{ 0x4c, 0x00 },
+	{ 0x77, 0x01 },
+	{ 0x3d, 0xc0 },
+	{ 0x4b, 0x09 },
+	{ 0xc9, 0x60 },
+	{ 0x41, 0x38 },
+	{ 0x56, 0x40 },
+
+	{ 0x34, 0x11 },
+	{ 0x3b, 0x02 },
+	{ 0xa4, 0x88 },
+	{ 0x96, 0x00 },
+	{ 0x97, 0x30 },
+	{ 0x98, 0x20 },
+	{ 0x99, 0x30 },
+	{ 0x9a, 0x84 },
+	{ 0x9b, 0x29 },
+	{ 0x9c, 0x03 },
+	{ 0x9d, 0x4c },
+	{ 0x9e, 0x3f },
+	{ 0x78, 0x04 },
+
+	{ 0x79, 0x01 },
+	{ 0xc8, 0xf0 },
+	{ 0x79, 0x0f },
+	{ 0xc8, 0x00 },
+	{ 0x79, 0x10 },
+	{ 0xc8, 0x7e },
+	{ 0x79, 0x0a },
+	{ 0xc8, 0x80 },
+	{ 0x79, 0x0b },
+	{ 0xc8, 0x01 },
+	{ 0x79, 0x0c },
+	{ 0xc8, 0x0f },
+	{ 0x79, 0x0d },
+	{ 0xc8, 0x20 },
+	{ 0x79, 0x09 },
+	{ 0xc8, 0x80 },
+	{ 0x79, 0x02 },
+	{ 0xc8, 0xc0 },
+	{ 0x79, 0x03 },
+	{ 0xc8, 0x40 },
+	{ 0x79, 0x05 },
+	{ 0xc8, 0x30 },
+	{ 0x79, 0x26 },
+};
+
+/* Not tested */
+u8 init_array_qqvga[][2] = {
+	{ 0x11, 0x03},	/* CLKRC */
+	{ 0x3a, 0x04 }, /* TSLB */
+	{ 0x12, 0x00 }, /* COM7 */
+	{ 0x17, 0x16 }, /* HSTART */
+	{ 0x18, 0x04 }, /* HSTOP */
+	{ 0x32, 0xA4 }, /* HREF */
+	{ 0x19, 0x02 }, /* VSTRT */
+	{ 0x1a, 0x7a }, /* VSTOP */
+	{ 0x03, 0x0a }, /* VREF */
+	{ 0x0c, 0x04 }, /* COM3 */
+	{ 0x3e, 0x1A }, /* COM14 */
+	{ 0x70, 0x3a }, /* SCALING_XSC */
+	{ 0x71, 0x35 }, /* SCALING_YSC */
+	{ 0x72, 0x22 }, /* SCALING_DCWCTR */
+	{ 0x73, 0xf2 }, /* SCALING_PCLK_DIV */
+	{ 0xa2, 0x02 }, /* SCALING_PCLK_DELAY */
+	/* Gamma */
+	{ 0x7a, 0x20 },
+	{ 0x7b, 0x1c },
+	{ 0x7c, 0x28 },
+	{ 0x7d, 0x3c },
+	{ 0x7e, 0x5a },
+	{ 0x7f, 0x68 },
+	{ 0x80, 0x76 },
+	{ 0x81, 0x80 },
+	{ 0x82, 0x88 },
+	{ 0x83, 0x8f },
+	{ 0x84, 0x96 },
+	{ 0x85, 0xa3 },
+	{ 0x86, 0xaf },
+	{ 0x87, 0xc4 },
+	{ 0x88, 0xd7 },
+	{ 0x89, 0xe8 },
+	/* Gain, exposure, banding, AGC/AEC */
+	{ 0x13, 0xe0 },
+	{ 0x00, 0x00 },
+	{ 0x10, 0x00 },
+	{ 0x0d, 0x40 },
+	{ 0x14, 0x18 },
+	{ 0xa5, 0x05 },
+	{ 0xab, 0x07 },
+	{ 0x24, 0x95 },
+	{ 0x25, 0x33 },
+	{ 0x26, 0xe3 },
+	{ 0x9f, 0x78 },
+	{ 0xa0, 0x68 },
+	{ 0xa1, 0x03 },
+	{ 0xa6, 0xd8 },
+	{ 0xa7, 0xd8 },
+	{ 0xa8, 0xf0 },
+	{ 0xa9, 0x90 },
+	{ 0xaa, 0x94 },
+	{ 0x13, 0xe5 },
+	/* Gain, ADC, PLL, etc... */
+	{ 0x0e, 0x61 },
+	{ 0x0f, 0x4b },
+	{ 0x16, 0x02 },
+	{ 0x1E, 0x07 },
+	{ 0x21, 0x02 },
+	{ 0x22, 0x91 },
+	{ 0x29, 0x07 },
+	{ 0x33, 0x0B },
+	{ 0x35, 0x0B },
+	{ 0x37, 0x1D },
+	{ 0x38, 0x71 },
+	{ 0x39, 0x2a },
+	{ 0x3c, 0x78 },
+	{ 0x4d, 0x40 },
+	{ 0x4e, 0x20 },
+	{ 0x69, 0x00 },
+	{ 0x74, 0x10 },
+	{ 0x8d, 0x4f },
+	{ 0x8e, 0x00 },
+	{ 0x8f, 0x00 },
+	{ 0x90, 0x00 },
+	{ 0x91, 0x00 },
+	{ 0x96, 0x00 },
+	{ 0x9a, 0x80 },
+	{ 0xb0, 0x84 },
+	{ 0xb1, 0x0c },
+	{ 0xb2, 0x0e },
+	{ 0xb3, 0x82 },
+	{ 0xb8, 0x0a },
+	/* Reserved, AWB, Blue-Red Channel gain */
+	{ 0x43, 0x0a },
+	{ 0x44, 0xf0 },
+	{ 0x45, 0x34 },
+	{ 0x46, 0x58 },
+	{ 0x47, 0x28 },
+	{ 0x48, 0x3a },
+	{ 0x59, 0x88 },
+	{ 0x5a, 0x88 },
+	{ 0x5b, 0x44 },
+	{ 0x5c, 0x67 },
+	{ 0x5d, 0x49 },
+	{ 0x5e, 0x0e },
+	{ 0x6c, 0x0a },
+	{ 0x6d, 0x55 },
+	{ 0x6e, 0x11 },
+	{ 0x6f, 0x9f },
+	{ 0x6a, 0x40 },
+	{ 0x01, 0x40 },
+	{ 0x02, 0x40 },
+	{ 0x13, 0xe7 },
+	/* Matrix coefficient */
+	{ 0x4f, 0x80 },
+	{ 0x50, 0x80 },
+	{ 0x51, 0x00 },
+	{ 0x52, 0x22 },
+	{ 0x53, 0x5e },
+	{ 0x54, 0x80 },
+	{ 0x58, 0x9e },
+	/* Edge, pixel correction, de-noise, COM13, UV, Contrast */
+	{ 0x41, 0x08 },
+	{ 0x3f, 0x00 },
+	{ 0x75, 0x05 },
+	{ 0x76, 0xe1 },
+	{ 0x4c, 0x00 },
+	{ 0x77, 0x01 },
+	{ 0x3d, 0xc1 },
+	{ 0x4b, 0x09 },
+	{ 0xc9, 0x60 },
+	{ 0x41, 0x38 },
+	{ 0x56, 0x40 },
+	/* Exposure timing, banding filter */
+	{ 0x34, 0x11 },
+	{ 0x3b, 0x02 },
+	{ 0xa4, 0x88 },
+	{ 0x96, 0x00 },
+	{ 0x97, 0x30 },
+	{ 0x98, 0x20 },
+	{ 0x99, 0x30 },
+	{ 0x9a, 0x84 },
+	{ 0x9b, 0x29 },
+	{ 0x9c, 0x03 },
+	{ 0x9d, 0x4c },
+	{ 0x9e, 0x3f },
+	{ 0x78, 0x04 },
+	/* ??? */
+	{ 0x79, 0x01 },
+	{ 0xc8, 0xf0 },
+	{ 0x79, 0x0f },
+	{ 0xc8, 0x20 },
+	{ 0x79, 0x10 },
+	{ 0xc8, 0x7e },
+	{ 0x79, 0x0b },
+	{ 0xc8, 0x01 },
+	{ 0x79, 0x0c },
+	{ 0xc8, 0x07 },
+	{ 0x79, 0x0d },
+	{ 0xc8, 0x20 },
+	{ 0x79, 0x09 },
+	{ 0xc8, 0x80 },
+	{ 0x79, 0x02 },
+	{ 0xc8, 0xc0 },
+	{ 0x79, 0x03 },
+	{ 0xc8, 0x40 },
+	{ 0x79, 0x05 },
+	{ 0xc8, 0x30 },
+	{ 0x79, 0x26 },
+	/* Lens correction */
+	{ 0x64, 0x50 },
+	{ 0x65, 0x50 },
+	{ 0x66, 0x01 },
+	{ 0x94, 0x50 },
+	{ 0x95, 0x50 },
+};
+
+/* Not tested */
+u8 init_array_cif[][2] = {
+	{ 0x11, 0x03},	/* CLKRC */
+	{ 0x3a, 0x04 }, /* TSLB */
+	{ 0x12, 0x00 }, /* COM7 */
+	{ 0x17, 0x15 }, /* HSTART */
+	{ 0x18, 0x0b }, /* HSTOP */
+	{ 0x32, 0xb6 }, /* HREF */
+	{ 0x19, 0x03 }, /* VSTRT */
+	{ 0x1a, 0x7b }, /* VSTOP */
+	{ 0x03, 0x02 }, /* VREF */
+	{ 0x0c, 0x08 }, /* COM3 */
+	{ 0x3e, 0x11 }, /* COM14 */
+	{ 0x70, 0x3a }, /* SCALING_XSC */
+	{ 0x71, 0x35 }, /* SCALING_YSC */
+	{ 0x72, 0x11 }, /* SCALING_DCWCTR */
+	{ 0x73, 0xf1 }, /* SCALING_PCLK_DIV */
+	{ 0xa2, 0x02 }, /* SCALING_PCLK_DELAY */
+	/* Gamma */
+	{ 0x7a, 0x20 },
+	{ 0x7b, 0x10 },
+	{ 0x7c, 0x1e },
+	{ 0x7d, 0x35 },
+	{ 0x7e, 0x5a },
+	{ 0x7f, 0x69 },
+	{ 0x80, 0x76 },
+	{ 0x81, 0x80 },
+	{ 0x82, 0x88 },
+	{ 0x83, 0x8f },
+	{ 0x84, 0x96 },
+	{ 0x85, 0xa3 },
+	{ 0x86, 0xaf },
+	{ 0x87, 0xc4 },
+	{ 0x88, 0xd7 },
+	{ 0x89, 0xe8 },
+	/* Gain, exposure, banding, AGC/AEC */
+	{ 0x13, 0xe0 },
+	{ 0x00, 0x00 },
+	{ 0x10, 0x00 },
+	{ 0x0d, 0x40 },
+	{ 0x14, 0x18 },
+	{ 0xa5, 0x05 },
+	{ 0xab, 0x07 },
+	{ 0x24, 0x95 },
+	{ 0x25, 0x33 },
+	{ 0x26, 0xe3 },
+	{ 0x9f, 0x78 },
+	{ 0xa0, 0x68 },
+	{ 0xa1, 0x03 },
+	{ 0xa6, 0xd8 },
+	{ 0xa7, 0xd8 },
+	{ 0xa8, 0xf0 },
+	{ 0xa9, 0x90 },
+	{ 0xaa, 0x94 },
+	{ 0x13, 0xe5 },
+	/* Gain, ADC, PLL, etc... */
+	{ 0x0e, 0x61 },
+	{ 0x0f, 0x4b },
+	{ 0x16, 0x02 },
+	{ 0x1E, 0x07 },
+	{ 0x21, 0x02 },
+	{ 0x22, 0x91 },
+	{ 0x29, 0x07 },
+	{ 0x33, 0x0B },
+	{ 0x35, 0x0B },
+	{ 0x37, 0x1D },
+	{ 0x38, 0x71 },
+	{ 0x39, 0x2a },
+	{ 0x3c, 0x78 },
+	{ 0x4d, 0x40 },
+	{ 0x4e, 0x20 },
+	{ 0x69, 0x00 },
+	{ 0x74, 0x10 },
+	{ 0x8d, 0x4f },
+	{ 0x8e, 0x00 },
+	{ 0x8f, 0x00 },
+	{ 0x90, 0x00 },
+	{ 0x91, 0x00 },
+	{ 0x96, 0x00 },
+	{ 0x9a, 0x80 },
+	{ 0xb0, 0x84 },
+	{ 0xb1, 0x0c },
+	{ 0xb2, 0x0e },
+	{ 0xb3, 0x82 },
+	{ 0xb8, 0x0a },
+	/* Reserved, AWB, Blue-Red Channel gain */
+	{ 0x43, 0x0a },
+	{ 0x44, 0xf0 },
+	{ 0x45, 0x34 },
+	{ 0x46, 0x58 },
+	{ 0x47, 0x28 },
+	{ 0x48, 0x3a },
+	{ 0x59, 0x88 },
+	{ 0x5a, 0x88 },
+	{ 0x5b, 0x44 },
+	{ 0x5c, 0x67 },
+	{ 0x5d, 0x49 },
+	{ 0x5e, 0x0e },
+	{ 0x6c, 0x0a },
+	{ 0x6d, 0x55 },
+	{ 0x6e, 0x11 },
+	{ 0x6f, 0x9f },
+	{ 0x6a, 0x40 },
+	{ 0x01, 0x40 },
+	{ 0x02, 0x40 },
+	{ 0x13, 0xe7 },
+	/* Matrix coefficient */
+	{ 0x4f, 0x80 },
+	{ 0x50, 0x80 },
+	{ 0x51, 0x00 },
+	{ 0x52, 0x22 },
+	{ 0x53, 0x5e },
+	{ 0x54, 0x80 },
+	{ 0x58, 0x9e },
+	/* Edge, pixel correction, de-noise, COM13, UV, Contrast */
+	{ 0x41, 0x08 },
+	{ 0x3f, 0x00 },
+	{ 0x75, 0x05 },
+	{ 0x76, 0xe1 },
+	{ 0x4c, 0x00 },
+	{ 0x77, 0x01 },
+	{ 0x3d, 0xc0 },
+	{ 0x4b, 0x09 },
+	{ 0xc9, 0x60 },
+	{ 0x41, 0x38 },
+	{ 0x56, 0x40 },
+	/* Exposure timing, banding filter */
+	{ 0x34, 0x11 },
+	{ 0x3b, 0x02 },
+	{ 0xa4, 0x88 },
+	{ 0x96, 0x00 },
+	{ 0x97, 0x30 },
+	{ 0x98, 0x20 },
+	{ 0x99, 0x30 },
+	{ 0x9a, 0x84 },
+	{ 0x9b, 0x29 },
+	{ 0x9c, 0x03 },
+	{ 0x9d, 0x4c },
+	{ 0x9e, 0x3f },
+	{ 0x78, 0x04 },
+	/* ??? */
+	{ 0x79, 0x01 },
+	{ 0xc8, 0xf0 },
+	{ 0x79, 0x0f },
+	{ 0xc8, 0x20 },
+	{ 0x79, 0x10 },
+	{ 0xc8, 0x7e },
+	{ 0x79, 0x0b },
+	{ 0xc8, 0x01 },
+	{ 0x79, 0x0c },
+	{ 0xc8, 0x07 },
+	{ 0x79, 0x0d },
+	{ 0xc8, 0x20 },
+	{ 0x79, 0x09 },
+	{ 0xc8, 0x80 },
+	{ 0x79, 0x02 },
+	{ 0xc8, 0xc0 },
+	{ 0x79, 0x03 },
+	{ 0xc8, 0x40 },
+	{ 0x79, 0x05 },
+	{ 0xc8, 0x30 },
+	{ 0x79, 0x26 },
+	/* Lens correction */
+	{ 0x64, 0x50 },
+	{ 0x65, 0x50 },
+	{ 0x66, 0x01 },
+	{ 0x94, 0x50 },
+	{ 0x95, 0x50 },
+};
+
+/* Not tested */
+u8 init_array_qcif[][2] = {
+	{ 0x11, 0x03 },	/* CLKRC */
+	{ 0x3a, 0x04 }, /* TSLB */
+	{ 0x12, 0x00 }, /* COM7 */
+	{ 0x17, 0x39 }, /* HSTART */
+	{ 0x18, 0x03 }, /* HSTOP */
+	{ 0x32, 0x80 }, /* HREF */
+	{ 0x19, 0x03 }, /* VSTRT */
+	{ 0x1a, 0x7b }, /* VSTOP */
+	{ 0x03, 0x02 }, /* VREF */
+	{ 0x0c, 0x0c }, /* COM3 */
+	{ 0x3e, 0x11 }, /* COM14 */
+	{ 0x70, 0x3a }, /* SCALING_XSC */
+	{ 0x71, 0x35 }, /* SCALING_YSC */
+	{ 0x72, 0x11 }, /* SCALING_DCWCTR */
+	{ 0x73, 0xf1 }, /* SCALING_PCLK_DIV */
+	{ 0xa2, 0x52 }, /* SCALING_PCLK_DELAY */
+	/* Gamma */
+	{ 0x7a, 0x20 },
+	{ 0x7b, 0x10 },
+	{ 0x7c, 0x1e },
+	{ 0x7d, 0x35 },
+	{ 0x7e, 0x5a },
+	{ 0x7f, 0x69 },
+	{ 0x80, 0x76 },
+	{ 0x81, 0x80 },
+	{ 0x82, 0x88 },
+	{ 0x83, 0x8f },
+	{ 0x84, 0x96 },
+	{ 0x85, 0xa3 },
+	{ 0x86, 0xaf },
+	{ 0x87, 0xc4 },
+	{ 0x88, 0xd7 },
+	{ 0x89, 0xe8 },
+	/* Gain, exposure, banding, AGC/AEC */
+	{ 0x13, 0xe0 },
+	{ 0x00, 0x00 },
+	{ 0x10, 0x00 },
+	{ 0x0d, 0x40 },
+	{ 0x14, 0x18 },
+	{ 0xa5, 0x05 },
+	{ 0xab, 0x07 },
+	{ 0x24, 0x95 },
+	{ 0x25, 0x33 },
+	{ 0x26, 0xe3 },
+	{ 0x9f, 0x78 },
+	{ 0xa0, 0x68 },
+	{ 0xa1, 0x03 },
+	{ 0xa6, 0xd8 },
+	{ 0xa7, 0xd8 },
+	{ 0xa8, 0xf0 },
+	{ 0xa9, 0x90 },
+	{ 0xaa, 0x94 },
+	{ 0x13, 0xe5 },
+	/* Gain, ADC, PLL, etc... */
+	{ 0x0e, 0x61 },
+	{ 0x0f, 0x4b },
+	{ 0x16, 0x02 },
+	{ 0x1E, 0x07 },
+	{ 0x21, 0x02 },
+	{ 0x22, 0x91 },
+	{ 0x29, 0x07 },
+	{ 0x33, 0x0B },
+	{ 0x35, 0x0B },
+	{ 0x37, 0x1D },
+	{ 0x38, 0x71 },
+	{ 0x39, 0x2a },
+	{ 0x3c, 0x78 },
+	{ 0x4d, 0x40 },
+	{ 0x4e, 0x20 },
+	{ 0x69, 0x00 },
+	{ 0x74, 0x10 },
+	{ 0x8d, 0x4f },
+	{ 0x8e, 0x00 },
+	{ 0x8f, 0x00 },
+	{ 0x90, 0x00 },
+	{ 0x91, 0x00 },
+	{ 0x96, 0x00 },
+	{ 0x9a, 0x80 },
+	{ 0xb0, 0x84 },
+	{ 0xb1, 0x0c },
+	{ 0xb2, 0x0e },
+	{ 0xb3, 0x82 },
+	{ 0xb8, 0x0a },
+	/* Reserved, AWB, Blue-Red Channel gain */
+	{ 0x43, 0x0a },
+	{ 0x44, 0xf0 },
+	{ 0x45, 0x34 },
+	{ 0x46, 0x58 },
+	{ 0x47, 0x28 },
+	{ 0x48, 0x3a },
+	{ 0x59, 0x88 },
+	{ 0x5a, 0x88 },
+	{ 0x5b, 0x44 },
+	{ 0x5c, 0x67 },
+	{ 0x5d, 0x49 },
+	{ 0x5e, 0x0e },
+	{ 0x6c, 0x0a },
+	{ 0x6d, 0x55 },
+	{ 0x6e, 0x11 },
+	{ 0x6f, 0x9f },
+	{ 0x6a, 0x40 },
+	{ 0x01, 0x40 },
+	{ 0x02, 0x40 },
+	{ 0x13, 0xe7 },
+	/* Matrix coefficient */
+	{ 0x4f, 0x80 },
+	{ 0x50, 0x80 },
+	{ 0x51, 0x00 },
+	{ 0x52, 0x22 },
+	{ 0x53, 0x5e },
+	{ 0x54, 0x80 },
+	{ 0x58, 0x9e },
+	/* Edge, pixel correction, de-noise, COM13, UV, Contrast */
+	{ 0x41, 0x08 },
+	{ 0x3f, 0x00 },
+	{ 0x75, 0x05 },
+	{ 0x76, 0xe1 },
+	{ 0x4c, 0x00 },
+	{ 0x77, 0x01 },
+	{ 0x3d, 0xc1 },
+	{ 0x4b, 0x09 },
+	{ 0xc9, 0x60 },
+	{ 0x41, 0x38 },
+	{ 0x56, 0x40 },
+	/* Exposure timing, banding filter */
+	{ 0x34, 0x11 },
+	{ 0x3b, 0x02 },
+	{ 0xa4, 0x88 },
+	{ 0x96, 0x00 },
+	{ 0x97, 0x30 },
+	{ 0x98, 0x20 },
+	{ 0x99, 0x30 },
+	{ 0x9a, 0x84 },
+	{ 0x9b, 0x29 },
+	{ 0x9c, 0x03 },
+	{ 0x9d, 0x4c },
+	{ 0x9e, 0x3f },
+	{ 0x78, 0x04 },
+	/* ??? */
+	{ 0x79, 0x01 },
+	{ 0xc8, 0xf0 },
+	{ 0x79, 0x0f },
+	{ 0xc8, 0x20 },
+	{ 0x79, 0x10 },
+	{ 0xc8, 0x7e },
+	{ 0x79, 0x0b },
+	{ 0xc8, 0x01 },
+	{ 0x79, 0x0c },
+	{ 0xc8, 0x07 },
+	{ 0x79, 0x0d },
+	{ 0xc8, 0x20 },
+	{ 0x79, 0x09 },
+	{ 0xc8, 0x80 },
+	{ 0x79, 0x02 },
+	{ 0xc8, 0xc0 },
+	{ 0x79, 0x03 },
+	{ 0xc8, 0x40 },
+	{ 0x79, 0x05 },
+	{ 0xc8, 0x30 },
+	{ 0x79, 0x26 },
+	/* Lens correction */
+	{ 0x64, 0x50 },
+	{ 0x65, 0x50 },
+	{ 0x66, 0x01 },
+	{ 0x94, 0x50 },
+	{ 0x95, 0x50 },
+};
+
+/* Camera sensor device functions */
+static int ov7670_video_probe(struct soc_camera_device *icd)
+{
+	struct ov7670 *ov7670 = container_of(icd, struct ov7670, icd);
+	u8 data;
+	int x;
+
+	dev_dbg(&icd->dev, "%s\n", __func__);
+
+	if (!icd->dev.parent ||
+		to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
+			return -ENODEV;
+
+	/* Read out Chip Id and version */
+	data = reg_read(ov7670->client, OV7670_MIDH);
+	dev_dbg(&icd->dev, "Chip IdH=0x%X\n", data);
+	if (data != 0x7F)
+		return -ENODEV;
+	data = reg_read(ov7670->client, OV7670_MIDL);
+	dev_dbg(&icd->dev, "Chip IdL=0x%X\n", data);
+	if (data != 0xA2)
+		return -ENODEV;
+	data = reg_read(ov7670->client, OV7670_PID);
+	dev_dbg(&icd->dev, "Chip PID=0x%X\n", data);
+	if (data != 0x76)
+		return -ENODEV;
+	data = reg_read(ov7670->client, OV7670_VER);
+	dev_dbg(&icd->dev, "Chip VER=0x%X\n", data);
+	if (data != 0x73)
+		return -ENODEV;
+
+	/* Now we already know that OV7670 chip is there */
+	dev_dbg(&icd->dev, "OmniVision camera OV7670 detected\n");
+
+	local_irq_disable();
+	/* Setup registers */
+	for (x = 0; x < ARRAY_SIZE(init_array_vga); x++)
+		reg_write(ov7670->client, init_array_vga[x][0],
+			init_array_vga[x][1]);
+	data = reg_read(ov7670->client, 0x11);
+	dev_dbg(&icd->dev, "Reg 0x11 is 0x%02X\n", data);
+	local_irq_enable();
+
+	/* Chip info setup */
+	ov7670->model = V4L2_IDENT_OV7670;
+	icd->formats = ov7670_formats;
+	icd->num_formats = ARRAY_SIZE(ov7670_formats);
+
+	/* If chip is detected, start video */
+	return soc_camera_video_start(icd);
+}
+
+static void ov7670_video_remove(struct soc_camera_device *icd)
+{
+	struct ov7670 *ov7670 = container_of(icd, struct ov7670, icd);
+	dev_dbg(&icd->dev, "%s\n", __func__);
+	soc_camera_video_stop(&ov7670->icd);
+}
+
+static int ov7670_init(struct soc_camera_device *icd)
+{
+	struct ov7670 *ov7670 = container_of(icd, struct ov7670, icd);
+
+	dev_dbg(&icd->dev, "%s\n", __func__);
+
+	/* Reset chip */
+	msleep(2);
+	reg_write(ov7670->client, OV7670_COM7, 0x80);
+	msleep(2);
+
+	return 0;
+}
+
+static int ov7670_release(struct soc_camera_device *icd)
+{
+	dev_dbg(&icd->dev, "%s\n", __func__);
+	return 0;
+}
+
+static int ov7670_start_capture(struct soc_camera_device *icd)
+{
+	dev_dbg(&icd->dev, "%s\n", __func__);
+	return 0;
+}
+
+static int ov7670_stop_capture(struct soc_camera_device *icd)
+{
+	dev_dbg(&icd->dev, "%s\n", __func__);
+	return 0;
+}
+
+static int ov7670_set_bus_param(struct soc_camera_device *icd,
+							unsigned long flags)
+{
+	dev_dbg(&icd->dev, "%s\n", __func__);
+	return 0;
+}
+
+static unsigned long ov7670_query_bus_param(struct soc_camera_device *icd)
+{
+	dev_dbg(&icd->dev, "%s\n", __func__);
+
+	/* Return OV7670 sensor bus capabilities */
+	return 	SOCAM_MASTER |
+		SOCAM_PCLK_SAMPLE_RISING |
+		SOCAM_HSYNC_ACTIVE_HIGH |
+		SOCAM_VSYNC_ACTIVE_HIGH	|
+		SOCAM_DATAWIDTH_8;
+}
+
+static int ov7670_set_fmt_cap(struct soc_camera_device *icd, __u32 pixfmt,
+							struct v4l2_rect *rect)
+{
+	struct ov7670 *ov7670 = container_of(icd, struct ov7670, icd);
+	int x;
+	u8 data;
+
+	/* Pixel format setup */
+	switch (pixfmt) {
+	case V4L2_PIX_FMT_YUYV:		/* YUV 4:2:2 */
+		dev_dbg(&icd->dev,
+			"%s: set format V4L2_PIX_FMT_YUYV\n", __func__);
+		break;
+	default:
+		goto exit1;
+	}
+
+
+	/* Resolution setup */
+	dev_dbg(&icd->dev, "%s: ask resolution W=%d, H=%d\n", __func__,
+		rect->width, rect->height);
+	dev_dbg(&icd->dev, "%s: current resolution is W=%d, H=%d\n", __func__,
+		icd->width, icd->height);
+
+	if ((rect->width != icd->width) && (rect->height != icd->height)) {
+
+		/* VGA */
+		if ((rect->width == 640) && (rect->height == 480)) {
+			dev_dbg(&icd->dev,
+				"%s: change resolution to W=%d, H=%d\n",
+					__func__, rect->width, rect->height);
+			local_irq_disable();
+			reg_write(ov7670->client, OV7670_COM7, 0x80);
+			msleep(2);
+			for (x = 0; x < ARRAY_SIZE(init_array_vga); x++)
+				reg_write(ov7670->client, init_array_vga[x][0],
+					init_array_vga[x][1]);
+			data = reg_read(ov7670->client, 0x11);
+			dev_dbg(&icd->dev, "Reg 0x11 is 0x%X\n", data);
+			local_irq_enable();
+		}
+		/* QVGA */
+		else if ((rect->width == 320) && (rect->height == 240)) {
+			dev_dbg(&icd->dev,
+				"%s: change resolution to W=%d, H=%d\n",
+					__func__, rect->width, rect->height);
+			local_irq_disable();
+			reg_write(ov7670->client, OV7670_COM7, 0x80);
+			msleep(2);
+			for (x = 0; x < ARRAY_SIZE(init_array_qvga); x++)
+				reg_write(ov7670->client, init_array_qvga[x][0],
+					init_array_qvga[x][1]);
+			data = reg_read(ov7670->client, 0x11);
+			dev_dbg(&icd->dev, "Reg 0x11 is 0x%X\n", data);
+			local_irq_enable();
+		}
+		/* QQVGA */
+		else if ((rect->width == 160) && (rect->height == 120)) {
+			dev_dbg(&icd->dev,
+				"%s: change resolution to W=%d, H=%d\n",
+					__func__, rect->width, rect->height);
+			local_irq_disable();
+			reg_write(ov7670->client, OV7670_COM7, 0x80);
+			msleep(2);
+			for (x = 0; x < ARRAY_SIZE(init_array_qqvga); x++)
+				reg_write(ov7670->client,
+					init_array_qqvga[x][0],
+						init_array_qqvga[x][1]);
+			data = reg_read(ov7670->client, 0x11);
+			dev_dbg(&icd->dev, "Reg 0x11 is 0x%X\n", data);
+			local_irq_enable();
+		}
+		/* CIF */
+		else if ((rect->width == 352) && (rect->height == 288)) {
+			dev_dbg(&icd->dev,
+				"%s: change resolution to W=%d, H=%d\n",
+					__func__, rect->width, rect->height);
+			local_irq_disable();
+			reg_write(ov7670->client, OV7670_COM7, 0x80);
+			msleep(2);
+			for (x = 0; x < ARRAY_SIZE(init_array_cif); x++)
+				reg_write(ov7670->client, init_array_cif[x][0],
+					init_array_cif[x][1]);
+			data = reg_read(ov7670->client, 0x11);
+			dev_dbg(&icd->dev, "Reg 0x11 is 0x%X\n", data);
+			local_irq_enable();
+		}
+		/* QCIF */
+		else if ((rect->width == 176) && (rect->height == 144)) {
+			dev_dbg(&icd->dev,
+				"%s: change resolution to W=%d, H=%d\n",
+					__func__, rect->width, rect->height);
+			local_irq_disable();
+			reg_write(ov7670->client, OV7670_COM7, 0x80);
+			msleep(2);
+			for (x = 0; x < ARRAY_SIZE(init_array_qcif); x++)
+				reg_write(ov7670->client, init_array_qcif[x][0],
+					init_array_qcif[x][1]);
+			data = reg_read(ov7670->client, 0x11);
+			dev_dbg(&icd->dev, "Reg 0x11 is 0x%X\n", data);
+			local_irq_enable();
+		} else
+			goto exit1;
+	}
+	return 0;
+exit1:
+	return -EINVAL;
+}
+
+static int ov7670_try_fmt_cap(struct soc_camera_device *icd,
+							struct v4l2_format *f)
+{
+	/* VGA */
+	if ((f->fmt.pix.width == 640) && (f->fmt.pix.height == 480))
+		goto exit1;
+	/* QVGA */
+	else if ((f->fmt.pix.width == 320) && (f->fmt.pix.height == 240))
+		goto exit1;
+	/* QQVGA */
+	else if ((f->fmt.pix.width == 160) && (f->fmt.pix.height == 120))
+		goto exit1;
+	/* CIF */
+	else if ((f->fmt.pix.width == 352) && (f->fmt.pix.height == 288))
+		goto exit1;
+	/* QCIF */
+	else if ((f->fmt.pix.width == 176) && (f->fmt.pix.height == 144))
+		goto exit1;
+	else
+		goto exit0;
+
+exit1:
+	dev_dbg(&icd->dev, "%s: Resolution W=%d, H=%d supported\n",
+		__func__, f->fmt.pix.width, f->fmt.pix.height);
+	return 0;
+exit0:
+	dev_dbg(&icd->dev, "%s: Resolution W=%d, H=%d NOT supported!\n",
+		__func__, f->fmt.pix.width, f->fmt.pix.height);
+	return -EINVAL;
+}
+
+static int ov7670_get_chip_id(struct soc_camera_device *icd,
+						struct v4l2_dbg_chip_ident *id)
+{
+	struct ov7670 *ov7670 = container_of(icd, struct ov7670, icd);
+
+	dev_dbg(&icd->dev, "%s\n", __func__);
+	if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
+		return -EINVAL;
+
+	if (id->match.addr != ov7670->client->addr)
+		return -ENODEV;
+
+	id->ident	= ov7670->model;
+	id->revision	= 0;
+	return 0;
+}
+
+static const struct v4l2_queryctrl ov7670_controls[] = {
+	{
+		.id		= V4L2_CID_BRIGHTNESS,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Brightness",
+		.minimum	= -127,
+		.maximum	= 127,
+		.step		= 1,
+		.default_value	= 0,
+		.flags		= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.id		= V4L2_CID_CONTRAST,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Contrast",
+		.minimum	= 0,
+		.maximum	= 127,
+		.step		= 1,
+		.default_value	= 0x40,
+		.flags		= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.id		= V4L2_CID_VFLIP,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "V Flip",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 0,
+	}, {
+		.id		= V4L2_CID_HFLIP,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "H Flip",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 0,
+	},
+};
+
+static int ov7670_get_control(struct soc_camera_device *,
+						struct v4l2_control *);
+static int ov7670_set_control(struct soc_camera_device *,
+						struct v4l2_control *);
+
+static struct soc_camera_ops ov7670_ops = {
+	.owner			= THIS_MODULE,
+	.probe			= ov7670_video_probe,
+	.remove			= ov7670_video_remove,
+	.init			= ov7670_init,
+	.release		= ov7670_release,
+	.start_capture		= ov7670_start_capture,
+	.stop_capture		= ov7670_stop_capture,
+	.set_fmt		= ov7670_set_fmt_cap,
+	.try_fmt		= ov7670_try_fmt_cap,
+	.set_bus_param		= ov7670_set_bus_param,
+	.query_bus_param	= ov7670_query_bus_param,
+	.controls		= ov7670_controls,
+	.num_controls		= ARRAY_SIZE(ov7670_controls),
+	.get_control		= ov7670_get_control,
+	.set_control		= ov7670_set_control,
+	.get_chip_id		= ov7670_get_chip_id,
+};
+
+static int ov7670_get_control(struct soc_camera_device *icd,
+						struct v4l2_control *ctrl)
+{
+	struct ov7670 *ov7670 = container_of(icd, struct ov7670, icd);
+
+	dev_dbg(&icd->dev, "%s\n", __func__);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		ctrl->value = reg_read(ov7670->client, OV7670_BRIGHT);
+		if (ctrl->value & 0x80)
+			ctrl->value = -(ctrl->value & ~0x80);
+		dev_dbg(&icd->dev, "%s Get control [V4L2_CID_BRIGHTNESS]=%d\n",
+			__func__, ctrl->value);
+		break;
+	case V4L2_CID_CONTRAST:
+		ctrl->value = reg_read(ov7670->client, OV7670_CONTRAS);
+		dev_dbg(&icd->dev, "%s Get control [V4L2_CID_CONTRAST]=%d\n",
+			__func__, ctrl->value);
+		break;
+	case V4L2_CID_VFLIP:
+		ctrl->value = (reg_read(ov7670->client,
+			OV7670_MVFP) >> 4) & 0x01;
+		dev_dbg(&icd->dev, "%s Get control [V4L2_CID_VFLIP]=%d\n",
+			__func__, ctrl->value);
+		break;
+	case V4L2_CID_HFLIP:
+		ctrl->value = (reg_read(ov7670->client,
+			OV7670_MVFP) >> 5) & 0x01;
+		dev_dbg(&icd->dev, "%s Get control [V4L2_CID_HFLIP]=%d\n",
+			__func__, ctrl->value);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ov7670_set_control(struct soc_camera_device *icd,
+						struct v4l2_control *ctrl)
+{
+	struct ov7670 *ov7670 = container_of(icd, struct ov7670, icd);
+	const struct v4l2_queryctrl *qctrl;
+	int ctrl_byte;
+
+	dev_dbg(&icd->dev, "%s\n", __func__);
+
+	qctrl = soc_camera_find_qctrl(&ov7670_ops, ctrl->id);
+	if (!qctrl)
+		return -EINVAL;
+
+	if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
+		return -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		dev_dbg(&icd->dev, "%s Set control [V4L2_CID_BRIGHTNESS]=%d\n",
+			__func__, ctrl->value);
+		if (ctrl->value < 0)
+			reg_write(ov7670->client, OV7670_BRIGHT,
+				abs(ctrl->value) | 0x80);
+		else
+			reg_write(ov7670->client, OV7670_BRIGHT, ctrl->value);
+		break;
+	case V4L2_CID_CONTRAST:
+		dev_dbg(&icd->dev, "%s Set control [V4L2_CID_CONTRAST]=%d\n",
+			__func__, ctrl->value);
+		reg_write(ov7670->client, OV7670_CONTRAS, ctrl->value);
+		break;
+	case V4L2_CID_VFLIP:
+		dev_dbg(&icd->dev, "%s Set control [V4L2_CID_VFLIP]=%d\n",
+			__func__, ctrl->value);
+		ctrl_byte = reg_read(ov7670->client, OV7670_MVFP);
+		ctrl_byte &= ~(1<<4);
+		ctrl_byte |= ctrl->value << 4;
+		reg_write(ov7670->client, OV7670_MVFP, ctrl_byte);
+		break;
+	case V4L2_CID_HFLIP:
+		dev_dbg(&icd->dev, "%s Set control [V4L2_CID_HFLIP]=%d\n",
+			__func__, ctrl->value);
+		ctrl_byte = reg_read(ov7670->client, OV7670_MVFP);
+		ctrl_byte &= ~(1<<5);
+		ctrl_byte |= ctrl->value << 5;
+		reg_write(ov7670->client, OV7670_MVFP, ctrl_byte);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ov7670_probe(struct i2c_client *client,
+						const struct i2c_device_id *did)
+{
+	struct ov7670 *ov7670;
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct soc_camera_device *icd;
+	struct soc_camera_link *icl = client->dev.platform_data;
+	int ret;
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	/* I2C adapter probe */
+	if (!icl) {
+		dev_err(&client->dev, "OV7670 driver needs platform data\n");
+		return -EINVAL;
+	}
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
+		dev_warn(&adapter->dev,
+			"I2C-Adapter doesn't support I2C_FUNC_I2C\n");
+		return -EIO;
+	}
+
+	ov7670 = kzalloc(sizeof(struct ov7670), GFP_KERNEL);
+	if (!ov7670)
+		return -ENOMEM;
+	ov7670->client = client;
+	i2c_set_clientdata(client, ov7670);
+
+	/* Setup camera capabilities in device structure for later use */
+	icd = &ov7670->icd;
+	icd->ops	= &ov7670_ops;
+	icd->control	= &client->dev;
+	icd->width	= 640;
+	icd->height	= 480;
+	icd->x_min	= 0;
+	icd->y_min	= 0;
+	icd->x_current	= 0;
+	icd->y_current	= 0;
+	icd->width_min	= 40;
+	icd->width_max	= 640;
+	icd->height_min	= 30;
+	icd->height_max	= 480;
+	icd->y_skip_top	= 0;
+	icd->iface	= icl->bus_id;
+
+	/* Register camera device */
+	ret = soc_camera_device_register(icd);
+	if (ret)
+		goto eisdr;
+	return 0;
+eisdr:
+	kfree(ov7670);
+	return ret;
+}
+
+static int ov7670_remove(struct i2c_client *client)
+{
+	struct ov7670 *ov7670 = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+	soc_camera_device_unregister(&ov7670->icd);
+	kfree(ov7670);
+	return 0;
+}
+
+static const struct i2c_device_id ov7670_id[] = {
+	{ "ov7xxx", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov7670_id);
+
+static struct i2c_driver ov7670_i2c_driver = {
+	.driver =
+			{
+				.name = "ov7670",
+			},
+	.probe		= ov7670_probe,
+	.remove		= ov7670_remove,
+	.id_table	= ov7670_id,
+};
+
+static int __init ov7670_mod_init(void)
+{
+	return i2c_add_driver(&ov7670_i2c_driver);
+}
+
+static void __exit ov7670_mod_exit(void)
+{
+	i2c_del_driver(&ov7670_i2c_driver);
+}
+
+module_init(ov7670_mod_init);
+module_exit(ov7670_mod_exit);
+
+MODULE_DESCRIPTION("OmniVision OV7670 Camera driver");
+MODULE_AUTHOR("Darius Augulis <darius.augulis@teltonika.lt>");
+MODULE_LICENSE("GPL");
