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