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