code.ossystems Code Review - openembedded-core.git/blob

   1 From 0d55b08388f12c7c22cae9c6c745995d051624ba Mon Sep 17 00:00:00 2001
   2 From: Zheng Ba <zheng.ba@intel.com>
   3 Date: Thu, 1 Apr 2010 16:29:43 +0800
   4 Subject: [PATCH 3/3] Moorestown Camera Imaging driver Beta 10.0
   5
   6 Patch-mainline: 2.6.35?
   7
   8 Changes from Beta 9.0:
   9 1. Fixed hsd sighting:
  10         3469638 3469639 3469710 3469822 (high)
  11         3469697 (medium)
  12
  13 Changes from Beta 8.0:
  14 1. Fixed hsd sighting
  15         3469056 3469058 (critical)
  16         3469705 3469696 3469709 3469510 (medium)
  17
  18 Changes from Beta 7.0:
  19 1. Fixed hsd sighting 3469681,3469682,3469683 (high)
  20
  21 Changes from Beta 6.0:
  22 1. Fixed hsd sighting 3469668 (high)
  23 2. Fixed ov5630 v4l2 view-finding dark issue
  24 3. Enabled support for popular v4l2 applications (cheese, skype, ffmpeg)
  25
  26 Changes from Beta 5.1:
  27 1. Fixed CRITICAL sighting 3469558 -- ciapp fails to launch with segment fault
  28 2. Fixed HIGH sighting 3479513 -- ov5630 AWB unstable
  29 3. Improved KMOT sensor 720p fps from 30 to 40
  30
  31 Changes from Beta 5.0:
  32 Fixed a critical issue of camera driver not loading -- hsd 3469557
  33
  34 Main changes from Beta 4.0:
  35 Fixed 4 HSD sightings: 3469392,3469099,3469470,3469500
  36
  37 Main changes from Beta 3.0:
  38 Fixed 7 HSD sightings: 3469264,3469112,3469395,3469103,3469105,3469471,3469484
  39
  40 Main changes from Beta 2.0:
  41 Fixed 6 HSD sightings: 3469047,3469315,3469317,3469101,3468409,3469391
  42
  43 Main changes from Beta 1.1:
  44 1. Added interrupt mode for jpeg capture and KMOT viewfinding
  45 2. Fixed HSD sighting 3469228 and 3469147
  46
  47 Main changes from Alpha2:
  48 Enabled MIPI interface in ISP driver and KMOT sensor s5k4e1.
  49 Enabled FIFO in ISP driver, which doubled the fps in view-finding mode.
  50 Enabled Subdev Framework in CI kernel driver.
  51 Enabled AF Continuous Mode.
  52 Enabled AE scene evaluation.
  53
  54 Enabled the camera drivers in kernel:
  55 Device Drivers --> Multimedia support --> Video For Linux
  56 Device Drivers --> Mulitmedia support --> Video capture adapters -->
  57 --> Moorestown Langwell Camera Imaging Subsystem support.
  58
  59 Kernel configs:
  60 1. camera driver depends on GPIO library and I2C driver.
  61 CONFIG_GENERIC_GPIO=y
  62 CONFIG_I2C=y
  63 CONFIG_GPIOLIB=y
  64 2. camera driver depends on videobuf-core and videobuf-dma-contig.
  65 VIDEOBUF_GEN=y
  66 VIDEOBUF_DMA_CONTIG=y
  67 3. enable multimedia support and video capture.
  68 CONFIG_MEDIA_SUPPORT=y
  69 CONFIG_VIDEO_DEV=y
  70 CONFIG_VIDEO_V4L2_COMMON=y
  71 CONFIG_VIDEO_MEDIA=y
  72 CONFIG_VIDEO_V4L2=y
  73 4. camera drivers incluing ISP, 5630, 5630-motor, s5k4e1, s5k4e1-motor, 2650,
  74 9665, flash.
  75 CONFIG_VIDEO_MRSTCI=y
  76 CONFIG_VIDEO_MRST_ISP=y
  77 CONFIG_VIDEO_MRST_OV5630=y
  78 CONFIG_VIDEO_MRST_OV5630_MOTOR=y
  79 CONFIG_VIDEO_MRST_S5K4E1=y
  80 CONFIG_VIDEO_MRST_S5K4E1_MOTOR=y
  81 CONFIG_VIDEO_MRST_FLASH=y
  82 CONFIG_VIDEO_MRST_OV2650=y
  83 CONFIG_VIDEO_MRST_OV9665=y
  84 Signed-off-by: Zheng Ba <zheng.ba@intel.com>
  85 ---
  86  drivers/media/video/mrstci/mrstflash/Kconfig       |    9 +
  87  drivers/media/video/mrstci/mrstflash/Makefile      |    3 +
  88  drivers/media/video/mrstci/mrstflash/mrstflash.c   |  150 +++
  89  drivers/media/video/mrstci/mrstov2650/Kconfig      |    9 +
  90  drivers/media/video/mrstci/mrstov2650/Makefile     |    3 +
  91  drivers/media/video/mrstci/mrstov2650/mrstov2650.c | 1190 ++++++++++++++++++++
  92  drivers/media/video/mrstci/mrstov2650/ov2650.h     |  766 +++++++++++++
  93  drivers/media/video/mrstci/mrstov5630/Kconfig      |    9 +
  94  drivers/media/video/mrstci/mrstov5630/Makefile     |    4 +
  95  drivers/media/video/mrstci/mrstov5630/ov5630.c     | 1153 +++++++++++++++++++
  96  drivers/media/video/mrstci/mrstov5630/ov5630.h     |  672 +++++++++++
  97  .../media/video/mrstci/mrstov5630_motor/Kconfig    |    9 +
  98  .../media/video/mrstci/mrstov5630_motor/Makefile   |    3 +
  99  .../mrstci/mrstov5630_motor/mrstov5630_motor.c     |  428 +++++++
 100  .../video/mrstci/mrstov5630_motor/ov5630_motor.h   |   86 ++
 101  drivers/media/video/mrstci/mrstov9665/Kconfig      |    9 +
 102  drivers/media/video/mrstci/mrstov9665/Makefile     |    3 +
 103  drivers/media/video/mrstci/mrstov9665/mrstov9665.c |  972 ++++++++++++++++
 104  drivers/media/video/mrstci/mrstov9665/ov9665.h     |  263 +++++
 105  drivers/media/video/mrstci/mrsts5k4e1/Kconfig      |    9 +
 106  drivers/media/video/mrstci/mrsts5k4e1/Makefile     |    3 +
 107  drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.c | 1024 +++++++++++++++++
 108  drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.h |  662 +++++++++++
 109  .../media/video/mrstci/mrsts5k4e1_motor/Kconfig    |    9 +
 110  .../media/video/mrstci/mrsts5k4e1_motor/Makefile   |    3 +
 111  .../mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.c     |  430 +++++++
 112  .../mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.h     |  102 ++
 113  27 files changed, 7983 insertions(+), 0 deletions(-)
 114  create mode 100644 drivers/media/video/mrstci/mrstflash/Kconfig
 115  create mode 100644 drivers/media/video/mrstci/mrstflash/Makefile
 116  create mode 100644 drivers/media/video/mrstci/mrstflash/mrstflash.c
 117  create mode 100644 drivers/media/video/mrstci/mrstov2650/Kconfig
 118  create mode 100644 drivers/media/video/mrstci/mrstov2650/Makefile
 119  create mode 100644 drivers/media/video/mrstci/mrstov2650/mrstov2650.c
 120  create mode 100644 drivers/media/video/mrstci/mrstov2650/ov2650.h
 121  create mode 100644 drivers/media/video/mrstci/mrstov5630/Kconfig
 122  create mode 100644 drivers/media/video/mrstci/mrstov5630/Makefile
 123  create mode 100644 drivers/media/video/mrstci/mrstov5630/ov5630.c
 124  create mode 100644 drivers/media/video/mrstci/mrstov5630/ov5630.h
 125  create mode 100644 drivers/media/video/mrstci/mrstov5630_motor/Kconfig
 126  create mode 100644 drivers/media/video/mrstci/mrstov5630_motor/Makefile
 127  create mode 100644 drivers/media/video/mrstci/mrstov5630_motor/mrstov5630_motor.c
 128  create mode 100644 drivers/media/video/mrstci/mrstov5630_motor/ov5630_motor.h
 129  create mode 100644 drivers/media/video/mrstci/mrstov9665/Kconfig
 130  create mode 100644 drivers/media/video/mrstci/mrstov9665/Makefile
 131  create mode 100644 drivers/media/video/mrstci/mrstov9665/mrstov9665.c
 132  create mode 100644 drivers/media/video/mrstci/mrstov9665/ov9665.h
 133  create mode 100755 drivers/media/video/mrstci/mrsts5k4e1/Kconfig
 134  create mode 100644 drivers/media/video/mrstci/mrsts5k4e1/Makefile
 135  create mode 100755 drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.c
 136  create mode 100755 drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.h
 137  create mode 100755 drivers/media/video/mrstci/mrsts5k4e1_motor/Kconfig
 138  create mode 100644 drivers/media/video/mrstci/mrsts5k4e1_motor/Makefile
 139  create mode 100644 drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.c
 140  create mode 100644 drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.h
 141
 142 diff --git a/drivers/media/video/mrstci/mrstflash/Kconfig b/drivers/media/video/mrstci/mrstflash/Kconfig
 143 new file mode 100644
 144 index 0000000..72099c5
 145 --- /dev/null
 146 +++ b/drivers/media/video/mrstci/mrstflash/Kconfig
 147 @@ -0,0 +1,9 @@
 148 +config VIDEO_MRST_FLASH
 149 +       tristate "Moorestown flash"
 150 +       depends on I2C && VIDEO_MRST_ISP
 151 +
 152 +       ---help---
 153 +         Say Y here if your platform support moorestown flash.
 154 +
 155 +         To compile this driver as a module, choose M here: the
 156 +         module will be called mrstov2650.ko.
 157 diff --git a/drivers/media/video/mrstci/mrstflash/Makefile b/drivers/media/video/mrstci/mrstflash/Makefile
 158 new file mode 100644
 159 index 0000000..068f638
 160 --- /dev/null
 161 +++ b/drivers/media/video/mrstci/mrstflash/Makefile
 162 @@ -0,0 +1,3 @@
 163 +obj-$(CONFIG_VIDEO_MRST_FLASH)  += mrstflash.o
 164 +
 165 +EXTRA_CFLAGS   +=      -I$(src)/../include
 166 diff --git a/drivers/media/video/mrstci/mrstflash/mrstflash.c b/drivers/media/video/mrstci/mrstflash/mrstflash.c
 167 new file mode 100644
 168 index 0000000..5611e6b
 169 --- /dev/null
 170 +++ b/drivers/media/video/mrstci/mrstflash/mrstflash.c
 171 @@ -0,0 +1,150 @@
 172 +/*
 173 + * Support for Moorestown Langwell Camera Imaging camera flash.
 174 + *
 175 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
 176 + *
 177 + * This program is free software; you can redistribute it and/or
 178 + * modify it under the terms of the GNU General Public License version
 179 + * 2 as published by the Free Software Foundation.
 180 + *
 181 + * This program is distributed in the hope that it will be useful,
 182 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
 183 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 184 + * GNU General Public License for more details.
 185 + *
 186 + * You should have received a copy of the GNU General Public License
 187 + * along with this program; if not, write to the Free Software
 188 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 189 + * 02110-1301, USA.
 190 + *
 191 + *
 192 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
 193 + */
 194 +
 195 +#include <linux/module.h>
 196 +#include <linux/types.h>
 197 +#include <linux/kernel.h>
 198 +#include <linux/mm.h>
 199 +#include <linux/string.h>
 200 +#include <linux/errno.h>
 201 +#include <linux/init.h>
 202 +#include <linux/kmod.h>
 203 +#include <linux/device.h>
 204 +#include <linux/delay.h>
 205 +#include <linux/fs.h>
 206 +#include <linux/init.h>
 207 +#include <linux/slab.h>
 208 +#include <linux/delay.h>
 209 +#include <linux/i2c.h>
 210 +#include <linux/gpio.h>
 211 +#include <linux/videodev2.h>
 212 +#include <media/v4l2-device.h>
 213 +#include <media/v4l2-chip-ident.h>
 214 +#include <media/v4l2-i2c-drv.h>
 215 +
 216 +static int debug;
 217 +module_param(debug, bool, 0644);
 218 +MODULE_PARM_DESC(debug, "Debug level (0-1)");
 219 +
 220 +MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
 221 +MODULE_DESCRIPTION("A low-level driver for mrst flash");
 222 +MODULE_LICENSE("GPL");
 223 +
 224 +static int flash_g_chip_ident(struct v4l2_subdev *sd,
 225 +               struct v4l2_dbg_chip_ident *chip)
 226 +{
 227 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 228 +
 229 +#define V4L2_IDENT_MRST_FLASH 8248
 230 +       return v4l2_chip_ident_i2c_client(client, chip,
 231 +                                         V4L2_IDENT_MRST_FLASH, 0);
 232 +}
 233 +
 234 +static const struct v4l2_subdev_core_ops flash_core_ops = {
 235 +       .g_chip_ident = flash_g_chip_ident,
 236 +};
 237 +static const struct v4l2_subdev_ops flash_ops = {
 238 +       .core = &flash_core_ops,
 239 +};
 240 +
 241 +static int flash_detect(struct i2c_client *client)
 242 +{
 243 +       struct i2c_adapter *adapter = client->adapter;
 244 +       u8 pid;
 245 +
 246 +       if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
 247 +               return -ENODEV;
 248 +
 249 +       if (adapter->nr != 0)
 250 +               return -ENODEV;
 251 +
 252 +       pid = i2c_smbus_read_byte_data(client, 0x10);
 253 +       if (pid == 0x18) {
 254 +               printk(KERN_ERR "camera flash device found\n");
 255 +               v4l_dbg(1, debug, client, "found camera flash device");
 256 +       } else {
 257 +               printk(KERN_ERR "no camera flash device found\n");
 258 +               return -ENODEV;
 259 +       }
 260 +
 261 +       return 0;
 262 +}
 263 +
 264 +static int flash_probe(struct i2c_client *client,
 265 +                       const struct i2c_device_id *id)
 266 +{
 267 +       u8 pid, ver;
 268 +       int ret = -1;
 269 +       struct v4l2_subdev *sd;
 270 +
 271 +       v4l_info(client, "chip found @ 0x%x (%s)\n",
 272 +                       client->addr << 1, client->adapter->name);
 273 +
 274 +       sd = kzalloc(sizeof(struct v4l2_subdev), GFP_KERNEL);
 275 +       ret = flash_detect(client);
 276 +       if (ret)
 277 +               return -ENODEV;
 278 +
 279 +       v4l2_i2c_subdev_init(sd, client, &flash_ops);
 280 +
 281 +       ver = i2c_smbus_read_byte_data(client, 0x50);
 282 +       v4l_dbg(1, debug, client, "detect:CST from device is 0x%x", ver);
 283 +       pid = i2c_smbus_read_byte_data(client, 0x20);
 284 +       v4l_dbg(1, debug, client, "detect:MFPC from device is 0x%x", pid);
 285 +       pid = i2c_smbus_read_byte_data(client, 0xA0);
 286 +       v4l_dbg(1, debug, client, "detect:TCC from device is 0x%x", pid);
 287 +       pid = i2c_smbus_read_byte_data(client, 0xB0);
 288 +       v4l_dbg(1, debug, client, "detect:FCC from device is 0x%x", pid);
 289 +       pid = i2c_smbus_read_byte_data(client, 0xC0);
 290 +       v4l_dbg(1, debug, client, "detect:FDC from device is 0x%x", pid);
 291 +       i2c_smbus_write_byte_data(client, 0xc0, 0xff); /*set FST to 1000us*/
 292 +       pid = i2c_smbus_read_byte_data(client, 0xc0);
 293 +       v4l_dbg(1, debug, client, "FDC from device is 0x%x", pid);
 294 +
 295 +       v4l_dbg(1, debug, client,
 296 +               "successfully load camera flash device driver");
 297 +       return 0;
 298 +}
 299 +
 300 +static int flash_remove(struct i2c_client *client)
 301 +{
 302 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 303 +
 304 +       v4l2_device_unregister_subdev(sd);
 305 +
 306 +       return 0;
 307 +}
 308 +
 309 +static const struct i2c_device_id flash_id[] = {
 310 +       {"mrst_camera_flash", 0},
 311 +       {}
 312 +};
 313 +
 314 +MODULE_DEVICE_TABLE(i2c, flash_id);
 315 +
 316 +static struct v4l2_i2c_driver_data v4l2_i2c_data = {
 317 +       .name = "mrst_camera_flash",
 318 +       .probe = flash_probe,
 319 +       .remove = flash_remove,
 320 +       .id_table = flash_id,
 321 +};
 322 diff --git a/drivers/media/video/mrstci/mrstov2650/Kconfig b/drivers/media/video/mrstci/mrstov2650/Kconfig
 323 new file mode 100644
 324 index 0000000..d39d894
 325 --- /dev/null
 326 +++ b/drivers/media/video/mrstci/mrstov2650/Kconfig
 327 @@ -0,0 +1,9 @@
 328 +config VIDEO_MRST_OV2650
 329 +       tristate "Moorestown OV2650 SoC Sensor"
 330 +       depends on I2C && VIDEO_MRST_ISP
 331 +
 332 +       ---help---
 333 +         Say Y here if your platform support OV2650 SoC Sensor.
 334 +
 335 +         To compile this driver as a module, choose M here: the
 336 +         module will be called mrstov2650.ko.
 337 diff --git a/drivers/media/video/mrstci/mrstov2650/Makefile b/drivers/media/video/mrstci/mrstov2650/Makefile
 338 new file mode 100644
 339 index 0000000..fb16d57
 340 --- /dev/null
 341 +++ b/drivers/media/video/mrstci/mrstov2650/Makefile
 342 @@ -0,0 +1,3 @@
 343 +obj-$(CONFIG_VIDEO_MRST_OV2650)         += mrstov2650.o
 344 +
 345 +EXTRA_CFLAGS   +=      -I$(src)/../include
 346 \ No newline at end of file
 347 diff --git a/drivers/media/video/mrstci/mrstov2650/mrstov2650.c b/drivers/media/video/mrstci/mrstov2650/mrstov2650.c
 348 new file mode 100644
 349 index 0000000..7f0d478
 350 --- /dev/null
 351 +++ b/drivers/media/video/mrstci/mrstov2650/mrstov2650.c
 352 @@ -0,0 +1,1190 @@
 353 +/*
 354 + * Support for Moorestown Langwell Camera Imaging ISP subsystem.
 355 + *
 356 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
 357 + *
 358 + * This program is free software; you can redistribute it and/or
 359 + * modify it under the terms of the GNU General Public License version
 360 + * 2 as published by the Free Software Foundation.
 361 + *
 362 + * This program is distributed in the hope that it will be useful,
 363 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
 364 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 365 + * GNU General Public License for more details.
 366 + *
 367 + * You should have received a copy of the GNU General Public License
 368 + * along with this program; if not, write to the Free Software
 369 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 370 + * 02110-1301, USA.
 371 + *
 372 + *
 373 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
 374 + */
 375 +
 376 +#include <linux/module.h>
 377 +#include <linux/types.h>
 378 +#include <linux/kernel.h>
 379 +#include <linux/mm.h>
 380 +#include <linux/string.h>
 381 +#include <linux/errno.h>
 382 +#include <linux/init.h>
 383 +#include <linux/kmod.h>
 384 +#include <linux/device.h>
 385 +#include <linux/delay.h>
 386 +#include <linux/fs.h>
 387 +#include <linux/init.h>
 388 +#include <linux/slab.h>
 389 +#include <linux/delay.h>
 390 +#include <linux/i2c.h>
 391 +#include <linux/gpio.h>
 392 +#include <linux/videodev2.h>
 393 +#include <media/v4l2-device.h>
 394 +#include <media/v4l2-chip-ident.h>
 395 +#include <media/v4l2-i2c-drv.h>
 396 +
 397 +#include "ci_sensor_common.h"
 398 +#include "ov2650.h"
 399 +
 400 +static int mrstov2650_debug;
 401 +module_param(mrstov2650_debug, int, 0644);
 402 +MODULE_PARM_DESC(mrstov2650_debug, "Debug level (0-1)");
 403 +
 404 +#define dprintk(level, fmt, arg...) do {                       \
 405 +       if (mrstov2650_debug >= level)                                  \
 406 +               printk(KERN_DEBUG "mrstisp@%s: " fmt "\n", \
 407 +                      __func__, ## arg); } \
 408 +       while (0)
 409 +
 410 +#define eprintk(fmt, arg...)   \
 411 +       printk(KERN_ERR "mrstisp@%s: line %d: " fmt "\n",       \
 412 +              __func__, __LINE__, ## arg);
 413 +
 414 +#define DBG_entering   dprintk(2, "entering");
 415 +#define DBG_leaving    dprintk(2, "leaving");
 416 +#define DBG_line       dprintk(2, " line: %d", __LINE__);
 417 +
 418 +static inline struct ci_sensor_config *to_sensor_config(struct v4l2_subdev *sd)
 419 +{
 420 +       return container_of(sd, struct ci_sensor_config, sd);
 421 +}
 422 +
 423 +static struct ov2650_format_struct {
 424 +       __u8 *desc;
 425 +       __u32 pixelformat;
 426 +       struct regval_list *regs;
 427 +} ov2650_formats[] = {
 428 +       {
 429 +               .desc           = "YUYV 4:2:2",
 430 +               .pixelformat    = SENSOR_MODE_BT601,
 431 +               .regs           = NULL,
 432 +       },
 433 +};
 434 +#define N_OV2650_FMTS ARRAY_SIZE(ov2650_formats)
 435 +
 436 +static struct ov2650_res_struct {
 437 +       __u8 *desc;
 438 +       int res;
 439 +       int width;
 440 +       int height;
 441 +       /* FIXME: correct the fps values.. */
 442 +       int fps;
 443 +       bool used;
 444 +       struct regval_list *regs;
 445 +} ov2650_res[] = {
 446 +       {
 447 +               .desc           = "UXGA",
 448 +               .res            = SENSOR_RES_UXGA,
 449 +               .width          = 1600,
 450 +               .height         = 1200,
 451 +               .fps            = 15,
 452 +               .used           = 0,
 453 +               .regs           = ov2650_res_uxga,
 454 +       },
 455 +       {
 456 +               .desc           = "SXGA",
 457 +               .res            = SENSOR_RES_SXGA,
 458 +               .width          = 1280,
 459 +               .height         = 1024,
 460 +               .fps            = 15,
 461 +               .used           = 0,
 462 +               .regs           = ov2650_res_sxga,
 463 +       },
 464 +       {
 465 +               .desc           = "SVGA",
 466 +               .res            = SENSOR_RES_SVGA,
 467 +               .width          = 800,
 468 +               .height         = 600,
 469 +               .fps            = 15,
 470 +               .used           = 0,
 471 +               .regs           = ov2650_res_svga,
 472 +       },
 473 +       {
 474 +               .desc           = "VGA",
 475 +               .res            = SENSOR_RES_VGA,
 476 +               .width          = 640,
 477 +               .height         = 480,
 478 +               .fps            = 15,
 479 +               .used           = 0,
 480 +               .regs           = ov2650_res_vga_vario,
 481 +       },
 482 +       {
 483 +               .desc           = "QVGA",
 484 +               .res            = SENSOR_RES_QVGA,
 485 +               .width          = 320,
 486 +               .height         = 240,
 487 +               .fps            = 15,
 488 +               .used           = 0,
 489 +               .regs           = ov2650_res_qvga,
 490 +       },
 491 +};
 492 +
 493 +#define N_RES (ARRAY_SIZE(ov2650_res))
 494 +
 495 +/*
 496 + * I2C Read & Write stuff
 497 + */
 498 +static int ov2650_read(struct i2c_client *c, u16 reg, u8 *value)
 499 +{
 500 +       int ret;
 501 +       int i;
 502 +       struct i2c_msg msg[2];
 503 +       u8 msgbuf[2];
 504 +       u8 ret_val = 0;
 505 +       *value = 0;
 506 +       /* Read needs two message to go */
 507 +       memset(&msg, 0, sizeof(msg));
 508 +       msgbuf[0] = 0;
 509 +       msgbuf[1] = 0;
 510 +       i = 0;
 511 +       msgbuf[i++] = reg >> 8;
 512 +       msgbuf[i++] = reg;
 513 +       msg[0].addr = c->addr;
 514 +       msg[0].buf = msgbuf;
 515 +       msg[0].len = i;
 516 +
 517 +       msg[1].addr = c->addr;
 518 +       msg[1].flags = I2C_M_RD;
 519 +       msg[1].buf = &ret_val;
 520 +       msg[1].len = 1;
 521 +
 522 +       ret = i2c_transfer(c->adapter, &msg[0], 2);
 523 +       *value = ret_val;
 524 +
 525 +       ret = (ret == 2) ? 0 : -1;
 526 +       return ret;
 527 +}
 528 +
 529 +static int ov2650_write(struct i2c_client *c, u16 reg, u8 value)
 530 +{
 531 +       int ret, i;
 532 +       struct i2c_msg msg;
 533 +       u8 msgbuf[3];
 534 +
 535 +       /* Writing only needs one message */
 536 +       memset(&msg, 0, sizeof(msg));
 537 +       i = 0;
 538 +       msgbuf[i++] = reg >> 8;
 539 +       msgbuf[i++] = reg;
 540 +       msgbuf[i++] = value;
 541 +
 542 +       msg.addr = c->addr;
 543 +       msg.flags = 0;
 544 +       msg.buf = msgbuf;
 545 +       msg.len = i;
 546 +
 547 +       ret = i2c_transfer(c->adapter, &msg, 1);
 548 +
 549 +       /* If this is a reset register, wait for 1ms */
 550 +       if (reg == OV2650_SYS && (value & 0x80))
 551 +               msleep(3);
 552 +
 553 +       ret = (ret == 1) ? 0 : -1;
 554 +       return ret;
 555 +}
 556 +
 557 +static int ov2650_write_array(struct i2c_client *c, struct regval_list *vals)
 558 +{
 559 +       struct regval_list *p;
 560 +       u8 read_val = 0;
 561 +       int err_num = 0;
 562 +       int i = 0;
 563 +       p = vals;
 564 +       while (p->reg_num != 0xffff) {
 565 +               ov2650_write(c, p->reg_num, p->value);
 566 +               ov2650_read(c, p->reg_num, &read_val);
 567 +               if (read_val != p->value)
 568 +                       err_num++;
 569 +               p++;
 570 +               i++;
 571 +       }
 572 +       return 0;
 573 +}
 574 +
 575 +static int ov2650_set_data_pin_in(struct i2c_client *client)
 576 +{
 577 +       int ret = 0;
 578 +       u8 reg;
 579 +
 580 +       ret += ov2650_write(client, 0x30b0, 0x00);
 581 +
 582 +       ret += ov2650_read(client, 0x30b1, &reg);
 583 +       reg &= 0xfc;
 584 +       ret += ov2650_write(client, 0x30b1, reg);
 585 +
 586 +       return ret;
 587 +}
 588 +
 589 +static int ov2650_set_data_pin_out(struct i2c_client *client)
 590 +{
 591 +       int ret = 0;
 592 +       u8 reg;
 593 +
 594 +       ret += ov2650_write(client, 0x30b0, 0xff);
 595 +
 596 +       ret += ov2650_read(client, 0x30b1, &reg);
 597 +       reg &= 0xfc;
 598 +       reg |= 0x03;
 599 +       ret += ov2650_write(client, 0x30b1, reg);
 600 +
 601 +       return ret;
 602 +}
 603 +/*
 604 + * Sensor specific helper function
 605 + */
 606 +static int ov2650_standby(void)
 607 +{
 608 +       gpio_set_value(GPIO_STDBY_PIN, 1);
 609 +       dprintk(1, "PM: standby called\n");
 610 +       return 0;
 611 +}
 612 +
 613 +static int ov2650_wakeup(void)
 614 +{
 615 +       gpio_set_value(GPIO_STDBY_PIN, 0);
 616 +       dprintk(1, "PM: wakeup called\n");
 617 +       return 0;
 618 +}
 619 +
 620 +static int ov2650_s_power(struct v4l2_subdev *sd, u32 val)
 621 +{
 622 +       if (val == 1)
 623 +               ov2650_standby();
 624 +       if (val == 0)
 625 +               ov2650_wakeup();
 626 +       return 0;
 627 +}
 628 +
 629 +static int ov2650_init(struct i2c_client *c)
 630 +{
 631 +       int ret;
 632 +       struct v4l2_subdev *sd = i2c_get_clientdata(c);
 633 +       struct ci_sensor_config *info = to_sensor_config(sd);
 634 +
 635 +       /* Fill the configuration structure */
 636 +       /* Note this default configuration value */
 637 +       info->mode = ov2650_formats[0].pixelformat;
 638 +       info->res = ov2650_res[0].res;
 639 +       info->type = SENSOR_TYPE_SOC;
 640 +       info->bls = SENSOR_BLS_OFF;
 641 +       info->gamma = SENSOR_GAMMA_ON;
 642 +       info->cconv = SENSOR_CCONV_ON;
 643 +       info->blc = SENSOR_BLC_AUTO;
 644 +       info->agc = SENSOR_AGC_AUTO;
 645 +       info->awb = SENSOR_AWB_AUTO;
 646 +       info->aec = SENSOR_AEC_AUTO;
 647 +       info->bus_width = SENSOR_BUSWIDTH_8BIT_ZZ;
 648 +       info->ycseq = SENSOR_YCSEQ_YCBYCR;
 649 +       info->conv422 = SENSOR_CONV422_COSITED;
 650 +       info->bpat = SENSOR_BPAT_BGBGGRGR;/* GRGRBGBG; */
 651 +       info->field_inv = SENSOR_FIELDINV_NOSWAP;
 652 +       info->field_sel = SENSOR_FIELDSEL_BOTH;
 653 +       info->hpol = SENSOR_HPOL_REFPOS;
 654 +       info->vpol = SENSOR_VPOL_POS;
 655 +       info->edge = SENSOR_EDGE_RISING;
 656 +       info->flicker_freq = SENSOR_FLICKER_100;
 657 +       info->cie_profile = 0;
 658 +       memcpy(info->name, "ov2650", 7);
 659 +
 660 +       ret = ov2650_write(c, OV2650_SYS, 0x80);
 661 +       /* Set registers into default config value */
 662 +       ret += ov2650_write_array(c, ov2650_def_reg);
 663 +
 664 +       /* added by wen to stop sensor from streaming */
 665 +       ov2650_write(c, 0x3086, 0x0f);
 666 +       ov2650_set_data_pin_in(c);
 667 +       ssleep(1);
 668 +
 669 +       return ret;
 670 +}
 671 +
 672 +static int distance(struct ov2650_res_struct *res, u32 w, u32 h)
 673 +{
 674 +       int ret;
 675 +       if (res->width < w || res->height < h)
 676 +               return -1;
 677 +
 678 +       ret = ((res->width - w) + (res->height - h));
 679 +       return ret;
 680 +}
 681 +
 682 +static int ov2650_try_res(u32 *w, u32 *h)
 683 +{
 684 +       struct ov2650_res_struct *res_index, *p = NULL;
 685 +       int dis, last_dis = ov2650_res->width + ov2650_res->height;
 686 +
 687 +       dprintk(1, "&&&&&  before %dx%d", *w, *h);
 688 +       for (res_index = ov2650_res;
 689 +            res_index < ov2650_res + N_RES;
 690 +            res_index++) {
 691 +               if ((res_index->width <= *w) && (res_index->height <= *h))
 692 +                       break;
 693 +               dis = distance(res_index, *w, *h);
 694 +               if (dis < last_dis) {
 695 +                       last_dis = dis;
 696 +                       p = res_index;
 697 +               }
 698 +       }
 699 +       if ((res_index->width < *w) || (res_index->height < *h)) {
 700 +               if (res_index != ov2650_res)
 701 +                       res_index--;
 702 +       }
 703 +
 704 +       /*
 705 +       if (p == NULL) {
 706 +               p = ov2650_res;
 707 +       }
 708 +
 709 +       if ((w != NULL) && (h != NULL)) {
 710 +               *w = p->width;
 711 +               *h = p->height;
 712 +       }
 713 +       */
 714 +       if (res_index == ov2650_res + N_RES)
 715 +               res_index = ov2650_res + N_RES - 1;
 716 +
 717 +       *w = res_index->width;
 718 +       *h = res_index->height;
 719 +
 720 +       dprintk(1, "&&&&&  after %dx%d", *w, *h);
 721 +       return 0;
 722 +}
 723 +
 724 +static struct ov2650_res_struct *ov2650_to_res(u32 w, u32 h)
 725 +{
 726 +       struct ov2650_res_struct *res_index;
 727 +
 728 +       for (res_index = ov2650_res;
 729 +            res_index < ov2650_res + N_RES;
 730 +            res_index++)
 731 +               if ((res_index->width == w) && (res_index->height == h))
 732 +                       break;
 733 +
 734 +       if (res_index >= ov2650_res + N_RES)
 735 +               res_index--;   /* Take the bigger one */
 736 +
 737 +       return res_index;
 738 +}
 739 +
 740 +static int ov2650_try_fmt(struct v4l2_subdev *sd,
 741 +                         struct v4l2_format *fmt)
 742 +{
 743 +       DBG_entering;
 744 +       dprintk(1, "&&&&&  before %dx%d", fmt->fmt.pix.width,
 745 +               fmt->fmt.pix.height);
 746 +       return ov2650_try_res(&fmt->fmt.pix.width, &fmt->fmt.pix.height);
 747 +       dprintk(1, "&&&&&  after %dx%d", fmt->fmt.pix.width,
 748 +               fmt->fmt.pix.height);
 749 +       DBG_leaving;
 750 +}
 751 +
 752 +static int ov2650_get_fmt(struct v4l2_subdev *sd,
 753 +                         struct v4l2_format *fmt)
 754 +{
 755 +       struct ci_sensor_config *info = to_sensor_config(sd);
 756 +       unsigned short width, height;
 757 +       int index;
 758 +
 759 +       ci_sensor_res2size(info->res, &width, &height);
 760 +
 761 +       /* Marked the current sensor res as being "used" */
 762 +       for (index = 0; index < N_RES; index++) {
 763 +               if ((width == ov2650_res[index].width) &&
 764 +                   (height == ov2650_res[index].height)) {
 765 +                       ov2650_res[index].used = 1;
 766 +                       continue;
 767 +               }
 768 +               ov2650_res[index].used = 0;
 769 +       }
 770 +
 771 +       fmt->fmt.pix.width = width;
 772 +       fmt->fmt.pix.height = height;
 773 +       return 0;
 774 +}
 775 +
 776 +static int ov2650_set_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
 777 +{
 778 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 779 +       struct ci_sensor_config *info = to_sensor_config(sd);
 780 +       int ret = 0;
 781 +       struct ov2650_res_struct *res_index;
 782 +       u32 width, height;
 783 +       int index;
 784 +
 785 +       DBG_entering;
 786 +
 787 +       width = fmt->fmt.pix.width;
 788 +       height = fmt->fmt.pix.height;
 789 +
 790 +       ret = ov2650_try_res(&width, &height);
 791 +       res_index = ov2650_to_res(width, height);
 792 +
 793 +       ov2650_wakeup();
 794 +
 795 +       /* if ((info->res != res_index->res) && (res_index->regs)) { */
 796 +       if (res_index->regs) {
 797 +
 798 +               dprintk(2, "changing res from to %dx%d", width, height);
 799 +               ret = ov2650_write(client, OV2650_SYS, 0x80);
 800 +               ret += ov2650_write_array(client, ov2650_def_reg);
 801 +               ret += ov2650_write_array(client, res_index->regs);
 802 +
 803 +               /* add to debug
 804 +               if(res_index->res == SENSOR_RES_VGA) {
 805 +                       ret += ov2650_write_array(c, ov2650_def_reg);
 806 +                       ret += ov2650_write_array(c, res_index->regs);
 807 +               } else {
 808 +                       ret += ov2650_write_array(c, ov2650_res_vga_reverse);
 809 +                       ret += ov2650_write_array(c, res_index->regs);
 810 +               }
 811 +               */
 812 +
 813 +               /* Add delay here to get better image */
 814 +               /*
 815 +               if (res_index->res == SENSOR_RES_SXGA ||
 816 +                  res_index->res == SENSOR_RES_UXGA)
 817 +                       msleep(2000);
 818 +               else
 819 +                       msleep(900);
 820 +                       */
 821 +
 822 +               /* Marked current sensor res as being "used" */
 823 +               for (index = 0; index < N_RES; index++) {
 824 +                       if ((width == ov2650_res[index].width) &&
 825 +                           (height == ov2650_res[index].height)) {
 826 +                               ov2650_res[index].used = 1;
 827 +                               continue;
 828 +                       }
 829 +                       ov2650_res[index].used = 0;
 830 +               }
 831 +
 832 +               for (index = 0; index < N_RES; index++)
 833 +                       dprintk(2, "index = %d, used = %d\n", index,
 834 +                               ov2650_res[index].used);
 835 +
 836 +       }
 837 +
 838 +       info->res = res_index->res;
 839 +
 840 +       /*
 841 +       int i;
 842 +       unsigned char value;
 843 +       printk(KERN_WARNING "2650 reg dumping start:\n");
 844 +       for(i = 0x3000; i <= 0x360B; i ++) {
 845 +               ov2650_read(c, i, &value);
 846 +               printk(KERN_WARNING "reg at offset %4x = %x\n", i, value);
 847 +       }
 848 +       printk(KERN_WARNING "2650 reg dumping finished:\n");
 849 +       */
 850 +
 851 +       DBG_leaving;
 852 +
 853 +       return ret;
 854 +}
 855 +
 856 +static int ov2650_q_hflip(struct v4l2_subdev *sd, __s32 *value)
 857 +{
 858 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 859 +       int err;
 860 +       unsigned char v;
 861 +
 862 +       err = ov2650_read(client, OV2650_TMC_6, &v);
 863 +       *value = (v & 0x02) == 0x02;
 864 +       return err;
 865 +}
 866 +
 867 +static int ov2650_t_hflip(struct v4l2_subdev *sd, int value)
 868 +{
 869 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 870 +       struct ci_sensor_config *info = to_sensor_config(sd);
 871 +       unsigned char v, v1 = 0;
 872 +       int err;
 873 +
 874 +       value = value >= 1 ? 1 : 0;
 875 +       err = ov2650_read(client, OV2650_TMC_6, &v);
 876 +       if (value) {
 877 +               v |= 0x02;
 878 +               v1 |= 0x08;
 879 +               info->bpat = SENSOR_BPAT_GRGRBGBG;/*BGBGGRGR;*/
 880 +       } else {
 881 +               v &= ~0x02;
 882 +               v1 &= ~0x08;
 883 +               info->bpat = SENSOR_BPAT_BGBGGRGR;
 884 +       }
 885 +       err += ov2650_write(client, OV2650_TMC_6, v);
 886 +       err += ov2650_write(client, 0x3090, v1);
 887 +       msleep(10);  /* FIXME */
 888 +
 889 +       return err;
 890 +}
 891 +
 892 +static int ov2650_q_vflip(struct v4l2_subdev *sd, __s32 *value)
 893 +{
 894 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 895 +       int err;
 896 +       unsigned char v;
 897 +
 898 +       err = ov2650_read(client, OV2650_TMC_6, &v);
 899 +       *value = (v & 0x01) == 0x01;
 900 +       return err;
 901 +}
 902 +
 903 +
 904 +static int ov2650_t_vflip(struct v4l2_subdev *sd, int value)
 905 +{
 906 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 907 +       int err = 0;
 908 +       unsigned char v;
 909 +
 910 +       value = value >= 1 ? 1 : 0;
 911 +       err = ov2650_read(client, OV2650_TMC_6, &v);
 912 +       if (value)
 913 +               v |= 0x01;
 914 +       else
 915 +               v &= ~0x01;
 916 +       err += ov2650_write(client, OV2650_TMC_6, v);
 917 +       msleep(10);  /* FIXME */
 918 +
 919 +       return err;
 920 +}
 921 +
 922 +#if 0
 923 +static int ov2650_t_awb(struct i2c_client *c, int value)
 924 +{
 925 +       unsigned char v;
 926 +       int ret;
 927 +       struct ci_sensor_config *info = i2c_get_clientdata(c);
 928 +
 929 +       value = value >= 1 ? 1 : 0;
 930 +       ret = ov2650_read(c, OV2650_ISP_CTL_0, &v);
 931 +       if (value & 0x01) {
 932 +               v |= 0x30;
 933 +               info->awb = SENSOR_AWB_AUTO;
 934 +       } else {
 935 +               v &= ~0x30;
 936 +               info->awb = SENSOR_AWB_OFF;
 937 +       }
 938 +       ret += ov2650_write(c, OV2650_ISP_CTL_0, v);
 939 +       msleep(10);  /* FIXME */
 940 +
 941 +       return ret;
 942 +}
 943 +
 944 +static int ov2650_q_awb(struct i2c_client *c, int *value)
 945 +{
 946 +       int ret;
 947 +       unsigned char v;
 948 +
 949 +       ret = ov2650_read(c, OV2650_ISP_CTL_0, &v);
 950 +       *value = (v & 0x30) == 0x30;
 951 +       return ret;
 952 +}
 953 +
 954 +static int ov2650_t_agc(struct i2c_client *c, int value)
 955 +{
 956 +       unsigned char v;
 957 +       int ret;
 958 +       struct ci_sensor_config *info = i2c_get_clientdata(c);
 959 +
 960 +       value = value >= 1 ? 1 : 0;
 961 +       ret = ov2650_read(c, OV2650_ISP_CTL_0, &v);
 962 +       if (value & 0x01) {
 963 +               v |= 0x10;
 964 +               info->agc = SENSOR_AGC_AUTO;
 965 +       } else {
 966 +               v &= ~0x10;
 967 +               info->agc = SENSOR_AGC_OFF;
 968 +       }
 969 +       ret += ov2650_write(c, OV2650_ISP_CTL_0, v);
 970 +       msleep(10);  /* FIXME */
 971 +
 972 +       return ret;
 973 +}
 974 +
 975 +static int ov2650_q_agc(struct i2c_client *c, int *value)
 976 +{
 977 +       int ret;
 978 +       unsigned char v;
 979 +
 980 +       ret = ov2650_read(c, OV2650_ISP_CTL_0, &v);
 981 +       *value = (v & 0x10) == 0x10;
 982 +       return ret;
 983 +}
 984 +
 985 +static int ov2650_t_blc(struct i2c_client *c, int value)
 986 +{
 987 +       unsigned char v;
 988 +       int ret;
 989 +
 990 +       value = value >= 1 ? 1 : 0;
 991 +
 992 +       ret = ov2650_read(c, OV2650_BLCC, &v);
 993 +       if (value & 0x01)
 994 +               v |= 0x10;
 995 +       else
 996 +               v &= ~0x10;
 997 +       ret += ov2650_write(c, OV2650_BLCC, v);
 998 +       msleep(10);  /* FIXME */
 999 +
1000 +       return ret;
1001 +}
1002 +
1003 +static int ov2650_q_blc(struct i2c_client *c, int *value)
1004 +{
1005 +       int ret;
1006 +       unsigned char v;
1007 +
1008 +       ret = ov2650_read(c, OV2650_BLCC, &v);
1009 +       *value = (v & 0x10) == 0x10;
1010 +       return ret;
1011 +}
1012 +#endif
1013 +
1014 +static struct ov2650_control {
1015 +       struct v4l2_queryctrl qc;
1016 +       int (*query)(struct v4l2_subdev *sd, __s32 *value);
1017 +       int (*tweak)(struct v4l2_subdev *sd, int value);
1018 +} ov2650_controls[] = {
1019 +       {
1020 +               .qc = {
1021 +                       .id = V4L2_CID_VFLIP,
1022 +                       .type = V4L2_CTRL_TYPE_BOOLEAN,
1023 +                       .name = "Vertical flip",
1024 +                       .minimum = 0,
1025 +                       .maximum = 1,
1026 +                       .step = 1,
1027 +                       .default_value = 0,
1028 +               },
1029 +               .tweak = ov2650_t_vflip,
1030 +               .query = ov2650_q_vflip,
1031 +       },
1032 +       {
1033 +               .qc = {
1034 +                       .id = V4L2_CID_HFLIP,
1035 +                       .type = V4L2_CTRL_TYPE_BOOLEAN,
1036 +                       .name = "Horizontal mirror",
1037 +                       .minimum = 0,
1038 +                       .maximum = 1,
1039 +                       .step = 1,
1040 +                       .default_value = 0,
1041 +               },
1042 +               .tweak = ov2650_t_hflip,
1043 +               .query = ov2650_q_hflip,
1044 +       },
1045 +#if 0
1046 +       {
1047 +               .parm = {
1048 +                       .index = V4L2_CID_AUTO_WHITE_BALANCE,
1049 +                       .type = V4L2_CTRL_TYPE_BOOLEAN,
1050 +                       .name = "Auto White Balance",
1051 +                       .min = 0,
1052 +                       .max = 1,
1053 +                       .step = 1,
1054 +                       .def_value = 1,
1055 +               },
1056 +               .tweak = ov2650_t_awb,
1057 +               .query = ov2650_q_awb,
1058 +       },
1059 +       {
1060 +               .parm = {
1061 +                       .index = V4L2_CID_AUTOGAIN,
1062 +                       .type = V4L2_CTRL_TYPE_BOOLEAN,
1063 +                       .name = "Auto Gain Control",
1064 +                       .min = 0,
1065 +                       .max = 1,
1066 +                       .step = 1,
1067 +                       .def_value = 1,
1068 +               },
1069 +               .tweak = ov2650_t_agc,
1070 +               .query = ov2650_q_agc,
1071 +
1072 +       },
1073 +       {
1074 +               .parm = {
1075 +                       .index = V4L2_CID_BLACK_LEVEL,
1076 +                       .type = V4L2_CTRL_TYPE_BOOLEAN,
1077 +                       .name = "Black Level Control",
1078 +                       .min = 0,
1079 +                       .max = 1,
1080 +                       .step = 1,
1081 +                       .def_value = 1,
1082 +               },
1083 +               .tweak = ov2650_t_blc,
1084 +               .query = ov2650_q_blc,
1085 +
1086 +       },
1087 +#endif
1088 +};
1089 +#define N_CONTROLS (ARRAY_SIZE(ov2650_controls))
1090 +
1091 +static struct ov2650_control *ov2650_find_control(__u32 id)
1092 +{
1093 +       int i;
1094 +
1095 +       for (i = 0; i < N_CONTROLS; i++)
1096 +               if (ov2650_controls[i].qc.id == id)
1097 +                       return ov2650_controls + i;
1098 +       return NULL;
1099 +}
1100 +
1101 +static int ov2650_queryctrl(struct v4l2_subdev *sd,
1102 +                           struct v4l2_queryctrl *qc)
1103 +{
1104 +       struct ov2650_control *octrl;
1105 +       octrl = ov2650_find_control(qc->id);
1106 +       if (NULL == octrl)
1107 +               return -EINVAL;
1108 +       *qc = octrl->qc;
1109 +       return 0;
1110 +}
1111 +
1112 +static int ov2650_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
1113 +{
1114 +       struct ov2650_control *octrl = ov2650_find_control(ctrl->id);
1115 +       int ret;
1116 +
1117 +       if (octrl == NULL)
1118 +               return -EINVAL;
1119 +       ret = octrl->query(sd, &ctrl->value);
1120 +       if (ret >= 0)
1121 +               return 0;
1122 +       return ret;
1123 +}
1124 +
1125 +static int ov2650_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
1126 +{
1127 +       struct ov2650_control *octrl = ov2650_find_control(ctrl->id);
1128 +       int ret;
1129 +
1130 +       if (octrl == NULL)
1131 +               return -EINVAL;
1132 +       ret =  octrl->tweak(sd, ctrl->value);
1133 +       if (ret >= 0)
1134 +               return 0;
1135 +       return ret;
1136 +}
1137 +#if 0
1138 +static int ov2650_get_caps(struct i2c_client *c, struct ci_sensor_caps *caps)
1139 +{
1140 +       if (caps == NULL)
1141 +               return -EIO;
1142 +
1143 +       caps->bus_width = SENSOR_BUSWIDTH_8BIT_ZZ;
1144 +       caps->mode      = SENSOR_MODE_BT601;
1145 +       caps->field_inv = SENSOR_FIELDINV_NOSWAP;
1146 +       caps->field_sel = SENSOR_FIELDSEL_BOTH;
1147 +       caps->ycseq     = SENSOR_YCSEQ_YCBYCR;
1148 +       caps->conv422   = SENSOR_CONV422_COSITED;
1149 +       caps->bpat      = SENSOR_BPAT_BGBGGRGR;
1150 +       caps->hpol      = SENSOR_HPOL_REFPOS;
1151 +       caps->vpol      = SENSOR_VPOL_POS;
1152 +       caps->edge      = SENSOR_EDGE_RISING;
1153 +       caps->bls       = SENSOR_BLS_OFF;
1154 +       caps->gamma     = SENSOR_GAMMA_ON;
1155 +       caps->cconv     = SENSOR_CCONV_ON;
1156 +       caps->res       = SENSOR_RES_UXGA | SENSOR_RES_SXGA | SENSOR_RES_SVGA
1157 +           | SENSOR_RES_VGA | SENSOR_RES_QVGA;
1158 +       caps->blc       = SENSOR_BLC_AUTO;
1159 +       caps->agc       = SENSOR_AGC_AUTO;
1160 +       caps->awb       = SENSOR_AWB_AUTO;
1161 +       caps->aec       = SENSOR_AEC_AUTO;
1162 +       caps->cie_profile       = 0;
1163 +       caps->flicker_freq      = SENSOR_FLICKER_100 | SENSOR_FLICKER_120;
1164 +       caps->type      = SENSOR_TYPE_SOC;
1165 +       /* caps->name   = "ov2650"; */
1166 +       strcpy(caps->name, "ov2650");
1167 +
1168 +       return 0;
1169 +}
1170 +
1171 +static int ov2650_get_config(struct i2c_client *c,
1172 +                            struct ci_sensor_config *config)
1173 +{
1174 +       struct ci_sensor_config *info = i2c_get_clientdata(c);
1175 +
1176 +       if (config == NULL) {
1177 +               printk(KERN_WARNING "sensor_get_config: NULL pointer\n");
1178 +               return -EIO;
1179 +       }
1180 +
1181 +       memcpy(config, info, sizeof(struct ci_sensor_config));
1182 +
1183 +       return 0;
1184 +}
1185 +
1186 +static int ov2650_setup(struct i2c_client *c,
1187 +                       const struct ci_sensor_config *config)
1188 +{
1189 +       int ret;
1190 +       struct ov2650_res_struct *res_index;
1191 +       struct ci_sensor_config *info = i2c_get_clientdata(c);
1192 +       u16 width, high;
1193 +
1194 +       /* Soft reset camera first*/
1195 +       ret = ov2650_write(c, OV2650_SYS, 0x80);
1196 +
1197 +       /* Set registers into default config value */
1198 +       ret += ov2650_write_array(c, ov2650_def_reg);
1199 +
1200 +       /* set image resolution */
1201 +       ci_sensor_res2size(config->res, &width, &high);
1202 +       ret += ov2650_try_res(c, &width, &high);
1203 +       res_index = ov2650_find_res(width, high);
1204 +       if (res_index->regs)
1205 +               ret += ov2650_write_array(c, res_index->regs);
1206 +       if (!ret)
1207 +               info->res = res_index->res;
1208 +
1209 +
1210 +       if (config->blc != info->blc) {
1211 +               ret += ov2650_t_blc(c, config->blc);
1212 +               info->blc = config->blc;
1213 +       }
1214 +
1215 +       if (config->agc != info->agc) {
1216 +               ret += ov2650_t_agc(c, config->agc);
1217 +               info->agc = config->agc;
1218 +       }
1219 +
1220 +       if (config->awb != info->awb) {
1221 +               ret += ov2650_t_awb(c, config->awb);
1222 +               info->awb = config->awb;
1223 +       }
1224 +       /* Add some delay here to get a better image*/
1225 +       if (res_index->res == SENSOR_RES_SXGA ||
1226 +           res_index->res == SENSOR_RES_UXGA)
1227 +               msleep(2000);
1228 +       else
1229 +               msleep(900);
1230 +
1231 +       return ret;
1232 +}
1233 +
1234 +/*
1235 + * File operation functions
1236 + */
1237 +
1238 +
1239 +
1240 +static int ov2650_open(struct i2c_setting *c, void *priv)
1241 +{
1242 +       struct i2c_client *client = c->sensor_client;
1243 +       /* Just wake up sensor */
1244 +       if (ov2650_wakeup())
1245 +               return -EIO;
1246 +       ov2650_init(client);
1247 +       /*Sleep sensor now*/
1248 +       ov2650_write(client, 0x3086, 0x0f);
1249 +
1250 +       /* set data pin to input */
1251 +       if (ov2650_set_data_pin_in(client))
1252 +               return -EIO;
1253 +
1254 +       return 0;
1255 +}
1256 +
1257 +static int ov2650_release(struct i2c_setting *c, void *priv)
1258 +{
1259 +       /* Just suspend the sensor */
1260 +       ov2650_standby();
1261 +       return 0;
1262 +}
1263 +
1264 +static int ov2650_on(struct i2c_setting *c)
1265 +{
1266 +       int ret;
1267 +
1268 +       /* Software wake up sensor */
1269 +       ret = ov2650_write(c->sensor_client, 0x3086, 0x00);
1270 +
1271 +       /* set data pin to output */
1272 +       return ret + ov2650_set_data_pin_out(c->sensor_client);
1273 +}
1274 +
1275 +static int ov2650_off(struct i2c_setting *c)
1276 +{
1277 +       int ret;
1278 +
1279 +       /* Software standby sensor */
1280 +       ret = ov2650_write(c->sensor_client, 0x3086, 0x0f);
1281 +
1282 +       /* set data pin to input */
1283 +       return ret + ov2650_set_data_pin_in(c->sensor_client);
1284 +}
1285 +
1286 +static struct sensor_device ov2650 = {
1287 +       .name   = "OV2650",
1288 +       .type   = SENSOR_TYPE_SOC,
1289 +       .minor  = -1,
1290 +       .open   = ov2650_open,
1291 +       .release = ov2650_release,
1292 +       .on = ov2650_on,
1293 +       .off = ov2650_off,
1294 +       .querycap = ov2650_get_caps,
1295 +       .get_config = ov2650_get_config,
1296 +       .set_config = ov2650_setup,
1297 +       .enum_parm = ov2650_queryctrl,
1298 +       .get_parm = ov2650_g_ctrl,
1299 +       .set_parm = ov2650_s_ctrl,
1300 +       .try_res = ov2650_try_res,
1301 +       .set_res = ov2650_set_res,
1302 +       .suspend = ov2650_standby,
1303 +       .resume = ov2650_wakeup,
1304 +       .get_ls_corr_config = NULL,
1305 +       .set_awb = NULL,
1306 +       .set_aec = NULL,
1307 +       .set_blc = NULL,
1308 +       /* TBC */
1309 +};
1310 +#endif
1311 +
1312 +static int ov2650_s_stream(struct v4l2_subdev *sd, int enable)
1313 +{
1314 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
1315 +
1316 +       DBG_entering;
1317 +
1318 +
1319 +       if (enable) {
1320 +               ov2650_write(client, 0x3086, 0x00);
1321 +               ov2650_set_data_pin_out(client);
1322 +               msleep(2000);
1323 +       } else {
1324 +               ov2650_write(client, 0x3086, 0x0f);
1325 +               ov2650_set_data_pin_in(client);
1326 +       }
1327 +
1328 +       DBG_leaving;
1329 +       return 0;
1330 +}
1331 +
1332 +static int ov2650_enum_framesizes(struct v4l2_subdev *sd,
1333 +                                 struct v4l2_frmsizeenum *fsize)
1334 +{
1335 +       unsigned int index = fsize->index;
1336 +
1337 +       DBG_entering;
1338 +
1339 +       if (index >= N_RES)
1340 +               return -EINVAL;
1341 +
1342 +       fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
1343 +       fsize->discrete.width = ov2650_res[index].width;
1344 +       fsize->discrete.height = ov2650_res[index].height;
1345 +       fsize->reserved[0] = ov2650_res[index].used;
1346 +
1347 +       DBG_leaving;
1348 +
1349 +       return 0;
1350 +}
1351 +
1352 +static int ov2650_enum_frameintervals(struct v4l2_subdev *sd,
1353 +                                     struct v4l2_frmivalenum *fival)
1354 +{
1355 +       unsigned int index = fival->index;
1356 +
1357 +       DBG_entering;
1358 +
1359 +       if (index >= N_RES)
1360 +               return -EINVAL;
1361 +
1362 +       fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
1363 +       fival->discrete.numerator = 1;
1364 +       fival->discrete.denominator = ov2650_res[index].fps;
1365 +
1366 +       DBG_leaving;
1367 +
1368 +       return 0;
1369 +}
1370 +static int ov2650_g_chip_ident(struct v4l2_subdev *sd,
1371 +               struct v4l2_dbg_chip_ident *chip)
1372 +{
1373 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
1374 +
1375 +#define V4L2_IDENT_OV2650 8244
1376 +       return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_OV2650, 0);
1377 +}
1378 +
1379 +#ifdef CONFIG_VIDEO_ADV_DEBUG
1380 +static int ov2650_g_register(struct v4l2_subdev *sd,
1381 +                            struct v4l2_dbg_register *reg)
1382 +{
1383 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
1384 +       unsigned char val = 0;
1385 +       int ret;
1386 +
1387 +       if (!v4l2_chip_match_i2c_client(client, &reg->match))
1388 +               return -EINVAL;
1389 +       if (!capable(CAP_SYS_ADMIN))
1390 +               return -EPERM;
1391 +       ret = ov2650_read(client, reg->reg & 0xffff, &val);
1392 +       reg->val = val;
1393 +       reg->size = 1;
1394 +       return ret;
1395 +}
1396 +
1397 +static int ov2650_s_register(struct v4l2_subdev *sd,
1398 +                            struct v4l2_dbg_register *reg)
1399 +{
1400 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
1401 +
1402 +       if (!v4l2_chip_match_i2c_client(client, &reg->match))
1403 +               return -EINVAL;
1404 +       if (!capable(CAP_SYS_ADMIN))
1405 +               return -EPERM;
1406 +       ov2650_write(client, reg->reg & 0xffff, reg->val & 0xff);
1407 +       return 0;
1408 +}
1409 +#endif
1410 +
1411 +static const struct v4l2_subdev_video_ops ov2650_video_ops = {
1412 +       .try_fmt = ov2650_try_fmt,
1413 +       .s_fmt = ov2650_set_fmt,
1414 +       .g_fmt = ov2650_get_fmt,
1415 +       .s_stream = ov2650_s_stream,
1416 +       .enum_framesizes = ov2650_enum_framesizes,
1417 +       .enum_frameintervals = ov2650_enum_frameintervals,
1418 +};
1419 +
1420 +static const struct v4l2_subdev_core_ops ov2650_core_ops = {
1421 +       .g_chip_ident = ov2650_g_chip_ident,
1422 +       .queryctrl = ov2650_queryctrl,
1423 +       .g_ctrl = ov2650_g_ctrl,
1424 +       .s_ctrl = ov2650_s_ctrl,
1425 +       .s_gpio = ov2650_s_power,
1426 +       /*.g_ext_ctrls = ov2650_g_ext_ctrls,*/
1427 +       /*.s_ext_ctrls = ov2650_s_ext_ctrls,*/
1428 +#ifdef CONFIG_VIDEO_ADV_DEBUG
1429 +       .g_register = ov2650_g_register,
1430 +       .s_register = ov2650_s_register,
1431 +#endif
1432 +};
1433 +
1434 +static const struct v4l2_subdev_ops ov2650_ops = {
1435 +       .core = &ov2650_core_ops,
1436 +       .video = &ov2650_video_ops,
1437 +};
1438 +
1439 +/*
1440 + * Basic i2c stuff
1441 + */
1442 +#if 0
1443 +static unsigned short normal_i2c[] = {I2C_OV2650 >> 1, I2C_CLIENT_END};
1444 +I2C_CLIENT_INSMOD;
1445 +
1446 +static struct i2c_driver ov2650_driver;
1447 +#endif
1448 +static int ov2650_detect(struct i2c_client *client)
1449 +{
1450 +       struct i2c_adapter *adapter = client->adapter;
1451 +       int adap_id = i2c_adapter_id(adapter);
1452 +       u8 value;
1453 +
1454 +       printk(KERN_WARNING "Now start ov2650 detect\n");
1455 +       if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
1456 +               return -ENODEV;
1457 +
1458 +       if (adap_id != 1)
1459 +               return -ENODEV;
1460 +
1461 +       /* if (ov2650_wakeup()) */
1462 +               /* return -ENODEV; */
1463 +       ov2650_wakeup();
1464 +
1465 +       ov2650_read(client, OV2650_PID_L, &value);
1466 +       if (value != 0x52)
1467 +               return -ENODEV;
1468 +
1469 +       return 0;
1470 +}
1471 +
1472 +static int ov2650_probe(struct i2c_client *client,
1473 +                       const struct i2c_device_id *id)
1474 +{
1475 +       struct ci_sensor_config *info;
1476 +       struct v4l2_subdev *sd;
1477 +       int ret = -1;
1478 +
1479 +       DBG_entering;
1480 +
1481 +       printk(KERN_INFO "Init ov2650 sensor \n");
1482 +
1483 +       v4l_info(client, "chip found @ 0x%x (%s)\n",
1484 +                       client->addr << 1, client->adapter->name);
1485 +       /*
1486 +        * Setup sensor configuration structure
1487 +        */
1488 +       info = kzalloc(sizeof(struct ci_sensor_config), GFP_KERNEL);
1489 +       if (!info)
1490 +               return -ENOMEM;
1491 +
1492 +       ret = ov2650_detect(client);
1493 +       if (ret) {
1494 +               kfree(info);
1495 +               return -ENODEV;
1496 +       }
1497 +
1498 +       sd = &info->sd;
1499 +       v4l2_i2c_subdev_init(sd, client, &ov2650_ops);
1500 +
1501 +       /*
1502 +        * TODO: Need to check if this can be here.
1503 +        * Turn into standby mode
1504 +        */
1505 +       /* ov2650_standby(); */
1506 +       ret += ov2650_init(client);
1507 +       ov2650_standby();
1508 +
1509 +       printk(KERN_INFO "Init ov2650 sensor success, ret = %d\n", ret);
1510 +
1511 +       DBG_leaving;
1512 +       return 0;
1513 +}
1514 +
1515 +static int ov2650_remove(struct i2c_client *client)
1516 +{
1517 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
1518 +
1519 +       v4l2_device_unregister_subdev(sd);
1520 +       kfree(to_sensor_config(sd));
1521 +       return 0;
1522 +}
1523 +
1524 +static const struct i2c_device_id ov2650_id[] = {
1525 +       {"ov2650", 0},
1526 +       {}
1527 +};
1528 +
1529 +MODULE_DEVICE_TABLE(i2c, ov2650_id);
1530 +
1531 +static struct v4l2_i2c_driver_data v4l2_i2c_data = {
1532 +       .name = "ov2650",
1533 +       .probe = ov2650_probe,
1534 +       .remove = ov2650_remove,
1535 +       /* .suspend = ov2650_suspend,
1536 +        * .resume = ov2650_resume, */
1537 +       .id_table = ov2650_id,
1538 +};
1539 +
1540 +MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
1541 +MODULE_DESCRIPTION("A low-level driver for OmniVision 2650 sensors");
1542 +MODULE_LICENSE("GPL");
1543 diff --git a/drivers/media/video/mrstci/mrstov2650/ov2650.h b/drivers/media/video/mrstci/mrstov2650/ov2650.h
1544 new file mode 100644
1545 index 0000000..f5c0418
1546 --- /dev/null
1547 +++ b/drivers/media/video/mrstci/mrstov2650/ov2650.h
1548 @@ -0,0 +1,766 @@
1549 +/*
1550 + * Support for Moorestown Langwell Camera Imaging ISP subsystem.
1551 + *
1552 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
1553 + *
1554 + * This program is free software; you can redistribute it and/or
1555 + * modify it under the terms of the GNU General Public License version
1556 + * 2 as published by the Free Software Foundation.
1557 + *
1558 + * This program is distributed in the hope that it will be useful,
1559 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
1560 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
1561 + * GNU General Public License for more details.
1562 + *
1563 + * You should have received a copy of the GNU General Public License
1564 + * along with this program; if not, write to the Free Software
1565 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
1566 + * 02110-1301, USA.
1567 + *
1568 + *
1569 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
1570 + */
1571 +
1572 +#define I2C_OV2650     0x60
1573 +/* Should add to kernel source */
1574 +#define I2C_DRIVERID_OV2650    1047
1575 +/* GPIO pin on Moorestown */
1576 +#define GPIO_SCLK_25   44
1577 +#define GPIO_STB_PIN   47
1578 +#define GPIO_STDBY_PIN 48
1579 +#define GPIO_RESET_PIN 50
1580 +
1581 +/* System control register */
1582 +#define OV2650_AGC             0x3000
1583 +#define OV2650_AGCS    0x3001
1584 +#define OV2650_AEC_H   0x3002
1585 +#define OV2650_AEC_L   0x3003
1586 +#define OV2650_AECL    0x3004
1587 +#define OV2650_AECS_H  0x3008
1588 +#define OV2650_AECS_L  0x3009
1589 +#define OV2650_PID_H   0x300A
1590 +#define OV2650_PID_L   0x300B
1591 +#define OV2650_SCCB            0x300C
1592 +#define OV2650_PCLK            0x300D
1593 +#define OV2650_PLL_1   0x300E
1594 +#define OV2650_PLL_2   0x300F
1595 +#define OV2650_PLL_3   0x3010
1596 +#define OV2650_CLK             0x3011
1597 +#define OV2650_SYS             0x3012
1598 +#define OV2650_AUTO_1  0x3013
1599 +#define OV2650_AUTO_2  0x3014
1600 +#define OV2650_AUTO_3  0x3015
1601 +#define OV2650_AUTO_4  0x3016
1602 +#define OV2650_AUTO_5  0x3017
1603 +#define OV2650_WPT             0x3018
1604 +#define OV2650_BPT             0x3019
1605 +#define OV2650_VPT             0x301A
1606 +#define OV2650_YAVG            0x301B
1607 +#define OV2650_AECG_50 0x301C
1608 +#define OV2650_AECG_60 0x301D
1609 +#define OV2650_RZM_H   0x301E
1610 +#define OV2650_RZM_L   0x301F
1611 +#define OV2650_HS_H            0x3020
1612 +#define OV2650_HS_L            0x3021
1613 +#define OV2650_VS_H            0x3022
1614 +#define OV2650_VS_L            0x3023
1615 +#define OV2650_HW_H            0x3024
1616 +#define OV2650_HW_L            0x3025
1617 +#define OV2650_VH_H            0x3026
1618 +#define OV2650_VH_L            0x3027
1619 +#define OV2650_HTS_H   0x3028
1620 +#define OV2650_HTS_L   0x3029
1621 +#define OV2650_VTS_H   0x302A
1622 +#define OV2650_VTS_L   0x302B
1623 +#define OV2650_EXHTS   0x302C
1624 +#define OV2650_EXVTS_H 0x302D
1625 +#define OV2650_EXVTS_L 0x302E
1626 +#define OV2650_WET_0   0x3030
1627 +#define OV2650_WET_1   0x3031
1628 +#define OV2650_WET_2   0x3032
1629 +#define OV2650_WET_3   0x3033
1630 +#define OV2650_AHS_H   0x3038
1631 +#define OV2650_AHS_L   0x3039
1632 +#define OV2650_AVS_H   0x303A
1633 +#define OV2650_AVS_L   0x303B
1634 +#define OV2650_AHW_H   0x303C
1635 +#define OV2650_AHW_L   0x303D
1636 +#define OV2650_AVH_H   0x303E
1637 +#define OV2650_AVH_L   0x303F
1638 +#define OV2650_HISTO_0 0x3040
1639 +#define OV2650_HISTO_1 0x3041
1640 +#define OV2650_HISTO_2 0x3042
1641 +#define OV2650_HISTO_3 0x3043
1642 +#define OV2650_HISTO_4 0x3044
1643 +#define OV2650_BLC9A   0x3069
1644 +#define OV2650_BLCC            0x306C
1645 +#define OV2650_BLCD            0x306D
1646 +#define OV2650_BLCF            0x306F
1647 +#define OV2650_BD50_L  0x3070
1648 +#define OV2650_BD50_H  0x3071
1649 +#define OV2650_BD60_L  0x3072
1650 +#define OV2650_BD60_H  0x3073
1651 +#define OV2650_TMC_0   0x3076
1652 +#define OV2650_TMC_1   0x3077
1653 +#define OV2650_TMC_2   0x3078
1654 +#define OV2650_TMC_4   0x307A
1655 +#define OV2650_TMC_6   0x307C
1656 +#define OV2650_TMC_8   0x307E
1657 +#define OV2650_TMC_I2C 0x3084
1658 +#define OV2650_TMC_10  0x3086
1659 +#define OV2650_TMC_11  0x3087
1660 +#define OV2650_ISP_XO_H        0x3088
1661 +#define OV2650_ISP_XO_L        0x3089
1662 +#define OV2650_ISP_YO_H        0x308A
1663 +#define OV2650_ISP_YO_L        0x308B
1664 +#define OV2650_TMC_12  0x308C
1665 +#define OV2650_TMC_13  0x308D
1666 +#define OV2650_EFUSE   0x308F
1667 +#define OV2650_IO_CTL_0        0x30B0
1668 +#define OV2650_IO_CRL_1 0x30B1
1669 +#define OV2650_IO_CTL_2 0x30B2
1670 +#define OV2650_LAEC            0x30F0
1671 +#define OV2650_GRP_EOP 0x30FF
1672 +
1673 +/* SC registers */
1674 +#define OV2650_SC_CTL_0        0x3100
1675 +#define OV2650_SC_SYN_CTL_0 0x3104
1676 +#define OV2650_SC_SYN_CTL_1 0x3105
1677 +#define OV2650_SC_SYN_CTL_3 0x3107
1678 +#define OV2650_SC_SYN_CTL_4 0x3108
1679 +
1680 +/* DSP control register */
1681 +#define OV2650_ISP_CTL_0       0x3300
1682 +#define OV2650_ISP_CTL_1       0x3301
1683 +#define OV2650_ISP_CTL_2       0x3302
1684 +#define OV2650_ISP_CTL_3       0x3303
1685 +#define OV2650_ISP_CTL_4       0x3304
1686 +#define OV2650_ISP_CTL_5       0x3305
1687 +#define OV2650_ISP_CTL_6       0x3306
1688 +#define OV2650_ISP_CTL_7       0x3307
1689 +#define OV2650_ISP_CTL_8       0x3308
1690 +#define OV2650_ISP_CTL_9       0x3309
1691 +#define OV2650_ISP_CTL_A       0x330A
1692 +#define OV2650_ISP_CTL_B       0x330B
1693 +#define OV2650_ISP_CTL_10      0x3310
1694 +#define OV2650_ISP_CTL_11      0x3311
1695 +#define OV2650_ISP_CTL_12      0x3312
1696 +#define OV2650_ISP_CTL_13      0x3313
1697 +#define OV2650_ISP_CTL_14      0x3314
1698 +#define OV2650_ISP_CTL_15      0x3315
1699 +#define OV2650_ISP_CTL_16      0x3316
1700 +#define OV2650_ISP_CTL_17      0x3317
1701 +#define OV2650_ISP_CTL_18      0x3318
1702 +#define OV2650_ISP_CTL_19      0x3319
1703 +#define OV2650_ISP_CTL_1A      0x331A
1704 +#define OV2650_ISP_CTL_1B      0x331B
1705 +#define OV2650_ISP_CTL_1C      0x331C
1706 +#define OV2650_ISP_CTL_1D      0x331D
1707 +#define OV2650_ISP_CTL_1E      0x331E
1708 +#define OV2650_ISP_CTL_20      0x3320
1709 +#define OV2650_ISP_CTL_21      0x3321
1710 +#define OV2650_ISP_CTL_22      0x3322
1711 +#define OV2650_ISP_CTL_23      0x3323
1712 +#define OV2650_ISP_CTL_24      0x3324
1713 +#define OV2650_ISP_CTL_27      0x3327
1714 +#define OV2650_ISP_CTL_28      0x3328
1715 +#define OV2650_ISP_CTL_29      0x3329
1716 +#define OV2650_ISP_CTL_2A      0x332A
1717 +#define OV2650_ISP_CTL_2B      0x332B
1718 +#define OV2650_ISP_CTL_2C      0x332C
1719 +#define OV2650_ISP_CTL_2D      0x332D
1720 +#define OV2650_ISP_CTL_2E      0x332E
1721 +#define OV2650_ISP_CTL_2F      0x332F
1722 +#define OV2650_ISP_CTL_30      0x3330
1723 +#define OV2650_ISP_CTL_31      0x3331
1724 +#define OV2650_ISP_CTL_32      0x3332
1725 +#define OV2650_ISP_CTL_33      0x3333
1726 +#define OV2650_ISP_CTL_34      0x3334
1727 +#define OV2650_ISP_CTL_35      0x3335
1728 +#define OV2650_ISP_CTL_36      0x3336
1729 +#define OV2650_ISP_CTL_40      0x3340
1730 +#define OV2650_ISP_CTL_41      0x3341
1731 +#define OV2650_ISP_CTL_42      0x3342
1732 +#define OV2650_ISP_CTL_43      0x3343
1733 +#define OV2650_ISP_CTL_44      0x3344
1734 +#define OV2650_ISP_CTL_45      0x3345
1735 +#define OV2650_ISP_CTL_46      0x3346
1736 +#define OV2650_ISP_CTL_47      0x3347
1737 +#define OV2650_ISP_CTL_48      0x3348
1738 +#define OV2650_ISP_CTL_49      0x3349
1739 +#define OV2650_ISP_CTL_4A      0x334A
1740 +#define OV2650_ISP_CTL_4B      0x334B
1741 +#define OV2650_ISP_CTL_4C      0x334C
1742 +#define OV2650_ISP_CTL_4D      0x334D
1743 +#define OV2650_ISP_CTL_4E      0x334E
1744 +#define OV2650_ISP_CTL_4F      0x334F
1745 +#define OV2650_ISP_CTL_50      0x3350
1746 +#define OV2650_ISP_CTL_51      0x3351
1747 +#define OV2650_ISP_CTL_52      0x3352
1748 +#define OV2650_ISP_CTL_53      0x3353
1749 +#define OV2650_ISP_CTL_54      0x3354
1750 +#define OV2650_ISP_CTL_55      0x3355
1751 +#define OV2650_ISP_CTL_56      0x3356
1752 +#define OV2650_ISP_CTL_57      0x3357
1753 +#define OV2650_ISP_CTL_58      0x3358
1754 +#define OV2650_ISP_CTL_59      0x3359
1755 +#define OV2650_ISP_CTL_5A      0x335A
1756 +#define OV2650_ISP_CTL_5B      0x335B
1757 +#define OV2650_ISP_CTL_5C      0x335C
1758 +#define OV2650_ISP_CTL_5D      0x335D
1759 +#define OV2650_ISP_CTL_5E      0x335E
1760 +#define OV2650_ISP_CTL_5F      0x335F
1761 +#define OV2650_ISP_CTL_60      0x3360
1762 +#define OV2650_ISP_CTL_61      0x3361
1763 +#define OV2650_ISP_CTL_62      0x3362
1764 +#define OV2650_ISP_CTL_63      0x3363
1765 +#define OV2650_ISP_CTL_64      0x3364
1766 +#define OV2650_ISP_CTL_65      0x3365
1767 +#define OV2650_ISP_CTL_6A      0x336A
1768 +#define OV2650_ISP_CTL_6B      0x336B
1769 +#define OV2650_ISP_CTL_6C      0x336C
1770 +#define OV2650_ISP_CTL_6E      0x336E
1771 +#define OV2650_ISP_CTL_71      0x3371
1772 +#define OV2650_ISP_CTL_72      0x3372
1773 +#define OV2650_ISP_CTL_73      0x3373
1774 +#define OV2650_ISP_CTL_74      0x3374
1775 +#define OV2650_ISP_CTL_75      0x3375
1776 +#define OV2650_ISP_CTL_76      0x3376
1777 +#define OV2650_ISP_CTL_77      0x3377
1778 +#define OV2650_ISP_CTL_78      0x3378
1779 +#define OV2650_ISP_CTL_79      0x3379
1780 +#define OV2650_ISP_CTL_7A      0x337A
1781 +#define OV2650_ISP_CTL_7B      0x337B
1782 +#define OV2650_ISP_CTL_7C      0x337C
1783 +#define OV2650_ISP_CTL_80      0x3380
1784 +#define OV2650_ISP_CTL_81      0x3381
1785 +#define OV2650_ISP_CTL_82      0x3382
1786 +#define OV2650_ISP_CTL_83      0x3383
1787 +#define OV2650_ISP_CTL_84      0x3384
1788 +#define OV2650_ISP_CTL_85      0x3385
1789 +#define OV2650_ISP_CTL_86      0x3386
1790 +#define OV2650_ISP_CTL_87      0x3387
1791 +#define OV2650_ISP_CTL_88      0x3388
1792 +#define OV2650_ISP_CTL_89      0x3389
1793 +#define OV2650_ISP_CTL_8A      0x338A
1794 +#define OV2650_ISP_CTL_8B      0x338B
1795 +#define OV2650_ISP_CTL_8C      0x338C
1796 +#define OV2650_ISP_CTL_8D      0x338D
1797 +#define OV2650_ISP_CTL_8E      0x338E
1798 +#define OV2650_ISP_CTL_90      0x3390
1799 +#define OV2650_ISP_CTL_91      0x3391
1800 +#define OV2650_ISP_CTL_92      0x3392
1801 +#define OV2650_ISP_CTL_93      0x3393
1802 +#define OV2650_ISP_CTL_94      0x3394
1803 +#define OV2650_ISP_CTL_95      0x3395
1804 +#define OV2650_ISP_CTL_96      0x3396
1805 +#define OV2650_ISP_CTL_97      0x3397
1806 +#define OV2650_ISP_CTL_98      0x3398
1807 +#define OV2650_ISP_CTL_99      0x3399
1808 +#define OV2650_ISP_CTL_9A      0x339A
1809 +#define OV2650_ISP_CTL_A0      0x33A0
1810 +#define OV2650_ISP_CTL_A1      0x33A1
1811 +#define OV2650_ISP_CTL_A2      0x33A2
1812 +#define OV2650_ISP_CTL_A3      0x33A3
1813 +#define OV2650_ISP_CTL_A4      0x33A4
1814 +#define OV2650_ISP_CTL_A5      0x33A5
1815 +#define OV2650_ISP_CTL_A6      0x33A6
1816 +#define OV2650_ISP_CTL_A7      0x33A7
1817 +#define OV2650_ISP_CTL_A8      0x33A8
1818 +#define OV2650_ISP_CTL_AA      0x33AA
1819 +#define OV2650_ISP_CTL_AB      0x33AB
1820 +#define OV2650_ISP_CTL_AC      0x33AC
1821 +#define OV2650_ISP_CTL_AD      0x33AD
1822 +#define OV2650_ISP_CTL_AE      0x33AE
1823 +#define OV2650_ISP_CTL_AF      0x33AF
1824 +#define OV2650_ISP_CTL_B0      0x33B0
1825 +#define OV2650_ISP_CTL_B1      0x33B1
1826 +#define OV2650_ISP_CTL_B2      0x33B2
1827 +#define OV2650_ISP_CTL_B3      0x33B3
1828 +#define OV2650_ISP_CTL_B4      0x33B4
1829 +#define OV2650_ISP_CTL_B5      0x33B5
1830 +#define OV2650_ISP_CTL_B6      0x33B6
1831 +#define OV2650_ISP_CTL_B7      0x33B7
1832 +#define OV2650_ISP_CTL_B8      0x33B8
1833 +#define OV2650_ISP_CTL_B9      0x33B9
1834 +
1835 +/* Format register */
1836 +#define OV2650_FMT_CTL_0       0x3400
1837 +#define OV2650_FMT_CTL_1       0x3401
1838 +#define OV2650_FMT_CTL_2       0x3402
1839 +#define OV2650_FMT_CTL_3       0x3403
1840 +#define OV2650_FMT_CTL_4       0x3404
1841 +#define OV2650_FMT_CTL_5       0x3405
1842 +#define OV2650_FMT_CTL_6       0x3406
1843 +#define OV2650_FMT_CTL_7       0x3407
1844 +#define OV2650_FMT_CTL_8       0x3408
1845 +#define OV2650_DITHER_CTL      0x3409
1846 +#define OV2650_DVP_CTL_0       0x3600
1847 +#define OV2650_DVP_CTL_1       0x3601
1848 +#define OV2650_DVP_CTL_6       0x3606
1849 +#define OV2650_DVP_CTL_7       0x3607
1850 +#define OV2650_DVP_CTL_9       0x3609
1851 +#define OV2650_DVP_CTL_B       0x360B
1852 +
1853 +/* General definition for ov2650 */
1854 +#define OV2650_OUTWND_MAX_H            UXGA_SIZE_H
1855 +#define OV2650_OUTWND_MAX_V            UXGA_SIZE_V
1856 +
1857 +struct regval_list {
1858 +       u16 reg_num;
1859 +       u8 value;
1860 +};
1861 +
1862 +/*
1863 + * Default register value
1864 + * 1600x1200 YUV
1865 + */
1866 +static struct regval_list ov2650_def_reg[] = {
1867 +       {0x3012, 0x80},
1868 +       {0x308c, 0x80},
1869 +       {0x308d, 0x0e},
1870 +       {0x360b, 0x00},
1871 +       {0x30b0, 0xff},
1872 +       {0x30b1, 0xff},
1873 +       {0x30b2, 0x27},
1874 +
1875 +       {0x300e, 0x34},
1876 +       {0x300f, 0xa6},
1877 +       {0x3010, 0x81},
1878 +       {0x3082, 0x01},
1879 +       {0x30f4, 0x01},
1880 +       {0x3090, 0x33},
1881 +       {0x3091, 0xc0},
1882 +       {0x30ac, 0x42},
1883 +
1884 +       {0x30d1, 0x08},
1885 +       {0x30a8, 0x56},
1886 +       {0x3015, 0x03},
1887 +       {0x3093, 0x00},
1888 +       {0x307e, 0xe5},
1889 +       {0x3079, 0x00},
1890 +       {0x30aa, 0x42},
1891 +       {0x3017, 0x40},
1892 +       {0x30f3, 0x82},
1893 +       {0x306a, 0x0c},
1894 +       {0x306d, 0x00},
1895 +       {0x336a, 0x3c},
1896 +       {0x3076, 0x6a},
1897 +       {0x30d9, 0x8c},
1898 +       {0x3016, 0x82},
1899 +       {0x3601, 0x30},
1900 +       {0x304e, 0x88},
1901 +       {0x30f1, 0x82},
1902 +       {0x3011, 0x02},
1903 +
1904 +       {0x3013, 0xf7},
1905 +       {0x301c, 0x13},
1906 +       {0x301d, 0x17},
1907 +       {0x3070, 0x3e},
1908 +       {0x3072, 0x34},
1909 +
1910 +       {0x30af, 0x00},
1911 +       {0x3048, 0x1f},
1912 +       {0x3049, 0x4e},
1913 +       {0x304a, 0x20},
1914 +       {0x304f, 0x20},
1915 +       {0x304b, 0x02},
1916 +       {0x304c, 0x00},
1917 +       {0x304d, 0x02},
1918 +       {0x304f, 0x20},
1919 +       {0x30a3, 0x10},
1920 +       {0x3013, 0xf7},
1921 +       {0x3014, 0x44},
1922 +       {0x3071, 0x00},
1923 +       {0x3070, 0x3e},
1924 +       {0x3073, 0x00},
1925 +       {0x3072, 0x34},
1926 +       {0x301c, 0x12},
1927 +       {0x301d, 0x16},
1928 +       {0x304d, 0x42},
1929 +       {0x304a, 0x40},
1930 +       {0x304f, 0x40},
1931 +       {0x3095, 0x07},
1932 +       {0x3096, 0x16},
1933 +       {0x3097, 0x1d},
1934 +
1935 +       {0x3020, 0x01},
1936 +       {0x3021, 0x18},
1937 +       {0x3022, 0x00},
1938 +       {0x3023, 0x0a},
1939 +       {0x3024, 0x06},
1940 +       {0x3025, 0x58},
1941 +       {0x3026, 0x04},
1942 +       {0x3027, 0xbc},
1943 +       {0x3088, 0x06},
1944 +       {0x3089, 0x40},
1945 +       {0x308a, 0x04},
1946 +       {0x308b, 0xb0},
1947 +       {0x3316, 0x64},
1948 +       {0x3317, 0x4b},
1949 +       {0x3318, 0x00},
1950 +       {0x331a, 0x64},
1951 +       {0x331b, 0x4b},
1952 +       {0x331c, 0x00},
1953 +       {0x3100, 0x00},
1954 +
1955 +       {0x3320, 0xfa},
1956 +       {0x3321, 0x11},
1957 +       {0x3322, 0x92},
1958 +       {0x3323, 0x01},
1959 +       {0x3324, 0x97},
1960 +       {0x3325, 0x02},
1961 +       {0x3326, 0xff},
1962 +       {0x3327, 0x0c},
1963 +       {0x3328, 0x10},
1964 +       {0x3329, 0x10},
1965 +       {0x332a, 0x58},
1966 +       {0x332b, 0x50},
1967 +       {0x332c, 0xbe},
1968 +       {0x332d, 0xe1},
1969 +       {0x332e, 0x43},
1970 +       {0x332f, 0x36},
1971 +       {0x3330, 0x4d},
1972 +       {0x3331, 0x44},
1973 +       {0x3332, 0xf8},
1974 +       {0x3333, 0x0a},
1975 +       {0x3334, 0xf0},
1976 +       {0x3335, 0xf0},
1977 +       {0x3336, 0xf0},
1978 +       {0x3337, 0x40},
1979 +       {0x3338, 0x40},
1980 +       {0x3339, 0x40},
1981 +       {0x333a, 0x00},
1982 +       {0x333b, 0x00},
1983 +
1984 +       {0x3380, 0x28},
1985 +       {0x3381, 0x48},
1986 +       {0x3382, 0x10},
1987 +       {0x3383, 0x23},
1988 +       {0x3384, 0xc0},
1989 +       {0x3385, 0xe5},
1990 +       {0x3386, 0xc2},
1991 +       {0x3387, 0xb3},
1992 +       {0x3388, 0x0e},
1993 +       {0x3389, 0x98},
1994 +       {0x338a, 0x01},
1995 +
1996 +       {0x3340, 0x0e},
1997 +       {0x3341, 0x1a},
1998 +       {0x3342, 0x31},
1999 +       {0x3343, 0x45},
2000 +       {0x3344, 0x5a},
2001 +       {0x3345, 0x69},
2002 +       {0x3346, 0x75},
2003 +       {0x3347, 0x7e},
2004 +       {0x3348, 0x88},
2005 +       {0x3349, 0x96},
2006 +       {0x334a, 0xa3},
2007 +       {0x334b, 0xaf},
2008 +       {0x334c, 0xc4},
2009 +       {0x334d, 0xd7},
2010 +       {0x334e, 0xe8},
2011 +       {0x334f, 0x20},
2012 +
2013 +       {0x3350, 0x32},
2014 +       {0x3351, 0x25},
2015 +       {0x3352, 0x80},
2016 +       {0x3353, 0x1e},
2017 +       {0x3354, 0x00},
2018 +       {0x3355, 0x85},
2019 +       {0x3356, 0x32},
2020 +       {0x3357, 0x25},
2021 +       {0x3358, 0x80},
2022 +       {0x3359, 0x1b},
2023 +       {0x335a, 0x00},
2024 +       {0x335b, 0x85},
2025 +       {0x335c, 0x32},
2026 +       {0x335d, 0x25},
2027 +       {0x335e, 0x80},
2028 +       {0x335f, 0x1b},
2029 +       {0x3360, 0x00},
2030 +       {0x3361, 0x85},
2031 +       {0x3363, 0x70},
2032 +       {0x3364, 0x7f},
2033 +       {0x3365, 0x00},
2034 +       {0x3366, 0x00},
2035 +
2036 +       {0x3301, 0xff},
2037 +       {0x338B, 0x11},
2038 +       {0x338c, 0x10},
2039 +       {0x338d, 0x40},
2040 +
2041 +       {0x3370, 0xd0},
2042 +       {0x3371, 0x00},
2043 +       {0x3372, 0x00},
2044 +       {0x3373, 0x40},
2045 +       {0x3374, 0x10},
2046 +       {0x3375, 0x10},
2047 +       {0x3376, 0x04},
2048 +       {0x3377, 0x00},
2049 +       {0x3378, 0x04},
2050 +       {0x3379, 0x80},
2051 +
2052 +       {0x3069, 0x84},
2053 +       {0x307c, 0x10},
2054 +       {0x3087, 0x02},
2055 +
2056 +       {0x3300, 0xfc},
2057 +       {0x3302, 0x01},
2058 +       {0x3400, 0x00},
2059 +       {0x3606, 0x20},
2060 +       {0x3601, 0x30},
2061 +       {0x30f3, 0x83},
2062 +       {0x304e, 0x88},
2063 +
2064 +       {0x3086, 0x0f},
2065 +       {0x3086, 0x00},
2066 +
2067 +       {0xffff, 0xff},
2068 +};
2069 +
2070 +/* 800x600 */
2071 +static struct regval_list ov2650_res_svga[] = {
2072 +
2073 +       {0x306f, 0x14},
2074 +       {0x302a, 0x02},
2075 +       {0x302b, 0x84},
2076 +       {0x3012, 0x10},
2077 +       {0x3011, 0x01},
2078 +
2079 +       {0x3070, 0x5d},
2080 +       {0x3072, 0x4d},
2081 +
2082 +       {0x3014, 0x84},
2083 +       {0x301c, 0x07},
2084 +       {0x301d, 0x09},
2085 +       {0x3070, 0x50},
2086 +       {0x3071, 0x00},
2087 +       {0x3072, 0x42},
2088 +       {0x3073, 0x00},
2089 +
2090 +       {0x3020, 0x01},
2091 +       {0x3021, 0x18},
2092 +       {0x3022, 0x00},
2093 +       {0x3023, 0x06},
2094 +       {0x3024, 0x06},
2095 +       {0x3025, 0x58},
2096 +       {0x3026, 0x02},
2097 +       {0x3027, 0x5e},
2098 +       {0x3088, 0x03},
2099 +       {0x3089, 0x20},
2100 +       {0x308a, 0x02},
2101 +       {0x308b, 0x58},
2102 +       {0x3316, 0x64},
2103 +       {0x3317, 0x25},
2104 +       {0x3318, 0x80},
2105 +       {0x3319, 0x08},
2106 +       {0x331a, 0x64},
2107 +       {0x331b, 0x4b},
2108 +       {0x331c, 0x00},
2109 +       {0x331d, 0x38},
2110 +       {0x3100, 0x00},
2111 +
2112 +       {0x3302, 0x11},
2113 +
2114 +       {0x3011, 0x01},
2115 +       {0x300f, 0xa6},
2116 +       {0x300e, 0x36},
2117 +       {0x3010, 0x81},
2118 +       {0x302e, 0x00},
2119 +       {0x302d, 0x00},
2120 +       {0x302c, 0x00},
2121 +       {0x302b, 0x84},
2122 +       {0x3014, 0x84},
2123 +       {0x301c, 0x07},
2124 +       {0x301d, 0x09},
2125 +       {0x3070, 0x50},
2126 +       {0x3071, 0x00},
2127 +       {0x3072, 0x42},
2128 +       {0x3073, 0x00},
2129 +
2130 +       {0x3086, 0x0f},
2131 +       {0x3086, 0x00},
2132 +       {0xffff, 0xff},
2133 +};
2134 +
2135 +/* 640x480 */
2136 +static struct regval_list ov2650_res_vga_vario[] = {
2137 +       {0x306f, 0x14},
2138 +       {0x302a, 0x02},
2139 +       {0x302b, 0x6a},
2140 +       {0x3012, 0x10},
2141 +       {0x3011, 0x01},
2142 +
2143 +       {0x3070, 0x5d},
2144 +       {0x3072, 0x4d},
2145 +
2146 +       {0x301c, 0x05},
2147 +       {0x301d, 0x06},
2148 +
2149 +       {0x3020, 0x01},
2150 +       {0x3021, 0x18},
2151 +       {0x3022, 0x00},
2152 +       {0x3023, 0x06},
2153 +       {0x3024, 0x06},
2154 +       {0x3025, 0x58},
2155 +       {0x3026, 0x02},
2156 +       {0x3027, 0x61},
2157 +       {0x3088, 0x02},
2158 +       {0x3089, 0x80},
2159 +       {0x308a, 0x01},
2160 +       {0x308b, 0xe0},
2161 +       {0x3316, 0x64},
2162 +       {0x3317, 0x25},
2163 +       {0x3318, 0x80},
2164 +       {0x3319, 0x08},
2165 +       {0x331a, 0x28},
2166 +       {0x331b, 0x1e},
2167 +       {0x331c, 0x00},
2168 +       {0x331d, 0x38},
2169 +       {0x3100, 0x00},
2170 +
2171 +       {0x3302, 0x11},
2172 +       {0x3011, 0x00},
2173 +
2174 +       {0x3014, 0x84}, /* note this */
2175 +       {0x3086, 0x0f},
2176 +       {0x3086, 0x00},
2177 +       {0xffff, 0xff},
2178 +};
2179 +
2180 +/* 640x480 reverse */
2181 +/*
2182 +static struct regval_list ov2650_res_vga_reverse[] = {
2183 +       {0x306f, 0x10},
2184 +       {0x302a, 0x04},
2185 +       {0x302b, 0xd4},
2186 +       {0x3012, 0x00},
2187 +       {0x3011, 0x02},
2188 +
2189 +       {0x3070, 0x3e},
2190 +       {0x3072, 0x34},
2191 +
2192 +       {0x301c, 0x12},
2193 +       {0x301d, 0x16},
2194 +
2195 +       {0x3020, 0x01},
2196 +       {0x3021, 0x18},
2197 +       {0x3022, 0x00},
2198 +       {0x3023, 0x0a},
2199 +       {0x3024, 0x06},
2200 +       {0x3025, 0x58},
2201 +       {0x3026, 0x04},
2202 +       {0x3027, 0xbc},
2203 +       {0x3088, 0x06},
2204 +       {0x3089, 0x40},
2205 +       {0x308a, 0x04},
2206 +       {0x308b, 0xb0},
2207 +       {0x3316, 0x64},
2208 +       {0x3317, 0xb4},
2209 +       {0x3318, 0x00},
2210 +       {0x3319, 0x6c},
2211 +       {0x331a, 0x64},
2212 +       {0x331b, 0x4b},
2213 +       {0x331c, 0x00},
2214 +       {0x331d, 0x6c},
2215 +       {0x3100, 0x00},
2216 +
2217 +       {0x3302, 0x01},
2218 +       {0x3011, 0x02},
2219 +
2220 +       {0x3014, 0x44},
2221 +       {0x3086, 0x0f},
2222 +       {0x3086, 0x00},
2223 +       {0xffff, 0xff},
2224 +};
2225 +
2226 +*/
2227 +/* 320x240 */
2228 +static struct regval_list ov2650_res_qvga[] = {
2229 +       {0x306f, 0x14},
2230 +       {0x302a, 0x02},
2231 +       {0x302b, 0x6a},
2232 +
2233 +       {0x3012, 0x10},
2234 +       {0x3011, 0x01},
2235 +
2236 +       {0x3070, 0x5d},
2237 +       {0x3072, 0x4d},
2238 +       {0x301c, 0x05},
2239 +       {0x301d, 0x06},
2240 +
2241 +       {0x3023, 0x06},
2242 +       {0x3026, 0x02},
2243 +       {0x3027, 0x61},
2244 +       {0x3088, 0x01},
2245 +       {0x3089, 0x40},
2246 +       {0x308a, 0x00},
2247 +       {0x308b, 0xf0},
2248 +       {0x3316, 0x64},
2249 +       {0x3317, 0x25},
2250 +       {0x3318, 0x80},
2251 +       {0x3319, 0x08},
2252 +       {0x331a, 0x14},
2253 +       {0x331b, 0x0f},
2254 +       {0x331c, 0x00},
2255 +       {0x331d, 0x38},
2256 +       {0x3100, 0x00},
2257 +
2258 +       {0x3015, 0x02}, /* note this */
2259 +       {0x3014, 0x84},
2260 +       {0x3302, 0x11},
2261 +       {0x3086, 0x0f},
2262 +       {0x3086, 0x00},
2263 +       {0xffff, 0xff},
2264 +};
2265 +
2266 +static struct regval_list ov2650_res_uxga[] = {
2267 +       /* Note this added by debug */
2268 +       {0x3014, 0x84},
2269 +       {0x301c, 0x13},
2270 +       {0x301d, 0x17},
2271 +       {0x3070, 0x40},
2272 +       {0x3071, 0x00},
2273 +       {0x3072, 0x36},
2274 +       {0x3073, 0x00},
2275 +
2276 +       {0xffff, 0xff},
2277 +};
2278 +
2279 +static struct regval_list ov2650_res_sxga[] = {
2280 +       {0x3011, 0x02},
2281 +
2282 +       {0x3020, 0x01},
2283 +       {0x3021, 0x18},
2284 +       {0x3022, 0x00},
2285 +       {0x3023, 0x0a},
2286 +       {0x3024, 0x06},
2287 +       {0x3025, 0x58},
2288 +       {0x3026, 0x04},
2289 +       {0x3027, 0xbc},
2290 +       {0x3088, 0x05},
2291 +       {0x3089, 0x00},
2292 +       {0x308a, 0x04},
2293 +       {0x308b, 0x00},
2294 +       {0x3316, 0x64},
2295 +       {0x3317, 0x4b},
2296 +       {0x3318, 0x00},
2297 +       {0x331a, 0x50},
2298 +       {0x331b, 0x40},
2299 +       {0x331c, 0x00},
2300 +
2301 +       {0x3302, 0x11},
2302 +
2303 +       {0x3014, 0x84},
2304 +       {0x301c, 0x13},
2305 +       {0x301d, 0x17},
2306 +       {0x3070, 0x40},
2307 +       {0x3071, 0x00},
2308 +       {0x3072, 0x36},
2309 +       {0x3073, 0x00},
2310 +
2311 +       {0x3086, 0x0f},
2312 +       {0x3086, 0x00},
2313 +       {0xffff, 0xff},
2314 +};
2315 diff --git a/drivers/media/video/mrstci/mrstov5630/Kconfig b/drivers/media/video/mrstci/mrstov5630/Kconfig
2316 new file mode 100644
2317 index 0000000..a28ddc2
2318 --- /dev/null
2319 +++ b/drivers/media/video/mrstci/mrstov5630/Kconfig
2320 @@ -0,0 +1,9 @@
2321 +config VIDEO_MRST_OV5630
2322 +       tristate "Moorestown OV5630 RAW Sensor"
2323 +       depends on I2C && VIDEO_MRST_ISP
2324 +
2325 +       ---help---
2326 +         Say Y here if your platform support OV5630 RAW Sensor.
2327 +
2328 +         To compile this driver as a module, choose M here: the
2329 +         module will be called mrstov2650.ko.
2330 diff --git a/drivers/media/video/mrstci/mrstov5630/Makefile b/drivers/media/video/mrstci/mrstov5630/Makefile
2331 new file mode 100644
2332 index 0000000..c67abff
2333 --- /dev/null
2334 +++ b/drivers/media/video/mrstci/mrstov5630/Makefile
2335 @@ -0,0 +1,4 @@
2336 +mrstov5630-objs        = ov5630.o
2337 +obj-$(CONFIG_VIDEO_MRST_OV5630)         += mrstov5630.o
2338 +
2339 +EXTRA_CFLAGS   +=      -I$(src)/../include
2340 diff --git a/drivers/media/video/mrstci/mrstov5630/ov5630.c b/drivers/media/video/mrstci/mrstov5630/ov5630.c
2341 new file mode 100644
2342 index 0000000..6498153
2343 --- /dev/null
2344 +++ b/drivers/media/video/mrstci/mrstov5630/ov5630.c
2345 @@ -0,0 +1,1153 @@
2346 +/*
2347 + * Support for Moorestown Langwell Camera Imaging ISP subsystem.
2348 + *
2349 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
2350 + *
2351 + * This program is free software; you can redistribute it and/or
2352 + * modify it under the terms of the GNU General Public License version
2353 + * 2 as published by the Free Software Foundation.
2354 + *
2355 + * This program is distributed in the hope that it will be useful,
2356 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
2357 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
2358 + * GNU General Public License for more details.
2359 + *
2360 + * You should have received a copy of the GNU General Public License
2361 + * along with this program; if not, write to the Free Software
2362 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
2363 + * 02110-1301, USA.
2364 + *
2365 + *
2366 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
2367 + */
2368 +
2369 +#include <linux/module.h>
2370 +#include <linux/types.h>
2371 +#include <linux/kernel.h>
2372 +#include <linux/mm.h>
2373 +#include <linux/string.h>
2374 +#include <linux/errno.h>
2375 +#include <linux/init.h>
2376 +#include <linux/kmod.h>
2377 +#include <linux/device.h>
2378 +#include <linux/delay.h>
2379 +#include <linux/fs.h>
2380 +#include <linux/init.h>
2381 +#include <linux/slab.h>
2382 +#include <linux/delay.h>
2383 +#include <linux/i2c.h>
2384 +#include <linux/gpio.h>
2385 +
2386 +#include <media/v4l2-device.h>
2387 +#include <media/v4l2-chip-ident.h>
2388 +#include <media/v4l2-i2c-drv.h>
2389 +
2390 +#include "ci_sensor_common.h"
2391 +#include "ov5630.h"
2392 +
2393 +static int mrstov5630_debug;
2394 +module_param(mrstov5630_debug, int, 0644);
2395 +MODULE_PARM_DESC(mrstov5630_debug, "Debug level (0-1)");
2396 +
2397 +#define dprintk(level, fmt, arg...) do {                       \
2398 +       if (mrstov5630_debug >= level)                                  \
2399 +               printk(KERN_DEBUG "mrstisp@%s: " fmt "\n",      \
2400 +                      __func__, ## arg); } \
2401 +       while (0)
2402 +
2403 +#define eprintk(fmt, arg...)   \
2404 +       printk(KERN_ERR "mrstisp@%s: line %d: " fmt "\n",       \
2405 +              __func__, __LINE__, ## arg);
2406 +
2407 +#define DBG_entering   dprintk(2, "entering");
2408 +#define DBG_leaving    dprintk(2, "leaving");
2409 +#define DBG_line       dprintk(2, " line: %d", __LINE__);
2410 +
2411 +static inline struct ci_sensor_config *to_sensor_config(struct v4l2_subdev *sd)
2412 +{
2413 +       return container_of(sd, struct ci_sensor_config, sd);
2414 +}
2415 +
2416 +/* static int ov5630_set_res(struct i2c_client *c, const int w, const int h);
2417 + */
2418 +static struct ov5630_format_struct {
2419 +       __u8 *desc;
2420 +       __u32 pixelformat;
2421 +       struct regval_list *regs;
2422 +} ov5630_formats[] = {
2423 +       {
2424 +               .desc           = "Raw RGB Bayer",
2425 +               .pixelformat    = SENSOR_MODE_BAYER,
2426 +               .regs           = NULL,
2427 +       },
2428 +};
2429 +#define N_OV5630_FMTS ARRAY_SIZE(ov5630_formats)
2430 +
2431 +static struct ov5630_res_struct {
2432 +       __u8 *desc;
2433 +       int res;
2434 +       int width;
2435 +       int height;
2436 +       /* FIXME: correct the fps values.. */
2437 +       int fps;
2438 +       bool used;
2439 +       struct regval_list *regs;
2440 +} ov5630_res[] = {
2441 +       {
2442 +               .desc           = "QSXGA_PLUS4",
2443 +               .res            = SENSOR_RES_QXGA_PLUS,
2444 +               .width          = 2592,
2445 +               .height         = 1944,
2446 +               .fps            = 15,
2447 +               .used           = 0,
2448 +               .regs           = ov5630_res_qsxga_plus4,
2449 +       },
2450 +       {
2451 +               .desc           = "1080P",
2452 +               .res            = SENSOR_RES_1080P,
2453 +               .width          = 1920,
2454 +               .height         = 1080,
2455 +               .fps            = 25,
2456 +               .used           = 0,
2457 +               .regs           = ov5630_res_1080p,
2458 +       },
2459 +       {
2460 +               .desc           = "XGA_PLUS",
2461 +               .res            = SENSOR_RES_XGA_PLUS,
2462 +               .width          = 1280,
2463 +               .height         = 960,
2464 +               .fps            = 30,
2465 +               .used           = 0,
2466 +               .regs           = ov5630_res_xga_plus,
2467 +       },
2468 +       {
2469 +               .desc           = "720p",
2470 +               .res            = SENSOR_RES_720P,
2471 +               .width          = 1280,
2472 +               .height         = 720,
2473 +               .fps            = 34,
2474 +               .used           = 0,
2475 +               .regs           = ov5630_res_720p,
2476 +       },
2477 +       {
2478 +               .desc           = "VGA",
2479 +               .res            = SENSOR_RES_VGA,
2480 +               .width          = 640,
2481 +               .height         = 480,
2482 +               .fps            = 39,
2483 +               .used           = 0,
2484 +               .regs           = ov5630_res_vga_ac04_bill,
2485 +       },
2486 +};
2487 +
2488 +#define N_RES (ARRAY_SIZE(ov5630_res))
2489 +
2490 +/*
2491 + * I2C Read & Write stuff
2492 + */
2493 +static int ov5630_read(struct i2c_client *c, u32 reg, u32 *value)
2494 +{
2495 +       int ret;
2496 +       int i;
2497 +       struct i2c_msg msg[2];
2498 +       u8 msgbuf[2];
2499 +       u8 ret_val = 0;
2500 +       *value = 0;
2501 +       /* Read needs two message to go */
2502 +       memset(&msg, 0, sizeof(msg));
2503 +       msgbuf[0] = 0;
2504 +       msgbuf[1] = 0;
2505 +       i = 0;
2506 +
2507 +       msgbuf[i++] = ((u16)reg) >> 8;
2508 +       msgbuf[i++] = ((u16)reg) & 0xff;
2509 +       msg[0].addr = c->addr;
2510 +       msg[0].buf = msgbuf;
2511 +       msg[0].len = i;
2512 +
2513 +       msg[1].addr = c->addr;
2514 +       msg[1].flags = I2C_M_RD;
2515 +       msg[1].buf = &ret_val;
2516 +       msg[1].len = 1;
2517 +
2518 +       ret = i2c_transfer(c->adapter, &msg[0], 2);
2519 +       *value = ret_val;
2520 +
2521 +       ret = (ret == 2) ? 0 : -1;
2522 +       return ret;
2523 +}
2524 +
2525 +static int ov5630_write(struct i2c_client *c, u32 reg, u32 value)
2526 +{
2527 +       int ret, i;
2528 +       struct i2c_msg msg;
2529 +       u8 msgbuf[3];
2530 +
2531 +       /* Writing only needs one message */
2532 +       memset(&msg, 0, sizeof(msg));
2533 +       i = 0;
2534 +       msgbuf[i++] = ((u16)reg) >> 8;
2535 +       msgbuf[i++] = (u16)reg & 0xff;
2536 +       msgbuf[i++] = (u8)value;
2537 +
2538 +       msg.addr = c->addr;
2539 +       msg.flags = 0;
2540 +       msg.buf = msgbuf;
2541 +       msg.len = i;
2542 +
2543 +       ret = i2c_transfer(c->adapter, &msg, 1);
2544 +
2545 +       /* If this is a reset register, wait for 1ms */
2546 +       if (reg == OV5630_SYS && (value & 0x80))
2547 +               msleep(3);
2548 +
2549 +       ret = (ret == 1) ? 0 : -1;
2550 +       return ret;
2551 +}
2552 +
2553 +static int ov5630_write_array(struct i2c_client *c, struct regval_list *vals)
2554 +{
2555 +       struct regval_list *p;
2556 +       u32 read_val = 0;
2557 +       int err_num = 0;
2558 +       int i = 0;
2559 +       p = vals;
2560 +       while (p->reg_num != 0xffff) {
2561 +               ov5630_write(c, (u32)p->reg_num, (u32)p->value);
2562 +               ov5630_read(c, (u32)p->reg_num, &read_val);
2563 +               if (read_val != p->value)
2564 +                       err_num++;
2565 +               p++;
2566 +               i++;
2567 +       }
2568 +       return 0;
2569 +}
2570 +
2571 +/*
2572 + * Sensor specific helper function
2573 + */
2574 +static int ov5630_standby(void)
2575 +{
2576 +       gpio_set_value(GPIO_STDBY_PIN, 1);
2577 +       /* ov5630_motor_standby(); */
2578 +       dprintk(1, "PM: standby called\n");
2579 +       return 0;
2580 +}
2581 +
2582 +static int ov5630_wakeup(void)
2583 +{
2584 +       gpio_set_value(GPIO_STDBY_PIN, 0);
2585 +       /* ov5630_motor_wakeup(); */
2586 +       dprintk(1, "PM: wakeup called\n");
2587 +       return 0;
2588 +}
2589 +
2590 +static int ov5630_s_power(struct v4l2_subdev *sd, u32 val)
2591 +{
2592 +       if (val == 1)
2593 +               ov5630_standby();
2594 +       if (val == 0)
2595 +               ov5630_wakeup();
2596 +       return 0;
2597 +}
2598 +
2599 +static int ov5630_set_img_ctrl(struct i2c_client *c,
2600 +                              const struct ci_sensor_config *config)
2601 +{
2602 +       int err = 0;
2603 +       u32 reg_val = 0;
2604 +       /* struct ci_sensor_config *info = i2c_get_clientdata(c); */
2605 +
2606 +       switch (config->blc) {
2607 +       case SENSOR_BLC_OFF:
2608 +               err |= ov5630_read(c, OV5630_ISP_CTL00, &reg_val);
2609 +               err |= ov5630_write(c, OV5630_ISP_CTL00, reg_val & 0xFE);
2610 +               break;
2611 +       case SENSOR_BLC_AUTO:
2612 +               err |= ov5630_read(c, OV5630_ISP_CTL00, &reg_val);
2613 +               err |= ov5630_write(c, OV5630_ISP_CTL00, reg_val | 0x01);
2614 +               break;
2615 +       }
2616 +
2617 +       switch (config->agc) {
2618 +       case SENSOR_AGC_AUTO:
2619 +               err |= ov5630_read(c, OV5630_AUTO_1, &reg_val);
2620 +               err |= ov5630_write(c, OV5630_AUTO_1, reg_val | 0x04);
2621 +               break;
2622 +       case SENSOR_AGC_OFF:
2623 +               err |= ov5630_read(c, OV5630_AUTO_1, &reg_val);
2624 +               err |= ov5630_write(c, OV5630_AUTO_1, reg_val & ~0x04);
2625 +               break;
2626 +       }
2627 +
2628 +       switch (config->awb) {
2629 +       case SENSOR_AWB_AUTO:
2630 +               err |= ov5630_read(c, OV5630_ISP_CTL00, &reg_val);
2631 +               err |= ov5630_write(c, OV5630_ISP_CTL00, reg_val | 0x30);
2632 +               break;
2633 +       case SENSOR_AWB_OFF:
2634 +               err |= ov5630_read(c, OV5630_ISP_CTL00, &reg_val);
2635 +               err |= ov5630_write(c, OV5630_ISP_CTL00, reg_val & ~0x30);
2636 +               break;
2637 +       }
2638 +
2639 +       switch (config->aec) {
2640 +       case SENSOR_AEC_AUTO:
2641 +               err |= ov5630_read(c, OV5630_AUTO_1, &reg_val);
2642 +               err |= ov5630_write(c, OV5630_AUTO_1, reg_val | 0xFB);
2643 +               break;
2644 +       case SENSOR_AEC_OFF:
2645 +               err |= ov5630_read(c, OV5630_AUTO_1, &reg_val);
2646 +               err |= ov5630_write(c, OV5630_AUTO_1, reg_val & 0xF6);
2647 +               break;
2648 +       }
2649 +
2650 +       return err;
2651 +}
2652 +
2653 +static int ov5630_init(struct i2c_client *c)
2654 +{
2655 +       int ret;
2656 +       struct v4l2_subdev *sd = i2c_get_clientdata(c);
2657 +       struct ci_sensor_config *info = to_sensor_config(sd);
2658 +       char *name = "";
2659 +
2660 +       /* Fill the configuration structure */
2661 +       /* Note this default configuration value */
2662 +       info->mode = ov5630_formats[0].pixelformat;
2663 +       info->res = ov5630_res[0].res;
2664 +       info->type = SENSOR_TYPE_RAW;
2665 +       info->bls = SENSOR_BLS_OFF;
2666 +       info->gamma = SENSOR_GAMMA_OFF;
2667 +       info->cconv = SENSOR_CCONV_OFF;
2668 +       info->blc = SENSOR_BLC_AUTO;
2669 +       info->agc = SENSOR_AGC_AUTO;
2670 +       info->awb = SENSOR_AWB_AUTO;
2671 +       info->aec = SENSOR_AEC_AUTO;
2672 +       /* info->bus_width = SENSOR_BUSWIDTH_10BIT; */
2673 +       info->bus_width = SENSOR_BUSWIDTH_10BIT_ZZ;
2674 +       info->ycseq = SENSOR_YCSEQ_YCBYCR;
2675 +       /* info->conv422 = SENSOR_CONV422_NOCOSITED; */
2676 +       info->conv422 = SENSOR_CONV422_COSITED;
2677 +       info->bpat = SENSOR_BPAT_BGBGGRGR;
2678 +       info->field_inv = SENSOR_FIELDINV_NOSWAP;
2679 +       info->field_sel = SENSOR_FIELDSEL_BOTH;
2680 +       info->hpol = SENSOR_HPOL_REFPOS;
2681 +       info->vpol = SENSOR_VPOL_NEG;
2682 +       info->edge = SENSOR_EDGE_RISING;
2683 +       info->flicker_freq = SENSOR_FLICKER_100;
2684 +       info->cie_profile = SENSOR_CIEPROF_F11;
2685 +       name = "ov5630";
2686 +       memcpy(info->name, name, 7);
2687 +
2688 +       /* Reset sensor hardware, and implement the setting*/
2689 +       ret = ov5630_write(c, (u32)OV5630_SYS, (u32)0x80);
2690 +       ret += ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
2691 +
2692 +       /* Set registers into default config value */
2693 +       ret += ov5630_write_array(c, ov5630_def_reg);
2694 +
2695 +       /* Set MIPI interface */
2696 +#ifdef OV5630_MIPI
2697 +       ret += ov5630_write_array(c, ov5630_mipi);
2698 +#endif
2699 +
2700 +       /* turn off AE AEB AGC */
2701 +       ret += ov5630_set_img_ctrl(c, info);
2702 +
2703 +       /* streaming */
2704 +       /* ret += ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x01); */
2705 +       /* ret += ov5630_write(c, (u32)0x3096, (u32)0x50); */
2706 +       /* /ssleep(1); */
2707 +
2708 +       /* Added by wen to stop sensor from streaming */
2709 +       ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
2710 +       ov5630_write(c, 0x30b0, 0x00);
2711 +       ov5630_write(c, 0x30b1, 0x00);
2712 +       return ret;
2713 +}
2714 +
2715 +static int distance(struct ov5630_res_struct *res, u32 w, u32 h)
2716 +{
2717 +       int ret;
2718 +       if (res->width < w || res->height < h)
2719 +               return -1;
2720 +
2721 +       ret = ((res->width - w) + (res->height - h));
2722 +       return ret;
2723 +}
2724 +static int ov5630_try_res(u32 *w, u32 *h)
2725 +{
2726 +       struct ov5630_res_struct *res_index, *p = NULL;
2727 +       int dis, last_dis = ov5630_res->width + ov5630_res->height;
2728 +
2729 +       DBG_entering;
2730 +
2731 +       for (res_index = ov5630_res;
2732 +            res_index < ov5630_res + N_RES;
2733 +            res_index++) {
2734 +               if ((res_index->width < *w) || (res_index->height < *h))
2735 +                       break;
2736 +               dis = distance(res_index, *w, *h);
2737 +               if (dis < last_dis) {
2738 +                       last_dis = dis;
2739 +                       p = res_index;
2740 +               }
2741 +       }
2742 +
2743 +       if (p == NULL)
2744 +               p = ov5630_res;
2745 +       else if ((p->width < *w) || (p->height < *h)) {
2746 +               if (p != ov5630_res)
2747 +                       p--;
2748 +       }
2749 +
2750 +       if ((w != NULL) && (h != NULL)) {
2751 +               *w = p->width;
2752 +               *h = p->height;
2753 +       }
2754 +
2755 +       DBG_leaving;
2756 +       return 0;
2757 +}
2758 +
2759 +static struct ov5630_res_struct *ov5630_to_res(u32 w, u32 h)
2760 +{
2761 +       struct ov5630_res_struct *res_index;
2762 +
2763 +       for (res_index = ov5630_res;
2764 +            res_index < ov5630_res + N_RES;
2765 +            res_index++)
2766 +               if ((res_index->width == w) && (res_index->height == h))
2767 +                       break;
2768 +
2769 +       if (res_index >= ov5630_res + N_RES)
2770 +               res_index--;   /* Take the bigger one */
2771 +
2772 +       return res_index;
2773 +}
2774 +
2775 +static int ov5630_try_fmt(struct v4l2_subdev *sd,
2776 +                         struct v4l2_format *fmt)
2777 +{
2778 +       DBG_entering;
2779 +       return ov5630_try_res(&fmt->fmt.pix.width, &fmt->fmt.pix.height);
2780 +       DBG_leaving;
2781 +}
2782 +
2783 +static int ov5630_get_fmt(struct v4l2_subdev *sd,
2784 +                         struct v4l2_format *fmt)
2785 +{
2786 +       struct ci_sensor_config *info = to_sensor_config(sd);
2787 +       unsigned short width, height;
2788 +       int index;
2789 +
2790 +       ci_sensor_res2size(info->res, &width, &height);
2791 +
2792 +       /* Marked the current sensor res as being "used" */
2793 +       for (index = 0; index < N_RES; index++) {
2794 +               if ((width == ov5630_res[index].width) &&
2795 +                   (height == ov5630_res[index].height)) {
2796 +                       ov5630_res[index].used = 1;
2797 +                       continue;
2798 +               }
2799 +               ov5630_res[index].used = 0;
2800 +       }
2801 +
2802 +       fmt->fmt.pix.width = width;
2803 +       fmt->fmt.pix.height = height;
2804 +       return 0;
2805 +}
2806 +
2807 +static int ov5630_set_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
2808 +{
2809 +       struct i2c_client *c = v4l2_get_subdevdata(sd);
2810 +       struct ci_sensor_config *info = to_sensor_config(sd);
2811 +       int ret = 0;
2812 +       struct ov5630_res_struct *res_index;
2813 +       u32 width, height;
2814 +       int index;
2815 +
2816 +       DBG_entering;
2817 +
2818 +       width = fmt->fmt.pix.width;
2819 +       height = fmt->fmt.pix.height;
2820 +
2821 +       dprintk(1, "was told to set fmt (%d x %d) ", width, height);
2822 +
2823 +       ret = ov5630_try_res(&width, &height);
2824 +
2825 +       dprintk(1, "setting fmt (%d x %d) ", width, height);
2826 +
2827 +       res_index = ov5630_to_res(width, height);
2828 +
2829 +       ov5630_wakeup();
2830 +
2831 +       if (res_index->regs) {
2832 +               /* Soft reset camera first*/
2833 +               ret = ov5630_write(c, (u32)OV5630_SYS, (u32)0x80);
2834 +
2835 +               /* software sleep/standby */
2836 +               ret += ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
2837 +
2838 +               /* Set registers into default config value */
2839 +               ret += ov5630_write_array(c, ov5630_def_reg);
2840 +
2841 +               /* set image resolution */
2842 +               ret += ov5630_write_array(c, res_index->regs);
2843 +
2844 +               /* turn off AE AEB AGC */
2845 +               ret += ov5630_set_img_ctrl(c, info);
2846 +
2847 +               /* Set MIPI interface */
2848 +#ifdef OV5630_MIPI
2849 +               ret += ov5630_write_array(c, ov5630_mipi);
2850 +#endif
2851 +
2852 +               if (res_index->res == SENSOR_RES_VGA)
2853 +                       ret += ov5630_write(c, (u32)0x3015, (u32)0x03);
2854 +
2855 +               /* streaming */
2856 +               ret = ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x01);
2857 +               ret = ov5630_write(c, (u32)0x3096, (u32)0x50);
2858 +
2859 +               info->res = res_index->res;
2860 +
2861 +               /* Marked current sensor res as being "used" */
2862 +               for (index = 0; index < N_RES; index++) {
2863 +                       if ((width == ov5630_res[index].width) &&
2864 +                           (height == ov5630_res[index].height)) {
2865 +                               ov5630_res[index].used = 1;
2866 +                               continue;
2867 +                       }
2868 +                       ov5630_res[index].used = 0;
2869 +               }
2870 +
2871 +               for (index = 0; index < N_RES; index++)
2872 +                       dprintk(2, "index = %d, used = %d\n", index,
2873 +                               ov5630_res[index].used);
2874 +       } else {
2875 +               eprintk("no res for (%d x %d)", width, height);
2876 +       }
2877 +
2878 +       DBG_leaving;
2879 +       return ret;
2880 +}
2881 +
2882 +static int ov5630_t_gain(struct v4l2_subdev *sd, int value)
2883 +{
2884 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
2885 +       u32 v;
2886 +
2887 +       DBG_entering;
2888 +
2889 +       dprintk(2, "writing gain %x to 0x3000", value);
2890 +
2891 +       ov5630_read(client, 0x3000, &v);
2892 +       v = (v & 0x80) + value;
2893 +       ov5630_write(client, 0x3000, v);
2894 +
2895 +       dprintk(2, "gain %x was writen to 0x3000", v);
2896 +
2897 +       DBG_leaving;
2898 +       return 0;
2899 +}
2900 +
2901 +static int ov5630_t_exposure(struct v4l2_subdev *sd, int value)
2902 +{
2903 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
2904 +       u32 v;
2905 +       u32 reg_val;
2906 +
2907 +       DBG_entering;
2908 +
2909 +       ov5630_read(client, 0x3013, &v);
2910 +       dprintk(2, "0x3013 = %x", v);
2911 +       if (v & 0x05) {
2912 +               /* turn off agc/aec */
2913 +               v = v & 0xfa;
2914 +               ov5630_write(client, 0x3013, v);
2915 +               /* turn off awb */
2916 +               ov5630_read(client, OV5630_ISP_CTL00, &reg_val);
2917 +               ov5630_write(client, OV5630_ISP_CTL00, reg_val & ~0x30);
2918 +       }
2919 +       ov5630_read(client, 0x3014, &v);
2920 +       dprintk(2, "0x3014 = %x", v);
2921 +       ov5630_read(client, 0x3002, &v);
2922 +       dprintk(2, "0x3002 = %x", v);
2923 +       ov5630_read(client, 0x3003, &v);
2924 +       dprintk(2, "0x3003 = %x", v);
2925 +
2926 +       dprintk(2, "writing exposure %x to 0x3002/3", value);
2927 +
2928 +       v = value >> 8;
2929 +       ov5630_write(client, 0x3002, v);
2930 +       dprintk(2, "exposure %x was writen to 0x3002", v);
2931 +
2932 +       v = value & 0xff;
2933 +       ov5630_write(client, 0x3003, v);
2934 +       dprintk(2, "exposure %x was writen to 0x3003", v);
2935 +
2936 +       DBG_leaving;
2937 +       return 0;
2938 +}
2939 +
2940 +static struct ov5630_control {
2941 +       struct v4l2_queryctrl qc;
2942 +       int (*query)(struct v4l2_subdev *sd, __s32 *value);
2943 +       int (*tweak)(struct v4l2_subdev *sd, int value);
2944 +} ov5630_controls[] = {
2945 +       {
2946 +               .qc = {
2947 +                       .id = V4L2_CID_GAIN,
2948 +                       .type = V4L2_CTRL_TYPE_INTEGER,
2949 +                       .name = "global gain",
2950 +                       .minimum = 0x0,
2951 +                       .maximum = 0xFF,
2952 +                       .step = 0x01,
2953 +                       .default_value = 0x00,
2954 +                       .flags = 0,
2955 +               },
2956 +               .tweak = ov5630_t_gain,
2957 +/*             .query = ov5630_q_gain, */
2958 +       },
2959 +       {
2960 +               .qc = {
2961 +                       .id = V4L2_CID_EXPOSURE,
2962 +                       .type = V4L2_CTRL_TYPE_INTEGER,
2963 +                       .name = "exposure",
2964 +                       .minimum = 0x0,
2965 +                       .maximum = 0xFFFF,
2966 +                       .step = 0x01,
2967 +                       .default_value = 0x00,
2968 +                       .flags = 0,
2969 +               },
2970 +               .tweak = ov5630_t_exposure,
2971 +/*             .query = ov5630_q_exposure; */
2972 +       },
2973 +};
2974 +#define N_CONTROLS (ARRAY_SIZE(ov5630_controls))
2975 +
2976 +/*
2977 +static int ov5630_g_gain(struct v4l2_subdev *sd, int value)
2978 +{
2979 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
2980 +       unsigned char v;
2981 +
2982 +       DBG_entering;
2983 +
2984 +       ov5630_write(client, 0x3000, &v);
2985 +       dprintk(2, "writing gain %x to 0x3000", value);
2986 +
2987 +       value
2988 +       DBG_leaving;
2989 +       return 0
2990 +}
2991 +*/
2992 +
2993 +static struct ov5630_control *ov5630_find_control(__u32 id)
2994 +{
2995 +       int i;
2996 +
2997 +       for (i = 0; i < N_CONTROLS; i++)
2998 +               if (ov5630_controls[i].qc.id == id)
2999 +                       return ov5630_controls + i;
3000 +       return NULL;
3001 +}
3002 +
3003 +static int ov5630_queryctrl(struct v4l2_subdev *sd,
3004 +                           struct v4l2_queryctrl *qc)
3005 +{
3006 +       struct ov5630_control *ctrl = ov5630_find_control(qc->id);
3007 +
3008 +       if (ctrl == NULL)
3009 +               return -EINVAL;
3010 +       *qc = ctrl->qc;
3011 +       return 0;
3012 +}
3013 +
3014 +static int ov5630_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
3015 +{
3016 +       /*
3017 +       struct ov5630_control *octrl = ov5630_find_control(ctrl->id);
3018 +
3019 +       int ret;
3020 +
3021 +       if (octrl == NULL)
3022 +               return -EINVAL;
3023 +       ret = octrl->query(sd, &ctrl->value);
3024 +       if (ret >= 0)
3025 +               return 0;
3026 +       return ret;
3027 +       */
3028 +       return 0;
3029 +}
3030 +
3031 +static int ov5630_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
3032 +{
3033 +       struct ov5630_control *octrl = ov5630_find_control(ctrl->id);
3034 +       int ret;
3035 +
3036 +       if (octrl == NULL)
3037 +               return -EINVAL;
3038 +       ret =  octrl->tweak(sd, ctrl->value);
3039 +       if (ret >= 0)
3040 +               return 0;
3041 +       return ret;
3042 +}
3043 +
3044 +#if 0
3045 +static int ov5630_get_caps(struct i2c_client *c, struct ci_sensor_caps *caps)
3046 +{
3047 +       if (caps == NULL)
3048 +               return -EIO;
3049 +
3050 +       caps->bus_width = SENSOR_BUSWIDTH_10BIT;
3051 +       caps->mode      = SENSOR_MODE_BAYER;
3052 +       caps->field_inv = SENSOR_FIELDINV_NOSWAP;
3053 +       caps->field_sel = SENSOR_FIELDSEL_BOTH;
3054 +       caps->ycseq     = SENSOR_YCSEQ_YCBYCR;
3055 +       caps->conv422   = SENSOR_CONV422_NOCOSITED;
3056 +       caps->bpat      = SENSOR_BPAT_BGBGGRGR;
3057 +       caps->hpol      = SENSOR_HPOL_REFPOS;
3058 +       caps->vpol      = SENSOR_VPOL_NEG;
3059 +       caps->edge      = SENSOR_EDGE_RISING;
3060 +       caps->bls       = SENSOR_BLS_OFF;
3061 +       caps->gamma     = SENSOR_GAMMA_OFF;
3062 +       caps->cconv     = SENSOR_CCONV_OFF;
3063 +       caps->res       = SENSOR_RES_QXGA_PLUS | SENSOR_RES_1080P |
3064 +           SENSOR_RES_XGA_PLUS | SENSOR_RES_720P | SENSOR_RES_VGA;
3065 +       caps->blc       = SENSOR_BLC_OFF;
3066 +       caps->agc       = SENSOR_AGC_OFF;
3067 +       caps->awb       = SENSOR_AWB_OFF;
3068 +       caps->aec       = SENSOR_AEC_OFF;
3069 +       caps->cie_profile = SENSOR_CIEPROF_D65 | SENSOR_CIEPROF_D75 |
3070 +           SENSOR_CIEPROF_F11 | SENSOR_CIEPROF_F12 | SENSOR_CIEPROF_A |
3071 +           SENSOR_CIEPROF_F2;
3072 +       caps->flicker_freq      = SENSOR_FLICKER_100 | SENSOR_FLICKER_120;
3073 +       caps->type      = SENSOR_TYPE_RAW;
3074 +       /* caps->name   = "ov5630"; */
3075 +       strcpy(caps->name, "ov5630");
3076 +
3077 +       return 0;
3078 +}
3079 +
3080 +static int ov5630_get_config(struct i2c_client *c,
3081 +                            struct ci_sensor_config *config)
3082 +{
3083 +       struct ci_sensor_config *info = i2c_get_clientdata(c);
3084 +
3085 +       if (config == NULL) {
3086 +               printk(KERN_WARNING "sensor_get_config: NULL pointer\n");
3087 +               return -EIO;
3088 +       }
3089 +
3090 +       memcpy(config, info, sizeof(struct ci_sensor_config));
3091 +
3092 +       return 0;
3093 +}
3094 +
3095 +static int ov5630_setup(struct i2c_client *c,
3096 +                       const struct ci_sensor_config *config)
3097 +{
3098 +       int ret;
3099 +       u16 width, high;
3100 +       struct ov5630_res_struct *res_index;
3101 +       struct ci_sensor_config *info = i2c_get_clientdata(c);
3102 +
3103 +       /* Soft reset camera first*/
3104 +       ret = ov5630_write(c, (u32)OV5630_SYS, (u32)0x80);
3105 +
3106 +       /* software sleep/standby */
3107 +       ret = ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
3108 +
3109 +       /* Set registers into default config value */
3110 +       ret = ov5630_write_array(c, ov5630_def_reg);
3111 +
3112 +       /* set image resolution */
3113 +       ci_sensor_res2size(config->res, &width, &high);
3114 +       ret += ov5630_try_res(&width, &high);
3115 +       res_index = ov5630_find_res(width, high);
3116 +       if (res_index->regs)
3117 +               ret += ov5630_write_array(c, res_index->regs);
3118 +       if (!ret)
3119 +               info->res = res_index->res;
3120 +
3121 +       ret += ov5630_set_img_ctrl(c, config);
3122 +
3123 +       /* Set MIPI interface */
3124 +#ifdef OV5630_MIPI
3125 +       ret += ov5630_write_array(c, ov5630_mipi);
3126 +#endif
3127 +
3128 +       /* streaming */
3129 +       ret += ov5630_write(c, (u32)OV5630_IMAGE_SYSTEM, (u32)0x01);
3130 +       ret += ov5630_write(c, (u32)0x3096, (u32)0x50);
3131 +
3132 +       /*Note here for the time delay */
3133 +       /* ssleep(1); */
3134 +       msleep(500);
3135 +       return ret;
3136 +}
3137 +
3138 +/*
3139 + * File operation functions
3140 + */
3141 +static int ov5630_dvp_enable(struct i2c_client *client)
3142 +{
3143 +       int ret;
3144 +
3145 +       u8 reg;
3146 +
3147 +       ret = ov5630_read(client, 0x3506, &reg);
3148 +       reg &= 0xdf;
3149 +       reg |= 0x20;
3150 +       ret += ov5630_write(client, 0x3506, reg);
3151 +
3152 +       return ret;
3153 +}
3154 +
3155 +static int ov5630_dvp_disable(struct i2c_client *client)
3156 +{
3157 +       int ret;
3158 +
3159 +       u8 reg;
3160 +
3161 +       ret = ov5630_read(client, 0x3506, &reg);
3162 +       reg &= 0xdf;
3163 +       ret += ov5630_write(client, 0x3506, reg);
3164 +
3165 +       return ret;
3166 +}
3167 +
3168 +static int ov5630_open(struct i2c_setting *c, void *priv)
3169 +{
3170 +       /* Just wake up sensor */
3171 +       if (ov5630_wakeup())
3172 +               return -EIO;
3173 +       ov5630_init(c->sensor_client);
3174 +       /* ov5630_motor_init(c->motor_client); */
3175 +       ov5630_write(c->sensor_client, (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
3176 +
3177 +       /* disable dvp_en */
3178 +       ov5630_dvp_disable(c->sensor_client);
3179 +
3180 +       return 0;
3181 +}
3182 +
3183 +static int ov5630_release(struct i2c_setting *c, void *priv)
3184 +{
3185 +       /* Just suspend the sensor */
3186 +       if (ov5630_standby())
3187 +               return -EIO;
3188 +       return 0;
3189 +}
3190 +
3191 +static int ov5630_on(struct i2c_setting *c)
3192 +{
3193 +       int ret;
3194 +
3195 +       /* Software wake up sensor */
3196 +       ret = ov5630_write(c->sensor_client,
3197 +                           (u32)OV5630_IMAGE_SYSTEM, (u32)0x01);
3198 +
3199 +       /* enable dvp_en */
3200 +       return ret + ov5630_dvp_enable(c->sensor_client);
3201 +}
3202 +
3203 +static int ov5630_off(struct i2c_setting *c)
3204 +{
3205 +       int ret;
3206 +
3207 +       /* Software standby sensor */
3208 +       ret = ov5630_write(c->sensor_client,
3209 +                           (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
3210 +       /* disable dvp_en */
3211 +       return ret + ov5630_dvp_disable(c->sensor_client);
3212 +}
3213 +
3214 +static struct sensor_device ov5630 = {
3215 +       .name   = "ov5630",
3216 +       .type   = SENSOR_TYPE_RAW,
3217 +       .minor  = -1,
3218 +       .open   = ov5630_open,
3219 +       .release = ov5630_release,
3220 +       .on = ov5630_on,
3221 +       .off = ov5630_off,
3222 +       .querycap = ov5630_get_caps,
3223 +       .get_config = ov5630_get_config,
3224 +       .set_config = ov5630_setup,
3225 +       .enum_parm = ov5630_queryctrl,
3226 +       .get_parm = ov5630_g_ctrl,
3227 +       .set_parm = ov5630_s_ctrl,
3228 +       .try_res = ov5630_try_res,
3229 +       .set_res = ov5630_set_res,
3230 +       .get_ls_corr_config = NULL,
3231 +       .mdi_get_focus = ov5630_motor_get_focus,
3232 +       .mdi_set_focus = ov5630_motor_set_focus,
3233 +       .mdi_max_step = ov5630_motor_max_step,
3234 +       .mdi_calibrate = NULL,
3235 +       .read = ov5630_read,
3236 +       .write = ov5630_write,
3237 +       .suspend = ov5630_standby,
3238 +       .resume = ov5630_wakeup,
3239 +       /* TBC */
3240 +};
3241 +#endif
3242 +
3243 +static int ov5630_s_stream(struct v4l2_subdev *sd, int enable)
3244 +{
3245 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
3246 +       DBG_entering;
3247 +
3248 +       if (enable) {
3249 +               ov5630_write(client, (u32)OV5630_IMAGE_SYSTEM, (u32)0x01);
3250 +               ov5630_write(client, 0x30b0, 0xff);
3251 +               ov5630_write(client, 0x30b1, 0xff);
3252 +               msleep(500);
3253 +       } else {
3254 +               ov5630_write(client, (u32)OV5630_IMAGE_SYSTEM, (u32)0x00);
3255 +               ov5630_write(client, 0x30b0, 0x00);
3256 +               ov5630_write(client, 0x30b1, 0x00);
3257 +       }
3258 +
3259 +       DBG_leaving;
3260 +       return 0;
3261 +}
3262 +
3263 +static int ov5630_enum_framesizes(struct v4l2_subdev *sd,
3264 +                                 struct v4l2_frmsizeenum *fsize)
3265 +{
3266 +       unsigned int index = fsize->index;
3267 +
3268 +       DBG_entering;
3269 +
3270 +       if (index >= N_RES)
3271 +               return -EINVAL;
3272 +
3273 +       fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
3274 +       fsize->discrete.width = ov5630_res[index].width;
3275 +       fsize->discrete.height = ov5630_res[index].height;
3276 +       fsize->reserved[0] = ov5630_res[index].used;
3277 +
3278 +       DBG_leaving;
3279 +
3280 +       return 0;
3281 +}
3282 +
3283 +static int ov5630_enum_frameintervals(struct v4l2_subdev *sd,
3284 +                                     struct v4l2_frmivalenum *fival)
3285 +{
3286 +       unsigned int index = fival->index;
3287 +
3288 +       DBG_entering;
3289 +
3290 +       if (index >= N_RES)
3291 +               return -EINVAL;
3292 +
3293 +       fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
3294 +       fival->discrete.numerator = 1;
3295 +       fival->discrete.denominator = ov5630_res[index].fps;
3296 +
3297 +       DBG_leaving;
3298 +
3299 +       return 0;
3300 +}
3301 +
3302 +static int ov5630_g_chip_ident(struct v4l2_subdev *sd,
3303 +               struct v4l2_dbg_chip_ident *chip)
3304 +{
3305 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
3306 +
3307 +#define V4L2_IDENT_OV5630 8245
3308 +       return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_OV5630, 0);
3309 +}
3310 +
3311 +#ifdef CONFIG_VIDEO_ADV_DEBUG
3312 +static int ov5630_g_register(struct v4l2_subdev *sd,
3313 +                            struct v4l2_dbg_register *reg)
3314 +{
3315 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
3316 +       unsigned char val = 0;
3317 +       int ret;
3318 +
3319 +       if (!v4l2_chip_match_i2c_client(client, &reg->match))
3320 +               return -EINVAL;
3321 +       if (!capable(CAP_SYS_ADMIN))
3322 +               return -EPERM;
3323 +       ret = ov5630_read(client, reg->reg & 0xffff, &val);
3324 +       reg->val = val;
3325 +       reg->size = 1;
3326 +       return ret;
3327 +}
3328 +
3329 +static int ov5630_s_register(struct v4l2_subdev *sd,
3330 +                            struct v4l2_dbg_register *reg)
3331 +{
3332 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
3333 +
3334 +       if (!v4l2_chip_match_i2c_client(client, &reg->match))
3335 +               return -EINVAL;
3336 +       if (!capable(CAP_SYS_ADMIN))
3337 +               return -EPERM;
3338 +       ov5630_write(client, reg->reg & 0xffff, reg->val & 0xff);
3339 +       return 0;
3340 +}
3341 +#endif
3342 +
3343 +static const struct v4l2_subdev_video_ops ov5630_video_ops = {
3344 +       .try_fmt = ov5630_try_fmt,
3345 +       .s_fmt = ov5630_set_fmt,
3346 +       .g_fmt = ov5630_get_fmt,
3347 +       .s_stream = ov5630_s_stream,
3348 +       .enum_framesizes = ov5630_enum_framesizes,
3349 +       .enum_frameintervals = ov5630_enum_frameintervals,
3350 +};
3351 +
3352 +static const struct v4l2_subdev_core_ops ov5630_core_ops = {
3353 +       .g_chip_ident = ov5630_g_chip_ident,
3354 +       .queryctrl = ov5630_queryctrl,
3355 +       .g_ctrl = ov5630_g_ctrl,
3356 +       .s_ctrl = ov5630_s_ctrl,
3357 +       .s_gpio = ov5630_s_power,
3358 +       /*.g_ext_ctrls = ov5630_g_ext_ctrls,*/
3359 +       /*.s_ext_ctrls = ov5630_s_ext_ctrls,*/
3360 +#ifdef CONFIG_VIDEO_ADV_DEBUG
3361 +       .g_register = ov5630_g_register,
3362 +       .s_register = ov5630_s_register,
3363 +#endif
3364 +};
3365 +
3366 +static const struct v4l2_subdev_ops ov5630_ops = {
3367 +       .core = &ov5630_core_ops,
3368 +       .video = &ov5630_video_ops,
3369 +};
3370 +
3371 +/*
3372 + * Basic i2c stuff
3373 + */
3374 +/*
3375 +static unsigned short normal_i2c[] = {I2C_OV5630 >> 1,
3376 +       I2C_CLIENT_END};
3377 +I2C_CLIENT_INSMOD;
3378 +
3379 +static struct i2c_driver ov5630_driver;
3380 +*/
3381 +static int ov5630_detect(struct i2c_client *client)
3382 +{
3383 +       struct i2c_adapter *adapter = client->adapter;
3384 +       int adap_id = i2c_adapter_id(adapter);
3385 +       u32 value;
3386 +
3387 +       if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
3388 +               eprintk("error i2c check func");
3389 +               return -ENODEV;
3390 +       }
3391 +
3392 +       if (adap_id != 1) {
3393 +               eprintk("adap_id != 1");
3394 +               return -ENODEV;
3395 +       }
3396 +
3397 +       /* if (ov5630_wakeup()) */
3398 +               /* return -ENODEV; */
3399 +       ov5630_wakeup();
3400 +
3401 +       ov5630_read(client, (u32)OV5630_PID_H, &value);
3402 +       if ((u8)value != 0x56) {
3403 +               dprintk(1, "PID != 0x56, but %x", value);
3404 +               dprintk(2, "client->addr = %x", client->addr);
3405 +               return -ENODEV;
3406 +       }
3407 +
3408 +       printk(KERN_INFO "Init ov5630 sensor success\n");
3409 +       return 0;
3410 +}
3411 +
3412 +static int ov5630_probe(struct i2c_client *client,
3413 +                       const struct i2c_device_id *id)
3414 +{
3415 +       struct ci_sensor_config *info;
3416 +       struct v4l2_subdev *sd;
3417 +       int ret = -1;
3418 +/*     struct i2c_client *motor; */
3419 +
3420 +       DBG_entering;
3421 +       v4l_info(client, "chip found @ 0x%x (%s)\n",
3422 +                client->addr << 1, client->adapter->name);
3423 +       /*
3424 +        * Setup sensor configuration structure
3425 +        */
3426 +       info = kzalloc(sizeof(struct ci_sensor_config), GFP_KERNEL);
3427 +       if (!info) {
3428 +               eprintk("fail to malloc for ci_sensor_config");
3429 +               ret = -ENOMEM;
3430 +               goto out;
3431 +       }
3432 +
3433 +       ret = ov5630_detect(client);
3434 +       if (ret) {
3435 +               dprintk(1, "error ov5630_detect");
3436 +               goto out_free;
3437 +       }
3438 +
3439 +       sd = &info->sd;
3440 +       v4l2_i2c_subdev_init(sd, client, &ov5630_ops);
3441 +
3442 +       /*
3443 +        * Initialization OV5630
3444 +        * then turn into standby mode
3445 +        */
3446 +       /* ret = ov5630_standby(); */
3447 +       ret = ov5630_init(client);
3448 +       if (ret) {
3449 +               eprintk("error calling ov5630_init");
3450 +               goto out_free;
3451 +       }
3452 +       ov5630_standby();
3453 +
3454 +       ret = 0;
3455 +       goto out;
3456 +
3457 +out_free:
3458 +       kfree(info);
3459 +       DBG_leaving;
3460 +out:
3461 +       return ret;
3462 +}
3463 +
3464 +/*
3465 + * XXX: Need to be checked
3466 + */
3467 +static int ov5630_remove(struct i2c_client *client)
3468 +{
3469 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
3470 +
3471 +       DBG_entering;
3472 +
3473 +       v4l2_device_unregister_subdev(sd);
3474 +       kfree(to_sensor_config(sd));
3475 +
3476 +       DBG_leaving;
3477 +       return 0;
3478 +}
3479 +
3480 +static const struct i2c_device_id ov5630_id[] = {
3481 +       {"ov5630", 0},
3482 +       {}
3483 +};
3484 +
3485 +MODULE_DEVICE_TABLE(i2c, ov5630_id);
3486 +
3487 +static struct v4l2_i2c_driver_data v4l2_i2c_data = {
3488 +       .name = "ov5630",
3489 +       .probe = ov5630_probe,
3490 +       .remove = ov5630_remove,
3491 +       /* .suspend = ov5630_suspend,
3492 +        * .resume = ov5630_resume, */
3493 +       .id_table = ov5630_id,
3494 +};
3495 +
3496 +MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
3497 +MODULE_DESCRIPTION("A low-level driver for OmniVision 5630 sensors");
3498 +MODULE_LICENSE("GPL");
3499 diff --git a/drivers/media/video/mrstci/mrstov5630/ov5630.h b/drivers/media/video/mrstci/mrstov5630/ov5630.h
3500 new file mode 100644
3501 index 0000000..3da0ecd
3502 --- /dev/null
3503 +++ b/drivers/media/video/mrstci/mrstov5630/ov5630.h
3504 @@ -0,0 +1,672 @@
3505 +/*
3506 + * Support for Moorestown Langwell Camera Imaging ISP subsystem.
3507 + *
3508 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
3509 + *
3510 + * This program is free software; you can redistribute it and/or
3511 + * modify it under the terms of the GNU General Public License version
3512 + * 2 as published by the Free Software Foundation.
3513 + *
3514 + * This program is distributed in the hope that it will be useful,
3515 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
3516 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
3517 + * GNU General Public License for more details.
3518 + *
3519 + * You should have received a copy of the GNU General Public License
3520 + * along with this program; if not, write to the Free Software
3521 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
3522 + * 02110-1301, USA.
3523 + *
3524 + *
3525 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
3526 + */
3527 +
3528 +#define I2C_OV5630     0x6C
3529 +/* Should add to kernel source */
3530 +#define I2C_DRIVERID_OV5630    1046
3531 +/* GPIO pin on Moorestown */
3532 +#define GPIO_SCLK_25   44
3533 +#define GPIO_STB_PIN   47
3534 +#define GPIO_STDBY_PIN 49
3535 +#define GPIO_RESET_PIN 50
3536 +
3537 +/* System control register */
3538 +#define OV5630_AGC             0x3000
3539 +#define OV5630_AGCS    0x3001
3540 +#define OV5630_AEC_H   0x3002
3541 +#define OV5630_AEC_L   0x3003
3542 +#define OV5630_LAEC_H  0x3004
3543 +#define OV5630_LAEC_L  0x3005
3544 +#define OV5630_AECS_H  0x3008
3545 +#define OV5630_AECS_L  0x3009
3546 +#define OV5630_PID_H   0x300A
3547 +#define OV5630_PID_L   0x300B
3548 +#define OV5630_SCCB_ID         0x300C
3549 +#define OV5630_PLL_1   0x300E
3550 +#define OV5630_PLL_2   0x300F
3551 +#define OV5630_PLL_3   0x3010
3552 +#define OV5630_PLL_4           0x3011
3553 +#define OV5630_SYS             0x3012
3554 +#define OV5630_AUTO_1  0x3013
3555 +#define OV5630_AUTO_2  0x3014
3556 +#define OV5630_AUTO_3  0x3015
3557 +#define OV5630_AUTO_4  0x3016
3558 +#define OV5630_AUTO_5  0x3017
3559 +#define OV5630_WPT             0x3018
3560 +#define OV5630_BPT             0x3019
3561 +#define OV5630_VPT             0x301A
3562 +#define OV5630_YAVG            0x301B
3563 +#define OV5630_AECG_50 0x301C
3564 +#define OV5630_AECG_60 0x301D
3565 +#define OV5630_ADDVS_H 0x301E
3566 +#define OV5630_ADDVS_L 0x301F
3567 +#define OV5630_FRAME_LENGTH_LINES_H    0x3020
3568 +#define OV5630_FRAME_LENGTH_LINES_L    0x3021
3569 +#define OV5630_LINE_LENGTH_PCK_H       0x3022
3570 +#define OV5630_LINE_LENGTH_PCK_L       0x3023
3571 +#define OV5630_X_ADDR_START_H  0x3024
3572 +#define OV5630_X_ADDR_START_L  0x3025
3573 +#define OV5630_Y_ADDR_START_H  0x3026
3574 +#define OV5630_Y_ADDR_START_L  0x3027
3575 +#define OV5630_X_ADDR_END_H    0x3028
3576 +#define OV5630_X_ADDR_END_L    0x3029
3577 +#define OV5630_Y_ADDR_END_H    0x302A
3578 +#define OV5630_Y_ADDR_END_L    0x302B
3579 +#define OV5630_X_OUTPUT_SIZE_H 0x302C
3580 +#define OV5630_X_OUTPUT_SIZE_L 0x302D
3581 +#define OV5630_Y_OUTPUT_SIZE_H 0x302E
3582 +#define OV5630_Y_OUTPUT_SIZE_L 0x302F
3583 +#define OV5630_FRAME_CNT       0x3030
3584 +#define OV5630_DATR_LMO_0      0x3038
3585 +#define OV5630_DATR_LMO_1      0x3039
3586 +#define OV5630_DATR_LMO_2      0x303A
3587 +#define OV5630_DATR_D56        0x303D
3588 +#define OV5630_DATR_EF 0x303E
3589 +#define OV5630_R_SIGMA_0       0x3048
3590 +#define OV5630_R_SIGMA_1       0x3049
3591 +#define OV5630_R_SIGMA_2       0x304A
3592 +#define OV5630_R_SIGMA_3       0x304B
3593 +#define OV5630_R_SIGMA_4       0x304C
3594 +#define OV5630_R_SIGMA_5       0x304D
3595 +#define OV5630_D56COM  0x304E
3596 +#define OV5630_5060TH  0x3050
3597 +#define OV5630_LMO_TH1 0x3058
3598 +#define OV5630_LMO_TH2 0x3059
3599 +#define OV5630_LMO_K   0x305A
3600 +#define OV5630_BD50ST_H        0x305C
3601 +#define OV5630_BD50ST_L        0x305D
3602 +#define OV5630_BD60ST_H        0x305E
3603 +#define OV5630_BD60ST_L        0x305F
3604 +#define OV5630_HSYNST  0x306D
3605 +#define OV5630_HSYNED  0x306E
3606 +#define OV5630_HSYNED_HSYNST   0x306F
3607 +#define OV5630_TMC_RWIN0       0x3070
3608 +#define OV5630_IO_CTRL0        0x30B0
3609 +#define OV5630_IO_CTRL1        0x30B1
3610 +#define OV5630_IO_CTRL2        0x30B2
3611 +#define OV5630_DSIO_0  0x30B3
3612 +#define OV5630_DSIO_1  0x30B4
3613 +#define OV5630_DSIO_2  0x30B5
3614 +#define OV5630_TMC_10  0x30B6
3615 +#define OV5630_TMC_12  0x30B7
3616 +#define OV5630_TMC_14  0x30B9
3617 +#define OV5630_TMC_COM4        0x30BA
3618 +#define OV5630_TMC_REG6C       0x30BB
3619 +#define OV5630_TMC_REG6E       0x30BC
3620 +#define OV5630_R_CLK_S 0x30BD
3621 +#define OV5630_R_CLK_A 0x30BE
3622 +#define OV5630_R_CLK_A1        0x30BF
3623 +#define OV5630_FRS_0   0x30E0
3624 +#define OV5630_FRS_1   0x30E1
3625 +#define OV5630_FRS_2   0x30E2
3626 +#define OV5630_FRS_3   0x30E3
3627 +#define OV5630_FRS_FECNT       0x30E4
3628 +#define OV5630_FRS_FECNT_0     0x30E5
3629 +#define OV5630_FRS_FECNT_1     0x30E6
3630 +#define OV5630_FRS_RFRM        0x30E7
3631 +#define OV5630_FRS_RSTRB       0x30E8
3632 +#define OV5630_SA1TMC  0x30E9
3633 +#define OV5630_TMC_MISC0       0x30EA
3634 +#define OV5630_TMC_MISC1       0x30EB
3635 +#define OV5630_FLEX_TXP        0x30F0
3636 +#define OV5630_FLEX_FLT        0x30F1
3637 +#define OV5630_FLEX_TXT        0x30F2
3638 +#define OV5630_FLEX_HBK        0x30F3
3639 +#define OV5630_FLEX_HSG        0x30F4
3640 +#define OV5630_FLEX_SA1SFT     0x30F5
3641 +#define OV5630_RVSOPT  0x30F6
3642 +#define OV5630_AUTO    0x30F7
3643 +#define OV5630_IMAGE_TRANSFORM 0x30F8
3644 +#define OV5630_IMAGE_LUM       0x30F9
3645 +#define OV5630_IMAGE_SYSTEM    0x30FA
3646 +#define OV5630_GROUP_WR        0x30FF
3647 +
3648 +/* CIF control register */
3649 +#define OV5630_CIF_CTRL2       0x3202
3650 +
3651 +/* ISP control register */
3652 +#define OV5630_ISP_CTL00       0x3300
3653 +#define OV5630_ISP_CTL01       0x3301
3654 +#define OV5630_ISP_CTL02       0x3302
3655 +#define OV5630_ISP_03  0x3303
3656 +#define OV5630_ISP_DIG_GAIN_MAN        0x3304
3657 +#define OV5630_ISP_BIAS_MAN    0x3305
3658 +#define OV5630_ISP_06  0x3306
3659 +#define OV5630_ISP_STABLE_RANGE        0x3307
3660 +#define OV5630_ISP_R_GAIN_MAN_1        0x3308
3661 +#define OV5630_ISP_R_GAIN_MAN_0        0x3309
3662 +#define OV5630_ISP_G_GAIN_MAN_1        0x330A
3663 +#define OV5630_ISP_G_GAIN_MAN_0        0x330B
3664 +#define OV5630_ISP_B_GAIN_MAN_1        0x330C
3665 +#define OV5630_ISP_B_GAIN_MAN_0        0x330D
3666 +#define OV5630_ISP_STABLE_RANGEW       0x330E
3667 +#define OV5630_ISP_AWB_FRAME_CNT       0x330F
3668 +#define OV5630_ISP_11  0x3311
3669 +#define OV5630_ISP_12  0x3312
3670 +#define OV5630_ISP_13  0x3313
3671 +#define OV5630_ISP_HSIZE_IN_1  0x3314
3672 +#define OV5630_ISP_HSIZE_IN_0  0x3315
3673 +#define OV5630_ISP_VSIZE_IN_1  0x3316
3674 +#define OV5630_ISP_VSIZE_IN_0  0x3317
3675 +#define OV5630_ISP_18  0x3318
3676 +#define OV5630_ISP_19  0x3319
3677 +#define OV5630_ISP_EVEN_MAN0   0x331A
3678 +#define OV5630_ISP_EVEN_MAN1   0x331B
3679 +#define OV5630_ISP_EVEN_MAN2   0x331C
3680 +#define OV5630_ISP_EVEN_MAN3   0x331D
3681 +#define OV5630_ISP_1E  0x331E
3682 +#define OV5630_ISP_1F  0x331F
3683 +#define OV5630_ISP_BLC_LMT_OPTION      0x3320
3684 +#define OV5630_ISP_BLC_THRE    0x3321
3685 +#define OV5630_ISP_22  0x3322
3686 +#define OV5630_ISP_23  0x3323
3687 +#define OV5630_ISP_BLC_MAN0_1  0x3324
3688 +#define OV5630_ISP_BLC_MAN0_0  0x3325
3689 +#define OV5630_ISP_BLC_MAN1_1  0x3326
3690 +#define OV5630_ISP_BLC_MAN1_0  0x3327
3691 +#define OV5630_ISP_BLC_MAN2_1  0x3328
3692 +#define OV5630_ISP_BLC_MAN2_0  0x3329
3693 +#define OV5630_ISP_BLC_MAN3_1  0x332A
3694 +#define OV5630_ISP_BLC_MAN3_0  0x332B
3695 +#define OV5630_ISP_BLC_MAN4_1  0x332C
3696 +#define OV5630_ISP_BLC_MAN4_0  0x332D
3697 +#define OV5630_ISP_BLC_MAN5_1  0x332E
3698 +#define OV5630_ISP_BLC_MAN5_0  0x332F
3699 +#define OV5630_ISP_BLC_MAN6_1  0x3330
3700 +#define OV5630_ISP_BLC_MAN6_0  0x3331
3701 +#define OV5630_ISP_BLC_MAN7_1  0x3332
3702 +#define OV5630_ISP_BLC_MAN7_0  0x3333
3703 +#define OV5630_ISP_CD  0x33CD
3704 +#define OV5630_ISP_FF  0x33FF
3705 +
3706 +/* clipping control register */
3707 +#define OV5630_CLIP_CTRL0      0x3400
3708 +#define OV5630_CLIP_CTRL1      0x3401
3709 +#define OV5630_CLIP_CTRL2      0x3402
3710 +#define OV5630_CLIP_CTRL3      0x3403
3711 +#define OV5630_CLIP_CTRL4      0x3404
3712 +#define OV5630_CLIP_CTRL5      0x3405
3713 +#define OV5630_CLIP_CTRL6      0x3406
3714 +#define OV5630_CLIP_CTRL7      0x3407
3715 +
3716 +/* DVP control register */
3717 +#define OV5630_DVP_CTRL00      0x3500
3718 +#define OV5630_DVP_CTRL01      0x3501
3719 +#define OV5630_DVP_CTRL02      0x3502
3720 +#define OV5630_DVP_CTRL03      0x3503
3721 +#define OV5630_DVP_CTRL04      0x3504
3722 +#define OV5630_DVP_CTRL05      0x3505
3723 +#define OV5630_DVP_CTRL06      0x3506
3724 +#define OV5630_DVP_CTRL07      0x3507
3725 +#define OV5630_DVP_CTRL08      0x3508
3726 +#define OV5630_DVP_CTRL09      0x3509
3727 +#define OV5630_DVP_CTRL0A      0x350A
3728 +#define OV5630_DVP_CTRL0B      0x350B
3729 +#define OV5630_DVP_CTRL0C      0x350C
3730 +#define OV5630_DVP_CTRL0D      0x350D
3731 +#define OV5630_DVP_CTRL0E      0x350E
3732 +#define OV5630_DVP_CTRL0F      0x350F
3733 +#define OV5630_DVP_CTRL10      0x3510
3734 +#define OV5630_DVP_CTRL11      0x3511
3735 +#define OV5630_DVP_CTRL12      0x3512
3736 +#define OV5630_DVP_CTRL13      0x3513
3737 +#define OV5630_DVP_CTRL14      0x3514
3738 +#define OV5630_DVP_CTRL15      0x3515
3739 +#define OV5630_DVP_CTRL16      0x3516
3740 +#define OV5630_DVP_CTRL17      0x3517
3741 +#define OV5630_DVP_CTRL18      0x3518
3742 +#define OV5630_DVP_CTRL19      0x3519
3743 +#define OV5630_DVP_CTRL1A      0x351A
3744 +#define OV5630_DVP_CTRL1B      0x351B
3745 +#define OV5630_DVP_CTRL1C      0x351C
3746 +#define OV5630_DVP_CTRL1D      0x351D
3747 +#define OV5630_DVP_CTRL1E      0x351E
3748 +#define OV5630_DVP_CTRL1F      0x351F
3749 +
3750 +/* MIPI control register */
3751 +#define OV5630_MIPI_CTRL00     0x3600
3752 +#define OV5630_MIPI_CTRL01     0x3601
3753 +#define OV5630_MIPI_CTRL02     0x3602
3754 +#define OV5630_MIPI_CTRL03     0x3603
3755 +#define OV5630_MIPI_CTRL04     0x3604
3756 +#define OV5630_MIPI_CTRL05     0x3605
3757 +#define OV5630_MIPI_CTRL06     0x3606
3758 +#define OV5630_MIPI_CTRL07     0x3607
3759 +#define OV5630_MIPI_CTRL08     0x3608
3760 +#define OV5630_MIPI_CTRL09     0x3609
3761 +#define OV5630_MIPI_CTRL0A     0x360A
3762 +#define OV5630_MIPI_CTRL0B     0x360B
3763 +#define OV5630_MIPI_CTRL0C     0x360C
3764 +#define OV5630_MIPI_CTRL0D     0x360D
3765 +#define OV5630_MIPI_CTRL0E     0x360E
3766 +#define OV5630_MIPI_CTRL0F     0x360F
3767 +#define OV5630_MIPI_CTRL10     0x3610
3768 +#define OV5630_MIPI_CTRL11     0x3611
3769 +#define OV5630_MIPI_CTRL12     0x3612
3770 +#define OV5630_MIPI_CTRL13     0x3613
3771 +#define OV5630_MIPI_CTRL14     0x3614
3772 +#define OV5630_MIPI_CTRL15     0x3615
3773 +#define OV5630_MIPI_CTRL16     0x3616
3774 +#define OV5630_MIPI_CTRL17     0x3617
3775 +#define OV5630_MIPI_CTRL18     0x3618
3776 +#define OV5630_MIPI_CTRL19     0x3619
3777 +#define OV5630_MIPI_CTRL1A     0x361A
3778 +#define OV5630_MIPI_CTRL1B     0x361B
3779 +#define OV5630_MIPI_CTRL1C     0x361C
3780 +#define OV5630_MIPI_CTRL1D     0x361D
3781 +#define OV5630_MIPI_CTRL1E     0x361E
3782 +#define OV5630_MIPI_CTRL1F     0x361F
3783 +#define OV5630_MIPI_CTRL20     0x3620
3784 +#define OV5630_MIPI_CTRL21     0x3621
3785 +#define OV5630_MIPI_CTRL22     0x3622
3786 +#define OV5630_MIPI_CTRL23     0x3623
3787 +#define OV5630_MIPI_CTRL24     0x3624
3788 +#define OV5630_MIPI_CTRL25     0x3625
3789 +#define OV5630_MIPI_CTRL26     0x3626
3790 +#define OV5630_MIPI_CTRL27     0x3627
3791 +#define OV5630_MIPI_CTRL28     0x3628
3792 +#define OV5630_MIPI_CTRL29     0x3629
3793 +#define OV5630_MIPI_CTRL2A     0x362A
3794 +#define OV5630_MIPI_CTRL2B     0x362B
3795 +#define OV5630_MIPI_CTRL2C     0x362C
3796 +#define OV5630_MIPI_CTRL2D     0x362D
3797 +#define OV5630_MIPI_CTRL2E     0x362E
3798 +#define OV5630_MIPI_CTRL2F     0x362F
3799 +#define OV5630_MIPI_CTRL30     0x3630
3800 +#define OV5630_MIPI_CTRL31     0x3631
3801 +#define OV5630_MIPI_CTRL32     0x3632
3802 +#define OV5630_MIPI_CTRL33     0x3633
3803 +#define OV5630_MIPI_CTRL34     0x3634
3804 +#define OV5630_MIPI_CTRL35     0x3635
3805 +#define OV5630_MIPI_CTRL36     0x3636
3806 +#define OV5630_MIPI_CTRL37     0x3637
3807 +#define OV5630_MIPI_CTRL38     0x3638
3808 +#define OV5630_MIPI_CTRL39     0x3639
3809 +#define OV5630_MIPI_CTRL3A     0x363A
3810 +#define OV5630_MIPI_CTRL3B     0x363B
3811 +#define OV5630_MIPI_CTRL3C     0x363C
3812 +#define OV5630_MIPI_CTRL3D     0x363D
3813 +#define OV5630_MIPI_CTRL3E     0x363E
3814 +#define OV5630_MIPI_CTRL3F     0x363F
3815 +#define OV5630_MIPI_RO61       0x3661
3816 +#define OV5630_MIPI_RO62       0x3662
3817 +#define OV5630_MIPI_RO63       0x3663
3818 +#define OV5630_MIPI_RO64       0x3664
3819 +#define OV5630_MIPI_RO65       0x3665
3820 +#define OV5630_MIPI_RO66       0x3666
3821 +
3822 +/* General definition for ov5630 */
3823 +#define OV5630_OUTWND_MAX_H            QSXXGA_PLUS4_SIZE_H
3824 +#define OV5630_OUTWND_MAX_V            QSXGA_PLUS4_SIZE_V
3825 +
3826 +struct regval_list {
3827 +       u16 reg_num;
3828 +       u8 value;
3829 +};
3830 +
3831 +/*
3832 + * Default register value
3833 + * 5Mega Pixel, 2592x1944
3834 + */
3835 +static struct regval_list ov5630_def_reg[] = {
3836 +       {0x300f, 0x00}, /*00*/
3837 +       {0x30b2, 0x32},
3838 +       {0x3084, 0x44},
3839 +       {0x3016, 0x01},
3840 +       {0x308a, 0x25},
3841 +
3842 +       {0x3013, 0xff},
3843 +       {0x3015, 0x03},
3844 +       {0x30bf, 0x02},
3845 +
3846 +       {0x3065, 0x50},
3847 +       {0x3068, 0x08},
3848 +       {0x30ac, 0x05},
3849 +       {0x309e, 0x24},
3850 +       {0x3091, 0x04},
3851 +
3852 +       {0x3075, 0x22},
3853 +       {0x3076, 0x23},
3854 +       {0x3077, 0x24},
3855 +       {0x3078, 0x25},
3856 +
3857 +       {0x30b5, 0x0c},
3858 +       {0x3090, 0x67},
3859 +
3860 +       {0x30f9, 0x11},
3861 +       {0x3311, 0x80},
3862 +       {0x3312, 0x1f},
3863 +
3864 +       {0x3103, 0x10},
3865 +       {0x305c, 0x01},
3866 +       {0x305d, 0x29},
3867 +       {0x305e, 0x00},
3868 +       {0x305f, 0xf7},
3869 +       {0x308d, 0x0b},
3870 +       {0x30ad, 0x20},
3871 +       {0x3072, 0x0d},
3872 +       {0x308b, 0x82},
3873 +       {0x3317, 0x9c},
3874 +       {0x3318, 0x22},
3875 +       {0x3025, 0x20},
3876 +       {0x3027, 0x08},
3877 +       {0x3029, 0x3f},
3878 +       {0x302b, 0xa3},
3879 +       {0x3319, 0x22},
3880 +       {0x30a1, 0xc4},
3881 +       {0x306a, 0x05},
3882 +       {0x3315, 0x22},
3883 +       {0x30ae, 0x25},
3884 +       {0x3304, 0x40},
3885 +       {0x3099, 0x49},
3886 +
3887 +       {0x300e, 0xb1/*b0*/}, /* Note this PLL setting*/
3888 +       {0x300f, 0x10}, /*00*/
3889 +       {0x3010, 0x07}, /*change from 0f according to SV */
3890 +       {0x3011, 0x40},
3891 +       {0x30af, 0x10},
3892 +       {0x304a, 0x00},
3893 +       {0x304d, 0x00},
3894 +
3895 +       {0x304e, 0x22},
3896 +       {0x304d, 0xa0},
3897 +       {0x3058, 0x00},
3898 +       {0x3059, 0xff},
3899 +       {0x305a, 0x00},
3900 +
3901 +       {0x30e9, 0x04},
3902 +       {0x3084, 0x44},
3903 +       {0x3090, 0x67},
3904 +       {0x30e9, 0x04},
3905 +
3906 +       {0x30b5, 0x1c},
3907 +       {0x331f, 0x22},
3908 +       {0x30ae, 0x15},
3909 +       {0x3304, 0x4c},
3910 +
3911 +       {0x3300, 0xfb},
3912 +       {0x3071, 0x34},
3913 +       {0x30e7, 0x01},
3914 +       {0x3302, 0x60},
3915 +       {0x331e, 0x05},
3916 +       {0x3321, 0x04},
3917 +
3918 +       /* Mark end */
3919 +       {0xffff, 0xff},
3920 +
3921 +};
3922 +
3923 +/* MIPI register are removed by Wen */
3924 +
3925 +/* 2592x1944 */
3926 +static struct regval_list ov5630_res_qsxga_plus4[] = {
3927 +       {0x3020, 0x07},
3928 +       {0x3021, 0xbc},
3929 +       {0x3022, 0x0c/*0a*/},
3930 +       {0x3023, 0xa0/*00*/},
3931 +       {0x305c, 0x01},
3932 +       {0x305d, 0x29},
3933 +       {0x305e, 0x00},
3934 +       {0x305f, 0xf7},
3935 +
3936 +       /* 30fps , 96 MHZ*/
3937 +       /* {0x300f, 0x10}, */
3938 +       {0x300f, 0x10},
3939 +       {0x300e, 0xb1},
3940 +       /* mipi */
3941 +#ifdef MIPI
3942 +       {0x30b0, 0x00},
3943 +       {0x30b1, 0xfc},
3944 +       {0x3603, 0x50},
3945 +       {0x3601, 0x0F},
3946 +       /* lan2 bit 10*/
3947 +       {0x3010, 0x07},
3948 +       {0x30fa, 0x01},
3949 +       /* {0x 30f8 09 */
3950 +       {0x3096, 0x50},
3951 +       /* end mipi*/
3952 +#else
3953 +       /* parrral */
3954 +       {0x30fa, 0x01},
3955 +#endif
3956 +       /* end post*/
3957 +       {0xffff, 0xff},
3958 +};
3959 +
3960 +/* 1920x1080 */
3961 +static struct regval_list ov5630_res_1080p[] = {
3962 +       /*res start*/
3963 +       {0x3020, 0x04},
3964 +       {0x3021, 0x5c},
3965 +       {0x3022, 0x0b/*0a*/},
3966 +       {0x3023, 0x32/*00*/},
3967 +       {0x305c, 0x01},
3968 +       {0x305d, 0x2c},
3969 +       {0x3024, 0x01},
3970 +       {0x3025, 0x6e/*70*/},
3971 +       {0x3026, 0x01},
3972 +       {0x3027, 0xb8},
3973 +       {0x3028, 0x08},
3974 +       {0x3029, 0xef},
3975 +       {0x302a, 0x05},
3976 +       {0x302b, 0xf3},
3977 +       {0x302c, 0x07},
3978 +       {0x302d, 0x80},
3979 +       {0x302e, 0x04},
3980 +       {0x302f, 0x38},
3981 +       {0x3314, 0x07},
3982 +       {0x3315, 0x82/*80*/},
3983 +       {0x3316, 0x04},
3984 +       {0x3317, 0x3c},
3985 +
3986 +       /* 30fps , 96 MHZ*/
3987 +       {0x300f, 0x10}, /* 00 */
3988 +       {0x300e, 0xb1},
3989 +
3990 +       /* mipi */
3991 +#ifdef MIPI
3992 +       {0x30b0, 0x00},
3993 +       {0x30b1, 0xfc},
3994 +       {0x3603, 0x50},
3995 +       {0x3601, 0x0F},
3996 +       /* lan2 bit 10*/
3997 +       {0x3010, 0x07},
3998 +       {0x30fa, 0x01},
3999 +       /* {0x 30f8 09 */
4000 +       {0x3096, 0x50},
4001 +       /* end mipi*/
4002 +#else
4003 +       /* parrral */
4004 +       {0x30fa, 0x01},
4005 +#endif
4006 +       /* end post*/
4007 +       {0xffff, 0xff},
4008 +};
4009 +
4010 +/* 1280x960 V1F2_H1F2 */
4011 +static struct regval_list ov5630_res_xga_plus[] = {
4012 +       {0x3020, 0x03},
4013 +       {0x3021, 0xe4},
4014 +       {0x3022, 0x0c/*07*/},
4015 +       {0x3023, 0x8c/*76*/},
4016 +       {0x305c, 0x00},
4017 +       {0x305d, 0xb1},
4018 +       {0x3024, 0x00},
4019 +       {0x3025, 0x30},
4020 +       {0x3026, 0x00},
4021 +       {0x3027, 0x10/*14*/},
4022 +       {0x3028, 0x0a},
4023 +       {0x3029, 0x2f},
4024 +       {0x302a, 0x07},
4025 +       {0x302b, 0xa7/*a7*/},
4026 +       {0x302c, 0x05},
4027 +       {0x302d, 0x00},
4028 +       {0x302e, 0x03},
4029 +       {0x302f, 0xc0},
4030 +
4031 +       {0x30f8, 0x05},
4032 +       {0x30f9, 0x13},
4033 +       {0x3314, 0x05},
4034 +       {0x3315, 0x02/*00*/},
4035 +       {0x3316, 0x03},
4036 +       {0x3317, 0xc4},
4037 +
4038 +       {0x300f, 0x10}, /* 00 */
4039 +       {0x300e, 0xb1},
4040 +
4041 +#ifdef MIPI
4042 +       {0x30b0, 0x00},
4043 +       {0x30b1, 0xfc},
4044 +       {0x3603, 0x50},
4045 +       {0x3601, 0x0F},
4046 +       /* lan2 bit 10*/
4047 +       {0x3010, 0x07},
4048 +       {0x30fa, 0x01},
4049 +       /* {0x 30f8 09 */
4050 +       {0x3096, 0x50},
4051 +       /* end mipi*/
4052 +#else
4053 +       /* parrral */
4054 +       {0x30fa, 0x01},
4055 +#endif
4056 +
4057 +       {0xffff, 0xff},
4058 +};
4059 +
4060 +/* 1280x720, V1F2 & H1F2 */
4061 +static struct regval_list ov5630_res_720p[] = {
4062 +       {0x3020, 0x02},
4063 +       {0x3021, 0xf4},
4064 +       {0x3022, 0x07},
4065 +       {0x3023, 0x80},
4066 +       {0x305c, 0x00},
4067 +       {0x305d, 0xff},
4068 +       {0x305e, 0x00},
4069 +       {0x305f, 0xd4},
4070 +
4071 +       /* Crop then downscale */
4072 +       {0x3024, 0x00},
4073 +       {0x3025, 0x2c},
4074 +       {0x3026, 0x00},
4075 +       {0x3027, 0xf0},
4076 +       {0x3028, 0x0a},
4077 +       {0x3029, 0x2f},
4078 +       {0x302a, 0x08},
4079 +       {0x302b, 0x97},
4080 +
4081 +       {0x30f8, 0x05},
4082 +
4083 +       {0x302c, 0x05},
4084 +       {0x302d, 0x00},
4085 +       {0x302e, 0x02},
4086 +       {0x302f, 0xd0},
4087 +
4088 +       {0x30f9, 0x13},
4089 +       {0x3314, 0x05},
4090 +       {0x3315, 0x04},
4091 +       {0x3316, 0x02},
4092 +       {0x3317, 0xd4},
4093 +
4094 +       /* Add this to test setting from OVT */
4095 +       {0x300f, 0x10}, /*00*/
4096 +       {0x300e, 0xb0},
4097 +
4098 +#ifdef MIPI
4099 +       {0x30b0, 0x00},
4100 +       {0x30b1, 0xfc},
4101 +       {0x3603, 0x50},
4102 +       {0x3601, 0x0F},
4103 +       /* lan2 bit 10*/
4104 +       {0x3010, 0x07},
4105 +       {0x30fa, 0x01},
4106 +       /* {0x 30f8 09 */
4107 +       {0x3096, 0x50},
4108 +       /* end mipi*/
4109 +#else
4110 +       /* parrral */
4111 +       {0x30fa, 0x01},
4112 +#endif
4113 +
4114 +       {0xffff, 0xff},
4115 +};
4116 +
4117 +/*VGA 40fps now*/
4118 +static struct regval_list ov5630_res_vga_ac04_bill[] = {
4119 +       /* res setting*/
4120 +       {0x3020, 0x02},
4121 +       {0x3021, 0x04},
4122 +       {0x3022, 0x08},
4123 +       {0x3023, 0x48},
4124 +       {0x305c, 0x00},
4125 +       {0x305d, 0x5e},
4126 +       {0x3024, 0x00},
4127 +       {0x3025, 0x2c},/*2c*/
4128 +       {0x3026, 0x00},
4129 +       {0x3027, 0x14},
4130 +       {0x3028, 0x0a},
4131 +       {0x3029, 0x2f},
4132 +       {0x302a, 0x07},
4133 +       {0x302b, 0xa3},
4134 +       {0x302c, 0x02},
4135 +       {0x302d, 0x80},
4136 +       {0x302e, 0x01},
4137 +       {0x302f, 0xe0},
4138 +
4139 +       {0x30b3, 0x09},
4140 +       {0x3301, 0xc1},
4141 +       {0x3313, 0xf1},
4142 +       {0x3314, 0x05},
4143 +       {0x3315, 0x04},/*04*/
4144 +       {0x3316, 0x01},
4145 +       {0x3317, 0xe4},
4146 +       {0x3318, 0x20},
4147 +
4148 +       {0x300f, 0x10/*00*/},
4149 +       {0x30f8, 0x09},
4150 +
4151 +       {0x300f, 0x11},
4152 +       {0x300e, 0xb2},
4153 +
4154 +       {0x3015, 0x02},
4155 +       /* mipi */
4156 +#ifdef MIPI
4157 +       {0x30b0, 0x00},
4158 +       {0x30b1, 0xfc},
4159 +       {0x3603, 0x50},
4160 +       {0x3601, 0x0F},
4161 +       /* lan2 bit 10*/
4162 +       {0x3010, 0x07},
4163 +       {0x30fa, 0x01},
4164 +       /* {0x 30f8 09 */
4165 +       {0x3096, 0x50},
4166 +       /* end mipi*/
4167 +#else
4168 +
4169 +       /* parrral */
4170 +       {0x30fa, 0x01},
4171 +       {0x30f8, 0x09},
4172 +       {0x3096, 0x50},
4173 +#endif
4174 +
4175 +       {0xffff, 0xff},
4176 +};
4177 diff --git a/drivers/media/video/mrstci/mrstov5630_motor/Kconfig b/drivers/media/video/mrstci/mrstov5630_motor/Kconfig
4178 new file mode 100644
4179 index 0000000..b6dcf62
4180 --- /dev/null
4181 +++ b/drivers/media/video/mrstci/mrstov5630_motor/Kconfig
4182 @@ -0,0 +1,9 @@
4183 +config VIDEO_MRST_OV5630_MOTOR
4184 +       tristate "Moorestown OV5630 motor"
4185 +       depends on I2C && VIDEO_MRST_ISP && VIDEO_MRST_OV5630
4186 +
4187 +       ---help---
4188 +         Say Y here if your platform support OV5630 motor
4189 +
4190 +         To compile this driver as a module, choose M here: the
4191 +         module will be called mrstov2650.ko.
4192 diff --git a/drivers/media/video/mrstci/mrstov5630_motor/Makefile b/drivers/media/video/mrstci/mrstov5630_motor/Makefile
4193 new file mode 100644
4194 index 0000000..056b2a6
4195 --- /dev/null
4196 +++ b/drivers/media/video/mrstci/mrstov5630_motor/Makefile
4197 @@ -0,0 +1,3 @@
4198 +obj-$(CONFIG_VIDEO_MRST_OV2650)         += mrstov5630_motor.o
4199 +
4200 +EXTRA_CFLAGS   +=      -I$(src)/../include
4201 diff --git a/drivers/media/video/mrstci/mrstov5630_motor/mrstov5630_motor.c b/drivers/media/video/mrstci/mrstov5630_motor/mrstov5630_motor.c
4202 new file mode 100644
4203 index 0000000..1bb7274
4204 --- /dev/null
4205 +++ b/drivers/media/video/mrstci/mrstov5630_motor/mrstov5630_motor.c
4206 @@ -0,0 +1,428 @@
4207 +/*
4208 + * Support for Moorestown Langwell Camera Imaging ISP subsystem.
4209 + *
4210 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
4211 + *
4212 + * This program is free software; you can redistribute it and/or
4213 + * modify it under the terms of the GNU General Public License version
4214 + * 2 as published by the Free Software Foundation.
4215 + *
4216 + * This program is distributed in the hope that it will be useful,
4217 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
4218 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
4219 + * GNU General Public License for more details.
4220 + *
4221 + * You should have received a copy of the GNU General Public License
4222 + * along with this program; if not, write to the Free Software
4223 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
4224 + * 02110-1301, USA.
4225 + *
4226 + *
4227 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
4228 + */
4229 +
4230 +#include <linux/module.h>
4231 +#include <linux/types.h>
4232 +#include <linux/kernel.h>
4233 +#include <linux/mm.h>
4234 +#include <linux/string.h>
4235 +#include <linux/errno.h>
4236 +#include <linux/init.h>
4237 +#include <linux/kmod.h>
4238 +#include <linux/device.h>
4239 +#include <linux/delay.h>
4240 +#include <linux/fs.h>
4241 +#include <linux/init.h>
4242 +#include <linux/slab.h>
4243 +#include <linux/delay.h>
4244 +#include <linux/i2c.h>
4245 +#include <linux/gpio.h>
4246 +
4247 +#include <media/v4l2-device.h>
4248 +#include <media/v4l2-chip-ident.h>
4249 +#include <media/v4l2-i2c-drv.h>
4250 +
4251 +#include "ov5630_motor.h"
4252 +
4253 +/* #define OSPM */
4254 +#include <asm/ipc_defs.h>
4255 +#define PMIC_WRITE1(ipcbuf, reg1, val1) \
4256 +       do { \
4257 +               memset(&ipcbuf, 0, sizeof(struct ipc_pmic_reg_data)); \
4258 +               ipcbuf.ioc = 0; \
4259 +               ipcbuf.num_entries = 1; \
4260 +               ipcbuf.pmic_reg_data[0].register_address = reg1; \
4261 +               ipcbuf.pmic_reg_data[0].value = val1; \
4262 +               if (ipc_pmic_register_write(&ipcbuf, 1) != 0) { \
4263 +                       return -1; \
4264 +               } \
4265 +       } while (0);
4266 +
4267 +static int mrstov5630_motor_debug;
4268 +module_param(mrstov5630_motor_debug, int, 0644);
4269 +MODULE_PARM_DESC(mrstov5630_motor_debug, "Debug level (0-1)");
4270 +
4271 +#define dprintk(level, fmt, arg...) do {                       \
4272 +       if (mrstov5630_motor_debug >= level)                            \
4273 +               printk(KERN_DEBUG "mrstisp@%s: " fmt "\n", \
4274 +                      __func__, ## arg); } \
4275 +       while (0)
4276 +
4277 +#define eprintk(fmt, arg...)   \
4278 +       printk(KERN_ERR "mrstisp@%s: line %d: " fmt "\n",       \
4279 +              __func__, __LINE__, ## arg);
4280 +
4281 +#define DBG_entering   dprintk(2, "entering");
4282 +#define DBG_leaving    dprintk(2, "leaving");
4283 +#define DBG_line       dprintk(2, " line: %d", __LINE__);
4284 +
4285 +static inline struct ov5630_motor *to_motor_config(struct v4l2_subdev *sd)
4286 +{
4287 +       return container_of(sd, struct ov5630_motor, sd);
4288 +}
4289 +
4290 +static int motor_read(struct i2c_client *c, u16 *reg)
4291 +{
4292 +       int ret;
4293 +       struct i2c_msg msg;
4294 +       u8 msgbuf[2];
4295 +
4296 +       /* Read needs two message to go */
4297 +       msgbuf[0] = 0;
4298 +       msgbuf[1] = 0;
4299 +
4300 +       memset(&msg, 0, sizeof(msg));
4301 +       msg.addr = c->addr;
4302 +       msg.buf = msgbuf;
4303 +       msg.len = 2;
4304 +       msg.flags = I2C_M_RD;
4305 +
4306 +       ret = i2c_transfer(c->adapter, &msg, 1);
4307 +
4308 +       *reg = (msgbuf[0] << 8 | msgbuf[1]);
4309 +
4310 +       ret = (ret == 1) ? 0 : -1;
4311 +       return ret;
4312 +}
4313 +
4314 +static int motor_write(struct i2c_client *c, u16 reg)
4315 +{
4316 +       int ret;
4317 +       struct i2c_msg msg;
4318 +       u8 msgbuf[2];
4319 +
4320 +       /* Writing only needs one message */
4321 +       memset(&msg, 0, sizeof(msg));
4322 +       msgbuf[0] = reg >> 8;
4323 +       msgbuf[1] = reg;
4324 +
4325 +       msg.addr = c->addr;
4326 +       msg.flags = 0;
4327 +       msg.buf = msgbuf;
4328 +       msg.len = 2;
4329 +
4330 +       ret = i2c_transfer(c->adapter, &msg, 1);
4331 +
4332 +       ret = (ret == 1) ? 0 : -1;
4333 +       return ret;
4334 +}
4335 +
4336 +static int ov5630_motor_goto_position(struct i2c_client *c,
4337 +                                     unsigned short code,
4338 +                                     struct ov5630_motor *config)
4339 +{
4340 +       int max_code, min_code;
4341 +       u8 cmdh, cmdl;
4342 +       u16 cmd, val = 0;
4343 +
4344 +       max_code = config->macro_code;
4345 +       min_code = config->infin_code;
4346 +
4347 +       if (code > max_code)
4348 +               code = max_code;
4349 +       if (code < min_code)
4350 +               code = min_code;
4351 +
4352 +       cmdh = (MOTOR_DAC_CODE_H(code));
4353 +       cmdl = (MOTOR_DAC_CODE_L(code) | MOTOR_DAC_CTRL_MODE_2(SUB_MODE_4));
4354 +       cmd = cmdh << 8 | cmdl;
4355 +
4356 +       motor_write(c, cmd);
4357 +       /*Delay more than full-scale transition time 8.8ms*/
4358 +       msleep(8);
4359 +       motor_read(c, &val);
4360 +
4361 +       return (cmd == val ? 0 : -1);
4362 +}
4363 +
4364 +int ov5630_motor_wakeup(void)
4365 +{
4366 +       return gpio_direction_output(GPIO_AF_PD, 1);
4367 +}
4368 +
4369 +int ov5630_motor_standby(void)
4370 +{
4371 +       return gpio_direction_output(GPIO_AF_PD, 0);
4372 +}
4373 +
4374 +int ov5630_motor_init(struct i2c_client *client, struct ov5630_motor *config)
4375 +{
4376 +       int ret;
4377 +       int infin_cur, macro_cur;
4378 +#ifdef OSPM
4379 +       /* Power on motor */
4380 +       struct ipc_pmic_reg_data ipcbuf;
4381 +
4382 +       PMIC_WRITE1(ipcbuf, 0x50, 0x27);
4383 +       printk(KERN_WARNING "Power on Vcc33 for motor\n");
4384 +#endif
4385 +
4386 +       infin_cur = MAX(MOTOR_INFIN_CUR, MOTOR_DAC_MIN_CUR);
4387 +       macro_cur = MIN(MOTOR_MACRO_CUR, MOTOR_DAC_MAX_CUR);
4388 +
4389 +       config->infin_cur = infin_cur;
4390 +       config->macro_cur = macro_cur;
4391 +
4392 +       config->infin_code = (int)((infin_cur * MOTOR_DAC_MAX_CODE)
4393 +                                  / MOTOR_DAC_MAX_CUR);
4394 +       config->macro_code = (int)((macro_cur * MOTOR_DAC_MAX_CODE)
4395 +                                  / MOTOR_DAC_MAX_CUR);
4396 +
4397 +       config->max_step = ((config->macro_code - config->infin_code)
4398 +                           >> MOTOR_STEP_SHIFT) + 1;
4399 +       /* Note here, maybe macro_code */
4400 +       ret = ov5630_motor_goto_position(client, config->infin_code, config);
4401 +       if (!ret)
4402 +               config->cur_code = config->infin_code;
4403 +       else
4404 +               printk(KERN_ERR "Error while initializing motor\n");
4405 +
4406 +       return ret;
4407 +}
4408 +
4409 +int ov5630_motor_set_focus(struct i2c_client *c, int step,
4410 +                          struct ov5630_motor *config)
4411 +{
4412 +       int s_code, ret;
4413 +       int max_step = config->max_step;
4414 +       unsigned int val = step;
4415 +
4416 +       DBG_entering;
4417 +       dprintk(1, "setting setp %d", step);
4418 +       if (val > max_step)
4419 +               val = max_step;
4420 +
4421 +       s_code = (val << MOTOR_STEP_SHIFT);
4422 +       s_code += config->infin_code;
4423 +
4424 +       ret = ov5630_motor_goto_position(c, s_code, config);
4425 +       if (!ret)
4426 +               config->cur_code = s_code;
4427 +
4428 +       DBG_leaving;
4429 +       return ret;
4430 +}
4431 +
4432 +static int ov5630_motor_s_ctrl(struct v4l2_subdev *sd,
4433 +                              struct v4l2_control *ctrl)
4434 +{
4435 +       struct i2c_client *c = v4l2_get_subdevdata(sd);
4436 +       struct ov5630_motor *config = to_motor_config(sd);
4437 +       int ret;
4438 +
4439 +       DBG_entering;
4440 +       ret = ov5630_motor_set_focus(c, ctrl->value, config);
4441 +       if (ret) {
4442 +               eprintk("error call ov5630_motor_set_focue");
4443 +               return ret;
4444 +       }
4445 +       DBG_leaving;
4446 +       return 0;
4447 +}
4448 +int ov5630_motor_get_focus(struct i2c_client *c, unsigned int *step,
4449 +                          struct ov5630_motor *config)
4450 +{
4451 +       int ret_step;
4452 +
4453 +       ret_step = ((config->cur_code - config->infin_code)
4454 +                   >> MOTOR_STEP_SHIFT);
4455 +
4456 +       if (ret_step <= config->max_step)
4457 +               *step = ret_step;
4458 +       else
4459 +               *step = config->max_step;
4460 +
4461 +       return 0;
4462 +}
4463 +
4464 +static int ov5630_motor_g_ctrl(struct v4l2_subdev *sd,
4465 +                              struct v4l2_control *ctrl)
4466 +{
4467 +       struct i2c_client *c = v4l2_get_subdevdata(sd);
4468 +       struct ov5630_motor *config = to_motor_config(sd);
4469 +       int ret;
4470 +
4471 +       DBG_entering;
4472 +       dprintk(2, "c = %p, config = %p, ctrl = %p", c, config, ctrl);
4473 +       ret = ov5630_motor_get_focus(c, &ctrl->value, config);
4474 +       if (ret) {
4475 +               eprintk("error call ov5630_motor_get_focue");
4476 +               return ret;
4477 +       }
4478 +       DBG_leaving;
4479 +       return 0;
4480 +}
4481 +int ov5630_motor_max_step(struct i2c_client *c, unsigned int *max_code,
4482 +                          struct ov5630_motor *config)
4483 +{
4484 +       if (config->max_step != 0)
4485 +               *max_code = config->max_step;
4486 +       return 0;
4487 +}
4488 +
4489 +static int ov5630_motor_queryctrl(struct v4l2_subdev *sd,
4490 +                           struct v4l2_queryctrl *qc)
4491 +{
4492 +       struct ov5630_motor *config = to_motor_config(sd);
4493 +
4494 +       DBG_entering;
4495 +
4496 +       if (qc->id != V4L2_CID_FOCUS_ABSOLUTE)
4497 +               return -EINVAL;
4498 +
4499 +       dprintk(1, "got focus range of %d", config->max_step);
4500 +       if (config->max_step != 0)
4501 +               qc->maximum = config->max_step;
4502 +       DBG_leaving;
4503 +       return 0;
4504 +}
4505 +static const struct v4l2_subdev_core_ops ov5630_motor_core_ops = {
4506 +       /*
4507 +       .queryctrl = ov5630_queryctrl,
4508 +       .g_ctrl = ov5630_g_ctrl,
4509 +       */
4510 +       .g_ctrl = ov5630_motor_g_ctrl,
4511 +       .s_ctrl = ov5630_motor_s_ctrl,
4512 +       .queryctrl = ov5630_motor_queryctrl,
4513 +};
4514 +
4515 +static const struct v4l2_subdev_ops ov5630_motor_ops = {
4516 +       .core = &ov5630_motor_core_ops,
4517 +};
4518 +
4519 +static int ov5630_motor_detect(struct i2c_client *client)
4520 +{
4521 +       struct i2c_adapter *adapter = client->adapter;
4522 +       int adap_id = i2c_adapter_id(adapter);
4523 +
4524 +       if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
4525 +               eprintk("error i2c check func");
4526 +               return -ENODEV;
4527 +       }
4528 +
4529 +       if (adap_id != 1) {
4530 +               eprintk("adap_id != 1");
4531 +               return -ENODEV;
4532 +       }
4533 +
4534 +       /* if (ov5630_motor_wakeup()) */
4535 +               /* return -ENODEV; */
4536 +       ov5630_motor_wakeup();
4537 +       ssleep(1);
4538 +
4539 +       /*
4540 +       ov5630_motor_read(client, (u32)OV5630_PID_H, &value);
4541 +       if ((u8)value != 0x56) {
4542 +               eprintk("PID != 0x56, but %x", value);
4543 +               dprintk(2, "client->addr = %x", client->addr);
4544 +               return -ENODEV;
4545 +       }
4546 +       */
4547 +
4548 +       return 0;
4549 +}
4550 +
4551 +static int ov5630_motor_probe(struct i2c_client *client,
4552 +                       const struct i2c_device_id *id)
4553 +{
4554 +       struct ov5630_motor *info;
4555 +       struct v4l2_subdev *sd;
4556 +       int ret = -1;
4557 +/*     struct i2c_client *motor; */
4558 +
4559 +       DBG_entering;
4560 +       v4l_info(client, "chip found @ 0x%x (%s)\n",
4561 +                client->addr << 1, client->adapter->name);
4562 +       /*
4563 +        * Setup sensor configuration structure
4564 +        */
4565 +       info = kzalloc(sizeof(struct ov5630_motor), GFP_KERNEL);
4566 +       if (!info) {
4567 +               eprintk("fail to malloc for ci_motor");
4568 +               ret = -ENOMEM;
4569 +               goto out;
4570 +       }
4571 +
4572 +       ret = ov5630_motor_detect(client);
4573 +       if (ret) {
4574 +               eprintk("error ov5630_motor_detect");
4575 +               goto out_free;
4576 +       }
4577 +
4578 +       sd = &info->sd;
4579 +       v4l2_i2c_subdev_init(sd, client, &ov5630_motor_ops);
4580 +
4581 +       /*
4582 +        * Initialization OV5630
4583 +        * then turn into standby mode
4584 +        */
4585 +       /* ret = ov5630_motor_standby(); */
4586 +       ret = ov5630_motor_init(client, info);
4587 +       if (ret) {
4588 +               eprintk("error calling ov5630_motor_init");
4589 +               goto out_free;
4590 +       }
4591 +
4592 +       ret = 0;
4593 +       goto out;
4594 +
4595 +out_free:
4596 +       kfree(info);
4597 +       DBG_leaving;
4598 +out:
4599 +       return ret;
4600 +}
4601 +
4602 +/*
4603 + * XXX: Need to be checked
4604 + */
4605 +static int ov5630_motor_remove(struct i2c_client *client)
4606 +{
4607 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
4608 +
4609 +       DBG_entering;
4610 +
4611 +       v4l2_device_unregister_subdev(sd);
4612 +       kfree(to_motor_config(sd));
4613 +
4614 +       DBG_leaving;
4615 +       return 0;
4616 +}
4617 +
4618 +static const struct i2c_device_id ov5630_motor_id[] = {
4619 +       {"ov5630_motor", 0},
4620 +       {}
4621 +};
4622 +MODULE_DEVICE_TABLE(i2c, ov5630_motor_id);
4623 +
4624 +static struct v4l2_i2c_driver_data v4l2_i2c_data = {
4625 +       .name = "ov5630_motor",
4626 +       .probe = ov5630_motor_probe,
4627 +       .remove = ov5630_motor_remove,
4628 +       /* .suspend = ov5630_suspend,
4629 +        * .resume = ov5630_resume, */
4630 +       .id_table = ov5630_motor_id,
4631 +};
4632 +MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
4633 +MODULE_DESCRIPTION("A low-level driver for OmniVision 5630 sensors");
4634 +MODULE_LICENSE("GPL");
4635 diff --git a/drivers/media/video/mrstci/mrstov5630_motor/ov5630_motor.h b/drivers/media/video/mrstci/mrstov5630_motor/ov5630_motor.h
4636 new file mode 100644
4637 index 0000000..302c218
4638 --- /dev/null
4639 +++ b/drivers/media/video/mrstci/mrstov5630_motor/ov5630_motor.h
4640 @@ -0,0 +1,86 @@
4641 +/*
4642 + * Support for Moorestown Langwell Camera Imaging ISP subsystem.
4643 + *
4644 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
4645 + *
4646 + * This program is free software; you can redistribute it and/or
4647 + * modify it under the terms of the GNU General Public License version
4648 + * 2 as published by the Free Software Foundation.
4649 + *
4650 + * This program is distributed in the hope that it will be useful,
4651 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
4652 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
4653 + * GNU General Public License for more details.
4654 + *
4655 + * You should have received a copy of the GNU General Public License
4656 + * along with this program; if not, write to the Free Software
4657 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
4658 + * 02110-1301, USA.
4659 + *
4660 + *
4661 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
4662 + */
4663 +
4664 +#include <media/v4l2-subdev.h>
4665 +
4666 +/* VCM start current (mA) */
4667 +#define MOTOR_INFIN_CUR                15
4668 +/* VCM max current for Macro (mA) */
4669 +#define MOTOR_MACRO_CUR                90
4670 +/* DAC output max current (mA) */
4671 +#define MOTOR_DAC_MAX_CUR      100
4672 +/* DAC output min current (mA) */
4673 +#define MOTOR_DAC_MIN_CUR      3
4674 +
4675 +#define MOTOR_DAC_BIT_RES      10
4676 +#define MOTOR_DAC_MAX_CODE     ((1 << MOTOR_DAC_BIT_RES) - 1)
4677 +
4678 +#define MOTOR_STEP_SHIFT       4
4679 +
4680 +#define MAX(x, y)      ((x) > (y) ? (x) : (y))
4681 +#define MIN(x, y)      ((x) < (y) ? (x) : (y))
4682 +
4683 +/* DAC register related define */
4684 +#define MOTOR_POWER_DOWN       (1 << 7)
4685 +#define                PD_ENABLE       (1 << 7)
4686 +#define                PD_DISABLE      (0)
4687 +
4688 +#define MOTOR_DAC_CODE_H(x)    ((x >> 4) & 0x3f)
4689 +#define MOTOR_DAC_CODE_L(x)    ((x << 4) & 0xf0)
4690 +
4691 +#define MOTOR_DAC_CTRL_MODE_0  0x00
4692 +#define MOTOR_DAC_CTRL_MODE_1(x)       (x & 0x07)
4693 +#define MOTOR_DAC_CTRL_MODE_2(x)       ((x & 0x07) | 0x08)
4694 +
4695 +#define SUB_MODE_1     0x01
4696 +#define SUB_MODE_2     0x02
4697 +#define SUB_MODE_3     0x03
4698 +#define SUB_MODE_4     0x04
4699 +#define SUB_MODE_5     0x05
4700 +#define SUB_MODE_6     0x06
4701 +#define SUB_MODE_7     0x07
4702 +
4703 +#define OV5630_MOTOR_ADDR      (0x18 >> 1)
4704 +#define POWER_EN_PIN   7
4705 +#define GPIO_AF_PD     95
4706 +
4707 +struct ov5630_motor{
4708 +       unsigned int infin_cur;
4709 +       unsigned int infin_code;
4710 +       unsigned int macro_cur;
4711 +       unsigned int macro_code;
4712 +       unsigned int max_step;
4713 +       unsigned int cur_code;
4714 +       struct v4l2_subdev sd;
4715 +};
4716 +
4717 +extern int ov5630_motor_init(struct i2c_client *client, struct ov5630_motor
4718 +                            *config);
4719 +extern int ov5630_motor_standby(void);
4720 +extern int ov5630_motor_wakeup(void);
4721 +extern int ov5630_motor_set_focus(struct i2c_client *c, int step,
4722 +                          struct ov5630_motor *config);
4723 +extern int ov5630_motor_get_focus(struct i2c_client *c, unsigned int *step,
4724 +                          struct ov5630_motor *config);
4725 +extern int ov5630_motor_max_step(struct i2c_client *c, unsigned int *max_code,
4726 +                          struct ov5630_motor *config);
4727 diff --git a/drivers/media/video/mrstci/mrstov9665/Kconfig b/drivers/media/video/mrstci/mrstov9665/Kconfig
4728 new file mode 100644
4729 index 0000000..ba9b692
4730 --- /dev/null
4731 +++ b/drivers/media/video/mrstci/mrstov9665/Kconfig
4732 @@ -0,0 +1,9 @@
4733 +config VIDEO_MRST_OV9665
4734 +       tristate "Moorestown OV9665 SoC Sensor"
4735 +       depends on I2C && VIDEO_MRST_ISP
4736 +
4737 +       ---help---
4738 +         Say Y here if your platform support OV9665 SoC Sensor.
4739 +
4740 +         To compile this driver as a module, choose M here: the
4741 +         module will be called mrstov9665.ko.
4742 diff --git a/drivers/media/video/mrstci/mrstov9665/Makefile b/drivers/media/video/mrstci/mrstov9665/Makefile
4743 new file mode 100644
4744 index 0000000..871b6bf
4745 --- /dev/null
4746 +++ b/drivers/media/video/mrstci/mrstov9665/Makefile
4747 @@ -0,0 +1,3 @@
4748 +obj-$(CONFIG_VIDEO_MRST_OV9665)         += mrstov9665.o
4749 +
4750 +EXTRA_CFLAGS   +=      -I$(src)/../include
4751 diff --git a/drivers/media/video/mrstci/mrstov9665/mrstov9665.c b/drivers/media/video/mrstci/mrstov9665/mrstov9665.c
4752 new file mode 100644
4753 index 0000000..04e553a
4754 --- /dev/null
4755 +++ b/drivers/media/video/mrstci/mrstov9665/mrstov9665.c
4756 @@ -0,0 +1,972 @@
4757 +/*
4758 + * Support for Moorestown Langwell Camera Imaging ISP subsystem.
4759 + *
4760 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
4761 + *
4762 + * This program is free software; you can redistribute it and/or
4763 + * modify it under the terms of the GNU General Public License version
4764 + * 2 as published by the Free Software Foundation.
4765 + *
4766 + * This program is distributed in the hope that it will be useful,
4767 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
4768 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
4769 + * GNU General Public License for more details.
4770 + *
4771 + * You should have received a copy of the GNU General Public License
4772 + * along with this program; if not, write to the Free Software
4773 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
4774 + * 02110-1301, USA.
4775 + *
4776 + *
4777 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
4778 + */
4779 +
4780 +#include <linux/module.h>
4781 +#include <linux/types.h>
4782 +#include <linux/kernel.h>
4783 +#include <linux/mm.h>
4784 +#include <linux/string.h>
4785 +#include <linux/errno.h>
4786 +#include <linux/init.h>
4787 +#include <linux/kmod.h>
4788 +#include <linux/device.h>
4789 +#include <linux/delay.h>
4790 +#include <linux/fs.h>
4791 +#include <linux/init.h>
4792 +#include <linux/slab.h>
4793 +#include <linux/delay.h>
4794 +#include <linux/i2c.h>
4795 +#include <linux/gpio.h>
4796 +#include <linux/videodev2.h>
4797 +
4798 +#include <media/v4l2-device.h>
4799 +#include <media/v4l2-chip-ident.h>
4800 +#include <media/v4l2-i2c-drv.h>
4801 +
4802 +#include "ci_sensor_common.h"
4803 +#include "ov9665.h"
4804 +
4805 +static int mrstov9665_debug;
4806 +module_param(mrstov9665_debug, int, 0644);
4807 +MODULE_PARM_DESC(mrstov9665_debug, "Debug level (0-1)");
4808 +
4809 +#define dprintk(level, fmt, arg...) do {                       \
4810 +       if (mrstov9665_debug >= level)                                  \
4811 +               printk(KERN_DEBUG "mrstisp@%s: " fmt "\n", \
4812 +                      __func__, ## arg); } \
4813 +       while (0)
4814 +
4815 +#define eprintk(fmt, arg...)   \
4816 +       printk(KERN_ERR "mrstisp@%s: line %d: " fmt "\n",       \
4817 +              __func__, __LINE__, ## arg);
4818 +
4819 +#define DBG_entering   dprintk(2, "entering");
4820 +#define DBG_leaving    dprintk(2, "leaving");
4821 +#define DBG_line       dprintk(2, " line: %d", __LINE__);
4822 +
4823 +static inline struct ci_sensor_config *to_sensor_config(struct v4l2_subdev *sd)
4824 +{
4825 +       return container_of(sd, struct ci_sensor_config, sd);
4826 +}
4827 +
4828 +static struct ov9665_format_struct {
4829 +       __u8 *desc;
4830 +       __u32 pixelformat;
4831 +       struct regval_list *regs;
4832 +} ov9665_formats[] = {
4833 +       {
4834 +               .desc           = "YUYV 4:2:2",
4835 +               .pixelformat    = SENSOR_MODE_BT601,
4836 +               .regs           = NULL,
4837 +       },
4838 +};
4839 +#define N_OV9665_FMTS ARRAY_SIZE(ov9665_formats)
4840 +
4841 +static struct ov9665_res_struct {
4842 +       __u8 *desc;
4843 +       int res;
4844 +       int width;
4845 +       int height;
4846 +       /* FIXME: correct the fps values.. */
4847 +       int fps;
4848 +       bool used;
4849 +       struct regval_list *regs;
4850 +} ov9665_res[] = {
4851 +       {
4852 +               .desc           = "SXGA",
4853 +               .res            = SENSOR_RES_SXGA,
4854 +               .width          = 1280,
4855 +               .height         = 1024,
4856 +               .fps            = 15,
4857 +               .used           = 0,
4858 +               .regs           = ov9665_res_sxga,
4859 +       },
4860 +       {
4861 +               .desc           = "VGA",
4862 +               .res            = SENSOR_RES_VGA,
4863 +               .width          = 640,
4864 +               .height         = 480,
4865 +               .fps            = 15,
4866 +               .used           = 0,
4867 +               .regs           = ov9665_res_vga,
4868 +       },
4869 +};
4870 +#define N_RES (ARRAY_SIZE(ov9665_res))
4871 +
4872 +/*
4873 + * I2C Read & Write stuff
4874 + */
4875 +static int ov9665_read(struct i2c_client *c, unsigned char reg,
4876 +                      unsigned char *value)
4877 +{
4878 +       int ret;
4879 +
4880 +       ret = i2c_smbus_read_byte_data(c, reg);
4881 +       if (ret >= 0) {
4882 +               *value = (unsigned char) ret;
4883 +               ret = 0;
4884 +       }
4885 +       return ret;
4886 +}
4887 +
4888 +static int ov9665_write(struct i2c_client *c, unsigned char reg,
4889 +                       unsigned char value)
4890 +{
4891 +       int ret = i2c_smbus_write_byte_data(c, reg, value);
4892 +       if (reg == 0x12 && (value & 0x80))
4893 +               msleep(2);  /* Wait for reset to run */
4894 +       return ret;
4895 +}
4896 +
4897 +/*
4898 + * Write a list of register settings; ff/ff stops the process.
4899 + */
4900 +static int ov9665_write_array(struct i2c_client *c, struct regval_list *vals)
4901 +{
4902 +       struct regval_list *p;
4903 +       u8 read_val = 0;
4904 +       int err_num = 0;
4905 +       int i = 0;
4906 +       p = vals;
4907 +       while (p->reg_num != 0xff) {
4908 +               ov9665_write(c, p->reg_num, p->value);
4909 +               ov9665_read(c, p->reg_num, &read_val);
4910 +               if (read_val != p->value)
4911 +                       err_num++;
4912 +               p++;
4913 +               i++;
4914 +       }
4915 +
4916 +       return 0;
4917 +}
4918 +
4919 +static int ov9665_set_data_pin_in(struct i2c_client *client)
4920 +{
4921 +       int ret = 0;
4922 +
4923 +       ret += ov9665_write(client, 0xd5, 0x00);
4924 +       ret += ov9665_write(client, 0xd6, 0x00);
4925 +
4926 +       return ret;
4927 +}
4928 +
4929 +static int ov9665_set_data_pin_out(struct i2c_client *client)
4930 +{
4931 +       int ret = 0;
4932 +
4933 +       ret += ov9665_write(client, 0xd5, 0xff);
4934 +       ret += ov9665_write(client, 0xd6, 0xff);
4935 +
4936 +       return ret;
4937 +}
4938 +/*
4939 + * Sensor specific helper function
4940 + */
4941 +static int ov9665_standby(void)
4942 +{
4943 +       /* Pull the pin to high to hardware standby */
4944 +       gpio_set_value(GPIO_STDBY_PIN, 1);
4945 +       dprintk(1, "PM: standby called\n");
4946 +       return 0;
4947 +}
4948 +
4949 +static int ov9665_wakeup(void)
4950 +{
4951 +       /* Pull the pin to low*/
4952 +       gpio_set_value(GPIO_STDBY_PIN, 0);
4953 +       dprintk(1, "PM: wakeup called\n");
4954 +       msleep(10);
4955 +       return 0;
4956 +}
4957 +
4958 +static int ov9665_s_power(struct v4l2_subdev *sd, u32 val)
4959 +{
4960 +       if (val == 1)
4961 +               ov9665_standby();
4962 +       if (val == 0)
4963 +               ov9665_wakeup();
4964 +       return 0;
4965 +}
4966 +
4967 +static int ov9665_init(struct i2c_client *c)
4968 +{
4969 +       int ret;
4970 +       struct v4l2_subdev *sd = i2c_get_clientdata(c);
4971 +       struct ci_sensor_config *info = to_sensor_config(sd);
4972 +       u8 reg = 0;
4973 +
4974 +       /* Fill the configuration structure */
4975 +       /* Note this default configuration value */
4976 +       info->mode = ov9665_formats[0].pixelformat;
4977 +       info->res = ov9665_res[0].res;
4978 +       info->type = SENSOR_TYPE_SOC;
4979 +       info->bls = SENSOR_BLS_OFF;
4980 +       info->gamma = SENSOR_GAMMA_ON;
4981 +       info->cconv = SENSOR_CCONV_ON;
4982 +       info->blc = SENSOR_BLC_AUTO;
4983 +       info->agc = SENSOR_AGC_AUTO;
4984 +       info->awb = SENSOR_AWB_AUTO;
4985 +       info->aec = SENSOR_AEC_AUTO;
4986 +       info->bus_width = SENSOR_BUSWIDTH_8BIT_ZZ;
4987 +       info->ycseq = SENSOR_YCSEQ_YCBYCR;
4988 +       info->conv422 = SENSOR_CONV422_COSITED;
4989 +       info->bpat = SENSOR_BPAT_GRGRBGBG;
4990 +       info->field_inv = SENSOR_FIELDINV_NOSWAP;
4991 +       info->field_sel = SENSOR_FIELDSEL_BOTH;
4992 +       info->hpol = SENSOR_HPOL_REFPOS;
4993 +       info->vpol = SENSOR_VPOL_POS;
4994 +       info->edge = SENSOR_EDGE_FALLING;
4995 +       info->flicker_freq = SENSOR_FLICKER_100;
4996 +       info->cie_profile = 0;
4997 +       memcpy(info->name, "ov9665", 7);
4998 +
4999 +       ret = ov9665_write(c, 0x12, 0x80);
5000 +       /* Set registers into default config value */
5001 +       ret += ov9665_write_array(c, ov9665_def_reg);
5002 +
5003 +       ov9665_read(c, 0x09, &reg);
5004 +       reg = reg | 0x10;
5005 +       ov9665_write(c, 0x09, reg);
5006 +       ov9665_set_data_pin_in(c);
5007 +       ssleep(1);
5008 +
5009 +       return ret;
5010 +}
5011 +
5012 +static int distance(struct ov9665_res_struct *res, u32 w, u32 h)
5013 +{
5014 +       int ret;
5015 +       if (res->width < w || res->height < h)
5016 +               return -1;
5017 +
5018 +       ret = ((res->width - w) + (res->height - h));
5019 +       return ret;
5020 +}
5021 +static int ov9665_try_res(u32 *w, u32 *h)
5022 +{
5023 +       struct ov9665_res_struct *res_index, *p = NULL;
5024 +       int dis, last_dis = ov9665_res->width + ov9665_res->height;
5025 +
5026 +       dprintk(1, "&&&&&  before %dx%d", *w, *h);
5027 +       for (res_index = ov9665_res;
5028 +            res_index < ov9665_res + N_RES;
5029 +            res_index++) {
5030 +               if ((res_index->width <= *w) && (res_index->height <= *h))
5031 +                       break;
5032 +               dis = distance(res_index, *w, *h);
5033 +               if (dis < last_dis) {
5034 +                       last_dis = dis;
5035 +                       p = res_index;
5036 +               }
5037 +       }
5038 +       if ((res_index->width < *w) || (res_index->height < *h)) {
5039 +               if (res_index != ov9665_res)
5040 +                       res_index--;
5041 +       }
5042 +
5043 +       /*
5044 +       if (p == NULL) {
5045 +               p = ov2650_res;
5046 +       }
5047 +
5048 +       if ((w != NULL) && (h != NULL)) {
5049 +               *w = p->width;
5050 +               *h = p->height;
5051 +       }
5052 +       */
5053 +       if (res_index == ov9665_res + N_RES)
5054 +               res_index = ov9665_res + N_RES - 1;
5055 +
5056 +       *w = res_index->width;
5057 +       *h = res_index->height;
5058 +
5059 +       dprintk(1, "&&&&&  after %dx%d", *w, *h);
5060 +       return 0;
5061 +}
5062 +
5063 +static struct ov9665_res_struct *ov9665_to_res(u32 w, u32 h)
5064 +{
5065 +       struct ov9665_res_struct *res_index;
5066 +
5067 +       for (res_index = ov9665_res;
5068 +            res_index < ov9665_res + N_RES;
5069 +            res_index++)
5070 +               if ((res_index->width == w) && (res_index->height == h))
5071 +                       break;
5072 +
5073 +       if (res_index >= ov9665_res + N_RES)
5074 +               res_index--;   /* Take the bigger one */
5075 +
5076 +       return res_index;
5077 +}
5078 +
5079 +static int ov9665_try_fmt(struct v4l2_subdev *sd,
5080 +                         struct v4l2_format *fmt)
5081 +{
5082 +       DBG_entering;
5083 +       return ov9665_try_res(&fmt->fmt.pix.width, &fmt->fmt.pix.height);
5084 +       DBG_leaving;
5085 +}
5086 +
5087 +static int ov9665_get_fmt(struct v4l2_subdev *sd,
5088 +                         struct v4l2_format *fmt)
5089 +{
5090 +       struct ci_sensor_config *info = to_sensor_config(sd);
5091 +       unsigned short width, height;
5092 +       int index;
5093 +
5094 +       ci_sensor_res2size(info->res, &width, &height);
5095 +
5096 +       /* Marked the current sensor res as being "used" */
5097 +       for (index = 0; index < N_RES; index++) {
5098 +               if ((width == ov9665_res[index].width) &&
5099 +                   (height == ov9665_res[index].height)) {
5100 +                       ov9665_res[index].used = 1;
5101 +                       continue;
5102 +               }
5103 +               ov9665_res[index].used = 0;
5104 +       }
5105 +
5106 +       fmt->fmt.pix.width = width;
5107 +       fmt->fmt.pix.height = height;
5108 +       return 0;
5109 +}
5110 +
5111 +static int ov9665_set_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
5112 +{
5113 +       struct i2c_client *c = v4l2_get_subdevdata(sd);
5114 +       struct ci_sensor_config *info = to_sensor_config(sd);
5115 +       int ret = 0;
5116 +       struct ov9665_res_struct *res_index;
5117 +       u32 width, height;
5118 +       int index;
5119 +
5120 +       DBG_entering;
5121 +
5122 +       width = fmt->fmt.pix.width;
5123 +       height = fmt->fmt.pix.height;
5124 +
5125 +       ret = ov9665_try_res(&width, &height);
5126 +       res_index = ov9665_to_res(width, height);
5127 +
5128 +       ov9665_wakeup();
5129 +       /* if ((info->res != res_index->res) && (res_index->regs)) { */
5130 +       if ( res_index->regs) {
5131 +               ret = ov9665_write(c, 0x12, 0x80);
5132 +               ret += ov9665_write_array(c, ov9665_def_reg);
5133 +               ret += ov9665_write_array(c, res_index->regs);
5134 +               /* Add delay here to get better image */
5135 +
5136 +               for (index = 0; index < N_RES; index++) {
5137 +                       if ((width == ov9665_res[index].width) &&
5138 +                           (height == ov9665_res[index].height)) {
5139 +                               ov9665_res[index].used = 1;
5140 +                               continue;
5141 +                       }
5142 +                       ov9665_res[index].used = 0;
5143 +               }
5144 +
5145 +               for (index = 0; index < N_RES; index++)
5146 +                       dprintk(2, "index = %d, used = %d\n", index,
5147 +                               ov9665_res[index].used);
5148 +
5149 +       }
5150 +       info->res = res_index->res;
5151 +
5152 +       DBG_leaving;
5153 +       return ret;
5154 +}
5155 +
5156 +static int ov9665_q_hflip(struct v4l2_subdev *sd, __s32 *value)
5157 +{
5158 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
5159 +       int ret;
5160 +       unsigned char v = 0;
5161 +
5162 +       ret = ov9665_read(client, 0x04, &v);
5163 +       *value = ((v & 0x80) == 0x80);
5164 +       return ret;
5165 +}
5166 +
5167 +static int ov9665_t_hflip(struct v4l2_subdev *sd, int value)
5168 +{
5169 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
5170 +       unsigned char v = 0;
5171 +       int ret;
5172 +
5173 +       value = value >= 1 ? 1 : 0;
5174 +       ret = ov9665_read(client, 0x33, &v);
5175 +       if (value)
5176 +               v |= 0x08;
5177 +       else
5178 +               v &= ~0x08;
5179 +       ret += ov9665_write(client, 0x33, v);
5180 +
5181 +       ret += ov9665_read(client, 0x04, &v);
5182 +       if (value)
5183 +               v |= 0x80;
5184 +       else
5185 +               v &= ~0x80;
5186 +       ret += ov9665_write(client, 0x04, v);
5187 +       msleep(10);  /* FIXME */
5188 +       return ret;
5189 +}
5190 +
5191 +static int ov9665_q_vflip(struct v4l2_subdev *sd, __s32 *value)
5192 +{
5193 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
5194 +       int ret;
5195 +       unsigned char v = 0;
5196 +
5197 +       ret = ov9665_read(client, 0x04, &v);
5198 +       *value = ((v & 0x40) == 0x40);
5199 +       return ret;
5200 +}
5201 +
5202 +static int ov9665_t_vflip(struct v4l2_subdev *sd, int value)
5203 +{
5204 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
5205 +       unsigned char v = 0;
5206 +       int ret;
5207 +
5208 +       value = value >= 1 ? 1 : 0;
5209 +       ret = ov9665_read(client, 0x04, &v);
5210 +       if (value)
5211 +               v |= 0x40;
5212 +       else
5213 +               v &= ~0x40;
5214 +       ret += ov9665_write(client, 0x04, v);
5215 +       msleep(10);  /* FIXME */
5216 +       return ret;
5217 +}
5218 +
5219 +static struct ov9665_control {
5220 +       struct v4l2_queryctrl qc;
5221 +       int (*query)(struct v4l2_subdev *sd, __s32 *value);
5222 +       int (*tweak)(struct v4l2_subdev *sd, int value);
5223 +} ov9665_controls[] = {
5224 +       {
5225 +               .qc = {
5226 +                       .id = V4L2_CID_VFLIP,
5227 +                       .type = V4L2_CTRL_TYPE_BOOLEAN,
5228 +                       .name = "Vertical flip",
5229 +                       .minimum = 0,
5230 +                       .maximum = 1,
5231 +                       .step = 1,
5232 +                       .default_value = 0,
5233 +               },
5234 +               .tweak = ov9665_t_vflip,
5235 +               .query = ov9665_q_vflip,
5236 +       },
5237 +       {
5238 +               .qc = {
5239 +                       .id = V4L2_CID_HFLIP,
5240 +                       .type = V4L2_CTRL_TYPE_BOOLEAN,
5241 +                       .name = "Horizontal mirror",
5242 +                       .minimum = 0,
5243 +                       .maximum = 1,
5244 +                       .step = 1,
5245 +                       .default_value = 0,
5246 +               },
5247 +               .tweak = ov9665_t_hflip,
5248 +               .query = ov9665_q_hflip,
5249 +       },
5250 +};
5251 +#define N_CONTROLS (ARRAY_SIZE(ov9665_controls))
5252 +
5253 +static struct ov9665_control *ov9665_find_control(__u32 id)
5254 +{
5255 +       int i;
5256 +
5257 +       for (i = 0; i < N_CONTROLS; i++)
5258 +               if (ov9665_controls[i].qc.id == id)
5259 +                       return ov9665_controls + i;
5260 +       return NULL;
5261 +}
5262 +
5263 +static int ov9665_queryctrl(struct v4l2_subdev *sd,
5264 +                           struct v4l2_queryctrl *qc)
5265 +{
5266 +       struct ov9665_control *ctrl = ov9665_find_control(qc->id);
5267 +
5268 +       if (ctrl == NULL)
5269 +               return -EINVAL;
5270 +       *qc = ctrl->qc;
5271 +       return 0;
5272 +}
5273 +
5274 +static int ov9665_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
5275 +{
5276 +       struct ov9665_control *octrl = ov9665_find_control(ctrl->id);
5277 +       int ret;
5278 +
5279 +       if (octrl == NULL)
5280 +               return -EINVAL;
5281 +       ret = octrl->query(sd, &ctrl->value);
5282 +       if (ret >= 0)
5283 +               return 0;
5284 +       return ret;
5285 +}
5286 +
5287 +static int ov9665_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
5288 +{
5289 +       struct ov9665_control *octrl = ov9665_find_control(ctrl->id);
5290 +       int ret;
5291 +
5292 +       if (octrl == NULL)
5293 +               return -EINVAL;
5294 +       ret =  octrl->tweak(sd, ctrl->value);
5295 +       if (ret >= 0)
5296 +               return 0;
5297 +       return ret;
5298 +}
5299 +
5300 +#if 0
5301 +static int ov9665_get_caps(struct i2c_client *c, struct ci_sensor_caps *caps)
5302 +{
5303 +       if (caps == NULL)
5304 +               return -EIO;
5305 +
5306 +       caps->bus_width = SENSOR_BUSWIDTH_8BIT_ZZ;
5307 +       caps->mode      = SENSOR_MODE_BT601;
5308 +       caps->field_inv = SENSOR_FIELDINV_NOSWAP;
5309 +       caps->field_sel = SENSOR_FIELDSEL_BOTH;
5310 +       caps->ycseq     = SENSOR_YCSEQ_YCBYCR;
5311 +       caps->conv422   = SENSOR_CONV422_COSITED;
5312 +       caps->bpat      = SENSOR_BPAT_GRGRBGBG;
5313 +       caps->hpol      = SENSOR_HPOL_REFPOS;
5314 +       caps->vpol      = SENSOR_VPOL_POS;
5315 +       caps->edge      = SENSOR_EDGE_FALLING;
5316 +       caps->bls       = SENSOR_BLS_OFF;
5317 +       caps->gamma     = SENSOR_GAMMA_ON;
5318 +       caps->cconv     = SENSOR_CCONV_ON;
5319 +       caps->res       = SENSOR_RES_SXGA | SENSOR_RES_VGA;
5320 +       caps->blc       = SENSOR_BLC_AUTO;
5321 +       caps->agc       = SENSOR_AGC_AUTO;
5322 +       caps->awb       = SENSOR_AWB_AUTO;
5323 +       caps->aec       = SENSOR_AEC_AUTO;
5324 +       caps->cie_profile       = 0;
5325 +       caps->flicker_freq      = SENSOR_FLICKER_100 | SENSOR_FLICKER_120;
5326 +       caps->type      = SENSOR_TYPE_SOC;
5327 +       /* caps->name   = "ov9665"; */
5328 +       strcpy(caps->name, "ov9665");
5329 +
5330 +       return 0;
5331 +}
5332 +
5333 +static int ov9665_get_config(struct i2c_client *c,
5334 +                            struct ci_sensor_config *config)
5335 +{
5336 +       struct ci_sensor_config *info = i2c_get_clientdata(c);
5337 +
5338 +       if (config == NULL) {
5339 +               printk(KERN_WARNING "sensor_get_config: NULL pointer\n");
5340 +               return -EIO;
5341 +       }
5342 +
5343 +       memset(config, 0, sizeof(struct ci_sensor_config *));
5344 +       memcpy(config, info, sizeof(struct ci_sensor_config));
5345 +
5346 +       return 0;
5347 +}
5348 +
5349 +static int ov9665_setup(struct i2c_client *c,
5350 +                       const struct ci_sensor_config *config)
5351 +{
5352 +       int ret;
5353 +       struct ov9665_res_struct *res_index;
5354 +       struct ci_sensor_config *info = i2c_get_clientdata(c);
5355 +       u16 width, high;
5356 +
5357 +       /* Soft reset camera first*/
5358 +       ret = ov9665_write(c, 0x12, 0x80);
5359 +
5360 +       /* Set registers into default config value */
5361 +       ret += ov9665_write_array(c, ov9665_def_reg);
5362 +
5363 +       /* set image resolution */
5364 +       ci_sensor_res2size(config->res, &width, &high);
5365 +       ret += ov9665_try_res(c, &width, &high);
5366 +       res_index = ov9665_find_res(width, high);
5367 +       if (res_index->regs)
5368 +               ret += ov9665_write_array(c, res_index->regs);
5369 +       if (!ret)
5370 +               info->res = res_index->res;
5371 +
5372 +       /* Add some delay here to get a better image*/
5373 +       ssleep(1);
5374 +
5375 +       return ret;
5376 +}
5377 +
5378 +static int ov9665_set_data_pin_in(struct i2c_client *client)
5379 +{
5380 +       int ret = 0;
5381 +
5382 +       ret += ov9665_write(client, 0xd5, 0x00);
5383 +       ret += ov9665_write(client, 0xd6, 0x00);
5384 +
5385 +       return ret;
5386 +}
5387 +
5388 +static int ov9665_set_data_pin_out(struct i2c_client *client)
5389 +{
5390 +       int ret = 0;
5391 +
5392 +       ret += ov9665_write(client, 0xd5, 0xff);
5393 +       ret += ov9665_write(client, 0xd6, 0xff);
5394 +
5395 +       return ret;
5396 +}
5397 +/*
5398 + * File operation functions
5399 + */
5400 +static int ov9665_open(struct i2c_setting *c, void *priv)
5401 +{
5402 +       struct i2c_client *client = c->sensor_client;
5403 +       int ret = 0;
5404 +       u8 reg = 0;
5405 +       /* Just wake up sensor */
5406 +       if (ov9665_wakeup())
5407 +               return -EIO;
5408 +
5409 +       ov9665_init(client);
5410 +       ret = ov9665_read(client, 0x09, &reg);
5411 +       reg = reg | 0x10;
5412 +       ret += ov9665_write(client, 0x09, reg);
5413 +
5414 +       if (ov9665_set_data_pin_in(client))
5415 +               return EIO;
5416 +/*
5417 +       if (ov9665_standby())
5418 +               return EIO;
5419 +*/
5420 +       return ret;
5421 +}
5422 +
5423 +static int ov9665_release(struct i2c_setting *c, void *priv)
5424 +{
5425 +       /* Just suspend the sensor */
5426 +       if (ov9665_standby())
5427 +               return EIO;
5428 +       return 0;
5429 +}
5430 +
5431 +static int ov9665_on(struct i2c_setting *c)
5432 +{
5433 +       struct i2c_client *client = c->sensor_client;
5434 +       int ret = 0;
5435 +       u8 reg = 0;
5436 +
5437 +       ret = ov9665_read(client, 0x09, &reg);
5438 +       reg = reg & ~0x10;
5439 +       ret = ov9665_write(client, 0x09, reg);
5440 +
5441 +       if (ov9665_set_data_pin_out(client))
5442 +               return EIO;
5443 +
5444 +       return ret;
5445 +}
5446 +
5447 +static int ov9665_off(struct i2c_setting *c)
5448 +{
5449 +       struct i2c_client *client = c->sensor_client;
5450 +       int ret = 0;
5451 +       u8 reg = 0;
5452 +/*
5453 +       ret = ov9665_read(client, 0x09, &reg);
5454 +       reg = reg | 0x10;
5455 +       ret += ov9665_write(client, 0x09, reg);
5456 +*/
5457 +       if (ov9665_set_data_pin_in(client))
5458 +               return EIO;
5459 +
5460 +       return ret;
5461 +}
5462 +
5463 +static struct sensor_device ov9665 = {
5464 +       .name   = "OV9665",
5465 +       .type   = SENSOR_TYPE_SOC,
5466 +       .minor  = -1,
5467 +       .open   = ov9665_open,
5468 +       .release = ov9665_release,
5469 +       .on = ov9665_on,
5470 +       .off = ov9665_off,
5471 +       .querycap = ov9665_get_caps,
5472 +       .get_config = ov9665_get_config,
5473 +       .set_config = ov9665_setup,
5474 +       .enum_parm = ov9665_queryctrl,
5475 +       .get_parm = ov9665_g_ctrl,
5476 +       .set_parm = ov9665_s_ctrl,
5477 +       .try_res = ov9665_try_res,
5478 +       .set_res = ov9665_set_res,
5479 +       .suspend = ov9665_standby,
5480 +       .resume = ov9665_wakeup,
5481 +       .get_ls_corr_config = NULL,
5482 +       .set_awb = NULL,
5483 +       .set_aec = NULL,
5484 +       .set_blc = NULL,
5485 +       /* TBC */
5486 +};
5487 +#endif
5488 +
5489 +static int ov9665_s_stream(struct v4l2_subdev *sd, int enable)
5490 +{
5491 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
5492 +       u8 reg = 0;
5493 +
5494 +       DBG_entering;
5495 +       if (enable) {
5496 +               ov9665_read(client, 0x09, &reg);
5497 +               reg = reg & ~0x10;
5498 +               ov9665_write(client, 0x09, reg);
5499 +               ov9665_set_data_pin_out(client);
5500 +               ssleep(1);
5501 +
5502 +       } else {
5503 +               ov9665_read(client, 0x09, &reg);
5504 +               reg = reg | 0x10;
5505 +               ov9665_write(client, 0x09, reg);
5506 +               ov9665_set_data_pin_in(client);
5507 +       }
5508 +
5509 +       DBG_leaving;
5510 +       return 0;
5511 +}
5512 +
5513 +static int ov9665_enum_framesizes(struct v4l2_subdev *sd,
5514 +                                 struct v4l2_frmsizeenum *fsize)
5515 +{
5516 +       unsigned int index = fsize->index;
5517 +
5518 +       DBG_entering;
5519 +
5520 +       if (index >= N_RES)
5521 +               return -EINVAL;
5522 +
5523 +       fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
5524 +       fsize->discrete.width = ov9665_res[index].width;
5525 +       fsize->discrete.height = ov9665_res[index].height;
5526 +       fsize->reserved[0] = ov9665_res[index].used;
5527 +
5528 +       DBG_leaving;
5529 +
5530 +       return 0;
5531 +}
5532 +
5533 +static int ov9665_enum_frameintervals(struct v4l2_subdev *sd,
5534 +                                     struct v4l2_frmivalenum *fival)
5535 +{
5536 +       unsigned int index = fival->index;
5537 +
5538 +       DBG_entering;
5539 +
5540 +       if (index >= N_RES)
5541 +               return -EINVAL;
5542 +
5543 +       fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
5544 +       fival->discrete.numerator = 1;
5545 +       fival->discrete.denominator = ov9665_res[index].fps;
5546 +
5547 +       DBG_leaving;
5548 +
5549 +       return 0;
5550 +}
5551 +
5552 +static int ov9665_g_chip_ident(struct v4l2_subdev *sd,
5553 +               struct v4l2_dbg_chip_ident *chip)
5554 +{
5555 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
5556 +
5557 +#define V4L2_IDENT_OV9665 8246
5558 +       return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_OV9665, 0);
5559 +}
5560 +
5561 +#ifdef CONFIG_VIDEO_ADV_DEBUG
5562 +static int ov9665_g_register(struct v4l2_subdev *sd,
5563 +                            struct v4l2_dbg_register *reg)
5564 +{
5565 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
5566 +       unsigned char val = 0;
5567 +       int ret;
5568 +
5569 +       if (!v4l2_chip_match_i2c_client(client, &reg->match))
5570 +               return -EINVAL;
5571 +       if (!capable(CAP_SYS_ADMIN))
5572 +               return -EPERM;
5573 +       ret = ov9665_read(client, reg->reg & 0xffff, &val);
5574 +       reg->val = val;
5575 +       reg->size = 1;
5576 +       return ret;
5577 +}
5578 +
5579 +static int ov9665_s_register(struct v4l2_subdev *sd,
5580 +                            struct v4l2_dbg_register *reg)
5581 +{
5582 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
5583 +
5584 +       if (!v4l2_chip_match_i2c_client(client, &reg->match))
5585 +               return -EINVAL;
5586 +       if (!capable(CAP_SYS_ADMIN))
5587 +               return -EPERM;
5588 +       ov9665_write(client, reg->reg & 0xffff, reg->val & 0xff);
5589 +       return 0;
5590 +}
5591 +#endif
5592 +
5593 +static const struct v4l2_subdev_video_ops ov9665_video_ops = {
5594 +       .try_fmt = ov9665_try_fmt,
5595 +       .s_fmt = ov9665_set_fmt,
5596 +       .g_fmt = ov9665_get_fmt,
5597 +       .s_stream = ov9665_s_stream,
5598 +       .enum_framesizes = ov9665_enum_framesizes,
5599 +       .enum_frameintervals = ov9665_enum_frameintervals,
5600 +};
5601 +
5602 +static const struct v4l2_subdev_core_ops ov9665_core_ops = {
5603 +       .g_chip_ident = ov9665_g_chip_ident,
5604 +       .queryctrl = ov9665_queryctrl,
5605 +       .g_ctrl = ov9665_g_ctrl,
5606 +       .s_ctrl = ov9665_s_ctrl,
5607 +       .s_gpio = ov9665_s_power,
5608 +       /*.g_ext_ctrls = ov9665_g_ext_ctrls,*/
5609 +       /*.s_ext_ctrls = ov9665_s_ext_ctrls,*/
5610 +#ifdef CONFIG_VIDEO_ADV_DEBUG
5611 +       .g_register = ov9665_g_register,
5612 +       .s_register = ov9665_s_register,
5613 +#endif
5614 +};
5615 +
5616 +static const struct v4l2_subdev_ops ov9665_ops = {
5617 +       .core = &ov9665_core_ops,
5618 +       .video = &ov9665_video_ops,
5619 +};
5620 +/*
5621 + * Basic i2c stuff
5622 + */
5623 +/*
5624 +static unsigned short normal_i2c[] = {0x30, I2C_CLIENT_END};
5625 +I2C_CLIENT_INSMOD;
5626 +
5627 +static struct i2c_driver ov9665_driver;
5628 +*/
5629 +static int ov9665_detect(struct i2c_client *client)
5630 +{
5631 +       struct i2c_adapter *adapter = client->adapter;
5632 +       int adap_id = i2c_adapter_id(adapter);
5633 +       u8 config = 0;
5634 +
5635 +       if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
5636 +               return -ENODEV;
5637 +
5638 +       if (adap_id != 1)
5639 +               return -ENODEV;
5640 +
5641 +       ov9665_wakeup();
5642 +
5643 +       ov9665_read(client, 0x0a, &config);
5644 +       if (config != 0x96)
5645 +               return -ENODEV;
5646 +
5647 +       ov9665_read(client, 0x0b, &config);
5648 +       if (config != 0x63)
5649 +               return -ENODEV;
5650 +
5651 +       return 0;
5652 +}
5653 +
5654 +static int ov9665_probe(struct i2c_client *client,
5655 +                       const struct i2c_device_id *id)
5656 +{
5657 +       struct ci_sensor_config *info;
5658 +       struct v4l2_subdev *sd;
5659 +       int ret = -1;
5660 +
5661 +       DBG_entering;
5662 +       /*
5663 +        * Setup sensor configuration structure
5664 +        */
5665 +       info = kzalloc(sizeof(struct ci_sensor_config), GFP_KERNEL);
5666 +       if (!info)
5667 +               return -ENOMEM;
5668 +
5669 +       ret = ov9665_detect(client);
5670 +       if (ret) {
5671 +               kfree(info);
5672 +               return -ENODEV;
5673 +       }
5674 +
5675 +       sd = &info->sd;
5676 +       v4l2_i2c_subdev_init(sd, client, &ov9665_ops);
5677 +
5678 +       /*
5679 +        * Initialization OV9665
5680 +        * then turn into standby mode
5681 +        */
5682 +       /* ret = ov9665_standby(); */
5683 +       ret = ov9665_init(client);
5684 +       if (ret) {
5685 +               eprintk("error init ov9665");
5686 +               goto err_1;
5687 +       }
5688 +
5689 +       ov9665_standby();
5690 +       printk(KERN_INFO "Init ov9665 sensor success\n");
5691 +       DBG_leaving;
5692 +       return 0;
5693 +
5694 +err_1:
5695 +       kfree(info);
5696 +       return ret;
5697 +}
5698 +
5699 +/*
5700 + * XXX: Need to be checked
5701 + */
5702 +static int ov9665_remove(struct i2c_client *client)
5703 +{
5704 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
5705 +
5706 +       v4l2_device_unregister_subdev(sd);
5707 +       kfree(to_sensor_config(sd));
5708 +
5709 +       return 0;
5710 +}
5711 +
5712 +static const struct i2c_device_id ov9665_id[] = {
5713 +       {"ov9665", 0},
5714 +       {}
5715 +};
5716 +
5717 +MODULE_DEVICE_TABLE(i2c, ov9665_id);
5718 +
5719 +static struct v4l2_i2c_driver_data v4l2_i2c_data = {
5720 +       .name = "ov9665",
5721 +       .probe = ov9665_probe,
5722 +       .remove = ov9665_remove,
5723 +       .id_table = ov9665_id,
5724 +};
5725 +
5726 +MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
5727 +MODULE_DESCRIPTION("A low-level driver for OmniVision 9665 sensors");
5728 +MODULE_LICENSE("GPL");
5729 diff --git a/drivers/media/video/mrstci/mrstov9665/ov9665.h b/drivers/media/video/mrstci/mrstov9665/ov9665.h
5730 new file mode 100644
5731 index 0000000..6fc9d12
5732 --- /dev/null
5733 +++ b/drivers/media/video/mrstci/mrstov9665/ov9665.h
5734 @@ -0,0 +1,263 @@
5735 +/*
5736 + * Support for Moorestown Langwell Camera Imaging ISP subsystem.
5737 + *
5738 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
5739 + *
5740 + * This program is free software; you can redistribute it and/or
5741 + * modify it under the terms of the GNU General Public License version
5742 + * 2 as published by the Free Software Foundation.
5743 + *
5744 + * This program is distributed in the hope that it will be useful,
5745 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
5746 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
5747 + * GNU General Public License for more details.
5748 + *
5749 + * You should have received a copy of the GNU General Public License
5750 + * along with this program; if not, write to the Free Software
5751 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
5752 + * 02110-1301, USA.
5753 + *
5754 + *
5755 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
5756 + */
5757 +
5758 +#define I2C_OV9665     0x60
5759 +/* Should add to kernel source */
5760 +#define I2C_DRIVERID_OV9665    1047
5761 +/* GPIO pin on Moorestown */
5762 +#define GPIO_SCLK_25   44
5763 +#define GPIO_STB_PIN   47
5764 +#define GPIO_STDBY_PIN 48
5765 +#define GPIO_RESET_PIN 50
5766 +
5767 +struct regval_list {
5768 +       u8 reg_num;
5769 +       u8 value;
5770 +};
5771 +
5772 +/*
5773 + * Default register value
5774 + * 1280x1024 YUV
5775 + */
5776 +static struct regval_list ov9665_def_reg[] = {
5777 +       {0x3E, 0x80},
5778 +       {0x12, 0x80},
5779 +
5780 +       {0xd5, 0xff},
5781 +       {0xd6, 0x3f},
5782 +
5783 +       {0x3d, 0x3c},
5784 +       {0x11, 0x81},
5785 +       {0x2a, 0x00},
5786 +       {0x2b, 0x00},
5787 +
5788 +       {0x3a, 0xf1},
5789 +       {0x3b, 0x00},
5790 +       {0x3c, 0x58},
5791 +       {0x3e, 0x50},
5792 +       {0x71, 0x00},
5793 +
5794 +       {0x15, 0x00},
5795 +       {0x6a, 0x24},
5796 +       {0x85, 0xe7},
5797 +
5798 +       {0x63, 0x01},
5799 +
5800 +       {0x17, 0x0c},
5801 +       {0x18, 0x5c},
5802 +       {0x19, 0x01},
5803 +       {0x1a, 0x82},
5804 +       {0x03, 0x03},
5805 +       {0x2b, 0x00},
5806 +
5807 +       {0x36, 0xb4},
5808 +       {0x65, 0x10},
5809 +       {0x70, 0x02},
5810 +       {0x71, 0x9f},
5811 +       {0x64, 0x24},
5812 +
5813 +       {0x43, 0x00},
5814 +       {0x5D, 0x55},
5815 +       {0x5E, 0x57},
5816 +       {0x5F, 0x21},
5817 +
5818 +       {0x24, 0x3e},
5819 +       {0x25, 0x38},
5820 +       {0x26, 0x72},
5821 +
5822 +       {0x14, 0x68},
5823 +       {0x0C, 0x3a}, /* Auto detect for 50/60 */
5824 +       {0x4F, 0x9E},
5825 +       {0x50, 0x84},
5826 +       {0x5A, 0x67},
5827 +
5828 +       {0x7d, 0x30},
5829 +       {0x7e, 0x00},
5830 +       {0x82, 0x03},
5831 +       {0x7f, 0x00},
5832 +       {0x83, 0x07},
5833 +       {0x80, 0x03},
5834 +       {0x81, 0x04},
5835 +
5836 +       {0x96, 0xf0},
5837 +       {0x97, 0x00},
5838 +       {0x92, 0x33},
5839 +       {0x94, 0x5a},
5840 +       {0x93, 0x3a},
5841 +       {0x95, 0x48},
5842 +       {0x91, 0xfc},
5843 +       {0x90, 0xff},
5844 +       {0x8e, 0x4e},
5845 +       {0x8f, 0x4e},
5846 +       {0x8d, 0x13},
5847 +       {0x8c, 0x0c},
5848 +       {0x8b, 0x0c},
5849 +       {0x86, 0x9e},
5850 +       {0x87, 0x11},
5851 +       {0x88, 0x22},
5852 +       {0x89, 0x05},
5853 +       {0x8a, 0x03},
5854 +
5855 +       {0x9b, 0x0e},
5856 +       {0x9c, 0x1c},
5857 +       {0x9d, 0x34},
5858 +       {0x9e, 0x5a},
5859 +       {0x9f, 0x68},
5860 +       {0xa0, 0x76},
5861 +       {0xa1, 0x82},
5862 +       {0xa2, 0x8e},
5863 +       {0xa3, 0x98},
5864 +       {0xa4, 0xa0},
5865 +       {0xa5, 0xb0},
5866 +       {0xa6, 0xbe},
5867 +       {0xa7, 0xd2},
5868 +       {0xa8, 0xe2},
5869 +       {0xa9, 0xee},
5870 +       {0xaa, 0x18},
5871 +
5872 +       {0xAB, 0xe7},
5873 +       {0xb0, 0x43},
5874 +       {0xac, 0x04},
5875 +       {0x84, 0x40},
5876 +
5877 +       {0xad, 0x84},
5878 +       {0xd9, 0x24},
5879 +       {0xda, 0x00},
5880 +       {0xae, 0x10},
5881 +
5882 +       {0xab, 0xe7},
5883 +       {0xb9, 0xa0},
5884 +       {0xba, 0x80},
5885 +       {0xbb, 0xa0},
5886 +       {0xbc, 0x80},
5887 +
5888 +       {0xbd, 0x08},
5889 +       {0xbe, 0x19},
5890 +       {0xbf, 0x02},
5891 +       {0xc0, 0x08},
5892 +       {0xc1, 0x2a},
5893 +       {0xc2, 0x34},
5894 +       {0xc3, 0x2d},
5895 +       {0xc4, 0x2d},
5896 +       {0xc5, 0x00},
5897 +       {0xc6, 0x98},
5898 +       {0xc7, 0x18},
5899 +       {0x69, 0x48},
5900 +
5901 +       {0x74, 0xc0},
5902 +
5903 +       {0x7c, 0x18},
5904 +       {0x65, 0x11},
5905 +       {0x66, 0x00},
5906 +       {0x41, 0xa0},
5907 +       {0x5b, 0x28},
5908 +       {0x60, 0x84},
5909 +       {0x05, 0x07},
5910 +       {0x03, 0x03},
5911 +       {0xd2, 0x8c},
5912 +
5913 +       {0xc7, 0x90},
5914 +       {0xc8, 0x06},
5915 +       {0xcb, 0x40},
5916 +       {0xcc, 0x40},
5917 +       {0xcf, 0x00},
5918 +       {0xd0, 0x20},
5919 +       {0xd1, 0x00},
5920 +       {0xc7, 0x18},
5921 +
5922 +       {0x0d, 0x82},
5923 +       {0x0d, 0x80},
5924 +
5925 +       {0x09, 0x01},
5926 +
5927 +       {0xff, 0xff},
5928 +};
5929 +
5930 +/* 1280x1024 */
5931 +static struct regval_list ov9665_res_sxga[] = {
5932 +       {0x0c, 0xbc}, /* note this */
5933 +       {0xff, 0xff},
5934 +};
5935 +
5936 +/* 640x480 */
5937 +static struct regval_list ov9665_res_vga[] = {
5938 +       /* Fclk/4 */
5939 +       {0x11, 0x80},
5940 +       {0x63, 0x00},
5941 +
5942 +       {0x12, 0x40}, /*VGA format*/
5943 +       {0x14, 0x30}, /*4x*/
5944 +       {0x0c, 0xbc},
5945 +       {0x4d, 0x09},
5946 +       {0x5c, 0x80}, /* Full average AEC */
5947 +
5948 +       /* Windows setting */
5949 +       {0x17, 0x0c},
5950 +       {0x18, 0x5c},
5951 +       {0x19, 0x02},
5952 +       {0x1a, 0x3f},
5953 +       {0x03, 0x03},
5954 +       {0x32, 0xad},
5955 +
5956 +       /* 50/60Hz AEC */
5957 +       {0x5a, 0x23},
5958 +       {0x2b, 0x00},
5959 +
5960 +       {0x64, 0xa4},
5961 +       /*
5962 +       {0x4F, 0x4f},
5963 +       {0x50, 0x42},
5964 +       */
5965 +       {0x4F, 0x9e},
5966 +       {0x50, 0x84},
5967 +       {0x97, 0x0a},
5968 +       {0xad, 0x82},
5969 +       {0xd9, 0x11},
5970 +
5971 +       /* Scale window */
5972 +       {0xb9, 0x50},
5973 +       {0xba, 0x3c},
5974 +       {0xbb, 0x50},
5975 +       {0xbc, 0x3c},
5976 +
5977 +       {0xad, 0x80},
5978 +       {0xd9, 0x00},
5979 +       {0xac, 0x0f},
5980 +       {0x84, 0x86},
5981 +
5982 +       /*This is for Color Matrix*/
5983 +       {0xbd, 0x05},
5984 +       {0xbe, 0x16},
5985 +       {0xbf, 0x05},
5986 +       {0xc0, 0x07},
5987 +       {0xc1, 0x18},
5988 +       {0xc2, 0x1f},
5989 +       {0xc3, 0x2b},
5990 +       {0xc4, 0x2b},
5991 +       {0xc5, 0x00},
5992 +
5993 +       {0x0d, 0x92},
5994 +       {0x0d, 0x90},
5995 +
5996 +       {0xff, 0xff},
5997 +};
5998 diff --git a/drivers/media/video/mrstci/mrsts5k4e1/Kconfig b/drivers/media/video/mrstci/mrsts5k4e1/Kconfig
5999 new file mode 100755
6000 index 0000000..7dee787
6001 --- /dev/null
6002 +++ b/drivers/media/video/mrstci/mrsts5k4e1/Kconfig
6003 @@ -0,0 +1,9 @@
6004 +config VIDEO_MRST_S5K4E1
6005 +       tristate "Moorestown s5k4e1 RAW Sensor"
6006 +       depends on I2C && VIDEO_MRST_ISP
6007 +
6008 +       ---help---
6009 +         Say Y here if your platform support s5k4e1 RAW Sensor.
6010 +
6011 +         To compile this driver as a module, choose M here: the
6012 +         module will be called mrstov2650.ko.
6013 diff --git a/drivers/media/video/mrstci/mrsts5k4e1/Makefile b/drivers/media/video/mrstci/mrsts5k4e1/Makefile
6014 new file mode 100644
6015 index 0000000..8733fa8
6016 --- /dev/null
6017 +++ b/drivers/media/video/mrstci/mrsts5k4e1/Makefile
6018 @@ -0,0 +1,3 @@
6019 +obj-$(CONFIG_VIDEO_MRST_S5K4E1)         += mrsts5k4e1.o
6020 +
6021 +EXTRA_CFLAGS   +=      -I$(src)/../include
6022 diff --git a/drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.c b/drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.c
6023 new file mode 100755
6024 index 0000000..f644531
6025 --- /dev/null
6026 +++ b/drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.c
6027 @@ -0,0 +1,1024 @@
6028 +/*
6029 + * Support for Moorestown Langwell Camera Imaging ISP subsystem.
6030 + *
6031 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
6032 + *
6033 + * This program is free software; you can redistribute it and/or
6034 + * modify it under the terms of the GNU General Public License version
6035 + * 2 as published by the Free Software Foundation.
6036 + *
6037 + * This program is distributed in the hope that it will be useful,
6038 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
6039 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
6040 + * GNU General Public License for more details.
6041 + *
6042 + * You should have received a copy of the GNU General Public License
6043 + * along with this program; if not, write to the Free Software
6044 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
6045 + * 02110-1301, USA.
6046 + *
6047 + *
6048 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
6049 + */
6050 +
6051 +#include <linux/module.h>
6052 +#include <linux/types.h>
6053 +#include <linux/kernel.h>
6054 +#include <linux/mm.h>
6055 +#include <linux/string.h>
6056 +#include <linux/errno.h>
6057 +#include <linux/init.h>
6058 +#include <linux/kmod.h>
6059 +#include <linux/device.h>
6060 +#include <linux/delay.h>
6061 +#include <linux/fs.h>
6062 +#include <linux/init.h>
6063 +#include <linux/slab.h>
6064 +#include <linux/delay.h>
6065 +#include <linux/i2c.h>
6066 +#include <linux/gpio.h>
6067 +
6068 +#include <media/v4l2-device.h>
6069 +#include <media/v4l2-chip-ident.h>
6070 +#include <media/v4l2-i2c-drv.h>
6071 +
6072 +#include "ci_sensor_common.h"
6073 +#include "mrsts5k4e1.h"
6074 +/* #include "priv.h" */
6075 +/* extern const struct DumpRegs regs_d[]; */
6076 +
6077 +static int s5k4e1_debug;
6078 +module_param(s5k4e1_debug, int, 0644);
6079 +MODULE_PARM_DESC(s5k4e1_debug, "Debug level (0-1)");
6080 +
6081 +#define dprintk(level, fmt, arg...) \
6082 +       do { \
6083 +               if (s5k4e1_debug >= level) \
6084 +                       printk(KERN_DEBUG "mrstisp@%s: " fmt "\n", \
6085 +                              __func__, ## arg);\
6086 +       } while (0)
6087 +
6088 +#define eprintk(fmt, arg...)   \
6089 +       printk(KERN_ERR "mrstisp@%s:" fmt "\n", \
6090 +              __func__, ## arg);
6091 +
6092 +#define DBG_entering   dprintk(1, "entering");
6093 +#define DBG_leaving    dprintk(1, "leaving");
6094 +#define DBG_line       dprintk(1, " line: %d", __LINE__);
6095 +
6096 +static inline struct ci_sensor_config *to_sensor_config(struct v4l2_subdev *sd)
6097 +{
6098 +       return container_of(sd, struct ci_sensor_config, sd);
6099 +}
6100 +
6101 +static struct s5k4e1_format_struct {
6102 +       __u8 *desc;
6103 +       __u32 pixelformat;
6104 +       struct regval_list *regs;
6105 +} s5k4e1_formats[] = {
6106 +       {
6107 +               .desc           = "Raw RGB Bayer",
6108 +               .pixelformat    = SENSOR_MODE_MIPI,
6109 +               .regs           = NULL,
6110 +       },
6111 +};
6112 +#define N_S5K4E1_FMTS ARRAY_SIZE(s5k4e1_formats)
6113 +
6114 +static struct s5k4e1_res_struct {
6115 +       __u8 *desc;
6116 +       int res;
6117 +       int width;
6118 +       int height;
6119 +       /* FIXME: correct the fps values.. */
6120 +       int fps;
6121 +       bool used;
6122 +       struct regval_list *regs;
6123 +} s5k4e1_res[] = {
6124 +       {
6125 +               .desc           = "QSXGA_PLUS4",
6126 +               .res            = SENSOR_RES_QXGA_PLUS,
6127 +               .width          = 2592,
6128 +               .height         = 1944,
6129 +               .fps            = 15,
6130 +               .used           = 0,
6131 +               .regs           = s5k4e1_res_qsxga_plus4,
6132 +       },
6133 +       {
6134 +               .desc           = "1080P",
6135 +               .res            = SENSOR_RES_1080P,
6136 +               .width          = 1920,
6137 +               .height         = 1080,
6138 +               .fps            = 25,
6139 +               .used           = 0,
6140 +               .regs           = s5k4e1_res_1080p,
6141 +       },
6142 +       {
6143 +               .desc           = "VGA_PLUS",
6144 +               .res            = SENSOR_RES_VGA_PLUS,
6145 +               .width          = 1304,
6146 +               .height         = 980,
6147 +               .fps            = 30,
6148 +               .used           = 0,
6149 +               .regs           = s5k4e1_res_vga_ac04_bill,
6150 +       },
6151 +       {
6152 +               .desc           = "720p",
6153 +               .res            = SENSOR_RES_720P,
6154 +               .width          = 1280,
6155 +               .height         = 720,
6156 +               .fps            = 30,
6157 +               .used           = 0,
6158 +               .regs           = s5k4e1_res_720p,
6159 +       },
6160 +       {
6161 +               .desc           = "VGA",
6162 +               .res            = SENSOR_RES_VGA,
6163 +               .width          = 640,
6164 +               .height         = 480,
6165 +               .used           = 0,
6166 +               .fps            = 40,
6167 +               .regs           = s5k4e1_res_vga_ac04_bill,
6168 +       },
6169 +};
6170 +
6171 +#define N_RES (ARRAY_SIZE(s5k4e1_res))
6172 +
6173 +/*
6174 + * I2C Read & Write stuff
6175 + */
6176 +static int s5k4e1_read(struct i2c_client *c, u32 reg, u32 *value)
6177 +{
6178 +       int ret;
6179 +       int i;
6180 +       struct i2c_msg msg[2];
6181 +       u8 msgbuf[2];
6182 +       u8 ret_val = 0;
6183 +       *value = 0;
6184 +       /* Read needs two message to go */
6185 +       memset(&msg, 0, sizeof(msg));
6186 +       msgbuf[0] = 0;
6187 +       msgbuf[1] = 0;
6188 +       i = 0;
6189 +
6190 +       msgbuf[i++] = ((u16)reg) >> 8;
6191 +       msgbuf[i++] = ((u16)reg) & 0xff;
6192 +       msg[0].addr = c->addr;
6193 +       msg[0].buf = msgbuf;
6194 +       msg[0].len = i;
6195 +
6196 +       msg[1].addr = c->addr;
6197 +       msg[1].flags = I2C_M_RD;
6198 +       msg[1].buf = &ret_val;
6199 +       msg[1].len = 1;
6200 +
6201 +       ret = i2c_transfer(c->adapter, &msg[0], 2);
6202 +       *value = ret_val;
6203 +
6204 +       ret = (ret == 2) ? 0 : -1;
6205 +       dprintk(2, "reg:0x%8x, value:0x%8x - %s", reg, *value,
6206 +               (ret ? "failed" : "succesfully"));
6207 +       return ret;
6208 +}
6209 +
6210 +static int s5k4e1_write(struct i2c_client *c, u32 reg, u32 value)
6211 +{
6212 +       int ret, i;
6213 +       struct i2c_msg msg;
6214 +       u8 msgbuf[3];
6215 +
6216 +       /* Writing only needs one message */
6217 +       memset(&msg, 0, sizeof(msg));
6218 +       i = 0;
6219 +       msgbuf[i++] = ((u16)reg) >> 8;
6220 +       msgbuf[i++] = (u16)reg & 0xff;
6221 +       msgbuf[i++] = (u8)value;
6222 +
6223 +       msg.addr = c->addr;
6224 +       msg.flags = 0;
6225 +       msg.buf = msgbuf;
6226 +       msg.len = i;
6227 +
6228 +       ret = i2c_transfer(c->adapter, &msg, 1);
6229 +
6230 +       /* If this is a reset register, wait for 1ms */
6231 +       if (reg == 0x0103 && (value & 0x01))
6232 +               /*Note here, check if this is needed */
6233 +               msleep(4);
6234 +
6235 +       ret = (ret == 1) ? 0 : -1;
6236 +       dprintk(2, "reg:0x%8x, value:0x%8x - %s", reg, value,
6237 +               (ret ? "failed" : "successfully"));
6238 +       return ret;
6239 +}
6240 +
6241 +static int s5k4e1_write_array(struct i2c_client *c, struct regval_list *vals)
6242 +{
6243 +       struct regval_list *p;
6244 +       u32 read_val = 0;
6245 +       int err_num = 0;
6246 +       int i = 0;
6247 +
6248 +       DBG_entering;
6249 +
6250 +       p = vals;
6251 +       while (p->reg_num != 0xffff) {
6252 +               s5k4e1_write(c, (u32)p->reg_num, (u32)p->value);
6253 +               s5k4e1_read(c, (u32)p->reg_num, &read_val);
6254 +               /* msleep(100);*/
6255 +               if (read_val != p->value) {
6256 +                       eprintk("0x%x write error:should be 0x%x, but 0x%x",
6257 +                               p->reg_num, p->value, read_val);
6258 +                       err_num++;
6259 +               }
6260 +               p++;
6261 +               i++;
6262 +       }
6263 +       dprintk(1, "sucessfully wrote %d registers, err is %d", i,
6264 +              err_num);
6265 +       return 0;
6266 +}
6267 +
6268 +/*
6269 + * Sensor specific helper function
6270 + */
6271 +static int s5k4e1_standby(void)
6272 +{
6273 +       gpio_set_value(GPIO_STDBY_PIN, 1);
6274 +       dprintk(1, "PM: standby called\n");
6275 +       return 0;
6276 +}
6277 +
6278 +static int s5k4e1_wakeup(void)
6279 +{
6280 +       gpio_set_value(GPIO_STDBY_PIN, 0);
6281 +       dprintk(1, "PM: wakeup called\n");
6282 +       return 0;
6283 +}
6284 +
6285 +static int s5k4e1_s_power(struct v4l2_subdev *sd, u32 val)
6286 +{
6287 +       if (val == 1)
6288 +               s5k4e1_standby();
6289 +       if (val == 0)
6290 +               s5k4e1_wakeup();
6291 +       return 0;
6292 +}
6293 +
6294 +static int s5k4e1_set_img_ctrl(struct i2c_client *c,
6295 +                              const struct ci_sensor_config *config)
6296 +{
6297 +       int err = 0;
6298 +
6299 +       DBG_entering;
6300 +
6301 +       switch (config->blc) {
6302 +               /* only SENSOR_BLC_AUTO supported */
6303 +       case SENSOR_BLC_AUTO:
6304 +               break;
6305 +       default:
6306 +               dprintk(1, "BLC not supported,\
6307 +                       set to BLC_AUTO by default.");
6308 +       }
6309 +
6310 +       switch (config->bls) {
6311 +               /* only SENSOR_BLS_OFF supported */
6312 +       case SENSOR_BLS_OFF:
6313 +               break;
6314 +       default:
6315 +               dprintk(1, "Black level not supported,\
6316 +                       set to BLS_OFF by default.");
6317 +       }
6318 +
6319 +       switch (config->agc) {
6320 +               /* only SENSOR_AGC_OFF supported */
6321 +       case SENSOR_AGC_OFF:
6322 +               break;
6323 +       default:
6324 +               dprintk(1, "AGC not supported,\
6325 +                       set to AGC_OFF by default.");
6326 +       }
6327 +
6328 +       switch (config->awb) {
6329 +               /* only SENSOR_AWB_OFF supported */
6330 +       case SENSOR_AWB_OFF:
6331 +               break;
6332 +       default:
6333 +               dprintk(1, "AWB not supported,\
6334 +                       set to AWB_OFF by default.");
6335 +       }
6336 +
6337 +       switch (config->aec) {
6338 +               /* only SENSOR_AEC_OFF supported */
6339 +       case SENSOR_AEC_OFF:
6340 +               break;
6341 +       default:
6342 +               dprintk(1, "AEC not supported,\
6343 +                       set to AEC_OFF by default.");
6344 +       }
6345 +
6346 +       DBG_leaving;
6347 +
6348 +       return err;
6349 +}
6350 +static int s5k4e1_init(struct i2c_client *c)
6351 +{
6352 +       int ret = 0;
6353 +       struct v4l2_subdev *sd = i2c_get_clientdata(c);
6354 +       struct ci_sensor_config *info = to_sensor_config(sd);
6355 +       char *name = "";
6356 +
6357 +       DBG_entering;
6358 +
6359 +       /* Fill the configuration structure */
6360 +       /* Note this default configuration value */
6361 +       info->mode = s5k4e1_formats[0].pixelformat;
6362 +       info->res = s5k4e1_res[0].res;
6363 +       info->type = SENSOR_TYPE_RAW;
6364 +       info->bls = SENSOR_BLS_OFF;
6365 +       info->gamma = SENSOR_GAMMA_OFF;
6366 +       info->cconv = SENSOR_CCONV_OFF;
6367 +       info->blc = SENSOR_BLC_AUTO;
6368 +       info->agc = SENSOR_AGC_OFF;
6369 +       info->awb = SENSOR_AWB_OFF;
6370 +       info->aec = SENSOR_AEC_OFF;
6371 +       /*info->bus_width = SENSOR_BUSWIDTH_10BIT_ZZ;*/
6372 +       info->bus_width = SENSOR_BUSWIDTH_12BIT;
6373 +       info->ycseq = SENSOR_YCSEQ_YCBYCR;
6374 +       info->conv422 = SENSOR_CONV422_COSITED;
6375 +       /*info->conv422 = SENSOR_CONV422_NOCOSITED;*/
6376 +       info->bpat = SENSOR_BPAT_GRGRBGBG;
6377 +       info->field_inv = SENSOR_FIELDINV_NOSWAP;
6378 +       info->field_sel = SENSOR_FIELDSEL_BOTH;
6379 +       info->hpol = SENSOR_HPOL_REFPOS;
6380 +       info->vpol = SENSOR_VPOL_NEG;
6381 +       info->edge = SENSOR_EDGE_RISING;
6382 +       info->flicker_freq = SENSOR_FLICKER_100;
6383 +       info->cie_profile = SENSOR_CIEPROF_F11;
6384 +       info->mipi_mode = SENSOR_MIPI_MODE_RAW_10;
6385 +       name = "s5k4e1";
6386 +       memcpy(info->name, name, 7);
6387 +
6388 +       /* Reset sensor hardware, and implement the setting*/
6389 +       ret += s5k4e1_write(c, 0x0100, (u32)0x00);
6390 +       /*TODO: See if we can ignore this*/
6391 +       ret = s5k4e1_write(c, 0x0103, (u32)0x01);
6392 +
6393 +       /* sw reset -- delay 3.1ms */
6394 +       msleep(4);
6395 +
6396 +       /* Set registers into default config value */
6397 +       /* ret += s5k4e1_write_array(c, s5k4e1_def_reg); */
6398 +
6399 +       /* Set MIPI interface */
6400 +#ifdef S5K4E1_MIPI
6401 +       ret += s5k4e1_write_array(c, s5k4e1_mipi);
6402 +#endif
6403 +
6404 +       ret += s5k4e1_set_img_ctrl(c, info); /*FIXME*/
6405 +
6406 +       /* streaming */
6407 +       /* ret += s5k4e1_write(c, 0x0100, (u32)0x01); */
6408 +       ret += s5k4e1_write(c, 0x0100, (u32)0x00);
6409 +
6410 +       msleep(1);
6411 +
6412 +       DBG_leaving;
6413 +
6414 +       return ret;
6415 +}
6416 +
6417 +static int distance(struct s5k4e1_res_struct *res, u32 w, u32 h)
6418 +{
6419 +       int ret;
6420 +
6421 +       DBG_entering;
6422 +
6423 +       if (res->width < w || res->height < h)
6424 +               return -1;
6425 +
6426 +       ret = ((res->width - w) + (res->height - h));
6427 +
6428 +       DBG_leaving;
6429 +
6430 +       return ret;
6431 +}
6432 +
6433 +static int s5k4e1_try_res(u32 *w, u32 *h)
6434 +{
6435 +       struct s5k4e1_res_struct *res_index, *p = NULL;
6436 +       int dis, last_dis = s5k4e1_res->width + s5k4e1_res->height;
6437 +
6438 +       DBG_entering;
6439 +
6440 +       for (res_index = s5k4e1_res;
6441 +            res_index < s5k4e1_res + N_RES;
6442 +            res_index++) {
6443 +               if ((res_index->width < *w) || (res_index->height < *h))
6444 +                       break;
6445 +               dis = distance(res_index, *w, *h);
6446 +               if (dis < last_dis) {
6447 +                       last_dis = dis;
6448 +                       p = res_index;
6449 +               }
6450 +       }
6451 +
6452 +       if (p == NULL)
6453 +               p = s5k4e1_res;
6454 +       else if ((p->width < *w) || (p->height < *h)) {
6455 +               if (p != s5k4e1_res)
6456 +                       p--;
6457 +       }
6458 +
6459 +       if ((w != NULL) && (h != NULL)) {
6460 +               *w = p->width;
6461 +               *h = p->height;
6462 +       }
6463 +
6464 +       DBG_leaving;
6465 +       return 0;
6466 +}
6467 +
6468 +static struct s5k4e1_res_struct *s5k4e1_to_res(u32 w, u32 h)
6469 +{
6470 +       struct s5k4e1_res_struct *res_index;
6471 +
6472 +       DBG_entering;
6473 +
6474 +       for (res_index = s5k4e1_res;
6475 +            res_index < s5k4e1_res + N_RES;
6476 +            res_index++)
6477 +               if ((res_index->width == w) && (res_index->height == h))
6478 +                       break;
6479 +
6480 +       if (res_index >= s5k4e1_res + N_RES)
6481 +               res_index--;   /* Take the bigger one */
6482 +
6483 +       DBG_leaving;
6484 +
6485 +       return res_index;
6486 +}
6487 +
6488 +static int s5k4e1_try_fmt(struct v4l2_subdev *sd,
6489 +                         struct v4l2_format *fmt)
6490 +{
6491 +       DBG_entering;
6492 +       return s5k4e1_try_res(&fmt->fmt.pix.width, &fmt->fmt.pix.height);
6493 +       DBG_leaving;
6494 +}
6495 +
6496 +static int s5k4e1_get_fmt(struct v4l2_subdev *sd,
6497 +                         struct v4l2_format *fmt)
6498 +{
6499 +       struct ci_sensor_config *info = to_sensor_config(sd);
6500 +       unsigned short width, height;
6501 +       int index;
6502 +
6503 +       ci_sensor_res2size(info->res, &width, &height);
6504 +
6505 +       /* Marked the current sensor res as being "used" */
6506 +       for (index = 0; index < N_RES; index++) {
6507 +               if ((width == s5k4e1_res[index].width) &&
6508 +                   (height == s5k4e1_res[index].height)) {
6509 +                       s5k4e1_res[index].used = 1;
6510 +                       continue;
6511 +               }
6512 +               s5k4e1_res[index].used = 0;
6513 +       }
6514 +
6515 +       fmt->fmt.pix.width = width;
6516 +       fmt->fmt.pix.height = height;
6517 +       return 0;
6518 +
6519 +}
6520 +
6521 +#if 0
6522 +/* chuanxiao add, to dump regs */
6523 +static int s5k4e1_dump_regs(struct i2c_client *c)
6524 +{
6525 +       /*struct i2c_client *c = v4l2_get_subdevdata(sd);*/
6526 +       const struct DumpRegs *p = regs_d;
6527 +       u32 value;
6528 +       u32 value1, value2, value3, value4;
6529 +       while (p->ulFlags != eTableEnd) {
6530 +               if (p->ulFlags & eFourBytes) {
6531 +                       s5k4e1_read(c, (u32)p->ulAddr, &value1);
6532 +                       s5k4e1_read(c, (u32)p->ulAddr+1, &value2);
6533 +                       s5k4e1_read(c, (u32)p->ulAddr+2, &value3);
6534 +                       s5k4e1_read(c, (u32)p->ulAddr+3, &value4);
6535 +                       value = value1<<24 | value2<<16 | value3<<8 | value4;
6536 +               } else if (p->ulFlags & eTwoBytes) {
6537 +                       s5k4e1_read(c, (u32)p->ulAddr, &value1);
6538 +                       s5k4e1_read(c, (u32)p->ulAddr+1, &value2);
6539 +                       value = value1<<8 | value2;
6540 +               } else
6541 +                       s5k4e1_read(c, (u32)p->ulAddr, &value);
6542 +               /*
6543 +               if (value == p->ulDefaultValue)
6544 +                       dprintk(0, "%s\t @ 0x%x = 0x%lx (= default value)\n",
6545 +                       p->pszName, p->ulAddr, value);
6546 +               else
6547 +                       dprintk(0, "%s\t @ 0x%x = 0x%lx (default was 0x%lx)\n",
6548 +                       p->pszName, p->ulAddr, value, p->ulDefaultValue);
6549 +               */
6550 +               dprintk(0, "%-30s @ 0x%04X = 0x%08X", p->pszName,
6551 +                       p->ulAddr, value);
6552 +               p++;
6553 +       }
6554 +       return 0;
6555 +}
6556 +#endif
6557 +
6558 +static int s5k4e1_set_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
6559 +{
6560 +       struct i2c_client *c = v4l2_get_subdevdata(sd);
6561 +       struct ci_sensor_config *info = to_sensor_config(sd);
6562 +       int ret = 0;
6563 +       struct s5k4e1_res_struct *res_index;
6564 +       u32 width, height;
6565 +       int index;
6566 +
6567 +       DBG_entering;
6568 +
6569 +       width = fmt->fmt.pix.width;
6570 +       height = fmt->fmt.pix.height;
6571 +
6572 +       dprintk(1, "was told to set fmt (%d x %d) ", width, height);
6573 +       ret = s5k4e1_try_res(&width, &height);
6574 +
6575 +       res_index = s5k4e1_to_res(width, height);
6576 +
6577 +       s5k4e1_wakeup();
6578 +       DBG_line;
6579 +       if (res_index->regs) {
6580 +               /* software sleep/standby */
6581 +               ret += s5k4e1_write(c, 0x0100, (u32)0x00);
6582 +
6583 +               /* Soft reset camera first*/
6584 +               /*TODO: See if we can ignore this*/
6585 +               ret = s5k4e1_write(c, 0x0103, (u32)0xff);
6586 +
6587 +               /* Set registers into default config value */
6588 +               /* ret += s5k4e1_write_array(c, s5k4e1_def_reg);*/
6589 +
6590 +               /* set image resolution */
6591 +               ret += s5k4e1_write_array(c, res_index->regs);
6592 +
6593 +               ret += s5k4e1_set_img_ctrl(c, info);
6594 +
6595 +               /* XXX setup with unknow meaning ... */
6596 +               /* ret += s5k4e1_write(c, 0x30b0, 0xfe); */
6597 +
6598 +               /* Set MIPI interface */
6599 +#ifdef S5K4E1_MIPI
6600 +               ret += s5k4e1_write_array(c, s5k4e1_mipi);
6601 +#endif
6602 +
6603 +               /* streaming */
6604 +               ret = s5k4e1_write(c, 0x0100, (u32)0x01);
6605 +               msleep(1);
6606 +
6607 +               info->res = res_index->res;
6608 +
6609 +               /* Marked current sensor res as being "used" */
6610 +               for (index = 0; index < N_RES; index++) {
6611 +                       if ((width == s5k4e1_res[index].width) &&
6612 +                           (height == s5k4e1_res[index].height)) {
6613 +                               s5k4e1_res[index].used = 1;
6614 +                               continue;
6615 +                       }
6616 +                       s5k4e1_res[index].used = 0;
6617 +               }
6618 +
6619 +               for (index = 0; index < N_RES; index++)
6620 +                       dprintk(2, "index = %d, used = %d\n", index,
6621 +                               s5k4e1_res[index].used);
6622 +
6623 +               DBG_line;
6624 +       } else {
6625 +               eprintk("no res for (%d x %d)", width, height);
6626 +       }
6627 +
6628 +       DBG_leaving;
6629 +       return ret;
6630 +}
6631 +
6632 +static int s5k4e1_t_gain(struct v4l2_subdev *sd, int value)
6633 +{
6634 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
6635 +
6636 +       DBG_entering;
6637 +
6638 +       s5k4e1_write(client, 0x0104, 1); /*hold*/
6639 +
6640 +       /* analog gain */
6641 +       s5k4e1_write(client, 0x0204, value >> 8);
6642 +
6643 +       s5k4e1_write(client, 0x0205, value & 0xff);
6644 +
6645 +       s5k4e1_write(client, 0x0104, 0); /*unhold*/
6646 +
6647 +       dprintk(1, "gain %x was writen to 0x0204/5", value);
6648 +
6649 +       DBG_leaving;
6650 +       return 0;
6651 +}
6652 +
6653 +static int s5k4e1_t_exposure(struct v4l2_subdev *sd, int value)
6654 +{
6655 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
6656 +
6657 +       DBG_entering;
6658 +
6659 +       s5k4e1_write(client, 0x0104, 1); /*hold*/
6660 +
6661 +       /* fine integration time */
6662 +       s5k4e1_write(client, 0x0200, value >> 24);
6663 +
6664 +       s5k4e1_write(client, 0x0201, (value >> 16) & 0xff);
6665 +
6666 +       /* coarse integration time */
6667 +       s5k4e1_write(client, 0x0202, (value & 0xff00) >> 8);
6668 +
6669 +       s5k4e1_write(client, 0x0203, value & 0xff);
6670 +
6671 +       s5k4e1_write(client, 0x0104, 0); /*unhold*/
6672 +
6673 +       dprintk(1, "exposure %x was writen to 0x0200/1/2/3", value);
6674 +
6675 +       DBG_leaving;
6676 +       return 0;
6677 +}
6678 +
6679 +static struct s5k4e1_control {
6680 +       struct v4l2_queryctrl qc;
6681 +       int (*query)(struct v4l2_subdev *sd, __s32 *value);
6682 +       int (*tweak)(struct v4l2_subdev *sd, int value);
6683 +} s5k4e1_controls[] = {
6684 +       {
6685 +               .qc = {
6686 +                       .id = V4L2_CID_GAIN,
6687 +                       .type = V4L2_CTRL_TYPE_INTEGER,
6688 +                       .name = "global gain",
6689 +                       .minimum = 0x0,
6690 +                       .maximum = 0xFFFF,
6691 +                       .step = 0x01,
6692 +                       .default_value = 0x00,
6693 +                       .flags = 0,
6694 +               },
6695 +               .tweak = s5k4e1_t_gain,
6696 +       },
6697 +       {
6698 +               .qc = {
6699 +                       .id = V4L2_CID_EXPOSURE,
6700 +                       .type = V4L2_CTRL_TYPE_INTEGER,
6701 +                       .name = "exposure",
6702 +                       .minimum = 0x0,
6703 +                       .maximum = 0xFFFF,
6704 +                       .step = 0x01,
6705 +                       .default_value = 0x00,
6706 +                       .flags = 0,
6707 +               },
6708 +               .tweak = s5k4e1_t_exposure,
6709 +       },
6710 +};
6711 +#define N_CONTROLS (ARRAY_SIZE(s5k4e1_controls))
6712 +
6713 +static struct s5k4e1_control *s5k4e1_find_control(__u32 id)
6714 +{
6715 +       int i;
6716 +
6717 +       DBG_entering;
6718 +       for (i = 0; i < N_CONTROLS; i++)
6719 +               if (s5k4e1_controls[i].qc.id == id)
6720 +                       return s5k4e1_controls + i;
6721 +       DBG_leaving;
6722 +       return NULL;
6723 +}
6724 +
6725 +static int s5k4e1_queryctrl(struct v4l2_subdev *sd,
6726 +                           struct v4l2_queryctrl *qc)
6727 +{
6728 +       struct s5k4e1_control *ctrl = s5k4e1_find_control(qc->id);
6729 +
6730 +       DBG_entering;
6731 +       if (ctrl == NULL)
6732 +               return -EINVAL;
6733 +       *qc = ctrl->qc;
6734 +
6735 +       DBG_leaving;
6736 +       return 0;
6737 +}
6738 +
6739 +static int s5k4e1_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
6740 +{
6741 +/*
6742 +       struct s5k4e1_control *octrl = s5k4e1_find_control(parm->index);
6743 +       int ret;
6744 +
6745 +       if (octrl == NULL)
6746 +               return -EINVAL;
6747 +       ret = octrl->query(client, &parm->value);
6748 +       if (ret >= 0)
6749 +               return 0;
6750 +*/
6751 +       return 0;
6752 +}
6753 +
6754 +static int s5k4e1_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
6755 +{
6756 +       struct s5k4e1_control *octrl = s5k4e1_find_control(ctrl->id);
6757 +       int ret;
6758 +
6759 +       DBG_entering;
6760 +
6761 +       if (octrl == NULL)
6762 +               return -EINVAL;
6763 +       ret =  octrl->tweak(sd, ctrl->value);
6764 +       if (ret >= 0)
6765 +               return 0;
6766 +
6767 +       DBG_leaving;
6768 +       return ret;
6769 +}
6770 +
6771 +static int s5k4e1_s_stream(struct v4l2_subdev *sd, int enable)
6772 +{
6773 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
6774 +       DBG_entering;
6775 +
6776 +       if (enable) {
6777 +               s5k4e1_write(client, (u32)0x0100, 0x01);
6778 +               /*chuanxiao add, dump s5k4e1 regs*/
6779 +               /* s5k4e1_dump_regs(client); */
6780 +       } else
6781 +               s5k4e1_write(client, (u32)0x0100, 0x00);
6782 +
6783 +       /*msleep(1);*/
6784 +
6785 +       DBG_leaving;
6786 +       return 0;
6787 +}
6788 +
6789 +static int s5k4e1_enum_framesizes(struct v4l2_subdev *sd,
6790 +                                 struct v4l2_frmsizeenum *fsize)
6791 +{
6792 +       unsigned int index = fsize->index;
6793 +
6794 +       DBG_entering;
6795 +
6796 +       if (index >= N_RES)
6797 +               return -EINVAL;
6798 +
6799 +       fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
6800 +       fsize->discrete.width = s5k4e1_res[index].width;
6801 +       fsize->discrete.height = s5k4e1_res[index].height;
6802 +       fsize->reserved[0] = s5k4e1_res[index].used;
6803 +
6804 +       DBG_leaving;
6805 +
6806 +       return 0;
6807 +}
6808 +
6809 +static int s5k4e1_enum_frameintervals(struct v4l2_subdev *sd,
6810 +                                     struct v4l2_frmivalenum *fival)
6811 +{
6812 +       unsigned int index = fival->index;
6813 +
6814 +       DBG_entering;
6815 +
6816 +       if (index >= N_RES)
6817 +               return -EINVAL;
6818 +
6819 +       fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
6820 +       fival->discrete.numerator = 1;
6821 +       fival->discrete.denominator = s5k4e1_res[index].fps;
6822 +
6823 +       DBG_leaving;
6824 +
6825 +       return 0;
6826 +}
6827 +
6828 +static int s5k4e1_g_chip_ident(struct v4l2_subdev *sd,
6829 +               struct v4l2_dbg_chip_ident *chip)
6830 +{
6831 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
6832 +
6833 +       DBG_entering;
6834 +
6835 +#define V4L2_IDENT_S5K4E1 8250
6836 +       DBG_leaving;
6837 +
6838 +       return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_S5K4E1, 0);
6839 +}
6840 +
6841 +#ifdef CONFIG_VIDEO_ADV_DEBUG
6842 +static int s5k4e1_g_register(struct v4l2_subdev *sd,
6843 +                            struct v4l2_dbg_register *reg)
6844 +{
6845 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
6846 +       unsigned char val = 0;
6847 +       int ret;
6848 +
6849 +       if (!v4l2_chip_match_i2c_client(client, &reg->match))
6850 +               return -EINVAL;
6851 +       if (!capable(CAP_SYS_ADMIN))
6852 +               return -EPERM;
6853 +       ret = s5k4e1_read(client, reg->reg & 0xffff, &val);
6854 +       reg->val = val;
6855 +       reg->size = 1;
6856 +       return ret;
6857 +}
6858 +
6859 +static int s5k4e1_s_register(struct v4l2_subdev *sd,
6860 +                            struct v4l2_dbg_register *reg)
6861 +{
6862 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
6863 +
6864 +       if (!v4l2_chip_match_i2c_client(client, &reg->match))
6865 +               return -EINVAL;
6866 +       if (!capable(CAP_SYS_ADMIN))
6867 +               return -EPERM;
6868 +       s5k4e1_write(client, reg->reg & 0xffff, reg->val & 0xff);
6869 +       return 0;
6870 +}
6871 +#endif
6872 +
6873 +static const struct v4l2_subdev_video_ops s5k4e1_video_ops = {
6874 +       .try_fmt = s5k4e1_try_fmt,
6875 +       .s_fmt = s5k4e1_set_fmt,
6876 +       .g_fmt = s5k4e1_get_fmt,
6877 +       .s_stream = s5k4e1_s_stream,
6878 +       .enum_framesizes = s5k4e1_enum_framesizes,
6879 +       .enum_frameintervals = s5k4e1_enum_frameintervals,
6880 +};
6881 +
6882 +static const struct v4l2_subdev_core_ops s5k4e1_core_ops = {
6883 +       .g_chip_ident = s5k4e1_g_chip_ident,
6884 +       .queryctrl = s5k4e1_queryctrl,
6885 +       .g_ctrl = s5k4e1_g_ctrl,
6886 +       .s_ctrl = s5k4e1_s_ctrl,
6887 +       .s_gpio = s5k4e1_s_power,
6888 +       /*.g_ext_ctrls = s5k4e1_g_ext_ctrls,*/
6889 +       /*.s_ext_ctrls = s5k4e1_s_ext_ctrls,*/
6890 +#ifdef CONFIG_VIDEO_ADV_DEBUG
6891 +       .g_register = s5k4e1_g_register,
6892 +       .s_register = s5k4e1_s_register,
6893 +#endif
6894 +};
6895 +
6896 +static const struct v4l2_subdev_ops s5k4e1_ops = {
6897 +       .core = &s5k4e1_core_ops,
6898 +       .video = &s5k4e1_video_ops,
6899 +};
6900 +
6901 +/*
6902 + * Basic i2c stuff
6903 + */
6904 +/*
6905 +static unsigned short normal_i2c[] = {0x36, I2C_CLIENT_END};
6906 +I2C_CLIENT_INSMOD;
6907 +
6908 +static struct i2c_driver i2c_driver_s5k4e1_sensor;
6909 +*/
6910 +static int s5k4e1_detect(struct i2c_client *client)
6911 +{
6912 +       struct i2c_adapter *adapter = client->adapter;
6913 +       int adap_id = i2c_adapter_id(adapter);
6914 +       u32 value;
6915 +
6916 +       DBG_entering;
6917 +
6918 +       if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
6919 +               eprintk("error i2c check func");
6920 +               return -ENODEV;
6921 +       }
6922 +
6923 +       if (adap_id != 1) {
6924 +               eprintk("adap_id != 1");
6925 +               return -ENODEV;
6926 +       }
6927 +
6928 +       if (s5k4e1_wakeup()) {
6929 +               eprintk("sensor wakeup failed");
6930 +               return -EIO;
6931 +       }
6932 +
6933 +       s5k4e1_read(client, 0x0003, &value);
6934 +       dprintk(1, "Read from 0x0003: %x", value);
6935 +       if ((value != 0x09))
6936 +               return -ENODEV;
6937 +
6938 +       s5k4e1_read(client, 0x0000, &value);
6939 +       dprintk(1, "Read from 0x0000: %x", value);
6940 +       if ((value != 0x4e) && (value != 0x10))
6941 +               return -ENODEV;
6942 +
6943 +       s5k4e1_read(client, 0x0001, &value);
6944 +       dprintk(1, "Read from 0x0001: %x", value);
6945 +       if ((value != 0x4e) && (value != 0x10))
6946 +               return -ENODEV;
6947 +
6948 +       /*TODO EVT3 detect*/
6949 +       s5k4e1_read(client, 0x0002, &value);
6950 +       dprintk(1, "Read from 0x0002: %x", value);
6951 +       if (value == 0x0010) {
6952 +               dprintk(1, "EVT3 module not supported!");
6953 +               return -ENODEV;
6954 +       }
6955 +
6956 +       DBG_leaving;
6957 +       return 0;
6958 +}
6959 +
6960 +static int s5k4e1_probe(struct i2c_client *client,
6961 +                       const struct i2c_device_id *id)
6962 +{
6963 +       struct ci_sensor_config *info;
6964 +       struct v4l2_subdev *sd;
6965 +       int ret = -1;
6966 +
6967 +       DBG_entering;
6968 +
6969 +       v4l_info(client, "chip found @ 0x%x (%s)\n",
6970 +                client->addr << 1, client->adapter->name);
6971 +
6972 +       /*
6973 +        * Setup sensor configuration structure
6974 +        */
6975 +       info = kzalloc(sizeof(struct ci_sensor_config), GFP_KERNEL);
6976 +       if (!info) {
6977 +               dprintk(0, "fail to malloc for ci_sensor_config");
6978 +               ret = -ENOMEM;
6979 +               goto out;
6980 +       }
6981 +
6982 +       ret = s5k4e1_detect(client);
6983 +       if (ret) {
6984 +               dprintk(0, "error s5k4e1_detect");
6985 +               goto out_free;
6986 +       }
6987 +
6988 +       sd = &info->sd;
6989 +       v4l2_i2c_subdev_init(sd, client, &s5k4e1_ops);
6990 +
6991 +       /*
6992 +        * Initialization S5K4E1
6993 +        * then turn into standby mode
6994 +        */
6995 +       ret = s5k4e1_init(client);
6996 +       if (ret) {
6997 +               dprintk(0, "error calling s5k4e1_init");
6998 +               goto out_free;
6999 +       }
7000 +
7001 +       s5k4e1_standby();
7002 +       dprintk(0, "Init s5k4e1 sensor successfully");
7003 +
7004 +       ret = 0;
7005 +       goto out;
7006 +
7007 +out_free:
7008 +       kfree(info);
7009 +       DBG_leaving;
7010 +out:
7011 +
7012 +       DBG_leaving;
7013 +       return ret;
7014 +}
7015 +
7016 +
7017 +static int s5k4e1_remove(struct i2c_client *client)
7018 +{
7019 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
7020 +
7021 +       DBG_entering;
7022 +
7023 +       v4l2_device_unregister_subdev(sd);
7024 +       kfree(to_sensor_config(sd));
7025 +
7026 +       DBG_leaving;
7027 +       return 0;
7028 +}
7029 +
7030 +/**
7031 + * i2c_driver for s5k4e1_sensor
7032 + */
7033 +static const struct i2c_device_id s5k4e1_id[] = {
7034 +       {"s5k4e1", 0},
7035 +       {}
7036 +};
7037 +
7038 +MODULE_DEVICE_TABLE(i2c, s5k4e1_id);
7039 +
7040 +static struct v4l2_i2c_driver_data v4l2_i2c_data = {
7041 +       .name = "s5k4e1",
7042 +       .probe = s5k4e1_probe,
7043 +       .remove = s5k4e1_remove,
7044 +       /* .suspend = s5k4e1_suspend,
7045 +        * .resume = s5k4e1_resume, */
7046 +       .id_table = s5k4e1_id,
7047 +};
7048 +
7049 +MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
7050 +MODULE_DESCRIPTION("A low-level driver for Samsung S5K4E1 sensors");
7051 +MODULE_LICENSE("GPL");
7052 diff --git a/drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.h b/drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.h
7053 new file mode 100755
7054 index 0000000..d722035
7055 --- /dev/null
7056 +++ b/drivers/media/video/mrstci/mrsts5k4e1/mrsts5k4e1.h
7057 @@ -0,0 +1,662 @@
7058 +/*
7059 + * Support for Moorestown Langwell Camera Imaging ISP subsystem.
7060 + *
7061 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
7062 + *
7063 + * This program is free software; you can redistribute it and/or
7064 + * modify it under the terms of the GNU General Public License version
7065 + * 2 as published by the Free Software Foundation.
7066 + *
7067 + * This program is distributed in the hope that it will be useful,
7068 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
7069 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
7070 + * GNU General Public License for more details.
7071 + *
7072 + * You should have received a copy of the GNU General Public License
7073 + * along with this program; if not, write to the Free Software
7074 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
7075 + * 02110-1301, USA.
7076 + *
7077 + *
7078 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
7079 + */
7080 +
7081 +#define I2C_S5K4E1     0x6C
7082 +/* Should add to kernel source */
7083 +#define I2C_DRIVERID_S5K4E1    1046
7084 +/* GPIO pin on Moorestown */
7085 +#define GPIO_SCLK_25   44
7086 +#define GPIO_STB_PIN   47
7087 +#define GPIO_STDBY_PIN 49
7088 +#define GPIO_RESET_PIN 50
7089 +
7090 +struct regval_list {
7091 +       u16 reg_num;
7092 +       u8 value;
7093 +};
7094 +
7095 +/*
7096 + * Default register value
7097 + * 5Mega Pixel, 2592x1944
7098 + */
7099 +/* MIPI register are removed by Wen */
7100 +
7101 +/* 2592x1944 */
7102 +static struct regval_list s5k4e1_res_qsxga_plus4[] = {
7103 +       /* Reset for operation */
7104 +       {0x0100, 0x00},         /* stream off */
7105 +       {0x0103, 0x01},         /* software reset */
7106 +
7107 +/*
7108 + * Analog Setting
7109 + * This register is for FACTORY ONLY.
7110 + * If you change it without prior notification,
7111 + * You are RESPONSIBLE for the FAILURE that will happen in the future.
7112 + */
7113 +
7114 +/* CDS timing setting ... */
7115 +       {0x3000, 0x04}, /* ct_ld_start (default = 07h) */
7116 +       {0x3001, 0x02}, /* ct_sl_start (default = 05h) */
7117 +       {0x3002, 0x0C}, /* ct_rx_start (default = 21h) */
7118 +       {0x3003, 0x0E}, /* ct_cds_start (default = 23h) */
7119 +       {0x3004, 0x2C}, /* ct_smp_width (default = 60h) */
7120 +       {0x3005, 0x0D}, /* ct_az_width (default = 28h) */
7121 +       {0x3006, 0x39}, /* ct_s1r_width (default = 88h) */
7122 +       {0x3007, 0x02}, /* ct_tx_start (default = 06h) */
7123 +       {0x3008, 0x3C}, /* ct_tx_width 1.5us (default = 7Ch) */
7124 +       {0x3009, 0x3C}, /* ct_stx_width 1.5us (default = 7Ch) */
7125 +       {0x300A, 0x28}, /* ct_dtx_width 1us (default = 3Eh) */
7126 +       {0x300B, 0x15}, /* ct_rmp_rst_start (default = 44h) */
7127 +       {0x300C, 0x15}, /* ct_rmp_sig_start (default = 48h) */
7128 +       {0x300D, 0x02}, /* ct_rmp_lat (default = 02h) */
7129 +       {0x300E, 0xA9}, /* D-Shut en[7], CLP On[5], LD high[4] */
7130 +
7131 +/* CDS option setting ... */
7132 +       {0x3010, 0x00}, /* smp_en[2]=0(00) 1(04) row_id[1:0] = 00 */
7133 +       {0x3011, 0x7A}, /* RST_MX (288), SIG_MX (1024+352) */
7134 +       {0x3012, 0x30}, /* SIG offset1  48 code  */
7135 +       {0x3013, 0xA0}, /* RST offset1  160 code */
7136 +       {0x3014, 0x00}, /* SIG offset2 */
7137 +       {0x3015, 0x00}, /* RST offset2 */
7138 +       {0x3016, 0x02}, /* ADC_SAT (510mV) */
7139 +       {0x3017, 0x94}, /* RMP_INIT[3:0](RMP_REG) 1.8V MS[6:4]=1 */
7140 +       {0x3018, 0x78}, /* rmp option - ramp connect[MSB] +RMP INIT DAC MIN */
7141 +       {0x301D, 0xD4}, /* CLP level (default = 0Fh) */
7142 +
7143 +       {0x3021, 0x02}, /* inrush ctrl[1] off */
7144 +       {0x3022, 0x44}, /* pump ring oscillator set [7:4]=CP, [3:0]=NCP  */
7145 +       {0x3024, 0x40}, /* pix voltage 2.8V   (default = 88h) */
7146 +       {0x3027, 0x08}, /* ntg voltage (default = 04h) */
7147 +
7148 +/* Pixel option setting ... */
7149 +       {0x301C, 0x05}, /* Pixel Bias [3:0] (default = 03h) */
7150 +       {0x30D8, 0x3F}, /* All tx off 2f, on 3f */
7151 +
7152 +/* ADLC setting ... */
7153 +       {0x3070, 0x5F}, /* [6]L-ADLC BPR, [4]ch sel, [3]L-ADLC, [2]F-ADLC */
7154 +       {0x3071, 0x00}, /* F&L-adlc max 127 (default = 11h, max 255) */
7155 +       {0x3080, 0x04}, /* F-ADLC filter A (default = 10h) */
7156 +       {0x3081, 0x38}, /* F-ADLC filter B (default = 20h) */
7157 +
7158 +/* Integration setting ... */
7159 +       {0x0202, 0x03}, /* coarse integration time */
7160 +       {0x0203, 0xCF},
7161 +       {0x0204, 0x00}, /* analog gain[msb] 0100 x8 0080 x4 */
7162 +       {0x0205, 0x80}, /* analog gain[lsb] 0040 x2 0020 x1 */
7163 +
7164 +/* Frame Length */
7165 +       {0x0340, 0x07}, /* Capture 07B4(1960[# of row]+12[V-blank]) */
7166 +       {0x0341, 0xA4}, /* Preview 03E0(980[# of row]+12[V-blank]) */
7167 +
7168 +/* Line Length */
7169 +       {0x0342, 0x0A}, /* 2738 */
7170 +       {0x0343, 0xB2}, /* (Same as sensor default) */
7171 +
7172 +/* embedded 2-line OFF setting ... */
7173 +/* 2608 x 1960  */
7174 +       {0x3084, 0x15}, /* SYNC Mode */
7175 +
7176 +/* (3) MIPI 2-lane Serial(TST = 0000b or TST = 0010b), 30 fps */
7177 +
7178 +       {0x30A9, 0x01},
7179 +       {0x0387, 0x01},
7180 +
7181 +       {0x30BD, 0x00},         /* SEL_CCP[0] */
7182 +       {0x30B2, 0x08},         /* PLL P = 8 */
7183 +       {0x30B3, 0x00},         /* PLL M[8] = 0 */
7184 +       {0x30B5, 0x01},         /* PLL S = 0 */
7185 +       {0x30BE, 0x1A},         /* M_PCLKDIV_AUTO[4], M_DIV_PCLK[3:0] */
7186 +
7187 +       {0x30BF, 0xAB},
7188 +       {0x30C0, 0x00},         /* video_offset[7:4] 3240%12 */
7189 +       {0x30C1, 0x01},         /* pack video enable [0] */
7190 +       {0x30C8, 0x0C},         /* video_data_length 3260 = 2608 * 1.25 */
7191 +       {0x30C9, 0xA8},
7192 +       {0x30E2, 0x02},         /* num lanes[1:0] = 2 */
7193 +       {0x30EE, 0x02},         /* DPHY enable [1] */
7194 +       {0x30F1, 0x70},         /* DPHY BANDCTRL 800MHz=80.6MHz */
7195 +       {0x3111, 0x86},         /* Embedded data off [5] */
7196 +
7197 +       {0x034C, 0x0A},
7198 +       {0x034D, 0x20},
7199 +       {0x044E, 0x07},
7200 +       {0x034F, 0x98},
7201 +
7202 +       {0x0344, 0x00},
7203 +       {0x0345, 0x08},
7204 +       {0x0346, 0x00},
7205 +       {0x0347, 0x08},
7206 +       {0x0348, 0x0A},
7207 +       {0x0349, 0x27},
7208 +       {0x034A, 0x07},
7209 +       {0x034B, 0x9F},
7210 +
7211 +       /* This is to set FRAME_NUM > 0 */
7212 +       {0x30d9, 0x00},
7213 +
7214 +       /* Add this setting according to Bill's test */
7215 +       {0x0305, 0x05},
7216 +       {0x0306, 0x00},
7217 +       {0x0307, 0x3c},
7218 +       {0x30b5, 0x02},
7219 +
7220 +       {0x020E, 0x01},      /* Gr Digital Gain */
7221 +       {0x020F, 0x00},
7222 +       {0x0210, 0x01},     /* Red Digital Gain */
7223 +       {0x0211, 0x00},
7224 +       {0x0212, 0x01},     /* Blue Digital Gain */
7225 +       {0x0213, 0x00},
7226 +       {0x0214, 0x01},    /* Gb Digital Gain */
7227 +       {0x0215, 0x00},
7228 +       {0x0204, 0x00},
7229 +       {0x0205, 0x80},
7230 +
7231 +#if 1
7232 +       /*Apply Bill's setting*/
7233 +       {0x30E2, 0x02},
7234 +       {0x0305, 0x05},
7235 +       {0x0306, 0x00},
7236 +       {0x0307, 0x50},         /* vcc_out = 80 */
7237 +       {0x30B5, 0x01},         /* pll_s = 1 */
7238 +       {0x30B4, 0x50},
7239 +
7240 +       {0x30B2, 0x05},
7241 +
7242 +       {0x30BE, 0x1A},         /* DIV_M_PCLK = 5 */
7243 +
7244 +       {0x0100, 0x01},         /* stream on */
7245 +       {0xffff, 0xff},
7246 +#endif
7247 +};
7248 +
7249 +/* 1920x1080 */
7250 +static struct regval_list s5k4e1_res_1080p[] = {
7251 +/* Reset for operation ... */
7252 +       {0x0100, 0x00},         /* stream off */
7253 +       {0x0103, 0x01},         /* software reset */
7254 +
7255 +/*
7256 + * Analog Setting
7257 + * This register is for FACTORY ONLY.
7258 + * If you change it without prior notification,
7259 + * You are RESPONSIBLE for the FAILURE that will happen in the future.
7260 + */
7261 +
7262 +/* CDS timing setting ... */
7263 +       {0x3000, 0x04}, /* ct_ld_start (default = 07h) */
7264 +       {0x3001, 0x02}, /* ct_sl_start (default = 05h) */
7265 +       {0x3002, 0x0C}, /* ct_rx_start (default = 21h) */
7266 +       {0x3003, 0x0E}, /* ct_cds_start (default = 23h) */
7267 +       {0x3004, 0x2C}, /* ct_smp_width (default = 60h) */
7268 +       {0x3005, 0x0D}, /* ct_az_width (default = 28h) */
7269 +       {0x3006, 0x39}, /* ct_s1r_width (default = 88h) */
7270 +       {0x3007, 0x02}, /* ct_tx_start (default = 06h) */
7271 +       {0x3008, 0x3C}, /* ct_tx_width 1.5us (default = 7Ch) */
7272 +       {0x300A, 0x28}, /* ct_dtx_width 1us (default = 3Eh) */
7273 +       {0x300B, 0x15}, /* ct_rmp_rst_start (default = 44h) */
7274 +       {0x300C, 0x15}, /* ct_rmp_sig_start (default = 48h) */
7275 +       {0x300D, 0x02}, /* ct_rmp_lat (default = 02h) */
7276 +       {0x300E, 0xA9}, /* D-Shut en[7], CLP On[5], LD high[4] */
7277 +
7278 +/* CDS option setting ... */
7279 +       {0x3010, 0x00}, /* smp_en[2]=0(00) 1(04) row_id[1:0] = 00 */
7280 +       {0x3011, 0x7A}, /* RST_MX (288), SIG_MX (1024+352) */
7281 +       {0x3012, 0x30}, /* SIG offset1  48 code */
7282 +       {0x3013, 0xA0}, /* RST offset1  160 code */
7283 +       {0x3014, 0x00}, /* SIG offset2 */
7284 +       {0x3015, 0x00}, /* RST offset2 */
7285 +       {0x3016, 0x0A}, /* ADC_SAT (510mV) */
7286 +       {0x3017, 0x94}, /* RMP_INIT[3:0](RMP_REG) 1.8V MS[6:4]=1 */
7287 +       {0x3018, 0x78}, /* rmp option - ramp connect[MSB] +RMP INIT DAC MIN */
7288 +
7289 +       {0x301D, 0xD4}, /* CLP level (default = 0Fh) */
7290 +
7291 +       {0x3021, 0x02}, /* inrush ctrl[1] off */
7292 +       {0x3022, 0x41}, /* pump ring oscillator set [7:4]=CP, [3:0]=NCP */
7293 +       {0x3024, 0x08}, /* pix voltage 2.8V   (default = 88h) */
7294 +       {0x3027, 0x08}, /* ntg voltage (default = 04h) */
7295 +
7296 +/* Pixel option setting ... */
7297 +       {0x301C, 0x05}, /* Pixel Bias [3:0] (default = 03h) */
7298 +       {0x30D8, 0x3F}, /* All tx off 2f, on 3f */
7299 +
7300 +/* ADLC setting ... */
7301 +       {0x3070, 0x5F}, /* [6]L-ADLC BPR, [4]ch sel, [3]L-ADLC, [2]F-ADLC */
7302 +       {0x3071, 0x00}, /* F&L-adlc max 127 (default = 11h, max 255) */
7303 +       {0x3080, 0x04}, /* F-ADLC filter A (default = 10h) */
7304 +       {0x3081, 0x38}, /* F-ADLC filter B (default = 20h) */
7305 +
7306 +/* Integration setting ... */
7307 +       {0x0202, 0x03}, /* coarse integration time */
7308 +       {0x0203, 0xCD},
7309 +       {0x0204, 0x00}, /* analog gain[msb] 0100 x8 0080 x4 */
7310 +       {0x0205, 0x80}, /* analog gain[lsb] 0040 x2 0020 x1 */
7311 +
7312 +/* Frame Length */
7313 +       {0x0340, 0x04}, /*Capture 07B4(1960[# of row]+12[V-blank]) */
7314 +       {0x0341, 0x44}, /*Preview 03E0(980[# of row]+12[V-blank]) */
7315 +
7316 +/* Line Length */
7317 +       {0x0342, 0x0A}, /* 2738 */
7318 +       {0x0343, 0xB2}, /*(Same as sensor default) */
7319 +
7320 +/* embedded 2-line OFF setting ... */
7321 +/* 1920 x 1080 */
7322 +       {0x3084, 0x15}, /* SYNC Mode */
7323 +
7324 +/* PLL & MIPI setting ... */
7325 +/* input clock 25MHz */
7326 +
7327 +/* (3) MIPI 2-lane Serial(TST = 0000b or TST = 0010b), 30 fps */
7328 +       {0x30BD, 0x00},         /* SEL_CCP[0] */
7329 +       {0x30B2, 0x08},         /* PLL P = 8 */
7330 +       {0x30B3, 0x00},         /* PLL M[8] = 0 */
7331 +       {0x30B4, 0x78},         /* PLL M = 129 */
7332 +       {0x30B5, 0x00},         /* PLL S = 0 */
7333 +       {0x30BE, 0x1A},         /* M_PCLKDIV_AUTO[4], M_DIV_PCLK[3:0] */
7334 +
7335 +       {0x30BF, 0xAB},
7336 +       {0x30C0, 0x00},         /* video_offset[7:4] 2400%12 */
7337 +       {0x30C1, 0x01},         /* pack video enable [0] */
7338 +       {0x30C8, 0x09},         /* video_data_length 2400 = 1920 * 1.25 */
7339 +       {0x30C9, 0x60},
7340 +       {0x30E2, 0x02},         /* num lanes[1:0] = 2 */
7341 +       {0x30EE, 0x02},         /* DPHY enable [1] */
7342 +       {0x30F1, 0x70},         /* DPHY BANDCTRL 800MHz=80.6MHz */
7343 +       {0x3111, 0x86},         /* Embedded data off [5] */
7344 +
7345 +       {0x30b4, 0x20},
7346 +       {0x30b5, 0x01},
7347 +
7348 +       {0x30A9, 0x01},
7349 +       {0x0387, 0x01},
7350 +       {0x0344, 0x01}, /*x_addr_start 344 */
7351 +       {0x0345, 0x58},
7352 +       {0x0348, 0x08}, /*x_addr_end 2263 */
7353 +       {0x0349, 0xD7},
7354 +       {0x0346, 0x01}, /*y_addr_start 440 */
7355 +       {0x0347, 0xB8},
7356 +       {0x034A, 0x05}, /*y_addr_end 1519 */
7357 +       {0x034B, 0xEF},
7358 +
7359 +       {0x034C, 0x07}, /*x_output_size 1920 */
7360 +       {0x034D, 0x80},
7361 +       {0x034E, 0x04}, /*y_output_size 1080 */
7362 +       {0x034F, 0x38},
7363 +
7364 +       {0x30d9, 0x00},
7365 +
7366 +       {0x020E, 0x01},      /*Gr Digital Gain */
7367 +       {0x020F, 0x00},
7368 +       {0x0210, 0x01},     /*Red Digital Gain */
7369 +       {0x0211, 0x00},
7370 +       {0x0212, 0x01},     /*Blue Digital Gain */
7371 +       {0x0213, 0x00},
7372 +       {0x0214, 0x01},    /*Gb Digital Gain */
7373 +       {0x0215, 0x00},
7374 +       {0x0204, 0x00},
7375 +       {0x0205, 0x80},
7376 +
7377 +
7378 +       /*Apply Bill's setting*/
7379 +       {0x30E2, 0x02},
7380 +       {0x0305, 0x05},
7381 +       {0x0306, 0x00},
7382 +       {0x0307, 0x50},         /*vcc_out = 80 */
7383 +       {0x30B5, 0x01},         /*pll_s = 1 */
7384 +       {0x30B4, 0x50},
7385 +
7386 +       {0x30B2, 0x05},
7387 +
7388 +       {0x30BE, 0x1A},         /*DIV_M_PCLK = 5 */
7389 +
7390 +       {0x0383, 0x01},
7391 +
7392 +       {0x0100, 0x01},         /* stream on */
7393 +       {0xffff, 0xff},
7394 +
7395 +};
7396 +
7397 +/* 1280x720, V1F2 & H1F2 */
7398 +static struct regval_list s5k4e1_res_720p[] = {
7399 +       {0x0100, 0x00},         /* stream off */
7400 +       {0x0103, 0x01},         /* software reset */
7401 +
7402 +/* CDS timing setting ... */
7403 +       {0x3000, 0x04},
7404 +       {0x3001, 0x02},
7405 +       {0x3002, 0x0C},
7406 +       {0x3003, 0x0E},
7407 +       {0x3004, 0x2C},
7408 +       {0x3005, 0x0D},
7409 +       {0x3006, 0x39},
7410 +       {0x3007, 0x02},
7411 +       {0x3008, 0x3C},
7412 +       {0x3009, 0x3C},
7413 +       {0x300A, 0x28},
7414 +       {0x300B, 0x15},
7415 +       {0x300C, 0x15},
7416 +       {0x300D, 0x02},
7417 +       {0x300E, 0xAB},
7418 +
7419 +/* CDS option setting ... */
7420 +       {0x3010, 0x00},
7421 +       {0x3011, 0x7A},
7422 +       {0x3012, 0x30},
7423 +       {0x3013, 0x90},
7424 +       {0x3014, 0x00},
7425 +       {0x3015, 0x00},
7426 +       {0x3016, 0x0A},
7427 +       {0x3017, 0x84},
7428 +       {0x3018, 0x78},
7429 +       {0x301D, 0xD4},
7430 +
7431 +       {0x3021, 0x02},
7432 +       {0x3022, 0x41},
7433 +       {0x3024, 0x08},
7434 +       {0x3027, 0x08},
7435 +
7436 +/* Pixel option setting ... */
7437 +       {0x301C, 0x05}, /* Pixel Bias [3:0] (default = 03h) */
7438 +       {0x30D8, 0x3F}, /* All tx off 2f, on 3f */
7439 +
7440 +/* ADLC setting ... */
7441 +       {0x3070, 0x5F},
7442 +       {0x3071, 0x00},
7443 +       {0x3080, 0x04},
7444 +       {0x3081, 0x38},
7445 +
7446 +/* Integration setting ...  */
7447 +       {0x0202, 0x03},
7448 +       {0x0203, 0xD8},
7449 +       {0x0204, 0x00},
7450 +       {0x0205, 0x80},
7451 +
7452 +/*Frame Length*/
7453 +       {0x0340, 0x02},
7454 +       {0x0341, 0xDC},
7455 +
7456 +/* Line Length */
7457 +       {0x0342, 0x0A}, /*2738 */
7458 +       {0x0343, 0xB2},
7459 +
7460 +/* Average Sub-sampling */
7461 +       {0x0387, 0x03},
7462 +       {0x30a9, 0x02},
7463 +
7464 +/* embedded 2-line OFF setting ... */
7465 +/* 1280 x 720 */
7466 +       {0x3084, 0x15},
7467 +
7468 +/* PLL & MIPI setting ... */
7469 +
7470 +/* (3) MIPI 2-lane Serial(TST = 0000b or TST = 0010b), 60 fps */
7471 +       {0x30BD, 0x00},
7472 +       {0x30B2, 0x08},
7473 +       {0x30B3, 0x00},
7474 +       {0x30B4, 0x78},
7475 +       {0x30B5, 0x00},
7476 +       {0x30BE, 0x1A},
7477 +
7478 +       {0x30BF, 0xAB},
7479 +       {0x30C0, 0x40},
7480 +       {0x30C1, 0x01},
7481 +       {0x30C8, 0x06},
7482 +       {0x30C9, 0x40},
7483 +
7484 +       {0x30E2, 0x02},
7485 +
7486 +       {0x30b4, 0x20},
7487 +       {0x30b5, 0x01},
7488 +
7489 +       {0x30EE, 0x02},
7490 +       {0x30F1, 0x70},
7491 +       {0x3111, 0x86},
7492 +
7493 +/* MIPI Size Setting ... */
7494 +/* 1304 x 980 */
7495 +       {0x0344, 0x00},
7496 +       {0x0345, 0x18},
7497 +       {0x0348, 0x0A},
7498 +       {0x0349, 0x17},
7499 +       {0x0346, 0x01},
7500 +       {0x0347, 0x04},
7501 +       {0x034A, 0x06},
7502 +       {0x034B, 0xA3},
7503 +
7504 +       {0x0380, 0x00},
7505 +       {0x0381, 0x01},
7506 +       {0x0382, 0x00},
7507 +       {0x0383, 0x01},
7508 +       {0x0384, 0x00},
7509 +       {0x0385, 0x01},
7510 +       {0x0386, 0x00},
7511 +       {0x0387, 0x03},
7512 +
7513 +       {0x034C, 0x05}, /* x_output_size = 1280 */
7514 +       {0x034D, 0x00},
7515 +       {0x034E, 0x02}, /* y_output_size = 720 */
7516 +       {0x034F, 0xD0},
7517 +
7518 +       {0x30d9, 0x00},
7519 +
7520 +       {0x020E, 0x01},
7521 +       {0x020F, 0x00},
7522 +       {0x0210, 0x01},
7523 +       {0x0211, 0x00},
7524 +       {0x0212, 0x01},
7525 +       {0x0213, 0x00},
7526 +       {0x0214, 0x01},
7527 +       {0x0215, 0x00},
7528 +       {0x0204, 0x01},
7529 +       {0x0205, 0x00},
7530 +
7531 +       /*Apply Bill's setting*/
7532 +       {0x30E2, 0x02},
7533 +       {0x0305, 0x05},
7534 +       {0x0306, 0x00},
7535 +       {0x0307, 0x50},         /*vcc_out = 80 */
7536 +       {0x30B5, 0x01},         /*pll_s = 1 */
7537 +       {0x30B4, 0x50},
7538 +
7539 +       {0x30B2, 0x05},
7540 +
7541 +       {0x30BE, 0x15},         /*DIV_M_PCLK = 5 */
7542 +
7543 +       {0x0100, 0x01},         /* stream on */
7544 +       {0xffff, 0xff},
7545 +};
7546 +
7547 +/*VGA*/
7548 +static struct regval_list s5k4e1_res_vga_ac04_bill[] = {
7549 +       {0x0100, 0x00},         /* stream off */
7550 +       {0x0103, 0x01},         /* software reset */
7551 +
7552 +       {0x3000, 0x04},
7553 +       {0x3001, 0x02},
7554 +       {0x3002, 0x0C},
7555 +       {0x3003, 0x0E},
7556 +       {0x3004, 0x2C},
7557 +       {0x3005, 0x0D},
7558 +       {0x3006, 0x39},
7559 +       {0x3007, 0x02},
7560 +       {0x3008, 0x3C},
7561 +       {0x3009, 0x3C},
7562 +       {0x300A, 0x28},
7563 +       {0x300B, 0x15},
7564 +       {0x300C, 0x15},
7565 +       {0x300D, 0x02},
7566 +       {0x300E, 0xA8},
7567 +
7568 +       {0x3010, 0x00},
7569 +       {0x3011, 0x7A},
7570 +       {0x3012, 0x30},
7571 +       {0x3013, 0xA0},
7572 +       {0x3014, 0x00},
7573 +       {0x3015, 0x00},
7574 +       {0x3016, 0x0A},
7575 +       {0x3017, 0x94},
7576 +       {0x3018, 0x78},
7577 +
7578 +       {0x301D, 0xD4},
7579 +
7580 +       {0x3021, 0x02},
7581 +       {0x3022, 0x41},
7582 +       {0x3024, 0x08},
7583 +       {0x3027, 0x08},
7584 +
7585 +       {0x301C, 0x05},
7586 +       {0x30D8, 0x3F},
7587 +
7588 +       {0x3070, 0x5F},
7589 +       {0x3071, 0x00},
7590 +       {0x3080, 0x04},
7591 +       {0x3081, 0x38},
7592 +
7593 +       {0x0202, 0x03},
7594 +       {0x0203, 0xD4},
7595 +       {0x0204, 0x00},
7596 +       {0x0205, 0x20},
7597 +
7598 +       {0x0340, 0x03},
7599 +       {0x0341, 0xE0},
7600 +
7601 +       {0x0342, 0x0A},
7602 +       {0x0343, 0xB2},
7603 +
7604 +       {0x0344, 0x00},
7605 +       {0x0345, 0x18},
7606 +       {0x0348, 0x0A},
7607 +       {0x0349, 0x17},
7608 +       {0x0346, 0x00},
7609 +       {0x0347, 0x14},
7610 +       {0x034A, 0x07},
7611 +       {0x034B, 0x93},
7612 +
7613 +       {0x034C, 0x02},
7614 +       {0x034D, 0x80},
7615 +       {0x034E, 0x01},
7616 +       {0x034F, 0xE0},
7617 +
7618 +       {0x0380, 0x00},
7619 +       {0x0381, 0x01},
7620 +       {0x0382, 0x00},
7621 +       {0x0383, 0x07},
7622 +       {0x0384, 0x00},
7623 +       {0x0385, 0x01},
7624 +       {0x0386, 0x00},
7625 +       {0x0387, 0x07},
7626 +
7627 +       {0x3084, 0x15},
7628 +
7629 +       {0x30BD, 0x00},
7630 +
7631 +
7632 +       {0x30b3, 0x00},
7633 +       {0x30b4, 0x57},
7634 +       {0x30b5, 0x01},
7635 +       {0x30f1, 0x70},
7636 +
7637 +       {0x30BE, 0x1A},
7638 +
7639 +       {0x30BF, 0xAB},
7640 +       {0x30C0, 0x80},
7641 +       {0x30C1, 0x01},
7642 +       {0x30C8, 0x03},
7643 +       {0x30C9, 0x20},
7644 +
7645 +       {0x30b2, 0x06},
7646 +       {0x30E2, 0x02},
7647 +
7648 +       {0x30EE, 0x02},
7649 +
7650 +       {0x3111, 0x86},
7651 +
7652 +       {0x30d9, 0x00},
7653 +
7654 +       {0x020E, 0x01},
7655 +       {0x020F, 0x00},
7656 +       {0x0210, 0x01},
7657 +       {0x0211, 0x00},
7658 +       {0x0212, 0x01},
7659 +       {0x0213, 0x00},
7660 +       {0x0214, 0x01},
7661 +       {0x0215, 0x00},
7662 +       {0x0204, 0x01},
7663 +       {0x0205, 0x00},
7664 +
7665 +#if 1
7666 +       /* Apply Bill's setting */
7667 +       {0x30E2, 0x02},
7668 +       {0x0305, 0x05},
7669 +       {0x0306, 0x00},
7670 +       {0x0307, 0x50},
7671 +       {0x30B5, 0x01},
7672 +       {0x30B4, 0x50},
7673 +
7674 +       {0x30B2, 0x05},
7675 +
7676 +       {0x30BE, 0x15},
7677 +
7678 +       /* {0x0100, 0x01}, */
7679 +       /* {0xffff, 0xff}, */
7680 +#endif
7681 +
7682 +#if 1
7683 +       /* 1304x980 */
7684 +       {0x3013, 0x90},
7685 +       {0x3017, 0x84},
7686 +       {0x30A9, 0x02},
7687 +       {0x300E, 0xAB},
7688 +
7689 +       {0x0387, 0x03},
7690 +       {0x0344, 0x00}, /* x_addr_start = 0 */
7691 +       {0x0345, 0x00},
7692 +       {0x0348, 0x0A}, /* x_addr_end = 2607 */
7693 +       {0x0349, 0x2F},
7694 +       {0x0346, 0x00}, /* y_addr_start = 0 */
7695 +       {0x0347, 0x00},
7696 +       {0x034A, 0x07}, /* y_addr_end = 1959 */
7697 +       {0x034B, 0xA7},
7698 +       {0x0380, 0x00},
7699 +       {0x0381, 0x01},
7700 +       {0x0382, 0x00},
7701 +       {0x0383, 0x01},
7702 +       {0x0384, 0x00},
7703 +       {0x0385, 0x01},
7704 +       {0x0386, 0x00},
7705 +       {0x0387, 0x03},
7706 +       {0x034c, 0x05}, /* x_output_size = 1304 */
7707 +       {0x034d, 0x18},
7708 +       {0x034e, 0x03}, /* y_output_size = 980 */
7709 +       {0x034f, 0xd4},
7710 +       {0x30BF, 0xAB},
7711 +       {0x30c0, 0xa0},
7712 +       {0x30C8, 0x06}, /* x_output_size * 1.25 */
7713 +       {0x30c9, 0x5e},
7714 +
7715 +       {0x0100, 0x01},
7716 +       {0xffff, 0xff},
7717 +
7718 +#endif
7719 +};
7720 diff --git a/drivers/media/video/mrstci/mrsts5k4e1_motor/Kconfig b/drivers/media/video/mrstci/mrsts5k4e1_motor/Kconfig
7721 new file mode 100755
7722 index 0000000..27cb730
7723 --- /dev/null
7724 +++ b/drivers/media/video/mrstci/mrsts5k4e1_motor/Kconfig
7725 @@ -0,0 +1,9 @@
7726 +config VIDEO_MRST_S5K4E1_MOTOR
7727 +       tristate "Moorestown s5k4e1 motor"
7728 +       depends on I2C && VIDEO_MRST_ISP && VIDEO_MRST_S5K4E1
7729 +
7730 +       ---help---
7731 +         Say Y here if your platform support s5k4e1 motor.
7732 +
7733 +         To compile this driver as a module, choose M here: the
7734 +         module will be called mrstov2650.ko.
7735 diff --git a/drivers/media/video/mrstci/mrsts5k4e1_motor/Makefile b/drivers/media/video/mrstci/mrsts5k4e1_motor/Makefile
7736 new file mode 100644
7737 index 0000000..68c9fbc
7738 --- /dev/null
7739 +++ b/drivers/media/video/mrstci/mrsts5k4e1_motor/Makefile
7740 @@ -0,0 +1,3 @@
7741 +obj-$(CONFIG_VIDEO_MRST_S5K4E1_MOTOR)   += mrsts5k4e1_motor.o
7742 +
7743 +EXTRA_CFLAGS   +=      -I$(src)/../include
7744 diff --git a/drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.c b/drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.c
7745 new file mode 100644
7746 index 0000000..cd2813b
7747 --- /dev/null
7748 +++ b/drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.c
7749 @@ -0,0 +1,430 @@
7750 +/*
7751 + * Support for Moorestown Langwell Camera Imaging ISP subsystem.
7752 + *
7753 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
7754 + *
7755 + * This program is free software; you can redistribute it and/or
7756 + * modify it under the terms of the GNU General Public License version
7757 + * 2 as published by the Free Software Foundation.
7758 + *
7759 + * This program is distributed in the hope that it will be useful,
7760 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
7761 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
7762 + * GNU General Public License for more details.
7763 + *
7764 + * You should have received a copy of the GNU General Public License
7765 + * along with this program; if not, write to the Free Software
7766 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
7767 + * 02110-1301, USA.
7768 + *
7769 + *
7770 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
7771 + */
7772 +
7773 +#include <linux/module.h>
7774 +#include <linux/types.h>
7775 +#include <linux/kernel.h>
7776 +#include <linux/mm.h>
7777 +#include <linux/string.h>
7778 +#include <linux/errno.h>
7779 +#include <linux/init.h>
7780 +#include <linux/kmod.h>
7781 +#include <linux/device.h>
7782 +#include <linux/delay.h>
7783 +#include <linux/fs.h>
7784 +#include <linux/slab.h>
7785 +#include <linux/delay.h>
7786 +#include <linux/i2c.h>
7787 +#include <linux/gpio.h>
7788 +
7789 +#include <media/v4l2-device.h>
7790 +#include <media/v4l2-chip-ident.h>
7791 +#include <media/v4l2-i2c-drv.h>
7792 +
7793 +#include "mrsts5k4e1_motor.h"
7794 +
7795 +static int s5k4e1_motor_debug;
7796 +module_param(s5k4e1_motor_debug, int, 0644);
7797 +MODULE_PARM_DESC(s5k4e1_motor_debug, "Debug level (0-1)");
7798 +
7799 +#define dprintk(level, fmt, arg...) \
7800 +    do {                       \
7801 +       if (s5k4e1_motor_debug >= level) \
7802 +               printk(KERN_DEBUG "mrstisp@%s: " fmt "\n", __func__, ## arg); \
7803 +    } while (0)
7804 +
7805 +#define eprintk(fmt, arg...)   \
7806 +       printk(KERN_ERR "mrstisp@%s: line %d: " fmt "\n",       \
7807 +              __func__, __LINE__, ## arg);
7808 +
7809 +#define DBG_entering   dprintk(1, "entering");
7810 +#define DBG_leaving    dprintk(1, "leaving");
7811 +#define DBG_line       dprintk(1, " line: %d", __LINE__);
7812 +
7813 +static inline struct s5k4e1_motor *to_motor_config(struct v4l2_subdev *sd)
7814 +{
7815 +       return container_of(sd, struct s5k4e1_motor, sd);
7816 +}
7817 +
7818 +/*static struct s5k4e1_motor *config; */
7819 +static int motor_read(struct i2c_client *c, u32 *reg)
7820 +{
7821 +       int ret;
7822 +       struct i2c_msg msg;
7823 +       u8 msgbuf[3];
7824 +
7825 +       msgbuf[0] = 0;
7826 +       msgbuf[1] = 0;
7827 +       msgbuf[2] = 0;
7828 +
7829 +       memset(&msg, 0, sizeof(msg));
7830 +       msg.addr = c->addr;
7831 +       msg.buf = msgbuf;
7832 +       msg.len = 3;
7833 +       msg.flags = I2C_M_RD;
7834 +
7835 +       ret = i2c_transfer(c->adapter, &msg, 1);
7836 +
7837 +       *reg = (msgbuf[0] << 16 | msgbuf[1] << 8 | msgbuf[2]);
7838 +
7839 +       ret = (ret == 1) ? 0 : -1;
7840 +       return ret;
7841 +}
7842 +
7843 +static int motor_write(struct i2c_client *c, u32 reg)
7844 +{
7845 +       int ret;
7846 +       struct i2c_msg msg;
7847 +       u8 msgbuf[3];
7848 +
7849 +       memset(&msg, 0, sizeof(msg));
7850 +       msgbuf[0] = (reg & 0x00FFFFFFFF) >> 16;
7851 +       msgbuf[1] = (reg & 0x0000FFFF) >> 8 ;
7852 +       msgbuf[2] = reg;
7853 +
7854 +       msg.addr = c->addr;
7855 +       msg.flags = 0;
7856 +       msg.buf = msgbuf;
7857 +       msg.len = 3;
7858 +
7859 +       ret = i2c_transfer(c->adapter, &msg, 1);
7860 +
7861 +       ret = (ret == 1) ? 0 : -1;
7862 +       return ret;
7863 +}
7864 +
7865 +static int s5k4e1_motor_goto_position(struct i2c_client *c,
7866 +                                     unsigned short code,
7867 +                                     struct s5k4e1_motor *config,
7868 +                                     unsigned int step)
7869 +{
7870 +       int max_code, min_code;
7871 +       int timeout = 25; /*TODO: check the timeout time */
7872 +       u8 cmdh, cmdl, finished;
7873 +       u32 cmd = 0, val = 0;
7874 +
7875 +       max_code = config->macro_code;
7876 +       min_code = config->infin_code;
7877 +
7878 +       if (code > max_code)
7879 +               code = max_code;
7880 +       if (code < min_code)
7881 +               code = min_code;
7882 +
7883 +       cmdh = MOTOR_DAC_CTRL_MODE_1 | (code >> 8); /* PS EN x x M W TD9 TD8*/
7884 +       cmdl = code; /* TD7 ~ TD0 */
7885 +       cmd |= (cmdh << 16) | (cmdl << 8);
7886 +
7887 +       dprintk(1, "cmdh: %x, cmdl: %x, cmd: %x", cmdh, cmdl, cmd);
7888 +       dprintk(1, "DAC code: %x", code);
7889 +
7890 +       motor_write(c, cmd);
7891 +       finished = 0;
7892 +       while ((!finished) && timeout--) {
7893 +               msleep(1);
7894 +               motor_read(c, &val);
7895 +               cmdh = val >> 16;
7896 +               cmdl = val >> 8;
7897 +
7898 +               dprintk(1, "cmdh & MOTOR_F = %x", cmdh & MOTOR_F);
7899 +               finished = cmdh & MOTOR_F;
7900 +               finished = (finished) ? 0 : 1;
7901 +       };
7902 +
7903 +       if (finished) {
7904 +               dprintk(1, "Moving from code %x to code %x takes %d ms.",
7905 +                      config->cur_code, code, 25-timeout);
7906 +               return 0;
7907 +       } else {
7908 +               eprintk("Unable to move motor to step %d, TIMEOUT!!", step);
7909 +               return -1;
7910 +       }
7911 +
7912 +}
7913 +
7914 +int s5k4e1_motor_wakeup(struct i2c_client *client)
7915 +{
7916 +       /* hardware wakeup: set PS = 1 */
7917 +       return motor_write(client, 0xC00000);
7918 +}
7919 +
7920 +int s5k4e1_motor_standby(struct i2c_client *client)
7921 +{
7922 +       /* hardware standby: set PS = 0 */
7923 +       return motor_write(client, 0x400000);
7924 +}
7925 +
7926 +int s5k4e1_motor_init(struct i2c_client *client, struct s5k4e1_motor *config)
7927 +{
7928 +
7929 +       int ret;
7930 +       int infin_cur, macro_cur;
7931 +       int step_res, step_time;
7932 +       int val;
7933 +
7934 +       DBG_entering;
7935 +       infin_cur = MAX(MOTOR_INFIN_CUR, MOTOR_DAC_MIN_CUR);
7936 +       macro_cur = MIN(MOTOR_MACRO_CUR, MOTOR_DAC_MAX_CUR);
7937 +       step_res = 1 << MOTOR_STEP_SHIFT;
7938 +       step_time = MOTOR_STEP_TIME;
7939 +
7940 +       /*config->motor = client;*/
7941 +       config->infin_cur = infin_cur;
7942 +       config->macro_cur = macro_cur;
7943 +
7944 +       config->infin_code = MOTOR_INFIN_CODE;
7945 +       config->macro_code = MOTOR_MACRO_CODE;
7946 +
7947 +       config->max_step = ((config->macro_code - config->infin_code)
7948 +                           >> MOTOR_STEP_SHIFT) + 1;
7949 +       config->step_res = step_res;
7950 +       config->step_time = step_time;
7951 +
7952 +       dprintk(1, "max_step: %d, step_res: %d, step_time: %d",
7953 +               config->max_step, step_res, step_time);
7954 +
7955 +       /* Set motor step time and resolution */
7956 +       val = (MOTOR_DAC_CTRL_MODE_0 << 16) | (step_res << 8) | step_time;
7957 +       ret = motor_write(client, val);
7958 +
7959 +       /* Note here, maybe macro_code */
7960 +       ret |= s5k4e1_motor_goto_position(client, config->infin_code,
7961 +                                         config, 0);
7962 +       if (!ret) {
7963 +               config->cur_code = config->infin_code;
7964 +               dprintk(1, "Motor initialization success!");
7965 +       } else
7966 +               eprintk("Error while initializing motor!!!");
7967 +
7968 +       return ret;
7969 +}
7970 +
7971 +int s5k4e1_motor_set_focus(struct i2c_client *c,
7972 +                          unsigned int step,
7973 +                          struct s5k4e1_motor *config)
7974 +{
7975 +       int s_code, ret;
7976 +       int max_step = config->max_step;
7977 +       unsigned int val = step;
7978 +
7979 +       if (val > max_step)
7980 +               val = max_step;
7981 +
7982 +       s_code = (val << MOTOR_STEP_SHIFT);
7983 +       s_code += config->infin_code;
7984 +
7985 +       ret = s5k4e1_motor_goto_position(c, s_code, config, step);
7986 +       if (!ret)
7987 +               config->cur_code = s_code;
7988 +
7989 +       return ret;
7990 +}
7991 +
7992 +static int s5k4e1_motor_g_ctrl(struct v4l2_subdev *sd,
7993 +                              struct v4l2_control *ctrl)
7994 +{
7995 +       struct i2c_client *c = v4l2_get_subdevdata(sd);
7996 +       struct s5k4e1_motor *config = to_motor_config(sd);
7997 +       int ret;
7998 +
7999 +       DBG_entering;
8000 +       ret = s5k4e1_motor_get_focus(c, &ctrl->value, config);
8001 +       if (ret) {
8002 +               eprintk("error call s5k4e1_motor_get_focue");
8003 +               return ret;
8004 +       }
8005 +       DBG_leaving;
8006 +       return 0;
8007 +}
8008 +
8009 +static int s5k4e1_motor_s_ctrl(struct v4l2_subdev *sd,
8010 +                              struct v4l2_control *ctrl)
8011 +{
8012 +       struct i2c_client *c = v4l2_get_subdevdata(sd);
8013 +       struct s5k4e1_motor *config = to_motor_config(sd);
8014 +       int ret;
8015 +
8016 +       DBG_entering;
8017 +       ret = s5k4e1_motor_set_focus(c, ctrl->value, config);
8018 +       if (ret) {
8019 +               eprintk("error call s5k4e1_motor_set_focue");
8020 +               return ret;
8021 +       }
8022 +       DBG_leaving;
8023 +       return 0;
8024 +}
8025 +
8026 +int s5k4e1_motor_get_focus(struct i2c_client *c,
8027 +                          unsigned int *step,
8028 +                          struct s5k4e1_motor *config)
8029 +{
8030 +       int ret_step;
8031 +
8032 +       ret_step = ((config->cur_code - config->infin_code)
8033 +                   >> MOTOR_STEP_SHIFT);
8034 +
8035 +       if (ret_step <= config->max_step)
8036 +               *step = ret_step;
8037 +       else
8038 +               *step = config->max_step;
8039 +
8040 +       return 0;
8041 +}
8042 +
8043 +int s5k4e1_motor_max_step(struct i2c_client *c,
8044 +                         unsigned int *max_code,
8045 +                         struct s5k4e1_motor *config)
8046 +{
8047 +       if (config->max_step != 0)
8048 +               *max_code = config->max_step;
8049 +       return 0;
8050 +
8051 +}
8052 +
8053 +static int s5k4e1_motor_queryctrl(struct v4l2_subdev *sd,
8054 +                           struct v4l2_queryctrl *qc)
8055 +{
8056 +       struct s5k4e1_motor *config = to_motor_config(sd);
8057 +
8058 +       DBG_entering;
8059 +       dprintk(1, "got focus range of %d", config->max_step);
8060 +       if (config->max_step != 0)
8061 +               qc->maximum = config->max_step;
8062 +       DBG_leaving;
8063 +       return 0;
8064 +}
8065 +
8066 +static const struct v4l2_subdev_core_ops s5k4e1_motor_core_ops = {
8067 +       .g_ctrl = s5k4e1_motor_g_ctrl,
8068 +       .s_ctrl = s5k4e1_motor_s_ctrl,
8069 +       .queryctrl = s5k4e1_motor_queryctrl,
8070 +};
8071 +
8072 +static const struct v4l2_subdev_ops s5k4e1_motor_ops = {
8073 +       .core = &s5k4e1_motor_core_ops,
8074 +};
8075 +
8076 +static int s5k4e1_motor_detect(struct i2c_client *client)
8077 +{
8078 +       struct i2c_adapter *adapter = client->adapter;
8079 +       int adap_id = i2c_adapter_id(adapter);
8080 +
8081 +       if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
8082 +               eprintk("error i2c check func");
8083 +               return -ENODEV;
8084 +       }
8085 +
8086 +       if (adap_id != 1) {
8087 +               eprintk("adap_id != 1");
8088 +               return -ENODEV;
8089 +       }
8090 +
8091 +       if (s5k4e1_motor_wakeup(client))
8092 +               eprintk("unable to wakeup s5k4e1 motor.");
8093 +
8094 +       return 0;
8095 +}
8096 +
8097 +static int s5k4e1_motor_probe(struct i2c_client *client,
8098 +                       const struct i2c_device_id *id)
8099 +{
8100 +       struct s5k4e1_motor *info;
8101 +       struct v4l2_subdev *sd;
8102 +       int ret = -1;
8103 +/*     struct i2c_client *motor; */
8104 +
8105 +       DBG_entering;
8106 +       v4l_info(client, "chip found @ 0x%x (%s)\n",
8107 +                client->addr << 1, client->adapter->name);
8108 +       /*
8109 +        * Setup sensor configuration structure
8110 +        */
8111 +       info = kzalloc(sizeof(struct s5k4e1_motor), GFP_KERNEL);
8112 +       if (!info) {
8113 +               eprintk("fail to malloc for ci_motor");
8114 +               ret = -ENOMEM;
8115 +               goto out;
8116 +       }
8117 +
8118 +       ret = s5k4e1_motor_detect(client);
8119 +       if (ret) {
8120 +               eprintk("error s5k4e1_motor_detect");
8121 +               goto out_free;
8122 +       }
8123 +
8124 +       sd = &info->sd;
8125 +       v4l2_i2c_subdev_init(sd, client, &s5k4e1_motor_ops);
8126 +
8127 +       /*
8128 +        * Initialization S5K4E1
8129 +        * then turn into standby mode
8130 +        */
8131 +       ret = s5k4e1_motor_init(client, info);
8132 +       if (ret) {
8133 +               eprintk("error calling s5k4e1_motor_init");
8134 +               goto out_free;
8135 +       }
8136 +
8137 +       ret = 0;
8138 +       goto out;
8139 +
8140 +out_free:
8141 +       kfree(info);
8142 +       DBG_leaving;
8143 +out:
8144 +       return ret;
8145 +}
8146 +
8147 +/*
8148 + * XXX: Need to be checked
8149 + */
8150 +static int s5k4e1_motor_remove(struct i2c_client *client)
8151 +{
8152 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
8153 +
8154 +       DBG_entering;
8155 +
8156 +       v4l2_device_unregister_subdev(sd);
8157 +       kfree(to_motor_config(sd));
8158 +
8159 +       DBG_leaving;
8160 +       return 0;
8161 +}
8162 +
8163 +static const struct i2c_device_id s5k4e1_motor_id[] = {
8164 +       {"s5k4e1_motor", 0},
8165 +       {}
8166 +};
8167 +MODULE_DEVICE_TABLE(i2c, s5k4e1_motor_id);
8168 +
8169 +static struct v4l2_i2c_driver_data v4l2_i2c_data = {
8170 +       .name = "s5k4e1_motor",
8171 +       .probe = s5k4e1_motor_probe,
8172 +       .remove = s5k4e1_motor_remove,
8173 +       /* .suspend = ov5630_suspend,
8174 +        * .resume = ov5630_resume, */
8175 +       .id_table = s5k4e1_motor_id,
8176 +};
8177 +MODULE_AUTHOR("Xiaolin Zhang <xiaolin.zhang@intel.com>");
8178 +MODULE_DESCRIPTION("A low-level driver for Samsung S5K4E1 sensor motor");
8179 +MODULE_LICENSE("GPL");
8180 diff --git a/drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.h b/drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.h
8181 new file mode 100644
8182 index 0000000..04f9436
8183 --- /dev/null
8184 +++ b/drivers/media/video/mrstci/mrsts5k4e1_motor/mrsts5k4e1_motor.h
8185 @@ -0,0 +1,102 @@
8186 +/*
8187 + * Support for Moorestown Langwell Camera Imaging ISP subsystem.
8188 + *
8189 + * Copyright (c) 2009 Intel Corporation. All Rights Reserved.
8190 + *
8191 + * This program is free software; you can redistribute it and/or
8192 + * modify it under the terms of the GNU General Public License version
8193 + * 2 as published by the Free Software Foundation.
8194 + *
8195 + * This program is distributed in the hope that it will be useful,
8196 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
8197 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
8198 + * GNU General Public License for more details.
8199 + *
8200 + * You should have received a copy of the GNU General Public License
8201 + * along with this program; if not, write to the Free Software
8202 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
8203 + * 02110-1301, USA.
8204 + *
8205 + *
8206 + * Xiaolin Zhang <xiaolin.zhang@intel.com>
8207 + */
8208 +
8209 +#include <media/v4l2-subdev.h>
8210 +
8211 +/* DAC output max current (mA) */
8212 +#define MOTOR_DAC_MAX_CUR      125
8213 +/* DAC output min current (mA) */
8214 +#define MOTOR_DAC_MIN_CUR      1
8215 +/* DAC max code (Hex) */
8216 +#define MOTOR_DAC_CODE_MAX     0x3FF
8217 +/* DAC min code (Hex) */
8218 +#define MOTOR_DAC_CODE_MIN     0x0
8219 +
8220 +/* VCM start code (Hex) */
8221 +#define MOTOR_INFIN_CODE       0x120
8222 +/* VCM stop code (Hex) */
8223 +#define MOTOR_MACRO_CODE       0x205
8224 +
8225 +#define MOTOR_STEP_SHIFT       4       /* Step res = 2^4 = 10H */
8226 +#define MOTOR_STEP_TIME                20      /* Step time = 50us x 20d = 1ms */
8227 +
8228 +/* VCM start current (mA) */
8229 +#define MOTOR_INFIN_CUR                ((MOTOR_DAC_MAX_CUR / MOTOR_DAC_CODE_MAX) \
8230 +                                * MOTOR_INFIN_CODE + 1)
8231 +/* VCM max current for Macro (mA) */
8232 +#define MOTOR_MACRO_CUR                ((MOTOR_DAC_MAX_CUR / MOTOR_DAC_CODE_MAX) \
8233 +                                * MOTOR_MACRO_CODE + 1)
8234 +
8235 +
8236 +#define MOTOR_DAC_BIT_RES      10
8237 +#define MOTOR_DAC_MAX_CODE     ((1 << MOTOR_DAC_BIT_RES) - 1)
8238 +
8239 +#define MOTOR_STEP_SHIFT       4
8240 +
8241 +#define MAX(x, y)      ((x) > (y) ? (x) : (y))
8242 +#define MIN(x, y)      ((x) < (y) ? (x) : (y))
8243 +
8244 +/* DAC register related define */
8245 +#define MOTOR_PS       (1 << 7) /* power save */
8246 +#define MOTOR_EN       (1 << 6) /* out pin status*/
8247 +#define MOTOR_M                (1 << 3) /* mode select */
8248 +#define MOTOR_W                (1 << 2) /* register address */
8249 +#define MOTOR_F                (1 << 4) /* finish flag */
8250 +
8251 +#define MOTOR_DAC_CODE_L(x)    (x & 0xff)
8252 +#define MOTOR_DAC_CODE_H(x)    ((x >> 8) & 0xf3)
8253 +
8254 +/* Step mode setting */
8255 +#define MOTOR_DAC_CTRL_MODE_0  0xCC
8256 +/* DAC code setting */
8257 +#define MOTOR_DAC_CTRL_MODE_1  0xC8
8258 +
8259 +#define S5K4E1_MOTOR_ADDR      (0x18 >> 1)
8260 +/*#define POWER_EN_PIN 7*/
8261 +#define GPIO_AF_PD     95
8262 +
8263 +#define DEBUG  0
8264 +
8265 +struct s5k4e1_motor{
8266 +       /*struct i2c_client *motor;*/
8267 +       unsigned int infin_cur;
8268 +       unsigned int infin_code;
8269 +       unsigned int macro_cur;
8270 +       unsigned int macro_code;
8271 +       unsigned int max_step;
8272 +       unsigned int cur_code;
8273 +       unsigned int step_res;
8274 +       unsigned int step_time;
8275 +       struct v4l2_subdev sd;
8276 +};
8277 +
8278 +extern int s5k4e1_motor_init(struct i2c_client *client,
8279 +                            struct s5k4e1_motor *config);
8280 +extern int s5k4e1_motor_standby(struct i2c_client *client);
8281 +extern int s5k4e1_motor_wakeup(struct i2c_client *client);
8282 +extern int s5k4e1_motor_set_focus(struct i2c_client *c, unsigned int step,
8283 +                                 struct s5k4e1_motor *config);
8284 +extern int s5k4e1_motor_get_focus(struct i2c_client *c, unsigned int *step,
8285 +                                 struct s5k4e1_motor *config);
8286 +extern int s5k4e1_motor_max_step(struct i2c_client *c, unsigned int *max_code,
8287 +                                struct s5k4e1_motor *config);
8288 --
8289 1.6.0.6
8290