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