summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--drivers/media/i2c/Kconfig10
-rw-r--r--drivers/media/i2c/Makefile1
-rw-r--r--drivers/media/i2c/imx219-rmk.c3019
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");
generated by cgit (git 2.34.1) at 2025-03-15 17:19:45 +0000