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