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