drivers/media/video/generic_sensor.c

   1 #include <linux/videodev2.h>
   2 #include <linux/slab.h>
   3 #include <linux/i2c.h>
   4 #include <linux/log2.h>
   5 #include <linux/platform_device.h>
   6 #include <linux/delay.h>
   7 #include <linux/circ_buf.h>
   8 #include <linux/miscdevice.h>
   9 #include <media/v4l2-common.h>
  10 #include <media/v4l2-chip-ident.h>
  11 #include <media/soc_camera.h>
  12 #include <linux/vmalloc.h>
  13 #include <linux/hardirq.h>
  14 #include "generic_sensor.h"
  15 #include <linux/moduleparam.h>
  16
  17 /*
  18 *      Driver Version Note
  19 *v0.0.1: this driver is compatible with generic_sensor
  20 *v0.1.1:
  21 *        add WqCmd_af_continues_pause;
  22 *v0.1.3:
  23 *        add support flash control;
  24 *
  25 *v0.1.5/v0.1.7:
  26 *        fix use v4l2_mbus_framefmt.reserved array overflow in generic_sensor_s_fmt;
  27 *v0.1.9:
  28 *        fix sensor_find_ctrl may be overflow;
  29 *v0.1.b:
  30 *        1. support sensor driver crop by redefine SENSOR_CROP_PERCENT;
  31 *        2. fix sensor_ops which is independent for driver;
  32 *        3. support cropcap;
  33 *v.0.1.c:
  34 *        1. modify generic_sensor_s_fmt, flash will work everytime when capture
  35 *v.0.1.d:
  36                  1. add some callbacks for icatch
  37 */
  38 static int version = KERNEL_VERSION(0,1,0xd);
  39 module_param(version, int, S_IRUGO);
  40
  41
  42 static int debug = 0;
  43 module_param(debug, int, S_IRUGO|S_IWUSR);
  44
  45 #define CAMMODULE_NAME    "rk_cam_sensor"
  46
  47 #define dprintk(level, fmt, arg...) do {                        \
  48         if (debug >= level)                                     \
  49             printk(KERN_WARNING fmt , ## arg); } while (0)
  50
  51 #define SENSOR_NAME_STRING() sensor->dev_name
  52
  53 #undef SENSOR_TR
  54 #undef SENSOR_DG
  55 #define SENSOR_TR(format, ...) printk(KERN_ERR "%s(%s:%d): " format"\n", SENSOR_NAME_STRING(),CAMMODULE_NAME,__LINE__, ## __VA_ARGS__)
  56 #define SENSOR_DG(format, ...) dprintk(1, "%s(%s:%d): "format"\n", SENSOR_NAME_STRING(),CAMMODULE_NAME,__LINE__,## __VA_ARGS__)
  57 #define debug_printk(format, ...) dprintk(3, "%s(%s:%d): "format"\n", SENSOR_NAME_STRING(),CAMMODULE_NAME,__LINE__,## __VA_ARGS__)
  58
  59
  60 #define CONFIG_SENSOR_I2C_RDWRCHK 0
  61
  62 static const struct rk_sensor_datafmt *generic_sensor_find_datafmt(
  63         enum v4l2_mbus_pixelcode code, const struct rk_sensor_datafmt *fmt,
  64         int n);
  65
  66 int sensor_write_reg2val1(struct i2c_client *client, u16 reg,u8 val){
  67         struct rk_sensor_reg tmp_reg;
  68
  69         tmp_reg.reg_mask = 0xffff;
  70         tmp_reg.val_mask = 0xff;
  71         tmp_reg.reg = reg;
  72         tmp_reg.val = val;
  73         return generic_sensor_write(client, &tmp_reg);
  74 }
  75 int sensor_write_reg2val2(struct i2c_client *client, u16 reg,u16 val){
  76         struct rk_sensor_reg tmp_reg;
  77
  78         tmp_reg.reg_mask = 0xffff;
  79         tmp_reg.val_mask = 0xffff;
  80         tmp_reg.reg = reg;
  81         tmp_reg.val = val;
  82         return generic_sensor_write(client, &tmp_reg);
  83 }
  84 int sensor_write_reg1val1(struct i2c_client *client, u8 reg,u8 val){
  85         struct rk_sensor_reg tmp_reg;
  86
  87         tmp_reg.reg_mask = 0xff;
  88         tmp_reg.val_mask = 0xff;
  89         tmp_reg.reg = reg;
  90         tmp_reg.val = val;
  91         return generic_sensor_write(client, &tmp_reg);
  92 }
  93 int sensor_write_reg1val2(struct i2c_client *client, u8 reg,u16 val){
  94         struct rk_sensor_reg tmp_reg;
  95
  96         tmp_reg.reg_mask = 0xff;
  97         tmp_reg.val_mask = 0xffff;
  98         tmp_reg.reg = reg;
  99         tmp_reg.val = val;
 100         return generic_sensor_write(client, &tmp_reg);
 101 }
 102 int sensor_write_reg0val0(struct i2c_client *client, u8 reg,u16 val)
 103 {
 104     struct generic_sensor *sensor = to_generic_sensor(client);
 105
 106     SENSOR_TR("SENSOR_REGISTER_LEN and SENSOR_VALUE_LEN is 0, please use generic_sensor_write directly!");
 107     return -1;
 108 }
 109 /* sensor register write */
 110 int generic_sensor_write(struct i2c_client *client,struct rk_sensor_reg* sensor_reg)
 111 {
 112         int err,cnt = 0,i;
 113         u8 buf[6];
 114         struct i2c_msg msg[1];
 115         u32 i2c_speed;
 116     struct generic_sensor *sensor = to_generic_sensor(client);
 117
 118         i2c_speed = sensor->info_priv.gI2c_speed;
 119         //debug_printk( "/$$$$$$$$$$$$$$$$$$$$$$//n Here I am: %s:%i-------%s()\n", __FILE__, __LINE__,__FUNCTION__);
 120         //debug_printk( "/~~~~~~~~~~~~/ %s:%i-------%s()client = %p\n", __FILE__, __LINE__,__FUNCTION__,client);
 121
 122         err = 0;
 123         switch(sensor_reg->reg){
 124                 case SEQCMD_WAIT_MS:
 125             if (in_atomic())
 126                 mdelay(sensor_reg->val);
 127             else
 128                             msleep(sensor_reg->val);
 129                         break;
 130                 case SEQCMD_WAIT_US:
 131                         udelay(sensor_reg->val);
 132                         break;
 133                 default:
 134             cnt=0;
 135             for (i=2; i>=0; i--) {
 136                 if(((sensor_reg->reg_mask) & (0xff<<(i*8)))) {
 137                     buf[cnt++] = ((sensor_reg->reg)>>(i*8))&0xff;
 138                 }
 139             }
 140             for (i=2; i>=0; i--) {
 141                 if(((sensor_reg->val_mask) & (0xff<<(i*8)))) {
 142                     buf[cnt++] = ((sensor_reg->val)>>(i*8))&0xff;
 143                 }
 144             }
 145
 146                         msg->addr = client->addr;
 147                         msg->flags = client->flags;
 148                         msg->buf = buf;
 149                         msg->scl_rate = i2c_speed;               /* ddl@rock-chips.com : 100kHz */
 150                 //      msg->read_type = 0;                       /* fpga i2c:0==I2C_NORMAL : direct use number not enum for don't want include spi_fpga.h */
 151                         msg->len = cnt;
 152                         cnt = 3;
 153                         err = -EAGAIN;
 154                         /*
 155                         debug_printk( "/___________/msg->addr = %x \n",msg->addr);
 156                         debug_printk( "/___________/msg->flags = %x\n",msg->flags);
 157                         debug_printk( "/___________/msg->buf = %d\n",msg->buf);
 158                         debug_printk( "/___________/msg->scl_rate = %d\n",msg->scl_rate);
 159                         debug_printk( "/___________/msg->len = %d\n",msg->len);
 160                         debug_printk( "/___________/client->adapter = %p\n",client->adapter);
 161                         */
 162                         while ((cnt-- > 0) && (err < 0)) {                                               /* ddl@rock-chips.com :  Transfer again if transent is failed   */
 163                                 err = i2c_transfer(client->adapter, msg, 1);
 164
 165                                 if (err >= 0) {
 166                     err = 0;
 167                                         goto write_end;
 168                                 } else {
 169                                         SENSOR_TR("write reg(0x%x, val:0x%x) failed, try to write again!",sensor_reg->reg, sensor_reg->val);
 170                                         udelay(10);
 171                                 }
 172                         }
 173
 174         }
 175
 176 write_end:
 177         return err;
 178 }
 179
 180 /* sensor register write buffer */
 181 int generic_sensor_writebuf(struct i2c_client *client, char *buf, int buf_size)
 182 {
 183         int err=0,cnt = 0;
 184         struct i2c_msg msg[1];
 185     struct generic_sensor *sensor = to_generic_sensor(client);
 186
 187         msg->addr = client->addr;
 188         msg->flags = client->flags;
 189         msg->buf = buf;
 190         msg->scl_rate = sensor->info_priv.gI2c_speed;            /* ddl@rock-chips.com : 100kHz */
 191         //msg->read_type = 0;
 192         msg->len = buf_size;
 193         cnt = 3;
 194         err = -EAGAIN;
 195
 196         while ((cnt-- > 0) && (err < 0)) {                                               /* ddl@rock-chips.com :  Transfer again if transent is failed   */
 197                 err = i2c_transfer(client->adapter, msg, 1);
 198
 199                 if (err >= 0) {
 200             err = 0;
 201                         goto write_end;
 202                 } else {
 203                         SENSOR_TR("generic_sensor_writebuf failed!");
 204                         udelay(10);
 205                 }
 206         }
 207
 208
 209 write_end:
 210         return err;
 211 }
 212 int sensor_read_reg1val1(struct i2c_client *client, u8 reg,u8* val){
 213
 214         struct rk_sensor_reg tmp_reg;
 215
 216         tmp_reg.reg_mask = 0xff;
 217         tmp_reg.val_mask = 0xff;
 218         tmp_reg.reg = reg;
 219         tmp_reg.val = 0;
 220         if(generic_sensor_read(client, &tmp_reg)==0){
 221                 *val = (u8)(tmp_reg.val & tmp_reg.val_mask);
 222         }else{
 223                 return -1;
 224         }
 225         return 0;
 226 }
 227 int sensor_read_reg2val1(struct i2c_client *client, u16 reg,u8* val){
 228
 229         struct rk_sensor_reg tmp_reg;
 230
 231         tmp_reg.reg_mask = 0xffff;
 232         tmp_reg.val_mask = 0xff;
 233         tmp_reg.reg = reg;
 234         tmp_reg.val = 0;
 235         if(generic_sensor_read(client, &tmp_reg)==0){
 236                 *val = (u8)(tmp_reg.val & tmp_reg.val_mask);
 237         }else{
 238                 return -1;
 239         }
 240         return 0;
 241 }
 242 int sensor_read_reg2val2(struct i2c_client *client, u16 reg,u16* val){
 243
 244         struct rk_sensor_reg tmp_reg;
 245
 246         tmp_reg.reg_mask = 0xffff;
 247         tmp_reg.val_mask = 0xffff;
 248         tmp_reg.reg = reg;
 249         tmp_reg.val = 0;
 250         if(generic_sensor_read(client, &tmp_reg)==0){
 251                 *val = (u16)(tmp_reg.val & tmp_reg.val_mask);
 252         }else{
 253                 return -1;
 254         }
 255         return 0;
 256 }
 257 int sensor_read_reg1val2(struct i2c_client *client, u8 reg,u16* val){
 258
 259         struct rk_sensor_reg tmp_reg;
 260
 261         tmp_reg.reg_mask = 0xff;
 262         tmp_reg.val_mask = 0xffff;
 263         tmp_reg.reg = reg;
 264         tmp_reg.val = 0;
 265         if(generic_sensor_read(client, &tmp_reg)==0){
 266                 *val = (u16)(tmp_reg.val & tmp_reg.val_mask);
 267         }else{
 268                 return -1;
 269         }
 270         return 0;
 271 }
 272 int sensor_read_reg0val0(struct i2c_client *client, u8 reg,u16 val)
 273 {
 274     struct generic_sensor *sensor = to_generic_sensor(client);
 275
 276     SENSOR_TR("SENSOR_REGISTER_LEN and SENSOR_VALUE_LEN is 0, please use generic_sensor_read directly!");
 277     return -1;
 278 }
 279 /* sensor register read */
 280 int generic_sensor_read(struct i2c_client *client, struct rk_sensor_reg* sensor_reg)
 281 {
 282         int err,cnt = 0,i,bytes;
 283         u8 buf_reg[3];
 284         u8 buf_val[3];
 285         struct i2c_msg msg[2];
 286         u32 i2c_speed;
 287     struct generic_sensor *sensor = to_generic_sensor(client);
 288         debug_printk( "/$$$$$$$$$$$$$$$$$$$$$$//n Here I am: %s:%i-------%s()\n", __FILE__, __LINE__,__FUNCTION__);
 289         i2c_speed = sensor->info_priv.gI2c_speed;
 290
 291     cnt=0;
 292     for (i=2; i>=0; i--) {
 293         if((sensor_reg->reg_mask) & (0xff<<(i*8))) {
 294             buf_reg[cnt++] = ((sensor_reg->reg)>>(i*8))&0xff;
 295         }
 296     }
 297
 298         msg[0].addr = client->addr;
 299         msg[0].flags = client->flags;
 300         msg[0].buf = buf_reg;
 301         msg[0].scl_rate = i2c_speed;             /* ddl@rock-chips.com : 100kHz */
 302         //msg[0].read_type = 2; /* fpga i2c:0==I2C_NO_STOP : direct use number not enum for don't want include spi_fpga.h */
 303         msg[0].len = cnt;
 304
 305     cnt=0;
 306     for (i=2; i>=0; i--) {
 307         if((sensor_reg->val_mask) & (0xff<<(i*8))) {
 308             cnt++;
 309         }
 310     }
 311     memset(buf_val,0x00,sizeof(buf_val));
 312
 313         msg[1].addr = client->addr;
 314         msg[1].flags = client->flags|I2C_M_RD;
 315         msg[1].buf = buf_val;
 316         msg[1].len = cnt;
 317         msg[1].scl_rate = i2c_speed;                                             /* ddl@rock-chips.com : 100kHz */
 318         //msg[1].read_type = 2;                                                   /* fpga i2c:0==I2C_NO_STOP : direct use number not enum for don't want include spi_fpga.h */
 319
 320         cnt = 1;
 321         err = -EAGAIN;
 322         while ((cnt-- > 0) && (err < 0)) {                                               /* ddl@rock-chips.com :  Transfer again if transent is failed   */
 323                 err = i2c_transfer(client->adapter, msg, 2);
 324                 if (err >= 0) {
 325             sensor_reg->val=0;
 326             bytes = 0x00;
 327             for (i=2; i>=0; i--) {
 328                 if((sensor_reg->val_mask) & (0xff<<(i*8))) {
 329                     sensor_reg->val |= (buf_val[bytes++]<<(i*8));
 330                 }
 331             }
 332                         err = 0;
 333             goto read_end;
 334                 } else {
 335                         SENSOR_TR("read reg(0x%x val:0x%x) failed, try to read again!",sensor_reg->reg, sensor_reg->val);
 336                         udelay(10);
 337                 }
 338         }
 339 read_end:
 340         return err;
 341 }
 342
 343 /* write a array of registers  */
 344  int generic_sensor_write_array(struct i2c_client *client, struct rk_sensor_reg *regarray)
 345 {
 346         int err = 0, cnt;
 347         int i = 0;
 348     bool streamchk;
 349
 350 #if CONFIG_SENSOR_I2C_RDWRCHK
 351         struct rk_sensor_reg check_reg;
 352 #endif
 353         struct generic_sensor *sensor = to_generic_sensor(client);
 354         debug_printk( "/$$$$$$$$$$$$$$$$$$$$$$//n Here I am: %s:%i-------%s()\n", __FILE__, __LINE__,__FUNCTION__);
 355
 356         if (regarray[0].reg == SEQCMD_STREAMCHK) {
 357         streamchk = true;
 358         i = 1;
 359     } else {
 360         streamchk = false;
 361         i = 0;
 362     }
 363
 364         cnt = 0;
 365         while ((regarray[i].reg != SEQCMD_END) && (regarray[i].reg != SEQCMD_INTERPOLATION))
 366         {
 367         if (streamchk) {
 368             if (sensor->info_priv.stream == false) {
 369                 err = -1;
 370                 SENSOR_DG("sensor is stream off, write array terminated!");
 371                 break;
 372             }
 373         }
 374
 375                 if((sensor->info_priv.gReg_mask != 0) /*&& (regarray[i].reg_mask != 0)*/)
 376                         regarray[i].reg_mask = sensor->info_priv.gReg_mask;
 377                 if((sensor->info_priv.gVal_mask != 0) /* && (regarray[i].val_mask != 0)*/)
 378                         regarray[i].val_mask = sensor->info_priv.gVal_mask;
 379                 err = generic_sensor_write(client, &(regarray[i]));
 380                 if (err < 0)
 381                 {
 382                         if (cnt-- > 0) {
 383                                 SENSOR_TR("write failed current reg:0x%x, Write array again !",regarray[i].reg);
 384                                 i = 0;
 385                                 continue;
 386                         } else {
 387                                 SENSOR_TR("write array failed!");
 388                                 err = -EPERM;
 389                                 goto sensor_write_array_end;
 390                         }
 391                 } else {
 392                 #if CONFIG_SENSOR_I2C_RDWRCHK
 393                         check_reg.reg_mask = regarray[i].reg_mask;
 394                         check_reg.val_mask = regarray[i].val_mask;
 395                         check_reg.reg = regarray[i].reg;
 396                         check_reg.val =0;
 397                         generic_sensor_read(client, &check_reg);
 398                         if (check_reg.val!= regarray[i].val)
 399                                 SENSOR_TR("Reg:0x%x write(0x%x, 0x%x) fail", regarray[i].reg, regarray[i].val, check_reg.val );
 400                 #endif
 401                 }
 402
 403                 i++;
 404         }
 405
 406
 407 sensor_write_array_end:
 408         return err;
 409 }
 410 #if CONFIG_SENSOR_I2C_RDWRCHK
 411 int generic_sensor_readchk_array(struct i2c_client *client, struct rk_sensor_reg  *regarray)
 412 {
 413         int cnt;
 414         int i = 0;
 415         struct rk_sensor_reg check_reg;
 416     struct generic_sensor *sensor = to_generic_sensor(client);
 417
 418         cnt = 0;
 419         while (regarray[i].reg != SEQCMD_END)
 420         {
 421                 check_reg.reg_mask = regarray[i].reg_mask;
 422                 check_reg.val_mask = regarray[i].val_mask;
 423                 check_reg.reg = regarray[i].reg;
 424                 check_reg.val =0;
 425                 generic_sensor_read(client, &check_reg);
 426                 if (check_reg.val!= regarray[i].val)
 427                         SENSOR_TR("Reg:0x%x write(0x%x, 0x%x) fail", regarray[i].reg, regarray[i].val, check_reg.val );
 428
 429                 i++;
 430         }
 431         return 0;
 432 }
 433 #endif
 434
 435 int generic_sensor_get_max_min_res(struct rk_sensor_sequence* res_array,int num,struct rk_sensor_seq_info * max_real_res,
 436                                                                                 struct rk_sensor_seq_info * max_res,struct rk_sensor_seq_info *min_res){
 437         int array_index = 0,err = 0;
 438
 439         max_real_res->w = max_res->w = 0;
 440         max_real_res->h = max_res->h =0;
 441         min_res->w =  min_res->h = 10000;
 442         if(!res_array || num <=0){
 443                 printk("resolution array not valid");
 444                 err = -1;
 445                 goto get_end;
 446     }
 447
 448         //serch min_res
 449         while(array_index <num) {
 450                 if(res_array->data && res_array->data[0].reg != SEQCMD_END){
 451                         if(res_array->gSeq_info.w < min_res->w ||res_array->gSeq_info.h < min_res->h){
 452                                         memcpy(min_res,&(res_array->gSeq_info),sizeof(struct rk_sensor_seq_info));
 453                         }
 454                         if((res_array->gSeq_info.w > max_real_res->w ||res_array->gSeq_info.h > max_real_res->h)
 455                                 && (res_array->data[0].reg != SEQCMD_INTERPOLATION)){
 456                                         memcpy(max_real_res,&(res_array->gSeq_info),sizeof(struct rk_sensor_seq_info));
 457                         }
 458                         if((res_array->gSeq_info.w > max_res->w ||res_array->gSeq_info.h > max_res->h)
 459                                 && (res_array->data[0].reg == SEQCMD_INTERPOLATION)){
 460                                         memcpy(max_res,&(res_array->gSeq_info),sizeof(struct rk_sensor_seq_info));
 461                         }
 462                 }
 463
 464                 array_index++;
 465                 res_array++;
 466
 467         }
 468         if((max_res->w < max_real_res->w) || (max_res->h < max_real_res->h)){
 469                 max_res->w = max_real_res->w;
 470                 max_res->h = max_real_res->h;
 471         }
 472         printk("min_w = %d,min_h = %d ,max_real_w = %d,max_real_h = %d,max_w = %d,max_h =%d\n",
 473                                 min_res->w,min_res->h,max_real_res->w,max_real_res->h,max_res->w,max_res->h);
 474         err = 0;
 475 get_end:
 476         return err;
 477 }
 478
 479
 480 // return value: -1 means erro; others means res_array array index
 481 //se_w & set_h have been set to between MAX and MIN
 482 static int sensor_try_fmt(struct i2c_client *client,unsigned int *set_w,unsigned int *set_h){
 483         int array_index = 0;
 484         struct generic_sensor *sensor = to_generic_sensor(client);
 485         struct rk_sensor_sequence* res_array = sensor->info_priv.sensor_series;
 486         int num = sensor->info_priv.num_series;
 487         int tmp_w = 10000,tmp_h = 10000,tmp_index = -1;
 488         int resolution_diff_min=10000*10000,resolution_diff;
 489
 490         while(array_index < num) {
 491         if ((res_array->data) && (res_array->data[0].reg != SEQCMD_END)) {
 492
 493             if(res_array->property == SEQUENCE_INIT) {
 494                                 tmp_index = array_index;
 495                                 array_index++;
 496                                 res_array++;
 497                                 continue;
 498             }
 499
 500             resolution_diff = abs(res_array->gSeq_info.w*res_array->gSeq_info.h - (*set_w)*(*set_h));
 501             if (resolution_diff<resolution_diff_min) {
 502                 tmp_w = res_array->gSeq_info.w;
 503                                 tmp_h = res_array->gSeq_info.h;
 504                                 tmp_index = array_index;
 505
 506                 resolution_diff_min = resolution_diff;
 507             }
 508
 509                 }
 510         array_index++;
 511             res_array++;
 512         }
 513         *set_w = tmp_w;
 514         *set_h =  tmp_h;
 515         //only has the init array
 516         if((tmp_w == 10000) && (tmp_index != -1)){
 517                 SENSOR_TR("have not other series meet the requirement except init_serie,array_index = %d",tmp_index);
 518                 *set_w = sensor->info_priv.sensor_series[tmp_index].gSeq_info.w;
 519                 *set_h = sensor->info_priv.sensor_series[tmp_index].gSeq_info.h;
 520                 goto try_end;
 521         }
 522         if((*set_w > sensor->info_priv.max_real_res.w) || (*set_h > sensor->info_priv.max_real_res.h)){
 523                 SENSOR_DG("it is a interpolation resolution!(%dx%d:%dx%d)",sensor->info_priv.max_real_res.w
 524                                         ,sensor->info_priv.max_real_res.h,*set_w,*set_h);
 525                 *set_w = sensor->info_priv.max_real_res.w;
 526                 *set_h = sensor->info_priv.max_real_res.h;
 527                 //find the max_real_res index
 528                 res_array = sensor->info_priv.sensor_series;
 529                 array_index = 0;
 530                 tmp_index = -1;
 531                 while(array_index < num){
 532                         if((res_array->data) && (res_array->data[0].reg != SEQCMD_END) && (*set_w  ==res_array->gSeq_info.w) && (*set_h ==res_array->gSeq_info.h)){
 533                                 if((res_array->property != SEQUENCE_INIT)){
 534                                         tmp_index = array_index;
 535                                         break;
 536                                 }else{
 537                                         tmp_index = array_index;
 538                                 }
 539                         }
 540                         array_index++;
 541                         res_array++ ;
 542                 }
 543
 544         }
 545 try_end:
 546         return tmp_index;
 547 }
 548 int generic_sensor_try_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
 549 {
 550     struct i2c_client *client = v4l2_get_subdevdata(sd);
 551     struct generic_sensor *sensor = to_generic_sensor(client);
 552     const struct rk_sensor_datafmt *fmt;
 553     int ret = 0;
 554     unsigned int set_w,set_h,ori_w,ori_h;
 555
 556     ori_w = mf->width;
 557     ori_h = mf->height;
 558
 559     fmt = generic_sensor_find_datafmt(mf->code, sensor->info_priv.datafmt,
 560                                                 sensor->info_priv.num_datafmt);
 561     if (fmt == NULL) {
 562         fmt = &(sensor->info_priv.curfmt);
 563         mf->code = fmt->code;
 564     }
 565     /* ddl@rock-chips.com : It is query max resolution only. */
 566     if (mf->reserved[6] == 0xfefe5a5a) {
 567         mf->height = sensor->info_priv.max_res.h ;
 568         mf->width = sensor->info_priv.max_res.w;
 569         ret = 0;
 570         SENSOR_DG("Query resolution: %dx%d",mf->width, mf->height);
 571         goto generic_sensor_try_fmt_end;
 572     }
 573     //use this to filter unsupported resolutions
 574     if (sensor->sensor_cb.sensor_try_fmt_cb_th){
 575             ret = sensor->sensor_cb.sensor_try_fmt_cb_th(client, mf);
 576             if(ret < 0)
 577                     goto generic_sensor_try_fmt_end;
 578         }
 579     if (mf->height > sensor->info_priv.max_res.h)
 580         mf->height = sensor->info_priv.max_res.h;
 581     else if (mf->height < sensor->info_priv.min_res.h)
 582         mf->height = sensor->info_priv.min_res.h;
 583
 584     if (mf->width > sensor->info_priv.max_res.w)
 585         mf->width = sensor->info_priv.max_res.w;
 586     else if (mf->width < sensor->info_priv.min_res.w)
 587         mf->width = sensor->info_priv.min_res.w;
 588     set_w = mf->width;
 589     set_h = mf->height;
 590     ret = sensor_try_fmt(client,&set_w,&set_h);
 591     mf->width = set_w;
 592     mf->height = set_h;
 593     mf->colorspace = fmt->colorspace;
 594     SENSOR_DG("%dx%d is the closest for %dx%d",ori_w,ori_h,set_w,set_h);
 595 generic_sensor_try_fmt_end:
 596     return ret;
 597 }
 598
 599 int generic_sensor_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *cc)
 600 {
 601     struct i2c_client *client = v4l2_get_subdevdata(sd);
 602     struct generic_sensor *sensor = to_generic_sensor(client);
 603     int ret=0;
 604
 605     cc->bounds.left = 0;
 606     cc->bounds.top = 0;
 607     cc->bounds.width = sensor->info_priv.max_res.w;
 608     cc->bounds.height = sensor->info_priv.max_res.h;
 609
 610     cc->pixelaspect.denominator = sensor->info_priv.max_res.w;
 611     cc->pixelaspect.numerator = sensor->info_priv.max_res.h;
 612
 613     return ret;
 614 }
 615
 616 int generic_sensor_enum_frameintervals(struct v4l2_subdev *sd, struct v4l2_frmivalenum *fival){
 617         int err = 0,index_tmp;
 618     unsigned int set_w,set_h;
 619         struct i2c_client *client = v4l2_get_subdevdata(sd);
 620         struct generic_sensor *sensor = to_generic_sensor(client);
 621
 622         if (fival->height > sensor->info_priv.max_res.h|| fival->width > sensor->info_priv.max_res.w){
 623         SENSOR_TR("this resolution(%dx%d) isn't support!",fival->width,fival->height);
 624         err = -1;
 625         goto enum_frameintervals_end;
 626         }
 627         set_w = fival->width;
 628     set_h = fival->height;
 629     index_tmp = sensor_try_fmt(client,&set_w,&set_h);
 630     fival->discrete.denominator = sensor->info_priv.sensor_series[index_tmp].gSeq_info.fps;
 631     fival->discrete.numerator = 1000;
 632     fival->reserved[1] = (set_w<<16)|set_h;
 633     fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 634
 635     SENSOR_DG("%dx%d(real:%dx%d) framerate: %d",fival->width,fival->height,set_w,set_h,fival->discrete.denominator);
 636 enum_frameintervals_end:
 637         return err;
 638 }
 639
 640 static enum hrtimer_restart generic_flash_off_func(struct hrtimer *timer){
 641         struct rk_flash_timer *fps_timer = container_of(timer, struct rk_flash_timer, timer);
 642
 643     generic_sensor_ioctrl(fps_timer->icd,Sensor_Flash,0);
 644         return 0;
 645 }
 646 /* Find a data format by a pixel code in an array */
 647 static const struct rk_sensor_datafmt *generic_sensor_find_datafmt(
 648         enum v4l2_mbus_pixelcode code, const struct rk_sensor_datafmt *fmt,
 649         int n)
 650 {
 651         int i;
 652         for (i = 0; i < n; i++)
 653                 if (fmt[i].code == code)
 654                         return fmt + i;
 655
 656         return NULL;
 657 }
 658 int generic_sensor_softreset(struct i2c_client *client, struct rk_sensor_reg *series) {
 659     int ret = 0;
 660     struct generic_sensor *sensor = to_generic_sensor(client);
 661
 662         debug_printk( "/$$$$$$$$$$$$$$$$$$$$$$//n Here I am: %s:%i-------%s()\n", __FILE__, __LINE__,__FUNCTION__);
 663         debug_printk( "/___________/client->addr = %x \n",client->addr);
 664
 665     if (sensor->sensor_cb.sensor_softreset_cb)
 666          sensor->sensor_cb.sensor_softreset_cb(client,series);
 667         debug_printk( "/~~~~~~~~~~~~/ %s:%i-------%s()client = %p\n", __FILE__, __LINE__,__FUNCTION__,client);
 668         /* soft reset */
 669     ret = generic_sensor_write_array(client,series);
 670         if (ret != 0) {
 671                 SENSOR_TR("soft reset failed\n");
 672                 ret = -ENODEV;
 673         }
 674     msleep(1);
 675         return ret;
 676
 677 }
 678 int generic_sensor_check_id(struct i2c_client *client, struct rk_sensor_reg *series)
 679 {
 680         int ret,pid = 0,i;
 681         struct generic_sensor *sensor = to_generic_sensor(client);
 682         debug_printk( "/$$$$$$$$$$$$$$$$$$$$$$//n Here I am: %s:%i-------%s()\n", __FILE__, __LINE__,__FUNCTION__);
 683
 684     if (sensor->sensor_cb.sensor_check_id_cb)
 685           pid = sensor->sensor_cb.sensor_check_id_cb(client,series);
 686
 687     /* check if it is an sensor sensor */
 688     while (series->reg != SEQCMD_END) {
 689
 690         pid <<= 8;
 691
 692         if (sensor->info_priv.gReg_mask != 0x00)
 693             series->reg_mask = sensor->info_priv.gReg_mask;
 694         if (sensor->info_priv.gVal_mask != 0x00)
 695             series->val_mask = sensor->info_priv.gVal_mask;
 696
 697         ret = generic_sensor_read(client, series);
 698         if (ret != 0) {
 699                 SENSOR_TR("read chip id failed");
 700                 ret = -ENODEV;
 701                 goto check_end;
 702         }
 703
 704         pid |= series->val;
 705         series++;
 706     }
 707
 708         SENSOR_DG("pid = 0x%x", pid);
 709
 710     for (i=0; i<sensor->info_priv.chip_id_num; i++) {
 711         if (pid == sensor->info_priv.chip_id[i]) {
 712             sensor->model = sensor->info_priv.chip_ident;
 713             break;
 714         }
 715     }
 716
 717         if (sensor->model != sensor->info_priv.chip_ident) {
 718                 SENSOR_TR("error: mismatched   pid = 0x%x\n", pid);
 719                 ret = -ENODEV;
 720                 goto check_end;
 721         } else {
 722         ret = 0;
 723         }
 724
 725 check_end:
 726         return ret;
 727 }
 728
 729 int generic_sensor_ioctrl(struct soc_camera_device *icd,enum rk29sensor_power_cmd cmd, int on)
 730 {
 731         //struct soc_camera_link *icl = to_soc_camera_link(icd);
 732         struct soc_camera_desc *desc = to_soc_camera_desc(icd);
 733     struct rk29camera_platform_data *pdata = desc->subdev_desc.drv_priv;//yzm
 734     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
 735     struct generic_sensor *sensor = to_generic_sensor(client);
 736         int ret = 0;
 737
 738         SENSOR_DG("%s cmd(%d) on(%d)\n",__FUNCTION__,cmd,on);
 739         switch (cmd)
 740         {
 741         case Sensor_Power:
 742         {
 743                         //if (icl->power) {
 744                         if(desc->subdev_desc.power) {//yzm
 745                                 ret = desc->subdev_desc.power(icd->pdev, on);//yzm
 746                         } else {
 747                             SENSOR_TR("haven't power callback");
 748                 ret = -EINVAL;
 749                         }
 750                         break;
 751                 }
 752                 case Sensor_PowerDown:
 753                 {
 754                         //if (icl->powerdown) {
 755                         if(desc->subdev_desc.powerdown) {//yzm
 756                                 ret = desc->subdev_desc.powerdown(icd->pdev, on);//yzm
 757                         } else {
 758                             SENSOR_TR("haven't power down callback");
 759                 ret = -EINVAL;
 760                         }
 761                         break;
 762                 }
 763                 case Sensor_Flash:
 764                 {
 765                         if (pdata && pdata->sensor_ioctrl) {
 766                                 pdata->sensor_ioctrl(icd->pdev,Cam_Flash, on);
 767                                 if(on==Flash_On){
 768                     mdelay(5);
 769                                         //flash off after 2 secs
 770                                         hrtimer_cancel(&(sensor->flash_off_timer.timer));
 771                                         hrtimer_start(&(sensor->flash_off_timer.timer),ktime_set(0, 2000*1000*1000),HRTIMER_MODE_REL);
 772                                 }
 773                         }
 774                         break;
 775                 }
 776                 default:
 777                 {
 778                         SENSOR_TR("%s cmd(%d) is unknown!",__FUNCTION__,cmd);
 779                         break;
 780                 }
 781         }
 782
 783         return ret;
 784 }
 785 static int sensor_v4l2ctrl_mirror_cb(struct soc_camera_device *icd, struct sensor_v4l2ctrl_info_s *ctrl_info,
 786                                                                                                          struct v4l2_ext_control *ext_ctrl){
 787         return 0;
 788 }
 789 static int sensor_v4l2ctrl_flip_cb(struct soc_camera_device *icd, struct sensor_v4l2ctrl_info_s *ctrl_info,
 790                                                                                                          struct v4l2_ext_control *ext_ctrl){
 791         return 0;
 792 }
 793 int generic_sensor_init(struct v4l2_subdev *sd, u32 val)
 794 {
 795         int ret = 0;
 796         struct i2c_client *client = v4l2_get_subdevdata(sd);
 797         struct soc_camera_subdev_desc *ssdd = client->dev.platform_data;
 798         struct soc_camera_device *icd = ssdd->socdev;
 799         struct generic_sensor *sensor = to_generic_sensor(client);
 800         int array_index = 0;
 801         int num = sensor->info_priv.num_series;
 802     //struct soc_camera_link *icl = to_soc_camera_link(icd);
 803         struct soc_camera_desc *desc = to_soc_camera_desc(icd);//yzm
 804     struct rk29camera_platform_data *pdata = desc->subdev_desc.drv_priv;//yzm
 805     struct rkcamera_platform_data *sensor_device=NULL,*new_camera;
 806
 807     new_camera = pdata->register_dev_new;
 808     //while (strstr(new_camera->dev_name,"end")==NULL) {
 809     while(new_camera != NULL){
 810         if (strcmp(dev_name(icd->pdev), new_camera->dev_name) == 0) {
 811             sensor_device = new_camera;
 812             break;
 813         }
 814         //new_camera++;
 815         new_camera = new_camera->next_camera;
 816     }
 817
 818     /* ddl@rock-chips.com : i2c speed is config in new_camera_device_ex macro */
 819     if (sensor_device) {
 820         sensor->info_priv.gI2c_speed = sensor_device->i2c_rate;
 821         sensor->info_priv.mirror = sensor_device->mirror;
 822     }
 823
 824     if (((sensor_device!=NULL) && Sensor_HasBeen_PwrOff(sensor_device->pwdn_info))
 825         || (sensor_device == NULL)) {
 826
 827         //softreset callback
 828
 829                 debug_printk( "/~~~~~~~~~~~~/ %s:%i-------%s()client->adapter = %p\n", __FILE__, __LINE__,__FUNCTION__,client->adapter);
 830         ret =  generic_sensor_softreset(client,sensor->info_priv.sensor_SfRstSeqe);
 831         if(ret != 0){
 832                 SENSOR_TR("soft reset failed!");
 833                 goto sensor_INIT_ERR;
 834         }
 835
 836         while(array_index < num){
 837                 if(sensor->info_priv.sensor_series[array_index].property == SEQUENCE_INIT)
 838                         break;
 839                 array_index++;
 840         }
 841         if(generic_sensor_write_array(client, sensor->info_priv.sensor_series[array_index].data)!=0){
 842                 SENSOR_TR("write init array failed!");
 843                 ret = -1;
 844                 goto sensor_INIT_ERR;
 845         }
 846         if (sensor_device!=NULL) {
 847             sensor_device->pwdn_info &= 0xfe;
 848             if (sensor->sensor_cb.sensor_mirror_cb)
 849                 sensor->sensor_cb.sensor_mirror_cb(client, sensor->info_priv.mirror&0x01);
 850             if (sensor->sensor_cb.sensor_flip_cb)
 851                 sensor->sensor_cb.sensor_flip_cb(client, sensor->info_priv.mirror&0x02);
 852         }
 853         sensor->info_priv.winseqe_cur_addr = sensor->info_priv.sensor_series + array_index;
 854
 855         //set focus status ,init focus
 856         sensor->sensor_focus.focus_state = FocusState_Inval;
 857         sensor->sensor_focus.focus_mode = WqCmd_af_invalid;
 858         sensor->sensor_focus.focus_delay = WqCmd_af_invalid;
 859
 860     }
 861
 862     if (sensor->sensor_cb.sensor_activate_cb)
 863         sensor->sensor_cb.sensor_activate_cb(client);
 864
 865
 866     if (sensor->flash_off_timer.timer.function==NULL)
 867         sensor->flash_off_timer.timer.function = generic_flash_off_func;
 868
 869         sensor->info_priv.funmodule_state |= SENSOR_INIT_IS_OK;
 870
 871         return 0;
 872 sensor_INIT_ERR:
 873         sensor->info_priv.funmodule_state &= ~SENSOR_INIT_IS_OK;
 874         if(sensor->sensor_cb.sensor_deactivate_cb)
 875                 sensor->sensor_cb.sensor_deactivate_cb(client);
 876         return ret;
 877 }
 878  int generic_sensor_set_bus_param(struct soc_camera_device *icd,
 879                                                                 unsigned long flags)
 880 {
 881
 882         return 0;
 883 }
 884
 885 unsigned long generic_sensor_query_bus_param(struct soc_camera_device *icd)
 886 {
 887         //struct soc_camera_link *icl = to_soc_camera_link(icd);
 888         struct soc_camera_desc *desc = to_soc_camera_desc(icd);//yzm
 889         struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
 890         struct generic_sensor *sensor = to_generic_sensor(client);
 891         /**************yzm************/
 892         struct v4l2_mbus_config cfg;
 893         debug_printk( "/$$$$$$$$$$$$$$$$$$$$$$//n Here I am: %s:%i-------%s()\n", __FILE__, __LINE__,__FUNCTION__);
 894         cfg.flags = sensor->info_priv.bus_parameter;
 895         return soc_camera_apply_board_flags(&(desc->subdev_desc), &cfg);//yzm
 896         /**************yzm*************/
 897 }
 898
 899 int generic_sensor_g_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
 900 {
 901         struct i2c_client *client = v4l2_get_subdevdata(sd);
 902         struct soc_camera_subdev_desc *ssdd = client->dev.platform_data;
 903         struct soc_camera_device *icd = ssdd->socdev;
 904         struct generic_sensor *sensor = to_generic_sensor(client);
 905
 906         mf->width       = icd->user_width;
 907         mf->height      = icd->user_height;
 908         mf->code        = sensor->info_priv.curfmt.code;
 909         mf->colorspace  = sensor->info_priv.curfmt.colorspace;
 910         mf->field       = V4L2_FIELD_NONE;
 911
 912         return 0;
 913 }
 914
 915 int generic_sensor_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
 916 {
 917     struct i2c_client *client = v4l2_get_subdevdata(sd);
 918         struct soc_camera_subdev_desc *ssdd = client->dev.platform_data;
 919         struct soc_camera_device *icd = ssdd->socdev;
 920     const struct rk_sensor_datafmt *fmt=NULL;
 921     struct generic_sensor *sensor = to_generic_sensor(client);
 922     struct rk_sensor_sequence *winseqe_set_addr=NULL;
 923     struct sensor_v4l2ctrl_info_s *v4l2ctrl_info=NULL;
 924     bool is_capture=(mf->reserved[0]==0xfefe5a5a)?true:false;    /* ddl@rock-chips.com : v0.1.5 */
 925     int ret=0;
 926
 927     fmt =generic_sensor_find_datafmt(mf->code, sensor->info_priv.datafmt,
 928                                         sensor->info_priv.num_datafmt);
 929     if (!fmt) {
 930         ret = -EINVAL;
 931         goto sensor_s_fmt_end;
 932     }
 933
 934     // get the proper series and write the array
 935     ret =generic_sensor_try_fmt(sd, mf);
 936     winseqe_set_addr = sensor->info_priv.sensor_series+ret;
 937
 938     ret = 0;
 939     if (sensor->sensor_cb.sensor_s_fmt_cb_th)
 940         ret |= sensor->sensor_cb.sensor_s_fmt_cb_th(client, mf, is_capture);
 941
 942     v4l2ctrl_info = sensor_find_ctrl(sensor->ctrls,V4L2_CID_FLASH); /* ddl@rock-chips.com: v0.1.3 */
 943     if (v4l2ctrl_info!=NULL) {
 944         if (is_capture) {
 945             if ((v4l2ctrl_info->cur_value == 2) || (v4l2ctrl_info->cur_value == 1)) {
 946                 generic_sensor_ioctrl(icd, Sensor_Flash, 1);
 947             }
 948         } else {
 949             generic_sensor_ioctrl(icd, Sensor_Flash, 0);
 950         }
 951     }
 952
 953     if(sensor->info_priv.winseqe_cur_addr->data != winseqe_set_addr->data){
 954         ret |= generic_sensor_write_array(client, winseqe_set_addr->data);
 955         if (ret != 0) {
 956             SENSOR_TR("set format capability failed");
 957             goto sensor_s_fmt_end;
 958         }
 959
 960         if (sensor->sensor_cb.sensor_s_fmt_cb_bh)
 961             ret |= sensor->sensor_cb.sensor_s_fmt_cb_bh(client, mf, is_capture);
 962         sensor->info_priv.winseqe_cur_addr  = winseqe_set_addr;
 963         SENSOR_DG("Sensor output is changed to %dx%d",winseqe_set_addr->gSeq_info.w,winseqe_set_addr->gSeq_info.h);
 964     } else {
 965         SENSOR_DG("Sensor output is still %dx%d",winseqe_set_addr->gSeq_info.w,winseqe_set_addr->gSeq_info.h);
 966     }
 967
 968 //video or capture special process
 969 sensor_s_fmt_end:
 970     Sensor_CropSet(mf,sensor->crop_percent);    /* ddl@rock-chips.com: v0.1.b */
 971         return ret;
 972 }
 973  int generic_sensor_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *id)
 974 {
 975         struct i2c_client *client = v4l2_get_subdevdata(sd);
 976         struct generic_sensor *sensor = to_generic_sensor(client);
 977
 978         if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
 979                 return -EINVAL;
 980
 981         if (id->match.addr != client->addr)
 982                 return -ENODEV;
 983
 984         id->ident = sensor->info_priv.chip_ident;               /* ddl@rock-chips.com :  Return OV2655  identifier */
 985         id->revision = 0;
 986
 987         return 0;
 988 }
 989
 990 int generic_sensor_g_control(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
 991 {
 992     struct i2c_client *client = v4l2_get_subdevdata(sd);
 993     struct generic_sensor *sensor = to_generic_sensor(client);
 994     struct sensor_v4l2ctrl_info_s *ctrl_info;
 995     struct v4l2_ext_control ext_ctrl;
 996         struct soc_camera_subdev_desc *ssdd = client->dev.platform_data;
 997         struct soc_camera_device *icd = ssdd->socdev;
 998     int ret = 0;
 999
1000     ctrl_info = sensor_find_ctrl(sensor->ctrls,ctrl->id);
1001     if (!ctrl_info) {
1002         SENSOR_TR("v4l2_control id(0x%x) is invalidate",ctrl->id);
1003         ret = -EINVAL;
1004     } else {
1005         ext_ctrl.value = ctrl->value;
1006         ext_ctrl.id = ctrl->id;
1007
1008         ctrl->value = ctrl_info->cur_value;
1009
1010         if (ctrl_info->cb) {
1011             ret = (ctrl_info->cb)(icd,ctrl_info, &ext_ctrl,false);
1012             if(ret == 0)
1013                 ctrl->value = ctrl_info->cur_value;
1014         } else {
1015             SENSOR_TR("v4l2_control id(0x%x) callback isn't exist",ctrl->id);
1016             ret = -EINVAL;
1017         }
1018     }
1019
1020     return ret;
1021 }
1022
1023 int generic_sensor_s_control(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
1024 {
1025     struct i2c_client *client = v4l2_get_subdevdata(sd);
1026     struct generic_sensor *sensor = to_generic_sensor(client);
1027     struct soc_camera_subdev_desc *ssdd = client->dev.platform_data;
1028         struct soc_camera_device *icd = ssdd->socdev;
1029     struct sensor_v4l2ctrl_info_s *ctrl_info;
1030     struct v4l2_ext_control ext_ctrl;
1031     int ret = 0;
1032
1033     ctrl_info = sensor_find_ctrl(sensor->ctrls,ctrl->id);
1034     if (!ctrl_info) {
1035         SENSOR_TR("v4l2_control id(0x%x) is invalidate",ctrl->id);
1036         ret = -EINVAL;
1037     } else {
1038
1039         ext_ctrl.id = ctrl->id;
1040         ext_ctrl.value = ctrl->value;
1041
1042         if (ctrl_info->cb) {
1043             ret = (ctrl_info->cb)(icd,ctrl_info, &ext_ctrl,true);
1044         } else {
1045             SENSOR_TR("v4l2_control id(0x%x) callback isn't exist",ctrl->id);
1046             ret = -EINVAL;
1047         }
1048     }
1049
1050     return ret;
1051 }
1052
1053 int generic_sensor_g_ext_control(struct soc_camera_device *icd , struct v4l2_ext_control *ext_ctrl)
1054 {
1055     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1056     struct generic_sensor *sensor = to_generic_sensor(client);
1057     struct sensor_v4l2ctrl_info_s *ctrl_info;
1058     int ret = 0;
1059
1060     ctrl_info = sensor_find_ctrl(sensor->ctrls,ext_ctrl->id);
1061     if (!ctrl_info) {
1062         SENSOR_TR("v4l2_control id(0x%x) is invalidate",ext_ctrl->id);
1063         ret = -EINVAL;
1064     } else {
1065         ext_ctrl->value = ctrl_info->cur_value;
1066         if (ctrl_info->cb) {
1067             ret = (ctrl_info->cb)(icd,ctrl_info, ext_ctrl,false);
1068         } else {
1069             SENSOR_TR("v4l2_control id(0x%x) callback isn't exist",ext_ctrl->id);
1070             ret = -EINVAL;
1071         }
1072
1073     }
1074
1075     return ret;
1076 }
1077
1078 int generic_sensor_s_ext_control(struct soc_camera_device *icd, struct v4l2_ext_control *ext_ctrl)
1079 {
1080     struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1081     struct generic_sensor *sensor = to_generic_sensor(client);
1082     struct sensor_v4l2ctrl_info_s *ctrl_info;
1083     int ret = 0;
1084
1085     ctrl_info = sensor_find_ctrl(sensor->ctrls,ext_ctrl->id);
1086     if (!ctrl_info) {
1087         SENSOR_TR("v4l2_ext_control id(0x%x) is invalidate",ext_ctrl->id);
1088         ret = -EINVAL;
1089     } else {
1090         if (ctrl_info->cb) {
1091             ret = (ctrl_info->cb)(icd,ctrl_info, ext_ctrl,true);
1092         } else {
1093             SENSOR_TR("v4l2_ext_control id(0x%x) callback isn't exist",ext_ctrl->id);
1094             ret = -EINVAL;
1095         }
1096     }
1097     return 0;
1098 }
1099   int generic_sensor_g_ext_controls(struct v4l2_subdev *sd, struct v4l2_ext_controls *ext_ctrl)
1100  {
1101          struct i2c_client *client = v4l2_get_subdevdata(sd);
1102          struct soc_camera_subdev_desc *ssdd = client->dev.platform_data;
1103         struct soc_camera_device *icd = ssdd->socdev;
1104      //struct generic_sensor *sensor = to_generic_sensor(client);
1105          int i, error_cnt=0, error_idx=-1;
1106
1107          for (i=0; i<ext_ctrl->count; i++) {
1108                  if (generic_sensor_g_ext_control(icd, &ext_ctrl->controls[i]) != 0) {
1109                          error_cnt++;
1110                          error_idx = i;
1111                  }
1112          }
1113
1114          if (error_cnt > 1)
1115                  error_idx = ext_ctrl->count;
1116
1117          if (error_idx != -1) {
1118                  ext_ctrl->error_idx = error_idx;
1119                  return -EINVAL;
1120          } else {
1121                  return 0;
1122          }
1123  }
1124 int generic_sensor_s_ext_controls(struct v4l2_subdev *sd, struct v4l2_ext_controls *ext_ctrl)
1125 {
1126     struct i2c_client *client = v4l2_get_subdevdata(sd);
1127     struct soc_camera_subdev_desc *ssdd = client->dev.platform_data;
1128         struct soc_camera_device *icd = ssdd->socdev;
1129     //struct generic_sensor*sensor = to_generic_sensor(client);
1130     int i, error_cnt=0, error_idx=-1;
1131
1132
1133     for (i=0; i<ext_ctrl->count; i++) {
1134
1135         if (generic_sensor_s_ext_control(icd, &ext_ctrl->controls[i]) != 0) {
1136             error_cnt++;
1137             error_idx = i;
1138         }
1139     }
1140
1141     if (error_cnt > 1)
1142         error_idx = ext_ctrl->count;
1143
1144     if (error_idx != -1) {
1145         ext_ctrl->error_idx = error_idx;
1146         return -EINVAL;
1147     } else {
1148         return 0;
1149     }
1150 }
1151
1152
1153 long generic_sensor_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
1154 {
1155     struct i2c_client *client = v4l2_get_subdevdata(sd);
1156     struct soc_camera_subdev_desc *ssdd = client->dev.platform_data;
1157         struct soc_camera_device *icd = ssdd->socdev;
1158     struct generic_sensor*sensor = to_generic_sensor(client);
1159     int ret = 0;
1160     int i;
1161     bool flash_attach=false;
1162     struct rkcamera_platform_data *new_camera;
1163         debug_printk( "/$$$$$$$$$$$$$$$$$$$$$$//n Here I am: %s:%i-------%s()\n", __FILE__, __LINE__,__FUNCTION__);
1164
1165     SENSOR_DG("%s cmd: 0x%x ",__FUNCTION__,cmd);
1166     switch (cmd)
1167     {
1168         case RK29_CAM_SUBDEV_DEACTIVATE:
1169         {
1170             if(sensor->sensor_cb.sensor_deactivate_cb)
1171                 sensor->sensor_cb.sensor_deactivate_cb(client);
1172             break;
1173         }
1174
1175         case RK29_CAM_SUBDEV_IOREQUEST:
1176         {
1177             sensor->sensor_io_request = (struct rk29camera_platform_data*)arg;
1178             if (sensor->sensor_io_request != NULL) {
1179                 sensor->sensor_gpio_res = NULL;
1180
1181             } else {
1182                 SENSOR_TR("RK29_CAM_SUBDEV_IOREQUEST fail");
1183                 ret = -EINVAL;
1184                 goto sensor_ioctl_end;
1185             }
1186             /* ddl@rock-chips.com : if gpio_flash havn't been set in board-xxx.c, sensor driver must notify is not support flash control
1187                for this project */
1188             if (sensor->sensor_gpio_res) {
1189                 if (sensor->sensor_gpio_res->gpio_flash == INVALID_GPIO) {
1190                     flash_attach = false;
1191                 } else {
1192                     flash_attach = true;
1193                 }
1194             }
1195
1196             new_camera = sensor->sensor_io_request->register_dev_new;
1197             //while (strstr(new_camera->dev_name,"end")==NULL) {
1198             while(new_camera != NULL){
1199                 if (strcmp(dev_name(icd->pdev), new_camera->dev_name) == 0) {
1200                     if (new_camera->flash){
1201                         flash_attach = true;
1202                     } else {
1203                         flash_attach = false;
1204                     }
1205                     break;
1206                 }
1207                 //new_camera++;
1208                 new_camera = new_camera->next_camera;
1209             }
1210
1211             if (flash_attach==false) {
1212                 for (i = 0; i < icd->ops->num_controls; i++) {
1213                     if (V4L2_CID_FLASH == icd->ops->controls[i].id) {
1214                         sensor->sensor_controls[i].id |= 0x80000000;
1215                     }
1216                 }
1217             } else {
1218                 for (i = 0; i < icd->ops->num_controls; i++) {
1219                     if(V4L2_CID_FLASH == (icd->ops->controls[i].id&0x7fffffff)){
1220                         sensor->sensor_controls[i].id &= 0x7fffffff;
1221                     }
1222                 }
1223             }
1224             break;
1225         }
1226         default:
1227         {
1228             SENSOR_TR("%s cmd(0x%x) is unknown !\n",__FUNCTION__,cmd);
1229             break;
1230         }
1231     }
1232 sensor_ioctl_end:
1233     return ret;
1234 }
1235
1236 int generic_sensor_s_power(struct v4l2_subdev *sd, int on)
1237 {
1238
1239         struct i2c_client *client = v4l2_get_subdevdata(sd);
1240     struct soc_camera_subdev_desc *ssdd = client->dev.platform_data;
1241         struct soc_camera_device *icd = ssdd->socdev;
1242     struct generic_sensor*sensor = to_generic_sensor(client);
1243     int ret = 0;
1244         debug_printk( "/$$$$$$$$$$$$$$$$$$$$$$//n Here I am: %s:%i-------%s()\n", __FILE__, __LINE__,__FUNCTION__);
1245         if(on)
1246                 {
1247                         ret = soc_camera_power_on(icd->pdev,ssdd);
1248                         if(ret < 0)
1249                 SENSOR_TR("%s(%d)power_on fail !\n",__FUNCTION__,__LINE__);
1250
1251                 }
1252         else
1253                 {
1254
1255                         v4l2_subdev_call(sd, core, ioctl, RK29_CAM_SUBDEV_DEACTIVATE,NULL);
1256                         soc_camera_power_off(icd->pdev,ssdd);
1257                         if(ret < 0)
1258                 SENSOR_TR("%s(%d)power_off fail !\n",__FUNCTION__,__LINE__);
1259
1260                 }
1261         return ret;
1262 }
1263
1264
1265
1266 int generic_sensor_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
1267          enum v4l2_mbus_pixelcode *code)
1268 {
1269
1270     struct i2c_client *client = v4l2_get_subdevdata(sd);
1271     struct generic_sensor*sensor = to_generic_sensor(client);
1272     if (index >= sensor->info_priv.num_datafmt)
1273         return -EINVAL;
1274
1275     *code = sensor->info_priv.datafmt[index].code;
1276     return 0;
1277 }
1278  static void sensor_af_workqueue(struct work_struct *work)
1279 {
1280         struct rk_sensor_focus_work *sensor_work = container_of(work, struct rk_sensor_focus_work, dwork.work);
1281         struct i2c_client *client = sensor_work->client;
1282         struct generic_sensor*sensor = to_generic_sensor(client);
1283         //struct rk_sensor_focus_cmd_info cmdinfo;
1284         int zone_tm_pos[4];
1285         int ret = 0;
1286
1287         SENSOR_DG("%s Enter, cmd:0x%x",__FUNCTION__,sensor_work->cmd);
1288
1289         switch (sensor_work->cmd)
1290         {
1291                 case WqCmd_af_init:
1292                 {
1293             if (sensor->sensor_focus.focus_state == FocusState_Inval) {
1294                         if(sensor->sensor_focus.focus_cb.sensor_focus_init_cb !=NULL) {
1295                                 ret = (sensor->sensor_focus.focus_cb.sensor_focus_init_cb)(client);
1296                         }
1297                         if (ret < 0) {
1298                                 SENSOR_TR("WqCmd_af_init is failed in sensor_af_workqueue!");
1299                         } else {
1300                                 if(sensor->sensor_focus.focus_delay != WqCmd_af_invalid) {
1301                                         generic_sensor_af_workqueue_set(client->dev.platform_data,sensor->sensor_focus.focus_delay,0,false);
1302                                         sensor->sensor_focus.focus_delay = WqCmd_af_invalid;
1303                                 }
1304                     sensor->sensor_focus.focus_state = FocusState_Inited;
1305                                 sensor_work->result = WqRet_success;
1306                         }
1307             } else {
1308                 sensor_work->result = WqRet_success;
1309                 SENSOR_DG("sensor af have been inited, WqCmd_af_init is ignore!");
1310             }
1311                         break;
1312                 }
1313                 case WqCmd_af_single:
1314                 {
1315                         if(sensor->sensor_focus.focus_cb.sensor_af_single_cb!=NULL){
1316                                 ret = (sensor->sensor_focus.focus_cb.sensor_af_single_cb)(client);
1317                         }
1318                         if (ret < 0) {
1319                                 SENSOR_TR("%s Sensor_af_single is failed in sensor_af_workqueue!",SENSOR_NAME_STRING());
1320                                 sensor_work->result = WqRet_fail;
1321                         } else {
1322                                 sensor_work->result = WqRet_success;
1323                         }
1324                         break;
1325                 }
1326                 case WqCmd_af_near_pos:
1327                 {
1328                         if(sensor->sensor_focus.focus_cb.sensor_af_near_cb!=NULL){
1329                                 ret = (sensor->sensor_focus.focus_cb.sensor_af_near_cb)(client);
1330                         }
1331                         if (ret < 0)
1332                            sensor_work->result = WqRet_fail;
1333                         else{
1334                            sensor_work->result = WqRet_success;
1335                                 }
1336                         break;
1337                 }
1338                 case WqCmd_af_far_pos:
1339                 {
1340                         if(sensor->sensor_focus.focus_cb.sensor_af_far_cb!=NULL){
1341                                 ret = (sensor->sensor_focus.focus_cb.sensor_af_far_cb)(client);
1342                         }
1343                         if (ret < 0)
1344                            sensor_work->result = WqRet_fail;
1345                         else
1346                            sensor_work->result = WqRet_success;
1347                         break;
1348                 }
1349                 case WqCmd_af_special_pos:
1350                 {
1351                         if(sensor->sensor_focus.focus_cb.sensor_af_specialpos_cb!=NULL){
1352                                 ret = (sensor->sensor_focus.focus_cb.sensor_af_specialpos_cb)(client,sensor_work->var);
1353                         }
1354                         if (ret < 0)
1355                            sensor_work->result = WqRet_fail;
1356                         else
1357                            sensor_work->result = WqRet_success;
1358                         break;
1359                 }
1360                 case WqCmd_af_continues:
1361                 {
1362                         if(sensor->sensor_focus.focus_cb.sensor_af_const_cb!=NULL){
1363                                 ret = (sensor->sensor_focus.focus_cb.sensor_af_const_cb)(client);
1364                         }
1365                         if (ret < 0)
1366                            sensor_work->result = WqRet_fail;
1367                         else
1368                            sensor_work->result = WqRet_success;
1369                         break;
1370                 }
1371         case WqCmd_af_continues_pause:
1372                 {
1373                         if(sensor->sensor_focus.focus_cb.sensor_af_const_pause_cb!=NULL){
1374                                 ret = (sensor->sensor_focus.focus_cb.sensor_af_const_pause_cb)(client);
1375                         }
1376                         if (ret < 0)
1377                            sensor_work->result = WqRet_fail;
1378                         else
1379                            sensor_work->result = WqRet_success;
1380                         break;
1381                 }
1382                 case WqCmd_af_update_zone:
1383                 {
1384             mutex_lock(&sensor->sensor_focus.focus_lock);
1385             zone_tm_pos[0] = sensor->sensor_focus.focus_zone.lx;
1386                 zone_tm_pos[1] = sensor->sensor_focus.focus_zone.ty;
1387                 zone_tm_pos[2] = sensor->sensor_focus.focus_zone.rx;
1388                 zone_tm_pos[3] = sensor->sensor_focus.focus_zone.dy;
1389             mutex_unlock(&sensor->sensor_focus.focus_lock);
1390
1391                         if(sensor->sensor_focus.focus_cb.sensor_af_zoneupdate_cb!=NULL){
1392                                 ret = (sensor->sensor_focus.focus_cb.sensor_af_zoneupdate_cb)(client,zone_tm_pos);
1393                         }
1394                         if (ret < 0)
1395                            sensor_work->result = WqRet_fail;
1396                         else
1397                            sensor_work->result = WqRet_success;
1398                         break;
1399                 }
1400                 case WqCmd_af_close:
1401                 {
1402                         if(sensor->sensor_focus.focus_cb.sensor_af_close_cb!=NULL){
1403                                 ret = (sensor->sensor_focus.focus_cb.sensor_af_close_cb)(client);
1404                         }
1405                         if (ret < 0)
1406                            sensor_work->result = WqRet_fail;
1407                         else
1408                            sensor_work->result = WqRet_success;
1409                         break;
1410                 }
1411                 default:
1412                         SENSOR_TR("Unknow command(%d) in %s af workqueue!",sensor_work->cmd,SENSOR_NAME_STRING());
1413                         break;
1414         }
1415
1416 //set_end:
1417         if (sensor_work->wait == false) {
1418                 kfree((void*)sensor_work);
1419         } else {
1420                 wake_up(&sensor_work->done);
1421         }
1422         return;
1423 }
1424
1425  int generic_sensor_af_workqueue_set(struct soc_camera_device *icd, enum rk_sensor_focus_wq_cmd cmd, int var, bool wait)
1426 {
1427         struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
1428         struct generic_sensor*sensor = to_generic_sensor(client);
1429         struct rk_sensor_focus_work *wk;
1430         int ret=0;
1431
1432         if (sensor->sensor_focus.sensor_wq == NULL) {
1433                 ret = -EINVAL;
1434                 goto sensor_af_workqueue_set_end;
1435         }
1436
1437         wk = kzalloc(sizeof(struct rk_sensor_focus_work), GFP_KERNEL);
1438         if (wk) {
1439                 wk->client = client;
1440                 INIT_DELAYED_WORK(&wk->dwork, sensor_af_workqueue);
1441                 wk->cmd = cmd;
1442                 wk->result = WqRet_inval;
1443                 wk->wait = wait;
1444                 wk->var = var;
1445                 init_waitqueue_head(&wk->done);
1446
1447         SENSOR_DG("generic_sensor_af_workqueue_set:  cmd: %d",cmd);
1448
1449                 /* ddl@rock-chips.com:
1450                 * video_lock is been locked in v4l2_ioctl function, but auto focus may slow,
1451                 * As a result any other ioctl calls will proceed very, very slowly since each call
1452                 * will have to wait for the AF to finish. Camera preview is pause,because VIDIOC_QBUF
1453                 * and VIDIOC_DQBUF is sched. so unlock video_lock here.
1454                 */
1455                 if (wait == true) {
1456                         queue_delayed_work(sensor->sensor_focus.sensor_wq,&(wk->dwork),0);
1457                         mutex_unlock(&icd->video_lock);
1458                         if (wait_event_timeout(wk->done, (wk->result != WqRet_inval), msecs_to_jiffies(5000)) == 0) {
1459                                 SENSOR_TR("af cmd(%d) is timeout!",cmd);
1460                         }
1461                         flush_workqueue(sensor->sensor_focus.sensor_wq);
1462                         ret = wk->result;
1463                         kfree((void*)wk);
1464                         mutex_lock(&icd->video_lock);
1465                 } else {
1466                         queue_delayed_work(sensor->sensor_focus.sensor_wq,&(wk->dwork),msecs_to_jiffies(10));
1467                 }
1468         } else {
1469                 SENSOR_TR("af cmd(%d) ingore,because struct sensor_work malloc failed!",cmd);
1470                 ret = -1;
1471         }
1472 sensor_af_workqueue_set_end:
1473         return ret;
1474 }
1475
1476
1477 int generic_sensor_s_stream(struct v4l2_subdev *sd, int enable)
1478 {
1479         struct i2c_client *client = v4l2_get_subdevdata(sd);
1480     struct generic_sensor *sensor = to_generic_sensor(client);
1481     struct soc_camera_subdev_desc *ssdd = client->dev.platform_data;
1482         struct soc_camera_device *icd = ssdd->socdev;
1483
1484     SENSOR_DG("s_stream: %d %d",enable,sensor->sensor_focus.focus_state);
1485         if (enable == 1) {
1486             if(sensor->sensor_cb.sensor_s_stream_cb){
1487                 sensor->sensor_cb.sensor_s_stream_cb(sd,enable);
1488             }
1489                 sensor->info_priv.stream = true;
1490         if (sensor->sensor_focus.sensor_wq) {
1491                         if (sensor->sensor_focus.focus_state == FocusState_Inval) {
1492                 generic_sensor_af_workqueue_set(icd, WqCmd_af_init, 0, false);
1493                         }
1494         }
1495         } else if (enable == 0) {
1496             sensor->info_priv.stream = false;
1497             if(sensor->sensor_cb.sensor_s_stream_cb){
1498                 sensor->sensor_cb.sensor_s_stream_cb(sd,enable);
1499             }
1500
1501                 if (sensor->sensor_focus.sensor_wq)
1502             flush_workqueue(sensor->sensor_focus.sensor_wq);
1503
1504         }
1505         return 0;
1506 }
1507
1508 int generic_sensor_enum_framesizes(struct v4l2_subdev *sd, struct v4l2_frmsizeenum *fsize){
1509         struct i2c_client *client = v4l2_get_subdevdata(sd);
1510     struct generic_sensor *sensor = to_generic_sensor(client);
1511
1512     if(sensor->sensor_cb.sensor_enum_framesizes){
1513         return sensor->sensor_cb.sensor_enum_framesizes(sd,fsize);
1514     }else{
1515         return -1;
1516     }
1517
1518 }
1519