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