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