drivers/media/i2c/mt9m111.c

   1 /*
   2  * Driver for MT9M111/MT9M112/MT9M131 CMOS Image Sensor from Micron/Aptina
   3  *
   4  * Copyright (C) 2008, Robert Jarzmik <[email protected]>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License version 2 as
   8  * published by the Free Software Foundation.
   9  */
  10 #include <linux/videodev2.h>
  11 #include <linux/slab.h>
  12 #include <linux/i2c.h>
  13 #include <linux/log2.h>
  14 #include <linux/gpio.h>
  15 #include <linux/delay.h>
  16 #include <linux/v4l2-mediabus.h>
  17 #include <linux/module.h>
  18 #include <linux/property.h>
  19
  20 #include <media/v4l2-async.h>
  21 #include <media/v4l2-clk.h>
  22 #include <media/v4l2-common.h>
  23 #include <media/v4l2-ctrls.h>
  24 #include <media/v4l2-device.h>
  25 #include <media/v4l2-event.h>
  26 #include <media/v4l2-fwnode.h>
  27
  28 /*
  29  * MT9M111, MT9M112 and MT9M131:
  30  * i2c address is 0x48 or 0x5d (depending on SADDR pin)
  31  * The platform has to define struct i2c_board_info objects and link to them
  32  * from struct soc_camera_host_desc
  33  */
  34
  35 /*
  36  * Sensor core register addresses (0x000..0x0ff)
  37  */
  38 #define MT9M111_CHIP_VERSION            0x000
  39 #define MT9M111_ROW_START               0x001
  40 #define MT9M111_COLUMN_START            0x002
  41 #define MT9M111_WINDOW_HEIGHT           0x003
  42 #define MT9M111_WINDOW_WIDTH            0x004
  43 #define MT9M111_HORIZONTAL_BLANKING_B   0x005
  44 #define MT9M111_VERTICAL_BLANKING_B     0x006
  45 #define MT9M111_HORIZONTAL_BLANKING_A   0x007
  46 #define MT9M111_VERTICAL_BLANKING_A     0x008
  47 #define MT9M111_SHUTTER_WIDTH           0x009
  48 #define MT9M111_ROW_SPEED               0x00a
  49 #define MT9M111_EXTRA_DELAY             0x00b
  50 #define MT9M111_SHUTTER_DELAY           0x00c
  51 #define MT9M111_RESET                   0x00d
  52 #define MT9M111_READ_MODE_B             0x020
  53 #define MT9M111_READ_MODE_A             0x021
  54 #define MT9M111_FLASH_CONTROL           0x023
  55 #define MT9M111_GREEN1_GAIN             0x02b
  56 #define MT9M111_BLUE_GAIN               0x02c
  57 #define MT9M111_RED_GAIN                0x02d
  58 #define MT9M111_GREEN2_GAIN             0x02e
  59 #define MT9M111_GLOBAL_GAIN             0x02f
  60 #define MT9M111_CONTEXT_CONTROL         0x0c8
  61 #define MT9M111_PAGE_MAP                0x0f0
  62 #define MT9M111_BYTE_WISE_ADDR          0x0f1
  63
  64 #define MT9M111_RESET_SYNC_CHANGES      (1 << 15)
  65 #define MT9M111_RESET_RESTART_BAD_FRAME (1 << 9)
  66 #define MT9M111_RESET_SHOW_BAD_FRAMES   (1 << 8)
  67 #define MT9M111_RESET_RESET_SOC         (1 << 5)
  68 #define MT9M111_RESET_OUTPUT_DISABLE    (1 << 4)
  69 #define MT9M111_RESET_CHIP_ENABLE       (1 << 3)
  70 #define MT9M111_RESET_ANALOG_STANDBY    (1 << 2)
  71 #define MT9M111_RESET_RESTART_FRAME     (1 << 1)
  72 #define MT9M111_RESET_RESET_MODE        (1 << 0)
  73
  74 #define MT9M111_RM_FULL_POWER_RD        (0 << 10)
  75 #define MT9M111_RM_LOW_POWER_RD         (1 << 10)
  76 #define MT9M111_RM_COL_SKIP_4X          (1 << 5)
  77 #define MT9M111_RM_ROW_SKIP_4X          (1 << 4)
  78 #define MT9M111_RM_COL_SKIP_2X          (1 << 3)
  79 #define MT9M111_RM_ROW_SKIP_2X          (1 << 2)
  80 #define MT9M111_RMB_MIRROR_COLS         (1 << 1)
  81 #define MT9M111_RMB_MIRROR_ROWS         (1 << 0)
  82 #define MT9M111_CTXT_CTRL_RESTART       (1 << 15)
  83 #define MT9M111_CTXT_CTRL_DEFECTCOR_B   (1 << 12)
  84 #define MT9M111_CTXT_CTRL_RESIZE_B      (1 << 10)
  85 #define MT9M111_CTXT_CTRL_CTRL2_B       (1 << 9)
  86 #define MT9M111_CTXT_CTRL_GAMMA_B       (1 << 8)
  87 #define MT9M111_CTXT_CTRL_XENON_EN      (1 << 7)
  88 #define MT9M111_CTXT_CTRL_READ_MODE_B   (1 << 3)
  89 #define MT9M111_CTXT_CTRL_LED_FLASH_EN  (1 << 2)
  90 #define MT9M111_CTXT_CTRL_VBLANK_SEL_B  (1 << 1)
  91 #define MT9M111_CTXT_CTRL_HBLANK_SEL_B  (1 << 0)
  92
  93 /*
  94  * Colorpipe register addresses (0x100..0x1ff)
  95  */
  96 #define MT9M111_OPER_MODE_CTRL          0x106
  97 #define MT9M111_OUTPUT_FORMAT_CTRL      0x108
  98 #define MT9M111_TPG_CTRL                0x148
  99 #define MT9M111_REDUCER_XZOOM_B         0x1a0
 100 #define MT9M111_REDUCER_XSIZE_B         0x1a1
 101 #define MT9M111_REDUCER_YZOOM_B         0x1a3
 102 #define MT9M111_REDUCER_YSIZE_B         0x1a4
 103 #define MT9M111_REDUCER_XZOOM_A         0x1a6
 104 #define MT9M111_REDUCER_XSIZE_A         0x1a7
 105 #define MT9M111_REDUCER_YZOOM_A         0x1a9
 106 #define MT9M111_REDUCER_YSIZE_A         0x1aa
 107 #define MT9M111_EFFECTS_MODE            0x1e2
 108
 109 #define MT9M111_OUTPUT_FORMAT_CTRL2_A   0x13a
 110 #define MT9M111_OUTPUT_FORMAT_CTRL2_B   0x19b
 111
 112 #define MT9M111_OPMODE_AUTOEXPO_EN      (1 << 14)
 113 #define MT9M111_OPMODE_AUTOWHITEBAL_EN  (1 << 1)
 114 #define MT9M111_OUTFMT_FLIP_BAYER_COL   (1 << 9)
 115 #define MT9M111_OUTFMT_FLIP_BAYER_ROW   (1 << 8)
 116 #define MT9M111_OUTFMT_PROCESSED_BAYER  (1 << 14)
 117 #define MT9M111_OUTFMT_BYPASS_IFP       (1 << 10)
 118 #define MT9M111_OUTFMT_INV_PIX_CLOCK    (1 << 9)
 119 #define MT9M111_OUTFMT_RGB              (1 << 8)
 120 #define MT9M111_OUTFMT_RGB565           (0 << 6)
 121 #define MT9M111_OUTFMT_RGB555           (1 << 6)
 122 #define MT9M111_OUTFMT_RGB444x          (2 << 6)
 123 #define MT9M111_OUTFMT_RGBx444          (3 << 6)
 124 #define MT9M111_OUTFMT_TST_RAMP_OFF     (0 << 4)
 125 #define MT9M111_OUTFMT_TST_RAMP_COL     (1 << 4)
 126 #define MT9M111_OUTFMT_TST_RAMP_ROW     (2 << 4)
 127 #define MT9M111_OUTFMT_TST_RAMP_FRAME   (3 << 4)
 128 #define MT9M111_OUTFMT_SHIFT_3_UP       (1 << 3)
 129 #define MT9M111_OUTFMT_AVG_CHROMA       (1 << 2)
 130 #define MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN  (1 << 1)
 131 #define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B (1 << 0)
 132 #define MT9M111_TPG_SEL_MASK            GENMASK(2, 0)
 133 #define MT9M111_EFFECTS_MODE_MASK       GENMASK(2, 0)
 134 #define MT9M111_RM_PWR_MASK             BIT(10)
 135 #define MT9M111_RM_SKIP2_MASK           GENMASK(3, 2)
 136
 137 /*
 138  * Camera control register addresses (0x200..0x2ff not implemented)
 139  */
 140
 141 #define reg_read(reg) mt9m111_reg_read(client, MT9M111_##reg)
 142 #define reg_write(reg, val) mt9m111_reg_write(client, MT9M111_##reg, (val))
 143 #define reg_set(reg, val) mt9m111_reg_set(client, MT9M111_##reg, (val))
 144 #define reg_clear(reg, val) mt9m111_reg_clear(client, MT9M111_##reg, (val))
 145 #define reg_mask(reg, val, mask) mt9m111_reg_mask(client, MT9M111_##reg, \
 146                 (val), (mask))
 147
 148 #define MT9M111_MIN_DARK_ROWS   8
 149 #define MT9M111_MIN_DARK_COLS   26
 150 #define MT9M111_MAX_HEIGHT      1024
 151 #define MT9M111_MAX_WIDTH       1280
 152
 153 struct mt9m111_context {
 154         u16 read_mode;
 155         u16 blanking_h;
 156         u16 blanking_v;
 157         u16 reducer_xzoom;
 158         u16 reducer_yzoom;
 159         u16 reducer_xsize;
 160         u16 reducer_ysize;
 161         u16 output_fmt_ctrl2;
 162         u16 control;
 163 };
 164
 165 static struct mt9m111_context context_a = {
 166         .read_mode              = MT9M111_READ_MODE_A,
 167         .blanking_h             = MT9M111_HORIZONTAL_BLANKING_A,
 168         .blanking_v             = MT9M111_VERTICAL_BLANKING_A,
 169         .reducer_xzoom          = MT9M111_REDUCER_XZOOM_A,
 170         .reducer_yzoom          = MT9M111_REDUCER_YZOOM_A,
 171         .reducer_xsize          = MT9M111_REDUCER_XSIZE_A,
 172         .reducer_ysize          = MT9M111_REDUCER_YSIZE_A,
 173         .output_fmt_ctrl2       = MT9M111_OUTPUT_FORMAT_CTRL2_A,
 174         .control                = MT9M111_CTXT_CTRL_RESTART,
 175 };
 176
 177 static struct mt9m111_context context_b = {
 178         .read_mode              = MT9M111_READ_MODE_B,
 179         .blanking_h             = MT9M111_HORIZONTAL_BLANKING_B,
 180         .blanking_v             = MT9M111_VERTICAL_BLANKING_B,
 181         .reducer_xzoom          = MT9M111_REDUCER_XZOOM_B,
 182         .reducer_yzoom          = MT9M111_REDUCER_YZOOM_B,
 183         .reducer_xsize          = MT9M111_REDUCER_XSIZE_B,
 184         .reducer_ysize          = MT9M111_REDUCER_YSIZE_B,
 185         .output_fmt_ctrl2       = MT9M111_OUTPUT_FORMAT_CTRL2_B,
 186         .control                = MT9M111_CTXT_CTRL_RESTART |
 187                 MT9M111_CTXT_CTRL_DEFECTCOR_B | MT9M111_CTXT_CTRL_RESIZE_B |
 188                 MT9M111_CTXT_CTRL_CTRL2_B | MT9M111_CTXT_CTRL_GAMMA_B |
 189                 MT9M111_CTXT_CTRL_READ_MODE_B | MT9M111_CTXT_CTRL_VBLANK_SEL_B |
 190                 MT9M111_CTXT_CTRL_HBLANK_SEL_B,
 191 };
 192
 193 /* MT9M111 has only one fixed colorspace per pixelcode */
 194 struct mt9m111_datafmt {
 195         u32     code;
 196         enum v4l2_colorspace            colorspace;
 197 };
 198
 199 static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
 200         {MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB},
 201         {MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_SRGB},
 202         {MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB},
 203         {MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_SRGB},
 204         {MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
 205         {MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
 206         {MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
 207         {MEDIA_BUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
 208         {MEDIA_BUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB},
 209         {MEDIA_BUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB},
 210         {MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
 211         {MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
 212 };
 213
 214 enum mt9m111_mode_id {
 215         MT9M111_MODE_SXGA_8FPS,
 216         MT9M111_MODE_SXGA_15FPS,
 217         MT9M111_MODE_QSXGA_30FPS,
 218         MT9M111_NUM_MODES,
 219 };
 220
 221 struct mt9m111_mode_info {
 222         unsigned int sensor_w;
 223         unsigned int sensor_h;
 224         unsigned int max_image_w;
 225         unsigned int max_image_h;
 226         unsigned int max_fps;
 227         unsigned int reg_val;
 228         unsigned int reg_mask;
 229 };
 230
 231 struct mt9m111 {
 232         struct v4l2_subdev subdev;
 233         struct v4l2_ctrl_handler hdl;
 234         struct v4l2_ctrl *gain;
 235         struct mt9m111_context *ctx;
 236         struct v4l2_rect rect;  /* cropping rectangle */
 237         struct v4l2_clk *clk;
 238         unsigned int width;     /* output */
 239         unsigned int height;    /* sizes */
 240         struct v4l2_fract frame_interval;
 241         const struct mt9m111_mode_info *current_mode;
 242         struct mutex power_lock; /* lock to protect power_count */
 243         int power_count;
 244         const struct mt9m111_datafmt *fmt;
 245         int lastpage;   /* PageMap cache value */
 246         bool is_streaming;
 247         /* user point of view - 0: falling 1: rising edge */
 248         unsigned int pclk_sample:1;
 249 #ifdef CONFIG_MEDIA_CONTROLLER
 250         struct media_pad pad;
 251 #endif
 252 };
 253
 254 static const struct mt9m111_mode_info mt9m111_mode_data[MT9M111_NUM_MODES] = {
 255         [MT9M111_MODE_SXGA_8FPS] = {
 256                 .sensor_w = 1280,
 257                 .sensor_h = 1024,
 258                 .max_image_w = 1280,
 259                 .max_image_h = 1024,
 260                 .max_fps = 8,
 261                 .reg_val = MT9M111_RM_LOW_POWER_RD,
 262                 .reg_mask = MT9M111_RM_PWR_MASK | MT9M111_RM_SKIP2_MASK,
 263         },
 264         [MT9M111_MODE_SXGA_15FPS] = {
 265                 .sensor_w = 1280,
 266                 .sensor_h = 1024,
 267                 .max_image_w = 1280,
 268                 .max_image_h = 1024,
 269                 .max_fps = 15,
 270                 .reg_val = MT9M111_RM_FULL_POWER_RD,
 271                 .reg_mask = MT9M111_RM_PWR_MASK | MT9M111_RM_SKIP2_MASK,
 272         },
 273         [MT9M111_MODE_QSXGA_30FPS] = {
 274                 .sensor_w = 1280,
 275                 .sensor_h = 1024,
 276                 .max_image_w = 640,
 277                 .max_image_h = 512,
 278                 .max_fps = 30,
 279                 .reg_val = MT9M111_RM_LOW_POWER_RD | MT9M111_RM_COL_SKIP_2X |
 280                            MT9M111_RM_ROW_SKIP_2X,
 281                 .reg_mask = MT9M111_RM_PWR_MASK | MT9M111_RM_SKIP2_MASK,
 282         },
 283 };
 284
 285 /* Find a data format by a pixel code */
 286 static const struct mt9m111_datafmt *mt9m111_find_datafmt(struct mt9m111 *mt9m111,
 287                                                 u32 code)
 288 {
 289         int i;
 290         for (i = 0; i < ARRAY_SIZE(mt9m111_colour_fmts); i++)
 291                 if (mt9m111_colour_fmts[i].code == code)
 292                         return mt9m111_colour_fmts + i;
 293
 294         return mt9m111->fmt;
 295 }
 296
 297 static struct mt9m111 *to_mt9m111(const struct i2c_client *client)
 298 {
 299         return container_of(i2c_get_clientdata(client), struct mt9m111, subdev);
 300 }
 301
 302 static int reg_page_map_set(struct i2c_client *client, const u16 reg)
 303 {
 304         int ret;
 305         u16 page;
 306         struct mt9m111 *mt9m111 = to_mt9m111(client);
 307
 308         page = (reg >> 8);
 309         if (page == mt9m111->lastpage)
 310                 return 0;
 311         if (page > 2)
 312                 return -EINVAL;
 313
 314         ret = i2c_smbus_write_word_swapped(client, MT9M111_PAGE_MAP, page);
 315         if (!ret)
 316                 mt9m111->lastpage = page;
 317         return ret;
 318 }
 319
 320 static int mt9m111_reg_read(struct i2c_client *client, const u16 reg)
 321 {
 322         int ret;
 323
 324         ret = reg_page_map_set(client, reg);
 325         if (!ret)
 326                 ret = i2c_smbus_read_word_swapped(client, reg & 0xff);
 327
 328         dev_dbg(&client->dev, "read  reg.%03x -> %04x\n", reg, ret);
 329         return ret;
 330 }
 331
 332 static int mt9m111_reg_write(struct i2c_client *client, const u16 reg,
 333                              const u16 data)
 334 {
 335         int ret;
 336
 337         ret = reg_page_map_set(client, reg);
 338         if (!ret)
 339                 ret = i2c_smbus_write_word_swapped(client, reg & 0xff, data);
 340         dev_dbg(&client->dev, "write reg.%03x = %04x -> %d\n", reg, data, ret);
 341         return ret;
 342 }
 343
 344 static int mt9m111_reg_set(struct i2c_client *client, const u16 reg,
 345                            const u16 data)
 346 {
 347         int ret;
 348
 349         ret = mt9m111_reg_read(client, reg);
 350         if (ret >= 0)
 351                 ret = mt9m111_reg_write(client, reg, ret | data);
 352         return ret;
 353 }
 354
 355 static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg,
 356                              const u16 data)
 357 {
 358         int ret;
 359
 360         ret = mt9m111_reg_read(client, reg);
 361         if (ret >= 0)
 362                 ret = mt9m111_reg_write(client, reg, ret & ~data);
 363         return ret;
 364 }
 365
 366 static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg,
 367                             const u16 data, const u16 mask)
 368 {
 369         int ret;
 370
 371         ret = mt9m111_reg_read(client, reg);
 372         if (ret >= 0)
 373                 ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data);
 374         return ret;
 375 }
 376
 377 static int mt9m111_set_context(struct mt9m111 *mt9m111,
 378                                struct mt9m111_context *ctx)
 379 {
 380         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 381         return reg_write(CONTEXT_CONTROL, ctx->control);
 382 }
 383
 384 static int mt9m111_setup_rect_ctx(struct mt9m111 *mt9m111,
 385                         struct mt9m111_context *ctx, struct v4l2_rect *rect,
 386                         unsigned int width, unsigned int height)
 387 {
 388         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 389         int ret = mt9m111_reg_write(client, ctx->reducer_xzoom, rect->width);
 390         if (!ret)
 391                 ret = mt9m111_reg_write(client, ctx->reducer_yzoom, rect->height);
 392         if (!ret)
 393                 ret = mt9m111_reg_write(client, ctx->reducer_xsize, width);
 394         if (!ret)
 395                 ret = mt9m111_reg_write(client, ctx->reducer_ysize, height);
 396         return ret;
 397 }
 398
 399 static int mt9m111_setup_geometry(struct mt9m111 *mt9m111, struct v4l2_rect *rect,
 400                         int width, int height, u32 code)
 401 {
 402         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 403         int ret;
 404
 405         ret = reg_write(COLUMN_START, rect->left);
 406         if (!ret)
 407                 ret = reg_write(ROW_START, rect->top);
 408
 409         if (!ret)
 410                 ret = reg_write(WINDOW_WIDTH, rect->width);
 411         if (!ret)
 412                 ret = reg_write(WINDOW_HEIGHT, rect->height);
 413
 414         if (code != MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
 415                 /* IFP in use, down-scaling possible */
 416                 if (!ret)
 417                         ret = mt9m111_setup_rect_ctx(mt9m111, &context_b,
 418                                                      rect, width, height);
 419                 if (!ret)
 420                         ret = mt9m111_setup_rect_ctx(mt9m111, &context_a,
 421                                                      rect, width, height);
 422         }
 423
 424         dev_dbg(&client->dev, "%s(%x): %ux%u@%u:%u -> %ux%u = %d\n",
 425                 __func__, code, rect->width, rect->height, rect->left, rect->top,
 426                 width, height, ret);
 427
 428         return ret;
 429 }
 430
 431 static int mt9m111_enable(struct mt9m111 *mt9m111)
 432 {
 433         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 434         return reg_write(RESET, MT9M111_RESET_CHIP_ENABLE);
 435 }
 436
 437 static int mt9m111_reset(struct mt9m111 *mt9m111)
 438 {
 439         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 440         int ret;
 441
 442         ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
 443         if (!ret)
 444                 ret = reg_set(RESET, MT9M111_RESET_RESET_SOC);
 445         if (!ret)
 446                 ret = reg_clear(RESET, MT9M111_RESET_RESET_MODE
 447                                 | MT9M111_RESET_RESET_SOC);
 448
 449         return ret;
 450 }
 451
 452 static int mt9m111_set_selection(struct v4l2_subdev *sd,
 453                                  struct v4l2_subdev_pad_config *cfg,
 454                                  struct v4l2_subdev_selection *sel)
 455 {
 456         struct i2c_client *client = v4l2_get_subdevdata(sd);
 457         struct mt9m111 *mt9m111 = to_mt9m111(client);
 458         struct v4l2_rect rect = sel->r;
 459         int width, height;
 460         int ret, align = 0;
 461
 462         if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
 463             sel->target != V4L2_SEL_TGT_CROP)
 464                 return -EINVAL;
 465
 466         if (mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
 467             mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
 468                 /* Bayer format - even size lengths */
 469                 align = 1;
 470                 /* Let the user play with the starting pixel */
 471         }
 472
 473         /* FIXME: the datasheet doesn't specify minimum sizes */
 474         v4l_bound_align_image(&rect.width, 2, MT9M111_MAX_WIDTH, align,
 475                               &rect.height, 2, MT9M111_MAX_HEIGHT, align, 0);
 476         rect.left = clamp(rect.left, MT9M111_MIN_DARK_COLS,
 477                           MT9M111_MIN_DARK_COLS + MT9M111_MAX_WIDTH -
 478                           (__s32)rect.width);
 479         rect.top = clamp(rect.top, MT9M111_MIN_DARK_ROWS,
 480                          MT9M111_MIN_DARK_ROWS + MT9M111_MAX_HEIGHT -
 481                          (__s32)rect.height);
 482
 483         width = min(mt9m111->width, rect.width);
 484         height = min(mt9m111->height, rect.height);
 485
 486         ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
 487         if (!ret) {
 488                 mt9m111->rect = rect;
 489                 mt9m111->width = width;
 490                 mt9m111->height = height;
 491         }
 492
 493         return ret;
 494 }
 495
 496 static int mt9m111_get_selection(struct v4l2_subdev *sd,
 497                                  struct v4l2_subdev_pad_config *cfg,
 498                                  struct v4l2_subdev_selection *sel)
 499 {
 500         struct i2c_client *client = v4l2_get_subdevdata(sd);
 501         struct mt9m111 *mt9m111 = to_mt9m111(client);
 502
 503         if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
 504                 return -EINVAL;
 505
 506         switch (sel->target) {
 507         case V4L2_SEL_TGT_CROP_BOUNDS:
 508                 sel->r.left = MT9M111_MIN_DARK_COLS;
 509                 sel->r.top = MT9M111_MIN_DARK_ROWS;
 510                 sel->r.width = MT9M111_MAX_WIDTH;
 511                 sel->r.height = MT9M111_MAX_HEIGHT;
 512                 return 0;
 513         case V4L2_SEL_TGT_CROP:
 514                 sel->r = mt9m111->rect;
 515                 return 0;
 516         default:
 517                 return -EINVAL;
 518         }
 519 }
 520
 521 static int mt9m111_get_fmt(struct v4l2_subdev *sd,
 522                 struct v4l2_subdev_pad_config *cfg,
 523                 struct v4l2_subdev_format *format)
 524 {
 525         struct v4l2_mbus_framefmt *mf = &format->format;
 526         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 527
 528         if (format->pad)
 529                 return -EINVAL;
 530
 531         if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
 532 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
 533                 mf = v4l2_subdev_get_try_format(sd, cfg, format->pad);
 534                 format->format = *mf;
 535                 return 0;
 536 #else
 537                 return -ENOTTY;
 538 #endif
 539         }
 540
 541         mf->width       = mt9m111->width;
 542         mf->height      = mt9m111->height;
 543         mf->code        = mt9m111->fmt->code;
 544         mf->colorspace  = mt9m111->fmt->colorspace;
 545         mf->field       = V4L2_FIELD_NONE;
 546         mf->ycbcr_enc   = V4L2_YCBCR_ENC_DEFAULT;
 547         mf->quantization        = V4L2_QUANTIZATION_DEFAULT;
 548         mf->xfer_func   = V4L2_XFER_FUNC_DEFAULT;
 549
 550         return 0;
 551 }
 552
 553 static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
 554                               u32 code)
 555 {
 556         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 557         u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
 558                 MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB |
 559                 MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
 560                 MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 |
 561                 MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
 562                 MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 563         int ret;
 564
 565         switch (code) {
 566         case MEDIA_BUS_FMT_SBGGR8_1X8:
 567                 data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
 568                         MT9M111_OUTFMT_RGB;
 569                 break;
 570         case MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE:
 571                 data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
 572                 break;
 573         case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
 574                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
 575                         MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
 576                 break;
 577         case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
 578                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
 579                 break;
 580         case MEDIA_BUS_FMT_RGB565_2X8_LE:
 581                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
 582                         MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
 583                 break;
 584         case MEDIA_BUS_FMT_RGB565_2X8_BE:
 585                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
 586                 break;
 587         case MEDIA_BUS_FMT_BGR565_2X8_BE:
 588                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
 589                         MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 590                 break;
 591         case MEDIA_BUS_FMT_BGR565_2X8_LE:
 592                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
 593                         MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
 594                         MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 595                 break;
 596         case MEDIA_BUS_FMT_UYVY8_2X8:
 597                 data_outfmt2 = 0;
 598                 break;
 599         case MEDIA_BUS_FMT_VYUY8_2X8:
 600                 data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 601                 break;
 602         case MEDIA_BUS_FMT_YUYV8_2X8:
 603                 data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
 604                 break;
 605         case MEDIA_BUS_FMT_YVYU8_2X8:
 606                 data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
 607                         MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 608                 break;
 609         default:
 610                 dev_err(&client->dev, "Pixel format not handled: %x\n", code);
 611                 return -EINVAL;
 612         }
 613
 614         /* receiver samples on falling edge, chip-hw default is rising */
 615         if (mt9m111->pclk_sample == 0)
 616                 mask_outfmt2 |= MT9M111_OUTFMT_INV_PIX_CLOCK;
 617
 618         ret = mt9m111_reg_mask(client, context_a.output_fmt_ctrl2,
 619                                data_outfmt2, mask_outfmt2);
 620         if (!ret)
 621                 ret = mt9m111_reg_mask(client, context_b.output_fmt_ctrl2,
 622                                        data_outfmt2, mask_outfmt2);
 623
 624         return ret;
 625 }
 626
 627 static int mt9m111_set_fmt(struct v4l2_subdev *sd,
 628                 struct v4l2_subdev_pad_config *cfg,
 629                 struct v4l2_subdev_format *format)
 630 {
 631         struct v4l2_mbus_framefmt *mf = &format->format;
 632         struct i2c_client *client = v4l2_get_subdevdata(sd);
 633         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 634         const struct mt9m111_datafmt *fmt;
 635         struct v4l2_rect *rect = &mt9m111->rect;
 636         bool bayer;
 637         int ret;
 638
 639         if (mt9m111->is_streaming)
 640                 return -EBUSY;
 641
 642         if (format->pad)
 643                 return -EINVAL;
 644
 645         fmt = mt9m111_find_datafmt(mt9m111, mf->code);
 646
 647         bayer = fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
 648                 fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE;
 649
 650         /*
 651          * With Bayer format enforce even side lengths, but let the user play
 652          * with the starting pixel
 653          */
 654         if (bayer) {
 655                 rect->width = ALIGN(rect->width, 2);
 656                 rect->height = ALIGN(rect->height, 2);
 657         }
 658
 659         if (fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
 660                 /* IFP bypass mode, no scaling */
 661                 mf->width = rect->width;
 662                 mf->height = rect->height;
 663         } else {
 664                 /* No upscaling */
 665                 if (mf->width > rect->width)
 666                         mf->width = rect->width;
 667                 if (mf->height > rect->height)
 668                         mf->height = rect->height;
 669         }
 670
 671         dev_dbg(&client->dev, "%s(): %ux%u, code=%x\n", __func__,
 672                 mf->width, mf->height, fmt->code);
 673
 674         mf->code = fmt->code;
 675         mf->colorspace = fmt->colorspace;
 676         mf->field       = V4L2_FIELD_NONE;
 677         mf->ycbcr_enc   = V4L2_YCBCR_ENC_DEFAULT;
 678         mf->quantization        = V4L2_QUANTIZATION_DEFAULT;
 679         mf->xfer_func   = V4L2_XFER_FUNC_DEFAULT;
 680
 681         if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
 682                 cfg->try_fmt = *mf;
 683                 return 0;
 684         }
 685
 686         ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
 687         if (!ret)
 688                 ret = mt9m111_set_pixfmt(mt9m111, mf->code);
 689         if (!ret) {
 690                 mt9m111->width  = mf->width;
 691                 mt9m111->height = mf->height;
 692                 mt9m111->fmt    = fmt;
 693         }
 694
 695         return ret;
 696 }
 697
 698 static const struct mt9m111_mode_info *
 699 mt9m111_find_mode(struct mt9m111 *mt9m111, unsigned int req_fps,
 700                   unsigned int width, unsigned int height)
 701 {
 702         const struct mt9m111_mode_info *mode;
 703         struct v4l2_rect *sensor_rect = &mt9m111->rect;
 704         unsigned int gap, gap_best = (unsigned int) -1;
 705         int i, best_gap_idx = MT9M111_MODE_SXGA_15FPS;
 706         bool skip_30fps = false;
 707
 708         /*
 709          * The fps selection is based on the row, column skipping mechanism.
 710          * So ensure that the sensor window is set to default else the fps
 711          * aren't calculated correctly within the sensor hw.
 712          */
 713         if (sensor_rect->width != MT9M111_MAX_WIDTH ||
 714             sensor_rect->height != MT9M111_MAX_HEIGHT) {
 715                 dev_info(mt9m111->subdev.dev,
 716                          "Framerate selection is not supported for cropped "
 717                          "images\n");
 718                 return NULL;
 719         }
 720
 721         /* 30fps only supported for images not exceeding 640x512 */
 722         if (width > MT9M111_MAX_WIDTH / 2 || height > MT9M111_MAX_HEIGHT / 2) {
 723                 dev_dbg(mt9m111->subdev.dev,
 724                         "Framerates > 15fps are supported only for images "
 725                         "not exceeding 640x512\n");
 726                 skip_30fps = true;
 727         }
 728
 729         /* find best matched fps */
 730         for (i = 0; i < MT9M111_NUM_MODES; i++) {
 731                 unsigned int fps = mt9m111_mode_data[i].max_fps;
 732
 733                 if (fps == 30 && skip_30fps)
 734                         continue;
 735
 736                 gap = abs(fps - req_fps);
 737                 if (gap < gap_best) {
 738                         best_gap_idx = i;
 739                         gap_best = gap;
 740                 }
 741         }
 742
 743         /*
 744          * Use context a/b default timing values instead of calculate blanking
 745          * timing values.
 746          */
 747         mode = &mt9m111_mode_data[best_gap_idx];
 748         mt9m111->ctx = (best_gap_idx == MT9M111_MODE_QSXGA_30FPS) ? &context_a :
 749                                                                     &context_b;
 750         return mode;
 751 }
 752
 753 #ifdef CONFIG_VIDEO_ADV_DEBUG
 754 static int mt9m111_g_register(struct v4l2_subdev *sd,
 755                               struct v4l2_dbg_register *reg)
 756 {
 757         struct i2c_client *client = v4l2_get_subdevdata(sd);
 758         int val;
 759
 760         if (reg->reg > 0x2ff)
 761                 return -EINVAL;
 762
 763         val = mt9m111_reg_read(client, reg->reg);
 764         reg->size = 2;
 765         reg->val = (u64)val;
 766
 767         if (reg->val > 0xffff)
 768                 return -EIO;
 769
 770         return 0;
 771 }
 772
 773 static int mt9m111_s_register(struct v4l2_subdev *sd,
 774                               const struct v4l2_dbg_register *reg)
 775 {
 776         struct i2c_client *client = v4l2_get_subdevdata(sd);
 777
 778         if (reg->reg > 0x2ff)
 779                 return -EINVAL;
 780
 781         if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
 782                 return -EIO;
 783
 784         return 0;
 785 }
 786 #endif
 787
 788 static int mt9m111_set_flip(struct mt9m111 *mt9m111, int flip, int mask)
 789 {
 790         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 791         int ret;
 792
 793         if (flip)
 794                 ret = mt9m111_reg_set(client, mt9m111->ctx->read_mode, mask);
 795         else
 796                 ret = mt9m111_reg_clear(client, mt9m111->ctx->read_mode, mask);
 797
 798         return ret;
 799 }
 800
 801 static int mt9m111_get_global_gain(struct mt9m111 *mt9m111)
 802 {
 803         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 804         int data;
 805
 806         data = reg_read(GLOBAL_GAIN);
 807         if (data >= 0)
 808                 return (data & 0x2f) * (1 << ((data >> 10) & 1)) *
 809                         (1 << ((data >> 9) & 1));
 810         return data;
 811 }
 812
 813 static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
 814 {
 815         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 816         u16 val;
 817
 818         if (gain > 63 * 2 * 2)
 819                 return -EINVAL;
 820
 821         if ((gain >= 64 * 2) && (gain < 63 * 2 * 2))
 822                 val = (1 << 10) | (1 << 9) | (gain / 4);
 823         else if ((gain >= 64) && (gain < 64 * 2))
 824                 val = (1 << 9) | (gain / 2);
 825         else
 826                 val = gain;
 827
 828         return reg_write(GLOBAL_GAIN, val);
 829 }
 830
 831 static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int val)
 832 {
 833         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 834
 835         if (val == V4L2_EXPOSURE_AUTO)
 836                 return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
 837         return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
 838 }
 839
 840 static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on)
 841 {
 842         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 843
 844         if (on)
 845                 return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
 846         return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
 847 }
 848
 849 static const char * const mt9m111_test_pattern_menu[] = {
 850         "Disabled",
 851         "Vertical monochrome gradient",
 852         "Flat color type 1",
 853         "Flat color type 2",
 854         "Flat color type 3",
 855         "Flat color type 4",
 856         "Flat color type 5",
 857         "Color bar",
 858 };
 859
 860 static int mt9m111_set_test_pattern(struct mt9m111 *mt9m111, int val)
 861 {
 862         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 863
 864         return mt9m111_reg_mask(client, MT9M111_TPG_CTRL, val,
 865                                 MT9M111_TPG_SEL_MASK);
 866 }
 867
 868 static int mt9m111_set_colorfx(struct mt9m111 *mt9m111, int val)
 869 {
 870         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 871         static const struct v4l2_control colorfx[] = {
 872                 { V4L2_COLORFX_NONE,            0 },
 873                 { V4L2_COLORFX_BW,              1 },
 874                 { V4L2_COLORFX_SEPIA,           2 },
 875                 { V4L2_COLORFX_NEGATIVE,        3 },
 876                 { V4L2_COLORFX_SOLARIZATION,    4 },
 877         };
 878         int i;
 879
 880         for (i = 0; i < ARRAY_SIZE(colorfx); i++) {
 881                 if (colorfx[i].id == val) {
 882                         return mt9m111_reg_mask(client, MT9M111_EFFECTS_MODE,
 883                                                 colorfx[i].value,
 884                                                 MT9M111_EFFECTS_MODE_MASK);
 885                 }
 886         }
 887
 888         return -EINVAL;
 889 }
 890
 891 static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
 892 {
 893         struct mt9m111 *mt9m111 = container_of(ctrl->handler,
 894                                                struct mt9m111, hdl);
 895
 896         switch (ctrl->id) {
 897         case V4L2_CID_VFLIP:
 898                 return mt9m111_set_flip(mt9m111, ctrl->val,
 899                                         MT9M111_RMB_MIRROR_ROWS);
 900         case V4L2_CID_HFLIP:
 901                 return mt9m111_set_flip(mt9m111, ctrl->val,
 902                                         MT9M111_RMB_MIRROR_COLS);
 903         case V4L2_CID_GAIN:
 904                 return mt9m111_set_global_gain(mt9m111, ctrl->val);
 905         case V4L2_CID_EXPOSURE_AUTO:
 906                 return mt9m111_set_autoexposure(mt9m111, ctrl->val);
 907         case V4L2_CID_AUTO_WHITE_BALANCE:
 908                 return mt9m111_set_autowhitebalance(mt9m111, ctrl->val);
 909         case V4L2_CID_TEST_PATTERN:
 910                 return mt9m111_set_test_pattern(mt9m111, ctrl->val);
 911         case V4L2_CID_COLORFX:
 912                 return mt9m111_set_colorfx(mt9m111, ctrl->val);
 913         }
 914
 915         return -EINVAL;
 916 }
 917
 918 static int mt9m111_suspend(struct mt9m111 *mt9m111)
 919 {
 920         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 921         int ret;
 922
 923         v4l2_ctrl_s_ctrl(mt9m111->gain, mt9m111_get_global_gain(mt9m111));
 924
 925         ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
 926         if (!ret)
 927                 ret = reg_set(RESET, MT9M111_RESET_RESET_SOC |
 928                               MT9M111_RESET_OUTPUT_DISABLE |
 929                               MT9M111_RESET_ANALOG_STANDBY);
 930         if (!ret)
 931                 ret = reg_clear(RESET, MT9M111_RESET_CHIP_ENABLE);
 932
 933         return ret;
 934 }
 935
 936 static void mt9m111_restore_state(struct mt9m111 *mt9m111)
 937 {
 938         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 939
 940         mt9m111_set_context(mt9m111, mt9m111->ctx);
 941         mt9m111_set_pixfmt(mt9m111, mt9m111->fmt->code);
 942         mt9m111_setup_geometry(mt9m111, &mt9m111->rect,
 943                         mt9m111->width, mt9m111->height, mt9m111->fmt->code);
 944         v4l2_ctrl_handler_setup(&mt9m111->hdl);
 945         mt9m111_reg_mask(client, mt9m111->ctx->read_mode,
 946                          mt9m111->current_mode->reg_val,
 947                          mt9m111->current_mode->reg_mask);
 948 }
 949
 950 static int mt9m111_resume(struct mt9m111 *mt9m111)
 951 {
 952         int ret = mt9m111_enable(mt9m111);
 953         if (!ret)
 954                 ret = mt9m111_reset(mt9m111);
 955         if (!ret)
 956                 mt9m111_restore_state(mt9m111);
 957
 958         return ret;
 959 }
 960
 961 static int mt9m111_init(struct mt9m111 *mt9m111)
 962 {
 963         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 964         int ret;
 965
 966         ret = mt9m111_enable(mt9m111);
 967         if (!ret)
 968                 ret = mt9m111_reset(mt9m111);
 969         if (!ret)
 970                 ret = mt9m111_set_context(mt9m111, mt9m111->ctx);
 971         if (ret)
 972                 dev_err(&client->dev, "mt9m111 init failed: %d\n", ret);
 973         return ret;
 974 }
 975
 976 static int mt9m111_power_on(struct mt9m111 *mt9m111)
 977 {
 978         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 979         int ret;
 980
 981         ret = v4l2_clk_enable(mt9m111->clk);
 982         if (ret < 0)
 983                 return ret;
 984
 985         ret = mt9m111_resume(mt9m111);
 986         if (ret < 0) {
 987                 dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
 988                 v4l2_clk_disable(mt9m111->clk);
 989         }
 990
 991         return ret;
 992 }
 993
 994 static void mt9m111_power_off(struct mt9m111 *mt9m111)
 995 {
 996         mt9m111_suspend(mt9m111);
 997         v4l2_clk_disable(mt9m111->clk);
 998 }
 999
1000 static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
1001 {
1002         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1003         int ret = 0;
1004
1005         mutex_lock(&mt9m111->power_lock);
1006
1007         /*
1008          * If the power count is modified from 0 to != 0 or from != 0 to 0,
1009          * update the power state.
1010          */
1011         if (mt9m111->power_count == !on) {
1012                 if (on)
1013                         ret = mt9m111_power_on(mt9m111);
1014                 else
1015                         mt9m111_power_off(mt9m111);
1016         }
1017
1018         if (!ret) {
1019                 /* Update the power count. */
1020                 mt9m111->power_count += on ? 1 : -1;
1021                 WARN_ON(mt9m111->power_count < 0);
1022         }
1023
1024         mutex_unlock(&mt9m111->power_lock);
1025         return ret;
1026 }
1027
1028 static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
1029         .s_ctrl = mt9m111_s_ctrl,
1030 };
1031
1032 static const struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
1033         .s_power        = mt9m111_s_power,
1034         .log_status = v4l2_ctrl_subdev_log_status,
1035         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1036         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1037 #ifdef CONFIG_VIDEO_ADV_DEBUG
1038         .g_register     = mt9m111_g_register,
1039         .s_register     = mt9m111_s_register,
1040 #endif
1041 };
1042
1043 static int mt9m111_g_frame_interval(struct v4l2_subdev *sd,
1044                                    struct v4l2_subdev_frame_interval *fi)
1045 {
1046         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1047
1048         fi->interval = mt9m111->frame_interval;
1049
1050         return 0;
1051 }
1052
1053 static int mt9m111_s_frame_interval(struct v4l2_subdev *sd,
1054                                    struct v4l2_subdev_frame_interval *fi)
1055 {
1056         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1057         const struct mt9m111_mode_info *mode;
1058         struct v4l2_fract *fract = &fi->interval;
1059         int fps;
1060
1061         if (mt9m111->is_streaming)
1062                 return -EBUSY;
1063
1064         if (fi->pad != 0)
1065                 return -EINVAL;
1066
1067         if (fract->numerator == 0) {
1068                 fract->denominator = 30;
1069                 fract->numerator = 1;
1070         }
1071
1072         fps = DIV_ROUND_CLOSEST(fract->denominator, fract->numerator);
1073
1074         /* Find best fitting mode. Do not update the mode if no one was found. */
1075         mode = mt9m111_find_mode(mt9m111, fps, mt9m111->width, mt9m111->height);
1076         if (!mode)
1077                 return 0;
1078
1079         if (mode->max_fps != fps) {
1080                 fract->denominator = mode->max_fps;
1081                 fract->numerator = 1;
1082         }
1083
1084         mt9m111->current_mode = mode;
1085         mt9m111->frame_interval = fi->interval;
1086
1087         return 0;
1088 }
1089
1090 static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
1091                 struct v4l2_subdev_pad_config *cfg,
1092                 struct v4l2_subdev_mbus_code_enum *code)
1093 {
1094         if (code->pad || code->index >= ARRAY_SIZE(mt9m111_colour_fmts))
1095                 return -EINVAL;
1096
1097         code->code = mt9m111_colour_fmts[code->index].code;
1098         return 0;
1099 }
1100
1101 static int mt9m111_s_stream(struct v4l2_subdev *sd, int enable)
1102 {
1103         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1104
1105         mt9m111->is_streaming = !!enable;
1106         return 0;
1107 }
1108
1109 static int mt9m111_init_cfg(struct v4l2_subdev *sd,
1110                             struct v4l2_subdev_pad_config *cfg)
1111 {
1112 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
1113         struct v4l2_mbus_framefmt *format =
1114                 v4l2_subdev_get_try_format(sd, cfg, 0);
1115
1116         format->width   = MT9M111_MAX_WIDTH;
1117         format->height  = MT9M111_MAX_HEIGHT;
1118         format->code    = mt9m111_colour_fmts[0].code;
1119         format->colorspace      = mt9m111_colour_fmts[0].colorspace;
1120         format->field   = V4L2_FIELD_NONE;
1121         format->ycbcr_enc       = V4L2_YCBCR_ENC_DEFAULT;
1122         format->quantization    = V4L2_QUANTIZATION_DEFAULT;
1123         format->xfer_func       = V4L2_XFER_FUNC_DEFAULT;
1124 #endif
1125         return 0;
1126 }
1127
1128 static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
1129                                 struct v4l2_mbus_config *cfg)
1130 {
1131         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1132
1133         cfg->flags = V4L2_MBUS_MASTER |
1134                 V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
1135                 V4L2_MBUS_DATA_ACTIVE_HIGH;
1136
1137         cfg->flags |= mt9m111->pclk_sample ? V4L2_MBUS_PCLK_SAMPLE_RISING :
1138                 V4L2_MBUS_PCLK_SAMPLE_FALLING;
1139
1140         cfg->type = V4L2_MBUS_PARALLEL;
1141
1142         return 0;
1143 }
1144
1145 static const struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
1146         .g_mbus_config  = mt9m111_g_mbus_config,
1147         .s_stream       = mt9m111_s_stream,
1148         .g_frame_interval = mt9m111_g_frame_interval,
1149         .s_frame_interval = mt9m111_s_frame_interval,
1150 };
1151
1152 static const struct v4l2_subdev_pad_ops mt9m111_subdev_pad_ops = {
1153         .init_cfg       = mt9m111_init_cfg,
1154         .enum_mbus_code = mt9m111_enum_mbus_code,
1155         .get_selection  = mt9m111_get_selection,
1156         .set_selection  = mt9m111_set_selection,
1157         .get_fmt        = mt9m111_get_fmt,
1158         .set_fmt        = mt9m111_set_fmt,
1159 };
1160
1161 static const struct v4l2_subdev_ops mt9m111_subdev_ops = {
1162         .core   = &mt9m111_subdev_core_ops,
1163         .video  = &mt9m111_subdev_video_ops,
1164         .pad    = &mt9m111_subdev_pad_ops,
1165 };
1166
1167 /*
1168  * Interface active, can use i2c. If it fails, it can indeed mean, that
1169  * this wasn't our capture interface, so, we wait for the right one
1170  */
1171 static int mt9m111_video_probe(struct i2c_client *client)
1172 {
1173         struct mt9m111 *mt9m111 = to_mt9m111(client);
1174         s32 data;
1175         int ret;
1176
1177         ret = mt9m111_s_power(&mt9m111->subdev, 1);
1178         if (ret < 0)
1179                 return ret;
1180
1181         data = reg_read(CHIP_VERSION);
1182
1183         switch (data) {
1184         case 0x143a: /* MT9M111 or MT9M131 */
1185                 dev_info(&client->dev,
1186                         "Detected a MT9M111/MT9M131 chip ID %x\n", data);
1187                 break;
1188         case 0x148c: /* MT9M112 */
1189                 dev_info(&client->dev, "Detected a MT9M112 chip ID %x\n", data);
1190                 break;
1191         default:
1192                 dev_err(&client->dev,
1193                         "No MT9M111/MT9M112/MT9M131 chip detected register read %x\n",
1194                         data);
1195                 ret = -ENODEV;
1196                 goto done;
1197         }
1198
1199         ret = mt9m111_init(mt9m111);
1200         if (ret)
1201                 goto done;
1202
1203         ret = v4l2_ctrl_handler_setup(&mt9m111->hdl);
1204
1205 done:
1206         mt9m111_s_power(&mt9m111->subdev, 0);
1207         return ret;
1208 }
1209
1210 static int mt9m111_probe_fw(struct i2c_client *client, struct mt9m111 *mt9m111)
1211 {
1212         struct v4l2_fwnode_endpoint bus_cfg = {
1213                 .bus_type = V4L2_MBUS_PARALLEL
1214         };
1215         struct fwnode_handle *np;
1216         int ret;
1217
1218         np = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev), NULL);
1219         if (!np)
1220                 return -EINVAL;
1221
1222         ret = v4l2_fwnode_endpoint_parse(np, &bus_cfg);
1223         if (ret)
1224                 goto out_put_fw;
1225
1226         mt9m111->pclk_sample = !!(bus_cfg.bus.parallel.flags &
1227                                   V4L2_MBUS_PCLK_SAMPLE_RISING);
1228
1229 out_put_fw:
1230         fwnode_handle_put(np);
1231         return ret;
1232 }
1233
1234 static int mt9m111_probe(struct i2c_client *client,
1235                          const struct i2c_device_id *did)
1236 {
1237         struct mt9m111 *mt9m111;
1238         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
1239         int ret;
1240
1241         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1242                 dev_warn(&adapter->dev,
1243                          "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1244                 return -EIO;
1245         }
1246
1247         mt9m111 = devm_kzalloc(&client->dev, sizeof(struct mt9m111), GFP_KERNEL);
1248         if (!mt9m111)
1249                 return -ENOMEM;
1250
1251         ret = mt9m111_probe_fw(client, mt9m111);
1252         if (ret)
1253                 return ret;
1254
1255         mt9m111->clk = v4l2_clk_get(&client->dev, "mclk");
1256         if (IS_ERR(mt9m111->clk))
1257                 return PTR_ERR(mt9m111->clk);
1258
1259         /* Default HIGHPOWER context */
1260         mt9m111->ctx = &context_b;
1261
1262         v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
1263         mt9m111->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1264                                  V4L2_SUBDEV_FL_HAS_EVENTS;
1265
1266         v4l2_ctrl_handler_init(&mt9m111->hdl, 7);
1267         v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
1268                         V4L2_CID_VFLIP, 0, 1, 1, 0);
1269         v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
1270                         V4L2_CID_HFLIP, 0, 1, 1, 0);
1271         v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
1272                         V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
1273         mt9m111->gain = v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
1274                         V4L2_CID_GAIN, 0, 63 * 2 * 2, 1, 32);
1275         v4l2_ctrl_new_std_menu(&mt9m111->hdl,
1276                         &mt9m111_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
1277                         V4L2_EXPOSURE_AUTO);
1278         v4l2_ctrl_new_std_menu_items(&mt9m111->hdl,
1279                         &mt9m111_ctrl_ops, V4L2_CID_TEST_PATTERN,
1280                         ARRAY_SIZE(mt9m111_test_pattern_menu) - 1, 0, 0,
1281                         mt9m111_test_pattern_menu);
1282         v4l2_ctrl_new_std_menu(&mt9m111->hdl, &mt9m111_ctrl_ops,
1283                         V4L2_CID_COLORFX, V4L2_COLORFX_SOLARIZATION,
1284                         ~(BIT(V4L2_COLORFX_NONE) |
1285                                 BIT(V4L2_COLORFX_BW) |
1286                                 BIT(V4L2_COLORFX_SEPIA) |
1287                                 BIT(V4L2_COLORFX_NEGATIVE) |
1288                                 BIT(V4L2_COLORFX_SOLARIZATION)),
1289                         V4L2_COLORFX_NONE);
1290         mt9m111->subdev.ctrl_handler = &mt9m111->hdl;
1291         if (mt9m111->hdl.error) {
1292                 ret = mt9m111->hdl.error;
1293                 goto out_clkput;
1294         }
1295
1296 #ifdef CONFIG_MEDIA_CONTROLLER
1297         mt9m111->pad.flags = MEDIA_PAD_FL_SOURCE;
1298         mt9m111->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1299         ret = media_entity_pads_init(&mt9m111->subdev.entity, 1, &mt9m111->pad);
1300         if (ret < 0)
1301                 goto out_hdlfree;
1302 #endif
1303
1304         mt9m111->current_mode = &mt9m111_mode_data[MT9M111_MODE_SXGA_15FPS];
1305         mt9m111->frame_interval.numerator = 1;
1306         mt9m111->frame_interval.denominator = mt9m111->current_mode->max_fps;
1307
1308         /* Second stage probe - when a capture adapter is there */
1309         mt9m111->rect.left      = MT9M111_MIN_DARK_COLS;
1310         mt9m111->rect.top       = MT9M111_MIN_DARK_ROWS;
1311         mt9m111->rect.width     = MT9M111_MAX_WIDTH;
1312         mt9m111->rect.height    = MT9M111_MAX_HEIGHT;
1313         mt9m111->width          = mt9m111->rect.width;
1314         mt9m111->height         = mt9m111->rect.height;
1315         mt9m111->fmt            = &mt9m111_colour_fmts[0];
1316         mt9m111->lastpage       = -1;
1317         mutex_init(&mt9m111->power_lock);
1318
1319         ret = mt9m111_video_probe(client);
1320         if (ret < 0)
1321                 goto out_entityclean;
1322
1323         mt9m111->subdev.dev = &client->dev;
1324         ret = v4l2_async_register_subdev(&mt9m111->subdev);
1325         if (ret < 0)
1326                 goto out_entityclean;
1327
1328         return 0;
1329
1330 out_entityclean:
1331 #ifdef CONFIG_MEDIA_CONTROLLER
1332         media_entity_cleanup(&mt9m111->subdev.entity);
1333 out_hdlfree:
1334 #endif
1335         v4l2_ctrl_handler_free(&mt9m111->hdl);
1336 out_clkput:
1337         v4l2_clk_put(mt9m111->clk);
1338
1339         return ret;
1340 }
1341
1342 static int mt9m111_remove(struct i2c_client *client)
1343 {
1344         struct mt9m111 *mt9m111 = to_mt9m111(client);
1345
1346         v4l2_async_unregister_subdev(&mt9m111->subdev);
1347         media_entity_cleanup(&mt9m111->subdev.entity);
1348         v4l2_clk_put(mt9m111->clk);
1349         v4l2_ctrl_handler_free(&mt9m111->hdl);
1350
1351         return 0;
1352 }
1353 static const struct of_device_id mt9m111_of_match[] = {
1354         { .compatible = "micron,mt9m111", },
1355         {},
1356 };
1357 MODULE_DEVICE_TABLE(of, mt9m111_of_match);
1358
1359 static const struct i2c_device_id mt9m111_id[] = {
1360         { "mt9m111", 0 },
1361         { }
1362 };
1363 MODULE_DEVICE_TABLE(i2c, mt9m111_id);
1364
1365 static struct i2c_driver mt9m111_i2c_driver = {
1366         .driver = {
1367                 .name = "mt9m111",
1368                 .of_match_table = of_match_ptr(mt9m111_of_match),
1369         },
1370         .probe          = mt9m111_probe,
1371         .remove         = mt9m111_remove,
1372         .id_table       = mt9m111_id,
1373 };
1374
1375 module_i2c_driver(mt9m111_i2c_driver);
1376
1377 MODULE_DESCRIPTION("Micron/Aptina MT9M111/MT9M112/MT9M131 Camera driver");
1378 MODULE_AUTHOR("Robert Jarzmik");
1379 MODULE_LICENSE("GPL");