git.oblomov.eu Git - linux-2.6/blob - drivers/media/video/mt9t031.c

   1 /*
   2  * Driver for MT9T031 CMOS Image Sensor from Micron
   3  *
   4  * Copyright (C) 2008, Guennadi Liakhovetski, DENX Software Engineering <lg@denx.de>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License version 2 as
   8  * published by the Free Software Foundation.
   9  */
  10
  11 #include <linux/videodev2.h>
  12 #include <linux/slab.h>
  13 #include <linux/i2c.h>
  14 #include <linux/log2.h>
  15
  16 #include <media/v4l2-common.h>
  17 #include <media/v4l2-chip-ident.h>
  18 #include <media/soc_camera.h>
  19
  20 /* mt9t031 i2c address 0x5d
  21  * The platform has to define i2c_board_info
  22  * and call i2c_register_board_info() */
  23
  24 /* mt9t031 selected register addresses */
  25 #define MT9T031_CHIP_VERSION            0x00
  26 #define MT9T031_ROW_START               0x01
  27 #define MT9T031_COLUMN_START            0x02
  28 #define MT9T031_WINDOW_HEIGHT           0x03
  29 #define MT9T031_WINDOW_WIDTH            0x04
  30 #define MT9T031_HORIZONTAL_BLANKING     0x05
  31 #define MT9T031_VERTICAL_BLANKING       0x06
  32 #define MT9T031_OUTPUT_CONTROL          0x07
  33 #define MT9T031_SHUTTER_WIDTH_UPPER     0x08
  34 #define MT9T031_SHUTTER_WIDTH           0x09
  35 #define MT9T031_PIXEL_CLOCK_CONTROL     0x0a
  36 #define MT9T031_FRAME_RESTART           0x0b
  37 #define MT9T031_SHUTTER_DELAY           0x0c
  38 #define MT9T031_RESET                   0x0d
  39 #define MT9T031_READ_MODE_1             0x1e
  40 #define MT9T031_READ_MODE_2             0x20
  41 #define MT9T031_READ_MODE_3             0x21
  42 #define MT9T031_ROW_ADDRESS_MODE        0x22
  43 #define MT9T031_COLUMN_ADDRESS_MODE     0x23
  44 #define MT9T031_GLOBAL_GAIN             0x35
  45 #define MT9T031_CHIP_ENABLE             0xF8
  46
  47 #define MT9T031_MAX_HEIGHT              1536
  48 #define MT9T031_MAX_WIDTH               2048
  49 #define MT9T031_MIN_HEIGHT              2
  50 #define MT9T031_MIN_WIDTH               2
  51 #define MT9T031_HORIZONTAL_BLANK        142
  52 #define MT9T031_VERTICAL_BLANK          25
  53 #define MT9T031_COLUMN_SKIP             32
  54 #define MT9T031_ROW_SKIP                20
  55
  56 #define MT9T031_BUS_PARAM       (SOCAM_PCLK_SAMPLE_RISING |     \
  57         SOCAM_PCLK_SAMPLE_FALLING | SOCAM_HSYNC_ACTIVE_HIGH |   \
  58         SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_DATA_ACTIVE_HIGH |      \
  59         SOCAM_MASTER | SOCAM_DATAWIDTH_10)
  60
  61 static const struct soc_camera_data_format mt9t031_colour_formats[] = {
  62         {
  63                 .name           = "Bayer (sRGB) 10 bit",
  64                 .depth          = 10,
  65                 .fourcc         = V4L2_PIX_FMT_SGRBG10,
  66                 .colorspace     = V4L2_COLORSPACE_SRGB,
  67         }
  68 };
  69
  70 struct mt9t031 {
  71         struct i2c_client *client;
  72         struct soc_camera_device icd;
  73         int model;      /* V4L2_IDENT_MT9T031* codes from v4l2-chip-ident.h */
  74         unsigned char autoexposure;
  75         u16 xskip;
  76         u16 yskip;
  77 };
  78
  79 static int reg_read(struct soc_camera_device *icd, const u8 reg)
  80 {
  81         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
  82         struct i2c_client *client = mt9t031->client;
  83         s32 data = i2c_smbus_read_word_data(client, reg);
  84         return data < 0 ? data : swab16(data);
  85 }
  86
  87 static int reg_write(struct soc_camera_device *icd, const u8 reg,
  88                      const u16 data)
  89 {
  90         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
  91         return i2c_smbus_write_word_data(mt9t031->client, reg, swab16(data));
  92 }
  93
  94 static int reg_set(struct soc_camera_device *icd, const u8 reg,
  95                    const u16 data)
  96 {
  97         int ret;
  98
  99         ret = reg_read(icd, reg);
 100         if (ret < 0)
 101                 return ret;
 102         return reg_write(icd, reg, ret | data);
 103 }
 104
 105 static int reg_clear(struct soc_camera_device *icd, const u8 reg,
 106                      const u16 data)
 107 {
 108         int ret;
 109
 110         ret = reg_read(icd, reg);
 111         if (ret < 0)
 112                 return ret;
 113         return reg_write(icd, reg, ret & ~data);
 114 }
 115
 116 static int set_shutter(struct soc_camera_device *icd, const u32 data)
 117 {
 118         int ret;
 119
 120         ret = reg_write(icd, MT9T031_SHUTTER_WIDTH_UPPER, data >> 16);
 121
 122         if (ret >= 0)
 123                 ret = reg_write(icd, MT9T031_SHUTTER_WIDTH, data & 0xffff);
 124
 125         return ret;
 126 }
 127
 128 static int get_shutter(struct soc_camera_device *icd, u32 *data)
 129 {
 130         int ret;
 131
 132         ret = reg_read(icd, MT9T031_SHUTTER_WIDTH_UPPER);
 133         *data = ret << 16;
 134
 135         if (ret >= 0)
 136                 ret = reg_read(icd, MT9T031_SHUTTER_WIDTH);
 137         *data |= ret & 0xffff;
 138
 139         return ret < 0 ? ret : 0;
 140 }
 141
 142 static int mt9t031_init(struct soc_camera_device *icd)
 143 {
 144         int ret;
 145
 146         /* Disable chip output, synchronous option update */
 147         ret = reg_write(icd, MT9T031_RESET, 1);
 148         if (ret >= 0)
 149                 ret = reg_write(icd, MT9T031_RESET, 0);
 150         if (ret >= 0)
 151                 ret = reg_clear(icd, MT9T031_OUTPUT_CONTROL, 2);
 152
 153         return ret >= 0 ? 0 : -EIO;
 154 }
 155
 156 static int mt9t031_release(struct soc_camera_device *icd)
 157 {
 158         /* Disable the chip */
 159         reg_clear(icd, MT9T031_OUTPUT_CONTROL, 2);
 160         return 0;
 161 }
 162
 163 static int mt9t031_start_capture(struct soc_camera_device *icd)
 164 {
 165         /* Switch to master "normal" mode */
 166         if (reg_set(icd, MT9T031_OUTPUT_CONTROL, 2) < 0)
 167                 return -EIO;
 168         return 0;
 169 }
 170
 171 static int mt9t031_stop_capture(struct soc_camera_device *icd)
 172 {
 173         /* Stop sensor readout */
 174         if (reg_clear(icd, MT9T031_OUTPUT_CONTROL, 2) < 0)
 175                 return -EIO;
 176         return 0;
 177 }
 178
 179 static int mt9t031_set_bus_param(struct soc_camera_device *icd,
 180                                  unsigned long flags)
 181 {
 182         /* The caller should have queried our parameters, check anyway */
 183         if (flags & ~MT9T031_BUS_PARAM)
 184                 return -EINVAL;
 185
 186         if (flags & SOCAM_PCLK_SAMPLE_FALLING)
 187                 reg_clear(icd, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
 188         else
 189                 reg_set(icd, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
 190
 191         return 0;
 192 }
 193
 194 static unsigned long mt9t031_query_bus_param(struct soc_camera_device *icd)
 195 {
 196         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
 197         struct soc_camera_link *icl = mt9t031->client->dev.platform_data;
 198
 199         return soc_camera_apply_sensor_flags(icl, MT9T031_BUS_PARAM);
 200 }
 201
 202 /* Round up minima and round down maxima */
 203 static void recalculate_limits(struct soc_camera_device *icd,
 204                                u16 xskip, u16 yskip)
 205 {
 206         icd->x_min = (MT9T031_COLUMN_SKIP + xskip - 1) / xskip;
 207         icd->y_min = (MT9T031_ROW_SKIP + yskip - 1) / yskip;
 208         icd->width_min = (MT9T031_MIN_WIDTH + xskip - 1) / xskip;
 209         icd->height_min = (MT9T031_MIN_HEIGHT + yskip - 1) / yskip;
 210         icd->width_max = MT9T031_MAX_WIDTH / xskip;
 211         icd->height_max = MT9T031_MAX_HEIGHT / yskip;
 212 }
 213
 214 static int mt9t031_set_params(struct soc_camera_device *icd,
 215                               struct v4l2_rect *rect, u16 xskip, u16 yskip)
 216 {
 217         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
 218         int ret;
 219         u16 xbin, ybin, width, height, left, top;
 220         const u16 hblank = MT9T031_HORIZONTAL_BLANK,
 221                 vblank = MT9T031_VERTICAL_BLANK;
 222
 223         /* Make sure we don't exceed sensor limits */
 224         if (rect->left + rect->width > icd->width_max)
 225                 rect->left = (icd->width_max - rect->width) / 2 + icd->x_min;
 226
 227         if (rect->top + rect->height > icd->height_max)
 228                 rect->top = (icd->height_max - rect->height) / 2 + icd->y_min;
 229
 230         width = rect->width * xskip;
 231         height = rect->height * yskip;
 232         left = rect->left * xskip;
 233         top = rect->top * yskip;
 234
 235         xbin = min(xskip, (u16)3);
 236         ybin = min(yskip, (u16)3);
 237
 238         dev_dbg(&icd->dev, "xskip %u, width %u/%u, yskip %u, height %u/%u\n",
 239                 xskip, width, rect->width, yskip, height, rect->height);
 240
 241         /* Could just do roundup(rect->left, [xy]bin * 2); but this is cheaper */
 242         switch (xbin) {
 243         case 2:
 244                 left = (left + 3) & ~3;
 245                 break;
 246         case 3:
 247                 left = roundup(left, 6);
 248         }
 249
 250         switch (ybin) {
 251         case 2:
 252                 top = (top + 3) & ~3;
 253                 break;
 254         case 3:
 255                 top = roundup(top, 6);
 256         }
 257
 258         /* Disable register update, reconfigure atomically */
 259         ret = reg_set(icd, MT9T031_OUTPUT_CONTROL, 1);
 260         if (ret < 0)
 261                 return ret;
 262
 263         /* Blanking and start values - default... */
 264         ret = reg_write(icd, MT9T031_HORIZONTAL_BLANKING, hblank);
 265         if (ret >= 0)
 266                 ret = reg_write(icd, MT9T031_VERTICAL_BLANKING, vblank);
 267
 268         if (yskip != mt9t031->yskip || xskip != mt9t031->xskip) {
 269                 /* Binning, skipping */
 270                 if (ret >= 0)
 271                         ret = reg_write(icd, MT9T031_COLUMN_ADDRESS_MODE,
 272                                         ((xbin - 1) << 4) | (xskip - 1));
 273                 if (ret >= 0)
 274                         ret = reg_write(icd, MT9T031_ROW_ADDRESS_MODE,
 275                                         ((ybin - 1) << 4) | (yskip - 1));
 276         }
 277         dev_dbg(&icd->dev, "new physical left %u, top %u\n", left, top);
 278
 279         /* The caller provides a supported format, as guaranteed by
 280          * icd->try_fmt_cap(), soc_camera_s_crop() and soc_camera_cropcap() */
 281         if (ret >= 0)
 282                 ret = reg_write(icd, MT9T031_COLUMN_START, left);
 283         if (ret >= 0)
 284                 ret = reg_write(icd, MT9T031_ROW_START, top);
 285         if (ret >= 0)
 286                 ret = reg_write(icd, MT9T031_WINDOW_WIDTH, width - 1);
 287         if (ret >= 0)
 288                 ret = reg_write(icd, MT9T031_WINDOW_HEIGHT,
 289                                 height + icd->y_skip_top - 1);
 290         if (ret >= 0 && mt9t031->autoexposure) {
 291                 ret = set_shutter(icd, height + icd->y_skip_top + vblank);
 292                 if (ret >= 0) {
 293                         const u32 shutter_max = MT9T031_MAX_HEIGHT + vblank;
 294                         const struct v4l2_queryctrl *qctrl =
 295                                 soc_camera_find_qctrl(icd->ops,
 296                                                       V4L2_CID_EXPOSURE);
 297                         icd->exposure = (shutter_max / 2 + (height +
 298                                          icd->y_skip_top + vblank - 1) *
 299                                          (qctrl->maximum - qctrl->minimum)) /
 300                                 shutter_max + qctrl->minimum;
 301                 }
 302         }
 303
 304         /* Re-enable register update, commit all changes */
 305         if (ret >= 0)
 306                 ret = reg_clear(icd, MT9T031_OUTPUT_CONTROL, 1);
 307
 308         return ret < 0 ? ret : 0;
 309 }
 310
 311 static int mt9t031_set_crop(struct soc_camera_device *icd,
 312                             struct v4l2_rect *rect)
 313 {
 314         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
 315
 316         /* CROP - no change in scaling, or in limits */
 317         return mt9t031_set_params(icd, rect, mt9t031->xskip, mt9t031->yskip);
 318 }
 319
 320 static int mt9t031_set_fmt(struct soc_camera_device *icd,
 321                            struct v4l2_format *f)
 322 {
 323         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
 324         int ret;
 325         u16 xskip, yskip;
 326         struct v4l2_rect rect = {
 327                 .left   = icd->x_current,
 328                 .top    = icd->y_current,
 329                 .width  = f->fmt.pix.width,
 330                 .height = f->fmt.pix.height,
 331         };
 332
 333         /*
 334          * try_fmt has put rectangle within limits.
 335          * S_FMT - use binning and skipping for scaling, recalculate
 336          * limits, used for cropping
 337          */
 338         /* Is this more optimal than just a division? */
 339         for (xskip = 8; xskip > 1; xskip--)
 340                 if (rect.width * xskip <= MT9T031_MAX_WIDTH)
 341                         break;
 342
 343         for (yskip = 8; yskip > 1; yskip--)
 344                 if (rect.height * yskip <= MT9T031_MAX_HEIGHT)
 345                         break;
 346
 347         recalculate_limits(icd, xskip, yskip);
 348
 349         ret = mt9t031_set_params(icd, &rect, xskip, yskip);
 350         if (!ret) {
 351                 mt9t031->xskip = xskip;
 352                 mt9t031->yskip = yskip;
 353         }
 354
 355         return ret;
 356 }
 357
 358 static int mt9t031_try_fmt(struct soc_camera_device *icd,
 359                            struct v4l2_format *f)
 360 {
 361         struct v4l2_pix_format *pix = &f->fmt.pix;
 362
 363         if (pix->height < MT9T031_MIN_HEIGHT)
 364                 pix->height = MT9T031_MIN_HEIGHT;
 365         if (pix->height > MT9T031_MAX_HEIGHT)
 366                 pix->height = MT9T031_MAX_HEIGHT;
 367         if (pix->width < MT9T031_MIN_WIDTH)
 368                 pix->width = MT9T031_MIN_WIDTH;
 369         if (pix->width > MT9T031_MAX_WIDTH)
 370                 pix->width = MT9T031_MAX_WIDTH;
 371
 372         pix->width &= ~0x01; /* has to be even */
 373         pix->height &= ~0x01; /* has to be even */
 374
 375         return 0;
 376 }
 377
 378 static int mt9t031_get_chip_id(struct soc_camera_device *icd,
 379                                struct v4l2_dbg_chip_ident *id)
 380 {
 381         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
 382
 383         if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
 384                 return -EINVAL;
 385
 386         if (id->match.addr != mt9t031->client->addr)
 387                 return -ENODEV;
 388
 389         id->ident       = mt9t031->model;
 390         id->revision    = 0;
 391
 392         return 0;
 393 }
 394
 395 #ifdef CONFIG_VIDEO_ADV_DEBUG
 396 static int mt9t031_get_register(struct soc_camera_device *icd,
 397                                 struct v4l2_dbg_register *reg)
 398 {
 399         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
 400
 401         if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
 402                 return -EINVAL;
 403
 404         if (reg->match.addr != mt9t031->client->addr)
 405                 return -ENODEV;
 406
 407         reg->val = reg_read(icd, reg->reg);
 408
 409         if (reg->val > 0xffff)
 410                 return -EIO;
 411
 412         return 0;
 413 }
 414
 415 static int mt9t031_set_register(struct soc_camera_device *icd,
 416                                 struct v4l2_dbg_register *reg)
 417 {
 418         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
 419
 420         if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
 421                 return -EINVAL;
 422
 423         if (reg->match.addr != mt9t031->client->addr)
 424                 return -ENODEV;
 425
 426         if (reg_write(icd, reg->reg, reg->val) < 0)
 427                 return -EIO;
 428
 429         return 0;
 430 }
 431 #endif
 432
 433 static const struct v4l2_queryctrl mt9t031_controls[] = {
 434         {
 435                 .id             = V4L2_CID_VFLIP,
 436                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 437                 .name           = "Flip Vertically",
 438                 .minimum        = 0,
 439                 .maximum        = 1,
 440                 .step           = 1,
 441                 .default_value  = 0,
 442         }, {
 443                 .id             = V4L2_CID_HFLIP,
 444                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 445                 .name           = "Flip Horizontally",
 446                 .minimum        = 0,
 447                 .maximum        = 1,
 448                 .step           = 1,
 449                 .default_value  = 0,
 450         }, {
 451                 .id             = V4L2_CID_GAIN,
 452                 .type           = V4L2_CTRL_TYPE_INTEGER,
 453                 .name           = "Gain",
 454                 .minimum        = 0,
 455                 .maximum        = 127,
 456                 .step           = 1,
 457                 .default_value  = 64,
 458                 .flags          = V4L2_CTRL_FLAG_SLIDER,
 459         }, {
 460                 .id             = V4L2_CID_EXPOSURE,
 461                 .type           = V4L2_CTRL_TYPE_INTEGER,
 462                 .name           = "Exposure",
 463                 .minimum        = 1,
 464                 .maximum        = 255,
 465                 .step           = 1,
 466                 .default_value  = 255,
 467                 .flags          = V4L2_CTRL_FLAG_SLIDER,
 468         }, {
 469                 .id             = V4L2_CID_EXPOSURE_AUTO,
 470                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 471                 .name           = "Automatic Exposure",
 472                 .minimum        = 0,
 473                 .maximum        = 1,
 474                 .step           = 1,
 475                 .default_value  = 1,
 476         }
 477 };
 478
 479 static int mt9t031_video_probe(struct soc_camera_device *);
 480 static void mt9t031_video_remove(struct soc_camera_device *);
 481 static int mt9t031_get_control(struct soc_camera_device *, struct v4l2_control *);
 482 static int mt9t031_set_control(struct soc_camera_device *, struct v4l2_control *);
 483
 484 static struct soc_camera_ops mt9t031_ops = {
 485         .owner                  = THIS_MODULE,
 486         .probe                  = mt9t031_video_probe,
 487         .remove                 = mt9t031_video_remove,
 488         .init                   = mt9t031_init,
 489         .release                = mt9t031_release,
 490         .start_capture          = mt9t031_start_capture,
 491         .stop_capture           = mt9t031_stop_capture,
 492         .set_crop               = mt9t031_set_crop,
 493         .set_fmt                = mt9t031_set_fmt,
 494         .try_fmt                = mt9t031_try_fmt,
 495         .set_bus_param          = mt9t031_set_bus_param,
 496         .query_bus_param        = mt9t031_query_bus_param,
 497         .controls               = mt9t031_controls,
 498         .num_controls           = ARRAY_SIZE(mt9t031_controls),
 499         .get_control            = mt9t031_get_control,
 500         .set_control            = mt9t031_set_control,
 501         .get_chip_id            = mt9t031_get_chip_id,
 502 #ifdef CONFIG_VIDEO_ADV_DEBUG
 503         .get_register           = mt9t031_get_register,
 504         .set_register           = mt9t031_set_register,
 505 #endif
 506 };
 507
 508 static int mt9t031_get_control(struct soc_camera_device *icd, struct v4l2_control *ctrl)
 509 {
 510         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
 511         int data;
 512
 513         switch (ctrl->id) {
 514         case V4L2_CID_VFLIP:
 515                 data = reg_read(icd, MT9T031_READ_MODE_2);
 516                 if (data < 0)
 517                         return -EIO;
 518                 ctrl->value = !!(data & 0x8000);
 519                 break;
 520         case V4L2_CID_HFLIP:
 521                 data = reg_read(icd, MT9T031_READ_MODE_2);
 522                 if (data < 0)
 523                         return -EIO;
 524                 ctrl->value = !!(data & 0x4000);
 525                 break;
 526         case V4L2_CID_EXPOSURE_AUTO:
 527                 ctrl->value = mt9t031->autoexposure;
 528                 break;
 529         }
 530         return 0;
 531 }
 532
 533 static int mt9t031_set_control(struct soc_camera_device *icd, struct v4l2_control *ctrl)
 534 {
 535         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
 536         const struct v4l2_queryctrl *qctrl;
 537         int data;
 538
 539         qctrl = soc_camera_find_qctrl(&mt9t031_ops, ctrl->id);
 540
 541         if (!qctrl)
 542                 return -EINVAL;
 543
 544         switch (ctrl->id) {
 545         case V4L2_CID_VFLIP:
 546                 if (ctrl->value)
 547                         data = reg_set(icd, MT9T031_READ_MODE_2, 0x8000);
 548                 else
 549                         data = reg_clear(icd, MT9T031_READ_MODE_2, 0x8000);
 550                 if (data < 0)
 551                         return -EIO;
 552                 break;
 553         case V4L2_CID_HFLIP:
 554                 if (ctrl->value)
 555                         data = reg_set(icd, MT9T031_READ_MODE_2, 0x4000);
 556                 else
 557                         data = reg_clear(icd, MT9T031_READ_MODE_2, 0x4000);
 558                 if (data < 0)
 559                         return -EIO;
 560                 break;
 561         case V4L2_CID_GAIN:
 562                 if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
 563                         return -EINVAL;
 564                 /* See Datasheet Table 7, Gain settings. */
 565                 if (ctrl->value <= qctrl->default_value) {
 566                         /* Pack it into 0..1 step 0.125, register values 0..8 */
 567                         unsigned long range = qctrl->default_value - qctrl->minimum;
 568                         data = ((ctrl->value - qctrl->minimum) * 8 + range / 2) / range;
 569
 570                         dev_dbg(&icd->dev, "Setting gain %d\n", data);
 571                         data = reg_write(icd, MT9T031_GLOBAL_GAIN, data);
 572                         if (data < 0)
 573                                 return -EIO;
 574                 } else {
 575                         /* Pack it into 1.125..128 variable step, register values 9..0x7860 */
 576                         /* We assume qctrl->maximum - qctrl->default_value - 1 > 0 */
 577                         unsigned long range = qctrl->maximum - qctrl->default_value - 1;
 578                         /* calculated gain: map 65..127 to 9..1024 step 0.125 */
 579                         unsigned long gain = ((ctrl->value - qctrl->default_value - 1) *
 580                                                1015 + range / 2) / range + 9;
 581
 582                         if (gain <= 32)         /* calculated gain 9..32 -> 9..32 */
 583                                 data = gain;
 584                         else if (gain <= 64)    /* calculated gain 33..64 -> 0x51..0x60 */
 585                                 data = ((gain - 32) * 16 + 16) / 32 + 80;
 586                         else
 587                                 /* calculated gain 65..1024 -> (1..120) << 8 + 0x60 */
 588                                 data = (((gain - 64 + 7) * 32) & 0xff00) | 0x60;
 589
 590                         dev_dbg(&icd->dev, "Setting gain from 0x%x to 0x%x\n",
 591                                 reg_read(icd, MT9T031_GLOBAL_GAIN), data);
 592                         data = reg_write(icd, MT9T031_GLOBAL_GAIN, data);
 593                         if (data < 0)
 594                                 return -EIO;
 595                 }
 596
 597                 /* Success */
 598                 icd->gain = ctrl->value;
 599                 break;
 600         case V4L2_CID_EXPOSURE:
 601                 /* mt9t031 has maximum == default */
 602                 if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
 603                         return -EINVAL;
 604                 else {
 605                         const unsigned long range = qctrl->maximum - qctrl->minimum;
 606                         const u32 shutter = ((ctrl->value - qctrl->minimum) * 1048 +
 607                                              range / 2) / range + 1;
 608                         u32 old;
 609
 610                         get_shutter(icd, &old);
 611                         dev_dbg(&icd->dev, "Setting shutter width from %u to %u\n",
 612                                 old, shutter);
 613                         if (set_shutter(icd, shutter) < 0)
 614                                 return -EIO;
 615                         icd->exposure = ctrl->value;
 616                         mt9t031->autoexposure = 0;
 617                 }
 618                 break;
 619         case V4L2_CID_EXPOSURE_AUTO:
 620                 if (ctrl->value) {
 621                         const u16 vblank = MT9T031_VERTICAL_BLANK;
 622                         const u32 shutter_max = MT9T031_MAX_HEIGHT + vblank;
 623                         if (set_shutter(icd, icd->height +
 624                                         icd->y_skip_top + vblank) < 0)
 625                                 return -EIO;
 626                         qctrl = soc_camera_find_qctrl(icd->ops, V4L2_CID_EXPOSURE);
 627                         icd->exposure = (shutter_max / 2 + (icd->height +
 628                                          icd->y_skip_top + vblank - 1) *
 629                                          (qctrl->maximum - qctrl->minimum)) /
 630                                 shutter_max + qctrl->minimum;
 631                         mt9t031->autoexposure = 1;
 632                 } else
 633                         mt9t031->autoexposure = 0;
 634                 break;
 635         }
 636         return 0;
 637 }
 638
 639 /* Interface active, can use i2c. If it fails, it can indeed mean, that
 640  * this wasn't our capture interface, so, we wait for the right one */
 641 static int mt9t031_video_probe(struct soc_camera_device *icd)
 642 {
 643         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
 644         s32 data;
 645         int ret;
 646
 647         /* We must have a parent by now. And it cannot be a wrong one.
 648          * So this entire test is completely redundant. */
 649         if (!icd->dev.parent ||
 650             to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
 651                 return -ENODEV;
 652
 653         /* Enable the chip */
 654         data = reg_write(icd, MT9T031_CHIP_ENABLE, 1);
 655         dev_dbg(&icd->dev, "write: %d\n", data);
 656
 657         /* Read out the chip version register */
 658         data = reg_read(icd, MT9T031_CHIP_VERSION);
 659
 660         switch (data) {
 661         case 0x1621:
 662                 mt9t031->model = V4L2_IDENT_MT9T031;
 663                 icd->formats = mt9t031_colour_formats;
 664                 icd->num_formats = ARRAY_SIZE(mt9t031_colour_formats);
 665                 break;
 666         default:
 667                 ret = -ENODEV;
 668                 dev_err(&icd->dev,
 669                         "No MT9T031 chip detected, register read %x\n", data);
 670                 goto ei2c;
 671         }
 672
 673         dev_info(&icd->dev, "Detected a MT9T031 chip ID %x\n", data);
 674
 675         /* Now that we know the model, we can start video */
 676         ret = soc_camera_video_start(icd);
 677         if (ret)
 678                 goto evstart;
 679
 680         return 0;
 681
 682 evstart:
 683 ei2c:
 684         return ret;
 685 }
 686
 687 static void mt9t031_video_remove(struct soc_camera_device *icd)
 688 {
 689         struct mt9t031 *mt9t031 = container_of(icd, struct mt9t031, icd);
 690
 691         dev_dbg(&icd->dev, "Video %x removed: %p, %p\n", mt9t031->client->addr,
 692                 icd->dev.parent, icd->vdev);
 693         soc_camera_video_stop(icd);
 694 }
 695
 696 static int mt9t031_probe(struct i2c_client *client,
 697                          const struct i2c_device_id *did)
 698 {
 699         struct mt9t031 *mt9t031;
 700         struct soc_camera_device *icd;
 701         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
 702         struct soc_camera_link *icl = client->dev.platform_data;
 703         int ret;
 704
 705         if (!icl) {
 706                 dev_err(&client->dev, "MT9T031 driver needs platform data\n");
 707                 return -EINVAL;
 708         }
 709
 710         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
 711                 dev_warn(&adapter->dev,
 712                          "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
 713                 return -EIO;
 714         }
 715
 716         mt9t031 = kzalloc(sizeof(struct mt9t031), GFP_KERNEL);
 717         if (!mt9t031)
 718                 return -ENOMEM;
 719
 720         mt9t031->client = client;
 721         i2c_set_clientdata(client, mt9t031);
 722
 723         /* Second stage probe - when a capture adapter is there */
 724         icd = &mt9t031->icd;
 725         icd->ops        = &mt9t031_ops;
 726         icd->control    = &client->dev;
 727         icd->x_min      = MT9T031_COLUMN_SKIP;
 728         icd->y_min      = MT9T031_ROW_SKIP;
 729         icd->x_current  = icd->x_min;
 730         icd->y_current  = icd->y_min;
 731         icd->width_min  = MT9T031_MIN_WIDTH;
 732         icd->width_max  = MT9T031_MAX_WIDTH;
 733         icd->height_min = MT9T031_MIN_HEIGHT;
 734         icd->height_max = MT9T031_MAX_HEIGHT;
 735         icd->y_skip_top = 0;
 736         icd->iface      = icl->bus_id;
 737         /* Simulated autoexposure. If enabled, we calculate shutter width
 738          * ourselves in the driver based on vertical blanking and frame width */
 739         mt9t031->autoexposure = 1;
 740
 741         mt9t031->xskip = 1;
 742         mt9t031->yskip = 1;
 743
 744         ret = soc_camera_device_register(icd);
 745         if (ret)
 746                 goto eisdr;
 747
 748         return 0;
 749
 750 eisdr:
 751         i2c_set_clientdata(client, NULL);
 752         kfree(mt9t031);
 753         return ret;
 754 }
 755
 756 static int mt9t031_remove(struct i2c_client *client)
 757 {
 758         struct mt9t031 *mt9t031 = i2c_get_clientdata(client);
 759
 760         soc_camera_device_unregister(&mt9t031->icd);
 761         i2c_set_clientdata(client, NULL);
 762         kfree(mt9t031);
 763
 764         return 0;
 765 }
 766
 767 static const struct i2c_device_id mt9t031_id[] = {
 768         { "mt9t031", 0 },
 769         { }
 770 };
 771 MODULE_DEVICE_TABLE(i2c, mt9t031_id);
 772
 773 static struct i2c_driver mt9t031_i2c_driver = {
 774         .driver = {
 775                 .name = "mt9t031",
 776         },
 777         .probe          = mt9t031_probe,
 778         .remove         = mt9t031_remove,
 779         .id_table       = mt9t031_id,
 780 };
 781
 782 static int __init mt9t031_mod_init(void)
 783 {
 784         return i2c_add_driver(&mt9t031_i2c_driver);
 785 }
 786
 787 static void __exit mt9t031_mod_exit(void)
 788 {
 789         i2c_del_driver(&mt9t031_i2c_driver);
 790 }
 791
 792 module_init(mt9t031_mod_init);
 793 module_exit(mt9t031_mod_exit);
 794
 795 MODULE_DESCRIPTION("Micron MT9T031 Camera driver");
 796 MODULE_AUTHOR("Guennadi Liakhovetski <lg@denx.de>");
 797 MODULE_LICENSE("GPL v2");