vd56g3.c

   1// SPDX-License-Identifier: GPL-2.0
   2/*
   3 * A V4L2 driver for ST VD56G3 (Mono) and VD66GY (RGB) global shutter cameras.
   4 * Copyright (C) 2024, STMicroelectronics SA
   5 */
   6
   7#include <linux/clk.h>
   8#include <linux/delay.h>
   9#include <linux/gpio/consumer.h>
  10#include <linux/i2c.h>
  11#include <linux/iopoll.h>
  12#include <linux/module.h>
  13#include <linux/pm_runtime.h>
  14#include <linux/regmap.h>
  15#include <linux/regulator/consumer.h>
  16#include <linux/unaligned.h>
  17#include <linux/units.h>
  18
  19#include <media/mipi-csi2.h>
  20#include <media/v4l2-async.h>
  21#include <media/v4l2-cci.h>
  22#include <media/v4l2-ctrls.h>
  23#include <media/v4l2-device.h>
  24#include <media/v4l2-fwnode.h>
  25#include <media/v4l2-subdev.h>
  26
  27/* Register Map */
  28#define VD56G3_REG_MODEL_ID				CCI_REG16_LE(0x0000)
  29#define VD56G3_MODEL_ID					0x5603
  30#define VD56G3_REG_REVISION				CCI_REG16_LE(0x0002)
  31#define VD56G3_REVISION_CUT3				0x31
  32#define VD56G3_REG_OPTICAL_REVISION			CCI_REG8(0x001a)
  33#define VD56G3_OPTICAL_REVISION_MONO			0
  34#define VD56G3_OPTICAL_REVISION_BAYER			1
  35#define VD56G3_REG_SYSTEM_FSM				CCI_REG8(0x0028)
  36#define VD56G3_SYSTEM_FSM_READY_TO_BOOT			0x01
  37#define VD56G3_SYSTEM_FSM_SW_STBY			0x02
  38#define VD56G3_SYSTEM_FSM_STREAMING			0x03
  39#define VD56G3_REG_APPLIED_COARSE_EXPOSURE		CCI_REG16_LE(0x0064)
  40#define VD56G3_REG_APPLIED_ANALOG_GAIN			CCI_REG8(0x0068)
  41#define VD56G3_REG_APPLIED_DIGITAL_GAIN			CCI_REG16_LE(0x006a)
  42#define VD56G3_REG_BOOT					CCI_REG8(0x0200)
  43#define VD56G3_CMD_ACK					0
  44#define VD56G3_CMD_BOOT					1
  45#define VD56G3_REG_STBY					CCI_REG8(0x0201)
  46#define VD56G3_CMD_START_STREAM				1
  47#define VD56G3_REG_STREAMING				CCI_REG8(0x0202)
  48#define VD56G3_CMD_STOP_STREAM				1
  49#define VD56G3_REG_EXT_CLOCK				CCI_REG32_LE(0x0220)
  50#define VD56G3_REG_CLK_PLL_PREDIV			CCI_REG8(0x0224)
  51#define VD56G3_REG_CLK_SYS_PLL_MULT			CCI_REG8(0x0226)
  52#define VD56G3_REG_ORIENTATION				CCI_REG8(0x0302)
  53#define VD56G3_REG_FORMAT_CTRL				CCI_REG8(0x030a)
  54#define VD56G3_REG_OIF_CTRL				CCI_REG16_LE(0x030c)
  55#define VD56G3_REG_OIF_IMG_CTRL				CCI_REG8(0x030f)
  56#define VD56G3_REG_OIF_CSI_BITRATE			CCI_REG16_LE(0x0312)
  57#define VD56G3_REG_DUSTER_CTRL				CCI_REG8(0x0318)
  58#define VD56G3_DUSTER_DISABLE				0
  59#define VD56G3_DUSTER_ENABLE_DEF_MODULES		0x13
  60#define VD56G3_REG_ISL_ENABLE				CCI_REG8(0x0333)
  61#define VD56G3_REG_DARKCAL_CTRL				CCI_REG8(0x0340)
  62#define VD56G3_DARKCAL_ENABLE				1
  63#define VD56G3_DARKCAL_DISABLE_DARKAVG			2
  64#define VD56G3_REG_PATGEN_CTRL				CCI_REG16_LE(0x0400)
  65#define VD56G3_PATGEN_ENABLE				1
  66#define VD56G3_PATGEN_TYPE_SHIFT			4
  67#define VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE		CCI_REG16_LE(0x042a)
  68#define VD56G3_REG_AE_COLDSTART_ANALOG_GAIN		CCI_REG8(0x042c)
  69#define VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN		CCI_REG16_LE(0x042e)
  70#define VD56G3_REG_AE_ROI_START_H			CCI_REG16_LE(0x0432)
  71#define VD56G3_REG_AE_ROI_START_V			CCI_REG16_LE(0x0434)
  72#define VD56G3_REG_AE_ROI_END_H				CCI_REG16_LE(0x0436)
  73#define VD56G3_REG_AE_ROI_END_V				CCI_REG16_LE(0x0438)
  74#define VD56G3_REG_AE_COMPENSATION			CCI_REG16_LE(0x043a)
  75#define VD56G3_REG_EXP_MODE				CCI_REG8(0x044c)
  76#define VD56G3_EXP_MODE_AUTO				0
  77#define VD56G3_EXP_MODE_FREEZE				1
  78#define VD56G3_EXP_MODE_MANUAL				2
  79#define VD56G3_REG_MANUAL_ANALOG_GAIN			CCI_REG8(0x044d)
  80#define VD56G3_REG_MANUAL_COARSE_EXPOSURE		CCI_REG16_LE(0x044e)
  81#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0		CCI_REG16_LE(0x0450)
  82#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1		CCI_REG16_LE(0x0452)
  83#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2		CCI_REG16_LE(0x0454)
  84#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3		CCI_REG16_LE(0x0456)
  85#define VD56G3_REG_FRAME_LENGTH				CCI_REG16_LE(0x0458)
  86#define VD56G3_REG_Y_START				CCI_REG16_LE(0x045a)
  87#define VD56G3_REG_Y_END				CCI_REG16_LE(0x045c)
  88#define VD56G3_REG_OUT_ROI_X_START			CCI_REG16_LE(0x045e)
  89#define VD56G3_REG_OUT_ROI_X_END			CCI_REG16_LE(0x0460)
  90#define VD56G3_REG_OUT_ROI_Y_START			CCI_REG16_LE(0x0462)
  91#define VD56G3_REG_OUT_ROI_Y_END			CCI_REG16_LE(0x0464)
  92#define VD56G3_REG_GPIO_0_CTRL				CCI_REG8(0x0467)
  93#define VD56G3_GPIOX_GPIO_IN				0x01
  94#define VD56G3_GPIOX_STROBE_MODE			0x02
  95#define VD56G3_REG_READOUT_CTRL				CCI_REG8(0x047e)
  96#define READOUT_NORMAL					0x00
  97#define READOUT_DIGITAL_BINNING_X2			0x01
  98
  99/* The VD56G3 is a portrait image sensor with native resolution of 1124x1364. */
 100#define VD56G3_NATIVE_WIDTH				1124
 101#define VD56G3_NATIVE_HEIGHT				1364
 102#define VD56G3_DEFAULT_MODE				0
 103
 104/* PLL settings */
 105#define VD56G3_TARGET_PLL				804000000UL
 106#define VD56G3_VT_CLOCK_DIV				5
 107
 108/* External clock must be in [6Mhz-27Mhz] */
 109#define VD56G3_XCLK_FREQ_MIN				 (6 * HZ_PER_MHZ)
 110#define VD56G3_XCLK_FREQ_MAX				 (27 * HZ_PER_MHZ)
 111
 112/* Line length and Frame length (settings are for standard 10bits ADC mode) */
 113#define VD56G3_LINE_LENGTH_MIN				1236
 114#define VD56G3_VBLANK_MIN				110
 115#define VD56G3_FRAME_LENGTH_DEF_60FPS			2168
 116#define VD56G3_FRAME_LENGTH_MAX				0xffff
 117
 118/* Exposure settings */
 119#define VD56G3_EXPOSURE_MARGIN				75
 120#define VD56G3_EXPOSURE_MIN				5
 121#define VD56G3_EXPOSURE_DEFAULT				1420
 122
 123/* Output Interface settings */
 124#define VD56G3_MAX_CSI_DATA_LANES			2
 125#define VD56G3_LINK_FREQ_DEF_1LANE			750000000UL
 126#define VD56G3_LINK_FREQ_DEF_2LANES			402000000UL
 127
 128/* GPIOs */
 129#define VD56G3_NB_GPIOS					8
 130
 131/* regulator supplies */
 132static const char *const vd56g3_supply_names[] = {
 133	"vcore",
 134	"vddio",
 135	"vana",
 136};
 137
 138/* -----------------------------------------------------------------------------
 139 * Models (VD56G3: Mono, VD66GY: Bayer RGB), Modes and formats
 140 */
 141
 142enum vd56g3_models {
 143	VD56G3_MODEL_VD56G3,
 144	VD56G3_MODEL_VD66GY,
 145};
 146
 147struct vd56g3_mode {
 148	u32 width;
 149	u32 height;
 150};
 151
 152static const struct vd56g3_mode vd56g3_supported_modes[] = {
 153	{
 154		.width = VD56G3_NATIVE_WIDTH,
 155		.height = VD56G3_NATIVE_HEIGHT,
 156	},
 157	{
 158		.width = 1120,
 159		.height = 1360,
 160	},
 161	{
 162		.width = 1024,
 163		.height = 1280,
 164	},
 165	{
 166		.width = 1024,
 167		.height = 768,
 168	},
 169	{
 170		.width = 768,
 171		.height = 1024,
 172	},
 173	{
 174		.width = 720,
 175		.height = 1280,
 176	},
 177	{
 178		.width = 640,
 179		.height = 480,
 180	},
 181	{
 182		.width = 480,
 183		.height = 640,
 184	},
 185	{
 186		.width = 320,
 187		.height = 240,
 188	},
 189};
 190
 191/*
 192 * Sensor support 8bits and 10bits output in both variants
 193 *  - Monochrome
 194 *  - RGB (with all H/V flip variations)
 195 */
 196static const unsigned int vd56g3_mbus_codes[2][5] = {
 197	{
 198		MEDIA_BUS_FMT_Y8_1X8,
 199		MEDIA_BUS_FMT_SGRBG8_1X8,
 200		MEDIA_BUS_FMT_SRGGB8_1X8,
 201		MEDIA_BUS_FMT_SBGGR8_1X8,
 202		MEDIA_BUS_FMT_SGBRG8_1X8,
 203	},
 204	{
 205		MEDIA_BUS_FMT_Y10_1X10,
 206		MEDIA_BUS_FMT_SGRBG10_1X10,
 207		MEDIA_BUS_FMT_SRGGB10_1X10,
 208		MEDIA_BUS_FMT_SBGGR10_1X10,
 209		MEDIA_BUS_FMT_SGBRG10_1X10,
 210	},
 211};
 212
 213struct vd56g3 {
 214	struct device *dev;
 215	struct v4l2_subdev sd;
 216	struct media_pad pad;
 217	struct regulator_bulk_data supplies[ARRAY_SIZE(vd56g3_supply_names)];
 218	struct gpio_desc *reset_gpio;
 219	struct clk *xclk;
 220	struct regmap *regmap;
 221	u32 xclk_freq;
 222	u32 pll_prediv;
 223	u32 pll_mult;
 224	u32 pixel_clock;
 225	u16 oif_ctrl;
 226	u8 nb_of_lane;
 227	u32 gpios[VD56G3_NB_GPIOS];
 228	unsigned long ext_leds_mask;
 229	bool is_mono;
 230	struct v4l2_ctrl_handler ctrl_handler;
 231	struct v4l2_ctrl *hblank_ctrl;
 232	struct v4l2_ctrl *vblank_ctrl;
 233	struct {
 234		struct v4l2_ctrl *hflip_ctrl;
 235		struct v4l2_ctrl *vflip_ctrl;
 236	};
 237	struct v4l2_ctrl *patgen_ctrl;
 238	struct {
 239		struct v4l2_ctrl *ae_ctrl;
 240		struct v4l2_ctrl *expo_ctrl;
 241		struct v4l2_ctrl *again_ctrl;
 242		struct v4l2_ctrl *dgain_ctrl;
 243	};
 244	struct v4l2_ctrl *ae_lock_ctrl;
 245	struct v4l2_ctrl *ae_bias_ctrl;
 246	struct v4l2_ctrl *led_ctrl;
 247};
 248
 249static inline struct vd56g3 *to_vd56g3(struct v4l2_subdev *sd)
 250{
 251	return container_of_const(sd, struct vd56g3, sd);
 252}
 253
 254static inline struct vd56g3 *ctrl_to_vd56g3(struct v4l2_ctrl *ctrl)
 255{
 256	return container_of_const(ctrl->handler, struct vd56g3, ctrl_handler);
 257}
 258
 259/* -----------------------------------------------------------------------------
 260 * Additional i2c register helpers
 261 */
 262
 263static int vd56g3_poll_reg(struct vd56g3 *sensor, u32 reg, u8 poll_val,
 264			   int *err)
 265{
 266	unsigned int val = 0;
 267	int ret;
 268
 269	if (err && *err)
 270		return *err;
 271
 272	/*
 273	 * Timeout must be higher than longuest frame duration. With current
 274	 * blanking constraints, frame duration can take up to 504ms.
 275	 */
 276	ret = regmap_read_poll_timeout(sensor->regmap, CCI_REG_ADDR(reg), val,
 277				       (val == poll_val), 2000,
 278				       600 * USEC_PER_MSEC);
 279
 280	if (ret && err)
 281		*err = ret;
 282
 283	return ret;
 284}
 285
 286static int vd56g3_wait_state(struct vd56g3 *sensor, int state, int *err)
 287{
 288	return vd56g3_poll_reg(sensor, VD56G3_REG_SYSTEM_FSM, state, err);
 289}
 290
 291/* -----------------------------------------------------------------------------
 292 * Controls: definitions, helpers and handlers
 293 */
 294
 295static const char *const vd56g3_tp_menu[] = { "Disabled",
 296					      "Solid Color",
 297					      "Vertical Color Bars",
 298					      "Horizontal Gray Scale",
 299					      "Vertical Gray Scale",
 300					      "Diagonal Gray Scale",
 301					      "Pseudo Random" };
 302
 303static const s64 vd56g3_ev_bias_qmenu[] = { -4000, -3500, -3000, -2500, -2000,
 304					    -1500, -1000, -500,	 0,	500,
 305					    1000,  1500,  2000,	 2500,	3000,
 306					    3500,  4000 };
 307
 308static const s64 vd56g3_link_freq_1lane[] = { VD56G3_LINK_FREQ_DEF_1LANE };
 309
 310static const s64 vd56g3_link_freq_2lanes[] = { VD56G3_LINK_FREQ_DEF_2LANES };
 311
 312static u8 vd56g3_get_bpp(__u32 code)
 313{
 314	switch (code) {
 315	case MEDIA_BUS_FMT_Y8_1X8:
 316	case MEDIA_BUS_FMT_SGRBG8_1X8:
 317	case MEDIA_BUS_FMT_SRGGB8_1X8:
 318	case MEDIA_BUS_FMT_SBGGR8_1X8:
 319	case MEDIA_BUS_FMT_SGBRG8_1X8:
 320	default:
 321		return 8;
 322	case MEDIA_BUS_FMT_Y10_1X10:
 323	case MEDIA_BUS_FMT_SGRBG10_1X10:
 324	case MEDIA_BUS_FMT_SRGGB10_1X10:
 325	case MEDIA_BUS_FMT_SBGGR10_1X10:
 326	case MEDIA_BUS_FMT_SGBRG10_1X10:
 327		return 10;
 328	}
 329}
 330
 331static u8 vd56g3_get_datatype(__u32 code)
 332{
 333	switch (code) {
 334	case MEDIA_BUS_FMT_Y8_1X8:
 335	case MEDIA_BUS_FMT_SGRBG8_1X8:
 336	case MEDIA_BUS_FMT_SRGGB8_1X8:
 337	case MEDIA_BUS_FMT_SBGGR8_1X8:
 338	case MEDIA_BUS_FMT_SGBRG8_1X8:
 339	default:
 340		return MIPI_CSI2_DT_RAW8;
 341	case MEDIA_BUS_FMT_Y10_1X10:
 342	case MEDIA_BUS_FMT_SGRBG10_1X10:
 343	case MEDIA_BUS_FMT_SRGGB10_1X10:
 344	case MEDIA_BUS_FMT_SBGGR10_1X10:
 345	case MEDIA_BUS_FMT_SGBRG10_1X10:
 346		return MIPI_CSI2_DT_RAW10;
 347	}
 348}
 349
 350static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
 351{
 352	u64 exposure;
 353	u64 again;
 354	u64 dgain;
 355	int ret = 0;
 356
 357	/*
 358	 * When 'force_cur_val' is enabled, save the ctrl value in 'cur.val'
 359	 * instead of the normal 'val', this is used during poweroff to cache
 360	 * volatile ctrls and enable coldstart.
 361	 */
 362	cci_read(sensor->regmap, VD56G3_REG_APPLIED_COARSE_EXPOSURE, &exposure,
 363		 &ret);
 364	cci_read(sensor->regmap, VD56G3_REG_APPLIED_ANALOG_GAIN, &again, &ret);
 365	cci_read(sensor->regmap, VD56G3_REG_APPLIED_DIGITAL_GAIN, &dgain, &ret);
 366	if (ret)
 367		return ret;
 368
 369	if (force_cur_val) {
 370		sensor->expo_ctrl->cur.val = exposure;
 371		sensor->again_ctrl->cur.val = again;
 372		sensor->dgain_ctrl->cur.val = dgain;
 373	} else {
 374		sensor->expo_ctrl->val = exposure;
 375		sensor->again_ctrl->val = again;
 376		sensor->dgain_ctrl->val = dgain;
 377	}
 378
 379	return ret;
 380}
 381
 382static int vd56g3_update_patgen(struct vd56g3 *sensor, u32 patgen_index)
 383{
 384	u32 pattern = patgen_index <= 2 ? patgen_index : patgen_index + 13;
 385	u16 patgen = pattern << VD56G3_PATGEN_TYPE_SHIFT;
 386	u8 duster = VD56G3_DUSTER_ENABLE_DEF_MODULES;
 387	u8 darkcal = VD56G3_DARKCAL_ENABLE;
 388	int ret = 0;
 389
 390	if (patgen_index) {
 391		patgen |= VD56G3_PATGEN_ENABLE;
 392		duster = VD56G3_DUSTER_DISABLE;
 393		darkcal = VD56G3_DARKCAL_DISABLE_DARKAVG;
 394	}
 395
 396	cci_write(sensor->regmap, VD56G3_REG_DUSTER_CTRL, duster, &ret);
 397	cci_write(sensor->regmap, VD56G3_REG_DARKCAL_CTRL, darkcal, &ret);
 398	cci_write(sensor->regmap, VD56G3_REG_PATGEN_CTRL, patgen, &ret);
 399
 400	return ret;
 401}
 402
 403static int vd56g3_update_expo_cluster(struct vd56g3 *sensor, bool is_auto)
 404{
 405	u8 expo_state = is_auto ? VD56G3_EXP_MODE_AUTO : VD56G3_EXP_MODE_MANUAL;
 406	int ret = 0;
 407
 408	if (sensor->ae_ctrl->is_new)
 409		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
 410			  &ret);
 411
 412	/* In Auto expo, set coldstart parameters */
 413	if (is_auto && sensor->ae_ctrl->is_new) {
 414		cci_write(sensor->regmap,
 415			  VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE,
 416			  sensor->expo_ctrl->val, &ret);
 417		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_ANALOG_GAIN,
 418			  sensor->again_ctrl->val, &ret);
 419		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN,
 420			  sensor->dgain_ctrl->val, &ret);
 421	}
 422
 423	/* In Manual expo, set exposure, analog and digital gains */
 424	if (!is_auto && sensor->expo_ctrl->is_new)
 425		cci_write(sensor->regmap, VD56G3_REG_MANUAL_COARSE_EXPOSURE,
 426			  sensor->expo_ctrl->val, &ret);
 427
 428	if (!is_auto && sensor->again_ctrl->is_new)
 429		cci_write(sensor->regmap, VD56G3_REG_MANUAL_ANALOG_GAIN,
 430			  sensor->again_ctrl->val, &ret);
 431
 432	if (!is_auto && sensor->dgain_ctrl->is_new) {
 433		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0,
 434			  sensor->dgain_ctrl->val, &ret);
 435		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1,
 436			  sensor->dgain_ctrl->val, &ret);
 437		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2,
 438			  sensor->dgain_ctrl->val, &ret);
 439		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3,
 440			  sensor->dgain_ctrl->val, &ret);
 441	}
 442
 443	return ret;
 444}
 445
 446static int vd56g3_lock_exposure(struct vd56g3 *sensor, u32 lock_val)
 447{
 448	bool ae_lock = lock_val & V4L2_LOCK_EXPOSURE;
 449	u8 expo_state = ae_lock ? VD56G3_EXP_MODE_FREEZE : VD56G3_EXP_MODE_AUTO;
 450
 451	if (sensor->ae_ctrl->val == V4L2_EXPOSURE_AUTO)
 452		return cci_write(sensor->regmap, VD56G3_REG_EXP_MODE,
 453				 expo_state, NULL);
 454
 455	return 0;
 456}
 457
 458static int vd56g3_write_gpiox(struct vd56g3 *sensor, unsigned long gpio_mask)
 459{
 460	unsigned long io;
 461	u32 gpio_val;
 462	int ret = 0;
 463
 464	for_each_set_bit(io, &gpio_mask, VD56G3_NB_GPIOS) {
 465		gpio_val = sensor->gpios[io];
 466
 467		if (gpio_val == VD56G3_GPIOX_STROBE_MODE &&
 468		    sensor->led_ctrl->val == V4L2_FLASH_LED_MODE_NONE)
 469			gpio_val = VD56G3_GPIOX_GPIO_IN;
 470
 471		cci_write(sensor->regmap, VD56G3_REG_GPIO_0_CTRL + io, gpio_val,
 472			  &ret);
 473	}
 474
 475	return ret;
 476}
 477
 478static int vd56g3_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 479{
 480	struct vd56g3 *sensor = ctrl_to_vd56g3(ctrl);
 481	int ret = 0;
 482
 483	/* Interact with HW only when it is powered ON */
 484	if (!pm_runtime_get_if_in_use(sensor->dev))
 485		return 0;
 486
 487	switch (ctrl->id) {
 488	case V4L2_CID_EXPOSURE_AUTO:
 489		ret = vd56g3_read_expo_cluster(sensor, false);
 490		break;
 491	default:
 492		ret = -EINVAL;
 493		break;
 494	}
 495
 496	pm_runtime_put_autosuspend(sensor->dev);
 497
 498	return ret;
 499}
 500
 501static int vd56g3_s_ctrl(struct v4l2_ctrl *ctrl)
 502{
 503	struct vd56g3 *sensor = ctrl_to_vd56g3(ctrl);
 504	struct v4l2_subdev_state *state;
 505	const struct v4l2_rect *crop;
 506	unsigned int frame_length = 0;
 507	unsigned int expo_max;
 508	unsigned int ae_compensation;
 509	bool is_auto = false;
 510	int ret = 0;
 511
 512	state = v4l2_subdev_get_locked_active_state(&sensor->sd);
 513	crop = v4l2_subdev_state_get_crop(state, 0);
 514
 515	if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
 516		return 0;
 517
 518	/* Update controls state, range, etc. whatever the state of the HW */
 519	switch (ctrl->id) {
 520	case V4L2_CID_VBLANK:
 521		frame_length = crop->height + ctrl->val;
 522		expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
 523		ret = __v4l2_ctrl_modify_range(sensor->expo_ctrl,
 524					       VD56G3_EXPOSURE_MIN, expo_max, 1,
 525					       min(VD56G3_EXPOSURE_DEFAULT,
 526						   expo_max));
 527		break;
 528	case V4L2_CID_EXPOSURE_AUTO:
 529		is_auto = (ctrl->val == V4L2_EXPOSURE_AUTO);
 530		__v4l2_ctrl_grab(sensor->ae_lock_ctrl, !is_auto);
 531		__v4l2_ctrl_grab(sensor->ae_bias_ctrl, !is_auto);
 532		break;
 533	default:
 534		break;
 535	}
 536
 537	if (ret)
 538		return ret;
 539
 540	/* Interact with HW only when it is powered ON */
 541	if (!pm_runtime_get_if_in_use(sensor->dev))
 542		return 0;
 543
 544	switch (ctrl->id) {
 545	case V4L2_CID_HFLIP:
 546		ret = cci_write(sensor->regmap, VD56G3_REG_ORIENTATION,
 547				sensor->hflip_ctrl->val |
 548					(sensor->vflip_ctrl->val << 1),
 549				NULL);
 550		break;
 551	case V4L2_CID_TEST_PATTERN:
 552		ret = vd56g3_update_patgen(sensor, ctrl->val);
 553		break;
 554	case V4L2_CID_EXPOSURE_AUTO:
 555		ret = vd56g3_update_expo_cluster(sensor, is_auto);
 556		break;
 557	case V4L2_CID_3A_LOCK:
 558		ret = vd56g3_lock_exposure(sensor, ctrl->val);
 559		break;
 560	case V4L2_CID_AUTO_EXPOSURE_BIAS:
 561		ae_compensation =
 562			DIV_ROUND_CLOSEST((int)vd56g3_ev_bias_qmenu[ctrl->val] *
 563					  256, 1000);
 564		ret = cci_write(sensor->regmap, VD56G3_REG_AE_COMPENSATION,
 565				ae_compensation, NULL);
 566		break;
 567	case V4L2_CID_VBLANK:
 568		ret = cci_write(sensor->regmap, VD56G3_REG_FRAME_LENGTH,
 569				frame_length, NULL);
 570		break;
 571	case V4L2_CID_FLASH_LED_MODE:
 572		ret = vd56g3_write_gpiox(sensor, sensor->ext_leds_mask);
 573		break;
 574	default:
 575		ret = -EINVAL;
 576		break;
 577	}
 578
 579	pm_runtime_put_autosuspend(sensor->dev);
 580
 581	return ret;
 582}
 583
 584static const struct v4l2_ctrl_ops vd56g3_ctrl_ops = {
 585	.g_volatile_ctrl = vd56g3_g_volatile_ctrl,
 586	.s_ctrl = vd56g3_s_ctrl,
 587};
 588
 589static int vd56g3_update_controls(struct vd56g3 *sensor)
 590{
 591	struct v4l2_subdev_state *state;
 592	const struct v4l2_rect *crop;
 593	unsigned int hblank;
 594	unsigned int vblank_min, vblank, vblank_max;
 595	unsigned int frame_length;
 596	unsigned int expo_max;
 597	int ret;
 598
 599	state = v4l2_subdev_get_locked_active_state(&sensor->sd);
 600	crop = v4l2_subdev_state_get_crop(state, 0);
 601	hblank = VD56G3_LINE_LENGTH_MIN - crop->width;
 602	vblank_min = VD56G3_VBLANK_MIN;
 603	vblank = VD56G3_FRAME_LENGTH_DEF_60FPS - crop->height;
 604	vblank_max = VD56G3_FRAME_LENGTH_MAX - crop->height;
 605	frame_length = crop->height + vblank;
 606	expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
 607
 608	/* Update blanking and exposure (ranges + values) */
 609	ret = __v4l2_ctrl_modify_range(sensor->hblank_ctrl, hblank, hblank, 1,
 610				       hblank);
 611	if (ret)
 612		return ret;
 613
 614	ret = __v4l2_ctrl_modify_range(sensor->vblank_ctrl, vblank_min,
 615				       vblank_max, 1, vblank);
 616	if (ret)
 617		return ret;
 618
 619	ret = __v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, vblank);
 620	if (ret)
 621		return ret;
 622
 623	ret = __v4l2_ctrl_modify_range(sensor->expo_ctrl, VD56G3_EXPOSURE_MIN,
 624				       expo_max, 1, VD56G3_EXPOSURE_DEFAULT);
 625	if (ret)
 626		return ret;
 627
 628	return __v4l2_ctrl_s_ctrl(sensor->expo_ctrl, VD56G3_EXPOSURE_DEFAULT);
 629}
 630
 631static int vd56g3_init_controls(struct vd56g3 *sensor)
 632{
 633	const struct v4l2_ctrl_ops *ops = &vd56g3_ctrl_ops;
 634	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
 635	struct v4l2_fwnode_device_properties fwnode_props;
 636	struct v4l2_ctrl *ctrl;
 637	int ret;
 638
 639	v4l2_ctrl_handler_init(hdl, 25);
 640
 641	/* Horizontal & vertical flips modify bayer code on RGB variant */
 642	sensor->hflip_ctrl =
 643		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
 644	if (sensor->hflip_ctrl)
 645		sensor->hflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
 646
 647	sensor->vflip_ctrl =
 648		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
 649	if (sensor->vflip_ctrl)
 650		sensor->vflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
 651
 652	sensor->patgen_ctrl =
 653		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
 654					     ARRAY_SIZE(vd56g3_tp_menu) - 1, 0,
 655					     0, vd56g3_tp_menu);
 656
 657	ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
 658				      ARRAY_SIZE(vd56g3_link_freq_1lane) - 1, 0,
 659				      (sensor->nb_of_lane == 2) ?
 660					      vd56g3_link_freq_2lanes :
 661					      vd56g3_link_freq_1lane);
 662	if (ctrl)
 663		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 664
 665	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
 666				 sensor->pixel_clock, sensor->pixel_clock, 1,
 667				 sensor->pixel_clock);
 668	if (ctrl)
 669		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 670
 671	sensor->ae_ctrl = v4l2_ctrl_new_std_menu(hdl, ops,
 672						 V4L2_CID_EXPOSURE_AUTO,
 673						 V4L2_EXPOSURE_MANUAL, 0,
 674						 V4L2_EXPOSURE_AUTO);
 675
 676	sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK, 0,
 677						 GENMASK(2, 0), 0, 0);
 678
 679	sensor->ae_bias_ctrl =
 680		v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
 681				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) - 1,
 682				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) / 2,
 683				       vd56g3_ev_bias_qmenu);
 684
 685	/*
 686	 * Analog gain [1, 8] is computed with the following logic :
 687	 * 32/(32 - again_reg), with again_reg in the range [0:28]
 688	 * Digital gain [1.00, 8.00] is coded as a Fixed Point 5.8
 689	 */
 690	sensor->again_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
 691					       0, 28, 1, 0);
 692	sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
 693					       0x100, 0x800, 1, 0x100);
 694
 695	/*
 696	 * Set the exposure, horizontal and vertical blanking ctrls
 697	 * to hardcoded values, they will be updated in vd56g3_update_controls.
 698	 * Exposure being in an auto-cluster, set a significant value here.
 699	 */
 700	sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
 701					      VD56G3_EXPOSURE_DEFAULT,
 702					      VD56G3_EXPOSURE_DEFAULT, 1,
 703					      VD56G3_EXPOSURE_DEFAULT);
 704	sensor->hblank_ctrl =
 705		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 1, 1, 1, 1);
 706	if (sensor->hblank_ctrl)
 707		sensor->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 708	sensor->vblank_ctrl =
 709		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, 1, 1, 1, 1);
 710
 711	/* Additional control based on device tree properties */
 712	if (sensor->ext_leds_mask)
 713		sensor->led_ctrl =
 714			v4l2_ctrl_new_std_menu(hdl, ops,
 715					       V4L2_CID_FLASH_LED_MODE,
 716					       V4L2_FLASH_LED_MODE_FLASH, 0,
 717					       V4L2_FLASH_LED_MODE_NONE);
 718
 719	if (hdl->error) {
 720		ret = hdl->error;
 721		goto free_ctrls;
 722	}
 723
 724	v4l2_ctrl_cluster(2, &sensor->hflip_ctrl);
 725	v4l2_ctrl_auto_cluster(4, &sensor->ae_ctrl, V4L2_EXPOSURE_MANUAL, true);
 726
 727	/* Optional controls coming from fwnode (e.g. rotation, orientation). */
 728	ret = v4l2_fwnode_device_parse(sensor->dev, &fwnode_props);
 729	if (ret)
 730		goto free_ctrls;
 731
 732	ret = v4l2_ctrl_new_fwnode_properties(hdl, ops, &fwnode_props);
 733	if (ret)
 734		goto free_ctrls;
 735
 736	sensor->sd.ctrl_handler = hdl;
 737
 738	return 0;
 739
 740free_ctrls:
 741	v4l2_ctrl_handler_free(hdl);
 742
 743	return ret;
 744}
 745
 746/* -----------------------------------------------------------------------------
 747 * Pad ops
 748 */
 749
 750/* Media bus code is dependent of :
 751 *      - 8bits or 10bits output
 752 *      - variant : Mono or RGB
 753 *      - H/V flips parameters in case of RGB
 754 */
 755static u32 vd56g3_get_mbus_code(struct vd56g3 *sensor, u32 code)
 756{
 757	unsigned int i_bpp;
 758	unsigned int j;
 759
 760	for (i_bpp = 0; i_bpp < ARRAY_SIZE(vd56g3_mbus_codes); i_bpp++) {
 761		for (j = 0; j < ARRAY_SIZE(vd56g3_mbus_codes[i_bpp]); j++) {
 762			if (vd56g3_mbus_codes[i_bpp][j] == code)
 763				goto endloops;
 764		}
 765	}
 766
 767endloops:
 768	if (i_bpp >= ARRAY_SIZE(vd56g3_mbus_codes))
 769		i_bpp = 0;
 770
 771	if (sensor->is_mono)
 772		j = 0;
 773	else
 774		j = 1 + (sensor->hflip_ctrl->val ? 1 : 0) +
 775		    (sensor->vflip_ctrl->val ? 2 : 0);
 776
 777	return vd56g3_mbus_codes[i_bpp][j];
 778}
 779
 780static int vd56g3_enum_mbus_code(struct v4l2_subdev *sd,
 781				 struct v4l2_subdev_state *sd_state,
 782				 struct v4l2_subdev_mbus_code_enum *code)
 783{
 784	struct vd56g3 *sensor = to_vd56g3(sd);
 785
 786	if (code->index >= ARRAY_SIZE(vd56g3_mbus_codes))
 787		return -EINVAL;
 788
 789	code->code =
 790		vd56g3_get_mbus_code(sensor, vd56g3_mbus_codes[code->index][0]);
 791
 792	return 0;
 793}
 794
 795static int vd56g3_enum_frame_size(struct v4l2_subdev *sd,
 796				  struct v4l2_subdev_state *sd_state,
 797				  struct v4l2_subdev_frame_size_enum *fse)
 798{
 799	if (fse->index >= ARRAY_SIZE(vd56g3_supported_modes))
 800		return -EINVAL;
 801
 802	fse->min_width = vd56g3_supported_modes[fse->index].width;
 803	fse->max_width = fse->min_width;
 804	fse->min_height = vd56g3_supported_modes[fse->index].height;
 805	fse->max_height = fse->min_height;
 806
 807	return 0;
 808}
 809
 810static void vd56g3_update_img_pad_format(struct vd56g3 *sensor,
 811					 const struct vd56g3_mode *mode,
 812					 u32 mbus_code,
 813					 struct v4l2_mbus_framefmt *mbus_fmt)
 814{
 815	mbus_fmt->width = mode->width;
 816	mbus_fmt->height = mode->height;
 817	mbus_fmt->code = vd56g3_get_mbus_code(sensor, mbus_code);
 818	mbus_fmt->colorspace = V4L2_COLORSPACE_RAW;
 819	mbus_fmt->field = V4L2_FIELD_NONE;
 820	mbus_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
 821	mbus_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
 822	mbus_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
 823}
 824
 825static int vd56g3_set_pad_fmt(struct v4l2_subdev *sd,
 826			      struct v4l2_subdev_state *sd_state,
 827			      struct v4l2_subdev_format *sd_fmt)
 828{
 829	struct vd56g3 *sensor = to_vd56g3(sd);
 830	const struct vd56g3_mode *new_mode;
 831	struct v4l2_rect pad_crop;
 832	unsigned int binning;
 833
 834	new_mode = v4l2_find_nearest_size(vd56g3_supported_modes,
 835					  ARRAY_SIZE(vd56g3_supported_modes),
 836					  width, height, sd_fmt->format.width,
 837					  sd_fmt->format.height);
 838
 839	vd56g3_update_img_pad_format(sensor, new_mode, sd_fmt->format.code,
 840				     &sd_fmt->format);
 841	*v4l2_subdev_state_get_format(sd_state, sd_fmt->pad) = sd_fmt->format;
 842
 843	/* Compute and update crop rectangle (maximized via binning) */
 844	binning = min(VD56G3_NATIVE_WIDTH / sd_fmt->format.width,
 845		      VD56G3_NATIVE_HEIGHT / sd_fmt->format.height);
 846	binning = min(binning, 2U);
 847	pad_crop.width = sd_fmt->format.width * binning;
 848	pad_crop.height = sd_fmt->format.height * binning;
 849	pad_crop.left = (VD56G3_NATIVE_WIDTH - pad_crop.width) / 2;
 850	pad_crop.top = (VD56G3_NATIVE_HEIGHT - pad_crop.height) / 2;
 851	*v4l2_subdev_state_get_crop(sd_state, sd_fmt->pad) = pad_crop;
 852
 853	/* Update controls in case of active state */
 854	if (sd_fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 855		return vd56g3_update_controls(sensor);
 856
 857	return 0;
 858}
 859
 860static int vd56g3_get_selection(struct v4l2_subdev *sd,
 861				struct v4l2_subdev_state *sd_state,
 862				struct v4l2_subdev_selection *sel)
 863{
 864	switch (sel->target) {
 865	case V4L2_SEL_TGT_CROP:
 866		sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
 867		break;
 868	case V4L2_SEL_TGT_NATIVE_SIZE:
 869	case V4L2_SEL_TGT_CROP_DEFAULT:
 870	case V4L2_SEL_TGT_CROP_BOUNDS:
 871		sel->r.top = 0;
 872		sel->r.left = 0;
 873		sel->r.width = VD56G3_NATIVE_WIDTH;
 874		sel->r.height = VD56G3_NATIVE_HEIGHT;
 875		break;
 876	default:
 877		return -EINVAL;
 878	}
 879
 880	return 0;
 881}
 882
 883static int vd56g3_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
 884				 struct v4l2_mbus_frame_desc *fd)
 885{
 886	struct v4l2_subdev_state *state;
 887	const struct v4l2_mbus_framefmt *format;
 888
 889	state = v4l2_subdev_lock_and_get_active_state(sd);
 890	format = v4l2_subdev_state_get_format(state, pad);
 891	v4l2_subdev_unlock_state(state);
 892
 893	fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
 894	fd->num_entries = 1;
 895	fd->entry[0].pixelcode = format->code;
 896	fd->entry[0].stream = 0;
 897	fd->entry[0].bus.csi2.vc = 0;
 898	fd->entry[0].bus.csi2.dt = vd56g3_get_datatype(format->code);
 899
 900	return 0;
 901}
 902
 903static int vd56g3_enable_streams(struct v4l2_subdev *sd,
 904				 struct v4l2_subdev_state *state, u32 pad,
 905				 u64 streams_mask)
 906{
 907	struct vd56g3 *sensor = to_vd56g3(sd);
 908	const struct v4l2_mbus_framefmt *format =
 909		v4l2_subdev_state_get_format(state, 0);
 910	const struct v4l2_rect *crop = v4l2_subdev_state_get_crop(state, 0);
 911	unsigned int csi_mbps = ((sensor->nb_of_lane == 2) ?
 912					 VD56G3_LINK_FREQ_DEF_2LANES :
 913					 VD56G3_LINK_FREQ_DEF_1LANE) *
 914				2 / MEGA;
 915	unsigned int binning;
 916	int ret;
 917
 918	ret = pm_runtime_resume_and_get(sensor->dev);
 919	if (ret < 0)
 920		return ret;
 921
 922	/* configure clocks */
 923	cci_write(sensor->regmap, VD56G3_REG_EXT_CLOCK, sensor->xclk_freq,
 924		  &ret);
 925	cci_write(sensor->regmap, VD56G3_REG_CLK_PLL_PREDIV, sensor->pll_prediv,
 926		  &ret);
 927	cci_write(sensor->regmap, VD56G3_REG_CLK_SYS_PLL_MULT, sensor->pll_mult,
 928		  &ret);
 929
 930	/* configure output */
 931	cci_write(sensor->regmap, VD56G3_REG_FORMAT_CTRL,
 932		  vd56g3_get_bpp(format->code), &ret);
 933	cci_write(sensor->regmap, VD56G3_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
 934	cci_write(sensor->regmap, VD56G3_REG_OIF_CSI_BITRATE, csi_mbps, &ret);
 935	cci_write(sensor->regmap, VD56G3_REG_OIF_IMG_CTRL,
 936		  vd56g3_get_datatype(format->code), &ret);
 937	cci_write(sensor->regmap, VD56G3_REG_ISL_ENABLE, 0, &ret);
 938
 939	/* configure binning mode */
 940	switch (crop->width / format->width) {
 941	case 1:
 942	default:
 943		binning = READOUT_NORMAL;
 944		break;
 945	case 2:
 946		binning = READOUT_DIGITAL_BINNING_X2;
 947		break;
 948	}
 949	cci_write(sensor->regmap, VD56G3_REG_READOUT_CTRL, binning, &ret);
 950
 951	/* configure ROIs */
 952	cci_write(sensor->regmap, VD56G3_REG_Y_START, crop->top, &ret);
 953	cci_write(sensor->regmap, VD56G3_REG_Y_END,
 954		  crop->top + crop->height - 1, &ret);
 955	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_START, crop->left, &ret);
 956	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_END,
 957		  crop->left + crop->width - 1, &ret);
 958	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_START, 0, &ret);
 959	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_END, crop->height - 1,
 960		  &ret);
 961	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_H, crop->left, &ret);
 962	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_H,
 963		  crop->left + crop->width - 1, &ret);
 964	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_V, 0, &ret);
 965	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_V, crop->height - 1,
 966		  &ret);
 967	if (ret)
 968		goto rpm_put;
 969
 970	/* Setup default GPIO values; could be overridden by V4L2 ctrl setup */
 971	ret = vd56g3_write_gpiox(sensor, GENMASK(VD56G3_NB_GPIOS - 1, 0));
 972	if (ret)
 973		goto rpm_put;
 974
 975	/* Apply settings from V4L2 ctrls */
 976	ret = __v4l2_ctrl_handler_setup(&sensor->ctrl_handler);
 977	if (ret)
 978		goto rpm_put;
 979
 980	/* start streaming */
 981	cci_write(sensor->regmap, VD56G3_REG_STBY, VD56G3_CMD_START_STREAM,
 982		  &ret);
 983	vd56g3_poll_reg(sensor, VD56G3_REG_STBY, VD56G3_CMD_ACK, &ret);
 984	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_STREAMING, &ret);
 985	if (ret)
 986		goto rpm_put;
 987
 988	/* some controls are locked during streaming */
 989	__v4l2_ctrl_grab(sensor->hflip_ctrl, true);
 990	__v4l2_ctrl_grab(sensor->vflip_ctrl, true);
 991	__v4l2_ctrl_grab(sensor->patgen_ctrl, true);
 992
 993	return ret;
 994
 995rpm_put:
 996	dev_err(sensor->dev, "Failed to start streaming\n");
 997	pm_runtime_put_sync(sensor->dev);
 998
 999	return ret;
1000}
1001
1002static int vd56g3_disable_streams(struct v4l2_subdev *sd,
1003				  struct v4l2_subdev_state *state, u32 pad,
1004				  u64 streams_mask)
1005{
1006	struct vd56g3 *sensor = to_vd56g3(sd);
1007	int ret;
1008
1009	/* Retrieve Expo cluster to enable coldstart of AE */
1010	ret = vd56g3_read_expo_cluster(sensor, true);
1011
1012	cci_write(sensor->regmap, VD56G3_REG_STREAMING, VD56G3_CMD_STOP_STREAM,
1013		  &ret);
1014	vd56g3_poll_reg(sensor, VD56G3_REG_STREAMING, VD56G3_CMD_ACK, &ret);
1015	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
1016
1017	/* locked controls must be unlocked */
1018	__v4l2_ctrl_grab(sensor->hflip_ctrl, false);
1019	__v4l2_ctrl_grab(sensor->vflip_ctrl, false);
1020	__v4l2_ctrl_grab(sensor->patgen_ctrl, false);
1021
1022	pm_runtime_put_autosuspend(sensor->dev);
1023
1024	return ret;
1025}
1026
1027static int vd56g3_init_state(struct v4l2_subdev *sd,
1028			     struct v4l2_subdev_state *sd_state)
1029{
1030	unsigned int def_mode = VD56G3_DEFAULT_MODE;
1031	struct v4l2_subdev_format fmt = {
1032		.which = V4L2_SUBDEV_FORMAT_TRY,
1033		.pad = 0,
1034		.format = {
1035			.code = vd56g3_mbus_codes[0][0],
1036			.width = vd56g3_supported_modes[def_mode].width,
1037			.height = vd56g3_supported_modes[def_mode].height,
1038		},
1039	};
1040
1041	return vd56g3_set_pad_fmt(sd, sd_state, &fmt);
1042}
1043
1044static const struct v4l2_subdev_video_ops vd56g3_video_ops = {
1045	.s_stream = v4l2_subdev_s_stream_helper,
1046};
1047
1048static const struct v4l2_subdev_pad_ops vd56g3_pad_ops = {
1049	.enum_mbus_code = vd56g3_enum_mbus_code,
1050	.enum_frame_size = vd56g3_enum_frame_size,
1051	.get_fmt = v4l2_subdev_get_fmt,
1052	.set_fmt = vd56g3_set_pad_fmt,
1053	.get_selection = vd56g3_get_selection,
1054	.get_frame_desc = vd56g3_get_frame_desc,
1055	.enable_streams = vd56g3_enable_streams,
1056	.disable_streams = vd56g3_disable_streams,
1057};
1058
1059static const struct v4l2_subdev_ops vd56g3_subdev_ops = {
1060	.video = &vd56g3_video_ops,
1061	.pad = &vd56g3_pad_ops,
1062};
1063
1064static const struct media_entity_operations vd56g3_subdev_entity_ops = {
1065	.link_validate = v4l2_subdev_link_validate,
1066};
1067
1068static const struct v4l2_subdev_internal_ops vd56g3_internal_ops = {
1069	.init_state = vd56g3_init_state,
1070};
1071
1072/* -----------------------------------------------------------------------------
1073 * Power management
1074 */
1075
1076static int vd56g3_power_on(struct device *dev)
1077{
1078	struct v4l2_subdev *sd = dev_get_drvdata(dev);
1079	struct vd56g3 *sensor = to_vd56g3(sd);
1080	int ret;
1081
1082	/* power on */
1083	ret = regulator_bulk_enable(ARRAY_SIZE(sensor->supplies),
1084				    sensor->supplies);
1085	if (ret) {
1086		dev_err(dev, "Failed to enable regulators: %d\n", ret);
1087		return ret;
1088	}
1089
1090	ret = clk_prepare_enable(sensor->xclk);
1091	if (ret) {
1092		dev_err(dev, "Failed to enable clock: %d\n", ret);
1093		goto disable_reg;
1094	}
1095
1096	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
1097	usleep_range(3500, 4000);
1098	ret = vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_READY_TO_BOOT, NULL);
1099	if (ret) {
1100		dev_err(dev, "Sensor reset failed: %d\n", ret);
1101		goto disable_clock;
1102	}
1103
1104	/* boot sensor */
1105	cci_write(sensor->regmap, VD56G3_REG_BOOT, VD56G3_CMD_BOOT, &ret);
1106	vd56g3_poll_reg(sensor, VD56G3_REG_BOOT, VD56G3_CMD_ACK, &ret);
1107	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
1108	if (ret) {
1109		dev_err(dev, "Sensor boot failed: %d\n", ret);
1110		goto disable_clock;
1111	}
1112
1113	return 0;
1114
1115disable_clock:
1116	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
1117	clk_disable_unprepare(sensor->xclk);
1118disable_reg:
1119	regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies);
1120
1121	return ret;
1122}
1123
1124static int vd56g3_power_off(struct device *dev)
1125{
1126	struct v4l2_subdev *sd = dev_get_drvdata(dev);
1127	struct vd56g3 *sensor = to_vd56g3(sd);
1128
1129	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
1130	clk_disable_unprepare(sensor->xclk);
1131	regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies);
1132
1133	return 0;
1134}
1135
1136static const struct dev_pm_ops vd56g3_pm_ops = {
1137	SET_RUNTIME_PM_OPS(vd56g3_power_off, vd56g3_power_on, NULL)
1138};
1139
1140/* -----------------------------------------------------------------------------
1141 * Probe and initialization
1142 */
1143
1144static int vd56g3_check_csi_conf(struct vd56g3 *sensor,
1145				 struct fwnode_handle *endpoint)
1146{
1147	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
1148	u32 phy_data_lanes[VD56G3_MAX_CSI_DATA_LANES] = { ~0, ~0 };
1149	u8 n_lanes;
1150	u64 frequency;
1151	int p, l;
1152	int ret = 0;
1153
1154	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
1155	if (ret)
1156		return -EINVAL;
1157
1158	/* Check lanes number */
1159	n_lanes = ep.bus.mipi_csi2.num_data_lanes;
1160	if (n_lanes != 1 && n_lanes != 2) {
1161		dev_err(sensor->dev, "Invalid data lane number: %d\n", n_lanes);
1162		ret = -EINVAL;
1163		goto done;
1164	}
1165	sensor->nb_of_lane = n_lanes;
1166
1167	/* Clock lane must be first */
1168	if (ep.bus.mipi_csi2.clock_lane != 0) {
1169		dev_err(sensor->dev, "Clock lane must be mapped to lane 0\n");
1170		ret = -EINVAL;
1171		goto done;
1172	}
1173
1174	/*
1175	 * Prepare Output Interface conf based on lane settings
1176	 * logical to physical lane conversion (+ pad remaining slots)
1177	 */
1178	for (l = 0; l < n_lanes; l++)
1179		phy_data_lanes[ep.bus.mipi_csi2.data_lanes[l] - 1] = l;
1180	for (p = 0; p < VD56G3_MAX_CSI_DATA_LANES; p++) {
1181		if (phy_data_lanes[p] != ~0)
1182			continue;
1183		phy_data_lanes[p] = l;
1184		l++;
1185	}
1186	sensor->oif_ctrl = n_lanes |
1187			   (ep.bus.mipi_csi2.lane_polarities[0] << 3) |
1188			   ((phy_data_lanes[0]) << 4) |
1189			   (ep.bus.mipi_csi2.lane_polarities[1] << 6) |
1190			   ((phy_data_lanes[1]) << 7) |
1191			   (ep.bus.mipi_csi2.lane_polarities[2] << 9);
1192
1193	/* Check link frequency */
1194	if (!ep.nr_of_link_frequencies) {
1195		dev_err(sensor->dev, "link-frequency not found in DT\n");
1196		ret = -EINVAL;
1197		goto done;
1198	}
1199	frequency = (n_lanes == 2) ? VD56G3_LINK_FREQ_DEF_2LANES :
1200				     VD56G3_LINK_FREQ_DEF_1LANE;
1201	if (ep.nr_of_link_frequencies != 1 ||
1202	    ep.link_frequencies[0] != frequency) {
1203		dev_err(sensor->dev, "Link frequency not supported: %lld\n",
1204			ep.link_frequencies[0]);
1205		ret = -EINVAL;
1206		goto done;
1207	}
1208
1209done:
1210	v4l2_fwnode_endpoint_free(&ep);
1211
1212	return ret;
1213}
1214
1215static int vd56g3_parse_dt_gpios_array(struct vd56g3 *sensor, char *prop_name,
1216				       u32 *array, unsigned int *nb)
1217{
1218	struct device *dev = sensor->dev;
1219	unsigned int i;
1220	int ret;
1221
1222	if (!device_property_present(dev, prop_name)) {
1223		*nb = 0;
1224		return 0;
1225	}
1226
1227	ret = device_property_count_u32(dev, prop_name);
1228	if (ret < 0) {
1229		dev_err(dev, "Failed to read %s count\n", prop_name);
1230		return ret;
1231	}
1232
1233	*nb = ret;
1234	ret = device_property_read_u32_array(dev, prop_name, array, *nb);
1235	if (ret) {
1236		dev_err(dev, "Failed to read %s prop\n", prop_name);
1237		return ret;
1238	}
1239
1240	for (i = 0; i < *nb; i++) {
1241		if (array[i] >= VD56G3_NB_GPIOS) {
1242			dev_err(dev, "Invalid GPIO: %d\n", array[i]);
1243			return -EINVAL;
1244		}
1245	}
1246
1247	return 0;
1248}
1249
1250static int vd56g3_parse_dt_gpios(struct vd56g3 *sensor)
1251{
1252	u32 led_gpios[VD56G3_NB_GPIOS];
1253	unsigned int nb_gpios_leds;
1254	unsigned int i;
1255	int ret;
1256
1257	/* Initialize GPIOs to default */
1258	for (i = 0; i < VD56G3_NB_GPIOS; i++)
1259		sensor->gpios[i] = VD56G3_GPIOX_GPIO_IN;
1260	sensor->ext_leds_mask = 0;
1261
1262	/* Take into account optional 'st,leds' output for GPIOs */
1263	ret = vd56g3_parse_dt_gpios_array(sensor, "st,leds", led_gpios,
1264					  &nb_gpios_leds);
1265	if (ret)
1266		return ret;
1267	for (i = 0; i < nb_gpios_leds; i++) {
1268		sensor->gpios[led_gpios[i]] = VD56G3_GPIOX_STROBE_MODE;
1269		set_bit(led_gpios[i], &sensor->ext_leds_mask);
1270	}
1271
1272	return 0;
1273}
1274
1275static int vd56g3_parse_dt(struct vd56g3 *sensor)
1276{
1277	struct fwnode_handle *endpoint;
1278	int ret;
1279
1280	endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(sensor->dev), 0,
1281						   0, 0);
1282	if (!endpoint) {
1283		dev_err(sensor->dev, "Endpoint node not found\n");
1284		return -EINVAL;
1285	}
1286
1287	ret = vd56g3_check_csi_conf(sensor, endpoint);
1288	fwnode_handle_put(endpoint);
1289	if (ret)
1290		return ret;
1291
1292	return vd56g3_parse_dt_gpios(sensor);
1293}
1294
1295static int vd56g3_get_regulators(struct vd56g3 *sensor)
1296{
1297	unsigned int i;
1298
1299	for (i = 0; i < ARRAY_SIZE(sensor->supplies); i++)
1300		sensor->supplies[i].supply = vd56g3_supply_names[i];
1301
1302	return devm_regulator_bulk_get(sensor->dev,
1303				       ARRAY_SIZE(sensor->supplies),
1304				       sensor->supplies);
1305}
1306
1307static int vd56g3_prepare_clock_tree(struct vd56g3 *sensor)
1308{
1309	const unsigned int predivs[] = { 1, 2, 4 };
1310	u32 pll_out;
1311	int i;
1312
1313	/* External clock must be in [6Mhz-27Mhz] */
1314	if (sensor->xclk_freq < VD56G3_XCLK_FREQ_MIN ||
1315	    sensor->xclk_freq > VD56G3_XCLK_FREQ_MAX) {
1316		dev_err(sensor->dev,
1317			"Only 6Mhz-27Mhz clock range supported. Provided %lu MHz\n",
1318			sensor->xclk_freq / HZ_PER_MHZ);
1319		return -EINVAL;
1320	}
1321
1322	/* PLL input should be in [6Mhz-12Mhz[ */
1323	for (i = 0; i < ARRAY_SIZE(predivs); i++) {
1324		sensor->pll_prediv = predivs[i];
1325		if (sensor->xclk_freq / sensor->pll_prediv < 12 * HZ_PER_MHZ)
1326			break;
1327	}
1328
1329	/* PLL output clock must be as close as possible to 804Mhz */
1330	sensor->pll_mult = (VD56G3_TARGET_PLL * sensor->pll_prediv +
1331			    sensor->xclk_freq / 2) /
1332			   sensor->xclk_freq;
1333	pll_out = sensor->xclk_freq * sensor->pll_mult / sensor->pll_prediv;
1334
1335	/* Target Pixel Clock for standard 10bit ADC mode : 160.8Mhz */
1336	sensor->pixel_clock = pll_out / VD56G3_VT_CLOCK_DIV;
1337
1338	return 0;
1339}
1340
1341static int vd56g3_detect(struct vd56g3 *sensor)
1342{
1343	struct device *dev = sensor->dev;
1344	unsigned int model;
1345	u64 model_id;
1346	u64 device_revision;
1347	u64 optical_revision;
1348	int ret = 0;
1349
1350	model = (uintptr_t)device_get_match_data(dev);
1351
1352	ret = cci_read(sensor->regmap, VD56G3_REG_MODEL_ID, &model_id, NULL);
1353	if (ret)
1354		return ret;
1355
1356	if (model_id != VD56G3_MODEL_ID) {
1357		dev_err(dev, "Unsupported sensor id: %x\n", (u16)model_id);
1358		return -ENODEV;
1359	}
1360
1361	ret = cci_read(sensor->regmap, VD56G3_REG_REVISION, &device_revision,
1362		       NULL);
1363	if (ret)
1364		return ret;
1365
1366	if ((device_revision >> 8) != VD56G3_REVISION_CUT3) {
1367		dev_err(dev, "Unsupported version: %x\n", (u16)device_revision);
1368		return -ENODEV;
1369	}
1370
1371	ret = cci_read(sensor->regmap, VD56G3_REG_OPTICAL_REVISION,
1372		       &optical_revision, NULL);
1373	if (ret)
1374		return ret;
1375
1376	sensor->is_mono =
1377		((optical_revision & 1) == VD56G3_OPTICAL_REVISION_MONO);
1378	if ((sensor->is_mono && model == VD56G3_MODEL_VD66GY) ||
1379	    (!sensor->is_mono && model == VD56G3_MODEL_VD56G3)) {
1380		dev_err(dev, "Found %s sensor, while %s model is defined in DT\n",
1381			(sensor->is_mono) ? "Mono" : "Bayer",
1382			(model == VD56G3_MODEL_VD56G3) ? "vd56g3" : "vd66gy");
1383		return -ENODEV;
1384	}
1385
1386	return 0;
1387}
1388
1389static int vd56g3_subdev_init(struct vd56g3 *sensor)
1390{
1391	struct v4l2_subdev_state *state;
1392	int ret;
1393
1394	/* Init remaining sub device ops */
1395	sensor->sd.internal_ops = &vd56g3_internal_ops;
1396	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1397	sensor->sd.entity.ops = &vd56g3_subdev_entity_ops;
1398
1399	/* Init source pad */
1400	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
1401	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1402	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
1403	if (ret) {
1404		dev_err(sensor->dev, "Failed to init media entity: %d\n", ret);
1405		return ret;
1406	}
1407
1408	/* Init controls */
1409	ret = vd56g3_init_controls(sensor);
1410	if (ret) {
1411		dev_err(sensor->dev, "Controls initialization failed: %d\n",
1412			ret);
1413		goto err_media;
1414	}
1415
1416	/* Init vd56g3 struct : default resolution + raw8 */
1417	sensor->sd.state_lock = sensor->ctrl_handler.lock;
1418	ret = v4l2_subdev_init_finalize(&sensor->sd);
1419	if (ret) {
1420		dev_err(sensor->dev, "Subdev init failed: %d\n", ret);
1421		goto err_ctrls;
1422	}
1423
1424	/* Update controls according to the resolution set */
1425	state = v4l2_subdev_lock_and_get_active_state(&sensor->sd);
1426	ret = vd56g3_update_controls(sensor);
1427	v4l2_subdev_unlock_state(state);
1428	if (ret) {
1429		dev_err(sensor->dev, "Controls update failed: %d\n", ret);
1430		goto err_ctrls;
1431	}
1432
1433	return 0;
1434
1435err_ctrls:
1436	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
1437
1438err_media:
1439	media_entity_cleanup(&sensor->sd.entity);
1440
1441	return ret;
1442}
1443
1444static void vd56g3_subdev_cleanup(struct vd56g3 *sensor)
1445{
1446	v4l2_async_unregister_subdev(&sensor->sd);
1447	v4l2_subdev_cleanup(&sensor->sd);
1448	media_entity_cleanup(&sensor->sd.entity);
1449	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
1450}
1451
1452static int vd56g3_probe(struct i2c_client *client)
1453{
1454	struct device *dev = &client->dev;
1455	struct vd56g3 *sensor;
1456	int ret;
1457
1458	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
1459	if (!sensor)
1460		return -ENOMEM;
1461
1462	v4l2_i2c_subdev_init(&sensor->sd, client, &vd56g3_subdev_ops);
1463	sensor->dev = dev;
1464
1465	ret = vd56g3_parse_dt(sensor);
1466	if (ret)
1467		return dev_err_probe(dev, ret, "Failed to parse Device Tree\n");
1468
1469	/* Get (and check) resources : power regs, ext clock, reset gpio */
1470	ret = vd56g3_get_regulators(sensor);
1471	if (ret)
1472		return dev_err_probe(dev, ret, "Failed to get regulators\n");
1473
1474	sensor->xclk = devm_v4l2_sensor_clk_get(dev, NULL);
1475	if (IS_ERR(sensor->xclk))
1476		return dev_err_probe(dev, PTR_ERR(sensor->xclk),
1477				     "Failed to get xclk\n");
1478	sensor->xclk_freq = clk_get_rate(sensor->xclk);
1479	ret = vd56g3_prepare_clock_tree(sensor);
1480	if (ret)
1481		return ret;
1482
1483	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
1484						     GPIOD_OUT_HIGH);
1485	if (IS_ERR(sensor->reset_gpio))
1486		return dev_err_probe(dev, PTR_ERR(sensor->reset_gpio),
1487				     "Failed to get reset gpio\n");
1488
1489	sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
1490	if (IS_ERR(sensor->regmap))
1491		return dev_err_probe(dev, PTR_ERR(sensor->regmap),
1492				     "Failed to init regmap\n");
1493
1494	/* Power ON */
1495	ret = vd56g3_power_on(dev);
1496	if (ret)
1497		return dev_err_probe(dev, ret, "Sensor power on failed\n");
1498
1499	/* Enable PM runtime with autosuspend (sensor being ON, set active) */
1500	pm_runtime_set_active(dev);
1501	pm_runtime_get_noresume(dev);
1502	pm_runtime_enable(dev);
1503	pm_runtime_set_autosuspend_delay(dev, 1000);
1504	pm_runtime_use_autosuspend(dev);
1505
1506	/* Check HW model/version */
1507	ret = vd56g3_detect(sensor);
1508	if (ret) {
1509		dev_err(dev, "Sensor detect failed: %d\n", ret);
1510		goto err_power_off;
1511	}
1512
1513	/* Initialize & register subdev (v4l2_i2c subdev already initialized) */
1514	ret = vd56g3_subdev_init(sensor);
1515	if (ret) {
1516		dev_err(dev, "V4l2 init failed: %d\n", ret);
1517		goto err_power_off;
1518	}
1519
1520	ret = v4l2_async_register_subdev(&sensor->sd);
1521	if (ret) {
1522		dev_err(dev, "Async subdev register failed: %d\n", ret);
1523		goto err_subdev;
1524	}
1525
1526	/* Sensor could now be powered off (after the autosuspend delay) */
1527	pm_runtime_put_autosuspend(dev);
1528
1529	dev_dbg(dev, "Successfully probe %s sensor\n",
1530		(sensor->is_mono) ? "vd56g3" : "vd66gy");
1531
1532	return 0;
1533
1534err_subdev:
1535	vd56g3_subdev_cleanup(sensor);
1536err_power_off:
1537	pm_runtime_disable(dev);
1538	pm_runtime_put_noidle(dev);
1539	pm_runtime_dont_use_autosuspend(dev);
1540	vd56g3_power_off(dev);
1541
1542	return ret;
1543}
1544
1545static void vd56g3_remove(struct i2c_client *client)
1546{
1547	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1548	struct vd56g3 *sensor = to_vd56g3(sd);
1549
1550	vd56g3_subdev_cleanup(sensor);
1551
1552	pm_runtime_disable(sensor->dev);
1553	if (!pm_runtime_status_suspended(sensor->dev))
1554		vd56g3_power_off(sensor->dev);
1555	pm_runtime_set_suspended(sensor->dev);
1556	pm_runtime_dont_use_autosuspend(sensor->dev);
1557}
1558
1559static const struct of_device_id vd56g3_dt_ids[] = {
1560	{ .compatible = "st,vd56g3", .data = (void *)VD56G3_MODEL_VD56G3 },
1561	{ .compatible = "st,vd66gy", .data = (void *)VD56G3_MODEL_VD66GY },
1562	{ /* sentinel */ }
1563};
1564MODULE_DEVICE_TABLE(of, vd56g3_dt_ids);
1565
1566static struct i2c_driver vd56g3_i2c_driver = {
1567	.driver = {
1568		.name  = "vd56g3",
1569		.of_match_table = vd56g3_dt_ids,
1570		.pm = &vd56g3_pm_ops,
1571	},
1572	.probe = vd56g3_probe,
1573	.remove = vd56g3_remove,
1574};
1575
1576module_i2c_driver(vd56g3_i2c_driver);
1577
1578MODULE_AUTHOR("Benjamin Mugnier <benjamin.mugnier@foss.st.com>");
1579MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>");
1580MODULE_AUTHOR("Sylvain Petinot <sylvain.petinot@foss.st.com>");
1581MODULE_DESCRIPTION("ST VD56G3 sensor driver");
1582MODULE_LICENSE("GPL");