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