drivers/media/i2c/mt9v032.c

   1 /*
   2  * Driver for MT9V022, MT9V024, MT9V032, and MT9V034 CMOS Image Sensors
   3  *
   4  * Copyright (C) 2010, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
   5  *
   6  * Based on the MT9M001 driver,
   7  *
   8  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
   9  *
  10  * This program is free software; you can redistribute it and/or modify
  11  * it under the terms of the GNU General Public License version 2 as
  12  * published by the Free Software Foundation.
  13  */
  14
  15 #include <linux/clk.h>
  16 #include <linux/delay.h>
  17 #include <linux/gpio/consumer.h>
  18 #include <linux/i2c.h>
  19 #include <linux/log2.h>
  20 #include <linux/mutex.h>
  21 #include <linux/of.h>
  22 #include <linux/of_gpio.h>
  23 #include <linux/regmap.h>
  24 #include <linux/slab.h>
  25 #include <linux/videodev2.h>
  26 #include <linux/v4l2-mediabus.h>
  27 #include <linux/module.h>
  28
  29 #include <media/i2c/mt9v032.h>
  30 #include <media/v4l2-ctrls.h>
  31 #include <media/v4l2-device.h>
  32 #include <media/v4l2-of.h>
  33 #include <media/v4l2-subdev.h>
  34
  35 /* The first four rows are black rows. The active area spans 753x481 pixels. */
  36 #define MT9V032_PIXEL_ARRAY_HEIGHT                      485
  37 #define MT9V032_PIXEL_ARRAY_WIDTH                       753
  38
  39 #define MT9V032_SYSCLK_FREQ_DEF                         26600000
  40
  41 #define MT9V032_CHIP_VERSION                            0x00
  42 #define         MT9V032_CHIP_ID_REV1                    0x1311
  43 #define         MT9V032_CHIP_ID_REV3                    0x1313
  44 #define         MT9V034_CHIP_ID_REV1                    0X1324
  45 #define MT9V032_COLUMN_START                            0x01
  46 #define         MT9V032_COLUMN_START_MIN                1
  47 #define         MT9V032_COLUMN_START_DEF                1
  48 #define         MT9V032_COLUMN_START_MAX                752
  49 #define MT9V032_ROW_START                               0x02
  50 #define         MT9V032_ROW_START_MIN                   4
  51 #define         MT9V032_ROW_START_DEF                   5
  52 #define         MT9V032_ROW_START_MAX                   482
  53 #define MT9V032_WINDOW_HEIGHT                           0x03
  54 #define         MT9V032_WINDOW_HEIGHT_MIN               1
  55 #define         MT9V032_WINDOW_HEIGHT_DEF               480
  56 #define         MT9V032_WINDOW_HEIGHT_MAX               480
  57 #define MT9V032_WINDOW_WIDTH                            0x04
  58 #define         MT9V032_WINDOW_WIDTH_MIN                1
  59 #define         MT9V032_WINDOW_WIDTH_DEF                752
  60 #define         MT9V032_WINDOW_WIDTH_MAX                752
  61 #define MT9V032_HORIZONTAL_BLANKING                     0x05
  62 #define         MT9V032_HORIZONTAL_BLANKING_MIN         43
  63 #define         MT9V034_HORIZONTAL_BLANKING_MIN         61
  64 #define         MT9V032_HORIZONTAL_BLANKING_DEF         94
  65 #define         MT9V032_HORIZONTAL_BLANKING_MAX         1023
  66 #define MT9V032_VERTICAL_BLANKING                       0x06
  67 #define         MT9V032_VERTICAL_BLANKING_MIN           4
  68 #define         MT9V034_VERTICAL_BLANKING_MIN           2
  69 #define         MT9V032_VERTICAL_BLANKING_DEF           45
  70 #define         MT9V032_VERTICAL_BLANKING_MAX           3000
  71 #define         MT9V034_VERTICAL_BLANKING_MAX           32288
  72 #define MT9V032_CHIP_CONTROL                            0x07
  73 #define         MT9V032_CHIP_CONTROL_MASTER_MODE        (1 << 3)
  74 #define         MT9V032_CHIP_CONTROL_DOUT_ENABLE        (1 << 7)
  75 #define         MT9V032_CHIP_CONTROL_SEQUENTIAL         (1 << 8)
  76 #define MT9V032_SHUTTER_WIDTH1                          0x08
  77 #define MT9V032_SHUTTER_WIDTH2                          0x09
  78 #define MT9V032_SHUTTER_WIDTH_CONTROL                   0x0a
  79 #define MT9V032_TOTAL_SHUTTER_WIDTH                     0x0b
  80 #define         MT9V032_TOTAL_SHUTTER_WIDTH_MIN         1
  81 #define         MT9V034_TOTAL_SHUTTER_WIDTH_MIN         0
  82 #define         MT9V032_TOTAL_SHUTTER_WIDTH_DEF         480
  83 #define         MT9V032_TOTAL_SHUTTER_WIDTH_MAX         32767
  84 #define         MT9V034_TOTAL_SHUTTER_WIDTH_MAX         32765
  85 #define MT9V032_RESET                                   0x0c
  86 #define MT9V032_READ_MODE                               0x0d
  87 #define         MT9V032_READ_MODE_ROW_BIN_MASK          (3 << 0)
  88 #define         MT9V032_READ_MODE_ROW_BIN_SHIFT         0
  89 #define         MT9V032_READ_MODE_COLUMN_BIN_MASK       (3 << 2)
  90 #define         MT9V032_READ_MODE_COLUMN_BIN_SHIFT      2
  91 #define         MT9V032_READ_MODE_ROW_FLIP              (1 << 4)
  92 #define         MT9V032_READ_MODE_COLUMN_FLIP           (1 << 5)
  93 #define         MT9V032_READ_MODE_DARK_COLUMNS          (1 << 6)
  94 #define         MT9V032_READ_MODE_DARK_ROWS             (1 << 7)
  95 #define         MT9V032_READ_MODE_RESERVED              0x0300
  96 #define MT9V032_PIXEL_OPERATION_MODE                    0x0f
  97 #define         MT9V034_PIXEL_OPERATION_MODE_HDR        (1 << 0)
  98 #define         MT9V034_PIXEL_OPERATION_MODE_COLOR      (1 << 1)
  99 #define         MT9V032_PIXEL_OPERATION_MODE_COLOR      (1 << 2)
 100 #define         MT9V032_PIXEL_OPERATION_MODE_HDR        (1 << 6)
 101 #define MT9V032_ANALOG_GAIN                             0x35
 102 #define         MT9V032_ANALOG_GAIN_MIN                 16
 103 #define         MT9V032_ANALOG_GAIN_DEF                 16
 104 #define         MT9V032_ANALOG_GAIN_MAX                 64
 105 #define MT9V032_MAX_ANALOG_GAIN                         0x36
 106 #define         MT9V032_MAX_ANALOG_GAIN_MAX             127
 107 #define MT9V032_FRAME_DARK_AVERAGE                      0x42
 108 #define MT9V032_DARK_AVG_THRESH                         0x46
 109 #define         MT9V032_DARK_AVG_LOW_THRESH_MASK        (255 << 0)
 110 #define         MT9V032_DARK_AVG_LOW_THRESH_SHIFT       0
 111 #define         MT9V032_DARK_AVG_HIGH_THRESH_MASK       (255 << 8)
 112 #define         MT9V032_DARK_AVG_HIGH_THRESH_SHIFT      8
 113 #define MT9V032_ROW_NOISE_CORR_CONTROL                  0x70
 114 #define         MT9V034_ROW_NOISE_CORR_ENABLE           (1 << 0)
 115 #define         MT9V034_ROW_NOISE_CORR_USE_BLK_AVG      (1 << 1)
 116 #define         MT9V032_ROW_NOISE_CORR_ENABLE           (1 << 5)
 117 #define         MT9V032_ROW_NOISE_CORR_USE_BLK_AVG      (1 << 7)
 118 #define MT9V032_PIXEL_CLOCK                             0x74
 119 #define MT9V034_PIXEL_CLOCK                             0x72
 120 #define         MT9V032_PIXEL_CLOCK_INV_LINE            (1 << 0)
 121 #define         MT9V032_PIXEL_CLOCK_INV_FRAME           (1 << 1)
 122 #define         MT9V032_PIXEL_CLOCK_XOR_LINE            (1 << 2)
 123 #define         MT9V032_PIXEL_CLOCK_CONT_LINE           (1 << 3)
 124 #define         MT9V032_PIXEL_CLOCK_INV_PXL_CLK         (1 << 4)
 125 #define MT9V032_TEST_PATTERN                            0x7f
 126 #define         MT9V032_TEST_PATTERN_DATA_MASK          (1023 << 0)
 127 #define         MT9V032_TEST_PATTERN_DATA_SHIFT         0
 128 #define         MT9V032_TEST_PATTERN_USE_DATA           (1 << 10)
 129 #define         MT9V032_TEST_PATTERN_GRAY_MASK          (3 << 11)
 130 #define         MT9V032_TEST_PATTERN_GRAY_NONE          (0 << 11)
 131 #define         MT9V032_TEST_PATTERN_GRAY_VERTICAL      (1 << 11)
 132 #define         MT9V032_TEST_PATTERN_GRAY_HORIZONTAL    (2 << 11)
 133 #define         MT9V032_TEST_PATTERN_GRAY_DIAGONAL      (3 << 11)
 134 #define         MT9V032_TEST_PATTERN_ENABLE             (1 << 13)
 135 #define         MT9V032_TEST_PATTERN_FLIP               (1 << 14)
 136 #define MT9V032_AEC_AGC_ENABLE                          0xaf
 137 #define         MT9V032_AEC_ENABLE                      (1 << 0)
 138 #define         MT9V032_AGC_ENABLE                      (1 << 1)
 139 #define MT9V032_THERMAL_INFO                            0xc1
 140
 141 enum mt9v032_model {
 142         MT9V032_MODEL_V022_COLOR,       /* MT9V022IX7ATC */
 143         MT9V032_MODEL_V022_MONO,        /* MT9V022IX7ATM */
 144         MT9V032_MODEL_V024_COLOR,       /* MT9V024IA7XTC */
 145         MT9V032_MODEL_V024_MONO,        /* MT9V024IA7XTM */
 146         MT9V032_MODEL_V032_COLOR,       /* MT9V032C12STM */
 147         MT9V032_MODEL_V032_MONO,        /* MT9V032C12STC */
 148         MT9V032_MODEL_V034_COLOR,
 149         MT9V032_MODEL_V034_MONO,
 150 };
 151
 152 struct mt9v032_model_version {
 153         unsigned int version;
 154         const char *name;
 155 };
 156
 157 struct mt9v032_model_data {
 158         unsigned int min_row_time;
 159         unsigned int min_hblank;
 160         unsigned int min_vblank;
 161         unsigned int max_vblank;
 162         unsigned int min_shutter;
 163         unsigned int max_shutter;
 164         unsigned int pclk_reg;
 165 };
 166
 167 struct mt9v032_model_info {
 168         const struct mt9v032_model_data *data;
 169         bool color;
 170 };
 171
 172 static const struct mt9v032_model_version mt9v032_versions[] = {
 173         { MT9V032_CHIP_ID_REV1, "MT9V022/MT9V032 rev1/2" },
 174         { MT9V032_CHIP_ID_REV3, "MT9V022/MT9V032 rev3" },
 175         { MT9V034_CHIP_ID_REV1, "MT9V024/MT9V034 rev1" },
 176 };
 177
 178 static const struct mt9v032_model_data mt9v032_model_data[] = {
 179         {
 180                 /* MT9V022, MT9V032 revisions 1/2/3 */
 181                 .min_row_time = 660,
 182                 .min_hblank = MT9V032_HORIZONTAL_BLANKING_MIN,
 183                 .min_vblank = MT9V032_VERTICAL_BLANKING_MIN,
 184                 .max_vblank = MT9V032_VERTICAL_BLANKING_MAX,
 185                 .min_shutter = MT9V032_TOTAL_SHUTTER_WIDTH_MIN,
 186                 .max_shutter = MT9V032_TOTAL_SHUTTER_WIDTH_MAX,
 187                 .pclk_reg = MT9V032_PIXEL_CLOCK,
 188         }, {
 189                 /* MT9V024, MT9V034 */
 190                 .min_row_time = 690,
 191                 .min_hblank = MT9V034_HORIZONTAL_BLANKING_MIN,
 192                 .min_vblank = MT9V034_VERTICAL_BLANKING_MIN,
 193                 .max_vblank = MT9V034_VERTICAL_BLANKING_MAX,
 194                 .min_shutter = MT9V034_TOTAL_SHUTTER_WIDTH_MIN,
 195                 .max_shutter = MT9V034_TOTAL_SHUTTER_WIDTH_MAX,
 196                 .pclk_reg = MT9V034_PIXEL_CLOCK,
 197         },
 198 };
 199
 200 static const struct mt9v032_model_info mt9v032_models[] = {
 201         [MT9V032_MODEL_V022_COLOR] = {
 202                 .data = &mt9v032_model_data[0],
 203                 .color = true,
 204         },
 205         [MT9V032_MODEL_V022_MONO] = {
 206                 .data = &mt9v032_model_data[0],
 207                 .color = false,
 208         },
 209         [MT9V032_MODEL_V024_COLOR] = {
 210                 .data = &mt9v032_model_data[1],
 211                 .color = true,
 212         },
 213         [MT9V032_MODEL_V024_MONO] = {
 214                 .data = &mt9v032_model_data[1],
 215                 .color = false,
 216         },
 217         [MT9V032_MODEL_V032_COLOR] = {
 218                 .data = &mt9v032_model_data[0],
 219                 .color = true,
 220         },
 221         [MT9V032_MODEL_V032_MONO] = {
 222                 .data = &mt9v032_model_data[0],
 223                 .color = false,
 224         },
 225         [MT9V032_MODEL_V034_COLOR] = {
 226                 .data = &mt9v032_model_data[1],
 227                 .color = true,
 228         },
 229         [MT9V032_MODEL_V034_MONO] = {
 230                 .data = &mt9v032_model_data[1],
 231                 .color = false,
 232         },
 233 };
 234
 235 struct mt9v032 {
 236         struct v4l2_subdev subdev;
 237         struct media_pad pad;
 238
 239         struct v4l2_mbus_framefmt format;
 240         struct v4l2_rect crop;
 241         unsigned int hratio;
 242         unsigned int vratio;
 243
 244         struct v4l2_ctrl_handler ctrls;
 245         struct {
 246                 struct v4l2_ctrl *link_freq;
 247                 struct v4l2_ctrl *pixel_rate;
 248         };
 249
 250         struct mutex power_lock;
 251         int power_count;
 252
 253         struct regmap *regmap;
 254         struct clk *clk;
 255         struct gpio_desc *reset_gpio;
 256         struct gpio_desc *standby_gpio;
 257
 258         struct mt9v032_platform_data *pdata;
 259         const struct mt9v032_model_info *model;
 260         const struct mt9v032_model_version *version;
 261
 262         u32 sysclk;
 263         u16 aec_agc;
 264         u16 hblank;
 265         struct {
 266                 struct v4l2_ctrl *test_pattern;
 267                 struct v4l2_ctrl *test_pattern_color;
 268         };
 269 };
 270
 271 static struct mt9v032 *to_mt9v032(struct v4l2_subdev *sd)
 272 {
 273         return container_of(sd, struct mt9v032, subdev);
 274 }
 275
 276 static int
 277 mt9v032_update_aec_agc(struct mt9v032 *mt9v032, u16 which, int enable)
 278 {
 279         struct regmap *map = mt9v032->regmap;
 280         u16 value = mt9v032->aec_agc;
 281         int ret;
 282
 283         if (enable)
 284                 value |= which;
 285         else
 286                 value &= ~which;
 287
 288         ret = regmap_write(map, MT9V032_AEC_AGC_ENABLE, value);
 289         if (ret < 0)
 290                 return ret;
 291
 292         mt9v032->aec_agc = value;
 293         return 0;
 294 }
 295
 296 static int
 297 mt9v032_update_hblank(struct mt9v032 *mt9v032)
 298 {
 299         struct v4l2_rect *crop = &mt9v032->crop;
 300         unsigned int min_hblank = mt9v032->model->data->min_hblank;
 301         unsigned int hblank;
 302
 303         if (mt9v032->version->version == MT9V034_CHIP_ID_REV1)
 304                 min_hblank += (mt9v032->hratio - 1) * 10;
 305         min_hblank = max_t(int, mt9v032->model->data->min_row_time - crop->width,
 306                            min_hblank);
 307         hblank = max_t(unsigned int, mt9v032->hblank, min_hblank);
 308
 309         return regmap_write(mt9v032->regmap, MT9V032_HORIZONTAL_BLANKING,
 310                             hblank);
 311 }
 312
 313 static int mt9v032_power_on(struct mt9v032 *mt9v032)
 314 {
 315         struct regmap *map = mt9v032->regmap;
 316         int ret;
 317
 318         if (mt9v032->reset_gpio)
 319                 gpiod_set_value_cansleep(mt9v032->reset_gpio, 1);
 320
 321         ret = clk_set_rate(mt9v032->clk, mt9v032->sysclk);
 322         if (ret < 0)
 323                 return ret;
 324
 325         /* System clock has to be enabled before releasing the reset */
 326         ret = clk_prepare_enable(mt9v032->clk);
 327         if (ret)
 328                 return ret;
 329
 330         udelay(1);
 331
 332         if (mt9v032->reset_gpio) {
 333                 gpiod_set_value_cansleep(mt9v032->reset_gpio, 0);
 334
 335                 /* After releasing reset we need to wait 10 clock cycles
 336                  * before accessing the sensor over I2C. As the minimum SYSCLK
 337                  * frequency is 13MHz, waiting 1µs will be enough in the worst
 338                  * case.
 339                  */
 340                 udelay(1);
 341         }
 342
 343         /* Reset the chip and stop data read out */
 344         ret = regmap_write(map, MT9V032_RESET, 1);
 345         if (ret < 0)
 346                 return ret;
 347
 348         ret = regmap_write(map, MT9V032_RESET, 0);
 349         if (ret < 0)
 350                 return ret;
 351
 352         return regmap_write(map, MT9V032_CHIP_CONTROL, 0);
 353 }
 354
 355 static void mt9v032_power_off(struct mt9v032 *mt9v032)
 356 {
 357         clk_disable_unprepare(mt9v032->clk);
 358 }
 359
 360 static int __mt9v032_set_power(struct mt9v032 *mt9v032, bool on)
 361 {
 362         struct regmap *map = mt9v032->regmap;
 363         int ret;
 364
 365         if (!on) {
 366                 mt9v032_power_off(mt9v032);
 367                 return 0;
 368         }
 369
 370         ret = mt9v032_power_on(mt9v032);
 371         if (ret < 0)
 372                 return ret;
 373
 374         /* Configure the pixel clock polarity */
 375         if (mt9v032->pdata && mt9v032->pdata->clk_pol) {
 376                 ret = regmap_write(map, mt9v032->model->data->pclk_reg,
 377                                 MT9V032_PIXEL_CLOCK_INV_PXL_CLK);
 378                 if (ret < 0)
 379                         return ret;
 380         }
 381
 382         /* Disable the noise correction algorithm and restore the controls. */
 383         ret = regmap_write(map, MT9V032_ROW_NOISE_CORR_CONTROL, 0);
 384         if (ret < 0)
 385                 return ret;
 386
 387         return v4l2_ctrl_handler_setup(&mt9v032->ctrls);
 388 }
 389
 390 /* -----------------------------------------------------------------------------
 391  * V4L2 subdev video operations
 392  */
 393
 394 static struct v4l2_mbus_framefmt *
 395 __mt9v032_get_pad_format(struct mt9v032 *mt9v032, struct v4l2_subdev_pad_config *cfg,
 396                          unsigned int pad, enum v4l2_subdev_format_whence which)
 397 {
 398         switch (which) {
 399         case V4L2_SUBDEV_FORMAT_TRY:
 400                 return v4l2_subdev_get_try_format(&mt9v032->subdev, cfg, pad);
 401         case V4L2_SUBDEV_FORMAT_ACTIVE:
 402                 return &mt9v032->format;
 403         default:
 404                 return NULL;
 405         }
 406 }
 407
 408 static struct v4l2_rect *
 409 __mt9v032_get_pad_crop(struct mt9v032 *mt9v032, struct v4l2_subdev_pad_config *cfg,
 410                        unsigned int pad, enum v4l2_subdev_format_whence which)
 411 {
 412         switch (which) {
 413         case V4L2_SUBDEV_FORMAT_TRY:
 414                 return v4l2_subdev_get_try_crop(&mt9v032->subdev, cfg, pad);
 415         case V4L2_SUBDEV_FORMAT_ACTIVE:
 416                 return &mt9v032->crop;
 417         default:
 418                 return NULL;
 419         }
 420 }
 421
 422 static int mt9v032_s_stream(struct v4l2_subdev *subdev, int enable)
 423 {
 424         const u16 mode = MT9V032_CHIP_CONTROL_MASTER_MODE
 425                        | MT9V032_CHIP_CONTROL_DOUT_ENABLE
 426                        | MT9V032_CHIP_CONTROL_SEQUENTIAL;
 427         struct mt9v032 *mt9v032 = to_mt9v032(subdev);
 428         struct v4l2_rect *crop = &mt9v032->crop;
 429         struct regmap *map = mt9v032->regmap;
 430         unsigned int hbin;
 431         unsigned int vbin;
 432         int ret;
 433
 434         if (!enable)
 435                 return regmap_update_bits(map, MT9V032_CHIP_CONTROL, mode, 0);
 436
 437         /* Configure the window size and row/column bin */
 438         hbin = fls(mt9v032->hratio) - 1;
 439         vbin = fls(mt9v032->vratio) - 1;
 440         ret = regmap_update_bits(map, MT9V032_READ_MODE,
 441                                  ~MT9V032_READ_MODE_RESERVED,
 442                                  hbin << MT9V032_READ_MODE_COLUMN_BIN_SHIFT |
 443                                  vbin << MT9V032_READ_MODE_ROW_BIN_SHIFT);
 444         if (ret < 0)
 445                 return ret;
 446
 447         ret = regmap_write(map, MT9V032_COLUMN_START, crop->left);
 448         if (ret < 0)
 449                 return ret;
 450
 451         ret = regmap_write(map, MT9V032_ROW_START, crop->top);
 452         if (ret < 0)
 453                 return ret;
 454
 455         ret = regmap_write(map, MT9V032_WINDOW_WIDTH, crop->width);
 456         if (ret < 0)
 457                 return ret;
 458
 459         ret = regmap_write(map, MT9V032_WINDOW_HEIGHT, crop->height);
 460         if (ret < 0)
 461                 return ret;
 462
 463         ret = mt9v032_update_hblank(mt9v032);
 464         if (ret < 0)
 465                 return ret;
 466
 467         /* Switch to master "normal" mode */
 468         return regmap_update_bits(map, MT9V032_CHIP_CONTROL, mode, mode);
 469 }
 470
 471 static int mt9v032_enum_mbus_code(struct v4l2_subdev *subdev,
 472                                   struct v4l2_subdev_pad_config *cfg,
 473                                   struct v4l2_subdev_mbus_code_enum *code)
 474 {
 475         if (code->index > 0)
 476                 return -EINVAL;
 477
 478         code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
 479         return 0;
 480 }
 481
 482 static int mt9v032_enum_frame_size(struct v4l2_subdev *subdev,
 483                                    struct v4l2_subdev_pad_config *cfg,
 484                                    struct v4l2_subdev_frame_size_enum *fse)
 485 {
 486         if (fse->index >= 3 || fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
 487                 return -EINVAL;
 488
 489         fse->min_width = MT9V032_WINDOW_WIDTH_DEF / (1 << fse->index);
 490         fse->max_width = fse->min_width;
 491         fse->min_height = MT9V032_WINDOW_HEIGHT_DEF / (1 << fse->index);
 492         fse->max_height = fse->min_height;
 493
 494         return 0;
 495 }
 496
 497 static int mt9v032_get_format(struct v4l2_subdev *subdev,
 498                               struct v4l2_subdev_pad_config *cfg,
 499                               struct v4l2_subdev_format *format)
 500 {
 501         struct mt9v032 *mt9v032 = to_mt9v032(subdev);
 502
 503         format->format = *__mt9v032_get_pad_format(mt9v032, cfg, format->pad,
 504                                                    format->which);
 505         return 0;
 506 }
 507
 508 static void mt9v032_configure_pixel_rate(struct mt9v032 *mt9v032)
 509 {
 510         struct i2c_client *client = v4l2_get_subdevdata(&mt9v032->subdev);
 511         int ret;
 512
 513         ret = v4l2_ctrl_s_ctrl_int64(mt9v032->pixel_rate,
 514                                      mt9v032->sysclk / mt9v032->hratio);
 515         if (ret < 0)
 516                 dev_warn(&client->dev, "failed to set pixel rate (%d)\n", ret);
 517 }
 518
 519 static unsigned int mt9v032_calc_ratio(unsigned int input, unsigned int output)
 520 {
 521         /* Compute the power-of-two binning factor closest to the input size to
 522          * output size ratio. Given that the output size is bounded by input/4
 523          * and input, a generic implementation would be an ineffective luxury.
 524          */
 525         if (output * 3 > input * 2)
 526                 return 1;
 527         if (output * 3 > input)
 528                 return 2;
 529         return 4;
 530 }
 531
 532 static int mt9v032_set_format(struct v4l2_subdev *subdev,
 533                               struct v4l2_subdev_pad_config *cfg,
 534                               struct v4l2_subdev_format *format)
 535 {
 536         struct mt9v032 *mt9v032 = to_mt9v032(subdev);
 537         struct v4l2_mbus_framefmt *__format;
 538         struct v4l2_rect *__crop;
 539         unsigned int width;
 540         unsigned int height;
 541         unsigned int hratio;
 542         unsigned int vratio;
 543
 544         __crop = __mt9v032_get_pad_crop(mt9v032, cfg, format->pad,
 545                                         format->which);
 546
 547         /* Clamp the width and height to avoid dividing by zero. */
 548         width = clamp(ALIGN(format->format.width, 2),
 549                       max_t(unsigned int, __crop->width / 4,
 550                             MT9V032_WINDOW_WIDTH_MIN),
 551                       __crop->width);
 552         height = clamp(ALIGN(format->format.height, 2),
 553                        max_t(unsigned int, __crop->height / 4,
 554                              MT9V032_WINDOW_HEIGHT_MIN),
 555                        __crop->height);
 556
 557         hratio = mt9v032_calc_ratio(__crop->width, width);
 558         vratio = mt9v032_calc_ratio(__crop->height, height);
 559
 560         __format = __mt9v032_get_pad_format(mt9v032, cfg, format->pad,
 561                                             format->which);
 562         __format->width = __crop->width / hratio;
 563         __format->height = __crop->height / vratio;
 564
 565         if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
 566                 mt9v032->hratio = hratio;
 567                 mt9v032->vratio = vratio;
 568                 mt9v032_configure_pixel_rate(mt9v032);
 569         }
 570
 571         format->format = *__format;
 572
 573         return 0;
 574 }
 575
 576 static int mt9v032_get_selection(struct v4l2_subdev *subdev,
 577                                  struct v4l2_subdev_pad_config *cfg,
 578                                  struct v4l2_subdev_selection *sel)
 579 {
 580         struct mt9v032 *mt9v032 = to_mt9v032(subdev);
 581
 582         if (sel->target != V4L2_SEL_TGT_CROP)
 583                 return -EINVAL;
 584
 585         sel->r = *__mt9v032_get_pad_crop(mt9v032, cfg, sel->pad, sel->which);
 586         return 0;
 587 }
 588
 589 static int mt9v032_set_selection(struct v4l2_subdev *subdev,
 590                                  struct v4l2_subdev_pad_config *cfg,
 591                                  struct v4l2_subdev_selection *sel)
 592 {
 593         struct mt9v032 *mt9v032 = to_mt9v032(subdev);
 594         struct v4l2_mbus_framefmt *__format;
 595         struct v4l2_rect *__crop;
 596         struct v4l2_rect rect;
 597
 598         if (sel->target != V4L2_SEL_TGT_CROP)
 599                 return -EINVAL;
 600
 601         /* Clamp the crop rectangle boundaries and align them to a non multiple
 602          * of 2 pixels to ensure a GRBG Bayer pattern.
 603          */
 604         rect.left = clamp(ALIGN(sel->r.left + 1, 2) - 1,
 605                           MT9V032_COLUMN_START_MIN,
 606                           MT9V032_COLUMN_START_MAX);
 607         rect.top = clamp(ALIGN(sel->r.top + 1, 2) - 1,
 608                          MT9V032_ROW_START_MIN,
 609                          MT9V032_ROW_START_MAX);
 610         rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 2),
 611                              MT9V032_WINDOW_WIDTH_MIN,
 612                              MT9V032_WINDOW_WIDTH_MAX);
 613         rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 2),
 614                               MT9V032_WINDOW_HEIGHT_MIN,
 615                               MT9V032_WINDOW_HEIGHT_MAX);
 616
 617         rect.width = min_t(unsigned int,
 618                            rect.width, MT9V032_PIXEL_ARRAY_WIDTH - rect.left);
 619         rect.height = min_t(unsigned int,
 620                             rect.height, MT9V032_PIXEL_ARRAY_HEIGHT - rect.top);
 621
 622         __crop = __mt9v032_get_pad_crop(mt9v032, cfg, sel->pad, sel->which);
 623
 624         if (rect.width != __crop->width || rect.height != __crop->height) {
 625                 /* Reset the output image size if the crop rectangle size has
 626                  * been modified.
 627                  */
 628                 __format = __mt9v032_get_pad_format(mt9v032, cfg, sel->pad,
 629                                                     sel->which);
 630                 __format->width = rect.width;
 631                 __format->height = rect.height;
 632                 if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
 633                         mt9v032->hratio = 1;
 634                         mt9v032->vratio = 1;
 635                         mt9v032_configure_pixel_rate(mt9v032);
 636                 }
 637         }
 638
 639         *__crop = rect;
 640         sel->r = rect;
 641
 642         return 0;
 643 }
 644
 645 /* -----------------------------------------------------------------------------
 646  * V4L2 subdev control operations
 647  */
 648
 649 #define V4L2_CID_TEST_PATTERN_COLOR     (V4L2_CID_USER_BASE | 0x1001)
 650
 651 static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
 652 {
 653         struct mt9v032 *mt9v032 =
 654                         container_of(ctrl->handler, struct mt9v032, ctrls);
 655         struct regmap *map = mt9v032->regmap;
 656         u32 freq;
 657         u16 data;
 658
 659         switch (ctrl->id) {
 660         case V4L2_CID_AUTOGAIN:
 661                 return mt9v032_update_aec_agc(mt9v032, MT9V032_AGC_ENABLE,
 662                                               ctrl->val);
 663
 664         case V4L2_CID_GAIN:
 665                 return regmap_write(map, MT9V032_ANALOG_GAIN, ctrl->val);
 666
 667         case V4L2_CID_EXPOSURE_AUTO:
 668                 return mt9v032_update_aec_agc(mt9v032, MT9V032_AEC_ENABLE,
 669                                               !ctrl->val);
 670
 671         case V4L2_CID_EXPOSURE:
 672                 return regmap_write(map, MT9V032_TOTAL_SHUTTER_WIDTH,
 673                                     ctrl->val);
 674
 675         case V4L2_CID_HBLANK:
 676                 mt9v032->hblank = ctrl->val;
 677                 return mt9v032_update_hblank(mt9v032);
 678
 679         case V4L2_CID_VBLANK:
 680                 return regmap_write(map, MT9V032_VERTICAL_BLANKING,
 681                                     ctrl->val);
 682
 683         case V4L2_CID_PIXEL_RATE:
 684         case V4L2_CID_LINK_FREQ:
 685                 if (mt9v032->link_freq == NULL)
 686                         break;
 687
 688                 freq = mt9v032->pdata->link_freqs[mt9v032->link_freq->val];
 689                 *mt9v032->pixel_rate->p_new.p_s64 = freq;
 690                 mt9v032->sysclk = freq;
 691                 break;
 692
 693         case V4L2_CID_TEST_PATTERN:
 694                 switch (mt9v032->test_pattern->val) {
 695                 case 0:
 696                         data = 0;
 697                         break;
 698                 case 1:
 699                         data = MT9V032_TEST_PATTERN_GRAY_VERTICAL
 700                              | MT9V032_TEST_PATTERN_ENABLE;
 701                         break;
 702                 case 2:
 703                         data = MT9V032_TEST_PATTERN_GRAY_HORIZONTAL
 704                              | MT9V032_TEST_PATTERN_ENABLE;
 705                         break;
 706                 case 3:
 707                         data = MT9V032_TEST_PATTERN_GRAY_DIAGONAL
 708                              | MT9V032_TEST_PATTERN_ENABLE;
 709                         break;
 710                 default:
 711                         data = (mt9v032->test_pattern_color->val <<
 712                                 MT9V032_TEST_PATTERN_DATA_SHIFT)
 713                              | MT9V032_TEST_PATTERN_USE_DATA
 714                              | MT9V032_TEST_PATTERN_ENABLE
 715                              | MT9V032_TEST_PATTERN_FLIP;
 716                         break;
 717                 }
 718                 return regmap_write(map, MT9V032_TEST_PATTERN, data);
 719         }
 720
 721         return 0;
 722 }
 723
 724 static const struct v4l2_ctrl_ops mt9v032_ctrl_ops = {
 725         .s_ctrl = mt9v032_s_ctrl,
 726 };
 727
 728 static const char * const mt9v032_test_pattern_menu[] = {
 729         "Disabled",
 730         "Gray Vertical Shade",
 731         "Gray Horizontal Shade",
 732         "Gray Diagonal Shade",
 733         "Plain",
 734 };
 735
 736 static const struct v4l2_ctrl_config mt9v032_test_pattern_color = {
 737         .ops            = &mt9v032_ctrl_ops,
 738         .id             = V4L2_CID_TEST_PATTERN_COLOR,
 739         .type           = V4L2_CTRL_TYPE_INTEGER,
 740         .name           = "Test Pattern Color",
 741         .min            = 0,
 742         .max            = 1023,
 743         .step           = 1,
 744         .def            = 0,
 745         .flags          = 0,
 746 };
 747
 748 /* -----------------------------------------------------------------------------
 749  * V4L2 subdev core operations
 750  */
 751
 752 static int mt9v032_set_power(struct v4l2_subdev *subdev, int on)
 753 {
 754         struct mt9v032 *mt9v032 = to_mt9v032(subdev);
 755         int ret = 0;
 756
 757         mutex_lock(&mt9v032->power_lock);
 758
 759         /* If the power count is modified from 0 to != 0 or from != 0 to 0,
 760          * update the power state.
 761          */
 762         if (mt9v032->power_count == !on) {
 763                 ret = __mt9v032_set_power(mt9v032, !!on);
 764                 if (ret < 0)
 765                         goto done;
 766         }
 767
 768         /* Update the power count. */
 769         mt9v032->power_count += on ? 1 : -1;
 770         WARN_ON(mt9v032->power_count < 0);
 771
 772 done:
 773         mutex_unlock(&mt9v032->power_lock);
 774         return ret;
 775 }
 776
 777 /* -----------------------------------------------------------------------------
 778  * V4L2 subdev internal operations
 779  */
 780
 781 static int mt9v032_registered(struct v4l2_subdev *subdev)
 782 {
 783         struct i2c_client *client = v4l2_get_subdevdata(subdev);
 784         struct mt9v032 *mt9v032 = to_mt9v032(subdev);
 785         unsigned int i;
 786         u32 version;
 787         int ret;
 788
 789         dev_info(&client->dev, "Probing MT9V032 at address 0x%02x\n",
 790                         client->addr);
 791
 792         ret = mt9v032_power_on(mt9v032);
 793         if (ret < 0) {
 794                 dev_err(&client->dev, "MT9V032 power up failed\n");
 795                 return ret;
 796         }
 797
 798         /* Read and check the sensor version */
 799         ret = regmap_read(mt9v032->regmap, MT9V032_CHIP_VERSION, &version);
 800         if (ret < 0) {
 801                 dev_err(&client->dev, "Failed reading chip version\n");
 802                 return ret;
 803         }
 804
 805         for (i = 0; i < ARRAY_SIZE(mt9v032_versions); ++i) {
 806                 if (mt9v032_versions[i].version == version) {
 807                         mt9v032->version = &mt9v032_versions[i];
 808                         break;
 809                 }
 810         }
 811
 812         if (mt9v032->version == NULL) {
 813                 dev_err(&client->dev, "Unsupported chip version 0x%04x\n",
 814                         version);
 815                 return -ENODEV;
 816         }
 817
 818         mt9v032_power_off(mt9v032);
 819
 820         dev_info(&client->dev, "%s detected at address 0x%02x\n",
 821                  mt9v032->version->name, client->addr);
 822
 823         mt9v032_configure_pixel_rate(mt9v032);
 824
 825         return ret;
 826 }
 827
 828 static int mt9v032_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 829 {
 830         struct mt9v032 *mt9v032 = to_mt9v032(subdev);
 831         struct v4l2_mbus_framefmt *format;
 832         struct v4l2_rect *crop;
 833
 834         crop = v4l2_subdev_get_try_crop(subdev, fh->pad, 0);
 835         crop->left = MT9V032_COLUMN_START_DEF;
 836         crop->top = MT9V032_ROW_START_DEF;
 837         crop->width = MT9V032_WINDOW_WIDTH_DEF;
 838         crop->height = MT9V032_WINDOW_HEIGHT_DEF;
 839
 840         format = v4l2_subdev_get_try_format(subdev, fh->pad, 0);
 841
 842         if (mt9v032->model->color)
 843                 format->code = MEDIA_BUS_FMT_SGRBG10_1X10;
 844         else
 845                 format->code = MEDIA_BUS_FMT_Y10_1X10;
 846
 847         format->width = MT9V032_WINDOW_WIDTH_DEF;
 848         format->height = MT9V032_WINDOW_HEIGHT_DEF;
 849         format->field = V4L2_FIELD_NONE;
 850         format->colorspace = V4L2_COLORSPACE_SRGB;
 851
 852         return mt9v032_set_power(subdev, 1);
 853 }
 854
 855 static int mt9v032_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 856 {
 857         return mt9v032_set_power(subdev, 0);
 858 }
 859
 860 static struct v4l2_subdev_core_ops mt9v032_subdev_core_ops = {
 861         .s_power        = mt9v032_set_power,
 862 };
 863
 864 static struct v4l2_subdev_video_ops mt9v032_subdev_video_ops = {
 865         .s_stream       = mt9v032_s_stream,
 866 };
 867
 868 static struct v4l2_subdev_pad_ops mt9v032_subdev_pad_ops = {
 869         .enum_mbus_code = mt9v032_enum_mbus_code,
 870         .enum_frame_size = mt9v032_enum_frame_size,
 871         .get_fmt = mt9v032_get_format,
 872         .set_fmt = mt9v032_set_format,
 873         .get_selection = mt9v032_get_selection,
 874         .set_selection = mt9v032_set_selection,
 875 };
 876
 877 static struct v4l2_subdev_ops mt9v032_subdev_ops = {
 878         .core   = &mt9v032_subdev_core_ops,
 879         .video  = &mt9v032_subdev_video_ops,
 880         .pad    = &mt9v032_subdev_pad_ops,
 881 };
 882
 883 static const struct v4l2_subdev_internal_ops mt9v032_subdev_internal_ops = {
 884         .registered = mt9v032_registered,
 885         .open = mt9v032_open,
 886         .close = mt9v032_close,
 887 };
 888
 889 static const struct regmap_config mt9v032_regmap_config = {
 890         .reg_bits = 8,
 891         .val_bits = 16,
 892         .max_register = 0xff,
 893         .cache_type = REGCACHE_RBTREE,
 894 };
 895
 896 /* -----------------------------------------------------------------------------
 897  * Driver initialization and probing
 898  */
 899
 900 static struct mt9v032_platform_data *
 901 mt9v032_get_pdata(struct i2c_client *client)
 902 {
 903         struct mt9v032_platform_data *pdata = NULL;
 904         struct v4l2_of_endpoint endpoint;
 905         struct device_node *np;
 906         struct property *prop;
 907
 908         if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
 909                 return client->dev.platform_data;
 910
 911         np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
 912         if (!np)
 913                 return NULL;
 914
 915         if (v4l2_of_parse_endpoint(np, &endpoint) < 0)
 916                 goto done;
 917
 918         pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
 919         if (!pdata)
 920                 goto done;
 921
 922         prop = of_find_property(np, "link-frequencies", NULL);
 923         if (prop) {
 924                 u64 *link_freqs;
 925                 size_t size = prop->length / sizeof(*link_freqs);
 926
 927                 link_freqs = devm_kcalloc(&client->dev, size,
 928                                           sizeof(*link_freqs), GFP_KERNEL);
 929                 if (!link_freqs)
 930                         goto done;
 931
 932                 if (of_property_read_u64_array(np, "link-frequencies",
 933                                                link_freqs, size) < 0)
 934                         goto done;
 935
 936                 pdata->link_freqs = link_freqs;
 937                 pdata->link_def_freq = link_freqs[0];
 938         }
 939
 940         pdata->clk_pol = !!(endpoint.bus.parallel.flags &
 941                             V4L2_MBUS_PCLK_SAMPLE_RISING);
 942
 943 done:
 944         of_node_put(np);
 945         return pdata;
 946 }
 947
 948 static int mt9v032_probe(struct i2c_client *client,
 949                 const struct i2c_device_id *did)
 950 {
 951         struct mt9v032_platform_data *pdata = mt9v032_get_pdata(client);
 952         struct mt9v032 *mt9v032;
 953         unsigned int i;
 954         int ret;
 955
 956         if (!i2c_check_functionality(client->adapter,
 957                                      I2C_FUNC_SMBUS_WORD_DATA)) {
 958                 dev_warn(&client->adapter->dev,
 959                          "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
 960                 return -EIO;
 961         }
 962
 963         mt9v032 = devm_kzalloc(&client->dev, sizeof(*mt9v032), GFP_KERNEL);
 964         if (!mt9v032)
 965                 return -ENOMEM;
 966
 967         mt9v032->regmap = devm_regmap_init_i2c(client, &mt9v032_regmap_config);
 968         if (IS_ERR(mt9v032->regmap))
 969                 return PTR_ERR(mt9v032->regmap);
 970
 971         mt9v032->clk = devm_clk_get(&client->dev, NULL);
 972         if (IS_ERR(mt9v032->clk))
 973                 return PTR_ERR(mt9v032->clk);
 974
 975         mt9v032->reset_gpio = devm_gpiod_get_optional(&client->dev, "reset",
 976                                                       GPIOD_OUT_HIGH);
 977         if (IS_ERR(mt9v032->reset_gpio))
 978                 return PTR_ERR(mt9v032->reset_gpio);
 979
 980         mt9v032->standby_gpio = devm_gpiod_get_optional(&client->dev, "standby",
 981                                                         GPIOD_OUT_LOW);
 982         if (IS_ERR(mt9v032->standby_gpio))
 983                 return PTR_ERR(mt9v032->standby_gpio);
 984
 985         mutex_init(&mt9v032->power_lock);
 986         mt9v032->pdata = pdata;
 987         mt9v032->model = (const void *)did->driver_data;
 988
 989         v4l2_ctrl_handler_init(&mt9v032->ctrls, 10);
 990
 991         v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
 992                           V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
 993         v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
 994                           V4L2_CID_GAIN, MT9V032_ANALOG_GAIN_MIN,
 995                           MT9V032_ANALOG_GAIN_MAX, 1, MT9V032_ANALOG_GAIN_DEF);
 996         v4l2_ctrl_new_std_menu(&mt9v032->ctrls, &mt9v032_ctrl_ops,
 997                                V4L2_CID_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL, 0,
 998                                V4L2_EXPOSURE_AUTO);
 999         v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
1000                           V4L2_CID_EXPOSURE, mt9v032->model->data->min_shutter,
1001                           mt9v032->model->data->max_shutter, 1,
1002                           MT9V032_TOTAL_SHUTTER_WIDTH_DEF);
1003         v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
1004                           V4L2_CID_HBLANK, mt9v032->model->data->min_hblank,
1005                           MT9V032_HORIZONTAL_BLANKING_MAX, 1,
1006                           MT9V032_HORIZONTAL_BLANKING_DEF);
1007         v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
1008                           V4L2_CID_VBLANK, mt9v032->model->data->min_vblank,
1009                           mt9v032->model->data->max_vblank, 1,
1010                           MT9V032_VERTICAL_BLANKING_DEF);
1011         mt9v032->test_pattern = v4l2_ctrl_new_std_menu_items(&mt9v032->ctrls,
1012                                 &mt9v032_ctrl_ops, V4L2_CID_TEST_PATTERN,
1013                                 ARRAY_SIZE(mt9v032_test_pattern_menu) - 1, 0, 0,
1014                                 mt9v032_test_pattern_menu);
1015         mt9v032->test_pattern_color = v4l2_ctrl_new_custom(&mt9v032->ctrls,
1016                                       &mt9v032_test_pattern_color, NULL);
1017
1018         v4l2_ctrl_cluster(2, &mt9v032->test_pattern);
1019
1020         mt9v032->pixel_rate =
1021                 v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
1022                                   V4L2_CID_PIXEL_RATE, 1, INT_MAX, 1, 1);
1023
1024         if (pdata && pdata->link_freqs) {
1025                 unsigned int def = 0;
1026
1027                 for (i = 0; pdata->link_freqs[i]; ++i) {
1028                         if (pdata->link_freqs[i] == pdata->link_def_freq)
1029                                 def = i;
1030                 }
1031
1032                 mt9v032->link_freq =
1033                         v4l2_ctrl_new_int_menu(&mt9v032->ctrls,
1034                                                &mt9v032_ctrl_ops,
1035                                                V4L2_CID_LINK_FREQ, i - 1, def,
1036                                                pdata->link_freqs);
1037                 v4l2_ctrl_cluster(2, &mt9v032->link_freq);
1038         }
1039
1040
1041         mt9v032->subdev.ctrl_handler = &mt9v032->ctrls;
1042
1043         if (mt9v032->ctrls.error) {
1044                 dev_err(&client->dev, "control initialization error %d\n",
1045                         mt9v032->ctrls.error);
1046                 ret = mt9v032->ctrls.error;
1047                 goto err;
1048         }
1049
1050         mt9v032->crop.left = MT9V032_COLUMN_START_DEF;
1051         mt9v032->crop.top = MT9V032_ROW_START_DEF;
1052         mt9v032->crop.width = MT9V032_WINDOW_WIDTH_DEF;
1053         mt9v032->crop.height = MT9V032_WINDOW_HEIGHT_DEF;
1054
1055         if (mt9v032->model->color)
1056                 mt9v032->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
1057         else
1058                 mt9v032->format.code = MEDIA_BUS_FMT_Y10_1X10;
1059
1060         mt9v032->format.width = MT9V032_WINDOW_WIDTH_DEF;
1061         mt9v032->format.height = MT9V032_WINDOW_HEIGHT_DEF;
1062         mt9v032->format.field = V4L2_FIELD_NONE;
1063         mt9v032->format.colorspace = V4L2_COLORSPACE_SRGB;
1064
1065         mt9v032->hratio = 1;
1066         mt9v032->vratio = 1;
1067
1068         mt9v032->aec_agc = MT9V032_AEC_ENABLE | MT9V032_AGC_ENABLE;
1069         mt9v032->hblank = MT9V032_HORIZONTAL_BLANKING_DEF;
1070         mt9v032->sysclk = MT9V032_SYSCLK_FREQ_DEF;
1071
1072         v4l2_i2c_subdev_init(&mt9v032->subdev, client, &mt9v032_subdev_ops);
1073         mt9v032->subdev.internal_ops = &mt9v032_subdev_internal_ops;
1074         mt9v032->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1075
1076         mt9v032->pad.flags = MEDIA_PAD_FL_SOURCE;
1077         ret = media_entity_pads_init(&mt9v032->subdev.entity, 1, &mt9v032->pad);
1078         if (ret < 0)
1079                 goto err;
1080
1081         mt9v032->subdev.dev = &client->dev;
1082         ret = v4l2_async_register_subdev(&mt9v032->subdev);
1083         if (ret < 0)
1084                 goto err;
1085
1086         return 0;
1087
1088 err:
1089         media_entity_cleanup(&mt9v032->subdev.entity);
1090         v4l2_ctrl_handler_free(&mt9v032->ctrls);
1091         return ret;
1092 }
1093
1094 static int mt9v032_remove(struct i2c_client *client)
1095 {
1096         struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1097         struct mt9v032 *mt9v032 = to_mt9v032(subdev);
1098
1099         v4l2_async_unregister_subdev(subdev);
1100         v4l2_ctrl_handler_free(&mt9v032->ctrls);
1101         media_entity_cleanup(&subdev->entity);
1102
1103         return 0;
1104 }
1105
1106 static const struct i2c_device_id mt9v032_id[] = {
1107         { "mt9v022", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V022_COLOR] },
1108         { "mt9v022m", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V022_MONO] },
1109         { "mt9v024", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V024_COLOR] },
1110         { "mt9v024m", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V024_MONO] },
1111         { "mt9v032", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V032_COLOR] },
1112         { "mt9v032m", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V032_MONO] },
1113         { "mt9v034", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V034_COLOR] },
1114         { "mt9v034m", (kernel_ulong_t)&mt9v032_models[MT9V032_MODEL_V034_MONO] },
1115         { }
1116 };
1117 MODULE_DEVICE_TABLE(i2c, mt9v032_id);
1118
1119 #if IS_ENABLED(CONFIG_OF)
1120 static const struct of_device_id mt9v032_of_match[] = {
1121         { .compatible = "aptina,mt9v022" },
1122         { .compatible = "aptina,mt9v022m" },
1123         { .compatible = "aptina,mt9v024" },
1124         { .compatible = "aptina,mt9v024m" },
1125         { .compatible = "aptina,mt9v032" },
1126         { .compatible = "aptina,mt9v032m" },
1127         { .compatible = "aptina,mt9v034" },
1128         { .compatible = "aptina,mt9v034m" },
1129         { /* Sentinel */ }
1130 };
1131 MODULE_DEVICE_TABLE(of, mt9v032_of_match);
1132 #endif
1133
1134 static struct i2c_driver mt9v032_driver = {
1135         .driver = {
1136                 .name = "mt9v032",
1137                 .of_match_table = of_match_ptr(mt9v032_of_match),
1138         },
1139         .probe          = mt9v032_probe,
1140         .remove         = mt9v032_remove,
1141         .id_table       = mt9v032_id,
1142 };
1143
1144 module_i2c_driver(mt9v032_driver);
1145
1146 MODULE_DESCRIPTION("Aptina MT9V032 Camera driver");
1147 MODULE_AUTHOR("Laurent Pinchart <laurent.pinchart@ideasonboard.com>");
1148 MODULE_LICENSE("GPL");