drivers/media/i2c/mt9m111.c

   1 /*
   2  * Driver for MT9M111/MT9M112/MT9M131 CMOS Image Sensor from Micron/Aptina
   3  *
   4  * Copyright (C) 2008, Robert Jarzmik <robert.jarzmik@free.fr>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License version 2 as
   8  * published by the Free Software Foundation.
   9  */
  10 #include <linux/videodev2.h>
  11 #include <linux/slab.h>
  12 #include <linux/i2c.h>
  13 #include <linux/log2.h>
  14 #include <linux/gpio.h>
  15 #include <linux/delay.h>
  16 #include <linux/v4l2-mediabus.h>
  17 #include <linux/module.h>
  18
  19 #include <media/v4l2-async.h>
  20 #include <media/v4l2-clk.h>
  21 #include <media/v4l2-common.h>
  22 #include <media/v4l2-ctrls.h>
  23 #include <media/v4l2-device.h>
  24
  25 /*
  26  * MT9M111, MT9M112 and MT9M131:
  27  * i2c address is 0x48 or 0x5d (depending on SADDR pin)
  28  * The platform has to define struct i2c_board_info objects and link to them
  29  * from struct soc_camera_host_desc
  30  */
  31
  32 /*
  33  * Sensor core register addresses (0x000..0x0ff)
  34  */
  35 #define MT9M111_CHIP_VERSION            0x000
  36 #define MT9M111_ROW_START               0x001
  37 #define MT9M111_COLUMN_START            0x002
  38 #define MT9M111_WINDOW_HEIGHT           0x003
  39 #define MT9M111_WINDOW_WIDTH            0x004
  40 #define MT9M111_HORIZONTAL_BLANKING_B   0x005
  41 #define MT9M111_VERTICAL_BLANKING_B     0x006
  42 #define MT9M111_HORIZONTAL_BLANKING_A   0x007
  43 #define MT9M111_VERTICAL_BLANKING_A     0x008
  44 #define MT9M111_SHUTTER_WIDTH           0x009
  45 #define MT9M111_ROW_SPEED               0x00a
  46 #define MT9M111_EXTRA_DELAY             0x00b
  47 #define MT9M111_SHUTTER_DELAY           0x00c
  48 #define MT9M111_RESET                   0x00d
  49 #define MT9M111_READ_MODE_B             0x020
  50 #define MT9M111_READ_MODE_A             0x021
  51 #define MT9M111_FLASH_CONTROL           0x023
  52 #define MT9M111_GREEN1_GAIN             0x02b
  53 #define MT9M111_BLUE_GAIN               0x02c
  54 #define MT9M111_RED_GAIN                0x02d
  55 #define MT9M111_GREEN2_GAIN             0x02e
  56 #define MT9M111_GLOBAL_GAIN             0x02f
  57 #define MT9M111_CONTEXT_CONTROL         0x0c8
  58 #define MT9M111_PAGE_MAP                0x0f0
  59 #define MT9M111_BYTE_WISE_ADDR          0x0f1
  60
  61 #define MT9M111_RESET_SYNC_CHANGES      (1 << 15)
  62 #define MT9M111_RESET_RESTART_BAD_FRAME (1 << 9)
  63 #define MT9M111_RESET_SHOW_BAD_FRAMES   (1 << 8)
  64 #define MT9M111_RESET_RESET_SOC         (1 << 5)
  65 #define MT9M111_RESET_OUTPUT_DISABLE    (1 << 4)
  66 #define MT9M111_RESET_CHIP_ENABLE       (1 << 3)
  67 #define MT9M111_RESET_ANALOG_STANDBY    (1 << 2)
  68 #define MT9M111_RESET_RESTART_FRAME     (1 << 1)
  69 #define MT9M111_RESET_RESET_MODE        (1 << 0)
  70
  71 #define MT9M111_RM_FULL_POWER_RD        (0 << 10)
  72 #define MT9M111_RM_LOW_POWER_RD         (1 << 10)
  73 #define MT9M111_RM_COL_SKIP_4X          (1 << 5)
  74 #define MT9M111_RM_ROW_SKIP_4X          (1 << 4)
  75 #define MT9M111_RM_COL_SKIP_2X          (1 << 3)
  76 #define MT9M111_RM_ROW_SKIP_2X          (1 << 2)
  77 #define MT9M111_RMB_MIRROR_COLS         (1 << 1)
  78 #define MT9M111_RMB_MIRROR_ROWS         (1 << 0)
  79 #define MT9M111_CTXT_CTRL_RESTART       (1 << 15)
  80 #define MT9M111_CTXT_CTRL_DEFECTCOR_B   (1 << 12)
  81 #define MT9M111_CTXT_CTRL_RESIZE_B      (1 << 10)
  82 #define MT9M111_CTXT_CTRL_CTRL2_B       (1 << 9)
  83 #define MT9M111_CTXT_CTRL_GAMMA_B       (1 << 8)
  84 #define MT9M111_CTXT_CTRL_XENON_EN      (1 << 7)
  85 #define MT9M111_CTXT_CTRL_READ_MODE_B   (1 << 3)
  86 #define MT9M111_CTXT_CTRL_LED_FLASH_EN  (1 << 2)
  87 #define MT9M111_CTXT_CTRL_VBLANK_SEL_B  (1 << 1)
  88 #define MT9M111_CTXT_CTRL_HBLANK_SEL_B  (1 << 0)
  89
  90 /*
  91  * Colorpipe register addresses (0x100..0x1ff)
  92  */
  93 #define MT9M111_OPER_MODE_CTRL          0x106
  94 #define MT9M111_OUTPUT_FORMAT_CTRL      0x108
  95 #define MT9M111_TPG_CTRL                0x148
  96 #define MT9M111_REDUCER_XZOOM_B         0x1a0
  97 #define MT9M111_REDUCER_XSIZE_B         0x1a1
  98 #define MT9M111_REDUCER_YZOOM_B         0x1a3
  99 #define MT9M111_REDUCER_YSIZE_B         0x1a4
 100 #define MT9M111_REDUCER_XZOOM_A         0x1a6
 101 #define MT9M111_REDUCER_XSIZE_A         0x1a7
 102 #define MT9M111_REDUCER_YZOOM_A         0x1a9
 103 #define MT9M111_REDUCER_YSIZE_A         0x1aa
 104
 105 #define MT9M111_OUTPUT_FORMAT_CTRL2_A   0x13a
 106 #define MT9M111_OUTPUT_FORMAT_CTRL2_B   0x19b
 107
 108 #define MT9M111_OPMODE_AUTOEXPO_EN      (1 << 14)
 109 #define MT9M111_OPMODE_AUTOWHITEBAL_EN  (1 << 1)
 110 #define MT9M111_OUTFMT_FLIP_BAYER_COL   (1 << 9)
 111 #define MT9M111_OUTFMT_FLIP_BAYER_ROW   (1 << 8)
 112 #define MT9M111_OUTFMT_PROCESSED_BAYER  (1 << 14)
 113 #define MT9M111_OUTFMT_BYPASS_IFP       (1 << 10)
 114 #define MT9M111_OUTFMT_INV_PIX_CLOCK    (1 << 9)
 115 #define MT9M111_OUTFMT_RGB              (1 << 8)
 116 #define MT9M111_OUTFMT_RGB565           (0 << 6)
 117 #define MT9M111_OUTFMT_RGB555           (1 << 6)
 118 #define MT9M111_OUTFMT_RGB444x          (2 << 6)
 119 #define MT9M111_OUTFMT_RGBx444          (3 << 6)
 120 #define MT9M111_OUTFMT_TST_RAMP_OFF     (0 << 4)
 121 #define MT9M111_OUTFMT_TST_RAMP_COL     (1 << 4)
 122 #define MT9M111_OUTFMT_TST_RAMP_ROW     (2 << 4)
 123 #define MT9M111_OUTFMT_TST_RAMP_FRAME   (3 << 4)
 124 #define MT9M111_OUTFMT_SHIFT_3_UP       (1 << 3)
 125 #define MT9M111_OUTFMT_AVG_CHROMA       (1 << 2)
 126 #define MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN  (1 << 1)
 127 #define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B (1 << 0)
 128 #define MT9M111_TPG_SEL_MASK            GENMASK(2, 0)
 129
 130 /*
 131  * Camera control register addresses (0x200..0x2ff not implemented)
 132  */
 133
 134 #define reg_read(reg) mt9m111_reg_read(client, MT9M111_##reg)
 135 #define reg_write(reg, val) mt9m111_reg_write(client, MT9M111_##reg, (val))
 136 #define reg_set(reg, val) mt9m111_reg_set(client, MT9M111_##reg, (val))
 137 #define reg_clear(reg, val) mt9m111_reg_clear(client, MT9M111_##reg, (val))
 138 #define reg_mask(reg, val, mask) mt9m111_reg_mask(client, MT9M111_##reg, \
 139                 (val), (mask))
 140
 141 #define MT9M111_MIN_DARK_ROWS   8
 142 #define MT9M111_MIN_DARK_COLS   26
 143 #define MT9M111_MAX_HEIGHT      1024
 144 #define MT9M111_MAX_WIDTH       1280
 145
 146 struct mt9m111_context {
 147         u16 read_mode;
 148         u16 blanking_h;
 149         u16 blanking_v;
 150         u16 reducer_xzoom;
 151         u16 reducer_yzoom;
 152         u16 reducer_xsize;
 153         u16 reducer_ysize;
 154         u16 output_fmt_ctrl2;
 155         u16 control;
 156 };
 157
 158 static struct mt9m111_context context_a = {
 159         .read_mode              = MT9M111_READ_MODE_A,
 160         .blanking_h             = MT9M111_HORIZONTAL_BLANKING_A,
 161         .blanking_v             = MT9M111_VERTICAL_BLANKING_A,
 162         .reducer_xzoom          = MT9M111_REDUCER_XZOOM_A,
 163         .reducer_yzoom          = MT9M111_REDUCER_YZOOM_A,
 164         .reducer_xsize          = MT9M111_REDUCER_XSIZE_A,
 165         .reducer_ysize          = MT9M111_REDUCER_YSIZE_A,
 166         .output_fmt_ctrl2       = MT9M111_OUTPUT_FORMAT_CTRL2_A,
 167         .control                = MT9M111_CTXT_CTRL_RESTART,
 168 };
 169
 170 static struct mt9m111_context context_b = {
 171         .read_mode              = MT9M111_READ_MODE_B,
 172         .blanking_h             = MT9M111_HORIZONTAL_BLANKING_B,
 173         .blanking_v             = MT9M111_VERTICAL_BLANKING_B,
 174         .reducer_xzoom          = MT9M111_REDUCER_XZOOM_B,
 175         .reducer_yzoom          = MT9M111_REDUCER_YZOOM_B,
 176         .reducer_xsize          = MT9M111_REDUCER_XSIZE_B,
 177         .reducer_ysize          = MT9M111_REDUCER_YSIZE_B,
 178         .output_fmt_ctrl2       = MT9M111_OUTPUT_FORMAT_CTRL2_B,
 179         .control                = MT9M111_CTXT_CTRL_RESTART |
 180                 MT9M111_CTXT_CTRL_DEFECTCOR_B | MT9M111_CTXT_CTRL_RESIZE_B |
 181                 MT9M111_CTXT_CTRL_CTRL2_B | MT9M111_CTXT_CTRL_GAMMA_B |
 182                 MT9M111_CTXT_CTRL_READ_MODE_B | MT9M111_CTXT_CTRL_VBLANK_SEL_B |
 183                 MT9M111_CTXT_CTRL_HBLANK_SEL_B,
 184 };
 185
 186 /* MT9M111 has only one fixed colorspace per pixelcode */
 187 struct mt9m111_datafmt {
 188         u32     code;
 189         enum v4l2_colorspace            colorspace;
 190 };
 191
 192 static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
 193         {MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB},
 194         {MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_SRGB},
 195         {MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB},
 196         {MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_SRGB},
 197         {MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
 198         {MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
 199         {MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
 200         {MEDIA_BUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
 201         {MEDIA_BUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB},
 202         {MEDIA_BUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB},
 203         {MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
 204         {MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
 205 };
 206
 207 struct mt9m111 {
 208         struct v4l2_subdev subdev;
 209         struct v4l2_ctrl_handler hdl;
 210         struct v4l2_ctrl *gain;
 211         struct mt9m111_context *ctx;
 212         struct v4l2_rect rect;  /* cropping rectangle */
 213         struct v4l2_clk *clk;
 214         unsigned int width;     /* output */
 215         unsigned int height;    /* sizes */
 216         struct mutex power_lock; /* lock to protect power_count */
 217         int power_count;
 218         const struct mt9m111_datafmt *fmt;
 219         int lastpage;   /* PageMap cache value */
 220 #ifdef CONFIG_MEDIA_CONTROLLER
 221         struct media_pad pad;
 222 #endif
 223 };
 224
 225 /* Find a data format by a pixel code */
 226 static const struct mt9m111_datafmt *mt9m111_find_datafmt(struct mt9m111 *mt9m111,
 227                                                 u32 code)
 228 {
 229         int i;
 230         for (i = 0; i < ARRAY_SIZE(mt9m111_colour_fmts); i++)
 231                 if (mt9m111_colour_fmts[i].code == code)
 232                         return mt9m111_colour_fmts + i;
 233
 234         return mt9m111->fmt;
 235 }
 236
 237 static struct mt9m111 *to_mt9m111(const struct i2c_client *client)
 238 {
 239         return container_of(i2c_get_clientdata(client), struct mt9m111, subdev);
 240 }
 241
 242 static int reg_page_map_set(struct i2c_client *client, const u16 reg)
 243 {
 244         int ret;
 245         u16 page;
 246         struct mt9m111 *mt9m111 = to_mt9m111(client);
 247
 248         page = (reg >> 8);
 249         if (page == mt9m111->lastpage)
 250                 return 0;
 251         if (page > 2)
 252                 return -EINVAL;
 253
 254         ret = i2c_smbus_write_word_swapped(client, MT9M111_PAGE_MAP, page);
 255         if (!ret)
 256                 mt9m111->lastpage = page;
 257         return ret;
 258 }
 259
 260 static int mt9m111_reg_read(struct i2c_client *client, const u16 reg)
 261 {
 262         int ret;
 263
 264         ret = reg_page_map_set(client, reg);
 265         if (!ret)
 266                 ret = i2c_smbus_read_word_swapped(client, reg & 0xff);
 267
 268         dev_dbg(&client->dev, "read  reg.%03x -> %04x\n", reg, ret);
 269         return ret;
 270 }
 271
 272 static int mt9m111_reg_write(struct i2c_client *client, const u16 reg,
 273                              const u16 data)
 274 {
 275         int ret;
 276
 277         ret = reg_page_map_set(client, reg);
 278         if (!ret)
 279                 ret = i2c_smbus_write_word_swapped(client, reg & 0xff, data);
 280         dev_dbg(&client->dev, "write reg.%03x = %04x -> %d\n", reg, data, ret);
 281         return ret;
 282 }
 283
 284 static int mt9m111_reg_set(struct i2c_client *client, const u16 reg,
 285                            const u16 data)
 286 {
 287         int ret;
 288
 289         ret = mt9m111_reg_read(client, reg);
 290         if (ret >= 0)
 291                 ret = mt9m111_reg_write(client, reg, ret | data);
 292         return ret;
 293 }
 294
 295 static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg,
 296                              const u16 data)
 297 {
 298         int ret;
 299
 300         ret = mt9m111_reg_read(client, reg);
 301         if (ret >= 0)
 302                 ret = mt9m111_reg_write(client, reg, ret & ~data);
 303         return ret;
 304 }
 305
 306 static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg,
 307                             const u16 data, const u16 mask)
 308 {
 309         int ret;
 310
 311         ret = mt9m111_reg_read(client, reg);
 312         if (ret >= 0)
 313                 ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data);
 314         return ret;
 315 }
 316
 317 static int mt9m111_set_context(struct mt9m111 *mt9m111,
 318                                struct mt9m111_context *ctx)
 319 {
 320         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 321         return reg_write(CONTEXT_CONTROL, ctx->control);
 322 }
 323
 324 static int mt9m111_setup_rect_ctx(struct mt9m111 *mt9m111,
 325                         struct mt9m111_context *ctx, struct v4l2_rect *rect,
 326                         unsigned int width, unsigned int height)
 327 {
 328         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 329         int ret = mt9m111_reg_write(client, ctx->reducer_xzoom, rect->width);
 330         if (!ret)
 331                 ret = mt9m111_reg_write(client, ctx->reducer_yzoom, rect->height);
 332         if (!ret)
 333                 ret = mt9m111_reg_write(client, ctx->reducer_xsize, width);
 334         if (!ret)
 335                 ret = mt9m111_reg_write(client, ctx->reducer_ysize, height);
 336         return ret;
 337 }
 338
 339 static int mt9m111_setup_geometry(struct mt9m111 *mt9m111, struct v4l2_rect *rect,
 340                         int width, int height, u32 code)
 341 {
 342         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 343         int ret;
 344
 345         ret = reg_write(COLUMN_START, rect->left);
 346         if (!ret)
 347                 ret = reg_write(ROW_START, rect->top);
 348
 349         if (!ret)
 350                 ret = reg_write(WINDOW_WIDTH, rect->width);
 351         if (!ret)
 352                 ret = reg_write(WINDOW_HEIGHT, rect->height);
 353
 354         if (code != MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
 355                 /* IFP in use, down-scaling possible */
 356                 if (!ret)
 357                         ret = mt9m111_setup_rect_ctx(mt9m111, &context_b,
 358                                                      rect, width, height);
 359                 if (!ret)
 360                         ret = mt9m111_setup_rect_ctx(mt9m111, &context_a,
 361                                                      rect, width, height);
 362         }
 363
 364         dev_dbg(&client->dev, "%s(%x): %ux%u@%u:%u -> %ux%u = %d\n",
 365                 __func__, code, rect->width, rect->height, rect->left, rect->top,
 366                 width, height, ret);
 367
 368         return ret;
 369 }
 370
 371 static int mt9m111_enable(struct mt9m111 *mt9m111)
 372 {
 373         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 374         return reg_write(RESET, MT9M111_RESET_CHIP_ENABLE);
 375 }
 376
 377 static int mt9m111_reset(struct mt9m111 *mt9m111)
 378 {
 379         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 380         int ret;
 381
 382         ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
 383         if (!ret)
 384                 ret = reg_set(RESET, MT9M111_RESET_RESET_SOC);
 385         if (!ret)
 386                 ret = reg_clear(RESET, MT9M111_RESET_RESET_MODE
 387                                 | MT9M111_RESET_RESET_SOC);
 388
 389         return ret;
 390 }
 391
 392 static int mt9m111_set_selection(struct v4l2_subdev *sd,
 393                                  struct v4l2_subdev_pad_config *cfg,
 394                                  struct v4l2_subdev_selection *sel)
 395 {
 396         struct i2c_client *client = v4l2_get_subdevdata(sd);
 397         struct mt9m111 *mt9m111 = to_mt9m111(client);
 398         struct v4l2_rect rect = sel->r;
 399         int width, height;
 400         int ret, align = 0;
 401
 402         if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
 403             sel->target != V4L2_SEL_TGT_CROP)
 404                 return -EINVAL;
 405
 406         if (mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
 407             mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
 408                 /* Bayer format - even size lengths */
 409                 align = 1;
 410                 /* Let the user play with the starting pixel */
 411         }
 412
 413         /* FIXME: the datasheet doesn't specify minimum sizes */
 414         v4l_bound_align_image(&rect.width, 2, MT9M111_MAX_WIDTH, align,
 415                               &rect.height, 2, MT9M111_MAX_HEIGHT, align, 0);
 416         rect.left = clamp(rect.left, MT9M111_MIN_DARK_COLS,
 417                           MT9M111_MIN_DARK_COLS + MT9M111_MAX_WIDTH -
 418                           (__s32)rect.width);
 419         rect.top = clamp(rect.top, MT9M111_MIN_DARK_ROWS,
 420                          MT9M111_MIN_DARK_ROWS + MT9M111_MAX_HEIGHT -
 421                          (__s32)rect.height);
 422
 423         width = min(mt9m111->width, rect.width);
 424         height = min(mt9m111->height, rect.height);
 425
 426         ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
 427         if (!ret) {
 428                 mt9m111->rect = rect;
 429                 mt9m111->width = width;
 430                 mt9m111->height = height;
 431         }
 432
 433         return ret;
 434 }
 435
 436 static int mt9m111_get_selection(struct v4l2_subdev *sd,
 437                                  struct v4l2_subdev_pad_config *cfg,
 438                                  struct v4l2_subdev_selection *sel)
 439 {
 440         struct i2c_client *client = v4l2_get_subdevdata(sd);
 441         struct mt9m111 *mt9m111 = to_mt9m111(client);
 442
 443         if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
 444                 return -EINVAL;
 445
 446         switch (sel->target) {
 447         case V4L2_SEL_TGT_CROP_BOUNDS:
 448         case V4L2_SEL_TGT_CROP_DEFAULT:
 449                 sel->r.left = MT9M111_MIN_DARK_COLS;
 450                 sel->r.top = MT9M111_MIN_DARK_ROWS;
 451                 sel->r.width = MT9M111_MAX_WIDTH;
 452                 sel->r.height = MT9M111_MAX_HEIGHT;
 453                 return 0;
 454         case V4L2_SEL_TGT_CROP:
 455                 sel->r = mt9m111->rect;
 456                 return 0;
 457         default:
 458                 return -EINVAL;
 459         }
 460 }
 461
 462 static int mt9m111_get_fmt(struct v4l2_subdev *sd,
 463                 struct v4l2_subdev_pad_config *cfg,
 464                 struct v4l2_subdev_format *format)
 465 {
 466         struct v4l2_mbus_framefmt *mf = &format->format;
 467         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 468
 469         if (format->pad)
 470                 return -EINVAL;
 471
 472         mf->width       = mt9m111->width;
 473         mf->height      = mt9m111->height;
 474         mf->code        = mt9m111->fmt->code;
 475         mf->colorspace  = mt9m111->fmt->colorspace;
 476         mf->field       = V4L2_FIELD_NONE;
 477
 478         return 0;
 479 }
 480
 481 static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
 482                               u32 code)
 483 {
 484         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 485         u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
 486                 MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB |
 487                 MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
 488                 MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 |
 489                 MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
 490                 MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 491         int ret;
 492
 493         switch (code) {
 494         case MEDIA_BUS_FMT_SBGGR8_1X8:
 495                 data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
 496                         MT9M111_OUTFMT_RGB;
 497                 break;
 498         case MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE:
 499                 data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
 500                 break;
 501         case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
 502                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
 503                         MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
 504                 break;
 505         case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
 506                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
 507                 break;
 508         case MEDIA_BUS_FMT_RGB565_2X8_LE:
 509                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
 510                         MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
 511                 break;
 512         case MEDIA_BUS_FMT_RGB565_2X8_BE:
 513                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
 514                 break;
 515         case MEDIA_BUS_FMT_BGR565_2X8_BE:
 516                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
 517                         MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 518                 break;
 519         case MEDIA_BUS_FMT_BGR565_2X8_LE:
 520                 data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
 521                         MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
 522                         MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 523                 break;
 524         case MEDIA_BUS_FMT_UYVY8_2X8:
 525                 data_outfmt2 = 0;
 526                 break;
 527         case MEDIA_BUS_FMT_VYUY8_2X8:
 528                 data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 529                 break;
 530         case MEDIA_BUS_FMT_YUYV8_2X8:
 531                 data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
 532                 break;
 533         case MEDIA_BUS_FMT_YVYU8_2X8:
 534                 data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
 535                         MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
 536                 break;
 537         default:
 538                 dev_err(&client->dev, "Pixel format not handled: %x\n", code);
 539                 return -EINVAL;
 540         }
 541
 542         ret = mt9m111_reg_mask(client, context_a.output_fmt_ctrl2,
 543                                data_outfmt2, mask_outfmt2);
 544         if (!ret)
 545                 ret = mt9m111_reg_mask(client, context_b.output_fmt_ctrl2,
 546                                        data_outfmt2, mask_outfmt2);
 547
 548         return ret;
 549 }
 550
 551 static int mt9m111_set_fmt(struct v4l2_subdev *sd,
 552                 struct v4l2_subdev_pad_config *cfg,
 553                 struct v4l2_subdev_format *format)
 554 {
 555         struct v4l2_mbus_framefmt *mf = &format->format;
 556         struct i2c_client *client = v4l2_get_subdevdata(sd);
 557         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 558         const struct mt9m111_datafmt *fmt;
 559         struct v4l2_rect *rect = &mt9m111->rect;
 560         bool bayer;
 561         int ret;
 562
 563         if (format->pad)
 564                 return -EINVAL;
 565
 566         fmt = mt9m111_find_datafmt(mt9m111, mf->code);
 567
 568         bayer = fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
 569                 fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE;
 570
 571         /*
 572          * With Bayer format enforce even side lengths, but let the user play
 573          * with the starting pixel
 574          */
 575         if (bayer) {
 576                 rect->width = ALIGN(rect->width, 2);
 577                 rect->height = ALIGN(rect->height, 2);
 578         }
 579
 580         if (fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
 581                 /* IFP bypass mode, no scaling */
 582                 mf->width = rect->width;
 583                 mf->height = rect->height;
 584         } else {
 585                 /* No upscaling */
 586                 if (mf->width > rect->width)
 587                         mf->width = rect->width;
 588                 if (mf->height > rect->height)
 589                         mf->height = rect->height;
 590         }
 591
 592         dev_dbg(&client->dev, "%s(): %ux%u, code=%x\n", __func__,
 593                 mf->width, mf->height, fmt->code);
 594
 595         mf->code = fmt->code;
 596         mf->colorspace = fmt->colorspace;
 597
 598         if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
 599                 cfg->try_fmt = *mf;
 600                 return 0;
 601         }
 602
 603         ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
 604         if (!ret)
 605                 ret = mt9m111_set_pixfmt(mt9m111, mf->code);
 606         if (!ret) {
 607                 mt9m111->width  = mf->width;
 608                 mt9m111->height = mf->height;
 609                 mt9m111->fmt    = fmt;
 610         }
 611
 612         return ret;
 613 }
 614
 615 #ifdef CONFIG_VIDEO_ADV_DEBUG
 616 static int mt9m111_g_register(struct v4l2_subdev *sd,
 617                               struct v4l2_dbg_register *reg)
 618 {
 619         struct i2c_client *client = v4l2_get_subdevdata(sd);
 620         int val;
 621
 622         if (reg->reg > 0x2ff)
 623                 return -EINVAL;
 624
 625         val = mt9m111_reg_read(client, reg->reg);
 626         reg->size = 2;
 627         reg->val = (u64)val;
 628
 629         if (reg->val > 0xffff)
 630                 return -EIO;
 631
 632         return 0;
 633 }
 634
 635 static int mt9m111_s_register(struct v4l2_subdev *sd,
 636                               const struct v4l2_dbg_register *reg)
 637 {
 638         struct i2c_client *client = v4l2_get_subdevdata(sd);
 639
 640         if (reg->reg > 0x2ff)
 641                 return -EINVAL;
 642
 643         if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
 644                 return -EIO;
 645
 646         return 0;
 647 }
 648 #endif
 649
 650 static int mt9m111_set_flip(struct mt9m111 *mt9m111, int flip, int mask)
 651 {
 652         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 653         int ret;
 654
 655         if (flip)
 656                 ret = mt9m111_reg_set(client, mt9m111->ctx->read_mode, mask);
 657         else
 658                 ret = mt9m111_reg_clear(client, mt9m111->ctx->read_mode, mask);
 659
 660         return ret;
 661 }
 662
 663 static int mt9m111_get_global_gain(struct mt9m111 *mt9m111)
 664 {
 665         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 666         int data;
 667
 668         data = reg_read(GLOBAL_GAIN);
 669         if (data >= 0)
 670                 return (data & 0x2f) * (1 << ((data >> 10) & 1)) *
 671                         (1 << ((data >> 9) & 1));
 672         return data;
 673 }
 674
 675 static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
 676 {
 677         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 678         u16 val;
 679
 680         if (gain > 63 * 2 * 2)
 681                 return -EINVAL;
 682
 683         if ((gain >= 64 * 2) && (gain < 63 * 2 * 2))
 684                 val = (1 << 10) | (1 << 9) | (gain / 4);
 685         else if ((gain >= 64) && (gain < 64 * 2))
 686                 val = (1 << 9) | (gain / 2);
 687         else
 688                 val = gain;
 689
 690         return reg_write(GLOBAL_GAIN, val);
 691 }
 692
 693 static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int val)
 694 {
 695         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 696
 697         if (val == V4L2_EXPOSURE_AUTO)
 698                 return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
 699         return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
 700 }
 701
 702 static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on)
 703 {
 704         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 705
 706         if (on)
 707                 return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
 708         return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
 709 }
 710
 711 static const char * const mt9m111_test_pattern_menu[] = {
 712         "Disabled",
 713         "Vertical monochrome gradient",
 714         "Flat color type 1",
 715         "Flat color type 2",
 716         "Flat color type 3",
 717         "Flat color type 4",
 718         "Flat color type 5",
 719         "Color bar",
 720 };
 721
 722 static int mt9m111_set_test_pattern(struct mt9m111 *mt9m111, int val)
 723 {
 724         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 725
 726         return mt9m111_reg_mask(client, MT9M111_TPG_CTRL, val,
 727                                 MT9M111_TPG_SEL_MASK);
 728 }
 729
 730 static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
 731 {
 732         struct mt9m111 *mt9m111 = container_of(ctrl->handler,
 733                                                struct mt9m111, hdl);
 734
 735         switch (ctrl->id) {
 736         case V4L2_CID_VFLIP:
 737                 return mt9m111_set_flip(mt9m111, ctrl->val,
 738                                         MT9M111_RMB_MIRROR_ROWS);
 739         case V4L2_CID_HFLIP:
 740                 return mt9m111_set_flip(mt9m111, ctrl->val,
 741                                         MT9M111_RMB_MIRROR_COLS);
 742         case V4L2_CID_GAIN:
 743                 return mt9m111_set_global_gain(mt9m111, ctrl->val);
 744         case V4L2_CID_EXPOSURE_AUTO:
 745                 return mt9m111_set_autoexposure(mt9m111, ctrl->val);
 746         case V4L2_CID_AUTO_WHITE_BALANCE:
 747                 return mt9m111_set_autowhitebalance(mt9m111, ctrl->val);
 748         case V4L2_CID_TEST_PATTERN:
 749                 return mt9m111_set_test_pattern(mt9m111, ctrl->val);
 750         }
 751
 752         return -EINVAL;
 753 }
 754
 755 static int mt9m111_suspend(struct mt9m111 *mt9m111)
 756 {
 757         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 758         int ret;
 759
 760         v4l2_ctrl_s_ctrl(mt9m111->gain, mt9m111_get_global_gain(mt9m111));
 761
 762         ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
 763         if (!ret)
 764                 ret = reg_set(RESET, MT9M111_RESET_RESET_SOC |
 765                               MT9M111_RESET_OUTPUT_DISABLE |
 766                               MT9M111_RESET_ANALOG_STANDBY);
 767         if (!ret)
 768                 ret = reg_clear(RESET, MT9M111_RESET_CHIP_ENABLE);
 769
 770         return ret;
 771 }
 772
 773 static void mt9m111_restore_state(struct mt9m111 *mt9m111)
 774 {
 775         mt9m111_set_context(mt9m111, mt9m111->ctx);
 776         mt9m111_set_pixfmt(mt9m111, mt9m111->fmt->code);
 777         mt9m111_setup_geometry(mt9m111, &mt9m111->rect,
 778                         mt9m111->width, mt9m111->height, mt9m111->fmt->code);
 779         v4l2_ctrl_handler_setup(&mt9m111->hdl);
 780 }
 781
 782 static int mt9m111_resume(struct mt9m111 *mt9m111)
 783 {
 784         int ret = mt9m111_enable(mt9m111);
 785         if (!ret)
 786                 ret = mt9m111_reset(mt9m111);
 787         if (!ret)
 788                 mt9m111_restore_state(mt9m111);
 789
 790         return ret;
 791 }
 792
 793 static int mt9m111_init(struct mt9m111 *mt9m111)
 794 {
 795         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 796         int ret;
 797
 798         ret = mt9m111_enable(mt9m111);
 799         if (!ret)
 800                 ret = mt9m111_reset(mt9m111);
 801         if (!ret)
 802                 ret = mt9m111_set_context(mt9m111, mt9m111->ctx);
 803         if (ret)
 804                 dev_err(&client->dev, "mt9m111 init failed: %d\n", ret);
 805         return ret;
 806 }
 807
 808 static int mt9m111_power_on(struct mt9m111 *mt9m111)
 809 {
 810         struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
 811         int ret;
 812
 813         ret = v4l2_clk_enable(mt9m111->clk);
 814         if (ret < 0)
 815                 return ret;
 816
 817         ret = mt9m111_resume(mt9m111);
 818         if (ret < 0) {
 819                 dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
 820                 v4l2_clk_disable(mt9m111->clk);
 821         }
 822
 823         return ret;
 824 }
 825
 826 static void mt9m111_power_off(struct mt9m111 *mt9m111)
 827 {
 828         mt9m111_suspend(mt9m111);
 829         v4l2_clk_disable(mt9m111->clk);
 830 }
 831
 832 static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
 833 {
 834         struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 835         int ret = 0;
 836
 837         mutex_lock(&mt9m111->power_lock);
 838
 839         /*
 840          * If the power count is modified from 0 to != 0 or from != 0 to 0,
 841          * update the power state.
 842          */
 843         if (mt9m111->power_count == !on) {
 844                 if (on)
 845                         ret = mt9m111_power_on(mt9m111);
 846                 else
 847                         mt9m111_power_off(mt9m111);
 848         }
 849
 850         if (!ret) {
 851                 /* Update the power count. */
 852                 mt9m111->power_count += on ? 1 : -1;
 853                 WARN_ON(mt9m111->power_count < 0);
 854         }
 855
 856         mutex_unlock(&mt9m111->power_lock);
 857         return ret;
 858 }
 859
 860 static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
 861         .s_ctrl = mt9m111_s_ctrl,
 862 };
 863
 864 static const struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
 865         .s_power        = mt9m111_s_power,
 866 #ifdef CONFIG_VIDEO_ADV_DEBUG
 867         .g_register     = mt9m111_g_register,
 868         .s_register     = mt9m111_s_register,
 869 #endif
 870 };
 871
 872 static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
 873                 struct v4l2_subdev_pad_config *cfg,
 874                 struct v4l2_subdev_mbus_code_enum *code)
 875 {
 876         if (code->pad || code->index >= ARRAY_SIZE(mt9m111_colour_fmts))
 877                 return -EINVAL;
 878
 879         code->code = mt9m111_colour_fmts[code->index].code;
 880         return 0;
 881 }
 882
 883 static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
 884                                 struct v4l2_mbus_config *cfg)
 885 {
 886         cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
 887                 V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
 888                 V4L2_MBUS_DATA_ACTIVE_HIGH;
 889         cfg->type = V4L2_MBUS_PARALLEL;
 890
 891         return 0;
 892 }
 893
 894 static const struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
 895         .g_mbus_config  = mt9m111_g_mbus_config,
 896 };
 897
 898 static const struct v4l2_subdev_pad_ops mt9m111_subdev_pad_ops = {
 899         .enum_mbus_code = mt9m111_enum_mbus_code,
 900         .get_selection  = mt9m111_get_selection,
 901         .set_selection  = mt9m111_set_selection,
 902         .get_fmt        = mt9m111_get_fmt,
 903         .set_fmt        = mt9m111_set_fmt,
 904 };
 905
 906 static const struct v4l2_subdev_ops mt9m111_subdev_ops = {
 907         .core   = &mt9m111_subdev_core_ops,
 908         .video  = &mt9m111_subdev_video_ops,
 909         .pad    = &mt9m111_subdev_pad_ops,
 910 };
 911
 912 /*
 913  * Interface active, can use i2c. If it fails, it can indeed mean, that
 914  * this wasn't our capture interface, so, we wait for the right one
 915  */
 916 static int mt9m111_video_probe(struct i2c_client *client)
 917 {
 918         struct mt9m111 *mt9m111 = to_mt9m111(client);
 919         s32 data;
 920         int ret;
 921
 922         ret = mt9m111_s_power(&mt9m111->subdev, 1);
 923         if (ret < 0)
 924                 return ret;
 925
 926         data = reg_read(CHIP_VERSION);
 927
 928         switch (data) {
 929         case 0x143a: /* MT9M111 or MT9M131 */
 930                 dev_info(&client->dev,
 931                         "Detected a MT9M111/MT9M131 chip ID %x\n", data);
 932                 break;
 933         case 0x148c: /* MT9M112 */
 934                 dev_info(&client->dev, "Detected a MT9M112 chip ID %x\n", data);
 935                 break;
 936         default:
 937                 dev_err(&client->dev,
 938                         "No MT9M111/MT9M112/MT9M131 chip detected register read %x\n",
 939                         data);
 940                 ret = -ENODEV;
 941                 goto done;
 942         }
 943
 944         ret = mt9m111_init(mt9m111);
 945         if (ret)
 946                 goto done;
 947
 948         ret = v4l2_ctrl_handler_setup(&mt9m111->hdl);
 949
 950 done:
 951         mt9m111_s_power(&mt9m111->subdev, 0);
 952         return ret;
 953 }
 954
 955 static int mt9m111_probe(struct i2c_client *client,
 956                          const struct i2c_device_id *did)
 957 {
 958         struct mt9m111 *mt9m111;
 959         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
 960         int ret;
 961
 962         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
 963                 dev_warn(&adapter->dev,
 964                          "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
 965                 return -EIO;
 966         }
 967
 968         mt9m111 = devm_kzalloc(&client->dev, sizeof(struct mt9m111), GFP_KERNEL);
 969         if (!mt9m111)
 970                 return -ENOMEM;
 971
 972         mt9m111->clk = v4l2_clk_get(&client->dev, "mclk");
 973         if (IS_ERR(mt9m111->clk))
 974                 return PTR_ERR(mt9m111->clk);
 975
 976         /* Default HIGHPOWER context */
 977         mt9m111->ctx = &context_b;
 978
 979         v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
 980         mt9m111->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 981
 982         v4l2_ctrl_handler_init(&mt9m111->hdl, 5);
 983         v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
 984                         V4L2_CID_VFLIP, 0, 1, 1, 0);
 985         v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
 986                         V4L2_CID_HFLIP, 0, 1, 1, 0);
 987         v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
 988                         V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
 989         mt9m111->gain = v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
 990                         V4L2_CID_GAIN, 0, 63 * 2 * 2, 1, 32);
 991         v4l2_ctrl_new_std_menu(&mt9m111->hdl,
 992                         &mt9m111_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
 993                         V4L2_EXPOSURE_AUTO);
 994         v4l2_ctrl_new_std_menu_items(&mt9m111->hdl,
 995                         &mt9m111_ctrl_ops, V4L2_CID_TEST_PATTERN,
 996                         ARRAY_SIZE(mt9m111_test_pattern_menu) - 1, 0, 0,
 997                         mt9m111_test_pattern_menu);
 998         mt9m111->subdev.ctrl_handler = &mt9m111->hdl;
 999         if (mt9m111->hdl.error) {
1000                 ret = mt9m111->hdl.error;
1001                 goto out_clkput;
1002         }
1003
1004 #ifdef CONFIG_MEDIA_CONTROLLER
1005         mt9m111->pad.flags = MEDIA_PAD_FL_SOURCE;
1006         mt9m111->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1007         ret = media_entity_pads_init(&mt9m111->subdev.entity, 1, &mt9m111->pad);
1008         if (ret < 0)
1009                 goto out_hdlfree;
1010 #endif
1011
1012         /* Second stage probe - when a capture adapter is there */
1013         mt9m111->rect.left      = MT9M111_MIN_DARK_COLS;
1014         mt9m111->rect.top       = MT9M111_MIN_DARK_ROWS;
1015         mt9m111->rect.width     = MT9M111_MAX_WIDTH;
1016         mt9m111->rect.height    = MT9M111_MAX_HEIGHT;
1017         mt9m111->width          = mt9m111->rect.width;
1018         mt9m111->height         = mt9m111->rect.height;
1019         mt9m111->fmt            = &mt9m111_colour_fmts[0];
1020         mt9m111->lastpage       = -1;
1021         mutex_init(&mt9m111->power_lock);
1022
1023         ret = mt9m111_video_probe(client);
1024         if (ret < 0)
1025                 goto out_entityclean;
1026
1027         mt9m111->subdev.dev = &client->dev;
1028         ret = v4l2_async_register_subdev(&mt9m111->subdev);
1029         if (ret < 0)
1030                 goto out_entityclean;
1031
1032         return 0;
1033
1034 out_entityclean:
1035 #ifdef CONFIG_MEDIA_CONTROLLER
1036         media_entity_cleanup(&mt9m111->subdev.entity);
1037 out_hdlfree:
1038 #endif
1039         v4l2_ctrl_handler_free(&mt9m111->hdl);
1040 out_clkput:
1041         v4l2_clk_put(mt9m111->clk);
1042
1043         return ret;
1044 }
1045
1046 static int mt9m111_remove(struct i2c_client *client)
1047 {
1048         struct mt9m111 *mt9m111 = to_mt9m111(client);
1049
1050         v4l2_async_unregister_subdev(&mt9m111->subdev);
1051         media_entity_cleanup(&mt9m111->subdev.entity);
1052         v4l2_clk_put(mt9m111->clk);
1053         v4l2_ctrl_handler_free(&mt9m111->hdl);
1054
1055         return 0;
1056 }
1057 static const struct of_device_id mt9m111_of_match[] = {
1058         { .compatible = "micron,mt9m111", },
1059         {},
1060 };
1061 MODULE_DEVICE_TABLE(of, mt9m111_of_match);
1062
1063 static const struct i2c_device_id mt9m111_id[] = {
1064         { "mt9m111", 0 },
1065         { }
1066 };
1067 MODULE_DEVICE_TABLE(i2c, mt9m111_id);
1068
1069 static struct i2c_driver mt9m111_i2c_driver = {
1070         .driver = {
1071                 .name = "mt9m111",
1072                 .of_match_table = of_match_ptr(mt9m111_of_match),
1073         },
1074         .probe          = mt9m111_probe,
1075         .remove         = mt9m111_remove,
1076         .id_table       = mt9m111_id,
1077 };
1078
1079 module_i2c_driver(mt9m111_i2c_driver);
1080
1081 MODULE_DESCRIPTION("Micron/Aptina MT9M111/MT9M112/MT9M131 Camera driver");
1082 MODULE_AUTHOR("Robert Jarzmik");
1083 MODULE_LICENSE("GPL");