drivers/media/video/ov7690.c

   1 /*
   2 o* Driver for MT9M001 CMOS Image Sensor from Micron
   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/miscdevice.h>
  19 #include <media/v4l2-common.h>
  20 #include <media/v4l2-chip-ident.h>
  21 #include <media/soc_camera.h>
  22 #include <mach/rk_camera.h>
  23 #include <linux/vmalloc.h>
  24 static int debug;
  25 module_param(debug, int, S_IRUGO|S_IWUSR);
  26
  27 #define dprintk(level, fmt, arg...) do {                        \
  28         if (debug >= level)                                     \
  29         printk(KERN_WARNING fmt , ## arg); } while (0)
  30
  31 #define SENSOR_TR(format, ...) printk(KERN_ERR format, ## __VA_ARGS__)
  32 #define SENSOR_DG(format, ...) dprintk(1, format, ## __VA_ARGS__)
  33
  34
  35 #define _CONS(a,b) a##b
  36 #define CONS(a,b) _CONS(a,b)
  37
  38 #define __STR(x) #x
  39 #define _STR(x) __STR(x)
  40 #define STR(x) _STR(x)
  41
  42 #define MIN(x,y)   ((x<y) ? x: y)
  43 #define MAX(x,y)    ((x>y) ? x: y)
  44
  45 /* Sensor Driver Configuration */
  46 #define SENSOR_NAME RK_CAM_SENSOR_OV7690
  47 #define SENSOR_V4L2_IDENT V4L2_IDENT_OV7690
  48 #define SENSOR_ID 0x76
  49 #define SENSOR_MIN_WIDTH    176
  50 #define SENSOR_MIN_HEIGHT   144
  51 #define SENSOR_MAX_WIDTH    640
  52 #define SENSOR_MAX_HEIGHT   480
  53 #define SENSOR_INIT_WIDTH       sensor_init_width                       /* Sensor pixel size for sensor_init_data array */
  54 #define SENSOR_INIT_HEIGHT  sensor_init_height
  55 #define SENSOR_INIT_WINSEQADR  sensor_init_winseq_p
  56 #define SENSOR_INIT_PIXFMT sensor_init_pixelcode
  57 #define SENSOR_BUS_PARAM  sensor_init_busparam
  58
  59 #define CONFIG_SENSOR_WhiteBalance      1
  60 #define CONFIG_SENSOR_Brightness        0
  61 #define CONFIG_SENSOR_Contrast      0
  62 #define CONFIG_SENSOR_Saturation    0
  63 #define CONFIG_SENSOR_Effect        1
  64 #define CONFIG_SENSOR_Scene         0
  65 #define CONFIG_SENSOR_DigitalZoom   0
  66 #define CONFIG_SENSOR_Focus         0
  67 #define CONFIG_SENSOR_Exposure      0
  68 #define CONFIG_SENSOR_Flash         0
  69 #define CONFIG_SENSOR_Mirror        0
  70 #define CONFIG_SENSOR_Flip          0
  71
  72 #define CONFIG_SENSOR_I2C_SPEED     250000       /* Hz */
  73 /* Sensor write register continues by preempt_disable/preempt_enable for current process not be scheduled */
  74 #define CONFIG_SENSOR_I2C_NOSCHED   0
  75 #define CONFIG_SENSOR_I2C_RDWRCHK   0
  76
  77 #define COLOR_TEMPERATURE_CLOUDY_DN  6500
  78 #define COLOR_TEMPERATURE_CLOUDY_UP    8000
  79 #define COLOR_TEMPERATURE_CLEARDAY_DN  5000
  80 #define COLOR_TEMPERATURE_CLEARDAY_UP    6500
  81 #define COLOR_TEMPERATURE_OFFICE_DN     3500
  82 #define COLOR_TEMPERATURE_OFFICE_UP     5000
  83 #define COLOR_TEMPERATURE_HOME_DN       2500
  84 #define COLOR_TEMPERATURE_HOME_UP       3500
  85
  86 #define SENSOR_NAME_STRING(a) STR(CONS(SENSOR_NAME, a))
  87 #define SENSOR_NAME_VARFUN(a) CONS(SENSOR_NAME, a)
  88
  89 struct reginfo
  90 {
  91     u8 reg;
  92     u8 val;
  93 };
  94
  95 static s32 sensor_init_width = 0;
  96 static s32 sensor_init_height = 0;
  97 static unsigned long sensor_init_busparam = (SOCAM_MASTER | SOCAM_PCLK_SAMPLE_RISING|SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_LOW |SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8  |SOCAM_MCLK_24MHZ);
  98 static enum v4l2_mbus_pixelcode sensor_init_pixelcode = V4L2_MBUS_FMT_YUYV8_2X8;
  99 static struct reginfo* sensor_init_data_p = NULL;
 100 static struct reginfo* sensor_init_winseq_p = NULL;
 101 static struct reginfo* sensor_init_winseq_board = NULL;
 102 /* init 800*600 SVGA */
 103 static struct reginfo sensor_init_data[] =
 104 {
 105         {0x12, 0x80},
 106         {0x49, 0x0c},
 107         {0x0c, 0x06},
 108         {0x48, 0x42},
 109         {0x41, 0x43},
 110         {0x4c, 0x73},
 111         {0x81, 0xff},
 112         {0x21, 0x44},
 113         {0x16, 0x03},
 114         {0x39, 0x80},
 115         {0x1e, 0xb1},
 116 // formate
 117         {0x12, 0x00},
 118         {0x82, 0x03},
 119         {0xd0, 0x48},
 120         {0x80, 0x7F},
 121         {0x3e, 0x30},
 122         {0x22, 0x00},
 123 // resolution
 124         {0x17, 0x69},
 125         {0x18, 0xa4},
 126         {0x19, 0x0c},
 127         {0x1a, 0xf6},
 128
 129         {0xc8, 0x02},
 130         {0xc9, 0x80},
 131         {0xca, 0x01},
 132         {0xcb, 0xe0},
 133
 134         {0xcc, 0x02},
 135         {0xcd, 0x80},
 136         {0xce, 0x01},
 137         {0xcf, 0xe0},
 138 // Lens Correction
 139         {0x85, 0x90},
 140         {0x86, 0x00},
 141         {0x87, 0x00},
 142         {0x88, 0x10},
 143         {0x89, 0x30},
 144         {0x8a, 0x29},
 145         {0x8b, 0x26},
 146 // Color Matrix
 147         {0xbb, 0x80},
 148         {0xbc, 0x62},
 149         {0xbd, 0x1e},
 150         {0xbe, 0x26},
 151         {0xbf, 0x7b},
 152         {0xc0, 0xac},
 153         {0xc1, 0x1e},
 154 // Edge + Denoise
 155         {0xb7, 0x05c},
 156         {0xb8, 0x09},
 157         {0xb9, 0x00},       //0x07//0x27
 158         {0xba, 0x18},
 159 //      UVAdjust
 160         {0x5a, 0x4a},
 161         {0x5b, 0x9f},
 162         {0x5c, 0x48},
 163         {0x5d, 0x32},
 164 // AEC/AGC target
 165         {0x24, 0x78},
 166         {0x25, 0x68},
 167         {0x26, 0xb3},
 168 // Gamma
 169         {0xa3, 0x0b},
 170         {0xa4, 0x15},
 171         {0xa5, 0x2a},
 172         {0xa6, 0x51},
 173         {0xa7, 0x63},
 174         {0xa8, 0x74},
 175         {0xa9, 0x83},
 176         {0xaa, 0x91},
 177         {0xab, 0x9e},
 178         {0xac, 0xaa},
 179         {0xad, 0xbe},
 180         {0xae, 0xce},
 181         {0xaf, 0xe5},
 182         {0xb0, 0xf3},
 183         {0xb1, 0xfb},
 184         {0xb2, 0x06},
 185 //simple
 186         {0x8e, 0x92},
 187         {0x96, 0xff},
 188         {0x97, 0x00},
 189 // Advance
 190         {0x8c, 0x5d},
 191         {0x8d, 0x11},
 192         {0x8e, 0x12},
 193         {0x8f, 0x11},
 194         {0x90, 0x50},
 195         {0x91, 0x22},
 196         {0x92, 0xd1},
 197         {0x93, 0xa7},
 198         {0x94, 0x23},
 199         {0x95, 0x3b},
 200         {0x96, 0xff},
 201         {0x97, 0x00},
 202         {0x98, 0x4a},
 203         {0x99, 0x46},
 204         {0x9a, 0x3d},
 205         {0x9b, 0x3a},
 206         {0x9c, 0xf0},
 207         {0x9d, 0xf0},
 208         {0x9e, 0xf0},
 209         {0x9f, 0xff},
 210         {0xa0, 0x56},
 211         {0xa1, 0x55},
 212         {0xa2, 0x13},
 213 // General Control
 214         {0x50, 0x4c},
 215         {0x51, 0x3f},
 216         {0x21, 0x57},
 217         {0x20, 0x00},
 218
 219         {0x14, 0x29},
 220         {0x13, 0xf7},
 221         {0x11, 0x01},
 222         {0xd2, 0x06},
 223
 224         {0x2a, 0x30},
 225         {0x2b, 0x0b},
 226         {0x2c, 0x00}
 227 };
 228
 229 /* 1280x720 */
 230 static struct reginfo sensor_720p[]=
 231 {
 232         {0x00 ,0x00}
 233
 234 };
 235
 236 /* 1600X1200 UXGA */
 237 static struct reginfo sensor_uxga[] =
 238 {
 239         {0x00, 0x00}
 240 };
 241
 242 /* 1280X1024 SXGA */
 243 static struct reginfo sensor_sxga[] =
 244 {
 245         {0x0, 0x0}
 246 };
 247 /* 1024X768 SXGA */
 248 static struct reginfo sensor_xga[] =
 249 {
 250         {0x0, 0x0}
 251 };
 252 /* 800X600 SVGA*/
 253 static struct reginfo sensor_svga[] =
 254 {
 255         {0x00, 0x00}
 256 };
 257
 258 /* 640X480 VGA */
 259 static struct reginfo sensor_vga[] =
 260 {
 261                 {0x16,0x03},
 262                 {0x17,0x69},
 263                 {0x18,0xa4},
 264                 {0x19,0x0c},
 265                 {0x1a,0xf6},
 266                 {0x22,0x00},
 267                 {0xc8,0x02},
 268                 {0xc9,0x80},
 269                 {0xca,0x01},
 270                 {0xcb,0xe0},
 271                 {0xcc,0x02},
 272                 {0xcd,0x80},
 273                 {0xce,0x01},
 274                 {0xcf,0xe0},
 275         {0x00,0x00}
 276 };
 277
 278 /* 352X288 CIF */
 279 static struct reginfo sensor_cif[] =
 280 {
 281     {0x00,0x00}
 282 };
 283
 284 /* 320*240 QVGA */
 285 static  struct reginfo sensor_qvga[] =
 286 {
 287                 {0x16,0x03},
 288                 {0x17,0x69},
 289                 {0x18,0xa4},
 290                 {0x19,0x06},
 291                 {0x1a,0xf6},
 292                 {0x22,0x10},
 293                 {0xc8,0x02},
 294                 {0xc9,0x80},
 295                 {0xca,0x00},
 296                 {0xcb,0xf0},
 297                 {0xcc,0x01},
 298                 {0xcd,0x40},
 299                 {0xce,0x00},
 300                 {0xcf,0xf0},
 301     {0x00,0x00}
 302 };
 303
 304 /* 176X144 QCIF*/
 305 static struct reginfo sensor_qcif[] =
 306 {
 307                 {0x16,0x40},
 308                 {0x17,0x83},
 309                 {0x18,0x96},
 310                 {0x19,0x06},
 311                 {0x1a,0xf6},
 312                 {0x22,0x10},
 313                 {0xc8,0x02},
 314                 {0xc9,0x4b},
 315                 {0xca,0x00},
 316                 {0xcb,0xf0},
 317                 {0xcc,0x00},
 318                 {0xcd,0xb0},
 319                 {0xce,0x00},
 320                 {0xcf,0x90},
 321     {0x00,0x00}
 322 };
 323 #if 0
 324 /* 160X120 QQVGA*/
 325 static struct reginfo ov2655_qqvga[] =
 326 {
 327     {0x0, 0x0},
 328 };
 329
 330
 331
 332 static  struct reginfo ov2655_Sharpness_auto[] =
 333 {
 334     {0x3306, 0x00},
 335 };
 336
 337 static  struct reginfo ov2655_Sharpness1[] =
 338 {
 339     {0x3306, 0x08},
 340     {0x3371, 0x00},
 341 };
 342
 343 static  struct reginfo ov2655_Sharpness2[][3] =
 344 {
 345     //Sharpness 2
 346     {0x3306, 0x08},
 347     {0x3371, 0x01},
 348 };
 349
 350 static  struct reginfo ov2655_Sharpness3[] =
 351 {
 352     //default
 353     {0x3306, 0x08},
 354     {0x332d, 0x02},
 355 };
 356 static  struct reginfo ov2655_Sharpness4[]=
 357 {
 358     //Sharpness 4
 359     {0x3306, 0x08},
 360     {0x332d, 0x03},
 361 };
 362
 363 static  struct reginfo ov2655_Sharpness5[] =
 364 {
 365     //Sharpness 5
 366     {0x3306, 0x08},
 367     {0x332d, 0x04},
 368 };
 369 #endif
 370
 371 static  struct reginfo sensor_ClrFmt_YUYV[]=
 372 {
 373     {0x00, 0x00}
 374 };
 375
 376 static  struct reginfo sensor_ClrFmt_UYVY[]=
 377 {
 378     {0x00, 0x00}
 379 };
 380
 381
 382 #if CONFIG_SENSOR_WhiteBalance
 383 static  struct reginfo sensor_WhiteB_Auto[]=
 384 {
 385                 {0x13, 0xf7},
 386     {0x00, 0x00}
 387 };
 388 /* Cloudy Colour Temperature : 6500K - 8000K  */
 389 static  struct reginfo sensor_WhiteB_Cloudy[]=
 390 {
 391                 {0x13, 0xf5},
 392                 {0x01, 0x40},
 393                 {0x02, 0x5d},
 394                 {0x03, 0x40},
 395     {0x00, 0x00}
 396 };
 397 /* ClearDay Colour Temperature : 5000K - 6500K  */
 398 static  struct reginfo sensor_WhiteB_ClearDay[]=
 399 {
 400                 {0x13, 0xf5},
 401                 {0x01, 0x44},
 402                 {0x02, 0x55},
 403                 {0x03, 0x40},
 404     {0x00, 0x00}
 405 };
 406 /* Office Colour Temperature : 3500K - 5000K  */
 407 static  struct reginfo sensor_WhiteB_TungstenLamp1[]=
 408 {
 409                 {0x13, 0xf5},
 410                 {0x01, 0x5b},
 411                 {0x02, 0x4c},
 412                 {0x03, 0x40},
 413     {0x00, 0x00}
 414
 415 };
 416 /* Home Colour Temperature : 2500K - 3500K  */
 417 static  struct reginfo sensor_WhiteB_TungstenLamp2[]=
 418 {
 419                 {0x13, 0xf5},
 420                 {0x01, 0x62},
 421                 {0x02, 0x40},
 422                 {0x03, 0x41},
 423     {0x00, 0x00}
 424 };
 425 static struct reginfo *sensor_WhiteBalanceSeqe[] = {sensor_WhiteB_Auto, sensor_WhiteB_TungstenLamp1,sensor_WhiteB_TungstenLamp2,
 426     sensor_WhiteB_ClearDay, sensor_WhiteB_Cloudy,NULL,
 427 };
 428 #endif
 429
 430 #if CONFIG_SENSOR_Brightness
 431 static  struct reginfo sensor_Brightness0[]=
 432 {
 433     // Brightness -2
 434     {0x0000, 0x00}
 435 };
 436
 437 static  struct reginfo sensor_Brightness1[]=
 438 {
 439     // Brightness -1
 440
 441     {0x0000, 0x00}
 442 };
 443
 444 static  struct reginfo sensor_Brightness2[]=
 445 {
 446     //  Brightness 0
 447
 448     {0x0000, 0x00}
 449 };
 450
 451 static  struct reginfo sensor_Brightness3[]=
 452 {
 453     // Brightness +1
 454
 455     {0x0000, 0x00}
 456 };
 457
 458 static  struct reginfo sensor_Brightness4[]=
 459 {
 460     //  Brightness +2
 461
 462     {0x0000, 0x00}
 463 };
 464
 465 static  struct reginfo sensor_Brightness5[]=
 466 {
 467     //  Brightness +3
 468
 469     {0x0000, 0x00}
 470 };
 471 static struct reginfo *sensor_BrightnessSeqe[] = {sensor_Brightness0, sensor_Brightness1, sensor_Brightness2, sensor_Brightness3,
 472     sensor_Brightness4, sensor_Brightness5,NULL,
 473 };
 474
 475 #endif
 476
 477 #if CONFIG_SENSOR_Effect
 478 static  struct reginfo sensor_Effect_Normal[] =
 479 {
 480         {0xd2,0x06},
 481     {0x00, 0x00}
 482 };
 483
 484 static  struct reginfo sensor_Effect_WandB[] =
 485 {
 486         {0xd2,0x1e},
 487         {0xda,0x80},
 488         {0xdb,0x80},
 489     {0x00, 0x00}
 490 };
 491
 492 static  struct reginfo sensor_Effect_Sepia[] =
 493 {
 494         {0xd2,0x1e},
 495         {0xda,0x40},
 496         {0xdb,0xa0},
 497     {0x00, 0x00}
 498 };
 499
 500 static  struct reginfo sensor_Effect_Negative[] =
 501 {
 502     //Negative
 503         {0xd2,0x46},
 504     {0x00, 0x00}
 505 };
 506 static  struct reginfo sensor_Effect_Bluish[] =
 507 {
 508     // Bluish
 509         {0xd2,0x1e},
 510         {0xda,0xa0},
 511         {0xdb,0x40},
 512     {0x00, 0x00}
 513 };
 514
 515 static  struct reginfo sensor_Effect_Green[] =
 516 {
 517     //  Greenish
 518         {0xd2,0x1e},
 519         {0xda,0x60},
 520         {0xdb,0x60},
 521     {0x00, 0x00}
 522 };
 523 static struct reginfo *sensor_EffectSeqe[] = {sensor_Effect_Normal, sensor_Effect_WandB, sensor_Effect_Negative,sensor_Effect_Sepia,
 524     sensor_Effect_Bluish, sensor_Effect_Green,NULL,
 525 };
 526 #endif
 527 #if CONFIG_SENSOR_Exposure
 528 static  struct reginfo sensor_Exposure0[]=
 529 {
 530     {0x00, 0x00}
 531 };
 532
 533 static  struct reginfo sensor_Exposure1[]=
 534 {
 535     {0x00, 0x00}
 536 };
 537
 538 static  struct reginfo sensor_Exposure2[]=
 539 {
 540     {0x00, 0x00}
 541 };
 542
 543 static  struct reginfo sensor_Exposure3[]=
 544 {
 545     {0x00, 0x00}
 546 };
 547
 548 static  struct reginfo sensor_Exposure4[]=
 549 {
 550     {0x00, 0x00}
 551 };
 552
 553 static  struct reginfo sensor_Exposure5[]=
 554 {
 555     {0x00, 0x00}
 556 };
 557
 558 static  struct reginfo sensor_Exposure6[]=
 559 {
 560     {0x00, 0x00}
 561 };
 562
 563 static struct reginfo *sensor_ExposureSeqe[] = {sensor_Exposure0, sensor_Exposure1, sensor_Exposure2, sensor_Exposure3,
 564     sensor_Exposure4, sensor_Exposure5,sensor_Exposure6,NULL,
 565 };
 566 #endif
 567 #if CONFIG_SENSOR_Saturation
 568 static  struct reginfo sensor_Saturation0[]=
 569 {
 570     {0x00, 0x00}
 571 };
 572
 573 static  struct reginfo sensor_Saturation1[]=
 574 {
 575     {0x00, 0x00}
 576 };
 577
 578 static  struct reginfo sensor_Saturation2[]=
 579 {
 580     {0x00, 0x00}
 581 };
 582 static struct reginfo *sensor_SaturationSeqe[] = {sensor_Saturation0, sensor_Saturation1, sensor_Saturation2, NULL,};
 583
 584 #endif
 585 #if CONFIG_SENSOR_Contrast
 586 static  struct reginfo sensor_Contrast0[]=
 587 {
 588     {0x00, 0x00}
 589 };
 590
 591 static  struct reginfo sensor_Contrast1[]=
 592 {
 593     {0x00, 0x00}
 594 };
 595
 596 static  struct reginfo sensor_Contrast2[]=
 597 {
 598     {0x00, 0x00}
 599 };
 600
 601 static  struct reginfo sensor_Contrast3[]=
 602 {
 603     {0x00, 0x00}
 604 };
 605
 606 static  struct reginfo sensor_Contrast4[]=
 607 {
 608     {0x00, 0x00}
 609 };
 610
 611
 612 static  struct reginfo sensor_Contrast5[]=
 613 {
 614     {0x00, 0x00}
 615 };
 616
 617 static  struct reginfo sensor_Contrast6[]=
 618 {
 619     {0x00, 0x00}
 620 };
 621 static struct reginfo *sensor_ContrastSeqe[] = {sensor_Contrast0, sensor_Contrast1, sensor_Contrast2, sensor_Contrast3,
 622     sensor_Contrast4, sensor_Contrast5, sensor_Contrast6, NULL,
 623 };
 624
 625 #endif
 626 #if CONFIG_SENSOR_Mirror
 627 static  struct reginfo sensor_MirrorOn[]=
 628 {
 629     {0x00, 0x00}
 630 };
 631
 632 static  struct reginfo sensor_MirrorOff[]=
 633 {
 634     {0x00, 0x00}
 635 };
 636 static struct reginfo *sensor_MirrorSeqe[] = {sensor_MirrorOff, sensor_MirrorOn,NULL,};
 637 #endif
 638 #if CONFIG_SENSOR_Flip
 639 static  struct reginfo sensor_FlipOn[]=
 640 {
 641     {0x00, 0x00}
 642 };
 643
 644 static  struct reginfo sensor_FlipOff[]=
 645 {
 646     {0x00, 0x00}
 647 };
 648 static struct reginfo *sensor_FlipSeqe[] = {sensor_FlipOff, sensor_FlipOn,NULL,};
 649
 650 #endif
 651 #if CONFIG_SENSOR_Scene
 652 static  struct reginfo sensor_SceneAuto[] =
 653 {
 654 {0x00, 0x00}
 655 };
 656
 657 static  struct reginfo sensor_SceneNight[] =
 658 {
 659 {0x00, 0x00}
 660 };
 661 static struct reginfo *sensor_SceneSeqe[] = {sensor_SceneAuto, sensor_SceneNight,NULL,};
 662
 663 #endif
 664 #if CONFIG_SENSOR_DigitalZoom
 665 static struct reginfo sensor_Zoom0[] =
 666 {
 667     {0x0, 0x0},
 668 };
 669
 670 static struct reginfo sensor_Zoom1[] =
 671 {
 672      {0x0, 0x0},
 673 };
 674
 675 static struct reginfo sensor_Zoom2[] =
 676 {
 677     {0x0, 0x0},
 678 };
 679
 680
 681 static struct reginfo sensor_Zoom3[] =
 682 {
 683     {0x0, 0x0},
 684 };
 685 static struct reginfo *sensor_ZoomSeqe[] = {sensor_Zoom0, sensor_Zoom1, sensor_Zoom2, sensor_Zoom3, NULL,};
 686 #endif
 687 static const struct v4l2_querymenu sensor_menus[] =
 688 {
 689         #if CONFIG_SENSOR_WhiteBalance
 690     { .id = V4L2_CID_DO_WHITE_BALANCE,  .index = 0,  .name = "auto",  .reserved = 0, }, {  .id = V4L2_CID_DO_WHITE_BALANCE,  .index = 1, .name = "incandescent",  .reserved = 0,},
 691     { .id = V4L2_CID_DO_WHITE_BALANCE,  .index = 2,  .name = "fluorescent", .reserved = 0,}, {  .id = V4L2_CID_DO_WHITE_BALANCE, .index = 3,  .name = "daylight", .reserved = 0,},
 692     { .id = V4L2_CID_DO_WHITE_BALANCE,  .index = 4,  .name = "cloudy-daylight", .reserved = 0,},
 693     #endif
 694
 695         #if CONFIG_SENSOR_Effect
 696     { .id = V4L2_CID_EFFECT,  .index = 0,  .name = "none",  .reserved = 0, }, {  .id = V4L2_CID_EFFECT,  .index = 1, .name = "mono",  .reserved = 0,},
 697     { .id = V4L2_CID_EFFECT,  .index = 2,  .name = "negative", .reserved = 0,}, {  .id = V4L2_CID_EFFECT, .index = 3,  .name = "sepia", .reserved = 0,},
 698     { .id = V4L2_CID_EFFECT,  .index = 4, .name = "posterize", .reserved = 0,} ,{ .id = V4L2_CID_EFFECT,  .index = 5,  .name = "aqua", .reserved = 0,},
 699     #endif
 700
 701         #if CONFIG_SENSOR_Scene
 702     { .id = V4L2_CID_SCENE,  .index = 0, .name = "auto", .reserved = 0,} ,{ .id = V4L2_CID_SCENE,  .index = 1,  .name = "night", .reserved = 0,},
 703     #endif
 704
 705         #if CONFIG_SENSOR_Flash
 706     { .id = V4L2_CID_FLASH,  .index = 0,  .name = "off",  .reserved = 0, }, {  .id = V4L2_CID_FLASH,  .index = 1, .name = "auto",  .reserved = 0,},
 707     { .id = V4L2_CID_FLASH,  .index = 2,  .name = "on", .reserved = 0,}, {  .id = V4L2_CID_FLASH, .index = 3,  .name = "torch", .reserved = 0,},
 708     #endif
 709 };
 710
 711 static const struct v4l2_queryctrl sensor_controls[] =
 712 {
 713         #if CONFIG_SENSOR_WhiteBalance
 714     {
 715         .id             = V4L2_CID_DO_WHITE_BALANCE,
 716         .type           = V4L2_CTRL_TYPE_MENU,
 717         .name           = "White Balance Control",
 718         .minimum        = 0,
 719         .maximum        = 4,
 720         .step           = 1,
 721         .default_value = 0,
 722     },
 723     #endif
 724
 725         #if CONFIG_SENSOR_Brightness
 726         {
 727         .id             = V4L2_CID_BRIGHTNESS,
 728         .type           = V4L2_CTRL_TYPE_INTEGER,
 729         .name           = "Brightness Control",
 730         .minimum        = -3,
 731         .maximum        = 2,
 732         .step           = 1,
 733         .default_value = 0,
 734     },
 735     #endif
 736
 737         #if CONFIG_SENSOR_Effect
 738         {
 739         .id             = V4L2_CID_EFFECT,
 740         .type           = V4L2_CTRL_TYPE_MENU,
 741         .name           = "Effect Control",
 742         .minimum        = 0,
 743         .maximum        = 5,
 744         .step           = 1,
 745         .default_value = 0,
 746     },
 747         #endif
 748
 749         #if CONFIG_SENSOR_Exposure
 750         {
 751         .id             = V4L2_CID_EXPOSURE,
 752         .type           = V4L2_CTRL_TYPE_INTEGER,
 753         .name           = "Exposure Control",
 754         .minimum        = 0,
 755         .maximum        = 6,
 756         .step           = 1,
 757         .default_value = 0,
 758     },
 759         #endif
 760
 761         #if CONFIG_SENSOR_Saturation
 762         {
 763         .id             = V4L2_CID_SATURATION,
 764         .type           = V4L2_CTRL_TYPE_INTEGER,
 765         .name           = "Saturation Control",
 766         .minimum        = 0,
 767         .maximum        = 2,
 768         .step           = 1,
 769         .default_value = 0,
 770     },
 771     #endif
 772
 773         #if CONFIG_SENSOR_Contrast
 774         {
 775         .id             = V4L2_CID_CONTRAST,
 776         .type           = V4L2_CTRL_TYPE_INTEGER,
 777         .name           = "Contrast Control",
 778         .minimum        = -3,
 779         .maximum        = 3,
 780         .step           = 1,
 781         .default_value = 0,
 782     },
 783         #endif
 784
 785         #if CONFIG_SENSOR_Mirror
 786         {
 787         .id             = V4L2_CID_HFLIP,
 788         .type           = V4L2_CTRL_TYPE_BOOLEAN,
 789         .name           = "Mirror Control",
 790         .minimum        = 0,
 791         .maximum        = 1,
 792         .step           = 1,
 793         .default_value = 0,
 794     },
 795     #endif
 796
 797         #if CONFIG_SENSOR_Flip
 798         {
 799         .id             = V4L2_CID_VFLIP,
 800         .type           = V4L2_CTRL_TYPE_BOOLEAN,
 801         .name           = "Flip Control",
 802         .minimum        = 0,
 803         .maximum        = 1,
 804         .step           = 1,
 805         .default_value = 0,
 806     },
 807     #endif
 808
 809         #if CONFIG_SENSOR_Scene
 810     {
 811         .id             = V4L2_CID_SCENE,
 812         .type           = V4L2_CTRL_TYPE_MENU,
 813         .name           = "Scene Control",
 814         .minimum        = 0,
 815         .maximum        = 1,
 816         .step           = 1,
 817         .default_value = 0,
 818     },
 819     #endif
 820
 821         #if CONFIG_SENSOR_DigitalZoom
 822     {
 823         .id             = V4L2_CID_ZOOM_RELATIVE,
 824         .type           = V4L2_CTRL_TYPE_INTEGER,
 825         .name           = "DigitalZoom Control",
 826         .minimum        = -1,
 827         .maximum        = 1,
 828         .step           = 1,
 829         .default_value = 0,
 830     }, {
 831         .id             = V4L2_CID_ZOOM_ABSOLUTE,
 832         .type           = V4L2_CTRL_TYPE_INTEGER,
 833         .name           = "DigitalZoom Control",
 834         .minimum        = 0,
 835         .maximum        = 3,
 836         .step           = 1,
 837         .default_value = 0,
 838     },
 839     #endif
 840
 841         #if CONFIG_SENSOR_Focus
 842         {
 843         .id             = V4L2_CID_FOCUS_RELATIVE,
 844         .type           = V4L2_CTRL_TYPE_INTEGER,
 845         .name           = "Focus Control",
 846         .minimum        = -1,
 847         .maximum        = 1,
 848         .step           = 1,
 849         .default_value = 0,
 850     }, {
 851         .id             = V4L2_CID_FOCUS_ABSOLUTE,
 852         .type           = V4L2_CTRL_TYPE_INTEGER,
 853         .name           = "Focus Control",
 854         .minimum        = 0,
 855         .maximum        = 255,
 856         .step           = 1,
 857         .default_value = 125,
 858     },
 859     #endif
 860
 861         #if CONFIG_SENSOR_Flash
 862         {
 863         .id             = V4L2_CID_FLASH,
 864         .type           = V4L2_CTRL_TYPE_MENU,
 865         .name           = "Flash Control",
 866         .minimum        = 0,
 867         .maximum        = 3,
 868         .step           = 1,
 869         .default_value = 0,
 870     },
 871         #endif
 872 };
 873
 874 static int sensor_probe(struct i2c_client *client, const struct i2c_device_id *did);
 875 static int sensor_video_probe(struct soc_camera_device *icd, struct i2c_client *client);
 876 static int sensor_g_control(struct v4l2_subdev *sd, struct v4l2_control *ctrl);
 877 static int sensor_s_control(struct v4l2_subdev *sd, struct v4l2_control *ctrl);
 878 static int sensor_g_ext_controls(struct v4l2_subdev *sd,  struct v4l2_ext_controls *ext_ctrl);
 879 static int sensor_s_ext_controls(struct v4l2_subdev *sd,  struct v4l2_ext_controls *ext_ctrl);
 880 static int sensor_suspend(struct soc_camera_device *icd, pm_message_t pm_msg);
 881 static int sensor_resume(struct soc_camera_device *icd);
 882 static int sensor_set_bus_param(struct soc_camera_device *icd,unsigned long flags);
 883 static unsigned long sensor_query_bus_param(struct soc_camera_device *icd);
 884 #if CONFIG_SENSOR_Effect
 885 static int sensor_set_effect(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value);
 886 #endif
 887 #if CONFIG_SENSOR_WhiteBalance
 888 static int sensor_set_whiteBalance(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value);
 889 #endif
 890 static int sensor_deactivate(struct i2c_client *client);
 891
 892 static struct soc_camera_ops sensor_ops =
 893 {
 894     .suspend                     = sensor_suspend,
 895     .resume                       = sensor_resume,
 896     .set_bus_param              = sensor_set_bus_param,
 897     .query_bus_param    = sensor_query_bus_param,
 898     .controls           = sensor_controls,
 899     .menus                         = sensor_menus,
 900     .num_controls               = ARRAY_SIZE(sensor_controls),
 901     .num_menus          = ARRAY_SIZE(sensor_menus),
 902 };
 903
 904 /* only one fixed colorspace per pixelcode */
 905 struct sensor_datafmt {
 906         enum v4l2_mbus_pixelcode code;
 907         enum v4l2_colorspace colorspace;
 908 };
 909
 910 /* Find a data format by a pixel code in an array */
 911 static const struct sensor_datafmt *sensor_find_datafmt(
 912         enum v4l2_mbus_pixelcode code, const struct sensor_datafmt *fmt,
 913         int n)
 914 {
 915         int i;
 916         for (i = 0; i < n; i++)
 917                 if (fmt[i].code == code)
 918                         return fmt + i;
 919
 920         return NULL;
 921 }
 922
 923 static const struct sensor_datafmt sensor_colour_fmts[] = {
 924     {V4L2_MBUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG},
 925     {V4L2_MBUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG}
 926 };
 927
 928 typedef struct sensor_info_priv_s
 929 {
 930     int whiteBalance;
 931     int brightness;
 932     int contrast;
 933     int saturation;
 934     int effect;
 935     int scene;
 936     int digitalzoom;
 937     int focus;
 938     int flash;
 939     int exposure;
 940         bool snap2preview;
 941         bool video2preview;
 942     unsigned char mirror;                                        /* HFLIP */
 943     unsigned char flip;                                          /* VFLIP */
 944     unsigned int winseqe_cur_addr;
 945     struct sensor_datafmt fmt;
 946     unsigned int funmodule_state;
 947 } sensor_info_priv_t;
 948
 949 struct sensor
 950 {
 951     struct v4l2_subdev subdev;
 952     struct i2c_client *client;
 953     sensor_info_priv_t info_priv;
 954     int model;  /* V4L2_IDENT_OV* codes from v4l2-chip-ident.h */
 955 #if CONFIG_SENSOR_I2C_NOSCHED
 956         atomic_t tasklock_cnt;
 957 #endif
 958         struct rkcamera_platform_data *sensor_io_request;
 959     struct rkcamera_gpio_res *sensor_gpio_res;
 960 };
 961
 962
 963 static struct sensor* to_sensor(const struct i2c_client *client)
 964 {
 965     return container_of(i2c_get_clientdata(client), struct sensor, subdev);
 966 }
 967
 968 static int sensor_task_lock(struct i2c_client *client, int lock)
 969 {
 970 #if CONFIG_SENSOR_I2C_NOSCHED
 971         int cnt = 3;
 972     struct sensor *sensor = to_sensor(client);
 973
 974         if (lock) {
 975                 if (atomic_read(&sensor->tasklock_cnt) == 0) {
 976                         while ((atomic_read(&client->adapter->bus_lock.count) < 1) && (cnt>0)) {
 977                                 SENSOR_TR("\n %s will obtain i2c in atomic, but i2c bus is locked! Wait...\n",SENSOR_NAME_STRING());
 978                                 msleep(35);
 979                                 cnt--;
 980                         }
 981                         if ((atomic_read(&client->adapter->bus_lock.count) < 1) && (cnt<=0)) {
 982                                 SENSOR_TR("\n %s obtain i2c fail in atomic!!\n",SENSOR_NAME_STRING());
 983                                 goto sensor_task_lock_err;
 984                         }
 985                         preempt_disable();
 986                 }
 987
 988                 atomic_add(1, &sensor->tasklock_cnt);
 989         } else {
 990                 if (atomic_read(&sensor->tasklock_cnt) > 0) {
 991                         atomic_sub(1, &sensor->tasklock_cnt);
 992
 993                         if (atomic_read(&sensor->tasklock_cnt) == 0)
 994                                 preempt_enable();
 995                 }
 996         }
 997         return 0;
 998 sensor_task_lock_err:
 999         return -1;
1000 #else
1001     return 0;
1002 #endif
1003
1004 }
1005
1006 /* sensor register write */
1007 static int sensor_write(struct i2c_client *client, u8 reg, u8 val)
1008 {
1009     int err,cnt;
1010     u8 buf[2];
1011     struct i2c_msg msg[1];
1012
1013     buf[0] = reg & 0xFF;
1014     buf[1] = val;
1015
1016     msg->addr = client->addr;
1017     msg->flags = client->flags;
1018     msg->buf = buf;
1019     msg->len = sizeof(buf);
1020     msg->scl_rate = CONFIG_SENSOR_I2C_SPEED;                                        /* ddl@rock-chips.com : 100kHz */
1021     msg->read_type = 0;               /* fpga i2c:0==I2C_NORMAL : direct use number not enum for don't want include spi_fpga.h */
1022
1023     cnt = 3;
1024     err = -EAGAIN;
1025
1026     while ((cnt-- > 0) && (err < 0)) {                       /* ddl@rock-chips.com :  Transfer again if transent is failed   */
1027         err = i2c_transfer(client->adapter, msg, 1);
1028                                 udelay(50);
1029         if (err >= 0) {
1030             return 0;
1031         } else {
1032                 SENSOR_TR("\n %s write reg(0x%x, val:0x%x) failed, try to write again!\n",SENSOR_NAME_STRING(),reg, val);
1033                 udelay(10);
1034         }
1035     }
1036
1037     return err;
1038 }
1039
1040 /* sensor register read */
1041 static int sensor_read(struct i2c_client *client, u8 reg, u8 *val)
1042 {
1043     int err,cnt;
1044     u8 buf[1];
1045     struct i2c_msg msg[2];
1046
1047     buf[0] = reg;
1048
1049     msg[0].addr = client->addr;
1050     msg[0].flags = client->flags;
1051     msg[0].buf = buf;
1052     msg[0].len = sizeof(buf);
1053     msg[0].scl_rate = CONFIG_SENSOR_I2C_SPEED;       /* ddl@rock-chips.com : 100kHz */
1054     msg[0].read_type = 2;   /* fpga i2c:0==I2C_NO_STOP : direct use number not enum for don't want include spi_fpga.h */
1055
1056     msg[1].addr = client->addr;
1057     msg[1].flags = client->flags|I2C_M_RD;
1058     msg[1].buf = buf;
1059     msg[1].len = 1;
1060     msg[1].scl_rate = CONFIG_SENSOR_I2C_SPEED;                       /* ddl@rock-chips.com : 100kHz */
1061     msg[1].read_type = 2;                             /* fpga i2c:0==I2C_NO_STOP : direct use number not enum for don't want include spi_fpga.h */
1062
1063     cnt = 1;
1064     err = -EAGAIN;
1065     while ((cnt-- > 0) && (err < 0)) {                       /* ddl@rock-chips.com :  Transfer again if transent is failed   */
1066         err = i2c_transfer(client->adapter, msg, 2);
1067
1068         if (err >= 0) {
1069             *val = buf[0];
1070             return 0;
1071         } else {
1072                 SENSOR_TR("\n %s read reg(0x%x val:0x%x) failed, try to read again! \n",SENSOR_NAME_STRING(),reg, *val);
1073             udelay(10);
1074         }
1075     }
1076
1077     return err;
1078 }
1079
1080 /* write a array of registers  */
1081 static int sensor_write_array(struct i2c_client *client, struct reginfo *regarray)
1082 {
1083     int err = 0, cnt;
1084     int i = 0;
1085 #if CONFIG_SENSOR_I2C_RDWRCHK
1086         char valchk;
1087 #endif
1088
1089         cnt = 0;
1090         if (sensor_task_lock(client, 1) < 0)
1091                 goto sensor_write_array_end;
1092
1093     while (regarray[i].reg != 0)
1094     {
1095         err = sensor_write(client, regarray[i].reg, regarray[i].val);
1096         if (err < 0)
1097         {
1098             if (cnt-- > 0) {
1099                             SENSOR_TR("%s..write failed current reg:0x%x, Write array again !\n", SENSOR_NAME_STRING(),regarray[i].reg);
1100                                 i = 0;
1101                                 continue;
1102             } else {
1103                 SENSOR_TR("%s..write array failed!!!\n", SENSOR_NAME_STRING());
1104                 err = -EPERM;
1105                                 goto sensor_write_array_end;
1106             }
1107         } else {
1108         #if CONFIG_SENSOR_I2C_RDWRCHK
1109                         sensor_read(client, regarray[i].reg, &valchk);
1110                         if (valchk != regarray[i].val)
1111                                 SENSOR_TR("%s Reg:0x%x write(0x%x, 0x%x) fail\n",SENSOR_NAME_STRING(), regarray[i].reg, regarray[i].val, valchk);
1112                 #endif
1113         }
1114         i++;
1115     }
1116
1117 sensor_write_array_end:
1118         sensor_task_lock(client,0);
1119         return err;
1120 }
1121 #if CONFIG_SENSOR_I2C_RDWRCHK
1122 static int sensor_readchk_array(struct i2c_client *client, struct reginfo *regarray)
1123 {
1124     int cnt;
1125     int i = 0;
1126         char valchk;
1127
1128         cnt = 0;
1129         valchk = 0;
1130     while (regarray[i].reg != 0)
1131     {
1132                 sensor_read(client, regarray[i].reg, &valchk);
1133                 if (valchk != regarray[i].val)
1134                         SENSOR_TR("%s Reg:0x%x read(0x%x, 0x%x) error\n",SENSOR_NAME_STRING(), regarray[i].reg, regarray[i].val, valchk);
1135
1136         i++;
1137     }
1138     return 0;
1139 }
1140 #endif
1141 static int sensor_ioctrl(struct soc_camera_device *icd,enum rksensor_power_cmd cmd, int on)
1142 {
1143         struct soc_camera_link *icl = to_soc_camera_link(icd);
1144         int ret = 0;
1145
1146     SENSOR_DG("%s %s  cmd(%d) on(%d)\n",SENSOR_NAME_STRING(),__FUNCTION__,cmd,on);
1147         switch (cmd)
1148         {
1149                 case Sensor_PowerDown:
1150                 {
1151                         if (icl->powerdown) {
1152                                 if (on == 0)
1153                                         mdelay(1);
1154                                 ret = icl->powerdown(icd->pdev, on);
1155                                 if (ret == RK_CAM_IO_SUCCESS) {
1156                                         if (on == 0) {
1157                                                 mdelay(20);
1158                                                 if (icl->reset)
1159                                                         icl->reset(icd->pdev);
1160                                         }
1161                                 } else if (ret == RK_CAM_EIO_REQUESTFAIL) {
1162                                         ret = -ENODEV;
1163                                         goto sensor_power_end;
1164                                 }
1165                         }
1166                         break;
1167                 }
1168                 case Sensor_Flash:
1169                 {
1170                         struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1171                 struct sensor *sensor = to_sensor(client);
1172
1173                         if (sensor->sensor_io_request && sensor->sensor_io_request->sensor_ioctrl) {
1174                                 sensor->sensor_io_request->sensor_ioctrl(icd->pdev,Cam_Flash, on);
1175                         }
1176             break;
1177                 }
1178                 default:
1179                 {
1180                         SENSOR_TR("%s %s cmd(0x%x) is unknown!",SENSOR_NAME_STRING(),__FUNCTION__,cmd);
1181                         break;
1182                 }
1183         }
1184 sensor_power_end:
1185         return ret;
1186 }
1187 static int sensor_init(struct v4l2_subdev *sd, u32 val)
1188 {
1189     struct i2c_client *client = v4l2_get_subdevdata(sd);
1190     struct soc_camera_device *icd = client->dev.platform_data;
1191     struct sensor *sensor = to_sensor(client);
1192         const struct v4l2_queryctrl *qctrl;
1193     const struct sensor_datafmt *fmt;
1194     int ret,pid = 0,i = 0,j=0;
1195     struct rkcamera_platform_data* tmp_plat_data =(struct rkcamera_platform_data*)val;
1196     sensor_init_data_p = sensor_init_data;
1197         sensor_init_winseq_p = sensor_vga;
1198         sensor_init_width = 640;
1199         sensor_init_height = 480;
1200         if (tmp_plat_data != NULL) {
1201                 for(i = 0;i < RK_CAM_NUM_PER_HOST;i++){
1202                         if ((tmp_plat_data->sensor_init_data[i])&& tmp_plat_data->info[i].dev_name &&
1203                                 (strcmp(tmp_plat_data->info[i].dev_name, dev_name(icd->pdev)) == 0))
1204                                         break;
1205                         }
1206                 }
1207         if(tmp_plat_data  && (i < RK_CAM_NUM_PER_HOST) && tmp_plat_data->sensor_init_data[i]){
1208         //user has defined the init data
1209                 //init reg
1210                 if(tmp_plat_data->sensor_init_data[i]->rk_sensor_init_data && (sizeof(struct reginfo) != sizeof(struct reginfo_t))){
1211                         for(j = 0;j< sizeof(sensor_init_data)/sizeof(struct reginfo);j++){
1212                                 sensor_init_data[j].reg = tmp_plat_data->sensor_init_data[i]->rk_sensor_init_data[j].reg;
1213                                 sensor_init_data[j].val = tmp_plat_data->sensor_init_data[i]->rk_sensor_init_data[j].val;
1214                                 }
1215                         sensor_init_data_p = sensor_init_data;
1216                         }
1217                 else if(tmp_plat_data->sensor_init_data[i]->rk_sensor_init_data){
1218                         sensor_init_data_p = (struct reginfo*)(tmp_plat_data->sensor_init_data[i]->rk_sensor_init_data);
1219                         }
1220                 //init winseq
1221                 if(tmp_plat_data->sensor_init_data[i]->rk_sensor_init_winseq && (sizeof(struct reginfo) != sizeof(struct reginfo_t))){
1222                         int tmp_winseq_size = tmp_plat_data->sensor_init_data[i]->rk_sensor_winseq_size;
1223                         if(sensor_init_winseq_board)
1224                                 {
1225                                 vfree(sensor_init_winseq_board);
1226                                 sensor_init_winseq_board = NULL;
1227                                 }
1228                         sensor_init_winseq_board = (struct reginfo*)vmalloc(tmp_winseq_size);
1229                         if(!sensor_init_winseq_board)
1230                                 SENSOR_TR("%s :vmalloc erro !",__FUNCTION__);
1231                         for(j = 0;j< tmp_winseq_size;j++){
1232                                 sensor_init_winseq_board[j].reg = tmp_plat_data->sensor_init_data[i]->rk_sensor_init_winseq[j].reg;
1233                                 sensor_init_winseq_board[j].val = tmp_plat_data->sensor_init_data[i]->rk_sensor_init_winseq[j].val;
1234                                 }
1235                         sensor_init_winseq_p = sensor_init_winseq_board;
1236                         }
1237                 else if(tmp_plat_data->sensor_init_data[i]->rk_sensor_init_winseq){
1238                         sensor_init_winseq_p = (struct reginfo*)(tmp_plat_data->sensor_init_data[i]->rk_sensor_init_winseq);
1239                         }
1240                 //init width,height,bus,pixelcode
1241                 if(tmp_plat_data->sensor_init_data[i] && tmp_plat_data->sensor_init_data[i]->rk_sensor_init_width != INVALID_VALUE)
1242                         sensor_init_width = tmp_plat_data->sensor_init_data[i]->rk_sensor_init_width;
1243                 if(tmp_plat_data->sensor_init_data[i] && tmp_plat_data->sensor_init_data[i]->rk_sensor_init_height != INVALID_VALUE)
1244                         sensor_init_height = tmp_plat_data->sensor_init_data[i]->rk_sensor_init_height;
1245                 if(tmp_plat_data->sensor_init_data[i] && tmp_plat_data->sensor_init_data[i]->rk_sensor_init_bus_param != INVALID_VALUE)
1246                         sensor_init_busparam = tmp_plat_data->sensor_init_data[i]->rk_sensor_init_bus_param;
1247                 if(tmp_plat_data->sensor_init_data[i] && tmp_plat_data->sensor_init_data[i]->rk_sensor_init_pixelcode != INVALID_VALUE)
1248                         sensor_init_pixelcode = tmp_plat_data->sensor_init_data[i]->rk_sensor_init_pixelcode;
1249         }
1250     SENSOR_DG("\n%s..%s.. \n",SENSOR_NAME_STRING(),__FUNCTION__);
1251
1252         if (sensor_ioctrl(icd, Sensor_PowerDown, 0) < 0) {
1253                 ret = -ENODEV;
1254                 goto sensor_INIT_ERR;
1255         }
1256
1257     /* soft reset */
1258         if (sensor_task_lock(client,1)<0)
1259                 goto sensor_INIT_ERR;
1260     ret = sensor_write(client, 0x12, 0x80);
1261     if (ret != 0)
1262     {
1263         SENSOR_TR("%s soft reset sensor failed\n",SENSOR_NAME_STRING());
1264         ret = -ENODEV;
1265                 goto sensor_INIT_ERR;
1266     }
1267     mdelay(5);  //delay 5 microseconds
1268
1269     ret = sensor_write_array(client, sensor_init_data_p);
1270     if (ret != 0)
1271     {
1272         SENSOR_TR("error: %s initial failed\n",SENSOR_NAME_STRING());
1273         goto sensor_INIT_ERR;
1274     }
1275         sensor_task_lock(client,0);
1276
1277     sensor->info_priv.winseqe_cur_addr  = (int)SENSOR_INIT_WINSEQADR;
1278     fmt = sensor_find_datafmt(SENSOR_INIT_PIXFMT,sensor_colour_fmts, ARRAY_SIZE(sensor_colour_fmts));
1279     if (!fmt) {
1280         SENSOR_TR("error: %s initial array colour fmts is not support!!",SENSOR_NAME_STRING());
1281         ret = -EINVAL;
1282         goto sensor_INIT_ERR;
1283     }
1284         sensor->info_priv.fmt = *fmt;
1285
1286     /* sensor sensor information for initialization  */
1287         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_DO_WHITE_BALANCE);
1288         if (qctrl)
1289         sensor->info_priv.whiteBalance = qctrl->default_value;
1290         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_BRIGHTNESS);
1291         if (qctrl)
1292         sensor->info_priv.brightness = qctrl->default_value;
1293         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_EFFECT);
1294         if (qctrl)
1295         sensor->info_priv.effect = qctrl->default_value;
1296         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_EXPOSURE);
1297         if (qctrl)
1298         sensor->info_priv.exposure = qctrl->default_value;
1299
1300         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_SATURATION);
1301         if (qctrl)
1302         sensor->info_priv.saturation = qctrl->default_value;
1303         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_CONTRAST);
1304         if (qctrl)
1305         sensor->info_priv.contrast = qctrl->default_value;
1306         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_HFLIP);
1307         if (qctrl)
1308         sensor->info_priv.mirror = qctrl->default_value;
1309         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_VFLIP);
1310         if (qctrl)
1311         sensor->info_priv.flip = qctrl->default_value;
1312         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_SCENE);
1313         if (qctrl)
1314         sensor->info_priv.scene = qctrl->default_value;
1315         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_ZOOM_ABSOLUTE);
1316         if (qctrl)
1317         sensor->info_priv.digitalzoom = qctrl->default_value;
1318
1319     /* ddl@rock-chips.com : if sensor support auto focus and flash, programer must run focus and flash code  */
1320         #if CONFIG_SENSOR_Focus
1321     sensor_set_focus();
1322     qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_FOCUS_ABSOLUTE);
1323         if (qctrl)
1324         sensor->info_priv.focus = qctrl->default_value;
1325         #endif
1326
1327         #if CONFIG_SENSOR_Flash
1328         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_FLASH);
1329         if (qctrl)
1330         sensor->info_priv.flash = qctrl->default_value;
1331     #endif
1332
1333     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);
1334
1335     return 0;
1336 sensor_INIT_ERR:
1337         sensor_task_lock(client,0);
1338         sensor_deactivate(client);
1339     return ret;
1340 }
1341
1342 static int sensor_deactivate(struct i2c_client *client)
1343 {
1344         struct soc_camera_device *icd = client->dev.platform_data;
1345         //u8 reg_val;
1346
1347         SENSOR_DG("\n%s..%s.. Enter\n",SENSOR_NAME_STRING(),__FUNCTION__);
1348
1349         /* ddl@rock-chips.com : all sensor output pin must change to input for other sensor */
1350         sensor_ioctrl(icd, Sensor_PowerDown, 1);
1351
1352         /* ddl@rock-chips.com : sensor config init width , because next open sensor quickly(soc_camera_open -> Try to configure with default parameters) */
1353         icd->user_width = SENSOR_INIT_WIDTH;
1354     icd->user_height = SENSOR_INIT_HEIGHT;
1355         msleep(100);
1356         return 0;
1357 }
1358 static  struct reginfo sensor_power_down_sequence[]=
1359 {
1360     {0x00,0x00}
1361 };
1362 static int sensor_suspend(struct soc_camera_device *icd, pm_message_t pm_msg)
1363 {
1364     int ret;
1365     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1366
1367     if (pm_msg.event == PM_EVENT_SUSPEND) {
1368         SENSOR_DG("\n %s Enter Suspend.. \n", SENSOR_NAME_STRING());
1369         ret = sensor_write_array(client, sensor_power_down_sequence) ;
1370         if (ret != 0) {
1371             SENSOR_TR("\n %s..%s WriteReg Fail.. \n", SENSOR_NAME_STRING(),__FUNCTION__);
1372             return ret;
1373         } else {
1374             ret = sensor_ioctrl(icd, Sensor_PowerDown, 1);
1375             if (ret < 0) {
1376                             SENSOR_TR("\n %s suspend fail for turn on power!\n", SENSOR_NAME_STRING());
1377                 return -EINVAL;
1378             }
1379         }
1380     } else {
1381         SENSOR_TR("\n %s cann't suppout Suspend..\n",SENSOR_NAME_STRING());
1382         return -EINVAL;
1383     }
1384     return 0;
1385 }
1386
1387 static int sensor_resume(struct soc_camera_device *icd)
1388 {
1389         int ret;
1390
1391     ret = sensor_ioctrl(icd, Sensor_PowerDown, 0);
1392     if (ret < 0) {
1393                 SENSOR_TR("\n %s resume fail for turn on power!\n", SENSOR_NAME_STRING());
1394         return -EINVAL;
1395     }
1396
1397         SENSOR_DG("\n %s Enter Resume.. \n", SENSOR_NAME_STRING());
1398
1399     return 0;
1400
1401 }
1402
1403 static int sensor_set_bus_param(struct soc_camera_device *icd,
1404                                 unsigned long flags)
1405 {
1406
1407     return 0;
1408 }
1409
1410 static unsigned long sensor_query_bus_param(struct soc_camera_device *icd)
1411 {
1412     struct soc_camera_link *icl = to_soc_camera_link(icd);
1413     unsigned long flags = SENSOR_BUS_PARAM;
1414
1415     return soc_camera_apply_sensor_flags(icl, flags);
1416 }
1417
1418 static int sensor_g_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
1419 {
1420     struct i2c_client *client = v4l2_get_subdevdata(sd);
1421     struct soc_camera_device *icd = client->dev.platform_data;
1422     struct sensor *sensor = to_sensor(client);
1423
1424     mf->width   = icd->user_width;
1425         mf->height      = icd->user_height;
1426         mf->code        = sensor->info_priv.fmt.code;
1427         mf->colorspace  = sensor->info_priv.fmt.colorspace;
1428         mf->field       = V4L2_FIELD_NONE;
1429
1430     return 0;
1431 }
1432 static bool sensor_fmt_capturechk(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
1433 {
1434     bool ret = false;
1435
1436         if ((mf->width == 1024) && (mf->height == 768)) {
1437                 ret = true;
1438         } else if ((mf->width == 1280) && (mf->height == 1024)) {
1439                 ret = true;
1440         } else if ((mf->width == 1600) && (mf->height == 1200)) {
1441                 ret = true;
1442         } else if ((mf->width == 2048) && (mf->height == 1536)) {
1443                 ret = true;
1444         } else if ((mf->width == 2592) && (mf->height == 1944)) {
1445                 ret = true;
1446         }
1447
1448         if (ret == true)
1449                 SENSOR_DG("%s %dx%d is capture format\n", __FUNCTION__, mf->width, mf->height);
1450         return ret;
1451 }
1452
1453 static bool sensor_fmt_videochk(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
1454 {
1455     bool ret = false;
1456
1457         if ((mf->width == 1280) && (mf->height == 720)) {
1458                 ret = true;
1459         } else if ((mf->width == 1920) && (mf->height == 1080)) {
1460                 ret = true;
1461         }
1462
1463         if (ret == true)
1464                 SENSOR_DG("%s %dx%d is video format\n", __FUNCTION__, mf->width, mf->height);
1465         return ret;
1466 }
1467 static int sensor_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
1468 {
1469     struct i2c_client *client = v4l2_get_subdevdata(sd);
1470     const struct sensor_datafmt *fmt;
1471     struct sensor *sensor = to_sensor(client);
1472         const struct v4l2_queryctrl *qctrl;
1473         struct soc_camera_device *icd = client->dev.platform_data;
1474     struct reginfo *winseqe_set_addr=NULL;
1475         char readval;
1476     int ret=0, set_w,set_h;
1477
1478         fmt = sensor_find_datafmt(mf->code, sensor_colour_fmts,
1479                                    ARRAY_SIZE(sensor_colour_fmts));
1480         if (!fmt) {
1481         ret = -EINVAL;
1482         goto sensor_s_fmt_end;
1483     }
1484
1485         if (sensor->info_priv.fmt.code != mf->code) {
1486                 switch (mf->code)
1487                 {
1488                         case V4L2_MBUS_FMT_YUYV8_2X8:
1489                         {
1490                                 //winseqe_set_addr = sensor_ClrFmt_YUYV;
1491                                 sensor_read(client, 0x3a, &readval);
1492                                 sensor_write(client,0x3a, readval&0xf7);
1493                                 sensor_read(client,0x3d,&readval);
1494                                 sensor_write(client,0x3d,readval&0xfe);
1495                                 break;
1496                         }
1497                         case V4L2_MBUS_FMT_UYVY8_2X8:
1498                         {
1499                                 //winseqe_set_addr = sensor_ClrFmt_UYVY;
1500                                 sensor_read(client, 0x3a, &readval);
1501                                 sensor_write(client,0x3a, readval|0x08);
1502                                 sensor_read(client,0x3d,&readval);
1503                                 sensor_write(client,0x3d,readval&0xfe);
1504                                 break;
1505                         }
1506                         default:
1507                                 break;
1508                 }
1509                 if (winseqe_set_addr != NULL) {
1510             sensor_write_array(client, winseqe_set_addr);
1511                         sensor->info_priv.fmt.code = mf->code;
1512             sensor->info_priv.fmt.colorspace= mf->colorspace;
1513                         SENSOR_DG("%s v4l2_mbus_code:%d set success!\n", SENSOR_NAME_STRING(),mf->code);
1514                 } else {
1515                         SENSOR_TR("%s v4l2_mbus_code:%d is invalidate!\n", SENSOR_NAME_STRING(),mf->code);
1516                 }
1517         }
1518
1519     set_w = mf->width;
1520     set_h = mf->height;
1521
1522         if (((set_w <= 176) && (set_h <= 144)) && sensor_qcif[0].reg)
1523         {
1524                 winseqe_set_addr = sensor_qcif;
1525         set_w = 176;
1526         set_h = 144;
1527         }
1528         else if (((set_w <= 320) && (set_h <= 240)) && sensor_qvga[0].reg)
1529     {
1530         winseqe_set_addr = sensor_qvga;
1531         set_w = 320;
1532         set_h = 240;
1533     }
1534     else if (((set_w <= 352) && (set_h<= 288)) && sensor_cif[0].reg)
1535     {
1536         winseqe_set_addr = sensor_cif;
1537         set_w = 352;
1538         set_h = 288;
1539     }
1540     else if (((set_w <= 640) && (set_h <= 480)) && sensor_vga[0].reg)
1541     {
1542         winseqe_set_addr = sensor_vga;
1543         set_w = 640;
1544         set_h = 480;
1545     }
1546     else if (((set_w <= 800) && (set_h <= 600)) && sensor_svga[0].reg)
1547     {
1548         winseqe_set_addr = sensor_svga;
1549         set_w = 800;
1550         set_h = 600;
1551     }
1552     else if (((set_w <= 1280) && (set_h <= 720)) && sensor_720p[0].reg)
1553     {
1554         winseqe_set_addr = sensor_720p;
1555         set_w = 1280;
1556         set_h = 720;
1557     }
1558         else if (((set_w <= 1024) && (set_h <= 768)) && sensor_xga[0].reg)
1559     {
1560         winseqe_set_addr = sensor_xga;
1561         set_w = 1024;
1562         set_h = 768;
1563     }
1564     else if (((set_w <= 1280) && (set_h <= 1024)) && sensor_sxga[0].reg)
1565     {
1566                 winseqe_set_addr = sensor_vga; //sensor_sxga;
1567         set_w = 1280;
1568         set_h = 1024;
1569     }
1570     else if (((set_w <= 1600) && (set_h <= 1200)) && sensor_uxga[0].reg)
1571     {
1572         winseqe_set_addr = sensor_uxga;
1573         set_w = 1600;
1574         set_h = 1200;
1575     }
1576     else
1577     {
1578         winseqe_set_addr = SENSOR_INIT_WINSEQADR;               /* ddl@rock-chips.com : Sensor output smallest size if  isn't support app  */
1579         set_w = SENSOR_INIT_WIDTH;
1580         set_h = SENSOR_INIT_HEIGHT;
1581                 SENSOR_TR("\n %s..%s Format is Invalidate. pix->width = %d.. pix->height = %d\n",SENSOR_NAME_STRING(),__FUNCTION__,mf->width,mf->height);
1582     }
1583
1584     if ((int)winseqe_set_addr  != sensor->info_priv.winseqe_cur_addr) {
1585         #if CONFIG_SENSOR_Flash
1586         if (sensor_fmt_capturechk(sd,mf) == true) {      /* ddl@rock-chips.com : Capture */
1587             if ((sensor->info_priv.flash == 1) || (sensor->info_priv.flash == 2)) {
1588                 sensor_ioctrl(icd, Sensor_Flash, Flash_On);
1589                 SENSOR_DG("%s flash on in capture!\n", SENSOR_NAME_STRING());
1590             }
1591         } else {                                        /* ddl@rock-chips.com : Video */
1592             if ((sensor->info_priv.flash == 1) || (sensor->info_priv.flash == 2)) {
1593                 sensor_ioctrl(icd, Sensor_Flash, Flash_Off);
1594                 SENSOR_DG("%s flash off in preivew!\n", SENSOR_NAME_STRING());
1595             }
1596         }
1597         #endif
1598         ret |= sensor_write_array(client, winseqe_set_addr);
1599         if (ret != 0) {
1600             SENSOR_TR("%s set format capability failed\n", SENSOR_NAME_STRING());
1601             #if CONFIG_SENSOR_Flash
1602             if (sensor_fmt_capturechk(sd,mf) == true) {
1603                 if ((sensor->info_priv.flash == 1) || (sensor->info_priv.flash == 2)) {
1604                     sensor_ioctrl(icd, Sensor_Flash, Flash_Off);
1605                     SENSOR_TR("%s Capture format set fail, flash off !\n", SENSOR_NAME_STRING());
1606                 }
1607             }
1608             #endif
1609             goto sensor_s_fmt_end;
1610         }
1611
1612         sensor->info_priv.winseqe_cur_addr  = (int)winseqe_set_addr;
1613
1614                 if (sensor_fmt_capturechk(sd,mf) == true) {                                 /* ddl@rock-chips.com : Capture */
1615         #if CONFIG_SENSOR_Effect
1616                         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_EFFECT);
1617                         sensor_set_effect(icd, qctrl,sensor->info_priv.effect);
1618         #endif
1619         #if CONFIG_SENSOR_WhiteBalance
1620                         if (sensor->info_priv.whiteBalance != 0) {
1621                                 qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_DO_WHITE_BALANCE);
1622                                 sensor_set_whiteBalance(icd, qctrl,sensor->info_priv.whiteBalance);
1623                         }
1624         #endif
1625                         sensor->info_priv.snap2preview = true;
1626                 } else if (sensor_fmt_videochk(sd,mf) == true) {                        /* ddl@rock-chips.com : Video */
1627                 #if CONFIG_SENSOR_Effect
1628                         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_EFFECT);
1629                         sensor_set_effect(icd, qctrl,sensor->info_priv.effect);
1630         #endif
1631         #if CONFIG_SENSOR_WhiteBalance
1632                         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_DO_WHITE_BALANCE);
1633                         sensor_set_whiteBalance(icd, qctrl,sensor->info_priv.whiteBalance);
1634         #endif
1635                         sensor->info_priv.video2preview = true;
1636                 } else if ((sensor->info_priv.snap2preview == true) || (sensor->info_priv.video2preview == true)) {
1637                 #if CONFIG_SENSOR_Effect
1638                         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_EFFECT);
1639                         sensor_set_effect(icd, qctrl,sensor->info_priv.effect);
1640         #endif
1641         #if CONFIG_SENSOR_WhiteBalance
1642                         qctrl = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_DO_WHITE_BALANCE);
1643                         sensor_set_whiteBalance(icd, qctrl,sensor->info_priv.whiteBalance);
1644         #endif
1645                         sensor->info_priv.video2preview = false;
1646                         sensor->info_priv.snap2preview = false;
1647                 }
1648
1649         SENSOR_DG("\n%s..%s.. icd->width = %d.. icd->height %d\n",SENSOR_NAME_STRING(),__FUNCTION__,set_w,set_h);
1650     }
1651     else
1652     {
1653         SENSOR_DG("\n %s .. Current Format is validate. icd->width = %d.. icd->height %d\n",SENSOR_NAME_STRING(),set_w,set_h);
1654     }
1655
1656         mf->width = set_w;
1657     mf->height = set_h;
1658
1659 sensor_s_fmt_end:
1660     return ret;
1661 }
1662
1663 static int sensor_try_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
1664 {
1665     struct i2c_client *client = v4l2_get_subdevdata(sd);
1666     struct sensor *sensor = to_sensor(client);
1667     const struct sensor_datafmt *fmt;
1668     int ret = 0;
1669
1670         fmt = sensor_find_datafmt(mf->code, sensor_colour_fmts,
1671                                    ARRAY_SIZE(sensor_colour_fmts));
1672         if (fmt == NULL) {
1673                 fmt = &sensor->info_priv.fmt;
1674         mf->code = fmt->code;
1675         }
1676
1677     if (mf->height > SENSOR_MAX_HEIGHT)
1678         mf->height = SENSOR_MAX_HEIGHT;
1679     else if (mf->height < SENSOR_MIN_HEIGHT)
1680         mf->height = SENSOR_MIN_HEIGHT;
1681
1682     if (mf->width > SENSOR_MAX_WIDTH)
1683         mf->width = SENSOR_MAX_WIDTH;
1684     else if (mf->width < SENSOR_MIN_WIDTH)
1685         mf->width = SENSOR_MIN_WIDTH;
1686
1687     mf->colorspace = fmt->colorspace;
1688
1689     return ret;
1690 }
1691
1692  static int sensor_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *id)
1693 {
1694     struct i2c_client *client = v4l2_get_subdevdata(sd);
1695
1696     if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
1697         return -EINVAL;
1698
1699     if (id->match.addr != client->addr)
1700         return -ENODEV;
1701
1702     id->ident = SENSOR_V4L2_IDENT;      /* ddl@rock-chips.com :  Return OV2655  identifier */
1703     id->revision = 0;
1704
1705     return 0;
1706 }
1707 #if CONFIG_SENSOR_Brightness
1708 static int sensor_set_brightness(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1709 {
1710     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1711
1712     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1713     {
1714         if (sensor_BrightnessSeqe[value - qctrl->minimum] != NULL)
1715         {
1716             if (sensor_write_array(client, sensor_BrightnessSeqe[value - qctrl->minimum]) != 0)
1717             {
1718                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1719                 return -EINVAL;
1720             }
1721             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1722             return 0;
1723         }
1724     }
1725         SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1726     return -EINVAL;
1727 }
1728 #endif
1729 #if CONFIG_SENSOR_Effect
1730 static int sensor_set_effect(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1731 {
1732     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1733
1734     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1735     {
1736         if (sensor_EffectSeqe[value - qctrl->minimum] != NULL)
1737         {
1738             if (sensor_write_array(client, sensor_EffectSeqe[value - qctrl->minimum]) != 0)
1739             {
1740                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1741                 return -EINVAL;
1742             }
1743             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1744             return 0;
1745         }
1746     }
1747         SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1748     return -EINVAL;
1749 }
1750 #endif
1751 #if CONFIG_SENSOR_Exposure
1752 static int sensor_set_exposure(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1753 {
1754     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1755
1756     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1757     {
1758         if (sensor_ExposureSeqe[value - qctrl->minimum] != NULL)
1759         {
1760             if (sensor_write_array(client, sensor_ExposureSeqe[value - qctrl->minimum]) != 0)
1761             {
1762                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1763                 return -EINVAL;
1764             }
1765             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1766             return 0;
1767         }
1768     }
1769         SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1770     return -EINVAL;
1771 }
1772 #endif
1773 #if CONFIG_SENSOR_Saturation
1774 static int sensor_set_saturation(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1775 {
1776     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1777
1778     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1779     {
1780         if (sensor_SaturationSeqe[value - qctrl->minimum] != NULL)
1781         {
1782             if (sensor_write_array(client, sensor_SaturationSeqe[value - qctrl->minimum]) != 0)
1783             {
1784                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1785                 return -EINVAL;
1786             }
1787             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1788             return 0;
1789         }
1790     }
1791     SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1792     return -EINVAL;
1793 }
1794 #endif
1795 #if CONFIG_SENSOR_Contrast
1796 static int sensor_set_contrast(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1797 {
1798     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1799
1800     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1801     {
1802         if (sensor_ContrastSeqe[value - qctrl->minimum] != NULL)
1803         {
1804             if (sensor_write_array(client, sensor_ContrastSeqe[value - qctrl->minimum]) != 0)
1805             {
1806                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1807                 return -EINVAL;
1808             }
1809             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1810             return 0;
1811         }
1812     }
1813     SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1814     return -EINVAL;
1815 }
1816 #endif
1817 #if CONFIG_SENSOR_Mirror
1818 static int sensor_set_mirror(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1819 {
1820     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1821
1822     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1823     {
1824         if (sensor_MirrorSeqe[value - qctrl->minimum] != NULL)
1825         {
1826             if (sensor_write_array(client, sensor_MirrorSeqe[value - qctrl->minimum]) != 0)
1827             {
1828                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1829                 return -EINVAL;
1830             }
1831             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1832             return 0;
1833         }
1834     }
1835     SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1836     return -EINVAL;
1837 }
1838 #endif
1839 #if CONFIG_SENSOR_Flip
1840 static int sensor_set_flip(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1841 {
1842     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1843
1844     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1845     {
1846         if (sensor_FlipSeqe[value - qctrl->minimum] != NULL)
1847         {
1848             if (sensor_write_array(client, sensor_FlipSeqe[value - qctrl->minimum]) != 0)
1849             {
1850                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1851                 return -EINVAL;
1852             }
1853             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1854             return 0;
1855         }
1856     }
1857     SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1858     return -EINVAL;
1859 }
1860 #endif
1861 #if CONFIG_SENSOR_Scene
1862 static int sensor_set_scene(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1863 {
1864     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1865
1866     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1867     {
1868         if (sensor_SceneSeqe[value - qctrl->minimum] != NULL)
1869         {
1870             if (sensor_write_array(client, sensor_SceneSeqe[value - qctrl->minimum]) != 0)
1871             {
1872                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1873                 return -EINVAL;
1874             }
1875             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1876             return 0;
1877         }
1878     }
1879     SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1880     return -EINVAL;
1881 }
1882 #endif
1883 #if CONFIG_SENSOR_WhiteBalance
1884 static int sensor_set_whiteBalance(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1885 {
1886     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1887
1888     if ((value >= qctrl->minimum) && (value <= qctrl->maximum))
1889     {
1890         if (sensor_WhiteBalanceSeqe[value - qctrl->minimum] != NULL)
1891         {
1892             if (sensor_write_array(client, sensor_WhiteBalanceSeqe[value - qctrl->minimum]) != 0)
1893             {
1894                 SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1895                 return -EINVAL;
1896             }
1897             SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1898             return 0;
1899         }
1900     }
1901         SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1902     return -EINVAL;
1903 }
1904 #endif
1905 #if CONFIG_SENSOR_DigitalZoom
1906 static int sensor_set_digitalzoom(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int *value)
1907 {
1908     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1909     struct sensor *sensor = to_sensor(client);
1910         const struct v4l2_queryctrl *qctrl_info;
1911     int digitalzoom_cur, digitalzoom_total;
1912
1913         qctrl_info = soc_camera_find_qctrl(&sensor_ops, V4L2_CID_ZOOM_ABSOLUTE);
1914         if (qctrl_info)
1915                 return -EINVAL;
1916
1917     digitalzoom_cur = sensor->info_priv.digitalzoom;
1918     digitalzoom_total = qctrl_info->maximum;
1919
1920     if ((value > 0) && (digitalzoom_cur >= digitalzoom_total))
1921     {
1922         SENSOR_TR("%s digitalzoom is maximum - %x\n", SENSOR_NAME_STRING(), digitalzoom_cur);
1923         return -EINVAL;
1924     }
1925
1926     if  ((value < 0) && (digitalzoom_cur <= qctrl_info->minimum))
1927     {
1928         SENSOR_TR("%s digitalzoom is minimum - %x\n", SENSOR_NAME_STRING(), digitalzoom_cur);
1929         return -EINVAL;
1930     }
1931
1932     if ((value > 0) && ((digitalzoom_cur + value) > digitalzoom_total))
1933     {
1934         value = digitalzoom_total - digitalzoom_cur;
1935     }
1936
1937     if ((value < 0) && ((digitalzoom_cur + value) < 0))
1938     {
1939         value = 0 - digitalzoom_cur;
1940     }
1941
1942     digitalzoom_cur += value;
1943
1944     if (sensor_ZoomSeqe[digitalzoom_cur] != NULL)
1945     {
1946         if (sensor_write_array(client, sensor_ZoomSeqe[digitalzoom_cur]) != 0)
1947         {
1948             SENSOR_TR("%s..%s WriteReg Fail.. \n",SENSOR_NAME_STRING(), __FUNCTION__);
1949             return -EINVAL;
1950         }
1951         SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1952         return 0;
1953     }
1954
1955     return -EINVAL;
1956 }
1957 #endif
1958 #if CONFIG_SENSOR_Flash
1959 static int sensor_set_flash(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value)
1960 {
1961     if ((value >= qctrl->minimum) && (value <= qctrl->maximum)) {
1962         if (value == 3) {       /* ddl@rock-chips.com: torch */
1963             sensor_ioctrl(icd, Sensor_Flash, Flash_Torch);   /* Flash On */
1964         } else {
1965             sensor_ioctrl(icd, Sensor_Flash, Flash_Off);
1966         }
1967         SENSOR_DG("%s..%s : %x\n",SENSOR_NAME_STRING(),__FUNCTION__, value);
1968         return 0;
1969     }
1970
1971         SENSOR_TR("\n %s..%s valure = %d is invalidate..    \n",SENSOR_NAME_STRING(),__FUNCTION__,value);
1972     return -EINVAL;
1973 }
1974 #endif
1975
1976 static int sensor_g_control(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
1977 {
1978     struct i2c_client *client = v4l2_get_subdevdata(sd);
1979     struct sensor *sensor = to_sensor(client);
1980     const struct v4l2_queryctrl *qctrl;
1981
1982     qctrl = soc_camera_find_qctrl(&sensor_ops, ctrl->id);
1983
1984     if (!qctrl)
1985     {
1986         SENSOR_TR("\n %s ioctrl id = %d  is invalidate \n", SENSOR_NAME_STRING(), ctrl->id);
1987         return -EINVAL;
1988     }
1989
1990     switch (ctrl->id)
1991     {
1992         case V4L2_CID_BRIGHTNESS:
1993             {
1994                 ctrl->value = sensor->info_priv.brightness;
1995                 break;
1996             }
1997         case V4L2_CID_SATURATION:
1998             {
1999                 ctrl->value = sensor->info_priv.saturation;
2000                 break;
2001             }
2002         case V4L2_CID_CONTRAST:
2003             {
2004                 ctrl->value = sensor->info_priv.contrast;
2005                 break;
2006             }
2007         case V4L2_CID_DO_WHITE_BALANCE:
2008             {
2009                 ctrl->value = sensor->info_priv.whiteBalance;
2010                 break;
2011             }
2012         case V4L2_CID_EXPOSURE:
2013             {
2014                 ctrl->value = sensor->info_priv.exposure;
2015                 break;
2016             }
2017         case V4L2_CID_HFLIP:
2018             {
2019                 ctrl->value = sensor->info_priv.mirror;
2020                 break;
2021             }
2022         case V4L2_CID_VFLIP:
2023             {
2024                 ctrl->value = sensor->info_priv.flip;
2025                 break;
2026             }
2027         default :
2028                 break;
2029     }
2030     return 0;
2031 }
2032
2033
2034
2035 static int sensor_s_control(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
2036 {
2037     struct i2c_client *client = v4l2_get_subdevdata(sd);
2038     struct sensor *sensor = to_sensor(client);
2039     struct soc_camera_device *icd = client->dev.platform_data;
2040     const struct v4l2_queryctrl *qctrl;
2041
2042
2043     qctrl = soc_camera_find_qctrl(&sensor_ops, ctrl->id);
2044
2045     if (!qctrl)
2046     {
2047         SENSOR_TR("\n %s ioctrl id = %d  is invalidate \n", SENSOR_NAME_STRING(), ctrl->id);
2048         return -EINVAL;
2049     }
2050
2051     switch (ctrl->id)
2052     {
2053 #if CONFIG_SENSOR_Brightness
2054         case V4L2_CID_BRIGHTNESS:
2055             {
2056                 if (ctrl->value != sensor->info_priv.brightness)
2057                 {
2058                     if (sensor_set_brightness(icd, qctrl,ctrl->value) != 0)
2059                     {
2060                         return -EINVAL;
2061                     }
2062                     sensor->info_priv.brightness = ctrl->value;
2063                 }
2064                 break;
2065             }
2066 #endif
2067 #if CONFIG_SENSOR_Exposure
2068         case V4L2_CID_EXPOSURE:
2069             {
2070                 if (ctrl->value != sensor->info_priv.exposure)
2071                 {
2072                     if (sensor_set_exposure(icd, qctrl,ctrl->value) != 0)
2073                     {
2074                         return -EINVAL;
2075                     }
2076                     sensor->info_priv.exposure = ctrl->value;
2077                 }
2078                 break;
2079             }
2080 #endif
2081 #if CONFIG_SENSOR_Saturation
2082         case V4L2_CID_SATURATION:
2083             {
2084                 if (ctrl->value != sensor->info_priv.saturation)
2085                 {
2086                     if (sensor_set_saturation(icd, qctrl,ctrl->value) != 0)
2087                     {
2088                         return -EINVAL;
2089                     }
2090                     sensor->info_priv.saturation = ctrl->value;
2091                 }
2092                 break;
2093             }
2094 #endif
2095 #if CONFIG_SENSOR_Contrast
2096         case V4L2_CID_CONTRAST:
2097             {
2098                 if (ctrl->value != sensor->info_priv.contrast)
2099                 {
2100                     if (sensor_set_contrast(icd, qctrl,ctrl->value) != 0)
2101                     {
2102                         return -EINVAL;
2103                     }
2104                     sensor->info_priv.contrast = ctrl->value;
2105                 }
2106                 break;
2107             }
2108 #endif
2109 #if CONFIG_SENSOR_WhiteBalance
2110         case V4L2_CID_DO_WHITE_BALANCE:
2111             {
2112                 if (ctrl->value != sensor->info_priv.whiteBalance)
2113                 {
2114                     if (sensor_set_whiteBalance(icd, qctrl,ctrl->value) != 0)
2115                     {
2116                         return -EINVAL;
2117                     }
2118                     sensor->info_priv.whiteBalance = ctrl->value;
2119                 }
2120                 break;
2121             }
2122 #endif
2123 #if CONFIG_SENSOR_Mirror
2124         case V4L2_CID_HFLIP:
2125             {
2126                 if (ctrl->value != sensor->info_priv.mirror)
2127                 {
2128                     if (sensor_set_mirror(icd, qctrl,ctrl->value) != 0)
2129                         return -EINVAL;
2130                     sensor->info_priv.mirror = ctrl->value;
2131                 }
2132                 break;
2133             }
2134 #endif
2135 #if CONFIG_SENSOR_Flip
2136         case V4L2_CID_VFLIP:
2137             {
2138                 if (ctrl->value != sensor->info_priv.flip)
2139                 {
2140                     if (sensor_set_flip(icd, qctrl,ctrl->value) != 0)
2141                         return -EINVAL;
2142                     sensor->info_priv.flip = ctrl->value;
2143                 }
2144                 break;
2145             }
2146 #endif
2147         default:
2148             break;
2149     }
2150
2151     return 0;
2152 }
2153 static int sensor_g_ext_control(struct soc_camera_device *icd , struct v4l2_ext_control *ext_ctrl)
2154 {
2155     const struct v4l2_queryctrl *qctrl;
2156     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
2157     struct sensor *sensor = to_sensor(client);
2158
2159     qctrl = soc_camera_find_qctrl(&sensor_ops, ext_ctrl->id);
2160
2161     if (!qctrl)
2162     {
2163         SENSOR_TR("\n %s ioctrl id = %d  is invalidate \n", SENSOR_NAME_STRING(), ext_ctrl->id);
2164         return -EINVAL;
2165     }
2166
2167     switch (ext_ctrl->id)
2168     {
2169         case V4L2_CID_SCENE:
2170             {
2171                 ext_ctrl->value = sensor->info_priv.scene;
2172                 break;
2173             }
2174         case V4L2_CID_EFFECT:
2175             {
2176                 ext_ctrl->value = sensor->info_priv.effect;
2177                 break;
2178             }
2179         case V4L2_CID_ZOOM_ABSOLUTE:
2180             {
2181                 ext_ctrl->value = sensor->info_priv.digitalzoom;
2182                 break;
2183             }
2184         case V4L2_CID_ZOOM_RELATIVE:
2185             {
2186                 return -EINVAL;
2187             }
2188         case V4L2_CID_FOCUS_ABSOLUTE:
2189             {
2190                 ext_ctrl->value = sensor->info_priv.focus;
2191                 break;
2192             }
2193         case V4L2_CID_FOCUS_RELATIVE:
2194             {
2195                 return -EINVAL;
2196             }
2197         case V4L2_CID_FLASH:
2198             {
2199                 ext_ctrl->value = sensor->info_priv.flash;
2200                 break;
2201             }
2202         default :
2203             break;
2204     }
2205     return 0;
2206 }
2207 static int sensor_s_ext_control(struct soc_camera_device *icd, struct v4l2_ext_control *ext_ctrl)
2208 {
2209     const struct v4l2_queryctrl *qctrl;
2210     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
2211     struct sensor *sensor = to_sensor(client);
2212     int val_offset;
2213
2214     qctrl = soc_camera_find_qctrl(&sensor_ops, ext_ctrl->id);
2215
2216     if (!qctrl)
2217     {
2218         SENSOR_TR("\n %s ioctrl id = %d  is invalidate \n", SENSOR_NAME_STRING(), ext_ctrl->id);
2219         return -EINVAL;
2220     }
2221
2222         val_offset = 0;
2223     switch (ext_ctrl->id)
2224     {
2225 #if CONFIG_SENSOR_Scene
2226         case V4L2_CID_SCENE:
2227             {
2228                 if (ext_ctrl->value != sensor->info_priv.scene)
2229                 {
2230                     if (sensor_set_scene(icd, qctrl,ext_ctrl->value) != 0)
2231                         return -EINVAL;
2232                     sensor->info_priv.scene = ext_ctrl->value;
2233                 }
2234                 break;
2235             }
2236 #endif
2237 #if CONFIG_SENSOR_Effect
2238         case V4L2_CID_EFFECT:
2239             {
2240                 if (ext_ctrl->value != sensor->info_priv.effect)
2241                 {
2242                     if (sensor_set_effect(icd, qctrl,ext_ctrl->value) != 0)
2243                         return -EINVAL;
2244                     sensor->info_priv.effect= ext_ctrl->value;
2245                 }
2246                 break;
2247             }
2248 #endif
2249 #if CONFIG_SENSOR_DigitalZoom
2250         case V4L2_CID_ZOOM_ABSOLUTE:
2251             {
2252                 if ((ext_ctrl->value < qctrl->minimum) || (ext_ctrl->value > qctrl->maximum))
2253                     return -EINVAL;
2254
2255                 if (ext_ctrl->value != sensor->info_priv.digitalzoom)
2256                 {
2257                     val_offset = ext_ctrl->value -sensor->info_priv.digitalzoom;
2258
2259                     if (sensor_set_digitalzoom(icd, qctrl,&val_offset) != 0)
2260                         return -EINVAL;
2261                     sensor->info_priv.digitalzoom += val_offset;
2262
2263                     SENSOR_DG("%s digitalzoom is %x\n",SENSOR_NAME_STRING(),  sensor->info_priv.digitalzoom);
2264                 }
2265
2266                 break;
2267             }
2268         case V4L2_CID_ZOOM_RELATIVE:
2269             {
2270                 if (ext_ctrl->value)
2271                 {
2272                     if (sensor_set_digitalzoom(icd, qctrl,&ext_ctrl->value) != 0)
2273                         return -EINVAL;
2274                     sensor->info_priv.digitalzoom += ext_ctrl->value;
2275
2276                     SENSOR_DG("%s digitalzoom is %x\n", SENSOR_NAME_STRING(), sensor->info_priv.digitalzoom);
2277                 }
2278                 break;
2279             }
2280 #endif
2281 #if CONFIG_SENSOR_Focus
2282         case V4L2_CID_FOCUS_ABSOLUTE:
2283             {
2284                 if ((ext_ctrl->value < qctrl->minimum) || (ext_ctrl->value > qctrl->maximum))
2285                     return -EINVAL;
2286
2287                 if (ext_ctrl->value != sensor->info_priv.focus)
2288                 {
2289                     val_offset = ext_ctrl->value -sensor->info_priv.focus;
2290
2291                     sensor->info_priv.focus += val_offset;
2292                 }
2293
2294                 break;
2295             }
2296         case V4L2_CID_FOCUS_RELATIVE:
2297             {
2298                 if (ext_ctrl->value)
2299                 {
2300                     sensor->info_priv.focus += ext_ctrl->value;
2301
2302                     SENSOR_DG("%s focus is %x\n", SENSOR_NAME_STRING(), sensor->info_priv.focus);
2303                 }
2304                 break;
2305             }
2306 #endif
2307 #if CONFIG_SENSOR_Flash
2308         case V4L2_CID_FLASH:
2309             {
2310                 if (sensor_set_flash(icd, qctrl,ext_ctrl->value) != 0)
2311                     return -EINVAL;
2312                 sensor->info_priv.flash = ext_ctrl->value;
2313
2314                 SENSOR_DG("%s flash is %x\n",SENSOR_NAME_STRING(), sensor->info_priv.flash);
2315                 break;
2316             }
2317 #endif
2318         default:
2319             break;
2320     }
2321
2322     return 0;
2323 }
2324
2325 static int sensor_g_ext_controls(struct v4l2_subdev *sd, struct v4l2_ext_controls *ext_ctrl)
2326 {
2327     struct i2c_client *client = v4l2_get_subdevdata(sd);
2328     struct soc_camera_device *icd = client->dev.platform_data;
2329     int i, error_cnt=0, error_idx=-1;
2330
2331
2332     for (i=0; i<ext_ctrl->count; i++) {
2333         if (sensor_g_ext_control(icd, &ext_ctrl->controls[i]) != 0) {
2334             error_cnt++;
2335             error_idx = i;
2336         }
2337     }
2338
2339     if (error_cnt > 1)
2340         error_idx = ext_ctrl->count;
2341
2342     if (error_idx != -1) {
2343         ext_ctrl->error_idx = error_idx;
2344         return -EINVAL;
2345     } else {
2346         return 0;
2347     }
2348 }
2349
2350 static int sensor_s_ext_controls(struct v4l2_subdev *sd, struct v4l2_ext_controls *ext_ctrl)
2351 {
2352     struct i2c_client *client = v4l2_get_subdevdata(sd);
2353     struct soc_camera_device *icd = client->dev.platform_data;
2354     int i, error_cnt=0, error_idx=-1;
2355
2356
2357     for (i=0; i<ext_ctrl->count; i++) {
2358         if (sensor_s_ext_control(icd, &ext_ctrl->controls[i]) != 0) {
2359             error_cnt++;
2360             error_idx = i;
2361         }
2362     }
2363
2364     if (error_cnt > 1)
2365         error_idx = ext_ctrl->count;
2366
2367     if (error_idx != -1) {
2368         ext_ctrl->error_idx = error_idx;
2369         return -EINVAL;
2370     } else {
2371         return 0;
2372     }
2373 }
2374
2375 /* Interface active, can use i2c. If it fails, it can indeed mean, that
2376  * this wasn't our capture interface, so, we wait for the right one */
2377 static int sensor_video_probe(struct soc_camera_device *icd,
2378                                struct i2c_client *client)
2379 {
2380     char pid = 0;
2381     int ret;
2382     struct sensor *sensor = to_sensor(client);
2383
2384     /* We must have a parent by now. And it cannot be a wrong one.
2385      * So this entire test is completely redundant. */
2386     if (!icd->dev.parent ||
2387             to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
2388                 return -ENODEV;
2389
2390         if (sensor_ioctrl(icd, Sensor_PowerDown, 0) < 0) {
2391                 ret = -ENODEV;
2392                 goto sensor_video_probe_err;
2393         }
2394     /* soft reset */
2395     ret = sensor_write(client, 0x12, 0x80);
2396     if (ret != 0)
2397     {
2398         SENSOR_TR("soft reset %s failed\n",SENSOR_NAME_STRING());
2399         return -ENODEV;
2400     }
2401     mdelay(50);          //delay 5 microseconds
2402
2403     /* check if it is an sensor sensor */
2404     ret = sensor_read(client, 0x0a, &pid);
2405     if (ret != 0) {
2406         SENSOR_TR("%s read chip id high byte failed\n",SENSOR_NAME_STRING());
2407         ret = -ENODEV;
2408         goto sensor_video_probe_err;
2409     }
2410
2411     SENSOR_DG("\n %s  pid = 0x%x\n", SENSOR_NAME_STRING(), pid);
2412     if (pid == SENSOR_ID) {
2413         sensor->model = SENSOR_V4L2_IDENT;
2414     } else {
2415         SENSOR_TR("error: %s mismatched   pid = 0x%x\n", SENSOR_NAME_STRING(), pid);
2416         ret = -ENODEV;
2417         goto sensor_video_probe_err;
2418     }
2419
2420     return 0;
2421
2422 sensor_video_probe_err:
2423
2424     return ret;
2425 }
2426
2427 static long sensor_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
2428 {
2429         struct i2c_client *client = v4l2_get_subdevdata(sd);
2430     struct soc_camera_device *icd = client->dev.platform_data;
2431     struct sensor *sensor = to_sensor(client);
2432     int ret = 0;
2433 #if CONFIG_SENSOR_Flash
2434     int i;
2435 #endif
2436
2437         SENSOR_DG("\n%s..%s..cmd:%x \n",SENSOR_NAME_STRING(),__FUNCTION__,cmd);
2438         switch (cmd)
2439         {
2440                 case RK_CAM_SUBDEV_DEACTIVATE:
2441                 {
2442                         sensor_deactivate(client);
2443                         break;
2444                 }
2445
2446                 case RK_CAM_SUBDEV_IOREQUEST:
2447                 {
2448                         sensor->sensor_io_request = (struct rkcamera_platform_data*)arg;
2449             if (sensor->sensor_io_request != NULL) {
2450                 if (sensor->sensor_io_request->gpio_res[0].dev_name &&
2451                     (strcmp(sensor->sensor_io_request->gpio_res[0].dev_name, dev_name(icd->pdev)) == 0)) {
2452                     sensor->sensor_gpio_res = (struct rkcamera_gpio_res*)&sensor->sensor_io_request->gpio_res[0];
2453                 } else if (sensor->sensor_io_request->gpio_res[1].dev_name &&
2454                     (strcmp(sensor->sensor_io_request->gpio_res[1].dev_name, dev_name(icd->pdev)) == 0)) {
2455                     sensor->sensor_gpio_res = (struct rkcamera_gpio_res*)&sensor->sensor_io_request->gpio_res[1];
2456                 }
2457             } else {
2458                 SENSOR_TR("%s %s RK_CAM_SUBDEV_IOREQUEST fail\n",SENSOR_NAME_STRING(),__FUNCTION__);
2459                 ret = -EINVAL;
2460                 goto sensor_ioctl_end;
2461             }
2462             /* ddl@rock-chips.com : if gpio_flash havn't been set in board-xxx.c, sensor driver must notify is not support flash control
2463                for this project */
2464             #if CONFIG_SENSOR_Flash
2465                 if (sensor->sensor_gpio_res) {
2466                 if (sensor->sensor_gpio_res->gpio_flash == INVALID_GPIO) {
2467                     for (i = 0; i < icd->ops->num_controls; i++) {
2468                                 if (V4L2_CID_FLASH == icd->ops->controls[i].id) {
2469                                         memset((char*)&icd->ops->controls[i],0x00,sizeof(struct v4l2_queryctrl));
2470                                 }
2471                     }
2472                     sensor->info_priv.flash = 0xff;
2473                     SENSOR_DG("%s flash gpio is invalidate!\n",SENSOR_NAME_STRING());
2474                 }
2475                 }
2476             #endif
2477                         break;
2478                 }
2479                 default:
2480                 {
2481                         SENSOR_TR("%s %s cmd(0x%x) is unknown !\n",SENSOR_NAME_STRING(),__FUNCTION__,cmd);
2482                         break;
2483                 }
2484         }
2485 sensor_ioctl_end:
2486         return ret;
2487
2488 }
2489 static int sensor_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
2490                             enum v4l2_mbus_pixelcode *code)
2491 {
2492         if (index >= ARRAY_SIZE(sensor_colour_fmts))
2493                 return -EINVAL;
2494
2495         *code = sensor_colour_fmts[index].code;
2496         return 0;
2497 }
2498 static struct v4l2_subdev_core_ops sensor_subdev_core_ops = {
2499         .init           = sensor_init,
2500         .g_ctrl         = sensor_g_control,
2501         .s_ctrl         = sensor_s_control,
2502         .g_ext_ctrls          = sensor_g_ext_controls,
2503         .s_ext_ctrls          = sensor_s_ext_controls,
2504         .g_chip_ident   = sensor_g_chip_ident,
2505         .ioctl = sensor_ioctl,
2506 };
2507
2508 static struct v4l2_subdev_video_ops sensor_subdev_video_ops = {
2509         .s_mbus_fmt     = sensor_s_fmt,
2510         .g_mbus_fmt     = sensor_g_fmt,
2511         .try_mbus_fmt   = sensor_try_fmt,
2512         .enum_mbus_fmt  = sensor_enum_fmt,
2513 };
2514 static struct v4l2_subdev_ops sensor_subdev_ops = {
2515         .core   = &sensor_subdev_core_ops,
2516         .video = &sensor_subdev_video_ops,
2517 };
2518
2519 static int sensor_probe(struct i2c_client *client,
2520                          const struct i2c_device_id *did)
2521 {
2522     struct sensor *sensor;
2523     struct soc_camera_device *icd = client->dev.platform_data;
2524     struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
2525     struct soc_camera_link *icl;
2526     int ret;
2527
2528     SENSOR_DG("\n%s..%s..%d..\n",__FUNCTION__,__FILE__,__LINE__);
2529     if (!icd) {
2530         dev_err(&client->dev, "%s: missing soc-camera data!\n",SENSOR_NAME_STRING());
2531         return -EINVAL;
2532     }
2533
2534     icl = to_soc_camera_link(icd);
2535     if (!icl) {
2536         dev_err(&client->dev, "%s driver needs platform data\n", SENSOR_NAME_STRING());
2537         return -EINVAL;
2538     }
2539
2540     if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
2541         dev_warn(&adapter->dev,
2542                  "I2C-Adapter doesn't support I2C_FUNC_I2C\n");
2543         return -EIO;
2544     }
2545
2546     sensor = kzalloc(sizeof(struct sensor), GFP_KERNEL);
2547     if (!sensor)
2548         return -ENOMEM;
2549
2550     v4l2_i2c_subdev_init(&sensor->subdev, client, &sensor_subdev_ops);
2551
2552     /* Second stage probe - when a capture adapter is there */
2553     icd->ops            = &sensor_ops;
2554
2555     sensor->info_priv.fmt = sensor_colour_fmts[0];
2556
2557         #if CONFIG_SENSOR_I2C_NOSCHED
2558         atomic_set(&sensor->tasklock_cnt,0);
2559         #endif
2560
2561     ret = sensor_video_probe(icd, client);
2562     if (ret < 0) {
2563         icd->ops = NULL;
2564         i2c_set_clientdata(client, NULL);
2565         kfree(sensor);
2566                 sensor = NULL;
2567     }
2568     SENSOR_DG("\n%s..%s..%d  ret = %x \n",__FUNCTION__,__FILE__,__LINE__,ret);
2569     return ret;
2570 }
2571
2572 static int sensor_remove(struct i2c_client *client)
2573 {
2574     struct sensor *sensor = to_sensor(client);
2575     struct soc_camera_device *icd = client->dev.platform_data;
2576
2577     icd->ops = NULL;
2578     i2c_set_clientdata(client, NULL);
2579     client->driver = NULL;
2580     kfree(sensor);
2581         sensor = NULL;
2582     return 0;
2583 }
2584
2585 static const struct i2c_device_id sensor_id[] = {
2586         {SENSOR_NAME_STRING(), 0 },
2587         { }
2588 };
2589 MODULE_DEVICE_TABLE(i2c, sensor_id);
2590
2591 static struct i2c_driver sensor_i2c_driver = {
2592         .driver = {
2593                 .name = SENSOR_NAME_STRING(),
2594         },
2595         .probe          = sensor_probe,
2596         .remove         = sensor_remove,
2597         .id_table       = sensor_id,
2598 };
2599
2600 static int __init sensor_mod_init(void)
2601 {
2602     SENSOR_DG("\n%s..%s.. \n",__FUNCTION__,SENSOR_NAME_STRING());
2603     return i2c_add_driver(&sensor_i2c_driver);
2604 }
2605
2606 static void __exit sensor_mod_exit(void)
2607 {
2608     i2c_del_driver(&sensor_i2c_driver);
2609 }
2610
2611 device_initcall_sync(sensor_mod_init);
2612 module_exit(sensor_mod_exit);
2613
2614 MODULE_DESCRIPTION(SENSOR_NAME_STRING(Camera sensor driver));
2615 MODULE_AUTHOR("ddl <kernel@rock-chips>");
2616 MODULE_LICENSE("GPL");
2617
2618