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