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.
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
120 +
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
125 +
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 +};
149 +
150 +static struct mt9p031 *to_mt9p031(const struct i2c_client *client)
151 +{
152 + return container_of(i2c_get_clientdata(client), struct mt9p031, subdev);
153 +}
154 +
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 +}
160 +
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 +}
166 +
167 +static int reg_write_cached(struct i2c_client *client, const u8 reg,
168 + const u16 data, u16 *cache)
169 +{
170 + int ret;
171 +
172 + ret = reg_write(client, reg, data);
173 + if (ret < 0)
174 + return ret;
175 + *cache = data;
176 + return 0;
177 +}
178 +
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;
184 +
185 + return reg_write_cached(client, MT9P031_OUTPUT_CONTROL, value,
186 + &mt9p031->output_control);
187 +}
188 +
189 +static int restore_registers(struct i2c_client *client)
190 +{
191 + int ret;
192 + struct mt9p031 *mt9p031 = to_mt9p031(client);
193 +
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;
199 +
200 + /* Blanking and start values - default... */
201 + ret = reg_write(client, MT9P031_H_BLANKING, mt9p031->h_blanking);
202 + if (ret < 0)
203 + return ret;
204 +
205 + ret = reg_write(client, MT9P031_V_BLANKING, mt9p031->v_blanking);
206 + if (ret < 0)
207 + return ret;
208 +
209 + ret = reg_write(client, MT9P031_COLUMN_ADDRESS_MODE,
210 + mt9p031->column_address_mode);
211 + if (ret < 0)
212 + return ret;
213 +
214 + ret = reg_write(client, MT9P031_ROW_ADDRESS_MODE,
215 + mt9p031->row_address_mode);
216 + if (ret < 0)
217 + return ret;
218 +
219 + ret = reg_write(client, MT9P031_COLUMN_START,
220 + mt9p031->column_start);
221 + if (ret < 0)
222 + return ret;
223 +
224 + ret = reg_write(client, MT9P031_ROW_START,
225 + mt9p031->row_start);
226 + if (ret < 0)
227 + return ret;
228 +
229 + ret = reg_write(client, MT9P031_WINDOW_WIDTH,
230 + mt9p031->window_width);
231 + if (ret < 0)
232 + return ret;
233 +
234 + ret = reg_write(client, MT9P031_WINDOW_HEIGHT,
235 + mt9p031->window_height);
236 + if (ret < 0)
237 + return ret;
238 +
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 +}
246 +
247 +static int mt9p031_reset(struct i2c_client *client)
248 +{
249 + struct mt9p031 *mt9p031 = to_mt9p031(client);
250 + int ret;
251 +
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 +}
262 +
263 +static int mt9p031_power_on(struct mt9p031 *mt9p031)
264 +{
265 + struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
266 + int ret;
267 +
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);
288 +
289 + if (mt9p031->pdata->set_xclk)
290 + mt9p031->pdata->set_xclk(&mt9p031->subdev, MT9P031_PIXCLK_FREQ);
291 +
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 + }
298 +
299 + ret = restore_registers(client);
300 + if (ret < 0) {
301 + dev_err(&client->dev, "Failed to restore registers\n");
302 + goto err_rst;
303 + }
304 +
305 + return 0;
306 +err_rst:
307 + regulator_disable(mt9p031->reg_1v8);
308 +err_1v8:
309 + return ret;
310 +
311 +}
312 +
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 +}
322 +
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);
328 +
329 + if (code->pad || code->index)
330 + return -EINVAL;
331 +
332 + code->code = mt9p031->format.code;
333 +
334 + return 0;
335 +}
336 +
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 +}
351 +
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 +}
364 +
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;
374 +
375 + crop->rect = *rect;
376 +
377 + return 0;
378 +}
379 +
380 +static u16 mt9p031_skip_for_crop(s32 source, s32 *target, s32 max_skip)
381 +{
382 + unsigned int skip;
383 +
384 + if (source - source / 4 < *target) {
385 + *target = source;
386 + return 1;
387 + }
388 +
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);
393 +
394 + return skip;
395 +}
396 +
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;
406 +
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;
424 +
425 + ybin = min(yskip, (u16)4);
426 +
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;
432 +
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);
435 +
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;
445 +
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;
456 +
457 + dev_dbg(&client->dev, "new physical left %u, top %u\n",
458 + rect->left, rect->top);
459 +
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;
480 +
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;
486 +
487 + mt9p031->xskip = xskip;
488 + mt9p031->yskip = yskip;
489 + return ret;
490 +}
491 +
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;
502 +
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);
506 +
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);
519 +
520 + c = mt9p031_get_pad_crop(mt9p031, fh, crop->pad, crop->which);
521 +
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;
531 +
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 + }
543 +
544 + *c = rect;
545 + crop->rect = rect;
546 +
547 + mt9p031->xskip = xskip;
548 + mt9p031->yskip = yskip;
549 + mt9p031->rect = *c;
550 + return 0;
551 +}
552 +
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);
558 +
559 + fmt->format =
560 + *mt9p031_get_pad_format(mt9p031, fh, fmt->pad, fmt->which);
561 + return 0;
562 +}
563 +
564 +static u16 mt9p031_skip_for_scale(s32 *source, s32 target,
565 + s32 max_skip, s32 max)
566 +{
567 + unsigned int skip;
568 +
569 + if (*source - *source / 4 < target) {
570 + *source = target;
571 + return 1;
572 + }
573 +
574 + skip = min(max, *source + target / 2) / target;
575 + if (skip > max_skip)
576 + skip = max_skip;
577 + *source = target * skip;
578 +
579 + return skip;
580 +}
581 +
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;
586 +
587 + /* Hardcode code and colorspace as sensor only supports one */
588 + format->code = V4L2_MBUS_FMT_SGRBG12_1X12;
589 + format->colorspace = V4L2_COLORSPACE_SRGB;
590 +
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;
596 +
597 + return 0;
598 +}
599 +
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;
610 +
611 + __crop = mt9p031_get_pad_crop(mt9p031, fh, format->pad, format->which);
612 +
613 + ret = mt9p031_fmt_validate(sd, &sdf);
614 + if (ret < 0)
615 + return ret;
616 + rect.width = __crop->width;
617 + rect.height = __crop->height;
618 +
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;
631 +
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;
636 +
637 + __format = mt9p031_get_pad_format(mt9p031, fh, format->pad, format->which);
638 + *__format = *format_bak;
639 + format->format = *format_bak;
640 +
641 + mt9p031->xskip = xskip;
642 + mt9p031->yskip = yskip;
643 + mt9p031->rect = *__crop;
644 + return 0;
645 +}
646 +
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;
655 +
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 +}
670 +
671 +static int mt9p031_video_probe(struct i2c_client *client)
672 +{
673 + s32 data;
674 +
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 + }
682 +
683 + dev_info(&client->dev, "Detected a MT9P031 chip ID %x\n", data);
684 +
685 + return 0;
686 +}
687 +
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;
692 +
693 + mutex_lock(&mt9p031->power_lock);
694 +
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 + }
711 +
712 + /* Update the power count. */
713 + mt9p031->power_count += on ? 1 : -1;
714 + WARN_ON(mt9p031->power_count < 0);
715 +
716 +out:
717 + mutex_unlock(&mt9p031->power_lock);
718 + return ret;
719 +}
720 +
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;
726 +
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 + }
733 +
734 + ret = mt9p031_video_probe(client);
735 + if (ret)
736 + goto err_evprobe;
737 +
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;
742 +
743 + mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
744 + mt9p031_set_power(&mt9p031->subdev, 0);
745 +
746 + return 0;
747 +err_evprobe:
748 + mt9p031_set_power(&mt9p031->subdev, 0);
749 +err_pwron:
750 + return ret;
751 +}
752 +
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);
757 +
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;
762 +
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;
768 +
769 + mt9p031->xskip = 1;
770 + mt9p031->yskip = 1;
771 + return mt9p031_set_power(sd, 1);
772 +}
773 +
774 +static int mt9p031_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
775 +{
776 + return mt9p031_set_power(sd, 0);
777 +}
778 +
779 +static struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
780 + .s_power = mt9p031_set_power,
781 +};
782 +
783 +static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
784 + .s_stream = mt9p031_s_stream,
785 +};
786 +
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 +};
794 +
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 +};
800 +
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 +};
806 +
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;
814 +
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 + }
820 +
821 + mt9p031 = kzalloc(sizeof(struct mt9p031), GFP_KERNEL);
822 + if (!mt9p031)
823 + return -ENOMEM;
824 +
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;
828 +
829 + mt9p031->pdata = pdata;
830 +
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 + }
838 +
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 +}
853 +
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);
858 +
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);
864 +
865 + return 0;
866 +}
867 +
868 +static const struct i2c_device_id mt9p031_id[] = {
869 + { "mt9p031", 0 },
870 + { }
871 +};
872 +MODULE_DEVICE_TABLE(i2c, mt9p031_id);
873 +
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 +};
882 +
883 +static int __init mt9p031_mod_init(void)
884 +{
885 + return i2c_add_driver(&mt9p031_i2c_driver);
886 +}
887 +
888 +static void __exit mt9p031_mod_exit(void)
889 +{
890 + i2c_del_driver(&mt9p031_i2c_driver);
891 +}
892 +
893 +module_init(mt9p031_mod_init);
894 +module_exit(mt9p031_mod_exit);
895 +
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
907 +
908 +struct v4l2_subdev;
909 +
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 +};
914 +
915 +#endif
916 --
917 1.6.6.1