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

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