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