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