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