linux/drivers/media/i2c/mt9m111.c
<<
>>
Prefs
   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/gpio.h>
  13#include <linux/delay.h>
  14#include <linux/regulator/consumer.h>
  15#include <linux/v4l2-mediabus.h>
  16#include <linux/module.h>
  17#include <linux/property.h>
  18
  19#include <media/v4l2-async.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
 151struct 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
 163static 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
 175static 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 */
 192struct mt9m111_datafmt {
 193        u32     code;
 194        enum v4l2_colorspace            colorspace;
 195};
 196
 197static 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
 212enum mt9m111_mode_id {
 213        MT9M111_MODE_SXGA_8FPS,
 214        MT9M111_MODE_SXGA_15FPS,
 215        MT9M111_MODE_QSXGA_30FPS,
 216        MT9M111_NUM_MODES,
 217};
 218
 219struct 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
 229struct 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 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
 253static 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 */
 285static 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
 296static struct mt9m111 *to_mt9m111(const struct i2c_client *client)
 297{
 298        return container_of(i2c_get_clientdata(client), struct mt9m111, subdev);
 299}
 300
 301static 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
 319static 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
 331static 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
 343static 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
 354static 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
 365static 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
 376static 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
 383static 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
 398static 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
 430static 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
 436static 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
 451static 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
 495static 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
 520static 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 -EINVAL;
 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
 552static 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
 626static 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
 697static const struct mt9m111_mode_info *
 698mt9m111_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
 753static 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
 772static 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
 787static 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
 800static 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
 812static 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
 830static 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
 839static 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
 848static 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
 859static 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
 867static 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
 890static 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
 917static 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
 935static 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
 949static 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
 960static 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
 975static int mt9m111_power_on(struct mt9m111 *mt9m111)
 976{
 977        struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 978        int ret;
 979
 980        ret = clk_prepare_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
 994out_regulator_disable:
 995        regulator_disable(mt9m111->regulator);
 996
 997out_clk_disable:
 998        clk_disable_unprepare(mt9m111->clk);
 999
1000        dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
1001
1002        return ret;
1003}
1004
1005static void mt9m111_power_off(struct mt9m111 *mt9m111)
1006{
1007        mt9m111_suspend(mt9m111);
1008        regulator_disable(mt9m111->regulator);
1009        clk_disable_unprepare(mt9m111->clk);
1010}
1011
1012static 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
1040static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
1041        .s_ctrl = mt9m111_s_ctrl,
1042};
1043
1044static 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
1055static 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
1065static 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
1102static 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
1113static 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
1121static 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
1140static int mt9m111_get_mbus_config(struct v4l2_subdev *sd,
1141                                   unsigned int pad,
1142                                   struct v4l2_mbus_config *cfg)
1143{
1144        struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1145
1146        cfg->flags = V4L2_MBUS_MASTER |
1147                V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
1148                V4L2_MBUS_DATA_ACTIVE_HIGH;
1149
1150        cfg->flags |= mt9m111->pclk_sample ? V4L2_MBUS_PCLK_SAMPLE_RISING :
1151                V4L2_MBUS_PCLK_SAMPLE_FALLING;
1152
1153        cfg->type = V4L2_MBUS_PARALLEL;
1154
1155        return 0;
1156}
1157
1158static const struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
1159        .s_stream       = mt9m111_s_stream,
1160        .g_frame_interval = mt9m111_g_frame_interval,
1161        .s_frame_interval = mt9m111_s_frame_interval,
1162};
1163
1164static 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        .get_mbus_config = mt9m111_get_mbus_config,
1172};
1173
1174static const struct v4l2_subdev_ops mt9m111_subdev_ops = {
1175        .core   = &mt9m111_subdev_core_ops,
1176        .video  = &mt9m111_subdev_video_ops,
1177        .pad    = &mt9m111_subdev_pad_ops,
1178};
1179
1180/*
1181 * Interface active, can use i2c. If it fails, it can indeed mean, that
1182 * this wasn't our capture interface, so, we wait for the right one
1183 */
1184static int mt9m111_video_probe(struct i2c_client *client)
1185{
1186        struct mt9m111 *mt9m111 = to_mt9m111(client);
1187        s32 data;
1188        int ret;
1189
1190        ret = mt9m111_s_power(&mt9m111->subdev, 1);
1191        if (ret < 0)
1192                return ret;
1193
1194        data = reg_read(CHIP_VERSION);
1195
1196        switch (data) {
1197        case 0x143a: /* MT9M111 or MT9M131 */
1198                dev_info(&client->dev,
1199                        "Detected a MT9M111/MT9M131 chip ID %x\n", data);
1200                break;
1201        case 0x148c: /* MT9M112 */
1202                dev_info(&client->dev, "Detected a MT9M112 chip ID %x\n", data);
1203                break;
1204        default:
1205                dev_err(&client->dev,
1206                        "No MT9M111/MT9M112/MT9M131 chip detected register read %x\n",
1207                        data);
1208                ret = -ENODEV;
1209                goto done;
1210        }
1211
1212        ret = mt9m111_init(mt9m111);
1213        if (ret)
1214                goto done;
1215
1216        ret = v4l2_ctrl_handler_setup(&mt9m111->hdl);
1217
1218done:
1219        mt9m111_s_power(&mt9m111->subdev, 0);
1220        return ret;
1221}
1222
1223static int mt9m111_probe_fw(struct i2c_client *client, struct mt9m111 *mt9m111)
1224{
1225        struct v4l2_fwnode_endpoint bus_cfg = {
1226                .bus_type = V4L2_MBUS_PARALLEL
1227        };
1228        struct fwnode_handle *np;
1229        int ret;
1230
1231        np = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev), NULL);
1232        if (!np)
1233                return -EINVAL;
1234
1235        ret = v4l2_fwnode_endpoint_parse(np, &bus_cfg);
1236        if (ret)
1237                goto out_put_fw;
1238
1239        mt9m111->pclk_sample = !!(bus_cfg.bus.parallel.flags &
1240                                  V4L2_MBUS_PCLK_SAMPLE_RISING);
1241
1242out_put_fw:
1243        fwnode_handle_put(np);
1244        return ret;
1245}
1246
1247static int mt9m111_probe(struct i2c_client *client)
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 = devm_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                return ret;
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
1351out_entityclean:
1352#ifdef CONFIG_MEDIA_CONTROLLER
1353        media_entity_cleanup(&mt9m111->subdev.entity);
1354out_hdlfree:
1355#endif
1356        v4l2_ctrl_handler_free(&mt9m111->hdl);
1357
1358        return ret;
1359}
1360
1361static int mt9m111_remove(struct i2c_client *client)
1362{
1363        struct mt9m111 *mt9m111 = to_mt9m111(client);
1364
1365        v4l2_async_unregister_subdev(&mt9m111->subdev);
1366        media_entity_cleanup(&mt9m111->subdev.entity);
1367        v4l2_ctrl_handler_free(&mt9m111->hdl);
1368
1369        return 0;
1370}
1371static const struct of_device_id mt9m111_of_match[] = {
1372        { .compatible = "micron,mt9m111", },
1373        {},
1374};
1375MODULE_DEVICE_TABLE(of, mt9m111_of_match);
1376
1377static const struct i2c_device_id mt9m111_id[] = {
1378        { "mt9m111", 0 },
1379        { }
1380};
1381MODULE_DEVICE_TABLE(i2c, mt9m111_id);
1382
1383static struct i2c_driver mt9m111_i2c_driver = {
1384        .driver = {
1385                .name = "mt9m111",
1386                .of_match_table = of_match_ptr(mt9m111_of_match),
1387        },
1388        .probe_new      = mt9m111_probe,
1389        .remove         = mt9m111_remove,
1390        .id_table       = mt9m111_id,
1391};
1392
1393module_i2c_driver(mt9m111_i2c_driver);
1394
1395MODULE_DESCRIPTION("Micron/Aptina MT9M111/MT9M112/MT9M131 Camera driver");
1396MODULE_AUTHOR("Robert Jarzmik");
1397MODULE_LICENSE("GPL");
1398