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_restore_blc(struct mt9p031 *mt9p031)
 651 {
 652         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 653         int ret;
 654
 655         if (mt9p031->blc_auto->cur.val != 0) {
 656                 ret = mt9p031_set_mode2(mt9p031, 0,
 657                                         MT9P031_READ_MODE_2_ROW_BLC);
 658                 if (ret < 0)
 659                         return ret;
 660         }
 661
 662         if (mt9p031->blc_offset->cur.val != 0) {
 663                 ret = mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
 664                                     mt9p031->blc_offset->cur.val);
 665                 if (ret < 0)
 666                         return ret;
 667         }
 668
 669         return 0;
 670 }
 671
 672 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 673 {
 674         struct mt9p031 *mt9p031 =
 675                         container_of(ctrl->handler, struct mt9p031, ctrls);
 676         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 677         u16 data;
 678         int ret;
 679
 680         if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
 681                 return 0;
 682
 683         switch (ctrl->id) {
 684         case V4L2_CID_EXPOSURE:
 685                 ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
 686                                     (ctrl->val >> 16) & 0xffff);
 687                 if (ret < 0)
 688                         return ret;
 689
 690                 return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
 691                                      ctrl->val & 0xffff);
 692
 693         case V4L2_CID_GAIN:
 694                 /* Gain is controlled by 2 analog stages and a digital stage.
 695                  * Valid values for the 3 stages are
 696                  *
 697                  * Stage                Min     Max     Step
 698                  * ------------------------------------------
 699                  * First analog stage   x1      x2      1
 700                  * Second analog stage  x1      x4      0.125
 701                  * Digital stage        x1      x16     0.125
 702                  *
 703                  * To minimize noise, the gain stages should be used in the
 704                  * second analog stage, first analog stage, digital stage order.
 705                  * Gain from a previous stage should be pushed to its maximum
 706                  * value before the next stage is used.
 707                  */
 708                 if (ctrl->val <= 32) {
 709                         data = ctrl->val;
 710                 } else if (ctrl->val <= 64) {
 711                         ctrl->val &= ~1;
 712                         data = (1 << 6) | (ctrl->val >> 1);
 713                 } else {
 714                         ctrl->val &= ~7;
 715                         data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
 716                 }
 717
 718                 return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
 719
 720         case V4L2_CID_HFLIP:
 721                 if (ctrl->val)
 722                         return mt9p031_set_mode2(mt9p031,
 723                                         0, MT9P031_READ_MODE_2_COL_MIR);
 724                 else
 725                         return mt9p031_set_mode2(mt9p031,
 726                                         MT9P031_READ_MODE_2_COL_MIR, 0);
 727
 728         case V4L2_CID_VFLIP:
 729                 if (ctrl->val)
 730                         return mt9p031_set_mode2(mt9p031,
 731                                         0, MT9P031_READ_MODE_2_ROW_MIR);
 732                 else
 733                         return mt9p031_set_mode2(mt9p031,
 734                                         MT9P031_READ_MODE_2_ROW_MIR, 0);
 735
 736         case V4L2_CID_TEST_PATTERN:
 737                 /* The digital side of the Black Level Calibration function must
 738                  * be disabled when generating a test pattern to avoid artifacts
 739                  * in the image. Activate (deactivate) the BLC-related controls
 740                  * when the test pattern is enabled (disabled).
 741                  */
 742                 v4l2_ctrl_activate(mt9p031->blc_auto, ctrl->val == 0);
 743                 v4l2_ctrl_activate(mt9p031->blc_offset, ctrl->val == 0);
 744
 745                 if (!ctrl->val) {
 746                         /* Restore the BLC settings. */
 747                         ret = mt9p031_restore_blc(mt9p031);
 748                         if (ret < 0)
 749                                 return ret;
 750
 751                         return mt9p031_write(client, MT9P031_TEST_PATTERN,
 752                                              MT9P031_TEST_PATTERN_DISABLE);
 753                 }
 754
 755                 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
 756                 if (ret < 0)
 757                         return ret;
 758                 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
 759                 if (ret < 0)
 760                         return ret;
 761                 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
 762                 if (ret < 0)
 763                         return ret;
 764
 765                 /* Disable digital BLC when generating a test pattern. */
 766                 ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
 767                                         0);
 768                 if (ret < 0)
 769                         return ret;
 770
 771                 ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
 772                 if (ret < 0)
 773                         return ret;
 774
 775                 return mt9p031_write(client, MT9P031_TEST_PATTERN,
 776                                 ((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
 777                                 | MT9P031_TEST_PATTERN_ENABLE);
 778
 779         case V4L2_CID_BLC_AUTO:
 780                 ret = mt9p031_set_mode2(mt9p031,
 781                                 ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
 782                                 ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
 783                 if (ret < 0)
 784                         return ret;
 785
 786                 return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
 787                                      ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
 788
 789         case V4L2_CID_BLC_TARGET_LEVEL:
 790                 return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
 791                                      ctrl->val);
 792
 793         case V4L2_CID_BLC_ANALOG_OFFSET:
 794                 data = ctrl->val & ((1 << 9) - 1);
 795
 796                 ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
 797                 if (ret < 0)
 798                         return ret;
 799                 ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
 800                 if (ret < 0)
 801                         return ret;
 802                 ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
 803                 if (ret < 0)
 804                         return ret;
 805                 return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
 806
 807         case V4L2_CID_BLC_DIGITAL_OFFSET:
 808                 return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
 809                                      ctrl->val & ((1 << 12) - 1));
 810         }
 811
 812         return 0;
 813 }
 814
 815 static struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
 816         .s_ctrl = mt9p031_s_ctrl,
 817 };
 818
 819 static const char * const mt9p031_test_pattern_menu[] = {
 820         "Disabled",
 821         "Color Field",
 822         "Horizontal Gradient",
 823         "Vertical Gradient",
 824         "Diagonal Gradient",
 825         "Classic Test Pattern",
 826         "Walking 1s",
 827         "Monochrome Horizontal Bars",
 828         "Monochrome Vertical Bars",
 829         "Vertical Color Bars",
 830 };
 831
 832 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
 833         {
 834                 .ops            = &mt9p031_ctrl_ops,
 835                 .id             = V4L2_CID_BLC_AUTO,
 836                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 837                 .name           = "BLC, Auto",
 838                 .min            = 0,
 839                 .max            = 1,
 840                 .step           = 1,
 841                 .def            = 1,
 842                 .flags          = 0,
 843         }, {
 844                 .ops            = &mt9p031_ctrl_ops,
 845                 .id             = V4L2_CID_BLC_TARGET_LEVEL,
 846                 .type           = V4L2_CTRL_TYPE_INTEGER,
 847                 .name           = "BLC Target Level",
 848                 .min            = 0,
 849                 .max            = 4095,
 850                 .step           = 1,
 851                 .def            = 168,
 852                 .flags          = 0,
 853         }, {
 854                 .ops            = &mt9p031_ctrl_ops,
 855                 .id             = V4L2_CID_BLC_ANALOG_OFFSET,
 856                 .type           = V4L2_CTRL_TYPE_INTEGER,
 857                 .name           = "BLC Analog Offset",
 858                 .min            = -255,
 859                 .max            = 255,
 860                 .step           = 1,
 861                 .def            = 32,
 862                 .flags          = 0,
 863         }, {
 864                 .ops            = &mt9p031_ctrl_ops,
 865                 .id             = V4L2_CID_BLC_DIGITAL_OFFSET,
 866                 .type           = V4L2_CTRL_TYPE_INTEGER,
 867                 .name           = "BLC Digital Offset",
 868                 .min            = -2048,
 869                 .max            = 2047,
 870                 .step           = 1,
 871                 .def            = 40,
 872                 .flags          = 0,
 873         }
 874 };
 875
 876 /* -----------------------------------------------------------------------------
 877  * V4L2 subdev core operations
 878  */
 879
 880 static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
 881 {
 882         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 883         int ret = 0;
 884
 885         mutex_lock(&mt9p031->power_lock);
 886
 887         /* If the power count is modified from 0 to != 0 or from != 0 to 0,
 888          * update the power state.
 889          */
 890         if (mt9p031->power_count == !on) {
 891                 ret = __mt9p031_set_power(mt9p031, !!on);
 892                 if (ret < 0)
 893                         goto out;
 894         }
 895
 896         /* Update the power count. */
 897         mt9p031->power_count += on ? 1 : -1;
 898         WARN_ON(mt9p031->power_count < 0);
 899
 900 out:
 901         mutex_unlock(&mt9p031->power_lock);
 902         return ret;
 903 }
 904
 905 /* -----------------------------------------------------------------------------
 906  * V4L2 subdev internal operations
 907  */
 908
 909 static int mt9p031_registered(struct v4l2_subdev *subdev)
 910 {
 911         struct i2c_client *client = v4l2_get_subdevdata(subdev);
 912         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 913         s32 data;
 914         int ret;
 915
 916         ret = mt9p031_power_on(mt9p031);
 917         if (ret < 0) {
 918                 dev_err(&client->dev, "MT9P031 power up failed\n");
 919                 return ret;
 920         }
 921
 922         /* Read out the chip version register */
 923         data = mt9p031_read(client, MT9P031_CHIP_VERSION);
 924         mt9p031_power_off(mt9p031);
 925
 926         if (data != MT9P031_CHIP_VERSION_VALUE) {
 927                 dev_err(&client->dev, "MT9P031 not detected, wrong version "
 928                         "0x%04x\n", data);
 929                 return -ENODEV;
 930         }
 931
 932         dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
 933                  client->addr);
 934
 935         return 0;
 936 }
 937
 938 static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 939 {
 940         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 941         struct v4l2_mbus_framefmt *format;
 942         struct v4l2_rect *crop;
 943
 944         crop = v4l2_subdev_get_try_crop(fh, 0);
 945         crop->left = MT9P031_COLUMN_START_DEF;
 946         crop->top = MT9P031_ROW_START_DEF;
 947         crop->width = MT9P031_WINDOW_WIDTH_DEF;
 948         crop->height = MT9P031_WINDOW_HEIGHT_DEF;
 949
 950         format = v4l2_subdev_get_try_format(fh, 0);
 951
 952         if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
 953                 format->code = V4L2_MBUS_FMT_Y12_1X12;
 954         else
 955                 format->code = V4L2_MBUS_FMT_SGRBG12_1X12;
 956
 957         format->width = MT9P031_WINDOW_WIDTH_DEF;
 958         format->height = MT9P031_WINDOW_HEIGHT_DEF;
 959         format->field = V4L2_FIELD_NONE;
 960         format->colorspace = V4L2_COLORSPACE_SRGB;
 961
 962         return mt9p031_set_power(subdev, 1);
 963 }
 964
 965 static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 966 {
 967         return mt9p031_set_power(subdev, 0);
 968 }
 969
 970 static struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
 971         .s_power        = mt9p031_set_power,
 972 };
 973
 974 static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
 975         .s_stream       = mt9p031_s_stream,
 976 };
 977
 978 static struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
 979         .enum_mbus_code = mt9p031_enum_mbus_code,
 980         .enum_frame_size = mt9p031_enum_frame_size,
 981         .get_fmt = mt9p031_get_format,
 982         .set_fmt = mt9p031_set_format,
 983         .get_crop = mt9p031_get_crop,
 984         .set_crop = mt9p031_set_crop,
 985 };
 986
 987 static struct v4l2_subdev_ops mt9p031_subdev_ops = {
 988         .core   = &mt9p031_subdev_core_ops,
 989         .video  = &mt9p031_subdev_video_ops,
 990         .pad    = &mt9p031_subdev_pad_ops,
 991 };
 992
 993 static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
 994         .registered = mt9p031_registered,
 995         .open = mt9p031_open,
 996         .close = mt9p031_close,
 997 };
 998
 999 /* -----------------------------------------------------------------------------
1000  * Driver initialization and probing
1001  */
1002
1003 static struct mt9p031_platform_data *
1004 mt9p031_get_pdata(struct i2c_client *client)
1005 {
1006         struct mt9p031_platform_data *pdata;
1007         struct device_node *np;
1008
1009         if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
1010                 return client->dev.platform_data;
1011
1012         np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
1013         if (!np)
1014                 return NULL;
1015
1016         pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
1017         if (!pdata)
1018                 goto done;
1019
1020         pdata->reset = of_get_named_gpio(client->dev.of_node, "reset-gpios", 0);
1021         of_property_read_u32(np, "input-clock-frequency", &pdata->ext_freq);
1022         of_property_read_u32(np, "pixel-clock-frequency", &pdata->target_freq);
1023
1024 done:
1025         of_node_put(np);
1026         return pdata;
1027 }
1028
1029 static int mt9p031_probe(struct i2c_client *client,
1030                          const struct i2c_device_id *did)
1031 {
1032         struct mt9p031_platform_data *pdata = mt9p031_get_pdata(client);
1033         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
1034         struct mt9p031 *mt9p031;
1035         unsigned int i;
1036         int ret;
1037
1038         if (pdata == NULL) {
1039                 dev_err(&client->dev, "No platform data\n");
1040                 return -EINVAL;
1041         }
1042
1043         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1044                 dev_warn(&client->dev,
1045                         "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1046                 return -EIO;
1047         }
1048
1049         mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
1050         if (mt9p031 == NULL)
1051                 return -ENOMEM;
1052
1053         mt9p031->pdata = pdata;
1054         mt9p031->output_control = MT9P031_OUTPUT_CONTROL_DEF;
1055         mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
1056         mt9p031->model = did->driver_data;
1057         mt9p031->reset = -1;
1058
1059         mt9p031->regulators[0].supply = "vdd";
1060         mt9p031->regulators[1].supply = "vdd_io";
1061         mt9p031->regulators[2].supply = "vaa";
1062
1063         ret = devm_regulator_bulk_get(&client->dev, 3, mt9p031->regulators);
1064         if (ret < 0) {
1065                 dev_err(&client->dev, "Unable to get regulators\n");
1066                 return ret;
1067         }
1068
1069         v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
1070
1071         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1072                           V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
1073                           MT9P031_SHUTTER_WIDTH_MAX, 1,
1074                           MT9P031_SHUTTER_WIDTH_DEF);
1075         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1076                           V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
1077                           MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
1078         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1079                           V4L2_CID_HFLIP, 0, 1, 1, 0);
1080         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1081                           V4L2_CID_VFLIP, 0, 1, 1, 0);
1082         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1083                           V4L2_CID_PIXEL_RATE, pdata->target_freq,
1084                           pdata->target_freq, 1, pdata->target_freq);
1085         v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1086                           V4L2_CID_TEST_PATTERN,
1087                           ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
1088                           0, mt9p031_test_pattern_menu);
1089
1090         for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
1091                 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
1092
1093         mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
1094
1095         if (mt9p031->ctrls.error) {
1096                 printk(KERN_INFO "%s: control initialization error %d\n",
1097                        __func__, mt9p031->ctrls.error);
1098                 ret = mt9p031->ctrls.error;
1099                 goto done;
1100         }
1101
1102         mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
1103         mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
1104                                              V4L2_CID_BLC_DIGITAL_OFFSET);
1105
1106         mutex_init(&mt9p031->power_lock);
1107         v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
1108         mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
1109
1110         mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
1111         ret = media_entity_init(&mt9p031->subdev.entity, 1, &mt9p031->pad, 0);
1112         if (ret < 0)
1113                 goto done;
1114
1115         mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1116
1117         mt9p031->crop.width = MT9P031_WINDOW_WIDTH_DEF;
1118         mt9p031->crop.height = MT9P031_WINDOW_HEIGHT_DEF;
1119         mt9p031->crop.left = MT9P031_COLUMN_START_DEF;
1120         mt9p031->crop.top = MT9P031_ROW_START_DEF;
1121
1122         if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
1123                 mt9p031->format.code = V4L2_MBUS_FMT_Y12_1X12;
1124         else
1125                 mt9p031->format.code = V4L2_MBUS_FMT_SGRBG12_1X12;
1126
1127         mt9p031->format.width = MT9P031_WINDOW_WIDTH_DEF;
1128         mt9p031->format.height = MT9P031_WINDOW_HEIGHT_DEF;
1129         mt9p031->format.field = V4L2_FIELD_NONE;
1130         mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
1131
1132         if (gpio_is_valid(pdata->reset)) {
1133                 ret = devm_gpio_request_one(&client->dev, pdata->reset,
1134                                             GPIOF_OUT_INIT_LOW, "mt9p031_rst");
1135                 if (ret < 0)
1136                         goto done;
1137
1138                 mt9p031->reset = pdata->reset;
1139         }
1140
1141         ret = mt9p031_clk_setup(mt9p031);
1142
1143 done:
1144         if (ret < 0) {
1145                 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1146                 media_entity_cleanup(&mt9p031->subdev.entity);
1147         }
1148
1149         return ret;
1150 }
1151
1152 static int mt9p031_remove(struct i2c_client *client)
1153 {
1154         struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1155         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1156
1157         v4l2_ctrl_handler_free(&mt9p031->ctrls);
1158         v4l2_device_unregister_subdev(subdev);
1159         media_entity_cleanup(&subdev->entity);
1160
1161         return 0;
1162 }
1163
1164 static const struct i2c_device_id mt9p031_id[] = {
1165         { "mt9p031", MT9P031_MODEL_COLOR },
1166         { "mt9p031m", MT9P031_MODEL_MONOCHROME },
1167         { }
1168 };
1169 MODULE_DEVICE_TABLE(i2c, mt9p031_id);
1170
1171 #if IS_ENABLED(CONFIG_OF)
1172 static const struct of_device_id mt9p031_of_match[] = {
1173         { .compatible = "aptina,mt9p031", },
1174         { .compatible = "aptina,mt9p031m", },
1175         { /* sentinel */ },
1176 };
1177 MODULE_DEVICE_TABLE(of, mt9p031_of_match);
1178 #endif
1179
1180 static struct i2c_driver mt9p031_i2c_driver = {
1181         .driver = {
1182                 .of_match_table = of_match_ptr(mt9p031_of_match),
1183                 .name = "mt9p031",
1184         },
1185         .probe          = mt9p031_probe,
1186         .remove         = mt9p031_remove,
1187         .id_table       = mt9p031_id,
1188 };
1189
1190 module_i2c_driver(mt9p031_i2c_driver);
1191
1192 MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
1193 MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
1194 MODULE_LICENSE("GPL v2");