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