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/clk.h>
16 #include <linux/delay.h>
17 #include <linux/device.h>
18 #include <linux/gpio/consumer.h>
19 #include <linux/i2c.h>
20 #include <linux/log2.h>
21 #include <linux/module.h>
22 #include <linux/of.h>
23 #include <linux/of_graph.h>
24 #include <linux/pm.h>
25 #include <linux/regulator/consumer.h>
26 #include <linux/slab.h>
27 #include <linux/videodev2.h>
28
29 #include <media/mt9p031.h>
30 #include <media/v4l2-async.h>
31 #include <media/v4l2-ctrls.h>
32 #include <media/v4l2-device.h>
33 #include <media/v4l2-subdev.h>
34
35 #include "aptina-pll.h"
36
37 #define MT9P031_PIXEL_ARRAY_WIDTH 2752
38 #define MT9P031_PIXEL_ARRAY_HEIGHT 2004
39
40 #define MT9P031_CHIP_VERSION 0x00
41 #define MT9P031_CHIP_VERSION_VALUE 0x1801
42 #define MT9P031_ROW_START 0x01
43 #define MT9P031_ROW_START_MIN 0
44 #define MT9P031_ROW_START_MAX 2004
45 #define MT9P031_ROW_START_DEF 54
46 #define MT9P031_COLUMN_START 0x02
47 #define MT9P031_COLUMN_START_MIN 0
48 #define MT9P031_COLUMN_START_MAX 2750
49 #define MT9P031_COLUMN_START_DEF 16
50 #define MT9P031_WINDOW_HEIGHT 0x03
51 #define MT9P031_WINDOW_HEIGHT_MIN 2
52 #define MT9P031_WINDOW_HEIGHT_MAX 2006
53 #define MT9P031_WINDOW_HEIGHT_DEF 1944
54 #define MT9P031_WINDOW_WIDTH 0x04
55 #define MT9P031_WINDOW_WIDTH_MIN 2
56 #define MT9P031_WINDOW_WIDTH_MAX 2752
57 #define MT9P031_WINDOW_WIDTH_DEF 2592
58 #define MT9P031_HORIZONTAL_BLANK 0x05
59 #define MT9P031_HORIZONTAL_BLANK_MIN 0
60 #define MT9P031_HORIZONTAL_BLANK_MAX 4095
61 #define MT9P031_VERTICAL_BLANK 0x06
62 #define MT9P031_VERTICAL_BLANK_MIN 1
63 #define MT9P031_VERTICAL_BLANK_MAX 4096
64 #define MT9P031_VERTICAL_BLANK_DEF 26
65 #define MT9P031_OUTPUT_CONTROL 0x07
66 #define MT9P031_OUTPUT_CONTROL_CEN 2
67 #define MT9P031_OUTPUT_CONTROL_SYN 1
68 #define MT9P031_OUTPUT_CONTROL_DEF 0x1f82
69 #define MT9P031_SHUTTER_WIDTH_UPPER 0x08
70 #define MT9P031_SHUTTER_WIDTH_LOWER 0x09
71 #define MT9P031_SHUTTER_WIDTH_MIN 1
72 #define MT9P031_SHUTTER_WIDTH_MAX 1048575
73 #define MT9P031_SHUTTER_WIDTH_DEF 1943
74 #define MT9P031_PLL_CONTROL 0x10
75 #define MT9P031_PLL_CONTROL_PWROFF 0x0050
76 #define MT9P031_PLL_CONTROL_PWRON 0x0051
77 #define MT9P031_PLL_CONTROL_USEPLL 0x0052
78 #define MT9P031_PLL_CONFIG_1 0x11
79 #define MT9P031_PLL_CONFIG_2 0x12
80 #define MT9P031_PIXEL_CLOCK_CONTROL 0x0a
81 #define MT9P031_PIXEL_CLOCK_INVERT (1 << 15)
82 #define MT9P031_PIXEL_CLOCK_SHIFT(n) ((n) << 8)
83 #define MT9P031_PIXEL_CLOCK_DIVIDE(n) ((n) << 0)
84 #define MT9P031_RESTART 0x0b
85 #define MT9P031_FRAME_PAUSE_RESTART (1 << 1)
86 #define MT9P031_FRAME_RESTART (1 << 0)
87 #define MT9P031_SHUTTER_DELAY 0x0c
88 #define MT9P031_RST 0x0d
89 #define MT9P031_RST_ENABLE 1
90 #define MT9P031_RST_DISABLE 0
91 #define MT9P031_READ_MODE_1 0x1e
92 #define MT9P031_READ_MODE_2 0x20
93 #define MT9P031_READ_MODE_2_ROW_MIR (1 << 15)
94 #define MT9P031_READ_MODE_2_COL_MIR (1 << 14)
95 #define MT9P031_READ_MODE_2_ROW_BLC (1 << 6)
96 #define MT9P031_ROW_ADDRESS_MODE 0x22
97 #define MT9P031_COLUMN_ADDRESS_MODE 0x23
98 #define MT9P031_GLOBAL_GAIN 0x35
99 #define MT9P031_GLOBAL_GAIN_MIN 8
100 #define MT9P031_GLOBAL_GAIN_MAX 1024
101 #define MT9P031_GLOBAL_GAIN_DEF 8
102 #define MT9P031_GLOBAL_GAIN_MULT (1 << 6)
103 #define MT9P031_ROW_BLACK_TARGET 0x49
104 #define MT9P031_ROW_BLACK_DEF_OFFSET 0x4b
105 #define MT9P031_GREEN1_OFFSET 0x60
106 #define MT9P031_GREEN2_OFFSET 0x61
107 #define MT9P031_BLACK_LEVEL_CALIBRATION 0x62
108 #define MT9P031_BLC_MANUAL_BLC (1 << 0)
109 #define MT9P031_RED_OFFSET 0x63
110 #define MT9P031_BLUE_OFFSET 0x64
111 #define MT9P031_TEST_PATTERN 0xa0
112 #define MT9P031_TEST_PATTERN_SHIFT 3
113 #define MT9P031_TEST_PATTERN_ENABLE (1 << 0)
114 #define MT9P031_TEST_PATTERN_DISABLE (0 << 0)
115 #define MT9P031_TEST_PATTERN_GREEN 0xa1
116 #define MT9P031_TEST_PATTERN_RED 0xa2
117 #define MT9P031_TEST_PATTERN_BLUE 0xa3
118
119 enum mt9p031_model {
120 MT9P031_MODEL_COLOR,
121 MT9P031_MODEL_MONOCHROME,
122 };
123
124 struct mt9p031 {
125 struct v4l2_subdev subdev;
126 struct media_pad pad;
127 struct v4l2_rect crop; /* Sensor window */
128 struct v4l2_mbus_framefmt format;
129 struct mt9p031_platform_data *pdata;
130 struct mutex power_lock; /* lock to protect power_count */
131 int power_count;
132
133 struct clk *clk;
134 struct regulator_bulk_data regulators[3];
135
136 enum mt9p031_model model;
137 struct aptina_pll pll;
138 unsigned int clk_div;
139 bool use_pll;
140 struct gpio_desc *reset;
141
142 struct v4l2_ctrl_handler ctrls;
143 struct v4l2_ctrl *blc_auto;
144 struct v4l2_ctrl *blc_offset;
145
146 /* Registers cache */
147 u16 output_control;
148 u16 mode2;
149 };
150
to_mt9p031(struct v4l2_subdev * sd)151 static struct mt9p031 *to_mt9p031(struct v4l2_subdev *sd)
152 {
153 return container_of(sd, struct mt9p031, subdev);
154 }
155
mt9p031_read(struct i2c_client * client,u8 reg)156 static int mt9p031_read(struct i2c_client *client, u8 reg)
157 {
158 return i2c_smbus_read_word_swapped(client, reg);
159 }
160
mt9p031_write(struct i2c_client * client,u8 reg,u16 data)161 static int mt9p031_write(struct i2c_client *client, u8 reg, u16 data)
162 {
163 return i2c_smbus_write_word_swapped(client, reg, data);
164 }
165
mt9p031_set_output_control(struct mt9p031 * mt9p031,u16 clear,u16 set)166 static int mt9p031_set_output_control(struct mt9p031 *mt9p031, u16 clear,
167 u16 set)
168 {
169 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
170 u16 value = (mt9p031->output_control & ~clear) | set;
171 int ret;
172
173 ret = mt9p031_write(client, MT9P031_OUTPUT_CONTROL, value);
174 if (ret < 0)
175 return ret;
176
177 mt9p031->output_control = value;
178 return 0;
179 }
180
mt9p031_set_mode2(struct mt9p031 * mt9p031,u16 clear,u16 set)181 static int mt9p031_set_mode2(struct mt9p031 *mt9p031, u16 clear, u16 set)
182 {
183 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
184 u16 value = (mt9p031->mode2 & ~clear) | set;
185 int ret;
186
187 ret = mt9p031_write(client, MT9P031_READ_MODE_2, value);
188 if (ret < 0)
189 return ret;
190
191 mt9p031->mode2 = value;
192 return 0;
193 }
194
mt9p031_reset(struct mt9p031 * mt9p031)195 static int mt9p031_reset(struct mt9p031 *mt9p031)
196 {
197 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
198 int ret;
199
200 /* Disable chip output, synchronous option update */
201 ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_ENABLE);
202 if (ret < 0)
203 return ret;
204 ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_DISABLE);
205 if (ret < 0)
206 return ret;
207
208 ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
209 MT9P031_PIXEL_CLOCK_DIVIDE(mt9p031->clk_div));
210 if (ret < 0)
211 return ret;
212
213 return mt9p031_set_output_control(mt9p031, MT9P031_OUTPUT_CONTROL_CEN,
214 0);
215 }
216
mt9p031_clk_setup(struct mt9p031 * mt9p031)217 static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
218 {
219 static const struct aptina_pll_limits limits = {
220 .ext_clock_min = 6000000,
221 .ext_clock_max = 27000000,
222 .int_clock_min = 2000000,
223 .int_clock_max = 13500000,
224 .out_clock_min = 180000000,
225 .out_clock_max = 360000000,
226 .pix_clock_max = 96000000,
227 .n_min = 1,
228 .n_max = 64,
229 .m_min = 16,
230 .m_max = 255,
231 .p1_min = 1,
232 .p1_max = 128,
233 };
234
235 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
236 struct mt9p031_platform_data *pdata = mt9p031->pdata;
237 int ret;
238
239 mt9p031->clk = devm_clk_get(&client->dev, NULL);
240 if (IS_ERR(mt9p031->clk))
241 return PTR_ERR(mt9p031->clk);
242
243 ret = clk_set_rate(mt9p031->clk, pdata->ext_freq);
244 if (ret < 0)
245 return ret;
246
247 /* If the external clock frequency is out of bounds for the PLL use the
248 * pixel clock divider only and disable the PLL.
249 */
250 if (pdata->ext_freq > limits.ext_clock_max) {
251 unsigned int div;
252
253 div = DIV_ROUND_UP(pdata->ext_freq, pdata->target_freq);
254 div = roundup_pow_of_two(div) / 2;
255
256 mt9p031->clk_div = min_t(unsigned int, div, 64);
257 mt9p031->use_pll = false;
258
259 return 0;
260 }
261
262 mt9p031->pll.ext_clock = pdata->ext_freq;
263 mt9p031->pll.pix_clock = pdata->target_freq;
264 mt9p031->use_pll = true;
265
266 return aptina_pll_calculate(&client->dev, &limits, &mt9p031->pll);
267 }
268
mt9p031_pll_enable(struct mt9p031 * mt9p031)269 static int mt9p031_pll_enable(struct mt9p031 *mt9p031)
270 {
271 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
272 int ret;
273
274 if (!mt9p031->use_pll)
275 return 0;
276
277 ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
278 MT9P031_PLL_CONTROL_PWRON);
279 if (ret < 0)
280 return ret;
281
282 ret = mt9p031_write(client, MT9P031_PLL_CONFIG_1,
283 (mt9p031->pll.m << 8) | (mt9p031->pll.n - 1));
284 if (ret < 0)
285 return ret;
286
287 ret = mt9p031_write(client, MT9P031_PLL_CONFIG_2, mt9p031->pll.p1 - 1);
288 if (ret < 0)
289 return ret;
290
291 usleep_range(1000, 2000);
292 ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
293 MT9P031_PLL_CONTROL_PWRON |
294 MT9P031_PLL_CONTROL_USEPLL);
295 return ret;
296 }
297
mt9p031_pll_disable(struct mt9p031 * mt9p031)298 static inline int mt9p031_pll_disable(struct mt9p031 *mt9p031)
299 {
300 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
301
302 if (!mt9p031->use_pll)
303 return 0;
304
305 return mt9p031_write(client, MT9P031_PLL_CONTROL,
306 MT9P031_PLL_CONTROL_PWROFF);
307 }
308
mt9p031_power_on(struct mt9p031 * mt9p031)309 static int mt9p031_power_on(struct mt9p031 *mt9p031)
310 {
311 int ret;
312
313 /* Ensure RESET_BAR is active */
314 if (mt9p031->reset) {
315 gpiod_set_value(mt9p031->reset, 1);
316 usleep_range(1000, 2000);
317 }
318
319 /* Bring up the supplies */
320 ret = regulator_bulk_enable(ARRAY_SIZE(mt9p031->regulators),
321 mt9p031->regulators);
322 if (ret < 0)
323 return ret;
324
325 /* Enable clock */
326 if (mt9p031->clk) {
327 ret = clk_prepare_enable(mt9p031->clk);
328 if (ret) {
329 regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
330 mt9p031->regulators);
331 return ret;
332 }
333 }
334
335 /* Now RESET_BAR must be high */
336 if (mt9p031->reset) {
337 gpiod_set_value(mt9p031->reset, 0);
338 usleep_range(1000, 2000);
339 }
340
341 return 0;
342 }
343
mt9p031_power_off(struct mt9p031 * mt9p031)344 static void mt9p031_power_off(struct mt9p031 *mt9p031)
345 {
346 if (mt9p031->reset) {
347 gpiod_set_value(mt9p031->reset, 1);
348 usleep_range(1000, 2000);
349 }
350
351 regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
352 mt9p031->regulators);
353
354 if (mt9p031->clk)
355 clk_disable_unprepare(mt9p031->clk);
356 }
357
__mt9p031_set_power(struct mt9p031 * mt9p031,bool on)358 static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
359 {
360 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
361 int ret;
362
363 if (!on) {
364 mt9p031_power_off(mt9p031);
365 return 0;
366 }
367
368 ret = mt9p031_power_on(mt9p031);
369 if (ret < 0)
370 return ret;
371
372 ret = mt9p031_reset(mt9p031);
373 if (ret < 0) {
374 dev_err(&client->dev, "Failed to reset the camera\n");
375 return ret;
376 }
377
378 return v4l2_ctrl_handler_setup(&mt9p031->ctrls);
379 }
380
381 /* -----------------------------------------------------------------------------
382 * V4L2 subdev video operations
383 */
384
mt9p031_set_params(struct mt9p031 * mt9p031)385 static int mt9p031_set_params(struct mt9p031 *mt9p031)
386 {
387 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
388 struct v4l2_mbus_framefmt *format = &mt9p031->format;
389 const struct v4l2_rect *crop = &mt9p031->crop;
390 unsigned int hblank;
391 unsigned int vblank;
392 unsigned int xskip;
393 unsigned int yskip;
394 unsigned int xbin;
395 unsigned int ybin;
396 int ret;
397
398 /* Windows position and size.
399 *
400 * TODO: Make sure the start coordinates and window size match the
401 * skipping, binning and mirroring (see description of registers 2 and 4
402 * in table 13, and Binning section on page 41).
403 */
404 ret = mt9p031_write(client, MT9P031_COLUMN_START, crop->left);
405 if (ret < 0)
406 return ret;
407 ret = mt9p031_write(client, MT9P031_ROW_START, crop->top);
408 if (ret < 0)
409 return ret;
410 ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, crop->width - 1);
411 if (ret < 0)
412 return ret;
413 ret = mt9p031_write(client, MT9P031_WINDOW_HEIGHT, crop->height - 1);
414 if (ret < 0)
415 return ret;
416
417 /* Row and column binning and skipping. Use the maximum binning value
418 * compatible with the skipping settings.
419 */
420 xskip = DIV_ROUND_CLOSEST(crop->width, format->width);
421 yskip = DIV_ROUND_CLOSEST(crop->height, format->height);
422 xbin = 1 << (ffs(xskip) - 1);
423 ybin = 1 << (ffs(yskip) - 1);
424
425 ret = mt9p031_write(client, MT9P031_COLUMN_ADDRESS_MODE,
426 ((xbin - 1) << 4) | (xskip - 1));
427 if (ret < 0)
428 return ret;
429 ret = mt9p031_write(client, MT9P031_ROW_ADDRESS_MODE,
430 ((ybin - 1) << 4) | (yskip - 1));
431 if (ret < 0)
432 return ret;
433
434 /* Blanking - use minimum value for horizontal blanking and default
435 * value for vertical blanking.
436 */
437 hblank = 346 * ybin + 64 + (80 >> min_t(unsigned int, xbin, 3));
438 vblank = MT9P031_VERTICAL_BLANK_DEF;
439
440 ret = mt9p031_write(client, MT9P031_HORIZONTAL_BLANK, hblank - 1);
441 if (ret < 0)
442 return ret;
443 ret = mt9p031_write(client, MT9P031_VERTICAL_BLANK, vblank - 1);
444 if (ret < 0)
445 return ret;
446
447 return ret;
448 }
449
mt9p031_s_stream(struct v4l2_subdev * subdev,int enable)450 static int mt9p031_s_stream(struct v4l2_subdev *subdev, int enable)
451 {
452 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
453 struct i2c_client *client = v4l2_get_subdevdata(subdev);
454 int val;
455 int ret;
456
457 if (!enable) {
458 /* enable pause restart */
459 val = MT9P031_FRAME_PAUSE_RESTART;
460 ret = mt9p031_write(client, MT9P031_RESTART, val);
461 if (ret < 0)
462 return ret;
463
464 /* enable restart + keep pause restart set */
465 val |= MT9P031_FRAME_RESTART;
466 ret = mt9p031_write(client, MT9P031_RESTART, val);
467 if (ret < 0)
468 return ret;
469
470 /* Stop sensor readout */
471 ret = mt9p031_set_output_control(mt9p031,
472 MT9P031_OUTPUT_CONTROL_CEN, 0);
473 if (ret < 0)
474 return ret;
475
476 return mt9p031_pll_disable(mt9p031);
477 }
478
479 ret = mt9p031_set_params(mt9p031);
480 if (ret < 0)
481 return ret;
482
483 /* Switch to master "normal" mode */
484 ret = mt9p031_set_output_control(mt9p031, 0,
485 MT9P031_OUTPUT_CONTROL_CEN);
486 if (ret < 0)
487 return ret;
488
489 /*
490 * - clear pause restart
491 * - don't clear restart as clearing restart manually can cause
492 * undefined behavior
493 */
494 val = MT9P031_FRAME_RESTART;
495 ret = mt9p031_write(client, MT9P031_RESTART, val);
496 if (ret < 0)
497 return ret;
498
499 return mt9p031_pll_enable(mt9p031);
500 }
501
mt9p031_enum_mbus_code(struct v4l2_subdev * subdev,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_mbus_code_enum * code)502 static int mt9p031_enum_mbus_code(struct v4l2_subdev *subdev,
503 struct v4l2_subdev_pad_config *cfg,
504 struct v4l2_subdev_mbus_code_enum *code)
505 {
506 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
507
508 if (code->pad || code->index)
509 return -EINVAL;
510
511 code->code = mt9p031->format.code;
512 return 0;
513 }
514
mt9p031_enum_frame_size(struct v4l2_subdev * subdev,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_frame_size_enum * fse)515 static int mt9p031_enum_frame_size(struct v4l2_subdev *subdev,
516 struct v4l2_subdev_pad_config *cfg,
517 struct v4l2_subdev_frame_size_enum *fse)
518 {
519 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
520
521 if (fse->index >= 8 || fse->code != mt9p031->format.code)
522 return -EINVAL;
523
524 fse->min_width = MT9P031_WINDOW_WIDTH_DEF
525 / min_t(unsigned int, 7, fse->index + 1);
526 fse->max_width = fse->min_width;
527 fse->min_height = MT9P031_WINDOW_HEIGHT_DEF / (fse->index + 1);
528 fse->max_height = fse->min_height;
529
530 return 0;
531 }
532
533 static struct v4l2_mbus_framefmt *
__mt9p031_get_pad_format(struct mt9p031 * mt9p031,struct v4l2_subdev_pad_config * cfg,unsigned int pad,u32 which)534 __mt9p031_get_pad_format(struct mt9p031 *mt9p031, struct v4l2_subdev_pad_config *cfg,
535 unsigned int pad, u32 which)
536 {
537 switch (which) {
538 case V4L2_SUBDEV_FORMAT_TRY:
539 return v4l2_subdev_get_try_format(&mt9p031->subdev, cfg, pad);
540 case V4L2_SUBDEV_FORMAT_ACTIVE:
541 return &mt9p031->format;
542 default:
543 return NULL;
544 }
545 }
546
547 static struct v4l2_rect *
__mt9p031_get_pad_crop(struct mt9p031 * mt9p031,struct v4l2_subdev_pad_config * cfg,unsigned int pad,u32 which)548 __mt9p031_get_pad_crop(struct mt9p031 *mt9p031, struct v4l2_subdev_pad_config *cfg,
549 unsigned int pad, u32 which)
550 {
551 switch (which) {
552 case V4L2_SUBDEV_FORMAT_TRY:
553 return v4l2_subdev_get_try_crop(&mt9p031->subdev, cfg, pad);
554 case V4L2_SUBDEV_FORMAT_ACTIVE:
555 return &mt9p031->crop;
556 default:
557 return NULL;
558 }
559 }
560
mt9p031_get_format(struct v4l2_subdev * subdev,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * fmt)561 static int mt9p031_get_format(struct v4l2_subdev *subdev,
562 struct v4l2_subdev_pad_config *cfg,
563 struct v4l2_subdev_format *fmt)
564 {
565 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
566
567 fmt->format = *__mt9p031_get_pad_format(mt9p031, cfg, fmt->pad,
568 fmt->which);
569 return 0;
570 }
571
mt9p031_set_format(struct v4l2_subdev * subdev,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * format)572 static int mt9p031_set_format(struct v4l2_subdev *subdev,
573 struct v4l2_subdev_pad_config *cfg,
574 struct v4l2_subdev_format *format)
575 {
576 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
577 struct v4l2_mbus_framefmt *__format;
578 struct v4l2_rect *__crop;
579 unsigned int width;
580 unsigned int height;
581 unsigned int hratio;
582 unsigned int vratio;
583
584 __crop = __mt9p031_get_pad_crop(mt9p031, cfg, format->pad,
585 format->which);
586
587 /* Clamp the width and height to avoid dividing by zero. */
588 width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
589 max_t(unsigned int, __crop->width / 7,
590 MT9P031_WINDOW_WIDTH_MIN),
591 __crop->width);
592 height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
593 max_t(unsigned int, __crop->height / 8,
594 MT9P031_WINDOW_HEIGHT_MIN),
595 __crop->height);
596
597 hratio = DIV_ROUND_CLOSEST(__crop->width, width);
598 vratio = DIV_ROUND_CLOSEST(__crop->height, height);
599
600 __format = __mt9p031_get_pad_format(mt9p031, cfg, format->pad,
601 format->which);
602 __format->width = __crop->width / hratio;
603 __format->height = __crop->height / vratio;
604
605 format->format = *__format;
606
607 return 0;
608 }
609
mt9p031_get_selection(struct v4l2_subdev * subdev,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_selection * sel)610 static int mt9p031_get_selection(struct v4l2_subdev *subdev,
611 struct v4l2_subdev_pad_config *cfg,
612 struct v4l2_subdev_selection *sel)
613 {
614 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
615
616 if (sel->target != V4L2_SEL_TGT_CROP)
617 return -EINVAL;
618
619 sel->r = *__mt9p031_get_pad_crop(mt9p031, cfg, sel->pad, sel->which);
620 return 0;
621 }
622
mt9p031_set_selection(struct v4l2_subdev * subdev,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_selection * sel)623 static int mt9p031_set_selection(struct v4l2_subdev *subdev,
624 struct v4l2_subdev_pad_config *cfg,
625 struct v4l2_subdev_selection *sel)
626 {
627 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
628 struct v4l2_mbus_framefmt *__format;
629 struct v4l2_rect *__crop;
630 struct v4l2_rect rect;
631
632 if (sel->target != V4L2_SEL_TGT_CROP)
633 return -EINVAL;
634
635 /* Clamp the crop rectangle boundaries and align them to a multiple of 2
636 * pixels to ensure a GRBG Bayer pattern.
637 */
638 rect.left = clamp(ALIGN(sel->r.left, 2), MT9P031_COLUMN_START_MIN,
639 MT9P031_COLUMN_START_MAX);
640 rect.top = clamp(ALIGN(sel->r.top, 2), MT9P031_ROW_START_MIN,
641 MT9P031_ROW_START_MAX);
642 rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 2),
643 MT9P031_WINDOW_WIDTH_MIN,
644 MT9P031_WINDOW_WIDTH_MAX);
645 rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 2),
646 MT9P031_WINDOW_HEIGHT_MIN,
647 MT9P031_WINDOW_HEIGHT_MAX);
648
649 rect.width = min_t(unsigned int, rect.width,
650 MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
651 rect.height = min_t(unsigned int, rect.height,
652 MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
653
654 __crop = __mt9p031_get_pad_crop(mt9p031, cfg, sel->pad, sel->which);
655
656 if (rect.width != __crop->width || rect.height != __crop->height) {
657 /* Reset the output image size if the crop rectangle size has
658 * been modified.
659 */
660 __format = __mt9p031_get_pad_format(mt9p031, cfg, sel->pad,
661 sel->which);
662 __format->width = rect.width;
663 __format->height = rect.height;
664 }
665
666 *__crop = rect;
667 sel->r = rect;
668
669 return 0;
670 }
671
672 /* -----------------------------------------------------------------------------
673 * V4L2 subdev control operations
674 */
675
676 #define V4L2_CID_BLC_AUTO (V4L2_CID_USER_BASE | 0x1002)
677 #define V4L2_CID_BLC_TARGET_LEVEL (V4L2_CID_USER_BASE | 0x1003)
678 #define V4L2_CID_BLC_ANALOG_OFFSET (V4L2_CID_USER_BASE | 0x1004)
679 #define V4L2_CID_BLC_DIGITAL_OFFSET (V4L2_CID_USER_BASE | 0x1005)
680
mt9p031_restore_blc(struct mt9p031 * mt9p031)681 static int mt9p031_restore_blc(struct mt9p031 *mt9p031)
682 {
683 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
684 int ret;
685
686 if (mt9p031->blc_auto->cur.val != 0) {
687 ret = mt9p031_set_mode2(mt9p031, 0,
688 MT9P031_READ_MODE_2_ROW_BLC);
689 if (ret < 0)
690 return ret;
691 }
692
693 if (mt9p031->blc_offset->cur.val != 0) {
694 ret = mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
695 mt9p031->blc_offset->cur.val);
696 if (ret < 0)
697 return ret;
698 }
699
700 return 0;
701 }
702
mt9p031_s_ctrl(struct v4l2_ctrl * ctrl)703 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
704 {
705 struct mt9p031 *mt9p031 =
706 container_of(ctrl->handler, struct mt9p031, ctrls);
707 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
708 u16 data;
709 int ret;
710
711 if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
712 return 0;
713
714 switch (ctrl->id) {
715 case V4L2_CID_EXPOSURE:
716 ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
717 (ctrl->val >> 16) & 0xffff);
718 if (ret < 0)
719 return ret;
720
721 return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
722 ctrl->val & 0xffff);
723
724 case V4L2_CID_GAIN:
725 /* Gain is controlled by 2 analog stages and a digital stage.
726 * Valid values for the 3 stages are
727 *
728 * Stage Min Max Step
729 * ------------------------------------------
730 * First analog stage x1 x2 1
731 * Second analog stage x1 x4 0.125
732 * Digital stage x1 x16 0.125
733 *
734 * To minimize noise, the gain stages should be used in the
735 * second analog stage, first analog stage, digital stage order.
736 * Gain from a previous stage should be pushed to its maximum
737 * value before the next stage is used.
738 */
739 if (ctrl->val <= 32) {
740 data = ctrl->val;
741 } else if (ctrl->val <= 64) {
742 ctrl->val &= ~1;
743 data = (1 << 6) | (ctrl->val >> 1);
744 } else {
745 ctrl->val &= ~7;
746 data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
747 }
748
749 return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
750
751 case V4L2_CID_HFLIP:
752 if (ctrl->val)
753 return mt9p031_set_mode2(mt9p031,
754 0, MT9P031_READ_MODE_2_COL_MIR);
755 else
756 return mt9p031_set_mode2(mt9p031,
757 MT9P031_READ_MODE_2_COL_MIR, 0);
758
759 case V4L2_CID_VFLIP:
760 if (ctrl->val)
761 return mt9p031_set_mode2(mt9p031,
762 0, MT9P031_READ_MODE_2_ROW_MIR);
763 else
764 return mt9p031_set_mode2(mt9p031,
765 MT9P031_READ_MODE_2_ROW_MIR, 0);
766
767 case V4L2_CID_TEST_PATTERN:
768 /* The digital side of the Black Level Calibration function must
769 * be disabled when generating a test pattern to avoid artifacts
770 * in the image. Activate (deactivate) the BLC-related controls
771 * when the test pattern is enabled (disabled).
772 */
773 v4l2_ctrl_activate(mt9p031->blc_auto, ctrl->val == 0);
774 v4l2_ctrl_activate(mt9p031->blc_offset, ctrl->val == 0);
775
776 if (!ctrl->val) {
777 /* Restore the BLC settings. */
778 ret = mt9p031_restore_blc(mt9p031);
779 if (ret < 0)
780 return ret;
781
782 return mt9p031_write(client, MT9P031_TEST_PATTERN,
783 MT9P031_TEST_PATTERN_DISABLE);
784 }
785
786 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
787 if (ret < 0)
788 return ret;
789 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
790 if (ret < 0)
791 return ret;
792 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
793 if (ret < 0)
794 return ret;
795
796 /* Disable digital BLC when generating a test pattern. */
797 ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
798 0);
799 if (ret < 0)
800 return ret;
801
802 ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
803 if (ret < 0)
804 return ret;
805
806 return mt9p031_write(client, MT9P031_TEST_PATTERN,
807 ((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
808 | MT9P031_TEST_PATTERN_ENABLE);
809
810 case V4L2_CID_BLC_AUTO:
811 ret = mt9p031_set_mode2(mt9p031,
812 ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
813 ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
814 if (ret < 0)
815 return ret;
816
817 return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
818 ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
819
820 case V4L2_CID_BLC_TARGET_LEVEL:
821 return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
822 ctrl->val);
823
824 case V4L2_CID_BLC_ANALOG_OFFSET:
825 data = ctrl->val & ((1 << 9) - 1);
826
827 ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
828 if (ret < 0)
829 return ret;
830 ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
831 if (ret < 0)
832 return ret;
833 ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
834 if (ret < 0)
835 return ret;
836 return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
837
838 case V4L2_CID_BLC_DIGITAL_OFFSET:
839 return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
840 ctrl->val & ((1 << 12) - 1));
841 }
842
843 return 0;
844 }
845
846 static struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
847 .s_ctrl = mt9p031_s_ctrl,
848 };
849
850 static const char * const mt9p031_test_pattern_menu[] = {
851 "Disabled",
852 "Color Field",
853 "Horizontal Gradient",
854 "Vertical Gradient",
855 "Diagonal Gradient",
856 "Classic Test Pattern",
857 "Walking 1s",
858 "Monochrome Horizontal Bars",
859 "Monochrome Vertical Bars",
860 "Vertical Color Bars",
861 };
862
863 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
864 {
865 .ops = &mt9p031_ctrl_ops,
866 .id = V4L2_CID_BLC_AUTO,
867 .type = V4L2_CTRL_TYPE_BOOLEAN,
868 .name = "BLC, Auto",
869 .min = 0,
870 .max = 1,
871 .step = 1,
872 .def = 1,
873 .flags = 0,
874 }, {
875 .ops = &mt9p031_ctrl_ops,
876 .id = V4L2_CID_BLC_TARGET_LEVEL,
877 .type = V4L2_CTRL_TYPE_INTEGER,
878 .name = "BLC Target Level",
879 .min = 0,
880 .max = 4095,
881 .step = 1,
882 .def = 168,
883 .flags = 0,
884 }, {
885 .ops = &mt9p031_ctrl_ops,
886 .id = V4L2_CID_BLC_ANALOG_OFFSET,
887 .type = V4L2_CTRL_TYPE_INTEGER,
888 .name = "BLC Analog Offset",
889 .min = -255,
890 .max = 255,
891 .step = 1,
892 .def = 32,
893 .flags = 0,
894 }, {
895 .ops = &mt9p031_ctrl_ops,
896 .id = V4L2_CID_BLC_DIGITAL_OFFSET,
897 .type = V4L2_CTRL_TYPE_INTEGER,
898 .name = "BLC Digital Offset",
899 .min = -2048,
900 .max = 2047,
901 .step = 1,
902 .def = 40,
903 .flags = 0,
904 }
905 };
906
907 /* -----------------------------------------------------------------------------
908 * V4L2 subdev core operations
909 */
910
mt9p031_set_power(struct v4l2_subdev * subdev,int on)911 static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
912 {
913 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
914 int ret = 0;
915
916 mutex_lock(&mt9p031->power_lock);
917
918 /* If the power count is modified from 0 to != 0 or from != 0 to 0,
919 * update the power state.
920 */
921 if (mt9p031->power_count == !on) {
922 ret = __mt9p031_set_power(mt9p031, !!on);
923 if (ret < 0)
924 goto out;
925 }
926
927 /* Update the power count. */
928 mt9p031->power_count += on ? 1 : -1;
929 WARN_ON(mt9p031->power_count < 0);
930
931 out:
932 mutex_unlock(&mt9p031->power_lock);
933 return ret;
934 }
935
936 /* -----------------------------------------------------------------------------
937 * V4L2 subdev internal operations
938 */
939
mt9p031_registered(struct v4l2_subdev * subdev)940 static int mt9p031_registered(struct v4l2_subdev *subdev)
941 {
942 struct i2c_client *client = v4l2_get_subdevdata(subdev);
943 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
944 s32 data;
945 int ret;
946
947 ret = mt9p031_power_on(mt9p031);
948 if (ret < 0) {
949 dev_err(&client->dev, "MT9P031 power up failed\n");
950 return ret;
951 }
952
953 /* Read out the chip version register */
954 data = mt9p031_read(client, MT9P031_CHIP_VERSION);
955 mt9p031_power_off(mt9p031);
956
957 if (data != MT9P031_CHIP_VERSION_VALUE) {
958 dev_err(&client->dev, "MT9P031 not detected, wrong version "
959 "0x%04x\n", data);
960 return -ENODEV;
961 }
962
963 dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
964 client->addr);
965
966 return 0;
967 }
968
mt9p031_open(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh)969 static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
970 {
971 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
972 struct v4l2_mbus_framefmt *format;
973 struct v4l2_rect *crop;
974
975 crop = v4l2_subdev_get_try_crop(subdev, fh->pad, 0);
976 crop->left = MT9P031_COLUMN_START_DEF;
977 crop->top = MT9P031_ROW_START_DEF;
978 crop->width = MT9P031_WINDOW_WIDTH_DEF;
979 crop->height = MT9P031_WINDOW_HEIGHT_DEF;
980
981 format = v4l2_subdev_get_try_format(subdev, fh->pad, 0);
982
983 if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
984 format->code = MEDIA_BUS_FMT_Y12_1X12;
985 else
986 format->code = MEDIA_BUS_FMT_SGRBG12_1X12;
987
988 format->width = MT9P031_WINDOW_WIDTH_DEF;
989 format->height = MT9P031_WINDOW_HEIGHT_DEF;
990 format->field = V4L2_FIELD_NONE;
991 format->colorspace = V4L2_COLORSPACE_SRGB;
992
993 return mt9p031_set_power(subdev, 1);
994 }
995
mt9p031_close(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh)996 static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
997 {
998 return mt9p031_set_power(subdev, 0);
999 }
1000
1001 static struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
1002 .s_power = mt9p031_set_power,
1003 };
1004
1005 static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
1006 .s_stream = mt9p031_s_stream,
1007 };
1008
1009 static struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
1010 .enum_mbus_code = mt9p031_enum_mbus_code,
1011 .enum_frame_size = mt9p031_enum_frame_size,
1012 .get_fmt = mt9p031_get_format,
1013 .set_fmt = mt9p031_set_format,
1014 .get_selection = mt9p031_get_selection,
1015 .set_selection = mt9p031_set_selection,
1016 };
1017
1018 static struct v4l2_subdev_ops mt9p031_subdev_ops = {
1019 .core = &mt9p031_subdev_core_ops,
1020 .video = &mt9p031_subdev_video_ops,
1021 .pad = &mt9p031_subdev_pad_ops,
1022 };
1023
1024 static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
1025 .registered = mt9p031_registered,
1026 .open = mt9p031_open,
1027 .close = mt9p031_close,
1028 };
1029
1030 /* -----------------------------------------------------------------------------
1031 * Driver initialization and probing
1032 */
1033
1034 static struct mt9p031_platform_data *
mt9p031_get_pdata(struct i2c_client * client)1035 mt9p031_get_pdata(struct i2c_client *client)
1036 {
1037 struct mt9p031_platform_data *pdata;
1038 struct device_node *np;
1039
1040 if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
1041 return client->dev.platform_data;
1042
1043 np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
1044 if (!np)
1045 return NULL;
1046
1047 pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
1048 if (!pdata)
1049 goto done;
1050
1051 of_property_read_u32(np, "input-clock-frequency", &pdata->ext_freq);
1052 of_property_read_u32(np, "pixel-clock-frequency", &pdata->target_freq);
1053
1054 done:
1055 of_node_put(np);
1056 return pdata;
1057 }
1058
mt9p031_probe(struct i2c_client * client,const struct i2c_device_id * did)1059 static int mt9p031_probe(struct i2c_client *client,
1060 const struct i2c_device_id *did)
1061 {
1062 struct mt9p031_platform_data *pdata = mt9p031_get_pdata(client);
1063 struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
1064 struct mt9p031 *mt9p031;
1065 unsigned int i;
1066 int ret;
1067
1068 if (pdata == NULL) {
1069 dev_err(&client->dev, "No platform data\n");
1070 return -EINVAL;
1071 }
1072
1073 if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1074 dev_warn(&client->dev,
1075 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1076 return -EIO;
1077 }
1078
1079 mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
1080 if (mt9p031 == NULL)
1081 return -ENOMEM;
1082
1083 mt9p031->pdata = pdata;
1084 mt9p031->output_control = MT9P031_OUTPUT_CONTROL_DEF;
1085 mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
1086 mt9p031->model = did->driver_data;
1087
1088 mt9p031->regulators[0].supply = "vdd";
1089 mt9p031->regulators[1].supply = "vdd_io";
1090 mt9p031->regulators[2].supply = "vaa";
1091
1092 ret = devm_regulator_bulk_get(&client->dev, 3, mt9p031->regulators);
1093 if (ret < 0) {
1094 dev_err(&client->dev, "Unable to get regulators\n");
1095 return ret;
1096 }
1097
1098 mutex_init(&mt9p031->power_lock);
1099
1100 v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
1101
1102 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1103 V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
1104 MT9P031_SHUTTER_WIDTH_MAX, 1,
1105 MT9P031_SHUTTER_WIDTH_DEF);
1106 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1107 V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
1108 MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
1109 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1110 V4L2_CID_HFLIP, 0, 1, 1, 0);
1111 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1112 V4L2_CID_VFLIP, 0, 1, 1, 0);
1113 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1114 V4L2_CID_PIXEL_RATE, pdata->target_freq,
1115 pdata->target_freq, 1, pdata->target_freq);
1116 v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1117 V4L2_CID_TEST_PATTERN,
1118 ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
1119 0, mt9p031_test_pattern_menu);
1120
1121 for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
1122 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
1123
1124 mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
1125
1126 if (mt9p031->ctrls.error) {
1127 printk(KERN_INFO "%s: control initialization error %d\n",
1128 __func__, mt9p031->ctrls.error);
1129 ret = mt9p031->ctrls.error;
1130 goto done;
1131 }
1132
1133 mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
1134 mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
1135 V4L2_CID_BLC_DIGITAL_OFFSET);
1136
1137 v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
1138 mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
1139
1140 mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
1141 ret = media_entity_init(&mt9p031->subdev.entity, 1, &mt9p031->pad, 0);
1142 if (ret < 0)
1143 goto done;
1144
1145 mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1146
1147 mt9p031->crop.width = MT9P031_WINDOW_WIDTH_DEF;
1148 mt9p031->crop.height = MT9P031_WINDOW_HEIGHT_DEF;
1149 mt9p031->crop.left = MT9P031_COLUMN_START_DEF;
1150 mt9p031->crop.top = MT9P031_ROW_START_DEF;
1151
1152 if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
1153 mt9p031->format.code = MEDIA_BUS_FMT_Y12_1X12;
1154 else
1155 mt9p031->format.code = MEDIA_BUS_FMT_SGRBG12_1X12;
1156
1157 mt9p031->format.width = MT9P031_WINDOW_WIDTH_DEF;
1158 mt9p031->format.height = MT9P031_WINDOW_HEIGHT_DEF;
1159 mt9p031->format.field = V4L2_FIELD_NONE;
1160 mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
1161
1162 mt9p031->reset = devm_gpiod_get_optional(&client->dev, "reset",
1163 GPIOD_OUT_HIGH);
1164
1165 ret = mt9p031_clk_setup(mt9p031);
1166 if (ret)
1167 goto done;
1168
1169 ret = v4l2_async_register_subdev(&mt9p031->subdev);
1170
1171 done:
1172 if (ret < 0) {
1173 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1174 media_entity_cleanup(&mt9p031->subdev.entity);
1175 mutex_destroy(&mt9p031->power_lock);
1176 }
1177
1178 return ret;
1179 }
1180
mt9p031_remove(struct i2c_client * client)1181 static int mt9p031_remove(struct i2c_client *client)
1182 {
1183 struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1184 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1185
1186 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1187 v4l2_async_unregister_subdev(subdev);
1188 media_entity_cleanup(&subdev->entity);
1189 mutex_destroy(&mt9p031->power_lock);
1190
1191 return 0;
1192 }
1193
1194 static const struct i2c_device_id mt9p031_id[] = {
1195 { "mt9p031", MT9P031_MODEL_COLOR },
1196 { "mt9p031m", MT9P031_MODEL_MONOCHROME },
1197 { }
1198 };
1199 MODULE_DEVICE_TABLE(i2c, mt9p031_id);
1200
1201 #if IS_ENABLED(CONFIG_OF)
1202 static const struct of_device_id mt9p031_of_match[] = {
1203 { .compatible = "aptina,mt9p031", },
1204 { .compatible = "aptina,mt9p031m", },
1205 { /* sentinel */ },
1206 };
1207 MODULE_DEVICE_TABLE(of, mt9p031_of_match);
1208 #endif
1209
1210 static struct i2c_driver mt9p031_i2c_driver = {
1211 .driver = {
1212 .of_match_table = of_match_ptr(mt9p031_of_match),
1213 .name = "mt9p031",
1214 },
1215 .probe = mt9p031_probe,
1216 .remove = mt9p031_remove,
1217 .id_table = mt9p031_id,
1218 };
1219
1220 module_i2c_driver(mt9p031_i2c_driver);
1221
1222 MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
1223 MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
1224 MODULE_LICENSE("GPL v2");
1225