diff options
Diffstat (limited to 'drivers/media/video/mt9m111.c')
| -rw-r--r-- | drivers/media/video/mt9m111.c | 359 |
1 files changed, 172 insertions, 187 deletions
diff --git a/drivers/media/video/mt9m111.c b/drivers/media/video/mt9m111.c index ebebed9..a357aa8 100644 --- a/drivers/media/video/mt9m111.c +++ b/drivers/media/video/mt9m111.c @@ -63,6 +63,12 @@ #define MT9M111_RESET_RESTART_FRAME (1 << 1) #define MT9M111_RESET_RESET_MODE (1 << 0) +#define MT9M111_RM_FULL_POWER_RD (0 << 10) +#define MT9M111_RM_LOW_POWER_RD (1 << 10) +#define MT9M111_RM_COL_SKIP_4X (1 << 5) +#define MT9M111_RM_ROW_SKIP_4X (1 << 4) +#define MT9M111_RM_COL_SKIP_2X (1 << 3) +#define MT9M111_RM_ROW_SKIP_2X (1 << 2) #define MT9M111_RMB_MIRROR_COLS (1 << 1) #define MT9M111_RMB_MIRROR_ROWS (1 << 0) #define MT9M111_CTXT_CTRL_RESTART (1 << 15) @@ -95,7 +101,8 @@ #define MT9M111_OPMODE_AUTOEXPO_EN (1 << 14) #define MT9M111_OPMODE_AUTOWHITEBAL_EN (1 << 1) - +#define MT9M111_OUTFMT_FLIP_BAYER_COL (1 << 9) +#define MT9M111_OUTFMT_FLIP_BAYER_ROW (1 << 8) #define MT9M111_OUTFMT_PROCESSED_BAYER (1 << 14) #define MT9M111_OUTFMT_BYPASS_IFP (1 << 10) #define MT9M111_OUTFMT_INV_PIX_CLOCK (1 << 9) @@ -110,9 +117,8 @@ #define MT9M111_OUTFMT_TST_RAMP_FRAME (3 << 4) #define MT9M111_OUTFMT_SHIFT_3_UP (1 << 3) #define MT9M111_OUTFMT_AVG_CHROMA (1 << 2) -#define MT9M111_OUTFMT_SWAP_YCbCr_C_Y (1 << 1) -#define MT9M111_OUTFMT_SWAP_RGB_EVEN (1 << 1) -#define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr (1 << 0) +#define MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN (1 << 1) +#define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B (1 << 0) /* * Camera control register addresses (0x200..0x2ff not implemented) @@ -122,6 +128,8 @@ #define reg_write(reg, val) mt9m111_reg_write(client, MT9M111_##reg, (val)) #define reg_set(reg, val) mt9m111_reg_set(client, MT9M111_##reg, (val)) #define reg_clear(reg, val) mt9m111_reg_clear(client, MT9M111_##reg, (val)) +#define reg_mask(reg, val, mask) mt9m111_reg_mask(client, MT9M111_##reg, \ + (val), (mask)) #define MT9M111_MIN_DARK_ROWS 8 #define MT9M111_MIN_DARK_COLS 26 @@ -153,7 +161,11 @@ static const struct mt9m111_datafmt mt9m111_colour_fmts[] = { {V4L2_MBUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG}, {V4L2_MBUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_JPEG}, {V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB}, + {V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB}, {V4L2_MBUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB}, + {V4L2_MBUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB}, + {V4L2_MBUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB}, + {V4L2_MBUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB}, {V4L2_MBUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB}, {V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB}, }; @@ -169,6 +181,8 @@ struct mt9m111 { * from v4l2-chip-ident.h */ enum mt9m111_context context; struct v4l2_rect rect; + struct mutex power_lock; /* lock to protect power_count */ + int power_count; const struct mt9m111_datafmt *fmt; unsigned int gain; unsigned char autoexposure; @@ -176,10 +190,6 @@ struct mt9m111 { unsigned int powered:1; unsigned int hflip:1; unsigned int vflip:1; - unsigned int swap_rgb_even_odd:1; - unsigned int swap_rgb_red_blue:1; - unsigned int swap_yuv_y_chromas:1; - unsigned int swap_yuv_cb_cr:1; unsigned int autowhitebalance:1; }; @@ -248,12 +258,26 @@ static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg, int ret; ret = mt9m111_reg_read(client, reg); - return mt9m111_reg_write(client, reg, ret & ~data); + if (ret >= 0) + ret = mt9m111_reg_write(client, reg, ret & ~data); + return ret; } -static int mt9m111_set_context(struct i2c_client *client, +static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg, + const u16 data, const u16 mask) +{ + int ret; + + ret = mt9m111_reg_read(client, reg); + if (ret >= 0) + ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data); + return ret; +} + +static int mt9m111_set_context(struct mt9m111 *mt9m111, enum mt9m111_context ctxt) { + struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev); int valB = MT9M111_CTXT_CTRL_RESTART | MT9M111_CTXT_CTRL_DEFECTCOR_B | MT9M111_CTXT_CTRL_RESIZE_B | MT9M111_CTXT_CTRL_CTRL2_B | MT9M111_CTXT_CTRL_GAMMA_B | MT9M111_CTXT_CTRL_READ_MODE_B @@ -267,10 +291,10 @@ static int mt9m111_set_context(struct i2c_client *client, return reg_write(CONTEXT_CONTROL, valA); } -static int mt9m111_setup_rect(struct i2c_client *client, +static int mt9m111_setup_rect(struct mt9m111 *mt9m111, struct v4l2_rect *rect) { - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev); int ret, is_raw_format; int width = rect->width; int height = rect->height; @@ -312,81 +336,9 @@ static int mt9m111_setup_rect(struct i2c_client *client, return ret; } -static int mt9m111_setup_pixfmt(struct i2c_client *client, u16 outfmt) +static int mt9m111_enable(struct mt9m111 *mt9m111) { - int ret; - u16 mask = MT9M111_OUTFMT_PROCESSED_BAYER | MT9M111_OUTFMT_RGB | - MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_SWAP_RGB_EVEN | - MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 | - MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr | - MT9M111_OUTFMT_SWAP_YCbCr_C_Y; - - ret = reg_read(OUTPUT_FORMAT_CTRL2_A); - if (ret >= 0) - ret = reg_write(OUTPUT_FORMAT_CTRL2_A, (ret & ~mask) | outfmt); - if (!ret) - ret = reg_read(OUTPUT_FORMAT_CTRL2_B); - if (ret >= 0) - ret = reg_write(OUTPUT_FORMAT_CTRL2_B, (ret & ~mask) | outfmt); - - return ret; -} - -static int mt9m111_setfmt_bayer8(struct i2c_client *client) -{ - return mt9m111_setup_pixfmt(client, MT9M111_OUTFMT_PROCESSED_BAYER | - MT9M111_OUTFMT_RGB); -} - -static int mt9m111_setfmt_bayer10(struct i2c_client *client) -{ - return mt9m111_setup_pixfmt(client, MT9M111_OUTFMT_BYPASS_IFP); -} - -static int mt9m111_setfmt_rgb565(struct i2c_client *client) -{ - struct mt9m111 *mt9m111 = to_mt9m111(client); - int val = 0; - - if (mt9m111->swap_rgb_red_blue) - val |= MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr; - if (mt9m111->swap_rgb_even_odd) - val |= MT9M111_OUTFMT_SWAP_RGB_EVEN; - val |= MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565; - - return mt9m111_setup_pixfmt(client, val); -} - -static int mt9m111_setfmt_rgb555(struct i2c_client *client) -{ - struct mt9m111 *mt9m111 = to_mt9m111(client); - int val = 0; - - if (mt9m111->swap_rgb_red_blue) - val |= MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr; - if (mt9m111->swap_rgb_even_odd) - val |= MT9M111_OUTFMT_SWAP_RGB_EVEN; - val |= MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555; - - return mt9m111_setup_pixfmt(client, val); -} - -static int mt9m111_setfmt_yuv(struct i2c_client *client) -{ - struct mt9m111 *mt9m111 = to_mt9m111(client); - int val = 0; - - if (mt9m111->swap_yuv_cb_cr) - val |= MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr; - if (mt9m111->swap_yuv_y_chromas) - val |= MT9M111_OUTFMT_SWAP_YCbCr_C_Y; - - return mt9m111_setup_pixfmt(client, val); -} - -static int mt9m111_enable(struct i2c_client *client) -{ - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev); int ret; ret = reg_set(RESET, MT9M111_RESET_CHIP_ENABLE); @@ -395,8 +347,9 @@ static int mt9m111_enable(struct i2c_client *client) return ret; } -static int mt9m111_reset(struct i2c_client *client) +static int mt9m111_reset(struct mt9m111 *mt9m111) { + struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev); int ret; ret = reg_set(RESET, MT9M111_RESET_RESET_MODE); @@ -424,11 +377,9 @@ static int mt9m111_set_bus_param(struct soc_camera_device *icd, unsigned long f) return 0; } -static int mt9m111_make_rect(struct i2c_client *client, +static int mt9m111_make_rect(struct mt9m111 *mt9m111, struct v4l2_rect *rect) { - struct mt9m111 *mt9m111 = to_mt9m111(client); - if (mt9m111->fmt->code == V4L2_MBUS_FMT_SBGGR8_1X8 || mt9m111->fmt->code == V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE) { /* Bayer format - even size lengths */ @@ -444,14 +395,14 @@ static int mt9m111_make_rect(struct i2c_client *client, soc_camera_limit_side(&rect->top, &rect->height, MT9M111_MIN_DARK_ROWS, 2, MT9M111_MAX_HEIGHT); - return mt9m111_setup_rect(client, rect); + return mt9m111_setup_rect(mt9m111, rect); } static int mt9m111_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a) { struct v4l2_rect rect = a->c; struct i2c_client *client = v4l2_get_subdevdata(sd); - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev); int ret; dev_dbg(&client->dev, "%s left=%d, top=%d, width=%d, height=%d\n", @@ -460,7 +411,7 @@ static int mt9m111_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a) if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) return -EINVAL; - ret = mt9m111_make_rect(client, &rect); + ret = mt9m111_make_rect(mt9m111, &rect); if (!ret) mt9m111->rect = rect; return ret; @@ -468,8 +419,7 @@ static int mt9m111_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a) static int mt9m111_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a) { - struct i2c_client *client = v4l2_get_subdevdata(sd); - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev); a->c = mt9m111->rect; a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; @@ -496,8 +446,7 @@ static int mt9m111_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a) static int mt9m111_g_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf) { - struct i2c_client *client = v4l2_get_subdevdata(sd); - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev); mf->width = mt9m111->rect.width; mf->height = mt9m111->rect.height; @@ -508,51 +457,73 @@ static int mt9m111_g_fmt(struct v4l2_subdev *sd, return 0; } -static int mt9m111_set_pixfmt(struct i2c_client *client, +static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111, enum v4l2_mbus_pixelcode code) { - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev); + u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER | + MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB | + MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 | + MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 | + MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN | + MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B; int ret; switch (code) { case V4L2_MBUS_FMT_SBGGR8_1X8: - ret = mt9m111_setfmt_bayer8(client); + data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER | + MT9M111_OUTFMT_RGB; break; case V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE: - ret = mt9m111_setfmt_bayer10(client); + data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB; break; case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE: - ret = mt9m111_setfmt_rgb555(client); + data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 | + MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN; + break; + case V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE: + data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555; break; case V4L2_MBUS_FMT_RGB565_2X8_LE: - ret = mt9m111_setfmt_rgb565(client); + data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 | + MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN; + break; + case V4L2_MBUS_FMT_RGB565_2X8_BE: + data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565; + break; + case V4L2_MBUS_FMT_BGR565_2X8_BE: + data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 | + MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B; + break; + case V4L2_MBUS_FMT_BGR565_2X8_LE: + data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 | + MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN | + MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B; break; case V4L2_MBUS_FMT_UYVY8_2X8: - mt9m111->swap_yuv_y_chromas = 0; - mt9m111->swap_yuv_cb_cr = 0; - ret = mt9m111_setfmt_yuv(client); + data_outfmt2 = 0; break; case V4L2_MBUS_FMT_VYUY8_2X8: - mt9m111->swap_yuv_y_chromas = 0; - mt9m111->swap_yuv_cb_cr = 1; - ret = mt9m111_setfmt_yuv(client); + data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B; break; case V4L2_MBUS_FMT_YUYV8_2X8: - mt9m111->swap_yuv_y_chromas = 1; - mt9m111->swap_yuv_cb_cr = 0; - ret = mt9m111_setfmt_yuv(client); + data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN; break; case V4L2_MBUS_FMT_YVYU8_2X8: - mt9m111->swap_yuv_y_chromas = 1; - mt9m111->swap_yuv_cb_cr = 1; - ret = mt9m111_setfmt_yuv(client); + data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN | + MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B; break; default: - dev_err(&client->dev, "Pixel format not handled : %x\n", - code); - ret = -EINVAL; + dev_err(&client->dev, "Pixel format not handled: %x\n", code); + return -EINVAL; } + ret = reg_mask(OUTPUT_FORMAT_CTRL2_A, data_outfmt2, + mask_outfmt2); + if (!ret) + ret = reg_mask(OUTPUT_FORMAT_CTRL2_B, data_outfmt2, + mask_outfmt2); + return ret; } @@ -561,7 +532,7 @@ static int mt9m111_s_fmt(struct v4l2_subdev *sd, { struct i2c_client *client = v4l2_get_subdevdata(sd); const struct mt9m111_datafmt *fmt; - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev); struct v4l2_rect rect = { .left = mt9m111->rect.left, .top = mt9m111->rect.top, @@ -579,9 +550,9 @@ static int mt9m111_s_fmt(struct v4l2_subdev *sd, "%s code=%x left=%d, top=%d, width=%d, height=%d\n", __func__, mf->code, rect.left, rect.top, rect.width, rect.height); - ret = mt9m111_make_rect(client, &rect); + ret = mt9m111_make_rect(mt9m111, &rect); if (!ret) - ret = mt9m111_set_pixfmt(client, mf->code); + ret = mt9m111_set_pixfmt(mt9m111, mf->code); if (!ret) { mt9m111->rect = rect; mt9m111->fmt = fmt; @@ -594,8 +565,7 @@ static int mt9m111_s_fmt(struct v4l2_subdev *sd, static int mt9m111_try_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf) { - struct i2c_client *client = v4l2_get_subdevdata(sd); - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev); const struct mt9m111_datafmt *fmt; bool bayer = mf->code == V4L2_MBUS_FMT_SBGGR8_1X8 || mf->code == V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE; @@ -635,7 +605,7 @@ static int mt9m111_g_chip_ident(struct v4l2_subdev *sd, struct v4l2_dbg_chip_ident *id) { struct i2c_client *client = v4l2_get_subdevdata(sd); - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev); if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR) return -EINVAL; @@ -726,21 +696,16 @@ static const struct v4l2_queryctrl mt9m111_controls[] = { } }; -static int mt9m111_resume(struct soc_camera_device *icd); -static int mt9m111_suspend(struct soc_camera_device *icd, pm_message_t state); - static struct soc_camera_ops mt9m111_ops = { - .suspend = mt9m111_suspend, - .resume = mt9m111_resume, .query_bus_param = mt9m111_query_bus_param, .set_bus_param = mt9m111_set_bus_param, .controls = mt9m111_controls, .num_controls = ARRAY_SIZE(mt9m111_controls), }; -static int mt9m111_set_flip(struct i2c_client *client, int flip, int mask) +static int mt9m111_set_flip(struct mt9m111 *mt9m111, int flip, int mask) { - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev); int ret; if (mt9m111->context == HIGHPOWER) { @@ -758,8 +723,9 @@ static int mt9m111_set_flip(struct i2c_client *client, int flip, int mask) return ret; } -static int mt9m111_get_global_gain(struct i2c_client *client) +static int mt9m111_get_global_gain(struct mt9m111 *mt9m111) { + struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev); int data; data = reg_read(GLOBAL_GAIN); @@ -769,9 +735,9 @@ static int mt9m111_get_global_gain(struct i2c_client *client) return data; } -static int mt9m111_set_global_gain(struct i2c_client *client, int gain) +static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain) { - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev); u16 val; if (gain > 63 * 2 * 2) @@ -788,9 +754,9 @@ static int mt9m111_set_global_gain(struct i2c_client *client, int gain) return reg_write(GLOBAL_GAIN, val); } -static int mt9m111_set_autoexposure(struct i2c_client *client, int on) +static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int on) { - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev); int ret; if (on) @@ -804,9 +770,9 @@ static int mt9m111_set_autoexposure(struct i2c_client *client, int on) return ret; } -static int mt9m111_set_autowhitebalance(struct i2c_client *client, int on) +static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on) { - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev); int ret; if (on) @@ -823,7 +789,7 @@ static int mt9m111_set_autowhitebalance(struct i2c_client *client, int on) static int mt9m111_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl) { struct i2c_client *client = v4l2_get_subdevdata(sd); - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev); int data; switch (ctrl->id) { @@ -848,7 +814,7 @@ static int mt9m111_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl) ctrl->value = !!(data & MT9M111_RMB_MIRROR_COLS); break; case V4L2_CID_GAIN: - data = mt9m111_get_global_gain(client); + data = mt9m111_get_global_gain(mt9m111); if (data < 0) return data; ctrl->value = data; @@ -865,8 +831,7 @@ static int mt9m111_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl) static int mt9m111_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl) { - struct i2c_client *client = v4l2_get_subdevdata(sd); - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev); const struct v4l2_queryctrl *qctrl; int ret; @@ -877,22 +842,22 @@ static int mt9m111_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl) switch (ctrl->id) { case V4L2_CID_VFLIP: mt9m111->vflip = ctrl->value; - ret = mt9m111_set_flip(client, ctrl->value, + ret = mt9m111_set_flip(mt9m111, ctrl->value, MT9M111_RMB_MIRROR_ROWS); break; case V4L2_CID_HFLIP: mt9m111->hflip = ctrl->value; - ret = mt9m111_set_flip(client, ctrl->value, + ret = mt9m111_set_flip(mt9m111, ctrl->value, MT9M111_RMB_MIRROR_COLS); break; case V4L2_CID_GAIN: - ret = mt9m111_set_global_gain(client, ctrl->value); + ret = mt9m111_set_global_gain(mt9m111, ctrl->value); break; case V4L2_CID_EXPOSURE_AUTO: - ret = mt9m111_set_autoexposure(client, ctrl->value); + ret = mt9m111_set_autoexposure(mt9m111, ctrl->value); break; case V4L2_CID_AUTO_WHITE_BALANCE: - ret = mt9m111_set_autowhitebalance(client, ctrl->value); + ret = mt9m111_set_autowhitebalance(mt9m111, ctrl->value); break; default: ret = -EINVAL; @@ -901,60 +866,52 @@ static int mt9m111_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl) return ret; } -static int mt9m111_suspend(struct soc_camera_device *icd, pm_message_t state) +static int mt9m111_suspend(struct mt9m111 *mt9m111) { - struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd)); - struct mt9m111 *mt9m111 = to_mt9m111(client); - - mt9m111->gain = mt9m111_get_global_gain(client); + mt9m111->gain = mt9m111_get_global_gain(mt9m111); return 0; } -static int mt9m111_restore_state(struct i2c_client *client) +static void mt9m111_restore_state(struct mt9m111 *mt9m111) { - struct mt9m111 *mt9m111 = to_mt9m111(client); - - mt9m111_set_context(client, mt9m111->context); - mt9m111_set_pixfmt(client, mt9m111->fmt->code); - mt9m111_setup_rect(client, &mt9m111->rect); - mt9m111_set_flip(client, mt9m111->hflip, MT9M111_RMB_MIRROR_COLS); - mt9m111_set_flip(client, mt9m111->vflip, MT9M111_RMB_MIRROR_ROWS); - mt9m111_set_global_gain(client, mt9m111->gain); - mt9m111_set_autoexposure(client, mt9m111->autoexposure); - mt9m111_set_autowhitebalance(client, mt9m111->autowhitebalance); - return 0; + mt9m111_set_context(mt9m111, mt9m111->context); + mt9m111_set_pixfmt(mt9m111, mt9m111->fmt->code); + mt9m111_setup_rect(mt9m111, &mt9m111->rect); + mt9m111_set_flip(mt9m111, mt9m111->hflip, MT9M111_RMB_MIRROR_COLS); + mt9m111_set_flip(mt9m111, mt9m111->vflip, MT9M111_RMB_MIRROR_ROWS); + mt9m111_set_global_gain(mt9m111, mt9m111->gain); + mt9m111_set_autoexposure(mt9m111, mt9m111->autoexposure); + mt9m111_set_autowhitebalance(mt9m111, mt9m111->autowhitebalance); } -static int mt9m111_resume(struct soc_camera_device *icd) +static int mt9m111_resume(struct mt9m111 *mt9m111) { - struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd)); - struct mt9m111 *mt9m111 = to_mt9m111(client); int ret = 0; if (mt9m111->powered) { - ret = mt9m111_enable(client); + ret = mt9m111_enable(mt9m111); if (!ret) - ret = mt9m111_reset(client); + ret = mt9m111_reset(mt9m111); if (!ret) - ret = mt9m111_restore_state(client); + mt9m111_restore_state(mt9m111); } return ret; } -static int mt9m111_init(struct i2c_client *client) +static int mt9m111_init(struct mt9m111 *mt9m111) { - struct mt9m111 *mt9m111 = to_mt9m111(client); + struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev); int ret; mt9m111->context = HIGHPOWER; - ret = mt9m111_enable(client); + ret = mt9m111_enable(mt9m111); if (!ret) - ret = mt9m111_reset(client); + ret = mt9m111_reset(mt9m111); if (!ret) - ret = mt9m111_set_context(client, mt9m111->context); + ret = mt9m111_set_context(mt9m111, mt9m111->context); if (!ret) - ret = mt9m111_set_autoexposure(client, mt9m111->autoexposure); + ret = mt9m111_set_autoexposure(mt9m111, mt9m111->autoexposure); if (ret) dev_err(&client->dev, "mt9m111 init failed: %d\n", ret); return ret; @@ -971,20 +928,13 @@ static int mt9m111_video_probe(struct soc_camera_device *icd, s32 data; int ret; - /* - * We must have a parent by now. And it cannot be a wrong one. - * So this entire test is completely redundant. - */ - if (!icd->dev.parent || - to_soc_camera_host(icd->dev.parent)->nr != icd->iface) - return -ENODEV; + /* We must have a parent by now. And it cannot be a wrong one. */ + BUG_ON(!icd->parent || + to_soc_camera_host(icd->parent)->nr != icd->iface); mt9m111->autoexposure = 1; mt9m111->autowhitebalance = 1; - mt9m111->swap_rgb_even_odd = 1; - mt9m111->swap_rgb_red_blue = 1; - data = reg_read(CHIP_VERSION); switch (data) { @@ -1005,16 +955,51 @@ static int mt9m111_video_probe(struct soc_camera_device *icd, goto ei2c; } - ret = mt9m111_init(client); + ret = mt9m111_init(mt9m111); ei2c: return ret; } +static int mt9m111_s_power(struct v4l2_subdev *sd, int on) +{ + struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev); + struct i2c_client *client = v4l2_get_subdevdata(sd); + int ret = 0; + + mutex_lock(&mt9m111->power_lock); + + /* + * If the power count is modified from 0 to != 0 or from != 0 to 0, + * update the power state. + */ + if (mt9m111->power_count == !on) { + if (on) { + ret = mt9m111_resume(mt9m111); + if (ret) { + dev_err(&client->dev, + "Failed to resume the sensor: %d\n", ret); + goto out; + } + } else { + mt9m111_suspend(mt9m111); + } + } + + /* Update the power count. */ + mt9m111->power_count += on ? 1 : -1; + WARN_ON(mt9m111->power_count < 0); + +out: + mutex_unlock(&mt9m111->power_lock); + return ret; +} + static struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = { .g_ctrl = mt9m111_g_ctrl, .s_ctrl = mt9m111_s_ctrl, .g_chip_ident = mt9m111_g_chip_ident, + .s_power = mt9m111_s_power, #ifdef CONFIG_VIDEO_ADV_DEBUG .g_register = mt9m111_g_register, .s_register = mt9m111_s_register, |
