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 <asm/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 min(min(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_set_fmt(struct v4l2_subdev *sd, 1185 struct v4l2_subdev_state *sd_state, 1186 struct v4l2_subdev_format *format) 1187{ 1188 struct vgxy61_dev *sensor = to_vgxy61_dev(sd); 1189 const struct vgxy61_mode_info *new_mode; 1190 struct v4l2_mbus_framefmt *fmt; 1191 int ret; 1192 1193 mutex_lock(&sensor->lock); 1194 1195 if (sensor->streaming) { 1196 ret = -EBUSY; 1197 goto out; 1198 } 1199 1200 ret = vgxy61_try_fmt_internal(sd, &format->format, &new_mode); 1201 if (ret) 1202 goto out; 1203 1204 if (format->which == V4L2_SUBDEV_FORMAT_TRY) { 1205 fmt = v4l2_subdev_state_get_format(sd_state, 0); 1206 *fmt = format->format; 1207 } else if (sensor->current_mode != new_mode || 1208 sensor->fmt.code != format->format.code) { 1209 fmt = &sensor->fmt; 1210 *fmt = format->format; 1211 1212 sensor->current_mode = new_mode; 1213 1214 /* Reset vblank and framelength to default */ 1215 ret = vgxy61_update_vblank(sensor, 1216 VGXY61_FRAME_LENGTH_DEF - 1217 new_mode->crop.height, 1218 sensor->hdr); 1219 1220 /* Update controls to reflect new mode */ 1221 __v4l2_ctrl_s_ctrl_int64(sensor->pixel_rate_ctrl, 1222 get_pixel_rate(sensor)); 1223 __v4l2_ctrl_modify_range(sensor->vblank_ctrl, 1224 sensor->vblank_min, 1225 0xffff - new_mode->crop.height, 1226 1, sensor->vblank); 1227 __v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, sensor->vblank); 1228 __v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min, 1229 sensor->expo_max, 1, 1230 sensor->expo_long); 1231 } 1232 1233out: 1234 mutex_unlock(&sensor->lock); 1235 1236 return ret; 1237} 1238 1239static int vgxy61_init_state(struct v4l2_subdev *sd, 1240 struct v4l2_subdev_state *sd_state) 1241{ 1242 struct vgxy61_dev *sensor = to_vgxy61_dev(sd); 1243 struct v4l2_subdev_format fmt = { 0 }; 1244 1245 vgxy61_fill_framefmt(sensor, sensor->current_mode, &fmt.format, 1246 VGXY61_MEDIA_BUS_FMT_DEF); 1247 1248 return vgxy61_set_fmt(sd, sd_state, &fmt); 1249} 1250 1251static int vgxy61_s_ctrl(struct v4l2_ctrl *ctrl) 1252{ 1253 struct v4l2_subdev *sd = ctrl_to_sd(ctrl); 1254 struct vgxy61_dev *sensor = to_vgxy61_dev(sd); 1255 const struct vgxy61_mode_info *cur_mode = sensor->current_mode; 1256 int ret; 1257 1258 switch (ctrl->id) { 1259 case V4L2_CID_EXPOSURE: 1260 ret = vgxy61_update_exposure(sensor, ctrl->val, sensor->hdr); 1261 ctrl->val = sensor->expo_long; 1262 break; 1263 case V4L2_CID_ANALOGUE_GAIN: 1264 ret = vgxy61_update_analog_gain(sensor, ctrl->val); 1265 break; 1266 case V4L2_CID_DIGITAL_GAIN: 1267 ret = vgxy61_update_digital_gain(sensor, ctrl->val); 1268 break; 1269 case V4L2_CID_VFLIP: 1270 case V4L2_CID_HFLIP: 1271 if (sensor->streaming) { 1272 ret = -EBUSY; 1273 break; 1274 } 1275 if (ctrl->id == V4L2_CID_VFLIP) 1276 sensor->vflip = ctrl->val; 1277 if (ctrl->id == V4L2_CID_HFLIP) 1278 sensor->hflip = ctrl->val; 1279 ret = 0; 1280 break; 1281 case V4L2_CID_TEST_PATTERN: 1282 ret = vgxy61_update_patgen(sensor, ctrl->val); 1283 break; 1284 case V4L2_CID_HDR_SENSOR_MODE: 1285 ret = vgxy61_update_hdr(sensor, ctrl->val); 1286 /* Update vblank and exposure controls to match new hdr */ 1287 __v4l2_ctrl_modify_range(sensor->vblank_ctrl, 1288 sensor->vblank_min, 1289 0xffff - cur_mode->crop.height, 1290 1, sensor->vblank); 1291 __v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min, 1292 sensor->expo_max, 1, 1293 sensor->expo_long); 1294 break; 1295 case V4L2_CID_VBLANK: 1296 ret = vgxy61_update_vblank(sensor, ctrl->val, sensor->hdr); 1297 /* Update exposure control to match new vblank */ 1298 __v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min, 1299 sensor->expo_max, 1, 1300 sensor->expo_long); 1301 break; 1302 default: 1303 ret = -EINVAL; 1304 break; 1305 } 1306 1307 return ret; 1308} 1309 1310static const struct v4l2_ctrl_ops vgxy61_ctrl_ops = { 1311 .s_ctrl = vgxy61_s_ctrl, 1312}; 1313 1314static int vgxy61_init_controls(struct vgxy61_dev *sensor) 1315{ 1316 const struct v4l2_ctrl_ops *ops = &vgxy61_ctrl_ops; 1317 struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler; 1318 const struct vgxy61_mode_info *cur_mode = sensor->current_mode; 1319 struct v4l2_fwnode_device_properties props; 1320 struct v4l2_ctrl *ctrl; 1321 int ret; 1322 1323 v4l2_ctrl_handler_init(hdl, 16); 1324 /* We can use our own mutex for the ctrl lock */ 1325 hdl->lock = &sensor->lock; 1326 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN, 0, 0x1c, 1, 1327 sensor->analog_gain); 1328 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN, 0, 0xfff, 1, 1329 sensor->digital_gain); 1330 v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN, 1331 ARRAY_SIZE(vgxy61_test_pattern_menu) - 1, 1332 0, 0, vgxy61_test_pattern_menu); 1333 ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 0, 1334 sensor->line_length, 1, 1335 sensor->line_length - cur_mode->width); 1336 if (ctrl) 1337 ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY; 1338 ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ, 1339 ARRAY_SIZE(link_freq) - 1, 0, link_freq); 1340 if (ctrl) 1341 ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY; 1342 v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_HDR_SENSOR_MODE, 1343 ARRAY_SIZE(vgxy61_hdr_mode_menu) - 1, 0, 1344 VGXY61_NO_HDR, vgxy61_hdr_mode_menu); 1345 1346 /* 1347 * Keep a pointer to these controls as we need to update them when 1348 * setting the format 1349 */ 1350 sensor->pixel_rate_ctrl = v4l2_ctrl_new_std(hdl, ops, 1351 V4L2_CID_PIXEL_RATE, 1, 1352 INT_MAX, 1, 1353 get_pixel_rate(sensor)); 1354 if (sensor->pixel_rate_ctrl) 1355 sensor->pixel_rate_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY; 1356 sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE, 1357 sensor->expo_min, 1358 sensor->expo_max, 1, 1359 sensor->expo_long); 1360 sensor->vblank_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, 1361 sensor->vblank_min, 1362 0xffff - cur_mode->crop.height, 1363 1, sensor->vblank); 1364 sensor->vflip_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 1365 0, 1, 1, sensor->vflip); 1366 sensor->hflip_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 1367 0, 1, 1, sensor->hflip); 1368 1369 if (hdl->error) { 1370 ret = hdl->error; 1371 goto free_ctrls; 1372 } 1373 1374 ret = v4l2_fwnode_device_parse(&sensor->i2c_client->dev, &props); 1375 if (ret) 1376 goto free_ctrls; 1377 1378 ret = v4l2_ctrl_new_fwnode_properties(hdl, ops, &props); 1379 if (ret) 1380 goto free_ctrls; 1381 1382 sensor->sd.ctrl_handler = hdl; 1383 return 0; 1384 1385free_ctrls: 1386 v4l2_ctrl_handler_free(hdl); 1387 return ret; 1388} 1389 1390static const struct v4l2_subdev_core_ops vgxy61_core_ops = { 1391 .subscribe_event = v4l2_ctrl_subdev_subscribe_event, 1392 .unsubscribe_event = v4l2_event_subdev_unsubscribe, 1393}; 1394 1395static const struct v4l2_subdev_video_ops vgxy61_video_ops = { 1396 .s_stream = vgxy61_s_stream, 1397}; 1398 1399static const struct v4l2_subdev_pad_ops vgxy61_pad_ops = { 1400 .enum_mbus_code = vgxy61_enum_mbus_code, 1401 .get_fmt = vgxy61_get_fmt, 1402 .set_fmt = vgxy61_set_fmt, 1403 .get_selection = vgxy61_get_selection, 1404 .enum_frame_size = vgxy61_enum_frame_size, 1405}; 1406 1407static const struct v4l2_subdev_ops vgxy61_subdev_ops = { 1408 .core = &vgxy61_core_ops, 1409 .video = &vgxy61_video_ops, 1410 .pad = &vgxy61_pad_ops, 1411}; 1412 1413static const struct v4l2_subdev_internal_ops vgxy61_internal_ops = { 1414 .init_state = vgxy61_init_state, 1415}; 1416 1417static const struct media_entity_operations vgxy61_subdev_entity_ops = { 1418 .link_validate = v4l2_subdev_link_validate, 1419}; 1420 1421static int vgxy61_tx_from_ep(struct vgxy61_dev *sensor, 1422 struct fwnode_handle *handle) 1423{ 1424 struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY }; 1425 struct i2c_client *client = sensor->i2c_client; 1426 u32 log2phy[VGXY61_NB_POLARITIES] = {~0, ~0, ~0, ~0, ~0}; 1427 u32 phy2log[VGXY61_NB_POLARITIES] = {~0, ~0, ~0, ~0, ~0}; 1428 int polarities[VGXY61_NB_POLARITIES] = {0, 0, 0, 0, 0}; 1429 int l_nb; 1430 unsigned int p, l, i; 1431 int ret; 1432 1433 ret = v4l2_fwnode_endpoint_alloc_parse(handle, &ep); 1434 if (ret) 1435 return -EINVAL; 1436 1437 l_nb = ep.bus.mipi_csi2.num_data_lanes; 1438 if (l_nb != 1 && l_nb != 2 && l_nb != 4) { 1439 dev_err(&client->dev, "invalid data lane number %d\n", l_nb); 1440 goto error_ep; 1441 } 1442 1443 /* Build log2phy, phy2log and polarities from ep info */ 1444 log2phy[0] = ep.bus.mipi_csi2.clock_lane; 1445 phy2log[log2phy[0]] = 0; 1446 for (l = 1; l < l_nb + 1; l++) { 1447 log2phy[l] = ep.bus.mipi_csi2.data_lanes[l - 1]; 1448 phy2log[log2phy[l]] = l; 1449 } 1450 /* 1451 * Then fill remaining slots for every physical slot to have something 1452 * valid for hardware stuff. 1453 */ 1454 for (p = 0; p < VGXY61_NB_POLARITIES; p++) { 1455 if (phy2log[p] != ~0) 1456 continue; 1457 phy2log[p] = l; 1458 log2phy[l] = p; 1459 l++; 1460 } 1461 for (l = 0; l < l_nb + 1; l++) 1462 polarities[l] = ep.bus.mipi_csi2.lane_polarities[l]; 1463 1464 if (log2phy[0] != 0) { 1465 dev_err(&client->dev, "clk lane must be map to physical lane 0\n"); 1466 goto error_ep; 1467 } 1468 sensor->oif_ctrl = (polarities[4] << 15) + ((phy2log[4] - 1) << 13) + 1469 (polarities[3] << 12) + ((phy2log[3] - 1) << 10) + 1470 (polarities[2] << 9) + ((phy2log[2] - 1) << 7) + 1471 (polarities[1] << 6) + ((phy2log[1] - 1) << 4) + 1472 (polarities[0] << 3) + 1473 l_nb; 1474 sensor->nb_of_lane = l_nb; 1475 1476 dev_dbg(&client->dev, "tx uses %d lanes", l_nb); 1477 for (i = 0; i < VGXY61_NB_POLARITIES; i++) { 1478 dev_dbg(&client->dev, "log2phy[%d] = %d\n", i, log2phy[i]); 1479 dev_dbg(&client->dev, "phy2log[%d] = %d\n", i, phy2log[i]); 1480 dev_dbg(&client->dev, "polarity[%d] = %d\n", i, polarities[i]); 1481 } 1482 dev_dbg(&client->dev, "oif_ctrl = 0x%04x\n", sensor->oif_ctrl); 1483 1484 v4l2_fwnode_endpoint_free(&ep); 1485 1486 return 0; 1487 1488error_ep: 1489 v4l2_fwnode_endpoint_free(&ep); 1490 1491 return -EINVAL; 1492} 1493 1494static int vgxy61_configure(struct vgxy61_dev *sensor) 1495{ 1496 u32 sensor_freq; 1497 u8 prediv, mult; 1498 u64 line_length; 1499 int ret = 0; 1500 1501 compute_pll_parameters_by_freq(sensor->clk_freq, &prediv, &mult); 1502 sensor_freq = (mult * sensor->clk_freq) / prediv; 1503 /* Frequency to data rate is 1:1 ratio for MIPI */ 1504 sensor->data_rate_in_mbps = sensor_freq; 1505 /* Video timing ISP path (pixel clock) requires 804/5 mhz = 160 mhz */ 1506 sensor->pclk = sensor_freq / 5; 1507 1508 cci_read(sensor->regmap, VGXY61_REG_LINE_LENGTH, &line_length, &ret); 1509 if (ret < 0) 1510 return ret; 1511 sensor->line_length = (u16)line_length; 1512 cci_write(sensor->regmap, VGXY61_REG_EXT_CLOCK, sensor->clk_freq, &ret); 1513 cci_write(sensor->regmap, VGXY61_REG_CLK_PLL_PREDIV, prediv, &ret); 1514 cci_write(sensor->regmap, VGXY61_REG_CLK_SYS_PLL_MULT, mult, &ret); 1515 cci_write(sensor->regmap, VGXY61_REG_OIF_CTRL, sensor->oif_ctrl, &ret); 1516 cci_write(sensor->regmap, VGXY61_REG_FRAME_CONTENT_CTRL, 0, &ret); 1517 cci_write(sensor->regmap, VGXY61_REG_BYPASS_CTRL, 4, &ret); 1518 if (ret) 1519 return ret; 1520 vgxy61_update_gpios_strobe_polarity(sensor, sensor->gpios_polarity); 1521 /* Set pattern generator solid to middle value */ 1522 cci_write(sensor->regmap, VGXY61_REG_PATGEN_LONG_DATA_GR, 0x800, &ret); 1523 cci_write(sensor->regmap, VGXY61_REG_PATGEN_LONG_DATA_R, 0x800, &ret); 1524 cci_write(sensor->regmap, VGXY61_REG_PATGEN_LONG_DATA_B, 0x800, &ret); 1525 cci_write(sensor->regmap, VGXY61_REG_PATGEN_LONG_DATA_GB, 0x800, &ret); 1526 cci_write(sensor->regmap, VGXY61_REG_PATGEN_SHORT_DATA_GR, 0x800, &ret); 1527 cci_write(sensor->regmap, VGXY61_REG_PATGEN_SHORT_DATA_R, 0x800, &ret); 1528 cci_write(sensor->regmap, VGXY61_REG_PATGEN_SHORT_DATA_B, 0x800, &ret); 1529 cci_write(sensor->regmap, VGXY61_REG_PATGEN_SHORT_DATA_GB, 0x800, &ret); 1530 if (ret) 1531 return ret; 1532 1533 return 0; 1534} 1535 1536static int vgxy61_patch(struct vgxy61_dev *sensor) 1537{ 1538 struct i2c_client *client = sensor->i2c_client; 1539 u64 patch; 1540 int ret; 1541 1542 ret = vgxy61_write_array(sensor, VGXY61_REG_FWPATCH_START_ADDR, 1543 sizeof(patch_array), patch_array); 1544 cci_write(sensor->regmap, VGXY61_REG_STBY, 0x10, &ret); 1545 if (ret) 1546 return ret; 1547 1548 ret = vgxy61_poll_reg(sensor, VGXY61_REG_STBY, 0, VGXY61_TIMEOUT_MS); 1549 cci_read(sensor->regmap, VGXY61_REG_FWPATCH_REVISION, &patch, &ret); 1550 if (ret < 0) 1551 return ret; 1552 1553 if (patch != (VGXY61_FWPATCH_REVISION_MAJOR << 12) + 1554 (VGXY61_FWPATCH_REVISION_MINOR << 8) + 1555 VGXY61_FWPATCH_REVISION_MICRO) { 1556 dev_err(&client->dev, 1557 "bad patch version expected %d.%d.%d got %u.%u.%u\n", 1558 VGXY61_FWPATCH_REVISION_MAJOR, 1559 VGXY61_FWPATCH_REVISION_MINOR, 1560 VGXY61_FWPATCH_REVISION_MICRO, 1561 (u16)patch >> 12, ((u16)patch >> 8) & 0x0f, (u16)patch & 0xff); 1562 return -ENODEV; 1563 } 1564 dev_dbg(&client->dev, "patch %u.%u.%u applied\n", 1565 (u16)patch >> 12, ((u16)patch >> 8) & 0x0f, (u16)patch & 0xff); 1566 1567 return 0; 1568} 1569 1570static int vgxy61_detect_cut_version(struct vgxy61_dev *sensor) 1571{ 1572 struct i2c_client *client = sensor->i2c_client; 1573 u64 device_rev; 1574 int ret; 1575 1576 ret = cci_read(sensor->regmap, VGXY61_REG_REVISION, &device_rev, NULL); 1577 if (ret < 0) 1578 return ret; 1579 1580 switch (device_rev >> 8) { 1581 case 0xA: 1582 dev_dbg(&client->dev, "Cut1 detected\n"); 1583 dev_err(&client->dev, "Cut1 not supported by this driver\n"); 1584 return -ENODEV; 1585 case 0xB: 1586 dev_dbg(&client->dev, "Cut2 detected\n"); 1587 return 0; 1588 case 0xC: 1589 dev_dbg(&client->dev, "Cut3 detected\n"); 1590 return 0; 1591 default: 1592 dev_err(&client->dev, "Unable to detect cut version\n"); 1593 return -ENODEV; 1594 } 1595} 1596 1597static int vgxy61_detect(struct vgxy61_dev *sensor) 1598{ 1599 struct i2c_client *client = sensor->i2c_client; 1600 u64 st, id = 0; 1601 int ret; 1602 1603 ret = cci_read(sensor->regmap, VGXY61_REG_MODEL_ID, &id, NULL); 1604 if (ret < 0) 1605 return ret; 1606 if (id != VG5661_MODEL_ID && id != VG5761_MODEL_ID) { 1607 dev_warn(&client->dev, "Unsupported sensor id %x\n", (u16)id); 1608 return -ENODEV; 1609 } 1610 dev_dbg(&client->dev, "detected sensor id = 0x%04x\n", (u16)id); 1611 sensor->id = id; 1612 1613 ret = vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_SW_STBY, 1614 VGXY61_TIMEOUT_MS); 1615 if (ret) 1616 return ret; 1617 1618 ret = cci_read(sensor->regmap, VGXY61_REG_NVM, &st, NULL); 1619 if (ret < 0) 1620 return st; 1621 if (st != VGXY61_NVM_OK) 1622 dev_warn(&client->dev, "Bad nvm state got %u\n", (u8)st); 1623 1624 ret = vgxy61_detect_cut_version(sensor); 1625 if (ret) 1626 return ret; 1627 1628 return 0; 1629} 1630 1631/* Power/clock management functions */ 1632static int vgxy61_power_on(struct device *dev) 1633{ 1634 struct i2c_client *client = to_i2c_client(dev); 1635 struct v4l2_subdev *sd = i2c_get_clientdata(client); 1636 struct vgxy61_dev *sensor = to_vgxy61_dev(sd); 1637 int ret; 1638 1639 ret = regulator_bulk_enable(ARRAY_SIZE(vgxy61_supply_name), 1640 sensor->supplies); 1641 if (ret) { 1642 dev_err(&client->dev, "failed to enable regulators %d\n", ret); 1643 return ret; 1644 } 1645 1646 ret = clk_prepare_enable(sensor->xclk); 1647 if (ret) { 1648 dev_err(&client->dev, "failed to enable clock %d\n", ret); 1649 goto disable_bulk; 1650 } 1651 1652 if (sensor->reset_gpio) { 1653 ret = vgxy61_apply_reset(sensor); 1654 if (ret) { 1655 dev_err(&client->dev, "sensor reset failed %d\n", ret); 1656 goto disable_clock; 1657 } 1658 } 1659 1660 ret = vgxy61_detect(sensor); 1661 if (ret) { 1662 dev_err(&client->dev, "sensor detect failed %d\n", ret); 1663 goto disable_clock; 1664 } 1665 1666 ret = vgxy61_patch(sensor); 1667 if (ret) { 1668 dev_err(&client->dev, "sensor patch failed %d\n", ret); 1669 goto disable_clock; 1670 } 1671 1672 ret = vgxy61_configure(sensor); 1673 if (ret) { 1674 dev_err(&client->dev, "sensor configuration failed %d\n", ret); 1675 goto disable_clock; 1676 } 1677 1678 return 0; 1679 1680disable_clock: 1681 clk_disable_unprepare(sensor->xclk); 1682disable_bulk: 1683 regulator_bulk_disable(ARRAY_SIZE(vgxy61_supply_name), 1684 sensor->supplies); 1685 1686 return ret; 1687} 1688 1689static int vgxy61_power_off(struct device *dev) 1690{ 1691 struct i2c_client *client = to_i2c_client(dev); 1692 struct v4l2_subdev *sd = i2c_get_clientdata(client); 1693 struct vgxy61_dev *sensor = to_vgxy61_dev(sd); 1694 1695 clk_disable_unprepare(sensor->xclk); 1696 regulator_bulk_disable(ARRAY_SIZE(vgxy61_supply_name), 1697 sensor->supplies); 1698 return 0; 1699} 1700 1701static void vgxy61_fill_sensor_param(struct vgxy61_dev *sensor) 1702{ 1703 if (sensor->id == VG5761_MODEL_ID) { 1704 sensor->sensor_width = VGX761_WIDTH; 1705 sensor->sensor_height = VGX761_HEIGHT; 1706 sensor->sensor_modes = vgx761_mode_data; 1707 sensor->sensor_modes_nb = ARRAY_SIZE(vgx761_mode_data); 1708 sensor->default_mode = &vgx761_mode_data[VGX761_DEFAULT_MODE]; 1709 sensor->rot_term = VGX761_SHORT_ROT_TERM; 1710 } else if (sensor->id == VG5661_MODEL_ID) { 1711 sensor->sensor_width = VGX661_WIDTH; 1712 sensor->sensor_height = VGX661_HEIGHT; 1713 sensor->sensor_modes = vgx661_mode_data; 1714 sensor->sensor_modes_nb = ARRAY_SIZE(vgx661_mode_data); 1715 sensor->default_mode = &vgx661_mode_data[VGX661_DEFAULT_MODE]; 1716 sensor->rot_term = VGX661_SHORT_ROT_TERM; 1717 } else { 1718 /* Should never happen */ 1719 WARN_ON(true); 1720 } 1721 sensor->current_mode = sensor->default_mode; 1722} 1723 1724static int vgxy61_probe(struct i2c_client *client) 1725{ 1726 struct device *dev = &client->dev; 1727 struct fwnode_handle *handle; 1728 struct vgxy61_dev *sensor; 1729 int ret; 1730 1731 sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL); 1732 if (!sensor) 1733 return -ENOMEM; 1734 1735 sensor->i2c_client = client; 1736 sensor->streaming = false; 1737 sensor->hdr = VGXY61_NO_HDR; 1738 sensor->expo_long = 200; 1739 sensor->expo_short = 0; 1740 sensor->hflip = false; 1741 sensor->vflip = false; 1742 sensor->analog_gain = 0; 1743 sensor->digital_gain = 256; 1744 1745 sensor->regmap = devm_cci_regmap_init_i2c(client, 16); 1746 if (IS_ERR(sensor->regmap)) { 1747 ret = PTR_ERR(sensor->regmap); 1748 return dev_err_probe(dev, ret, "Failed to init regmap\n"); 1749 } 1750 1751 handle = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0, 0); 1752 if (!handle) { 1753 dev_err(dev, "handle node not found\n"); 1754 return -EINVAL; 1755 } 1756 1757 ret = vgxy61_tx_from_ep(sensor, handle); 1758 fwnode_handle_put(handle); 1759 if (ret) { 1760 dev_err(dev, "Failed to parse handle %d\n", ret); 1761 return ret; 1762 } 1763 1764 sensor->xclk = devm_clk_get(dev, NULL); 1765 if (IS_ERR(sensor->xclk)) { 1766 dev_err(dev, "failed to get xclk\n"); 1767 return PTR_ERR(sensor->xclk); 1768 } 1769 sensor->clk_freq = clk_get_rate(sensor->xclk); 1770 if (sensor->clk_freq < 6 * HZ_PER_MHZ || 1771 sensor->clk_freq > 27 * HZ_PER_MHZ) { 1772 dev_err(dev, "Only 6Mhz-27Mhz clock range supported. provide %lu MHz\n", 1773 sensor->clk_freq / HZ_PER_MHZ); 1774 return -EINVAL; 1775 } 1776 sensor->gpios_polarity = 1777 device_property_read_bool(dev, "st,strobe-gpios-polarity"); 1778 1779 v4l2_i2c_subdev_init(&sensor->sd, client, &vgxy61_subdev_ops); 1780 sensor->sd.internal_ops = &vgxy61_internal_ops; 1781 sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | 1782 V4L2_SUBDEV_FL_HAS_EVENTS; 1783 sensor->pad.flags = MEDIA_PAD_FL_SOURCE; 1784 sensor->sd.entity.ops = &vgxy61_subdev_entity_ops; 1785 sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR; 1786 1787 sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset", 1788 GPIOD_OUT_HIGH); 1789 1790 ret = vgxy61_get_regulators(sensor); 1791 if (ret) { 1792 dev_err(&client->dev, "failed to get regulators %d\n", ret); 1793 return ret; 1794 } 1795 1796 ret = vgxy61_power_on(dev); 1797 if (ret) 1798 return ret; 1799 1800 vgxy61_fill_sensor_param(sensor); 1801 vgxy61_fill_framefmt(sensor, sensor->current_mode, &sensor->fmt, 1802 VGXY61_MEDIA_BUS_FMT_DEF); 1803 1804 mutex_init(&sensor->lock); 1805 1806 ret = vgxy61_update_hdr(sensor, sensor->hdr); 1807 if (ret) 1808 goto error_power_off; 1809 1810 ret = vgxy61_init_controls(sensor); 1811 if (ret) { 1812 dev_err(&client->dev, "controls initialization failed %d\n", 1813 ret); 1814 goto error_power_off; 1815 } 1816 1817 ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad); 1818 if (ret) { 1819 dev_err(&client->dev, "pads init failed %d\n", ret); 1820 goto error_handler_free; 1821 } 1822 1823 /* Enable runtime PM and turn off the device */ 1824 pm_runtime_set_active(dev); 1825 pm_runtime_enable(dev); 1826 pm_runtime_idle(dev); 1827 1828 ret = v4l2_async_register_subdev(&sensor->sd); 1829 if (ret) { 1830 dev_err(&client->dev, "async subdev register failed %d\n", ret); 1831 goto error_pm_runtime; 1832 } 1833 1834 pm_runtime_set_autosuspend_delay(&client->dev, 1000); 1835 pm_runtime_use_autosuspend(&client->dev); 1836 1837 dev_dbg(&client->dev, "vgxy61 probe successfully\n"); 1838 1839 return 0; 1840 1841error_pm_runtime: 1842 pm_runtime_disable(&client->dev); 1843 pm_runtime_set_suspended(&client->dev); 1844 media_entity_cleanup(&sensor->sd.entity); 1845error_handler_free: 1846 v4l2_ctrl_handler_free(sensor->sd.ctrl_handler); 1847error_power_off: 1848 mutex_destroy(&sensor->lock); 1849 vgxy61_power_off(dev); 1850 1851 return ret; 1852} 1853 1854static void vgxy61_remove(struct i2c_client *client) 1855{ 1856 struct v4l2_subdev *sd = i2c_get_clientdata(client); 1857 struct vgxy61_dev *sensor = to_vgxy61_dev(sd); 1858 1859 v4l2_async_unregister_subdev(&sensor->sd); 1860 mutex_destroy(&sensor->lock); 1861 media_entity_cleanup(&sensor->sd.entity); 1862 1863 pm_runtime_disable(&client->dev); 1864 if (!pm_runtime_status_suspended(&client->dev)) 1865 vgxy61_power_off(&client->dev); 1866 pm_runtime_set_suspended(&client->dev); 1867} 1868 1869static const struct of_device_id vgxy61_dt_ids[] = { 1870 { .compatible = "st,st-vgxy61" }, 1871 { /* sentinel */ } 1872}; 1873MODULE_DEVICE_TABLE(of, vgxy61_dt_ids); 1874 1875static const struct dev_pm_ops vgxy61_pm_ops = { 1876 SET_RUNTIME_PM_OPS(vgxy61_power_off, vgxy61_power_on, NULL) 1877}; 1878 1879static struct i2c_driver vgxy61_i2c_driver = { 1880 .driver = { 1881 .name = "st-vgxy61", 1882 .of_match_table = vgxy61_dt_ids, 1883 .pm = &vgxy61_pm_ops, 1884 }, 1885 .probe = vgxy61_probe, 1886 .remove = vgxy61_remove, 1887}; 1888 1889module_i2c_driver(vgxy61_i2c_driver); 1890 1891MODULE_AUTHOR("Benjamin Mugnier <benjamin.mugnier@foss.st.com>"); 1892MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>"); 1893MODULE_AUTHOR("Sylvain Petinot <sylvain.petinot@foss.st.com>"); 1894MODULE_DESCRIPTION("VGXY61 camera subdev driver"); 1895MODULE_LICENSE("GPL"); 1896