drivers/media/i2c/mt9t001.c

   1 /*
   2  * Driver for MT9T001 CMOS Image Sensor from Aptina (Micron)
   3  *
   4  * Copyright (C) 2010-2011, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
   5  *
   6  * Based on the MT9M001 driver,
   7  *
   8  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
   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/clk.h>
  16 #include <linux/i2c.h>
  17 #include <linux/log2.h>
  18 #include <linux/module.h>
  19 #include <linux/regulator/consumer.h>
  20 #include <linux/slab.h>
  21 #include <linux/videodev2.h>
  22 #include <linux/v4l2-mediabus.h>
  23
  24 #include <media/i2c/mt9t001.h>
  25 #include <media/v4l2-ctrls.h>
  26 #include <media/v4l2-device.h>
  27 #include <media/v4l2-subdev.h>
  28
  29 #define MT9T001_PIXEL_ARRAY_HEIGHT                      1568
  30 #define MT9T001_PIXEL_ARRAY_WIDTH                       2112
  31
  32 #define MT9T001_CHIP_VERSION                            0x00
  33 #define         MT9T001_CHIP_ID                         0x1621
  34 #define MT9T001_ROW_START                               0x01
  35 #define         MT9T001_ROW_START_MIN                   0
  36 #define         MT9T001_ROW_START_DEF                   20
  37 #define         MT9T001_ROW_START_MAX                   1534
  38 #define MT9T001_COLUMN_START                            0x02
  39 #define         MT9T001_COLUMN_START_MIN                0
  40 #define         MT9T001_COLUMN_START_DEF                32
  41 #define         MT9T001_COLUMN_START_MAX                2046
  42 #define MT9T001_WINDOW_HEIGHT                           0x03
  43 #define         MT9T001_WINDOW_HEIGHT_MIN               1
  44 #define         MT9T001_WINDOW_HEIGHT_DEF               1535
  45 #define         MT9T001_WINDOW_HEIGHT_MAX               1567
  46 #define MT9T001_WINDOW_WIDTH                            0x04
  47 #define         MT9T001_WINDOW_WIDTH_MIN                1
  48 #define         MT9T001_WINDOW_WIDTH_DEF                2047
  49 #define         MT9T001_WINDOW_WIDTH_MAX                2111
  50 #define MT9T001_HORIZONTAL_BLANKING                     0x05
  51 #define         MT9T001_HORIZONTAL_BLANKING_MIN         21
  52 #define         MT9T001_HORIZONTAL_BLANKING_MAX         1023
  53 #define MT9T001_VERTICAL_BLANKING                       0x06
  54 #define         MT9T001_VERTICAL_BLANKING_MIN           3
  55 #define         MT9T001_VERTICAL_BLANKING_MAX           1023
  56 #define MT9T001_OUTPUT_CONTROL                          0x07
  57 #define         MT9T001_OUTPUT_CONTROL_SYNC             (1 << 0)
  58 #define         MT9T001_OUTPUT_CONTROL_CHIP_ENABLE      (1 << 1)
  59 #define         MT9T001_OUTPUT_CONTROL_TEST_DATA        (1 << 6)
  60 #define         MT9T001_OUTPUT_CONTROL_DEF              0x0002
  61 #define MT9T001_SHUTTER_WIDTH_HIGH                      0x08
  62 #define MT9T001_SHUTTER_WIDTH_LOW                       0x09
  63 #define         MT9T001_SHUTTER_WIDTH_MIN               1
  64 #define         MT9T001_SHUTTER_WIDTH_DEF               1561
  65 #define         MT9T001_SHUTTER_WIDTH_MAX               (1024 * 1024)
  66 #define MT9T001_PIXEL_CLOCK                             0x0a
  67 #define         MT9T001_PIXEL_CLOCK_INVERT              (1 << 15)
  68 #define         MT9T001_PIXEL_CLOCK_SHIFT_MASK          (7 << 8)
  69 #define         MT9T001_PIXEL_CLOCK_SHIFT_SHIFT         8
  70 #define         MT9T001_PIXEL_CLOCK_DIVIDE_MASK         (0x7f << 0)
  71 #define MT9T001_FRAME_RESTART                           0x0b
  72 #define MT9T001_SHUTTER_DELAY                           0x0c
  73 #define         MT9T001_SHUTTER_DELAY_MAX               2047
  74 #define MT9T001_RESET                                   0x0d
  75 #define MT9T001_READ_MODE1                              0x1e
  76 #define         MT9T001_READ_MODE_SNAPSHOT              (1 << 8)
  77 #define         MT9T001_READ_MODE_STROBE_ENABLE         (1 << 9)
  78 #define         MT9T001_READ_MODE_STROBE_WIDTH          (1 << 10)
  79 #define         MT9T001_READ_MODE_STROBE_OVERRIDE       (1 << 11)
  80 #define MT9T001_READ_MODE2                              0x20
  81 #define         MT9T001_READ_MODE_BAD_FRAMES            (1 << 0)
  82 #define         MT9T001_READ_MODE_LINE_VALID_CONTINUOUS (1 << 9)
  83 #define         MT9T001_READ_MODE_LINE_VALID_FRAME      (1 << 10)
  84 #define MT9T001_READ_MODE3                              0x21
  85 #define         MT9T001_READ_MODE_GLOBAL_RESET          (1 << 0)
  86 #define         MT9T001_READ_MODE_GHST_CTL              (1 << 1)
  87 #define MT9T001_ROW_ADDRESS_MODE                        0x22
  88 #define         MT9T001_ROW_SKIP_MASK                   (7 << 0)
  89 #define         MT9T001_ROW_BIN_MASK                    (3 << 3)
  90 #define         MT9T001_ROW_BIN_SHIFT                   3
  91 #define MT9T001_COLUMN_ADDRESS_MODE                     0x23
  92 #define         MT9T001_COLUMN_SKIP_MASK                (7 << 0)
  93 #define         MT9T001_COLUMN_BIN_MASK                 (3 << 3)
  94 #define         MT9T001_COLUMN_BIN_SHIFT                3
  95 #define MT9T001_GREEN1_GAIN                             0x2b
  96 #define MT9T001_BLUE_GAIN                               0x2c
  97 #define MT9T001_RED_GAIN                                0x2d
  98 #define MT9T001_GREEN2_GAIN                             0x2e
  99 #define MT9T001_TEST_DATA                               0x32
 100 #define MT9T001_GLOBAL_GAIN                             0x35
 101 #define         MT9T001_GLOBAL_GAIN_MIN                 8
 102 #define         MT9T001_GLOBAL_GAIN_MAX                 1024
 103 #define MT9T001_BLACK_LEVEL                             0x49
 104 #define MT9T001_ROW_BLACK_DEFAULT_OFFSET                0x4b
 105 #define MT9T001_BLC_DELTA_THRESHOLDS                    0x5d
 106 #define MT9T001_CAL_THRESHOLDS                          0x5f
 107 #define MT9T001_GREEN1_OFFSET                           0x60
 108 #define MT9T001_GREEN2_OFFSET                           0x61
 109 #define MT9T001_BLACK_LEVEL_CALIBRATION                 0x62
 110 #define         MT9T001_BLACK_LEVEL_OVERRIDE            (1 << 0)
 111 #define         MT9T001_BLACK_LEVEL_DISABLE_OFFSET      (1 << 1)
 112 #define         MT9T001_BLACK_LEVEL_RECALCULATE         (1 << 12)
 113 #define         MT9T001_BLACK_LEVEL_LOCK_RED_BLUE       (1 << 13)
 114 #define         MT9T001_BLACK_LEVEL_LOCK_GREEN          (1 << 14)
 115 #define MT9T001_RED_OFFSET                              0x63
 116 #define MT9T001_BLUE_OFFSET                             0x64
 117
 118 struct mt9t001 {
 119         struct v4l2_subdev subdev;
 120         struct media_pad pad;
 121
 122         struct clk *clk;
 123         struct regulator_bulk_data regulators[2];
 124
 125         struct mutex power_lock; /* lock to protect power_count */
 126         int power_count;
 127
 128         struct v4l2_mbus_framefmt format;
 129         struct v4l2_rect crop;
 130
 131         struct v4l2_ctrl_handler ctrls;
 132         struct v4l2_ctrl *gains[4];
 133
 134         u16 output_control;
 135         u16 black_level;
 136 };
 137
 138 static inline struct mt9t001 *to_mt9t001(struct v4l2_subdev *sd)
 139 {
 140         return container_of(sd, struct mt9t001, subdev);
 141 }
 142
 143 static int mt9t001_read(struct i2c_client *client, u8 reg)
 144 {
 145         return i2c_smbus_read_word_swapped(client, reg);
 146 }
 147
 148 static int mt9t001_write(struct i2c_client *client, u8 reg, u16 data)
 149 {
 150         return i2c_smbus_write_word_swapped(client, reg, data);
 151 }
 152
 153 static int mt9t001_set_output_control(struct mt9t001 *mt9t001, u16 clear,
 154                                       u16 set)
 155 {
 156         struct i2c_client *client = v4l2_get_subdevdata(&mt9t001->subdev);
 157         u16 value = (mt9t001->output_control & ~clear) | set;
 158         int ret;
 159
 160         if (value == mt9t001->output_control)
 161                 return 0;
 162
 163         ret = mt9t001_write(client, MT9T001_OUTPUT_CONTROL, value);
 164         if (ret < 0)
 165                 return ret;
 166
 167         mt9t001->output_control = value;
 168         return 0;
 169 }
 170
 171 static int mt9t001_reset(struct mt9t001 *mt9t001)
 172 {
 173         struct i2c_client *client = v4l2_get_subdevdata(&mt9t001->subdev);
 174         int ret;
 175
 176         /* Reset the chip and stop data read out */
 177         ret = mt9t001_write(client, MT9T001_RESET, 1);
 178         if (ret < 0)
 179                 return ret;
 180
 181         ret = mt9t001_write(client, MT9T001_RESET, 0);
 182         if (ret < 0)
 183                 return ret;
 184
 185         mt9t001->output_control = MT9T001_OUTPUT_CONTROL_DEF;
 186
 187         return mt9t001_set_output_control(mt9t001,
 188                                           MT9T001_OUTPUT_CONTROL_CHIP_ENABLE,
 189                                           0);
 190 }
 191
 192 static int mt9t001_power_on(struct mt9t001 *mt9t001)
 193 {
 194         int ret;
 195
 196         /* Bring up the supplies */
 197         ret = regulator_bulk_enable(ARRAY_SIZE(mt9t001->regulators),
 198                                    mt9t001->regulators);
 199         if (ret < 0)
 200                 return ret;
 201
 202         /* Enable clock */
 203         ret = clk_prepare_enable(mt9t001->clk);
 204         if (ret < 0)
 205                 regulator_bulk_disable(ARRAY_SIZE(mt9t001->regulators),
 206                                        mt9t001->regulators);
 207
 208         return ret;
 209 }
 210
 211 static void mt9t001_power_off(struct mt9t001 *mt9t001)
 212 {
 213         regulator_bulk_disable(ARRAY_SIZE(mt9t001->regulators),
 214                                mt9t001->regulators);
 215
 216         clk_disable_unprepare(mt9t001->clk);
 217 }
 218
 219 static int __mt9t001_set_power(struct mt9t001 *mt9t001, bool on)
 220 {
 221         struct i2c_client *client = v4l2_get_subdevdata(&mt9t001->subdev);
 222         int ret;
 223
 224         if (!on) {
 225                 mt9t001_power_off(mt9t001);
 226                 return 0;
 227         }
 228
 229         ret = mt9t001_power_on(mt9t001);
 230         if (ret < 0)
 231                 return ret;
 232
 233         ret = mt9t001_reset(mt9t001);
 234         if (ret < 0) {
 235                 dev_err(&client->dev, "Failed to reset the camera\n");
 236                 goto e_power;
 237         }
 238
 239         ret = v4l2_ctrl_handler_setup(&mt9t001->ctrls);
 240         if (ret < 0) {
 241                 dev_err(&client->dev, "Failed to set up control handlers\n");
 242                 goto e_power;
 243         }
 244
 245         return 0;
 246
 247 e_power:
 248         mt9t001_power_off(mt9t001);
 249
 250         return ret;
 251 }
 252
 253 /* -----------------------------------------------------------------------------
 254  * V4L2 subdev video operations
 255  */
 256
 257 static struct v4l2_mbus_framefmt *
 258 __mt9t001_get_pad_format(struct mt9t001 *mt9t001, struct v4l2_subdev_pad_config *cfg,
 259                          unsigned int pad, enum v4l2_subdev_format_whence which)
 260 {
 261         switch (which) {
 262         case V4L2_SUBDEV_FORMAT_TRY:
 263                 return v4l2_subdev_get_try_format(&mt9t001->subdev, cfg, pad);
 264         case V4L2_SUBDEV_FORMAT_ACTIVE:
 265                 return &mt9t001->format;
 266         default:
 267                 return NULL;
 268         }
 269 }
 270
 271 static struct v4l2_rect *
 272 __mt9t001_get_pad_crop(struct mt9t001 *mt9t001, struct v4l2_subdev_pad_config *cfg,
 273                        unsigned int pad, enum v4l2_subdev_format_whence which)
 274 {
 275         switch (which) {
 276         case V4L2_SUBDEV_FORMAT_TRY:
 277                 return v4l2_subdev_get_try_crop(&mt9t001->subdev, cfg, pad);
 278         case V4L2_SUBDEV_FORMAT_ACTIVE:
 279                 return &mt9t001->crop;
 280         default:
 281                 return NULL;
 282         }
 283 }
 284
 285 static int mt9t001_s_stream(struct v4l2_subdev *subdev, int enable)
 286 {
 287         const u16 mode = MT9T001_OUTPUT_CONTROL_CHIP_ENABLE;
 288         struct i2c_client *client = v4l2_get_subdevdata(subdev);
 289         struct mt9t001_platform_data *pdata = client->dev.platform_data;
 290         struct mt9t001 *mt9t001 = to_mt9t001(subdev);
 291         struct v4l2_mbus_framefmt *format = &mt9t001->format;
 292         struct v4l2_rect *crop = &mt9t001->crop;
 293         unsigned int hratio;
 294         unsigned int vratio;
 295         int ret;
 296
 297         if (!enable)
 298                 return mt9t001_set_output_control(mt9t001, mode, 0);
 299
 300         /* Configure the pixel clock polarity */
 301         if (pdata->clk_pol) {
 302                 ret  = mt9t001_write(client, MT9T001_PIXEL_CLOCK,
 303                                      MT9T001_PIXEL_CLOCK_INVERT);
 304                 if (ret < 0)
 305                         return ret;
 306         }
 307
 308         /* Configure the window size and row/column bin */
 309         hratio = DIV_ROUND_CLOSEST(crop->width, format->width);
 310         vratio = DIV_ROUND_CLOSEST(crop->height, format->height);
 311
 312         ret = mt9t001_write(client, MT9T001_ROW_ADDRESS_MODE, hratio - 1);
 313         if (ret < 0)
 314                 return ret;
 315
 316         ret = mt9t001_write(client, MT9T001_COLUMN_ADDRESS_MODE, vratio - 1);
 317         if (ret < 0)
 318                 return ret;
 319
 320         ret = mt9t001_write(client, MT9T001_COLUMN_START, crop->left);
 321         if (ret < 0)
 322                 return ret;
 323
 324         ret = mt9t001_write(client, MT9T001_ROW_START, crop->top);
 325         if (ret < 0)
 326                 return ret;
 327
 328         ret = mt9t001_write(client, MT9T001_WINDOW_WIDTH, crop->width - 1);
 329         if (ret < 0)
 330                 return ret;
 331
 332         ret = mt9t001_write(client, MT9T001_WINDOW_HEIGHT, crop->height - 1);
 333         if (ret < 0)
 334                 return ret;
 335
 336         /* Switch to master "normal" mode */
 337         return mt9t001_set_output_control(mt9t001, 0, mode);
 338 }
 339
 340 static int mt9t001_enum_mbus_code(struct v4l2_subdev *subdev,
 341                                   struct v4l2_subdev_pad_config *cfg,
 342                                   struct v4l2_subdev_mbus_code_enum *code)
 343 {
 344         if (code->index > 0)
 345                 return -EINVAL;
 346
 347         code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
 348         return 0;
 349 }
 350
 351 static int mt9t001_enum_frame_size(struct v4l2_subdev *subdev,
 352                                    struct v4l2_subdev_pad_config *cfg,
 353                                    struct v4l2_subdev_frame_size_enum *fse)
 354 {
 355         if (fse->index >= 8 || fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
 356                 return -EINVAL;
 357
 358         fse->min_width = (MT9T001_WINDOW_WIDTH_DEF + 1) / fse->index;
 359         fse->max_width = fse->min_width;
 360         fse->min_height = (MT9T001_WINDOW_HEIGHT_DEF + 1) / fse->index;
 361         fse->max_height = fse->min_height;
 362
 363         return 0;
 364 }
 365
 366 static int mt9t001_get_format(struct v4l2_subdev *subdev,
 367                               struct v4l2_subdev_pad_config *cfg,
 368                               struct v4l2_subdev_format *format)
 369 {
 370         struct mt9t001 *mt9t001 = to_mt9t001(subdev);
 371
 372         format->format = *__mt9t001_get_pad_format(mt9t001, cfg, format->pad,
 373                                                    format->which);
 374         return 0;
 375 }
 376
 377 static int mt9t001_set_format(struct v4l2_subdev *subdev,
 378                               struct v4l2_subdev_pad_config *cfg,
 379                               struct v4l2_subdev_format *format)
 380 {
 381         struct mt9t001 *mt9t001 = to_mt9t001(subdev);
 382         struct v4l2_mbus_framefmt *__format;
 383         struct v4l2_rect *__crop;
 384         unsigned int width;
 385         unsigned int height;
 386         unsigned int hratio;
 387         unsigned int vratio;
 388
 389         __crop = __mt9t001_get_pad_crop(mt9t001, cfg, format->pad,
 390                                         format->which);
 391
 392         /* Clamp the width and height to avoid dividing by zero. */
 393         width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
 394                         max_t(unsigned int, __crop->width / 8,
 395                               MT9T001_WINDOW_HEIGHT_MIN + 1),
 396                         __crop->width);
 397         height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
 398                          max_t(unsigned int, __crop->height / 8,
 399                                MT9T001_WINDOW_HEIGHT_MIN + 1),
 400                          __crop->height);
 401
 402         hratio = DIV_ROUND_CLOSEST(__crop->width, width);
 403         vratio = DIV_ROUND_CLOSEST(__crop->height, height);
 404
 405         __format = __mt9t001_get_pad_format(mt9t001, cfg, format->pad,
 406                                             format->which);
 407         __format->width = __crop->width / hratio;
 408         __format->height = __crop->height / vratio;
 409
 410         format->format = *__format;
 411
 412         return 0;
 413 }
 414
 415 static int mt9t001_get_selection(struct v4l2_subdev *subdev,
 416                                  struct v4l2_subdev_pad_config *cfg,
 417                                  struct v4l2_subdev_selection *sel)
 418 {
 419         struct mt9t001 *mt9t001 = to_mt9t001(subdev);
 420
 421         if (sel->target != V4L2_SEL_TGT_CROP)
 422                 return -EINVAL;
 423
 424         sel->r = *__mt9t001_get_pad_crop(mt9t001, cfg, sel->pad, sel->which);
 425         return 0;
 426 }
 427
 428 static int mt9t001_set_selection(struct v4l2_subdev *subdev,
 429                                  struct v4l2_subdev_pad_config *cfg,
 430                                  struct v4l2_subdev_selection *sel)
 431 {
 432         struct mt9t001 *mt9t001 = to_mt9t001(subdev);
 433         struct v4l2_mbus_framefmt *__format;
 434         struct v4l2_rect *__crop;
 435         struct v4l2_rect rect;
 436
 437         if (sel->target != V4L2_SEL_TGT_CROP)
 438                 return -EINVAL;
 439
 440         /* Clamp the crop rectangle boundaries and align them to a multiple of 2
 441          * pixels.
 442          */
 443         rect.left = clamp(ALIGN(sel->r.left, 2),
 444                           MT9T001_COLUMN_START_MIN,
 445                           MT9T001_COLUMN_START_MAX);
 446         rect.top = clamp(ALIGN(sel->r.top, 2),
 447                          MT9T001_ROW_START_MIN,
 448                          MT9T001_ROW_START_MAX);
 449         rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 2),
 450                              MT9T001_WINDOW_WIDTH_MIN + 1,
 451                              MT9T001_WINDOW_WIDTH_MAX + 1);
 452         rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 2),
 453                               MT9T001_WINDOW_HEIGHT_MIN + 1,
 454                               MT9T001_WINDOW_HEIGHT_MAX + 1);
 455
 456         rect.width = min_t(unsigned int, rect.width,
 457                            MT9T001_PIXEL_ARRAY_WIDTH - rect.left);
 458         rect.height = min_t(unsigned int, rect.height,
 459                             MT9T001_PIXEL_ARRAY_HEIGHT - rect.top);
 460
 461         __crop = __mt9t001_get_pad_crop(mt9t001, cfg, sel->pad, sel->which);
 462
 463         if (rect.width != __crop->width || rect.height != __crop->height) {
 464                 /* Reset the output image size if the crop rectangle size has
 465                  * been modified.
 466                  */
 467                 __format = __mt9t001_get_pad_format(mt9t001, cfg, sel->pad,
 468                                                     sel->which);
 469                 __format->width = rect.width;
 470                 __format->height = rect.height;
 471         }
 472
 473         *__crop = rect;
 474         sel->r = rect;
 475
 476         return 0;
 477 }
 478
 479 /* -----------------------------------------------------------------------------
 480  * V4L2 subdev control operations
 481  */
 482
 483 #define V4L2_CID_TEST_PATTERN_COLOR     (V4L2_CID_USER_BASE | 0x1001)
 484 #define V4L2_CID_BLACK_LEVEL_AUTO       (V4L2_CID_USER_BASE | 0x1002)
 485 #define V4L2_CID_BLACK_LEVEL_OFFSET     (V4L2_CID_USER_BASE | 0x1003)
 486 #define V4L2_CID_BLACK_LEVEL_CALIBRATE  (V4L2_CID_USER_BASE | 0x1004)
 487
 488 #define V4L2_CID_GAIN_RED               (V4L2_CTRL_CLASS_CAMERA | 0x1001)
 489 #define V4L2_CID_GAIN_GREEN_RED         (V4L2_CTRL_CLASS_CAMERA | 0x1002)
 490 #define V4L2_CID_GAIN_GREEN_BLUE        (V4L2_CTRL_CLASS_CAMERA | 0x1003)
 491 #define V4L2_CID_GAIN_BLUE              (V4L2_CTRL_CLASS_CAMERA | 0x1004)
 492
 493 static u16 mt9t001_gain_value(s32 *gain)
 494 {
 495         /* Gain is controlled by 2 analog stages and a digital stage. Valid
 496          * values for the 3 stages are
 497          *
 498          * Stage                Min     Max     Step
 499          * ------------------------------------------
 500          * First analog stage   x1      x2      1
 501          * Second analog stage  x1      x4      0.125
 502          * Digital stage        x1      x16     0.125
 503          *
 504          * To minimize noise, the gain stages should be used in the second
 505          * analog stage, first analog stage, digital stage order. Gain from a
 506          * previous stage should be pushed to its maximum value before the next
 507          * stage is used.
 508          */
 509         if (*gain <= 32)
 510                 return *gain;
 511
 512         if (*gain <= 64) {
 513                 *gain &= ~1;
 514                 return (1 << 6) | (*gain >> 1);
 515         }
 516
 517         *gain &= ~7;
 518         return ((*gain - 64) << 5) | (1 << 6) | 32;
 519 }
 520
 521 static int mt9t001_ctrl_freeze(struct mt9t001 *mt9t001, bool freeze)
 522 {
 523         return mt9t001_set_output_control(mt9t001,
 524                 freeze ? 0 : MT9T001_OUTPUT_CONTROL_SYNC,
 525                 freeze ? MT9T001_OUTPUT_CONTROL_SYNC : 0);
 526 }
 527
 528 static int mt9t001_s_ctrl(struct v4l2_ctrl *ctrl)
 529 {
 530         static const u8 gains[4] = {
 531                 MT9T001_RED_GAIN, MT9T001_GREEN1_GAIN,
 532                 MT9T001_GREEN2_GAIN, MT9T001_BLUE_GAIN
 533         };
 534
 535         struct mt9t001 *mt9t001 =
 536                         container_of(ctrl->handler, struct mt9t001, ctrls);
 537         struct i2c_client *client = v4l2_get_subdevdata(&mt9t001->subdev);
 538         unsigned int count;
 539         unsigned int i;
 540         u16 value;
 541         int ret;
 542
 543         switch (ctrl->id) {
 544         case V4L2_CID_GAIN_RED:
 545         case V4L2_CID_GAIN_GREEN_RED:
 546         case V4L2_CID_GAIN_GREEN_BLUE:
 547         case V4L2_CID_GAIN_BLUE:
 548
 549                 /* Disable control updates if more than one control has changed
 550                  * in the cluster.
 551                  */
 552                 for (i = 0, count = 0; i < 4; ++i) {
 553                         struct v4l2_ctrl *gain = mt9t001->gains[i];
 554
 555                         if (gain->val != gain->cur.val)
 556                                 count++;
 557                 }
 558
 559                 if (count > 1) {
 560                         ret = mt9t001_ctrl_freeze(mt9t001, true);
 561                         if (ret < 0)
 562                                 return ret;
 563                 }
 564
 565                 /* Update the gain controls. */
 566                 for (i = 0; i < 4; ++i) {
 567                         struct v4l2_ctrl *gain = mt9t001->gains[i];
 568
 569                         if (gain->val == gain->cur.val)
 570                                 continue;
 571
 572                         value = mt9t001_gain_value(&gain->val);
 573                         ret = mt9t001_write(client, gains[i], value);
 574                         if (ret < 0) {
 575                                 mt9t001_ctrl_freeze(mt9t001, false);
 576                                 return ret;
 577                         }
 578                 }
 579
 580                 /* Enable control updates. */
 581                 if (count > 1) {
 582                         ret = mt9t001_ctrl_freeze(mt9t001, false);
 583                         if (ret < 0)
 584                                 return ret;
 585                 }
 586
 587                 break;
 588
 589         case V4L2_CID_EXPOSURE:
 590                 ret = mt9t001_write(client, MT9T001_SHUTTER_WIDTH_LOW,
 591                                     ctrl->val & 0xffff);
 592                 if (ret < 0)
 593                         return ret;
 594
 595                 return mt9t001_write(client, MT9T001_SHUTTER_WIDTH_HIGH,
 596                                      ctrl->val >> 16);
 597
 598         case V4L2_CID_TEST_PATTERN:
 599                 return mt9t001_set_output_control(mt9t001,
 600                         ctrl->val ? 0 : MT9T001_OUTPUT_CONTROL_TEST_DATA,
 601                         ctrl->val ? MT9T001_OUTPUT_CONTROL_TEST_DATA : 0);
 602
 603         case V4L2_CID_TEST_PATTERN_COLOR:
 604                 return mt9t001_write(client, MT9T001_TEST_DATA, ctrl->val << 2);
 605
 606         case V4L2_CID_BLACK_LEVEL_AUTO:
 607                 value = ctrl->val ? 0 : MT9T001_BLACK_LEVEL_OVERRIDE;
 608                 ret = mt9t001_write(client, MT9T001_BLACK_LEVEL_CALIBRATION,
 609                                     value);
 610                 if (ret < 0)
 611                         return ret;
 612
 613                 mt9t001->black_level = value;
 614                 break;
 615
 616         case V4L2_CID_BLACK_LEVEL_OFFSET:
 617                 ret = mt9t001_write(client, MT9T001_GREEN1_OFFSET, ctrl->val);
 618                 if (ret < 0)
 619                         return ret;
 620
 621                 ret = mt9t001_write(client, MT9T001_GREEN2_OFFSET, ctrl->val);
 622                 if (ret < 0)
 623                         return ret;
 624
 625                 ret = mt9t001_write(client, MT9T001_RED_OFFSET, ctrl->val);
 626                 if (ret < 0)
 627                         return ret;
 628
 629                 return mt9t001_write(client, MT9T001_BLUE_OFFSET, ctrl->val);
 630
 631         case V4L2_CID_BLACK_LEVEL_CALIBRATE:
 632                 return mt9t001_write(client, MT9T001_BLACK_LEVEL_CALIBRATION,
 633                                      MT9T001_BLACK_LEVEL_RECALCULATE |
 634                                      mt9t001->black_level);
 635         }
 636
 637         return 0;
 638 }
 639
 640 static const struct v4l2_ctrl_ops mt9t001_ctrl_ops = {
 641         .s_ctrl = mt9t001_s_ctrl,
 642 };
 643
 644 static const char * const mt9t001_test_pattern_menu[] = {
 645         "Disabled",
 646         "Enabled",
 647 };
 648
 649 static const struct v4l2_ctrl_config mt9t001_ctrls[] = {
 650         {
 651                 .ops            = &mt9t001_ctrl_ops,
 652                 .id             = V4L2_CID_TEST_PATTERN_COLOR,
 653                 .type           = V4L2_CTRL_TYPE_INTEGER,
 654                 .name           = "Test Pattern Color",
 655                 .min            = 0,
 656                 .max            = 1023,
 657                 .step           = 1,
 658                 .def            = 0,
 659                 .flags          = 0,
 660         }, {
 661                 .ops            = &mt9t001_ctrl_ops,
 662                 .id             = V4L2_CID_BLACK_LEVEL_AUTO,
 663                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 664                 .name           = "Black Level, Auto",
 665                 .min            = 0,
 666                 .max            = 1,
 667                 .step           = 1,
 668                 .def            = 1,
 669                 .flags          = 0,
 670         }, {
 671                 .ops            = &mt9t001_ctrl_ops,
 672                 .id             = V4L2_CID_BLACK_LEVEL_OFFSET,
 673                 .type           = V4L2_CTRL_TYPE_INTEGER,
 674                 .name           = "Black Level, Offset",
 675                 .min            = -256,
 676                 .max            = 255,
 677                 .step           = 1,
 678                 .def            = 32,
 679                 .flags          = 0,
 680         }, {
 681                 .ops            = &mt9t001_ctrl_ops,
 682                 .id             = V4L2_CID_BLACK_LEVEL_CALIBRATE,
 683                 .type           = V4L2_CTRL_TYPE_BUTTON,
 684                 .name           = "Black Level, Calibrate",
 685                 .min            = 0,
 686                 .max            = 0,
 687                 .step           = 0,
 688                 .def            = 0,
 689                 .flags          = V4L2_CTRL_FLAG_WRITE_ONLY,
 690         },
 691 };
 692
 693 static const struct v4l2_ctrl_config mt9t001_gains[] = {
 694         {
 695                 .ops            = &mt9t001_ctrl_ops,
 696                 .id             = V4L2_CID_GAIN_RED,
 697                 .type           = V4L2_CTRL_TYPE_INTEGER,
 698                 .name           = "Gain, Red",
 699                 .min            = MT9T001_GLOBAL_GAIN_MIN,
 700                 .max            = MT9T001_GLOBAL_GAIN_MAX,
 701                 .step           = 1,
 702                 .def            = MT9T001_GLOBAL_GAIN_MIN,
 703                 .flags          = 0,
 704         }, {
 705                 .ops            = &mt9t001_ctrl_ops,
 706                 .id             = V4L2_CID_GAIN_GREEN_RED,
 707                 .type           = V4L2_CTRL_TYPE_INTEGER,
 708                 .name           = "Gain, Green (R)",
 709                 .min            = MT9T001_GLOBAL_GAIN_MIN,
 710                 .max            = MT9T001_GLOBAL_GAIN_MAX,
 711                 .step           = 1,
 712                 .def            = MT9T001_GLOBAL_GAIN_MIN,
 713                 .flags          = 0,
 714         }, {
 715                 .ops            = &mt9t001_ctrl_ops,
 716                 .id             = V4L2_CID_GAIN_GREEN_BLUE,
 717                 .type           = V4L2_CTRL_TYPE_INTEGER,
 718                 .name           = "Gain, Green (B)",
 719                 .min            = MT9T001_GLOBAL_GAIN_MIN,
 720                 .max            = MT9T001_GLOBAL_GAIN_MAX,
 721                 .step           = 1,
 722                 .def            = MT9T001_GLOBAL_GAIN_MIN,
 723                 .flags          = 0,
 724         }, {
 725                 .ops            = &mt9t001_ctrl_ops,
 726                 .id             = V4L2_CID_GAIN_BLUE,
 727                 .type           = V4L2_CTRL_TYPE_INTEGER,
 728                 .name           = "Gain, Blue",
 729                 .min            = MT9T001_GLOBAL_GAIN_MIN,
 730                 .max            = MT9T001_GLOBAL_GAIN_MAX,
 731                 .step           = 1,
 732                 .def            = MT9T001_GLOBAL_GAIN_MIN,
 733                 .flags          = 0,
 734         },
 735 };
 736
 737 /* -----------------------------------------------------------------------------
 738  * V4L2 subdev core operations
 739  */
 740
 741 static int mt9t001_set_power(struct v4l2_subdev *subdev, int on)
 742 {
 743         struct mt9t001 *mt9t001 = to_mt9t001(subdev);
 744         int ret = 0;
 745
 746         mutex_lock(&mt9t001->power_lock);
 747
 748         /* If the power count is modified from 0 to != 0 or from != 0 to 0,
 749          * update the power state.
 750          */
 751         if (mt9t001->power_count == !on) {
 752                 ret = __mt9t001_set_power(mt9t001, !!on);
 753                 if (ret < 0)
 754                         goto out;
 755         }
 756
 757         /* Update the power count. */
 758         mt9t001->power_count += on ? 1 : -1;
 759         WARN_ON(mt9t001->power_count < 0);
 760
 761 out:
 762         mutex_unlock(&mt9t001->power_lock);
 763         return ret;
 764 }
 765
 766 /* -----------------------------------------------------------------------------
 767  * V4L2 subdev internal operations
 768  */
 769
 770 static int mt9t001_registered(struct v4l2_subdev *subdev)
 771 {
 772         struct i2c_client *client = v4l2_get_subdevdata(subdev);
 773         struct mt9t001 *mt9t001 = to_mt9t001(subdev);
 774         s32 data;
 775         int ret;
 776
 777         ret = mt9t001_power_on(mt9t001);
 778         if (ret < 0) {
 779                 dev_err(&client->dev, "MT9T001 power up failed\n");
 780                 return ret;
 781         }
 782
 783         /* Read out the chip version register */
 784         data = mt9t001_read(client, MT9T001_CHIP_VERSION);
 785         mt9t001_power_off(mt9t001);
 786
 787         if (data != MT9T001_CHIP_ID) {
 788                 dev_err(&client->dev,
 789                         "MT9T001 not detected, wrong version 0x%04x\n", data);
 790                 return -ENODEV;
 791         }
 792
 793         dev_info(&client->dev, "MT9T001 detected at address 0x%02x\n",
 794                  client->addr);
 795
 796         return 0;
 797 }
 798
 799 static int mt9t001_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 800 {
 801         struct v4l2_mbus_framefmt *format;
 802         struct v4l2_rect *crop;
 803
 804         crop = v4l2_subdev_get_try_crop(subdev, fh->pad, 0);
 805         crop->left = MT9T001_COLUMN_START_DEF;
 806         crop->top = MT9T001_ROW_START_DEF;
 807         crop->width = MT9T001_WINDOW_WIDTH_DEF + 1;
 808         crop->height = MT9T001_WINDOW_HEIGHT_DEF + 1;
 809
 810         format = v4l2_subdev_get_try_format(subdev, fh->pad, 0);
 811         format->code = MEDIA_BUS_FMT_SGRBG10_1X10;
 812         format->width = MT9T001_WINDOW_WIDTH_DEF + 1;
 813         format->height = MT9T001_WINDOW_HEIGHT_DEF + 1;
 814         format->field = V4L2_FIELD_NONE;
 815         format->colorspace = V4L2_COLORSPACE_SRGB;
 816
 817         return mt9t001_set_power(subdev, 1);
 818 }
 819
 820 static int mt9t001_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
 821 {
 822         return mt9t001_set_power(subdev, 0);
 823 }
 824
 825 static struct v4l2_subdev_core_ops mt9t001_subdev_core_ops = {
 826         .s_power = mt9t001_set_power,
 827 };
 828
 829 static struct v4l2_subdev_video_ops mt9t001_subdev_video_ops = {
 830         .s_stream = mt9t001_s_stream,
 831 };
 832
 833 static struct v4l2_subdev_pad_ops mt9t001_subdev_pad_ops = {
 834         .enum_mbus_code = mt9t001_enum_mbus_code,
 835         .enum_frame_size = mt9t001_enum_frame_size,
 836         .get_fmt = mt9t001_get_format,
 837         .set_fmt = mt9t001_set_format,
 838         .get_selection = mt9t001_get_selection,
 839         .set_selection = mt9t001_set_selection,
 840 };
 841
 842 static struct v4l2_subdev_ops mt9t001_subdev_ops = {
 843         .core = &mt9t001_subdev_core_ops,
 844         .video = &mt9t001_subdev_video_ops,
 845         .pad = &mt9t001_subdev_pad_ops,
 846 };
 847
 848 static const struct v4l2_subdev_internal_ops mt9t001_subdev_internal_ops = {
 849         .registered = mt9t001_registered,
 850         .open = mt9t001_open,
 851         .close = mt9t001_close,
 852 };
 853
 854 static int mt9t001_probe(struct i2c_client *client,
 855                          const struct i2c_device_id *did)
 856 {
 857         struct mt9t001_platform_data *pdata = client->dev.platform_data;
 858         struct mt9t001 *mt9t001;
 859         unsigned int i;
 860         int ret;
 861
 862         if (pdata == NULL) {
 863                 dev_err(&client->dev, "No platform data\n");
 864                 return -EINVAL;
 865         }
 866
 867         if (!i2c_check_functionality(client->adapter,
 868                                      I2C_FUNC_SMBUS_WORD_DATA)) {
 869                 dev_warn(&client->adapter->dev,
 870                          "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
 871                 return -EIO;
 872         }
 873
 874         mt9t001 = devm_kzalloc(&client->dev, sizeof(*mt9t001), GFP_KERNEL);
 875         if (!mt9t001)
 876                 return -ENOMEM;
 877
 878         mutex_init(&mt9t001->power_lock);
 879         mt9t001->output_control = MT9T001_OUTPUT_CONTROL_DEF;
 880
 881         mt9t001->regulators[0].supply = "vdd";
 882         mt9t001->regulators[1].supply = "vaa";
 883
 884         ret = devm_regulator_bulk_get(&client->dev, 2, mt9t001->regulators);
 885         if (ret < 0) {
 886                 dev_err(&client->dev, "Unable to get regulators\n");
 887                 return ret;
 888         }
 889
 890         mt9t001->clk = devm_clk_get(&client->dev, NULL);
 891         if (IS_ERR(mt9t001->clk)) {
 892                 dev_err(&client->dev, "Unable to get clock\n");
 893                 return PTR_ERR(mt9t001->clk);
 894         }
 895
 896         v4l2_ctrl_handler_init(&mt9t001->ctrls, ARRAY_SIZE(mt9t001_ctrls) +
 897                                                 ARRAY_SIZE(mt9t001_gains) + 4);
 898
 899         v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
 900                           V4L2_CID_EXPOSURE, MT9T001_SHUTTER_WIDTH_MIN,
 901                           MT9T001_SHUTTER_WIDTH_MAX, 1,
 902                           MT9T001_SHUTTER_WIDTH_DEF);
 903         v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
 904                           V4L2_CID_BLACK_LEVEL, 1, 1, 1, 1);
 905         v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
 906                           V4L2_CID_PIXEL_RATE, pdata->ext_clk, pdata->ext_clk,
 907                           1, pdata->ext_clk);
 908         v4l2_ctrl_new_std_menu_items(&mt9t001->ctrls, &mt9t001_ctrl_ops,
 909                         V4L2_CID_TEST_PATTERN,
 910                         ARRAY_SIZE(mt9t001_test_pattern_menu) - 1, 0,
 911                         0, mt9t001_test_pattern_menu);
 912
 913         for (i = 0; i < ARRAY_SIZE(mt9t001_ctrls); ++i)
 914                 v4l2_ctrl_new_custom(&mt9t001->ctrls, &mt9t001_ctrls[i], NULL);
 915
 916         for (i = 0; i < ARRAY_SIZE(mt9t001_gains); ++i)
 917                 mt9t001->gains[i] = v4l2_ctrl_new_custom(&mt9t001->ctrls,
 918                         &mt9t001_gains[i], NULL);
 919
 920         v4l2_ctrl_cluster(ARRAY_SIZE(mt9t001_gains), mt9t001->gains);
 921
 922         mt9t001->subdev.ctrl_handler = &mt9t001->ctrls;
 923
 924         if (mt9t001->ctrls.error) {
 925                 printk(KERN_INFO "%s: control initialization error %d\n",
 926                        __func__, mt9t001->ctrls.error);
 927                 ret = -EINVAL;
 928                 goto done;
 929         }
 930
 931         mt9t001->crop.left = MT9T001_COLUMN_START_DEF;
 932         mt9t001->crop.top = MT9T001_ROW_START_DEF;
 933         mt9t001->crop.width = MT9T001_WINDOW_WIDTH_DEF + 1;
 934         mt9t001->crop.height = MT9T001_WINDOW_HEIGHT_DEF + 1;
 935
 936         mt9t001->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
 937         mt9t001->format.width = MT9T001_WINDOW_WIDTH_DEF + 1;
 938         mt9t001->format.height = MT9T001_WINDOW_HEIGHT_DEF + 1;
 939         mt9t001->format.field = V4L2_FIELD_NONE;
 940         mt9t001->format.colorspace = V4L2_COLORSPACE_SRGB;
 941
 942         v4l2_i2c_subdev_init(&mt9t001->subdev, client, &mt9t001_subdev_ops);
 943         mt9t001->subdev.internal_ops = &mt9t001_subdev_internal_ops;
 944         mt9t001->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 945
 946         mt9t001->pad.flags = MEDIA_PAD_FL_SOURCE;
 947         ret = media_entity_pads_init(&mt9t001->subdev.entity, 1, &mt9t001->pad);
 948
 949 done:
 950         if (ret < 0) {
 951                 v4l2_ctrl_handler_free(&mt9t001->ctrls);
 952                 media_entity_cleanup(&mt9t001->subdev.entity);
 953         }
 954
 955         return ret;
 956 }
 957
 958 static int mt9t001_remove(struct i2c_client *client)
 959 {
 960         struct v4l2_subdev *subdev = i2c_get_clientdata(client);
 961         struct mt9t001 *mt9t001 = to_mt9t001(subdev);
 962
 963         v4l2_ctrl_handler_free(&mt9t001->ctrls);
 964         v4l2_device_unregister_subdev(subdev);
 965         media_entity_cleanup(&subdev->entity);
 966         return 0;
 967 }
 968
 969 static const struct i2c_device_id mt9t001_id[] = {
 970         { "mt9t001", 0 },
 971         { }
 972 };
 973 MODULE_DEVICE_TABLE(i2c, mt9t001_id);
 974
 975 static struct i2c_driver mt9t001_driver = {
 976         .driver = {
 977                 .name = "mt9t001",
 978         },
 979         .probe          = mt9t001_probe,
 980         .remove         = mt9t001_remove,
 981         .id_table       = mt9t001_id,
 982 };
 983
 984 module_i2c_driver(mt9t001_driver);
 985
 986 MODULE_DESCRIPTION("Aptina (Micron) MT9T001 Camera driver");
 987 MODULE_AUTHOR("Laurent Pinchart <laurent.pinchart@ideasonboard.com>");
 988 MODULE_LICENSE("GPL");