V4L/DVB (10099): soc-camera: add support for MT9T031 CMOS camera sensor from Micron
[linux-2.6.git] / 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");