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 249 static inline struct vd56g3 *to_vd56g3(struct v4l2_subdev *sd) 250 { 251 return container_of_const(sd, struct vd56g3, sd); 252 } 253 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 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 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 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 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 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 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 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 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 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 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 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 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 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 */ 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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