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