drivers/media/i2c/mt9p031.c

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