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