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