diff mbox

[RFC] Another OV7670 Soc-camera driver

Message ID 49C89518.90804@gmail.com (mailing list archive)
State RFC
Headers show

Commit Message

Darius March 24, 2009, 8:08 a.m. UTC 
Hi All,

seems everybody has their own OV7670 driver... I have one written from scratch too.
It is written using Omnivision user manual, application note, and register reference settings.
Omnivision provides settings arrays for every resolution (VGA, QVGA, QQVGA, CIF, QCIF).
This driver has lot of harcoded magic numbers. Of course OV7670 has lot of undocumented mystery and strange bugs.
Maybe my work could be useful merging all available OV7670 drivers into single one.
This driver is tested with MXLADS v2.0 board.

So there it is:
---
From: Darius Augulis <augulis.darius@gmail.com>

Soc camera driver for OV7670 sensor from OMNIVISION.

Signed-off-by: Darius Augulis <augulis.darius@gmail.com>
diff mbox

Patch

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");