Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * Driver for MT9P031 CMOS Image Sensor from Aptina
0004  *
0005  * Copyright (C) 2011, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
0006  * Copyright (C) 2011, Javier Martin <javier.martin@vista-silicon.com>
0007  * Copyright (C) 2011, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
0008  *
0009  * Based on the MT9V032 driver and Bastian Hecht's code.
0010  */
0011 
0012 #include <linux/clk.h>
0013 #include <linux/delay.h>
0014 #include <linux/device.h>
0015 #include <linux/gpio/consumer.h>
0016 #include <linux/i2c.h>
0017 #include <linux/log2.h>
0018 #include <linux/module.h>
0019 #include <linux/of.h>
0020 #include <linux/of_graph.h>
0021 #include <linux/pm.h>
0022 #include <linux/regulator/consumer.h>
0023 #include <linux/slab.h>
0024 #include <linux/videodev2.h>
0025 
0026 #include <media/i2c/mt9p031.h>
0027 #include <media/v4l2-async.h>
0028 #include <media/v4l2-ctrls.h>
0029 #include <media/v4l2-device.h>
0030 #include <media/v4l2-fwnode.h>
0031 #include <media/v4l2-subdev.h>
0032 
0033 #include "aptina-pll.h"
0034 
0035 #define MT9P031_PIXEL_ARRAY_WIDTH           2752
0036 #define MT9P031_PIXEL_ARRAY_HEIGHT          2004
0037 
0038 #define MT9P031_CHIP_VERSION                0x00
0039 #define     MT9P031_CHIP_VERSION_VALUE      0x1801
0040 #define MT9P031_ROW_START               0x01
0041 #define     MT9P031_ROW_START_MIN           0
0042 #define     MT9P031_ROW_START_MAX           2004
0043 #define     MT9P031_ROW_START_DEF           54
0044 #define MT9P031_COLUMN_START                0x02
0045 #define     MT9P031_COLUMN_START_MIN        0
0046 #define     MT9P031_COLUMN_START_MAX        2750
0047 #define     MT9P031_COLUMN_START_DEF        16
0048 #define MT9P031_WINDOW_HEIGHT               0x03
0049 #define     MT9P031_WINDOW_HEIGHT_MIN       2
0050 #define     MT9P031_WINDOW_HEIGHT_MAX       2006
0051 #define     MT9P031_WINDOW_HEIGHT_DEF       1944
0052 #define MT9P031_WINDOW_WIDTH                0x04
0053 #define     MT9P031_WINDOW_WIDTH_MIN        2
0054 #define     MT9P031_WINDOW_WIDTH_MAX        2752
0055 #define     MT9P031_WINDOW_WIDTH_DEF        2592
0056 #define MT9P031_HORIZONTAL_BLANK            0x05
0057 #define     MT9P031_HORIZONTAL_BLANK_MIN        0
0058 #define     MT9P031_HORIZONTAL_BLANK_MAX        4095
0059 #define MT9P031_VERTICAL_BLANK              0x06
0060 #define     MT9P031_VERTICAL_BLANK_MIN      1
0061 #define     MT9P031_VERTICAL_BLANK_MAX      4096
0062 #define     MT9P031_VERTICAL_BLANK_DEF      26
0063 #define MT9P031_OUTPUT_CONTROL              0x07
0064 #define     MT9P031_OUTPUT_CONTROL_CEN      2
0065 #define     MT9P031_OUTPUT_CONTROL_SYN      1
0066 #define     MT9P031_OUTPUT_CONTROL_DEF      0x1f82
0067 #define MT9P031_SHUTTER_WIDTH_UPPER         0x08
0068 #define MT9P031_SHUTTER_WIDTH_LOWER         0x09
0069 #define     MT9P031_SHUTTER_WIDTH_MIN       1
0070 #define     MT9P031_SHUTTER_WIDTH_MAX       1048575
0071 #define     MT9P031_SHUTTER_WIDTH_DEF       1943
0072 #define MT9P031_PLL_CONTROL             0x10
0073 #define     MT9P031_PLL_CONTROL_PWROFF      0x0050
0074 #define     MT9P031_PLL_CONTROL_PWRON       0x0051
0075 #define     MT9P031_PLL_CONTROL_USEPLL      0x0052
0076 #define MT9P031_PLL_CONFIG_1                0x11
0077 #define MT9P031_PLL_CONFIG_2                0x12
0078 #define MT9P031_PIXEL_CLOCK_CONTROL         0x0a
0079 #define     MT9P031_PIXEL_CLOCK_INVERT      BIT(15)
0080 #define     MT9P031_PIXEL_CLOCK_SHIFT(n)        ((n) << 8)
0081 #define     MT9P031_PIXEL_CLOCK_DIVIDE(n)       ((n) << 0)
0082 #define MT9P031_RESTART                 0x0b
0083 #define     MT9P031_FRAME_PAUSE_RESTART     BIT(1)
0084 #define     MT9P031_FRAME_RESTART           BIT(0)
0085 #define MT9P031_SHUTTER_DELAY               0x0c
0086 #define MT9P031_RST                 0x0d
0087 #define     MT9P031_RST_ENABLE          BIT(0)
0088 #define MT9P031_READ_MODE_1             0x1e
0089 #define MT9P031_READ_MODE_2             0x20
0090 #define     MT9P031_READ_MODE_2_ROW_MIR     BIT(15)
0091 #define     MT9P031_READ_MODE_2_COL_MIR     BIT(14)
0092 #define     MT9P031_READ_MODE_2_ROW_BLC     BIT(6)
0093 #define MT9P031_ROW_ADDRESS_MODE            0x22
0094 #define MT9P031_COLUMN_ADDRESS_MODE         0x23
0095 #define MT9P031_GLOBAL_GAIN             0x35
0096 #define     MT9P031_GLOBAL_GAIN_MIN         8
0097 #define     MT9P031_GLOBAL_GAIN_MAX         1024
0098 #define     MT9P031_GLOBAL_GAIN_DEF         8
0099 #define     MT9P031_GLOBAL_GAIN_MULT        BIT(6)
0100 #define MT9P031_ROW_BLACK_TARGET            0x49
0101 #define MT9P031_ROW_BLACK_DEF_OFFSET            0x4b
0102 #define MT9P031_GREEN1_OFFSET               0x60
0103 #define MT9P031_GREEN2_OFFSET               0x61
0104 #define MT9P031_BLACK_LEVEL_CALIBRATION         0x62
0105 #define     MT9P031_BLC_MANUAL_BLC          BIT(0)
0106 #define MT9P031_RED_OFFSET              0x63
0107 #define MT9P031_BLUE_OFFSET             0x64
0108 #define MT9P031_TEST_PATTERN                0xa0
0109 #define     MT9P031_TEST_PATTERN_SHIFT      3
0110 #define     MT9P031_TEST_PATTERN_ENABLE     BIT(0)
0111 #define MT9P031_TEST_PATTERN_GREEN          0xa1
0112 #define MT9P031_TEST_PATTERN_RED            0xa2
0113 #define MT9P031_TEST_PATTERN_BLUE           0xa3
0114 
0115 enum mt9p031_model {
0116     MT9P031_MODEL_COLOR,
0117     MT9P031_MODEL_MONOCHROME,
0118 };
0119 
0120 struct mt9p031 {
0121     struct v4l2_subdev subdev;
0122     struct media_pad pad;
0123     struct v4l2_rect crop;  /* Sensor window */
0124     struct v4l2_mbus_framefmt format;
0125     struct mt9p031_platform_data *pdata;
0126     struct mutex power_lock; /* lock to protect power_count */
0127     int power_count;
0128 
0129     struct clk *clk;
0130     struct regulator_bulk_data regulators[3];
0131 
0132     enum mt9p031_model model;
0133     struct aptina_pll pll;
0134     unsigned int clk_div;
0135     bool use_pll;
0136     struct gpio_desc *reset;
0137 
0138     struct v4l2_ctrl_handler ctrls;
0139     struct v4l2_ctrl *blc_auto;
0140     struct v4l2_ctrl *blc_offset;
0141 
0142     /* Registers cache */
0143     u16 output_control;
0144     u16 mode2;
0145 };
0146 
0147 static struct mt9p031 *to_mt9p031(struct v4l2_subdev *sd)
0148 {
0149     return container_of(sd, struct mt9p031, subdev);
0150 }
0151 
0152 static int mt9p031_read(struct i2c_client *client, u8 reg)
0153 {
0154     return i2c_smbus_read_word_swapped(client, reg);
0155 }
0156 
0157 static int mt9p031_write(struct i2c_client *client, u8 reg, u16 data)
0158 {
0159     return i2c_smbus_write_word_swapped(client, reg, data);
0160 }
0161 
0162 static int mt9p031_set_output_control(struct mt9p031 *mt9p031, u16 clear,
0163                       u16 set)
0164 {
0165     struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
0166     u16 value = (mt9p031->output_control & ~clear) | set;
0167     int ret;
0168 
0169     ret = mt9p031_write(client, MT9P031_OUTPUT_CONTROL, value);
0170     if (ret < 0)
0171         return ret;
0172 
0173     mt9p031->output_control = value;
0174     return 0;
0175 }
0176 
0177 static int mt9p031_set_mode2(struct mt9p031 *mt9p031, u16 clear, u16 set)
0178 {
0179     struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
0180     u16 value = (mt9p031->mode2 & ~clear) | set;
0181     int ret;
0182 
0183     ret = mt9p031_write(client, MT9P031_READ_MODE_2, value);
0184     if (ret < 0)
0185         return ret;
0186 
0187     mt9p031->mode2 = value;
0188     return 0;
0189 }
0190 
0191 static int mt9p031_reset(struct mt9p031 *mt9p031)
0192 {
0193     struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
0194     int ret;
0195 
0196     /* Disable chip output, synchronous option update */
0197     ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_ENABLE);
0198     if (ret < 0)
0199         return ret;
0200     ret = mt9p031_write(client, MT9P031_RST, 0);
0201     if (ret < 0)
0202         return ret;
0203 
0204     ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
0205                 MT9P031_PIXEL_CLOCK_DIVIDE(mt9p031->clk_div));
0206     if (ret < 0)
0207         return ret;
0208 
0209     return mt9p031_set_output_control(mt9p031, MT9P031_OUTPUT_CONTROL_CEN,
0210                       0);
0211 }
0212 
0213 static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
0214 {
0215     static const struct aptina_pll_limits limits = {
0216         .ext_clock_min = 6000000,
0217         .ext_clock_max = 27000000,
0218         .int_clock_min = 2000000,
0219         .int_clock_max = 13500000,
0220         .out_clock_min = 180000000,
0221         .out_clock_max = 360000000,
0222         .pix_clock_max = 96000000,
0223         .n_min = 1,
0224         .n_max = 64,
0225         .m_min = 16,
0226         .m_max = 255,
0227         .p1_min = 1,
0228         .p1_max = 128,
0229     };
0230 
0231     struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
0232     struct mt9p031_platform_data *pdata = mt9p031->pdata;
0233     unsigned long ext_freq;
0234     int ret;
0235 
0236     mt9p031->clk = devm_clk_get(&client->dev, NULL);
0237     if (IS_ERR(mt9p031->clk))
0238         return PTR_ERR(mt9p031->clk);
0239 
0240     ret = clk_set_rate(mt9p031->clk, pdata->ext_freq);
0241     if (ret < 0)
0242         return ret;
0243 
0244     ext_freq = clk_get_rate(mt9p031->clk);
0245 
0246     /* If the external clock frequency is out of bounds for the PLL use the
0247      * pixel clock divider only and disable the PLL.
0248      */
0249     if (ext_freq > limits.ext_clock_max) {
0250         unsigned int div;
0251 
0252         div = DIV_ROUND_UP(ext_freq, pdata->target_freq);
0253         div = roundup_pow_of_two(div) / 2;
0254 
0255         mt9p031->clk_div = min_t(unsigned int, div, 64);
0256         mt9p031->use_pll = false;
0257 
0258         return 0;
0259     }
0260 
0261     mt9p031->pll.ext_clock = ext_freq;
0262     mt9p031->pll.pix_clock = pdata->target_freq;
0263     mt9p031->use_pll = true;
0264 
0265     return aptina_pll_calculate(&client->dev, &limits, &mt9p031->pll);
0266 }
0267 
0268 static int mt9p031_pll_enable(struct mt9p031 *mt9p031)
0269 {
0270     struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
0271     int ret;
0272 
0273     if (!mt9p031->use_pll)
0274         return 0;
0275 
0276     ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
0277                 MT9P031_PLL_CONTROL_PWRON);
0278     if (ret < 0)
0279         return ret;
0280 
0281     ret = mt9p031_write(client, MT9P031_PLL_CONFIG_1,
0282                 (mt9p031->pll.m << 8) | (mt9p031->pll.n - 1));
0283     if (ret < 0)
0284         return ret;
0285 
0286     ret = mt9p031_write(client, MT9P031_PLL_CONFIG_2, mt9p031->pll.p1 - 1);
0287     if (ret < 0)
0288         return ret;
0289 
0290     usleep_range(1000, 2000);
0291     ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
0292                 MT9P031_PLL_CONTROL_PWRON |
0293                 MT9P031_PLL_CONTROL_USEPLL);
0294     return ret;
0295 }
0296 
0297 static inline int mt9p031_pll_disable(struct mt9p031 *mt9p031)
0298 {
0299     struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
0300 
0301     if (!mt9p031->use_pll)
0302         return 0;
0303 
0304     return mt9p031_write(client, MT9P031_PLL_CONTROL,
0305                  MT9P031_PLL_CONTROL_PWROFF);
0306 }
0307 
0308 static int mt9p031_power_on(struct mt9p031 *mt9p031)
0309 {
0310     int ret;
0311 
0312     /* Ensure RESET_BAR is active */
0313     if (mt9p031->reset) {
0314         gpiod_set_value(mt9p031->reset, 1);
0315         usleep_range(1000, 2000);
0316     }
0317 
0318     /* Bring up the supplies */
0319     ret = regulator_bulk_enable(ARRAY_SIZE(mt9p031->regulators),
0320                    mt9p031->regulators);
0321     if (ret < 0)
0322         return ret;
0323 
0324     /* Enable clock */
0325     if (mt9p031->clk) {
0326         ret = clk_prepare_enable(mt9p031->clk);
0327         if (ret) {
0328             regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
0329                            mt9p031->regulators);
0330             return ret;
0331         }
0332     }
0333 
0334     /* Now RESET_BAR must be high */
0335     if (mt9p031->reset) {
0336         gpiod_set_value(mt9p031->reset, 0);
0337         usleep_range(1000, 2000);
0338     }
0339 
0340     return 0;
0341 }
0342 
0343 static void mt9p031_power_off(struct mt9p031 *mt9p031)
0344 {
0345     if (mt9p031->reset) {
0346         gpiod_set_value(mt9p031->reset, 1);
0347         usleep_range(1000, 2000);
0348     }
0349 
0350     regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
0351                    mt9p031->regulators);
0352 
0353     clk_disable_unprepare(mt9p031->clk);
0354 }
0355 
0356 static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
0357 {
0358     struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
0359     int ret;
0360 
0361     if (!on) {
0362         mt9p031_power_off(mt9p031);
0363         return 0;
0364     }
0365 
0366     ret = mt9p031_power_on(mt9p031);
0367     if (ret < 0)
0368         return ret;
0369 
0370     ret = mt9p031_reset(mt9p031);
0371     if (ret < 0) {
0372         dev_err(&client->dev, "Failed to reset the camera\n");
0373         return ret;
0374     }
0375 
0376     /* Configure the pixel clock polarity */
0377     if (mt9p031->pdata && mt9p031->pdata->pixclk_pol) {
0378         ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
0379                 MT9P031_PIXEL_CLOCK_INVERT);
0380         if (ret < 0)
0381             return ret;
0382     }
0383 
0384     return v4l2_ctrl_handler_setup(&mt9p031->ctrls);
0385 }
0386 
0387 /* -----------------------------------------------------------------------------
0388  * V4L2 subdev video operations
0389  */
0390 
0391 static int mt9p031_set_params(struct mt9p031 *mt9p031)
0392 {
0393     struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
0394     struct v4l2_mbus_framefmt *format = &mt9p031->format;
0395     const struct v4l2_rect *crop = &mt9p031->crop;
0396     unsigned int hblank;
0397     unsigned int vblank;
0398     unsigned int xskip;
0399     unsigned int yskip;
0400     unsigned int xbin;
0401     unsigned int ybin;
0402     int ret;
0403 
0404     /* Windows position and size.
0405      *
0406      * TODO: Make sure the start coordinates and window size match the
0407      * skipping, binning and mirroring (see description of registers 2 and 4
0408      * in table 13, and Binning section on page 41).
0409      */
0410     ret = mt9p031_write(client, MT9P031_COLUMN_START, crop->left);
0411     if (ret < 0)
0412         return ret;
0413     ret = mt9p031_write(client, MT9P031_ROW_START, crop->top);
0414     if (ret < 0)
0415         return ret;
0416     ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, crop->width - 1);
0417     if (ret < 0)
0418         return ret;
0419     ret = mt9p031_write(client, MT9P031_WINDOW_HEIGHT, crop->height - 1);
0420     if (ret < 0)
0421         return ret;
0422 
0423     /* Row and column binning and skipping. Use the maximum binning value
0424      * compatible with the skipping settings.
0425      */
0426     xskip = DIV_ROUND_CLOSEST(crop->width, format->width);
0427     yskip = DIV_ROUND_CLOSEST(crop->height, format->height);
0428     xbin = 1 << (ffs(xskip) - 1);
0429     ybin = 1 << (ffs(yskip) - 1);
0430 
0431     ret = mt9p031_write(client, MT9P031_COLUMN_ADDRESS_MODE,
0432                 ((xbin - 1) << 4) | (xskip - 1));
0433     if (ret < 0)
0434         return ret;
0435     ret = mt9p031_write(client, MT9P031_ROW_ADDRESS_MODE,
0436                 ((ybin - 1) << 4) | (yskip - 1));
0437     if (ret < 0)
0438         return ret;
0439 
0440     /* Blanking - use minimum value for horizontal blanking and default
0441      * value for vertical blanking.
0442      */
0443     hblank = 346 * ybin + 64 + (80 >> min_t(unsigned int, xbin, 3));
0444     vblank = MT9P031_VERTICAL_BLANK_DEF;
0445 
0446     ret = mt9p031_write(client, MT9P031_HORIZONTAL_BLANK, hblank - 1);
0447     if (ret < 0)
0448         return ret;
0449     ret = mt9p031_write(client, MT9P031_VERTICAL_BLANK, vblank - 1);
0450     if (ret < 0)
0451         return ret;
0452 
0453     return ret;
0454 }
0455 
0456 static int mt9p031_s_stream(struct v4l2_subdev *subdev, int enable)
0457 {
0458     struct mt9p031 *mt9p031 = to_mt9p031(subdev);
0459     struct i2c_client *client = v4l2_get_subdevdata(subdev);
0460     int val;
0461     int ret;
0462 
0463     if (!enable) {
0464         /* enable pause restart */
0465         val = MT9P031_FRAME_PAUSE_RESTART;
0466         ret = mt9p031_write(client, MT9P031_RESTART, val);
0467         if (ret < 0)
0468             return ret;
0469 
0470         /* enable restart + keep pause restart set */
0471         val |= MT9P031_FRAME_RESTART;
0472         ret = mt9p031_write(client, MT9P031_RESTART, val);
0473         if (ret < 0)
0474             return ret;
0475 
0476         /* Stop sensor readout */
0477         ret = mt9p031_set_output_control(mt9p031,
0478                          MT9P031_OUTPUT_CONTROL_CEN, 0);
0479         if (ret < 0)
0480             return ret;
0481 
0482         return mt9p031_pll_disable(mt9p031);
0483     }
0484 
0485     ret = mt9p031_set_params(mt9p031);
0486     if (ret < 0)
0487         return ret;
0488 
0489     /* Switch to master "normal" mode */
0490     ret = mt9p031_set_output_control(mt9p031, 0,
0491                      MT9P031_OUTPUT_CONTROL_CEN);
0492     if (ret < 0)
0493         return ret;
0494 
0495     /*
0496      * - clear pause restart
0497      * - don't clear restart as clearing restart manually can cause
0498      *   undefined behavior
0499      */
0500     val = MT9P031_FRAME_RESTART;
0501     ret = mt9p031_write(client, MT9P031_RESTART, val);
0502     if (ret < 0)
0503         return ret;
0504 
0505     return mt9p031_pll_enable(mt9p031);
0506 }
0507 
0508 static int mt9p031_enum_mbus_code(struct v4l2_subdev *subdev,
0509                   struct v4l2_subdev_state *sd_state,
0510                   struct v4l2_subdev_mbus_code_enum *code)
0511 {
0512     struct mt9p031 *mt9p031 = to_mt9p031(subdev);
0513 
0514     if (code->pad || code->index)
0515         return -EINVAL;
0516 
0517     code->code = mt9p031->format.code;
0518     return 0;
0519 }
0520 
0521 static int mt9p031_enum_frame_size(struct v4l2_subdev *subdev,
0522                    struct v4l2_subdev_state *sd_state,
0523                    struct v4l2_subdev_frame_size_enum *fse)
0524 {
0525     struct mt9p031 *mt9p031 = to_mt9p031(subdev);
0526 
0527     if (fse->index >= 8 || fse->code != mt9p031->format.code)
0528         return -EINVAL;
0529 
0530     fse->min_width = MT9P031_WINDOW_WIDTH_DEF
0531                / min_t(unsigned int, 7, fse->index + 1);
0532     fse->max_width = fse->min_width;
0533     fse->min_height = MT9P031_WINDOW_HEIGHT_DEF / (fse->index + 1);
0534     fse->max_height = fse->min_height;
0535 
0536     return 0;
0537 }
0538 
0539 static struct v4l2_mbus_framefmt *
0540 __mt9p031_get_pad_format(struct mt9p031 *mt9p031,
0541              struct v4l2_subdev_state *sd_state,
0542              unsigned int pad, u32 which)
0543 {
0544     switch (which) {
0545     case V4L2_SUBDEV_FORMAT_TRY:
0546         return v4l2_subdev_get_try_format(&mt9p031->subdev, sd_state,
0547                           pad);
0548     case V4L2_SUBDEV_FORMAT_ACTIVE:
0549         return &mt9p031->format;
0550     default:
0551         return NULL;
0552     }
0553 }
0554 
0555 static struct v4l2_rect *
0556 __mt9p031_get_pad_crop(struct mt9p031 *mt9p031,
0557                struct v4l2_subdev_state *sd_state,
0558                unsigned int pad, u32 which)
0559 {
0560     switch (which) {
0561     case V4L2_SUBDEV_FORMAT_TRY:
0562         return v4l2_subdev_get_try_crop(&mt9p031->subdev, sd_state,
0563                         pad);
0564     case V4L2_SUBDEV_FORMAT_ACTIVE:
0565         return &mt9p031->crop;
0566     default:
0567         return NULL;
0568     }
0569 }
0570 
0571 static int mt9p031_get_format(struct v4l2_subdev *subdev,
0572                   struct v4l2_subdev_state *sd_state,
0573                   struct v4l2_subdev_format *fmt)
0574 {
0575     struct mt9p031 *mt9p031 = to_mt9p031(subdev);
0576 
0577     fmt->format = *__mt9p031_get_pad_format(mt9p031, sd_state, fmt->pad,
0578                         fmt->which);
0579     return 0;
0580 }
0581 
0582 static int mt9p031_set_format(struct v4l2_subdev *subdev,
0583                   struct v4l2_subdev_state *sd_state,
0584                   struct v4l2_subdev_format *format)
0585 {
0586     struct mt9p031 *mt9p031 = to_mt9p031(subdev);
0587     struct v4l2_mbus_framefmt *__format;
0588     struct v4l2_rect *__crop;
0589     unsigned int width;
0590     unsigned int height;
0591     unsigned int hratio;
0592     unsigned int vratio;
0593 
0594     __crop = __mt9p031_get_pad_crop(mt9p031, sd_state, format->pad,
0595                     format->which);
0596 
0597     /* Clamp the width and height to avoid dividing by zero. */
0598     width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
0599             max_t(unsigned int, __crop->width / 7,
0600                   MT9P031_WINDOW_WIDTH_MIN),
0601             __crop->width);
0602     height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
0603              max_t(unsigned int, __crop->height / 8,
0604                    MT9P031_WINDOW_HEIGHT_MIN),
0605              __crop->height);
0606 
0607     hratio = DIV_ROUND_CLOSEST(__crop->width, width);
0608     vratio = DIV_ROUND_CLOSEST(__crop->height, height);
0609 
0610     __format = __mt9p031_get_pad_format(mt9p031, sd_state, format->pad,
0611                         format->which);
0612     __format->width = __crop->width / hratio;
0613     __format->height = __crop->height / vratio;
0614 
0615     format->format = *__format;
0616 
0617     return 0;
0618 }
0619 
0620 static int mt9p031_get_selection(struct v4l2_subdev *subdev,
0621                  struct v4l2_subdev_state *sd_state,
0622                  struct v4l2_subdev_selection *sel)
0623 {
0624     struct mt9p031 *mt9p031 = to_mt9p031(subdev);
0625 
0626     switch (sel->target) {
0627     case V4L2_SEL_TGT_CROP_BOUNDS:
0628         sel->r.left = MT9P031_COLUMN_START_MIN;
0629         sel->r.top = MT9P031_ROW_START_MIN;
0630         sel->r.width = MT9P031_WINDOW_WIDTH_MAX;
0631         sel->r.height = MT9P031_WINDOW_HEIGHT_MAX;
0632         return 0;
0633 
0634     case V4L2_SEL_TGT_CROP:
0635         sel->r = *__mt9p031_get_pad_crop(mt9p031, sd_state,
0636                          sel->pad, sel->which);
0637         return 0;
0638 
0639     default:
0640         return -EINVAL;
0641     }
0642 }
0643 
0644 static int mt9p031_set_selection(struct v4l2_subdev *subdev,
0645                  struct v4l2_subdev_state *sd_state,
0646                  struct v4l2_subdev_selection *sel)
0647 {
0648     struct mt9p031 *mt9p031 = to_mt9p031(subdev);
0649     struct v4l2_mbus_framefmt *__format;
0650     struct v4l2_rect *__crop;
0651     struct v4l2_rect rect;
0652 
0653     if (sel->target != V4L2_SEL_TGT_CROP)
0654         return -EINVAL;
0655 
0656     /* Clamp the crop rectangle boundaries and align them to a multiple of 2
0657      * pixels to ensure a GRBG Bayer pattern.
0658      */
0659     rect.left = clamp(ALIGN(sel->r.left, 2), MT9P031_COLUMN_START_MIN,
0660               MT9P031_COLUMN_START_MAX);
0661     rect.top = clamp(ALIGN(sel->r.top, 2), MT9P031_ROW_START_MIN,
0662              MT9P031_ROW_START_MAX);
0663     rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 2),
0664                  MT9P031_WINDOW_WIDTH_MIN,
0665                  MT9P031_WINDOW_WIDTH_MAX);
0666     rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 2),
0667                   MT9P031_WINDOW_HEIGHT_MIN,
0668                   MT9P031_WINDOW_HEIGHT_MAX);
0669 
0670     rect.width = min_t(unsigned int, rect.width,
0671                MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
0672     rect.height = min_t(unsigned int, rect.height,
0673                 MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
0674 
0675     __crop = __mt9p031_get_pad_crop(mt9p031, sd_state, sel->pad,
0676                     sel->which);
0677 
0678     if (rect.width != __crop->width || rect.height != __crop->height) {
0679         /* Reset the output image size if the crop rectangle size has
0680          * been modified.
0681          */
0682         __format = __mt9p031_get_pad_format(mt9p031, sd_state,
0683                             sel->pad,
0684                             sel->which);
0685         __format->width = rect.width;
0686         __format->height = rect.height;
0687     }
0688 
0689     *__crop = rect;
0690     sel->r = rect;
0691 
0692     return 0;
0693 }
0694 
0695 static int mt9p031_init_cfg(struct v4l2_subdev *subdev,
0696                 struct v4l2_subdev_state *sd_state)
0697 {
0698     struct mt9p031 *mt9p031 = to_mt9p031(subdev);
0699     struct v4l2_mbus_framefmt *format;
0700     struct v4l2_rect *crop;
0701     const int which = sd_state == NULL ? V4L2_SUBDEV_FORMAT_ACTIVE :
0702                          V4L2_SUBDEV_FORMAT_TRY;
0703 
0704     crop = __mt9p031_get_pad_crop(mt9p031, sd_state, 0, which);
0705     v4l2_subdev_get_try_crop(subdev, sd_state, 0);
0706     crop->left = MT9P031_COLUMN_START_DEF;
0707     crop->top = MT9P031_ROW_START_DEF;
0708     crop->width = MT9P031_WINDOW_WIDTH_DEF;
0709     crop->height = MT9P031_WINDOW_HEIGHT_DEF;
0710 
0711     format = __mt9p031_get_pad_format(mt9p031, sd_state, 0, which);
0712 
0713     if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
0714         format->code = MEDIA_BUS_FMT_Y12_1X12;
0715     else
0716         format->code = MEDIA_BUS_FMT_SGRBG12_1X12;
0717 
0718     format->width = MT9P031_WINDOW_WIDTH_DEF;
0719     format->height = MT9P031_WINDOW_HEIGHT_DEF;
0720     format->field = V4L2_FIELD_NONE;
0721     format->colorspace = V4L2_COLORSPACE_SRGB;
0722 
0723     return 0;
0724 }
0725 
0726 /* -----------------------------------------------------------------------------
0727  * V4L2 subdev control operations
0728  */
0729 
0730 #define V4L2_CID_BLC_AUTO       (V4L2_CID_USER_BASE | 0x1002)
0731 #define V4L2_CID_BLC_TARGET_LEVEL   (V4L2_CID_USER_BASE | 0x1003)
0732 #define V4L2_CID_BLC_ANALOG_OFFSET  (V4L2_CID_USER_BASE | 0x1004)
0733 #define V4L2_CID_BLC_DIGITAL_OFFSET (V4L2_CID_USER_BASE | 0x1005)
0734 
0735 static int mt9p031_restore_blc(struct mt9p031 *mt9p031)
0736 {
0737     struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
0738     int ret;
0739 
0740     if (mt9p031->blc_auto->cur.val != 0) {
0741         ret = mt9p031_set_mode2(mt9p031, 0,
0742                     MT9P031_READ_MODE_2_ROW_BLC);
0743         if (ret < 0)
0744             return ret;
0745     }
0746 
0747     if (mt9p031->blc_offset->cur.val != 0) {
0748         ret = mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
0749                     mt9p031->blc_offset->cur.val);
0750         if (ret < 0)
0751             return ret;
0752     }
0753 
0754     return 0;
0755 }
0756 
0757 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
0758 {
0759     struct mt9p031 *mt9p031 =
0760             container_of(ctrl->handler, struct mt9p031, ctrls);
0761     struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
0762     u16 data;
0763     int ret;
0764 
0765     if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
0766         return 0;
0767 
0768     switch (ctrl->id) {
0769     case V4L2_CID_EXPOSURE:
0770         ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
0771                     (ctrl->val >> 16) & 0xffff);
0772         if (ret < 0)
0773             return ret;
0774 
0775         return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
0776                      ctrl->val & 0xffff);
0777 
0778     case V4L2_CID_GAIN:
0779         /* Gain is controlled by 2 analog stages and a digital stage.
0780          * Valid values for the 3 stages are
0781          *
0782          * Stage                Min     Max     Step
0783          * ------------------------------------------
0784          * First analog stage   x1      x2      1
0785          * Second analog stage  x1      x4      0.125
0786          * Digital stage        x1      x16     0.125
0787          *
0788          * To minimize noise, the gain stages should be used in the
0789          * second analog stage, first analog stage, digital stage order.
0790          * Gain from a previous stage should be pushed to its maximum
0791          * value before the next stage is used.
0792          */
0793         if (ctrl->val <= 32) {
0794             data = ctrl->val;
0795         } else if (ctrl->val <= 64) {
0796             ctrl->val &= ~1;
0797             data = (1 << 6) | (ctrl->val >> 1);
0798         } else {
0799             ctrl->val &= ~7;
0800             data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
0801         }
0802 
0803         return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
0804 
0805     case V4L2_CID_HFLIP:
0806         if (ctrl->val)
0807             return mt9p031_set_mode2(mt9p031,
0808                     0, MT9P031_READ_MODE_2_COL_MIR);
0809         else
0810             return mt9p031_set_mode2(mt9p031,
0811                     MT9P031_READ_MODE_2_COL_MIR, 0);
0812 
0813     case V4L2_CID_VFLIP:
0814         if (ctrl->val)
0815             return mt9p031_set_mode2(mt9p031,
0816                     0, MT9P031_READ_MODE_2_ROW_MIR);
0817         else
0818             return mt9p031_set_mode2(mt9p031,
0819                     MT9P031_READ_MODE_2_ROW_MIR, 0);
0820 
0821     case V4L2_CID_TEST_PATTERN:
0822         /* The digital side of the Black Level Calibration function must
0823          * be disabled when generating a test pattern to avoid artifacts
0824          * in the image. Activate (deactivate) the BLC-related controls
0825          * when the test pattern is enabled (disabled).
0826          */
0827         v4l2_ctrl_activate(mt9p031->blc_auto, ctrl->val == 0);
0828         v4l2_ctrl_activate(mt9p031->blc_offset, ctrl->val == 0);
0829 
0830         if (!ctrl->val) {
0831             /* Restore the BLC settings. */
0832             ret = mt9p031_restore_blc(mt9p031);
0833             if (ret < 0)
0834                 return ret;
0835 
0836             return mt9p031_write(client, MT9P031_TEST_PATTERN, 0);
0837         }
0838 
0839         ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
0840         if (ret < 0)
0841             return ret;
0842         ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
0843         if (ret < 0)
0844             return ret;
0845         ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
0846         if (ret < 0)
0847             return ret;
0848 
0849         /* Disable digital BLC when generating a test pattern. */
0850         ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
0851                     0);
0852         if (ret < 0)
0853             return ret;
0854 
0855         ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
0856         if (ret < 0)
0857             return ret;
0858 
0859         return mt9p031_write(client, MT9P031_TEST_PATTERN,
0860                 ((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
0861                 | MT9P031_TEST_PATTERN_ENABLE);
0862 
0863     case V4L2_CID_BLC_AUTO:
0864         ret = mt9p031_set_mode2(mt9p031,
0865                 ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
0866                 ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
0867         if (ret < 0)
0868             return ret;
0869 
0870         return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
0871                      ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
0872 
0873     case V4L2_CID_BLC_TARGET_LEVEL:
0874         return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
0875                      ctrl->val);
0876 
0877     case V4L2_CID_BLC_ANALOG_OFFSET:
0878         data = ctrl->val & ((1 << 9) - 1);
0879 
0880         ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
0881         if (ret < 0)
0882             return ret;
0883         ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
0884         if (ret < 0)
0885             return ret;
0886         ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
0887         if (ret < 0)
0888             return ret;
0889         return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
0890 
0891     case V4L2_CID_BLC_DIGITAL_OFFSET:
0892         return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
0893                      ctrl->val & ((1 << 12) - 1));
0894     }
0895 
0896     return 0;
0897 }
0898 
0899 static const struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
0900     .s_ctrl = mt9p031_s_ctrl,
0901 };
0902 
0903 static const char * const mt9p031_test_pattern_menu[] = {
0904     "Disabled",
0905     "Color Field",
0906     "Horizontal Gradient",
0907     "Vertical Gradient",
0908     "Diagonal Gradient",
0909     "Classic Test Pattern",
0910     "Walking 1s",
0911     "Monochrome Horizontal Bars",
0912     "Monochrome Vertical Bars",
0913     "Vertical Color Bars",
0914 };
0915 
0916 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
0917     {
0918         .ops        = &mt9p031_ctrl_ops,
0919         .id     = V4L2_CID_BLC_AUTO,
0920         .type       = V4L2_CTRL_TYPE_BOOLEAN,
0921         .name       = "BLC, Auto",
0922         .min        = 0,
0923         .max        = 1,
0924         .step       = 1,
0925         .def        = 1,
0926         .flags      = 0,
0927     }, {
0928         .ops        = &mt9p031_ctrl_ops,
0929         .id     = V4L2_CID_BLC_TARGET_LEVEL,
0930         .type       = V4L2_CTRL_TYPE_INTEGER,
0931         .name       = "BLC Target Level",
0932         .min        = 0,
0933         .max        = 4095,
0934         .step       = 1,
0935         .def        = 168,
0936         .flags      = 0,
0937     }, {
0938         .ops        = &mt9p031_ctrl_ops,
0939         .id     = V4L2_CID_BLC_ANALOG_OFFSET,
0940         .type       = V4L2_CTRL_TYPE_INTEGER,
0941         .name       = "BLC Analog Offset",
0942         .min        = -255,
0943         .max        = 255,
0944         .step       = 1,
0945         .def        = 32,
0946         .flags      = 0,
0947     }, {
0948         .ops        = &mt9p031_ctrl_ops,
0949         .id     = V4L2_CID_BLC_DIGITAL_OFFSET,
0950         .type       = V4L2_CTRL_TYPE_INTEGER,
0951         .name       = "BLC Digital Offset",
0952         .min        = -2048,
0953         .max        = 2047,
0954         .step       = 1,
0955         .def        = 40,
0956         .flags      = 0,
0957     }
0958 };
0959 
0960 /* -----------------------------------------------------------------------------
0961  * V4L2 subdev core operations
0962  */
0963 
0964 static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
0965 {
0966     struct mt9p031 *mt9p031 = to_mt9p031(subdev);
0967     int ret = 0;
0968 
0969     mutex_lock(&mt9p031->power_lock);
0970 
0971     /* If the power count is modified from 0 to != 0 or from != 0 to 0,
0972      * update the power state.
0973      */
0974     if (mt9p031->power_count == !on) {
0975         ret = __mt9p031_set_power(mt9p031, !!on);
0976         if (ret < 0)
0977             goto out;
0978     }
0979 
0980     /* Update the power count. */
0981     mt9p031->power_count += on ? 1 : -1;
0982     WARN_ON(mt9p031->power_count < 0);
0983 
0984 out:
0985     mutex_unlock(&mt9p031->power_lock);
0986     return ret;
0987 }
0988 
0989 /* -----------------------------------------------------------------------------
0990  * V4L2 subdev internal operations
0991  */
0992 
0993 static int mt9p031_registered(struct v4l2_subdev *subdev)
0994 {
0995     struct i2c_client *client = v4l2_get_subdevdata(subdev);
0996     struct mt9p031 *mt9p031 = to_mt9p031(subdev);
0997     s32 data;
0998     int ret;
0999 
1000     ret = mt9p031_power_on(mt9p031);
1001     if (ret < 0) {
1002         dev_err(&client->dev, "MT9P031 power up failed\n");
1003         return ret;
1004     }
1005 
1006     /* Read out the chip version register */
1007     data = mt9p031_read(client, MT9P031_CHIP_VERSION);
1008     mt9p031_power_off(mt9p031);
1009 
1010     if (data != MT9P031_CHIP_VERSION_VALUE) {
1011         dev_err(&client->dev, "MT9P031 not detected, wrong version "
1012             "0x%04x\n", data);
1013         return -ENODEV;
1014     }
1015 
1016     dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
1017          client->addr);
1018 
1019     return 0;
1020 }
1021 
1022 static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
1023 {
1024     return mt9p031_set_power(subdev, 1);
1025 }
1026 
1027 static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
1028 {
1029     return mt9p031_set_power(subdev, 0);
1030 }
1031 
1032 static const struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
1033     .s_power        = mt9p031_set_power,
1034 };
1035 
1036 static const struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
1037     .s_stream       = mt9p031_s_stream,
1038 };
1039 
1040 static const struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
1041     .init_cfg = mt9p031_init_cfg,
1042     .enum_mbus_code = mt9p031_enum_mbus_code,
1043     .enum_frame_size = mt9p031_enum_frame_size,
1044     .get_fmt = mt9p031_get_format,
1045     .set_fmt = mt9p031_set_format,
1046     .get_selection = mt9p031_get_selection,
1047     .set_selection = mt9p031_set_selection,
1048 };
1049 
1050 static const struct v4l2_subdev_ops mt9p031_subdev_ops = {
1051     .core   = &mt9p031_subdev_core_ops,
1052     .video  = &mt9p031_subdev_video_ops,
1053     .pad    = &mt9p031_subdev_pad_ops,
1054 };
1055 
1056 static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
1057     .registered = mt9p031_registered,
1058     .open = mt9p031_open,
1059     .close = mt9p031_close,
1060 };
1061 
1062 /* -----------------------------------------------------------------------------
1063  * Driver initialization and probing
1064  */
1065 
1066 static struct mt9p031_platform_data *
1067 mt9p031_get_pdata(struct i2c_client *client)
1068 {
1069     struct mt9p031_platform_data *pdata = NULL;
1070     struct device_node *np;
1071     struct v4l2_fwnode_endpoint endpoint = {
1072         .bus_type = V4L2_MBUS_PARALLEL
1073     };
1074 
1075     if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
1076         return client->dev.platform_data;
1077 
1078     np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
1079     if (!np)
1080         return NULL;
1081 
1082     if (v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &endpoint) < 0)
1083         goto done;
1084 
1085     pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
1086     if (!pdata)
1087         goto done;
1088 
1089     of_property_read_u32(np, "input-clock-frequency", &pdata->ext_freq);
1090     of_property_read_u32(np, "pixel-clock-frequency", &pdata->target_freq);
1091 
1092     pdata->pixclk_pol = !!(endpoint.bus.parallel.flags &
1093                    V4L2_MBUS_PCLK_SAMPLE_RISING);
1094 
1095 done:
1096     of_node_put(np);
1097     return pdata;
1098 }
1099 
1100 static int mt9p031_probe(struct i2c_client *client,
1101              const struct i2c_device_id *did)
1102 {
1103     struct mt9p031_platform_data *pdata = mt9p031_get_pdata(client);
1104     struct i2c_adapter *adapter = client->adapter;
1105     struct mt9p031 *mt9p031;
1106     unsigned int i;
1107     int ret;
1108 
1109     if (pdata == NULL) {
1110         dev_err(&client->dev, "No platform data\n");
1111         return -EINVAL;
1112     }
1113 
1114     if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1115         dev_warn(&client->dev,
1116             "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1117         return -EIO;
1118     }
1119 
1120     mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
1121     if (mt9p031 == NULL)
1122         return -ENOMEM;
1123 
1124     mt9p031->pdata = pdata;
1125     mt9p031->output_control = MT9P031_OUTPUT_CONTROL_DEF;
1126     mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
1127     mt9p031->model = did->driver_data;
1128 
1129     mt9p031->regulators[0].supply = "vdd";
1130     mt9p031->regulators[1].supply = "vdd_io";
1131     mt9p031->regulators[2].supply = "vaa";
1132 
1133     ret = devm_regulator_bulk_get(&client->dev, 3, mt9p031->regulators);
1134     if (ret < 0) {
1135         dev_err(&client->dev, "Unable to get regulators\n");
1136         return ret;
1137     }
1138 
1139     mutex_init(&mt9p031->power_lock);
1140 
1141     v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
1142 
1143     v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1144               V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
1145               MT9P031_SHUTTER_WIDTH_MAX, 1,
1146               MT9P031_SHUTTER_WIDTH_DEF);
1147     v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1148               V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
1149               MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
1150     v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1151               V4L2_CID_HFLIP, 0, 1, 1, 0);
1152     v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1153               V4L2_CID_VFLIP, 0, 1, 1, 0);
1154     v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1155               V4L2_CID_PIXEL_RATE, pdata->target_freq,
1156               pdata->target_freq, 1, pdata->target_freq);
1157     v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1158               V4L2_CID_TEST_PATTERN,
1159               ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
1160               0, mt9p031_test_pattern_menu);
1161 
1162     for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
1163         v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
1164 
1165     mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
1166 
1167     if (mt9p031->ctrls.error) {
1168         printk(KERN_INFO "%s: control initialization error %d\n",
1169                __func__, mt9p031->ctrls.error);
1170         ret = mt9p031->ctrls.error;
1171         goto done;
1172     }
1173 
1174     mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
1175     mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
1176                          V4L2_CID_BLC_DIGITAL_OFFSET);
1177 
1178     v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
1179     mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
1180 
1181     mt9p031->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1182     mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
1183     ret = media_entity_pads_init(&mt9p031->subdev.entity, 1, &mt9p031->pad);
1184     if (ret < 0)
1185         goto done;
1186 
1187     mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1188 
1189     ret = mt9p031_init_cfg(&mt9p031->subdev, NULL);
1190     if (ret)
1191         goto done;
1192 
1193     mt9p031->reset = devm_gpiod_get_optional(&client->dev, "reset",
1194                          GPIOD_OUT_HIGH);
1195 
1196     ret = mt9p031_clk_setup(mt9p031);
1197     if (ret)
1198         goto done;
1199 
1200     ret = v4l2_async_register_subdev(&mt9p031->subdev);
1201 
1202 done:
1203     if (ret < 0) {
1204         v4l2_ctrl_handler_free(&mt9p031->ctrls);
1205         media_entity_cleanup(&mt9p031->subdev.entity);
1206         mutex_destroy(&mt9p031->power_lock);
1207     }
1208 
1209     return ret;
1210 }
1211 
1212 static int mt9p031_remove(struct i2c_client *client)
1213 {
1214     struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1215     struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1216 
1217     v4l2_ctrl_handler_free(&mt9p031->ctrls);
1218     v4l2_async_unregister_subdev(subdev);
1219     media_entity_cleanup(&subdev->entity);
1220     mutex_destroy(&mt9p031->power_lock);
1221 
1222     return 0;
1223 }
1224 
1225 static const struct i2c_device_id mt9p031_id[] = {
1226     { "mt9p006", MT9P031_MODEL_COLOR },
1227     { "mt9p031", MT9P031_MODEL_COLOR },
1228     { "mt9p031m", MT9P031_MODEL_MONOCHROME },
1229     { }
1230 };
1231 MODULE_DEVICE_TABLE(i2c, mt9p031_id);
1232 
1233 #if IS_ENABLED(CONFIG_OF)
1234 static const struct of_device_id mt9p031_of_match[] = {
1235     { .compatible = "aptina,mt9p006", },
1236     { .compatible = "aptina,mt9p031", },
1237     { .compatible = "aptina,mt9p031m", },
1238     { /* sentinel */ },
1239 };
1240 MODULE_DEVICE_TABLE(of, mt9p031_of_match);
1241 #endif
1242 
1243 static struct i2c_driver mt9p031_i2c_driver = {
1244     .driver = {
1245         .of_match_table = of_match_ptr(mt9p031_of_match),
1246         .name = "mt9p031",
1247     },
1248     .probe          = mt9p031_probe,
1249     .remove         = mt9p031_remove,
1250     .id_table       = mt9p031_id,
1251 };
1252 
1253 module_i2c_driver(mt9p031_i2c_driver);
1254 
1255 MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
1256 MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
1257 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!