Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * Driver for MT9M111/MT9M112/MT9M131 CMOS Image Sensor from Micron/Aptina
0004  *
0005  * Copyright (C) 2008, Robert Jarzmik <robert.jarzmik@free.fr>
0006  */
0007 #include <linux/clk.h>
0008 #include <linux/videodev2.h>
0009 #include <linux/slab.h>
0010 #include <linux/i2c.h>
0011 #include <linux/log2.h>
0012 #include <linux/delay.h>
0013 #include <linux/regulator/consumer.h>
0014 #include <linux/v4l2-mediabus.h>
0015 #include <linux/module.h>
0016 #include <linux/property.h>
0017 
0018 #include <media/v4l2-async.h>
0019 #include <media/v4l2-common.h>
0020 #include <media/v4l2-ctrls.h>
0021 #include <media/v4l2-device.h>
0022 #include <media/v4l2-event.h>
0023 #include <media/v4l2-fwnode.h>
0024 
0025 /*
0026  * MT9M111, MT9M112 and MT9M131:
0027  * i2c address is 0x48 or 0x5d (depending on SADDR pin)
0028  * The platform has to define struct i2c_board_info objects and link to them
0029  * from struct soc_camera_host_desc
0030  */
0031 
0032 /*
0033  * Sensor core register addresses (0x000..0x0ff)
0034  */
0035 #define MT9M111_CHIP_VERSION        0x000
0036 #define MT9M111_ROW_START       0x001
0037 #define MT9M111_COLUMN_START        0x002
0038 #define MT9M111_WINDOW_HEIGHT       0x003
0039 #define MT9M111_WINDOW_WIDTH        0x004
0040 #define MT9M111_HORIZONTAL_BLANKING_B   0x005
0041 #define MT9M111_VERTICAL_BLANKING_B 0x006
0042 #define MT9M111_HORIZONTAL_BLANKING_A   0x007
0043 #define MT9M111_VERTICAL_BLANKING_A 0x008
0044 #define MT9M111_SHUTTER_WIDTH       0x009
0045 #define MT9M111_ROW_SPEED       0x00a
0046 #define MT9M111_EXTRA_DELAY     0x00b
0047 #define MT9M111_SHUTTER_DELAY       0x00c
0048 #define MT9M111_RESET           0x00d
0049 #define MT9M111_READ_MODE_B     0x020
0050 #define MT9M111_READ_MODE_A     0x021
0051 #define MT9M111_FLASH_CONTROL       0x023
0052 #define MT9M111_GREEN1_GAIN     0x02b
0053 #define MT9M111_BLUE_GAIN       0x02c
0054 #define MT9M111_RED_GAIN        0x02d
0055 #define MT9M111_GREEN2_GAIN     0x02e
0056 #define MT9M111_GLOBAL_GAIN     0x02f
0057 #define MT9M111_CONTEXT_CONTROL     0x0c8
0058 #define MT9M111_PAGE_MAP        0x0f0
0059 #define MT9M111_BYTE_WISE_ADDR      0x0f1
0060 
0061 #define MT9M111_RESET_SYNC_CHANGES  (1 << 15)
0062 #define MT9M111_RESET_RESTART_BAD_FRAME (1 << 9)
0063 #define MT9M111_RESET_SHOW_BAD_FRAMES   (1 << 8)
0064 #define MT9M111_RESET_RESET_SOC     (1 << 5)
0065 #define MT9M111_RESET_OUTPUT_DISABLE    (1 << 4)
0066 #define MT9M111_RESET_CHIP_ENABLE   (1 << 3)
0067 #define MT9M111_RESET_ANALOG_STANDBY    (1 << 2)
0068 #define MT9M111_RESET_RESTART_FRAME (1 << 1)
0069 #define MT9M111_RESET_RESET_MODE    (1 << 0)
0070 
0071 #define MT9M111_RM_FULL_POWER_RD    (0 << 10)
0072 #define MT9M111_RM_LOW_POWER_RD     (1 << 10)
0073 #define MT9M111_RM_COL_SKIP_4X      (1 << 5)
0074 #define MT9M111_RM_ROW_SKIP_4X      (1 << 4)
0075 #define MT9M111_RM_COL_SKIP_2X      (1 << 3)
0076 #define MT9M111_RM_ROW_SKIP_2X      (1 << 2)
0077 #define MT9M111_RMB_MIRROR_COLS     (1 << 1)
0078 #define MT9M111_RMB_MIRROR_ROWS     (1 << 0)
0079 #define MT9M111_CTXT_CTRL_RESTART   (1 << 15)
0080 #define MT9M111_CTXT_CTRL_DEFECTCOR_B   (1 << 12)
0081 #define MT9M111_CTXT_CTRL_RESIZE_B  (1 << 10)
0082 #define MT9M111_CTXT_CTRL_CTRL2_B   (1 << 9)
0083 #define MT9M111_CTXT_CTRL_GAMMA_B   (1 << 8)
0084 #define MT9M111_CTXT_CTRL_XENON_EN  (1 << 7)
0085 #define MT9M111_CTXT_CTRL_READ_MODE_B   (1 << 3)
0086 #define MT9M111_CTXT_CTRL_LED_FLASH_EN  (1 << 2)
0087 #define MT9M111_CTXT_CTRL_VBLANK_SEL_B  (1 << 1)
0088 #define MT9M111_CTXT_CTRL_HBLANK_SEL_B  (1 << 0)
0089 
0090 /*
0091  * Colorpipe register addresses (0x100..0x1ff)
0092  */
0093 #define MT9M111_OPER_MODE_CTRL      0x106
0094 #define MT9M111_OUTPUT_FORMAT_CTRL  0x108
0095 #define MT9M111_TPG_CTRL        0x148
0096 #define MT9M111_REDUCER_XZOOM_B     0x1a0
0097 #define MT9M111_REDUCER_XSIZE_B     0x1a1
0098 #define MT9M111_REDUCER_YZOOM_B     0x1a3
0099 #define MT9M111_REDUCER_YSIZE_B     0x1a4
0100 #define MT9M111_REDUCER_XZOOM_A     0x1a6
0101 #define MT9M111_REDUCER_XSIZE_A     0x1a7
0102 #define MT9M111_REDUCER_YZOOM_A     0x1a9
0103 #define MT9M111_REDUCER_YSIZE_A     0x1aa
0104 #define MT9M111_EFFECTS_MODE        0x1e2
0105 
0106 #define MT9M111_OUTPUT_FORMAT_CTRL2_A   0x13a
0107 #define MT9M111_OUTPUT_FORMAT_CTRL2_B   0x19b
0108 
0109 #define MT9M111_OPMODE_AUTOEXPO_EN  (1 << 14)
0110 #define MT9M111_OPMODE_AUTOWHITEBAL_EN  (1 << 1)
0111 #define MT9M111_OUTFMT_FLIP_BAYER_COL   (1 << 9)
0112 #define MT9M111_OUTFMT_FLIP_BAYER_ROW   (1 << 8)
0113 #define MT9M111_OUTFMT_PROCESSED_BAYER  (1 << 14)
0114 #define MT9M111_OUTFMT_BYPASS_IFP   (1 << 10)
0115 #define MT9M111_OUTFMT_INV_PIX_CLOCK    (1 << 9)
0116 #define MT9M111_OUTFMT_RGB      (1 << 8)
0117 #define MT9M111_OUTFMT_RGB565       (0 << 6)
0118 #define MT9M111_OUTFMT_RGB555       (1 << 6)
0119 #define MT9M111_OUTFMT_RGB444x      (2 << 6)
0120 #define MT9M111_OUTFMT_RGBx444      (3 << 6)
0121 #define MT9M111_OUTFMT_TST_RAMP_OFF (0 << 4)
0122 #define MT9M111_OUTFMT_TST_RAMP_COL (1 << 4)
0123 #define MT9M111_OUTFMT_TST_RAMP_ROW (2 << 4)
0124 #define MT9M111_OUTFMT_TST_RAMP_FRAME   (3 << 4)
0125 #define MT9M111_OUTFMT_SHIFT_3_UP   (1 << 3)
0126 #define MT9M111_OUTFMT_AVG_CHROMA   (1 << 2)
0127 #define MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN  (1 << 1)
0128 #define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B (1 << 0)
0129 #define MT9M111_TPG_SEL_MASK        GENMASK(2, 0)
0130 #define MT9M111_EFFECTS_MODE_MASK   GENMASK(2, 0)
0131 #define MT9M111_RM_PWR_MASK     BIT(10)
0132 #define MT9M111_RM_SKIP2_MASK       GENMASK(3, 2)
0133 
0134 /*
0135  * Camera control register addresses (0x200..0x2ff not implemented)
0136  */
0137 
0138 #define reg_read(reg) mt9m111_reg_read(client, MT9M111_##reg)
0139 #define reg_write(reg, val) mt9m111_reg_write(client, MT9M111_##reg, (val))
0140 #define reg_set(reg, val) mt9m111_reg_set(client, MT9M111_##reg, (val))
0141 #define reg_clear(reg, val) mt9m111_reg_clear(client, MT9M111_##reg, (val))
0142 #define reg_mask(reg, val, mask) mt9m111_reg_mask(client, MT9M111_##reg, \
0143         (val), (mask))
0144 
0145 #define MT9M111_MIN_DARK_ROWS   8
0146 #define MT9M111_MIN_DARK_COLS   26
0147 #define MT9M111_MAX_HEIGHT  1024
0148 #define MT9M111_MAX_WIDTH   1280
0149 
0150 struct mt9m111_context {
0151     u16 read_mode;
0152     u16 blanking_h;
0153     u16 blanking_v;
0154     u16 reducer_xzoom;
0155     u16 reducer_yzoom;
0156     u16 reducer_xsize;
0157     u16 reducer_ysize;
0158     u16 output_fmt_ctrl2;
0159     u16 control;
0160 };
0161 
0162 static struct mt9m111_context context_a = {
0163     .read_mode      = MT9M111_READ_MODE_A,
0164     .blanking_h     = MT9M111_HORIZONTAL_BLANKING_A,
0165     .blanking_v     = MT9M111_VERTICAL_BLANKING_A,
0166     .reducer_xzoom      = MT9M111_REDUCER_XZOOM_A,
0167     .reducer_yzoom      = MT9M111_REDUCER_YZOOM_A,
0168     .reducer_xsize      = MT9M111_REDUCER_XSIZE_A,
0169     .reducer_ysize      = MT9M111_REDUCER_YSIZE_A,
0170     .output_fmt_ctrl2   = MT9M111_OUTPUT_FORMAT_CTRL2_A,
0171     .control        = MT9M111_CTXT_CTRL_RESTART,
0172 };
0173 
0174 static struct mt9m111_context context_b = {
0175     .read_mode      = MT9M111_READ_MODE_B,
0176     .blanking_h     = MT9M111_HORIZONTAL_BLANKING_B,
0177     .blanking_v     = MT9M111_VERTICAL_BLANKING_B,
0178     .reducer_xzoom      = MT9M111_REDUCER_XZOOM_B,
0179     .reducer_yzoom      = MT9M111_REDUCER_YZOOM_B,
0180     .reducer_xsize      = MT9M111_REDUCER_XSIZE_B,
0181     .reducer_ysize      = MT9M111_REDUCER_YSIZE_B,
0182     .output_fmt_ctrl2   = MT9M111_OUTPUT_FORMAT_CTRL2_B,
0183     .control        = MT9M111_CTXT_CTRL_RESTART |
0184         MT9M111_CTXT_CTRL_DEFECTCOR_B | MT9M111_CTXT_CTRL_RESIZE_B |
0185         MT9M111_CTXT_CTRL_CTRL2_B | MT9M111_CTXT_CTRL_GAMMA_B |
0186         MT9M111_CTXT_CTRL_READ_MODE_B | MT9M111_CTXT_CTRL_VBLANK_SEL_B |
0187         MT9M111_CTXT_CTRL_HBLANK_SEL_B,
0188 };
0189 
0190 /* MT9M111 has only one fixed colorspace per pixelcode */
0191 struct mt9m111_datafmt {
0192     u32 code;
0193     enum v4l2_colorspace        colorspace;
0194 };
0195 
0196 static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
0197     {MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB},
0198     {MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_SRGB},
0199     {MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB},
0200     {MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_SRGB},
0201     {MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
0202     {MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
0203     {MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
0204     {MEDIA_BUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
0205     {MEDIA_BUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB},
0206     {MEDIA_BUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB},
0207     {MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
0208     {MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
0209 };
0210 
0211 enum mt9m111_mode_id {
0212     MT9M111_MODE_SXGA_8FPS,
0213     MT9M111_MODE_SXGA_15FPS,
0214     MT9M111_MODE_QSXGA_30FPS,
0215     MT9M111_NUM_MODES,
0216 };
0217 
0218 struct mt9m111_mode_info {
0219     unsigned int sensor_w;
0220     unsigned int sensor_h;
0221     unsigned int max_image_w;
0222     unsigned int max_image_h;
0223     unsigned int max_fps;
0224     unsigned int reg_val;
0225     unsigned int reg_mask;
0226 };
0227 
0228 struct mt9m111 {
0229     struct v4l2_subdev subdev;
0230     struct v4l2_ctrl_handler hdl;
0231     struct v4l2_ctrl *gain;
0232     struct mt9m111_context *ctx;
0233     struct v4l2_rect rect;  /* cropping rectangle */
0234     struct clk *clk;
0235     unsigned int width; /* output */
0236     unsigned int height;    /* sizes */
0237     struct v4l2_fract frame_interval;
0238     const struct mt9m111_mode_info *current_mode;
0239     struct mutex power_lock; /* lock to protect power_count */
0240     int power_count;
0241     const struct mt9m111_datafmt *fmt;
0242     int lastpage;   /* PageMap cache value */
0243     struct regulator *regulator;
0244     bool is_streaming;
0245     /* user point of view - 0: falling 1: rising edge */
0246     unsigned int pclk_sample:1;
0247 #ifdef CONFIG_MEDIA_CONTROLLER
0248     struct media_pad pad;
0249 #endif
0250 };
0251 
0252 static const struct mt9m111_mode_info mt9m111_mode_data[MT9M111_NUM_MODES] = {
0253     [MT9M111_MODE_SXGA_8FPS] = {
0254         .sensor_w = 1280,
0255         .sensor_h = 1024,
0256         .max_image_w = 1280,
0257         .max_image_h = 1024,
0258         .max_fps = 8,
0259         .reg_val = MT9M111_RM_LOW_POWER_RD,
0260         .reg_mask = MT9M111_RM_PWR_MASK | MT9M111_RM_SKIP2_MASK,
0261     },
0262     [MT9M111_MODE_SXGA_15FPS] = {
0263         .sensor_w = 1280,
0264         .sensor_h = 1024,
0265         .max_image_w = 1280,
0266         .max_image_h = 1024,
0267         .max_fps = 15,
0268         .reg_val = MT9M111_RM_FULL_POWER_RD,
0269         .reg_mask = MT9M111_RM_PWR_MASK | MT9M111_RM_SKIP2_MASK,
0270     },
0271     [MT9M111_MODE_QSXGA_30FPS] = {
0272         .sensor_w = 1280,
0273         .sensor_h = 1024,
0274         .max_image_w = 640,
0275         .max_image_h = 512,
0276         .max_fps = 30,
0277         .reg_val = MT9M111_RM_LOW_POWER_RD | MT9M111_RM_COL_SKIP_2X |
0278                MT9M111_RM_ROW_SKIP_2X,
0279         .reg_mask = MT9M111_RM_PWR_MASK | MT9M111_RM_SKIP2_MASK,
0280     },
0281 };
0282 
0283 /* Find a data format by a pixel code */
0284 static const struct mt9m111_datafmt *mt9m111_find_datafmt(struct mt9m111 *mt9m111,
0285                         u32 code)
0286 {
0287     int i;
0288     for (i = 0; i < ARRAY_SIZE(mt9m111_colour_fmts); i++)
0289         if (mt9m111_colour_fmts[i].code == code)
0290             return mt9m111_colour_fmts + i;
0291 
0292     return mt9m111->fmt;
0293 }
0294 
0295 static struct mt9m111 *to_mt9m111(const struct i2c_client *client)
0296 {
0297     return container_of(i2c_get_clientdata(client), struct mt9m111, subdev);
0298 }
0299 
0300 static int reg_page_map_set(struct i2c_client *client, const u16 reg)
0301 {
0302     int ret;
0303     u16 page;
0304     struct mt9m111 *mt9m111 = to_mt9m111(client);
0305 
0306     page = (reg >> 8);
0307     if (page == mt9m111->lastpage)
0308         return 0;
0309     if (page > 2)
0310         return -EINVAL;
0311 
0312     ret = i2c_smbus_write_word_swapped(client, MT9M111_PAGE_MAP, page);
0313     if (!ret)
0314         mt9m111->lastpage = page;
0315     return ret;
0316 }
0317 
0318 static int mt9m111_reg_read(struct i2c_client *client, const u16 reg)
0319 {
0320     int ret;
0321 
0322     ret = reg_page_map_set(client, reg);
0323     if (!ret)
0324         ret = i2c_smbus_read_word_swapped(client, reg & 0xff);
0325 
0326     dev_dbg(&client->dev, "read  reg.%03x -> %04x\n", reg, ret);
0327     return ret;
0328 }
0329 
0330 static int mt9m111_reg_write(struct i2c_client *client, const u16 reg,
0331                  const u16 data)
0332 {
0333     int ret;
0334 
0335     ret = reg_page_map_set(client, reg);
0336     if (!ret)
0337         ret = i2c_smbus_write_word_swapped(client, reg & 0xff, data);
0338     dev_dbg(&client->dev, "write reg.%03x = %04x -> %d\n", reg, data, ret);
0339     return ret;
0340 }
0341 
0342 static int mt9m111_reg_set(struct i2c_client *client, const u16 reg,
0343                const u16 data)
0344 {
0345     int ret;
0346 
0347     ret = mt9m111_reg_read(client, reg);
0348     if (ret >= 0)
0349         ret = mt9m111_reg_write(client, reg, ret | data);
0350     return ret;
0351 }
0352 
0353 static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg,
0354                  const u16 data)
0355 {
0356     int ret;
0357 
0358     ret = mt9m111_reg_read(client, reg);
0359     if (ret >= 0)
0360         ret = mt9m111_reg_write(client, reg, ret & ~data);
0361     return ret;
0362 }
0363 
0364 static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg,
0365                 const u16 data, const u16 mask)
0366 {
0367     int ret;
0368 
0369     ret = mt9m111_reg_read(client, reg);
0370     if (ret >= 0)
0371         ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data);
0372     return ret;
0373 }
0374 
0375 static int mt9m111_set_context(struct mt9m111 *mt9m111,
0376                    struct mt9m111_context *ctx)
0377 {
0378     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0379     return reg_write(CONTEXT_CONTROL, ctx->control);
0380 }
0381 
0382 static int mt9m111_setup_rect_ctx(struct mt9m111 *mt9m111,
0383             struct mt9m111_context *ctx, struct v4l2_rect *rect,
0384             unsigned int width, unsigned int height)
0385 {
0386     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0387     int ret = mt9m111_reg_write(client, ctx->reducer_xzoom, rect->width);
0388     if (!ret)
0389         ret = mt9m111_reg_write(client, ctx->reducer_yzoom, rect->height);
0390     if (!ret)
0391         ret = mt9m111_reg_write(client, ctx->reducer_xsize, width);
0392     if (!ret)
0393         ret = mt9m111_reg_write(client, ctx->reducer_ysize, height);
0394     return ret;
0395 }
0396 
0397 static int mt9m111_setup_geometry(struct mt9m111 *mt9m111, struct v4l2_rect *rect,
0398             int width, int height, u32 code)
0399 {
0400     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0401     int ret;
0402 
0403     ret = reg_write(COLUMN_START, rect->left);
0404     if (!ret)
0405         ret = reg_write(ROW_START, rect->top);
0406 
0407     if (!ret)
0408         ret = reg_write(WINDOW_WIDTH, rect->width);
0409     if (!ret)
0410         ret = reg_write(WINDOW_HEIGHT, rect->height);
0411 
0412     if (code != MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
0413         /* IFP in use, down-scaling possible */
0414         if (!ret)
0415             ret = mt9m111_setup_rect_ctx(mt9m111, &context_b,
0416                              rect, width, height);
0417         if (!ret)
0418             ret = mt9m111_setup_rect_ctx(mt9m111, &context_a,
0419                              rect, width, height);
0420     }
0421 
0422     dev_dbg(&client->dev, "%s(%x): %ux%u@%u:%u -> %ux%u = %d\n",
0423         __func__, code, rect->width, rect->height, rect->left, rect->top,
0424         width, height, ret);
0425 
0426     return ret;
0427 }
0428 
0429 static int mt9m111_enable(struct mt9m111 *mt9m111)
0430 {
0431     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0432     return reg_write(RESET, MT9M111_RESET_CHIP_ENABLE);
0433 }
0434 
0435 static int mt9m111_reset(struct mt9m111 *mt9m111)
0436 {
0437     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0438     int ret;
0439 
0440     ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
0441     if (!ret)
0442         ret = reg_set(RESET, MT9M111_RESET_RESET_SOC);
0443     if (!ret)
0444         ret = reg_clear(RESET, MT9M111_RESET_RESET_MODE
0445                 | MT9M111_RESET_RESET_SOC);
0446 
0447     return ret;
0448 }
0449 
0450 static int mt9m111_set_selection(struct v4l2_subdev *sd,
0451                  struct v4l2_subdev_state *sd_state,
0452                  struct v4l2_subdev_selection *sel)
0453 {
0454     struct i2c_client *client = v4l2_get_subdevdata(sd);
0455     struct mt9m111 *mt9m111 = to_mt9m111(client);
0456     struct v4l2_rect rect = sel->r;
0457     int width, height;
0458     int ret, align = 0;
0459 
0460     if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
0461         sel->target != V4L2_SEL_TGT_CROP)
0462         return -EINVAL;
0463 
0464     if (mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
0465         mt9m111->fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
0466         /* Bayer format - even size lengths */
0467         align = 1;
0468         /* Let the user play with the starting pixel */
0469     }
0470 
0471     /* FIXME: the datasheet doesn't specify minimum sizes */
0472     v4l_bound_align_image(&rect.width, 2, MT9M111_MAX_WIDTH, align,
0473                   &rect.height, 2, MT9M111_MAX_HEIGHT, align, 0);
0474     rect.left = clamp(rect.left, MT9M111_MIN_DARK_COLS,
0475               MT9M111_MIN_DARK_COLS + MT9M111_MAX_WIDTH -
0476               (__s32)rect.width);
0477     rect.top = clamp(rect.top, MT9M111_MIN_DARK_ROWS,
0478              MT9M111_MIN_DARK_ROWS + MT9M111_MAX_HEIGHT -
0479              (__s32)rect.height);
0480 
0481     width = min(mt9m111->width, rect.width);
0482     height = min(mt9m111->height, rect.height);
0483 
0484     ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
0485     if (!ret) {
0486         mt9m111->rect = rect;
0487         mt9m111->width = width;
0488         mt9m111->height = height;
0489     }
0490 
0491     return ret;
0492 }
0493 
0494 static int mt9m111_get_selection(struct v4l2_subdev *sd,
0495                  struct v4l2_subdev_state *sd_state,
0496                  struct v4l2_subdev_selection *sel)
0497 {
0498     struct i2c_client *client = v4l2_get_subdevdata(sd);
0499     struct mt9m111 *mt9m111 = to_mt9m111(client);
0500 
0501     if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
0502         return -EINVAL;
0503 
0504     switch (sel->target) {
0505     case V4L2_SEL_TGT_CROP_BOUNDS:
0506         sel->r.left = MT9M111_MIN_DARK_COLS;
0507         sel->r.top = MT9M111_MIN_DARK_ROWS;
0508         sel->r.width = MT9M111_MAX_WIDTH;
0509         sel->r.height = MT9M111_MAX_HEIGHT;
0510         return 0;
0511     case V4L2_SEL_TGT_CROP:
0512         sel->r = mt9m111->rect;
0513         return 0;
0514     default:
0515         return -EINVAL;
0516     }
0517 }
0518 
0519 static int mt9m111_get_fmt(struct v4l2_subdev *sd,
0520         struct v4l2_subdev_state *sd_state,
0521         struct v4l2_subdev_format *format)
0522 {
0523     struct v4l2_mbus_framefmt *mf = &format->format;
0524     struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
0525 
0526     if (format->pad)
0527         return -EINVAL;
0528 
0529     if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
0530 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
0531         mf = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
0532         format->format = *mf;
0533         return 0;
0534 #else
0535         return -EINVAL;
0536 #endif
0537     }
0538 
0539     mf->width   = mt9m111->width;
0540     mf->height  = mt9m111->height;
0541     mf->code    = mt9m111->fmt->code;
0542     mf->colorspace  = mt9m111->fmt->colorspace;
0543     mf->field   = V4L2_FIELD_NONE;
0544     mf->ycbcr_enc   = V4L2_YCBCR_ENC_DEFAULT;
0545     mf->quantization    = V4L2_QUANTIZATION_DEFAULT;
0546     mf->xfer_func   = V4L2_XFER_FUNC_DEFAULT;
0547 
0548     return 0;
0549 }
0550 
0551 static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
0552                   u32 code)
0553 {
0554     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0555     u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
0556         MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB |
0557         MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
0558         MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 |
0559         MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
0560         MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
0561     int ret;
0562 
0563     switch (code) {
0564     case MEDIA_BUS_FMT_SBGGR8_1X8:
0565         data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
0566             MT9M111_OUTFMT_RGB;
0567         break;
0568     case MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE:
0569         data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
0570         break;
0571     case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
0572         data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
0573             MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
0574         break;
0575     case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
0576         data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
0577         break;
0578     case MEDIA_BUS_FMT_RGB565_2X8_LE:
0579         data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
0580             MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
0581         break;
0582     case MEDIA_BUS_FMT_RGB565_2X8_BE:
0583         data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
0584         break;
0585     case MEDIA_BUS_FMT_BGR565_2X8_BE:
0586         data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
0587             MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
0588         break;
0589     case MEDIA_BUS_FMT_BGR565_2X8_LE:
0590         data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
0591             MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
0592             MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
0593         break;
0594     case MEDIA_BUS_FMT_UYVY8_2X8:
0595         data_outfmt2 = 0;
0596         break;
0597     case MEDIA_BUS_FMT_VYUY8_2X8:
0598         data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
0599         break;
0600     case MEDIA_BUS_FMT_YUYV8_2X8:
0601         data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
0602         break;
0603     case MEDIA_BUS_FMT_YVYU8_2X8:
0604         data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
0605             MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
0606         break;
0607     default:
0608         dev_err(&client->dev, "Pixel format not handled: %x\n", code);
0609         return -EINVAL;
0610     }
0611 
0612     /* receiver samples on falling edge, chip-hw default is rising */
0613     if (mt9m111->pclk_sample == 0)
0614         mask_outfmt2 |= MT9M111_OUTFMT_INV_PIX_CLOCK;
0615 
0616     ret = mt9m111_reg_mask(client, context_a.output_fmt_ctrl2,
0617                    data_outfmt2, mask_outfmt2);
0618     if (!ret)
0619         ret = mt9m111_reg_mask(client, context_b.output_fmt_ctrl2,
0620                        data_outfmt2, mask_outfmt2);
0621 
0622     return ret;
0623 }
0624 
0625 static int mt9m111_set_fmt(struct v4l2_subdev *sd,
0626         struct v4l2_subdev_state *sd_state,
0627         struct v4l2_subdev_format *format)
0628 {
0629     struct v4l2_mbus_framefmt *mf = &format->format;
0630     struct i2c_client *client = v4l2_get_subdevdata(sd);
0631     struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
0632     const struct mt9m111_datafmt *fmt;
0633     struct v4l2_rect *rect = &mt9m111->rect;
0634     bool bayer;
0635     int ret;
0636 
0637     if (mt9m111->is_streaming)
0638         return -EBUSY;
0639 
0640     if (format->pad)
0641         return -EINVAL;
0642 
0643     fmt = mt9m111_find_datafmt(mt9m111, mf->code);
0644 
0645     bayer = fmt->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
0646         fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE;
0647 
0648     /*
0649      * With Bayer format enforce even side lengths, but let the user play
0650      * with the starting pixel
0651      */
0652     if (bayer) {
0653         rect->width = ALIGN(rect->width, 2);
0654         rect->height = ALIGN(rect->height, 2);
0655     }
0656 
0657     if (fmt->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) {
0658         /* IFP bypass mode, no scaling */
0659         mf->width = rect->width;
0660         mf->height = rect->height;
0661     } else {
0662         /* No upscaling */
0663         if (mf->width > rect->width)
0664             mf->width = rect->width;
0665         if (mf->height > rect->height)
0666             mf->height = rect->height;
0667     }
0668 
0669     dev_dbg(&client->dev, "%s(): %ux%u, code=%x\n", __func__,
0670         mf->width, mf->height, fmt->code);
0671 
0672     mf->code = fmt->code;
0673     mf->colorspace = fmt->colorspace;
0674     mf->field   = V4L2_FIELD_NONE;
0675     mf->ycbcr_enc   = V4L2_YCBCR_ENC_DEFAULT;
0676     mf->quantization    = V4L2_QUANTIZATION_DEFAULT;
0677     mf->xfer_func   = V4L2_XFER_FUNC_DEFAULT;
0678 
0679     if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
0680         sd_state->pads->try_fmt = *mf;
0681         return 0;
0682     }
0683 
0684     ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
0685     if (!ret)
0686         ret = mt9m111_set_pixfmt(mt9m111, mf->code);
0687     if (!ret) {
0688         mt9m111->width  = mf->width;
0689         mt9m111->height = mf->height;
0690         mt9m111->fmt    = fmt;
0691     }
0692 
0693     return ret;
0694 }
0695 
0696 static const struct mt9m111_mode_info *
0697 mt9m111_find_mode(struct mt9m111 *mt9m111, unsigned int req_fps,
0698           unsigned int width, unsigned int height)
0699 {
0700     const struct mt9m111_mode_info *mode;
0701     struct v4l2_rect *sensor_rect = &mt9m111->rect;
0702     unsigned int gap, gap_best = (unsigned int) -1;
0703     int i, best_gap_idx = MT9M111_MODE_SXGA_15FPS;
0704     bool skip_30fps = false;
0705 
0706     /*
0707      * The fps selection is based on the row, column skipping mechanism.
0708      * So ensure that the sensor window is set to default else the fps
0709      * aren't calculated correctly within the sensor hw.
0710      */
0711     if (sensor_rect->width != MT9M111_MAX_WIDTH ||
0712         sensor_rect->height != MT9M111_MAX_HEIGHT) {
0713         dev_info(mt9m111->subdev.dev,
0714              "Framerate selection is not supported for cropped "
0715              "images\n");
0716         return NULL;
0717     }
0718 
0719     /* 30fps only supported for images not exceeding 640x512 */
0720     if (width > MT9M111_MAX_WIDTH / 2 || height > MT9M111_MAX_HEIGHT / 2) {
0721         dev_dbg(mt9m111->subdev.dev,
0722             "Framerates > 15fps are supported only for images "
0723             "not exceeding 640x512\n");
0724         skip_30fps = true;
0725     }
0726 
0727     /* find best matched fps */
0728     for (i = 0; i < MT9M111_NUM_MODES; i++) {
0729         unsigned int fps = mt9m111_mode_data[i].max_fps;
0730 
0731         if (fps == 30 && skip_30fps)
0732             continue;
0733 
0734         gap = abs(fps - req_fps);
0735         if (gap < gap_best) {
0736             best_gap_idx = i;
0737             gap_best = gap;
0738         }
0739     }
0740 
0741     /*
0742      * Use context a/b default timing values instead of calculate blanking
0743      * timing values.
0744      */
0745     mode = &mt9m111_mode_data[best_gap_idx];
0746     mt9m111->ctx = (best_gap_idx == MT9M111_MODE_QSXGA_30FPS) ? &context_a :
0747                                     &context_b;
0748     return mode;
0749 }
0750 
0751 #ifdef CONFIG_VIDEO_ADV_DEBUG
0752 static int mt9m111_g_register(struct v4l2_subdev *sd,
0753                   struct v4l2_dbg_register *reg)
0754 {
0755     struct i2c_client *client = v4l2_get_subdevdata(sd);
0756     int val;
0757 
0758     if (reg->reg > 0x2ff)
0759         return -EINVAL;
0760 
0761     val = mt9m111_reg_read(client, reg->reg);
0762     reg->size = 2;
0763     reg->val = (u64)val;
0764 
0765     if (reg->val > 0xffff)
0766         return -EIO;
0767 
0768     return 0;
0769 }
0770 
0771 static int mt9m111_s_register(struct v4l2_subdev *sd,
0772                   const struct v4l2_dbg_register *reg)
0773 {
0774     struct i2c_client *client = v4l2_get_subdevdata(sd);
0775 
0776     if (reg->reg > 0x2ff)
0777         return -EINVAL;
0778 
0779     if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
0780         return -EIO;
0781 
0782     return 0;
0783 }
0784 #endif
0785 
0786 static int mt9m111_set_flip(struct mt9m111 *mt9m111, int flip, int mask)
0787 {
0788     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0789     int ret;
0790 
0791     if (flip)
0792         ret = mt9m111_reg_set(client, mt9m111->ctx->read_mode, mask);
0793     else
0794         ret = mt9m111_reg_clear(client, mt9m111->ctx->read_mode, mask);
0795 
0796     return ret;
0797 }
0798 
0799 static int mt9m111_get_global_gain(struct mt9m111 *mt9m111)
0800 {
0801     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0802     int data;
0803 
0804     data = reg_read(GLOBAL_GAIN);
0805     if (data >= 0)
0806         return (data & 0x2f) * (1 << ((data >> 10) & 1)) *
0807             (1 << ((data >> 9) & 1));
0808     return data;
0809 }
0810 
0811 static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
0812 {
0813     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0814     u16 val;
0815 
0816     if (gain > 63 * 2 * 2)
0817         return -EINVAL;
0818 
0819     if ((gain >= 64 * 2) && (gain < 63 * 2 * 2))
0820         val = (1 << 10) | (1 << 9) | (gain / 4);
0821     else if ((gain >= 64) && (gain < 64 * 2))
0822         val = (1 << 9) | (gain / 2);
0823     else
0824         val = gain;
0825 
0826     return reg_write(GLOBAL_GAIN, val);
0827 }
0828 
0829 static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int val)
0830 {
0831     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0832 
0833     if (val == V4L2_EXPOSURE_AUTO)
0834         return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
0835     return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
0836 }
0837 
0838 static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on)
0839 {
0840     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0841 
0842     if (on)
0843         return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
0844     return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
0845 }
0846 
0847 static const char * const mt9m111_test_pattern_menu[] = {
0848     "Disabled",
0849     "Vertical monochrome gradient",
0850     "Flat color type 1",
0851     "Flat color type 2",
0852     "Flat color type 3",
0853     "Flat color type 4",
0854     "Flat color type 5",
0855     "Color bar",
0856 };
0857 
0858 static int mt9m111_set_test_pattern(struct mt9m111 *mt9m111, int val)
0859 {
0860     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0861 
0862     return mt9m111_reg_mask(client, MT9M111_TPG_CTRL, val,
0863                 MT9M111_TPG_SEL_MASK);
0864 }
0865 
0866 static int mt9m111_set_colorfx(struct mt9m111 *mt9m111, int val)
0867 {
0868     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0869     static const struct v4l2_control colorfx[] = {
0870         { V4L2_COLORFX_NONE,        0 },
0871         { V4L2_COLORFX_BW,      1 },
0872         { V4L2_COLORFX_SEPIA,       2 },
0873         { V4L2_COLORFX_NEGATIVE,    3 },
0874         { V4L2_COLORFX_SOLARIZATION,    4 },
0875     };
0876     int i;
0877 
0878     for (i = 0; i < ARRAY_SIZE(colorfx); i++) {
0879         if (colorfx[i].id == val) {
0880             return mt9m111_reg_mask(client, MT9M111_EFFECTS_MODE,
0881                         colorfx[i].value,
0882                         MT9M111_EFFECTS_MODE_MASK);
0883         }
0884     }
0885 
0886     return -EINVAL;
0887 }
0888 
0889 static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
0890 {
0891     struct mt9m111 *mt9m111 = container_of(ctrl->handler,
0892                            struct mt9m111, hdl);
0893 
0894     switch (ctrl->id) {
0895     case V4L2_CID_VFLIP:
0896         return mt9m111_set_flip(mt9m111, ctrl->val,
0897                     MT9M111_RMB_MIRROR_ROWS);
0898     case V4L2_CID_HFLIP:
0899         return mt9m111_set_flip(mt9m111, ctrl->val,
0900                     MT9M111_RMB_MIRROR_COLS);
0901     case V4L2_CID_GAIN:
0902         return mt9m111_set_global_gain(mt9m111, ctrl->val);
0903     case V4L2_CID_EXPOSURE_AUTO:
0904         return mt9m111_set_autoexposure(mt9m111, ctrl->val);
0905     case V4L2_CID_AUTO_WHITE_BALANCE:
0906         return mt9m111_set_autowhitebalance(mt9m111, ctrl->val);
0907     case V4L2_CID_TEST_PATTERN:
0908         return mt9m111_set_test_pattern(mt9m111, ctrl->val);
0909     case V4L2_CID_COLORFX:
0910         return mt9m111_set_colorfx(mt9m111, ctrl->val);
0911     }
0912 
0913     return -EINVAL;
0914 }
0915 
0916 static int mt9m111_suspend(struct mt9m111 *mt9m111)
0917 {
0918     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0919     int ret;
0920 
0921     v4l2_ctrl_s_ctrl(mt9m111->gain, mt9m111_get_global_gain(mt9m111));
0922 
0923     ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
0924     if (!ret)
0925         ret = reg_set(RESET, MT9M111_RESET_RESET_SOC |
0926                   MT9M111_RESET_OUTPUT_DISABLE |
0927                   MT9M111_RESET_ANALOG_STANDBY);
0928     if (!ret)
0929         ret = reg_clear(RESET, MT9M111_RESET_CHIP_ENABLE);
0930 
0931     return ret;
0932 }
0933 
0934 static void mt9m111_restore_state(struct mt9m111 *mt9m111)
0935 {
0936     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0937 
0938     mt9m111_set_context(mt9m111, mt9m111->ctx);
0939     mt9m111_set_pixfmt(mt9m111, mt9m111->fmt->code);
0940     mt9m111_setup_geometry(mt9m111, &mt9m111->rect,
0941             mt9m111->width, mt9m111->height, mt9m111->fmt->code);
0942     v4l2_ctrl_handler_setup(&mt9m111->hdl);
0943     mt9m111_reg_mask(client, mt9m111->ctx->read_mode,
0944              mt9m111->current_mode->reg_val,
0945              mt9m111->current_mode->reg_mask);
0946 }
0947 
0948 static int mt9m111_resume(struct mt9m111 *mt9m111)
0949 {
0950     int ret = mt9m111_enable(mt9m111);
0951     if (!ret)
0952         ret = mt9m111_reset(mt9m111);
0953     if (!ret)
0954         mt9m111_restore_state(mt9m111);
0955 
0956     return ret;
0957 }
0958 
0959 static int mt9m111_init(struct mt9m111 *mt9m111)
0960 {
0961     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0962     int ret;
0963 
0964     ret = mt9m111_enable(mt9m111);
0965     if (!ret)
0966         ret = mt9m111_reset(mt9m111);
0967     if (!ret)
0968         ret = mt9m111_set_context(mt9m111, mt9m111->ctx);
0969     if (ret)
0970         dev_err(&client->dev, "mt9m111 init failed: %d\n", ret);
0971     return ret;
0972 }
0973 
0974 static int mt9m111_power_on(struct mt9m111 *mt9m111)
0975 {
0976     struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
0977     int ret;
0978 
0979     ret = clk_prepare_enable(mt9m111->clk);
0980     if (ret < 0)
0981         return ret;
0982 
0983     ret = regulator_enable(mt9m111->regulator);
0984     if (ret < 0)
0985         goto out_clk_disable;
0986 
0987     ret = mt9m111_resume(mt9m111);
0988     if (ret < 0)
0989         goto out_regulator_disable;
0990 
0991     return 0;
0992 
0993 out_regulator_disable:
0994     regulator_disable(mt9m111->regulator);
0995 
0996 out_clk_disable:
0997     clk_disable_unprepare(mt9m111->clk);
0998 
0999     dev_err(&client->dev, "Failed to resume the sensor: %d\n", ret);
1000 
1001     return ret;
1002 }
1003 
1004 static void mt9m111_power_off(struct mt9m111 *mt9m111)
1005 {
1006     mt9m111_suspend(mt9m111);
1007     regulator_disable(mt9m111->regulator);
1008     clk_disable_unprepare(mt9m111->clk);
1009 }
1010 
1011 static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
1012 {
1013     struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1014     int ret = 0;
1015 
1016     mutex_lock(&mt9m111->power_lock);
1017 
1018     /*
1019      * If the power count is modified from 0 to != 0 or from != 0 to 0,
1020      * update the power state.
1021      */
1022     if (mt9m111->power_count == !on) {
1023         if (on)
1024             ret = mt9m111_power_on(mt9m111);
1025         else
1026             mt9m111_power_off(mt9m111);
1027     }
1028 
1029     if (!ret) {
1030         /* Update the power count. */
1031         mt9m111->power_count += on ? 1 : -1;
1032         WARN_ON(mt9m111->power_count < 0);
1033     }
1034 
1035     mutex_unlock(&mt9m111->power_lock);
1036     return ret;
1037 }
1038 
1039 static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
1040     .s_ctrl = mt9m111_s_ctrl,
1041 };
1042 
1043 static const struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
1044     .s_power    = mt9m111_s_power,
1045     .log_status = v4l2_ctrl_subdev_log_status,
1046     .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1047     .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1048 #ifdef CONFIG_VIDEO_ADV_DEBUG
1049     .g_register = mt9m111_g_register,
1050     .s_register = mt9m111_s_register,
1051 #endif
1052 };
1053 
1054 static int mt9m111_g_frame_interval(struct v4l2_subdev *sd,
1055                    struct v4l2_subdev_frame_interval *fi)
1056 {
1057     struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1058 
1059     fi->interval = mt9m111->frame_interval;
1060 
1061     return 0;
1062 }
1063 
1064 static int mt9m111_s_frame_interval(struct v4l2_subdev *sd,
1065                    struct v4l2_subdev_frame_interval *fi)
1066 {
1067     struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1068     const struct mt9m111_mode_info *mode;
1069     struct v4l2_fract *fract = &fi->interval;
1070     int fps;
1071 
1072     if (mt9m111->is_streaming)
1073         return -EBUSY;
1074 
1075     if (fi->pad != 0)
1076         return -EINVAL;
1077 
1078     if (fract->numerator == 0) {
1079         fract->denominator = 30;
1080         fract->numerator = 1;
1081     }
1082 
1083     fps = DIV_ROUND_CLOSEST(fract->denominator, fract->numerator);
1084 
1085     /* Find best fitting mode. Do not update the mode if no one was found. */
1086     mode = mt9m111_find_mode(mt9m111, fps, mt9m111->width, mt9m111->height);
1087     if (!mode)
1088         return 0;
1089 
1090     if (mode->max_fps != fps) {
1091         fract->denominator = mode->max_fps;
1092         fract->numerator = 1;
1093     }
1094 
1095     mt9m111->current_mode = mode;
1096     mt9m111->frame_interval = fi->interval;
1097 
1098     return 0;
1099 }
1100 
1101 static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
1102         struct v4l2_subdev_state *sd_state,
1103         struct v4l2_subdev_mbus_code_enum *code)
1104 {
1105     if (code->pad || code->index >= ARRAY_SIZE(mt9m111_colour_fmts))
1106         return -EINVAL;
1107 
1108     code->code = mt9m111_colour_fmts[code->index].code;
1109     return 0;
1110 }
1111 
1112 static int mt9m111_s_stream(struct v4l2_subdev *sd, int enable)
1113 {
1114     struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1115 
1116     mt9m111->is_streaming = !!enable;
1117     return 0;
1118 }
1119 
1120 static int mt9m111_init_cfg(struct v4l2_subdev *sd,
1121                 struct v4l2_subdev_state *sd_state)
1122 {
1123 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
1124     struct v4l2_mbus_framefmt *format =
1125         v4l2_subdev_get_try_format(sd, sd_state, 0);
1126 
1127     format->width   = MT9M111_MAX_WIDTH;
1128     format->height  = MT9M111_MAX_HEIGHT;
1129     format->code    = mt9m111_colour_fmts[0].code;
1130     format->colorspace  = mt9m111_colour_fmts[0].colorspace;
1131     format->field   = V4L2_FIELD_NONE;
1132     format->ycbcr_enc   = V4L2_YCBCR_ENC_DEFAULT;
1133     format->quantization    = V4L2_QUANTIZATION_DEFAULT;
1134     format->xfer_func   = V4L2_XFER_FUNC_DEFAULT;
1135 #endif
1136     return 0;
1137 }
1138 
1139 static int mt9m111_get_mbus_config(struct v4l2_subdev *sd,
1140                    unsigned int pad,
1141                    struct v4l2_mbus_config *cfg)
1142 {
1143     struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
1144 
1145     cfg->type = V4L2_MBUS_PARALLEL;
1146 
1147     cfg->bus.parallel.flags = V4L2_MBUS_MASTER |
1148                   V4L2_MBUS_HSYNC_ACTIVE_HIGH |
1149                   V4L2_MBUS_VSYNC_ACTIVE_HIGH |
1150                   V4L2_MBUS_DATA_ACTIVE_HIGH;
1151 
1152     cfg->bus.parallel.flags |= mt9m111->pclk_sample ?
1153                    V4L2_MBUS_PCLK_SAMPLE_RISING :
1154                    V4L2_MBUS_PCLK_SAMPLE_FALLING;
1155 
1156     return 0;
1157 }
1158 
1159 static const struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
1160     .s_stream   = mt9m111_s_stream,
1161     .g_frame_interval = mt9m111_g_frame_interval,
1162     .s_frame_interval = mt9m111_s_frame_interval,
1163 };
1164 
1165 static const struct v4l2_subdev_pad_ops mt9m111_subdev_pad_ops = {
1166     .init_cfg   = mt9m111_init_cfg,
1167     .enum_mbus_code = mt9m111_enum_mbus_code,
1168     .get_selection  = mt9m111_get_selection,
1169     .set_selection  = mt9m111_set_selection,
1170     .get_fmt    = mt9m111_get_fmt,
1171     .set_fmt    = mt9m111_set_fmt,
1172     .get_mbus_config = mt9m111_get_mbus_config,
1173 };
1174 
1175 static const struct v4l2_subdev_ops mt9m111_subdev_ops = {
1176     .core   = &mt9m111_subdev_core_ops,
1177     .video  = &mt9m111_subdev_video_ops,
1178     .pad    = &mt9m111_subdev_pad_ops,
1179 };
1180 
1181 /*
1182  * Interface active, can use i2c. If it fails, it can indeed mean, that
1183  * this wasn't our capture interface, so, we wait for the right one
1184  */
1185 static int mt9m111_video_probe(struct i2c_client *client)
1186 {
1187     struct mt9m111 *mt9m111 = to_mt9m111(client);
1188     s32 data;
1189     int ret;
1190 
1191     ret = mt9m111_s_power(&mt9m111->subdev, 1);
1192     if (ret < 0)
1193         return ret;
1194 
1195     data = reg_read(CHIP_VERSION);
1196 
1197     switch (data) {
1198     case 0x143a: /* MT9M111 or MT9M131 */
1199         dev_info(&client->dev,
1200             "Detected a MT9M111/MT9M131 chip ID %x\n", data);
1201         break;
1202     case 0x148c: /* MT9M112 */
1203         dev_info(&client->dev, "Detected a MT9M112 chip ID %x\n", data);
1204         break;
1205     default:
1206         dev_err(&client->dev,
1207             "No MT9M111/MT9M112/MT9M131 chip detected register read %x\n",
1208             data);
1209         ret = -ENODEV;
1210         goto done;
1211     }
1212 
1213     ret = mt9m111_init(mt9m111);
1214     if (ret)
1215         goto done;
1216 
1217     ret = v4l2_ctrl_handler_setup(&mt9m111->hdl);
1218 
1219 done:
1220     mt9m111_s_power(&mt9m111->subdev, 0);
1221     return ret;
1222 }
1223 
1224 static int mt9m111_probe_fw(struct i2c_client *client, struct mt9m111 *mt9m111)
1225 {
1226     struct v4l2_fwnode_endpoint bus_cfg = {
1227         .bus_type = V4L2_MBUS_PARALLEL
1228     };
1229     struct fwnode_handle *np;
1230     int ret;
1231 
1232     np = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev), NULL);
1233     if (!np)
1234         return -EINVAL;
1235 
1236     ret = v4l2_fwnode_endpoint_parse(np, &bus_cfg);
1237     if (ret)
1238         goto out_put_fw;
1239 
1240     mt9m111->pclk_sample = !!(bus_cfg.bus.parallel.flags &
1241                   V4L2_MBUS_PCLK_SAMPLE_RISING);
1242 
1243 out_put_fw:
1244     fwnode_handle_put(np);
1245     return ret;
1246 }
1247 
1248 static int mt9m111_probe(struct i2c_client *client)
1249 {
1250     struct mt9m111 *mt9m111;
1251     struct i2c_adapter *adapter = client->adapter;
1252     int ret;
1253 
1254     if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1255         dev_warn(&adapter->dev,
1256              "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1257         return -EIO;
1258     }
1259 
1260     mt9m111 = devm_kzalloc(&client->dev, sizeof(struct mt9m111), GFP_KERNEL);
1261     if (!mt9m111)
1262         return -ENOMEM;
1263 
1264     if (dev_fwnode(&client->dev)) {
1265         ret = mt9m111_probe_fw(client, mt9m111);
1266         if (ret)
1267             return ret;
1268     }
1269 
1270     mt9m111->clk = devm_clk_get(&client->dev, "mclk");
1271     if (IS_ERR(mt9m111->clk))
1272         return PTR_ERR(mt9m111->clk);
1273 
1274     mt9m111->regulator = devm_regulator_get(&client->dev, "vdd");
1275     if (IS_ERR(mt9m111->regulator)) {
1276         dev_err(&client->dev, "regulator not found: %ld\n",
1277             PTR_ERR(mt9m111->regulator));
1278         return PTR_ERR(mt9m111->regulator);
1279     }
1280 
1281     /* Default HIGHPOWER context */
1282     mt9m111->ctx = &context_b;
1283 
1284     v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
1285     mt9m111->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1286                  V4L2_SUBDEV_FL_HAS_EVENTS;
1287 
1288     v4l2_ctrl_handler_init(&mt9m111->hdl, 7);
1289     v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
1290             V4L2_CID_VFLIP, 0, 1, 1, 0);
1291     v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
1292             V4L2_CID_HFLIP, 0, 1, 1, 0);
1293     v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
1294             V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
1295     mt9m111->gain = v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
1296             V4L2_CID_GAIN, 0, 63 * 2 * 2, 1, 32);
1297     v4l2_ctrl_new_std_menu(&mt9m111->hdl,
1298             &mt9m111_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
1299             V4L2_EXPOSURE_AUTO);
1300     v4l2_ctrl_new_std_menu_items(&mt9m111->hdl,
1301             &mt9m111_ctrl_ops, V4L2_CID_TEST_PATTERN,
1302             ARRAY_SIZE(mt9m111_test_pattern_menu) - 1, 0, 0,
1303             mt9m111_test_pattern_menu);
1304     v4l2_ctrl_new_std_menu(&mt9m111->hdl, &mt9m111_ctrl_ops,
1305             V4L2_CID_COLORFX, V4L2_COLORFX_SOLARIZATION,
1306             ~(BIT(V4L2_COLORFX_NONE) |
1307                 BIT(V4L2_COLORFX_BW) |
1308                 BIT(V4L2_COLORFX_SEPIA) |
1309                 BIT(V4L2_COLORFX_NEGATIVE) |
1310                 BIT(V4L2_COLORFX_SOLARIZATION)),
1311             V4L2_COLORFX_NONE);
1312     mt9m111->subdev.ctrl_handler = &mt9m111->hdl;
1313     if (mt9m111->hdl.error) {
1314         ret = mt9m111->hdl.error;
1315         return ret;
1316     }
1317 
1318 #ifdef CONFIG_MEDIA_CONTROLLER
1319     mt9m111->pad.flags = MEDIA_PAD_FL_SOURCE;
1320     mt9m111->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1321     ret = media_entity_pads_init(&mt9m111->subdev.entity, 1, &mt9m111->pad);
1322     if (ret < 0)
1323         goto out_hdlfree;
1324 #endif
1325 
1326     mt9m111->current_mode = &mt9m111_mode_data[MT9M111_MODE_SXGA_15FPS];
1327     mt9m111->frame_interval.numerator = 1;
1328     mt9m111->frame_interval.denominator = mt9m111->current_mode->max_fps;
1329 
1330     /* Second stage probe - when a capture adapter is there */
1331     mt9m111->rect.left  = MT9M111_MIN_DARK_COLS;
1332     mt9m111->rect.top   = MT9M111_MIN_DARK_ROWS;
1333     mt9m111->rect.width = MT9M111_MAX_WIDTH;
1334     mt9m111->rect.height    = MT9M111_MAX_HEIGHT;
1335     mt9m111->width      = mt9m111->rect.width;
1336     mt9m111->height     = mt9m111->rect.height;
1337     mt9m111->fmt        = &mt9m111_colour_fmts[0];
1338     mt9m111->lastpage   = -1;
1339     mutex_init(&mt9m111->power_lock);
1340 
1341     ret = mt9m111_video_probe(client);
1342     if (ret < 0)
1343         goto out_entityclean;
1344 
1345     mt9m111->subdev.dev = &client->dev;
1346     ret = v4l2_async_register_subdev(&mt9m111->subdev);
1347     if (ret < 0)
1348         goto out_entityclean;
1349 
1350     return 0;
1351 
1352 out_entityclean:
1353 #ifdef CONFIG_MEDIA_CONTROLLER
1354     media_entity_cleanup(&mt9m111->subdev.entity);
1355 out_hdlfree:
1356 #endif
1357     v4l2_ctrl_handler_free(&mt9m111->hdl);
1358 
1359     return ret;
1360 }
1361 
1362 static int mt9m111_remove(struct i2c_client *client)
1363 {
1364     struct mt9m111 *mt9m111 = to_mt9m111(client);
1365 
1366     v4l2_async_unregister_subdev(&mt9m111->subdev);
1367     media_entity_cleanup(&mt9m111->subdev.entity);
1368     v4l2_ctrl_handler_free(&mt9m111->hdl);
1369 
1370     return 0;
1371 }
1372 static const struct of_device_id mt9m111_of_match[] = {
1373     { .compatible = "micron,mt9m111", },
1374     {},
1375 };
1376 MODULE_DEVICE_TABLE(of, mt9m111_of_match);
1377 
1378 static const struct i2c_device_id mt9m111_id[] = {
1379     { "mt9m111", 0 },
1380     { }
1381 };
1382 MODULE_DEVICE_TABLE(i2c, mt9m111_id);
1383 
1384 static struct i2c_driver mt9m111_i2c_driver = {
1385     .driver = {
1386         .name = "mt9m111",
1387         .of_match_table = of_match_ptr(mt9m111_of_match),
1388     },
1389     .probe_new  = mt9m111_probe,
1390     .remove     = mt9m111_remove,
1391     .id_table   = mt9m111_id,
1392 };
1393 
1394 module_i2c_driver(mt9m111_i2c_driver);
1395 
1396 MODULE_DESCRIPTION("Micron/Aptina MT9M111/MT9M112/MT9M131 Camera driver");
1397 MODULE_AUTHOR("Robert Jarzmik");
1398 MODULE_LICENSE("GPL");
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!