drivers/media/i2c/mt9v111.c

/* [<][>][^][v][top][bottom][index][help] */
This source file includes following definitions.
__mt9v111_read
__mt9v111_write
__mt9v111_addr_space_select
mt9v111_read
mt9v111_write
mt9v111_update
__mt9v111_power_on
__mt9v111_power_off
__mt9v111_hw_reset
__mt9v111_sw_reset
mt9v111_calc_frame_rate
mt9v111_hw_config
mt9v111_s_power
mt9v111_s_stream
mt9v111_s_frame_interval
mt9v111_g_frame_interval
__mt9v111_get_pad_format
mt9v111_enum_mbus_code
mt9v111_enum_frame_interval
mt9v111_enum_frame_size
mt9v111_get_format
mt9v111_set_format
mt9v111_init_cfg
mt9v111_s_ctrl
mt9v111_chip_probe
mt9v111_probe
mt9v111_remove
   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 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_pad_config *cfg,
 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, cfg, pad);
 802 #else
 803                 return &cfg->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_pad_config *cfg,
 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_pad_config *cfg,
 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_pad_config *cfg,
 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_pad_config *cfg,
 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, cfg, format->pad,
 873                                                    format->which);
 874         mutex_unlock(&mt9v111->stream_mutex);
 875 
 876         return 0;
 877 }
 878 
 879 static int mt9v111_set_format(struct v4l2_subdev *subdev,
 880                               struct v4l2_subdev_pad_config *cfg,
 881                               struct v4l2_subdev_format *format)
 882 {
 883         struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
 884         struct v4l2_mbus_framefmt new_fmt;
 885         struct v4l2_mbus_framefmt *__fmt;
 886         unsigned int best_fit = ~0L;
 887         unsigned int idx = 0;
 888         unsigned int i;
 889 
 890         mutex_lock(&mt9v111->stream_mutex);
 891         if (mt9v111->streaming) {
 892                 mutex_unlock(&mt9v111->stream_mutex);
 893                 return -EBUSY;
 894         }
 895 
 896         if (format->pad) {
 897                 mutex_unlock(&mt9v111->stream_mutex);
 898                 return -EINVAL;
 899         }
 900 
 901         /* Update mbus format code and sizes. */
 902         for (i = 0; i < ARRAY_SIZE(mt9v111_formats); i++) {
 903                 if (format->format.code == mt9v111_formats[i].code) {
 904                         new_fmt.code = mt9v111_formats[i].code;
 905                         break;
 906                 }
 907         }
 908         if (i == ARRAY_SIZE(mt9v111_formats))
 909                 new_fmt.code = mt9v111_formats[0].code;
 910 
 911         for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) {
 912                 unsigned int fit = abs(mt9v111_frame_sizes[i].width -
 913                                        format->format.width) +
 914                                    abs(mt9v111_frame_sizes[i].height -
 915                                        format->format.height);
 916                 if (fit < best_fit) {
 917                         best_fit = fit;
 918                         idx = i;
 919 
 920                         if (fit == 0)
 921                                 break;
 922                 }
 923         }
 924         new_fmt.width = mt9v111_frame_sizes[idx].width;
 925         new_fmt.height = mt9v111_frame_sizes[idx].height;
 926 
 927         /* Update the device (or pad) format if it has changed. */
 928         __fmt = __mt9v111_get_pad_format(mt9v111, cfg, format->pad,
 929                                          format->which);
 930 
 931         /* Format hasn't changed, stop here. */
 932         if (__fmt->code == new_fmt.code &&
 933             __fmt->width == new_fmt.width &&
 934             __fmt->height == new_fmt.height)
 935                 goto done;
 936 
 937         /* Update the format and sizes, then  mark changes as pending. */
 938         __fmt->code = new_fmt.code;
 939         __fmt->width = new_fmt.width;
 940         __fmt->height = new_fmt.height;
 941 
 942         if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 943                 mt9v111->pending = true;
 944 
 945         dev_dbg(mt9v111->dev, "%s: mbus_code: %x - (%ux%u)\n",
 946                 __func__, __fmt->code, __fmt->width, __fmt->height);
 947 
 948 done:
 949         format->format = *__fmt;
 950 
 951         mutex_unlock(&mt9v111->stream_mutex);
 952 
 953         return 0;
 954 }
 955 
 956 static int mt9v111_init_cfg(struct v4l2_subdev *subdev,
 957                             struct v4l2_subdev_pad_config *cfg)
 958 {
 959         cfg->try_fmt = mt9v111_def_fmt;
 960 
 961         return 0;
 962 }
 963 
 964 static const struct v4l2_subdev_core_ops mt9v111_core_ops = {
 965         .s_power                = mt9v111_s_power,
 966 };
 967 
 968 static const struct v4l2_subdev_video_ops mt9v111_video_ops = {
 969         .s_stream               = mt9v111_s_stream,
 970         .s_frame_interval       = mt9v111_s_frame_interval,
 971         .g_frame_interval       = mt9v111_g_frame_interval,
 972 };
 973 
 974 static const struct v4l2_subdev_pad_ops mt9v111_pad_ops = {
 975         .init_cfg               = mt9v111_init_cfg,
 976         .enum_mbus_code         = mt9v111_enum_mbus_code,
 977         .enum_frame_size        = mt9v111_enum_frame_size,
 978         .enum_frame_interval    = mt9v111_enum_frame_interval,
 979         .get_fmt                = mt9v111_get_format,
 980         .set_fmt                = mt9v111_set_format,
 981 };
 982 
 983 static const struct v4l2_subdev_ops mt9v111_ops = {
 984         .core   = &mt9v111_core_ops,
 985         .video  = &mt9v111_video_ops,
 986         .pad    = &mt9v111_pad_ops,
 987 };
 988 
 989 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
 990 static const struct media_entity_operations mt9v111_subdev_entity_ops = {
 991         .link_validate = v4l2_subdev_link_validate,
 992 };
 993 #endif
 994 
 995 /* --- V4L2 ctrl --- */
 996 static int mt9v111_s_ctrl(struct v4l2_ctrl *ctrl)
 997 {
 998         struct mt9v111_dev *mt9v111 = container_of(ctrl->handler,
 999                                                    struct mt9v111_dev,
1000                                                    ctrls);
1001         int ret;
1002 
1003         mutex_lock(&mt9v111->pwr_mutex);
1004         /*
1005          * If sensor is powered down, just cache new control values,
1006          * no actual register access.
1007          */
1008         if (!mt9v111->pwr_count) {
1009                 mt9v111->pending = true;
1010                 mutex_unlock(&mt9v111->pwr_mutex);
1011                 return 0;
1012         }
1013         mutex_unlock(&mt9v111->pwr_mutex);
1014 
1015         /*
1016          * Flickering control gets disabled if both auto exp and auto awb
1017          * are disabled too. If any of the two is enabled, enable it.
1018          *
1019          * Disabling flickering when ae and awb are off allows a more precise
1020          * control of the programmed frame rate.
1021          */
1022         if (mt9v111->auto_exp->is_new || mt9v111->auto_awb->is_new) {
1023                 if (mt9v111->auto_exp->val == V4L2_EXPOSURE_MANUAL &&
1024                     mt9v111->auto_awb->val == V4L2_WHITE_BALANCE_MANUAL)
1025                         ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1026                                              MT9V111_IFP_R08_OUTFMT_CTRL,
1027                                              MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
1028                                              0);
1029                 else
1030                         ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1031                                              MT9V111_IFP_R08_OUTFMT_CTRL,
1032                                              MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
1033                                              1);
1034                 if (ret)
1035                         return ret;
1036         }
1037 
1038         ret = -EINVAL;
1039         switch (ctrl->id) {
1040         case V4L2_CID_AUTO_WHITE_BALANCE:
1041                 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1042                                      MT9V111_IFP_R06_OPMODE_CTRL,
1043                                      MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN,
1044                                      ctrl->val == V4L2_WHITE_BALANCE_AUTO ?
1045                                      MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN : 0);
1046                 break;
1047         case V4L2_CID_EXPOSURE_AUTO:
1048                 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1049                                      MT9V111_IFP_R06_OPMODE_CTRL,
1050                                      MT9V111_IFP_R06_OPMODE_CTRL_AE_EN,
1051                                      ctrl->val == V4L2_EXPOSURE_AUTO ?
1052                                      MT9V111_IFP_R06_OPMODE_CTRL_AE_EN : 0);
1053                 break;
1054         case V4L2_CID_HBLANK:
1055                 ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
1056                                      MT9V111_CORE_R05_HBLANK,
1057                                      MT9V111_CORE_R05_MAX_HBLANK,
1058                                      mt9v111->hblank->val);
1059                 break;
1060         case V4L2_CID_VBLANK:
1061                 ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
1062                                      MT9V111_CORE_R06_VBLANK,
1063                                      MT9V111_CORE_R06_MAX_VBLANK,
1064                                      mt9v111->vblank->val);
1065                 break;
1066         }
1067 
1068         return ret;
1069 }
1070 
1071 static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = {
1072         .s_ctrl = mt9v111_s_ctrl,
1073 };
1074 
1075 static int mt9v111_chip_probe(struct mt9v111_dev *mt9v111)
1076 {
1077         int ret;
1078         u16 val;
1079 
1080         ret = __mt9v111_power_on(&mt9v111->sd);
1081         if (ret)
1082                 return ret;
1083 
1084         ret = mt9v111_read(mt9v111->client, MT9V111_R01_CORE,
1085                            MT9V111_CORE_RFF_CHIP_VER, &val);
1086         if (ret)
1087                 goto power_off;
1088 
1089         if ((val >> 8) != MT9V111_CHIP_ID_HIGH &&
1090             (val & 0xff) != MT9V111_CHIP_ID_LOW) {
1091                 dev_err(mt9v111->dev,
1092                         "Unable to identify MT9V111 chip: 0x%2x%2x\n",
1093                         val >> 8, val & 0xff);
1094                 ret = -EIO;
1095                 goto power_off;
1096         }
1097 
1098         dev_dbg(mt9v111->dev, "Chip identified: 0x%2x%2x\n",
1099                 val >> 8, val & 0xff);
1100 
1101 power_off:
1102         __mt9v111_power_off(&mt9v111->sd);
1103 
1104         return ret;
1105 }
1106 
1107 static int mt9v111_probe(struct i2c_client *client)
1108 {
1109         struct mt9v111_dev *mt9v111;
1110         struct v4l2_fract tpf;
1111         int ret;
1112 
1113         mt9v111 = devm_kzalloc(&client->dev, sizeof(*mt9v111), GFP_KERNEL);
1114         if (!mt9v111)
1115                 return -ENOMEM;
1116 
1117         mt9v111->dev = &client->dev;
1118         mt9v111->client = client;
1119 
1120         mt9v111->clk = devm_clk_get(&client->dev, NULL);
1121         if (IS_ERR(mt9v111->clk))
1122                 return PTR_ERR(mt9v111->clk);
1123 
1124         mt9v111->sysclk = clk_get_rate(mt9v111->clk);
1125         if (mt9v111->sysclk > MT9V111_MAX_CLKIN)
1126                 return -EINVAL;
1127 
1128         mt9v111->oe = devm_gpiod_get_optional(&client->dev, "enable",
1129                                               GPIOD_OUT_LOW);
1130         if (IS_ERR(mt9v111->oe)) {
1131                 dev_err(&client->dev, "Unable to get GPIO \"enable\": %ld\n",
1132                         PTR_ERR(mt9v111->oe));
1133                 return PTR_ERR(mt9v111->oe);
1134         }
1135 
1136         mt9v111->standby = devm_gpiod_get_optional(&client->dev, "standby",
1137                                                    GPIOD_OUT_HIGH);
1138         if (IS_ERR(mt9v111->standby)) {
1139                 dev_err(&client->dev, "Unable to get GPIO \"standby\": %ld\n",
1140                         PTR_ERR(mt9v111->standby));
1141                 return PTR_ERR(mt9v111->standby);
1142         }
1143 
1144         mt9v111->reset = devm_gpiod_get_optional(&client->dev, "reset",
1145                                                  GPIOD_OUT_LOW);
1146         if (IS_ERR(mt9v111->reset)) {
1147                 dev_err(&client->dev, "Unable to get GPIO \"reset\": %ld\n",
1148                         PTR_ERR(mt9v111->reset));
1149                 return PTR_ERR(mt9v111->reset);
1150         }
1151 
1152         mutex_init(&mt9v111->pwr_mutex);
1153         mutex_init(&mt9v111->stream_mutex);
1154 
1155         v4l2_ctrl_handler_init(&mt9v111->ctrls, 5);
1156 
1157         mt9v111->auto_awb = v4l2_ctrl_new_std(&mt9v111->ctrls,
1158                                               &mt9v111_ctrl_ops,
1159                                               V4L2_CID_AUTO_WHITE_BALANCE,
1160                                               0, 1, 1,
1161                                               V4L2_WHITE_BALANCE_AUTO);
1162         mt9v111->auto_exp = v4l2_ctrl_new_std_menu(&mt9v111->ctrls,
1163                                                    &mt9v111_ctrl_ops,
1164                                                    V4L2_CID_EXPOSURE_AUTO,
1165                                                    V4L2_EXPOSURE_MANUAL,
1166                                                    0, V4L2_EXPOSURE_AUTO);
1167         mt9v111->hblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1168                                             V4L2_CID_HBLANK,
1169                                             MT9V111_CORE_R05_MIN_HBLANK,
1170                                             MT9V111_CORE_R05_MAX_HBLANK, 1,
1171                                             MT9V111_CORE_R05_DEF_HBLANK);
1172         mt9v111->vblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1173                                             V4L2_CID_VBLANK,
1174                                             MT9V111_CORE_R06_MIN_VBLANK,
1175                                             MT9V111_CORE_R06_MAX_VBLANK, 1,
1176                                             MT9V111_CORE_R06_DEF_VBLANK);
1177 
1178         /* PIXEL_RATE is fixed: just expose it to user space. */
1179         v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1180                           V4L2_CID_PIXEL_RATE, 0,
1181                           DIV_ROUND_CLOSEST(mt9v111->sysclk, 2), 1,
1182                           DIV_ROUND_CLOSEST(mt9v111->sysclk, 2));
1183 
1184         if (mt9v111->ctrls.error) {
1185                 ret = mt9v111->ctrls.error;
1186                 goto error_free_ctrls;
1187         }
1188         mt9v111->sd.ctrl_handler = &mt9v111->ctrls;
1189 
1190         /* Start with default configuration: 640x480 UYVY. */
1191         mt9v111->fmt    = mt9v111_def_fmt;
1192 
1193         /* Re-calculate blankings for 640x480@15fps. */
1194         mt9v111->fps            = 15;
1195         tpf.numerator           = 1;
1196         tpf.denominator         = mt9v111->fps;
1197         mt9v111_calc_frame_rate(mt9v111, &tpf);
1198 
1199         mt9v111->pwr_count      = 0;
1200         mt9v111->addr_space     = MT9V111_R01_IFP;
1201         mt9v111->pending        = true;
1202 
1203         v4l2_i2c_subdev_init(&mt9v111->sd, client, &mt9v111_ops);
1204 
1205 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1206         mt9v111->sd.flags       |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1207         mt9v111->sd.entity.ops  = &mt9v111_subdev_entity_ops;
1208         mt9v111->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1209 
1210         mt9v111->pad.flags      = MEDIA_PAD_FL_SOURCE;
1211         ret = media_entity_pads_init(&mt9v111->sd.entity, 1, &mt9v111->pad);
1212         if (ret)
1213                 goto error_free_entity;
1214 #endif
1215 
1216         ret = mt9v111_chip_probe(mt9v111);
1217         if (ret)
1218                 goto error_free_entity;
1219 
1220         ret = v4l2_async_register_subdev(&mt9v111->sd);
1221         if (ret)
1222                 goto error_free_entity;
1223 
1224         return 0;
1225 
1226 error_free_entity:
1227 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1228         media_entity_cleanup(&mt9v111->sd.entity);
1229 #endif
1230 
1231 error_free_ctrls:
1232         v4l2_ctrl_handler_free(&mt9v111->ctrls);
1233 
1234         mutex_destroy(&mt9v111->pwr_mutex);
1235         mutex_destroy(&mt9v111->stream_mutex);
1236 
1237         return ret;
1238 }
1239 
1240 static int mt9v111_remove(struct i2c_client *client)
1241 {
1242         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1243         struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
1244 
1245         v4l2_async_unregister_subdev(sd);
1246 
1247 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1248         media_entity_cleanup(&sd->entity);
1249 #endif
1250 
1251         v4l2_ctrl_handler_free(&mt9v111->ctrls);
1252 
1253         mutex_destroy(&mt9v111->pwr_mutex);
1254         mutex_destroy(&mt9v111->stream_mutex);
1255 
1256         devm_gpiod_put(mt9v111->dev, mt9v111->oe);
1257         devm_gpiod_put(mt9v111->dev, mt9v111->standby);
1258         devm_gpiod_put(mt9v111->dev, mt9v111->reset);
1259 
1260         devm_clk_put(mt9v111->dev, mt9v111->clk);
1261 
1262         return 0;
1263 }
1264 
1265 static const struct of_device_id mt9v111_of_match[] = {
1266         { .compatible = "aptina,mt9v111", },
1267         { /* sentinel */ },
1268 };
1269 
1270 static struct i2c_driver mt9v111_driver = {
1271         .driver = {
1272                 .name = "mt9v111",
1273                 .of_match_table = mt9v111_of_match,
1274         },
1275         .probe_new      = mt9v111_probe,
1276         .remove         = mt9v111_remove,
1277 };
1278 
1279 module_i2c_driver(mt9v111_driver);
1280 
1281 MODULE_DESCRIPTION("V4L2 sensor driver for Aptina MT9V111");
1282 MODULE_AUTHOR("Jacopo Mondi <jacopo@jmondi.org>");
1283 MODULE_LICENSE("GPL v2");
/* [<][>][^][v][top][bottom][index][help] */
root/drivers/media/i2c/mt9v111.c

DEFINITIONS
