7f68de4048a371b62b32d36426af0205eebfc139
[glsdk/meta-ti-glsdk.git] / recipes-kernel / linux / linux-omap-2.6.39 / camera / 0001-Add-support-for-mt9p031-Aptina-Micron-sensor.patch
1 From 244f96b15afbd73942762e2c07523d6c5cf68a7e Mon Sep 17 00:00:00 2001
2 From: Javier Martin <javier.martin@vista-silicon.com>
3 Date: Tue, 31 May 2011 11:46:49 +0200
4 Subject: [PATCH 1/3] Add support for mt9p031 Aptina (Micron) sensor.
6 This patch adds basic support (no controls) for
7 mt9p031 sensor. It applies on kernel 2.6.39.
9 Signed-off-by: Javier Martin <javier.martin@vista-silicon.com>
10 ---
11  drivers/media/video/Kconfig   |    7 +
12  drivers/media/video/Makefile  |    1 +
13  drivers/media/video/mt9p031.c |  699 +++++++++++++++++++++++++++++++++++++++++
14  include/media/mt9p031.h       |   11 +
15  4 files changed, 718 insertions(+), 0 deletions(-)
16  create mode 100644 drivers/media/video/mt9p031.c
17  create mode 100644 include/media/mt9p031.h
19 diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
20 index 00f51dd..cb87e35 100644
21 --- a/drivers/media/video/Kconfig
22 +++ b/drivers/media/video/Kconfig
23 @@ -329,6 +329,13 @@ config VIDEO_OV7670
24           OV7670 VGA camera.  It currently only works with the M88ALP01
25           controller.
26  
27 +config VIDEO_MT9P031
28 +       tristate "Aptina MT9P031 support"
29 +       depends on I2C && VIDEO_V4L2
30 +       ---help---
31 +        This is a Video4Linux2 sensor-level driver for the Aptina
32 +        (Micron) mt9p031 5 Mpixel camera.
33 +
34  config VIDEO_MT9V011
35         tristate "Micron mt9v011 sensor support"
36         depends on I2C && VIDEO_V4L2
37 diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
38 index ace5d8b..912b29b 100644
39 --- a/drivers/media/video/Makefile
40 +++ b/drivers/media/video/Makefile
41 @@ -65,6 +65,7 @@ obj-$(CONFIG_VIDEO_UPD64083) += upd64083.o
42  obj-$(CONFIG_VIDEO_OV7670)     += ov7670.o
43  obj-$(CONFIG_VIDEO_TCM825X) += tcm825x.o
44  obj-$(CONFIG_VIDEO_TVEEPROM) += tveeprom.o
45 +obj-$(CONFIG_VIDEO_MT9P031) += mt9p031.o
46  obj-$(CONFIG_VIDEO_MT9V011) += mt9v011.o
47  obj-$(CONFIG_VIDEO_SR030PC30)  += sr030pc30.o
48  obj-$(CONFIG_VIDEO_NOON010PC30)        += noon010pc30.o
49 diff --git a/drivers/media/video/mt9p031.c b/drivers/media/video/mt9p031.c
50 new file mode 100644
51 index 0000000..61b46a0
52 --- /dev/null
53 +++ b/drivers/media/video/mt9p031.c
54 @@ -0,0 +1,699 @@
55 +/*
56 + * Driver for MT9P031 CMOS Image Sensor from Aptina
57 + *
58 + * Copyright (C) 2011, Javier Martin <javier.martin@vista-silicon.com>
59 + *
60 + * Copyright (C) 2011, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
61 + *
62 + * Based on the MT9V032 driver and Bastian Hecht's code.
63 + *
64 + * This program is free software; you can redistribute it and/or modify
65 + * it under the terms of the GNU General Public License version 2 as
66 + * published by the Free Software Foundation.
67 + */
68 +
69 +#include <linux/delay.h>
70 +#include <linux/device.h>
71 +#include <linux/i2c.h>
72 +#include <linux/log2.h>
73 +#include <linux/pm.h>
74 +#include <linux/slab.h>
75 +#include <media/v4l2-subdev.h>
76 +#include <linux/videodev2.h>
77 +
78 +#include <media/mt9p031.h>
79 +#include <media/v4l2-chip-ident.h>
80 +#include <media/v4l2-subdev.h>
81 +#include <media/v4l2-device.h>
82 +
83 +#define MT9P031_PIXCLK_FREQ                    54000000
84 +
85 +#define MT9P031_CHIP_VERSION                   0x00
86 +#define                MT9P031_CHIP_VERSION_VALUE      0x1801
87 +#define MT9P031_ROW_START                      0x01
88 +#define                MT9P031_ROW_START_MIN           1
89 +#define                MT9P031_ROW_START_MAX           2004
90 +#define                MT9P031_ROW_START_DEF           54
91 +#define MT9P031_COLUMN_START                   0x02
92 +#define                MT9P031_COLUMN_START_MIN        1
93 +#define                MT9P031_COLUMN_START_MAX        2750
94 +#define                MT9P031_COLUMN_START_DEF        16
95 +#define MT9P031_WINDOW_HEIGHT                  0x03
96 +#define                MT9P031_WINDOW_HEIGHT_MIN       2
97 +#define                MT9P031_WINDOW_HEIGHT_MAX       2003
98 +#define                MT9P031_WINDOW_HEIGHT_DEF       2003
99 +#define MT9P031_WINDOW_WIDTH                   0x04
100 +#define                MT9P031_WINDOW_WIDTH_MIN        18
101 +#define                MT9P031_WINDOW_WIDTH_MAX        2751
102 +#define                MT9P031_WINDOW_WIDTH_DEF        2751
103 +#define MT9P031_H_BLANKING                     0x05
104 +#define                MT9P031_H_BLANKING_VALUE        0
105 +#define MT9P031_V_BLANKING                     0x06
106 +#define                MT9P031_V_BLANKING_VALUE        25
107 +#define MT9P031_OUTPUT_CONTROL                 0x07
108 +#define                MT9P031_OUTPUT_CONTROL_CEN      2
109 +#define                MT9P031_OUTPUT_CONTROL_SYN      1
110 +#define MT9P031_SHUTTER_WIDTH_UPPER            0x08
111 +#define MT9P031_SHUTTER_WIDTH                  0x09
112 +#define MT9P031_PIXEL_CLOCK_CONTROL            0x0a
113 +#define MT9P031_FRAME_RESTART                  0x0b
114 +#define MT9P031_SHUTTER_DELAY                  0x0c
115 +#define MT9P031_RST                            0x0d
116 +#define                MT9P031_RST_ENABLE              1
117 +#define                MT9P031_RST_DISABLE             0
118 +#define MT9P031_READ_MODE_1                    0x1e
119 +#define MT9P031_READ_MODE_2                    0x20
120 +#define                MT9P031_READ_MODE_2_ROW_MIR     0x8000
121 +#define                MT9P031_READ_MODE_2_COL_MIR     0x4000
122 +#define MT9P031_ROW_ADDRESS_MODE               0x22
123 +#define MT9P031_COLUMN_ADDRESS_MODE            0x23
124 +#define MT9P031_GLOBAL_GAIN                    0x35
129 +struct mt9p031 {
130 +       struct v4l2_subdev subdev;
131 +       struct media_pad pad;
132 +       struct v4l2_rect rect;  /* Sensor window */
133 +       struct v4l2_mbus_framefmt format;
134 +       struct mt9p031_platform_data *pdata;
135 +       struct mutex power_lock; /* lock to protect power_count */
136 +       int power_count;
137 +       u16 xskip;
138 +       u16 yskip;
139 +       /* cache register values */
140 +       u16 output_control;
141 +};
143 +static struct mt9p031 *to_mt9p031(const struct i2c_client *client)
144 +{
145 +       return container_of(i2c_get_clientdata(client), struct mt9p031, subdev);
146 +}
148 +static int reg_read(struct i2c_client *client, const u8 reg)
149 +{
150 +       s32 data = i2c_smbus_read_word_data(client, reg);
151 +       return data < 0 ? data : swab16(data);
152 +}
154 +static int reg_write(struct i2c_client *client, const u8 reg,
155 +                       const u16 data)
156 +{
157 +       return i2c_smbus_write_word_data(client, reg, swab16(data));
158 +}
160 +static int mt9p031_set_output_control(struct mt9p031 *mt9p031, u16 clear,
161 +                                       u16 set)
162 +{
163 +       struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
164 +       u16 value = (mt9p031->output_control & ~clear) | set;
165 +       int ret;
167 +       ret = reg_write(client, MT9P031_OUTPUT_CONTROL, value);
168 +       if (ret < 0)
169 +               return ret;
170 +       mt9p031->output_control = value;
171 +       return 0;
172 +}
174 +static int mt9p031_reset(struct i2c_client *client)
175 +{
176 +       struct mt9p031 *mt9p031 = to_mt9p031(client);
177 +       int ret;
179 +       /* Disable chip output, synchronous option update */
180 +       ret = reg_write(client, MT9P031_RST, MT9P031_RST_ENABLE);
181 +       if (ret < 0)
182 +               return ret;
183 +       ret = reg_write(client, MT9P031_RST, MT9P031_RST_DISABLE);
184 +       if (ret < 0)
185 +               return ret;
186 +       return mt9p031_set_output_control(mt9p031,
187 +                                       MT9P031_OUTPUT_CONTROL_CEN, 0);
188 +}
190 +static int mt9p031_power_on(struct mt9p031 *mt9p031)
191 +{
192 +       struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
193 +       int ret;
195 +       /* Ensure RESET_BAR is low */
196 +       if (mt9p031->pdata->reset) {
197 +               mt9p031->pdata->reset(&mt9p031->subdev, 1);
198 +               msleep(1);
199 +       }
200 +       /* Emable clock */
201 +       if (mt9p031->pdata->set_xclk)
202 +               mt9p031->pdata->set_xclk(&mt9p031->subdev, MT9P031_PIXCLK_FREQ);
203 +       /* Now RESET_BAR must be high */
204 +       if (mt9p031->pdata->reset) {
205 +               mt9p031->pdata->reset(&mt9p031->subdev, 0);
206 +               msleep(1);
207 +       }
208 +       /* soft reset */
209 +       ret = mt9p031_reset(client);
210 +       if (ret < 0) {
211 +               dev_err(&client->dev, "Failed to reset the camera\n");
212 +               return ret;
213 +       }
214 +       return 0;
215 +}
217 +static void mt9p031_power_off(struct mt9p031 *mt9p031)
218 +{
219 +       if (mt9p031->pdata->reset) {
220 +               mt9p031->pdata->reset(&mt9p031->subdev, 1);
221 +               msleep(1);
222 +       }
223 +       if (mt9p031->pdata->set_xclk)
224 +               mt9p031->pdata->set_xclk(&mt9p031->subdev, 0);
225 +}
227 +static int mt9p031_enum_mbus_code(struct v4l2_subdev *sd,
228 +                               struct v4l2_subdev_fh *fh,
229 +                               struct v4l2_subdev_mbus_code_enum *code)
230 +{
231 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
233 +       if (code->pad || code->index)
234 +               return -EINVAL;
236 +       code->code = mt9p031->format.code;
237 +       return 0;
238 +}
240 +static struct v4l2_mbus_framefmt *mt9p031_get_pad_format(
241 +       struct mt9p031 *mt9p031,
242 +       struct v4l2_subdev_fh *fh,
243 +       unsigned int pad, u32 which)
244 +{
245 +       switch (which) {
246 +       case V4L2_SUBDEV_FORMAT_TRY:
247 +               return v4l2_subdev_get_try_format(fh, pad);
248 +       case V4L2_SUBDEV_FORMAT_ACTIVE:
249 +               return &mt9p031->format;
250 +       default:
251 +               return NULL;
252 +       }
253 +}
255 +static struct v4l2_rect *mt9p031_get_pad_crop(struct mt9p031 *mt9p031,
256 +                       struct v4l2_subdev_fh *fh, unsigned int pad, u32 which)
257 +{
258 +       switch (which) {
259 +       case V4L2_SUBDEV_FORMAT_TRY:
260 +               return v4l2_subdev_get_try_crop(fh, pad);
261 +       case V4L2_SUBDEV_FORMAT_ACTIVE:
262 +               return &mt9p031->rect;
263 +       default:
264 +               return NULL;
265 +       }
266 +}
268 +static int mt9p031_get_crop(struct v4l2_subdev *sd,
269 +                               struct v4l2_subdev_fh *fh,
270 +                               struct v4l2_subdev_crop *crop)
271 +{
272 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
273 +       struct v4l2_rect *rect = mt9p031_get_pad_crop(mt9p031, fh, crop->pad,
274 +                                                       crop->which);
275 +       if (!rect)
276 +               return -EINVAL;
278 +       crop->rect = *rect;
280 +       return 0;
281 +}
283 +static u16 mt9p031_skip_for_crop(s32 source, s32 *target, s32 max_skip)
284 +{
285 +       unsigned int skip;
287 +       if (source - source / 4 < *target) {
288 +               *target = source;
289 +               return 1;
290 +       }
292 +       skip = DIV_ROUND_CLOSEST(source, *target);
293 +       if (skip > max_skip)
294 +               skip = max_skip;
295 +       *target = 2 * DIV_ROUND_UP(source, 2 * skip);
297 +       return skip;
298 +}
300 +static int mt9p031_set_params(struct i2c_client *client,
301 +                               struct v4l2_rect *rect, u16 xskip, u16 yskip)
302 +{
303 +       struct mt9p031 *mt9p031 = to_mt9p031(client);
304 +       int ret;
305 +       u16 xbin, ybin;
306 +       const u16 hblank = MT9P031_H_BLANKING_VALUE,
307 +               vblank = MT9P031_V_BLANKING_VALUE;
308 +       __s32 left;
310 +       /*
311 +       * TODO: Attention! When implementing horizontal flipping, adjust
312 +       * alignment according to R2 "Column Start" description in the datasheet
313 +       */
314 +       if (xskip & 1) {
315 +               xbin = 1;
316 +               left = rect->left & (~3);
317 +       } else if (xskip & 2) {
318 +               xbin = 2;
319 +               left = rect->left & (~7);
320 +       } else {
321 +               xbin = 4;
322 +               left = rect->left & (~15);
323 +       }
324 +       ybin = min(yskip, (u16)4);
326 +       /* Disable register update, reconfigure atomically */
327 +       ret = mt9p031_set_output_control(mt9p031, 0,
328 +                                       MT9P031_OUTPUT_CONTROL_SYN);
329 +       if (ret < 0)
330 +               return ret;
332 +       dev_dbg(&client->dev, "skip %u:%u, rect %ux%u@%u:%u\n",
333 +               xskip, yskip, rect->width, rect->height, rect->left, rect->top);
335 +       /* Blanking and start values - default... */
336 +       ret = reg_write(client, MT9P031_H_BLANKING, hblank);
337 +       if (ret < 0)
338 +               return ret;
339 +       ret = reg_write(client, MT9P031_V_BLANKING, vblank);
340 +       if (ret < 0)
341 +               return ret;
343 +       ret = reg_write(client, MT9P031_COLUMN_ADDRESS_MODE,
344 +                               ((xbin - 1) << 4) | (xskip - 1));
345 +       if (ret < 0)
346 +               return ret;
347 +       ret = reg_write(client, MT9P031_ROW_ADDRESS_MODE,
348 +                               ((ybin - 1) << 4) | (yskip - 1));
349 +       if (ret < 0)
350 +               return ret;
352 +       dev_dbg(&client->dev, "new physical left %u, top %u\n",
353 +               rect->left, rect->top);
355 +       ret = reg_write(client, MT9P031_COLUMN_START,
356 +                               rect->left);
357 +       if (ret < 0)
358 +               return ret;
359 +       ret = reg_write(client, MT9P031_ROW_START,
360 +                               rect->top);
361 +       if (ret < 0)
362 +               return ret;
364 +       ret = reg_write(client, MT9P031_WINDOW_WIDTH,
365 +                               rect->width - 1);
366 +       if (ret < 0)
367 +               return ret;
368 +       ret = reg_write(client, MT9P031_WINDOW_HEIGHT,
369 +                               rect->height - 1);
370 +       if (ret < 0)
371 +               return ret;
373 +       /* Re-enable register update, commit all changes */
374 +       ret = mt9p031_set_output_control(mt9p031,
375 +                                       MT9P031_OUTPUT_CONTROL_SYN, 0);
376 +       if (ret < 0)
377 +               return ret;
379 +       mt9p031->xskip = xskip;
380 +       mt9p031->yskip = yskip;
381 +       return ret;
382 +}
384 +static int mt9p031_set_crop(struct v4l2_subdev *sd,
385 +                               struct v4l2_subdev_fh *fh,
386 +                               struct v4l2_subdev_crop *crop)
387 +{
388 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
389 +       struct v4l2_mbus_framefmt *f;
390 +       struct v4l2_rect *c;
391 +       struct v4l2_rect rect;
392 +       u16 xskip, yskip;
393 +       s32 width, height;
395 +       dev_dbg(mt9p031->subdev.v4l2_dev->dev, "%s(%ux%u@%u:%u : %u)\n",
396 +                       __func__, crop->rect.width, crop->rect.height,
397 +                       crop->rect.left, crop->rect.top, crop->which);
399 +       /*
400 +       * Clamp the crop rectangle boundaries and align them to a multiple of 2
401 +       * pixels.
402 +       */
403 +       rect.width = ALIGN(clamp(crop->rect.width,
404 +                               MT9P031_WINDOW_WIDTH_MIN,
405 +                               MT9P031_WINDOW_WIDTH_MAX), 2);
406 +       rect.height = ALIGN(clamp(crop->rect.height,
407 +                               MT9P031_WINDOW_HEIGHT_MIN,
408 +                               MT9P031_WINDOW_HEIGHT_MAX), 2);
409 +       rect.left = ALIGN(clamp(crop->rect.left,
410 +                               MT9P031_COLUMN_START_MIN,
411 +                               MT9P031_COLUMN_START_MAX), 2);
412 +       rect.top = ALIGN(clamp(crop->rect.top,
413 +                               MT9P031_ROW_START_MIN,
414 +                               MT9P031_ROW_START_MAX), 2);
416 +       c = mt9p031_get_pad_crop(mt9p031, fh, crop->pad, crop->which);
418 +       if (rect.width != c->width || rect.height != c->height) {
419 +               /*
420 +               * Reset the output image size if the crop rectangle size has
421 +               * been modified.
422 +               */
423 +               f = mt9p031_get_pad_format(mt9p031, fh, crop->pad,
424 +                                               crop->which);
425 +               width = f->width;
426 +               height = f->height;
428 +               xskip = mt9p031_skip_for_crop(rect.width, &width, 7);
429 +               yskip = mt9p031_skip_for_crop(rect.height, &height, 8);
430 +       } else {
431 +               xskip = mt9p031->xskip;
432 +               yskip = mt9p031->yskip;
433 +               f = NULL;
434 +       }
435 +       if (f) {
436 +               f->width = width;
437 +               f->height = height;
438 +       }
440 +       *c = rect;
441 +       crop->rect = rect;
443 +       mt9p031->xskip = xskip;
444 +       mt9p031->yskip = yskip;
445 +       mt9p031->rect = *c;
446 +       return 0;
447 +}
449 +static int mt9p031_get_format(struct v4l2_subdev *sd,
450 +                               struct v4l2_subdev_fh *fh,
451 +                               struct v4l2_subdev_format *fmt)
452 +{
453 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
455 +       fmt->format =
456 +               *mt9p031_get_pad_format(mt9p031, fh, fmt->pad, fmt->which);
457 +       return 0;
458 +}
460 +static u16 mt9p031_skip_for_scale(s32 *source, s32 target,
461 +                                       s32 max_skip, s32 max)
462 +{
463 +       unsigned int skip;
465 +       if (*source - *source / 4 < target) {
466 +               *source = target;
467 +               return 1;
468 +       }
470 +       skip = min(max, *source + target / 2) / target;
471 +       if (skip > max_skip)
472 +               skip = max_skip;
473 +       *source = target * skip;
475 +       return skip;
476 +}
478 +static int mt9p031_set_format(struct v4l2_subdev *sd,
479 +                               struct v4l2_subdev_fh *fh,
480 +                               struct v4l2_subdev_format *format)
481 +{
482 +       struct v4l2_mbus_framefmt *__format;
483 +       struct v4l2_rect *__crop, rect;
484 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
485 +       unsigned int width;
486 +       unsigned int height;
487 +       u16 xskip, yskip;
489 +       __crop = mt9p031_get_pad_crop(mt9p031, fh, format->pad, format->which);
491 +       width = clamp_t(int, ALIGN(format->format.width, 2), 2,
492 +                                               MT9P031_WINDOW_WIDTH_MAX);
493 +       height = clamp_t(int, ALIGN(format->format.height, 2), 2,
494 +                                               MT9P031_WINDOW_HEIGHT_MAX);
496 +       rect.width = __crop->width;
497 +       rect.height = __crop->height;
499 +       xskip = mt9p031_skip_for_scale(&rect.width, width, 7,
500 +                               MT9P031_WINDOW_WIDTH_MAX);
501 +       if (rect.width + __crop->left > MT9P031_WINDOW_WIDTH_MAX)
502 +               rect.left = (MT9P031_WINDOW_WIDTH_MAX - rect.width) / 2;
503 +       else
504 +               rect.left = __crop->left;
505 +       yskip = mt9p031_skip_for_scale(&rect.height, height, 8,
506 +                               MT9P031_WINDOW_HEIGHT_MAX);
507 +       if (rect.height + __crop->top > MT9P031_WINDOW_HEIGHT_MAX)
508 +               rect.top = (MT9P031_WINDOW_HEIGHT_MAX - rect.height) / 2;
509 +       else
510 +               rect.top = __crop->top;
512 +       dev_dbg(mt9p031->subdev.v4l2_dev->dev, "%s(%ux%u : %u)\n", __func__,
513 +               width, height, format->which);
514 +       if (__crop)
515 +               *__crop = rect;
517 +       __format = mt9p031_get_pad_format(mt9p031, fh, format->pad,
518 +                                               format->which);
519 +       __format->width = width;
520 +       __format->height = height;
521 +       format->format = *__format;
523 +       mt9p031->xskip = xskip;
524 +       mt9p031->yskip = yskip;
525 +       mt9p031->rect = *__crop;
526 +       return 0;
527 +}
529 +static int mt9p031_s_stream(struct v4l2_subdev *sd, int enable)
530 +{
531 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
532 +       struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
533 +       struct v4l2_rect rect = mt9p031->rect;
534 +       u16 xskip = mt9p031->xskip;
535 +       u16 yskip = mt9p031->yskip;
536 +       int ret;
538 +       if (enable) {
539 +               ret = mt9p031_set_params(client, &rect, xskip, yskip);
540 +               if (ret < 0)
541 +                       return ret;
542 +               /* Switch to master "normal" mode */
543 +               ret = mt9p031_set_output_control(mt9p031, 0,
544 +                                               MT9P031_OUTPUT_CONTROL_CEN);
545 +       } else {
546 +               /* Stop sensor readout */
547 +               ret = mt9p031_set_output_control(mt9p031,
548 +                                               MT9P031_OUTPUT_CONTROL_CEN, 0);
549 +       }
550 +       return ret;
551 +}
553 +static int mt9p031_video_probe(struct i2c_client *client)
554 +{
555 +       s32 data;
557 +       /* Read out the chip version register */
558 +       data = reg_read(client, MT9P031_CHIP_VERSION);
559 +       if (data != MT9P031_CHIP_VERSION_VALUE) {
560 +               dev_err(&client->dev,
561 +                       "No MT9P031 chip detected, register read %x\n", data);
562 +               return -ENODEV;
563 +       }
565 +       dev_info(&client->dev, "Detected a MT9P031 chip ID %x\n", data);
567 +       return 0;
568 +}
570 +static int mt9p031_set_power(struct v4l2_subdev *sd, int on)
571 +{
572 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
573 +       int ret = 0;
575 +       mutex_lock(&mt9p031->power_lock);
577 +       /*
578 +       * If the power count is modified from 0 to != 0 or from != 0 to 0,
579 +       * update the power state.
580 +       */
581 +       if (mt9p031->power_count == !on) {
582 +               if (on) {
583 +                       ret = mt9p031_power_on(mt9p031);
584 +                       if (ret) {
585 +                               dev_err(mt9p031->subdev.v4l2_dev->dev,
586 +                               "Failed to power on: %d\n", ret);
587 +                               goto out;
588 +                       }
589 +               } else {
590 +                       mt9p031_power_off(mt9p031);
591 +               }
592 +       }
594 +       /* Update the power count. */
595 +       mt9p031->power_count += on ? 1 : -1;
596 +       WARN_ON(mt9p031->power_count < 0);
598 +out:
599 +       mutex_unlock(&mt9p031->power_lock);
600 +       return ret;
601 +}
603 +static int mt9p031_registered(struct v4l2_subdev *sd)
604 +{
605 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
606 +       struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
607 +       int ret;
609 +       ret = mt9p031_set_power(&mt9p031->subdev, 1);
610 +       if (ret) {
611 +               dev_err(&client->dev,
612 +                       "Failed to power on device: %d\n", ret);
613 +               return ret;
614 +       }
616 +       ret = mt9p031_video_probe(client);
618 +       mt9p031_set_power(&mt9p031->subdev, 0);
620 +       return ret;
621 +}
623 +static int mt9p031_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
624 +{
625 +       struct mt9p031 *mt9p031;
626 +       mt9p031 = container_of(sd, struct mt9p031, subdev);
628 +       mt9p031->rect.width     = MT9P031_WINDOW_WIDTH_DEF;
629 +       mt9p031->rect.height    = MT9P031_WINDOW_HEIGHT_DEF;
630 +       mt9p031->rect.left      = MT9P031_COLUMN_START_DEF;
631 +       mt9p031->rect.top       = MT9P031_ROW_START_DEF;
633 +       mt9p031->format.code = V4L2_MBUS_FMT_SGRBG12_1X12;
634 +       mt9p031->format.width = MT9P031_WINDOW_WIDTH_DEF;
635 +       mt9p031->format.height = MT9P031_WINDOW_HEIGHT_DEF;
636 +       mt9p031->format.field = V4L2_FIELD_NONE;
637 +       mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
639 +       mt9p031->xskip = 1;
640 +       mt9p031->yskip = 1;
641 +       return mt9p031_set_power(sd, 1);
642 +}
644 +static int mt9p031_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
645 +{
646 +       return mt9p031_set_power(sd, 0);
647 +}
649 +static struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
650 +       .s_power        = mt9p031_set_power,
651 +};
653 +static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
654 +       .s_stream       = mt9p031_s_stream,
655 +};
657 +static struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
658 +       .enum_mbus_code = mt9p031_enum_mbus_code,
659 +       .get_fmt = mt9p031_get_format,
660 +       .set_fmt = mt9p031_set_format,
661 +       .get_crop = mt9p031_get_crop,
662 +       .set_crop = mt9p031_set_crop,
663 +};
665 +static struct v4l2_subdev_ops mt9p031_subdev_ops = {
666 +       .core   = &mt9p031_subdev_core_ops,
667 +       .video  = &mt9p031_subdev_video_ops,
668 +       .pad    = &mt9p031_subdev_pad_ops,
669 +};
671 +static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
672 +       .registered = mt9p031_registered,
673 +       .open = mt9p031_open,
674 +       .close = mt9p031_close,
675 +};
677 +static int mt9p031_probe(struct i2c_client *client,
678 +                               const struct i2c_device_id *did)
679 +{
680 +       int ret;
681 +       struct mt9p031 *mt9p031;
682 +       struct mt9p031_platform_data *pdata = client->dev.platform_data;
683 +       struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
685 +       if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
686 +               dev_warn(&adapter->dev,
687 +                       "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
688 +               return -EIO;
689 +       }
691 +       mt9p031 = kzalloc(sizeof(struct mt9p031), GFP_KERNEL);
692 +       if (!mt9p031)
693 +               return -ENOMEM;
695 +       mutex_init(&mt9p031->power_lock);
696 +       v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
697 +       mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
699 +       mt9p031->pdata          = pdata;
701 +       mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
702 +       ret = media_entity_init(&mt9p031->subdev.entity, 1, &mt9p031->pad, 0);
703 +       if (ret)
704 +               return ret;
706 +       mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
708 +       return 0;
709 +}
711 +static int mt9p031_remove(struct i2c_client *client)
712 +{
713 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
714 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
716 +       v4l2_device_unregister_subdev(sd);
717 +       media_entity_cleanup(&sd->entity);
718 +       kfree(mt9p031);
720 +       return 0;
721 +}
723 +static const struct i2c_device_id mt9p031_id[] = {
724 +       { "mt9p031", 0 },
725 +       { }
726 +};
727 +MODULE_DEVICE_TABLE(i2c, mt9p031_id);
729 +static struct i2c_driver mt9p031_i2c_driver = {
730 +       .driver = {
731 +               .name = "mt9p031",
732 +       },
733 +       .probe          = mt9p031_probe,
734 +       .remove         = mt9p031_remove,
735 +       .id_table       = mt9p031_id,
736 +};
738 +static int __init mt9p031_mod_init(void)
739 +{
740 +       return i2c_add_driver(&mt9p031_i2c_driver);
741 +}
743 +static void __exit mt9p031_mod_exit(void)
744 +{
745 +       i2c_del_driver(&mt9p031_i2c_driver);
746 +}
748 +module_init(mt9p031_mod_init);
749 +module_exit(mt9p031_mod_exit);
751 +MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
752 +MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
753 +MODULE_LICENSE("GPL v2");
754 diff --git a/include/media/mt9p031.h b/include/media/mt9p031.h
755 new file mode 100644
756 index 0000000..ad37eb3
757 --- /dev/null
758 +++ b/include/media/mt9p031.h
759 @@ -0,0 +1,11 @@
760 +#ifndef MT9P031_H
761 +#define MT9P031_H
763 +struct v4l2_subdev;
765 +struct mt9p031_platform_data {
766 +       int (*set_xclk)(struct v4l2_subdev *subdev, int hz);
767 +       int (*reset)(struct v4l2_subdev *subdev, int active);
768 +};
770 +#endif
771 -- 
772 1.6.6.1