kernel/drivers/media/i2c/mt9p031.c

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