File Index
Files and directories
Sign in
FILE

vgxy61.c

File preview with syntax highlighting.

linux-6.19.14/drivers/media/i2c/vgxy61.c C 
   1// SPDX-License-Identifier: GPL-2.0
   2/*
   3 * Driver for VGXY61 global shutter sensor family driver
   4 *
   5 * Copyright (C) 2022 STMicroelectronics SA
   6 */
   7
   8#include <linux/clk.h>
   9#include <linux/delay.h>
  10#include <linux/gpio/consumer.h>
  11#include <linux/i2c.h>
  12#include <linux/iopoll.h>
  13#include <linux/module.h>
  14#include <linux/pm_runtime.h>
  15#include <linux/regmap.h>
  16#include <linux/regulator/consumer.h>
  17#include <linux/units.h>
  18
  19#include <linux/unaligned.h>
  20
  21#include <media/mipi-csi2.h>
  22#include <media/v4l2-async.h>
  23#include <media/v4l2-cci.h>
  24#include <media/v4l2-ctrls.h>
  25#include <media/v4l2-device.h>
  26#include <media/v4l2-event.h>
  27#include <media/v4l2-fwnode.h>
  28#include <media/v4l2-subdev.h>
  29
  30#define VGXY61_REG_MODEL_ID				CCI_REG16_LE(0x0000)
  31#define VG5661_MODEL_ID					0x5661
  32#define VG5761_MODEL_ID					0x5761
  33#define VGXY61_REG_REVISION				CCI_REG16_LE(0x0002)
  34#define VGXY61_REG_FWPATCH_REVISION			CCI_REG16_LE(0x0014)
  35#define VGXY61_REG_FWPATCH_START_ADDR			CCI_REG8(0x2000)
  36#define VGXY61_REG_SYSTEM_FSM				CCI_REG8(0x0020)
  37#define VGXY61_SYSTEM_FSM_SW_STBY			0x03
  38#define VGXY61_SYSTEM_FSM_STREAMING			0x04
  39#define VGXY61_REG_NVM					CCI_REG8(0x0023)
  40#define VGXY61_NVM_OK					0x04
  41#define VGXY61_REG_STBY					CCI_REG8(0x0201)
  42#define VGXY61_STBY_NO_REQ				0
  43#define VGXY61_STBY_REQ_TMP_READ			BIT(2)
  44#define VGXY61_REG_STREAMING				CCI_REG8(0x0202)
  45#define VGXY61_STREAMING_NO_REQ				0
  46#define VGXY61_STREAMING_REQ_STOP			BIT(0)
  47#define VGXY61_STREAMING_REQ_START			BIT(1)
  48#define VGXY61_REG_EXT_CLOCK				CCI_REG32_LE(0x0220)
  49#define VGXY61_REG_CLK_PLL_PREDIV			CCI_REG8(0x0224)
  50#define VGXY61_REG_CLK_SYS_PLL_MULT			CCI_REG8(0x0225)
  51#define VGXY61_REG_GPIO_0_CTRL				CCI_REG8(0x0236)
  52#define VGXY61_REG_GPIO_1_CTRL				CCI_REG8(0x0237)
  53#define VGXY61_REG_GPIO_2_CTRL				CCI_REG8(0x0238)
  54#define VGXY61_REG_GPIO_3_CTRL				CCI_REG8(0x0239)
  55#define VGXY61_REG_SIGNALS_POLARITY_CTRL		CCI_REG8(0x023b)
  56#define VGXY61_REG_LINE_LENGTH				CCI_REG16_LE(0x0300)
  57#define VGXY61_REG_ORIENTATION				CCI_REG8(0x0302)
  58#define VGXY61_REG_VT_CTRL				CCI_REG8(0x0304)
  59#define VGXY61_REG_FORMAT_CTRL				CCI_REG8(0x0305)
  60#define VGXY61_REG_OIF_CTRL				CCI_REG16_LE(0x0306)
  61#define VGXY61_REG_OIF_ROI0_CTRL			CCI_REG8(0x030a)
  62#define VGXY61_REG_ROI0_START_H				CCI_REG16_LE(0x0400)
  63#define VGXY61_REG_ROI0_START_V				CCI_REG16_LE(0x0402)
  64#define VGXY61_REG_ROI0_END_H				CCI_REG16_LE(0x0404)
  65#define VGXY61_REG_ROI0_END_V				CCI_REG16_LE(0x0406)
  66#define VGXY61_REG_PATGEN_CTRL				CCI_REG32_LE(0x0440)
  67#define VGXY61_PATGEN_LONG_ENABLE			BIT(16)
  68#define VGXY61_PATGEN_SHORT_ENABLE			BIT(0)
  69#define VGXY61_PATGEN_LONG_TYPE_SHIFT			18
  70#define VGXY61_PATGEN_SHORT_TYPE_SHIFT			4
  71#define VGXY61_REG_FRAME_CONTENT_CTRL			CCI_REG8(0x0478)
  72#define VGXY61_REG_COARSE_EXPOSURE_LONG			CCI_REG16_LE(0x0500)
  73#define VGXY61_REG_COARSE_EXPOSURE_SHORT		CCI_REG16_LE(0x0504)
  74#define VGXY61_REG_ANALOG_GAIN				CCI_REG8(0x0508)
  75#define VGXY61_REG_DIGITAL_GAIN_LONG			CCI_REG16_LE(0x050a)
  76#define VGXY61_REG_DIGITAL_GAIN_SHORT			CCI_REG16_LE(0x0512)
  77#define VGXY61_REG_FRAME_LENGTH				CCI_REG16_LE(0x051a)
  78#define VGXY61_REG_SIGNALS_CTRL				CCI_REG16_LE(0x0522)
  79#define VGXY61_SIGNALS_GPIO_ID_SHIFT			4
  80#define VGXY61_REG_READOUT_CTRL				CCI_REG8(0x0530)
  81#define VGXY61_REG_HDR_CTRL				CCI_REG8(0x0532)
  82#define VGXY61_REG_PATGEN_LONG_DATA_GR			CCI_REG16_LE(0x092c)
  83#define VGXY61_REG_PATGEN_LONG_DATA_R			CCI_REG16_LE(0x092e)
  84#define VGXY61_REG_PATGEN_LONG_DATA_B			CCI_REG16_LE(0x0930)
  85#define VGXY61_REG_PATGEN_LONG_DATA_GB			CCI_REG16_LE(0x0932)
  86#define VGXY61_REG_PATGEN_SHORT_DATA_GR			CCI_REG16_LE(0x0950)
  87#define VGXY61_REG_PATGEN_SHORT_DATA_R			CCI_REG16_LE(0x0952)
  88#define VGXY61_REG_PATGEN_SHORT_DATA_B			CCI_REG16_LE(0x0954)
  89#define VGXY61_REG_PATGEN_SHORT_DATA_GB			CCI_REG16_LE(0x0956)
  90#define VGXY61_REG_BYPASS_CTRL				CCI_REG8(0x0a60)
  91
  92#define VGX661_WIDTH					1464
  93#define VGX661_HEIGHT					1104
  94#define VGX761_WIDTH					1944
  95#define VGX761_HEIGHT					1204
  96#define VGX661_DEFAULT_MODE				1
  97#define VGX761_DEFAULT_MODE				1
  98#define VGX661_SHORT_ROT_TERM				93
  99#define VGX761_SHORT_ROT_TERM				90
 100#define VGXY61_EXPOS_ROT_TERM				66
 101#define VGXY61_WRITE_MULTIPLE_CHUNK_MAX			16
 102#define VGXY61_NB_GPIOS					4
 103#define VGXY61_NB_POLARITIES				5
 104#define VGXY61_FRAME_LENGTH_DEF				1313
 105#define VGXY61_MIN_FRAME_LENGTH				1288
 106#define VGXY61_MIN_EXPOSURE				10
 107#define VGXY61_HDR_LINEAR_RATIO				10
 108#define VGXY61_TIMEOUT_MS				500
 109#define VGXY61_MEDIA_BUS_FMT_DEF			MEDIA_BUS_FMT_Y8_1X8
 110
 111#define VGXY61_FWPATCH_REVISION_MAJOR			2
 112#define VGXY61_FWPATCH_REVISION_MINOR			0
 113#define VGXY61_FWPATCH_REVISION_MICRO			5
 114
 115static const u8 patch_array[] = {
 116	0xbf, 0x00, 0x05, 0x20, 0x06, 0x01, 0xe0, 0xe0, 0x04, 0x80, 0xe6, 0x45,
 117	0xed, 0x6f, 0xfe, 0xff, 0x14, 0x80, 0x1f, 0x84, 0x10, 0x42, 0x05, 0x7c,
 118	0x01, 0xc4, 0x1e, 0x80, 0xb6, 0x42, 0x00, 0xe0, 0x1e, 0x82, 0x1e, 0xc0,
 119	0x93, 0xdd, 0xc3, 0xc1, 0x0c, 0x04, 0x00, 0xfa, 0x86, 0x0d, 0x70, 0xe1,
 120	0x04, 0x98, 0x15, 0x00, 0x28, 0xe0, 0x14, 0x02, 0x08, 0xfc, 0x15, 0x40,
 121	0x28, 0xe0, 0x98, 0x58, 0xe0, 0xef, 0x04, 0x98, 0x0e, 0x04, 0x00, 0xf0,
 122	0x15, 0x00, 0x28, 0xe0, 0x19, 0xc8, 0x15, 0x40, 0x28, 0xe0, 0xc6, 0x41,
 123	0xfc, 0xe0, 0x14, 0x80, 0x1f, 0x84, 0x14, 0x02, 0xa0, 0xfc, 0x1e, 0x80,
 124	0x14, 0x80, 0x14, 0x02, 0x80, 0xfb, 0x14, 0x02, 0xe0, 0xfc, 0x1e, 0x80,
 125	0x14, 0xc0, 0x1f, 0x84, 0x14, 0x02, 0xa4, 0xfc, 0x1e, 0xc0, 0x14, 0xc0,
 126	0x14, 0x02, 0x80, 0xfb, 0x14, 0x02, 0xe4, 0xfc, 0x1e, 0xc0, 0x0c, 0x0c,
 127	0x00, 0xf2, 0x93, 0xdd, 0x86, 0x00, 0xf8, 0xe0, 0x04, 0x80, 0xc6, 0x03,
 128	0x70, 0xe1, 0x0e, 0x84, 0x93, 0xdd, 0xc3, 0xc1, 0x0c, 0x04, 0x00, 0xfa,
 129	0x6b, 0x80, 0x06, 0x40, 0x6c, 0xe1, 0x04, 0x80, 0x09, 0x00, 0xe0, 0xe0,
 130	0x0b, 0xa1, 0x95, 0x84, 0x05, 0x0c, 0x1c, 0xe0, 0x86, 0x02, 0xf9, 0x60,
 131	0xe0, 0xcf, 0x78, 0x6e, 0x80, 0xef, 0x25, 0x0c, 0x18, 0xe0, 0x05, 0x4c,
 132	0x1c, 0xe0, 0x86, 0x02, 0xf9, 0x60, 0xe0, 0xcf, 0x0b, 0x84, 0xd8, 0x6d,
 133	0x80, 0xef, 0x05, 0x4c, 0x18, 0xe0, 0x04, 0xd8, 0x0b, 0xa5, 0x95, 0x84,
 134	0x05, 0x0c, 0x2c, 0xe0, 0x06, 0x02, 0x01, 0x60, 0xe0, 0xce, 0x18, 0x6d,
 135	0x80, 0xef, 0x25, 0x0c, 0x30, 0xe0, 0x05, 0x4c, 0x2c, 0xe0, 0x06, 0x02,
 136	0x01, 0x60, 0xe0, 0xce, 0x0b, 0x84, 0x78, 0x6c, 0x80, 0xef, 0x05, 0x4c,
 137	0x30, 0xe0, 0x0c, 0x0c, 0x00, 0xf2, 0x93, 0xdd, 0x46, 0x01, 0x70, 0xe1,
 138	0x08, 0x80, 0x0b, 0xa1, 0x08, 0x5c, 0x00, 0xda, 0x06, 0x01, 0x68, 0xe1,
 139	0x04, 0x80, 0x4a, 0x40, 0x84, 0xe0, 0x08, 0x5c, 0x00, 0x9a, 0x06, 0x01,
 140	0xe0, 0xe0, 0x04, 0x80, 0x15, 0x00, 0x60, 0xe0, 0x19, 0xc4, 0x15, 0x40,
 141	0x60, 0xe0, 0x15, 0x00, 0x78, 0xe0, 0x19, 0xc4, 0x15, 0x40, 0x78, 0xe0,
 142	0x93, 0xdd, 0xc3, 0xc1, 0x46, 0x01, 0x70, 0xe1, 0x08, 0x80, 0x0b, 0xa1,
 143	0x08, 0x5c, 0x00, 0xda, 0x06, 0x01, 0x68, 0xe1, 0x04, 0x80, 0x4a, 0x40,
 144	0x84, 0xe0, 0x08, 0x5c, 0x00, 0x9a, 0x06, 0x01, 0xe0, 0xe0, 0x14, 0x80,
 145	0x25, 0x02, 0x54, 0xe0, 0x29, 0xc4, 0x25, 0x42, 0x54, 0xe0, 0x24, 0x80,
 146	0x35, 0x04, 0x6c, 0xe0, 0x39, 0xc4, 0x35, 0x44, 0x6c, 0xe0, 0x25, 0x02,
 147	0x64, 0xe0, 0x29, 0xc4, 0x25, 0x42, 0x64, 0xe0, 0x04, 0x80, 0x15, 0x00,
 148	0x7c, 0xe0, 0x19, 0xc4, 0x15, 0x40, 0x7c, 0xe0, 0x93, 0xdd, 0xc3, 0xc1,
 149	0x4c, 0x04, 0x7c, 0xfa, 0x86, 0x40, 0x98, 0xe0, 0x14, 0x80, 0x1b, 0xa1,
 150	0x06, 0x00, 0x00, 0xc0, 0x08, 0x42, 0x38, 0xdc, 0x08, 0x64, 0xa0, 0xef,
 151	0x86, 0x42, 0x3c, 0xe0, 0x68, 0x49, 0x80, 0xef, 0x6b, 0x80, 0x78, 0x53,
 152	0xc8, 0xef, 0xc6, 0x54, 0x6c, 0xe1, 0x7b, 0x80, 0xb5, 0x14, 0x0c, 0xf8,
 153	0x05, 0x14, 0x14, 0xf8, 0x1a, 0xac, 0x8a, 0x80, 0x0b, 0x90, 0x38, 0x55,
 154	0x80, 0xef, 0x1a, 0xae, 0x17, 0xc2, 0x03, 0x82, 0x88, 0x65, 0x80, 0xef,
 155	0x1b, 0x80, 0x0b, 0x8e, 0x68, 0x65, 0x80, 0xef, 0x9b, 0x80, 0x0b, 0x8c,
 156	0x08, 0x65, 0x80, 0xef, 0x6b, 0x80, 0x0b, 0x92, 0x1b, 0x8c, 0x98, 0x64,
 157	0x80, 0xef, 0x1a, 0xec, 0x9b, 0x80, 0x0b, 0x90, 0x95, 0x54, 0x10, 0xe0,
 158	0xa8, 0x53, 0x80, 0xef, 0x1a, 0xee, 0x17, 0xc2, 0x03, 0x82, 0xf8, 0x63,
 159	0x80, 0xef, 0x1b, 0x80, 0x0b, 0x8e, 0xd8, 0x63, 0x80, 0xef, 0x1b, 0x8c,
 160	0x68, 0x63, 0x80, 0xef, 0x6b, 0x80, 0x0b, 0x92, 0x65, 0x54, 0x14, 0xe0,
 161	0x08, 0x65, 0x84, 0xef, 0x68, 0x63, 0x80, 0xef, 0x7b, 0x80, 0x0b, 0x8c,
 162	0xa8, 0x64, 0x84, 0xef, 0x08, 0x63, 0x80, 0xef, 0x14, 0xe8, 0x46, 0x44,
 163	0x94, 0xe1, 0x24, 0x88, 0x4a, 0x4e, 0x04, 0xe0, 0x14, 0xea, 0x1a, 0x04,
 164	0x08, 0xe0, 0x0a, 0x40, 0x84, 0xed, 0x0c, 0x04, 0x00, 0xe2, 0x4a, 0x40,
 165	0x04, 0xe0, 0x19, 0x16, 0xc0, 0xe0, 0x0a, 0x40, 0x84, 0xed, 0x21, 0x54,
 166	0x60, 0xe0, 0x0c, 0x04, 0x00, 0xe2, 0x1b, 0xa5, 0x0e, 0xea, 0x01, 0x89,
 167	0x21, 0x54, 0x64, 0xe0, 0x7e, 0xe8, 0x65, 0x82, 0x1b, 0xa7, 0x26, 0x00,
 168	0x00, 0x80, 0xa5, 0x82, 0x1b, 0xa9, 0x65, 0x82, 0x1b, 0xa3, 0x01, 0x85,
 169	0x16, 0x00, 0x00, 0xc0, 0x01, 0x54, 0x04, 0xf8, 0x06, 0xaa, 0x01, 0x83,
 170	0x06, 0xa8, 0x65, 0x81, 0x06, 0xa8, 0x01, 0x54, 0x04, 0xf8, 0x01, 0x83,
 171	0x06, 0xaa, 0x09, 0x14, 0x18, 0xf8, 0x0b, 0xa1, 0x05, 0x84, 0xc6, 0x42,
 172	0xd4, 0xe0, 0x14, 0x84, 0x01, 0x83, 0x01, 0x54, 0x60, 0xe0, 0x01, 0x54,
 173	0x64, 0xe0, 0x0b, 0x02, 0x90, 0xe0, 0x10, 0x02, 0x90, 0xe5, 0x01, 0x54,
 174	0x88, 0xe0, 0xb5, 0x81, 0xc6, 0x40, 0xd4, 0xe0, 0x14, 0x80, 0x0b, 0x02,
 175	0xe0, 0xe4, 0x10, 0x02, 0x31, 0x66, 0x02, 0xc0, 0x01, 0x54, 0x88, 0xe0,
 176	0x1a, 0x84, 0x29, 0x14, 0x10, 0xe0, 0x1c, 0xaa, 0x2b, 0xa1, 0xf5, 0x82,
 177	0x25, 0x14, 0x10, 0xf8, 0x2b, 0x04, 0xa8, 0xe0, 0x20, 0x44, 0x0d, 0x70,
 178	0x03, 0xc0, 0x2b, 0xa1, 0x04, 0x00, 0x80, 0x9a, 0x02, 0x40, 0x84, 0x90,
 179	0x03, 0x54, 0x04, 0x80, 0x4c, 0x0c, 0x7c, 0xf2, 0x93, 0xdd, 0x00, 0x00,
 180	0x02, 0xa9, 0x00, 0x00, 0x64, 0x4a, 0x40, 0x00, 0x08, 0x2d, 0x58, 0xe0,
 181	0xa8, 0x98, 0x40, 0x00, 0x28, 0x07, 0x34, 0xe0, 0x05, 0xb9, 0x00, 0x00,
 182	0x28, 0x00, 0x41, 0x05, 0x88, 0x00, 0x41, 0x3c, 0x98, 0x00, 0x41, 0x52,
 183	0x04, 0x01, 0x41, 0x79, 0x3c, 0x01, 0x41, 0x6a, 0x3d, 0xfe, 0x00, 0x00,
 184};
 185
 186static const char * const vgxy61_test_pattern_menu[] = {
 187	"Disabled",
 188	"Solid",
 189	"Colorbar",
 190	"Gradbar",
 191	"Hgrey",
 192	"Vgrey",
 193	"Dgrey",
 194	"PN28",
 195};
 196
 197static const char * const vgxy61_hdr_mode_menu[] = {
 198	"HDR linearize",
 199	"HDR substraction",
 200	"No HDR",
 201};
 202
 203static const char * const vgxy61_supply_name[] = {
 204	"VCORE",
 205	"VDDIO",
 206	"VANA",
 207};
 208
 209static const s64 link_freq[] = {
 210	/*
 211	 * MIPI output freq is 804Mhz / 2, as it uses both rising edge and
 212	 * falling edges to send data
 213	 */
 214	402000000ULL
 215};
 216
 217enum vgxy61_bin_mode {
 218	VGXY61_BIN_MODE_NORMAL,
 219	VGXY61_BIN_MODE_DIGITAL_X2,
 220	VGXY61_BIN_MODE_DIGITAL_X4,
 221};
 222
 223enum vgxy61_hdr_mode {
 224	VGXY61_HDR_LINEAR,
 225	VGXY61_HDR_SUB,
 226	VGXY61_NO_HDR,
 227};
 228
 229enum vgxy61_strobe_mode {
 230	VGXY61_STROBE_DISABLED,
 231	VGXY61_STROBE_LONG,
 232	VGXY61_STROBE_ENABLED,
 233};
 234
 235struct vgxy61_mode_info {
 236	u32 width;
 237	u32 height;
 238	enum vgxy61_bin_mode bin_mode;
 239	struct v4l2_rect crop;
 240};
 241
 242struct vgxy61_fmt_desc {
 243	u32 code;
 244	u8 bpp;
 245	u8 data_type;
 246};
 247
 248static const struct vgxy61_fmt_desc vgxy61_supported_codes[] = {
 249	{
 250		.code = MEDIA_BUS_FMT_Y8_1X8,
 251		.bpp = 8,
 252		.data_type = MIPI_CSI2_DT_RAW8,
 253	},
 254	{
 255		.code = MEDIA_BUS_FMT_Y10_1X10,
 256		.bpp = 10,
 257		.data_type = MIPI_CSI2_DT_RAW10,
 258	},
 259	{
 260		.code = MEDIA_BUS_FMT_Y12_1X12,
 261		.bpp = 12,
 262		.data_type = MIPI_CSI2_DT_RAW12,
 263	},
 264	{
 265		.code = MEDIA_BUS_FMT_Y14_1X14,
 266		.bpp = 14,
 267		.data_type = MIPI_CSI2_DT_RAW14,
 268	},
 269	{
 270		.code = MEDIA_BUS_FMT_Y16_1X16,
 271		.bpp = 16,
 272		.data_type = MIPI_CSI2_DT_RAW16,
 273	},
 274};
 275
 276static const struct vgxy61_mode_info vgx661_mode_data[] = {
 277	{
 278		.width = VGX661_WIDTH,
 279		.height = VGX661_HEIGHT,
 280		.bin_mode = VGXY61_BIN_MODE_NORMAL,
 281		.crop = {
 282			.left = 0,
 283			.top = 0,
 284			.width = VGX661_WIDTH,
 285			.height = VGX661_HEIGHT,
 286		},
 287	},
 288	{
 289		.width = 1280,
 290		.height = 720,
 291		.bin_mode = VGXY61_BIN_MODE_NORMAL,
 292		.crop = {
 293			.left = 92,
 294			.top = 192,
 295			.width = 1280,
 296			.height = 720,
 297		},
 298	},
 299	{
 300		.width = 640,
 301		.height = 480,
 302		.bin_mode = VGXY61_BIN_MODE_DIGITAL_X2,
 303		.crop = {
 304			.left = 92,
 305			.top = 72,
 306			.width = 1280,
 307			.height = 960,
 308		},
 309	},
 310	{
 311		.width = 320,
 312		.height = 240,
 313		.bin_mode = VGXY61_BIN_MODE_DIGITAL_X4,
 314		.crop = {
 315			.left = 92,
 316			.top = 72,
 317			.width = 1280,
 318			.height = 960,
 319		},
 320	},
 321};
 322
 323static const struct vgxy61_mode_info vgx761_mode_data[] = {
 324	{
 325		.width = VGX761_WIDTH,
 326		.height = VGX761_HEIGHT,
 327		.bin_mode = VGXY61_BIN_MODE_NORMAL,
 328		.crop = {
 329			.left = 0,
 330			.top = 0,
 331			.width = VGX761_WIDTH,
 332			.height = VGX761_HEIGHT,
 333		},
 334	},
 335	{
 336		.width = 1920,
 337		.height = 1080,
 338		.bin_mode = VGXY61_BIN_MODE_NORMAL,
 339		.crop = {
 340			.left = 12,
 341			.top = 62,
 342			.width = 1920,
 343			.height = 1080,
 344		},
 345	},
 346	{
 347		.width = 1280,
 348		.height = 720,
 349		.bin_mode = VGXY61_BIN_MODE_NORMAL,
 350		.crop = {
 351			.left = 332,
 352			.top = 242,
 353			.width = 1280,
 354			.height = 720,
 355		},
 356	},
 357	{
 358		.width = 640,
 359		.height = 480,
 360		.bin_mode = VGXY61_BIN_MODE_DIGITAL_X2,
 361		.crop = {
 362			.left = 332,
 363			.top = 122,
 364			.width = 1280,
 365			.height = 960,
 366		},
 367	},
 368	{
 369		.width = 320,
 370		.height = 240,
 371		.bin_mode = VGXY61_BIN_MODE_DIGITAL_X4,
 372		.crop = {
 373			.left = 332,
 374			.top = 122,
 375			.width = 1280,
 376			.height = 960,
 377		},
 378	},
 379};
 380
 381struct vgxy61_dev {
 382	struct i2c_client *i2c_client;
 383	struct regmap *regmap;
 384	struct v4l2_subdev sd;
 385	struct media_pad pad;
 386	struct regulator_bulk_data supplies[ARRAY_SIZE(vgxy61_supply_name)];
 387	struct gpio_desc *reset_gpio;
 388	struct clk *xclk;
 389	u32 clk_freq;
 390	u16 id;
 391	u16 sensor_width;
 392	u16 sensor_height;
 393	u16 oif_ctrl;
 394	unsigned int nb_of_lane;
 395	u32 data_rate_in_mbps;
 396	u32 pclk;
 397	u16 line_length;
 398	u16 rot_term;
 399	bool gpios_polarity;
 400	/* Lock to protect all members below */
 401	struct mutex lock;
 402	struct v4l2_ctrl_handler ctrl_handler;
 403	struct v4l2_ctrl *pixel_rate_ctrl;
 404	struct v4l2_ctrl *expo_ctrl;
 405	struct v4l2_ctrl *vblank_ctrl;
 406	struct v4l2_ctrl *vflip_ctrl;
 407	struct v4l2_ctrl *hflip_ctrl;
 408	bool streaming;
 409	struct v4l2_mbus_framefmt fmt;
 410	const struct vgxy61_mode_info *sensor_modes;
 411	unsigned int sensor_modes_nb;
 412	const struct vgxy61_mode_info *default_mode;
 413	const struct vgxy61_mode_info *current_mode;
 414	bool hflip;
 415	bool vflip;
 416	enum vgxy61_hdr_mode hdr;
 417	u16 expo_long;
 418	u16 expo_short;
 419	u16 expo_max;
 420	u16 expo_min;
 421	u16 vblank;
 422	u16 vblank_min;
 423	u16 frame_length;
 424	u16 digital_gain;
 425	u8 analog_gain;
 426	enum vgxy61_strobe_mode strobe_mode;
 427	u32 pattern;
 428};
 429
 430static u8 get_bpp_by_code(__u32 code)
 431{
 432	unsigned int i;
 433
 434	for (i = 0; i < ARRAY_SIZE(vgxy61_supported_codes); i++) {
 435		if (vgxy61_supported_codes[i].code == code)
 436			return vgxy61_supported_codes[i].bpp;
 437	}
 438	/* Should never happen */
 439	WARN(1, "Unsupported code %d. default to 8 bpp", code);
 440	return 8;
 441}
 442
 443static u8 get_data_type_by_code(__u32 code)
 444{
 445	unsigned int i;
 446
 447	for (i = 0; i < ARRAY_SIZE(vgxy61_supported_codes); i++) {
 448		if (vgxy61_supported_codes[i].code == code)
 449			return vgxy61_supported_codes[i].data_type;
 450	}
 451	/* Should never happen */
 452	WARN(1, "Unsupported code %d. default to MIPI_CSI2_DT_RAW8 data type",
 453	     code);
 454	return MIPI_CSI2_DT_RAW8;
 455}
 456
 457static void compute_pll_parameters_by_freq(u32 freq, u8 *prediv, u8 *mult)
 458{
 459	const unsigned int predivs[] = {1, 2, 4};
 460	unsigned int i;
 461
 462	/*
 463	 * Freq range is [6Mhz-27Mhz] already checked.
 464	 * Output of divider should be in [6Mhz-12Mhz[.
 465	 */
 466	for (i = 0; i < ARRAY_SIZE(predivs); i++) {
 467		*prediv = predivs[i];
 468		if (freq / *prediv < 12 * HZ_PER_MHZ)
 469			break;
 470	}
 471	WARN_ON(i == ARRAY_SIZE(predivs));
 472
 473	/*
 474	 * Target freq is 804Mhz. Don't change this as it will impact image
 475	 * quality.
 476	 */
 477	*mult = ((804 * HZ_PER_MHZ) * (*prediv) + freq / 2) / freq;
 478}
 479
 480static s32 get_pixel_rate(struct vgxy61_dev *sensor)
 481{
 482	return div64_u64((u64)sensor->data_rate_in_mbps * sensor->nb_of_lane,
 483			 get_bpp_by_code(sensor->fmt.code));
 484}
 485
 486static inline struct vgxy61_dev *to_vgxy61_dev(struct v4l2_subdev *sd)
 487{
 488	return container_of(sd, struct vgxy61_dev, sd);
 489}
 490
 491static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
 492{
 493	return &container_of(ctrl->handler, struct vgxy61_dev,
 494			     ctrl_handler)->sd;
 495}
 496
 497static unsigned int get_chunk_size(struct vgxy61_dev *sensor)
 498{
 499	struct i2c_adapter *adapter = sensor->i2c_client->adapter;
 500	int max_write_len = VGXY61_WRITE_MULTIPLE_CHUNK_MAX;
 501
 502	if (adapter->quirks && adapter->quirks->max_write_len)
 503		max_write_len = adapter->quirks->max_write_len - 2;
 504
 505	max_write_len = min(max_write_len, VGXY61_WRITE_MULTIPLE_CHUNK_MAX);
 506
 507	return max(max_write_len, 1);
 508}
 509
 510static int vgxy61_write_array(struct vgxy61_dev *sensor, u32 reg,
 511			      unsigned int nb, const u8 *array)
 512{
 513	const unsigned int chunk_size = get_chunk_size(sensor);
 514	int ret;
 515	unsigned int sz;
 516
 517	while (nb) {
 518		sz = min(nb, chunk_size);
 519		ret = regmap_bulk_write(sensor->regmap, CCI_REG_ADDR(reg),
 520					array, sz);
 521		if (ret < 0)
 522			return ret;
 523		nb -= sz;
 524		reg += sz;
 525		array += sz;
 526	}
 527
 528	return 0;
 529}
 530
 531static int vgxy61_poll_reg(struct vgxy61_dev *sensor, u32 reg, u8 poll_val,
 532			   unsigned int timeout_ms)
 533{
 534	const unsigned int loop_delay_ms = 10;
 535	u64 val;
 536	int ret;
 537
 538	return read_poll_timeout(cci_read, ret,
 539				 ((ret < 0) || (val == poll_val)),
 540				 loop_delay_ms * 1000, timeout_ms * 1000,
 541				 false, sensor->regmap, reg, &val, NULL);
 542}
 543
 544static int vgxy61_wait_state(struct vgxy61_dev *sensor, int state,
 545			     unsigned int timeout_ms)
 546{
 547	return vgxy61_poll_reg(sensor, VGXY61_REG_SYSTEM_FSM, state,
 548			       timeout_ms);
 549}
 550
 551static int vgxy61_check_bw(struct vgxy61_dev *sensor)
 552{
 553	/*
 554	 * Simplification of time needed to send short packets and for the MIPI
 555	 * to add transition times (EoT, LPS, and SoT packet delimiters) needed
 556	 * by the protocol to go in low power between 2 packets of data. This
 557	 * is a mipi IP constant for the sensor.
 558	 */
 559	const unsigned int mipi_margin = 1056;
 560	unsigned int binning_scale = sensor->current_mode->crop.height /
 561				     sensor->current_mode->height;
 562	u8 bpp = get_bpp_by_code(sensor->fmt.code);
 563	unsigned int max_bit_per_line;
 564	unsigned int bit_per_line;
 565	u64 line_rate;
 566
 567	line_rate = sensor->nb_of_lane * (u64)sensor->data_rate_in_mbps *
 568		    sensor->line_length;
 569	max_bit_per_line = div64_u64(line_rate, sensor->pclk) - mipi_margin;
 570	bit_per_line = (bpp * sensor->current_mode->width) / binning_scale;
 571
 572	return bit_per_line > max_bit_per_line ? -EINVAL : 0;
 573}
 574
 575static int vgxy61_apply_exposure(struct vgxy61_dev *sensor)
 576{
 577	int ret = 0;
 578
 579	 /* We first set expo to zero to avoid forbidden parameters couple */
 580	cci_write(sensor->regmap, VGXY61_REG_COARSE_EXPOSURE_SHORT, 0, &ret);
 581	cci_write(sensor->regmap, VGXY61_REG_COARSE_EXPOSURE_LONG,
 582		  sensor->expo_long, &ret);
 583	cci_write(sensor->regmap, VGXY61_REG_COARSE_EXPOSURE_SHORT,
 584		  sensor->expo_short, &ret);
 585
 586	return ret;
 587}
 588
 589static int vgxy61_get_regulators(struct vgxy61_dev *sensor)
 590{
 591	unsigned int i;
 592
 593	for (i = 0; i < ARRAY_SIZE(vgxy61_supply_name); i++)
 594		sensor->supplies[i].supply = vgxy61_supply_name[i];
 595
 596	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
 597				       ARRAY_SIZE(vgxy61_supply_name),
 598				       sensor->supplies);
 599}
 600
 601static int vgxy61_apply_reset(struct vgxy61_dev *sensor)
 602{
 603	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
 604	usleep_range(5000, 10000);
 605	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
 606	usleep_range(5000, 10000);
 607	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
 608	usleep_range(40000, 100000);
 609	return vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_SW_STBY,
 610				 VGXY61_TIMEOUT_MS);
 611}
 612
 613static void vgxy61_fill_framefmt(struct vgxy61_dev *sensor,
 614				 const struct vgxy61_mode_info *mode,
 615				 struct v4l2_mbus_framefmt *fmt, u32 code)
 616{
 617	fmt->code = code;
 618	fmt->width = mode->width;
 619	fmt->height = mode->height;
 620	fmt->colorspace = V4L2_COLORSPACE_RAW;
 621	fmt->field = V4L2_FIELD_NONE;
 622	fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
 623	fmt->quantization = V4L2_QUANTIZATION_DEFAULT;
 624	fmt->xfer_func = V4L2_XFER_FUNC_DEFAULT;
 625}
 626
 627static int vgxy61_try_fmt_internal(struct v4l2_subdev *sd,
 628				   struct v4l2_mbus_framefmt *fmt,
 629				   const struct vgxy61_mode_info **new_mode)
 630{
 631	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
 632	const struct vgxy61_mode_info *mode;
 633	unsigned int index;
 634
 635	for (index = 0; index < ARRAY_SIZE(vgxy61_supported_codes); index++) {
 636		if (vgxy61_supported_codes[index].code == fmt->code)
 637			break;
 638	}
 639	if (index == ARRAY_SIZE(vgxy61_supported_codes))
 640		index = 0;
 641
 642	mode = v4l2_find_nearest_size(sensor->sensor_modes,
 643				      sensor->sensor_modes_nb, width, height,
 644				      fmt->width, fmt->height);
 645	if (new_mode)
 646		*new_mode = mode;
 647
 648	vgxy61_fill_framefmt(sensor, mode, fmt,
 649			     vgxy61_supported_codes[index].code);
 650
 651	return 0;
 652}
 653
 654static int vgxy61_get_selection(struct v4l2_subdev *sd,
 655				struct v4l2_subdev_state *sd_state,
 656				struct v4l2_subdev_selection *sel)
 657{
 658	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
 659
 660	switch (sel->target) {
 661	case V4L2_SEL_TGT_CROP:
 662		sel->r = sensor->current_mode->crop;
 663		return 0;
 664	case V4L2_SEL_TGT_NATIVE_SIZE:
 665	case V4L2_SEL_TGT_CROP_DEFAULT:
 666	case V4L2_SEL_TGT_CROP_BOUNDS:
 667		sel->r.top = 0;
 668		sel->r.left = 0;
 669		sel->r.width = sensor->sensor_width;
 670		sel->r.height = sensor->sensor_height;
 671		return 0;
 672	}
 673
 674	return -EINVAL;
 675}
 676
 677static int vgxy61_enum_mbus_code(struct v4l2_subdev *sd,
 678				 struct v4l2_subdev_state *sd_state,
 679				 struct v4l2_subdev_mbus_code_enum *code)
 680{
 681	if (code->index >= ARRAY_SIZE(vgxy61_supported_codes))
 682		return -EINVAL;
 683
 684	code->code = vgxy61_supported_codes[code->index].code;
 685
 686	return 0;
 687}
 688
 689static int vgxy61_get_fmt(struct v4l2_subdev *sd,
 690			  struct v4l2_subdev_state *sd_state,
 691			  struct v4l2_subdev_format *format)
 692{
 693	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
 694	struct v4l2_mbus_framefmt *fmt;
 695
 696	mutex_lock(&sensor->lock);
 697
 698	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
 699		fmt = v4l2_subdev_state_get_format(sd_state, format->pad);
 700	else
 701		fmt = &sensor->fmt;
 702
 703	format->format = *fmt;
 704
 705	mutex_unlock(&sensor->lock);
 706
 707	return 0;
 708}
 709
 710static u16 vgxy61_get_vblank_min(struct vgxy61_dev *sensor,
 711				 enum vgxy61_hdr_mode hdr)
 712{
 713	u16 min_vblank =  VGXY61_MIN_FRAME_LENGTH -
 714			  sensor->current_mode->crop.height;
 715	/* Ensure the first rule of thumb can't be negative */
 716	u16 min_vblank_hdr =  VGXY61_MIN_EXPOSURE + sensor->rot_term + 1;
 717
 718	if (hdr != VGXY61_NO_HDR)
 719		return max(min_vblank, min_vblank_hdr);
 720	return min_vblank;
 721}
 722
 723static int vgxy61_enum_frame_size(struct v4l2_subdev *sd,
 724				  struct v4l2_subdev_state *sd_state,
 725				  struct v4l2_subdev_frame_size_enum *fse)
 726{
 727	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
 728
 729	if (fse->index >= sensor->sensor_modes_nb)
 730		return -EINVAL;
 731
 732	fse->min_width = sensor->sensor_modes[fse->index].width;
 733	fse->max_width = fse->min_width;
 734	fse->min_height = sensor->sensor_modes[fse->index].height;
 735	fse->max_height = fse->min_height;
 736
 737	return 0;
 738}
 739
 740static int vgxy61_update_analog_gain(struct vgxy61_dev *sensor, u32 target)
 741{
 742	sensor->analog_gain = target;
 743
 744	if (sensor->streaming)
 745		return cci_write(sensor->regmap, VGXY61_REG_ANALOG_GAIN, target,
 746				 NULL);
 747	return 0;
 748}
 749
 750static int vgxy61_apply_digital_gain(struct vgxy61_dev *sensor,
 751				     u32 digital_gain)
 752{
 753	int ret = 0;
 754
 755	/*
 756	 * For a monochrome version, configuring DIGITAL_GAIN_LONG_CH0 and
 757	 * DIGITAL_GAIN_SHORT_CH0 is enough to configure the gain of all
 758	 * four sub pixels.
 759	 */
 760	cci_write(sensor->regmap, VGXY61_REG_DIGITAL_GAIN_LONG, digital_gain,
 761		  &ret);
 762	cci_write(sensor->regmap, VGXY61_REG_DIGITAL_GAIN_SHORT, digital_gain,
 763		  &ret);
 764
 765	return ret;
 766}
 767
 768static int vgxy61_update_digital_gain(struct vgxy61_dev *sensor, u32 target)
 769{
 770	sensor->digital_gain = target;
 771
 772	if (sensor->streaming)
 773		return vgxy61_apply_digital_gain(sensor, sensor->digital_gain);
 774	return 0;
 775}
 776
 777static int vgxy61_apply_patgen(struct vgxy61_dev *sensor, u32 index)
 778{
 779	static const u8 index2val[] = {
 780		0x0, 0x1, 0x2, 0x3, 0x10, 0x11, 0x12, 0x13
 781	};
 782	u32 pattern = index2val[index];
 783	u32 reg = (pattern << VGXY61_PATGEN_LONG_TYPE_SHIFT) |
 784	      (pattern << VGXY61_PATGEN_SHORT_TYPE_SHIFT);
 785
 786	if (pattern)
 787		reg |= VGXY61_PATGEN_LONG_ENABLE | VGXY61_PATGEN_SHORT_ENABLE;
 788	return cci_write(sensor->regmap, VGXY61_REG_PATGEN_CTRL, reg, NULL);
 789}
 790
 791static int vgxy61_update_patgen(struct vgxy61_dev *sensor, u32 pattern)
 792{
 793	sensor->pattern = pattern;
 794
 795	if (sensor->streaming)
 796		return vgxy61_apply_patgen(sensor, sensor->pattern);
 797	return 0;
 798}
 799
 800static int vgxy61_apply_gpiox_strobe_mode(struct vgxy61_dev *sensor,
 801					  enum vgxy61_strobe_mode mode,
 802					  unsigned int idx)
 803{
 804	static const u8 index2val[] = {0x0, 0x1, 0x3};
 805	u16 mask, val;
 806
 807	mask = 0xf << (idx * VGXY61_SIGNALS_GPIO_ID_SHIFT);
 808	val = index2val[mode] << (idx * VGXY61_SIGNALS_GPIO_ID_SHIFT);
 809
 810	return cci_update_bits(sensor->regmap, VGXY61_REG_SIGNALS_CTRL,
 811			       mask, val, NULL);
 812}
 813
 814static int vgxy61_update_gpios_strobe_mode(struct vgxy61_dev *sensor,
 815					   enum vgxy61_hdr_mode hdr)
 816{
 817	unsigned int i;
 818	int ret;
 819
 820	switch (hdr) {
 821	case VGXY61_HDR_LINEAR:
 822		sensor->strobe_mode = VGXY61_STROBE_ENABLED;
 823		break;
 824	case VGXY61_HDR_SUB:
 825	case VGXY61_NO_HDR:
 826		sensor->strobe_mode = VGXY61_STROBE_LONG;
 827		break;
 828	default:
 829		/* Should never happen */
 830		WARN_ON(true);
 831		break;
 832	}
 833
 834	if (!sensor->streaming)
 835		return 0;
 836
 837	for (i = 0; i < VGXY61_NB_GPIOS; i++) {
 838		ret = vgxy61_apply_gpiox_strobe_mode(sensor,
 839						     sensor->strobe_mode,
 840						     i);
 841		if (ret)
 842			return ret;
 843	}
 844
 845	return 0;
 846}
 847
 848static int vgxy61_update_gpios_strobe_polarity(struct vgxy61_dev *sensor,
 849					       bool polarity)
 850{
 851	int ret = 0;
 852
 853	if (sensor->streaming)
 854		return -EBUSY;
 855
 856	cci_write(sensor->regmap, VGXY61_REG_GPIO_0_CTRL, polarity << 1, &ret);
 857	cci_write(sensor->regmap, VGXY61_REG_GPIO_1_CTRL, polarity << 1, &ret);
 858	cci_write(sensor->regmap, VGXY61_REG_GPIO_2_CTRL, polarity << 1, &ret);
 859	cci_write(sensor->regmap, VGXY61_REG_GPIO_3_CTRL, polarity << 1, &ret);
 860	cci_write(sensor->regmap, VGXY61_REG_SIGNALS_POLARITY_CTRL, polarity,
 861		  &ret);
 862
 863	return ret;
 864}
 865
 866static u32 vgxy61_get_expo_long_max(struct vgxy61_dev *sensor,
 867				    unsigned int short_expo_ratio)
 868{
 869	u32 first_rot_max_expo, second_rot_max_expo, third_rot_max_expo;
 870
 871	/* Apply sensor's rules of thumb */
 872	/*
 873	 * Short exposure + height must be less than frame length to avoid bad
 874	 * pixel line at the botom of the image
 875	 */
 876	first_rot_max_expo =
 877		((sensor->frame_length - sensor->current_mode->crop.height -
 878		sensor->rot_term) * short_expo_ratio) - 1;
 879
 880	/*
 881	 * Total exposition time must be less than frame length to avoid sensor
 882	 * crash
 883	 */
 884	second_rot_max_expo =
 885		(((sensor->frame_length - VGXY61_EXPOS_ROT_TERM) *
 886		short_expo_ratio) / (short_expo_ratio + 1)) - 1;
 887
 888	/*
 889	 * Short exposure times 71 must be less than frame length to avoid
 890	 * sensor crash
 891	 */
 892	third_rot_max_expo = (sensor->frame_length / 71) * short_expo_ratio;
 893
 894	/* Take the minimum from all rules */
 895	return min3(first_rot_max_expo, second_rot_max_expo,
 896		    third_rot_max_expo);
 897}
 898
 899static int vgxy61_update_exposure(struct vgxy61_dev *sensor, u16 new_expo_long,
 900				  enum vgxy61_hdr_mode hdr)
 901{
 902	struct i2c_client *client = sensor->i2c_client;
 903	u16 new_expo_short = 0;
 904	u16 expo_short_max = 0;
 905	u16 expo_long_min = VGXY61_MIN_EXPOSURE;
 906	u16 expo_long_max = 0;
 907
 908	/* Compute short exposure according to hdr mode and long exposure */
 909	switch (hdr) {
 910	case VGXY61_HDR_LINEAR:
 911		/*
 912		 * Take ratio into account for minimal exposures in
 913		 * VGXY61_HDR_LINEAR
 914		 */
 915		expo_long_min = VGXY61_MIN_EXPOSURE * VGXY61_HDR_LINEAR_RATIO;
 916		new_expo_long = max(expo_long_min, new_expo_long);
 917
 918		expo_long_max =
 919			vgxy61_get_expo_long_max(sensor,
 920						 VGXY61_HDR_LINEAR_RATIO);
 921		expo_short_max = (expo_long_max +
 922				 (VGXY61_HDR_LINEAR_RATIO / 2)) /
 923				 VGXY61_HDR_LINEAR_RATIO;
 924		new_expo_short = (new_expo_long +
 925				 (VGXY61_HDR_LINEAR_RATIO / 2)) /
 926				 VGXY61_HDR_LINEAR_RATIO;
 927		break;
 928	case VGXY61_HDR_SUB:
 929		new_expo_long = max(expo_long_min, new_expo_long);
 930
 931		expo_long_max = vgxy61_get_expo_long_max(sensor, 1);
 932		/* Short and long are the same in VGXY61_HDR_SUB */
 933		expo_short_max = expo_long_max;
 934		new_expo_short = new_expo_long;
 935		break;
 936	case VGXY61_NO_HDR:
 937		new_expo_long = max(expo_long_min, new_expo_long);
 938
 939		/*
 940		 * As short expo is 0 here, only the second rule of thumb
 941		 * applies, see vgxy61_get_expo_long_max for more
 942		 */
 943		expo_long_max = sensor->frame_length - VGXY61_EXPOS_ROT_TERM;
 944		break;
 945	default:
 946		/* Should never happen */
 947		WARN_ON(true);
 948		break;
 949	}
 950
 951	/* If this happens, something is wrong with formulas */
 952	WARN_ON(expo_long_min > expo_long_max);
 953
 954	if (new_expo_long > expo_long_max) {
 955		dev_warn(&client->dev, "Exposure %d too high, clamping to %d\n",
 956			 new_expo_long, expo_long_max);
 957		new_expo_long = expo_long_max;
 958		new_expo_short = expo_short_max;
 959	}
 960
 961	sensor->expo_long = new_expo_long;
 962	sensor->expo_short = new_expo_short;
 963	sensor->expo_max = expo_long_max;
 964	sensor->expo_min = expo_long_min;
 965
 966	if (sensor->streaming)
 967		return vgxy61_apply_exposure(sensor);
 968	return 0;
 969}
 970
 971static int vgxy61_apply_framelength(struct vgxy61_dev *sensor)
 972{
 973	return cci_write(sensor->regmap, VGXY61_REG_FRAME_LENGTH,
 974			 sensor->frame_length, NULL);
 975}
 976
 977static int vgxy61_update_vblank(struct vgxy61_dev *sensor, u16 vblank,
 978				enum vgxy61_hdr_mode hdr)
 979{
 980	int ret;
 981
 982	sensor->vblank_min = vgxy61_get_vblank_min(sensor, hdr);
 983	sensor->vblank = max(sensor->vblank_min, vblank);
 984	sensor->frame_length = sensor->current_mode->crop.height +
 985			       sensor->vblank;
 986
 987	/* Update exposure according to vblank */
 988	ret = vgxy61_update_exposure(sensor, sensor->expo_long, hdr);
 989	if (ret)
 990		return ret;
 991
 992	if (sensor->streaming)
 993		return vgxy61_apply_framelength(sensor);
 994	return 0;
 995}
 996
 997static int vgxy61_apply_hdr(struct vgxy61_dev *sensor,
 998			    enum vgxy61_hdr_mode index)
 999{
1000	static const u8 index2val[] = {0x1, 0x4, 0xa};
1001
1002	return cci_write(sensor->regmap, VGXY61_REG_HDR_CTRL, index2val[index],
1003			 NULL);
1004}
1005
1006static int vgxy61_update_hdr(struct vgxy61_dev *sensor,
1007			     enum vgxy61_hdr_mode index)
1008{
1009	int ret;
1010
1011	/*
1012	 * vblank and short exposure change according to HDR mode, do it first
1013	 * as it can violate sensors 'rule of thumbs' and therefore will require
1014	 * to change the long exposure.
1015	 */
1016	ret = vgxy61_update_vblank(sensor, sensor->vblank, index);
1017	if (ret)
1018		return ret;
1019
1020	/* Update strobe mode according to HDR */
1021	ret = vgxy61_update_gpios_strobe_mode(sensor, index);
1022	if (ret)
1023		return ret;
1024
1025	sensor->hdr = index;
1026
1027	if (sensor->streaming)
1028		return vgxy61_apply_hdr(sensor, sensor->hdr);
1029	return 0;
1030}
1031
1032static int vgxy61_apply_settings(struct vgxy61_dev *sensor)
1033{
1034	int ret;
1035	unsigned int i;
1036
1037	ret = vgxy61_apply_hdr(sensor, sensor->hdr);
1038	if (ret)
1039		return ret;
1040
1041	ret = vgxy61_apply_framelength(sensor);
1042	if (ret)
1043		return ret;
1044
1045	ret = vgxy61_apply_exposure(sensor);
1046	if (ret)
1047		return ret;
1048
1049	ret = cci_write(sensor->regmap, VGXY61_REG_ANALOG_GAIN,
1050			sensor->analog_gain, NULL);
1051	if (ret)
1052		return ret;
1053	ret = vgxy61_apply_digital_gain(sensor, sensor->digital_gain);
1054	if (ret)
1055		return ret;
1056
1057	ret = cci_write(sensor->regmap, VGXY61_REG_ORIENTATION,
1058			sensor->hflip | (sensor->vflip << 1), NULL);
1059	if (ret)
1060		return ret;
1061
1062	ret = vgxy61_apply_patgen(sensor, sensor->pattern);
1063	if (ret)
1064		return ret;
1065
1066	for (i = 0; i < VGXY61_NB_GPIOS; i++) {
1067		ret = vgxy61_apply_gpiox_strobe_mode(sensor,
1068						     sensor->strobe_mode, i);
1069		if (ret)
1070			return ret;
1071	}
1072
1073	return 0;
1074}
1075
1076static int vgxy61_stream_enable(struct vgxy61_dev *sensor)
1077{
1078	struct i2c_client *client = v4l2_get_subdevdata(&sensor->sd);
1079	const struct v4l2_rect *crop = &sensor->current_mode->crop;
1080	int ret = 0;
1081
1082	ret = vgxy61_check_bw(sensor);
1083	if (ret)
1084		return ret;
1085
1086	ret = pm_runtime_resume_and_get(&client->dev);
1087	if (ret)
1088		return ret;
1089
1090	cci_write(sensor->regmap, VGXY61_REG_FORMAT_CTRL,
1091		  get_bpp_by_code(sensor->fmt.code), &ret);
1092	cci_write(sensor->regmap, VGXY61_REG_OIF_ROI0_CTRL,
1093		  get_data_type_by_code(sensor->fmt.code), &ret);
1094
1095	cci_write(sensor->regmap, VGXY61_REG_READOUT_CTRL,
1096		  sensor->current_mode->bin_mode, &ret);
1097	cci_write(sensor->regmap, VGXY61_REG_ROI0_START_H, crop->left, &ret);
1098	cci_write(sensor->regmap, VGXY61_REG_ROI0_END_H,
1099		  crop->left + crop->width - 1, &ret);
1100	cci_write(sensor->regmap, VGXY61_REG_ROI0_START_V, crop->top, &ret);
1101	cci_write(sensor->regmap, VGXY61_REG_ROI0_END_V,
1102		  crop->top + crop->height - 1, &ret);
1103	if (ret)
1104		goto err_rpm_put;
1105
1106	ret = vgxy61_apply_settings(sensor);
1107	if (ret)
1108		goto err_rpm_put;
1109
1110	ret = cci_write(sensor->regmap, VGXY61_REG_STREAMING,
1111			VGXY61_STREAMING_REQ_START, NULL);
1112	if (ret)
1113		goto err_rpm_put;
1114
1115	ret = vgxy61_poll_reg(sensor, VGXY61_REG_STREAMING,
1116			      VGXY61_STREAMING_NO_REQ, VGXY61_TIMEOUT_MS);
1117	if (ret)
1118		goto err_rpm_put;
1119
1120	ret = vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_STREAMING,
1121				VGXY61_TIMEOUT_MS);
1122	if (ret)
1123		goto err_rpm_put;
1124
1125	/* vflip and hflip cannot change during streaming */
1126	__v4l2_ctrl_grab(sensor->vflip_ctrl, true);
1127	__v4l2_ctrl_grab(sensor->hflip_ctrl, true);
1128
1129	return 0;
1130
1131err_rpm_put:
1132	pm_runtime_put(&client->dev);
1133	return ret;
1134}
1135
1136static int vgxy61_stream_disable(struct vgxy61_dev *sensor)
1137{
1138	struct i2c_client *client = v4l2_get_subdevdata(&sensor->sd);
1139	int ret;
1140
1141	ret = cci_write(sensor->regmap, VGXY61_REG_STREAMING,
1142			VGXY61_STREAMING_REQ_STOP, NULL);
1143	if (ret)
1144		goto err_str_dis;
1145
1146	ret = vgxy61_poll_reg(sensor, VGXY61_REG_STREAMING,
1147			      VGXY61_STREAMING_NO_REQ, 2000);
1148	if (ret)
1149		goto err_str_dis;
1150
1151	ret = vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_SW_STBY,
1152				VGXY61_TIMEOUT_MS);
1153	if (ret)
1154		goto err_str_dis;
1155
1156	__v4l2_ctrl_grab(sensor->vflip_ctrl, false);
1157	__v4l2_ctrl_grab(sensor->hflip_ctrl, false);
1158
1159err_str_dis:
1160	if (ret)
1161		WARN(1, "Can't disable stream");
1162	pm_runtime_put(&client->dev);
1163
1164	return ret;
1165}
1166
1167static int vgxy61_s_stream(struct v4l2_subdev *sd, int enable)
1168{
1169	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1170	int ret = 0;
1171
1172	mutex_lock(&sensor->lock);
1173
1174	ret = enable ? vgxy61_stream_enable(sensor) :
1175	      vgxy61_stream_disable(sensor);
1176	if (!ret)
1177		sensor->streaming = enable;
1178
1179	mutex_unlock(&sensor->lock);
1180
1181	return ret;
1182}
1183
1184static int vgxy61_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
1185				 struct v4l2_mbus_frame_desc *fd)
1186{
1187	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1188
1189	fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
1190	fd->num_entries = 1;
1191	fd->entry[0].pixelcode = sensor->fmt.code;
1192	fd->entry[0].stream = 0;
1193	fd->entry[0].bus.csi2.vc = 0;
1194	fd->entry[0].bus.csi2.dt = get_data_type_by_code(sensor->fmt.code);
1195
1196	return 0;
1197}
1198
1199static int vgxy61_set_fmt(struct v4l2_subdev *sd,
1200			  struct v4l2_subdev_state *sd_state,
1201			  struct v4l2_subdev_format *format)
1202{
1203	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1204	const struct vgxy61_mode_info *new_mode;
1205	struct v4l2_mbus_framefmt *fmt;
1206	int ret;
1207
1208	mutex_lock(&sensor->lock);
1209
1210	if (sensor->streaming) {
1211		ret = -EBUSY;
1212		goto out;
1213	}
1214
1215	ret = vgxy61_try_fmt_internal(sd, &format->format, &new_mode);
1216	if (ret)
1217		goto out;
1218
1219	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
1220		fmt = v4l2_subdev_state_get_format(sd_state, 0);
1221		*fmt = format->format;
1222	} else if (sensor->current_mode != new_mode ||
1223		   sensor->fmt.code != format->format.code) {
1224		fmt = &sensor->fmt;
1225		*fmt = format->format;
1226
1227		sensor->current_mode = new_mode;
1228
1229		/* Reset vblank and framelength to default */
1230		ret = vgxy61_update_vblank(sensor,
1231					   VGXY61_FRAME_LENGTH_DEF -
1232					   new_mode->crop.height,
1233					   sensor->hdr);
1234
1235		/* Update controls to reflect new mode */
1236		__v4l2_ctrl_s_ctrl_int64(sensor->pixel_rate_ctrl,
1237					 get_pixel_rate(sensor));
1238		__v4l2_ctrl_modify_range(sensor->vblank_ctrl,
1239					 sensor->vblank_min,
1240					 0xffff - new_mode->crop.height,
1241					 1, sensor->vblank);
1242		__v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, sensor->vblank);
1243		__v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min,
1244					 sensor->expo_max, 1,
1245					 sensor->expo_long);
1246	}
1247
1248out:
1249	mutex_unlock(&sensor->lock);
1250
1251	return ret;
1252}
1253
1254static int vgxy61_init_state(struct v4l2_subdev *sd,
1255			     struct v4l2_subdev_state *sd_state)
1256{
1257	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1258	struct v4l2_subdev_format fmt = { 0 };
1259
1260	vgxy61_fill_framefmt(sensor, sensor->current_mode, &fmt.format,
1261			     VGXY61_MEDIA_BUS_FMT_DEF);
1262
1263	return vgxy61_set_fmt(sd, sd_state, &fmt);
1264}
1265
1266static int vgxy61_s_ctrl(struct v4l2_ctrl *ctrl)
1267{
1268	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
1269	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1270	const struct vgxy61_mode_info *cur_mode = sensor->current_mode;
1271	int ret;
1272
1273	switch (ctrl->id) {
1274	case V4L2_CID_EXPOSURE:
1275		ret = vgxy61_update_exposure(sensor, ctrl->val, sensor->hdr);
1276		ctrl->val = sensor->expo_long;
1277		break;
1278	case V4L2_CID_ANALOGUE_GAIN:
1279		ret = vgxy61_update_analog_gain(sensor, ctrl->val);
1280		break;
1281	case V4L2_CID_DIGITAL_GAIN:
1282		ret = vgxy61_update_digital_gain(sensor, ctrl->val);
1283		break;
1284	case V4L2_CID_VFLIP:
1285	case V4L2_CID_HFLIP:
1286		if (sensor->streaming) {
1287			ret = -EBUSY;
1288			break;
1289		}
1290		if (ctrl->id == V4L2_CID_VFLIP)
1291			sensor->vflip = ctrl->val;
1292		if (ctrl->id == V4L2_CID_HFLIP)
1293			sensor->hflip = ctrl->val;
1294		ret = 0;
1295		break;
1296	case V4L2_CID_TEST_PATTERN:
1297		ret = vgxy61_update_patgen(sensor, ctrl->val);
1298		break;
1299	case V4L2_CID_HDR_SENSOR_MODE:
1300		ret = vgxy61_update_hdr(sensor, ctrl->val);
1301		/* Update vblank and exposure controls to match new hdr */
1302		__v4l2_ctrl_modify_range(sensor->vblank_ctrl,
1303					 sensor->vblank_min,
1304					 0xffff - cur_mode->crop.height,
1305					 1, sensor->vblank);
1306		__v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min,
1307					 sensor->expo_max, 1,
1308					 sensor->expo_long);
1309		break;
1310	case V4L2_CID_VBLANK:
1311		ret = vgxy61_update_vblank(sensor, ctrl->val, sensor->hdr);
1312		/* Update exposure control to match new vblank */
1313		__v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min,
1314					 sensor->expo_max, 1,
1315					 sensor->expo_long);
1316		break;
1317	default:
1318		ret = -EINVAL;
1319		break;
1320	}
1321
1322	return ret;
1323}
1324
1325static const struct v4l2_ctrl_ops vgxy61_ctrl_ops = {
1326	.s_ctrl = vgxy61_s_ctrl,
1327};
1328
1329static int vgxy61_init_controls(struct vgxy61_dev *sensor)
1330{
1331	const struct v4l2_ctrl_ops *ops = &vgxy61_ctrl_ops;
1332	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
1333	const struct vgxy61_mode_info *cur_mode = sensor->current_mode;
1334	struct v4l2_fwnode_device_properties props;
1335	struct v4l2_ctrl *ctrl;
1336	int ret;
1337
1338	v4l2_ctrl_handler_init(hdl, 16);
1339	/* We can use our own mutex for the ctrl lock */
1340	hdl->lock = &sensor->lock;
1341	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN, 0, 0x1c, 1,
1342			  sensor->analog_gain);
1343	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN, 0, 0xfff, 1,
1344			  sensor->digital_gain);
1345	v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
1346				     ARRAY_SIZE(vgxy61_test_pattern_menu) - 1,
1347				     0, 0, vgxy61_test_pattern_menu);
1348	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 0,
1349				 sensor->line_length, 1,
1350				 sensor->line_length - cur_mode->width);
1351	if (ctrl)
1352		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1353	ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
1354				      ARRAY_SIZE(link_freq) - 1, 0, link_freq);
1355	if (ctrl)
1356		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1357	v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_HDR_SENSOR_MODE,
1358				     ARRAY_SIZE(vgxy61_hdr_mode_menu) - 1, 0,
1359				     VGXY61_NO_HDR, vgxy61_hdr_mode_menu);
1360
1361	/*
1362	 * Keep a pointer to these controls as we need to update them when
1363	 * setting the format
1364	 */
1365	sensor->pixel_rate_ctrl = v4l2_ctrl_new_std(hdl, ops,
1366						    V4L2_CID_PIXEL_RATE, 1,
1367						    INT_MAX, 1,
1368						    get_pixel_rate(sensor));
1369	if (sensor->pixel_rate_ctrl)
1370		sensor->pixel_rate_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1371	sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
1372					      sensor->expo_min,
1373					      sensor->expo_max, 1,
1374					      sensor->expo_long);
1375	sensor->vblank_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK,
1376						sensor->vblank_min,
1377						0xffff - cur_mode->crop.height,
1378						1, sensor->vblank);
1379	sensor->vflip_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
1380					       0, 1, 1, sensor->vflip);
1381	sensor->hflip_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
1382					       0, 1, 1, sensor->hflip);
1383
1384	if (hdl->error) {
1385		ret = hdl->error;
1386		goto free_ctrls;
1387	}
1388
1389	ret = v4l2_fwnode_device_parse(&sensor->i2c_client->dev, &props);
1390	if (ret)
1391		goto free_ctrls;
1392
1393	ret = v4l2_ctrl_new_fwnode_properties(hdl, ops, &props);
1394	if (ret)
1395		goto free_ctrls;
1396
1397	sensor->sd.ctrl_handler = hdl;
1398	return 0;
1399
1400free_ctrls:
1401	v4l2_ctrl_handler_free(hdl);
1402	return ret;
1403}
1404
1405static const struct v4l2_subdev_core_ops vgxy61_core_ops = {
1406	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1407	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
1408};
1409
1410static const struct v4l2_subdev_video_ops vgxy61_video_ops = {
1411	.s_stream = vgxy61_s_stream,
1412};
1413
1414static const struct v4l2_subdev_pad_ops vgxy61_pad_ops = {
1415	.enum_mbus_code = vgxy61_enum_mbus_code,
1416	.get_fmt = vgxy61_get_fmt,
1417	.set_fmt = vgxy61_set_fmt,
1418	.get_selection = vgxy61_get_selection,
1419	.enum_frame_size = vgxy61_enum_frame_size,
1420	.get_frame_desc = vgxy61_get_frame_desc,
1421};
1422
1423static const struct v4l2_subdev_ops vgxy61_subdev_ops = {
1424	.core = &vgxy61_core_ops,
1425	.video = &vgxy61_video_ops,
1426	.pad = &vgxy61_pad_ops,
1427};
1428
1429static const struct v4l2_subdev_internal_ops vgxy61_internal_ops = {
1430	.init_state = vgxy61_init_state,
1431};
1432
1433static const struct media_entity_operations vgxy61_subdev_entity_ops = {
1434	.link_validate = v4l2_subdev_link_validate,
1435};
1436
1437static int vgxy61_tx_from_ep(struct vgxy61_dev *sensor,
1438			     struct fwnode_handle *handle)
1439{
1440	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
1441	struct i2c_client *client = sensor->i2c_client;
1442	u32 log2phy[VGXY61_NB_POLARITIES] = {~0, ~0, ~0, ~0, ~0};
1443	u32 phy2log[VGXY61_NB_POLARITIES] = {~0, ~0, ~0, ~0, ~0};
1444	int polarities[VGXY61_NB_POLARITIES] = {0, 0, 0, 0, 0};
1445	int l_nb;
1446	unsigned int p, l, i;
1447	int ret;
1448
1449	ret = v4l2_fwnode_endpoint_alloc_parse(handle, &ep);
1450	if (ret)
1451		return -EINVAL;
1452
1453	l_nb = ep.bus.mipi_csi2.num_data_lanes;
1454	if (l_nb != 1 && l_nb != 2 && l_nb != 4) {
1455		dev_err(&client->dev, "invalid data lane number %d\n", l_nb);
1456		goto error_ep;
1457	}
1458
1459	/* Build log2phy, phy2log and polarities from ep info */
1460	log2phy[0] = ep.bus.mipi_csi2.clock_lane;
1461	phy2log[log2phy[0]] = 0;
1462	for (l = 1; l < l_nb + 1; l++) {
1463		log2phy[l] = ep.bus.mipi_csi2.data_lanes[l - 1];
1464		phy2log[log2phy[l]] = l;
1465	}
1466	/*
1467	 * Then fill remaining slots for every physical slot to have something
1468	 * valid for hardware stuff.
1469	 */
1470	for (p = 0; p < VGXY61_NB_POLARITIES; p++) {
1471		if (phy2log[p] != ~0)
1472			continue;
1473		phy2log[p] = l;
1474		log2phy[l] = p;
1475		l++;
1476	}
1477	for (l = 0; l < l_nb + 1; l++)
1478		polarities[l] = ep.bus.mipi_csi2.lane_polarities[l];
1479
1480	if (log2phy[0] != 0) {
1481		dev_err(&client->dev, "clk lane must be map to physical lane 0\n");
1482		goto error_ep;
1483	}
1484	sensor->oif_ctrl = (polarities[4] << 15) + ((phy2log[4] - 1) << 13) +
1485			   (polarities[3] << 12) + ((phy2log[3] - 1) << 10) +
1486			   (polarities[2] <<  9) + ((phy2log[2] - 1) <<  7) +
1487			   (polarities[1] <<  6) + ((phy2log[1] - 1) <<  4) +
1488			   (polarities[0] <<  3) +
1489			   l_nb;
1490	sensor->nb_of_lane = l_nb;
1491
1492	dev_dbg(&client->dev, "tx uses %d lanes", l_nb);
1493	for (i = 0; i < VGXY61_NB_POLARITIES; i++) {
1494		dev_dbg(&client->dev, "log2phy[%d] = %d\n", i, log2phy[i]);
1495		dev_dbg(&client->dev, "phy2log[%d] = %d\n", i, phy2log[i]);
1496		dev_dbg(&client->dev, "polarity[%d] = %d\n", i, polarities[i]);
1497	}
1498	dev_dbg(&client->dev, "oif_ctrl = 0x%04x\n", sensor->oif_ctrl);
1499
1500	v4l2_fwnode_endpoint_free(&ep);
1501
1502	return 0;
1503
1504error_ep:
1505	v4l2_fwnode_endpoint_free(&ep);
1506
1507	return -EINVAL;
1508}
1509
1510static int vgxy61_configure(struct vgxy61_dev *sensor)
1511{
1512	u32 sensor_freq;
1513	u8 prediv, mult;
1514	u64 line_length;
1515	int ret = 0;
1516
1517	compute_pll_parameters_by_freq(sensor->clk_freq, &prediv, &mult);
1518	sensor_freq = (mult * sensor->clk_freq) / prediv;
1519	/* Frequency to data rate is 1:1 ratio for MIPI */
1520	sensor->data_rate_in_mbps = sensor_freq;
1521	/* Video timing ISP path (pixel clock)  requires 804/5 mhz = 160 mhz */
1522	sensor->pclk = sensor_freq / 5;
1523
1524	cci_read(sensor->regmap, VGXY61_REG_LINE_LENGTH, &line_length, &ret);
1525	if (ret < 0)
1526		return ret;
1527	sensor->line_length = (u16)line_length;
1528	cci_write(sensor->regmap, VGXY61_REG_EXT_CLOCK, sensor->clk_freq, &ret);
1529	cci_write(sensor->regmap, VGXY61_REG_CLK_PLL_PREDIV, prediv, &ret);
1530	cci_write(sensor->regmap, VGXY61_REG_CLK_SYS_PLL_MULT, mult, &ret);
1531	cci_write(sensor->regmap, VGXY61_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
1532	cci_write(sensor->regmap, VGXY61_REG_FRAME_CONTENT_CTRL, 0, &ret);
1533	cci_write(sensor->regmap, VGXY61_REG_BYPASS_CTRL, 4, &ret);
1534	if (ret)
1535		return ret;
1536	vgxy61_update_gpios_strobe_polarity(sensor, sensor->gpios_polarity);
1537	/* Set pattern generator solid to middle value */
1538	cci_write(sensor->regmap, VGXY61_REG_PATGEN_LONG_DATA_GR, 0x800, &ret);
1539	cci_write(sensor->regmap, VGXY61_REG_PATGEN_LONG_DATA_R, 0x800, &ret);
1540	cci_write(sensor->regmap, VGXY61_REG_PATGEN_LONG_DATA_B, 0x800, &ret);
1541	cci_write(sensor->regmap, VGXY61_REG_PATGEN_LONG_DATA_GB, 0x800, &ret);
1542	cci_write(sensor->regmap, VGXY61_REG_PATGEN_SHORT_DATA_GR, 0x800, &ret);
1543	cci_write(sensor->regmap, VGXY61_REG_PATGEN_SHORT_DATA_R, 0x800, &ret);
1544	cci_write(sensor->regmap, VGXY61_REG_PATGEN_SHORT_DATA_B, 0x800, &ret);
1545	cci_write(sensor->regmap, VGXY61_REG_PATGEN_SHORT_DATA_GB, 0x800, &ret);
1546	if (ret)
1547		return ret;
1548
1549	return 0;
1550}
1551
1552static int vgxy61_patch(struct vgxy61_dev *sensor)
1553{
1554	struct i2c_client *client = sensor->i2c_client;
1555	u64 patch;
1556	int ret;
1557
1558	ret = vgxy61_write_array(sensor, VGXY61_REG_FWPATCH_START_ADDR,
1559				 sizeof(patch_array), patch_array);
1560	cci_write(sensor->regmap, VGXY61_REG_STBY, 0x10, &ret);
1561	if (ret)
1562		return ret;
1563
1564	ret = vgxy61_poll_reg(sensor, VGXY61_REG_STBY, 0, VGXY61_TIMEOUT_MS);
1565	cci_read(sensor->regmap, VGXY61_REG_FWPATCH_REVISION, &patch, &ret);
1566	if (ret < 0)
1567		return ret;
1568
1569	if (patch != (VGXY61_FWPATCH_REVISION_MAJOR << 12) +
1570		     (VGXY61_FWPATCH_REVISION_MINOR << 8) +
1571		     VGXY61_FWPATCH_REVISION_MICRO) {
1572		dev_err(&client->dev,
1573			"bad patch version expected %d.%d.%d got %u.%u.%u\n",
1574			VGXY61_FWPATCH_REVISION_MAJOR,
1575			VGXY61_FWPATCH_REVISION_MINOR,
1576			VGXY61_FWPATCH_REVISION_MICRO,
1577			(u16)patch >> 12, ((u16)patch >> 8) & 0x0f, (u16)patch & 0xff);
1578		return -ENODEV;
1579	}
1580	dev_dbg(&client->dev, "patch %u.%u.%u applied\n",
1581		(u16)patch >> 12, ((u16)patch >> 8) & 0x0f, (u16)patch & 0xff);
1582
1583	return 0;
1584}
1585
1586static int vgxy61_detect_cut_version(struct vgxy61_dev *sensor)
1587{
1588	struct i2c_client *client = sensor->i2c_client;
1589	u64 device_rev;
1590	int ret;
1591
1592	ret = cci_read(sensor->regmap, VGXY61_REG_REVISION, &device_rev, NULL);
1593	if (ret < 0)
1594		return ret;
1595
1596	switch (device_rev >> 8) {
1597	case 0xA:
1598		dev_dbg(&client->dev, "Cut1 detected\n");
1599		dev_err(&client->dev, "Cut1 not supported by this driver\n");
1600		return -ENODEV;
1601	case 0xB:
1602		dev_dbg(&client->dev, "Cut2 detected\n");
1603		return 0;
1604	case 0xC:
1605		dev_dbg(&client->dev, "Cut3 detected\n");
1606		return 0;
1607	default:
1608		dev_err(&client->dev, "Unable to detect cut version\n");
1609		return -ENODEV;
1610	}
1611}
1612
1613static int vgxy61_detect(struct vgxy61_dev *sensor)
1614{
1615	struct i2c_client *client = sensor->i2c_client;
1616	u64 st, id = 0;
1617	int ret;
1618
1619	ret = cci_read(sensor->regmap, VGXY61_REG_MODEL_ID, &id, NULL);
1620	if (ret < 0)
1621		return ret;
1622	if (id != VG5661_MODEL_ID && id != VG5761_MODEL_ID) {
1623		dev_warn(&client->dev, "Unsupported sensor id %x\n", (u16)id);
1624		return -ENODEV;
1625	}
1626	dev_dbg(&client->dev, "detected sensor id = 0x%04x\n", (u16)id);
1627	sensor->id = id;
1628
1629	ret = vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_SW_STBY,
1630				VGXY61_TIMEOUT_MS);
1631	if (ret)
1632		return ret;
1633
1634	ret = cci_read(sensor->regmap, VGXY61_REG_NVM, &st, NULL);
1635	if (ret < 0)
1636		return ret;
1637	if (st != VGXY61_NVM_OK)
1638		dev_warn(&client->dev, "Bad nvm state got %u\n", (u8)st);
1639
1640	ret = vgxy61_detect_cut_version(sensor);
1641	if (ret)
1642		return ret;
1643
1644	return 0;
1645}
1646
1647/* Power/clock management functions */
1648static int vgxy61_power_on(struct device *dev)
1649{
1650	struct i2c_client *client = to_i2c_client(dev);
1651	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1652	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1653	int ret;
1654
1655	ret = regulator_bulk_enable(ARRAY_SIZE(vgxy61_supply_name),
1656				    sensor->supplies);
1657	if (ret) {
1658		dev_err(&client->dev, "failed to enable regulators %d\n", ret);
1659		return ret;
1660	}
1661
1662	ret = clk_prepare_enable(sensor->xclk);
1663	if (ret) {
1664		dev_err(&client->dev, "failed to enable clock %d\n", ret);
1665		goto disable_bulk;
1666	}
1667
1668	if (sensor->reset_gpio) {
1669		ret = vgxy61_apply_reset(sensor);
1670		if (ret) {
1671			dev_err(&client->dev, "sensor reset failed %d\n", ret);
1672			goto disable_clock;
1673		}
1674	}
1675
1676	ret = vgxy61_detect(sensor);
1677	if (ret) {
1678		dev_err(&client->dev, "sensor detect failed %d\n", ret);
1679		goto disable_clock;
1680	}
1681
1682	ret = vgxy61_patch(sensor);
1683	if (ret) {
1684		dev_err(&client->dev, "sensor patch failed %d\n", ret);
1685		goto disable_clock;
1686	}
1687
1688	ret = vgxy61_configure(sensor);
1689	if (ret) {
1690		dev_err(&client->dev, "sensor configuration failed %d\n", ret);
1691		goto disable_clock;
1692	}
1693
1694	return 0;
1695
1696disable_clock:
1697	clk_disable_unprepare(sensor->xclk);
1698disable_bulk:
1699	regulator_bulk_disable(ARRAY_SIZE(vgxy61_supply_name),
1700			       sensor->supplies);
1701
1702	return ret;
1703}
1704
1705static int vgxy61_power_off(struct device *dev)
1706{
1707	struct i2c_client *client = to_i2c_client(dev);
1708	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1709	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1710
1711	clk_disable_unprepare(sensor->xclk);
1712	regulator_bulk_disable(ARRAY_SIZE(vgxy61_supply_name),
1713			       sensor->supplies);
1714	return 0;
1715}
1716
1717static void vgxy61_fill_sensor_param(struct vgxy61_dev *sensor)
1718{
1719	if (sensor->id == VG5761_MODEL_ID) {
1720		sensor->sensor_width = VGX761_WIDTH;
1721		sensor->sensor_height = VGX761_HEIGHT;
1722		sensor->sensor_modes = vgx761_mode_data;
1723		sensor->sensor_modes_nb = ARRAY_SIZE(vgx761_mode_data);
1724		sensor->default_mode = &vgx761_mode_data[VGX761_DEFAULT_MODE];
1725		sensor->rot_term = VGX761_SHORT_ROT_TERM;
1726	} else if (sensor->id == VG5661_MODEL_ID) {
1727		sensor->sensor_width = VGX661_WIDTH;
1728		sensor->sensor_height = VGX661_HEIGHT;
1729		sensor->sensor_modes = vgx661_mode_data;
1730		sensor->sensor_modes_nb = ARRAY_SIZE(vgx661_mode_data);
1731		sensor->default_mode = &vgx661_mode_data[VGX661_DEFAULT_MODE];
1732		sensor->rot_term = VGX661_SHORT_ROT_TERM;
1733	} else {
1734		/* Should never happen */
1735		WARN_ON(true);
1736	}
1737	sensor->current_mode = sensor->default_mode;
1738}
1739
1740static int vgxy61_probe(struct i2c_client *client)
1741{
1742	struct device *dev = &client->dev;
1743	struct fwnode_handle *handle;
1744	struct vgxy61_dev *sensor;
1745	int ret;
1746
1747	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
1748	if (!sensor)
1749		return -ENOMEM;
1750
1751	sensor->i2c_client = client;
1752	sensor->streaming = false;
1753	sensor->hdr = VGXY61_NO_HDR;
1754	sensor->expo_long = 200;
1755	sensor->expo_short = 0;
1756	sensor->hflip = false;
1757	sensor->vflip = false;
1758	sensor->analog_gain = 0;
1759	sensor->digital_gain = 256;
1760
1761	sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
1762	if (IS_ERR(sensor->regmap)) {
1763		ret = PTR_ERR(sensor->regmap);
1764		return dev_err_probe(dev, ret, "Failed to init regmap\n");
1765	}
1766
1767	handle = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0, 0);
1768	if (!handle) {
1769		dev_err(dev, "handle node not found\n");
1770		return -EINVAL;
1771	}
1772
1773	ret = vgxy61_tx_from_ep(sensor, handle);
1774	fwnode_handle_put(handle);
1775	if (ret) {
1776		dev_err(dev, "Failed to parse handle %d\n", ret);
1777		return ret;
1778	}
1779
1780	sensor->xclk = devm_v4l2_sensor_clk_get(dev, NULL);
1781	if (IS_ERR(sensor->xclk))
1782		return dev_err_probe(dev, PTR_ERR(sensor->xclk),
1783				     "failed to get xclk\n");
1784
1785	sensor->clk_freq = clk_get_rate(sensor->xclk);
1786	if (sensor->clk_freq < 6 * HZ_PER_MHZ ||
1787	    sensor->clk_freq > 27 * HZ_PER_MHZ) {
1788		dev_err(dev, "Only 6Mhz-27Mhz clock range supported. provide %lu MHz\n",
1789			sensor->clk_freq / HZ_PER_MHZ);
1790		return -EINVAL;
1791	}
1792	sensor->gpios_polarity =
1793		device_property_read_bool(dev, "st,strobe-gpios-polarity");
1794
1795	v4l2_i2c_subdev_init(&sensor->sd, client, &vgxy61_subdev_ops);
1796	sensor->sd.internal_ops = &vgxy61_internal_ops;
1797	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1798			    V4L2_SUBDEV_FL_HAS_EVENTS;
1799	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
1800	sensor->sd.entity.ops = &vgxy61_subdev_entity_ops;
1801	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1802
1803	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
1804						     GPIOD_OUT_HIGH);
1805
1806	ret = vgxy61_get_regulators(sensor);
1807	if (ret) {
1808		dev_err(&client->dev, "failed to get regulators %d\n", ret);
1809		return ret;
1810	}
1811
1812	ret = vgxy61_power_on(dev);
1813	if (ret)
1814		return ret;
1815
1816	vgxy61_fill_sensor_param(sensor);
1817	vgxy61_fill_framefmt(sensor, sensor->current_mode, &sensor->fmt,
1818			     VGXY61_MEDIA_BUS_FMT_DEF);
1819
1820	mutex_init(&sensor->lock);
1821
1822	ret = vgxy61_update_hdr(sensor, sensor->hdr);
1823	if (ret)
1824		goto error_power_off;
1825
1826	ret = vgxy61_init_controls(sensor);
1827	if (ret) {
1828		dev_err(&client->dev, "controls initialization failed %d\n",
1829			ret);
1830		goto error_power_off;
1831	}
1832
1833	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
1834	if (ret) {
1835		dev_err(&client->dev, "pads init failed %d\n", ret);
1836		goto error_handler_free;
1837	}
1838
1839	/* Enable runtime PM and turn off the device */
1840	pm_runtime_set_active(dev);
1841	pm_runtime_enable(dev);
1842	pm_runtime_idle(dev);
1843
1844	ret = v4l2_async_register_subdev(&sensor->sd);
1845	if (ret) {
1846		dev_err(&client->dev, "async subdev register failed %d\n", ret);
1847		goto error_pm_runtime;
1848	}
1849
1850	pm_runtime_set_autosuspend_delay(&client->dev, 1000);
1851	pm_runtime_use_autosuspend(&client->dev);
1852
1853	dev_dbg(&client->dev, "vgxy61 probe successfully\n");
1854
1855	return 0;
1856
1857error_pm_runtime:
1858	pm_runtime_disable(&client->dev);
1859	pm_runtime_set_suspended(&client->dev);
1860	media_entity_cleanup(&sensor->sd.entity);
1861error_handler_free:
1862	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
1863error_power_off:
1864	mutex_destroy(&sensor->lock);
1865	vgxy61_power_off(dev);
1866
1867	return ret;
1868}
1869
1870static void vgxy61_remove(struct i2c_client *client)
1871{
1872	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1873	struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1874
1875	v4l2_async_unregister_subdev(&sensor->sd);
1876	mutex_destroy(&sensor->lock);
1877	media_entity_cleanup(&sensor->sd.entity);
1878
1879	pm_runtime_disable(&client->dev);
1880	if (!pm_runtime_status_suspended(&client->dev))
1881		vgxy61_power_off(&client->dev);
1882	pm_runtime_set_suspended(&client->dev);
1883}
1884
1885static const struct of_device_id vgxy61_dt_ids[] = {
1886	{ .compatible = "st,st-vgxy61" },
1887	{ /* sentinel */ }
1888};
1889MODULE_DEVICE_TABLE(of, vgxy61_dt_ids);
1890
1891static const struct dev_pm_ops vgxy61_pm_ops = {
1892	SET_RUNTIME_PM_OPS(vgxy61_power_off, vgxy61_power_on, NULL)
1893};
1894
1895static struct i2c_driver vgxy61_i2c_driver = {
1896	.driver = {
1897		.name  = "vgxy61",
1898		.of_match_table = vgxy61_dt_ids,
1899		.pm = &vgxy61_pm_ops,
1900	},
1901	.probe = vgxy61_probe,
1902	.remove = vgxy61_remove,
1903};
1904
1905module_i2c_driver(vgxy61_i2c_driver);
1906
1907MODULE_AUTHOR("Benjamin Mugnier <benjamin.mugnier@foss.st.com>");
1908MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>");
1909MODULE_AUTHOR("Sylvain Petinot <sylvain.petinot@foss.st.com>");
1910MODULE_DESCRIPTION("VGXY61 camera subdev driver");
1911MODULE_LICENSE("GPL");
~/ Read-only file preview.