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 115 static 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 186 static 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 197 static const char * const vgxy61_hdr_mode_menu[] = { 198 "HDR linearize", 199 "HDR substraction", 200 "No HDR", 201 }; 202 203 static const char * const vgxy61_supply_name[] = { 204 "VCORE", 205 "VDDIO", 206 "VANA", 207 }; 208 209 static 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 217 enum vgxy61_bin_mode { 218 VGXY61_BIN_MODE_NORMAL, 219 VGXY61_BIN_MODE_DIGITAL_X2, 220 VGXY61_BIN_MODE_DIGITAL_X4, 221 }; 222 223 enum vgxy61_hdr_mode { 224 VGXY61_HDR_LINEAR, 225 VGXY61_HDR_SUB, 226 VGXY61_NO_HDR, 227 }; 228 229 enum vgxy61_strobe_mode { 230 VGXY61_STROBE_DISABLED, 231 VGXY61_STROBE_LONG, 232 VGXY61_STROBE_ENABLED, 233 }; 234 235 struct vgxy61_mode_info { 236 u32 width; 237 u32 height; 238 enum vgxy61_bin_mode bin_mode; 239 struct v4l2_rect crop; 240 }; 241 242 struct vgxy61_fmt_desc { 243 u32 code; 244 u8 bpp; 245 u8 data_type; 246 }; 247 248 static 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 276 static 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 323 static 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 381 struct 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 430 static 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 443 static 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 457 static 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 480 static 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 486 static inline struct vgxy61_dev *to_vgxy61_dev(struct v4l2_subdev *sd) 487 { 488 return container_of(sd, struct vgxy61_dev, sd); 489 } 490 491 static 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 497 static 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 510 static 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 531 static 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 544 static 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 551 static 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 575 static 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 589 static 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 601 static 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 613 static 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 627 static 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 654 static 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 677 static 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 689 static 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 710 static 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 723 static 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 740 static 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 750 static 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 768 static 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 777 static 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 791 static 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 800 static 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 814 static 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 848 static 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 866 static 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 899 static 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 971 static 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 977 static 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 997 static 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 1006 static 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 1032 static 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 1076 static 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 1131 err_rpm_put: 1132 pm_runtime_put(&client->dev); 1133 return ret; 1134 } 1135 1136 static 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 1159 err_str_dis: 1160 if (ret) 1161 WARN(1, "Can't disable stream"); 1162 pm_runtime_put(&client->dev); 1163 1164 return ret; 1165 } 1166 1167 static 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 1184 static 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 1199 static 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 1248 out: 1249 mutex_unlock(&sensor->lock); 1250 1251 return ret; 1252 } 1253 1254 static 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 1266 static 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 1325 static const struct v4l2_ctrl_ops vgxy61_ctrl_ops = { 1326 .s_ctrl = vgxy61_s_ctrl, 1327 }; 1328 1329 static 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 1400 free_ctrls: 1401 v4l2_ctrl_handler_free(hdl); 1402 return ret; 1403 } 1404 1405 static 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 1410 static const struct v4l2_subdev_video_ops vgxy61_video_ops = { 1411 .s_stream = vgxy61_s_stream, 1412 }; 1413 1414 static 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 1423 static 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 1429 static const struct v4l2_subdev_internal_ops vgxy61_internal_ops = { 1430 .init_state = vgxy61_init_state, 1431 }; 1432 1433 static const struct media_entity_operations vgxy61_subdev_entity_ops = { 1434 .link_validate = v4l2_subdev_link_validate, 1435 }; 1436 1437 static 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 1504 error_ep: 1505 v4l2_fwnode_endpoint_free(&ep); 1506 1507 return -EINVAL; 1508 } 1509 1510 static 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 1552 static 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 1586 static 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 1613 static 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 */ 1648 static 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 1696 disable_clock: 1697 clk_disable_unprepare(sensor->xclk); 1698 disable_bulk: 1699 regulator_bulk_disable(ARRAY_SIZE(vgxy61_supply_name), 1700 sensor->supplies); 1701 1702 return ret; 1703 } 1704 1705 static 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 1717 static 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 1740 static 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 1857 error_pm_runtime: 1858 pm_runtime_disable(&client->dev); 1859 pm_runtime_set_suspended(&client->dev); 1860 media_entity_cleanup(&sensor->sd.entity); 1861 error_handler_free: 1862 v4l2_ctrl_handler_free(sensor->sd.ctrl_handler); 1863 error_power_off: 1864 mutex_destroy(&sensor->lock); 1865 vgxy61_power_off(dev); 1866 1867 return ret; 1868 } 1869 1870 static 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 1885 static const struct of_device_id vgxy61_dt_ids[] = { 1886 { .compatible = "st,st-vgxy61" }, 1887 { /* sentinel */ } 1888 }; 1889 MODULE_DEVICE_TABLE(of, vgxy61_dt_ids); 1890 1891 static const struct dev_pm_ops vgxy61_pm_ops = { 1892 SET_RUNTIME_PM_OPS(vgxy61_power_off, vgxy61_power_on, NULL) 1893 }; 1894 1895 static 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 1905 module_i2c_driver(vgxy61_i2c_driver); 1906 1907 MODULE_AUTHOR("Benjamin Mugnier <benjamin.mugnier@foss.st.com>"); 1908 MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>"); 1909 MODULE_AUTHOR("Sylvain Petinot <sylvain.petinot@foss.st.com>"); 1910 MODULE_DESCRIPTION("VGXY61 camera subdev driver"); 1911 MODULE_LICENSE("GPL"); 1912