xref: /openbmc/linux/drivers/media/i2c/mt9v111.c (revision aaeb31c0)
1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * V4L2 sensor driver for Aptina MT9V111 image sensor
4  * Copyright (C) 2018 Jacopo Mondi <jacopo@jmondi.org>
5  *
6  * Based on mt9v032 driver
7  * Copyright (C) 2010, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
8  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
9  *
10  * Based on mt9v011 driver
11  * Copyright (c) 2009 Mauro Carvalho Chehab <mchehab@kernel.org>
12  */
13 
14 #include <linux/clk.h>
15 #include <linux/delay.h>
16 #include <linux/gpio/consumer.h>
17 #include <linux/i2c.h>
18 #include <linux/of.h>
19 #include <linux/slab.h>
20 #include <linux/videodev2.h>
21 #include <linux/v4l2-mediabus.h>
22 #include <linux/module.h>
23 
24 #include <media/v4l2-ctrls.h>
25 #include <media/v4l2-device.h>
26 #include <media/v4l2-fwnode.h>
27 #include <media/v4l2-image-sizes.h>
28 #include <media/v4l2-subdev.h>
29 
30 /*
31  * MT9V111 is a 1/4-Inch CMOS digital image sensor with an integrated
32  * Image Flow Processing (IFP) engine and a sensor core loosely based on
33  * MT9V011.
34  *
35  * The IFP can produce several output image formats from the sensor core
36  * output. This driver currently supports only YUYV format permutations.
37  *
38  * The driver allows manual frame rate control through s_frame_interval subdev
39  * operation or V4L2_CID_V/HBLANK controls, but it is known that the
40  * auto-exposure algorithm might modify the programmed frame rate. While the
41  * driver initially programs the sensor with auto-exposure and
42  * auto-white-balancing enabled, it is possible to disable them and more
43  * precisely control the frame rate.
44  *
45  * While it seems possible to instruct the auto-exposure control algorithm to
46  * respect a programmed frame rate when adjusting the pixel integration time,
47  * registers controlling this feature are not documented in the public
48  * available sensor manual used to develop this driver (09005aef80e90084,
49  * MT9V111_1.fm - Rev. G 1/05 EN).
50  */
51 
52 #define MT9V111_CHIP_ID_HIGH				0x82
53 #define MT9V111_CHIP_ID_LOW				0x3a
54 
55 #define MT9V111_R01_ADDR_SPACE				0x01
56 #define MT9V111_R01_IFP					0x01
57 #define MT9V111_R01_CORE				0x04
58 
59 #define MT9V111_IFP_R06_OPMODE_CTRL			0x06
60 #define		MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN	BIT(1)
61 #define		MT9V111_IFP_R06_OPMODE_CTRL_AE_EN	BIT(14)
62 #define MT9V111_IFP_R07_IFP_RESET			0x07
63 #define		MT9V111_IFP_R07_IFP_RESET_MASK		BIT(0)
64 #define MT9V111_IFP_R08_OUTFMT_CTRL			0x08
65 #define		MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER	BIT(11)
66 #define		MT9V111_IFP_R08_OUTFMT_CTRL_PCLK	BIT(5)
67 #define MT9V111_IFP_R3A_OUTFMT_CTRL2			0x3a
68 #define		MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR	BIT(0)
69 #define		MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC	BIT(1)
70 #define		MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK	GENMASK(2, 0)
71 #define MT9V111_IFP_RA5_HPAN				0xa5
72 #define MT9V111_IFP_RA6_HZOOM				0xa6
73 #define MT9V111_IFP_RA7_HOUT				0xa7
74 #define MT9V111_IFP_RA8_VPAN				0xa8
75 #define MT9V111_IFP_RA9_VZOOM				0xa9
76 #define MT9V111_IFP_RAA_VOUT				0xaa
77 #define MT9V111_IFP_DECIMATION_MASK			GENMASK(9, 0)
78 #define MT9V111_IFP_DECIMATION_FREEZE			BIT(15)
79 
80 #define MT9V111_CORE_R03_WIN_HEIGHT			0x03
81 #define		MT9V111_CORE_R03_WIN_V_OFFS		2
82 #define MT9V111_CORE_R04_WIN_WIDTH			0x04
83 #define		MT9V111_CORE_R04_WIN_H_OFFS		114
84 #define MT9V111_CORE_R05_HBLANK				0x05
85 #define		MT9V111_CORE_R05_MIN_HBLANK		0x09
86 #define		MT9V111_CORE_R05_MAX_HBLANK		GENMASK(9, 0)
87 #define		MT9V111_CORE_R05_DEF_HBLANK		0x26
88 #define MT9V111_CORE_R06_VBLANK				0x06
89 #define		MT9V111_CORE_R06_MIN_VBLANK		0x03
90 #define		MT9V111_CORE_R06_MAX_VBLANK		GENMASK(11, 0)
91 #define		MT9V111_CORE_R06_DEF_VBLANK		0x04
92 #define MT9V111_CORE_R07_OUT_CTRL			0x07
93 #define		MT9V111_CORE_R07_OUT_CTRL_SAMPLE	BIT(4)
94 #define MT9V111_CORE_R09_PIXEL_INT			0x09
95 #define		MT9V111_CORE_R09_PIXEL_INT_MASK		GENMASK(11, 0)
96 #define MT9V111_CORE_R0D_CORE_RESET			0x0d
97 #define		MT9V111_CORE_R0D_CORE_RESET_MASK	BIT(0)
98 #define MT9V111_CORE_RFF_CHIP_VER			0xff
99 
100 #define MT9V111_PIXEL_ARRAY_WIDTH			640
101 #define MT9V111_PIXEL_ARRAY_HEIGHT			480
102 
103 #define MT9V111_MAX_CLKIN				27000000
104 
105 /* The default sensor configuration at startup time. */
106 static const struct v4l2_mbus_framefmt mt9v111_def_fmt = {
107 	.width		= 640,
108 	.height		= 480,
109 	.code		= MEDIA_BUS_FMT_UYVY8_2X8,
110 	.field		= V4L2_FIELD_NONE,
111 	.colorspace	= V4L2_COLORSPACE_SRGB,
112 	.ycbcr_enc	= V4L2_YCBCR_ENC_601,
113 	.quantization	= V4L2_QUANTIZATION_LIM_RANGE,
114 	.xfer_func	= V4L2_XFER_FUNC_SRGB,
115 };
116 
117 struct mt9v111_dev {
118 	struct device *dev;
119 	struct i2c_client *client;
120 
121 	u8 addr_space;
122 
123 	struct v4l2_subdev sd;
124 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
125 	struct media_pad pad;
126 #endif
127 
128 	struct v4l2_ctrl *auto_awb;
129 	struct v4l2_ctrl *auto_exp;
130 	struct v4l2_ctrl *hblank;
131 	struct v4l2_ctrl *vblank;
132 	struct v4l2_ctrl_handler ctrls;
133 
134 	/* Output image format and sizes. */
135 	struct v4l2_mbus_framefmt fmt;
136 	unsigned int fps;
137 
138 	/* Protects power up/down sequences. */
139 	struct mutex pwr_mutex;
140 	int pwr_count;
141 
142 	/* Protects stream on/off sequences. */
143 	struct mutex stream_mutex;
144 	bool streaming;
145 
146 	/* Flags to mark HW settings as not yet applied. */
147 	bool pending;
148 
149 	/* Clock provider and system clock frequency. */
150 	struct clk *clk;
151 	u32 sysclk;
152 
153 	struct gpio_desc *oe;
154 	struct gpio_desc *standby;
155 	struct gpio_desc *reset;
156 };
157 
158 #define sd_to_mt9v111(__sd) container_of((__sd), struct mt9v111_dev, sd)
159 
160 /*
161  * mt9v111_mbus_fmt - List all media bus formats supported by the driver.
162  *
163  * Only list the media bus code here. The image sizes are freely configurable
164  * in the pixel array sizes range.
165  *
166  * The desired frame interval, in the supported frame interval range, is
167  * obtained by configuring blanking as the sensor does not have a PLL but
168  * only a fixed clock divider that generates the output pixel clock.
169  */
170 static struct mt9v111_mbus_fmt {
171 	u32	code;
172 } mt9v111_formats[] = {
173 	{
174 		.code	= MEDIA_BUS_FMT_UYVY8_2X8,
175 	},
176 	{
177 		.code	= MEDIA_BUS_FMT_YUYV8_2X8,
178 	},
179 	{
180 		.code	= MEDIA_BUS_FMT_VYUY8_2X8,
181 	},
182 	{
183 		.code	= MEDIA_BUS_FMT_YVYU8_2X8,
184 	},
185 };
186 
187 static u32 mt9v111_frame_intervals[] = {5, 10, 15, 20, 30};
188 
189 /*
190  * mt9v111_frame_sizes - List sensor's supported resolutions.
191  *
192  * Resolution generated through decimation in the IFP block from the
193  * full VGA pixel array.
194  */
195 static struct v4l2_rect mt9v111_frame_sizes[] = {
196 	{
197 		.width	= 640,
198 		.height	= 480,
199 	},
200 	{
201 		.width	= 352,
202 		.height	= 288
203 	},
204 	{
205 		.width	= 320,
206 		.height	= 240,
207 	},
208 	{
209 		.width	= 176,
210 		.height	= 144,
211 	},
212 	{
213 		.width	= 160,
214 		.height	= 120,
215 	},
216 };
217 
218 /* --- Device I/O access --- */
219 
__mt9v111_read(struct i2c_client * c,u8 reg,u16 * val)220 static int __mt9v111_read(struct i2c_client *c, u8 reg, u16 *val)
221 {
222 	struct i2c_msg msg[2];
223 	__be16 buf;
224 	int ret;
225 
226 	msg[0].addr = c->addr;
227 	msg[0].flags = 0;
228 	msg[0].len = 1;
229 	msg[0].buf = &reg;
230 
231 	msg[1].addr = c->addr;
232 	msg[1].flags = I2C_M_RD;
233 	msg[1].len = 2;
234 	msg[1].buf = (char *)&buf;
235 
236 	ret = i2c_transfer(c->adapter, msg, 2);
237 	if (ret < 0) {
238 		dev_err(&c->dev, "i2c read transfer error: %d\n", ret);
239 		return ret;
240 	}
241 
242 	*val = be16_to_cpu(buf);
243 
244 	dev_dbg(&c->dev, "%s: %x=%x\n", __func__, reg, *val);
245 
246 	return 0;
247 }
248 
__mt9v111_write(struct i2c_client * c,u8 reg,u16 val)249 static int __mt9v111_write(struct i2c_client *c, u8 reg, u16 val)
250 {
251 	struct i2c_msg msg;
252 	u8 buf[3] = { 0 };
253 	int ret;
254 
255 	buf[0] = reg;
256 	buf[1] = val >> 8;
257 	buf[2] = val & 0xff;
258 
259 	msg.addr = c->addr;
260 	msg.flags = 0;
261 	msg.len = 3;
262 	msg.buf = (char *)buf;
263 
264 	dev_dbg(&c->dev, "%s: %x = %x%x\n", __func__, reg, buf[1], buf[2]);
265 
266 	ret = i2c_transfer(c->adapter, &msg, 1);
267 	if (ret < 0) {
268 		dev_err(&c->dev, "i2c write transfer error: %d\n", ret);
269 		return ret;
270 	}
271 
272 	return 0;
273 }
274 
__mt9v111_addr_space_select(struct i2c_client * c,u16 addr_space)275 static int __mt9v111_addr_space_select(struct i2c_client *c, u16 addr_space)
276 {
277 	struct v4l2_subdev *sd = i2c_get_clientdata(c);
278 	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
279 	u16 val;
280 	int ret;
281 
282 	if (mt9v111->addr_space == addr_space)
283 		return 0;
284 
285 	ret = __mt9v111_write(c, MT9V111_R01_ADDR_SPACE, addr_space);
286 	if (ret)
287 		return ret;
288 
289 	/* Verify address space has been updated */
290 	ret = __mt9v111_read(c, MT9V111_R01_ADDR_SPACE, &val);
291 	if (ret)
292 		return ret;
293 
294 	if (val != addr_space)
295 		return -EINVAL;
296 
297 	mt9v111->addr_space = addr_space;
298 
299 	return 0;
300 }
301 
mt9v111_read(struct i2c_client * c,u8 addr_space,u8 reg,u16 * val)302 static int mt9v111_read(struct i2c_client *c, u8 addr_space, u8 reg, u16 *val)
303 {
304 	int ret;
305 
306 	/* Select register address space first. */
307 	ret = __mt9v111_addr_space_select(c, addr_space);
308 	if (ret)
309 		return ret;
310 
311 	ret = __mt9v111_read(c, reg, val);
312 	if (ret)
313 		return ret;
314 
315 	return 0;
316 }
317 
mt9v111_write(struct i2c_client * c,u8 addr_space,u8 reg,u16 val)318 static int mt9v111_write(struct i2c_client *c, u8 addr_space, u8 reg, u16 val)
319 {
320 	int ret;
321 
322 	/* Select register address space first. */
323 	ret = __mt9v111_addr_space_select(c, addr_space);
324 	if (ret)
325 		return ret;
326 
327 	ret = __mt9v111_write(c, reg, val);
328 	if (ret)
329 		return ret;
330 
331 	return 0;
332 }
333 
mt9v111_update(struct i2c_client * c,u8 addr_space,u8 reg,u16 mask,u16 val)334 static int mt9v111_update(struct i2c_client *c, u8 addr_space, u8 reg,
335 			  u16 mask, u16 val)
336 {
337 	u16 current_val;
338 	int ret;
339 
340 	/* Select register address space first. */
341 	ret = __mt9v111_addr_space_select(c, addr_space);
342 	if (ret)
343 		return ret;
344 
345 	/* Read the current register value, then update it. */
346 	ret = __mt9v111_read(c, reg, &current_val);
347 	if (ret)
348 		return ret;
349 
350 	current_val &= ~mask;
351 	current_val |= (val & mask);
352 	ret = __mt9v111_write(c, reg, current_val);
353 	if (ret)
354 		return ret;
355 
356 	return 0;
357 }
358 
359 /* --- Sensor HW operations --- */
360 
__mt9v111_power_on(struct v4l2_subdev * sd)361 static int __mt9v111_power_on(struct v4l2_subdev *sd)
362 {
363 	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
364 	int ret;
365 
366 	ret = clk_prepare_enable(mt9v111->clk);
367 	if (ret)
368 		return ret;
369 
370 	clk_set_rate(mt9v111->clk, mt9v111->sysclk);
371 
372 	gpiod_set_value(mt9v111->standby, 0);
373 	usleep_range(500, 1000);
374 
375 	gpiod_set_value(mt9v111->oe, 1);
376 	usleep_range(500, 1000);
377 
378 	return 0;
379 }
380 
__mt9v111_power_off(struct v4l2_subdev * sd)381 static int __mt9v111_power_off(struct v4l2_subdev *sd)
382 {
383 	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
384 
385 	gpiod_set_value(mt9v111->oe, 0);
386 	usleep_range(500, 1000);
387 
388 	gpiod_set_value(mt9v111->standby, 1);
389 	usleep_range(500, 1000);
390 
391 	clk_disable_unprepare(mt9v111->clk);
392 
393 	return 0;
394 }
395 
__mt9v111_hw_reset(struct mt9v111_dev * mt9v111)396 static int __mt9v111_hw_reset(struct mt9v111_dev *mt9v111)
397 {
398 	if (!mt9v111->reset)
399 		return -EINVAL;
400 
401 	gpiod_set_value(mt9v111->reset, 1);
402 	usleep_range(500, 1000);
403 
404 	gpiod_set_value(mt9v111->reset, 0);
405 	usleep_range(500, 1000);
406 
407 	return 0;
408 }
409 
__mt9v111_sw_reset(struct mt9v111_dev * mt9v111)410 static int __mt9v111_sw_reset(struct mt9v111_dev *mt9v111)
411 {
412 	struct i2c_client *c = mt9v111->client;
413 	int ret;
414 
415 	/* Software reset core and IFP blocks. */
416 
417 	ret = mt9v111_update(c, MT9V111_R01_CORE,
418 			     MT9V111_CORE_R0D_CORE_RESET,
419 			     MT9V111_CORE_R0D_CORE_RESET_MASK, 1);
420 	if (ret)
421 		return ret;
422 	usleep_range(500, 1000);
423 
424 	ret = mt9v111_update(c, MT9V111_R01_CORE,
425 			     MT9V111_CORE_R0D_CORE_RESET,
426 			     MT9V111_CORE_R0D_CORE_RESET_MASK, 0);
427 	if (ret)
428 		return ret;
429 	usleep_range(500, 1000);
430 
431 	ret = mt9v111_update(c, MT9V111_R01_IFP,
432 			     MT9V111_IFP_R07_IFP_RESET,
433 			     MT9V111_IFP_R07_IFP_RESET_MASK, 1);
434 	if (ret)
435 		return ret;
436 	usleep_range(500, 1000);
437 
438 	ret = mt9v111_update(c, MT9V111_R01_IFP,
439 			     MT9V111_IFP_R07_IFP_RESET,
440 			     MT9V111_IFP_R07_IFP_RESET_MASK, 0);
441 	if (ret)
442 		return ret;
443 	usleep_range(500, 1000);
444 
445 	return 0;
446 }
447 
mt9v111_calc_frame_rate(struct mt9v111_dev * mt9v111,struct v4l2_fract * tpf)448 static int mt9v111_calc_frame_rate(struct mt9v111_dev *mt9v111,
449 				   struct v4l2_fract *tpf)
450 {
451 	unsigned int fps = tpf->numerator ?
452 			   tpf->denominator / tpf->numerator :
453 			   tpf->denominator;
454 	unsigned int best_diff;
455 	unsigned int frm_cols;
456 	unsigned int row_pclk;
457 	unsigned int best_fps;
458 	unsigned int pclk;
459 	unsigned int diff;
460 	unsigned int idx;
461 	unsigned int hb;
462 	unsigned int vb;
463 	unsigned int i;
464 	int ret;
465 
466 	/* Approximate to the closest supported frame interval. */
467 	best_diff = ~0L;
468 	for (i = 0, idx = 0; i < ARRAY_SIZE(mt9v111_frame_intervals); i++) {
469 		diff = abs(fps - mt9v111_frame_intervals[i]);
470 		if (diff < best_diff) {
471 			idx = i;
472 			best_diff = diff;
473 		}
474 	}
475 	fps = mt9v111_frame_intervals[idx];
476 
477 	/*
478 	 * The sensor does not provide a PLL circuitry and pixel clock is
479 	 * generated dividing the master clock source by two.
480 	 *
481 	 * Trow = (W + Hblank + 114) * 2 * (1 / SYSCLK)
482 	 * TFrame = Trow * (H + Vblank + 2)
483 	 *
484 	 * FPS = (SYSCLK / 2) / (Trow * (H + Vblank + 2))
485 	 *
486 	 * This boils down to tune H and V blanks to best approximate the
487 	 * above equation.
488 	 *
489 	 * Test all available H/V blank values, until we reach the
490 	 * desired frame rate.
491 	 */
492 	best_fps = vb = hb = 0;
493 	pclk = DIV_ROUND_CLOSEST(mt9v111->sysclk, 2);
494 	row_pclk = MT9V111_PIXEL_ARRAY_WIDTH + 7 + MT9V111_CORE_R04_WIN_H_OFFS;
495 	frm_cols = MT9V111_PIXEL_ARRAY_HEIGHT + 7 + MT9V111_CORE_R03_WIN_V_OFFS;
496 
497 	best_diff = ~0L;
498 	for (vb = MT9V111_CORE_R06_MIN_VBLANK;
499 	     vb < MT9V111_CORE_R06_MAX_VBLANK; vb++) {
500 		for (hb = MT9V111_CORE_R05_MIN_HBLANK;
501 		     hb < MT9V111_CORE_R05_MAX_HBLANK; hb += 10) {
502 			unsigned int t_frame = (row_pclk + hb) *
503 					       (frm_cols + vb);
504 			unsigned int t_fps = DIV_ROUND_CLOSEST(pclk, t_frame);
505 
506 			diff = abs(fps - t_fps);
507 			if (diff < best_diff) {
508 				best_diff = diff;
509 				best_fps = t_fps;
510 
511 				if (diff == 0)
512 					break;
513 			}
514 		}
515 
516 		if (diff == 0)
517 			break;
518 	}
519 
520 	ret = v4l2_ctrl_s_ctrl_int64(mt9v111->hblank, hb);
521 	if (ret)
522 		return ret;
523 
524 	ret = v4l2_ctrl_s_ctrl_int64(mt9v111->vblank, vb);
525 	if (ret)
526 		return ret;
527 
528 	tpf->numerator = 1;
529 	tpf->denominator = best_fps;
530 
531 	return 0;
532 }
533 
mt9v111_hw_config(struct mt9v111_dev * mt9v111)534 static int mt9v111_hw_config(struct mt9v111_dev *mt9v111)
535 {
536 	struct i2c_client *c = mt9v111->client;
537 	unsigned int ret;
538 	u16 outfmtctrl2;
539 
540 	/* Force device reset. */
541 	ret = __mt9v111_hw_reset(mt9v111);
542 	if (ret == -EINVAL)
543 		ret = __mt9v111_sw_reset(mt9v111);
544 	if (ret)
545 		return ret;
546 
547 	/* Configure internal clock sample rate. */
548 	ret = mt9v111->sysclk < DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ?
549 				mt9v111_update(c, MT9V111_R01_CORE,
550 					MT9V111_CORE_R07_OUT_CTRL,
551 					MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 1) :
552 				mt9v111_update(c, MT9V111_R01_CORE,
553 					MT9V111_CORE_R07_OUT_CTRL,
554 					MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 0);
555 	if (ret)
556 		return ret;
557 
558 	/*
559 	 * Configure output image format components ordering.
560 	 *
561 	 * TODO: IFP block can also output several RGB permutations, we only
562 	 *	 support YUYV permutations at the moment.
563 	 */
564 	switch (mt9v111->fmt.code) {
565 	case MEDIA_BUS_FMT_YUYV8_2X8:
566 			outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC;
567 			break;
568 	case MEDIA_BUS_FMT_VYUY8_2X8:
569 			outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR;
570 			break;
571 	case MEDIA_BUS_FMT_YVYU8_2X8:
572 			outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC |
573 				      MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR;
574 			break;
575 	case MEDIA_BUS_FMT_UYVY8_2X8:
576 	default:
577 			outfmtctrl2 = 0;
578 			break;
579 	}
580 
581 	ret = mt9v111_update(c, MT9V111_R01_IFP, MT9V111_IFP_R3A_OUTFMT_CTRL2,
582 			     MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK,
583 			     outfmtctrl2);
584 	if (ret)
585 		return ret;
586 
587 	/*
588 	 * Do not change default sensor's core configuration:
589 	 * output the whole 640x480 pixel array, skip 18 columns and 6 rows.
590 	 *
591 	 * Instead, control the output image size through IFP block.
592 	 *
593 	 * TODO: No zoom&pan support. Currently we control the output image
594 	 *	 size only through decimation, with no zoom support.
595 	 */
596 	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA5_HPAN,
597 			    MT9V111_IFP_DECIMATION_FREEZE);
598 	if (ret)
599 		return ret;
600 
601 	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA8_VPAN,
602 			    MT9V111_IFP_DECIMATION_FREEZE);
603 	if (ret)
604 		return ret;
605 
606 	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA6_HZOOM,
607 			    MT9V111_IFP_DECIMATION_FREEZE |
608 			    MT9V111_PIXEL_ARRAY_WIDTH);
609 	if (ret)
610 		return ret;
611 
612 	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA9_VZOOM,
613 			    MT9V111_IFP_DECIMATION_FREEZE |
614 			    MT9V111_PIXEL_ARRAY_HEIGHT);
615 	if (ret)
616 		return ret;
617 
618 	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA7_HOUT,
619 			    MT9V111_IFP_DECIMATION_FREEZE |
620 			    mt9v111->fmt.width);
621 	if (ret)
622 		return ret;
623 
624 	ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RAA_VOUT,
625 			    mt9v111->fmt.height);
626 	if (ret)
627 		return ret;
628 
629 	/* Apply controls to set auto exp, auto awb and timings */
630 	ret = v4l2_ctrl_handler_setup(&mt9v111->ctrls);
631 	if (ret)
632 		return ret;
633 
634 	/*
635 	 * Set pixel integration time to the whole frame time.
636 	 * This value controls the shutter delay when running with AE
637 	 * disabled. If longer than frame time, it affects the output
638 	 * frame rate.
639 	 */
640 	return mt9v111_write(c, MT9V111_R01_CORE, MT9V111_CORE_R09_PIXEL_INT,
641 			     MT9V111_PIXEL_ARRAY_HEIGHT);
642 }
643 
644 /* ---  V4L2 subdev operations --- */
645 
mt9v111_s_power(struct v4l2_subdev * sd,int on)646 static int mt9v111_s_power(struct v4l2_subdev *sd, int on)
647 {
648 	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
649 	int pwr_count;
650 	int ret = 0;
651 
652 	mutex_lock(&mt9v111->pwr_mutex);
653 
654 	/*
655 	 * Make sure we're transitioning from 0 to 1, or viceversa,
656 	 * before actually changing the power state.
657 	 */
658 	pwr_count = mt9v111->pwr_count;
659 	pwr_count += on ? 1 : -1;
660 	if (pwr_count == !!on) {
661 		ret = on ? __mt9v111_power_on(sd) :
662 			   __mt9v111_power_off(sd);
663 		if (!ret)
664 			/* All went well, updated power counter. */
665 			mt9v111->pwr_count = pwr_count;
666 
667 		mutex_unlock(&mt9v111->pwr_mutex);
668 
669 		return ret;
670 	}
671 
672 	/*
673 	 * Update power counter to keep track of how many nested calls we
674 	 * received.
675 	 */
676 	WARN_ON(pwr_count < 0 || pwr_count > 1);
677 	mt9v111->pwr_count = pwr_count;
678 
679 	mutex_unlock(&mt9v111->pwr_mutex);
680 
681 	return ret;
682 }
683 
mt9v111_s_stream(struct v4l2_subdev * subdev,int enable)684 static int mt9v111_s_stream(struct v4l2_subdev *subdev, int enable)
685 {
686 	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
687 	int ret;
688 
689 	mutex_lock(&mt9v111->stream_mutex);
690 
691 	if (mt9v111->streaming == enable) {
692 		mutex_unlock(&mt9v111->stream_mutex);
693 		return 0;
694 	}
695 
696 	ret = mt9v111_s_power(subdev, enable);
697 	if (ret)
698 		goto error_unlock;
699 
700 	if (enable && mt9v111->pending) {
701 		ret = mt9v111_hw_config(mt9v111);
702 		if (ret)
703 			goto error_unlock;
704 
705 		/*
706 		 * No need to update control here as far as only H/VBLANK are
707 		 * supported and immediately programmed to registers in .s_ctrl
708 		 */
709 
710 		mt9v111->pending = false;
711 	}
712 
713 	mt9v111->streaming = enable ? true : false;
714 	mutex_unlock(&mt9v111->stream_mutex);
715 
716 	return 0;
717 
718 error_unlock:
719 	mutex_unlock(&mt9v111->stream_mutex);
720 
721 	return ret;
722 }
723 
mt9v111_s_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_frame_interval * ival)724 static int mt9v111_s_frame_interval(struct v4l2_subdev *sd,
725 				    struct v4l2_subdev_frame_interval *ival)
726 {
727 	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
728 	struct v4l2_fract *tpf = &ival->interval;
729 	unsigned int fps = tpf->numerator ?
730 			   tpf->denominator / tpf->numerator :
731 			   tpf->denominator;
732 	unsigned int max_fps;
733 
734 	if (!tpf->numerator)
735 		tpf->numerator = 1;
736 
737 	mutex_lock(&mt9v111->stream_mutex);
738 
739 	if (mt9v111->streaming) {
740 		mutex_unlock(&mt9v111->stream_mutex);
741 		return -EBUSY;
742 	}
743 
744 	if (mt9v111->fps == fps) {
745 		mutex_unlock(&mt9v111->stream_mutex);
746 		return 0;
747 	}
748 
749 	/* Make sure frame rate/image sizes constraints are respected. */
750 	if (mt9v111->fmt.width < QVGA_WIDTH &&
751 	    mt9v111->fmt.height < QVGA_HEIGHT)
752 		max_fps = 90;
753 	else if (mt9v111->fmt.width < CIF_WIDTH &&
754 		 mt9v111->fmt.height < CIF_HEIGHT)
755 		max_fps = 60;
756 	else
757 		max_fps = mt9v111->sysclk <
758 				DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ? 15 :
759 									  30;
760 
761 	if (fps > max_fps) {
762 		mutex_unlock(&mt9v111->stream_mutex);
763 		return -EINVAL;
764 	}
765 
766 	mt9v111_calc_frame_rate(mt9v111, tpf);
767 
768 	mt9v111->fps = fps;
769 	mt9v111->pending = true;
770 
771 	mutex_unlock(&mt9v111->stream_mutex);
772 
773 	return 0;
774 }
775 
mt9v111_g_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_frame_interval * ival)776 static int mt9v111_g_frame_interval(struct v4l2_subdev *sd,
777 				    struct v4l2_subdev_frame_interval *ival)
778 {
779 	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
780 	struct v4l2_fract *tpf = &ival->interval;
781 
782 	mutex_lock(&mt9v111->stream_mutex);
783 
784 	tpf->numerator = 1;
785 	tpf->denominator = mt9v111->fps;
786 
787 	mutex_unlock(&mt9v111->stream_mutex);
788 
789 	return 0;
790 }
791 
__mt9v111_get_pad_format(struct mt9v111_dev * mt9v111,struct v4l2_subdev_state * sd_state,unsigned int pad,enum v4l2_subdev_format_whence which)792 static struct v4l2_mbus_framefmt *__mt9v111_get_pad_format(
793 					struct mt9v111_dev *mt9v111,
794 					struct v4l2_subdev_state *sd_state,
795 					unsigned int pad,
796 					enum v4l2_subdev_format_whence which)
797 {
798 	switch (which) {
799 	case V4L2_SUBDEV_FORMAT_TRY:
800 #if IS_ENABLED(CONFIG_VIDEO_V4L2_SUBDEV_API)
801 		return v4l2_subdev_get_try_format(&mt9v111->sd, sd_state, pad);
802 #else
803 		return &sd_state->pads->try_fmt;
804 #endif
805 	case V4L2_SUBDEV_FORMAT_ACTIVE:
806 		return &mt9v111->fmt;
807 	default:
808 		return NULL;
809 	}
810 }
811 
mt9v111_enum_mbus_code(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)812 static int mt9v111_enum_mbus_code(struct v4l2_subdev *subdev,
813 				  struct v4l2_subdev_state *sd_state,
814 				  struct v4l2_subdev_mbus_code_enum *code)
815 {
816 	if (code->pad || code->index > ARRAY_SIZE(mt9v111_formats) - 1)
817 		return -EINVAL;
818 
819 	code->code = mt9v111_formats[code->index].code;
820 
821 	return 0;
822 }
823 
mt9v111_enum_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_interval_enum * fie)824 static int mt9v111_enum_frame_interval(struct v4l2_subdev *sd,
825 				struct v4l2_subdev_state *sd_state,
826 				struct v4l2_subdev_frame_interval_enum *fie)
827 {
828 	unsigned int i;
829 
830 	if (fie->pad || fie->index >= ARRAY_SIZE(mt9v111_frame_intervals))
831 		return -EINVAL;
832 
833 	for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++)
834 		if (fie->width == mt9v111_frame_sizes[i].width &&
835 		    fie->height == mt9v111_frame_sizes[i].height)
836 			break;
837 
838 	if (i == ARRAY_SIZE(mt9v111_frame_sizes))
839 		return -EINVAL;
840 
841 	fie->interval.numerator = 1;
842 	fie->interval.denominator = mt9v111_frame_intervals[fie->index];
843 
844 	return 0;
845 }
846 
mt9v111_enum_frame_size(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_size_enum * fse)847 static int mt9v111_enum_frame_size(struct v4l2_subdev *subdev,
848 				   struct v4l2_subdev_state *sd_state,
849 				   struct v4l2_subdev_frame_size_enum *fse)
850 {
851 	if (fse->pad || fse->index >= ARRAY_SIZE(mt9v111_frame_sizes))
852 		return -EINVAL;
853 
854 	fse->min_width = mt9v111_frame_sizes[fse->index].width;
855 	fse->max_width = mt9v111_frame_sizes[fse->index].width;
856 	fse->min_height = mt9v111_frame_sizes[fse->index].height;
857 	fse->max_height = mt9v111_frame_sizes[fse->index].height;
858 
859 	return 0;
860 }
861 
mt9v111_get_format(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * format)862 static int mt9v111_get_format(struct v4l2_subdev *subdev,
863 			      struct v4l2_subdev_state *sd_state,
864 			      struct v4l2_subdev_format *format)
865 {
866 	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
867 
868 	if (format->pad)
869 		return -EINVAL;
870 
871 	mutex_lock(&mt9v111->stream_mutex);
872 	format->format = *__mt9v111_get_pad_format(mt9v111, sd_state,
873 						   format->pad,
874 						   format->which);
875 	mutex_unlock(&mt9v111->stream_mutex);
876 
877 	return 0;
878 }
879 
mt9v111_set_format(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * format)880 static int mt9v111_set_format(struct v4l2_subdev *subdev,
881 			      struct v4l2_subdev_state *sd_state,
882 			      struct v4l2_subdev_format *format)
883 {
884 	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
885 	struct v4l2_mbus_framefmt new_fmt;
886 	struct v4l2_mbus_framefmt *__fmt;
887 	unsigned int best_fit = ~0L;
888 	unsigned int idx = 0;
889 	unsigned int i;
890 
891 	mutex_lock(&mt9v111->stream_mutex);
892 	if (mt9v111->streaming) {
893 		mutex_unlock(&mt9v111->stream_mutex);
894 		return -EBUSY;
895 	}
896 
897 	if (format->pad) {
898 		mutex_unlock(&mt9v111->stream_mutex);
899 		return -EINVAL;
900 	}
901 
902 	/* Update mbus format code and sizes. */
903 	for (i = 0; i < ARRAY_SIZE(mt9v111_formats); i++) {
904 		if (format->format.code == mt9v111_formats[i].code) {
905 			new_fmt.code = mt9v111_formats[i].code;
906 			break;
907 		}
908 	}
909 	if (i == ARRAY_SIZE(mt9v111_formats))
910 		new_fmt.code = mt9v111_formats[0].code;
911 
912 	for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) {
913 		unsigned int fit = abs(mt9v111_frame_sizes[i].width -
914 				       format->format.width) +
915 				   abs(mt9v111_frame_sizes[i].height -
916 				       format->format.height);
917 		if (fit < best_fit) {
918 			best_fit = fit;
919 			idx = i;
920 
921 			if (fit == 0)
922 				break;
923 		}
924 	}
925 	new_fmt.width = mt9v111_frame_sizes[idx].width;
926 	new_fmt.height = mt9v111_frame_sizes[idx].height;
927 
928 	/* Update the device (or pad) format if it has changed. */
929 	__fmt = __mt9v111_get_pad_format(mt9v111, sd_state, format->pad,
930 					 format->which);
931 
932 	/* Format hasn't changed, stop here. */
933 	if (__fmt->code == new_fmt.code &&
934 	    __fmt->width == new_fmt.width &&
935 	    __fmt->height == new_fmt.height)
936 		goto done;
937 
938 	/* Update the format and sizes, then  mark changes as pending. */
939 	__fmt->code = new_fmt.code;
940 	__fmt->width = new_fmt.width;
941 	__fmt->height = new_fmt.height;
942 
943 	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
944 		mt9v111->pending = true;
945 
946 	dev_dbg(mt9v111->dev, "%s: mbus_code: %x - (%ux%u)\n",
947 		__func__, __fmt->code, __fmt->width, __fmt->height);
948 
949 done:
950 	format->format = *__fmt;
951 
952 	mutex_unlock(&mt9v111->stream_mutex);
953 
954 	return 0;
955 }
956 
mt9v111_init_cfg(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state)957 static int mt9v111_init_cfg(struct v4l2_subdev *subdev,
958 			    struct v4l2_subdev_state *sd_state)
959 {
960 	sd_state->pads->try_fmt = mt9v111_def_fmt;
961 
962 	return 0;
963 }
964 
965 static const struct v4l2_subdev_core_ops mt9v111_core_ops = {
966 	.s_power		= mt9v111_s_power,
967 };
968 
969 static const struct v4l2_subdev_video_ops mt9v111_video_ops = {
970 	.s_stream		= mt9v111_s_stream,
971 	.s_frame_interval	= mt9v111_s_frame_interval,
972 	.g_frame_interval	= mt9v111_g_frame_interval,
973 };
974 
975 static const struct v4l2_subdev_pad_ops mt9v111_pad_ops = {
976 	.init_cfg		= mt9v111_init_cfg,
977 	.enum_mbus_code		= mt9v111_enum_mbus_code,
978 	.enum_frame_size	= mt9v111_enum_frame_size,
979 	.enum_frame_interval	= mt9v111_enum_frame_interval,
980 	.get_fmt		= mt9v111_get_format,
981 	.set_fmt		= mt9v111_set_format,
982 };
983 
984 static const struct v4l2_subdev_ops mt9v111_ops = {
985 	.core	= &mt9v111_core_ops,
986 	.video	= &mt9v111_video_ops,
987 	.pad	= &mt9v111_pad_ops,
988 };
989 
990 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
991 static const struct media_entity_operations mt9v111_subdev_entity_ops = {
992 	.link_validate = v4l2_subdev_link_validate,
993 };
994 #endif
995 
996 /* --- V4L2 ctrl --- */
mt9v111_s_ctrl(struct v4l2_ctrl * ctrl)997 static int mt9v111_s_ctrl(struct v4l2_ctrl *ctrl)
998 {
999 	struct mt9v111_dev *mt9v111 = container_of(ctrl->handler,
1000 						   struct mt9v111_dev,
1001 						   ctrls);
1002 	int ret;
1003 
1004 	mutex_lock(&mt9v111->pwr_mutex);
1005 	/*
1006 	 * If sensor is powered down, just cache new control values,
1007 	 * no actual register access.
1008 	 */
1009 	if (!mt9v111->pwr_count) {
1010 		mt9v111->pending = true;
1011 		mutex_unlock(&mt9v111->pwr_mutex);
1012 		return 0;
1013 	}
1014 	mutex_unlock(&mt9v111->pwr_mutex);
1015 
1016 	/*
1017 	 * Flickering control gets disabled if both auto exp and auto awb
1018 	 * are disabled too. If any of the two is enabled, enable it.
1019 	 *
1020 	 * Disabling flickering when ae and awb are off allows a more precise
1021 	 * control of the programmed frame rate.
1022 	 */
1023 	if (mt9v111->auto_exp->is_new || mt9v111->auto_awb->is_new) {
1024 		if (mt9v111->auto_exp->val == V4L2_EXPOSURE_MANUAL &&
1025 		    mt9v111->auto_awb->val == V4L2_WHITE_BALANCE_MANUAL)
1026 			ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1027 					     MT9V111_IFP_R08_OUTFMT_CTRL,
1028 					     MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
1029 					     0);
1030 		else
1031 			ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1032 					     MT9V111_IFP_R08_OUTFMT_CTRL,
1033 					     MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
1034 					     1);
1035 		if (ret)
1036 			return ret;
1037 	}
1038 
1039 	ret = -EINVAL;
1040 	switch (ctrl->id) {
1041 	case V4L2_CID_AUTO_WHITE_BALANCE:
1042 		ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1043 				     MT9V111_IFP_R06_OPMODE_CTRL,
1044 				     MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN,
1045 				     ctrl->val == V4L2_WHITE_BALANCE_AUTO ?
1046 				     MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN : 0);
1047 		break;
1048 	case V4L2_CID_EXPOSURE_AUTO:
1049 		ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1050 				     MT9V111_IFP_R06_OPMODE_CTRL,
1051 				     MT9V111_IFP_R06_OPMODE_CTRL_AE_EN,
1052 				     ctrl->val == V4L2_EXPOSURE_AUTO ?
1053 				     MT9V111_IFP_R06_OPMODE_CTRL_AE_EN : 0);
1054 		break;
1055 	case V4L2_CID_HBLANK:
1056 		ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
1057 				     MT9V111_CORE_R05_HBLANK,
1058 				     MT9V111_CORE_R05_MAX_HBLANK,
1059 				     mt9v111->hblank->val);
1060 		break;
1061 	case V4L2_CID_VBLANK:
1062 		ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
1063 				     MT9V111_CORE_R06_VBLANK,
1064 				     MT9V111_CORE_R06_MAX_VBLANK,
1065 				     mt9v111->vblank->val);
1066 		break;
1067 	}
1068 
1069 	return ret;
1070 }
1071 
1072 static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = {
1073 	.s_ctrl = mt9v111_s_ctrl,
1074 };
1075 
mt9v111_chip_probe(struct mt9v111_dev * mt9v111)1076 static int mt9v111_chip_probe(struct mt9v111_dev *mt9v111)
1077 {
1078 	int ret;
1079 	u16 val;
1080 
1081 	ret = __mt9v111_power_on(&mt9v111->sd);
1082 	if (ret)
1083 		return ret;
1084 
1085 	ret = mt9v111_read(mt9v111->client, MT9V111_R01_CORE,
1086 			   MT9V111_CORE_RFF_CHIP_VER, &val);
1087 	if (ret)
1088 		goto power_off;
1089 
1090 	if ((val >> 8) != MT9V111_CHIP_ID_HIGH &&
1091 	    (val & 0xff) != MT9V111_CHIP_ID_LOW) {
1092 		dev_err(mt9v111->dev,
1093 			"Unable to identify MT9V111 chip: 0x%2x%2x\n",
1094 			val >> 8, val & 0xff);
1095 		ret = -EIO;
1096 		goto power_off;
1097 	}
1098 
1099 	dev_dbg(mt9v111->dev, "Chip identified: 0x%2x%2x\n",
1100 		val >> 8, val & 0xff);
1101 
1102 power_off:
1103 	__mt9v111_power_off(&mt9v111->sd);
1104 
1105 	return ret;
1106 }
1107 
mt9v111_probe(struct i2c_client * client)1108 static int mt9v111_probe(struct i2c_client *client)
1109 {
1110 	struct mt9v111_dev *mt9v111;
1111 	struct v4l2_fract tpf;
1112 	int ret;
1113 
1114 	mt9v111 = devm_kzalloc(&client->dev, sizeof(*mt9v111), GFP_KERNEL);
1115 	if (!mt9v111)
1116 		return -ENOMEM;
1117 
1118 	mt9v111->dev = &client->dev;
1119 	mt9v111->client = client;
1120 
1121 	mt9v111->clk = devm_clk_get(&client->dev, NULL);
1122 	if (IS_ERR(mt9v111->clk))
1123 		return PTR_ERR(mt9v111->clk);
1124 
1125 	mt9v111->sysclk = clk_get_rate(mt9v111->clk);
1126 	if (mt9v111->sysclk > MT9V111_MAX_CLKIN)
1127 		return -EINVAL;
1128 
1129 	mt9v111->oe = devm_gpiod_get_optional(&client->dev, "enable",
1130 					      GPIOD_OUT_LOW);
1131 	if (IS_ERR(mt9v111->oe)) {
1132 		dev_err(&client->dev, "Unable to get GPIO \"enable\": %ld\n",
1133 			PTR_ERR(mt9v111->oe));
1134 		return PTR_ERR(mt9v111->oe);
1135 	}
1136 
1137 	mt9v111->standby = devm_gpiod_get_optional(&client->dev, "standby",
1138 						   GPIOD_OUT_HIGH);
1139 	if (IS_ERR(mt9v111->standby)) {
1140 		dev_err(&client->dev, "Unable to get GPIO \"standby\": %ld\n",
1141 			PTR_ERR(mt9v111->standby));
1142 		return PTR_ERR(mt9v111->standby);
1143 	}
1144 
1145 	mt9v111->reset = devm_gpiod_get_optional(&client->dev, "reset",
1146 						 GPIOD_OUT_LOW);
1147 	if (IS_ERR(mt9v111->reset)) {
1148 		dev_err(&client->dev, "Unable to get GPIO \"reset\": %ld\n",
1149 			PTR_ERR(mt9v111->reset));
1150 		return PTR_ERR(mt9v111->reset);
1151 	}
1152 
1153 	mutex_init(&mt9v111->pwr_mutex);
1154 	mutex_init(&mt9v111->stream_mutex);
1155 
1156 	v4l2_ctrl_handler_init(&mt9v111->ctrls, 5);
1157 
1158 	mt9v111->auto_awb = v4l2_ctrl_new_std(&mt9v111->ctrls,
1159 					      &mt9v111_ctrl_ops,
1160 					      V4L2_CID_AUTO_WHITE_BALANCE,
1161 					      0, 1, 1,
1162 					      V4L2_WHITE_BALANCE_AUTO);
1163 	mt9v111->auto_exp = v4l2_ctrl_new_std_menu(&mt9v111->ctrls,
1164 						   &mt9v111_ctrl_ops,
1165 						   V4L2_CID_EXPOSURE_AUTO,
1166 						   V4L2_EXPOSURE_MANUAL,
1167 						   0, V4L2_EXPOSURE_AUTO);
1168 	mt9v111->hblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1169 					    V4L2_CID_HBLANK,
1170 					    MT9V111_CORE_R05_MIN_HBLANK,
1171 					    MT9V111_CORE_R05_MAX_HBLANK, 1,
1172 					    MT9V111_CORE_R05_DEF_HBLANK);
1173 	mt9v111->vblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1174 					    V4L2_CID_VBLANK,
1175 					    MT9V111_CORE_R06_MIN_VBLANK,
1176 					    MT9V111_CORE_R06_MAX_VBLANK, 1,
1177 					    MT9V111_CORE_R06_DEF_VBLANK);
1178 
1179 	/* PIXEL_RATE is fixed: just expose it to user space. */
1180 	v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1181 			  V4L2_CID_PIXEL_RATE, 0,
1182 			  DIV_ROUND_CLOSEST(mt9v111->sysclk, 2), 1,
1183 			  DIV_ROUND_CLOSEST(mt9v111->sysclk, 2));
1184 
1185 	if (mt9v111->ctrls.error) {
1186 		ret = mt9v111->ctrls.error;
1187 		goto error_free_ctrls;
1188 	}
1189 	mt9v111->sd.ctrl_handler = &mt9v111->ctrls;
1190 
1191 	/* Start with default configuration: 640x480 UYVY. */
1192 	mt9v111->fmt	= mt9v111_def_fmt;
1193 
1194 	/* Re-calculate blankings for 640x480@15fps. */
1195 	mt9v111->fps		= 15;
1196 	tpf.numerator		= 1;
1197 	tpf.denominator		= mt9v111->fps;
1198 	mt9v111_calc_frame_rate(mt9v111, &tpf);
1199 
1200 	mt9v111->pwr_count	= 0;
1201 	mt9v111->addr_space	= MT9V111_R01_IFP;
1202 	mt9v111->pending	= true;
1203 
1204 	v4l2_i2c_subdev_init(&mt9v111->sd, client, &mt9v111_ops);
1205 
1206 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1207 	mt9v111->sd.flags	|= V4L2_SUBDEV_FL_HAS_DEVNODE;
1208 	mt9v111->sd.entity.ops	= &mt9v111_subdev_entity_ops;
1209 	mt9v111->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1210 
1211 	mt9v111->pad.flags	= MEDIA_PAD_FL_SOURCE;
1212 	ret = media_entity_pads_init(&mt9v111->sd.entity, 1, &mt9v111->pad);
1213 	if (ret)
1214 		goto error_free_entity;
1215 #endif
1216 
1217 	ret = mt9v111_chip_probe(mt9v111);
1218 	if (ret)
1219 		goto error_free_entity;
1220 
1221 	ret = v4l2_async_register_subdev(&mt9v111->sd);
1222 	if (ret)
1223 		goto error_free_entity;
1224 
1225 	return 0;
1226 
1227 error_free_entity:
1228 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1229 	media_entity_cleanup(&mt9v111->sd.entity);
1230 #endif
1231 
1232 error_free_ctrls:
1233 	v4l2_ctrl_handler_free(&mt9v111->ctrls);
1234 
1235 	mutex_destroy(&mt9v111->pwr_mutex);
1236 	mutex_destroy(&mt9v111->stream_mutex);
1237 
1238 	return ret;
1239 }
1240 
mt9v111_remove(struct i2c_client * client)1241 static void mt9v111_remove(struct i2c_client *client)
1242 {
1243 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1244 	struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
1245 
1246 	v4l2_async_unregister_subdev(sd);
1247 
1248 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1249 	media_entity_cleanup(&sd->entity);
1250 #endif
1251 
1252 	v4l2_ctrl_handler_free(&mt9v111->ctrls);
1253 
1254 	mutex_destroy(&mt9v111->pwr_mutex);
1255 	mutex_destroy(&mt9v111->stream_mutex);
1256 }
1257 
1258 static const struct of_device_id mt9v111_of_match[] = {
1259 	{ .compatible = "aptina,mt9v111", },
1260 	{ /* sentinel */ },
1261 };
1262 
1263 static struct i2c_driver mt9v111_driver = {
1264 	.driver = {
1265 		.name = "mt9v111",
1266 		.of_match_table = mt9v111_of_match,
1267 	},
1268 	.probe		= mt9v111_probe,
1269 	.remove		= mt9v111_remove,
1270 };
1271 
1272 module_i2c_driver(mt9v111_driver);
1273 
1274 MODULE_DESCRIPTION("V4L2 sensor driver for Aptina MT9V111");
1275 MODULE_AUTHOR("Jacopo Mondi <jacopo@jmondi.org>");
1276 MODULE_LICENSE("GPL v2");
1277