1 /*
2 * Driver for MT9P031 CMOS Image Sensor from Aptina
3 *
4 * Copyright (C) 2011, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
5 * Copyright (C) 2011, Javier Martin <javier.martin@vista-silicon.com>
6 * Copyright (C) 2011, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
7 *
8 * Based on the MT9V032 driver and Bastian Hecht's code.
9 *
10 * This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License version 2 as
12 * published by the Free Software Foundation.
13 */
14
15 #include <linux/delay.h>
16 #include <linux/device.h>
17 #include <linux/module.h>
18 #include <linux/i2c.h>
19 #include <linux/log2.h>
20 #include <linux/pm.h>
21 #include <linux/slab.h>
22 #include <media/v4l2-subdev.h>
23 #include <linux/videodev2.h>
24
25 #include <media/mt9p031.h>
26 #include <media/v4l2-chip-ident.h>
27 #include <media/v4l2-ctrls.h>
28 #include <media/v4l2-device.h>
29 #include <media/v4l2-subdev.h>
30
31 #define MT9P031_PIXEL_ARRAY_WIDTH 2752
32 #define MT9P031_PIXEL_ARRAY_HEIGHT 2004
33
34 #define MT9P031_CHIP_VERSION 0x00
35 #define MT9P031_CHIP_VERSION_VALUE 0x1801
36 #define MT9P031_ROW_START 0x01
37 #define MT9P031_ROW_START_MIN 0
38 #define MT9P031_ROW_START_MAX 2004
39 #define MT9P031_ROW_START_DEF 54
40 #define MT9P031_COLUMN_START 0x02
41 #define MT9P031_COLUMN_START_MIN 0
42 #define MT9P031_COLUMN_START_MAX 2750
43 #define MT9P031_COLUMN_START_DEF 16
44 #define MT9P031_WINDOW_HEIGHT 0x03
45 #define MT9P031_WINDOW_HEIGHT_MIN 2
46 #define MT9P031_WINDOW_HEIGHT_MAX 2006
47 #define MT9P031_WINDOW_HEIGHT_DEF 1944
48 #define MT9P031_WINDOW_WIDTH 0x04
49 #define MT9P031_WINDOW_WIDTH_MIN 2
50 #define MT9P031_WINDOW_WIDTH_MAX 2752
51 #define MT9P031_WINDOW_WIDTH_DEF 2592
52 #define MT9P031_HORIZONTAL_BLANK 0x05
53 #define MT9P031_HORIZONTAL_BLANK_MIN 0
54 #define MT9P031_HORIZONTAL_BLANK_MAX 4095
55 #define MT9P031_VERTICAL_BLANK 0x06
56 #define MT9P031_VERTICAL_BLANK_MIN 0
57 #define MT9P031_VERTICAL_BLANK_MAX 4095
58 #define MT9P031_VERTICAL_BLANK_DEF 25
59 #define MT9P031_OUTPUT_CONTROL 0x07
60 #define MT9P031_OUTPUT_CONTROL_CEN 2
61 #define MT9P031_OUTPUT_CONTROL_SYN 1
62 #define MT9P031_OUTPUT_CONTROL_DEF 0x1f82
63 #define MT9P031_SHUTTER_WIDTH_UPPER 0x08
64 #define MT9P031_SHUTTER_WIDTH_LOWER 0x09
65 #define MT9P031_SHUTTER_WIDTH_MIN 1
66 #define MT9P031_SHUTTER_WIDTH_MAX 1048575
67 #define MT9P031_SHUTTER_WIDTH_DEF 1943
68 #define MT9P031_PLL_CONTROL 0x10
69 #define MT9P031_PLL_CONTROL_PWROFF 0x0050
70 #define MT9P031_PLL_CONTROL_PWRON 0x0051
71 #define MT9P031_PLL_CONTROL_USEPLL 0x0052
72 #define MT9P031_PLL_CONFIG_1 0x11
73 #define MT9P031_PLL_CONFIG_2 0x12
74 #define MT9P031_PIXEL_CLOCK_CONTROL 0x0a
75 #define MT9P031_FRAME_RESTART 0x0b
76 #define MT9P031_SHUTTER_DELAY 0x0c
77 #define MT9P031_RST 0x0d
78 #define MT9P031_RST_ENABLE 1
79 #define MT9P031_RST_DISABLE 0
80 #define MT9P031_READ_MODE_1 0x1e
81 #define MT9P031_READ_MODE_2 0x20
82 #define MT9P031_READ_MODE_2_ROW_MIR (1 << 15)
83 #define MT9P031_READ_MODE_2_COL_MIR (1 << 14)
84 #define MT9P031_READ_MODE_2_ROW_BLC (1 << 6)
85 #define MT9P031_ROW_ADDRESS_MODE 0x22
86 #define MT9P031_COLUMN_ADDRESS_MODE 0x23
87 #define MT9P031_GLOBAL_GAIN 0x35
88 #define MT9P031_GLOBAL_GAIN_MIN 8
89 #define MT9P031_GLOBAL_GAIN_MAX 1024
90 #define MT9P031_GLOBAL_GAIN_DEF 8
91 #define MT9P031_GLOBAL_GAIN_MULT (1 << 6)
92 #define MT9P031_ROW_BLACK_DEF_OFFSET 0x4b
93 #define MT9P031_TEST_PATTERN 0xa0
94 #define MT9P031_TEST_PATTERN_SHIFT 3
95 #define MT9P031_TEST_PATTERN_ENABLE (1 << 0)
96 #define MT9P031_TEST_PATTERN_DISABLE (0 << 0)
97 #define MT9P031_TEST_PATTERN_GREEN 0xa1
98 #define MT9P031_TEST_PATTERN_RED 0xa2
99 #define MT9P031_TEST_PATTERN_BLUE 0xa3
100
101 struct mt9p031_pll_divs {
102 u32 ext_freq;
103 u32 target_freq;
104 u8 m;
105 u8 n;
106 u8 p1;
107 };
108
109 struct mt9p031 {
110 struct v4l2_subdev subdev;
111 struct media_pad pad;
112 struct v4l2_rect crop; /* Sensor window */
113 struct v4l2_mbus_framefmt format;
114 struct v4l2_ctrl_handler ctrls;
115 struct mt9p031_platform_data *pdata;
116 struct mutex power_lock; /* lock to protect power_count */
117 int power_count;
118 u16 xskip;
119 u16 yskip;
120
121 const struct mt9p031_pll_divs *pll;
122
123 /* Registers cache */
124 u16 output_control;
125 u16 mode2;
126 };
127
to_mt9p031(struct v4l2_subdev * sd)128 static struct mt9p031 *to_mt9p031(struct v4l2_subdev *sd)
129 {
130 return container_of(sd, struct mt9p031, subdev);
131 }
132
mt9p031_read(struct i2c_client * client,u8 reg)133 static int mt9p031_read(struct i2c_client *client, u8 reg)
134 {
135 return i2c_smbus_read_word_swapped(client, reg);
136 }
137
mt9p031_write(struct i2c_client * client,u8 reg,u16 data)138 static int mt9p031_write(struct i2c_client *client, u8 reg, u16 data)
139 {
140 return i2c_smbus_write_word_swapped(client, reg, data);
141 }
142
mt9p031_set_output_control(struct mt9p031 * mt9p031,u16 clear,u16 set)143 static int mt9p031_set_output_control(struct mt9p031 *mt9p031, u16 clear,
144 u16 set)
145 {
146 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
147 u16 value = (mt9p031->output_control & ~clear) | set;
148 int ret;
149
150 ret = mt9p031_write(client, MT9P031_OUTPUT_CONTROL, value);
151 if (ret < 0)
152 return ret;
153
154 mt9p031->output_control = value;
155 return 0;
156 }
157
mt9p031_set_mode2(struct mt9p031 * mt9p031,u16 clear,u16 set)158 static int mt9p031_set_mode2(struct mt9p031 *mt9p031, u16 clear, u16 set)
159 {
160 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
161 u16 value = (mt9p031->mode2 & ~clear) | set;
162 int ret;
163
164 ret = mt9p031_write(client, MT9P031_READ_MODE_2, value);
165 if (ret < 0)
166 return ret;
167
168 mt9p031->mode2 = value;
169 return 0;
170 }
171
mt9p031_reset(struct mt9p031 * mt9p031)172 static int mt9p031_reset(struct mt9p031 *mt9p031)
173 {
174 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
175 int ret;
176
177 /* Disable chip output, synchronous option update */
178 ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_ENABLE);
179 if (ret < 0)
180 return ret;
181 ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_DISABLE);
182 if (ret < 0)
183 return ret;
184
185 return mt9p031_set_output_control(mt9p031, MT9P031_OUTPUT_CONTROL_CEN,
186 0);
187 }
188
189 /*
190 * This static table uses ext_freq and vdd_io values to select suitable
191 * PLL dividers m, n and p1 which have been calculated as specifiec in p36
192 * of Aptina's mt9p031 datasheet. New values should be added here.
193 */
194 static const struct mt9p031_pll_divs mt9p031_divs[] = {
195 /* ext_freq target_freq m n p1 */
196 {21000000, 48000000, 26, 2, 6}
197 };
198
mt9p031_pll_get_divs(struct mt9p031 * mt9p031)199 static int mt9p031_pll_get_divs(struct mt9p031 *mt9p031)
200 {
201 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
202 int i;
203
204 for (i = 0; i < ARRAY_SIZE(mt9p031_divs); i++) {
205 if (mt9p031_divs[i].ext_freq == mt9p031->pdata->ext_freq &&
206 mt9p031_divs[i].target_freq == mt9p031->pdata->target_freq) {
207 mt9p031->pll = &mt9p031_divs[i];
208 return 0;
209 }
210 }
211
212 dev_err(&client->dev, "Couldn't find PLL dividers for ext_freq = %d, "
213 "target_freq = %d\n", mt9p031->pdata->ext_freq,
214 mt9p031->pdata->target_freq);
215 return -EINVAL;
216 }
217
mt9p031_pll_enable(struct mt9p031 * mt9p031)218 static int mt9p031_pll_enable(struct mt9p031 *mt9p031)
219 {
220 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
221 int ret;
222
223 ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
224 MT9P031_PLL_CONTROL_PWRON);
225 if (ret < 0)
226 return ret;
227
228 ret = mt9p031_write(client, MT9P031_PLL_CONFIG_1,
229 (mt9p031->pll->m << 8) | (mt9p031->pll->n - 1));
230 if (ret < 0)
231 return ret;
232
233 ret = mt9p031_write(client, MT9P031_PLL_CONFIG_2, mt9p031->pll->p1 - 1);
234 if (ret < 0)
235 return ret;
236
237 usleep_range(1000, 2000);
238 ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
239 MT9P031_PLL_CONTROL_PWRON |
240 MT9P031_PLL_CONTROL_USEPLL);
241 return ret;
242 }
243
mt9p031_pll_disable(struct mt9p031 * mt9p031)244 static inline int mt9p031_pll_disable(struct mt9p031 *mt9p031)
245 {
246 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
247
248 return mt9p031_write(client, MT9P031_PLL_CONTROL,
249 MT9P031_PLL_CONTROL_PWROFF);
250 }
251
mt9p031_power_on(struct mt9p031 * mt9p031)252 static int mt9p031_power_on(struct mt9p031 *mt9p031)
253 {
254 /* Ensure RESET_BAR is low */
255 if (mt9p031->pdata->reset) {
256 mt9p031->pdata->reset(&mt9p031->subdev, 1);
257 usleep_range(1000, 2000);
258 }
259
260 /* Emable clock */
261 if (mt9p031->pdata->set_xclk)
262 mt9p031->pdata->set_xclk(&mt9p031->subdev,
263 mt9p031->pdata->ext_freq);
264
265 /* Now RESET_BAR must be high */
266 if (mt9p031->pdata->reset) {
267 mt9p031->pdata->reset(&mt9p031->subdev, 0);
268 usleep_range(1000, 2000);
269 }
270
271 return 0;
272 }
273
mt9p031_power_off(struct mt9p031 * mt9p031)274 static void mt9p031_power_off(struct mt9p031 *mt9p031)
275 {
276 if (mt9p031->pdata->reset) {
277 mt9p031->pdata->reset(&mt9p031->subdev, 1);
278 usleep_range(1000, 2000);
279 }
280
281 if (mt9p031->pdata->set_xclk)
282 mt9p031->pdata->set_xclk(&mt9p031->subdev, 0);
283 }
284
__mt9p031_set_power(struct mt9p031 * mt9p031,bool on)285 static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
286 {
287 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
288 int ret;
289
290 if (!on) {
291 mt9p031_power_off(mt9p031);
292 return 0;
293 }
294
295 ret = mt9p031_power_on(mt9p031);
296 if (ret < 0)
297 return ret;
298
299 ret = mt9p031_reset(mt9p031);
300 if (ret < 0) {
301 dev_err(&client->dev, "Failed to reset the camera\n");
302 return ret;
303 }
304
305 return v4l2_ctrl_handler_setup(&mt9p031->ctrls);
306 }
307
308 /* -----------------------------------------------------------------------------
309 * V4L2 subdev video operations
310 */
311
mt9p031_set_params(struct mt9p031 * mt9p031)312 static int mt9p031_set_params(struct mt9p031 *mt9p031)
313 {
314 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
315 struct v4l2_mbus_framefmt *format = &mt9p031->format;
316 const struct v4l2_rect *crop = &mt9p031->crop;
317 unsigned int hblank;
318 unsigned int vblank;
319 unsigned int xskip;
320 unsigned int yskip;
321 unsigned int xbin;
322 unsigned int ybin;
323 int ret;
324
325 /* Windows position and size.
326 *
327 * TODO: Make sure the start coordinates and window size match the
328 * skipping, binning and mirroring (see description of registers 2 and 4
329 * in table 13, and Binning section on page 41).
330 */
331 ret = mt9p031_write(client, MT9P031_COLUMN_START, crop->left);
332 if (ret < 0)
333 return ret;
334 ret = mt9p031_write(client, MT9P031_ROW_START, crop->top);
335 if (ret < 0)
336 return ret;
337 ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, crop->width - 1);
338 if (ret < 0)
339 return ret;
340 ret = mt9p031_write(client, MT9P031_WINDOW_HEIGHT, crop->height - 1);
341 if (ret < 0)
342 return ret;
343
344 /* Row and column binning and skipping. Use the maximum binning value
345 * compatible with the skipping settings.
346 */
347 xskip = DIV_ROUND_CLOSEST(crop->width, format->width);
348 yskip = DIV_ROUND_CLOSEST(crop->height, format->height);
349 xbin = 1 << (ffs(xskip) - 1);
350 ybin = 1 << (ffs(yskip) - 1);
351
352 ret = mt9p031_write(client, MT9P031_COLUMN_ADDRESS_MODE,
353 ((xbin - 1) << 4) | (xskip - 1));
354 if (ret < 0)
355 return ret;
356 ret = mt9p031_write(client, MT9P031_ROW_ADDRESS_MODE,
357 ((ybin - 1) << 4) | (yskip - 1));
358 if (ret < 0)
359 return ret;
360
361 /* Blanking - use minimum value for horizontal blanking and default
362 * value for vertical blanking.
363 */
364 hblank = 346 * ybin + 64 + (80 >> max_t(unsigned int, xbin, 3));
365 vblank = MT9P031_VERTICAL_BLANK_DEF;
366
367 ret = mt9p031_write(client, MT9P031_HORIZONTAL_BLANK, hblank);
368 if (ret < 0)
369 return ret;
370 ret = mt9p031_write(client, MT9P031_VERTICAL_BLANK, vblank);
371 if (ret < 0)
372 return ret;
373
374 return ret;
375 }
376
mt9p031_s_stream(struct v4l2_subdev * subdev,int enable)377 static int mt9p031_s_stream(struct v4l2_subdev *subdev, int enable)
378 {
379 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
380 int ret;
381
382 if (!enable) {
383 /* Stop sensor readout */
384 ret = mt9p031_set_output_control(mt9p031,
385 MT9P031_OUTPUT_CONTROL_CEN, 0);
386 if (ret < 0)
387 return ret;
388
389 return mt9p031_pll_disable(mt9p031);
390 }
391
392 ret = mt9p031_set_params(mt9p031);
393 if (ret < 0)
394 return ret;
395
396 /* Switch to master "normal" mode */
397 ret = mt9p031_set_output_control(mt9p031, 0,
398 MT9P031_OUTPUT_CONTROL_CEN);
399 if (ret < 0)
400 return ret;
401
402 return mt9p031_pll_enable(mt9p031);
403 }
404
mt9p031_enum_mbus_code(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh,struct v4l2_subdev_mbus_code_enum * code)405 static int mt9p031_enum_mbus_code(struct v4l2_subdev *subdev,
406 struct v4l2_subdev_fh *fh,
407 struct v4l2_subdev_mbus_code_enum *code)
408 {
409 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
410
411 if (code->pad || code->index)
412 return -EINVAL;
413
414 code->code = mt9p031->format.code;
415 return 0;
416 }
417
mt9p031_enum_frame_size(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh,struct v4l2_subdev_frame_size_enum * fse)418 static int mt9p031_enum_frame_size(struct v4l2_subdev *subdev,
419 struct v4l2_subdev_fh *fh,
420 struct v4l2_subdev_frame_size_enum *fse)
421 {
422 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
423
424 if (fse->index >= 8 || fse->code != mt9p031->format.code)
425 return -EINVAL;
426
427 fse->min_width = MT9P031_WINDOW_WIDTH_DEF
428 / min_t(unsigned int, 7, fse->index + 1);
429 fse->max_width = fse->min_width;
430 fse->min_height = MT9P031_WINDOW_HEIGHT_DEF / (fse->index + 1);
431 fse->max_height = fse->min_height;
432
433 return 0;
434 }
435
436 static struct v4l2_mbus_framefmt *
__mt9p031_get_pad_format(struct mt9p031 * mt9p031,struct v4l2_subdev_fh * fh,unsigned int pad,u32 which)437 __mt9p031_get_pad_format(struct mt9p031 *mt9p031, struct v4l2_subdev_fh *fh,
438 unsigned int pad, u32 which)
439 {
440 switch (which) {
441 case V4L2_SUBDEV_FORMAT_TRY:
442 return v4l2_subdev_get_try_format(fh, pad);
443 case V4L2_SUBDEV_FORMAT_ACTIVE:
444 return &mt9p031->format;
445 default:
446 return NULL;
447 }
448 }
449
450 static struct v4l2_rect *
__mt9p031_get_pad_crop(struct mt9p031 * mt9p031,struct v4l2_subdev_fh * fh,unsigned int pad,u32 which)451 __mt9p031_get_pad_crop(struct mt9p031 *mt9p031, struct v4l2_subdev_fh *fh,
452 unsigned int pad, u32 which)
453 {
454 switch (which) {
455 case V4L2_SUBDEV_FORMAT_TRY:
456 return v4l2_subdev_get_try_crop(fh, pad);
457 case V4L2_SUBDEV_FORMAT_ACTIVE:
458 return &mt9p031->crop;
459 default:
460 return NULL;
461 }
462 }
463
mt9p031_get_format(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh,struct v4l2_subdev_format * fmt)464 static int mt9p031_get_format(struct v4l2_subdev *subdev,
465 struct v4l2_subdev_fh *fh,
466 struct v4l2_subdev_format *fmt)
467 {
468 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
469
470 fmt->format = *__mt9p031_get_pad_format(mt9p031, fh, fmt->pad,
471 fmt->which);
472 return 0;
473 }
474
mt9p031_set_format(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh,struct v4l2_subdev_format * format)475 static int mt9p031_set_format(struct v4l2_subdev *subdev,
476 struct v4l2_subdev_fh *fh,
477 struct v4l2_subdev_format *format)
478 {
479 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
480 struct v4l2_mbus_framefmt *__format;
481 struct v4l2_rect *__crop;
482 unsigned int width;
483 unsigned int height;
484 unsigned int hratio;
485 unsigned int vratio;
486
487 __crop = __mt9p031_get_pad_crop(mt9p031, fh, format->pad,
488 format->which);
489
490 /* Clamp the width and height to avoid dividing by zero. */
491 width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
492 max(__crop->width / 7, MT9P031_WINDOW_WIDTH_MIN),
493 __crop->width);
494 height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
495 max(__crop->height / 8, MT9P031_WINDOW_HEIGHT_MIN),
496 __crop->height);
497
498 hratio = DIV_ROUND_CLOSEST(__crop->width, width);
499 vratio = DIV_ROUND_CLOSEST(__crop->height, height);
500
501 __format = __mt9p031_get_pad_format(mt9p031, fh, format->pad,
502 format->which);
503 __format->width = __crop->width / hratio;
504 __format->height = __crop->height / vratio;
505
506 format->format = *__format;
507
508 return 0;
509 }
510
mt9p031_get_crop(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh,struct v4l2_subdev_crop * crop)511 static int mt9p031_get_crop(struct v4l2_subdev *subdev,
512 struct v4l2_subdev_fh *fh,
513 struct v4l2_subdev_crop *crop)
514 {
515 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
516
517 crop->rect = *__mt9p031_get_pad_crop(mt9p031, fh, crop->pad,
518 crop->which);
519 return 0;
520 }
521
mt9p031_set_crop(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh,struct v4l2_subdev_crop * crop)522 static int mt9p031_set_crop(struct v4l2_subdev *subdev,
523 struct v4l2_subdev_fh *fh,
524 struct v4l2_subdev_crop *crop)
525 {
526 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
527 struct v4l2_mbus_framefmt *__format;
528 struct v4l2_rect *__crop;
529 struct v4l2_rect rect;
530
531 /* Clamp the crop rectangle boundaries and align them to a multiple of 2
532 * pixels to ensure a GRBG Bayer pattern.
533 */
534 rect.left = clamp(ALIGN(crop->rect.left, 2), MT9P031_COLUMN_START_MIN,
535 MT9P031_COLUMN_START_MAX);
536 rect.top = clamp(ALIGN(crop->rect.top, 2), MT9P031_ROW_START_MIN,
537 MT9P031_ROW_START_MAX);
538 rect.width = clamp(ALIGN(crop->rect.width, 2),
539 MT9P031_WINDOW_WIDTH_MIN,
540 MT9P031_WINDOW_WIDTH_MAX);
541 rect.height = clamp(ALIGN(crop->rect.height, 2),
542 MT9P031_WINDOW_HEIGHT_MIN,
543 MT9P031_WINDOW_HEIGHT_MAX);
544
545 rect.width = min(rect.width, MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
546 rect.height = min(rect.height, MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
547
548 __crop = __mt9p031_get_pad_crop(mt9p031, fh, crop->pad, crop->which);
549
550 if (rect.width != __crop->width || rect.height != __crop->height) {
551 /* Reset the output image size if the crop rectangle size has
552 * been modified.
553 */
554 __format = __mt9p031_get_pad_format(mt9p031, fh, crop->pad,
555 crop->which);
556 __format->width = rect.width;
557 __format->height = rect.height;
558 }
559
560 *__crop = rect;
561 crop->rect = rect;
562
563 return 0;
564 }
565
566 /* -----------------------------------------------------------------------------
567 * V4L2 subdev control operations
568 */
569
570 #define V4L2_CID_TEST_PATTERN (V4L2_CID_USER_BASE | 0x1001)
571
mt9p031_s_ctrl(struct v4l2_ctrl * ctrl)572 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
573 {
574 struct mt9p031 *mt9p031 =
575 container_of(ctrl->handler, struct mt9p031, ctrls);
576 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
577 u16 data;
578 int ret;
579
580 switch (ctrl->id) {
581 case V4L2_CID_EXPOSURE:
582 ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
583 (ctrl->val >> 16) & 0xffff);
584 if (ret < 0)
585 return ret;
586
587 return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
588 ctrl->val & 0xffff);
589
590 case V4L2_CID_GAIN:
591 /* Gain is controlled by 2 analog stages and a digital stage.
592 * Valid values for the 3 stages are
593 *
594 * Stage Min Max Step
595 * ------------------------------------------
596 * First analog stage x1 x2 1
597 * Second analog stage x1 x4 0.125
598 * Digital stage x1 x16 0.125
599 *
600 * To minimize noise, the gain stages should be used in the
601 * second analog stage, first analog stage, digital stage order.
602 * Gain from a previous stage should be pushed to its maximum
603 * value before the next stage is used.
604 */
605 if (ctrl->val <= 32) {
606 data = ctrl->val;
607 } else if (ctrl->val <= 64) {
608 ctrl->val &= ~1;
609 data = (1 << 6) | (ctrl->val >> 1);
610 } else {
611 ctrl->val &= ~7;
612 data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
613 }
614
615 return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
616
617 case V4L2_CID_HFLIP:
618 if (ctrl->val)
619 return mt9p031_set_mode2(mt9p031,
620 0, MT9P031_READ_MODE_2_COL_MIR);
621 else
622 return mt9p031_set_mode2(mt9p031,
623 MT9P031_READ_MODE_2_COL_MIR, 0);
624
625 case V4L2_CID_VFLIP:
626 if (ctrl->val)
627 return mt9p031_set_mode2(mt9p031,
628 0, MT9P031_READ_MODE_2_ROW_MIR);
629 else
630 return mt9p031_set_mode2(mt9p031,
631 MT9P031_READ_MODE_2_ROW_MIR, 0);
632
633 case V4L2_CID_TEST_PATTERN:
634 if (!ctrl->val) {
635 ret = mt9p031_set_mode2(mt9p031,
636 0, MT9P031_READ_MODE_2_ROW_BLC);
637 if (ret < 0)
638 return ret;
639
640 return mt9p031_write(client, MT9P031_TEST_PATTERN,
641 MT9P031_TEST_PATTERN_DISABLE);
642 }
643
644 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
645 if (ret < 0)
646 return ret;
647 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
648 if (ret < 0)
649 return ret;
650 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
651 if (ret < 0)
652 return ret;
653
654 ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
655 0);
656 if (ret < 0)
657 return ret;
658 ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
659 if (ret < 0)
660 return ret;
661
662 return mt9p031_write(client, MT9P031_TEST_PATTERN,
663 ((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
664 | MT9P031_TEST_PATTERN_ENABLE);
665 }
666 return 0;
667 }
668
669 static struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
670 .s_ctrl = mt9p031_s_ctrl,
671 };
672
673 static const char * const mt9p031_test_pattern_menu[] = {
674 "Disabled",
675 "Color Field",
676 "Horizontal Gradient",
677 "Vertical Gradient",
678 "Diagonal Gradient",
679 "Classic Test Pattern",
680 "Walking 1s",
681 "Monochrome Horizontal Bars",
682 "Monochrome Vertical Bars",
683 "Vertical Color Bars",
684 };
685
686 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
687 {
688 .ops = &mt9p031_ctrl_ops,
689 .id = V4L2_CID_TEST_PATTERN,
690 .type = V4L2_CTRL_TYPE_MENU,
691 .name = "Test Pattern",
692 .min = 0,
693 .max = ARRAY_SIZE(mt9p031_test_pattern_menu) - 1,
694 .step = 0,
695 .def = 0,
696 .flags = 0,
697 .menu_skip_mask = 0,
698 .qmenu = mt9p031_test_pattern_menu,
699 }
700 };
701
702 /* -----------------------------------------------------------------------------
703 * V4L2 subdev core operations
704 */
705
mt9p031_set_power(struct v4l2_subdev * subdev,int on)706 static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
707 {
708 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
709 int ret = 0;
710
711 mutex_lock(&mt9p031->power_lock);
712
713 /* If the power count is modified from 0 to != 0 or from != 0 to 0,
714 * update the power state.
715 */
716 if (mt9p031->power_count == !on) {
717 ret = __mt9p031_set_power(mt9p031, !!on);
718 if (ret < 0)
719 goto out;
720 }
721
722 /* Update the power count. */
723 mt9p031->power_count += on ? 1 : -1;
724 WARN_ON(mt9p031->power_count < 0);
725
726 out:
727 mutex_unlock(&mt9p031->power_lock);
728 return ret;
729 }
730
731 /* -----------------------------------------------------------------------------
732 * V4L2 subdev internal operations
733 */
734
mt9p031_registered(struct v4l2_subdev * subdev)735 static int mt9p031_registered(struct v4l2_subdev *subdev)
736 {
737 struct i2c_client *client = v4l2_get_subdevdata(subdev);
738 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
739 s32 data;
740 int ret;
741
742 ret = mt9p031_power_on(mt9p031);
743 if (ret < 0) {
744 dev_err(&client->dev, "MT9P031 power up failed\n");
745 return ret;
746 }
747
748 /* Read out the chip version register */
749 data = mt9p031_read(client, MT9P031_CHIP_VERSION);
750 if (data != MT9P031_CHIP_VERSION_VALUE) {
751 dev_err(&client->dev, "MT9P031 not detected, wrong version "
752 "0x%04x\n", data);
753 return -ENODEV;
754 }
755
756 mt9p031_power_off(mt9p031);
757
758 dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
759 client->addr);
760
761 return ret;
762 }
763
mt9p031_open(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh)764 static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
765 {
766 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
767 struct v4l2_mbus_framefmt *format;
768 struct v4l2_rect *crop;
769
770 crop = v4l2_subdev_get_try_crop(fh, 0);
771 crop->left = MT9P031_COLUMN_START_DEF;
772 crop->top = MT9P031_ROW_START_DEF;
773 crop->width = MT9P031_WINDOW_WIDTH_DEF;
774 crop->height = MT9P031_WINDOW_HEIGHT_DEF;
775
776 format = v4l2_subdev_get_try_format(fh, 0);
777
778 if (mt9p031->pdata->version == MT9P031_MONOCHROME_VERSION)
779 format->code = V4L2_MBUS_FMT_Y12_1X12;
780 else
781 format->code = V4L2_MBUS_FMT_SGRBG12_1X12;
782
783 format->width = MT9P031_WINDOW_WIDTH_DEF;
784 format->height = MT9P031_WINDOW_HEIGHT_DEF;
785 format->field = V4L2_FIELD_NONE;
786 format->colorspace = V4L2_COLORSPACE_SRGB;
787
788 mt9p031->xskip = 1;
789 mt9p031->yskip = 1;
790 return mt9p031_set_power(subdev, 1);
791 }
792
mt9p031_close(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh)793 static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
794 {
795 return mt9p031_set_power(subdev, 0);
796 }
797
798 static struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
799 .s_power = mt9p031_set_power,
800 };
801
802 static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
803 .s_stream = mt9p031_s_stream,
804 };
805
806 static struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
807 .enum_mbus_code = mt9p031_enum_mbus_code,
808 .enum_frame_size = mt9p031_enum_frame_size,
809 .get_fmt = mt9p031_get_format,
810 .set_fmt = mt9p031_set_format,
811 .get_crop = mt9p031_get_crop,
812 .set_crop = mt9p031_set_crop,
813 };
814
815 static struct v4l2_subdev_ops mt9p031_subdev_ops = {
816 .core = &mt9p031_subdev_core_ops,
817 .video = &mt9p031_subdev_video_ops,
818 .pad = &mt9p031_subdev_pad_ops,
819 };
820
821 static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
822 .registered = mt9p031_registered,
823 .open = mt9p031_open,
824 .close = mt9p031_close,
825 };
826
827 /* -----------------------------------------------------------------------------
828 * Driver initialization and probing
829 */
830
mt9p031_probe(struct i2c_client * client,const struct i2c_device_id * did)831 static int mt9p031_probe(struct i2c_client *client,
832 const struct i2c_device_id *did)
833 {
834 struct mt9p031_platform_data *pdata = client->dev.platform_data;
835 struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
836 struct mt9p031 *mt9p031;
837 unsigned int i;
838 int ret;
839
840 if (pdata == NULL) {
841 dev_err(&client->dev, "No platform data\n");
842 return -EINVAL;
843 }
844
845 if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
846 dev_warn(&client->dev,
847 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
848 return -EIO;
849 }
850
851 mt9p031 = kzalloc(sizeof(*mt9p031), GFP_KERNEL);
852 if (mt9p031 == NULL)
853 return -ENOMEM;
854
855 mt9p031->pdata = pdata;
856 mt9p031->output_control = MT9P031_OUTPUT_CONTROL_DEF;
857 mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
858
859 v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 4);
860
861 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
862 V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
863 MT9P031_SHUTTER_WIDTH_MAX, 1,
864 MT9P031_SHUTTER_WIDTH_DEF);
865 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
866 V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
867 MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
868 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
869 V4L2_CID_HFLIP, 0, 1, 1, 0);
870 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
871 V4L2_CID_VFLIP, 0, 1, 1, 0);
872
873 for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
874 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
875
876 mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
877
878 if (mt9p031->ctrls.error)
879 printk(KERN_INFO "%s: control initialization error %d\n",
880 __func__, mt9p031->ctrls.error);
881
882 mutex_init(&mt9p031->power_lock);
883 v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
884 mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
885
886 mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
887 ret = media_entity_init(&mt9p031->subdev.entity, 1, &mt9p031->pad, 0);
888 if (ret < 0)
889 goto done;
890
891 mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
892
893 mt9p031->crop.width = MT9P031_WINDOW_WIDTH_DEF;
894 mt9p031->crop.height = MT9P031_WINDOW_HEIGHT_DEF;
895 mt9p031->crop.left = MT9P031_COLUMN_START_DEF;
896 mt9p031->crop.top = MT9P031_ROW_START_DEF;
897
898 if (mt9p031->pdata->version == MT9P031_MONOCHROME_VERSION)
899 mt9p031->format.code = V4L2_MBUS_FMT_Y12_1X12;
900 else
901 mt9p031->format.code = V4L2_MBUS_FMT_SGRBG12_1X12;
902
903 mt9p031->format.width = MT9P031_WINDOW_WIDTH_DEF;
904 mt9p031->format.height = MT9P031_WINDOW_HEIGHT_DEF;
905 mt9p031->format.field = V4L2_FIELD_NONE;
906 mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
907
908 ret = mt9p031_pll_get_divs(mt9p031);
909
910 done:
911 if (ret < 0) {
912 v4l2_ctrl_handler_free(&mt9p031->ctrls);
913 media_entity_cleanup(&mt9p031->subdev.entity);
914 kfree(mt9p031);
915 }
916
917 return ret;
918 }
919
mt9p031_remove(struct i2c_client * client)920 static int mt9p031_remove(struct i2c_client *client)
921 {
922 struct v4l2_subdev *subdev = i2c_get_clientdata(client);
923 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
924
925 v4l2_ctrl_handler_free(&mt9p031->ctrls);
926 v4l2_device_unregister_subdev(subdev);
927 media_entity_cleanup(&subdev->entity);
928 kfree(mt9p031);
929
930 return 0;
931 }
932
933 static const struct i2c_device_id mt9p031_id[] = {
934 { "mt9p031", 0 },
935 { }
936 };
937 MODULE_DEVICE_TABLE(i2c, mt9p031_id);
938
939 static struct i2c_driver mt9p031_i2c_driver = {
940 .driver = {
941 .name = "mt9p031",
942 },
943 .probe = mt9p031_probe,
944 .remove = mt9p031_remove,
945 .id_table = mt9p031_id,
946 };
947
mt9p031_mod_init(void)948 static int __init mt9p031_mod_init(void)
949 {
950 return i2c_add_driver(&mt9p031_i2c_driver);
951 }
952
mt9p031_mod_exit(void)953 static void __exit mt9p031_mod_exit(void)
954 {
955 i2c_del_driver(&mt9p031_i2c_driver);
956 }
957
958 module_init(mt9p031_mod_init);
959 module_exit(mt9p031_mod_exit);
960
961 MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
962 MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
963 MODULE_LICENSE("GPL v2");
964