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 <plat/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 RK29_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 /* init 640*480 VGA */
  96 static struct reginfo sensor_init_data[] =
  97 {
  98         {0x12, 0x80},
  99         {0x49, 0x0c},
 100         {0x0c, 0x06},
 101         {0x48, 0x42},
 102         {0x41, 0x43},
 103         {0x4c, 0x73},
 104         {0x81, 0xff},
 105         {0x21, 0x44},
 106         {0x16, 0x03},
 107         {0x39, 0x80},
 108         {0x1e, 0xb1},
 109 // formate
 110         {0x12, 0x00},
 111         {0x82, 0x03},
 112         {0xd0, 0x48},
 113         {0x80, 0x7F},
 114         {0x3e, 0x30},
 115         {0x22, 0x00},
 116 // resolution
 117         {0x17, 0x69},
 118         {0x18, 0xa4},
 119         {0x19, 0x0c},
 120         {0x1a, 0xf6},
 121
 122         {0xc8, 0x02},
 123         {0xc9, 0x80},
 124         {0xca, 0x01},
 125         {0xcb, 0xe0},
 126
 127         {0xcc, 0x02},
 128         {0xcd, 0x80},
 129         {0xce, 0x01},
 130         {0xcf, 0xe0},
 131 // Lens Correction
 132         {0x85, 0x90},
 133         {0x86, 0x00},
 134         {0x87, 0x00},
 135         {0x88, 0x10},
 136         {0x89, 0x30},
 137         {0x8a, 0x29},
 138         {0x8b, 0x26},
 139 // Color Matrix
 140         {0xbb, 0x80},
 141         {0xbc, 0x62},
 142         {0xbd, 0x1e},
 143         {0xbe, 0x26},
 144         {0xbf, 0x7b},
 145         {0xc0, 0xac},
 146         {0xc1, 0x1e},
 147 // Edge + Denoise
 148         {0xb7, 0x05c},
 149         {0xb8, 0x09},
 150         {0xb9, 0x00},       //0x07//0x27
 151         {0xba, 0x18},
 152 //      UVAdjust
 153         {0x5a, 0x4a},
 154         {0x5b, 0x9f},
 155         {0x5c, 0x48},
 156         {0x5d, 0x32},
 157 // AEC/AGC target
 158         {0x24, 0x78},
 159         {0x25, 0x68},
 160         {0x26, 0xb3},
 161 // Gamma
 162         {0xa3, 0x0b},
 163         {0xa4, 0x15},
 164         {0xa5, 0x2a},
 165         {0xa6, 0x51},
 166         {0xa7, 0x63},
 167         {0xa8, 0x74},
 168         {0xa9, 0x83},
 169         {0xaa, 0x91},
 170         {0xab, 0x9e},
 171         {0xac, 0xaa},
 172         {0xad, 0xbe},
 173         {0xae, 0xce},
 174         {0xaf, 0xe5},
 175         {0xb0, 0xf3},
 176         {0xb1, 0xfb},
 177         {0xb2, 0x06},
 178 //simple
 179         {0x8e, 0x92},
 180         {0x96, 0xff},
 181         {0x97, 0x00},
 182 // Advance
 183         {0x8c, 0x5d},
 184         {0x8d, 0x11},
 185         {0x8e, 0x12},
 186         {0x8f, 0x11},
 187         {0x90, 0x50},
 188         {0x91, 0x22},
 189         {0x92, 0xd1},
 190         {0x93, 0xa7},
 191         {0x94, 0x23},
 192         {0x95, 0x3b},
 193         {0x96, 0xff},
 194         {0x97, 0x00},
 195         {0x98, 0x4a},
 196         {0x99, 0x46},
 197         {0x9a, 0x3d},
 198         {0x9b, 0x3a},
 199         {0x9c, 0xf0},
 200         {0x9d, 0xf0},
 201         {0x9e, 0xf0},
 202         {0x9f, 0xff},
 203         {0xa0, 0x56},
 204         {0xa1, 0x55},
 205         {0xa2, 0x13},
 206 // General Control
 207         {0x50, 0x4c},
 208         {0x51, 0x3f},
 209         {0x21, 0x57},
 210         {0x20, 0x00},
 211
 212         {0x14, 0x29},
 213         {0x13, 0xf7},
 214         {0x11, 0x01},
 215         {0xd2, 0x06},
 216
 217         {0x2a, 0x30},
 218         {0x2b, 0x0b},
 219         {0x2c, 0x00}
 220 };
 221
 222 /* 1280x720 */
 223 static struct reginfo sensor_720p[]=
 224 {
 225         {0x00 ,0x00}
 226
 227 };
 228
 229 /* 1600X1200 UXGA */
 230 static struct reginfo sensor_uxga[] =
 231 {
 232         {0x00, 0x00}
 233 };
 234
 235 /* 1280X1024 SXGA */
 236 static struct reginfo sensor_sxga[] =
 237 {
 238         {0x0, 0x0}
 239 };
 240 /* 1024X768 SXGA */
 241 static struct reginfo sensor_xga[] =
 242 {
 243         {0x0, 0x0}
 244 };
 245 /* 800X600 SVGA*/
 246 static struct reginfo sensor_svga[] =
 247 {
 248         {0x00, 0x00}
 249 };
 250
 251 /* 640X480 VGA */
 252 static struct reginfo sensor_vga[] =
 253 {
 254                 {0x16,0x03},
 255                 {0x17,0x69},
 256                 {0x18,0xa4},
 257                 {0x19,0x0c},
 258                 {0x1a,0xf6},
 259                 {0x22,0x00},
 260                 {0xc8,0x02},
 261                 {0xc9,0x80},
 262                 {0xca,0x01},
 263                 {0xcb,0xe0},
 264                 {0xcc,0x02},
 265                 {0xcd,0x80},
 266                 {0xce,0x01},
 267                 {0xcf,0xe0},
 268         {0x00,0x00}
 269 };
 270
 271 /* 352X288 CIF */
 272 static struct reginfo sensor_cif[] =
 273 {
 274     {0x00,0x00}
 275 };
 276
 277 /* 320*240 QVGA */
 278 static  struct reginfo sensor_qvga[] =
 279 {
 280                 {0x16,0x03},
 281                 {0x17,0x69},
 282                 {0x18,0xa4},
 283                 {0x19,0x06},
 284                 {0x1a,0xf6},
 285                 {0x22,0x10},
 286                 {0xc8,0x02},
 287                 {0xc9,0x80},
 288                 {0xca,0x00},
 289                 {0xcb,0xf0},
 290                 {0xcc,0x01},
 291                 {0xcd,0x40},
 292                 {0xce,0x00},
 293                 {0xcf,0xf0},
 294     {0x00,0x00}
 295 };
 296
 297 /* 176X144 QCIF*/
 298 static struct reginfo sensor_qcif[] =
 299 {
 300                 {0x16,0x40},
 301                 {0x17,0x83},
 302                 {0x18,0x96},
 303                 {0x19,0x06},
 304                 {0x1a,0xf6},
 305                 {0x22,0x10},
 306                 {0xc8,0x02},
 307                 {0xc9,0x4b},
 308                 {0xca,0x00},
 309                 {0xcb,0xf0},
 310                 {0xcc,0x00},
 311                 {0xcd,0xb0},
 312                 {0xce,0x00},
 313                 {0xcf,0x90},
 314     {0x00,0x00}
 315 };
 316 #if 0
 317 /* 160X120 QQVGA*/
 318 static struct reginfo ov2655_qqvga[] =
 319 {
 320     {0x0, 0x0},
 321 };
 322
 323
 324
 325 static  struct reginfo ov2655_Sharpness_auto[] =
 326 {
 327     {0x3306, 0x00},
 328 };
 329
 330 static  struct reginfo ov2655_Sharpness1[] =
 331 {
 332     {0x3306, 0x08},
 333     {0x3371, 0x00},
 334 };
 335
 336 static  struct reginfo ov2655_Sharpness2[][3] =
 337 {
 338     //Sharpness 2
 339     {0x3306, 0x08},
 340     {0x3371, 0x01},
 341 };
 342
 343 static  struct reginfo ov2655_Sharpness3[] =
 344 {
 345     //default
 346     {0x3306, 0x08},
 347     {0x332d, 0x02},
 348 };
 349 static  struct reginfo ov2655_Sharpness4[]=
 350 {
 351     //Sharpness 4
 352     {0x3306, 0x08},
 353     {0x332d, 0x03},
 354 };
 355
 356 static  struct reginfo ov2655_Sharpness5[] =
 357 {
 358     //Sharpness 5
 359     {0x3306, 0x08},
 360     {0x332d, 0x04},
 361 };
 362 #endif
 363
 364 static  struct reginfo sensor_ClrFmt_YUYV[]=
 365 {
 366     {0x00, 0x00}
 367 };
 368
 369 static  struct reginfo sensor_ClrFmt_UYVY[]=
 370 {
 371     {0x00, 0x00}
 372 };
 373
 374
 375 #if CONFIG_SENSOR_WhiteBalance
 376 static  struct reginfo sensor_WhiteB_Auto[]=
 377 {
 378                 {0x13, 0xf7},
 379     {0x00, 0x00}
 380 };
 381 /* Cloudy Colour Temperature : 6500K - 8000K  */
 382 static  struct reginfo sensor_WhiteB_Cloudy[]=
 383 {
 384                 {0x13, 0xf5},
 385                 {0x01, 0x40},
 386                 {0x02, 0x5d},
 387                 {0x03, 0x40},
 388     {0x00, 0x00}
 389 };
 390 /* ClearDay Colour Temperature : 5000K - 6500K  */
 391 static  struct reginfo sensor_WhiteB_ClearDay[]=
 392 {
 393                 {0x13, 0xf5},
 394                 {0x01, 0x44},
 395                 {0x02, 0x55},
 396                 {0x03, 0x40},
 397     {0x00, 0x00}
 398 };
 399 /* Office Colour Temperature : 3500K - 5000K  */
 400 static  struct reginfo sensor_WhiteB_TungstenLamp1[]=
 401 {
 402                 {0x13, 0xf5},
 403                 {0x01, 0x5b},
 404                 {0x02, 0x4c},
 405                 {0x03, 0x40},
 406     {0x00, 0x00}
 407
 408 };
 409 /* Home Colour Temperature : 2500K - 3500K  */
 410 static  struct reginfo sensor_WhiteB_TungstenLamp2[]=
 411 {
 412                 {0x13, 0xf5},
 413                 {0x01, 0x62},
 414                 {0x02, 0x40},
 415                 {0x03, 0x41},
 416     {0x00, 0x00}
 417 };
 418 static struct reginfo *sensor_WhiteBalanceSeqe[] = {sensor_WhiteB_Auto, sensor_WhiteB_TungstenLamp1,sensor_WhiteB_TungstenLamp2,
 419     sensor_WhiteB_ClearDay, sensor_WhiteB_Cloudy,NULL,
 420 };
 421 #endif
 422
 423 #if CONFIG_SENSOR_Brightness
 424 static  struct reginfo sensor_Brightness0[]=
 425 {
 426     // Brightness -2
 427     {0x0000, 0x00}
 428 };
 429
 430 static  struct reginfo sensor_Brightness1[]=
 431 {
 432     // Brightness -1
 433
 434     {0x0000, 0x00}
 435 };
 436
 437 static  struct reginfo sensor_Brightness2[]=
 438 {
 439     //  Brightness 0
 440
 441     {0x0000, 0x00}
 442 };
 443
 444 static  struct reginfo sensor_Brightness3[]=
 445 {
 446     // Brightness +1
 447
 448     {0x0000, 0x00}
 449 };
 450
 451 static  struct reginfo sensor_Brightness4[]=
 452 {
 453     //  Brightness +2
 454
 455     {0x0000, 0x00}
 456 };
 457
 458 static  struct reginfo sensor_Brightness5[]=
 459 {
 460     //  Brightness +3
 461
 462     {0x0000, 0x00}
 463 };
 464 static struct reginfo *sensor_BrightnessSeqe[] = {sensor_Brightness0, sensor_Brightness1, sensor_Brightness2, sensor_Brightness3,
 465     sensor_Brightness4, sensor_Brightness5,NULL,
 466 };
 467
 468 #endif
 469
 470 #if CONFIG_SENSOR_Effect
 471 static  struct reginfo sensor_Effect_Normal[] =
 472 {
 473         {0xd2,0x06},
 474     {0x00, 0x00}
 475 };
 476
 477 static  struct reginfo sensor_Effect_WandB[] =
 478 {
 479         {0xd2,0x1e},
 480         {0xda,0x80},
 481         {0xdb,0x80},
 482     {0x00, 0x00}
 483 };
 484
 485 static  struct reginfo sensor_Effect_Sepia[] =
 486 {
 487         {0xd2,0x1e},
 488         {0xda,0x40},
 489         {0xdb,0xa0},
 490     {0x00, 0x00}
 491 };
 492
 493 static  struct reginfo sensor_Effect_Negative[] =
 494 {
 495     //Negative
 496         {0xd2,0x46},
 497     {0x00, 0x00}
 498 };
 499 static  struct reginfo sensor_Effect_Bluish[] =
 500 {
 501     // Bluish
 502         {0xd2,0x1e},
 503         {0xda,0xa0},
 504         {0xdb,0x40},
 505     {0x00, 0x00}
 506 };
 507
 508 static  struct reginfo sensor_Effect_Green[] =
 509 {
 510     //  Greenish
 511         {0xd2,0x1e},
 512         {0xda,0x60},
 513         {0xdb,0x60},
 514     {0x00, 0x00}
 515 };
 516 static struct reginfo *sensor_EffectSeqe[] = {sensor_Effect_Normal, sensor_Effect_WandB, sensor_Effect_Negative,sensor_Effect_Sepia,
 517     sensor_Effect_Bluish, sensor_Effect_Green,NULL,
 518 };
 519 #endif
 520 #if CONFIG_SENSOR_Exposure
 521 static  struct reginfo sensor_Exposure0[]=
 522 {
 523     {0x00, 0x00}
 524 };
 525
 526 static  struct reginfo sensor_Exposure1[]=
 527 {
 528     {0x00, 0x00}
 529 };
 530
 531 static  struct reginfo sensor_Exposure2[]=
 532 {
 533     {0x00, 0x00}
 534 };
 535
 536 static  struct reginfo sensor_Exposure3[]=
 537 {
 538     {0x00, 0x00}
 539 };
 540
 541 static  struct reginfo sensor_Exposure4[]=
 542 {
 543     {0x00, 0x00}
 544 };
 545
 546 static  struct reginfo sensor_Exposure5[]=
 547 {
 548     {0x00, 0x00}
 549 };
 550
 551 static  struct reginfo sensor_Exposure6[]=
 552 {
 553     {0x00, 0x00}
 554 };
 555
 556 static struct reginfo *sensor_ExposureSeqe[] = {sensor_Exposure0, sensor_Exposure1, sensor_Exposure2, sensor_Exposure3,
 557     sensor_Exposure4, sensor_Exposure5,sensor_Exposure6,NULL,
 558 };
 559 #endif
 560 #if CONFIG_SENSOR_Saturation
 561 static  struct reginfo sensor_Saturation0[]=
 562 {
 563     {0x00, 0x00}
 564 };
 565
 566 static  struct reginfo sensor_Saturation1[]=
 567 {
 568     {0x00, 0x00}
 569 };
 570
 571 static  struct reginfo sensor_Saturation2[]=
 572 {
 573     {0x00, 0x00}
 574 };
 575 static struct reginfo *sensor_SaturationSeqe[] = {sensor_Saturation0, sensor_Saturation1, sensor_Saturation2, NULL,};
 576
 577 #endif
 578 #if CONFIG_SENSOR_Contrast
 579 static  struct reginfo sensor_Contrast0[]=
 580 {
 581     {0x00, 0x00}
 582 };
 583
 584 static  struct reginfo sensor_Contrast1[]=
 585 {
 586     {0x00, 0x00}
 587 };
 588
 589 static  struct reginfo sensor_Contrast2[]=
 590 {
 591     {0x00, 0x00}
 592 };
 593
 594 static  struct reginfo sensor_Contrast3[]=
 595 {
 596     {0x00, 0x00}
 597 };
 598
 599 static  struct reginfo sensor_Contrast4[]=
 600 {
 601     {0x00, 0x00}
 602 };
 603
 604
 605 static  struct reginfo sensor_Contrast5[]=
 606 {
 607     {0x00, 0x00}
 608 };
 609
 610 static  struct reginfo sensor_Contrast6[]=
 611 {
 612     {0x00, 0x00}
 613 };
 614 static struct reginfo *sensor_ContrastSeqe[] = {sensor_Contrast0, sensor_Contrast1, sensor_Contrast2, sensor_Contrast3,
 615     sensor_Contrast4, sensor_Contrast5, sensor_Contrast6, NULL,
 616 };
 617
 618 #endif
 619 #if CONFIG_SENSOR_Mirror
 620 static  struct reginfo sensor_MirrorOn[]=
 621 {
 622     {0x00, 0x00}
 623 };
 624
 625 static  struct reginfo sensor_MirrorOff[]=
 626 {
 627     {0x00, 0x00}
 628 };
 629 static struct reginfo *sensor_MirrorSeqe[] = {sensor_MirrorOff, sensor_MirrorOn,NULL,};
 630 #endif
 631 #if CONFIG_SENSOR_Flip
 632 static  struct reginfo sensor_FlipOn[]=
 633 {
 634     {0x00, 0x00}
 635 };
 636
 637 static  struct reginfo sensor_FlipOff[]=
 638 {
 639     {0x00, 0x00}
 640 };
 641 static struct reginfo *sensor_FlipSeqe[] = {sensor_FlipOff, sensor_FlipOn,NULL,};
 642
 643 #endif
 644 #if CONFIG_SENSOR_Scene
 645 static  struct reginfo sensor_SceneAuto[] =
 646 {
 647 {0x00, 0x00}
 648 };
 649
 650 static  struct reginfo sensor_SceneNight[] =
 651 {
 652 {0x00, 0x00}
 653 };
 654 static struct reginfo *sensor_SceneSeqe[] = {sensor_SceneAuto, sensor_SceneNight,NULL,};
 655
 656 #endif
 657 #if CONFIG_SENSOR_DigitalZoom
 658 static struct reginfo sensor_Zoom0[] =
 659 {
 660     {0x0, 0x0},
 661 };
 662
 663 static struct reginfo sensor_Zoom1[] =
 664 {
 665      {0x0, 0x0},
 666 };
 667
 668 static struct reginfo sensor_Zoom2[] =
 669 {
 670     {0x0, 0x0},
 671 };
 672
 673
 674 static struct reginfo sensor_Zoom3[] =
 675 {
 676     {0x0, 0x0},
 677 };
 678 static struct reginfo *sensor_ZoomSeqe[] = {sensor_Zoom0, sensor_Zoom1, sensor_Zoom2, sensor_Zoom3, NULL,};
 679 #endif
 680 static const struct v4l2_querymenu sensor_menus[] =
 681 {
 682         #if CONFIG_SENSOR_WhiteBalance
 683     { .id = V4L2_CID_DO_WHITE_BALANCE,  .index = 0,  .name = "auto",  .reserved = 0, }, {  .id = V4L2_CID_DO_WHITE_BALANCE,  .index = 1, .name = "incandescent",  .reserved = 0,},
 684     { .id = V4L2_CID_DO_WHITE_BALANCE,  .index = 2,  .name = "fluorescent", .reserved = 0,}, {  .id = V4L2_CID_DO_WHITE_BALANCE, .index = 3,  .name = "daylight", .reserved = 0,},
 685     { .id = V4L2_CID_DO_WHITE_BALANCE,  .index = 4,  .name = "cloudy-daylight", .reserved = 0,},
 686     #endif
 687
 688         #if CONFIG_SENSOR_Effect
 689     { .id = V4L2_CID_EFFECT,  .index = 0,  .name = "none",  .reserved = 0, }, {  .id = V4L2_CID_EFFECT,  .index = 1, .name = "mono",  .reserved = 0,},
 690     { .id = V4L2_CID_EFFECT,  .index = 2,  .name = "negative", .reserved = 0,}, {  .id = V4L2_CID_EFFECT, .index = 3,  .name = "sepia", .reserved = 0,},
 691     { .id = V4L2_CID_EFFECT,  .index = 4, .name = "posterize", .reserved = 0,} ,{ .id = V4L2_CID_EFFECT,  .index = 5,  .name = "aqua", .reserved = 0,},
 692     #endif
 693
 694         #if CONFIG_SENSOR_Scene
 695     { .id = V4L2_CID_SCENE,  .index = 0, .name = "auto", .reserved = 0,} ,{ .id = V4L2_CID_SCENE,  .index = 1,  .name = "night", .reserved = 0,},
 696     #endif
 697
 698         #if CONFIG_SENSOR_Flash
 699     { .id = V4L2_CID_FLASH,  .index = 0,  .name = "off",  .reserved = 0, }, {  .id = V4L2_CID_FLASH,  .index = 1, .name = "auto",  .reserved = 0,},
 700     { .id = V4L2_CID_FLASH,  .index = 2,  .name = "on", .reserved = 0,}, {  .id = V4L2_CID_FLASH, .index = 3,  .name = "torch", .reserved = 0,},
 701     #endif
 702 };
 703
 704 static const struct v4l2_queryctrl sensor_controls[] =
 705 {
 706         #if CONFIG_SENSOR_WhiteBalance
 707     {
 708         .id             = V4L2_CID_DO_WHITE_BALANCE,
 709         .type           = V4L2_CTRL_TYPE_MENU,
 710         .name           = "White Balance Control",
 711         .minimum        = 0,
 712         .maximum        = 4,
 713         .step           = 1,
 714         .default_value = 0,
 715     },
 716     #endif
 717
 718         #if CONFIG_SENSOR_Brightness
 719         {
 720         .id             = V4L2_CID_BRIGHTNESS,
 721         .type           = V4L2_CTRL_TYPE_INTEGER,
 722         .name           = "Brightness Control",
 723         .minimum        = -3,
 724         .maximum        = 2,
 725         .step           = 1,
 726         .default_value = 0,
 727     },
 728     #endif
 729
 730         #if CONFIG_SENSOR_Effect
 731         {
 732         .id             = V4L2_CID_EFFECT,
 733         .type           = V4L2_CTRL_TYPE_MENU,
 734         .name           = "Effect Control",
 735         .minimum        = 0,
 736         .maximum        = 5,
 737         .step           = 1,
 738         .default_value = 0,
 739     },
 740         #endif
 741
 742         #if CONFIG_SENSOR_Exposure
 743         {
 744         .id             = V4L2_CID_EXPOSURE,
 745         .type           = V4L2_CTRL_TYPE_INTEGER,
 746         .name           = "Exposure Control",
 747         .minimum        = 0,
 748         .maximum        = 6,
 749         .step           = 1,
 750         .default_value = 0,
 751     },
 752         #endif
 753
 754         #if CONFIG_SENSOR_Saturation
 755         {
 756         .id             = V4L2_CID_SATURATION,
 757         .type           = V4L2_CTRL_TYPE_INTEGER,
 758         .name           = "Saturation Control",
 759         .minimum        = 0,
 760         .maximum        = 2,
 761         .step           = 1,
 762         .default_value = 0,
 763     },
 764     #endif
 765
 766         #if CONFIG_SENSOR_Contrast
 767         {
 768         .id             = V4L2_CID_CONTRAST,
 769         .type           = V4L2_CTRL_TYPE_INTEGER,
 770         .name           = "Contrast Control",
 771         .minimum        = -3,
 772         .maximum        = 3,
 773         .step           = 1,
 774         .default_value = 0,
 775     },
 776         #endif
 777
 778         #if CONFIG_SENSOR_Mirror
 779         {
 780         .id             = V4L2_CID_HFLIP,
 781         .type           = V4L2_CTRL_TYPE_BOOLEAN,
 782         .name           = "Mirror Control",
 783         .minimum        = 0,
 784         .maximum        = 1,
 785         .step           = 1,
 786         .default_value = 0,
 787     },
 788     #endif
 789
 790         #if CONFIG_SENSOR_Flip
 791         {
 792         .id             = V4L2_CID_VFLIP,
 793         .type           = V4L2_CTRL_TYPE_BOOLEAN,
 794         .name           = "Flip Control",
 795         .minimum        = 0,
 796         .maximum        = 1,
 797         .step           = 1,
 798         .default_value = 0,
 799     },
 800     #endif
 801
 802         #if CONFIG_SENSOR_Scene
 803     {
 804         .id             = V4L2_CID_SCENE,
 805         .type           = V4L2_CTRL_TYPE_MENU,
 806         .name           = "Scene Control",
 807         .minimum        = 0,
 808         .maximum        = 1,
 809         .step           = 1,
 810         .default_value = 0,
 811     },
 812     #endif
 813
 814         #if CONFIG_SENSOR_DigitalZoom
 815     {
 816         .id             = V4L2_CID_ZOOM_RELATIVE,
 817         .type           = V4L2_CTRL_TYPE_INTEGER,
 818         .name           = "DigitalZoom Control",
 819         .minimum        = -1,
 820         .maximum        = 1,
 821         .step           = 1,
 822         .default_value = 0,
 823     }, {
 824         .id             = V4L2_CID_ZOOM_ABSOLUTE,
 825         .type           = V4L2_CTRL_TYPE_INTEGER,
 826         .name           = "DigitalZoom Control",
 827         .minimum        = 0,
 828         .maximum        = 3,
 829         .step           = 1,
 830         .default_value = 0,
 831     },
 832     #endif
 833
 834         #if CONFIG_SENSOR_Focus
 835         {
 836         .id             = V4L2_CID_FOCUS_RELATIVE,
 837         .type           = V4L2_CTRL_TYPE_INTEGER,
 838         .name           = "Focus Control",
 839         .minimum        = -1,
 840         .maximum        = 1,
 841         .step           = 1,
 842         .default_value = 0,
 843     }, {
 844         .id             = V4L2_CID_FOCUS_ABSOLUTE,
 845         .type           = V4L2_CTRL_TYPE_INTEGER,
 846         .name           = "Focus Control",
 847         .minimum        = 0,
 848         .maximum        = 255,
 849         .step           = 1,
 850         .default_value = 125,
 851     },
 852     #endif
 853
 854         #if CONFIG_SENSOR_Flash
 855         {
 856         .id             = V4L2_CID_FLASH,
 857         .type           = V4L2_CTRL_TYPE_MENU,
 858         .name           = "Flash Control",
 859         .minimum        = 0,
 860         .maximum        = 3,
 861         .step           = 1,
 862         .default_value = 0,
 863     },
 864         #endif
 865 };
 866
 867 static int sensor_probe(struct i2c_client *client, const struct i2c_device_id *did);
 868 static int sensor_video_probe(struct soc_camera_device *icd, struct i2c_client *client);
 869 static int sensor_g_control(struct v4l2_subdev *sd, struct v4l2_control *ctrl);
 870 static int sensor_s_control(struct v4l2_subdev *sd, struct v4l2_control *ctrl);
 871 static int sensor_g_ext_controls(struct v4l2_subdev *sd,  struct v4l2_ext_controls *ext_ctrl);
 872 static int sensor_s_ext_controls(struct v4l2_subdev *sd,  struct v4l2_ext_controls *ext_ctrl);
 873 static int sensor_suspend(struct soc_camera_device *icd, pm_message_t pm_msg);
 874 static int sensor_resume(struct soc_camera_device *icd);
 875 static int sensor_set_bus_param(struct soc_camera_device *icd,unsigned long flags);
 876 static unsigned long sensor_query_bus_param(struct soc_camera_device *icd);
 877 #if CONFIG_SENSOR_Effect
 878 static int sensor_set_effect(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value);
 879 #endif
 880 #if CONFIG_SENSOR_WhiteBalance
 881 static int sensor_set_whiteBalance(struct soc_camera_device *icd, const struct v4l2_queryctrl *qctrl, int value);
 882 #endif
 883 static int sensor_deactivate(struct i2c_client *client);
 884
 885 static struct soc_camera_ops sensor_ops =
 886 {
 887     .suspend                     = sensor_suspend,
 888     .resume                       = sensor_resume,
 889     .set_bus_param              = sensor_set_bus_param,
 890     .query_bus_param    = sensor_query_bus_param,
 891     .controls           = sensor_controls,
 892     .menus                         = sensor_menus,
 893     .num_controls               = ARRAY_SIZE(sensor_controls),
 894     .num_menus          = ARRAY_SIZE(sensor_menus),
 895 };
 896
 897 /* only one fixed colorspace per pixelcode */
 898 struct sensor_datafmt {
 899         enum v4l2_mbus_pixelcode code;
 900         enum v4l2_colorspace colorspace;
 901 };
 902
 903 /* Find a data format by a pixel code in an array */
 904 static const struct sensor_datafmt *sensor_find_datafmt(
 905         enum v4l2_mbus_pixelcode code, const struct sensor_datafmt *fmt,
 906         int n)
 907 {
 908         int i;
 909         for (i = 0; i < n; i++)
 910                 if (fmt[i].code == code)
 911                         return fmt + i;
 912
 913         return NULL;
 914 }
 915
 916 static const struct sensor_datafmt sensor_colour_fmts[] = {
 917     {V4L2_MBUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG},
 918     {V4L2_MBUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG}
 919 };
 920
 921 typedef struct sensor_info_priv_s
 922 {
 923     int whiteBalance;
 924     int brightness;
 925     int contrast;
 926     int saturation;
 927     int effect;
 928     int scene;
 929     int digitalzoom;
 930     int focus;
 931     int flash;
 932     int exposure;
 933         bool snap2preview;
 934         bool video2preview;
 935     unsigned char mirror;                                        /* HFLIP */
 936     unsigned char flip;                                          /* VFLIP */
 937     unsigned int winseqe_cur_addr;
 938     struct sensor_datafmt fmt;
 939     unsigned int funmodule_state;
 940 } sensor_info_priv_t;
 941
 942 struct sensor
 943 {
 944     struct v4l2_subdev subdev;
 945     struct i2c_client *client;
 946     sensor_info_priv_t info_priv;
 947     int model;  /* V4L2_IDENT_OV* codes from v4l2-chip-ident.h */
 948 #if CONFIG_SENSOR_I2C_NOSCHED
 949         atomic_t tasklock_cnt;
 950 #endif
 951         struct rk29camera_platform_data *sensor_io_request;
 952     struct rk29camera_gpio_res *sensor_gpio_res;
 953 };
 954
 955
 956 static struct sensor* to_sensor(const struct i2c_client *client)
 957 {
 958     return container_of(i2c_get_clientdata(client), struct sensor, subdev);
 959 }
 960
 961 static int sensor_task_lock(struct i2c_client *client, int lock)
 962 {
 963 #if CONFIG_SENSOR_I2C_NOSCHED
 964         int cnt = 3;
 965     struct sensor *sensor = to_sensor(client);
 966
 967         if (lock) {
 968                 if (atomic_read(&sensor->tasklock_cnt) == 0) {
 969                         while ((atomic_read(&client->adapter->bus_lock.count) < 1) && (cnt>0)) {
 970                                 SENSOR_TR("\n %s will obtain i2c in atomic, but i2c bus is locked! Wait...\n",SENSOR_NAME_STRING());
 971                                 msleep(35);
 972                                 cnt--;
 973                         }
 974                         if ((atomic_read(&client->adapter->bus_lock.count) < 1) && (cnt<=0)) {
 975                                 SENSOR_TR("\n %s obtain i2c fail in atomic!!\n",SENSOR_NAME_STRING());
 976                                 goto sensor_task_lock_err;
 977                         }
 978                         preempt_disable();
 979                 }
 980
 981                 atomic_add(1, &sensor->tasklock_cnt);
 982         } else {
 983                 if (atomic_read(&sensor->tasklock_cnt) > 0) {
 984                         atomic_sub(1, &sensor->tasklock_cnt);
 985
 986                         if (atomic_read(&sensor->tasklock_cnt) == 0)
 987                                 preempt_enable();
 988                 }
 989         }
 990         return 0;
 991 sensor_task_lock_err:
 992         return -1;
 993 #else
 994     return 0;
 995 #endif
 996
 997 }
 998
 999 /* sensor register write */
1000 static int sensor_write(struct i2c_client *client, u8 reg, u8 val)
1001 {
1002     int err,cnt;
1003     u8 buf[2];
1004     struct i2c_msg msg[1];
1005
1006     buf[0] = reg & 0xFF;
1007     buf[1] = val;
1008
1009     msg->addr = client->addr;
1010     msg->flags = client->flags;
1011     msg->buf = buf;
1012     msg->len = sizeof(buf);
1013     msg->scl_rate = CONFIG_SENSOR_I2C_SPEED;                                        /* ddl@rock-chips.com : 100kHz */
1014     msg->read_type = 0;               /* fpga i2c:0==I2C_NORMAL : direct use number not enum for don't want include spi_fpga.h */
1015
1016     cnt = 3;
1017     err = -EAGAIN;
1018
1019     while ((cnt-- > 0) && (err < 0)) {                       /* ddl@rock-chips.com :  Transfer again if transent is failed   */
1020         err = i2c_transfer(client->adapter, msg, 1);
1021                                 udelay(50);
1022         if (err >= 0) {
1023             return 0;
1024         } else {
1025                 SENSOR_TR("\n %s write reg(0x%x, val:0x%x) failed, try to write again!\n",SENSOR_NAME_STRING(),reg, val);
1026                 udelay(10);
1027         }
1028     }
1029
1030     return err;
1031 }
1032
1033 /* sensor register read */
1034 static int sensor_read(struct i2c_client *client, u8 reg, u8 *val)
1035 {
1036     int err,cnt;
1037     u8 buf[1];
1038     struct i2c_msg msg[2];
1039
1040     buf[0] = reg;
1041
1042     msg[0].addr = client->addr;
1043     msg[0].flags = client->flags;
1044     msg[0].buf = buf;
1045     msg[0].len = sizeof(buf);
1046     msg[0].scl_rate = CONFIG_SENSOR_I2C_SPEED;       /* ddl@rock-chips.com : 100kHz */
1047     msg[0].read_type = 2;   /* fpga i2c:0==I2C_NO_STOP : direct use number not enum for don't want include spi_fpga.h */
1048
1049     msg[1].addr = client->addr;
1050     msg[1].flags = client->flags|I2C_M_RD;
1051     msg[1].buf = buf;
1052     msg[1].len = 1;
1053     msg[1].scl_rate = CONFIG_SENSOR_I2C_SPEED;                       /* ddl@rock-chips.com : 100kHz */
1054     msg[1].read_type = 2;                             /* fpga i2c:0==I2C_NO_STOP : direct use number not enum for don't want include spi_fpga.h */
1055
1056     cnt = 1;
1057     err = -EAGAIN;
1058     while ((cnt-- > 0) && (err < 0)) {                       /* ddl@rock-chips.com :  Transfer again if transent is failed   */
1059         err = i2c_transfer(client->adapter, msg, 2);
1060
1061         if (err >= 0) {
1062             *val = buf[0];
1063             return 0;
1064         } else {
1065                 SENSOR_TR("\n %s read reg(0x%x val:0x%x) failed, try to read again! \n",SENSOR_NAME_STRING(),reg, *val);
1066             udelay(10);
1067         }
1068     }
1069
1070     return err;
1071 }
1072
1073 /* write a array of registers  */
1074 static int sensor_write_array(struct i2c_client *client, struct reginfo *regarray)
1075 {
1076     int err = 0, cnt;
1077     int i = 0;
1078 #if CONFIG_SENSOR_I2C_RDWRCHK
1079         char valchk;
1080 #endif
1081
1082         cnt = 0;
1083         if (sensor_task_lock(client, 1) < 0)
1084                 goto sensor_write_array_end;
1085
1086     while (regarray[i].reg != 0)
1087     {
1088         err = sensor_write(client, regarray[i].reg, regarray[i].val);
1089         if (err < 0)
1090         {
1091             if (cnt-- > 0) {
1092                             SENSOR_TR("%s..write failed current reg:0x%x, Write array again !\n", SENSOR_NAME_STRING(),regarray[i].reg);
1093                                 i = 0;
1094                                 continue;
1095             } else {
1096                 SENSOR_TR("%s..write array failed!!!\n", SENSOR_NAME_STRING());
1097                 err = -EPERM;
1098                                 goto sensor_write_array_end;
1099             }
1100         } else {
1101         #if CONFIG_SENSOR_I2C_RDWRCHK
1102                         sensor_read(client, regarray[i].reg, &valchk);
1103                         if (valchk != regarray[i].val)
1104                                 SENSOR_TR("%s Reg:0x%x write(0x%x, 0x%x) fail\n",SENSOR_NAME_STRING(), regarray[i].reg, regarray[i].val, valchk);
1105                 #endif
1106         }
1107         i++;
1108     }
1109
1110 sensor_write_array_end:
1111         sensor_task_lock(client,0);
1112         return err;
1113 }
1114 #if CONFIG_SENSOR_I2C_RDWRCHK
1115 static int sensor_readchk_array(struct i2c_client *client, struct reginfo *regarray)
1116 {
1117     int cnt;
1118     int i = 0;
1119         char valchk;
1120
1121         cnt = 0;
1122         valchk = 0;
1123     while (regarray[i].reg != 0)
1124     {
1125                 sensor_read(client, regarray[i].reg, &valchk);
1126                 if (valchk != regarray[i].val)
1127                         SENSOR_TR("%s Reg:0x%x read(0x%x, 0x%x) error\n",SENSOR_NAME_STRING(), regarray[i].reg, regarray[i].val, valchk);
1128
1129         i++;
1130     }
1131     return 0;
1132 }
1133 #endif
1134 static int sensor_ioctrl(struct soc_camera_device *icd,enum rk29sensor_power_cmd cmd, int on)
1135 {
1136         struct soc_camera_link *icl = to_soc_camera_link(icd);
1137         int ret = 0;
1138
1139     SENSOR_DG("%s %s  cmd(%d) on(%d)\n",SENSOR_NAME_STRING(),__FUNCTION__,cmd,on);
1140         switch (cmd)
1141         {
1142                 case Sensor_PowerDown:
1143                 {
1144                         if (icl->powerdown) {
1145                                 if (on == 0)
1146                                         mdelay(1);
1147                                 ret = icl->powerdown(icd->pdev, on);
1148                                 if (ret == RK29_CAM_IO_SUCCESS) {
1149                                         if (on == 0) {
1150                                                 mdelay(20);
1151                                                 if (icl->reset)
1152                                                         icl->reset(icd->pdev);
1153                                         }
1154                                 } else if (ret == RK29_CAM_EIO_REQUESTFAIL) {
1155                                         ret = -ENODEV;
1156                                         goto sensor_power_end;
1157                                 }
1158                         }
1159                         break;
1160                 }
1161                 case Sensor_Flash:
1162                 {
1163                         struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1164                 struct sensor *sensor = to_sensor(client);
1165
1166                         if (sensor->sensor_io_request && sensor->sensor_io_request->sensor_ioctrl) {
1167                                 sensor->sensor_io_request->sensor_ioctrl(icd->pdev,Cam_Flash, on);
1168                         }
1169             break;
1170                 }
1171                 default:
1172                 {
1173                         SENSOR_TR("%s %s cmd(0x%x) is unknown!",SENSOR_NAME_STRING(),__FUNCTION__,cmd);
1174                         break;
1175                 }
1176         }
1177 sensor_power_end:
1178         return ret;
1179 }
1180 static s32 sensor_init_width = 640;
1181 static s32 sensor_init_height = 480;
1182 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);
1183 static enum v4l2_mbus_pixelcode sensor_init_pixelcode = V4L2_MBUS_FMT_YUYV8_2X8;
1184 static struct reginfo* sensor_init_data_p = NULL;
1185 static struct reginfo* sensor_init_winseq_p = NULL;
1186 static struct reginfo* sensor_init_winseq_board = NULL;
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 rk29camera_platform_data* tmp_plat_data =(struct rk29camera_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 && (u32)tmp_plat_data != 1) {
1201                 for(i = 0;i < RK_CAM_NUM;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  && ((u32)tmp_plat_data != 1) &&(i < RK_CAM_NUM) && 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 RK29_CAM_SUBDEV_DEACTIVATE:
2441                 {
2442                         sensor_deactivate(client);
2443                         break;
2444                 }
2445
2446                 case RK29_CAM_SUBDEV_IOREQUEST:
2447                 {
2448                         sensor->sensor_io_request = (struct rk29camera_platform_data*)arg;
2449                         if (sensor->sensor_io_request != NULL) {
2450                         int j = 0;
2451                         for(j = 0;j < RK_CAM_NUM;j++){
2452                                         if (sensor->sensor_io_request->gpio_res[j].dev_name &&
2453                                                 (strcmp(sensor->sensor_io_request->gpio_res[j].dev_name, dev_name(icd->pdev)) == 0)) {
2454                                                 sensor->sensor_gpio_res = (struct rk29camera_gpio_res*)&sensor->sensor_io_request->gpio_res[j];
2455                                 break;
2456                                           }
2457                         }
2458                         if(j == RK_CAM_NUM){
2459                                 SENSOR_TR("%s %s RK_CAM_SUBDEV_IOREQUEST fail\n",SENSOR_NAME_STRING(),__FUNCTION__);
2460                                 ret = -EINVAL;
2461                                 goto sensor_ioctl_end;
2462                                 }
2463                                 }
2464             /* ddl@rock-chips.com : if gpio_flash havn't been set in board-xxx.c, sensor driver must notify is not support flash control
2465                for this project */
2466             #if CONFIG_SENSOR_Flash
2467                 if (sensor->sensor_gpio_res) {
2468                 if (sensor->sensor_gpio_res->gpio_flash == INVALID_GPIO) {
2469                     for (i = 0; i < icd->ops->num_controls; i++) {
2470                                 if (V4L2_CID_FLASH == icd->ops->controls[i].id) {
2471                                         memset((char*)&icd->ops->controls[i],0x00,sizeof(struct v4l2_queryctrl));
2472                                 }
2473                     }
2474                     sensor->info_priv.flash = 0xff;
2475                     SENSOR_DG("%s flash gpio is invalidate!\n",SENSOR_NAME_STRING());
2476                 }
2477                 }
2478             #endif
2479                         break;
2480                 }
2481                 default:
2482                 {
2483                         SENSOR_TR("%s %s cmd(0x%x) is unknown !\n",SENSOR_NAME_STRING(),__FUNCTION__,cmd);
2484                         break;
2485                 }
2486         }
2487 sensor_ioctl_end:
2488         return ret;
2489
2490 }
2491 static int sensor_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
2492                             enum v4l2_mbus_pixelcode *code)
2493 {
2494         if (index >= ARRAY_SIZE(sensor_colour_fmts))
2495                 return -EINVAL;
2496
2497         *code = sensor_colour_fmts[index].code;
2498         return 0;
2499 }
2500 static struct v4l2_subdev_core_ops sensor_subdev_core_ops = {
2501         .init           = sensor_init,
2502         .g_ctrl         = sensor_g_control,
2503         .s_ctrl         = sensor_s_control,
2504         .g_ext_ctrls          = sensor_g_ext_controls,
2505         .s_ext_ctrls          = sensor_s_ext_controls,
2506         .g_chip_ident   = sensor_g_chip_ident,
2507         .ioctl = sensor_ioctl,
2508 };
2509
2510 static struct v4l2_subdev_video_ops sensor_subdev_video_ops = {
2511         .s_mbus_fmt     = sensor_s_fmt,
2512         .g_mbus_fmt     = sensor_g_fmt,
2513         .try_mbus_fmt   = sensor_try_fmt,
2514         .enum_mbus_fmt  = sensor_enum_fmt,
2515 };
2516 static struct v4l2_subdev_ops sensor_subdev_ops = {
2517         .core   = &sensor_subdev_core_ops,
2518         .video = &sensor_subdev_video_ops,
2519 };
2520
2521 static int sensor_probe(struct i2c_client *client,
2522                          const struct i2c_device_id *did)
2523 {
2524     struct sensor *sensor;
2525     struct soc_camera_device *icd = client->dev.platform_data;
2526     struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
2527     struct soc_camera_link *icl;
2528     int ret;
2529
2530     SENSOR_DG("\n%s..%s..%d..\n",__FUNCTION__,__FILE__,__LINE__);
2531     if (!icd) {
2532         dev_err(&client->dev, "%s: missing soc-camera data!\n",SENSOR_NAME_STRING());
2533         return -EINVAL;
2534     }
2535
2536     icl = to_soc_camera_link(icd);
2537     if (!icl) {
2538         dev_err(&client->dev, "%s driver needs platform data\n", SENSOR_NAME_STRING());
2539         return -EINVAL;
2540     }
2541
2542     if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
2543         dev_warn(&adapter->dev,
2544                  "I2C-Adapter doesn't support I2C_FUNC_I2C\n");
2545         return -EIO;
2546     }
2547
2548     sensor = kzalloc(sizeof(struct sensor), GFP_KERNEL);
2549     if (!sensor)
2550         return -ENOMEM;
2551
2552     v4l2_i2c_subdev_init(&sensor->subdev, client, &sensor_subdev_ops);
2553
2554     /* Second stage probe - when a capture adapter is there */
2555     icd->ops            = &sensor_ops;
2556
2557     sensor->info_priv.fmt = sensor_colour_fmts[0];
2558
2559         #if CONFIG_SENSOR_I2C_NOSCHED
2560         atomic_set(&sensor->tasklock_cnt,0);
2561         #endif
2562
2563     ret = sensor_video_probe(icd, client);
2564     if (ret < 0) {
2565         icd->ops = NULL;
2566         i2c_set_clientdata(client, NULL);
2567         kfree(sensor);
2568                 sensor = NULL;
2569     }
2570     SENSOR_DG("\n%s..%s..%d  ret = %x \n",__FUNCTION__,__FILE__,__LINE__,ret);
2571     return ret;
2572 }
2573
2574 static int sensor_remove(struct i2c_client *client)
2575 {
2576     struct sensor *sensor = to_sensor(client);
2577     struct soc_camera_device *icd = client->dev.platform_data;
2578
2579     icd->ops = NULL;
2580     i2c_set_clientdata(client, NULL);
2581     client->driver = NULL;
2582     kfree(sensor);
2583         sensor = NULL;
2584     return 0;
2585 }
2586
2587 static const struct i2c_device_id sensor_id[] = {
2588         {SENSOR_NAME_STRING(), 0 },
2589         { }
2590 };
2591 MODULE_DEVICE_TABLE(i2c, sensor_id);
2592
2593 static struct i2c_driver sensor_i2c_driver = {
2594         .driver = {
2595                 .name = SENSOR_NAME_STRING(),
2596         },
2597         .probe          = sensor_probe,
2598         .remove         = sensor_remove,
2599         .id_table       = sensor_id,
2600 };
2601
2602 static int __init sensor_mod_init(void)
2603 {
2604     SENSOR_DG("\n%s..%s.. \n",__FUNCTION__,SENSOR_NAME_STRING());
2605     return i2c_add_driver(&sensor_i2c_driver);
2606 }
2607
2608 static void __exit sensor_mod_exit(void)
2609 {
2610     i2c_del_driver(&sensor_i2c_driver);
2611 }
2612
2613 device_initcall_sync(sensor_mod_init);
2614 module_exit(sensor_mod_exit);
2615
2616 MODULE_DESCRIPTION(SENSOR_NAME_STRING(Camera sensor driver));
2617 MODULE_AUTHOR("ddl <kernel@rock-chips>");
2618 MODULE_LICENSE("GPL");
2619
2620