drivers/media/i2c/mt9p031.c

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