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