[2/2] media: i2c: Add driver for ST VD56G3 camera sensor

Commit Message

Sylvain Petinot April 17, 2024, 1:34 p.m. UTC

Add V4L2 sub-device driver for STMicroelectronics VD56G3 camera sensor.
This is a 1.5 M pixel global shutter image sensor with an active array
size of 1124 x 1364 (portrait orientation).

The driver supports Mono (VD56G3) and Color (VD66GY) variants.

Signed-off-by: Sylvain Petinot <[e-mail]>
---
 drivers/media/i2c/Kconfig     |   11 +
 drivers/media/i2c/Makefile    |    1 +
 drivers/media/i2c/st-vd56g3.c | 1619 +++++++++++++++++++++++++++++++++
 3 files changed, 1631 insertions(+)
 create mode 100644 drivers/media/i2c/st-vd56g3.c

Comments

kernel test robot April 18, 2024, 5:39 a.m. UTC | #1

Hi Sylvain,

kernel test robot noticed the following build warnings:

[auto build test WARNING on media-tree/master]
[also build test WARNING on linus/master v6.9-rc4 next-20240417]
[If your patch is applied to the wrong git tree, kindly drop us a note.
And when submitting patch, we suggest to use '--base' as documented in
https://git-scm.com/docs/git-format-patch#_base_tree_information]

url:    https://github.com/intel-lab-lkp/linux/commits/Sylvain-Petinot/media-dt-bindings-Add-ST-VD56G3-camera-sensor-binding/20240417-213838
base:   git://linuxtv.org/media_tree.git master
patch link:    https://lore.kernel.org/r/20240417133453.17406-3-sylvain.petinot%40foss.st.com
patch subject: [PATCH 2/2] media: i2c: Add driver for ST VD56G3 camera sensor
config: m68k-randconfig-r071-20240418 (https://download.01.org/0day-ci/archive/20240418/[e-mail]/config)
compiler: m68k-linux-gcc (GCC) 13.2.0
reproduce (this is a W=1 build): (https://download.01.org/0day-ci/archive/20240418/[e-mail]/reproduce)

If you fix the issue in a separate patch/commit (i.e. not just a new version of
the same patch/commit), kindly add following tags
| Reported-by: kernel test robot <[e-mail]>
| Closes: https://lore.kernel.org/oe-kbuild-all/[e-mail]/

All warnings (new ones prefixed by >>):

>> drivers/media/i2c/st-vd56g3.c:1164:12: warning: 'vd56g3_runtime_suspend' defined but not used [-Wunused-function]
    1164 | static int vd56g3_runtime_suspend(struct device *dev)
         |            ^~~~~~~~~~~~~~~~~~~~~~
>> drivers/media/i2c/st-vd56g3.c:1142:12: warning: 'vd56g3_runtime_resume' defined but not used [-Wunused-function]
    1142 | static int vd56g3_runtime_resume(struct device *dev)
         |            ^~~~~~~~~~~~~~~~~~~~~


vim +/vd56g3_runtime_suspend +1164 drivers/media/i2c/st-vd56g3.c

  1141	
> 1142	static int vd56g3_runtime_resume(struct device *dev)
  1143	{
  1144		struct v4l2_subdev *sd = dev_get_drvdata(dev);
  1145		struct vd56g3 *sensor = to_vd56g3(sd);
  1146		struct i2c_client *client = sensor->i2c_client;
  1147		int ret;
  1148	
  1149		ret = vd56g3_power_on(sensor);
  1150		if (ret) {
  1151			dev_err(&client->dev, "Failed to power on %d", ret);
  1152			return ret;
  1153		}
  1154	
  1155		ret = vd56g3_boot(sensor);
  1156		if (ret) {
  1157			dev_err(&client->dev, "sensor boot failed %d", ret);
  1158			return ret;
  1159		}
  1160	
  1161		return ret;
  1162	}
  1163	
> 1164	static int vd56g3_runtime_suspend(struct device *dev)
  1165	{
  1166		struct v4l2_subdev *sd = dev_get_drvdata(dev);
  1167		struct vd56g3 *sensor = to_vd56g3(sd);
  1168	
  1169		return vd56g3_power_off(sensor);
  1170	}
  1171

Tommaso Merciai April 18, 2024, 9:17 a.m. UTC | #2

Hi Sylvain,

Thanks for the patch.
Quick review on my side :)

Premise:
Unfortunately I'm not able to test this drv on my side (missing hw)
either I'm not able to get the datasheet Googling a bit.

On Wed, Apr 17, 2024 at 03:34:53PM +0200, Sylvain Petinot wrote:
> Add V4L2 sub-device driver for STMicroelectronics VD56G3 camera sensor.
> This is a 1.5 M pixel global shutter image sensor with an active array
> size of 1124 x 1364 (portrait orientation).
> 
> The driver supports Mono (VD56G3) and Color (VD66GY) variants.
> 
> Signed-off-by: Sylvain Petinot <[e-mail]>
> ---
>  drivers/media/i2c/Kconfig     |   11 +
>  drivers/media/i2c/Makefile    |    1 +
>  drivers/media/i2c/st-vd56g3.c | 1619 +++++++++++++++++++++++++++++++++
>  3 files changed, 1631 insertions(+)
>  create mode 100644 drivers/media/i2c/st-vd56g3.c
> 
> diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
> index 56f276b920ab..a2d7271eef88 100644
> --- a/drivers/media/i2c/Kconfig
> +++ b/drivers/media/i2c/Kconfig
> @@ -657,6 +657,17 @@ config VIDEO_S5K6A3
>  	  This is a V4L2 sensor driver for Samsung S5K6A3 raw
>  	  camera sensor.
>  
> +config VIDEO_ST_VD56G3
> +	tristate "ST VD56G3 sensor support"
> +	select V4L2_CCI_I2C
> +	depends on OF && GPIOLIB
> +	help
> +	  This is a Video4Linux2 sensor driver for the ST VD56G3
> +	  camera sensor.
> +
> +	  To compile this driver as a module, choose M here: the
> +	  module will be called st-vd56g3.
> +
>  config VIDEO_ST_VGXY61
>  	tristate "ST VGXY61 sensor support"
>  	select V4L2_CCI_I2C
> diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
> index dfbe6448b549..b469cf0f8113 100644
> --- a/drivers/media/i2c/Makefile
> +++ b/drivers/media/i2c/Makefile
> @@ -124,6 +124,7 @@ obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
>  obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
>  obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
>  obj-$(CONFIG_VIDEO_ST_MIPID02) += st-mipid02.o
> +obj-$(CONFIG_VIDEO_ST_VD56G3) += st-vd56g3.o
>  obj-$(CONFIG_VIDEO_ST_VGXY61) += st-vgxy61.o
>  obj-$(CONFIG_VIDEO_TC358743) += tc358743.o
>  obj-$(CONFIG_VIDEO_TC358746) += tc358746.o
> diff --git a/drivers/media/i2c/st-vd56g3.c b/drivers/media/i2c/st-vd56g3.c
> new file mode 100644
> index 000000000000..eff819941456
> --- /dev/null
> +++ b/drivers/media/i2c/st-vd56g3.c
> @@ -0,0 +1,1619 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * A V4L2 driver for ST VD56G3 (Mono) and VD66GY (RGB) global shutter cameras.
> + * Copyright (C) 2024, STMicroelectronics SA
> + */
> +
> +#include <linux/clk.h>
> +#include <linux/delay.h>
> +#include <linux/gpio/consumer.h>
> +#include <linux/i2c.h>
> +#include <linux/iopoll.h>
> +#include <linux/module.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/regmap.h>
> +#include <linux/regulator/consumer.h>
> +#include <linux/units.h>
> +
> +#include <asm/unaligned.h>
> +
> +#include <media/mipi-csi2.h>
> +#include <media/v4l2-async.h>
> +#include <media/v4l2-cci.h>
> +#include <media/v4l2-ctrls.h>
> +#include <media/v4l2-device.h>
> +#include <media/v4l2-event.h>
> +#include <media/v4l2-fwnode.h>
> +#include <media/v4l2-subdev.h>
> +
> +/* Register Map */
> +#define VD56G3_REG_MODEL_ID				CCI_REG16_LE(0x0000)
> +#define VD56G3_MODEL_ID					0x5603
> +#define VD56G3_REG_REVISION				CCI_REG16_LE(0x0002)
> +#define VD56G3_REG_OPTICAL_REVISION			CCI_REG8(0x001a)
> +#define VD56G3_OPTICAL_REVISION_MONO			0
> +#define VD56G3_OPTICAL_REVISION_BAYER			1
> +#define VD56G3_REG_SYSTEM_FSM				CCI_REG8(0x0028)
> +#define VD56G3_SYSTEM_FSM_READY_TO_BOOT			0x01
> +#define VD56G3_SYSTEM_FSM_SW_STBY			0x02
> +#define VD56G3_SYSTEM_FSM_STREAMING			0x03
> +#define VD56G3_REG_APPLIED_COARSE_EXPOSURE		CCI_REG16_LE(0x0064)
> +#define VD56G3_REG_APPLIED_ANALOG_GAIN			CCI_REG8(0x0068)
> +#define VD56G3_REG_APPLIED_DIGITAL_GAIN			CCI_REG16_LE(0x006a)
> +#define VD56G3_REG_BOOT					CCI_REG8(0x0200)
> +#define VD56G3_CMD_ACK					0
> +#define VD56G3_CMD_BOOT					1
> +#define VD56G3_REG_STBY					CCI_REG8(0x0201)
> +#define VD56G3_CMD_START_STREAM				1
> +#define VD56G3_REG_STREAMING				CCI_REG8(0x0202)
> +#define VD56G3_CMD_STOP_STREAM				1
> +#define VD56G3_REG_EXT_CLOCK				CCI_REG32_LE(0x0220)
> +#define VD56G3_REG_CLK_PLL_PREDIV			CCI_REG8(0x0224)
> +#define VD56G3_REG_CLK_SYS_PLL_MULT			CCI_REG8(0x0226)
> +#define VD56G3_REG_ORIENTATION				CCI_REG8(0x0302)
> +#define VD56G3_REG_FORMAT_CTRL				CCI_REG8(0x030a)
> +#define VD56G3_REG_OIF_CTRL				CCI_REG16_LE(0x030c)
> +#define VD56G3_REG_OIF_IMG_CTRL				CCI_REG8(0x030f)
> +#define VD56G3_REG_OIF_CSI_BITRATE			CCI_REG16_LE(0x0312)
> +#define VD56G3_REG_DUSTER_CTRL				CCI_REG8(0x0318)
> +#define VD56G3_DUSTER_DISABLE				0
> +#define VD56G3_DUSTER_ENABLE_DEF_MODULES		0x13
> +#define VD56G3_REG_ISL_ENABLE				CCI_REG8(0x0333)
> +#define VD56G3_REG_DARKCAL_CTRL				CCI_REG8(0x0340)
> +#define VD56G3_DARKCAL_ENABLE				1
> +#define VD56G3_DARKCAL_DISABLE_DARKAVG			2
> +#define VD56G3_REG_PATGEN_CTRL				CCI_REG16_LE(0x0400)
> +#define VD56G3_PATGEN_ENABLE				1
> +#define VD56G3_PATGEN_TYPE_SHIFT			4
> +#define VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE		CCI_REG16_LE(0x042a)
> +#define VD56G3_REG_AE_COLDSTART_ANALOG_GAIN		CCI_REG8(0x042c)
> +#define VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN		CCI_REG16_LE(0x042e)
> +#define VD56G3_REG_AE_ROI_START_H			CCI_REG16_LE(0x0432)
> +#define VD56G3_REG_AE_ROI_START_V			CCI_REG16_LE(0x0434)
> +#define VD56G3_REG_AE_ROI_END_H				CCI_REG16_LE(0x0436)
> +#define VD56G3_REG_AE_ROI_END_V				CCI_REG16_LE(0x0438)
> +#define VD56G3_REG_AE_COMPENSATION			CCI_REG16_LE(0x043a)
> +#define VD56G3_REG_EXP_MODE				CCI_REG8(0x044c)
> +#define VD56G3_EXP_MODE_AUTO				0
> +#define VD56G3_EXP_MODE_FREEZE				1
> +#define VD56G3_EXP_MODE_MANUAL				2
> +#define VD56G3_REG_MANUAL_ANALOG_GAIN			CCI_REG8(0x044d)
> +#define VD56G3_REG_MANUAL_COARSE_EXPOSURE		CCI_REG16_LE(0x044e)
> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0		CCI_REG16_LE(0x0450)
> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1		CCI_REG16_LE(0x0452)
> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2		CCI_REG16_LE(0x0454)
> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3		CCI_REG16_LE(0x0456)
> +#define VD56G3_REG_FRAME_LENGTH				CCI_REG16_LE(0x0458)
> +#define VD56G3_REG_Y_START				CCI_REG16_LE(0x045a)
> +#define VD56G3_REG_Y_END				CCI_REG16_LE(0x045c)
> +#define VD56G3_REG_OUT_ROI_X_START			CCI_REG16_LE(0x045e)
> +#define VD56G3_REG_OUT_ROI_X_END			CCI_REG16_LE(0x0460)
> +#define VD56G3_REG_OUT_ROI_Y_START			CCI_REG16_LE(0x0462)
> +#define VD56G3_REG_OUT_ROI_Y_END			CCI_REG16_LE(0x0464)
> +#define VD56G3_REG_GPIO_0_CTRL				CCI_REG8(0x0467)
> +#define VD56G3_GPIOX_GPIO_IN				0x01
> +#define VD56G3_GPIOX_STROBE_MODE			0x02
> +#define VD56G3_REG_READOUT_CTRL				CCI_REG8(0x047e)
> +#define READOUT_NORMAL					0x00
> +#define READOUT_DIGITAL_BINNING_X2			0x01
> +
> +/*
> + * The VD56G3 pixel array is organized as follows:
> + *
> + * +--------------------------------+
> + * |                                | \
> + * |   +------------------------+   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   |   Default resolution   |   |  | Native height (1364)
> + * |   |      1120 x 1360       |   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   +------------------------+   |  |
> + * |                                | /
> + * +--------------------------------+
> + *   <----------------------------->
> + *                     \-------------------  Native width (1124)
> + *
> + * The native resolution is 1124x1364.
> + * The recommended/default resolution is 1120x1360 (multiple of 16).
> + */
> +#define VD56G3_NATIVE_WIDTH				1124
> +#define VD56G3_NATIVE_HEIGHT				1364
> +#define VD56G3_DEFAULT_WIDTH				1120
> +#define VD56G3_DEFAULT_HEIGHT				1360
> +#define VD56G3_DEFAULT_MODE				1
> +
> +/* PLL settings */
> +#define VD56G3_TARGET_PLL				804000000UL
> +#define VD56G3_VT_CLOCK_DIV				5
> +
> +/* Line length and Frame length (settings are for standard 10bits ADC mode) */
> +#define VD56G3_LINE_LENGTH_MIN				1236
> +#define VD56G3_VBLANK_MIN				110
> +#define VD56G3_FRAME_LENGTH_DEF_60FPS			2168
> +
> +/* Exposure settings */
> +#define VD56G3_EXPOSURE_MARGIN				75
> +#define VD56G3_EXPOSURE_DEFAULT				1420
> +
> +/* Output Interface settings */
> +#define VD56G3_MAX_CSI_DATA_LANES			2
> +#define VD56G3_LINK_FREQ_DEF_1LANE			750000000UL
> +#define VD56G3_LINK_FREQ_DEF_2LANES			402000000UL
> +
> +/* GPIOs */
> +#define VD56G3_NB_GPIOS					8
> +
> +/* regulator supplies */
> +static const char *const vd56g3_supply_names[] = {
> +	"VCORE",
> +	"VDDIO",
> +	"VANA",
> +};
> +
> +#define VD56G3_NUM_SUPPLIES		ARRAY_SIZE(vd56g3_supply_names)
> +
> +/* -----------------------------------------------------------------------------
> + * Models (VD56G3: Mono, VD66GY: Bayer RGB), Modes and formats
> + */
> +
> +enum vd56g3_models {
> +	VD56G3_MODEL_VD56G3,
> +	VD56G3_MODEL_VD66GY,
> +};
> +
> +struct vd56g3_mode {
> +	u32 width;
> +	u32 height;
> +};
> +
> +static const struct vd56g3_mode vd56g3_supported_modes[] = {
> +	{
> +		.width = VD56G3_NATIVE_WIDTH,
> +		.height = VD56G3_NATIVE_HEIGHT,
> +	},
> +	{
> +		.width = VD56G3_DEFAULT_WIDTH,
> +		.height = VD56G3_DEFAULT_HEIGHT,
> +	},
> +	{
> +		.width = 1024,
> +		.height = 1280,
> +	},
> +	{
> +		.width = 1024,
> +		.height = 768,
> +	},
> +	{
> +		.width = 768,
> +		.height = 1024,
> +	},
> +	{
> +		.width = 720,
> +		.height = 1280,
> +	},
> +	{
> +		.width = 640,
> +		.height = 480,
> +	},
> +	{
> +		.width = 480,
> +		.height = 640,
> +	},
> +	{
> +		.width = 320,
> +		.height = 240,
> +	},
> +};
> +
> +/*
> + * Sensor support 8bits and 10bits output in both variants
> + *  - Monochrome
> + *  - RGB (with all H/V flip variations)
> + */
> +static const unsigned int vd56g3_mbus_codes[2][5] = {
> +	{
> +		MEDIA_BUS_FMT_Y8_1X8,
> +		MEDIA_BUS_FMT_SGRBG8_1X8,
> +		MEDIA_BUS_FMT_SRGGB8_1X8,
> +		MEDIA_BUS_FMT_SBGGR8_1X8,
> +		MEDIA_BUS_FMT_SGBRG8_1X8,
> +	},
> +	{
> +		MEDIA_BUS_FMT_Y10_1X10,
> +		MEDIA_BUS_FMT_SGRBG10_1X10,
> +		MEDIA_BUS_FMT_SRGGB10_1X10,
> +		MEDIA_BUS_FMT_SBGGR10_1X10,
> +		MEDIA_BUS_FMT_SGBRG10_1X10,
> +	},
> +};
> +
> +enum vd56g3_expo_state {
> +	VD56G3_EXPO_AUTO,
> +	VD56G3_EXPO_AUTO_FREEZE,
> +	VD56G3_EXPO_MANUAL
> +};
> +
> +struct vd56g3 {
> +	struct i2c_client *i2c_client;
> +	struct v4l2_subdev sd;
> +	struct media_pad pad;
> +	struct regulator_bulk_data supplies[VD56G3_NUM_SUPPLIES];
> +	struct gpio_desc *reset_gpio;
> +	struct clk *xclk;
> +	struct regmap *regmap;
> +	u32 ext_clock;
> +	u32 pll_prediv;
> +	u32 pll_mult;
> +	u32 pixel_clock;
> +	u16 oif_ctrl;
> +	int nb_of_lane;

I think you can use u8 for nb_of_lane;

> +	u32 gpios[VD56G3_NB_GPIOS];
> +	unsigned long ext_leds_mask;
> +	bool is_mono;
> +	struct v4l2_ctrl_handler ctrl_handler;
> +	struct v4l2_ctrl *hblank_ctrl;
> +	struct v4l2_ctrl *vblank_ctrl;
> +	struct {
> +		struct v4l2_ctrl *hflip_ctrl;
> +		struct v4l2_ctrl *vflip_ctrl;
> +	};
> +	struct v4l2_ctrl *patgen_ctrl;
> +	struct {
> +		struct v4l2_ctrl *ae_ctrl;
> +		struct v4l2_ctrl *expo_ctrl;
> +		struct v4l2_ctrl *again_ctrl;
> +		struct v4l2_ctrl *dgain_ctrl;
> +	};
> +	struct v4l2_ctrl *ae_lock_ctrl;
> +	struct v4l2_ctrl *ae_bias_ctrl;
> +	struct v4l2_ctrl *led_ctrl;
> +	bool streaming;
> +};
> +
> +static inline struct vd56g3 *to_vd56g3(struct v4l2_subdev *sd)
> +{
> +	return container_of(sd, struct vd56g3, sd);

I think here you can use container_of_const

> +}
> +
> +static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
> +{
> +	return &container_of(ctrl->handler, struct vd56g3, ctrl_handler)->sd;

Same here.

> +}
> +
> +/* -----------------------------------------------------------------------------
> + * Additional i2c register helpers
> + */
> +
> +static int vd56g3_poll_reg(struct vd56g3 *sensor, u32 reg, u8 poll_val,
> +			   int *err)
> +{
> +	unsigned int val = 0;
> +	int ret;
> +
> +	if (err && *err)
> +		return *err;
> +
> +	reg = reg & CCI_REG_ADDR_MASK;
> +	ret = regmap_read_poll_timeout(sensor->regmap, reg, val,
> +				       (val == poll_val), 2000,
> +				       500 * USEC_PER_MSEC);
> +
> +	if (ret && err)
> +		*err = ret;
> +
> +	return ret;
> +}
> +
> +static int vd56g3_wait_state(struct vd56g3 *sensor, int state, int *err)
> +{
> +	return vd56g3_poll_reg(sensor, VD56G3_REG_SYSTEM_FSM, state, err);
> +}
> +
> +/* -----------------------------------------------------------------------------
> + * Controls: definitions, helpers and handlers
> + */
> +
> +static const char *const vd56g3_tp_menu[] = { "Disabled", "Solid", "Colorbar",
> +					      "Gradbar",  "Hgrey", "Vgrey",
> +					      "Dgrey",	  "PN28" };
> +
> +static const s64 vd56g3_ev_bias_qmenu[] = { -4000, -3500, -3000, -2500, -2000,
> +					    -1500, -1000, -500,	 0,	500,
> +					    1000,  1500,  2000,	 2500,	3000,
> +					    3500,  4000 };
> +
> +static const s64 vd56g3_link_freq_1lane[] = { VD56G3_LINK_FREQ_DEF_1LANE };
> +
> +static const s64 vd56g3_link_freq_2lanes[] = { VD56G3_LINK_FREQ_DEF_2LANES };
> +
> +static u8 vd56g3_get_bpp(__u32 code)
> +{
> +	switch (code) {
> +	case MEDIA_BUS_FMT_Y8_1X8:
> +	case MEDIA_BUS_FMT_SGRBG8_1X8:
> +	case MEDIA_BUS_FMT_SRGGB8_1X8:
> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
> +	case MEDIA_BUS_FMT_SGBRG8_1X8:
> +		return 8;
> +	case MEDIA_BUS_FMT_Y10_1X10:
> +	case MEDIA_BUS_FMT_SGRBG10_1X10:
> +	case MEDIA_BUS_FMT_SRGGB10_1X10:
> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
> +	case MEDIA_BUS_FMT_SGBRG10_1X10:
> +		return 10;
> +	default:
> +		/* Should never happen */
> +		WARN(1, "Unsupported code %d. default to 8 bpp", code);
> +	}
> +
> +	return 8;
> +}
> +
> +static u8 vd56g3_get_datatype(__u32 code)
> +{
> +	switch (code) {
> +	case MEDIA_BUS_FMT_Y8_1X8:
> +	case MEDIA_BUS_FMT_SGRBG8_1X8:
> +	case MEDIA_BUS_FMT_SRGGB8_1X8:
> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
> +	case MEDIA_BUS_FMT_SGBRG8_1X8:
> +		return MIPI_CSI2_DT_RAW8;
> +	case MEDIA_BUS_FMT_Y10_1X10:
> +	case MEDIA_BUS_FMT_SGRBG10_1X10:
> +	case MEDIA_BUS_FMT_SRGGB10_1X10:
> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
> +	case MEDIA_BUS_FMT_SGBRG10_1X10:
> +		return MIPI_CSI2_DT_RAW10;
> +	default:
> +		/* Should never happen */
> +		WARN(1, "Unsupported code %d. default to MIPI_CSI2_DT_RAW8",
> +		     code);
> +	}
> +
> +	return MIPI_CSI2_DT_RAW8;
> +}
> +
> +static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
> +{
> +	u64 exposure = 0;
> +	u64 again = 0;
> +	u64 dgain = 0;
> +	int ret = 0;
> +
> +	/*
> +	 * When 'force_cur_val' is enabled, save the ctrl value in 'cur.val'
> +	 * instead of the normal 'val', this is used during poweroff to cache
> +	 * volatile ctrls and enable coldstart.
> +	 */
> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_COARSE_EXPOSURE, &exposure,
> +		 &ret);
> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_ANALOG_GAIN, &again, &ret);
> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_DIGITAL_GAIN, &dgain, &ret);
> +	if (ret)
> +		return ret;
> +
> +	if (force_cur_val) {
> +		sensor->expo_ctrl->cur.val = exposure;
> +		sensor->again_ctrl->cur.val = again;
> +		sensor->dgain_ctrl->cur.val = dgain;
> +	} else {
> +		sensor->expo_ctrl->val = exposure;
> +		sensor->again_ctrl->val = again;
> +		sensor->dgain_ctrl->val = dgain;
> +	}
> +
> +	return ret;
> +}
> +
> +static int vd56g3_update_patgen(struct vd56g3 *sensor, u32 patgen_index)
> +{
> +	u32 pattern = patgen_index <= 3 ? patgen_index : patgen_index + 12;
> +	u16 patgen = pattern << VD56G3_PATGEN_TYPE_SHIFT;
> +	u8 duster = VD56G3_DUSTER_ENABLE_DEF_MODULES;
> +	u8 darkcal = VD56G3_DARKCAL_ENABLE;
> +	int ret = 0;
> +
> +	if (patgen_index) {
> +		patgen |= VD56G3_PATGEN_ENABLE;
> +		duster = VD56G3_DUSTER_DISABLE;
> +		darkcal = VD56G3_DARKCAL_DISABLE_DARKAVG;
> +	}
> +
> +	cci_write(sensor->regmap, VD56G3_REG_DUSTER_CTRL, duster, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_DARKCAL_CTRL, darkcal, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_PATGEN_CTRL, patgen, &ret);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_update_expo_cluster(struct vd56g3 *sensor, bool is_auto)
> +{
> +	int ret = 0;
> +	enum vd56g3_expo_state expo_state = is_auto ? VD56G3_EXP_MODE_AUTO :
> +						      VD56G3_EXP_MODE_MANUAL;
> +
> +	if (sensor->ae_ctrl->is_new)
> +		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
> +			  &ret);
> +
> +	/* In Auto expo, set coldstart parameters */
> +	if (is_auto && sensor->ae_ctrl->is_new) {
> +		cci_write(sensor->regmap,
> +			  VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE,
> +			  sensor->expo_ctrl->val, &ret);
> +		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_ANALOG_GAIN,
> +			  sensor->again_ctrl->val, &ret);
> +		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN,
> +			  sensor->dgain_ctrl->val, &ret);
> +	}
> +
> +	/* In Manual expo, set exposure, analog and digital gains */
> +	if (!is_auto && sensor->expo_ctrl->is_new)
> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_COARSE_EXPOSURE,
> +			  sensor->expo_ctrl->val, &ret);
> +
> +	if (!is_auto && sensor->again_ctrl->is_new)
> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_ANALOG_GAIN,
> +			  sensor->again_ctrl->val, &ret);
> +
> +	if (!is_auto && sensor->dgain_ctrl->is_new) {
> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0,
> +			  sensor->dgain_ctrl->val, &ret);
> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1,
> +			  sensor->dgain_ctrl->val, &ret);
> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2,
> +			  sensor->dgain_ctrl->val, &ret);
> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3,
> +			  sensor->dgain_ctrl->val, &ret);
> +	}
> +
> +	return ret;
> +}
> +
> +static int vd56g3_lock_exposure(struct vd56g3 *sensor, u32 lock_val)
> +{
> +	bool ae_lock = lock_val & V4L2_LOCK_EXPOSURE;
> +	enum vd56g3_expo_state expo_state = ae_lock ? VD56G3_EXP_MODE_FREEZE :
> +						      VD56G3_EXP_MODE_AUTO;
> +	int ret = 0;
> +
> +	if (sensor->ae_ctrl->val == V4L2_EXPOSURE_AUTO)
> +		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
> +			  &ret);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_write_gpiox(struct vd56g3 *sensor, unsigned long gpio_mask)
> +{
> +	unsigned long io;
> +	u32 gpio_val;
> +	int ret = 0;
> +
> +	for_each_set_bit(io, &gpio_mask, VD56G3_NB_GPIOS) {
> +		gpio_val = sensor->gpios[io];
> +
> +		if (gpio_val == VD56G3_GPIOX_STROBE_MODE &&
> +		    sensor->led_ctrl->val == V4L2_FLASH_LED_MODE_NONE)
> +			gpio_val = VD56G3_GPIOX_GPIO_IN;
> +
> +		cci_write(sensor->regmap, VD56G3_REG_GPIO_0_CTRL + io, gpio_val,
> +			  &ret);
> +	}
> +
> +	return ret;
> +}
> +
> +static int vd56g3_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
> +{
> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
> +	int ret = 0;
> +
> +	/* Interact with HW only when it is powered ON */
> +	if (!pm_runtime_get_if_in_use(&client->dev))
> +		return 0;
> +
> +	switch (ctrl->id) {
> +	case V4L2_CID_EXPOSURE_AUTO:
> +		ret = vd56g3_read_expo_cluster(sensor, false);
> +		break;
> +	default:
> +		ret = -EINVAL;
> +		break;
> +	}
> +
> +	pm_runtime_mark_last_busy(&client->dev);
> +	pm_runtime_put_autosuspend(&client->dev);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_s_ctrl(struct v4l2_ctrl *ctrl)
> +{
> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
> +	const struct v4l2_rect *crop;
> +	unsigned int frame_length = 0;
> +	unsigned int expo_max;
> +	unsigned int ae_compensation;
> +	bool is_auto = false;
> +	int ret;
> +
> +	struct v4l2_subdev_state *state =
> +		v4l2_subdev_get_locked_active_state(sd);
> +	crop = v4l2_subdev_state_get_crop(state, 0);
> +
> +	if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
> +		return 0;
> +
> +	/* Update controls state, range, etc. whatever the state of the HW */
> +	switch (ctrl->id) {
> +	case V4L2_CID_VBLANK:
> +		frame_length = crop->height + ctrl->val;
> +		expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
> +		__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
> +					 VD56G3_EXPOSURE_DEFAULT);
> +		break;
> +	case V4L2_CID_EXPOSURE_AUTO:
> +		is_auto = (ctrl->val == V4L2_EXPOSURE_AUTO);
> +		__v4l2_ctrl_grab(sensor->ae_lock_ctrl, !is_auto);
> +		__v4l2_ctrl_grab(sensor->ae_bias_ctrl, !is_auto);
> +		break;
> +	default:
> +		break;
> +	}
> +
> +	/* Interact with HW only when it is powered ON */
> +	if (!pm_runtime_get_if_in_use(&client->dev))
> +		return 0;
> +
> +	switch (ctrl->id) {
> +	case V4L2_CID_HFLIP:
> +		ret = cci_write(sensor->regmap, VD56G3_REG_ORIENTATION,
> +				sensor->hflip_ctrl->val |
> +					(sensor->vflip_ctrl->val << 1),
> +				NULL);
> +		break;
> +	case V4L2_CID_TEST_PATTERN:
> +		ret = vd56g3_update_patgen(sensor, ctrl->val);
> +		break;
> +	case V4L2_CID_EXPOSURE_AUTO:
> +		ret = vd56g3_update_expo_cluster(sensor, is_auto);
> +		break;
> +	case V4L2_CID_3A_LOCK:
> +		ret = vd56g3_lock_exposure(sensor, ctrl->val);
> +		break;
> +	case V4L2_CID_AUTO_EXPOSURE_BIAS:
> +		ae_compensation =
> +			DIV_ROUND_CLOSEST((int)vd56g3_ev_bias_qmenu[ctrl->val] *
> +					  256, 1000);
> +		ret = cci_write(sensor->regmap, VD56G3_REG_AE_COMPENSATION,
> +				ae_compensation, NULL);
> +		break;
> +	case V4L2_CID_VBLANK:
> +		ret = cci_write(sensor->regmap, VD56G3_REG_FRAME_LENGTH,
> +				frame_length, NULL);
> +		break;
> +	case V4L2_CID_FLASH_LED_MODE:
> +		ret = vd56g3_write_gpiox(sensor, sensor->ext_leds_mask);
> +		break;
> +	default:
> +		ret = -EINVAL;
> +		break;
> +	}
> +
> +	pm_runtime_mark_last_busy(&client->dev);
> +	pm_runtime_put_autosuspend(&client->dev);
> +
> +	return ret;
> +}
> +
> +static const struct v4l2_ctrl_ops vd56g3_ctrl_ops = {
> +	.g_volatile_ctrl = vd56g3_g_volatile_ctrl,
> +	.s_ctrl = vd56g3_s_ctrl,
> +};
> +
> +static void vd56g3_update_controls(struct vd56g3 *sensor)
> +{
> +	const struct v4l2_rect *crop;
> +	unsigned int hblank;
> +	unsigned int vblank_min, vblank, vblank_max;
> +	unsigned int frame_length;
> +	unsigned int expo_max;
> +
> +	struct v4l2_subdev_state *state =
> +		v4l2_subdev_get_locked_active_state(&sensor->sd);
> +	crop = v4l2_subdev_state_get_crop(state, 0);
> +
> +	hblank = VD56G3_LINE_LENGTH_MIN - crop->width;
> +	vblank_min = VD56G3_VBLANK_MIN;
> +	vblank = VD56G3_FRAME_LENGTH_DEF_60FPS - crop->height;
> +	vblank_max = 0xffff - crop->height;
> +	frame_length = crop->height + vblank;
> +	expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
> +

--

> +	/* Update blanking and exposure (ranges + values) */
> +	__v4l2_ctrl_modify_range(sensor->hblank_ctrl, hblank, hblank, 1,
> +				 hblank);
> +	__v4l2_ctrl_modify_range(sensor->vblank_ctrl, vblank_min, vblank_max, 1,
> +				 vblank);
> +	__v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, vblank);
> +	__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
> +				 VD56G3_EXPOSURE_DEFAULT);
> +	__v4l2_ctrl_s_ctrl(sensor->expo_ctrl, VD56G3_EXPOSURE_DEFAULT);

Here you don't have to check the return value fo the __v4l2__ctrl
functions using a ret value? Or I'm completely wrong?

Please check drivers/media/i2c/imx334.c:624
imx334_update_controls

> +}
> +
> +static int vd56g3_init_controls(struct vd56g3 *sensor)
> +{
> +	const struct v4l2_ctrl_ops *ops = &vd56g3_ctrl_ops;
> +	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
> +	struct v4l2_ctrl *ctrl;
> +	int ret;
> +
> +	v4l2_ctrl_handler_init(hdl, 25);
> +
> +	/* Horizontal & vertical flips modify bayer code on RGB variant */
> +	sensor->hflip_ctrl =
> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
> +	if (sensor->hflip_ctrl)
> +		sensor->hflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
> +
> +	sensor->vflip_ctrl =
> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
> +	if (sensor->vflip_ctrl)
> +		sensor->vflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
> +
> +	sensor->patgen_ctrl =
> +		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
> +					     ARRAY_SIZE(vd56g3_tp_menu) - 1, 0,
> +					     0, vd56g3_tp_menu);
> +
> +	ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
> +				      ARRAY_SIZE(vd56g3_link_freq_1lane) - 1, 0,
> +				      (sensor->nb_of_lane == 2) ?
> +					      vd56g3_link_freq_2lanes :
> +					      vd56g3_link_freq_1lane);
> +	if (ctrl)
> +		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
> +
> +	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
> +				 sensor->pixel_clock, sensor->pixel_clock, 1,
> +				 sensor->pixel_clock);

I personally prefer:
	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
				 sensor->pixel_clock,
				 sensor->pixel_clock, 1,
				 sensor->pixel_clock);

up to you :)

> +	if (ctrl)
> +		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
> +
> +	sensor->ae_ctrl = v4l2_ctrl_new_std_menu(hdl, ops,
> +						 V4L2_CID_EXPOSURE_AUTO,
> +						 V4L2_EXPOSURE_MANUAL, 0,
> +						 V4L2_EXPOSURE_AUTO);
> +
> +	sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK, 0,
> +						 GENMASK(2, 0), 0, 0);
> +
> +	sensor->ae_bias_ctrl =
> +		v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
> +				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) - 1,
> +				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) / 2,
> +				       vd56g3_ev_bias_qmenu);
> +
> +	/*
> +	 * Analog gain [1, 8] is computed with the following logic :
> +	 * 32/(32 - again_reg), with again_reg in the range [0:28]
> +	 * Digital gain [1.00, 8.00] is coded as a Fixed Point 5.8
> +	 */
> +	sensor->again_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
> +					       0, 28, 1, 0);
> +	sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
> +					       0x100, 0x800, 1, 0x100);
> +
> +	/*
> +	 * Set the exposure, horizontal and vertical blanking ctrls
> +	 * to hardcoded values, they will be updated in vd56g3_update_controls.
> +	 * Exposure being in an auto-cluster, set a significant value here.
> +	 */
> +	sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
> +					      VD56G3_EXPOSURE_DEFAULT,
> +					      VD56G3_EXPOSURE_DEFAULT, 1,
> +					      VD56G3_EXPOSURE_DEFAULT);
> +	sensor->hblank_ctrl =
> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 1, 1, 1, 1);
> +	if (sensor->hblank_ctrl)
> +		sensor->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
> +	sensor->vblank_ctrl =
> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, 1, 1, 1, 1);

Doubt on my side:

Here you flag hblank as read only. What about vblank?
Thanks in advance.

> +
> +	/* Additional control based on device tree properties */
> +	if (sensor->ext_leds_mask)
> +		sensor->led_ctrl =
> +			v4l2_ctrl_new_std_menu(hdl, ops,
> +					       V4L2_CID_FLASH_LED_MODE,
> +					       V4L2_FLASH_LED_MODE_FLASH, 0,
> +					       V4L2_FLASH_LED_MODE_NONE);
> +
> +	if (hdl->error) {
> +		ret = hdl->error;
> +		goto free_ctrls;
> +	}
> +
> +	v4l2_ctrl_cluster(2, &sensor->hflip_ctrl);
> +	v4l2_ctrl_auto_cluster(4, &sensor->ae_ctrl, V4L2_EXPOSURE_MANUAL, true);
> +
> +	sensor->sd.ctrl_handler = hdl;
> +	return 0;
> +
> +free_ctrls:
> +	v4l2_ctrl_handler_free(hdl);
> +	return ret;
> +}
> +
> +/* -----------------------------------------------------------------------------
> + * Videos ops
> + */
> +
> +static int vd56g3_stream_on(struct vd56g3 *sensor,
> +			    struct v4l2_subdev_state *state)
> +{
> +	const struct v4l2_mbus_framefmt *format =
> +		v4l2_subdev_state_get_format(state, 0);
> +	const struct v4l2_rect *crop =
> +		v4l2_subdev_state_get_crop(state, 0);
> +	int ret = 0;
> +	unsigned int csi_mbps = ((sensor->nb_of_lane == 2) ?
> +					 VD56G3_LINK_FREQ_DEF_2LANES :
> +					 VD56G3_LINK_FREQ_DEF_1LANE) * 2 / MEGA;
> +	unsigned int binning;
> +
> +	/* configure clocks */
> +	cci_write(sensor->regmap, VD56G3_REG_EXT_CLOCK, sensor->ext_clock,
> +		  &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_CLK_PLL_PREDIV, sensor->pll_prediv,
> +		  &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_CLK_SYS_PLL_MULT, sensor->pll_mult,
> +		  &ret);
> +
> +	/* configure output */
> +	cci_write(sensor->regmap, VD56G3_REG_FORMAT_CTRL,
> +		  vd56g3_get_bpp(format->code), &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OIF_CSI_BITRATE, csi_mbps, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OIF_IMG_CTRL,
> +		  vd56g3_get_datatype(format->code), &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_ISL_ENABLE, 0, &ret);
> +
> +	/* configure binning mode */
> +	switch (crop->width / format->width) {
> +	case 1:
> +	default:
> +		binning = READOUT_NORMAL;
> +		break;
> +	case 2:
> +		binning = READOUT_DIGITAL_BINNING_X2;
> +		break;
> +	}
> +	cci_write(sensor->regmap, VD56G3_REG_READOUT_CTRL, binning, &ret);
> +
> +	/* configure ROIs */
> +	cci_write(sensor->regmap, VD56G3_REG_Y_START, crop->top, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_Y_END,
> +		  crop->top + crop->height - 1, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_START, crop->left, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_END,
> +		  crop->left + crop->width - 1, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_START, 0, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_END, crop->height - 1,
> +		  &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_H, crop->left, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_H,
> +		  crop->left + crop->width - 1, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_V, 0, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_V, crop->height - 1,
> +		  &ret);
> +	if (ret)
> +		return ret;
> +
> +	/* Setup default GPIO values; could be overridden by V4L2 ctrl setup */
> +	ret = vd56g3_write_gpiox(sensor, GENMASK(VD56G3_NB_GPIOS - 1, 0));
> +	if (ret)
> +		return ret;
> +
> +	/* Apply settings from V4L2 ctrls */
> +	ret = __v4l2_ctrl_handler_setup(&sensor->ctrl_handler);
> +	if (ret)
> +		return ret;
> +
> +	/* start streaming */
> +	cci_write(sensor->regmap, VD56G3_REG_STBY, VD56G3_CMD_START_STREAM,
> +		  &ret);
> +	vd56g3_poll_reg(sensor, VD56G3_REG_STBY, VD56G3_CMD_ACK, &ret);
> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_STREAMING, &ret);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_stream_off(struct vd56g3 *sensor)
> +{
> +	int ret = 0;
> +
> +	/* Retrieve Expo cluster to enable coldstart of AE */
> +	ret = vd56g3_read_expo_cluster(sensor, true);
> +
> +	cci_write(sensor->regmap, VD56G3_REG_STREAMING, VD56G3_CMD_STOP_STREAM,
> +		  &ret);
> +	vd56g3_poll_reg(sensor, VD56G3_REG_STREAMING, VD56G3_CMD_ACK, &ret);
> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_s_stream(struct v4l2_subdev *sd, int enable)
> +{
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
> +	int ret = 0;
> +
> +	struct v4l2_subdev_state *state =
> +		v4l2_subdev_lock_and_get_active_state(sd);
> +
> +	if (enable) {
> +		ret = pm_runtime_resume_and_get(&client->dev);
> +		if (ret < 0)
> +			goto unlock;
> +		ret = vd56g3_stream_on(sensor, state);
> +		if (ret) {
> +			dev_err(&client->dev, "Failed to start streaming\n");
> +			pm_runtime_put_sync(&client->dev);
> +		}
> +	} else {
> +		vd56g3_stream_off(sensor);
> +		pm_runtime_mark_last_busy(&client->dev);
> +		pm_runtime_put_autosuspend(&client->dev);
> +	}
> +
> +unlock:
> +	v4l2_subdev_unlock_state(state);
> +	if (!ret) {
> +		sensor->streaming = enable;
> +		v4l2_ctrl_grab(sensor->hflip_ctrl, enable);
> +		v4l2_ctrl_grab(sensor->vflip_ctrl, enable);
> +		v4l2_ctrl_grab(sensor->patgen_ctrl, enable);
> +	}
> +
> +	return ret;
> +}
> +
> +static const struct v4l2_subdev_video_ops vd56g3_video_ops = {
> +	.s_stream = vd56g3_s_stream,
> +};
> +
> +/* -----------------------------------------------------------------------------
> + * Pad ops
> + */
> +
> +/* Media bus code is dependent of :
> + *      - 8bits or 10bits output
> + *      - variant : Mono or RGB
> + *      - H/V flips parameters in case of RGB
> + */
> +static u32 vd56g3_get_mbus_code(struct vd56g3 *sensor, u32 code)
> +{
> +	unsigned int i_bpp;
> +	unsigned int j;
> +
> +	for (i_bpp = 0; i_bpp < ARRAY_SIZE(vd56g3_mbus_codes); i_bpp++) {
> +		for (j = 0; j < ARRAY_SIZE(vd56g3_mbus_codes[i_bpp]); j++) {
> +			if (vd56g3_mbus_codes[i_bpp][j] == code)
> +				goto endloops;
> +		}
> +	}
> +
> +endloops:
> +	if (i_bpp >= ARRAY_SIZE(vd56g3_mbus_codes))
> +		i_bpp = 0;
> +
> +	if (sensor->is_mono)
> +		j = 0;
> +	else
> +		j = 1 + (sensor->hflip_ctrl->val ? 1 : 0) +
> +		    (sensor->vflip_ctrl->val ? 2 : 0);
> +
> +	return vd56g3_mbus_codes[i_bpp][j];
> +}
> +
> +static int vd56g3_enum_mbus_code(struct v4l2_subdev *sd,
> +				 struct v4l2_subdev_state *sd_state,
> +				 struct v4l2_subdev_mbus_code_enum *code)
> +{
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +
> +	if (code->index >= ARRAY_SIZE(vd56g3_mbus_codes))
> +		return -EINVAL;
> +
> +	code->code =
> +		vd56g3_get_mbus_code(sensor, vd56g3_mbus_codes[code->index][0]);
> +
> +	return 0;
> +}
> +
> +static int vd56g3_enum_frame_size(struct v4l2_subdev *sd,
> +				  struct v4l2_subdev_state *sd_state,
> +				  struct v4l2_subdev_frame_size_enum *fse)
> +{
> +	if (fse->index >= ARRAY_SIZE(vd56g3_supported_modes))
> +		return -EINVAL;
> +
> +	fse->min_width = vd56g3_supported_modes[fse->index].width;
> +	fse->max_width = fse->min_width;
> +	fse->min_height = vd56g3_supported_modes[fse->index].height;
> +	fse->max_height = fse->min_height;
> +
> +	return 0;
> +}
> +
> +static void vd56g3_update_img_pad_format(struct vd56g3 *sensor,
> +					 const struct vd56g3_mode *mode,
> +					 u32 mbus_code,
> +					 struct v4l2_mbus_framefmt *mbus_fmt)
> +{
> +	mbus_fmt->width = mode->width;
> +	mbus_fmt->height = mode->height;
> +	mbus_fmt->code = vd56g3_get_mbus_code(sensor, mbus_code);
> +	mbus_fmt->colorspace = V4L2_COLORSPACE_RAW;
> +	mbus_fmt->field = V4L2_FIELD_NONE;
> +	mbus_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
> +	mbus_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
> +	mbus_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
> +}
> +
> +static int vd56g3_set_pad_fmt(struct v4l2_subdev *sd,
> +			      struct v4l2_subdev_state *sd_state,
> +			      struct v4l2_subdev_format *sd_fmt)
> +{
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +	const struct vd56g3_mode *new_mode;
> +	struct v4l2_rect pad_crop;
> +	unsigned int binning;
> +
> +	if (sensor->streaming)
> +		return -EBUSY;
> +
> +	new_mode = v4l2_find_nearest_size(vd56g3_supported_modes,
> +					  ARRAY_SIZE(vd56g3_supported_modes),
> +					  width, height, sd_fmt->format.width,
> +					  sd_fmt->format.height);
> +
> +	vd56g3_update_img_pad_format(sensor, new_mode, sd_fmt->format.code,
> +				     &sd_fmt->format);
> +
> +	/* Compute crop rectangle (maximized via binning) */
> +	binning = min(VD56G3_NATIVE_WIDTH / sd_fmt->format.width,
> +		      VD56G3_NATIVE_HEIGHT / sd_fmt->format.height);
> +	binning = min(binning, 2U);
> +	pad_crop.width = sd_fmt->format.width * binning;
> +	pad_crop.height = sd_fmt->format.height * binning;
> +	pad_crop.left = (VD56G3_NATIVE_WIDTH - pad_crop.width) / 2;
> +	pad_crop.top = (VD56G3_NATIVE_HEIGHT - pad_crop.height) / 2;
> +
> +	/* Update active state's format and crop */
> +	*v4l2_subdev_state_get_format(sd_state, sd_fmt->pad) = sd_fmt->format;
> +	*v4l2_subdev_state_get_crop(sd_state, sd_fmt->pad) = pad_crop;
> +
> +	if (sd_fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
> +		vd56g3_update_controls(sensor);
> +
> +	return 0;
> +}
> +
> +static int vd56g3_get_selection(struct v4l2_subdev *sd,
> +				struct v4l2_subdev_state *sd_state,
> +				struct v4l2_subdev_selection *sel)
> +{
> +	switch (sel->target) {
> +	case V4L2_SEL_TGT_CROP:
> +		sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
> +		break;
> +	case V4L2_SEL_TGT_NATIVE_SIZE:
> +	case V4L2_SEL_TGT_CROP_DEFAULT:
> +	case V4L2_SEL_TGT_CROP_BOUNDS:
> +		sel->r.top = 0;
> +		sel->r.left = 0;
> +		sel->r.width = VD56G3_NATIVE_WIDTH;
> +		sel->r.height = VD56G3_NATIVE_HEIGHT;
> +		break;
> +	default:
> +		return -EINVAL;
> +	}
> +
> +	return 0;
> +}
> +
> +static int vd56g3_init_state(struct v4l2_subdev *sd,
> +			     struct v4l2_subdev_state *sd_state)
> +{
> +	unsigned int def_mode = VD56G3_DEFAULT_MODE;
> +	struct v4l2_subdev_format fmt = {
> +		.which = V4L2_SUBDEV_FORMAT_TRY,
> +		.pad = 0,
> +		.format = {
> +			.code = vd56g3_mbus_codes[0][0],
> +			.width = vd56g3_supported_modes[def_mode].width,
> +			.height = vd56g3_supported_modes[def_mode].height,
> +		},
> +	};
> +
> +	vd56g3_set_pad_fmt(sd, sd_state, &fmt);
> +	return 0;
> +}
> +
> +static const struct v4l2_subdev_core_ops vd56g3_core_ops = {
> +	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
> +	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
> +};
> +
> +static const struct v4l2_subdev_pad_ops vd56g3_pad_ops = {
> +	.enum_mbus_code = vd56g3_enum_mbus_code,
> +	.enum_frame_size = vd56g3_enum_frame_size,
> +	.get_fmt = v4l2_subdev_get_fmt,
> +	.set_fmt = vd56g3_set_pad_fmt,
> +	.get_selection = vd56g3_get_selection,
> +};
> +
> +static const struct v4l2_subdev_ops vd56g3_subdev_ops = {
> +	.core = &vd56g3_core_ops,
> +	.video = &vd56g3_video_ops,
> +	.pad = &vd56g3_pad_ops,
> +};
> +
> +static const struct media_entity_operations vd56g3_subdev_entity_ops = {
> +	.link_validate = v4l2_subdev_link_validate,
> +};
> +
> +static const struct v4l2_subdev_internal_ops vd56g3_internal_ops = {
> +	.init_state = vd56g3_init_state,
> +};
> +
> +/* -----------------------------------------------------------------------------
> + * Power management
> + */
> +
> +static int vd56g3_boot(struct vd56g3 *sensor)
> +{
> +	int ret = 0;
> +
> +	cci_write(sensor->regmap, VD56G3_REG_BOOT, VD56G3_CMD_BOOT, &ret);
> +	vd56g3_poll_reg(sensor, VD56G3_REG_BOOT, VD56G3_CMD_ACK, &ret);
> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_power_on(struct vd56g3 *sensor)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	int ret;
> +
> +	ret = regulator_bulk_enable(VD56G3_NUM_SUPPLIES, sensor->supplies);
> +	if (ret) {
> +		dev_err(&client->dev, "Failed to enable regulators %d", ret);
> +		return ret;
> +	}
> +
> +	ret = clk_prepare_enable(sensor->xclk);
> +	if (ret) {
> +		dev_err(&client->dev, "Failed to enable clock %d", ret);
> +		goto disable_reg;
> +	}
> +
> +	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
> +	ret = vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_READY_TO_BOOT, NULL);
> +	if (ret) {
> +		dev_err(&client->dev, "Sensor reset failed %d\n", ret);
> +		goto disable_clock;
> +	}
> +
> +	return 0;
> +
> +disable_clock:
> +	clk_disable_unprepare(sensor->xclk);
> +	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
> +disable_reg:
> +	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_power_off(struct vd56g3 *sensor)
> +{
> +	clk_disable_unprepare(sensor->xclk);
> +	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
> +	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
> +	return 0;
> +}
> +
> +static int vd56g3_runtime_resume(struct device *dev)
> +{
> +	struct v4l2_subdev *sd = dev_get_drvdata(dev);
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +	struct i2c_client *client = sensor->i2c_client;
> +	int ret;
> +
> +	ret = vd56g3_power_on(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "Failed to power on %d", ret);
> +		return ret;
> +	}
> +
> +	ret = vd56g3_boot(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "sensor boot failed %d", ret);
> +		return ret;
> +	}
> +
> +	return ret;
> +}
> +
> +static int vd56g3_runtime_suspend(struct device *dev)
> +{
> +	struct v4l2_subdev *sd = dev_get_drvdata(dev);
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +
> +	return vd56g3_power_off(sensor);
> +}
> +
> +static const struct dev_pm_ops vd56g3_pm_ops = {
> +	SET_RUNTIME_PM_OPS(vd56g3_runtime_suspend, vd56g3_runtime_resume, NULL)
> +};
> +
> +/* -----------------------------------------------------------------------------
> + * Probe and initialization
> + */
> +
> +static int vd56g3_check_csi_conf(struct vd56g3 *sensor,
> +				 struct fwnode_handle *endpoint)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
> +	u32 phy_data_lanes[VD56G3_MAX_CSI_DATA_LANES] = { ~0, ~0 };
> +	int n_lanes;

use u8 for n_lanes

> +	int frequency;

use u64

> +	int p, l;
> +	int ret = 0;
> +
> +	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
> +	if (ret)
> +		return -EINVAL;
> +
> +	/* Check lanes number */
> +	n_lanes = ep.bus.mipi_csi2.num_data_lanes;
> +	if (n_lanes != 1 && n_lanes != 2) {
> +		dev_err(&client->dev, "Invalid data lane number %d\n", n_lanes);
> +		ret = -EINVAL;
> +		goto done;
> +	}
> +	sensor->nb_of_lane = n_lanes;
> +
> +	/* Clock lane must be first */
> +	if (ep.bus.mipi_csi2.clock_lane != 0) {
> +		dev_err(&client->dev, "Clk lane must be mapped to lane 0\n");
> +		ret = -EINVAL;
> +		goto done;
> +	}
> +
> +	/* Prepare Output Interface conf based on lane settings
> +	 * logical to physical lane conversion (+ pad remaining slots)
> +	 */
> +	for (l = 0; l < n_lanes; l++)
> +		phy_data_lanes[ep.bus.mipi_csi2.data_lanes[l] - 1] = l;
> +	for (p = 0; p < VD56G3_MAX_CSI_DATA_LANES; p++) {
> +		if (phy_data_lanes[p] != ~0)
> +			continue;
> +		phy_data_lanes[p] = l;
> +		l++;
> +	}
> +	sensor->oif_ctrl = n_lanes |
> +			   (ep.bus.mipi_csi2.lane_polarities[0] << 3) |
> +			   ((phy_data_lanes[0]) << 4) |
> +			   (ep.bus.mipi_csi2.lane_polarities[1] << 6) |
> +			   ((phy_data_lanes[1]) << 7) |
> +			   (ep.bus.mipi_csi2.lane_polarities[2] << 9);
> +
> +	/* Check link frequency */
> +	if (!ep.nr_of_link_frequencies) {
> +		dev_err(&client->dev, "link-frequency not found in DT\n");
> +		ret = -EINVAL;
> +		goto done;
> +	}
> +	frequency = (n_lanes == 2) ? VD56G3_LINK_FREQ_DEF_2LANES :
> +				     VD56G3_LINK_FREQ_DEF_1LANE;
> +	if (ep.nr_of_link_frequencies != 1 ||
> +	    ep.link_frequencies[0] != frequency) {
> +		dev_err(&client->dev, "Link frequency not supported: %lld\n",
> +			ep.link_frequencies[0]);
> +		ret = -EINVAL;
> +		goto done;
> +	}
> +
> +done:
> +	v4l2_fwnode_endpoint_free(&ep);
> +	return ret;
> +}
> +
> +static int vd56g3_parse_dt_gpios_array(struct vd56g3 *sensor, char *prop_name,
> +				       u32 *array, int *nb)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	struct device_node *np = client->dev.of_node;
> +	unsigned int i;
> +
> +	*nb = of_property_read_variable_u32_array(np, prop_name, array, 0,
> +						  VD56G3_NB_GPIOS);
> +
> +	if (*nb == -EINVAL) {
> +		*nb = 0;
> +		return *nb;
> +	} else if (*nb < 0) {
> +		dev_err(&client->dev, "Failed to read %s prop\n", prop_name);
> +		return *nb;
> +	}
> +
> +	for (i = 0; i < *nb; i++) {
> +		if (array[i] >= VD56G3_NB_GPIOS) {
> +			dev_err(&client->dev, "Invalid GPIO : %d\n", array[i]);
> +			return -EINVAL;
> +		}
> +	}
> +
> +	return 0;
> +}
> +
> +static int vd56g3_parse_dt_gpios(struct vd56g3 *sensor)
> +{
> +	u32 led_gpios[VD56G3_NB_GPIOS];
> +	int nb_gpios_leds;
> +	int ret;
> +	unsigned int i;
> +
> +	/* Initialize GPIOs to default */
> +	for (i = 0; i < VD56G3_NB_GPIOS; i++)
> +		sensor->gpios[i] = VD56G3_GPIOX_GPIO_IN;
> +	sensor->ext_leds_mask = 0;
> +
> +	/* Take into account optional 'st,leds' output for GPIOs */
> +	ret = vd56g3_parse_dt_gpios_array(sensor, "st,leds", led_gpios,
> +					  &nb_gpios_leds);
> +	if (ret)
> +		return ret;
> +	for (i = 0; i < nb_gpios_leds; i++) {
> +		sensor->gpios[led_gpios[i]] = VD56G3_GPIOX_STROBE_MODE;
> +		set_bit(led_gpios[i], &sensor->ext_leds_mask);
> +	}
> +
> +	return 0;
> +}
> +
> +static int vd56g3_parse_dt(struct vd56g3 *sensor)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	struct device *dev = &client->dev;
> +	struct fwnode_handle *endpoint;
> +	int ret;
> +
> +	endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0, 0);
> +	if (!endpoint) {
> +		dev_err(dev, "endpoint node not found\n");
> +		return -EINVAL;
> +	}
> +
> +	ret = vd56g3_check_csi_conf(sensor, endpoint);
> +	fwnode_handle_put(endpoint);
> +	if (ret)
> +		return ret;
> +
> +	ret = vd56g3_parse_dt_gpios(sensor);
> +	if (ret)
> +		return ret;
> +
> +	return 0;
> +}
> +
> +static int vd56g3_get_regulators(struct vd56g3 *sensor)
> +{
> +	int i;
> +
> +	for (i = 0; i < VD56G3_NUM_SUPPLIES; i++)
> +		sensor->supplies[i].supply = vd56g3_supply_names[i];
> +
> +	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
> +				       VD56G3_NUM_SUPPLIES, sensor->supplies);
> +}
> +
> +static int vd56g3_prepare_clock_tree(struct vd56g3 *sensor)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	const unsigned int predivs[] = { 1, 2, 4 };
> +	u32 pll_out;
> +	int i;
> +
> +	/* External clock must be in [6Mhz-27Mhz] */
> +	if (sensor->ext_clock < 6 * HZ_PER_MHZ ||
> +	    sensor->ext_clock > 27 * HZ_PER_MHZ) {
> +		dev_err(&client->dev,
> +			"Only 6Mhz-27Mhz clock range supported. Provided %lu MHz\n",
> +			sensor->ext_clock / HZ_PER_MHZ);
> +		return -EINVAL;
> +	}
> +
> +	/* PLL input should be in [6Mhz-12Mhz[ */
> +	for (i = 0; i < ARRAY_SIZE(predivs); i++) {
> +		sensor->pll_prediv = predivs[i];
> +		if (sensor->ext_clock / sensor->pll_prediv < 12 * HZ_PER_MHZ)
> +			break;
> +	}
> +
> +	/* PLL output clock must be as close as possible to 804Mhz */
> +	sensor->pll_mult = (VD56G3_TARGET_PLL * sensor->pll_prediv +
> +			    sensor->ext_clock / 2) /
> +			   sensor->ext_clock;
> +	pll_out = sensor->ext_clock * sensor->pll_mult / sensor->pll_prediv;
> +
> +	/* Target Pixel Clock for standard 10bit ADC mode : 160.8Mhz */
> +	sensor->pixel_clock = pll_out / VD56G3_VT_CLOCK_DIV;
> +
> +	return 0;
> +}
> +
> +static int vd56g3_detect(struct vd56g3 *sensor)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	struct device *dev = &client->dev;
> +	unsigned int model;
> +	u64 model_id = 0;
> +	u64 device_revision = 0;
> +	u64 optical_revision = 0;
> +	int ret = 0;
> +
> +	model = (uintptr_t)device_get_match_data(dev);
> +
> +	cci_read(sensor->regmap, VD56G3_REG_MODEL_ID, &model_id, &ret);
> +	if (ret)
> +		return ret;
> +
> +	if (model_id != VD56G3_MODEL_ID) {
> +		dev_warn(&client->dev, "Unsupported sensor id %x",
> +			 (u16)model_id);
> +		return -ENODEV;
> +	}
> +
> +	cci_read(sensor->regmap, VD56G3_REG_REVISION, &device_revision, &ret);
> +	if (ret)
> +		return ret;
> +
> +	if ((device_revision >> 8) != 0x31) {

What about create a define that explain 0x31 number to avoid magic
number?

#define VD56G3_DEV_CUTV = 0x31?

> +		dev_warn(&client->dev, "Unsupported Cut version %x",
> +			 (u16)device_revision);
> +		return -ENODEV;
> +	}
> +
> +	cci_read(sensor->regmap, VD56G3_REG_OPTICAL_REVISION, &optical_revision,
> +		 &ret);
> +	if (ret)
> +		return ret;
> +
> +	sensor->is_mono =
> +		((optical_revision & 1) == VD56G3_OPTICAL_REVISION_MONO);
> +	if ((sensor->is_mono && model == VD56G3_MODEL_VD66GY) ||
> +	    (!sensor->is_mono && model == VD56G3_MODEL_VD56G3)) {
> +		dev_warn(&client->dev,
> +			 "Found %s sensor, while %s model is defined in DT",
> +			 (sensor->is_mono) ? "Mono" : "Bayer",
> +			 (model == VD56G3_MODEL_VD56G3) ? "vd56g3" : "vd66gy");
> +		return -ENODEV;
> +	}
> +
> +	return 0;
> +}
> +
> +static int vd56g3_subdev_init(struct vd56g3 *sensor)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	int ret;
> +
> +	/* Init sub device */
> +	v4l2_i2c_subdev_init(&sensor->sd, client, &vd56g3_subdev_ops);
> +	sensor->sd.internal_ops = &vd56g3_internal_ops;
> +	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
> +			    V4L2_SUBDEV_FL_HAS_EVENTS;
> +	sensor->sd.entity.ops = &vd56g3_subdev_entity_ops;
> +
> +	/* Init source pad */
> +	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
> +	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
> +	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
> +	if (ret) {
> +		dev_err(&client->dev, "Failed to init media entity : %d", ret);
> +		return ret;
> +	}
> +
> +	/* Init controls */
> +	ret = vd56g3_init_controls(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "Controls initialization failed %d", ret);
> +		goto err_media;
> +	}
> +
> +	/* Init vd56g3 struct : default resolution + raw8 */
> +	sensor->streaming = false;
> +	sensor->sd.state_lock = sensor->ctrl_handler.lock;
> +	ret = v4l2_subdev_init_finalize(&sensor->sd);
> +	if (ret) {
> +		dev_err(&client->dev, "subdev init error: %d", ret);
> +		goto err_ctrls;
> +	}
> +
> +	vd56g3_update_controls(sensor);
> +
> +	return 0;
> +
> +err_ctrls:
> +	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
> +
> +err_media:
> +	media_entity_cleanup(&sensor->sd.entity);
> +	return ret;
> +}
> +
> +static void vd56g3_subdev_cleanup(struct vd56g3 *sensor)
> +{
> +	v4l2_async_unregister_subdev(&sensor->sd);
> +	v4l2_subdev_cleanup(&sensor->sd);
> +	media_entity_cleanup(&sensor->sd.entity);
> +	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
> +}
> +
> +static int vd56g3_probe(struct i2c_client *client)
> +{
> +	struct device *dev = &client->dev;
> +	struct vd56g3 *sensor;
> +	int ret;
> +
> +	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
> +	if (!sensor)
> +		return -ENOMEM;
> +
> +	sensor->i2c_client = client;
> +
> +	ret = vd56g3_parse_dt(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "Failed to parse Device Tree : %d", ret);

Why not dev_err_probe here?

> +		return ret;
> +	}
> +
> +	/* Get (and check) resources : power regs, ext clock, reset gpio */
> +	ret = vd56g3_get_regulators(sensor);
> +	if (ret)
> +		return dev_err_probe(dev, ret, "Failed to get regulators.");
> +
> +	sensor->xclk = devm_clk_get(dev, NULL);
> +	if (IS_ERR(sensor->xclk))
> +		return dev_err_probe(dev, PTR_ERR(sensor->xclk),
> +				     "Failed to get xclk.");
> +	sensor->ext_clock = clk_get_rate(sensor->xclk);

What about use of xclk_freq?
I think this is more understandable

sensor->xclk
sensor->xclk_freq

better I think :)

> +	ret = vd56g3_prepare_clock_tree(sensor);
> +	if (ret)
> +		return ret;
> +
> +	sensor->reset_gpio =
> +		devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
> +	if (IS_ERR(sensor->reset_gpio))
> +		return dev_err_probe(dev, PTR_ERR(sensor->reset_gpio),
> +				     "Failed to get reset gpio.");
> +
> +	sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
> +	if (IS_ERR(sensor->regmap))
> +		return dev_err_probe(dev, PTR_ERR(sensor->regmap),
> +				     "Failed to init regmap.");
> +
> +	/* Power ON */
> +	ret = vd56g3_power_on(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "Sensor power on failed : %d", ret);

dev_err_probe?

> +		return ret;
> +	}
> +
> +	/* Enable PM runtime with autosuspend (sensor being ON, set active) */
> +	pm_runtime_set_active(dev);
> +	pm_runtime_get_noresume(dev);
> +	pm_runtime_enable(dev);
> +	pm_runtime_set_autosuspend_delay(dev, 1000);
> +	pm_runtime_use_autosuspend(dev);
> +
> +	/* Check HW model/version */
> +	ret = vd56g3_boot(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "Sensor boot failed : %d", ret);
> +		goto err_power_off;
> +	}
> +
> +	ret = vd56g3_detect(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "Sensor detect failed : %d", ret);
> +		goto err_power_off;
> +	}
> +
> +	/* Initialize, then register V4L2 subdev */
> +	ret = vd56g3_subdev_init(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "V4l2 init failed : %d", ret);
> +		goto err_power_off;
> +	}
> +
> +	ret = v4l2_async_register_subdev(&sensor->sd);
> +	if (ret) {
> +		dev_err(&client->dev, "async subdev register failed %d", ret);
> +		goto err_subdev;
> +	}
> +
> +	/* Sensor could now be powered off (after the autosuspend delay) */
> +	pm_runtime_mark_last_busy(dev);
> +	pm_runtime_put_autosuspend(dev);
> +
> +	dev_dbg(&client->dev, "Successfully probe %s sensor",
> +		(sensor->is_mono) ? "vd56g3" : "vd66gy");
> +
> +	return 0;
> +
> +err_subdev:
> +	vd56g3_subdev_cleanup(sensor);
> +err_power_off:
> +	pm_runtime_disable(dev);
> +	pm_runtime_put_noidle(dev);
> +	vd56g3_power_off(sensor);
> +	return ret;
> +}
> +
> +static void vd56g3_remove(struct i2c_client *client)
> +{
> +	struct v4l2_subdev *sd = i2c_get_clientdata(client);
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +
> +	vd56g3_subdev_cleanup(sensor);
> +
> +	pm_runtime_disable(&client->dev);
> +	if (!pm_runtime_status_suspended(&client->dev))
> +		vd56g3_power_off(sensor);
> +	pm_runtime_set_suspended(&client->dev);
> +}
> +
> +static const struct of_device_id vd56g3_dt_ids[] = {
> +	{ .compatible = "st,st-vd56g3", .data = (void *)VD56G3_MODEL_VD56G3 },
> +	{ .compatible = "st,st-vd66gy", .data = (void *)VD56G3_MODEL_VD66GY },
> +	{ /* sentinel */ }
> +};
> +MODULE_DEVICE_TABLE(of, vd56g3_dt_ids);
> +
> +static struct i2c_driver vd56g3_i2c_driver = {
> +	.driver = {
> +		.name  = "st-vd56g3",
> +		.of_match_table = vd56g3_dt_ids,
> +		.pm = &vd56g3_pm_ops,
> +	},
> +	.probe = vd56g3_probe,
> +	.remove = vd56g3_remove,
> +};
> +
> +module_i2c_driver(vd56g3_i2c_driver);
> +
> +MODULE_AUTHOR("Benjamin Mugnier <[e-mail]>");
> +MODULE_AUTHOR("Mickael Guene <[e-mail]>");
> +MODULE_AUTHOR("Sylvain Petinot <[e-mail]>");
> +MODULE_DESCRIPTION("ST VD56G3 sensor driver");
> +MODULE_LICENSE("GPL");
> -- 
> 2.17.1
> 
>

Hope this help :)

Thanks & Regards,
Tommaso

Sakari Ailus April 24, 2024, 9:12 p.m. UTC | #3

Hi Sylvain,

Some quick comments below...

On Wed, Apr 17, 2024 at 03:34:53PM +0200, Sylvain Petinot wrote:
> Add V4L2 sub-device driver for STMicroelectronics VD56G3 camera sensor.
> This is a 1.5 M pixel global shutter image sensor with an active array
> size of 1124 x 1364 (portrait orientation).
> 
> The driver supports Mono (VD56G3) and Color (VD66GY) variants.
> 
> Signed-off-by: Sylvain Petinot <[e-mail]>
> ---
>  drivers/media/i2c/Kconfig     |   11 +
>  drivers/media/i2c/Makefile    |    1 +
>  drivers/media/i2c/st-vd56g3.c | 1619 +++++++++++++++++++++++++++++++++
>  3 files changed, 1631 insertions(+)
>  create mode 100644 drivers/media/i2c/st-vd56g3.c
> 
> diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
> index 56f276b920ab..a2d7271eef88 100644
> --- a/drivers/media/i2c/Kconfig
> +++ b/drivers/media/i2c/Kconfig
> @@ -657,6 +657,17 @@ config VIDEO_S5K6A3
>  	  This is a V4L2 sensor driver for Samsung S5K6A3 raw
>  	  camera sensor.
>  
> +config VIDEO_ST_VD56G3
> +	tristate "ST VD56G3 sensor support"
> +	select V4L2_CCI_I2C
> +	depends on OF && GPIOLIB
> +	help
> +	  This is a Video4Linux2 sensor driver for the ST VD56G3
> +	  camera sensor.
> +
> +	  To compile this driver as a module, choose M here: the
> +	  module will be called st-vd56g3.
> +
>  config VIDEO_ST_VGXY61
>  	tristate "ST VGXY61 sensor support"
>  	select V4L2_CCI_I2C
> diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
> index dfbe6448b549..b469cf0f8113 100644
> --- a/drivers/media/i2c/Makefile
> +++ b/drivers/media/i2c/Makefile
> @@ -124,6 +124,7 @@ obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
>  obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
>  obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
>  obj-$(CONFIG_VIDEO_ST_MIPID02) += st-mipid02.o
> +obj-$(CONFIG_VIDEO_ST_VD56G3) += st-vd56g3.o
>  obj-$(CONFIG_VIDEO_ST_VGXY61) += st-vgxy61.o
>  obj-$(CONFIG_VIDEO_TC358743) += tc358743.o
>  obj-$(CONFIG_VIDEO_TC358746) += tc358746.o
> diff --git a/drivers/media/i2c/st-vd56g3.c b/drivers/media/i2c/st-vd56g3.c
> new file mode 100644
> index 000000000000..eff819941456
> --- /dev/null
> +++ b/drivers/media/i2c/st-vd56g3.c
> @@ -0,0 +1,1619 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * A V4L2 driver for ST VD56G3 (Mono) and VD66GY (RGB) global shutter cameras.
> + * Copyright (C) 2024, STMicroelectronics SA
> + */
> +
> +#include <linux/clk.h>
> +#include <linux/delay.h>
> +#include <linux/gpio/consumer.h>
> +#include <linux/i2c.h>
> +#include <linux/iopoll.h>
> +#include <linux/module.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/regmap.h>
> +#include <linux/regulator/consumer.h>
> +#include <linux/units.h>
> +
> +#include <asm/unaligned.h>
> +
> +#include <media/mipi-csi2.h>
> +#include <media/v4l2-async.h>
> +#include <media/v4l2-cci.h>
> +#include <media/v4l2-ctrls.h>
> +#include <media/v4l2-device.h>
> +#include <media/v4l2-event.h>
> +#include <media/v4l2-fwnode.h>
> +#include <media/v4l2-subdev.h>
> +
> +/* Register Map */
> +#define VD56G3_REG_MODEL_ID				CCI_REG16_LE(0x0000)
> +#define VD56G3_MODEL_ID					0x5603
> +#define VD56G3_REG_REVISION				CCI_REG16_LE(0x0002)
> +#define VD56G3_REG_OPTICAL_REVISION			CCI_REG8(0x001a)
> +#define VD56G3_OPTICAL_REVISION_MONO			0
> +#define VD56G3_OPTICAL_REVISION_BAYER			1
> +#define VD56G3_REG_SYSTEM_FSM				CCI_REG8(0x0028)
> +#define VD56G3_SYSTEM_FSM_READY_TO_BOOT			0x01
> +#define VD56G3_SYSTEM_FSM_SW_STBY			0x02
> +#define VD56G3_SYSTEM_FSM_STREAMING			0x03
> +#define VD56G3_REG_APPLIED_COARSE_EXPOSURE		CCI_REG16_LE(0x0064)
> +#define VD56G3_REG_APPLIED_ANALOG_GAIN			CCI_REG8(0x0068)
> +#define VD56G3_REG_APPLIED_DIGITAL_GAIN			CCI_REG16_LE(0x006a)
> +#define VD56G3_REG_BOOT					CCI_REG8(0x0200)
> +#define VD56G3_CMD_ACK					0
> +#define VD56G3_CMD_BOOT					1
> +#define VD56G3_REG_STBY					CCI_REG8(0x0201)
> +#define VD56G3_CMD_START_STREAM				1
> +#define VD56G3_REG_STREAMING				CCI_REG8(0x0202)
> +#define VD56G3_CMD_STOP_STREAM				1
> +#define VD56G3_REG_EXT_CLOCK				CCI_REG32_LE(0x0220)
> +#define VD56G3_REG_CLK_PLL_PREDIV			CCI_REG8(0x0224)
> +#define VD56G3_REG_CLK_SYS_PLL_MULT			CCI_REG8(0x0226)
> +#define VD56G3_REG_ORIENTATION				CCI_REG8(0x0302)
> +#define VD56G3_REG_FORMAT_CTRL				CCI_REG8(0x030a)
> +#define VD56G3_REG_OIF_CTRL				CCI_REG16_LE(0x030c)
> +#define VD56G3_REG_OIF_IMG_CTRL				CCI_REG8(0x030f)
> +#define VD56G3_REG_OIF_CSI_BITRATE			CCI_REG16_LE(0x0312)
> +#define VD56G3_REG_DUSTER_CTRL				CCI_REG8(0x0318)
> +#define VD56G3_DUSTER_DISABLE				0
> +#define VD56G3_DUSTER_ENABLE_DEF_MODULES		0x13
> +#define VD56G3_REG_ISL_ENABLE				CCI_REG8(0x0333)
> +#define VD56G3_REG_DARKCAL_CTRL				CCI_REG8(0x0340)
> +#define VD56G3_DARKCAL_ENABLE				1
> +#define VD56G3_DARKCAL_DISABLE_DARKAVG			2
> +#define VD56G3_REG_PATGEN_CTRL				CCI_REG16_LE(0x0400)
> +#define VD56G3_PATGEN_ENABLE				1
> +#define VD56G3_PATGEN_TYPE_SHIFT			4
> +#define VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE		CCI_REG16_LE(0x042a)
> +#define VD56G3_REG_AE_COLDSTART_ANALOG_GAIN		CCI_REG8(0x042c)
> +#define VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN		CCI_REG16_LE(0x042e)
> +#define VD56G3_REG_AE_ROI_START_H			CCI_REG16_LE(0x0432)
> +#define VD56G3_REG_AE_ROI_START_V			CCI_REG16_LE(0x0434)
> +#define VD56G3_REG_AE_ROI_END_H				CCI_REG16_LE(0x0436)
> +#define VD56G3_REG_AE_ROI_END_V				CCI_REG16_LE(0x0438)
> +#define VD56G3_REG_AE_COMPENSATION			CCI_REG16_LE(0x043a)
> +#define VD56G3_REG_EXP_MODE				CCI_REG8(0x044c)
> +#define VD56G3_EXP_MODE_AUTO				0
> +#define VD56G3_EXP_MODE_FREEZE				1
> +#define VD56G3_EXP_MODE_MANUAL				2
> +#define VD56G3_REG_MANUAL_ANALOG_GAIN			CCI_REG8(0x044d)
> +#define VD56G3_REG_MANUAL_COARSE_EXPOSURE		CCI_REG16_LE(0x044e)
> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0		CCI_REG16_LE(0x0450)
> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1		CCI_REG16_LE(0x0452)
> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2		CCI_REG16_LE(0x0454)
> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3		CCI_REG16_LE(0x0456)
> +#define VD56G3_REG_FRAME_LENGTH				CCI_REG16_LE(0x0458)
> +#define VD56G3_REG_Y_START				CCI_REG16_LE(0x045a)
> +#define VD56G3_REG_Y_END				CCI_REG16_LE(0x045c)
> +#define VD56G3_REG_OUT_ROI_X_START			CCI_REG16_LE(0x045e)
> +#define VD56G3_REG_OUT_ROI_X_END			CCI_REG16_LE(0x0460)
> +#define VD56G3_REG_OUT_ROI_Y_START			CCI_REG16_LE(0x0462)
> +#define VD56G3_REG_OUT_ROI_Y_END			CCI_REG16_LE(0x0464)
> +#define VD56G3_REG_GPIO_0_CTRL				CCI_REG8(0x0467)
> +#define VD56G3_GPIOX_GPIO_IN				0x01
> +#define VD56G3_GPIOX_STROBE_MODE			0x02
> +#define VD56G3_REG_READOUT_CTRL				CCI_REG8(0x047e)
> +#define READOUT_NORMAL					0x00
> +#define READOUT_DIGITAL_BINNING_X2			0x01
> +
> +/*
> + * The VD56G3 pixel array is organized as follows:
> + *
> + * +--------------------------------+
> + * |                                | \
> + * |   +------------------------+   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   |   Default resolution   |   |  | Native height (1364)
> + * |   |      1120 x 1360       |   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   |                        |   |  |
> + * |   +------------------------+   |  |
> + * |                                | /
> + * +--------------------------------+
> + *   <----------------------------->
> + *                     \-------------------  Native width (1124)
> + *
> + * The native resolution is 1124x1364.
> + * The recommended/default resolution is 1120x1360 (multiple of 16).
> + */
> +#define VD56G3_NATIVE_WIDTH				1124
> +#define VD56G3_NATIVE_HEIGHT				1364
> +#define VD56G3_DEFAULT_WIDTH				1120
> +#define VD56G3_DEFAULT_HEIGHT				1360
> +#define VD56G3_DEFAULT_MODE				1
> +
> +/* PLL settings */
> +#define VD56G3_TARGET_PLL				804000000UL
> +#define VD56G3_VT_CLOCK_DIV				5
> +
> +/* Line length and Frame length (settings are for standard 10bits ADC mode) */
> +#define VD56G3_LINE_LENGTH_MIN				1236
> +#define VD56G3_VBLANK_MIN				110
> +#define VD56G3_FRAME_LENGTH_DEF_60FPS			2168
> +
> +/* Exposure settings */
> +#define VD56G3_EXPOSURE_MARGIN				75
> +#define VD56G3_EXPOSURE_DEFAULT				1420
> +
> +/* Output Interface settings */
> +#define VD56G3_MAX_CSI_DATA_LANES			2
> +#define VD56G3_LINK_FREQ_DEF_1LANE			750000000UL
> +#define VD56G3_LINK_FREQ_DEF_2LANES			402000000UL
> +
> +/* GPIOs */
> +#define VD56G3_NB_GPIOS					8
> +
> +/* regulator supplies */
> +static const char *const vd56g3_supply_names[] = {
> +	"VCORE",
> +	"VDDIO",
> +	"VANA",
> +};
> +
> +#define VD56G3_NUM_SUPPLIES		ARRAY_SIZE(vd56g3_supply_names)

Please use ARRAY_SIZE(...) directly.

> +
> +/* -----------------------------------------------------------------------------
> + * Models (VD56G3: Mono, VD66GY: Bayer RGB), Modes and formats
> + */
> +
> +enum vd56g3_models {
> +	VD56G3_MODEL_VD56G3,
> +	VD56G3_MODEL_VD66GY,
> +};
> +
> +struct vd56g3_mode {
> +	u32 width;
> +	u32 height;
> +};
> +
> +static const struct vd56g3_mode vd56g3_supported_modes[] = {
> +	{
> +		.width = VD56G3_NATIVE_WIDTH,
> +		.height = VD56G3_NATIVE_HEIGHT,
> +	},
> +	{
> +		.width = VD56G3_DEFAULT_WIDTH,
> +		.height = VD56G3_DEFAULT_HEIGHT,
> +	},
> +	{
> +		.width = 1024,
> +		.height = 1280,
> +	},
> +	{
> +		.width = 1024,
> +		.height = 768,
> +	},
> +	{
> +		.width = 768,
> +		.height = 1024,
> +	},
> +	{
> +		.width = 720,
> +		.height = 1280,
> +	},
> +	{
> +		.width = 640,
> +		.height = 480,
> +	},
> +	{
> +		.width = 480,
> +		.height = 640,
> +	},
> +	{
> +		.width = 320,
> +		.height = 240,
> +	},
> +};
> +
> +/*
> + * Sensor support 8bits and 10bits output in both variants
> + *  - Monochrome
> + *  - RGB (with all H/V flip variations)
> + */
> +static const unsigned int vd56g3_mbus_codes[2][5] = {
> +	{
> +		MEDIA_BUS_FMT_Y8_1X8,
> +		MEDIA_BUS_FMT_SGRBG8_1X8,
> +		MEDIA_BUS_FMT_SRGGB8_1X8,
> +		MEDIA_BUS_FMT_SBGGR8_1X8,
> +		MEDIA_BUS_FMT_SGBRG8_1X8,
> +	},
> +	{
> +		MEDIA_BUS_FMT_Y10_1X10,
> +		MEDIA_BUS_FMT_SGRBG10_1X10,
> +		MEDIA_BUS_FMT_SRGGB10_1X10,
> +		MEDIA_BUS_FMT_SBGGR10_1X10,
> +		MEDIA_BUS_FMT_SGBRG10_1X10,
> +	},
> +};
> +
> +enum vd56g3_expo_state {
> +	VD56G3_EXPO_AUTO,
> +	VD56G3_EXPO_AUTO_FREEZE,
> +	VD56G3_EXPO_MANUAL
> +};
> +
> +struct vd56g3 {
> +	struct i2c_client *i2c_client;
> +	struct v4l2_subdev sd;
> +	struct media_pad pad;
> +	struct regulator_bulk_data supplies[VD56G3_NUM_SUPPLIES];
> +	struct gpio_desc *reset_gpio;
> +	struct clk *xclk;
> +	struct regmap *regmap;
> +	u32 ext_clock;
> +	u32 pll_prediv;
> +	u32 pll_mult;
> +	u32 pixel_clock;
> +	u16 oif_ctrl;
> +	int nb_of_lane;
> +	u32 gpios[VD56G3_NB_GPIOS];
> +	unsigned long ext_leds_mask;
> +	bool is_mono;
> +	struct v4l2_ctrl_handler ctrl_handler;
> +	struct v4l2_ctrl *hblank_ctrl;
> +	struct v4l2_ctrl *vblank_ctrl;
> +	struct {
> +		struct v4l2_ctrl *hflip_ctrl;
> +		struct v4l2_ctrl *vflip_ctrl;
> +	};
> +	struct v4l2_ctrl *patgen_ctrl;
> +	struct {
> +		struct v4l2_ctrl *ae_ctrl;
> +		struct v4l2_ctrl *expo_ctrl;
> +		struct v4l2_ctrl *again_ctrl;
> +		struct v4l2_ctrl *dgain_ctrl;
> +	};
> +	struct v4l2_ctrl *ae_lock_ctrl;
> +	struct v4l2_ctrl *ae_bias_ctrl;
> +	struct v4l2_ctrl *led_ctrl;
> +	bool streaming;
> +};
> +
> +static inline struct vd56g3 *to_vd56g3(struct v4l2_subdev *sd)
> +{
> +	return container_of(sd, struct vd56g3, sd);
> +}
> +
> +static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
> +{
> +	return &container_of(ctrl->handler, struct vd56g3, ctrl_handler)->sd;
> +}
> +
> +/* -----------------------------------------------------------------------------
> + * Additional i2c register helpers
> + */
> +
> +static int vd56g3_poll_reg(struct vd56g3 *sensor, u32 reg, u8 poll_val,
> +			   int *err)
> +{
> +	unsigned int val = 0;
> +	int ret;
> +
> +	if (err && *err)
> +		return *err;
> +
> +	reg = reg & CCI_REG_ADDR_MASK;

Please use CCI_REG_ADDR() instead. I'd write this without the assignment,
i.e. call CCI_REG_ADDR() as an argument to the function.

> +	ret = regmap_read_poll_timeout(sensor->regmap, reg, val,
> +				       (val == poll_val), 2000,
> +				       500 * USEC_PER_MSEC);
> +
> +	if (ret && err)
> +		*err = ret;
> +
> +	return ret;
> +}
> +
> +static int vd56g3_wait_state(struct vd56g3 *sensor, int state, int *err)
> +{
> +	return vd56g3_poll_reg(sensor, VD56G3_REG_SYSTEM_FSM, state, err);
> +}
> +
> +/* -----------------------------------------------------------------------------
> + * Controls: definitions, helpers and handlers
> + */
> +
> +static const char *const vd56g3_tp_menu[] = { "Disabled", "Solid", "Colorbar",
> +					      "Gradbar",  "Hgrey", "Vgrey",
> +					      "Dgrey",	  "PN28" };
> +
> +static const s64 vd56g3_ev_bias_qmenu[] = { -4000, -3500, -3000, -2500, -2000,
> +					    -1500, -1000, -500,	 0,	500,
> +					    1000,  1500,  2000,	 2500,	3000,
> +					    3500,  4000 };
> +
> +static const s64 vd56g3_link_freq_1lane[] = { VD56G3_LINK_FREQ_DEF_1LANE };
> +
> +static const s64 vd56g3_link_freq_2lanes[] = { VD56G3_LINK_FREQ_DEF_2LANES };
> +
> +static u8 vd56g3_get_bpp(__u32 code)
> +{
> +	switch (code) {
> +	case MEDIA_BUS_FMT_Y8_1X8:
> +	case MEDIA_BUS_FMT_SGRBG8_1X8:
> +	case MEDIA_BUS_FMT_SRGGB8_1X8:
> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
> +	case MEDIA_BUS_FMT_SGBRG8_1X8:
> +		return 8;
> +	case MEDIA_BUS_FMT_Y10_1X10:
> +	case MEDIA_BUS_FMT_SGRBG10_1X10:
> +	case MEDIA_BUS_FMT_SRGGB10_1X10:
> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
> +	case MEDIA_BUS_FMT_SGBRG10_1X10:
> +		return 10;
> +	default:
> +		/* Should never happen */
> +		WARN(1, "Unsupported code %d. default to 8 bpp", code);

WARN() is discouraged nowadays. Same for the rest.

> +	}
> +
> +	return 8;
> +}
> +
> +static u8 vd56g3_get_datatype(__u32 code)
> +{
> +	switch (code) {
> +	case MEDIA_BUS_FMT_Y8_1X8:
> +	case MEDIA_BUS_FMT_SGRBG8_1X8:
> +	case MEDIA_BUS_FMT_SRGGB8_1X8:
> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
> +	case MEDIA_BUS_FMT_SGBRG8_1X8:
> +		return MIPI_CSI2_DT_RAW8;
> +	case MEDIA_BUS_FMT_Y10_1X10:
> +	case MEDIA_BUS_FMT_SGRBG10_1X10:
> +	case MEDIA_BUS_FMT_SRGGB10_1X10:
> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
> +	case MEDIA_BUS_FMT_SGBRG10_1X10:
> +		return MIPI_CSI2_DT_RAW10;
> +	default:
> +		/* Should never happen */
> +		WARN(1, "Unsupported code %d. default to MIPI_CSI2_DT_RAW8",
> +		     code);
> +	}
> +
> +	return MIPI_CSI2_DT_RAW8;
> +}
> +
> +static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
> +{
> +	u64 exposure = 0;
> +	u64 again = 0;
> +	u64 dgain = 0;

Do you need the assignments? The values are assigned by cci_read() below,
right?

> +	int ret = 0;
> +
> +	/*
> +	 * When 'force_cur_val' is enabled, save the ctrl value in 'cur.val'
> +	 * instead of the normal 'val', this is used during poweroff to cache
> +	 * volatile ctrls and enable coldstart.
> +	 */
> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_COARSE_EXPOSURE, &exposure,
> +		 &ret);
> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_ANALOG_GAIN, &again, &ret);
> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_DIGITAL_GAIN, &dgain, &ret);
> +	if (ret)
> +		return ret;
> +
> +	if (force_cur_val) {
> +		sensor->expo_ctrl->cur.val = exposure;
> +		sensor->again_ctrl->cur.val = again;
> +		sensor->dgain_ctrl->cur.val = dgain;
> +	} else {
> +		sensor->expo_ctrl->val = exposure;
> +		sensor->again_ctrl->val = again;
> +		sensor->dgain_ctrl->val = dgain;
> +	}
> +
> +	return ret;
> +}
> +
> +static int vd56g3_update_patgen(struct vd56g3 *sensor, u32 patgen_index)
> +{
> +	u32 pattern = patgen_index <= 3 ? patgen_index : patgen_index + 12;
> +	u16 patgen = pattern << VD56G3_PATGEN_TYPE_SHIFT;
> +	u8 duster = VD56G3_DUSTER_ENABLE_DEF_MODULES;
> +	u8 darkcal = VD56G3_DARKCAL_ENABLE;
> +	int ret = 0;
> +
> +	if (patgen_index) {
> +		patgen |= VD56G3_PATGEN_ENABLE;
> +		duster = VD56G3_DUSTER_DISABLE;
> +		darkcal = VD56G3_DARKCAL_DISABLE_DARKAVG;
> +	}
> +
> +	cci_write(sensor->regmap, VD56G3_REG_DUSTER_CTRL, duster, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_DARKCAL_CTRL, darkcal, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_PATGEN_CTRL, patgen, &ret);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_update_expo_cluster(struct vd56g3 *sensor, bool is_auto)
> +{
> +	int ret = 0;

This would be nicer as declared last.

> +	enum vd56g3_expo_state expo_state = is_auto ? VD56G3_EXP_MODE_AUTO :
> +						      VD56G3_EXP_MODE_MANUAL;
> +
> +	if (sensor->ae_ctrl->is_new)
> +		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
> +			  &ret);
> +
> +	/* In Auto expo, set coldstart parameters */
> +	if (is_auto && sensor->ae_ctrl->is_new) {
> +		cci_write(sensor->regmap,
> +			  VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE,
> +			  sensor->expo_ctrl->val, &ret);
> +		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_ANALOG_GAIN,
> +			  sensor->again_ctrl->val, &ret);
> +		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN,
> +			  sensor->dgain_ctrl->val, &ret);
> +	}
> +
> +	/* In Manual expo, set exposure, analog and digital gains */
> +	if (!is_auto && sensor->expo_ctrl->is_new)
> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_COARSE_EXPOSURE,
> +			  sensor->expo_ctrl->val, &ret);
> +
> +	if (!is_auto && sensor->again_ctrl->is_new)
> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_ANALOG_GAIN,
> +			  sensor->again_ctrl->val, &ret);
> +
> +	if (!is_auto && sensor->dgain_ctrl->is_new) {
> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0,
> +			  sensor->dgain_ctrl->val, &ret);
> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1,
> +			  sensor->dgain_ctrl->val, &ret);
> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2,
> +			  sensor->dgain_ctrl->val, &ret);
> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3,
> +			  sensor->dgain_ctrl->val, &ret);
> +	}
> +
> +	return ret;
> +}
> +
> +static int vd56g3_lock_exposure(struct vd56g3 *sensor, u32 lock_val)
> +{
> +	bool ae_lock = lock_val & V4L2_LOCK_EXPOSURE;
> +	enum vd56g3_expo_state expo_state = ae_lock ? VD56G3_EXP_MODE_FREEZE :
> +						      VD56G3_EXP_MODE_AUTO;
> +	int ret = 0;
> +
> +	if (sensor->ae_ctrl->val == V4L2_EXPOSURE_AUTO)
> +		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
> +			  &ret);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_write_gpiox(struct vd56g3 *sensor, unsigned long gpio_mask)
> +{
> +	unsigned long io;
> +	u32 gpio_val;
> +	int ret = 0;
> +
> +	for_each_set_bit(io, &gpio_mask, VD56G3_NB_GPIOS) {
> +		gpio_val = sensor->gpios[io];
> +
> +		if (gpio_val == VD56G3_GPIOX_STROBE_MODE &&
> +		    sensor->led_ctrl->val == V4L2_FLASH_LED_MODE_NONE)
> +			gpio_val = VD56G3_GPIOX_GPIO_IN;
> +
> +		cci_write(sensor->regmap, VD56G3_REG_GPIO_0_CTRL + io, gpio_val,
> +			  &ret);
> +	}
> +
> +	return ret;
> +}
> +
> +static int vd56g3_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
> +{
> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
> +	int ret = 0;
> +
> +	/* Interact with HW only when it is powered ON */
> +	if (!pm_runtime_get_if_in_use(&client->dev))
> +		return 0;
> +
> +	switch (ctrl->id) {
> +	case V4L2_CID_EXPOSURE_AUTO:
> +		ret = vd56g3_read_expo_cluster(sensor, false);
> +		break;
> +	default:
> +		ret = -EINVAL;
> +		break;
> +	}
> +
> +	pm_runtime_mark_last_busy(&client->dev);
> +	pm_runtime_put_autosuspend(&client->dev);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_s_ctrl(struct v4l2_ctrl *ctrl)
> +{
> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
> +	const struct v4l2_rect *crop;
> +	unsigned int frame_length = 0;
> +	unsigned int expo_max;
> +	unsigned int ae_compensation;
> +	bool is_auto = false;
> +	int ret;
> +
> +	struct v4l2_subdev_state *state =
> +		v4l2_subdev_get_locked_active_state(sd);
> +	crop = v4l2_subdev_state_get_crop(state, 0);
> +
> +	if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
> +		return 0;
> +
> +	/* Update controls state, range, etc. whatever the state of the HW */
> +	switch (ctrl->id) {
> +	case V4L2_CID_VBLANK:
> +		frame_length = crop->height + ctrl->val;
> +		expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
> +		__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
> +					 VD56G3_EXPOSURE_DEFAULT);
> +		break;
> +	case V4L2_CID_EXPOSURE_AUTO:
> +		is_auto = (ctrl->val == V4L2_EXPOSURE_AUTO);
> +		__v4l2_ctrl_grab(sensor->ae_lock_ctrl, !is_auto);
> +		__v4l2_ctrl_grab(sensor->ae_bias_ctrl, !is_auto);
> +		break;
> +	default:
> +		break;
> +	}
> +
> +	/* Interact with HW only when it is powered ON */
> +	if (!pm_runtime_get_if_in_use(&client->dev))
> +		return 0;
> +
> +	switch (ctrl->id) {
> +	case V4L2_CID_HFLIP:
> +		ret = cci_write(sensor->regmap, VD56G3_REG_ORIENTATION,
> +				sensor->hflip_ctrl->val |
> +					(sensor->vflip_ctrl->val << 1),
> +				NULL);
> +		break;
> +	case V4L2_CID_TEST_PATTERN:
> +		ret = vd56g3_update_patgen(sensor, ctrl->val);
> +		break;
> +	case V4L2_CID_EXPOSURE_AUTO:
> +		ret = vd56g3_update_expo_cluster(sensor, is_auto);
> +		break;
> +	case V4L2_CID_3A_LOCK:
> +		ret = vd56g3_lock_exposure(sensor, ctrl->val);
> +		break;
> +	case V4L2_CID_AUTO_EXPOSURE_BIAS:
> +		ae_compensation =
> +			DIV_ROUND_CLOSEST((int)vd56g3_ev_bias_qmenu[ctrl->val] *
> +					  256, 1000);
> +		ret = cci_write(sensor->regmap, VD56G3_REG_AE_COMPENSATION,
> +				ae_compensation, NULL);
> +		break;
> +	case V4L2_CID_VBLANK:
> +		ret = cci_write(sensor->regmap, VD56G3_REG_FRAME_LENGTH,
> +				frame_length, NULL);
> +		break;
> +	case V4L2_CID_FLASH_LED_MODE:
> +		ret = vd56g3_write_gpiox(sensor, sensor->ext_leds_mask);
> +		break;
> +	default:
> +		ret = -EINVAL;
> +		break;
> +	}
> +
> +	pm_runtime_mark_last_busy(&client->dev);
> +	pm_runtime_put_autosuspend(&client->dev);
> +
> +	return ret;
> +}
> +
> +static const struct v4l2_ctrl_ops vd56g3_ctrl_ops = {
> +	.g_volatile_ctrl = vd56g3_g_volatile_ctrl,
> +	.s_ctrl = vd56g3_s_ctrl,
> +};
> +
> +static void vd56g3_update_controls(struct vd56g3 *sensor)
> +{
> +	const struct v4l2_rect *crop;
> +	unsigned int hblank;
> +	unsigned int vblank_min, vblank, vblank_max;
> +	unsigned int frame_length;
> +	unsigned int expo_max;
> +
> +	struct v4l2_subdev_state *state =
> +		v4l2_subdev_get_locked_active_state(&sensor->sd);
> +	crop = v4l2_subdev_state_get_crop(state, 0);
> +
> +	hblank = VD56G3_LINE_LENGTH_MIN - crop->width;
> +	vblank_min = VD56G3_VBLANK_MIN;
> +	vblank = VD56G3_FRAME_LENGTH_DEF_60FPS - crop->height;
> +	vblank_max = 0xffff - crop->height;
> +	frame_length = crop->height + vblank;
> +	expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
> +
> +	/* Update blanking and exposure (ranges + values) */
> +	__v4l2_ctrl_modify_range(sensor->hblank_ctrl, hblank, hblank, 1,
> +				 hblank);
> +	__v4l2_ctrl_modify_range(sensor->vblank_ctrl, vblank_min, vblank_max, 1,
> +				 vblank);
> +	__v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, vblank);
> +	__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
> +				 VD56G3_EXPOSURE_DEFAULT);
> +	__v4l2_ctrl_s_ctrl(sensor->expo_ctrl, VD56G3_EXPOSURE_DEFAULT);

Note that __v4l2_ctrl_modify_range() and __v4l2_ctrl_s_ctrl() may fail.

> +}
> +
> +static int vd56g3_init_controls(struct vd56g3 *sensor)
> +{
> +	const struct v4l2_ctrl_ops *ops = &vd56g3_ctrl_ops;
> +	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
> +	struct v4l2_ctrl *ctrl;
> +	int ret;
> +
> +	v4l2_ctrl_handler_init(hdl, 25);
> +
> +	/* Horizontal & vertical flips modify bayer code on RGB variant */
> +	sensor->hflip_ctrl =
> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
> +	if (sensor->hflip_ctrl)
> +		sensor->hflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
> +
> +	sensor->vflip_ctrl =
> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
> +	if (sensor->vflip_ctrl)
> +		sensor->vflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
> +
> +	sensor->patgen_ctrl =
> +		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
> +					     ARRAY_SIZE(vd56g3_tp_menu) - 1, 0,
> +					     0, vd56g3_tp_menu);
> +
> +	ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
> +				      ARRAY_SIZE(vd56g3_link_freq_1lane) - 1, 0,
> +				      (sensor->nb_of_lane == 2) ?
> +					      vd56g3_link_freq_2lanes :
> +					      vd56g3_link_freq_1lane);
> +	if (ctrl)
> +		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
> +
> +	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
> +				 sensor->pixel_clock, sensor->pixel_clock, 1,
> +				 sensor->pixel_clock);
> +	if (ctrl)
> +		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
> +
> +	sensor->ae_ctrl = v4l2_ctrl_new_std_menu(hdl, ops,
> +						 V4L2_CID_EXPOSURE_AUTO,
> +						 V4L2_EXPOSURE_MANUAL, 0,
> +						 V4L2_EXPOSURE_AUTO);
> +
> +	sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK, 0,
> +						 GENMASK(2, 0), 0, 0);
> +
> +	sensor->ae_bias_ctrl =
> +		v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
> +				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) - 1,
> +				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) / 2,
> +				       vd56g3_ev_bias_qmenu);
> +
> +	/*
> +	 * Analog gain [1, 8] is computed with the following logic :
> +	 * 32/(32 - again_reg), with again_reg in the range [0:28]
> +	 * Digital gain [1.00, 8.00] is coded as a Fixed Point 5.8
> +	 */
> +	sensor->again_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
> +					       0, 28, 1, 0);
> +	sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
> +					       0x100, 0x800, 1, 0x100);
> +
> +	/*
> +	 * Set the exposure, horizontal and vertical blanking ctrls
> +	 * to hardcoded values, they will be updated in vd56g3_update_controls.
> +	 * Exposure being in an auto-cluster, set a significant value here.
> +	 */
> +	sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
> +					      VD56G3_EXPOSURE_DEFAULT,
> +					      VD56G3_EXPOSURE_DEFAULT, 1,
> +					      VD56G3_EXPOSURE_DEFAULT);
> +	sensor->hblank_ctrl =
> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 1, 1, 1, 1);
> +	if (sensor->hblank_ctrl)
> +		sensor->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
> +	sensor->vblank_ctrl =
> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, 1, 1, 1, 1);
> +
> +	/* Additional control based on device tree properties */
> +	if (sensor->ext_leds_mask)
> +		sensor->led_ctrl =
> +			v4l2_ctrl_new_std_menu(hdl, ops,
> +					       V4L2_CID_FLASH_LED_MODE,
> +					       V4L2_FLASH_LED_MODE_FLASH, 0,
> +					       V4L2_FLASH_LED_MODE_NONE);
> +
> +	if (hdl->error) {
> +		ret = hdl->error;
> +		goto free_ctrls;
> +	}
> +
> +	v4l2_ctrl_cluster(2, &sensor->hflip_ctrl);
> +	v4l2_ctrl_auto_cluster(4, &sensor->ae_ctrl, V4L2_EXPOSURE_MANUAL, true);
> +
> +	sensor->sd.ctrl_handler = hdl;

A newline here?

> +	return 0;
> +
> +free_ctrls:
> +	v4l2_ctrl_handler_free(hdl);

Here, too?

> +	return ret;
> +}
> +
> +/* -----------------------------------------------------------------------------
> + * Videos ops
> + */
> +
> +static int vd56g3_stream_on(struct vd56g3 *sensor,
> +			    struct v4l2_subdev_state *state)
> +{
> +	const struct v4l2_mbus_framefmt *format =
> +		v4l2_subdev_state_get_format(state, 0);
> +	const struct v4l2_rect *crop =
> +		v4l2_subdev_state_get_crop(state, 0);
> +	int ret = 0;
> +	unsigned int csi_mbps = ((sensor->nb_of_lane == 2) ?
> +					 VD56G3_LINK_FREQ_DEF_2LANES :
> +					 VD56G3_LINK_FREQ_DEF_1LANE) * 2 / MEGA;
> +	unsigned int binning;
> +
> +	/* configure clocks */
> +	cci_write(sensor->regmap, VD56G3_REG_EXT_CLOCK, sensor->ext_clock,
> +		  &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_CLK_PLL_PREDIV, sensor->pll_prediv,
> +		  &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_CLK_SYS_PLL_MULT, sensor->pll_mult,
> +		  &ret);
> +
> +	/* configure output */
> +	cci_write(sensor->regmap, VD56G3_REG_FORMAT_CTRL,
> +		  vd56g3_get_bpp(format->code), &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OIF_CSI_BITRATE, csi_mbps, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OIF_IMG_CTRL,
> +		  vd56g3_get_datatype(format->code), &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_ISL_ENABLE, 0, &ret);
> +
> +	/* configure binning mode */
> +	switch (crop->width / format->width) {
> +	case 1:
> +	default:
> +		binning = READOUT_NORMAL;
> +		break;
> +	case 2:
> +		binning = READOUT_DIGITAL_BINNING_X2;
> +		break;
> +	}
> +	cci_write(sensor->regmap, VD56G3_REG_READOUT_CTRL, binning, &ret);
> +
> +	/* configure ROIs */
> +	cci_write(sensor->regmap, VD56G3_REG_Y_START, crop->top, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_Y_END,
> +		  crop->top + crop->height - 1, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_START, crop->left, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_END,
> +		  crop->left + crop->width - 1, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_START, 0, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_END, crop->height - 1,
> +		  &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_H, crop->left, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_H,
> +		  crop->left + crop->width - 1, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_V, 0, &ret);
> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_V, crop->height - 1,
> +		  &ret);
> +	if (ret)
> +		return ret;
> +
> +	/* Setup default GPIO values; could be overridden by V4L2 ctrl setup */
> +	ret = vd56g3_write_gpiox(sensor, GENMASK(VD56G3_NB_GPIOS - 1, 0));
> +	if (ret)
> +		return ret;
> +
> +	/* Apply settings from V4L2 ctrls */
> +	ret = __v4l2_ctrl_handler_setup(&sensor->ctrl_handler);
> +	if (ret)
> +		return ret;
> +
> +	/* start streaming */
> +	cci_write(sensor->regmap, VD56G3_REG_STBY, VD56G3_CMD_START_STREAM,
> +		  &ret);
> +	vd56g3_poll_reg(sensor, VD56G3_REG_STBY, VD56G3_CMD_ACK, &ret);
> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_STREAMING, &ret);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_stream_off(struct vd56g3 *sensor)
> +{
> +	int ret = 0;

No need to initialise ret.

> +
> +	/* Retrieve Expo cluster to enable coldstart of AE */
> +	ret = vd56g3_read_expo_cluster(sensor, true);
> +
> +	cci_write(sensor->regmap, VD56G3_REG_STREAMING, VD56G3_CMD_STOP_STREAM,
> +		  &ret);
> +	vd56g3_poll_reg(sensor, VD56G3_REG_STREAMING, VD56G3_CMD_ACK, &ret);
> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_s_stream(struct v4l2_subdev *sd, int enable)
> +{
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
> +	int ret = 0;

ret should be declared as last.

> +

Extra newline.

> +	struct v4l2_subdev_state *state =
> +		v4l2_subdev_lock_and_get_active_state(sd);

Please do the assignment outside declarations---it has side effects.

> +
> +	if (enable) {
> +		ret = pm_runtime_resume_and_get(&client->dev);
> +		if (ret < 0)
> +			goto unlock;
> +		ret = vd56g3_stream_on(sensor, state);
> +		if (ret) {
> +			dev_err(&client->dev, "Failed to start streaming\n");
> +			pm_runtime_put_sync(&client->dev);
> +		}
> +	} else {
> +		vd56g3_stream_off(sensor);
> +		pm_runtime_mark_last_busy(&client->dev);
> +		pm_runtime_put_autosuspend(&client->dev);
> +	}
> +
> +unlock:
> +	v4l2_subdev_unlock_state(state);
> +	if (!ret) {
> +		sensor->streaming = enable;

This assignment may be done only with sub-device active state locked.

> +		v4l2_ctrl_grab(sensor->hflip_ctrl, enable);
> +		v4l2_ctrl_grab(sensor->vflip_ctrl, enable);
> +		v4l2_ctrl_grab(sensor->patgen_ctrl, enable);
> +	}
> +
> +	return ret;
> +}
> +
> +static const struct v4l2_subdev_video_ops vd56g3_video_ops = {
> +	.s_stream = vd56g3_s_stream,
> +};
> +
> +/* -----------------------------------------------------------------------------
> + * Pad ops
> + */
> +
> +/* Media bus code is dependent of :
> + *      - 8bits or 10bits output
> + *      - variant : Mono or RGB
> + *      - H/V flips parameters in case of RGB
> + */
> +static u32 vd56g3_get_mbus_code(struct vd56g3 *sensor, u32 code)
> +{
> +	unsigned int i_bpp;
> +	unsigned int j;
> +
> +	for (i_bpp = 0; i_bpp < ARRAY_SIZE(vd56g3_mbus_codes); i_bpp++) {
> +		for (j = 0; j < ARRAY_SIZE(vd56g3_mbus_codes[i_bpp]); j++) {
> +			if (vd56g3_mbus_codes[i_bpp][j] == code)
> +				goto endloops;
> +		}
> +	}

No need for braces. Up to you.

> +
> +endloops:
> +	if (i_bpp >= ARRAY_SIZE(vd56g3_mbus_codes))
> +		i_bpp = 0;
> +
> +	if (sensor->is_mono)
> +		j = 0;
> +	else
> +		j = 1 + (sensor->hflip_ctrl->val ? 1 : 0) +
> +		    (sensor->vflip_ctrl->val ? 2 : 0);
> +
> +	return vd56g3_mbus_codes[i_bpp][j];
> +}
> +
> +static int vd56g3_enum_mbus_code(struct v4l2_subdev *sd,
> +				 struct v4l2_subdev_state *sd_state,
> +				 struct v4l2_subdev_mbus_code_enum *code)
> +{
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +
> +	if (code->index >= ARRAY_SIZE(vd56g3_mbus_codes))
> +		return -EINVAL;
> +
> +	code->code =
> +		vd56g3_get_mbus_code(sensor, vd56g3_mbus_codes[code->index][0]);
> +
> +	return 0;
> +}
> +
> +static int vd56g3_enum_frame_size(struct v4l2_subdev *sd,
> +				  struct v4l2_subdev_state *sd_state,
> +				  struct v4l2_subdev_frame_size_enum *fse)
> +{
> +	if (fse->index >= ARRAY_SIZE(vd56g3_supported_modes))
> +		return -EINVAL;
> +
> +	fse->min_width = vd56g3_supported_modes[fse->index].width;
> +	fse->max_width = fse->min_width;
> +	fse->min_height = vd56g3_supported_modes[fse->index].height;
> +	fse->max_height = fse->min_height;
> +
> +	return 0;
> +}
> +
> +static void vd56g3_update_img_pad_format(struct vd56g3 *sensor,
> +					 const struct vd56g3_mode *mode,
> +					 u32 mbus_code,
> +					 struct v4l2_mbus_framefmt *mbus_fmt)
> +{
> +	mbus_fmt->width = mode->width;
> +	mbus_fmt->height = mode->height;
> +	mbus_fmt->code = vd56g3_get_mbus_code(sensor, mbus_code);
> +	mbus_fmt->colorspace = V4L2_COLORSPACE_RAW;
> +	mbus_fmt->field = V4L2_FIELD_NONE;
> +	mbus_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
> +	mbus_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
> +	mbus_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
> +}
> +
> +static int vd56g3_set_pad_fmt(struct v4l2_subdev *sd,
> +			      struct v4l2_subdev_state *sd_state,
> +			      struct v4l2_subdev_format *sd_fmt)
> +{
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +	const struct vd56g3_mode *new_mode;
> +	struct v4l2_rect pad_crop;
> +	unsigned int binning;
> +
> +	if (sensor->streaming)
> +		return -EBUSY;
> +
> +	new_mode = v4l2_find_nearest_size(vd56g3_supported_modes,
> +					  ARRAY_SIZE(vd56g3_supported_modes),
> +					  width, height, sd_fmt->format.width,
> +					  sd_fmt->format.height);
> +
> +	vd56g3_update_img_pad_format(sensor, new_mode, sd_fmt->format.code,
> +				     &sd_fmt->format);
> +
> +	/* Compute crop rectangle (maximized via binning) */
> +	binning = min(VD56G3_NATIVE_WIDTH / sd_fmt->format.width,
> +		      VD56G3_NATIVE_HEIGHT / sd_fmt->format.height);
> +	binning = min(binning, 2U);
> +	pad_crop.width = sd_fmt->format.width * binning;
> +	pad_crop.height = sd_fmt->format.height * binning;
> +	pad_crop.left = (VD56G3_NATIVE_WIDTH - pad_crop.width) / 2;
> +	pad_crop.top = (VD56G3_NATIVE_HEIGHT - pad_crop.height) / 2;
> +
> +	/* Update active state's format and crop */
> +	*v4l2_subdev_state_get_format(sd_state, sd_fmt->pad) = sd_fmt->format;
> +	*v4l2_subdev_state_get_crop(sd_state, sd_fmt->pad) = pad_crop;
> +
> +	if (sd_fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
> +		vd56g3_update_controls(sensor);
> +
> +	return 0;
> +}
> +
> +static int vd56g3_get_selection(struct v4l2_subdev *sd,
> +				struct v4l2_subdev_state *sd_state,
> +				struct v4l2_subdev_selection *sel)
> +{
> +	switch (sel->target) {
> +	case V4L2_SEL_TGT_CROP:
> +		sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
> +		break;
> +	case V4L2_SEL_TGT_NATIVE_SIZE:
> +	case V4L2_SEL_TGT_CROP_DEFAULT:
> +	case V4L2_SEL_TGT_CROP_BOUNDS:
> +		sel->r.top = 0;
> +		sel->r.left = 0;
> +		sel->r.width = VD56G3_NATIVE_WIDTH;
> +		sel->r.height = VD56G3_NATIVE_HEIGHT;
> +		break;
> +	default:
> +		return -EINVAL;
> +	}
> +
> +	return 0;
> +}
> +
> +static int vd56g3_init_state(struct v4l2_subdev *sd,
> +			     struct v4l2_subdev_state *sd_state)
> +{
> +	unsigned int def_mode = VD56G3_DEFAULT_MODE;
> +	struct v4l2_subdev_format fmt = {
> +		.which = V4L2_SUBDEV_FORMAT_TRY,
> +		.pad = 0,
> +		.format = {
> +			.code = vd56g3_mbus_codes[0][0],
> +			.width = vd56g3_supported_modes[def_mode].width,
> +			.height = vd56g3_supported_modes[def_mode].height,
> +		},
> +	};
> +
> +	vd56g3_set_pad_fmt(sd, sd_state, &fmt);

Newline here?

> +	return 0;
> +}
> +
> +static const struct v4l2_subdev_core_ops vd56g3_core_ops = {
> +	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
> +	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
> +};
> +
> +static const struct v4l2_subdev_pad_ops vd56g3_pad_ops = {
> +	.enum_mbus_code = vd56g3_enum_mbus_code,
> +	.enum_frame_size = vd56g3_enum_frame_size,
> +	.get_fmt = v4l2_subdev_get_fmt,
> +	.set_fmt = vd56g3_set_pad_fmt,
> +	.get_selection = vd56g3_get_selection,
> +};
> +
> +static const struct v4l2_subdev_ops vd56g3_subdev_ops = {
> +	.core = &vd56g3_core_ops,
> +	.video = &vd56g3_video_ops,
> +	.pad = &vd56g3_pad_ops,
> +};
> +
> +static const struct media_entity_operations vd56g3_subdev_entity_ops = {
> +	.link_validate = v4l2_subdev_link_validate,
> +};
> +
> +static const struct v4l2_subdev_internal_ops vd56g3_internal_ops = {
> +	.init_state = vd56g3_init_state,
> +};
> +
> +/* -----------------------------------------------------------------------------
> + * Power management
> + */
> +
> +static int vd56g3_boot(struct vd56g3 *sensor)
> +{
> +	int ret = 0;
> +
> +	cci_write(sensor->regmap, VD56G3_REG_BOOT, VD56G3_CMD_BOOT, &ret);
> +	vd56g3_poll_reg(sensor, VD56G3_REG_BOOT, VD56G3_CMD_ACK, &ret);
> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_power_on(struct vd56g3 *sensor)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	int ret;
> +
> +	ret = regulator_bulk_enable(VD56G3_NUM_SUPPLIES, sensor->supplies);
> +	if (ret) {
> +		dev_err(&client->dev, "Failed to enable regulators %d", ret);
> +		return ret;
> +	}
> +
> +	ret = clk_prepare_enable(sensor->xclk);
> +	if (ret) {
> +		dev_err(&client->dev, "Failed to enable clock %d", ret);
> +		goto disable_reg;
> +	}
> +
> +	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
> +	ret = vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_READY_TO_BOOT, NULL);
> +	if (ret) {
> +		dev_err(&client->dev, "Sensor reset failed %d\n", ret);
> +		goto disable_clock;
> +	}
> +
> +	return 0;
> +
> +disable_clock:
> +	clk_disable_unprepare(sensor->xclk);
> +	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
> +disable_reg:
> +	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
> +
> +	return ret;
> +}
> +
> +static int vd56g3_power_off(struct vd56g3 *sensor)
> +{
> +	clk_disable_unprepare(sensor->xclk);
> +	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
> +	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
> +	return 0;
> +}
> +
> +static int vd56g3_runtime_resume(struct device *dev)
> +{
> +	struct v4l2_subdev *sd = dev_get_drvdata(dev);
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +	struct i2c_client *client = sensor->i2c_client;
> +	int ret;
> +
> +	ret = vd56g3_power_on(sensor);

I think you could unify the runtime resume and suspend functions with the
power_on/off, using struct device * argument. vd56g3_power_on() won't be
called separately from vd65g3_boot either.

Speaking of which---do you really call vd65g3_boot() before identifying the
sensor?

> +	if (ret) {
> +		dev_err(&client->dev, "Failed to power on %d", ret);
> +		return ret;
> +	}
> +
> +	ret = vd56g3_boot(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "sensor boot failed %d", ret);
> +		return ret;
> +	}
> +
> +	return ret;
> +}
> +
> +static int vd56g3_runtime_suspend(struct device *dev)
> +{
> +	struct v4l2_subdev *sd = dev_get_drvdata(dev);
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +
> +	return vd56g3_power_off(sensor);
> +}
> +
> +static const struct dev_pm_ops vd56g3_pm_ops = {
> +	SET_RUNTIME_PM_OPS(vd56g3_runtime_suspend, vd56g3_runtime_resume, NULL)
> +};
> +
> +/* -----------------------------------------------------------------------------
> + * Probe and initialization
> + */
> +
> +static int vd56g3_check_csi_conf(struct vd56g3 *sensor,
> +				 struct fwnode_handle *endpoint)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
> +	u32 phy_data_lanes[VD56G3_MAX_CSI_DATA_LANES] = { ~0, ~0 };
> +	int n_lanes;
> +	int frequency;
> +	int p, l;
> +	int ret = 0;
> +
> +	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
> +	if (ret)
> +		return -EINVAL;
> +
> +	/* Check lanes number */
> +	n_lanes = ep.bus.mipi_csi2.num_data_lanes;
> +	if (n_lanes != 1 && n_lanes != 2) {
> +		dev_err(&client->dev, "Invalid data lane number %d\n", n_lanes);
> +		ret = -EINVAL;
> +		goto done;
> +	}
> +	sensor->nb_of_lane = n_lanes;
> +
> +	/* Clock lane must be first */
> +	if (ep.bus.mipi_csi2.clock_lane != 0) {
> +		dev_err(&client->dev, "Clk lane must be mapped to lane 0\n");
> +		ret = -EINVAL;
> +		goto done;
> +	}
> +
> +	/* Prepare Output Interface conf based on lane settings
> +	 * logical to physical lane conversion (+ pad remaining slots)
> +	 */
> +	for (l = 0; l < n_lanes; l++)
> +		phy_data_lanes[ep.bus.mipi_csi2.data_lanes[l] - 1] = l;
> +	for (p = 0; p < VD56G3_MAX_CSI_DATA_LANES; p++) {
> +		if (phy_data_lanes[p] != ~0)
> +			continue;
> +		phy_data_lanes[p] = l;
> +		l++;
> +	}
> +	sensor->oif_ctrl = n_lanes |
> +			   (ep.bus.mipi_csi2.lane_polarities[0] << 3) |
> +			   ((phy_data_lanes[0]) << 4) |
> +			   (ep.bus.mipi_csi2.lane_polarities[1] << 6) |
> +			   ((phy_data_lanes[1]) << 7) |
> +			   (ep.bus.mipi_csi2.lane_polarities[2] << 9);
> +
> +	/* Check link frequency */
> +	if (!ep.nr_of_link_frequencies) {
> +		dev_err(&client->dev, "link-frequency not found in DT\n");
> +		ret = -EINVAL;
> +		goto done;
> +	}
> +	frequency = (n_lanes == 2) ? VD56G3_LINK_FREQ_DEF_2LANES :
> +				     VD56G3_LINK_FREQ_DEF_1LANE;
> +	if (ep.nr_of_link_frequencies != 1 ||
> +	    ep.link_frequencies[0] != frequency) {
> +		dev_err(&client->dev, "Link frequency not supported: %lld\n",
> +			ep.link_frequencies[0]);
> +		ret = -EINVAL;
> +		goto done;
> +	}
> +
> +done:
> +	v4l2_fwnode_endpoint_free(&ep);
> +	return ret;
> +}
> +
> +static int vd56g3_parse_dt_gpios_array(struct vd56g3 *sensor, char *prop_name,
> +				       u32 *array, int *nb)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	struct device_node *np = client->dev.of_node;
> +	unsigned int i;
> +
> +	*nb = of_property_read_variable_u32_array(np, prop_name, array, 0,
> +						  VD56G3_NB_GPIOS);
> +
> +	if (*nb == -EINVAL) {
> +		*nb = 0;
> +		return *nb;
> +	} else if (*nb < 0) {
> +		dev_err(&client->dev, "Failed to read %s prop\n", prop_name);
> +		return *nb;
> +	}
> +
> +	for (i = 0; i < *nb; i++) {
> +		if (array[i] >= VD56G3_NB_GPIOS) {
> +			dev_err(&client->dev, "Invalid GPIO : %d\n", array[i]);
> +			return -EINVAL;
> +		}
> +	}
> +
> +	return 0;
> +}
> +
> +static int vd56g3_parse_dt_gpios(struct vd56g3 *sensor)
> +{
> +	u32 led_gpios[VD56G3_NB_GPIOS];
> +	int nb_gpios_leds;
> +	int ret;
> +	unsigned int i;
> +
> +	/* Initialize GPIOs to default */
> +	for (i = 0; i < VD56G3_NB_GPIOS; i++)
> +		sensor->gpios[i] = VD56G3_GPIOX_GPIO_IN;
> +	sensor->ext_leds_mask = 0;
> +
> +	/* Take into account optional 'st,leds' output for GPIOs */
> +	ret = vd56g3_parse_dt_gpios_array(sensor, "st,leds", led_gpios,
> +					  &nb_gpios_leds);
> +	if (ret)
> +		return ret;
> +	for (i = 0; i < nb_gpios_leds; i++) {
> +		sensor->gpios[led_gpios[i]] = VD56G3_GPIOX_STROBE_MODE;
> +		set_bit(led_gpios[i], &sensor->ext_leds_mask);
> +	}
> +
> +	return 0;
> +}
> +
> +static int vd56g3_parse_dt(struct vd56g3 *sensor)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	struct device *dev = &client->dev;
> +	struct fwnode_handle *endpoint;
> +	int ret;
> +
> +	endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0, 0);
> +	if (!endpoint) {
> +		dev_err(dev, "endpoint node not found\n");
> +		return -EINVAL;
> +	}
> +
> +	ret = vd56g3_check_csi_conf(sensor, endpoint);
> +	fwnode_handle_put(endpoint);
> +	if (ret)
> +		return ret;
> +
> +	ret = vd56g3_parse_dt_gpios(sensor);

	return ...;

> +	if (ret)
> +		return ret;
> +
> +	return 0;
> +}
> +
> +static int vd56g3_get_regulators(struct vd56g3 *sensor)
> +{
> +	int i;

unsigned int i?

> +
> +	for (i = 0; i < VD56G3_NUM_SUPPLIES; i++)
> +		sensor->supplies[i].supply = vd56g3_supply_names[i];
> +
> +	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
> +				       VD56G3_NUM_SUPPLIES, sensor->supplies);
> +}
> +
> +static int vd56g3_prepare_clock_tree(struct vd56g3 *sensor)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	const unsigned int predivs[] = { 1, 2, 4 };
> +	u32 pll_out;
> +	int i;
> +
> +	/* External clock must be in [6Mhz-27Mhz] */
> +	if (sensor->ext_clock < 6 * HZ_PER_MHZ ||
> +	    sensor->ext_clock > 27 * HZ_PER_MHZ) {
> +		dev_err(&client->dev,
> +			"Only 6Mhz-27Mhz clock range supported. Provided %lu MHz\n",
> +			sensor->ext_clock / HZ_PER_MHZ);
> +		return -EINVAL;
> +	}
> +
> +	/* PLL input should be in [6Mhz-12Mhz[ */
> +	for (i = 0; i < ARRAY_SIZE(predivs); i++) {
> +		sensor->pll_prediv = predivs[i];
> +		if (sensor->ext_clock / sensor->pll_prediv < 12 * HZ_PER_MHZ)
> +			break;
> +	}
> +
> +	/* PLL output clock must be as close as possible to 804Mhz */
> +	sensor->pll_mult = (VD56G3_TARGET_PLL * sensor->pll_prediv +
> +			    sensor->ext_clock / 2) /
> +			   sensor->ext_clock;
> +	pll_out = sensor->ext_clock * sensor->pll_mult / sensor->pll_prediv;
> +
> +	/* Target Pixel Clock for standard 10bit ADC mode : 160.8Mhz */
> +	sensor->pixel_clock = pll_out / VD56G3_VT_CLOCK_DIV;
> +
> +	return 0;
> +}
> +
> +static int vd56g3_detect(struct vd56g3 *sensor)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	struct device *dev = &client->dev;
> +	unsigned int model;
> +	u64 model_id = 0;
> +	u64 device_revision = 0;
> +	u64 optical_revision = 0;
> +	int ret = 0;
> +
> +	model = (uintptr_t)device_get_match_data(dev);
> +
> +	cci_read(sensor->regmap, VD56G3_REG_MODEL_ID, &model_id, &ret);
> +	if (ret)
> +		return ret;
> +
> +	if (model_id != VD56G3_MODEL_ID) {
> +		dev_warn(&client->dev, "Unsupported sensor id %x",
> +			 (u16)model_id);
> +		return -ENODEV;
> +	}
> +
> +	cci_read(sensor->regmap, VD56G3_REG_REVISION, &device_revision, &ret);
> +	if (ret)
> +		return ret;
> +
> +	if ((device_revision >> 8) != 0x31) {
> +		dev_warn(&client->dev, "Unsupported Cut version %x",
> +			 (u16)device_revision);
> +		return -ENODEV;
> +	}
> +
> +	cci_read(sensor->regmap, VD56G3_REG_OPTICAL_REVISION, &optical_revision,
> +		 &ret);
> +	if (ret)
> +		return ret;
> +
> +	sensor->is_mono =
> +		((optical_revision & 1) == VD56G3_OPTICAL_REVISION_MONO);
> +	if ((sensor->is_mono && model == VD56G3_MODEL_VD66GY) ||
> +	    (!sensor->is_mono && model == VD56G3_MODEL_VD56G3)) {
> +		dev_warn(&client->dev,

As you're returning an error, I'd use dev_err().

> +			 "Found %s sensor, while %s model is defined in DT",
> +			 (sensor->is_mono) ? "Mono" : "Bayer",
> +			 (model == VD56G3_MODEL_VD56G3) ? "vd56g3" : "vd66gy");
> +		return -ENODEV;
> +	}
> +
> +	return 0;
> +}
> +
> +static int vd56g3_subdev_init(struct vd56g3 *sensor)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	int ret;
> +
> +	/* Init sub device */
> +	v4l2_i2c_subdev_init(&sensor->sd, client, &vd56g3_subdev_ops);
> +	sensor->sd.internal_ops = &vd56g3_internal_ops;
> +	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
> +			    V4L2_SUBDEV_FL_HAS_EVENTS;
> +	sensor->sd.entity.ops = &vd56g3_subdev_entity_ops;
> +
> +	/* Init source pad */
> +	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
> +	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
> +	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
> +	if (ret) {
> +		dev_err(&client->dev, "Failed to init media entity : %d", ret);
> +		return ret;
> +	}
> +
> +	/* Init controls */
> +	ret = vd56g3_init_controls(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "Controls initialization failed %d", ret);
> +		goto err_media;
> +	}
> +
> +	/* Init vd56g3 struct : default resolution + raw8 */
> +	sensor->streaming = false;

sensor->streaming was already false (initialised to zero).

> +	sensor->sd.state_lock = sensor->ctrl_handler.lock;
> +	ret = v4l2_subdev_init_finalize(&sensor->sd);
> +	if (ret) {
> +		dev_err(&client->dev, "subdev init error: %d", ret);
> +		goto err_ctrls;
> +	}
> +
> +	vd56g3_update_controls(sensor);

Error handling is missing.

> +
> +	return 0;
> +
> +err_ctrls:
> +	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
> +
> +err_media:
> +	media_entity_cleanup(&sensor->sd.entity);
> +	return ret;
> +}
> +
> +static void vd56g3_subdev_cleanup(struct vd56g3 *sensor)
> +{
> +	v4l2_async_unregister_subdev(&sensor->sd);
> +	v4l2_subdev_cleanup(&sensor->sd);
> +	media_entity_cleanup(&sensor->sd.entity);
> +	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
> +}
> +
> +static int vd56g3_probe(struct i2c_client *client)
> +{
> +	struct device *dev = &client->dev;
> +	struct vd56g3 *sensor;
> +	int ret;
> +
> +	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
> +	if (!sensor)
> +		return -ENOMEM;
> +
> +	sensor->i2c_client = client;
> +
> +	ret = vd56g3_parse_dt(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "Failed to parse Device Tree : %d", ret);
> +		return ret;
> +	}
> +
> +	/* Get (and check) resources : power regs, ext clock, reset gpio */
> +	ret = vd56g3_get_regulators(sensor);
> +	if (ret)
> +		return dev_err_probe(dev, ret, "Failed to get regulators.");
> +
> +	sensor->xclk = devm_clk_get(dev, NULL);
> +	if (IS_ERR(sensor->xclk))
> +		return dev_err_probe(dev, PTR_ERR(sensor->xclk),
> +				     "Failed to get xclk.");
> +	sensor->ext_clock = clk_get_rate(sensor->xclk);
> +	ret = vd56g3_prepare_clock_tree(sensor);
> +	if (ret)
> +		return ret;
> +
> +	sensor->reset_gpio =
> +		devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
> +	if (IS_ERR(sensor->reset_gpio))
> +		return dev_err_probe(dev, PTR_ERR(sensor->reset_gpio),
> +				     "Failed to get reset gpio.");
> +
> +	sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
> +	if (IS_ERR(sensor->regmap))
> +		return dev_err_probe(dev, PTR_ERR(sensor->regmap),
> +				     "Failed to init regmap.");
> +
> +	/* Power ON */
> +	ret = vd56g3_power_on(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "Sensor power on failed : %d", ret);
> +		return ret;
> +	}
> +
> +	/* Enable PM runtime with autosuspend (sensor being ON, set active) */
> +	pm_runtime_set_active(dev);
> +	pm_runtime_get_noresume(dev);
> +	pm_runtime_enable(dev);
> +	pm_runtime_set_autosuspend_delay(dev, 1000);
> +	pm_runtime_use_autosuspend(dev);
> +
> +	/* Check HW model/version */
> +	ret = vd56g3_boot(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "Sensor boot failed : %d", ret);
> +		goto err_power_off;
> +	}
> +
> +	ret = vd56g3_detect(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "Sensor detect failed : %d", ret);
> +		goto err_power_off;
> +	}
> +
> +	/* Initialize, then register V4L2 subdev */
> +	ret = vd56g3_subdev_init(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "V4l2 init failed : %d", ret);
> +		goto err_power_off;
> +	}
> +
> +	ret = v4l2_async_register_subdev(&sensor->sd);
> +	if (ret) {
> +		dev_err(&client->dev, "async subdev register failed %d", ret);
> +		goto err_subdev;
> +	}
> +
> +	/* Sensor could now be powered off (after the autosuspend delay) */
> +	pm_runtime_mark_last_busy(dev);
> +	pm_runtime_put_autosuspend(dev);
> +
> +	dev_dbg(&client->dev, "Successfully probe %s sensor",
> +		(sensor->is_mono) ? "vd56g3" : "vd66gy");
> +
> +	return 0;
> +
> +err_subdev:
> +	vd56g3_subdev_cleanup(sensor);
> +err_power_off:
> +	pm_runtime_disable(dev);
> +	pm_runtime_put_noidle(dev);
> +	vd56g3_power_off(sensor);
> +	return ret;
> +}
> +
> +static void vd56g3_remove(struct i2c_client *client)
> +{
> +	struct v4l2_subdev *sd = i2c_get_clientdata(client);
> +	struct vd56g3 *sensor = to_vd56g3(sd);
> +
> +	vd56g3_subdev_cleanup(sensor);
> +
> +	pm_runtime_disable(&client->dev);
> +	if (!pm_runtime_status_suspended(&client->dev))
> +		vd56g3_power_off(sensor);
> +	pm_runtime_set_suspended(&client->dev);
> +}
> +
> +static const struct of_device_id vd56g3_dt_ids[] = {
> +	{ .compatible = "st,st-vd56g3", .data = (void *)VD56G3_MODEL_VD56G3 },
> +	{ .compatible = "st,st-vd66gy", .data = (void *)VD56G3_MODEL_VD66GY },
> +	{ /* sentinel */ }
> +};
> +MODULE_DEVICE_TABLE(of, vd56g3_dt_ids);
> +
> +static struct i2c_driver vd56g3_i2c_driver = {
> +	.driver = {
> +		.name  = "st-vd56g3",
> +		.of_match_table = vd56g3_dt_ids,
> +		.pm = &vd56g3_pm_ops,
> +	},
> +	.probe = vd56g3_probe,
> +	.remove = vd56g3_remove,
> +};
> +
> +module_i2c_driver(vd56g3_i2c_driver);
> +
> +MODULE_AUTHOR("Benjamin Mugnier <[e-mail]>");
> +MODULE_AUTHOR("Mickael Guene <[e-mail]>");
> +MODULE_AUTHOR("Sylvain Petinot <[e-mail]>");
> +MODULE_DESCRIPTION("ST VD56G3 sensor driver");
> +MODULE_LICENSE("GPL");

Sylvain Petinot May 3, 2024, 7:54 a.m. UTC | #4

Hi Tommaso,

Thanks for the review !

On 4/18/2024 11:17 AM, Tommaso Merciai wrote:
> Hi Sylvain,
> 
> Thanks for the patch.
> Quick review on my side :)
> 
> Premise:
> Unfortunately I'm not able to test this drv on my side (missing hw)
> either I'm not able to get the datasheet Googling a bit.

Sure, we are on the way to make it available on mass market. Datasheets
should be available soon for VD56G3 ([1]) and VD66GY ([2]).

[1] https://www.st.com/en/imaging-and-photonics-solutions/vd56g3.html
[2] https://www.st.com/en/imaging-and-photonics-solutions/vd66gy.html

> 
> On Wed, Apr 17, 2024 at 03:34:53PM +0200, Sylvain Petinot wrote:
>> Add V4L2 sub-device driver for STMicroelectronics VD56G3 camera sensor.
>> This is a 1.5 M pixel global shutter image sensor with an active array
>> size of 1124 x 1364 (portrait orientation).
>>
>> The driver supports Mono (VD56G3) and Color (VD66GY) variants.
>>
>> Signed-off-by: Sylvain Petinot <[e-mail]>
>> ---
>>  drivers/media/i2c/Kconfig     |   11 +
>>  drivers/media/i2c/Makefile    |    1 +
>>  drivers/media/i2c/st-vd56g3.c | 1619 +++++++++++++++++++++++++++++++++
>>  3 files changed, 1631 insertions(+)
>>  create mode 100644 drivers/media/i2c/st-vd56g3.c
>>
>> diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
>> index 56f276b920ab..a2d7271eef88 100644
>> --- a/drivers/media/i2c/Kconfig
>> +++ b/drivers/media/i2c/Kconfig
>> @@ -657,6 +657,17 @@ config VIDEO_S5K6A3
>>  	  This is a V4L2 sensor driver for Samsung S5K6A3 raw
>>  	  camera sensor.
>>  
>> +config VIDEO_ST_VD56G3
>> +	tristate "ST VD56G3 sensor support"
>> +	select V4L2_CCI_I2C
>> +	depends on OF && GPIOLIB
>> +	help
>> +	  This is a Video4Linux2 sensor driver for the ST VD56G3
>> +	  camera sensor.
>> +
>> +	  To compile this driver as a module, choose M here: the
>> +	  module will be called st-vd56g3.
>> +
>>  config VIDEO_ST_VGXY61
>>  	tristate "ST VGXY61 sensor support"
>>  	select V4L2_CCI_I2C
>> diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
>> index dfbe6448b549..b469cf0f8113 100644
>> --- a/drivers/media/i2c/Makefile
>> +++ b/drivers/media/i2c/Makefile
>> @@ -124,6 +124,7 @@ obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
>>  obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
>>  obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
>>  obj-$(CONFIG_VIDEO_ST_MIPID02) += st-mipid02.o
>> +obj-$(CONFIG_VIDEO_ST_VD56G3) += st-vd56g3.o
>>  obj-$(CONFIG_VIDEO_ST_VGXY61) += st-vgxy61.o
>>  obj-$(CONFIG_VIDEO_TC358743) += tc358743.o
>>  obj-$(CONFIG_VIDEO_TC358746) += tc358746.o
>> diff --git a/drivers/media/i2c/st-vd56g3.c b/drivers/media/i2c/st-vd56g3.c
>> new file mode 100644
>> index 000000000000..eff819941456
>> --- /dev/null
>> +++ b/drivers/media/i2c/st-vd56g3.c
>> @@ -0,0 +1,1619 @@
>> +// SPDX-License-Identifier: GPL-2.0
>> +/*
>> + * A V4L2 driver for ST VD56G3 (Mono) and VD66GY (RGB) global shutter cameras.
>> + * Copyright (C) 2024, STMicroelectronics SA
>> + */
>> +
>> +#include <linux/clk.h>
>> +#include <linux/delay.h>
>> +#include <linux/gpio/consumer.h>
>> +#include <linux/i2c.h>
>> +#include <linux/iopoll.h>
>> +#include <linux/module.h>
>> +#include <linux/pm_runtime.h>
>> +#include <linux/regmap.h>
>> +#include <linux/regulator/consumer.h>
>> +#include <linux/units.h>
>> +
>> +#include <asm/unaligned.h>
>> +
>> +#include <media/mipi-csi2.h>
>> +#include <media/v4l2-async.h>
>> +#include <media/v4l2-cci.h>
>> +#include <media/v4l2-ctrls.h>
>> +#include <media/v4l2-device.h>
>> +#include <media/v4l2-event.h>
>> +#include <media/v4l2-fwnode.h>
>> +#include <media/v4l2-subdev.h>
>> +
>> +/* Register Map */
>> +#define VD56G3_REG_MODEL_ID				CCI_REG16_LE(0x0000)
>> +#define VD56G3_MODEL_ID					0x5603
>> +#define VD56G3_REG_REVISION				CCI_REG16_LE(0x0002)
>> +#define VD56G3_REG_OPTICAL_REVISION			CCI_REG8(0x001a)
>> +#define VD56G3_OPTICAL_REVISION_MONO			0
>> +#define VD56G3_OPTICAL_REVISION_BAYER			1
>> +#define VD56G3_REG_SYSTEM_FSM				CCI_REG8(0x0028)
>> +#define VD56G3_SYSTEM_FSM_READY_TO_BOOT			0x01
>> +#define VD56G3_SYSTEM_FSM_SW_STBY			0x02
>> +#define VD56G3_SYSTEM_FSM_STREAMING			0x03
>> +#define VD56G3_REG_APPLIED_COARSE_EXPOSURE		CCI_REG16_LE(0x0064)
>> +#define VD56G3_REG_APPLIED_ANALOG_GAIN			CCI_REG8(0x0068)
>> +#define VD56G3_REG_APPLIED_DIGITAL_GAIN			CCI_REG16_LE(0x006a)
>> +#define VD56G3_REG_BOOT					CCI_REG8(0x0200)
>> +#define VD56G3_CMD_ACK					0
>> +#define VD56G3_CMD_BOOT					1
>> +#define VD56G3_REG_STBY					CCI_REG8(0x0201)
>> +#define VD56G3_CMD_START_STREAM				1
>> +#define VD56G3_REG_STREAMING				CCI_REG8(0x0202)
>> +#define VD56G3_CMD_STOP_STREAM				1
>> +#define VD56G3_REG_EXT_CLOCK				CCI_REG32_LE(0x0220)
>> +#define VD56G3_REG_CLK_PLL_PREDIV			CCI_REG8(0x0224)
>> +#define VD56G3_REG_CLK_SYS_PLL_MULT			CCI_REG8(0x0226)
>> +#define VD56G3_REG_ORIENTATION				CCI_REG8(0x0302)
>> +#define VD56G3_REG_FORMAT_CTRL				CCI_REG8(0x030a)
>> +#define VD56G3_REG_OIF_CTRL				CCI_REG16_LE(0x030c)
>> +#define VD56G3_REG_OIF_IMG_CTRL				CCI_REG8(0x030f)
>> +#define VD56G3_REG_OIF_CSI_BITRATE			CCI_REG16_LE(0x0312)
>> +#define VD56G3_REG_DUSTER_CTRL				CCI_REG8(0x0318)
>> +#define VD56G3_DUSTER_DISABLE				0
>> +#define VD56G3_DUSTER_ENABLE_DEF_MODULES		0x13
>> +#define VD56G3_REG_ISL_ENABLE				CCI_REG8(0x0333)
>> +#define VD56G3_REG_DARKCAL_CTRL				CCI_REG8(0x0340)
>> +#define VD56G3_DARKCAL_ENABLE				1
>> +#define VD56G3_DARKCAL_DISABLE_DARKAVG			2
>> +#define VD56G3_REG_PATGEN_CTRL				CCI_REG16_LE(0x0400)
>> +#define VD56G3_PATGEN_ENABLE				1
>> +#define VD56G3_PATGEN_TYPE_SHIFT			4
>> +#define VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE		CCI_REG16_LE(0x042a)
>> +#define VD56G3_REG_AE_COLDSTART_ANALOG_GAIN		CCI_REG8(0x042c)
>> +#define VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN		CCI_REG16_LE(0x042e)
>> +#define VD56G3_REG_AE_ROI_START_H			CCI_REG16_LE(0x0432)
>> +#define VD56G3_REG_AE_ROI_START_V			CCI_REG16_LE(0x0434)
>> +#define VD56G3_REG_AE_ROI_END_H				CCI_REG16_LE(0x0436)
>> +#define VD56G3_REG_AE_ROI_END_V				CCI_REG16_LE(0x0438)
>> +#define VD56G3_REG_AE_COMPENSATION			CCI_REG16_LE(0x043a)
>> +#define VD56G3_REG_EXP_MODE				CCI_REG8(0x044c)
>> +#define VD56G3_EXP_MODE_AUTO				0
>> +#define VD56G3_EXP_MODE_FREEZE				1
>> +#define VD56G3_EXP_MODE_MANUAL				2
>> +#define VD56G3_REG_MANUAL_ANALOG_GAIN			CCI_REG8(0x044d)
>> +#define VD56G3_REG_MANUAL_COARSE_EXPOSURE		CCI_REG16_LE(0x044e)
>> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0		CCI_REG16_LE(0x0450)
>> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1		CCI_REG16_LE(0x0452)
>> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2		CCI_REG16_LE(0x0454)
>> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3		CCI_REG16_LE(0x0456)
>> +#define VD56G3_REG_FRAME_LENGTH				CCI_REG16_LE(0x0458)
>> +#define VD56G3_REG_Y_START				CCI_REG16_LE(0x045a)
>> +#define VD56G3_REG_Y_END				CCI_REG16_LE(0x045c)
>> +#define VD56G3_REG_OUT_ROI_X_START			CCI_REG16_LE(0x045e)
>> +#define VD56G3_REG_OUT_ROI_X_END			CCI_REG16_LE(0x0460)
>> +#define VD56G3_REG_OUT_ROI_Y_START			CCI_REG16_LE(0x0462)
>> +#define VD56G3_REG_OUT_ROI_Y_END			CCI_REG16_LE(0x0464)
>> +#define VD56G3_REG_GPIO_0_CTRL				CCI_REG8(0x0467)
>> +#define VD56G3_GPIOX_GPIO_IN				0x01
>> +#define VD56G3_GPIOX_STROBE_MODE			0x02
>> +#define VD56G3_REG_READOUT_CTRL				CCI_REG8(0x047e)
>> +#define READOUT_NORMAL					0x00
>> +#define READOUT_DIGITAL_BINNING_X2			0x01
>> +
>> +/*
>> + * The VD56G3 pixel array is organized as follows:
>> + *
>> + * +--------------------------------+
>> + * |                                | \
>> + * |   +------------------------+   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   |   Default resolution   |   |  | Native height (1364)
>> + * |   |      1120 x 1360       |   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   +------------------------+   |  |
>> + * |                                | /
>> + * +--------------------------------+
>> + *   <----------------------------->
>> + *                     \-------------------  Native width (1124)
>> + *
>> + * The native resolution is 1124x1364.
>> + * The recommended/default resolution is 1120x1360 (multiple of 16).
>> + */
>> +#define VD56G3_NATIVE_WIDTH				1124
>> +#define VD56G3_NATIVE_HEIGHT				1364
>> +#define VD56G3_DEFAULT_WIDTH				1120
>> +#define VD56G3_DEFAULT_HEIGHT				1360
>> +#define VD56G3_DEFAULT_MODE				1
>> +
>> +/* PLL settings */
>> +#define VD56G3_TARGET_PLL				804000000UL
>> +#define VD56G3_VT_CLOCK_DIV				5
>> +
>> +/* Line length and Frame length (settings are for standard 10bits ADC mode) */
>> +#define VD56G3_LINE_LENGTH_MIN				1236
>> +#define VD56G3_VBLANK_MIN				110
>> +#define VD56G3_FRAME_LENGTH_DEF_60FPS			2168
>> +
>> +/* Exposure settings */
>> +#define VD56G3_EXPOSURE_MARGIN				75
>> +#define VD56G3_EXPOSURE_DEFAULT				1420
>> +
>> +/* Output Interface settings */
>> +#define VD56G3_MAX_CSI_DATA_LANES			2
>> +#define VD56G3_LINK_FREQ_DEF_1LANE			750000000UL
>> +#define VD56G3_LINK_FREQ_DEF_2LANES			402000000UL
>> +
>> +/* GPIOs */
>> +#define VD56G3_NB_GPIOS					8
>> +
>> +/* regulator supplies */
>> +static const char *const vd56g3_supply_names[] = {
>> +	"VCORE",
>> +	"VDDIO",
>> +	"VANA",
>> +};
>> +
>> +#define VD56G3_NUM_SUPPLIES		ARRAY_SIZE(vd56g3_supply_names)
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Models (VD56G3: Mono, VD66GY: Bayer RGB), Modes and formats
>> + */
>> +
>> +enum vd56g3_models {
>> +	VD56G3_MODEL_VD56G3,
>> +	VD56G3_MODEL_VD66GY,
>> +};
>> +
>> +struct vd56g3_mode {
>> +	u32 width;
>> +	u32 height;
>> +};
>> +
>> +static const struct vd56g3_mode vd56g3_supported_modes[] = {
>> +	{
>> +		.width = VD56G3_NATIVE_WIDTH,
>> +		.height = VD56G3_NATIVE_HEIGHT,
>> +	},
>> +	{
>> +		.width = VD56G3_DEFAULT_WIDTH,
>> +		.height = VD56G3_DEFAULT_HEIGHT,
>> +	},
>> +	{
>> +		.width = 1024,
>> +		.height = 1280,
>> +	},
>> +	{
>> +		.width = 1024,
>> +		.height = 768,
>> +	},
>> +	{
>> +		.width = 768,
>> +		.height = 1024,
>> +	},
>> +	{
>> +		.width = 720,
>> +		.height = 1280,
>> +	},
>> +	{
>> +		.width = 640,
>> +		.height = 480,
>> +	},
>> +	{
>> +		.width = 480,
>> +		.height = 640,
>> +	},
>> +	{
>> +		.width = 320,
>> +		.height = 240,
>> +	},
>> +};
>> +
>> +/*
>> + * Sensor support 8bits and 10bits output in both variants
>> + *  - Monochrome
>> + *  - RGB (with all H/V flip variations)
>> + */
>> +static const unsigned int vd56g3_mbus_codes[2][5] = {
>> +	{
>> +		MEDIA_BUS_FMT_Y8_1X8,
>> +		MEDIA_BUS_FMT_SGRBG8_1X8,
>> +		MEDIA_BUS_FMT_SRGGB8_1X8,
>> +		MEDIA_BUS_FMT_SBGGR8_1X8,
>> +		MEDIA_BUS_FMT_SGBRG8_1X8,
>> +	},
>> +	{
>> +		MEDIA_BUS_FMT_Y10_1X10,
>> +		MEDIA_BUS_FMT_SGRBG10_1X10,
>> +		MEDIA_BUS_FMT_SRGGB10_1X10,
>> +		MEDIA_BUS_FMT_SBGGR10_1X10,
>> +		MEDIA_BUS_FMT_SGBRG10_1X10,
>> +	},
>> +};
>> +
>> +enum vd56g3_expo_state {
>> +	VD56G3_EXPO_AUTO,
>> +	VD56G3_EXPO_AUTO_FREEZE,
>> +	VD56G3_EXPO_MANUAL
>> +};
>> +
>> +struct vd56g3 {
>> +	struct i2c_client *i2c_client;
>> +	struct v4l2_subdev sd;
>> +	struct media_pad pad;
>> +	struct regulator_bulk_data supplies[VD56G3_NUM_SUPPLIES];
>> +	struct gpio_desc *reset_gpio;
>> +	struct clk *xclk;
>> +	struct regmap *regmap;
>> +	u32 ext_clock;
>> +	u32 pll_prediv;
>> +	u32 pll_mult;
>> +	u32 pixel_clock;
>> +	u16 oif_ctrl;
>> +	int nb_of_lane;
> 
> I think you can use u8 for nb_of_lane;

Yes, better !
Applied for V2.

> 
>> +	u32 gpios[VD56G3_NB_GPIOS];
>> +	unsigned long ext_leds_mask;
>> +	bool is_mono;
>> +	struct v4l2_ctrl_handler ctrl_handler;
>> +	struct v4l2_ctrl *hblank_ctrl;
>> +	struct v4l2_ctrl *vblank_ctrl;
>> +	struct {
>> +		struct v4l2_ctrl *hflip_ctrl;
>> +		struct v4l2_ctrl *vflip_ctrl;
>> +	};
>> +	struct v4l2_ctrl *patgen_ctrl;
>> +	struct {
>> +		struct v4l2_ctrl *ae_ctrl;
>> +		struct v4l2_ctrl *expo_ctrl;
>> +		struct v4l2_ctrl *again_ctrl;
>> +		struct v4l2_ctrl *dgain_ctrl;
>> +	};
>> +	struct v4l2_ctrl *ae_lock_ctrl;
>> +	struct v4l2_ctrl *ae_bias_ctrl;
>> +	struct v4l2_ctrl *led_ctrl;
>> +	bool streaming;
>> +};
>> +
>> +static inline struct vd56g3 *to_vd56g3(struct v4l2_subdev *sd)
>> +{
>> +	return container_of(sd, struct vd56g3, sd);
> 
> I think here you can use container_of_const

Better. I didn't know about it.

> 
>> +}
>> +
>> +static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
>> +{
>> +	return &container_of(ctrl->handler, struct vd56g3, ctrl_handler)->sd;
> 
> Same here.

dito

> 
>> +}
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Additional i2c register helpers
>> + */
>> +
>> +static int vd56g3_poll_reg(struct vd56g3 *sensor, u32 reg, u8 poll_val,
>> +			   int *err)
>> +{
>> +	unsigned int val = 0;
>> +	int ret;
>> +
>> +	if (err && *err)
>> +		return *err;
>> +
>> +	reg = reg & CCI_REG_ADDR_MASK;
>> +	ret = regmap_read_poll_timeout(sensor->regmap, reg, val,
>> +				       (val == poll_val), 2000,
>> +				       500 * USEC_PER_MSEC);
>> +
>> +	if (ret && err)
>> +		*err = ret;
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_wait_state(struct vd56g3 *sensor, int state, int *err)
>> +{
>> +	return vd56g3_poll_reg(sensor, VD56G3_REG_SYSTEM_FSM, state, err);
>> +}
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Controls: definitions, helpers and handlers
>> + */
>> +
>> +static const char *const vd56g3_tp_menu[] = { "Disabled", "Solid", "Colorbar",
>> +					      "Gradbar",  "Hgrey", "Vgrey",
>> +					      "Dgrey",	  "PN28" };
>> +
>> +static const s64 vd56g3_ev_bias_qmenu[] = { -4000, -3500, -3000, -2500, -2000,
>> +					    -1500, -1000, -500,	 0,	500,
>> +					    1000,  1500,  2000,	 2500,	3000,
>> +					    3500,  4000 };
>> +
>> +static const s64 vd56g3_link_freq_1lane[] = { VD56G3_LINK_FREQ_DEF_1LANE };
>> +
>> +static const s64 vd56g3_link_freq_2lanes[] = { VD56G3_LINK_FREQ_DEF_2LANES };
>> +
>> +static u8 vd56g3_get_bpp(__u32 code)
>> +{
>> +	switch (code) {
>> +	case MEDIA_BUS_FMT_Y8_1X8:
>> +	case MEDIA_BUS_FMT_SGRBG8_1X8:
>> +	case MEDIA_BUS_FMT_SRGGB8_1X8:
>> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
>> +	case MEDIA_BUS_FMT_SGBRG8_1X8:
>> +		return 8;
>> +	case MEDIA_BUS_FMT_Y10_1X10:
>> +	case MEDIA_BUS_FMT_SGRBG10_1X10:
>> +	case MEDIA_BUS_FMT_SRGGB10_1X10:
>> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
>> +	case MEDIA_BUS_FMT_SGBRG10_1X10:
>> +		return 10;
>> +	default:
>> +		/* Should never happen */
>> +		WARN(1, "Unsupported code %d. default to 8 bpp", code);
>> +	}
>> +
>> +	return 8;
>> +}
>> +
>> +static u8 vd56g3_get_datatype(__u32 code)
>> +{
>> +	switch (code) {
>> +	case MEDIA_BUS_FMT_Y8_1X8:
>> +	case MEDIA_BUS_FMT_SGRBG8_1X8:
>> +	case MEDIA_BUS_FMT_SRGGB8_1X8:
>> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
>> +	case MEDIA_BUS_FMT_SGBRG8_1X8:
>> +		return MIPI_CSI2_DT_RAW8;
>> +	case MEDIA_BUS_FMT_Y10_1X10:
>> +	case MEDIA_BUS_FMT_SGRBG10_1X10:
>> +	case MEDIA_BUS_FMT_SRGGB10_1X10:
>> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
>> +	case MEDIA_BUS_FMT_SGBRG10_1X10:
>> +		return MIPI_CSI2_DT_RAW10;
>> +	default:
>> +		/* Should never happen */
>> +		WARN(1, "Unsupported code %d. default to MIPI_CSI2_DT_RAW8",
>> +		     code);
>> +	}
>> +
>> +	return MIPI_CSI2_DT_RAW8;
>> +}
>> +
>> +static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
>> +{
>> +	u64 exposure = 0;
>> +	u64 again = 0;
>> +	u64 dgain = 0;
>> +	int ret = 0;
>> +
>> +	/*
>> +	 * When 'force_cur_val' is enabled, save the ctrl value in 'cur.val'
>> +	 * instead of the normal 'val', this is used during poweroff to cache
>> +	 * volatile ctrls and enable coldstart.
>> +	 */
>> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_COARSE_EXPOSURE, &exposure,
>> +		 &ret);
>> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_ANALOG_GAIN, &again, &ret);
>> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_DIGITAL_GAIN, &dgain, &ret);
>> +	if (ret)
>> +		return ret;
>> +
>> +	if (force_cur_val) {
>> +		sensor->expo_ctrl->cur.val = exposure;
>> +		sensor->again_ctrl->cur.val = again;
>> +		sensor->dgain_ctrl->cur.val = dgain;
>> +	} else {
>> +		sensor->expo_ctrl->val = exposure;
>> +		sensor->again_ctrl->val = again;
>> +		sensor->dgain_ctrl->val = dgain;
>> +	}
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_update_patgen(struct vd56g3 *sensor, u32 patgen_index)
>> +{
>> +	u32 pattern = patgen_index <= 3 ? patgen_index : patgen_index + 12;
>> +	u16 patgen = pattern << VD56G3_PATGEN_TYPE_SHIFT;
>> +	u8 duster = VD56G3_DUSTER_ENABLE_DEF_MODULES;
>> +	u8 darkcal = VD56G3_DARKCAL_ENABLE;
>> +	int ret = 0;
>> +
>> +	if (patgen_index) {
>> +		patgen |= VD56G3_PATGEN_ENABLE;
>> +		duster = VD56G3_DUSTER_DISABLE;
>> +		darkcal = VD56G3_DARKCAL_DISABLE_DARKAVG;
>> +	}
>> +
>> +	cci_write(sensor->regmap, VD56G3_REG_DUSTER_CTRL, duster, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_DARKCAL_CTRL, darkcal, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_PATGEN_CTRL, patgen, &ret);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_update_expo_cluster(struct vd56g3 *sensor, bool is_auto)
>> +{
>> +	int ret = 0;
>> +	enum vd56g3_expo_state expo_state = is_auto ? VD56G3_EXP_MODE_AUTO :
>> +						      VD56G3_EXP_MODE_MANUAL;
>> +
>> +	if (sensor->ae_ctrl->is_new)
>> +		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
>> +			  &ret);
>> +
>> +	/* In Auto expo, set coldstart parameters */
>> +	if (is_auto && sensor->ae_ctrl->is_new) {
>> +		cci_write(sensor->regmap,
>> +			  VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE,
>> +			  sensor->expo_ctrl->val, &ret);
>> +		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_ANALOG_GAIN,
>> +			  sensor->again_ctrl->val, &ret);
>> +		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN,
>> +			  sensor->dgain_ctrl->val, &ret);
>> +	}
>> +
>> +	/* In Manual expo, set exposure, analog and digital gains */
>> +	if (!is_auto && sensor->expo_ctrl->is_new)
>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_COARSE_EXPOSURE,
>> +			  sensor->expo_ctrl->val, &ret);
>> +
>> +	if (!is_auto && sensor->again_ctrl->is_new)
>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_ANALOG_GAIN,
>> +			  sensor->again_ctrl->val, &ret);
>> +
>> +	if (!is_auto && sensor->dgain_ctrl->is_new) {
>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0,
>> +			  sensor->dgain_ctrl->val, &ret);
>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1,
>> +			  sensor->dgain_ctrl->val, &ret);
>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2,
>> +			  sensor->dgain_ctrl->val, &ret);
>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3,
>> +			  sensor->dgain_ctrl->val, &ret);
>> +	}
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_lock_exposure(struct vd56g3 *sensor, u32 lock_val)
>> +{
>> +	bool ae_lock = lock_val & V4L2_LOCK_EXPOSURE;
>> +	enum vd56g3_expo_state expo_state = ae_lock ? VD56G3_EXP_MODE_FREEZE :
>> +						      VD56G3_EXP_MODE_AUTO;
>> +	int ret = 0;
>> +
>> +	if (sensor->ae_ctrl->val == V4L2_EXPOSURE_AUTO)
>> +		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
>> +			  &ret);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_write_gpiox(struct vd56g3 *sensor, unsigned long gpio_mask)
>> +{
>> +	unsigned long io;
>> +	u32 gpio_val;
>> +	int ret = 0;
>> +
>> +	for_each_set_bit(io, &gpio_mask, VD56G3_NB_GPIOS) {
>> +		gpio_val = sensor->gpios[io];
>> +
>> +		if (gpio_val == VD56G3_GPIOX_STROBE_MODE &&
>> +		    sensor->led_ctrl->val == V4L2_FLASH_LED_MODE_NONE)
>> +			gpio_val = VD56G3_GPIOX_GPIO_IN;
>> +
>> +		cci_write(sensor->regmap, VD56G3_REG_GPIO_0_CTRL + io, gpio_val,
>> +			  &ret);
>> +	}
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
>> +{
>> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
>> +	int ret = 0;
>> +
>> +	/* Interact with HW only when it is powered ON */
>> +	if (!pm_runtime_get_if_in_use(&client->dev))
>> +		return 0;
>> +
>> +	switch (ctrl->id) {
>> +	case V4L2_CID_EXPOSURE_AUTO:
>> +		ret = vd56g3_read_expo_cluster(sensor, false);
>> +		break;
>> +	default:
>> +		ret = -EINVAL;
>> +		break;
>> +	}
>> +
>> +	pm_runtime_mark_last_busy(&client->dev);
>> +	pm_runtime_put_autosuspend(&client->dev);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_s_ctrl(struct v4l2_ctrl *ctrl)
>> +{
>> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
>> +	const struct v4l2_rect *crop;
>> +	unsigned int frame_length = 0;
>> +	unsigned int expo_max;
>> +	unsigned int ae_compensation;
>> +	bool is_auto = false;
>> +	int ret;
>> +
>> +	struct v4l2_subdev_state *state =
>> +		v4l2_subdev_get_locked_active_state(sd);
>> +	crop = v4l2_subdev_state_get_crop(state, 0);
>> +
>> +	if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
>> +		return 0;
>> +
>> +	/* Update controls state, range, etc. whatever the state of the HW */
>> +	switch (ctrl->id) {
>> +	case V4L2_CID_VBLANK:
>> +		frame_length = crop->height + ctrl->val;
>> +		expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
>> +		__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
>> +					 VD56G3_EXPOSURE_DEFAULT);
>> +		break;
>> +	case V4L2_CID_EXPOSURE_AUTO:
>> +		is_auto = (ctrl->val == V4L2_EXPOSURE_AUTO);
>> +		__v4l2_ctrl_grab(sensor->ae_lock_ctrl, !is_auto);
>> +		__v4l2_ctrl_grab(sensor->ae_bias_ctrl, !is_auto);
>> +		break;
>> +	default:
>> +		break;
>> +	}
>> +
>> +	/* Interact with HW only when it is powered ON */
>> +	if (!pm_runtime_get_if_in_use(&client->dev))
>> +		return 0;
>> +
>> +	switch (ctrl->id) {
>> +	case V4L2_CID_HFLIP:
>> +		ret = cci_write(sensor->regmap, VD56G3_REG_ORIENTATION,
>> +				sensor->hflip_ctrl->val |
>> +					(sensor->vflip_ctrl->val << 1),
>> +				NULL);
>> +		break;
>> +	case V4L2_CID_TEST_PATTERN:
>> +		ret = vd56g3_update_patgen(sensor, ctrl->val);
>> +		break;
>> +	case V4L2_CID_EXPOSURE_AUTO:
>> +		ret = vd56g3_update_expo_cluster(sensor, is_auto);
>> +		break;
>> +	case V4L2_CID_3A_LOCK:
>> +		ret = vd56g3_lock_exposure(sensor, ctrl->val);
>> +		break;
>> +	case V4L2_CID_AUTO_EXPOSURE_BIAS:
>> +		ae_compensation =
>> +			DIV_ROUND_CLOSEST((int)vd56g3_ev_bias_qmenu[ctrl->val] *
>> +					  256, 1000);
>> +		ret = cci_write(sensor->regmap, VD56G3_REG_AE_COMPENSATION,
>> +				ae_compensation, NULL);
>> +		break;
>> +	case V4L2_CID_VBLANK:
>> +		ret = cci_write(sensor->regmap, VD56G3_REG_FRAME_LENGTH,
>> +				frame_length, NULL);
>> +		break;
>> +	case V4L2_CID_FLASH_LED_MODE:
>> +		ret = vd56g3_write_gpiox(sensor, sensor->ext_leds_mask);
>> +		break;
>> +	default:
>> +		ret = -EINVAL;
>> +		break;
>> +	}
>> +
>> +	pm_runtime_mark_last_busy(&client->dev);
>> +	pm_runtime_put_autosuspend(&client->dev);
>> +
>> +	return ret;
>> +}
>> +
>> +static const struct v4l2_ctrl_ops vd56g3_ctrl_ops = {
>> +	.g_volatile_ctrl = vd56g3_g_volatile_ctrl,
>> +	.s_ctrl = vd56g3_s_ctrl,
>> +};
>> +
>> +static void vd56g3_update_controls(struct vd56g3 *sensor)
>> +{
>> +	const struct v4l2_rect *crop;
>> +	unsigned int hblank;
>> +	unsigned int vblank_min, vblank, vblank_max;
>> +	unsigned int frame_length;
>> +	unsigned int expo_max;
>> +
>> +	struct v4l2_subdev_state *state =
>> +		v4l2_subdev_get_locked_active_state(&sensor->sd);
>> +	crop = v4l2_subdev_state_get_crop(state, 0);
>> +
>> +	hblank = VD56G3_LINE_LENGTH_MIN - crop->width;
>> +	vblank_min = VD56G3_VBLANK_MIN;
>> +	vblank = VD56G3_FRAME_LENGTH_DEF_60FPS - crop->height;
>> +	vblank_max = 0xffff - crop->height;
>> +	frame_length = crop->height + vblank;
>> +	expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
>> +
> 
> --
> 
>> +	/* Update blanking and exposure (ranges + values) */
>> +	__v4l2_ctrl_modify_range(sensor->hblank_ctrl, hblank, hblank, 1,
>> +				 hblank);
>> +	__v4l2_ctrl_modify_range(sensor->vblank_ctrl, vblank_min, vblank_max, 1,
>> +				 vblank);
>> +	__v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, vblank);
>> +	__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
>> +				 VD56G3_EXPOSURE_DEFAULT);
>> +	__v4l2_ctrl_s_ctrl(sensor->expo_ctrl, VD56G3_EXPOSURE_DEFAULT);
> 
> Here you don't have to check the return value fo the __v4l2__ctrl
> functions using a ret value? Or I'm completely wrong?
> 

Well good question... Honestly I didn't really give it much thought (I
took inspiration from the imx219 and imx290 drivers which do not have
the return test).
But Sakari made the same kind of comment, so I definitely have to check
how to handle this properly.

> Please check drivers/media/i2c/imx334.c:624
> imx334_update_controls
> 
>> +}
>> +
>> +static int vd56g3_init_controls(struct vd56g3 *sensor)
>> +{
>> +	const struct v4l2_ctrl_ops *ops = &vd56g3_ctrl_ops;
>> +	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
>> +	struct v4l2_ctrl *ctrl;
>> +	int ret;
>> +
>> +	v4l2_ctrl_handler_init(hdl, 25);
>> +
>> +	/* Horizontal & vertical flips modify bayer code on RGB variant */
>> +	sensor->hflip_ctrl =
>> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
>> +	if (sensor->hflip_ctrl)
>> +		sensor->hflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
>> +
>> +	sensor->vflip_ctrl =
>> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
>> +	if (sensor->vflip_ctrl)
>> +		sensor->vflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
>> +
>> +	sensor->patgen_ctrl =
>> +		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
>> +					     ARRAY_SIZE(vd56g3_tp_menu) - 1, 0,
>> +					     0, vd56g3_tp_menu);
>> +
>> +	ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
>> +				      ARRAY_SIZE(vd56g3_link_freq_1lane) - 1, 0,
>> +				      (sensor->nb_of_lane == 2) ?
>> +					      vd56g3_link_freq_2lanes :
>> +					      vd56g3_link_freq_1lane);
>> +	if (ctrl)
>> +		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
>> +
>> +	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
>> +				 sensor->pixel_clock, sensor->pixel_clock, 1,
>> +				 sensor->pixel_clock);
> 
> I personally prefer:
> 	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
> 				 sensor->pixel_clock,
> 				 sensor->pixel_clock, 1,
> 				 sensor->pixel_clock);

In fact this is my .clang-format who formatted it that way.
Your proposal is more elegant to the eye.

> 
> up to you :)
> 
>> +	if (ctrl)
>> +		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
>> +
>> +	sensor->ae_ctrl = v4l2_ctrl_new_std_menu(hdl, ops,
>> +						 V4L2_CID_EXPOSURE_AUTO,
>> +						 V4L2_EXPOSURE_MANUAL, 0,
>> +						 V4L2_EXPOSURE_AUTO);
>> +
>> +	sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK, 0,
>> +						 GENMASK(2, 0), 0, 0);
>> +
>> +	sensor->ae_bias_ctrl =
>> +		v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
>> +				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) - 1,
>> +				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) / 2,
>> +				       vd56g3_ev_bias_qmenu);
>> +
>> +	/*
>> +	 * Analog gain [1, 8] is computed with the following logic :
>> +	 * 32/(32 - again_reg), with again_reg in the range [0:28]
>> +	 * Digital gain [1.00, 8.00] is coded as a Fixed Point 5.8
>> +	 */
>> +	sensor->again_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
>> +					       0, 28, 1, 0);
>> +	sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
>> +					       0x100, 0x800, 1, 0x100);
>> +
>> +	/*
>> +	 * Set the exposure, horizontal and vertical blanking ctrls
>> +	 * to hardcoded values, they will be updated in vd56g3_update_controls.
>> +	 * Exposure being in an auto-cluster, set a significant value here.
>> +	 */
>> +	sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
>> +					      VD56G3_EXPOSURE_DEFAULT,
>> +					      VD56G3_EXPOSURE_DEFAULT, 1,
>> +					      VD56G3_EXPOSURE_DEFAULT);
>> +	sensor->hblank_ctrl =
>> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 1, 1, 1, 1);
>> +	if (sensor->hblank_ctrl)
>> +		sensor->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
>> +	sensor->vblank_ctrl =
>> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, 1, 1, 1, 1);
> 
> Doubt on my side:
> 
> Here you flag hblank as read only. What about vblank?
> Thanks in advance.

Yes hblank is read only while vblank is writable.
Vblank is adjustable to allow different framerates.

> 
>> +
>> +	/* Additional control based on device tree properties */
>> +	if (sensor->ext_leds_mask)
>> +		sensor->led_ctrl =
>> +			v4l2_ctrl_new_std_menu(hdl, ops,
>> +					       V4L2_CID_FLASH_LED_MODE,
>> +					       V4L2_FLASH_LED_MODE_FLASH, 0,
>> +					       V4L2_FLASH_LED_MODE_NONE);
>> +
>> +	if (hdl->error) {
>> +		ret = hdl->error;
>> +		goto free_ctrls;
>> +	}
>> +
>> +	v4l2_ctrl_cluster(2, &sensor->hflip_ctrl);
>> +	v4l2_ctrl_auto_cluster(4, &sensor->ae_ctrl, V4L2_EXPOSURE_MANUAL, true);
>> +
>> +	sensor->sd.ctrl_handler = hdl;
>> +	return 0;
>> +
>> +free_ctrls:
>> +	v4l2_ctrl_handler_free(hdl);
>> +	return ret;
>> +}
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Videos ops
>> + */
>> +
>> +static int vd56g3_stream_on(struct vd56g3 *sensor,
>> +			    struct v4l2_subdev_state *state)
>> +{
>> +	const struct v4l2_mbus_framefmt *format =
>> +		v4l2_subdev_state_get_format(state, 0);
>> +	const struct v4l2_rect *crop =
>> +		v4l2_subdev_state_get_crop(state, 0);
>> +	int ret = 0;
>> +	unsigned int csi_mbps = ((sensor->nb_of_lane == 2) ?
>> +					 VD56G3_LINK_FREQ_DEF_2LANES :
>> +					 VD56G3_LINK_FREQ_DEF_1LANE) * 2 / MEGA;
>> +	unsigned int binning;
>> +
>> +	/* configure clocks */
>> +	cci_write(sensor->regmap, VD56G3_REG_EXT_CLOCK, sensor->ext_clock,
>> +		  &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_CLK_PLL_PREDIV, sensor->pll_prediv,
>> +		  &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_CLK_SYS_PLL_MULT, sensor->pll_mult,
>> +		  &ret);
>> +
>> +	/* configure output */
>> +	cci_write(sensor->regmap, VD56G3_REG_FORMAT_CTRL,
>> +		  vd56g3_get_bpp(format->code), &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OIF_CSI_BITRATE, csi_mbps, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OIF_IMG_CTRL,
>> +		  vd56g3_get_datatype(format->code), &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_ISL_ENABLE, 0, &ret);
>> +
>> +	/* configure binning mode */
>> +	switch (crop->width / format->width) {
>> +	case 1:
>> +	default:
>> +		binning = READOUT_NORMAL;
>> +		break;
>> +	case 2:
>> +		binning = READOUT_DIGITAL_BINNING_X2;
>> +		break;
>> +	}
>> +	cci_write(sensor->regmap, VD56G3_REG_READOUT_CTRL, binning, &ret);
>> +
>> +	/* configure ROIs */
>> +	cci_write(sensor->regmap, VD56G3_REG_Y_START, crop->top, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_Y_END,
>> +		  crop->top + crop->height - 1, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_START, crop->left, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_END,
>> +		  crop->left + crop->width - 1, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_START, 0, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_END, crop->height - 1,
>> +		  &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_H, crop->left, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_H,
>> +		  crop->left + crop->width - 1, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_V, 0, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_V, crop->height - 1,
>> +		  &ret);
>> +	if (ret)
>> +		return ret;
>> +
>> +	/* Setup default GPIO values; could be overridden by V4L2 ctrl setup */
>> +	ret = vd56g3_write_gpiox(sensor, GENMASK(VD56G3_NB_GPIOS - 1, 0));
>> +	if (ret)
>> +		return ret;
>> +
>> +	/* Apply settings from V4L2 ctrls */
>> +	ret = __v4l2_ctrl_handler_setup(&sensor->ctrl_handler);
>> +	if (ret)
>> +		return ret;
>> +
>> +	/* start streaming */
>> +	cci_write(sensor->regmap, VD56G3_REG_STBY, VD56G3_CMD_START_STREAM,
>> +		  &ret);
>> +	vd56g3_poll_reg(sensor, VD56G3_REG_STBY, VD56G3_CMD_ACK, &ret);
>> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_STREAMING, &ret);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_stream_off(struct vd56g3 *sensor)
>> +{
>> +	int ret = 0;
>> +
>> +	/* Retrieve Expo cluster to enable coldstart of AE */
>> +	ret = vd56g3_read_expo_cluster(sensor, true);
>> +
>> +	cci_write(sensor->regmap, VD56G3_REG_STREAMING, VD56G3_CMD_STOP_STREAM,
>> +		  &ret);
>> +	vd56g3_poll_reg(sensor, VD56G3_REG_STREAMING, VD56G3_CMD_ACK, &ret);
>> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_s_stream(struct v4l2_subdev *sd, int enable)
>> +{
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
>> +	int ret = 0;
>> +
>> +	struct v4l2_subdev_state *state =
>> +		v4l2_subdev_lock_and_get_active_state(sd);
>> +
>> +	if (enable) {
>> +		ret = pm_runtime_resume_and_get(&client->dev);
>> +		if (ret < 0)
>> +			goto unlock;
>> +		ret = vd56g3_stream_on(sensor, state);
>> +		if (ret) {
>> +			dev_err(&client->dev, "Failed to start streaming\n");
>> +			pm_runtime_put_sync(&client->dev);
>> +		}
>> +	} else {
>> +		vd56g3_stream_off(sensor);
>> +		pm_runtime_mark_last_busy(&client->dev);
>> +		pm_runtime_put_autosuspend(&client->dev);
>> +	}
>> +
>> +unlock:
>> +	v4l2_subdev_unlock_state(state);
>> +	if (!ret) {
>> +		sensor->streaming = enable;
>> +		v4l2_ctrl_grab(sensor->hflip_ctrl, enable);
>> +		v4l2_ctrl_grab(sensor->vflip_ctrl, enable);
>> +		v4l2_ctrl_grab(sensor->patgen_ctrl, enable);
>> +	}
>> +
>> +	return ret;
>> +}
>> +
>> +static const struct v4l2_subdev_video_ops vd56g3_video_ops = {
>> +	.s_stream = vd56g3_s_stream,
>> +};
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Pad ops
>> + */
>> +
>> +/* Media bus code is dependent of :
>> + *      - 8bits or 10bits output
>> + *      - variant : Mono or RGB
>> + *      - H/V flips parameters in case of RGB
>> + */
>> +static u32 vd56g3_get_mbus_code(struct vd56g3 *sensor, u32 code)
>> +{
>> +	unsigned int i_bpp;
>> +	unsigned int j;
>> +
>> +	for (i_bpp = 0; i_bpp < ARRAY_SIZE(vd56g3_mbus_codes); i_bpp++) {
>> +		for (j = 0; j < ARRAY_SIZE(vd56g3_mbus_codes[i_bpp]); j++) {
>> +			if (vd56g3_mbus_codes[i_bpp][j] == code)
>> +				goto endloops;
>> +		}
>> +	}
>> +
>> +endloops:
>> +	if (i_bpp >= ARRAY_SIZE(vd56g3_mbus_codes))
>> +		i_bpp = 0;
>> +
>> +	if (sensor->is_mono)
>> +		j = 0;
>> +	else
>> +		j = 1 + (sensor->hflip_ctrl->val ? 1 : 0) +
>> +		    (sensor->vflip_ctrl->val ? 2 : 0);
>> +
>> +	return vd56g3_mbus_codes[i_bpp][j];
>> +}
>> +
>> +static int vd56g3_enum_mbus_code(struct v4l2_subdev *sd,
>> +				 struct v4l2_subdev_state *sd_state,
>> +				 struct v4l2_subdev_mbus_code_enum *code)
>> +{
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +
>> +	if (code->index >= ARRAY_SIZE(vd56g3_mbus_codes))
>> +		return -EINVAL;
>> +
>> +	code->code =
>> +		vd56g3_get_mbus_code(sensor, vd56g3_mbus_codes[code->index][0]);
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_enum_frame_size(struct v4l2_subdev *sd,
>> +				  struct v4l2_subdev_state *sd_state,
>> +				  struct v4l2_subdev_frame_size_enum *fse)
>> +{
>> +	if (fse->index >= ARRAY_SIZE(vd56g3_supported_modes))
>> +		return -EINVAL;
>> +
>> +	fse->min_width = vd56g3_supported_modes[fse->index].width;
>> +	fse->max_width = fse->min_width;
>> +	fse->min_height = vd56g3_supported_modes[fse->index].height;
>> +	fse->max_height = fse->min_height;
>> +
>> +	return 0;
>> +}
>> +
>> +static void vd56g3_update_img_pad_format(struct vd56g3 *sensor,
>> +					 const struct vd56g3_mode *mode,
>> +					 u32 mbus_code,
>> +					 struct v4l2_mbus_framefmt *mbus_fmt)
>> +{
>> +	mbus_fmt->width = mode->width;
>> +	mbus_fmt->height = mode->height;
>> +	mbus_fmt->code = vd56g3_get_mbus_code(sensor, mbus_code);
>> +	mbus_fmt->colorspace = V4L2_COLORSPACE_RAW;
>> +	mbus_fmt->field = V4L2_FIELD_NONE;
>> +	mbus_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
>> +	mbus_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
>> +	mbus_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
>> +}
>> +
>> +static int vd56g3_set_pad_fmt(struct v4l2_subdev *sd,
>> +			      struct v4l2_subdev_state *sd_state,
>> +			      struct v4l2_subdev_format *sd_fmt)
>> +{
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +	const struct vd56g3_mode *new_mode;
>> +	struct v4l2_rect pad_crop;
>> +	unsigned int binning;
>> +
>> +	if (sensor->streaming)
>> +		return -EBUSY;
>> +
>> +	new_mode = v4l2_find_nearest_size(vd56g3_supported_modes,
>> +					  ARRAY_SIZE(vd56g3_supported_modes),
>> +					  width, height, sd_fmt->format.width,
>> +					  sd_fmt->format.height);
>> +
>> +	vd56g3_update_img_pad_format(sensor, new_mode, sd_fmt->format.code,
>> +				     &sd_fmt->format);
>> +
>> +	/* Compute crop rectangle (maximized via binning) */
>> +	binning = min(VD56G3_NATIVE_WIDTH / sd_fmt->format.width,
>> +		      VD56G3_NATIVE_HEIGHT / sd_fmt->format.height);
>> +	binning = min(binning, 2U);
>> +	pad_crop.width = sd_fmt->format.width * binning;
>> +	pad_crop.height = sd_fmt->format.height * binning;
>> +	pad_crop.left = (VD56G3_NATIVE_WIDTH - pad_crop.width) / 2;
>> +	pad_crop.top = (VD56G3_NATIVE_HEIGHT - pad_crop.height) / 2;
>> +
>> +	/* Update active state's format and crop */
>> +	*v4l2_subdev_state_get_format(sd_state, sd_fmt->pad) = sd_fmt->format;
>> +	*v4l2_subdev_state_get_crop(sd_state, sd_fmt->pad) = pad_crop;
>> +
>> +	if (sd_fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
>> +		vd56g3_update_controls(sensor);
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_get_selection(struct v4l2_subdev *sd,
>> +				struct v4l2_subdev_state *sd_state,
>> +				struct v4l2_subdev_selection *sel)
>> +{
>> +	switch (sel->target) {
>> +	case V4L2_SEL_TGT_CROP:
>> +		sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
>> +		break;
>> +	case V4L2_SEL_TGT_NATIVE_SIZE:
>> +	case V4L2_SEL_TGT_CROP_DEFAULT:
>> +	case V4L2_SEL_TGT_CROP_BOUNDS:
>> +		sel->r.top = 0;
>> +		sel->r.left = 0;
>> +		sel->r.width = VD56G3_NATIVE_WIDTH;
>> +		sel->r.height = VD56G3_NATIVE_HEIGHT;
>> +		break;
>> +	default:
>> +		return -EINVAL;
>> +	}
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_init_state(struct v4l2_subdev *sd,
>> +			     struct v4l2_subdev_state *sd_state)
>> +{
>> +	unsigned int def_mode = VD56G3_DEFAULT_MODE;
>> +	struct v4l2_subdev_format fmt = {
>> +		.which = V4L2_SUBDEV_FORMAT_TRY,
>> +		.pad = 0,
>> +		.format = {
>> +			.code = vd56g3_mbus_codes[0][0],
>> +			.width = vd56g3_supported_modes[def_mode].width,
>> +			.height = vd56g3_supported_modes[def_mode].height,
>> +		},
>> +	};
>> +
>> +	vd56g3_set_pad_fmt(sd, sd_state, &fmt);
>> +	return 0;
>> +}
>> +
>> +static const struct v4l2_subdev_core_ops vd56g3_core_ops = {
>> +	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
>> +	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
>> +};
>> +
>> +static const struct v4l2_subdev_pad_ops vd56g3_pad_ops = {
>> +	.enum_mbus_code = vd56g3_enum_mbus_code,
>> +	.enum_frame_size = vd56g3_enum_frame_size,
>> +	.get_fmt = v4l2_subdev_get_fmt,
>> +	.set_fmt = vd56g3_set_pad_fmt,
>> +	.get_selection = vd56g3_get_selection,
>> +};
>> +
>> +static const struct v4l2_subdev_ops vd56g3_subdev_ops = {
>> +	.core = &vd56g3_core_ops,
>> +	.video = &vd56g3_video_ops,
>> +	.pad = &vd56g3_pad_ops,
>> +};
>> +
>> +static const struct media_entity_operations vd56g3_subdev_entity_ops = {
>> +	.link_validate = v4l2_subdev_link_validate,
>> +};
>> +
>> +static const struct v4l2_subdev_internal_ops vd56g3_internal_ops = {
>> +	.init_state = vd56g3_init_state,
>> +};
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Power management
>> + */
>> +
>> +static int vd56g3_boot(struct vd56g3 *sensor)
>> +{
>> +	int ret = 0;
>> +
>> +	cci_write(sensor->regmap, VD56G3_REG_BOOT, VD56G3_CMD_BOOT, &ret);
>> +	vd56g3_poll_reg(sensor, VD56G3_REG_BOOT, VD56G3_CMD_ACK, &ret);
>> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_power_on(struct vd56g3 *sensor)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	int ret;
>> +
>> +	ret = regulator_bulk_enable(VD56G3_NUM_SUPPLIES, sensor->supplies);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Failed to enable regulators %d", ret);
>> +		return ret;
>> +	}
>> +
>> +	ret = clk_prepare_enable(sensor->xclk);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Failed to enable clock %d", ret);
>> +		goto disable_reg;
>> +	}
>> +
>> +	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
>> +	ret = vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_READY_TO_BOOT, NULL);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Sensor reset failed %d\n", ret);
>> +		goto disable_clock;
>> +	}
>> +
>> +	return 0;
>> +
>> +disable_clock:
>> +	clk_disable_unprepare(sensor->xclk);
>> +	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
>> +disable_reg:
>> +	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_power_off(struct vd56g3 *sensor)
>> +{
>> +	clk_disable_unprepare(sensor->xclk);
>> +	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
>> +	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_runtime_resume(struct device *dev)
>> +{
>> +	struct v4l2_subdev *sd = dev_get_drvdata(dev);
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	int ret;
>> +
>> +	ret = vd56g3_power_on(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Failed to power on %d", ret);
>> +		return ret;
>> +	}
>> +
>> +	ret = vd56g3_boot(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "sensor boot failed %d", ret);
>> +		return ret;
>> +	}
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_runtime_suspend(struct device *dev)
>> +{
>> +	struct v4l2_subdev *sd = dev_get_drvdata(dev);
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +
>> +	return vd56g3_power_off(sensor);
>> +}
>> +
>> +static const struct dev_pm_ops vd56g3_pm_ops = {
>> +	SET_RUNTIME_PM_OPS(vd56g3_runtime_suspend, vd56g3_runtime_resume, NULL)
>> +};
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Probe and initialization
>> + */
>> +
>> +static int vd56g3_check_csi_conf(struct vd56g3 *sensor,
>> +				 struct fwnode_handle *endpoint)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
>> +	u32 phy_data_lanes[VD56G3_MAX_CSI_DATA_LANES] = { ~0, ~0 };
>> +	int n_lanes;
> 
> use u8 for n_lanes
> 

Ok, applied for V2

>> +	int frequency;
> 
> use u64

Ok, applied for V2

> 
>> +	int p, l;
>> +	int ret = 0;
>> +
>> +	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
>> +	if (ret)
>> +		return -EINVAL;
>> +
>> +	/* Check lanes number */
>> +	n_lanes = ep.bus.mipi_csi2.num_data_lanes;
>> +	if (n_lanes != 1 && n_lanes != 2) {
>> +		dev_err(&client->dev, "Invalid data lane number %d\n", n_lanes);
>> +		ret = -EINVAL;
>> +		goto done;
>> +	}
>> +	sensor->nb_of_lane = n_lanes;
>> +
>> +	/* Clock lane must be first */
>> +	if (ep.bus.mipi_csi2.clock_lane != 0) {
>> +		dev_err(&client->dev, "Clk lane must be mapped to lane 0\n");
>> +		ret = -EINVAL;
>> +		goto done;
>> +	}
>> +
>> +	/* Prepare Output Interface conf based on lane settings
>> +	 * logical to physical lane conversion (+ pad remaining slots)
>> +	 */
>> +	for (l = 0; l < n_lanes; l++)
>> +		phy_data_lanes[ep.bus.mipi_csi2.data_lanes[l] - 1] = l;
>> +	for (p = 0; p < VD56G3_MAX_CSI_DATA_LANES; p++) {
>> +		if (phy_data_lanes[p] != ~0)
>> +			continue;
>> +		phy_data_lanes[p] = l;
>> +		l++;
>> +	}
>> +	sensor->oif_ctrl = n_lanes |
>> +			   (ep.bus.mipi_csi2.lane_polarities[0] << 3) |
>> +			   ((phy_data_lanes[0]) << 4) |
>> +			   (ep.bus.mipi_csi2.lane_polarities[1] << 6) |
>> +			   ((phy_data_lanes[1]) << 7) |
>> +			   (ep.bus.mipi_csi2.lane_polarities[2] << 9);
>> +
>> +	/* Check link frequency */
>> +	if (!ep.nr_of_link_frequencies) {
>> +		dev_err(&client->dev, "link-frequency not found in DT\n");
>> +		ret = -EINVAL;
>> +		goto done;
>> +	}
>> +	frequency = (n_lanes == 2) ? VD56G3_LINK_FREQ_DEF_2LANES :
>> +				     VD56G3_LINK_FREQ_DEF_1LANE;
>> +	if (ep.nr_of_link_frequencies != 1 ||
>> +	    ep.link_frequencies[0] != frequency) {
>> +		dev_err(&client->dev, "Link frequency not supported: %lld\n",
>> +			ep.link_frequencies[0]);
>> +		ret = -EINVAL;
>> +		goto done;
>> +	}
>> +
>> +done:
>> +	v4l2_fwnode_endpoint_free(&ep);
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_parse_dt_gpios_array(struct vd56g3 *sensor, char *prop_name,
>> +				       u32 *array, int *nb)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	struct device_node *np = client->dev.of_node;
>> +	unsigned int i;
>> +
>> +	*nb = of_property_read_variable_u32_array(np, prop_name, array, 0,
>> +						  VD56G3_NB_GPIOS);
>> +
>> +	if (*nb == -EINVAL) {
>> +		*nb = 0;
>> +		return *nb;
>> +	} else if (*nb < 0) {
>> +		dev_err(&client->dev, "Failed to read %s prop\n", prop_name);
>> +		return *nb;
>> +	}
>> +
>> +	for (i = 0; i < *nb; i++) {
>> +		if (array[i] >= VD56G3_NB_GPIOS) {
>> +			dev_err(&client->dev, "Invalid GPIO : %d\n", array[i]);
>> +			return -EINVAL;
>> +		}
>> +	}
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_parse_dt_gpios(struct vd56g3 *sensor)
>> +{
>> +	u32 led_gpios[VD56G3_NB_GPIOS];
>> +	int nb_gpios_leds;
>> +	int ret;
>> +	unsigned int i;
>> +
>> +	/* Initialize GPIOs to default */
>> +	for (i = 0; i < VD56G3_NB_GPIOS; i++)
>> +		sensor->gpios[i] = VD56G3_GPIOX_GPIO_IN;
>> +	sensor->ext_leds_mask = 0;
>> +
>> +	/* Take into account optional 'st,leds' output for GPIOs */
>> +	ret = vd56g3_parse_dt_gpios_array(sensor, "st,leds", led_gpios,
>> +					  &nb_gpios_leds);
>> +	if (ret)
>> +		return ret;
>> +	for (i = 0; i < nb_gpios_leds; i++) {
>> +		sensor->gpios[led_gpios[i]] = VD56G3_GPIOX_STROBE_MODE;
>> +		set_bit(led_gpios[i], &sensor->ext_leds_mask);
>> +	}
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_parse_dt(struct vd56g3 *sensor)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	struct device *dev = &client->dev;
>> +	struct fwnode_handle *endpoint;
>> +	int ret;
>> +
>> +	endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0, 0);
>> +	if (!endpoint) {
>> +		dev_err(dev, "endpoint node not found\n");
>> +		return -EINVAL;
>> +	}
>> +
>> +	ret = vd56g3_check_csi_conf(sensor, endpoint);
>> +	fwnode_handle_put(endpoint);
>> +	if (ret)
>> +		return ret;
>> +
>> +	ret = vd56g3_parse_dt_gpios(sensor);
>> +	if (ret)
>> +		return ret;
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_get_regulators(struct vd56g3 *sensor)
>> +{
>> +	int i;
>> +
>> +	for (i = 0; i < VD56G3_NUM_SUPPLIES; i++)
>> +		sensor->supplies[i].supply = vd56g3_supply_names[i];
>> +
>> +	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
>> +				       VD56G3_NUM_SUPPLIES, sensor->supplies);
>> +}
>> +
>> +static int vd56g3_prepare_clock_tree(struct vd56g3 *sensor)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	const unsigned int predivs[] = { 1, 2, 4 };
>> +	u32 pll_out;
>> +	int i;
>> +
>> +	/* External clock must be in [6Mhz-27Mhz] */
>> +	if (sensor->ext_clock < 6 * HZ_PER_MHZ ||
>> +	    sensor->ext_clock > 27 * HZ_PER_MHZ) {
>> +		dev_err(&client->dev,
>> +			"Only 6Mhz-27Mhz clock range supported. Provided %lu MHz\n",
>> +			sensor->ext_clock / HZ_PER_MHZ);
>> +		return -EINVAL;
>> +	}
>> +
>> +	/* PLL input should be in [6Mhz-12Mhz[ */
>> +	for (i = 0; i < ARRAY_SIZE(predivs); i++) {
>> +		sensor->pll_prediv = predivs[i];
>> +		if (sensor->ext_clock / sensor->pll_prediv < 12 * HZ_PER_MHZ)
>> +			break;
>> +	}
>> +
>> +	/* PLL output clock must be as close as possible to 804Mhz */
>> +	sensor->pll_mult = (VD56G3_TARGET_PLL * sensor->pll_prediv +
>> +			    sensor->ext_clock / 2) /
>> +			   sensor->ext_clock;
>> +	pll_out = sensor->ext_clock * sensor->pll_mult / sensor->pll_prediv;
>> +
>> +	/* Target Pixel Clock for standard 10bit ADC mode : 160.8Mhz */
>> +	sensor->pixel_clock = pll_out / VD56G3_VT_CLOCK_DIV;
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_detect(struct vd56g3 *sensor)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	struct device *dev = &client->dev;
>> +	unsigned int model;
>> +	u64 model_id = 0;
>> +	u64 device_revision = 0;
>> +	u64 optical_revision = 0;
>> +	int ret = 0;
>> +
>> +	model = (uintptr_t)device_get_match_data(dev);
>> +
>> +	cci_read(sensor->regmap, VD56G3_REG_MODEL_ID, &model_id, &ret);
>> +	if (ret)
>> +		return ret;
>> +
>> +	if (model_id != VD56G3_MODEL_ID) {
>> +		dev_warn(&client->dev, "Unsupported sensor id %x",
>> +			 (u16)model_id);
>> +		return -ENODEV;
>> +	}
>> +
>> +	cci_read(sensor->regmap, VD56G3_REG_REVISION, &device_revision, &ret);
>> +	if (ret)
>> +		return ret;
>> +
>> +	if ((device_revision >> 8) != 0x31) {
> 
> What about create a define that explain 0x31 number to avoid magic
> number?
> 
> #define VD56G3_DEV_CUTV = 0x31?

Yes more readable. Applied for V2

> 
>> +		dev_warn(&client->dev, "Unsupported Cut version %x",
>> +			 (u16)device_revision);
>> +		return -ENODEV;
>> +	}
>> +
>> +	cci_read(sensor->regmap, VD56G3_REG_OPTICAL_REVISION, &optical_revision,
>> +		 &ret);
>> +	if (ret)
>> +		return ret;
>> +
>> +	sensor->is_mono =
>> +		((optical_revision & 1) == VD56G3_OPTICAL_REVISION_MONO);
>> +	if ((sensor->is_mono && model == VD56G3_MODEL_VD66GY) ||
>> +	    (!sensor->is_mono && model == VD56G3_MODEL_VD56G3)) {
>> +		dev_warn(&client->dev,
>> +			 "Found %s sensor, while %s model is defined in DT",
>> +			 (sensor->is_mono) ? "Mono" : "Bayer",
>> +			 (model == VD56G3_MODEL_VD56G3) ? "vd56g3" : "vd66gy");
>> +		return -ENODEV;
>> +	}
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_subdev_init(struct vd56g3 *sensor)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	int ret;
>> +
>> +	/* Init sub device */
>> +	v4l2_i2c_subdev_init(&sensor->sd, client, &vd56g3_subdev_ops);
>> +	sensor->sd.internal_ops = &vd56g3_internal_ops;
>> +	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
>> +			    V4L2_SUBDEV_FL_HAS_EVENTS;
>> +	sensor->sd.entity.ops = &vd56g3_subdev_entity_ops;
>> +
>> +	/* Init source pad */
>> +	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
>> +	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
>> +	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Failed to init media entity : %d", ret);
>> +		return ret;
>> +	}
>> +
>> +	/* Init controls */
>> +	ret = vd56g3_init_controls(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Controls initialization failed %d", ret);
>> +		goto err_media;
>> +	}
>> +
>> +	/* Init vd56g3 struct : default resolution + raw8 */
>> +	sensor->streaming = false;
>> +	sensor->sd.state_lock = sensor->ctrl_handler.lock;
>> +	ret = v4l2_subdev_init_finalize(&sensor->sd);
>> +	if (ret) {
>> +		dev_err(&client->dev, "subdev init error: %d", ret);
>> +		goto err_ctrls;
>> +	}
>> +
>> +	vd56g3_update_controls(sensor);
>> +
>> +	return 0;
>> +
>> +err_ctrls:
>> +	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
>> +
>> +err_media:
>> +	media_entity_cleanup(&sensor->sd.entity);
>> +	return ret;
>> +}
>> +
>> +static void vd56g3_subdev_cleanup(struct vd56g3 *sensor)
>> +{
>> +	v4l2_async_unregister_subdev(&sensor->sd);
>> +	v4l2_subdev_cleanup(&sensor->sd);
>> +	media_entity_cleanup(&sensor->sd.entity);
>> +	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
>> +}
>> +
>> +static int vd56g3_probe(struct i2c_client *client)
>> +{
>> +	struct device *dev = &client->dev;
>> +	struct vd56g3 *sensor;
>> +	int ret;
>> +
>> +	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
>> +	if (!sensor)
>> +		return -ENOMEM;
>> +
>> +	sensor->i2c_client = client;
>> +
>> +	ret = vd56g3_parse_dt(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Failed to parse Device Tree : %d", ret);
> 
> Why not dev_err_probe here?

Sure, thanks.

> 
>> +		return ret;
>> +	}
>> +
>> +	/* Get (and check) resources : power regs, ext clock, reset gpio */
>> +	ret = vd56g3_get_regulators(sensor);
>> +	if (ret)
>> +		return dev_err_probe(dev, ret, "Failed to get regulators.");
>> +
>> +	sensor->xclk = devm_clk_get(dev, NULL);
>> +	if (IS_ERR(sensor->xclk))
>> +		return dev_err_probe(dev, PTR_ERR(sensor->xclk),
>> +				     "Failed to get xclk.");
>> +	sensor->ext_clock = clk_get_rate(sensor->xclk);
> 
> What about use of xclk_freq?
> I think this is more understandable
> 
> sensor->xclk
> sensor->xclk_freq
> 
> better I think :)

Yes, why not.
Applied for V2.

> 
>> +	ret = vd56g3_prepare_clock_tree(sensor);
>> +	if (ret)
>> +		return ret;
>> +
>> +	sensor->reset_gpio =
>> +		devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
>> +	if (IS_ERR(sensor->reset_gpio))
>> +		return dev_err_probe(dev, PTR_ERR(sensor->reset_gpio),
>> +				     "Failed to get reset gpio.");
>> +
>> +	sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
>> +	if (IS_ERR(sensor->regmap))
>> +		return dev_err_probe(dev, PTR_ERR(sensor->regmap),
>> +				     "Failed to init regmap.");
>> +
>> +	/* Power ON */
>> +	ret = vd56g3_power_on(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Sensor power on failed : %d", ret);
> 
> dev_err_probe?

Applied for V2.

> 
>> +		return ret;
>> +	}
>> +
>> +	/* Enable PM runtime with autosuspend (sensor being ON, set active) */
>> +	pm_runtime_set_active(dev);
>> +	pm_runtime_get_noresume(dev);
>> +	pm_runtime_enable(dev);
>> +	pm_runtime_set_autosuspend_delay(dev, 1000);
>> +	pm_runtime_use_autosuspend(dev);
>> +
>> +	/* Check HW model/version */
>> +	ret = vd56g3_boot(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Sensor boot failed : %d", ret);
>> +		goto err_power_off;
>> +	}
>> +
>> +	ret = vd56g3_detect(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Sensor detect failed : %d", ret);
>> +		goto err_power_off;
>> +	}
>> +
>> +	/* Initialize, then register V4L2 subdev */
>> +	ret = vd56g3_subdev_init(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "V4l2 init failed : %d", ret);
>> +		goto err_power_off;
>> +	}
>> +
>> +	ret = v4l2_async_register_subdev(&sensor->sd);
>> +	if (ret) {
>> +		dev_err(&client->dev, "async subdev register failed %d", ret);
>> +		goto err_subdev;
>> +	}
>> +
>> +	/* Sensor could now be powered off (after the autosuspend delay) */
>> +	pm_runtime_mark_last_busy(dev);
>> +	pm_runtime_put_autosuspend(dev);
>> +
>> +	dev_dbg(&client->dev, "Successfully probe %s sensor",
>> +		(sensor->is_mono) ? "vd56g3" : "vd66gy");
>> +
>> +	return 0;
>> +
>> +err_subdev:
>> +	vd56g3_subdev_cleanup(sensor);
>> +err_power_off:
>> +	pm_runtime_disable(dev);
>> +	pm_runtime_put_noidle(dev);
>> +	vd56g3_power_off(sensor);
>> +	return ret;
>> +}
>> +
>> +static void vd56g3_remove(struct i2c_client *client)
>> +{
>> +	struct v4l2_subdev *sd = i2c_get_clientdata(client);
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +
>> +	vd56g3_subdev_cleanup(sensor);
>> +
>> +	pm_runtime_disable(&client->dev);
>> +	if (!pm_runtime_status_suspended(&client->dev))
>> +		vd56g3_power_off(sensor);
>> +	pm_runtime_set_suspended(&client->dev);
>> +}
>> +
>> +static const struct of_device_id vd56g3_dt_ids[] = {
>> +	{ .compatible = "st,st-vd56g3", .data = (void *)VD56G3_MODEL_VD56G3 },
>> +	{ .compatible = "st,st-vd66gy", .data = (void *)VD56G3_MODEL_VD66GY },
>> +	{ /* sentinel */ }
>> +};
>> +MODULE_DEVICE_TABLE(of, vd56g3_dt_ids);
>> +
>> +static struct i2c_driver vd56g3_i2c_driver = {
>> +	.driver = {
>> +		.name  = "st-vd56g3",
>> +		.of_match_table = vd56g3_dt_ids,
>> +		.pm = &vd56g3_pm_ops,
>> +	},
>> +	.probe = vd56g3_probe,
>> +	.remove = vd56g3_remove,
>> +};
>> +
>> +module_i2c_driver(vd56g3_i2c_driver);
>> +
>> +MODULE_AUTHOR("Benjamin Mugnier <[e-mail]>");
>> +MODULE_AUTHOR("Mickael Guene <[e-mail]>");
>> +MODULE_AUTHOR("Sylvain Petinot <[e-mail]>");
>> +MODULE_DESCRIPTION("ST VD56G3 sensor driver");
>> +MODULE_LICENSE("GPL");
>> -- 
>> 2.17.1
>>
>>
> 
> Hope this help :)

Sure, thanks !

> 
> Thanks & Regards,
> Tommaso
--
Sylvain

Sylvain Petinot May 3, 2024, 10:10 a.m. UTC | #5

Hi Sakari,

Thanks for this first review.

On 4/24/2024 11:12 PM, Sakari Ailus wrote:
> Hi Sylvain,
> 
> Some quick comments below...
> 
> On Wed, Apr 17, 2024 at 03:34:53PM +0200, Sylvain Petinot wrote:
>> Add V4L2 sub-device driver for STMicroelectronics VD56G3 camera sensor.
>> This is a 1.5 M pixel global shutter image sensor with an active array
>> size of 1124 x 1364 (portrait orientation).
>>
>> The driver supports Mono (VD56G3) and Color (VD66GY) variants.
>>
>> Signed-off-by: Sylvain Petinot <[e-mail]>
>> ---
>>  drivers/media/i2c/Kconfig     |   11 +
>>  drivers/media/i2c/Makefile    |    1 +
>>  drivers/media/i2c/st-vd56g3.c | 1619 +++++++++++++++++++++++++++++++++
>>  3 files changed, 1631 insertions(+)
>>  create mode 100644 drivers/media/i2c/st-vd56g3.c
>>
>> diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
>> index 56f276b920ab..a2d7271eef88 100644
>> --- a/drivers/media/i2c/Kconfig
>> +++ b/drivers/media/i2c/Kconfig
>> @@ -657,6 +657,17 @@ config VIDEO_S5K6A3
>>  	  This is a V4L2 sensor driver for Samsung S5K6A3 raw
>>  	  camera sensor.
>>  
>> +config VIDEO_ST_VD56G3
>> +	tristate "ST VD56G3 sensor support"
>> +	select V4L2_CCI_I2C
>> +	depends on OF && GPIOLIB
>> +	help
>> +	  This is a Video4Linux2 sensor driver for the ST VD56G3
>> +	  camera sensor.
>> +
>> +	  To compile this driver as a module, choose M here: the
>> +	  module will be called st-vd56g3.
>> +
>>  config VIDEO_ST_VGXY61
>>  	tristate "ST VGXY61 sensor support"
>>  	select V4L2_CCI_I2C
>> diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
>> index dfbe6448b549..b469cf0f8113 100644
>> --- a/drivers/media/i2c/Makefile
>> +++ b/drivers/media/i2c/Makefile
>> @@ -124,6 +124,7 @@ obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
>>  obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
>>  obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
>>  obj-$(CONFIG_VIDEO_ST_MIPID02) += st-mipid02.o
>> +obj-$(CONFIG_VIDEO_ST_VD56G3) += st-vd56g3.o
>>  obj-$(CONFIG_VIDEO_ST_VGXY61) += st-vgxy61.o
>>  obj-$(CONFIG_VIDEO_TC358743) += tc358743.o
>>  obj-$(CONFIG_VIDEO_TC358746) += tc358746.o
>> diff --git a/drivers/media/i2c/st-vd56g3.c b/drivers/media/i2c/st-vd56g3.c
>> new file mode 100644
>> index 000000000000..eff819941456
>> --- /dev/null
>> +++ b/drivers/media/i2c/st-vd56g3.c
>> @@ -0,0 +1,1619 @@
>> +// SPDX-License-Identifier: GPL-2.0
>> +/*
>> + * A V4L2 driver for ST VD56G3 (Mono) and VD66GY (RGB) global shutter cameras.
>> + * Copyright (C) 2024, STMicroelectronics SA
>> + */
>> +
>> +#include <linux/clk.h>
>> +#include <linux/delay.h>
>> +#include <linux/gpio/consumer.h>
>> +#include <linux/i2c.h>
>> +#include <linux/iopoll.h>
>> +#include <linux/module.h>
>> +#include <linux/pm_runtime.h>
>> +#include <linux/regmap.h>
>> +#include <linux/regulator/consumer.h>
>> +#include <linux/units.h>
>> +
>> +#include <asm/unaligned.h>
>> +
>> +#include <media/mipi-csi2.h>
>> +#include <media/v4l2-async.h>
>> +#include <media/v4l2-cci.h>
>> +#include <media/v4l2-ctrls.h>
>> +#include <media/v4l2-device.h>
>> +#include <media/v4l2-event.h>
>> +#include <media/v4l2-fwnode.h>
>> +#include <media/v4l2-subdev.h>
>> +
>> +/* Register Map */
>> +#define VD56G3_REG_MODEL_ID				CCI_REG16_LE(0x0000)
>> +#define VD56G3_MODEL_ID					0x5603
>> +#define VD56G3_REG_REVISION				CCI_REG16_LE(0x0002)
>> +#define VD56G3_REG_OPTICAL_REVISION			CCI_REG8(0x001a)
>> +#define VD56G3_OPTICAL_REVISION_MONO			0
>> +#define VD56G3_OPTICAL_REVISION_BAYER			1
>> +#define VD56G3_REG_SYSTEM_FSM				CCI_REG8(0x0028)
>> +#define VD56G3_SYSTEM_FSM_READY_TO_BOOT			0x01
>> +#define VD56G3_SYSTEM_FSM_SW_STBY			0x02
>> +#define VD56G3_SYSTEM_FSM_STREAMING			0x03
>> +#define VD56G3_REG_APPLIED_COARSE_EXPOSURE		CCI_REG16_LE(0x0064)
>> +#define VD56G3_REG_APPLIED_ANALOG_GAIN			CCI_REG8(0x0068)
>> +#define VD56G3_REG_APPLIED_DIGITAL_GAIN			CCI_REG16_LE(0x006a)
>> +#define VD56G3_REG_BOOT					CCI_REG8(0x0200)
>> +#define VD56G3_CMD_ACK					0
>> +#define VD56G3_CMD_BOOT					1
>> +#define VD56G3_REG_STBY					CCI_REG8(0x0201)
>> +#define VD56G3_CMD_START_STREAM				1
>> +#define VD56G3_REG_STREAMING				CCI_REG8(0x0202)
>> +#define VD56G3_CMD_STOP_STREAM				1
>> +#define VD56G3_REG_EXT_CLOCK				CCI_REG32_LE(0x0220)
>> +#define VD56G3_REG_CLK_PLL_PREDIV			CCI_REG8(0x0224)
>> +#define VD56G3_REG_CLK_SYS_PLL_MULT			CCI_REG8(0x0226)
>> +#define VD56G3_REG_ORIENTATION				CCI_REG8(0x0302)
>> +#define VD56G3_REG_FORMAT_CTRL				CCI_REG8(0x030a)
>> +#define VD56G3_REG_OIF_CTRL				CCI_REG16_LE(0x030c)
>> +#define VD56G3_REG_OIF_IMG_CTRL				CCI_REG8(0x030f)
>> +#define VD56G3_REG_OIF_CSI_BITRATE			CCI_REG16_LE(0x0312)
>> +#define VD56G3_REG_DUSTER_CTRL				CCI_REG8(0x0318)
>> +#define VD56G3_DUSTER_DISABLE				0
>> +#define VD56G3_DUSTER_ENABLE_DEF_MODULES		0x13
>> +#define VD56G3_REG_ISL_ENABLE				CCI_REG8(0x0333)
>> +#define VD56G3_REG_DARKCAL_CTRL				CCI_REG8(0x0340)
>> +#define VD56G3_DARKCAL_ENABLE				1
>> +#define VD56G3_DARKCAL_DISABLE_DARKAVG			2
>> +#define VD56G3_REG_PATGEN_CTRL				CCI_REG16_LE(0x0400)
>> +#define VD56G3_PATGEN_ENABLE				1
>> +#define VD56G3_PATGEN_TYPE_SHIFT			4
>> +#define VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE		CCI_REG16_LE(0x042a)
>> +#define VD56G3_REG_AE_COLDSTART_ANALOG_GAIN		CCI_REG8(0x042c)
>> +#define VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN		CCI_REG16_LE(0x042e)
>> +#define VD56G3_REG_AE_ROI_START_H			CCI_REG16_LE(0x0432)
>> +#define VD56G3_REG_AE_ROI_START_V			CCI_REG16_LE(0x0434)
>> +#define VD56G3_REG_AE_ROI_END_H				CCI_REG16_LE(0x0436)
>> +#define VD56G3_REG_AE_ROI_END_V				CCI_REG16_LE(0x0438)
>> +#define VD56G3_REG_AE_COMPENSATION			CCI_REG16_LE(0x043a)
>> +#define VD56G3_REG_EXP_MODE				CCI_REG8(0x044c)
>> +#define VD56G3_EXP_MODE_AUTO				0
>> +#define VD56G3_EXP_MODE_FREEZE				1
>> +#define VD56G3_EXP_MODE_MANUAL				2
>> +#define VD56G3_REG_MANUAL_ANALOG_GAIN			CCI_REG8(0x044d)
>> +#define VD56G3_REG_MANUAL_COARSE_EXPOSURE		CCI_REG16_LE(0x044e)
>> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0		CCI_REG16_LE(0x0450)
>> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1		CCI_REG16_LE(0x0452)
>> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2		CCI_REG16_LE(0x0454)
>> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3		CCI_REG16_LE(0x0456)
>> +#define VD56G3_REG_FRAME_LENGTH				CCI_REG16_LE(0x0458)
>> +#define VD56G3_REG_Y_START				CCI_REG16_LE(0x045a)
>> +#define VD56G3_REG_Y_END				CCI_REG16_LE(0x045c)
>> +#define VD56G3_REG_OUT_ROI_X_START			CCI_REG16_LE(0x045e)
>> +#define VD56G3_REG_OUT_ROI_X_END			CCI_REG16_LE(0x0460)
>> +#define VD56G3_REG_OUT_ROI_Y_START			CCI_REG16_LE(0x0462)
>> +#define VD56G3_REG_OUT_ROI_Y_END			CCI_REG16_LE(0x0464)
>> +#define VD56G3_REG_GPIO_0_CTRL				CCI_REG8(0x0467)
>> +#define VD56G3_GPIOX_GPIO_IN				0x01
>> +#define VD56G3_GPIOX_STROBE_MODE			0x02
>> +#define VD56G3_REG_READOUT_CTRL				CCI_REG8(0x047e)
>> +#define READOUT_NORMAL					0x00
>> +#define READOUT_DIGITAL_BINNING_X2			0x01
>> +
>> +/*
>> + * The VD56G3 pixel array is organized as follows:
>> + *
>> + * +--------------------------------+
>> + * |                                | \
>> + * |   +------------------------+   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   |   Default resolution   |   |  | Native height (1364)
>> + * |   |      1120 x 1360       |   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   |                        |   |  |
>> + * |   +------------------------+   |  |
>> + * |                                | /
>> + * +--------------------------------+
>> + *   <----------------------------->
>> + *                     \-------------------  Native width (1124)
>> + *
>> + * The native resolution is 1124x1364.
>> + * The recommended/default resolution is 1120x1360 (multiple of 16).
>> + */
>> +#define VD56G3_NATIVE_WIDTH				1124
>> +#define VD56G3_NATIVE_HEIGHT				1364
>> +#define VD56G3_DEFAULT_WIDTH				1120
>> +#define VD56G3_DEFAULT_HEIGHT				1360
>> +#define VD56G3_DEFAULT_MODE				1
>> +
>> +/* PLL settings */
>> +#define VD56G3_TARGET_PLL				804000000UL
>> +#define VD56G3_VT_CLOCK_DIV				5
>> +
>> +/* Line length and Frame length (settings are for standard 10bits ADC mode) */
>> +#define VD56G3_LINE_LENGTH_MIN				1236
>> +#define VD56G3_VBLANK_MIN				110
>> +#define VD56G3_FRAME_LENGTH_DEF_60FPS			2168
>> +
>> +/* Exposure settings */
>> +#define VD56G3_EXPOSURE_MARGIN				75
>> +#define VD56G3_EXPOSURE_DEFAULT				1420
>> +
>> +/* Output Interface settings */
>> +#define VD56G3_MAX_CSI_DATA_LANES			2
>> +#define VD56G3_LINK_FREQ_DEF_1LANE			750000000UL
>> +#define VD56G3_LINK_FREQ_DEF_2LANES			402000000UL
>> +
>> +/* GPIOs */
>> +#define VD56G3_NB_GPIOS					8
>> +
>> +/* regulator supplies */
>> +static const char *const vd56g3_supply_names[] = {
>> +	"VCORE",
>> +	"VDDIO",
>> +	"VANA",
>> +};
>> +
>> +#define VD56G3_NUM_SUPPLIES		ARRAY_SIZE(vd56g3_supply_names)
> 
> Please use ARRAY_SIZE(...) directly.

Ok, applied for V2

> 
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Models (VD56G3: Mono, VD66GY: Bayer RGB), Modes and formats
>> + */
>> +
>> +enum vd56g3_models {
>> +	VD56G3_MODEL_VD56G3,
>> +	VD56G3_MODEL_VD66GY,
>> +};
>> +
>> +struct vd56g3_mode {
>> +	u32 width;
>> +	u32 height;
>> +};
>> +
>> +static const struct vd56g3_mode vd56g3_supported_modes[] = {
>> +	{
>> +		.width = VD56G3_NATIVE_WIDTH,
>> +		.height = VD56G3_NATIVE_HEIGHT,
>> +	},
>> +	{
>> +		.width = VD56G3_DEFAULT_WIDTH,
>> +		.height = VD56G3_DEFAULT_HEIGHT,
>> +	},
>> +	{
>> +		.width = 1024,
>> +		.height = 1280,
>> +	},
>> +	{
>> +		.width = 1024,
>> +		.height = 768,
>> +	},
>> +	{
>> +		.width = 768,
>> +		.height = 1024,
>> +	},
>> +	{
>> +		.width = 720,
>> +		.height = 1280,
>> +	},
>> +	{
>> +		.width = 640,
>> +		.height = 480,
>> +	},
>> +	{
>> +		.width = 480,
>> +		.height = 640,
>> +	},
>> +	{
>> +		.width = 320,
>> +		.height = 240,
>> +	},
>> +};
>> +
>> +/*
>> + * Sensor support 8bits and 10bits output in both variants
>> + *  - Monochrome
>> + *  - RGB (with all H/V flip variations)
>> + */
>> +static const unsigned int vd56g3_mbus_codes[2][5] = {
>> +	{
>> +		MEDIA_BUS_FMT_Y8_1X8,
>> +		MEDIA_BUS_FMT_SGRBG8_1X8,
>> +		MEDIA_BUS_FMT_SRGGB8_1X8,
>> +		MEDIA_BUS_FMT_SBGGR8_1X8,
>> +		MEDIA_BUS_FMT_SGBRG8_1X8,
>> +	},
>> +	{
>> +		MEDIA_BUS_FMT_Y10_1X10,
>> +		MEDIA_BUS_FMT_SGRBG10_1X10,
>> +		MEDIA_BUS_FMT_SRGGB10_1X10,
>> +		MEDIA_BUS_FMT_SBGGR10_1X10,
>> +		MEDIA_BUS_FMT_SGBRG10_1X10,
>> +	},
>> +};
>> +
>> +enum vd56g3_expo_state {
>> +	VD56G3_EXPO_AUTO,
>> +	VD56G3_EXPO_AUTO_FREEZE,
>> +	VD56G3_EXPO_MANUAL
>> +};
>> +
>> +struct vd56g3 {
>> +	struct i2c_client *i2c_client;
>> +	struct v4l2_subdev sd;
>> +	struct media_pad pad;
>> +	struct regulator_bulk_data supplies[VD56G3_NUM_SUPPLIES];
>> +	struct gpio_desc *reset_gpio;
>> +	struct clk *xclk;
>> +	struct regmap *regmap;
>> +	u32 ext_clock;
>> +	u32 pll_prediv;
>> +	u32 pll_mult;
>> +	u32 pixel_clock;
>> +	u16 oif_ctrl;
>> +	int nb_of_lane;
>> +	u32 gpios[VD56G3_NB_GPIOS];
>> +	unsigned long ext_leds_mask;
>> +	bool is_mono;
>> +	struct v4l2_ctrl_handler ctrl_handler;
>> +	struct v4l2_ctrl *hblank_ctrl;
>> +	struct v4l2_ctrl *vblank_ctrl;
>> +	struct {
>> +		struct v4l2_ctrl *hflip_ctrl;
>> +		struct v4l2_ctrl *vflip_ctrl;
>> +	};
>> +	struct v4l2_ctrl *patgen_ctrl;
>> +	struct {
>> +		struct v4l2_ctrl *ae_ctrl;
>> +		struct v4l2_ctrl *expo_ctrl;
>> +		struct v4l2_ctrl *again_ctrl;
>> +		struct v4l2_ctrl *dgain_ctrl;
>> +	};
>> +	struct v4l2_ctrl *ae_lock_ctrl;
>> +	struct v4l2_ctrl *ae_bias_ctrl;
>> +	struct v4l2_ctrl *led_ctrl;
>> +	bool streaming;
>> +};
>> +
>> +static inline struct vd56g3 *to_vd56g3(struct v4l2_subdev *sd)
>> +{
>> +	return container_of(sd, struct vd56g3, sd);
>> +}
>> +
>> +static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
>> +{
>> +	return &container_of(ctrl->handler, struct vd56g3, ctrl_handler)->sd;
>> +}
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Additional i2c register helpers
>> + */
>> +
>> +static int vd56g3_poll_reg(struct vd56g3 *sensor, u32 reg, u8 poll_val,
>> +			   int *err)
>> +{
>> +	unsigned int val = 0;
>> +	int ret;
>> +
>> +	if (err && *err)
>> +		return *err;
>> +
>> +	reg = reg & CCI_REG_ADDR_MASK;
> 
> Please use CCI_REG_ADDR() instead. I'd write this without the assignment,
> i.e. call CCI_REG_ADDR() as an argument to the function.

Yes, this is quite better !
Applied for V2.

> 
>> +	ret = regmap_read_poll_timeout(sensor->regmap, reg, val,
>> +				       (val == poll_val), 2000,
>> +				       500 * USEC_PER_MSEC);
>> +
>> +	if (ret && err)
>> +		*err = ret;
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_wait_state(struct vd56g3 *sensor, int state, int *err)
>> +{
>> +	return vd56g3_poll_reg(sensor, VD56G3_REG_SYSTEM_FSM, state, err);
>> +}
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Controls: definitions, helpers and handlers
>> + */
>> +
>> +static const char *const vd56g3_tp_menu[] = { "Disabled", "Solid", "Colorbar",
>> +					      "Gradbar",  "Hgrey", "Vgrey",
>> +					      "Dgrey",	  "PN28" };
>> +
>> +static const s64 vd56g3_ev_bias_qmenu[] = { -4000, -3500, -3000, -2500, -2000,
>> +					    -1500, -1000, -500,	 0,	500,
>> +					    1000,  1500,  2000,	 2500,	3000,
>> +					    3500,  4000 };
>> +
>> +static const s64 vd56g3_link_freq_1lane[] = { VD56G3_LINK_FREQ_DEF_1LANE };
>> +
>> +static const s64 vd56g3_link_freq_2lanes[] = { VD56G3_LINK_FREQ_DEF_2LANES };
>> +
>> +static u8 vd56g3_get_bpp(__u32 code)
>> +{
>> +	switch (code) {
>> +	case MEDIA_BUS_FMT_Y8_1X8:
>> +	case MEDIA_BUS_FMT_SGRBG8_1X8:
>> +	case MEDIA_BUS_FMT_SRGGB8_1X8:
>> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
>> +	case MEDIA_BUS_FMT_SGBRG8_1X8:
>> +		return 8;
>> +	case MEDIA_BUS_FMT_Y10_1X10:
>> +	case MEDIA_BUS_FMT_SGRBG10_1X10:
>> +	case MEDIA_BUS_FMT_SRGGB10_1X10:
>> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
>> +	case MEDIA_BUS_FMT_SGBRG10_1X10:
>> +		return 10;
>> +	default:
>> +		/* Should never happen */
>> +		WARN(1, "Unsupported code %d. default to 8 bpp", code);
> 
> WARN() is discouraged nowadays. Same for the rest.

Thanks for pointing me.
Unless otherwise proposed, I will use dev_warn for V2.

> 
>> +	}
>> +
>> +	return 8;
>> +}
>> +
>> +static u8 vd56g3_get_datatype(__u32 code)
>> +{
>> +	switch (code) {
>> +	case MEDIA_BUS_FMT_Y8_1X8:
>> +	case MEDIA_BUS_FMT_SGRBG8_1X8:
>> +	case MEDIA_BUS_FMT_SRGGB8_1X8:
>> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
>> +	case MEDIA_BUS_FMT_SGBRG8_1X8:
>> +		return MIPI_CSI2_DT_RAW8;
>> +	case MEDIA_BUS_FMT_Y10_1X10:
>> +	case MEDIA_BUS_FMT_SGRBG10_1X10:
>> +	case MEDIA_BUS_FMT_SRGGB10_1X10:
>> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
>> +	case MEDIA_BUS_FMT_SGBRG10_1X10:
>> +		return MIPI_CSI2_DT_RAW10;
>> +	default:
>> +		/* Should never happen */
>> +		WARN(1, "Unsupported code %d. default to MIPI_CSI2_DT_RAW8",
>> +		     code);
>> +	}
>> +
>> +	return MIPI_CSI2_DT_RAW8;
>> +}
>> +
>> +static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
>> +{
>> +	u64 exposure = 0;
>> +	u64 again = 0;
>> +	u64 dgain = 0;
> 
> Do you need the assignments? The values are assigned by cci_read() below,
> right?

Well initially, I didn't have those assignments and some checker
complains (Honestly I didn't investigate the warning).
I will double check and see if they are necessary.

> 
>> +	int ret = 0;
>> +
>> +	/*
>> +	 * When 'force_cur_val' is enabled, save the ctrl value in 'cur.val'
>> +	 * instead of the normal 'val', this is used during poweroff to cache
>> +	 * volatile ctrls and enable coldstart.
>> +	 */
>> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_COARSE_EXPOSURE, &exposure,
>> +		 &ret);
>> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_ANALOG_GAIN, &again, &ret);
>> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_DIGITAL_GAIN, &dgain, &ret);
>> +	if (ret)
>> +		return ret;
>> +
>> +	if (force_cur_val) {
>> +		sensor->expo_ctrl->cur.val = exposure;
>> +		sensor->again_ctrl->cur.val = again;
>> +		sensor->dgain_ctrl->cur.val = dgain;
>> +	} else {
>> +		sensor->expo_ctrl->val = exposure;
>> +		sensor->again_ctrl->val = again;
>> +		sensor->dgain_ctrl->val = dgain;
>> +	}
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_update_patgen(struct vd56g3 *sensor, u32 patgen_index)
>> +{
>> +	u32 pattern = patgen_index <= 3 ? patgen_index : patgen_index + 12;
>> +	u16 patgen = pattern << VD56G3_PATGEN_TYPE_SHIFT;
>> +	u8 duster = VD56G3_DUSTER_ENABLE_DEF_MODULES;
>> +	u8 darkcal = VD56G3_DARKCAL_ENABLE;
>> +	int ret = 0;
>> +
>> +	if (patgen_index) {
>> +		patgen |= VD56G3_PATGEN_ENABLE;
>> +		duster = VD56G3_DUSTER_DISABLE;
>> +		darkcal = VD56G3_DARKCAL_DISABLE_DARKAVG;
>> +	}
>> +
>> +	cci_write(sensor->regmap, VD56G3_REG_DUSTER_CTRL, duster, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_DARKCAL_CTRL, darkcal, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_PATGEN_CTRL, patgen, &ret);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_update_expo_cluster(struct vd56g3 *sensor, bool is_auto)
>> +{
>> +	int ret = 0;
> 
> This would be nicer as declared last.

Sure !

> 
>> +	enum vd56g3_expo_state expo_state = is_auto ? VD56G3_EXP_MODE_AUTO :
>> +						      VD56G3_EXP_MODE_MANUAL;
>> +
>> +	if (sensor->ae_ctrl->is_new)
>> +		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
>> +			  &ret);
>> +
>> +	/* In Auto expo, set coldstart parameters */
>> +	if (is_auto && sensor->ae_ctrl->is_new) {
>> +		cci_write(sensor->regmap,
>> +			  VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE,
>> +			  sensor->expo_ctrl->val, &ret);
>> +		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_ANALOG_GAIN,
>> +			  sensor->again_ctrl->val, &ret);
>> +		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN,
>> +			  sensor->dgain_ctrl->val, &ret);
>> +	}
>> +
>> +	/* In Manual expo, set exposure, analog and digital gains */
>> +	if (!is_auto && sensor->expo_ctrl->is_new)
>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_COARSE_EXPOSURE,
>> +			  sensor->expo_ctrl->val, &ret);
>> +
>> +	if (!is_auto && sensor->again_ctrl->is_new)
>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_ANALOG_GAIN,
>> +			  sensor->again_ctrl->val, &ret);
>> +
>> +	if (!is_auto && sensor->dgain_ctrl->is_new) {
>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0,
>> +			  sensor->dgain_ctrl->val, &ret);
>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1,
>> +			  sensor->dgain_ctrl->val, &ret);
>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2,
>> +			  sensor->dgain_ctrl->val, &ret);
>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3,
>> +			  sensor->dgain_ctrl->val, &ret);
>> +	}
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_lock_exposure(struct vd56g3 *sensor, u32 lock_val)
>> +{
>> +	bool ae_lock = lock_val & V4L2_LOCK_EXPOSURE;
>> +	enum vd56g3_expo_state expo_state = ae_lock ? VD56G3_EXP_MODE_FREEZE :
>> +						      VD56G3_EXP_MODE_AUTO;
>> +	int ret = 0;
>> +
>> +	if (sensor->ae_ctrl->val == V4L2_EXPOSURE_AUTO)
>> +		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
>> +			  &ret);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_write_gpiox(struct vd56g3 *sensor, unsigned long gpio_mask)
>> +{
>> +	unsigned long io;
>> +	u32 gpio_val;
>> +	int ret = 0;
>> +
>> +	for_each_set_bit(io, &gpio_mask, VD56G3_NB_GPIOS) {
>> +		gpio_val = sensor->gpios[io];
>> +
>> +		if (gpio_val == VD56G3_GPIOX_STROBE_MODE &&
>> +		    sensor->led_ctrl->val == V4L2_FLASH_LED_MODE_NONE)
>> +			gpio_val = VD56G3_GPIOX_GPIO_IN;
>> +
>> +		cci_write(sensor->regmap, VD56G3_REG_GPIO_0_CTRL + io, gpio_val,
>> +			  &ret);
>> +	}
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
>> +{
>> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
>> +	int ret = 0;
>> +
>> +	/* Interact with HW only when it is powered ON */
>> +	if (!pm_runtime_get_if_in_use(&client->dev))
>> +		return 0;
>> +
>> +	switch (ctrl->id) {
>> +	case V4L2_CID_EXPOSURE_AUTO:
>> +		ret = vd56g3_read_expo_cluster(sensor, false);
>> +		break;
>> +	default:
>> +		ret = -EINVAL;
>> +		break;
>> +	}
>> +
>> +	pm_runtime_mark_last_busy(&client->dev);
>> +	pm_runtime_put_autosuspend(&client->dev);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_s_ctrl(struct v4l2_ctrl *ctrl)
>> +{
>> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
>> +	const struct v4l2_rect *crop;
>> +	unsigned int frame_length = 0;
>> +	unsigned int expo_max;
>> +	unsigned int ae_compensation;
>> +	bool is_auto = false;
>> +	int ret;
>> +
>> +	struct v4l2_subdev_state *state =
>> +		v4l2_subdev_get_locked_active_state(sd);
>> +	crop = v4l2_subdev_state_get_crop(state, 0);
>> +
>> +	if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
>> +		return 0;
>> +
>> +	/* Update controls state, range, etc. whatever the state of the HW */
>> +	switch (ctrl->id) {
>> +	case V4L2_CID_VBLANK:
>> +		frame_length = crop->height + ctrl->val;
>> +		expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
>> +		__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
>> +					 VD56G3_EXPOSURE_DEFAULT);
>> +		break;
>> +	case V4L2_CID_EXPOSURE_AUTO:
>> +		is_auto = (ctrl->val == V4L2_EXPOSURE_AUTO);
>> +		__v4l2_ctrl_grab(sensor->ae_lock_ctrl, !is_auto);
>> +		__v4l2_ctrl_grab(sensor->ae_bias_ctrl, !is_auto);
>> +		break;
>> +	default:
>> +		break;
>> +	}
>> +
>> +	/* Interact with HW only when it is powered ON */
>> +	if (!pm_runtime_get_if_in_use(&client->dev))
>> +		return 0;
>> +
>> +	switch (ctrl->id) {
>> +	case V4L2_CID_HFLIP:
>> +		ret = cci_write(sensor->regmap, VD56G3_REG_ORIENTATION,
>> +				sensor->hflip_ctrl->val |
>> +					(sensor->vflip_ctrl->val << 1),
>> +				NULL);
>> +		break;
>> +	case V4L2_CID_TEST_PATTERN:
>> +		ret = vd56g3_update_patgen(sensor, ctrl->val);
>> +		break;
>> +	case V4L2_CID_EXPOSURE_AUTO:
>> +		ret = vd56g3_update_expo_cluster(sensor, is_auto);
>> +		break;
>> +	case V4L2_CID_3A_LOCK:
>> +		ret = vd56g3_lock_exposure(sensor, ctrl->val);
>> +		break;
>> +	case V4L2_CID_AUTO_EXPOSURE_BIAS:
>> +		ae_compensation =
>> +			DIV_ROUND_CLOSEST((int)vd56g3_ev_bias_qmenu[ctrl->val] *
>> +					  256, 1000);
>> +		ret = cci_write(sensor->regmap, VD56G3_REG_AE_COMPENSATION,
>> +				ae_compensation, NULL);
>> +		break;
>> +	case V4L2_CID_VBLANK:
>> +		ret = cci_write(sensor->regmap, VD56G3_REG_FRAME_LENGTH,
>> +				frame_length, NULL);
>> +		break;
>> +	case V4L2_CID_FLASH_LED_MODE:
>> +		ret = vd56g3_write_gpiox(sensor, sensor->ext_leds_mask);
>> +		break;
>> +	default:
>> +		ret = -EINVAL;
>> +		break;
>> +	}
>> +
>> +	pm_runtime_mark_last_busy(&client->dev);
>> +	pm_runtime_put_autosuspend(&client->dev);
>> +
>> +	return ret;
>> +}
>> +
>> +static const struct v4l2_ctrl_ops vd56g3_ctrl_ops = {
>> +	.g_volatile_ctrl = vd56g3_g_volatile_ctrl,
>> +	.s_ctrl = vd56g3_s_ctrl,
>> +};
>> +
>> +static void vd56g3_update_controls(struct vd56g3 *sensor)
>> +{
>> +	const struct v4l2_rect *crop;
>> +	unsigned int hblank;
>> +	unsigned int vblank_min, vblank, vblank_max;
>> +	unsigned int frame_length;
>> +	unsigned int expo_max;
>> +
>> +	struct v4l2_subdev_state *state =
>> +		v4l2_subdev_get_locked_active_state(&sensor->sd);
>> +	crop = v4l2_subdev_state_get_crop(state, 0);
>> +
>> +	hblank = VD56G3_LINE_LENGTH_MIN - crop->width;
>> +	vblank_min = VD56G3_VBLANK_MIN;
>> +	vblank = VD56G3_FRAME_LENGTH_DEF_60FPS - crop->height;
>> +	vblank_max = 0xffff - crop->height;
>> +	frame_length = crop->height + vblank;
>> +	expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
>> +
>> +	/* Update blanking and exposure (ranges + values) */
>> +	__v4l2_ctrl_modify_range(sensor->hblank_ctrl, hblank, hblank, 1,
>> +				 hblank);
>> +	__v4l2_ctrl_modify_range(sensor->vblank_ctrl, vblank_min, vblank_max, 1,
>> +				 vblank);
>> +	__v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, vblank);
>> +	__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
>> +				 VD56G3_EXPOSURE_DEFAULT);
>> +	__v4l2_ctrl_s_ctrl(sensor->expo_ctrl, VD56G3_EXPOSURE_DEFAULT);
> 
> Note that __v4l2_ctrl_modify_range() and __v4l2_ctrl_s_ctrl() may fail.

Yes, Tommaso pointed me also. (I took inspiration from the imx219 and
imx290 drivers which do not have the return test).

I will take this into consideration for V2.

> 
>> +}
>> +
>> +static int vd56g3_init_controls(struct vd56g3 *sensor)
>> +{
>> +	const struct v4l2_ctrl_ops *ops = &vd56g3_ctrl_ops;
>> +	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
>> +	struct v4l2_ctrl *ctrl;
>> +	int ret;
>> +
>> +	v4l2_ctrl_handler_init(hdl, 25);
>> +
>> +	/* Horizontal & vertical flips modify bayer code on RGB variant */
>> +	sensor->hflip_ctrl =
>> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
>> +	if (sensor->hflip_ctrl)
>> +		sensor->hflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
>> +
>> +	sensor->vflip_ctrl =
>> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
>> +	if (sensor->vflip_ctrl)
>> +		sensor->vflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
>> +
>> +	sensor->patgen_ctrl =
>> +		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
>> +					     ARRAY_SIZE(vd56g3_tp_menu) - 1, 0,
>> +					     0, vd56g3_tp_menu);
>> +
>> +	ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
>> +				      ARRAY_SIZE(vd56g3_link_freq_1lane) - 1, 0,
>> +				      (sensor->nb_of_lane == 2) ?
>> +					      vd56g3_link_freq_2lanes :
>> +					      vd56g3_link_freq_1lane);
>> +	if (ctrl)
>> +		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
>> +
>> +	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
>> +				 sensor->pixel_clock, sensor->pixel_clock, 1,
>> +				 sensor->pixel_clock);
>> +	if (ctrl)
>> +		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
>> +
>> +	sensor->ae_ctrl = v4l2_ctrl_new_std_menu(hdl, ops,
>> +						 V4L2_CID_EXPOSURE_AUTO,
>> +						 V4L2_EXPOSURE_MANUAL, 0,
>> +						 V4L2_EXPOSURE_AUTO);
>> +
>> +	sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK, 0,
>> +						 GENMASK(2, 0), 0, 0);
>> +
>> +	sensor->ae_bias_ctrl =
>> +		v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
>> +				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) - 1,
>> +				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) / 2,
>> +				       vd56g3_ev_bias_qmenu);
>> +
>> +	/*
>> +	 * Analog gain [1, 8] is computed with the following logic :
>> +	 * 32/(32 - again_reg), with again_reg in the range [0:28]
>> +	 * Digital gain [1.00, 8.00] is coded as a Fixed Point 5.8
>> +	 */
>> +	sensor->again_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
>> +					       0, 28, 1, 0);
>> +	sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
>> +					       0x100, 0x800, 1, 0x100);
>> +
>> +	/*
>> +	 * Set the exposure, horizontal and vertical blanking ctrls
>> +	 * to hardcoded values, they will be updated in vd56g3_update_controls.
>> +	 * Exposure being in an auto-cluster, set a significant value here.
>> +	 */
>> +	sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
>> +					      VD56G3_EXPOSURE_DEFAULT,
>> +					      VD56G3_EXPOSURE_DEFAULT, 1,
>> +					      VD56G3_EXPOSURE_DEFAULT);
>> +	sensor->hblank_ctrl =
>> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 1, 1, 1, 1);
>> +	if (sensor->hblank_ctrl)
>> +		sensor->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
>> +	sensor->vblank_ctrl =
>> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, 1, 1, 1, 1);
>> +
>> +	/* Additional control based on device tree properties */
>> +	if (sensor->ext_leds_mask)
>> +		sensor->led_ctrl =
>> +			v4l2_ctrl_new_std_menu(hdl, ops,
>> +					       V4L2_CID_FLASH_LED_MODE,
>> +					       V4L2_FLASH_LED_MODE_FLASH, 0,
>> +					       V4L2_FLASH_LED_MODE_NONE);
>> +
>> +	if (hdl->error) {
>> +		ret = hdl->error;
>> +		goto free_ctrls;
>> +	}
>> +
>> +	v4l2_ctrl_cluster(2, &sensor->hflip_ctrl);
>> +	v4l2_ctrl_auto_cluster(4, &sensor->ae_ctrl, V4L2_EXPOSURE_MANUAL, true);
>> +
>> +	sensor->sd.ctrl_handler = hdl;
> 
> A newline here?

Sure, this isn't too difficult !

> 
>> +	return 0;
>> +
>> +free_ctrls:
>> +	v4l2_ctrl_handler_free(hdl);
> 
> Here, too?
> 
>> +	return ret;
>> +}
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Videos ops
>> + */
>> +
>> +static int vd56g3_stream_on(struct vd56g3 *sensor,
>> +			    struct v4l2_subdev_state *state)
>> +{
>> +	const struct v4l2_mbus_framefmt *format =
>> +		v4l2_subdev_state_get_format(state, 0);
>> +	const struct v4l2_rect *crop =
>> +		v4l2_subdev_state_get_crop(state, 0);
>> +	int ret = 0;
>> +	unsigned int csi_mbps = ((sensor->nb_of_lane == 2) ?
>> +					 VD56G3_LINK_FREQ_DEF_2LANES :
>> +					 VD56G3_LINK_FREQ_DEF_1LANE) * 2 / MEGA;
>> +	unsigned int binning;
>> +
>> +	/* configure clocks */
>> +	cci_write(sensor->regmap, VD56G3_REG_EXT_CLOCK, sensor->ext_clock,
>> +		  &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_CLK_PLL_PREDIV, sensor->pll_prediv,
>> +		  &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_CLK_SYS_PLL_MULT, sensor->pll_mult,
>> +		  &ret);
>> +
>> +	/* configure output */
>> +	cci_write(sensor->regmap, VD56G3_REG_FORMAT_CTRL,
>> +		  vd56g3_get_bpp(format->code), &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OIF_CSI_BITRATE, csi_mbps, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OIF_IMG_CTRL,
>> +		  vd56g3_get_datatype(format->code), &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_ISL_ENABLE, 0, &ret);
>> +
>> +	/* configure binning mode */
>> +	switch (crop->width / format->width) {
>> +	case 1:
>> +	default:
>> +		binning = READOUT_NORMAL;
>> +		break;
>> +	case 2:
>> +		binning = READOUT_DIGITAL_BINNING_X2;
>> +		break;
>> +	}
>> +	cci_write(sensor->regmap, VD56G3_REG_READOUT_CTRL, binning, &ret);
>> +
>> +	/* configure ROIs */
>> +	cci_write(sensor->regmap, VD56G3_REG_Y_START, crop->top, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_Y_END,
>> +		  crop->top + crop->height - 1, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_START, crop->left, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_END,
>> +		  crop->left + crop->width - 1, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_START, 0, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_END, crop->height - 1,
>> +		  &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_H, crop->left, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_H,
>> +		  crop->left + crop->width - 1, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_V, 0, &ret);
>> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_V, crop->height - 1,
>> +		  &ret);
>> +	if (ret)
>> +		return ret;
>> +
>> +	/* Setup default GPIO values; could be overridden by V4L2 ctrl setup */
>> +	ret = vd56g3_write_gpiox(sensor, GENMASK(VD56G3_NB_GPIOS - 1, 0));
>> +	if (ret)
>> +		return ret;
>> +
>> +	/* Apply settings from V4L2 ctrls */
>> +	ret = __v4l2_ctrl_handler_setup(&sensor->ctrl_handler);
>> +	if (ret)
>> +		return ret;
>> +
>> +	/* start streaming */
>> +	cci_write(sensor->regmap, VD56G3_REG_STBY, VD56G3_CMD_START_STREAM,
>> +		  &ret);
>> +	vd56g3_poll_reg(sensor, VD56G3_REG_STBY, VD56G3_CMD_ACK, &ret);
>> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_STREAMING, &ret);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_stream_off(struct vd56g3 *sensor)
>> +{
>> +	int ret = 0;
> 
> No need to initialise ret.

Yes, right.

> 
>> +
>> +	/* Retrieve Expo cluster to enable coldstart of AE */
>> +	ret = vd56g3_read_expo_cluster(sensor, true);
>> +
>> +	cci_write(sensor->regmap, VD56G3_REG_STREAMING, VD56G3_CMD_STOP_STREAM,
>> +		  &ret);
>> +	vd56g3_poll_reg(sensor, VD56G3_REG_STREAMING, VD56G3_CMD_ACK, &ret);
>> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_s_stream(struct v4l2_subdev *sd, int enable)
>> +{
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
>> +	int ret = 0;
> 
> ret should be declared as last.
> 
>> +
> 
> Extra newline.
> 
>> +	struct v4l2_subdev_state *state =
>> +		v4l2_subdev_lock_and_get_active_state(sd);
> 
> Please do the assignment outside declarations---it has side effects.

Ok, I'll clean the declarations for V2.

> 
>> +
>> +	if (enable) {
>> +		ret = pm_runtime_resume_and_get(&client->dev);
>> +		if (ret < 0)
>> +			goto unlock;
>> +		ret = vd56g3_stream_on(sensor, state);
>> +		if (ret) {
>> +			dev_err(&client->dev, "Failed to start streaming\n");
>> +			pm_runtime_put_sync(&client->dev);
>> +		}
>> +	} else {
>> +		vd56g3_stream_off(sensor);
>> +		pm_runtime_mark_last_busy(&client->dev);
>> +		pm_runtime_put_autosuspend(&client->dev);
>> +	}
>> +
>> +unlock:
>> +	v4l2_subdev_unlock_state(state);
>> +	if (!ret) {
>> +		sensor->streaming = enable;
> 
> This assignment may be done only with sub-device active state locked.

I'm wondering what I had in mind while writing this ... I'll make it
proper for V2.

> 
>> +		v4l2_ctrl_grab(sensor->hflip_ctrl, enable);
>> +		v4l2_ctrl_grab(sensor->vflip_ctrl, enable);
>> +		v4l2_ctrl_grab(sensor->patgen_ctrl, enable);
>> +	}
>> +
>> +	return ret;
>> +}
>> +
>> +static const struct v4l2_subdev_video_ops vd56g3_video_ops = {
>> +	.s_stream = vd56g3_s_stream,
>> +};
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Pad ops
>> + */
>> +
>> +/* Media bus code is dependent of :
>> + *      - 8bits or 10bits output
>> + *      - variant : Mono or RGB
>> + *      - H/V flips parameters in case of RGB
>> + */
>> +static u32 vd56g3_get_mbus_code(struct vd56g3 *sensor, u32 code)
>> +{
>> +	unsigned int i_bpp;
>> +	unsigned int j;
>> +
>> +	for (i_bpp = 0; i_bpp < ARRAY_SIZE(vd56g3_mbus_codes); i_bpp++) {
>> +		for (j = 0; j < ARRAY_SIZE(vd56g3_mbus_codes[i_bpp]); j++) {
>> +			if (vd56g3_mbus_codes[i_bpp][j] == code)
>> +				goto endloops;
>> +		}
>> +	}
> 
> No need for braces. Up to you.

I'll probably keep them ... habits ?!

> 
>> +
>> +endloops:
>> +	if (i_bpp >= ARRAY_SIZE(vd56g3_mbus_codes))
>> +		i_bpp = 0;
>> +
>> +	if (sensor->is_mono)
>> +		j = 0;
>> +	else
>> +		j = 1 + (sensor->hflip_ctrl->val ? 1 : 0) +
>> +		    (sensor->vflip_ctrl->val ? 2 : 0);
>> +
>> +	return vd56g3_mbus_codes[i_bpp][j];
>> +}
>> +
>> +static int vd56g3_enum_mbus_code(struct v4l2_subdev *sd,
>> +				 struct v4l2_subdev_state *sd_state,
>> +				 struct v4l2_subdev_mbus_code_enum *code)
>> +{
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +
>> +	if (code->index >= ARRAY_SIZE(vd56g3_mbus_codes))
>> +		return -EINVAL;
>> +
>> +	code->code =
>> +		vd56g3_get_mbus_code(sensor, vd56g3_mbus_codes[code->index][0]);
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_enum_frame_size(struct v4l2_subdev *sd,
>> +				  struct v4l2_subdev_state *sd_state,
>> +				  struct v4l2_subdev_frame_size_enum *fse)
>> +{
>> +	if (fse->index >= ARRAY_SIZE(vd56g3_supported_modes))
>> +		return -EINVAL;
>> +
>> +	fse->min_width = vd56g3_supported_modes[fse->index].width;
>> +	fse->max_width = fse->min_width;
>> +	fse->min_height = vd56g3_supported_modes[fse->index].height;
>> +	fse->max_height = fse->min_height;
>> +
>> +	return 0;
>> +}
>> +
>> +static void vd56g3_update_img_pad_format(struct vd56g3 *sensor,
>> +					 const struct vd56g3_mode *mode,
>> +					 u32 mbus_code,
>> +					 struct v4l2_mbus_framefmt *mbus_fmt)
>> +{
>> +	mbus_fmt->width = mode->width;
>> +	mbus_fmt->height = mode->height;
>> +	mbus_fmt->code = vd56g3_get_mbus_code(sensor, mbus_code);
>> +	mbus_fmt->colorspace = V4L2_COLORSPACE_RAW;
>> +	mbus_fmt->field = V4L2_FIELD_NONE;
>> +	mbus_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
>> +	mbus_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
>> +	mbus_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
>> +}
>> +
>> +static int vd56g3_set_pad_fmt(struct v4l2_subdev *sd,
>> +			      struct v4l2_subdev_state *sd_state,
>> +			      struct v4l2_subdev_format *sd_fmt)
>> +{
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +	const struct vd56g3_mode *new_mode;
>> +	struct v4l2_rect pad_crop;
>> +	unsigned int binning;
>> +
>> +	if (sensor->streaming)
>> +		return -EBUSY;
>> +
>> +	new_mode = v4l2_find_nearest_size(vd56g3_supported_modes,
>> +					  ARRAY_SIZE(vd56g3_supported_modes),
>> +					  width, height, sd_fmt->format.width,
>> +					  sd_fmt->format.height);
>> +
>> +	vd56g3_update_img_pad_format(sensor, new_mode, sd_fmt->format.code,
>> +				     &sd_fmt->format);
>> +
>> +	/* Compute crop rectangle (maximized via binning) */
>> +	binning = min(VD56G3_NATIVE_WIDTH / sd_fmt->format.width,
>> +		      VD56G3_NATIVE_HEIGHT / sd_fmt->format.height);
>> +	binning = min(binning, 2U);
>> +	pad_crop.width = sd_fmt->format.width * binning;
>> +	pad_crop.height = sd_fmt->format.height * binning;
>> +	pad_crop.left = (VD56G3_NATIVE_WIDTH - pad_crop.width) / 2;
>> +	pad_crop.top = (VD56G3_NATIVE_HEIGHT - pad_crop.height) / 2;
>> +
>> +	/* Update active state's format and crop */
>> +	*v4l2_subdev_state_get_format(sd_state, sd_fmt->pad) = sd_fmt->format;
>> +	*v4l2_subdev_state_get_crop(sd_state, sd_fmt->pad) = pad_crop;
>> +
>> +	if (sd_fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
>> +		vd56g3_update_controls(sensor);
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_get_selection(struct v4l2_subdev *sd,
>> +				struct v4l2_subdev_state *sd_state,
>> +				struct v4l2_subdev_selection *sel)
>> +{
>> +	switch (sel->target) {
>> +	case V4L2_SEL_TGT_CROP:
>> +		sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
>> +		break;
>> +	case V4L2_SEL_TGT_NATIVE_SIZE:
>> +	case V4L2_SEL_TGT_CROP_DEFAULT:
>> +	case V4L2_SEL_TGT_CROP_BOUNDS:
>> +		sel->r.top = 0;
>> +		sel->r.left = 0;
>> +		sel->r.width = VD56G3_NATIVE_WIDTH;
>> +		sel->r.height = VD56G3_NATIVE_HEIGHT;
>> +		break;
>> +	default:
>> +		return -EINVAL;
>> +	}
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_init_state(struct v4l2_subdev *sd,
>> +			     struct v4l2_subdev_state *sd_state)
>> +{
>> +	unsigned int def_mode = VD56G3_DEFAULT_MODE;
>> +	struct v4l2_subdev_format fmt = {
>> +		.which = V4L2_SUBDEV_FORMAT_TRY,
>> +		.pad = 0,
>> +		.format = {
>> +			.code = vd56g3_mbus_codes[0][0],
>> +			.width = vd56g3_supported_modes[def_mode].width,
>> +			.height = vd56g3_supported_modes[def_mode].height,
>> +		},
>> +	};
>> +
>> +	vd56g3_set_pad_fmt(sd, sd_state, &fmt);
> 
> Newline here?

Ok.

> 
>> +	return 0;
>> +}
>> +
>> +static const struct v4l2_subdev_core_ops vd56g3_core_ops = {
>> +	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
>> +	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
>> +};
>> +
>> +static const struct v4l2_subdev_pad_ops vd56g3_pad_ops = {
>> +	.enum_mbus_code = vd56g3_enum_mbus_code,
>> +	.enum_frame_size = vd56g3_enum_frame_size,
>> +	.get_fmt = v4l2_subdev_get_fmt,
>> +	.set_fmt = vd56g3_set_pad_fmt,
>> +	.get_selection = vd56g3_get_selection,
>> +};
>> +
>> +static const struct v4l2_subdev_ops vd56g3_subdev_ops = {
>> +	.core = &vd56g3_core_ops,
>> +	.video = &vd56g3_video_ops,
>> +	.pad = &vd56g3_pad_ops,
>> +};
>> +
>> +static const struct media_entity_operations vd56g3_subdev_entity_ops = {
>> +	.link_validate = v4l2_subdev_link_validate,
>> +};
>> +
>> +static const struct v4l2_subdev_internal_ops vd56g3_internal_ops = {
>> +	.init_state = vd56g3_init_state,
>> +};
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Power management
>> + */
>> +
>> +static int vd56g3_boot(struct vd56g3 *sensor)
>> +{
>> +	int ret = 0;
>> +
>> +	cci_write(sensor->regmap, VD56G3_REG_BOOT, VD56G3_CMD_BOOT, &ret);
>> +	vd56g3_poll_reg(sensor, VD56G3_REG_BOOT, VD56G3_CMD_ACK, &ret);
>> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_power_on(struct vd56g3 *sensor)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	int ret;
>> +
>> +	ret = regulator_bulk_enable(VD56G3_NUM_SUPPLIES, sensor->supplies);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Failed to enable regulators %d", ret);
>> +		return ret;
>> +	}
>> +
>> +	ret = clk_prepare_enable(sensor->xclk);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Failed to enable clock %d", ret);
>> +		goto disable_reg;
>> +	}
>> +
>> +	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
>> +	ret = vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_READY_TO_BOOT, NULL);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Sensor reset failed %d\n", ret);
>> +		goto disable_clock;
>> +	}
>> +
>> +	return 0;
>> +
>> +disable_clock:
>> +	clk_disable_unprepare(sensor->xclk);
>> +	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
>> +disable_reg:
>> +	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_power_off(struct vd56g3 *sensor)
>> +{
>> +	clk_disable_unprepare(sensor->xclk);
>> +	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
>> +	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_runtime_resume(struct device *dev)
>> +{
>> +	struct v4l2_subdev *sd = dev_get_drvdata(dev);
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	int ret;
>> +
>> +	ret = vd56g3_power_on(sensor);
> 
> I think you could unify the runtime resume and suspend functions with the
> power_on/off, using struct device * argument. vd56g3_power_on() won't be
> called separately from vd65g3_boot either.

I'll give it a try for V2.

> 
> Speaking of which---do you really call vd65g3_boot() before identifying the
> sensor?

Good point :-)

In fact on older HW cuts (I hope this won't happens in the future), we
need to apply some FW patches. Once those patches are applied, sensor
need to be (re)booted : that's why I decoupled vd56g3_power_on() and
vd65g3_boot().
That being said, for the current submission this serves nothing : I may
try to gathered those two functions.

> 
>> +	if (ret) {
>> +		dev_err(&client->dev, "Failed to power on %d", ret);
>> +		return ret;
>> +	}
>> +
>> +	ret = vd56g3_boot(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "sensor boot failed %d", ret);
>> +		return ret;
>> +	}
>> +
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_runtime_suspend(struct device *dev)
>> +{
>> +	struct v4l2_subdev *sd = dev_get_drvdata(dev);
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +
>> +	return vd56g3_power_off(sensor);
>> +}
>> +
>> +static const struct dev_pm_ops vd56g3_pm_ops = {
>> +	SET_RUNTIME_PM_OPS(vd56g3_runtime_suspend, vd56g3_runtime_resume, NULL)
>> +};
>> +
>> +/* -----------------------------------------------------------------------------
>> + * Probe and initialization
>> + */
>> +
>> +static int vd56g3_check_csi_conf(struct vd56g3 *sensor,
>> +				 struct fwnode_handle *endpoint)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
>> +	u32 phy_data_lanes[VD56G3_MAX_CSI_DATA_LANES] = { ~0, ~0 };
>> +	int n_lanes;
>> +	int frequency;
>> +	int p, l;
>> +	int ret = 0;
>> +
>> +	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
>> +	if (ret)
>> +		return -EINVAL;
>> +
>> +	/* Check lanes number */
>> +	n_lanes = ep.bus.mipi_csi2.num_data_lanes;
>> +	if (n_lanes != 1 && n_lanes != 2) {
>> +		dev_err(&client->dev, "Invalid data lane number %d\n", n_lanes);
>> +		ret = -EINVAL;
>> +		goto done;
>> +	}
>> +	sensor->nb_of_lane = n_lanes;
>> +
>> +	/* Clock lane must be first */
>> +	if (ep.bus.mipi_csi2.clock_lane != 0) {
>> +		dev_err(&client->dev, "Clk lane must be mapped to lane 0\n");
>> +		ret = -EINVAL;
>> +		goto done;
>> +	}
>> +
>> +	/* Prepare Output Interface conf based on lane settings
>> +	 * logical to physical lane conversion (+ pad remaining slots)
>> +	 */
>> +	for (l = 0; l < n_lanes; l++)
>> +		phy_data_lanes[ep.bus.mipi_csi2.data_lanes[l] - 1] = l;
>> +	for (p = 0; p < VD56G3_MAX_CSI_DATA_LANES; p++) {
>> +		if (phy_data_lanes[p] != ~0)
>> +			continue;
>> +		phy_data_lanes[p] = l;
>> +		l++;
>> +	}
>> +	sensor->oif_ctrl = n_lanes |
>> +			   (ep.bus.mipi_csi2.lane_polarities[0] << 3) |
>> +			   ((phy_data_lanes[0]) << 4) |
>> +			   (ep.bus.mipi_csi2.lane_polarities[1] << 6) |
>> +			   ((phy_data_lanes[1]) << 7) |
>> +			   (ep.bus.mipi_csi2.lane_polarities[2] << 9);
>> +
>> +	/* Check link frequency */
>> +	if (!ep.nr_of_link_frequencies) {
>> +		dev_err(&client->dev, "link-frequency not found in DT\n");
>> +		ret = -EINVAL;
>> +		goto done;
>> +	}
>> +	frequency = (n_lanes == 2) ? VD56G3_LINK_FREQ_DEF_2LANES :
>> +				     VD56G3_LINK_FREQ_DEF_1LANE;
>> +	if (ep.nr_of_link_frequencies != 1 ||
>> +	    ep.link_frequencies[0] != frequency) {
>> +		dev_err(&client->dev, "Link frequency not supported: %lld\n",
>> +			ep.link_frequencies[0]);
>> +		ret = -EINVAL;
>> +		goto done;
>> +	}
>> +
>> +done:
>> +	v4l2_fwnode_endpoint_free(&ep);
>> +	return ret;
>> +}
>> +
>> +static int vd56g3_parse_dt_gpios_array(struct vd56g3 *sensor, char *prop_name,
>> +				       u32 *array, int *nb)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	struct device_node *np = client->dev.of_node;
>> +	unsigned int i;
>> +
>> +	*nb = of_property_read_variable_u32_array(np, prop_name, array, 0,
>> +						  VD56G3_NB_GPIOS);
>> +
>> +	if (*nb == -EINVAL) {
>> +		*nb = 0;
>> +		return *nb;
>> +	} else if (*nb < 0) {
>> +		dev_err(&client->dev, "Failed to read %s prop\n", prop_name);
>> +		return *nb;
>> +	}
>> +
>> +	for (i = 0; i < *nb; i++) {
>> +		if (array[i] >= VD56G3_NB_GPIOS) {
>> +			dev_err(&client->dev, "Invalid GPIO : %d\n", array[i]);
>> +			return -EINVAL;
>> +		}
>> +	}
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_parse_dt_gpios(struct vd56g3 *sensor)
>> +{
>> +	u32 led_gpios[VD56G3_NB_GPIOS];
>> +	int nb_gpios_leds;
>> +	int ret;
>> +	unsigned int i;
>> +
>> +	/* Initialize GPIOs to default */
>> +	for (i = 0; i < VD56G3_NB_GPIOS; i++)
>> +		sensor->gpios[i] = VD56G3_GPIOX_GPIO_IN;
>> +	sensor->ext_leds_mask = 0;
>> +
>> +	/* Take into account optional 'st,leds' output for GPIOs */
>> +	ret = vd56g3_parse_dt_gpios_array(sensor, "st,leds", led_gpios,
>> +					  &nb_gpios_leds);
>> +	if (ret)
>> +		return ret;
>> +	for (i = 0; i < nb_gpios_leds; i++) {
>> +		sensor->gpios[led_gpios[i]] = VD56G3_GPIOX_STROBE_MODE;
>> +		set_bit(led_gpios[i], &sensor->ext_leds_mask);
>> +	}
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_parse_dt(struct vd56g3 *sensor)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	struct device *dev = &client->dev;
>> +	struct fwnode_handle *endpoint;
>> +	int ret;
>> +
>> +	endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0, 0);
>> +	if (!endpoint) {
>> +		dev_err(dev, "endpoint node not found\n");
>> +		return -EINVAL;
>> +	}
>> +
>> +	ret = vd56g3_check_csi_conf(sensor, endpoint);
>> +	fwnode_handle_put(endpoint);
>> +	if (ret)
>> +		return ret;
>> +
>> +	ret = vd56g3_parse_dt_gpios(sensor);
> 
> 	return ...;

Shame on me ... ^^.
Will be applied on V2.

> 
>> +	if (ret)
>> +		return ret;
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_get_regulators(struct vd56g3 *sensor)
>> +{
>> +	int i;
> 
> unsigned int i?
> 
>> +
>> +	for (i = 0; i < VD56G3_NUM_SUPPLIES; i++)
>> +		sensor->supplies[i].supply = vd56g3_supply_names[i];
>> +
>> +	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
>> +				       VD56G3_NUM_SUPPLIES, sensor->supplies);
>> +}
>> +
>> +static int vd56g3_prepare_clock_tree(struct vd56g3 *sensor)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	const unsigned int predivs[] = { 1, 2, 4 };
>> +	u32 pll_out;
>> +	int i;
>> +
>> +	/* External clock must be in [6Mhz-27Mhz] */
>> +	if (sensor->ext_clock < 6 * HZ_PER_MHZ ||
>> +	    sensor->ext_clock > 27 * HZ_PER_MHZ) {
>> +		dev_err(&client->dev,
>> +			"Only 6Mhz-27Mhz clock range supported. Provided %lu MHz\n",
>> +			sensor->ext_clock / HZ_PER_MHZ);
>> +		return -EINVAL;
>> +	}
>> +
>> +	/* PLL input should be in [6Mhz-12Mhz[ */
>> +	for (i = 0; i < ARRAY_SIZE(predivs); i++) {
>> +		sensor->pll_prediv = predivs[i];
>> +		if (sensor->ext_clock / sensor->pll_prediv < 12 * HZ_PER_MHZ)
>> +			break;
>> +	}
>> +
>> +	/* PLL output clock must be as close as possible to 804Mhz */
>> +	sensor->pll_mult = (VD56G3_TARGET_PLL * sensor->pll_prediv +
>> +			    sensor->ext_clock / 2) /
>> +			   sensor->ext_clock;
>> +	pll_out = sensor->ext_clock * sensor->pll_mult / sensor->pll_prediv;
>> +
>> +	/* Target Pixel Clock for standard 10bit ADC mode : 160.8Mhz */
>> +	sensor->pixel_clock = pll_out / VD56G3_VT_CLOCK_DIV;
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_detect(struct vd56g3 *sensor)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	struct device *dev = &client->dev;
>> +	unsigned int model;
>> +	u64 model_id = 0;
>> +	u64 device_revision = 0;
>> +	u64 optical_revision = 0;
>> +	int ret = 0;
>> +
>> +	model = (uintptr_t)device_get_match_data(dev);
>> +
>> +	cci_read(sensor->regmap, VD56G3_REG_MODEL_ID, &model_id, &ret);
>> +	if (ret)
>> +		return ret;
>> +
>> +	if (model_id != VD56G3_MODEL_ID) {
>> +		dev_warn(&client->dev, "Unsupported sensor id %x",
>> +			 (u16)model_id);
>> +		return -ENODEV;
>> +	}
>> +
>> +	cci_read(sensor->regmap, VD56G3_REG_REVISION, &device_revision, &ret);
>> +	if (ret)
>> +		return ret;
>> +
>> +	if ((device_revision >> 8) != 0x31) {
>> +		dev_warn(&client->dev, "Unsupported Cut version %x",
>> +			 (u16)device_revision);
>> +		return -ENODEV;
>> +	}
>> +
>> +	cci_read(sensor->regmap, VD56G3_REG_OPTICAL_REVISION, &optical_revision,
>> +		 &ret);
>> +	if (ret)
>> +		return ret;
>> +
>> +	sensor->is_mono =
>> +		((optical_revision & 1) == VD56G3_OPTICAL_REVISION_MONO);
>> +	if ((sensor->is_mono && model == VD56G3_MODEL_VD66GY) ||
>> +	    (!sensor->is_mono && model == VD56G3_MODEL_VD56G3)) {
>> +		dev_warn(&client->dev,
> 
> As you're returning an error, I'd use dev_err().

Correct, I'll also update the 2 upper dev_warn() - same situation.

> 
>> +			 "Found %s sensor, while %s model is defined in DT",
>> +			 (sensor->is_mono) ? "Mono" : "Bayer",
>> +			 (model == VD56G3_MODEL_VD56G3) ? "vd56g3" : "vd66gy");
>> +		return -ENODEV;
>> +	}
>> +
>> +	return 0;
>> +}
>> +
>> +static int vd56g3_subdev_init(struct vd56g3 *sensor)
>> +{
>> +	struct i2c_client *client = sensor->i2c_client;
>> +	int ret;
>> +
>> +	/* Init sub device */
>> +	v4l2_i2c_subdev_init(&sensor->sd, client, &vd56g3_subdev_ops);
>> +	sensor->sd.internal_ops = &vd56g3_internal_ops;
>> +	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
>> +			    V4L2_SUBDEV_FL_HAS_EVENTS;
>> +	sensor->sd.entity.ops = &vd56g3_subdev_entity_ops;
>> +
>> +	/* Init source pad */
>> +	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
>> +	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
>> +	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Failed to init media entity : %d", ret);
>> +		return ret;
>> +	}
>> +
>> +	/* Init controls */
>> +	ret = vd56g3_init_controls(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Controls initialization failed %d", ret);
>> +		goto err_media;
>> +	}
>> +
>> +	/* Init vd56g3 struct : default resolution + raw8 */
>> +	sensor->streaming = false;
> 
> sensor->streaming was already false (initialised to zero).

Elegant - however not very explicit (I hadn't even thought of it)

> 
>> +	sensor->sd.state_lock = sensor->ctrl_handler.lock;
>> +	ret = v4l2_subdev_init_finalize(&sensor->sd);
>> +	if (ret) {
>> +		dev_err(&client->dev, "subdev init error: %d", ret);
>> +		goto err_ctrls;
>> +	}
>> +
>> +	vd56g3_update_controls(sensor);
> 
> Error handling is missing.

Good point, I'll update it for V2.

> 
>> +
>> +	return 0;
>> +
>> +err_ctrls:
>> +	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
>> +
>> +err_media:
>> +	media_entity_cleanup(&sensor->sd.entity);
>> +	return ret;
>> +}
>> +
>> +static void vd56g3_subdev_cleanup(struct vd56g3 *sensor)
>> +{
>> +	v4l2_async_unregister_subdev(&sensor->sd);
>> +	v4l2_subdev_cleanup(&sensor->sd);
>> +	media_entity_cleanup(&sensor->sd.entity);
>> +	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
>> +}
>> +
>> +static int vd56g3_probe(struct i2c_client *client)
>> +{
>> +	struct device *dev = &client->dev;
>> +	struct vd56g3 *sensor;
>> +	int ret;
>> +
>> +	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
>> +	if (!sensor)
>> +		return -ENOMEM;
>> +
>> +	sensor->i2c_client = client;
>> +
>> +	ret = vd56g3_parse_dt(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Failed to parse Device Tree : %d", ret);
>> +		return ret;
>> +	}
>> +
>> +	/* Get (and check) resources : power regs, ext clock, reset gpio */
>> +	ret = vd56g3_get_regulators(sensor);
>> +	if (ret)
>> +		return dev_err_probe(dev, ret, "Failed to get regulators.");
>> +
>> +	sensor->xclk = devm_clk_get(dev, NULL);
>> +	if (IS_ERR(sensor->xclk))
>> +		return dev_err_probe(dev, PTR_ERR(sensor->xclk),
>> +				     "Failed to get xclk.");
>> +	sensor->ext_clock = clk_get_rate(sensor->xclk);
>> +	ret = vd56g3_prepare_clock_tree(sensor);
>> +	if (ret)
>> +		return ret;
>> +
>> +	sensor->reset_gpio =
>> +		devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
>> +	if (IS_ERR(sensor->reset_gpio))
>> +		return dev_err_probe(dev, PTR_ERR(sensor->reset_gpio),
>> +				     "Failed to get reset gpio.");
>> +
>> +	sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
>> +	if (IS_ERR(sensor->regmap))
>> +		return dev_err_probe(dev, PTR_ERR(sensor->regmap),
>> +				     "Failed to init regmap.");
>> +
>> +	/* Power ON */
>> +	ret = vd56g3_power_on(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Sensor power on failed : %d", ret);
>> +		return ret;
>> +	}
>> +
>> +	/* Enable PM runtime with autosuspend (sensor being ON, set active) */
>> +	pm_runtime_set_active(dev);
>> +	pm_runtime_get_noresume(dev);
>> +	pm_runtime_enable(dev);
>> +	pm_runtime_set_autosuspend_delay(dev, 1000);
>> +	pm_runtime_use_autosuspend(dev);
>> +
>> +	/* Check HW model/version */
>> +	ret = vd56g3_boot(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Sensor boot failed : %d", ret);
>> +		goto err_power_off;
>> +	}
>> +
>> +	ret = vd56g3_detect(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "Sensor detect failed : %d", ret);
>> +		goto err_power_off;
>> +	}
>> +
>> +	/* Initialize, then register V4L2 subdev */
>> +	ret = vd56g3_subdev_init(sensor);
>> +	if (ret) {
>> +		dev_err(&client->dev, "V4l2 init failed : %d", ret);
>> +		goto err_power_off;
>> +	}
>> +
>> +	ret = v4l2_async_register_subdev(&sensor->sd);
>> +	if (ret) {
>> +		dev_err(&client->dev, "async subdev register failed %d", ret);
>> +		goto err_subdev;
>> +	}
>> +
>> +	/* Sensor could now be powered off (after the autosuspend delay) */
>> +	pm_runtime_mark_last_busy(dev);
>> +	pm_runtime_put_autosuspend(dev);
>> +
>> +	dev_dbg(&client->dev, "Successfully probe %s sensor",
>> +		(sensor->is_mono) ? "vd56g3" : "vd66gy");
>> +
>> +	return 0;
>> +
>> +err_subdev:
>> +	vd56g3_subdev_cleanup(sensor);
>> +err_power_off:
>> +	pm_runtime_disable(dev);
>> +	pm_runtime_put_noidle(dev);
>> +	vd56g3_power_off(sensor);
>> +	return ret;
>> +}
>> +
>> +static void vd56g3_remove(struct i2c_client *client)
>> +{
>> +	struct v4l2_subdev *sd = i2c_get_clientdata(client);
>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>> +
>> +	vd56g3_subdev_cleanup(sensor);
>> +
>> +	pm_runtime_disable(&client->dev);
>> +	if (!pm_runtime_status_suspended(&client->dev))
>> +		vd56g3_power_off(sensor);
>> +	pm_runtime_set_suspended(&client->dev);
>> +}
>> +
>> +static const struct of_device_id vd56g3_dt_ids[] = {
>> +	{ .compatible = "st,st-vd56g3", .data = (void *)VD56G3_MODEL_VD56G3 },
>> +	{ .compatible = "st,st-vd66gy", .data = (void *)VD56G3_MODEL_VD66GY },
>> +	{ /* sentinel */ }
>> +};
>> +MODULE_DEVICE_TABLE(of, vd56g3_dt_ids);
>> +
>> +static struct i2c_driver vd56g3_i2c_driver = {
>> +	.driver = {
>> +		.name  = "st-vd56g3",
>> +		.of_match_table = vd56g3_dt_ids,
>> +		.pm = &vd56g3_pm_ops,
>> +	},
>> +	.probe = vd56g3_probe,
>> +	.remove = vd56g3_remove,
>> +};
>> +
>> +module_i2c_driver(vd56g3_i2c_driver);
>> +
>> +MODULE_AUTHOR("Benjamin Mugnier <[e-mail]>");
>> +MODULE_AUTHOR("Mickael Guene <[e-mail]>");
>> +MODULE_AUTHOR("Sylvain Petinot <[e-mail]>");
>> +MODULE_DESCRIPTION("ST VD56G3 sensor driver");
>> +MODULE_LICENSE("GPL");
> 

--
Sylvain

Tommaso Merciai May 6, 2024, 8:23 a.m. UTC | #6

Hi Sylvain,

On Fri, May 03, 2024 at 09:54:20AM +0200, Sylvain Petinot wrote:
> Hi Tommaso,
> 
> Thanks for the review !
> 
> On 4/18/2024 11:17 AM, Tommaso Merciai wrote:
> > Hi Sylvain,
> > 
> > Thanks for the patch.
> > Quick review on my side :)
> > 
> > Premise:
> > Unfortunately I'm not able to test this drv on my side (missing hw)
> > either I'm not able to get the datasheet Googling a bit.
> 
> Sure, we are on the way to make it available on mass market. Datasheets
> should be available soon for VD56G3 ([1]) and VD66GY ([2]).
> 
> [1] https://www.st.com/en/imaging-and-photonics-solutions/vd56g3.html
> [2] https://www.st.com/en/imaging-and-photonics-solutions/vd66gy.html

Thanks for sharing! :)

> 
> > 
> > On Wed, Apr 17, 2024 at 03:34:53PM +0200, Sylvain Petinot wrote:
> >> Add V4L2 sub-device driver for STMicroelectronics VD56G3 camera sensor.
> >> This is a 1.5 M pixel global shutter image sensor with an active array
> >> size of 1124 x 1364 (portrait orientation).
> >>
> >> The driver supports Mono (VD56G3) and Color (VD66GY) variants.
> >>
> >> Signed-off-by: Sylvain Petinot <[e-mail]>
> >> ---
> >>  drivers/media/i2c/Kconfig     |   11 +
> >>  drivers/media/i2c/Makefile    |    1 +
> >>  drivers/media/i2c/st-vd56g3.c | 1619 +++++++++++++++++++++++++++++++++
> >>  3 files changed, 1631 insertions(+)
> >>  create mode 100644 drivers/media/i2c/st-vd56g3.c
> >>
> >> diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
> >> index 56f276b920ab..a2d7271eef88 100644
> >> --- a/drivers/media/i2c/Kconfig
> >> +++ b/drivers/media/i2c/Kconfig
> >> @@ -657,6 +657,17 @@ config VIDEO_S5K6A3
> >>  	  This is a V4L2 sensor driver for Samsung S5K6A3 raw
> >>  	  camera sensor.
> >>  
> >> +config VIDEO_ST_VD56G3
> >> +	tristate "ST VD56G3 sensor support"
> >> +	select V4L2_CCI_I2C
> >> +	depends on OF && GPIOLIB
> >> +	help
> >> +	  This is a Video4Linux2 sensor driver for the ST VD56G3
> >> +	  camera sensor.
> >> +
> >> +	  To compile this driver as a module, choose M here: the
> >> +	  module will be called st-vd56g3.
> >> +
> >>  config VIDEO_ST_VGXY61
> >>  	tristate "ST VGXY61 sensor support"
> >>  	select V4L2_CCI_I2C
> >> diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
> >> index dfbe6448b549..b469cf0f8113 100644
> >> --- a/drivers/media/i2c/Makefile
> >> +++ b/drivers/media/i2c/Makefile
> >> @@ -124,6 +124,7 @@ obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
> >>  obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
> >>  obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
> >>  obj-$(CONFIG_VIDEO_ST_MIPID02) += st-mipid02.o
> >> +obj-$(CONFIG_VIDEO_ST_VD56G3) += st-vd56g3.o
> >>  obj-$(CONFIG_VIDEO_ST_VGXY61) += st-vgxy61.o
> >>  obj-$(CONFIG_VIDEO_TC358743) += tc358743.o
> >>  obj-$(CONFIG_VIDEO_TC358746) += tc358746.o
> >> diff --git a/drivers/media/i2c/st-vd56g3.c b/drivers/media/i2c/st-vd56g3.c
> >> new file mode 100644
> >> index 000000000000..eff819941456
> >> --- /dev/null
> >> +++ b/drivers/media/i2c/st-vd56g3.c
> >> @@ -0,0 +1,1619 @@
> >> +// SPDX-License-Identifier: GPL-2.0
> >> +/*
> >> + * A V4L2 driver for ST VD56G3 (Mono) and VD66GY (RGB) global shutter cameras.
> >> + * Copyright (C) 2024, STMicroelectronics SA
> >> + */
> >> +
> >> +#include <linux/clk.h>
> >> +#include <linux/delay.h>
> >> +#include <linux/gpio/consumer.h>
> >> +#include <linux/i2c.h>
> >> +#include <linux/iopoll.h>
> >> +#include <linux/module.h>
> >> +#include <linux/pm_runtime.h>
> >> +#include <linux/regmap.h>
> >> +#include <linux/regulator/consumer.h>
> >> +#include <linux/units.h>
> >> +
> >> +#include <asm/unaligned.h>
> >> +
> >> +#include <media/mipi-csi2.h>
> >> +#include <media/v4l2-async.h>
> >> +#include <media/v4l2-cci.h>
> >> +#include <media/v4l2-ctrls.h>
> >> +#include <media/v4l2-device.h>
> >> +#include <media/v4l2-event.h>
> >> +#include <media/v4l2-fwnode.h>
> >> +#include <media/v4l2-subdev.h>
> >> +
> >> +/* Register Map */
> >> +#define VD56G3_REG_MODEL_ID				CCI_REG16_LE(0x0000)
> >> +#define VD56G3_MODEL_ID					0x5603
> >> +#define VD56G3_REG_REVISION				CCI_REG16_LE(0x0002)
> >> +#define VD56G3_REG_OPTICAL_REVISION			CCI_REG8(0x001a)
> >> +#define VD56G3_OPTICAL_REVISION_MONO			0
> >> +#define VD56G3_OPTICAL_REVISION_BAYER			1
> >> +#define VD56G3_REG_SYSTEM_FSM				CCI_REG8(0x0028)
> >> +#define VD56G3_SYSTEM_FSM_READY_TO_BOOT			0x01
> >> +#define VD56G3_SYSTEM_FSM_SW_STBY			0x02
> >> +#define VD56G3_SYSTEM_FSM_STREAMING			0x03
> >> +#define VD56G3_REG_APPLIED_COARSE_EXPOSURE		CCI_REG16_LE(0x0064)
> >> +#define VD56G3_REG_APPLIED_ANALOG_GAIN			CCI_REG8(0x0068)
> >> +#define VD56G3_REG_APPLIED_DIGITAL_GAIN			CCI_REG16_LE(0x006a)
> >> +#define VD56G3_REG_BOOT					CCI_REG8(0x0200)
> >> +#define VD56G3_CMD_ACK					0
> >> +#define VD56G3_CMD_BOOT					1
> >> +#define VD56G3_REG_STBY					CCI_REG8(0x0201)
> >> +#define VD56G3_CMD_START_STREAM				1
> >> +#define VD56G3_REG_STREAMING				CCI_REG8(0x0202)
> >> +#define VD56G3_CMD_STOP_STREAM				1
> >> +#define VD56G3_REG_EXT_CLOCK				CCI_REG32_LE(0x0220)
> >> +#define VD56G3_REG_CLK_PLL_PREDIV			CCI_REG8(0x0224)
> >> +#define VD56G3_REG_CLK_SYS_PLL_MULT			CCI_REG8(0x0226)
> >> +#define VD56G3_REG_ORIENTATION				CCI_REG8(0x0302)
> >> +#define VD56G3_REG_FORMAT_CTRL				CCI_REG8(0x030a)
> >> +#define VD56G3_REG_OIF_CTRL				CCI_REG16_LE(0x030c)
> >> +#define VD56G3_REG_OIF_IMG_CTRL				CCI_REG8(0x030f)
> >> +#define VD56G3_REG_OIF_CSI_BITRATE			CCI_REG16_LE(0x0312)
> >> +#define VD56G3_REG_DUSTER_CTRL				CCI_REG8(0x0318)
> >> +#define VD56G3_DUSTER_DISABLE				0
> >> +#define VD56G3_DUSTER_ENABLE_DEF_MODULES		0x13
> >> +#define VD56G3_REG_ISL_ENABLE				CCI_REG8(0x0333)
> >> +#define VD56G3_REG_DARKCAL_CTRL				CCI_REG8(0x0340)
> >> +#define VD56G3_DARKCAL_ENABLE				1
> >> +#define VD56G3_DARKCAL_DISABLE_DARKAVG			2
> >> +#define VD56G3_REG_PATGEN_CTRL				CCI_REG16_LE(0x0400)
> >> +#define VD56G3_PATGEN_ENABLE				1
> >> +#define VD56G3_PATGEN_TYPE_SHIFT			4
> >> +#define VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE		CCI_REG16_LE(0x042a)
> >> +#define VD56G3_REG_AE_COLDSTART_ANALOG_GAIN		CCI_REG8(0x042c)
> >> +#define VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN		CCI_REG16_LE(0x042e)
> >> +#define VD56G3_REG_AE_ROI_START_H			CCI_REG16_LE(0x0432)
> >> +#define VD56G3_REG_AE_ROI_START_V			CCI_REG16_LE(0x0434)
> >> +#define VD56G3_REG_AE_ROI_END_H				CCI_REG16_LE(0x0436)
> >> +#define VD56G3_REG_AE_ROI_END_V				CCI_REG16_LE(0x0438)
> >> +#define VD56G3_REG_AE_COMPENSATION			CCI_REG16_LE(0x043a)
> >> +#define VD56G3_REG_EXP_MODE				CCI_REG8(0x044c)
> >> +#define VD56G3_EXP_MODE_AUTO				0
> >> +#define VD56G3_EXP_MODE_FREEZE				1
> >> +#define VD56G3_EXP_MODE_MANUAL				2
> >> +#define VD56G3_REG_MANUAL_ANALOG_GAIN			CCI_REG8(0x044d)
> >> +#define VD56G3_REG_MANUAL_COARSE_EXPOSURE		CCI_REG16_LE(0x044e)
> >> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0		CCI_REG16_LE(0x0450)
> >> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1		CCI_REG16_LE(0x0452)
> >> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2		CCI_REG16_LE(0x0454)
> >> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3		CCI_REG16_LE(0x0456)
> >> +#define VD56G3_REG_FRAME_LENGTH				CCI_REG16_LE(0x0458)
> >> +#define VD56G3_REG_Y_START				CCI_REG16_LE(0x045a)
> >> +#define VD56G3_REG_Y_END				CCI_REG16_LE(0x045c)
> >> +#define VD56G3_REG_OUT_ROI_X_START			CCI_REG16_LE(0x045e)
> >> +#define VD56G3_REG_OUT_ROI_X_END			CCI_REG16_LE(0x0460)
> >> +#define VD56G3_REG_OUT_ROI_Y_START			CCI_REG16_LE(0x0462)
> >> +#define VD56G3_REG_OUT_ROI_Y_END			CCI_REG16_LE(0x0464)
> >> +#define VD56G3_REG_GPIO_0_CTRL				CCI_REG8(0x0467)
> >> +#define VD56G3_GPIOX_GPIO_IN				0x01
> >> +#define VD56G3_GPIOX_STROBE_MODE			0x02
> >> +#define VD56G3_REG_READOUT_CTRL				CCI_REG8(0x047e)
> >> +#define READOUT_NORMAL					0x00
> >> +#define READOUT_DIGITAL_BINNING_X2			0x01
> >> +
> >> +/*
> >> + * The VD56G3 pixel array is organized as follows:
> >> + *
> >> + * +--------------------------------+
> >> + * |                                | \
> >> + * |   +------------------------+   |  |
> >> + * |   |                        |   |  |
> >> + * |   |                        |   |  |
> >> + * |   |                        |   |  |
> >> + * |   |                        |   |  |
> >> + * |   |                        |   |  |
> >> + * |   |   Default resolution   |   |  | Native height (1364)
> >> + * |   |      1120 x 1360       |   |  |
> >> + * |   |                        |   |  |
> >> + * |   |                        |   |  |
> >> + * |   |                        |   |  |
> >> + * |   |                        |   |  |
> >> + * |   +------------------------+   |  |
> >> + * |                                | /
> >> + * +--------------------------------+
> >> + *   <----------------------------->
> >> + *                     \-------------------  Native width (1124)
> >> + *
> >> + * The native resolution is 1124x1364.
> >> + * The recommended/default resolution is 1120x1360 (multiple of 16).
> >> + */
> >> +#define VD56G3_NATIVE_WIDTH				1124
> >> +#define VD56G3_NATIVE_HEIGHT				1364
> >> +#define VD56G3_DEFAULT_WIDTH				1120
> >> +#define VD56G3_DEFAULT_HEIGHT				1360
> >> +#define VD56G3_DEFAULT_MODE				1
> >> +
> >> +/* PLL settings */
> >> +#define VD56G3_TARGET_PLL				804000000UL
> >> +#define VD56G3_VT_CLOCK_DIV				5
> >> +
> >> +/* Line length and Frame length (settings are for standard 10bits ADC mode) */
> >> +#define VD56G3_LINE_LENGTH_MIN				1236
> >> +#define VD56G3_VBLANK_MIN				110
> >> +#define VD56G3_FRAME_LENGTH_DEF_60FPS			2168
> >> +
> >> +/* Exposure settings */
> >> +#define VD56G3_EXPOSURE_MARGIN				75
> >> +#define VD56G3_EXPOSURE_DEFAULT				1420
> >> +
> >> +/* Output Interface settings */
> >> +#define VD56G3_MAX_CSI_DATA_LANES			2
> >> +#define VD56G3_LINK_FREQ_DEF_1LANE			750000000UL
> >> +#define VD56G3_LINK_FREQ_DEF_2LANES			402000000UL
> >> +
> >> +/* GPIOs */
> >> +#define VD56G3_NB_GPIOS					8
> >> +
> >> +/* regulator supplies */
> >> +static const char *const vd56g3_supply_names[] = {
> >> +	"VCORE",
> >> +	"VDDIO",
> >> +	"VANA",
> >> +};
> >> +
> >> +#define VD56G3_NUM_SUPPLIES		ARRAY_SIZE(vd56g3_supply_names)
> >> +
> >> +/* -----------------------------------------------------------------------------
> >> + * Models (VD56G3: Mono, VD66GY: Bayer RGB), Modes and formats
> >> + */
> >> +
> >> +enum vd56g3_models {
> >> +	VD56G3_MODEL_VD56G3,
> >> +	VD56G3_MODEL_VD66GY,
> >> +};
> >> +
> >> +struct vd56g3_mode {
> >> +	u32 width;
> >> +	u32 height;
> >> +};
> >> +
> >> +static const struct vd56g3_mode vd56g3_supported_modes[] = {
> >> +	{
> >> +		.width = VD56G3_NATIVE_WIDTH,
> >> +		.height = VD56G3_NATIVE_HEIGHT,
> >> +	},
> >> +	{
> >> +		.width = VD56G3_DEFAULT_WIDTH,
> >> +		.height = VD56G3_DEFAULT_HEIGHT,
> >> +	},
> >> +	{
> >> +		.width = 1024,
> >> +		.height = 1280,
> >> +	},
> >> +	{
> >> +		.width = 1024,
> >> +		.height = 768,
> >> +	},
> >> +	{
> >> +		.width = 768,
> >> +		.height = 1024,
> >> +	},
> >> +	{
> >> +		.width = 720,
> >> +		.height = 1280,
> >> +	},
> >> +	{
> >> +		.width = 640,
> >> +		.height = 480,
> >> +	},
> >> +	{
> >> +		.width = 480,
> >> +		.height = 640,
> >> +	},
> >> +	{
> >> +		.width = 320,
> >> +		.height = 240,
> >> +	},
> >> +};
> >> +
> >> +/*
> >> + * Sensor support 8bits and 10bits output in both variants
> >> + *  - Monochrome
> >> + *  - RGB (with all H/V flip variations)
> >> + */
> >> +static const unsigned int vd56g3_mbus_codes[2][5] = {
> >> +	{
> >> +		MEDIA_BUS_FMT_Y8_1X8,
> >> +		MEDIA_BUS_FMT_SGRBG8_1X8,
> >> +		MEDIA_BUS_FMT_SRGGB8_1X8,
> >> +		MEDIA_BUS_FMT_SBGGR8_1X8,
> >> +		MEDIA_BUS_FMT_SGBRG8_1X8,
> >> +	},
> >> +	{
> >> +		MEDIA_BUS_FMT_Y10_1X10,
> >> +		MEDIA_BUS_FMT_SGRBG10_1X10,
> >> +		MEDIA_BUS_FMT_SRGGB10_1X10,
> >> +		MEDIA_BUS_FMT_SBGGR10_1X10,
> >> +		MEDIA_BUS_FMT_SGBRG10_1X10,
> >> +	},
> >> +};
> >> +
> >> +enum vd56g3_expo_state {
> >> +	VD56G3_EXPO_AUTO,
> >> +	VD56G3_EXPO_AUTO_FREEZE,
> >> +	VD56G3_EXPO_MANUAL
> >> +};
> >> +
> >> +struct vd56g3 {
> >> +	struct i2c_client *i2c_client;
> >> +	struct v4l2_subdev sd;
> >> +	struct media_pad pad;
> >> +	struct regulator_bulk_data supplies[VD56G3_NUM_SUPPLIES];
> >> +	struct gpio_desc *reset_gpio;
> >> +	struct clk *xclk;
> >> +	struct regmap *regmap;
> >> +	u32 ext_clock;
> >> +	u32 pll_prediv;
> >> +	u32 pll_mult;
> >> +	u32 pixel_clock;
> >> +	u16 oif_ctrl;
> >> +	int nb_of_lane;
> > 
> > I think you can use u8 for nb_of_lane;
> 
> Yes, better !
> Applied for V2.
> 
> > 
> >> +	u32 gpios[VD56G3_NB_GPIOS];
> >> +	unsigned long ext_leds_mask;
> >> +	bool is_mono;
> >> +	struct v4l2_ctrl_handler ctrl_handler;
> >> +	struct v4l2_ctrl *hblank_ctrl;
> >> +	struct v4l2_ctrl *vblank_ctrl;
> >> +	struct {
> >> +		struct v4l2_ctrl *hflip_ctrl;
> >> +		struct v4l2_ctrl *vflip_ctrl;
> >> +	};
> >> +	struct v4l2_ctrl *patgen_ctrl;
> >> +	struct {
> >> +		struct v4l2_ctrl *ae_ctrl;
> >> +		struct v4l2_ctrl *expo_ctrl;
> >> +		struct v4l2_ctrl *again_ctrl;
> >> +		struct v4l2_ctrl *dgain_ctrl;
> >> +	};
> >> +	struct v4l2_ctrl *ae_lock_ctrl;
> >> +	struct v4l2_ctrl *ae_bias_ctrl;
> >> +	struct v4l2_ctrl *led_ctrl;
> >> +	bool streaming;
> >> +};
> >> +
> >> +static inline struct vd56g3 *to_vd56g3(struct v4l2_subdev *sd)
> >> +{
> >> +	return container_of(sd, struct vd56g3, sd);
> > 
> > I think here you can use container_of_const
> 
> Better. I didn't know about it.
> 
> > 
> >> +}
> >> +
> >> +static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
> >> +{
> >> +	return &container_of(ctrl->handler, struct vd56g3, ctrl_handler)->sd;
> > 
> > Same here.
> 
> dito
> 
> > 
> >> +}
> >> +
> >> +/* -----------------------------------------------------------------------------
> >> + * Additional i2c register helpers
> >> + */
> >> +
> >> +static int vd56g3_poll_reg(struct vd56g3 *sensor, u32 reg, u8 poll_val,
> >> +			   int *err)
> >> +{
> >> +	unsigned int val = 0;
> >> +	int ret;
> >> +
> >> +	if (err && *err)
> >> +		return *err;
> >> +
> >> +	reg = reg & CCI_REG_ADDR_MASK;
> >> +	ret = regmap_read_poll_timeout(sensor->regmap, reg, val,
> >> +				       (val == poll_val), 2000,
> >> +				       500 * USEC_PER_MSEC);
> >> +
> >> +	if (ret && err)
> >> +		*err = ret;
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_wait_state(struct vd56g3 *sensor, int state, int *err)
> >> +{
> >> +	return vd56g3_poll_reg(sensor, VD56G3_REG_SYSTEM_FSM, state, err);
> >> +}
> >> +
> >> +/* -----------------------------------------------------------------------------
> >> + * Controls: definitions, helpers and handlers
> >> + */
> >> +
> >> +static const char *const vd56g3_tp_menu[] = { "Disabled", "Solid", "Colorbar",
> >> +					      "Gradbar",  "Hgrey", "Vgrey",
> >> +					      "Dgrey",	  "PN28" };
> >> +
> >> +static const s64 vd56g3_ev_bias_qmenu[] = { -4000, -3500, -3000, -2500, -2000,
> >> +					    -1500, -1000, -500,	 0,	500,
> >> +					    1000,  1500,  2000,	 2500,	3000,
> >> +					    3500,  4000 };
> >> +
> >> +static const s64 vd56g3_link_freq_1lane[] = { VD56G3_LINK_FREQ_DEF_1LANE };
> >> +
> >> +static const s64 vd56g3_link_freq_2lanes[] = { VD56G3_LINK_FREQ_DEF_2LANES };
> >> +
> >> +static u8 vd56g3_get_bpp(__u32 code)
> >> +{
> >> +	switch (code) {
> >> +	case MEDIA_BUS_FMT_Y8_1X8:
> >> +	case MEDIA_BUS_FMT_SGRBG8_1X8:
> >> +	case MEDIA_BUS_FMT_SRGGB8_1X8:
> >> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
> >> +	case MEDIA_BUS_FMT_SGBRG8_1X8:
> >> +		return 8;
> >> +	case MEDIA_BUS_FMT_Y10_1X10:
> >> +	case MEDIA_BUS_FMT_SGRBG10_1X10:
> >> +	case MEDIA_BUS_FMT_SRGGB10_1X10:
> >> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
> >> +	case MEDIA_BUS_FMT_SGBRG10_1X10:
> >> +		return 10;
> >> +	default:
> >> +		/* Should never happen */
> >> +		WARN(1, "Unsupported code %d. default to 8 bpp", code);
> >> +	}
> >> +
> >> +	return 8;
> >> +}
> >> +
> >> +static u8 vd56g3_get_datatype(__u32 code)
> >> +{
> >> +	switch (code) {
> >> +	case MEDIA_BUS_FMT_Y8_1X8:
> >> +	case MEDIA_BUS_FMT_SGRBG8_1X8:
> >> +	case MEDIA_BUS_FMT_SRGGB8_1X8:
> >> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
> >> +	case MEDIA_BUS_FMT_SGBRG8_1X8:
> >> +		return MIPI_CSI2_DT_RAW8;
> >> +	case MEDIA_BUS_FMT_Y10_1X10:
> >> +	case MEDIA_BUS_FMT_SGRBG10_1X10:
> >> +	case MEDIA_BUS_FMT_SRGGB10_1X10:
> >> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
> >> +	case MEDIA_BUS_FMT_SGBRG10_1X10:
> >> +		return MIPI_CSI2_DT_RAW10;
> >> +	default:
> >> +		/* Should never happen */
> >> +		WARN(1, "Unsupported code %d. default to MIPI_CSI2_DT_RAW8",
> >> +		     code);
> >> +	}
> >> +
> >> +	return MIPI_CSI2_DT_RAW8;
> >> +}
> >> +
> >> +static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
> >> +{
> >> +	u64 exposure = 0;
> >> +	u64 again = 0;
> >> +	u64 dgain = 0;
> >> +	int ret = 0;
> >> +
> >> +	/*
> >> +	 * When 'force_cur_val' is enabled, save the ctrl value in 'cur.val'
> >> +	 * instead of the normal 'val', this is used during poweroff to cache
> >> +	 * volatile ctrls and enable coldstart.
> >> +	 */
> >> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_COARSE_EXPOSURE, &exposure,
> >> +		 &ret);
> >> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_ANALOG_GAIN, &again, &ret);
> >> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_DIGITAL_GAIN, &dgain, &ret);
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	if (force_cur_val) {
> >> +		sensor->expo_ctrl->cur.val = exposure;
> >> +		sensor->again_ctrl->cur.val = again;
> >> +		sensor->dgain_ctrl->cur.val = dgain;
> >> +	} else {
> >> +		sensor->expo_ctrl->val = exposure;
> >> +		sensor->again_ctrl->val = again;
> >> +		sensor->dgain_ctrl->val = dgain;
> >> +	}
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_update_patgen(struct vd56g3 *sensor, u32 patgen_index)
> >> +{
> >> +	u32 pattern = patgen_index <= 3 ? patgen_index : patgen_index + 12;
> >> +	u16 patgen = pattern << VD56G3_PATGEN_TYPE_SHIFT;
> >> +	u8 duster = VD56G3_DUSTER_ENABLE_DEF_MODULES;
> >> +	u8 darkcal = VD56G3_DARKCAL_ENABLE;
> >> +	int ret = 0;
> >> +
> >> +	if (patgen_index) {
> >> +		patgen |= VD56G3_PATGEN_ENABLE;
> >> +		duster = VD56G3_DUSTER_DISABLE;
> >> +		darkcal = VD56G3_DARKCAL_DISABLE_DARKAVG;
> >> +	}
> >> +
> >> +	cci_write(sensor->regmap, VD56G3_REG_DUSTER_CTRL, duster, &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_DARKCAL_CTRL, darkcal, &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_PATGEN_CTRL, patgen, &ret);
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_update_expo_cluster(struct vd56g3 *sensor, bool is_auto)
> >> +{
> >> +	int ret = 0;
> >> +	enum vd56g3_expo_state expo_state = is_auto ? VD56G3_EXP_MODE_AUTO :
> >> +						      VD56G3_EXP_MODE_MANUAL;
> >> +
> >> +	if (sensor->ae_ctrl->is_new)
> >> +		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
> >> +			  &ret);
> >> +
> >> +	/* In Auto expo, set coldstart parameters */
> >> +	if (is_auto && sensor->ae_ctrl->is_new) {
> >> +		cci_write(sensor->regmap,
> >> +			  VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE,
> >> +			  sensor->expo_ctrl->val, &ret);
> >> +		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_ANALOG_GAIN,
> >> +			  sensor->again_ctrl->val, &ret);
> >> +		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN,
> >> +			  sensor->dgain_ctrl->val, &ret);
> >> +	}
> >> +
> >> +	/* In Manual expo, set exposure, analog and digital gains */
> >> +	if (!is_auto && sensor->expo_ctrl->is_new)
> >> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_COARSE_EXPOSURE,
> >> +			  sensor->expo_ctrl->val, &ret);
> >> +
> >> +	if (!is_auto && sensor->again_ctrl->is_new)
> >> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_ANALOG_GAIN,
> >> +			  sensor->again_ctrl->val, &ret);
> >> +
> >> +	if (!is_auto && sensor->dgain_ctrl->is_new) {
> >> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0,
> >> +			  sensor->dgain_ctrl->val, &ret);
> >> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1,
> >> +			  sensor->dgain_ctrl->val, &ret);
> >> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2,
> >> +			  sensor->dgain_ctrl->val, &ret);
> >> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3,
> >> +			  sensor->dgain_ctrl->val, &ret);
> >> +	}
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_lock_exposure(struct vd56g3 *sensor, u32 lock_val)
> >> +{
> >> +	bool ae_lock = lock_val & V4L2_LOCK_EXPOSURE;
> >> +	enum vd56g3_expo_state expo_state = ae_lock ? VD56G3_EXP_MODE_FREEZE :
> >> +						      VD56G3_EXP_MODE_AUTO;
> >> +	int ret = 0;
> >> +
> >> +	if (sensor->ae_ctrl->val == V4L2_EXPOSURE_AUTO)
> >> +		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
> >> +			  &ret);
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_write_gpiox(struct vd56g3 *sensor, unsigned long gpio_mask)
> >> +{
> >> +	unsigned long io;
> >> +	u32 gpio_val;
> >> +	int ret = 0;
> >> +
> >> +	for_each_set_bit(io, &gpio_mask, VD56G3_NB_GPIOS) {
> >> +		gpio_val = sensor->gpios[io];
> >> +
> >> +		if (gpio_val == VD56G3_GPIOX_STROBE_MODE &&
> >> +		    sensor->led_ctrl->val == V4L2_FLASH_LED_MODE_NONE)
> >> +			gpio_val = VD56G3_GPIOX_GPIO_IN;
> >> +
> >> +		cci_write(sensor->regmap, VD56G3_REG_GPIO_0_CTRL + io, gpio_val,
> >> +			  &ret);
> >> +	}
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
> >> +{
> >> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
> >> +	struct vd56g3 *sensor = to_vd56g3(sd);
> >> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
> >> +	int ret = 0;
> >> +
> >> +	/* Interact with HW only when it is powered ON */
> >> +	if (!pm_runtime_get_if_in_use(&client->dev))
> >> +		return 0;
> >> +
> >> +	switch (ctrl->id) {
> >> +	case V4L2_CID_EXPOSURE_AUTO:
> >> +		ret = vd56g3_read_expo_cluster(sensor, false);
> >> +		break;
> >> +	default:
> >> +		ret = -EINVAL;
> >> +		break;
> >> +	}
> >> +
> >> +	pm_runtime_mark_last_busy(&client->dev);
> >> +	pm_runtime_put_autosuspend(&client->dev);
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_s_ctrl(struct v4l2_ctrl *ctrl)
> >> +{
> >> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
> >> +	struct vd56g3 *sensor = to_vd56g3(sd);
> >> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
> >> +	const struct v4l2_rect *crop;
> >> +	unsigned int frame_length = 0;
> >> +	unsigned int expo_max;
> >> +	unsigned int ae_compensation;
> >> +	bool is_auto = false;
> >> +	int ret;
> >> +
> >> +	struct v4l2_subdev_state *state =
> >> +		v4l2_subdev_get_locked_active_state(sd);
> >> +	crop = v4l2_subdev_state_get_crop(state, 0);
> >> +
> >> +	if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
> >> +		return 0;
> >> +
> >> +	/* Update controls state, range, etc. whatever the state of the HW */
> >> +	switch (ctrl->id) {
> >> +	case V4L2_CID_VBLANK:
> >> +		frame_length = crop->height + ctrl->val;
> >> +		expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
> >> +		__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
> >> +					 VD56G3_EXPOSURE_DEFAULT);
> >> +		break;
> >> +	case V4L2_CID_EXPOSURE_AUTO:
> >> +		is_auto = (ctrl->val == V4L2_EXPOSURE_AUTO);
> >> +		__v4l2_ctrl_grab(sensor->ae_lock_ctrl, !is_auto);
> >> +		__v4l2_ctrl_grab(sensor->ae_bias_ctrl, !is_auto);
> >> +		break;
> >> +	default:
> >> +		break;
> >> +	}
> >> +
> >> +	/* Interact with HW only when it is powered ON */
> >> +	if (!pm_runtime_get_if_in_use(&client->dev))
> >> +		return 0;
> >> +
> >> +	switch (ctrl->id) {
> >> +	case V4L2_CID_HFLIP:
> >> +		ret = cci_write(sensor->regmap, VD56G3_REG_ORIENTATION,
> >> +				sensor->hflip_ctrl->val |
> >> +					(sensor->vflip_ctrl->val << 1),
> >> +				NULL);
> >> +		break;
> >> +	case V4L2_CID_TEST_PATTERN:
> >> +		ret = vd56g3_update_patgen(sensor, ctrl->val);
> >> +		break;
> >> +	case V4L2_CID_EXPOSURE_AUTO:
> >> +		ret = vd56g3_update_expo_cluster(sensor, is_auto);
> >> +		break;
> >> +	case V4L2_CID_3A_LOCK:
> >> +		ret = vd56g3_lock_exposure(sensor, ctrl->val);
> >> +		break;
> >> +	case V4L2_CID_AUTO_EXPOSURE_BIAS:
> >> +		ae_compensation =
> >> +			DIV_ROUND_CLOSEST((int)vd56g3_ev_bias_qmenu[ctrl->val] *
> >> +					  256, 1000);
> >> +		ret = cci_write(sensor->regmap, VD56G3_REG_AE_COMPENSATION,
> >> +				ae_compensation, NULL);
> >> +		break;
> >> +	case V4L2_CID_VBLANK:
> >> +		ret = cci_write(sensor->regmap, VD56G3_REG_FRAME_LENGTH,
> >> +				frame_length, NULL);
> >> +		break;
> >> +	case V4L2_CID_FLASH_LED_MODE:
> >> +		ret = vd56g3_write_gpiox(sensor, sensor->ext_leds_mask);
> >> +		break;
> >> +	default:
> >> +		ret = -EINVAL;
> >> +		break;
> >> +	}
> >> +
> >> +	pm_runtime_mark_last_busy(&client->dev);
> >> +	pm_runtime_put_autosuspend(&client->dev);
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static const struct v4l2_ctrl_ops vd56g3_ctrl_ops = {
> >> +	.g_volatile_ctrl = vd56g3_g_volatile_ctrl,
> >> +	.s_ctrl = vd56g3_s_ctrl,
> >> +};
> >> +
> >> +static void vd56g3_update_controls(struct vd56g3 *sensor)
> >> +{
> >> +	const struct v4l2_rect *crop;
> >> +	unsigned int hblank;
> >> +	unsigned int vblank_min, vblank, vblank_max;
> >> +	unsigned int frame_length;
> >> +	unsigned int expo_max;
> >> +
> >> +	struct v4l2_subdev_state *state =
> >> +		v4l2_subdev_get_locked_active_state(&sensor->sd);
> >> +	crop = v4l2_subdev_state_get_crop(state, 0);
> >> +
> >> +	hblank = VD56G3_LINE_LENGTH_MIN - crop->width;
> >> +	vblank_min = VD56G3_VBLANK_MIN;
> >> +	vblank = VD56G3_FRAME_LENGTH_DEF_60FPS - crop->height;
> >> +	vblank_max = 0xffff - crop->height;
> >> +	frame_length = crop->height + vblank;
> >> +	expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
> >> +
> > 
> > --
> > 
> >> +	/* Update blanking and exposure (ranges + values) */
> >> +	__v4l2_ctrl_modify_range(sensor->hblank_ctrl, hblank, hblank, 1,
> >> +				 hblank);
> >> +	__v4l2_ctrl_modify_range(sensor->vblank_ctrl, vblank_min, vblank_max, 1,
> >> +				 vblank);
> >> +	__v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, vblank);
> >> +	__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
> >> +				 VD56G3_EXPOSURE_DEFAULT);
> >> +	__v4l2_ctrl_s_ctrl(sensor->expo_ctrl, VD56G3_EXPOSURE_DEFAULT);
> > 
> > Here you don't have to check the return value fo the __v4l2__ctrl
> > functions using a ret value? Or I'm completely wrong?
> > 
> 
> Well good question... Honestly I didn't really give it much thought (I
> took inspiration from the imx219 and imx290 drivers which do not have
> the return test).
> But Sakari made the same kind of comment, so I definitely have to check
> how to handle this properly.
> 
> > Please check drivers/media/i2c/imx334.c:624
> > imx334_update_controls
> > 
> >> +}
> >> +
> >> +static int vd56g3_init_controls(struct vd56g3 *sensor)
> >> +{
> >> +	const struct v4l2_ctrl_ops *ops = &vd56g3_ctrl_ops;
> >> +	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
> >> +	struct v4l2_ctrl *ctrl;
> >> +	int ret;
> >> +
> >> +	v4l2_ctrl_handler_init(hdl, 25);
> >> +
> >> +	/* Horizontal & vertical flips modify bayer code on RGB variant */
> >> +	sensor->hflip_ctrl =
> >> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
> >> +	if (sensor->hflip_ctrl)
> >> +		sensor->hflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
> >> +
> >> +	sensor->vflip_ctrl =
> >> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
> >> +	if (sensor->vflip_ctrl)
> >> +		sensor->vflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
> >> +
> >> +	sensor->patgen_ctrl =
> >> +		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
> >> +					     ARRAY_SIZE(vd56g3_tp_menu) - 1, 0,
> >> +					     0, vd56g3_tp_menu);
> >> +
> >> +	ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
> >> +				      ARRAY_SIZE(vd56g3_link_freq_1lane) - 1, 0,
> >> +				      (sensor->nb_of_lane == 2) ?
> >> +					      vd56g3_link_freq_2lanes :
> >> +					      vd56g3_link_freq_1lane);
> >> +	if (ctrl)
> >> +		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
> >> +
> >> +	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
> >> +				 sensor->pixel_clock, sensor->pixel_clock, 1,
> >> +				 sensor->pixel_clock);
> > 
> > I personally prefer:
> > 	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
> > 				 sensor->pixel_clock,
> > 				 sensor->pixel_clock, 1,
> > 				 sensor->pixel_clock);
> 
> In fact this is my .clang-format who formatted it that way.
> Your proposal is more elegant to the eye.
> 
> > 
> > up to you :)
> > 
> >> +	if (ctrl)
> >> +		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
> >> +
> >> +	sensor->ae_ctrl = v4l2_ctrl_new_std_menu(hdl, ops,
> >> +						 V4L2_CID_EXPOSURE_AUTO,
> >> +						 V4L2_EXPOSURE_MANUAL, 0,
> >> +						 V4L2_EXPOSURE_AUTO);
> >> +
> >> +	sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK, 0,
> >> +						 GENMASK(2, 0), 0, 0);
> >> +
> >> +	sensor->ae_bias_ctrl =
> >> +		v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
> >> +				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) - 1,
> >> +				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) / 2,
> >> +				       vd56g3_ev_bias_qmenu);
> >> +
> >> +	/*
> >> +	 * Analog gain [1, 8] is computed with the following logic :
> >> +	 * 32/(32 - again_reg), with again_reg in the range [0:28]
> >> +	 * Digital gain [1.00, 8.00] is coded as a Fixed Point 5.8
> >> +	 */
> >> +	sensor->again_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
> >> +					       0, 28, 1, 0);
> >> +	sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
> >> +					       0x100, 0x800, 1, 0x100);
> >> +
> >> +	/*
> >> +	 * Set the exposure, horizontal and vertical blanking ctrls
> >> +	 * to hardcoded values, they will be updated in vd56g3_update_controls.
> >> +	 * Exposure being in an auto-cluster, set a significant value here.
> >> +	 */
> >> +	sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
> >> +					      VD56G3_EXPOSURE_DEFAULT,
> >> +					      VD56G3_EXPOSURE_DEFAULT, 1,
> >> +					      VD56G3_EXPOSURE_DEFAULT);
> >> +	sensor->hblank_ctrl =
> >> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 1, 1, 1, 1);
> >> +	if (sensor->hblank_ctrl)
> >> +		sensor->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
> >> +	sensor->vblank_ctrl =
> >> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, 1, 1, 1, 1);
> > 
> > Doubt on my side:
> > 
> > Here you flag hblank as read only. What about vblank?
> > Thanks in advance.
> 
> Yes hblank is read only while vblank is writable.
> Vblank is adjustable to allow different framerates.

Thanks for clarify.

> 
> > 
> >> +
> >> +	/* Additional control based on device tree properties */
> >> +	if (sensor->ext_leds_mask)
> >> +		sensor->led_ctrl =
> >> +			v4l2_ctrl_new_std_menu(hdl, ops,
> >> +					       V4L2_CID_FLASH_LED_MODE,
> >> +					       V4L2_FLASH_LED_MODE_FLASH, 0,
> >> +					       V4L2_FLASH_LED_MODE_NONE);
> >> +
> >> +	if (hdl->error) {
> >> +		ret = hdl->error;
> >> +		goto free_ctrls;
> >> +	}
> >> +
> >> +	v4l2_ctrl_cluster(2, &sensor->hflip_ctrl);
> >> +	v4l2_ctrl_auto_cluster(4, &sensor->ae_ctrl, V4L2_EXPOSURE_MANUAL, true);
> >> +
> >> +	sensor->sd.ctrl_handler = hdl;
> >> +	return 0;
> >> +
> >> +free_ctrls:
> >> +	v4l2_ctrl_handler_free(hdl);
> >> +	return ret;
> >> +}
> >> +
> >> +/* -----------------------------------------------------------------------------
> >> + * Videos ops
> >> + */
> >> +
> >> +static int vd56g3_stream_on(struct vd56g3 *sensor,
> >> +			    struct v4l2_subdev_state *state)
> >> +{
> >> +	const struct v4l2_mbus_framefmt *format =
> >> +		v4l2_subdev_state_get_format(state, 0);
> >> +	const struct v4l2_rect *crop =
> >> +		v4l2_subdev_state_get_crop(state, 0);
> >> +	int ret = 0;
> >> +	unsigned int csi_mbps = ((sensor->nb_of_lane == 2) ?
> >> +					 VD56G3_LINK_FREQ_DEF_2LANES :
> >> +					 VD56G3_LINK_FREQ_DEF_1LANE) * 2 / MEGA;
> >> +	unsigned int binning;
> >> +
> >> +	/* configure clocks */
> >> +	cci_write(sensor->regmap, VD56G3_REG_EXT_CLOCK, sensor->ext_clock,
> >> +		  &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_CLK_PLL_PREDIV, sensor->pll_prediv,
> >> +		  &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_CLK_SYS_PLL_MULT, sensor->pll_mult,
> >> +		  &ret);
> >> +
> >> +	/* configure output */
> >> +	cci_write(sensor->regmap, VD56G3_REG_FORMAT_CTRL,
> >> +		  vd56g3_get_bpp(format->code), &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_OIF_CSI_BITRATE, csi_mbps, &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_OIF_IMG_CTRL,
> >> +		  vd56g3_get_datatype(format->code), &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_ISL_ENABLE, 0, &ret);
> >> +
> >> +	/* configure binning mode */
> >> +	switch (crop->width / format->width) {
> >> +	case 1:
> >> +	default:
> >> +		binning = READOUT_NORMAL;
> >> +		break;
> >> +	case 2:
> >> +		binning = READOUT_DIGITAL_BINNING_X2;
> >> +		break;
> >> +	}
> >> +	cci_write(sensor->regmap, VD56G3_REG_READOUT_CTRL, binning, &ret);
> >> +
> >> +	/* configure ROIs */
> >> +	cci_write(sensor->regmap, VD56G3_REG_Y_START, crop->top, &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_Y_END,
> >> +		  crop->top + crop->height - 1, &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_START, crop->left, &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_END,
> >> +		  crop->left + crop->width - 1, &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_START, 0, &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_END, crop->height - 1,
> >> +		  &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_H, crop->left, &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_H,
> >> +		  crop->left + crop->width - 1, &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_V, 0, &ret);
> >> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_V, crop->height - 1,
> >> +		  &ret);
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	/* Setup default GPIO values; could be overridden by V4L2 ctrl setup */
> >> +	ret = vd56g3_write_gpiox(sensor, GENMASK(VD56G3_NB_GPIOS - 1, 0));
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	/* Apply settings from V4L2 ctrls */
> >> +	ret = __v4l2_ctrl_handler_setup(&sensor->ctrl_handler);
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	/* start streaming */
> >> +	cci_write(sensor->regmap, VD56G3_REG_STBY, VD56G3_CMD_START_STREAM,
> >> +		  &ret);
> >> +	vd56g3_poll_reg(sensor, VD56G3_REG_STBY, VD56G3_CMD_ACK, &ret);
> >> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_STREAMING, &ret);
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_stream_off(struct vd56g3 *sensor)
> >> +{
> >> +	int ret = 0;
> >> +
> >> +	/* Retrieve Expo cluster to enable coldstart of AE */
> >> +	ret = vd56g3_read_expo_cluster(sensor, true);
> >> +
> >> +	cci_write(sensor->regmap, VD56G3_REG_STREAMING, VD56G3_CMD_STOP_STREAM,
> >> +		  &ret);
> >> +	vd56g3_poll_reg(sensor, VD56G3_REG_STREAMING, VD56G3_CMD_ACK, &ret);
> >> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_s_stream(struct v4l2_subdev *sd, int enable)
> >> +{
> >> +	struct vd56g3 *sensor = to_vd56g3(sd);
> >> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
> >> +	int ret = 0;
> >> +
> >> +	struct v4l2_subdev_state *state =
> >> +		v4l2_subdev_lock_and_get_active_state(sd);
> >> +
> >> +	if (enable) {
> >> +		ret = pm_runtime_resume_and_get(&client->dev);
> >> +		if (ret < 0)
> >> +			goto unlock;
> >> +		ret = vd56g3_stream_on(sensor, state);
> >> +		if (ret) {
> >> +			dev_err(&client->dev, "Failed to start streaming\n");
> >> +			pm_runtime_put_sync(&client->dev);
> >> +		}
> >> +	} else {
> >> +		vd56g3_stream_off(sensor);
> >> +		pm_runtime_mark_last_busy(&client->dev);
> >> +		pm_runtime_put_autosuspend(&client->dev);
> >> +	}
> >> +
> >> +unlock:
> >> +	v4l2_subdev_unlock_state(state);
> >> +	if (!ret) {
> >> +		sensor->streaming = enable;
> >> +		v4l2_ctrl_grab(sensor->hflip_ctrl, enable);
> >> +		v4l2_ctrl_grab(sensor->vflip_ctrl, enable);
> >> +		v4l2_ctrl_grab(sensor->patgen_ctrl, enable);
> >> +	}
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static const struct v4l2_subdev_video_ops vd56g3_video_ops = {
> >> +	.s_stream = vd56g3_s_stream,
> >> +};
> >> +
> >> +/* -----------------------------------------------------------------------------
> >> + * Pad ops
> >> + */
> >> +
> >> +/* Media bus code is dependent of :
> >> + *      - 8bits or 10bits output
> >> + *      - variant : Mono or RGB
> >> + *      - H/V flips parameters in case of RGB
> >> + */
> >> +static u32 vd56g3_get_mbus_code(struct vd56g3 *sensor, u32 code)
> >> +{
> >> +	unsigned int i_bpp;
> >> +	unsigned int j;
> >> +
> >> +	for (i_bpp = 0; i_bpp < ARRAY_SIZE(vd56g3_mbus_codes); i_bpp++) {
> >> +		for (j = 0; j < ARRAY_SIZE(vd56g3_mbus_codes[i_bpp]); j++) {
> >> +			if (vd56g3_mbus_codes[i_bpp][j] == code)
> >> +				goto endloops;
> >> +		}
> >> +	}
> >> +
> >> +endloops:
> >> +	if (i_bpp >= ARRAY_SIZE(vd56g3_mbus_codes))
> >> +		i_bpp = 0;
> >> +
> >> +	if (sensor->is_mono)
> >> +		j = 0;
> >> +	else
> >> +		j = 1 + (sensor->hflip_ctrl->val ? 1 : 0) +
> >> +		    (sensor->vflip_ctrl->val ? 2 : 0);
> >> +
> >> +	return vd56g3_mbus_codes[i_bpp][j];
> >> +}
> >> +
> >> +static int vd56g3_enum_mbus_code(struct v4l2_subdev *sd,
> >> +				 struct v4l2_subdev_state *sd_state,
> >> +				 struct v4l2_subdev_mbus_code_enum *code)
> >> +{
> >> +	struct vd56g3 *sensor = to_vd56g3(sd);
> >> +
> >> +	if (code->index >= ARRAY_SIZE(vd56g3_mbus_codes))
> >> +		return -EINVAL;
> >> +
> >> +	code->code =
> >> +		vd56g3_get_mbus_code(sensor, vd56g3_mbus_codes[code->index][0]);
> >> +
> >> +	return 0;
> >> +}
> >> +
> >> +static int vd56g3_enum_frame_size(struct v4l2_subdev *sd,
> >> +				  struct v4l2_subdev_state *sd_state,
> >> +				  struct v4l2_subdev_frame_size_enum *fse)
> >> +{
> >> +	if (fse->index >= ARRAY_SIZE(vd56g3_supported_modes))
> >> +		return -EINVAL;
> >> +
> >> +	fse->min_width = vd56g3_supported_modes[fse->index].width;
> >> +	fse->max_width = fse->min_width;
> >> +	fse->min_height = vd56g3_supported_modes[fse->index].height;
> >> +	fse->max_height = fse->min_height;
> >> +
> >> +	return 0;
> >> +}
> >> +
> >> +static void vd56g3_update_img_pad_format(struct vd56g3 *sensor,
> >> +					 const struct vd56g3_mode *mode,
> >> +					 u32 mbus_code,
> >> +					 struct v4l2_mbus_framefmt *mbus_fmt)
> >> +{
> >> +	mbus_fmt->width = mode->width;
> >> +	mbus_fmt->height = mode->height;
> >> +	mbus_fmt->code = vd56g3_get_mbus_code(sensor, mbus_code);
> >> +	mbus_fmt->colorspace = V4L2_COLORSPACE_RAW;
> >> +	mbus_fmt->field = V4L2_FIELD_NONE;
> >> +	mbus_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
> >> +	mbus_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
> >> +	mbus_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
> >> +}
> >> +
> >> +static int vd56g3_set_pad_fmt(struct v4l2_subdev *sd,
> >> +			      struct v4l2_subdev_state *sd_state,
> >> +			      struct v4l2_subdev_format *sd_fmt)
> >> +{
> >> +	struct vd56g3 *sensor = to_vd56g3(sd);
> >> +	const struct vd56g3_mode *new_mode;
> >> +	struct v4l2_rect pad_crop;
> >> +	unsigned int binning;
> >> +
> >> +	if (sensor->streaming)
> >> +		return -EBUSY;
> >> +
> >> +	new_mode = v4l2_find_nearest_size(vd56g3_supported_modes,
> >> +					  ARRAY_SIZE(vd56g3_supported_modes),
> >> +					  width, height, sd_fmt->format.width,
> >> +					  sd_fmt->format.height);
> >> +
> >> +	vd56g3_update_img_pad_format(sensor, new_mode, sd_fmt->format.code,
> >> +				     &sd_fmt->format);
> >> +
> >> +	/* Compute crop rectangle (maximized via binning) */
> >> +	binning = min(VD56G3_NATIVE_WIDTH / sd_fmt->format.width,
> >> +		      VD56G3_NATIVE_HEIGHT / sd_fmt->format.height);
> >> +	binning = min(binning, 2U);
> >> +	pad_crop.width = sd_fmt->format.width * binning;
> >> +	pad_crop.height = sd_fmt->format.height * binning;
> >> +	pad_crop.left = (VD56G3_NATIVE_WIDTH - pad_crop.width) / 2;
> >> +	pad_crop.top = (VD56G3_NATIVE_HEIGHT - pad_crop.height) / 2;
> >> +
> >> +	/* Update active state's format and crop */
> >> +	*v4l2_subdev_state_get_format(sd_state, sd_fmt->pad) = sd_fmt->format;
> >> +	*v4l2_subdev_state_get_crop(sd_state, sd_fmt->pad) = pad_crop;
> >> +
> >> +	if (sd_fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
> >> +		vd56g3_update_controls(sensor);
> >> +
> >> +	return 0;
> >> +}
> >> +
> >> +static int vd56g3_get_selection(struct v4l2_subdev *sd,
> >> +				struct v4l2_subdev_state *sd_state,
> >> +				struct v4l2_subdev_selection *sel)
> >> +{
> >> +	switch (sel->target) {
> >> +	case V4L2_SEL_TGT_CROP:
> >> +		sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
> >> +		break;
> >> +	case V4L2_SEL_TGT_NATIVE_SIZE:
> >> +	case V4L2_SEL_TGT_CROP_DEFAULT:
> >> +	case V4L2_SEL_TGT_CROP_BOUNDS:
> >> +		sel->r.top = 0;
> >> +		sel->r.left = 0;
> >> +		sel->r.width = VD56G3_NATIVE_WIDTH;
> >> +		sel->r.height = VD56G3_NATIVE_HEIGHT;
> >> +		break;
> >> +	default:
> >> +		return -EINVAL;
> >> +	}
> >> +
> >> +	return 0;
> >> +}
> >> +
> >> +static int vd56g3_init_state(struct v4l2_subdev *sd,
> >> +			     struct v4l2_subdev_state *sd_state)
> >> +{
> >> +	unsigned int def_mode = VD56G3_DEFAULT_MODE;
> >> +	struct v4l2_subdev_format fmt = {
> >> +		.which = V4L2_SUBDEV_FORMAT_TRY,
> >> +		.pad = 0,
> >> +		.format = {
> >> +			.code = vd56g3_mbus_codes[0][0],
> >> +			.width = vd56g3_supported_modes[def_mode].width,
> >> +			.height = vd56g3_supported_modes[def_mode].height,
> >> +		},
> >> +	};
> >> +
> >> +	vd56g3_set_pad_fmt(sd, sd_state, &fmt);
> >> +	return 0;
> >> +}
> >> +
> >> +static const struct v4l2_subdev_core_ops vd56g3_core_ops = {
> >> +	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
> >> +	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
> >> +};
> >> +
> >> +static const struct v4l2_subdev_pad_ops vd56g3_pad_ops = {
> >> +	.enum_mbus_code = vd56g3_enum_mbus_code,
> >> +	.enum_frame_size = vd56g3_enum_frame_size,
> >> +	.get_fmt = v4l2_subdev_get_fmt,
> >> +	.set_fmt = vd56g3_set_pad_fmt,
> >> +	.get_selection = vd56g3_get_selection,
> >> +};
> >> +
> >> +static const struct v4l2_subdev_ops vd56g3_subdev_ops = {
> >> +	.core = &vd56g3_core_ops,
> >> +	.video = &vd56g3_video_ops,
> >> +	.pad = &vd56g3_pad_ops,
> >> +};
> >> +
> >> +static const struct media_entity_operations vd56g3_subdev_entity_ops = {
> >> +	.link_validate = v4l2_subdev_link_validate,
> >> +};
> >> +
> >> +static const struct v4l2_subdev_internal_ops vd56g3_internal_ops = {
> >> +	.init_state = vd56g3_init_state,
> >> +};
> >> +
> >> +/* -----------------------------------------------------------------------------
> >> + * Power management
> >> + */
> >> +
> >> +static int vd56g3_boot(struct vd56g3 *sensor)
> >> +{
> >> +	int ret = 0;
> >> +
> >> +	cci_write(sensor->regmap, VD56G3_REG_BOOT, VD56G3_CMD_BOOT, &ret);
> >> +	vd56g3_poll_reg(sensor, VD56G3_REG_BOOT, VD56G3_CMD_ACK, &ret);
> >> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_power_on(struct vd56g3 *sensor)
> >> +{
> >> +	struct i2c_client *client = sensor->i2c_client;
> >> +	int ret;
> >> +
> >> +	ret = regulator_bulk_enable(VD56G3_NUM_SUPPLIES, sensor->supplies);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "Failed to enable regulators %d", ret);
> >> +		return ret;
> >> +	}
> >> +
> >> +	ret = clk_prepare_enable(sensor->xclk);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "Failed to enable clock %d", ret);
> >> +		goto disable_reg;
> >> +	}
> >> +
> >> +	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
> >> +	ret = vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_READY_TO_BOOT, NULL);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "Sensor reset failed %d\n", ret);
> >> +		goto disable_clock;
> >> +	}
> >> +
> >> +	return 0;
> >> +
> >> +disable_clock:
> >> +	clk_disable_unprepare(sensor->xclk);
> >> +	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
> >> +disable_reg:
> >> +	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_power_off(struct vd56g3 *sensor)
> >> +{
> >> +	clk_disable_unprepare(sensor->xclk);
> >> +	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
> >> +	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
> >> +	return 0;
> >> +}
> >> +
> >> +static int vd56g3_runtime_resume(struct device *dev)
> >> +{
> >> +	struct v4l2_subdev *sd = dev_get_drvdata(dev);
> >> +	struct vd56g3 *sensor = to_vd56g3(sd);
> >> +	struct i2c_client *client = sensor->i2c_client;
> >> +	int ret;
> >> +
> >> +	ret = vd56g3_power_on(sensor);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "Failed to power on %d", ret);
> >> +		return ret;
> >> +	}
> >> +
> >> +	ret = vd56g3_boot(sensor);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "sensor boot failed %d", ret);
> >> +		return ret;
> >> +	}
> >> +
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_runtime_suspend(struct device *dev)
> >> +{
> >> +	struct v4l2_subdev *sd = dev_get_drvdata(dev);
> >> +	struct vd56g3 *sensor = to_vd56g3(sd);
> >> +
> >> +	return vd56g3_power_off(sensor);
> >> +}
> >> +
> >> +static const struct dev_pm_ops vd56g3_pm_ops = {
> >> +	SET_RUNTIME_PM_OPS(vd56g3_runtime_suspend, vd56g3_runtime_resume, NULL)
> >> +};
> >> +
> >> +/* -----------------------------------------------------------------------------
> >> + * Probe and initialization
> >> + */
> >> +
> >> +static int vd56g3_check_csi_conf(struct vd56g3 *sensor,
> >> +				 struct fwnode_handle *endpoint)
> >> +{
> >> +	struct i2c_client *client = sensor->i2c_client;
> >> +	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
> >> +	u32 phy_data_lanes[VD56G3_MAX_CSI_DATA_LANES] = { ~0, ~0 };
> >> +	int n_lanes;
> > 
> > use u8 for n_lanes
> > 
> 
> Ok, applied for V2
> 
> >> +	int frequency;
> > 
> > use u64
> 
> Ok, applied for V2
> 
> > 
> >> +	int p, l;
> >> +	int ret = 0;
> >> +
> >> +	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
> >> +	if (ret)
> >> +		return -EINVAL;
> >> +
> >> +	/* Check lanes number */
> >> +	n_lanes = ep.bus.mipi_csi2.num_data_lanes;
> >> +	if (n_lanes != 1 && n_lanes != 2) {
> >> +		dev_err(&client->dev, "Invalid data lane number %d\n", n_lanes);
> >> +		ret = -EINVAL;
> >> +		goto done;
> >> +	}
> >> +	sensor->nb_of_lane = n_lanes;
> >> +
> >> +	/* Clock lane must be first */
> >> +	if (ep.bus.mipi_csi2.clock_lane != 0) {
> >> +		dev_err(&client->dev, "Clk lane must be mapped to lane 0\n");
> >> +		ret = -EINVAL;
> >> +		goto done;
> >> +	}
> >> +
> >> +	/* Prepare Output Interface conf based on lane settings
> >> +	 * logical to physical lane conversion (+ pad remaining slots)
> >> +	 */
> >> +	for (l = 0; l < n_lanes; l++)
> >> +		phy_data_lanes[ep.bus.mipi_csi2.data_lanes[l] - 1] = l;
> >> +	for (p = 0; p < VD56G3_MAX_CSI_DATA_LANES; p++) {
> >> +		if (phy_data_lanes[p] != ~0)
> >> +			continue;
> >> +		phy_data_lanes[p] = l;
> >> +		l++;
> >> +	}
> >> +	sensor->oif_ctrl = n_lanes |
> >> +			   (ep.bus.mipi_csi2.lane_polarities[0] << 3) |
> >> +			   ((phy_data_lanes[0]) << 4) |
> >> +			   (ep.bus.mipi_csi2.lane_polarities[1] << 6) |
> >> +			   ((phy_data_lanes[1]) << 7) |
> >> +			   (ep.bus.mipi_csi2.lane_polarities[2] << 9);
> >> +
> >> +	/* Check link frequency */
> >> +	if (!ep.nr_of_link_frequencies) {
> >> +		dev_err(&client->dev, "link-frequency not found in DT\n");
> >> +		ret = -EINVAL;
> >> +		goto done;
> >> +	}
> >> +	frequency = (n_lanes == 2) ? VD56G3_LINK_FREQ_DEF_2LANES :
> >> +				     VD56G3_LINK_FREQ_DEF_1LANE;
> >> +	if (ep.nr_of_link_frequencies != 1 ||
> >> +	    ep.link_frequencies[0] != frequency) {
> >> +		dev_err(&client->dev, "Link frequency not supported: %lld\n",
> >> +			ep.link_frequencies[0]);
> >> +		ret = -EINVAL;
> >> +		goto done;
> >> +	}
> >> +
> >> +done:
> >> +	v4l2_fwnode_endpoint_free(&ep);
> >> +	return ret;
> >> +}
> >> +
> >> +static int vd56g3_parse_dt_gpios_array(struct vd56g3 *sensor, char *prop_name,
> >> +				       u32 *array, int *nb)
> >> +{
> >> +	struct i2c_client *client = sensor->i2c_client;
> >> +	struct device_node *np = client->dev.of_node;
> >> +	unsigned int i;
> >> +
> >> +	*nb = of_property_read_variable_u32_array(np, prop_name, array, 0,
> >> +						  VD56G3_NB_GPIOS);
> >> +
> >> +	if (*nb == -EINVAL) {
> >> +		*nb = 0;
> >> +		return *nb;
> >> +	} else if (*nb < 0) {
> >> +		dev_err(&client->dev, "Failed to read %s prop\n", prop_name);
> >> +		return *nb;
> >> +	}
> >> +
> >> +	for (i = 0; i < *nb; i++) {
> >> +		if (array[i] >= VD56G3_NB_GPIOS) {
> >> +			dev_err(&client->dev, "Invalid GPIO : %d\n", array[i]);
> >> +			return -EINVAL;
> >> +		}
> >> +	}
> >> +
> >> +	return 0;
> >> +}
> >> +
> >> +static int vd56g3_parse_dt_gpios(struct vd56g3 *sensor)
> >> +{
> >> +	u32 led_gpios[VD56G3_NB_GPIOS];
> >> +	int nb_gpios_leds;
> >> +	int ret;
> >> +	unsigned int i;
> >> +
> >> +	/* Initialize GPIOs to default */
> >> +	for (i = 0; i < VD56G3_NB_GPIOS; i++)
> >> +		sensor->gpios[i] = VD56G3_GPIOX_GPIO_IN;
> >> +	sensor->ext_leds_mask = 0;
> >> +
> >> +	/* Take into account optional 'st,leds' output for GPIOs */
> >> +	ret = vd56g3_parse_dt_gpios_array(sensor, "st,leds", led_gpios,
> >> +					  &nb_gpios_leds);
> >> +	if (ret)
> >> +		return ret;
> >> +	for (i = 0; i < nb_gpios_leds; i++) {
> >> +		sensor->gpios[led_gpios[i]] = VD56G3_GPIOX_STROBE_MODE;
> >> +		set_bit(led_gpios[i], &sensor->ext_leds_mask);
> >> +	}
> >> +
> >> +	return 0;
> >> +}
> >> +
> >> +static int vd56g3_parse_dt(struct vd56g3 *sensor)
> >> +{
> >> +	struct i2c_client *client = sensor->i2c_client;
> >> +	struct device *dev = &client->dev;
> >> +	struct fwnode_handle *endpoint;
> >> +	int ret;
> >> +
> >> +	endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0, 0);
> >> +	if (!endpoint) {
> >> +		dev_err(dev, "endpoint node not found\n");
> >> +		return -EINVAL;
> >> +	}
> >> +
> >> +	ret = vd56g3_check_csi_conf(sensor, endpoint);
> >> +	fwnode_handle_put(endpoint);
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	ret = vd56g3_parse_dt_gpios(sensor);
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	return 0;
> >> +}
> >> +
> >> +static int vd56g3_get_regulators(struct vd56g3 *sensor)
> >> +{
> >> +	int i;
> >> +
> >> +	for (i = 0; i < VD56G3_NUM_SUPPLIES; i++)
> >> +		sensor->supplies[i].supply = vd56g3_supply_names[i];
> >> +
> >> +	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
> >> +				       VD56G3_NUM_SUPPLIES, sensor->supplies);
> >> +}
> >> +
> >> +static int vd56g3_prepare_clock_tree(struct vd56g3 *sensor)
> >> +{
> >> +	struct i2c_client *client = sensor->i2c_client;
> >> +	const unsigned int predivs[] = { 1, 2, 4 };
> >> +	u32 pll_out;
> >> +	int i;
> >> +
> >> +	/* External clock must be in [6Mhz-27Mhz] */
> >> +	if (sensor->ext_clock < 6 * HZ_PER_MHZ ||
> >> +	    sensor->ext_clock > 27 * HZ_PER_MHZ) {
> >> +		dev_err(&client->dev,
> >> +			"Only 6Mhz-27Mhz clock range supported. Provided %lu MHz\n",
> >> +			sensor->ext_clock / HZ_PER_MHZ);
> >> +		return -EINVAL;
> >> +	}
> >> +
> >> +	/* PLL input should be in [6Mhz-12Mhz[ */
> >> +	for (i = 0; i < ARRAY_SIZE(predivs); i++) {
> >> +		sensor->pll_prediv = predivs[i];
> >> +		if (sensor->ext_clock / sensor->pll_prediv < 12 * HZ_PER_MHZ)
> >> +			break;
> >> +	}
> >> +
> >> +	/* PLL output clock must be as close as possible to 804Mhz */
> >> +	sensor->pll_mult = (VD56G3_TARGET_PLL * sensor->pll_prediv +
> >> +			    sensor->ext_clock / 2) /
> >> +			   sensor->ext_clock;
> >> +	pll_out = sensor->ext_clock * sensor->pll_mult / sensor->pll_prediv;
> >> +
> >> +	/* Target Pixel Clock for standard 10bit ADC mode : 160.8Mhz */
> >> +	sensor->pixel_clock = pll_out / VD56G3_VT_CLOCK_DIV;
> >> +
> >> +	return 0;
> >> +}
> >> +
> >> +static int vd56g3_detect(struct vd56g3 *sensor)
> >> +{
> >> +	struct i2c_client *client = sensor->i2c_client;
> >> +	struct device *dev = &client->dev;
> >> +	unsigned int model;
> >> +	u64 model_id = 0;
> >> +	u64 device_revision = 0;
> >> +	u64 optical_revision = 0;
> >> +	int ret = 0;
> >> +
> >> +	model = (uintptr_t)device_get_match_data(dev);
> >> +
> >> +	cci_read(sensor->regmap, VD56G3_REG_MODEL_ID, &model_id, &ret);
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	if (model_id != VD56G3_MODEL_ID) {
> >> +		dev_warn(&client->dev, "Unsupported sensor id %x",
> >> +			 (u16)model_id);
> >> +		return -ENODEV;
> >> +	}
> >> +
> >> +	cci_read(sensor->regmap, VD56G3_REG_REVISION, &device_revision, &ret);
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	if ((device_revision >> 8) != 0x31) {
> > 
> > What about create a define that explain 0x31 number to avoid magic
> > number?
> > 
> > #define VD56G3_DEV_CUTV = 0x31?
> 
> Yes more readable. Applied for V2
> 
> > 
> >> +		dev_warn(&client->dev, "Unsupported Cut version %x",
> >> +			 (u16)device_revision);
> >> +		return -ENODEV;
> >> +	}
> >> +
> >> +	cci_read(sensor->regmap, VD56G3_REG_OPTICAL_REVISION, &optical_revision,
> >> +		 &ret);
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	sensor->is_mono =
> >> +		((optical_revision & 1) == VD56G3_OPTICAL_REVISION_MONO);
> >> +	if ((sensor->is_mono && model == VD56G3_MODEL_VD66GY) ||
> >> +	    (!sensor->is_mono && model == VD56G3_MODEL_VD56G3)) {
> >> +		dev_warn(&client->dev,
> >> +			 "Found %s sensor, while %s model is defined in DT",
> >> +			 (sensor->is_mono) ? "Mono" : "Bayer",
> >> +			 (model == VD56G3_MODEL_VD56G3) ? "vd56g3" : "vd66gy");
> >> +		return -ENODEV;
> >> +	}
> >> +
> >> +	return 0;
> >> +}
> >> +
> >> +static int vd56g3_subdev_init(struct vd56g3 *sensor)
> >> +{
> >> +	struct i2c_client *client = sensor->i2c_client;
> >> +	int ret;
> >> +
> >> +	/* Init sub device */
> >> +	v4l2_i2c_subdev_init(&sensor->sd, client, &vd56g3_subdev_ops);
> >> +	sensor->sd.internal_ops = &vd56g3_internal_ops;
> >> +	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
> >> +			    V4L2_SUBDEV_FL_HAS_EVENTS;
> >> +	sensor->sd.entity.ops = &vd56g3_subdev_entity_ops;
> >> +
> >> +	/* Init source pad */
> >> +	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
> >> +	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
> >> +	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "Failed to init media entity : %d", ret);
> >> +		return ret;
> >> +	}
> >> +
> >> +	/* Init controls */
> >> +	ret = vd56g3_init_controls(sensor);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "Controls initialization failed %d", ret);
> >> +		goto err_media;
> >> +	}
> >> +
> >> +	/* Init vd56g3 struct : default resolution + raw8 */
> >> +	sensor->streaming = false;
> >> +	sensor->sd.state_lock = sensor->ctrl_handler.lock;
> >> +	ret = v4l2_subdev_init_finalize(&sensor->sd);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "subdev init error: %d", ret);
> >> +		goto err_ctrls;
> >> +	}
> >> +
> >> +	vd56g3_update_controls(sensor);
> >> +
> >> +	return 0;
> >> +
> >> +err_ctrls:
> >> +	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
> >> +
> >> +err_media:
> >> +	media_entity_cleanup(&sensor->sd.entity);
> >> +	return ret;
> >> +}
> >> +
> >> +static void vd56g3_subdev_cleanup(struct vd56g3 *sensor)
> >> +{
> >> +	v4l2_async_unregister_subdev(&sensor->sd);
> >> +	v4l2_subdev_cleanup(&sensor->sd);
> >> +	media_entity_cleanup(&sensor->sd.entity);
> >> +	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
> >> +}
> >> +
> >> +static int vd56g3_probe(struct i2c_client *client)
> >> +{
> >> +	struct device *dev = &client->dev;
> >> +	struct vd56g3 *sensor;
> >> +	int ret;
> >> +
> >> +	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
> >> +	if (!sensor)
> >> +		return -ENOMEM;
> >> +
> >> +	sensor->i2c_client = client;
> >> +
> >> +	ret = vd56g3_parse_dt(sensor);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "Failed to parse Device Tree : %d", ret);
> > 
> > Why not dev_err_probe here?
> 
> Sure, thanks.
> 
> > 
> >> +		return ret;
> >> +	}
> >> +
> >> +	/* Get (and check) resources : power regs, ext clock, reset gpio */
> >> +	ret = vd56g3_get_regulators(sensor);
> >> +	if (ret)
> >> +		return dev_err_probe(dev, ret, "Failed to get regulators.");
> >> +
> >> +	sensor->xclk = devm_clk_get(dev, NULL);
> >> +	if (IS_ERR(sensor->xclk))
> >> +		return dev_err_probe(dev, PTR_ERR(sensor->xclk),
> >> +				     "Failed to get xclk.");
> >> +	sensor->ext_clock = clk_get_rate(sensor->xclk);
> > 
> > What about use of xclk_freq?
> > I think this is more understandable
> > 
> > sensor->xclk
> > sensor->xclk_freq
> > 
> > better I think :)
> 
> Yes, why not.
> Applied for V2.
> 
> > 
> >> +	ret = vd56g3_prepare_clock_tree(sensor);
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	sensor->reset_gpio =
> >> +		devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
> >> +	if (IS_ERR(sensor->reset_gpio))
> >> +		return dev_err_probe(dev, PTR_ERR(sensor->reset_gpio),
> >> +				     "Failed to get reset gpio.");
> >> +
> >> +	sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
> >> +	if (IS_ERR(sensor->regmap))
> >> +		return dev_err_probe(dev, PTR_ERR(sensor->regmap),
> >> +				     "Failed to init regmap.");
> >> +
> >> +	/* Power ON */
> >> +	ret = vd56g3_power_on(sensor);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "Sensor power on failed : %d", ret);
> > 
> > dev_err_probe?
> 
> Applied for V2.
> 
> > 
> >> +		return ret;
> >> +	}
> >> +
> >> +	/* Enable PM runtime with autosuspend (sensor being ON, set active) */
> >> +	pm_runtime_set_active(dev);
> >> +	pm_runtime_get_noresume(dev);
> >> +	pm_runtime_enable(dev);
> >> +	pm_runtime_set_autosuspend_delay(dev, 1000);
> >> +	pm_runtime_use_autosuspend(dev);
> >> +
> >> +	/* Check HW model/version */
> >> +	ret = vd56g3_boot(sensor);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "Sensor boot failed : %d", ret);
> >> +		goto err_power_off;
> >> +	}
> >> +
> >> +	ret = vd56g3_detect(sensor);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "Sensor detect failed : %d", ret);
> >> +		goto err_power_off;
> >> +	}
> >> +
> >> +	/* Initialize, then register V4L2 subdev */
> >> +	ret = vd56g3_subdev_init(sensor);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "V4l2 init failed : %d", ret);
> >> +		goto err_power_off;
> >> +	}
> >> +
> >> +	ret = v4l2_async_register_subdev(&sensor->sd);
> >> +	if (ret) {
> >> +		dev_err(&client->dev, "async subdev register failed %d", ret);
> >> +		goto err_subdev;
> >> +	}
> >> +
> >> +	/* Sensor could now be powered off (after the autosuspend delay) */
> >> +	pm_runtime_mark_last_busy(dev);
> >> +	pm_runtime_put_autosuspend(dev);
> >> +
> >> +	dev_dbg(&client->dev, "Successfully probe %s sensor",
> >> +		(sensor->is_mono) ? "vd56g3" : "vd66gy");
> >> +
> >> +	return 0;
> >> +
> >> +err_subdev:
> >> +	vd56g3_subdev_cleanup(sensor);
> >> +err_power_off:
> >> +	pm_runtime_disable(dev);
> >> +	pm_runtime_put_noidle(dev);
> >> +	vd56g3_power_off(sensor);
> >> +	return ret;
> >> +}
> >> +
> >> +static void vd56g3_remove(struct i2c_client *client)
> >> +{
> >> +	struct v4l2_subdev *sd = i2c_get_clientdata(client);
> >> +	struct vd56g3 *sensor = to_vd56g3(sd);
> >> +
> >> +	vd56g3_subdev_cleanup(sensor);
> >> +
> >> +	pm_runtime_disable(&client->dev);
> >> +	if (!pm_runtime_status_suspended(&client->dev))
> >> +		vd56g3_power_off(sensor);
> >> +	pm_runtime_set_suspended(&client->dev);
> >> +}
> >> +
> >> +static const struct of_device_id vd56g3_dt_ids[] = {
> >> +	{ .compatible = "st,st-vd56g3", .data = (void *)VD56G3_MODEL_VD56G3 },
> >> +	{ .compatible = "st,st-vd66gy", .data = (void *)VD56G3_MODEL_VD66GY },
> >> +	{ /* sentinel */ }
> >> +};
> >> +MODULE_DEVICE_TABLE(of, vd56g3_dt_ids);
> >> +
> >> +static struct i2c_driver vd56g3_i2c_driver = {
> >> +	.driver = {
> >> +		.name  = "st-vd56g3",
> >> +		.of_match_table = vd56g3_dt_ids,
> >> +		.pm = &vd56g3_pm_ops,
> >> +	},
> >> +	.probe = vd56g3_probe,
> >> +	.remove = vd56g3_remove,
> >> +};
> >> +
> >> +module_i2c_driver(vd56g3_i2c_driver);
> >> +
> >> +MODULE_AUTHOR("Benjamin Mugnier <[e-mail]>");
> >> +MODULE_AUTHOR("Mickael Guene <[e-mail]>");
> >> +MODULE_AUTHOR("Sylvain Petinot <[e-mail]>");
> >> +MODULE_DESCRIPTION("ST VD56G3 sensor driver");
> >> +MODULE_LICENSE("GPL");
> >> -- 
> >> 2.17.1
> >>
> >>
> > 
> > Hope this help :)
> 
> Sure, thanks !
> 
> > 
> > Thanks & Regards,
> > Tommaso
> --
> Sylvain

Thanks & Regards,
Tommaso

Benjamin Mugnier May 13, 2024, 1:37 p.m. UTC | #7

Hi Sylvain and Sakari,

On 5/3/24 12:10, Sylvain Petinot wrote:
> Hi Sakari,
> 
> Thanks for this first review.
> 
> On 4/24/2024 11:12 PM, Sakari Ailus wrote:
>> Hi Sylvain,
>>
>> Some quick comments below...
>>
>> On Wed, Apr 17, 2024 at 03:34:53PM +0200, Sylvain Petinot wrote:
>>> Add V4L2 sub-device driver for STMicroelectronics VD56G3 camera sensor.
>>> This is a 1.5 M pixel global shutter image sensor with an active array
>>> size of 1124 x 1364 (portrait orientation).
>>>
>>> The driver supports Mono (VD56G3) and Color (VD66GY) variants.
>>>
>>> Signed-off-by: Sylvain Petinot <[e-mail]>
>>> ---
>>>  drivers/media/i2c/Kconfig     |   11 +
>>>  drivers/media/i2c/Makefile    |    1 +
>>>  drivers/media/i2c/st-vd56g3.c | 1619 +++++++++++++++++++++++++++++++++
>>>  3 files changed, 1631 insertions(+)
>>>  create mode 100644 drivers/media/i2c/st-vd56g3.c
>>>
>>> diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
>>> index 56f276b920ab..a2d7271eef88 100644
>>> --- a/drivers/media/i2c/Kconfig
>>> +++ b/drivers/media/i2c/Kconfig
>>> @@ -657,6 +657,17 @@ config VIDEO_S5K6A3
>>>  	  This is a V4L2 sensor driver for Samsung S5K6A3 raw
>>>  	  camera sensor.
>>>  
>>> +config VIDEO_ST_VD56G3
>>> +	tristate "ST VD56G3 sensor support"
>>> +	select V4L2_CCI_I2C
>>> +	depends on OF && GPIOLIB
>>> +	help
>>> +	  This is a Video4Linux2 sensor driver for the ST VD56G3
>>> +	  camera sensor.
>>> +
>>> +	  To compile this driver as a module, choose M here: the
>>> +	  module will be called st-vd56g3.
>>> +
>>>  config VIDEO_ST_VGXY61
>>>  	tristate "ST VGXY61 sensor support"
>>>  	select V4L2_CCI_I2C
>>> diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
>>> index dfbe6448b549..b469cf0f8113 100644
>>> --- a/drivers/media/i2c/Makefile
>>> +++ b/drivers/media/i2c/Makefile
>>> @@ -124,6 +124,7 @@ obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
>>>  obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
>>>  obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
>>>  obj-$(CONFIG_VIDEO_ST_MIPID02) += st-mipid02.o
>>> +obj-$(CONFIG_VIDEO_ST_VD56G3) += st-vd56g3.o
>>>  obj-$(CONFIG_VIDEO_ST_VGXY61) += st-vgxy61.o
>>>  obj-$(CONFIG_VIDEO_TC358743) += tc358743.o
>>>  obj-$(CONFIG_VIDEO_TC358746) += tc358746.o
>>> diff --git a/drivers/media/i2c/st-vd56g3.c b/drivers/media/i2c/st-vd56g3.c
>>> new file mode 100644
>>> index 000000000000..eff819941456
>>> --- /dev/null
>>> +++ b/drivers/media/i2c/st-vd56g3.c
>>> @@ -0,0 +1,1619 @@
>>> +// SPDX-License-Identifier: GPL-2.0
>>> +/*
>>> + * A V4L2 driver for ST VD56G3 (Mono) and VD66GY (RGB) global shutter cameras.
>>> + * Copyright (C) 2024, STMicroelectronics SA
>>> + */
>>> +
>>> +#include <linux/clk.h>
>>> +#include <linux/delay.h>
>>> +#include <linux/gpio/consumer.h>
>>> +#include <linux/i2c.h>
>>> +#include <linux/iopoll.h>
>>> +#include <linux/module.h>
>>> +#include <linux/pm_runtime.h>
>>> +#include <linux/regmap.h>
>>> +#include <linux/regulator/consumer.h>
>>> +#include <linux/units.h>
>>> +
>>> +#include <asm/unaligned.h>
>>> +
>>> +#include <media/mipi-csi2.h>
>>> +#include <media/v4l2-async.h>
>>> +#include <media/v4l2-cci.h>
>>> +#include <media/v4l2-ctrls.h>
>>> +#include <media/v4l2-device.h>
>>> +#include <media/v4l2-event.h>
>>> +#include <media/v4l2-fwnode.h>
>>> +#include <media/v4l2-subdev.h>
>>> +
>>> +/* Register Map */
>>> +#define VD56G3_REG_MODEL_ID				CCI_REG16_LE(0x0000)
>>> +#define VD56G3_MODEL_ID					0x5603
>>> +#define VD56G3_REG_REVISION				CCI_REG16_LE(0x0002)
>>> +#define VD56G3_REG_OPTICAL_REVISION			CCI_REG8(0x001a)
>>> +#define VD56G3_OPTICAL_REVISION_MONO			0
>>> +#define VD56G3_OPTICAL_REVISION_BAYER			1
>>> +#define VD56G3_REG_SYSTEM_FSM				CCI_REG8(0x0028)
>>> +#define VD56G3_SYSTEM_FSM_READY_TO_BOOT			0x01
>>> +#define VD56G3_SYSTEM_FSM_SW_STBY			0x02
>>> +#define VD56G3_SYSTEM_FSM_STREAMING			0x03
>>> +#define VD56G3_REG_APPLIED_COARSE_EXPOSURE		CCI_REG16_LE(0x0064)
>>> +#define VD56G3_REG_APPLIED_ANALOG_GAIN			CCI_REG8(0x0068)
>>> +#define VD56G3_REG_APPLIED_DIGITAL_GAIN			CCI_REG16_LE(0x006a)
>>> +#define VD56G3_REG_BOOT					CCI_REG8(0x0200)
>>> +#define VD56G3_CMD_ACK					0
>>> +#define VD56G3_CMD_BOOT					1
>>> +#define VD56G3_REG_STBY					CCI_REG8(0x0201)
>>> +#define VD56G3_CMD_START_STREAM				1
>>> +#define VD56G3_REG_STREAMING				CCI_REG8(0x0202)
>>> +#define VD56G3_CMD_STOP_STREAM				1
>>> +#define VD56G3_REG_EXT_CLOCK				CCI_REG32_LE(0x0220)
>>> +#define VD56G3_REG_CLK_PLL_PREDIV			CCI_REG8(0x0224)
>>> +#define VD56G3_REG_CLK_SYS_PLL_MULT			CCI_REG8(0x0226)
>>> +#define VD56G3_REG_ORIENTATION				CCI_REG8(0x0302)
>>> +#define VD56G3_REG_FORMAT_CTRL				CCI_REG8(0x030a)
>>> +#define VD56G3_REG_OIF_CTRL				CCI_REG16_LE(0x030c)
>>> +#define VD56G3_REG_OIF_IMG_CTRL				CCI_REG8(0x030f)
>>> +#define VD56G3_REG_OIF_CSI_BITRATE			CCI_REG16_LE(0x0312)
>>> +#define VD56G3_REG_DUSTER_CTRL				CCI_REG8(0x0318)
>>> +#define VD56G3_DUSTER_DISABLE				0
>>> +#define VD56G3_DUSTER_ENABLE_DEF_MODULES		0x13
>>> +#define VD56G3_REG_ISL_ENABLE				CCI_REG8(0x0333)
>>> +#define VD56G3_REG_DARKCAL_CTRL				CCI_REG8(0x0340)
>>> +#define VD56G3_DARKCAL_ENABLE				1
>>> +#define VD56G3_DARKCAL_DISABLE_DARKAVG			2
>>> +#define VD56G3_REG_PATGEN_CTRL				CCI_REG16_LE(0x0400)
>>> +#define VD56G3_PATGEN_ENABLE				1
>>> +#define VD56G3_PATGEN_TYPE_SHIFT			4
>>> +#define VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE		CCI_REG16_LE(0x042a)
>>> +#define VD56G3_REG_AE_COLDSTART_ANALOG_GAIN		CCI_REG8(0x042c)
>>> +#define VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN		CCI_REG16_LE(0x042e)
>>> +#define VD56G3_REG_AE_ROI_START_H			CCI_REG16_LE(0x0432)
>>> +#define VD56G3_REG_AE_ROI_START_V			CCI_REG16_LE(0x0434)
>>> +#define VD56G3_REG_AE_ROI_END_H				CCI_REG16_LE(0x0436)
>>> +#define VD56G3_REG_AE_ROI_END_V				CCI_REG16_LE(0x0438)
>>> +#define VD56G3_REG_AE_COMPENSATION			CCI_REG16_LE(0x043a)
>>> +#define VD56G3_REG_EXP_MODE				CCI_REG8(0x044c)
>>> +#define VD56G3_EXP_MODE_AUTO				0
>>> +#define VD56G3_EXP_MODE_FREEZE				1
>>> +#define VD56G3_EXP_MODE_MANUAL				2
>>> +#define VD56G3_REG_MANUAL_ANALOG_GAIN			CCI_REG8(0x044d)
>>> +#define VD56G3_REG_MANUAL_COARSE_EXPOSURE		CCI_REG16_LE(0x044e)
>>> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0		CCI_REG16_LE(0x0450)
>>> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1		CCI_REG16_LE(0x0452)
>>> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2		CCI_REG16_LE(0x0454)
>>> +#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3		CCI_REG16_LE(0x0456)
>>> +#define VD56G3_REG_FRAME_LENGTH				CCI_REG16_LE(0x0458)
>>> +#define VD56G3_REG_Y_START				CCI_REG16_LE(0x045a)
>>> +#define VD56G3_REG_Y_END				CCI_REG16_LE(0x045c)
>>> +#define VD56G3_REG_OUT_ROI_X_START			CCI_REG16_LE(0x045e)
>>> +#define VD56G3_REG_OUT_ROI_X_END			CCI_REG16_LE(0x0460)
>>> +#define VD56G3_REG_OUT_ROI_Y_START			CCI_REG16_LE(0x0462)
>>> +#define VD56G3_REG_OUT_ROI_Y_END			CCI_REG16_LE(0x0464)
>>> +#define VD56G3_REG_GPIO_0_CTRL				CCI_REG8(0x0467)
>>> +#define VD56G3_GPIOX_GPIO_IN				0x01
>>> +#define VD56G3_GPIOX_STROBE_MODE			0x02
>>> +#define VD56G3_REG_READOUT_CTRL				CCI_REG8(0x047e)
>>> +#define READOUT_NORMAL					0x00
>>> +#define READOUT_DIGITAL_BINNING_X2			0x01
>>> +
>>> +/*
>>> + * The VD56G3 pixel array is organized as follows:
>>> + *
>>> + * +--------------------------------+
>>> + * |                                | \
>>> + * |   +------------------------+   |  |
>>> + * |   |                        |   |  |
>>> + * |   |                        |   |  |
>>> + * |   |                        |   |  |
>>> + * |   |                        |   |  |
>>> + * |   |                        |   |  |
>>> + * |   |   Default resolution   |   |  | Native height (1364)
>>> + * |   |      1120 x 1360       |   |  |
>>> + * |   |                        |   |  |
>>> + * |   |                        |   |  |
>>> + * |   |                        |   |  |
>>> + * |   |                        |   |  |
>>> + * |   +------------------------+   |  |
>>> + * |                                | /
>>> + * +--------------------------------+
>>> + *   <----------------------------->
>>> + *                     \-------------------  Native width (1124)
>>> + *
>>> + * The native resolution is 1124x1364.
>>> + * The recommended/default resolution is 1120x1360 (multiple of 16).
>>> + */
>>> +#define VD56G3_NATIVE_WIDTH				1124
>>> +#define VD56G3_NATIVE_HEIGHT				1364
>>> +#define VD56G3_DEFAULT_WIDTH				1120
>>> +#define VD56G3_DEFAULT_HEIGHT				1360
>>> +#define VD56G3_DEFAULT_MODE				1
>>> +
>>> +/* PLL settings */
>>> +#define VD56G3_TARGET_PLL				804000000UL
>>> +#define VD56G3_VT_CLOCK_DIV				5
>>> +
>>> +/* Line length and Frame length (settings are for standard 10bits ADC mode) */
>>> +#define VD56G3_LINE_LENGTH_MIN				1236
>>> +#define VD56G3_VBLANK_MIN				110
>>> +#define VD56G3_FRAME_LENGTH_DEF_60FPS			2168
>>> +
>>> +/* Exposure settings */
>>> +#define VD56G3_EXPOSURE_MARGIN				75
>>> +#define VD56G3_EXPOSURE_DEFAULT				1420
>>> +
>>> +/* Output Interface settings */
>>> +#define VD56G3_MAX_CSI_DATA_LANES			2
>>> +#define VD56G3_LINK_FREQ_DEF_1LANE			750000000UL
>>> +#define VD56G3_LINK_FREQ_DEF_2LANES			402000000UL
>>> +
>>> +/* GPIOs */
>>> +#define VD56G3_NB_GPIOS					8
>>> +
>>> +/* regulator supplies */
>>> +static const char *const vd56g3_supply_names[] = {
>>> +	"VCORE",
>>> +	"VDDIO",
>>> +	"VANA",
>>> +};
>>> +
>>> +#define VD56G3_NUM_SUPPLIES		ARRAY_SIZE(vd56g3_supply_names)
>>
>> Please use ARRAY_SIZE(...) directly.
> 
> Ok, applied for V2
> 
>>
>>> +
>>> +/* -----------------------------------------------------------------------------
>>> + * Models (VD56G3: Mono, VD66GY: Bayer RGB), Modes and formats
>>> + */
>>> +
>>> +enum vd56g3_models {
>>> +	VD56G3_MODEL_VD56G3,
>>> +	VD56G3_MODEL_VD66GY,
>>> +};
>>> +
>>> +struct vd56g3_mode {
>>> +	u32 width;
>>> +	u32 height;
>>> +};
>>> +
>>> +static const struct vd56g3_mode vd56g3_supported_modes[] = {
>>> +	{
>>> +		.width = VD56G3_NATIVE_WIDTH,
>>> +		.height = VD56G3_NATIVE_HEIGHT,
>>> +	},
>>> +	{
>>> +		.width = VD56G3_DEFAULT_WIDTH,
>>> +		.height = VD56G3_DEFAULT_HEIGHT,
>>> +	},
>>> +	{
>>> +		.width = 1024,
>>> +		.height = 1280,
>>> +	},
>>> +	{
>>> +		.width = 1024,
>>> +		.height = 768,
>>> +	},
>>> +	{
>>> +		.width = 768,
>>> +		.height = 1024,
>>> +	},
>>> +	{
>>> +		.width = 720,
>>> +		.height = 1280,
>>> +	},
>>> +	{
>>> +		.width = 640,
>>> +		.height = 480,
>>> +	},
>>> +	{
>>> +		.width = 480,
>>> +		.height = 640,
>>> +	},
>>> +	{
>>> +		.width = 320,
>>> +		.height = 240,
>>> +	},
>>> +};
>>> +
>>> +/*
>>> + * Sensor support 8bits and 10bits output in both variants
>>> + *  - Monochrome
>>> + *  - RGB (with all H/V flip variations)
>>> + */
>>> +static const unsigned int vd56g3_mbus_codes[2][5] = {
>>> +	{
>>> +		MEDIA_BUS_FMT_Y8_1X8,
>>> +		MEDIA_BUS_FMT_SGRBG8_1X8,
>>> +		MEDIA_BUS_FMT_SRGGB8_1X8,
>>> +		MEDIA_BUS_FMT_SBGGR8_1X8,
>>> +		MEDIA_BUS_FMT_SGBRG8_1X8,
>>> +	},
>>> +	{
>>> +		MEDIA_BUS_FMT_Y10_1X10,
>>> +		MEDIA_BUS_FMT_SGRBG10_1X10,
>>> +		MEDIA_BUS_FMT_SRGGB10_1X10,
>>> +		MEDIA_BUS_FMT_SBGGR10_1X10,
>>> +		MEDIA_BUS_FMT_SGBRG10_1X10,
>>> +	},
>>> +};
>>> +
>>> +enum vd56g3_expo_state {
>>> +	VD56G3_EXPO_AUTO,
>>> +	VD56G3_EXPO_AUTO_FREEZE,
>>> +	VD56G3_EXPO_MANUAL
>>> +};
>>> +
>>> +struct vd56g3 {
>>> +	struct i2c_client *i2c_client;
>>> +	struct v4l2_subdev sd;
>>> +	struct media_pad pad;
>>> +	struct regulator_bulk_data supplies[VD56G3_NUM_SUPPLIES];
>>> +	struct gpio_desc *reset_gpio;
>>> +	struct clk *xclk;
>>> +	struct regmap *regmap;
>>> +	u32 ext_clock;
>>> +	u32 pll_prediv;
>>> +	u32 pll_mult;
>>> +	u32 pixel_clock;
>>> +	u16 oif_ctrl;
>>> +	int nb_of_lane;
>>> +	u32 gpios[VD56G3_NB_GPIOS];
>>> +	unsigned long ext_leds_mask;
>>> +	bool is_mono;
>>> +	struct v4l2_ctrl_handler ctrl_handler;
>>> +	struct v4l2_ctrl *hblank_ctrl;
>>> +	struct v4l2_ctrl *vblank_ctrl;
>>> +	struct {
>>> +		struct v4l2_ctrl *hflip_ctrl;
>>> +		struct v4l2_ctrl *vflip_ctrl;
>>> +	};
>>> +	struct v4l2_ctrl *patgen_ctrl;
>>> +	struct {
>>> +		struct v4l2_ctrl *ae_ctrl;
>>> +		struct v4l2_ctrl *expo_ctrl;
>>> +		struct v4l2_ctrl *again_ctrl;
>>> +		struct v4l2_ctrl *dgain_ctrl;
>>> +	};
>>> +	struct v4l2_ctrl *ae_lock_ctrl;
>>> +	struct v4l2_ctrl *ae_bias_ctrl;
>>> +	struct v4l2_ctrl *led_ctrl;
>>> +	bool streaming;
>>> +};
>>> +
>>> +static inline struct vd56g3 *to_vd56g3(struct v4l2_subdev *sd)
>>> +{
>>> +	return container_of(sd, struct vd56g3, sd);
>>> +}
>>> +
>>> +static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
>>> +{
>>> +	return &container_of(ctrl->handler, struct vd56g3, ctrl_handler)->sd;
>>> +}
>>> +
>>> +/* -----------------------------------------------------------------------------
>>> + * Additional i2c register helpers
>>> + */
>>> +
>>> +static int vd56g3_poll_reg(struct vd56g3 *sensor, u32 reg, u8 poll_val,
>>> +			   int *err)
>>> +{
>>> +	unsigned int val = 0;
>>> +	int ret;
>>> +
>>> +	if (err && *err)
>>> +		return *err;
>>> +
>>> +	reg = reg & CCI_REG_ADDR_MASK;
>>
>> Please use CCI_REG_ADDR() instead. I'd write this without the assignment,
>> i.e. call CCI_REG_ADDR() as an argument to the function.
> 
> Yes, this is quite better !
> Applied for V2.
> 
>>
>>> +	ret = regmap_read_poll_timeout(sensor->regmap, reg, val,
>>> +				       (val == poll_val), 2000,
>>> +				       500 * USEC_PER_MSEC);
>>> +
>>> +	if (ret && err)
>>> +		*err = ret;
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_wait_state(struct vd56g3 *sensor, int state, int *err)
>>> +{
>>> +	return vd56g3_poll_reg(sensor, VD56G3_REG_SYSTEM_FSM, state, err);
>>> +}
>>> +
>>> +/* -----------------------------------------------------------------------------
>>> + * Controls: definitions, helpers and handlers
>>> + */
>>> +
>>> +static const char *const vd56g3_tp_menu[] = { "Disabled", "Solid", "Colorbar",
>>> +					      "Gradbar",  "Hgrey", "Vgrey",
>>> +					      "Dgrey",	  "PN28" };
>>> +
>>> +static const s64 vd56g3_ev_bias_qmenu[] = { -4000, -3500, -3000, -2500, -2000,
>>> +					    -1500, -1000, -500,	 0,	500,
>>> +					    1000,  1500,  2000,	 2500,	3000,
>>> +					    3500,  4000 };
>>> +
>>> +static const s64 vd56g3_link_freq_1lane[] = { VD56G3_LINK_FREQ_DEF_1LANE };
>>> +
>>> +static const s64 vd56g3_link_freq_2lanes[] = { VD56G3_LINK_FREQ_DEF_2LANES };
>>> +
>>> +static u8 vd56g3_get_bpp(__u32 code)
>>> +{
>>> +	switch (code) {
>>> +	case MEDIA_BUS_FMT_Y8_1X8:
>>> +	case MEDIA_BUS_FMT_SGRBG8_1X8:
>>> +	case MEDIA_BUS_FMT_SRGGB8_1X8:
>>> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
>>> +	case MEDIA_BUS_FMT_SGBRG8_1X8:
>>> +		return 8;
>>> +	case MEDIA_BUS_FMT_Y10_1X10:
>>> +	case MEDIA_BUS_FMT_SGRBG10_1X10:
>>> +	case MEDIA_BUS_FMT_SRGGB10_1X10:
>>> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
>>> +	case MEDIA_BUS_FMT_SGBRG10_1X10:
>>> +		return 10;
>>> +	default:
>>> +		/* Should never happen */
>>> +		WARN(1, "Unsupported code %d. default to 8 bpp", code);
>>
>> WARN() is discouraged nowadays. Same for the rest.
> 
> Thanks for pointing me.
> Unless otherwise proposed, I will use dev_warn for V2.
> 
>>
>>> +	}
>>> +
>>> +	return 8;
>>> +}
>>> +
>>> +static u8 vd56g3_get_datatype(__u32 code)
>>> +{
>>> +	switch (code) {
>>> +	case MEDIA_BUS_FMT_Y8_1X8:
>>> +	case MEDIA_BUS_FMT_SGRBG8_1X8:
>>> +	case MEDIA_BUS_FMT_SRGGB8_1X8:
>>> +	case MEDIA_BUS_FMT_SBGGR8_1X8:
>>> +	case MEDIA_BUS_FMT_SGBRG8_1X8:
>>> +		return MIPI_CSI2_DT_RAW8;
>>> +	case MEDIA_BUS_FMT_Y10_1X10:
>>> +	case MEDIA_BUS_FMT_SGRBG10_1X10:
>>> +	case MEDIA_BUS_FMT_SRGGB10_1X10:
>>> +	case MEDIA_BUS_FMT_SBGGR10_1X10:
>>> +	case MEDIA_BUS_FMT_SGBRG10_1X10:
>>> +		return MIPI_CSI2_DT_RAW10;
>>> +	default:
>>> +		/* Should never happen */
>>> +		WARN(1, "Unsupported code %d. default to MIPI_CSI2_DT_RAW8",
>>> +		     code);
>>> +	}
>>> +
>>> +	return MIPI_CSI2_DT_RAW8;
>>> +}
>>> +
>>> +static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
>>> +{
>>> +	u64 exposure = 0;
>>> +	u64 again = 0;
>>> +	u64 dgain = 0;
>>
>> Do you need the assignments? The values are assigned by cci_read() below,
>> right?
> 
> Well initially, I didn't have those assignments and some checker
> complains (Honestly I didn't investigate the warning).
> I will double check and see if they are necessary.
> 

Since I have smatch ready, I ran it without these initialization against
the latest media tree. I got these :

drivers/media/i2c/st-vd56g3.c:403 vd56g3_read_expo_cluster() error:
uninitialized symbol 'exposure'.
drivers/media/i2c/st-vd56g3.c:404 vd56g3_read_expo_cluster() error:
uninitialized symbol 'again'.
drivers/media/i2c/st-vd56g3.c:405 vd56g3_read_expo_cluster() error:
uninitialized symbol 'dgain'.
drivers/media/i2c/st-vd56g3.c:407 vd56g3_read_expo_cluster() error:
uninitialized symbol 'exposure'.
drivers/media/i2c/st-vd56g3.c:408 vd56g3_read_expo_cluster() error:
uninitialized symbol 'again'.
drivers/media/i2c/st-vd56g3.c:409 vd56g3_read_expo_cluster() error:
uninitialized symbol 'dgain'.

cci_read() is indeed not modifying its 'var' argument in the default
case of the switch [1]. Spawning these errors.
Adding a '*val = 0' in the default case would also fix it, but will
change the function behavior.

[1]
https://elixir.bootlin.com/linux/latest/source/drivers/media/v4l2-core/v4l2-cci.c#L19

>>
>>> +	int ret = 0;
>>> +
>>> +	/*
>>> +	 * When 'force_cur_val' is enabled, save the ctrl value in 'cur.val'
>>> +	 * instead of the normal 'val', this is used during poweroff to cache
>>> +	 * volatile ctrls and enable coldstart.
>>> +	 */
>>> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_COARSE_EXPOSURE, &exposure,
>>> +		 &ret);
>>> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_ANALOG_GAIN, &again, &ret);
>>> +	cci_read(sensor->regmap, VD56G3_REG_APPLIED_DIGITAL_GAIN, &dgain, &ret);
>>> +	if (ret)
>>> +		return ret;
>>> +
>>> +	if (force_cur_val) {
>>> +		sensor->expo_ctrl->cur.val = exposure;
>>> +		sensor->again_ctrl->cur.val = again;
>>> +		sensor->dgain_ctrl->cur.val = dgain;
>>> +	} else {
>>> +		sensor->expo_ctrl->val = exposure;
>>> +		sensor->again_ctrl->val = again;
>>> +		sensor->dgain_ctrl->val = dgain;
>>> +	}
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_update_patgen(struct vd56g3 *sensor, u32 patgen_index)
>>> +{
>>> +	u32 pattern = patgen_index <= 3 ? patgen_index : patgen_index + 12;
>>> +	u16 patgen = pattern << VD56G3_PATGEN_TYPE_SHIFT;
>>> +	u8 duster = VD56G3_DUSTER_ENABLE_DEF_MODULES;
>>> +	u8 darkcal = VD56G3_DARKCAL_ENABLE;
>>> +	int ret = 0;
>>> +
>>> +	if (patgen_index) {
>>> +		patgen |= VD56G3_PATGEN_ENABLE;
>>> +		duster = VD56G3_DUSTER_DISABLE;
>>> +		darkcal = VD56G3_DARKCAL_DISABLE_DARKAVG;
>>> +	}
>>> +
>>> +	cci_write(sensor->regmap, VD56G3_REG_DUSTER_CTRL, duster, &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_DARKCAL_CTRL, darkcal, &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_PATGEN_CTRL, patgen, &ret);
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_update_expo_cluster(struct vd56g3 *sensor, bool is_auto)
>>> +{
>>> +	int ret = 0;
>>
>> This would be nicer as declared last.
> 
> Sure !
> 
>>
>>> +	enum vd56g3_expo_state expo_state = is_auto ? VD56G3_EXP_MODE_AUTO :
>>> +						      VD56G3_EXP_MODE_MANUAL;
>>> +
>>> +	if (sensor->ae_ctrl->is_new)
>>> +		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
>>> +			  &ret);
>>> +
>>> +	/* In Auto expo, set coldstart parameters */
>>> +	if (is_auto && sensor->ae_ctrl->is_new) {
>>> +		cci_write(sensor->regmap,
>>> +			  VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE,
>>> +			  sensor->expo_ctrl->val, &ret);
>>> +		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_ANALOG_GAIN,
>>> +			  sensor->again_ctrl->val, &ret);
>>> +		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN,
>>> +			  sensor->dgain_ctrl->val, &ret);
>>> +	}
>>> +
>>> +	/* In Manual expo, set exposure, analog and digital gains */
>>> +	if (!is_auto && sensor->expo_ctrl->is_new)
>>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_COARSE_EXPOSURE,
>>> +			  sensor->expo_ctrl->val, &ret);
>>> +
>>> +	if (!is_auto && sensor->again_ctrl->is_new)
>>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_ANALOG_GAIN,
>>> +			  sensor->again_ctrl->val, &ret);
>>> +
>>> +	if (!is_auto && sensor->dgain_ctrl->is_new) {
>>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0,
>>> +			  sensor->dgain_ctrl->val, &ret);
>>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1,
>>> +			  sensor->dgain_ctrl->val, &ret);
>>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2,
>>> +			  sensor->dgain_ctrl->val, &ret);
>>> +		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3,
>>> +			  sensor->dgain_ctrl->val, &ret);
>>> +	}
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_lock_exposure(struct vd56g3 *sensor, u32 lock_val)
>>> +{
>>> +	bool ae_lock = lock_val & V4L2_LOCK_EXPOSURE;
>>> +	enum vd56g3_expo_state expo_state = ae_lock ? VD56G3_EXP_MODE_FREEZE :
>>> +						      VD56G3_EXP_MODE_AUTO;
>>> +	int ret = 0;
>>> +
>>> +	if (sensor->ae_ctrl->val == V4L2_EXPOSURE_AUTO)
>>> +		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
>>> +			  &ret);
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_write_gpiox(struct vd56g3 *sensor, unsigned long gpio_mask)
>>> +{
>>> +	unsigned long io;
>>> +	u32 gpio_val;
>>> +	int ret = 0;
>>> +
>>> +	for_each_set_bit(io, &gpio_mask, VD56G3_NB_GPIOS) {
>>> +		gpio_val = sensor->gpios[io];
>>> +
>>> +		if (gpio_val == VD56G3_GPIOX_STROBE_MODE &&
>>> +		    sensor->led_ctrl->val == V4L2_FLASH_LED_MODE_NONE)
>>> +			gpio_val = VD56G3_GPIOX_GPIO_IN;
>>> +
>>> +		cci_write(sensor->regmap, VD56G3_REG_GPIO_0_CTRL + io, gpio_val,
>>> +			  &ret);
>>> +	}
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
>>> +{
>>> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
>>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>>> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
>>> +	int ret = 0;
>>> +
>>> +	/* Interact with HW only when it is powered ON */
>>> +	if (!pm_runtime_get_if_in_use(&client->dev))
>>> +		return 0;
>>> +
>>> +	switch (ctrl->id) {
>>> +	case V4L2_CID_EXPOSURE_AUTO:
>>> +		ret = vd56g3_read_expo_cluster(sensor, false);
>>> +		break;
>>> +	default:
>>> +		ret = -EINVAL;
>>> +		break;
>>> +	}
>>> +
>>> +	pm_runtime_mark_last_busy(&client->dev);
>>> +	pm_runtime_put_autosuspend(&client->dev);
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_s_ctrl(struct v4l2_ctrl *ctrl)
>>> +{
>>> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
>>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>>> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
>>> +	const struct v4l2_rect *crop;
>>> +	unsigned int frame_length = 0;
>>> +	unsigned int expo_max;
>>> +	unsigned int ae_compensation;
>>> +	bool is_auto = false;
>>> +	int ret;
>>> +
>>> +	struct v4l2_subdev_state *state =
>>> +		v4l2_subdev_get_locked_active_state(sd);
>>> +	crop = v4l2_subdev_state_get_crop(state, 0);
>>> +
>>> +	if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
>>> +		return 0;
>>> +
>>> +	/* Update controls state, range, etc. whatever the state of the HW */
>>> +	switch (ctrl->id) {
>>> +	case V4L2_CID_VBLANK:
>>> +		frame_length = crop->height + ctrl->val;
>>> +		expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
>>> +		__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
>>> +					 VD56G3_EXPOSURE_DEFAULT);
>>> +		break;
>>> +	case V4L2_CID_EXPOSURE_AUTO:
>>> +		is_auto = (ctrl->val == V4L2_EXPOSURE_AUTO);
>>> +		__v4l2_ctrl_grab(sensor->ae_lock_ctrl, !is_auto);
>>> +		__v4l2_ctrl_grab(sensor->ae_bias_ctrl, !is_auto);
>>> +		break;
>>> +	default:
>>> +		break;
>>> +	}
>>> +
>>> +	/* Interact with HW only when it is powered ON */
>>> +	if (!pm_runtime_get_if_in_use(&client->dev))
>>> +		return 0;
>>> +
>>> +	switch (ctrl->id) {
>>> +	case V4L2_CID_HFLIP:
>>> +		ret = cci_write(sensor->regmap, VD56G3_REG_ORIENTATION,
>>> +				sensor->hflip_ctrl->val |
>>> +					(sensor->vflip_ctrl->val << 1),
>>> +				NULL);
>>> +		break;
>>> +	case V4L2_CID_TEST_PATTERN:
>>> +		ret = vd56g3_update_patgen(sensor, ctrl->val);
>>> +		break;
>>> +	case V4L2_CID_EXPOSURE_AUTO:
>>> +		ret = vd56g3_update_expo_cluster(sensor, is_auto);
>>> +		break;
>>> +	case V4L2_CID_3A_LOCK:
>>> +		ret = vd56g3_lock_exposure(sensor, ctrl->val);
>>> +		break;
>>> +	case V4L2_CID_AUTO_EXPOSURE_BIAS:
>>> +		ae_compensation =
>>> +			DIV_ROUND_CLOSEST((int)vd56g3_ev_bias_qmenu[ctrl->val] *
>>> +					  256, 1000);
>>> +		ret = cci_write(sensor->regmap, VD56G3_REG_AE_COMPENSATION,
>>> +				ae_compensation, NULL);
>>> +		break;
>>> +	case V4L2_CID_VBLANK:
>>> +		ret = cci_write(sensor->regmap, VD56G3_REG_FRAME_LENGTH,
>>> +				frame_length, NULL);
>>> +		break;
>>> +	case V4L2_CID_FLASH_LED_MODE:
>>> +		ret = vd56g3_write_gpiox(sensor, sensor->ext_leds_mask);
>>> +		break;
>>> +	default:
>>> +		ret = -EINVAL;
>>> +		break;
>>> +	}
>>> +
>>> +	pm_runtime_mark_last_busy(&client->dev);
>>> +	pm_runtime_put_autosuspend(&client->dev);
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static const struct v4l2_ctrl_ops vd56g3_ctrl_ops = {
>>> +	.g_volatile_ctrl = vd56g3_g_volatile_ctrl,
>>> +	.s_ctrl = vd56g3_s_ctrl,
>>> +};
>>> +
>>> +static void vd56g3_update_controls(struct vd56g3 *sensor)
>>> +{
>>> +	const struct v4l2_rect *crop;
>>> +	unsigned int hblank;
>>> +	unsigned int vblank_min, vblank, vblank_max;
>>> +	unsigned int frame_length;
>>> +	unsigned int expo_max;
>>> +
>>> +	struct v4l2_subdev_state *state =
>>> +		v4l2_subdev_get_locked_active_state(&sensor->sd);
>>> +	crop = v4l2_subdev_state_get_crop(state, 0);
>>> +
>>> +	hblank = VD56G3_LINE_LENGTH_MIN - crop->width;
>>> +	vblank_min = VD56G3_VBLANK_MIN;
>>> +	vblank = VD56G3_FRAME_LENGTH_DEF_60FPS - crop->height;
>>> +	vblank_max = 0xffff - crop->height;
>>> +	frame_length = crop->height + vblank;
>>> +	expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
>>> +
>>> +	/* Update blanking and exposure (ranges + values) */
>>> +	__v4l2_ctrl_modify_range(sensor->hblank_ctrl, hblank, hblank, 1,
>>> +				 hblank);
>>> +	__v4l2_ctrl_modify_range(sensor->vblank_ctrl, vblank_min, vblank_max, 1,
>>> +				 vblank);
>>> +	__v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, vblank);
>>> +	__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
>>> +				 VD56G3_EXPOSURE_DEFAULT);
>>> +	__v4l2_ctrl_s_ctrl(sensor->expo_ctrl, VD56G3_EXPOSURE_DEFAULT);
>>
>> Note that __v4l2_ctrl_modify_range() and __v4l2_ctrl_s_ctrl() may fail.
> 
> Yes, Tommaso pointed me also. (I took inspiration from the imx219 and
> imx290 drivers which do not have the return test).
> 
> I will take this into consideration for V2.
> 
>>
>>> +}
>>> +
>>> +static int vd56g3_init_controls(struct vd56g3 *sensor)
>>> +{
>>> +	const struct v4l2_ctrl_ops *ops = &vd56g3_ctrl_ops;
>>> +	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
>>> +	struct v4l2_ctrl *ctrl;
>>> +	int ret;
>>> +
>>> +	v4l2_ctrl_handler_init(hdl, 25);
>>> +
>>> +	/* Horizontal & vertical flips modify bayer code on RGB variant */
>>> +	sensor->hflip_ctrl =
>>> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
>>> +	if (sensor->hflip_ctrl)
>>> +		sensor->hflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
>>> +
>>> +	sensor->vflip_ctrl =
>>> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
>>> +	if (sensor->vflip_ctrl)
>>> +		sensor->vflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
>>> +
>>> +	sensor->patgen_ctrl =
>>> +		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
>>> +					     ARRAY_SIZE(vd56g3_tp_menu) - 1, 0,
>>> +					     0, vd56g3_tp_menu);
>>> +
>>> +	ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
>>> +				      ARRAY_SIZE(vd56g3_link_freq_1lane) - 1, 0,
>>> +				      (sensor->nb_of_lane == 2) ?
>>> +					      vd56g3_link_freq_2lanes :
>>> +					      vd56g3_link_freq_1lane);
>>> +	if (ctrl)
>>> +		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
>>> +
>>> +	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
>>> +				 sensor->pixel_clock, sensor->pixel_clock, 1,
>>> +				 sensor->pixel_clock);
>>> +	if (ctrl)
>>> +		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
>>> +
>>> +	sensor->ae_ctrl = v4l2_ctrl_new_std_menu(hdl, ops,
>>> +						 V4L2_CID_EXPOSURE_AUTO,
>>> +						 V4L2_EXPOSURE_MANUAL, 0,
>>> +						 V4L2_EXPOSURE_AUTO);
>>> +
>>> +	sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK, 0,
>>> +						 GENMASK(2, 0), 0, 0);
>>> +
>>> +	sensor->ae_bias_ctrl =
>>> +		v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
>>> +				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) - 1,
>>> +				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) / 2,
>>> +				       vd56g3_ev_bias_qmenu);
>>> +
>>> +	/*
>>> +	 * Analog gain [1, 8] is computed with the following logic :
>>> +	 * 32/(32 - again_reg), with again_reg in the range [0:28]
>>> +	 * Digital gain [1.00, 8.00] is coded as a Fixed Point 5.8
>>> +	 */
>>> +	sensor->again_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
>>> +					       0, 28, 1, 0);
>>> +	sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
>>> +					       0x100, 0x800, 1, 0x100);
>>> +
>>> +	/*
>>> +	 * Set the exposure, horizontal and vertical blanking ctrls
>>> +	 * to hardcoded values, they will be updated in vd56g3_update_controls.
>>> +	 * Exposure being in an auto-cluster, set a significant value here.
>>> +	 */
>>> +	sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
>>> +					      VD56G3_EXPOSURE_DEFAULT,
>>> +					      VD56G3_EXPOSURE_DEFAULT, 1,
>>> +					      VD56G3_EXPOSURE_DEFAULT);
>>> +	sensor->hblank_ctrl =
>>> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 1, 1, 1, 1);
>>> +	if (sensor->hblank_ctrl)
>>> +		sensor->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
>>> +	sensor->vblank_ctrl =
>>> +		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, 1, 1, 1, 1);
>>> +
>>> +	/* Additional control based on device tree properties */
>>> +	if (sensor->ext_leds_mask)
>>> +		sensor->led_ctrl =
>>> +			v4l2_ctrl_new_std_menu(hdl, ops,
>>> +					       V4L2_CID_FLASH_LED_MODE,
>>> +					       V4L2_FLASH_LED_MODE_FLASH, 0,
>>> +					       V4L2_FLASH_LED_MODE_NONE);
>>> +
>>> +	if (hdl->error) {
>>> +		ret = hdl->error;
>>> +		goto free_ctrls;
>>> +	}
>>> +
>>> +	v4l2_ctrl_cluster(2, &sensor->hflip_ctrl);
>>> +	v4l2_ctrl_auto_cluster(4, &sensor->ae_ctrl, V4L2_EXPOSURE_MANUAL, true);
>>> +
>>> +	sensor->sd.ctrl_handler = hdl;
>>
>> A newline here?
> 
> Sure, this isn't too difficult !
> 
>>
>>> +	return 0;
>>> +
>>> +free_ctrls:
>>> +	v4l2_ctrl_handler_free(hdl);
>>
>> Here, too?
>>
>>> +	return ret;
>>> +}
>>> +
>>> +/* -----------------------------------------------------------------------------
>>> + * Videos ops
>>> + */
>>> +
>>> +static int vd56g3_stream_on(struct vd56g3 *sensor,
>>> +			    struct v4l2_subdev_state *state)
>>> +{
>>> +	const struct v4l2_mbus_framefmt *format =
>>> +		v4l2_subdev_state_get_format(state, 0);
>>> +	const struct v4l2_rect *crop =
>>> +		v4l2_subdev_state_get_crop(state, 0);
>>> +	int ret = 0;
>>> +	unsigned int csi_mbps = ((sensor->nb_of_lane == 2) ?
>>> +					 VD56G3_LINK_FREQ_DEF_2LANES :
>>> +					 VD56G3_LINK_FREQ_DEF_1LANE) * 2 / MEGA;
>>> +	unsigned int binning;
>>> +
>>> +	/* configure clocks */
>>> +	cci_write(sensor->regmap, VD56G3_REG_EXT_CLOCK, sensor->ext_clock,
>>> +		  &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_CLK_PLL_PREDIV, sensor->pll_prediv,
>>> +		  &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_CLK_SYS_PLL_MULT, sensor->pll_mult,
>>> +		  &ret);
>>> +
>>> +	/* configure output */
>>> +	cci_write(sensor->regmap, VD56G3_REG_FORMAT_CTRL,
>>> +		  vd56g3_get_bpp(format->code), &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_OIF_CSI_BITRATE, csi_mbps, &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_OIF_IMG_CTRL,
>>> +		  vd56g3_get_datatype(format->code), &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_ISL_ENABLE, 0, &ret);
>>> +
>>> +	/* configure binning mode */
>>> +	switch (crop->width / format->width) {
>>> +	case 1:
>>> +	default:
>>> +		binning = READOUT_NORMAL;
>>> +		break;
>>> +	case 2:
>>> +		binning = READOUT_DIGITAL_BINNING_X2;
>>> +		break;
>>> +	}
>>> +	cci_write(sensor->regmap, VD56G3_REG_READOUT_CTRL, binning, &ret);
>>> +
>>> +	/* configure ROIs */
>>> +	cci_write(sensor->regmap, VD56G3_REG_Y_START, crop->top, &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_Y_END,
>>> +		  crop->top + crop->height - 1, &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_START, crop->left, &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_END,
>>> +		  crop->left + crop->width - 1, &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_START, 0, &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_END, crop->height - 1,
>>> +		  &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_H, crop->left, &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_H,
>>> +		  crop->left + crop->width - 1, &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_V, 0, &ret);
>>> +	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_V, crop->height - 1,
>>> +		  &ret);
>>> +	if (ret)
>>> +		return ret;
>>> +
>>> +	/* Setup default GPIO values; could be overridden by V4L2 ctrl setup */
>>> +	ret = vd56g3_write_gpiox(sensor, GENMASK(VD56G3_NB_GPIOS - 1, 0));
>>> +	if (ret)
>>> +		return ret;
>>> +
>>> +	/* Apply settings from V4L2 ctrls */
>>> +	ret = __v4l2_ctrl_handler_setup(&sensor->ctrl_handler);
>>> +	if (ret)
>>> +		return ret;
>>> +
>>> +	/* start streaming */
>>> +	cci_write(sensor->regmap, VD56G3_REG_STBY, VD56G3_CMD_START_STREAM,
>>> +		  &ret);
>>> +	vd56g3_poll_reg(sensor, VD56G3_REG_STBY, VD56G3_CMD_ACK, &ret);
>>> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_STREAMING, &ret);
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_stream_off(struct vd56g3 *sensor)
>>> +{
>>> +	int ret = 0;
>>
>> No need to initialise ret.
> 
> Yes, right.
> 
>>
>>> +
>>> +	/* Retrieve Expo cluster to enable coldstart of AE */
>>> +	ret = vd56g3_read_expo_cluster(sensor, true);
>>> +
>>> +	cci_write(sensor->regmap, VD56G3_REG_STREAMING, VD56G3_CMD_STOP_STREAM,
>>> +		  &ret);
>>> +	vd56g3_poll_reg(sensor, VD56G3_REG_STREAMING, VD56G3_CMD_ACK, &ret);
>>> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_s_stream(struct v4l2_subdev *sd, int enable)
>>> +{
>>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>>> +	struct i2c_client *client = v4l2_get_subdevdata(sd);
>>> +	int ret = 0;
>>
>> ret should be declared as last.
>>
>>> +
>>
>> Extra newline.
>>
>>> +	struct v4l2_subdev_state *state =
>>> +		v4l2_subdev_lock_and_get_active_state(sd);
>>
>> Please do the assignment outside declarations---it has side effects.
> 
> Ok, I'll clean the declarations for V2.
> 
>>
>>> +
>>> +	if (enable) {
>>> +		ret = pm_runtime_resume_and_get(&client->dev);
>>> +		if (ret < 0)
>>> +			goto unlock;
>>> +		ret = vd56g3_stream_on(sensor, state);
>>> +		if (ret) {
>>> +			dev_err(&client->dev, "Failed to start streaming\n");
>>> +			pm_runtime_put_sync(&client->dev);
>>> +		}
>>> +	} else {
>>> +		vd56g3_stream_off(sensor);
>>> +		pm_runtime_mark_last_busy(&client->dev);
>>> +		pm_runtime_put_autosuspend(&client->dev);
>>> +	}
>>> +
>>> +unlock:
>>> +	v4l2_subdev_unlock_state(state);
>>> +	if (!ret) {
>>> +		sensor->streaming = enable;
>>
>> This assignment may be done only with sub-device active state locked.
> 
> I'm wondering what I had in mind while writing this ... I'll make it
> proper for V2.
> 
>>
>>> +		v4l2_ctrl_grab(sensor->hflip_ctrl, enable);
>>> +		v4l2_ctrl_grab(sensor->vflip_ctrl, enable);
>>> +		v4l2_ctrl_grab(sensor->patgen_ctrl, enable);
>>> +	}
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static const struct v4l2_subdev_video_ops vd56g3_video_ops = {
>>> +	.s_stream = vd56g3_s_stream,
>>> +};
>>> +
>>> +/* -----------------------------------------------------------------------------
>>> + * Pad ops
>>> + */
>>> +
>>> +/* Media bus code is dependent of :
>>> + *      - 8bits or 10bits output
>>> + *      - variant : Mono or RGB
>>> + *      - H/V flips parameters in case of RGB
>>> + */
>>> +static u32 vd56g3_get_mbus_code(struct vd56g3 *sensor, u32 code)
>>> +{
>>> +	unsigned int i_bpp;
>>> +	unsigned int j;
>>> +
>>> +	for (i_bpp = 0; i_bpp < ARRAY_SIZE(vd56g3_mbus_codes); i_bpp++) {
>>> +		for (j = 0; j < ARRAY_SIZE(vd56g3_mbus_codes[i_bpp]); j++) {
>>> +			if (vd56g3_mbus_codes[i_bpp][j] == code)
>>> +				goto endloops;
>>> +		}
>>> +	}
>>
>> No need for braces. Up to you.
> 
> I'll probably keep them ... habits ?!
> 
>>
>>> +
>>> +endloops:
>>> +	if (i_bpp >= ARRAY_SIZE(vd56g3_mbus_codes))
>>> +		i_bpp = 0;
>>> +
>>> +	if (sensor->is_mono)
>>> +		j = 0;
>>> +	else
>>> +		j = 1 + (sensor->hflip_ctrl->val ? 1 : 0) +
>>> +		    (sensor->vflip_ctrl->val ? 2 : 0);
>>> +
>>> +	return vd56g3_mbus_codes[i_bpp][j];
>>> +}
>>> +
>>> +static int vd56g3_enum_mbus_code(struct v4l2_subdev *sd,
>>> +				 struct v4l2_subdev_state *sd_state,
>>> +				 struct v4l2_subdev_mbus_code_enum *code)
>>> +{
>>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>>> +
>>> +	if (code->index >= ARRAY_SIZE(vd56g3_mbus_codes))
>>> +		return -EINVAL;
>>> +
>>> +	code->code =
>>> +		vd56g3_get_mbus_code(sensor, vd56g3_mbus_codes[code->index][0]);
>>> +
>>> +	return 0;
>>> +}
>>> +
>>> +static int vd56g3_enum_frame_size(struct v4l2_subdev *sd,
>>> +				  struct v4l2_subdev_state *sd_state,
>>> +				  struct v4l2_subdev_frame_size_enum *fse)
>>> +{
>>> +	if (fse->index >= ARRAY_SIZE(vd56g3_supported_modes))
>>> +		return -EINVAL;
>>> +
>>> +	fse->min_width = vd56g3_supported_modes[fse->index].width;
>>> +	fse->max_width = fse->min_width;
>>> +	fse->min_height = vd56g3_supported_modes[fse->index].height;
>>> +	fse->max_height = fse->min_height;
>>> +
>>> +	return 0;
>>> +}
>>> +
>>> +static void vd56g3_update_img_pad_format(struct vd56g3 *sensor,
>>> +					 const struct vd56g3_mode *mode,
>>> +					 u32 mbus_code,
>>> +					 struct v4l2_mbus_framefmt *mbus_fmt)
>>> +{
>>> +	mbus_fmt->width = mode->width;
>>> +	mbus_fmt->height = mode->height;
>>> +	mbus_fmt->code = vd56g3_get_mbus_code(sensor, mbus_code);
>>> +	mbus_fmt->colorspace = V4L2_COLORSPACE_RAW;
>>> +	mbus_fmt->field = V4L2_FIELD_NONE;
>>> +	mbus_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
>>> +	mbus_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
>>> +	mbus_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
>>> +}
>>> +
>>> +static int vd56g3_set_pad_fmt(struct v4l2_subdev *sd,
>>> +			      struct v4l2_subdev_state *sd_state,
>>> +			      struct v4l2_subdev_format *sd_fmt)
>>> +{
>>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>>> +	const struct vd56g3_mode *new_mode;
>>> +	struct v4l2_rect pad_crop;
>>> +	unsigned int binning;
>>> +
>>> +	if (sensor->streaming)
>>> +		return -EBUSY;
>>> +
>>> +	new_mode = v4l2_find_nearest_size(vd56g3_supported_modes,
>>> +					  ARRAY_SIZE(vd56g3_supported_modes),
>>> +					  width, height, sd_fmt->format.width,
>>> +					  sd_fmt->format.height);
>>> +
>>> +	vd56g3_update_img_pad_format(sensor, new_mode, sd_fmt->format.code,
>>> +				     &sd_fmt->format);
>>> +
>>> +	/* Compute crop rectangle (maximized via binning) */
>>> +	binning = min(VD56G3_NATIVE_WIDTH / sd_fmt->format.width,
>>> +		      VD56G3_NATIVE_HEIGHT / sd_fmt->format.height);
>>> +	binning = min(binning, 2U);
>>> +	pad_crop.width = sd_fmt->format.width * binning;
>>> +	pad_crop.height = sd_fmt->format.height * binning;
>>> +	pad_crop.left = (VD56G3_NATIVE_WIDTH - pad_crop.width) / 2;
>>> +	pad_crop.top = (VD56G3_NATIVE_HEIGHT - pad_crop.height) / 2;
>>> +
>>> +	/* Update active state's format and crop */
>>> +	*v4l2_subdev_state_get_format(sd_state, sd_fmt->pad) = sd_fmt->format;
>>> +	*v4l2_subdev_state_get_crop(sd_state, sd_fmt->pad) = pad_crop;
>>> +
>>> +	if (sd_fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
>>> +		vd56g3_update_controls(sensor);
>>> +
>>> +	return 0;
>>> +}
>>> +
>>> +static int vd56g3_get_selection(struct v4l2_subdev *sd,
>>> +				struct v4l2_subdev_state *sd_state,
>>> +				struct v4l2_subdev_selection *sel)
>>> +{
>>> +	switch (sel->target) {
>>> +	case V4L2_SEL_TGT_CROP:
>>> +		sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
>>> +		break;
>>> +	case V4L2_SEL_TGT_NATIVE_SIZE:
>>> +	case V4L2_SEL_TGT_CROP_DEFAULT:
>>> +	case V4L2_SEL_TGT_CROP_BOUNDS:
>>> +		sel->r.top = 0;
>>> +		sel->r.left = 0;
>>> +		sel->r.width = VD56G3_NATIVE_WIDTH;
>>> +		sel->r.height = VD56G3_NATIVE_HEIGHT;
>>> +		break;
>>> +	default:
>>> +		return -EINVAL;
>>> +	}
>>> +
>>> +	return 0;
>>> +}
>>> +
>>> +static int vd56g3_init_state(struct v4l2_subdev *sd,
>>> +			     struct v4l2_subdev_state *sd_state)
>>> +{
>>> +	unsigned int def_mode = VD56G3_DEFAULT_MODE;
>>> +	struct v4l2_subdev_format fmt = {
>>> +		.which = V4L2_SUBDEV_FORMAT_TRY,
>>> +		.pad = 0,
>>> +		.format = {
>>> +			.code = vd56g3_mbus_codes[0][0],
>>> +			.width = vd56g3_supported_modes[def_mode].width,
>>> +			.height = vd56g3_supported_modes[def_mode].height,
>>> +		},
>>> +	};
>>> +
>>> +	vd56g3_set_pad_fmt(sd, sd_state, &fmt);
>>
>> Newline here?
> 
> Ok.
> 
>>
>>> +	return 0;
>>> +}
>>> +
>>> +static const struct v4l2_subdev_core_ops vd56g3_core_ops = {
>>> +	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
>>> +	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
>>> +};
>>> +
>>> +static const struct v4l2_subdev_pad_ops vd56g3_pad_ops = {
>>> +	.enum_mbus_code = vd56g3_enum_mbus_code,
>>> +	.enum_frame_size = vd56g3_enum_frame_size,
>>> +	.get_fmt = v4l2_subdev_get_fmt,
>>> +	.set_fmt = vd56g3_set_pad_fmt,
>>> +	.get_selection = vd56g3_get_selection,
>>> +};
>>> +
>>> +static const struct v4l2_subdev_ops vd56g3_subdev_ops = {
>>> +	.core = &vd56g3_core_ops,
>>> +	.video = &vd56g3_video_ops,
>>> +	.pad = &vd56g3_pad_ops,
>>> +};
>>> +
>>> +static const struct media_entity_operations vd56g3_subdev_entity_ops = {
>>> +	.link_validate = v4l2_subdev_link_validate,
>>> +};
>>> +
>>> +static const struct v4l2_subdev_internal_ops vd56g3_internal_ops = {
>>> +	.init_state = vd56g3_init_state,
>>> +};
>>> +
>>> +/* -----------------------------------------------------------------------------
>>> + * Power management
>>> + */
>>> +
>>> +static int vd56g3_boot(struct vd56g3 *sensor)
>>> +{
>>> +	int ret = 0;
>>> +
>>> +	cci_write(sensor->regmap, VD56G3_REG_BOOT, VD56G3_CMD_BOOT, &ret);
>>> +	vd56g3_poll_reg(sensor, VD56G3_REG_BOOT, VD56G3_CMD_ACK, &ret);
>>> +	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_power_on(struct vd56g3 *sensor)
>>> +{
>>> +	struct i2c_client *client = sensor->i2c_client;
>>> +	int ret;
>>> +
>>> +	ret = regulator_bulk_enable(VD56G3_NUM_SUPPLIES, sensor->supplies);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "Failed to enable regulators %d", ret);
>>> +		return ret;
>>> +	}
>>> +
>>> +	ret = clk_prepare_enable(sensor->xclk);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "Failed to enable clock %d", ret);
>>> +		goto disable_reg;
>>> +	}
>>> +
>>> +	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
>>> +	ret = vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_READY_TO_BOOT, NULL);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "Sensor reset failed %d\n", ret);
>>> +		goto disable_clock;
>>> +	}
>>> +
>>> +	return 0;
>>> +
>>> +disable_clock:
>>> +	clk_disable_unprepare(sensor->xclk);
>>> +	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
>>> +disable_reg:
>>> +	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_power_off(struct vd56g3 *sensor)
>>> +{
>>> +	clk_disable_unprepare(sensor->xclk);
>>> +	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
>>> +	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
>>> +	return 0;
>>> +}
>>> +
>>> +static int vd56g3_runtime_resume(struct device *dev)
>>> +{
>>> +	struct v4l2_subdev *sd = dev_get_drvdata(dev);
>>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>>> +	struct i2c_client *client = sensor->i2c_client;
>>> +	int ret;
>>> +
>>> +	ret = vd56g3_power_on(sensor);
>>
>> I think you could unify the runtime resume and suspend functions with the
>> power_on/off, using struct device * argument. vd56g3_power_on() won't be
>> called separately from vd65g3_boot either.
> 
> I'll give it a try for V2.
> 
>>
>> Speaking of which---do you really call vd65g3_boot() before identifying the
>> sensor?
> 
> Good point :-)
> 
> In fact on older HW cuts (I hope this won't happens in the future), we
> need to apply some FW patches. Once those patches are applied, sensor
> need to be (re)booted : that's why I decoupled vd56g3_power_on() and
> vd65g3_boot().
> That being said, for the current submission this serves nothing : I may
> try to gathered those two functions.
> 
>>
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "Failed to power on %d", ret);
>>> +		return ret;
>>> +	}
>>> +
>>> +	ret = vd56g3_boot(sensor);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "sensor boot failed %d", ret);
>>> +		return ret;
>>> +	}
>>> +
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_runtime_suspend(struct device *dev)
>>> +{
>>> +	struct v4l2_subdev *sd = dev_get_drvdata(dev);
>>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>>> +
>>> +	return vd56g3_power_off(sensor);
>>> +}
>>> +
>>> +static const struct dev_pm_ops vd56g3_pm_ops = {
>>> +	SET_RUNTIME_PM_OPS(vd56g3_runtime_suspend, vd56g3_runtime_resume, NULL)
>>> +};
>>> +
>>> +/* -----------------------------------------------------------------------------
>>> + * Probe and initialization
>>> + */
>>> +
>>> +static int vd56g3_check_csi_conf(struct vd56g3 *sensor,
>>> +				 struct fwnode_handle *endpoint)
>>> +{
>>> +	struct i2c_client *client = sensor->i2c_client;
>>> +	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
>>> +	u32 phy_data_lanes[VD56G3_MAX_CSI_DATA_LANES] = { ~0, ~0 };
>>> +	int n_lanes;
>>> +	int frequency;
>>> +	int p, l;
>>> +	int ret = 0;
>>> +
>>> +	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
>>> +	if (ret)
>>> +		return -EINVAL;
>>> +
>>> +	/* Check lanes number */
>>> +	n_lanes = ep.bus.mipi_csi2.num_data_lanes;
>>> +	if (n_lanes != 1 && n_lanes != 2) {
>>> +		dev_err(&client->dev, "Invalid data lane number %d\n", n_lanes);
>>> +		ret = -EINVAL;
>>> +		goto done;
>>> +	}
>>> +	sensor->nb_of_lane = n_lanes;
>>> +
>>> +	/* Clock lane must be first */
>>> +	if (ep.bus.mipi_csi2.clock_lane != 0) {
>>> +		dev_err(&client->dev, "Clk lane must be mapped to lane 0\n");
>>> +		ret = -EINVAL;
>>> +		goto done;
>>> +	}
>>> +
>>> +	/* Prepare Output Interface conf based on lane settings
>>> +	 * logical to physical lane conversion (+ pad remaining slots)
>>> +	 */
>>> +	for (l = 0; l < n_lanes; l++)
>>> +		phy_data_lanes[ep.bus.mipi_csi2.data_lanes[l] - 1] = l;
>>> +	for (p = 0; p < VD56G3_MAX_CSI_DATA_LANES; p++) {
>>> +		if (phy_data_lanes[p] != ~0)
>>> +			continue;
>>> +		phy_data_lanes[p] = l;
>>> +		l++;
>>> +	}
>>> +	sensor->oif_ctrl = n_lanes |
>>> +			   (ep.bus.mipi_csi2.lane_polarities[0] << 3) |
>>> +			   ((phy_data_lanes[0]) << 4) |
>>> +			   (ep.bus.mipi_csi2.lane_polarities[1] << 6) |
>>> +			   ((phy_data_lanes[1]) << 7) |
>>> +			   (ep.bus.mipi_csi2.lane_polarities[2] << 9);
>>> +
>>> +	/* Check link frequency */
>>> +	if (!ep.nr_of_link_frequencies) {
>>> +		dev_err(&client->dev, "link-frequency not found in DT\n");
>>> +		ret = -EINVAL;
>>> +		goto done;
>>> +	}
>>> +	frequency = (n_lanes == 2) ? VD56G3_LINK_FREQ_DEF_2LANES :
>>> +				     VD56G3_LINK_FREQ_DEF_1LANE;
>>> +	if (ep.nr_of_link_frequencies != 1 ||
>>> +	    ep.link_frequencies[0] != frequency) {
>>> +		dev_err(&client->dev, "Link frequency not supported: %lld\n",
>>> +			ep.link_frequencies[0]);
>>> +		ret = -EINVAL;
>>> +		goto done;
>>> +	}
>>> +
>>> +done:
>>> +	v4l2_fwnode_endpoint_free(&ep);
>>> +	return ret;
>>> +}
>>> +
>>> +static int vd56g3_parse_dt_gpios_array(struct vd56g3 *sensor, char *prop_name,
>>> +				       u32 *array, int *nb)
>>> +{
>>> +	struct i2c_client *client = sensor->i2c_client;
>>> +	struct device_node *np = client->dev.of_node;
>>> +	unsigned int i;
>>> +
>>> +	*nb = of_property_read_variable_u32_array(np, prop_name, array, 0,
>>> +						  VD56G3_NB_GPIOS);
>>> +
>>> +	if (*nb == -EINVAL) {
>>> +		*nb = 0;
>>> +		return *nb;
>>> +	} else if (*nb < 0) {
>>> +		dev_err(&client->dev, "Failed to read %s prop\n", prop_name);
>>> +		return *nb;
>>> +	}
>>> +
>>> +	for (i = 0; i < *nb; i++) {
>>> +		if (array[i] >= VD56G3_NB_GPIOS) {
>>> +			dev_err(&client->dev, "Invalid GPIO : %d\n", array[i]);
>>> +			return -EINVAL;
>>> +		}
>>> +	}
>>> +
>>> +	return 0;
>>> +}
>>> +
>>> +static int vd56g3_parse_dt_gpios(struct vd56g3 *sensor)
>>> +{
>>> +	u32 led_gpios[VD56G3_NB_GPIOS];
>>> +	int nb_gpios_leds;
>>> +	int ret;
>>> +	unsigned int i;
>>> +
>>> +	/* Initialize GPIOs to default */
>>> +	for (i = 0; i < VD56G3_NB_GPIOS; i++)
>>> +		sensor->gpios[i] = VD56G3_GPIOX_GPIO_IN;
>>> +	sensor->ext_leds_mask = 0;
>>> +
>>> +	/* Take into account optional 'st,leds' output for GPIOs */
>>> +	ret = vd56g3_parse_dt_gpios_array(sensor, "st,leds", led_gpios,
>>> +					  &nb_gpios_leds);
>>> +	if (ret)
>>> +		return ret;
>>> +	for (i = 0; i < nb_gpios_leds; i++) {
>>> +		sensor->gpios[led_gpios[i]] = VD56G3_GPIOX_STROBE_MODE;
>>> +		set_bit(led_gpios[i], &sensor->ext_leds_mask);
>>> +	}
>>> +
>>> +	return 0;
>>> +}
>>> +
>>> +static int vd56g3_parse_dt(struct vd56g3 *sensor)
>>> +{
>>> +	struct i2c_client *client = sensor->i2c_client;
>>> +	struct device *dev = &client->dev;
>>> +	struct fwnode_handle *endpoint;
>>> +	int ret;
>>> +
>>> +	endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0, 0);
>>> +	if (!endpoint) {
>>> +		dev_err(dev, "endpoint node not found\n");
>>> +		return -EINVAL;
>>> +	}
>>> +
>>> +	ret = vd56g3_check_csi_conf(sensor, endpoint);
>>> +	fwnode_handle_put(endpoint);
>>> +	if (ret)
>>> +		return ret;
>>> +
>>> +	ret = vd56g3_parse_dt_gpios(sensor);
>>
>> 	return ...;
> 
> Shame on me ... ^^.
> Will be applied on V2.
> 
>>
>>> +	if (ret)
>>> +		return ret;
>>> +
>>> +	return 0;
>>> +}
>>> +
>>> +static int vd56g3_get_regulators(struct vd56g3 *sensor)
>>> +{
>>> +	int i;
>>
>> unsigned int i?
>>
>>> +
>>> +	for (i = 0; i < VD56G3_NUM_SUPPLIES; i++)
>>> +		sensor->supplies[i].supply = vd56g3_supply_names[i];
>>> +
>>> +	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
>>> +				       VD56G3_NUM_SUPPLIES, sensor->supplies);
>>> +}
>>> +
>>> +static int vd56g3_prepare_clock_tree(struct vd56g3 *sensor)
>>> +{
>>> +	struct i2c_client *client = sensor->i2c_client;
>>> +	const unsigned int predivs[] = { 1, 2, 4 };
>>> +	u32 pll_out;
>>> +	int i;
>>> +
>>> +	/* External clock must be in [6Mhz-27Mhz] */
>>> +	if (sensor->ext_clock < 6 * HZ_PER_MHZ ||
>>> +	    sensor->ext_clock > 27 * HZ_PER_MHZ) {
>>> +		dev_err(&client->dev,
>>> +			"Only 6Mhz-27Mhz clock range supported. Provided %lu MHz\n",
>>> +			sensor->ext_clock / HZ_PER_MHZ);
>>> +		return -EINVAL;
>>> +	}
>>> +
>>> +	/* PLL input should be in [6Mhz-12Mhz[ */
>>> +	for (i = 0; i < ARRAY_SIZE(predivs); i++) {
>>> +		sensor->pll_prediv = predivs[i];
>>> +		if (sensor->ext_clock / sensor->pll_prediv < 12 * HZ_PER_MHZ)
>>> +			break;
>>> +	}
>>> +
>>> +	/* PLL output clock must be as close as possible to 804Mhz */
>>> +	sensor->pll_mult = (VD56G3_TARGET_PLL * sensor->pll_prediv +
>>> +			    sensor->ext_clock / 2) /
>>> +			   sensor->ext_clock;
>>> +	pll_out = sensor->ext_clock * sensor->pll_mult / sensor->pll_prediv;
>>> +
>>> +	/* Target Pixel Clock for standard 10bit ADC mode : 160.8Mhz */
>>> +	sensor->pixel_clock = pll_out / VD56G3_VT_CLOCK_DIV;
>>> +
>>> +	return 0;
>>> +}
>>> +
>>> +static int vd56g3_detect(struct vd56g3 *sensor)
>>> +{
>>> +	struct i2c_client *client = sensor->i2c_client;
>>> +	struct device *dev = &client->dev;
>>> +	unsigned int model;
>>> +	u64 model_id = 0;
>>> +	u64 device_revision = 0;
>>> +	u64 optical_revision = 0;
>>> +	int ret = 0;
>>> +
>>> +	model = (uintptr_t)device_get_match_data(dev);
>>> +
>>> +	cci_read(sensor->regmap, VD56G3_REG_MODEL_ID, &model_id, &ret);
>>> +	if (ret)
>>> +		return ret;
>>> +
>>> +	if (model_id != VD56G3_MODEL_ID) {
>>> +		dev_warn(&client->dev, "Unsupported sensor id %x",
>>> +			 (u16)model_id);
>>> +		return -ENODEV;
>>> +	}
>>> +
>>> +	cci_read(sensor->regmap, VD56G3_REG_REVISION, &device_revision, &ret);
>>> +	if (ret)
>>> +		return ret;
>>> +
>>> +	if ((device_revision >> 8) != 0x31) {
>>> +		dev_warn(&client->dev, "Unsupported Cut version %x",
>>> +			 (u16)device_revision);
>>> +		return -ENODEV;
>>> +	}
>>> +
>>> +	cci_read(sensor->regmap, VD56G3_REG_OPTICAL_REVISION, &optical_revision,
>>> +		 &ret);
>>> +	if (ret)
>>> +		return ret;
>>> +
>>> +	sensor->is_mono =
>>> +		((optical_revision & 1) == VD56G3_OPTICAL_REVISION_MONO);
>>> +	if ((sensor->is_mono && model == VD56G3_MODEL_VD66GY) ||
>>> +	    (!sensor->is_mono && model == VD56G3_MODEL_VD56G3)) {
>>> +		dev_warn(&client->dev,
>>
>> As you're returning an error, I'd use dev_err().
> 
> Correct, I'll also update the 2 upper dev_warn() - same situation.
> 
>>
>>> +			 "Found %s sensor, while %s model is defined in DT",
>>> +			 (sensor->is_mono) ? "Mono" : "Bayer",
>>> +			 (model == VD56G3_MODEL_VD56G3) ? "vd56g3" : "vd66gy");
>>> +		return -ENODEV;
>>> +	}
>>> +
>>> +	return 0;
>>> +}
>>> +
>>> +static int vd56g3_subdev_init(struct vd56g3 *sensor)
>>> +{
>>> +	struct i2c_client *client = sensor->i2c_client;
>>> +	int ret;
>>> +
>>> +	/* Init sub device */
>>> +	v4l2_i2c_subdev_init(&sensor->sd, client, &vd56g3_subdev_ops);
>>> +	sensor->sd.internal_ops = &vd56g3_internal_ops;
>>> +	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
>>> +			    V4L2_SUBDEV_FL_HAS_EVENTS;
>>> +	sensor->sd.entity.ops = &vd56g3_subdev_entity_ops;
>>> +
>>> +	/* Init source pad */
>>> +	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
>>> +	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
>>> +	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "Failed to init media entity : %d", ret);
>>> +		return ret;
>>> +	}
>>> +
>>> +	/* Init controls */
>>> +	ret = vd56g3_init_controls(sensor);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "Controls initialization failed %d", ret);
>>> +		goto err_media;
>>> +	}
>>> +
>>> +	/* Init vd56g3 struct : default resolution + raw8 */
>>> +	sensor->streaming = false;
>>
>> sensor->streaming was already false (initialised to zero).
> 
> Elegant - however not very explicit (I hadn't even thought of it)
> 
>>
>>> +	sensor->sd.state_lock = sensor->ctrl_handler.lock;
>>> +	ret = v4l2_subdev_init_finalize(&sensor->sd);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "subdev init error: %d", ret);
>>> +		goto err_ctrls;
>>> +	}
>>> +
>>> +	vd56g3_update_controls(sensor);
>>
>> Error handling is missing.
> 
> Good point, I'll update it for V2.
> 
>>
>>> +
>>> +	return 0;
>>> +
>>> +err_ctrls:
>>> +	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
>>> +
>>> +err_media:
>>> +	media_entity_cleanup(&sensor->sd.entity);
>>> +	return ret;
>>> +}
>>> +
>>> +static void vd56g3_subdev_cleanup(struct vd56g3 *sensor)
>>> +{
>>> +	v4l2_async_unregister_subdev(&sensor->sd);
>>> +	v4l2_subdev_cleanup(&sensor->sd);
>>> +	media_entity_cleanup(&sensor->sd.entity);
>>> +	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
>>> +}
>>> +
>>> +static int vd56g3_probe(struct i2c_client *client)
>>> +{
>>> +	struct device *dev = &client->dev;
>>> +	struct vd56g3 *sensor;
>>> +	int ret;
>>> +
>>> +	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
>>> +	if (!sensor)
>>> +		return -ENOMEM;
>>> +
>>> +	sensor->i2c_client = client;
>>> +
>>> +	ret = vd56g3_parse_dt(sensor);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "Failed to parse Device Tree : %d", ret);
>>> +		return ret;
>>> +	}
>>> +
>>> +	/* Get (and check) resources : power regs, ext clock, reset gpio */
>>> +	ret = vd56g3_get_regulators(sensor);
>>> +	if (ret)
>>> +		return dev_err_probe(dev, ret, "Failed to get regulators.");
>>> +
>>> +	sensor->xclk = devm_clk_get(dev, NULL);
>>> +	if (IS_ERR(sensor->xclk))
>>> +		return dev_err_probe(dev, PTR_ERR(sensor->xclk),
>>> +				     "Failed to get xclk.");
>>> +	sensor->ext_clock = clk_get_rate(sensor->xclk);
>>> +	ret = vd56g3_prepare_clock_tree(sensor);
>>> +	if (ret)
>>> +		return ret;
>>> +
>>> +	sensor->reset_gpio =
>>> +		devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
>>> +	if (IS_ERR(sensor->reset_gpio))
>>> +		return dev_err_probe(dev, PTR_ERR(sensor->reset_gpio),
>>> +				     "Failed to get reset gpio.");
>>> +
>>> +	sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
>>> +	if (IS_ERR(sensor->regmap))
>>> +		return dev_err_probe(dev, PTR_ERR(sensor->regmap),
>>> +				     "Failed to init regmap.");
>>> +
>>> +	/* Power ON */
>>> +	ret = vd56g3_power_on(sensor);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "Sensor power on failed : %d", ret);
>>> +		return ret;
>>> +	}
>>> +
>>> +	/* Enable PM runtime with autosuspend (sensor being ON, set active) */
>>> +	pm_runtime_set_active(dev);
>>> +	pm_runtime_get_noresume(dev);
>>> +	pm_runtime_enable(dev);
>>> +	pm_runtime_set_autosuspend_delay(dev, 1000);
>>> +	pm_runtime_use_autosuspend(dev);
>>> +
>>> +	/* Check HW model/version */
>>> +	ret = vd56g3_boot(sensor);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "Sensor boot failed : %d", ret);
>>> +		goto err_power_off;
>>> +	}
>>> +
>>> +	ret = vd56g3_detect(sensor);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "Sensor detect failed : %d", ret);
>>> +		goto err_power_off;
>>> +	}
>>> +
>>> +	/* Initialize, then register V4L2 subdev */
>>> +	ret = vd56g3_subdev_init(sensor);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "V4l2 init failed : %d", ret);
>>> +		goto err_power_off;
>>> +	}
>>> +
>>> +	ret = v4l2_async_register_subdev(&sensor->sd);
>>> +	if (ret) {
>>> +		dev_err(&client->dev, "async subdev register failed %d", ret);
>>> +		goto err_subdev;
>>> +	}
>>> +
>>> +	/* Sensor could now be powered off (after the autosuspend delay) */
>>> +	pm_runtime_mark_last_busy(dev);
>>> +	pm_runtime_put_autosuspend(dev);
>>> +
>>> +	dev_dbg(&client->dev, "Successfully probe %s sensor",
>>> +		(sensor->is_mono) ? "vd56g3" : "vd66gy");
>>> +
>>> +	return 0;
>>> +
>>> +err_subdev:
>>> +	vd56g3_subdev_cleanup(sensor);
>>> +err_power_off:
>>> +	pm_runtime_disable(dev);
>>> +	pm_runtime_put_noidle(dev);
>>> +	vd56g3_power_off(sensor);
>>> +	return ret;
>>> +}
>>> +
>>> +static void vd56g3_remove(struct i2c_client *client)
>>> +{
>>> +	struct v4l2_subdev *sd = i2c_get_clientdata(client);
>>> +	struct vd56g3 *sensor = to_vd56g3(sd);
>>> +
>>> +	vd56g3_subdev_cleanup(sensor);
>>> +
>>> +	pm_runtime_disable(&client->dev);
>>> +	if (!pm_runtime_status_suspended(&client->dev))
>>> +		vd56g3_power_off(sensor);
>>> +	pm_runtime_set_suspended(&client->dev);
>>> +}
>>> +
>>> +static const struct of_device_id vd56g3_dt_ids[] = {
>>> +	{ .compatible = "st,st-vd56g3", .data = (void *)VD56G3_MODEL_VD56G3 },
>>> +	{ .compatible = "st,st-vd66gy", .data = (void *)VD56G3_MODEL_VD66GY },
>>> +	{ /* sentinel */ }
>>> +};
>>> +MODULE_DEVICE_TABLE(of, vd56g3_dt_ids);
>>> +
>>> +static struct i2c_driver vd56g3_i2c_driver = {
>>> +	.driver = {
>>> +		.name  = "st-vd56g3",
>>> +		.of_match_table = vd56g3_dt_ids,
>>> +		.pm = &vd56g3_pm_ops,
>>> +	},
>>> +	.probe = vd56g3_probe,
>>> +	.remove = vd56g3_remove,
>>> +};
>>> +
>>> +module_i2c_driver(vd56g3_i2c_driver);
>>> +
>>> +MODULE_AUTHOR("Benjamin Mugnier <[e-mail]>");
>>> +MODULE_AUTHOR("Mickael Guene <[e-mail]>");
>>> +MODULE_AUTHOR("Sylvain Petinot <[e-mail]>");
>>> +MODULE_DESCRIPTION("ST VD56G3 sensor driver");
>>> +MODULE_LICENSE("GPL");
>>
> 
> --
> Sylvain

Sakari Ailus May 27, 2024, 7:31 a.m. UTC | #8

Hi Benjamin,

On Mon, May 13, 2024 at 03:37:25PM +0200, Benjamin Mugnier wrote:
> >>> +static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
> >>> +{
> >>> +	u64 exposure = 0;
> >>> +	u64 again = 0;
> >>> +	u64 dgain = 0;
> >>
> >> Do you need the assignments? The values are assigned by cci_read() below,
> >> right?
> > 
> > Well initially, I didn't have those assignments and some checker
> > complains (Honestly I didn't investigate the warning).
> > I will double check and see if they are necessary.
> > 
> 
> Since I have smatch ready, I ran it without these initialization against
> the latest media tree. I got these :
> 
> drivers/media/i2c/st-vd56g3.c:403 vd56g3_read_expo_cluster() error:
> uninitialized symbol 'exposure'.
> drivers/media/i2c/st-vd56g3.c:404 vd56g3_read_expo_cluster() error:
> uninitialized symbol 'again'.
> drivers/media/i2c/st-vd56g3.c:405 vd56g3_read_expo_cluster() error:
> uninitialized symbol 'dgain'.
> drivers/media/i2c/st-vd56g3.c:407 vd56g3_read_expo_cluster() error:
> uninitialized symbol 'exposure'.
> drivers/media/i2c/st-vd56g3.c:408 vd56g3_read_expo_cluster() error:
> uninitialized symbol 'again'.
> drivers/media/i2c/st-vd56g3.c:409 vd56g3_read_expo_cluster() error:
> uninitialized symbol 'dgain'.
> 
> cci_read() is indeed not modifying its 'var' argument in the default
> case of the switch [1]. Spawning these errors.
> Adding a '*val = 0' in the default case would also fix it, but will
> change the function behavior.

I think I'd add the assignment to cci_read(). This isn't the only driver
that's affected. In the best case smatch would be fixed to figure this out.

Any thoughts?

Cc also Hans and Laurent.

Benjamin Mugnier May 27, 2024, 7:44 a.m. UTC | #9

Hi Sakari,

On 5/27/24 09:31, Sakari Ailus wrote:
> Hi Benjamin,
> 
> On Mon, May 13, 2024 at 03:37:25PM +0200, Benjamin Mugnier wrote:
>>>>> +static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
>>>>> +{
>>>>> +	u64 exposure = 0;
>>>>> +	u64 again = 0;
>>>>> +	u64 dgain = 0;
>>>>
>>>> Do you need the assignments? The values are assigned by cci_read() below,
>>>> right?
>>>
>>> Well initially, I didn't have those assignments and some checker
>>> complains (Honestly I didn't investigate the warning).
>>> I will double check and see if they are necessary.
>>>
>>
>> Since I have smatch ready, I ran it without these initialization against
>> the latest media tree. I got these :
>>
>> drivers/media/i2c/st-vd56g3.c:403 vd56g3_read_expo_cluster() error:
>> uninitialized symbol 'exposure'.
>> drivers/media/i2c/st-vd56g3.c:404 vd56g3_read_expo_cluster() error:
>> uninitialized symbol 'again'.
>> drivers/media/i2c/st-vd56g3.c:405 vd56g3_read_expo_cluster() error:
>> uninitialized symbol 'dgain'.
>> drivers/media/i2c/st-vd56g3.c:407 vd56g3_read_expo_cluster() error:
>> uninitialized symbol 'exposure'.
>> drivers/media/i2c/st-vd56g3.c:408 vd56g3_read_expo_cluster() error:
>> uninitialized symbol 'again'.
>> drivers/media/i2c/st-vd56g3.c:409 vd56g3_read_expo_cluster() error:
>> uninitialized symbol 'dgain'.
>>
>> cci_read() is indeed not modifying its 'var' argument in the default
>> case of the switch [1]. Spawning these errors.
>> Adding a '*val = 0' in the default case would also fix it, but will
>> change the function behavior.
> 
> I think I'd add the assignment to cci_read(). This isn't the only driver
> that's affected. In the best case smatch would be fixed to figure this out.
> 
> Any thoughts?

IMHO adding the assignment to cci_read() is the cleanest way to handle it.

> 
> Cc also Hans and Laurent.
>

Sakari Ailus May 27, 2024, 7:45 a.m. UTC | #10

Hi Benjamin,

On Mon, May 27, 2024 at 09:44:15AM +0200, Benjamin Mugnier wrote:
> Hi Sakari,
> 
> On 5/27/24 09:31, Sakari Ailus wrote:
> > Hi Benjamin,
> > 
> > On Mon, May 13, 2024 at 03:37:25PM +0200, Benjamin Mugnier wrote:
> >>>>> +static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
> >>>>> +{
> >>>>> +	u64 exposure = 0;
> >>>>> +	u64 again = 0;
> >>>>> +	u64 dgain = 0;
> >>>>
> >>>> Do you need the assignments? The values are assigned by cci_read() below,
> >>>> right?
> >>>
> >>> Well initially, I didn't have those assignments and some checker
> >>> complains (Honestly I didn't investigate the warning).
> >>> I will double check and see if they are necessary.
> >>>
> >>
> >> Since I have smatch ready, I ran it without these initialization against
> >> the latest media tree. I got these :
> >>
> >> drivers/media/i2c/st-vd56g3.c:403 vd56g3_read_expo_cluster() error:
> >> uninitialized symbol 'exposure'.
> >> drivers/media/i2c/st-vd56g3.c:404 vd56g3_read_expo_cluster() error:
> >> uninitialized symbol 'again'.
> >> drivers/media/i2c/st-vd56g3.c:405 vd56g3_read_expo_cluster() error:
> >> uninitialized symbol 'dgain'.
> >> drivers/media/i2c/st-vd56g3.c:407 vd56g3_read_expo_cluster() error:
> >> uninitialized symbol 'exposure'.
> >> drivers/media/i2c/st-vd56g3.c:408 vd56g3_read_expo_cluster() error:
> >> uninitialized symbol 'again'.
> >> drivers/media/i2c/st-vd56g3.c:409 vd56g3_read_expo_cluster() error:
> >> uninitialized symbol 'dgain'.
> >>
> >> cci_read() is indeed not modifying its 'var' argument in the default
> >> case of the switch [1]. Spawning these errors.
> >> Adding a '*val = 0' in the default case would also fix it, but will
> >> change the function behavior.
> > 
> > I think I'd add the assignment to cci_read(). This isn't the only driver
> > that's affected. In the best case smatch would be fixed to figure this out.
> > 
> > Any thoughts?
> 
> IMHO adding the assignment to cci_read() is the cleanest way to handle it.

A fix in smatch would be the best in my opinion as it could catch the use
of the value in case of an error. But it's not going to be a short term
solution, hence I'd also assign *val to 0 now.

> 
> > 
> > Cc also Hans and Laurent.
> > 
>

Patch

diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index 56f276b920ab..a2d7271eef88 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -657,6 +657,17 @@  config VIDEO_S5K6A3
 	  This is a V4L2 sensor driver for Samsung S5K6A3 raw
 	  camera sensor.
 
+config VIDEO_ST_VD56G3
+	tristate "ST VD56G3 sensor support"
+	select V4L2_CCI_I2C
+	depends on OF && GPIOLIB
+	help
+	  This is a Video4Linux2 sensor driver for the ST VD56G3
+	  camera sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called st-vd56g3.
+
 config VIDEO_ST_VGXY61
 	tristate "ST VGXY61 sensor support"
 	select V4L2_CCI_I2C
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index dfbe6448b549..b469cf0f8113 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -124,6 +124,7 @@  obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
 obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
 obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
 obj-$(CONFIG_VIDEO_ST_MIPID02) += st-mipid02.o
+obj-$(CONFIG_VIDEO_ST_VD56G3) += st-vd56g3.o
 obj-$(CONFIG_VIDEO_ST_VGXY61) += st-vgxy61.o
 obj-$(CONFIG_VIDEO_TC358743) += tc358743.o
 obj-$(CONFIG_VIDEO_TC358746) += tc358746.o
diff --git a/drivers/media/i2c/st-vd56g3.c b/drivers/media/i2c/st-vd56g3.c
new file mode 100644
index 000000000000..eff819941456
--- /dev/null
+++ b/drivers/media/i2c/st-vd56g3.c
@@ -0,0 +1,1619 @@ 
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * A V4L2 driver for ST VD56G3 (Mono) and VD66GY (RGB) global shutter cameras.
+ * Copyright (C) 2024, STMicroelectronics SA
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/units.h>
+
+#include <asm/unaligned.h>
+
+#include <media/mipi-csi2.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/* Register Map */
+#define VD56G3_REG_MODEL_ID				CCI_REG16_LE(0x0000)
+#define VD56G3_MODEL_ID					0x5603
+#define VD56G3_REG_REVISION				CCI_REG16_LE(0x0002)
+#define VD56G3_REG_OPTICAL_REVISION			CCI_REG8(0x001a)
+#define VD56G3_OPTICAL_REVISION_MONO			0
+#define VD56G3_OPTICAL_REVISION_BAYER			1
+#define VD56G3_REG_SYSTEM_FSM				CCI_REG8(0x0028)
+#define VD56G3_SYSTEM_FSM_READY_TO_BOOT			0x01
+#define VD56G3_SYSTEM_FSM_SW_STBY			0x02
+#define VD56G3_SYSTEM_FSM_STREAMING			0x03
+#define VD56G3_REG_APPLIED_COARSE_EXPOSURE		CCI_REG16_LE(0x0064)
+#define VD56G3_REG_APPLIED_ANALOG_GAIN			CCI_REG8(0x0068)
+#define VD56G3_REG_APPLIED_DIGITAL_GAIN			CCI_REG16_LE(0x006a)
+#define VD56G3_REG_BOOT					CCI_REG8(0x0200)
+#define VD56G3_CMD_ACK					0
+#define VD56G3_CMD_BOOT					1
+#define VD56G3_REG_STBY					CCI_REG8(0x0201)
+#define VD56G3_CMD_START_STREAM				1
+#define VD56G3_REG_STREAMING				CCI_REG8(0x0202)
+#define VD56G3_CMD_STOP_STREAM				1
+#define VD56G3_REG_EXT_CLOCK				CCI_REG32_LE(0x0220)
+#define VD56G3_REG_CLK_PLL_PREDIV			CCI_REG8(0x0224)
+#define VD56G3_REG_CLK_SYS_PLL_MULT			CCI_REG8(0x0226)
+#define VD56G3_REG_ORIENTATION				CCI_REG8(0x0302)
+#define VD56G3_REG_FORMAT_CTRL				CCI_REG8(0x030a)
+#define VD56G3_REG_OIF_CTRL				CCI_REG16_LE(0x030c)
+#define VD56G3_REG_OIF_IMG_CTRL				CCI_REG8(0x030f)
+#define VD56G3_REG_OIF_CSI_BITRATE			CCI_REG16_LE(0x0312)
+#define VD56G3_REG_DUSTER_CTRL				CCI_REG8(0x0318)
+#define VD56G3_DUSTER_DISABLE				0
+#define VD56G3_DUSTER_ENABLE_DEF_MODULES		0x13
+#define VD56G3_REG_ISL_ENABLE				CCI_REG8(0x0333)
+#define VD56G3_REG_DARKCAL_CTRL				CCI_REG8(0x0340)
+#define VD56G3_DARKCAL_ENABLE				1
+#define VD56G3_DARKCAL_DISABLE_DARKAVG			2
+#define VD56G3_REG_PATGEN_CTRL				CCI_REG16_LE(0x0400)
+#define VD56G3_PATGEN_ENABLE				1
+#define VD56G3_PATGEN_TYPE_SHIFT			4
+#define VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE		CCI_REG16_LE(0x042a)
+#define VD56G3_REG_AE_COLDSTART_ANALOG_GAIN		CCI_REG8(0x042c)
+#define VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN		CCI_REG16_LE(0x042e)
+#define VD56G3_REG_AE_ROI_START_H			CCI_REG16_LE(0x0432)
+#define VD56G3_REG_AE_ROI_START_V			CCI_REG16_LE(0x0434)
+#define VD56G3_REG_AE_ROI_END_H				CCI_REG16_LE(0x0436)
+#define VD56G3_REG_AE_ROI_END_V				CCI_REG16_LE(0x0438)
+#define VD56G3_REG_AE_COMPENSATION			CCI_REG16_LE(0x043a)
+#define VD56G3_REG_EXP_MODE				CCI_REG8(0x044c)
+#define VD56G3_EXP_MODE_AUTO				0
+#define VD56G3_EXP_MODE_FREEZE				1
+#define VD56G3_EXP_MODE_MANUAL				2
+#define VD56G3_REG_MANUAL_ANALOG_GAIN			CCI_REG8(0x044d)
+#define VD56G3_REG_MANUAL_COARSE_EXPOSURE		CCI_REG16_LE(0x044e)
+#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0		CCI_REG16_LE(0x0450)
+#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1		CCI_REG16_LE(0x0452)
+#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2		CCI_REG16_LE(0x0454)
+#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3		CCI_REG16_LE(0x0456)
+#define VD56G3_REG_FRAME_LENGTH				CCI_REG16_LE(0x0458)
+#define VD56G3_REG_Y_START				CCI_REG16_LE(0x045a)
+#define VD56G3_REG_Y_END				CCI_REG16_LE(0x045c)
+#define VD56G3_REG_OUT_ROI_X_START			CCI_REG16_LE(0x045e)
+#define VD56G3_REG_OUT_ROI_X_END			CCI_REG16_LE(0x0460)
+#define VD56G3_REG_OUT_ROI_Y_START			CCI_REG16_LE(0x0462)
+#define VD56G3_REG_OUT_ROI_Y_END			CCI_REG16_LE(0x0464)
+#define VD56G3_REG_GPIO_0_CTRL				CCI_REG8(0x0467)
+#define VD56G3_GPIOX_GPIO_IN				0x01
+#define VD56G3_GPIOX_STROBE_MODE			0x02
+#define VD56G3_REG_READOUT_CTRL				CCI_REG8(0x047e)
+#define READOUT_NORMAL					0x00
+#define READOUT_DIGITAL_BINNING_X2			0x01
+
+/*
+ * The VD56G3 pixel array is organized as follows:
+ *
+ * +--------------------------------+
+ * |                                | \
+ * |   +------------------------+   |  |
+ * |   |                        |   |  |
+ * |   |                        |   |  |
+ * |   |                        |   |  |
+ * |   |                        |   |  |
+ * |   |                        |   |  |
+ * |   |   Default resolution   |   |  | Native height (1364)
+ * |   |      1120 x 1360       |   |  |
+ * |   |                        |   |  |
+ * |   |                        |   |  |
+ * |   |                        |   |  |
+ * |   |                        |   |  |
+ * |   +------------------------+   |  |
+ * |                                | /
+ * +--------------------------------+
+ *   <----------------------------->
+ *                     \-------------------  Native width (1124)
+ *
+ * The native resolution is 1124x1364.
+ * The recommended/default resolution is 1120x1360 (multiple of 16).
+ */
+#define VD56G3_NATIVE_WIDTH				1124
+#define VD56G3_NATIVE_HEIGHT				1364
+#define VD56G3_DEFAULT_WIDTH				1120
+#define VD56G3_DEFAULT_HEIGHT				1360
+#define VD56G3_DEFAULT_MODE				1
+
+/* PLL settings */
+#define VD56G3_TARGET_PLL				804000000UL
+#define VD56G3_VT_CLOCK_DIV				5
+
+/* Line length and Frame length (settings are for standard 10bits ADC mode) */
+#define VD56G3_LINE_LENGTH_MIN				1236
+#define VD56G3_VBLANK_MIN				110
+#define VD56G3_FRAME_LENGTH_DEF_60FPS			2168
+
+/* Exposure settings */
+#define VD56G3_EXPOSURE_MARGIN				75
+#define VD56G3_EXPOSURE_DEFAULT				1420
+
+/* Output Interface settings */
+#define VD56G3_MAX_CSI_DATA_LANES			2
+#define VD56G3_LINK_FREQ_DEF_1LANE			750000000UL
+#define VD56G3_LINK_FREQ_DEF_2LANES			402000000UL
+
+/* GPIOs */
+#define VD56G3_NB_GPIOS					8
+
+/* regulator supplies */
+static const char *const vd56g3_supply_names[] = {
+	"VCORE",
+	"VDDIO",
+	"VANA",
+};
+
+#define VD56G3_NUM_SUPPLIES		ARRAY_SIZE(vd56g3_supply_names)
+
+/* -----------------------------------------------------------------------------
+ * Models (VD56G3: Mono, VD66GY: Bayer RGB), Modes and formats
+ */
+
+enum vd56g3_models {
+	VD56G3_MODEL_VD56G3,
+	VD56G3_MODEL_VD66GY,
+};
+
+struct vd56g3_mode {
+	u32 width;
+	u32 height;
+};
+
+static const struct vd56g3_mode vd56g3_supported_modes[] = {
+	{
+		.width = VD56G3_NATIVE_WIDTH,
+		.height = VD56G3_NATIVE_HEIGHT,
+	},
+	{
+		.width = VD56G3_DEFAULT_WIDTH,
+		.height = VD56G3_DEFAULT_HEIGHT,
+	},
+	{
+		.width = 1024,
+		.height = 1280,
+	},
+	{
+		.width = 1024,
+		.height = 768,
+	},
+	{
+		.width = 768,
+		.height = 1024,
+	},
+	{
+		.width = 720,
+		.height = 1280,
+	},
+	{
+		.width = 640,
+		.height = 480,
+	},
+	{
+		.width = 480,
+		.height = 640,
+	},
+	{
+		.width = 320,
+		.height = 240,
+	},
+};
+
+/*
+ * Sensor support 8bits and 10bits output in both variants
+ *  - Monochrome
+ *  - RGB (with all H/V flip variations)
+ */
+static const unsigned int vd56g3_mbus_codes[2][5] = {
+	{
+		MEDIA_BUS_FMT_Y8_1X8,
+		MEDIA_BUS_FMT_SGRBG8_1X8,
+		MEDIA_BUS_FMT_SRGGB8_1X8,
+		MEDIA_BUS_FMT_SBGGR8_1X8,
+		MEDIA_BUS_FMT_SGBRG8_1X8,
+	},
+	{
+		MEDIA_BUS_FMT_Y10_1X10,
+		MEDIA_BUS_FMT_SGRBG10_1X10,
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		MEDIA_BUS_FMT_SBGGR10_1X10,
+		MEDIA_BUS_FMT_SGBRG10_1X10,
+	},
+};
+
+enum vd56g3_expo_state {
+	VD56G3_EXPO_AUTO,
+	VD56G3_EXPO_AUTO_FREEZE,
+	VD56G3_EXPO_MANUAL
+};
+
+struct vd56g3 {
+	struct i2c_client *i2c_client;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct regulator_bulk_data supplies[VD56G3_NUM_SUPPLIES];
+	struct gpio_desc *reset_gpio;
+	struct clk *xclk;
+	struct regmap *regmap;
+	u32 ext_clock;
+	u32 pll_prediv;
+	u32 pll_mult;
+	u32 pixel_clock;
+	u16 oif_ctrl;
+	int nb_of_lane;
+	u32 gpios[VD56G3_NB_GPIOS];
+	unsigned long ext_leds_mask;
+	bool is_mono;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *hblank_ctrl;
+	struct v4l2_ctrl *vblank_ctrl;
+	struct {
+		struct v4l2_ctrl *hflip_ctrl;
+		struct v4l2_ctrl *vflip_ctrl;
+	};
+	struct v4l2_ctrl *patgen_ctrl;
+	struct {
+		struct v4l2_ctrl *ae_ctrl;
+		struct v4l2_ctrl *expo_ctrl;
+		struct v4l2_ctrl *again_ctrl;
+		struct v4l2_ctrl *dgain_ctrl;
+	};
+	struct v4l2_ctrl *ae_lock_ctrl;
+	struct v4l2_ctrl *ae_bias_ctrl;
+	struct v4l2_ctrl *led_ctrl;
+	bool streaming;
+};
+
+static inline struct vd56g3 *to_vd56g3(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct vd56g3, sd);
+}
+
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct vd56g3, ctrl_handler)->sd;
+}
+
+/* -----------------------------------------------------------------------------
+ * Additional i2c register helpers
+ */
+
+static int vd56g3_poll_reg(struct vd56g3 *sensor, u32 reg, u8 poll_val,
+			   int *err)
+{
+	unsigned int val = 0;
+	int ret;
+
+	if (err && *err)
+		return *err;
+
+	reg = reg & CCI_REG_ADDR_MASK;
+	ret = regmap_read_poll_timeout(sensor->regmap, reg, val,
+				       (val == poll_val), 2000,
+				       500 * USEC_PER_MSEC);
+
+	if (ret && err)
+		*err = ret;
+
+	return ret;
+}
+
+static int vd56g3_wait_state(struct vd56g3 *sensor, int state, int *err)
+{
+	return vd56g3_poll_reg(sensor, VD56G3_REG_SYSTEM_FSM, state, err);
+}
+
+/* -----------------------------------------------------------------------------
+ * Controls: definitions, helpers and handlers
+ */
+
+static const char *const vd56g3_tp_menu[] = { "Disabled", "Solid", "Colorbar",
+					      "Gradbar",  "Hgrey", "Vgrey",
+					      "Dgrey",	  "PN28" };
+
+static const s64 vd56g3_ev_bias_qmenu[] = { -4000, -3500, -3000, -2500, -2000,
+					    -1500, -1000, -500,	 0,	500,
+					    1000,  1500,  2000,	 2500,	3000,
+					    3500,  4000 };
+
+static const s64 vd56g3_link_freq_1lane[] = { VD56G3_LINK_FREQ_DEF_1LANE };
+
+static const s64 vd56g3_link_freq_2lanes[] = { VD56G3_LINK_FREQ_DEF_2LANES };
+
+static u8 vd56g3_get_bpp(__u32 code)
+{
+	switch (code) {
+	case MEDIA_BUS_FMT_Y8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+		return 8;
+	case MEDIA_BUS_FMT_Y10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+		return 10;
+	default:
+		/* Should never happen */
+		WARN(1, "Unsupported code %d. default to 8 bpp", code);
+	}
+
+	return 8;
+}
+
+static u8 vd56g3_get_datatype(__u32 code)
+{
+	switch (code) {
+	case MEDIA_BUS_FMT_Y8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+		return MIPI_CSI2_DT_RAW8;
+	case MEDIA_BUS_FMT_Y10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+		return MIPI_CSI2_DT_RAW10;
+	default:
+		/* Should never happen */
+		WARN(1, "Unsupported code %d. default to MIPI_CSI2_DT_RAW8",
+		     code);
+	}
+
+	return MIPI_CSI2_DT_RAW8;
+}
+
+static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
+{
+	u64 exposure = 0;
+	u64 again = 0;
+	u64 dgain = 0;
+	int ret = 0;
+
+	/*
+	 * When 'force_cur_val' is enabled, save the ctrl value in 'cur.val'
+	 * instead of the normal 'val', this is used during poweroff to cache
+	 * volatile ctrls and enable coldstart.
+	 */
+	cci_read(sensor->regmap, VD56G3_REG_APPLIED_COARSE_EXPOSURE, &exposure,
+		 &ret);
+	cci_read(sensor->regmap, VD56G3_REG_APPLIED_ANALOG_GAIN, &again, &ret);
+	cci_read(sensor->regmap, VD56G3_REG_APPLIED_DIGITAL_GAIN, &dgain, &ret);
+	if (ret)
+		return ret;
+
+	if (force_cur_val) {
+		sensor->expo_ctrl->cur.val = exposure;
+		sensor->again_ctrl->cur.val = again;
+		sensor->dgain_ctrl->cur.val = dgain;
+	} else {
+		sensor->expo_ctrl->val = exposure;
+		sensor->again_ctrl->val = again;
+		sensor->dgain_ctrl->val = dgain;
+	}
+
+	return ret;
+}
+
+static int vd56g3_update_patgen(struct vd56g3 *sensor, u32 patgen_index)
+{
+	u32 pattern = patgen_index <= 3 ? patgen_index : patgen_index + 12;
+	u16 patgen = pattern << VD56G3_PATGEN_TYPE_SHIFT;
+	u8 duster = VD56G3_DUSTER_ENABLE_DEF_MODULES;
+	u8 darkcal = VD56G3_DARKCAL_ENABLE;
+	int ret = 0;
+
+	if (patgen_index) {
+		patgen |= VD56G3_PATGEN_ENABLE;
+		duster = VD56G3_DUSTER_DISABLE;
+		darkcal = VD56G3_DARKCAL_DISABLE_DARKAVG;
+	}
+
+	cci_write(sensor->regmap, VD56G3_REG_DUSTER_CTRL, duster, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_DARKCAL_CTRL, darkcal, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_PATGEN_CTRL, patgen, &ret);
+
+	return ret;
+}
+
+static int vd56g3_update_expo_cluster(struct vd56g3 *sensor, bool is_auto)
+{
+	int ret = 0;
+	enum vd56g3_expo_state expo_state = is_auto ? VD56G3_EXP_MODE_AUTO :
+						      VD56G3_EXP_MODE_MANUAL;
+
+	if (sensor->ae_ctrl->is_new)
+		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
+			  &ret);
+
+	/* In Auto expo, set coldstart parameters */
+	if (is_auto && sensor->ae_ctrl->is_new) {
+		cci_write(sensor->regmap,
+			  VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE,
+			  sensor->expo_ctrl->val, &ret);
+		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_ANALOG_GAIN,
+			  sensor->again_ctrl->val, &ret);
+		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN,
+			  sensor->dgain_ctrl->val, &ret);
+	}
+
+	/* In Manual expo, set exposure, analog and digital gains */
+	if (!is_auto && sensor->expo_ctrl->is_new)
+		cci_write(sensor->regmap, VD56G3_REG_MANUAL_COARSE_EXPOSURE,
+			  sensor->expo_ctrl->val, &ret);
+
+	if (!is_auto && sensor->again_ctrl->is_new)
+		cci_write(sensor->regmap, VD56G3_REG_MANUAL_ANALOG_GAIN,
+			  sensor->again_ctrl->val, &ret);
+
+	if (!is_auto && sensor->dgain_ctrl->is_new) {
+		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0,
+			  sensor->dgain_ctrl->val, &ret);
+		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1,
+			  sensor->dgain_ctrl->val, &ret);
+		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2,
+			  sensor->dgain_ctrl->val, &ret);
+		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3,
+			  sensor->dgain_ctrl->val, &ret);
+	}
+
+	return ret;
+}
+
+static int vd56g3_lock_exposure(struct vd56g3 *sensor, u32 lock_val)
+{
+	bool ae_lock = lock_val & V4L2_LOCK_EXPOSURE;
+	enum vd56g3_expo_state expo_state = ae_lock ? VD56G3_EXP_MODE_FREEZE :
+						      VD56G3_EXP_MODE_AUTO;
+	int ret = 0;
+
+	if (sensor->ae_ctrl->val == V4L2_EXPOSURE_AUTO)
+		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
+			  &ret);
+
+	return ret;
+}
+
+static int vd56g3_write_gpiox(struct vd56g3 *sensor, unsigned long gpio_mask)
+{
+	unsigned long io;
+	u32 gpio_val;
+	int ret = 0;
+
+	for_each_set_bit(io, &gpio_mask, VD56G3_NB_GPIOS) {
+		gpio_val = sensor->gpios[io];
+
+		if (gpio_val == VD56G3_GPIOX_STROBE_MODE &&
+		    sensor->led_ctrl->val == V4L2_FLASH_LED_MODE_NONE)
+			gpio_val = VD56G3_GPIOX_GPIO_IN;
+
+		cci_write(sensor->regmap, VD56G3_REG_GPIO_0_CTRL + io, gpio_val,
+			  &ret);
+	}
+
+	return ret;
+}
+
+static int vd56g3_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct vd56g3 *sensor = to_vd56g3(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	/* Interact with HW only when it is powered ON */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = vd56g3_read_expo_cluster(sensor, false);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_mark_last_busy(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
+
+	return ret;
+}
+
+static int vd56g3_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
+	struct vd56g3 *sensor = to_vd56g3(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	const struct v4l2_rect *crop;
+	unsigned int frame_length = 0;
+	unsigned int expo_max;
+	unsigned int ae_compensation;
+	bool is_auto = false;
+	int ret;
+
+	struct v4l2_subdev_state *state =
+		v4l2_subdev_get_locked_active_state(sd);
+	crop = v4l2_subdev_state_get_crop(state, 0);
+
+	if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
+		return 0;
+
+	/* Update controls state, range, etc. whatever the state of the HW */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		frame_length = crop->height + ctrl->val;
+		expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
+		__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
+					 VD56G3_EXPOSURE_DEFAULT);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		is_auto = (ctrl->val == V4L2_EXPOSURE_AUTO);
+		__v4l2_ctrl_grab(sensor->ae_lock_ctrl, !is_auto);
+		__v4l2_ctrl_grab(sensor->ae_bias_ctrl, !is_auto);
+		break;
+	default:
+		break;
+	}
+
+	/* Interact with HW only when it is powered ON */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		ret = cci_write(sensor->regmap, VD56G3_REG_ORIENTATION,
+				sensor->hflip_ctrl->val |
+					(sensor->vflip_ctrl->val << 1),
+				NULL);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = vd56g3_update_patgen(sensor, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = vd56g3_update_expo_cluster(sensor, is_auto);
+		break;
+	case V4L2_CID_3A_LOCK:
+		ret = vd56g3_lock_exposure(sensor, ctrl->val);
+		break;
+	case V4L2_CID_AUTO_EXPOSURE_BIAS:
+		ae_compensation =
+			DIV_ROUND_CLOSEST((int)vd56g3_ev_bias_qmenu[ctrl->val] *
+					  256, 1000);
+		ret = cci_write(sensor->regmap, VD56G3_REG_AE_COMPENSATION,
+				ae_compensation, NULL);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = cci_write(sensor->regmap, VD56G3_REG_FRAME_LENGTH,
+				frame_length, NULL);
+		break;
+	case V4L2_CID_FLASH_LED_MODE:
+		ret = vd56g3_write_gpiox(sensor, sensor->ext_leds_mask);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_mark_last_busy(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops vd56g3_ctrl_ops = {
+	.g_volatile_ctrl = vd56g3_g_volatile_ctrl,
+	.s_ctrl = vd56g3_s_ctrl,
+};
+
+static void vd56g3_update_controls(struct vd56g3 *sensor)
+{
+	const struct v4l2_rect *crop;
+	unsigned int hblank;
+	unsigned int vblank_min, vblank, vblank_max;
+	unsigned int frame_length;
+	unsigned int expo_max;
+
+	struct v4l2_subdev_state *state =
+		v4l2_subdev_get_locked_active_state(&sensor->sd);
+	crop = v4l2_subdev_state_get_crop(state, 0);
+
+	hblank = VD56G3_LINE_LENGTH_MIN - crop->width;
+	vblank_min = VD56G3_VBLANK_MIN;
+	vblank = VD56G3_FRAME_LENGTH_DEF_60FPS - crop->height;
+	vblank_max = 0xffff - crop->height;
+	frame_length = crop->height + vblank;
+	expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
+
+	/* Update blanking and exposure (ranges + values) */
+	__v4l2_ctrl_modify_range(sensor->hblank_ctrl, hblank, hblank, 1,
+				 hblank);
+	__v4l2_ctrl_modify_range(sensor->vblank_ctrl, vblank_min, vblank_max, 1,
+				 vblank);
+	__v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, vblank);
+	__v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
+				 VD56G3_EXPOSURE_DEFAULT);
+	__v4l2_ctrl_s_ctrl(sensor->expo_ctrl, VD56G3_EXPOSURE_DEFAULT);
+}
+
+static int vd56g3_init_controls(struct vd56g3 *sensor)
+{
+	const struct v4l2_ctrl_ops *ops = &vd56g3_ctrl_ops;
+	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 25);
+
+	/* Horizontal & vertical flips modify bayer code on RGB variant */
+	sensor->hflip_ctrl =
+		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (sensor->hflip_ctrl)
+		sensor->hflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	sensor->vflip_ctrl =
+		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (sensor->vflip_ctrl)
+		sensor->vflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	sensor->patgen_ctrl =
+		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+					     ARRAY_SIZE(vd56g3_tp_menu) - 1, 0,
+					     0, vd56g3_tp_menu);
+
+	ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
+				      ARRAY_SIZE(vd56g3_link_freq_1lane) - 1, 0,
+				      (sensor->nb_of_lane == 2) ?
+					      vd56g3_link_freq_2lanes :
+					      vd56g3_link_freq_1lane);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
+				 sensor->pixel_clock, sensor->pixel_clock, 1,
+				 sensor->pixel_clock);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	sensor->ae_ctrl = v4l2_ctrl_new_std_menu(hdl, ops,
+						 V4L2_CID_EXPOSURE_AUTO,
+						 V4L2_EXPOSURE_MANUAL, 0,
+						 V4L2_EXPOSURE_AUTO);
+
+	sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK, 0,
+						 GENMASK(2, 0), 0, 0);
+
+	sensor->ae_bias_ctrl =
+		v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
+				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) - 1,
+				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) / 2,
+				       vd56g3_ev_bias_qmenu);
+
+	/*
+	 * Analog gain [1, 8] is computed with the following logic :
+	 * 32/(32 - again_reg), with again_reg in the range [0:28]
+	 * Digital gain [1.00, 8.00] is coded as a Fixed Point 5.8
+	 */
+	sensor->again_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
+					       0, 28, 1, 0);
+	sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
+					       0x100, 0x800, 1, 0x100);
+
+	/*
+	 * Set the exposure, horizontal and vertical blanking ctrls
+	 * to hardcoded values, they will be updated in vd56g3_update_controls.
+	 * Exposure being in an auto-cluster, set a significant value here.
+	 */
+	sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+					      VD56G3_EXPOSURE_DEFAULT,
+					      VD56G3_EXPOSURE_DEFAULT, 1,
+					      VD56G3_EXPOSURE_DEFAULT);
+	sensor->hblank_ctrl =
+		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 1, 1, 1, 1);
+	if (sensor->hblank_ctrl)
+		sensor->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	sensor->vblank_ctrl =
+		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, 1, 1, 1, 1);
+
+	/* Additional control based on device tree properties */
+	if (sensor->ext_leds_mask)
+		sensor->led_ctrl =
+			v4l2_ctrl_new_std_menu(hdl, ops,
+					       V4L2_CID_FLASH_LED_MODE,
+					       V4L2_FLASH_LED_MODE_FLASH, 0,
+					       V4L2_FLASH_LED_MODE_NONE);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		goto free_ctrls;
+	}
+
+	v4l2_ctrl_cluster(2, &sensor->hflip_ctrl);
+	v4l2_ctrl_auto_cluster(4, &sensor->ae_ctrl, V4L2_EXPOSURE_MANUAL, true);
+
+	sensor->sd.ctrl_handler = hdl;
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Videos ops
+ */
+
+static int vd56g3_stream_on(struct vd56g3 *sensor,
+			    struct v4l2_subdev_state *state)
+{
+	const struct v4l2_mbus_framefmt *format =
+		v4l2_subdev_state_get_format(state, 0);
+	const struct v4l2_rect *crop =
+		v4l2_subdev_state_get_crop(state, 0);
+	int ret = 0;
+	unsigned int csi_mbps = ((sensor->nb_of_lane == 2) ?
+					 VD56G3_LINK_FREQ_DEF_2LANES :
+					 VD56G3_LINK_FREQ_DEF_1LANE) * 2 / MEGA;
+	unsigned int binning;
+
+	/* configure clocks */
+	cci_write(sensor->regmap, VD56G3_REG_EXT_CLOCK, sensor->ext_clock,
+		  &ret);
+	cci_write(sensor->regmap, VD56G3_REG_CLK_PLL_PREDIV, sensor->pll_prediv,
+		  &ret);
+	cci_write(sensor->regmap, VD56G3_REG_CLK_SYS_PLL_MULT, sensor->pll_mult,
+		  &ret);
+
+	/* configure output */
+	cci_write(sensor->regmap, VD56G3_REG_FORMAT_CTRL,
+		  vd56g3_get_bpp(format->code), &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OIF_CSI_BITRATE, csi_mbps, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OIF_IMG_CTRL,
+		  vd56g3_get_datatype(format->code), &ret);
+	cci_write(sensor->regmap, VD56G3_REG_ISL_ENABLE, 0, &ret);
+
+	/* configure binning mode */
+	switch (crop->width / format->width) {
+	case 1:
+	default:
+		binning = READOUT_NORMAL;
+		break;
+	case 2:
+		binning = READOUT_DIGITAL_BINNING_X2;
+		break;
+	}
+	cci_write(sensor->regmap, VD56G3_REG_READOUT_CTRL, binning, &ret);
+
+	/* configure ROIs */
+	cci_write(sensor->regmap, VD56G3_REG_Y_START, crop->top, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_Y_END,
+		  crop->top + crop->height - 1, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_START, crop->left, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_END,
+		  crop->left + crop->width - 1, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_START, 0, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_END, crop->height - 1,
+		  &ret);
+	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_H, crop->left, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_H,
+		  crop->left + crop->width - 1, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_V, 0, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_V, crop->height - 1,
+		  &ret);
+	if (ret)
+		return ret;
+
+	/* Setup default GPIO values; could be overridden by V4L2 ctrl setup */
+	ret = vd56g3_write_gpiox(sensor, GENMASK(VD56G3_NB_GPIOS - 1, 0));
+	if (ret)
+		return ret;
+
+	/* Apply settings from V4L2 ctrls */
+	ret = __v4l2_ctrl_handler_setup(&sensor->ctrl_handler);
+	if (ret)
+		return ret;
+
+	/* start streaming */
+	cci_write(sensor->regmap, VD56G3_REG_STBY, VD56G3_CMD_START_STREAM,
+		  &ret);
+	vd56g3_poll_reg(sensor, VD56G3_REG_STBY, VD56G3_CMD_ACK, &ret);
+	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_STREAMING, &ret);
+
+	return ret;
+}
+
+static int vd56g3_stream_off(struct vd56g3 *sensor)
+{
+	int ret = 0;
+
+	/* Retrieve Expo cluster to enable coldstart of AE */
+	ret = vd56g3_read_expo_cluster(sensor, true);
+
+	cci_write(sensor->regmap, VD56G3_REG_STREAMING, VD56G3_CMD_STOP_STREAM,
+		  &ret);
+	vd56g3_poll_reg(sensor, VD56G3_REG_STREAMING, VD56G3_CMD_ACK, &ret);
+	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
+
+	return ret;
+}
+
+static int vd56g3_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct vd56g3 *sensor = to_vd56g3(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = 0;
+
+	struct v4l2_subdev_state *state =
+		v4l2_subdev_lock_and_get_active_state(sd);
+
+	if (enable) {
+		ret = pm_runtime_resume_and_get(&client->dev);
+		if (ret < 0)
+			goto unlock;
+		ret = vd56g3_stream_on(sensor, state);
+		if (ret) {
+			dev_err(&client->dev, "Failed to start streaming\n");
+			pm_runtime_put_sync(&client->dev);
+		}
+	} else {
+		vd56g3_stream_off(sensor);
+		pm_runtime_mark_last_busy(&client->dev);
+		pm_runtime_put_autosuspend(&client->dev);
+	}
+
+unlock:
+	v4l2_subdev_unlock_state(state);
+	if (!ret) {
+		sensor->streaming = enable;
+		v4l2_ctrl_grab(sensor->hflip_ctrl, enable);
+		v4l2_ctrl_grab(sensor->vflip_ctrl, enable);
+		v4l2_ctrl_grab(sensor->patgen_ctrl, enable);
+	}
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops vd56g3_video_ops = {
+	.s_stream = vd56g3_s_stream,
+};
+
+/* -----------------------------------------------------------------------------
+ * Pad ops
+ */
+
+/* Media bus code is dependent of :
+ *      - 8bits or 10bits output
+ *      - variant : Mono or RGB
+ *      - H/V flips parameters in case of RGB
+ */
+static u32 vd56g3_get_mbus_code(struct vd56g3 *sensor, u32 code)
+{
+	unsigned int i_bpp;
+	unsigned int j;
+
+	for (i_bpp = 0; i_bpp < ARRAY_SIZE(vd56g3_mbus_codes); i_bpp++) {
+		for (j = 0; j < ARRAY_SIZE(vd56g3_mbus_codes[i_bpp]); j++) {
+			if (vd56g3_mbus_codes[i_bpp][j] == code)
+				goto endloops;
+		}
+	}
+
+endloops:
+	if (i_bpp >= ARRAY_SIZE(vd56g3_mbus_codes))
+		i_bpp = 0;
+
+	if (sensor->is_mono)
+		j = 0;
+	else
+		j = 1 + (sensor->hflip_ctrl->val ? 1 : 0) +
+		    (sensor->vflip_ctrl->val ? 2 : 0);
+
+	return vd56g3_mbus_codes[i_bpp][j];
+}
+
+static int vd56g3_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct vd56g3 *sensor = to_vd56g3(sd);
+
+	if (code->index >= ARRAY_SIZE(vd56g3_mbus_codes))
+		return -EINVAL;
+
+	code->code =
+		vd56g3_get_mbus_code(sensor, vd56g3_mbus_codes[code->index][0]);
+
+	return 0;
+}
+
+static int vd56g3_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(vd56g3_supported_modes))
+		return -EINVAL;
+
+	fse->min_width = vd56g3_supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = vd56g3_supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static void vd56g3_update_img_pad_format(struct vd56g3 *sensor,
+					 const struct vd56g3_mode *mode,
+					 u32 mbus_code,
+					 struct v4l2_mbus_framefmt *mbus_fmt)
+{
+	mbus_fmt->width = mode->width;
+	mbus_fmt->height = mode->height;
+	mbus_fmt->code = vd56g3_get_mbus_code(sensor, mbus_code);
+	mbus_fmt->colorspace = V4L2_COLORSPACE_RAW;
+	mbus_fmt->field = V4L2_FIELD_NONE;
+	mbus_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	mbus_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	mbus_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+}
+
+static int vd56g3_set_pad_fmt(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *sd_fmt)
+{
+	struct vd56g3 *sensor = to_vd56g3(sd);
+	const struct vd56g3_mode *new_mode;
+	struct v4l2_rect pad_crop;
+	unsigned int binning;
+
+	if (sensor->streaming)
+		return -EBUSY;
+
+	new_mode = v4l2_find_nearest_size(vd56g3_supported_modes,
+					  ARRAY_SIZE(vd56g3_supported_modes),
+					  width, height, sd_fmt->format.width,
+					  sd_fmt->format.height);
+
+	vd56g3_update_img_pad_format(sensor, new_mode, sd_fmt->format.code,
+				     &sd_fmt->format);
+
+	/* Compute crop rectangle (maximized via binning) */
+	binning = min(VD56G3_NATIVE_WIDTH / sd_fmt->format.width,
+		      VD56G3_NATIVE_HEIGHT / sd_fmt->format.height);
+	binning = min(binning, 2U);
+	pad_crop.width = sd_fmt->format.width * binning;
+	pad_crop.height = sd_fmt->format.height * binning;
+	pad_crop.left = (VD56G3_NATIVE_WIDTH - pad_crop.width) / 2;
+	pad_crop.top = (VD56G3_NATIVE_HEIGHT - pad_crop.height) / 2;
+
+	/* Update active state's format and crop */
+	*v4l2_subdev_state_get_format(sd_state, sd_fmt->pad) = sd_fmt->format;
+	*v4l2_subdev_state_get_crop(sd_state, sd_fmt->pad) = pad_crop;
+
+	if (sd_fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		vd56g3_update_controls(sensor);
+
+	return 0;
+}
+
+static int vd56g3_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
+		break;
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = VD56G3_NATIVE_WIDTH;
+		sel->r.height = VD56G3_NATIVE_HEIGHT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vd56g3_init_state(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state)
+{
+	unsigned int def_mode = VD56G3_DEFAULT_MODE;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+		.pad = 0,
+		.format = {
+			.code = vd56g3_mbus_codes[0][0],
+			.width = vd56g3_supported_modes[def_mode].width,
+			.height = vd56g3_supported_modes[def_mode].height,
+		},
+	};
+
+	vd56g3_set_pad_fmt(sd, sd_state, &fmt);
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops vd56g3_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_pad_ops vd56g3_pad_ops = {
+	.enum_mbus_code = vd56g3_enum_mbus_code,
+	.enum_frame_size = vd56g3_enum_frame_size,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = vd56g3_set_pad_fmt,
+	.get_selection = vd56g3_get_selection,
+};
+
+static const struct v4l2_subdev_ops vd56g3_subdev_ops = {
+	.core = &vd56g3_core_ops,
+	.video = &vd56g3_video_ops,
+	.pad = &vd56g3_pad_ops,
+};
+
+static const struct media_entity_operations vd56g3_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops vd56g3_internal_ops = {
+	.init_state = vd56g3_init_state,
+};
+
+/* -----------------------------------------------------------------------------
+ * Power management
+ */
+
+static int vd56g3_boot(struct vd56g3 *sensor)
+{
+	int ret = 0;
+
+	cci_write(sensor->regmap, VD56G3_REG_BOOT, VD56G3_CMD_BOOT, &ret);
+	vd56g3_poll_reg(sensor, VD56G3_REG_BOOT, VD56G3_CMD_ACK, &ret);
+	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
+
+	return ret;
+}
+
+static int vd56g3_power_on(struct vd56g3 *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int ret;
+
+	ret = regulator_bulk_enable(VD56G3_NUM_SUPPLIES, sensor->supplies);
+	if (ret) {
+		dev_err(&client->dev, "Failed to enable regulators %d", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(sensor->xclk);
+	if (ret) {
+		dev_err(&client->dev, "Failed to enable clock %d", ret);
+		goto disable_reg;
+	}
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+	ret = vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_READY_TO_BOOT, NULL);
+	if (ret) {
+		dev_err(&client->dev, "Sensor reset failed %d\n", ret);
+		goto disable_clock;
+	}
+
+	return 0;
+
+disable_clock:
+	clk_disable_unprepare(sensor->xclk);
+	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+disable_reg:
+	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
+
+	return ret;
+}
+
+static int vd56g3_power_off(struct vd56g3 *sensor)
+{
+	clk_disable_unprepare(sensor->xclk);
+	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+	regulator_bulk_disable(VD56G3_NUM_SUPPLIES, sensor->supplies);
+	return 0;
+}
+
+static int vd56g3_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct vd56g3 *sensor = to_vd56g3(sd);
+	struct i2c_client *client = sensor->i2c_client;
+	int ret;
+
+	ret = vd56g3_power_on(sensor);
+	if (ret) {
+		dev_err(&client->dev, "Failed to power on %d", ret);
+		return ret;
+	}
+
+	ret = vd56g3_boot(sensor);
+	if (ret) {
+		dev_err(&client->dev, "sensor boot failed %d", ret);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int vd56g3_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct vd56g3 *sensor = to_vd56g3(sd);
+
+	return vd56g3_power_off(sensor);
+}
+
+static const struct dev_pm_ops vd56g3_pm_ops = {
+	SET_RUNTIME_PM_OPS(vd56g3_runtime_suspend, vd56g3_runtime_resume, NULL)
+};
+
+/* -----------------------------------------------------------------------------
+ * Probe and initialization
+ */
+
+static int vd56g3_check_csi_conf(struct vd56g3 *sensor,
+				 struct fwnode_handle *endpoint)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
+	u32 phy_data_lanes[VD56G3_MAX_CSI_DATA_LANES] = { ~0, ~0 };
+	int n_lanes;
+	int frequency;
+	int p, l;
+	int ret = 0;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
+	if (ret)
+		return -EINVAL;
+
+	/* Check lanes number */
+	n_lanes = ep.bus.mipi_csi2.num_data_lanes;
+	if (n_lanes != 1 && n_lanes != 2) {
+		dev_err(&client->dev, "Invalid data lane number %d\n", n_lanes);
+		ret = -EINVAL;
+		goto done;
+	}
+	sensor->nb_of_lane = n_lanes;
+
+	/* Clock lane must be first */
+	if (ep.bus.mipi_csi2.clock_lane != 0) {
+		dev_err(&client->dev, "Clk lane must be mapped to lane 0\n");
+		ret = -EINVAL;
+		goto done;
+	}
+
+	/* Prepare Output Interface conf based on lane settings
+	 * logical to physical lane conversion (+ pad remaining slots)
+	 */
+	for (l = 0; l < n_lanes; l++)
+		phy_data_lanes[ep.bus.mipi_csi2.data_lanes[l] - 1] = l;
+	for (p = 0; p < VD56G3_MAX_CSI_DATA_LANES; p++) {
+		if (phy_data_lanes[p] != ~0)
+			continue;
+		phy_data_lanes[p] = l;
+		l++;
+	}
+	sensor->oif_ctrl = n_lanes |
+			   (ep.bus.mipi_csi2.lane_polarities[0] << 3) |
+			   ((phy_data_lanes[0]) << 4) |
+			   (ep.bus.mipi_csi2.lane_polarities[1] << 6) |
+			   ((phy_data_lanes[1]) << 7) |
+			   (ep.bus.mipi_csi2.lane_polarities[2] << 9);
+
+	/* Check link frequency */
+	if (!ep.nr_of_link_frequencies) {
+		dev_err(&client->dev, "link-frequency not found in DT\n");
+		ret = -EINVAL;
+		goto done;
+	}
+	frequency = (n_lanes == 2) ? VD56G3_LINK_FREQ_DEF_2LANES :
+				     VD56G3_LINK_FREQ_DEF_1LANE;
+	if (ep.nr_of_link_frequencies != 1 ||
+	    ep.link_frequencies[0] != frequency) {
+		dev_err(&client->dev, "Link frequency not supported: %lld\n",
+			ep.link_frequencies[0]);
+		ret = -EINVAL;
+		goto done;
+	}
+
+done:
+	v4l2_fwnode_endpoint_free(&ep);
+	return ret;
+}
+
+static int vd56g3_parse_dt_gpios_array(struct vd56g3 *sensor, char *prop_name,
+				       u32 *array, int *nb)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct device_node *np = client->dev.of_node;
+	unsigned int i;
+
+	*nb = of_property_read_variable_u32_array(np, prop_name, array, 0,
+						  VD56G3_NB_GPIOS);
+
+	if (*nb == -EINVAL) {
+		*nb = 0;
+		return *nb;
+	} else if (*nb < 0) {
+		dev_err(&client->dev, "Failed to read %s prop\n", prop_name);
+		return *nb;
+	}
+
+	for (i = 0; i < *nb; i++) {
+		if (array[i] >= VD56G3_NB_GPIOS) {
+			dev_err(&client->dev, "Invalid GPIO : %d\n", array[i]);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int vd56g3_parse_dt_gpios(struct vd56g3 *sensor)
+{
+	u32 led_gpios[VD56G3_NB_GPIOS];
+	int nb_gpios_leds;
+	int ret;
+	unsigned int i;
+
+	/* Initialize GPIOs to default */
+	for (i = 0; i < VD56G3_NB_GPIOS; i++)
+		sensor->gpios[i] = VD56G3_GPIOX_GPIO_IN;
+	sensor->ext_leds_mask = 0;
+
+	/* Take into account optional 'st,leds' output for GPIOs */
+	ret = vd56g3_parse_dt_gpios_array(sensor, "st,leds", led_gpios,
+					  &nb_gpios_leds);
+	if (ret)
+		return ret;
+	for (i = 0; i < nb_gpios_leds; i++) {
+		sensor->gpios[led_gpios[i]] = VD56G3_GPIOX_STROBE_MODE;
+		set_bit(led_gpios[i], &sensor->ext_leds_mask);
+	}
+
+	return 0;
+}
+
+static int vd56g3_parse_dt(struct vd56g3 *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct device *dev = &client->dev;
+	struct fwnode_handle *endpoint;
+	int ret;
+
+	endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0, 0);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = vd56g3_check_csi_conf(sensor, endpoint);
+	fwnode_handle_put(endpoint);
+	if (ret)
+		return ret;
+
+	ret = vd56g3_parse_dt_gpios(sensor);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int vd56g3_get_regulators(struct vd56g3 *sensor)
+{
+	int i;
+
+	for (i = 0; i < VD56G3_NUM_SUPPLIES; i++)
+		sensor->supplies[i].supply = vd56g3_supply_names[i];
+
+	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
+				       VD56G3_NUM_SUPPLIES, sensor->supplies);
+}
+
+static int vd56g3_prepare_clock_tree(struct vd56g3 *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	const unsigned int predivs[] = { 1, 2, 4 };
+	u32 pll_out;
+	int i;
+
+	/* External clock must be in [6Mhz-27Mhz] */
+	if (sensor->ext_clock < 6 * HZ_PER_MHZ ||
+	    sensor->ext_clock > 27 * HZ_PER_MHZ) {
+		dev_err(&client->dev,
+			"Only 6Mhz-27Mhz clock range supported. Provided %lu MHz\n",
+			sensor->ext_clock / HZ_PER_MHZ);
+		return -EINVAL;
+	}
+
+	/* PLL input should be in [6Mhz-12Mhz[ */
+	for (i = 0; i < ARRAY_SIZE(predivs); i++) {
+		sensor->pll_prediv = predivs[i];
+		if (sensor->ext_clock / sensor->pll_prediv < 12 * HZ_PER_MHZ)
+			break;
+	}
+
+	/* PLL output clock must be as close as possible to 804Mhz */
+	sensor->pll_mult = (VD56G3_TARGET_PLL * sensor->pll_prediv +
+			    sensor->ext_clock / 2) /
+			   sensor->ext_clock;
+	pll_out = sensor->ext_clock * sensor->pll_mult / sensor->pll_prediv;
+
+	/* Target Pixel Clock for standard 10bit ADC mode : 160.8Mhz */
+	sensor->pixel_clock = pll_out / VD56G3_VT_CLOCK_DIV;
+
+	return 0;
+}
+
+static int vd56g3_detect(struct vd56g3 *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	struct device *dev = &client->dev;
+	unsigned int model;
+	u64 model_id = 0;
+	u64 device_revision = 0;
+	u64 optical_revision = 0;
+	int ret = 0;
+
+	model = (uintptr_t)device_get_match_data(dev);
+
+	cci_read(sensor->regmap, VD56G3_REG_MODEL_ID, &model_id, &ret);
+	if (ret)
+		return ret;
+
+	if (model_id != VD56G3_MODEL_ID) {
+		dev_warn(&client->dev, "Unsupported sensor id %x",
+			 (u16)model_id);
+		return -ENODEV;
+	}
+
+	cci_read(sensor->regmap, VD56G3_REG_REVISION, &device_revision, &ret);
+	if (ret)
+		return ret;
+
+	if ((device_revision >> 8) != 0x31) {
+		dev_warn(&client->dev, "Unsupported Cut version %x",
+			 (u16)device_revision);
+		return -ENODEV;
+	}
+
+	cci_read(sensor->regmap, VD56G3_REG_OPTICAL_REVISION, &optical_revision,
+		 &ret);
+	if (ret)
+		return ret;
+
+	sensor->is_mono =
+		((optical_revision & 1) == VD56G3_OPTICAL_REVISION_MONO);
+	if ((sensor->is_mono && model == VD56G3_MODEL_VD66GY) ||
+	    (!sensor->is_mono && model == VD56G3_MODEL_VD56G3)) {
+		dev_warn(&client->dev,
+			 "Found %s sensor, while %s model is defined in DT",
+			 (sensor->is_mono) ? "Mono" : "Bayer",
+			 (model == VD56G3_MODEL_VD56G3) ? "vd56g3" : "vd66gy");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int vd56g3_subdev_init(struct vd56g3 *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int ret;
+
+	/* Init sub device */
+	v4l2_i2c_subdev_init(&sensor->sd, client, &vd56g3_subdev_ops);
+	sensor->sd.internal_ops = &vd56g3_internal_ops;
+	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			    V4L2_SUBDEV_FL_HAS_EVENTS;
+	sensor->sd.entity.ops = &vd56g3_subdev_entity_ops;
+
+	/* Init source pad */
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+	if (ret) {
+		dev_err(&client->dev, "Failed to init media entity : %d", ret);
+		return ret;
+	}
+
+	/* Init controls */
+	ret = vd56g3_init_controls(sensor);
+	if (ret) {
+		dev_err(&client->dev, "Controls initialization failed %d", ret);
+		goto err_media;
+	}
+
+	/* Init vd56g3 struct : default resolution + raw8 */
+	sensor->streaming = false;
+	sensor->sd.state_lock = sensor->ctrl_handler.lock;
+	ret = v4l2_subdev_init_finalize(&sensor->sd);
+	if (ret) {
+		dev_err(&client->dev, "subdev init error: %d", ret);
+		goto err_ctrls;
+	}
+
+	vd56g3_update_controls(sensor);
+
+	return 0;
+
+err_ctrls:
+	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
+
+err_media:
+	media_entity_cleanup(&sensor->sd.entity);
+	return ret;
+}
+
+static void vd56g3_subdev_cleanup(struct vd56g3 *sensor)
+{
+	v4l2_async_unregister_subdev(&sensor->sd);
+	v4l2_subdev_cleanup(&sensor->sd);
+	media_entity_cleanup(&sensor->sd.entity);
+	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
+}
+
+static int vd56g3_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct vd56g3 *sensor;
+	int ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->i2c_client = client;
+
+	ret = vd56g3_parse_dt(sensor);
+	if (ret) {
+		dev_err(&client->dev, "Failed to parse Device Tree : %d", ret);
+		return ret;
+	}
+
+	/* Get (and check) resources : power regs, ext clock, reset gpio */
+	ret = vd56g3_get_regulators(sensor);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to get regulators.");
+
+	sensor->xclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(sensor->xclk))
+		return dev_err_probe(dev, PTR_ERR(sensor->xclk),
+				     "Failed to get xclk.");
+	sensor->ext_clock = clk_get_rate(sensor->xclk);
+	ret = vd56g3_prepare_clock_tree(sensor);
+	if (ret)
+		return ret;
+
+	sensor->reset_gpio =
+		devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->reset_gpio))
+		return dev_err_probe(dev, PTR_ERR(sensor->reset_gpio),
+				     "Failed to get reset gpio.");
+
+	sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(sensor->regmap))
+		return dev_err_probe(dev, PTR_ERR(sensor->regmap),
+				     "Failed to init regmap.");
+
+	/* Power ON */
+	ret = vd56g3_power_on(sensor);
+	if (ret) {
+		dev_err(&client->dev, "Sensor power on failed : %d", ret);
+		return ret;
+	}
+
+	/* Enable PM runtime with autosuspend (sensor being ON, set active) */
+	pm_runtime_set_active(dev);
+	pm_runtime_get_noresume(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_set_autosuspend_delay(dev, 1000);
+	pm_runtime_use_autosuspend(dev);
+
+	/* Check HW model/version */
+	ret = vd56g3_boot(sensor);
+	if (ret) {
+		dev_err(&client->dev, "Sensor boot failed : %d", ret);
+		goto err_power_off;
+	}
+
+	ret = vd56g3_detect(sensor);
+	if (ret) {
+		dev_err(&client->dev, "Sensor detect failed : %d", ret);
+		goto err_power_off;
+	}
+
+	/* Initialize, then register V4L2 subdev */
+	ret = vd56g3_subdev_init(sensor);
+	if (ret) {
+		dev_err(&client->dev, "V4l2 init failed : %d", ret);
+		goto err_power_off;
+	}
+
+	ret = v4l2_async_register_subdev(&sensor->sd);
+	if (ret) {
+		dev_err(&client->dev, "async subdev register failed %d", ret);
+		goto err_subdev;
+	}
+
+	/* Sensor could now be powered off (after the autosuspend delay) */
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
+
+	dev_dbg(&client->dev, "Successfully probe %s sensor",
+		(sensor->is_mono) ? "vd56g3" : "vd66gy");
+
+	return 0;
+
+err_subdev:
+	vd56g3_subdev_cleanup(sensor);
+err_power_off:
+	pm_runtime_disable(dev);
+	pm_runtime_put_noidle(dev);
+	vd56g3_power_off(sensor);
+	return ret;
+}
+
+static void vd56g3_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct vd56g3 *sensor = to_vd56g3(sd);
+
+	vd56g3_subdev_cleanup(sensor);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		vd56g3_power_off(sensor);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct of_device_id vd56g3_dt_ids[] = {
+	{ .compatible = "st,st-vd56g3", .data = (void *)VD56G3_MODEL_VD56G3 },
+	{ .compatible = "st,st-vd66gy", .data = (void *)VD56G3_MODEL_VD66GY },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, vd56g3_dt_ids);
+
+static struct i2c_driver vd56g3_i2c_driver = {
+	.driver = {
+		.name  = "st-vd56g3",
+		.of_match_table = vd56g3_dt_ids,
+		.pm = &vd56g3_pm_ops,
+	},
+	.probe = vd56g3_probe,
+	.remove = vd56g3_remove,
+};
+
+module_i2c_driver(vd56g3_i2c_driver);
+
+MODULE_AUTHOR("Benjamin Mugnier <[e-mail]>");
+MODULE_AUTHOR("Mickael Guene <[e-mail]>");
+MODULE_AUTHOR("Sylvain Petinot <[e-mail]>");
+MODULE_DESCRIPTION("ST VD56G3 sensor driver");
+MODULE_LICENSE("GPL");