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