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