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