?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
added 2.6.29.6 aldebaran kernel
[nao-ulib.git] / kernel / 2.6.29.6-aldebaran-rt / drivers / media / video / mt9t031.c
blob349d8e36553087886354ff8771cf4b1cab12c4c5
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_dbg_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.addr != 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_dbg_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.addr != 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_dbg_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.addr != 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");
Nao low-level kernelspace/userspace library that runs on our robots, Nao-Team HTWK