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