Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * V4L2 sensor driver for Aptina MT9V111 image sensor
0004  * Copyright (C) 2018 Jacopo Mondi <jacopo@jmondi.org>
0005  *
0006  * Based on mt9v032 driver
0007  * Copyright (C) 2010, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
0008  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
0009  *
0010  * Based on mt9v011 driver
0011  * Copyright (c) 2009 Mauro Carvalho Chehab <mchehab@kernel.org>
0012  */
0013 
0014 #include <linux/clk.h>
0015 #include <linux/delay.h>
0016 #include <linux/gpio/consumer.h>
0017 #include <linux/i2c.h>
0018 #include <linux/of.h>
0019 #include <linux/slab.h>
0020 #include <linux/videodev2.h>
0021 #include <linux/v4l2-mediabus.h>
0022 #include <linux/module.h>
0023 
0024 #include <media/v4l2-ctrls.h>
0025 #include <media/v4l2-device.h>
0026 #include <media/v4l2-fwnode.h>
0027 #include <media/v4l2-image-sizes.h>
0028 #include <media/v4l2-subdev.h>
0029 
0030 /*
0031  * MT9V111 is a 1/4-Inch CMOS digital image sensor with an integrated
0032  * Image Flow Processing (IFP) engine and a sensor core loosely based on
0033  * MT9V011.
0034  *
0035  * The IFP can produce several output image formats from the sensor core
0036  * output. This driver currently supports only YUYV format permutations.
0037  *
0038  * The driver allows manual frame rate control through s_frame_interval subdev
0039  * operation or V4L2_CID_V/HBLANK controls, but it is known that the
0040  * auto-exposure algorithm might modify the programmed frame rate. While the
0041  * driver initially programs the sensor with auto-exposure and
0042  * auto-white-balancing enabled, it is possible to disable them and more
0043  * precisely control the frame rate.
0044  *
0045  * While it seems possible to instruct the auto-exposure control algorithm to
0046  * respect a programmed frame rate when adjusting the pixel integration time,
0047  * registers controlling this feature are not documented in the public
0048  * available sensor manual used to develop this driver (09005aef80e90084,
0049  * MT9V111_1.fm - Rev. G 1/05 EN).
0050  */
0051 
0052 #define MT9V111_CHIP_ID_HIGH                0x82
0053 #define MT9V111_CHIP_ID_LOW             0x3a
0054 
0055 #define MT9V111_R01_ADDR_SPACE              0x01
0056 #define MT9V111_R01_IFP                 0x01
0057 #define MT9V111_R01_CORE                0x04
0058 
0059 #define MT9V111_IFP_R06_OPMODE_CTRL         0x06
0060 #define     MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN  BIT(1)
0061 #define     MT9V111_IFP_R06_OPMODE_CTRL_AE_EN   BIT(14)
0062 #define MT9V111_IFP_R07_IFP_RESET           0x07
0063 #define     MT9V111_IFP_R07_IFP_RESET_MASK      BIT(0)
0064 #define MT9V111_IFP_R08_OUTFMT_CTRL         0x08
0065 #define     MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER BIT(11)
0066 #define     MT9V111_IFP_R08_OUTFMT_CTRL_PCLK    BIT(5)
0067 #define MT9V111_IFP_R3A_OUTFMT_CTRL2            0x3a
0068 #define     MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR  BIT(0)
0069 #define     MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC    BIT(1)
0070 #define     MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK  GENMASK(2, 0)
0071 #define MT9V111_IFP_RA5_HPAN                0xa5
0072 #define MT9V111_IFP_RA6_HZOOM               0xa6
0073 #define MT9V111_IFP_RA7_HOUT                0xa7
0074 #define MT9V111_IFP_RA8_VPAN                0xa8
0075 #define MT9V111_IFP_RA9_VZOOM               0xa9
0076 #define MT9V111_IFP_RAA_VOUT                0xaa
0077 #define MT9V111_IFP_DECIMATION_MASK         GENMASK(9, 0)
0078 #define MT9V111_IFP_DECIMATION_FREEZE           BIT(15)
0079 
0080 #define MT9V111_CORE_R03_WIN_HEIGHT         0x03
0081 #define     MT9V111_CORE_R03_WIN_V_OFFS     2
0082 #define MT9V111_CORE_R04_WIN_WIDTH          0x04
0083 #define     MT9V111_CORE_R04_WIN_H_OFFS     114
0084 #define MT9V111_CORE_R05_HBLANK             0x05
0085 #define     MT9V111_CORE_R05_MIN_HBLANK     0x09
0086 #define     MT9V111_CORE_R05_MAX_HBLANK     GENMASK(9, 0)
0087 #define     MT9V111_CORE_R05_DEF_HBLANK     0x26
0088 #define MT9V111_CORE_R06_VBLANK             0x06
0089 #define     MT9V111_CORE_R06_MIN_VBLANK     0x03
0090 #define     MT9V111_CORE_R06_MAX_VBLANK     GENMASK(11, 0)
0091 #define     MT9V111_CORE_R06_DEF_VBLANK     0x04
0092 #define MT9V111_CORE_R07_OUT_CTRL           0x07
0093 #define     MT9V111_CORE_R07_OUT_CTRL_SAMPLE    BIT(4)
0094 #define MT9V111_CORE_R09_PIXEL_INT          0x09
0095 #define     MT9V111_CORE_R09_PIXEL_INT_MASK     GENMASK(11, 0)
0096 #define MT9V111_CORE_R0D_CORE_RESET         0x0d
0097 #define     MT9V111_CORE_R0D_CORE_RESET_MASK    BIT(0)
0098 #define MT9V111_CORE_RFF_CHIP_VER           0xff
0099 
0100 #define MT9V111_PIXEL_ARRAY_WIDTH           640
0101 #define MT9V111_PIXEL_ARRAY_HEIGHT          480
0102 
0103 #define MT9V111_MAX_CLKIN               27000000
0104 
0105 /* The default sensor configuration at startup time. */
0106 static const struct v4l2_mbus_framefmt mt9v111_def_fmt = {
0107     .width      = 640,
0108     .height     = 480,
0109     .code       = MEDIA_BUS_FMT_UYVY8_2X8,
0110     .field      = V4L2_FIELD_NONE,
0111     .colorspace = V4L2_COLORSPACE_SRGB,
0112     .ycbcr_enc  = V4L2_YCBCR_ENC_601,
0113     .quantization   = V4L2_QUANTIZATION_LIM_RANGE,
0114     .xfer_func  = V4L2_XFER_FUNC_SRGB,
0115 };
0116 
0117 struct mt9v111_dev {
0118     struct device *dev;
0119     struct i2c_client *client;
0120 
0121     u8 addr_space;
0122 
0123     struct v4l2_subdev sd;
0124 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
0125     struct media_pad pad;
0126 #endif
0127 
0128     struct v4l2_ctrl *auto_awb;
0129     struct v4l2_ctrl *auto_exp;
0130     struct v4l2_ctrl *hblank;
0131     struct v4l2_ctrl *vblank;
0132     struct v4l2_ctrl_handler ctrls;
0133 
0134     /* Output image format and sizes. */
0135     struct v4l2_mbus_framefmt fmt;
0136     unsigned int fps;
0137 
0138     /* Protects power up/down sequences. */
0139     struct mutex pwr_mutex;
0140     int pwr_count;
0141 
0142     /* Protects stream on/off sequences. */
0143     struct mutex stream_mutex;
0144     bool streaming;
0145 
0146     /* Flags to mark HW settings as not yet applied. */
0147     bool pending;
0148 
0149     /* Clock provider and system clock frequency. */
0150     struct clk *clk;
0151     u32 sysclk;
0152 
0153     struct gpio_desc *oe;
0154     struct gpio_desc *standby;
0155     struct gpio_desc *reset;
0156 };
0157 
0158 #define sd_to_mt9v111(__sd) container_of((__sd), struct mt9v111_dev, sd)
0159 
0160 /*
0161  * mt9v111_mbus_fmt - List all media bus formats supported by the driver.
0162  *
0163  * Only list the media bus code here. The image sizes are freely configurable
0164  * in the pixel array sizes range.
0165  *
0166  * The desired frame interval, in the supported frame interval range, is
0167  * obtained by configuring blanking as the sensor does not have a PLL but
0168  * only a fixed clock divider that generates the output pixel clock.
0169  */
0170 static struct mt9v111_mbus_fmt {
0171     u32 code;
0172 } mt9v111_formats[] = {
0173     {
0174         .code   = MEDIA_BUS_FMT_UYVY8_2X8,
0175     },
0176     {
0177         .code   = MEDIA_BUS_FMT_YUYV8_2X8,
0178     },
0179     {
0180         .code   = MEDIA_BUS_FMT_VYUY8_2X8,
0181     },
0182     {
0183         .code   = MEDIA_BUS_FMT_YVYU8_2X8,
0184     },
0185 };
0186 
0187 static u32 mt9v111_frame_intervals[] = {5, 10, 15, 20, 30};
0188 
0189 /*
0190  * mt9v111_frame_sizes - List sensor's supported resolutions.
0191  *
0192  * Resolution generated through decimation in the IFP block from the
0193  * full VGA pixel array.
0194  */
0195 static struct v4l2_rect mt9v111_frame_sizes[] = {
0196     {
0197         .width  = 640,
0198         .height = 480,
0199     },
0200     {
0201         .width  = 352,
0202         .height = 288
0203     },
0204     {
0205         .width  = 320,
0206         .height = 240,
0207     },
0208     {
0209         .width  = 176,
0210         .height = 144,
0211     },
0212     {
0213         .width  = 160,
0214         .height = 120,
0215     },
0216 };
0217 
0218 /* --- Device I/O access --- */
0219 
0220 static int __mt9v111_read(struct i2c_client *c, u8 reg, u16 *val)
0221 {
0222     struct i2c_msg msg[2];
0223     __be16 buf;
0224     int ret;
0225 
0226     msg[0].addr = c->addr;
0227     msg[0].flags = 0;
0228     msg[0].len = 1;
0229     msg[0].buf = &reg;
0230 
0231     msg[1].addr = c->addr;
0232     msg[1].flags = I2C_M_RD;
0233     msg[1].len = 2;
0234     msg[1].buf = (char *)&buf;
0235 
0236     ret = i2c_transfer(c->adapter, msg, 2);
0237     if (ret < 0) {
0238         dev_err(&c->dev, "i2c read transfer error: %d\n", ret);
0239         return ret;
0240     }
0241 
0242     *val = be16_to_cpu(buf);
0243 
0244     dev_dbg(&c->dev, "%s: %x=%x\n", __func__, reg, *val);
0245 
0246     return 0;
0247 }
0248 
0249 static int __mt9v111_write(struct i2c_client *c, u8 reg, u16 val)
0250 {
0251     struct i2c_msg msg;
0252     u8 buf[3] = { 0 };
0253     int ret;
0254 
0255     buf[0] = reg;
0256     buf[1] = val >> 8;
0257     buf[2] = val & 0xff;
0258 
0259     msg.addr = c->addr;
0260     msg.flags = 0;
0261     msg.len = 3;
0262     msg.buf = (char *)buf;
0263 
0264     dev_dbg(&c->dev, "%s: %x = %x%x\n", __func__, reg, buf[1], buf[2]);
0265 
0266     ret = i2c_transfer(c->adapter, &msg, 1);
0267     if (ret < 0) {
0268         dev_err(&c->dev, "i2c write transfer error: %d\n", ret);
0269         return ret;
0270     }
0271 
0272     return 0;
0273 }
0274 
0275 static int __mt9v111_addr_space_select(struct i2c_client *c, u16 addr_space)
0276 {
0277     struct v4l2_subdev *sd = i2c_get_clientdata(c);
0278     struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
0279     u16 val;
0280     int ret;
0281 
0282     if (mt9v111->addr_space == addr_space)
0283         return 0;
0284 
0285     ret = __mt9v111_write(c, MT9V111_R01_ADDR_SPACE, addr_space);
0286     if (ret)
0287         return ret;
0288 
0289     /* Verify address space has been updated */
0290     ret = __mt9v111_read(c, MT9V111_R01_ADDR_SPACE, &val);
0291     if (ret)
0292         return ret;
0293 
0294     if (val != addr_space)
0295         return -EINVAL;
0296 
0297     mt9v111->addr_space = addr_space;
0298 
0299     return 0;
0300 }
0301 
0302 static int mt9v111_read(struct i2c_client *c, u8 addr_space, u8 reg, u16 *val)
0303 {
0304     int ret;
0305 
0306     /* Select register address space first. */
0307     ret = __mt9v111_addr_space_select(c, addr_space);
0308     if (ret)
0309         return ret;
0310 
0311     ret = __mt9v111_read(c, reg, val);
0312     if (ret)
0313         return ret;
0314 
0315     return 0;
0316 }
0317 
0318 static int mt9v111_write(struct i2c_client *c, u8 addr_space, u8 reg, u16 val)
0319 {
0320     int ret;
0321 
0322     /* Select register address space first. */
0323     ret = __mt9v111_addr_space_select(c, addr_space);
0324     if (ret)
0325         return ret;
0326 
0327     ret = __mt9v111_write(c, reg, val);
0328     if (ret)
0329         return ret;
0330 
0331     return 0;
0332 }
0333 
0334 static int mt9v111_update(struct i2c_client *c, u8 addr_space, u8 reg,
0335               u16 mask, u16 val)
0336 {
0337     u16 current_val;
0338     int ret;
0339 
0340     /* Select register address space first. */
0341     ret = __mt9v111_addr_space_select(c, addr_space);
0342     if (ret)
0343         return ret;
0344 
0345     /* Read the current register value, then update it. */
0346     ret = __mt9v111_read(c, reg, &current_val);
0347     if (ret)
0348         return ret;
0349 
0350     current_val &= ~mask;
0351     current_val |= (val & mask);
0352     ret = __mt9v111_write(c, reg, current_val);
0353     if (ret)
0354         return ret;
0355 
0356     return 0;
0357 }
0358 
0359 /* --- Sensor HW operations --- */
0360 
0361 static int __mt9v111_power_on(struct v4l2_subdev *sd)
0362 {
0363     struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
0364     int ret;
0365 
0366     ret = clk_prepare_enable(mt9v111->clk);
0367     if (ret)
0368         return ret;
0369 
0370     clk_set_rate(mt9v111->clk, mt9v111->sysclk);
0371 
0372     gpiod_set_value(mt9v111->standby, 0);
0373     usleep_range(500, 1000);
0374 
0375     gpiod_set_value(mt9v111->oe, 1);
0376     usleep_range(500, 1000);
0377 
0378     return 0;
0379 }
0380 
0381 static int __mt9v111_power_off(struct v4l2_subdev *sd)
0382 {
0383     struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
0384 
0385     gpiod_set_value(mt9v111->oe, 0);
0386     usleep_range(500, 1000);
0387 
0388     gpiod_set_value(mt9v111->standby, 1);
0389     usleep_range(500, 1000);
0390 
0391     clk_disable_unprepare(mt9v111->clk);
0392 
0393     return 0;
0394 }
0395 
0396 static int __mt9v111_hw_reset(struct mt9v111_dev *mt9v111)
0397 {
0398     if (!mt9v111->reset)
0399         return -EINVAL;
0400 
0401     gpiod_set_value(mt9v111->reset, 1);
0402     usleep_range(500, 1000);
0403 
0404     gpiod_set_value(mt9v111->reset, 0);
0405     usleep_range(500, 1000);
0406 
0407     return 0;
0408 }
0409 
0410 static int __mt9v111_sw_reset(struct mt9v111_dev *mt9v111)
0411 {
0412     struct i2c_client *c = mt9v111->client;
0413     int ret;
0414 
0415     /* Software reset core and IFP blocks. */
0416 
0417     ret = mt9v111_update(c, MT9V111_R01_CORE,
0418                  MT9V111_CORE_R0D_CORE_RESET,
0419                  MT9V111_CORE_R0D_CORE_RESET_MASK, 1);
0420     if (ret)
0421         return ret;
0422     usleep_range(500, 1000);
0423 
0424     ret = mt9v111_update(c, MT9V111_R01_CORE,
0425                  MT9V111_CORE_R0D_CORE_RESET,
0426                  MT9V111_CORE_R0D_CORE_RESET_MASK, 0);
0427     if (ret)
0428         return ret;
0429     usleep_range(500, 1000);
0430 
0431     ret = mt9v111_update(c, MT9V111_R01_IFP,
0432                  MT9V111_IFP_R07_IFP_RESET,
0433                  MT9V111_IFP_R07_IFP_RESET_MASK, 1);
0434     if (ret)
0435         return ret;
0436     usleep_range(500, 1000);
0437 
0438     ret = mt9v111_update(c, MT9V111_R01_IFP,
0439                  MT9V111_IFP_R07_IFP_RESET,
0440                  MT9V111_IFP_R07_IFP_RESET_MASK, 0);
0441     if (ret)
0442         return ret;
0443     usleep_range(500, 1000);
0444 
0445     return 0;
0446 }
0447 
0448 static int mt9v111_calc_frame_rate(struct mt9v111_dev *mt9v111,
0449                    struct v4l2_fract *tpf)
0450 {
0451     unsigned int fps = tpf->numerator ?
0452                tpf->denominator / tpf->numerator :
0453                tpf->denominator;
0454     unsigned int best_diff;
0455     unsigned int frm_cols;
0456     unsigned int row_pclk;
0457     unsigned int best_fps;
0458     unsigned int pclk;
0459     unsigned int diff;
0460     unsigned int idx;
0461     unsigned int hb;
0462     unsigned int vb;
0463     unsigned int i;
0464     int ret;
0465 
0466     /* Approximate to the closest supported frame interval. */
0467     best_diff = ~0L;
0468     for (i = 0, idx = 0; i < ARRAY_SIZE(mt9v111_frame_intervals); i++) {
0469         diff = abs(fps - mt9v111_frame_intervals[i]);
0470         if (diff < best_diff) {
0471             idx = i;
0472             best_diff = diff;
0473         }
0474     }
0475     fps = mt9v111_frame_intervals[idx];
0476 
0477     /*
0478      * The sensor does not provide a PLL circuitry and pixel clock is
0479      * generated dividing the master clock source by two.
0480      *
0481      * Trow = (W + Hblank + 114) * 2 * (1 / SYSCLK)
0482      * TFrame = Trow * (H + Vblank + 2)
0483      *
0484      * FPS = (SYSCLK / 2) / (Trow * (H + Vblank + 2))
0485      *
0486      * This boils down to tune H and V blanks to best approximate the
0487      * above equation.
0488      *
0489      * Test all available H/V blank values, until we reach the
0490      * desired frame rate.
0491      */
0492     best_fps = vb = hb = 0;
0493     pclk = DIV_ROUND_CLOSEST(mt9v111->sysclk, 2);
0494     row_pclk = MT9V111_PIXEL_ARRAY_WIDTH + 7 + MT9V111_CORE_R04_WIN_H_OFFS;
0495     frm_cols = MT9V111_PIXEL_ARRAY_HEIGHT + 7 + MT9V111_CORE_R03_WIN_V_OFFS;
0496 
0497     best_diff = ~0L;
0498     for (vb = MT9V111_CORE_R06_MIN_VBLANK;
0499          vb < MT9V111_CORE_R06_MAX_VBLANK; vb++) {
0500         for (hb = MT9V111_CORE_R05_MIN_HBLANK;
0501              hb < MT9V111_CORE_R05_MAX_HBLANK; hb += 10) {
0502             unsigned int t_frame = (row_pclk + hb) *
0503                            (frm_cols + vb);
0504             unsigned int t_fps = DIV_ROUND_CLOSEST(pclk, t_frame);
0505 
0506             diff = abs(fps - t_fps);
0507             if (diff < best_diff) {
0508                 best_diff = diff;
0509                 best_fps = t_fps;
0510 
0511                 if (diff == 0)
0512                     break;
0513             }
0514         }
0515 
0516         if (diff == 0)
0517             break;
0518     }
0519 
0520     ret = v4l2_ctrl_s_ctrl_int64(mt9v111->hblank, hb);
0521     if (ret)
0522         return ret;
0523 
0524     ret = v4l2_ctrl_s_ctrl_int64(mt9v111->vblank, vb);
0525     if (ret)
0526         return ret;
0527 
0528     tpf->numerator = 1;
0529     tpf->denominator = best_fps;
0530 
0531     return 0;
0532 }
0533 
0534 static int mt9v111_hw_config(struct mt9v111_dev *mt9v111)
0535 {
0536     struct i2c_client *c = mt9v111->client;
0537     unsigned int ret;
0538     u16 outfmtctrl2;
0539 
0540     /* Force device reset. */
0541     ret = __mt9v111_hw_reset(mt9v111);
0542     if (ret == -EINVAL)
0543         ret = __mt9v111_sw_reset(mt9v111);
0544     if (ret)
0545         return ret;
0546 
0547     /* Configure internal clock sample rate. */
0548     ret = mt9v111->sysclk < DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ?
0549                 mt9v111_update(c, MT9V111_R01_CORE,
0550                     MT9V111_CORE_R07_OUT_CTRL,
0551                     MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 1) :
0552                 mt9v111_update(c, MT9V111_R01_CORE,
0553                     MT9V111_CORE_R07_OUT_CTRL,
0554                     MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 0);
0555     if (ret)
0556         return ret;
0557 
0558     /*
0559      * Configure output image format components ordering.
0560      *
0561      * TODO: IFP block can also output several RGB permutations, we only
0562      *   support YUYV permutations at the moment.
0563      */
0564     switch (mt9v111->fmt.code) {
0565     case MEDIA_BUS_FMT_YUYV8_2X8:
0566             outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC;
0567             break;
0568     case MEDIA_BUS_FMT_VYUY8_2X8:
0569             outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR;
0570             break;
0571     case MEDIA_BUS_FMT_YVYU8_2X8:
0572             outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC |
0573                       MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR;
0574             break;
0575     case MEDIA_BUS_FMT_UYVY8_2X8:
0576     default:
0577             outfmtctrl2 = 0;
0578             break;
0579     }
0580 
0581     ret = mt9v111_update(c, MT9V111_R01_IFP, MT9V111_IFP_R3A_OUTFMT_CTRL2,
0582                  MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK,
0583                  outfmtctrl2);
0584     if (ret)
0585         return ret;
0586 
0587     /*
0588      * Do not change default sensor's core configuration:
0589      * output the whole 640x480 pixel array, skip 18 columns and 6 rows.
0590      *
0591      * Instead, control the output image size through IFP block.
0592      *
0593      * TODO: No zoom&pan support. Currently we control the output image
0594      *   size only through decimation, with no zoom support.
0595      */
0596     ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA5_HPAN,
0597                 MT9V111_IFP_DECIMATION_FREEZE);
0598     if (ret)
0599         return ret;
0600 
0601     ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA8_VPAN,
0602                 MT9V111_IFP_DECIMATION_FREEZE);
0603     if (ret)
0604         return ret;
0605 
0606     ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA6_HZOOM,
0607                 MT9V111_IFP_DECIMATION_FREEZE |
0608                 MT9V111_PIXEL_ARRAY_WIDTH);
0609     if (ret)
0610         return ret;
0611 
0612     ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA9_VZOOM,
0613                 MT9V111_IFP_DECIMATION_FREEZE |
0614                 MT9V111_PIXEL_ARRAY_HEIGHT);
0615     if (ret)
0616         return ret;
0617 
0618     ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA7_HOUT,
0619                 MT9V111_IFP_DECIMATION_FREEZE |
0620                 mt9v111->fmt.width);
0621     if (ret)
0622         return ret;
0623 
0624     ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RAA_VOUT,
0625                 mt9v111->fmt.height);
0626     if (ret)
0627         return ret;
0628 
0629     /* Apply controls to set auto exp, auto awb and timings */
0630     ret = v4l2_ctrl_handler_setup(&mt9v111->ctrls);
0631     if (ret)
0632         return ret;
0633 
0634     /*
0635      * Set pixel integration time to the whole frame time.
0636      * This value controls the the shutter delay when running with AE
0637      * disabled. If longer than frame time, it affects the output
0638      * frame rate.
0639      */
0640     return mt9v111_write(c, MT9V111_R01_CORE, MT9V111_CORE_R09_PIXEL_INT,
0641                  MT9V111_PIXEL_ARRAY_HEIGHT);
0642 }
0643 
0644 /* ---  V4L2 subdev operations --- */
0645 
0646 static int mt9v111_s_power(struct v4l2_subdev *sd, int on)
0647 {
0648     struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
0649     int pwr_count;
0650     int ret = 0;
0651 
0652     mutex_lock(&mt9v111->pwr_mutex);
0653 
0654     /*
0655      * Make sure we're transitioning from 0 to 1, or viceversa,
0656      * before actually changing the power state.
0657      */
0658     pwr_count = mt9v111->pwr_count;
0659     pwr_count += on ? 1 : -1;
0660     if (pwr_count == !!on) {
0661         ret = on ? __mt9v111_power_on(sd) :
0662                __mt9v111_power_off(sd);
0663         if (!ret)
0664             /* All went well, updated power counter. */
0665             mt9v111->pwr_count = pwr_count;
0666 
0667         mutex_unlock(&mt9v111->pwr_mutex);
0668 
0669         return ret;
0670     }
0671 
0672     /*
0673      * Update power counter to keep track of how many nested calls we
0674      * received.
0675      */
0676     WARN_ON(pwr_count < 0 || pwr_count > 1);
0677     mt9v111->pwr_count = pwr_count;
0678 
0679     mutex_unlock(&mt9v111->pwr_mutex);
0680 
0681     return ret;
0682 }
0683 
0684 static int mt9v111_s_stream(struct v4l2_subdev *subdev, int enable)
0685 {
0686     struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
0687     int ret;
0688 
0689     mutex_lock(&mt9v111->stream_mutex);
0690 
0691     if (mt9v111->streaming == enable) {
0692         mutex_unlock(&mt9v111->stream_mutex);
0693         return 0;
0694     }
0695 
0696     ret = mt9v111_s_power(subdev, enable);
0697     if (ret)
0698         goto error_unlock;
0699 
0700     if (enable && mt9v111->pending) {
0701         ret = mt9v111_hw_config(mt9v111);
0702         if (ret)
0703             goto error_unlock;
0704 
0705         /*
0706          * No need to update control here as far as only H/VBLANK are
0707          * supported and immediately programmed to registers in .s_ctrl
0708          */
0709 
0710         mt9v111->pending = false;
0711     }
0712 
0713     mt9v111->streaming = enable ? true : false;
0714     mutex_unlock(&mt9v111->stream_mutex);
0715 
0716     return 0;
0717 
0718 error_unlock:
0719     mutex_unlock(&mt9v111->stream_mutex);
0720 
0721     return ret;
0722 }
0723 
0724 static int mt9v111_s_frame_interval(struct v4l2_subdev *sd,
0725                     struct v4l2_subdev_frame_interval *ival)
0726 {
0727     struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
0728     struct v4l2_fract *tpf = &ival->interval;
0729     unsigned int fps = tpf->numerator ?
0730                tpf->denominator / tpf->numerator :
0731                tpf->denominator;
0732     unsigned int max_fps;
0733 
0734     if (!tpf->numerator)
0735         tpf->numerator = 1;
0736 
0737     mutex_lock(&mt9v111->stream_mutex);
0738 
0739     if (mt9v111->streaming) {
0740         mutex_unlock(&mt9v111->stream_mutex);
0741         return -EBUSY;
0742     }
0743 
0744     if (mt9v111->fps == fps) {
0745         mutex_unlock(&mt9v111->stream_mutex);
0746         return 0;
0747     }
0748 
0749     /* Make sure frame rate/image sizes constraints are respected. */
0750     if (mt9v111->fmt.width < QVGA_WIDTH &&
0751         mt9v111->fmt.height < QVGA_HEIGHT)
0752         max_fps = 90;
0753     else if (mt9v111->fmt.width < CIF_WIDTH &&
0754          mt9v111->fmt.height < CIF_HEIGHT)
0755         max_fps = 60;
0756     else
0757         max_fps = mt9v111->sysclk <
0758                 DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ? 15 :
0759                                       30;
0760 
0761     if (fps > max_fps) {
0762         mutex_unlock(&mt9v111->stream_mutex);
0763         return -EINVAL;
0764     }
0765 
0766     mt9v111_calc_frame_rate(mt9v111, tpf);
0767 
0768     mt9v111->fps = fps;
0769     mt9v111->pending = true;
0770 
0771     mutex_unlock(&mt9v111->stream_mutex);
0772 
0773     return 0;
0774 }
0775 
0776 static int mt9v111_g_frame_interval(struct v4l2_subdev *sd,
0777                     struct v4l2_subdev_frame_interval *ival)
0778 {
0779     struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
0780     struct v4l2_fract *tpf = &ival->interval;
0781 
0782     mutex_lock(&mt9v111->stream_mutex);
0783 
0784     tpf->numerator = 1;
0785     tpf->denominator = mt9v111->fps;
0786 
0787     mutex_unlock(&mt9v111->stream_mutex);
0788 
0789     return 0;
0790 }
0791 
0792 static struct v4l2_mbus_framefmt *__mt9v111_get_pad_format(
0793                     struct mt9v111_dev *mt9v111,
0794                     struct v4l2_subdev_state *sd_state,
0795                     unsigned int pad,
0796                     enum v4l2_subdev_format_whence which)
0797 {
0798     switch (which) {
0799     case V4L2_SUBDEV_FORMAT_TRY:
0800 #if IS_ENABLED(CONFIG_VIDEO_V4L2_SUBDEV_API)
0801         return v4l2_subdev_get_try_format(&mt9v111->sd, sd_state, pad);
0802 #else
0803         return &sd_state->pads->try_fmt;
0804 #endif
0805     case V4L2_SUBDEV_FORMAT_ACTIVE:
0806         return &mt9v111->fmt;
0807     default:
0808         return NULL;
0809     }
0810 }
0811 
0812 static int mt9v111_enum_mbus_code(struct v4l2_subdev *subdev,
0813                   struct v4l2_subdev_state *sd_state,
0814                   struct v4l2_subdev_mbus_code_enum *code)
0815 {
0816     if (code->pad || code->index > ARRAY_SIZE(mt9v111_formats) - 1)
0817         return -EINVAL;
0818 
0819     code->code = mt9v111_formats[code->index].code;
0820 
0821     return 0;
0822 }
0823 
0824 static int mt9v111_enum_frame_interval(struct v4l2_subdev *sd,
0825                 struct v4l2_subdev_state *sd_state,
0826                 struct v4l2_subdev_frame_interval_enum *fie)
0827 {
0828     unsigned int i;
0829 
0830     if (fie->pad || fie->index >= ARRAY_SIZE(mt9v111_frame_intervals))
0831         return -EINVAL;
0832 
0833     for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++)
0834         if (fie->width == mt9v111_frame_sizes[i].width &&
0835             fie->height == mt9v111_frame_sizes[i].height)
0836             break;
0837 
0838     if (i == ARRAY_SIZE(mt9v111_frame_sizes))
0839         return -EINVAL;
0840 
0841     fie->interval.numerator = 1;
0842     fie->interval.denominator = mt9v111_frame_intervals[fie->index];
0843 
0844     return 0;
0845 }
0846 
0847 static int mt9v111_enum_frame_size(struct v4l2_subdev *subdev,
0848                    struct v4l2_subdev_state *sd_state,
0849                    struct v4l2_subdev_frame_size_enum *fse)
0850 {
0851     if (fse->pad || fse->index >= ARRAY_SIZE(mt9v111_frame_sizes))
0852         return -EINVAL;
0853 
0854     fse->min_width = mt9v111_frame_sizes[fse->index].width;
0855     fse->max_width = mt9v111_frame_sizes[fse->index].width;
0856     fse->min_height = mt9v111_frame_sizes[fse->index].height;
0857     fse->max_height = mt9v111_frame_sizes[fse->index].height;
0858 
0859     return 0;
0860 }
0861 
0862 static int mt9v111_get_format(struct v4l2_subdev *subdev,
0863                   struct v4l2_subdev_state *sd_state,
0864                   struct v4l2_subdev_format *format)
0865 {
0866     struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
0867 
0868     if (format->pad)
0869         return -EINVAL;
0870 
0871     mutex_lock(&mt9v111->stream_mutex);
0872     format->format = *__mt9v111_get_pad_format(mt9v111, sd_state,
0873                            format->pad,
0874                            format->which);
0875     mutex_unlock(&mt9v111->stream_mutex);
0876 
0877     return 0;
0878 }
0879 
0880 static int mt9v111_set_format(struct v4l2_subdev *subdev,
0881                   struct v4l2_subdev_state *sd_state,
0882                   struct v4l2_subdev_format *format)
0883 {
0884     struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
0885     struct v4l2_mbus_framefmt new_fmt;
0886     struct v4l2_mbus_framefmt *__fmt;
0887     unsigned int best_fit = ~0L;
0888     unsigned int idx = 0;
0889     unsigned int i;
0890 
0891     mutex_lock(&mt9v111->stream_mutex);
0892     if (mt9v111->streaming) {
0893         mutex_unlock(&mt9v111->stream_mutex);
0894         return -EBUSY;
0895     }
0896 
0897     if (format->pad) {
0898         mutex_unlock(&mt9v111->stream_mutex);
0899         return -EINVAL;
0900     }
0901 
0902     /* Update mbus format code and sizes. */
0903     for (i = 0; i < ARRAY_SIZE(mt9v111_formats); i++) {
0904         if (format->format.code == mt9v111_formats[i].code) {
0905             new_fmt.code = mt9v111_formats[i].code;
0906             break;
0907         }
0908     }
0909     if (i == ARRAY_SIZE(mt9v111_formats))
0910         new_fmt.code = mt9v111_formats[0].code;
0911 
0912     for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) {
0913         unsigned int fit = abs(mt9v111_frame_sizes[i].width -
0914                        format->format.width) +
0915                    abs(mt9v111_frame_sizes[i].height -
0916                        format->format.height);
0917         if (fit < best_fit) {
0918             best_fit = fit;
0919             idx = i;
0920 
0921             if (fit == 0)
0922                 break;
0923         }
0924     }
0925     new_fmt.width = mt9v111_frame_sizes[idx].width;
0926     new_fmt.height = mt9v111_frame_sizes[idx].height;
0927 
0928     /* Update the device (or pad) format if it has changed. */
0929     __fmt = __mt9v111_get_pad_format(mt9v111, sd_state, format->pad,
0930                      format->which);
0931 
0932     /* Format hasn't changed, stop here. */
0933     if (__fmt->code == new_fmt.code &&
0934         __fmt->width == new_fmt.width &&
0935         __fmt->height == new_fmt.height)
0936         goto done;
0937 
0938     /* Update the format and sizes, then  mark changes as pending. */
0939     __fmt->code = new_fmt.code;
0940     __fmt->width = new_fmt.width;
0941     __fmt->height = new_fmt.height;
0942 
0943     if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
0944         mt9v111->pending = true;
0945 
0946     dev_dbg(mt9v111->dev, "%s: mbus_code: %x - (%ux%u)\n",
0947         __func__, __fmt->code, __fmt->width, __fmt->height);
0948 
0949 done:
0950     format->format = *__fmt;
0951 
0952     mutex_unlock(&mt9v111->stream_mutex);
0953 
0954     return 0;
0955 }
0956 
0957 static int mt9v111_init_cfg(struct v4l2_subdev *subdev,
0958                 struct v4l2_subdev_state *sd_state)
0959 {
0960     sd_state->pads->try_fmt = mt9v111_def_fmt;
0961 
0962     return 0;
0963 }
0964 
0965 static const struct v4l2_subdev_core_ops mt9v111_core_ops = {
0966     .s_power        = mt9v111_s_power,
0967 };
0968 
0969 static const struct v4l2_subdev_video_ops mt9v111_video_ops = {
0970     .s_stream       = mt9v111_s_stream,
0971     .s_frame_interval   = mt9v111_s_frame_interval,
0972     .g_frame_interval   = mt9v111_g_frame_interval,
0973 };
0974 
0975 static const struct v4l2_subdev_pad_ops mt9v111_pad_ops = {
0976     .init_cfg       = mt9v111_init_cfg,
0977     .enum_mbus_code     = mt9v111_enum_mbus_code,
0978     .enum_frame_size    = mt9v111_enum_frame_size,
0979     .enum_frame_interval    = mt9v111_enum_frame_interval,
0980     .get_fmt        = mt9v111_get_format,
0981     .set_fmt        = mt9v111_set_format,
0982 };
0983 
0984 static const struct v4l2_subdev_ops mt9v111_ops = {
0985     .core   = &mt9v111_core_ops,
0986     .video  = &mt9v111_video_ops,
0987     .pad    = &mt9v111_pad_ops,
0988 };
0989 
0990 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
0991 static const struct media_entity_operations mt9v111_subdev_entity_ops = {
0992     .link_validate = v4l2_subdev_link_validate,
0993 };
0994 #endif
0995 
0996 /* --- V4L2 ctrl --- */
0997 static int mt9v111_s_ctrl(struct v4l2_ctrl *ctrl)
0998 {
0999     struct mt9v111_dev *mt9v111 = container_of(ctrl->handler,
1000                            struct mt9v111_dev,
1001                            ctrls);
1002     int ret;
1003 
1004     mutex_lock(&mt9v111->pwr_mutex);
1005     /*
1006      * If sensor is powered down, just cache new control values,
1007      * no actual register access.
1008      */
1009     if (!mt9v111->pwr_count) {
1010         mt9v111->pending = true;
1011         mutex_unlock(&mt9v111->pwr_mutex);
1012         return 0;
1013     }
1014     mutex_unlock(&mt9v111->pwr_mutex);
1015 
1016     /*
1017      * Flickering control gets disabled if both auto exp and auto awb
1018      * are disabled too. If any of the two is enabled, enable it.
1019      *
1020      * Disabling flickering when ae and awb are off allows a more precise
1021      * control of the programmed frame rate.
1022      */
1023     if (mt9v111->auto_exp->is_new || mt9v111->auto_awb->is_new) {
1024         if (mt9v111->auto_exp->val == V4L2_EXPOSURE_MANUAL &&
1025             mt9v111->auto_awb->val == V4L2_WHITE_BALANCE_MANUAL)
1026             ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1027                          MT9V111_IFP_R08_OUTFMT_CTRL,
1028                          MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
1029                          0);
1030         else
1031             ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1032                          MT9V111_IFP_R08_OUTFMT_CTRL,
1033                          MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
1034                          1);
1035         if (ret)
1036             return ret;
1037     }
1038 
1039     ret = -EINVAL;
1040     switch (ctrl->id) {
1041     case V4L2_CID_AUTO_WHITE_BALANCE:
1042         ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1043                      MT9V111_IFP_R06_OPMODE_CTRL,
1044                      MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN,
1045                      ctrl->val == V4L2_WHITE_BALANCE_AUTO ?
1046                      MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN : 0);
1047         break;
1048     case V4L2_CID_EXPOSURE_AUTO:
1049         ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1050                      MT9V111_IFP_R06_OPMODE_CTRL,
1051                      MT9V111_IFP_R06_OPMODE_CTRL_AE_EN,
1052                      ctrl->val == V4L2_EXPOSURE_AUTO ?
1053                      MT9V111_IFP_R06_OPMODE_CTRL_AE_EN : 0);
1054         break;
1055     case V4L2_CID_HBLANK:
1056         ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
1057                      MT9V111_CORE_R05_HBLANK,
1058                      MT9V111_CORE_R05_MAX_HBLANK,
1059                      mt9v111->hblank->val);
1060         break;
1061     case V4L2_CID_VBLANK:
1062         ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
1063                      MT9V111_CORE_R06_VBLANK,
1064                      MT9V111_CORE_R06_MAX_VBLANK,
1065                      mt9v111->vblank->val);
1066         break;
1067     }
1068 
1069     return ret;
1070 }
1071 
1072 static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = {
1073     .s_ctrl = mt9v111_s_ctrl,
1074 };
1075 
1076 static int mt9v111_chip_probe(struct mt9v111_dev *mt9v111)
1077 {
1078     int ret;
1079     u16 val;
1080 
1081     ret = __mt9v111_power_on(&mt9v111->sd);
1082     if (ret)
1083         return ret;
1084 
1085     ret = mt9v111_read(mt9v111->client, MT9V111_R01_CORE,
1086                MT9V111_CORE_RFF_CHIP_VER, &val);
1087     if (ret)
1088         goto power_off;
1089 
1090     if ((val >> 8) != MT9V111_CHIP_ID_HIGH &&
1091         (val & 0xff) != MT9V111_CHIP_ID_LOW) {
1092         dev_err(mt9v111->dev,
1093             "Unable to identify MT9V111 chip: 0x%2x%2x\n",
1094             val >> 8, val & 0xff);
1095         ret = -EIO;
1096         goto power_off;
1097     }
1098 
1099     dev_dbg(mt9v111->dev, "Chip identified: 0x%2x%2x\n",
1100         val >> 8, val & 0xff);
1101 
1102 power_off:
1103     __mt9v111_power_off(&mt9v111->sd);
1104 
1105     return ret;
1106 }
1107 
1108 static int mt9v111_probe(struct i2c_client *client)
1109 {
1110     struct mt9v111_dev *mt9v111;
1111     struct v4l2_fract tpf;
1112     int ret;
1113 
1114     mt9v111 = devm_kzalloc(&client->dev, sizeof(*mt9v111), GFP_KERNEL);
1115     if (!mt9v111)
1116         return -ENOMEM;
1117 
1118     mt9v111->dev = &client->dev;
1119     mt9v111->client = client;
1120 
1121     mt9v111->clk = devm_clk_get(&client->dev, NULL);
1122     if (IS_ERR(mt9v111->clk))
1123         return PTR_ERR(mt9v111->clk);
1124 
1125     mt9v111->sysclk = clk_get_rate(mt9v111->clk);
1126     if (mt9v111->sysclk > MT9V111_MAX_CLKIN)
1127         return -EINVAL;
1128 
1129     mt9v111->oe = devm_gpiod_get_optional(&client->dev, "enable",
1130                           GPIOD_OUT_LOW);
1131     if (IS_ERR(mt9v111->oe)) {
1132         dev_err(&client->dev, "Unable to get GPIO \"enable\": %ld\n",
1133             PTR_ERR(mt9v111->oe));
1134         return PTR_ERR(mt9v111->oe);
1135     }
1136 
1137     mt9v111->standby = devm_gpiod_get_optional(&client->dev, "standby",
1138                            GPIOD_OUT_HIGH);
1139     if (IS_ERR(mt9v111->standby)) {
1140         dev_err(&client->dev, "Unable to get GPIO \"standby\": %ld\n",
1141             PTR_ERR(mt9v111->standby));
1142         return PTR_ERR(mt9v111->standby);
1143     }
1144 
1145     mt9v111->reset = devm_gpiod_get_optional(&client->dev, "reset",
1146                          GPIOD_OUT_LOW);
1147     if (IS_ERR(mt9v111->reset)) {
1148         dev_err(&client->dev, "Unable to get GPIO \"reset\": %ld\n",
1149             PTR_ERR(mt9v111->reset));
1150         return PTR_ERR(mt9v111->reset);
1151     }
1152 
1153     mutex_init(&mt9v111->pwr_mutex);
1154     mutex_init(&mt9v111->stream_mutex);
1155 
1156     v4l2_ctrl_handler_init(&mt9v111->ctrls, 5);
1157 
1158     mt9v111->auto_awb = v4l2_ctrl_new_std(&mt9v111->ctrls,
1159                           &mt9v111_ctrl_ops,
1160                           V4L2_CID_AUTO_WHITE_BALANCE,
1161                           0, 1, 1,
1162                           V4L2_WHITE_BALANCE_AUTO);
1163     mt9v111->auto_exp = v4l2_ctrl_new_std_menu(&mt9v111->ctrls,
1164                            &mt9v111_ctrl_ops,
1165                            V4L2_CID_EXPOSURE_AUTO,
1166                            V4L2_EXPOSURE_MANUAL,
1167                            0, V4L2_EXPOSURE_AUTO);
1168     mt9v111->hblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1169                         V4L2_CID_HBLANK,
1170                         MT9V111_CORE_R05_MIN_HBLANK,
1171                         MT9V111_CORE_R05_MAX_HBLANK, 1,
1172                         MT9V111_CORE_R05_DEF_HBLANK);
1173     mt9v111->vblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1174                         V4L2_CID_VBLANK,
1175                         MT9V111_CORE_R06_MIN_VBLANK,
1176                         MT9V111_CORE_R06_MAX_VBLANK, 1,
1177                         MT9V111_CORE_R06_DEF_VBLANK);
1178 
1179     /* PIXEL_RATE is fixed: just expose it to user space. */
1180     v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1181               V4L2_CID_PIXEL_RATE, 0,
1182               DIV_ROUND_CLOSEST(mt9v111->sysclk, 2), 1,
1183               DIV_ROUND_CLOSEST(mt9v111->sysclk, 2));
1184 
1185     if (mt9v111->ctrls.error) {
1186         ret = mt9v111->ctrls.error;
1187         goto error_free_ctrls;
1188     }
1189     mt9v111->sd.ctrl_handler = &mt9v111->ctrls;
1190 
1191     /* Start with default configuration: 640x480 UYVY. */
1192     mt9v111->fmt    = mt9v111_def_fmt;
1193 
1194     /* Re-calculate blankings for 640x480@15fps. */
1195     mt9v111->fps        = 15;
1196     tpf.numerator       = 1;
1197     tpf.denominator     = mt9v111->fps;
1198     mt9v111_calc_frame_rate(mt9v111, &tpf);
1199 
1200     mt9v111->pwr_count  = 0;
1201     mt9v111->addr_space = MT9V111_R01_IFP;
1202     mt9v111->pending    = true;
1203 
1204     v4l2_i2c_subdev_init(&mt9v111->sd, client, &mt9v111_ops);
1205 
1206 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1207     mt9v111->sd.flags   |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1208     mt9v111->sd.entity.ops  = &mt9v111_subdev_entity_ops;
1209     mt9v111->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1210 
1211     mt9v111->pad.flags  = MEDIA_PAD_FL_SOURCE;
1212     ret = media_entity_pads_init(&mt9v111->sd.entity, 1, &mt9v111->pad);
1213     if (ret)
1214         goto error_free_entity;
1215 #endif
1216 
1217     ret = mt9v111_chip_probe(mt9v111);
1218     if (ret)
1219         goto error_free_entity;
1220 
1221     ret = v4l2_async_register_subdev(&mt9v111->sd);
1222     if (ret)
1223         goto error_free_entity;
1224 
1225     return 0;
1226 
1227 error_free_entity:
1228 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1229     media_entity_cleanup(&mt9v111->sd.entity);
1230 #endif
1231 
1232 error_free_ctrls:
1233     v4l2_ctrl_handler_free(&mt9v111->ctrls);
1234 
1235     mutex_destroy(&mt9v111->pwr_mutex);
1236     mutex_destroy(&mt9v111->stream_mutex);
1237 
1238     return ret;
1239 }
1240 
1241 static int mt9v111_remove(struct i2c_client *client)
1242 {
1243     struct v4l2_subdev *sd = i2c_get_clientdata(client);
1244     struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
1245 
1246     v4l2_async_unregister_subdev(sd);
1247 
1248 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1249     media_entity_cleanup(&sd->entity);
1250 #endif
1251 
1252     v4l2_ctrl_handler_free(&mt9v111->ctrls);
1253 
1254     mutex_destroy(&mt9v111->pwr_mutex);
1255     mutex_destroy(&mt9v111->stream_mutex);
1256 
1257     return 0;
1258 }
1259 
1260 static const struct of_device_id mt9v111_of_match[] = {
1261     { .compatible = "aptina,mt9v111", },
1262     { /* sentinel */ },
1263 };
1264 
1265 static struct i2c_driver mt9v111_driver = {
1266     .driver = {
1267         .name = "mt9v111",
1268         .of_match_table = mt9v111_of_match,
1269     },
1270     .probe_new  = mt9v111_probe,
1271     .remove     = mt9v111_remove,
1272 };
1273 
1274 module_i2c_driver(mt9v111_driver);
1275 
1276 MODULE_DESCRIPTION("V4L2 sensor driver for Aptina MT9V111");
1277 MODULE_AUTHOR("Jacopo Mondi <jacopo@jmondi.org>");
1278 MODULE_LICENSE("GPL v2");
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!