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