1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * A V4L2 driver for ST VD56G3 (Mono) and VD66GY (RGB) global shutter cameras.
4 * Copyright (C) 2024, STMicroelectronics SA
5 */
6
7 #include <linux/clk.h>
8 #include <linux/delay.h>
9 #include <linux/gpio/consumer.h>
10 #include <linux/i2c.h>
11 #include <linux/iopoll.h>
12 #include <linux/module.h>
13 #include <linux/pm_runtime.h>
14 #include <linux/regmap.h>
15 #include <linux/regulator/consumer.h>
16 #include <linux/unaligned.h>
17 #include <linux/units.h>
18
19 #include <media/mipi-csi2.h>
20 #include <media/v4l2-async.h>
21 #include <media/v4l2-cci.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 /* Register Map */
28 #define VD56G3_REG_MODEL_ID CCI_REG16_LE(0x0000)
29 #define VD56G3_MODEL_ID 0x5603
30 #define VD56G3_REG_REVISION CCI_REG16_LE(0x0002)
31 #define VD56G3_REVISION_CUT3 0x31
32 #define VD56G3_REG_OPTICAL_REVISION CCI_REG8(0x001a)
33 #define VD56G3_OPTICAL_REVISION_MONO 0
34 #define VD56G3_OPTICAL_REVISION_BAYER 1
35 #define VD56G3_REG_SYSTEM_FSM CCI_REG8(0x0028)
36 #define VD56G3_SYSTEM_FSM_READY_TO_BOOT 0x01
37 #define VD56G3_SYSTEM_FSM_SW_STBY 0x02
38 #define VD56G3_SYSTEM_FSM_STREAMING 0x03
39 #define VD56G3_REG_APPLIED_COARSE_EXPOSURE CCI_REG16_LE(0x0064)
40 #define VD56G3_REG_APPLIED_ANALOG_GAIN CCI_REG8(0x0068)
41 #define VD56G3_REG_APPLIED_DIGITAL_GAIN CCI_REG16_LE(0x006a)
42 #define VD56G3_REG_BOOT CCI_REG8(0x0200)
43 #define VD56G3_CMD_ACK 0
44 #define VD56G3_CMD_BOOT 1
45 #define VD56G3_REG_STBY CCI_REG8(0x0201)
46 #define VD56G3_CMD_START_STREAM 1
47 #define VD56G3_REG_STREAMING CCI_REG8(0x0202)
48 #define VD56G3_CMD_STOP_STREAM 1
49 #define VD56G3_REG_EXT_CLOCK CCI_REG32_LE(0x0220)
50 #define VD56G3_REG_CLK_PLL_PREDIV CCI_REG8(0x0224)
51 #define VD56G3_REG_CLK_SYS_PLL_MULT CCI_REG8(0x0226)
52 #define VD56G3_REG_ORIENTATION CCI_REG8(0x0302)
53 #define VD56G3_REG_FORMAT_CTRL CCI_REG8(0x030a)
54 #define VD56G3_REG_OIF_CTRL CCI_REG16_LE(0x030c)
55 #define VD56G3_REG_OIF_IMG_CTRL CCI_REG8(0x030f)
56 #define VD56G3_REG_OIF_CSI_BITRATE CCI_REG16_LE(0x0312)
57 #define VD56G3_REG_DUSTER_CTRL CCI_REG8(0x0318)
58 #define VD56G3_DUSTER_DISABLE 0
59 #define VD56G3_DUSTER_ENABLE_DEF_MODULES 0x13
60 #define VD56G3_REG_ISL_ENABLE CCI_REG8(0x0333)
61 #define VD56G3_REG_DARKCAL_CTRL CCI_REG8(0x0340)
62 #define VD56G3_DARKCAL_ENABLE 1
63 #define VD56G3_DARKCAL_DISABLE_DARKAVG 2
64 #define VD56G3_REG_PATGEN_CTRL CCI_REG16_LE(0x0400)
65 #define VD56G3_PATGEN_ENABLE 1
66 #define VD56G3_PATGEN_TYPE_SHIFT 4
67 #define VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE CCI_REG16_LE(0x042a)
68 #define VD56G3_REG_AE_COLDSTART_ANALOG_GAIN CCI_REG8(0x042c)
69 #define VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN CCI_REG16_LE(0x042e)
70 #define VD56G3_REG_AE_ROI_START_H CCI_REG16_LE(0x0432)
71 #define VD56G3_REG_AE_ROI_START_V CCI_REG16_LE(0x0434)
72 #define VD56G3_REG_AE_ROI_END_H CCI_REG16_LE(0x0436)
73 #define VD56G3_REG_AE_ROI_END_V CCI_REG16_LE(0x0438)
74 #define VD56G3_REG_AE_COMPENSATION CCI_REG16_LE(0x043a)
75 #define VD56G3_REG_EXP_MODE CCI_REG8(0x044c)
76 #define VD56G3_EXP_MODE_AUTO 0
77 #define VD56G3_EXP_MODE_FREEZE 1
78 #define VD56G3_EXP_MODE_MANUAL 2
79 #define VD56G3_REG_MANUAL_ANALOG_GAIN CCI_REG8(0x044d)
80 #define VD56G3_REG_MANUAL_COARSE_EXPOSURE CCI_REG16_LE(0x044e)
81 #define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0 CCI_REG16_LE(0x0450)
82 #define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1 CCI_REG16_LE(0x0452)
83 #define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2 CCI_REG16_LE(0x0454)
84 #define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3 CCI_REG16_LE(0x0456)
85 #define VD56G3_REG_FRAME_LENGTH CCI_REG16_LE(0x0458)
86 #define VD56G3_REG_Y_START CCI_REG16_LE(0x045a)
87 #define VD56G3_REG_Y_END CCI_REG16_LE(0x045c)
88 #define VD56G3_REG_OUT_ROI_X_START CCI_REG16_LE(0x045e)
89 #define VD56G3_REG_OUT_ROI_X_END CCI_REG16_LE(0x0460)
90 #define VD56G3_REG_OUT_ROI_Y_START CCI_REG16_LE(0x0462)
91 #define VD56G3_REG_OUT_ROI_Y_END CCI_REG16_LE(0x0464)
92 #define VD56G3_REG_GPIO_0_CTRL CCI_REG8(0x0467)
93 #define VD56G3_GPIOX_GPIO_IN 0x01
94 #define VD56G3_GPIOX_STROBE_MODE 0x02
95 #define VD56G3_REG_READOUT_CTRL CCI_REG8(0x047e)
96 #define READOUT_NORMAL 0x00
97 #define READOUT_DIGITAL_BINNING_X2 0x01
98
99 /* The VD56G3 is a portrait image sensor with native resolution of 1124x1364. */
100 #define VD56G3_NATIVE_WIDTH 1124
101 #define VD56G3_NATIVE_HEIGHT 1364
102 #define VD56G3_DEFAULT_MODE 0
103
104 /* PLL settings */
105 #define VD56G3_TARGET_PLL 804000000UL
106 #define VD56G3_VT_CLOCK_DIV 5
107
108 /* External clock must be in [6Mhz-27Mhz] */
109 #define VD56G3_XCLK_FREQ_MIN (6 * HZ_PER_MHZ)
110 #define VD56G3_XCLK_FREQ_MAX (27 * HZ_PER_MHZ)
111
112 /* Line length and Frame length (settings are for standard 10bits ADC mode) */
113 #define VD56G3_LINE_LENGTH_MIN 1236
114 #define VD56G3_VBLANK_MIN 110
115 #define VD56G3_FRAME_LENGTH_DEF_60FPS 2168
116 #define VD56G3_FRAME_LENGTH_MAX 0xffff
117
118 /* Exposure settings */
119 #define VD56G3_EXPOSURE_MARGIN 75
120 #define VD56G3_EXPOSURE_MIN 5
121 #define VD56G3_EXPOSURE_DEFAULT 1420
122
123 /* Output Interface settings */
124 #define VD56G3_MAX_CSI_DATA_LANES 2
125 #define VD56G3_LINK_FREQ_DEF_1LANE 750000000UL
126 #define VD56G3_LINK_FREQ_DEF_2LANES 402000000UL
127
128 /* GPIOs */
129 #define VD56G3_NB_GPIOS 8
130
131 /* regulator supplies */
132 static const char *const vd56g3_supply_names[] = {
133 "vcore",
134 "vddio",
135 "vana",
136 };
137
138 /* -----------------------------------------------------------------------------
139 * Models (VD56G3: Mono, VD66GY: Bayer RGB), Modes and formats
140 */
141
142 enum vd56g3_models {
143 VD56G3_MODEL_VD56G3,
144 VD56G3_MODEL_VD66GY,
145 };
146
147 struct vd56g3_mode {
148 u32 width;
149 u32 height;
150 };
151
152 static const struct vd56g3_mode vd56g3_supported_modes[] = {
153 {
154 .width = VD56G3_NATIVE_WIDTH,
155 .height = VD56G3_NATIVE_HEIGHT,
156 },
157 {
158 .width = 1120,
159 .height = 1360,
160 },
161 {
162 .width = 1024,
163 .height = 1280,
164 },
165 {
166 .width = 1024,
167 .height = 768,
168 },
169 {
170 .width = 768,
171 .height = 1024,
172 },
173 {
174 .width = 720,
175 .height = 1280,
176 },
177 {
178 .width = 640,
179 .height = 480,
180 },
181 {
182 .width = 480,
183 .height = 640,
184 },
185 {
186 .width = 320,
187 .height = 240,
188 },
189 };
190
191 /*
192 * Sensor support 8bits and 10bits output in both variants
193 * - Monochrome
194 * - RGB (with all H/V flip variations)
195 */
196 static const unsigned int vd56g3_mbus_codes[2][5] = {
197 {
198 MEDIA_BUS_FMT_Y8_1X8,
199 MEDIA_BUS_FMT_SGRBG8_1X8,
200 MEDIA_BUS_FMT_SRGGB8_1X8,
201 MEDIA_BUS_FMT_SBGGR8_1X8,
202 MEDIA_BUS_FMT_SGBRG8_1X8,
203 },
204 {
205 MEDIA_BUS_FMT_Y10_1X10,
206 MEDIA_BUS_FMT_SGRBG10_1X10,
207 MEDIA_BUS_FMT_SRGGB10_1X10,
208 MEDIA_BUS_FMT_SBGGR10_1X10,
209 MEDIA_BUS_FMT_SGBRG10_1X10,
210 },
211 };
212
213 struct vd56g3 {
214 struct device *dev;
215 struct v4l2_subdev sd;
216 struct media_pad pad;
217 struct regulator_bulk_data supplies[ARRAY_SIZE(vd56g3_supply_names)];
218 struct gpio_desc *reset_gpio;
219 struct clk *xclk;
220 struct regmap *regmap;
221 u32 xclk_freq;
222 u32 pll_prediv;
223 u32 pll_mult;
224 u32 pixel_clock;
225 u16 oif_ctrl;
226 u8 nb_of_lane;
227 u32 gpios[VD56G3_NB_GPIOS];
228 unsigned long ext_leds_mask;
229 bool is_mono;
230 struct v4l2_ctrl_handler ctrl_handler;
231 struct v4l2_ctrl *hblank_ctrl;
232 struct v4l2_ctrl *vblank_ctrl;
233 struct {
234 struct v4l2_ctrl *hflip_ctrl;
235 struct v4l2_ctrl *vflip_ctrl;
236 };
237 struct v4l2_ctrl *patgen_ctrl;
238 struct {
239 struct v4l2_ctrl *ae_ctrl;
240 struct v4l2_ctrl *expo_ctrl;
241 struct v4l2_ctrl *again_ctrl;
242 struct v4l2_ctrl *dgain_ctrl;
243 };
244 struct v4l2_ctrl *ae_lock_ctrl;
245 struct v4l2_ctrl *ae_bias_ctrl;
246 struct v4l2_ctrl *led_ctrl;
247 };
248
to_vd56g3(struct v4l2_subdev * sd)249 static inline struct vd56g3 *to_vd56g3(struct v4l2_subdev *sd)
250 {
251 return container_of_const(sd, struct vd56g3, sd);
252 }
253
ctrl_to_vd56g3(struct v4l2_ctrl * ctrl)254 static inline struct vd56g3 *ctrl_to_vd56g3(struct v4l2_ctrl *ctrl)
255 {
256 return container_of_const(ctrl->handler, struct vd56g3, ctrl_handler);
257 }
258
259 /* -----------------------------------------------------------------------------
260 * Additional i2c register helpers
261 */
262
vd56g3_poll_reg(struct vd56g3 * sensor,u32 reg,u8 poll_val,int * err)263 static int vd56g3_poll_reg(struct vd56g3 *sensor, u32 reg, u8 poll_val,
264 int *err)
265 {
266 unsigned int val = 0;
267 int ret;
268
269 if (err && *err)
270 return *err;
271
272 /*
273 * Timeout must be higher than longuest frame duration. With current
274 * blanking constraints, frame duration can take up to 504ms.
275 */
276 ret = regmap_read_poll_timeout(sensor->regmap, CCI_REG_ADDR(reg), val,
277 (val == poll_val), 2000,
278 600 * USEC_PER_MSEC);
279
280 if (ret && err)
281 *err = ret;
282
283 return ret;
284 }
285
vd56g3_wait_state(struct vd56g3 * sensor,int state,int * err)286 static int vd56g3_wait_state(struct vd56g3 *sensor, int state, int *err)
287 {
288 return vd56g3_poll_reg(sensor, VD56G3_REG_SYSTEM_FSM, state, err);
289 }
290
291 /* -----------------------------------------------------------------------------
292 * Controls: definitions, helpers and handlers
293 */
294
295 static const char *const vd56g3_tp_menu[] = { "Disabled",
296 "Solid Color",
297 "Vertical Color Bars",
298 "Horizontal Gray Scale",
299 "Vertical Gray Scale",
300 "Diagonal Gray Scale",
301 "Pseudo Random" };
302
303 static const s64 vd56g3_ev_bias_qmenu[] = { -4000, -3500, -3000, -2500, -2000,
304 -1500, -1000, -500, 0, 500,
305 1000, 1500, 2000, 2500, 3000,
306 3500, 4000 };
307
308 static const s64 vd56g3_link_freq_1lane[] = { VD56G3_LINK_FREQ_DEF_1LANE };
309
310 static const s64 vd56g3_link_freq_2lanes[] = { VD56G3_LINK_FREQ_DEF_2LANES };
311
vd56g3_get_bpp(__u32 code)312 static u8 vd56g3_get_bpp(__u32 code)
313 {
314 switch (code) {
315 case MEDIA_BUS_FMT_Y8_1X8:
316 case MEDIA_BUS_FMT_SGRBG8_1X8:
317 case MEDIA_BUS_FMT_SRGGB8_1X8:
318 case MEDIA_BUS_FMT_SBGGR8_1X8:
319 case MEDIA_BUS_FMT_SGBRG8_1X8:
320 default:
321 return 8;
322 case MEDIA_BUS_FMT_Y10_1X10:
323 case MEDIA_BUS_FMT_SGRBG10_1X10:
324 case MEDIA_BUS_FMT_SRGGB10_1X10:
325 case MEDIA_BUS_FMT_SBGGR10_1X10:
326 case MEDIA_BUS_FMT_SGBRG10_1X10:
327 return 10;
328 }
329 }
330
vd56g3_get_datatype(__u32 code)331 static u8 vd56g3_get_datatype(__u32 code)
332 {
333 switch (code) {
334 case MEDIA_BUS_FMT_Y8_1X8:
335 case MEDIA_BUS_FMT_SGRBG8_1X8:
336 case MEDIA_BUS_FMT_SRGGB8_1X8:
337 case MEDIA_BUS_FMT_SBGGR8_1X8:
338 case MEDIA_BUS_FMT_SGBRG8_1X8:
339 default:
340 return MIPI_CSI2_DT_RAW8;
341 case MEDIA_BUS_FMT_Y10_1X10:
342 case MEDIA_BUS_FMT_SGRBG10_1X10:
343 case MEDIA_BUS_FMT_SRGGB10_1X10:
344 case MEDIA_BUS_FMT_SBGGR10_1X10:
345 case MEDIA_BUS_FMT_SGBRG10_1X10:
346 return MIPI_CSI2_DT_RAW10;
347 }
348 }
349
vd56g3_read_expo_cluster(struct vd56g3 * sensor,bool force_cur_val)350 static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
351 {
352 u64 exposure;
353 u64 again;
354 u64 dgain;
355 int ret = 0;
356
357 /*
358 * When 'force_cur_val' is enabled, save the ctrl value in 'cur.val'
359 * instead of the normal 'val', this is used during poweroff to cache
360 * volatile ctrls and enable coldstart.
361 */
362 cci_read(sensor->regmap, VD56G3_REG_APPLIED_COARSE_EXPOSURE, &exposure,
363 &ret);
364 cci_read(sensor->regmap, VD56G3_REG_APPLIED_ANALOG_GAIN, &again, &ret);
365 cci_read(sensor->regmap, VD56G3_REG_APPLIED_DIGITAL_GAIN, &dgain, &ret);
366 if (ret)
367 return ret;
368
369 if (force_cur_val) {
370 sensor->expo_ctrl->cur.val = exposure;
371 sensor->again_ctrl->cur.val = again;
372 sensor->dgain_ctrl->cur.val = dgain;
373 } else {
374 sensor->expo_ctrl->val = exposure;
375 sensor->again_ctrl->val = again;
376 sensor->dgain_ctrl->val = dgain;
377 }
378
379 return ret;
380 }
381
vd56g3_update_patgen(struct vd56g3 * sensor,u32 patgen_index)382 static int vd56g3_update_patgen(struct vd56g3 *sensor, u32 patgen_index)
383 {
384 u32 pattern = patgen_index <= 2 ? patgen_index : patgen_index + 13;
385 u16 patgen = pattern << VD56G3_PATGEN_TYPE_SHIFT;
386 u8 duster = VD56G3_DUSTER_ENABLE_DEF_MODULES;
387 u8 darkcal = VD56G3_DARKCAL_ENABLE;
388 int ret = 0;
389
390 if (patgen_index) {
391 patgen |= VD56G3_PATGEN_ENABLE;
392 duster = VD56G3_DUSTER_DISABLE;
393 darkcal = VD56G3_DARKCAL_DISABLE_DARKAVG;
394 }
395
396 cci_write(sensor->regmap, VD56G3_REG_DUSTER_CTRL, duster, &ret);
397 cci_write(sensor->regmap, VD56G3_REG_DARKCAL_CTRL, darkcal, &ret);
398 cci_write(sensor->regmap, VD56G3_REG_PATGEN_CTRL, patgen, &ret);
399
400 return ret;
401 }
402
vd56g3_update_expo_cluster(struct vd56g3 * sensor,bool is_auto)403 static int vd56g3_update_expo_cluster(struct vd56g3 *sensor, bool is_auto)
404 {
405 u8 expo_state = is_auto ? VD56G3_EXP_MODE_AUTO : VD56G3_EXP_MODE_MANUAL;
406 int ret = 0;
407
408 if (sensor->ae_ctrl->is_new)
409 cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
410 &ret);
411
412 /* In Auto expo, set coldstart parameters */
413 if (is_auto && sensor->ae_ctrl->is_new) {
414 cci_write(sensor->regmap,
415 VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE,
416 sensor->expo_ctrl->val, &ret);
417 cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_ANALOG_GAIN,
418 sensor->again_ctrl->val, &ret);
419 cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN,
420 sensor->dgain_ctrl->val, &ret);
421 }
422
423 /* In Manual expo, set exposure, analog and digital gains */
424 if (!is_auto && sensor->expo_ctrl->is_new)
425 cci_write(sensor->regmap, VD56G3_REG_MANUAL_COARSE_EXPOSURE,
426 sensor->expo_ctrl->val, &ret);
427
428 if (!is_auto && sensor->again_ctrl->is_new)
429 cci_write(sensor->regmap, VD56G3_REG_MANUAL_ANALOG_GAIN,
430 sensor->again_ctrl->val, &ret);
431
432 if (!is_auto && sensor->dgain_ctrl->is_new) {
433 cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0,
434 sensor->dgain_ctrl->val, &ret);
435 cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1,
436 sensor->dgain_ctrl->val, &ret);
437 cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2,
438 sensor->dgain_ctrl->val, &ret);
439 cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3,
440 sensor->dgain_ctrl->val, &ret);
441 }
442
443 return ret;
444 }
445
vd56g3_lock_exposure(struct vd56g3 * sensor,u32 lock_val)446 static int vd56g3_lock_exposure(struct vd56g3 *sensor, u32 lock_val)
447 {
448 bool ae_lock = lock_val & V4L2_LOCK_EXPOSURE;
449 u8 expo_state = ae_lock ? VD56G3_EXP_MODE_FREEZE : VD56G3_EXP_MODE_AUTO;
450
451 if (sensor->ae_ctrl->val == V4L2_EXPOSURE_AUTO)
452 return cci_write(sensor->regmap, VD56G3_REG_EXP_MODE,
453 expo_state, NULL);
454
455 return 0;
456 }
457
vd56g3_write_gpiox(struct vd56g3 * sensor,unsigned long gpio_mask)458 static int vd56g3_write_gpiox(struct vd56g3 *sensor, unsigned long gpio_mask)
459 {
460 unsigned long io;
461 u32 gpio_val;
462 int ret = 0;
463
464 for_each_set_bit(io, &gpio_mask, VD56G3_NB_GPIOS) {
465 gpio_val = sensor->gpios[io];
466
467 if (gpio_val == VD56G3_GPIOX_STROBE_MODE &&
468 sensor->led_ctrl->val == V4L2_FLASH_LED_MODE_NONE)
469 gpio_val = VD56G3_GPIOX_GPIO_IN;
470
471 cci_write(sensor->regmap, VD56G3_REG_GPIO_0_CTRL + io, gpio_val,
472 &ret);
473 }
474
475 return ret;
476 }
477
vd56g3_g_volatile_ctrl(struct v4l2_ctrl * ctrl)478 static int vd56g3_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
479 {
480 struct vd56g3 *sensor = ctrl_to_vd56g3(ctrl);
481 int ret = 0;
482
483 /* Interact with HW only when it is powered ON */
484 if (!pm_runtime_get_if_in_use(sensor->dev))
485 return 0;
486
487 switch (ctrl->id) {
488 case V4L2_CID_EXPOSURE_AUTO:
489 ret = vd56g3_read_expo_cluster(sensor, false);
490 break;
491 default:
492 ret = -EINVAL;
493 break;
494 }
495
496 pm_runtime_put_autosuspend(sensor->dev);
497
498 return ret;
499 }
500
vd56g3_s_ctrl(struct v4l2_ctrl * ctrl)501 static int vd56g3_s_ctrl(struct v4l2_ctrl *ctrl)
502 {
503 struct vd56g3 *sensor = ctrl_to_vd56g3(ctrl);
504 struct v4l2_subdev_state *state;
505 const struct v4l2_rect *crop;
506 unsigned int frame_length = 0;
507 unsigned int expo_max;
508 unsigned int ae_compensation;
509 bool is_auto = false;
510 int ret = 0;
511
512 state = v4l2_subdev_get_locked_active_state(&sensor->sd);
513 crop = v4l2_subdev_state_get_crop(state, 0);
514
515 if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
516 return 0;
517
518 /* Update controls state, range, etc. whatever the state of the HW */
519 switch (ctrl->id) {
520 case V4L2_CID_VBLANK:
521 frame_length = crop->height + ctrl->val;
522 expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
523 ret = __v4l2_ctrl_modify_range(sensor->expo_ctrl,
524 VD56G3_EXPOSURE_MIN, expo_max, 1,
525 min(VD56G3_EXPOSURE_DEFAULT,
526 expo_max));
527 break;
528 case V4L2_CID_EXPOSURE_AUTO:
529 is_auto = (ctrl->val == V4L2_EXPOSURE_AUTO);
530 __v4l2_ctrl_grab(sensor->ae_lock_ctrl, !is_auto);
531 __v4l2_ctrl_grab(sensor->ae_bias_ctrl, !is_auto);
532 break;
533 default:
534 break;
535 }
536
537 if (ret)
538 return ret;
539
540 /* Interact with HW only when it is powered ON */
541 if (!pm_runtime_get_if_in_use(sensor->dev))
542 return 0;
543
544 switch (ctrl->id) {
545 case V4L2_CID_HFLIP:
546 ret = cci_write(sensor->regmap, VD56G3_REG_ORIENTATION,
547 sensor->hflip_ctrl->val |
548 (sensor->vflip_ctrl->val << 1),
549 NULL);
550 break;
551 case V4L2_CID_TEST_PATTERN:
552 ret = vd56g3_update_patgen(sensor, ctrl->val);
553 break;
554 case V4L2_CID_EXPOSURE_AUTO:
555 ret = vd56g3_update_expo_cluster(sensor, is_auto);
556 break;
557 case V4L2_CID_3A_LOCK:
558 ret = vd56g3_lock_exposure(sensor, ctrl->val);
559 break;
560 case V4L2_CID_AUTO_EXPOSURE_BIAS:
561 ae_compensation =
562 DIV_ROUND_CLOSEST((int)vd56g3_ev_bias_qmenu[ctrl->val] *
563 256, 1000);
564 ret = cci_write(sensor->regmap, VD56G3_REG_AE_COMPENSATION,
565 ae_compensation, NULL);
566 break;
567 case V4L2_CID_VBLANK:
568 ret = cci_write(sensor->regmap, VD56G3_REG_FRAME_LENGTH,
569 frame_length, NULL);
570 break;
571 case V4L2_CID_FLASH_LED_MODE:
572 ret = vd56g3_write_gpiox(sensor, sensor->ext_leds_mask);
573 break;
574 default:
575 ret = -EINVAL;
576 break;
577 }
578
579 pm_runtime_put_autosuspend(sensor->dev);
580
581 return ret;
582 }
583
584 static const struct v4l2_ctrl_ops vd56g3_ctrl_ops = {
585 .g_volatile_ctrl = vd56g3_g_volatile_ctrl,
586 .s_ctrl = vd56g3_s_ctrl,
587 };
588
vd56g3_update_controls(struct vd56g3 * sensor)589 static int vd56g3_update_controls(struct vd56g3 *sensor)
590 {
591 struct v4l2_subdev_state *state;
592 const struct v4l2_rect *crop;
593 unsigned int hblank;
594 unsigned int vblank_min, vblank, vblank_max;
595 unsigned int frame_length;
596 unsigned int expo_max;
597 int ret;
598
599 state = v4l2_subdev_get_locked_active_state(&sensor->sd);
600 crop = v4l2_subdev_state_get_crop(state, 0);
601 hblank = VD56G3_LINE_LENGTH_MIN - crop->width;
602 vblank_min = VD56G3_VBLANK_MIN;
603 vblank = VD56G3_FRAME_LENGTH_DEF_60FPS - crop->height;
604 vblank_max = VD56G3_FRAME_LENGTH_MAX - crop->height;
605 frame_length = crop->height + vblank;
606 expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
607
608 /* Update blanking and exposure (ranges + values) */
609 ret = __v4l2_ctrl_modify_range(sensor->hblank_ctrl, hblank, hblank, 1,
610 hblank);
611 if (ret)
612 return ret;
613
614 ret = __v4l2_ctrl_modify_range(sensor->vblank_ctrl, vblank_min,
615 vblank_max, 1, vblank);
616 if (ret)
617 return ret;
618
619 ret = __v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, vblank);
620 if (ret)
621 return ret;
622
623 ret = __v4l2_ctrl_modify_range(sensor->expo_ctrl, VD56G3_EXPOSURE_MIN,
624 expo_max, 1, VD56G3_EXPOSURE_DEFAULT);
625 if (ret)
626 return ret;
627
628 return __v4l2_ctrl_s_ctrl(sensor->expo_ctrl, VD56G3_EXPOSURE_DEFAULT);
629 }
630
vd56g3_init_controls(struct vd56g3 * sensor)631 static int vd56g3_init_controls(struct vd56g3 *sensor)
632 {
633 const struct v4l2_ctrl_ops *ops = &vd56g3_ctrl_ops;
634 struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
635 struct v4l2_fwnode_device_properties fwnode_props;
636 struct v4l2_ctrl *ctrl;
637 int ret;
638
639 v4l2_ctrl_handler_init(hdl, 25);
640
641 /* Horizontal & vertical flips modify bayer code on RGB variant */
642 sensor->hflip_ctrl =
643 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
644 if (sensor->hflip_ctrl)
645 sensor->hflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
646
647 sensor->vflip_ctrl =
648 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
649 if (sensor->vflip_ctrl)
650 sensor->vflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
651
652 sensor->patgen_ctrl =
653 v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
654 ARRAY_SIZE(vd56g3_tp_menu) - 1, 0,
655 0, vd56g3_tp_menu);
656
657 ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
658 ARRAY_SIZE(vd56g3_link_freq_1lane) - 1, 0,
659 (sensor->nb_of_lane == 2) ?
660 vd56g3_link_freq_2lanes :
661 vd56g3_link_freq_1lane);
662 if (ctrl)
663 ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
664
665 ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
666 sensor->pixel_clock, sensor->pixel_clock, 1,
667 sensor->pixel_clock);
668 if (ctrl)
669 ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
670
671 sensor->ae_ctrl = v4l2_ctrl_new_std_menu(hdl, ops,
672 V4L2_CID_EXPOSURE_AUTO,
673 V4L2_EXPOSURE_MANUAL, 0,
674 V4L2_EXPOSURE_AUTO);
675
676 sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK, 0,
677 GENMASK(2, 0), 0, 0);
678
679 sensor->ae_bias_ctrl =
680 v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
681 ARRAY_SIZE(vd56g3_ev_bias_qmenu) - 1,
682 ARRAY_SIZE(vd56g3_ev_bias_qmenu) / 2,
683 vd56g3_ev_bias_qmenu);
684
685 /*
686 * Analog gain [1, 8] is computed with the following logic :
687 * 32/(32 - again_reg), with again_reg in the range [0:28]
688 * Digital gain [1.00, 8.00] is coded as a Fixed Point 5.8
689 */
690 sensor->again_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
691 0, 28, 1, 0);
692 sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
693 0x100, 0x800, 1, 0x100);
694
695 /*
696 * Set the exposure, horizontal and vertical blanking ctrls
697 * to hardcoded values, they will be updated in vd56g3_update_controls.
698 * Exposure being in an auto-cluster, set a significant value here.
699 */
700 sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
701 VD56G3_EXPOSURE_DEFAULT,
702 VD56G3_EXPOSURE_DEFAULT, 1,
703 VD56G3_EXPOSURE_DEFAULT);
704 sensor->hblank_ctrl =
705 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 1, 1, 1, 1);
706 if (sensor->hblank_ctrl)
707 sensor->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
708 sensor->vblank_ctrl =
709 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, 1, 1, 1, 1);
710
711 /* Additional control based on device tree properties */
712 if (sensor->ext_leds_mask)
713 sensor->led_ctrl =
714 v4l2_ctrl_new_std_menu(hdl, ops,
715 V4L2_CID_FLASH_LED_MODE,
716 V4L2_FLASH_LED_MODE_FLASH, 0,
717 V4L2_FLASH_LED_MODE_NONE);
718
719 if (hdl->error) {
720 ret = hdl->error;
721 goto free_ctrls;
722 }
723
724 v4l2_ctrl_cluster(2, &sensor->hflip_ctrl);
725 v4l2_ctrl_auto_cluster(4, &sensor->ae_ctrl, V4L2_EXPOSURE_MANUAL, true);
726
727 /* Optional controls coming from fwnode (e.g. rotation, orientation). */
728 ret = v4l2_fwnode_device_parse(sensor->dev, &fwnode_props);
729 if (ret)
730 goto free_ctrls;
731
732 ret = v4l2_ctrl_new_fwnode_properties(hdl, ops, &fwnode_props);
733 if (ret)
734 goto free_ctrls;
735
736 sensor->sd.ctrl_handler = hdl;
737
738 return 0;
739
740 free_ctrls:
741 v4l2_ctrl_handler_free(hdl);
742
743 return ret;
744 }
745
746 /* -----------------------------------------------------------------------------
747 * Pad ops
748 */
749
750 /* Media bus code is dependent of :
751 * - 8bits or 10bits output
752 * - variant : Mono or RGB
753 * - H/V flips parameters in case of RGB
754 */
vd56g3_get_mbus_code(struct vd56g3 * sensor,u32 code)755 static u32 vd56g3_get_mbus_code(struct vd56g3 *sensor, u32 code)
756 {
757 unsigned int i_bpp;
758 unsigned int j;
759
760 for (i_bpp = 0; i_bpp < ARRAY_SIZE(vd56g3_mbus_codes); i_bpp++) {
761 for (j = 0; j < ARRAY_SIZE(vd56g3_mbus_codes[i_bpp]); j++) {
762 if (vd56g3_mbus_codes[i_bpp][j] == code)
763 goto endloops;
764 }
765 }
766
767 endloops:
768 if (i_bpp >= ARRAY_SIZE(vd56g3_mbus_codes))
769 i_bpp = 0;
770
771 if (sensor->is_mono)
772 j = 0;
773 else
774 j = 1 + (sensor->hflip_ctrl->val ? 1 : 0) +
775 (sensor->vflip_ctrl->val ? 2 : 0);
776
777 return vd56g3_mbus_codes[i_bpp][j];
778 }
779
vd56g3_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)780 static int vd56g3_enum_mbus_code(struct v4l2_subdev *sd,
781 struct v4l2_subdev_state *sd_state,
782 struct v4l2_subdev_mbus_code_enum *code)
783 {
784 struct vd56g3 *sensor = to_vd56g3(sd);
785
786 if (code->index >= ARRAY_SIZE(vd56g3_mbus_codes))
787 return -EINVAL;
788
789 code->code =
790 vd56g3_get_mbus_code(sensor, vd56g3_mbus_codes[code->index][0]);
791
792 return 0;
793 }
794
vd56g3_enum_frame_size(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_size_enum * fse)795 static int vd56g3_enum_frame_size(struct v4l2_subdev *sd,
796 struct v4l2_subdev_state *sd_state,
797 struct v4l2_subdev_frame_size_enum *fse)
798 {
799 if (fse->index >= ARRAY_SIZE(vd56g3_supported_modes))
800 return -EINVAL;
801
802 fse->min_width = vd56g3_supported_modes[fse->index].width;
803 fse->max_width = fse->min_width;
804 fse->min_height = vd56g3_supported_modes[fse->index].height;
805 fse->max_height = fse->min_height;
806
807 return 0;
808 }
809
vd56g3_update_img_pad_format(struct vd56g3 * sensor,const struct vd56g3_mode * mode,u32 mbus_code,struct v4l2_mbus_framefmt * mbus_fmt)810 static void vd56g3_update_img_pad_format(struct vd56g3 *sensor,
811 const struct vd56g3_mode *mode,
812 u32 mbus_code,
813 struct v4l2_mbus_framefmt *mbus_fmt)
814 {
815 mbus_fmt->width = mode->width;
816 mbus_fmt->height = mode->height;
817 mbus_fmt->code = vd56g3_get_mbus_code(sensor, mbus_code);
818 mbus_fmt->colorspace = V4L2_COLORSPACE_RAW;
819 mbus_fmt->field = V4L2_FIELD_NONE;
820 mbus_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
821 mbus_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
822 mbus_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
823 }
824
vd56g3_set_pad_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * sd_fmt)825 static int vd56g3_set_pad_fmt(struct v4l2_subdev *sd,
826 struct v4l2_subdev_state *sd_state,
827 struct v4l2_subdev_format *sd_fmt)
828 {
829 struct vd56g3 *sensor = to_vd56g3(sd);
830 const struct vd56g3_mode *new_mode;
831 struct v4l2_rect pad_crop;
832 unsigned int binning;
833
834 new_mode = v4l2_find_nearest_size(vd56g3_supported_modes,
835 ARRAY_SIZE(vd56g3_supported_modes),
836 width, height, sd_fmt->format.width,
837 sd_fmt->format.height);
838
839 vd56g3_update_img_pad_format(sensor, new_mode, sd_fmt->format.code,
840 &sd_fmt->format);
841 *v4l2_subdev_state_get_format(sd_state, sd_fmt->pad) = sd_fmt->format;
842
843 /* Compute and update crop rectangle (maximized via binning) */
844 binning = min(VD56G3_NATIVE_WIDTH / sd_fmt->format.width,
845 VD56G3_NATIVE_HEIGHT / sd_fmt->format.height);
846 binning = min(binning, 2U);
847 pad_crop.width = sd_fmt->format.width * binning;
848 pad_crop.height = sd_fmt->format.height * binning;
849 pad_crop.left = (VD56G3_NATIVE_WIDTH - pad_crop.width) / 2;
850 pad_crop.top = (VD56G3_NATIVE_HEIGHT - pad_crop.height) / 2;
851 *v4l2_subdev_state_get_crop(sd_state, sd_fmt->pad) = pad_crop;
852
853 /* Update controls in case of active state */
854 if (sd_fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
855 return vd56g3_update_controls(sensor);
856
857 return 0;
858 }
859
vd56g3_get_selection(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_selection * sel)860 static int vd56g3_get_selection(struct v4l2_subdev *sd,
861 struct v4l2_subdev_state *sd_state,
862 struct v4l2_subdev_selection *sel)
863 {
864 switch (sel->target) {
865 case V4L2_SEL_TGT_CROP:
866 sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
867 break;
868 case V4L2_SEL_TGT_NATIVE_SIZE:
869 case V4L2_SEL_TGT_CROP_DEFAULT:
870 case V4L2_SEL_TGT_CROP_BOUNDS:
871 sel->r.top = 0;
872 sel->r.left = 0;
873 sel->r.width = VD56G3_NATIVE_WIDTH;
874 sel->r.height = VD56G3_NATIVE_HEIGHT;
875 break;
876 default:
877 return -EINVAL;
878 }
879
880 return 0;
881 }
882
vd56g3_get_frame_desc(struct v4l2_subdev * sd,unsigned int pad,struct v4l2_mbus_frame_desc * fd)883 static int vd56g3_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
884 struct v4l2_mbus_frame_desc *fd)
885 {
886 struct v4l2_subdev_state *state;
887 const struct v4l2_mbus_framefmt *format;
888
889 state = v4l2_subdev_lock_and_get_active_state(sd);
890 format = v4l2_subdev_state_get_format(state, pad);
891 v4l2_subdev_unlock_state(state);
892
893 fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
894 fd->num_entries = 1;
895 fd->entry[0].pixelcode = format->code;
896 fd->entry[0].stream = 0;
897 fd->entry[0].bus.csi2.vc = 0;
898 fd->entry[0].bus.csi2.dt = vd56g3_get_datatype(format->code);
899
900 return 0;
901 }
902
vd56g3_enable_streams(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,u32 pad,u64 streams_mask)903 static int vd56g3_enable_streams(struct v4l2_subdev *sd,
904 struct v4l2_subdev_state *state, u32 pad,
905 u64 streams_mask)
906 {
907 struct vd56g3 *sensor = to_vd56g3(sd);
908 const struct v4l2_mbus_framefmt *format =
909 v4l2_subdev_state_get_format(state, 0);
910 const struct v4l2_rect *crop = v4l2_subdev_state_get_crop(state, 0);
911 unsigned int csi_mbps = ((sensor->nb_of_lane == 2) ?
912 VD56G3_LINK_FREQ_DEF_2LANES :
913 VD56G3_LINK_FREQ_DEF_1LANE) *
914 2 / MEGA;
915 unsigned int binning;
916 int ret;
917
918 ret = pm_runtime_resume_and_get(sensor->dev);
919 if (ret < 0)
920 return ret;
921
922 /* configure clocks */
923 cci_write(sensor->regmap, VD56G3_REG_EXT_CLOCK, sensor->xclk_freq,
924 &ret);
925 cci_write(sensor->regmap, VD56G3_REG_CLK_PLL_PREDIV, sensor->pll_prediv,
926 &ret);
927 cci_write(sensor->regmap, VD56G3_REG_CLK_SYS_PLL_MULT, sensor->pll_mult,
928 &ret);
929
930 /* configure output */
931 cci_write(sensor->regmap, VD56G3_REG_FORMAT_CTRL,
932 vd56g3_get_bpp(format->code), &ret);
933 cci_write(sensor->regmap, VD56G3_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
934 cci_write(sensor->regmap, VD56G3_REG_OIF_CSI_BITRATE, csi_mbps, &ret);
935 cci_write(sensor->regmap, VD56G3_REG_OIF_IMG_CTRL,
936 vd56g3_get_datatype(format->code), &ret);
937 cci_write(sensor->regmap, VD56G3_REG_ISL_ENABLE, 0, &ret);
938
939 /* configure binning mode */
940 switch (crop->width / format->width) {
941 case 1:
942 default:
943 binning = READOUT_NORMAL;
944 break;
945 case 2:
946 binning = READOUT_DIGITAL_BINNING_X2;
947 break;
948 }
949 cci_write(sensor->regmap, VD56G3_REG_READOUT_CTRL, binning, &ret);
950
951 /* configure ROIs */
952 cci_write(sensor->regmap, VD56G3_REG_Y_START, crop->top, &ret);
953 cci_write(sensor->regmap, VD56G3_REG_Y_END,
954 crop->top + crop->height - 1, &ret);
955 cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_START, crop->left, &ret);
956 cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_END,
957 crop->left + crop->width - 1, &ret);
958 cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_START, 0, &ret);
959 cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_END, crop->height - 1,
960 &ret);
961 cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_H, crop->left, &ret);
962 cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_H,
963 crop->left + crop->width - 1, &ret);
964 cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_V, 0, &ret);
965 cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_V, crop->height - 1,
966 &ret);
967 if (ret)
968 goto rpm_put;
969
970 /* Setup default GPIO values; could be overridden by V4L2 ctrl setup */
971 ret = vd56g3_write_gpiox(sensor, GENMASK(VD56G3_NB_GPIOS - 1, 0));
972 if (ret)
973 goto rpm_put;
974
975 /* Apply settings from V4L2 ctrls */
976 ret = __v4l2_ctrl_handler_setup(&sensor->ctrl_handler);
977 if (ret)
978 goto rpm_put;
979
980 /* start streaming */
981 cci_write(sensor->regmap, VD56G3_REG_STBY, VD56G3_CMD_START_STREAM,
982 &ret);
983 vd56g3_poll_reg(sensor, VD56G3_REG_STBY, VD56G3_CMD_ACK, &ret);
984 vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_STREAMING, &ret);
985 if (ret)
986 goto rpm_put;
987
988 /* some controls are locked during streaming */
989 __v4l2_ctrl_grab(sensor->hflip_ctrl, true);
990 __v4l2_ctrl_grab(sensor->vflip_ctrl, true);
991 __v4l2_ctrl_grab(sensor->patgen_ctrl, true);
992
993 return ret;
994
995 rpm_put:
996 dev_err(sensor->dev, "Failed to start streaming\n");
997 pm_runtime_put_sync(sensor->dev);
998
999 return ret;
1000 }
1001
vd56g3_disable_streams(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,u32 pad,u64 streams_mask)1002 static int vd56g3_disable_streams(struct v4l2_subdev *sd,
1003 struct v4l2_subdev_state *state, u32 pad,
1004 u64 streams_mask)
1005 {
1006 struct vd56g3 *sensor = to_vd56g3(sd);
1007 int ret;
1008
1009 /* Retrieve Expo cluster to enable coldstart of AE */
1010 ret = vd56g3_read_expo_cluster(sensor, true);
1011
1012 cci_write(sensor->regmap, VD56G3_REG_STREAMING, VD56G3_CMD_STOP_STREAM,
1013 &ret);
1014 vd56g3_poll_reg(sensor, VD56G3_REG_STREAMING, VD56G3_CMD_ACK, &ret);
1015 vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
1016
1017 /* locked controls must be unlocked */
1018 __v4l2_ctrl_grab(sensor->hflip_ctrl, false);
1019 __v4l2_ctrl_grab(sensor->vflip_ctrl, false);
1020 __v4l2_ctrl_grab(sensor->patgen_ctrl, false);
1021
1022 pm_runtime_put_autosuspend(sensor->dev);
1023
1024 return ret;
1025 }
1026
vd56g3_init_state(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state)1027 static int vd56g3_init_state(struct v4l2_subdev *sd,
1028 struct v4l2_subdev_state *sd_state)
1029 {
1030 unsigned int def_mode = VD56G3_DEFAULT_MODE;
1031 struct v4l2_subdev_format fmt = {
1032 .which = V4L2_SUBDEV_FORMAT_TRY,
1033 .pad = 0,
1034 .format = {
1035 .code = vd56g3_mbus_codes[0][0],
1036 .width = vd56g3_supported_modes[def_mode].width,
1037 .height = vd56g3_supported_modes[def_mode].height,
1038 },
1039 };
1040
1041 return vd56g3_set_pad_fmt(sd, sd_state, &fmt);
1042 }
1043
1044 static const struct v4l2_subdev_video_ops vd56g3_video_ops = {
1045 .s_stream = v4l2_subdev_s_stream_helper,
1046 };
1047
1048 static const struct v4l2_subdev_pad_ops vd56g3_pad_ops = {
1049 .enum_mbus_code = vd56g3_enum_mbus_code,
1050 .enum_frame_size = vd56g3_enum_frame_size,
1051 .get_fmt = v4l2_subdev_get_fmt,
1052 .set_fmt = vd56g3_set_pad_fmt,
1053 .get_selection = vd56g3_get_selection,
1054 .get_frame_desc = vd56g3_get_frame_desc,
1055 .enable_streams = vd56g3_enable_streams,
1056 .disable_streams = vd56g3_disable_streams,
1057 };
1058
1059 static const struct v4l2_subdev_ops vd56g3_subdev_ops = {
1060 .video = &vd56g3_video_ops,
1061 .pad = &vd56g3_pad_ops,
1062 };
1063
1064 static const struct media_entity_operations vd56g3_subdev_entity_ops = {
1065 .link_validate = v4l2_subdev_link_validate,
1066 };
1067
1068 static const struct v4l2_subdev_internal_ops vd56g3_internal_ops = {
1069 .init_state = vd56g3_init_state,
1070 };
1071
1072 /* -----------------------------------------------------------------------------
1073 * Power management
1074 */
1075
vd56g3_power_on(struct device * dev)1076 static int vd56g3_power_on(struct device *dev)
1077 {
1078 struct v4l2_subdev *sd = dev_get_drvdata(dev);
1079 struct vd56g3 *sensor = to_vd56g3(sd);
1080 int ret;
1081
1082 /* power on */
1083 ret = regulator_bulk_enable(ARRAY_SIZE(sensor->supplies),
1084 sensor->supplies);
1085 if (ret) {
1086 dev_err(dev, "Failed to enable regulators: %d\n", ret);
1087 return ret;
1088 }
1089
1090 ret = clk_prepare_enable(sensor->xclk);
1091 if (ret) {
1092 dev_err(dev, "Failed to enable clock: %d\n", ret);
1093 goto disable_reg;
1094 }
1095
1096 gpiod_set_value_cansleep(sensor->reset_gpio, 0);
1097 usleep_range(3500, 4000);
1098 ret = vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_READY_TO_BOOT, NULL);
1099 if (ret) {
1100 dev_err(dev, "Sensor reset failed: %d\n", ret);
1101 goto disable_clock;
1102 }
1103
1104 /* boot sensor */
1105 cci_write(sensor->regmap, VD56G3_REG_BOOT, VD56G3_CMD_BOOT, &ret);
1106 vd56g3_poll_reg(sensor, VD56G3_REG_BOOT, VD56G3_CMD_ACK, &ret);
1107 vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
1108 if (ret) {
1109 dev_err(dev, "Sensor boot failed: %d\n", ret);
1110 goto disable_clock;
1111 }
1112
1113 return 0;
1114
1115 disable_clock:
1116 gpiod_set_value_cansleep(sensor->reset_gpio, 1);
1117 clk_disable_unprepare(sensor->xclk);
1118 disable_reg:
1119 regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies);
1120
1121 return ret;
1122 }
1123
vd56g3_power_off(struct device * dev)1124 static int vd56g3_power_off(struct device *dev)
1125 {
1126 struct v4l2_subdev *sd = dev_get_drvdata(dev);
1127 struct vd56g3 *sensor = to_vd56g3(sd);
1128
1129 gpiod_set_value_cansleep(sensor->reset_gpio, 1);
1130 clk_disable_unprepare(sensor->xclk);
1131 regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies);
1132
1133 return 0;
1134 }
1135
1136 static const struct dev_pm_ops vd56g3_pm_ops = {
1137 SET_RUNTIME_PM_OPS(vd56g3_power_off, vd56g3_power_on, NULL)
1138 };
1139
1140 /* -----------------------------------------------------------------------------
1141 * Probe and initialization
1142 */
1143
vd56g3_check_csi_conf(struct vd56g3 * sensor,struct fwnode_handle * endpoint)1144 static int vd56g3_check_csi_conf(struct vd56g3 *sensor,
1145 struct fwnode_handle *endpoint)
1146 {
1147 struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
1148 u32 phy_data_lanes[VD56G3_MAX_CSI_DATA_LANES] = { ~0, ~0 };
1149 u8 n_lanes;
1150 u64 frequency;
1151 int p, l;
1152 int ret = 0;
1153
1154 ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
1155 if (ret)
1156 return -EINVAL;
1157
1158 /* Check lanes number */
1159 n_lanes = ep.bus.mipi_csi2.num_data_lanes;
1160 if (n_lanes != 1 && n_lanes != 2) {
1161 dev_err(sensor->dev, "Invalid data lane number: %d\n", n_lanes);
1162 ret = -EINVAL;
1163 goto done;
1164 }
1165 sensor->nb_of_lane = n_lanes;
1166
1167 /* Clock lane must be first */
1168 if (ep.bus.mipi_csi2.clock_lane != 0) {
1169 dev_err(sensor->dev, "Clock lane must be mapped to lane 0\n");
1170 ret = -EINVAL;
1171 goto done;
1172 }
1173
1174 /*
1175 * Prepare Output Interface conf based on lane settings
1176 * logical to physical lane conversion (+ pad remaining slots)
1177 */
1178 for (l = 0; l < n_lanes; l++)
1179 phy_data_lanes[ep.bus.mipi_csi2.data_lanes[l] - 1] = l;
1180 for (p = 0; p < VD56G3_MAX_CSI_DATA_LANES; p++) {
1181 if (phy_data_lanes[p] != ~0)
1182 continue;
1183 phy_data_lanes[p] = l;
1184 l++;
1185 }
1186 sensor->oif_ctrl = n_lanes |
1187 (ep.bus.mipi_csi2.lane_polarities[0] << 3) |
1188 ((phy_data_lanes[0]) << 4) |
1189 (ep.bus.mipi_csi2.lane_polarities[1] << 6) |
1190 ((phy_data_lanes[1]) << 7) |
1191 (ep.bus.mipi_csi2.lane_polarities[2] << 9);
1192
1193 /* Check link frequency */
1194 if (!ep.nr_of_link_frequencies) {
1195 dev_err(sensor->dev, "link-frequency not found in DT\n");
1196 ret = -EINVAL;
1197 goto done;
1198 }
1199 frequency = (n_lanes == 2) ? VD56G3_LINK_FREQ_DEF_2LANES :
1200 VD56G3_LINK_FREQ_DEF_1LANE;
1201 if (ep.nr_of_link_frequencies != 1 ||
1202 ep.link_frequencies[0] != frequency) {
1203 dev_err(sensor->dev, "Link frequency not supported: %lld\n",
1204 ep.link_frequencies[0]);
1205 ret = -EINVAL;
1206 goto done;
1207 }
1208
1209 done:
1210 v4l2_fwnode_endpoint_free(&ep);
1211
1212 return ret;
1213 }
1214
vd56g3_parse_dt_gpios_array(struct vd56g3 * sensor,char * prop_name,u32 * array,unsigned int * nb)1215 static int vd56g3_parse_dt_gpios_array(struct vd56g3 *sensor, char *prop_name,
1216 u32 *array, unsigned int *nb)
1217 {
1218 struct device *dev = sensor->dev;
1219 unsigned int i;
1220 int ret;
1221
1222 if (!device_property_present(dev, prop_name)) {
1223 *nb = 0;
1224 return 0;
1225 }
1226
1227 ret = device_property_count_u32(dev, prop_name);
1228 if (ret < 0) {
1229 dev_err(dev, "Failed to read %s count\n", prop_name);
1230 return ret;
1231 }
1232
1233 *nb = ret;
1234 ret = device_property_read_u32_array(dev, prop_name, array, *nb);
1235 if (ret) {
1236 dev_err(dev, "Failed to read %s prop\n", prop_name);
1237 return ret;
1238 }
1239
1240 for (i = 0; i < *nb; i++) {
1241 if (array[i] >= VD56G3_NB_GPIOS) {
1242 dev_err(dev, "Invalid GPIO: %d\n", array[i]);
1243 return -EINVAL;
1244 }
1245 }
1246
1247 return 0;
1248 }
1249
vd56g3_parse_dt_gpios(struct vd56g3 * sensor)1250 static int vd56g3_parse_dt_gpios(struct vd56g3 *sensor)
1251 {
1252 u32 led_gpios[VD56G3_NB_GPIOS];
1253 unsigned int nb_gpios_leds;
1254 unsigned int i;
1255 int ret;
1256
1257 /* Initialize GPIOs to default */
1258 for (i = 0; i < VD56G3_NB_GPIOS; i++)
1259 sensor->gpios[i] = VD56G3_GPIOX_GPIO_IN;
1260 sensor->ext_leds_mask = 0;
1261
1262 /* Take into account optional 'st,leds' output for GPIOs */
1263 ret = vd56g3_parse_dt_gpios_array(sensor, "st,leds", led_gpios,
1264 &nb_gpios_leds);
1265 if (ret)
1266 return ret;
1267 for (i = 0; i < nb_gpios_leds; i++) {
1268 sensor->gpios[led_gpios[i]] = VD56G3_GPIOX_STROBE_MODE;
1269 set_bit(led_gpios[i], &sensor->ext_leds_mask);
1270 }
1271
1272 return 0;
1273 }
1274
vd56g3_parse_dt(struct vd56g3 * sensor)1275 static int vd56g3_parse_dt(struct vd56g3 *sensor)
1276 {
1277 struct fwnode_handle *endpoint;
1278 int ret;
1279
1280 endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(sensor->dev), 0,
1281 0, 0);
1282 if (!endpoint) {
1283 dev_err(sensor->dev, "Endpoint node not found\n");
1284 return -EINVAL;
1285 }
1286
1287 ret = vd56g3_check_csi_conf(sensor, endpoint);
1288 fwnode_handle_put(endpoint);
1289 if (ret)
1290 return ret;
1291
1292 return vd56g3_parse_dt_gpios(sensor);
1293 }
1294
vd56g3_get_regulators(struct vd56g3 * sensor)1295 static int vd56g3_get_regulators(struct vd56g3 *sensor)
1296 {
1297 unsigned int i;
1298
1299 for (i = 0; i < ARRAY_SIZE(sensor->supplies); i++)
1300 sensor->supplies[i].supply = vd56g3_supply_names[i];
1301
1302 return devm_regulator_bulk_get(sensor->dev,
1303 ARRAY_SIZE(sensor->supplies),
1304 sensor->supplies);
1305 }
1306
vd56g3_prepare_clock_tree(struct vd56g3 * sensor)1307 static int vd56g3_prepare_clock_tree(struct vd56g3 *sensor)
1308 {
1309 const unsigned int predivs[] = { 1, 2, 4 };
1310 u32 pll_out;
1311 int i;
1312
1313 /* External clock must be in [6Mhz-27Mhz] */
1314 if (sensor->xclk_freq < VD56G3_XCLK_FREQ_MIN ||
1315 sensor->xclk_freq > VD56G3_XCLK_FREQ_MAX) {
1316 dev_err(sensor->dev,
1317 "Only 6Mhz-27Mhz clock range supported. Provided %lu MHz\n",
1318 sensor->xclk_freq / HZ_PER_MHZ);
1319 return -EINVAL;
1320 }
1321
1322 /* PLL input should be in [6Mhz-12Mhz[ */
1323 for (i = 0; i < ARRAY_SIZE(predivs); i++) {
1324 sensor->pll_prediv = predivs[i];
1325 if (sensor->xclk_freq / sensor->pll_prediv < 12 * HZ_PER_MHZ)
1326 break;
1327 }
1328
1329 /* PLL output clock must be as close as possible to 804Mhz */
1330 sensor->pll_mult = (VD56G3_TARGET_PLL * sensor->pll_prediv +
1331 sensor->xclk_freq / 2) /
1332 sensor->xclk_freq;
1333 pll_out = sensor->xclk_freq * sensor->pll_mult / sensor->pll_prediv;
1334
1335 /* Target Pixel Clock for standard 10bit ADC mode : 160.8Mhz */
1336 sensor->pixel_clock = pll_out / VD56G3_VT_CLOCK_DIV;
1337
1338 return 0;
1339 }
1340
vd56g3_detect(struct vd56g3 * sensor)1341 static int vd56g3_detect(struct vd56g3 *sensor)
1342 {
1343 struct device *dev = sensor->dev;
1344 unsigned int model;
1345 u64 model_id;
1346 u64 device_revision;
1347 u64 optical_revision;
1348 int ret = 0;
1349
1350 model = (uintptr_t)device_get_match_data(dev);
1351
1352 ret = cci_read(sensor->regmap, VD56G3_REG_MODEL_ID, &model_id, NULL);
1353 if (ret)
1354 return ret;
1355
1356 if (model_id != VD56G3_MODEL_ID) {
1357 dev_err(dev, "Unsupported sensor id: %x\n", (u16)model_id);
1358 return -ENODEV;
1359 }
1360
1361 ret = cci_read(sensor->regmap, VD56G3_REG_REVISION, &device_revision,
1362 NULL);
1363 if (ret)
1364 return ret;
1365
1366 if ((device_revision >> 8) != VD56G3_REVISION_CUT3) {
1367 dev_err(dev, "Unsupported version: %x\n", (u16)device_revision);
1368 return -ENODEV;
1369 }
1370
1371 ret = cci_read(sensor->regmap, VD56G3_REG_OPTICAL_REVISION,
1372 &optical_revision, NULL);
1373 if (ret)
1374 return ret;
1375
1376 sensor->is_mono =
1377 ((optical_revision & 1) == VD56G3_OPTICAL_REVISION_MONO);
1378 if ((sensor->is_mono && model == VD56G3_MODEL_VD66GY) ||
1379 (!sensor->is_mono && model == VD56G3_MODEL_VD56G3)) {
1380 dev_err(dev, "Found %s sensor, while %s model is defined in DT\n",
1381 (sensor->is_mono) ? "Mono" : "Bayer",
1382 (model == VD56G3_MODEL_VD56G3) ? "vd56g3" : "vd66gy");
1383 return -ENODEV;
1384 }
1385
1386 return 0;
1387 }
1388
vd56g3_subdev_init(struct vd56g3 * sensor)1389 static int vd56g3_subdev_init(struct vd56g3 *sensor)
1390 {
1391 struct v4l2_subdev_state *state;
1392 int ret;
1393
1394 /* Init remaining sub device ops */
1395 sensor->sd.internal_ops = &vd56g3_internal_ops;
1396 sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1397 sensor->sd.entity.ops = &vd56g3_subdev_entity_ops;
1398
1399 /* Init source pad */
1400 sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
1401 sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1402 ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
1403 if (ret) {
1404 dev_err(sensor->dev, "Failed to init media entity: %d\n", ret);
1405 return ret;
1406 }
1407
1408 /* Init controls */
1409 ret = vd56g3_init_controls(sensor);
1410 if (ret) {
1411 dev_err(sensor->dev, "Controls initialization failed: %d\n",
1412 ret);
1413 goto err_media;
1414 }
1415
1416 /* Init vd56g3 struct : default resolution + raw8 */
1417 sensor->sd.state_lock = sensor->ctrl_handler.lock;
1418 ret = v4l2_subdev_init_finalize(&sensor->sd);
1419 if (ret) {
1420 dev_err(sensor->dev, "Subdev init failed: %d\n", ret);
1421 goto err_ctrls;
1422 }
1423
1424 /* Update controls according to the resolution set */
1425 state = v4l2_subdev_lock_and_get_active_state(&sensor->sd);
1426 ret = vd56g3_update_controls(sensor);
1427 v4l2_subdev_unlock_state(state);
1428 if (ret) {
1429 dev_err(sensor->dev, "Controls update failed: %d\n", ret);
1430 goto err_ctrls;
1431 }
1432
1433 return 0;
1434
1435 err_ctrls:
1436 v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
1437
1438 err_media:
1439 media_entity_cleanup(&sensor->sd.entity);
1440
1441 return ret;
1442 }
1443
vd56g3_subdev_cleanup(struct vd56g3 * sensor)1444 static void vd56g3_subdev_cleanup(struct vd56g3 *sensor)
1445 {
1446 v4l2_async_unregister_subdev(&sensor->sd);
1447 v4l2_subdev_cleanup(&sensor->sd);
1448 media_entity_cleanup(&sensor->sd.entity);
1449 v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
1450 }
1451
vd56g3_probe(struct i2c_client * client)1452 static int vd56g3_probe(struct i2c_client *client)
1453 {
1454 struct device *dev = &client->dev;
1455 struct vd56g3 *sensor;
1456 int ret;
1457
1458 sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
1459 if (!sensor)
1460 return -ENOMEM;
1461
1462 v4l2_i2c_subdev_init(&sensor->sd, client, &vd56g3_subdev_ops);
1463 sensor->dev = dev;
1464
1465 ret = vd56g3_parse_dt(sensor);
1466 if (ret)
1467 return dev_err_probe(dev, ret, "Failed to parse Device Tree\n");
1468
1469 /* Get (and check) resources : power regs, ext clock, reset gpio */
1470 ret = vd56g3_get_regulators(sensor);
1471 if (ret)
1472 return dev_err_probe(dev, ret, "Failed to get regulators\n");
1473
1474 sensor->xclk = devm_clk_get(dev, NULL);
1475 if (IS_ERR(sensor->xclk))
1476 return dev_err_probe(dev, PTR_ERR(sensor->xclk),
1477 "Failed to get xclk\n");
1478 sensor->xclk_freq = clk_get_rate(sensor->xclk);
1479 ret = vd56g3_prepare_clock_tree(sensor);
1480 if (ret)
1481 return ret;
1482
1483 sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
1484 GPIOD_OUT_HIGH);
1485 if (IS_ERR(sensor->reset_gpio))
1486 return dev_err_probe(dev, PTR_ERR(sensor->reset_gpio),
1487 "Failed to get reset gpio\n");
1488
1489 sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
1490 if (IS_ERR(sensor->regmap))
1491 return dev_err_probe(dev, PTR_ERR(sensor->regmap),
1492 "Failed to init regmap\n");
1493
1494 /* Power ON */
1495 ret = vd56g3_power_on(dev);
1496 if (ret)
1497 return dev_err_probe(dev, ret, "Sensor power on failed\n");
1498
1499 /* Enable PM runtime with autosuspend (sensor being ON, set active) */
1500 pm_runtime_set_active(dev);
1501 pm_runtime_get_noresume(dev);
1502 pm_runtime_enable(dev);
1503 pm_runtime_set_autosuspend_delay(dev, 1000);
1504 pm_runtime_use_autosuspend(dev);
1505
1506 /* Check HW model/version */
1507 ret = vd56g3_detect(sensor);
1508 if (ret) {
1509 dev_err(dev, "Sensor detect failed: %d\n", ret);
1510 goto err_power_off;
1511 }
1512
1513 /* Initialize & register subdev (v4l2_i2c subdev already initialized) */
1514 ret = vd56g3_subdev_init(sensor);
1515 if (ret) {
1516 dev_err(dev, "V4l2 init failed: %d\n", ret);
1517 goto err_power_off;
1518 }
1519
1520 ret = v4l2_async_register_subdev(&sensor->sd);
1521 if (ret) {
1522 dev_err(dev, "Async subdev register failed: %d\n", ret);
1523 goto err_subdev;
1524 }
1525
1526 /* Sensor could now be powered off (after the autosuspend delay) */
1527 pm_runtime_put_autosuspend(dev);
1528
1529 dev_dbg(dev, "Successfully probe %s sensor\n",
1530 (sensor->is_mono) ? "vd56g3" : "vd66gy");
1531
1532 return 0;
1533
1534 err_subdev:
1535 vd56g3_subdev_cleanup(sensor);
1536 err_power_off:
1537 pm_runtime_disable(dev);
1538 pm_runtime_put_noidle(dev);
1539 pm_runtime_dont_use_autosuspend(dev);
1540 vd56g3_power_off(dev);
1541
1542 return ret;
1543 }
1544
vd56g3_remove(struct i2c_client * client)1545 static void vd56g3_remove(struct i2c_client *client)
1546 {
1547 struct v4l2_subdev *sd = i2c_get_clientdata(client);
1548 struct vd56g3 *sensor = to_vd56g3(sd);
1549
1550 vd56g3_subdev_cleanup(sensor);
1551
1552 pm_runtime_disable(sensor->dev);
1553 if (!pm_runtime_status_suspended(sensor->dev))
1554 vd56g3_power_off(sensor->dev);
1555 pm_runtime_set_suspended(sensor->dev);
1556 pm_runtime_dont_use_autosuspend(sensor->dev);
1557 }
1558
1559 static const struct of_device_id vd56g3_dt_ids[] = {
1560 { .compatible = "st,vd56g3", .data = (void *)VD56G3_MODEL_VD56G3 },
1561 { .compatible = "st,vd66gy", .data = (void *)VD56G3_MODEL_VD66GY },
1562 { /* sentinel */ }
1563 };
1564 MODULE_DEVICE_TABLE(of, vd56g3_dt_ids);
1565
1566 static struct i2c_driver vd56g3_i2c_driver = {
1567 .driver = {
1568 .name = "vd56g3",
1569 .of_match_table = vd56g3_dt_ids,
1570 .pm = &vd56g3_pm_ops,
1571 },
1572 .probe = vd56g3_probe,
1573 .remove = vd56g3_remove,
1574 };
1575
1576 module_i2c_driver(vd56g3_i2c_driver);
1577
1578 MODULE_AUTHOR("Benjamin Mugnier <benjamin.mugnier@foss.st.com>");
1579 MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>");
1580 MODULE_AUTHOR("Sylvain Petinot <sylvain.petinot@foss.st.com>");
1581 MODULE_DESCRIPTION("ST VD56G3 sensor driver");
1582 MODULE_LICENSE("GPL");
1583