drivers/media/i2c/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 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         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_s_frame_interval(struct v4l2_subdev *sd,
 723                                     struct v4l2_subdev_frame_interval *ival)
 724 {
 725         struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
 726         struct v4l2_fract *tpf = &ival->interval;
 727         unsigned int fps = tpf->numerator ?
 728                            tpf->denominator / tpf->numerator :
 729                            tpf->denominator;
 730         unsigned int max_fps;
 731
 732         if (!tpf->numerator)
 733                 tpf->numerator = 1;
 734
 735         mutex_lock(&mt9v111->stream_mutex);
 736
 737         if (mt9v111->streaming) {
 738                 mutex_unlock(&mt9v111->stream_mutex);
 739                 return -EBUSY;
 740         }
 741
 742         if (mt9v111->fps == fps) {
 743                 mutex_unlock(&mt9v111->stream_mutex);
 744                 return 0;
 745         }
 746
 747         /* Make sure frame rate/image sizes constraints are respected. */
 748         if (mt9v111->fmt.width < QVGA_WIDTH &&
 749             mt9v111->fmt.height < QVGA_HEIGHT)
 750                 max_fps = 90;
 751         else if (mt9v111->fmt.width < CIF_WIDTH &&
 752                  mt9v111->fmt.height < CIF_HEIGHT)
 753                 max_fps = 60;
 754         else
 755                 max_fps = mt9v111->sysclk <
 756                                 DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ? 15 :
 757                                                                           30;
 758
 759         if (fps > max_fps) {
 760                 mutex_unlock(&mt9v111->stream_mutex);
 761                 return -EINVAL;
 762         }
 763
 764         mt9v111_calc_frame_rate(mt9v111, tpf);
 765
 766         mt9v111->fps = fps;
 767         mt9v111->pending = true;
 768
 769         mutex_unlock(&mt9v111->stream_mutex);
 770
 771         return 0;
 772 }
 773
 774 static int mt9v111_g_frame_interval(struct v4l2_subdev *sd,
 775                                     struct v4l2_subdev_frame_interval *ival)
 776 {
 777         struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
 778         struct v4l2_fract *tpf = &ival->interval;
 779
 780         mutex_lock(&mt9v111->stream_mutex);
 781
 782         tpf->numerator = 1;
 783         tpf->denominator = mt9v111->fps;
 784
 785         mutex_unlock(&mt9v111->stream_mutex);
 786
 787         return 0;
 788 }
 789
 790 static struct v4l2_mbus_framefmt *__mt9v111_get_pad_format(
 791                                         struct mt9v111_dev *mt9v111,
 792                                         struct v4l2_subdev_state *sd_state,
 793                                         unsigned int pad,
 794                                         enum v4l2_subdev_format_whence which)
 795 {
 796         switch (which) {
 797         case V4L2_SUBDEV_FORMAT_TRY:
 798                 return v4l2_subdev_get_try_format(&mt9v111->sd, sd_state, pad);
 799         case V4L2_SUBDEV_FORMAT_ACTIVE:
 800                 return &mt9v111->fmt;
 801         default:
 802                 return NULL;
 803         }
 804 }
 805
 806 static int mt9v111_enum_mbus_code(struct v4l2_subdev *subdev,
 807                                   struct v4l2_subdev_state *sd_state,
 808                                   struct v4l2_subdev_mbus_code_enum *code)
 809 {
 810         if (code->pad || code->index > ARRAY_SIZE(mt9v111_formats) - 1)
 811                 return -EINVAL;
 812
 813         code->code = mt9v111_formats[code->index].code;
 814
 815         return 0;
 816 }
 817
 818 static int mt9v111_enum_frame_interval(struct v4l2_subdev *sd,
 819                                 struct v4l2_subdev_state *sd_state,
 820                                 struct v4l2_subdev_frame_interval_enum *fie)
 821 {
 822         unsigned int i;
 823
 824         if (fie->pad || fie->index >= ARRAY_SIZE(mt9v111_frame_intervals))
 825                 return -EINVAL;
 826
 827         for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++)
 828                 if (fie->width == mt9v111_frame_sizes[i].width &&
 829                     fie->height == mt9v111_frame_sizes[i].height)
 830                         break;
 831
 832         if (i == ARRAY_SIZE(mt9v111_frame_sizes))
 833                 return -EINVAL;
 834
 835         fie->interval.numerator = 1;
 836         fie->interval.denominator = mt9v111_frame_intervals[fie->index];
 837
 838         return 0;
 839 }
 840
 841 static int mt9v111_enum_frame_size(struct v4l2_subdev *subdev,
 842                                    struct v4l2_subdev_state *sd_state,
 843                                    struct v4l2_subdev_frame_size_enum *fse)
 844 {
 845         if (fse->pad || fse->index >= ARRAY_SIZE(mt9v111_frame_sizes))
 846                 return -EINVAL;
 847
 848         fse->min_width = mt9v111_frame_sizes[fse->index].width;
 849         fse->max_width = mt9v111_frame_sizes[fse->index].width;
 850         fse->min_height = mt9v111_frame_sizes[fse->index].height;
 851         fse->max_height = mt9v111_frame_sizes[fse->index].height;
 852
 853         return 0;
 854 }
 855
 856 static int mt9v111_get_format(struct v4l2_subdev *subdev,
 857                               struct v4l2_subdev_state *sd_state,
 858                               struct v4l2_subdev_format *format)
 859 {
 860         struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
 861
 862         if (format->pad)
 863                 return -EINVAL;
 864
 865         mutex_lock(&mt9v111->stream_mutex);
 866         format->format = *__mt9v111_get_pad_format(mt9v111, sd_state,
 867                                                    format->pad,
 868                                                    format->which);
 869         mutex_unlock(&mt9v111->stream_mutex);
 870
 871         return 0;
 872 }
 873
 874 static int mt9v111_set_format(struct v4l2_subdev *subdev,
 875                               struct v4l2_subdev_state *sd_state,
 876                               struct v4l2_subdev_format *format)
 877 {
 878         struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
 879         struct v4l2_mbus_framefmt new_fmt;
 880         struct v4l2_mbus_framefmt *__fmt;
 881         unsigned int best_fit = ~0L;
 882         unsigned int idx = 0;
 883         unsigned int i;
 884
 885         mutex_lock(&mt9v111->stream_mutex);
 886         if (mt9v111->streaming) {
 887                 mutex_unlock(&mt9v111->stream_mutex);
 888                 return -EBUSY;
 889         }
 890
 891         if (format->pad) {
 892                 mutex_unlock(&mt9v111->stream_mutex);
 893                 return -EINVAL;
 894         }
 895
 896         /* Update mbus format code and sizes. */
 897         for (i = 0; i < ARRAY_SIZE(mt9v111_formats); i++) {
 898                 if (format->format.code == mt9v111_formats[i].code) {
 899                         new_fmt.code = mt9v111_formats[i].code;
 900                         break;
 901                 }
 902         }
 903         if (i == ARRAY_SIZE(mt9v111_formats))
 904                 new_fmt.code = mt9v111_formats[0].code;
 905
 906         for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) {
 907                 unsigned int fit = abs(mt9v111_frame_sizes[i].width -
 908                                        format->format.width) +
 909                                    abs(mt9v111_frame_sizes[i].height -
 910                                        format->format.height);
 911                 if (fit < best_fit) {
 912                         best_fit = fit;
 913                         idx = i;
 914
 915                         if (fit == 0)
 916                                 break;
 917                 }
 918         }
 919         new_fmt.width = mt9v111_frame_sizes[idx].width;
 920         new_fmt.height = mt9v111_frame_sizes[idx].height;
 921
 922         /* Update the device (or pad) format if it has changed. */
 923         __fmt = __mt9v111_get_pad_format(mt9v111, sd_state, format->pad,
 924                                          format->which);
 925
 926         /* Format hasn't changed, stop here. */
 927         if (__fmt->code == new_fmt.code &&
 928             __fmt->width == new_fmt.width &&
 929             __fmt->height == new_fmt.height)
 930                 goto done;
 931
 932         /* Update the format and sizes, then  mark changes as pending. */
 933         __fmt->code = new_fmt.code;
 934         __fmt->width = new_fmt.width;
 935         __fmt->height = new_fmt.height;
 936
 937         if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 938                 mt9v111->pending = true;
 939
 940         dev_dbg(mt9v111->dev, "%s: mbus_code: %x - (%ux%u)\n",
 941                 __func__, __fmt->code, __fmt->width, __fmt->height);
 942
 943 done:
 944         format->format = *__fmt;
 945
 946         mutex_unlock(&mt9v111->stream_mutex);
 947
 948         return 0;
 949 }
 950
 951 static int mt9v111_init_cfg(struct v4l2_subdev *subdev,
 952                             struct v4l2_subdev_state *sd_state)
 953 {
 954         sd_state->pads->try_fmt = mt9v111_def_fmt;
 955
 956         return 0;
 957 }
 958
 959 static const struct v4l2_subdev_core_ops mt9v111_core_ops = {
 960         .s_power                = mt9v111_s_power,
 961 };
 962
 963 static const struct v4l2_subdev_video_ops mt9v111_video_ops = {
 964         .s_stream               = mt9v111_s_stream,
 965         .s_frame_interval       = mt9v111_s_frame_interval,
 966         .g_frame_interval       = mt9v111_g_frame_interval,
 967 };
 968
 969 static const struct v4l2_subdev_pad_ops mt9v111_pad_ops = {
 970         .init_cfg               = mt9v111_init_cfg,
 971         .enum_mbus_code         = mt9v111_enum_mbus_code,
 972         .enum_frame_size        = mt9v111_enum_frame_size,
 973         .enum_frame_interval    = mt9v111_enum_frame_interval,
 974         .get_fmt                = mt9v111_get_format,
 975         .set_fmt                = mt9v111_set_format,
 976 };
 977
 978 static const struct v4l2_subdev_ops mt9v111_ops = {
 979         .core   = &mt9v111_core_ops,
 980         .video  = &mt9v111_video_ops,
 981         .pad    = &mt9v111_pad_ops,
 982 };
 983
 984 static const struct media_entity_operations mt9v111_subdev_entity_ops = {
 985         .link_validate = v4l2_subdev_link_validate,
 986 };
 987
 988 /* --- V4L2 ctrl --- */
 989 static int mt9v111_s_ctrl(struct v4l2_ctrl *ctrl)
 990 {
 991         struct mt9v111_dev *mt9v111 = container_of(ctrl->handler,
 992                                                    struct mt9v111_dev,
 993                                                    ctrls);
 994         int ret;
 995
 996         mutex_lock(&mt9v111->pwr_mutex);
 997         /*
 998          * If sensor is powered down, just cache new control values,
 999          * no actual register access.
1000          */
1001         if (!mt9v111->pwr_count) {
1002                 mt9v111->pending = true;
1003                 mutex_unlock(&mt9v111->pwr_mutex);
1004                 return 0;
1005         }
1006         mutex_unlock(&mt9v111->pwr_mutex);
1007
1008         /*
1009          * Flickering control gets disabled if both auto exp and auto awb
1010          * are disabled too. If any of the two is enabled, enable it.
1011          *
1012          * Disabling flickering when ae and awb are off allows a more precise
1013          * control of the programmed frame rate.
1014          */
1015         if (mt9v111->auto_exp->is_new || mt9v111->auto_awb->is_new) {
1016                 if (mt9v111->auto_exp->val == V4L2_EXPOSURE_MANUAL &&
1017                     mt9v111->auto_awb->val == V4L2_WHITE_BALANCE_MANUAL)
1018                         ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1019                                              MT9V111_IFP_R08_OUTFMT_CTRL,
1020                                              MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
1021                                              0);
1022                 else
1023                         ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1024                                              MT9V111_IFP_R08_OUTFMT_CTRL,
1025                                              MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
1026                                              1);
1027                 if (ret)
1028                         return ret;
1029         }
1030
1031         ret = -EINVAL;
1032         switch (ctrl->id) {
1033         case V4L2_CID_AUTO_WHITE_BALANCE:
1034                 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1035                                      MT9V111_IFP_R06_OPMODE_CTRL,
1036                                      MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN,
1037                                      ctrl->val == V4L2_WHITE_BALANCE_AUTO ?
1038                                      MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN : 0);
1039                 break;
1040         case V4L2_CID_EXPOSURE_AUTO:
1041                 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1042                                      MT9V111_IFP_R06_OPMODE_CTRL,
1043                                      MT9V111_IFP_R06_OPMODE_CTRL_AE_EN,
1044                                      ctrl->val == V4L2_EXPOSURE_AUTO ?
1045                                      MT9V111_IFP_R06_OPMODE_CTRL_AE_EN : 0);
1046                 break;
1047         case V4L2_CID_HBLANK:
1048                 ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
1049                                      MT9V111_CORE_R05_HBLANK,
1050                                      MT9V111_CORE_R05_MAX_HBLANK,
1051                                      mt9v111->hblank->val);
1052                 break;
1053         case V4L2_CID_VBLANK:
1054                 ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
1055                                      MT9V111_CORE_R06_VBLANK,
1056                                      MT9V111_CORE_R06_MAX_VBLANK,
1057                                      mt9v111->vblank->val);
1058                 break;
1059         }
1060
1061         return ret;
1062 }
1063
1064 static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = {
1065         .s_ctrl = mt9v111_s_ctrl,
1066 };
1067
1068 static int mt9v111_chip_probe(struct mt9v111_dev *mt9v111)
1069 {
1070         int ret;
1071         u16 val;
1072
1073         ret = __mt9v111_power_on(&mt9v111->sd);
1074         if (ret)
1075                 return ret;
1076
1077         ret = mt9v111_read(mt9v111->client, MT9V111_R01_CORE,
1078                            MT9V111_CORE_RFF_CHIP_VER, &val);
1079         if (ret)
1080                 goto power_off;
1081
1082         if ((val >> 8) != MT9V111_CHIP_ID_HIGH &&
1083             (val & 0xff) != MT9V111_CHIP_ID_LOW) {
1084                 dev_err(mt9v111->dev,
1085                         "Unable to identify MT9V111 chip: 0x%2x%2x\n",
1086                         val >> 8, val & 0xff);
1087                 ret = -EIO;
1088                 goto power_off;
1089         }
1090
1091         dev_dbg(mt9v111->dev, "Chip identified: 0x%2x%2x\n",
1092                 val >> 8, val & 0xff);
1093
1094 power_off:
1095         __mt9v111_power_off(&mt9v111->sd);
1096
1097         return ret;
1098 }
1099
1100 static int mt9v111_probe(struct i2c_client *client)
1101 {
1102         struct mt9v111_dev *mt9v111;
1103         struct v4l2_fract tpf;
1104         int ret;
1105
1106         mt9v111 = devm_kzalloc(&client->dev, sizeof(*mt9v111), GFP_KERNEL);
1107         if (!mt9v111)
1108                 return -ENOMEM;
1109
1110         mt9v111->dev = &client->dev;
1111         mt9v111->client = client;
1112
1113         mt9v111->clk = devm_clk_get(&client->dev, NULL);
1114         if (IS_ERR(mt9v111->clk))
1115                 return PTR_ERR(mt9v111->clk);
1116
1117         mt9v111->sysclk = clk_get_rate(mt9v111->clk);
1118         if (mt9v111->sysclk > MT9V111_MAX_CLKIN)
1119                 return -EINVAL;
1120
1121         mt9v111->oe = devm_gpiod_get_optional(&client->dev, "enable",
1122                                               GPIOD_OUT_LOW);
1123         if (IS_ERR(mt9v111->oe)) {
1124                 dev_err(&client->dev, "Unable to get GPIO \"enable\": %ld\n",
1125                         PTR_ERR(mt9v111->oe));
1126                 return PTR_ERR(mt9v111->oe);
1127         }
1128
1129         mt9v111->standby = devm_gpiod_get_optional(&client->dev, "standby",
1130                                                    GPIOD_OUT_HIGH);
1131         if (IS_ERR(mt9v111->standby)) {
1132                 dev_err(&client->dev, "Unable to get GPIO \"standby\": %ld\n",
1133                         PTR_ERR(mt9v111->standby));
1134                 return PTR_ERR(mt9v111->standby);
1135         }
1136
1137         mt9v111->reset = devm_gpiod_get_optional(&client->dev, "reset",
1138                                                  GPIOD_OUT_LOW);
1139         if (IS_ERR(mt9v111->reset)) {
1140                 dev_err(&client->dev, "Unable to get GPIO \"reset\": %ld\n",
1141                         PTR_ERR(mt9v111->reset));
1142                 return PTR_ERR(mt9v111->reset);
1143         }
1144
1145         mutex_init(&mt9v111->pwr_mutex);
1146         mutex_init(&mt9v111->stream_mutex);
1147
1148         v4l2_ctrl_handler_init(&mt9v111->ctrls, 5);
1149
1150         mt9v111->auto_awb = v4l2_ctrl_new_std(&mt9v111->ctrls,
1151                                               &mt9v111_ctrl_ops,
1152                                               V4L2_CID_AUTO_WHITE_BALANCE,
1153                                               0, 1, 1,
1154                                               V4L2_WHITE_BALANCE_AUTO);
1155         mt9v111->auto_exp = v4l2_ctrl_new_std_menu(&mt9v111->ctrls,
1156                                                    &mt9v111_ctrl_ops,
1157                                                    V4L2_CID_EXPOSURE_AUTO,
1158                                                    V4L2_EXPOSURE_MANUAL,
1159                                                    0, V4L2_EXPOSURE_AUTO);
1160         mt9v111->hblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1161                                             V4L2_CID_HBLANK,
1162                                             MT9V111_CORE_R05_MIN_HBLANK,
1163                                             MT9V111_CORE_R05_MAX_HBLANK, 1,
1164                                             MT9V111_CORE_R05_DEF_HBLANK);
1165         mt9v111->vblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1166                                             V4L2_CID_VBLANK,
1167                                             MT9V111_CORE_R06_MIN_VBLANK,
1168                                             MT9V111_CORE_R06_MAX_VBLANK, 1,
1169                                             MT9V111_CORE_R06_DEF_VBLANK);
1170
1171         /* PIXEL_RATE is fixed: just expose it to user space. */
1172         v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1173                           V4L2_CID_PIXEL_RATE, 0,
1174                           DIV_ROUND_CLOSEST(mt9v111->sysclk, 2), 1,
1175                           DIV_ROUND_CLOSEST(mt9v111->sysclk, 2));
1176
1177         if (mt9v111->ctrls.error) {
1178                 ret = mt9v111->ctrls.error;
1179                 goto error_free_ctrls;
1180         }
1181         mt9v111->sd.ctrl_handler = &mt9v111->ctrls;
1182
1183         /* Start with default configuration: 640x480 UYVY. */
1184         mt9v111->fmt    = mt9v111_def_fmt;
1185
1186         /* Re-calculate blankings for 640x480@15fps. */
1187         mt9v111->fps            = 15;
1188         tpf.numerator           = 1;
1189         tpf.denominator         = mt9v111->fps;
1190         mt9v111_calc_frame_rate(mt9v111, &tpf);
1191
1192         mt9v111->pwr_count      = 0;
1193         mt9v111->addr_space     = MT9V111_R01_IFP;
1194         mt9v111->pending        = true;
1195
1196         v4l2_i2c_subdev_init(&mt9v111->sd, client, &mt9v111_ops);
1197
1198         mt9v111->sd.flags       |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1199         mt9v111->sd.entity.ops  = &mt9v111_subdev_entity_ops;
1200         mt9v111->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1201
1202         mt9v111->pad.flags      = MEDIA_PAD_FL_SOURCE;
1203         ret = media_entity_pads_init(&mt9v111->sd.entity, 1, &mt9v111->pad);
1204         if (ret)
1205                 goto error_free_entity;
1206
1207         ret = mt9v111_chip_probe(mt9v111);
1208         if (ret)
1209                 goto error_free_entity;
1210
1211         ret = v4l2_async_register_subdev(&mt9v111->sd);
1212         if (ret)
1213                 goto error_free_entity;
1214
1215         return 0;
1216
1217 error_free_entity:
1218         media_entity_cleanup(&mt9v111->sd.entity);
1219
1220 error_free_ctrls:
1221         v4l2_ctrl_handler_free(&mt9v111->ctrls);
1222
1223         mutex_destroy(&mt9v111->pwr_mutex);
1224         mutex_destroy(&mt9v111->stream_mutex);
1225
1226         return ret;
1227 }
1228
1229 static void mt9v111_remove(struct i2c_client *client)
1230 {
1231         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1232         struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
1233
1234         v4l2_async_unregister_subdev(sd);
1235
1236         media_entity_cleanup(&sd->entity);
1237
1238         v4l2_ctrl_handler_free(&mt9v111->ctrls);
1239
1240         mutex_destroy(&mt9v111->pwr_mutex);
1241         mutex_destroy(&mt9v111->stream_mutex);
1242 }
1243
1244 static const struct of_device_id mt9v111_of_match[] = {
1245         { .compatible = "aptina,mt9v111", },
1246         { /* sentinel */ },
1247 };
1248
1249 static struct i2c_driver mt9v111_driver = {
1250         .driver = {
1251                 .name = "mt9v111",
1252                 .of_match_table = mt9v111_of_match,
1253         },
1254         .probe          = mt9v111_probe,
1255         .remove         = mt9v111_remove,
1256 };
1257
1258 module_i2c_driver(mt9v111_driver);
1259
1260 MODULE_DESCRIPTION("V4L2 sensor driver for Aptina MT9V111");
1261 MODULE_AUTHOR("Jacopo Mondi <jacopo@jmondi.org>");
1262 MODULE_LICENSE("GPL v2");