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 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");