Merge branch 'core/iommu' into core/urgent
[linux-2.6] / drivers / media / video / mt9m001.c
1 /*
2  * Driver for MT9M001 CMOS Image Sensor from Micron
3  *
4  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.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 #include <linux/gpio.h>
16
17 #include <media/v4l2-common.h>
18 #include <media/v4l2-chip-ident.h>
19 #include <media/soc_camera.h>
20
21 /* mt9m001 i2c address 0x5d
22  * The platform has to define i2c_board_info
23  * and call i2c_register_board_info() */
24
25 /* mt9m001 selected register addresses */
26 #define MT9M001_CHIP_VERSION            0x00
27 #define MT9M001_ROW_START               0x01
28 #define MT9M001_COLUMN_START            0x02
29 #define MT9M001_WINDOW_HEIGHT           0x03
30 #define MT9M001_WINDOW_WIDTH            0x04
31 #define MT9M001_HORIZONTAL_BLANKING     0x05
32 #define MT9M001_VERTICAL_BLANKING       0x06
33 #define MT9M001_OUTPUT_CONTROL          0x07
34 #define MT9M001_SHUTTER_WIDTH           0x09
35 #define MT9M001_FRAME_RESTART           0x0b
36 #define MT9M001_SHUTTER_DELAY           0x0c
37 #define MT9M001_RESET                   0x0d
38 #define MT9M001_READ_OPTIONS1           0x1e
39 #define MT9M001_READ_OPTIONS2           0x20
40 #define MT9M001_GLOBAL_GAIN             0x35
41 #define MT9M001_CHIP_ENABLE             0xF1
42
43 static const struct soc_camera_data_format mt9m001_colour_formats[] = {
44         /* Order important: first natively supported,
45          * second supported with a GPIO extender */
46         {
47                 .name           = "Bayer (sRGB) 10 bit",
48                 .depth          = 10,
49                 .fourcc         = V4L2_PIX_FMT_SBGGR16,
50                 .colorspace     = V4L2_COLORSPACE_SRGB,
51         }, {
52                 .name           = "Bayer (sRGB) 8 bit",
53                 .depth          = 8,
54                 .fourcc         = V4L2_PIX_FMT_SBGGR8,
55                 .colorspace     = V4L2_COLORSPACE_SRGB,
56         }
57 };
58
59 static const struct soc_camera_data_format mt9m001_monochrome_formats[] = {
60         /* Order important - see above */
61         {
62                 .name           = "Monochrome 10 bit",
63                 .depth          = 10,
64                 .fourcc         = V4L2_PIX_FMT_Y16,
65         }, {
66                 .name           = "Monochrome 8 bit",
67                 .depth          = 8,
68                 .fourcc         = V4L2_PIX_FMT_GREY,
69         },
70 };
71
72 struct mt9m001 {
73         struct i2c_client *client;
74         struct soc_camera_device icd;
75         int model;      /* V4L2_IDENT_MT9M001* codes from v4l2-chip-ident.h */
76         int switch_gpio;
77         unsigned char autoexposure;
78         unsigned char datawidth;
79 };
80
81 static int reg_read(struct soc_camera_device *icd, const u8 reg)
82 {
83         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
84         struct i2c_client *client = mt9m001->client;
85         s32 data = i2c_smbus_read_word_data(client, reg);
86         return data < 0 ? data : swab16(data);
87 }
88
89 static int reg_write(struct soc_camera_device *icd, const u8 reg,
90                      const u16 data)
91 {
92         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
93         return i2c_smbus_write_word_data(mt9m001->client, reg, swab16(data));
94 }
95
96 static int reg_set(struct soc_camera_device *icd, const u8 reg,
97                    const u16 data)
98 {
99         int ret;
100
101         ret = reg_read(icd, reg);
102         if (ret < 0)
103                 return ret;
104         return reg_write(icd, reg, ret | data);
105 }
106
107 static int reg_clear(struct soc_camera_device *icd, const u8 reg,
108                      const u16 data)
109 {
110         int ret;
111
112         ret = reg_read(icd, reg);
113         if (ret < 0)
114                 return ret;
115         return reg_write(icd, reg, ret & ~data);
116 }
117
118 static int mt9m001_init(struct soc_camera_device *icd)
119 {
120         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
121         struct soc_camera_link *icl = mt9m001->client->dev.platform_data;
122         int ret;
123
124         dev_dbg(icd->vdev->parent, "%s\n", __func__);
125
126         if (icl->power) {
127                 ret = icl->power(&mt9m001->client->dev, 1);
128                 if (ret < 0) {
129                         dev_err(icd->vdev->parent,
130                                 "Platform failed to power-on the camera.\n");
131                         return ret;
132                 }
133         }
134
135         /* The camera could have been already on, we reset it additionally */
136         if (icl->reset)
137                 ret = icl->reset(&mt9m001->client->dev);
138         else
139                 ret = -ENODEV;
140
141         if (ret < 0) {
142                 /* Either no platform reset, or platform reset failed */
143                 ret = reg_write(icd, MT9M001_RESET, 1);
144                 if (!ret)
145                         ret = reg_write(icd, MT9M001_RESET, 0);
146         }
147         /* Disable chip, synchronous option update */
148         if (!ret)
149                 ret = reg_write(icd, MT9M001_OUTPUT_CONTROL, 0);
150
151         return ret;
152 }
153
154 static int mt9m001_release(struct soc_camera_device *icd)
155 {
156         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
157         struct soc_camera_link *icl = mt9m001->client->dev.platform_data;
158
159         /* Disable the chip */
160         reg_write(icd, MT9M001_OUTPUT_CONTROL, 0);
161
162         if (icl->power)
163                 icl->power(&mt9m001->client->dev, 0);
164
165         return 0;
166 }
167
168 static int mt9m001_start_capture(struct soc_camera_device *icd)
169 {
170         /* Switch to master "normal" mode */
171         if (reg_write(icd, MT9M001_OUTPUT_CONTROL, 2) < 0)
172                 return -EIO;
173         return 0;
174 }
175
176 static int mt9m001_stop_capture(struct soc_camera_device *icd)
177 {
178         /* Stop sensor readout */
179         if (reg_write(icd, MT9M001_OUTPUT_CONTROL, 0) < 0)
180                 return -EIO;
181         return 0;
182 }
183
184 static int bus_switch_request(struct mt9m001 *mt9m001,
185                               struct soc_camera_link *icl)
186 {
187 #ifdef CONFIG_MT9M001_PCA9536_SWITCH
188         int ret;
189         unsigned int gpio = icl->gpio;
190
191         if (gpio_is_valid(gpio)) {
192                 /* We have a data bus switch. */
193                 ret = gpio_request(gpio, "mt9m001");
194                 if (ret < 0) {
195                         dev_err(&mt9m001->client->dev, "Cannot get GPIO %u\n",
196                                 gpio);
197                         return ret;
198                 }
199
200                 ret = gpio_direction_output(gpio, 0);
201                 if (ret < 0) {
202                         dev_err(&mt9m001->client->dev,
203                                 "Cannot set GPIO %u to output\n", gpio);
204                         gpio_free(gpio);
205                         return ret;
206                 }
207         }
208
209         mt9m001->switch_gpio = gpio;
210 #else
211         mt9m001->switch_gpio = -EINVAL;
212 #endif
213         return 0;
214 }
215
216 static void bus_switch_release(struct mt9m001 *mt9m001)
217 {
218 #ifdef CONFIG_MT9M001_PCA9536_SWITCH
219         if (gpio_is_valid(mt9m001->switch_gpio))
220                 gpio_free(mt9m001->switch_gpio);
221 #endif
222 }
223
224 static int bus_switch_act(struct mt9m001 *mt9m001, int go8bit)
225 {
226 #ifdef CONFIG_MT9M001_PCA9536_SWITCH
227         if (!gpio_is_valid(mt9m001->switch_gpio))
228                 return -ENODEV;
229
230         gpio_set_value_cansleep(mt9m001->switch_gpio, go8bit);
231         return 0;
232 #else
233         return -ENODEV;
234 #endif
235 }
236
237 static int bus_switch_possible(struct mt9m001 *mt9m001)
238 {
239 #ifdef CONFIG_MT9M001_PCA9536_SWITCH
240         return gpio_is_valid(mt9m001->switch_gpio);
241 #else
242         return 0;
243 #endif
244 }
245
246 static int mt9m001_set_bus_param(struct soc_camera_device *icd,
247                                  unsigned long flags)
248 {
249         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
250         unsigned int width_flag = flags & SOCAM_DATAWIDTH_MASK;
251         int ret;
252
253         /* Flags validity verified in test_bus_param */
254
255         if ((mt9m001->datawidth != 10 && (width_flag == SOCAM_DATAWIDTH_10)) ||
256             (mt9m001->datawidth != 9  && (width_flag == SOCAM_DATAWIDTH_9)) ||
257             (mt9m001->datawidth != 8  && (width_flag == SOCAM_DATAWIDTH_8))) {
258                 /* Well, we actually only can do 10 or 8 bits... */
259                 if (width_flag == SOCAM_DATAWIDTH_9)
260                         return -EINVAL;
261                 ret = bus_switch_act(mt9m001,
262                                      width_flag == SOCAM_DATAWIDTH_8);
263                 if (ret < 0)
264                         return ret;
265
266                 mt9m001->datawidth = width_flag == SOCAM_DATAWIDTH_8 ? 8 : 10;
267         }
268
269         return 0;
270 }
271
272 static unsigned long mt9m001_query_bus_param(struct soc_camera_device *icd)
273 {
274         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
275         struct soc_camera_link *icl = mt9m001->client->dev.platform_data;
276         /* MT9M001 has all capture_format parameters fixed */
277         unsigned long flags = SOCAM_DATAWIDTH_10 | SOCAM_PCLK_SAMPLE_RISING |
278                 SOCAM_HSYNC_ACTIVE_HIGH | SOCAM_VSYNC_ACTIVE_HIGH |
279                 SOCAM_MASTER;
280
281         if (bus_switch_possible(mt9m001))
282                 flags |= SOCAM_DATAWIDTH_8;
283
284         return soc_camera_apply_sensor_flags(icl, flags);
285 }
286
287 static int mt9m001_set_fmt(struct soc_camera_device *icd,
288                            __u32 pixfmt, struct v4l2_rect *rect)
289 {
290         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
291         int ret;
292         const u16 hblank = 9, vblank = 25;
293
294         /* Blanking and start values - default... */
295         ret = reg_write(icd, MT9M001_HORIZONTAL_BLANKING, hblank);
296         if (!ret)
297                 ret = reg_write(icd, MT9M001_VERTICAL_BLANKING, vblank);
298
299         /* The caller provides a supported format, as verified per
300          * call to icd->try_fmt() */
301         if (!ret)
302                 ret = reg_write(icd, MT9M001_COLUMN_START, rect->left);
303         if (!ret)
304                 ret = reg_write(icd, MT9M001_ROW_START, rect->top);
305         if (!ret)
306                 ret = reg_write(icd, MT9M001_WINDOW_WIDTH, rect->width - 1);
307         if (!ret)
308                 ret = reg_write(icd, MT9M001_WINDOW_HEIGHT,
309                                 rect->height + icd->y_skip_top - 1);
310         if (!ret && mt9m001->autoexposure) {
311                 ret = reg_write(icd, MT9M001_SHUTTER_WIDTH,
312                                 rect->height + icd->y_skip_top + vblank);
313                 if (!ret) {
314                         const struct v4l2_queryctrl *qctrl =
315                                 soc_camera_find_qctrl(icd->ops,
316                                                       V4L2_CID_EXPOSURE);
317                         icd->exposure = (524 + (rect->height + icd->y_skip_top +
318                                                 vblank - 1) *
319                                          (qctrl->maximum - qctrl->minimum)) /
320                                 1048 + qctrl->minimum;
321                 }
322         }
323
324         return ret;
325 }
326
327 static int mt9m001_try_fmt(struct soc_camera_device *icd,
328                            struct v4l2_format *f)
329 {
330         struct v4l2_pix_format *pix = &f->fmt.pix;
331
332         if (pix->height < 32 + icd->y_skip_top)
333                 pix->height = 32 + icd->y_skip_top;
334         if (pix->height > 1024 + icd->y_skip_top)
335                 pix->height = 1024 + icd->y_skip_top;
336         if (pix->width < 48)
337                 pix->width = 48;
338         if (pix->width > 1280)
339                 pix->width = 1280;
340         pix->width &= ~0x01; /* has to be even, unsure why was ~3 */
341
342         return 0;
343 }
344
345 static int mt9m001_get_chip_id(struct soc_camera_device *icd,
346                                struct v4l2_dbg_chip_ident *id)
347 {
348         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
349
350         if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
351                 return -EINVAL;
352
353         if (id->match.addr != mt9m001->client->addr)
354                 return -ENODEV;
355
356         id->ident       = mt9m001->model;
357         id->revision    = 0;
358
359         return 0;
360 }
361
362 #ifdef CONFIG_VIDEO_ADV_DEBUG
363 static int mt9m001_get_register(struct soc_camera_device *icd,
364                                 struct v4l2_dbg_register *reg)
365 {
366         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
367
368         if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
369                 return -EINVAL;
370
371         if (reg->match.addr != mt9m001->client->addr)
372                 return -ENODEV;
373
374         reg->size = 2;
375         reg->val = reg_read(icd, reg->reg);
376
377         if (reg->val > 0xffff)
378                 return -EIO;
379
380         return 0;
381 }
382
383 static int mt9m001_set_register(struct soc_camera_device *icd,
384                                 struct v4l2_dbg_register *reg)
385 {
386         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
387
388         if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
389                 return -EINVAL;
390
391         if (reg->match.addr != mt9m001->client->addr)
392                 return -ENODEV;
393
394         if (reg_write(icd, reg->reg, reg->val) < 0)
395                 return -EIO;
396
397         return 0;
398 }
399 #endif
400
401 static const struct v4l2_queryctrl mt9m001_controls[] = {
402         {
403                 .id             = V4L2_CID_VFLIP,
404                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
405                 .name           = "Flip Vertically",
406                 .minimum        = 0,
407                 .maximum        = 1,
408                 .step           = 1,
409                 .default_value  = 0,
410         }, {
411                 .id             = V4L2_CID_GAIN,
412                 .type           = V4L2_CTRL_TYPE_INTEGER,
413                 .name           = "Gain",
414                 .minimum        = 0,
415                 .maximum        = 127,
416                 .step           = 1,
417                 .default_value  = 64,
418                 .flags          = V4L2_CTRL_FLAG_SLIDER,
419         }, {
420                 .id             = V4L2_CID_EXPOSURE,
421                 .type           = V4L2_CTRL_TYPE_INTEGER,
422                 .name           = "Exposure",
423                 .minimum        = 1,
424                 .maximum        = 255,
425                 .step           = 1,
426                 .default_value  = 255,
427                 .flags          = V4L2_CTRL_FLAG_SLIDER,
428         }, {
429                 .id             = V4L2_CID_EXPOSURE_AUTO,
430                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
431                 .name           = "Automatic Exposure",
432                 .minimum        = 0,
433                 .maximum        = 1,
434                 .step           = 1,
435                 .default_value  = 1,
436         }
437 };
438
439 static int mt9m001_video_probe(struct soc_camera_device *);
440 static void mt9m001_video_remove(struct soc_camera_device *);
441 static int mt9m001_get_control(struct soc_camera_device *, struct v4l2_control *);
442 static int mt9m001_set_control(struct soc_camera_device *, struct v4l2_control *);
443
444 static struct soc_camera_ops mt9m001_ops = {
445         .owner                  = THIS_MODULE,
446         .probe                  = mt9m001_video_probe,
447         .remove                 = mt9m001_video_remove,
448         .init                   = mt9m001_init,
449         .release                = mt9m001_release,
450         .start_capture          = mt9m001_start_capture,
451         .stop_capture           = mt9m001_stop_capture,
452         .set_fmt                = mt9m001_set_fmt,
453         .try_fmt                = mt9m001_try_fmt,
454         .set_bus_param          = mt9m001_set_bus_param,
455         .query_bus_param        = mt9m001_query_bus_param,
456         .controls               = mt9m001_controls,
457         .num_controls           = ARRAY_SIZE(mt9m001_controls),
458         .get_control            = mt9m001_get_control,
459         .set_control            = mt9m001_set_control,
460         .get_chip_id            = mt9m001_get_chip_id,
461 #ifdef CONFIG_VIDEO_ADV_DEBUG
462         .get_register           = mt9m001_get_register,
463         .set_register           = mt9m001_set_register,
464 #endif
465 };
466
467 static int mt9m001_get_control(struct soc_camera_device *icd, struct v4l2_control *ctrl)
468 {
469         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
470         int data;
471
472         switch (ctrl->id) {
473         case V4L2_CID_VFLIP:
474                 data = reg_read(icd, MT9M001_READ_OPTIONS2);
475                 if (data < 0)
476                         return -EIO;
477                 ctrl->value = !!(data & 0x8000);
478                 break;
479         case V4L2_CID_EXPOSURE_AUTO:
480                 ctrl->value = mt9m001->autoexposure;
481                 break;
482         }
483         return 0;
484 }
485
486 static int mt9m001_set_control(struct soc_camera_device *icd, struct v4l2_control *ctrl)
487 {
488         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
489         const struct v4l2_queryctrl *qctrl;
490         int data;
491
492         qctrl = soc_camera_find_qctrl(&mt9m001_ops, ctrl->id);
493
494         if (!qctrl)
495                 return -EINVAL;
496
497         switch (ctrl->id) {
498         case V4L2_CID_VFLIP:
499                 if (ctrl->value)
500                         data = reg_set(icd, MT9M001_READ_OPTIONS2, 0x8000);
501                 else
502                         data = reg_clear(icd, MT9M001_READ_OPTIONS2, 0x8000);
503                 if (data < 0)
504                         return -EIO;
505                 break;
506         case V4L2_CID_GAIN:
507                 if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
508                         return -EINVAL;
509                 /* See Datasheet Table 7, Gain settings. */
510                 if (ctrl->value <= qctrl->default_value) {
511                         /* Pack it into 0..1 step 0.125, register values 0..8 */
512                         unsigned long range = qctrl->default_value - qctrl->minimum;
513                         data = ((ctrl->value - qctrl->minimum) * 8 + range / 2) / range;
514
515                         dev_dbg(&icd->dev, "Setting gain %d\n", data);
516                         data = reg_write(icd, MT9M001_GLOBAL_GAIN, data);
517                         if (data < 0)
518                                 return -EIO;
519                 } else {
520                         /* Pack it into 1.125..15 variable step, register values 9..67 */
521                         /* We assume qctrl->maximum - qctrl->default_value - 1 > 0 */
522                         unsigned long range = qctrl->maximum - qctrl->default_value - 1;
523                         unsigned long gain = ((ctrl->value - qctrl->default_value - 1) *
524                                                111 + range / 2) / range + 9;
525
526                         if (gain <= 32)
527                                 data = gain;
528                         else if (gain <= 64)
529                                 data = ((gain - 32) * 16 + 16) / 32 + 80;
530                         else
531                                 data = ((gain - 64) * 7 + 28) / 56 + 96;
532
533                         dev_dbg(&icd->dev, "Setting gain from %d to %d\n",
534                                  reg_read(icd, MT9M001_GLOBAL_GAIN), data);
535                         data = reg_write(icd, MT9M001_GLOBAL_GAIN, data);
536                         if (data < 0)
537                                 return -EIO;
538                 }
539
540                 /* Success */
541                 icd->gain = ctrl->value;
542                 break;
543         case V4L2_CID_EXPOSURE:
544                 /* mt9m001 has maximum == default */
545                 if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
546                         return -EINVAL;
547                 else {
548                         unsigned long range = qctrl->maximum - qctrl->minimum;
549                         unsigned long shutter = ((ctrl->value - qctrl->minimum) * 1048 +
550                                                  range / 2) / range + 1;
551
552                         dev_dbg(&icd->dev, "Setting shutter width from %d to %lu\n",
553                                  reg_read(icd, MT9M001_SHUTTER_WIDTH), shutter);
554                         if (reg_write(icd, MT9M001_SHUTTER_WIDTH, shutter) < 0)
555                                 return -EIO;
556                         icd->exposure = ctrl->value;
557                         mt9m001->autoexposure = 0;
558                 }
559                 break;
560         case V4L2_CID_EXPOSURE_AUTO:
561                 if (ctrl->value) {
562                         const u16 vblank = 25;
563                         if (reg_write(icd, MT9M001_SHUTTER_WIDTH, icd->height +
564                                       icd->y_skip_top + vblank) < 0)
565                                 return -EIO;
566                         qctrl = soc_camera_find_qctrl(icd->ops, V4L2_CID_EXPOSURE);
567                         icd->exposure = (524 + (icd->height + icd->y_skip_top + vblank - 1) *
568                                          (qctrl->maximum - qctrl->minimum)) /
569                                 1048 + qctrl->minimum;
570                         mt9m001->autoexposure = 1;
571                 } else
572                         mt9m001->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 mt9m001_video_probe(struct soc_camera_device *icd)
581 {
582         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
583         struct soc_camera_link *icl = mt9m001->client->dev.platform_data;
584         s32 data;
585         int ret;
586
587         /* We must have a parent by now. And it cannot be a wrong one.
588          * So this entire test is completely redundant. */
589         if (!icd->dev.parent ||
590             to_soc_camera_host(icd->dev.parent)->nr != icd->iface)
591                 return -ENODEV;
592
593         /* Enable the chip */
594         data = reg_write(icd, MT9M001_CHIP_ENABLE, 1);
595         dev_dbg(&icd->dev, "write: %d\n", data);
596
597         /* Read out the chip version register */
598         data = reg_read(icd, MT9M001_CHIP_VERSION);
599
600         /* must be 0x8411 or 0x8421 for colour sensor and 8431 for bw */
601         switch (data) {
602         case 0x8411:
603         case 0x8421:
604                 mt9m001->model = V4L2_IDENT_MT9M001C12ST;
605                 icd->formats = mt9m001_colour_formats;
606                 if (gpio_is_valid(icl->gpio))
607                         icd->num_formats = ARRAY_SIZE(mt9m001_colour_formats);
608                 else
609                         icd->num_formats = 1;
610                 break;
611         case 0x8431:
612                 mt9m001->model = V4L2_IDENT_MT9M001C12STM;
613                 icd->formats = mt9m001_monochrome_formats;
614                 if (gpio_is_valid(icl->gpio))
615                         icd->num_formats = ARRAY_SIZE(mt9m001_monochrome_formats);
616                 else
617                         icd->num_formats = 1;
618                 break;
619         default:
620                 ret = -ENODEV;
621                 dev_err(&icd->dev,
622                         "No MT9M001 chip detected, register read %x\n", data);
623                 goto ei2c;
624         }
625
626         dev_info(&icd->dev, "Detected a MT9M001 chip ID %x (%s)\n", data,
627                  data == 0x8431 ? "C12STM" : "C12ST");
628
629         /* Now that we know the model, we can start video */
630         ret = soc_camera_video_start(icd);
631         if (ret)
632                 goto eisis;
633
634         return 0;
635
636 eisis:
637 ei2c:
638         return ret;
639 }
640
641 static void mt9m001_video_remove(struct soc_camera_device *icd)
642 {
643         struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
644
645         dev_dbg(&icd->dev, "Video %x removed: %p, %p\n", mt9m001->client->addr,
646                 icd->dev.parent, icd->vdev);
647         soc_camera_video_stop(icd);
648 }
649
650 static int mt9m001_probe(struct i2c_client *client,
651                          const struct i2c_device_id *did)
652 {
653         struct mt9m001 *mt9m001;
654         struct soc_camera_device *icd;
655         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
656         struct soc_camera_link *icl = client->dev.platform_data;
657         int ret;
658
659         if (!icl) {
660                 dev_err(&client->dev, "MT9M001 driver needs platform data\n");
661                 return -EINVAL;
662         }
663
664         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
665                 dev_warn(&adapter->dev,
666                          "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
667                 return -EIO;
668         }
669
670         mt9m001 = kzalloc(sizeof(struct mt9m001), GFP_KERNEL);
671         if (!mt9m001)
672                 return -ENOMEM;
673
674         mt9m001->client = client;
675         i2c_set_clientdata(client, mt9m001);
676
677         /* Second stage probe - when a capture adapter is there */
678         icd = &mt9m001->icd;
679         icd->ops        = &mt9m001_ops;
680         icd->control    = &client->dev;
681         icd->x_min      = 20;
682         icd->y_min      = 12;
683         icd->x_current  = 20;
684         icd->y_current  = 12;
685         icd->width_min  = 48;
686         icd->width_max  = 1280;
687         icd->height_min = 32;
688         icd->height_max = 1024;
689         icd->y_skip_top = 1;
690         icd->iface      = icl->bus_id;
691         /* Default datawidth - this is the only width this camera (normally)
692          * supports. It is only with extra logic that it can support
693          * other widths. Therefore it seems to be a sensible default. */
694         mt9m001->datawidth = 10;
695         /* Simulated autoexposure. If enabled, we calculate shutter width
696          * ourselves in the driver based on vertical blanking and frame width */
697         mt9m001->autoexposure = 1;
698
699         ret = bus_switch_request(mt9m001, icl);
700         if (ret)
701                 goto eswinit;
702
703         ret = soc_camera_device_register(icd);
704         if (ret)
705                 goto eisdr;
706
707         return 0;
708
709 eisdr:
710         bus_switch_release(mt9m001);
711 eswinit:
712         kfree(mt9m001);
713         return ret;
714 }
715
716 static int mt9m001_remove(struct i2c_client *client)
717 {
718         struct mt9m001 *mt9m001 = i2c_get_clientdata(client);
719
720         soc_camera_device_unregister(&mt9m001->icd);
721         bus_switch_release(mt9m001);
722         kfree(mt9m001);
723
724         return 0;
725 }
726
727 static const struct i2c_device_id mt9m001_id[] = {
728         { "mt9m001", 0 },
729         { }
730 };
731 MODULE_DEVICE_TABLE(i2c, mt9m001_id);
732
733 static struct i2c_driver mt9m001_i2c_driver = {
734         .driver = {
735                 .name = "mt9m001",
736         },
737         .probe          = mt9m001_probe,
738         .remove         = mt9m001_remove,
739         .id_table       = mt9m001_id,
740 };
741
742 static int __init mt9m001_mod_init(void)
743 {
744         return i2c_add_driver(&mt9m001_i2c_driver);
745 }
746
747 static void __exit mt9m001_mod_exit(void)
748 {
749         i2c_del_driver(&mt9m001_i2c_driver);
750 }
751
752 module_init(mt9m001_mod_init);
753 module_exit(mt9m001_mod_exit);
754
755 MODULE_DESCRIPTION("Micron MT9M001 Camera driver");
756 MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
757 MODULE_LICENSE("GPL");