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