drivers/media/video/mt9m112.c

   1 /*
   2  * Driver for OV5642 CMOS Image Sensor from OmniVision
   3  *
   4  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License version 2 as
   8  * published by the Free Software Foundation.
   9  */
  10
  11 #include <linux/videodev2.h>
  12 #include <linux/slab.h>
  13 #include <linux/i2c.h>
  14 #include <linux/log2.h>
  15 #include <linux/platform_device.h>
  16 #include <linux/delay.h>
  17 #include <linux/circ_buf.h>
  18 #include <linux/hardirq.h>
  19 #include <linux/miscdevice.h>
  20 #include <media/v4l2-common.h>
  21 #include <media/v4l2-chip-ident.h>
  22 #include <media/soc_camera.h>
  23 #include <plat/rk_camera.h>
  24 #include "mt9m112.h"
  25
  26 static int debug;
  27 module_param(debug, int, S_IRUGO|S_IWUSR);
  28
  29 #define dprintk(level, fmt, arg...) do {                        \
  30         if (debug >= level)                                     \
  31         printk(KERN_WARNING fmt , ## arg); } while (0)
  32
  33 #define SENSOR_TR(format, ...) printk(KERN_ERR format, ## __VA_ARGS__)
  34 #define SENSOR_DG(format, ...) dprintk(0, format, ## __VA_ARGS__)
  35
  36 #define _CONS(a,b) a##b
  37 #define CONS(a,b) _CONS(a,b)
  38
  39 #define __STR(x) #x
  40 #define _STR(x) __STR(x)
  41 #define STR(x) _STR(x)
  42
  43 #define MIN(x,y)   ((x<y) ? x: y)
  44 #define MAX(x,y)    ((x>y) ? x: y)
  45
  46 /* Sensor Driver Configuration */
  47 #define SENSOR_NAME mt9m112
  48 #define SENSOR_V4L2_IDENT V4L2_IDENT_MT9M112
  49 #define SENSOR_ID SEQUENCE_END
  50 #define SENSOR_ID_REG SEQUENCE_END
  51 #define SENSOR_RESET_REG SEQUENCE_END
  52 #define SENSOR_RESET_VAL SEQUENCE_END
  53 #define SENSOR_RESET_REG_LEN  WORD_LEN
  54 #define SENSOR_MIN_WIDTH    640
  55 #define SENSOR_MIN_HEIGHT   480
  56 #define SENSOR_MAX_WIDTH    1280
  57 #define SENSOR_MAX_HEIGHT   1024
  58 #define SENSOR_INIT_WIDTH       640                     /* Sensor pixel size for sensor_init_data array */
  59 #define SENSOR_INIT_HEIGHT  480
  60 #define SENSOR_INIT_WINSEQADR sensor_vga
  61 #define SENSOR_INIT_PIXFMT V4L2_PIX_FMT_UYVY
  62
  63 #define CONFIG_SENSOR_WhiteBalance      0
  64 #define CONFIG_SENSOR_Brightness        0
  65 #define CONFIG_SENSOR_Contrast      0
  66 #define CONFIG_SENSOR_Saturation    0
  67 #define CONFIG_SENSOR_Effect        0
  68 #define CONFIG_SENSOR_Scene         0
  69 #define CONFIG_SENSOR_DigitalZoom   0
  70 #define CONFIG_SENSOR_Exposure      0
  71 #define CONFIG_SENSOR_Flash         0
  72 #define CONFIG_SENSOR_Mirror        0
  73 #define CONFIG_SENSOR_Flip          0
  74 #define CONFIG_SENSOR_Focus         0
  75
  76
  77 #define CONFIG_SENSOR_I2C_SPEED     100000       /* Hz */
  78 /* Sensor write register continues by preempt_disable/preempt_enable for current process not be scheduled */
  79 #define CONFIG_SENSOR_I2C_NOSCHED   0
  80 #define CONFIG_SENSOR_I2C_RDWRCHK   0
  81
  82
  83 #define SENSOR_BUS_PARAM  (SOCAM_MASTER | SOCAM_PCLK_SAMPLE_RISING |\
  84                           SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_HIGH |\
  85                           SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8  |SOCAM_MCLK_24MHZ)
  86
  87 #define COLOR_TEMPERATURE_CLOUDY_DN  6500
  88 #define COLOR_TEMPERATURE_CLOUDY_UP    8000
  89 #define COLOR_TEMPERATURE_CLEARDAY_DN  5000
  90 #define COLOR_TEMPERATURE_CLEARDAY_UP    6500
  91 #define COLOR_TEMPERATURE_OFFICE_DN     3500
  92 #define COLOR_TEMPERATURE_OFFICE_UP     5000
  93 #define COLOR_TEMPERATURE_HOME_DN       2500
  94 #define COLOR_TEMPERATURE_HOME_UP       3500
  95
  96 #define SENSOR_NAME_STRING(a) STR(CONS(SENSOR_NAME, a))
  97 #define SENSOR_NAME_VARFUN(a) CONS(SENSOR_NAME, a)
  98
  99 #define SENSOR_AF_IS_ERR    (0x00<<0)
 100 #define SENSOR_AF_IS_OK         (0x01<<0)
 101
 102 #if CONFIG_SENSOR_Focus
 103 #define SENSOR_AF_MODE_INFINITY    0
 104 #define SENSOR_AF_MODE_MACRO       1
 105 #define SENSOR_AF_MODE_FIXED       2
 106 #define SENSOR_AF_MODE_AUTO        3
 107 #define SENSOR_AF_MODE_CONTINUOUS  4
 108 #define SENSOR_AF_MODE_CLOSE       5
 109 #endif
 110
 111 /* init 640X480 VGA */
 112 static struct reginfo sensor_init_data[] =
 113 {
 114         {0xf0, 0x0000},
 115     {0x35, 0x0022},
 116     //0x20, 0x0103,  // mirror
 117     {0x0c, 0x0000},
 118     {0x21, 0x8400},
 119
 120     {0x05,0x022e},
 121    // 0x06,0x0300,
 122
 123     {0xf0, 0x0001},
 124     {0x06,0x708e},
 125     {0x05,0x000e},
 126
 127    { 0xf0, 0x0002},
 128     {0xCB, 0x0001},               // PROGRAM_ADVANCE
 129     {0xCC, 0x0004},               // PROGRAM_SELECT
 130     {0xD2, 0x0000},               // DEFAULT_CONFIG
 131     {0x5B, 0x0001},
 132
 133     {0xf0, 0x0001},
 134
 135     //[Lens Correction 05/16/08 11:19:07]
 136     {0x80, 0x000A }, //LENS_CORRECT_CONTROL
 137     {0x81, 0xED0F }, //LENS_ADJ_VERT_RED_0
 138     {0x82, 0xF8F1 }, //LENS_ADJ_VERT_RED_1_2
 139     {0x83, 0x00FD }, //LENS_ADJ_VERT_RED_3_4
 140     {0x84, 0xEF0D }, //LENS_ADJ_VERT_GREEN_0
 141     {0x85, 0xF9F3 }, //LENS_ADJ_VERT_GREEN_1_2
 142     {0x86, 0x00FD }, //LENS_ADJ_VERT_GREEN_3_4
 143     {0x87, 0xF30C }, //LENS_ADJ_VERT_BLUE_0
 144     {0x88, 0xF9F3 }, //LENS_ADJ_VERT_BLUE_1_2
 145     {0x89, 0x00FD }, //LENS_ADJ_VERT_BLUE_3_4
 146     {0x8A, 0xE616 }, //LENS_ADJ_HORIZ_RED_0
 147     {0x8B, 0xF5F0 }, //LENS_ADJ_HORIZ_RED_1_2
 148     {0x8C, 0xFDF9 }, //LENS_ADJ_HORIZ_RED_3_4
 149     {0x8D, 0x0002 }, //LENS_ADJ_HORIZ_RED_5
 150     {0x8E, 0xE714 }, //LENS_ADJ_HORIZ_GREEN_0
 151     {0x8F, 0xF5F1 }, //LENS_ADJ_HORIZ_GREEN_1_2
 152     {0x90, 0xFEFA }, //LENS_ADJ_HORIZ_GREEN_3_4
 153     {0x91, 0x0001} , //LENS_ADJ_HORIZ_GREEN_5
 154     {0x92, 0xE913} , //LENS_ADJ_HORIZ_BLUE_0
 155     {0x93, 0xF7F2 }, //LENS_ADJ_HORIZ_BLUE_1_2
 156     {0x94, 0xFEFB }, //LENS_ADJ_HORIZ_BLUE_3_4
 157     {0x95, 0x0002} , //LENS_ADJ_HORIZ_BLUE_5
 158     {0xB6, 0x0904} , //LENS_ADJ_VERT_RED_5_6
 159     {0xB7, 0x1E0C }, //LENS_ADJ_VERT_RED_7_8
 160     {0xB8, 0x0904 }, //LENS_ADJ_VERT_GREEN_5_6
 161     {0xB9, 0x1B0A} , //LENS_ADJ_VERT_GREEN_7_8
 162     {0xBA, 0x0704} , //LENS_ADJ_VERT_BLUE_5_6
 163     {0xBB, 0x1A0A} , //LENS_ADJ_VERT_BLUE_7_8
 164     {0xBC, 0x0906 }, //LENS_ADJ_HORIZ_RED_6_7
 165     {0xBD, 0x1410} , //LENS_ADJ_HORIZ_RED_8_9
 166     {0xBE, 0x0026 }, //LENS_ADJ_HORIZ_RED_10
 167     {0xBF, 0x0806}, //LENS_ADJ_HORIZ_GREEN_6_7
 168     {0xC0, 0x110E} , //LENS_ADJ_HORIZ_GREEN_8_9
 169     {0xC1, 0x0024} , //LENS_ADJ_HORIZ_GREEN_10
 170     {0xC2, 0x0A05} , //LENS_ADJ_HORIZ_BLUE_6_7
 171     {0xC3, 0x100F} , //LENS_ADJ_HORIZ_BLUE_8_9
 172     {0xC4, 0x0027} , //LENS_ADJ_HORIZ_BLUE_10
 173     {0x06, 0x640e}, //LENS_CORRECTION
 174
 175     {0x53,0x0602},
 176     {0x54,0x4416},
 177     {0x55,0xb68a},
 178     {0x56,0xe0cf},
 179     {0x57,0xf7e0},
 180     {0x58,0xff00},
 181
 182     //Êä³ö´°¿Ú´óÐ¡
 183     {0xA7, 640},
 184     {0xaa, 512},
 185
 186     //±³¹â³õÊ¼»¯
 187     {0xf0,      0x0002},
 188     {0x37,   0x0080},//0x0300,
 189     {0x2f,   0x9100},
 190     {0x2e,   0x0c44},//0x0c30, //
 191     //0x39, 0x03D2,  // AE Line size Context A d:6A6
 192     //0x3A, 0x03D2,  // AE Line size Context B d:6A6
 193     //0x3B, 0x055B,  // AE shutter delay limit Context A
 194     //0x3C, 0x055B,  // AE shutter delay limit Context B
 195     {0x57, 0x2CA},  // Context A Flicker full frame time (60Hz)
 196     {0x58, 0x2CA},              //0x01E9,  // Context A Flicker full frame time (50Hz)
 197     {0x59, 0x2CA},  // Context B Flicker full frame time (60Hz)
 198     {0x5A, 0x2CA},  //0x01E9,  // Context B Flicker full frame time (50Hz)
 199     {0x5C, 0x0E0A},  // 60Hz Flicker Search Range
 200     {0x5D, 0x120E},  // 50Hz Flicker Search Range
 201     {0x64, 0x5E1C},  // Flicker parameter
 202     {0x5B,   0x0001},
 203
 204         {SEQUENCE_END, 0x00}
 205 };
 206
 207 /* 720p 15fps @ 1280x720 */
 208
 209 static struct reginfo sensor_720p[]=
 210 {
 211         {SEQUENCE_END, 0x00}
 212 };
 213
 214 /*      1080p, 0x15fps, 0xyuv @1920x1080 */
 215
 216 static struct reginfo sensor_1080p[]=
 217 {
 218         {SEQUENCE_END, 0x00}
 219 };
 220
 221 /* 2592X1944 QSXGA */
 222 static struct reginfo sensor_qsxga[] =
 223 {
 224         {SEQUENCE_END, 0x00}
 225 };
 226 /* 2048*1536 QXGA */
 227 static struct reginfo sensor_qxga[] =
 228 {
 229         {SEQUENCE_END, 0x00}
 230 };
 231
 232 /* 1600X1200 UXGA */
 233 static struct reginfo sensor_uxga[] =
 234 {
 235         {SEQUENCE_END, 0x00}
 236 };
 237 /* 1280X1024 SXGA */
 238 static struct reginfo sensor_sxga[] =
 239 {
 240         {SEQUENCE_PROPERTY,SEQUENCE_CAPTURE},
 241         {SEQUENCE_END, 0x00}
 242 };
 243 /*  1024X768 XGA */
 244 static struct reginfo sensor_xga[] =
 245 {
 246         {SEQUENCE_END, 0x00}
 247 };
 248
 249 /* 800X600 SVGA*/
 250 static struct reginfo sensor_svga[] =
 251 {
 252         {SEQUENCE_END, 0x00}
 253 };
 254
 255 /* 640X480 VGA */
 256 static struct reginfo sensor_vga[] =
 257 {
 258         {0xf0, 0x0000},
 259     {0x35, 0x0022},
 260     //0x20, 0x0103,  // mirror
 261     {0x0c, 0x0000},
 262     {0x21, 0x8400},
 263
 264     {0x05,0x022e},
 265    // 0x06,0x0300,
 266
 267     {0xf0, 0x0001},
 268     {0x06,0x708e},
 269     {0x05,0x000e},
 270
 271    { 0xf0, 0x0002},
 272     {0xCB, 0x0001},               // PROGRAM_ADVANCE
 273     {0xCC, 0x0004},               // PROGRAM_SELECT
 274     {0xD2, 0x0000},               // DEFAULT_CONFIG
 275     {0x5B, 0x0001},
 276
 277     {0xf0, 0x0001},
 278
 279     //[Lens Correction 05/16/08 11:19:07]
 280     {0x80, 0x000A }, //LENS_CORRECT_CONTROL
 281     {0x81, 0xED0F }, //LENS_ADJ_VERT_RED_0
 282     {0x82, 0xF8F1 }, //LENS_ADJ_VERT_RED_1_2
 283     {0x83, 0x00FD }, //LENS_ADJ_VERT_RED_3_4
 284     {0x84, 0xEF0D }, //LENS_ADJ_VERT_GREEN_0
 285     {0x85, 0xF9F3 }, //LENS_ADJ_VERT_GREEN_1_2
 286     {0x86, 0x00FD }, //LENS_ADJ_VERT_GREEN_3_4
 287     {0x87, 0xF30C }, //LENS_ADJ_VERT_BLUE_0
 288     {0x88, 0xF9F3 }, //LENS_ADJ_VERT_BLUE_1_2
 289     {0x89, 0x00FD }, //LENS_ADJ_VERT_BLUE_3_4
 290     {0x8A, 0xE616 }, //LENS_ADJ_HORIZ_RED_0
 291     {0x8B, 0xF5F0 }, //LENS_ADJ_HORIZ_RED_1_2
 292     {0x8C, 0xFDF9 }, //LENS_ADJ_HORIZ_RED_3_4
 293     {0x8D, 0x0002 }, //LENS_ADJ_HORIZ_RED_5
 294     {0x8E, 0xE714 }, //LENS_ADJ_HORIZ_GREEN_0
 295     {0x8F, 0xF5F1 }, //LENS_ADJ_HORIZ_GREEN_1_2
 296     {0x90, 0xFEFA }, //LENS_ADJ_HORIZ_GREEN_3_4
 297     {0x91, 0x0001} , //LENS_ADJ_HORIZ_GREEN_5
 298     {0x92, 0xE913} , //LENS_ADJ_HORIZ_BLUE_0
 299     {0x93, 0xF7F2 }, //LENS_ADJ_HORIZ_BLUE_1_2
 300     {0x94, 0xFEFB }, //LENS_ADJ_HORIZ_BLUE_3_4
 301     {0x95, 0x0002} , //LENS_ADJ_HORIZ_BLUE_5
 302     {0xB6, 0x0904} , //LENS_ADJ_VERT_RED_5_6
 303     {0xB7, 0x1E0C }, //LENS_ADJ_VERT_RED_7_8
 304     {0xB8, 0x0904 }, //LENS_ADJ_VERT_GREEN_5_6
 305     {0xB9, 0x1B0A} , //LENS_ADJ_VERT_GREEN_7_8
 306     {0xBA, 0x0704} , //LENS_ADJ_VERT_BLUE_5_6
 307     {0xBB, 0x1A0A} , //LENS_ADJ_VERT_BLUE_7_8
 308     {0xBC, 0x0906 }, //LENS_ADJ_HORIZ_RED_6_7
 309     {0xBD, 0x1410} , //LENS_ADJ_HORIZ_RED_8_9
 310     {0xBE, 0x0026 }, //LENS_ADJ_HORIZ_RED_10
 311     {0xBF, 0x0806}, //LENS_ADJ_HORIZ_GREEN_6_7
 312     {0xC0, 0x110E} , //LENS_ADJ_HORIZ_GREEN_8_9
 313     {0xC1, 0x0024} , //LENS_ADJ_HORIZ_GREEN_10
 314     {0xC2, 0x0A05} , //LENS_ADJ_HORIZ_BLUE_6_7
 315     {0xC3, 0x100F} , //LENS_ADJ_HORIZ_BLUE_8_9
 316     {0xC4, 0x0027} , //LENS_ADJ_HORIZ_BLUE_10
 317     {0x06, 0x640e}, //LENS_CORRECTION
 318
 319     {0x53,0x0602},
 320     {0x54,0x4416},
 321     {0x55,0xb68a},
 322     {0x56,0xe0cf},
 323     {0x57,0xf7e0},
 324     {0x58,0xff00},
 325
 326     //Êä³ö´°¿Ú´óÐ¡
 327     {0xA7, 640},
 328     {0xaa, 512},
 329
 330     //±³¹â³õÊ¼»¯
 331     {0xf0,      0x0002},
 332     {0x37,   0x0080},//0x0300,
 333     {0x2f,   0x9100},
 334     {0x2e,   0x0c44},//0x0c30, //
 335     //0x39, 0x03D2,  // AE Line size Context A d:6A6
 336     //0x3A, 0x03D2,  // AE Line size Context B d:6A6
 337     //0x3B, 0x055B,  // AE shutter delay limit Context A
 338     //0x3C, 0x055B,  // AE shutter delay limit Context B
 339     {0x57, 0x2CA},  // Context A Flicker full frame time (60Hz)
 340     {0x58, 0x2CA},              //0x01E9,  // Context A Flicker full frame time (50Hz)
 341     {0x59, 0x2CA},  // Context B Flicker full frame time (60Hz)
 342     {0x5A, 0x2CA},  //0x01E9,  // Context B Flicker full frame time (50Hz)
 343     {0x5C, 0x0E0A},  // 60Hz Flicker Search Range
 344     {0x5D, 0x120E},  // 50Hz Flicker Search Range
 345     {0x64, 0x5E1C},  // Flicker parameter
 346     {0x5B,   0x0001},
 347     {SEQUENCE_END, 0x00}
 348 };
 349
 350 /* 352X288 CIF */
 351 static struct reginfo sensor_cif[] =
 352 {
 353         {SEQUENCE_END, 0x00}
 354 };
 355
 356 /* 320*240 QVGA */
 357 static  struct reginfo sensor_qvga[] =
 358 {
 359         {SEQUENCE_END, 0x00}
 360 };
 361
 362 /* 176X144 QCIF*/
 363 static struct reginfo sensor_qcif[] =
 364 {
 365         {SEQUENCE_END, 0x00}
 366 };
 367 #if 0
 368 /* 160X120 QQVGA*/
 369 static struct reginfo ov2655_qqvga[] =
 370 {
 371
 372     {0x300E, 0x34},
 373     {0x3011, 0x01},
 374     {0x3012, 0x10},
 375     {0x302a, 0x02},
 376     {0x302b, 0xE6},
 377     {0x306f, 0x14},
 378     {0x3362, 0x90},
 379
 380     {0x3070, 0x5d},
 381     {0x3072, 0x5d},
 382     {0x301c, 0x07},
 383     {0x301d, 0x07},
 384
 385     {0x3020, 0x01},
 386     {0x3021, 0x18},
 387     {0x3022, 0x00},
 388     {0x3023, 0x06},
 389     {0x3024, 0x06},
 390     {0x3025, 0x58},
 391     {0x3026, 0x02},
 392     {0x3027, 0x61},
 393     {0x3088, 0x00},
 394     {0x3089, 0xa0},
 395     {0x308a, 0x00},
 396     {0x308b, 0x78},
 397     {0x3316, 0x64},
 398     {0x3317, 0x25},
 399     {0x3318, 0x80},
 400     {0x3319, 0x08},
 401     {0x331a, 0x0a},
 402     {0x331b, 0x07},
 403     {0x331c, 0x80},
 404     {0x331d, 0x38},
 405     {0x3100, 0x00},
 406     {0x3302, 0x11},
 407
 408     {0x0, 0x0},
 409 };
 410
 411
 412
 413 static  struct reginfo ov2655_Sharpness_auto[] =
 414 {
 415     {0x3306, 0x00},
 416 };
 417
 418 static  struct reginfo ov2655_Sharpness1[] =
 419 {
 420     {0x3306, 0x08},
 421     {0x3371, 0x00},
 422 };
 423
 424 static  struct reginfo ov2655_Sharpness2[][3] =
 425 {
 426     //Sharpness 2
 427     {0x3306, 0x08},
 428     {0x3371, 0x01},
 429 };
 430
 431 static  struct reginfo ov2655_Sharpness3[] =
 432 {
 433     //default
 434     {0x3306, 0x08},
 435     {0x332d, 0x02},
 436 };
 437 static  struct reginfo ov2655_Sharpness4[]=
 438 {
 439     //Sharpness 4
 440     {0x3306, 0x08},
 441     {0x332d, 0x03},
 442 };
 443
 444 static  struct reginfo ov2655_Sharpness5[] =
 445 {
 446     //Sharpness 5
 447     {0x3306, 0x08},
 448     {0x332d, 0x04},
 449 };
 450 #endif
 451 static  struct reginfo sensor_Preview2Capture[]=
 452 {
 453         //capture2preview
 454     {0xf0, 0x0002},
 455     {0xCB, 0x0001},          // PROGRAM_ADVANCE
 456     {0xCC, 0x0004},          // PROGRAM_SELECT
 457     {0xD2, 0x007f},          // DEFAULT_CONFIG
 458         {SEQUENCE_END, 0x00}
 459 };
 460
 461 static  struct reginfo sensor_Capture2Preview[]=
 462 {
 463         //snap2preview
 464     {0xf0, 0x0002},
 465     {0xCB, 0x0001},               // PROGRAM_ADVANCE
 466     {0xCC, 0x0004},               // PROGRAM_SELECT
 467     {0xD2, 0x0000},               // DEFAULT_CONFIG
 468         {SEQUENCE_END, 0x00}
 469 };
 470 static  struct reginfo sensor_ClrFmt_YUYV[]=
 471 {
 472         {SEQUENCE_END, 0x00}
 473 };
 474
 475 static  struct reginfo sensor_ClrFmt_UYVY[]=
 476 {
 477         {SEQUENCE_END, 0x00}
 478 };
 479
 480
 481 #if CONFIG_SENSOR_WhiteBalance
 482 static  struct reginfo sensor_WhiteB_Auto[]=
 483 {
 484         //Auto
 485         {SEQUENCE_END, 0x00}
 486 };
 487 /* Cloudy Colour Temperature : 6500K - 8000K  */
 488 static  struct reginfo sensor_WhiteB_Cloudy[]=
 489 {
 490         {SEQUENCE_END, 0x00}
 491 };
 492 /* ClearDay Colour Temperature : 5000K - 6500K  */
 493 static  struct reginfo sensor_WhiteB_ClearDay[]=
 494 {
 495         {SEQUENCE_END, 0x00}
 496 };
 497 /* Office Colour Temperature : 3500K - 5000K  */
 498 static  struct reginfo sensor_WhiteB_TungstenLamp1[]=
 499 {
 500         {SEQUENCE_END, 0x00}
 501 };
 502 /* Home Colour Temperature : 2500K - 3500K  */
 503 static  struct reginfo sensor_WhiteB_TungstenLamp2[]=
 504 {
 505         {SEQUENCE_END, 0x00}
 506 };
 507 static struct reginfo *sensor_WhiteBalanceSeqe[] = {sensor_WhiteB_Auto, sensor_WhiteB_TungstenLamp1,sensor_WhiteB_TungstenLamp2,
 508     sensor_WhiteB_ClearDay, sensor_WhiteB_Cloudy,NULL,
 509 };
 510 #endif
 511
 512 #if CONFIG_SENSOR_Brightness
 513 static  struct reginfo sensor_Brightness0[]=
 514 {
 515         {SEQUENCE_END, 0x00}
 516 };
 517
 518 static  struct reginfo sensor_Brightness1[]=
 519 {
 520         {SEQUENCE_END, 0x00}
 521 };
 522
 523 static  struct reginfo sensor_Brightness2[]=
 524 {
 525         {SEQUENCE_END, 0x00}
 526 };
 527
 528 static  struct reginfo sensor_Brightness3[]=
 529 {
 530         {SEQUENCE_END, 0x00}
 531 };
 532
 533 static  struct reginfo sensor_Brightness4[]=
 534 {
 535         {SEQUENCE_END, 0x00}
 536 };
 537
 538 static  struct reginfo sensor_Brightness5[]=
 539 {
 540         {SEQUENCE_END, 0x00}
 541 };
 542 static struct reginfo *sensor_BrightnessSeqe[] = {sensor_Brightness0, sensor_Brightness1, sensor_Brightness2, sensor_Brightness3,
 543     sensor_Brightness4, sensor_Brightness5,NULL,
 544 };
 545
 546 #endif
 547
 548 #if CONFIG_SENSOR_Effect
 549 static  struct reginfo sensor_Effect_Normal[] =
 550 {
 551         {SEQUENCE_END, 0x00}
 552 };
 553
 554 static  struct reginfo sensor_Effect_WandB[] =
 555 {
 556         {SEQUENCE_END, 0x00}
 557 };
 558
 559 static  struct reginfo sensor_Effect_Sepia[] =
 560 {
 561         {SEQUENCE_END, 0x00}
 562 };
 563
 564 static  struct reginfo sensor_Effect_Negative[] =
 565 {
 566         {SEQUENCE_END, 0x00}
 567 };
 568 static  struct reginfo sensor_Effect_Bluish[] =
 569 {
 570         {SEQUENCE_END, 0x00}
 571 };
 572
 573 static  struct reginfo sensor_Effect_Green[] =
 574 {
 575         {SEQUENCE_END, 0x00}
 576 };
 577
 578 static struct reginfo sensor_Effect_Solarize[] =
 579 {
 580         {SEQUENCE_END, 0x00}
 581 };
 582 static struct reginfo *sensor_EffectSeqe[] = {sensor_Effect_Normal, sensor_Effect_Negative,sensor_Effect_Sepia,
 583     sensor_Effect_Solarize,NULL,
 584 };
 585 #endif
 586 #if CONFIG_SENSOR_Exposure
 587 static  struct reginfo sensor_Exposure0[]=
 588 {
 589         {SEQUENCE_END, 0x00}
 590 };
 591
 592 static  struct reginfo sensor_Exposure1[]=
 593 {
 594         {SEQUENCE_END, 0x00}
 595 };
 596
 597 static  struct reginfo sensor_Exposure2[]=
 598 {
 599         {SEQUENCE_END, 0x00}
 600 };
 601
 602 static  struct reginfo sensor_Exposure3[]=
 603 {
 604         {SEQUENCE_END, 0x00}
 605 };
 606
 607 static  struct reginfo sensor_Exposure4[]=
 608 {
 609         {SEQUENCE_END, 0x00}
 610 };
 611
 612 static  struct reginfo sensor_Exposure5[]=
 613 {
 614         {SEQUENCE_END, 0x00}
 615 };
 616
 617 static  struct reginfo sensor_Exposure6[]=
 618 {
 619         {SEQUENCE_END, 0x00}
 620 };
 621
 622 static struct reginfo *sensor_ExposureSeqe[] = {sensor_Exposure0, sensor_Exposure1, sensor_Exposure2, sensor_Exposure3,
 623     sensor_Exposure4, sensor_Exposure5,sensor_Exposure6,NULL,
 624 };
 625 #endif
 626 #if CONFIG_SENSOR_Saturation
 627 static  struct reginfo sensor_Saturation0[]=
 628 {
 629         {SEQUENCE_END, 0x00}
 630 };
 631
 632 static  struct reginfo sensor_Saturation1[]=
 633 {
 634         {SEQUENCE_END, 0x00}
 635 };
 636
 637 static  struct reginfo sensor_Saturation2[]=
 638 {
 639         {SEQUENCE_END, 0x00}
 640 };
 641 static struct reginfo *sensor_SaturationSeqe[] = {sensor_Saturation0, sensor_Saturation1, sensor_Saturation2, NULL,};
 642
 643 #endif
 644 #if CONFIG_SENSOR_Contrast
 645 static  struct reginfo sensor_Contrast0[]=
 646 {
 647         {SEQUENCE_END, 0x00}
 648 };
 649
 650 static  struct reginfo sensor_Contrast1[]=
 651 {
 652         {SEQUENCE_END, 0x00}
 653 };
 654
 655 static  struct reginfo sensor_Contrast2[]=
 656 {
 657         {SEQUENCE_END, 0x00}
 658 };
 659
 660 static  struct reginfo sensor_Contrast3[]=
 661 {
 662         {SEQUENCE_END, 0x00}
 663 };
 664
 665 static  struct reginfo sensor_Contrast4[]=
 666 {
 667         {SEQUENCE_END, 0x00}
 668 };
 669
 670
 671 static  struct reginfo sensor_Contrast5[]=
 672 {
 673         {SEQUENCE_END, 0x00}
 674 };
 675
 676 static  struct reginfo sensor_Contrast6[]=
 677 {
 678         {SEQUENCE_END, 0x00}
 679 };
 680 static struct reginfo *sensor_ContrastSeqe[] = {sensor_Contrast0, sensor_Contrast1, sensor_Contrast2, sensor_Contrast3,
 681     sensor_Contrast4, sensor_Contrast5, sensor_Contrast6, NULL,
 682 };
 683
 684 #endif
 685 #if CONFIG_SENSOR_Mirror
 686 static  struct reginfo sensor_MirrorOn[]=
 687 {
 688         {SEQUENCE_END, 0x00}
 689 };
 690
 691 static  struct reginfo sensor_MirrorOff[]=
 692 {
 693         {SEQUENCE_END, 0x00}
 694 };
 695 static struct reginfo *sensor_MirrorSeqe[] = {sensor_MirrorOff, sensor_MirrorOn,NULL,};
 696 #endif
 697 #if CONFIG_SENSOR_Flip
 698 static  struct reginfo sensor_FlipOn[]=
 699 {
 700         {SEQUENCE_END, 0x00}
 701 };
 702
 703 static  struct reginfo sensor_FlipOff[]=
 704 {
 705         {SEQUENCE_END, 0x00}
 706 };
 707 static struct reginfo *sensor_FlipSeqe[] = {sensor_FlipOff, sensor_FlipOn,NULL,};
 708
 709 #endif
 710 #if CONFIG_SENSOR_Scene
 711 static  struct reginfo sensor_SceneAuto[] =
 712 {
 713         {SEQUENCE_END, 0x00}
 714 };
 715
 716 static  struct reginfo sensor_SceneNight[] =
 717 {
 718         {SEQUENCE_END, 0x00}
 719 };
 720 static struct reginfo *sensor_SceneSeqe[] = {sensor_SceneAuto, sensor_SceneNight,NULL,};
 721
 722 #endif
 723 #if CONFIG_SENSOR_DigitalZoom
 724 static struct reginfo sensor_Zoom0[] =
 725 {
 726         {SEQUENCE_END, 0x00}
 727 };
 728
 729 static struct reginfo sensor_Zoom1[] =
 730 {
 731         {SEQUENCE_END, 0x00}
 732 };
 733
 734 static struct reginfo sensor_Zoom2[] =
 735 {
 736         {SEQUENCE_END, 0x00}
 737 };
 738
 739
 740 static struct reginfo sensor_Zoom3[] =
 741 {
 742         {SEQUENCE_END, 0x00}
 743 };
 744 static struct reginfo *sensor_ZoomSeqe[] = {sensor_Zoom0, sensor_Zoom1, sensor_Zoom2, sensor_Zoom3, NULL};
 745 #endif
 746 static const struct v4l2_querymenu sensor_menus[] =
 747 {
 748         #if CONFIG_SENSOR_WhiteBalance
 749     { .id = V4L2_CID_DO_WHITE_BALANCE,  .index = 0,  .name = "auto",  .reserved = 0, }, {  .id = V4L2_CID_DO_WHITE_BALANCE,  .index = 1, .name = "incandescent",  .reserved = 0,},
 750     { .id = V4L2_CID_DO_WHITE_BALANCE,  .index = 2,  .name = "fluorescent", .reserved = 0,}, {  .id = V4L2_CID_DO_WHITE_BALANCE, .index = 3,  .name = "daylight", .reserved = 0,},
 751     { .id = V4L2_CID_DO_WHITE_BALANCE,  .index = 4,  .name = "cloudy-daylight", .reserved = 0,},
 752     #endif
 753
 754         #if CONFIG_SENSOR_Effect
 755     { .id = V4L2_CID_EFFECT,  .index = 0,  .name = "none",  .reserved = 0, }, {  .id = V4L2_CID_EFFECT,  .index = 1, .name = "negative",  .reserved = 0,},
 756     { .id = V4L2_CID_EFFECT,  .index = 2,  .name = "sepia", .reserved = 0,}, {  .id = V4L2_CID_EFFECT, .index = 3,  .name = "solarize", .reserved = 0,},
 757     #endif
 758
 759         #if CONFIG_SENSOR_Scene
 760     { .id = V4L2_CID_SCENE,  .index = 0, .name = "auto", .reserved = 0,} ,{ .id = V4L2_CID_SCENE,  .index = 1,  .name = "night", .reserved = 0,},
 761     #endif
 762
 763         #if CONFIG_SENSOR_Flash
 764     { .id = V4L2_CID_FLASH,  .index = 0,  .name = "off",  .reserved = 0, }, {  .id = V4L2_CID_FLASH,  .index = 1, .name = "auto",  .reserved = 0,},
 765     { .id = V4L2_CID_FLASH,  .index = 2,  .name = "on", .reserved = 0,}, {  .id = V4L2_CID_FLASH, .index = 3,  .name = "torch", .reserved = 0,},
 766     #endif
 767 };
 768
 769 static const struct v4l2_queryctrl sensor_controls[] =
 770 {
 771         #if CONFIG_SENSOR_WhiteBalance
 772     {
 773         .id             = V4L2_CID_DO_WHITE_BALANCE,
 774         .type           = V4L2_CTRL_TYPE_MENU,
 775         .name           = "White Balance Control",
 776         .minimum        = 0,
 777         .maximum        = 4,
 778         .step           = 1,
 779         .default_value = 0,
 780     },
 781     #endif
 782
 783         #if CONFIG_SENSOR_Brightness
 784         {
 785         .id             = V4L2_CID_BRIGHTNESS,
 786         .type           = V4L2_CTRL_TYPE_INTEGER,
 787         .name           = "Brightness Control",
 788         .minimum        = -3,
 789         .maximum        = 2,
 790         .step           = 1,
 791         .default_value = 0,
 792     },
 793     #endif
 794
 795         #if CONFIG_SENSOR_Effect
 796         {
 797         .id             = V4L2_CID_EFFECT,
 798         .type           = V4L2_CTRL_TYPE_MENU,
 799         .name           = "Effect Control",
 800         .minimum        = 0,
 801         .maximum        = 3,
 802         .step           = 1,
 803         .default_value = 0,
 804     },
 805         #endif
 806
 807         #if CONFIG_SENSOR_Exposure
 808         {
 809         .id             = V4L2_CID_EXPOSURE,
 810         .type           = V4L2_CTRL_TYPE_INTEGER,
 811         .name           = "Exposure Control",
 812         .minimum        = 0,
 813         .maximum        = 6,
 814         .step           = 1,
 815         .default_value = 0,
 816     },
 817         #endif
 818
 819         #if CONFIG_SENSOR_Saturation
 820         {
 821         .id             = V4L2_CID_SATURATION,
 822         .type           = V4L2_CTRL_TYPE_INTEGER,
 823         .name           = "Saturation Control",
 824         .minimum        = 0,
 825         .maximum        = 2,
 826         .step           = 1,
 827         .default_value = 0,
 828     },
 829     #endif
 830
 831         #if CONFIG_SENSOR_Contrast
 832         {
 833         .id             = V4L2_CID_CONTRAST,
 834         .type           = V4L2_CTRL_TYPE_INTEGER,
 835         .name           = "Contrast Control",
 836         .minimum        = -3,
 837         .maximum        = 3,
 838         .step           = 1,
 839         .default_value = 0,
 840     },
 841         #endif
 842
 843         #if CONFIG_SENSOR_Mirror
 844         {
 845         .id             = V4L2_CID_HFLIP,
 846         .type           = V4L2_CTRL_TYPE_BOOLEAN,
 847         .name           = "Mirror Control",
 848         .minimum        = 0,
 849         .maximum        = 1,
 850         .step           = 1,
 851         .default_value = 1,
 852     },
 853     #endif
 854
 855         #if CONFIG_SENSOR_Flip
 856         {
 857         .id             = V4L2_CID_VFLIP,
 858         .type           = V4L2_CTRL_TYPE_BOOLEAN,
 859         .name           = "Flip Control",
 860         .minimum        = 0,
 861         .maximum        = 1,
 862         .step           = 1,
 863         .default_value = 1,
 864     },
 865     #endif
 866
 867         #if CONFIG_SENSOR_Scene
 868     {
 869         .id             = V4L2_CID_SCENE,
 870         .type           = V4L2_CTRL_TYPE_MENU,
 871         .name           = "Scene Control",
 872         .minimum        = 0,
 873         .maximum        = 1,
 874         .step           = 1,
 875         .default_value = 0,
 876     },
 877     #endif
 878
 879         #if CONFIG_SENSOR_DigitalZoom
 880     {
 881         .id             = V4L2_CID_ZOOM_RELATIVE,
 882         .type           = V4L2_CTRL_TYPE_INTEGER,
 883         .name           = "DigitalZoom Control",
 884         .minimum        = -1,
 885         .maximum        = 1,
 886         .step           = 1,
 887         .default_value = 0,
 888     }, {
 889         .id             = V4L2_CID_ZOOM_ABSOLUTE,
 890         .type           = V4L2_CTRL_TYPE_INTEGER,
 891         .name           = "DigitalZoom Control",
 892         .minimum        = 0,
 893         .maximum        = 3,
 894         .step           = 1,
 895         .default_value = 0,
 896     },
 897     #endif
 898
 899         #if CONFIG_SENSOR_Focus
 900         {
 901         .id             = V4L2_CID_FOCUS_RELATIVE,
 902         .type           = V4L2_CTRL_TYPE_INTEGER,
 903         .name           = "Focus Control",
 904         .minimum        = -1,
 905         .maximum        = 1,
 906         .step           = 1,
 907         .default_value = 0,
 908     }, {
 909         .id             = V4L2_CID_FOCUS_ABSOLUTE,
 910         .type           = V4L2_CTRL_TYPE_INTEGER,
 911         .name           = "Focus Control",
 912         .minimum        = 0,
 913         .maximum        = 255,
 914         .step           = 1,
 915         .default_value = 125,
 916     },
 917         {
 918         .id             = V4L2_CID_FOCUS_AUTO,
 919         .type           = V4L2_CTRL_TYPE_BOOLEAN,
 920         .name           = "Focus Control",
 921         .minimum        = 0,
 922         .maximum        = 1,
 923         .step           = 1,
 924         .default_value = 0,
 925     },{
 926         .id             = V4L2_CID_FOCUS_CONTINUOUS,
 927         .type           = V4L2_CTRL_TYPE_BOOLEAN,
 928         .name           = "Focus Control",
 929         .minimum        = 0,
 930         .maximum        = 1,
 931         .step           = 1,
 932         .default_value = 0,
 933     },
 934     #endif
 935
 936         #if CONFIG_SENSOR_Flash
 937         {
 938         .id             = V4L2_CID_FLASH,
 939         .type           = V4L2_CTRL_TYPE_MENU,
 940         .name           = "Flash Control",
 941         .minimum        = 0,
 942         .maximum        = 3,
 943         .step           = 1,
 944         .default_value = 0,
 945     },
 946         #endif
 947 };
 948
 949 static int sensor_probe(struct i2c_client *client, const struct i2c_device_id *did);
 950 static int sensor_video_probe(struct soc_camera_device *icd, struct i2c_client *client);
 951 static int sensor_g_control(struct v4l2_subdev *sd, struct v4l2_control *ctrl);
 952 static int sensor_s_control(struct v4l2_subdev *sd, struct v4l2_control *ctrl);
 953 static int sensor_g_ext_controls(struct v4l2_subdev *sd,  struct v4l2_ext_controls *ext_ctrl);
 954 static int sensor_s_ext_controls(struct v4l2_subdev *sd,  struct v4l2_ext_controls *ext_ctrl);
 955 static int sensor_suspend(struct soc_camera_device *icd, pm_message_t pm_msg);
 956 static int sensor_resume(struct soc_camera_device *icd);
 957 static int sensor_set_bus_param(struct soc_camera_device *icd,unsigned long flags);
 958 static unsigned long sensor_query_bus_param(struct soc_camera_device *icd);
 959 static int sensor_set_effect(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value);
 960 static int sensor_set_whiteBalance(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value);
 961 static int sensor_deactivate(struct i2c_client *client);
 962
 963 static struct soc_camera_ops sensor_ops =
 964 {
 965     .suspend                     = sensor_suspend,
 966     .resume                       = sensor_resume,
 967     .set_bus_param              = sensor_set_bus_param,
 968     .query_bus_param    = sensor_query_bus_param,
 969     .controls           = sensor_controls,
 970     .menus                         = sensor_menus,
 971     .num_controls               = ARRAY_SIZE(sensor_controls),
 972     .num_menus          = ARRAY_SIZE(sensor_menus),
 973 };
 974
 975 #define COL_FMT(_name, _depth, _fourcc, _colorspace) \
 976         { .name = _name, .depth = _depth, .fourcc = _fourcc, \
 977         .colorspace = _colorspace }
 978
 979 #define JPG_FMT(_name, _depth, _fourcc) \
 980         COL_FMT(_name, _depth, _fourcc, V4L2_COLORSPACE_JPEG)
 981
 982 static const struct soc_camera_data_format sensor_colour_formats[] = {
 983         JPG_FMT(SENSOR_NAME_STRING(UYVY), 16, V4L2_PIX_FMT_UYVY),
 984         JPG_FMT(SENSOR_NAME_STRING(YUYV), 16, V4L2_PIX_FMT_YUYV),
 985 };
 986 enum sensor_work_state
 987 {
 988         sensor_work_ready = 0,
 989         sensor_working,
 990 };
 991 struct sensor_work
 992 {
 993         struct i2c_client *client;
 994         struct delayed_work dwork;
 995         enum sensor_work_state state;
 996 };
 997
 998 typedef struct sensor_info_priv_s
 999 {
1000     int whiteBalance;
1001     int brightness;
1002     int contrast;
1003     int saturation;
1004     int effect;
1005     int scene;
1006     int digitalzoom;
1007     int focus;
1008         int auto_focus;
1009         int affm_reinit;
1010     int flash;
1011     int exposure;
1012     unsigned char mirror;                                        /* HFLIP */
1013     unsigned char flip;                                          /* VFLIP */
1014         bool snap2preview;
1015         bool video2preview;
1016         int capture_w;
1017         int capture_h;
1018         int preview_w;
1019         int preview_h;
1020     struct reginfo *winseqe_cur_addr;
1021         unsigned int pixfmt;
1022         unsigned int enable;
1023         unsigned int funmodule_state;
1024 } sensor_info_priv_t;
1025
1026
1027
1028 struct sensor_parameter
1029 {
1030         unsigned short int preview_maxlines;
1031         unsigned short int preview_exposure;
1032         unsigned short int preview_line_width;
1033         unsigned short int preview_gain;
1034
1035         unsigned short int capture_framerate;
1036         unsigned short int preview_framerate;
1037 };
1038
1039 struct sensor
1040 {
1041     struct v4l2_subdev subdev;
1042     struct i2c_client *client;
1043     sensor_info_priv_t info_priv;
1044         struct sensor_parameter parameter;
1045         struct workqueue_struct *sensor_wq;
1046         struct sensor_work sensor_wk;
1047         struct mutex wq_lock;
1048     int model;  /* V4L2_IDENT_OV* codes from v4l2-chip-ident.h */
1049 #if CONFIG_SENSOR_I2C_NOSCHED
1050         atomic_t tasklock_cnt;
1051 #endif
1052         struct rk29camera_platform_data *sensor_io_request;
1053 };
1054
1055 static struct sensor* to_sensor(const struct i2c_client *client)
1056 {
1057     return container_of(i2c_get_clientdata(client), struct sensor, subdev);
1058 }
1059
1060 static int sensor_task_lock(struct i2c_client *client, int lock)
1061 {
1062 #if CONFIG_SENSOR_I2C_NOSCHED
1063         int cnt = 3;
1064     struct sensor *sensor = to_sensor(client);
1065
1066         if (lock) {
1067                 if (atomic_read(&sensor->tasklock_cnt) == 0) {
1068                         while ((atomic_read(&client->adapter->bus_lock.count) < 1) && (cnt>0)) {
1069                                 SENSOR_TR("\n %s will obtain i2c in atomic, but i2c bus is locked! Wait...\n",SENSOR_NAME_STRING());
1070                                 msleep(35);
1071                                 cnt--;
1072                         }
1073                         if ((atomic_read(&client->adapter->bus_lock.count) < 1) && (cnt<=0)) {
1074                                 SENSOR_TR("\n %s obtain i2c fail in atomic!!\n",SENSOR_NAME_STRING());
1075                                 goto sensor_task_lock_err;
1076                         }
1077                         preempt_disable();
1078                 }
1079
1080                 atomic_add(1, &sensor->tasklock_cnt);
1081         } else {
1082                 if (atomic_read(&sensor->tasklock_cnt) > 0) {
1083                         atomic_sub(1, &sensor->tasklock_cnt);
1084
1085                         if (atomic_read(&sensor->tasklock_cnt) == 0)
1086                                 preempt_enable();
1087                 }
1088         }
1089 #endif
1090         return 0;
1091 sensor_task_lock_err:
1092         return -1;
1093 }
1094
1095 /* sensor register write */
1096 static int sensor_write(struct i2c_client *client, struct reginfo *reg_info)
1097 {
1098     int err=0,cnt;
1099     u8 buf[3];
1100     struct i2c_msg msg[1];
1101
1102         switch (reg_info->reg)
1103         {
1104                 case SEQUENCE_WAIT_MS:
1105                 {
1106                         if (in_atomic())
1107                                 mdelay(reg_info->val);
1108                         else
1109                                 msleep(reg_info->val);
1110                         break;
1111                 }
1112
1113                 case SEQUENCE_WAIT_US:
1114                 {
1115                         udelay(reg_info->val);
1116                         break;
1117                 }
1118
1119                 case SEQUENCE_PROPERTY:
1120                 {
1121                         break;
1122                 }
1123                 default:
1124                 {
1125                     buf[0] = reg_info->reg & 0xFF;
1126                         buf[1] = reg_info->val >> 8;
1127                         buf[2] = reg_info->val & 0xFF;
1128                         msg->len = 3;
1129                     msg->addr = client->addr;
1130                     msg->flags = client->flags;
1131                     msg->buf = buf;
1132                     msg->scl_rate = CONFIG_SENSOR_I2C_SPEED;         /* ddl@rock-chips.com : 100kHz */
1133                     msg->read_type = 0;               /* fpga i2c:0==I2C_NORMAL : direct use number not enum for don't want include spi_fpga.h */
1134
1135                     cnt = 3;
1136                     err = -EAGAIN;
1137
1138                     while ((cnt-- > 0) && (err < 0)) {                       /* ddl@rock-chips.com :  Transfer again if transent is failed   */
1139                         err = i2c_transfer(client->adapter, msg, 1);
1140
1141                         if (err >= 0) {
1142                             return 0;
1143                         } else {
1144                             SENSOR_TR("\n %s write reg(0x%x, val:0x%x) failed, try to write again!\n",SENSOR_NAME_STRING(),reg_info->reg, reg_info->val);
1145                             udelay(10);
1146                         }
1147                     }
1148                 }
1149         }
1150     return err;
1151 }
1152
1153 /* sensor register read */
1154 static int sensor_read(struct i2c_client *client, u16 reg, u16 *val)
1155 {
1156     int err,cnt;
1157     u8 buf[1];
1158     struct i2c_msg msg[2];
1159
1160     buf[0] = reg & 0xFF;
1161
1162     msg[0].addr = client->addr;
1163     msg[0].flags = client->flags;
1164     msg[0].buf = buf;
1165     msg[0].len = sizeof(buf);
1166     msg[0].scl_rate = CONFIG_SENSOR_I2C_SPEED;       /* ddl@rock-chips.com : 100kHz */
1167     msg[0].read_type = 2;   /* fpga i2c:0==I2C_NO_STOP : direct use number not enum for don't want include spi_fpga.h */
1168
1169     msg[1].addr = client->addr;
1170     msg[1].flags = client->flags|I2C_M_RD;
1171     msg[1].buf = buf;
1172     msg[1].len = 1;
1173     msg[1].scl_rate = CONFIG_SENSOR_I2C_SPEED;                       /* ddl@rock-chips.com : 100kHz */
1174     msg[1].read_type = 2;                             /* fpga i2c:0==I2C_NO_STOP : direct use number not enum for don't want include spi_fpga.h */
1175
1176     cnt = 3;
1177     err = -EAGAIN;
1178     while ((cnt-- > 0) && (err < 0)) {                       /* ddl@rock-chips.com :  Transfer again if transent is failed   */
1179         err = i2c_transfer(client->adapter, msg, 2);
1180
1181         if (err >= 0) {
1182             *val = buf[0];
1183             return 0;
1184         } else {
1185                 SENSOR_TR("\n %s read reg(0x%x val:0x%x) failed, try to read again! \n",SENSOR_NAME_STRING(),reg, *val);
1186             udelay(10);
1187         }
1188     }
1189
1190     return err;
1191 }
1192
1193 /* write a array of registers  */
1194 static int sensor_write_array(struct i2c_client *client, struct reginfo *regarray)
1195 {
1196     int err = 0, cnt;
1197     int i = 0;
1198 #if CONFIG_SENSOR_I2C_RDWRCHK
1199         char valchk;
1200 #endif
1201
1202         cnt = 0;
1203         if (sensor_task_lock(client, 1) < 0)
1204                 goto sensor_write_array_end;
1205     while (regarray[i].reg != SEQUENCE_END) {
1206         err = sensor_write(client, &regarray[i]);
1207         if (err < 0)
1208         {
1209             if (cnt-- > 0) {
1210                             SENSOR_TR("%s..write failed current reg:0x%x, Write array again !\n", SENSOR_NAME_STRING(),regarray[i].reg);
1211                                 i = 0;
1212                                 continue;
1213             } else {
1214                 SENSOR_TR("%s..write array failed!!!\n", SENSOR_NAME_STRING());
1215                 err = -EPERM;
1216                                 goto sensor_write_array_end;
1217             }
1218         } else {
1219         #if CONFIG_SENSOR_I2C_RDWRCHK
1220                         sensor_read(client, regarray[i].reg, &valchk);
1221                         if (valchk != regarray[i].val)
1222                                 SENSOR_TR("%s Reg:0x%x write(0x%x, 0x%x) fail\n",SENSOR_NAME_STRING(), regarray[i].reg, regarray[i].val, valchk);
1223                 #endif
1224         }
1225
1226         i++;
1227     }
1228
1229 sensor_write_array_end:
1230         sensor_task_lock(client,0);
1231     return err;
1232 }
1233 #if CONFIG_SENSOR_I2C_RDWRCHK
1234 static int sensor_readchk_array(struct i2c_client *client, struct reginfo *regarray)
1235 {
1236     int cnt;
1237     int i = 0;
1238         char valchk;
1239
1240         cnt = 0;
1241         valchk = 0;
1242     while (regarray[i].reg != SEQUENCE_END)
1243     {
1244                 sensor_read(client, regarray[i].reg, &valchk);
1245                 if (valchk != regarray[i].val)
1246                         SENSOR_TR("%s Reg:0x%x read(0x%x, 0x%x) error\n",SENSOR_NAME_STRING(), regarray[i].reg, regarray[i].val, valchk);
1247
1248         i++;
1249     }
1250     return 0;
1251 }
1252 #endif
1253 #if CONFIG_SENSOR_Focus
1254 static struct reginfo sensor_af_init0[] =
1255 {
1256         {SEQUENCE_END, 0x00}
1257 };
1258 static struct reginfo sensor_af_init1[] =
1259 {
1260         {SEQUENCE_END, 0x00}
1261 };
1262
1263
1264 static struct reginfo sensor_af_trigger[] =
1265 {
1266         {SEQUENCE_END, 0x00}
1267 };
1268 static int sensor_af_single(struct i2c_client *client)
1269 {
1270         int ret = 0;
1271
1272         ret = sensor_write_array(client, sensor_af_trigger);
1273         if (ret<0)
1274                 SENSOR_TR("%s sensor auto focus trigger fail!!\n",SENSOR_NAME_STRING());
1275         else
1276                 SENSOR_DG("%s sensor auto focus trigger success!\n",SENSOR_NAME_STRING());
1277 sensor_af_single_end:
1278         return ret;
1279 }
1280
1281 static int sensor_af_const(struct i2c_client *client)
1282 {
1283         int ret = 0;
1284
1285 sensor_af_const_end:
1286         return ret;
1287 }
1288
1289 static int sensor_af_zoneupdate(struct i2c_client *client)
1290 {
1291         int ret = 0;
1292
1293 sensor_af_zoneupdate_end:
1294         return ret;
1295 }
1296
1297 static int sensor_af_init(struct i2c_client *client)
1298 {
1299         int ret = 0;
1300
1301         ret = sensor_write_array(client, sensor_af_init0);
1302         if (ret<0) {
1303                 SENSOR_TR("%s sensor auto focus init_0 fail!!",SENSOR_NAME_STRING());
1304         } else {
1305                 if (sensor_af_zoneupdate(client) == 0) {
1306                         ret = sensor_write_array(client, sensor_af_init1);
1307                         if (ret<0) {
1308                                 SENSOR_TR("%s sensor auto focus init_1 fail!!",SENSOR_NAME_STRING());
1309                         }
1310                 }
1311         }
1312
1313         return ret;
1314 }
1315 #endif
1316
1317 static int sensor_ioctrl(struct soc_camera_device *icd,enum rk29sensor_power_cmd cmd, int on)
1318 {
1319         struct soc_camera_link *icl = to_soc_camera_link(icd);
1320         int ret = 0;
1321
1322
1323         switch (cmd)
1324         {
1325                 case Sensor_PowerDown:
1326                 {
1327                         if (icl->powerdown) {
1328                                 ret = icl->powerdown(icd->pdev, on);
1329                                 if (ret == RK29_CAM_IO_SUCCESS) {
1330                                         if (on == 0) {
1331                                                 mdelay(2);
1332                                                 if (icl->reset)
1333                                                         icl->reset(icd->pdev);
1334                                         }
1335                                 } else if (ret == RK29_CAM_EIO_REQUESTFAIL) {
1336                                         ret = -ENODEV;
1337                                         goto sensor_power_end;
1338                                 }
1339                         }
1340                         break;
1341                 }
1342                 case Sensor_Flash:
1343                 {
1344                         struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1345                 struct sensor *sensor = to_sensor(client);
1346
1347                         if (sensor->sensor_io_request && sensor->sensor_io_request->sensor_ioctrl) {
1348                                 sensor->sensor_io_request->sensor_ioctrl(icd->pdev,Cam_Flash, on);
1349                         }
1350                         break;
1351                 }
1352                 default:
1353                 {
1354                         SENSOR_TR("%s power cmd(0x%x) is unknown!",SENSOR_NAME_STRING(),cmd);
1355                         break;
1356                 }
1357         }
1358
1359 sensor_power_end:
1360         return ret;
1361 }
1362 static int sensor_init(struct v4l2_subdev *sd, u32 val)
1363 {
1364     struct i2c_client *client = sd->priv;
1365     struct soc_camera_device *icd = client->dev.platform_data;
1366     struct sensor *sensor = to_sensor(client);
1367         const struct v4l2_queryctrl *qctrl;
1368     int ret,pid = 0;
1369
1370     SENSOR_DG("\n%s..%s.. \n",SENSOR_NAME_STRING(),__FUNCTION__);
1371
1372         if (sensor_ioctrl(icd, Sensor_PowerDown, 0) < 0) {
1373                 ret = -ENODEV;
1374                 goto sensor_INIT_ERR;
1375         }
1376
1377     /* soft reset */
1378         if (sensor_task_lock(client,1)<0)
1379                 goto sensor_INIT_ERR;
1380
1381 #if (SENSOR_RESET_REG != SEQUENCE_END)
1382     struct reginfo reg_info;
1383         reg_info.reg = SENSOR_RESET_REG;
1384         reg_info.val = SENSOR_RESET_VAL;
1385         reg_info.reg_len = SENSOR_RESET_REG_LEN;
1386     ret = sensor_write(client, &reg_info);
1387     if (ret != 0) {
1388         SENSOR_TR("%s soft reset sensor failed\n",SENSOR_NAME_STRING());
1389         ret = -ENODEV;
1390                 goto sensor_INIT_ERR;
1391     }
1392
1393     mdelay(5);  //delay 5 microseconds
1394 #endif
1395
1396         /* check if it is an sensor sensor */
1397 #if (SENSOR_ID_REG != SEQUENCE_END)
1398     ret = sensor_read(client, SENSOR_ID_REG, &pid);
1399     if (ret != 0) {
1400         SENSOR_TR("read chip id failed\n");
1401         ret = -ENODEV;
1402         goto sensor_INIT_ERR;
1403     }
1404
1405     SENSOR_DG("\n %s  pid = 0x%x \n", SENSOR_NAME_STRING(), pid);
1406 #else
1407         pid = SENSOR_ID;
1408 #endif
1409     if (pid == SENSOR_ID) {
1410         sensor->model = SENSOR_V4L2_IDENT;
1411     } else {
1412         SENSOR_TR("error: %s mismatched   pid = 0x%x\n", SENSOR_NAME_STRING(), pid);
1413         ret = -ENODEV;
1414         goto sensor_INIT_ERR;
1415     }
1416
1417     ret = sensor_write_array(client, sensor_init_data);
1418     if (ret != 0)
1419     {
1420         SENSOR_TR("error: %s initial failed\n",SENSOR_NAME_STRING());
1421         goto sensor_INIT_ERR;
1422     }
1423         sensor_task_lock(client,0);
1424     sensor->info_priv.preview_w = SENSOR_INIT_WIDTH;
1425     sensor->info_priv.preview_h = SENSOR_INIT_HEIGHT;
1426     sensor->info_priv.capture_w = SENSOR_MAX_WIDTH;
1427     sensor->info_priv.capture_h = SENSOR_MAX_HEIGHT;
1428     sensor->info_priv.winseqe_cur_addr  = SENSOR_INIT_WINSEQADR;
1429         sensor->info_priv.pixfmt = SENSOR_INIT_PIXFMT;
1430
1431     /* sensor sensor information for initialization  */
1432         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_DO_WHITE_BALANCE);
1433         if (qctrl)
1434         sensor->info_priv.whiteBalance = qctrl->default_value;
1435         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_BRIGHTNESS);
1436         if (qctrl)
1437         sensor->info_priv.brightness = qctrl->default_value;
1438         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_EFFECT);
1439         if (qctrl)
1440         sensor->info_priv.effect = qctrl->default_value;
1441         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_EXPOSURE);
1442         if (qctrl)
1443         sensor->info_priv.exposure = qctrl->default_value;
1444
1445         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_SATURATION);
1446         if (qctrl)
1447         sensor->info_priv.saturation = qctrl->default_value;
1448         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_CONTRAST);
1449         if (qctrl)
1450         sensor->info_priv.contrast = qctrl->default_value;
1451         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_HFLIP);
1452         if (qctrl)
1453         sensor->info_priv.mirror = qctrl->default_value;
1454         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_VFLIP);
1455         if (qctrl)
1456         sensor->info_priv.flip = qctrl->default_value;
1457         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_SCENE);
1458         if (qctrl)
1459         sensor->info_priv.scene = qctrl->default_value;
1460         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_ZOOM_ABSOLUTE);
1461         if (qctrl)
1462         sensor->info_priv.digitalzoom = qctrl->default_value;
1463
1464     /* ddl@rock-chips.com : if sensor support auto focus and flash, programer must run focus and flash code  */
1465         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_FOCUS_ABSOLUTE);
1466         if (qctrl)
1467         sensor->info_priv.focus = qctrl->default_value;
1468         #if CONFIG_SENSOR_Focus
1469         if (sensor_af_init(client) < 0) {
1470                 sensor->info_priv.funmodule_state &= ~SENSOR_AF_IS_OK;
1471                 SENSOR_TR("%s auto focus module init is fail!\n",SENSOR_NAME_STRING());
1472         } else {
1473                 sensor->info_priv.funmodule_state |= SENSOR_AF_IS_OK;
1474                 SENSOR_DG("%s auto focus module init is success!\n",SENSOR_NAME_STRING());
1475         }
1476         #endif
1477         #if CONFIG_SENSOR_Flash
1478         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_FLASH);
1479         if (qctrl)
1480         sensor->info_priv.flash = qctrl->default_value;
1481     #endif
1482     SENSOR_DG("\n%s..%s.. icd->width = %d.. icd->height %d\n",SENSOR_NAME_STRING(),((val == 0)?__FUNCTION__:"sensor_reinit"),icd->user_width,icd->user_height);
1483
1484     return 0;
1485 sensor_INIT_ERR:
1486         sensor_task_lock(client,0);
1487         sensor_deactivate(client);
1488     return ret;
1489 }
1490 static int sensor_deactivate(struct i2c_client *client)
1491 {
1492         struct soc_camera_device *icd = client->dev.platform_data;
1493
1494         SENSOR_DG("\n%s..%s.. Enter\n",SENSOR_NAME_STRING(),__FUNCTION__);
1495
1496         /* ddl@rock-chips.com : all sensor output pin must change to input for other sensor */
1497
1498
1499         sensor_ioctrl(icd, Sensor_PowerDown, 1);
1500         /* ddl@rock-chips.com : sensor config init width , because next open sensor quickly(soc_camera_open -> Try to configure with default parameters) */
1501         icd->user_width = SENSOR_INIT_WIDTH;
1502     icd->user_height = SENSOR_INIT_HEIGHT;
1503
1504         return 0;
1505 }
1506 static  struct reginfo sensor_power_down_sequence[]=
1507 {
1508     {0x00,0x00}
1509 };
1510 static int sensor_suspend(struct soc_camera_device *icd, pm_message_t pm_msg)
1511 {
1512     int ret;
1513     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1514
1515     if (pm_msg.event == PM_EVENT_SUSPEND) {
1516         SENSOR_DG("\n %s Enter Suspend.. \n", SENSOR_NAME_STRING());
1517         ret = sensor_write_array(client, sensor_power_down_sequence) ;
1518         if (ret != 0) {
1519             SENSOR_TR("\n %s..%s WriteReg Fail.. \n", SENSOR_NAME_STRING(),__FUNCTION__);
1520             return ret;
1521         } else {
1522             ret = sensor_ioctrl(icd, Sensor_PowerDown, 1);
1523             if (ret < 0) {
1524                             SENSOR_TR("\n %s suspend fail for turn on power!\n", SENSOR_NAME_STRING());
1525                 return -EINVAL;
1526             }
1527         }
1528     } else {
1529         SENSOR_TR("\n %s cann't suppout Suspend..\n",SENSOR_NAME_STRING());
1530         return -EINVAL;
1531     }
1532
1533     return 0;
1534 }
1535
1536 static int sensor_resume(struct soc_camera_device *icd)
1537 {
1538         int ret;
1539
1540     ret = sensor_ioctrl(icd, Sensor_PowerDown, 0);
1541     if (ret < 0) {
1542                 SENSOR_TR("\n %s resume fail for turn on power!\n", SENSOR_NAME_STRING());
1543         return -EINVAL;
1544     }
1545
1546         SENSOR_DG("\n %s Enter Resume.. \n", SENSOR_NAME_STRING());
1547         return 0;
1548 }
1549
1550 static int sensor_set_bus_param(struct soc_camera_device *icd,
1551                                 unsigned long flags)
1552 {
1553
1554     return 0;
1555 }
1556
1557 static unsigned long sensor_query_bus_param(struct soc_camera_device *icd)
1558 {
1559     struct soc_camera_link *icl = to_soc_camera_link(icd);
1560     unsigned long flags = SENSOR_BUS_PARAM;
1561
1562     return soc_camera_apply_sensor_flags(icl, flags);
1563 }
1564
1565 static int sensor_g_fmt(struct v4l2_subdev *sd, struct v4l2_format *f)
1566 {
1567     struct i2c_client *client = sd->priv;
1568     struct soc_camera_device *icd = client->dev.platform_data;
1569     struct sensor *sensor = to_sensor(client);
1570     struct v4l2_pix_format *pix = &f->fmt.pix;
1571
1572     pix->width          = icd->user_width;
1573     pix->height         = icd->user_height;
1574     pix->pixelformat    = sensor->info_priv.pixfmt;
1575     pix->field          = V4L2_FIELD_NONE;
1576     pix->colorspace             = V4L2_COLORSPACE_JPEG;
1577
1578     return 0;
1579 }
1580 static bool sensor_fmt_capturechk(struct v4l2_subdev *sd, struct v4l2_format *f)
1581 {
1582     bool ret = false;
1583
1584         if ((f->fmt.pix.width == 1024) && (f->fmt.pix.height == 768)) {
1585                 ret = true;
1586         } else if ((f->fmt.pix.width == 1280) && (f->fmt.pix.height == 1024)) {
1587                 ret = true;
1588         } else if ((f->fmt.pix.width == 1600) && (f->fmt.pix.height == 1200)) {
1589                 ret = true;
1590         } else if ((f->fmt.pix.width == 2048) && (f->fmt.pix.height == 1536)) {
1591                 ret = true;
1592         } else if ((f->fmt.pix.width == 2592) && (f->fmt.pix.height == 1944)) {
1593                 ret = true;
1594         }
1595
1596         if (ret == true)
1597                 SENSOR_DG("%s %dx%d is capture format\n", __FUNCTION__, f->fmt.pix.width, f->fmt.pix.height);
1598         return ret;
1599 }
1600
1601 static bool sensor_fmt_videochk(struct v4l2_subdev *sd, struct v4l2_format *f)
1602 {
1603     bool ret = false;
1604
1605         if ((f->fmt.pix.width == 1280) && (f->fmt.pix.height == 720)) {
1606                 ret = true;
1607         } else if ((f->fmt.pix.width == 1920) && (f->fmt.pix.height == 1080)) {
1608                 ret = true;
1609         }
1610
1611         if (ret == true)
1612                 SENSOR_DG("%s %dx%d is video format\n", __FUNCTION__, f->fmt.pix.width, f->fmt.pix.height);
1613         return ret;
1614 }
1615 static struct reginfo* sensor_fmt_catch(int set_w, int set_h, int *ret_w, int *ret_h)
1616 {
1617         struct reginfo *winseqe_set_addr = NULL;
1618
1619         if (((set_w <= 176) && (set_h <= 144)) && (sensor_qcif[0].reg!=SEQUENCE_END)) {
1620                 winseqe_set_addr = sensor_qcif;
1621         *ret_w = 176;
1622         *ret_h = 144;
1623         } else if (((set_w <= 320) && (set_h <= 240)) && (sensor_qvga[0].reg!=SEQUENCE_END)) {
1624         winseqe_set_addr = sensor_qvga;
1625         *ret_w = 320;
1626         *ret_h = 240;
1627         } else if (((set_w <= 352) && (set_h<= 288)) && (sensor_cif[0].reg!=SEQUENCE_END)) {
1628         winseqe_set_addr = sensor_cif;
1629         *ret_w = 352;
1630         *ret_h = 288;
1631     } else if (((set_w <= 640) && (set_h <= 480)) && (sensor_vga[0].reg!=SEQUENCE_END)) {
1632         winseqe_set_addr = sensor_vga;
1633         *ret_w = 640;
1634         *ret_h = 480;
1635     } else if (((set_w <= 800) && (set_h <= 600)) && (sensor_svga[0].reg!=SEQUENCE_END)) {
1636         winseqe_set_addr = sensor_svga;
1637         *ret_w = 800;
1638         *ret_h = 600;
1639     } else if (((set_w <= 1024) && (set_h <= 768)) && (sensor_xga[0].reg!=SEQUENCE_END)) {
1640         winseqe_set_addr = sensor_xga;
1641         *ret_w = 1024;
1642         *ret_h = 768;
1643         } else if (((set_w <= 1280) && (set_h <= 720)) && (sensor_720p[0].reg!=SEQUENCE_END)) {
1644         winseqe_set_addr = sensor_720p;
1645         *ret_w = 1280;
1646         *ret_h = 720;
1647     } else if (((set_w <= 1280) && (set_h <= 1024)) && (sensor_sxga[0].reg!=SEQUENCE_END)) {
1648         winseqe_set_addr = sensor_sxga;
1649         *ret_w = 1280;
1650         *ret_h = 1024;
1651     } else if (((set_w <= 1600) && (set_h <= 1200)) && (sensor_uxga[0].reg!=SEQUENCE_END)) {
1652         winseqe_set_addr = sensor_uxga;
1653         *ret_w = 1600;
1654         *ret_h = 1200;
1655         } else if (((set_w <= 1920) && (set_h <= 1080)) && (sensor_1080p[0].reg!=SEQUENCE_END)) {
1656         winseqe_set_addr = sensor_1080p;
1657         *ret_w = 1920;
1658         *ret_h = 1080;
1659     } else if (((set_w <= 2048) && (set_h <= 1536)) && (sensor_qxga[0].reg!=SEQUENCE_END)) {
1660         winseqe_set_addr = sensor_qxga;
1661         *ret_w = 2048;
1662         *ret_h = 1536;
1663     } else if (((set_w <= 2592) && (set_h <= 1944)) && (sensor_qsxga[0].reg!=SEQUENCE_END)) {
1664         winseqe_set_addr = sensor_qsxga;
1665         *ret_w = 2592;
1666         *ret_h = 1944;
1667     }
1668
1669         return winseqe_set_addr;
1670 }
1671
1672 static int sensor_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *f)
1673 {
1674     struct i2c_client *client = sd->priv;
1675     struct sensor *sensor = to_sensor(client);
1676     struct v4l2_pix_format *pix = &f->fmt.pix;
1677     struct reginfo *winseqe_set_addr=NULL;
1678     int ret = 0, set_w,set_h;
1679
1680         if (sensor->info_priv.pixfmt != pix->pixelformat) {
1681                 switch (pix->pixelformat)
1682                 {
1683                         case V4L2_PIX_FMT_YUYV:
1684                         {
1685                                 winseqe_set_addr = sensor_ClrFmt_YUYV;
1686                                 break;
1687                         }
1688                         case V4L2_PIX_FMT_UYVY:
1689                         {
1690                                 winseqe_set_addr = sensor_ClrFmt_UYVY;
1691                                 break;
1692                         }
1693                         default:
1694                                 break;
1695                 }
1696                 if (winseqe_set_addr != NULL) {
1697             sensor_write_array(client, winseqe_set_addr);
1698                         sensor->info_priv.pixfmt = pix->pixelformat;
1699
1700                         SENSOR_DG("%s Pixelformat(0x%x) set success!\n", SENSOR_NAME_STRING(),pix->pixelformat);
1701                 } else {
1702                         SENSOR_TR("%s Pixelformat(0x%x) is invalidate!\n", SENSOR_NAME_STRING(),pix->pixelformat);
1703                 }
1704         }
1705
1706     set_w = pix->width;
1707     set_h = pix->height;
1708
1709         winseqe_set_addr = sensor_fmt_catch(set_w, set_h, &set_w, &set_h);
1710
1711     if ((winseqe_set_addr  != sensor->info_priv.winseqe_cur_addr) && winseqe_set_addr) {
1712         ret |= sensor_write_array(client, winseqe_set_addr);
1713         if (ret != 0) {
1714             SENSOR_TR("%s set format capability failed\n", SENSOR_NAME_STRING());
1715             goto sensor_s_fmt_end;
1716         }
1717         sensor->info_priv.winseqe_cur_addr  = winseqe_set_addr;
1718                 if ((winseqe_set_addr[0].reg==SEQUENCE_PROPERTY) && (winseqe_set_addr[0].val==SEQUENCE_CAPTURE)) {
1719                 SENSOR_DG("\n%s..%s..Capture icd->width = %d.. icd->height %d\n",SENSOR_NAME_STRING(),__FUNCTION__,set_w,set_h);
1720                 } else {
1721                         SENSOR_DG("\n%s..%s..Video icd->width = %d.. icd->height %d\n",SENSOR_NAME_STRING(),__FUNCTION__,set_w,set_h);
1722                         sensor->info_priv.preview_w = pix->width;
1723                         sensor->info_priv.preview_h = pix->height;
1724                 }
1725     }
1726
1727         if (winseqe_set_addr && (winseqe_set_addr[0].reg==SEQUENCE_PROPERTY) && (winseqe_set_addr[0].val==SEQUENCE_CAPTURE)) {
1728                 ret |= sensor_write_array(client, sensor_Preview2Capture);
1729                 if (ret != 0) {
1730                 SENSOR_TR("%s Preview 2 Capture failed\n", SENSOR_NAME_STRING());
1731                 goto sensor_s_fmt_end;
1732         }
1733                 sensor->info_priv.capture_w = set_w;
1734                 sensor->info_priv.capture_h = set_h;
1735                 sensor->info_priv.snap2preview = true;
1736         } else if (sensor->info_priv.snap2preview == true) {
1737                 if (winseqe_set_addr || ((sensor->info_priv.preview_w == pix->width) && (sensor->info_priv.preview_h == pix->height))) {
1738                         ret |= sensor_write_array(client, sensor_Capture2Preview);
1739                         if (ret != 0) {
1740                         SENSOR_TR("%s Capture 2 Preview failed\n", SENSOR_NAME_STRING());
1741                         goto sensor_s_fmt_end;
1742                 }
1743                         sensor->info_priv.preview_w = pix->width;
1744                         sensor->info_priv.preview_h = pix->height;
1745                         sensor->info_priv.snap2preview = false;
1746                 } else {
1747                         SENSOR_TR("\n %s..%s Format is Invalidate. pix->width = %d.. pix->height = %d\n",SENSOR_NAME_STRING(),__FUNCTION__,pix->width,pix->height);
1748                 }
1749         }
1750
1751         pix->width = set_w;
1752         pix->height = set_h;
1753 sensor_s_fmt_end:
1754     return ret;
1755 }
1756
1757 static int sensor_try_fmt(struct v4l2_subdev *sd, struct v4l2_format *f)
1758 {
1759         int ret = 0;
1760     struct v4l2_pix_format *pix = &f->fmt.pix;
1761     bool bayer = pix->pixelformat == V4L2_PIX_FMT_UYVY ||
1762         pix->pixelformat == V4L2_PIX_FMT_YUYV;
1763
1764     /*
1765     * With Bayer format enforce even side lengths, but let the user play
1766     * with the starting pixel
1767     */
1768
1769     if (pix->height > SENSOR_MAX_HEIGHT)
1770         pix->height = SENSOR_MAX_HEIGHT;
1771     else if (pix->height < SENSOR_MIN_HEIGHT)
1772         pix->height = SENSOR_MIN_HEIGHT;
1773     else if (bayer)
1774         pix->height = ALIGN(pix->height, 2);
1775
1776     if (pix->width > SENSOR_MAX_WIDTH)
1777         pix->width = SENSOR_MAX_WIDTH;
1778     else if (pix->width < SENSOR_MIN_WIDTH)
1779         pix->width = SENSOR_MIN_WIDTH;
1780     else if (bayer)
1781         pix->width = ALIGN(pix->width, 2);
1782
1783         if (sensor_fmt_catch(pix->width, pix->height, &pix->width, &pix->height) == NULL) {
1784                 pix->width = 0;
1785                 pix->height = 0;
1786         }
1787
1788     return ret;
1789 }
1790
1791  static int sensor_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *id)
1792 {
1793     struct i2c_client *client = sd->priv;
1794
1795     if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
1796         return -EINVAL;
1797
1798     if (id->match.addr != client->addr)
1799         return -ENODEV;
1800
1801     id->ident = SENSOR_V4L2_IDENT;      /* ddl@rock-chips.com :  Return OV2655  identifier */
1802     id->revision = 0;
1803
1804     return 0;
1805 }
1806 #if CONFIG_SENSOR_Brightness
1807 static int sensor_set_brightness(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1808 {
1809     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1810
1811     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1812     {
1813         if (sensor_BrightnessSeqe[value - qctrl->minimum] != NULL)
1814         {
1815             if (sensor_write_array(client, sensor_BrightnessSeqe[value - qctrl->minimum]) != 0)
1816             {
1817                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1818                 return -EINVAL;
1819             }
1820             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1821             return 0;
1822         }
1823     }
1824         SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1825     return -EINVAL;
1826 }
1827 #endif
1828 #if CONFIG_SENSOR_Effect
1829 static int sensor_set_effect(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1830 {
1831     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1832
1833     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1834     {
1835         if (sensor_EffectSeqe[value - qctrl->minimum] != NULL)
1836         {
1837             if (sensor_write_array(client, sensor_EffectSeqe[value - qctrl->minimum]) != 0)
1838             {
1839                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1840                 return -EINVAL;
1841             }
1842             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1843             return 0;
1844         }
1845     }
1846         SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1847     return -EINVAL;
1848 }
1849 #endif
1850 #if CONFIG_SENSOR_Exposure
1851 static int sensor_set_exposure(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1852 {
1853     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1854
1855     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1856     {
1857         if (sensor_ExposureSeqe[value - qctrl->minimum] != NULL)
1858         {
1859             if (sensor_write_array(client, sensor_ExposureSeqe[value - qctrl->minimum]) != 0)
1860             {
1861                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1862                 return -EINVAL;
1863             }
1864             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1865             return 0;
1866         }
1867     }
1868         SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1869     return -EINVAL;
1870 }
1871 #endif
1872 #if CONFIG_SENSOR_Saturation
1873 static int sensor_set_saturation(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1874 {
1875     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1876
1877     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1878     {
1879         if (sensor_SaturationSeqe[value - qctrl->minimum] != NULL)
1880         {
1881             if (sensor_write_array(client, sensor_SaturationSeqe[value - qctrl->minimum]) != 0)
1882             {
1883                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1884                 return -EINVAL;
1885             }
1886             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1887             return 0;
1888         }
1889     }
1890     SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1891     return -EINVAL;
1892 }
1893 #endif
1894 #if CONFIG_SENSOR_Contrast
1895 static int sensor_set_contrast(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1896 {
1897     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1898
1899     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1900     {
1901         if (sensor_ContrastSeqe[value - qctrl->minimum] != NULL)
1902         {
1903             if (sensor_write_array(client, sensor_ContrastSeqe[value - qctrl->minimum]) != 0)
1904             {
1905                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1906                 return -EINVAL;
1907             }
1908             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1909             return 0;
1910         }
1911     }
1912     SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1913     return -EINVAL;
1914 }
1915 #endif
1916 #if CONFIG_SENSOR_Mirror
1917 static int sensor_set_mirror(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1918 {
1919     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1920
1921     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1922     {
1923         if (sensor_MirrorSeqe[value - qctrl->minimum] != NULL)
1924         {
1925             if (sensor_write_array(client, sensor_MirrorSeqe[value - qctrl->minimum]) != 0)
1926             {
1927                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1928                 return -EINVAL;
1929             }
1930             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1931             return 0;
1932         }
1933     }
1934     SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1935     return -EINVAL;
1936 }
1937 #endif
1938 #if CONFIG_SENSOR_Flip
1939 static int sensor_set_flip(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1940 {
1941     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1942
1943     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1944     {
1945         if (sensor_FlipSeqe[value - qctrl->minimum] != NULL)
1946         {
1947             if (sensor_write_array(client, sensor_FlipSeqe[value - qctrl->minimum]) != 0)
1948             {
1949                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1950                 return -EINVAL;
1951             }
1952             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1953             return 0;
1954         }
1955     }
1956     SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1957     return -EINVAL;
1958 }
1959 #endif
1960 #if CONFIG_SENSOR_Scene
1961 static int sensor_set_scene(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1962 {
1963     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1964
1965     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1966     {
1967         if (sensor_SceneSeqe[value - qctrl->minimum] != NULL)
1968         {
1969             if (sensor_write_array(client, sensor_SceneSeqe[value - qctrl->minimum]) != 0)
1970             {
1971                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1972                 return -EINVAL;
1973             }
1974             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1975             return 0;
1976         }
1977     }
1978     SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1979     return -EINVAL;
1980 }
1981 #endif
1982 #if CONFIG_SENSOR_WhiteBalance
1983 static int sensor_set_whiteBalance(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1984 {
1985     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1986
1987     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1988     {
1989         if (sensor_WhiteBalanceSeqe[value - qctrl->minimum] != NULL)
1990         {
1991             if (sensor_write_array(client, sensor_WhiteBalanceSeqe[value - qctrl->minimum]) != 0)
1992             {
1993                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1994                 return -EINVAL;
1995             }
1996             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1997             return 0;
1998         }
1999     }
2000         SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
2001     return -EINVAL;
2002 }
2003 #endif
2004 #if CONFIG_SENSOR_DigitalZoom
2005 static int sensor_set_digitalzoom(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int *value)
2006 {
2007     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
2008     struct sensor *sensor = to_sensor(client);
2009         const struct v4l2_queryctrl *qctrl_info;
2010     int digitalzoom_cur, digitalzoom_total;
2011
2012         qctrl_info = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_ZOOM_ABSOLUTE);
2013         if (qctrl_info)
2014                 return -EINVAL;
2015
2016     digitalzoom_cur = sensor->info_priv.digitalzoom;
2017     digitalzoom_total = qctrl_info->maximum;
2018
2019     if ((*value > 0) && (digitalzoom_cur >= digitalzoom_total))
2020     {
2021         SENSOR_TR("%s digitalzoom is maximum - %x\n", SENSOR_NAME_STRING(), digitalzoom_cur);
2022         return -EINVAL;
2023     }
2024
2025     if  ((*value < 0) && (digitalzoom_cur <= qctrl_info->minimum))
2026     {
2027         SENSOR_TR("%s digitalzoom is minimum - %x\n", SENSOR_NAME_STRING(), digitalzoom_cur);
2028         return -EINVAL;
2029     }
2030
2031     if ((*value > 0) && ((digitalzoom_cur + *value) > digitalzoom_total))
2032     {
2033         *value = digitalzoom_total - digitalzoom_cur;
2034     }
2035
2036     if ((*value < 0) && ((digitalzoom_cur + *value) < 0))
2037     {
2038         *value = 0 - digitalzoom_cur;
2039     }
2040
2041     digitalzoom_cur += *value;
2042
2043     if (sensor_ZoomSeqe[digitalzoom_cur] != NULL)
2044     {
2045         if (sensor_write_array(client, sensor_ZoomSeqe[digitalzoom_cur]) != 0)
2046         {
2047             SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
2048             return -EINVAL;
2049         }
2050         SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, *value);
2051         return 0;
2052     }
2053
2054     return -EINVAL;
2055 }
2056 #endif
2057 #if CONFIG_SENSOR_Focus
2058 static int sensor_set_focus_absolute(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
2059 {
2060         struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
2061     struct sensor *sensor = to_sensor(client);
2062         const struct v4l2_queryctrl *qctrl_info;
2063         int ret = 0;
2064
2065         qctrl_info = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_FOCUS_ABSOLUTE);
2066         if (!qctrl_info)
2067                 return -EINVAL;
2068
2069         if ((sensor->info_priv.funmodule_state & SENSOR_AF_IS_OK) && (sensor->info_priv.affm_reinit == 0)) {
2070                 if ((value >= qctrl_info->minimum) && (value <= qctrl_info->maximum)) {
2071
2072                         SENSOR_DG("%s..%s : %d  ret:0x%x\n",SENSOR_NAME_STRING(),__FUNCTION__, value,ret);
2073                 } else {
2074                         ret = -EINVAL;
2075                         SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
2076                 }
2077         } else {
2078                 ret = -EACCES;
2079                 SENSOR_TR("\n %s..%s AF module state(0x%x, 0x%x) is error!\n",SENSOR_NAME_STRING(),__FUNCTION__,
2080                         sensor->info_priv.funmodule_state,sensor->info_priv.affm_reinit);
2081         }
2082
2083 sensor_set_focus_absolute_end:
2084         return ret;
2085 }
2086 static int sensor_set_focus_relative(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
2087 {
2088         struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
2089         struct sensor *sensor = to_sensor(client);
2090         const struct v4l2_queryctrl *qctrl_info;
2091         int ret = 0;
2092
2093         qctrl_info = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_FOCUS_RELATIVE);
2094         if (!qctrl_info)
2095                 return -EINVAL;
2096
2097         if ((sensor->info_priv.funmodule_state & SENSOR_AF_IS_OK) && (sensor->info_priv.affm_reinit == 0)) {
2098                 if ((value >= qctrl_info->minimum) && (value <= qctrl_info->maximum)) {
2099
2100                         SENSOR_DG("%s..%s : %d  ret:0x%x\n",SENSOR_NAME_STRING(),__FUNCTION__, value,ret);
2101                 } else {
2102                         ret = -EINVAL;
2103                         SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
2104                 }
2105         } else {
2106                 ret = -EACCES;
2107                 SENSOR_TR("\n %s..%s AF module state(0x%x, 0x%x) is error!\n",SENSOR_NAME_STRING(),__FUNCTION__,
2108                         sensor->info_priv.funmodule_state,sensor->info_priv.affm_reinit);
2109         }
2110 sensor_set_focus_relative_end:
2111         return ret;
2112 }
2113
2114 static int sensor_set_focus_mode(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
2115 {
2116         struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
2117         struct sensor *sensor = to_sensor(client);
2118         int ret = 0;
2119
2120         if ((sensor->info_priv.funmodule_state & SENSOR_AF_IS_OK)  && (sensor->info_priv.affm_reinit == 0)) {
2121                 switch (value)
2122                 {
2123                         case SENSOR_AF_MODE_AUTO:
2124                         {
2125                                 ret = sensor_af_single(client);
2126                                 break;
2127                         }
2128
2129                         case SENSOR_AF_MODE_MACRO:
2130                         {
2131                                 ret = sensor_set_focus_absolute(icd, qctrl, 0xff);
2132                                 break;
2133                         }
2134
2135                         case SENSOR_AF_MODE_INFINITY:
2136                         {
2137                                 ret = sensor_set_focus_absolute(icd, qctrl, 0x00);
2138                                 break;
2139                         }
2140
2141                         case SENSOR_AF_MODE_CONTINUOUS:
2142                         {
2143                                 ret = sensor_af_const(client);
2144                                 break;
2145                         }
2146                         default:
2147                                 SENSOR_TR("\n %s..%s AF value(0x%x) is error!\n",SENSOR_NAME_STRING(),__FUNCTION__,value);
2148                                 break;
2149
2150                 }
2151
2152                 SENSOR_DG("%s..%s : %d  ret:0x%x\n",SENSOR_NAME_STRING(),__FUNCTION__, value,ret);
2153         } else {
2154                 ret = -EACCES;
2155                 SENSOR_TR("\n %s..%s AF module state(0x%x, 0x%x) is error!\n",SENSOR_NAME_STRING(),__FUNCTION__,
2156                         sensor->info_priv.funmodule_state,sensor->info_priv.affm_reinit);
2157         }
2158
2159         return ret;
2160 }
2161 #endif
2162 static int sensor_g_control(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
2163 {
2164     struct i2c_client *client = sd->priv;
2165     struct sensor *sensor = to_sensor(client);
2166     const struct v4l2_queryctrl *qctrl;
2167
2168     qctrl = soc_camera_find_qctrl(&sensor_ops, ctrl->id);
2169
2170     if (!qctrl)
2171     {
2172         SENSOR_TR("\n %s ioctrl id = 0x%x  is invalidate \n", SENSOR_NAME_STRING(), ctrl->id);
2173         return -EINVAL;
2174     }
2175
2176     switch (ctrl->id)
2177     {
2178         case V4L2_CID_BRIGHTNESS:
2179             {
2180                 ctrl->value = sensor->info_priv.brightness;
2181                 break;
2182             }
2183         case V4L2_CID_SATURATION:
2184             {
2185                 ctrl->value = sensor->info_priv.saturation;
2186                 break;
2187             }
2188         case V4L2_CID_CONTRAST:
2189             {
2190                 ctrl->value = sensor->info_priv.contrast;
2191                 break;
2192             }
2193         case V4L2_CID_DO_WHITE_BALANCE:
2194             {
2195                 ctrl->value = sensor->info_priv.whiteBalance;
2196                 break;
2197             }
2198         case V4L2_CID_EXPOSURE:
2199             {
2200                 ctrl->value = sensor->info_priv.exposure;
2201                 break;
2202             }
2203         case V4L2_CID_HFLIP:
2204             {
2205                 ctrl->value = sensor->info_priv.mirror;
2206                 break;
2207             }
2208         case V4L2_CID_VFLIP:
2209             {
2210                 ctrl->value = sensor->info_priv.flip;
2211                 break;
2212             }
2213         default :
2214                 break;
2215     }
2216     return 0;
2217 }
2218
2219
2220
2221 static int sensor_s_control(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
2222 {
2223     struct i2c_client *client = sd->priv;
2224     struct sensor *sensor = to_sensor(client);
2225     struct soc_camera_device *icd = client->dev.platform_data;
2226     const struct v4l2_queryctrl *qctrl;
2227
2228
2229     qctrl = soc_camera_find_qctrl(&sensor_ops, ctrl->id);
2230
2231     if (!qctrl)
2232     {
2233         SENSOR_TR("\n %s ioctrl id = 0x%x  is invalidate \n", SENSOR_NAME_STRING(), ctrl->id);
2234         return -EINVAL;
2235     }
2236
2237     switch (ctrl->id)
2238     {
2239 #if CONFIG_SENSOR_Brightness
2240         case V4L2_CID_BRIGHTNESS:
2241             {
2242                 if (ctrl->value != sensor->info_priv.brightness)
2243                 {
2244                     if (sensor_set_brightness(icd, qctrl,ctrl->value) != 0)
2245                     {
2246                         return -EINVAL;
2247                     }
2248                     sensor->info_priv.brightness = ctrl->value;
2249                 }
2250                 break;
2251             }
2252 #endif
2253 #if CONFIG_SENSOR_Exposure
2254         case V4L2_CID_EXPOSURE:
2255             {
2256                 if (ctrl->value != sensor->info_priv.exposure)
2257                 {
2258                     if (sensor_set_exposure(icd, qctrl,ctrl->value) != 0)
2259                     {
2260                         return -EINVAL;
2261                     }
2262                     sensor->info_priv.exposure = ctrl->value;
2263                 }
2264                 break;
2265             }
2266 #endif
2267 #if CONFIG_SENSOR_Saturation
2268         case V4L2_CID_SATURATION:
2269             {
2270                 if (ctrl->value != sensor->info_priv.saturation)
2271                 {
2272                     if (sensor_set_saturation(icd, qctrl,ctrl->value) != 0)
2273                     {
2274                         return -EINVAL;
2275                     }
2276                     sensor->info_priv.saturation = ctrl->value;
2277                 }
2278                 break;
2279             }
2280 #endif
2281 #if CONFIG_SENSOR_Contrast
2282         case V4L2_CID_CONTRAST:
2283             {
2284                 if (ctrl->value != sensor->info_priv.contrast)
2285                 {
2286                     if (sensor_set_contrast(icd, qctrl,ctrl->value) != 0)
2287                     {
2288                         return -EINVAL;
2289                     }
2290                     sensor->info_priv.contrast = ctrl->value;
2291                 }
2292                 break;
2293             }
2294 #endif
2295 #if CONFIG_SENSOR_WhiteBalance
2296         case V4L2_CID_DO_WHITE_BALANCE:
2297             {
2298                 if (ctrl->value != sensor->info_priv.whiteBalance)
2299                 {
2300                     if (sensor_set_whiteBalance(icd, qctrl,ctrl->value) != 0)
2301                     {
2302                         return -EINVAL;
2303                     }
2304                     sensor->info_priv.whiteBalance = ctrl->value;
2305                 }
2306                 break;
2307             }
2308 #endif
2309 #if CONFIG_SENSOR_Mirror
2310         case V4L2_CID_HFLIP:
2311             {
2312                 if (ctrl->value != sensor->info_priv.mirror)
2313                 {
2314                     if (sensor_set_mirror(icd, qctrl,ctrl->value) != 0)
2315                         return -EINVAL;
2316                     sensor->info_priv.mirror = ctrl->value;
2317                 }
2318                 break;
2319             }
2320 #endif
2321 #if CONFIG_SENSOR_Flip
2322         case V4L2_CID_VFLIP:
2323             {
2324                 if (ctrl->value != sensor->info_priv.flip)
2325                 {
2326                     if (sensor_set_flip(icd, qctrl,ctrl->value) != 0)
2327                         return -EINVAL;
2328                     sensor->info_priv.flip = ctrl->value;
2329                 }
2330                 break;
2331             }
2332 #endif
2333         default:
2334             break;
2335     }
2336
2337     return 0;
2338 }
2339 static int sensor_g_ext_control(struct soc_camera_device *icd , struct v4l2_ext_control *ext_ctrl)
2340 {
2341     const struct v4l2_queryctrl *qctrl;
2342     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
2343     struct sensor *sensor = to_sensor(client);
2344
2345     qctrl = soc_camera_find_qctrl(&sensor_ops, ext_ctrl->id);
2346
2347     if (!qctrl)
2348     {
2349         SENSOR_TR("\n %s ioctrl id = 0x%x  is invalidate \n", SENSOR_NAME_STRING(), ext_ctrl->id);
2350         return -EINVAL;
2351     }
2352
2353     switch (ext_ctrl->id)
2354     {
2355         case V4L2_CID_SCENE:
2356             {
2357                 ext_ctrl->value = sensor->info_priv.scene;
2358                 break;
2359             }
2360         case V4L2_CID_EFFECT:
2361             {
2362                 ext_ctrl->value = sensor->info_priv.effect;
2363                 break;
2364             }
2365         case V4L2_CID_ZOOM_ABSOLUTE:
2366             {
2367                 ext_ctrl->value = sensor->info_priv.digitalzoom;
2368                 break;
2369             }
2370         case V4L2_CID_ZOOM_RELATIVE:
2371             {
2372                 return -EINVAL;
2373             }
2374         case V4L2_CID_FOCUS_ABSOLUTE:
2375             {
2376                 return -EINVAL;
2377             }
2378         case V4L2_CID_FOCUS_RELATIVE:
2379             {
2380                 return -EINVAL;
2381             }
2382         case V4L2_CID_FLASH:
2383             {
2384                 ext_ctrl->value = sensor->info_priv.flash;
2385                 break;
2386             }
2387         default :
2388             break;
2389     }
2390     return 0;
2391 }
2392 static int sensor_s_ext_control(struct soc_camera_device *icd, struct v4l2_ext_control *ext_ctrl)
2393 {
2394     const struct v4l2_queryctrl *qctrl;
2395     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
2396     struct sensor *sensor = to_sensor(client);
2397     int val_offset;
2398
2399     qctrl = soc_camera_find_qctrl(&sensor_ops, ext_ctrl->id);
2400
2401     if (!qctrl)
2402     {
2403         SENSOR_TR("\n %s ioctrl id = 0x%x  is invalidate \n", SENSOR_NAME_STRING(), ext_ctrl->id);
2404         return -EINVAL;
2405     }
2406
2407         val_offset = 0;
2408     switch (ext_ctrl->id)
2409     {
2410 #if CONFIG_SENSOR_Scene
2411         case V4L2_CID_SCENE:
2412             {
2413                 if (ext_ctrl->value != sensor->info_priv.scene)
2414                 {
2415                     if (sensor_set_scene(icd, qctrl,ext_ctrl->value) != 0)
2416                         return -EINVAL;
2417                     sensor->info_priv.scene = ext_ctrl->value;
2418                 }
2419                 break;
2420             }
2421 #endif
2422 #if CONFIG_SENSOR_Effect
2423         case V4L2_CID_EFFECT:
2424             {
2425                 if (ext_ctrl->value != sensor->info_priv.effect)
2426                 {
2427                     if (sensor_set_effect(icd, qctrl,ext_ctrl->value) != 0)
2428                         return -EINVAL;
2429                     sensor->info_priv.effect= ext_ctrl->value;
2430                 }
2431                 break;
2432             }
2433 #endif
2434 #if CONFIG_SENSOR_DigitalZoom
2435         case V4L2_CID_ZOOM_ABSOLUTE:
2436             {
2437                 if ((ext_ctrl->value < qctrl->minimum) || (ext_ctrl->value > qctrl->maximum))
2438                     return -EINVAL;
2439
2440                 if (ext_ctrl->value != sensor->info_priv.digitalzoom)
2441                 {
2442                     val_offset = ext_ctrl->value -sensor->info_priv.digitalzoom;
2443
2444                     if (sensor_set_digitalzoom(icd, qctrl,&val_offset) != 0)
2445                         return -EINVAL;
2446                     sensor->info_priv.digitalzoom += val_offset;
2447
2448                     SENSOR_DG("%s digitalzoom is %x\n",SENSOR_NAME_STRING(),  sensor->info_priv.digitalzoom);
2449                 }
2450
2451                 break;
2452             }
2453         case V4L2_CID_ZOOM_RELATIVE:
2454             {
2455                 if (ext_ctrl->value)
2456                 {
2457                     if (sensor_set_digitalzoom(icd, qctrl,&ext_ctrl->value) != 0)
2458                         return -EINVAL;
2459                     sensor->info_priv.digitalzoom += ext_ctrl->value;
2460
2461                     SENSOR_DG("%s digitalzoom is %x\n", SENSOR_NAME_STRING(), sensor->info_priv.digitalzoom);
2462                 }
2463                 break;
2464             }
2465 #endif
2466 #if CONFIG_SENSOR_Focus
2467         case V4L2_CID_FOCUS_ABSOLUTE:
2468             {
2469                 if ((ext_ctrl->value < qctrl->minimum) || (ext_ctrl->value > qctrl->maximum))
2470                     return -EINVAL;
2471
2472                                 if (sensor_set_focus_absolute(icd, qctrl,ext_ctrl->value) == 0) {
2473                                         if (ext_ctrl->value == qctrl->minimum) {
2474                                                 sensor->info_priv.auto_focus = SENSOR_AF_MODE_INFINITY;
2475                                         } else if (ext_ctrl->value == qctrl->maximum) {
2476                                                 sensor->info_priv.auto_focus = SENSOR_AF_MODE_MACRO;
2477                                         } else {
2478                                                 sensor->info_priv.auto_focus = SENSOR_AF_MODE_FIXED;
2479                                         }
2480                                 }
2481
2482                 break;
2483             }
2484         case V4L2_CID_FOCUS_RELATIVE:
2485             {
2486                 if ((ext_ctrl->value < qctrl->minimum) || (ext_ctrl->value > qctrl->maximum))
2487                     return -EINVAL;
2488
2489                 sensor_set_focus_relative(icd, qctrl,ext_ctrl->value);
2490                 break;
2491             }
2492                 case V4L2_CID_FOCUS_AUTO:
2493                         {
2494                                 if (ext_ctrl->value == 1) {
2495                                         if (sensor_set_focus_mode(icd, qctrl,SENSOR_AF_MODE_AUTO) != 0)
2496                                                 return -EINVAL;
2497                                         sensor->info_priv.auto_focus = SENSOR_AF_MODE_AUTO;
2498                                 } else if (SENSOR_AF_MODE_AUTO == sensor->info_priv.auto_focus){
2499                                         if (ext_ctrl->value == 0)
2500                                                 sensor->info_priv.auto_focus = SENSOR_AF_MODE_CLOSE;
2501                                 }
2502                                 break;
2503                         }
2504                 case V4L2_CID_FOCUS_CONTINUOUS:
2505                         {
2506                                 if (SENSOR_AF_MODE_CONTINUOUS != sensor->info_priv.auto_focus) {
2507                                         if (ext_ctrl->value == 1) {
2508                                                 if (sensor_set_focus_mode(icd, qctrl,SENSOR_AF_MODE_CONTINUOUS) != 0)
2509                                                         return -EINVAL;
2510                                                 sensor->info_priv.auto_focus = SENSOR_AF_MODE_CONTINUOUS;
2511                                         }
2512                                 } else {
2513                                         if (ext_ctrl->value == 0)
2514                                                 sensor->info_priv.auto_focus = SENSOR_AF_MODE_CLOSE;
2515                                 }
2516                                 break;
2517                         }
2518 #endif
2519 #if CONFIG_SENSOR_Flash
2520         case V4L2_CID_FLASH:
2521             {
2522                 sensor->info_priv.flash = ext_ctrl->value;
2523
2524                 SENSOR_DG("%s flash is %x\n",SENSOR_NAME_STRING(), sensor->info_priv.flash);
2525                 break;
2526             }
2527 #endif
2528         default:
2529             break;
2530     }
2531
2532     return 0;
2533 }
2534
2535 static int sensor_g_ext_controls(struct v4l2_subdev *sd, struct v4l2_ext_controls *ext_ctrl)
2536 {
2537     struct i2c_client *client = sd->priv;
2538     struct soc_camera_device *icd = client->dev.platform_data;
2539     int i, error_cnt=0, error_idx=-1;
2540
2541
2542     for (i=0; i<ext_ctrl->count; i++) {
2543         if (sensor_g_ext_control(icd, &ext_ctrl->controls[i]) != 0) {
2544             error_cnt++;
2545             error_idx = i;
2546         }
2547     }
2548
2549     if (error_cnt > 1)
2550         error_idx = ext_ctrl->count;
2551
2552     if (error_idx != -1) {
2553         ext_ctrl->error_idx = error_idx;
2554         return -EINVAL;
2555     } else {
2556         return 0;
2557     }
2558 }
2559
2560 static int sensor_s_ext_controls(struct v4l2_subdev *sd, struct v4l2_ext_controls *ext_ctrl)
2561 {
2562     struct i2c_client *client = sd->priv;
2563     struct soc_camera_device *icd = client->dev.platform_data;
2564     int i, error_cnt=0, error_idx=-1;
2565
2566     for (i=0; i<ext_ctrl->count; i++) {
2567         if (sensor_s_ext_control(icd, &ext_ctrl->controls[i]) != 0) {
2568             error_cnt++;
2569             error_idx = i;
2570         }
2571     }
2572
2573     if (error_cnt > 1)
2574         error_idx = ext_ctrl->count;
2575
2576     if (error_idx != -1) {
2577         ext_ctrl->error_idx = error_idx;
2578         return -EINVAL;
2579     } else {
2580         return 0;
2581     }
2582 }
2583
2584 static int sensor_s_stream(struct v4l2_subdev *sd, int enable)
2585 {
2586         struct i2c_client *client = sd->priv;
2587     struct sensor *sensor = to_sensor(client);
2588
2589         if (enable == 1) {
2590                 sensor->info_priv.enable = 1;
2591         } else if (enable == 0) {
2592                 sensor->info_priv.enable = 0;
2593         }
2594
2595 sensor_s_stream_end:
2596         return 0;
2597 }
2598
2599 /* Interface active, can use i2c. If it fails, it can indeed mean, that
2600  * this wasn't our capture interface, so, we wait for the right one */
2601 static int sensor_video_probe(struct soc_camera_device *icd,
2602                                struct i2c_client *client)
2603 {
2604     int ret,pid = 0;
2605     struct sensor *sensor = to_sensor(client);
2606
2607     /* We must have a parent by now. And it cannot be a wrong one.
2608      * So this entire test is completely redundant. */
2609     if (!icd->dev.parent ||
2610             to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
2611                 return -ENODEV;
2612
2613         if (sensor_ioctrl(icd, Sensor_PowerDown, 0) < 0) {
2614                 ret = -ENODEV;
2615                 goto sensor_video_probe_err;
2616         }
2617
2618     /* soft reset */
2619 #if (SENSOR_RESET_REG != SEQUENCE_END)
2620     struct reginfo reg_info;
2621         reg_info.reg = SENSOR_RESET_REG;
2622         reg_info.val = SENSOR_RESET_VAL;
2623         reg_info.reg_len = SENSOR_RESET_REG_LEN;
2624     ret = sensor_write(client, &reg_info);
2625     if (ret != 0) {
2626         SENSOR_TR("%s soft reset sensor failed\n",SENSOR_NAME_STRING());
2627         ret = -ENODEV;
2628                 goto sensor_video_probe_err;
2629     }
2630
2631     mdelay(5);  //delay 5 microseconds
2632 #endif
2633
2634         /* check if it is an sensor sensor */
2635 #if (SENSOR_ID_REG != SEQUENCE_END)
2636     ret = sensor_read(client, SENSOR_ID_REG, &pid);
2637     if (ret != 0) {
2638         SENSOR_TR("read chip id failed\n");
2639         ret = -ENODEV;
2640         goto sensor_video_probe_err;
2641     }
2642
2643     SENSOR_DG("\n %s  pid = 0x%x \n", SENSOR_NAME_STRING(), pid);
2644 #else
2645         pid = SENSOR_ID;
2646 #endif
2647
2648     if (pid == SENSOR_ID) {
2649         sensor->model = SENSOR_V4L2_IDENT;
2650     } else {
2651         SENSOR_TR("error: %s mismatched   pid = 0x%x\n", SENSOR_NAME_STRING(), pid);
2652         ret = -ENODEV;
2653         goto sensor_video_probe_err;
2654     }
2655
2656     icd->formats = sensor_colour_formats;
2657     icd->num_formats = ARRAY_SIZE(sensor_colour_formats);
2658
2659     return 0;
2660
2661 sensor_video_probe_err:
2662
2663     return ret;
2664 }
2665 static long sensor_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
2666 {
2667         struct i2c_client *client = sd->priv;
2668     struct sensor *sensor = to_sensor(client);
2669
2670         SENSOR_DG("\n%s..%s..cmd:%x \n",SENSOR_NAME_STRING(),__FUNCTION__,cmd);
2671         switch (cmd)
2672         {
2673                 case RK29_CAM_SUBDEV_DEACTIVATE:
2674                 {
2675                         sensor_deactivate(client);
2676                         break;
2677                 }
2678                 case RK29_CAM_SUBDEV_IOREQUEST:
2679                 {
2680                         sensor->sensor_io_request = (struct rk29camera_platform_data*)arg;
2681                         break;
2682                 }
2683                 default:
2684                 {
2685                         SENSOR_TR("%s %s cmd(0x%x) is unknown !\n",SENSOR_NAME_STRING(),__FUNCTION__,cmd);
2686                         break;
2687                 }
2688         }
2689
2690         return 0;
2691
2692 }
2693
2694 static struct v4l2_subdev_core_ops sensor_subdev_core_ops = {
2695         .init           = sensor_init,
2696         .g_ctrl         = sensor_g_control,
2697         .s_ctrl         = sensor_s_control,
2698         .g_ext_ctrls          = sensor_g_ext_controls,
2699         .s_ext_ctrls          = sensor_s_ext_controls,
2700         .g_chip_ident   = sensor_g_chip_ident,
2701         .ioctl = sensor_ioctl,
2702 };
2703
2704 static struct v4l2_subdev_video_ops sensor_subdev_video_ops = {
2705         .s_fmt          = sensor_s_fmt,
2706         .g_fmt          = sensor_g_fmt,
2707         .try_fmt        = sensor_try_fmt,
2708         .s_stream   = sensor_s_stream,
2709 };
2710
2711 static struct v4l2_subdev_ops sensor_subdev_ops = {
2712         .core   = &sensor_subdev_core_ops,
2713         .video = &sensor_subdev_video_ops,
2714 };
2715
2716 static int sensor_probe(struct i2c_client *client,
2717                          const struct i2c_device_id *did)
2718 {
2719     struct sensor *sensor;
2720     struct soc_camera_device *icd = client->dev.platform_data;
2721     struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
2722     struct soc_camera_link *icl;
2723     int ret;
2724
2725     SENSOR_DG("\n%s..%s..%d..\n",__FUNCTION__,__FILE__,__LINE__);
2726     if (!icd) {
2727         dev_err(&client->dev, "%s: missing soc-camera data!\n",SENSOR_NAME_STRING());
2728         return -EINVAL;
2729     }
2730
2731     icl = to_soc_camera_link(icd);
2732     if (!icl) {
2733         dev_err(&client->dev, "%s driver needs platform data\n", SENSOR_NAME_STRING());
2734         return -EINVAL;
2735     }
2736
2737     if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
2738         dev_warn(&adapter->dev,
2739                  "I2C-Adapter doesn't support I2C_FUNC_I2C\n");
2740         return -EIO;
2741     }
2742
2743     sensor = kzalloc(sizeof(struct sensor), GFP_KERNEL);
2744     if (!sensor)
2745         return -ENOMEM;
2746
2747     v4l2_i2c_subdev_init(&sensor->subdev, client, &sensor_subdev_ops);
2748
2749     /* Second stage probe - when a capture adapter is there */
2750     icd->ops            = &sensor_ops;
2751     icd->y_skip_top             = 0;
2752         #if CONFIG_SENSOR_I2C_NOSCHED
2753         atomic_set(&sensor->tasklock_cnt,0);
2754         #endif
2755
2756     ret = sensor_video_probe(icd, client);
2757     if (ret < 0) {
2758         icd->ops = NULL;
2759         i2c_set_clientdata(client, NULL);
2760         kfree(sensor);
2761                 sensor = NULL;
2762     }
2763     SENSOR_DG("\n%s..%s..%d  ret = %x \n",__FUNCTION__,__FILE__,__LINE__,ret);
2764     return ret;
2765 }
2766
2767 static int sensor_remove(struct i2c_client *client)
2768 {
2769     struct sensor *sensor = to_sensor(client);
2770     struct soc_camera_device *icd = client->dev.platform_data;
2771
2772         #if CONFIG_SENSOR_Focus
2773         if (sensor->sensor_wq) {
2774                 destroy_workqueue(sensor->sensor_wq);
2775                 sensor->sensor_wq = NULL;
2776         }
2777         #endif
2778
2779     icd->ops = NULL;
2780     i2c_set_clientdata(client, NULL);
2781     client->driver = NULL;
2782     kfree(sensor);
2783         sensor = NULL;
2784     return 0;
2785 }
2786
2787 static const struct i2c_device_id sensor_id[] = {
2788         {SENSOR_NAME_STRING(), 0 },
2789         { }
2790 };
2791 MODULE_DEVICE_TABLE(i2c, sensor_id);
2792
2793 static struct i2c_driver sensor_i2c_driver = {
2794         .driver = {
2795                 .name = SENSOR_NAME_STRING(),
2796         },
2797         .probe          = sensor_probe,
2798         .remove         = sensor_remove,
2799         .id_table       = sensor_id,
2800 };
2801
2802 static int __init sensor_mod_init(void)
2803 {
2804     SENSOR_DG("\n%s..%s.. \n",__FUNCTION__,SENSOR_NAME_STRING());
2805     return i2c_add_driver(&sensor_i2c_driver);
2806 }
2807
2808 static void __exit sensor_mod_exit(void)
2809 {
2810     i2c_del_driver(&sensor_i2c_driver);
2811 }
2812
2813 device_initcall_sync(sensor_mod_init);
2814 module_exit(sensor_mod_exit);
2815
2816 MODULE_DESCRIPTION(SENSOR_NAME_STRING(Camera sensor driver));
2817 MODULE_AUTHOR("ddl <kernel@rock-chips>");
2818 MODULE_LICENSE("GPL");
2819
2820