forked from Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1277 lines
31 KiB
1277 lines
31 KiB
// SPDX-License-Identifier: GPL-2.0 |
|
/* |
|
* V4L2 sensor driver for Aptina MT9V111 image sensor |
|
* Copyright (C) 2018 Jacopo Mondi <[email protected]> |
|
* |
|
* Based on mt9v032 driver |
|
* Copyright (C) 2010, Laurent Pinchart <[email protected]> |
|
* Copyright (C) 2008, Guennadi Liakhovetski <[email protected]> |
|
* |
|
* Based on mt9v011 driver |
|
* Copyright (c) 2009 Mauro Carvalho Chehab <[email protected]> |
|
*/ |
|
|
|
#include <linux/clk.h> |
|
#include <linux/delay.h> |
|
#include <linux/gpio/consumer.h> |
|
#include <linux/i2c.h> |
|
#include <linux/of.h> |
|
#include <linux/slab.h> |
|
#include <linux/videodev2.h> |
|
#include <linux/v4l2-mediabus.h> |
|
#include <linux/module.h> |
|
|
|
#include <media/v4l2-ctrls.h> |
|
#include <media/v4l2-device.h> |
|
#include <media/v4l2-fwnode.h> |
|
#include <media/v4l2-image-sizes.h> |
|
#include <media/v4l2-subdev.h> |
|
|
|
/* |
|
* MT9V111 is a 1/4-Inch CMOS digital image sensor with an integrated |
|
* Image Flow Processing (IFP) engine and a sensor core loosely based on |
|
* MT9V011. |
|
* |
|
* The IFP can produce several output image formats from the sensor core |
|
* output. This driver currently supports only YUYV format permutations. |
|
* |
|
* The driver allows manual frame rate control through s_frame_interval subdev |
|
* operation or V4L2_CID_V/HBLANK controls, but it is known that the |
|
* auto-exposure algorithm might modify the programmed frame rate. While the |
|
* driver initially programs the sensor with auto-exposure and |
|
* auto-white-balancing enabled, it is possible to disable them and more |
|
* precisely control the frame rate. |
|
* |
|
* While it seems possible to instruct the auto-exposure control algorithm to |
|
* respect a programmed frame rate when adjusting the pixel integration time, |
|
* registers controlling this feature are not documented in the public |
|
* available sensor manual used to develop this driver (09005aef80e90084, |
|
* MT9V111_1.fm - Rev. G 1/05 EN). |
|
*/ |
|
|
|
#define MT9V111_CHIP_ID_HIGH 0x82 |
|
#define MT9V111_CHIP_ID_LOW 0x3a |
|
|
|
#define MT9V111_R01_ADDR_SPACE 0x01 |
|
#define MT9V111_R01_IFP 0x01 |
|
#define MT9V111_R01_CORE 0x04 |
|
|
|
#define MT9V111_IFP_R06_OPMODE_CTRL 0x06 |
|
#define MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN BIT(1) |
|
#define MT9V111_IFP_R06_OPMODE_CTRL_AE_EN BIT(14) |
|
#define MT9V111_IFP_R07_IFP_RESET 0x07 |
|
#define MT9V111_IFP_R07_IFP_RESET_MASK BIT(0) |
|
#define MT9V111_IFP_R08_OUTFMT_CTRL 0x08 |
|
#define MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER BIT(11) |
|
#define MT9V111_IFP_R08_OUTFMT_CTRL_PCLK BIT(5) |
|
#define MT9V111_IFP_R3A_OUTFMT_CTRL2 0x3a |
|
#define MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR BIT(0) |
|
#define MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC BIT(1) |
|
#define MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK GENMASK(2, 0) |
|
#define MT9V111_IFP_RA5_HPAN 0xa5 |
|
#define MT9V111_IFP_RA6_HZOOM 0xa6 |
|
#define MT9V111_IFP_RA7_HOUT 0xa7 |
|
#define MT9V111_IFP_RA8_VPAN 0xa8 |
|
#define MT9V111_IFP_RA9_VZOOM 0xa9 |
|
#define MT9V111_IFP_RAA_VOUT 0xaa |
|
#define MT9V111_IFP_DECIMATION_MASK GENMASK(9, 0) |
|
#define MT9V111_IFP_DECIMATION_FREEZE BIT(15) |
|
|
|
#define MT9V111_CORE_R03_WIN_HEIGHT 0x03 |
|
#define MT9V111_CORE_R03_WIN_V_OFFS 2 |
|
#define MT9V111_CORE_R04_WIN_WIDTH 0x04 |
|
#define MT9V111_CORE_R04_WIN_H_OFFS 114 |
|
#define MT9V111_CORE_R05_HBLANK 0x05 |
|
#define MT9V111_CORE_R05_MIN_HBLANK 0x09 |
|
#define MT9V111_CORE_R05_MAX_HBLANK GENMASK(9, 0) |
|
#define MT9V111_CORE_R05_DEF_HBLANK 0x26 |
|
#define MT9V111_CORE_R06_VBLANK 0x06 |
|
#define MT9V111_CORE_R06_MIN_VBLANK 0x03 |
|
#define MT9V111_CORE_R06_MAX_VBLANK GENMASK(11, 0) |
|
#define MT9V111_CORE_R06_DEF_VBLANK 0x04 |
|
#define MT9V111_CORE_R07_OUT_CTRL 0x07 |
|
#define MT9V111_CORE_R07_OUT_CTRL_SAMPLE BIT(4) |
|
#define MT9V111_CORE_R09_PIXEL_INT 0x09 |
|
#define MT9V111_CORE_R09_PIXEL_INT_MASK GENMASK(11, 0) |
|
#define MT9V111_CORE_R0D_CORE_RESET 0x0d |
|
#define MT9V111_CORE_R0D_CORE_RESET_MASK BIT(0) |
|
#define MT9V111_CORE_RFF_CHIP_VER 0xff |
|
|
|
#define MT9V111_PIXEL_ARRAY_WIDTH 640 |
|
#define MT9V111_PIXEL_ARRAY_HEIGHT 480 |
|
|
|
#define MT9V111_MAX_CLKIN 27000000 |
|
|
|
/* The default sensor configuration at startup time. */ |
|
static const struct v4l2_mbus_framefmt mt9v111_def_fmt = { |
|
.width = 640, |
|
.height = 480, |
|
.code = MEDIA_BUS_FMT_UYVY8_2X8, |
|
.field = V4L2_FIELD_NONE, |
|
.colorspace = V4L2_COLORSPACE_SRGB, |
|
.ycbcr_enc = V4L2_YCBCR_ENC_601, |
|
.quantization = V4L2_QUANTIZATION_LIM_RANGE, |
|
.xfer_func = V4L2_XFER_FUNC_SRGB, |
|
}; |
|
|
|
struct mt9v111_dev { |
|
struct device *dev; |
|
struct i2c_client *client; |
|
|
|
u8 addr_space; |
|
|
|
struct v4l2_subdev sd; |
|
#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) |
|
struct media_pad pad; |
|
#endif |
|
|
|
struct v4l2_ctrl *auto_awb; |
|
struct v4l2_ctrl *auto_exp; |
|
struct v4l2_ctrl *hblank; |
|
struct v4l2_ctrl *vblank; |
|
struct v4l2_ctrl_handler ctrls; |
|
|
|
/* Output image format and sizes. */ |
|
struct v4l2_mbus_framefmt fmt; |
|
unsigned int fps; |
|
|
|
/* Protects power up/down sequences. */ |
|
struct mutex pwr_mutex; |
|
int pwr_count; |
|
|
|
/* Protects stream on/off sequences. */ |
|
struct mutex stream_mutex; |
|
bool streaming; |
|
|
|
/* Flags to mark HW settings as not yet applied. */ |
|
bool pending; |
|
|
|
/* Clock provider and system clock frequency. */ |
|
struct clk *clk; |
|
u32 sysclk; |
|
|
|
struct gpio_desc *oe; |
|
struct gpio_desc *standby; |
|
struct gpio_desc *reset; |
|
}; |
|
|
|
#define sd_to_mt9v111(__sd) container_of((__sd), struct mt9v111_dev, sd) |
|
|
|
/* |
|
* mt9v111_mbus_fmt - List all media bus formats supported by the driver. |
|
* |
|
* Only list the media bus code here. The image sizes are freely configurable |
|
* in the pixel array sizes range. |
|
* |
|
* The desired frame interval, in the supported frame interval range, is |
|
* obtained by configuring blanking as the sensor does not have a PLL but |
|
* only a fixed clock divider that generates the output pixel clock. |
|
*/ |
|
static struct mt9v111_mbus_fmt { |
|
u32 code; |
|
} mt9v111_formats[] = { |
|
{ |
|
.code = MEDIA_BUS_FMT_UYVY8_2X8, |
|
}, |
|
{ |
|
.code = MEDIA_BUS_FMT_YUYV8_2X8, |
|
}, |
|
{ |
|
.code = MEDIA_BUS_FMT_VYUY8_2X8, |
|
}, |
|
{ |
|
.code = MEDIA_BUS_FMT_YVYU8_2X8, |
|
}, |
|
}; |
|
|
|
static u32 mt9v111_frame_intervals[] = {5, 10, 15, 20, 30}; |
|
|
|
/* |
|
* mt9v111_frame_sizes - List sensor's supported resolutions. |
|
* |
|
* Resolution generated through decimation in the IFP block from the |
|
* full VGA pixel array. |
|
*/ |
|
static struct v4l2_rect mt9v111_frame_sizes[] = { |
|
{ |
|
.width = 640, |
|
.height = 480, |
|
}, |
|
{ |
|
.width = 352, |
|
.height = 288 |
|
}, |
|
{ |
|
.width = 320, |
|
.height = 240, |
|
}, |
|
{ |
|
.width = 176, |
|
.height = 144, |
|
}, |
|
{ |
|
.width = 160, |
|
.height = 120, |
|
}, |
|
}; |
|
|
|
/* --- Device I/O access --- */ |
|
|
|
static int __mt9v111_read(struct i2c_client *c, u8 reg, u16 *val) |
|
{ |
|
struct i2c_msg msg[2]; |
|
__be16 buf; |
|
int ret; |
|
|
|
msg[0].addr = c->addr; |
|
msg[0].flags = 0; |
|
msg[0].len = 1; |
|
msg[0].buf = ® |
|
|
|
msg[1].addr = c->addr; |
|
msg[1].flags = I2C_M_RD; |
|
msg[1].len = 2; |
|
msg[1].buf = (char *)&buf; |
|
|
|
ret = i2c_transfer(c->adapter, msg, 2); |
|
if (ret < 0) { |
|
dev_err(&c->dev, "i2c read transfer error: %d\n", ret); |
|
return ret; |
|
} |
|
|
|
*val = be16_to_cpu(buf); |
|
|
|
dev_dbg(&c->dev, "%s: %x=%x\n", __func__, reg, *val); |
|
|
|
return 0; |
|
} |
|
|
|
static int __mt9v111_write(struct i2c_client *c, u8 reg, u16 val) |
|
{ |
|
struct i2c_msg msg; |
|
u8 buf[3] = { 0 }; |
|
int ret; |
|
|
|
buf[0] = reg; |
|
buf[1] = val >> 8; |
|
buf[2] = val & 0xff; |
|
|
|
msg.addr = c->addr; |
|
msg.flags = 0; |
|
msg.len = 3; |
|
msg.buf = (char *)buf; |
|
|
|
dev_dbg(&c->dev, "%s: %x = %x%x\n", __func__, reg, buf[1], buf[2]); |
|
|
|
ret = i2c_transfer(c->adapter, &msg, 1); |
|
if (ret < 0) { |
|
dev_err(&c->dev, "i2c write transfer error: %d\n", ret); |
|
return ret; |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
static int __mt9v111_addr_space_select(struct i2c_client *c, u16 addr_space) |
|
{ |
|
struct v4l2_subdev *sd = i2c_get_clientdata(c); |
|
struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd); |
|
u16 val; |
|
int ret; |
|
|
|
if (mt9v111->addr_space == addr_space) |
|
return 0; |
|
|
|
ret = __mt9v111_write(c, MT9V111_R01_ADDR_SPACE, addr_space); |
|
if (ret) |
|
return ret; |
|
|
|
/* Verify address space has been updated */ |
|
ret = __mt9v111_read(c, MT9V111_R01_ADDR_SPACE, &val); |
|
if (ret) |
|
return ret; |
|
|
|
if (val != addr_space) |
|
return -EINVAL; |
|
|
|
mt9v111->addr_space = addr_space; |
|
|
|
return 0; |
|
} |
|
|
|
static int mt9v111_read(struct i2c_client *c, u8 addr_space, u8 reg, u16 *val) |
|
{ |
|
int ret; |
|
|
|
/* Select register address space first. */ |
|
ret = __mt9v111_addr_space_select(c, addr_space); |
|
if (ret) |
|
return ret; |
|
|
|
ret = __mt9v111_read(c, reg, val); |
|
if (ret) |
|
return ret; |
|
|
|
return 0; |
|
} |
|
|
|
static int mt9v111_write(struct i2c_client *c, u8 addr_space, u8 reg, u16 val) |
|
{ |
|
int ret; |
|
|
|
/* Select register address space first. */ |
|
ret = __mt9v111_addr_space_select(c, addr_space); |
|
if (ret) |
|
return ret; |
|
|
|
ret = __mt9v111_write(c, reg, val); |
|
if (ret) |
|
return ret; |
|
|
|
return 0; |
|
} |
|
|
|
static int mt9v111_update(struct i2c_client *c, u8 addr_space, u8 reg, |
|
u16 mask, u16 val) |
|
{ |
|
u16 current_val; |
|
int ret; |
|
|
|
/* Select register address space first. */ |
|
ret = __mt9v111_addr_space_select(c, addr_space); |
|
if (ret) |
|
return ret; |
|
|
|
/* Read the current register value, then update it. */ |
|
ret = __mt9v111_read(c, reg, ¤t_val); |
|
if (ret) |
|
return ret; |
|
|
|
current_val &= ~mask; |
|
current_val |= (val & mask); |
|
ret = __mt9v111_write(c, reg, current_val); |
|
if (ret) |
|
return ret; |
|
|
|
return 0; |
|
} |
|
|
|
/* --- Sensor HW operations --- */ |
|
|
|
static int __mt9v111_power_on(struct v4l2_subdev *sd) |
|
{ |
|
struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd); |
|
int ret; |
|
|
|
ret = clk_prepare_enable(mt9v111->clk); |
|
if (ret) |
|
return ret; |
|
|
|
clk_set_rate(mt9v111->clk, mt9v111->sysclk); |
|
|
|
gpiod_set_value(mt9v111->standby, 0); |
|
usleep_range(500, 1000); |
|
|
|
gpiod_set_value(mt9v111->oe, 1); |
|
usleep_range(500, 1000); |
|
|
|
return 0; |
|
} |
|
|
|
static int __mt9v111_power_off(struct v4l2_subdev *sd) |
|
{ |
|
struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd); |
|
|
|
gpiod_set_value(mt9v111->oe, 0); |
|
usleep_range(500, 1000); |
|
|
|
gpiod_set_value(mt9v111->standby, 1); |
|
usleep_range(500, 1000); |
|
|
|
clk_disable_unprepare(mt9v111->clk); |
|
|
|
return 0; |
|
} |
|
|
|
static int __mt9v111_hw_reset(struct mt9v111_dev *mt9v111) |
|
{ |
|
if (!mt9v111->reset) |
|
return -EINVAL; |
|
|
|
gpiod_set_value(mt9v111->reset, 1); |
|
usleep_range(500, 1000); |
|
|
|
gpiod_set_value(mt9v111->reset, 0); |
|
usleep_range(500, 1000); |
|
|
|
return 0; |
|
} |
|
|
|
static int __mt9v111_sw_reset(struct mt9v111_dev *mt9v111) |
|
{ |
|
struct i2c_client *c = mt9v111->client; |
|
int ret; |
|
|
|
/* Software reset core and IFP blocks. */ |
|
|
|
ret = mt9v111_update(c, MT9V111_R01_CORE, |
|
MT9V111_CORE_R0D_CORE_RESET, |
|
MT9V111_CORE_R0D_CORE_RESET_MASK, 1); |
|
if (ret) |
|
return ret; |
|
usleep_range(500, 1000); |
|
|
|
ret = mt9v111_update(c, MT9V111_R01_CORE, |
|
MT9V111_CORE_R0D_CORE_RESET, |
|
MT9V111_CORE_R0D_CORE_RESET_MASK, 0); |
|
if (ret) |
|
return ret; |
|
usleep_range(500, 1000); |
|
|
|
ret = mt9v111_update(c, MT9V111_R01_IFP, |
|
MT9V111_IFP_R07_IFP_RESET, |
|
MT9V111_IFP_R07_IFP_RESET_MASK, 1); |
|
if (ret) |
|
return ret; |
|
usleep_range(500, 1000); |
|
|
|
ret = mt9v111_update(c, MT9V111_R01_IFP, |
|
MT9V111_IFP_R07_IFP_RESET, |
|
MT9V111_IFP_R07_IFP_RESET_MASK, 0); |
|
if (ret) |
|
return ret; |
|
usleep_range(500, 1000); |
|
|
|
return 0; |
|
} |
|
|
|
static int mt9v111_calc_frame_rate(struct mt9v111_dev *mt9v111, |
|
struct v4l2_fract *tpf) |
|
{ |
|
unsigned int fps = tpf->numerator ? |
|
tpf->denominator / tpf->numerator : |
|
tpf->denominator; |
|
unsigned int best_diff; |
|
unsigned int frm_cols; |
|
unsigned int row_pclk; |
|
unsigned int best_fps; |
|
unsigned int pclk; |
|
unsigned int diff; |
|
unsigned int idx; |
|
unsigned int hb; |
|
unsigned int vb; |
|
unsigned int i; |
|
int ret; |
|
|
|
/* Approximate to the closest supported frame interval. */ |
|
best_diff = ~0L; |
|
for (i = 0, idx = 0; i < ARRAY_SIZE(mt9v111_frame_intervals); i++) { |
|
diff = abs(fps - mt9v111_frame_intervals[i]); |
|
if (diff < best_diff) { |
|
idx = i; |
|
best_diff = diff; |
|
} |
|
} |
|
fps = mt9v111_frame_intervals[idx]; |
|
|
|
/* |
|
* The sensor does not provide a PLL circuitry and pixel clock is |
|
* generated dividing the master clock source by two. |
|
* |
|
* Trow = (W + Hblank + 114) * 2 * (1 / SYSCLK) |
|
* TFrame = Trow * (H + Vblank + 2) |
|
* |
|
* FPS = (SYSCLK / 2) / (Trow * (H + Vblank + 2)) |
|
* |
|
* This boils down to tune H and V blanks to best approximate the |
|
* above equation. |
|
* |
|
* Test all available H/V blank values, until we reach the |
|
* desired frame rate. |
|
*/ |
|
best_fps = vb = hb = 0; |
|
pclk = DIV_ROUND_CLOSEST(mt9v111->sysclk, 2); |
|
row_pclk = MT9V111_PIXEL_ARRAY_WIDTH + 7 + MT9V111_CORE_R04_WIN_H_OFFS; |
|
frm_cols = MT9V111_PIXEL_ARRAY_HEIGHT + 7 + MT9V111_CORE_R03_WIN_V_OFFS; |
|
|
|
best_diff = ~0L; |
|
for (vb = MT9V111_CORE_R06_MIN_VBLANK; |
|
vb < MT9V111_CORE_R06_MAX_VBLANK; vb++) { |
|
for (hb = MT9V111_CORE_R05_MIN_HBLANK; |
|
hb < MT9V111_CORE_R05_MAX_HBLANK; hb += 10) { |
|
unsigned int t_frame = (row_pclk + hb) * |
|
(frm_cols + vb); |
|
unsigned int t_fps = DIV_ROUND_CLOSEST(pclk, t_frame); |
|
|
|
diff = abs(fps - t_fps); |
|
if (diff < best_diff) { |
|
best_diff = diff; |
|
best_fps = t_fps; |
|
|
|
if (diff == 0) |
|
break; |
|
} |
|
} |
|
|
|
if (diff == 0) |
|
break; |
|
} |
|
|
|
ret = v4l2_ctrl_s_ctrl_int64(mt9v111->hblank, hb); |
|
if (ret) |
|
return ret; |
|
|
|
ret = v4l2_ctrl_s_ctrl_int64(mt9v111->vblank, vb); |
|
if (ret) |
|
return ret; |
|
|
|
tpf->numerator = 1; |
|
tpf->denominator = best_fps; |
|
|
|
return 0; |
|
} |
|
|
|
static int mt9v111_hw_config(struct mt9v111_dev *mt9v111) |
|
{ |
|
struct i2c_client *c = mt9v111->client; |
|
unsigned int ret; |
|
u16 outfmtctrl2; |
|
|
|
/* Force device reset. */ |
|
ret = __mt9v111_hw_reset(mt9v111); |
|
if (ret == -EINVAL) |
|
ret = __mt9v111_sw_reset(mt9v111); |
|
if (ret) |
|
return ret; |
|
|
|
/* Configure internal clock sample rate. */ |
|
ret = mt9v111->sysclk < DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ? |
|
mt9v111_update(c, MT9V111_R01_CORE, |
|
MT9V111_CORE_R07_OUT_CTRL, |
|
MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 1) : |
|
mt9v111_update(c, MT9V111_R01_CORE, |
|
MT9V111_CORE_R07_OUT_CTRL, |
|
MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 0); |
|
if (ret) |
|
return ret; |
|
|
|
/* |
|
* Configure output image format components ordering. |
|
* |
|
* TODO: IFP block can also output several RGB permutations, we only |
|
* support YUYV permutations at the moment. |
|
*/ |
|
switch (mt9v111->fmt.code) { |
|
case MEDIA_BUS_FMT_YUYV8_2X8: |
|
outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC; |
|
break; |
|
case MEDIA_BUS_FMT_VYUY8_2X8: |
|
outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR; |
|
break; |
|
case MEDIA_BUS_FMT_YVYU8_2X8: |
|
outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC | |
|
MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR; |
|
break; |
|
case MEDIA_BUS_FMT_UYVY8_2X8: |
|
default: |
|
outfmtctrl2 = 0; |
|
break; |
|
} |
|
|
|
ret = mt9v111_update(c, MT9V111_R01_IFP, MT9V111_IFP_R3A_OUTFMT_CTRL2, |
|
MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK, |
|
outfmtctrl2); |
|
if (ret) |
|
return ret; |
|
|
|
/* |
|
* Do not change default sensor's core configuration: |
|
* output the whole 640x480 pixel array, skip 18 columns and 6 rows. |
|
* |
|
* Instead, control the output image size through IFP block. |
|
* |
|
* TODO: No zoom&pan support. Currently we control the output image |
|
* size only through decimation, with no zoom support. |
|
*/ |
|
ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA5_HPAN, |
|
MT9V111_IFP_DECIMATION_FREEZE); |
|
if (ret) |
|
return ret; |
|
|
|
ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA8_VPAN, |
|
MT9V111_IFP_DECIMATION_FREEZE); |
|
if (ret) |
|
return ret; |
|
|
|
ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA6_HZOOM, |
|
MT9V111_IFP_DECIMATION_FREEZE | |
|
MT9V111_PIXEL_ARRAY_WIDTH); |
|
if (ret) |
|
return ret; |
|
|
|
ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA9_VZOOM, |
|
MT9V111_IFP_DECIMATION_FREEZE | |
|
MT9V111_PIXEL_ARRAY_HEIGHT); |
|
if (ret) |
|
return ret; |
|
|
|
ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA7_HOUT, |
|
MT9V111_IFP_DECIMATION_FREEZE | |
|
mt9v111->fmt.width); |
|
if (ret) |
|
return ret; |
|
|
|
ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RAA_VOUT, |
|
mt9v111->fmt.height); |
|
if (ret) |
|
return ret; |
|
|
|
/* Apply controls to set auto exp, auto awb and timings */ |
|
ret = v4l2_ctrl_handler_setup(&mt9v111->ctrls); |
|
if (ret) |
|
return ret; |
|
|
|
/* |
|
* Set pixel integration time to the whole frame time. |
|
* This value controls the the shutter delay when running with AE |
|
* disabled. If longer than frame time, it affects the output |
|
* frame rate. |
|
*/ |
|
return mt9v111_write(c, MT9V111_R01_CORE, MT9V111_CORE_R09_PIXEL_INT, |
|
MT9V111_PIXEL_ARRAY_HEIGHT); |
|
} |
|
|
|
/* --- V4L2 subdev operations --- */ |
|
|
|
static int mt9v111_s_power(struct v4l2_subdev *sd, int on) |
|
{ |
|
struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd); |
|
int pwr_count; |
|
int ret = 0; |
|
|
|
mutex_lock(&mt9v111->pwr_mutex); |
|
|
|
/* |
|
* Make sure we're transitioning from 0 to 1, or viceversa, |
|
* before actually changing the power state. |
|
*/ |
|
pwr_count = mt9v111->pwr_count; |
|
pwr_count += on ? 1 : -1; |
|
if (pwr_count == !!on) { |
|
ret = on ? __mt9v111_power_on(sd) : |
|
__mt9v111_power_off(sd); |
|
if (!ret) |
|
/* All went well, updated power counter. */ |
|
mt9v111->pwr_count = pwr_count; |
|
|
|
mutex_unlock(&mt9v111->pwr_mutex); |
|
|
|
return ret; |
|
} |
|
|
|
/* |
|
* Update power counter to keep track of how many nested calls we |
|
* received. |
|
*/ |
|
WARN_ON(pwr_count < 0 || pwr_count > 1); |
|
mt9v111->pwr_count = pwr_count; |
|
|
|
mutex_unlock(&mt9v111->pwr_mutex); |
|
|
|
return ret; |
|
} |
|
|
|
static int mt9v111_s_stream(struct v4l2_subdev *subdev, int enable) |
|
{ |
|
struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev); |
|
int ret; |
|
|
|
mutex_lock(&mt9v111->stream_mutex); |
|
|
|
if (mt9v111->streaming == enable) { |
|
mutex_unlock(&mt9v111->stream_mutex); |
|
return 0; |
|
} |
|
|
|
ret = mt9v111_s_power(subdev, enable); |
|
if (ret) |
|
goto error_unlock; |
|
|
|
if (enable && mt9v111->pending) { |
|
ret = mt9v111_hw_config(mt9v111); |
|
if (ret) |
|
goto error_unlock; |
|
|
|
/* |
|
* No need to update control here as far as only H/VBLANK are |
|
* supported and immediately programmed to registers in .s_ctrl |
|
*/ |
|
|
|
mt9v111->pending = false; |
|
} |
|
|
|
mt9v111->streaming = enable ? true : false; |
|
mutex_unlock(&mt9v111->stream_mutex); |
|
|
|
return 0; |
|
|
|
error_unlock: |
|
mutex_unlock(&mt9v111->stream_mutex); |
|
|
|
return ret; |
|
} |
|
|
|
static int mt9v111_s_frame_interval(struct v4l2_subdev *sd, |
|
struct v4l2_subdev_frame_interval *ival) |
|
{ |
|
struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd); |
|
struct v4l2_fract *tpf = &ival->interval; |
|
unsigned int fps = tpf->numerator ? |
|
tpf->denominator / tpf->numerator : |
|
tpf->denominator; |
|
unsigned int max_fps; |
|
|
|
if (!tpf->numerator) |
|
tpf->numerator = 1; |
|
|
|
mutex_lock(&mt9v111->stream_mutex); |
|
|
|
if (mt9v111->streaming) { |
|
mutex_unlock(&mt9v111->stream_mutex); |
|
return -EBUSY; |
|
} |
|
|
|
if (mt9v111->fps == fps) { |
|
mutex_unlock(&mt9v111->stream_mutex); |
|
return 0; |
|
} |
|
|
|
/* Make sure frame rate/image sizes constraints are respected. */ |
|
if (mt9v111->fmt.width < QVGA_WIDTH && |
|
mt9v111->fmt.height < QVGA_HEIGHT) |
|
max_fps = 90; |
|
else if (mt9v111->fmt.width < CIF_WIDTH && |
|
mt9v111->fmt.height < CIF_HEIGHT) |
|
max_fps = 60; |
|
else |
|
max_fps = mt9v111->sysclk < |
|
DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ? 15 : |
|
30; |
|
|
|
if (fps > max_fps) { |
|
mutex_unlock(&mt9v111->stream_mutex); |
|
return -EINVAL; |
|
} |
|
|
|
mt9v111_calc_frame_rate(mt9v111, tpf); |
|
|
|
mt9v111->fps = fps; |
|
mt9v111->pending = true; |
|
|
|
mutex_unlock(&mt9v111->stream_mutex); |
|
|
|
return 0; |
|
} |
|
|
|
static int mt9v111_g_frame_interval(struct v4l2_subdev *sd, |
|
struct v4l2_subdev_frame_interval *ival) |
|
{ |
|
struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd); |
|
struct v4l2_fract *tpf = &ival->interval; |
|
|
|
mutex_lock(&mt9v111->stream_mutex); |
|
|
|
tpf->numerator = 1; |
|
tpf->denominator = mt9v111->fps; |
|
|
|
mutex_unlock(&mt9v111->stream_mutex); |
|
|
|
return 0; |
|
} |
|
|
|
static struct v4l2_mbus_framefmt *__mt9v111_get_pad_format( |
|
struct mt9v111_dev *mt9v111, |
|
struct v4l2_subdev_pad_config *cfg, |
|
unsigned int pad, |
|
enum v4l2_subdev_format_whence which) |
|
{ |
|
switch (which) { |
|
case V4L2_SUBDEV_FORMAT_TRY: |
|
#if IS_ENABLED(CONFIG_VIDEO_V4L2_SUBDEV_API) |
|
return v4l2_subdev_get_try_format(&mt9v111->sd, cfg, pad); |
|
#else |
|
return &cfg->try_fmt; |
|
#endif |
|
case V4L2_SUBDEV_FORMAT_ACTIVE: |
|
return &mt9v111->fmt; |
|
default: |
|
return NULL; |
|
} |
|
} |
|
|
|
static int mt9v111_enum_mbus_code(struct v4l2_subdev *subdev, |
|
struct v4l2_subdev_pad_config *cfg, |
|
struct v4l2_subdev_mbus_code_enum *code) |
|
{ |
|
if (code->pad || code->index > ARRAY_SIZE(mt9v111_formats) - 1) |
|
return -EINVAL; |
|
|
|
code->code = mt9v111_formats[code->index].code; |
|
|
|
return 0; |
|
} |
|
|
|
static int mt9v111_enum_frame_interval(struct v4l2_subdev *sd, |
|
struct v4l2_subdev_pad_config *cfg, |
|
struct v4l2_subdev_frame_interval_enum *fie) |
|
{ |
|
unsigned int i; |
|
|
|
if (fie->pad || fie->index >= ARRAY_SIZE(mt9v111_frame_intervals)) |
|
return -EINVAL; |
|
|
|
for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) |
|
if (fie->width == mt9v111_frame_sizes[i].width && |
|
fie->height == mt9v111_frame_sizes[i].height) |
|
break; |
|
|
|
if (i == ARRAY_SIZE(mt9v111_frame_sizes)) |
|
return -EINVAL; |
|
|
|
fie->interval.numerator = 1; |
|
fie->interval.denominator = mt9v111_frame_intervals[fie->index]; |
|
|
|
return 0; |
|
} |
|
|
|
static int mt9v111_enum_frame_size(struct v4l2_subdev *subdev, |
|
struct v4l2_subdev_pad_config *cfg, |
|
struct v4l2_subdev_frame_size_enum *fse) |
|
{ |
|
if (fse->pad || fse->index >= ARRAY_SIZE(mt9v111_frame_sizes)) |
|
return -EINVAL; |
|
|
|
fse->min_width = mt9v111_frame_sizes[fse->index].width; |
|
fse->max_width = mt9v111_frame_sizes[fse->index].width; |
|
fse->min_height = mt9v111_frame_sizes[fse->index].height; |
|
fse->max_height = mt9v111_frame_sizes[fse->index].height; |
|
|
|
return 0; |
|
} |
|
|
|
static int mt9v111_get_format(struct v4l2_subdev *subdev, |
|
struct v4l2_subdev_pad_config *cfg, |
|
struct v4l2_subdev_format *format) |
|
{ |
|
struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev); |
|
|
|
if (format->pad) |
|
return -EINVAL; |
|
|
|
mutex_lock(&mt9v111->stream_mutex); |
|
format->format = *__mt9v111_get_pad_format(mt9v111, cfg, format->pad, |
|
format->which); |
|
mutex_unlock(&mt9v111->stream_mutex); |
|
|
|
return 0; |
|
} |
|
|
|
static int mt9v111_set_format(struct v4l2_subdev *subdev, |
|
struct v4l2_subdev_pad_config *cfg, |
|
struct v4l2_subdev_format *format) |
|
{ |
|
struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev); |
|
struct v4l2_mbus_framefmt new_fmt; |
|
struct v4l2_mbus_framefmt *__fmt; |
|
unsigned int best_fit = ~0L; |
|
unsigned int idx = 0; |
|
unsigned int i; |
|
|
|
mutex_lock(&mt9v111->stream_mutex); |
|
if (mt9v111->streaming) { |
|
mutex_unlock(&mt9v111->stream_mutex); |
|
return -EBUSY; |
|
} |
|
|
|
if (format->pad) { |
|
mutex_unlock(&mt9v111->stream_mutex); |
|
return -EINVAL; |
|
} |
|
|
|
/* Update mbus format code and sizes. */ |
|
for (i = 0; i < ARRAY_SIZE(mt9v111_formats); i++) { |
|
if (format->format.code == mt9v111_formats[i].code) { |
|
new_fmt.code = mt9v111_formats[i].code; |
|
break; |
|
} |
|
} |
|
if (i == ARRAY_SIZE(mt9v111_formats)) |
|
new_fmt.code = mt9v111_formats[0].code; |
|
|
|
for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) { |
|
unsigned int fit = abs(mt9v111_frame_sizes[i].width - |
|
format->format.width) + |
|
abs(mt9v111_frame_sizes[i].height - |
|
format->format.height); |
|
if (fit < best_fit) { |
|
best_fit = fit; |
|
idx = i; |
|
|
|
if (fit == 0) |
|
break; |
|
} |
|
} |
|
new_fmt.width = mt9v111_frame_sizes[idx].width; |
|
new_fmt.height = mt9v111_frame_sizes[idx].height; |
|
|
|
/* Update the device (or pad) format if it has changed. */ |
|
__fmt = __mt9v111_get_pad_format(mt9v111, cfg, format->pad, |
|
format->which); |
|
|
|
/* Format hasn't changed, stop here. */ |
|
if (__fmt->code == new_fmt.code && |
|
__fmt->width == new_fmt.width && |
|
__fmt->height == new_fmt.height) |
|
goto done; |
|
|
|
/* Update the format and sizes, then mark changes as pending. */ |
|
__fmt->code = new_fmt.code; |
|
__fmt->width = new_fmt.width; |
|
__fmt->height = new_fmt.height; |
|
|
|
if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) |
|
mt9v111->pending = true; |
|
|
|
dev_dbg(mt9v111->dev, "%s: mbus_code: %x - (%ux%u)\n", |
|
__func__, __fmt->code, __fmt->width, __fmt->height); |
|
|
|
done: |
|
format->format = *__fmt; |
|
|
|
mutex_unlock(&mt9v111->stream_mutex); |
|
|
|
return 0; |
|
} |
|
|
|
static int mt9v111_init_cfg(struct v4l2_subdev *subdev, |
|
struct v4l2_subdev_pad_config *cfg) |
|
{ |
|
cfg->try_fmt = mt9v111_def_fmt; |
|
|
|
return 0; |
|
} |
|
|
|
static const struct v4l2_subdev_core_ops mt9v111_core_ops = { |
|
.s_power = mt9v111_s_power, |
|
}; |
|
|
|
static const struct v4l2_subdev_video_ops mt9v111_video_ops = { |
|
.s_stream = mt9v111_s_stream, |
|
.s_frame_interval = mt9v111_s_frame_interval, |
|
.g_frame_interval = mt9v111_g_frame_interval, |
|
}; |
|
|
|
static const struct v4l2_subdev_pad_ops mt9v111_pad_ops = { |
|
.init_cfg = mt9v111_init_cfg, |
|
.enum_mbus_code = mt9v111_enum_mbus_code, |
|
.enum_frame_size = mt9v111_enum_frame_size, |
|
.enum_frame_interval = mt9v111_enum_frame_interval, |
|
.get_fmt = mt9v111_get_format, |
|
.set_fmt = mt9v111_set_format, |
|
}; |
|
|
|
static const struct v4l2_subdev_ops mt9v111_ops = { |
|
.core = &mt9v111_core_ops, |
|
.video = &mt9v111_video_ops, |
|
.pad = &mt9v111_pad_ops, |
|
}; |
|
|
|
#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) |
|
static const struct media_entity_operations mt9v111_subdev_entity_ops = { |
|
.link_validate = v4l2_subdev_link_validate, |
|
}; |
|
#endif |
|
|
|
/* --- V4L2 ctrl --- */ |
|
static int mt9v111_s_ctrl(struct v4l2_ctrl *ctrl) |
|
{ |
|
struct mt9v111_dev *mt9v111 = container_of(ctrl->handler, |
|
struct mt9v111_dev, |
|
ctrls); |
|
int ret; |
|
|
|
mutex_lock(&mt9v111->pwr_mutex); |
|
/* |
|
* If sensor is powered down, just cache new control values, |
|
* no actual register access. |
|
*/ |
|
if (!mt9v111->pwr_count) { |
|
mt9v111->pending = true; |
|
mutex_unlock(&mt9v111->pwr_mutex); |
|
return 0; |
|
} |
|
mutex_unlock(&mt9v111->pwr_mutex); |
|
|
|
/* |
|
* Flickering control gets disabled if both auto exp and auto awb |
|
* are disabled too. If any of the two is enabled, enable it. |
|
* |
|
* Disabling flickering when ae and awb are off allows a more precise |
|
* control of the programmed frame rate. |
|
*/ |
|
if (mt9v111->auto_exp->is_new || mt9v111->auto_awb->is_new) { |
|
if (mt9v111->auto_exp->val == V4L2_EXPOSURE_MANUAL && |
|
mt9v111->auto_awb->val == V4L2_WHITE_BALANCE_MANUAL) |
|
ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP, |
|
MT9V111_IFP_R08_OUTFMT_CTRL, |
|
MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER, |
|
0); |
|
else |
|
ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP, |
|
MT9V111_IFP_R08_OUTFMT_CTRL, |
|
MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER, |
|
1); |
|
if (ret) |
|
return ret; |
|
} |
|
|
|
ret = -EINVAL; |
|
switch (ctrl->id) { |
|
case V4L2_CID_AUTO_WHITE_BALANCE: |
|
ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP, |
|
MT9V111_IFP_R06_OPMODE_CTRL, |
|
MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN, |
|
ctrl->val == V4L2_WHITE_BALANCE_AUTO ? |
|
MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN : 0); |
|
break; |
|
case V4L2_CID_EXPOSURE_AUTO: |
|
ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP, |
|
MT9V111_IFP_R06_OPMODE_CTRL, |
|
MT9V111_IFP_R06_OPMODE_CTRL_AE_EN, |
|
ctrl->val == V4L2_EXPOSURE_AUTO ? |
|
MT9V111_IFP_R06_OPMODE_CTRL_AE_EN : 0); |
|
break; |
|
case V4L2_CID_HBLANK: |
|
ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE, |
|
MT9V111_CORE_R05_HBLANK, |
|
MT9V111_CORE_R05_MAX_HBLANK, |
|
mt9v111->hblank->val); |
|
break; |
|
case V4L2_CID_VBLANK: |
|
ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE, |
|
MT9V111_CORE_R06_VBLANK, |
|
MT9V111_CORE_R06_MAX_VBLANK, |
|
mt9v111->vblank->val); |
|
break; |
|
} |
|
|
|
return ret; |
|
} |
|
|
|
static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = { |
|
.s_ctrl = mt9v111_s_ctrl, |
|
}; |
|
|
|
static int mt9v111_chip_probe(struct mt9v111_dev *mt9v111) |
|
{ |
|
int ret; |
|
u16 val; |
|
|
|
ret = __mt9v111_power_on(&mt9v111->sd); |
|
if (ret) |
|
return ret; |
|
|
|
ret = mt9v111_read(mt9v111->client, MT9V111_R01_CORE, |
|
MT9V111_CORE_RFF_CHIP_VER, &val); |
|
if (ret) |
|
goto power_off; |
|
|
|
if ((val >> 8) != MT9V111_CHIP_ID_HIGH && |
|
(val & 0xff) != MT9V111_CHIP_ID_LOW) { |
|
dev_err(mt9v111->dev, |
|
"Unable to identify MT9V111 chip: 0x%2x%2x\n", |
|
val >> 8, val & 0xff); |
|
ret = -EIO; |
|
goto power_off; |
|
} |
|
|
|
dev_dbg(mt9v111->dev, "Chip identified: 0x%2x%2x\n", |
|
val >> 8, val & 0xff); |
|
|
|
power_off: |
|
__mt9v111_power_off(&mt9v111->sd); |
|
|
|
return ret; |
|
} |
|
|
|
static int mt9v111_probe(struct i2c_client *client) |
|
{ |
|
struct mt9v111_dev *mt9v111; |
|
struct v4l2_fract tpf; |
|
int ret; |
|
|
|
mt9v111 = devm_kzalloc(&client->dev, sizeof(*mt9v111), GFP_KERNEL); |
|
if (!mt9v111) |
|
return -ENOMEM; |
|
|
|
mt9v111->dev = &client->dev; |
|
mt9v111->client = client; |
|
|
|
mt9v111->clk = devm_clk_get(&client->dev, NULL); |
|
if (IS_ERR(mt9v111->clk)) |
|
return PTR_ERR(mt9v111->clk); |
|
|
|
mt9v111->sysclk = clk_get_rate(mt9v111->clk); |
|
if (mt9v111->sysclk > MT9V111_MAX_CLKIN) |
|
return -EINVAL; |
|
|
|
mt9v111->oe = devm_gpiod_get_optional(&client->dev, "enable", |
|
GPIOD_OUT_LOW); |
|
if (IS_ERR(mt9v111->oe)) { |
|
dev_err(&client->dev, "Unable to get GPIO \"enable\": %ld\n", |
|
PTR_ERR(mt9v111->oe)); |
|
return PTR_ERR(mt9v111->oe); |
|
} |
|
|
|
mt9v111->standby = devm_gpiod_get_optional(&client->dev, "standby", |
|
GPIOD_OUT_HIGH); |
|
if (IS_ERR(mt9v111->standby)) { |
|
dev_err(&client->dev, "Unable to get GPIO \"standby\": %ld\n", |
|
PTR_ERR(mt9v111->standby)); |
|
return PTR_ERR(mt9v111->standby); |
|
} |
|
|
|
mt9v111->reset = devm_gpiod_get_optional(&client->dev, "reset", |
|
GPIOD_OUT_LOW); |
|
if (IS_ERR(mt9v111->reset)) { |
|
dev_err(&client->dev, "Unable to get GPIO \"reset\": %ld\n", |
|
PTR_ERR(mt9v111->reset)); |
|
return PTR_ERR(mt9v111->reset); |
|
} |
|
|
|
mutex_init(&mt9v111->pwr_mutex); |
|
mutex_init(&mt9v111->stream_mutex); |
|
|
|
v4l2_ctrl_handler_init(&mt9v111->ctrls, 5); |
|
|
|
mt9v111->auto_awb = v4l2_ctrl_new_std(&mt9v111->ctrls, |
|
&mt9v111_ctrl_ops, |
|
V4L2_CID_AUTO_WHITE_BALANCE, |
|
0, 1, 1, |
|
V4L2_WHITE_BALANCE_AUTO); |
|
mt9v111->auto_exp = v4l2_ctrl_new_std_menu(&mt9v111->ctrls, |
|
&mt9v111_ctrl_ops, |
|
V4L2_CID_EXPOSURE_AUTO, |
|
V4L2_EXPOSURE_MANUAL, |
|
0, V4L2_EXPOSURE_AUTO); |
|
mt9v111->hblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops, |
|
V4L2_CID_HBLANK, |
|
MT9V111_CORE_R05_MIN_HBLANK, |
|
MT9V111_CORE_R05_MAX_HBLANK, 1, |
|
MT9V111_CORE_R05_DEF_HBLANK); |
|
mt9v111->vblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops, |
|
V4L2_CID_VBLANK, |
|
MT9V111_CORE_R06_MIN_VBLANK, |
|
MT9V111_CORE_R06_MAX_VBLANK, 1, |
|
MT9V111_CORE_R06_DEF_VBLANK); |
|
|
|
/* PIXEL_RATE is fixed: just expose it to user space. */ |
|
v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops, |
|
V4L2_CID_PIXEL_RATE, 0, |
|
DIV_ROUND_CLOSEST(mt9v111->sysclk, 2), 1, |
|
DIV_ROUND_CLOSEST(mt9v111->sysclk, 2)); |
|
|
|
if (mt9v111->ctrls.error) { |
|
ret = mt9v111->ctrls.error; |
|
goto error_free_ctrls; |
|
} |
|
mt9v111->sd.ctrl_handler = &mt9v111->ctrls; |
|
|
|
/* Start with default configuration: 640x480 UYVY. */ |
|
mt9v111->fmt = mt9v111_def_fmt; |
|
|
|
/* Re-calculate blankings for 640x480@15fps. */ |
|
mt9v111->fps = 15; |
|
tpf.numerator = 1; |
|
tpf.denominator = mt9v111->fps; |
|
mt9v111_calc_frame_rate(mt9v111, &tpf); |
|
|
|
mt9v111->pwr_count = 0; |
|
mt9v111->addr_space = MT9V111_R01_IFP; |
|
mt9v111->pending = true; |
|
|
|
v4l2_i2c_subdev_init(&mt9v111->sd, client, &mt9v111_ops); |
|
|
|
#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) |
|
mt9v111->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; |
|
mt9v111->sd.entity.ops = &mt9v111_subdev_entity_ops; |
|
mt9v111->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR; |
|
|
|
mt9v111->pad.flags = MEDIA_PAD_FL_SOURCE; |
|
ret = media_entity_pads_init(&mt9v111->sd.entity, 1, &mt9v111->pad); |
|
if (ret) |
|
goto error_free_entity; |
|
#endif |
|
|
|
ret = mt9v111_chip_probe(mt9v111); |
|
if (ret) |
|
goto error_free_entity; |
|
|
|
ret = v4l2_async_register_subdev(&mt9v111->sd); |
|
if (ret) |
|
goto error_free_entity; |
|
|
|
return 0; |
|
|
|
error_free_entity: |
|
#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) |
|
media_entity_cleanup(&mt9v111->sd.entity); |
|
#endif |
|
|
|
error_free_ctrls: |
|
v4l2_ctrl_handler_free(&mt9v111->ctrls); |
|
|
|
mutex_destroy(&mt9v111->pwr_mutex); |
|
mutex_destroy(&mt9v111->stream_mutex); |
|
|
|
return ret; |
|
} |
|
|
|
static int mt9v111_remove(struct i2c_client *client) |
|
{ |
|
struct v4l2_subdev *sd = i2c_get_clientdata(client); |
|
struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd); |
|
|
|
v4l2_async_unregister_subdev(sd); |
|
|
|
#if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) |
|
media_entity_cleanup(&sd->entity); |
|
#endif |
|
|
|
v4l2_ctrl_handler_free(&mt9v111->ctrls); |
|
|
|
mutex_destroy(&mt9v111->pwr_mutex); |
|
mutex_destroy(&mt9v111->stream_mutex); |
|
|
|
return 0; |
|
} |
|
|
|
static const struct of_device_id mt9v111_of_match[] = { |
|
{ .compatible = "aptina,mt9v111", }, |
|
{ /* sentinel */ }, |
|
}; |
|
|
|
static struct i2c_driver mt9v111_driver = { |
|
.driver = { |
|
.name = "mt9v111", |
|
.of_match_table = mt9v111_of_match, |
|
}, |
|
.probe_new = mt9v111_probe, |
|
.remove = mt9v111_remove, |
|
}; |
|
|
|
module_i2c_driver(mt9v111_driver); |
|
|
|
MODULE_DESCRIPTION("V4L2 sensor driver for Aptina MT9V111"); |
|
MODULE_AUTHOR("Jacopo Mondi <[email protected]>"); |
|
MODULE_LICENSE("GPL v2");
|
|
|