drivers/staging/dream/camera/mt9t013.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 <asm/mach-types.h>
  15 #include "mt9t013.h"
  16
  17 /*=============================================================
  18         SENSOR REGISTER DEFINES
  19 ==============================================================*/
  20 #define MT9T013_REG_MODEL_ID             0x0000
  21 #define MT9T013_MODEL_ID                 0x2600
  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_INT_TIME            0x3014
  42 #define REG_ROW_SPEED                0x3016
  43 #define MT9T013_REG_RESET_REGISTER   0x301A
  44 #define MT9T013_RESET_REGISTER_PWON  0x10CC
  45 #define MT9T013_RESET_REGISTER_PWOFF 0x1008 /* 0x10C8 stop streaming*/
  46 #define REG_READ_MODE                0x3040
  47 #define REG_GLOBAL_GAIN              0x305E
  48 #define REG_TEST_PATTERN_MODE        0x3070
  49
  50
  51 enum mt9t013_test_mode {
  52         TEST_OFF,
  53         TEST_1,
  54         TEST_2,
  55         TEST_3
  56 };
  57
  58 enum mt9t013_resolution {
  59         QTR_SIZE,
  60         FULL_SIZE,
  61         INVALID_SIZE
  62 };
  63
  64 enum mt9t013_reg_update {
  65         REG_INIT, /* registers that need to be updated during initialization */
  66         UPDATE_PERIODIC, /* registers that needs periodic I2C writes */
  67         UPDATE_ALL, /* all registers will be updated */
  68         UPDATE_INVALID
  69 };
  70
  71 enum mt9t013_setting {
  72         RES_PREVIEW,
  73         RES_CAPTURE
  74 };
  75
  76 /* actuator's Slave Address */
  77 #define MT9T013_AF_I2C_ADDR   0x18
  78
  79 /*
  80 * AF Total steps parameters
  81 */
  82 #define MT9T013_TOTAL_STEPS_NEAR_TO_FAR    30
  83
  84 /*
  85  * Time in milisecs for waiting for the sensor to reset.
  86  */
  87 #define MT9T013_RESET_DELAY_MSECS   66
  88
  89 /* for 30 fps preview */
  90 #define MT9T013_DEFAULT_CLOCK_RATE  24000000
  91 #define MT9T013_DEFAULT_MAX_FPS     26
  92
  93
  94 /* FIXME: Changes from here */
  95 struct mt9t013_work {
  96         struct work_struct work;
  97 };
  98
  99 static struct  mt9t013_work *mt9t013_sensorw;
 100 static struct  i2c_client *mt9t013_client;
 101
 102 struct mt9t013_ctrl {
 103         const struct msm_camera_sensor_info *sensordata;
 104
 105         int sensormode;
 106         uint32_t fps_divider;           /* init to 1 * 0x00000400 */
 107         uint32_t pict_fps_divider;      /* init to 1 * 0x00000400 */
 108
 109         uint16_t curr_lens_pos;
 110         uint16_t init_curr_lens_pos;
 111         uint16_t my_reg_gain;
 112         uint32_t my_reg_line_count;
 113
 114         enum mt9t013_resolution prev_res;
 115         enum mt9t013_resolution pict_res;
 116         enum mt9t013_resolution curr_res;
 117         enum mt9t013_test_mode  set_test;
 118
 119         unsigned short imgaddr;
 120 };
 121
 122
 123 static struct mt9t013_ctrl *mt9t013_ctrl;
 124 static DECLARE_WAIT_QUEUE_HEAD(mt9t013_wait_queue);
 125 DECLARE_MUTEX(mt9t013_sem);
 126
 127 extern struct mt9t013_reg mt9t013_regs; /* from mt9t013_reg.c */
 128
 129 static int mt9t013_i2c_rxdata(unsigned short saddr,
 130         unsigned char *rxdata, int length)
 131 {
 132         struct i2c_msg msgs[] = {
 133         {
 134                 .addr   = saddr,
 135                 .flags = 0,
 136                 .len   = 2,
 137                 .buf   = rxdata,
 138         },
 139         {
 140                 .addr  = saddr,
 141                 .flags = I2C_M_RD,
 142                 .len   = length,
 143                 .buf   = rxdata,
 144         },
 145         };
 146
 147         if (i2c_transfer(mt9t013_client->adapter, msgs, 2) < 0) {
 148                 pr_err("mt9t013_i2c_rxdata failed!\n");
 149                 return -EIO;
 150         }
 151
 152         return 0;
 153 }
 154
 155 static int32_t mt9t013_i2c_read_w(unsigned short saddr,
 156         unsigned short raddr, unsigned short *rdata)
 157 {
 158         int32_t rc = 0;
 159         unsigned char buf[4];
 160
 161         if (!rdata)
 162                 return -EIO;
 163
 164         memset(buf, 0, sizeof(buf));
 165
 166         buf[0] = (raddr & 0xFF00)>>8;
 167         buf[1] = (raddr & 0x00FF);
 168
 169         rc = mt9t013_i2c_rxdata(saddr, buf, 2);
 170         if (rc < 0)
 171                 return rc;
 172
 173         *rdata = buf[0] << 8 | buf[1];
 174
 175         if (rc < 0)
 176                 pr_err("mt9t013_i2c_read failed!\n");
 177
 178         return rc;
 179 }
 180
 181 static int32_t mt9t013_i2c_txdata(unsigned short saddr,
 182         unsigned char *txdata, int length)
 183 {
 184         struct i2c_msg msg[] = {
 185         {
 186                 .addr = saddr,
 187                 .flags = 0,
 188                 .len = length,
 189                 .buf = txdata,
 190         },
 191         };
 192
 193         if (i2c_transfer(mt9t013_client->adapter, msg, 1) < 0) {
 194                 pr_err("mt9t013_i2c_txdata failed\n");
 195                 return -EIO;
 196         }
 197
 198         return 0;
 199 }
 200
 201 static int32_t mt9t013_i2c_write_b(unsigned short saddr,
 202         unsigned short waddr, unsigned short wdata)
 203 {
 204         int32_t rc = -EIO;
 205         unsigned char buf[2];
 206
 207         memset(buf, 0, sizeof(buf));
 208         buf[0] = waddr;
 209         buf[1] = wdata;
 210         rc = mt9t013_i2c_txdata(saddr, buf, 2);
 211
 212         if (rc < 0)
 213                 pr_err("i2c_write failed, addr = 0x%x, val = 0x%x!\n",
 214                 waddr, wdata);
 215
 216         return rc;
 217 }
 218
 219 static int32_t mt9t013_i2c_write_w(unsigned short saddr,
 220         unsigned short waddr, unsigned short wdata)
 221 {
 222         int32_t rc = -EIO;
 223         unsigned char buf[4];
 224
 225         memset(buf, 0, sizeof(buf));
 226         buf[0] = (waddr & 0xFF00)>>8;
 227         buf[1] = (waddr & 0x00FF);
 228         buf[2] = (wdata & 0xFF00)>>8;
 229         buf[3] = (wdata & 0x00FF);
 230
 231         rc = mt9t013_i2c_txdata(saddr, buf, 4);
 232
 233         if (rc < 0)
 234                 pr_err("i2c_write_w failed, addr = 0x%x, val = 0x%x!\n",
 235                 waddr, wdata);
 236
 237         return rc;
 238 }
 239
 240 static int32_t mt9t013_i2c_write_w_table(
 241         struct mt9t013_i2c_reg_conf *reg_conf_tbl, int num_of_items_in_table)
 242 {
 243         int i;
 244         int32_t rc = -EIO;
 245
 246         for (i = 0; i < num_of_items_in_table; i++) {
 247                 rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 248                         reg_conf_tbl->waddr, reg_conf_tbl->wdata);
 249                 if (rc < 0)
 250                         break;
 251                 reg_conf_tbl++;
 252         }
 253
 254         return rc;
 255 }
 256
 257 static int32_t mt9t013_test(enum mt9t013_test_mode mo)
 258 {
 259         int32_t rc = 0;
 260
 261         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 262                         REG_GROUPED_PARAMETER_HOLD,
 263                         GROUPED_PARAMETER_HOLD);
 264         if (rc < 0)
 265                 return rc;
 266
 267         if (mo == TEST_OFF)
 268                 return 0;
 269         else {
 270                 rc = mt9t013_i2c_write_w_table(mt9t013_regs.ttbl,
 271                                 mt9t013_regs.ttbl_size);
 272                 if (rc < 0)
 273                         return rc;
 274                 rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 275                                 REG_TEST_PATTERN_MODE, (uint16_t)mo);
 276                 if (rc < 0)
 277                         return rc;
 278         }
 279
 280         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 281                         REG_GROUPED_PARAMETER_HOLD,
 282                         GROUPED_PARAMETER_UPDATE);
 283         if (rc < 0)
 284                 return rc;
 285
 286         return rc;
 287 }
 288
 289 static int32_t mt9t013_set_lc(void)
 290 {
 291         int32_t rc;
 292
 293         rc = mt9t013_i2c_write_w_table(mt9t013_regs.lctbl, mt9t013_regs.lctbl_size);
 294         if (rc < 0)
 295                 return rc;
 296
 297         return rc;
 298 }
 299
 300 static int32_t mt9t013_set_default_focus(uint8_t af_step)
 301 {
 302         int32_t rc = 0;
 303         uint8_t code_val_msb, code_val_lsb;
 304         code_val_msb = 0x01;
 305         code_val_lsb = af_step;
 306
 307         /* Write the digital code for current to the actuator */
 308         rc = mt9t013_i2c_write_b(MT9T013_AF_I2C_ADDR>>1,
 309                         code_val_msb, code_val_lsb);
 310
 311         mt9t013_ctrl->curr_lens_pos = 0;
 312         mt9t013_ctrl->init_curr_lens_pos = 0;
 313         return rc;
 314 }
 315
 316 static void mt9t013_get_pict_fps(uint16_t fps, uint16_t *pfps)
 317 {
 318         /* input fps is preview fps in Q8 format */
 319         uint32_t divider;   /*Q10 */
 320         uint32_t pclk_mult; /*Q10 */
 321
 322         if (mt9t013_ctrl->prev_res == QTR_SIZE) {
 323                 divider =
 324                         (uint32_t)(
 325                 ((mt9t013_regs.reg_pat[RES_PREVIEW].frame_length_lines *
 326                 mt9t013_regs.reg_pat[RES_PREVIEW].line_length_pck) *
 327                 0x00000400) /
 328                 (mt9t013_regs.reg_pat[RES_CAPTURE].frame_length_lines *
 329                 mt9t013_regs.reg_pat[RES_CAPTURE].line_length_pck));
 330
 331                 pclk_mult =
 332                 (uint32_t) ((mt9t013_regs.reg_pat[RES_CAPTURE].pll_multiplier *
 333                 0x00000400) /
 334                 (mt9t013_regs.reg_pat[RES_PREVIEW].pll_multiplier));
 335
 336         } else {
 337                 /* full size resolution used for preview. */
 338                 divider   = 0x00000400;  /*1.0 */
 339                 pclk_mult = 0x00000400;  /*1.0 */
 340         }
 341
 342         /* Verify PCLK settings and frame sizes. */
 343         *pfps =
 344                 (uint16_t) (fps * divider * pclk_mult /
 345                 0x00000400 / 0x00000400);
 346 }
 347
 348 static uint16_t mt9t013_get_prev_lines_pf(void)
 349 {
 350         if (mt9t013_ctrl->prev_res == QTR_SIZE)
 351                 return mt9t013_regs.reg_pat[RES_PREVIEW].frame_length_lines;
 352         else
 353                 return mt9t013_regs.reg_pat[RES_CAPTURE].frame_length_lines;
 354 }
 355
 356 static uint16_t mt9t013_get_prev_pixels_pl(void)
 357 {
 358         if (mt9t013_ctrl->prev_res == QTR_SIZE)
 359                 return mt9t013_regs.reg_pat[RES_PREVIEW].line_length_pck;
 360         else
 361                 return mt9t013_regs.reg_pat[RES_CAPTURE].line_length_pck;
 362 }
 363
 364 static uint16_t mt9t013_get_pict_lines_pf(void)
 365 {
 366         return mt9t013_regs.reg_pat[RES_CAPTURE].frame_length_lines;
 367 }
 368
 369 static uint16_t mt9t013_get_pict_pixels_pl(void)
 370 {
 371         return mt9t013_regs.reg_pat[RES_CAPTURE].line_length_pck;
 372 }
 373
 374 static uint32_t mt9t013_get_pict_max_exp_lc(void)
 375 {
 376         uint16_t snapshot_lines_per_frame;
 377
 378         if (mt9t013_ctrl->pict_res == QTR_SIZE) {
 379                 snapshot_lines_per_frame =
 380                 mt9t013_regs.reg_pat[RES_PREVIEW].frame_length_lines - 1;
 381         } else  {
 382                 snapshot_lines_per_frame =
 383                 mt9t013_regs.reg_pat[RES_CAPTURE].frame_length_lines - 1;
 384         }
 385
 386         return snapshot_lines_per_frame * 24;
 387 }
 388
 389 static int32_t mt9t013_set_fps(struct fps_cfg *fps)
 390 {
 391         /* input is new fps in Q8 format */
 392         int32_t rc = 0;
 393
 394         mt9t013_ctrl->fps_divider = fps->fps_div;
 395         mt9t013_ctrl->pict_fps_divider = fps->pict_fps_div;
 396
 397         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 398                         REG_GROUPED_PARAMETER_HOLD,
 399                         GROUPED_PARAMETER_HOLD);
 400         if (rc < 0)
 401                 return -EBUSY;
 402
 403         CDBG("mt9t013_set_fps: fps_div is %d, frame_rate is %d\n",
 404                         fps->fps_div,
 405                         (uint16_t) (mt9t013_regs.reg_pat[RES_PREVIEW].
 406                                                 frame_length_lines *
 407                                         fps->fps_div/0x00000400));
 408
 409         CDBG("mt9t013_set_fps: fps_mult is %d, frame_rate is %d\n",
 410                         fps->f_mult,
 411                         (uint16_t)(mt9t013_regs.reg_pat[RES_PREVIEW].
 412                                         line_length_pck *
 413                                         fps->f_mult / 0x00000400));
 414
 415         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 416                         REG_LINE_LENGTH_PCK,
 417                         (uint16_t) (
 418                         mt9t013_regs.reg_pat[RES_PREVIEW].line_length_pck *
 419                         fps->f_mult / 0x00000400));
 420         if (rc < 0)
 421                 return rc;
 422
 423         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 424                         REG_GROUPED_PARAMETER_HOLD,
 425                         GROUPED_PARAMETER_UPDATE);
 426         if (rc < 0)
 427                 return rc;
 428
 429         return rc;
 430 }
 431
 432 static int32_t mt9t013_write_exp_gain(uint16_t gain, uint32_t line)
 433 {
 434         const uint16_t max_legal_gain = 0x01FF;
 435         uint32_t line_length_ratio = 0x00000400;
 436         enum mt9t013_setting setting;
 437         int32_t rc = 0;
 438
 439         if (mt9t013_ctrl->sensormode == SENSOR_PREVIEW_MODE) {
 440                 mt9t013_ctrl->my_reg_gain = gain;
 441                 mt9t013_ctrl->my_reg_line_count = (uint16_t) line;
 442         }
 443
 444         if (gain > max_legal_gain)
 445                 gain = max_legal_gain;
 446
 447         /* Verify no overflow */
 448         if (mt9t013_ctrl->sensormode != SENSOR_SNAPSHOT_MODE) {
 449                 line = (uint32_t) (line * mt9t013_ctrl->fps_divider /
 450                         0x00000400);
 451
 452                 setting = RES_PREVIEW;
 453
 454         } else {
 455                 line = (uint32_t) (line * mt9t013_ctrl->pict_fps_divider /
 456                         0x00000400);
 457
 458                 setting = RES_CAPTURE;
 459         }
 460
 461         /*Set digital gain to 1 */
 462         gain |= 0x0200;
 463
 464         if ((mt9t013_regs.reg_pat[setting].frame_length_lines - 1) < line) {
 465
 466                 line_length_ratio =
 467                 (uint32_t) (line * 0x00000400) /
 468                 (mt9t013_regs.reg_pat[setting].frame_length_lines - 1);
 469         } else
 470                 line_length_ratio = 0x00000400;
 471
 472         /* There used to be PARAMETER_HOLD register write before and
 473          * after REG_GLOBAL_GAIN & REG_COARSE_INIT_TIME. This causes
 474          * aec oscillation. Hence removed. */
 475
 476         rc = mt9t013_i2c_write_w(mt9t013_client->addr, REG_GLOBAL_GAIN, gain);
 477         if (rc < 0)
 478                 return rc;
 479
 480         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 481                         REG_COARSE_INT_TIME,
 482                         (uint16_t)((uint32_t) line * 0x00000400 /
 483                         line_length_ratio));
 484         if (rc < 0)
 485                 return rc;
 486
 487         return rc;
 488 }
 489
 490 static int32_t mt9t013_set_pict_exp_gain(uint16_t gain, uint32_t line)
 491 {
 492         int32_t rc = 0;
 493
 494         rc = mt9t013_write_exp_gain(gain, line);
 495         if (rc < 0)
 496                 return rc;
 497
 498         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 499                         MT9T013_REG_RESET_REGISTER,
 500                         0x10CC | 0x0002);
 501
 502         mdelay(5);
 503
 504         /* camera_timed_wait(snapshot_wait*exposure_ratio); */
 505         return rc;
 506 }
 507
 508 static int32_t mt9t013_setting(enum mt9t013_reg_update rupdate,
 509         enum mt9t013_setting rt)
 510 {
 511         int32_t rc = 0;
 512
 513         switch (rupdate) {
 514         case UPDATE_PERIODIC: {
 515
 516         if (rt == RES_PREVIEW || rt == RES_CAPTURE) {
 517 #if 0
 518                 rc =
 519                         mt9t013_i2c_write_w(mt9t013_client->addr,
 520                                 MT9T013_REG_RESET_REGISTER,
 521                                 MT9T013_RESET_REGISTER_PWOFF);
 522                 if (rc < 0)
 523                         return rc;
 524 #endif
 525
 526                 rc =
 527                         mt9t013_i2c_write_w(mt9t013_client->addr,
 528                                 REG_VT_PIX_CLK_DIV,
 529                                 mt9t013_regs.reg_pat[rt].vt_pix_clk_div);
 530                 if (rc < 0)
 531                         return rc;
 532
 533                 rc =
 534                         mt9t013_i2c_write_w(mt9t013_client->addr,
 535                                 REG_VT_SYS_CLK_DIV,
 536                                 mt9t013_regs.reg_pat[rt].vt_sys_clk_div);
 537                 if (rc < 0)
 538                         return rc;
 539
 540                 rc =
 541                         mt9t013_i2c_write_w(mt9t013_client->addr,
 542                                 REG_PRE_PLL_CLK_DIV,
 543                                 mt9t013_regs.reg_pat[rt].pre_pll_clk_div);
 544                 if (rc < 0)
 545                         return rc;
 546
 547                 rc =
 548                         mt9t013_i2c_write_w(mt9t013_client->addr,
 549                                 REG_PLL_MULTIPLIER,
 550                                 mt9t013_regs.reg_pat[rt].pll_multiplier);
 551                 if (rc < 0)
 552                         return rc;
 553
 554                 rc =
 555                         mt9t013_i2c_write_w(mt9t013_client->addr,
 556                                 REG_OP_PIX_CLK_DIV,
 557                                 mt9t013_regs.reg_pat[rt].op_pix_clk_div);
 558                 if (rc < 0)
 559                         return rc;
 560
 561                 rc =
 562                         mt9t013_i2c_write_w(mt9t013_client->addr,
 563                                 REG_OP_SYS_CLK_DIV,
 564                                 mt9t013_regs.reg_pat[rt].op_sys_clk_div);
 565                 if (rc < 0)
 566                         return rc;
 567
 568                 mdelay(5);
 569
 570                 rc =
 571                         mt9t013_i2c_write_w(mt9t013_client->addr,
 572                                 REG_GROUPED_PARAMETER_HOLD,
 573                                 GROUPED_PARAMETER_HOLD);
 574                 if (rc < 0)
 575                         return rc;
 576
 577                 rc =
 578                         mt9t013_i2c_write_w(mt9t013_client->addr,
 579                                 REG_ROW_SPEED,
 580                                 mt9t013_regs.reg_pat[rt].row_speed);
 581                 if (rc < 0)
 582                         return rc;
 583
 584                 rc =
 585                         mt9t013_i2c_write_w(mt9t013_client->addr,
 586                                 REG_X_ADDR_START,
 587                                 mt9t013_regs.reg_pat[rt].x_addr_start);
 588                 if (rc < 0)
 589                         return rc;
 590
 591                 rc =
 592                         mt9t013_i2c_write_w(mt9t013_client->addr,
 593                                 REG_X_ADDR_END,
 594                                 mt9t013_regs.reg_pat[rt].x_addr_end);
 595                 if (rc < 0)
 596                         return rc;
 597
 598                 rc =
 599                         mt9t013_i2c_write_w(mt9t013_client->addr,
 600                                 REG_Y_ADDR_START,
 601                                 mt9t013_regs.reg_pat[rt].y_addr_start);
 602                 if (rc < 0)
 603                         return rc;
 604
 605                 rc =
 606                         mt9t013_i2c_write_w(mt9t013_client->addr,
 607                                 REG_Y_ADDR_END,
 608                                 mt9t013_regs.reg_pat[rt].y_addr_end);
 609                 if (rc < 0)
 610                         return rc;
 611
 612                 if (machine_is_sapphire()) {
 613                         if (rt == 0)
 614                                 rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 615                                         REG_READ_MODE,
 616                                         0x046F);
 617                         else
 618                                 rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 619                                         REG_READ_MODE,
 620                                         0x0027);
 621                 } else
 622                         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 623                                         REG_READ_MODE,
 624                                         mt9t013_regs.reg_pat[rt].read_mode);
 625                 if (rc < 0)
 626                         return rc;
 627
 628                 rc =
 629                         mt9t013_i2c_write_w(mt9t013_client->addr,
 630                                 REG_SCALE_M,
 631                                 mt9t013_regs.reg_pat[rt].scale_m);
 632                 if (rc < 0)
 633                         return rc;
 634
 635
 636                 rc =
 637                         mt9t013_i2c_write_w(mt9t013_client->addr,
 638                                 REG_X_OUTPUT_SIZE,
 639                                 mt9t013_regs.reg_pat[rt].x_output_size);
 640                 if (rc < 0)
 641                         return rc;
 642
 643                 rc =
 644                         mt9t013_i2c_write_w(mt9t013_client->addr,
 645                                 REG_Y_OUTPUT_SIZE,
 646                                 mt9t013_regs.reg_pat[rt].y_output_size);
 647                 if (rc < 0)
 648                         return rc;
 649
 650                 rc =
 651                         mt9t013_i2c_write_w(mt9t013_client->addr,
 652                                 REG_LINE_LENGTH_PCK,
 653                                 mt9t013_regs.reg_pat[rt].line_length_pck);
 654                 if (rc < 0)
 655                         return rc;
 656
 657                 rc =
 658                         mt9t013_i2c_write_w(mt9t013_client->addr,
 659                         REG_FRAME_LENGTH_LINES,
 660                         (mt9t013_regs.reg_pat[rt].frame_length_lines *
 661                         mt9t013_ctrl->fps_divider / 0x00000400));
 662                 if (rc < 0)
 663                         return rc;
 664
 665                 rc =
 666                         mt9t013_i2c_write_w(mt9t013_client->addr,
 667                         REG_COARSE_INT_TIME,
 668                         mt9t013_regs.reg_pat[rt].coarse_int_time);
 669                 if (rc < 0)
 670                         return rc;
 671
 672                 rc =
 673                         mt9t013_i2c_write_w(mt9t013_client->addr,
 674                         REG_FINE_INT_TIME,
 675                         mt9t013_regs.reg_pat[rt].fine_int_time);
 676                 if (rc < 0)
 677                         return rc;
 678
 679                 rc =
 680                         mt9t013_i2c_write_w(mt9t013_client->addr,
 681                         REG_GROUPED_PARAMETER_HOLD,
 682                         GROUPED_PARAMETER_UPDATE);
 683                 if (rc < 0)
 684                         return rc;
 685
 686                 rc = mt9t013_test(mt9t013_ctrl->set_test);
 687                 if (rc < 0)
 688                         return rc;
 689
 690                 rc =
 691                         mt9t013_i2c_write_w(mt9t013_client->addr,
 692                         MT9T013_REG_RESET_REGISTER,
 693                         MT9T013_RESET_REGISTER_PWON);
 694                 if (rc < 0)
 695                         return rc;
 696
 697                 mdelay(5);
 698
 699                 return rc;
 700         }
 701         }
 702                 break;
 703
 704         /*CAMSENSOR_REG_UPDATE_PERIODIC */
 705         case REG_INIT: {
 706         if (rt == RES_PREVIEW || rt == RES_CAPTURE) {
 707
 708                 rc =
 709                         mt9t013_i2c_write_w(mt9t013_client->addr,
 710                                 MT9T013_REG_RESET_REGISTER,
 711                                 MT9T013_RESET_REGISTER_PWOFF);
 712                 if (rc < 0)
 713                         /* MODE_SELECT, stop streaming */
 714                         return rc;
 715
 716                 rc =
 717                         mt9t013_i2c_write_w(mt9t013_client->addr,
 718                                 REG_VT_PIX_CLK_DIV,
 719                                 mt9t013_regs.reg_pat[rt].vt_pix_clk_div);
 720                 if (rc < 0)
 721                         return rc;
 722
 723                 rc =
 724                         mt9t013_i2c_write_w(mt9t013_client->addr,
 725                                 REG_VT_SYS_CLK_DIV,
 726                                 mt9t013_regs.reg_pat[rt].vt_sys_clk_div);
 727                 if (rc < 0)
 728                         return rc;
 729
 730                 rc =
 731                         mt9t013_i2c_write_w(mt9t013_client->addr,
 732                                 REG_PRE_PLL_CLK_DIV,
 733                                 mt9t013_regs.reg_pat[rt].pre_pll_clk_div);
 734                 if (rc < 0)
 735                         return rc;
 736
 737                 rc =
 738                         mt9t013_i2c_write_w(mt9t013_client->addr,
 739                                 REG_PLL_MULTIPLIER,
 740                                 mt9t013_regs.reg_pat[rt].pll_multiplier);
 741                 if (rc < 0)
 742                         return rc;
 743
 744                 rc =
 745                         mt9t013_i2c_write_w(mt9t013_client->addr,
 746                                 REG_OP_PIX_CLK_DIV,
 747                                 mt9t013_regs.reg_pat[rt].op_pix_clk_div);
 748                 if (rc < 0)
 749                         return rc;
 750
 751                 rc =
 752                         mt9t013_i2c_write_w(mt9t013_client->addr,
 753                                 REG_OP_SYS_CLK_DIV,
 754                                 mt9t013_regs.reg_pat[rt].op_sys_clk_div);
 755                 if (rc < 0)
 756                         return rc;
 757
 758                 mdelay(5);
 759
 760                 rc =
 761                         mt9t013_i2c_write_w(mt9t013_client->addr,
 762                                 REG_GROUPED_PARAMETER_HOLD,
 763                                 GROUPED_PARAMETER_HOLD);
 764                 if (rc < 0)
 765                         return rc;
 766
 767                 /* additional power saving mode ok around 38.2MHz */
 768                 rc =
 769                         mt9t013_i2c_write_w(mt9t013_client->addr,
 770                                 0x3084, 0x2409);
 771                 if (rc < 0)
 772                         return rc;
 773
 774                 rc =
 775                         mt9t013_i2c_write_w(mt9t013_client->addr,
 776                                 0x3092, 0x0A49);
 777                 if (rc < 0)
 778                         return rc;
 779
 780                 rc =
 781                         mt9t013_i2c_write_w(mt9t013_client->addr,
 782                                 0x3094, 0x4949);
 783                 if (rc < 0)
 784                         return rc;
 785
 786                 rc =
 787                         mt9t013_i2c_write_w(mt9t013_client->addr,
 788                                 0x3096, 0x4949);
 789                 if (rc < 0)
 790                         return rc;
 791
 792                 /* Set preview or snapshot mode */
 793                 rc =
 794                         mt9t013_i2c_write_w(mt9t013_client->addr,
 795                                 REG_ROW_SPEED,
 796                                 mt9t013_regs.reg_pat[rt].row_speed);
 797                 if (rc < 0)
 798                         return rc;
 799
 800                 rc =
 801                         mt9t013_i2c_write_w(mt9t013_client->addr,
 802                                 REG_X_ADDR_START,
 803                                 mt9t013_regs.reg_pat[rt].x_addr_start);
 804                 if (rc < 0)
 805                         return rc;
 806
 807                 rc =
 808                         mt9t013_i2c_write_w(mt9t013_client->addr,
 809                                 REG_X_ADDR_END,
 810                                 mt9t013_regs.reg_pat[rt].x_addr_end);
 811                 if (rc < 0)
 812                         return rc;
 813
 814                 rc =
 815                         mt9t013_i2c_write_w(mt9t013_client->addr,
 816                                 REG_Y_ADDR_START,
 817                                 mt9t013_regs.reg_pat[rt].y_addr_start);
 818                 if (rc < 0)
 819                         return rc;
 820
 821                 rc =
 822                         mt9t013_i2c_write_w(mt9t013_client->addr,
 823                                 REG_Y_ADDR_END,
 824                                 mt9t013_regs.reg_pat[rt].y_addr_end);
 825                 if (rc < 0)
 826                         return rc;
 827
 828                 if (machine_is_sapphire()) {
 829                         if (rt == 0)
 830                                 rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 831                                         REG_READ_MODE,
 832                                         0x046F);
 833                         else
 834                                 rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 835                                         REG_READ_MODE,
 836                                         0x0027);
 837                 } else
 838                         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
 839                                         REG_READ_MODE,
 840                                         mt9t013_regs.reg_pat[rt].read_mode);
 841                 if (rc < 0)
 842                         return rc;
 843
 844                 rc =
 845                         mt9t013_i2c_write_w(mt9t013_client->addr,
 846                                 REG_SCALE_M,
 847                                 mt9t013_regs.reg_pat[rt].scale_m);
 848                 if (rc < 0)
 849                         return rc;
 850
 851                 rc =
 852                         mt9t013_i2c_write_w(mt9t013_client->addr,
 853                                 REG_X_OUTPUT_SIZE,
 854                                 mt9t013_regs.reg_pat[rt].x_output_size);
 855                 if (rc < 0)
 856                         return rc;
 857
 858                 rc =
 859                         mt9t013_i2c_write_w(mt9t013_client->addr,
 860                                 REG_Y_OUTPUT_SIZE,
 861                                 mt9t013_regs.reg_pat[rt].y_output_size);
 862                 if (rc < 0)
 863                         return 0;
 864
 865                 rc =
 866                         mt9t013_i2c_write_w(mt9t013_client->addr,
 867                                 REG_LINE_LENGTH_PCK,
 868                                 mt9t013_regs.reg_pat[rt].line_length_pck);
 869                 if (rc < 0)
 870                         return rc;
 871
 872                 rc =
 873                         mt9t013_i2c_write_w(mt9t013_client->addr,
 874                                 REG_FRAME_LENGTH_LINES,
 875                                 mt9t013_regs.reg_pat[rt].frame_length_lines);
 876                 if (rc < 0)
 877                         return rc;
 878
 879                 rc =
 880                         mt9t013_i2c_write_w(mt9t013_client->addr,
 881                                 REG_COARSE_INT_TIME,
 882                                 mt9t013_regs.reg_pat[rt].coarse_int_time);
 883                 if (rc < 0)
 884                         return rc;
 885
 886                 rc =
 887                         mt9t013_i2c_write_w(mt9t013_client->addr,
 888                                 REG_FINE_INT_TIME,
 889                                 mt9t013_regs.reg_pat[rt].fine_int_time);
 890                 if (rc < 0)
 891                         return rc;
 892
 893                 rc =
 894                         mt9t013_i2c_write_w(mt9t013_client->addr,
 895                                 REG_GROUPED_PARAMETER_HOLD,
 896                                 GROUPED_PARAMETER_UPDATE);
 897                         if (rc < 0)
 898                                 return rc;
 899
 900                 /* load lens shading */
 901                 rc =
 902                         mt9t013_i2c_write_w(mt9t013_client->addr,
 903                                 REG_GROUPED_PARAMETER_HOLD,
 904                                 GROUPED_PARAMETER_HOLD);
 905                 if (rc < 0)
 906                         return rc;
 907
 908                 /* most likely needs to be written only once. */
 909                 rc = mt9t013_set_lc();
 910                 if (rc < 0)
 911                         return -EBUSY;
 912
 913                 rc =
 914                         mt9t013_i2c_write_w(mt9t013_client->addr,
 915                                 REG_GROUPED_PARAMETER_HOLD,
 916                                 GROUPED_PARAMETER_UPDATE);
 917                 if (rc < 0)
 918                         return rc;
 919
 920                 rc = mt9t013_test(mt9t013_ctrl->set_test);
 921                 if (rc < 0)
 922                         return rc;
 923
 924                 mdelay(5);
 925
 926                 rc =
 927                         mt9t013_i2c_write_w(mt9t013_client->addr,
 928                                 MT9T013_REG_RESET_REGISTER,
 929                                 MT9T013_RESET_REGISTER_PWON);
 930                 if (rc < 0)
 931                         /* MODE_SELECT, stop streaming */
 932                         return rc;
 933
 934                 CDBG("!!! mt9t013 !!! PowerOn is done!\n");
 935                 mdelay(5);
 936                 return rc;
 937                 }
 938         } /* case CAMSENSOR_REG_INIT: */
 939         break;
 940
 941         /*CAMSENSOR_REG_INIT */
 942         default:
 943                 rc = -EINVAL;
 944                 break;
 945         } /* switch (rupdate) */
 946
 947         return rc;
 948 }
 949
 950 static int32_t mt9t013_video_config(int mode, int res)
 951 {
 952         int32_t rc;
 953
 954         switch (res) {
 955         case QTR_SIZE:
 956                 rc = mt9t013_setting(UPDATE_PERIODIC, RES_PREVIEW);
 957                 if (rc < 0)
 958                         return rc;
 959                 CDBG("sensor configuration done!\n");
 960                 break;
 961
 962         case FULL_SIZE:
 963                 rc = mt9t013_setting(UPDATE_PERIODIC, RES_CAPTURE);
 964                 if (rc < 0)
 965                         return rc;
 966                 break;
 967
 968         default:
 969                 return -EINVAL;
 970         } /* switch */
 971
 972         mt9t013_ctrl->prev_res = res;
 973         mt9t013_ctrl->curr_res = res;
 974         mt9t013_ctrl->sensormode = mode;
 975
 976         return mt9t013_write_exp_gain(mt9t013_ctrl->my_reg_gain,
 977                         mt9t013_ctrl->my_reg_line_count);
 978 }
 979
 980 static int32_t mt9t013_snapshot_config(int mode)
 981 {
 982         int32_t rc = 0;
 983
 984         rc = mt9t013_setting(UPDATE_PERIODIC, RES_CAPTURE);
 985         if (rc < 0)
 986                 return rc;
 987
 988         mt9t013_ctrl->curr_res = mt9t013_ctrl->pict_res;
 989         mt9t013_ctrl->sensormode = mode;
 990         return rc;
 991 }
 992
 993 static int32_t mt9t013_raw_snapshot_config(int mode)
 994 {
 995         int32_t rc = 0;
 996
 997         rc = mt9t013_setting(UPDATE_PERIODIC, RES_CAPTURE);
 998         if (rc < 0)
 999                 return rc;
1000
1001         mt9t013_ctrl->curr_res = mt9t013_ctrl->pict_res;
1002         mt9t013_ctrl->sensormode = mode;
1003         return rc;
1004 }
1005
1006 static int32_t mt9t013_power_down(void)
1007 {
1008         int32_t rc = 0;
1009
1010         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
1011                         MT9T013_REG_RESET_REGISTER,
1012                         MT9T013_RESET_REGISTER_PWOFF);
1013         if (rc >= 0)
1014                 mdelay(5);
1015         return rc;
1016 }
1017
1018 static int32_t mt9t013_move_focus(int direction, int32_t num_steps)
1019 {
1020         int16_t step_direction;
1021         int16_t actual_step;
1022         int16_t next_position;
1023         int16_t break_steps[4];
1024         uint8_t code_val_msb, code_val_lsb;
1025         int16_t i;
1026
1027         if (num_steps > MT9T013_TOTAL_STEPS_NEAR_TO_FAR)
1028                 num_steps = MT9T013_TOTAL_STEPS_NEAR_TO_FAR;
1029         else if (num_steps == 0)
1030                 return -EINVAL;
1031
1032         if (direction == MOVE_NEAR)
1033                 step_direction = 4;
1034         else if (direction == MOVE_FAR)
1035                 step_direction = -4;
1036         else
1037                 return -EINVAL;
1038
1039         if (mt9t013_ctrl->curr_lens_pos < mt9t013_ctrl->init_curr_lens_pos)
1040                 mt9t013_ctrl->curr_lens_pos = mt9t013_ctrl->init_curr_lens_pos;
1041
1042         actual_step =
1043                 (int16_t) (step_direction *
1044                 (int16_t) num_steps);
1045
1046         for (i = 0; i < 4; i++)
1047                 break_steps[i] =
1048                         actual_step / 4 * (i + 1) - actual_step / 4 * i;
1049
1050         for (i = 0; i < 4; i++) {
1051                 next_position =
1052                 (int16_t)
1053                 (mt9t013_ctrl->curr_lens_pos + break_steps[i]);
1054
1055                 if (next_position > 255)
1056                         next_position = 255;
1057                 else if (next_position < 0)
1058                         next_position = 0;
1059
1060                 code_val_msb =
1061                 ((next_position >> 4) << 2) |
1062                 ((next_position << 4) >> 6);
1063
1064                 code_val_lsb =
1065                 ((next_position & 0x03) << 6);
1066
1067                 /* Writing the digital code for current to the actuator */
1068                 if (mt9t013_i2c_write_b(MT9T013_AF_I2C_ADDR>>1,
1069                                 code_val_msb, code_val_lsb) < 0)
1070                         return -EBUSY;
1071
1072                 /* Storing the current lens Position */
1073                 mt9t013_ctrl->curr_lens_pos = next_position;
1074
1075                 if (i < 3)
1076                         mdelay(1);
1077         } /* for */
1078
1079         return 0;
1080 }
1081
1082 static int mt9t013_sensor_init_done(const struct msm_camera_sensor_info *data)
1083 {
1084         gpio_direction_output(data->sensor_reset, 0);
1085         gpio_free(data->sensor_reset);
1086         return 0;
1087 }
1088
1089 static int mt9t013_probe_init_sensor(const struct msm_camera_sensor_info *data)
1090 {
1091         int rc;
1092         uint16_t chipid;
1093
1094         rc = gpio_request(data->sensor_reset, "mt9t013");
1095         if (!rc)
1096                 gpio_direction_output(data->sensor_reset, 1);
1097         else
1098                 goto init_probe_done;
1099
1100         mdelay(20);
1101
1102         /* RESET the sensor image part via I2C command */
1103         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
1104                 MT9T013_REG_RESET_REGISTER, 0x1009);
1105         if (rc < 0)
1106                 goto init_probe_fail;
1107
1108         /* 3. Read sensor Model ID: */
1109         rc = mt9t013_i2c_read_w(mt9t013_client->addr,
1110                 MT9T013_REG_MODEL_ID, &chipid);
1111
1112         if (rc < 0)
1113                 goto init_probe_fail;
1114
1115         CDBG("mt9t013 model_id = 0x%x\n", chipid);
1116
1117         /* 4. Compare sensor ID to MT9T012VC ID: */
1118         if (chipid != MT9T013_MODEL_ID) {
1119                 rc = -ENODEV;
1120                 goto init_probe_fail;
1121         }
1122
1123         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
1124                 0x3064, 0x0805);
1125         if (rc < 0)
1126                 goto init_probe_fail;
1127
1128         mdelay(MT9T013_RESET_DELAY_MSECS);
1129
1130         goto init_probe_done;
1131
1132         /* sensor: output enable */
1133 #if 0
1134         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
1135                 MT9T013_REG_RESET_REGISTER,
1136                 MT9T013_RESET_REGISTER_PWON);
1137
1138         /* if this fails, the sensor is not the MT9T013 */
1139         rc = mt9t013_set_default_focus(0);
1140 #endif
1141
1142 init_probe_fail:
1143         gpio_direction_output(data->sensor_reset, 0);
1144         gpio_free(data->sensor_reset);
1145 init_probe_done:
1146         return rc;
1147 }
1148
1149 static int32_t mt9t013_poweron_af(void)
1150 {
1151         int32_t rc = 0;
1152
1153         /* enable AF actuator */
1154         CDBG("enable AF actuator, gpio = %d\n",
1155                         mt9t013_ctrl->sensordata->vcm_pwd);
1156         rc = gpio_request(mt9t013_ctrl->sensordata->vcm_pwd, "mt9t013");
1157         if (!rc) {
1158                 gpio_direction_output(mt9t013_ctrl->sensordata->vcm_pwd, 0);
1159                 mdelay(20);
1160                 rc = mt9t013_set_default_focus(0);
1161         } else
1162                 pr_err("%s, gpio_request failed (%d)!\n", __func__, rc);
1163         return rc;
1164 }
1165
1166 static void mt9t013_poweroff_af(void)
1167 {
1168         gpio_direction_output(mt9t013_ctrl->sensordata->vcm_pwd, 1);
1169         gpio_free(mt9t013_ctrl->sensordata->vcm_pwd);
1170 }
1171
1172 int mt9t013_sensor_open_init(const struct msm_camera_sensor_info *data)
1173 {
1174         int32_t  rc;
1175
1176         mt9t013_ctrl = kzalloc(sizeof(struct mt9t013_ctrl), GFP_KERNEL);
1177         if (!mt9t013_ctrl) {
1178                 pr_err("mt9t013_init failed!\n");
1179                 rc = -ENOMEM;
1180                 goto init_done;
1181         }
1182
1183         mt9t013_ctrl->fps_divider = 1 * 0x00000400;
1184         mt9t013_ctrl->pict_fps_divider = 1 * 0x00000400;
1185         mt9t013_ctrl->set_test = TEST_OFF;
1186         mt9t013_ctrl->prev_res = QTR_SIZE;
1187         mt9t013_ctrl->pict_res = FULL_SIZE;
1188
1189         if (data)
1190                 mt9t013_ctrl->sensordata = data;
1191
1192         /* enable mclk first */
1193         msm_camio_clk_rate_set(MT9T013_DEFAULT_CLOCK_RATE);
1194         mdelay(20);
1195
1196         msm_camio_camif_pad_reg_reset();
1197         mdelay(20);
1198
1199         rc = mt9t013_probe_init_sensor(data);
1200         if (rc < 0)
1201                 goto init_fail;
1202
1203         if (mt9t013_ctrl->prev_res == QTR_SIZE)
1204                 rc = mt9t013_setting(REG_INIT, RES_PREVIEW);
1205         else
1206                 rc = mt9t013_setting(REG_INIT, RES_CAPTURE);
1207
1208         if (rc >= 0)
1209                 rc = mt9t013_poweron_af();
1210
1211         if (rc < 0)
1212                 goto init_fail;
1213         else
1214                 goto init_done;
1215
1216 init_fail:
1217         kfree(mt9t013_ctrl);
1218 init_done:
1219         return rc;
1220 }
1221
1222 static int mt9t013_init_client(struct i2c_client *client)
1223 {
1224         /* Initialize the MSM_CAMI2C Chip */
1225         init_waitqueue_head(&mt9t013_wait_queue);
1226         return 0;
1227 }
1228
1229
1230 static int32_t mt9t013_set_sensor_mode(int mode, int res)
1231 {
1232         int32_t rc = 0;
1233         rc = mt9t013_i2c_write_w(mt9t013_client->addr,
1234                         REG_GROUPED_PARAMETER_HOLD,
1235                         GROUPED_PARAMETER_HOLD);
1236         if (rc < 0)
1237                 return rc;
1238
1239         switch (mode) {
1240         case SENSOR_PREVIEW_MODE:
1241                 rc = mt9t013_video_config(mode, res);
1242                 break;
1243
1244         case SENSOR_SNAPSHOT_MODE:
1245                 rc = mt9t013_snapshot_config(mode);
1246                 break;
1247
1248         case SENSOR_RAW_SNAPSHOT_MODE:
1249                 rc = mt9t013_raw_snapshot_config(mode);
1250                 break;
1251
1252         default:
1253                 return -EINVAL;
1254         }
1255
1256         /* FIXME: what should we do if rc < 0? */
1257         if (rc >= 0)
1258                 return mt9t013_i2c_write_w(mt9t013_client->addr,
1259                                 REG_GROUPED_PARAMETER_HOLD,
1260                                 GROUPED_PARAMETER_UPDATE);
1261         return rc;
1262 }
1263
1264 int mt9t013_sensor_config(void __user *argp)
1265 {
1266         struct sensor_cfg_data cdata;
1267         long   rc = 0;
1268
1269         if (copy_from_user(&cdata, (void *)argp,
1270                         sizeof(struct sensor_cfg_data)))
1271                 return -EFAULT;
1272
1273         down(&mt9t013_sem);
1274
1275         CDBG("mt9t013_sensor_config: cfgtype = %d\n", cdata.cfgtype);
1276         switch (cdata.cfgtype) {
1277         case CFG_GET_PICT_FPS:
1278                 mt9t013_get_pict_fps(cdata.cfg.gfps.prevfps,
1279                                 &(cdata.cfg.gfps.pictfps));
1280                 if (copy_to_user((void *)argp,
1281                                 &cdata,
1282                                 sizeof(struct sensor_cfg_data)))
1283                         rc = -EFAULT;
1284                 break;
1285
1286         case CFG_GET_PREV_L_PF:
1287                 cdata.cfg.prevl_pf = mt9t013_get_prev_lines_pf();
1288                 if (copy_to_user((void *)argp,
1289                                 &cdata,
1290                                 sizeof(struct sensor_cfg_data)))
1291                         rc = -EFAULT;
1292                 break;
1293
1294         case CFG_GET_PREV_P_PL:
1295                 cdata.cfg.prevp_pl = mt9t013_get_prev_pixels_pl();
1296                 if (copy_to_user((void *)argp,
1297                                 &cdata,
1298                                 sizeof(struct sensor_cfg_data)))
1299                         rc = -EFAULT;
1300                 break;
1301
1302         case CFG_GET_PICT_L_PF:
1303                 cdata.cfg.pictl_pf = mt9t013_get_pict_lines_pf();
1304                 if (copy_to_user((void *)argp,
1305                                 &cdata,
1306                                 sizeof(struct sensor_cfg_data)))
1307                         rc = -EFAULT;
1308                 break;
1309
1310         case CFG_GET_PICT_P_PL:
1311                 cdata.cfg.pictp_pl =
1312                         mt9t013_get_pict_pixels_pl();
1313
1314                 if (copy_to_user((void *)argp,
1315                                 &cdata,
1316                                 sizeof(struct sensor_cfg_data)))
1317                         rc = -EFAULT;
1318                 break;
1319
1320         case CFG_GET_PICT_MAX_EXP_LC:
1321                 cdata.cfg.pict_max_exp_lc =
1322                         mt9t013_get_pict_max_exp_lc();
1323
1324                 if (copy_to_user((void *)argp,
1325                                 &cdata,
1326                                 sizeof(struct sensor_cfg_data)))
1327                         rc = -EFAULT;
1328                 break;
1329
1330         case CFG_SET_FPS:
1331         case CFG_SET_PICT_FPS:
1332                 rc = mt9t013_set_fps(&(cdata.cfg.fps));
1333                 break;
1334
1335         case CFG_SET_EXP_GAIN:
1336                 rc = mt9t013_write_exp_gain(cdata.cfg.exp_gain.gain,
1337                                 cdata.cfg.exp_gain.line);
1338                 break;
1339
1340         case CFG_SET_PICT_EXP_GAIN:
1341                 rc = mt9t013_set_pict_exp_gain(cdata.cfg.exp_gain.gain,
1342                                 cdata.cfg.exp_gain.line);
1343                 break;
1344
1345         case CFG_SET_MODE:
1346                 rc = mt9t013_set_sensor_mode(cdata.mode, cdata.rs);
1347                 break;
1348
1349         case CFG_PWR_DOWN:
1350                 rc = mt9t013_power_down();
1351                 break;
1352
1353         case CFG_MOVE_FOCUS:
1354                 rc = mt9t013_move_focus(cdata.cfg.focus.dir,
1355                                 cdata.cfg.focus.steps);
1356                 break;
1357
1358         case CFG_SET_DEFAULT_FOCUS:
1359                 rc = mt9t013_set_default_focus(cdata.cfg.focus.steps);
1360                 break;
1361
1362         case CFG_GET_AF_MAX_STEPS:
1363                 cdata.max_steps = MT9T013_TOTAL_STEPS_NEAR_TO_FAR;
1364                 if (copy_to_user((void *)argp,
1365                                 &cdata,
1366                                 sizeof(struct sensor_cfg_data)))
1367                         rc = -EFAULT;
1368                 break;
1369
1370         case CFG_SET_EFFECT:
1371         default:
1372                 rc = -EINVAL;
1373                 break;
1374         }
1375
1376         up(&mt9t013_sem);
1377         return rc;
1378 }
1379
1380 int mt9t013_sensor_release(void)
1381 {
1382         int rc = -EBADF;
1383
1384         down(&mt9t013_sem);
1385
1386         mt9t013_poweroff_af();
1387         mt9t013_power_down();
1388
1389         gpio_direction_output(mt9t013_ctrl->sensordata->sensor_reset,
1390                         0);
1391         gpio_free(mt9t013_ctrl->sensordata->sensor_reset);
1392
1393         kfree(mt9t013_ctrl);
1394
1395         up(&mt9t013_sem);
1396         CDBG("mt9t013_release completed!\n");
1397         return rc;
1398 }
1399
1400 static int mt9t013_i2c_probe(struct i2c_client *client,
1401         const struct i2c_device_id *id)
1402 {
1403         int rc = 0;
1404         if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
1405                 rc = -ENOTSUPP;
1406                 goto probe_failure;
1407         }
1408
1409         mt9t013_sensorw =
1410                 kzalloc(sizeof(struct mt9t013_work), GFP_KERNEL);
1411
1412         if (!mt9t013_sensorw) {
1413                 rc = -ENOMEM;
1414                 goto probe_failure;
1415         }
1416
1417         i2c_set_clientdata(client, mt9t013_sensorw);
1418         mt9t013_init_client(client);
1419         mt9t013_client = client;
1420         mt9t013_client->addr = mt9t013_client->addr >> 1;
1421         mdelay(50);
1422
1423         CDBG("i2c probe ok\n");
1424         return 0;
1425
1426 probe_failure:
1427         kfree(mt9t013_sensorw);
1428         mt9t013_sensorw = NULL;
1429         pr_err("i2c probe failure %d\n", rc);
1430         return rc;
1431 }
1432
1433 static const struct i2c_device_id mt9t013_i2c_id[] = {
1434         { "mt9t013", 0},
1435         { }
1436 };
1437
1438 static struct i2c_driver mt9t013_i2c_driver = {
1439         .id_table = mt9t013_i2c_id,
1440         .probe  = mt9t013_i2c_probe,
1441         .remove = __exit_p(mt9t013_i2c_remove),
1442         .driver = {
1443                 .name = "mt9t013",
1444         },
1445 };
1446
1447 static int mt9t013_sensor_probe(
1448                 const struct msm_camera_sensor_info *info,
1449                 struct msm_sensor_ctrl *s)
1450 {
1451         /* We expect this driver to match with the i2c device registered
1452          * in the board file immediately. */
1453         int rc = i2c_add_driver(&mt9t013_i2c_driver);
1454         if (rc < 0 || mt9t013_client == NULL) {
1455                 rc = -ENOTSUPP;
1456                 goto probe_done;
1457         }
1458
1459         /* enable mclk first */
1460         msm_camio_clk_rate_set(MT9T013_DEFAULT_CLOCK_RATE);
1461         mdelay(20);
1462
1463         rc = mt9t013_probe_init_sensor(info);
1464         if (rc < 0) {
1465                 i2c_del_driver(&mt9t013_i2c_driver);
1466                 goto probe_done;
1467         }
1468
1469         s->s_init = mt9t013_sensor_open_init;
1470         s->s_release = mt9t013_sensor_release;
1471         s->s_config  = mt9t013_sensor_config;
1472         mt9t013_sensor_init_done(info);
1473
1474 probe_done:
1475         return rc;
1476 }
1477
1478 static int __mt9t013_probe(struct platform_device *pdev)
1479 {
1480         return msm_camera_drv_start(pdev, mt9t013_sensor_probe);
1481 }
1482
1483 static struct platform_driver msm_camera_driver = {
1484         .probe = __mt9t013_probe,
1485         .driver = {
1486                 .name = "msm_camera_mt9t013",
1487                 .owner = THIS_MODULE,
1488         },
1489 };
1490
1491 static int __init mt9t013_init(void)
1492 {
1493         return platform_driver_register(&msm_camera_driver);
1494 }
1495
1496 module_init(mt9t013_init);