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