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_BLC_AUTO               (V4L2_CID_USER_BASE | 0x1002)
 578 #define V4L2_CID_BLC_TARGET_LEVEL       (V4L2_CID_USER_BASE | 0x1003)
 579 #define V4L2_CID_BLC_ANALOG_OFFSET      (V4L2_CID_USER_BASE | 0x1004)
 580 #define V4L2_CID_BLC_DIGITAL_OFFSET     (V4L2_CID_USER_BASE | 0x1005)
 581
 582 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 583 {
 584         struct mt9p031 *mt9p031 =
 585                         container_of(ctrl->handler, struct mt9p031, ctrls);
 586         struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
 587         u16 data;
 588         int ret;
 589
 590         switch (ctrl->id) {
 591         case V4L2_CID_EXPOSURE:
 592                 ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
 593                                     (ctrl->val >> 16) & 0xffff);
 594                 if (ret < 0)
 595                         return ret;
 596
 597                 return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
 598                                      ctrl->val & 0xffff);
 599
 600         case V4L2_CID_GAIN:
 601                 /* Gain is controlled by 2 analog stages and a digital stage.
 602                  * Valid values for the 3 stages are
 603                  *
 604                  * Stage                Min     Max     Step
 605                  * ------------------------------------------
 606                  * First analog stage   x1      x2      1
 607                  * Second analog stage  x1      x4      0.125
 608                  * Digital stage        x1      x16     0.125
 609                  *
 610                  * To minimize noise, the gain stages should be used in the
 611                  * second analog stage, first analog stage, digital stage order.
 612                  * Gain from a previous stage should be pushed to its maximum
 613                  * value before the next stage is used.
 614                  */
 615                 if (ctrl->val <= 32) {
 616                         data = ctrl->val;
 617                 } else if (ctrl->val <= 64) {
 618                         ctrl->val &= ~1;
 619                         data = (1 << 6) | (ctrl->val >> 1);
 620                 } else {
 621                         ctrl->val &= ~7;
 622                         data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
 623                 }
 624
 625                 return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
 626
 627         case V4L2_CID_HFLIP:
 628                 if (ctrl->val)
 629                         return mt9p031_set_mode2(mt9p031,
 630                                         0, MT9P031_READ_MODE_2_COL_MIR);
 631                 else
 632                         return mt9p031_set_mode2(mt9p031,
 633                                         MT9P031_READ_MODE_2_COL_MIR, 0);
 634
 635         case V4L2_CID_VFLIP:
 636                 if (ctrl->val)
 637                         return mt9p031_set_mode2(mt9p031,
 638                                         0, MT9P031_READ_MODE_2_ROW_MIR);
 639                 else
 640                         return mt9p031_set_mode2(mt9p031,
 641                                         MT9P031_READ_MODE_2_ROW_MIR, 0);
 642
 643         case V4L2_CID_TEST_PATTERN:
 644                 if (!ctrl->val) {
 645                         /* Restore the black level compensation settings. */
 646                         if (mt9p031->blc_auto->cur.val != 0) {
 647                                 ret = mt9p031_s_ctrl(mt9p031->blc_auto);
 648                                 if (ret < 0)
 649                                         return ret;
 650                         }
 651                         if (mt9p031->blc_offset->cur.val != 0) {
 652                                 ret = mt9p031_s_ctrl(mt9p031->blc_offset);
 653                                 if (ret < 0)
 654                                         return ret;
 655                         }
 656                         return mt9p031_write(client, MT9P031_TEST_PATTERN,
 657                                              MT9P031_TEST_PATTERN_DISABLE);
 658                 }
 659
 660                 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
 661                 if (ret < 0)
 662                         return ret;
 663                 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
 664                 if (ret < 0)
 665                         return ret;
 666                 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
 667                 if (ret < 0)
 668                         return ret;
 669
 670                 /* Disable digital black level compensation when using a test
 671                  * pattern.
 672                  */
 673                 ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
 674                                         0);
 675                 if (ret < 0)
 676                         return ret;
 677
 678                 ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
 679                 if (ret < 0)
 680                         return ret;
 681
 682                 return mt9p031_write(client, MT9P031_TEST_PATTERN,
 683                                 ((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
 684                                 | MT9P031_TEST_PATTERN_ENABLE);
 685
 686         case V4L2_CID_BLC_AUTO:
 687                 ret = mt9p031_set_mode2(mt9p031,
 688                                 ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
 689                                 ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
 690                 if (ret < 0)
 691                         return ret;
 692
 693                 return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
 694                                      ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
 695
 696         case V4L2_CID_BLC_TARGET_LEVEL:
 697                 return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
 698                                      ctrl->val);
 699
 700         case V4L2_CID_BLC_ANALOG_OFFSET:
 701                 data = ctrl->val & ((1 << 9) - 1);
 702
 703                 ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
 704                 if (ret < 0)
 705                         return ret;
 706                 ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
 707                 if (ret < 0)
 708                         return ret;
 709                 ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
 710                 if (ret < 0)
 711                         return ret;
 712                 return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
 713
 714         case V4L2_CID_BLC_DIGITAL_OFFSET:
 715                 return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
 716                                      ctrl->val & ((1 << 12) - 1));
 717         }
 718
 719         return 0;
 720 }
 721
 722 static struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
 723         .s_ctrl = mt9p031_s_ctrl,
 724 };
 725
 726 static const char * const mt9p031_test_pattern_menu[] = {
 727         "Disabled",
 728         "Color Field",
 729         "Horizontal Gradient",
 730         "Vertical Gradient",
 731         "Diagonal Gradient",
 732         "Classic Test Pattern",
 733         "Walking 1s",
 734         "Monochrome Horizontal Bars",
 735         "Monochrome Vertical Bars",
 736         "Vertical Color Bars",
 737 };
 738
 739 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
 740         {
 741                 .ops            = &mt9p031_ctrl_ops,
 742                 .id             = V4L2_CID_BLC_AUTO,
 743                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 744                 .name           = "BLC, Auto",
 745                 .min            = 0,
 746                 .max            = 1,
 747                 .step           = 1,
 748                 .def            = 1,
 749                 .flags          = 0,
 750         }, {
 751                 .ops            = &mt9p031_ctrl_ops,
 752                 .id             = V4L2_CID_BLC_TARGET_LEVEL,
 753                 .type           = V4L2_CTRL_TYPE_INTEGER,
 754                 .name           = "BLC Target Level",
 755                 .min            = 0,
 756                 .max            = 4095,
 757                 .step           = 1,
 758                 .def            = 168,
 759                 .flags          = 0,
 760         }, {
 761                 .ops            = &mt9p031_ctrl_ops,
 762                 .id             = V4L2_CID_BLC_ANALOG_OFFSET,
 763                 .type           = V4L2_CTRL_TYPE_INTEGER,
 764                 .name           = "BLC Analog Offset",
 765                 .min            = -255,
 766                 .max            = 255,
 767                 .step           = 1,
 768                 .def            = 32,
 769                 .flags          = 0,
 770         }, {
 771                 .ops            = &mt9p031_ctrl_ops,
 772                 .id             = V4L2_CID_BLC_DIGITAL_OFFSET,
 773                 .type           = V4L2_CTRL_TYPE_INTEGER,
 774                 .name           = "BLC Digital Offset",
 775                 .min            = -2048,
 776                 .max            = 2047,
 777                 .step           = 1,
 778                 .def            = 40,
 779                 .flags          = 0,
 780         }
 781 };
 782
 783 /* -----------------------------------------------------------------------------
 784  * V4L2 subdev core operations
 785  */
 786
 787 static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
 788 {
 789         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 790         int ret = 0;
 791
 792         mutex_lock(&mt9p031->power_lock);
 793
 794         /* If the power count is modified from 0 to != 0 or from != 0 to 0,
 795          * update the power state.
 796          */
 797         if (mt9p031->power_count == !on) {
 798                 ret = __mt9p031_set_power(mt9p031, !!on);
 799                 if (ret < 0)
 800                         goto out;
 801         }
 802
 803         /* Update the power count. */
 804         mt9p031->power_count += on ? 1 : -1;
 805         WARN_ON(mt9p031->power_count < 0);
 806
 807 out:
 808         mutex_unlock(&mt9p031->power_lock);
 809         return ret;
 810 }
 811
 812 /* -----------------------------------------------------------------------------
 813  * V4L2 subdev internal operations
 814  */
 815
 816 static int mt9p031_registered(struct v4l2_subdev *subdev)
 817 {
 818         struct i2c_client *client = v4l2_get_subdevdata(subdev);
 819         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 820         s32 data;
 821         int ret;
 822
 823         ret = mt9p031_power_on(mt9p031);
 824         if (ret < 0) {
 825                 dev_err(&client->dev, "MT9P031 power up failed\n");
 826                 return ret;
 827         }
 828
 829         /* Read out the chip version register */
 830         data = mt9p031_read(client, MT9P031_CHIP_VERSION);
 831         if (data != MT9P031_CHIP_VERSION_VALUE) {
 832                 dev_err(&client->dev, "MT9P031 not detected, wrong version "
 833                         "0x%04x\n", data);
 834                 return -ENODEV;
 835         }
 836
 837         mt9p031_power_off(mt9p031);
 838
 839         dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
 840                  client->addr);
 841
 842         return ret;
 843 }
 844
 845 static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 846 {
 847         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
 848         struct v4l2_mbus_framefmt *format;
 849         struct v4l2_rect *crop;
 850
 851         crop = v4l2_subdev_get_try_crop(fh, 0);
 852         crop->left = MT9P031_COLUMN_START_DEF;
 853         crop->top = MT9P031_ROW_START_DEF;
 854         crop->width = MT9P031_WINDOW_WIDTH_DEF;
 855         crop->height = MT9P031_WINDOW_HEIGHT_DEF;
 856
 857         format = v4l2_subdev_get_try_format(fh, 0);
 858
 859         if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
 860                 format->code = V4L2_MBUS_FMT_Y12_1X12;
 861         else
 862                 format->code = V4L2_MBUS_FMT_SGRBG12_1X12;
 863
 864         format->width = MT9P031_WINDOW_WIDTH_DEF;
 865         format->height = MT9P031_WINDOW_HEIGHT_DEF;
 866         format->field = V4L2_FIELD_NONE;
 867         format->colorspace = V4L2_COLORSPACE_SRGB;
 868
 869         return mt9p031_set_power(subdev, 1);
 870 }
 871
 872 static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 873 {
 874         return mt9p031_set_power(subdev, 0);
 875 }
 876
 877 static struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
 878         .s_power        = mt9p031_set_power,
 879 };
 880
 881 static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
 882         .s_stream       = mt9p031_s_stream,
 883 };
 884
 885 static struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
 886         .enum_mbus_code = mt9p031_enum_mbus_code,
 887         .enum_frame_size = mt9p031_enum_frame_size,
 888         .get_fmt = mt9p031_get_format,
 889         .set_fmt = mt9p031_set_format,
 890         .get_crop = mt9p031_get_crop,
 891         .set_crop = mt9p031_set_crop,
 892 };
 893
 894 static struct v4l2_subdev_ops mt9p031_subdev_ops = {
 895         .core   = &mt9p031_subdev_core_ops,
 896         .video  = &mt9p031_subdev_video_ops,
 897         .pad    = &mt9p031_subdev_pad_ops,
 898 };
 899
 900 static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
 901         .registered = mt9p031_registered,
 902         .open = mt9p031_open,
 903         .close = mt9p031_close,
 904 };
 905
 906 /* -----------------------------------------------------------------------------
 907  * Driver initialization and probing
 908  */
 909
 910 static int mt9p031_probe(struct i2c_client *client,
 911                          const struct i2c_device_id *did)
 912 {
 913         struct mt9p031_platform_data *pdata = client->dev.platform_data;
 914         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
 915         struct mt9p031 *mt9p031;
 916         unsigned int i;
 917         int ret;
 918
 919         if (pdata == NULL) {
 920                 dev_err(&client->dev, "No platform data\n");
 921                 return -EINVAL;
 922         }
 923
 924         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
 925                 dev_warn(&client->dev,
 926                         "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
 927                 return -EIO;
 928         }
 929
 930         mt9p031 = kzalloc(sizeof(*mt9p031), GFP_KERNEL);
 931         if (mt9p031 == NULL)
 932                 return -ENOMEM;
 933
 934         mt9p031->pdata = pdata;
 935         mt9p031->output_control = MT9P031_OUTPUT_CONTROL_DEF;
 936         mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
 937         mt9p031->model = did->driver_data;
 938         mt9p031->reset = -1;
 939
 940         v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
 941
 942         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
 943                           V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
 944                           MT9P031_SHUTTER_WIDTH_MAX, 1,
 945                           MT9P031_SHUTTER_WIDTH_DEF);
 946         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
 947                           V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
 948                           MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
 949         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
 950                           V4L2_CID_HFLIP, 0, 1, 1, 0);
 951         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
 952                           V4L2_CID_VFLIP, 0, 1, 1, 0);
 953         v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
 954                           V4L2_CID_PIXEL_RATE, pdata->target_freq,
 955                           pdata->target_freq, 1, pdata->target_freq);
 956         v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
 957                           V4L2_CID_TEST_PATTERN,
 958                           ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
 959                           0, mt9p031_test_pattern_menu);
 960
 961         for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
 962                 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
 963
 964         mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
 965
 966         if (mt9p031->ctrls.error) {
 967                 printk(KERN_INFO "%s: control initialization error %d\n",
 968                        __func__, mt9p031->ctrls.error);
 969                 ret = mt9p031->ctrls.error;
 970                 goto done;
 971         }
 972
 973         mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
 974         mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
 975                                              V4L2_CID_BLC_DIGITAL_OFFSET);
 976
 977         mutex_init(&mt9p031->power_lock);
 978         v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
 979         mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
 980
 981         mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
 982         ret = media_entity_init(&mt9p031->subdev.entity, 1, &mt9p031->pad, 0);
 983         if (ret < 0)
 984                 goto done;
 985
 986         mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 987
 988         mt9p031->crop.width = MT9P031_WINDOW_WIDTH_DEF;
 989         mt9p031->crop.height = MT9P031_WINDOW_HEIGHT_DEF;
 990         mt9p031->crop.left = MT9P031_COLUMN_START_DEF;
 991         mt9p031->crop.top = MT9P031_ROW_START_DEF;
 992
 993         if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
 994                 mt9p031->format.code = V4L2_MBUS_FMT_Y12_1X12;
 995         else
 996                 mt9p031->format.code = V4L2_MBUS_FMT_SGRBG12_1X12;
 997
 998         mt9p031->format.width = MT9P031_WINDOW_WIDTH_DEF;
 999         mt9p031->format.height = MT9P031_WINDOW_HEIGHT_DEF;
1000         mt9p031->format.field = V4L2_FIELD_NONE;
1001         mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
1002
1003         if (pdata->reset != -1) {
1004                 ret = gpio_request_one(pdata->reset, GPIOF_OUT_INIT_LOW,
1005                                        "mt9p031_rst");
1006                 if (ret < 0)
1007                         goto done;
1008
1009                 mt9p031->reset = pdata->reset;
1010         }
1011
1012         ret = mt9p031_pll_setup(mt9p031);
1013
1014 done:
1015         if (ret < 0) {
1016                 if (mt9p031->reset != -1)
1017                         gpio_free(mt9p031->reset);
1018
1019                 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1020                 media_entity_cleanup(&mt9p031->subdev.entity);
1021                 kfree(mt9p031);
1022         }
1023
1024         return ret;
1025 }
1026
1027 static int mt9p031_remove(struct i2c_client *client)
1028 {
1029         struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1030         struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1031
1032         v4l2_ctrl_handler_free(&mt9p031->ctrls);
1033         v4l2_device_unregister_subdev(subdev);
1034         media_entity_cleanup(&subdev->entity);
1035         if (mt9p031->reset != -1)
1036                 gpio_free(mt9p031->reset);
1037         kfree(mt9p031);
1038
1039         return 0;
1040 }
1041
1042 static const struct i2c_device_id mt9p031_id[] = {
1043         { "mt9p031", MT9P031_MODEL_COLOR },
1044         { "mt9p031m", MT9P031_MODEL_MONOCHROME },
1045         { }
1046 };
1047 MODULE_DEVICE_TABLE(i2c, mt9p031_id);
1048
1049 static struct i2c_driver mt9p031_i2c_driver = {
1050         .driver = {
1051                 .name = "mt9p031",
1052         },
1053         .probe          = mt9p031_probe,
1054         .remove         = mt9p031_remove,
1055         .id_table       = mt9p031_id,
1056 };
1057
1058 module_i2c_driver(mt9p031_i2c_driver);
1059
1060 MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
1061 MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
1062 MODULE_LICENSE("GPL v2");