drivers/media/i2c/mt9m111.c

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