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