9746415fdc2aa46f210c641df7fefdb15598d17c
[glsdk/meta-ti-glsdk.git] / recipes-kernel / linux / linux-omap-2.6.39 / camera / 0001-Add-mt9p031-sensor-driver.patch
1 From 327e65fc0d86911bda7c1f37fb5c3534d1b2c849 Mon Sep 17 00:00:00 2001
2 From: Javier Martin <javier.martin@vista-silicon.com>
3 Date: Thu, 26 May 2011 07:20:53 +0000
4 Subject: [PATCH 1/4] Add mt9p031 sensor driver.
6 This patch applies on 2.6.39 (commit 61c4f2c81c61f73549928dfd9f3e8f26aa36a8cf).
7 It has some power management issue which causes horizontal random lines on
8 the image. If we do not disable 1v8 regulator, the problem disappears.
9 I would like to get some help on fixing whatever the problem with power
10 management is.
12 Signed-off-by: Javier Martin <javier.martin@vista-silicon.com>
13 ---
14  drivers/media/video/Kconfig   |    7 +
15  drivers/media/video/Makefile  |    1 +
16  drivers/media/video/mt9p031.c |  841 +++++++++++++++++++++++++++++++++++++++++
17  include/media/mt9p031.h       |   11 +
18  4 files changed, 860 insertions(+), 0 deletions(-)
19  create mode 100644 drivers/media/video/mt9p031.c
20  create mode 100644 include/media/mt9p031.h
22 diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
23 index 00f51dd..cb87e35 100644
24 --- a/drivers/media/video/Kconfig
25 +++ b/drivers/media/video/Kconfig
26 @@ -329,6 +329,13 @@ config VIDEO_OV7670
27           OV7670 VGA camera.  It currently only works with the M88ALP01
28           controller.
29  
30 +config VIDEO_MT9P031
31 +       tristate "Aptina MT9P031 support"
32 +       depends on I2C && VIDEO_V4L2
33 +       ---help---
34 +        This is a Video4Linux2 sensor-level driver for the Aptina
35 +        (Micron) mt9p031 5 Mpixel camera.
36 +
37  config VIDEO_MT9V011
38         tristate "Micron mt9v011 sensor support"
39         depends on I2C && VIDEO_V4L2
40 diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
41 index ace5d8b..912b29b 100644
42 --- a/drivers/media/video/Makefile
43 +++ b/drivers/media/video/Makefile
44 @@ -65,6 +65,7 @@ obj-$(CONFIG_VIDEO_UPD64083) += upd64083.o
45  obj-$(CONFIG_VIDEO_OV7670)     += ov7670.o
46  obj-$(CONFIG_VIDEO_TCM825X) += tcm825x.o
47  obj-$(CONFIG_VIDEO_TVEEPROM) += tveeprom.o
48 +obj-$(CONFIG_VIDEO_MT9P031) += mt9p031.o
49  obj-$(CONFIG_VIDEO_MT9V011) += mt9v011.o
50  obj-$(CONFIG_VIDEO_SR030PC30)  += sr030pc30.o
51  obj-$(CONFIG_VIDEO_NOON010PC30)        += noon010pc30.o
52 diff --git a/drivers/media/video/mt9p031.c b/drivers/media/video/mt9p031.c
53 new file mode 100644
54 index 0000000..242bea9
55 --- /dev/null
56 +++ b/drivers/media/video/mt9p031.c
57 @@ -0,0 +1,841 @@
58 +/*
59 + * Driver for MT9P031 CMOS Image Sensor from Aptina
60 + *
61 + * Copyright (C) 2011, Javier Martin <javier.martin@vista-silicon.com>
62 + *
63 + * Copyright (C) 2011, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
64 + *
65 + * Based on the MT9V032 driver and Bastian Hecht's code.
66 + *
67 + * This program is free software; you can redistribute it and/or modify
68 + * it under the terms of the GNU General Public License version 2 as
69 + * published by the Free Software Foundation.
70 + */
71 +
72 +#include <linux/delay.h>
73 +#include <linux/device.h>
74 +#include <linux/i2c.h>
75 +#include <linux/log2.h>
76 +#include <linux/pm.h>
77 +#include <linux/regulator/consumer.h>
78 +#include <linux/slab.h>
79 +#include <media/v4l2-subdev.h>
80 +#include <linux/videodev2.h>
81 +
82 +#include <media/mt9p031.h>
83 +#include <media/v4l2-chip-ident.h>
84 +#include <media/v4l2-subdev.h>
85 +#include <media/v4l2-device.h>
86 +
87 +#define MT9P031_PIXCLK_FREQ                    54000000
88 +
89 +/* mt9p031 selected register addresses */
90 +#define MT9P031_CHIP_VERSION                   0x00
91 +#define                MT9P031_CHIP_VERSION_VALUE      0x1801
92 +#define MT9P031_ROW_START                      0x01
93 +#define                MT9P031_ROW_START_DEF           54
94 +#define MT9P031_COLUMN_START                   0x02
95 +#define                MT9P031_COLUMN_START_DEF        16
96 +#define MT9P031_WINDOW_HEIGHT                  0x03
97 +#define MT9P031_WINDOW_WIDTH                   0x04
98 +#define MT9P031_H_BLANKING                     0x05
99 +#define                MT9P031_H_BLANKING_VALUE        0
100 +#define MT9P031_V_BLANKING                     0x06
101 +#define                MT9P031_V_BLANKING_VALUE        25
102 +#define MT9P031_OUTPUT_CONTROL                 0x07
103 +#define                MT9P031_OUTPUT_CONTROL_CEN      2
104 +#define                MT9P031_OUTPUT_CONTROL_SYN      1
105 +#define MT9P031_SHUTTER_WIDTH_UPPER            0x08
106 +#define MT9P031_SHUTTER_WIDTH                  0x09
107 +#define MT9P031_PIXEL_CLOCK_CONTROL            0x0a
108 +#define MT9P031_FRAME_RESTART                  0x0b
109 +#define MT9P031_SHUTTER_DELAY                  0x0c
110 +#define MT9P031_RST                            0x0d
111 +#define                MT9P031_RST_ENABLE              1
112 +#define                MT9P031_RST_DISABLE             0
113 +#define MT9P031_READ_MODE_1                    0x1e
114 +#define MT9P031_READ_MODE_2                    0x20
115 +#define                MT9P031_READ_MODE_2_ROW_MIR     0x8000
116 +#define                MT9P031_READ_MODE_2_COL_MIR     0x4000
117 +#define MT9P031_ROW_ADDRESS_MODE               0x22
118 +#define MT9P031_COLUMN_ADDRESS_MODE            0x23
119 +#define MT9P031_GLOBAL_GAIN                    0x35
121 +#define MT9P031_WINDOW_HEIGHT_MAX              1944
122 +#define MT9P031_WINDOW_WIDTH_MAX               2592
123 +#define MT9P031_WINDOW_HEIGHT_MIN              2
124 +#define MT9P031_WINDOW_WIDTH_MIN               18
126 +struct mt9p031 {
127 +       struct v4l2_subdev subdev;
128 +       struct media_pad pad;
129 +       struct v4l2_rect rect;  /* Sensor window */
130 +       struct v4l2_mbus_framefmt format;
131 +       struct mt9p031_platform_data *pdata;
132 +       struct mutex power_lock; /* lock to protect power_count */
133 +       int power_count;
134 +       u16 xskip;
135 +       u16 yskip;
136 +       /* cache register values */
137 +       u16 output_control;
138 +       u16 h_blanking;
139 +       u16 v_blanking;
140 +       u16 column_address_mode;
141 +       u16 row_address_mode;
142 +       u16 column_start;
143 +       u16 row_start;
144 +       u16 window_width;
145 +       u16 window_height;
146 +       struct regulator *reg_1v8;
147 +       struct regulator *reg_2v8;
148 +};
150 +static struct mt9p031 *to_mt9p031(const struct i2c_client *client)
151 +{
152 +       return container_of(i2c_get_clientdata(client), struct mt9p031, subdev);
153 +}
155 +static int reg_read(struct i2c_client *client, const u8 reg)
156 +{
157 +       s32 data = i2c_smbus_read_word_data(client, reg);
158 +       return data < 0 ? data : swab16(data);
159 +}
161 +static int reg_write(struct i2c_client *client, const u8 reg,
162 +                       const u16 data)
163 +{
164 +       return i2c_smbus_write_word_data(client, reg, swab16(data));
165 +}
167 +static int reg_write_cached(struct i2c_client *client, const u8 reg,
168 +                       const u16 data, u16 *cache)
169 +{
170 +       int ret;
172 +       ret = reg_write(client, reg, data);
173 +       if (ret < 0)
174 +               return ret;
175 +       *cache = data;
176 +       return 0;
177 +}
179 +static int mt9p031_set_output_control(struct mt9p031 *mt9p031, u16 clear,
180 +                                     u16 set)
181 +{
182 +       struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
183 +       u16 value = (mt9p031->output_control & ~clear) | set;
185 +       return reg_write_cached(client, MT9P031_OUTPUT_CONTROL, value,
186 +                               &mt9p031->output_control);
187 +}
189 +static int restore_registers(struct i2c_client *client)
190 +{
191 +       int ret;
192 +       struct mt9p031 *mt9p031 = to_mt9p031(client);
194 +       /* Disable register update, reconfigure atomically */
195 +       ret = mt9p031_set_output_control(mt9p031, 0,
196 +                                       MT9P031_OUTPUT_CONTROL_SYN);
197 +       if (ret < 0)
198 +               return ret;
200 +       /* Blanking and start values - default... */
201 +       ret = reg_write(client, MT9P031_H_BLANKING, mt9p031->h_blanking);
202 +       if (ret < 0)
203 +               return ret;
205 +       ret = reg_write(client, MT9P031_V_BLANKING, mt9p031->v_blanking);
206 +       if (ret < 0)
207 +               return ret;
209 +       ret = reg_write(client, MT9P031_COLUMN_ADDRESS_MODE,
210 +                               mt9p031->column_address_mode);
211 +       if (ret < 0)
212 +               return ret;
214 +       ret = reg_write(client, MT9P031_ROW_ADDRESS_MODE,
215 +                               mt9p031->row_address_mode);
216 +       if (ret < 0)
217 +               return ret;
219 +       ret = reg_write(client, MT9P031_COLUMN_START,
220 +                               mt9p031->column_start);
221 +       if (ret < 0)
222 +               return ret;
224 +       ret = reg_write(client, MT9P031_ROW_START,
225 +                               mt9p031->row_start);
226 +       if (ret < 0)
227 +               return ret;
229 +       ret = reg_write(client, MT9P031_WINDOW_WIDTH,
230 +                               mt9p031->window_width);
231 +       if (ret < 0)
232 +               return ret;
234 +       ret = reg_write(client, MT9P031_WINDOW_HEIGHT,
235 +                               mt9p031->window_height);
236 +       if (ret < 0)
237 +               return ret;
239 +       /* Re-enable register update, commit all changes */
240 +       ret = mt9p031_set_output_control(mt9p031,
241 +                                       MT9P031_OUTPUT_CONTROL_SYN, 0);
242 +       if (ret < 0)
243 +               return ret;
244 +       return 0;
245 +}
247 +static int mt9p031_reset(struct i2c_client *client)
248 +{
249 +       struct mt9p031 *mt9p031 = to_mt9p031(client);
250 +       int ret;
252 +       /* Disable chip output, synchronous option update */
253 +       ret = reg_write(client, MT9P031_RST, MT9P031_RST_ENABLE);
254 +       if (ret < 0)
255 +               return ret;
256 +       ret = reg_write(client, MT9P031_RST, MT9P031_RST_DISABLE);
257 +       if (ret < 0)
258 +               return ret;
259 +       return mt9p031_set_output_control(mt9p031,
260 +                                       MT9P031_OUTPUT_CONTROL_CEN, 0);
261 +}
263 +static int mt9p031_power_on(struct mt9p031 *mt9p031)
264 +{
265 +       struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
266 +       int ret;
268 +       /* Ensure RESET_BAR is low */
269 +       if (mt9p031->pdata->reset)
270 +               mt9p031->pdata->reset(&mt9p031->subdev, 1);
271 +       /* turn on digital supply first */
272 +       ret = regulator_enable(mt9p031->reg_1v8);
273 +       if (ret) {
274 +               dev_err(&client->dev,
275 +                       "Failed to enable 1.8v regulator: %d\n", ret);
276 +               goto err_1v8;
277 +       }
278 +       /* now turn on analog supply */
279 +       ret = regulator_enable(mt9p031->reg_2v8);
280 +       if (ret) {
281 +               dev_err(&client->dev,
282 +                       "Failed to enable 2.8v regulator: %d\n", ret);
283 +               goto err_rst;
284 +       }
285 +       /* Now RESET_BAR must be high */
286 +       if (mt9p031->pdata->reset)
287 +               mt9p031->pdata->reset(&mt9p031->subdev, 0);
289 +       if (mt9p031->pdata->set_xclk)
290 +               mt9p031->pdata->set_xclk(&mt9p031->subdev, MT9P031_PIXCLK_FREQ);
292 +       /* soft reset */
293 +       ret = mt9p031_reset(client);
294 +       if (ret < 0) {
295 +               dev_err(&client->dev, "Failed to reset the camera\n");
296 +               goto err_rst;
297 +       }
299 +       ret = restore_registers(client);
300 +       if (ret < 0) {
301 +               dev_err(&client->dev, "Failed to restore registers\n");
302 +               goto err_rst;
303 +       }
305 +       return 0;
306 +err_rst:
307 +       regulator_disable(mt9p031->reg_1v8);
308 +err_1v8:
309 +       return ret;
311 +}
313 +static void mt9p031_power_off(struct mt9p031 *mt9p031)
314 +{
315 +       if (mt9p031->pdata->set_xclk)
316 +               mt9p031->pdata->set_xclk(&mt9p031->subdev, 0);
317 +       if (mt9p031->pdata->reset)
318 +               mt9p031->pdata->reset(&mt9p031->subdev, 1);
319 +       regulator_disable(mt9p031->reg_1v8);
320 +       regulator_disable(mt9p031->reg_2v8);
321 +}
323 +static int mt9p031_enum_mbus_code(struct v4l2_subdev *sd,
324 +                               struct v4l2_subdev_fh *fh,
325 +                               struct v4l2_subdev_mbus_code_enum *code)
326 +{
327 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
329 +       if (code->pad || code->index)
330 +               return -EINVAL;
332 +       code->code = mt9p031->format.code;
334 +       return 0;
335 +}
337 +static struct v4l2_mbus_framefmt *mt9p031_get_pad_format(
338 +                       struct mt9p031 *mt9p031,
339 +                       struct v4l2_subdev_fh *fh,
340 +                       unsigned int pad, u32 which)
341 +{
342 +       switch (which) {
343 +       case V4L2_SUBDEV_FORMAT_TRY:
344 +               return v4l2_subdev_get_try_format(fh, pad);
345 +       case V4L2_SUBDEV_FORMAT_ACTIVE:
346 +               return &mt9p031->format;
347 +       default:
348 +               return NULL;
349 +       }
350 +}
352 +static struct v4l2_rect *mt9p031_get_pad_crop(struct mt9p031 *mt9p031,
353 +                       struct v4l2_subdev_fh *fh, unsigned int pad, u32 which)
354 +{
355 +       switch (which) {
356 +       case V4L2_SUBDEV_FORMAT_TRY:
357 +               return v4l2_subdev_get_try_crop(fh, pad);
358 +       case V4L2_SUBDEV_FORMAT_ACTIVE:
359 +               return &mt9p031->rect;
360 +       default:
361 +               return NULL;
362 +       }
363 +}
365 +static int mt9p031_get_crop(struct v4l2_subdev *sd,
366 +                               struct v4l2_subdev_fh *fh,
367 +                               struct v4l2_subdev_crop *crop)
368 +{
369 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
370 +       struct v4l2_rect *rect = mt9p031_get_pad_crop(mt9p031, fh, crop->pad,
371 +                                                       crop->which);
372 +       if (!rect)
373 +               return -EINVAL;
375 +       crop->rect = *rect;
377 +       return 0;
378 +}
380 +static u16 mt9p031_skip_for_crop(s32 source, s32 *target, s32 max_skip)
381 +{
382 +       unsigned int skip;
384 +       if (source - source / 4 < *target) {
385 +               *target = source;
386 +               return 1;
387 +       }
389 +       skip = DIV_ROUND_CLOSEST(source, *target);
390 +       if (skip > max_skip)
391 +               skip = max_skip;
392 +       *target = 2 * DIV_ROUND_UP(source, 2 * skip);
394 +       return skip;
395 +}
397 +static int mt9p031_set_params(struct i2c_client *client,
398 +                             struct v4l2_rect *rect, u16 xskip, u16 yskip)
399 +{
400 +       struct mt9p031 *mt9p031 = to_mt9p031(client);
401 +       int ret;
402 +       u16 xbin, ybin;
403 +       const u16 hblank = MT9P031_H_BLANKING_VALUE,
404 +               vblank = MT9P031_V_BLANKING_VALUE;
405 +       __s32 left, top, width, height;
407 +       /*
408 +        * TODO: Attention! When implementing horizontal flipping, adjust
409 +        * alignment according to R2 "Column Start" description in the datasheet
410 +        */
411 +       if (xskip & 1) {
412 +               xbin = 1;
413 +               left = rect->left & (~3);
414 +       } else if (xskip & 2) {
415 +               xbin = 2;
416 +               left = rect->left & (~7);
417 +       } else {
418 +               xbin = 4;
419 +               left = rect->left & (~15);
420 +       }
421 +       top = rect->top & (~1);
422 +       width = rect->width;
423 +       height = rect->height;
425 +       ybin = min(yskip, (u16)4);
427 +       /* Disable register update, reconfigure atomically */
428 +       ret = mt9p031_set_output_control(mt9p031, 0,
429 +                                       MT9P031_OUTPUT_CONTROL_SYN);
430 +       if (ret < 0)
431 +               return ret;
433 +       dev_dbg(&client->dev, "skip %u:%u, rect %ux%u@%u:%u\n",
434 +               xskip, yskip, rect->width, rect->height, rect->left, rect->top);
436 +       /* Blanking and start values - default... */
437 +       ret = reg_write_cached(client, MT9P031_H_BLANKING, hblank,
438 +                               &mt9p031->h_blanking);
439 +       if (ret < 0)
440 +               return ret;
441 +       ret = reg_write_cached(client, MT9P031_V_BLANKING, vblank,
442 +                               &mt9p031->v_blanking);
443 +       if (ret < 0)
444 +               return ret;
446 +       ret = reg_write_cached(client, MT9P031_COLUMN_ADDRESS_MODE,
447 +                               ((xbin - 1) << 4) | (xskip - 1),
448 +                               &mt9p031->column_address_mode);
449 +       if (ret < 0)
450 +               return ret;
451 +       ret = reg_write_cached(client, MT9P031_ROW_ADDRESS_MODE,
452 +                               ((ybin - 1) << 4) | (yskip - 1),
453 +                               &mt9p031->row_address_mode);
454 +       if (ret < 0)
455 +               return ret;
457 +       dev_dbg(&client->dev, "new physical left %u, top %u\n",
458 +               rect->left, rect->top);
460 +       ret = reg_write_cached(client, MT9P031_COLUMN_START,
461 +                               rect->left + MT9P031_COLUMN_START_DEF,
462 +                               &mt9p031->column_start);
463 +       if (ret < 0)
464 +               return ret;
465 +       ret = reg_write_cached(client, MT9P031_ROW_START,
466 +                               rect->top + MT9P031_ROW_START_DEF,
467 +                               &mt9p031->row_start);
468 +       if (ret < 0)
469 +               return ret;
470 +       ret = reg_write_cached(client, MT9P031_WINDOW_WIDTH,
471 +                               rect->width - 1,
472 +                               &mt9p031->window_width);
473 +       if (ret < 0)
474 +               return ret;
475 +       ret = reg_write_cached(client, MT9P031_WINDOW_HEIGHT,
476 +                               rect->height - 1,
477 +                               &mt9p031->window_height);
478 +       if (ret < 0)
479 +               return ret;
481 +       /* Re-enable register update, commit all changes */
482 +       ret = mt9p031_set_output_control(mt9p031,
483 +                                       MT9P031_OUTPUT_CONTROL_SYN, 0);
484 +       if (ret < 0)
485 +               return ret;
487 +       mt9p031->xskip = xskip;
488 +       mt9p031->yskip = yskip;
489 +       return ret;
490 +}
492 +static int mt9p031_set_crop(struct v4l2_subdev *sd,
493 +                               struct v4l2_subdev_fh *fh,
494 +                               struct v4l2_subdev_crop *crop)
495 +{
496 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
497 +       struct v4l2_mbus_framefmt *f;
498 +       struct v4l2_rect *c;
499 +       struct v4l2_rect rect;
500 +       u16 xskip, yskip;
501 +       s32 width, height;
503 +       dev_dbg(mt9p031->subdev.v4l2_dev->dev, "%s(%ux%u@%u:%u : %u)\n",
504 +                       __func__, crop->rect.width, crop->rect.height,
505 +                       crop->rect.left, crop->rect.top, crop->which);
507 +       /*
508 +        * Clamp the crop rectangle boundaries and align them to a multiple of 2
509 +        * pixels.
510 +        */
511 +       rect.width = ALIGN(clamp(crop->rect.width,
512 +                                MT9P031_WINDOW_WIDTH_MIN, MT9P031_WINDOW_WIDTH_MAX), 2);
513 +       rect.height = ALIGN(clamp(crop->rect.height,
514 +                                 MT9P031_WINDOW_HEIGHT_MIN, MT9P031_WINDOW_HEIGHT_MAX), 2);
515 +       rect.left = ALIGN(clamp(crop->rect.left,
516 +                               0, MT9P031_WINDOW_WIDTH_MAX - rect.width), 2);
517 +       rect.top = ALIGN(clamp(crop->rect.top,
518 +                              0, MT9P031_WINDOW_HEIGHT_MAX - rect.height), 2);
520 +       c = mt9p031_get_pad_crop(mt9p031, fh, crop->pad, crop->which);
522 +       if (rect.width != c->width || rect.height != c->height) {
523 +               /*
524 +                * Reset the output image size if the crop rectangle size has
525 +                * been modified.
526 +                */
527 +               f = mt9p031_get_pad_format(mt9p031, fh, crop->pad,
528 +                                                   crop->which);
529 +               width = f->width;
530 +               height = f->height;
532 +               xskip = mt9p031_skip_for_crop(rect.width, &width, 7);
533 +               yskip = mt9p031_skip_for_crop(rect.height, &height, 8);
534 +       } else {
535 +               xskip = mt9p031->xskip;
536 +               yskip = mt9p031->yskip;
537 +               f = NULL;
538 +       }
539 +       if (f) {
540 +               f->width = width;
541 +               f->height = height;
542 +       }
544 +       *c = rect;
545 +       crop->rect = rect;
547 +       mt9p031->xskip = xskip;
548 +       mt9p031->yskip = yskip;
549 +       mt9p031->rect = *c;
550 +       return 0;
551 +}
553 +static int mt9p031_get_format(struct v4l2_subdev *sd,
554 +                               struct v4l2_subdev_fh *fh,
555 +                               struct v4l2_subdev_format *fmt)
556 +{
557 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
559 +       fmt->format =
560 +               *mt9p031_get_pad_format(mt9p031, fh, fmt->pad, fmt->which);
561 +       return 0;
562 +}
564 +static u16 mt9p031_skip_for_scale(s32 *source, s32 target,
565 +                                       s32 max_skip, s32 max)
566 +{
567 +       unsigned int skip;
569 +       if (*source - *source / 4 < target) {
570 +               *source = target;
571 +               return 1;
572 +       }
574 +       skip = min(max, *source + target / 2) / target;
575 +       if (skip > max_skip)
576 +               skip = max_skip;
577 +       *source = target * skip;
579 +       return skip;
580 +}
582 +static int mt9p031_fmt_validate(struct v4l2_subdev *sd,
583 +                               struct v4l2_subdev_format *fmt)
584 +{
585 +       struct v4l2_mbus_framefmt *format = &fmt->format;
587 +       /* Hardcode code and colorspace as sensor only supports one */
588 +       format->code = V4L2_MBUS_FMT_SGRBG12_1X12;
589 +       format->colorspace = V4L2_COLORSPACE_SRGB;
591 +       format->width = clamp_t(int, ALIGN(format->width, 2), 2,
592 +                                               MT9P031_WINDOW_WIDTH_MAX);
593 +       format->height = clamp_t(int, ALIGN(format->height, 2), 2,
594 +                                               MT9P031_WINDOW_HEIGHT_MAX);
595 +       format->field = V4L2_FIELD_NONE;
597 +       return 0;
598 +}
600 +static int mt9p031_set_format(struct v4l2_subdev *sd,
601 +                               struct v4l2_subdev_fh *fh,
602 +                               struct v4l2_subdev_format *format)
603 +{
604 +       struct v4l2_subdev_format sdf = *format;
605 +       struct v4l2_mbus_framefmt *__format, *format_bak = &sdf.format;
606 +       struct v4l2_rect *__crop, rect;
607 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
608 +       u16 xskip, yskip;
609 +       int ret;
611 +       __crop = mt9p031_get_pad_crop(mt9p031, fh, format->pad, format->which);
613 +       ret = mt9p031_fmt_validate(sd, &sdf);
614 +       if (ret < 0)
615 +               return ret;
616 +       rect.width = __crop->width;
617 +       rect.height = __crop->height;
619 +       xskip = mt9p031_skip_for_scale(&rect.width, format_bak->width, 7,
620 +                                      MT9P031_WINDOW_WIDTH_MAX);
621 +       if (rect.width + __crop->left > MT9P031_WINDOW_WIDTH_MAX)
622 +               rect.left = (MT9P031_WINDOW_WIDTH_MAX - rect.width) / 2;
623 +       else
624 +               rect.left = __crop->left;
625 +       yskip = mt9p031_skip_for_scale(&rect.height, format_bak->height, 8,
626 +                                      MT9P031_WINDOW_HEIGHT_MAX);
627 +       if (rect.height + __crop->top > MT9P031_WINDOW_HEIGHT_MAX)
628 +               rect.top = (MT9P031_WINDOW_HEIGHT_MAX - rect.height) / 2;
629 +       else
630 +               rect.top = __crop->top;
632 +       dev_dbg(mt9p031->subdev.v4l2_dev->dev, "%s(%ux%u : %u)\n", __func__,
633 +               format_bak->width, format_bak->height, format->which);
634 +       if (__crop)
635 +               *__crop = rect;
637 +       __format = mt9p031_get_pad_format(mt9p031, fh, format->pad, format->which);
638 +       *__format = *format_bak;
639 +       format->format = *format_bak;
641 +       mt9p031->xskip = xskip;
642 +       mt9p031->yskip = yskip;
643 +       mt9p031->rect = *__crop;
644 +       return 0;
645 +}
647 +static int mt9p031_s_stream(struct v4l2_subdev *sd, int enable)
648 +{
649 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
650 +       struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
651 +       struct v4l2_rect rect = mt9p031->rect;
652 +       u16 xskip = mt9p031->xskip;
653 +       u16 yskip = mt9p031->yskip;
654 +       int ret;
656 +       if (enable) {
657 +               ret = mt9p031_set_params(client, &rect, xskip, yskip);
658 +               if (ret < 0)
659 +                       return ret;
660 +               /* Switch to master "normal" mode */
661 +               ret = mt9p031_set_output_control(mt9p031, 0,
662 +                                               MT9P031_OUTPUT_CONTROL_CEN);
663 +       } else {
664 +               /* Stop sensor readout */
665 +               ret = mt9p031_set_output_control(mt9p031,
666 +                                               MT9P031_OUTPUT_CONTROL_CEN, 0);
667 +       }
668 +       return ret;
669 +}
671 +static int mt9p031_video_probe(struct i2c_client *client)
672 +{
673 +       s32 data;
675 +       /* Read out the chip version register */
676 +       data = reg_read(client, MT9P031_CHIP_VERSION);
677 +       if (data != MT9P031_CHIP_VERSION_VALUE) {
678 +               dev_err(&client->dev,
679 +                       "No MT9P031 chip detected, register read %x\n", data);
680 +               return -ENODEV;
681 +       }
683 +       dev_info(&client->dev, "Detected a MT9P031 chip ID %x\n", data);
685 +       return 0;
686 +}
688 +static int mt9p031_set_power(struct v4l2_subdev *sd, int on)
689 +{
690 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
691 +       int ret = 0;
693 +       mutex_lock(&mt9p031->power_lock);
695 +       /*
696 +        * If the power count is modified from 0 to != 0 or from != 0 to 0,
697 +        * update the power state.
698 +        */
699 +       if (mt9p031->power_count == !on) {
700 +               if (on) {
701 +                       ret = mt9p031_power_on(mt9p031);
702 +                       if (ret) {
703 +                               dev_err(mt9p031->subdev.v4l2_dev->dev,
704 +                               "Failed to enable 2.8v regulator: %d\n", ret);
705 +                               goto out;
706 +                       }
707 +               } else {
708 +                       mt9p031_power_off(mt9p031);
709 +               }
710 +       }
712 +       /* Update the power count. */
713 +       mt9p031->power_count += on ? 1 : -1;
714 +       WARN_ON(mt9p031->power_count < 0);
716 +out:
717 +       mutex_unlock(&mt9p031->power_lock);
718 +       return ret;
719 +}
721 +static int mt9p031_registered(struct v4l2_subdev *sd)
722 +{
723 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
724 +       struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
725 +       int ret;
727 +       ret = mt9p031_set_power(&mt9p031->subdev, 1);
728 +       if (ret) {
729 +               dev_err(&client->dev,
730 +                       "Failed to power on device: %d\n", ret);
731 +               goto err_pwron;
732 +       }
734 +       ret = mt9p031_video_probe(client);
735 +       if (ret)
736 +               goto err_evprobe;
738 +       mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
739 +       ret = media_entity_init(&mt9p031->subdev.entity, 1, &mt9p031->pad, 0);
740 +       if (ret)
741 +               goto err_evprobe;
743 +       mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
744 +       mt9p031_set_power(&mt9p031->subdev, 0);
746 +       return 0;
747 +err_evprobe:
748 +       mt9p031_set_power(&mt9p031->subdev, 0);
749 +err_pwron:
750 +       return ret;
751 +}
753 +static int mt9p031_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
754 +{
755 +       struct mt9p031 *mt9p031;
756 +       mt9p031 = container_of(sd, struct mt9p031, subdev);
758 +       mt9p031->rect.width     = MT9P031_WINDOW_WIDTH_MAX;
759 +       mt9p031->rect.height    = MT9P031_WINDOW_HEIGHT_MAX;
760 +       mt9p031->rect.left      = MT9P031_COLUMN_START_DEF;
761 +       mt9p031->rect.top       = MT9P031_ROW_START_DEF;
763 +       mt9p031->format.code = V4L2_MBUS_FMT_SGRBG12_1X12;
764 +       mt9p031->format.width = MT9P031_WINDOW_WIDTH_MAX;
765 +       mt9p031->format.height = MT9P031_WINDOW_HEIGHT_MAX;
766 +       mt9p031->format.field = V4L2_FIELD_NONE;
767 +       mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
769 +       mt9p031->xskip = 1;
770 +       mt9p031->yskip = 1;
771 +       return mt9p031_set_power(sd, 1);
772 +}
774 +static int mt9p031_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
775 +{
776 +       return mt9p031_set_power(sd, 0);
777 +}
779 +static struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
780 +       .s_power        = mt9p031_set_power,
781 +};
783 +static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
784 +       .s_stream       = mt9p031_s_stream,
785 +};
787 +static struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
788 +       .enum_mbus_code = mt9p031_enum_mbus_code,
789 +       .get_fmt = mt9p031_get_format,
790 +       .set_fmt = mt9p031_set_format,
791 +       .get_crop = mt9p031_get_crop,
792 +       .set_crop = mt9p031_set_crop,
793 +};
795 +static struct v4l2_subdev_ops mt9p031_subdev_ops = {
796 +       .core   = &mt9p031_subdev_core_ops,
797 +       .video  = &mt9p031_subdev_video_ops,
798 +       .pad    = &mt9p031_subdev_pad_ops,
799 +};
801 +static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
802 +       .registered = mt9p031_registered,
803 +       .open = mt9p031_open,
804 +       .close = mt9p031_close,
805 +};
807 +static int mt9p031_probe(struct i2c_client *client,
808 +                        const struct i2c_device_id *did)
809 +{
810 +       struct mt9p031 *mt9p031;
811 +       struct mt9p031_platform_data *pdata = client->dev.platform_data;
812 +       struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
813 +       int ret;
815 +       if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
816 +               dev_warn(&adapter->dev,
817 +                        "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
818 +               return -EIO;
819 +       }
821 +       mt9p031 = kzalloc(sizeof(struct mt9p031), GFP_KERNEL);
822 +       if (!mt9p031)
823 +               return -ENOMEM;
825 +       mutex_init(&mt9p031->power_lock);
826 +       v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
827 +       mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
829 +       mt9p031->pdata          = pdata;
831 +       mt9p031->reg_1v8 = regulator_get(NULL, "cam_1v8");
832 +       if (IS_ERR(mt9p031->reg_1v8)) {
833 +               ret = PTR_ERR(mt9p031->reg_1v8);
834 +               dev_err(mt9p031->subdev.v4l2_dev->dev,
835 +                       "Failed 1.8v regulator: %d\n", ret);
836 +               goto err_e1v8;
837 +       }
839 +       mt9p031->reg_2v8 = regulator_get(NULL, "cam_2v8");
840 +       if (IS_ERR(mt9p031->reg_2v8)) {
841 +               ret = PTR_ERR(mt9p031->reg_2v8);
842 +               dev_err(mt9p031->subdev.v4l2_dev->dev,
843 +                       "Failed 2.8v regulator: %d\n", ret);
844 +               goto err_e2v8;
845 +       }
846 +       return 0;
847 +err_e2v8:
848 +       regulator_put(mt9p031->reg_1v8);
849 +err_e1v8:
850 +       kfree(mt9p031);
851 +       return ret;
852 +}
854 +static int mt9p031_remove(struct i2c_client *client)
855 +{
856 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
857 +       struct mt9p031 *mt9p031 = container_of(sd, struct mt9p031, subdev);
859 +       v4l2_device_unregister_subdev(sd);
860 +       media_entity_cleanup(&sd->entity);
861 +       regulator_put(mt9p031->reg_2v8);
862 +       regulator_put(mt9p031->reg_1v8);
863 +       kfree(mt9p031);
865 +       return 0;
866 +}
868 +static const struct i2c_device_id mt9p031_id[] = {
869 +       { "mt9p031", 0 },
870 +       { }
871 +};
872 +MODULE_DEVICE_TABLE(i2c, mt9p031_id);
874 +static struct i2c_driver mt9p031_i2c_driver = {
875 +       .driver = {
876 +               .name = "mt9p031",
877 +       },
878 +       .probe          = mt9p031_probe,
879 +       .remove         = mt9p031_remove,
880 +       .id_table       = mt9p031_id,
881 +};
883 +static int __init mt9p031_mod_init(void)
884 +{
885 +       return i2c_add_driver(&mt9p031_i2c_driver);
886 +}
888 +static void __exit mt9p031_mod_exit(void)
889 +{
890 +       i2c_del_driver(&mt9p031_i2c_driver);
891 +}
893 +module_init(mt9p031_mod_init);
894 +module_exit(mt9p031_mod_exit);
896 +MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
897 +MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
898 +MODULE_LICENSE("GPL v2");
899 diff --git a/include/media/mt9p031.h b/include/media/mt9p031.h
900 new file mode 100644
901 index 0000000..7ee9733
902 --- /dev/null
903 +++ b/include/media/mt9p031.h
904 @@ -0,0 +1,11 @@
905 +#ifndef MT9P031_H
906 +#define MT9P031_H
908 +struct v4l2_subdev;
910 +struct mt9p031_platform_data {
911 +       int (*set_xclk)(struct v4l2_subdev *subdev, int hz);
912 +       int (*reset)(struct v4l2_subdev *subdev, int active);
913 +};
915 +#endif
916 -- 
917 1.6.6.1