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