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_mark_last_busy(sensor->dev); 497 pm_runtime_put_autosuspend(sensor->dev); 498 499 return ret; 500 } 501 502 static 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(&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(sensor->expo_ctrl, 525 VD56G3_EXPOSURE_MIN, expo_max, 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(sensor->ae_lock_ctrl, !is_auto); 532 __v4l2_ctrl_grab(sensor->ae_bias_ctrl, !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(sensor->dev)) 543 return 0; 544 545 switch (ctrl->id) { 546 case V4L2_CID_HFLIP: 547 ret = cci_write(sensor->regmap, VD56G3_REG_ORIENTATION, 548 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, 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, 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(sensor->regmap, VD56G3_REG_AE_COMPENSATION, 566 ae_compensation, NULL); 567 break; 568 case V4L2_CID_VBLANK: 569 ret = cci_write(sensor->regmap, VD56G3_REG_FRAME_LENGTH, 570 frame_length, NULL); 571 break; 572 case V4L2_CID_FLASH_LED_MODE: 573 ret = vd56g3_write_gpiox(sensor, sensor->ext_leds_mask); 574 break; 575 default: 576 ret = -EINVAL; 577 break; 578 } 579 580 pm_runtime_mark_last_busy(sensor->dev); 581 pm_runtime_put_autosuspend(sensor->dev); 582 583 return ret; 584 } 585 586 static const struct v4l2_ctrl_ops vd56g3_ctrl_ops = { 587 .g_volatile_ctrl = vd56g3_g_volatile_ctrl, 588 .s_ctrl = vd56g3_s_ctrl, 589 }; 590 591 static 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(&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(sensor->hblank_ctrl, hblank, hblank, 1, 612 hblank); 613 if (ret) 614 return ret; 615 616 ret = __v4l2_ctrl_modify_range(sensor->vblank_ctrl, vblank_min, 617 vblank_max, 1, vblank); 618 if (ret) 619 return ret; 620 621 ret = __v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, vblank); 622 if (ret) 623 return ret; 624 625 ret = __v4l2_ctrl_modify_range(sensor->expo_ctrl, VD56G3_EXPOSURE_MIN, 626 expo_max, 1, VD56G3_EXPOSURE_DEFAULT); 627 if (ret) 628 return ret; 629 630 return __v4l2_ctrl_s_ctrl(sensor->expo_ctrl, VD56G3_EXPOSURE_DEFAULT); 631 } 632 633 static 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, 0, 1, 1, 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, 0, 1, 1, 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, 0, 657 0, 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, 0, 661 (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 sensor->pixel_clock, sensor->pixel_clock, 1, 669 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 V4L2_EXPOSURE_MANUAL, 0, 676 V4L2_EXPOSURE_AUTO); 677 678 sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK, 0, 679 GENMASK(2, 0), 0, 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 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 0, 28, 1, 0); 694 sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN, 695 0x100, 0x800, 1, 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, 1, 705 VD56G3_EXPOSURE_DEFAULT); 706 sensor->hblank_ctrl = 707 v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 1, 1, 1, 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, 1, 1, 1, 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 V4L2_FLASH_LED_MODE_FLASH, 0, 719 V4L2_FLASH_LED_MODE_NONE); 720 721 if (hdl->error) { 722 ret = hdl->error; 723 goto free_ctrls; 724 } 725 726 v4l2_ctrl_cluster(2, &sensor->hflip_ctrl); 727 v4l2_ctrl_auto_cluster(4, &sensor->ae_ctrl, V4L2_EXPOSURE_MANUAL, true); 728 729 /* Optional controls coming from fwnode (e.g. rotation, orientation). */ 730 ret = v4l2_fwnode_device_parse(sensor->dev, &fwnode_props); 731 if (ret) 732 goto free_ctrls; 733 734 ret = v4l2_ctrl_new_fwnode_properties(hdl, ops, &fwnode_props); 735 if (ret) 736 goto free_ctrls; 737 738 sensor->sd.ctrl_handler = hdl; 739 740 return 0; 741 742 free_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 */ 757 static 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 769 endloops: 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 782 static 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, vd56g3_mbus_codes[code->index][0]); 793 794 return 0; 795 } 796 797 static 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 812 static 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, 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 827 static 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, new_mode, sd_fmt->format.code, 842 &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 862 static 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 885 static 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(format->code); 901 902 return 0; 903 } 904 905 static 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(sensor->dev); 921 if (ret < 0) 922 return ret; 923 924 /* configure clocks */ 925 cci_write(sensor->regmap, VD56G3_REG_EXT_CLOCK, sensor->xclk_freq, 926 &ret); 927 cci_write(sensor->regmap, VD56G3_REG_CLK_PLL_PREDIV, sensor->pll_prediv, 928 &ret); 929 cci_write(sensor->regmap, VD56G3_REG_CLK_SYS_PLL_MULT, sensor->pll_mult, 930 &ret); 931 932 /* configure output */ 933 cci_write(sensor->regmap, VD56G3_REG_FORMAT_CTRL, 934 vd56g3_get_bpp(format->code), &ret); 935 cci_write(sensor->regmap, VD56G3_REG_OIF_CTRL, sensor->oif_ctrl, &ret); 936 cci_write(sensor->regmap, VD56G3_REG_OIF_CSI_BITRATE, csi_mbps, &ret); 937 cci_write(sensor->regmap, VD56G3_REG_OIF_IMG_CTRL, 938 vd56g3_get_datatype(format->code), &ret); 939 cci_write(sensor->regmap, VD56G3_REG_ISL_ENABLE, 0, &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(sensor->regmap, VD56G3_REG_READOUT_CTRL, binning, &ret); 952 953 /* configure ROIs */ 954 cci_write(sensor->regmap, VD56G3_REG_Y_START, crop->top, &ret); 955 cci_write(sensor->regmap, VD56G3_REG_Y_END, 956 crop->top + crop->height - 1, &ret); 957 cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_START, crop->left, &ret); 958 cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_END, 959 crop->left + crop->width - 1, &ret); 960 cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_START, 0, &ret); 961 cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_END, crop->height - 1, 962 &ret); 963 cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_H, crop->left, &ret); 964 cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_H, 965 crop->left + crop->width - 1, &ret); 966 cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_V, 0, &ret); 967 cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_V, crop->height - 1, 968 &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(&sensor->ctrl_handler); 979 if (ret) 980 goto rpm_put; 981 982 /* start streaming */ 983 cci_write(sensor->regmap, VD56G3_REG_STBY, VD56G3_CMD_START_STREAM, 984 &ret); 985 vd56g3_poll_reg(sensor, VD56G3_REG_STBY, VD56G3_CMD_ACK, &ret); 986 vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_STREAMING, &ret); 987 if (ret) 988 goto rpm_put; 989 990 /* some controls are locked during streaming */ 991 __v4l2_ctrl_grab(sensor->hflip_ctrl, true); 992 __v4l2_ctrl_grab(sensor->vflip_ctrl, true); 993 __v4l2_ctrl_grab(sensor->patgen_ctrl, true); 994 995 return ret; 996 997 rpm_put: 998 dev_err(sensor->dev, "Failed to start streaming\n"); 999 pm_runtime_put_sync(sensor->dev); 1000 1001 return ret; 1002 } 1003 1004 static 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, true); 1013 1014 cci_write(sensor->regmap, VD56G3_REG_STREAMING, VD56G3_CMD_STOP_STREAM, 1015 &ret); 1016 vd56g3_poll_reg(sensor, VD56G3_REG_STREAMING, VD56G3_CMD_ACK, &ret); 1017 vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret); 1018 1019 /* locked controls must be unlocked */ 1020 __v4l2_ctrl_grab(sensor->hflip_ctrl, false); 1021 __v4l2_ctrl_grab(sensor->vflip_ctrl, false); 1022 __v4l2_ctrl_grab(sensor->patgen_ctrl, false); 1023 1024 pm_runtime_mark_last_busy(sensor->dev); 1025 pm_runtime_put_autosuspend(sensor->dev); 1026 1027 return ret; 1028 } 1029 1030 static 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, &fmt); 1045 } 1046 1047 static const struct v4l2_subdev_video_ops vd56g3_video_ops = { 1048 .s_stream = v4l2_subdev_s_stream_helper, 1049 }; 1050 1051 static 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 1062 static const struct v4l2_subdev_ops vd56g3_subdev_ops = { 1063 .video = &vd56g3_video_ops, 1064 .pad = &vd56g3_pad_ops, 1065 }; 1066 1067 static const struct media_entity_operations vd56g3_subdev_entity_ops = { 1068 .link_validate = v4l2_subdev_link_validate, 1069 }; 1070 1071 static const struct v4l2_subdev_internal_ops vd56g3_internal_ops = { 1072 .init_state = vd56g3_init_state, 1073 }; 1074 1075 /* ----------------------------------------------------------------------------- 1076 * Power management 1077 */ 1078 1079 static 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 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(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(sensor->reset_gpio, 0); 1100 usleep_range(3500, 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(sensor->regmap, VD56G3_REG_BOOT, VD56G3_CMD_BOOT, &ret); 1109 vd56g3_poll_reg(sensor, VD56G3_REG_BOOT, VD56G3_CMD_ACK, &ret); 1110 vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret); 1111 if (ret) { 1112 dev_err(dev, "Sensor boot failed: %d\n", ret); 1113 goto disable_clock; 1114 } 1115 1116 return 0; 1117 1118 disable_clock: 1119 gpiod_set_value_cansleep(sensor->reset_gpio, 1); 1120 clk_disable_unprepare(sensor->xclk); 1121 disable_reg: 1122 regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies); 1123 1124 return ret; 1125 } 1126 1127 static 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(sensor->reset_gpio, 1); 1133 clk_disable_unprepare(sensor->xclk); 1134 regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies); 1135 1136 return 0; 1137 } 1138 1139 static 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 1147 static 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(endpoint, &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 1212 done: 1213 v4l2_fwnode_endpoint_free(&ep); 1214 1215 return ret; 1216 } 1217 1218 static 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, prop_name)) { 1226 *nb = 0; 1227 return 0; 1228 } 1229 1230 ret = device_property_count_u32(dev, 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, prop_name, array, *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 1253 static 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, "st,leds", led_gpios, 1267 &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(led_gpios[i], &sensor->ext_leds_mask); 1273 } 1274 1275 return 0; 1276 } 1277 1278 static 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), 0, 1284 0, 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(endpoint); 1292 if (ret) 1293 return ret; 1294 1295 return vd56g3_parse_dt_gpios(sensor); 1296 } 1297 1298 static 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(sensor->dev, 1306 ARRAY_SIZE(sensor->supplies), 1307 sensor->supplies); 1308 } 1309 1310 static 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 1344 static 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(sensor->regmap, VD56G3_REG_MODEL_ID, &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(sensor->regmap, VD56G3_REG_REVISION, &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(sensor->regmap, VD56G3_REG_OPTICAL_REVISION, 1375 &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 1392 static 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(&sensor->sd.entity, 1, &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(&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 1438 err_ctrls: 1439 v4l2_ctrl_handler_free(sensor->sd.ctrl_handler); 1440 1441 err_media: 1442 media_entity_cleanup(&sensor->sd.entity); 1443 1444 return ret; 1445 } 1446 1447 static void vd56g3_subdev_cleanup(struct vd56g3 *sensor) 1448 { 1449 v4l2_async_unregister_subdev(&sensor->sd); 1450 v4l2_subdev_cleanup(&sensor->sd); 1451 media_entity_cleanup(&sensor->sd.entity); 1452 v4l2_ctrl_handler_free(sensor->sd.ctrl_handler); 1453 } 1454 1455 static 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, sizeof(*sensor), GFP_KERNEL); 1462 if (!sensor) 1463 return -ENOMEM; 1464 1465 v4l2_i2c_subdev_init(&sensor->sd, client, &vd56g3_subdev_ops); 1466 sensor->dev = dev; 1467 1468 ret = vd56g3_parse_dt(sensor); 1469 if (ret) 1470 return dev_err_probe(dev, ret, "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, ret, "Failed to get regulators\n"); 1476 1477 sensor->xclk = devm_clk_get(dev, NULL); 1478 if (IS_ERR(sensor->xclk)) 1479 return dev_err_probe(dev, PTR_ERR(sensor->xclk), 1480 "Failed to get xclk\n"); 1481 sensor->xclk_freq = clk_get_rate(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, "reset", 1487 GPIOD_OUT_HIGH); 1488 if (IS_ERR(sensor->reset_gpio)) 1489 return dev_err_probe(dev, PTR_ERR(sensor->reset_gpio), 1490 "Failed to get reset gpio\n"); 1491 1492 sensor->regmap = devm_cci_regmap_init_i2c(client, 16); 1493 if (IS_ERR(sensor->regmap)) 1494 return dev_err_probe(dev, PTR_ERR(sensor->regmap), 1495 "Failed to init regmap\n"); 1496 1497 /* Power ON */ 1498 ret = vd56g3_power_on(dev); 1499 if (ret) 1500 return dev_err_probe(dev, ret, "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, 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 1538 err_subdev: 1539 vd56g3_subdev_cleanup(sensor); 1540 err_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 1549 static 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(sensor->dev); 1557 if (!pm_runtime_status_suspended(sensor->dev)) 1558 vd56g3_power_off(sensor->dev); 1559 pm_runtime_set_suspended(sensor->dev); 1560 pm_runtime_dont_use_autosuspend(sensor->dev); 1561 } 1562 1563 static 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 }; 1568 MODULE_DEVICE_TABLE(of, vd56g3_dt_ids); 1569 1570 static 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 1580 module_i2c_driver(vd56g3_i2c_driver); 1581 1582 MODULE_AUTHOR("Benjamin Mugnier <benjamin.mugnier@foss.st.com>"); 1583 MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>"); 1584 MODULE_AUTHOR("Sylvain Petinot <sylvain.petinot@foss.st.com>"); 1585 MODULE_DESCRIPTION("ST VD56G3 sensor driver"); 1586 MODULE_LICENSE("GPL"); 1587