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         if (mt9p031->clk)
 350                 clk_disable_unprepare(mt9p031->clk);
 351 }
 352
 353 static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
 354 {
 355         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 356         int ret;
 357
 358         if (!on) {
 359                 mt9p031_power_off(mt9p031);
 360                 return 0;
 361         }
 362
 363         ret = mt9p031_power_on(mt9p031);
 364         if (ret < 0)
 365                 return ret;
 366
 367         ret = mt9p031_reset(mt9p031);
 368         if (ret < 0) {
 369                 dev_err(&client->dev, "Failed to reset the camera\n");
 370                 return ret;
 371         }
 372
 373         return v4l2_ctrl_handler_setup(&mt9p031->ctrls);
 374 }
 375
 376 /* -----------------------------------------------------------------------------
 377  * V4L2 subdev video operations
 378  */
 379
 380 static int mt9p031_set_params(struct mt9p031 *mt9p031)
 381 {
 382         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 383         struct v4l2_mbus_framefmt *format = &mt9p031->format;
 384         const struct v4l2_rect *crop = &mt9p031->crop;
 385         unsigned int hblank;
 386         unsigned int vblank;
 387         unsigned int xskip;
 388         unsigned int yskip;
 389         unsigned int xbin;
 390         unsigned int ybin;
 391         int ret;
 392
 393         /* Windows position and size.
 394          *
 395          * TODO: Make sure the start coordinates and window size match the
 396          * skipping, binning and mirroring (see description of registers 2 and 4
 397          * in table 13, and Binning section on page 41).
 398          */
 399         ret = mt9p031_write(client, MT9P031_COLUMN_START, crop->left);
 400         if (ret < 0)
 401                 return ret;
 402         ret = mt9p031_write(client, MT9P031_ROW_START, crop->top);
 403         if (ret < 0)
 404                 return ret;
 405         ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, crop->width - 1);
 406         if (ret < 0)
 407                 return ret;
 408         ret = mt9p031_write(client, MT9P031_WINDOW_HEIGHT, crop->height - 1);
 409         if (ret < 0)
 410                 return ret;
 411
 412         /* Row and column binning and skipping. Use the maximum binning value
 413          * compatible with the skipping settings.
 414          */
 415         xskip = DIV_ROUND_CLOSEST(crop->width, format->width);
 416         yskip = DIV_ROUND_CLOSEST(crop->height, format->height);
 417         xbin = 1 << (ffs(xskip) - 1);
 418         ybin = 1 << (ffs(yskip) - 1);
 419
 420         ret = mt9p031_write(client, MT9P031_COLUMN_ADDRESS_MODE,
 421                             ((xbin - 1) << 4) | (xskip - 1));
 422         if (ret < 0)
 423                 return ret;
 424         ret = mt9p031_write(client, MT9P031_ROW_ADDRESS_MODE,
 425                             ((ybin - 1) << 4) | (yskip - 1));
 426         if (ret < 0)
 427                 return ret;
 428
 429         /* Blanking - use minimum value for horizontal blanking and default
 430          * value for vertical blanking.
 431          */
 432         hblank = 346 * ybin + 64 + (80 >> min_t(unsigned int, xbin, 3));
 433         vblank = MT9P031_VERTICAL_BLANK_DEF;
 434
 435         ret = mt9p031_write(client, MT9P031_HORIZONTAL_BLANK, hblank - 1);
 436         if (ret < 0)
 437                 return ret;
 438         ret = mt9p031_write(client, MT9P031_VERTICAL_BLANK, vblank - 1);
 439         if (ret < 0)
 440                 return ret;
 441
 442         return ret;
 443 }
 444
 445 static int mt9p031_s_stream(struct v4l2_subdev *subdev, int enable)
 446 {
 447         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 448         int ret;
 449
 450         if (!enable) {
 451                 /* Stop sensor readout */
 452                 ret = mt9p031_set_output_control(mt9p031,
 453                                                  MT9P031_OUTPUT_CONTROL_CEN, 0);
 454                 if (ret < 0)
 455                         return ret;
 456
 457                 return mt9p031_pll_disable(mt9p031);
 458         }
 459
 460         ret = mt9p031_set_params(mt9p031);
 461         if (ret < 0)
 462                 return ret;
 463
 464         /* Switch to master "normal" mode */
 465         ret = mt9p031_set_output_control(mt9p031, 0,
 466                                          MT9P031_OUTPUT_CONTROL_CEN);
 467         if (ret < 0)
 468                 return ret;
 469
 470         return mt9p031_pll_enable(mt9p031);
 471 }
 472
 473 static int mt9p031_enum_mbus_code(struct v4l2_subdev *subdev,
 474                                   struct v4l2_subdev_pad_config *cfg,
 475                                   struct v4l2_subdev_mbus_code_enum *code)
 476 {
 477         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 478
 479         if (code->pad || code->index)
 480                 return -EINVAL;
 481
 482         code->code = mt9p031->format.code;
 483         return 0;
 484 }
 485
 486 static int mt9p031_enum_frame_size(struct v4l2_subdev *subdev,
 487                                    struct v4l2_subdev_pad_config *cfg,
 488                                    struct v4l2_subdev_frame_size_enum *fse)
 489 {
 490         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 491
 492         if (fse->index >= 8 || fse->code != mt9p031->format.code)
 493                 return -EINVAL;
 494
 495         fse->min_width = MT9P031_WINDOW_WIDTH_DEF
 496                        / min_t(unsigned int, 7, fse->index + 1);
 497         fse->max_width = fse->min_width;
 498         fse->min_height = MT9P031_WINDOW_HEIGHT_DEF / (fse->index + 1);
 499         fse->max_height = fse->min_height;
 500
 501         return 0;
 502 }
 503
 504 static struct v4l2_mbus_framefmt *
 505 __mt9p031_get_pad_format(struct mt9p031 *mt9p031, struct v4l2_subdev_pad_config *cfg,
 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, cfg, pad);
 511         case V4L2_SUBDEV_FORMAT_ACTIVE:
 512                 return &mt9p031->format;
 513         default:
 514                 return NULL;
 515         }
 516 }
 517
 518 static struct v4l2_rect *
 519 __mt9p031_get_pad_crop(struct mt9p031 *mt9p031, struct v4l2_subdev_pad_config *cfg,
 520                      unsigned int pad, u32 which)
 521 {
 522         switch (which) {
 523         case V4L2_SUBDEV_FORMAT_TRY:
 524                 return v4l2_subdev_get_try_crop(&mt9p031->subdev, cfg, pad);
 525         case V4L2_SUBDEV_FORMAT_ACTIVE:
 526                 return &mt9p031->crop;
 527         default:
 528                 return NULL;
 529         }
 530 }
 531
 532 static int mt9p031_get_format(struct v4l2_subdev *subdev,
 533                               struct v4l2_subdev_pad_config *cfg,
 534                               struct v4l2_subdev_format *fmt)
 535 {
 536         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 537
 538         fmt->format = *__mt9p031_get_pad_format(mt9p031, cfg, fmt->pad,
 539                                                 fmt->which);
 540         return 0;
 541 }
 542
 543 static int mt9p031_set_format(struct v4l2_subdev *subdev,
 544                               struct v4l2_subdev_pad_config *cfg,
 545                               struct v4l2_subdev_format *format)
 546 {
 547         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 548         struct v4l2_mbus_framefmt *__format;
 549         struct v4l2_rect *__crop;
 550         unsigned int width;
 551         unsigned int height;
 552         unsigned int hratio;
 553         unsigned int vratio;
 554
 555         __crop = __mt9p031_get_pad_crop(mt9p031, cfg, format->pad,
 556                                         format->which);
 557
 558         /* Clamp the width and height to avoid dividing by zero. */
 559         width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
 560                         max_t(unsigned int, __crop->width / 7,
 561                               MT9P031_WINDOW_WIDTH_MIN),
 562                         __crop->width);
 563         height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
 564                          max_t(unsigned int, __crop->height / 8,
 565                                MT9P031_WINDOW_HEIGHT_MIN),
 566                          __crop->height);
 567
 568         hratio = DIV_ROUND_CLOSEST(__crop->width, width);
 569         vratio = DIV_ROUND_CLOSEST(__crop->height, height);
 570
 571         __format = __mt9p031_get_pad_format(mt9p031, cfg, format->pad,
 572                                             format->which);
 573         __format->width = __crop->width / hratio;
 574         __format->height = __crop->height / vratio;
 575
 576         format->format = *__format;
 577
 578         return 0;
 579 }
 580
 581 static int mt9p031_get_selection(struct v4l2_subdev *subdev,
 582                                  struct v4l2_subdev_pad_config *cfg,
 583                                  struct v4l2_subdev_selection *sel)
 584 {
 585         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 586
 587         if (sel->target != V4L2_SEL_TGT_CROP)
 588                 return -EINVAL;
 589
 590         sel->r = *__mt9p031_get_pad_crop(mt9p031, cfg, sel->pad, sel->which);
 591         return 0;
 592 }
 593
 594 static int mt9p031_set_selection(struct v4l2_subdev *subdev,
 595                                  struct v4l2_subdev_pad_config *cfg,
 596                                  struct v4l2_subdev_selection *sel)
 597 {
 598         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 599         struct v4l2_mbus_framefmt *__format;
 600         struct v4l2_rect *__crop;
 601         struct v4l2_rect rect;
 602
 603         if (sel->target != V4L2_SEL_TGT_CROP)
 604                 return -EINVAL;
 605
 606         /* Clamp the crop rectangle boundaries and align them to a multiple of 2
 607          * pixels to ensure a GRBG Bayer pattern.
 608          */
 609         rect.left = clamp(ALIGN(sel->r.left, 2), MT9P031_COLUMN_START_MIN,
 610                           MT9P031_COLUMN_START_MAX);
 611         rect.top = clamp(ALIGN(sel->r.top, 2), MT9P031_ROW_START_MIN,
 612                          MT9P031_ROW_START_MAX);
 613         rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 2),
 614                              MT9P031_WINDOW_WIDTH_MIN,
 615                              MT9P031_WINDOW_WIDTH_MAX);
 616         rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 2),
 617                               MT9P031_WINDOW_HEIGHT_MIN,
 618                               MT9P031_WINDOW_HEIGHT_MAX);
 619
 620         rect.width = min_t(unsigned int, rect.width,
 621                            MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
 622         rect.height = min_t(unsigned int, rect.height,
 623                             MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
 624
 625         __crop = __mt9p031_get_pad_crop(mt9p031, cfg, sel->pad, sel->which);
 626
 627         if (rect.width != __crop->width || rect.height != __crop->height) {
 628                 /* Reset the output image size if the crop rectangle size has
 629                  * been modified.
 630                  */
 631                 __format = __mt9p031_get_pad_format(mt9p031, cfg, sel->pad,
 632                                                     sel->which);
 633                 __format->width = rect.width;
 634                 __format->height = rect.height;
 635         }
 636
 637         *__crop = rect;
 638         sel->r = rect;
 639
 640         return 0;
 641 }
 642
 643 /* -----------------------------------------------------------------------------
 644  * V4L2 subdev control operations
 645  */
 646
 647 #define V4L2_CID_BLC_AUTO               (V4L2_CID_USER_BASE | 0x1002)
 648 #define V4L2_CID_BLC_TARGET_LEVEL       (V4L2_CID_USER_BASE | 0x1003)
 649 #define V4L2_CID_BLC_ANALOG_OFFSET      (V4L2_CID_USER_BASE | 0x1004)
 650 #define V4L2_CID_BLC_DIGITAL_OFFSET     (V4L2_CID_USER_BASE | 0x1005)
 651
 652 static int mt9p031_restore_blc(struct mt9p031 *mt9p031)
 653 {
 654         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 655         int ret;
 656
 657         if (mt9p031->blc_auto->cur.val != 0) {
 658                 ret = mt9p031_set_mode2(mt9p031, 0,
 659                                         MT9P031_READ_MODE_2_ROW_BLC);
 660                 if (ret < 0)
 661                         return ret;
 662         }
 663
 664         if (mt9p031->blc_offset->cur.val != 0) {
 665                 ret = mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
 666                                     mt9p031->blc_offset->cur.val);
 667                 if (ret < 0)
 668                         return ret;
 669         }
 670
 671         return 0;
 672 }
 673
 674 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 675 {
 676         struct mt9p031 *mt9p031 =
 677                         container_of(ctrl->handler, struct mt9p031, ctrls);
 678         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 679         u16 data;
 680         int ret;
 681
 682         if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
 683                 return 0;
 684
 685         switch (ctrl->id) {
 686         case V4L2_CID_EXPOSURE:
 687                 ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
 688                                     (ctrl->val >> 16) & 0xffff);
 689                 if (ret < 0)
 690                         return ret;
 691
 692                 return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
 693                                      ctrl->val & 0xffff);
 694
 695         case V4L2_CID_GAIN:
 696                 /* Gain is controlled by 2 analog stages and a digital stage.
 697                  * Valid values for the 3 stages are
 698                  *
 699                  * Stage                Min     Max     Step
 700                  * ------------------------------------------
 701                  * First analog stage   x1      x2      1
 702                  * Second analog stage  x1      x4      0.125
 703                  * Digital stage        x1      x16     0.125
 704                  *
 705                  * To minimize noise, the gain stages should be used in the
 706                  * second analog stage, first analog stage, digital stage order.
 707                  * Gain from a previous stage should be pushed to its maximum
 708                  * value before the next stage is used.
 709                  */
 710                 if (ctrl->val <= 32) {
 711                         data = ctrl->val;
 712                 } else if (ctrl->val <= 64) {
 713                         ctrl->val &= ~1;
 714                         data = (1 << 6) | (ctrl->val >> 1);
 715                 } else {
 716                         ctrl->val &= ~7;
 717                         data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
 718                 }
 719
 720                 return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
 721
 722         case V4L2_CID_HFLIP:
 723                 if (ctrl->val)
 724                         return mt9p031_set_mode2(mt9p031,
 725                                         0, MT9P031_READ_MODE_2_COL_MIR);
 726                 else
 727                         return mt9p031_set_mode2(mt9p031,
 728                                         MT9P031_READ_MODE_2_COL_MIR, 0);
 729
 730         case V4L2_CID_VFLIP:
 731                 if (ctrl->val)
 732                         return mt9p031_set_mode2(mt9p031,
 733                                         0, MT9P031_READ_MODE_2_ROW_MIR);
 734                 else
 735                         return mt9p031_set_mode2(mt9p031,
 736                                         MT9P031_READ_MODE_2_ROW_MIR, 0);
 737
 738         case V4L2_CID_TEST_PATTERN:
 739                 /* The digital side of the Black Level Calibration function must
 740                  * be disabled when generating a test pattern to avoid artifacts
 741                  * in the image. Activate (deactivate) the BLC-related controls
 742                  * when the test pattern is enabled (disabled).
 743                  */
 744                 v4l2_ctrl_activate(mt9p031->blc_auto, ctrl->val == 0);
 745                 v4l2_ctrl_activate(mt9p031->blc_offset, ctrl->val == 0);
 746
 747                 if (!ctrl->val) {
 748                         /* Restore the BLC settings. */
 749                         ret = mt9p031_restore_blc(mt9p031);
 750                         if (ret < 0)
 751                                 return ret;
 752
 753                         return mt9p031_write(client, MT9P031_TEST_PATTERN,
 754                                              MT9P031_TEST_PATTERN_DISABLE);
 755                 }
 756
 757                 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
 758                 if (ret < 0)
 759                         return ret;
 760                 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
 761                 if (ret < 0)
 762                         return ret;
 763                 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
 764                 if (ret < 0)
 765                         return ret;
 766
 767                 /* Disable digital BLC when generating a test pattern. */
 768                 ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
 769                                         0);
 770                 if (ret < 0)
 771                         return ret;
 772
 773                 ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
 774                 if (ret < 0)
 775                         return ret;
 776
 777                 return mt9p031_write(client, MT9P031_TEST_PATTERN,
 778                                 ((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
 779                                 | MT9P031_TEST_PATTERN_ENABLE);
 780
 781         case V4L2_CID_BLC_AUTO:
 782                 ret = mt9p031_set_mode2(mt9p031,
 783                                 ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
 784                                 ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
 785                 if (ret < 0)
 786                         return ret;
 787
 788                 return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
 789                                      ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
 790
 791         case V4L2_CID_BLC_TARGET_LEVEL:
 792                 return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
 793                                      ctrl->val);
 794
 795         case V4L2_CID_BLC_ANALOG_OFFSET:
 796                 data = ctrl->val & ((1 << 9) - 1);
 797
 798                 ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
 799                 if (ret < 0)
 800                         return ret;
 801                 ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
 802                 if (ret < 0)
 803                         return ret;
 804                 ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
 805                 if (ret < 0)
 806                         return ret;
 807                 return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
 808
 809         case V4L2_CID_BLC_DIGITAL_OFFSET:
 810                 return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
 811                                      ctrl->val & ((1 << 12) - 1));
 812         }
 813
 814         return 0;
 815 }
 816
 817 static const struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
 818         .s_ctrl = mt9p031_s_ctrl,
 819 };
 820
 821 static const char * const mt9p031_test_pattern_menu[] = {
 822         "Disabled",
 823         "Color Field",
 824         "Horizontal Gradient",
 825         "Vertical Gradient",
 826         "Diagonal Gradient",
 827         "Classic Test Pattern",
 828         "Walking 1s",
 829         "Monochrome Horizontal Bars",
 830         "Monochrome Vertical Bars",
 831         "Vertical Color Bars",
 832 };
 833
 834 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
 835         {
 836                 .ops            = &mt9p031_ctrl_ops,
 837                 .id             = V4L2_CID_BLC_AUTO,
 838                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 839                 .name           = "BLC, Auto",
 840                 .min            = 0,
 841                 .max            = 1,
 842                 .step           = 1,
 843                 .def            = 1,
 844                 .flags          = 0,
 845         }, {
 846                 .ops            = &mt9p031_ctrl_ops,
 847                 .id             = V4L2_CID_BLC_TARGET_LEVEL,
 848                 .type           = V4L2_CTRL_TYPE_INTEGER,
 849                 .name           = "BLC Target Level",
 850                 .min            = 0,
 851                 .max            = 4095,
 852                 .step           = 1,
 853                 .def            = 168,
 854                 .flags          = 0,
 855         }, {
 856                 .ops            = &mt9p031_ctrl_ops,
 857                 .id             = V4L2_CID_BLC_ANALOG_OFFSET,
 858                 .type           = V4L2_CTRL_TYPE_INTEGER,
 859                 .name           = "BLC Analog Offset",
 860                 .min            = -255,
 861                 .max            = 255,
 862                 .step           = 1,
 863                 .def            = 32,
 864                 .flags          = 0,
 865         }, {
 866                 .ops            = &mt9p031_ctrl_ops,
 867                 .id             = V4L2_CID_BLC_DIGITAL_OFFSET,
 868                 .type           = V4L2_CTRL_TYPE_INTEGER,
 869                 .name           = "BLC Digital Offset",
 870                 .min            = -2048,
 871                 .max            = 2047,
 872                 .step           = 1,
 873                 .def            = 40,
 874                 .flags          = 0,
 875         }
 876 };
 877
 878 /* -----------------------------------------------------------------------------
 879  * V4L2 subdev core operations
 880  */
 881
 882 static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
 883 {
 884         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 885         int ret = 0;
 886
 887         mutex_lock(&mt9p031->power_lock);
 888
 889         /* If the power count is modified from 0 to != 0 or from != 0 to 0,
 890          * update the power state.
 891          */
 892         if (mt9p031->power_count == !on) {
 893                 ret = __mt9p031_set_power(mt9p031, !!on);
 894                 if (ret < 0)
 895                         goto out;
 896         }
 897
 898         /* Update the power count. */
 899         mt9p031->power_count += on ? 1 : -1;
 900         WARN_ON(mt9p031->power_count < 0);
 901
 902 out:
 903         mutex_unlock(&mt9p031->power_lock);
 904         return ret;
 905 }
 906
 907 /* -----------------------------------------------------------------------------
 908  * V4L2 subdev internal operations
 909  */
 910
 911 static int mt9p031_registered(struct v4l2_subdev *subdev)
 912 {
 913         struct i2c_client *client = v4l2_get_subdevdata(subdev);
 914         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 915         s32 data;
 916         int ret;
 917
 918         ret = mt9p031_power_on(mt9p031);
 919         if (ret < 0) {
 920                 dev_err(&client->dev, "MT9P031 power up failed\n");
 921                 return ret;
 922         }
 923
 924         /* Read out the chip version register */
 925         data = mt9p031_read(client, MT9P031_CHIP_VERSION);
 926         mt9p031_power_off(mt9p031);
 927
 928         if (data != MT9P031_CHIP_VERSION_VALUE) {
 929                 dev_err(&client->dev, "MT9P031 not detected, wrong version "
 930                         "0x%04x\n", data);
 931                 return -ENODEV;
 932         }
 933
 934         dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
 935                  client->addr);
 936
 937         return 0;
 938 }
 939
 940 static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 941 {
 942         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 943         struct v4l2_mbus_framefmt *format;
 944         struct v4l2_rect *crop;
 945
 946         crop = v4l2_subdev_get_try_crop(subdev, fh->pad, 0);
 947         crop->left = MT9P031_COLUMN_START_DEF;
 948         crop->top = MT9P031_ROW_START_DEF;
 949         crop->width = MT9P031_WINDOW_WIDTH_DEF;
 950         crop->height = MT9P031_WINDOW_HEIGHT_DEF;
 951
 952         format = v4l2_subdev_get_try_format(subdev, fh->pad, 0);
 953
 954         if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
 955                 format->code = MEDIA_BUS_FMT_Y12_1X12;
 956         else
 957                 format->code = MEDIA_BUS_FMT_SGRBG12_1X12;
 958
 959         format->width = MT9P031_WINDOW_WIDTH_DEF;
 960         format->height = MT9P031_WINDOW_HEIGHT_DEF;
 961         format->field = V4L2_FIELD_NONE;
 962         format->colorspace = V4L2_COLORSPACE_SRGB;
 963
 964         return mt9p031_set_power(subdev, 1);
 965 }
 966
 967 static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 968 {
 969         return mt9p031_set_power(subdev, 0);
 970 }
 971
 972 static const struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
 973         .s_power        = mt9p031_set_power,
 974 };
 975
 976 static const struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
 977         .s_stream       = mt9p031_s_stream,
 978 };
 979
 980 static const struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
 981         .enum_mbus_code = mt9p031_enum_mbus_code,
 982         .enum_frame_size = mt9p031_enum_frame_size,
 983         .get_fmt = mt9p031_get_format,
 984         .set_fmt = mt9p031_set_format,
 985         .get_selection = mt9p031_get_selection,
 986         .set_selection = mt9p031_set_selection,
 987 };
 988
 989 static const struct v4l2_subdev_ops mt9p031_subdev_ops = {
 990         .core   = &mt9p031_subdev_core_ops,
 991         .video  = &mt9p031_subdev_video_ops,
 992         .pad    = &mt9p031_subdev_pad_ops,
 993 };
 994
 995 static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
 996         .registered = mt9p031_registered,
 997         .open = mt9p031_open,
 998         .close = mt9p031_close,
 999 };
1000
1001 /* -----------------------------------------------------------------------------
1002  * Driver initialization and probing
1003  */
1004
1005 static struct mt9p031_platform_data *
1006 mt9p031_get_pdata(struct i2c_client *client)
1007 {
1008         struct mt9p031_platform_data *pdata;
1009         struct device_node *np;
1010
1011         if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
1012                 return client->dev.platform_data;
1013
1014         np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
1015         if (!np)
1016                 return NULL;
1017
1018         pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
1019         if (!pdata)
1020                 goto done;
1021
1022         of_property_read_u32(np, "input-clock-frequency", &pdata->ext_freq);
1023         of_property_read_u32(np, "pixel-clock-frequency", &pdata->target_freq);
1024
1025 done:
1026         of_node_put(np);
1027         return pdata;
1028 }
1029
1030 static int mt9p031_probe(struct i2c_client *client,
1031                          const struct i2c_device_id *did)
1032 {
1033         struct mt9p031_platform_data *pdata = mt9p031_get_pdata(client);
1034         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
1035         struct mt9p031 *mt9p031;
1036         unsigned int i;
1037         int ret;
1038
1039         if (pdata == NULL) {
1040                 dev_err(&client->dev, "No platform data\n");
1041                 return -EINVAL;
1042         }
1043
1044         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1045                 dev_warn(&client->dev,
1046                         "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1047                 return -EIO;
1048         }
1049
1050         mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
1051         if (mt9p031 == NULL)
1052                 return -ENOMEM;
1053
1054         mt9p031->pdata = pdata;
1055         mt9p031->output_control = MT9P031_OUTPUT_CONTROL_DEF;
1056         mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
1057         mt9p031->model = did->driver_data;
1058
1059         mt9p031->regulators[0].supply = "vdd";
1060         mt9p031->regulators[1].supply = "vdd_io";
1061         mt9p031->regulators[2].supply = "vaa";
1062
1063         ret = devm_regulator_bulk_get(&client->dev, 3, mt9p031->regulators);
1064         if (ret < 0) {
1065                 dev_err(&client->dev, "Unable to get regulators\n");
1066                 return ret;
1067         }
1068
1069         mutex_init(&mt9p031->power_lock);
1070
1071         v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
1072
1073         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1074                           V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
1075                           MT9P031_SHUTTER_WIDTH_MAX, 1,
1076                           MT9P031_SHUTTER_WIDTH_DEF);
1077         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1078                           V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
1079                           MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
1080         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1081                           V4L2_CID_HFLIP, 0, 1, 1, 0);
1082         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1083                           V4L2_CID_VFLIP, 0, 1, 1, 0);
1084         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1085                           V4L2_CID_PIXEL_RATE, pdata->target_freq,
1086                           pdata->target_freq, 1, pdata->target_freq);
1087         v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1088                           V4L2_CID_TEST_PATTERN,
1089                           ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
1090                           0, mt9p031_test_pattern_menu);
1091
1092         for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
1093                 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
1094
1095         mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
1096
1097         if (mt9p031->ctrls.error) {
1098                 printk(KERN_INFO "%s: control initialization error %d\n",
1099                        __func__, mt9p031->ctrls.error);
1100                 ret = mt9p031->ctrls.error;
1101                 goto done;
1102         }
1103
1104         mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
1105         mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
1106                                              V4L2_CID_BLC_DIGITAL_OFFSET);
1107
1108         v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
1109         mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
1110
1111         mt9p031->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1112         mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
1113         ret = media_entity_pads_init(&mt9p031->subdev.entity, 1, &mt9p031->pad);
1114         if (ret < 0)
1115                 goto done;
1116
1117         mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1118
1119         mt9p031->crop.width = MT9P031_WINDOW_WIDTH_DEF;
1120         mt9p031->crop.height = MT9P031_WINDOW_HEIGHT_DEF;
1121         mt9p031->crop.left = MT9P031_COLUMN_START_DEF;
1122         mt9p031->crop.top = MT9P031_ROW_START_DEF;
1123
1124         if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
1125                 mt9p031->format.code = MEDIA_BUS_FMT_Y12_1X12;
1126         else
1127                 mt9p031->format.code = MEDIA_BUS_FMT_SGRBG12_1X12;
1128
1129         mt9p031->format.width = MT9P031_WINDOW_WIDTH_DEF;
1130         mt9p031->format.height = MT9P031_WINDOW_HEIGHT_DEF;
1131         mt9p031->format.field = V4L2_FIELD_NONE;
1132         mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
1133
1134         mt9p031->reset = devm_gpiod_get_optional(&client->dev, "reset",
1135                                                  GPIOD_OUT_HIGH);
1136
1137         ret = mt9p031_clk_setup(mt9p031);
1138         if (ret)
1139                 goto done;
1140
1141         ret = v4l2_async_register_subdev(&mt9p031->subdev);
1142
1143 done:
1144         if (ret < 0) {
1145                 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1146                 media_entity_cleanup(&mt9p031->subdev.entity);
1147                 mutex_destroy(&mt9p031->power_lock);
1148         }
1149
1150         return ret;
1151 }
1152
1153 static int mt9p031_remove(struct i2c_client *client)
1154 {
1155         struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1156         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1157
1158         v4l2_ctrl_handler_free(&mt9p031->ctrls);
1159         v4l2_async_unregister_subdev(subdev);
1160         media_entity_cleanup(&subdev->entity);
1161         mutex_destroy(&mt9p031->power_lock);
1162
1163         return 0;
1164 }
1165
1166 static const struct i2c_device_id mt9p031_id[] = {
1167         { "mt9p031", MT9P031_MODEL_COLOR },
1168         { "mt9p031m", MT9P031_MODEL_MONOCHROME },
1169         { }
1170 };
1171 MODULE_DEVICE_TABLE(i2c, mt9p031_id);
1172
1173 #if IS_ENABLED(CONFIG_OF)
1174 static const struct of_device_id mt9p031_of_match[] = {
1175         { .compatible = "aptina,mt9p031", },
1176         { .compatible = "aptina,mt9p031m", },
1177         { /* sentinel */ },
1178 };
1179 MODULE_DEVICE_TABLE(of, mt9p031_of_match);
1180 #endif
1181
1182 static struct i2c_driver mt9p031_i2c_driver = {
1183         .driver = {
1184                 .of_match_table = of_match_ptr(mt9p031_of_match),
1185                 .name = "mt9p031",
1186         },
1187         .probe          = mt9p031_probe,
1188         .remove         = mt9p031_remove,
1189         .id_table       = mt9p031_id,
1190 };
1191
1192 module_i2c_driver(mt9p031_i2c_driver);
1193
1194 MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
1195 MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
1196 MODULE_LICENSE("GPL v2");