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