diff options
-rw-r--r-- | drivers/media/i2c/Kconfig | 10 | ||||
-rw-r--r-- | drivers/media/i2c/Makefile | 1 | ||||
-rw-r--r-- | drivers/media/i2c/imx219-rmk.c | 3019 |
3 files changed, 3030 insertions, 0 deletions
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig index 8ba096b8ebca..ecc018c70cea 100644 --- a/drivers/media/i2c/Kconfig +++ b/drivers/media/i2c/Kconfig @@ -157,6 +157,16 @@ config VIDEO_IMX219 To compile this driver as a module, choose M here: the module will be called imx219. +config VIDEO_IMX219_RMK + tristate "Sony IMX219 sensor support" + depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API + help + This is a Video4Linux2 sensor-level driver for the Sony IMX219 + camera. + + To compile this driver as a module, choose M here: the + module will be called imx219-rmk. + config VIDEO_IMX258 tristate "Sony IMX258 sensor support" select V4L2_CCI_I2C diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile index fbb988bd067a..e9758c60fc28 100644 --- a/drivers/media/i2c/Makefile +++ b/drivers/media/i2c/Makefile @@ -47,6 +47,7 @@ obj-$(CONFIG_VIDEO_HI847) += hi847.o obj-$(CONFIG_VIDEO_I2C) += video-i2c.o obj-$(CONFIG_VIDEO_IMX208) += imx208.o obj-$(CONFIG_VIDEO_IMX214) += imx214.o +obj-$(CONFIG_VIDEO_IMX219_RMK) += imx219-rmk.o obj-$(CONFIG_VIDEO_IMX219) += imx219.o obj-$(CONFIG_VIDEO_IMX258) += imx258.o obj-$(CONFIG_VIDEO_IMX274) += imx274.o diff --git a/drivers/media/i2c/imx219-rmk.c b/drivers/media/i2c/imx219-rmk.c new file mode 100644 index 000000000000..8c6589e46084 --- /dev/null +++ b/drivers/media/i2c/imx219-rmk.c @@ -0,0 +1,3019 @@ +/* + * Sony IMX219 sensor driver + * + * This is almost SMIA compliant, but most of the identifying locations + * are used for different purposes, which makes reuse of SMIA particularly + * difficult. The sensor has two separate PLLs, one for video timing and + * one for the output stage. Also, the registers used to program the + * output format are located in a completely different place. + * + * Notes: + * 1. With iMX6, use of the CSI compander results in severe horizontal + * pixelation of the image, resulting in an unusable image. + * 2. 3200x2200 doesn't work in 2-lane mode as we run out of time to + * transmit a line of pixels. Increasing LINE_LENGTH doesn't appear + * to solve this, despite there being ample bus idle time per frame. + * + * INCK -+-> PREDIV ------> PLL ---------+-> DIV ---> CONTROL + * 12A | VT:304(1/2/3) VT:306[10:0] | SY:303(1) + * | `-> DIV ---> PIPELINE + * | PX:301(4,5,8,10) + * `-> PREDIV ------> PLL ---------+----------> MIPI output + * OP:305 (1/2/3) OP:30C[10:0] | + * +-> DIV ---> FIFO + * | SY:30B(1=1/2) + * `-> DIV ---> + * PX:309(8/10) + * + * 2 lane 4 lane + * VT: 304=3 306=57 303=1 301=5 304=3 306=87 303=1 301=5 + * vt = input / 3 * 57 = 456MHz = input / 3 * 87 = 696MHz + * vt_sys = vt / 1 => 456MHz = vt / 1 => 696MHz + * vt_pix = vt / 5 => 91.2MHz = vt / 5 => 139.2MHz + * OP: 305=3 30C=114 30b=1 309=10 305=3 30C=90 30B=1 309=10 + * op = input / 3 * 114 = 912MHz = input / 3 * 90 = 720MHz + * op_sys = op / 1 => 912MHz = op / 1 => 720MHz + * op_pix = op / 10 => 91.2MHz = op / 10 => 72.0MHz + * + * frame_length = max(coarse_integration_time + 4, frame_length_lines) + * line_length_pck = max(3448, ...) + * + * frame_rate = line_rate / frame_length + * line_rate = 2 * vt_pix / line_length_pck + *=== + * line_rate = frame_rate * frame_length + * vt_pix = line_rate * line_length_pck + */ +#include <linux/clk.h> +#include <linux/err.h> +#include <linux/gpio/consumer.h> +#include <linux/i2c.h> +#include <linux/module.h> +#include <linux/of.h> +#include <linux/of_graph.h> +#include <linux/pm_runtime.h> +#include <linux/delay.h> + +#include <media/v4l2-ctrls.h> +#include <media/v4l2-device.h> +#include <media/v4l2-fwnode.h> +#include <media/v4l2-subdev.h> + +enum { +#define REG(addr, size) (addr) + R_MODEL_ID = REG(0x0000, 2), + R_LOT_ID = REG(0x0002, 3), + R_WAFER_NUM = REG(0x0006, 1), + R_CHIP_NUM = REG(0x0007, 2), + R_FRM_CNT = REG(0x0018, 1), + R_PX_ORDER = REG(0x0019, 1), + R_DT_PEDESTAL = REG(0x001a, 2), + R_FRM_FMT_TYPE = REG(0x0040, 1), + FMT_TYPE_2BYTE = 1, + R_FRM_FMT_SUBTYPE = REG(0x0041, 1), +#define FRM_FMT_COLS(x) ((x) >> 4) +#define FRM_FMT_ROWS(x) ((x) & 0x0f) + R_FRM_FMT_DESC = REG(0x0042, 2), +#define R_FRM_FMT_DESC(x) (R_FRM_FMT_DESC + 2 * (x)) +#define FRM_FMT_DESC_CODE(x) ((x) >> 12) + FRM_FMT_DESC_EMBEDDED = 1, + FRM_FMT_DESC_VISIBLE = 5, +#define FRM_FMT_DESC_VAL(x) ((x) & 0x0fff) + R_ANA_GAIN_CAP = REG(0x0081, 1), + R_ANA_GAIN_MIN = REG(0x0085, 1), + R_ANA_GAIN_MAX = REG(0x0086, 2), + R_ANA_GAIN_STEP = REG(0x0088, 2), + R_ANA_GAIN_M0 = REG(0x008c, 2), + R_ANA_GAIN_C0 = REG(0x008e, 2), + R_ANA_GAIN_M1 = REG(0x0090, 2), + R_ANA_GAIN_C1 = REG(0x0092, 2), + R_DT_FMT_TYPE = REG(0x00c0, 1), + R_DT_FMT_SUBTYPE = REG(0x00c1, 1), + R_DT_FMT_DESC = REG(0x00c2, 2), +#define R_DT_FMT_DESC(x) (R_DT_FMT_DESC + 2 * (x)) + R_MODE_SELECT = REG(0x0100, 1), + R_SW_RESET = REG(0x0103, 1), + R_CORRUPT_STATUS = REG(0x0104, 1), + R_MASK_CORRUPT_FRAMES = REG(0x0105, 1), + R_FAST_STANDBY = REG(0x0106, 1), + R_CSI_CH_ID = REG(0x0110, 1), + R_CSI_SIG_MODE = REG(0x0111, 1), + R_CSI_LANE_MODE = REG(0x0114, 1), + R_TCLK_POST = REG(0x0118, 2), + R_THS_PREPARE = REG(0x011a, 2), + R_THS_ZERO_MIN = REG(0x011c, 2), + R_THS_TRAIL = REG(0x011e, 2), + R_TCLK_TRAIL_MIN = REG(0x0120, 2), + R_TCLK_PREPARE = REG(0x0122, 2), + R_TCLK_ZERO = REG(0x0124, 2), + R_TLPX = REG(0x0126, 2), + R_DPHY_CTRL = REG(0x0128, 1), + R_EXCK_FREQ = REG(0x012a, 2), + R_TEMPERATURE = REG(0x0140, 1), + R_READOUT_V_CNT = REG(0x0142, 2), + R_VSYNC_POL = REG(0x0144, 1), + R_FLASH_POL = REG(0x0146, 1), + R_VSYNC_TYPE = REG(0x0147, 1), + R_FRAME_BANK_CTRL = REG(0x0150, 1), + R_FRAME_BANK_FRM_CNT = REG(0x0151, 1), + R_FRAME_BANK_FAST_TRACKING = REG(0x0152, 1), + R_FRAME_DURATION_A = REG(0x0154, 1), + R_COMP_ENABLE_A = REG(0x0155, 1), + R_ANA_GAIN_GLOBAL_A = REG(0x0157, 1), + R_DIG_GAIN_GLOBAL_A = REG(0x0158, 2), + R_COARSE_INTEGRATION_A = REG(0x015a, 2), + R_SENSOR_MODE_A = REG(0x015d, 1), + R_FRM_LENGTH_A = REG(0x0160, 2), + R_LINE_LENGTH_A = REG(0x0162, 2), + R_X_ADD_STA_A = REG(0x0164, 2), + R_X_ADD_END_A = REG(0x0166, 2), + R_Y_ADD_STA_A = REG(0x0168, 2), + R_Y_ADD_END_A = REG(0x016a, 2), + R_X_OUTPUT_SIZE = REG(0x016c, 2), + R_Y_OUTPUT_SIZE = REG(0x016e, 2), + R_X_ODD_INC_A = REG(0x0170, 1), + R_Y_ODD_INC_A = REG(0x0171, 1), + R_IMG_ORIENTATION_A = REG(0x0172, 1), + R_BINNING_MODE_H_A = REG(0x0174, 1), + R_BINNING_MODE_V_A = REG(0x0175, 1), + R_BINNING_CAL_MODE_H_A = REG(0x0176, 1), + R_BINNING_CAL_MODE_V_A = REG(0x0177, 1), + R_ANA_GAIN_GLOBAL_SHORT_A = REG(0x0189, 1), + R_COARSE_INTEG_TIME_SHORT_A = REG(0x018a, 2), + R_CSI_DATA_FORMAT_A = REG(0x018c, 2), + R_VTPXCK_DIV = REG(0x0301, 1), + R_VTSYCK_DIV = REG(0x0303, 1), + R_PREPLLCK_VT_DIV = REG(0x0304, 1), + R_PREPLLCK_OP_DIV = REG(0x0305, 1), + R_PLL_VT_MPY = REG(0x0306, 2), + R_OPPXCK_DIV = REG(0x0309, 1), + R_OPSYCK_DIV = REG(0x030b, 1), + R_PLL_OP_MPY = REG(0x030c, 2), + R_X_EVN_INC = REG(0x0381, 1), + R_Y_EVN_INC = REG(0x0383, 1), + R_FINE_INTEG_TIME = REG(0x0388, 2), + R_TEST_PATTERN_MODE = REG(0x0600, 2), + R_TD_R = REG(0x0602, 2), + R_TD_GR = REG(0x0604, 2), + R_TD_B = REG(0x0606, 2), + R_TD_GB = REG(0x0608, 2), + R_H_CUR_WIDTH = REG(0x060a, 2), + R_H_CUR_POS = REG(0x060c, 2), + R_V_CUR_WIDTH = REG(0x060e, 2), + R_V_CUR_POS = REG(0x0610, 2), + R_TP_WINDOW_X_OFFSET = REG(0x0620, 2), + R_TP_WINDOW_Y_OFFSET = REG(0x0622, 2), + R_TP_WINDOW_WIDTH = REG(0x0624, 2), + R_TP_WINDOW_HEIGHT = REG(0x0626, 2), + R_LIMIT_INTEG = REG(0x1000, 0), + R_LIMIT_DIG_GAIN = REG(0x1080, 0), + R_LIMIT_PLL = REG(0x1100, 0), + R_LIMIT_VTCLK = REG(0x1120, 0), + R_LIMIT_FRAME = REG(0x1140, 0), + R_LIMIT_OPCLK = REG(0x1160, 0), + R_LIMIT_IMAGE = REG(0x1180, 0), +#undef REG +}; + +/* These represent the register layouts above */ +struct imx219_image_params { /* R_FRM_LENGTH_A to R_BINNING_CAL_MODE_V_A */ + __be16 frame_length; + __be16 line_length; + __be16 x_add_sta; + __be16 x_add_end; + __be16 y_add_sta; + __be16 y_add_end; + __be16 x_output_size; + __be16 y_output_size; + u8 x_odd_inc; + u8 y_odd_inc; + u8 image_orientation; + u8 zero; + u8 binning_mode_h; + u8 binning_mode_v; + u8 binning_cal_mode_h; + u8 binning_cal_mode_v; +} __packed __aligned(2); + +struct imx219_limit_integration_time { /* R_LIMIT_INTEG */ + __be16 integration_time_capability; + __be16 reserved; + __be16 coarse_integration_time_min; + __be16 coarse_integration_time_max_margin; +} __packed __aligned(2); + +struct imx219_limit_dig_gain { /* R_LIMIT_DIG_GAIN */ + __be16 digital_gain_capability; + u8 reserved2; + u8 reserved3; + __be16 digital_gain_min; + __be16 digital_gain_max; + __be16 digital_gain_step_size; +} __packed __aligned(2); + +struct imx219_limit_pll { /* R_LIMIT_PLL */ + __be32 min_ext_clk_freq_mhz; /* float */ + __be32 max_ext_clk_freq_mhz; /* float */ + __be16 min_pre_pll_clk_div; + __be16 max_pre_pll_clk_div; + __be32 min_pll_ip_freq_mhz; /* float */ + __be32 max_pll_ip_freq_mhz; /* float */ + __be16 min_pll_multiplier; + __be16 max_pll_multiplier; + __be32 min_pll_op_freq_mhz; /* float */ + __be32 max_pll_op_freq_mhz; /* float */ +}; + +struct imx219_limit_clk { /* R_LIMIT_OPCLK or R_LIMIT_VTCLK */ + __be16 min_sys_clk_div; + __be16 max_sys_clk_div; + __be32 min_sys_clk_freq_mhz; /* float */ + __be32 max_sys_clk_freq_mhz; /* float */ + __be32 min_pix_clk_freq_mhz; /* float */ + __be32 max_pix_clk_freq_mhz; /* float */ + __be16 min_pix_clk_div; + __be16 max_pix_clk_div; +} __packed __aligned(4); + +struct imx219_limit_frame { /* R_LIMIT_FRAME */ + __be16 min_frame_length_lines; + __be16 max_frame_length_lines; + __be16 min_line_length_pck; + __be16 max_line_length_pck; + __be16 min_link_blank_pck; + __be16 min_frame_blank_lines; +} __packed __aligned(2); + +struct imx219_limit_image { /* R_LIMIT_IMAGE */ + __be16 x_addr_min; + __be16 y_addr_min; + __be16 x_addr_max; + __be16 y_addr_max; + __be16 min_x_output_size; + __be16 min_y_output_size; + __be16 max_x_output_size; + __be16 max_y_output_size; +} __packed __aligned(2); + +enum { + CTRL_R_R = 0, + CTRL_R_GR, + CTRL_R_B, + CTRL_R_GB, + N_R_CTRLS, + + CTRL_P_PIXEL_RATE = 0, + CTRL_P_HBLANK, + CTRL_P_VBLANK, + CTRL_P_EXPOSURE, + N_P_CTRLS = N_R_CTRLS, + N_CTRLS = N_R_CTRLS > N_P_CTRLS ? N_R_CTRLS : N_P_CTRLS, + + PLL_VT = 0, + PLL_OP, + N_PLLS, + + CLK_SYS = 0, + CLK_PIX, + N_CLKS, + + PAD_SRC = 0, + PAD_SINK, + N_PADS, +}; + +struct imx219_private; +struct imx219_sub; + +struct imx219_pad_ops { + void (*init_pad)(struct imx219_private *priv, unsigned int pad, + struct v4l2_mbus_framefmt *mf, + struct v4l2_rect *crop); + void (*adj_format)(struct imx219_private *priv, + struct v4l2_subdev_format *fmt, + const struct v4l2_mbus_framefmt *sink_fmt, + const struct v4l2_rect *sink_compose); + int (*set_fmt)(struct imx219_private *priv, struct imx219_sub *sub, + unsigned int pad, const struct v4l2_rect *r, + const struct v4l2_mbus_framefmt *fmt); + int (*adj_selection)(struct imx219_private *priv, unsigned int target, + unsigned int flags, struct v4l2_rect *rect, + struct v4l2_rect * const this[]); + int (*set_selection)(struct imx219_private *priv, + struct imx219_sub *sub, unsigned int pad, + unsigned int target, const struct v4l2_rect *rect, + const struct v4l2_rect *upstream); +}; + +struct imx219_sub { + struct v4l2_subdev sd; + struct v4l2_ctrl_handler ctrl_handler; + struct media_pad pad[N_PADS]; + const struct imx219_pad_ops *pad_ops; + struct v4l2_ctrl *ctrls[N_CTRLS]; + + /* mutex-protected data */ + struct v4l2_mbus_framefmt format[N_PADS]; + struct v4l2_rect sink_crop; + struct v4l2_rect sink_compose; +}; + +struct imx219_private { + /* v4l2 data */ + struct imx219_sub root; + struct imx219_sub pixel; + + /* hardware resources */ + struct clk *inck; + struct gpio_desc *xclr_gpio; + + /* static settings */ + struct v4l2_rect visible; + struct v4l2_rect pixarray; + u8 mipi_vc; + u8 mipi_lanes; + u8 pixel_order; + + u32 pixel_format; + + /* parameters / limits */ + s16 ana_gain_m0; + s16 ana_gain_c0; + s16 ana_gain_m1; + s16 ana_gain_c1; + u16 coarse_integration_time_min; + u16 coarse_integration_time_max_margin; + u16 min_frame_lines; + u16 max_frame_lines; + u16 min_line_length_pck; + u16 max_line_length_pck; + u16 min_line_blank_pck; + u16 min_frame_blank_lines; + u16 min_output_width; + u16 min_output_height; + u16 max_output_width; + u16 max_output_height; + + /* not really a limit, but static on imx219 */ + u16 fine_integ_time; + + /* calculated limits */ + u32 inck_freq; + u32 pll_ip_freq; + u32 pll_min_freq; + u32 pll_max_freq; + u16 pll_min_mpy; + u16 pll_max_mpy; + u8 pll_pre_div; + + struct imx219_pll_limit { + u32 min_freq; + u32 max_freq; + u16 min_mpy; + u16 max_mpy; + struct imx219_clk_limit { + u32 min_freq; + u32 max_freq; + u16 min_div; + u16 max_div; + } clk[N_CLKS]; + } pll[N_PLLS]; + + struct mutex mutex; + /* mutex-protected data */ + struct v4l2_fract frame_interval; + bool streaming; + + struct imx219_image_params params; + + u16 vt_mpy; + u16 op_mpy; + u8 op_sys_div; + u8 bits_per_pixel; +}; + +static const u32 imx219_formats[][4] = { + /* IMX216 can produce either RAW10 or RAW8 MIPI CSI2 format */ +#define FMT_S(o,b) MEDIA_BUS_FMT_S##o##b##_1X##b +#define FMT(b) FMT_S(GRBG, b), FMT_S(RGGB, b), FMT_S(BGGR, b), FMT_S(GBRG, b) + { FMT(10) }, + { FMT(8) }, +#undef FMT +#undef FMT_S +}; + +static const char *imx219_test_pattern_menu[] = { + "Disabled", + "Solid Color", + "100% Color Bars", + "Fade to Grey Color Bar", + "PN9", + "16 Split Color Bar", + "16 Split Inverted Color Bar", + "Column Counter", + "Inverted Column Counter", + "PN31", +}; + +struct imx219_reg { + u16 offset; + u8 val; +}; + +static const struct imx219_reg misc_regs[] = { + { 0x30eb, 0x05 }, { 0x30eb, 0x0c }, + { 0x300a, 0xff }, { 0x300b, 0xff }, + { 0x30eb, 0x05 }, { 0x30eb, 0x09 }, + { 0x455e, 0x00 }, { 0x471e, 0x4b }, + { 0x4767, 0x0f }, { 0x4750, 0x14 }, + { 0x4540, 0x00 }, { 0x47b4, 0x14 }, + { 0x4713, 0x30 }, { 0x478b, 0x10 }, + { 0x478f, 0x10 }, { 0x4793, 0x10 }, + { 0x4797, 0x0e }, { 0x479b, 0x0e } +}; + +static struct imx219_private *imx219_get_root_priv(struct v4l2_subdev *sd) +{ + return container_of(sd, struct imx219_private, root.sd); +} + +static struct imx219_sub *imx219_get_sub(struct v4l2_subdev *sd) +{ + return container_of(sd, struct imx219_sub, sd); +} + +static struct imx219_private *imx219_get_priv(struct v4l2_subdev *sd) +{ + return imx219_get_root_priv(dev_get_drvdata(sd->dev)); +} + +static struct imx219_private *imx219_get_clientdata(struct i2c_client *client) +{ + return imx219_get_root_priv(i2c_get_clientdata(client)); +} + +static int imx219_read(struct imx219_private *priv, unsigned int reg, + void *val, size_t n) +{ + struct i2c_client *client = v4l2_get_subdevdata(&priv->root.sd); + __be16 addr = cpu_to_be16(reg); + int ret; + struct i2c_msg msg[] = { + { + .addr = client->addr, + .len = 2, + .buf = (u8 *)&addr, + }, { + .addr = client->addr, + .flags = I2C_M_RD, + .len = n, + .buf = val, + } + }; + + ret = i2c_transfer(client->adapter, msg, 2); + if (ret != 2) + v4l_err(client, "read reg 0x%04x failed: %d\n", reg, ret); + + return ret == 2 ? 0 : ret < 0 ? ret : -EINVAL; +} + +static int imx219_readb(struct imx219_private *priv, unsigned int reg) +{ + u8 val; + int ret = imx219_read(priv, reg, &val, sizeof(val)); + + return ret < 0 ? ret : val; +} + +static int imx219_readw(struct imx219_private *priv, unsigned int reg) +{ + __be16 val; + int ret = imx219_read(priv, reg, &val, sizeof(val)); + + return ret < 0 ? ret : be16_to_cpu(val); +} + +static int imx219_write(struct imx219_private *priv, unsigned int reg, + const void *v, size_t n) +{ + struct i2c_client *client = v4l2_get_subdevdata(&priv->root.sd); + u8 buf[32]; + int ret; + struct i2c_msg msg[] = { + { + .addr = client->addr, + .len = 2 + n, + .buf = buf, + } + }; + + if (n > sizeof(buf) - 2) + return -EIO; + + buf[0] = reg >> 8; + buf[1] = reg; + memcpy(buf + 2, v, n); + + ret = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg)); + if (ret != ARRAY_SIZE(msg)) + v4l_err(client, "write reg 0x%04x failed: %d\n", reg, ret); + + return ret < 0 ? ret : ret != 1 ? -EINVAL : 0; +} + +static int imx219_writeb(struct imx219_private *priv, unsigned int reg, u8 val) +{ + return imx219_write(priv, reg, &val, sizeof(val)); +} + +static int imx219_writeb_pm(struct imx219_private *priv, unsigned int reg, + u16 val) +{ + struct device *dev = priv->root.sd.dev; + int pm = pm_runtime_get_if_in_use(dev); + int ret = 0; + + if (pm) + ret = imx219_writeb(priv, reg, val); + + if (pm > 0) { + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + } + return ret; +} + +static int imx219_writew(struct imx219_private *priv, unsigned int reg, u16 val) +{ + __be16 v = cpu_to_be16(val); + return imx219_write(priv, reg, &v, sizeof(v)); +} + +static int imx219_writew_pm(struct imx219_private *priv, unsigned int reg, + u16 val) +{ + struct device *dev = priv->root.sd.dev; + int pm = pm_runtime_get_if_in_use(dev); + int ret = 0; + + if (pm) + ret = imx219_writew(priv, reg, val); + + if (pm > 0) { + pm_runtime_mark_last_busy(dev); + pm_runtime_put_autosuspend(dev); + } + return ret; +} + +static int imx219_write_array(struct imx219_private *priv, + const struct imx219_reg *regs, size_t num) +{ + int ret = 0; + + while (num--) { + ret = imx219_writeb(priv, regs->offset, regs->val); + if (ret < 0) + break; + regs++; + } + + return ret; +} + +static u32 imx219_lookup_format(u32 code, unsigned int order) +{ + int i, j; + + for (i = ARRAY_SIZE(imx219_formats); --i; ) + for (j = 0; j < ARRAY_SIZE(imx219_formats[i]); j++) + if (code == imx219_formats[i][j]) + goto out; +out: + return imx219_formats[i][order]; +} + +/* Convert a big-endian 32-bit float to a u32 scaled by 1M */ +static u32 be32f_to_u32m(__be32 v_float) +{ + u32 v = be32_to_cpu(v_float); + int exponent; + u64 mantissa; + + if (v & BIT(31) || v == 0) /* -ve or 0 */ + return 0; + if (v == 0x7f800000) /* inf */ + return ~0; + if ((v & 0x7f800000) == 0x7f800000) /* nan */ + return 0; + if (v > 0x458637bd) /* maximum - limits shift */ + return ~0; + if (v < 0x358637bd) /* minimum - limits shift */ + return 0; + exponent = (v >> 23) - 127; + mantissa = 1000000ull * ((v & 0x7fffff) | 0x800000); + if (exponent < 0) + mantissa >>= -exponent; + else + mantissa <<= exponent; + return mantissa >> 23; +} + +static int imx219_check_limit(struct device *dev, unsigned long v, + u32 min_v, u32 max_v, const char *what, + const char *unit) +{ + if (min_v >= max_v) { + dev_err(dev, "%s range %u-%u%s invalid\n", + what, min_v, max_v, unit); + return -EINVAL; + } + if (v < min_v || v > max_v) { + dev_err(dev, "%s %lu%s outside range %u-%u%s\n", + what, v, unit, min_v, max_v, unit); + return -EINVAL; + } + + return 0; +} + +#define MHZ(x) ((x) / 1000000), (((x) / 100000) % 10) +static int imx219_calculate_clk_limits(struct imx219_private *priv, + const struct imx219_limit_clk *cl, + struct imx219_pll_limit *pl, + const char *what, bool op) +{ + struct device *dev = priv->root.sd.dev; + struct { + u32 min_freq, max_freq; + u16 min_div, max_div; + int dep_clk; + } limits[N_CLKS]; + u32 min_v, max_v; + u64 v; + int i; + + limits[CLK_SYS].min_freq = be32f_to_u32m(cl->min_sys_clk_freq_mhz); + limits[CLK_SYS].max_freq = be32f_to_u32m(cl->max_sys_clk_freq_mhz); + limits[CLK_SYS].min_div = be16_to_cpu(cl->min_sys_clk_div); + limits[CLK_SYS].max_div = be16_to_cpu(cl->max_sys_clk_div); + limits[CLK_PIX].min_freq = be32f_to_u32m(cl->min_pix_clk_freq_mhz); + limits[CLK_PIX].max_freq = be32f_to_u32m(cl->max_pix_clk_freq_mhz); + limits[CLK_PIX].min_div = be16_to_cpu(cl->min_pix_clk_div); + limits[CLK_PIX].max_div = be16_to_cpu(cl->max_pix_clk_div); + + limits[CLK_SYS].dep_clk = -1; + limits[CLK_PIX].dep_clk = op - 1; + + for (i = 0; i < N_CLKS; i++) + dev_dbg(dev, "%s_%s_clk limits %u.%u-%u.%uMHz div %04x-%04x\n", + what, i ? "pix" : "sys", + MHZ(limits[i].min_freq), MHZ(limits[i].max_freq), + limits[i].min_div, limits[i].max_div); + + /* + * Calculate the resulting minimum and maximum divisor input + * frequencies that can be generated to satisfy the divisor + * input and output limits. + * + * The VT setup appears to be: + * PLL -> sys divider -> sysclk output + * `-> pix divider -> pixclk output + * + * and OP setup appears to be: + * PLL -> sys divider -> sysclk output + * `-> pix divider -> pixclk output + */ + pl->min_freq = priv->pll_min_freq; + pl->max_freq = priv->pll_max_freq; + for (i = N_CLKS; i--; ) { + u32 *min_f, *max_f; + + if (limits[i].dep_clk == -1) { + min_f = &pl->min_freq; + max_f = &pl->max_freq; + } else { + min_f = &limits[limits[i].dep_clk].min_freq; + max_f = &limits[limits[i].dep_clk].max_freq; + } + v = limits[i].min_freq * limits[i].min_div; + if (v > *min_f) + *min_f = v; + v = limits[i].max_freq * limits[i].max_div; + if (v < *max_f) + *max_f = v; + } + + if (pl->max_freq < pl->min_freq) { + dev_err(dev, "inconsistent %s PLL parameters: %u.%u-%u.%uMHz\n", + what, MHZ(pl->min_freq), MHZ(pl->max_freq)); + return -EINVAL; + } + + /* + * Update the PLL frequency range according to the resolution of the + * PLL itself. The minimum will always be larger than the PLL's + * minimum multiplier, and the maximum will always be smaller than + * the PLL's maximum multiplier. + */ + pl->min_mpy = DIV_ROUND_UP(pl->min_freq, priv->pll_ip_freq); + pl->max_mpy = pl->max_freq / priv->pll_ip_freq; + + /* Re-calculate the exact PLL frequencies for these multipliers */ + pl->min_freq = priv->pll_ip_freq * pl->min_mpy; + pl->max_freq = priv->pll_ip_freq * pl->max_mpy; + + dev_info(dev, "%s PLL %u.%u-%u.%uMHz for mpy %04x-%04x\n", + what, MHZ(pl->min_freq), MHZ(pl->max_freq), + pl->min_mpy, pl->max_mpy); + + /* Now calculate the resulting min/max output clocks and divisors. */ + for (i = 0; i < N_CLKS; i++) { + u32 *min_f, *max_f; + + if (limits[i].dep_clk == -1) { + min_f = &pl->min_freq; + max_f = &pl->max_freq; + } else { + min_f = &pl->clk[limits[i].dep_clk].min_freq; + max_f = &pl->clk[limits[i].dep_clk].max_freq; + } + + min_v = *min_f / limits[i].max_div; + if (min_v >= limits[i].min_freq) { + pl->clk[i].min_freq = min_v; + pl->clk[i].max_div = limits[i].max_div; + } else { + pl->clk[i].max_div = DIV_ROUND_UP(pl->min_freq, + limits[i].min_freq); + pl->clk[i].min_freq = pl->min_freq / pl->clk[i].max_div; + } + + max_v = *max_f / limits[i].min_div; + if (max_v <= limits[i].max_freq) { + pl->clk[i].max_freq = max_v; + pl->clk[i].min_div = limits[i].min_div; + } else { + pl->clk[i].min_div = pl->max_freq / limits[i].max_freq; + pl->clk[i].max_freq = pl->max_freq / pl->clk[i].min_div; + } + + dev_info(dev, "%s_%s_clk %u.%u-%u.%uMHz for div %04x-%04x\n", + what, i ? "pix" : "sys", + MHZ(pl->clk[i].min_freq), MHZ(pl->clk[i].max_freq), + pl->clk[i].max_div, pl->clk[i].min_div); + } + return 0; +} + +static int imx219_calculate_clks_limits(struct imx219_private *priv) +{ + struct imx219_limit_clk lc; + struct imx219_pll_limit *pll; + int ret; + + ret = imx219_read(priv, R_LIMIT_VTCLK, &lc, sizeof(lc)); + if (ret < 0) + return ret; + + pll = &priv->pll[PLL_VT]; + + imx219_calculate_clk_limits(priv, &lc, pll, "vt", false); + + if (priv->pixel.ctrls[CTRL_P_PIXEL_RATE]) + v4l2_ctrl_modify_range(priv->pixel.ctrls[CTRL_P_PIXEL_RATE], + 2 * pll->clk[CLK_PIX].min_freq, + 2 * pll->clk[CLK_PIX].max_freq, + 1, 2 * pll->clk[CLK_PIX].max_freq); + + ret = imx219_read(priv, R_LIMIT_OPCLK, &lc, sizeof(lc)); + if (ret < 0) + return ret; + + imx219_calculate_clk_limits(priv, &lc, &priv->pll[PLL_OP], "op", true); + + /* Set defaults. FIXME: we should recalculate these parameters */ + priv->vt_mpy = priv->pll[PLL_VT].max_mpy; + + /* + * Configure the PLL multipliers according to the number of lanes. + * Documented maximum lane Mbps is 912 for 2-lane, 755 for 4-lane. + */ + if (priv->mipi_lanes == 2) + priv->op_mpy = priv->pll[PLL_OP].max_mpy; + else + priv->op_mpy = 0x005a; + + priv->op_sys_div = priv->pll[PLL_OP].clk[CLK_SYS].min_div; + + return 0; +} + +static int imx219_calculate_pll_limits(struct imx219_private *priv, + unsigned long inck) +{ + struct device *dev = priv->root.sd.dev; + struct imx219_limit_pll limit_pll; + u16 min_pll_mpy, max_pll_mpy, pre_div; + u32 pll_ip_freq, min_v, max_v; + u64 v; + int ret; + + if (priv->inck_freq == inck) + return 0; + + ret = imx219_read(priv, R_LIMIT_PLL, &limit_pll, sizeof(limit_pll)); + if (ret < 0) + return ret; + + min_v = be32f_to_u32m(limit_pll.min_ext_clk_freq_mhz); + max_v = be32f_to_u32m(limit_pll.max_ext_clk_freq_mhz); + ret = imx219_check_limit(dev, inck, min_v, max_v, + "external frequency", "Hz"); + if (ret < 0) + return ret; + + min_v = __be16_to_cpu(limit_pll.min_pre_pll_clk_div); + max_v = __be16_to_cpu(limit_pll.max_pre_pll_clk_div); + ret = imx219_check_limit(dev, min_v, min_v, max_v, + "pre-pll divider", ""); + + /* + * Set the PLL pre-divider to give 6-12MHz input to the PLLs. + */ + pre_div = clamp((u32)(1 + inck / 12000000), min_v, max_v); + pll_ip_freq = inck / pre_div; + + min_v = be32f_to_u32m(limit_pll.min_pll_ip_freq_mhz); + max_v = be32f_to_u32m(limit_pll.max_pll_ip_freq_mhz); + ret = imx219_check_limit(dev, pll_ip_freq, min_v, max_v, + "pll input frequency", "Hz"); + if (ret < 0) + return ret; + + min_pll_mpy = be16_to_cpu(limit_pll.min_pll_multiplier); + max_pll_mpy = be16_to_cpu(limit_pll.max_pll_multiplier); + ret = imx219_check_limit(dev, min_pll_mpy, min_pll_mpy, max_pll_mpy, + "pll multiplier", ""); + if (ret < 0) + return ret; + + min_v = be32f_to_u32m(limit_pll.min_pll_op_freq_mhz); + max_v = be32f_to_u32m(limit_pll.max_pll_op_freq_mhz); + ret = imx219_check_limit(dev, min_v, min_v, max_v, + "pll output frequency", "Hz"); + if (ret < 0) + return ret; + + /* Everything looks good */ + priv->inck_freq = inck; + priv->pll_ip_freq = pll_ip_freq; + priv->pll_pre_div = pre_div; + + /* Calculate the real PLL limits given the input clock */ + v = (u64)pll_ip_freq * min_pll_mpy; + if (v < min_v) { + priv->pll_min_mpy = DIV_ROUND_UP(min_v, pll_ip_freq); + priv->pll_min_freq = pll_ip_freq * priv->pll_min_mpy; + } else { + priv->pll_min_mpy = min_pll_mpy; + priv->pll_min_freq = v; + } + + v = (u64)pll_ip_freq * max_pll_mpy; + if (v > max_v) { + priv->pll_max_mpy = max_v / pll_ip_freq; + priv->pll_max_freq = pll_ip_freq * priv->pll_max_mpy; + } else { + priv->pll_max_mpy = max_pll_mpy; + priv->pll_max_freq = v; + } + + dev_info(dev, "Inck %u.%uMHz PLL in %u.%uMHz out %u.%u-%u.%uMHz mpy %04x-%04x\n", + MHZ(priv->inck_freq), MHZ(priv->pll_ip_freq), + MHZ(priv->pll_min_freq), MHZ(priv->pll_max_freq), + priv->pll_min_mpy, priv->pll_max_mpy); + + return imx219_calculate_clks_limits(priv); +} +#undef MHZ + +/* + * Calculate the number of lines output on the MIPI bus. Allow 2 lines + * for start frame and end frame, plus some blanking. Observation and + * experimentation shows that 22 blanking lines are required. (With + * 720 output lines, reducing frame length from 746 to 744 results in + * no change in output timing. 746 to 748 adds two lines to the timing.) + * Limit the minimum number of lines to "min_frame_lines", although this + * will never apply on the iMX219. + */ +static unsigned int imx219_calc_total_lines(struct imx219_private *priv) +{ + unsigned int lines = be16_to_cpu(priv->params.y_output_size) + + priv->visible.top + 2 + 22; + + if (lines < priv->min_frame_lines) + lines = priv->min_frame_lines; + + return lines; +} + +/* + * Calculate the number of bits per lane for each line packet sent. + * This is the number of active bits output, plus the 32-bit header + * and 16-bit footer. The lanes always send a whole number of bytes, + * so round up to a whole number of 8 bits. + */ +static unsigned int imx219_calc_lane_active_line_bits(struct imx219_private *priv) +{ + unsigned int line_bits; + + line_bits = priv->bits_per_pixel * + be16_to_cpu(priv->params.x_output_size) + 32 + 16; + + return 8 * DIV_ROUND_UP(line_bits, 8 * priv->mipi_lanes); +} + +/* + * Calculate the number of bit periods for a full line, including the + * "blanking" period on the bus (which allows it to go into LP-11 state.) + * Allow a minimum 488 bits for blanking - this seems to be what's required, + * found via measurement and experimentation. This appears to no parameter + * for this, and being very different from the min_line_blank_pck parameter. + */ +static unsigned int imx219_calc_lane_line_bits(struct imx219_private *priv) +{ + return imx219_calc_lane_active_line_bits(priv) + 488; +} + +/* + * Analogue gain. The values given in the capability registers scale + * the analogue gain to a linear scale between 1x and 8x. Give the + * user the linear scale, but with a value scaled by 1000 so the full + * range is available, and the value is at least meaningful. + * + * gain = (m0 * reg + c0) / (m1 * reg + c1) + * reg = (c0 - gain * c1) / (gain * m1 - m0) + */ +#define GAIN_SCALE 1000 +static s32 imx219_reg_to_gain(struct imx219_private *priv, u16 reg) +{ + long num, div; + + /* + * Although reg is a 16-bit number, it's for a register whose + * maximum value is limited to 8-bit. So limit it to 8-bit. + */ + if (reg > 255) + reg = 255; + + num = priv->ana_gain_m0 * reg + priv->ana_gain_c0; + div = priv->ana_gain_m1 * reg + priv->ana_gain_c1; + + if (WARN_ON(div == 0)) + div = 1; + + return num * GAIN_SCALE / div; +} + +static u16 imx219_gain_to_reg(struct imx219_private *priv, s32 val) +{ + long num = priv->ana_gain_c0 * GAIN_SCALE - priv->ana_gain_c1 * val; + long div = val * priv->ana_gain_m1 - priv->ana_gain_m0 * GAIN_SCALE; + + /* If div does end up being zero, then we've gone wrong */ + if (WARN_ON(div == 0)) + div = 1; + + /* signed div-round-closest */ + if ((num < 0) ^ (div < 0)) + num -= div / 2; + else + num += div / 2; + + return num / div; +} +#undef GAIN_SCALE + +static unsigned long imx219_get_vt_pixclk(struct imx219_private *priv) +{ + return priv->pll_ip_freq * priv->vt_mpy / 5; +} + +/* + * Calculate the exposure register setting given a pixel clock, line + * length and exposure time in units of 100us, rounded to nearest. + * + * tsh = (coarse_integ_time * line_length + fine_integ_time) / + * (2 * pixclk_freq) + * + * With a fixed fine time: + * coarse = (tsh * 2 * pixclk_freq - fine) / line_length + */ +#define EXP_UNITS_US 100 +#define TSH_SCALE (1000000 / EXP_UNITS_US) +static u16 imx219_exposure_to_reg(struct imx219_private *priv, s32 val) +{ + unsigned int line_length = be16_to_cpu(priv->params.line_length); + unsigned long vt_pixclk = imx219_get_vt_pixclk(priv); + u64 tsh; + + tsh = (u64)2 * vt_pixclk * val - priv->fine_integ_time * TSH_SCALE; + + return (u16)DIV_ROUND_CLOSEST_ULL(tsh, line_length * TSH_SCALE); +} + +static void imx219_set_exposure_limits(struct imx219_private *priv) +{ + unsigned int line_length = be16_to_cpu(priv->params.line_length); + unsigned int frame_length = be16_to_cpu(priv->params.frame_length); + unsigned int fine_integ_time = priv->fine_integ_time; + unsigned long vt_pixclk = imx219_get_vt_pixclk(priv); + unsigned int min_exp, max_exp; + bool at_init; + u64 tsh; + + /* Scale tsh up so we end up with microseconds rather than seconds */ + tsh = (u64)(priv->coarse_integration_time_min * line_length + + fine_integ_time) * 1000000; + /* and ensure that the division rounds up */ + tsh += 2 * vt_pixclk - 1; + tsh = div_u64(tsh, 2 * vt_pixclk); + min_exp = DIV_ROUND_UP((u32)tsh, EXP_UNITS_US); + + frame_length -= priv->coarse_integration_time_max_margin; + tsh = (u64)(frame_length * line_length + fine_integ_time) * 1000000; + /* round down */ + tsh = div_u64(tsh, 2 * vt_pixclk); + max_exp = (u32)tsh / EXP_UNITS_US; + + /* + * The normal behaviour is to "round the existing value to closest" + * but that results in an initial value of min_exp for this control, + * which is silly. Since the minimum will never be 0 except on + * initialisation, override it here. + */ + at_init = priv->pixel.ctrls[CTRL_P_EXPOSURE]->cur.val == 0; + v4l2_ctrl_modify_range(priv->pixel.ctrls[CTRL_P_EXPOSURE], min_exp, + max_exp, 1, max_exp); + if (at_init) + v4l2_ctrl_s_ctrl(priv->pixel.ctrls[CTRL_P_EXPOSURE], max_exp); +} +#undef EXP_UNITS_US +#undef TSH_SCALE + +static void imx219_update_output_bpp(struct imx219_private *priv, + const struct v4l2_mbus_framefmt *fmt) +{ + switch (fmt->code) { + case MEDIA_BUS_FMT_SGRBG10_1X10: + case MEDIA_BUS_FMT_SRGGB10_1X10: + case MEDIA_BUS_FMT_SBGGR10_1X10: + case MEDIA_BUS_FMT_SGBRG10_1X10: + priv->bits_per_pixel = 10; + break; + default: + WARN_ON(1); + fallthrough; + case MEDIA_BUS_FMT_SGRBG8_1X8: + case MEDIA_BUS_FMT_SRGGB8_1X8: + case MEDIA_BUS_FMT_SBGGR8_1X8: + case MEDIA_BUS_FMT_SGBRG8_1X8: + priv->bits_per_pixel = 8; + break; + } +} + +/* + * The line length in conjunction with the VT clock determines the + * start of transmission of each line on the CSI bus. + * + * The frame length in conjunction with the line length and VT clock + * determines the start of transmission of each frame on the CSI bus. + * + * Note: these two parameters have nothing to do with the pixel array + * addressing parameters. + */ +static void imx219_update_frame_size(struct imx219_private *priv) +{ + unsigned long vt_pixclk = imx219_get_vt_pixclk(priv); + unsigned long op_sysclk; + unsigned int div, lane_line_bits; + unsigned int line_clk, line_length, line_blank; + unsigned int min_frame_length, frame_length, frame_blank; + + /* see comments above, this is definitely wrong */ + unsigned int width = be16_to_cpu(priv->params.x_output_size); + unsigned int height = be16_to_cpu(priv->params.y_output_size); + + dev_info(priv->root.sd.dev, "%s() for %ux%u %ubpp fi=%u/%u:\n", __func__, + width, height, priv->bits_per_pixel, + priv->frame_interval.numerator, + priv->frame_interval.denominator); + + /* + * This works for calculating a working line length, but is + * less than ideal, as we always run the VT block at maximum, + * and wind the output block up to maximum for large widths. + */ + lane_line_bits = imx219_calc_lane_line_bits(priv); + + line_clk = 2 * vt_pixclk / priv->min_line_length_pck; + + dev_info(priv->root.sd.dev, "VT: pixclk=%luHz line=%uHz\n", + vt_pixclk, line_clk); + + /* Calculate the required output sysclk */ + op_sysclk = line_clk * lane_line_bits; + + /* + * Try to find a sysclk divisor that gives us the highest PLL + * speed - otherwise errors seem to occur at the CSI2 receiver: + * i.MX6 CS2 error 2/1 registers = 0x00000120/0x11000113 + * - header ecc contains 2 errors + * - crc error on vc0 + * - incorrect frame sequence on vc0 + * - error matching frame start with frame end on vc0 + * - sot sync failed on lane 1 + * - sot sync failed on lane 0 + * - sot error on lane 1 + * - header error detected and corrected on vc0 + * Note: docs say 432MHz as the PLL low bound, hardware says 400MHz, + * experimentation with my imx219 is stable at 464MHz but not 456MHz. + */ + for (div = priv->pll[PLL_OP].clk[CLK_SYS].max_div; + div > priv->pll[PLL_OP].clk[CLK_SYS].min_div; div--) + if (op_sysclk * div <= priv->pll[PLL_OP].clk[CLK_SYS].max_freq) + break; + + priv->op_sys_div = div; + op_sysclk *= div; + + dev_info(priv->root.sd.dev, "OP: sysclk=%luHz/%u for %u bits/line/lane\n", + op_sysclk, div, lane_line_bits); + + priv->op_mpy = clamp_val(DIV_ROUND_UP(op_sysclk, + priv->pll_ip_freq), + priv->pll[PLL_OP].min_mpy, + priv->pll[PLL_OP].max_mpy); + + /* Now calculate the real output sysclk */ + op_sysclk = priv->pll_ip_freq * priv->op_mpy / div; + + /* ... and the resulting line clock for the output */ + line_clk = op_sysclk / lane_line_bits; + + dev_info(priv->root.sd.dev, "OP: real sysclk %luHz line %uHz\n", + op_sysclk, line_clk); + + /* ... and the final line length */ + line_length = clamp_val(DIV_ROUND_UP(2 * vt_pixclk, line_clk), + priv->min_line_length_pck, + priv->max_line_length_pck); + + priv->params.line_length = cpu_to_be16(line_length); + + /* ... and the final line rate */ + line_clk = 2 * vt_pixclk / line_length; + + /* + * Calculate the frame length required to give the requested + * frame interval. + */ + min_frame_length = imx219_calc_total_lines(priv); + frame_length = div_u64((u64)line_clk * priv->frame_interval.numerator, + priv->frame_interval.denominator); + + frame_length = clamp_val(frame_length, min_frame_length, + priv->max_frame_lines); + + priv->params.frame_length = cpu_to_be16(frame_length); + + dev_info(priv->root.sd.dev, "VT: line length %u frame length %u line %uHz\n", + line_length, frame_length, line_clk); + + + + /* See comments above, this is definitely wrong. */ + line_blank = line_length - width; + frame_blank = frame_length - height; + + v4l2_ctrl_s_ctrl_int64(priv->pixel.ctrls[CTRL_P_PIXEL_RATE], + 2 * vt_pixclk); + + v4l2_ctrl_modify_range(priv->pixel.ctrls[CTRL_P_HBLANK], + max_t(s32, priv->min_line_length_pck - width, + priv->min_line_blank_pck), + priv->max_line_length_pck - width, 1, + line_blank); + v4l2_ctrl_s_ctrl(priv->pixel.ctrls[CTRL_P_HBLANK], line_blank); + + v4l2_ctrl_modify_range(priv->pixel.ctrls[CTRL_P_VBLANK], + max_t(s32, priv->min_frame_lines - height, + priv->min_frame_blank_lines), + priv->max_frame_lines - height, 1, frame_blank); + v4l2_ctrl_s_ctrl(priv->pixel.ctrls[CTRL_P_VBLANK], frame_blank); + + imx219_set_exposure_limits(priv); +} + +/* Core ops */ + +static int imx219_s_power(struct v4l2_subdev *sd, int on) +{ + int ret = 0; + + if (on) { + ret = pm_runtime_get_sync(sd->dev); + if (ret > 0) + ret = 0; + } else { + pm_runtime_mark_last_busy(sd->dev); + pm_runtime_put_autosuspend(sd->dev); + + } + return ret; +} + +static int __maybe_unused imx219_g_register(struct v4l2_subdev *sd, + struct v4l2_dbg_register *reg) +{ + struct imx219_private *priv = imx219_get_priv(sd); + int ret; + + if (reg->reg > 0xffff) + return -EINVAL; + + ret = pm_runtime_get_sync(sd->dev); + if (ret < 0) + return ret; + + ret = imx219_readb(priv, reg->reg); + + pm_runtime_mark_last_busy(sd->dev); + pm_runtime_put_autosuspend(sd->dev); + + if (ret < 0) + return ret; + + reg->size = 1; + reg->val = ret; + + return 0; +} + +static int __maybe_unused imx219_s_register(struct v4l2_subdev *sd, + const struct v4l2_dbg_register *reg) +{ + struct imx219_private *priv = imx219_get_priv(sd); + int ret; + + if (reg->reg > 0xffff) + return -EINVAL; + + ret = pm_runtime_get_sync(sd->dev); + if (ret < 0) + return ret; + + ret = imx219_writeb(priv, reg->reg, reg->val); + + pm_runtime_mark_last_busy(sd->dev); + pm_runtime_put_autosuspend(sd->dev); + + return ret < 0 ? ret : 0; +} + +static const struct v4l2_subdev_core_ops imx219_root_core_ops = { +#ifdef CONFIG_VIDEO_ADV_DEBUG + .g_register = imx219_g_register, + .s_register = imx219_s_register, +#endif + .s_power = imx219_s_power, +}; + +/* Video ops */ + +static int imx219_g_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fiv) +{ + struct imx219_private *priv = imx219_get_priv(sd); + + if (fiv->pad != PAD_SRC) + return -EINVAL; + + mutex_lock(&priv->mutex); + fiv->interval = priv->frame_interval; + mutex_unlock(&priv->mutex); + + return 0; +} + +static int imx219_s_frame_interval(struct v4l2_subdev *sd, + struct v4l2_subdev_frame_interval *fiv) +{ + struct imx219_private *priv = imx219_get_priv(sd); + + if (fiv->pad != PAD_SRC) + return -EINVAL; + + mutex_lock(&priv->mutex); + if (fiv->interval.denominator == 0) { + unsigned long vt_pixclk = imx219_get_vt_pixclk(priv); + unsigned int line_length = be16_to_cpu(priv->params.line_length); + unsigned int line_clk = 2 * vt_pixclk / line_length; + + fiv->interval.numerator = priv->max_frame_lines; + fiv->interval.denominator = line_clk; + } else { + unsigned int max_vt_line_clk, max_op_line_clk, max_line_clk; + unsigned int min_frame_lines; + u64 v; + + /* + * This could be buggy for large resolutions: we should be + * calculting the max_vt_line_clk based on what we think is + * the real line length. However, in this case, the output + * line clock is the limiting factor. + */ + max_vt_line_clk = priv->pll[PLL_VT].clk[CLK_PIX].max_freq * 2 / + priv->min_line_length_pck; + + max_op_line_clk = priv->pll[PLL_OP].clk[CLK_SYS].max_freq / + imx219_calc_lane_line_bits(priv); + + /* The maximum line clock is the minimum of the above two. */ + max_line_clk = min(max_vt_line_clk, max_op_line_clk); + + /* The minimum frame lines that we could output */ + min_frame_lines = imx219_calc_total_lines(priv); + + v = max_line_clk * fiv->interval.numerator; + v = div_u64(v, fiv->interval.denominator); + if (v > priv->max_frame_lines) { + /* + * FIXME: this can do with improvement, it's the + * slowest frame rate based on the fastest clock! + */ + fiv->interval.numerator = priv->max_frame_lines; + fiv->interval.denominator = max_line_clk; + } else if (v < min_frame_lines) { + fiv->interval.numerator = min_frame_lines; + fiv->interval.denominator = max_line_clk; + } + } + + priv->frame_interval = fiv->interval; + + imx219_update_frame_size(priv); + mutex_unlock(&priv->mutex); + + return 0; +} + +static int imx219_prepare_stream(struct v4l2_subdev *sd) +{ + struct imx219_private *priv = imx219_get_priv(sd); + int ret; + + mutex_lock(&priv->mutex); + + /* Initialise the sensor to place the outputs in LP-11 state. */ + + /* Select MIPI virtual channel */ + imx219_writeb(priv, R_CSI_CH_ID, priv->mipi_vc); + + /* Select number of lanes */ + imx219_writeb(priv, R_CSI_LANE_MODE, priv->mipi_lanes - 1); + + /* Select output format */ + imx219_writew(priv, R_CSI_DATA_FORMAT_A, + priv->bits_per_pixel << 8 | priv->bits_per_pixel); + + /* Set the output clock divisor */ + imx219_writeb(priv, R_OPPXCK_DIV, priv->bits_per_pixel); + + /* Set PLL multipliers */ + imx219_writew(priv, R_PLL_VT_MPY, priv->vt_mpy); + imx219_writew(priv, R_PLL_OP_MPY, priv->op_mpy); + imx219_writeb(priv, R_OPSYCK_DIV, priv->op_sys_div); + + /* Set the start/end and output size */ + imx219_write(priv, R_FRM_LENGTH_A, &priv->params, sizeof(priv->params)); + + /* Set the miscellaneous vendor registers */ + imx219_write_array(priv, misc_regs, ARRAY_SIZE(misc_regs)); + + /* Enable fast standby, and enable the sensor to initialise */ + imx219_writeb(priv, R_FAST_STANDBY, 1); + imx219_writeb(priv, R_MODE_SELECT, 1); + /* Allow D-PHY power up (t7 = 1.1ms) and initialisation (t8 = 110us) */ + msleep(2); + imx219_writeb(priv, R_MODE_SELECT, 0); + /* Allow sensor to stop (1 line) */ + msleep(2); + /* Disable fast standby */ + imx219_writeb(priv, R_FAST_STANDBY, 0); + + /* Set the test pattern window to the output size */ + imx219_writew(priv, R_TP_WINDOW_WIDTH, priv->root.format[PAD_SRC].width); + imx219_writew(priv, R_TP_WINDOW_HEIGHT, priv->root.format[PAD_SRC].height); + + mutex_unlock(&priv->mutex); + + /* Finally, set the current user control settings */ + ret = v4l2_ctrl_handler_setup(&priv->root.ctrl_handler); + if (ret == 0) + ret = v4l2_ctrl_handler_setup(&priv->pixel.ctrl_handler); + + return ret; +} + +static int imx219_s_stream(struct v4l2_subdev *sd, int on) +{ + struct imx219_private *priv = imx219_get_priv(sd); + int ret; + + if (on) { + ret = imx219_prepare_stream(sd); + if (ret) + return ret; + } + + mutex_lock(&priv->mutex); + if (priv->streaming != !!on) { + imx219_writeb(priv, R_MODE_SELECT, !!on); + priv->streaming = !!on; + } + mutex_unlock(&priv->mutex); + + return 0; +} + +static const struct v4l2_subdev_video_ops imx219_root_video_ops = { + .g_frame_interval = imx219_g_frame_interval, + .s_frame_interval = imx219_s_frame_interval, + .s_stream = imx219_s_stream, +}; + +/* Pad operations */ + +static int imx219_root_enum_mbus_code(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + struct v4l2_subdev_mbus_code_enum *code) +{ + struct imx219_private *priv = imx219_get_priv(sd); + + if (code->index >= ARRAY_SIZE(imx219_formats)) + return -EINVAL; + + code->code = imx219_formats[code->index][priv->pixel_order]; + + return 0; +} + +static struct v4l2_mbus_framefmt *imx219_get_pad_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + u32 which, u32 pad) +{ + struct imx219_sub *sub = imx219_get_sub(sd); + + switch (which) { + case V4L2_SUBDEV_FORMAT_TRY: + return v4l2_subdev_get_try_format(sd, state, pad); + case V4L2_SUBDEV_FORMAT_ACTIVE: + return &sub->format[pad]; + default: + return NULL; + } +} + +static struct v4l2_rect *imx219_get_pad_crop(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + u32 which, u32 pad) +{ + struct imx219_sub *sub = imx219_get_sub(sd); + + switch (which) { + case V4L2_SUBDEV_FORMAT_TRY: + return v4l2_subdev_get_try_crop(sd, state, pad); + case V4L2_SUBDEV_FORMAT_ACTIVE: + return pad ? &sub->sink_crop : NULL; + default: + return NULL; + } +} + +static struct v4l2_rect *imx219_get_pad_compose(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + u32 which, u32 pad) +{ + struct imx219_sub *sub = imx219_get_sub(sd); + + switch (which) { + case V4L2_SUBDEV_FORMAT_TRY: + return v4l2_subdev_get_try_compose(sd, state, pad); + case V4L2_SUBDEV_FORMAT_ACTIVE: + return pad ? &sub->sink_compose : NULL; + default: + return NULL; + } +} + +static void imx219_framefmt_to_rect(struct v4l2_rect *r, + const struct v4l2_mbus_framefmt *mf) +{ + r->left = r->top = 0; + r->width = mf->width; + r->height = mf->height; +} + +static void imx219_pix_init_pad(struct imx219_private *priv, unsigned int pad, + struct v4l2_mbus_framefmt *mf, + struct v4l2_rect *crop) +{ + mf->width = priv->pixarray.width; + mf->height = priv->pixarray.height; + mf->code = priv->pixel_format; + mf->field = V4L2_FIELD_NONE; + mf->colorspace = V4L2_COLORSPACE_SRGB; + mf->ycbcr_enc = V4L2_YCBCR_ENC_601; + mf->xfer_func = V4L2_XFER_FUNC_SRGB; +} + +static void imx219_pix_adj_format(struct imx219_private *priv, + struct v4l2_subdev_format *fmt, + const struct v4l2_mbus_framefmt *sink_fmt, + const struct v4l2_rect *sink_compose) +{ + /* The pixel subdev is fixed at the pixel array size and format */ + fmt->format.width = priv->pixarray.width; + fmt->format.height = priv->pixarray.height; + fmt->format.code = priv->pixel_format; + fmt->format.field = V4L2_FIELD_NONE; + fmt->format.colorspace = V4L2_COLORSPACE_SRGB; + fmt->format.xfer_func = V4L2_XFER_FUNC_SRGB; +} + +static int imx219_pix_set_fmt(struct imx219_private *priv, + struct imx219_sub *sub, unsigned int pad, + const struct v4l2_rect *crop, + const struct v4l2_mbus_framefmt *fmt) +{ + /* Nothing to set */ + return 0; +} + +static const struct imx219_pad_ops imx219_pix_pad_ops = { + .init_pad = imx219_pix_init_pad, + .adj_format = imx219_pix_adj_format, + .set_fmt = imx219_pix_set_fmt, +}; + +static void imx219_root_init_pad(struct imx219_private *priv, unsigned int pad, + struct v4l2_mbus_framefmt *mf, + struct v4l2_rect *crop) +{ + if (pad == PAD_SINK) { + mf->width = priv->pixarray.width; + mf->height = priv->pixarray.height; + } else { + mf->width = priv->max_output_width; + mf->height = priv->max_output_height; + } + + mf->code = priv->pixel_format; + mf->field = V4L2_FIELD_NONE; + mf->colorspace = V4L2_COLORSPACE_SRGB; + mf->xfer_func = V4L2_XFER_FUNC_SRGB; + + imx219_framefmt_to_rect(crop, mf); +} + +static void imx219_root_adj_format(struct imx219_private *priv, + struct v4l2_subdev_format *fmt, + const struct v4l2_mbus_framefmt *sink_fmt, + const struct v4l2_rect *sink_compose) +{ + /* The sink pad format is the same as the pixel's source pad. */ + if (fmt->pad == PAD_SINK) { + imx219_pix_adj_format(priv, fmt, NULL, NULL); + return; + } + + /* + * We don't allow cropping the output after scaling, so + * mirror the sink's compose rectangle. + */ + fmt->format.width = sink_compose->width; + fmt->format.height = sink_compose->height; + + /* The format code can be different (10bit to 8bit conversion) */ + fmt->format.code = imx219_lookup_format(fmt->format.code, + priv->pixel_order); + + /* + * Field, colorspace, etc all come from the sink pad and + * can't be changed. + */ + fmt->format.field = sink_fmt->field; + fmt->format.colorspace = sink_fmt->colorspace; + fmt->format.ycbcr_enc = sink_fmt->ycbcr_enc; + fmt->format.xfer_func = sink_fmt->xfer_func; +} + +static int imx219_root_set_fmt(struct imx219_private *priv, + struct imx219_sub *sub, unsigned int pad, + const struct v4l2_rect *crop, + const struct v4l2_mbus_framefmt *fmt) +{ + if (pad == PAD_SRC) { + imx219_update_output_bpp(priv, fmt); + } else if (pad == PAD_SINK) { + /* Setting the sink format re-sets the crop and compose */ + sub->pad_ops->set_selection(priv, sub, PAD_SINK, + V4L2_SEL_TGT_CROP, + crop + 1, crop); + sub->pad_ops->set_selection(priv, sub, PAD_SINK, + V4L2_SEL_TGT_COMPOSE, + crop + 2, crop + 1); + } + + return 0; +} + +/* + * V4L2 documentation says: + * + * The closest possible values of horizontal and vertical offset and sizes + * are chosen according to following priority: + * + * 1. Satisfy constraints from struct v4l2_selection flags. + * 2. Adjust width, height, left, and top to hardware limits and alignments. + * 3. Keep center of adjusted rectangle as close as possible to the original + * one. + * 4. Keep width and height as close as possible to original ones. + * 5. Keep horizontal and vertical offset as close as possible to original + * ones. + * + * This brings up at least the following questions: + * - if we adjust to the hardware limits and alignments as priority 2, + * what's the point of doing further adjustments in 3..5? + * - ignoring that, if we give (3) a higher priority than (4), this has + * the effect that a rectangle who's centre is close but not outside + * of the bounds will have it's width/height shrunk _and_ position + * moved in order to keep the centre the same, meaning we violate (4) + * and (5). + * + * the "this" rectangle points to the existing parameters of the rectangle + * being modified. this[-1] is the upstream rectangle, guaranteed to always + * be valid. this[1] is the downstream rectangle, also guaranteed. + */ +static int imx219_root_adj_selection(struct imx219_private *priv, + unsigned int target, unsigned int flags, + struct v4l2_rect *r, + struct v4l2_rect * const this[]) +{ + unsigned int adj_flags = flags & (V4L2_SEL_FLAG_GE | V4L2_SEL_FLAG_LE); + struct v4l2_rect rect; + + if (target == V4L2_SEL_TGT_COMPOSE) { + unsigned int ratio_w, ratio_h; + u32 min_w, max_w, min_h, max_h; + + /* + * For the compose rectangle size and source pad format + * size must be identical. Since KEEP_CONFIG means we + * are not permitted to propagate to the source pad, we + * must keep the current configuration. + */ + if (flags & V4L2_SEL_FLAG_KEEP_CONFIG) { + *r = *this[0]; + return 0; + } + + min_w = max_t(u32, this[-1]->width / 4, priv->min_output_width); + min_h = max_t(u32, this[-1]->height / 4, priv->min_output_height); + max_w = min_t(u32, this[-1]->width, priv->max_output_width); + max_h = min_t(u32, this[-1]->height, priv->max_output_height); + + /* + * The hardware binning can reduce the cropped rectangle + * by /2 or /4. However, the output image size does not + * have to be strictly /2 or /4, but should be equal or + * smaller. + */ + if (adj_flags == V4L2_SEL_FLAG_LE) { + /* We are allowed to decrease rectangle size */ + rect.width = clamp_val(r->width, 1, max_w); + rect.height = clamp_val(r->height, 1, max_h); + + ratio_w = DIV_ROUND_UP(this[-1]->width, rect.width); + if (ratio_w >= 3) + ratio_w = 4; + rect.width = this[-1]->width / ratio_w; + rect.width &= ~1; + + ratio_h = DIV_ROUND_UP(this[-1]->height, rect.height); + if (ratio_h >= 3) + ratio_h = 4; + rect.height = this[-1]->height / ratio_h; + rect.height &= ~1; + } else { + /* + * We are allowed to increase rectangle size. + * However, the resulting rectangle must lie + * within the hardware bounds. The crop window + * will be limited to the hardware maximum output + * size, and min_w/min_h will be limited by the + * hardware minimum output size. + * + * One issue not handled well: if the crop window + * is even, but the compose window width/height is + * odd, is it correct (hardware wise) to round up? + */ + rect.width = max(r->width, min_w); + rect.height = max(r->height, min_h); + + ratio_w = fls(this[-1]->width / rect.width); + ratio_w = clamp_val(ratio_w, 1, 3) - 1; + rect.width = ALIGN(this[-1]->width >> ratio_w, 2); + + ratio_h = fls(this[-1]->height / rect.height); + ratio_h = clamp_val(ratio_h, 1, 3) - 1; + rect.height = ALIGN(this[-1]->height >> ratio_h, 2); + } + + /* + * Validate that we ended up with something suitable + * for the hardware. Complain and error out if not. + */ + if (rect.width < priv->min_output_width || + rect.width > priv->max_output_width || + rect.height < priv->min_output_height || + rect.height > priv->max_output_height) { + v4l2_err(&priv->root.sd, + "BUG: compose rectangle (%ux%u) outside hardware bounds (%u,%u)-(%u,%u)\n", + rect.width, rect.height, + priv->min_output_width, priv->min_output_height, + priv->max_output_width, priv->max_output_height); + return -ERANGE; + } + + /* + * Documentation/media/uapi/v4l/dev-subdev.rst: + * "If the subdev supports scaling but not composing, + * the top and left values are not used and must + * always be set to zero." + */ + rect.left = rect.top = 0; + } else { + s32 centre_x, centre_y; + int delta; + + /* + * FIXME: we don't (yet) support this flag - need to work + * out how to rework the code below to achieve _all_ the + * requirements of v4l uapi documentation. + */ + if (flags & V4L2_SEL_FLAG_KEEP_CONFIG) + return -ERANGE; + + centre_x = r->width / 2; + centre_y = r->height / 2; + + /* Reject outright values that cause math overflow */ + if (0x7fffffff - centre_x < r->left || + 0x7fffffff - centre_y < r->top) + return -ERANGE; + + /* The centre coordinate of the rectangle */ + centre_x += r->left; + centre_y += r->top; + + /* Clamp size to the hardware output limits */ + rect.width = clamp_val(r->width, priv->min_output_width, + this[-1]->width); + rect.height = clamp_val(r->height, priv->min_output_height, + this[-1]->height); + + /* Ensure that the centre is within the pixel area */ + centre_x = clamp_val(centre_x, rect.width / 2, + this[-1]->width - rect.width / 2); + centre_y = clamp_val(centre_y, rect.height / 2, + this[-1]->height - rect.height / 2); + + /* Calculate the new top and left coordinates */ + rect.left = centre_x - rect.width / 2; + rect.top = centre_y - rect.height / 2; + + /* + * Adjust size and origin to the alignment according to + * constraints + */ + if (adj_flags == V4L2_SEL_FLAG_LE) { + rect.left += delta = rect.left & 1; + rect.width -= delta; + rect.width -= rect.width & 1; + rect.top += delta = rect.top & 1; + rect.height -= delta; + rect.height -= rect.height & 1; + } else { + /* If we're reducing a coordinate, increase the size */ + rect.left -= delta = rect.left & 1; + rect.width += delta; + rect.width += rect.width & 1; + rect.top -= delta = rect.top & 1; + rect.height += delta; + rect.height += rect.height & 1; + } + } + + /* + * Documentation/media/uapi/v4l/vidioc-g-selection.rst: + * - V4L2_SEL_FLAG_GE - The driver is not allowed to shrink the + * rectangle. + * - V4L2_SEL_FLAG_LE - The driver is not allowed to enlarge the + * rectangle. + * - V4L2_SEL_FLAG_GE | V4L2_SEL_FLAG_LE - The driver must choose + * the size exactly the same as in the requested rectangle. + * + * Doesn't say we have to check the location of the rectangle. + */ + switch (adj_flags) { + case V4L2_SEL_FLAG_GE: + if (rect.width < r->width || rect.height < r->height) + return -ERANGE; + break; + + case V4L2_SEL_FLAG_LE: + if (rect.width > r->width || rect.height > r->height) + return -ERANGE; + break; + + case V4L2_SEL_FLAG_GE | V4L2_SEL_FLAG_LE: + if (rect.width != r->width || rect.height != r->height) + return -ERANGE; + break; + } + + *r = rect; + + return 0; +} + +static int imx219_root_set_selection(struct imx219_private *priv, + struct imx219_sub *sub, unsigned int pad, + unsigned int target, + const struct v4l2_rect *rect, + const struct v4l2_rect *upstream) +{ + if (target == V4L2_SEL_TGT_CROP) { + unsigned int left, top; + + /* + * We hide the pixel array offset (if any) from the user, + * adding it in here, as otherwise we would violate the + * ->set_format requirement to set the left,top of the + * crop region to 0,0. + */ + left = priv->pixarray.left + rect->left; + top = priv->pixarray.top + rect->top; + + priv->params.x_add_sta = cpu_to_be16(left); + priv->params.x_add_end = cpu_to_be16(left + rect->width - 1); + priv->params.y_add_sta = cpu_to_be16(top); + priv->params.y_add_end = cpu_to_be16(top + rect->height - 1); + } else { + unsigned int scale_x, scale_y; + + /* Set the output size */ + priv->params.x_output_size = cpu_to_be16(rect->width); + priv->params.y_output_size = cpu_to_be16(rect->height); + + scale_x = fls(upstream->width / rect->width) - 1; + scale_y = fls(upstream->height / rect->height) - 1; + +#if 0 + /* + * If scale_x and scale_y are both 1, we can use analog + * binning. (but we need to modify the frame length). + */ + if (scale_x == 1 && scale_y == 1) + scale_x = scale_y = 3; +#endif + + priv->params.binning_mode_h = scale_x; + priv->params.binning_mode_v = scale_y; + + imx219_update_frame_size(priv); + } + return 0; +} + +static const struct imx219_pad_ops imx219_int_root_pad_ops = { + .init_pad = imx219_root_init_pad, + .adj_format = imx219_root_adj_format, + .set_fmt = imx219_root_set_fmt, + .adj_selection = imx219_root_adj_selection, + .set_selection = imx219_root_set_selection, +}; + +static int imx219_init_cfg(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state) +{ + struct imx219_private *priv = imx219_get_priv(sd); + struct imx219_sub *sub = imx219_get_sub(sd); + unsigned int pad; + + for (pad = 0; pad < sd->entity.num_pads; pad++) { + struct v4l2_mbus_framefmt *mf; + struct v4l2_rect *crop, *compose, rect; + + memset(&rect, 0, sizeof(rect)); + + mf = v4l2_subdev_get_try_format(sd, state, pad); + crop = v4l2_subdev_get_try_crop(sd, state, pad); + compose = v4l2_subdev_get_try_compose(sd, state, pad); + + sub->pad_ops->init_pad(priv, pad, mf, &rect); + + *crop = rect; + *compose = rect; + } + + return 0; +} + +static int imx219_get_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + struct v4l2_subdev_format *fmt) +{ + struct imx219_private *priv = imx219_get_priv(sd); + struct v4l2_mbus_framefmt *pad_fmt; + + pad_fmt = imx219_get_pad_fmt(sd, state, fmt->which, fmt->pad); + if (!pad_fmt) + return -EINVAL; + + mutex_lock(&priv->mutex); + fmt->format = *pad_fmt; + mutex_unlock(&priv->mutex); + + return 0; +} + +static int imx219_set_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + struct v4l2_subdev_format *fmt) +{ + struct imx219_private *priv = imx219_get_priv(sd); + struct imx219_sub *sub = imx219_get_sub(sd); + struct v4l2_mbus_framefmt *pad_fmt, *other_fmt; + struct v4l2_rect *pad_crop, *other_rect, fmtr, crop, compose; + struct v4l2_rect *rect[4] = { + &fmtr, + &crop, + &compose, + &compose, + }; + struct v4l2_rect *pad_compose; + unsigned int other; + int ret = 0; + + if (!sub->pad_ops->adj_format) + return -ENOTTY; + + pad_compose = imx219_get_pad_compose(sd, state, fmt->which, fmt->pad); + pad_crop = imx219_get_pad_crop(sd, state, fmt->which, fmt->pad); + pad_fmt = imx219_get_pad_fmt(sd, state, fmt->which, fmt->pad); + if (!pad_fmt) + return -EINVAL; + + other = !fmt->pad; + if (other < sd->entity.num_pads) { + other_fmt = imx219_get_pad_fmt(sd, state, fmt->which, other); + other_rect = imx219_get_pad_compose(sd, state, fmt->which, + other); + } else { + other_fmt = NULL; + other_rect = NULL; + } + + mutex_lock(&priv->mutex); + + /* Sink pads never have to consider their sources, enforce that. */ + if (fmt->pad == PAD_SINK) + sub->pad_ops->adj_format(priv, fmt, NULL, NULL); + else + sub->pad_ops->adj_format(priv, fmt, other_fmt, other_rect); + + /* Setting the format sets the crop to the same size, located at 0,0 */ + imx219_framefmt_to_rect(&fmtr, &fmt->format); + compose = crop = fmtr; + if (sub->pad_ops->adj_selection) { + sub->pad_ops->adj_selection(priv, V4L2_SEL_TGT_CROP, 0, + &crop, rect + 1); + sub->pad_ops->adj_selection(priv, V4L2_SEL_TGT_COMPOSE, 0, + &compose, rect + 2); + } + + if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) { + ret = sub->pad_ops->set_fmt(priv, sub, fmt->pad, &crop, + &fmt->format); + + /* Propagate to source pad if setting sink pad */ + if (ret == 0 && fmt->pad == PAD_SINK) + ret = sub->pad_ops->set_fmt(priv, sub, PAD_SRC, NULL, + &fmt->format); + } + + if (ret == 0) { + *pad_fmt = fmt->format; + + /* + * Setting the sink pad sets the sink crop and compose + * rectangles, and propagates the format to the source pad + */ + if (fmt->pad == PAD_SINK) { + if (pad_crop) + *pad_crop = crop; + if (pad_compose) + *pad_compose = compose; + if (other_fmt) { + *other_fmt = fmt->format; + other_fmt->width = compose.width; + other_fmt->height = compose.height; + } + } + } + mutex_unlock(&priv->mutex); + + return ret; +} + +static int imx219_get_selection(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + struct v4l2_subdev_selection *sel) +{ + struct imx219_private *priv = imx219_get_priv(sd); + struct imx219_sub *sub = imx219_get_sub(sd); + struct v4l2_mbus_framefmt *pad_fmt; + struct v4l2_rect *pad_crop; + struct v4l2_rect *pad_compose; + int ret; + + if (!sub->pad_ops->adj_selection) + return -ENOTTY; + + pad_fmt = imx219_get_pad_fmt(sd, state, sel->which, sel->pad); + pad_crop = imx219_get_pad_crop(sd, state, sel->which, sel->pad); + pad_compose = imx219_get_pad_compose(sd, state, sel->which, sel->pad); + if (!pad_fmt || !pad_crop || !pad_compose) + return -EINVAL; + + mutex_lock(&priv->mutex); + ret = -EINVAL; + + switch (sel->target) { + case V4L2_SEL_TGT_CROP_BOUNDS: + /* can't be larger than the sink format size */ + case V4L2_SEL_TGT_NATIVE_SIZE: + imx219_framefmt_to_rect(&sel->r, pad_fmt); + ret = 0; + break; + +// case V4L2_SEL_TGT_COMPOSE_BOUNDS: + /* can't scale up */ + case V4L2_SEL_TGT_CROP: + sel->r = *pad_crop; + if (sel->r.width > priv->max_output_width) + sel->r.width = priv->max_output_width; + if (sel->r.height > priv->max_output_height) + sel->r.height = priv->max_output_height; + ret = 0; + break; + + case V4L2_SEL_TGT_COMPOSE: + sel->r = *pad_compose; + ret = 0; + break; + + default: + break; + } + mutex_unlock(&priv->mutex); + + return ret; +} + +/* + * Hardware limitations: + * The sink crop rectangle must always fit within the sink pad format size. + * The sink compose rectangle must always fit within the sink crop rectangle. + * The source pad format size must always match the compose rectangle size. + * + * Even if V4L2_SEL_FLAG_KEEP_CONFIG is set, we _must_ propagate a change + * in the compose rectangle size to the source pad format, because the + * difference is a pure software fallacy. + */ +static int imx219_set_selection(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + struct v4l2_subdev_selection *sel) +{ + struct imx219_private *priv = imx219_get_priv(sd); + struct imx219_sub *sub = imx219_get_sub(sd); + struct v4l2_mbus_framefmt *pad_fmt, *src_fmt; + struct v4l2_rect *pad_crop, *pad_compose; + struct v4l2_rect *rects[4], sink_fmt_rect, src_fmt_rect; + int ret, idx; + + if (!sub->pad_ops->adj_selection) + return -ENOTTY; + + switch (sel->target) { + case V4L2_SEL_TGT_CROP: + idx = 1; + break; + case V4L2_SEL_TGT_COMPOSE: + idx = 2; + break; + default: + return -EINVAL; + } + + pad_fmt = imx219_get_pad_fmt(sd, state, sel->which, sel->pad); + pad_crop = imx219_get_pad_crop(sd, state, sel->which, sel->pad); + pad_compose = imx219_get_pad_compose(sd, state, sel->which, sel->pad); + if (!pad_fmt || !pad_compose || !pad_crop) + return -EINVAL; + + src_fmt = imx219_get_pad_fmt(sd, state, sel->which, PAD_SRC); + + mutex_lock(&priv->mutex); + imx219_framefmt_to_rect(&sink_fmt_rect, pad_fmt); + imx219_framefmt_to_rect(&src_fmt_rect, src_fmt); + + rects[0] = &sink_fmt_rect; + rects[1] = pad_crop; + rects[2] = pad_compose; + rects[3] = &src_fmt_rect; + + ret = sub->pad_ops->adj_selection(priv, sel->target, sel->flags, + &sel->r, rects + idx); + if (ret) + goto out_unlock; + + if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) { + ret = sub->pad_ops->set_selection(priv, sub, sel->pad, + sel->target, + &sel->r, rects[idx - 1]); + + /* Setting the crop also propagates to the compose */ + if (sel->target == V4L2_SEL_TGT_CROP && ret == 0) + ret = sub->pad_ops->set_selection(priv, sub, sel->pad, + V4L2_SEL_TGT_COMPOSE, + &sel->r, &sel->r); + } + + if (ret == 0) { + *rects[idx] = sel->r; + + if (!(sel->flags & V4L2_SEL_FLAG_KEEP_CONFIG)) { + while (++idx < ARRAY_SIZE(rects)) + *rects[idx] = sel->r; + + src_fmt->width = src_fmt_rect.width; + src_fmt->height = src_fmt_rect.height; + } + } + + out_unlock: + mutex_unlock(&priv->mutex); + + return ret; +} + +static int imx219_pix_enum_mbus_code(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + struct v4l2_subdev_mbus_code_enum *code) +{ + struct imx219_private *priv = imx219_get_priv(sd); + + if (code->index) + return -EINVAL; + + code->code = priv->pixel_format; + + return 0; +} + +static int imx219_pix_enum_frame_size(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + struct v4l2_subdev_frame_size_enum *fse) +{ + struct imx219_private *priv = imx219_get_priv(sd); + + if (fse->index) + return -EINVAL; + + fse->min_width = priv->min_output_width; + fse->max_width = priv->pixarray.width; + fse->min_height = priv->min_output_height; + fse->max_height = priv->pixarray.height; + fse->code = priv->pixel_format; + + return 0; +} + +static const struct v4l2_subdev_pad_ops imx219_sub_pad_ops = { + .init_cfg = imx219_init_cfg, + .enum_mbus_code = imx219_pix_enum_mbus_code, + .enum_frame_size = imx219_pix_enum_frame_size, + .get_fmt = imx219_get_fmt, + .set_fmt = imx219_set_fmt, + .get_selection = imx219_get_selection, + .set_selection = imx219_set_selection, +}; + +static int imx219_root_enum_frame_size(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + struct v4l2_subdev_frame_size_enum *fse) +{ + struct imx219_private *priv = imx219_get_priv(sd); + + if (fse->index) + return -EINVAL; + + fse->min_width = priv->min_output_width; + fse->min_height = priv->min_output_height; + + if (fse->pad == PAD_SINK) { + /* sink pad */ + fse->max_width = priv->pixarray.width; + fse->max_height = priv->pixarray.height; + fse->code = priv->pixel_format; + } else { + /* source pad */ + struct v4l2_mbus_framefmt *mf; + + mf = imx219_get_pad_fmt(sd, state, fse->which, PAD_SINK); + + fse->max_width = min_t(u32, mf->width, priv->max_output_width); + fse->max_height = min_t(u32, mf->height, priv->max_output_height); + fse->code = imx219_lookup_format(fse->code, + priv->pixel_order); + } + + return 0; +} + +static int imx219_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad, + struct v4l2_mbus_config *cfg) +{ + struct imx219_private *priv = imx219_get_priv(sd); + + if (pad == PAD_SINK) + return -EINVAL; + + cfg->type = V4L2_MBUS_CSI2_DPHY; + cfg->flags = V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK; + if (priv->mipi_lanes == 2) + cfg->flags |= V4L2_MBUS_CSI2_2_LANE; + else + cfg->flags |= V4L2_MBUS_CSI2_4_LANE; + + return 0; +} + +static const struct v4l2_subdev_pad_ops imx219_root_pad_ops = { + .init_cfg = imx219_init_cfg, + .enum_mbus_code = imx219_root_enum_mbus_code, + .enum_frame_size = imx219_root_enum_frame_size, + .get_fmt = imx219_get_fmt, + .set_fmt = imx219_set_fmt, + .get_selection = imx219_get_selection, + .set_selection = imx219_set_selection, + .get_mbus_config = imx219_get_mbus_config, +}; + +/* Controls */ + +static void imx219_ctrls_activate(struct imx219_private *priv, int s, int n, + bool active) +{ + while (n--) + v4l2_ctrl_activate(priv->root.ctrls[s++], active); +} + +static int imx219_pix_s_ctrl(struct v4l2_ctrl *c) +{ + struct imx219_private *priv = container_of(c->handler, + struct imx219_private, + pixel.ctrl_handler); + + switch (c->id) { + case V4L2_CID_GAIN: + return imx219_writew_pm(priv, R_DIG_GAIN_GLOBAL_A, c->val); + case V4L2_CID_ANALOGUE_GAIN: + return imx219_writeb_pm(priv, R_ANA_GAIN_GLOBAL_A, + imx219_gain_to_reg(priv, c->val)); + case V4L2_CID_EXPOSURE_ABSOLUTE: + return imx219_writew_pm(priv, R_COARSE_INTEGRATION_A, + imx219_exposure_to_reg(priv, c->val)); + case V4L2_CID_PIXEL_RATE: + case V4L2_CID_HBLANK: + case V4L2_CID_VBLANK: + /* ignored, so we can set the read-only value */ + return 0; + default: + return -EINVAL; + } +} + +static const struct v4l2_ctrl_ops imx219_pix_ctrl_ops = { + .s_ctrl = imx219_pix_s_ctrl, +}; + +static int imx219_root_s_ctrl(struct v4l2_ctrl *c) +{ + struct imx219_private *priv = container_of(c->handler, + struct imx219_private, + root.ctrl_handler); + + switch (c->id) { + case V4L2_CID_TEST_PATTERN_RED: + return imx219_writew_pm(priv, R_TD_R, c->val); + case V4L2_CID_TEST_PATTERN_GREENR: + return imx219_writew_pm(priv, R_TD_GR, c->val); + case V4L2_CID_TEST_PATTERN_BLUE: + return imx219_writew_pm(priv, R_TD_B, c->val); + case V4L2_CID_TEST_PATTERN_GREENB: + return imx219_writew_pm(priv, R_TD_GB, c->val); + case V4L2_CID_TEST_PATTERN: + imx219_ctrls_activate(priv, CTRL_R_R, CTRL_R_GB - CTRL_R_R + 1, + c->val == 1); + return imx219_writew_pm(priv, R_TEST_PATTERN_MODE, c->val); + default: + return -EINVAL; + } +} + +static const struct v4l2_ctrl_ops imx219_root_ctrl_ops = { + .s_ctrl = imx219_root_s_ctrl, +}; + +static int imx219_create_controls(struct imx219_private *priv) +{ + int i; + static const u32 test_ids[CTRL_R_GB - CTRL_R_R + 1] = { + V4L2_CID_TEST_PATTERN_RED, + V4L2_CID_TEST_PATTERN_GREENR, + V4L2_CID_TEST_PATTERN_BLUE, + V4L2_CID_TEST_PATTERN_GREENB, + }; + + v4l2_ctrl_handler_init(&priv->root.ctrl_handler, 5); + + for (i = 0; i < ARRAY_SIZE(test_ids); i++) + priv->root.ctrls[CTRL_R_R + i] = + v4l2_ctrl_new_std(&priv->root.ctrl_handler, + &imx219_root_ctrl_ops, test_ids[i], + 0, 0x3ff, 1, 0); + + imx219_ctrls_activate(priv, CTRL_R_R, CTRL_R_GB - CTRL_R_R + 1, false); + + v4l2_ctrl_new_std_menu_items(&priv->root.ctrl_handler, + &imx219_root_ctrl_ops, + V4L2_CID_TEST_PATTERN, + ARRAY_SIZE(imx219_test_pattern_menu) - 1, + 0, 0, imx219_test_pattern_menu); + + if (priv->root.ctrl_handler.error) + return priv->root.ctrl_handler.error; + + priv->root.sd.ctrl_handler = &priv->root.ctrl_handler; + + return 0; +} + +static const struct v4l2_subdev_ops imx219_root_subdev_ops = { + .core = &imx219_root_core_ops, + .video = &imx219_root_video_ops, + .pad = &imx219_root_pad_ops, +}; + +static const struct v4l2_subdev_ops imx219_sub_sd_ops = { + .pad = &imx219_sub_pad_ops, +}; + +static const struct media_entity_operations imx219_entity_ops = { + .link_validate = v4l2_subdev_link_validate, +}; + +static int imx219_registered(struct v4l2_subdev *sd) +{ + struct imx219_private *priv = imx219_get_priv(sd); + struct imx219_sub *parent = &priv->root; + struct imx219_sub *sub = &priv->pixel; + int ret; + + ret = v4l2_device_register_subdev(parent->sd.v4l2_dev, &sub->sd); + if (ret) + return ret; + + ret = media_create_pad_link(&sub->sd.entity, 0, &parent->sd.entity, 1, + MEDIA_LNK_FL_ENABLED | + MEDIA_LNK_FL_IMMUTABLE); + if (ret) { + v4l2_device_unregister_subdev(&sub->sd); + return ret; + } + + return 0; +} + +static void imx219_unregistered(struct v4l2_subdev *sd) +{ + struct imx219_private *priv = imx219_get_priv(sd); + + v4l2_device_unregister_subdev(&priv->pixel.sd); +} + +static const struct v4l2_subdev_internal_ops imx219_root_internal_ops = { + .registered = imx219_registered, + .unregistered = imx219_unregistered, +}; + +static int imx219_sub_init(struct imx219_private *priv, struct imx219_sub *sub, + size_t n_pad, const char *name) +{ + struct device *dev = priv->root.sd.dev; + int ret; + + v4l2_subdev_init(&sub->sd, &imx219_sub_sd_ops); + sub->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; + sub->sd.owner = THIS_MODULE; + sub->sd.entity.ops = &imx219_entity_ops; + sub->sd.dev = dev; + + snprintf(sub->sd.name, sizeof(sub->sd.name), "%s %s %s", + dev->driver->name, name, dev_name(dev)); + + sub->pad[0].flags = MEDIA_PAD_FL_SOURCE; + if (n_pad) + sub->pad[1].flags = MEDIA_PAD_FL_SINK; + + ret = media_entity_pads_init(&sub->sd.entity, n_pad, sub->pad); + if (ret) + return ret; + + return 0; +} + +static void imx219_sub_remove(struct imx219_sub *sub) +{ + media_entity_cleanup(&sub->sd.entity); +} + +static int imx219_read_caps(struct imx219_private *priv, struct device *dev) +{ + int val, fmt_type, fmt_subtype, i, n_cols, n_rows; + unsigned int n; + union { + struct imx219_limit_integration_time integ; + struct imx219_limit_pll pll; + struct imx219_limit_frame frame; + struct imx219_limit_image image; + } u; + + priv->ana_gain_m0 = val = imx219_readw(priv, R_ANA_GAIN_M0); + if (val < 0) + return val; + + priv->ana_gain_c0 = val = imx219_readw(priv, R_ANA_GAIN_C0); + if (val < 0) + return val; + + priv->ana_gain_m1 = val = imx219_readw(priv, R_ANA_GAIN_M1); + if (val < 0) + return val; + + priv->ana_gain_c1 = val = imx219_readw(priv, R_ANA_GAIN_C1); + if (val < 0) + return val; + + fmt_type = imx219_readb(priv, R_FRM_FMT_TYPE); + if (fmt_type < 0) + return val; + + if (fmt_type != FMT_TYPE_2BYTE) { + dev_err(dev, "unknown frame format type %u\n", fmt_type); + return -EINVAL; + } + + fmt_subtype = imx219_readb(priv, R_FRM_FMT_SUBTYPE); + if (fmt_subtype < 0) + return val; + + n_cols = FRM_FMT_COLS(fmt_subtype); + n_rows = FRM_FMT_ROWS(fmt_subtype); + for (i = n = 0; i < n_cols + n_rows; i++) { + const char *w = i < n_cols ? "columns" : "rows"; + int val; + + if (i == n_cols) + n = 0; + + val = imx219_readw(priv, R_FRM_FMT_DESC(i)); + if (val < 0) + return val; + + switch (FRM_FMT_DESC_CODE(val)) { + case FRM_FMT_DESC_EMBEDDED: + dev_info(dev, "%u embedded %s at %u\n", + FRM_FMT_DESC_VAL(val), w, n); + break; + + case FRM_FMT_DESC_VISIBLE: + dev_info(dev, "%u visible %s at %u\n", + FRM_FMT_DESC_VAL(val), w, n); + if (i < n_cols) { + priv->visible.left = n; + priv->visible.width = FRM_FMT_DESC_VAL(val); + } else { + priv->visible.top = n; + priv->visible.height = FRM_FMT_DESC_VAL(val); + } + break; + } + + n += FRM_FMT_DESC_VAL(val); + } + + if (priv->visible.width == 0 || priv->visible.height == 0) { + dev_err(dev, "unable to parse frame format\n"); + return -EINVAL; + } + + val = imx219_read(priv, R_LIMIT_INTEG, &u.integ, sizeof(u.integ)); + if (val < 0) + return val; + + priv->coarse_integration_time_min = + be16_to_cpu(u.integ.coarse_integration_time_min); + priv->coarse_integration_time_max_margin = + be16_to_cpu(u.integ.coarse_integration_time_max_margin); + + val = imx219_read(priv, R_LIMIT_FRAME, &u.frame, sizeof(u.frame)); + if (val < 0) + return val; + + priv->min_frame_lines = be16_to_cpu(u.frame.min_frame_length_lines); + priv->max_frame_lines = be16_to_cpu(u.frame.max_frame_length_lines); + priv->min_line_length_pck = be16_to_cpu(u.frame.min_line_length_pck); + priv->max_line_length_pck = be16_to_cpu(u.frame.max_line_length_pck); + priv->min_line_blank_pck = be16_to_cpu(u.frame.min_link_blank_pck); + priv->min_frame_blank_lines = + be16_to_cpu(u.frame.min_frame_blank_lines); + + val = imx219_read(priv, R_LIMIT_IMAGE, &u.image, sizeof(u.image)); + if (val < 0) + return val; + + priv->pixarray.left = be16_to_cpu(u.image.x_addr_min); + priv->pixarray.top = be16_to_cpu(u.image.y_addr_min); + priv->pixarray.width = be16_to_cpu(u.image.x_addr_max) - + priv->pixarray.left + 1; + priv->pixarray.height = be16_to_cpu(u.image.y_addr_max) - + priv->pixarray.top + 1; + priv->min_output_width = be16_to_cpu(u.image.min_x_output_size); + priv->min_output_height = be16_to_cpu(u.image.min_y_output_size); + priv->max_output_width = be16_to_cpu(u.image.max_x_output_size); + priv->max_output_height = be16_to_cpu(u.image.max_y_output_size); + + /* Clamp the maximum output width/height to the pixel array */ + priv->max_output_width = max_t(u32, priv->max_output_width, + priv->pixarray.width); + priv->max_output_height = max_t(u32, priv->max_output_height, + priv->pixarray.height); + + /* Some sanity checks - we make assumptions elsewhere */ + if (priv->max_output_height + priv->min_frame_blank_lines > + priv->max_frame_lines || + priv->max_output_height + priv->min_line_blank_pck > + priv->max_line_length_pck) + dev_warn(dev, "max frame size exceeds limit\n"); + + priv->fine_integ_time = val = imx219_readw(priv, R_FINE_INTEG_TIME); + if (val < 0) + return val; + + return 0; +} + +static int imx219_init_pixarray(struct imx219_private *priv) +{ + struct imx219_sub *sub = &priv->pixel; + struct imx219_limit_dig_gain dig_gain; + int ana_min, ana_max, ana_step; + int err; + + err = imx219_sub_init(priv, sub, 1, "pixel"); + if (err) + return err; + + sub->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR; + sub->pad_ops = &imx219_pix_pad_ops; + + /* Set the default sensor format and crop */ + sub->pad_ops->init_pad(priv, PAD_SRC, &sub->format[PAD_SRC], NULL); + + ana_min = imx219_readw(priv, R_ANA_GAIN_MIN); + if (ana_min < 0) + return ana_min; + + ana_max = imx219_readw(priv, R_ANA_GAIN_MAX); + if (ana_max < 0) + return ana_max; + + ana_step = imx219_readw(priv, R_ANA_GAIN_STEP); + if (ana_step < 0) + return ana_step; + + err = imx219_read(priv, R_LIMIT_DIG_GAIN, &dig_gain, sizeof(dig_gain)); + if (err < 0) + return err; + + v4l2_ctrl_handler_init(&sub->ctrl_handler, 6); + v4l2_ctrl_new_std(&sub->ctrl_handler, &imx219_pix_ctrl_ops, + V4L2_CID_GAIN, + be16_to_cpu(dig_gain.digital_gain_min), + be16_to_cpu(dig_gain.digital_gain_max), + be16_to_cpu(dig_gain.digital_gain_step_size), + be16_to_cpu(dig_gain.digital_gain_min)); + sub->ctrls[CTRL_P_VBLANK] = + v4l2_ctrl_new_std(&sub->ctrl_handler, &imx219_pix_ctrl_ops, + V4L2_CID_VBLANK, + priv->min_frame_blank_lines, + priv->max_frame_lines, 1, + priv->min_frame_blank_lines); + sub->ctrls[CTRL_P_HBLANK] = + v4l2_ctrl_new_std(&sub->ctrl_handler, &imx219_pix_ctrl_ops, + V4L2_CID_HBLANK, + priv->min_line_blank_pck, + priv->max_line_length_pck, 1, + priv->min_line_blank_pck); + v4l2_ctrl_new_std(&sub->ctrl_handler, &imx219_pix_ctrl_ops, + V4L2_CID_ANALOGUE_GAIN, + imx219_reg_to_gain(priv, ana_min), + imx219_reg_to_gain(priv, ana_max), + 1, imx219_reg_to_gain(priv, ana_min)); + sub->ctrls[CTRL_P_PIXEL_RATE] = + v4l2_ctrl_new_std(&sub->ctrl_handler, &imx219_pix_ctrl_ops, + V4L2_CID_PIXEL_RATE, + 2 * priv->pll[PLL_VT].clk[CLK_PIX].min_freq, + 2 * priv->pll[PLL_VT].clk[CLK_PIX].max_freq, + 1, + 2 * imx219_get_vt_pixclk(priv)); + sub->ctrls[CTRL_P_EXPOSURE] = + v4l2_ctrl_new_std(&sub->ctrl_handler, &imx219_pix_ctrl_ops, + V4L2_CID_EXPOSURE_ABSOLUTE, 0, 1, 1, 0); + + if (sub->ctrl_handler.error) + return sub->ctrl_handler.error; + + /* Read-only during implementation */ + sub->ctrls[CTRL_P_HBLANK]->flags |= V4L2_CTRL_FLAG_READ_ONLY; + sub->ctrls[CTRL_P_VBLANK]->flags |= V4L2_CTRL_FLAG_READ_ONLY; + + sub->sd.ctrl_handler = &sub->ctrl_handler; + + return 0; +} + +static int imx219_parse_dt(struct imx219_private *priv, struct device *dev) +{ + struct device_node *ep; + u32 val; + int err = 0; + + val = priv->mipi_vc; + of_property_read_u32(dev->of_node, "virtual-channel", &val); + priv->mipi_vc = val; + + for_each_endpoint_of_node(dev->of_node, ep) { + struct v4l2_fwnode_endpoint endpoint = { }; + int i; + + v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &endpoint); + if (endpoint.base.port != 0) { + dev_err(dev, "imx219 only has one endpoint\n"); + err = -EINVAL; + break; + } + + if (endpoint.bus_type != V4L2_MBUS_CSI2_DPHY) { + dev_err(dev, "imx219 requires a CSI2 DPHY endpoint\n"); + err = -EINVAL; + break; + } + + if (endpoint.bus.mipi_csi2.clock_lane || + endpoint.bus.mipi_csi2.lane_polarities[0]) { + dev_err(dev, "bad clock lane configuration\n"); + err = -EINVAL; + break; + } + + if (endpoint.bus.mipi_csi2.flags != + V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK) { + dev_err(dev, "clock must be non-continuous\n"); + err = -EINVAL; + break; + } + + priv->mipi_lanes = endpoint.bus.mipi_csi2.num_data_lanes; + for (i = 0; i < priv->mipi_lanes; i++) + if (endpoint.bus.mipi_csi2.data_lanes[i] != i + 1 || + endpoint.bus.mipi_csi2.lane_polarities[i + 1]) + break; + + if (i != priv->mipi_lanes) { + dev_err(dev, "bad data line configuration at index %d\n", i); + err = -EINVAL; + break; + } + } + + if (err) + of_node_put(ep); + + return err; +} + +static int imx219_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct imx219_private *priv; + int err, val; + + priv = devm_kzalloc(&client->dev, sizeof(*priv), GFP_KERNEL); + if (!priv) + return -ENOMEM; + + priv->inck = devm_clk_get(&client->dev, NULL); + if (IS_ERR(priv->inck)) + return PTR_ERR(priv->inck); + + priv->mipi_lanes = 4; + + v4l2_i2c_subdev_init(&priv->root.sd, client, &imx219_root_subdev_ops); + priv->root.sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; + priv->root.sd.internal_ops = &imx219_root_internal_ops; + priv->root.sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER; + priv->root.sd.entity.ops = &imx219_entity_ops; + priv->root.pad[0].flags = MEDIA_PAD_FL_SOURCE; + priv->root.pad[1].flags = MEDIA_PAD_FL_SINK; + priv->root.pad_ops = &imx219_int_root_pad_ops; + + if (client->dev.of_node) { + err = imx219_parse_dt(priv, &client->dev); + if (err) + return err; + } + + if (priv->mipi_lanes != 2 && priv->mipi_lanes != 4) { + dev_err(&client->dev, "imx219 requires either 2 or 4 lanes\n"); + return -EINVAL; + } + + priv->xclr_gpio = devm_gpiod_get(&client->dev, "xclr", GPIOD_OUT_HIGH); + if (IS_ERR(priv->xclr_gpio)) + return PTR_ERR(priv->xclr_gpio); + + pm_runtime_set_autosuspend_delay(&client->dev, 100); + pm_runtime_use_autosuspend(&client->dev); + pm_runtime_enable(&client->dev); + + err = pm_runtime_get_sync(&client->dev); + if (err) + goto rpm_cleanup; + + val = imx219_readb(priv, R_PX_ORDER); + if (val < 0) { + err = val; + goto rpm_cleanup_put; + } + + priv->pixel_order = val; + priv->pixel_format = imx219_formats[0][val]; + + val = imx219_readw(priv, R_MODEL_ID); + if (val < 0) { + err = val; + goto rpm_cleanup_put; + } + + dev_info(&client->dev, "detected IMX%X sensor\n", val); + + mutex_init(&priv->mutex); + + err = imx219_read_caps(priv, &client->dev); + if (err) + goto mutex_cleanup; + + err = imx219_create_controls(priv); + if (err) + goto mutex_cleanup; + + err = media_entity_pads_init(&priv->root.sd.entity, 2, priv->root.pad); + if (err) + goto mutex_cleanup; + + err = imx219_init_pixarray(priv); + if (err) + goto media_cleanup; + + /* Copy the pixel source format to the root sink */ + priv->root.format[PAD_SINK] = priv->pixel.format[PAD_SRC]; + imx219_framefmt_to_rect(&priv->root.sink_crop, + &priv->root.format[PAD_SINK]); + + /* Set the default output format */ + priv->root.format[PAD_SRC] = priv->root.format[PAD_SINK]; + priv->root.format[PAD_SRC].width = priv->max_output_width; + priv->root.format[PAD_SRC].height = priv->max_output_height; + imx219_framefmt_to_rect(&priv->root.sink_compose, + &priv->root.format[PAD_SRC]); + + priv->frame_interval.numerator = 1; + priv->frame_interval.denominator = 60; + + /* Set hardware settings */ + priv->params.line_length = cpu_to_be16(3448); + imx219_update_output_bpp(priv, &priv->root.format[PAD_SRC]); + imx219_root_set_selection(priv, &priv->root, PAD_SINK, + V4L2_SEL_TGT_CROP, &priv->root.sink_crop, + NULL); + imx219_root_set_selection(priv, &priv->root, PAD_SRC, + V4L2_SEL_TGT_COMPOSE, + &priv->root.sink_compose, + &priv->root.sink_crop); + priv->params.x_odd_inc = 1; + priv->params.y_odd_inc = 1; + + err = v4l2_async_register_subdev(&priv->root.sd); + if (err) + goto pixel_cleanup; + + pm_runtime_put_autosuspend(&client->dev); + + return 0; + +pixel_cleanup: + imx219_sub_remove(&priv->pixel); +media_cleanup: + media_entity_cleanup(&priv->root.sd.entity); +mutex_cleanup: + v4l2_ctrl_handler_free(&priv->root.ctrl_handler); + mutex_destroy(&priv->mutex); +rpm_cleanup_put: + pm_runtime_put_autosuspend(&client->dev); +rpm_cleanup: + pm_runtime_disable(&client->dev); + return err; +} + +static int imx219_remove(struct i2c_client *client) +{ + struct imx219_private *priv = imx219_get_clientdata(client); + + v4l2_async_unregister_subdev(&priv->root.sd); + imx219_sub_remove(&priv->pixel); + media_entity_cleanup(&priv->root.sd.entity); + v4l2_ctrl_handler_free(&priv->root.ctrl_handler); + mutex_destroy(&priv->mutex); + pm_runtime_disable(&client->dev); + gpiod_set_value(priv->xclr_gpio, 1); + + return 0; +} + +static int imx219_rpm_suspend(struct device *dev) +{ + struct imx219_private *priv = imx219_get_root_priv(dev_get_drvdata(dev)); + + gpiod_set_value(priv->xclr_gpio, 1); + + clk_disable_unprepare(priv->inck); + + return 0; +} + +static int imx219_rpm_resume(struct device *dev) +{ + struct imx219_private *priv = imx219_get_root_priv(dev_get_drvdata(dev)); + unsigned long inck; + unsigned int t6_ms; + int ret; + + ret = clk_prepare_enable(priv->inck); + if (ret) + return ret; + + gpiod_set_value(priv->xclr_gpio, 0); + + inck = clk_get_rate(priv->inck); + + /* + * Calculate initialisation delay + * Silicon initialisation time, t6 = 32000 clocks. + */ + t6_ms = DIV_ROUND_UP(32000 * 1000, inck); + + msleep(t6_ms); + + /* Reset the device */ + imx219_writeb(priv, R_SW_RESET, 1); + msleep(2); + imx219_writeb(priv, R_SW_RESET, 0); + msleep(2); + + /* + * Obtain and calculate the PLL limits given the current input + * clock. + */ + ret = imx219_calculate_pll_limits(priv, inck); + if (ret < 0) { + gpiod_set_value(priv->xclr_gpio, 1); + clk_disable_unprepare(priv->inck); + return ret; + } + + /* + * Select automatic DPHY control - the sensor will calculate + * the DPHY settings in registers 0x0118 to 0x0126 inclusive + * depending on clock rate, PLL parameters and format. + */ + imx219_writeb(priv, R_DPHY_CTRL, 0); + /* Set the clock frequency - in units of 1/256 MHz */ + imx219_writew(priv, R_EXCK_FREQ, mult_frac(inck, 256, 1000000)); + + /* + * Set the PLL pre-divider to our calculated setting. According + * to documentation, this should be set automatically, but doesn't + * appear to happen. + */ + imx219_writeb(priv, R_PREPLLCK_VT_DIV, priv->pll_pre_div); + imx219_writeb(priv, R_PREPLLCK_OP_DIV, priv->pll_pre_div); + + /* + * Set the PLL divider to a sane setting as the default may + * be in excess of the allowable limits given the input and + * output clocks. + */ + imx219_writew(priv, R_PLL_VT_MPY, priv->pll[PLL_VT].max_mpy); + imx219_writew(priv, R_PLL_OP_MPY, priv->pll[PLL_OP].max_mpy); + + /* + * Wait for the remainder of the VRL charge time + * VRL charge time, t5 = 6ms + * We have already waited t6_ms and 4ms + */ + if (t6_ms + 4 < 6) + msleep(2 - t6_ms); + + return 0; +} + +static const struct dev_pm_ops imx219_pm_ops = { + SET_RUNTIME_PM_OPS(imx219_rpm_suspend, imx219_rpm_resume, NULL) +}; + +static const struct of_device_id imx219_of_id[] = { + { .compatible = "sony,imx219" }, + { }, +}; +MODULE_DEVICE_TABLE(of, imx219_of_id); + +static const struct i2c_device_id imx219_i2c_id[] = { + { "imx219", 0 }, + { }, +}; +MODULE_DEVICE_TABLE(i2c, imx219_i2c_id); + +static struct i2c_driver imx219_i2c_driver = { + .driver = { + .name = "imx219", + .of_match_table = imx219_of_id, + .pm = &imx219_pm_ops, + }, + .probe = imx219_probe, + .remove = imx219_remove, + .id_table = imx219_i2c_id, +}; + +module_i2c_driver(imx219_i2c_driver); + +MODULE_LICENSE("GPL"); |