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