drivers/media/video/generic_sensor.c

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