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 vgxy61_fill_framefmt(sensor, sensor->current_mode, &fmt.format, 1338 VGXY61_MEDIA_BUS_FMT_DEF); 1339 1340 return vgxy61_set_fmt(sd, sd_state, &fmt); 1341 } 1342 1343 static int vgxy61_s_ctrl(struct v4l2_ctrl *ctrl) 1344 { 1345 struct v4l2_subdev *sd = ctrl_to_sd(ctrl); 1346 struct vgxy61_dev *sensor = to_vgxy61_dev(sd); 1347 const struct vgxy61_mode_info *cur_mode = sensor->current_mode; 1348 int ret; 1349 1350 switch (ctrl->id) { 1351 case V4L2_CID_EXPOSURE: 1352 ret = vgxy61_update_exposure(sensor, ctrl->val, sensor->hdr); 1353 ctrl->val = sensor->expo_long; 1354 break; 1355 case V4L2_CID_ANALOGUE_GAIN: 1356 ret = vgxy61_update_analog_gain(sensor, ctrl->val); 1357 break; 1358 case V4L2_CID_DIGITAL_GAIN: 1359 ret = vgxy61_update_digital_gain(sensor, ctrl->val); 1360 break; 1361 case V4L2_CID_VFLIP: 1362 case V4L2_CID_HFLIP: 1363 if (sensor->streaming) { 1364 ret = -EBUSY; 1365 break; 1366 } 1367 if (ctrl->id == V4L2_CID_VFLIP) 1368 sensor->vflip = ctrl->val; 1369 if (ctrl->id == V4L2_CID_HFLIP) 1370 sensor->hflip = ctrl->val; 1371 ret = 0; 1372 break; 1373 case V4L2_CID_TEST_PATTERN: 1374 ret = vgxy61_update_patgen(sensor, ctrl->val); 1375 break; 1376 case V4L2_CID_HDR_SENSOR_MODE: 1377 ret = vgxy61_update_hdr(sensor, ctrl->val); 1378 /* Update vblank and exposure controls to match new hdr */ 1379 __v4l2_ctrl_modify_range(sensor->vblank_ctrl, 1380 sensor->vblank_min, 1381 0xffff - cur_mode->crop.height, 1382 1, sensor->vblank); 1383 __v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min, 1384 sensor->expo_max, 1, 1385 sensor->expo_long); 1386 break; 1387 case V4L2_CID_VBLANK: 1388 ret = vgxy61_update_vblank(sensor, ctrl->val, sensor->hdr); 1389 /* Update exposure control to match new vblank */ 1390 __v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min, 1391 sensor->expo_max, 1, 1392 sensor->expo_long); 1393 break; 1394 default: 1395 ret = -EINVAL; 1396 break; 1397 } 1398 1399 return ret; 1400 } 1401 1402 static const struct v4l2_ctrl_ops vgxy61_ctrl_ops = { 1403 .s_ctrl = vgxy61_s_ctrl, 1404 }; 1405 1406 static int vgxy61_init_controls(struct vgxy61_dev *sensor) 1407 { 1408 const struct v4l2_ctrl_ops *ops = &vgxy61_ctrl_ops; 1409 struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler; 1410 const struct vgxy61_mode_info *cur_mode = sensor->current_mode; 1411 struct v4l2_ctrl *ctrl; 1412 int ret; 1413 1414 v4l2_ctrl_handler_init(hdl, 16); 1415 /* We can use our own mutex for the ctrl lock */ 1416 hdl->lock = &sensor->lock; 1417 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN, 0, 0x1c, 1, 1418 sensor->analog_gain); 1419 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN, 0, 0xfff, 1, 1420 sensor->digital_gain); 1421 v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN, 1422 ARRAY_SIZE(vgxy61_test_pattern_menu) - 1, 1423 0, 0, vgxy61_test_pattern_menu); 1424 ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 0, 1425 sensor->line_length, 1, 1426 sensor->line_length - cur_mode->width); 1427 if (ctrl) 1428 ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY; 1429 ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ, 1430 ARRAY_SIZE(link_freq) - 1, 0, link_freq); 1431 if (ctrl) 1432 ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY; 1433 v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_HDR_SENSOR_MODE, 1434 ARRAY_SIZE(vgxy61_hdr_mode_menu) - 1, 0, 1435 VGXY61_NO_HDR, vgxy61_hdr_mode_menu); 1436 1437 /* 1438 * Keep a pointer to these controls as we need to update them when 1439 * setting the format 1440 */ 1441 sensor->pixel_rate_ctrl = v4l2_ctrl_new_std(hdl, ops, 1442 V4L2_CID_PIXEL_RATE, 1, 1443 INT_MAX, 1, 1444 get_pixel_rate(sensor)); 1445 if (sensor->pixel_rate_ctrl) 1446 sensor->pixel_rate_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY; 1447 sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE, 1448 sensor->expo_min, 1449 sensor->expo_max, 1, 1450 sensor->expo_long); 1451 sensor->vblank_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, 1452 sensor->vblank_min, 1453 0xffff - cur_mode->crop.height, 1454 1, sensor->vblank); 1455 sensor->vflip_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 1456 0, 1, 1, sensor->vflip); 1457 sensor->hflip_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 1458 0, 1, 1, sensor->hflip); 1459 1460 if (hdl->error) { 1461 ret = hdl->error; 1462 goto free_ctrls; 1463 } 1464 1465 sensor->sd.ctrl_handler = hdl; 1466 return 0; 1467 1468 free_ctrls: 1469 v4l2_ctrl_handler_free(hdl); 1470 return ret; 1471 } 1472 1473 static const struct v4l2_subdev_video_ops vgxy61_video_ops = { 1474 .s_stream = vgxy61_s_stream, 1475 }; 1476 1477 static const struct v4l2_subdev_pad_ops vgxy61_pad_ops = { 1478 .init_cfg = vgxy61_init_cfg, 1479 .enum_mbus_code = vgxy61_enum_mbus_code, 1480 .get_fmt = vgxy61_get_fmt, 1481 .set_fmt = vgxy61_set_fmt, 1482 .get_selection = vgxy61_get_selection, 1483 .enum_frame_size = vgxy61_enum_frame_size, 1484 }; 1485 1486 static const struct v4l2_subdev_ops vgxy61_subdev_ops = { 1487 .video = &vgxy61_video_ops, 1488 .pad = &vgxy61_pad_ops, 1489 }; 1490 1491 static const struct media_entity_operations vgxy61_subdev_entity_ops = { 1492 .link_validate = v4l2_subdev_link_validate, 1493 }; 1494 1495 static int vgxy61_tx_from_ep(struct vgxy61_dev *sensor, 1496 struct fwnode_handle *handle) 1497 { 1498 struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY }; 1499 struct i2c_client *client = sensor->i2c_client; 1500 u32 log2phy[VGXY61_NB_POLARITIES] = {~0, ~0, ~0, ~0, ~0}; 1501 u32 phy2log[VGXY61_NB_POLARITIES] = {~0, ~0, ~0, ~0, ~0}; 1502 int polarities[VGXY61_NB_POLARITIES] = {0, 0, 0, 0, 0}; 1503 int l_nb; 1504 unsigned int p, l, i; 1505 int ret; 1506 1507 ret = v4l2_fwnode_endpoint_alloc_parse(handle, &ep); 1508 if (ret) 1509 return -EINVAL; 1510 1511 l_nb = ep.bus.mipi_csi2.num_data_lanes; 1512 if (l_nb != 1 && l_nb != 2 && l_nb != 4) { 1513 dev_err(&client->dev, "invalid data lane number %d\n", l_nb); 1514 goto error_ep; 1515 } 1516 1517 /* Build log2phy, phy2log and polarities from ep info */ 1518 log2phy[0] = ep.bus.mipi_csi2.clock_lane; 1519 phy2log[log2phy[0]] = 0; 1520 for (l = 1; l < l_nb + 1; l++) { 1521 log2phy[l] = ep.bus.mipi_csi2.data_lanes[l - 1]; 1522 phy2log[log2phy[l]] = l; 1523 } 1524 /* 1525 * Then fill remaining slots for every physical slot to have something 1526 * valid for hardware stuff. 1527 */ 1528 for (p = 0; p < VGXY61_NB_POLARITIES; p++) { 1529 if (phy2log[p] != ~0) 1530 continue; 1531 phy2log[p] = l; 1532 log2phy[l] = p; 1533 l++; 1534 } 1535 for (l = 0; l < l_nb + 1; l++) 1536 polarities[l] = ep.bus.mipi_csi2.lane_polarities[l]; 1537 1538 if (log2phy[0] != 0) { 1539 dev_err(&client->dev, "clk lane must be map to physical lane 0\n"); 1540 goto error_ep; 1541 } 1542 sensor->oif_ctrl = (polarities[4] << 15) + ((phy2log[4] - 1) << 13) + 1543 (polarities[3] << 12) + ((phy2log[3] - 1) << 10) + 1544 (polarities[2] << 9) + ((phy2log[2] - 1) << 7) + 1545 (polarities[1] << 6) + ((phy2log[1] - 1) << 4) + 1546 (polarities[0] << 3) + 1547 l_nb; 1548 sensor->nb_of_lane = l_nb; 1549 1550 dev_dbg(&client->dev, "tx uses %d lanes", l_nb); 1551 for (i = 0; i < VGXY61_NB_POLARITIES; i++) { 1552 dev_dbg(&client->dev, "log2phy[%d] = %d\n", i, log2phy[i]); 1553 dev_dbg(&client->dev, "phy2log[%d] = %d\n", i, phy2log[i]); 1554 dev_dbg(&client->dev, "polarity[%d] = %d\n", i, polarities[i]); 1555 } 1556 dev_dbg(&client->dev, "oif_ctrl = 0x%04x\n", sensor->oif_ctrl); 1557 1558 v4l2_fwnode_endpoint_free(&ep); 1559 1560 return 0; 1561 1562 error_ep: 1563 v4l2_fwnode_endpoint_free(&ep); 1564 1565 return -EINVAL; 1566 } 1567 1568 static int vgxy61_configure(struct vgxy61_dev *sensor) 1569 { 1570 u32 sensor_freq; 1571 u8 prediv, mult; 1572 int line_length; 1573 int ret = 0; 1574 1575 compute_pll_parameters_by_freq(sensor->clk_freq, &prediv, &mult); 1576 sensor_freq = (mult * sensor->clk_freq) / prediv; 1577 /* Frequency to data rate is 1:1 ratio for MIPI */ 1578 sensor->data_rate_in_mbps = sensor_freq; 1579 /* Video timing ISP path (pixel clock) requires 804/5 mhz = 160 mhz */ 1580 sensor->pclk = sensor_freq / 5; 1581 1582 line_length = vgxy61_read_reg(sensor, VGXY61_REG_LINE_LENGTH); 1583 if (line_length < 0) 1584 return line_length; 1585 sensor->line_length = line_length; 1586 vgxy61_write_reg(sensor, VGXY61_REG_EXT_CLOCK, sensor->clk_freq, &ret); 1587 vgxy61_write_reg(sensor, VGXY61_REG_CLK_PLL_PREDIV, prediv, &ret); 1588 vgxy61_write_reg(sensor, VGXY61_REG_CLK_SYS_PLL_MULT, mult, &ret); 1589 vgxy61_write_reg(sensor, VGXY61_REG_OIF_CTRL, sensor->oif_ctrl, &ret); 1590 vgxy61_write_reg(sensor, VGXY61_REG_FRAME_CONTENT_CTRL, 0, &ret); 1591 vgxy61_write_reg(sensor, VGXY61_REG_BYPASS_CTRL, 4, &ret); 1592 if (ret) 1593 return ret; 1594 vgxy61_update_gpios_strobe_polarity(sensor, sensor->gpios_polarity); 1595 /* Set pattern generator solid to middle value */ 1596 vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_LONG_DATA_GR, 0x800, &ret); 1597 vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_LONG_DATA_R, 0x800, &ret); 1598 vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_LONG_DATA_B, 0x800, &ret); 1599 vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_LONG_DATA_GB, 0x800, &ret); 1600 vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_SHORT_DATA_GR, 0x800, &ret); 1601 vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_SHORT_DATA_R, 0x800, &ret); 1602 vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_SHORT_DATA_B, 0x800, &ret); 1603 vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_SHORT_DATA_GB, 0x800, &ret); 1604 if (ret) 1605 return ret; 1606 1607 return 0; 1608 } 1609 1610 static int vgxy61_patch(struct vgxy61_dev *sensor) 1611 { 1612 struct i2c_client *client = sensor->i2c_client; 1613 int patch, ret; 1614 1615 ret = vgxy61_write_array(sensor, VGXY61_REG_FWPATCH_START_ADDR, 1616 sizeof(patch_array), patch_array); 1617 if (ret) 1618 return ret; 1619 1620 ret = vgxy61_write_reg(sensor, VGXY61_REG_STBY, 0x10, NULL); 1621 if (ret) 1622 return ret; 1623 1624 ret = vgxy61_poll_reg(sensor, VGXY61_REG_STBY, 0, VGXY61_TIMEOUT_MS); 1625 if (ret) 1626 return ret; 1627 1628 patch = vgxy61_read_reg(sensor, VGXY61_REG_FWPATCH_REVISION); 1629 if (patch < 0) 1630 return patch; 1631 1632 if (patch != (VGXY61_FWPATCH_REVISION_MAJOR << 12) + 1633 (VGXY61_FWPATCH_REVISION_MINOR << 8) + 1634 VGXY61_FWPATCH_REVISION_MICRO) { 1635 dev_err(&client->dev, "bad patch version expected %d.%d.%d got %d.%d.%d\n", 1636 VGXY61_FWPATCH_REVISION_MAJOR, 1637 VGXY61_FWPATCH_REVISION_MINOR, 1638 VGXY61_FWPATCH_REVISION_MICRO, 1639 patch >> 12, (patch >> 8) & 0x0f, patch & 0xff); 1640 return -ENODEV; 1641 } 1642 dev_dbg(&client->dev, "patch %d.%d.%d applied\n", 1643 patch >> 12, (patch >> 8) & 0x0f, patch & 0xff); 1644 1645 return 0; 1646 } 1647 1648 static int vgxy61_detect_cut_version(struct vgxy61_dev *sensor) 1649 { 1650 struct i2c_client *client = sensor->i2c_client; 1651 int device_rev; 1652 1653 device_rev = vgxy61_read_reg(sensor, VGXY61_REG_REVISION); 1654 if (device_rev < 0) 1655 return device_rev; 1656 1657 switch (device_rev >> 8) { 1658 case 0xA: 1659 dev_dbg(&client->dev, "Cut1 detected\n"); 1660 dev_err(&client->dev, "Cut1 not supported by this driver\n"); 1661 return -ENODEV; 1662 case 0xB: 1663 dev_dbg(&client->dev, "Cut2 detected\n"); 1664 return 0; 1665 case 0xC: 1666 dev_dbg(&client->dev, "Cut3 detected\n"); 1667 return 0; 1668 default: 1669 dev_err(&client->dev, "Unable to detect cut version\n"); 1670 return -ENODEV; 1671 } 1672 } 1673 1674 static int vgxy61_detect(struct vgxy61_dev *sensor) 1675 { 1676 struct i2c_client *client = sensor->i2c_client; 1677 int id = 0; 1678 int ret, st; 1679 1680 id = vgxy61_read_reg(sensor, VGXY61_REG_MODEL_ID); 1681 if (id < 0) 1682 return id; 1683 if (id != VG5661_MODEL_ID && id != VG5761_MODEL_ID) { 1684 dev_warn(&client->dev, "Unsupported sensor id %x\n", id); 1685 return -ENODEV; 1686 } 1687 dev_dbg(&client->dev, "detected sensor id = 0x%04x\n", id); 1688 sensor->id = id; 1689 1690 ret = vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_SW_STBY, 1691 VGXY61_TIMEOUT_MS); 1692 if (ret) 1693 return ret; 1694 1695 st = vgxy61_read_reg(sensor, VGXY61_REG_NVM); 1696 if (st < 0) 1697 return st; 1698 if (st != VGXY61_NVM_OK) 1699 dev_warn(&client->dev, "Bad nvm state got %d\n", st); 1700 1701 ret = vgxy61_detect_cut_version(sensor); 1702 if (ret) 1703 return ret; 1704 1705 return 0; 1706 } 1707 1708 /* Power/clock management functions */ 1709 static int vgxy61_power_on(struct device *dev) 1710 { 1711 struct i2c_client *client = to_i2c_client(dev); 1712 struct v4l2_subdev *sd = i2c_get_clientdata(client); 1713 struct vgxy61_dev *sensor = to_vgxy61_dev(sd); 1714 int ret; 1715 1716 ret = regulator_bulk_enable(ARRAY_SIZE(vgxy61_supply_name), 1717 sensor->supplies); 1718 if (ret) { 1719 dev_err(&client->dev, "failed to enable regulators %d\n", ret); 1720 return ret; 1721 } 1722 1723 ret = clk_prepare_enable(sensor->xclk); 1724 if (ret) { 1725 dev_err(&client->dev, "failed to enable clock %d\n", ret); 1726 goto disable_bulk; 1727 } 1728 1729 if (sensor->reset_gpio) { 1730 ret = vgxy61_apply_reset(sensor); 1731 if (ret) { 1732 dev_err(&client->dev, "sensor reset failed %d\n", ret); 1733 goto disable_clock; 1734 } 1735 } 1736 1737 ret = vgxy61_detect(sensor); 1738 if (ret) { 1739 dev_err(&client->dev, "sensor detect failed %d\n", ret); 1740 goto disable_clock; 1741 } 1742 1743 ret = vgxy61_patch(sensor); 1744 if (ret) { 1745 dev_err(&client->dev, "sensor patch failed %d\n", ret); 1746 goto disable_clock; 1747 } 1748 1749 ret = vgxy61_configure(sensor); 1750 if (ret) { 1751 dev_err(&client->dev, "sensor configuration failed %d\n", ret); 1752 goto disable_clock; 1753 } 1754 1755 return 0; 1756 1757 disable_clock: 1758 clk_disable_unprepare(sensor->xclk); 1759 disable_bulk: 1760 regulator_bulk_disable(ARRAY_SIZE(vgxy61_supply_name), 1761 sensor->supplies); 1762 1763 return ret; 1764 } 1765 1766 static int vgxy61_power_off(struct device *dev) 1767 { 1768 struct i2c_client *client = to_i2c_client(dev); 1769 struct v4l2_subdev *sd = i2c_get_clientdata(client); 1770 struct vgxy61_dev *sensor = to_vgxy61_dev(sd); 1771 1772 clk_disable_unprepare(sensor->xclk); 1773 regulator_bulk_disable(ARRAY_SIZE(vgxy61_supply_name), 1774 sensor->supplies); 1775 return 0; 1776 } 1777 1778 static void vgxy61_fill_sensor_param(struct vgxy61_dev *sensor) 1779 { 1780 if (sensor->id == VG5761_MODEL_ID) { 1781 sensor->sensor_width = VGX761_WIDTH; 1782 sensor->sensor_height = VGX761_HEIGHT; 1783 sensor->sensor_modes = vgx761_mode_data; 1784 sensor->sensor_modes_nb = ARRAY_SIZE(vgx761_mode_data); 1785 sensor->default_mode = &vgx761_mode_data[VGX761_DEFAULT_MODE]; 1786 sensor->rot_term = VGX761_SHORT_ROT_TERM; 1787 } else if (sensor->id == VG5661_MODEL_ID) { 1788 sensor->sensor_width = VGX661_WIDTH; 1789 sensor->sensor_height = VGX661_HEIGHT; 1790 sensor->sensor_modes = vgx661_mode_data; 1791 sensor->sensor_modes_nb = ARRAY_SIZE(vgx661_mode_data); 1792 sensor->default_mode = &vgx661_mode_data[VGX661_DEFAULT_MODE]; 1793 sensor->rot_term = VGX661_SHORT_ROT_TERM; 1794 } else { 1795 /* Should never happen */ 1796 WARN_ON(true); 1797 } 1798 sensor->current_mode = sensor->default_mode; 1799 } 1800 1801 static int vgxy61_probe(struct i2c_client *client) 1802 { 1803 struct device *dev = &client->dev; 1804 struct fwnode_handle *handle; 1805 struct vgxy61_dev *sensor; 1806 int ret; 1807 1808 sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL); 1809 if (!sensor) 1810 return -ENOMEM; 1811 1812 sensor->i2c_client = client; 1813 sensor->streaming = false; 1814 sensor->hdr = VGXY61_NO_HDR; 1815 sensor->expo_long = 200; 1816 sensor->expo_short = 0; 1817 sensor->hflip = false; 1818 sensor->vflip = false; 1819 sensor->analog_gain = 0; 1820 sensor->digital_gain = 256; 1821 1822 handle = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0, 0); 1823 if (!handle) { 1824 dev_err(dev, "handle node not found\n"); 1825 return -EINVAL; 1826 } 1827 1828 ret = vgxy61_tx_from_ep(sensor, handle); 1829 fwnode_handle_put(handle); 1830 if (ret) { 1831 dev_err(dev, "Failed to parse handle %d\n", ret); 1832 return ret; 1833 } 1834 1835 sensor->xclk = devm_clk_get(dev, NULL); 1836 if (IS_ERR(sensor->xclk)) { 1837 dev_err(dev, "failed to get xclk\n"); 1838 return PTR_ERR(sensor->xclk); 1839 } 1840 sensor->clk_freq = clk_get_rate(sensor->xclk); 1841 if (sensor->clk_freq < 6 * HZ_PER_MHZ || 1842 sensor->clk_freq > 27 * HZ_PER_MHZ) { 1843 dev_err(dev, "Only 6Mhz-27Mhz clock range supported. provide %lu MHz\n", 1844 sensor->clk_freq / HZ_PER_MHZ); 1845 return -EINVAL; 1846 } 1847 sensor->gpios_polarity = 1848 device_property_read_bool(dev, "st,strobe-gpios-polarity"); 1849 1850 v4l2_i2c_subdev_init(&sensor->sd, client, &vgxy61_subdev_ops); 1851 sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; 1852 sensor->pad.flags = MEDIA_PAD_FL_SOURCE; 1853 sensor->sd.entity.ops = &vgxy61_subdev_entity_ops; 1854 sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR; 1855 1856 sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset", 1857 GPIOD_OUT_HIGH); 1858 1859 ret = vgxy61_get_regulators(sensor); 1860 if (ret) { 1861 dev_err(&client->dev, "failed to get regulators %d\n", ret); 1862 return ret; 1863 } 1864 1865 ret = vgxy61_power_on(dev); 1866 if (ret) 1867 return ret; 1868 1869 vgxy61_fill_sensor_param(sensor); 1870 vgxy61_fill_framefmt(sensor, sensor->current_mode, &sensor->fmt, 1871 VGXY61_MEDIA_BUS_FMT_DEF); 1872 1873 mutex_init(&sensor->lock); 1874 1875 ret = vgxy61_update_hdr(sensor, sensor->hdr); 1876 if (ret) 1877 goto error_power_off; 1878 1879 ret = vgxy61_init_controls(sensor); 1880 if (ret) { 1881 dev_err(&client->dev, "controls initialization failed %d\n", 1882 ret); 1883 goto error_power_off; 1884 } 1885 1886 ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad); 1887 if (ret) { 1888 dev_err(&client->dev, "pads init failed %d\n", ret); 1889 goto error_handler_free; 1890 } 1891 1892 /* Enable runtime PM and turn off the device */ 1893 pm_runtime_set_active(dev); 1894 pm_runtime_enable(dev); 1895 pm_runtime_idle(dev); 1896 1897 ret = v4l2_async_register_subdev(&sensor->sd); 1898 if (ret) { 1899 dev_err(&client->dev, "async subdev register failed %d\n", ret); 1900 goto error_pm_runtime; 1901 } 1902 1903 pm_runtime_set_autosuspend_delay(&client->dev, 1000); 1904 pm_runtime_use_autosuspend(&client->dev); 1905 1906 dev_dbg(&client->dev, "vgxy61 probe successfully\n"); 1907 1908 return 0; 1909 1910 error_pm_runtime: 1911 pm_runtime_disable(&client->dev); 1912 pm_runtime_set_suspended(&client->dev); 1913 media_entity_cleanup(&sensor->sd.entity); 1914 error_handler_free: 1915 v4l2_ctrl_handler_free(sensor->sd.ctrl_handler); 1916 error_power_off: 1917 mutex_destroy(&sensor->lock); 1918 vgxy61_power_off(dev); 1919 1920 return ret; 1921 } 1922 1923 static void vgxy61_remove(struct i2c_client *client) 1924 { 1925 struct v4l2_subdev *sd = i2c_get_clientdata(client); 1926 struct vgxy61_dev *sensor = to_vgxy61_dev(sd); 1927 1928 v4l2_async_unregister_subdev(&sensor->sd); 1929 mutex_destroy(&sensor->lock); 1930 media_entity_cleanup(&sensor->sd.entity); 1931 1932 pm_runtime_disable(&client->dev); 1933 if (!pm_runtime_status_suspended(&client->dev)) 1934 vgxy61_power_off(&client->dev); 1935 pm_runtime_set_suspended(&client->dev); 1936 } 1937 1938 static const struct of_device_id vgxy61_dt_ids[] = { 1939 { .compatible = "st,st-vgxy61" }, 1940 { /* sentinel */ } 1941 }; 1942 MODULE_DEVICE_TABLE(of, vgxy61_dt_ids); 1943 1944 static const struct dev_pm_ops vgxy61_pm_ops = { 1945 SET_RUNTIME_PM_OPS(vgxy61_power_off, vgxy61_power_on, NULL) 1946 }; 1947 1948 static struct i2c_driver vgxy61_i2c_driver = { 1949 .driver = { 1950 .name = "st-vgxy61", 1951 .of_match_table = vgxy61_dt_ids, 1952 .pm = &vgxy61_pm_ops, 1953 }, 1954 .probe_new = vgxy61_probe, 1955 .remove = vgxy61_remove, 1956 }; 1957 1958 module_i2c_driver(vgxy61_i2c_driver); 1959 1960 MODULE_AUTHOR("Benjamin Mugnier <benjamin.mugnier@foss.st.com>"); 1961 MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>"); 1962 MODULE_AUTHOR("Sylvain Petinot <sylvain.petinot@foss.st.com>"); 1963 MODULE_DESCRIPTION("VGXY61 camera subdev driver"); 1964 MODULE_LICENSE("GPL"); 1965