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