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