path: root/meta-moblin/packages/linux/linux-moblin-2.6.33.2/linux-2.6.35-moorestown-camera-driver-10.0-3-3.patch

From 0d55b08388f12c7c22cae9c6c745995d051624ba Mon Sep 17 00:00:00 2001
From: Zheng Ba <zheng.ba@intel.com>
Date: Thu, 1 Apr 2010 16:29:43 +0800
Subject: [PATCH 3/3] Moorestown Camera Imaging driver Beta 10.0

Patch-mainline: 2.6.35?

Changes from Beta 9.0:
1. Fixed hsd sighting:
	3469638 3469639 3469710 3469822 (high)
	3469697 (medium)

Changes from Beta 8.0:
1. Fixed hsd sighting
	3469056 3469058 (critical)
	3469705 3469696 3469709 3469510 (medium)

Changes from Beta 7.0:
1. Fixed hsd sighting 3469681,3469682,3469683 (high)

Changes from Beta 6.0:
1. Fixed hsd sighting 3469668 (high)
2. Fixed ov5630 v4l2 view-finding dark issue
3. Enabled support for popular v4l2 applications (cheese, skype, ffmpeg)

Changes from Beta 5.1:
1. Fixed CRITICAL sighting 3469558 -- ciapp fails to launch with segment fault
2. Fixed HIGH sighting 3479513 -- ov5630 AWB unstable
3. Improved KMOT sensor 720p fps from 30 to 40

Changes from Beta 5.0:
Fixed a critical issue of camera driver not loading -- hsd 3469557

Main changes from Beta 4.0:
Fixed 4 HSD sightings: 3469392,3469099,3469470,3469500

Main changes from Beta 3.0:
Fixed 7 HSD sightings: 3469264,3469112,3469395,3469103,3469105,3469471,3469484

Main changes from Beta 2.0:
Fixed 6 HSD sightings: 3469047,3469315,3469317,3469101,3468409,3469391

Main changes from Beta 1.1:
1. Added interrupt mode for jpeg capture and KMOT viewfinding
2. Fixed HSD sighting 3469228 and 3469147

Main changes from Alpha2:
Enabled MIPI interface in ISP driver and KMOT sensor s5k4e1.
Enabled FIFO in ISP driver, which doubled the fps in view-finding mode.
Enabled Subdev Framework in CI kernel driver.
Enabled AF Continuous Mode.
Enabled AE scene evaluation.

Enabled the camera drivers in kernel:
Device Drivers --> Multimedia support --> Video For Linux
Device Drivers --> Mulitmedia support --> Video capture adapters -->
--> Moorestown Langwell Camera Imaging Subsystem support.

Kernel configs:
1. camera driver depends on GPIO library and I2C driver.
CONFIG_GENERIC_GPIO=y
CONFIG_I2C=y
CONFIG_GPIOLIB=y
2. camera driver depends on videobuf-core and videobuf-dma-contig.
VIDEOBUF_GEN=y
VIDEOBUF_DMA_CONTIG=y
3. enable multimedia support and video capture.
CONFIG_MEDIA_SUPPORT=y
CONFIG_VIDEO_DEV=y
CONFIG_VIDEO_V4L2_COMMON=y
CONFIG_VIDEO_MEDIA=y
CONFIG_VIDEO_V4L2=y
4. camera drivers incluing ISP, 5630, 5630-motor, s5k4e1, s5k4e1-motor, 2650,
9665, flash.
CONFIG_VIDEO_MRSTCI=y
CONFIG_VIDEO_MRST_ISP=y
CONFIG_VIDEO_MRST_OV5630=y
CONFIG_VIDEO_MRST_OV5630_MOTOR=y
CONFIG_VIDEO_MRST_S5K4E1=y
CONFIG_VIDEO_MRST_S5K4E1_MOTOR=y
CONFIG_VIDEO_MRST_FLASH=y
CONFIG_VIDEO_MRST_OV2650=y
CONFIG_VIDEO_MRST_OV9665=y
Signed-off-by: Zheng Ba <zheng.ba@intel.com>
---
 drivers/media/video/mrstci/mrstflash/Kconfig       |    9 +
 drivers/media/video/mrstci/mrstflash/Makefile      |    3 +
 drivers/media/video/mrstci/mrstflash/mrstflash.c   |  150 +++
 drivers/media/video/mrstci/mrstov2650/Kconfig      |    9 +
 drivers/media/video/mrstci/mrstov2650/Makefile     |    3 +
 drivers/media/video/mrstci/mrstov2650/mrstov2650.c | 1190 ++++++++++++++++++++
 drivers/media/video/mrstci/mrstov2650/ov2650.h     |  766 +++++++++++++
 drivers/media/video/mrstci/mrstov5630/Kconfig      |    9 +
 drivers/media/video/mrstci/mrstov5630/Makefile     |    4 +
 drivers/media/video/mrstci/mrstov5630/ov5630.c     | 1153 +++++++++++++++++++
 drivers/media/video/mrstci/mrstov5630/ov5630.h     |  672 +++++++++++
 .../media/video/mrstci/mrstov5630_motor/Kconfig    |    9 +
 .../media/video/mrstci/mrstov5630_motor/Makefile   |    3 +
 .../mrstci/mrstov5630_motor/mrstov5630_motor.c     |  428 +++++++
 .../video/mrstci/mrstov5630_motor/ov5630_motor.h   |   86 ++
 drivers/media/video/mrstci/mrstov9665/Kconfig      |    9 +
 drivers/media/video/mrstci/mrstov9665/Makefile     |    3 +
 drivers/media/video/mrstci/mrstov9665/mrstov9665.c |  972 ++++++++++++++++
 drivers/media/video/mrstci/mrstov9665/ov9665.h     |  263 +++++
 drivers/media/video/mrstci/mrsts5k4e1/Kconfig      |    9 +
 drivers/media/video/mrstci/mrsts5k4e1/Makefile     |    3 +
 drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.c | 1024 +++++++++++++++++
 drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.h |  662 +++++++++++
 .../media/video/mrstci/mrsts5k4e1_motor/Kconfig    |    9 +
 .../media/video/mrstci/mrsts5k4e1_motor/Makefile   |    3 +
 .../mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.c     |  430 +++++++
 .../mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.h     |  102 ++
 27 files changed, 7983 insertions(+), 0 deletions(-)
 create mode 100644 drivers/media/video/mrstci/mrstflash/Kconfig
 create mode 100644 drivers/media/video/mrstci/mrstflash/Makefile
 create mode 100644 drivers/media/video/mrstci/mrstflash/mrstflash.c
 create mode 100644 drivers/media/video/mrstci/mrstov2650/Kconfig
 create mode 100644 drivers/media/video/mrstci/mrstov2650/Makefile
 create mode 100644 drivers/media/video/mrstci/mrstov2650/mrstov2650.c
 create mode 100644 drivers/media/video/mrstci/mrstov2650/ov2650.h
 create mode 100644 drivers/media/video/mrstci/mrstov5630/Kconfig
 create mode 100644 drivers/media/video/mrstci/mrstov5630/Makefile
 create mode 100644 drivers/media/video/mrstci/mrstov5630/ov5630.c
 create mode 100644 drivers/media/video/mrstci/mrstov5630/ov5630.h
 create mode 100644 drivers/media/video/mrstci/mrstov5630_motor/Kconfig
 create mode 100644 drivers/media/video/mrstci/mrstov5630_motor/Makefile
 create mode 100644 drivers/media/video/mrstci/mrstov5630_motor/mrstov5630_motor.c
 create mode 100644 drivers/media/video/mrstci/mrstov5630_motor/ov5630_motor.h
 create mode 100644 drivers/media/video/mrstci/mrstov9665/Kconfig
 create mode 100644 drivers/media/video/mrstci/mrstov9665/Makefile
 create mode 100644 drivers/media/video/mrstci/mrstov9665/mrstov9665.c
 create mode 100644 drivers/media/video/mrstci/mrstov9665/ov9665.h
 create mode 100755 drivers/media/video/mrstci/mrsts5k4e1/Kconfig
 create mode 100644 drivers/media/video/mrstci/mrsts5k4e1/Makefile
 create mode 100755 drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.c
 create mode 100755 drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.h
 create mode 100755 drivers/media/video/mrstci/mrsts5k4e1_motor/Kconfig
 create mode 100644 drivers/media/video/mrstci/mrsts5k4e1_motor/Makefile
 create mode 100644 drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.c
 create mode 100644 drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.h

diff --git a/drivers/media/video/mrstci/mrstflash/Kconfig b/drivers/media/video/mrstci/mrstflash/Kconfig
new file mode 100644
index 0000000..72099c5
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstflash/Kconfig
@@ -0,0 +1,9 @@
+config VIDEO_MRST_FLASH
+	tristate "Moorestown flash"
+	depends on I2C && VIDEO_MRST_ISP
+
+	---help---
+	  Say Y here if your platform support moorestown flash.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mrstov2650.ko.
diff --git a/drivers/media/video/mrstci/mrstflash/Makefile b/drivers/media/video/mrstci/mrstflash/Makefile
new file mode 100644
index 0000000..068f638
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstflash/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VIDEO_MRST_FLASH)	 += mrstflash.o
+
+EXTRA_CFLAGS	+=	-I$(src)/../include
diff --git a/drivers/media/video/mrstci/mrstflash/mrstflash.c b/drivers/media/video/mrstci/mrstflash/mrstflash.c
new file mode 100644
index 0000000..5611e6b
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstflash/mrstflash.c
@@ -0,0 +1,150 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging camera flash.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-i2c-drv.h>
+
+static int debug;
+module_param(debug, bool, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
+MODULE_DESCRIPTION("A low-level driver for mrst flash");
+MODULE_LICENSE("GPL");
+
+static int flash_g_chip_ident(struct v4l2_subdev *sd,
+		struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+#define V4L2_IDENT_MRST_FLASH 8248
+	return v4l2_chip_ident_i2c_client(client, chip,
+					  V4L2_IDENT_MRST_FLASH, 0);
+}
+
+static const struct v4l2_subdev_core_ops flash_core_ops = {
+	.g_chip_ident = flash_g_chip_ident,
+};
+static const struct v4l2_subdev_ops flash_ops = {
+	.core = &flash_core_ops,
+};
+
+static int flash_detect(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	u8 pid;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	if (adapter->nr != 0)
+		return -ENODEV;
+
+	pid = i2c_smbus_read_byte_data(client, 0x10);
+	if (pid == 0x18) {
+		printk(KERN_ERR "camera flash device found\n");
+		v4l_dbg(1, debug, client, "found camera flash device");
+	} else {
+		printk(KERN_ERR "no camera flash device found\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int flash_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	u8 pid, ver;
+	int ret = -1;
+	struct v4l2_subdev *sd;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	sd = kzalloc(sizeof(struct v4l2_subdev), GFP_KERNEL);
+	ret = flash_detect(client);
+	if (ret)
+		return -ENODEV;
+
+	v4l2_i2c_subdev_init(sd, client, &flash_ops);
+
+	ver = i2c_smbus_read_byte_data(client, 0x50);
+	v4l_dbg(1, debug, client, "detect:CST from device is 0x%x", ver);
+	pid = i2c_smbus_read_byte_data(client, 0x20);
+	v4l_dbg(1, debug, client, "detect:MFPC from device is 0x%x", pid);
+	pid = i2c_smbus_read_byte_data(client, 0xA0);
+	v4l_dbg(1, debug, client, "detect:TCC from device is 0x%x", pid);
+	pid = i2c_smbus_read_byte_data(client, 0xB0);
+	v4l_dbg(1, debug, client, "detect:FCC from device is 0x%x", pid);
+	pid = i2c_smbus_read_byte_data(client, 0xC0);
+	v4l_dbg(1, debug, client, "detect:FDC from device is 0x%x", pid);
+	i2c_smbus_write_byte_data(client, 0xc0, 0xff); /*set FST to 1000us*/
+	pid = i2c_smbus_read_byte_data(client, 0xc0);
+	v4l_dbg(1, debug, client, "FDC from device is 0x%x", pid);
+
+	v4l_dbg(1, debug, client,
+		"successfully load camera flash device driver");
+	return 0;
+}
+
+static int flash_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+
+	return 0;
+}
+
+static const struct i2c_device_id flash_id[] = {
+	{"mrst_camera_flash", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, flash_id);
+
+static struct v4l2_i2c_driver_data v4l2_i2c_data = {
+	.name = "mrst_camera_flash",
+	.probe = flash_probe,
+	.remove = flash_remove,
+	.id_table = flash_id,
+};
diff --git a/drivers/media/video/mrstci/mrstov2650/Kconfig b/drivers/media/video/mrstci/mrstov2650/Kconfig
new file mode 100644
index 0000000..d39d894
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov2650/Kconfig
@@ -0,0 +1,9 @@
+config VIDEO_MRST_OV2650
+	tristate "Moorestown OV2650 SoC Sensor"
+	depends on I2C && VIDEO_MRST_ISP
+
+	---help---
+	  Say Y here if your platform support OV2650 SoC Sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mrstov2650.ko.
diff --git a/drivers/media/video/mrstci/mrstov2650/Makefile b/drivers/media/video/mrstci/mrstov2650/Makefile
new file mode 100644
index 0000000..fb16d57
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov2650/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VIDEO_MRST_OV2650)	 += mrstov2650.o
+
+EXTRA_CFLAGS	+=	-I$(src)/../include
\ No newline at end of file
diff --git a/drivers/media/video/mrstci/mrstov2650/mrstov2650.c b/drivers/media/video/mrstci/mrstov2650/mrstov2650.c
new file mode 100644
index 0000000..7f0d478
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov2650/mrstov2650.c
@@ -0,0 +1,1190 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-i2c-drv.h>
+
+#include "ci_sensor_common.h"
+#include "ov2650.h"
+
+static int mrstov2650_debug;
+module_param(mrstov2650_debug, int, 0644);
+MODULE_PARM_DESC(mrstov2650_debug, "Debug level (0-1)");
+
+#define dprintk(level, fmt, arg...) do {			\
+	if (mrstov2650_debug >= level) 					\
+		printk(KERN_DEBUG "mrstisp@%s: " fmt "\n", \
+		       __func__, ## arg); } \
+	while (0)
+
+#define eprintk(fmt, arg...)	\
+	printk(KERN_ERR "mrstisp@%s: line %d: " fmt "\n",	\
+	       __func__, __LINE__, ## arg);
+
+#define DBG_entering	dprintk(2, "entering");
+#define DBG_leaving	dprintk(2, "leaving");
+#define DBG_line	dprintk(2, " line: %d", __LINE__);
+
+static inline struct ci_sensor_config *to_sensor_config(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ci_sensor_config, sd);
+}
+
+static struct ov2650_format_struct {
+	__u8 *desc;
+	__u32 pixelformat;
+	struct regval_list *regs;
+} ov2650_formats[] = {
+	{
+		.desc		= "YUYV 4:2:2",
+		.pixelformat	= SENSOR_MODE_BT601,
+		.regs 		= NULL,
+	},
+};
+#define N_OV2650_FMTS ARRAY_SIZE(ov2650_formats)
+
+static struct ov2650_res_struct {
+	__u8 *desc;
+	int res;
+	int width;
+	int height;
+	/* FIXME: correct the fps values.. */
+	int fps;
+	bool used;
+	struct regval_list *regs;
+} ov2650_res[] = {
+	{
+		.desc		= "UXGA",
+		.res		= SENSOR_RES_UXGA,
+		.width		= 1600,
+		.height		= 1200,
+		.fps		= 15,
+		.used		= 0,
+		.regs		= ov2650_res_uxga,
+	},
+	{
+		.desc		= "SXGA",
+		.res		= SENSOR_RES_SXGA,
+		.width		= 1280,
+		.height		= 1024,
+		.fps		= 15,
+		.used		= 0,
+		.regs		= ov2650_res_sxga,
+	},
+	{
+		.desc 		= "SVGA",
+		.res		= SENSOR_RES_SVGA,
+		.width		= 800,
+		.height		= 600,
+		.fps		= 15,
+		.used		= 0,
+		.regs		= ov2650_res_svga,
+	},
+	{
+		.desc		= "VGA",
+		.res		= SENSOR_RES_VGA,
+		.width		= 640,
+		.height		= 480,
+		.fps		= 15,
+		.used		= 0,
+		.regs		= ov2650_res_vga_vario,
+	},
+	{
+		.desc 		= "QVGA",
+		.res		= SENSOR_RES_QVGA,
+		.width		= 320,
+		.height		= 240,
+		.fps		= 15,
+		.used		= 0,
+		.regs		= ov2650_res_qvga,
+	},
+};
+
+#define N_RES (ARRAY_SIZE(ov2650_res))
+
+/*
+ * I2C Read & Write stuff
+ */
+static int ov2650_read(struct i2c_client *c, u16 reg, u8 *value)
+{
+	int ret;
+	int i;
+	struct i2c_msg msg[2];
+	u8 msgbuf[2];
+	u8 ret_val = 0;
+	*value = 0;
+	/* Read needs two message to go */
+	memset(&msg, 0, sizeof(msg));
+	msgbuf[0] = 0;
+	msgbuf[1] = 0;
+	i = 0;
+	msgbuf[i++] = reg >> 8;
+	msgbuf[i++] = reg;
+	msg[0].addr = c->addr;
+	msg[0].buf = msgbuf;
+	msg[0].len = i;
+
+	msg[1].addr = c->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = &ret_val;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(c->adapter, &msg[0], 2);
+	*value = ret_val;
+
+	ret = (ret == 2) ? 0 : -1;
+	return ret;
+}
+
+static int ov2650_write(struct i2c_client *c, u16 reg, u8 value)
+{
+	int ret, i;
+	struct i2c_msg msg;
+	u8 msgbuf[3];
+
+	/* Writing only needs one message */
+	memset(&msg, 0, sizeof(msg));
+	i = 0;
+	msgbuf[i++] = reg >> 8;
+	msgbuf[i++] = reg;
+	msgbuf[i++] = value;
+
+	msg.addr = c->addr;
+	msg.flags = 0;
+	msg.buf = msgbuf;
+	msg.len = i;
+
+	ret = i2c_transfer(c->adapter, &msg, 1);
+
+	/* If this is a reset register, wait for 1ms */
+	if (reg == OV2650_SYS && (value & 0x80))
+		msleep(3);
+
+	ret = (ret == 1) ? 0 : -1;
+	return ret;
+}
+
+static int ov2650_write_array(struct i2c_client *c, struct regval_list *vals)
+{
+	struct regval_list *p;
+	u8 read_val = 0;
+	int err_num = 0;
+	int i = 0;
+	p = vals;
+	while (p->reg_num != 0xffff) {
+		ov2650_write(c, p->reg_num, p->value);
+		ov2650_read(c, p->reg_num, &read_val);
+		if (read_val != p->value)
+			err_num++;
+		p++;
+		i++;
+	}
+	return 0;
+}
+
+static int ov2650_set_data_pin_in(struct i2c_client *client)
+{
+	int ret = 0;
+	u8 reg;
+
+	ret += ov2650_write(client, 0x30b0, 0x00);
+
+	ret += ov2650_read(client, 0x30b1, &reg);
+	reg &= 0xfc;
+	ret += ov2650_write(client, 0x30b1, reg);
+
+	return ret;
+}
+
+static int ov2650_set_data_pin_out(struct i2c_client *client)
+{
+	int ret = 0;
+	u8 reg;
+
+	ret += ov2650_write(client, 0x30b0, 0xff);
+
+	ret += ov2650_read(client, 0x30b1, &reg);
+	reg &= 0xfc;
+	reg |= 0x03;
+	ret += ov2650_write(client, 0x30b1, reg);
+
+	return ret;
+}
+/*
+ * Sensor specific helper function
+ */
+static int ov2650_standby(void)
+{
+	gpio_set_value(GPIO_STDBY_PIN, 1);
+	dprintk(1, "PM: standby called\n");
+	return 0;
+}
+
+static int ov2650_wakeup(void)
+{
+	gpio_set_value(GPIO_STDBY_PIN, 0);
+	dprintk(1, "PM: wakeup called\n");
+	return 0;
+}
+
+static int ov2650_s_power(struct v4l2_subdev *sd, u32 val)
+{
+	if (val == 1)
+		ov2650_standby();
+	if (val == 0)
+		ov2650_wakeup();
+	return 0;
+}
+
+static int ov2650_init(struct i2c_client *c)
+{
+	int ret;
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct ci_sensor_config *info = to_sensor_config(sd);
+
+	/* Fill the configuration structure */
+	/* Note this default configuration value */
+	info->mode = ov2650_formats[0].pixelformat;
+	info->res = ov2650_res[0].res;
+	info->type = SENSOR_TYPE_SOC;
+	info->bls = SENSOR_BLS_OFF;
+	info->gamma = SENSOR_GAMMA_ON;
+	info->cconv = SENSOR_CCONV_ON;
+	info->blc = SENSOR_BLC_AUTO;
+	info->agc = SENSOR_AGC_AUTO;
+	info->awb = SENSOR_AWB_AUTO;
+	info->aec = SENSOR_AEC_AUTO;
+	info->bus_width = SENSOR_BUSWIDTH_8BIT_ZZ;
+	info->ycseq = SENSOR_YCSEQ_YCBYCR;
+	info->conv422 = SENSOR_CONV422_COSITED;
+	info->bpat = SENSOR_BPAT_BGBGGRGR;/* GRGRBGBG; */
+	info->field_inv = SENSOR_FIELDINV_NOSWAP;
+	info->field_sel = SENSOR_FIELDSEL_BOTH;
+	info->hpol = SENSOR_HPOL_REFPOS;
+	info->vpol = SENSOR_VPOL_POS;
+	info->edge = SENSOR_EDGE_RISING;
+	info->flicker_freq = SENSOR_FLICKER_100;
+	info->cie_profile = 0;
+	memcpy(info->name, "ov2650", 7);
+
+	ret = ov2650_write(c, OV2650_SYS, 0x80);
+	/* Set registers into default config value */
+	ret += ov2650_write_array(c, ov2650_def_reg);
+
+	/* added by wen to stop sensor from streaming */
+	ov2650_write(c, 0x3086, 0x0f);
+	ov2650_set_data_pin_in(c);
+	ssleep(1);
+
+	return ret;
+}
+
+static int distance(struct ov2650_res_struct *res, u32 w, u32 h)
+{
+	int ret;
+	if (res->width < w || res->height < h)
+		return -1;
+
+	ret = ((res->width - w) + (res->height - h));
+	return ret;
+}
+
+static int ov2650_try_res(u32 *w, u32 *h)
+{
+	struct ov2650_res_struct *res_index, *p = NULL;
+	int dis, last_dis = ov2650_res->width + ov2650_res->height;
+
+	dprintk(1, "&&&&&  before %dx%d", *w, *h);
+	for (res_index = ov2650_res;
+	     res_index < ov2650_res + N_RES;
+	     res_index++) {
+		if ((res_index->width <= *w) && (res_index->height <= *h))
+			break;
+		dis = distance(res_index, *w, *h);
+		if (dis < last_dis) {
+			last_dis = dis;
+			p = res_index;
+		}
+	}
+	if ((res_index->width < *w) || (res_index->height < *h)) {
+		if (res_index != ov2650_res)
+			res_index--;
+	}
+
+	/*
+	if (p == NULL) {
+		p = ov2650_res;
+	}
+
+	if ((w != NULL) && (h != NULL)) {
+		*w = p->width;
+		*h = p->height;
+	}
+	*/
+	if (res_index == ov2650_res + N_RES)
+		res_index = ov2650_res + N_RES - 1;
+
+	*w = res_index->width;
+	*h = res_index->height;
+
+	dprintk(1, "&&&&&  after %dx%d", *w, *h);
+	return 0;
+}
+
+static struct ov2650_res_struct *ov2650_to_res(u32 w, u32 h)
+{
+	struct ov2650_res_struct *res_index;
+
+	for (res_index = ov2650_res;
+	     res_index < ov2650_res + N_RES;
+	     res_index++)
+		if ((res_index->width == w) && (res_index->height == h))
+			break;
+
+	if (res_index >= ov2650_res + N_RES)
+		res_index--;   /* Take the bigger one */
+
+	return res_index;
+}
+
+static int ov2650_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_format *fmt)
+{
+	DBG_entering;
+	dprintk(1, "&&&&&  before %dx%d", fmt->fmt.pix.width,
+		fmt->fmt.pix.height);
+	return ov2650_try_res(&fmt->fmt.pix.width, &fmt->fmt.pix.height);
+	dprintk(1, "&&&&&  after %dx%d", fmt->fmt.pix.width,
+		fmt->fmt.pix.height);
+	DBG_leaving;
+}
+
+static int ov2650_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_format *fmt)
+{
+	struct ci_sensor_config *info = to_sensor_config(sd);
+	unsigned short width, height;
+	int index;
+
+	ci_sensor_res2size(info->res, &width, &height);
+
+	/* Marked the current sensor res as being "used" */
+	for (index = 0; index < N_RES; index++) {
+		if ((width == ov2650_res[index].width) &&
+		    (height == ov2650_res[index].height)) {
+			ov2650_res[index].used = 1;
+			continue;
+		}
+		ov2650_res[index].used = 0;
+	}
+
+	fmt->fmt.pix.width = width;
+	fmt->fmt.pix.height = height;
+	return 0;
+}
+
+static int ov2650_set_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ci_sensor_config *info = to_sensor_config(sd);
+	int ret = 0;
+	struct ov2650_res_struct *res_index;
+	u32 width, height;
+	int index;
+
+	DBG_entering;
+
+	width = fmt->fmt.pix.width;
+	height = fmt->fmt.pix.height;
+
+	ret = ov2650_try_res(&width, &height);
+	res_index = ov2650_to_res(width, height);
+
+	ov2650_wakeup();
+
+	/* if ((info->res != res_index->res) && (res_index->regs)) { */
+	if (res_index->regs) {
+
+		dprintk(2, "changing res from to %dx%d", width, height);
+		ret = ov2650_write(client, OV2650_SYS, 0x80);
+		ret += ov2650_write_array(client, ov2650_def_reg);
+		ret += ov2650_write_array(client, res_index->regs);
+
+		/* add to debug
+		if(res_index->res == SENSOR_RES_VGA) {
+			ret += ov2650_write_array(c, ov2650_def_reg);
+			ret += ov2650_write_array(c, res_index->regs);
+		} else {
+			ret += ov2650_write_array(c, ov2650_res_vga_reverse);
+			ret += ov2650_write_array(c, res_index->regs);
+		}
+		*/
+
+		/* Add delay here to get better image */
+		/*
+		if (res_index->res == SENSOR_RES_SXGA ||
+		   res_index->res == SENSOR_RES_UXGA)
+			msleep(2000);
+		else
+			msleep(900);
+			*/
+
+		/* Marked current sensor res as being "used" */
+		for (index = 0; index < N_RES; index++) {
+			if ((width == ov2650_res[index].width) &&
+			    (height == ov2650_res[index].height)) {
+				ov2650_res[index].used = 1;
+				continue;
+			}
+			ov2650_res[index].used = 0;
+		}
+
+		for (index = 0; index < N_RES; index++)
+			dprintk(2, "index = %d, used = %d\n", index,
+				ov2650_res[index].used);
+
+	}
+
+	info->res = res_index->res;
+
+	/*
+	int i;
+	unsigned char value;
+	printk(KERN_WARNING "2650 reg dumping start:\n");
+	for(i = 0x3000; i <= 0x360B; i ++) {
+		ov2650_read(c, i, &value);
+		printk(KERN_WARNING "reg at offset %4x = %x\n", i, value);
+	}
+	printk(KERN_WARNING "2650 reg dumping finished:\n");
+	*/
+
+	DBG_leaving;
+
+	return ret;
+}
+
+static int ov2650_q_hflip(struct v4l2_subdev *sd, __s32 *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int err;
+	unsigned char v;
+
+	err = ov2650_read(client, OV2650_TMC_6, &v);
+	*value = (v & 0x02) == 0x02;
+	return err;
+}
+
+static int ov2650_t_hflip(struct v4l2_subdev *sd, int value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ci_sensor_config *info = to_sensor_config(sd);
+	unsigned char v, v1 = 0;
+	int err;
+
+	value = value >= 1 ? 1 : 0;
+	err = ov2650_read(client, OV2650_TMC_6, &v);
+	if (value) {
+		v |= 0x02;
+		v1 |= 0x08;
+		info->bpat = SENSOR_BPAT_GRGRBGBG;/*BGBGGRGR;*/
+	} else {
+		v &= ~0x02;
+		v1 &= ~0x08;
+		info->bpat = SENSOR_BPAT_BGBGGRGR;
+	}
+	err += ov2650_write(client, OV2650_TMC_6, v);
+	err += ov2650_write(client, 0x3090, v1);
+	msleep(10);  /* FIXME */
+
+	return err;
+}
+
+static int ov2650_q_vflip(struct v4l2_subdev *sd, __s32 *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int err;
+	unsigned char v;
+
+	err = ov2650_read(client, OV2650_TMC_6, &v);
+	*value = (v & 0x01) == 0x01;
+	return err;
+}
+
+
+static int ov2650_t_vflip(struct v4l2_subdev *sd, int value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int err = 0;
+	unsigned char v;
+
+	value = value >= 1 ? 1 : 0;
+	err = ov2650_read(client, OV2650_TMC_6, &v);
+	if (value)
+		v |= 0x01;
+	else
+		v &= ~0x01;
+	err += ov2650_write(client, OV2650_TMC_6, v);
+	msleep(10);  /* FIXME */
+
+	return err;
+}
+
+#if 0
+static int ov2650_t_awb(struct i2c_client *c, int value)
+{
+	unsigned char v;
+	int ret;
+	struct ci_sensor_config *info = i2c_get_clientdata(c);
+
+	value = value >= 1 ? 1 : 0;
+	ret = ov2650_read(c, OV2650_ISP_CTL_0, &v);
+	if (value & 0x01) {
+		v |= 0x30;
+		info->awb = SENSOR_AWB_AUTO;
+	} else {
+		v &= ~0x30;
+		info->awb = SENSOR_AWB_OFF;
+	}
+	ret += ov2650_write(c, OV2650_ISP_CTL_0, v);
+	msleep(10);  /* FIXME */
+
+	return ret;
+}
+
+static int ov2650_q_awb(struct i2c_client *c, int *value)
+{
+	int ret;
+	unsigned char v;
+
+	ret = ov2650_read(c, OV2650_ISP_CTL_0, &v);
+	*value = (v & 0x30) == 0x30;
+	return ret;
+}
+
+static int ov2650_t_agc(struct i2c_client *c, int value)
+{
+	unsigned char v;
+	int ret;
+	struct ci_sensor_config *info = i2c_get_clientdata(c);
+
+	value = value >= 1 ? 1 : 0;
+	ret = ov2650_read(c, OV2650_ISP_CTL_0, &v);
+	if (value & 0x01) {
+		v |= 0x10;
+		info->agc = SENSOR_AGC_AUTO;
+	} else {
+		v &= ~0x10;
+		info->agc = SENSOR_AGC_OFF;
+	}
+	ret += ov2650_write(c, OV2650_ISP_CTL_0, v);
+	msleep(10);  /* FIXME */
+
+	return ret;
+}
+
+static int ov2650_q_agc(struct i2c_client *c, int *value)
+{
+	int ret;
+	unsigned char v;
+
+	ret = ov2650_read(c, OV2650_ISP_CTL_0, &v);
+	*value = (v & 0x10) == 0x10;
+	return ret;
+}
+
+static int ov2650_t_blc(struct i2c_client *c, int value)
+{
+	unsigned char v;
+	int ret;
+
+	value = value >= 1 ? 1 : 0;
+
+	ret = ov2650_read(c, OV2650_BLCC, &v);
+	if (value & 0x01)
+		v |= 0x10;
+	else
+		v &= ~0x10;
+	ret += ov2650_write(c, OV2650_BLCC, v);
+	msleep(10);  /* FIXME */
+
+	return ret;
+}
+
+static int ov2650_q_blc(struct i2c_client *c, int *value)
+{
+	int ret;
+	unsigned char v;
+
+	ret = ov2650_read(c, OV2650_BLCC, &v);
+	*value = (v & 0x10) == 0x10;
+	return ret;
+}
+#endif
+
+static struct ov2650_control {
+	struct v4l2_queryctrl qc;
+	int (*query)(struct v4l2_subdev *sd, __s32 *value);
+	int (*tweak)(struct v4l2_subdev *sd, int value);
+} ov2650_controls[] = {
+	{
+		.qc = {
+			.id = V4L2_CID_VFLIP,
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.name = "Vertical flip",
+			.minimum = 0,
+			.maximum = 1,
+			.step = 1,
+			.default_value = 0,
+		},
+		.tweak = ov2650_t_vflip,
+		.query = ov2650_q_vflip,
+	},
+	{
+		.qc = {
+			.id = V4L2_CID_HFLIP,
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.name = "Horizontal mirror",
+			.minimum = 0,
+			.maximum = 1,
+			.step = 1,
+			.default_value = 0,
+		},
+		.tweak = ov2650_t_hflip,
+		.query = ov2650_q_hflip,
+	},
+#if 0
+	{
+		.parm = {
+			.index = V4L2_CID_AUTO_WHITE_BALANCE,
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.name = "Auto White Balance",
+			.min = 0,
+			.max = 1,
+			.step = 1,
+			.def_value = 1,
+		},
+		.tweak = ov2650_t_awb,
+		.query = ov2650_q_awb,
+	},
+	{
+		.parm = {
+			.index = V4L2_CID_AUTOGAIN,
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.name = "Auto Gain Control",
+			.min = 0,
+			.max = 1,
+			.step = 1,
+			.def_value = 1,
+		},
+		.tweak = ov2650_t_agc,
+		.query = ov2650_q_agc,
+
+	},
+	{
+		.parm = {
+			.index = V4L2_CID_BLACK_LEVEL,
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.name = "Black Level Control",
+			.min = 0,
+			.max = 1,
+			.step = 1,
+			.def_value = 1,
+		},
+		.tweak = ov2650_t_blc,
+		.query = ov2650_q_blc,
+
+	},
+#endif
+};
+#define N_CONTROLS (ARRAY_SIZE(ov2650_controls))
+
+static struct ov2650_control *ov2650_find_control(__u32 id)
+{
+	int i;
+
+	for (i = 0; i < N_CONTROLS; i++)
+		if (ov2650_controls[i].qc.id == id)
+			return ov2650_controls + i;
+	return NULL;
+}
+
+static int ov2650_queryctrl(struct v4l2_subdev *sd,
+			    struct v4l2_queryctrl *qc)
+{
+	struct ov2650_control *octrl;
+	octrl = ov2650_find_control(qc->id);
+	if (NULL == octrl)
+		return -EINVAL;
+	*qc = octrl->qc;
+	return 0;
+}
+
+static int ov2650_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct ov2650_control *octrl = ov2650_find_control(ctrl->id);
+	int ret;
+
+	if (octrl == NULL)
+		return -EINVAL;
+	ret = octrl->query(sd, &ctrl->value);
+	if (ret >= 0)
+		return 0;
+	return ret;
+}
+
+static int ov2650_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct ov2650_control *octrl = ov2650_find_control(ctrl->id);
+	int ret;
+
+	if (octrl == NULL)
+		return -EINVAL;
+	ret =  octrl->tweak(sd, ctrl->value);
+	if (ret >= 0)
+		return 0;
+	return ret;
+}
+#if 0
+static int ov2650_get_caps(struct i2c_client *c, struct ci_sensor_caps *caps)
+{
+	if (caps == NULL)
+		return -EIO;
+
+	caps->bus_width = SENSOR_BUSWIDTH_8BIT_ZZ;
+	caps->mode	= SENSOR_MODE_BT601;
+	caps->field_inv = SENSOR_FIELDINV_NOSWAP;
+	caps->field_sel = SENSOR_FIELDSEL_BOTH;
+	caps->ycseq	= SENSOR_YCSEQ_YCBYCR;
+	caps->conv422	= SENSOR_CONV422_COSITED;
+	caps->bpat	= SENSOR_BPAT_BGBGGRGR;
+	caps->hpol	= SENSOR_HPOL_REFPOS;
+	caps->vpol	= SENSOR_VPOL_POS;
+	caps->edge	= SENSOR_EDGE_RISING;
+	caps->bls	= SENSOR_BLS_OFF;
+	caps->gamma	= SENSOR_GAMMA_ON;
+	caps->cconv	= SENSOR_CCONV_ON;
+	caps->res	= SENSOR_RES_UXGA | SENSOR_RES_SXGA | SENSOR_RES_SVGA
+	    | SENSOR_RES_VGA | SENSOR_RES_QVGA;
+	caps->blc	= SENSOR_BLC_AUTO;
+	caps->agc	= SENSOR_AGC_AUTO;
+	caps->awb	= SENSOR_AWB_AUTO;
+	caps->aec	= SENSOR_AEC_AUTO;
+	caps->cie_profile	= 0;
+	caps->flicker_freq	= SENSOR_FLICKER_100 | SENSOR_FLICKER_120;
+	caps->type	= SENSOR_TYPE_SOC;
+	/* caps->name	= "ov2650"; */
+	strcpy(caps->name, "ov2650");
+
+	return 0;
+}
+
+static int ov2650_get_config(struct i2c_client *c,
+			     struct ci_sensor_config *config)
+{
+	struct ci_sensor_config *info = i2c_get_clientdata(c);
+
+	if (config == NULL) {
+		printk(KERN_WARNING "sensor_get_config: NULL pointer\n");
+		return -EIO;
+	}
+
+	memcpy(config, info, sizeof(struct ci_sensor_config));
+
+	return 0;
+}
+
+static int ov2650_setup(struct i2c_client *c,
+			const struct ci_sensor_config *config)
+{
+	int ret;
+	struct ov2650_res_struct *res_index;
+	struct ci_sensor_config *info = i2c_get_clientdata(c);
+	u16 width, high;
+
+	/* Soft reset camera first*/
+	ret = ov2650_write(c, OV2650_SYS, 0x80);
+
+	/* Set registers into default config value */
+	ret += ov2650_write_array(c, ov2650_def_reg);
+
+	/* set image resolution */
+	ci_sensor_res2size(config->res, &width, &high);
+	ret += ov2650_try_res(c, &width, &high);
+	res_index = ov2650_find_res(width, high);
+	if (res_index->regs)
+		ret += ov2650_write_array(c, res_index->regs);
+	if (!ret)
+		info->res = res_index->res;
+
+
+	if (config->blc != info->blc) {
+		ret += ov2650_t_blc(c, config->blc);
+		info->blc = config->blc;
+	}
+
+	if (config->agc != info->agc) {
+		ret += ov2650_t_agc(c, config->agc);
+		info->agc = config->agc;
+	}
+
+	if (config->awb != info->awb) {
+		ret += ov2650_t_awb(c, config->awb);
+		info->awb = config->awb;
+	}
+	/* Add some delay here to get a better image*/
+	if (res_index->res == SENSOR_RES_SXGA ||
+	    res_index->res == SENSOR_RES_UXGA)
+		msleep(2000);
+	else
+		msleep(900);
+
+	return ret;
+}
+
+/*
+ * File operation functions
+ */
+
+
+
+static int ov2650_open(struct i2c_setting *c, void *priv)
+{
+	struct i2c_client *client = c->sensor_client;
+	/* Just wake up sensor */
+	if (ov2650_wakeup())
+		return -EIO;
+	ov2650_init(client);
+	/*Sleep sensor now*/
+	ov2650_write(client, 0x3086, 0x0f);
+
+	/* set data pin to input */
+	if (ov2650_set_data_pin_in(client))
+		return -EIO;
+
+	return 0;
+}
+
+static int ov2650_release(struct i2c_setting *c, void *priv)
+{
+	/* Just suspend the sensor */
+	ov2650_standby();
+	return 0;
+}
+
+static int ov2650_on(struct i2c_setting *c)
+{
+	int ret;
+
+	/* Software wake up sensor */
+	ret = ov2650_write(c->sensor_client, 0x3086, 0x00);
+
+	/* set data pin to output */
+	return ret + ov2650_set_data_pin_out(c->sensor_client);
+}
+
+static int ov2650_off(struct i2c_setting *c)
+{
+	int ret;
+
+	/* Software standby sensor */
+	ret = ov2650_write(c->sensor_client, 0x3086, 0x0f);
+
+	/* set data pin to input */
+	return ret + ov2650_set_data_pin_in(c->sensor_client);
+}
+
+static struct sensor_device ov2650 = {
+	.name	= "OV2650",
+	.type	= SENSOR_TYPE_SOC,
+	.minor	= -1,
+	.open	= ov2650_open,
+	.release = ov2650_release,
+	.on = ov2650_on,
+	.off = ov2650_off,
+	.querycap = ov2650_get_caps,
+	.get_config = ov2650_get_config,
+	.set_config = ov2650_setup,
+	.enum_parm = ov2650_queryctrl,
+	.get_parm = ov2650_g_ctrl,
+	.set_parm = ov2650_s_ctrl,
+	.try_res = ov2650_try_res,
+	.set_res = ov2650_set_res,
+	.suspend = ov2650_standby,
+	.resume = ov2650_wakeup,
+	.get_ls_corr_config = NULL,
+	.set_awb = NULL,
+	.set_aec = NULL,
+	.set_blc = NULL,
+	/* TBC */
+};
+#endif
+
+static int ov2650_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	DBG_entering;
+
+
+	if (enable) {
+		ov2650_write(client, 0x3086, 0x00);
+		ov2650_set_data_pin_out(client);
+		msleep(2000);
+	} else {
+		ov2650_write(client, 0x3086, 0x0f);
+		ov2650_set_data_pin_in(client);
+	}
+
+	DBG_leaving;
+	return 0;
+}
+
+static int ov2650_enum_framesizes(struct v4l2_subdev *sd,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	unsigned int index = fsize->index;
+
+	DBG_entering;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = ov2650_res[index].width;
+	fsize->discrete.height = ov2650_res[index].height;
+	fsize->reserved[0] = ov2650_res[index].used;
+
+	DBG_leaving;
+
+	return 0;
+}
+
+static int ov2650_enum_frameintervals(struct v4l2_subdev *sd,
+				      struct v4l2_frmivalenum *fival)
+{
+	unsigned int index = fival->index;
+
+	DBG_entering;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete.numerator = 1;
+	fival->discrete.denominator = ov2650_res[index].fps;
+
+	DBG_leaving;
+
+	return 0;
+}
+static int ov2650_g_chip_ident(struct v4l2_subdev *sd,
+		struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+#define V4L2_IDENT_OV2650 8244
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_OV2650, 0);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov2650_g_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned char val = 0;
+	int ret;
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	ret = ov2650_read(client, reg->reg & 0xffff, &val);
+	reg->val = val;
+	reg->size = 1;
+	return ret;
+}
+
+static int ov2650_s_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	ov2650_write(client, reg->reg & 0xffff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static const struct v4l2_subdev_video_ops ov2650_video_ops = {
+	.try_fmt = ov2650_try_fmt,
+	.s_fmt = ov2650_set_fmt,
+	.g_fmt = ov2650_get_fmt,
+	.s_stream = ov2650_s_stream,
+	.enum_framesizes = ov2650_enum_framesizes,
+	.enum_frameintervals = ov2650_enum_frameintervals,
+};
+
+static const struct v4l2_subdev_core_ops ov2650_core_ops = {
+	.g_chip_ident = ov2650_g_chip_ident,
+	.queryctrl = ov2650_queryctrl,
+	.g_ctrl = ov2650_g_ctrl,
+	.s_ctrl = ov2650_s_ctrl,
+	.s_gpio = ov2650_s_power,
+	/*.g_ext_ctrls = ov2650_g_ext_ctrls,*/
+	/*.s_ext_ctrls = ov2650_s_ext_ctrls,*/
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = ov2650_g_register,
+	.s_register = ov2650_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_ops ov2650_ops = {
+	.core = &ov2650_core_ops,
+	.video = &ov2650_video_ops,
+};
+
+/*
+ * Basic i2c stuff
+ */
+#if 0
+static unsigned short normal_i2c[] = {I2C_OV2650 >> 1, I2C_CLIENT_END};
+I2C_CLIENT_INSMOD;
+
+static struct i2c_driver ov2650_driver;
+#endif
+static int ov2650_detect(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	int adap_id = i2c_adapter_id(adapter);
+	u8 value;
+
+	printk(KERN_WARNING "Now start ov2650 detect\n");
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -ENODEV;
+
+	if (adap_id != 1)
+		return -ENODEV;
+
+	/* if (ov2650_wakeup()) */
+		/* return -ENODEV; */
+	ov2650_wakeup();
+
+	ov2650_read(client, OV2650_PID_L, &value);
+	if (value != 0x52)
+		return -ENODEV;
+
+	return 0;
+}
+
+static int ov2650_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct ci_sensor_config *info;
+	struct v4l2_subdev *sd;
+	int ret = -1;
+
+	DBG_entering;
+
+	printk(KERN_INFO "Init ov2650 sensor \n");
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+	/*
+	 * Setup sensor configuration structure
+	 */
+	info = kzalloc(sizeof(struct ci_sensor_config), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	ret = ov2650_detect(client);
+	if (ret) {
+		kfree(info);
+		return -ENODEV;
+	}
+
+	sd = &info->sd;
+	v4l2_i2c_subdev_init(sd, client, &ov2650_ops);
+
+	/*
+	 * TODO: Need to check if this can be here.
+	 * Turn into standby mode
+	 */
+	/* ov2650_standby(); */
+	ret += ov2650_init(client);
+	ov2650_standby();
+
+	printk(KERN_INFO "Init ov2650 sensor success, ret = %d\n", ret);
+
+	DBG_leaving;
+	return 0;
+}
+
+static int ov2650_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_sensor_config(sd));
+	return 0;
+}
+
+static const struct i2c_device_id ov2650_id[] = {
+	{"ov2650", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ov2650_id);
+
+static struct v4l2_i2c_driver_data v4l2_i2c_data = {
+	.name = "ov2650",
+	.probe = ov2650_probe,
+	.remove = ov2650_remove,
+	/* .suspend = ov2650_suspend,
+	 * .resume = ov2650_resume, */
+	.id_table = ov2650_id,
+};
+
+MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
+MODULE_DESCRIPTION("A low-level driver for OmniVision 2650 sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mrstci/mrstov2650/ov2650.h b/drivers/media/video/mrstci/mrstov2650/ov2650.h
new file mode 100644
index 0000000..f5c0418
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov2650/ov2650.h
@@ -0,0 +1,766 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#define I2C_OV2650	0x60
+/* Should add to kernel source */
+#define I2C_DRIVERID_OV2650	1047
+/* GPIO pin on Moorestown */
+#define GPIO_SCLK_25	44
+#define GPIO_STB_PIN	47
+#define GPIO_STDBY_PIN	48
+#define GPIO_RESET_PIN	50
+
+/* System control register */
+#define OV2650_AGC 		0x3000
+#define OV2650_AGCS 	0x3001
+#define OV2650_AEC_H	0x3002
+#define OV2650_AEC_L  	0x3003
+#define OV2650_AECL 	0x3004
+#define OV2650_AECS_H	0x3008
+#define OV2650_AECS_L	0x3009
+#define OV2650_PID_H	0x300A
+#define OV2650_PID_L	0x300B
+#define OV2650_SCCB		0x300C
+#define OV2650_PCLK		0x300D
+#define OV2650_PLL_1	0x300E
+#define OV2650_PLL_2	0x300F
+#define OV2650_PLL_3	0x3010
+#define OV2650_CLK		0x3011
+#define OV2650_SYS		0x3012
+#define OV2650_AUTO_1	0x3013
+#define OV2650_AUTO_2	0x3014
+#define OV2650_AUTO_3	0x3015
+#define OV2650_AUTO_4	0x3016
+#define OV2650_AUTO_5	0x3017
+#define OV2650_WPT		0x3018
+#define OV2650_BPT		0x3019
+#define OV2650_VPT		0x301A
+#define OV2650_YAVG		0x301B
+#define OV2650_AECG_50	0x301C
+#define OV2650_AECG_60	0x301D
+#define OV2650_RZM_H	0x301E
+#define OV2650_RZM_L	0x301F
+#define OV2650_HS_H		0x3020
+#define OV2650_HS_L		0x3021
+#define OV2650_VS_H		0x3022
+#define OV2650_VS_L		0x3023
+#define OV2650_HW_H		0x3024
+#define OV2650_HW_L		0x3025
+#define OV2650_VH_H		0x3026
+#define OV2650_VH_L		0x3027
+#define OV2650_HTS_H	0x3028
+#define OV2650_HTS_L	0x3029
+#define OV2650_VTS_H	0x302A
+#define OV2650_VTS_L	0x302B
+#define OV2650_EXHTS	0x302C
+#define OV2650_EXVTS_H	0x302D
+#define OV2650_EXVTS_L	0x302E
+#define OV2650_WET_0	0x3030
+#define OV2650_WET_1	0x3031
+#define OV2650_WET_2	0x3032
+#define OV2650_WET_3	0x3033
+#define OV2650_AHS_H	0x3038
+#define OV2650_AHS_L	0x3039
+#define OV2650_AVS_H	0x303A
+#define OV2650_AVS_L	0x303B
+#define OV2650_AHW_H	0x303C
+#define OV2650_AHW_L	0x303D
+#define OV2650_AVH_H	0x303E
+#define OV2650_AVH_L	0x303F
+#define OV2650_HISTO_0	0x3040
+#define OV2650_HISTO_1	0x3041
+#define OV2650_HISTO_2	0x3042
+#define OV2650_HISTO_3	0x3043
+#define OV2650_HISTO_4	0x3044
+#define OV2650_BLC9A	0x3069
+#define OV2650_BLCC		0x306C
+#define OV2650_BLCD		0x306D
+#define OV2650_BLCF		0x306F
+#define OV2650_BD50_L	0x3070
+#define OV2650_BD50_H	0x3071
+#define OV2650_BD60_L	0x3072
+#define OV2650_BD60_H	0x3073
+#define OV2650_TMC_0	0x3076
+#define OV2650_TMC_1	0x3077
+#define OV2650_TMC_2	0x3078
+#define OV2650_TMC_4	0x307A
+#define OV2650_TMC_6	0x307C
+#define OV2650_TMC_8	0x307E
+#define OV2650_TMC_I2C	0x3084
+#define OV2650_TMC_10	0x3086
+#define OV2650_TMC_11	0x3087
+#define OV2650_ISP_XO_H	0x3088
+#define OV2650_ISP_XO_L	0x3089
+#define OV2650_ISP_YO_H	0x308A
+#define OV2650_ISP_YO_L	0x308B
+#define OV2650_TMC_12	0x308C
+#define OV2650_TMC_13	0x308D
+#define OV2650_EFUSE	0x308F
+#define OV2650_IO_CTL_0	0x30B0
+#define OV2650_IO_CRL_1 0x30B1
+#define OV2650_IO_CTL_2 0x30B2
+#define OV2650_LAEC		0x30F0
+#define OV2650_GRP_EOP	0x30FF
+
+/* SC registers */
+#define OV2650_SC_CTL_0	0x3100
+#define OV2650_SC_SYN_CTL_0 0x3104
+#define OV2650_SC_SYN_CTL_1 0x3105
+#define OV2650_SC_SYN_CTL_3 0x3107
+#define OV2650_SC_SYN_CTL_4 0x3108
+
+/* DSP control register */
+#define OV2650_ISP_CTL_0	0x3300
+#define OV2650_ISP_CTL_1	0x3301
+#define OV2650_ISP_CTL_2	0x3302
+#define OV2650_ISP_CTL_3	0x3303
+#define OV2650_ISP_CTL_4	0x3304
+#define OV2650_ISP_CTL_5	0x3305
+#define OV2650_ISP_CTL_6	0x3306
+#define OV2650_ISP_CTL_7	0x3307
+#define OV2650_ISP_CTL_8	0x3308
+#define OV2650_ISP_CTL_9	0x3309
+#define OV2650_ISP_CTL_A	0x330A
+#define OV2650_ISP_CTL_B	0x330B
+#define OV2650_ISP_CTL_10	0x3310
+#define OV2650_ISP_CTL_11	0x3311
+#define OV2650_ISP_CTL_12	0x3312
+#define OV2650_ISP_CTL_13	0x3313
+#define OV2650_ISP_CTL_14	0x3314
+#define OV2650_ISP_CTL_15	0x3315
+#define OV2650_ISP_CTL_16	0x3316
+#define OV2650_ISP_CTL_17	0x3317
+#define OV2650_ISP_CTL_18	0x3318
+#define OV2650_ISP_CTL_19	0x3319
+#define OV2650_ISP_CTL_1A	0x331A
+#define OV2650_ISP_CTL_1B	0x331B
+#define OV2650_ISP_CTL_1C	0x331C
+#define OV2650_ISP_CTL_1D	0x331D
+#define OV2650_ISP_CTL_1E	0x331E
+#define OV2650_ISP_CTL_20	0x3320
+#define OV2650_ISP_CTL_21	0x3321
+#define OV2650_ISP_CTL_22	0x3322
+#define OV2650_ISP_CTL_23	0x3323
+#define OV2650_ISP_CTL_24	0x3324
+#define OV2650_ISP_CTL_27	0x3327
+#define OV2650_ISP_CTL_28	0x3328
+#define OV2650_ISP_CTL_29	0x3329
+#define OV2650_ISP_CTL_2A	0x332A
+#define OV2650_ISP_CTL_2B	0x332B
+#define OV2650_ISP_CTL_2C	0x332C
+#define OV2650_ISP_CTL_2D	0x332D
+#define OV2650_ISP_CTL_2E	0x332E
+#define OV2650_ISP_CTL_2F	0x332F
+#define OV2650_ISP_CTL_30	0x3330
+#define OV2650_ISP_CTL_31	0x3331
+#define OV2650_ISP_CTL_32	0x3332
+#define OV2650_ISP_CTL_33	0x3333
+#define OV2650_ISP_CTL_34	0x3334
+#define OV2650_ISP_CTL_35	0x3335
+#define OV2650_ISP_CTL_36	0x3336
+#define OV2650_ISP_CTL_40	0x3340
+#define OV2650_ISP_CTL_41	0x3341
+#define OV2650_ISP_CTL_42	0x3342
+#define OV2650_ISP_CTL_43	0x3343
+#define OV2650_ISP_CTL_44	0x3344
+#define OV2650_ISP_CTL_45	0x3345
+#define OV2650_ISP_CTL_46	0x3346
+#define OV2650_ISP_CTL_47	0x3347
+#define OV2650_ISP_CTL_48	0x3348
+#define OV2650_ISP_CTL_49	0x3349
+#define OV2650_ISP_CTL_4A	0x334A
+#define OV2650_ISP_CTL_4B	0x334B
+#define OV2650_ISP_CTL_4C	0x334C
+#define OV2650_ISP_CTL_4D	0x334D
+#define OV2650_ISP_CTL_4E	0x334E
+#define OV2650_ISP_CTL_4F	0x334F
+#define OV2650_ISP_CTL_50	0x3350
+#define OV2650_ISP_CTL_51	0x3351
+#define OV2650_ISP_CTL_52	0x3352
+#define OV2650_ISP_CTL_53	0x3353
+#define OV2650_ISP_CTL_54	0x3354
+#define OV2650_ISP_CTL_55	0x3355
+#define OV2650_ISP_CTL_56	0x3356
+#define OV2650_ISP_CTL_57	0x3357
+#define OV2650_ISP_CTL_58	0x3358
+#define OV2650_ISP_CTL_59	0x3359
+#define OV2650_ISP_CTL_5A	0x335A
+#define OV2650_ISP_CTL_5B	0x335B
+#define OV2650_ISP_CTL_5C	0x335C
+#define OV2650_ISP_CTL_5D	0x335D
+#define OV2650_ISP_CTL_5E	0x335E
+#define OV2650_ISP_CTL_5F	0x335F
+#define OV2650_ISP_CTL_60	0x3360
+#define OV2650_ISP_CTL_61	0x3361
+#define OV2650_ISP_CTL_62	0x3362
+#define OV2650_ISP_CTL_63	0x3363
+#define OV2650_ISP_CTL_64	0x3364
+#define OV2650_ISP_CTL_65	0x3365
+#define OV2650_ISP_CTL_6A	0x336A
+#define OV2650_ISP_CTL_6B	0x336B
+#define OV2650_ISP_CTL_6C	0x336C
+#define OV2650_ISP_CTL_6E	0x336E
+#define OV2650_ISP_CTL_71	0x3371
+#define OV2650_ISP_CTL_72	0x3372
+#define OV2650_ISP_CTL_73	0x3373
+#define OV2650_ISP_CTL_74	0x3374
+#define OV2650_ISP_CTL_75	0x3375
+#define OV2650_ISP_CTL_76	0x3376
+#define OV2650_ISP_CTL_77	0x3377
+#define OV2650_ISP_CTL_78	0x3378
+#define OV2650_ISP_CTL_79	0x3379
+#define OV2650_ISP_CTL_7A	0x337A
+#define OV2650_ISP_CTL_7B	0x337B
+#define OV2650_ISP_CTL_7C	0x337C
+#define OV2650_ISP_CTL_80	0x3380
+#define OV2650_ISP_CTL_81	0x3381
+#define OV2650_ISP_CTL_82	0x3382
+#define OV2650_ISP_CTL_83	0x3383
+#define OV2650_ISP_CTL_84	0x3384
+#define OV2650_ISP_CTL_85	0x3385
+#define OV2650_ISP_CTL_86	0x3386
+#define OV2650_ISP_CTL_87	0x3387
+#define OV2650_ISP_CTL_88	0x3388
+#define OV2650_ISP_CTL_89	0x3389
+#define OV2650_ISP_CTL_8A	0x338A
+#define OV2650_ISP_CTL_8B	0x338B
+#define OV2650_ISP_CTL_8C	0x338C
+#define OV2650_ISP_CTL_8D	0x338D
+#define OV2650_ISP_CTL_8E	0x338E
+#define OV2650_ISP_CTL_90	0x3390
+#define OV2650_ISP_CTL_91	0x3391
+#define OV2650_ISP_CTL_92	0x3392
+#define OV2650_ISP_CTL_93	0x3393
+#define OV2650_ISP_CTL_94	0x3394
+#define OV2650_ISP_CTL_95	0x3395
+#define OV2650_ISP_CTL_96	0x3396
+#define OV2650_ISP_CTL_97	0x3397
+#define OV2650_ISP_CTL_98	0x3398
+#define OV2650_ISP_CTL_99	0x3399
+#define OV2650_ISP_CTL_9A	0x339A
+#define OV2650_ISP_CTL_A0	0x33A0
+#define OV2650_ISP_CTL_A1	0x33A1
+#define OV2650_ISP_CTL_A2	0x33A2
+#define OV2650_ISP_CTL_A3	0x33A3
+#define OV2650_ISP_CTL_A4	0x33A4
+#define OV2650_ISP_CTL_A5	0x33A5
+#define OV2650_ISP_CTL_A6	0x33A6
+#define OV2650_ISP_CTL_A7	0x33A7
+#define OV2650_ISP_CTL_A8	0x33A8
+#define OV2650_ISP_CTL_AA	0x33AA
+#define OV2650_ISP_CTL_AB	0x33AB
+#define OV2650_ISP_CTL_AC	0x33AC
+#define OV2650_ISP_CTL_AD	0x33AD
+#define OV2650_ISP_CTL_AE	0x33AE
+#define OV2650_ISP_CTL_AF	0x33AF
+#define OV2650_ISP_CTL_B0	0x33B0
+#define OV2650_ISP_CTL_B1	0x33B1
+#define OV2650_ISP_CTL_B2	0x33B2
+#define OV2650_ISP_CTL_B3	0x33B3
+#define OV2650_ISP_CTL_B4	0x33B4
+#define OV2650_ISP_CTL_B5	0x33B5
+#define OV2650_ISP_CTL_B6	0x33B6
+#define OV2650_ISP_CTL_B7	0x33B7
+#define OV2650_ISP_CTL_B8	0x33B8
+#define OV2650_ISP_CTL_B9	0x33B9
+
+/* Format register */
+#define OV2650_FMT_CTL_0	0x3400
+#define OV2650_FMT_CTL_1	0x3401
+#define OV2650_FMT_CTL_2	0x3402
+#define OV2650_FMT_CTL_3	0x3403
+#define OV2650_FMT_CTL_4	0x3404
+#define OV2650_FMT_CTL_5	0x3405
+#define OV2650_FMT_CTL_6	0x3406
+#define OV2650_FMT_CTL_7	0x3407
+#define OV2650_FMT_CTL_8	0x3408
+#define OV2650_DITHER_CTL	0x3409
+#define OV2650_DVP_CTL_0	0x3600
+#define OV2650_DVP_CTL_1	0x3601
+#define OV2650_DVP_CTL_6	0x3606
+#define OV2650_DVP_CTL_7	0x3607
+#define OV2650_DVP_CTL_9	0x3609
+#define OV2650_DVP_CTL_B 	0x360B
+
+/* General definition for ov2650 */
+#define OV2650_OUTWND_MAX_H		UXGA_SIZE_H
+#define OV2650_OUTWND_MAX_V		UXGA_SIZE_V
+
+struct regval_list {
+	u16 reg_num;
+	u8 value;
+};
+
+/*
+ * Default register value
+ * 1600x1200 YUV
+ */
+static struct regval_list ov2650_def_reg[] = {
+	{0x3012, 0x80},
+	{0x308c, 0x80},
+	{0x308d, 0x0e},
+	{0x360b, 0x00},
+	{0x30b0, 0xff},
+	{0x30b1, 0xff},
+	{0x30b2, 0x27},
+
+	{0x300e, 0x34},
+	{0x300f, 0xa6},
+	{0x3010, 0x81},
+	{0x3082, 0x01},
+	{0x30f4, 0x01},
+	{0x3090, 0x33},
+	{0x3091, 0xc0},
+	{0x30ac, 0x42},
+
+	{0x30d1, 0x08},
+	{0x30a8, 0x56},
+	{0x3015, 0x03},
+	{0x3093, 0x00},
+	{0x307e, 0xe5},
+	{0x3079, 0x00},
+	{0x30aa, 0x42},
+	{0x3017, 0x40},
+	{0x30f3, 0x82},
+	{0x306a, 0x0c},
+	{0x306d, 0x00},
+	{0x336a, 0x3c},
+	{0x3076, 0x6a},
+	{0x30d9, 0x8c},
+	{0x3016, 0x82},
+	{0x3601, 0x30},
+	{0x304e, 0x88},
+	{0x30f1, 0x82},
+	{0x3011, 0x02},
+
+	{0x3013, 0xf7},
+	{0x301c, 0x13},
+	{0x301d, 0x17},
+	{0x3070, 0x3e},
+	{0x3072, 0x34},
+
+	{0x30af, 0x00},
+	{0x3048, 0x1f},
+	{0x3049, 0x4e},
+	{0x304a, 0x20},
+	{0x304f, 0x20},
+	{0x304b, 0x02},
+	{0x304c, 0x00},
+	{0x304d, 0x02},
+	{0x304f, 0x20},
+	{0x30a3, 0x10},
+	{0x3013, 0xf7},
+	{0x3014, 0x44},
+	{0x3071, 0x00},
+	{0x3070, 0x3e},
+	{0x3073, 0x00},
+	{0x3072, 0x34},
+	{0x301c, 0x12},
+	{0x301d, 0x16},
+	{0x304d, 0x42},
+	{0x304a, 0x40},
+	{0x304f, 0x40},
+	{0x3095, 0x07},
+	{0x3096, 0x16},
+	{0x3097, 0x1d},
+
+	{0x3020, 0x01},
+	{0x3021, 0x18},
+	{0x3022, 0x00},
+	{0x3023, 0x0a},
+	{0x3024, 0x06},
+	{0x3025, 0x58},
+	{0x3026, 0x04},
+	{0x3027, 0xbc},
+	{0x3088, 0x06},
+	{0x3089, 0x40},
+	{0x308a, 0x04},
+	{0x308b, 0xb0},
+	{0x3316, 0x64},
+	{0x3317, 0x4b},
+	{0x3318, 0x00},
+	{0x331a, 0x64},
+	{0x331b, 0x4b},
+	{0x331c, 0x00},
+	{0x3100, 0x00},
+
+	{0x3320, 0xfa},
+	{0x3321, 0x11},
+	{0x3322, 0x92},
+	{0x3323, 0x01},
+	{0x3324, 0x97},
+	{0x3325, 0x02},
+	{0x3326, 0xff},
+	{0x3327, 0x0c},
+	{0x3328, 0x10},
+	{0x3329, 0x10},
+	{0x332a, 0x58},
+	{0x332b, 0x50},
+	{0x332c, 0xbe},
+	{0x332d, 0xe1},
+	{0x332e, 0x43},
+	{0x332f, 0x36},
+	{0x3330, 0x4d},
+	{0x3331, 0x44},
+	{0x3332, 0xf8},
+	{0x3333, 0x0a},
+	{0x3334, 0xf0},
+	{0x3335, 0xf0},
+	{0x3336, 0xf0},
+	{0x3337, 0x40},
+	{0x3338, 0x40},
+	{0x3339, 0x40},
+	{0x333a, 0x00},
+	{0x333b, 0x00},
+
+	{0x3380, 0x28},
+	{0x3381, 0x48},
+	{0x3382, 0x10},
+	{0x3383, 0x23},
+	{0x3384, 0xc0},
+	{0x3385, 0xe5},
+	{0x3386, 0xc2},
+	{0x3387, 0xb3},
+	{0x3388, 0x0e},
+	{0x3389, 0x98},
+	{0x338a, 0x01},
+
+	{0x3340, 0x0e},
+	{0x3341, 0x1a},
+	{0x3342, 0x31},
+	{0x3343, 0x45},
+	{0x3344, 0x5a},
+	{0x3345, 0x69},
+	{0x3346, 0x75},
+	{0x3347, 0x7e},
+	{0x3348, 0x88},
+	{0x3349, 0x96},
+	{0x334a, 0xa3},
+	{0x334b, 0xaf},
+	{0x334c, 0xc4},
+	{0x334d, 0xd7},
+	{0x334e, 0xe8},
+	{0x334f, 0x20},
+
+	{0x3350, 0x32},
+	{0x3351, 0x25},
+	{0x3352, 0x80},
+	{0x3353, 0x1e},
+	{0x3354, 0x00},
+	{0x3355, 0x85},
+	{0x3356, 0x32},
+	{0x3357, 0x25},
+	{0x3358, 0x80},
+	{0x3359, 0x1b},
+	{0x335a, 0x00},
+	{0x335b, 0x85},
+	{0x335c, 0x32},
+	{0x335d, 0x25},
+	{0x335e, 0x80},
+	{0x335f, 0x1b},
+	{0x3360, 0x00},
+	{0x3361, 0x85},
+	{0x3363, 0x70},
+	{0x3364, 0x7f},
+	{0x3365, 0x00},
+	{0x3366, 0x00},
+
+	{0x3301, 0xff},
+	{0x338B, 0x11},
+	{0x338c, 0x10},
+	{0x338d, 0x40},
+
+	{0x3370, 0xd0},
+	{0x3371, 0x00},
+	{0x3372, 0x00},
+	{0x3373, 0x40},
+	{0x3374, 0x10},
+	{0x3375, 0x10},
+	{0x3376, 0x04},
+	{0x3377, 0x00},
+	{0x3378, 0x04},
+	{0x3379, 0x80},
+
+	{0x3069, 0x84},
+	{0x307c, 0x10},
+	{0x3087, 0x02},
+
+	{0x3300, 0xfc},
+	{0x3302, 0x01},
+	{0x3400, 0x00},
+	{0x3606, 0x20},
+	{0x3601, 0x30},
+	{0x30f3, 0x83},
+	{0x304e, 0x88},
+
+	{0x3086, 0x0f},
+	{0x3086, 0x00},
+
+	{0xffff, 0xff},
+};
+
+/* 800x600 */
+static struct regval_list ov2650_res_svga[] = {
+
+	{0x306f, 0x14},
+	{0x302a, 0x02},
+	{0x302b, 0x84},
+	{0x3012, 0x10},
+	{0x3011, 0x01},
+
+	{0x3070, 0x5d},
+	{0x3072, 0x4d},
+
+	{0x3014, 0x84},
+	{0x301c, 0x07},
+	{0x301d, 0x09},
+	{0x3070, 0x50},
+	{0x3071, 0x00},
+	{0x3072, 0x42},
+	{0x3073, 0x00},
+
+	{0x3020, 0x01},
+	{0x3021, 0x18},
+	{0x3022, 0x00},
+	{0x3023, 0x06},
+	{0x3024, 0x06},
+	{0x3025, 0x58},
+	{0x3026, 0x02},
+	{0x3027, 0x5e},
+	{0x3088, 0x03},
+	{0x3089, 0x20},
+	{0x308a, 0x02},
+	{0x308b, 0x58},
+	{0x3316, 0x64},
+	{0x3317, 0x25},
+	{0x3318, 0x80},
+	{0x3319, 0x08},
+	{0x331a, 0x64},
+	{0x331b, 0x4b},
+	{0x331c, 0x00},
+	{0x331d, 0x38},
+	{0x3100, 0x00},
+
+	{0x3302, 0x11},
+
+	{0x3011, 0x01},
+	{0x300f, 0xa6},
+	{0x300e, 0x36},
+	{0x3010, 0x81},
+	{0x302e, 0x00},
+	{0x302d, 0x00},
+	{0x302c, 0x00},
+	{0x302b, 0x84},
+	{0x3014, 0x84},
+	{0x301c, 0x07},
+	{0x301d, 0x09},
+	{0x3070, 0x50},
+	{0x3071, 0x00},
+	{0x3072, 0x42},
+	{0x3073, 0x00},
+
+	{0x3086, 0x0f},
+	{0x3086, 0x00},
+	{0xffff, 0xff},
+};
+
+/* 640x480 */
+static struct regval_list ov2650_res_vga_vario[] = {
+	{0x306f, 0x14},
+	{0x302a, 0x02},
+	{0x302b, 0x6a},
+	{0x3012, 0x10},
+	{0x3011, 0x01},
+
+	{0x3070, 0x5d},
+	{0x3072, 0x4d},
+
+	{0x301c, 0x05},
+	{0x301d, 0x06},
+
+	{0x3020, 0x01},
+	{0x3021, 0x18},
+	{0x3022, 0x00},
+	{0x3023, 0x06},
+	{0x3024, 0x06},
+	{0x3025, 0x58},
+	{0x3026, 0x02},
+	{0x3027, 0x61},
+	{0x3088, 0x02},
+	{0x3089, 0x80},
+	{0x308a, 0x01},
+	{0x308b, 0xe0},
+	{0x3316, 0x64},
+	{0x3317, 0x25},
+	{0x3318, 0x80},
+	{0x3319, 0x08},
+	{0x331a, 0x28},
+	{0x331b, 0x1e},
+	{0x331c, 0x00},
+	{0x331d, 0x38},
+	{0x3100, 0x00},
+
+	{0x3302, 0x11},
+	{0x3011, 0x00},
+
+	{0x3014, 0x84}, /* note this */
+	{0x3086, 0x0f},
+	{0x3086, 0x00},
+	{0xffff, 0xff},
+};
+
+/* 640x480 reverse */
+/*
+static struct regval_list ov2650_res_vga_reverse[] = {
+	{0x306f, 0x10},
+	{0x302a, 0x04},
+	{0x302b, 0xd4},
+	{0x3012, 0x00},
+	{0x3011, 0x02},
+
+	{0x3070, 0x3e},
+	{0x3072, 0x34},
+
+	{0x301c, 0x12},
+	{0x301d, 0x16},
+
+	{0x3020, 0x01},
+	{0x3021, 0x18},
+	{0x3022, 0x00},
+	{0x3023, 0x0a},
+	{0x3024, 0x06},
+	{0x3025, 0x58},
+	{0x3026, 0x04},
+	{0x3027, 0xbc},
+	{0x3088, 0x06},
+	{0x3089, 0x40},
+	{0x308a, 0x04},
+	{0x308b, 0xb0},
+	{0x3316, 0x64},
+	{0x3317, 0xb4},
+	{0x3318, 0x00},
+	{0x3319, 0x6c},
+	{0x331a, 0x64},
+	{0x331b, 0x4b},
+	{0x331c, 0x00},
+	{0x331d, 0x6c},
+	{0x3100, 0x00},
+
+	{0x3302, 0x01},
+	{0x3011, 0x02},
+
+	{0x3014, 0x44},
+	{0x3086, 0x0f},
+	{0x3086, 0x00},
+	{0xffff, 0xff},
+};
+
+*/
+/* 320x240 */
+static struct regval_list ov2650_res_qvga[] = {
+	{0x306f, 0x14},
+	{0x302a, 0x02},
+	{0x302b, 0x6a},
+
+	{0x3012, 0x10},
+	{0x3011, 0x01},
+
+	{0x3070, 0x5d},
+	{0x3072, 0x4d},
+	{0x301c, 0x05},
+	{0x301d, 0x06},
+
+	{0x3023, 0x06},
+	{0x3026, 0x02},
+	{0x3027, 0x61},
+	{0x3088, 0x01},
+	{0x3089, 0x40},
+	{0x308a, 0x00},
+	{0x308b, 0xf0},
+	{0x3316, 0x64},
+	{0x3317, 0x25},
+	{0x3318, 0x80},
+	{0x3319, 0x08},
+	{0x331a, 0x14},
+	{0x331b, 0x0f},
+	{0x331c, 0x00},
+	{0x331d, 0x38},
+	{0x3100, 0x00},
+
+	{0x3015, 0x02}, /* note this */
+	{0x3014, 0x84},
+	{0x3302, 0x11},
+	{0x3086, 0x0f},
+	{0x3086, 0x00},
+	{0xffff, 0xff},
+};
+
+static struct regval_list ov2650_res_uxga[] = {
+	/* Note this added by debug */
+	{0x3014, 0x84},
+	{0x301c, 0x13},
+	{0x301d, 0x17},
+	{0x3070, 0x40},
+	{0x3071, 0x00},
+	{0x3072, 0x36},
+	{0x3073, 0x00},
+
+	{0xffff, 0xff},
+};
+
+static struct regval_list ov2650_res_sxga[] = {
+	{0x3011, 0x02},
+
+	{0x3020, 0x01},
+	{0x3021, 0x18},
+	{0x3022, 0x00},
+	{0x3023, 0x0a},
+	{0x3024, 0x06},
+	{0x3025, 0x58},
+	{0x3026, 0x04},
+	{0x3027, 0xbc},
+	{0x3088, 0x05},
+	{0x3089, 0x00},
+	{0x308a, 0x04},
+	{0x308b, 0x00},
+	{0x3316, 0x64},
+	{0x3317, 0x4b},
+	{0x3318, 0x00},
+	{0x331a, 0x50},
+	{0x331b, 0x40},
+	{0x331c, 0x00},
+
+	{0x3302, 0x11},
+
+	{0x3014, 0x84},
+	{0x301c, 0x13},
+	{0x301d, 0x17},
+	{0x3070, 0x40},
+	{0x3071, 0x00},
+	{0x3072, 0x36},
+	{0x3073, 0x00},
+
+	{0x3086, 0x0f},
+	{0x3086, 0x00},
+	{0xffff, 0xff},
+};
diff --git a/drivers/media/video/mrstci/mrstov5630/Kconfig b/drivers/media/video/mrstci/mrstov5630/Kconfig
new file mode 100644
index 0000000..a28ddc2
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov5630/Kconfig
@@ -0,0 +1,9 @@
+config VIDEO_MRST_OV5630
+	tristate "Moorestown OV5630 RAW Sensor"
+	depends on I2C && VIDEO_MRST_ISP
+
+	---help---
+	  Say Y here if your platform support OV5630 RAW Sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mrstov2650.ko.
diff --git a/drivers/media/video/mrstci/mrstov5630/Makefile b/drivers/media/video/mrstci/mrstov5630/Makefile
new file mode 100644
index 0000000..c67abff
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov5630/Makefile
@@ -0,0 +1,4 @@
+mrstov5630-objs 	= ov5630.o
+obj-$(CONFIG_VIDEO_MRST_OV5630)	 += mrstov5630.o
+
+EXTRA_CFLAGS	+=	-I$(src)/../include
diff --git a/drivers/media/video/mrstci/mrstov5630/ov5630.c b/drivers/media/video/mrstci/mrstov5630/ov5630.c
new file mode 100644
index 0000000..6498153
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov5630/ov5630.c
@@ -0,0 +1,1153 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-i2c-drv.h>
+
+#include "ci_sensor_common.h"
+#include "ov5630.h"
+
+static int mrstov5630_debug;
+module_param(mrstov5630_debug, int, 0644);
+MODULE_PARM_DESC(mrstov5630_debug, "Debug level (0-1)");
+
+#define dprintk(level, fmt, arg...) do {			\
+	if (mrstov5630_debug >= level) 					\
+		printk(KERN_DEBUG "mrstisp@%s: " fmt "\n",	\
+		       __func__, ## arg); } \
+	while (0)
+
+#define eprintk(fmt, arg...)	\
+	printk(KERN_ERR "mrstisp@%s: line %d: " fmt "\n",	\
+	       __func__, __LINE__, ## arg);
+
+#define DBG_entering	dprintk(2, "entering");
+#define DBG_leaving	dprintk(2, "leaving");
+#define DBG_line	dprintk(2, " line: %d", __LINE__);
+
+static inline struct ci_sensor_config *to_sensor_config(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ci_sensor_config, sd);
+}
+
+/* static int ov5630_set_res(struct i2c_client *c, const int w, const int h);
+ */
+static struct ov5630_format_struct {
+	__u8 *desc;
+	__u32 pixelformat;
+	struct regval_list *regs;
+} ov5630_formats[] = {
+	{
+		.desc		= "Raw RGB Bayer",
+		.pixelformat	= SENSOR_MODE_BAYER,
+		.regs 		= NULL,
+	},
+};
+#define N_OV5630_FMTS ARRAY_SIZE(ov5630_formats)
+
+static struct ov5630_res_struct {
+	__u8 *desc;
+	int res;
+	int width;
+	int height;
+	/* FIXME: correct the fps values.. */
+	int fps;
+	bool used;
+	struct regval_list *regs;
+} ov5630_res[] = {
+	{
+		.desc		= "QSXGA_PLUS4",
+		.res		= SENSOR_RES_QXGA_PLUS,
+		.width		= 2592,
+		.height		= 1944,
+		.fps		= 15,
+		.used		= 0,
+		.regs		= ov5630_res_qsxga_plus4,
+	},
+	{
+		.desc		= "1080P",
+		.res		= SENSOR_RES_1080P,
+		.width		= 1920,
+		.height		= 1080,
+		.fps		= 25,
+		.used		= 0,
+		.regs		= ov5630_res_1080p,
+	},
+	{
+		.desc 		= "XGA_PLUS",
+		.res		= SENSOR_RES_XGA_PLUS,
+		.width		= 1280,
+		.height		= 960,
+		.fps		= 30,
+		.used		= 0,
+		.regs		= ov5630_res_xga_plus,
+	},
+	{
+		.desc		= "720p",
+		.res		= SENSOR_RES_720P,
+		.width		= 1280,
+		.height		= 720,
+		.fps		= 34,
+		.used		= 0,
+		.regs		= ov5630_res_720p,
+	},
+	{
+		.desc 		= "VGA",
+		.res		= SENSOR_RES_VGA,
+		.width		= 640,
+		.height		= 480,
+		.fps		= 39,
+		.used		= 0,
+		.regs		= ov5630_res_vga_ac04_bill,
+	},
+};
+
+#define N_RES (ARRAY_SIZE(ov5630_res))
+
+/*
+ * I2C Read & Write stuff
+ */
+static int ov5630_read(struct i2c_client *c, u32 reg, u32 *value)
+{
+	int ret;
+	int i;
+	struct i2c_msg msg[2];
+	u8 msgbuf[2];
+	u8 ret_val = 0;
+	*value = 0;
+	/* Read needs two message to go */
+	memset(&msg, 0, sizeof(msg));
+	msgbuf[0] = 0;
+	msgbuf[1] = 0;
+	i = 0;
+
+	msgbuf[i++] = ((u16)reg) >> 8;
+	msgbuf[i++] = ((u16)reg) & 0xff;
+	msg[0].addr = c->addr;
+	msg[0].buf = msgbuf;
+	msg[0].len = i;
+
+	msg[1].addr = c->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = &ret_val;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(c->adapter, &msg[0], 2);
+	*value = ret_val;
+
+	ret = (ret == 2) ? 0 : -1;
+	return ret;
+}
+
+static int ov5630_write(struct i2c_client *c, u32 reg, u32 value)
+{
+	int ret, i;
+	struct i2c_msg msg;
+	u8 msgbuf[3];
+
+	/* Writing only needs one message */
+	memset(&msg, 0, sizeof(msg));
+	i = 0;
+	msgbuf[i++] = ((u16)reg) >> 8;
+	msgbuf[i++] = (u16)reg & 0xff;
+	msgbuf[i++] = (u8)value;
+
+	msg.addr = c->addr;
+	msg.flags = 0;
+	msg.buf = msgbuf;
+	msg.len = i;
+
+	ret = i2c_transfer(c->adapter, &msg, 1);
+
+	/* If this is a reset register, wait for 1ms */
+	if (reg == OV5630_SYS && (value & 0x80))
+		msleep(3);
+
+	ret = (ret == 1) ? 0 : -1;
+	return ret;
+}
+
+static int ov5630_write_array(struct i2c_client *c, struct regval_list *vals)
+{
+	struct regval_list *p;
+	u32 read_val = 0;
+	int err_num = 0;
+	int i = 0;
+	p = vals;
+	while (p->reg_num != 0xffff) {
+		ov5630_write(c, (u32)p->reg_num, (u32)p->value);
+		ov5630_read(c, (u32)p->reg_num, &read_val);
+		if (read_val != p->value)
+			err_num++;
+		p++;
+		i++;
+	}
+	return 0;
+}
+
+/*
+ * Sensor specific helper function
+ */
+static int ov5630_standby(void)
+{
+	gpio_set_value(GPIO_STDBY_PIN, 1);
+	/* ov5630_motor_standby(); */
+	dprintk(1, "PM: standby called\n");
+	return 0;
+}
+
+static int ov5630_wakeup(void)
+{
+	gpio_set_value(GPIO_STDBY_PIN, 0);
+	/* ov5630_motor_wakeup(); */
+	dprintk(1, "PM: wakeup called\n");
+	return 0;
+}
+
+static int ov5630_s_power(struct v4l2_subdev *sd, u32 val)
+{
+	if (val == 1)
+		ov5630_standby();
+	if (val == 0)
+		ov5630_wakeup();
+	return 0;
+}
+
+static int ov5630_set_img_ctrl(struct i2c_client *c,
+			       const struct ci_sensor_config *config)
+{
+	int err = 0;
+	u32 reg_val = 0;
+	/* struct ci_sensor_config *info = i2c_get_clientdata(c); */
+
+	switch (config->blc) {
+	case SENSOR_BLC_OFF:
+		err |= ov5630_read(c, OV5630_ISP_CTL00, &reg_val);
+		err |= ov5630_write(c, OV5630_ISP_CTL00, reg_val & 0xFE);
+		break;
+	case SENSOR_BLC_AUTO:
+		err |= ov5630_read(c, OV5630_ISP_CTL00, &reg_val);
+		err |= ov5630_write(c, OV5630_ISP_CTL00, reg_val | 0x01);
+		break;
+	}
+
+	switch (config->agc) {
+	case SENSOR_AGC_AUTO:
+		err |= ov5630_read(c, OV5630_AUTO_1, &reg_val);
+		err |= ov5630_write(c, OV5630_AUTO_1, reg_val | 0x04);
+		break;
+	case SENSOR_AGC_OFF:
+		err |= ov5630_read(c, OV5630_AUTO_1, &reg_val);
+		err |= ov5630_write(c, OV5630_AUTO_1, reg_val & ~0x04);
+		break;
+	}
+
+	switch (config->awb) {
+	case SENSOR_AWB_AUTO:
+		err |= ov5630_read(c, OV5630_ISP_CTL00, &reg_val);
+		err |= ov5630_write(c, OV5630_ISP_CTL00, reg_val | 0x30);
+		break;
+	case SENSOR_AWB_OFF:
+		err |= ov5630_read(c, OV5630_ISP_CTL00, &reg_val);
+		err |= ov5630_write(c, OV5630_ISP_CTL00, reg_val & ~0x30);
+		break;
+	}
+
+	switch (config->aec) {
+	case SENSOR_AEC_AUTO:
+		err |= ov5630_read(c, OV5630_AUTO_1, &reg_val);
+		err |= ov5630_write(c, OV5630_AUTO_1, reg_val | 0xFB);
+		break;
+	case SENSOR_AEC_OFF:
+		err |= ov5630_read(c, OV5630_AUTO_1, &reg_val);
+		err |= ov5630_write(c, OV5630_AUTO_1, reg_val & 0xF6);
+		break;
+	}
+
+	return err;
+}
+
+static int ov5630_init(struct i2c_client *c)
+{
+	int ret;
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct ci_sensor_config *info = to_sensor_config(sd);
+	char *name = "";
+
+	/* Fill the configuration structure */
+	/* Note this default configuration value */
+	info->mode = ov5630_formats[0].pixelformat;
+	info->res = ov5630_res[0].res;
+	info->type = SENSOR_TYPE_RAW;
+	info->bls = SENSOR_BLS_OFF;
+	info->gamma = SENSOR_GAMMA_OFF;
+	info->cconv = SENSOR_CCONV_OFF;
+	info->blc = SENSOR_BLC_AUTO;
+	info->agc = SENSOR_AGC_AUTO;
+	info->awb = SENSOR_AWB_AUTO;
+	info->aec = SENSOR_AEC_AUTO;
+	/* info->bus_width = SENSOR_BUSWIDTH_10BIT; */
+	info->bus_width = SENSOR_BUSWIDTH_10BIT_ZZ;
+	info->ycseq = SENSOR_YCSEQ_YCBYCR;
+	/* info->conv422 = SENSOR_CONV422_NOCOSITED; */
+	info->conv422 = SENSOR_CONV422_COSITED;
+	info->bpat = SENSOR_BPAT_BGBGGRGR;
+	info->field_inv = SENSOR_FIELDINV_NOSWAP;
+	info->field_sel = SENSOR_FIELDSEL_BOTH;
+	info->hpol = SENSOR_HPOL_REFPOS;
+	info->vpol = SENSOR_VPOL_NEG;
+	info->edge = SENSOR_EDGE_RISING;
+	info->flicker_freq = SENSOR_FLICKER_100;
+	info->cie_profile = SENSOR_CIEPROF_F11;
+	name = "ov5630";
+	memcpy(info->name, name, 7);
+
+	/* Reset sensor hardware, and implement the setting*/
+	ret = ov5630_write(c, (u32)OV5630_SYS, (u32)0x80);
+	ret += ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
+
+	/* Set registers into default config value */
+	ret += ov5630_write_array(c, ov5630_def_reg);
+
+	/* Set MIPI interface */
+#ifdef OV5630_MIPI
+	ret += ov5630_write_array(c, ov5630_mipi);
+#endif
+
+	/* turn off AE AEB AGC */
+	ret += ov5630_set_img_ctrl(c, info);
+
+	/* streaming */
+	/* ret += ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x01); */
+	/* ret += ov5630_write(c, (u32)0x3096, (u32)0x50); */
+	/* /ssleep(1); */
+
+	/* Added by wen to stop sensor from streaming */
+	ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
+	ov5630_write(c, 0x30b0, 0x00);
+	ov5630_write(c, 0x30b1, 0x00);
+	return ret;
+}
+
+static int distance(struct ov5630_res_struct *res, u32 w, u32 h)
+{
+	int ret;
+	if (res->width < w || res->height < h)
+		return -1;
+
+	ret = ((res->width - w) + (res->height - h));
+	return ret;
+}
+static int ov5630_try_res(u32 *w, u32 *h)
+{
+	struct ov5630_res_struct *res_index, *p = NULL;
+	int dis, last_dis = ov5630_res->width + ov5630_res->height;
+
+	DBG_entering;
+
+	for (res_index = ov5630_res;
+	     res_index < ov5630_res + N_RES;
+	     res_index++) {
+		if ((res_index->width < *w) || (res_index->height < *h))
+			break;
+		dis = distance(res_index, *w, *h);
+		if (dis < last_dis) {
+			last_dis = dis;
+			p = res_index;
+		}
+	}
+
+	if (p == NULL)
+		p = ov5630_res;
+	else if ((p->width < *w) || (p->height < *h)) {
+		if (p != ov5630_res)
+			p--;
+	}
+
+	if ((w != NULL) && (h != NULL)) {
+		*w = p->width;
+		*h = p->height;
+	}
+
+	DBG_leaving;
+	return 0;
+}
+
+static struct ov5630_res_struct *ov5630_to_res(u32 w, u32 h)
+{
+	struct ov5630_res_struct *res_index;
+
+	for (res_index = ov5630_res;
+	     res_index < ov5630_res + N_RES;
+	     res_index++)
+		if ((res_index->width == w) && (res_index->height == h))
+			break;
+
+	if (res_index >= ov5630_res + N_RES)
+		res_index--;   /* Take the bigger one */
+
+	return res_index;
+}
+
+static int ov5630_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_format *fmt)
+{
+	DBG_entering;
+	return ov5630_try_res(&fmt->fmt.pix.width, &fmt->fmt.pix.height);
+	DBG_leaving;
+}
+
+static int ov5630_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_format *fmt)
+{
+	struct ci_sensor_config *info = to_sensor_config(sd);
+	unsigned short width, height;
+	int index;
+
+	ci_sensor_res2size(info->res, &width, &height);
+
+	/* Marked the current sensor res as being "used" */
+	for (index = 0; index < N_RES; index++) {
+		if ((width == ov5630_res[index].width) &&
+		    (height == ov5630_res[index].height)) {
+			ov5630_res[index].used = 1;
+			continue;
+		}
+		ov5630_res[index].used = 0;
+	}
+
+	fmt->fmt.pix.width = width;
+	fmt->fmt.pix.height = height;
+	return 0;
+}
+
+static int ov5630_set_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	struct ci_sensor_config *info = to_sensor_config(sd);
+	int ret = 0;
+	struct ov5630_res_struct *res_index;
+	u32 width, height;
+	int index;
+
+	DBG_entering;
+
+	width = fmt->fmt.pix.width;
+	height = fmt->fmt.pix.height;
+
+	dprintk(1, "was told to set fmt (%d x %d) ", width, height);
+
+	ret = ov5630_try_res(&width, &height);
+
+	dprintk(1, "setting fmt (%d x %d) ", width, height);
+
+	res_index = ov5630_to_res(width, height);
+
+	ov5630_wakeup();
+
+	if (res_index->regs) {
+		/* Soft reset camera first*/
+		ret = ov5630_write(c, (u32)OV5630_SYS, (u32)0x80);
+
+		/* software sleep/standby */
+		ret += ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
+
+		/* Set registers into default config value */
+		ret += ov5630_write_array(c, ov5630_def_reg);
+
+		/* set image resolution */
+		ret += ov5630_write_array(c, res_index->regs);
+
+		/* turn off AE AEB AGC */
+		ret += ov5630_set_img_ctrl(c, info);
+
+		/* Set MIPI interface */
+#ifdef OV5630_MIPI
+		ret += ov5630_write_array(c, ov5630_mipi);
+#endif
+
+		if (res_index->res == SENSOR_RES_VGA)
+			ret += ov5630_write(c, (u32)0x3015, (u32)0x03);
+
+		/* streaming */
+		ret = ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x01);
+		ret = ov5630_write(c, (u32)0x3096, (u32)0x50);
+
+		info->res = res_index->res;
+
+		/* Marked current sensor res as being "used" */
+		for (index = 0; index < N_RES; index++) {
+			if ((width == ov5630_res[index].width) &&
+			    (height == ov5630_res[index].height)) {
+				ov5630_res[index].used = 1;
+				continue;
+			}
+			ov5630_res[index].used = 0;
+		}
+
+		for (index = 0; index < N_RES; index++)
+			dprintk(2, "index = %d, used = %d\n", index,
+				ov5630_res[index].used);
+	} else {
+		eprintk("no res for (%d x %d)", width, height);
+	}
+
+	DBG_leaving;
+	return ret;
+}
+
+static int ov5630_t_gain(struct v4l2_subdev *sd, int value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u32 v;
+
+	DBG_entering;
+
+	dprintk(2, "writing gain %x to 0x3000", value);
+
+	ov5630_read(client, 0x3000, &v);
+	v = (v & 0x80) + value;
+	ov5630_write(client, 0x3000, v);
+
+	dprintk(2, "gain %x was writen to 0x3000", v);
+
+	DBG_leaving;
+	return 0;
+}
+
+static int ov5630_t_exposure(struct v4l2_subdev *sd, int value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u32 v;
+	u32 reg_val;
+
+	DBG_entering;
+
+	ov5630_read(client, 0x3013, &v);
+	dprintk(2, "0x3013 = %x", v);
+	if (v & 0x05) {
+		/* turn off agc/aec */
+		v = v & 0xfa;
+		ov5630_write(client, 0x3013, v);
+		/* turn off awb */
+		ov5630_read(client, OV5630_ISP_CTL00, &reg_val);
+		ov5630_write(client, OV5630_ISP_CTL00, reg_val & ~0x30);
+	}
+	ov5630_read(client, 0x3014, &v);
+	dprintk(2, "0x3014 = %x", v);
+	ov5630_read(client, 0x3002, &v);
+	dprintk(2, "0x3002 = %x", v);
+	ov5630_read(client, 0x3003, &v);
+	dprintk(2, "0x3003 = %x", v);
+
+	dprintk(2, "writing exposure %x to 0x3002/3", value);
+
+	v = value >> 8;
+	ov5630_write(client, 0x3002, v);
+	dprintk(2, "exposure %x was writen to 0x3002", v);
+
+	v = value & 0xff;
+	ov5630_write(client, 0x3003, v);
+	dprintk(2, "exposure %x was writen to 0x3003", v);
+
+	DBG_leaving;
+	return 0;
+}
+
+static struct ov5630_control {
+	struct v4l2_queryctrl qc;
+	int (*query)(struct v4l2_subdev *sd, __s32 *value);
+	int (*tweak)(struct v4l2_subdev *sd, int value);
+} ov5630_controls[] = {
+	{
+		.qc = {
+			.id = V4L2_CID_GAIN,
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.name = "global gain",
+			.minimum = 0x0,
+			.maximum = 0xFF,
+			.step = 0x01,
+			.default_value = 0x00,
+			.flags = 0,
+		},
+		.tweak = ov5630_t_gain,
+/* 		.query = ov5630_q_gain, */
+	},
+	{
+		.qc = {
+			.id = V4L2_CID_EXPOSURE,
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.name = "exposure",
+			.minimum = 0x0,
+			.maximum = 0xFFFF,
+			.step = 0x01,
+			.default_value = 0x00,
+			.flags = 0,
+		},
+		.tweak = ov5630_t_exposure,
+/* 		.query = ov5630_q_exposure; */
+	},
+};
+#define N_CONTROLS (ARRAY_SIZE(ov5630_controls))
+
+/*
+static int ov5630_g_gain(struct v4l2_subdev *sd, int value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned char v;
+
+	DBG_entering;
+
+	ov5630_write(client, 0x3000, &v);
+	dprintk(2, "writing gain %x to 0x3000", value);
+
+	value
+	DBG_leaving;
+	return 0
+}
+*/
+
+static struct ov5630_control *ov5630_find_control(__u32 id)
+{
+	int i;
+
+	for (i = 0; i < N_CONTROLS; i++)
+		if (ov5630_controls[i].qc.id == id)
+			return ov5630_controls + i;
+	return NULL;
+}
+
+static int ov5630_queryctrl(struct v4l2_subdev *sd,
+			    struct v4l2_queryctrl *qc)
+{
+	struct ov5630_control *ctrl = ov5630_find_control(qc->id);
+
+	if (ctrl == NULL)
+		return -EINVAL;
+	*qc = ctrl->qc;
+	return 0;
+}
+
+static int ov5630_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	/*
+	struct ov5630_control *octrl = ov5630_find_control(ctrl->id);
+
+	int ret;
+
+	if (octrl == NULL)
+		return -EINVAL;
+	ret = octrl->query(sd, &ctrl->value);
+	if (ret >= 0)
+		return 0;
+	return ret;
+	*/
+	return 0;
+}
+
+static int ov5630_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct ov5630_control *octrl = ov5630_find_control(ctrl->id);
+	int ret;
+
+	if (octrl == NULL)
+		return -EINVAL;
+	ret =  octrl->tweak(sd, ctrl->value);
+	if (ret >= 0)
+		return 0;
+	return ret;
+}
+
+#if 0
+static int ov5630_get_caps(struct i2c_client *c, struct ci_sensor_caps *caps)
+{
+	if (caps == NULL)
+		return -EIO;
+
+	caps->bus_width = SENSOR_BUSWIDTH_10BIT;
+	caps->mode	= SENSOR_MODE_BAYER;
+	caps->field_inv = SENSOR_FIELDINV_NOSWAP;
+	caps->field_sel = SENSOR_FIELDSEL_BOTH;
+	caps->ycseq	= SENSOR_YCSEQ_YCBYCR;
+	caps->conv422	= SENSOR_CONV422_NOCOSITED;
+	caps->bpat	= SENSOR_BPAT_BGBGGRGR;
+	caps->hpol	= SENSOR_HPOL_REFPOS;
+	caps->vpol	= SENSOR_VPOL_NEG;
+	caps->edge	= SENSOR_EDGE_RISING;
+	caps->bls	= SENSOR_BLS_OFF;
+	caps->gamma	= SENSOR_GAMMA_OFF;
+	caps->cconv	= SENSOR_CCONV_OFF;
+	caps->res	= SENSOR_RES_QXGA_PLUS | SENSOR_RES_1080P |
+	    SENSOR_RES_XGA_PLUS | SENSOR_RES_720P | SENSOR_RES_VGA;
+	caps->blc	= SENSOR_BLC_OFF;
+	caps->agc	= SENSOR_AGC_OFF;
+	caps->awb	= SENSOR_AWB_OFF;
+	caps->aec	= SENSOR_AEC_OFF;
+	caps->cie_profile = SENSOR_CIEPROF_D65 | SENSOR_CIEPROF_D75 |
+	    SENSOR_CIEPROF_F11 | SENSOR_CIEPROF_F12 | SENSOR_CIEPROF_A |
+	    SENSOR_CIEPROF_F2;
+	caps->flicker_freq	= SENSOR_FLICKER_100 | SENSOR_FLICKER_120;
+	caps->type	= SENSOR_TYPE_RAW;
+	/* caps->name	= "ov5630"; */
+	strcpy(caps->name, "ov5630");
+
+	return 0;
+}
+
+static int ov5630_get_config(struct i2c_client *c,
+			     struct ci_sensor_config *config)
+{
+	struct ci_sensor_config *info = i2c_get_clientdata(c);
+
+	if (config == NULL) {
+		printk(KERN_WARNING "sensor_get_config: NULL pointer\n");
+		return -EIO;
+	}
+
+	memcpy(config, info, sizeof(struct ci_sensor_config));
+
+	return 0;
+}
+
+static int ov5630_setup(struct i2c_client *c,
+			const struct ci_sensor_config *config)
+{
+	int ret;
+	u16 width, high;
+	struct ov5630_res_struct *res_index;
+	struct ci_sensor_config *info = i2c_get_clientdata(c);
+
+	/* Soft reset camera first*/
+	ret = ov5630_write(c, (u32)OV5630_SYS, (u32)0x80);
+
+	/* software sleep/standby */
+	ret = ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
+
+	/* Set registers into default config value */
+	ret = ov5630_write_array(c, ov5630_def_reg);
+
+	/* set image resolution */
+	ci_sensor_res2size(config->res, &width, &high);
+	ret += ov5630_try_res(&width, &high);
+	res_index = ov5630_find_res(width, high);
+	if (res_index->regs)
+		ret += ov5630_write_array(c, res_index->regs);
+	if (!ret)
+		info->res = res_index->res;
+
+	ret += ov5630_set_img_ctrl(c, config);
+
+	/* Set MIPI interface */
+#ifdef OV5630_MIPI
+	ret += ov5630_write_array(c, ov5630_mipi);
+#endif
+
+	/* streaming */
+	ret += ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x01);
+	ret += ov5630_write(c, (u32)0x3096, (u32)0x50);
+
+	/*Note here for the time delay */
+	/* ssleep(1); */
+	msleep(500);
+	return ret;
+}
+
+/*
+ * File operation functions
+ */
+static int ov5630_dvp_enable(struct i2c_client *client)
+{
+	int ret;
+
+	u8 reg;
+
+	ret = ov5630_read(client, 0x3506, &reg);
+	reg &= 0xdf;
+	reg |= 0x20;
+	ret += ov5630_write(client, 0x3506, reg);
+
+	return ret;
+}
+
+static int ov5630_dvp_disable(struct i2c_client *client)
+{
+	int ret;
+
+	u8 reg;
+
+	ret = ov5630_read(client, 0x3506, &reg);
+	reg &= 0xdf;
+	ret += ov5630_write(client, 0x3506, reg);
+
+	return ret;
+}
+
+static int ov5630_open(struct i2c_setting *c, void *priv)
+{
+	/* Just wake up sensor */
+	if (ov5630_wakeup())
+		return -EIO;
+	ov5630_init(c->sensor_client);
+	/* ov5630_motor_init(c->motor_client); */
+	ov5630_write(c->sensor_client, (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
+
+	/* disable dvp_en */
+	ov5630_dvp_disable(c->sensor_client);
+
+	return 0;
+}
+
+static int ov5630_release(struct i2c_setting *c, void *priv)
+{
+	/* Just suspend the sensor */
+	if (ov5630_standby())
+		return -EIO;
+	return 0;
+}
+
+static int ov5630_on(struct i2c_setting *c)
+{
+	int ret;
+
+	/* Software wake up sensor */
+	ret = ov5630_write(c->sensor_client,
+			    (u32)OV5630_IMAGE_SYSTEM, (u32)0x01);
+
+	/* enable dvp_en */
+	return ret + ov5630_dvp_enable(c->sensor_client);
+}
+
+static int ov5630_off(struct i2c_setting *c)
+{
+	int ret;
+
+	/* Software standby sensor */
+	ret = ov5630_write(c->sensor_client,
+			    (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
+	/* disable dvp_en */
+	return ret + ov5630_dvp_disable(c->sensor_client);
+}
+
+static struct sensor_device ov5630 = {
+	.name	= "ov5630",
+	.type	= SENSOR_TYPE_RAW,
+	.minor	= -1,
+	.open	= ov5630_open,
+	.release = ov5630_release,
+	.on = ov5630_on,
+	.off = ov5630_off,
+	.querycap = ov5630_get_caps,
+	.get_config = ov5630_get_config,
+	.set_config = ov5630_setup,
+	.enum_parm = ov5630_queryctrl,
+	.get_parm = ov5630_g_ctrl,
+	.set_parm = ov5630_s_ctrl,
+	.try_res = ov5630_try_res,
+	.set_res = ov5630_set_res,
+	.get_ls_corr_config = NULL,
+	.mdi_get_focus = ov5630_motor_get_focus,
+	.mdi_set_focus = ov5630_motor_set_focus,
+	.mdi_max_step = ov5630_motor_max_step,
+	.mdi_calibrate = NULL,
+	.read = ov5630_read,
+	.write = ov5630_write,
+	.suspend = ov5630_standby,
+	.resume = ov5630_wakeup,
+	/* TBC */
+};
+#endif
+
+static int ov5630_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	DBG_entering;
+
+	if (enable) {
+		ov5630_write(client, (u32)OV5630_IMAGE_SYSTEM, (u32)0x01);
+		ov5630_write(client, 0x30b0, 0xff);
+		ov5630_write(client, 0x30b1, 0xff);
+		msleep(500);
+	} else {
+		ov5630_write(client, (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
+		ov5630_write(client, 0x30b0, 0x00);
+		ov5630_write(client, 0x30b1, 0x00);
+	}
+
+	DBG_leaving;
+	return 0;
+}
+
+static int ov5630_enum_framesizes(struct v4l2_subdev *sd,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	unsigned int index = fsize->index;
+
+	DBG_entering;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = ov5630_res[index].width;
+	fsize->discrete.height = ov5630_res[index].height;
+	fsize->reserved[0] = ov5630_res[index].used;
+
+	DBG_leaving;
+
+	return 0;
+}
+
+static int ov5630_enum_frameintervals(struct v4l2_subdev *sd,
+				      struct v4l2_frmivalenum *fival)
+{
+	unsigned int index = fival->index;
+
+	DBG_entering;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete.numerator = 1;
+	fival->discrete.denominator = ov5630_res[index].fps;
+
+	DBG_leaving;
+
+	return 0;
+}
+
+static int ov5630_g_chip_ident(struct v4l2_subdev *sd,
+		struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+#define V4L2_IDENT_OV5630 8245
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_OV5630, 0);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov5630_g_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned char val = 0;
+	int ret;
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	ret = ov5630_read(client, reg->reg & 0xffff, &val);
+	reg->val = val;
+	reg->size = 1;
+	return ret;
+}
+
+static int ov5630_s_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	ov5630_write(client, reg->reg & 0xffff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static const struct v4l2_subdev_video_ops ov5630_video_ops = {
+	.try_fmt = ov5630_try_fmt,
+	.s_fmt = ov5630_set_fmt,
+	.g_fmt = ov5630_get_fmt,
+	.s_stream = ov5630_s_stream,
+	.enum_framesizes = ov5630_enum_framesizes,
+	.enum_frameintervals = ov5630_enum_frameintervals,
+};
+
+static const struct v4l2_subdev_core_ops ov5630_core_ops = {
+	.g_chip_ident = ov5630_g_chip_ident,
+	.queryctrl = ov5630_queryctrl,
+	.g_ctrl = ov5630_g_ctrl,
+	.s_ctrl = ov5630_s_ctrl,
+	.s_gpio = ov5630_s_power,
+	/*.g_ext_ctrls = ov5630_g_ext_ctrls,*/
+	/*.s_ext_ctrls = ov5630_s_ext_ctrls,*/
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = ov5630_g_register,
+	.s_register = ov5630_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_ops ov5630_ops = {
+	.core = &ov5630_core_ops,
+	.video = &ov5630_video_ops,
+};
+
+/*
+ * Basic i2c stuff
+ */
+/*
+static unsigned short normal_i2c[] = {I2C_OV5630 >> 1,
+	I2C_CLIENT_END};
+I2C_CLIENT_INSMOD;
+
+static struct i2c_driver ov5630_driver;
+*/
+static int ov5630_detect(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	int adap_id = i2c_adapter_id(adapter);
+	u32 value;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
+		eprintk("error i2c check func");
+		return -ENODEV;
+	}
+
+	if (adap_id != 1) {
+		eprintk("adap_id != 1");
+		return -ENODEV;
+	}
+
+	/* if (ov5630_wakeup()) */
+		/* return -ENODEV; */
+	ov5630_wakeup();
+
+	ov5630_read(client, (u32)OV5630_PID_H, &value);
+	if ((u8)value != 0x56) {
+		dprintk(1, "PID != 0x56, but %x", value);
+		dprintk(2, "client->addr = %x", client->addr);
+		return -ENODEV;
+	}
+
+	printk(KERN_INFO "Init ov5630 sensor success\n");
+	return 0;
+}
+
+static int ov5630_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct ci_sensor_config *info;
+	struct v4l2_subdev *sd;
+	int ret = -1;
+/* 	struct i2c_client *motor; */
+
+	DBG_entering;
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+		 client->addr << 1, client->adapter->name);
+	/*
+	 * Setup sensor configuration structure
+	 */
+	info = kzalloc(sizeof(struct ci_sensor_config), GFP_KERNEL);
+	if (!info) {
+		eprintk("fail to malloc for ci_sensor_config");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = ov5630_detect(client);
+	if (ret) {
+		dprintk(1, "error ov5630_detect");
+		goto out_free;
+	}
+
+	sd = &info->sd;
+	v4l2_i2c_subdev_init(sd, client, &ov5630_ops);
+
+	/*
+	 * Initialization OV5630
+	 * then turn into standby mode
+	 */
+	/* ret = ov5630_standby(); */
+	ret = ov5630_init(client);
+	if (ret) {
+		eprintk("error calling ov5630_init");
+		goto out_free;
+	}
+	ov5630_standby();
+
+	ret = 0;
+	goto out;
+
+out_free:
+	kfree(info);
+	DBG_leaving;
+out:
+	return ret;
+}
+
+/*
+ * XXX: Need to be checked
+ */
+static int ov5630_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	DBG_entering;
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_sensor_config(sd));
+
+	DBG_leaving;
+	return 0;
+}
+
+static const struct i2c_device_id ov5630_id[] = {
+	{"ov5630", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ov5630_id);
+
+static struct v4l2_i2c_driver_data v4l2_i2c_data = {
+	.name = "ov5630",
+	.probe = ov5630_probe,
+	.remove = ov5630_remove,
+	/* .suspend = ov5630_suspend,
+	 * .resume = ov5630_resume, */
+	.id_table = ov5630_id,
+};
+
+MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
+MODULE_DESCRIPTION("A low-level driver for OmniVision 5630 sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mrstci/mrstov5630/ov5630.h b/drivers/media/video/mrstci/mrstov5630/ov5630.h
new file mode 100644
index 0000000..3da0ecd
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov5630/ov5630.h
@@ -0,0 +1,672 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#define I2C_OV5630	0x6C
+/* Should add to kernel source */
+#define I2C_DRIVERID_OV5630	1046
+/* GPIO pin on Moorestown */
+#define GPIO_SCLK_25	44
+#define GPIO_STB_PIN	47
+#define GPIO_STDBY_PIN	49
+#define GPIO_RESET_PIN	50
+
+/* System control register */
+#define OV5630_AGC 		0x3000
+#define OV5630_AGCS 	0x3001
+#define OV5630_AEC_H	0x3002
+#define OV5630_AEC_L  	0x3003
+#define OV5630_LAEC_H 	0x3004
+#define OV5630_LAEC_L 	0x3005
+#define OV5630_AECS_H	0x3008
+#define OV5630_AECS_L	0x3009
+#define OV5630_PID_H	0x300A
+#define OV5630_PID_L	0x300B
+#define OV5630_SCCB_ID		0x300C
+#define OV5630_PLL_1	0x300E
+#define OV5630_PLL_2	0x300F
+#define OV5630_PLL_3	0x3010
+#define OV5630_PLL_4		0x3011
+#define OV5630_SYS		0x3012
+#define OV5630_AUTO_1	0x3013
+#define OV5630_AUTO_2	0x3014
+#define OV5630_AUTO_3	0x3015
+#define OV5630_AUTO_4	0x3016
+#define OV5630_AUTO_5	0x3017
+#define OV5630_WPT		0x3018
+#define OV5630_BPT		0x3019
+#define OV5630_VPT		0x301A
+#define OV5630_YAVG		0x301B
+#define OV5630_AECG_50	0x301C
+#define OV5630_AECG_60	0x301D
+#define OV5630_ADDVS_H	0x301E
+#define OV5630_ADDVS_L	0x301F
+#define OV5630_FRAME_LENGTH_LINES_H	0x3020
+#define OV5630_FRAME_LENGTH_LINES_L	0x3021
+#define OV5630_LINE_LENGTH_PCK_H	0x3022
+#define OV5630_LINE_LENGTH_PCK_L	0x3023
+#define OV5630_X_ADDR_START_H	0x3024
+#define OV5630_X_ADDR_START_L	0x3025
+#define OV5630_Y_ADDR_START_H	0x3026
+#define OV5630_Y_ADDR_START_L	0x3027
+#define OV5630_X_ADDR_END_H	0x3028
+#define OV5630_X_ADDR_END_L	0x3029
+#define OV5630_Y_ADDR_END_H	0x302A
+#define OV5630_Y_ADDR_END_L	0x302B
+#define OV5630_X_OUTPUT_SIZE_H	0x302C
+#define OV5630_X_OUTPUT_SIZE_L	0x302D
+#define OV5630_Y_OUTPUT_SIZE_H	0x302E
+#define OV5630_Y_OUTPUT_SIZE_L	0x302F
+#define OV5630_FRAME_CNT	0x3030
+#define OV5630_DATR_LMO_0	0x3038
+#define OV5630_DATR_LMO_1	0x3039
+#define OV5630_DATR_LMO_2	0x303A
+#define OV5630_DATR_D56	0x303D
+#define OV5630_DATR_EF	0x303E
+#define OV5630_R_SIGMA_0	0x3048
+#define OV5630_R_SIGMA_1	0x3049
+#define OV5630_R_SIGMA_2	0x304A
+#define OV5630_R_SIGMA_3	0x304B
+#define OV5630_R_SIGMA_4	0x304C
+#define OV5630_R_SIGMA_5	0x304D
+#define OV5630_D56COM	0x304E
+#define OV5630_5060TH	0x3050
+#define OV5630_LMO_TH1	0x3058
+#define OV5630_LMO_TH2	0x3059
+#define OV5630_LMO_K	0x305A
+#define OV5630_BD50ST_H	0x305C
+#define OV5630_BD50ST_L	0x305D
+#define OV5630_BD60ST_H	0x305E
+#define OV5630_BD60ST_L	0x305F
+#define OV5630_HSYNST	0x306D
+#define OV5630_HSYNED	0x306E
+#define OV5630_HSYNED_HSYNST	0x306F
+#define OV5630_TMC_RWIN0	0x3070
+#define OV5630_IO_CTRL0	0x30B0
+#define OV5630_IO_CTRL1	0x30B1
+#define OV5630_IO_CTRL2	0x30B2
+#define OV5630_DSIO_0	0x30B3
+#define OV5630_DSIO_1	0x30B4
+#define OV5630_DSIO_2	0x30B5
+#define OV5630_TMC_10	0x30B6
+#define OV5630_TMC_12	0x30B7
+#define OV5630_TMC_14	0x30B9
+#define OV5630_TMC_COM4	0x30BA
+#define OV5630_TMC_REG6C	0x30BB
+#define OV5630_TMC_REG6E	0x30BC
+#define OV5630_R_CLK_S	0x30BD
+#define OV5630_R_CLK_A	0x30BE
+#define OV5630_R_CLK_A1	0x30BF
+#define OV5630_FRS_0	0x30E0
+#define OV5630_FRS_1	0x30E1
+#define OV5630_FRS_2	0x30E2
+#define OV5630_FRS_3	0x30E3
+#define OV5630_FRS_FECNT	0x30E4
+#define OV5630_FRS_FECNT_0	0x30E5
+#define OV5630_FRS_FECNT_1	0x30E6
+#define OV5630_FRS_RFRM	0x30E7
+#define OV5630_FRS_RSTRB	0x30E8
+#define OV5630_SA1TMC	0x30E9
+#define OV5630_TMC_MISC0	0x30EA
+#define OV5630_TMC_MISC1	0x30EB
+#define OV5630_FLEX_TXP	0x30F0
+#define OV5630_FLEX_FLT	0x30F1
+#define OV5630_FLEX_TXT	0x30F2
+#define OV5630_FLEX_HBK	0x30F3
+#define OV5630_FLEX_HSG	0x30F4
+#define OV5630_FLEX_SA1SFT	0x30F5
+#define OV5630_RVSOPT	0x30F6
+#define OV5630_AUTO	0x30F7
+#define OV5630_IMAGE_TRANSFORM	0x30F8
+#define OV5630_IMAGE_LUM	0x30F9
+#define OV5630_IMAGE_SYSTEM	0x30FA
+#define OV5630_GROUP_WR	0x30FF
+
+/* CIF control register */
+#define OV5630_CIF_CTRL2	0x3202
+
+/* ISP control register */
+#define OV5630_ISP_CTL00	0x3300
+#define OV5630_ISP_CTL01	0x3301
+#define OV5630_ISP_CTL02	0x3302
+#define OV5630_ISP_03	0x3303
+#define OV5630_ISP_DIG_GAIN_MAN	0x3304
+#define OV5630_ISP_BIAS_MAN	0x3305
+#define OV5630_ISP_06	0x3306
+#define OV5630_ISP_STABLE_RANGE	0x3307
+#define OV5630_ISP_R_GAIN_MAN_1	0x3308
+#define OV5630_ISP_R_GAIN_MAN_0	0x3309
+#define OV5630_ISP_G_GAIN_MAN_1	0x330A
+#define OV5630_ISP_G_GAIN_MAN_0	0x330B
+#define OV5630_ISP_B_GAIN_MAN_1	0x330C
+#define OV5630_ISP_B_GAIN_MAN_0	0x330D
+#define OV5630_ISP_STABLE_RANGEW	0x330E
+#define OV5630_ISP_AWB_FRAME_CNT	0x330F
+#define OV5630_ISP_11	0x3311
+#define OV5630_ISP_12	0x3312
+#define OV5630_ISP_13	0x3313
+#define OV5630_ISP_HSIZE_IN_1	0x3314
+#define OV5630_ISP_HSIZE_IN_0	0x3315
+#define OV5630_ISP_VSIZE_IN_1	0x3316
+#define OV5630_ISP_VSIZE_IN_0	0x3317
+#define OV5630_ISP_18	0x3318
+#define OV5630_ISP_19	0x3319
+#define OV5630_ISP_EVEN_MAN0	0x331A
+#define OV5630_ISP_EVEN_MAN1	0x331B
+#define OV5630_ISP_EVEN_MAN2	0x331C
+#define OV5630_ISP_EVEN_MAN3	0x331D
+#define OV5630_ISP_1E	0x331E
+#define OV5630_ISP_1F	0x331F
+#define OV5630_ISP_BLC_LMT_OPTION	0x3320
+#define OV5630_ISP_BLC_THRE	0x3321
+#define OV5630_ISP_22	0x3322
+#define OV5630_ISP_23	0x3323
+#define OV5630_ISP_BLC_MAN0_1	0x3324
+#define OV5630_ISP_BLC_MAN0_0	0x3325
+#define OV5630_ISP_BLC_MAN1_1	0x3326
+#define OV5630_ISP_BLC_MAN1_0	0x3327
+#define OV5630_ISP_BLC_MAN2_1	0x3328
+#define OV5630_ISP_BLC_MAN2_0	0x3329
+#define OV5630_ISP_BLC_MAN3_1	0x332A
+#define OV5630_ISP_BLC_MAN3_0	0x332B
+#define OV5630_ISP_BLC_MAN4_1	0x332C
+#define OV5630_ISP_BLC_MAN4_0	0x332D
+#define OV5630_ISP_BLC_MAN5_1	0x332E
+#define OV5630_ISP_BLC_MAN5_0	0x332F
+#define OV5630_ISP_BLC_MAN6_1	0x3330
+#define OV5630_ISP_BLC_MAN6_0	0x3331
+#define OV5630_ISP_BLC_MAN7_1	0x3332
+#define OV5630_ISP_BLC_MAN7_0	0x3333
+#define OV5630_ISP_CD	0x33CD
+#define OV5630_ISP_FF	0x33FF
+
+/* clipping control register */
+#define OV5630_CLIP_CTRL0	0x3400
+#define OV5630_CLIP_CTRL1	0x3401
+#define OV5630_CLIP_CTRL2	0x3402
+#define OV5630_CLIP_CTRL3	0x3403
+#define OV5630_CLIP_CTRL4	0x3404
+#define OV5630_CLIP_CTRL5	0x3405
+#define OV5630_CLIP_CTRL6	0x3406
+#define OV5630_CLIP_CTRL7	0x3407
+
+/* DVP control register */
+#define OV5630_DVP_CTRL00	0x3500
+#define OV5630_DVP_CTRL01	0x3501
+#define OV5630_DVP_CTRL02	0x3502
+#define OV5630_DVP_CTRL03	0x3503
+#define OV5630_DVP_CTRL04	0x3504
+#define OV5630_DVP_CTRL05	0x3505
+#define OV5630_DVP_CTRL06	0x3506
+#define OV5630_DVP_CTRL07	0x3507
+#define OV5630_DVP_CTRL08	0x3508
+#define OV5630_DVP_CTRL09	0x3509
+#define OV5630_DVP_CTRL0A	0x350A
+#define OV5630_DVP_CTRL0B	0x350B
+#define OV5630_DVP_CTRL0C	0x350C
+#define OV5630_DVP_CTRL0D	0x350D
+#define OV5630_DVP_CTRL0E	0x350E
+#define OV5630_DVP_CTRL0F	0x350F
+#define OV5630_DVP_CTRL10	0x3510
+#define OV5630_DVP_CTRL11	0x3511
+#define OV5630_DVP_CTRL12	0x3512
+#define OV5630_DVP_CTRL13	0x3513
+#define OV5630_DVP_CTRL14	0x3514
+#define OV5630_DVP_CTRL15	0x3515
+#define OV5630_DVP_CTRL16	0x3516
+#define OV5630_DVP_CTRL17	0x3517
+#define OV5630_DVP_CTRL18	0x3518
+#define OV5630_DVP_CTRL19	0x3519
+#define OV5630_DVP_CTRL1A	0x351A
+#define OV5630_DVP_CTRL1B	0x351B
+#define OV5630_DVP_CTRL1C	0x351C
+#define OV5630_DVP_CTRL1D	0x351D
+#define OV5630_DVP_CTRL1E	0x351E
+#define OV5630_DVP_CTRL1F	0x351F
+
+/* MIPI control register */
+#define OV5630_MIPI_CTRL00	0x3600
+#define OV5630_MIPI_CTRL01	0x3601
+#define OV5630_MIPI_CTRL02	0x3602
+#define OV5630_MIPI_CTRL03	0x3603
+#define OV5630_MIPI_CTRL04	0x3604
+#define OV5630_MIPI_CTRL05	0x3605
+#define OV5630_MIPI_CTRL06	0x3606
+#define OV5630_MIPI_CTRL07	0x3607
+#define OV5630_MIPI_CTRL08	0x3608
+#define OV5630_MIPI_CTRL09	0x3609
+#define OV5630_MIPI_CTRL0A	0x360A
+#define OV5630_MIPI_CTRL0B	0x360B
+#define OV5630_MIPI_CTRL0C	0x360C
+#define OV5630_MIPI_CTRL0D	0x360D
+#define OV5630_MIPI_CTRL0E	0x360E
+#define OV5630_MIPI_CTRL0F	0x360F
+#define OV5630_MIPI_CTRL10	0x3610
+#define OV5630_MIPI_CTRL11	0x3611
+#define OV5630_MIPI_CTRL12	0x3612
+#define OV5630_MIPI_CTRL13	0x3613
+#define OV5630_MIPI_CTRL14	0x3614
+#define OV5630_MIPI_CTRL15	0x3615
+#define OV5630_MIPI_CTRL16	0x3616
+#define OV5630_MIPI_CTRL17	0x3617
+#define OV5630_MIPI_CTRL18	0x3618
+#define OV5630_MIPI_CTRL19	0x3619
+#define OV5630_MIPI_CTRL1A	0x361A
+#define OV5630_MIPI_CTRL1B	0x361B
+#define OV5630_MIPI_CTRL1C	0x361C
+#define OV5630_MIPI_CTRL1D	0x361D
+#define OV5630_MIPI_CTRL1E	0x361E
+#define OV5630_MIPI_CTRL1F	0x361F
+#define OV5630_MIPI_CTRL20	0x3620
+#define OV5630_MIPI_CTRL21	0x3621
+#define OV5630_MIPI_CTRL22	0x3622
+#define OV5630_MIPI_CTRL23	0x3623
+#define OV5630_MIPI_CTRL24	0x3624
+#define OV5630_MIPI_CTRL25	0x3625
+#define OV5630_MIPI_CTRL26	0x3626
+#define OV5630_MIPI_CTRL27	0x3627
+#define OV5630_MIPI_CTRL28	0x3628
+#define OV5630_MIPI_CTRL29	0x3629
+#define OV5630_MIPI_CTRL2A	0x362A
+#define OV5630_MIPI_CTRL2B	0x362B
+#define OV5630_MIPI_CTRL2C	0x362C
+#define OV5630_MIPI_CTRL2D	0x362D
+#define OV5630_MIPI_CTRL2E	0x362E
+#define OV5630_MIPI_CTRL2F	0x362F
+#define OV5630_MIPI_CTRL30	0x3630
+#define OV5630_MIPI_CTRL31	0x3631
+#define OV5630_MIPI_CTRL32	0x3632
+#define OV5630_MIPI_CTRL33	0x3633
+#define OV5630_MIPI_CTRL34	0x3634
+#define OV5630_MIPI_CTRL35	0x3635
+#define OV5630_MIPI_CTRL36	0x3636
+#define OV5630_MIPI_CTRL37	0x3637
+#define OV5630_MIPI_CTRL38	0x3638
+#define OV5630_MIPI_CTRL39	0x3639
+#define OV5630_MIPI_CTRL3A	0x363A
+#define OV5630_MIPI_CTRL3B	0x363B
+#define OV5630_MIPI_CTRL3C	0x363C
+#define OV5630_MIPI_CTRL3D	0x363D
+#define OV5630_MIPI_CTRL3E	0x363E
+#define OV5630_MIPI_CTRL3F	0x363F
+#define OV5630_MIPI_RO61	0x3661
+#define OV5630_MIPI_RO62	0x3662
+#define OV5630_MIPI_RO63	0x3663
+#define OV5630_MIPI_RO64	0x3664
+#define OV5630_MIPI_RO65	0x3665
+#define OV5630_MIPI_RO66	0x3666
+
+/* General definition for ov5630 */
+#define OV5630_OUTWND_MAX_H		QSXXGA_PLUS4_SIZE_H
+#define OV5630_OUTWND_MAX_V		QSXGA_PLUS4_SIZE_V
+
+struct regval_list {
+	u16 reg_num;
+	u8 value;
+};
+
+/*
+ * Default register value
+ * 5Mega Pixel, 2592x1944
+ */
+static struct regval_list ov5630_def_reg[] = {
+	{0x300f, 0x00}, /*00*/
+	{0x30b2, 0x32},
+	{0x3084, 0x44},
+	{0x3016, 0x01},
+	{0x308a, 0x25},
+
+	{0x3013, 0xff},
+	{0x3015, 0x03},
+	{0x30bf, 0x02},
+
+	{0x3065, 0x50},
+	{0x3068, 0x08},
+	{0x30ac, 0x05},
+	{0x309e, 0x24},
+	{0x3091, 0x04},
+
+	{0x3075, 0x22},
+	{0x3076, 0x23},
+	{0x3077, 0x24},
+	{0x3078, 0x25},
+
+	{0x30b5, 0x0c},
+	{0x3090, 0x67},
+
+	{0x30f9, 0x11},
+	{0x3311, 0x80},
+	{0x3312, 0x1f},
+
+	{0x3103, 0x10},
+	{0x305c, 0x01},
+	{0x305d, 0x29},
+	{0x305e, 0x00},
+	{0x305f, 0xf7},
+	{0x308d, 0x0b},
+	{0x30ad, 0x20},
+	{0x3072, 0x0d},
+	{0x308b, 0x82},
+	{0x3317, 0x9c},
+	{0x3318, 0x22},
+	{0x3025, 0x20},
+	{0x3027, 0x08},
+	{0x3029, 0x3f},
+	{0x302b, 0xa3},
+	{0x3319, 0x22},
+	{0x30a1, 0xc4},
+	{0x306a, 0x05},
+	{0x3315, 0x22},
+	{0x30ae, 0x25},
+	{0x3304, 0x40},
+	{0x3099, 0x49},
+
+	{0x300e, 0xb1/*b0*/}, /* Note this PLL setting*/
+	{0x300f, 0x10}, /*00*/
+	{0x3010, 0x07}, /*change from 0f according to SV */
+	{0x3011, 0x40},
+	{0x30af, 0x10},
+	{0x304a, 0x00},
+	{0x304d, 0x00},
+
+	{0x304e, 0x22},
+	{0x304d, 0xa0},
+	{0x3058, 0x00},
+	{0x3059, 0xff},
+	{0x305a, 0x00},
+
+	{0x30e9, 0x04},
+	{0x3084, 0x44},
+	{0x3090, 0x67},
+	{0x30e9, 0x04},
+
+	{0x30b5, 0x1c},
+	{0x331f, 0x22},
+	{0x30ae, 0x15},
+	{0x3304, 0x4c},
+
+	{0x3300, 0xfb},
+	{0x3071, 0x34},
+	{0x30e7, 0x01},
+	{0x3302, 0x60},
+	{0x331e, 0x05},
+	{0x3321, 0x04},
+
+	/* Mark end */
+	{0xffff, 0xff},
+
+};
+
+/* MIPI register are removed by Wen */
+
+/* 2592x1944 */
+static struct regval_list ov5630_res_qsxga_plus4[] = {
+	{0x3020, 0x07},
+	{0x3021, 0xbc},
+	{0x3022, 0x0c/*0a*/},
+	{0x3023, 0xa0/*00*/},
+	{0x305c, 0x01},
+	{0x305d, 0x29},
+	{0x305e, 0x00},
+	{0x305f, 0xf7},
+
+	/* 30fps , 96 MHZ*/
+	/* {0x300f, 0x10}, */
+	{0x300f, 0x10},
+	{0x300e, 0xb1},
+	/* mipi */
+#ifdef MIPI
+	{0x30b0, 0x00},
+	{0x30b1, 0xfc},
+	{0x3603, 0x50},
+	{0x3601, 0x0F},
+	/* lan2 bit 10*/
+	{0x3010, 0x07},
+	{0x30fa, 0x01},
+	/* {0x 30f8 09 */
+	{0x3096, 0x50},
+	/* end mipi*/
+#else
+	/* parrral */
+	{0x30fa, 0x01},
+#endif
+	/* end post*/
+	{0xffff, 0xff},
+};
+
+/* 1920x1080 */
+static struct regval_list ov5630_res_1080p[] = {
+	/*res start*/
+	{0x3020, 0x04},
+	{0x3021, 0x5c},
+	{0x3022, 0x0b/*0a*/},
+	{0x3023, 0x32/*00*/},
+	{0x305c, 0x01},
+	{0x305d, 0x2c},
+	{0x3024, 0x01},
+	{0x3025, 0x6e/*70*/},
+	{0x3026, 0x01},
+	{0x3027, 0xb8},
+	{0x3028, 0x08},
+	{0x3029, 0xef},
+	{0x302a, 0x05},
+	{0x302b, 0xf3},
+	{0x302c, 0x07},
+	{0x302d, 0x80},
+	{0x302e, 0x04},
+	{0x302f, 0x38},
+	{0x3314, 0x07},
+	{0x3315, 0x82/*80*/},
+	{0x3316, 0x04},
+	{0x3317, 0x3c},
+
+	/* 30fps , 96 MHZ*/
+	{0x300f, 0x10}, /* 00 */
+	{0x300e, 0xb1},
+
+	/* mipi */
+#ifdef MIPI
+	{0x30b0, 0x00},
+	{0x30b1, 0xfc},
+	{0x3603, 0x50},
+	{0x3601, 0x0F},
+	/* lan2 bit 10*/
+	{0x3010, 0x07},
+	{0x30fa, 0x01},
+	/* {0x 30f8 09 */
+	{0x3096, 0x50},
+	/* end mipi*/
+#else
+	/* parrral */
+	{0x30fa, 0x01},
+#endif
+	/* end post*/
+	{0xffff, 0xff},
+};
+
+/* 1280x960 V1F2_H1F2 */
+static struct regval_list ov5630_res_xga_plus[] = {
+	{0x3020, 0x03},
+	{0x3021, 0xe4},
+	{0x3022, 0x0c/*07*/},
+	{0x3023, 0x8c/*76*/},
+	{0x305c, 0x00},
+	{0x305d, 0xb1},
+	{0x3024, 0x00},
+	{0x3025, 0x30},
+	{0x3026, 0x00},
+	{0x3027, 0x10/*14*/},
+	{0x3028, 0x0a},
+	{0x3029, 0x2f},
+	{0x302a, 0x07},
+	{0x302b, 0xa7/*a7*/},
+	{0x302c, 0x05},
+	{0x302d, 0x00},
+	{0x302e, 0x03},
+	{0x302f, 0xc0},
+
+	{0x30f8, 0x05},
+	{0x30f9, 0x13},
+	{0x3314, 0x05},
+	{0x3315, 0x02/*00*/},
+	{0x3316, 0x03},
+	{0x3317, 0xc4},
+
+	{0x300f, 0x10}, /* 00 */
+	{0x300e, 0xb1},
+
+#ifdef MIPI
+	{0x30b0, 0x00},
+	{0x30b1, 0xfc},
+	{0x3603, 0x50},
+	{0x3601, 0x0F},
+	/* lan2 bit 10*/
+	{0x3010, 0x07},
+	{0x30fa, 0x01},
+	/* {0x 30f8 09 */
+	{0x3096, 0x50},
+	/* end mipi*/
+#else
+	/* parrral */
+	{0x30fa, 0x01},
+#endif
+
+	{0xffff, 0xff},
+};
+
+/* 1280x720, V1F2 & H1F2 */
+static struct regval_list ov5630_res_720p[] = {
+	{0x3020, 0x02},
+	{0x3021, 0xf4},
+	{0x3022, 0x07},
+	{0x3023, 0x80},
+	{0x305c, 0x00},
+	{0x305d, 0xff},
+	{0x305e, 0x00},
+	{0x305f, 0xd4},
+
+	/* Crop then downscale */
+	{0x3024, 0x00},
+	{0x3025, 0x2c},
+	{0x3026, 0x00},
+	{0x3027, 0xf0},
+	{0x3028, 0x0a},
+	{0x3029, 0x2f},
+	{0x302a, 0x08},
+	{0x302b, 0x97},
+
+	{0x30f8, 0x05},
+
+	{0x302c, 0x05},
+	{0x302d, 0x00},
+	{0x302e, 0x02},
+	{0x302f, 0xd0},
+
+	{0x30f9, 0x13},
+	{0x3314, 0x05},
+	{0x3315, 0x04},
+	{0x3316, 0x02},
+	{0x3317, 0xd4},
+
+	/* Add this to test setting from OVT */
+	{0x300f, 0x10}, /*00*/
+	{0x300e, 0xb0},
+
+#ifdef MIPI
+	{0x30b0, 0x00},
+	{0x30b1, 0xfc},
+	{0x3603, 0x50},
+	{0x3601, 0x0F},
+	/* lan2 bit 10*/
+	{0x3010, 0x07},
+	{0x30fa, 0x01},
+	/* {0x 30f8 09 */
+	{0x3096, 0x50},
+	/* end mipi*/
+#else
+	/* parrral */
+	{0x30fa, 0x01},
+#endif
+
+	{0xffff, 0xff},
+};
+
+/*VGA 40fps now*/
+static struct regval_list ov5630_res_vga_ac04_bill[] = {
+	/* res setting*/
+	{0x3020, 0x02},
+	{0x3021, 0x04},
+	{0x3022, 0x08},
+	{0x3023, 0x48},
+	{0x305c, 0x00},
+	{0x305d, 0x5e},
+	{0x3024, 0x00},
+	{0x3025, 0x2c},/*2c*/
+	{0x3026, 0x00},
+	{0x3027, 0x14},
+	{0x3028, 0x0a},
+	{0x3029, 0x2f},
+	{0x302a, 0x07},
+	{0x302b, 0xa3},
+	{0x302c, 0x02},
+	{0x302d, 0x80},
+	{0x302e, 0x01},
+	{0x302f, 0xe0},
+
+	{0x30b3, 0x09},
+	{0x3301, 0xc1},
+	{0x3313, 0xf1},
+	{0x3314, 0x05},
+	{0x3315, 0x04},/*04*/
+	{0x3316, 0x01},
+	{0x3317, 0xe4},
+	{0x3318, 0x20},
+
+	{0x300f, 0x10/*00*/},
+	{0x30f8, 0x09},
+
+	{0x300f, 0x11},
+	{0x300e, 0xb2},
+
+	{0x3015, 0x02},
+	/* mipi */
+#ifdef MIPI
+	{0x30b0, 0x00},
+	{0x30b1, 0xfc},
+	{0x3603, 0x50},
+	{0x3601, 0x0F},
+	/* lan2 bit 10*/
+	{0x3010, 0x07},
+	{0x30fa, 0x01},
+	/* {0x 30f8 09 */
+	{0x3096, 0x50},
+	/* end mipi*/
+#else
+
+	/* parrral */
+	{0x30fa, 0x01},
+	{0x30f8, 0x09},
+	{0x3096, 0x50},
+#endif
+
+	{0xffff, 0xff},
+};
diff --git a/drivers/media/video/mrstci/mrstov5630_motor/Kconfig b/drivers/media/video/mrstci/mrstov5630_motor/Kconfig
new file mode 100644
index 0000000..b6dcf62
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov5630_motor/Kconfig
@@ -0,0 +1,9 @@
+config VIDEO_MRST_OV5630_MOTOR
+	tristate "Moorestown OV5630 motor"
+	depends on I2C && VIDEO_MRST_ISP && VIDEO_MRST_OV5630
+
+	---help---
+	  Say Y here if your platform support OV5630 motor
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mrstov2650.ko.
diff --git a/drivers/media/video/mrstci/mrstov5630_motor/Makefile b/drivers/media/video/mrstci/mrstov5630_motor/Makefile
new file mode 100644
index 0000000..056b2a6
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov5630_motor/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VIDEO_MRST_OV2650)	 += mrstov5630_motor.o
+
+EXTRA_CFLAGS	+=	-I$(src)/../include
diff --git a/drivers/media/video/mrstci/mrstov5630_motor/mrstov5630_motor.c b/drivers/media/video/mrstci/mrstov5630_motor/mrstov5630_motor.c
new file mode 100644
index 0000000..1bb7274
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov5630_motor/mrstov5630_motor.c
@@ -0,0 +1,428 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-i2c-drv.h>
+
+#include "ov5630_motor.h"
+
+/* #define OSPM */
+#include <asm/ipc_defs.h>
+#define PMIC_WRITE1(ipcbuf, reg1, val1) \
+	do { \
+		memset(&ipcbuf, 0, sizeof(struct ipc_pmic_reg_data)); \
+		ipcbuf.ioc = 0; \
+		ipcbuf.num_entries = 1; \
+		ipcbuf.pmic_reg_data[0].register_address = reg1; \
+		ipcbuf.pmic_reg_data[0].value = val1; \
+		if (ipc_pmic_register_write(&ipcbuf, 1) != 0) { \
+			return -1; \
+		} \
+	} while (0);
+
+static int mrstov5630_motor_debug;
+module_param(mrstov5630_motor_debug, int, 0644);
+MODULE_PARM_DESC(mrstov5630_motor_debug, "Debug level (0-1)");
+
+#define dprintk(level, fmt, arg...) do {			\
+	if (mrstov5630_motor_debug >= level) 				\
+		printk(KERN_DEBUG "mrstisp@%s: " fmt "\n", \
+		       __func__, ## arg); } \
+	while (0)
+
+#define eprintk(fmt, arg...)	\
+	printk(KERN_ERR "mrstisp@%s: line %d: " fmt "\n",	\
+	       __func__, __LINE__, ## arg);
+
+#define DBG_entering	dprintk(2, "entering");
+#define DBG_leaving	dprintk(2, "leaving");
+#define DBG_line	dprintk(2, " line: %d", __LINE__);
+
+static inline struct ov5630_motor *to_motor_config(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov5630_motor, sd);
+}
+
+static int motor_read(struct i2c_client *c, u16 *reg)
+{
+	int ret;
+	struct i2c_msg msg;
+	u8 msgbuf[2];
+
+	/* Read needs two message to go */
+	msgbuf[0] = 0;
+	msgbuf[1] = 0;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.addr = c->addr;
+	msg.buf = msgbuf;
+	msg.len = 2;
+	msg.flags = I2C_M_RD;
+
+	ret = i2c_transfer(c->adapter, &msg, 1);
+
+	*reg = (msgbuf[0] << 8 | msgbuf[1]);
+
+	ret = (ret == 1) ? 0 : -1;
+	return ret;
+}
+
+static int motor_write(struct i2c_client *c, u16 reg)
+{
+	int ret;
+	struct i2c_msg msg;
+	u8 msgbuf[2];
+
+	/* Writing only needs one message */
+	memset(&msg, 0, sizeof(msg));
+	msgbuf[0] = reg >> 8;
+	msgbuf[1] = reg;
+
+	msg.addr = c->addr;
+	msg.flags = 0;
+	msg.buf = msgbuf;
+	msg.len = 2;
+
+	ret = i2c_transfer(c->adapter, &msg, 1);
+
+	ret = (ret == 1) ? 0 : -1;
+	return ret;
+}
+
+static int ov5630_motor_goto_position(struct i2c_client *c,
+				      unsigned short code,
+				      struct ov5630_motor *config)
+{
+	int max_code, min_code;
+	u8 cmdh, cmdl;
+	u16 cmd, val = 0;
+
+	max_code = config->macro_code;
+	min_code = config->infin_code;
+
+	if (code > max_code)
+		code = max_code;
+	if (code < min_code)
+		code = min_code;
+
+	cmdh = (MOTOR_DAC_CODE_H(code));
+	cmdl = (MOTOR_DAC_CODE_L(code) | MOTOR_DAC_CTRL_MODE_2(SUB_MODE_4));
+	cmd = cmdh << 8 | cmdl;
+
+	motor_write(c, cmd);
+	/*Delay more than full-scale transition time 8.8ms*/
+	msleep(8);
+	motor_read(c, &val);
+
+	return (cmd == val ? 0 : -1);
+}
+
+int ov5630_motor_wakeup(void)
+{
+	return gpio_direction_output(GPIO_AF_PD, 1);
+}
+
+int ov5630_motor_standby(void)
+{
+	return gpio_direction_output(GPIO_AF_PD, 0);
+}
+
+int ov5630_motor_init(struct i2c_client *client, struct ov5630_motor *config)
+{
+	int ret;
+	int infin_cur, macro_cur;
+#ifdef OSPM
+	/* Power on motor */
+	struct ipc_pmic_reg_data ipcbuf;
+
+	PMIC_WRITE1(ipcbuf, 0x50, 0x27);
+	printk(KERN_WARNING "Power on Vcc33 for motor\n");
+#endif
+
+	infin_cur = MAX(MOTOR_INFIN_CUR, MOTOR_DAC_MIN_CUR);
+	macro_cur = MIN(MOTOR_MACRO_CUR, MOTOR_DAC_MAX_CUR);
+
+	config->infin_cur = infin_cur;
+	config->macro_cur = macro_cur;
+
+	config->infin_code = (int)((infin_cur * MOTOR_DAC_MAX_CODE)
+				   / MOTOR_DAC_MAX_CUR);
+	config->macro_code = (int)((macro_cur * MOTOR_DAC_MAX_CODE)
+				   / MOTOR_DAC_MAX_CUR);
+
+	config->max_step = ((config->macro_code - config->infin_code)
+			    >> MOTOR_STEP_SHIFT) + 1;
+	/* Note here, maybe macro_code */
+	ret = ov5630_motor_goto_position(client, config->infin_code, config);
+	if (!ret)
+		config->cur_code = config->infin_code;
+	else
+		printk(KERN_ERR "Error while initializing motor\n");
+
+	return ret;
+}
+
+int ov5630_motor_set_focus(struct i2c_client *c, int step,
+			   struct ov5630_motor *config)
+{
+	int s_code, ret;
+	int max_step = config->max_step;
+	unsigned int val = step;
+
+	DBG_entering;
+	dprintk(1, "setting setp %d", step);
+	if (val > max_step)
+		val = max_step;
+
+	s_code = (val << MOTOR_STEP_SHIFT);
+	s_code += config->infin_code;
+
+	ret = ov5630_motor_goto_position(c, s_code, config);
+	if (!ret)
+		config->cur_code = s_code;
+
+	DBG_leaving;
+	return ret;
+}
+
+static int ov5630_motor_s_ctrl(struct v4l2_subdev *sd,
+			       struct v4l2_control *ctrl)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	struct ov5630_motor *config = to_motor_config(sd);
+	int ret;
+
+	DBG_entering;
+	ret = ov5630_motor_set_focus(c, ctrl->value, config);
+	if (ret) {
+		eprintk("error call ov5630_motor_set_focue");
+		return ret;
+	}
+	DBG_leaving;
+	return 0;
+}
+int ov5630_motor_get_focus(struct i2c_client *c, unsigned int *step,
+			   struct ov5630_motor *config)
+{
+	int ret_step;
+
+	ret_step = ((config->cur_code - config->infin_code)
+		    >> MOTOR_STEP_SHIFT);
+
+	if (ret_step <= config->max_step)
+		*step = ret_step;
+	else
+		*step = config->max_step;
+
+	return 0;
+}
+
+static int ov5630_motor_g_ctrl(struct v4l2_subdev *sd,
+			       struct v4l2_control *ctrl)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	struct ov5630_motor *config = to_motor_config(sd);
+	int ret;
+
+	DBG_entering;
+	dprintk(2, "c = %p, config = %p, ctrl = %p", c, config, ctrl);
+	ret = ov5630_motor_get_focus(c, &ctrl->value, config);
+	if (ret) {
+		eprintk("error call ov5630_motor_get_focue");
+		return ret;
+	}
+	DBG_leaving;
+	return 0;
+}
+int ov5630_motor_max_step(struct i2c_client *c, unsigned int *max_code,
+			   struct ov5630_motor *config)
+{
+	if (config->max_step != 0)
+		*max_code = config->max_step;
+	return 0;
+}
+
+static int ov5630_motor_queryctrl(struct v4l2_subdev *sd,
+			    struct v4l2_queryctrl *qc)
+{
+	struct ov5630_motor *config = to_motor_config(sd);
+
+	DBG_entering;
+
+	if (qc->id != V4L2_CID_FOCUS_ABSOLUTE)
+		return -EINVAL;
+
+	dprintk(1, "got focus range of %d", config->max_step);
+	if (config->max_step != 0)
+		qc->maximum = config->max_step;
+	DBG_leaving;
+	return 0;
+}
+static const struct v4l2_subdev_core_ops ov5630_motor_core_ops = {
+	/*
+	.queryctrl = ov5630_queryctrl,
+	.g_ctrl = ov5630_g_ctrl,
+	*/
+	.g_ctrl = ov5630_motor_g_ctrl,
+	.s_ctrl = ov5630_motor_s_ctrl,
+	.queryctrl = ov5630_motor_queryctrl,
+};
+
+static const struct v4l2_subdev_ops ov5630_motor_ops = {
+	.core = &ov5630_motor_core_ops,
+};
+
+static int ov5630_motor_detect(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	int adap_id = i2c_adapter_id(adapter);
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
+		eprintk("error i2c check func");
+		return -ENODEV;
+	}
+
+	if (adap_id != 1) {
+		eprintk("adap_id != 1");
+		return -ENODEV;
+	}
+
+	/* if (ov5630_motor_wakeup()) */
+		/* return -ENODEV; */
+	ov5630_motor_wakeup();
+	ssleep(1);
+
+	/*
+	ov5630_motor_read(client, (u32)OV5630_PID_H, &value);
+	if ((u8)value != 0x56) {
+		eprintk("PID != 0x56, but %x", value);
+		dprintk(2, "client->addr = %x", client->addr);
+		return -ENODEV;
+	}
+	*/
+
+	return 0;
+}
+
+static int ov5630_motor_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct ov5630_motor *info;
+	struct v4l2_subdev *sd;
+	int ret = -1;
+/* 	struct i2c_client *motor; */
+
+	DBG_entering;
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+		 client->addr << 1, client->adapter->name);
+	/*
+	 * Setup sensor configuration structure
+	 */
+	info = kzalloc(sizeof(struct ov5630_motor), GFP_KERNEL);
+	if (!info) {
+		eprintk("fail to malloc for ci_motor");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = ov5630_motor_detect(client);
+	if (ret) {
+		eprintk("error ov5630_motor_detect");
+		goto out_free;
+	}
+
+	sd = &info->sd;
+	v4l2_i2c_subdev_init(sd, client, &ov5630_motor_ops);
+
+	/*
+	 * Initialization OV5630
+	 * then turn into standby mode
+	 */
+	/* ret = ov5630_motor_standby(); */
+	ret = ov5630_motor_init(client, info);
+	if (ret) {
+		eprintk("error calling ov5630_motor_init");
+		goto out_free;
+	}
+
+	ret = 0;
+	goto out;
+
+out_free:
+	kfree(info);
+	DBG_leaving;
+out:
+	return ret;
+}
+
+/*
+ * XXX: Need to be checked
+ */
+static int ov5630_motor_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	DBG_entering;
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_motor_config(sd));
+
+	DBG_leaving;
+	return 0;
+}
+
+static const struct i2c_device_id ov5630_motor_id[] = {
+	{"ov5630_motor", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, ov5630_motor_id);
+
+static struct v4l2_i2c_driver_data v4l2_i2c_data = {
+	.name = "ov5630_motor",
+	.probe = ov5630_motor_probe,
+	.remove = ov5630_motor_remove,
+	/* .suspend = ov5630_suspend,
+	 * .resume = ov5630_resume, */
+	.id_table = ov5630_motor_id,
+};
+MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
+MODULE_DESCRIPTION("A low-level driver for OmniVision 5630 sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mrstci/mrstov5630_motor/ov5630_motor.h b/drivers/media/video/mrstci/mrstov5630_motor/ov5630_motor.h
new file mode 100644
index 0000000..302c218
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov5630_motor/ov5630_motor.h
@@ -0,0 +1,86 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#include <media/v4l2-subdev.h>
+
+/* VCM start current (mA) */
+#define MOTOR_INFIN_CUR		15
+/* VCM max current for Macro (mA) */
+#define MOTOR_MACRO_CUR		90
+/* DAC output max current (mA) */
+#define MOTOR_DAC_MAX_CUR	100
+/* DAC output min current (mA) */
+#define MOTOR_DAC_MIN_CUR	3
+
+#define MOTOR_DAC_BIT_RES	10
+#define MOTOR_DAC_MAX_CODE	((1 << MOTOR_DAC_BIT_RES) - 1)
+
+#define MOTOR_STEP_SHIFT	4
+
+#define MAX(x, y)	((x) > (y) ? (x) : (y))
+#define MIN(x, y)	((x) < (y) ? (x) : (y))
+
+/* DAC register related define */
+#define MOTOR_POWER_DOWN	(1 << 7)
+#define		PD_ENABLE	(1 << 7)
+#define		PD_DISABLE	(0)
+
+#define MOTOR_DAC_CODE_H(x)	((x >> 4) & 0x3f)
+#define MOTOR_DAC_CODE_L(x)	((x << 4) & 0xf0)
+
+#define MOTOR_DAC_CTRL_MODE_0	0x00
+#define MOTOR_DAC_CTRL_MODE_1(x)	(x & 0x07)
+#define MOTOR_DAC_CTRL_MODE_2(x)	((x & 0x07) | 0x08)
+
+#define SUB_MODE_1	0x01
+#define SUB_MODE_2	0x02
+#define SUB_MODE_3	0x03
+#define SUB_MODE_4	0x04
+#define SUB_MODE_5	0x05
+#define SUB_MODE_6	0x06
+#define SUB_MODE_7	0x07
+
+#define OV5630_MOTOR_ADDR	(0x18 >> 1)
+#define POWER_EN_PIN	7
+#define GPIO_AF_PD	95
+
+struct ov5630_motor{
+	unsigned int infin_cur;
+	unsigned int infin_code;
+	unsigned int macro_cur;
+	unsigned int macro_code;
+	unsigned int max_step;
+	unsigned int cur_code;
+	struct v4l2_subdev sd;
+};
+
+extern int ov5630_motor_init(struct i2c_client *client, struct ov5630_motor
+			     *config);
+extern int ov5630_motor_standby(void);
+extern int ov5630_motor_wakeup(void);
+extern int ov5630_motor_set_focus(struct i2c_client *c, int step,
+			   struct ov5630_motor *config);
+extern int ov5630_motor_get_focus(struct i2c_client *c, unsigned int *step,
+			   struct ov5630_motor *config);
+extern int ov5630_motor_max_step(struct i2c_client *c, unsigned int *max_code,
+			   struct ov5630_motor *config);
diff --git a/drivers/media/video/mrstci/mrstov9665/Kconfig b/drivers/media/video/mrstci/mrstov9665/Kconfig
new file mode 100644
index 0000000..ba9b692
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov9665/Kconfig
@@ -0,0 +1,9 @@
+config VIDEO_MRST_OV9665
+	tristate "Moorestown OV9665 SoC Sensor"
+	depends on I2C && VIDEO_MRST_ISP
+
+	---help---
+	  Say Y here if your platform support OV9665 SoC Sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mrstov9665.ko.
diff --git a/drivers/media/video/mrstci/mrstov9665/Makefile b/drivers/media/video/mrstci/mrstov9665/Makefile
new file mode 100644
index 0000000..871b6bf
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov9665/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VIDEO_MRST_OV9665)	 += mrstov9665.o
+
+EXTRA_CFLAGS	+=	-I$(src)/../include
diff --git a/drivers/media/video/mrstci/mrstov9665/mrstov9665.c b/drivers/media/video/mrstci/mrstov9665/mrstov9665.c
new file mode 100644
index 0000000..04e553a
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov9665/mrstov9665.c
@@ -0,0 +1,972 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-i2c-drv.h>
+
+#include "ci_sensor_common.h"
+#include "ov9665.h"
+
+static int mrstov9665_debug;
+module_param(mrstov9665_debug, int, 0644);
+MODULE_PARM_DESC(mrstov9665_debug, "Debug level (0-1)");
+
+#define dprintk(level, fmt, arg...) do {			\
+	if (mrstov9665_debug >= level) 					\
+		printk(KERN_DEBUG "mrstisp@%s: " fmt "\n", \
+		       __func__, ## arg); } \
+	while (0)
+
+#define eprintk(fmt, arg...)	\
+	printk(KERN_ERR "mrstisp@%s: line %d: " fmt "\n",	\
+	       __func__, __LINE__, ## arg);
+
+#define DBG_entering	dprintk(2, "entering");
+#define DBG_leaving	dprintk(2, "leaving");
+#define DBG_line	dprintk(2, " line: %d", __LINE__);
+
+static inline struct ci_sensor_config *to_sensor_config(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ci_sensor_config, sd);
+}
+
+static struct ov9665_format_struct {
+	__u8 *desc;
+	__u32 pixelformat;
+	struct regval_list *regs;
+} ov9665_formats[] = {
+	{
+		.desc		= "YUYV 4:2:2",
+		.pixelformat	= SENSOR_MODE_BT601,
+		.regs 		= NULL,
+	},
+};
+#define N_OV9665_FMTS ARRAY_SIZE(ov9665_formats)
+
+static struct ov9665_res_struct {
+	__u8 *desc;
+	int res;
+	int width;
+	int height;
+	/* FIXME: correct the fps values.. */
+	int fps;
+	bool used;
+	struct regval_list *regs;
+} ov9665_res[] = {
+	{
+		.desc		= "SXGA",
+		.res		= SENSOR_RES_SXGA,
+		.width		= 1280,
+		.height		= 1024,
+		.fps		= 15,
+		.used		= 0,
+		.regs		= ov9665_res_sxga,
+	},
+	{
+		.desc		= "VGA",
+		.res		= SENSOR_RES_VGA,
+		.width		= 640,
+		.height		= 480,
+		.fps		= 15,
+		.used		= 0,
+		.regs		= ov9665_res_vga,
+	},
+};
+#define N_RES (ARRAY_SIZE(ov9665_res))
+
+/*
+ * I2C Read & Write stuff
+ */
+static int ov9665_read(struct i2c_client *c, unsigned char reg,
+		       unsigned char *value)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(c, reg);
+	if (ret >= 0) {
+		*value = (unsigned char) ret;
+		ret = 0;
+	}
+	return ret;
+}
+
+static int ov9665_write(struct i2c_client *c, unsigned char reg,
+			unsigned char value)
+{
+	int ret = i2c_smbus_write_byte_data(c, reg, value);
+	if (reg == 0x12 && (value & 0x80))
+		msleep(2);  /* Wait for reset to run */
+	return ret;
+}
+
+/*
+ * Write a list of register settings; ff/ff stops the process.
+ */
+static int ov9665_write_array(struct i2c_client *c, struct regval_list *vals)
+{
+	struct regval_list *p;
+	u8 read_val = 0;
+	int err_num = 0;
+	int i = 0;
+	p = vals;
+	while (p->reg_num != 0xff) {
+		ov9665_write(c, p->reg_num, p->value);
+		ov9665_read(c, p->reg_num, &read_val);
+		if (read_val != p->value)
+			err_num++;
+		p++;
+		i++;
+	}
+
+	return 0;
+}
+
+static int ov9665_set_data_pin_in(struct i2c_client *client)
+{
+	int ret = 0;
+
+	ret += ov9665_write(client, 0xd5, 0x00);
+	ret += ov9665_write(client, 0xd6, 0x00);
+
+	return ret;
+}
+
+static int ov9665_set_data_pin_out(struct i2c_client *client)
+{
+	int ret = 0;
+
+	ret += ov9665_write(client, 0xd5, 0xff);
+	ret += ov9665_write(client, 0xd6, 0xff);
+
+	return ret;
+}
+/*
+ * Sensor specific helper function
+ */
+static int ov9665_standby(void)
+{
+	/* Pull the pin to high to hardware standby */
+	gpio_set_value(GPIO_STDBY_PIN, 1);
+	dprintk(1, "PM: standby called\n");
+	return 0;
+}
+
+static int ov9665_wakeup(void)
+{
+	/* Pull the pin to low*/
+	gpio_set_value(GPIO_STDBY_PIN, 0);
+	dprintk(1, "PM: wakeup called\n");
+	msleep(10);
+	return 0;
+}
+
+static int ov9665_s_power(struct v4l2_subdev *sd, u32 val)
+{
+	if (val == 1)
+		ov9665_standby();
+	if (val == 0)
+		ov9665_wakeup();
+	return 0;
+}
+
+static int ov9665_init(struct i2c_client *c)
+{
+	int ret;
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct ci_sensor_config *info = to_sensor_config(sd);
+	u8 reg = 0;
+
+	/* Fill the configuration structure */
+	/* Note this default configuration value */
+	info->mode = ov9665_formats[0].pixelformat;
+	info->res = ov9665_res[0].res;
+	info->type = SENSOR_TYPE_SOC;
+	info->bls = SENSOR_BLS_OFF;
+	info->gamma = SENSOR_GAMMA_ON;
+	info->cconv = SENSOR_CCONV_ON;
+	info->blc = SENSOR_BLC_AUTO;
+	info->agc = SENSOR_AGC_AUTO;
+	info->awb = SENSOR_AWB_AUTO;
+	info->aec = SENSOR_AEC_AUTO;
+	info->bus_width = SENSOR_BUSWIDTH_8BIT_ZZ;
+	info->ycseq = SENSOR_YCSEQ_YCBYCR;
+	info->conv422 = SENSOR_CONV422_COSITED;
+	info->bpat = SENSOR_BPAT_GRGRBGBG;
+	info->field_inv = SENSOR_FIELDINV_NOSWAP;
+	info->field_sel = SENSOR_FIELDSEL_BOTH;
+	info->hpol = SENSOR_HPOL_REFPOS;
+	info->vpol = SENSOR_VPOL_POS;
+	info->edge = SENSOR_EDGE_FALLING;
+	info->flicker_freq = SENSOR_FLICKER_100;
+	info->cie_profile = 0;
+	memcpy(info->name, "ov9665", 7);
+
+	ret = ov9665_write(c, 0x12, 0x80);
+	/* Set registers into default config value */
+	ret += ov9665_write_array(c, ov9665_def_reg);
+
+	ov9665_read(c, 0x09, &reg);
+	reg = reg | 0x10;
+	ov9665_write(c, 0x09, reg);
+	ov9665_set_data_pin_in(c);
+	ssleep(1);
+
+	return ret;
+}
+
+static int distance(struct ov9665_res_struct *res, u32 w, u32 h)
+{
+	int ret;
+	if (res->width < w || res->height < h)
+		return -1;
+
+	ret = ((res->width - w) + (res->height - h));
+	return ret;
+}
+static int ov9665_try_res(u32 *w, u32 *h)
+{
+	struct ov9665_res_struct *res_index, *p = NULL;
+	int dis, last_dis = ov9665_res->width + ov9665_res->height;
+
+	dprintk(1, "&&&&&  before %dx%d", *w, *h);
+	for (res_index = ov9665_res;
+	     res_index < ov9665_res + N_RES;
+	     res_index++) {
+		if ((res_index->width <= *w) && (res_index->height <= *h))
+			break;
+		dis = distance(res_index, *w, *h);
+		if (dis < last_dis) {
+			last_dis = dis;
+			p = res_index;
+		}
+	}
+	if ((res_index->width < *w) || (res_index->height < *h)) {
+		if (res_index != ov9665_res)
+			res_index--;
+	}
+
+	/*
+	if (p == NULL) {
+		p = ov2650_res;
+	}
+
+	if ((w != NULL) && (h != NULL)) {
+		*w = p->width;
+		*h = p->height;
+	}
+	*/
+	if (res_index == ov9665_res + N_RES)
+		res_index = ov9665_res + N_RES - 1;
+
+	*w = res_index->width;
+	*h = res_index->height;
+
+	dprintk(1, "&&&&&  after %dx%d", *w, *h);
+	return 0;
+}
+
+static struct ov9665_res_struct *ov9665_to_res(u32 w, u32 h)
+{
+	struct ov9665_res_struct *res_index;
+
+	for (res_index = ov9665_res;
+	     res_index < ov9665_res + N_RES;
+	     res_index++)
+		if ((res_index->width == w) && (res_index->height == h))
+			break;
+
+	if (res_index >= ov9665_res + N_RES)
+		res_index--;   /* Take the bigger one */
+
+	return res_index;
+}
+
+static int ov9665_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_format *fmt)
+{
+	DBG_entering;
+	return ov9665_try_res(&fmt->fmt.pix.width, &fmt->fmt.pix.height);
+	DBG_leaving;
+}
+
+static int ov9665_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_format *fmt)
+{
+	struct ci_sensor_config *info = to_sensor_config(sd);
+	unsigned short width, height;
+	int index;
+
+	ci_sensor_res2size(info->res, &width, &height);
+
+	/* Marked the current sensor res as being "used" */
+	for (index = 0; index < N_RES; index++) {
+		if ((width == ov9665_res[index].width) &&
+		    (height == ov9665_res[index].height)) {
+			ov9665_res[index].used = 1;
+			continue;
+		}
+		ov9665_res[index].used = 0;
+	}
+
+	fmt->fmt.pix.width = width;
+	fmt->fmt.pix.height = height;
+	return 0;
+}
+
+static int ov9665_set_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	struct ci_sensor_config *info = to_sensor_config(sd);
+	int ret = 0;
+	struct ov9665_res_struct *res_index;
+	u32 width, height;
+	int index;
+
+	DBG_entering;
+
+	width = fmt->fmt.pix.width;
+	height = fmt->fmt.pix.height;
+
+	ret = ov9665_try_res(&width, &height);
+	res_index = ov9665_to_res(width, height);
+
+	ov9665_wakeup();
+	/* if ((info->res != res_index->res) && (res_index->regs)) { */
+	if ( res_index->regs) {
+		ret = ov9665_write(c, 0x12, 0x80);
+		ret += ov9665_write_array(c, ov9665_def_reg);
+		ret += ov9665_write_array(c, res_index->regs);
+		/* Add delay here to get better image */
+
+		for (index = 0; index < N_RES; index++) {
+			if ((width == ov9665_res[index].width) &&
+			    (height == ov9665_res[index].height)) {
+				ov9665_res[index].used = 1;
+				continue;
+			}
+			ov9665_res[index].used = 0;
+		}
+
+		for (index = 0; index < N_RES; index++)
+			dprintk(2, "index = %d, used = %d\n", index,
+				ov9665_res[index].used);
+
+	}
+	info->res = res_index->res;
+
+	DBG_leaving;
+	return ret;
+}
+
+static int ov9665_q_hflip(struct v4l2_subdev *sd, __s32 *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	unsigned char v = 0;
+
+	ret = ov9665_read(client, 0x04, &v);
+	*value = ((v & 0x80) == 0x80);
+	return ret;
+}
+
+static int ov9665_t_hflip(struct v4l2_subdev *sd, int value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned char v = 0;
+	int ret;
+
+	value = value >= 1 ? 1 : 0;
+	ret = ov9665_read(client, 0x33, &v);
+	if (value)
+		v |= 0x08;
+	else
+		v &= ~0x08;
+	ret += ov9665_write(client, 0x33, v);
+
+	ret += ov9665_read(client, 0x04, &v);
+	if (value)
+		v |= 0x80;
+	else
+		v &= ~0x80;
+	ret += ov9665_write(client, 0x04, v);
+	msleep(10);  /* FIXME */
+	return ret;
+}
+
+static int ov9665_q_vflip(struct v4l2_subdev *sd, __s32 *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	unsigned char v = 0;
+
+	ret = ov9665_read(client, 0x04, &v);
+	*value = ((v & 0x40) == 0x40);
+	return ret;
+}
+
+static int ov9665_t_vflip(struct v4l2_subdev *sd, int value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned char v = 0;
+	int ret;
+
+	value = value >= 1 ? 1 : 0;
+	ret = ov9665_read(client, 0x04, &v);
+	if (value)
+		v |= 0x40;
+	else
+		v &= ~0x40;
+	ret += ov9665_write(client, 0x04, v);
+	msleep(10);  /* FIXME */
+	return ret;
+}
+
+static struct ov9665_control {
+	struct v4l2_queryctrl qc;
+	int (*query)(struct v4l2_subdev *sd, __s32 *value);
+	int (*tweak)(struct v4l2_subdev *sd, int value);
+} ov9665_controls[] = {
+	{
+		.qc = {
+			.id = V4L2_CID_VFLIP,
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.name = "Vertical flip",
+			.minimum = 0,
+			.maximum = 1,
+			.step = 1,
+			.default_value = 0,
+		},
+		.tweak = ov9665_t_vflip,
+		.query = ov9665_q_vflip,
+	},
+	{
+		.qc = {
+			.id = V4L2_CID_HFLIP,
+			.type = V4L2_CTRL_TYPE_BOOLEAN,
+			.name = "Horizontal mirror",
+			.minimum = 0,
+			.maximum = 1,
+			.step = 1,
+			.default_value = 0,
+		},
+		.tweak = ov9665_t_hflip,
+		.query = ov9665_q_hflip,
+	},
+};
+#define N_CONTROLS (ARRAY_SIZE(ov9665_controls))
+
+static struct ov9665_control *ov9665_find_control(__u32 id)
+{
+	int i;
+
+	for (i = 0; i < N_CONTROLS; i++)
+		if (ov9665_controls[i].qc.id == id)
+			return ov9665_controls + i;
+	return NULL;
+}
+
+static int ov9665_queryctrl(struct v4l2_subdev *sd,
+			    struct v4l2_queryctrl *qc)
+{
+	struct ov9665_control *ctrl = ov9665_find_control(qc->id);
+
+	if (ctrl == NULL)
+		return -EINVAL;
+	*qc = ctrl->qc;
+	return 0;
+}
+
+static int ov9665_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct ov9665_control *octrl = ov9665_find_control(ctrl->id);
+	int ret;
+
+	if (octrl == NULL)
+		return -EINVAL;
+	ret = octrl->query(sd, &ctrl->value);
+	if (ret >= 0)
+		return 0;
+	return ret;
+}
+
+static int ov9665_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct ov9665_control *octrl = ov9665_find_control(ctrl->id);
+	int ret;
+
+	if (octrl == NULL)
+		return -EINVAL;
+	ret =  octrl->tweak(sd, ctrl->value);
+	if (ret >= 0)
+		return 0;
+	return ret;
+}
+
+#if 0
+static int ov9665_get_caps(struct i2c_client *c, struct ci_sensor_caps *caps)
+{
+	if (caps == NULL)
+		return -EIO;
+
+	caps->bus_width = SENSOR_BUSWIDTH_8BIT_ZZ;
+	caps->mode	= SENSOR_MODE_BT601;
+	caps->field_inv = SENSOR_FIELDINV_NOSWAP;
+	caps->field_sel = SENSOR_FIELDSEL_BOTH;
+	caps->ycseq	= SENSOR_YCSEQ_YCBYCR;
+	caps->conv422	= SENSOR_CONV422_COSITED;
+	caps->bpat	= SENSOR_BPAT_GRGRBGBG;
+	caps->hpol	= SENSOR_HPOL_REFPOS;
+	caps->vpol	= SENSOR_VPOL_POS;
+	caps->edge	= SENSOR_EDGE_FALLING;
+	caps->bls	= SENSOR_BLS_OFF;
+	caps->gamma	= SENSOR_GAMMA_ON;
+	caps->cconv	= SENSOR_CCONV_ON;
+	caps->res	= SENSOR_RES_SXGA | SENSOR_RES_VGA;
+	caps->blc	= SENSOR_BLC_AUTO;
+	caps->agc	= SENSOR_AGC_AUTO;
+	caps->awb	= SENSOR_AWB_AUTO;
+	caps->aec	= SENSOR_AEC_AUTO;
+	caps->cie_profile	= 0;
+	caps->flicker_freq	= SENSOR_FLICKER_100 | SENSOR_FLICKER_120;
+	caps->type	= SENSOR_TYPE_SOC;
+	/* caps->name	= "ov9665"; */
+	strcpy(caps->name, "ov9665");
+
+	return 0;
+}
+
+static int ov9665_get_config(struct i2c_client *c,
+			     struct ci_sensor_config *config)
+{
+	struct ci_sensor_config *info = i2c_get_clientdata(c);
+
+	if (config == NULL) {
+		printk(KERN_WARNING "sensor_get_config: NULL pointer\n");
+		return -EIO;
+	}
+
+	memset(config, 0, sizeof(struct ci_sensor_config *));
+	memcpy(config, info, sizeof(struct ci_sensor_config));
+
+	return 0;
+}
+
+static int ov9665_setup(struct i2c_client *c,
+			const struct ci_sensor_config *config)
+{
+	int ret;
+	struct ov9665_res_struct *res_index;
+	struct ci_sensor_config *info = i2c_get_clientdata(c);
+	u16 width, high;
+
+	/* Soft reset camera first*/
+	ret = ov9665_write(c, 0x12, 0x80);
+
+	/* Set registers into default config value */
+	ret += ov9665_write_array(c, ov9665_def_reg);
+
+	/* set image resolution */
+	ci_sensor_res2size(config->res, &width, &high);
+	ret += ov9665_try_res(c, &width, &high);
+	res_index = ov9665_find_res(width, high);
+	if (res_index->regs)
+		ret += ov9665_write_array(c, res_index->regs);
+	if (!ret)
+		info->res = res_index->res;
+
+	/* Add some delay here to get a better image*/
+	ssleep(1);
+
+	return ret;
+}
+
+static int ov9665_set_data_pin_in(struct i2c_client *client)
+{
+	int ret = 0;
+
+	ret += ov9665_write(client, 0xd5, 0x00);
+	ret += ov9665_write(client, 0xd6, 0x00);
+
+	return ret;
+}
+
+static int ov9665_set_data_pin_out(struct i2c_client *client)
+{
+	int ret = 0;
+
+	ret += ov9665_write(client, 0xd5, 0xff);
+	ret += ov9665_write(client, 0xd6, 0xff);
+
+	return ret;
+}
+/*
+ * File operation functions
+ */
+static int ov9665_open(struct i2c_setting *c, void *priv)
+{
+	struct i2c_client *client = c->sensor_client;
+	int ret = 0;
+	u8 reg = 0;
+	/* Just wake up sensor */
+	if (ov9665_wakeup())
+		return -EIO;
+
+	ov9665_init(client);
+	ret = ov9665_read(client, 0x09, &reg);
+	reg = reg | 0x10;
+	ret += ov9665_write(client, 0x09, reg);
+
+	if (ov9665_set_data_pin_in(client))
+		return EIO;
+/*
+	if (ov9665_standby())
+		return EIO;
+*/
+	return ret;
+}
+
+static int ov9665_release(struct i2c_setting *c, void *priv)
+{
+	/* Just suspend the sensor */
+	if (ov9665_standby())
+		return EIO;
+	return 0;
+}
+
+static int ov9665_on(struct i2c_setting *c)
+{
+	struct i2c_client *client = c->sensor_client;
+	int ret = 0;
+	u8 reg = 0;
+
+	ret = ov9665_read(client, 0x09, &reg);
+	reg = reg & ~0x10;
+	ret = ov9665_write(client, 0x09, reg);
+
+	if (ov9665_set_data_pin_out(client))
+		return EIO;
+
+	return ret;
+}
+
+static int ov9665_off(struct i2c_setting *c)
+{
+	struct i2c_client *client = c->sensor_client;
+	int ret = 0;
+	u8 reg = 0;
+/*
+	ret = ov9665_read(client, 0x09, &reg);
+	reg = reg | 0x10;
+	ret += ov9665_write(client, 0x09, reg);
+*/
+	if (ov9665_set_data_pin_in(client))
+		return EIO;
+
+	return ret;
+}
+
+static struct sensor_device ov9665 = {
+	.name	= "OV9665",
+	.type	= SENSOR_TYPE_SOC,
+	.minor	= -1,
+	.open	= ov9665_open,
+	.release = ov9665_release,
+	.on = ov9665_on,
+	.off = ov9665_off,
+	.querycap = ov9665_get_caps,
+	.get_config = ov9665_get_config,
+	.set_config = ov9665_setup,
+	.enum_parm = ov9665_queryctrl,
+	.get_parm = ov9665_g_ctrl,
+	.set_parm = ov9665_s_ctrl,
+	.try_res = ov9665_try_res,
+	.set_res = ov9665_set_res,
+	.suspend = ov9665_standby,
+	.resume = ov9665_wakeup,
+	.get_ls_corr_config = NULL,
+	.set_awb = NULL,
+	.set_aec = NULL,
+	.set_blc = NULL,
+	/* TBC */
+};
+#endif
+
+static int ov9665_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 reg = 0;
+
+	DBG_entering;
+	if (enable) {
+		ov9665_read(client, 0x09, &reg);
+		reg = reg & ~0x10;
+		ov9665_write(client, 0x09, reg);
+		ov9665_set_data_pin_out(client);
+		ssleep(1);
+
+	} else {
+		ov9665_read(client, 0x09, &reg);
+		reg = reg | 0x10;
+		ov9665_write(client, 0x09, reg);
+		ov9665_set_data_pin_in(client);
+	}
+
+	DBG_leaving;
+	return 0;
+}
+
+static int ov9665_enum_framesizes(struct v4l2_subdev *sd,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	unsigned int index = fsize->index;
+
+	DBG_entering;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = ov9665_res[index].width;
+	fsize->discrete.height = ov9665_res[index].height;
+	fsize->reserved[0] = ov9665_res[index].used;
+
+	DBG_leaving;
+
+	return 0;
+}
+
+static int ov9665_enum_frameintervals(struct v4l2_subdev *sd,
+				      struct v4l2_frmivalenum *fival)
+{
+	unsigned int index = fival->index;
+
+	DBG_entering;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete.numerator = 1;
+	fival->discrete.denominator = ov9665_res[index].fps;
+
+	DBG_leaving;
+
+	return 0;
+}
+
+static int ov9665_g_chip_ident(struct v4l2_subdev *sd,
+		struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+#define V4L2_IDENT_OV9665 8246
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_OV9665, 0);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov9665_g_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned char val = 0;
+	int ret;
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	ret = ov9665_read(client, reg->reg & 0xffff, &val);
+	reg->val = val;
+	reg->size = 1;
+	return ret;
+}
+
+static int ov9665_s_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	ov9665_write(client, reg->reg & 0xffff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static const struct v4l2_subdev_video_ops ov9665_video_ops = {
+	.try_fmt = ov9665_try_fmt,
+	.s_fmt = ov9665_set_fmt,
+	.g_fmt = ov9665_get_fmt,
+	.s_stream = ov9665_s_stream,
+	.enum_framesizes = ov9665_enum_framesizes,
+	.enum_frameintervals = ov9665_enum_frameintervals,
+};
+
+static const struct v4l2_subdev_core_ops ov9665_core_ops = {
+	.g_chip_ident = ov9665_g_chip_ident,
+	.queryctrl = ov9665_queryctrl,
+	.g_ctrl = ov9665_g_ctrl,
+	.s_ctrl = ov9665_s_ctrl,
+	.s_gpio = ov9665_s_power,
+	/*.g_ext_ctrls = ov9665_g_ext_ctrls,*/
+	/*.s_ext_ctrls = ov9665_s_ext_ctrls,*/
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = ov9665_g_register,
+	.s_register = ov9665_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_ops ov9665_ops = {
+	.core = &ov9665_core_ops,
+	.video = &ov9665_video_ops,
+};
+/*
+ * Basic i2c stuff
+ */
+/*
+static unsigned short normal_i2c[] = {0x30, I2C_CLIENT_END};
+I2C_CLIENT_INSMOD;
+
+static struct i2c_driver ov9665_driver;
+*/
+static int ov9665_detect(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	int adap_id = i2c_adapter_id(adapter);
+	u8 config = 0;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	if (adap_id != 1)
+		return -ENODEV;
+
+	ov9665_wakeup();
+
+	ov9665_read(client, 0x0a, &config);
+	if (config != 0x96)
+		return -ENODEV;
+
+	ov9665_read(client, 0x0b, &config);
+	if (config != 0x63)
+		return -ENODEV;
+
+	return 0;
+}
+
+static int ov9665_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct ci_sensor_config *info;
+	struct v4l2_subdev *sd;
+	int ret = -1;
+
+	DBG_entering;
+	/*
+	 * Setup sensor configuration structure
+	 */
+	info = kzalloc(sizeof(struct ci_sensor_config), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	ret = ov9665_detect(client);
+	if (ret) {
+		kfree(info);
+		return -ENODEV;
+	}
+
+	sd = &info->sd;
+	v4l2_i2c_subdev_init(sd, client, &ov9665_ops);
+
+	/*
+	 * Initialization OV9665
+	 * then turn into standby mode
+	 */
+	/* ret = ov9665_standby(); */
+	ret = ov9665_init(client);
+	if (ret) {
+		eprintk("error init ov9665");
+		goto err_1;
+	}
+
+	ov9665_standby();
+	printk(KERN_INFO "Init ov9665 sensor success\n");
+	DBG_leaving;
+	return 0;
+
+err_1:
+	kfree(info);
+	return ret;
+}
+
+/*
+ * XXX: Need to be checked
+ */
+static int ov9665_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_sensor_config(sd));
+
+	return 0;
+}
+
+static const struct i2c_device_id ov9665_id[] = {
+	{"ov9665", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ov9665_id);
+
+static struct v4l2_i2c_driver_data v4l2_i2c_data = {
+	.name = "ov9665",
+	.probe = ov9665_probe,
+	.remove = ov9665_remove,
+	.id_table = ov9665_id,
+};
+
+MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
+MODULE_DESCRIPTION("A low-level driver for OmniVision 9665 sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mrstci/mrstov9665/ov9665.h b/drivers/media/video/mrstci/mrstov9665/ov9665.h
new file mode 100644
index 0000000..6fc9d12
--- /dev/null
+++ b/drivers/media/video/mrstci/mrstov9665/ov9665.h
@@ -0,0 +1,263 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#define I2C_OV9665	0x60
+/* Should add to kernel source */
+#define I2C_DRIVERID_OV9665	1047
+/* GPIO pin on Moorestown */
+#define GPIO_SCLK_25	44
+#define GPIO_STB_PIN	47
+#define GPIO_STDBY_PIN	48
+#define GPIO_RESET_PIN	50
+
+struct regval_list {
+	u8 reg_num;
+	u8 value;
+};
+
+/*
+ * Default register value
+ * 1280x1024 YUV
+ */
+static struct regval_list ov9665_def_reg[] = {
+	{0x3E, 0x80},
+	{0x12, 0x80},
+
+	{0xd5, 0xff},
+	{0xd6, 0x3f},
+
+	{0x3d, 0x3c},
+	{0x11, 0x81},
+	{0x2a, 0x00},
+	{0x2b, 0x00},
+
+	{0x3a, 0xf1},
+	{0x3b, 0x00},
+	{0x3c, 0x58},
+	{0x3e, 0x50},
+	{0x71, 0x00},
+
+	{0x15, 0x00},
+	{0x6a, 0x24},
+	{0x85, 0xe7},
+
+	{0x63, 0x01},
+
+	{0x17, 0x0c},
+	{0x18, 0x5c},
+	{0x19, 0x01},
+	{0x1a, 0x82},
+	{0x03, 0x03},
+	{0x2b, 0x00},
+
+	{0x36, 0xb4},
+	{0x65, 0x10},
+	{0x70, 0x02},
+	{0x71, 0x9f},
+	{0x64, 0x24},
+
+	{0x43, 0x00},
+	{0x5D, 0x55},
+	{0x5E, 0x57},
+	{0x5F, 0x21},
+
+	{0x24, 0x3e},
+	{0x25, 0x38},
+	{0x26, 0x72},
+
+	{0x14, 0x68},
+	{0x0C, 0x3a}, /* Auto detect for 50/60 */
+	{0x4F, 0x9E},
+	{0x50, 0x84},
+	{0x5A, 0x67},
+
+	{0x7d, 0x30},
+	{0x7e, 0x00},
+	{0x82, 0x03},
+	{0x7f, 0x00},
+	{0x83, 0x07},
+	{0x80, 0x03},
+	{0x81, 0x04},
+
+	{0x96, 0xf0},
+	{0x97, 0x00},
+	{0x92, 0x33},
+	{0x94, 0x5a},
+	{0x93, 0x3a},
+	{0x95, 0x48},
+	{0x91, 0xfc},
+	{0x90, 0xff},
+	{0x8e, 0x4e},
+	{0x8f, 0x4e},
+	{0x8d, 0x13},
+	{0x8c, 0x0c},
+	{0x8b, 0x0c},
+	{0x86, 0x9e},
+	{0x87, 0x11},
+	{0x88, 0x22},
+	{0x89, 0x05},
+	{0x8a, 0x03},
+
+	{0x9b, 0x0e},
+	{0x9c, 0x1c},
+	{0x9d, 0x34},
+	{0x9e, 0x5a},
+	{0x9f, 0x68},
+	{0xa0, 0x76},
+	{0xa1, 0x82},
+	{0xa2, 0x8e},
+	{0xa3, 0x98},
+	{0xa4, 0xa0},
+	{0xa5, 0xb0},
+	{0xa6, 0xbe},
+	{0xa7, 0xd2},
+	{0xa8, 0xe2},
+	{0xa9, 0xee},
+	{0xaa, 0x18},
+
+	{0xAB, 0xe7},
+	{0xb0, 0x43},
+	{0xac, 0x04},
+	{0x84, 0x40},
+
+	{0xad, 0x84},
+	{0xd9, 0x24},
+	{0xda, 0x00},
+	{0xae, 0x10},
+
+	{0xab, 0xe7},
+	{0xb9, 0xa0},
+	{0xba, 0x80},
+	{0xbb, 0xa0},
+	{0xbc, 0x80},
+
+	{0xbd, 0x08},
+	{0xbe, 0x19},
+	{0xbf, 0x02},
+	{0xc0, 0x08},
+	{0xc1, 0x2a},
+	{0xc2, 0x34},
+	{0xc3, 0x2d},
+	{0xc4, 0x2d},
+	{0xc5, 0x00},
+	{0xc6, 0x98},
+	{0xc7, 0x18},
+	{0x69, 0x48},
+
+	{0x74, 0xc0},
+
+	{0x7c, 0x18},
+	{0x65, 0x11},
+	{0x66, 0x00},
+	{0x41, 0xa0},
+	{0x5b, 0x28},
+	{0x60, 0x84},
+	{0x05, 0x07},
+	{0x03, 0x03},
+	{0xd2, 0x8c},
+
+	{0xc7, 0x90},
+	{0xc8, 0x06},
+	{0xcb, 0x40},
+	{0xcc, 0x40},
+	{0xcf, 0x00},
+	{0xd0, 0x20},
+	{0xd1, 0x00},
+	{0xc7, 0x18},
+
+	{0x0d, 0x82},
+	{0x0d, 0x80},
+
+	{0x09, 0x01},
+
+	{0xff, 0xff},
+};
+
+/* 1280x1024 */
+static struct regval_list ov9665_res_sxga[] = {
+	{0x0c, 0xbc}, /* note this */
+	{0xff, 0xff},
+};
+
+/* 640x480 */
+static struct regval_list ov9665_res_vga[] = {
+	/* Fclk/4 */
+	{0x11, 0x80},
+	{0x63, 0x00},
+
+	{0x12, 0x40}, /*VGA format*/
+	{0x14, 0x30}, /*4x*/
+	{0x0c, 0xbc},
+	{0x4d, 0x09},
+	{0x5c, 0x80}, /* Full average AEC */
+
+	/* Windows setting */
+	{0x17, 0x0c},
+	{0x18, 0x5c},
+	{0x19, 0x02},
+	{0x1a, 0x3f},
+	{0x03, 0x03},
+	{0x32, 0xad},
+
+	/* 50/60Hz AEC */
+	{0x5a, 0x23},
+	{0x2b, 0x00},
+
+	{0x64, 0xa4},
+	/*
+	{0x4F, 0x4f},
+	{0x50, 0x42},
+	*/
+	{0x4F, 0x9e},
+	{0x50, 0x84},
+	{0x97, 0x0a},
+	{0xad, 0x82},
+	{0xd9, 0x11},
+
+	/* Scale window */
+	{0xb9, 0x50},
+	{0xba, 0x3c},
+	{0xbb, 0x50},
+	{0xbc, 0x3c},
+
+	{0xad, 0x80},
+	{0xd9, 0x00},
+	{0xac, 0x0f},
+	{0x84, 0x86},
+
+	/*This is for Color Matrix*/
+	{0xbd, 0x05},
+	{0xbe, 0x16},
+	{0xbf, 0x05},
+	{0xc0, 0x07},
+	{0xc1, 0x18},
+	{0xc2, 0x1f},
+	{0xc3, 0x2b},
+	{0xc4, 0x2b},
+	{0xc5, 0x00},
+
+	{0x0d, 0x92},
+	{0x0d, 0x90},
+
+	{0xff, 0xff},
+};
diff --git a/drivers/media/video/mrstci/mrsts5k4e1/Kconfig b/drivers/media/video/mrstci/mrsts5k4e1/Kconfig
new file mode 100755
index 0000000..7dee787
--- /dev/null
+++ b/drivers/media/video/mrstci/mrsts5k4e1/Kconfig
@@ -0,0 +1,9 @@
+config VIDEO_MRST_S5K4E1
+	tristate "Moorestown s5k4e1 RAW Sensor"
+	depends on I2C && VIDEO_MRST_ISP
+
+	---help---
+	  Say Y here if your platform support s5k4e1 RAW Sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mrstov2650.ko.
diff --git a/drivers/media/video/mrstci/mrsts5k4e1/Makefile b/drivers/media/video/mrstci/mrsts5k4e1/Makefile
new file mode 100644
index 0000000..8733fa8
--- /dev/null
+++ b/drivers/media/video/mrstci/mrsts5k4e1/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VIDEO_MRST_S5K4E1)	 += mrsts5k4e1.o
+
+EXTRA_CFLAGS	+=	-I$(src)/../include
diff --git a/drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.c b/drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.c
new file mode 100755
index 0000000..f644531
--- /dev/null
+++ b/drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.c
@@ -0,0 +1,1024 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-i2c-drv.h>
+
+#include "ci_sensor_common.h"
+#include "mrsts5k4e1.h"
+/* #include "priv.h" */
+/* extern const struct DumpRegs regs_d[]; */
+
+static int s5k4e1_debug;
+module_param(s5k4e1_debug, int, 0644);
+MODULE_PARM_DESC(s5k4e1_debug, "Debug level (0-1)");
+
+#define dprintk(level, fmt, arg...) \
+	do { \
+		if (s5k4e1_debug >= level) \
+			printk(KERN_DEBUG "mrstisp@%s: " fmt "\n", \
+			       __func__, ## arg);\
+	} while (0)
+
+#define eprintk(fmt, arg...)	\
+	printk(KERN_ERR "mrstisp@%s:" fmt "\n",	\
+	       __func__, ## arg);
+
+#define DBG_entering	dprintk(1, "entering");
+#define DBG_leaving	dprintk(1, "leaving");
+#define DBG_line	dprintk(1, " line: %d", __LINE__);
+
+static inline struct ci_sensor_config *to_sensor_config(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ci_sensor_config, sd);
+}
+
+static struct s5k4e1_format_struct {
+	__u8 *desc;
+	__u32 pixelformat;
+	struct regval_list *regs;
+} s5k4e1_formats[] = {
+	{
+		.desc		= "Raw RGB Bayer",
+		.pixelformat	= SENSOR_MODE_MIPI,
+		.regs 		= NULL,
+	},
+};
+#define N_S5K4E1_FMTS ARRAY_SIZE(s5k4e1_formats)
+
+static struct s5k4e1_res_struct {
+	__u8 *desc;
+	int res;
+	int width;
+	int height;
+	/* FIXME: correct the fps values.. */
+	int fps;
+	bool used;
+	struct regval_list *regs;
+} s5k4e1_res[] = {
+	{
+		.desc		= "QSXGA_PLUS4",
+		.res		= SENSOR_RES_QXGA_PLUS,
+		.width		= 2592,
+		.height		= 1944,
+		.fps		= 15,
+		.used		= 0,
+		.regs		= s5k4e1_res_qsxga_plus4,
+	},
+	{
+		.desc		= "1080P",
+		.res		= SENSOR_RES_1080P,
+		.width		= 1920,
+		.height		= 1080,
+		.fps		= 25,
+		.used		= 0,
+		.regs		= s5k4e1_res_1080p,
+	},
+	{
+		.desc 		= "VGA_PLUS",
+		.res		= SENSOR_RES_VGA_PLUS,
+		.width		= 1304,
+		.height		= 980,
+		.fps		= 30,
+		.used		= 0,
+		.regs		= s5k4e1_res_vga_ac04_bill,
+	},
+	{
+		.desc		= "720p",
+		.res		= SENSOR_RES_720P,
+		.width		= 1280,
+		.height		= 720,
+		.fps		= 30,
+		.used		= 0,
+		.regs		= s5k4e1_res_720p,
+	},
+	{
+		.desc 		= "VGA",
+		.res		= SENSOR_RES_VGA,
+		.width		= 640,
+		.height		= 480,
+		.used		= 0,
+		.fps		= 40,
+		.regs		= s5k4e1_res_vga_ac04_bill,
+	},
+};
+
+#define N_RES (ARRAY_SIZE(s5k4e1_res))
+
+/*
+ * I2C Read & Write stuff
+ */
+static int s5k4e1_read(struct i2c_client *c, u32 reg, u32 *value)
+{
+	int ret;
+	int i;
+	struct i2c_msg msg[2];
+	u8 msgbuf[2];
+	u8 ret_val = 0;
+	*value = 0;
+	/* Read needs two message to go */
+	memset(&msg, 0, sizeof(msg));
+	msgbuf[0] = 0;
+	msgbuf[1] = 0;
+	i = 0;
+
+	msgbuf[i++] = ((u16)reg) >> 8;
+	msgbuf[i++] = ((u16)reg) & 0xff;
+	msg[0].addr = c->addr;
+	msg[0].buf = msgbuf;
+	msg[0].len = i;
+
+	msg[1].addr = c->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = &ret_val;
+	msg[1].len = 1;
+
+	ret = i2c_transfer(c->adapter, &msg[0], 2);
+	*value = ret_val;
+
+	ret = (ret == 2) ? 0 : -1;
+	dprintk(2, "reg:0x%8x, value:0x%8x - %s", reg, *value,
+		(ret ? "failed" : "succesfully"));
+	return ret;
+}
+
+static int s5k4e1_write(struct i2c_client *c, u32 reg, u32 value)
+{
+	int ret, i;
+	struct i2c_msg msg;
+	u8 msgbuf[3];
+
+	/* Writing only needs one message */
+	memset(&msg, 0, sizeof(msg));
+	i = 0;
+	msgbuf[i++] = ((u16)reg) >> 8;
+	msgbuf[i++] = (u16)reg & 0xff;
+	msgbuf[i++] = (u8)value;
+
+	msg.addr = c->addr;
+	msg.flags = 0;
+	msg.buf = msgbuf;
+	msg.len = i;
+
+	ret = i2c_transfer(c->adapter, &msg, 1);
+
+	/* If this is a reset register, wait for 1ms */
+	if (reg == 0x0103 && (value & 0x01))
+		/*Note here, check if this is needed */
+		msleep(4);
+
+	ret = (ret == 1) ? 0 : -1;
+	dprintk(2, "reg:0x%8x, value:0x%8x - %s", reg, value,
+		(ret ? "failed" : "successfully"));
+	return ret;
+}
+
+static int s5k4e1_write_array(struct i2c_client *c, struct regval_list *vals)
+{
+	struct regval_list *p;
+	u32 read_val = 0;
+	int err_num = 0;
+	int i = 0;
+
+	DBG_entering;
+
+	p = vals;
+	while (p->reg_num != 0xffff) {
+		s5k4e1_write(c, (u32)p->reg_num, (u32)p->value);
+		s5k4e1_read(c, (u32)p->reg_num, &read_val);
+		/* msleep(100);*/
+		if (read_val != p->value) {
+			eprintk("0x%x write error:should be 0x%x, but 0x%x",
+				p->reg_num, p->value, read_val);
+			err_num++;
+		}
+		p++;
+		i++;
+	}
+	dprintk(1, "sucessfully wrote %d registers, err is %d", i,
+	       err_num);
+	return 0;
+}
+
+/*
+ * Sensor specific helper function
+ */
+static int s5k4e1_standby(void)
+{
+	gpio_set_value(GPIO_STDBY_PIN, 1);
+	dprintk(1, "PM: standby called\n");
+	return 0;
+}
+
+static int s5k4e1_wakeup(void)
+{
+	gpio_set_value(GPIO_STDBY_PIN, 0);
+	dprintk(1, "PM: wakeup called\n");
+	return 0;
+}
+
+static int s5k4e1_s_power(struct v4l2_subdev *sd, u32 val)
+{
+	if (val == 1)
+		s5k4e1_standby();
+	if (val == 0)
+		s5k4e1_wakeup();
+	return 0;
+}
+
+static int s5k4e1_set_img_ctrl(struct i2c_client *c,
+			       const struct ci_sensor_config *config)
+{
+	int err = 0;
+
+	DBG_entering;
+
+	switch (config->blc) {
+		/* only SENSOR_BLC_AUTO supported */
+	case SENSOR_BLC_AUTO:
+		break;
+	default:
+		dprintk(1, "BLC not supported,\
+			set to BLC_AUTO by default.");
+	}
+
+	switch (config->bls) {
+		/* only SENSOR_BLS_OFF supported */
+	case SENSOR_BLS_OFF:
+		break;
+	default:
+		dprintk(1, "Black level not supported,\
+			set to BLS_OFF by default.");
+	}
+
+	switch (config->agc) {
+		/* only SENSOR_AGC_OFF supported */
+	case SENSOR_AGC_OFF:
+		break;
+	default:
+		dprintk(1, "AGC not supported,\
+			set to AGC_OFF by default.");
+	}
+
+	switch (config->awb) {
+		/* only SENSOR_AWB_OFF supported */
+	case SENSOR_AWB_OFF:
+		break;
+	default:
+		dprintk(1, "AWB not supported,\
+			set to AWB_OFF by default.");
+	}
+
+	switch (config->aec) {
+		/* only SENSOR_AEC_OFF supported */
+	case SENSOR_AEC_OFF:
+		break;
+	default:
+		dprintk(1, "AEC not supported,\
+			set to AEC_OFF by default.");
+	}
+
+	DBG_leaving;
+
+	return err;
+}
+static int s5k4e1_init(struct i2c_client *c)
+{
+	int ret = 0;
+	struct v4l2_subdev *sd = i2c_get_clientdata(c);
+	struct ci_sensor_config *info = to_sensor_config(sd);
+	char *name = "";
+
+	DBG_entering;
+
+	/* Fill the configuration structure */
+	/* Note this default configuration value */
+	info->mode = s5k4e1_formats[0].pixelformat;
+	info->res = s5k4e1_res[0].res;
+	info->type = SENSOR_TYPE_RAW;
+	info->bls = SENSOR_BLS_OFF;
+	info->gamma = SENSOR_GAMMA_OFF;
+	info->cconv = SENSOR_CCONV_OFF;
+	info->blc = SENSOR_BLC_AUTO;
+	info->agc = SENSOR_AGC_OFF;
+	info->awb = SENSOR_AWB_OFF;
+	info->aec = SENSOR_AEC_OFF;
+	/*info->bus_width = SENSOR_BUSWIDTH_10BIT_ZZ;*/
+	info->bus_width = SENSOR_BUSWIDTH_12BIT;
+	info->ycseq = SENSOR_YCSEQ_YCBYCR;
+	info->conv422 = SENSOR_CONV422_COSITED;
+	/*info->conv422 = SENSOR_CONV422_NOCOSITED;*/
+	info->bpat = SENSOR_BPAT_GRGRBGBG;
+	info->field_inv = SENSOR_FIELDINV_NOSWAP;
+	info->field_sel = SENSOR_FIELDSEL_BOTH;
+	info->hpol = SENSOR_HPOL_REFPOS;
+	info->vpol = SENSOR_VPOL_NEG;
+	info->edge = SENSOR_EDGE_RISING;
+	info->flicker_freq = SENSOR_FLICKER_100;
+	info->cie_profile = SENSOR_CIEPROF_F11;
+	info->mipi_mode = SENSOR_MIPI_MODE_RAW_10;
+	name = "s5k4e1";
+	memcpy(info->name, name, 7);
+
+	/* Reset sensor hardware, and implement the setting*/
+	ret += s5k4e1_write(c, 0x0100, (u32)0x00);
+	/*TODO: See if we can ignore this*/
+	ret = s5k4e1_write(c, 0x0103, (u32)0x01);
+
+	/* sw reset -- delay 3.1ms */
+	msleep(4);
+
+	/* Set registers into default config value */
+	/* ret += s5k4e1_write_array(c, s5k4e1_def_reg); */
+
+	/* Set MIPI interface */
+#ifdef S5K4E1_MIPI
+	ret += s5k4e1_write_array(c, s5k4e1_mipi);
+#endif
+
+	ret += s5k4e1_set_img_ctrl(c, info); /*FIXME*/
+
+	/* streaming */
+	/* ret += s5k4e1_write(c, 0x0100, (u32)0x01); */
+	ret += s5k4e1_write(c, 0x0100, (u32)0x00);
+
+	msleep(1);
+
+	DBG_leaving;
+
+	return ret;
+}
+
+static int distance(struct s5k4e1_res_struct *res, u32 w, u32 h)
+{
+	int ret;
+
+	DBG_entering;
+
+	if (res->width < w || res->height < h)
+		return -1;
+
+	ret = ((res->width - w) + (res->height - h));
+
+	DBG_leaving;
+
+	return ret;
+}
+
+static int s5k4e1_try_res(u32 *w, u32 *h)
+{
+	struct s5k4e1_res_struct *res_index, *p = NULL;
+	int dis, last_dis = s5k4e1_res->width + s5k4e1_res->height;
+
+	DBG_entering;
+
+	for (res_index = s5k4e1_res;
+	     res_index < s5k4e1_res + N_RES;
+	     res_index++) {
+		if ((res_index->width < *w) || (res_index->height < *h))
+			break;
+		dis = distance(res_index, *w, *h);
+		if (dis < last_dis) {
+			last_dis = dis;
+			p = res_index;
+		}
+	}
+
+	if (p == NULL)
+		p = s5k4e1_res;
+	else if ((p->width < *w) || (p->height < *h)) {
+		if (p != s5k4e1_res)
+			p--;
+	}
+
+	if ((w != NULL) && (h != NULL)) {
+		*w = p->width;
+		*h = p->height;
+	}
+
+	DBG_leaving;
+	return 0;
+}
+
+static struct s5k4e1_res_struct *s5k4e1_to_res(u32 w, u32 h)
+{
+	struct s5k4e1_res_struct *res_index;
+
+	DBG_entering;
+
+	for (res_index = s5k4e1_res;
+	     res_index < s5k4e1_res + N_RES;
+	     res_index++)
+		if ((res_index->width == w) && (res_index->height == h))
+			break;
+
+	if (res_index >= s5k4e1_res + N_RES)
+		res_index--;   /* Take the bigger one */
+
+	DBG_leaving;
+
+	return res_index;
+}
+
+static int s5k4e1_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_format *fmt)
+{
+	DBG_entering;
+	return s5k4e1_try_res(&fmt->fmt.pix.width, &fmt->fmt.pix.height);
+	DBG_leaving;
+}
+
+static int s5k4e1_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_format *fmt)
+{
+	struct ci_sensor_config *info = to_sensor_config(sd);
+	unsigned short width, height;
+	int index;
+
+	ci_sensor_res2size(info->res, &width, &height);
+
+	/* Marked the current sensor res as being "used" */
+	for (index = 0; index < N_RES; index++) {
+		if ((width == s5k4e1_res[index].width) &&
+		    (height == s5k4e1_res[index].height)) {
+			s5k4e1_res[index].used = 1;
+			continue;
+		}
+		s5k4e1_res[index].used = 0;
+	}
+
+	fmt->fmt.pix.width = width;
+	fmt->fmt.pix.height = height;
+	return 0;
+
+}
+
+#if 0
+/* chuanxiao add, to dump regs */
+static int s5k4e1_dump_regs(struct i2c_client *c)
+{
+	/*struct i2c_client *c = v4l2_get_subdevdata(sd);*/
+	const struct DumpRegs *p = regs_d;
+	u32 value;
+	u32 value1, value2, value3, value4;
+	while (p->ulFlags != eTableEnd) {
+		if (p->ulFlags & eFourBytes) {
+			s5k4e1_read(c, (u32)p->ulAddr, &value1);
+			s5k4e1_read(c, (u32)p->ulAddr+1, &value2);
+			s5k4e1_read(c, (u32)p->ulAddr+2, &value3);
+			s5k4e1_read(c, (u32)p->ulAddr+3, &value4);
+			value = value1<<24 | value2<<16 | value3<<8 | value4;
+		} else if (p->ulFlags & eTwoBytes) {
+			s5k4e1_read(c, (u32)p->ulAddr, &value1);
+			s5k4e1_read(c, (u32)p->ulAddr+1, &value2);
+			value = value1<<8 | value2;
+		} else
+			s5k4e1_read(c, (u32)p->ulAddr, &value);
+		/*
+		if (value == p->ulDefaultValue)
+			dprintk(0, "%s\t @ 0x%x = 0x%lx (= default value)\n",
+			p->pszName, p->ulAddr, value);
+		else
+			dprintk(0, "%s\t @ 0x%x = 0x%lx (default was 0x%lx)\n",
+			p->pszName, p->ulAddr, value, p->ulDefaultValue);
+		*/
+		dprintk(0, "%-30s @ 0x%04X = 0x%08X", p->pszName,
+			p->ulAddr, value);
+		p++;
+	}
+	return 0;
+}
+#endif
+
+static int s5k4e1_set_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	struct ci_sensor_config *info = to_sensor_config(sd);
+	int ret = 0;
+	struct s5k4e1_res_struct *res_index;
+	u32 width, height;
+	int index;
+
+	DBG_entering;
+
+	width = fmt->fmt.pix.width;
+	height = fmt->fmt.pix.height;
+
+	dprintk(1, "was told to set fmt (%d x %d) ", width, height);
+	ret = s5k4e1_try_res(&width, &height);
+
+	res_index = s5k4e1_to_res(width, height);
+
+	s5k4e1_wakeup();
+	DBG_line;
+	if (res_index->regs) {
+		/* software sleep/standby */
+		ret += s5k4e1_write(c, 0x0100, (u32)0x00);
+
+		/* Soft reset camera first*/
+		/*TODO: See if we can ignore this*/
+		ret = s5k4e1_write(c, 0x0103, (u32)0xff);
+
+		/* Set registers into default config value */
+		/* ret += s5k4e1_write_array(c, s5k4e1_def_reg);*/
+
+		/* set image resolution */
+		ret += s5k4e1_write_array(c, res_index->regs);
+
+		ret += s5k4e1_set_img_ctrl(c, info);
+
+		/* XXX setup with unknow meaning ... */
+		/* ret += s5k4e1_write(c, 0x30b0, 0xfe); */
+
+		/* Set MIPI interface */
+#ifdef S5K4E1_MIPI
+		ret += s5k4e1_write_array(c, s5k4e1_mipi);
+#endif
+
+		/* streaming */
+		ret = s5k4e1_write(c, 0x0100, (u32)0x01);
+		msleep(1);
+
+		info->res = res_index->res;
+
+		/* Marked current sensor res as being "used" */
+		for (index = 0; index < N_RES; index++) {
+			if ((width == s5k4e1_res[index].width) &&
+			    (height == s5k4e1_res[index].height)) {
+				s5k4e1_res[index].used = 1;
+				continue;
+			}
+			s5k4e1_res[index].used = 0;
+		}
+
+		for (index = 0; index < N_RES; index++)
+			dprintk(2, "index = %d, used = %d\n", index,
+				s5k4e1_res[index].used);
+
+		DBG_line;
+	} else {
+		eprintk("no res for (%d x %d)", width, height);
+	}
+
+	DBG_leaving;
+	return ret;
+}
+
+static int s5k4e1_t_gain(struct v4l2_subdev *sd, int value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	DBG_entering;
+
+	s5k4e1_write(client, 0x0104, 1); /*hold*/
+
+	/* analog gain */
+	s5k4e1_write(client, 0x0204, value >> 8);
+
+	s5k4e1_write(client, 0x0205, value & 0xff);
+
+	s5k4e1_write(client, 0x0104, 0); /*unhold*/
+
+	dprintk(1, "gain %x was writen to 0x0204/5", value);
+
+	DBG_leaving;
+	return 0;
+}
+
+static int s5k4e1_t_exposure(struct v4l2_subdev *sd, int value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	DBG_entering;
+
+	s5k4e1_write(client, 0x0104, 1); /*hold*/
+
+	/* fine integration time */
+	s5k4e1_write(client, 0x0200, value >> 24);
+
+	s5k4e1_write(client, 0x0201, (value >> 16) & 0xff);
+
+	/* coarse integration time */
+	s5k4e1_write(client, 0x0202, (value & 0xff00) >> 8);
+
+	s5k4e1_write(client, 0x0203, value & 0xff);
+
+	s5k4e1_write(client, 0x0104, 0); /*unhold*/
+
+	dprintk(1, "exposure %x was writen to 0x0200/1/2/3", value);
+
+	DBG_leaving;
+	return 0;
+}
+
+static struct s5k4e1_control {
+	struct v4l2_queryctrl qc;
+	int (*query)(struct v4l2_subdev *sd, __s32 *value);
+	int (*tweak)(struct v4l2_subdev *sd, int value);
+} s5k4e1_controls[] = {
+	{
+		.qc = {
+			.id = V4L2_CID_GAIN,
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.name = "global gain",
+			.minimum = 0x0,
+			.maximum = 0xFFFF,
+			.step = 0x01,
+			.default_value = 0x00,
+			.flags = 0,
+		},
+		.tweak = s5k4e1_t_gain,
+	},
+	{
+		.qc = {
+			.id = V4L2_CID_EXPOSURE,
+			.type = V4L2_CTRL_TYPE_INTEGER,
+			.name = "exposure",
+			.minimum = 0x0,
+			.maximum = 0xFFFF,
+			.step = 0x01,
+			.default_value = 0x00,
+			.flags = 0,
+		},
+		.tweak = s5k4e1_t_exposure,
+	},
+};
+#define N_CONTROLS (ARRAY_SIZE(s5k4e1_controls))
+
+static struct s5k4e1_control *s5k4e1_find_control(__u32 id)
+{
+	int i;
+
+	DBG_entering;
+	for (i = 0; i < N_CONTROLS; i++)
+		if (s5k4e1_controls[i].qc.id == id)
+			return s5k4e1_controls + i;
+	DBG_leaving;
+	return NULL;
+}
+
+static int s5k4e1_queryctrl(struct v4l2_subdev *sd,
+			    struct v4l2_queryctrl *qc)
+{
+	struct s5k4e1_control *ctrl = s5k4e1_find_control(qc->id);
+
+	DBG_entering;
+	if (ctrl == NULL)
+		return -EINVAL;
+	*qc = ctrl->qc;
+
+	DBG_leaving;
+	return 0;
+}
+
+static int s5k4e1_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+/*
+	struct s5k4e1_control *octrl = s5k4e1_find_control(parm->index);
+	int ret;
+
+	if (octrl == NULL)
+		return -EINVAL;
+	ret = octrl->query(client, &parm->value);
+	if (ret >= 0)
+		return 0;
+*/
+	return 0;
+}
+
+static int s5k4e1_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct s5k4e1_control *octrl = s5k4e1_find_control(ctrl->id);
+	int ret;
+
+	DBG_entering;
+
+	if (octrl == NULL)
+		return -EINVAL;
+	ret =  octrl->tweak(sd, ctrl->value);
+	if (ret >= 0)
+		return 0;
+
+	DBG_leaving;
+	return ret;
+}
+
+static int s5k4e1_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	DBG_entering;
+
+	if (enable) {
+		s5k4e1_write(client, (u32)0x0100, 0x01);
+		/*chuanxiao add, dump s5k4e1 regs*/
+		/* s5k4e1_dump_regs(client); */
+	} else
+		s5k4e1_write(client, (u32)0x0100, 0x00);
+
+	/*msleep(1);*/
+
+	DBG_leaving;
+	return 0;
+}
+
+static int s5k4e1_enum_framesizes(struct v4l2_subdev *sd,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	unsigned int index = fsize->index;
+
+	DBG_entering;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = s5k4e1_res[index].width;
+	fsize->discrete.height = s5k4e1_res[index].height;
+	fsize->reserved[0] = s5k4e1_res[index].used;
+
+	DBG_leaving;
+
+	return 0;
+}
+
+static int s5k4e1_enum_frameintervals(struct v4l2_subdev *sd,
+				      struct v4l2_frmivalenum *fival)
+{
+	unsigned int index = fival->index;
+
+	DBG_entering;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete.numerator = 1;
+	fival->discrete.denominator = s5k4e1_res[index].fps;
+
+	DBG_leaving;
+
+	return 0;
+}
+
+static int s5k4e1_g_chip_ident(struct v4l2_subdev *sd,
+		struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	DBG_entering;
+
+#define V4L2_IDENT_S5K4E1 8250
+	DBG_leaving;
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_S5K4E1, 0);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int s5k4e1_g_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned char val = 0;
+	int ret;
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	ret = s5k4e1_read(client, reg->reg & 0xffff, &val);
+	reg->val = val;
+	reg->size = 1;
+	return ret;
+}
+
+static int s5k4e1_s_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!v4l2_chip_match_i2c_client(client, &reg->match))
+		return -EINVAL;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	s5k4e1_write(client, reg->reg & 0xffff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+static const struct v4l2_subdev_video_ops s5k4e1_video_ops = {
+	.try_fmt = s5k4e1_try_fmt,
+	.s_fmt = s5k4e1_set_fmt,
+	.g_fmt = s5k4e1_get_fmt,
+	.s_stream = s5k4e1_s_stream,
+	.enum_framesizes = s5k4e1_enum_framesizes,
+	.enum_frameintervals = s5k4e1_enum_frameintervals,
+};
+
+static const struct v4l2_subdev_core_ops s5k4e1_core_ops = {
+	.g_chip_ident = s5k4e1_g_chip_ident,
+	.queryctrl = s5k4e1_queryctrl,
+	.g_ctrl = s5k4e1_g_ctrl,
+	.s_ctrl = s5k4e1_s_ctrl,
+	.s_gpio = s5k4e1_s_power,
+	/*.g_ext_ctrls = s5k4e1_g_ext_ctrls,*/
+	/*.s_ext_ctrls = s5k4e1_s_ext_ctrls,*/
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = s5k4e1_g_register,
+	.s_register = s5k4e1_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_ops s5k4e1_ops = {
+	.core = &s5k4e1_core_ops,
+	.video = &s5k4e1_video_ops,
+};
+
+/*
+ * Basic i2c stuff
+ */
+/*
+static unsigned short normal_i2c[] = {0x36, I2C_CLIENT_END};
+I2C_CLIENT_INSMOD;
+
+static struct i2c_driver i2c_driver_s5k4e1_sensor;
+*/
+static int s5k4e1_detect(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	int adap_id = i2c_adapter_id(adapter);
+	u32 value;
+
+	DBG_entering;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
+		eprintk("error i2c check func");
+		return -ENODEV;
+	}
+
+	if (adap_id != 1) {
+		eprintk("adap_id != 1");
+		return -ENODEV;
+	}
+
+	if (s5k4e1_wakeup()) {
+		eprintk("sensor wakeup failed");
+		return -EIO;
+	}
+
+	s5k4e1_read(client, 0x0003, &value);
+	dprintk(1, "Read from 0x0003: %x", value);
+	if ((value != 0x09))
+		return -ENODEV;
+
+	s5k4e1_read(client, 0x0000, &value);
+	dprintk(1, "Read from 0x0000: %x", value);
+	if ((value != 0x4e) && (value != 0x10))
+		return -ENODEV;
+
+	s5k4e1_read(client, 0x0001, &value);
+	dprintk(1, "Read from 0x0001: %x", value);
+	if ((value != 0x4e) && (value != 0x10))
+		return -ENODEV;
+
+	/*TODO EVT3 detect*/
+	s5k4e1_read(client, 0x0002, &value);
+	dprintk(1, "Read from 0x0002: %x", value);
+	if (value == 0x0010) {
+		dprintk(1, "EVT3 module not supported!");
+		return -ENODEV;
+	}
+
+	DBG_leaving;
+	return 0;
+}
+
+static int s5k4e1_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct ci_sensor_config *info;
+	struct v4l2_subdev *sd;
+	int ret = -1;
+
+	DBG_entering;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+		 client->addr << 1, client->adapter->name);
+
+	/*
+	 * Setup sensor configuration structure
+	 */
+	info = kzalloc(sizeof(struct ci_sensor_config), GFP_KERNEL);
+	if (!info) {
+		dprintk(0, "fail to malloc for ci_sensor_config");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = s5k4e1_detect(client);
+	if (ret) {
+		dprintk(0, "error s5k4e1_detect");
+		goto out_free;
+	}
+
+	sd = &info->sd;
+	v4l2_i2c_subdev_init(sd, client, &s5k4e1_ops);
+
+	/*
+	 * Initialization S5K4E1
+	 * then turn into standby mode
+	 */
+	ret = s5k4e1_init(client);
+	if (ret) {
+		dprintk(0, "error calling s5k4e1_init");
+		goto out_free;
+	}
+
+	s5k4e1_standby();
+	dprintk(0, "Init s5k4e1 sensor successfully");
+
+	ret = 0;
+	goto out;
+
+out_free:
+	kfree(info);
+	DBG_leaving;
+out:
+
+	DBG_leaving;
+	return ret;
+}
+
+
+static int s5k4e1_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	DBG_entering;
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_sensor_config(sd));
+
+	DBG_leaving;
+	return 0;
+}
+
+/**
+ * i2c_driver for s5k4e1_sensor
+ */
+static const struct i2c_device_id s5k4e1_id[] = {
+	{"s5k4e1", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, s5k4e1_id);
+
+static struct v4l2_i2c_driver_data v4l2_i2c_data = {
+	.name = "s5k4e1",
+	.probe = s5k4e1_probe,
+	.remove = s5k4e1_remove,
+	/* .suspend = s5k4e1_suspend,
+	 * .resume = s5k4e1_resume, */
+	.id_table = s5k4e1_id,
+};
+
+MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
+MODULE_DESCRIPTION("A low-level driver for Samsung S5K4E1 sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.h b/drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.h
new file mode 100755
index 0000000..d722035
--- /dev/null
+++ b/drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.h
@@ -0,0 +1,662 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#define I2C_S5K4E1	0x6C
+/* Should add to kernel source */
+#define I2C_DRIVERID_S5K4E1	1046
+/* GPIO pin on Moorestown */
+#define GPIO_SCLK_25	44
+#define GPIO_STB_PIN	47
+#define GPIO_STDBY_PIN	49
+#define GPIO_RESET_PIN	50
+
+struct regval_list {
+	u16 reg_num;
+	u8 value;
+};
+
+/*
+ * Default register value
+ * 5Mega Pixel, 2592x1944
+ */
+/* MIPI register are removed by Wen */
+
+/* 2592x1944 */
+static struct regval_list s5k4e1_res_qsxga_plus4[] = {
+	/* Reset for operation */
+	{0x0100, 0x00},		/* stream off */
+	{0x0103, 0x01}, 	/* software reset */
+
+/*
+ * Analog Setting
+ * This register is for FACTORY ONLY.
+ * If you change it without prior notification,
+ * You are RESPONSIBLE for the FAILURE that will happen in the future.
+ */
+
+/* CDS timing setting ... */
+	{0x3000, 0x04},	/* ct_ld_start (default = 07h) */
+	{0x3001, 0x02},	/* ct_sl_start (default = 05h) */
+	{0x3002, 0x0C},	/* ct_rx_start (default = 21h) */
+	{0x3003, 0x0E},	/* ct_cds_start (default = 23h) */
+	{0x3004, 0x2C},	/* ct_smp_width (default = 60h) */
+	{0x3005, 0x0D},	/* ct_az_width (default = 28h) */
+	{0x3006, 0x39},	/* ct_s1r_width (default = 88h) */
+	{0x3007, 0x02},	/* ct_tx_start (default = 06h) */
+	{0x3008, 0x3C},	/* ct_tx_width 1.5us (default = 7Ch) */
+	{0x3009, 0x3C},	/* ct_stx_width 1.5us (default = 7Ch) */
+	{0x300A, 0x28},	/* ct_dtx_width 1us (default = 3Eh) */
+	{0x300B, 0x15},	/* ct_rmp_rst_start (default = 44h) */
+	{0x300C, 0x15},	/* ct_rmp_sig_start (default = 48h) */
+	{0x300D, 0x02},	/* ct_rmp_lat (default = 02h) */
+	{0x300E, 0xA9},	/* D-Shut en[7], CLP On[5], LD high[4] */
+
+/* CDS option setting ... */
+	{0x3010, 0x00},	/* smp_en[2]=0(00) 1(04) row_id[1:0] = 00 */
+	{0x3011, 0x7A},	/* RST_MX (288), SIG_MX (1024+352) */
+	{0x3012, 0x30},	/* SIG offset1  48 code  */
+	{0x3013, 0xA0},	/* RST offset1  160 code */
+	{0x3014, 0x00},	/* SIG offset2 */
+	{0x3015, 0x00},	/* RST offset2 */
+	{0x3016, 0x02},	/* ADC_SAT (510mV) */
+	{0x3017, 0x94},	/* RMP_INIT[3:0](RMP_REG) 1.8V MS[6:4]=1 */
+	{0x3018, 0x78},	/* rmp option - ramp connect[MSB] +RMP INIT DAC MIN */
+	{0x301D, 0xD4},	/* CLP level (default = 0Fh) */
+
+	{0x3021, 0x02},	/* inrush ctrl[1] off */
+	{0x3022, 0x44},	/* pump ring oscillator set [7:4]=CP, [3:0]=NCP  */
+	{0x3024, 0x40},	/* pix voltage 2.8V   (default = 88h) */
+	{0x3027, 0x08},	/* ntg voltage (default = 04h) */
+
+/* Pixel option setting ... */
+	{0x301C, 0x05},	/* Pixel Bias [3:0] (default = 03h) */
+	{0x30D8, 0x3F},	/* All tx off 2f, on 3f */
+
+/* ADLC setting ... */
+	{0x3070, 0x5F},	/* [6]L-ADLC BPR, [4]ch sel, [3]L-ADLC, [2]F-ADLC */
+	{0x3071, 0x00},	/* F&L-adlc max 127 (default = 11h, max 255) */
+	{0x3080, 0x04},	/* F-ADLC filter A (default = 10h) */
+	{0x3081, 0x38},	/* F-ADLC filter B (default = 20h) */
+
+/* Integration setting ... */
+	{0x0202, 0x03},	/* coarse integration time */
+	{0x0203, 0xCF},
+	{0x0204, 0x00},	/* analog gain[msb] 0100 x8 0080 x4 */
+	{0x0205, 0x80},	/* analog gain[lsb] 0040 x2 0020 x1 */
+
+/* Frame Length */
+	{0x0340, 0x07},	/* Capture 07B4(1960[# of row]+12[V-blank]) */
+	{0x0341, 0xA4},	/* Preview 03E0(980[# of row]+12[V-blank]) */
+
+/* Line Length */
+	{0x0342, 0x0A}, /* 2738 */
+	{0x0343, 0xB2},	/* (Same as sensor default) */
+
+/* embedded 2-line OFF setting ... */
+/* 2608 x 1960  */
+	{0x3084, 0x15},	/* SYNC Mode */
+
+/* (3) MIPI 2-lane Serial(TST = 0000b or TST = 0010b), 30 fps */
+
+	{0x30A9, 0x01},
+	{0x0387, 0x01},
+
+	{0x30BD, 0x00},		/* SEL_CCP[0] */
+	{0x30B2, 0x08},		/* PLL P = 8 */
+	{0x30B3, 0x00},		/* PLL M[8] = 0 */
+	{0x30B5, 0x01},		/* PLL S = 0 */
+	{0x30BE, 0x1A},		/* M_PCLKDIV_AUTO[4], M_DIV_PCLK[3:0] */
+
+	{0x30BF, 0xAB},
+	{0x30C0, 0x00},		/* video_offset[7:4] 3240%12 */
+	{0x30C1, 0x01},		/* pack video enable [0] */
+	{0x30C8, 0x0C},		/* video_data_length 3260 = 2608 * 1.25 */
+	{0x30C9, 0xA8},
+	{0x30E2, 0x02},		/* num lanes[1:0] = 2 */
+	{0x30EE, 0x02},		/* DPHY enable [1] */
+	{0x30F1, 0x70},		/* DPHY BANDCTRL 800MHz=80.6MHz */
+	{0x3111, 0x86},		/* Embedded data off [5] */
+
+	{0x034C, 0x0A},
+	{0x034D, 0x20},
+	{0x044E, 0x07},
+	{0x034F, 0x98},
+
+	{0x0344, 0x00},
+	{0x0345, 0x08},
+	{0x0346, 0x00},
+	{0x0347, 0x08},
+	{0x0348, 0x0A},
+	{0x0349, 0x27},
+	{0x034A, 0x07},
+	{0x034B, 0x9F},
+
+	/* This is to set FRAME_NUM > 0 */
+	{0x30d9, 0x00},
+
+	/* Add this setting according to Bill's test */
+	{0x0305, 0x05},
+	{0x0306, 0x00},
+	{0x0307, 0x3c},
+	{0x30b5, 0x02},
+
+	{0x020E, 0x01},      /* Gr Digital Gain */
+	{0x020F, 0x00},
+	{0x0210, 0x01},     /* Red Digital Gain */
+	{0x0211, 0x00},
+	{0x0212, 0x01},     /* Blue Digital Gain */
+	{0x0213, 0x00},
+	{0x0214, 0x01},    /* Gb Digital Gain */
+	{0x0215, 0x00},
+	{0x0204, 0x00},
+	{0x0205, 0x80},
+
+#if 1
+	/*Apply Bill's setting*/
+	{0x30E2, 0x02},
+	{0x0305, 0x05},
+	{0x0306, 0x00},
+	{0x0307, 0x50},		/* vcc_out = 80 */
+	{0x30B5, 0x01},		/* pll_s = 1 */
+	{0x30B4, 0x50},
+
+	{0x30B2, 0x05},
+
+	{0x30BE, 0x1A},		/* DIV_M_PCLK = 5 */
+
+	{0x0100, 0x01},		/* stream on */
+	{0xffff, 0xff},
+#endif
+};
+
+/* 1920x1080 */
+static struct regval_list s5k4e1_res_1080p[] = {
+/* Reset for operation ... */
+	{0x0100, 0x00},		/* stream off */
+	{0x0103, 0x01}, 	/* software reset */
+
+/*
+ * Analog Setting
+ * This register is for FACTORY ONLY.
+ * If you change it without prior notification,
+ * You are RESPONSIBLE for the FAILURE that will happen in the future.
+ */
+
+/* CDS timing setting ... */
+	{0x3000, 0x04},	/* ct_ld_start (default = 07h) */
+	{0x3001, 0x02},	/* ct_sl_start (default = 05h) */
+	{0x3002, 0x0C},	/* ct_rx_start (default = 21h) */
+	{0x3003, 0x0E},	/* ct_cds_start (default = 23h) */
+	{0x3004, 0x2C},	/* ct_smp_width (default = 60h) */
+	{0x3005, 0x0D},	/* ct_az_width (default = 28h) */
+	{0x3006, 0x39},	/* ct_s1r_width (default = 88h) */
+	{0x3007, 0x02},	/* ct_tx_start (default = 06h) */
+	{0x3008, 0x3C},	/* ct_tx_width 1.5us (default = 7Ch) */
+	{0x300A, 0x28},	/* ct_dtx_width 1us (default = 3Eh) */
+	{0x300B, 0x15},	/* ct_rmp_rst_start (default = 44h) */
+	{0x300C, 0x15},	/* ct_rmp_sig_start (default = 48h) */
+	{0x300D, 0x02},	/* ct_rmp_lat (default = 02h) */
+	{0x300E, 0xA9},	/* D-Shut en[7], CLP On[5], LD high[4] */
+
+/* CDS option setting ... */
+	{0x3010, 0x00},	/* smp_en[2]=0(00) 1(04) row_id[1:0] = 00 */
+	{0x3011, 0x7A},	/* RST_MX (288), SIG_MX (1024+352) */
+	{0x3012, 0x30},	/* SIG offset1  48 code */
+	{0x3013, 0xA0},	/* RST offset1  160 code */
+	{0x3014, 0x00},	/* SIG offset2 */
+	{0x3015, 0x00},	/* RST offset2 */
+	{0x3016, 0x0A},	/* ADC_SAT (510mV) */
+	{0x3017, 0x94},	/* RMP_INIT[3:0](RMP_REG) 1.8V MS[6:4]=1 */
+	{0x3018, 0x78},	/* rmp option - ramp connect[MSB] +RMP INIT DAC MIN */
+
+	{0x301D, 0xD4},	/* CLP level (default = 0Fh) */
+
+	{0x3021, 0x02},	/* inrush ctrl[1] off */
+	{0x3022, 0x41},	/* pump ring oscillator set [7:4]=CP, [3:0]=NCP */
+	{0x3024, 0x08},	/* pix voltage 2.8V   (default = 88h) */
+	{0x3027, 0x08},	/* ntg voltage (default = 04h) */
+
+/* Pixel option setting ... */
+	{0x301C, 0x05},	/* Pixel Bias [3:0] (default = 03h) */
+	{0x30D8, 0x3F},	/* All tx off 2f, on 3f */
+
+/* ADLC setting ... */
+	{0x3070, 0x5F},	/* [6]L-ADLC BPR, [4]ch sel, [3]L-ADLC, [2]F-ADLC */
+	{0x3071, 0x00},	/* F&L-adlc max 127 (default = 11h, max 255) */
+	{0x3080, 0x04},	/* F-ADLC filter A (default = 10h) */
+	{0x3081, 0x38},	/* F-ADLC filter B (default = 20h) */
+
+/* Integration setting ... */
+	{0x0202, 0x03},	/* coarse integration time */
+	{0x0203, 0xCD},
+	{0x0204, 0x00},	/* analog gain[msb] 0100 x8 0080 x4 */
+	{0x0205, 0x80},	/* analog gain[lsb] 0040 x2 0020 x1 */
+
+/* Frame Length */
+	{0x0340, 0x04},	/*Capture 07B4(1960[# of row]+12[V-blank]) */
+	{0x0341, 0x44},	/*Preview 03E0(980[# of row]+12[V-blank]) */
+
+/* Line Length */
+	{0x0342, 0x0A}, /* 2738 */
+	{0x0343, 0xB2},	/*(Same as sensor default) */
+
+/* embedded 2-line OFF setting ... */
+/* 1920 x 1080 */
+	{0x3084, 0x15},	/* SYNC Mode */
+
+/* PLL & MIPI setting ... */
+/* input clock 25MHz */
+
+/* (3) MIPI 2-lane Serial(TST = 0000b or TST = 0010b), 30 fps */
+	{0x30BD, 0x00},		/* SEL_CCP[0] */
+	{0x30B2, 0x08},		/* PLL P = 8 */
+	{0x30B3, 0x00},		/* PLL M[8] = 0 */
+	{0x30B4, 0x78},		/* PLL M = 129 */
+	{0x30B5, 0x00},		/* PLL S = 0 */
+	{0x30BE, 0x1A},		/* M_PCLKDIV_AUTO[4], M_DIV_PCLK[3:0] */
+
+	{0x30BF, 0xAB},
+	{0x30C0, 0x00},		/* video_offset[7:4] 2400%12 */
+	{0x30C1, 0x01},		/* pack video enable [0] */
+	{0x30C8, 0x09},		/* video_data_length 2400 = 1920 * 1.25 */
+	{0x30C9, 0x60},
+	{0x30E2, 0x02},		/* num lanes[1:0] = 2 */
+	{0x30EE, 0x02},		/* DPHY enable [1] */
+	{0x30F1, 0x70},		/* DPHY BANDCTRL 800MHz=80.6MHz */
+	{0x3111, 0x86},		/* Embedded data off [5] */
+
+	{0x30b4, 0x20},
+	{0x30b5, 0x01},
+
+	{0x30A9, 0x01},
+	{0x0387, 0x01},
+	{0x0344, 0x01},	/*x_addr_start 344 */
+	{0x0345, 0x58},
+	{0x0348, 0x08},	/*x_addr_end 2263 */
+	{0x0349, 0xD7},
+	{0x0346, 0x01},	/*y_addr_start 440 */
+	{0x0347, 0xB8},
+	{0x034A, 0x05},	/*y_addr_end 1519 */
+	{0x034B, 0xEF},
+
+	{0x034C, 0x07},	/*x_output_size 1920 */
+	{0x034D, 0x80},
+	{0x034E, 0x04},	/*y_output_size 1080 */
+	{0x034F, 0x38},
+
+	{0x30d9, 0x00},
+
+	{0x020E, 0x01},      /*Gr Digital Gain */
+	{0x020F, 0x00},
+	{0x0210, 0x01},     /*Red Digital Gain */
+	{0x0211, 0x00},
+	{0x0212, 0x01},     /*Blue Digital Gain */
+	{0x0213, 0x00},
+	{0x0214, 0x01},    /*Gb Digital Gain */
+	{0x0215, 0x00},
+	{0x0204, 0x00},
+	{0x0205, 0x80},
+
+
+	/*Apply Bill's setting*/
+	{0x30E2, 0x02},
+	{0x0305, 0x05},
+	{0x0306, 0x00},
+	{0x0307, 0x50},		/*vcc_out = 80 */
+	{0x30B5, 0x01},		/*pll_s = 1 */
+	{0x30B4, 0x50},
+
+	{0x30B2, 0x05},
+
+	{0x30BE, 0x1A},		/*DIV_M_PCLK = 5 */
+
+	{0x0383, 0x01},
+
+	{0x0100, 0x01},		/* stream on */
+	{0xffff, 0xff},
+
+};
+
+/* 1280x720, V1F2 & H1F2 */
+static struct regval_list s5k4e1_res_720p[] = {
+	{0x0100, 0x00},		/* stream off */
+	{0x0103, 0x01}, 	/* software reset */
+
+/* CDS timing setting ... */
+	{0x3000, 0x04},
+	{0x3001, 0x02},
+	{0x3002, 0x0C},
+	{0x3003, 0x0E},
+	{0x3004, 0x2C},
+	{0x3005, 0x0D},
+	{0x3006, 0x39},
+	{0x3007, 0x02},
+	{0x3008, 0x3C},
+	{0x3009, 0x3C},
+	{0x300A, 0x28},
+	{0x300B, 0x15},
+	{0x300C, 0x15},
+	{0x300D, 0x02},
+	{0x300E, 0xAB},
+
+/* CDS option setting ... */
+	{0x3010, 0x00},
+	{0x3011, 0x7A},
+	{0x3012, 0x30},
+	{0x3013, 0x90},
+	{0x3014, 0x00},
+	{0x3015, 0x00},
+	{0x3016, 0x0A},
+	{0x3017, 0x84},
+	{0x3018, 0x78},
+	{0x301D, 0xD4},
+
+	{0x3021, 0x02},
+	{0x3022, 0x41},
+	{0x3024, 0x08},
+	{0x3027, 0x08},
+
+/* Pixel option setting ... */
+	{0x301C, 0x05},	/* Pixel Bias [3:0] (default = 03h) */
+	{0x30D8, 0x3F},	/* All tx off 2f, on 3f */
+
+/* ADLC setting ... */
+	{0x3070, 0x5F},
+	{0x3071, 0x00},
+	{0x3080, 0x04},
+	{0x3081, 0x38},
+
+/* Integration setting ...  */
+	{0x0202, 0x03},
+	{0x0203, 0xD8},
+	{0x0204, 0x00},
+	{0x0205, 0x80},
+
+/*Frame Length*/
+	{0x0340, 0x02},
+	{0x0341, 0xDC},
+
+/* Line Length */
+	{0x0342, 0x0A}, /*2738 */
+	{0x0343, 0xB2},
+
+/* Average Sub-sampling */
+	{0x0387, 0x03},
+	{0x30a9, 0x02},
+
+/* embedded 2-line OFF setting ... */
+/* 1280 x 720 */
+	{0x3084, 0x15},
+
+/* PLL & MIPI setting ... */
+
+/* (3) MIPI 2-lane Serial(TST = 0000b or TST = 0010b), 60 fps */
+	{0x30BD, 0x00},
+	{0x30B2, 0x08},
+	{0x30B3, 0x00},
+	{0x30B4, 0x78},
+	{0x30B5, 0x00},
+	{0x30BE, 0x1A},
+
+	{0x30BF, 0xAB},
+	{0x30C0, 0x40},
+	{0x30C1, 0x01},
+	{0x30C8, 0x06},
+	{0x30C9, 0x40},
+
+	{0x30E2, 0x02},
+
+	{0x30b4, 0x20},
+	{0x30b5, 0x01},
+
+	{0x30EE, 0x02},
+	{0x30F1, 0x70},
+	{0x3111, 0x86},
+
+/* MIPI Size Setting ... */
+/* 1304 x 980 */
+	{0x0344, 0x00},
+	{0x0345, 0x18},
+	{0x0348, 0x0A},
+	{0x0349, 0x17},
+	{0x0346, 0x01},
+	{0x0347, 0x04},
+	{0x034A, 0x06},
+	{0x034B, 0xA3},
+
+	{0x0380, 0x00},
+	{0x0381, 0x01},
+	{0x0382, 0x00},
+	{0x0383, 0x01},
+	{0x0384, 0x00},
+	{0x0385, 0x01},
+	{0x0386, 0x00},
+	{0x0387, 0x03},
+
+	{0x034C, 0x05}, /* x_output_size = 1280 */
+	{0x034D, 0x00},
+	{0x034E, 0x02}, /* y_output_size = 720 */
+	{0x034F, 0xD0},
+
+	{0x30d9, 0x00},
+
+	{0x020E, 0x01},
+	{0x020F, 0x00},
+	{0x0210, 0x01},
+	{0x0211, 0x00},
+	{0x0212, 0x01},
+	{0x0213, 0x00},
+	{0x0214, 0x01},
+	{0x0215, 0x00},
+	{0x0204, 0x01},
+	{0x0205, 0x00},
+
+	/*Apply Bill's setting*/
+	{0x30E2, 0x02},
+	{0x0305, 0x05},
+	{0x0306, 0x00},
+	{0x0307, 0x50},		/*vcc_out = 80 */
+	{0x30B5, 0x01},		/*pll_s = 1 */
+	{0x30B4, 0x50},
+
+	{0x30B2, 0x05},
+
+	{0x30BE, 0x15},		/*DIV_M_PCLK = 5 */
+
+	{0x0100, 0x01},		/* stream on */
+	{0xffff, 0xff},
+};
+
+/*VGA*/
+static struct regval_list s5k4e1_res_vga_ac04_bill[] = {
+	{0x0100, 0x00},		/* stream off */
+	{0x0103, 0x01}, 	/* software reset */
+
+	{0x3000, 0x04},
+	{0x3001, 0x02},
+	{0x3002, 0x0C},
+	{0x3003, 0x0E},
+	{0x3004, 0x2C},
+	{0x3005, 0x0D},
+	{0x3006, 0x39},
+	{0x3007, 0x02},
+	{0x3008, 0x3C},
+	{0x3009, 0x3C},
+	{0x300A, 0x28},
+	{0x300B, 0x15},
+	{0x300C, 0x15},
+	{0x300D, 0x02},
+	{0x300E, 0xA8},
+
+	{0x3010, 0x00},
+	{0x3011, 0x7A},
+	{0x3012, 0x30},
+	{0x3013, 0xA0},
+	{0x3014, 0x00},
+	{0x3015, 0x00},
+	{0x3016, 0x0A},
+	{0x3017, 0x94},
+	{0x3018, 0x78},
+
+	{0x301D, 0xD4},
+
+	{0x3021, 0x02},
+	{0x3022, 0x41},
+	{0x3024, 0x08},
+	{0x3027, 0x08},
+
+	{0x301C, 0x05},
+	{0x30D8, 0x3F},
+
+	{0x3070, 0x5F},
+	{0x3071, 0x00},
+	{0x3080, 0x04},
+	{0x3081, 0x38},
+
+	{0x0202, 0x03},
+	{0x0203, 0xD4},
+	{0x0204, 0x00},
+	{0x0205, 0x20},
+
+	{0x0340, 0x03},
+	{0x0341, 0xE0},
+
+	{0x0342, 0x0A},
+	{0x0343, 0xB2},
+
+	{0x0344, 0x00},
+	{0x0345, 0x18},
+	{0x0348, 0x0A},
+	{0x0349, 0x17},
+	{0x0346, 0x00},
+	{0x0347, 0x14},
+	{0x034A, 0x07},
+	{0x034B, 0x93},
+
+	{0x034C, 0x02},
+	{0x034D, 0x80},
+	{0x034E, 0x01},
+	{0x034F, 0xE0},
+
+	{0x0380, 0x00},
+	{0x0381, 0x01},
+	{0x0382, 0x00},
+	{0x0383, 0x07},
+	{0x0384, 0x00},
+	{0x0385, 0x01},
+	{0x0386, 0x00},
+	{0x0387, 0x07},
+
+	{0x3084, 0x15},
+
+	{0x30BD, 0x00},
+
+
+	{0x30b3, 0x00},
+	{0x30b4, 0x57},
+	{0x30b5, 0x01},
+	{0x30f1, 0x70},
+
+	{0x30BE, 0x1A},
+
+	{0x30BF, 0xAB},
+	{0x30C0, 0x80},
+	{0x30C1, 0x01},
+	{0x30C8, 0x03},
+	{0x30C9, 0x20},
+
+	{0x30b2, 0x06},
+	{0x30E2, 0x02},
+
+	{0x30EE, 0x02},
+
+	{0x3111, 0x86},
+
+	{0x30d9, 0x00},
+
+	{0x020E, 0x01},
+	{0x020F, 0x00},
+	{0x0210, 0x01},
+	{0x0211, 0x00},
+	{0x0212, 0x01},
+	{0x0213, 0x00},
+	{0x0214, 0x01},
+	{0x0215, 0x00},
+	{0x0204, 0x01},
+	{0x0205, 0x00},
+
+#if 1
+	/* Apply Bill's setting */
+	{0x30E2, 0x02},
+	{0x0305, 0x05},
+	{0x0306, 0x00},
+	{0x0307, 0x50},
+	{0x30B5, 0x01},
+	{0x30B4, 0x50},
+
+	{0x30B2, 0x05},
+
+	{0x30BE, 0x15},
+
+	/* {0x0100, 0x01}, */
+	/* {0xffff, 0xff}, */
+#endif
+
+#if 1
+	/* 1304x980 */
+	{0x3013, 0x90},
+	{0x3017, 0x84},
+	{0x30A9, 0x02},
+	{0x300E, 0xAB},
+
+	{0x0387, 0x03},
+	{0x0344, 0x00}, /* x_addr_start = 0 */
+	{0x0345, 0x00},
+	{0x0348, 0x0A}, /* x_addr_end = 2607 */
+	{0x0349, 0x2F},
+	{0x0346, 0x00}, /* y_addr_start = 0 */
+	{0x0347, 0x00},
+	{0x034A, 0x07}, /* y_addr_end = 1959 */
+	{0x034B, 0xA7},
+	{0x0380, 0x00},
+	{0x0381, 0x01},
+	{0x0382, 0x00},
+	{0x0383, 0x01},
+	{0x0384, 0x00},
+	{0x0385, 0x01},
+	{0x0386, 0x00},
+	{0x0387, 0x03},
+	{0x034c, 0x05}, /* x_output_size = 1304 */
+	{0x034d, 0x18},
+	{0x034e, 0x03}, /* y_output_size = 980 */
+	{0x034f, 0xd4},
+	{0x30BF, 0xAB},
+	{0x30c0, 0xa0},
+	{0x30C8, 0x06}, /* x_output_size * 1.25 */
+	{0x30c9, 0x5e},
+
+	{0x0100, 0x01},
+	{0xffff, 0xff},
+
+#endif
+};
diff --git a/drivers/media/video/mrstci/mrsts5k4e1_motor/Kconfig b/drivers/media/video/mrstci/mrsts5k4e1_motor/Kconfig
new file mode 100755
index 0000000..27cb730
--- /dev/null
+++ b/drivers/media/video/mrstci/mrsts5k4e1_motor/Kconfig
@@ -0,0 +1,9 @@
+config VIDEO_MRST_S5K4E1_MOTOR
+	tristate "Moorestown s5k4e1 motor"
+	depends on I2C && VIDEO_MRST_ISP && VIDEO_MRST_S5K4E1
+
+	---help---
+	  Say Y here if your platform support s5k4e1 motor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mrstov2650.ko.
diff --git a/drivers/media/video/mrstci/mrsts5k4e1_motor/Makefile b/drivers/media/video/mrstci/mrsts5k4e1_motor/Makefile
new file mode 100644
index 0000000..68c9fbc
--- /dev/null
+++ b/drivers/media/video/mrstci/mrsts5k4e1_motor/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VIDEO_MRST_S5K4E1_MOTOR)	 += mrsts5k4e1_motor.o
+
+EXTRA_CFLAGS	+=	-I$(src)/../include
diff --git a/drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.c b/drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.c
new file mode 100644
index 0000000..cd2813b
--- /dev/null
+++ b/drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.c
@@ -0,0 +1,430 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-i2c-drv.h>
+
+#include "mrsts5k4e1_motor.h"
+
+static int s5k4e1_motor_debug;
+module_param(s5k4e1_motor_debug, int, 0644);
+MODULE_PARM_DESC(s5k4e1_motor_debug, "Debug level (0-1)");
+
+#define dprintk(level, fmt, arg...) \
+    do {			\
+	if (s5k4e1_motor_debug >= level) \
+		printk(KERN_DEBUG "mrstisp@%s: " fmt "\n", __func__, ## arg); \
+    } while (0)
+
+#define eprintk(fmt, arg...)	\
+	printk(KERN_ERR "mrstisp@%s: line %d: " fmt "\n",	\
+	       __func__, __LINE__, ## arg);
+
+#define DBG_entering	dprintk(1, "entering");
+#define DBG_leaving	dprintk(1, "leaving");
+#define DBG_line	dprintk(1, " line: %d", __LINE__);
+
+static inline struct s5k4e1_motor *to_motor_config(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct s5k4e1_motor, sd);
+}
+
+/*static struct s5k4e1_motor *config; */
+static int motor_read(struct i2c_client *c, u32 *reg)
+{
+	int ret;
+	struct i2c_msg msg;
+	u8 msgbuf[3];
+
+	msgbuf[0] = 0;
+	msgbuf[1] = 0;
+	msgbuf[2] = 0;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.addr = c->addr;
+	msg.buf = msgbuf;
+	msg.len = 3;
+	msg.flags = I2C_M_RD;
+
+	ret = i2c_transfer(c->adapter, &msg, 1);
+
+	*reg = (msgbuf[0] << 16 | msgbuf[1] << 8 | msgbuf[2]);
+
+	ret = (ret == 1) ? 0 : -1;
+	return ret;
+}
+
+static int motor_write(struct i2c_client *c, u32 reg)
+{
+	int ret;
+	struct i2c_msg msg;
+	u8 msgbuf[3];
+
+	memset(&msg, 0, sizeof(msg));
+	msgbuf[0] = (reg & 0x00FFFFFFFF) >> 16;
+	msgbuf[1] = (reg & 0x0000FFFF) >> 8 ;
+	msgbuf[2] = reg;
+
+	msg.addr = c->addr;
+	msg.flags = 0;
+	msg.buf = msgbuf;
+	msg.len = 3;
+
+	ret = i2c_transfer(c->adapter, &msg, 1);
+
+	ret = (ret == 1) ? 0 : -1;
+	return ret;
+}
+
+static int s5k4e1_motor_goto_position(struct i2c_client *c,
+				      unsigned short code,
+				      struct s5k4e1_motor *config,
+				      unsigned int step)
+{
+	int max_code, min_code;
+	int timeout = 25; /*TODO: check the timeout time */
+	u8 cmdh, cmdl, finished;
+	u32 cmd = 0, val = 0;
+
+	max_code = config->macro_code;
+	min_code = config->infin_code;
+
+	if (code > max_code)
+		code = max_code;
+	if (code < min_code)
+		code = min_code;
+
+	cmdh = MOTOR_DAC_CTRL_MODE_1 | (code >> 8); /* PS EN x x M W TD9 TD8*/
+	cmdl = code; /* TD7 ~ TD0 */
+	cmd |= (cmdh << 16) | (cmdl << 8);
+
+	dprintk(1, "cmdh: %x, cmdl: %x, cmd: %x", cmdh, cmdl, cmd);
+	dprintk(1, "DAC code: %x", code);
+
+	motor_write(c, cmd);
+	finished = 0;
+	while ((!finished) && timeout--) {
+		msleep(1);
+		motor_read(c, &val);
+		cmdh = val >> 16;
+		cmdl = val >> 8;
+
+		dprintk(1, "cmdh & MOTOR_F = %x", cmdh & MOTOR_F);
+		finished = cmdh & MOTOR_F;
+		finished = (finished) ? 0 : 1;
+	};
+
+	if (finished) {
+		dprintk(1, "Moving from code %x to code %x takes %d ms.",
+		       config->cur_code, code, 25-timeout);
+		return 0;
+	} else {
+		eprintk("Unable to move motor to step %d, TIMEOUT!!", step);
+		return -1;
+	}
+
+}
+
+int s5k4e1_motor_wakeup(struct i2c_client *client)
+{
+	/* hardware wakeup: set PS = 1 */
+	return motor_write(client, 0xC00000);
+}
+
+int s5k4e1_motor_standby(struct i2c_client *client)
+{
+	/* hardware standby: set PS = 0 */
+	return motor_write(client, 0x400000);
+}
+
+int s5k4e1_motor_init(struct i2c_client *client, struct s5k4e1_motor *config)
+{
+
+	int ret;
+	int infin_cur, macro_cur;
+	int step_res, step_time;
+	int val;
+
+	DBG_entering;
+	infin_cur = MAX(MOTOR_INFIN_CUR, MOTOR_DAC_MIN_CUR);
+	macro_cur = MIN(MOTOR_MACRO_CUR, MOTOR_DAC_MAX_CUR);
+	step_res = 1 << MOTOR_STEP_SHIFT;
+	step_time = MOTOR_STEP_TIME;
+
+	/*config->motor = client;*/
+	config->infin_cur = infin_cur;
+	config->macro_cur = macro_cur;
+
+	config->infin_code = MOTOR_INFIN_CODE;
+	config->macro_code = MOTOR_MACRO_CODE;
+
+	config->max_step = ((config->macro_code - config->infin_code)
+			    >> MOTOR_STEP_SHIFT) + 1;
+	config->step_res = step_res;
+	config->step_time = step_time;
+
+	dprintk(1, "max_step: %d, step_res: %d, step_time: %d",
+		config->max_step, step_res, step_time);
+
+	/* Set motor step time and resolution */
+	val = (MOTOR_DAC_CTRL_MODE_0 << 16) | (step_res << 8) | step_time;
+	ret = motor_write(client, val);
+
+	/* Note here, maybe macro_code */
+	ret |= s5k4e1_motor_goto_position(client, config->infin_code,
+					  config, 0);
+	if (!ret) {
+		config->cur_code = config->infin_code;
+		dprintk(1, "Motor initialization success!");
+	} else
+		eprintk("Error while initializing motor!!!");
+
+	return ret;
+}
+
+int s5k4e1_motor_set_focus(struct i2c_client *c,
+			   unsigned int step,
+			   struct s5k4e1_motor *config)
+{
+	int s_code, ret;
+	int max_step = config->max_step;
+	unsigned int val = step;
+
+	if (val > max_step)
+		val = max_step;
+
+	s_code = (val << MOTOR_STEP_SHIFT);
+	s_code += config->infin_code;
+
+	ret = s5k4e1_motor_goto_position(c, s_code, config, step);
+	if (!ret)
+		config->cur_code = s_code;
+
+	return ret;
+}
+
+static int s5k4e1_motor_g_ctrl(struct v4l2_subdev *sd,
+			       struct v4l2_control *ctrl)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	struct s5k4e1_motor *config = to_motor_config(sd);
+	int ret;
+
+	DBG_entering;
+	ret = s5k4e1_motor_get_focus(c, &ctrl->value, config);
+	if (ret) {
+		eprintk("error call s5k4e1_motor_get_focue");
+		return ret;
+	}
+	DBG_leaving;
+	return 0;
+}
+
+static int s5k4e1_motor_s_ctrl(struct v4l2_subdev *sd,
+			       struct v4l2_control *ctrl)
+{
+	struct i2c_client *c = v4l2_get_subdevdata(sd);
+	struct s5k4e1_motor *config = to_motor_config(sd);
+	int ret;
+
+	DBG_entering;
+	ret = s5k4e1_motor_set_focus(c, ctrl->value, config);
+	if (ret) {
+		eprintk("error call s5k4e1_motor_set_focue");
+		return ret;
+	}
+	DBG_leaving;
+	return 0;
+}
+
+int s5k4e1_motor_get_focus(struct i2c_client *c,
+			   unsigned int *step,
+			   struct s5k4e1_motor *config)
+{
+	int ret_step;
+
+	ret_step = ((config->cur_code - config->infin_code)
+		    >> MOTOR_STEP_SHIFT);
+
+	if (ret_step <= config->max_step)
+		*step = ret_step;
+	else
+		*step = config->max_step;
+
+	return 0;
+}
+
+int s5k4e1_motor_max_step(struct i2c_client *c,
+			  unsigned int *max_code,
+			  struct s5k4e1_motor *config)
+{
+	if (config->max_step != 0)
+		*max_code = config->max_step;
+	return 0;
+
+}
+
+static int s5k4e1_motor_queryctrl(struct v4l2_subdev *sd,
+			    struct v4l2_queryctrl *qc)
+{
+	struct s5k4e1_motor *config = to_motor_config(sd);
+
+	DBG_entering;
+	dprintk(1, "got focus range of %d", config->max_step);
+	if (config->max_step != 0)
+		qc->maximum = config->max_step;
+	DBG_leaving;
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops s5k4e1_motor_core_ops = {
+	.g_ctrl = s5k4e1_motor_g_ctrl,
+	.s_ctrl = s5k4e1_motor_s_ctrl,
+	.queryctrl = s5k4e1_motor_queryctrl,
+};
+
+static const struct v4l2_subdev_ops s5k4e1_motor_ops = {
+	.core = &s5k4e1_motor_core_ops,
+};
+
+static int s5k4e1_motor_detect(struct i2c_client *client)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	int adap_id = i2c_adapter_id(adapter);
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
+		eprintk("error i2c check func");
+		return -ENODEV;
+	}
+
+	if (adap_id != 1) {
+		eprintk("adap_id != 1");
+		return -ENODEV;
+	}
+
+	if (s5k4e1_motor_wakeup(client))
+		eprintk("unable to wakeup s5k4e1 motor.");
+
+	return 0;
+}
+
+static int s5k4e1_motor_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct s5k4e1_motor *info;
+	struct v4l2_subdev *sd;
+	int ret = -1;
+/* 	struct i2c_client *motor; */
+
+	DBG_entering;
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+		 client->addr << 1, client->adapter->name);
+	/*
+	 * Setup sensor configuration structure
+	 */
+	info = kzalloc(sizeof(struct s5k4e1_motor), GFP_KERNEL);
+	if (!info) {
+		eprintk("fail to malloc for ci_motor");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = s5k4e1_motor_detect(client);
+	if (ret) {
+		eprintk("error s5k4e1_motor_detect");
+		goto out_free;
+	}
+
+	sd = &info->sd;
+	v4l2_i2c_subdev_init(sd, client, &s5k4e1_motor_ops);
+
+	/*
+	 * Initialization S5K4E1
+	 * then turn into standby mode
+	 */
+	ret = s5k4e1_motor_init(client, info);
+	if (ret) {
+		eprintk("error calling s5k4e1_motor_init");
+		goto out_free;
+	}
+
+	ret = 0;
+	goto out;
+
+out_free:
+	kfree(info);
+	DBG_leaving;
+out:
+	return ret;
+}
+
+/*
+ * XXX: Need to be checked
+ */
+static int s5k4e1_motor_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	DBG_entering;
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_motor_config(sd));
+
+	DBG_leaving;
+	return 0;
+}
+
+static const struct i2c_device_id s5k4e1_motor_id[] = {
+	{"s5k4e1_motor", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, s5k4e1_motor_id);
+
+static struct v4l2_i2c_driver_data v4l2_i2c_data = {
+	.name = "s5k4e1_motor",
+	.probe = s5k4e1_motor_probe,
+	.remove = s5k4e1_motor_remove,
+	/* .suspend = ov5630_suspend,
+	 * .resume = ov5630_resume, */
+	.id_table = s5k4e1_motor_id,
+};
+MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
+MODULE_DESCRIPTION("A low-level driver for Samsung S5K4E1 sensor motor");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.h b/drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.h
new file mode 100644
index 0000000..04f9436
--- /dev/null
+++ b/drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.h
@@ -0,0 +1,102 @@
+/*
+ * Support for Moorestown Langwell Camera Imaging ISP subsystem.
+ *
+ * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ *
+ * Xiaolin Zhang <xiaolin.zhang@intel.com>
+ */
+
+#include <media/v4l2-subdev.h>
+
+/* DAC output max current (mA) */
+#define MOTOR_DAC_MAX_CUR	125
+/* DAC output min current (mA) */
+#define MOTOR_DAC_MIN_CUR	1
+/* DAC max code (Hex) */
+#define MOTOR_DAC_CODE_MAX 	0x3FF
+/* DAC min code (Hex) */
+#define MOTOR_DAC_CODE_MIN	0x0
+
+/* VCM start code (Hex) */
+#define MOTOR_INFIN_CODE	0x120
+/* VCM stop code (Hex) */
+#define MOTOR_MACRO_CODE	0x205
+
+#define MOTOR_STEP_SHIFT	4	/* Step res = 2^4 = 10H */
+#define MOTOR_STEP_TIME		20	/* Step time = 50us x 20d = 1ms */
+
+/* VCM start current (mA) */
+#define MOTOR_INFIN_CUR		((MOTOR_DAC_MAX_CUR / MOTOR_DAC_CODE_MAX) \
+				 * MOTOR_INFIN_CODE + 1)
+/* VCM max current for Macro (mA) */
+#define MOTOR_MACRO_CUR		((MOTOR_DAC_MAX_CUR / MOTOR_DAC_CODE_MAX) \
+				 * MOTOR_MACRO_CODE + 1)
+
+
+#define MOTOR_DAC_BIT_RES	10
+#define MOTOR_DAC_MAX_CODE	((1 << MOTOR_DAC_BIT_RES) - 1)
+
+#define MOTOR_STEP_SHIFT	4
+
+#define MAX(x, y)	((x) > (y) ? (x) : (y))
+#define MIN(x, y)	((x) < (y) ? (x) : (y))
+
+/* DAC register related define */
+#define MOTOR_PS	(1 << 7) /* power save */
+#define MOTOR_EN	(1 << 6) /* out pin status*/
+#define MOTOR_M		(1 << 3) /* mode select */
+#define MOTOR_W		(1 << 2) /* register address */
+#define MOTOR_F		(1 << 4) /* finish flag */
+
+#define MOTOR_DAC_CODE_L(x)	(x & 0xff)
+#define MOTOR_DAC_CODE_H(x)	((x >> 8) & 0xf3)
+
+/* Step mode setting */
+#define MOTOR_DAC_CTRL_MODE_0	0xCC
+/* DAC code setting */
+#define MOTOR_DAC_CTRL_MODE_1	0xC8
+
+#define S5K4E1_MOTOR_ADDR	(0x18 >> 1)
+/*#define POWER_EN_PIN	7*/
+#define GPIO_AF_PD	95
+
+#define DEBUG	0
+
+struct s5k4e1_motor{
+	/*struct i2c_client *motor;*/
+	unsigned int infin_cur;
+	unsigned int infin_code;
+	unsigned int macro_cur;
+	unsigned int macro_code;
+	unsigned int max_step;
+	unsigned int cur_code;
+	unsigned int step_res;
+	unsigned int step_time;
+	struct v4l2_subdev sd;
+};
+
+extern int s5k4e1_motor_init(struct i2c_client *client,
+			     struct s5k4e1_motor *config);
+extern int s5k4e1_motor_standby(struct i2c_client *client);
+extern int s5k4e1_motor_wakeup(struct i2c_client *client);
+extern int s5k4e1_motor_set_focus(struct i2c_client *c, unsigned int step,
+				  struct s5k4e1_motor *config);
+extern int s5k4e1_motor_get_focus(struct i2c_client *c, unsigned int *step,
+				  struct s5k4e1_motor *config);
+extern int s5k4e1_motor_max_step(struct i2c_client *c, unsigned int *max_code,
+				 struct s5k4e1_motor *config);
-- 
1.6.0.6