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