mt9v111.c

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