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