drivers/media/i2c/mt9p031.c

   1 /*
   2  * Driver for MT9P031 CMOS Image Sensor from Aptina
   3  *
   4  * Copyright (C) 2011, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
   5  * Copyright (C) 2011, Javier Martin <javier.martin@vista-silicon.com>
   6  * Copyright (C) 2011, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
   7  *
   8  * Based on the MT9V032 driver and Bastian Hecht's code.
   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/device.h>
  18 #include <linux/gpio.h>
  19 #include <linux/i2c.h>
  20 #include <linux/log2.h>
  21 #include <linux/module.h>
  22 #include <linux/of.h>
  23 #include <linux/of_gpio.h>
  24 #include <linux/of_graph.h>
  25 #include <linux/pm.h>
  26 #include <linux/regulator/consumer.h>
  27 #include <linux/slab.h>
  28 #include <linux/videodev2.h>
  29
  30 #include <media/mt9p031.h>
  31 #include <media/v4l2-ctrls.h>
  32 #include <media/v4l2-device.h>
  33 #include <media/v4l2-subdev.h>
  34
  35 #include "aptina-pll.h"
  36
  37 #define MT9P031_PIXEL_ARRAY_WIDTH                       2752
  38 #define MT9P031_PIXEL_ARRAY_HEIGHT                      2004
  39
  40 #define MT9P031_CHIP_VERSION                            0x00
  41 #define         MT9P031_CHIP_VERSION_VALUE              0x1801
  42 #define MT9P031_ROW_START                               0x01
  43 #define         MT9P031_ROW_START_MIN                   0
  44 #define         MT9P031_ROW_START_MAX                   2004
  45 #define         MT9P031_ROW_START_DEF                   54
  46 #define MT9P031_COLUMN_START                            0x02
  47 #define         MT9P031_COLUMN_START_MIN                0
  48 #define         MT9P031_COLUMN_START_MAX                2750
  49 #define         MT9P031_COLUMN_START_DEF                16
  50 #define MT9P031_WINDOW_HEIGHT                           0x03
  51 #define         MT9P031_WINDOW_HEIGHT_MIN               2
  52 #define         MT9P031_WINDOW_HEIGHT_MAX               2006
  53 #define         MT9P031_WINDOW_HEIGHT_DEF               1944
  54 #define MT9P031_WINDOW_WIDTH                            0x04
  55 #define         MT9P031_WINDOW_WIDTH_MIN                2
  56 #define         MT9P031_WINDOW_WIDTH_MAX                2752
  57 #define         MT9P031_WINDOW_WIDTH_DEF                2592
  58 #define MT9P031_HORIZONTAL_BLANK                        0x05
  59 #define         MT9P031_HORIZONTAL_BLANK_MIN            0
  60 #define         MT9P031_HORIZONTAL_BLANK_MAX            4095
  61 #define MT9P031_VERTICAL_BLANK                          0x06
  62 #define         MT9P031_VERTICAL_BLANK_MIN              1
  63 #define         MT9P031_VERTICAL_BLANK_MAX              4096
  64 #define         MT9P031_VERTICAL_BLANK_DEF              26
  65 #define MT9P031_OUTPUT_CONTROL                          0x07
  66 #define         MT9P031_OUTPUT_CONTROL_CEN              2
  67 #define         MT9P031_OUTPUT_CONTROL_SYN              1
  68 #define         MT9P031_OUTPUT_CONTROL_DEF              0x1f82
  69 #define MT9P031_SHUTTER_WIDTH_UPPER                     0x08
  70 #define MT9P031_SHUTTER_WIDTH_LOWER                     0x09
  71 #define         MT9P031_SHUTTER_WIDTH_MIN               1
  72 #define         MT9P031_SHUTTER_WIDTH_MAX               1048575
  73 #define         MT9P031_SHUTTER_WIDTH_DEF               1943
  74 #define MT9P031_PLL_CONTROL                             0x10
  75 #define         MT9P031_PLL_CONTROL_PWROFF              0x0050
  76 #define         MT9P031_PLL_CONTROL_PWRON               0x0051
  77 #define         MT9P031_PLL_CONTROL_USEPLL              0x0052
  78 #define MT9P031_PLL_CONFIG_1                            0x11
  79 #define MT9P031_PLL_CONFIG_2                            0x12
  80 #define MT9P031_PIXEL_CLOCK_CONTROL                     0x0a
  81 #define         MT9P031_PIXEL_CLOCK_INVERT              (1 << 15)
  82 #define         MT9P031_PIXEL_CLOCK_SHIFT(n)            ((n) << 8)
  83 #define         MT9P031_PIXEL_CLOCK_DIVIDE(n)           ((n) << 0)
  84 #define MT9P031_FRAME_RESTART                           0x0b
  85 #define MT9P031_SHUTTER_DELAY                           0x0c
  86 #define MT9P031_RST                                     0x0d
  87 #define         MT9P031_RST_ENABLE                      1
  88 #define         MT9P031_RST_DISABLE                     0
  89 #define MT9P031_READ_MODE_1                             0x1e
  90 #define MT9P031_READ_MODE_2                             0x20
  91 #define         MT9P031_READ_MODE_2_ROW_MIR             (1 << 15)
  92 #define         MT9P031_READ_MODE_2_COL_MIR             (1 << 14)
  93 #define         MT9P031_READ_MODE_2_ROW_BLC             (1 << 6)
  94 #define MT9P031_ROW_ADDRESS_MODE                        0x22
  95 #define MT9P031_COLUMN_ADDRESS_MODE                     0x23
  96 #define MT9P031_GLOBAL_GAIN                             0x35
  97 #define         MT9P031_GLOBAL_GAIN_MIN                 8
  98 #define         MT9P031_GLOBAL_GAIN_MAX                 1024
  99 #define         MT9P031_GLOBAL_GAIN_DEF                 8
 100 #define         MT9P031_GLOBAL_GAIN_MULT                (1 << 6)
 101 #define MT9P031_ROW_BLACK_TARGET                        0x49
 102 #define MT9P031_ROW_BLACK_DEF_OFFSET                    0x4b
 103 #define MT9P031_GREEN1_OFFSET                           0x60
 104 #define MT9P031_GREEN2_OFFSET                           0x61
 105 #define MT9P031_BLACK_LEVEL_CALIBRATION                 0x62
 106 #define         MT9P031_BLC_MANUAL_BLC                  (1 << 0)
 107 #define MT9P031_RED_OFFSET                              0x63
 108 #define MT9P031_BLUE_OFFSET                             0x64
 109 #define MT9P031_TEST_PATTERN                            0xa0
 110 #define         MT9P031_TEST_PATTERN_SHIFT              3
 111 #define         MT9P031_TEST_PATTERN_ENABLE             (1 << 0)
 112 #define         MT9P031_TEST_PATTERN_DISABLE            (0 << 0)
 113 #define MT9P031_TEST_PATTERN_GREEN                      0xa1
 114 #define MT9P031_TEST_PATTERN_RED                        0xa2
 115 #define MT9P031_TEST_PATTERN_BLUE                       0xa3
 116
 117 enum mt9p031_model {
 118         MT9P031_MODEL_COLOR,
 119         MT9P031_MODEL_MONOCHROME,
 120 };
 121
 122 struct mt9p031 {
 123         struct v4l2_subdev subdev;
 124         struct media_pad pad;
 125         struct v4l2_rect crop;  /* Sensor window */
 126         struct v4l2_mbus_framefmt format;
 127         struct mt9p031_platform_data *pdata;
 128         struct mutex power_lock; /* lock to protect power_count */
 129         int power_count;
 130
 131         struct clk *clk;
 132         struct regulator_bulk_data regulators[3];
 133
 134         enum mt9p031_model model;
 135         struct aptina_pll pll;
 136         unsigned int clk_div;
 137         bool use_pll;
 138         int reset;
 139
 140         struct v4l2_ctrl_handler ctrls;
 141         struct v4l2_ctrl *blc_auto;
 142         struct v4l2_ctrl *blc_offset;
 143
 144         /* Registers cache */
 145         u16 output_control;
 146         u16 mode2;
 147 };
 148
 149 static struct mt9p031 *to_mt9p031(struct v4l2_subdev *sd)
 150 {
 151         return container_of(sd, struct mt9p031, subdev);
 152 }
 153
 154 static int mt9p031_read(struct i2c_client *client, u8 reg)
 155 {
 156         return i2c_smbus_read_word_swapped(client, reg);
 157 }
 158
 159 static int mt9p031_write(struct i2c_client *client, u8 reg, u16 data)
 160 {
 161         return i2c_smbus_write_word_swapped(client, reg, data);
 162 }
 163
 164 static int mt9p031_set_output_control(struct mt9p031 *mt9p031, u16 clear,
 165                                       u16 set)
 166 {
 167         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 168         u16 value = (mt9p031->output_control & ~clear) | set;
 169         int ret;
 170
 171         ret = mt9p031_write(client, MT9P031_OUTPUT_CONTROL, value);
 172         if (ret < 0)
 173                 return ret;
 174
 175         mt9p031->output_control = value;
 176         return 0;
 177 }
 178
 179 static int mt9p031_set_mode2(struct mt9p031 *mt9p031, u16 clear, u16 set)
 180 {
 181         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 182         u16 value = (mt9p031->mode2 & ~clear) | set;
 183         int ret;
 184
 185         ret = mt9p031_write(client, MT9P031_READ_MODE_2, value);
 186         if (ret < 0)
 187                 return ret;
 188
 189         mt9p031->mode2 = value;
 190         return 0;
 191 }
 192
 193 static int mt9p031_reset(struct mt9p031 *mt9p031)
 194 {
 195         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 196         int ret;
 197
 198         /* Disable chip output, synchronous option update */
 199         ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_ENABLE);
 200         if (ret < 0)
 201                 return ret;
 202         ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_DISABLE);
 203         if (ret < 0)
 204                 return ret;
 205
 206         ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
 207                             MT9P031_PIXEL_CLOCK_DIVIDE(mt9p031->clk_div));
 208         if (ret < 0)
 209                 return ret;
 210
 211         return mt9p031_set_output_control(mt9p031, MT9P031_OUTPUT_CONTROL_CEN,
 212                                           0);
 213 }
 214
 215 static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
 216 {
 217         static const struct aptina_pll_limits limits = {
 218                 .ext_clock_min = 6000000,
 219                 .ext_clock_max = 27000000,
 220                 .int_clock_min = 2000000,
 221                 .int_clock_max = 13500000,
 222                 .out_clock_min = 180000000,
 223                 .out_clock_max = 360000000,
 224                 .pix_clock_max = 96000000,
 225                 .n_min = 1,
 226                 .n_max = 64,
 227                 .m_min = 16,
 228                 .m_max = 255,
 229                 .p1_min = 1,
 230                 .p1_max = 128,
 231         };
 232
 233         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 234         struct mt9p031_platform_data *pdata = mt9p031->pdata;
 235         int ret;
 236
 237         mt9p031->clk = devm_clk_get(&client->dev, NULL);
 238         if (IS_ERR(mt9p031->clk))
 239                 return PTR_ERR(mt9p031->clk);
 240
 241         ret = clk_set_rate(mt9p031->clk, pdata->ext_freq);
 242         if (ret < 0)
 243                 return ret;
 244
 245         /* If the external clock frequency is out of bounds for the PLL use the
 246          * pixel clock divider only and disable the PLL.
 247          */
 248         if (pdata->ext_freq > limits.ext_clock_max) {
 249                 unsigned int div;
 250
 251                 div = DIV_ROUND_UP(pdata->ext_freq, pdata->target_freq);
 252                 div = roundup_pow_of_two(div) / 2;
 253
 254                 mt9p031->clk_div = max_t(unsigned int, div, 64);
 255                 mt9p031->use_pll = false;
 256
 257                 return 0;
 258         }
 259
 260         mt9p031->pll.ext_clock = pdata->ext_freq;
 261         mt9p031->pll.pix_clock = pdata->target_freq;
 262         mt9p031->use_pll = true;
 263
 264         return aptina_pll_calculate(&client->dev, &limits, &mt9p031->pll);
 265 }
 266
 267 static int mt9p031_pll_enable(struct mt9p031 *mt9p031)
 268 {
 269         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 270         int ret;
 271
 272         if (!mt9p031->use_pll)
 273                 return 0;
 274
 275         ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
 276                             MT9P031_PLL_CONTROL_PWRON);
 277         if (ret < 0)
 278                 return ret;
 279
 280         ret = mt9p031_write(client, MT9P031_PLL_CONFIG_1,
 281                             (mt9p031->pll.m << 8) | (mt9p031->pll.n - 1));
 282         if (ret < 0)
 283                 return ret;
 284
 285         ret = mt9p031_write(client, MT9P031_PLL_CONFIG_2, mt9p031->pll.p1 - 1);
 286         if (ret < 0)
 287                 return ret;
 288
 289         usleep_range(1000, 2000);
 290         ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
 291                             MT9P031_PLL_CONTROL_PWRON |
 292                             MT9P031_PLL_CONTROL_USEPLL);
 293         return ret;
 294 }
 295
 296 static inline int mt9p031_pll_disable(struct mt9p031 *mt9p031)
 297 {
 298         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 299
 300         if (!mt9p031->use_pll)
 301                 return 0;
 302
 303         return mt9p031_write(client, MT9P031_PLL_CONTROL,
 304                              MT9P031_PLL_CONTROL_PWROFF);
 305 }
 306
 307 static int mt9p031_power_on(struct mt9p031 *mt9p031)
 308 {
 309         int ret;
 310
 311         /* Ensure RESET_BAR is low */
 312         if (gpio_is_valid(mt9p031->reset)) {
 313                 gpio_set_value(mt9p031->reset, 0);
 314                 usleep_range(1000, 2000);
 315         }
 316
 317         /* Bring up the supplies */
 318         ret = regulator_bulk_enable(ARRAY_SIZE(mt9p031->regulators),
 319                                    mt9p031->regulators);
 320         if (ret < 0)
 321                 return ret;
 322
 323         /* Enable clock */
 324         if (mt9p031->clk) {
 325                 ret = clk_prepare_enable(mt9p031->clk);
 326                 if (ret) {
 327                         regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
 328                                                mt9p031->regulators);
 329                         return ret;
 330                 }
 331         }
 332
 333         /* Now RESET_BAR must be high */
 334         if (gpio_is_valid(mt9p031->reset)) {
 335                 gpio_set_value(mt9p031->reset, 1);
 336                 usleep_range(1000, 2000);
 337         }
 338
 339         return 0;
 340 }
 341
 342 static void mt9p031_power_off(struct mt9p031 *mt9p031)
 343 {
 344         if (gpio_is_valid(mt9p031->reset)) {
 345                 gpio_set_value(mt9p031->reset, 0);
 346                 usleep_range(1000, 2000);
 347         }
 348
 349         regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
 350                                mt9p031->regulators);
 351
 352         if (mt9p031->clk)
 353                 clk_disable_unprepare(mt9p031->clk);
 354 }
 355
 356 static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
 357 {
 358         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 359         int ret;
 360
 361         if (!on) {
 362                 mt9p031_power_off(mt9p031);
 363                 return 0;
 364         }
 365
 366         ret = mt9p031_power_on(mt9p031);
 367         if (ret < 0)
 368                 return ret;
 369
 370         ret = mt9p031_reset(mt9p031);
 371         if (ret < 0) {
 372                 dev_err(&client->dev, "Failed to reset the camera\n");
 373                 return ret;
 374         }
 375
 376         return v4l2_ctrl_handler_setup(&mt9p031->ctrls);
 377 }
 378
 379 /* -----------------------------------------------------------------------------
 380  * V4L2 subdev video operations
 381  */
 382
 383 static int mt9p031_set_params(struct mt9p031 *mt9p031)
 384 {
 385         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 386         struct v4l2_mbus_framefmt *format = &mt9p031->format;
 387         const struct v4l2_rect *crop = &mt9p031->crop;
 388         unsigned int hblank;
 389         unsigned int vblank;
 390         unsigned int xskip;
 391         unsigned int yskip;
 392         unsigned int xbin;
 393         unsigned int ybin;
 394         int ret;
 395
 396         /* Windows position and size.
 397          *
 398          * TODO: Make sure the start coordinates and window size match the
 399          * skipping, binning and mirroring (see description of registers 2 and 4
 400          * in table 13, and Binning section on page 41).
 401          */
 402         ret = mt9p031_write(client, MT9P031_COLUMN_START, crop->left);
 403         if (ret < 0)
 404                 return ret;
 405         ret = mt9p031_write(client, MT9P031_ROW_START, crop->top);
 406         if (ret < 0)
 407                 return ret;
 408         ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, crop->width - 1);
 409         if (ret < 0)
 410                 return ret;
 411         ret = mt9p031_write(client, MT9P031_WINDOW_HEIGHT, crop->height - 1);
 412         if (ret < 0)
 413                 return ret;
 414
 415         /* Row and column binning and skipping. Use the maximum binning value
 416          * compatible with the skipping settings.
 417          */
 418         xskip = DIV_ROUND_CLOSEST(crop->width, format->width);
 419         yskip = DIV_ROUND_CLOSEST(crop->height, format->height);
 420         xbin = 1 << (ffs(xskip) - 1);
 421         ybin = 1 << (ffs(yskip) - 1);
 422
 423         ret = mt9p031_write(client, MT9P031_COLUMN_ADDRESS_MODE,
 424                             ((xbin - 1) << 4) | (xskip - 1));
 425         if (ret < 0)
 426                 return ret;
 427         ret = mt9p031_write(client, MT9P031_ROW_ADDRESS_MODE,
 428                             ((ybin - 1) << 4) | (yskip - 1));
 429         if (ret < 0)
 430                 return ret;
 431
 432         /* Blanking - use minimum value for horizontal blanking and default
 433          * value for vertical blanking.
 434          */
 435         hblank = 346 * ybin + 64 + (80 >> min_t(unsigned int, xbin, 3));
 436         vblank = MT9P031_VERTICAL_BLANK_DEF;
 437
 438         ret = mt9p031_write(client, MT9P031_HORIZONTAL_BLANK, hblank - 1);
 439         if (ret < 0)
 440                 return ret;
 441         ret = mt9p031_write(client, MT9P031_VERTICAL_BLANK, vblank - 1);
 442         if (ret < 0)
 443                 return ret;
 444
 445         return ret;
 446 }
 447
 448 static int mt9p031_s_stream(struct v4l2_subdev *subdev, int enable)
 449 {
 450         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 451         int ret;
 452
 453         if (!enable) {
 454                 /* Stop sensor readout */
 455                 ret = mt9p031_set_output_control(mt9p031,
 456                                                  MT9P031_OUTPUT_CONTROL_CEN, 0);
 457                 if (ret < 0)
 458                         return ret;
 459
 460                 return mt9p031_pll_disable(mt9p031);
 461         }
 462
 463         ret = mt9p031_set_params(mt9p031);
 464         if (ret < 0)
 465                 return ret;
 466
 467         /* Switch to master "normal" mode */
 468         ret = mt9p031_set_output_control(mt9p031, 0,
 469                                          MT9P031_OUTPUT_CONTROL_CEN);
 470         if (ret < 0)
 471                 return ret;
 472
 473         return mt9p031_pll_enable(mt9p031);
 474 }
 475
 476 static int mt9p031_enum_mbus_code(struct v4l2_subdev *subdev,
 477                                   struct v4l2_subdev_fh *fh,
 478                                   struct v4l2_subdev_mbus_code_enum *code)
 479 {
 480         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 481
 482         if (code->pad || code->index)
 483                 return -EINVAL;
 484
 485         code->code = mt9p031->format.code;
 486         return 0;
 487 }
 488
 489 static int mt9p031_enum_frame_size(struct v4l2_subdev *subdev,
 490                                    struct v4l2_subdev_fh *fh,
 491                                    struct v4l2_subdev_frame_size_enum *fse)
 492 {
 493         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 494
 495         if (fse->index >= 8 || fse->code != mt9p031->format.code)
 496                 return -EINVAL;
 497
 498         fse->min_width = MT9P031_WINDOW_WIDTH_DEF
 499                        / min_t(unsigned int, 7, fse->index + 1);
 500         fse->max_width = fse->min_width;
 501         fse->min_height = MT9P031_WINDOW_HEIGHT_DEF / (fse->index + 1);
 502         fse->max_height = fse->min_height;
 503
 504         return 0;
 505 }
 506
 507 static struct v4l2_mbus_framefmt *
 508 __mt9p031_get_pad_format(struct mt9p031 *mt9p031, struct v4l2_subdev_fh *fh,
 509                          unsigned int pad, u32 which)
 510 {
 511         switch (which) {
 512         case V4L2_SUBDEV_FORMAT_TRY:
 513                 return v4l2_subdev_get_try_format(fh, pad);
 514         case V4L2_SUBDEV_FORMAT_ACTIVE:
 515                 return &mt9p031->format;
 516         default:
 517                 return NULL;
 518         }
 519 }
 520
 521 static struct v4l2_rect *
 522 __mt9p031_get_pad_crop(struct mt9p031 *mt9p031, struct v4l2_subdev_fh *fh,
 523                      unsigned int pad, u32 which)
 524 {
 525         switch (which) {
 526         case V4L2_SUBDEV_FORMAT_TRY:
 527                 return v4l2_subdev_get_try_crop(fh, pad);
 528         case V4L2_SUBDEV_FORMAT_ACTIVE:
 529                 return &mt9p031->crop;
 530         default:
 531                 return NULL;
 532         }
 533 }
 534
 535 static int mt9p031_get_format(struct v4l2_subdev *subdev,
 536                               struct v4l2_subdev_fh *fh,
 537                               struct v4l2_subdev_format *fmt)
 538 {
 539         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 540
 541         fmt->format = *__mt9p031_get_pad_format(mt9p031, fh, fmt->pad,
 542                                                 fmt->which);
 543         return 0;
 544 }
 545
 546 static int mt9p031_set_format(struct v4l2_subdev *subdev,
 547                               struct v4l2_subdev_fh *fh,
 548                               struct v4l2_subdev_format *format)
 549 {
 550         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 551         struct v4l2_mbus_framefmt *__format;
 552         struct v4l2_rect *__crop;
 553         unsigned int width;
 554         unsigned int height;
 555         unsigned int hratio;
 556         unsigned int vratio;
 557
 558         __crop = __mt9p031_get_pad_crop(mt9p031, fh, format->pad,
 559                                         format->which);
 560
 561         /* Clamp the width and height to avoid dividing by zero. */
 562         width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
 563                         max_t(unsigned int, __crop->width / 7,
 564                               MT9P031_WINDOW_WIDTH_MIN),
 565                         __crop->width);
 566         height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
 567                          max_t(unsigned int, __crop->height / 8,
 568                                MT9P031_WINDOW_HEIGHT_MIN),
 569                          __crop->height);
 570
 571         hratio = DIV_ROUND_CLOSEST(__crop->width, width);
 572         vratio = DIV_ROUND_CLOSEST(__crop->height, height);
 573
 574         __format = __mt9p031_get_pad_format(mt9p031, fh, format->pad,
 575                                             format->which);
 576         __format->width = __crop->width / hratio;
 577         __format->height = __crop->height / vratio;
 578
 579         format->format = *__format;
 580
 581         return 0;
 582 }
 583
 584 static int mt9p031_get_crop(struct v4l2_subdev *subdev,
 585                             struct v4l2_subdev_fh *fh,
 586                             struct v4l2_subdev_crop *crop)
 587 {
 588         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 589
 590         crop->rect = *__mt9p031_get_pad_crop(mt9p031, fh, crop->pad,
 591                                              crop->which);
 592         return 0;
 593 }
 594
 595 static int mt9p031_set_crop(struct v4l2_subdev *subdev,
 596                             struct v4l2_subdev_fh *fh,
 597                             struct v4l2_subdev_crop *crop)
 598 {
 599         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 600         struct v4l2_mbus_framefmt *__format;
 601         struct v4l2_rect *__crop;
 602         struct v4l2_rect rect;
 603
 604         /* Clamp the crop rectangle boundaries and align them to a multiple of 2
 605          * pixels to ensure a GRBG Bayer pattern.
 606          */
 607         rect.left = clamp(ALIGN(crop->rect.left, 2), MT9P031_COLUMN_START_MIN,
 608                           MT9P031_COLUMN_START_MAX);
 609         rect.top = clamp(ALIGN(crop->rect.top, 2), MT9P031_ROW_START_MIN,
 610                          MT9P031_ROW_START_MAX);
 611         rect.width = clamp_t(unsigned int, ALIGN(crop->rect.width, 2),
 612                              MT9P031_WINDOW_WIDTH_MIN,
 613                              MT9P031_WINDOW_WIDTH_MAX);
 614         rect.height = clamp_t(unsigned int, ALIGN(crop->rect.height, 2),
 615                               MT9P031_WINDOW_HEIGHT_MIN,
 616                               MT9P031_WINDOW_HEIGHT_MAX);
 617
 618         rect.width = min_t(unsigned int, rect.width,
 619                            MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
 620         rect.height = min_t(unsigned int, rect.height,
 621                             MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
 622
 623         __crop = __mt9p031_get_pad_crop(mt9p031, fh, crop->pad, crop->which);
 624
 625         if (rect.width != __crop->width || rect.height != __crop->height) {
 626                 /* Reset the output image size if the crop rectangle size has
 627                  * been modified.
 628                  */
 629                 __format = __mt9p031_get_pad_format(mt9p031, fh, crop->pad,
 630                                                     crop->which);
 631                 __format->width = rect.width;
 632                 __format->height = rect.height;
 633         }
 634
 635         *__crop = rect;
 636         crop->rect = rect;
 637
 638         return 0;
 639 }
 640
 641 /* -----------------------------------------------------------------------------
 642  * V4L2 subdev control operations
 643  */
 644
 645 #define V4L2_CID_BLC_AUTO               (V4L2_CID_USER_BASE | 0x1002)
 646 #define V4L2_CID_BLC_TARGET_LEVEL       (V4L2_CID_USER_BASE | 0x1003)
 647 #define V4L2_CID_BLC_ANALOG_OFFSET      (V4L2_CID_USER_BASE | 0x1004)
 648 #define V4L2_CID_BLC_DIGITAL_OFFSET     (V4L2_CID_USER_BASE | 0x1005)
 649
 650 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 651 {
 652         struct mt9p031 *mt9p031 =
 653                         container_of(ctrl->handler, struct mt9p031, ctrls);
 654         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 655         u16 data;
 656         int ret;
 657
 658         switch (ctrl->id) {
 659         case V4L2_CID_EXPOSURE:
 660                 ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
 661                                     (ctrl->val >> 16) & 0xffff);
 662                 if (ret < 0)
 663                         return ret;
 664
 665                 return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
 666                                      ctrl->val & 0xffff);
 667
 668         case V4L2_CID_GAIN:
 669                 /* Gain is controlled by 2 analog stages and a digital stage.
 670                  * Valid values for the 3 stages are
 671                  *
 672                  * Stage                Min     Max     Step
 673                  * ------------------------------------------
 674                  * First analog stage   x1      x2      1
 675                  * Second analog stage  x1      x4      0.125
 676                  * Digital stage        x1      x16     0.125
 677                  *
 678                  * To minimize noise, the gain stages should be used in the
 679                  * second analog stage, first analog stage, digital stage order.
 680                  * Gain from a previous stage should be pushed to its maximum
 681                  * value before the next stage is used.
 682                  */
 683                 if (ctrl->val <= 32) {
 684                         data = ctrl->val;
 685                 } else if (ctrl->val <= 64) {
 686                         ctrl->val &= ~1;
 687                         data = (1 << 6) | (ctrl->val >> 1);
 688                 } else {
 689                         ctrl->val &= ~7;
 690                         data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
 691                 }
 692
 693                 return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
 694
 695         case V4L2_CID_HFLIP:
 696                 if (ctrl->val)
 697                         return mt9p031_set_mode2(mt9p031,
 698                                         0, MT9P031_READ_MODE_2_COL_MIR);
 699                 else
 700                         return mt9p031_set_mode2(mt9p031,
 701                                         MT9P031_READ_MODE_2_COL_MIR, 0);
 702
 703         case V4L2_CID_VFLIP:
 704                 if (ctrl->val)
 705                         return mt9p031_set_mode2(mt9p031,
 706                                         0, MT9P031_READ_MODE_2_ROW_MIR);
 707                 else
 708                         return mt9p031_set_mode2(mt9p031,
 709                                         MT9P031_READ_MODE_2_ROW_MIR, 0);
 710
 711         case V4L2_CID_TEST_PATTERN:
 712                 if (!ctrl->val) {
 713                         /* Restore the black level compensation settings. */
 714                         if (mt9p031->blc_auto->cur.val != 0) {
 715                                 ret = mt9p031_s_ctrl(mt9p031->blc_auto);
 716                                 if (ret < 0)
 717                                         return ret;
 718                         }
 719                         if (mt9p031->blc_offset->cur.val != 0) {
 720                                 ret = mt9p031_s_ctrl(mt9p031->blc_offset);
 721                                 if (ret < 0)
 722                                         return ret;
 723                         }
 724                         return mt9p031_write(client, MT9P031_TEST_PATTERN,
 725                                              MT9P031_TEST_PATTERN_DISABLE);
 726                 }
 727
 728                 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
 729                 if (ret < 0)
 730                         return ret;
 731                 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
 732                 if (ret < 0)
 733                         return ret;
 734                 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
 735                 if (ret < 0)
 736                         return ret;
 737
 738                 /* Disable digital black level compensation when using a test
 739                  * pattern.
 740                  */
 741                 ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
 742                                         0);
 743                 if (ret < 0)
 744                         return ret;
 745
 746                 ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
 747                 if (ret < 0)
 748                         return ret;
 749
 750                 return mt9p031_write(client, MT9P031_TEST_PATTERN,
 751                                 ((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
 752                                 | MT9P031_TEST_PATTERN_ENABLE);
 753
 754         case V4L2_CID_BLC_AUTO:
 755                 ret = mt9p031_set_mode2(mt9p031,
 756                                 ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
 757                                 ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
 758                 if (ret < 0)
 759                         return ret;
 760
 761                 return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
 762                                      ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
 763
 764         case V4L2_CID_BLC_TARGET_LEVEL:
 765                 return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
 766                                      ctrl->val);
 767
 768         case V4L2_CID_BLC_ANALOG_OFFSET:
 769                 data = ctrl->val & ((1 << 9) - 1);
 770
 771                 ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
 772                 if (ret < 0)
 773                         return ret;
 774                 ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
 775                 if (ret < 0)
 776                         return ret;
 777                 ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
 778                 if (ret < 0)
 779                         return ret;
 780                 return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
 781
 782         case V4L2_CID_BLC_DIGITAL_OFFSET:
 783                 return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
 784                                      ctrl->val & ((1 << 12) - 1));
 785         }
 786
 787         return 0;
 788 }
 789
 790 static struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
 791         .s_ctrl = mt9p031_s_ctrl,
 792 };
 793
 794 static const char * const mt9p031_test_pattern_menu[] = {
 795         "Disabled",
 796         "Color Field",
 797         "Horizontal Gradient",
 798         "Vertical Gradient",
 799         "Diagonal Gradient",
 800         "Classic Test Pattern",
 801         "Walking 1s",
 802         "Monochrome Horizontal Bars",
 803         "Monochrome Vertical Bars",
 804         "Vertical Color Bars",
 805 };
 806
 807 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
 808         {
 809                 .ops            = &mt9p031_ctrl_ops,
 810                 .id             = V4L2_CID_BLC_AUTO,
 811                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 812                 .name           = "BLC, Auto",
 813                 .min            = 0,
 814                 .max            = 1,
 815                 .step           = 1,
 816                 .def            = 1,
 817                 .flags          = 0,
 818         }, {
 819                 .ops            = &mt9p031_ctrl_ops,
 820                 .id             = V4L2_CID_BLC_TARGET_LEVEL,
 821                 .type           = V4L2_CTRL_TYPE_INTEGER,
 822                 .name           = "BLC Target Level",
 823                 .min            = 0,
 824                 .max            = 4095,
 825                 .step           = 1,
 826                 .def            = 168,
 827                 .flags          = 0,
 828         }, {
 829                 .ops            = &mt9p031_ctrl_ops,
 830                 .id             = V4L2_CID_BLC_ANALOG_OFFSET,
 831                 .type           = V4L2_CTRL_TYPE_INTEGER,
 832                 .name           = "BLC Analog Offset",
 833                 .min            = -255,
 834                 .max            = 255,
 835                 .step           = 1,
 836                 .def            = 32,
 837                 .flags          = 0,
 838         }, {
 839                 .ops            = &mt9p031_ctrl_ops,
 840                 .id             = V4L2_CID_BLC_DIGITAL_OFFSET,
 841                 .type           = V4L2_CTRL_TYPE_INTEGER,
 842                 .name           = "BLC Digital Offset",
 843                 .min            = -2048,
 844                 .max            = 2047,
 845                 .step           = 1,
 846                 .def            = 40,
 847                 .flags          = 0,
 848         }
 849 };
 850
 851 /* -----------------------------------------------------------------------------
 852  * V4L2 subdev core operations
 853  */
 854
 855 static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
 856 {
 857         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 858         int ret = 0;
 859
 860         mutex_lock(&mt9p031->power_lock);
 861
 862         /* If the power count is modified from 0 to != 0 or from != 0 to 0,
 863          * update the power state.
 864          */
 865         if (mt9p031->power_count == !on) {
 866                 ret = __mt9p031_set_power(mt9p031, !!on);
 867                 if (ret < 0)
 868                         goto out;
 869         }
 870
 871         /* Update the power count. */
 872         mt9p031->power_count += on ? 1 : -1;
 873         WARN_ON(mt9p031->power_count < 0);
 874
 875 out:
 876         mutex_unlock(&mt9p031->power_lock);
 877         return ret;
 878 }
 879
 880 /* -----------------------------------------------------------------------------
 881  * V4L2 subdev internal operations
 882  */
 883
 884 static int mt9p031_registered(struct v4l2_subdev *subdev)
 885 {
 886         struct i2c_client *client = v4l2_get_subdevdata(subdev);
 887         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 888         s32 data;
 889         int ret;
 890
 891         ret = mt9p031_power_on(mt9p031);
 892         if (ret < 0) {
 893                 dev_err(&client->dev, "MT9P031 power up failed\n");
 894                 return ret;
 895         }
 896
 897         /* Read out the chip version register */
 898         data = mt9p031_read(client, MT9P031_CHIP_VERSION);
 899         mt9p031_power_off(mt9p031);
 900
 901         if (data != MT9P031_CHIP_VERSION_VALUE) {
 902                 dev_err(&client->dev, "MT9P031 not detected, wrong version "
 903                         "0x%04x\n", data);
 904                 return -ENODEV;
 905         }
 906
 907         dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
 908                  client->addr);
 909
 910         return 0;
 911 }
 912
 913 static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 914 {
 915         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 916         struct v4l2_mbus_framefmt *format;
 917         struct v4l2_rect *crop;
 918
 919         crop = v4l2_subdev_get_try_crop(fh, 0);
 920         crop->left = MT9P031_COLUMN_START_DEF;
 921         crop->top = MT9P031_ROW_START_DEF;
 922         crop->width = MT9P031_WINDOW_WIDTH_DEF;
 923         crop->height = MT9P031_WINDOW_HEIGHT_DEF;
 924
 925         format = v4l2_subdev_get_try_format(fh, 0);
 926
 927         if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
 928                 format->code = V4L2_MBUS_FMT_Y12_1X12;
 929         else
 930                 format->code = V4L2_MBUS_FMT_SGRBG12_1X12;
 931
 932         format->width = MT9P031_WINDOW_WIDTH_DEF;
 933         format->height = MT9P031_WINDOW_HEIGHT_DEF;
 934         format->field = V4L2_FIELD_NONE;
 935         format->colorspace = V4L2_COLORSPACE_SRGB;
 936
 937         return mt9p031_set_power(subdev, 1);
 938 }
 939
 940 static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 941 {
 942         return mt9p031_set_power(subdev, 0);
 943 }
 944
 945 static struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
 946         .s_power        = mt9p031_set_power,
 947 };
 948
 949 static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
 950         .s_stream       = mt9p031_s_stream,
 951 };
 952
 953 static struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
 954         .enum_mbus_code = mt9p031_enum_mbus_code,
 955         .enum_frame_size = mt9p031_enum_frame_size,
 956         .get_fmt = mt9p031_get_format,
 957         .set_fmt = mt9p031_set_format,
 958         .get_crop = mt9p031_get_crop,
 959         .set_crop = mt9p031_set_crop,
 960 };
 961
 962 static struct v4l2_subdev_ops mt9p031_subdev_ops = {
 963         .core   = &mt9p031_subdev_core_ops,
 964         .video  = &mt9p031_subdev_video_ops,
 965         .pad    = &mt9p031_subdev_pad_ops,
 966 };
 967
 968 static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
 969         .registered = mt9p031_registered,
 970         .open = mt9p031_open,
 971         .close = mt9p031_close,
 972 };
 973
 974 /* -----------------------------------------------------------------------------
 975  * Driver initialization and probing
 976  */
 977
 978 static struct mt9p031_platform_data *
 979 mt9p031_get_pdata(struct i2c_client *client)
 980 {
 981         struct mt9p031_platform_data *pdata;
 982         struct device_node *np;
 983
 984         if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
 985                 return client->dev.platform_data;
 986
 987         np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
 988         if (!np)
 989                 return NULL;
 990
 991         pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
 992         if (!pdata)
 993                 goto done;
 994
 995         pdata->reset = of_get_named_gpio(client->dev.of_node, "reset-gpios", 0);
 996         of_property_read_u32(np, "input-clock-frequency", &pdata->ext_freq);
 997         of_property_read_u32(np, "pixel-clock-frequency", &pdata->target_freq);
 998
 999 done:
1000         of_node_put(np);
1001         return pdata;
1002 }
1003
1004 static int mt9p031_probe(struct i2c_client *client,
1005                          const struct i2c_device_id *did)
1006 {
1007         struct mt9p031_platform_data *pdata = mt9p031_get_pdata(client);
1008         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
1009         struct mt9p031 *mt9p031;
1010         unsigned int i;
1011         int ret;
1012
1013         if (pdata == NULL) {
1014                 dev_err(&client->dev, "No platform data\n");
1015                 return -EINVAL;
1016         }
1017
1018         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1019                 dev_warn(&client->dev,
1020                         "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1021                 return -EIO;
1022         }
1023
1024         mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
1025         if (mt9p031 == NULL)
1026                 return -ENOMEM;
1027
1028         mt9p031->pdata = pdata;
1029         mt9p031->output_control = MT9P031_OUTPUT_CONTROL_DEF;
1030         mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
1031         mt9p031->model = did->driver_data;
1032         mt9p031->reset = -1;
1033
1034         mt9p031->regulators[0].supply = "vdd";
1035         mt9p031->regulators[1].supply = "vdd_io";
1036         mt9p031->regulators[2].supply = "vaa";
1037
1038         ret = devm_regulator_bulk_get(&client->dev, 3, mt9p031->regulators);
1039         if (ret < 0) {
1040                 dev_err(&client->dev, "Unable to get regulators\n");
1041                 return ret;
1042         }
1043
1044         v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
1045
1046         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1047                           V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
1048                           MT9P031_SHUTTER_WIDTH_MAX, 1,
1049                           MT9P031_SHUTTER_WIDTH_DEF);
1050         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1051                           V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
1052                           MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
1053         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1054                           V4L2_CID_HFLIP, 0, 1, 1, 0);
1055         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1056                           V4L2_CID_VFLIP, 0, 1, 1, 0);
1057         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1058                           V4L2_CID_PIXEL_RATE, pdata->target_freq,
1059                           pdata->target_freq, 1, pdata->target_freq);
1060         v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1061                           V4L2_CID_TEST_PATTERN,
1062                           ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
1063                           0, mt9p031_test_pattern_menu);
1064
1065         for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
1066                 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
1067
1068         mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
1069
1070         if (mt9p031->ctrls.error) {
1071                 printk(KERN_INFO "%s: control initialization error %d\n",
1072                        __func__, mt9p031->ctrls.error);
1073                 ret = mt9p031->ctrls.error;
1074                 goto done;
1075         }
1076
1077         mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
1078         mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
1079                                              V4L2_CID_BLC_DIGITAL_OFFSET);
1080
1081         mutex_init(&mt9p031->power_lock);
1082         v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
1083         mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
1084
1085         mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
1086         ret = media_entity_init(&mt9p031->subdev.entity, 1, &mt9p031->pad, 0);
1087         if (ret < 0)
1088                 goto done;
1089
1090         mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1091
1092         mt9p031->crop.width = MT9P031_WINDOW_WIDTH_DEF;
1093         mt9p031->crop.height = MT9P031_WINDOW_HEIGHT_DEF;
1094         mt9p031->crop.left = MT9P031_COLUMN_START_DEF;
1095         mt9p031->crop.top = MT9P031_ROW_START_DEF;
1096
1097         if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
1098                 mt9p031->format.code = V4L2_MBUS_FMT_Y12_1X12;
1099         else
1100                 mt9p031->format.code = V4L2_MBUS_FMT_SGRBG12_1X12;
1101
1102         mt9p031->format.width = MT9P031_WINDOW_WIDTH_DEF;
1103         mt9p031->format.height = MT9P031_WINDOW_HEIGHT_DEF;
1104         mt9p031->format.field = V4L2_FIELD_NONE;
1105         mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
1106
1107         if (gpio_is_valid(pdata->reset)) {
1108                 ret = devm_gpio_request_one(&client->dev, pdata->reset,
1109                                             GPIOF_OUT_INIT_LOW, "mt9p031_rst");
1110                 if (ret < 0)
1111                         goto done;
1112
1113                 mt9p031->reset = pdata->reset;
1114         }
1115
1116         ret = mt9p031_clk_setup(mt9p031);
1117
1118 done:
1119         if (ret < 0) {
1120                 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1121                 media_entity_cleanup(&mt9p031->subdev.entity);
1122         }
1123
1124         return ret;
1125 }
1126
1127 static int mt9p031_remove(struct i2c_client *client)
1128 {
1129         struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1130         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1131
1132         v4l2_ctrl_handler_free(&mt9p031->ctrls);
1133         v4l2_device_unregister_subdev(subdev);
1134         media_entity_cleanup(&subdev->entity);
1135
1136         return 0;
1137 }
1138
1139 static const struct i2c_device_id mt9p031_id[] = {
1140         { "mt9p031", MT9P031_MODEL_COLOR },
1141         { "mt9p031m", MT9P031_MODEL_MONOCHROME },
1142         { }
1143 };
1144 MODULE_DEVICE_TABLE(i2c, mt9p031_id);
1145
1146 #if IS_ENABLED(CONFIG_OF)
1147 static const struct of_device_id mt9p031_of_match[] = {
1148         { .compatible = "aptina,mt9p031", },
1149         { .compatible = "aptina,mt9p031m", },
1150         { /* sentinel */ },
1151 };
1152 MODULE_DEVICE_TABLE(of, mt9p031_of_match);
1153 #endif
1154
1155 static struct i2c_driver mt9p031_i2c_driver = {
1156         .driver = {
1157                 .of_match_table = of_match_ptr(mt9p031_of_match),
1158                 .name = "mt9p031",
1159         },
1160         .probe          = mt9p031_probe,
1161         .remove         = mt9p031_remove,
1162         .id_table       = mt9p031_id,
1163 };
1164
1165 module_i2c_driver(mt9p031_i2c_driver);
1166
1167 MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
1168 MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
1169 MODULE_LICENSE("GPL v2");