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.
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
125 +
126 +
127 +
128 +
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 +};
142 +
143 +static struct mt9p031 *to_mt9p031(const struct i2c_client *client)
144 +{
145 + return container_of(i2c_get_clientdata(client), struct mt9p031, subdev);
146 +}
147 +
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 +}
153 +
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 +}
159 +
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;
166 +
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 +}
173 +
174 +static int mt9p031_reset(struct i2c_client *client)
175 +{
176 + struct mt9p031 *mt9p031 = to_mt9p031(client);
177 + int ret;
178 +
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 +}
189 +
190 +static int mt9p031_power_on(struct mt9p031 *mt9p031)
191 +{
192 + struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
193 + int ret;
194 +
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 +}
216 +
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 +}
226 +
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);
232 +
233 + if (code->pad || code->index)
234 + return -EINVAL;
235 +
236 + code->code = mt9p031->format.code;
237 + return 0;
238 +}
239 +
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 +}
254 +
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 +}
267 +
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;
277 +
278 + crop->rect = *rect;
279 +
280 + return 0;
281 +}
282 +
283 +static u16 mt9p031_skip_for_crop(s32 source, s32 *target, s32 max_skip)
284 +{
285 + unsigned int skip;
286 +
287 + if (source - source / 4 < *target) {
288 + *target = source;
289 + return 1;
290 + }
291 +
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);
296 +
297 + return skip;
298 +}
299 +
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;
309 +
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);
325 +
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;
331 +
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);
334 +
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;
342 +
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;
351 +
352 + dev_dbg(&client->dev, "new physical left %u, top %u\n",
353 + rect->left, rect->top);
354 +
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;
363 +
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;
372 +
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;
378 +
379 + mt9p031->xskip = xskip;
380 + mt9p031->yskip = yskip;
381 + return ret;
382 +}
383 +
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;
394 +
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);
398 +
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);
415 +
416 + c = mt9p031_get_pad_crop(mt9p031, fh, crop->pad, crop->which);
417 +
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;
427 +
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 + }
439 +
440 + *c = rect;
441 + crop->rect = rect;
442 +
443 + mt9p031->xskip = xskip;
444 + mt9p031->yskip = yskip;
445 + mt9p031->rect = *c;
446 + return 0;
447 +}
448 +
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);
454 +
455 + fmt->format =
456 + *mt9p031_get_pad_format(mt9p031, fh, fmt->pad, fmt->which);
457 + return 0;
458 +}
459 +
460 +static u16 mt9p031_skip_for_scale(s32 *source, s32 target,
461 + s32 max_skip, s32 max)
462 +{
463 + unsigned int skip;
464 +
465 + if (*source - *source / 4 < target) {
466 + *source = target;
467 + return 1;
468 + }
469 +
470 + skip = min(max, *source + target / 2) / target;
471 + if (skip > max_skip)
472 + skip = max_skip;
473 + *source = target * skip;
474 +
475 + return skip;
476 +}
477 +
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;
488 +
489 + __crop = mt9p031_get_pad_crop(mt9p031, fh, format->pad, format->which);
490 +
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);
495 +
496 + rect.width = __crop->width;
497 + rect.height = __crop->height;
498 +
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;
511 +
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;
516 +
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;
522 +
523 + mt9p031->xskip = xskip;
524 + mt9p031->yskip = yskip;
525 + mt9p031->rect = *__crop;
526 + return 0;
527 +}
528 +
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;
537 +
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 +}
552 +
553 +static int mt9p031_video_probe(struct i2c_client *client)
554 +{
555 + s32 data;
556 +
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 + }
564 +
565 + dev_info(&client->dev, "Detected a MT9P031 chip ID %x\n", data);
566 +
567 + return 0;
568 +}
569 +
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;
574 +
575 + mutex_lock(&mt9p031->power_lock);
576 +
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 + }
593 +
594 + /* Update the power count. */
595 + mt9p031->power_count += on ? 1 : -1;
596 + WARN_ON(mt9p031->power_count < 0);
597 +
598 +out:
599 + mutex_unlock(&mt9p031->power_lock);
600 + return ret;
601 +}
602 +
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;
608 +
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 + }
615 +
616 + ret = mt9p031_video_probe(client);
617 +
618 + mt9p031_set_power(&mt9p031->subdev, 0);
619 +
620 + return ret;
621 +}
622 +
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);
627 +
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;
632 +
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;
638 +
639 + mt9p031->xskip = 1;
640 + mt9p031->yskip = 1;
641 + return mt9p031_set_power(sd, 1);
642 +}
643 +
644 +static int mt9p031_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
645 +{
646 + return mt9p031_set_power(sd, 0);
647 +}
648 +
649 +static struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
650 + .s_power = mt9p031_set_power,
651 +};
652 +
653 +static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
654 + .s_stream = mt9p031_s_stream,
655 +};
656 +
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 +};
664 +
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 +};
670 +
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 +};
676 +
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);
684 +
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 + }
690 +
691 + mt9p031 = kzalloc(sizeof(struct mt9p031), GFP_KERNEL);
692 + if (!mt9p031)
693 + return -ENOMEM;
694 +
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;
698 +
699 + mt9p031->pdata = pdata;
700 +
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;
705 +
706 + mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
707 +
708 + return 0;
709 +}
710 +
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);
715 +
716 + v4l2_device_unregister_subdev(sd);
717 + media_entity_cleanup(&sd->entity);
718 + kfree(mt9p031);
719 +
720 + return 0;
721 +}
722 +
723 +static const struct i2c_device_id mt9p031_id[] = {
724 + { "mt9p031", 0 },
725 + { }
726 +};
727 +MODULE_DEVICE_TABLE(i2c, mt9p031_id);
728 +
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 +};
737 +
738 +static int __init mt9p031_mod_init(void)
739 +{
740 + return i2c_add_driver(&mt9p031_i2c_driver);
741 +}
742 +
743 +static void __exit mt9p031_mod_exit(void)
744 +{
745 + i2c_del_driver(&mt9p031_i2c_driver);
746 +}
747 +
748 +module_init(mt9p031_mod_init);
749 +module_exit(mt9p031_mod_exit);
750 +
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
762 +
763 +struct v4l2_subdev;
764 +
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 +};
769 +
770 +#endif
771 --
772 1.6.6.1