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