linux/drivers/media/i2c/mt9v111.c
<<
>>
Prefs
   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. */
 106static const struct v4l2_mbus_framefmt mt9v111_def_fmt = {
 107        .width          = 640,
 108        .height         = 480,
 109        .code           = MEDIA_BUS_FMT_UYVY8_2X8,
 110        .field          = V4L2_FIELD_NONE,
 111        .colorspace     = V4L2_COLORSPACE_SRGB,
 112        .ycbcr_enc      = V4L2_YCBCR_ENC_601,
 113        .quantization   = V4L2_QUANTIZATION_LIM_RANGE,
 114        .xfer_func      = V4L2_XFER_FUNC_SRGB,
 115};
 116
 117struct mt9v111_dev {
 118        struct device *dev;
 119        struct i2c_client *client;
 120
 121        u8 addr_space;
 122
 123        struct v4l2_subdev sd;
 124#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 */
 170static 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
 187static 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 */
 195static 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
 220static 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
 249static 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
 275static 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
 302static 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
 318static 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
 334static 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
 361static 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
 381static 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
 396static 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
 410static 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
 448static 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
 534static 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
 646static 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
 684static 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
 718error_unlock:
 719        mutex_unlock(&mt9v111->stream_mutex);
 720
 721        return ret;
 722}
 723
 724static 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
 776static 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
 792static 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
 812static 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
 824static 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
 847static 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
 862static 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
 879static 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
 948done:
 949        format->format = *__fmt;
 950
 951        mutex_unlock(&mt9v111->stream_mutex);
 952
 953        return 0;
 954}
 955
 956static 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
 964static const struct v4l2_subdev_core_ops mt9v111_core_ops = {
 965        .s_power                = mt9v111_s_power,
 966};
 967
 968static 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
 974static 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
 983static 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)
 990static const struct media_entity_operations mt9v111_subdev_entity_ops = {
 991        .link_validate = v4l2_subdev_link_validate,
 992};
 993#endif
 994
 995/* --- V4L2 ctrl --- */
 996static 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
1071static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = {
1072        .s_ctrl = mt9v111_s_ctrl,
1073};
1074
1075static 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
1101power_off:
1102        __mt9v111_power_off(&mt9v111->sd);
1103
1104        return ret;
1105}
1106
1107static 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
1226error_free_entity:
1227#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1228        media_entity_cleanup(&mt9v111->sd.entity);
1229#endif
1230
1231error_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
1240static 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        return 0;
1257}
1258
1259static const struct of_device_id mt9v111_of_match[] = {
1260        { .compatible = "aptina,mt9v111", },
1261        { /* sentinel */ },
1262};
1263
1264static struct i2c_driver mt9v111_driver = {
1265        .driver = {
1266                .name = "mt9v111",
1267                .of_match_table = mt9v111_of_match,
1268        },
1269        .probe_new      = mt9v111_probe,
1270        .remove         = mt9v111_remove,
1271};
1272
1273module_i2c_driver(mt9v111_driver);
1274
1275MODULE_DESCRIPTION("V4L2 sensor driver for Aptina MT9V111");
1276MODULE_AUTHOR("Jacopo Mondi <jacopo@jmondi.org>");
1277MODULE_LICENSE("GPL v2");
1278