media: OV5640: add OV5640 SoC camera I2C device driver

OV5650 is a RAW sensor outputing YUV data. This driver uses SoC
camera interface and supports several different resolution.

Based on ov5642 driver. There seem to be some register differences,
so forking the driver to make those changes.

Change-Id: I5d889ca2ccf22a7c3be8cb26c1420b0fde0f1acf
Signed-off-by: Andrew Chew <achew@nvidia.com>
Signed-off-by: Bryan Wu <pengw@nvidia.com>
Reviewed-on: http://git-master/r/226464
GVS: Gerrit_Virtual_Submit
Reviewed-by: Allen Martin <amartin@nvidia.com>

1 files changed, 1569 insertions, 0 deletions

diff --git a/drivers/media/i2c/soc_camera/ov5640.c b/drivers/media/i2c/soc_camera/ov5640.c
new file mode 100644
index 000000000000..4ac6316aee5e
--- /dev/null
+++ b/drivers/media/i2c/soc_camera/ov5640.c
@@ -0,0 +1,1569 @@
+/*
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/soc_camera.h>
+
+struct ov5640_reg {
+	u16 addr;
+	u16 val;
+};
+
+#define OV5640_TABLE_WAIT_MS 0
+#define OV5640_TABLE_END 1
+
+static struct ov5640_reg mode_2592x1944[] = {
+	/* PLL Control MIPI bit rate/lane = 672MHz, 16-bit mode.
+	 * Output size: 2608x1948 (0, 0) - (2623, 1951),
+	 * Line Length = 2844, Frame Length = 1968
+	 */
+	{0x3103, 0x11},
+	{0x3008, 0x82},
+	{OV5640_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{0x3103, 0x03},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x3034, 0x18},
+	{0x3035, 0x11},
+	{0x3036, 0x7d},
+	{0x3037, 0x13},
+	{0x3108, 0x01},
+	{0x3630, 0x36},
+	{0x3631, 0x0e},
+	{0x3632, 0xe2},
+	{0x3633, 0x12},
+	{0x3621, 0xe0},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3905, 0x02},
+	{0x3906, 0x10},
+	{0x3901, 0x0a},
+	{0x3731, 0x12},
+	{0x3600, 0x08},
+	{0x3601, 0x33},
+	{0x302d, 0x60},
+	{0x3620, 0x52},
+	{0x371b, 0x20},
+	{0x471c, 0x50},
+	{0x3a13, 0x43},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3635, 0x13},
+	{0x3636, 0x03},
+	{0x3634, 0x40},
+	{0x3622, 0x01},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c06, 0x00},
+	{0x3c07, 0x07},
+	{0x3c08, 0x00},
+	{0x3c09, 0x1c},
+	{0x3c0a, 0x9c},
+	{0x3c0b, 0x40},
+	{0x3820, 0x40},
+	{0x3821, 0x06},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0x9f},
+	{0x3808, 0x0a},
+	{0x3809, 0x30},
+	{0x380a, 0x07},
+	{0x380b, 0x9c},
+	{0x380c, 0x0b},
+	{0x380d, 0x1c},
+	{0x380e, 0x07},
+	{0x380f, 0xb0},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x02},
+	{0x3618, 0x04},
+	{0x3612, 0x2b},
+	{0x3708, 0x64},
+	{0x3709, 0x12},
+	{0x370c, 0x00},
+	{0x3a02, 0x07},
+	{0x3a03, 0xb0},
+	{0x3a08, 0x01},
+	{0x3a09, 0x27},
+	{0x3a0a, 0x00},
+	{0x3a0b, 0xf6},
+	{0x3a0e, 0x06},
+	{0x3a0d, 0x08},
+	{0x3a14, 0x07},
+	{0x3a15, 0xb0},
+	{0x4001, 0x02},
+	{0x4004, 0x06},
+	{0x3000, 0x00},
+	{0x3002, 0x1c},
+	{0x3004, 0xff},
+	{0x3006, 0xc3},
+	{0x300e, 0x45},
+	{0x302e, 0x08},
+	{0x4300, 0x32},
+	{0x4800, 0x24},
+	{0x4837, 0x0a},
+	{0x501f, 0x00},
+	{0x440e, 0x00},
+	{0x5000, 0xa7},
+	{0x5001, 0x83},
+	{0x5180, 0xff},
+	{0x5181, 0xf2},
+	{0x5182, 0x00},
+	{0x5183, 0x14},
+	{0x5184, 0x25},
+	{0x5185, 0x24},
+	{0x5186, 0x09},
+	{0x5187, 0x09},
+	{0x5188, 0x09},
+	{0x5189, 0x75},
+	{0x518a, 0x54},
+	{0x518b, 0xe0},
+	{0x518c, 0xb2},
+	{0x518d, 0x42},
+	{0x518e, 0x3d},
+	{0x518f, 0x56},
+	{0x5190, 0x46},
+	{0x5191, 0xf8},
+	{0x5192, 0x04},
+	{0x5193, 0x70},
+	{0x5194, 0xf0},
+	{0x5195, 0xf0},
+	{0x5196, 0x03},
+	{0x5197, 0x01},
+	{0x5198, 0x04},
+	{0x5199, 0x12},
+	{0x519a, 0x04},
+	{0x519b, 0x00},
+	{0x519c, 0x06},
+	{0x519d, 0x82},
+	{0x519e, 0x38},
+	{0x5381, 0x1e},
+	{0x5382, 0x5b},
+	{0x5383, 0x08},
+	{0x5384, 0x0a},
+	{0x5385, 0x7e},
+	{0x5386, 0x88},
+	{0x5387, 0x7c},
+	{0x5388, 0x6c},
+	{0x5389, 0x10},
+	{0x538a, 0x01},
+	{0x538b, 0x98},
+	{0x5300, 0x08},
+	{0x5301, 0x30},
+	{0x5302, 0x10},
+	{0x5303, 0x00},
+	{0x5304, 0x08},
+	{0x5305, 0x30},
+	{0x5306, 0x08},
+	{0x5307, 0x16},
+	{0x5309, 0x08},
+	{0x530a, 0x30},
+	{0x530b, 0x04},
+	{0x530c, 0x06},
+	{0x5480, 0x01},
+	{0x5481, 0x08},
+	{0x5482, 0x14},
+	{0x5483, 0x28},
+	{0x5484, 0x51},
+	{0x5485, 0x65},
+	{0x5486, 0x71},
+	{0x5487, 0x7d},
+	{0x5488, 0x87},
+	{0x5489, 0x91},
+	{0x548a, 0x9a},
+	{0x548b, 0xaa},
+	{0x548c, 0xb8},
+	{0x548d, 0xcd},
+	{0x548e, 0xdd},
+	{0x548f, 0xea},
+	{0x5490, 0x1d},
+	{0x5580, 0x02},
+	{0x5583, 0x40},
+	{0x5584, 0x10},
+	{0x5589, 0x10},
+	{0x558a, 0x00},
+	{0x558b, 0xf8},
+	{0x5800, 0x23},
+	{0x5801, 0x14},
+	{0x5802, 0x0f},
+	{0x5803, 0x0f},
+	{0x5804, 0x12},
+	{0x5805, 0x26},
+	{0x5806, 0x0c},
+	{0x5807, 0x08},
+	{0x5808, 0x05},
+	{0x5809, 0x05},
+	{0x580a, 0x08},
+	{0x580b, 0x0d},
+	{0x580c, 0x08},
+	{0x580d, 0x03},
+	{0x580e, 0x00},
+	{0x580f, 0x00},
+	{0x5810, 0x03},
+	{0x5811, 0x09},
+	{0x5812, 0x07},
+	{0x5813, 0x03},
+	{0x5814, 0x00},
+	{0x5815, 0x01},
+	{0x5816, 0x03},
+	{0x5817, 0x08},
+	{0x5818, 0x0d},
+	{0x5819, 0x08},
+	{0x581a, 0x05},
+	{0x581b, 0x06},
+	{0x581c, 0x08},
+	{0x581d, 0x0e},
+	{0x581e, 0x29},
+	{0x581f, 0x17},
+	{0x5820, 0x11},
+	{0x5821, 0x11},
+	{0x5822, 0x15},
+	{0x5823, 0x28},
+	{0x5824, 0x46},
+	{0x5825, 0x26},
+	{0x5826, 0x08},
+	{0x5827, 0x26},
+	{0x5828, 0x64},
+	{0x5829, 0x26},
+	{0x582a, 0x24},
+	{0x582b, 0x22},
+	{0x582c, 0x24},
+	{0x582d, 0x24},
+	{0x582e, 0x06},
+	{0x582f, 0x22},
+	{0x5830, 0x40},
+	{0x5831, 0x42},
+	{0x5832, 0x24},
+	{0x5833, 0x26},
+	{0x5834, 0x24},
+	{0x5835, 0x22},
+	{0x5836, 0x22},
+	{0x5837, 0x26},
+	{0x5838, 0x44},
+	{0x5839, 0x24},
+	{0x583a, 0x26},
+	{0x583b, 0x28},
+	{0x583c, 0x42},
+	{0x583d, 0xce},
+	{0x5025, 0x00},
+	{0x3a0f, 0x30},
+	{0x3a10, 0x28},
+	{0x3a1b, 0x30},
+	{0x3a1e, 0x26},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x14},
+	{0x3008, 0x02},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+static struct ov5640_reg mode_1920x1080[] = {
+	/* PLL Control MIPI bit rate/lane = 672MHz, 16-bit mode.
+	 * Output size: 1936x1096 (336, 426) - (2287, 1529),
+	 * Line Length = 2500, Frame Length = 1120.
+	 */
+	{0x3103, 0x11},
+	{0x3008, 0x82},
+	{OV5640_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{0x3103, 0x03},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x3034, 0x18},
+	{0x3035, 0x11},
+	{0x3036, 0x54},
+	{0x3037, 0x13},
+	{0x3108, 0x01},
+	{0x3630, 0x36},
+	{0x3631, 0x0e},
+	{0x3632, 0xe2},
+	{0x3633, 0x12},
+	{0x3621, 0xe0},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3905, 0x02},
+	{0x3906, 0x10},
+	{0x3901, 0x0a},
+	{0x3731, 0x12},
+	{0x3600, 0x08},
+	{0x3601, 0x33},
+	{0x302d, 0x60},
+	{0x3620, 0x52},
+	{0x371b, 0x20},
+	{0x471c, 0x50},
+	{0x3a13, 0x43},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3635, 0x13},
+	{0x3636, 0x03},
+	{0x3634, 0x40},
+	{0x3622, 0x01},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c06, 0x00},
+	{0x3c07, 0x07},
+	{0x3c08, 0x00},
+	{0x3c09, 0x1c},
+	{0x3c0a, 0x9c},
+	{0x3c0b, 0x40},
+	{0x3820, 0x40},
+	{0x3821, 0x06},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3800, 0x01},
+	{0x3801, 0x50},
+	{0x3802, 0x01},
+	{0x3803, 0xaa},
+	{0x3804, 0x08},
+	{0x3805, 0xef},
+	{0x3806, 0x05},
+	{0x3807, 0xf9},
+	{0x3808, 0x07},
+	{0x3809, 0x90},
+	{0x380a, 0x04},
+	{0x380b, 0x48},
+	{0x380c, 0x09},
+	{0x380d, 0xc4},
+	{0x380e, 0x04},
+	{0x380f, 0x60},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x04},
+	{0x3618, 0x04},
+	{0x3612, 0x2b},
+	{0x3708, 0x64},
+	{0x3709, 0x12},
+	{0x370c, 0x00},
+	{0x3a02, 0x04},
+	{0x3a03, 0x60},
+	{0x3a08, 0x01},
+	{0x3a09, 0x50},
+	{0x3a0a, 0x01},
+	{0x3a0b, 0x18},
+	{0x3a0e, 0x03},
+	{0x3a0d, 0x04},
+	{0x3a14, 0x04},
+	{0x3a15, 0x60},
+	{0x4001, 0x02},
+	{0x4004, 0x06},
+	{0x3000, 0x00},
+	{0x3002, 0x1c},
+	{0x3004, 0xff},
+	{0x3006, 0xc3},
+	{0x300e, 0x45},
+	{0x302e, 0x08},
+	{0x4300, 0x32},
+	{0x501f, 0x00},
+	{0x4713, 0x02},
+	{0x4407, 0x04},
+	{0x440e, 0x00},
+	{0x460b, 0x37},
+	{0x460c, 0x20},
+	{0x4800, 0x24},
+	{0x4837, 0x0a},
+	{0x3824, 0x04},
+	{0x5000, 0xa7},
+	{0x5001, 0x83},
+	{0x5180, 0xff},
+	{0x5181, 0xf2},
+	{0x5182, 0x00},
+	{0x5183, 0x14},
+	{0x5184, 0x25},
+	{0x5185, 0x24},
+	{0x5186, 0x09},
+	{0x5187, 0x09},
+	{0x5188, 0x09},
+	{0x5189, 0x75},
+	{0x518a, 0x54},
+	{0x518b, 0xe0},
+	{0x518c, 0xb2},
+	{0x518d, 0x42},
+	{0x518e, 0x3d},
+	{0x518f, 0x56},
+	{0x5190, 0x46},
+	{0x5191, 0xf8},
+	{0x5192, 0x04},
+	{0x5193, 0x70},
+	{0x5194, 0xf0},
+	{0x5195, 0xf0},
+	{0x5196, 0x03},
+	{0x5197, 0x01},
+	{0x5198, 0x04},
+	{0x5199, 0x12},
+	{0x519a, 0x04},
+	{0x519b, 0x00},
+	{0x519c, 0x06},
+	{0x519d, 0x82},
+	{0x519e, 0x38},
+	{0x5381, 0x1e},
+	{0x5382, 0x5b},
+	{0x5383, 0x08},
+	{0x5384, 0x0a},
+	{0x5385, 0x7e},
+	{0x5386, 0x88},
+	{0x5387, 0x7c},
+	{0x5388, 0x6c},
+	{0x5389, 0x10},
+	{0x538a, 0x01},
+	{0x538b, 0x98},
+	{0x5300, 0x08},
+	{0x5301, 0x30},
+	{0x5302, 0x10},
+	{0x5303, 0x00},
+	{0x5304, 0x08},
+	{0x5305, 0x30},
+	{0x5306, 0x08},
+	{0x5307, 0x16},
+	{0x5309, 0x08},
+	{0x530a, 0x30},
+	{0x530b, 0x04},
+	{0x530c, 0x06},
+	{0x5480, 0x01},
+	{0x5481, 0x08},
+	{0x5482, 0x14},
+	{0x5483, 0x28},
+	{0x5484, 0x51},
+	{0x5485, 0x65},
+	{0x5486, 0x71},
+	{0x5487, 0x7d},
+
+	{0x5488, 0x87},
+	{0x5489, 0x91},
+	{0x548a, 0x9a},
+	{0x548b, 0xaa},
+	{0x548c, 0xb8},
+	{0x548d, 0xcd},
+	{0x548e, 0xdd},
+	{0x548f, 0xea},
+	{0x5490, 0x1d},
+	{0x5580, 0x02},
+	{0x5583, 0x40},
+	{0x5584, 0x10},
+	{0x5589, 0x10},
+	{0x558a, 0x00},
+	{0x558b, 0xf8},
+	{0x5800, 0x23},
+	{0x5801, 0x14},
+	{0x5802, 0x0f},
+	{0x5803, 0x0f},
+	{0x5804, 0x12},
+	{0x5805, 0x26},
+	{0x5806, 0x0c},
+	{0x5807, 0x08},
+	{0x5808, 0x05},
+	{0x5809, 0x05},
+	{0x580a, 0x08},
+	{0x580b, 0x0d},
+	{0x580c, 0x08},
+	{0x580d, 0x03},
+	{0x580e, 0x00},
+	{0x580f, 0x00},
+	{0x5810, 0x03},
+	{0x5811, 0x09},
+	{0x5812, 0x07},
+	{0x5813, 0x03},
+	{0x5814, 0x00},
+	{0x5815, 0x01},
+	{0x5816, 0x03},
+	{0x5817, 0x08},
+	{0x5818, 0x0d},
+	{0x5819, 0x08},
+	{0x581a, 0x05},
+	{0x581b, 0x06},
+	{0x581c, 0x08},
+	{0x581d, 0x0e},
+	{0x581e, 0x29},
+	{0x581f, 0x17},
+	{0x5820, 0x11},
+	{0x5821, 0x11},
+	{0x5822, 0x15},
+	{0x5823, 0x28},
+	{0x5824, 0x46},
+	{0x5825, 0x26},
+	{0x5826, 0x08},
+	{0x5827, 0x26},
+	{0x5828, 0x64},
+	{0x5829, 0x26},
+	{0x582a, 0x24},
+	{0x582b, 0x22},
+	{0x582c, 0x24},
+	{0x582d, 0x24},
+	{0x582e, 0x06},
+	{0x582f, 0x22},
+	{0x5830, 0x40},
+	{0x5831, 0x42},
+	{0x5832, 0x24},
+	{0x5833, 0x26},
+	{0x5834, 0x24},
+	{0x5835, 0x22},
+	{0x5836, 0x22},
+	{0x5837, 0x26},
+	{0x5838, 0x44},
+	{0x5839, 0x24},
+	{0x583a, 0x26},
+	{0x583b, 0x28},
+	{0x583c, 0x42},
+	{0x583d, 0xce},
+	{0x5025, 0x00},
+	{0x3a0f, 0x30},
+	{0x3a10, 0x28},
+	{0x3a1b, 0x30},
+	{0x3a1e, 0x26},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x14},
+	{0x3008, 0x02},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+static struct ov5640_reg mode_1296x972[] = {
+	/* PLL Control MIPI bit rate/lane = 448MHz, 16-bit mode.
+	 * Output size: 1304x972 (0, 0) - (2623, 1951),
+	 * Line Length = 1886, Frame Length = 990.
+	 */
+	{0x3103, 0x11},
+	{0x3008, 0x82},
+	{OV5640_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{0x3103, 0x03},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x3034, 0x18},
+	{0x3035, 0x21},
+	{0x3036, 0x70},
+	{0x3037, 0x13},
+	{0x3108, 0x01},
+	{0x3630, 0x36},
+	{0x3631, 0x0e},
+	{0x3632, 0xe2},
+	{0x3633, 0x12},
+	{0x3621, 0xe0},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3905, 0x02},
+	{0x3906, 0x10},
+	{0x3901, 0x0a},
+	{0x3731, 0x12},
+	{0x3600, 0x08},
+	{0x3601, 0x33},
+	{0x302d, 0x60},
+	{0x3620, 0x52},
+	{0x371b, 0x20},
+	{0x471c, 0x50},
+	{0x3a13, 0x43},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3635, 0x13},
+	{0x3636, 0x03},
+	{0x3634, 0x40},
+	{0x3622, 0x01},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c06, 0x00},
+	{0x3c07, 0x07},
+	{0x3c08, 0x00},
+	{0x3c09, 0x1c},
+	{0x3c0a, 0x9c},
+	{0x3c0b, 0x40},
+	{0x3820, 0x41},
+	{0x3821, 0x07},
+	{0x3814, 0x31},
+	{0x3815, 0x31},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0x9f},
+	{0x3808, 0x05},
+	{0x3809, 0x18},
+	{0x380a, 0x03},
+	{0x380b, 0xcc},
+	{0x380c, 0x07},
+	{0x380d, 0x5e},
+	{0x380e, 0x03},
+	{0x380f, 0xde},
+	{0x3810, 0x00},
+	{0x3811, 0x06},
+	{0x3812, 0x00},
+	{0x3813, 0x02},
+	{0x3618, 0x00},
+	{0x3612, 0x29},
+	{0x3708, 0x62},
+	{0x3709, 0x52},
+	{0x370c, 0x03},
+	{0x3a02, 0x03},
+	{0x3a03, 0xd8},
+	{0x3a08, 0x01},
+	{0x3a09, 0x27},
+	{0x3a0a, 0x00},
+	{0x3a0b, 0xf6},
+	{0x3a0e, 0x03},
+	{0x3a0d, 0x04},
+	{0x3a14, 0x03},
+	{0x3a15, 0xd8},
+	{0x4001, 0x02},
+	{0x4004, 0x02},
+	{0x3000, 0x00},
+	{0x3002, 0x1c},
+	{0x3004, 0xff},
+	{0x3006, 0xc3},
+	{0x300e, 0x45},
+	{0x302e, 0x08},
+	{0x4300, 0x32},
+	{0x501f, 0x00},
+	{0x4713, 0x02},
+	{0x4407, 0x04},
+	{0x440e, 0x00},
+	{0x460b, 0x37},
+	{0x460c, 0x20},
+	{0x4800, 0x24},
+	{0x4837, 0x10},
+	{0x3824, 0x04},
+	{0x5000, 0xa7},
+	{0x5001, 0x83},
+	{0x5180, 0xff},
+	{0x5181, 0xf2},
+	{0x5182, 0x00},
+	{0x5183, 0x14},
+	{0x5184, 0x25},
+	{0x5185, 0x24},
+	{0x5186, 0x09},
+	{0x5187, 0x09},
+	{0x5188, 0x09},
+	{0x5189, 0x75},
+	{0x518a, 0x54},
+	{0x518b, 0xe0},
+	{0x518c, 0xb2},
+	{0x518d, 0x42},
+	{0x518e, 0x3d},
+	{0x518f, 0x56},
+	{0x5190, 0x46},
+	{0x5191, 0xf8},
+	{0x5192, 0x04},
+	{0x5193, 0x70},
+	{0x5194, 0xf0},
+	{0x5195, 0xf0},
+	{0x5196, 0x03},
+	{0x5197, 0x01},
+	{0x5198, 0x04},
+	{0x5199, 0x12},
+	{0x519a, 0x04},
+	{0x519b, 0x00},
+	{0x519c, 0x06},
+	{0x519d, 0x82},
+	{0x519e, 0x38},
+	{0x5381, 0x1e},
+	{0x5382, 0x5b},
+	{0x5383, 0x08},
+	{0x5384, 0x0a},
+	{0x5385, 0x7e},
+	{0x5386, 0x88},
+	{0x5387, 0x7c},
+	{0x5388, 0x6c},
+	{0x5389, 0x10},
+	{0x538a, 0x01},
+	{0x538b, 0x98},
+	{0x5300, 0x08},
+	{0x5301, 0x30},
+	{0x5302, 0x10},
+	{0x5303, 0x00},
+	{0x5304, 0x08},
+	{0x5305, 0x30},
+	{0x5306, 0x08},
+	{0x5307, 0x16},
+	{0x5309, 0x08},
+	{0x530a, 0x30},
+	{0x530b, 0x04},
+	{0x530c, 0x06},
+	{0x5480, 0x01},
+	{0x5481, 0x08},
+	{0x5482, 0x14},
+	{0x5483, 0x28},
+	{0x5484, 0x51},
+	{0x5485, 0x65},
+	{0x5486, 0x71},
+	{0x5487, 0x7d},
+	{0x5488, 0x87},
+	{0x5489, 0x91},
+	{0x548a, 0x9a},
+	{0x548b, 0xaa},
+	{0x548c, 0xb8},
+	{0x548d, 0xcd},
+	{0x548e, 0xdd},
+	{0x548f, 0xea},
+	{0x5490, 0x1d},
+	{0x5580, 0x02},
+	{0x5583, 0x40},
+	{0x5584, 0x10},
+	{0x5589, 0x10},
+	{0x558a, 0x00},
+	{0x558b, 0xf8},
+	{0x5800, 0x23},
+	{0x5801, 0x14},
+	{0x5802, 0x0f},
+	{0x5803, 0x0f},
+	{0x5804, 0x12},
+	{0x5805, 0x26},
+	{0x5806, 0x0c},
+	{0x5807, 0x08},
+	{0x5808, 0x05},
+	{0x5809, 0x05},
+	{0x580a, 0x08},
+	{0x580b, 0x0d},
+	{0x580c, 0x08},
+	{0x580d, 0x03},
+	{0x580e, 0x00},
+	{0x580f, 0x00},
+	{0x5810, 0x03},
+	{0x5811, 0x09},
+	{0x5812, 0x07},
+	{0x5813, 0x03},
+	{0x5814, 0x00},
+	{0x5815, 0x01},
+	{0x5816, 0x03},
+	{0x5817, 0x08},
+	{0x5818, 0x0d},
+	{0x5819, 0x08},
+	{0x581a, 0x05},
+	{0x581b, 0x06},
+	{0x581c, 0x08},
+	{0x581d, 0x0e},
+	{0x581e, 0x29},
+	{0x581f, 0x17},
+	{0x5820, 0x11},
+	{0x5821, 0x11},
+	{0x5822, 0x15},
+	{0x5823, 0x28},
+	{0x5824, 0x46},
+	{0x5825, 0x26},
+	{0x5826, 0x08},
+	{0x5827, 0x26},
+	{0x5828, 0x64},
+	{0x5829, 0x26},
+	{0x582a, 0x24},
+	{0x582b, 0x22},
+	{0x582c, 0x24},
+	{0x582d, 0x24},
+	{0x582e, 0x06},
+	{0x582f, 0x22},
+	{0x5830, 0x40},
+	{0x5831, 0x42},
+	{0x5832, 0x24},
+	{0x5833, 0x26},
+	{0x5834, 0x24},
+	{0x5835, 0x22},
+	{0x5836, 0x22},
+	{0x5837, 0x26},
+	{0x5838, 0x44},
+	{0x5839, 0x24},
+	{0x583a, 0x26},
+	{0x583b, 0x28},
+	{0x583c, 0x42},
+	{0x583d, 0xce},
+	{0x5025, 0x00},
+	{0x3a0f, 0x30},
+	{0x3a10, 0x28},
+	{0x3a1b, 0x30},
+	{0x3a1e, 0x26},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x14},
+	{0x3008, 0x02},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+static struct ov5640_reg mode_640x480[] = {
+	{0x3103, 0x11},
+	{0x3008, 0x82},
+	{OV5640_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{0x3103, 0x03},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x3034, 0x18},
+	{0x3035, 0x14},
+	{0x3036, 0x70},
+	{0x3037, 0x13},
+	{0x4800, 0x24}, /* noncontinuous clock */
+	{0x3108, 0x01},
+	{0x3630, 0x36},
+	{0x3631, 0x0e},
+	{0x3632, 0xe2},
+	{0x3633, 0x12},
+	{0x3621, 0xe0},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3905, 0x02},
+	{0x3906, 0x10},
+	{0x3901, 0x0a},
+	{0x3731, 0x12},
+	{0x3600, 0x08},
+	{0x3601, 0x33},
+	{0x302d, 0x60},
+	{0x3620, 0x52},
+	{0x371b, 0x20},
+	{0x471c, 0x50},
+	{0x3a13, 0x43},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3635, 0x13},
+	{0x3636, 0x03},
+	{0x3634, 0x40},
+	{0x3622, 0x01},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c06, 0x00},
+	{0x3c07, 0x08},
+	{0x3c08, 0x00},
+	{0x3c09, 0x1c},
+	{0x3c0a, 0x9c},
+	{0x3c0b, 0x40},
+	{0x3820, 0x41},
+	{0x3821, 0x07},
+	{0x3814, 0x31},
+	{0x3815, 0x31},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x04},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0x9b},
+	{0x3808, 0x02},
+	{0x3809, 0x80},
+	{0x380a, 0x01},
+	{0x380b, 0xe0},
+	{0x380c, 0x07},
+	{0x380d, 0x68},
+	{0x380e, 0x03},
+	{0x380f, 0xd8},
+	{0x3810, 0x00},
+	{0x3811, 0x10},
+	{0x3812, 0x00},
+	{0x3813, 0x06},
+	{0x3618, 0x00},
+	{0x3612, 0x29},
+	{0x3708, 0x64},
+	{0x3709, 0x52},
+	{0x370c, 0x03},
+
+	/* AEC/AGC Power Down Domain Control */
+	{0x3a02, 0x03},
+	{0x3a03, 0xd8},
+	{0x3a08, 0x01},
+	{0x3a09, 0x27},
+	{0x3a0a, 0x00},
+	{0x3a0b, 0xf6},
+	{0x3a0e, 0x03},
+	{0x3a0d, 0x04},
+	{0x3a14, 0x03},
+	{0x3a15, 0xd8},
+
+	{0x4001, 0x02},
+	{0x4004, 0x02},
+	{0x3000, 0x00},
+	{0x3002, 0x1c},
+	{0x3004, 0xff},
+	{0x3006, 0xc3},
+	{0x300e, 0x45},
+	{0x302e, 0x08},
+	/* org:30 bit[3:0]
+	   0x0:YUYV 0x1:YVYU 0x2:UYVY
+	   0x3:VYUY 0xF:UYVY 0x4~0xE:Not-allowed
+	 */
+	{0x4300, 0x32},
+	{0x501f, 0x00},
+	{0x4713, 0x03},
+	{0x4407, 0x04},
+	{0x440e, 0x00},
+	{0x460b, 0x35},
+	{0x460c, 0x22},
+	{0x4837, 0x44},
+	{0x3824, 0x02},
+	{0x5000, 0xa7},
+	{0x5001, 0xa3},
+
+	/* AWB Control */
+	{0x5180, 0xff},
+	{0x5181, 0xf2},
+	{0x5182, 0x00},
+	{0x5183, 0x14},
+	{0x5184, 0x25},
+	{0x5185, 0x24},
+	{0x5186, 0x09},
+	{0x5187, 0x09},
+	{0x5188, 0x09},
+	{0x5189, 0x75},
+	{0x518a, 0x54},
+	{0x518b, 0xe0},
+	{0x518c, 0xb2},
+	{0x518d, 0x42},
+	{0x518e, 0x3d},
+	{0x518d, 0x56},
+	{0x5190, 0x46},
+	{0x5191, 0xf8},
+	{0x5192, 0x04},
+	{0x5193, 0x70},
+	{0x5194, 0xf0},
+	{0x5195, 0xf0},
+	{0x5196, 0x03},
+	{0x5197, 0x01},
+	{0x5198, 0x04},
+	{0x5199, 0x12},
+	{0x519a, 0x04},
+	{0x519b, 0x00},
+	{0x519c, 0x06},
+	{0x519d, 0x82},
+	{0x519e, 0x38},
+
+	/* CMX Control */
+	{0x5381, 0x1e},
+	{0x5382, 0x5b},
+	{0x5383, 0x08},
+	{0x5384, 0x0a},
+	{0x5385, 0x7e},
+	{0x5386, 0x88},
+	{0x5387, 0x7c},
+	{0x5388, 0x6c},
+	{0x5389, 0x10},
+	{0x538a, 0x01},
+	{0x538b, 0x98},
+
+	/* CIP Control */
+	{0x5300, 0x08},
+	{0x5301, 0x30},
+	{0x5302, 0x10},
+	{0x5303, 0x00},
+	{0x5304, 0x08},
+	{0x5305, 0x30},
+	{0x5306, 0x08},
+	{0x5307, 0x16},
+	{0x5309, 0x08},
+	{0x530a, 0x30},
+	{0x530b, 0x04},
+	{0x530c, 0x06},
+
+	/* Gamma Control */
+	{0x5480, 0x01},
+	{0x5481, 0x08},
+	{0x5482, 0x14},
+	{0x5483, 0x28},
+	{0x5484, 0x51},
+	{0x5485, 0x65},
+	{0x5486, 0x71},
+	{0x5487, 0x7d},
+	{0x5488, 0x87},
+	{0x5489, 0x91},
+	{0x548a, 0x9a},
+	{0x548b, 0xaa},
+	{0x548c, 0xb8},
+	{0x548d, 0xcd},
+	{0x548e, 0xdd},
+	{0x548f, 0xea},
+	{0x5490, 0x1d},
+
+	/* SDE Control */
+	{0x5580, 0x02},
+	{0x5583, 0x40},
+	{0x5584, 0x10},
+	{0x5589, 0x10},
+	{0x558a, 0x00},
+	{0x558b, 0xf8},
+
+	/* LENC Control */
+	{0x5800, 0x23},
+	{0x5801, 0x14},
+	{0x5802, 0x0f},
+	{0x5803, 0x0f},
+	{0x5804, 0x12},
+	{0x5805, 0x26},
+	{0x5806, 0x0c},
+	{0x5807, 0x08},
+	{0x5808, 0x05},
+	{0x5809, 0x05},
+	{0x580a, 0x08},
+	{0x580b, 0x0d},
+	{0x580c, 0x08},
+	{0x580d, 0x03},
+	{0x580e, 0x00},
+	{0x580f, 0x00},
+	{0x5810, 0x03},
+	{0x5811, 0x09},
+	{0x5812, 0x07},
+	{0x5813, 0x03},
+	{0x5814, 0x00},
+	{0x5815, 0x01},
+	{0x5816, 0x03},
+	{0x5817, 0x08},
+	{0x5818, 0x0d},
+	{0x5819, 0x08},
+	{0x581a, 0x05},
+	{0x581b, 0x06},
+	{0x581c, 0x08},
+	{0x581d, 0x0e},
+	{0x581e, 0x29},
+	{0x581f, 0x17},
+	{0x5820, 0x11},
+	{0x5821, 0x11},
+	{0x5822, 0x15},
+	{0x5823, 0x28},
+	{0x5824, 0x46},
+	{0x5825, 0x26},
+	{0x5826, 0x08},
+	{0x5827, 0x26},
+	{0x5828, 0x64},
+	{0x5829, 0x26},
+	{0x582a, 0x24},
+	{0x582b, 0x22},
+	{0x582c, 0x24},
+	{0x582d, 0x24},
+	{0x582e, 0x06},
+	{0x582f, 0x22},
+	{0x5830, 0x40},
+	{0x5831, 0x42},
+	{0x5832, 0x24},
+	{0x5833, 0x26},
+	{0x5834, 0x24},
+	{0x5835, 0x22},
+	{0x5836, 0x22},
+	{0x5837, 0x26},
+	{0x5838, 0x44},
+	{0x5839, 0x24},
+	{0x583a, 0x26},
+	{0x583b, 0x28},
+	{0x583c, 0x42},
+	{0x583d, 0xce},
+
+	{0x5025, 0x00},
+	{0x3a0f, 0x30},
+	{0x3a10, 0x28},
+	{0x3a1b, 0x30},
+	{0x3a1e, 0x26},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x14},
+	{0x3008, 0x02},
+	{OV5640_TABLE_END, 0x0000},
+};
+
+enum {
+	OV5640_MODE_640x480,
+	OV5640_MODE_1296x972,
+	OV5640_MODE_1920x1080,
+	OV5640_MODE_2592x1944,
+	OV5640_SIZE_LAST,
+};
+
+static struct ov5640_reg *mode_table[] = {
+	[OV5640_MODE_640x480] = mode_640x480,
+	[OV5640_MODE_1296x972] = mode_1296x972,
+	[OV5640_MODE_1920x1080] = mode_1920x1080,
+	[OV5640_MODE_2592x1944] = mode_2592x1944,
+};
+
+static int test_pattern;
+module_param(test_pattern, int, 0644);
+
+static struct ov5640_reg tp_cbars[] = {
+	{0x503D, 0x80},
+	{0x503E, 0x00},
+	{0x5046, 0x01},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+#define to_ov5640(sd)		container_of(sd, struct ov5640_priv, subdev)
+
+#define SIZEOF_I2C_TRANSBUF 32
+
+struct ov5640_priv {
+	struct v4l2_subdev		subdev;
+	struct v4l2_mbus_framefmt	mf;
+
+	int				ident;
+	u16				chip_id;
+	u8				revision;
+
+	int mode;
+
+	struct i2c_client *client;
+	u8 i2c_trans_buf[SIZEOF_I2C_TRANSBUF];
+};
+
+static enum v4l2_mbus_pixelcode ov5640_codes[] = {
+	V4L2_MBUS_FMT_UYVY8_2X8,
+};
+
+static const struct v4l2_frmsize_discrete ov5640_frmsizes[OV5640_SIZE_LAST] = {
+	{640, 480},
+	{1296, 972},
+	{1920, 1080},
+	{2592, 1944},
+};
+
+static int ov5640_find_mode(u32 width, u32 height)
+{
+	int i;
+
+	for (i = 0; i < OV5640_SIZE_LAST; i++) {
+		if ((ov5640_frmsizes[i].width >= width) &&
+		    (ov5640_frmsizes[i].height >= height))
+			break;
+	}
+
+	/* If not found, select biggest */
+	if (i >= OV5640_SIZE_LAST)
+		i = OV5640_SIZE_LAST - 1;
+
+	return i;
+}
+
+static int ov5640_read_reg(struct i2c_client *client, u16 addr, u8 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[3];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 2;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (addr >> 8);
+	data[1] = (u8) (addr & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = data + 2;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+
+	if (err != 2)
+		return -EINVAL;
+
+	*val = data[2];
+
+	return 0;
+}
+
+static int ov5640_write_reg(struct i2c_client *client, u16 addr, u8 value)
+{
+	int count;
+	struct i2c_msg msg[1];
+	unsigned char data[4];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = addr;
+	data[1] = (u8) (addr & 0xff);
+	data[2] = value;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 3;
+	msg[0].buf = data;
+
+	count = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg));
+	if (count == ARRAY_SIZE(msg))
+		return 0;
+	dev_err(&client->dev,
+		"ov5840: i2c transfer failed, addr: %x, value: %02x\n",
+	       addr, (u32)value);
+	return -EIO;
+}
+
+static int ov5640_write_bulk_reg(struct i2c_client *client, u8 *data, int len)
+{
+	int err;
+	struct i2c_msg msg;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = len;
+	msg.buf = data;
+
+	err = i2c_transfer(client->adapter, &msg, 1);
+	if (err == 1)
+		return 0;
+
+	dev_err(&client->dev, "ov5640: i2c transfer failed at %x\n",
+		(int)data[0] << 8 | data[1]);
+
+	return err;
+}
+
+static int ov5640_write_table(struct ov5640_priv *priv,
+			      struct ov5640_reg table[])
+{
+	int err;
+	struct ov5640_reg *next, *n_next;
+	u8 *b_ptr = priv->i2c_trans_buf;
+	unsigned int buf_filled = 0;
+	u16 val;
+
+	for (next = table; next->addr != OV5640_TABLE_END; next++) {
+		if (next->addr == OV5640_TABLE_WAIT_MS) {
+			msleep(next->val);
+			continue;
+		}
+
+		val = next->val;
+
+		if (!buf_filled) {
+			b_ptr = priv->i2c_trans_buf;
+			*b_ptr++ = next->addr >> 8;
+			*b_ptr++ = next->addr & 0xff;
+			buf_filled = 2;
+		}
+		*b_ptr++ = val;
+		buf_filled++;
+
+		n_next = next + 1;
+		if (n_next->addr != OV5640_TABLE_END &&
+			n_next->addr != OV5640_TABLE_WAIT_MS &&
+			buf_filled < SIZEOF_I2C_TRANSBUF &&
+			n_next->addr == next->addr + 1) {
+			continue;
+		}
+
+		err = ov5640_write_bulk_reg(priv->client,
+			priv->i2c_trans_buf, buf_filled);
+		if (err)
+			return err;
+
+		buf_filled = 0;
+	}
+	return 0;
+}
+
+static void ov5640_set_default_fmt(struct ov5640_priv *priv)
+{
+	struct v4l2_mbus_framefmt *mf = &priv->mf;
+
+	mf->width = ov5640_frmsizes[OV5640_MODE_2592x1944].width;
+	mf->height = ov5640_frmsizes[OV5640_MODE_2592x1944].height;
+	mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
+	mf->field = V4L2_FIELD_NONE;
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+/* Start/Stop streaming from the device */
+static int ov5640_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov5640_priv *priv = to_ov5640(sd);
+	int ret = 0;
+
+	if (!enable) {
+		ov5640_set_default_fmt(priv);
+		return 0;
+	}
+
+	ret = ov5640_write_table(priv, mode_table[priv->mode]);
+	if (ret)
+		return ret;
+
+	if (test_pattern == 1) {
+		ret = ov5640_write_table(priv, tp_cbars);
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
+static int ov5640_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	int mode;
+
+	mode = ov5640_find_mode(mf->width, mf->height);
+	mf->width = ov5640_frmsizes[mode].width;
+	mf->height = ov5640_frmsizes[mode].height;
+
+	mf->field = V4L2_FIELD_NONE;
+	mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+
+	return 0;
+}
+
+/* set the format we will capture in */
+static int ov5640_s_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct ov5640_priv *priv = to_ov5640(sd);
+	int ret;
+
+	ret = ov5640_try_fmt(sd, mf);
+	if (ret < 0)
+		return ret;
+
+	priv->mode = ov5640_find_mode(mf->width, mf->height);
+
+	memcpy(&priv->mf, mf, sizeof(struct v4l2_mbus_framefmt));
+
+	return 0;
+}
+
+static int ov5640_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(ov5640_codes))
+		return -EINVAL;
+
+	*code = ov5640_codes[index];
+
+	return 0;
+}
+
+static int ov5640_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left		= 0;
+	a->bounds.top		= 0;
+	a->bounds.width		= ov5640_frmsizes[OV5640_MODE_2592x1944].width;
+	a->bounds.height	= ov5640_frmsizes[OV5640_MODE_2592x1944].height;
+	a->defrect		= a->bounds;
+	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int ov5640_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	a->c.left		= 0;
+	a->c.top		= 0;
+	a->c.width		= ov5640_frmsizes[OV5640_MODE_2592x1944].width;
+	a->c.height		= ov5640_frmsizes[OV5640_MODE_2592x1944].height;
+	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+/* Get chip identification */
+static int ov5640_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct ov5640_priv *priv = to_ov5640(sd);
+
+	id->ident = priv->ident;
+	id->revision = priv->revision;
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov5640_get_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u8 val;
+
+	if (reg->reg & ~0xffff)
+		return -EINVAL;
+
+	reg->size = 2;
+
+	ret = ov5640_read_reg(client, reg->reg, &val);
+	if (ret)
+		return ret;
+
+	reg->val = (__u64)val;
+
+	return ret;
+}
+
+static int ov5640_set_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg & ~0xffff || reg->val & ~0xff)
+		return -EINVAL;
+
+	return ov5640_write_reg(client, reg->reg, reg->val);
+}
+#endif
+
+static struct v4l2_subdev_video_ops ov5640_video_ops = {
+	.s_stream		= ov5640_s_stream,
+	.s_mbus_fmt		= ov5640_s_fmt,
+	.try_mbus_fmt		= ov5640_try_fmt,
+	.enum_mbus_fmt		= ov5640_enum_fmt,
+	.cropcap		= ov5640_cropcap,
+	.g_crop			= ov5640_g_crop,
+};
+
+static struct v4l2_subdev_core_ops ov5640_core_ops = {
+	.g_chip_ident		= ov5640_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register		= ov5640_get_register,
+	.s_register		= ov5640_set_register,
+#endif
+};
+
+static struct v4l2_subdev_ops ov5640_subdev_ops = {
+	.core			= &ov5640_core_ops,
+	.video			= &ov5640_video_ops,
+};
+
+/*
+ * i2c_driver function
+ */
+static int ov5640_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+{
+	struct ov5640_priv *priv;
+	struct soc_camera_device *icd	= client->dev.platform_data;
+	struct soc_camera_link *icl;
+	u8 chip_id_hi, chip_id_lo;
+	int ret;
+
+	if (!icd) {
+		dev_err(&client->dev, "Missing soc-camera data!\n");
+		return -EINVAL;
+	}
+
+	icl = to_soc_camera_link(icd);
+	if (!icl) {
+		dev_err(&client->dev, "Missing platform_data for driver\n");
+		return -EINVAL;
+	}
+
+	priv = devm_kzalloc(&client->dev, sizeof(struct ov5640_priv),
+				GFP_KERNEL);
+	if (!priv) {
+		dev_err(&client->dev, "Failed to allocate private data!\n");
+		return -ENOMEM;
+	}
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov5640_subdev_ops);
+
+	priv->client = client;
+
+	priv->ident = V4L2_IDENT_OV5640;
+
+	/*
+	 * check and show product ID and manufacturer ID
+	 */
+	ret = ov5640_read_reg(client, 0x300A, &chip_id_hi);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failure to read Chip ID (high byte)\n");
+		return ret;
+	}
+
+	ret = ov5640_read_reg(client, 0x300B, &chip_id_lo);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failure to read Chip ID (low byte)\n");
+		return ret;
+	}
+
+	priv->chip_id = (chip_id_hi << 8) | chip_id_lo;
+
+	if (priv->chip_id != 0x5640) {
+		dev_err(&client->dev, "Chip ID: %x not supported!\n",
+			priv->chip_id);
+		ret = -ENODEV;
+		return ret;
+	}
+
+	ov5640_set_default_fmt(priv);
+
+	dev_info(&client->dev, "Chip ID 0x%04x\n", priv->chip_id);
+	return ret;
+}
+
+static int ov5640_remove(struct i2c_client *client)
+{
+	return 0;
+}
+
+static const struct i2c_device_id ov5640_id[] = {
+	{ "ov5640", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov5640_id);
+
+static struct i2c_driver ov5640_i2c_driver = {
+	.driver = {
+		.name = "ov5640",
+	},
+	.probe    = ov5640_probe,
+	.remove   = ov5640_remove,
+	.id_table = ov5640_id,
+};
+
+static int __init ov5640_module_init(void)
+{
+	return i2c_add_driver(&ov5640_i2c_driver);
+}
+
+static void __exit ov5640_module_exit(void)
+{
+	i2c_del_driver(&ov5640_i2c_driver);
+}
+
+module_init(ov5640_module_init);
+module_exit(ov5640_module_exit);
+
+MODULE_DESCRIPTION("SoC Camera driver for OmniVision OV5640");
+MODULE_AUTHOR("Andrew Chew <achew@nvidia.com>");
+MODULE_LICENSE("GPL v2");