drivers/staging/dream/camera/mt9p012_fox.c

   1 /*
   2  * Copyright (C) 2008-2009 QUALCOMM Incorporated.
   3  */
   4
   5 #include <linux/delay.h>
   6 #include <linux/types.h>
   7 #include <linux/i2c.h>
   8 #include <linux/uaccess.h>
   9 #include <linux/miscdevice.h>
  10 #include <linux/kernel.h>
  11 #include <media/msm_camera.h>
  12 #include <mach/gpio.h>
  13 #include <mach/camera.h>
  14 #include "mt9p012.h"
  15
  16 /*=============================================================
  17     SENSOR REGISTER DEFINES
  18 ==============================================================*/
  19 #define MT9P012_REG_MODEL_ID         0x0000
  20 #define MT9P012_MODEL_ID             0x2801
  21 #define REG_GROUPED_PARAMETER_HOLD   0x0104
  22 #define GROUPED_PARAMETER_HOLD       0x0100
  23 #define GROUPED_PARAMETER_UPDATE     0x0000
  24 #define REG_COARSE_INT_TIME          0x3012
  25 #define REG_VT_PIX_CLK_DIV           0x0300
  26 #define REG_VT_SYS_CLK_DIV           0x0302
  27 #define REG_PRE_PLL_CLK_DIV          0x0304
  28 #define REG_PLL_MULTIPLIER           0x0306
  29 #define REG_OP_PIX_CLK_DIV           0x0308
  30 #define REG_OP_SYS_CLK_DIV           0x030A
  31 #define REG_SCALE_M                  0x0404
  32 #define REG_FRAME_LENGTH_LINES       0x300A
  33 #define REG_LINE_LENGTH_PCK          0x300C
  34 #define REG_X_ADDR_START             0x3004
  35 #define REG_Y_ADDR_START             0x3002
  36 #define REG_X_ADDR_END               0x3008
  37 #define REG_Y_ADDR_END               0x3006
  38 #define REG_X_OUTPUT_SIZE            0x034C
  39 #define REG_Y_OUTPUT_SIZE            0x034E
  40 #define REG_FINE_INTEGRATION_TIME    0x3014
  41 #define REG_ROW_SPEED                0x3016
  42 #define MT9P012_REG_RESET_REGISTER   0x301A
  43 #define MT9P012_RESET_REGISTER_PWON  0x10CC
  44 #define MT9P012_RESET_REGISTER_PWOFF 0x10C8
  45 #define REG_READ_MODE                0x3040
  46 #define REG_GLOBAL_GAIN              0x305E
  47 #define REG_TEST_PATTERN_MODE        0x3070
  48
  49 #define MT9P012_REV_7
  50
  51
  52 enum mt9p012_test_mode {
  53         TEST_OFF,
  54         TEST_1,
  55         TEST_2,
  56         TEST_3
  57 };
  58
  59 enum mt9p012_resolution {
  60         QTR_SIZE,
  61         FULL_SIZE,
  62         INVALID_SIZE
  63 };
  64
  65 enum mt9p012_reg_update {
  66         /* Sensor egisters that need to be updated during initialization */
  67         REG_INIT,
  68         /* Sensor egisters that needs periodic I2C writes */
  69         UPDATE_PERIODIC,
  70         /* All the sensor Registers will be updated */
  71         UPDATE_ALL,
  72         /* Not valid update */
  73         UPDATE_INVALID
  74 };
  75
  76 enum mt9p012_setting {
  77         RES_PREVIEW,
  78         RES_CAPTURE
  79 };
  80
  81 /* actuator's Slave Address */
  82 #define MT9P012_AF_I2C_ADDR   0x18
  83
  84 /* AF Total steps parameters */
  85 #define MT9P012_STEPS_NEAR_TO_CLOSEST_INF  32
  86 #define MT9P012_TOTAL_STEPS_NEAR_TO_FAR    32
  87
  88 #define MT9P012_MU5M0_PREVIEW_DUMMY_PIXELS 0
  89 #define MT9P012_MU5M0_PREVIEW_DUMMY_LINES  0
  90
  91 /* Time in milisecs for waiting for the sensor to reset.*/
  92 #define MT9P012_RESET_DELAY_MSECS   66
  93
  94 /* for 20 fps preview */
  95 #define MT9P012_DEFAULT_CLOCK_RATE  24000000
  96 #define MT9P012_DEFAULT_MAX_FPS     26 /* ???? */
  97
  98 struct mt9p012_work {
  99         struct work_struct work;
 100 };
 101 static struct mt9p012_work *mt9p012_sensorw;
 102 static struct i2c_client *mt9p012_client;
 103
 104 struct mt9p012_ctrl {
 105         const struct msm_camera_sensor_info *sensordata;
 106
 107         int sensormode;
 108         uint32_t fps_divider; /* init to 1 * 0x00000400 */
 109         uint32_t pict_fps_divider; /* init to 1 * 0x00000400 */
 110
 111         uint16_t curr_lens_pos;
 112         uint16_t init_curr_lens_pos;
 113         uint16_t my_reg_gain;
 114         uint32_t my_reg_line_count;
 115
 116         enum mt9p012_resolution prev_res;
 117         enum mt9p012_resolution pict_res;
 118         enum mt9p012_resolution curr_res;
 119         enum mt9p012_test_mode  set_test;
 120 };
 121
 122
 123 static struct mt9p012_ctrl *mt9p012_ctrl;
 124 static DECLARE_WAIT_QUEUE_HEAD(mt9p012_wait_queue);
 125 DECLARE_MUTEX(mt9p012_sem);
 126
 127 /*=============================================================
 128         EXTERNAL DECLARATIONS
 129 ==============================================================*/
 130 extern struct mt9p012_reg mt9p012_regs; /* from mt9p012_reg.c */
 131
 132
 133
 134 /*=============================================================*/
 135
 136 static int mt9p012_i2c_rxdata(unsigned short saddr, unsigned char *rxdata,
 137         int length)
 138 {
 139         struct i2c_msg msgs[] = {
 140                 {
 141                         .addr   = saddr,
 142                         .flags = 0,
 143                         .len   = 2,
 144                         .buf   = rxdata,
 145                 },
 146                 {
 147                         .addr   = saddr,
 148                         .flags = I2C_M_RD,
 149                         .len   = length,
 150                         .buf   = rxdata,
 151                 },
 152         };
 153
 154         if (i2c_transfer(mt9p012_client->adapter, msgs, 2) < 0) {
 155                 CDBG("mt9p012_i2c_rxdata failed!\n");
 156                 return -EIO;
 157         }
 158
 159         return 0;
 160 }
 161
 162 static int32_t mt9p012_i2c_read_w(unsigned short saddr, unsigned short raddr,
 163         unsigned short *rdata)
 164 {
 165         int32_t rc = 0;
 166         unsigned char buf[4];
 167
 168         if (!rdata)
 169                 return -EIO;
 170
 171         memset(buf, 0, sizeof(buf));
 172
 173         buf[0] = (raddr & 0xFF00)>>8;
 174         buf[1] = (raddr & 0x00FF);
 175
 176         rc = mt9p012_i2c_rxdata(saddr, buf, 2);
 177         if (rc < 0)
 178                 return rc;
 179
 180         *rdata = buf[0] << 8 | buf[1];
 181
 182         if (rc < 0)
 183                 CDBG("mt9p012_i2c_read failed!\n");
 184
 185         return rc;
 186 }
 187
 188 static int32_t mt9p012_i2c_txdata(unsigned short saddr, unsigned char *txdata,
 189         int length)
 190 {
 191         struct i2c_msg msg[] = {
 192                 {
 193                 .addr  = saddr,
 194                 .flags = 0,
 195                 .len = length,
 196                 .buf = txdata,
 197                 },
 198         };
 199
 200         if (i2c_transfer(mt9p012_client->adapter, msg, 1) < 0) {
 201                 CDBG("mt9p012_i2c_txdata failed\n");
 202                 return -EIO;
 203         }
 204
 205         return 0;
 206 }
 207
 208 static int32_t mt9p012_i2c_write_b(unsigned short saddr, unsigned short baddr,
 209         unsigned short bdata)
 210 {
 211         int32_t rc = -EIO;
 212         unsigned char buf[2];
 213
 214         memset(buf, 0, sizeof(buf));
 215         buf[0] = baddr;
 216         buf[1] = bdata;
 217         rc = mt9p012_i2c_txdata(saddr, buf, 2);
 218
 219         if (rc < 0)
 220                 CDBG("i2c_write failed, saddr = 0x%x addr = 0x%x, val =0x%x!\n",
 221                 saddr, baddr, bdata);
 222
 223         return rc;
 224 }
 225
 226 static int32_t mt9p012_i2c_write_w(unsigned short saddr, unsigned short waddr,
 227         unsigned short wdata)
 228 {
 229         int32_t rc = -EIO;
 230         unsigned char buf[4];
 231
 232         memset(buf, 0, sizeof(buf));
 233         buf[0] = (waddr & 0xFF00)>>8;
 234         buf[1] = (waddr & 0x00FF);
 235         buf[2] = (wdata & 0xFF00)>>8;
 236         buf[3] = (wdata & 0x00FF);
 237
 238         rc = mt9p012_i2c_txdata(saddr, buf, 4);
 239
 240         if (rc < 0)
 241                 CDBG("i2c_write_w failed, addr = 0x%x, val = 0x%x!\n",
 242                         waddr, wdata);
 243
 244         return rc;
 245 }
 246
 247 static int32_t mt9p012_i2c_write_w_table(
 248         struct mt9p012_i2c_reg_conf *reg_conf_tbl, int num)
 249 {
 250         int i;
 251         int32_t rc = -EIO;
 252
 253         for (i = 0; i < num; i++) {
 254                 rc = mt9p012_i2c_write_w(mt9p012_client->addr,
 255                         reg_conf_tbl->waddr, reg_conf_tbl->wdata);
 256                 if (rc < 0)
 257                         break;
 258                 reg_conf_tbl++;
 259         }
 260
 261         return rc;
 262 }
 263
 264 static int32_t mt9p012_test(enum mt9p012_test_mode mo)
 265 {
 266         int32_t rc = 0;
 267
 268         rc = mt9p012_i2c_write_w(mt9p012_client->addr,
 269                 REG_GROUPED_PARAMETER_HOLD,
 270                 GROUPED_PARAMETER_HOLD);
 271         if (rc < 0)
 272                 return rc;
 273
 274         if (mo == TEST_OFF)
 275                 return 0;
 276         else {
 277                 rc = mt9p012_i2c_write_w_table(mt9p012_regs.ttbl, mt9p012_regs.ttbl_size);
 278                 if (rc < 0)
 279                         return rc;
 280
 281                 rc = mt9p012_i2c_write_w(mt9p012_client->addr,
 282                                 REG_TEST_PATTERN_MODE, (uint16_t)mo);
 283                 if (rc < 0)
 284                         return rc;
 285         }
 286
 287         rc = mt9p012_i2c_write_w(mt9p012_client->addr,
 288                 REG_GROUPED_PARAMETER_HOLD,
 289                 GROUPED_PARAMETER_UPDATE);
 290         if (rc < 0)
 291                 return rc;
 292
 293         return rc;
 294 }
 295
 296 static int32_t mt9p012_lens_shading_enable(uint8_t is_enable)
 297 {
 298         int32_t rc = 0;
 299
 300         CDBG("%s: entered. enable = %d\n", __func__, is_enable);
 301
 302         rc = mt9p012_i2c_write_w(mt9p012_client->addr,
 303                 REG_GROUPED_PARAMETER_HOLD, GROUPED_PARAMETER_HOLD);
 304         if (rc < 0)
 305                 return rc;
 306
 307         rc = mt9p012_i2c_write_w(mt9p012_client->addr, 0x3780,
 308                 ((uint16_t) is_enable) << 15);
 309         if (rc < 0)
 310                 return rc;
 311
 312         rc = mt9p012_i2c_write_w(mt9p012_client->addr,
 313                 REG_GROUPED_PARAMETER_HOLD, GROUPED_PARAMETER_UPDATE);
 314
 315         CDBG("%s: exiting. rc = %d\n", __func__, rc);
 316         return rc;
 317 }
 318
 319 static int32_t mt9p012_set_lc(void)
 320 {
 321         int32_t rc;
 322
 323         rc = mt9p012_i2c_write_w_table(mt9p012_regs.lctbl, mt9p012_regs.lctbl_size);
 324         if (rc < 0)
 325                 return rc;
 326
 327         rc = mt9p012_i2c_write_w_table(mt9p012_regs.rftbl, mt9p012_regs.rftbl_size);
 328
 329         return rc;
 330 }
 331
 332 static void mt9p012_get_pict_fps(uint16_t fps, uint16_t *pfps)
 333 {
 334         /* input fps is preview fps in Q8 format */
 335         uint32_t divider;   /*Q10 */
 336         uint32_t pclk_mult; /*Q10 */
 337
 338         if (mt9p012_ctrl->prev_res == QTR_SIZE) {
 339                 divider = (uint32_t)
 340                 (((mt9p012_regs.reg_pat[RES_PREVIEW].frame_length_lines *
 341                 mt9p012_regs.reg_pat[RES_PREVIEW].line_length_pck) * 0x00000400) /
 342                 (mt9p012_regs.reg_pat[RES_CAPTURE].frame_length_lines *
 343                 mt9p012_regs.reg_pat[RES_CAPTURE].line_length_pck));
 344
 345                 pclk_mult =
 346                 (uint32_t) ((mt9p012_regs.reg_pat[RES_CAPTURE].pll_multiplier *
 347                 0x00000400) / (mt9p012_regs.reg_pat[RES_PREVIEW].pll_multiplier));
 348         } else {
 349                 /* full size resolution used for preview. */
 350                 divider   = 0x00000400;  /*1.0 */
 351                 pclk_mult = 0x00000400;  /*1.0 */
 352         }
 353
 354         /* Verify PCLK settings and frame sizes. */
 355         *pfps = (uint16_t) (fps * divider * pclk_mult / 0x00000400 /
 356                 0x00000400);
 357 }
 358
 359 static uint16_t mt9p012_get_prev_lines_pf(void)
 360 {
 361         if (mt9p012_ctrl->prev_res == QTR_SIZE)
 362                 return mt9p012_regs.reg_pat[RES_PREVIEW].frame_length_lines;
 363         else
 364                 return mt9p012_regs.reg_pat[RES_CAPTURE].frame_length_lines;
 365 }
 366
 367 static uint16_t mt9p012_get_prev_pixels_pl(void)
 368 {
 369         if (mt9p012_ctrl->prev_res == QTR_SIZE)
 370                 return mt9p012_regs.reg_pat[RES_PREVIEW].line_length_pck;
 371         else
 372                 return mt9p012_regs.reg_pat[RES_CAPTURE].line_length_pck;
 373 }
 374
 375 static uint16_t mt9p012_get_pict_lines_pf(void)
 376 {
 377         return mt9p012_regs.reg_pat[RES_CAPTURE].frame_length_lines;
 378 }
 379
 380 static uint16_t mt9p012_get_pict_pixels_pl(void)
 381 {
 382         return mt9p012_regs.reg_pat[RES_CAPTURE].line_length_pck;
 383 }
 384
 385 static uint32_t mt9p012_get_pict_max_exp_lc(void)
 386 {
 387         uint16_t snapshot_lines_per_frame;
 388
 389         if (mt9p012_ctrl->pict_res == QTR_SIZE)
 390                 snapshot_lines_per_frame =
 391                 mt9p012_regs.reg_pat[RES_PREVIEW].frame_length_lines - 1;
 392         else
 393                 snapshot_lines_per_frame =
 394                 mt9p012_regs.reg_pat[RES_CAPTURE].frame_length_lines - 1;
 395
 396         return snapshot_lines_per_frame * 24;
 397 }
 398
 399 static int32_t mt9p012_set_fps(struct fps_cfg *fps)
 400 {
 401         /* input is new fps in Q10 format */
 402         int32_t rc = 0;
 403
 404         mt9p012_ctrl->fps_divider = fps->fps_div;
 405         mt9p012_ctrl->pict_fps_divider = fps->pict_fps_div;
 406
 407         rc =
 408                 mt9p012_i2c_write_w(mt9p012_client->addr,
 409                         REG_GROUPED_PARAMETER_HOLD,
 410                         GROUPED_PARAMETER_HOLD);
 411         if (rc < 0)
 412                 return -EBUSY;
 413
 414         rc =
 415                 mt9p012_i2c_write_w(mt9p012_client->addr,
 416                         REG_LINE_LENGTH_PCK,
 417                         (mt9p012_regs.reg_pat[RES_PREVIEW].line_length_pck *
 418                         fps->f_mult / 0x00000400));
 419         if (rc < 0)
 420                 return rc;
 421
 422         rc =
 423                 mt9p012_i2c_write_w(mt9p012_client->addr,
 424                         REG_GROUPED_PARAMETER_HOLD,
 425                         GROUPED_PARAMETER_UPDATE);
 426
 427         return rc;
 428 }
 429
 430 static int32_t mt9p012_write_exp_gain(uint16_t gain, uint32_t line)
 431 {
 432         uint16_t max_legal_gain = 0x01FF;
 433         uint32_t line_length_ratio = 0x00000400;
 434         enum mt9p012_setting setting;
 435         int32_t rc = 0;
 436
 437         CDBG("Line:%d mt9p012_write_exp_gain \n", __LINE__);
 438
 439         if (mt9p012_ctrl->sensormode == SENSOR_PREVIEW_MODE) {
 440                 mt9p012_ctrl->my_reg_gain = gain;
 441                 mt9p012_ctrl->my_reg_line_count = (uint16_t)line;
 442         }
 443
 444         if (gain > max_legal_gain) {
 445                 CDBG("Max legal gain Line:%d \n", __LINE__);
 446                 gain = max_legal_gain;
 447         }
 448
 449         /* Verify no overflow */
 450         if (mt9p012_ctrl->sensormode != SENSOR_SNAPSHOT_MODE) {
 451                 line = (uint32_t)(line * mt9p012_ctrl->fps_divider /
 452                         0x00000400);
 453                 setting = RES_PREVIEW;
 454         } else {
 455                 line = (uint32_t)(line * mt9p012_ctrl->pict_fps_divider /
 456                         0x00000400);
 457                 setting = RES_CAPTURE;
 458         }
 459
 460         /* Set digital gain to 1 */
 461 #ifdef MT9P012_REV_7
 462         gain |= 0x1000;
 463 #else
 464         gain |= 0x0200;
 465 #endif
 466
 467         if ((mt9p012_regs.reg_pat[setting].frame_length_lines - 1) < line) {
 468                 line_length_ratio = (uint32_t) (line * 0x00000400) /
 469                 (mt9p012_regs.reg_pat[setting].frame_length_lines - 1);
 470         } else
 471                 line_length_ratio = 0x00000400;
 472
 473         rc =
 474                 mt9p012_i2c_write_w(mt9p012_client->addr,
 475                         REG_GROUPED_PARAMETER_HOLD,
 476                         GROUPED_PARAMETER_HOLD);
 477         if (rc < 0) {
 478                 CDBG("mt9p012_i2c_write_w failed... Line:%d \n", __LINE__);
 479                 return rc;
 480         }
 481
 482         rc =
 483                 mt9p012_i2c_write_w(
 484                         mt9p012_client->addr,
 485                         REG_GLOBAL_GAIN, gain);
 486         if (rc < 0) {
 487                 CDBG("mt9p012_i2c_write_w failed... Line:%d \n", __LINE__);
 488                 return rc;
 489         }
 490
 491         rc =
 492                 mt9p012_i2c_write_w(mt9p012_client->addr,
 493                         REG_COARSE_INT_TIME,
 494                         line);
 495         if (rc < 0) {
 496                 CDBG("mt9p012_i2c_write_w failed... Line:%d \n", __LINE__);
 497                 return rc;
 498         }
 499
 500         CDBG("mt9p012_write_exp_gain: gain = %d, line = %d\n", gain, line);
 501
 502         rc =
 503                 mt9p012_i2c_write_w(mt9p012_client->addr,
 504                         REG_GROUPED_PARAMETER_HOLD,
 505                         GROUPED_PARAMETER_UPDATE);
 506         if (rc < 0)
 507                 CDBG("mt9p012_i2c_write_w failed... Line:%d \n", __LINE__);
 508
 509         return rc;
 510 }
 511
 512 static int32_t mt9p012_set_pict_exp_gain(uint16_t gain, uint32_t line)
 513 {
 514         int32_t rc = 0;
 515
 516         CDBG("Line:%d mt9p012_set_pict_exp_gain \n", __LINE__);
 517
 518         rc =
 519                 mt9p012_write_exp_gain(gain, line);
 520         if (rc < 0) {
 521                 CDBG("Line:%d mt9p012_set_pict_exp_gain failed... \n",
 522                         __LINE__);
 523                 return rc;
 524         }
 525
 526         rc =
 527         mt9p012_i2c_write_w(mt9p012_client->addr,
 528                 MT9P012_REG_RESET_REGISTER,
 529                 0x10CC | 0x0002);
 530         if (rc < 0) {
 531                 CDBG("mt9p012_i2c_write_w failed... Line:%d \n", __LINE__);
 532                 return rc;
 533         }
 534
 535         mdelay(5);
 536
 537         /* camera_timed_wait(snapshot_wait*exposure_ratio); */
 538         return rc;
 539 }
 540
 541 static int32_t mt9p012_setting(enum mt9p012_reg_update rupdate,
 542         enum mt9p012_setting rt)
 543 {
 544         int32_t rc = 0;
 545
 546         switch (rupdate) {
 547         case UPDATE_PERIODIC:
 548         if (rt == RES_PREVIEW || rt == RES_CAPTURE) {
 549
 550                 struct mt9p012_i2c_reg_conf ppc_tbl[] = {
 551                 {REG_GROUPED_PARAMETER_HOLD, GROUPED_PARAMETER_HOLD},
 552                 {REG_ROW_SPEED, mt9p012_regs.reg_pat[rt].row_speed},
 553                 {REG_X_ADDR_START, mt9p012_regs.reg_pat[rt].x_addr_start},
 554                 {REG_X_ADDR_END, mt9p012_regs.reg_pat[rt].x_addr_end},
 555                 {REG_Y_ADDR_START, mt9p012_regs.reg_pat[rt].y_addr_start},
 556                 {REG_Y_ADDR_END, mt9p012_regs.reg_pat[rt].y_addr_end},
 557                 {REG_READ_MODE, mt9p012_regs.reg_pat[rt].read_mode},
 558                 {REG_SCALE_M, mt9p012_regs.reg_pat[rt].scale_m},
 559                 {REG_X_OUTPUT_SIZE, mt9p012_regs.reg_pat[rt].x_output_size},
 560                 {REG_Y_OUTPUT_SIZE, mt9p012_regs.reg_pat[rt].y_output_size},
 561
 562                 {REG_LINE_LENGTH_PCK, mt9p012_regs.reg_pat[rt].line_length_pck},
 563                 {REG_FRAME_LENGTH_LINES,
 564                         (mt9p012_regs.reg_pat[rt].frame_length_lines *
 565                         mt9p012_ctrl->fps_divider / 0x00000400)},
 566                 {REG_COARSE_INT_TIME, mt9p012_regs.reg_pat[rt].coarse_int_time},
 567                 {REG_FINE_INTEGRATION_TIME, mt9p012_regs.reg_pat[rt].fine_int_time},
 568                 {REG_GROUPED_PARAMETER_HOLD, GROUPED_PARAMETER_UPDATE},
 569                 };
 570
 571                 rc = mt9p012_i2c_write_w_table(&ppc_tbl[0],
 572                         ARRAY_SIZE(ppc_tbl));
 573                 if (rc < 0)
 574                         return rc;
 575
 576                 rc = mt9p012_test(mt9p012_ctrl->set_test);
 577                 if (rc < 0)
 578                         return rc;
 579
 580                 rc =
 581                         mt9p012_i2c_write_w(mt9p012_client->addr,
 582                         MT9P012_REG_RESET_REGISTER,
 583                         MT9P012_RESET_REGISTER_PWON | 0x0002);
 584                 if (rc < 0)
 585                         return rc;
 586
 587                 mdelay(5); /* 15? wait for sensor to transition*/
 588
 589                 return rc;
 590         }
 591         break; /* UPDATE_PERIODIC */
 592
 593         case REG_INIT:
 594         if (rt == RES_PREVIEW || rt == RES_CAPTURE) {
 595                 struct mt9p012_i2c_reg_conf ipc_tbl1[] = {
 596                 {MT9P012_REG_RESET_REGISTER, MT9P012_RESET_REGISTER_PWOFF},
 597                 {REG_VT_PIX_CLK_DIV, mt9p012_regs.reg_pat[rt].vt_pix_clk_div},
 598                 {REG_VT_SYS_CLK_DIV, mt9p012_regs.reg_pat[rt].vt_sys_clk_div},
 599                 {REG_PRE_PLL_CLK_DIV, mt9p012_regs.reg_pat[rt].pre_pll_clk_div},
 600                 {REG_PLL_MULTIPLIER, mt9p012_regs.reg_pat[rt].pll_multiplier},
 601                 {REG_OP_PIX_CLK_DIV, mt9p012_regs.reg_pat[rt].op_pix_clk_div},
 602                 {REG_OP_SYS_CLK_DIV, mt9p012_regs.reg_pat[rt].op_sys_clk_div},
 603 #ifdef MT9P012_REV_7
 604                 {0x30B0, 0x0001},
 605                 {0x308E, 0xE060},
 606                 {0x3092, 0x0A52},
 607                 {0x3094, 0x4656},
 608                 {0x3096, 0x5652},
 609                 {0x30CA, 0x8006},
 610                 {0x312A, 0xDD02},
 611                 {0x312C, 0x00E4},
 612                 {0x3170, 0x299A},
 613 #endif
 614                 /* optimized settings for noise */
 615                 {0x3088, 0x6FF6},
 616                 {0x3154, 0x0282},
 617                 {0x3156, 0x0381},
 618                 {0x3162, 0x04CE},
 619                 {0x0204, 0x0010},
 620                 {0x0206, 0x0010},
 621                 {0x0208, 0x0010},
 622                 {0x020A, 0x0010},
 623                 {0x020C, 0x0010},
 624                 {MT9P012_REG_RESET_REGISTER, MT9P012_RESET_REGISTER_PWON},
 625                 };
 626
 627                 struct mt9p012_i2c_reg_conf ipc_tbl2[] = {
 628                 {MT9P012_REG_RESET_REGISTER, MT9P012_RESET_REGISTER_PWOFF},
 629                 {REG_VT_PIX_CLK_DIV, mt9p012_regs.reg_pat[rt].vt_pix_clk_div},
 630                 {REG_VT_SYS_CLK_DIV, mt9p012_regs.reg_pat[rt].vt_sys_clk_div},
 631                 {REG_PRE_PLL_CLK_DIV, mt9p012_regs.reg_pat[rt].pre_pll_clk_div},
 632                 {REG_PLL_MULTIPLIER, mt9p012_regs.reg_pat[rt].pll_multiplier},
 633                 {REG_OP_PIX_CLK_DIV, mt9p012_regs.reg_pat[rt].op_pix_clk_div},
 634                 {REG_OP_SYS_CLK_DIV, mt9p012_regs.reg_pat[rt].op_sys_clk_div},
 635 #ifdef MT9P012_REV_7
 636                 {0x30B0, 0x0001},
 637                 {0x308E, 0xE060},
 638                 {0x3092, 0x0A52},
 639                 {0x3094, 0x4656},
 640                 {0x3096, 0x5652},
 641                 {0x30CA, 0x8006},
 642                 {0x312A, 0xDD02},
 643                 {0x312C, 0x00E4},
 644                 {0x3170, 0x299A},
 645 #endif
 646                 /* optimized settings for noise */
 647                 {0x3088, 0x6FF6},
 648                 {0x3154, 0x0282},
 649                 {0x3156, 0x0381},
 650                 {0x3162, 0x04CE},
 651                 {0x0204, 0x0010},
 652                 {0x0206, 0x0010},
 653                 {0x0208, 0x0010},
 654                 {0x020A, 0x0010},
 655                 {0x020C, 0x0010},
 656                 {MT9P012_REG_RESET_REGISTER, MT9P012_RESET_REGISTER_PWON},
 657                 };
 658
 659                 struct mt9p012_i2c_reg_conf ipc_tbl3[] = {
 660                 {REG_GROUPED_PARAMETER_HOLD, GROUPED_PARAMETER_HOLD},
 661                 /* Set preview or snapshot mode */
 662                 {REG_ROW_SPEED, mt9p012_regs.reg_pat[rt].row_speed},
 663                 {REG_X_ADDR_START, mt9p012_regs.reg_pat[rt].x_addr_start},
 664                 {REG_X_ADDR_END, mt9p012_regs.reg_pat[rt].x_addr_end},
 665                 {REG_Y_ADDR_START, mt9p012_regs.reg_pat[rt].y_addr_start},
 666                 {REG_Y_ADDR_END, mt9p012_regs.reg_pat[rt].y_addr_end},
 667                 {REG_READ_MODE, mt9p012_regs.reg_pat[rt].read_mode},
 668                 {REG_SCALE_M, mt9p012_regs.reg_pat[rt].scale_m},
 669                 {REG_X_OUTPUT_SIZE, mt9p012_regs.reg_pat[rt].x_output_size},
 670                 {REG_Y_OUTPUT_SIZE, mt9p012_regs.reg_pat[rt].y_output_size},
 671                 {REG_LINE_LENGTH_PCK, mt9p012_regs.reg_pat[rt].line_length_pck},
 672                 {REG_FRAME_LENGTH_LINES,
 673                         mt9p012_regs.reg_pat[rt].frame_length_lines},
 674                 {REG_COARSE_INT_TIME, mt9p012_regs.reg_pat[rt].coarse_int_time},
 675                 {REG_FINE_INTEGRATION_TIME, mt9p012_regs.reg_pat[rt].fine_int_time},
 676                 {REG_GROUPED_PARAMETER_HOLD, GROUPED_PARAMETER_UPDATE},
 677                 };
 678
 679                 /* reset fps_divider */
 680                 mt9p012_ctrl->fps_divider = 1 * 0x0400;
 681
 682                 rc = mt9p012_i2c_write_w_table(&ipc_tbl1[0],
 683                         ARRAY_SIZE(ipc_tbl1));
 684                 if (rc < 0)
 685                         return rc;
 686
 687                 rc = mt9p012_i2c_write_w_table(&ipc_tbl2[0],
 688                         ARRAY_SIZE(ipc_tbl2));
 689                 if (rc < 0)
 690                         return rc;
 691
 692                 mdelay(5);
 693
 694                 rc = mt9p012_i2c_write_w_table(&ipc_tbl3[0],
 695                         ARRAY_SIZE(ipc_tbl3));
 696                 if (rc < 0)
 697                         return rc;
 698
 699                 /* load lens shading */
 700                 rc = mt9p012_i2c_write_w(mt9p012_client->addr,
 701                         REG_GROUPED_PARAMETER_HOLD, GROUPED_PARAMETER_HOLD);
 702                 if (rc < 0)
 703                         return rc;
 704
 705                 rc = mt9p012_set_lc();
 706                 if (rc < 0)
 707                         return rc;
 708
 709                 rc = mt9p012_i2c_write_w(mt9p012_client->addr,
 710                         REG_GROUPED_PARAMETER_HOLD, GROUPED_PARAMETER_UPDATE);
 711
 712                 if (rc < 0)
 713                         return rc;
 714         }
 715         break; /* case REG_INIT: */
 716
 717         default:
 718                 rc = -EINVAL;
 719                 break;
 720         } /* switch (rupdate) */
 721
 722         return rc;
 723 }
 724
 725 static int32_t mt9p012_video_config(int mode, int res)
 726 {
 727         int32_t rc;
 728
 729         switch (res) {
 730         case QTR_SIZE:
 731                 rc = mt9p012_setting(UPDATE_PERIODIC, RES_PREVIEW);
 732                 if (rc < 0)
 733                         return rc;
 734
 735                 CDBG("mt9p012 sensor configuration done!\n");
 736                 break;
 737
 738         case FULL_SIZE:
 739                 rc =
 740                 mt9p012_setting(UPDATE_PERIODIC, RES_CAPTURE);
 741                 if (rc < 0)
 742                         return rc;
 743
 744                 break;
 745
 746         default:
 747                 return 0;
 748         } /* switch */
 749
 750         mt9p012_ctrl->prev_res = res;
 751         mt9p012_ctrl->curr_res = res;
 752         mt9p012_ctrl->sensormode = mode;
 753
 754         rc =
 755                 mt9p012_write_exp_gain(mt9p012_ctrl->my_reg_gain,
 756                         mt9p012_ctrl->my_reg_line_count);
 757
 758         rc =
 759                 mt9p012_i2c_write_w(mt9p012_client->addr,
 760                         MT9P012_REG_RESET_REGISTER,
 761                         0x10cc|0x0002);
 762
 763         return rc;
 764 }
 765
 766 static int32_t mt9p012_snapshot_config(int mode)
 767 {
 768         int32_t rc = 0;
 769
 770         rc = mt9p012_setting(UPDATE_PERIODIC, RES_CAPTURE);
 771         if (rc < 0)
 772                 return rc;
 773
 774         mt9p012_ctrl->curr_res = mt9p012_ctrl->pict_res;
 775
 776         mt9p012_ctrl->sensormode = mode;
 777
 778         return rc;
 779 }
 780
 781 static int32_t mt9p012_raw_snapshot_config(int mode)
 782 {
 783         int32_t rc = 0;
 784
 785         rc = mt9p012_setting(UPDATE_PERIODIC, RES_CAPTURE);
 786         if (rc < 0)
 787                 return rc;
 788
 789         mt9p012_ctrl->curr_res = mt9p012_ctrl->pict_res;
 790
 791         mt9p012_ctrl->sensormode = mode;
 792
 793         return rc;
 794 }
 795
 796 static int32_t mt9p012_power_down(void)
 797 {
 798         int32_t rc = 0;
 799
 800         rc = mt9p012_i2c_write_w(mt9p012_client->addr,
 801                 MT9P012_REG_RESET_REGISTER,
 802                 MT9P012_RESET_REGISTER_PWOFF);
 803
 804         mdelay(5);
 805         return rc;
 806 }
 807
 808 static int32_t mt9p012_move_focus(int direction, int32_t num_steps)
 809 {
 810         int16_t step_direction;
 811         int16_t actual_step;
 812         int16_t next_position;
 813         uint8_t code_val_msb, code_val_lsb;
 814
 815         if (num_steps > MT9P012_TOTAL_STEPS_NEAR_TO_FAR)
 816                 num_steps = MT9P012_TOTAL_STEPS_NEAR_TO_FAR;
 817         else if (num_steps == 0) {
 818                 CDBG("mt9p012_move_focus failed at line %d ...\n", __LINE__);
 819                 return -EINVAL;
 820         }
 821
 822         if (direction == MOVE_NEAR)
 823                 step_direction = 16; /* 10bit */
 824         else if (direction == MOVE_FAR)
 825                 step_direction = -16; /* 10 bit */
 826         else {
 827                 CDBG("mt9p012_move_focus failed at line %d ...\n", __LINE__);
 828                 return -EINVAL;
 829         }
 830
 831         if (mt9p012_ctrl->curr_lens_pos < mt9p012_ctrl->init_curr_lens_pos)
 832                 mt9p012_ctrl->curr_lens_pos =
 833                         mt9p012_ctrl->init_curr_lens_pos;
 834
 835         actual_step = (int16_t)(step_direction * (int16_t)num_steps);
 836         next_position = (int16_t)(mt9p012_ctrl->curr_lens_pos + actual_step);
 837
 838         if (next_position > 1023)
 839                 next_position = 1023;
 840         else if (next_position < 0)
 841                 next_position = 0;
 842
 843         code_val_msb = next_position >> 4;
 844         code_val_lsb = (next_position & 0x000F) << 4;
 845         /* code_val_lsb |= mode_mask; */
 846
 847         /* Writing the digital code for current to the actuator */
 848         if (mt9p012_i2c_write_b(MT9P012_AF_I2C_ADDR >> 1,
 849                 code_val_msb, code_val_lsb) < 0) {
 850                 CDBG("mt9p012_move_focus failed at line %d ...\n", __LINE__);
 851                 return -EBUSY;
 852         }
 853
 854         /* Storing the current lens Position */
 855         mt9p012_ctrl->curr_lens_pos = next_position;
 856
 857         return 0;
 858 }
 859
 860 static int32_t mt9p012_set_default_focus(void)
 861 {
 862         int32_t rc = 0;
 863         uint8_t code_val_msb, code_val_lsb;
 864
 865         code_val_msb = 0x00;
 866         code_val_lsb = 0x00;
 867
 868         /* Write the digital code for current to the actuator */
 869         rc = mt9p012_i2c_write_b(MT9P012_AF_I2C_ADDR >> 1,
 870                 code_val_msb, code_val_lsb);
 871
 872         mt9p012_ctrl->curr_lens_pos = 0;
 873         mt9p012_ctrl->init_curr_lens_pos = 0;
 874
 875         return rc;
 876 }
 877
 878 static int mt9p012_probe_init_done(const struct msm_camera_sensor_info *data)
 879 {
 880         gpio_direction_output(data->sensor_reset, 0);
 881         gpio_free(data->sensor_reset);
 882         return 0;
 883 }
 884
 885 static int mt9p012_probe_init_sensor(const struct msm_camera_sensor_info *data)
 886 {
 887         int32_t  rc;
 888         uint16_t chipid;
 889
 890         rc = gpio_request(data->sensor_reset, "mt9p012");
 891         if (!rc)
 892                 gpio_direction_output(data->sensor_reset, 1);
 893         else
 894                 goto init_probe_done;
 895
 896         mdelay(20);
 897
 898         /* RESET the sensor image part via I2C command */
 899         CDBG("mt9p012_sensor_init(): reseting sensor.\n");
 900         rc = mt9p012_i2c_write_w(mt9p012_client->addr,
 901                 MT9P012_REG_RESET_REGISTER, 0x10CC|0x0001);
 902         if (rc < 0) {
 903                 CDBG("sensor reset failed. rc = %d\n", rc);
 904                 goto init_probe_fail;
 905         }
 906
 907         mdelay(MT9P012_RESET_DELAY_MSECS);
 908
 909         /* 3. Read sensor Model ID: */
 910         rc = mt9p012_i2c_read_w(mt9p012_client->addr,
 911                 MT9P012_REG_MODEL_ID, &chipid);
 912         if (rc < 0)
 913                 goto init_probe_fail;
 914
 915         /* 4. Compare sensor ID to MT9T012VC ID: */
 916         if (chipid != MT9P012_MODEL_ID) {
 917                 CDBG("mt9p012 wrong model_id = 0x%x\n", chipid);
 918                 rc = -ENODEV;
 919                 goto init_probe_fail;
 920         }
 921
 922         rc = mt9p012_i2c_write_w(mt9p012_client->addr, 0x306E, 0x9000);
 923         if (rc < 0) {
 924                 CDBG("REV_7 write failed. rc = %d\n", rc);
 925                 goto init_probe_fail;
 926         }
 927
 928         /* RESET_REGISTER, enable parallel interface and disable serialiser */
 929         CDBG("mt9p012_sensor_init(): enabling parallel interface.\n");
 930         rc = mt9p012_i2c_write_w(mt9p012_client->addr, 0x301A, 0x10CC);
 931         if (rc < 0) {
 932                 CDBG("enable parallel interface failed. rc = %d\n", rc);
 933                 goto init_probe_fail;
 934         }
 935
 936         /* To disable the 2 extra lines */
 937         rc = mt9p012_i2c_write_w(mt9p012_client->addr,
 938                 0x3064, 0x0805);
 939
 940         if (rc < 0) {
 941                 CDBG("disable the 2 extra lines failed. rc = %d\n", rc);
 942                 goto init_probe_fail;
 943         }
 944
 945         mdelay(MT9P012_RESET_DELAY_MSECS);
 946         goto init_probe_done;
 947
 948 init_probe_fail:
 949         mt9p012_probe_init_done(data);
 950 init_probe_done:
 951         return rc;
 952 }
 953
 954 static int mt9p012_sensor_open_init(const struct msm_camera_sensor_info *data)
 955 {
 956         int32_t  rc;
 957
 958         mt9p012_ctrl = kzalloc(sizeof(struct mt9p012_ctrl), GFP_KERNEL);
 959         if (!mt9p012_ctrl) {
 960                 CDBG("mt9p012_init failed!\n");
 961                 rc = -ENOMEM;
 962                 goto init_done;
 963         }
 964
 965         mt9p012_ctrl->fps_divider = 1 * 0x00000400;
 966         mt9p012_ctrl->pict_fps_divider = 1 * 0x00000400;
 967         mt9p012_ctrl->set_test = TEST_OFF;
 968         mt9p012_ctrl->prev_res = QTR_SIZE;
 969         mt9p012_ctrl->pict_res = FULL_SIZE;
 970
 971         if (data)
 972                 mt9p012_ctrl->sensordata = data;
 973
 974         /* enable mclk first */
 975         msm_camio_clk_rate_set(MT9P012_DEFAULT_CLOCK_RATE);
 976         mdelay(20);
 977
 978         msm_camio_camif_pad_reg_reset();
 979         mdelay(20);
 980
 981         rc = mt9p012_probe_init_sensor(data);
 982         if (rc < 0)
 983                 goto init_fail1;
 984
 985         if (mt9p012_ctrl->prev_res == QTR_SIZE)
 986                 rc = mt9p012_setting(REG_INIT, RES_PREVIEW);
 987         else
 988                 rc = mt9p012_setting(REG_INIT, RES_CAPTURE);
 989
 990         if (rc < 0) {
 991                 CDBG("mt9p012_setting failed. rc = %d\n", rc);
 992                 goto init_fail1;
 993         }
 994
 995         /* sensor : output enable */
 996         CDBG("mt9p012_sensor_open_init(): enabling output.\n");
 997         rc = mt9p012_i2c_write_w(mt9p012_client->addr,
 998                 MT9P012_REG_RESET_REGISTER, MT9P012_RESET_REGISTER_PWON);
 999         if (rc < 0) {
1000                 CDBG("sensor output enable failed. rc = %d\n", rc);
1001                 goto init_fail1;
1002         }
1003
1004         /* TODO: enable AF actuator */
1005 #if 0
1006         CDBG("enable AF actuator, gpio = %d\n",
1007                 mt9p012_ctrl->sensordata->vcm_pwd);
1008         rc = gpio_request(mt9p012_ctrl->sensordata->vcm_pwd, "mt9p012");
1009         if (!rc)
1010                 gpio_direction_output(mt9p012_ctrl->sensordata->vcm_pwd, 1);
1011         else {
1012                 CDBG("mt9p012_ctrl gpio request failed!\n");
1013                 goto init_fail1;
1014         }
1015         mdelay(20);
1016
1017         rc = mt9p012_set_default_focus();
1018 #endif
1019         if (rc >= 0)
1020                 goto init_done;
1021
1022         /* TODO:
1023          * gpio_direction_output(mt9p012_ctrl->sensordata->vcm_pwd, 0);
1024          * gpio_free(mt9p012_ctrl->sensordata->vcm_pwd); */
1025 init_fail1:
1026         mt9p012_probe_init_done(data);
1027         kfree(mt9p012_ctrl);
1028 init_done:
1029         return rc;
1030 }
1031
1032 static int mt9p012_init_client(struct i2c_client *client)
1033 {
1034         /* Initialize the MSM_CAMI2C Chip */
1035         init_waitqueue_head(&mt9p012_wait_queue);
1036         return 0;
1037 }
1038
1039 static int32_t mt9p012_set_sensor_mode(int mode, int res)
1040 {
1041         int32_t rc = 0;
1042
1043         switch (mode) {
1044         case SENSOR_PREVIEW_MODE:
1045                 rc = mt9p012_video_config(mode, res);
1046                 break;
1047
1048         case SENSOR_SNAPSHOT_MODE:
1049                 rc = mt9p012_snapshot_config(mode);
1050                 break;
1051
1052         case SENSOR_RAW_SNAPSHOT_MODE:
1053                 rc = mt9p012_raw_snapshot_config(mode);
1054                 break;
1055
1056         default:
1057                 rc = -EINVAL;
1058                 break;
1059         }
1060
1061         return rc;
1062 }
1063
1064 int mt9p012_sensor_config(void __user *argp)
1065 {
1066         struct sensor_cfg_data cdata;
1067         int rc = 0;
1068
1069         if (copy_from_user(&cdata,
1070                         (void *)argp,
1071                         sizeof(struct sensor_cfg_data)))
1072                 return -EFAULT;
1073
1074         down(&mt9p012_sem);
1075
1076                 CDBG("%s: cfgtype = %d\n", __func__, cdata.cfgtype);
1077         switch (cdata.cfgtype) {
1078         case CFG_GET_PICT_FPS:
1079                 mt9p012_get_pict_fps(cdata.cfg.gfps.prevfps,
1080                                 &(cdata.cfg.gfps.pictfps));
1081
1082                 if (copy_to_user((void *)argp, &cdata,
1083                                 sizeof(struct sensor_cfg_data)))
1084                         rc = -EFAULT;
1085                 break;
1086
1087         case CFG_GET_PREV_L_PF:
1088                 cdata.cfg.prevl_pf = mt9p012_get_prev_lines_pf();
1089
1090                 if (copy_to_user((void *)argp,
1091                                 &cdata,
1092                                 sizeof(struct sensor_cfg_data)))
1093                         rc = -EFAULT;
1094                 break;
1095
1096         case CFG_GET_PREV_P_PL:
1097                 cdata.cfg.prevp_pl = mt9p012_get_prev_pixels_pl();
1098
1099                 if (copy_to_user((void *)argp,
1100                                 &cdata,
1101                                 sizeof(struct sensor_cfg_data)))
1102                         rc = -EFAULT;
1103                 break;
1104
1105         case CFG_GET_PICT_L_PF:
1106                 cdata.cfg.pictl_pf = mt9p012_get_pict_lines_pf();
1107
1108                 if (copy_to_user((void *)argp,
1109                                 &cdata,
1110                                 sizeof(struct sensor_cfg_data)))
1111                         rc = -EFAULT;
1112                 break;
1113
1114         case CFG_GET_PICT_P_PL:
1115                 cdata.cfg.pictp_pl = mt9p012_get_pict_pixels_pl();
1116
1117                 if (copy_to_user((void *)argp,
1118                                 &cdata,
1119                                 sizeof(struct sensor_cfg_data)))
1120                         rc = -EFAULT;
1121                 break;
1122
1123         case CFG_GET_PICT_MAX_EXP_LC:
1124                 cdata.cfg.pict_max_exp_lc =
1125                         mt9p012_get_pict_max_exp_lc();
1126
1127                 if (copy_to_user((void *)argp,
1128                                 &cdata,
1129                                 sizeof(struct sensor_cfg_data)))
1130                         rc = -EFAULT;
1131                 break;
1132
1133         case CFG_SET_FPS:
1134         case CFG_SET_PICT_FPS:
1135                 rc = mt9p012_set_fps(&(cdata.cfg.fps));
1136                 break;
1137
1138         case CFG_SET_EXP_GAIN:
1139                 rc = mt9p012_write_exp_gain(cdata.cfg.exp_gain.gain,
1140                                 cdata.cfg.exp_gain.line);
1141                 break;
1142
1143         case CFG_SET_PICT_EXP_GAIN:
1144                 CDBG("Line:%d CFG_SET_PICT_EXP_GAIN \n", __LINE__);
1145                 rc = mt9p012_set_pict_exp_gain(cdata.cfg.exp_gain.gain,
1146                                 cdata.cfg.exp_gain.line);
1147                 break;
1148
1149         case CFG_SET_MODE:
1150                 rc = mt9p012_set_sensor_mode(cdata.mode, cdata.rs);
1151                 break;
1152
1153         case CFG_PWR_DOWN:
1154                 rc = mt9p012_power_down();
1155                 break;
1156
1157         case CFG_MOVE_FOCUS:
1158                 CDBG("mt9p012_ioctl: CFG_MOVE_FOCUS: cdata.cfg.focus.dir=%d cdata.cfg.focus.steps=%d\n",
1159                                 cdata.cfg.focus.dir, cdata.cfg.focus.steps);
1160                 rc = mt9p012_move_focus(cdata.cfg.focus.dir,
1161                                         cdata.cfg.focus.steps);
1162                 break;
1163
1164         case CFG_SET_DEFAULT_FOCUS:
1165                 rc = mt9p012_set_default_focus();
1166                 break;
1167
1168         case CFG_SET_LENS_SHADING:
1169                 CDBG("%s: CFG_SET_LENS_SHADING\n", __func__);
1170                 rc = mt9p012_lens_shading_enable(cdata.cfg.lens_shading);
1171                 break;
1172
1173         case CFG_GET_AF_MAX_STEPS:
1174                 cdata.max_steps = MT9P012_STEPS_NEAR_TO_CLOSEST_INF;
1175                 if (copy_to_user((void *)argp,
1176                                 &cdata,
1177                                 sizeof(struct sensor_cfg_data)))
1178                         rc = -EFAULT;
1179                 break;
1180
1181         case CFG_SET_EFFECT:
1182         default:
1183                 rc = -EINVAL;
1184                 break;
1185         }
1186
1187         up(&mt9p012_sem);
1188         return rc;
1189 }
1190
1191 int mt9p012_sensor_release(void)
1192 {
1193         int rc = -EBADF;
1194
1195         down(&mt9p012_sem);
1196
1197         mt9p012_power_down();
1198
1199         gpio_direction_output(mt9p012_ctrl->sensordata->sensor_reset,
1200                 0);
1201         gpio_free(mt9p012_ctrl->sensordata->sensor_reset);
1202
1203         gpio_direction_output(mt9p012_ctrl->sensordata->vcm_pwd, 0);
1204         gpio_free(mt9p012_ctrl->sensordata->vcm_pwd);
1205
1206         kfree(mt9p012_ctrl);
1207         mt9p012_ctrl = NULL;
1208
1209         CDBG("mt9p012_release completed\n");
1210
1211         up(&mt9p012_sem);
1212         return rc;
1213 }
1214
1215 static int mt9p012_i2c_probe(struct i2c_client *client,
1216         const struct i2c_device_id *id)
1217 {
1218         int rc = 0;
1219         CDBG("mt9p012_probe called!\n");
1220
1221         if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
1222                 CDBG("i2c_check_functionality failed\n");
1223                 goto probe_failure;
1224         }
1225
1226         mt9p012_sensorw = kzalloc(sizeof(struct mt9p012_work), GFP_KERNEL);
1227         if (!mt9p012_sensorw) {
1228                 CDBG("kzalloc failed.\n");
1229                 rc = -ENOMEM;
1230                 goto probe_failure;
1231         }
1232
1233         i2c_set_clientdata(client, mt9p012_sensorw);
1234         mt9p012_init_client(client);
1235         mt9p012_client = client;
1236
1237         mdelay(50);
1238
1239         CDBG("mt9p012_probe successed! rc = %d\n", rc);
1240         return 0;
1241
1242 probe_failure:
1243         CDBG("mt9p012_probe failed! rc = %d\n", rc);
1244         return rc;
1245 }
1246
1247 static const struct i2c_device_id mt9p012_i2c_id[] = {
1248         { "mt9p012", 0},
1249         { }
1250 };
1251
1252 static struct i2c_driver mt9p012_i2c_driver = {
1253         .id_table = mt9p012_i2c_id,
1254         .probe  = mt9p012_i2c_probe,
1255         .remove = __exit_p(mt9p012_i2c_remove),
1256         .driver = {
1257                 .name = "mt9p012",
1258         },
1259 };
1260
1261 static int mt9p012_sensor_probe(const struct msm_camera_sensor_info *info,
1262                                 struct msm_sensor_ctrl *s)
1263 {
1264         int rc = i2c_add_driver(&mt9p012_i2c_driver);
1265         if (rc < 0 || mt9p012_client == NULL) {
1266                 rc = -ENOTSUPP;
1267                 goto probe_done;
1268         }
1269
1270         msm_camio_clk_rate_set(MT9P012_DEFAULT_CLOCK_RATE);
1271         mdelay(20);
1272
1273         rc = mt9p012_probe_init_sensor(info);
1274         if (rc < 0)
1275                 goto probe_done;
1276
1277         s->s_init = mt9p012_sensor_open_init;
1278         s->s_release = mt9p012_sensor_release;
1279         s->s_config  = mt9p012_sensor_config;
1280         mt9p012_probe_init_done(info);
1281
1282 probe_done:
1283         CDBG("%s %s:%d\n", __FILE__, __func__, __LINE__);
1284         return rc;
1285 }
1286
1287 static int __mt9p012_probe(struct platform_device *pdev)
1288 {
1289         return msm_camera_drv_start(pdev, mt9p012_sensor_probe);
1290 }
1291
1292 static struct platform_driver msm_camera_driver = {
1293         .probe = __mt9p012_probe,
1294         .driver = {
1295                 .name = "msm_camera_mt9p012",
1296                 .owner = THIS_MODULE,
1297         },
1298 };
1299
1300 static int __init mt9p012_init(void)
1301 {
1302         return platform_driver_register(&msm_camera_driver);
1303 }
1304
1305 module_init(mt9p012_init);