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 #ifdef CONFIG_MEDIA_CONTROLLER
 248         struct media_pad pad;
 249 #endif
 250 };
 251
 252 static const struct mt9m111_mode_info mt9m111_mode_data[MT9M111_NUM_MODES] = {
 253         [MT9M111_MODE_SXGA_8FPS] = {
 254                 .sensor_w = 1280,
 255                 .sensor_h = 1024,
 256                 .max_image_w = 1280,
 257                 .max_image_h = 1024,
 258                 .max_fps = 8,
 259                 .reg_val = MT9M111_RM_LOW_POWER_RD,
 260                 .reg_mask = MT9M111_RM_PWR_MASK | MT9M111_RM_SKIP2_MASK,
 261         },
 262         [MT9M111_MODE_SXGA_15FPS] = {
 263                 .sensor_w = 1280,
 264                 .sensor_h = 1024,
 265                 .max_image_w = 1280,
 266                 .max_image_h = 1024,
 267                 .max_fps = 15,
 268                 .reg_val = MT9M111_RM_FULL_POWER_RD,
 269                 .reg_mask = MT9M111_RM_PWR_MASK | MT9M111_RM_SKIP2_MASK,
 270         },
 271         [MT9M111_MODE_QSXGA_30FPS] = {
 272                 .sensor_w = 1280,
 273                 .sensor_h = 1024,
 274                 .max_image_w = 640,
 275                 .max_image_h = 512,
 276                 .max_fps = 30,
 277                 .reg_val = MT9M111_RM_LOW_POWER_RD | MT9M111_RM_COL_SKIP_2X |
 278                            MT9M111_RM_ROW_SKIP_2X,
 279                 .reg_mask = MT9M111_RM_PWR_MASK | MT9M111_RM_SKIP2_MASK,
 280         },
 281 };
 282
 283 /* Find a data format by a pixel code */
 284 static const struct mt9m111_datafmt *mt9m111_find_datafmt(struct mt9m111 *mt9m111,
 285                                                 u32 code)
 286 {
 287         int i;
 288         for (i = 0; i < ARRAY_SIZE(mt9m111_colour_fmts); i++)
 289                 if (mt9m111_colour_fmts[i].code == code)
 290                         return mt9m111_colour_fmts + i;
 291
 292         return mt9m111->fmt;
 293 }
 294
 295 static struct mt9m111 *to_mt9m111(const struct i2c_client *client)
 296 {
 297         return container_of(i2c_get_clientdata(client), struct mt9m111, subdev);
 298 }
 299
 300 static int reg_page_map_set(struct i2c_client *client, const u16 reg)
 301 {
 302         int ret;
 303         u16 page;
 304         struct mt9m111 *mt9m111 = to_mt9m111(client);
 305
 306         page = (reg >> 8);
 307         if (page == mt9m111->lastpage)
 308                 return 0;
 309         if (page > 2)
 310                 return -EINVAL;
 311
 312         ret = i2c_smbus_write_word_swapped(client, MT9M111_PAGE_MAP, page);
 313         if (!ret)
 314                 mt9m111->lastpage = page;
 315         return ret;
 316 }
 317
 318 static int mt9m111_reg_read(struct i2c_client *client, const u16 reg)
 319 {
 320         int ret;
 321
 322         ret = reg_page_map_set(client, reg);
 323         if (!ret)
 324                 ret = i2c_smbus_read_word_swapped(client, reg & 0xff);
 325
 326         dev_dbg(&client->dev, "read  reg.%03x -> %04x\n", reg, ret);
 327         return ret;
 328 }
 329
 330 static int mt9m111_reg_write(struct i2c_client *client, const u16 reg,
 331                              const u16 data)
 332 {
 333         int ret;
 334
 335         ret = reg_page_map_set(client, reg);
 336         if (!ret)
 337                 ret = i2c_smbus_write_word_swapped(client, reg & 0xff, data);
 338         dev_dbg(&client->dev, "write reg.%03x = %04x -> %d\n", reg, data, ret);
 339         return ret;
 340 }
 341
 342 static int mt9m111_reg_set(struct i2c_client *client, const u16 reg,
 343                            const u16 data)
 344 {
 345         int ret;
 346
 347         ret = mt9m111_reg_read(client, reg);
 348         if (ret >= 0)
 349                 ret = mt9m111_reg_write(client, reg, ret | data);
 350         return ret;
 351 }
 352
 353 static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg,
 354                              const u16 data)
 355 {
 356         int ret;
 357
 358         ret = mt9m111_reg_read(client, reg);
 359         if (ret >= 0)
 360                 ret = mt9m111_reg_write(client, reg, ret & ~data);
 361         return ret;
 362 }
 363
 364 static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg,
 365                             const u16 data, const u16 mask)
 366 {
 367         int ret;
 368
 369         ret = mt9m111_reg_read(client, reg);
 370         if (ret >= 0)
 371                 ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data);
 372         return ret;
 373 }
 374
 375 static int mt9m111_set_context(struct mt9m111 *mt9m111,
 376                                struct mt9m111_context *ctx)
 377 {
 378         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 379         return reg_write(CONTEXT_CONTROL, ctx->control);
 380 }
 381
 382 static int mt9m111_setup_rect_ctx(struct mt9m111 *mt9m111,
 383                         struct mt9m111_context *ctx, struct v4l2_rect *rect,
 384                         unsigned int width, unsigned int height)
 385 {
 386         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 387         int ret = mt9m111_reg_write(client, ctx->reducer_xzoom, rect->width);
 388         if (!ret)
 389                 ret = mt9m111_reg_write(client, ctx->reducer_yzoom, rect->height);
 390         if (!ret)
 391                 ret = mt9m111_reg_write(client, ctx->reducer_xsize, width);
 392         if (!ret)
 393                 ret = mt9m111_reg_write(client, ctx->reducer_ysize, height);
 394         return ret;
 395 }
 396
 397 static int mt9m111_setup_geometry(struct mt9m111 *mt9m111, struct v4l2_rect *rect,
 398                         int width, int height, u32 code)
 399 {
 400         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 401         int ret;
 402
 403         ret = reg_write(COLUMN_START, rect->left);
 404         if (!ret)
 405                 ret = reg_write(ROW_START, rect->top);
 406
 407         if (!ret)
 408                 ret = reg_write(WINDOW_WIDTH, rect->width);
 409         if (!ret)
 410                 ret = reg_write(WINDOW_HEIGHT, rect->height);
 411
 412         if (code != MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
 413                 /* IFP in use, down-scaling possible */
 414                 if (!ret)
 415                         ret = mt9m111_setup_rect_ctx(mt9m111, &context_b,
 416                                                      rect, width, height);
 417                 if (!ret)
 418                         ret = mt9m111_setup_rect_ctx(mt9m111, &context_a,
 419                                                      rect, width, height);
 420         }
 421
 422         dev_dbg(&client->dev, "%s(%x): %ux%u@%u:%u -> %ux%u = %d\n",
 423                 __func__, code, rect->width, rect->height, rect->left, rect->top,
 424                 width, height, ret);
 425
 426         return ret;
 427 }
 428
 429 static int mt9m111_enable(struct mt9m111 *mt9m111)
 430 {
 431         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 432         return reg_write(RESET, MT9M111_RESET_CHIP_ENABLE);
 433 }
 434
 435 static int mt9m111_reset(struct mt9m111 *mt9m111)
 436 {
 437         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 438         int ret;
 439
 440         ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
 441         if (!ret)
 442                 ret = reg_set(RESET, MT9M111_RESET_RESET_SOC);
 443         if (!ret)
 444                 ret = reg_clear(RESET, MT9M111_RESET_RESET_MODE
 445                                 | MT9M111_RESET_RESET_SOC);
 446
 447         return ret;
 448 }
 449
 450 static int mt9m111_set_selection(struct v4l2_subdev *sd,
 451                                  struct v4l2_subdev_state *sd_state,
 452                                  struct v4l2_subdev_selection *sel)
 453 {
 454         struct i2c_client *client = v4l2_get_subdevdata(sd);
 455         struct mt9m111 *mt9m111 = to_mt9m111(client);
 456         struct v4l2_rect rect = sel->r;
 457         int width, height;
 458         int ret, align = 0;
 459
 460         if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
 461             sel->target != V4L2_SEL_TGT_CROP)
 462                 return -EINVAL;
 463
 464         if (mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
 465             mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
 466                 /* Bayer format - even size lengths */
 467                 align = 1;
 468                 /* Let the user play with the starting pixel */
 469         }
 470
 471         /* FIXME: the datasheet doesn't specify minimum sizes */
 472         v4l_bound_align_image(&rect.width, 2, MT9M111_MAX_WIDTH, align,
 473                               &rect.height, 2, MT9M111_MAX_HEIGHT, align, 0);
 474         rect.left = clamp(rect.left, MT9M111_MIN_DARK_COLS,
 475                           MT9M111_MIN_DARK_COLS + MT9M111_MAX_WIDTH -
 476                           (__s32)rect.width);
 477         rect.top = clamp(rect.top, MT9M111_MIN_DARK_ROWS,
 478                          MT9M111_MIN_DARK_ROWS + MT9M111_MAX_HEIGHT -
 479                          (__s32)rect.height);
 480
 481         width = min(mt9m111->width, rect.width);
 482         height = min(mt9m111->height, rect.height);
 483
 484         ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
 485         if (!ret) {
 486                 mt9m111->rect = rect;
 487                 mt9m111->width = width;
 488                 mt9m111->height = height;
 489         }
 490
 491         return ret;
 492 }
 493
 494 static int mt9m111_get_selection(struct v4l2_subdev *sd,
 495                                  struct v4l2_subdev_state *sd_state,
 496                                  struct v4l2_subdev_selection *sel)
 497 {
 498         struct i2c_client *client = v4l2_get_subdevdata(sd);
 499         struct mt9m111 *mt9m111 = to_mt9m111(client);
 500
 501         if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
 502                 return -EINVAL;
 503
 504         switch (sel->target) {
 505         case V4L2_SEL_TGT_CROP_BOUNDS:
 506                 sel->r.left = MT9M111_MIN_DARK_COLS;
 507                 sel->r.top = MT9M111_MIN_DARK_ROWS;
 508                 sel->r.width = MT9M111_MAX_WIDTH;
 509                 sel->r.height = MT9M111_MAX_HEIGHT;
 510                 return 0;
 511         case V4L2_SEL_TGT_CROP:
 512                 sel->r = mt9m111->rect;
 513                 return 0;
 514         default:
 515                 return -EINVAL;
 516         }
 517 }
 518
 519 static int mt9m111_get_fmt(struct v4l2_subdev *sd,
 520                 struct v4l2_subdev_state *sd_state,
 521                 struct v4l2_subdev_format *format)
 522 {
 523         struct v4l2_mbus_framefmt *mf = &format->format;
 524         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 525
 526         if (format->pad)
 527                 return -EINVAL;
 528
 529         if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
 530 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
 531                 mf = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
 532                 format->format = *mf;
 533                 return 0;
 534 #else
 535                 return -EINVAL;
 536 #endif
 537         }
 538
 539         mf->width       = mt9m111->width;
 540         mf->height      = mt9m111->height;
 541         mf->code        = mt9m111->fmt->code;
 542         mf->colorspace  = mt9m111->fmt->colorspace;
 543         mf->field       = V4L2_FIELD_NONE;
 544         mf->ycbcr_enc   = V4L2_YCBCR_ENC_DEFAULT;
 545         mf->quantization        = V4L2_QUANTIZATION_DEFAULT;
 546         mf->xfer_func   = V4L2_XFER_FUNC_DEFAULT;
 547
 548         return 0;
 549 }
 550
 551 static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
 552                               u32 code)
 553 {
 554         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 555         u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
 556                 MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB |
 557                 MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
 558                 MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 |
 559                 MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
 560                 MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 561         int ret;
 562
 563         switch (code) {
 564         case MEDIA_BUS_FMT_SBGGR8_1X8:
 565                 data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
 566                         MT9M111_OUTFMT_RGB;
 567                 break;
 568         case MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE:
 569                 data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
 570                 break;
 571         case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
 572                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
 573                         MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
 574                 break;
 575         case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
 576                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
 577                 break;
 578         case MEDIA_BUS_FMT_RGB565_2X8_LE:
 579                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
 580                         MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
 581                 break;
 582         case MEDIA_BUS_FMT_RGB565_2X8_BE:
 583                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
 584                 break;
 585         case MEDIA_BUS_FMT_BGR565_2X8_BE:
 586                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
 587                         MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 588                 break;
 589         case MEDIA_BUS_FMT_BGR565_2X8_LE:
 590                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
 591                         MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
 592                         MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 593                 break;
 594         case MEDIA_BUS_FMT_UYVY8_2X8:
 595                 data_outfmt2 = 0;
 596                 break;
 597         case MEDIA_BUS_FMT_VYUY8_2X8:
 598                 data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 599                 break;
 600         case MEDIA_BUS_FMT_YUYV8_2X8:
 601                 data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
 602                 break;
 603         case MEDIA_BUS_FMT_YVYU8_2X8:
 604                 data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
 605                         MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 606                 break;
 607         default:
 608                 dev_err(&client->dev, "Pixel format not handled: %x\n", code);
 609                 return -EINVAL;
 610         }
 611
 612         /* receiver samples on falling edge, chip-hw default is rising */
 613         if (mt9m111->pclk_sample == 0)
 614                 mask_outfmt2 |= MT9M111_OUTFMT_INV_PIX_CLOCK;
 615
 616         ret = mt9m111_reg_mask(client, context_a.output_fmt_ctrl2,
 617                                data_outfmt2, mask_outfmt2);
 618         if (!ret)
 619                 ret = mt9m111_reg_mask(client, context_b.output_fmt_ctrl2,
 620                                        data_outfmt2, mask_outfmt2);
 621
 622         return ret;
 623 }
 624
 625 static int mt9m111_set_fmt(struct v4l2_subdev *sd,
 626                 struct v4l2_subdev_state *sd_state,
 627                 struct v4l2_subdev_format *format)
 628 {
 629         struct v4l2_mbus_framefmt *mf = &format->format;
 630         struct i2c_client *client = v4l2_get_subdevdata(sd);
 631         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 632         const struct mt9m111_datafmt *fmt;
 633         struct v4l2_rect *rect = &mt9m111->rect;
 634         bool bayer;
 635         int ret;
 636
 637         if (mt9m111->is_streaming)
 638                 return -EBUSY;
 639
 640         if (format->pad)
 641                 return -EINVAL;
 642
 643         fmt = mt9m111_find_datafmt(mt9m111, mf->code);
 644
 645         bayer = fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
 646                 fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE;
 647
 648         /*
 649          * With Bayer format enforce even side lengths, but let the user play
 650          * with the starting pixel
 651          */
 652         if (bayer) {
 653                 rect->width = ALIGN(rect->width, 2);
 654                 rect->height = ALIGN(rect->height, 2);
 655         }
 656
 657         if (fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
 658                 /* IFP bypass mode, no scaling */
 659                 mf->width = rect->width;
 660                 mf->height = rect->height;
 661         } else {
 662                 /* No upscaling */
 663                 if (mf->width > rect->width)
 664                         mf->width = rect->width;
 665                 if (mf->height > rect->height)
 666                         mf->height = rect->height;
 667         }
 668
 669         dev_dbg(&client->dev, "%s(): %ux%u, code=%x\n", __func__,
 670                 mf->width, mf->height, fmt->code);
 671
 672         mf->code = fmt->code;
 673         mf->colorspace = fmt->colorspace;
 674         mf->field       = V4L2_FIELD_NONE;
 675         mf->ycbcr_enc   = V4L2_YCBCR_ENC_DEFAULT;
 676         mf->quantization        = V4L2_QUANTIZATION_DEFAULT;
 677         mf->xfer_func   = V4L2_XFER_FUNC_DEFAULT;
 678
 679         if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
 680                 sd_state->pads->try_fmt = *mf;
 681                 return 0;
 682         }
 683
 684         ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
 685         if (!ret)
 686                 ret = mt9m111_set_pixfmt(mt9m111, mf->code);
 687         if (!ret) {
 688                 mt9m111->width  = mf->width;
 689                 mt9m111->height = mf->height;
 690                 mt9m111->fmt    = fmt;
 691         }
 692
 693         return ret;
 694 }
 695
 696 static const struct mt9m111_mode_info *
 697 mt9m111_find_mode(struct mt9m111 *mt9m111, unsigned int req_fps,
 698                   unsigned int width, unsigned int height)
 699 {
 700         const struct mt9m111_mode_info *mode;
 701         struct v4l2_rect *sensor_rect = &mt9m111->rect;
 702         unsigned int gap, gap_best = (unsigned int) -1;
 703         int i, best_gap_idx = MT9M111_MODE_SXGA_15FPS;
 704         bool skip_30fps = false;
 705
 706         /*
 707          * The fps selection is based on the row, column skipping mechanism.
 708          * So ensure that the sensor window is set to default else the fps
 709          * aren't calculated correctly within the sensor hw.
 710          */
 711         if (sensor_rect->width != MT9M111_MAX_WIDTH ||
 712             sensor_rect->height != MT9M111_MAX_HEIGHT) {
 713                 dev_info(mt9m111->subdev.dev,
 714                          "Framerate selection is not supported for cropped "
 715                          "images\n");
 716                 return NULL;
 717         }
 718
 719         /* 30fps only supported for images not exceeding 640x512 */
 720         if (width > MT9M111_MAX_WIDTH / 2 || height > MT9M111_MAX_HEIGHT / 2) {
 721                 dev_dbg(mt9m111->subdev.dev,
 722                         "Framerates > 15fps are supported only for images "
 723                         "not exceeding 640x512\n");
 724                 skip_30fps = true;
 725         }
 726
 727         /* find best matched fps */
 728         for (i = 0; i < MT9M111_NUM_MODES; i++) {
 729                 unsigned int fps = mt9m111_mode_data[i].max_fps;
 730
 731                 if (fps == 30 && skip_30fps)
 732                         continue;
 733
 734                 gap = abs(fps - req_fps);
 735                 if (gap < gap_best) {
 736                         best_gap_idx = i;
 737                         gap_best = gap;
 738                 }
 739         }
 740
 741         /*
 742          * Use context a/b default timing values instead of calculate blanking
 743          * timing values.
 744          */
 745         mode = &mt9m111_mode_data[best_gap_idx];
 746         mt9m111->ctx = (best_gap_idx == MT9M111_MODE_QSXGA_30FPS) ? &context_a :
 747                                                                     &context_b;
 748         return mode;
 749 }
 750
 751 #ifdef CONFIG_VIDEO_ADV_DEBUG
 752 static int mt9m111_g_register(struct v4l2_subdev *sd,
 753                               struct v4l2_dbg_register *reg)
 754 {
 755         struct i2c_client *client = v4l2_get_subdevdata(sd);
 756         int val;
 757
 758         if (reg->reg > 0x2ff)
 759                 return -EINVAL;
 760
 761         val = mt9m111_reg_read(client, reg->reg);
 762         reg->size = 2;
 763         reg->val = (u64)val;
 764
 765         if (reg->val > 0xffff)
 766                 return -EIO;
 767
 768         return 0;
 769 }
 770
 771 static int mt9m111_s_register(struct v4l2_subdev *sd,
 772                               const struct v4l2_dbg_register *reg)
 773 {
 774         struct i2c_client *client = v4l2_get_subdevdata(sd);
 775
 776         if (reg->reg > 0x2ff)
 777                 return -EINVAL;
 778
 779         if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
 780                 return -EIO;
 781
 782         return 0;
 783 }
 784 #endif
 785
 786 static int mt9m111_set_flip(struct mt9m111 *mt9m111, int flip, int mask)
 787 {
 788         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 789         int ret;
 790
 791         if (flip)
 792                 ret = mt9m111_reg_set(client, mt9m111->ctx->read_mode, mask);
 793         else
 794                 ret = mt9m111_reg_clear(client, mt9m111->ctx->read_mode, mask);
 795
 796         return ret;
 797 }
 798
 799 static int mt9m111_get_global_gain(struct mt9m111 *mt9m111)
 800 {
 801         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 802         int data;
 803
 804         data = reg_read(GLOBAL_GAIN);
 805         if (data >= 0)
 806                 return (data & 0x2f) * (1 << ((data >> 10) & 1)) *
 807                         (1 << ((data >> 9) & 1));
 808         return data;
 809 }
 810
 811 static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
 812 {
 813         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 814         u16 val;
 815
 816         if (gain > 63 * 2 * 2)
 817                 return -EINVAL;
 818
 819         if ((gain >= 64 * 2) && (gain < 63 * 2 * 2))
 820                 val = (1 << 10) | (1 << 9) | (gain / 4);
 821         else if ((gain >= 64) && (gain < 64 * 2))
 822                 val = (1 << 9) | (gain / 2);
 823         else
 824                 val = gain;
 825
 826         return reg_write(GLOBAL_GAIN, val);
 827 }
 828
 829 static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int val)
 830 {
 831         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 832
 833         if (val == V4L2_EXPOSURE_AUTO)
 834                 return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
 835         return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
 836 }
 837
 838 static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on)
 839 {
 840         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 841
 842         if (on)
 843                 return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
 844         return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
 845 }
 846
 847 static const char * const mt9m111_test_pattern_menu[] = {
 848         "Disabled",
 849         "Vertical monochrome gradient",
 850         "Flat color type 1",
 851         "Flat color type 2",
 852         "Flat color type 3",
 853         "Flat color type 4",
 854         "Flat color type 5",
 855         "Color bar",
 856 };
 857
 858 static int mt9m111_set_test_pattern(struct mt9m111 *mt9m111, int val)
 859 {
 860         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 861
 862         return mt9m111_reg_mask(client, MT9M111_TPG_CTRL, val,
 863                                 MT9M111_TPG_SEL_MASK);
 864 }
 865
 866 static int mt9m111_set_colorfx(struct mt9m111 *mt9m111, int val)
 867 {
 868         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 869         static const struct v4l2_control colorfx[] = {
 870                 { V4L2_COLORFX_NONE,            0 },
 871                 { V4L2_COLORFX_BW,              1 },
 872                 { V4L2_COLORFX_SEPIA,           2 },
 873                 { V4L2_COLORFX_NEGATIVE,        3 },
 874                 { V4L2_COLORFX_SOLARIZATION,    4 },
 875         };
 876         int i;
 877
 878         for (i = 0; i < ARRAY_SIZE(colorfx); i++) {
 879                 if (colorfx[i].id == val) {
 880                         return mt9m111_reg_mask(client, MT9M111_EFFECTS_MODE,
 881                                                 colorfx[i].value,
 882                                                 MT9M111_EFFECTS_MODE_MASK);
 883                 }
 884         }
 885
 886         return -EINVAL;
 887 }
 888
 889 static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
 890 {
 891         struct mt9m111 *mt9m111 = container_of(ctrl->handler,
 892                                                struct mt9m111, hdl);
 893
 894         switch (ctrl->id) {
 895         case V4L2_CID_VFLIP:
 896                 return mt9m111_set_flip(mt9m111, ctrl->val,
 897                                         MT9M111_RMB_MIRROR_ROWS);
 898         case V4L2_CID_HFLIP:
 899                 return mt9m111_set_flip(mt9m111, ctrl->val,
 900                                         MT9M111_RMB_MIRROR_COLS);
 901         case V4L2_CID_GAIN:
 902                 return mt9m111_set_global_gain(mt9m111, ctrl->val);
 903         case V4L2_CID_EXPOSURE_AUTO:
 904                 return mt9m111_set_autoexposure(mt9m111, ctrl->val);
 905         case V4L2_CID_AUTO_WHITE_BALANCE:
 906                 return mt9m111_set_autowhitebalance(mt9m111, ctrl->val);
 907         case V4L2_CID_TEST_PATTERN:
 908                 return mt9m111_set_test_pattern(mt9m111, ctrl->val);
 909         case V4L2_CID_COLORFX:
 910                 return mt9m111_set_colorfx(mt9m111, ctrl->val);
 911         }
 912
 913         return -EINVAL;
 914 }
 915
 916 static int mt9m111_suspend(struct mt9m111 *mt9m111)
 917 {
 918         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 919         int ret;
 920
 921         v4l2_ctrl_s_ctrl(mt9m111->gain, mt9m111_get_global_gain(mt9m111));
 922
 923         ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
 924         if (!ret)
 925                 ret = reg_set(RESET, MT9M111_RESET_RESET_SOC |
 926                               MT9M111_RESET_OUTPUT_DISABLE |
 927                               MT9M111_RESET_ANALOG_STANDBY);
 928         if (!ret)
 929                 ret = reg_clear(RESET, MT9M111_RESET_CHIP_ENABLE);
 930
 931         return ret;
 932 }
 933
 934 static void mt9m111_restore_state(struct mt9m111 *mt9m111)
 935 {
 936         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 937
 938         mt9m111_set_context(mt9m111, mt9m111->ctx);
 939         mt9m111_set_pixfmt(mt9m111, mt9m111->fmt->code);
 940         mt9m111_setup_geometry(mt9m111, &mt9m111->rect,
 941                         mt9m111->width, mt9m111->height, mt9m111->fmt->code);
 942         v4l2_ctrl_handler_setup(&mt9m111->hdl);
 943         mt9m111_reg_mask(client, mt9m111->ctx->read_mode,
 944                          mt9m111->current_mode->reg_val,
 945                          mt9m111->current_mode->reg_mask);
 946 }
 947
 948 static int mt9m111_resume(struct mt9m111 *mt9m111)
 949 {
 950         int ret = mt9m111_enable(mt9m111);
 951         if (!ret)
 952                 ret = mt9m111_reset(mt9m111);
 953         if (!ret)
 954                 mt9m111_restore_state(mt9m111);
 955
 956         return ret;
 957 }
 958
 959 static int mt9m111_init(struct mt9m111 *mt9m111)
 960 {
 961         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 962         int ret;
 963
 964         ret = mt9m111_enable(mt9m111);
 965         if (!ret)
 966                 ret = mt9m111_reset(mt9m111);
 967         if (!ret)
 968                 ret = mt9m111_set_context(mt9m111, mt9m111->ctx);
 969         if (ret)
 970                 dev_err(&client->dev, "mt9m111 init failed: %d\n", ret);
 971         return ret;
 972 }
 973
 974 static int mt9m111_power_on(struct mt9m111 *mt9m111)
 975 {
 976         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 977         int ret;
 978
 979         ret = clk_prepare_enable(mt9m111->clk);
 980         if (ret < 0)
 981                 return ret;
 982
 983         ret = regulator_enable(mt9m111->regulator);
 984         if (ret < 0)
 985                 goto out_clk_disable;
 986
 987         ret = mt9m111_resume(mt9m111);
 988         if (ret < 0)
 989                 goto out_regulator_disable;
 990
 991         return 0;
 992
 993 out_regulator_disable:
 994         regulator_disable(mt9m111->regulator);
 995
 996 out_clk_disable:
 997         clk_disable_unprepare(mt9m111->clk);
 998
 999         dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
1000
1001         return ret;
1002 }
1003
1004 static void mt9m111_power_off(struct mt9m111 *mt9m111)
1005 {
1006         mt9m111_suspend(mt9m111);
1007         regulator_disable(mt9m111->regulator);
1008         clk_disable_unprepare(mt9m111->clk);
1009 }
1010
1011 static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
1012 {
1013         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1014         int ret = 0;
1015
1016         mutex_lock(&mt9m111->power_lock);
1017
1018         /*
1019          * If the power count is modified from 0 to != 0 or from != 0 to 0,
1020          * update the power state.
1021          */
1022         if (mt9m111->power_count == !on) {
1023                 if (on)
1024                         ret = mt9m111_power_on(mt9m111);
1025                 else
1026                         mt9m111_power_off(mt9m111);
1027         }
1028
1029         if (!ret) {
1030                 /* Update the power count. */
1031                 mt9m111->power_count += on ? 1 : -1;
1032                 WARN_ON(mt9m111->power_count < 0);
1033         }
1034
1035         mutex_unlock(&mt9m111->power_lock);
1036         return ret;
1037 }
1038
1039 static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
1040         .s_ctrl = mt9m111_s_ctrl,
1041 };
1042
1043 static const struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
1044         .s_power        = mt9m111_s_power,
1045         .log_status = v4l2_ctrl_subdev_log_status,
1046         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1047         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1048 #ifdef CONFIG_VIDEO_ADV_DEBUG
1049         .g_register     = mt9m111_g_register,
1050         .s_register     = mt9m111_s_register,
1051 #endif
1052 };
1053
1054 static int mt9m111_g_frame_interval(struct v4l2_subdev *sd,
1055                                    struct v4l2_subdev_frame_interval *fi)
1056 {
1057         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1058
1059         fi->interval = mt9m111->frame_interval;
1060
1061         return 0;
1062 }
1063
1064 static int mt9m111_s_frame_interval(struct v4l2_subdev *sd,
1065                                    struct v4l2_subdev_frame_interval *fi)
1066 {
1067         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1068         const struct mt9m111_mode_info *mode;
1069         struct v4l2_fract *fract = &fi->interval;
1070         int fps;
1071
1072         if (mt9m111->is_streaming)
1073                 return -EBUSY;
1074
1075         if (fi->pad != 0)
1076                 return -EINVAL;
1077
1078         if (fract->numerator == 0) {
1079                 fract->denominator = 30;
1080                 fract->numerator = 1;
1081         }
1082
1083         fps = DIV_ROUND_CLOSEST(fract->denominator, fract->numerator);
1084
1085         /* Find best fitting mode. Do not update the mode if no one was found. */
1086         mode = mt9m111_find_mode(mt9m111, fps, mt9m111->width, mt9m111->height);
1087         if (!mode)
1088                 return 0;
1089
1090         if (mode->max_fps != fps) {
1091                 fract->denominator = mode->max_fps;
1092                 fract->numerator = 1;
1093         }
1094
1095         mt9m111->current_mode = mode;
1096         mt9m111->frame_interval = fi->interval;
1097
1098         return 0;
1099 }
1100
1101 static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
1102                 struct v4l2_subdev_state *sd_state,
1103                 struct v4l2_subdev_mbus_code_enum *code)
1104 {
1105         if (code->pad || code->index >= ARRAY_SIZE(mt9m111_colour_fmts))
1106                 return -EINVAL;
1107
1108         code->code = mt9m111_colour_fmts[code->index].code;
1109         return 0;
1110 }
1111
1112 static int mt9m111_s_stream(struct v4l2_subdev *sd, int enable)
1113 {
1114         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1115
1116         mt9m111->is_streaming = !!enable;
1117         return 0;
1118 }
1119
1120 static int mt9m111_init_cfg(struct v4l2_subdev *sd,
1121                             struct v4l2_subdev_state *sd_state)
1122 {
1123 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
1124         struct v4l2_mbus_framefmt *format =
1125                 v4l2_subdev_get_try_format(sd, sd_state, 0);
1126
1127         format->width   = MT9M111_MAX_WIDTH;
1128         format->height  = MT9M111_MAX_HEIGHT;
1129         format->code    = mt9m111_colour_fmts[0].code;
1130         format->colorspace      = mt9m111_colour_fmts[0].colorspace;
1131         format->field   = V4L2_FIELD_NONE;
1132         format->ycbcr_enc       = V4L2_YCBCR_ENC_DEFAULT;
1133         format->quantization    = V4L2_QUANTIZATION_DEFAULT;
1134         format->xfer_func       = V4L2_XFER_FUNC_DEFAULT;
1135 #endif
1136         return 0;
1137 }
1138
1139 static int mt9m111_get_mbus_config(struct v4l2_subdev *sd,
1140                                    unsigned int pad,
1141                                    struct v4l2_mbus_config *cfg)
1142 {
1143         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1144
1145         cfg->type = V4L2_MBUS_PARALLEL;
1146
1147         cfg->bus.parallel.flags = V4L2_MBUS_MASTER |
1148                                   V4L2_MBUS_HSYNC_ACTIVE_HIGH |
1149                                   V4L2_MBUS_VSYNC_ACTIVE_HIGH |
1150                                   V4L2_MBUS_DATA_ACTIVE_HIGH;
1151
1152         cfg->bus.parallel.flags |= mt9m111->pclk_sample ?
1153                                    V4L2_MBUS_PCLK_SAMPLE_RISING :
1154                                    V4L2_MBUS_PCLK_SAMPLE_FALLING;
1155
1156         return 0;
1157 }
1158
1159 static const struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
1160         .s_stream       = mt9m111_s_stream,
1161         .g_frame_interval = mt9m111_g_frame_interval,
1162         .s_frame_interval = mt9m111_s_frame_interval,
1163 };
1164
1165 static const struct v4l2_subdev_pad_ops mt9m111_subdev_pad_ops = {
1166         .init_cfg       = mt9m111_init_cfg,
1167         .enum_mbus_code = mt9m111_enum_mbus_code,
1168         .get_selection  = mt9m111_get_selection,
1169         .set_selection  = mt9m111_set_selection,
1170         .get_fmt        = mt9m111_get_fmt,
1171         .set_fmt        = mt9m111_set_fmt,
1172         .get_mbus_config = mt9m111_get_mbus_config,
1173 };
1174
1175 static const struct v4l2_subdev_ops mt9m111_subdev_ops = {
1176         .core   = &mt9m111_subdev_core_ops,
1177         .video  = &mt9m111_subdev_video_ops,
1178         .pad    = &mt9m111_subdev_pad_ops,
1179 };
1180
1181 /*
1182  * Interface active, can use i2c. If it fails, it can indeed mean, that
1183  * this wasn't our capture interface, so, we wait for the right one
1184  */
1185 static int mt9m111_video_probe(struct i2c_client *client)
1186 {
1187         struct mt9m111 *mt9m111 = to_mt9m111(client);
1188         s32 data;
1189         int ret;
1190
1191         ret = mt9m111_s_power(&mt9m111->subdev, 1);
1192         if (ret < 0)
1193                 return ret;
1194
1195         data = reg_read(CHIP_VERSION);
1196
1197         switch (data) {
1198         case 0x143a: /* MT9M111 or MT9M131 */
1199                 dev_info(&client->dev,
1200                         "Detected a MT9M111/MT9M131 chip ID %x\n", data);
1201                 break;
1202         case 0x148c: /* MT9M112 */
1203                 dev_info(&client->dev, "Detected a MT9M112 chip ID %x\n", data);
1204                 break;
1205         default:
1206                 dev_err(&client->dev,
1207                         "No MT9M111/MT9M112/MT9M131 chip detected register read %x\n",
1208                         data);
1209                 ret = -ENODEV;
1210                 goto done;
1211         }
1212
1213         ret = mt9m111_init(mt9m111);
1214         if (ret)
1215                 goto done;
1216
1217         ret = v4l2_ctrl_handler_setup(&mt9m111->hdl);
1218
1219 done:
1220         mt9m111_s_power(&mt9m111->subdev, 0);
1221         return ret;
1222 }
1223
1224 static int mt9m111_probe_fw(struct i2c_client *client, struct mt9m111 *mt9m111)
1225 {
1226         struct v4l2_fwnode_endpoint bus_cfg = {
1227                 .bus_type = V4L2_MBUS_PARALLEL
1228         };
1229         struct fwnode_handle *np;
1230         int ret;
1231
1232         np = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev), NULL);
1233         if (!np)
1234                 return -EINVAL;
1235
1236         ret = v4l2_fwnode_endpoint_parse(np, &bus_cfg);
1237         if (ret)
1238                 goto out_put_fw;
1239
1240         mt9m111->pclk_sample = !!(bus_cfg.bus.parallel.flags &
1241                                   V4L2_MBUS_PCLK_SAMPLE_RISING);
1242
1243 out_put_fw:
1244         fwnode_handle_put(np);
1245         return ret;
1246 }
1247
1248 static int mt9m111_probe(struct i2c_client *client)
1249 {
1250         struct mt9m111 *mt9m111;
1251         struct i2c_adapter *adapter = client->adapter;
1252         int ret;
1253
1254         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1255                 dev_warn(&adapter->dev,
1256                          "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1257                 return -EIO;
1258         }
1259
1260         mt9m111 = devm_kzalloc(&client->dev, sizeof(struct mt9m111), GFP_KERNEL);
1261         if (!mt9m111)
1262                 return -ENOMEM;
1263
1264         if (dev_fwnode(&client->dev)) {
1265                 ret = mt9m111_probe_fw(client, mt9m111);
1266                 if (ret)
1267                         return ret;
1268         }
1269
1270         mt9m111->clk = devm_clk_get(&client->dev, "mclk");
1271         if (IS_ERR(mt9m111->clk))
1272                 return PTR_ERR(mt9m111->clk);
1273
1274         mt9m111->regulator = devm_regulator_get(&client->dev, "vdd");
1275         if (IS_ERR(mt9m111->regulator)) {
1276                 dev_err(&client->dev, "regulator not found: %ld\n",
1277                         PTR_ERR(mt9m111->regulator));
1278                 return PTR_ERR(mt9m111->regulator);
1279         }
1280
1281         /* Default HIGHPOWER context */
1282         mt9m111->ctx = &context_b;
1283
1284         v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
1285         mt9m111->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1286                                  V4L2_SUBDEV_FL_HAS_EVENTS;
1287
1288         v4l2_ctrl_handler_init(&mt9m111->hdl, 7);
1289         v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
1290                         V4L2_CID_VFLIP, 0, 1, 1, 0);
1291         v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
1292                         V4L2_CID_HFLIP, 0, 1, 1, 0);
1293         v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
1294                         V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
1295         mt9m111->gain = v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
1296                         V4L2_CID_GAIN, 0, 63 * 2 * 2, 1, 32);
1297         v4l2_ctrl_new_std_menu(&mt9m111->hdl,
1298                         &mt9m111_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
1299                         V4L2_EXPOSURE_AUTO);
1300         v4l2_ctrl_new_std_menu_items(&mt9m111->hdl,
1301                         &mt9m111_ctrl_ops, V4L2_CID_TEST_PATTERN,
1302                         ARRAY_SIZE(mt9m111_test_pattern_menu) - 1, 0, 0,
1303                         mt9m111_test_pattern_menu);
1304         v4l2_ctrl_new_std_menu(&mt9m111->hdl, &mt9m111_ctrl_ops,
1305                         V4L2_CID_COLORFX, V4L2_COLORFX_SOLARIZATION,
1306                         ~(BIT(V4L2_COLORFX_NONE) |
1307                                 BIT(V4L2_COLORFX_BW) |
1308                                 BIT(V4L2_COLORFX_SEPIA) |
1309                                 BIT(V4L2_COLORFX_NEGATIVE) |
1310                                 BIT(V4L2_COLORFX_SOLARIZATION)),
1311                         V4L2_COLORFX_NONE);
1312         mt9m111->subdev.ctrl_handler = &mt9m111->hdl;
1313         if (mt9m111->hdl.error) {
1314                 ret = mt9m111->hdl.error;
1315                 return ret;
1316         }
1317
1318 #ifdef CONFIG_MEDIA_CONTROLLER
1319         mt9m111->pad.flags = MEDIA_PAD_FL_SOURCE;
1320         mt9m111->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1321         ret = media_entity_pads_init(&mt9m111->subdev.entity, 1, &mt9m111->pad);
1322         if (ret < 0)
1323                 goto out_hdlfree;
1324 #endif
1325
1326         mt9m111->current_mode = &mt9m111_mode_data[MT9M111_MODE_SXGA_15FPS];
1327         mt9m111->frame_interval.numerator = 1;
1328         mt9m111->frame_interval.denominator = mt9m111->current_mode->max_fps;
1329
1330         /* Second stage probe - when a capture adapter is there */
1331         mt9m111->rect.left      = MT9M111_MIN_DARK_COLS;
1332         mt9m111->rect.top       = MT9M111_MIN_DARK_ROWS;
1333         mt9m111->rect.width     = MT9M111_MAX_WIDTH;
1334         mt9m111->rect.height    = MT9M111_MAX_HEIGHT;
1335         mt9m111->width          = mt9m111->rect.width;
1336         mt9m111->height         = mt9m111->rect.height;
1337         mt9m111->fmt            = &mt9m111_colour_fmts[0];
1338         mt9m111->lastpage       = -1;
1339         mutex_init(&mt9m111->power_lock);
1340
1341         ret = mt9m111_video_probe(client);
1342         if (ret < 0)
1343                 goto out_entityclean;
1344
1345         mt9m111->subdev.dev = &client->dev;
1346         ret = v4l2_async_register_subdev(&mt9m111->subdev);
1347         if (ret < 0)
1348                 goto out_entityclean;
1349
1350         return 0;
1351
1352 out_entityclean:
1353 #ifdef CONFIG_MEDIA_CONTROLLER
1354         media_entity_cleanup(&mt9m111->subdev.entity);
1355 out_hdlfree:
1356 #endif
1357         v4l2_ctrl_handler_free(&mt9m111->hdl);
1358
1359         return ret;
1360 }
1361
1362 static void mt9m111_remove(struct i2c_client *client)
1363 {
1364         struct mt9m111 *mt9m111 = to_mt9m111(client);
1365
1366         v4l2_async_unregister_subdev(&mt9m111->subdev);
1367         media_entity_cleanup(&mt9m111->subdev.entity);
1368         v4l2_ctrl_handler_free(&mt9m111->hdl);
1369 }
1370 static const struct of_device_id mt9m111_of_match[] = {
1371         { .compatible = "micron,mt9m111", },
1372         {},
1373 };
1374 MODULE_DEVICE_TABLE(of, mt9m111_of_match);
1375
1376 static const struct i2c_device_id mt9m111_id[] = {
1377         { "mt9m111", 0 },
1378         { }
1379 };
1380 MODULE_DEVICE_TABLE(i2c, mt9m111_id);
1381
1382 static struct i2c_driver mt9m111_i2c_driver = {
1383         .driver = {
1384                 .name = "mt9m111",
1385                 .of_match_table = of_match_ptr(mt9m111_of_match),
1386         },
1387         .probe_new      = mt9m111_probe,
1388         .remove         = mt9m111_remove,
1389         .id_table       = mt9m111_id,
1390 };
1391
1392 module_i2c_driver(mt9m111_i2c_driver);
1393
1394 MODULE_DESCRIPTION("Micron/Aptina MT9M111/MT9M112/MT9M131 Camera driver");
1395 MODULE_AUTHOR("Robert Jarzmik");
1396 MODULE_LICENSE("GPL");