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 */
132static 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
142enum vd56g3_models {
143 VD56G3_MODEL_VD56G3,
144 VD56G3_MODEL_VD66GY,
145};
146
147struct vd56g3_mode {
148 u32 width;
149 u32 height;
150};
151
152static 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 */
196static 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
213struct 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
249static inline struct vd56g3 *to_vd56g3(struct v4l2_subdev *sd)
250{
251 return container_of_const(sd, struct vd56g3, sd);
252}
253
254static 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
263static 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
286static int vd56g3_wait_state(struct vd56g3 *sensor, int state, int *err)
287{
288 return vd56g3_poll_reg(sensor, VD56G3_REG_SYSTEM_FSM, poll_val: state, err);
289}
290
291/* -----------------------------------------------------------------------------
292 * Controls: definitions, helpers and handlers
293 */
294
295static 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
303static 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
308static const s64 vd56g3_link_freq_1lane[] = { VD56G3_LINK_FREQ_DEF_1LANE };
309
310static const s64 vd56g3_link_freq_2lanes[] = { VD56G3_LINK_FREQ_DEF_2LANES };
311
312static 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
331static 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
350static 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(map: sensor->regmap, VD56G3_REG_APPLIED_COARSE_EXPOSURE, val: &exposure,
363 err: &ret);
364 cci_read(map: sensor->regmap, VD56G3_REG_APPLIED_ANALOG_GAIN, val: &again, err: &ret);
365 cci_read(map: sensor->regmap, VD56G3_REG_APPLIED_DIGITAL_GAIN, val: &dgain, err: &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
382static 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(map: sensor->regmap, VD56G3_REG_DUSTER_CTRL, val: duster, err: &ret);
397 cci_write(map: sensor->regmap, VD56G3_REG_DARKCAL_CTRL, val: darkcal, err: &ret);
398 cci_write(map: sensor->regmap, VD56G3_REG_PATGEN_CTRL, val: patgen, err: &ret);
399
400 return ret;
401}
402
403static 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(map: sensor->regmap, VD56G3_REG_EXP_MODE, val: expo_state,
410 err: &ret);
411
412 /* In Auto expo, set coldstart parameters */
413 if (is_auto && sensor->ae_ctrl->is_new) {
414 cci_write(map: sensor->regmap,
415 VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE,
416 val: sensor->expo_ctrl->val, err: &ret);
417 cci_write(map: sensor->regmap, VD56G3_REG_AE_COLDSTART_ANALOG_GAIN,
418 val: sensor->again_ctrl->val, err: &ret);
419 cci_write(map: sensor->regmap, VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN,
420 val: sensor->dgain_ctrl->val, err: &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(map: sensor->regmap, VD56G3_REG_MANUAL_COARSE_EXPOSURE,
426 val: sensor->expo_ctrl->val, err: &ret);
427
428 if (!is_auto && sensor->again_ctrl->is_new)
429 cci_write(map: sensor->regmap, VD56G3_REG_MANUAL_ANALOG_GAIN,
430 val: sensor->again_ctrl->val, err: &ret);
431
432 if (!is_auto && sensor->dgain_ctrl->is_new) {
433 cci_write(map: sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0,
434 val: sensor->dgain_ctrl->val, err: &ret);
435 cci_write(map: sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1,
436 val: sensor->dgain_ctrl->val, err: &ret);
437 cci_write(map: sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2,
438 val: sensor->dgain_ctrl->val, err: &ret);
439 cci_write(map: sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3,
440 val: sensor->dgain_ctrl->val, err: &ret);
441 }
442
443 return ret;
444}
445
446static 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(map: sensor->regmap, VD56G3_REG_EXP_MODE,
453 val: expo_state, NULL);
454
455 return 0;
456}
457
458static 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(map: sensor->regmap, VD56G3_REG_GPIO_0_CTRL + io, val: gpio_val,
472 err: &ret);
473 }
474
475 return ret;
476}
477
478static 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(dev: sensor->dev))
485 return 0;
486
487 switch (ctrl->id) {
488 case V4L2_CID_EXPOSURE_AUTO:
489 ret = vd56g3_read_expo_cluster(sensor, force_cur_val: false);
490 break;
491 default:
492 ret = -EINVAL;
493 break;
494 }
495
496 pm_runtime_mark_last_busy(dev: sensor->dev);
497 pm_runtime_put_autosuspend(dev: sensor->dev);
498
499 return ret;
500}
501
502static int vd56g3_s_ctrl(struct v4l2_ctrl *ctrl)
503{
504 struct vd56g3 *sensor = ctrl_to_vd56g3(ctrl);
505 struct v4l2_subdev_state *state;
506 const struct v4l2_rect *crop;
507 unsigned int frame_length = 0;
508 unsigned int expo_max;
509 unsigned int ae_compensation;
510 bool is_auto = false;
511 int ret = 0;
512
513 state = v4l2_subdev_get_locked_active_state(sd: &sensor->sd);
514 crop = v4l2_subdev_state_get_crop(state, 0);
515
516 if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
517 return 0;
518
519 /* Update controls state, range, etc. whatever the state of the HW */
520 switch (ctrl->id) {
521 case V4L2_CID_VBLANK:
522 frame_length = crop->height + ctrl->val;
523 expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
524 ret = __v4l2_ctrl_modify_range(ctrl: sensor->expo_ctrl,
525 VD56G3_EXPOSURE_MIN, max: expo_max, step: 1,
526 min(VD56G3_EXPOSURE_DEFAULT,
527 expo_max));
528 break;
529 case V4L2_CID_EXPOSURE_AUTO:
530 is_auto = (ctrl->val == V4L2_EXPOSURE_AUTO);
531 __v4l2_ctrl_grab(ctrl: sensor->ae_lock_ctrl, grabbed: !is_auto);
532 __v4l2_ctrl_grab(ctrl: sensor->ae_bias_ctrl, grabbed: !is_auto);
533 break;
534 default:
535 break;
536 }
537
538 if (ret)
539 return ret;
540
541 /* Interact with HW only when it is powered ON */
542 if (!pm_runtime_get_if_in_use(dev: sensor->dev))
543 return 0;
544
545 switch (ctrl->id) {
546 case V4L2_CID_HFLIP:
547 ret = cci_write(map: sensor->regmap, VD56G3_REG_ORIENTATION,
548 val: sensor->hflip_ctrl->val |
549 (sensor->vflip_ctrl->val << 1),
550 NULL);
551 break;
552 case V4L2_CID_TEST_PATTERN:
553 ret = vd56g3_update_patgen(sensor, patgen_index: ctrl->val);
554 break;
555 case V4L2_CID_EXPOSURE_AUTO:
556 ret = vd56g3_update_expo_cluster(sensor, is_auto);
557 break;
558 case V4L2_CID_3A_LOCK:
559 ret = vd56g3_lock_exposure(sensor, lock_val: ctrl->val);
560 break;
561 case V4L2_CID_AUTO_EXPOSURE_BIAS:
562 ae_compensation =
563 DIV_ROUND_CLOSEST((int)vd56g3_ev_bias_qmenu[ctrl->val] *
564 256, 1000);
565 ret = cci_write(map: sensor->regmap, VD56G3_REG_AE_COMPENSATION,
566 val: ae_compensation, NULL);
567 break;
568 case V4L2_CID_VBLANK:
569 ret = cci_write(map: sensor->regmap, VD56G3_REG_FRAME_LENGTH,
570 val: frame_length, NULL);
571 break;
572 case V4L2_CID_FLASH_LED_MODE:
573 ret = vd56g3_write_gpiox(sensor, gpio_mask: sensor->ext_leds_mask);
574 break;
575 default:
576 ret = -EINVAL;
577 break;
578 }
579
580 pm_runtime_mark_last_busy(dev: sensor->dev);
581 pm_runtime_put_autosuspend(dev: sensor->dev);
582
583 return ret;
584}
585
586static const struct v4l2_ctrl_ops vd56g3_ctrl_ops = {
587 .g_volatile_ctrl = vd56g3_g_volatile_ctrl,
588 .s_ctrl = vd56g3_s_ctrl,
589};
590
591static int vd56g3_update_controls(struct vd56g3 *sensor)
592{
593 struct v4l2_subdev_state *state;
594 const struct v4l2_rect *crop;
595 unsigned int hblank;
596 unsigned int vblank_min, vblank, vblank_max;
597 unsigned int frame_length;
598 unsigned int expo_max;
599 int ret;
600
601 state = v4l2_subdev_get_locked_active_state(sd: &sensor->sd);
602 crop = v4l2_subdev_state_get_crop(state, 0);
603 hblank = VD56G3_LINE_LENGTH_MIN - crop->width;
604 vblank_min = VD56G3_VBLANK_MIN;
605 vblank = VD56G3_FRAME_LENGTH_DEF_60FPS - crop->height;
606 vblank_max = VD56G3_FRAME_LENGTH_MAX - crop->height;
607 frame_length = crop->height + vblank;
608 expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
609
610 /* Update blanking and exposure (ranges + values) */
611 ret = __v4l2_ctrl_modify_range(ctrl: sensor->hblank_ctrl, min: hblank, max: hblank, step: 1,
612 def: hblank);
613 if (ret)
614 return ret;
615
616 ret = __v4l2_ctrl_modify_range(ctrl: sensor->vblank_ctrl, min: vblank_min,
617 max: vblank_max, step: 1, def: vblank);
618 if (ret)
619 return ret;
620
621 ret = __v4l2_ctrl_s_ctrl(ctrl: sensor->vblank_ctrl, val: vblank);
622 if (ret)
623 return ret;
624
625 ret = __v4l2_ctrl_modify_range(ctrl: sensor->expo_ctrl, VD56G3_EXPOSURE_MIN,
626 max: expo_max, step: 1, VD56G3_EXPOSURE_DEFAULT);
627 if (ret)
628 return ret;
629
630 return __v4l2_ctrl_s_ctrl(ctrl: sensor->expo_ctrl, VD56G3_EXPOSURE_DEFAULT);
631}
632
633static int vd56g3_init_controls(struct vd56g3 *sensor)
634{
635 const struct v4l2_ctrl_ops *ops = &vd56g3_ctrl_ops;
636 struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
637 struct v4l2_fwnode_device_properties fwnode_props;
638 struct v4l2_ctrl *ctrl;
639 int ret;
640
641 v4l2_ctrl_handler_init(hdl, 25);
642
643 /* Horizontal & vertical flips modify bayer code on RGB variant */
644 sensor->hflip_ctrl =
645 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, min: 0, max: 1, step: 1, def: 0);
646 if (sensor->hflip_ctrl)
647 sensor->hflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
648
649 sensor->vflip_ctrl =
650 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, min: 0, max: 1, step: 1, def: 0);
651 if (sensor->vflip_ctrl)
652 sensor->vflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
653
654 sensor->patgen_ctrl =
655 v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
656 ARRAY_SIZE(vd56g3_tp_menu) - 1, mask: 0,
657 def: 0, qmenu: vd56g3_tp_menu);
658
659 ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
660 ARRAY_SIZE(vd56g3_link_freq_1lane) - 1, def: 0,
661 qmenu_int: (sensor->nb_of_lane == 2) ?
662 vd56g3_link_freq_2lanes :
663 vd56g3_link_freq_1lane);
664 if (ctrl)
665 ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
666
667 ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
668 min: sensor->pixel_clock, max: sensor->pixel_clock, step: 1,
669 def: sensor->pixel_clock);
670 if (ctrl)
671 ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
672
673 sensor->ae_ctrl = v4l2_ctrl_new_std_menu(hdl, ops,
674 V4L2_CID_EXPOSURE_AUTO,
675 max: V4L2_EXPOSURE_MANUAL, mask: 0,
676 def: V4L2_EXPOSURE_AUTO);
677
678 sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK, min: 0,
679 GENMASK(2, 0), step: 0, def: 0);
680
681 sensor->ae_bias_ctrl =
682 v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
683 ARRAY_SIZE(vd56g3_ev_bias_qmenu) - 1,
684 ARRAY_SIZE(vd56g3_ev_bias_qmenu) / 2,
685 qmenu_int: vd56g3_ev_bias_qmenu);
686
687 /*
688 * Analog gain [1, 8] is computed with the following logic :
689 * 32/(32 - again_reg), with again_reg in the range [0:28]
690 * Digital gain [1.00, 8.00] is coded as a Fixed Point 5.8
691 */
692 sensor->again_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
693 min: 0, max: 28, step: 1, def: 0);
694 sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
695 min: 0x100, max: 0x800, step: 1, def: 0x100);
696
697 /*
698 * Set the exposure, horizontal and vertical blanking ctrls
699 * to hardcoded values, they will be updated in vd56g3_update_controls.
700 * Exposure being in an auto-cluster, set a significant value here.
701 */
702 sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
703 VD56G3_EXPOSURE_DEFAULT,
704 VD56G3_EXPOSURE_DEFAULT, step: 1,
705 VD56G3_EXPOSURE_DEFAULT);
706 sensor->hblank_ctrl =
707 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, min: 1, max: 1, step: 1, def: 1);
708 if (sensor->hblank_ctrl)
709 sensor->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
710 sensor->vblank_ctrl =
711 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, min: 1, max: 1, step: 1, def: 1);
712
713 /* Additional control based on device tree properties */
714 if (sensor->ext_leds_mask)
715 sensor->led_ctrl =
716 v4l2_ctrl_new_std_menu(hdl, ops,
717 V4L2_CID_FLASH_LED_MODE,
718 max: V4L2_FLASH_LED_MODE_FLASH, mask: 0,
719 def: V4L2_FLASH_LED_MODE_NONE);
720
721 if (hdl->error) {
722 ret = hdl->error;
723 goto free_ctrls;
724 }
725
726 v4l2_ctrl_cluster(ncontrols: 2, controls: &sensor->hflip_ctrl);
727 v4l2_ctrl_auto_cluster(ncontrols: 4, controls: &sensor->ae_ctrl, manual_val: V4L2_EXPOSURE_MANUAL, set_volatile: true);
728
729 /* Optional controls coming from fwnode (e.g. rotation, orientation). */
730 ret = v4l2_fwnode_device_parse(dev: sensor->dev, props: &fwnode_props);
731 if (ret)
732 goto free_ctrls;
733
734 ret = v4l2_ctrl_new_fwnode_properties(hdl, ctrl_ops: ops, p: &fwnode_props);
735 if (ret)
736 goto free_ctrls;
737
738 sensor->sd.ctrl_handler = hdl;
739
740 return 0;
741
742free_ctrls:
743 v4l2_ctrl_handler_free(hdl);
744
745 return ret;
746}
747
748/* -----------------------------------------------------------------------------
749 * Pad ops
750 */
751
752/* Media bus code is dependent of :
753 * - 8bits or 10bits output
754 * - variant : Mono or RGB
755 * - H/V flips parameters in case of RGB
756 */
757static u32 vd56g3_get_mbus_code(struct vd56g3 *sensor, u32 code)
758{
759 unsigned int i_bpp;
760 unsigned int j;
761
762 for (i_bpp = 0; i_bpp < ARRAY_SIZE(vd56g3_mbus_codes); i_bpp++) {
763 for (j = 0; j < ARRAY_SIZE(vd56g3_mbus_codes[i_bpp]); j++) {
764 if (vd56g3_mbus_codes[i_bpp][j] == code)
765 goto endloops;
766 }
767 }
768
769endloops:
770 if (i_bpp >= ARRAY_SIZE(vd56g3_mbus_codes))
771 i_bpp = 0;
772
773 if (sensor->is_mono)
774 j = 0;
775 else
776 j = 1 + (sensor->hflip_ctrl->val ? 1 : 0) +
777 (sensor->vflip_ctrl->val ? 2 : 0);
778
779 return vd56g3_mbus_codes[i_bpp][j];
780}
781
782static int vd56g3_enum_mbus_code(struct v4l2_subdev *sd,
783 struct v4l2_subdev_state *sd_state,
784 struct v4l2_subdev_mbus_code_enum *code)
785{
786 struct vd56g3 *sensor = to_vd56g3(sd);
787
788 if (code->index >= ARRAY_SIZE(vd56g3_mbus_codes))
789 return -EINVAL;
790
791 code->code =
792 vd56g3_get_mbus_code(sensor, code: vd56g3_mbus_codes[code->index][0]);
793
794 return 0;
795}
796
797static int vd56g3_enum_frame_size(struct v4l2_subdev *sd,
798 struct v4l2_subdev_state *sd_state,
799 struct v4l2_subdev_frame_size_enum *fse)
800{
801 if (fse->index >= ARRAY_SIZE(vd56g3_supported_modes))
802 return -EINVAL;
803
804 fse->min_width = vd56g3_supported_modes[fse->index].width;
805 fse->max_width = fse->min_width;
806 fse->min_height = vd56g3_supported_modes[fse->index].height;
807 fse->max_height = fse->min_height;
808
809 return 0;
810}
811
812static void vd56g3_update_img_pad_format(struct vd56g3 *sensor,
813 const struct vd56g3_mode *mode,
814 u32 mbus_code,
815 struct v4l2_mbus_framefmt *mbus_fmt)
816{
817 mbus_fmt->width = mode->width;
818 mbus_fmt->height = mode->height;
819 mbus_fmt->code = vd56g3_get_mbus_code(sensor, code: mbus_code);
820 mbus_fmt->colorspace = V4L2_COLORSPACE_RAW;
821 mbus_fmt->field = V4L2_FIELD_NONE;
822 mbus_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
823 mbus_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
824 mbus_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
825}
826
827static int vd56g3_set_pad_fmt(struct v4l2_subdev *sd,
828 struct v4l2_subdev_state *sd_state,
829 struct v4l2_subdev_format *sd_fmt)
830{
831 struct vd56g3 *sensor = to_vd56g3(sd);
832 const struct vd56g3_mode *new_mode;
833 struct v4l2_rect pad_crop;
834 unsigned int binning;
835
836 new_mode = v4l2_find_nearest_size(vd56g3_supported_modes,
837 ARRAY_SIZE(vd56g3_supported_modes),
838 width, height, sd_fmt->format.width,
839 sd_fmt->format.height);
840
841 vd56g3_update_img_pad_format(sensor, mode: new_mode, mbus_code: sd_fmt->format.code,
842 mbus_fmt: &sd_fmt->format);
843 *v4l2_subdev_state_get_format(sd_state, sd_fmt->pad) = sd_fmt->format;
844
845 /* Compute and update crop rectangle (maximized via binning) */
846 binning = min(VD56G3_NATIVE_WIDTH / sd_fmt->format.width,
847 VD56G3_NATIVE_HEIGHT / sd_fmt->format.height);
848 binning = min(binning, 2U);
849 pad_crop.width = sd_fmt->format.width * binning;
850 pad_crop.height = sd_fmt->format.height * binning;
851 pad_crop.left = (VD56G3_NATIVE_WIDTH - pad_crop.width) / 2;
852 pad_crop.top = (VD56G3_NATIVE_HEIGHT - pad_crop.height) / 2;
853 *v4l2_subdev_state_get_crop(sd_state, sd_fmt->pad) = pad_crop;
854
855 /* Update controls in case of active state */
856 if (sd_fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
857 return vd56g3_update_controls(sensor);
858
859 return 0;
860}
861
862static int vd56g3_get_selection(struct v4l2_subdev *sd,
863 struct v4l2_subdev_state *sd_state,
864 struct v4l2_subdev_selection *sel)
865{
866 switch (sel->target) {
867 case V4L2_SEL_TGT_CROP:
868 sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
869 break;
870 case V4L2_SEL_TGT_NATIVE_SIZE:
871 case V4L2_SEL_TGT_CROP_DEFAULT:
872 case V4L2_SEL_TGT_CROP_BOUNDS:
873 sel->r.top = 0;
874 sel->r.left = 0;
875 sel->r.width = VD56G3_NATIVE_WIDTH;
876 sel->r.height = VD56G3_NATIVE_HEIGHT;
877 break;
878 default:
879 return -EINVAL;
880 }
881
882 return 0;
883}
884
885static int vd56g3_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
886 struct v4l2_mbus_frame_desc *fd)
887{
888 struct v4l2_subdev_state *state;
889 const struct v4l2_mbus_framefmt *format;
890
891 state = v4l2_subdev_lock_and_get_active_state(sd);
892 format = v4l2_subdev_state_get_format(state, pad);
893 v4l2_subdev_unlock_state(state);
894
895 fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
896 fd->num_entries = 1;
897 fd->entry[0].pixelcode = format->code;
898 fd->entry[0].stream = 0;
899 fd->entry[0].bus.csi2.vc = 0;
900 fd->entry[0].bus.csi2.dt = vd56g3_get_datatype(code: format->code);
901
902 return 0;
903}
904
905static int vd56g3_enable_streams(struct v4l2_subdev *sd,
906 struct v4l2_subdev_state *state, u32 pad,
907 u64 streams_mask)
908{
909 struct vd56g3 *sensor = to_vd56g3(sd);
910 const struct v4l2_mbus_framefmt *format =
911 v4l2_subdev_state_get_format(state, 0);
912 const struct v4l2_rect *crop = v4l2_subdev_state_get_crop(state, 0);
913 unsigned int csi_mbps = ((sensor->nb_of_lane == 2) ?
914 VD56G3_LINK_FREQ_DEF_2LANES :
915 VD56G3_LINK_FREQ_DEF_1LANE) *
916 2 / MEGA;
917 unsigned int binning;
918 int ret;
919
920 ret = pm_runtime_resume_and_get(dev: sensor->dev);
921 if (ret < 0)
922 return ret;
923
924 /* configure clocks */
925 cci_write(map: sensor->regmap, VD56G3_REG_EXT_CLOCK, val: sensor->xclk_freq,
926 err: &ret);
927 cci_write(map: sensor->regmap, VD56G3_REG_CLK_PLL_PREDIV, val: sensor->pll_prediv,
928 err: &ret);
929 cci_write(map: sensor->regmap, VD56G3_REG_CLK_SYS_PLL_MULT, val: sensor->pll_mult,
930 err: &ret);
931
932 /* configure output */
933 cci_write(map: sensor->regmap, VD56G3_REG_FORMAT_CTRL,
934 val: vd56g3_get_bpp(code: format->code), err: &ret);
935 cci_write(map: sensor->regmap, VD56G3_REG_OIF_CTRL, val: sensor->oif_ctrl, err: &ret);
936 cci_write(map: sensor->regmap, VD56G3_REG_OIF_CSI_BITRATE, val: csi_mbps, err: &ret);
937 cci_write(map: sensor->regmap, VD56G3_REG_OIF_IMG_CTRL,
938 val: vd56g3_get_datatype(code: format->code), err: &ret);
939 cci_write(map: sensor->regmap, VD56G3_REG_ISL_ENABLE, val: 0, err: &ret);
940
941 /* configure binning mode */
942 switch (crop->width / format->width) {
943 case 1:
944 default:
945 binning = READOUT_NORMAL;
946 break;
947 case 2:
948 binning = READOUT_DIGITAL_BINNING_X2;
949 break;
950 }
951 cci_write(map: sensor->regmap, VD56G3_REG_READOUT_CTRL, val: binning, err: &ret);
952
953 /* configure ROIs */
954 cci_write(map: sensor->regmap, VD56G3_REG_Y_START, val: crop->top, err: &ret);
955 cci_write(map: sensor->regmap, VD56G3_REG_Y_END,
956 val: crop->top + crop->height - 1, err: &ret);
957 cci_write(map: sensor->regmap, VD56G3_REG_OUT_ROI_X_START, val: crop->left, err: &ret);
958 cci_write(map: sensor->regmap, VD56G3_REG_OUT_ROI_X_END,
959 val: crop->left + crop->width - 1, err: &ret);
960 cci_write(map: sensor->regmap, VD56G3_REG_OUT_ROI_Y_START, val: 0, err: &ret);
961 cci_write(map: sensor->regmap, VD56G3_REG_OUT_ROI_Y_END, val: crop->height - 1,
962 err: &ret);
963 cci_write(map: sensor->regmap, VD56G3_REG_AE_ROI_START_H, val: crop->left, err: &ret);
964 cci_write(map: sensor->regmap, VD56G3_REG_AE_ROI_END_H,
965 val: crop->left + crop->width - 1, err: &ret);
966 cci_write(map: sensor->regmap, VD56G3_REG_AE_ROI_START_V, val: 0, err: &ret);
967 cci_write(map: sensor->regmap, VD56G3_REG_AE_ROI_END_V, val: crop->height - 1,
968 err: &ret);
969 if (ret)
970 goto rpm_put;
971
972 /* Setup default GPIO values; could be overridden by V4L2 ctrl setup */
973 ret = vd56g3_write_gpiox(sensor, GENMASK(VD56G3_NB_GPIOS - 1, 0));
974 if (ret)
975 goto rpm_put;
976
977 /* Apply settings from V4L2 ctrls */
978 ret = __v4l2_ctrl_handler_setup(hdl: &sensor->ctrl_handler);
979 if (ret)
980 goto rpm_put;
981
982 /* start streaming */
983 cci_write(map: sensor->regmap, VD56G3_REG_STBY, VD56G3_CMD_START_STREAM,
984 err: &ret);
985 vd56g3_poll_reg(sensor, VD56G3_REG_STBY, VD56G3_CMD_ACK, err: &ret);
986 vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_STREAMING, err: &ret);
987 if (ret)
988 goto rpm_put;
989
990 /* some controls are locked during streaming */
991 __v4l2_ctrl_grab(ctrl: sensor->hflip_ctrl, grabbed: true);
992 __v4l2_ctrl_grab(ctrl: sensor->vflip_ctrl, grabbed: true);
993 __v4l2_ctrl_grab(ctrl: sensor->patgen_ctrl, grabbed: true);
994
995 return ret;
996
997rpm_put:
998 dev_err(sensor->dev, "Failed to start streaming\n");
999 pm_runtime_put_sync(dev: sensor->dev);
1000
1001 return ret;
1002}
1003
1004static int vd56g3_disable_streams(struct v4l2_subdev *sd,
1005 struct v4l2_subdev_state *state, u32 pad,
1006 u64 streams_mask)
1007{
1008 struct vd56g3 *sensor = to_vd56g3(sd);
1009 int ret;
1010
1011 /* Retrieve Expo cluster to enable coldstart of AE */
1012 ret = vd56g3_read_expo_cluster(sensor, force_cur_val: true);
1013
1014 cci_write(map: sensor->regmap, VD56G3_REG_STREAMING, VD56G3_CMD_STOP_STREAM,
1015 err: &ret);
1016 vd56g3_poll_reg(sensor, VD56G3_REG_STREAMING, VD56G3_CMD_ACK, err: &ret);
1017 vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, err: &ret);
1018
1019 /* locked controls must be unlocked */
1020 __v4l2_ctrl_grab(ctrl: sensor->hflip_ctrl, grabbed: false);
1021 __v4l2_ctrl_grab(ctrl: sensor->vflip_ctrl, grabbed: false);
1022 __v4l2_ctrl_grab(ctrl: sensor->patgen_ctrl, grabbed: false);
1023
1024 pm_runtime_mark_last_busy(dev: sensor->dev);
1025 pm_runtime_put_autosuspend(dev: sensor->dev);
1026
1027 return ret;
1028}
1029
1030static int vd56g3_init_state(struct v4l2_subdev *sd,
1031 struct v4l2_subdev_state *sd_state)
1032{
1033 unsigned int def_mode = VD56G3_DEFAULT_MODE;
1034 struct v4l2_subdev_format fmt = {
1035 .which = V4L2_SUBDEV_FORMAT_TRY,
1036 .pad = 0,
1037 .format = {
1038 .code = vd56g3_mbus_codes[0][0],
1039 .width = vd56g3_supported_modes[def_mode].width,
1040 .height = vd56g3_supported_modes[def_mode].height,
1041 },
1042 };
1043
1044 return vd56g3_set_pad_fmt(sd, sd_state, sd_fmt: &fmt);
1045}
1046
1047static const struct v4l2_subdev_video_ops vd56g3_video_ops = {
1048 .s_stream = v4l2_subdev_s_stream_helper,
1049};
1050
1051static const struct v4l2_subdev_pad_ops vd56g3_pad_ops = {
1052 .enum_mbus_code = vd56g3_enum_mbus_code,
1053 .enum_frame_size = vd56g3_enum_frame_size,
1054 .get_fmt = v4l2_subdev_get_fmt,
1055 .set_fmt = vd56g3_set_pad_fmt,
1056 .get_selection = vd56g3_get_selection,
1057 .get_frame_desc = vd56g3_get_frame_desc,
1058 .enable_streams = vd56g3_enable_streams,
1059 .disable_streams = vd56g3_disable_streams,
1060};
1061
1062static const struct v4l2_subdev_ops vd56g3_subdev_ops = {
1063 .video = &vd56g3_video_ops,
1064 .pad = &vd56g3_pad_ops,
1065};
1066
1067static const struct media_entity_operations vd56g3_subdev_entity_ops = {
1068 .link_validate = v4l2_subdev_link_validate,
1069};
1070
1071static const struct v4l2_subdev_internal_ops vd56g3_internal_ops = {
1072 .init_state = vd56g3_init_state,
1073};
1074
1075/* -----------------------------------------------------------------------------
1076 * Power management
1077 */
1078
1079static int vd56g3_power_on(struct device *dev)
1080{
1081 struct v4l2_subdev *sd = dev_get_drvdata(dev);
1082 struct vd56g3 *sensor = to_vd56g3(sd);
1083 int ret;
1084
1085 /* power on */
1086 ret = regulator_bulk_enable(ARRAY_SIZE(sensor->supplies),
1087 consumers: sensor->supplies);
1088 if (ret) {
1089 dev_err(dev, "Failed to enable regulators: %d\n", ret);
1090 return ret;
1091 }
1092
1093 ret = clk_prepare_enable(clk: sensor->xclk);
1094 if (ret) {
1095 dev_err(dev, "Failed to enable clock: %d\n", ret);
1096 goto disable_reg;
1097 }
1098
1099 gpiod_set_value_cansleep(desc: sensor->reset_gpio, value: 0);
1100 usleep_range(min: 3500, max: 4000);
1101 ret = vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_READY_TO_BOOT, NULL);
1102 if (ret) {
1103 dev_err(dev, "Sensor reset failed: %d\n", ret);
1104 goto disable_clock;
1105 }
1106
1107 /* boot sensor */
1108 cci_write(map: sensor->regmap, VD56G3_REG_BOOT, VD56G3_CMD_BOOT, err: &ret);
1109 vd56g3_poll_reg(sensor, VD56G3_REG_BOOT, VD56G3_CMD_ACK, err: &ret);
1110 vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, err: &ret);
1111 if (ret) {
1112 dev_err(dev, "Sensor boot failed: %d\n", ret);
1113 goto disable_clock;
1114 }
1115
1116 return 0;
1117
1118disable_clock:
1119 gpiod_set_value_cansleep(desc: sensor->reset_gpio, value: 1);
1120 clk_disable_unprepare(clk: sensor->xclk);
1121disable_reg:
1122 regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), consumers: sensor->supplies);
1123
1124 return ret;
1125}
1126
1127static int vd56g3_power_off(struct device *dev)
1128{
1129 struct v4l2_subdev *sd = dev_get_drvdata(dev);
1130 struct vd56g3 *sensor = to_vd56g3(sd);
1131
1132 gpiod_set_value_cansleep(desc: sensor->reset_gpio, value: 1);
1133 clk_disable_unprepare(clk: sensor->xclk);
1134 regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), consumers: sensor->supplies);
1135
1136 return 0;
1137}
1138
1139static const struct dev_pm_ops vd56g3_pm_ops = {
1140 SET_RUNTIME_PM_OPS(vd56g3_power_off, vd56g3_power_on, NULL)
1141};
1142
1143/* -----------------------------------------------------------------------------
1144 * Probe and initialization
1145 */
1146
1147static int vd56g3_check_csi_conf(struct vd56g3 *sensor,
1148 struct fwnode_handle *endpoint)
1149{
1150 struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
1151 u32 phy_data_lanes[VD56G3_MAX_CSI_DATA_LANES] = { ~0, ~0 };
1152 u8 n_lanes;
1153 u64 frequency;
1154 int p, l;
1155 int ret = 0;
1156
1157 ret = v4l2_fwnode_endpoint_alloc_parse(fwnode: endpoint, vep: &ep);
1158 if (ret)
1159 return -EINVAL;
1160
1161 /* Check lanes number */
1162 n_lanes = ep.bus.mipi_csi2.num_data_lanes;
1163 if (n_lanes != 1 && n_lanes != 2) {
1164 dev_err(sensor->dev, "Invalid data lane number: %d\n", n_lanes);
1165 ret = -EINVAL;
1166 goto done;
1167 }
1168 sensor->nb_of_lane = n_lanes;
1169
1170 /* Clock lane must be first */
1171 if (ep.bus.mipi_csi2.clock_lane != 0) {
1172 dev_err(sensor->dev, "Clock lane must be mapped to lane 0\n");
1173 ret = -EINVAL;
1174 goto done;
1175 }
1176
1177 /*
1178 * Prepare Output Interface conf based on lane settings
1179 * logical to physical lane conversion (+ pad remaining slots)
1180 */
1181 for (l = 0; l < n_lanes; l++)
1182 phy_data_lanes[ep.bus.mipi_csi2.data_lanes[l] - 1] = l;
1183 for (p = 0; p < VD56G3_MAX_CSI_DATA_LANES; p++) {
1184 if (phy_data_lanes[p] != ~0)
1185 continue;
1186 phy_data_lanes[p] = l;
1187 l++;
1188 }
1189 sensor->oif_ctrl = n_lanes |
1190 (ep.bus.mipi_csi2.lane_polarities[0] << 3) |
1191 ((phy_data_lanes[0]) << 4) |
1192 (ep.bus.mipi_csi2.lane_polarities[1] << 6) |
1193 ((phy_data_lanes[1]) << 7) |
1194 (ep.bus.mipi_csi2.lane_polarities[2] << 9);
1195
1196 /* Check link frequency */
1197 if (!ep.nr_of_link_frequencies) {
1198 dev_err(sensor->dev, "link-frequency not found in DT\n");
1199 ret = -EINVAL;
1200 goto done;
1201 }
1202 frequency = (n_lanes == 2) ? VD56G3_LINK_FREQ_DEF_2LANES :
1203 VD56G3_LINK_FREQ_DEF_1LANE;
1204 if (ep.nr_of_link_frequencies != 1 ||
1205 ep.link_frequencies[0] != frequency) {
1206 dev_err(sensor->dev, "Link frequency not supported: %lld\n",
1207 ep.link_frequencies[0]);
1208 ret = -EINVAL;
1209 goto done;
1210 }
1211
1212done:
1213 v4l2_fwnode_endpoint_free(vep: &ep);
1214
1215 return ret;
1216}
1217
1218static int vd56g3_parse_dt_gpios_array(struct vd56g3 *sensor, char *prop_name,
1219 u32 *array, unsigned int *nb)
1220{
1221 struct device *dev = sensor->dev;
1222 unsigned int i;
1223 int ret;
1224
1225 if (!device_property_present(dev, propname: prop_name)) {
1226 *nb = 0;
1227 return 0;
1228 }
1229
1230 ret = device_property_count_u32(dev, propname: prop_name);
1231 if (ret < 0) {
1232 dev_err(dev, "Failed to read %s count\n", prop_name);
1233 return ret;
1234 }
1235
1236 *nb = ret;
1237 ret = device_property_read_u32_array(dev, propname: prop_name, val: array, nval: *nb);
1238 if (ret) {
1239 dev_err(dev, "Failed to read %s prop\n", prop_name);
1240 return ret;
1241 }
1242
1243 for (i = 0; i < *nb; i++) {
1244 if (array[i] >= VD56G3_NB_GPIOS) {
1245 dev_err(dev, "Invalid GPIO: %d\n", array[i]);
1246 return -EINVAL;
1247 }
1248 }
1249
1250 return 0;
1251}
1252
1253static int vd56g3_parse_dt_gpios(struct vd56g3 *sensor)
1254{
1255 u32 led_gpios[VD56G3_NB_GPIOS];
1256 unsigned int nb_gpios_leds;
1257 unsigned int i;
1258 int ret;
1259
1260 /* Initialize GPIOs to default */
1261 for (i = 0; i < VD56G3_NB_GPIOS; i++)
1262 sensor->gpios[i] = VD56G3_GPIOX_GPIO_IN;
1263 sensor->ext_leds_mask = 0;
1264
1265 /* Take into account optional 'st,leds' output for GPIOs */
1266 ret = vd56g3_parse_dt_gpios_array(sensor, prop_name: "st,leds", array: led_gpios,
1267 nb: &nb_gpios_leds);
1268 if (ret)
1269 return ret;
1270 for (i = 0; i < nb_gpios_leds; i++) {
1271 sensor->gpios[led_gpios[i]] = VD56G3_GPIOX_STROBE_MODE;
1272 set_bit(nr: led_gpios[i], addr: &sensor->ext_leds_mask);
1273 }
1274
1275 return 0;
1276}
1277
1278static int vd56g3_parse_dt(struct vd56g3 *sensor)
1279{
1280 struct fwnode_handle *endpoint;
1281 int ret;
1282
1283 endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(sensor->dev), port: 0,
1284 endpoint: 0, flags: 0);
1285 if (!endpoint) {
1286 dev_err(sensor->dev, "Endpoint node not found\n");
1287 return -EINVAL;
1288 }
1289
1290 ret = vd56g3_check_csi_conf(sensor, endpoint);
1291 fwnode_handle_put(fwnode: endpoint);
1292 if (ret)
1293 return ret;
1294
1295 return vd56g3_parse_dt_gpios(sensor);
1296}
1297
1298static int vd56g3_get_regulators(struct vd56g3 *sensor)
1299{
1300 unsigned int i;
1301
1302 for (i = 0; i < ARRAY_SIZE(sensor->supplies); i++)
1303 sensor->supplies[i].supply = vd56g3_supply_names[i];
1304
1305 return devm_regulator_bulk_get(dev: sensor->dev,
1306 ARRAY_SIZE(sensor->supplies),
1307 consumers: sensor->supplies);
1308}
1309
1310static int vd56g3_prepare_clock_tree(struct vd56g3 *sensor)
1311{
1312 const unsigned int predivs[] = { 1, 2, 4 };
1313 u32 pll_out;
1314 int i;
1315
1316 /* External clock must be in [6Mhz-27Mhz] */
1317 if (sensor->xclk_freq < VD56G3_XCLK_FREQ_MIN ||
1318 sensor->xclk_freq > VD56G3_XCLK_FREQ_MAX) {
1319 dev_err(sensor->dev,
1320 "Only 6Mhz-27Mhz clock range supported. Provided %lu MHz\n",
1321 sensor->xclk_freq / HZ_PER_MHZ);
1322 return -EINVAL;
1323 }
1324
1325 /* PLL input should be in [6Mhz-12Mhz[ */
1326 for (i = 0; i < ARRAY_SIZE(predivs); i++) {
1327 sensor->pll_prediv = predivs[i];
1328 if (sensor->xclk_freq / sensor->pll_prediv < 12 * HZ_PER_MHZ)
1329 break;
1330 }
1331
1332 /* PLL output clock must be as close as possible to 804Mhz */
1333 sensor->pll_mult = (VD56G3_TARGET_PLL * sensor->pll_prediv +
1334 sensor->xclk_freq / 2) /
1335 sensor->xclk_freq;
1336 pll_out = sensor->xclk_freq * sensor->pll_mult / sensor->pll_prediv;
1337
1338 /* Target Pixel Clock for standard 10bit ADC mode : 160.8Mhz */
1339 sensor->pixel_clock = pll_out / VD56G3_VT_CLOCK_DIV;
1340
1341 return 0;
1342}
1343
1344static int vd56g3_detect(struct vd56g3 *sensor)
1345{
1346 struct device *dev = sensor->dev;
1347 unsigned int model;
1348 u64 model_id;
1349 u64 device_revision;
1350 u64 optical_revision;
1351 int ret = 0;
1352
1353 model = (uintptr_t)device_get_match_data(dev);
1354
1355 ret = cci_read(map: sensor->regmap, VD56G3_REG_MODEL_ID, val: &model_id, NULL);
1356 if (ret)
1357 return ret;
1358
1359 if (model_id != VD56G3_MODEL_ID) {
1360 dev_err(dev, "Unsupported sensor id: %x\n", (u16)model_id);
1361 return -ENODEV;
1362 }
1363
1364 ret = cci_read(map: sensor->regmap, VD56G3_REG_REVISION, val: &device_revision,
1365 NULL);
1366 if (ret)
1367 return ret;
1368
1369 if ((device_revision >> 8) != VD56G3_REVISION_CUT3) {
1370 dev_err(dev, "Unsupported version: %x\n", (u16)device_revision);
1371 return -ENODEV;
1372 }
1373
1374 ret = cci_read(map: sensor->regmap, VD56G3_REG_OPTICAL_REVISION,
1375 val: &optical_revision, NULL);
1376 if (ret)
1377 return ret;
1378
1379 sensor->is_mono =
1380 ((optical_revision & 1) == VD56G3_OPTICAL_REVISION_MONO);
1381 if ((sensor->is_mono && model == VD56G3_MODEL_VD66GY) ||
1382 (!sensor->is_mono && model == VD56G3_MODEL_VD56G3)) {
1383 dev_err(dev, "Found %s sensor, while %s model is defined in DT\n",
1384 (sensor->is_mono) ? "Mono" : "Bayer",
1385 (model == VD56G3_MODEL_VD56G3) ? "vd56g3" : "vd66gy");
1386 return -ENODEV;
1387 }
1388
1389 return 0;
1390}
1391
1392static int vd56g3_subdev_init(struct vd56g3 *sensor)
1393{
1394 struct v4l2_subdev_state *state;
1395 int ret;
1396
1397 /* Init remaining sub device ops */
1398 sensor->sd.internal_ops = &vd56g3_internal_ops;
1399 sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1400 sensor->sd.entity.ops = &vd56g3_subdev_entity_ops;
1401
1402 /* Init source pad */
1403 sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
1404 sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1405 ret = media_entity_pads_init(entity: &sensor->sd.entity, num_pads: 1, pads: &sensor->pad);
1406 if (ret) {
1407 dev_err(sensor->dev, "Failed to init media entity: %d\n", ret);
1408 return ret;
1409 }
1410
1411 /* Init controls */
1412 ret = vd56g3_init_controls(sensor);
1413 if (ret) {
1414 dev_err(sensor->dev, "Controls initialization failed: %d\n",
1415 ret);
1416 goto err_media;
1417 }
1418
1419 /* Init vd56g3 struct : default resolution + raw8 */
1420 sensor->sd.state_lock = sensor->ctrl_handler.lock;
1421 ret = v4l2_subdev_init_finalize(&sensor->sd);
1422 if (ret) {
1423 dev_err(sensor->dev, "Subdev init failed: %d\n", ret);
1424 goto err_ctrls;
1425 }
1426
1427 /* Update controls according to the resolution set */
1428 state = v4l2_subdev_lock_and_get_active_state(sd: &sensor->sd);
1429 ret = vd56g3_update_controls(sensor);
1430 v4l2_subdev_unlock_state(state);
1431 if (ret) {
1432 dev_err(sensor->dev, "Controls update failed: %d\n", ret);
1433 goto err_ctrls;
1434 }
1435
1436 return 0;
1437
1438err_ctrls:
1439 v4l2_ctrl_handler_free(hdl: sensor->sd.ctrl_handler);
1440
1441err_media:
1442 media_entity_cleanup(entity: &sensor->sd.entity);
1443
1444 return ret;
1445}
1446
1447static void vd56g3_subdev_cleanup(struct vd56g3 *sensor)
1448{
1449 v4l2_async_unregister_subdev(sd: &sensor->sd);
1450 v4l2_subdev_cleanup(sd: &sensor->sd);
1451 media_entity_cleanup(entity: &sensor->sd.entity);
1452 v4l2_ctrl_handler_free(hdl: sensor->sd.ctrl_handler);
1453}
1454
1455static int vd56g3_probe(struct i2c_client *client)
1456{
1457 struct device *dev = &client->dev;
1458 struct vd56g3 *sensor;
1459 int ret;
1460
1461 sensor = devm_kzalloc(dev, size: sizeof(*sensor), GFP_KERNEL);
1462 if (!sensor)
1463 return -ENOMEM;
1464
1465 v4l2_i2c_subdev_init(sd: &sensor->sd, client, ops: &vd56g3_subdev_ops);
1466 sensor->dev = dev;
1467
1468 ret = vd56g3_parse_dt(sensor);
1469 if (ret)
1470 return dev_err_probe(dev, err: ret, fmt: "Failed to parse Device Tree\n");
1471
1472 /* Get (and check) resources : power regs, ext clock, reset gpio */
1473 ret = vd56g3_get_regulators(sensor);
1474 if (ret)
1475 return dev_err_probe(dev, err: ret, fmt: "Failed to get regulators\n");
1476
1477 sensor->xclk = devm_clk_get(dev, NULL);
1478 if (IS_ERR(ptr: sensor->xclk))
1479 return dev_err_probe(dev, err: PTR_ERR(ptr: sensor->xclk),
1480 fmt: "Failed to get xclk\n");
1481 sensor->xclk_freq = clk_get_rate(clk: sensor->xclk);
1482 ret = vd56g3_prepare_clock_tree(sensor);
1483 if (ret)
1484 return ret;
1485
1486 sensor->reset_gpio = devm_gpiod_get_optional(dev, con_id: "reset",
1487 flags: GPIOD_OUT_HIGH);
1488 if (IS_ERR(ptr: sensor->reset_gpio))
1489 return dev_err_probe(dev, err: PTR_ERR(ptr: sensor->reset_gpio),
1490 fmt: "Failed to get reset gpio\n");
1491
1492 sensor->regmap = devm_cci_regmap_init_i2c(client, reg_addr_bits: 16);
1493 if (IS_ERR(ptr: sensor->regmap))
1494 return dev_err_probe(dev, err: PTR_ERR(ptr: sensor->regmap),
1495 fmt: "Failed to init regmap\n");
1496
1497 /* Power ON */
1498 ret = vd56g3_power_on(dev);
1499 if (ret)
1500 return dev_err_probe(dev, err: ret, fmt: "Sensor power on failed\n");
1501
1502 /* Enable PM runtime with autosuspend (sensor being ON, set active) */
1503 pm_runtime_set_active(dev);
1504 pm_runtime_get_noresume(dev);
1505 pm_runtime_enable(dev);
1506 pm_runtime_set_autosuspend_delay(dev, delay: 1000);
1507 pm_runtime_use_autosuspend(dev);
1508
1509 /* Check HW model/version */
1510 ret = vd56g3_detect(sensor);
1511 if (ret) {
1512 dev_err(dev, "Sensor detect failed: %d\n", ret);
1513 goto err_power_off;
1514 }
1515
1516 /* Initialize & register subdev (v4l2_i2c subdev already initialized) */
1517 ret = vd56g3_subdev_init(sensor);
1518 if (ret) {
1519 dev_err(dev, "V4l2 init failed: %d\n", ret);
1520 goto err_power_off;
1521 }
1522
1523 ret = v4l2_async_register_subdev(&sensor->sd);
1524 if (ret) {
1525 dev_err(dev, "Async subdev register failed: %d\n", ret);
1526 goto err_subdev;
1527 }
1528
1529 /* Sensor could now be powered off (after the autosuspend delay) */
1530 pm_runtime_mark_last_busy(dev);
1531 pm_runtime_put_autosuspend(dev);
1532
1533 dev_dbg(dev, "Successfully probe %s sensor\n",
1534 (sensor->is_mono) ? "vd56g3" : "vd66gy");
1535
1536 return 0;
1537
1538err_subdev:
1539 vd56g3_subdev_cleanup(sensor);
1540err_power_off:
1541 pm_runtime_disable(dev);
1542 pm_runtime_put_noidle(dev);
1543 pm_runtime_dont_use_autosuspend(dev);
1544 vd56g3_power_off(dev);
1545
1546 return ret;
1547}
1548
1549static void vd56g3_remove(struct i2c_client *client)
1550{
1551 struct v4l2_subdev *sd = i2c_get_clientdata(client);
1552 struct vd56g3 *sensor = to_vd56g3(sd);
1553
1554 vd56g3_subdev_cleanup(sensor);
1555
1556 pm_runtime_disable(dev: sensor->dev);
1557 if (!pm_runtime_status_suspended(dev: sensor->dev))
1558 vd56g3_power_off(dev: sensor->dev);
1559 pm_runtime_set_suspended(dev: sensor->dev);
1560 pm_runtime_dont_use_autosuspend(dev: sensor->dev);
1561}
1562
1563static const struct of_device_id vd56g3_dt_ids[] = {
1564 { .compatible = "st,vd56g3", .data = (void *)VD56G3_MODEL_VD56G3 },
1565 { .compatible = "st,vd66gy", .data = (void *)VD56G3_MODEL_VD66GY },
1566 { /* sentinel */ }
1567};
1568MODULE_DEVICE_TABLE(of, vd56g3_dt_ids);
1569
1570static struct i2c_driver vd56g3_i2c_driver = {
1571 .driver = {
1572 .name = "vd56g3",
1573 .of_match_table = vd56g3_dt_ids,
1574 .pm = &vd56g3_pm_ops,
1575 },
1576 .probe = vd56g3_probe,
1577 .remove = vd56g3_remove,
1578};
1579
1580module_i2c_driver(vd56g3_i2c_driver);
1581
1582MODULE_AUTHOR("Benjamin Mugnier <benjamin.mugnier@foss.st.com>");
1583MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>");
1584MODULE_AUTHOR("Sylvain Petinot <sylvain.petinot@foss.st.com>");
1585MODULE_DESCRIPTION("ST VD56G3 sensor driver");
1586MODULE_LICENSE("GPL");
1587

source code of linux/drivers/media/i2c/vd56g3.c