drivers/media/i2c/st-vgxy61.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Driver for VGXY61 global shutter sensor family driver
   4  *
   5  * Copyright (C) 2022 STMicroelectronics SA
   6  */
   7
   8 #include <linux/clk.h>
   9 #include <linux/delay.h>
  10 #include <linux/gpio/consumer.h>
  11 #include <linux/i2c.h>
  12 #include <linux/iopoll.h>
  13 #include <linux/module.h>
  14 #include <linux/pm_runtime.h>
  15 #include <linux/regulator/consumer.h>
  16 #include <linux/units.h>
  17
  18 #include <asm/unaligned.h>
  19
  20 #include <media/mipi-csi2.h>
  21 #include <media/v4l2-async.h>
  22 #include <media/v4l2-ctrls.h>
  23 #include <media/v4l2-device.h>
  24 #include <media/v4l2-fwnode.h>
  25 #include <media/v4l2-subdev.h>
  26
  27 #define VGXY61_REG_8BIT(n)                              ((1 << 16) | (n))
  28 #define VGXY61_REG_16BIT(n)                             ((2 << 16) | (n))
  29 #define VGXY61_REG_32BIT(n)                             ((4 << 16) | (n))
  30 #define VGXY61_REG_SIZE_SHIFT                           16
  31 #define VGXY61_REG_ADDR_MASK                            0xffff
  32
  33 #define VGXY61_REG_MODEL_ID                             VGXY61_REG_16BIT(0x0000)
  34 #define VG5661_MODEL_ID                                 0x5661
  35 #define VG5761_MODEL_ID                                 0x5761
  36 #define VGXY61_REG_REVISION                             VGXY61_REG_16BIT(0x0002)
  37 #define VGXY61_REG_FWPATCH_REVISION                     VGXY61_REG_16BIT(0x0014)
  38 #define VGXY61_REG_FWPATCH_START_ADDR                   VGXY61_REG_8BIT(0x2000)
  39 #define VGXY61_REG_SYSTEM_FSM                           VGXY61_REG_8BIT(0x0020)
  40 #define VGXY61_SYSTEM_FSM_SW_STBY                       0x03
  41 #define VGXY61_SYSTEM_FSM_STREAMING                     0x04
  42 #define VGXY61_REG_NVM                                  VGXY61_REG_8BIT(0x0023)
  43 #define VGXY61_NVM_OK                                   0x04
  44 #define VGXY61_REG_STBY                                 VGXY61_REG_8BIT(0x0201)
  45 #define VGXY61_STBY_NO_REQ                              0
  46 #define VGXY61_STBY_REQ_TMP_READ                        BIT(2)
  47 #define VGXY61_REG_STREAMING                            VGXY61_REG_8BIT(0x0202)
  48 #define VGXY61_STREAMING_NO_REQ                         0
  49 #define VGXY61_STREAMING_REQ_STOP                       BIT(0)
  50 #define VGXY61_STREAMING_REQ_START                      BIT(1)
  51 #define VGXY61_REG_EXT_CLOCK                            VGXY61_REG_32BIT(0x0220)
  52 #define VGXY61_REG_CLK_PLL_PREDIV                       VGXY61_REG_8BIT(0x0224)
  53 #define VGXY61_REG_CLK_SYS_PLL_MULT                     VGXY61_REG_8BIT(0x0225)
  54 #define VGXY61_REG_GPIO_0_CTRL                          VGXY61_REG_8BIT(0x0236)
  55 #define VGXY61_REG_GPIO_1_CTRL                          VGXY61_REG_8BIT(0x0237)
  56 #define VGXY61_REG_GPIO_2_CTRL                          VGXY61_REG_8BIT(0x0238)
  57 #define VGXY61_REG_GPIO_3_CTRL                          VGXY61_REG_8BIT(0x0239)
  58 #define VGXY61_REG_SIGNALS_POLARITY_CTRL                VGXY61_REG_8BIT(0x023b)
  59 #define VGXY61_REG_LINE_LENGTH                          VGXY61_REG_16BIT(0x0300)
  60 #define VGXY61_REG_ORIENTATION                          VGXY61_REG_8BIT(0x0302)
  61 #define VGXY61_REG_VT_CTRL                              VGXY61_REG_8BIT(0x0304)
  62 #define VGXY61_REG_FORMAT_CTRL                          VGXY61_REG_8BIT(0x0305)
  63 #define VGXY61_REG_OIF_CTRL                             VGXY61_REG_16BIT(0x0306)
  64 #define VGXY61_REG_OIF_ROI0_CTRL                        VGXY61_REG_8BIT(0x030a)
  65 #define VGXY61_REG_ROI0_START_H                         VGXY61_REG_16BIT(0x0400)
  66 #define VGXY61_REG_ROI0_START_V                         VGXY61_REG_16BIT(0x0402)
  67 #define VGXY61_REG_ROI0_END_H                           VGXY61_REG_16BIT(0x0404)
  68 #define VGXY61_REG_ROI0_END_V                           VGXY61_REG_16BIT(0x0406)
  69 #define VGXY61_REG_PATGEN_CTRL                          VGXY61_REG_32BIT(0x0440)
  70 #define VGXY61_PATGEN_LONG_ENABLE                       BIT(16)
  71 #define VGXY61_PATGEN_SHORT_ENABLE                      BIT(0)
  72 #define VGXY61_PATGEN_LONG_TYPE_SHIFT                   18
  73 #define VGXY61_PATGEN_SHORT_TYPE_SHIFT                  4
  74 #define VGXY61_REG_FRAME_CONTENT_CTRL                   VGXY61_REG_8BIT(0x0478)
  75 #define VGXY61_REG_COARSE_EXPOSURE_LONG                 VGXY61_REG_16BIT(0x0500)
  76 #define VGXY61_REG_COARSE_EXPOSURE_SHORT                VGXY61_REG_16BIT(0x0504)
  77 #define VGXY61_REG_ANALOG_GAIN                          VGXY61_REG_8BIT(0x0508)
  78 #define VGXY61_REG_DIGITAL_GAIN_LONG                    VGXY61_REG_16BIT(0x050a)
  79 #define VGXY61_REG_DIGITAL_GAIN_SHORT                   VGXY61_REG_16BIT(0x0512)
  80 #define VGXY61_REG_FRAME_LENGTH                         VGXY61_REG_16BIT(0x051a)
  81 #define VGXY61_REG_SIGNALS_CTRL                         VGXY61_REG_16BIT(0x0522)
  82 #define VGXY61_SIGNALS_GPIO_ID_SHIFT                    4
  83 #define VGXY61_REG_READOUT_CTRL                         VGXY61_REG_8BIT(0x0530)
  84 #define VGXY61_REG_HDR_CTRL                             VGXY61_REG_8BIT(0x0532)
  85 #define VGXY61_REG_PATGEN_LONG_DATA_GR                  VGXY61_REG_16BIT(0x092c)
  86 #define VGXY61_REG_PATGEN_LONG_DATA_R                   VGXY61_REG_16BIT(0x092e)
  87 #define VGXY61_REG_PATGEN_LONG_DATA_B                   VGXY61_REG_16BIT(0x0930)
  88 #define VGXY61_REG_PATGEN_LONG_DATA_GB                  VGXY61_REG_16BIT(0x0932)
  89 #define VGXY61_REG_PATGEN_SHORT_DATA_GR                 VGXY61_REG_16BIT(0x0950)
  90 #define VGXY61_REG_PATGEN_SHORT_DATA_R                  VGXY61_REG_16BIT(0x0952)
  91 #define VGXY61_REG_PATGEN_SHORT_DATA_B                  VGXY61_REG_16BIT(0x0954)
  92 #define VGXY61_REG_PATGEN_SHORT_DATA_GB                 VGXY61_REG_16BIT(0x0956)
  93 #define VGXY61_REG_BYPASS_CTRL                          VGXY61_REG_8BIT(0x0a60)
  94
  95 #define VGX661_WIDTH                                    1464
  96 #define VGX661_HEIGHT                                   1104
  97 #define VGX761_WIDTH                                    1944
  98 #define VGX761_HEIGHT                                   1204
  99 #define VGX661_DEFAULT_MODE                             1
 100 #define VGX761_DEFAULT_MODE                             1
 101 #define VGX661_SHORT_ROT_TERM                           93
 102 #define VGX761_SHORT_ROT_TERM                           90
 103 #define VGXY61_EXPOS_ROT_TERM                           66
 104 #define VGXY61_WRITE_MULTIPLE_CHUNK_MAX                 16
 105 #define VGXY61_NB_GPIOS                                 4
 106 #define VGXY61_NB_POLARITIES                            5
 107 #define VGXY61_FRAME_LENGTH_DEF                         1313
 108 #define VGXY61_MIN_FRAME_LENGTH                         1288
 109 #define VGXY61_MIN_EXPOSURE                             10
 110 #define VGXY61_HDR_LINEAR_RATIO                         10
 111 #define VGXY61_TIMEOUT_MS                               500
 112 #define VGXY61_MEDIA_BUS_FMT_DEF                        MEDIA_BUS_FMT_Y8_1X8
 113
 114 #define VGXY61_FWPATCH_REVISION_MAJOR                   2
 115 #define VGXY61_FWPATCH_REVISION_MINOR                   0
 116 #define VGXY61_FWPATCH_REVISION_MICRO                   5
 117
 118 /*(DEBLOBBED)*/
 119
 120 static const char * const vgxy61_test_pattern_menu[] = {
 121         "Disabled",
 122         "Solid",
 123         "Colorbar",
 124         "Gradbar",
 125         "Hgrey",
 126         "Vgrey",
 127         "Dgrey",
 128         "PN28",
 129 };
 130
 131 static const char * const vgxy61_hdr_mode_menu[] = {
 132         "HDR linearize",
 133         "HDR substraction",
 134         "No HDR",
 135 };
 136
 137 static const char * const vgxy61_supply_name[] = {
 138         "VCORE",
 139         "VDDIO",
 140         "VANA",
 141 };
 142
 143 static const s64 link_freq[] = {
 144         /*
 145          * MIPI output freq is 804Mhz / 2, as it uses both rising edge and
 146          * falling edges to send data
 147          */
 148         402000000ULL
 149 };
 150
 151 enum vgxy61_bin_mode {
 152         VGXY61_BIN_MODE_NORMAL,
 153         VGXY61_BIN_MODE_DIGITAL_X2,
 154         VGXY61_BIN_MODE_DIGITAL_X4,
 155 };
 156
 157 enum vgxy61_hdr_mode {
 158         VGXY61_HDR_LINEAR,
 159         VGXY61_HDR_SUB,
 160         VGXY61_NO_HDR,
 161 };
 162
 163 enum vgxy61_strobe_mode {
 164         VGXY61_STROBE_DISABLED,
 165         VGXY61_STROBE_LONG,
 166         VGXY61_STROBE_ENABLED,
 167 };
 168
 169 struct vgxy61_mode_info {
 170         u32 width;
 171         u32 height;
 172         enum vgxy61_bin_mode bin_mode;
 173         struct v4l2_rect crop;
 174 };
 175
 176 struct vgxy61_fmt_desc {
 177         u32 code;
 178         u8 bpp;
 179         u8 data_type;
 180 };
 181
 182 static const struct vgxy61_fmt_desc vgxy61_supported_codes[] = {
 183         {
 184                 .code = MEDIA_BUS_FMT_Y8_1X8,
 185                 .bpp = 8,
 186                 .data_type = MIPI_CSI2_DT_RAW8,
 187         },
 188         {
 189                 .code = MEDIA_BUS_FMT_Y10_1X10,
 190                 .bpp = 10,
 191                 .data_type = MIPI_CSI2_DT_RAW10,
 192         },
 193         {
 194                 .code = MEDIA_BUS_FMT_Y12_1X12,
 195                 .bpp = 12,
 196                 .data_type = MIPI_CSI2_DT_RAW12,
 197         },
 198         {
 199                 .code = MEDIA_BUS_FMT_Y14_1X14,
 200                 .bpp = 14,
 201                 .data_type = MIPI_CSI2_DT_RAW14,
 202         },
 203         {
 204                 .code = MEDIA_BUS_FMT_Y16_1X16,
 205                 .bpp = 16,
 206                 .data_type = MIPI_CSI2_DT_RAW16,
 207         },
 208 };
 209
 210 static const struct vgxy61_mode_info vgx661_mode_data[] = {
 211         {
 212                 .width = VGX661_WIDTH,
 213                 .height = VGX661_HEIGHT,
 214                 .bin_mode = VGXY61_BIN_MODE_NORMAL,
 215                 .crop = {
 216                         .left = 0,
 217                         .top = 0,
 218                         .width = VGX661_WIDTH,
 219                         .height = VGX661_HEIGHT,
 220                 },
 221         },
 222         {
 223                 .width = 1280,
 224                 .height = 720,
 225                 .bin_mode = VGXY61_BIN_MODE_NORMAL,
 226                 .crop = {
 227                         .left = 92,
 228                         .top = 192,
 229                         .width = 1280,
 230                         .height = 720,
 231                 },
 232         },
 233         {
 234                 .width = 640,
 235                 .height = 480,
 236                 .bin_mode = VGXY61_BIN_MODE_DIGITAL_X2,
 237                 .crop = {
 238                         .left = 92,
 239                         .top = 72,
 240                         .width = 1280,
 241                         .height = 960,
 242                 },
 243         },
 244         {
 245                 .width = 320,
 246                 .height = 240,
 247                 .bin_mode = VGXY61_BIN_MODE_DIGITAL_X4,
 248                 .crop = {
 249                         .left = 92,
 250                         .top = 72,
 251                         .width = 1280,
 252                         .height = 960,
 253                 },
 254         },
 255 };
 256
 257 static const struct vgxy61_mode_info vgx761_mode_data[] = {
 258         {
 259                 .width = VGX761_WIDTH,
 260                 .height = VGX761_HEIGHT,
 261                 .bin_mode = VGXY61_BIN_MODE_NORMAL,
 262                 .crop = {
 263                         .left = 0,
 264                         .top = 0,
 265                         .width = VGX761_WIDTH,
 266                         .height = VGX761_HEIGHT,
 267                 },
 268         },
 269         {
 270                 .width = 1920,
 271                 .height = 1080,
 272                 .bin_mode = VGXY61_BIN_MODE_NORMAL,
 273                 .crop = {
 274                         .left = 12,
 275                         .top = 62,
 276                         .width = 1920,
 277                         .height = 1080,
 278                 },
 279         },
 280         {
 281                 .width = 1280,
 282                 .height = 720,
 283                 .bin_mode = VGXY61_BIN_MODE_NORMAL,
 284                 .crop = {
 285                         .left = 332,
 286                         .top = 242,
 287                         .width = 1280,
 288                         .height = 720,
 289                 },
 290         },
 291         {
 292                 .width = 640,
 293                 .height = 480,
 294                 .bin_mode = VGXY61_BIN_MODE_DIGITAL_X2,
 295                 .crop = {
 296                         .left = 332,
 297                         .top = 122,
 298                         .width = 1280,
 299                         .height = 960,
 300                 },
 301         },
 302         {
 303                 .width = 320,
 304                 .height = 240,
 305                 .bin_mode = VGXY61_BIN_MODE_DIGITAL_X4,
 306                 .crop = {
 307                         .left = 332,
 308                         .top = 122,
 309                         .width = 1280,
 310                         .height = 960,
 311                 },
 312         },
 313 };
 314
 315 struct vgxy61_dev {
 316         struct i2c_client *i2c_client;
 317         struct v4l2_subdev sd;
 318         struct media_pad pad;
 319         struct regulator_bulk_data supplies[ARRAY_SIZE(vgxy61_supply_name)];
 320         struct gpio_desc *reset_gpio;
 321         struct clk *xclk;
 322         u32 clk_freq;
 323         u16 id;
 324         u16 sensor_width;
 325         u16 sensor_height;
 326         u16 oif_ctrl;
 327         unsigned int nb_of_lane;
 328         u32 data_rate_in_mbps;
 329         u32 pclk;
 330         u16 line_length;
 331         u16 rot_term;
 332         bool gpios_polarity;
 333         /* Lock to protect all members below */
 334         struct mutex lock;
 335         struct v4l2_ctrl_handler ctrl_handler;
 336         struct v4l2_ctrl *pixel_rate_ctrl;
 337         struct v4l2_ctrl *expo_ctrl;
 338         struct v4l2_ctrl *vblank_ctrl;
 339         struct v4l2_ctrl *vflip_ctrl;
 340         struct v4l2_ctrl *hflip_ctrl;
 341         bool streaming;
 342         struct v4l2_mbus_framefmt fmt;
 343         const struct vgxy61_mode_info *sensor_modes;
 344         unsigned int sensor_modes_nb;
 345         const struct vgxy61_mode_info *default_mode;
 346         const struct vgxy61_mode_info *current_mode;
 347         bool hflip;
 348         bool vflip;
 349         enum vgxy61_hdr_mode hdr;
 350         u16 expo_long;
 351         u16 expo_short;
 352         u16 expo_max;
 353         u16 expo_min;
 354         u16 vblank;
 355         u16 vblank_min;
 356         u16 frame_length;
 357         u16 digital_gain;
 358         u8 analog_gain;
 359         enum vgxy61_strobe_mode strobe_mode;
 360         u32 pattern;
 361 };
 362
 363 static u8 get_bpp_by_code(__u32 code)
 364 {
 365         unsigned int i;
 366
 367         for (i = 0; i < ARRAY_SIZE(vgxy61_supported_codes); i++) {
 368                 if (vgxy61_supported_codes[i].code == code)
 369                         return vgxy61_supported_codes[i].bpp;
 370         }
 371         /* Should never happen */
 372         WARN(1, "Unsupported code %d. default to 8 bpp", code);
 373         return 8;
 374 }
 375
 376 static u8 get_data_type_by_code(__u32 code)
 377 {
 378         unsigned int i;
 379
 380         for (i = 0; i < ARRAY_SIZE(vgxy61_supported_codes); i++) {
 381                 if (vgxy61_supported_codes[i].code == code)
 382                         return vgxy61_supported_codes[i].data_type;
 383         }
 384         /* Should never happen */
 385         WARN(1, "Unsupported code %d. default to MIPI_CSI2_DT_RAW8 data type",
 386              code);
 387         return MIPI_CSI2_DT_RAW8;
 388 }
 389
 390 static void compute_pll_parameters_by_freq(u32 freq, u8 *prediv, u8 *mult)
 391 {
 392         const unsigned int predivs[] = {1, 2, 4};
 393         unsigned int i;
 394
 395         /*
 396          * Freq range is [6Mhz-27Mhz] already checked.
 397          * Output of divider should be in [6Mhz-12Mhz[.
 398          */
 399         for (i = 0; i < ARRAY_SIZE(predivs); i++) {
 400                 *prediv = predivs[i];
 401                 if (freq / *prediv < 12 * HZ_PER_MHZ)
 402                         break;
 403         }
 404         WARN_ON(i == ARRAY_SIZE(predivs));
 405
 406         /*
 407          * Target freq is 804Mhz. Don't change this as it will impact image
 408          * quality.
 409          */
 410         *mult = ((804 * HZ_PER_MHZ) * (*prediv) + freq / 2) / freq;
 411 }
 412
 413 static s32 get_pixel_rate(struct vgxy61_dev *sensor)
 414 {
 415         return div64_u64((u64)sensor->data_rate_in_mbps * sensor->nb_of_lane,
 416                          get_bpp_by_code(sensor->fmt.code));
 417 }
 418
 419 static inline struct vgxy61_dev *to_vgxy61_dev(struct v4l2_subdev *sd)
 420 {
 421         return container_of(sd, struct vgxy61_dev, sd);
 422 }
 423
 424 static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
 425 {
 426         return &container_of(ctrl->handler, struct vgxy61_dev,
 427                              ctrl_handler)->sd;
 428 }
 429
 430 static unsigned int get_chunk_size(struct vgxy61_dev *sensor)
 431 {
 432         struct i2c_adapter *adapter = sensor->i2c_client->adapter;
 433         int max_write_len = VGXY61_WRITE_MULTIPLE_CHUNK_MAX;
 434
 435         if (adapter->quirks && adapter->quirks->max_write_len)
 436                 max_write_len = adapter->quirks->max_write_len - 2;
 437
 438         max_write_len = min(max_write_len, VGXY61_WRITE_MULTIPLE_CHUNK_MAX);
 439
 440         return max(max_write_len, 1);
 441 }
 442
 443 static int vgxy61_read_multiple(struct vgxy61_dev *sensor, u32 reg,
 444                                 unsigned int len)
 445 {
 446         struct i2c_client *client = sensor->i2c_client;
 447         struct i2c_msg msg[2];
 448         u8 buf[2];
 449         u8 val[sizeof(u32)] = {0};
 450         int ret;
 451
 452         if (len > sizeof(u32))
 453                 return -EINVAL;
 454         buf[0] = reg >> 8;
 455         buf[1] = reg & 0xff;
 456
 457         msg[0].addr = client->addr;
 458         msg[0].flags = client->flags;
 459         msg[0].buf = buf;
 460         msg[0].len = sizeof(buf);
 461
 462         msg[1].addr = client->addr;
 463         msg[1].flags = client->flags | I2C_M_RD;
 464         msg[1].buf = val;
 465         msg[1].len = len;
 466
 467         ret = i2c_transfer(client->adapter, msg, 2);
 468         if (ret < 0) {
 469                 dev_dbg(&client->dev, "%s: %x i2c_transfer, reg: %x => %d\n",
 470                         __func__, client->addr, reg, ret);
 471                 return ret;
 472         }
 473
 474         return get_unaligned_le32(val);
 475 }
 476
 477 static inline int vgxy61_read_reg(struct vgxy61_dev *sensor, u32 reg)
 478 {
 479         return vgxy61_read_multiple(sensor, reg & VGXY61_REG_ADDR_MASK,
 480                                      (reg >> VGXY61_REG_SIZE_SHIFT) & 7);
 481 }
 482
 483 static int vgxy61_write_multiple(struct vgxy61_dev *sensor, u32 reg,
 484                                  const u8 *data, unsigned int len, int *err)
 485 {
 486         struct i2c_client *client = sensor->i2c_client;
 487         struct i2c_msg msg;
 488         u8 buf[VGXY61_WRITE_MULTIPLE_CHUNK_MAX + 2];
 489         unsigned int i;
 490         int ret;
 491
 492         if (err && *err)
 493                 return *err;
 494
 495         if (len > VGXY61_WRITE_MULTIPLE_CHUNK_MAX)
 496                 return -EINVAL;
 497         buf[0] = reg >> 8;
 498         buf[1] = reg & 0xff;
 499         for (i = 0; i < len; i++)
 500                 buf[i + 2] = data[i];
 501
 502         msg.addr = client->addr;
 503         msg.flags = client->flags;
 504         msg.buf = buf;
 505         msg.len = len + 2;
 506
 507         ret = i2c_transfer(client->adapter, &msg, 1);
 508         if (ret < 0) {
 509                 dev_dbg(&client->dev, "%s: i2c_transfer, reg: %x => %d\n",
 510                         __func__, reg, ret);
 511                 if (err)
 512                         *err = ret;
 513                 return ret;
 514         }
 515
 516         return 0;
 517 }
 518
 519 static int vgxy61_write_array(struct vgxy61_dev *sensor, u32 reg,
 520                               unsigned int nb, const u8 *array)
 521 {
 522         const unsigned int chunk_size = get_chunk_size(sensor);
 523         int ret;
 524         unsigned int sz;
 525
 526         while (nb) {
 527                 sz = min(nb, chunk_size);
 528                 ret = vgxy61_write_multiple(sensor, reg, array, sz, NULL);
 529                 if (ret < 0)
 530                         return ret;
 531                 nb -= sz;
 532                 reg += sz;
 533                 array += sz;
 534         }
 535
 536         return 0;
 537 }
 538
 539 static inline int vgxy61_write_reg(struct vgxy61_dev *sensor, u32 reg, u32 val,
 540                                    int *err)
 541 {
 542         return vgxy61_write_multiple(sensor, reg & VGXY61_REG_ADDR_MASK,
 543                                      (u8 *)&val,
 544                                      (reg >> VGXY61_REG_SIZE_SHIFT) & 7, err);
 545 }
 546
 547 static int vgxy61_poll_reg(struct vgxy61_dev *sensor, u32 reg, u8 poll_val,
 548                            unsigned int timeout_ms)
 549 {
 550         const unsigned int loop_delay_ms = 10;
 551         int ret;
 552
 553         return read_poll_timeout(vgxy61_read_reg, ret,
 554                                  ((ret < 0) || (ret == poll_val)),
 555                                  loop_delay_ms * 1000, timeout_ms * 1000,
 556                                  false, sensor, reg);
 557 }
 558
 559 static int vgxy61_wait_state(struct vgxy61_dev *sensor, int state,
 560                              unsigned int timeout_ms)
 561 {
 562         return vgxy61_poll_reg(sensor, VGXY61_REG_SYSTEM_FSM, state,
 563                                timeout_ms);
 564 }
 565
 566 static int vgxy61_check_bw(struct vgxy61_dev *sensor)
 567 {
 568         /*
 569          * Simplification of time needed to send short packets and for the MIPI
 570          * to add transition times (EoT, LPS, and SoT packet delimiters) needed
 571          * by the protocol to go in low power between 2 packets of data. This
 572          * is a mipi IP constant for the sensor.
 573          */
 574         const unsigned int mipi_margin = 1056;
 575         unsigned int binning_scale = sensor->current_mode->crop.height /
 576                                      sensor->current_mode->height;
 577         u8 bpp = get_bpp_by_code(sensor->fmt.code);
 578         unsigned int max_bit_per_line;
 579         unsigned int bit_per_line;
 580         u64 line_rate;
 581
 582         line_rate = sensor->nb_of_lane * (u64)sensor->data_rate_in_mbps *
 583                     sensor->line_length;
 584         max_bit_per_line = div64_u64(line_rate, sensor->pclk) - mipi_margin;
 585         bit_per_line = (bpp * sensor->current_mode->width) / binning_scale;
 586
 587         return bit_per_line > max_bit_per_line ? -EINVAL : 0;
 588 }
 589
 590 static int vgxy61_apply_exposure(struct vgxy61_dev *sensor)
 591 {
 592         int ret = 0;
 593
 594          /* We first set expo to zero to avoid forbidden parameters couple */
 595         vgxy61_write_reg(sensor, VGXY61_REG_COARSE_EXPOSURE_SHORT, 0, &ret);
 596         vgxy61_write_reg(sensor, VGXY61_REG_COARSE_EXPOSURE_LONG,
 597                          sensor->expo_long, &ret);
 598         vgxy61_write_reg(sensor, VGXY61_REG_COARSE_EXPOSURE_SHORT,
 599                          sensor->expo_short, &ret);
 600
 601         return ret;
 602 }
 603
 604 static int vgxy61_get_regulators(struct vgxy61_dev *sensor)
 605 {
 606         unsigned int i;
 607
 608         for (i = 0; i < ARRAY_SIZE(vgxy61_supply_name); i++)
 609                 sensor->supplies[i].supply = vgxy61_supply_name[i];
 610
 611         return devm_regulator_bulk_get(&sensor->i2c_client->dev,
 612                                        ARRAY_SIZE(vgxy61_supply_name),
 613                                        sensor->supplies);
 614 }
 615
 616 static int vgxy61_apply_reset(struct vgxy61_dev *sensor)
 617 {
 618         gpiod_set_value_cansleep(sensor->reset_gpio, 0);
 619         usleep_range(5000, 10000);
 620         gpiod_set_value_cansleep(sensor->reset_gpio, 1);
 621         usleep_range(5000, 10000);
 622         gpiod_set_value_cansleep(sensor->reset_gpio, 0);
 623         usleep_range(40000, 100000);
 624         return vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_SW_STBY,
 625                                  VGXY61_TIMEOUT_MS);
 626 }
 627
 628 static void vgxy61_fill_framefmt(struct vgxy61_dev *sensor,
 629                                  const struct vgxy61_mode_info *mode,
 630                                  struct v4l2_mbus_framefmt *fmt, u32 code)
 631 {
 632         fmt->code = code;
 633         fmt->width = mode->width;
 634         fmt->height = mode->height;
 635         fmt->colorspace = V4L2_COLORSPACE_RAW;
 636         fmt->field = V4L2_FIELD_NONE;
 637         fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
 638         fmt->quantization = V4L2_QUANTIZATION_DEFAULT;
 639         fmt->xfer_func = V4L2_XFER_FUNC_DEFAULT;
 640 }
 641
 642 static int vgxy61_try_fmt_internal(struct v4l2_subdev *sd,
 643                                    struct v4l2_mbus_framefmt *fmt,
 644                                    const struct vgxy61_mode_info **new_mode)
 645 {
 646         struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
 647         const struct vgxy61_mode_info *mode = sensor->sensor_modes;
 648         unsigned int index;
 649
 650         for (index = 0; index < ARRAY_SIZE(vgxy61_supported_codes); index++) {
 651                 if (vgxy61_supported_codes[index].code == fmt->code)
 652                         break;
 653         }
 654         if (index == ARRAY_SIZE(vgxy61_supported_codes))
 655                 index = 0;
 656
 657         mode = v4l2_find_nearest_size(sensor->sensor_modes,
 658                                       sensor->sensor_modes_nb, width, height,
 659                                       fmt->width, fmt->height);
 660         if (new_mode)
 661                 *new_mode = mode;
 662
 663         vgxy61_fill_framefmt(sensor, mode, fmt,
 664                              vgxy61_supported_codes[index].code);
 665
 666         return 0;
 667 }
 668
 669 static int vgxy61_get_selection(struct v4l2_subdev *sd,
 670                                 struct v4l2_subdev_state *sd_state,
 671                                 struct v4l2_subdev_selection *sel)
 672 {
 673         struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
 674
 675         switch (sel->target) {
 676         case V4L2_SEL_TGT_CROP:
 677                 sel->r = sensor->current_mode->crop;
 678                 return 0;
 679         case V4L2_SEL_TGT_NATIVE_SIZE:
 680         case V4L2_SEL_TGT_CROP_DEFAULT:
 681         case V4L2_SEL_TGT_CROP_BOUNDS:
 682                 sel->r.top = 0;
 683                 sel->r.left = 0;
 684                 sel->r.width = sensor->sensor_width;
 685                 sel->r.height = sensor->sensor_height;
 686                 return 0;
 687         }
 688
 689         return -EINVAL;
 690 }
 691
 692 static int vgxy61_enum_mbus_code(struct v4l2_subdev *sd,
 693                                  struct v4l2_subdev_state *sd_state,
 694                                  struct v4l2_subdev_mbus_code_enum *code)
 695 {
 696         if (code->index >= ARRAY_SIZE(vgxy61_supported_codes))
 697                 return -EINVAL;
 698
 699         code->code = vgxy61_supported_codes[code->index].code;
 700
 701         return 0;
 702 }
 703
 704 static int vgxy61_get_fmt(struct v4l2_subdev *sd,
 705                           struct v4l2_subdev_state *sd_state,
 706                           struct v4l2_subdev_format *format)
 707 {
 708         struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
 709         struct v4l2_mbus_framefmt *fmt;
 710
 711         mutex_lock(&sensor->lock);
 712
 713         if (format->which == V4L2_SUBDEV_FORMAT_TRY)
 714                 fmt = v4l2_subdev_get_try_format(&sensor->sd, sd_state,
 715                                                  format->pad);
 716         else
 717                 fmt = &sensor->fmt;
 718
 719         format->format = *fmt;
 720
 721         mutex_unlock(&sensor->lock);
 722
 723         return 0;
 724 }
 725
 726 static u16 vgxy61_get_vblank_min(struct vgxy61_dev *sensor,
 727                                  enum vgxy61_hdr_mode hdr)
 728 {
 729         u16 min_vblank =  VGXY61_MIN_FRAME_LENGTH -
 730                           sensor->current_mode->crop.height;
 731         /* Ensure the first rule of thumb can't be negative */
 732         u16 min_vblank_hdr =  VGXY61_MIN_EXPOSURE + sensor->rot_term + 1;
 733
 734         if (hdr != VGXY61_NO_HDR)
 735                 return max(min_vblank, min_vblank_hdr);
 736         return min_vblank;
 737 }
 738
 739 static int vgxy61_enum_frame_size(struct v4l2_subdev *sd,
 740                                   struct v4l2_subdev_state *sd_state,
 741                                   struct v4l2_subdev_frame_size_enum *fse)
 742 {
 743         struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
 744
 745         if (fse->index >= sensor->sensor_modes_nb)
 746                 return -EINVAL;
 747
 748         fse->min_width = sensor->sensor_modes[fse->index].width;
 749         fse->max_width = fse->min_width;
 750         fse->min_height = sensor->sensor_modes[fse->index].height;
 751         fse->max_height = fse->min_height;
 752
 753         return 0;
 754 }
 755
 756 static int vgxy61_update_analog_gain(struct vgxy61_dev *sensor, u32 target)
 757 {
 758         sensor->analog_gain = target;
 759
 760         if (sensor->streaming)
 761                 return vgxy61_write_reg(sensor, VGXY61_REG_ANALOG_GAIN, target,
 762                                         NULL);
 763         return 0;
 764 }
 765
 766 static int vgxy61_apply_digital_gain(struct vgxy61_dev *sensor,
 767                                      u32 digital_gain)
 768 {
 769         int ret = 0;
 770
 771         /*
 772          * For a monochrome version, configuring DIGITAL_GAIN_LONG_CH0 and
 773          * DIGITAL_GAIN_SHORT_CH0 is enough to configure the gain of all
 774          * four sub pixels.
 775          */
 776         vgxy61_write_reg(sensor, VGXY61_REG_DIGITAL_GAIN_LONG, digital_gain,
 777                          &ret);
 778         vgxy61_write_reg(sensor, VGXY61_REG_DIGITAL_GAIN_SHORT, digital_gain,
 779                          &ret);
 780
 781         return ret;
 782 }
 783
 784 static int vgxy61_update_digital_gain(struct vgxy61_dev *sensor, u32 target)
 785 {
 786         sensor->digital_gain = target;
 787
 788         if (sensor->streaming)
 789                 return vgxy61_apply_digital_gain(sensor, sensor->digital_gain);
 790         return 0;
 791 }
 792
 793 static int vgxy61_apply_patgen(struct vgxy61_dev *sensor, u32 index)
 794 {
 795         static const u8 index2val[] = {
 796                 0x0, 0x1, 0x2, 0x3, 0x10, 0x11, 0x12, 0x13
 797         };
 798         u32 pattern = index2val[index];
 799         u32 reg = (pattern << VGXY61_PATGEN_LONG_TYPE_SHIFT) |
 800               (pattern << VGXY61_PATGEN_SHORT_TYPE_SHIFT);
 801
 802         if (pattern)
 803                 reg |= VGXY61_PATGEN_LONG_ENABLE | VGXY61_PATGEN_SHORT_ENABLE;
 804         return vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_CTRL, reg, NULL);
 805 }
 806
 807 static int vgxy61_update_patgen(struct vgxy61_dev *sensor, u32 pattern)
 808 {
 809         sensor->pattern = pattern;
 810
 811         if (sensor->streaming)
 812                 return vgxy61_apply_patgen(sensor, sensor->pattern);
 813         return 0;
 814 }
 815
 816 static int vgxy61_apply_gpiox_strobe_mode(struct vgxy61_dev *sensor,
 817                                           enum vgxy61_strobe_mode mode,
 818                                           unsigned int idx)
 819 {
 820         static const u8 index2val[] = {0x0, 0x1, 0x3};
 821         int reg;
 822
 823         reg = vgxy61_read_reg(sensor, VGXY61_REG_SIGNALS_CTRL);
 824         if (reg < 0)
 825                 return reg;
 826         reg &= ~(0xf << (idx * VGXY61_SIGNALS_GPIO_ID_SHIFT));
 827         reg |= index2val[mode] << (idx * VGXY61_SIGNALS_GPIO_ID_SHIFT);
 828
 829         return vgxy61_write_reg(sensor, VGXY61_REG_SIGNALS_CTRL, reg, NULL);
 830 }
 831
 832 static int vgxy61_update_gpios_strobe_mode(struct vgxy61_dev *sensor,
 833                                            enum vgxy61_hdr_mode hdr)
 834 {
 835         unsigned int i;
 836         int ret;
 837
 838         switch (hdr) {
 839         case VGXY61_HDR_LINEAR:
 840                 sensor->strobe_mode = VGXY61_STROBE_ENABLED;
 841                 break;
 842         case VGXY61_HDR_SUB:
 843         case VGXY61_NO_HDR:
 844                 sensor->strobe_mode = VGXY61_STROBE_LONG;
 845                 break;
 846         default:
 847                 /* Should never happen */
 848                 WARN_ON(true);
 849                 break;
 850         }
 851
 852         if (!sensor->streaming)
 853                 return 0;
 854
 855         for (i = 0; i < VGXY61_NB_GPIOS; i++) {
 856                 ret = vgxy61_apply_gpiox_strobe_mode(sensor,
 857                                                      sensor->strobe_mode,
 858                                                      i);
 859                 if (ret)
 860                         return ret;
 861         }
 862
 863         return 0;
 864 }
 865
 866 static int vgxy61_update_gpios_strobe_polarity(struct vgxy61_dev *sensor,
 867                                                bool polarity)
 868 {
 869         int ret = 0;
 870
 871         if (sensor->streaming)
 872                 return -EBUSY;
 873
 874         vgxy61_write_reg(sensor, VGXY61_REG_GPIO_0_CTRL, polarity << 1, &ret);
 875         vgxy61_write_reg(sensor, VGXY61_REG_GPIO_1_CTRL, polarity << 1, &ret);
 876         vgxy61_write_reg(sensor, VGXY61_REG_GPIO_2_CTRL, polarity << 1, &ret);
 877         vgxy61_write_reg(sensor, VGXY61_REG_GPIO_3_CTRL, polarity << 1, &ret);
 878         vgxy61_write_reg(sensor, VGXY61_REG_SIGNALS_POLARITY_CTRL, polarity,
 879                          &ret);
 880
 881         return ret;
 882 }
 883
 884 static u32 vgxy61_get_expo_long_max(struct vgxy61_dev *sensor,
 885                                     unsigned int short_expo_ratio)
 886 {
 887         u32 first_rot_max_expo, second_rot_max_expo, third_rot_max_expo;
 888
 889         /* Apply sensor's rules of thumb */
 890         /*
 891          * Short exposure + height must be less than frame length to avoid bad
 892          * pixel line at the botom of the image
 893          */
 894         first_rot_max_expo =
 895                 ((sensor->frame_length - sensor->current_mode->crop.height -
 896                 sensor->rot_term) * short_expo_ratio) - 1;
 897
 898         /*
 899          * Total exposition time must be less than frame length to avoid sensor
 900          * crash
 901          */
 902         second_rot_max_expo =
 903                 (((sensor->frame_length - VGXY61_EXPOS_ROT_TERM) *
 904                 short_expo_ratio) / (short_expo_ratio + 1)) - 1;
 905
 906         /*
 907          * Short exposure times 71 must be less than frame length to avoid
 908          * sensor crash
 909          */
 910         third_rot_max_expo = (sensor->frame_length / 71) * short_expo_ratio;
 911
 912         /* Take the minimum from all rules */
 913         return min(min(first_rot_max_expo, second_rot_max_expo),
 914                    third_rot_max_expo);
 915 }
 916
 917 static int vgxy61_update_exposure(struct vgxy61_dev *sensor, u16 new_expo_long,
 918                                   enum vgxy61_hdr_mode hdr)
 919 {
 920         struct i2c_client *client = sensor->i2c_client;
 921         u16 new_expo_short = 0;
 922         u16 expo_short_max = 0;
 923         u16 expo_long_min = VGXY61_MIN_EXPOSURE;
 924         u16 expo_long_max = 0;
 925
 926         /* Compute short exposure according to hdr mode and long exposure */
 927         switch (hdr) {
 928         case VGXY61_HDR_LINEAR:
 929                 /*
 930                  * Take ratio into account for minimal exposures in
 931                  * VGXY61_HDR_LINEAR
 932                  */
 933                 expo_long_min = VGXY61_MIN_EXPOSURE * VGXY61_HDR_LINEAR_RATIO;
 934                 new_expo_long = max(expo_long_min, new_expo_long);
 935
 936                 expo_long_max =
 937                         vgxy61_get_expo_long_max(sensor,
 938                                                  VGXY61_HDR_LINEAR_RATIO);
 939                 expo_short_max = (expo_long_max +
 940                                  (VGXY61_HDR_LINEAR_RATIO / 2)) /
 941                                  VGXY61_HDR_LINEAR_RATIO;
 942                 new_expo_short = (new_expo_long +
 943                                  (VGXY61_HDR_LINEAR_RATIO / 2)) /
 944                                  VGXY61_HDR_LINEAR_RATIO;
 945                 break;
 946         case VGXY61_HDR_SUB:
 947                 new_expo_long = max(expo_long_min, new_expo_long);
 948
 949                 expo_long_max = vgxy61_get_expo_long_max(sensor, 1);
 950                 /* Short and long are the same in VGXY61_HDR_SUB */
 951                 expo_short_max = expo_long_max;
 952                 new_expo_short = new_expo_long;
 953                 break;
 954         case VGXY61_NO_HDR:
 955                 new_expo_long = max(expo_long_min, new_expo_long);
 956
 957                 /*
 958                  * As short expo is 0 here, only the second rule of thumb
 959                  * applies, see vgxy61_get_expo_long_max for more
 960                  */
 961                 expo_long_max = sensor->frame_length - VGXY61_EXPOS_ROT_TERM;
 962                 break;
 963         default:
 964                 /* Should never happen */
 965                 WARN_ON(true);
 966                 break;
 967         }
 968
 969         /* If this happens, something is wrong with formulas */
 970         WARN_ON(expo_long_min > expo_long_max);
 971
 972         if (new_expo_long > expo_long_max) {
 973                 dev_warn(&client->dev, "Exposure %d too high, clamping to %d\n",
 974                          new_expo_long, expo_long_max);
 975                 new_expo_long = expo_long_max;
 976                 new_expo_short = expo_short_max;
 977         }
 978
 979         sensor->expo_long = new_expo_long;
 980         sensor->expo_short = new_expo_short;
 981         sensor->expo_max = expo_long_max;
 982         sensor->expo_min = expo_long_min;
 983
 984         if (sensor->streaming)
 985                 return vgxy61_apply_exposure(sensor);
 986         return 0;
 987 }
 988
 989 static int vgxy61_apply_framelength(struct vgxy61_dev *sensor)
 990 {
 991         return vgxy61_write_reg(sensor, VGXY61_REG_FRAME_LENGTH,
 992                                 sensor->frame_length, NULL);
 993 }
 994
 995 static int vgxy61_update_vblank(struct vgxy61_dev *sensor, u16 vblank,
 996                                 enum vgxy61_hdr_mode hdr)
 997 {
 998         int ret;
 999
1000         sensor->vblank_min = vgxy61_get_vblank_min(sensor, hdr);
1001         sensor->vblank = max(sensor->vblank_min, vblank);
1002         sensor->frame_length = sensor->current_mode->crop.height +
1003                                sensor->vblank;
1004
1005         /* Update exposure according to vblank */
1006         ret = vgxy61_update_exposure(sensor, sensor->expo_long, hdr);
1007         if (ret)
1008                 return ret;
1009
1010         if (sensor->streaming)
1011                 return vgxy61_apply_framelength(sensor);
1012         return 0;
1013 }
1014
1015 static int vgxy61_apply_hdr(struct vgxy61_dev *sensor,
1016                             enum vgxy61_hdr_mode index)
1017 {
1018         static const u8 index2val[] = {0x1, 0x4, 0xa};
1019
1020         return vgxy61_write_reg(sensor, VGXY61_REG_HDR_CTRL, index2val[index],
1021                                 NULL);
1022 }
1023
1024 static int vgxy61_update_hdr(struct vgxy61_dev *sensor,
1025                              enum vgxy61_hdr_mode index)
1026 {
1027         int ret;
1028
1029         /*
1030          * vblank and short exposure change according to HDR mode, do it first
1031          * as it can violate sensors 'rule of thumbs' and therefore will require
1032          * to change the long exposure.
1033          */
1034         ret = vgxy61_update_vblank(sensor, sensor->vblank, index);
1035         if (ret)
1036                 return ret;
1037
1038         /* Update strobe mode according to HDR */
1039         ret = vgxy61_update_gpios_strobe_mode(sensor, index);
1040         if (ret)
1041                 return ret;
1042
1043         sensor->hdr = index;
1044
1045         if (sensor->streaming)
1046                 return vgxy61_apply_hdr(sensor, sensor->hdr);
1047         return 0;
1048 }
1049
1050 static int vgxy61_apply_settings(struct vgxy61_dev *sensor)
1051 {
1052         int ret;
1053         unsigned int i;
1054
1055         ret = vgxy61_apply_hdr(sensor, sensor->hdr);
1056         if (ret)
1057                 return ret;
1058
1059         ret = vgxy61_apply_framelength(sensor);
1060         if (ret)
1061                 return ret;
1062
1063         ret = vgxy61_apply_exposure(sensor);
1064         if (ret)
1065                 return ret;
1066
1067         ret = vgxy61_write_reg(sensor, VGXY61_REG_ANALOG_GAIN,
1068                                sensor->analog_gain, NULL);
1069         if (ret)
1070                 return ret;
1071         ret = vgxy61_apply_digital_gain(sensor, sensor->digital_gain);
1072         if (ret)
1073                 return ret;
1074
1075         ret = vgxy61_write_reg(sensor, VGXY61_REG_ORIENTATION,
1076                                sensor->hflip | (sensor->vflip << 1), NULL);
1077         if (ret)
1078                 return ret;
1079
1080         ret = vgxy61_apply_patgen(sensor, sensor->pattern);
1081         if (ret)
1082                 return ret;
1083
1084         for (i = 0; i < VGXY61_NB_GPIOS; i++) {
1085                 ret = vgxy61_apply_gpiox_strobe_mode(sensor,
1086                                                      sensor->strobe_mode, i);
1087                 if (ret)
1088                         return ret;
1089         }
1090
1091         return 0;
1092 }
1093
1094 static int vgxy61_stream_enable(struct vgxy61_dev *sensor)
1095 {
1096         struct i2c_client *client = v4l2_get_subdevdata(&sensor->sd);
1097         const struct v4l2_rect *crop = &sensor->current_mode->crop;
1098         int ret = 0;
1099
1100         ret = vgxy61_check_bw(sensor);
1101         if (ret)
1102                 return ret;
1103
1104         ret = pm_runtime_resume_and_get(&client->dev);
1105         if (ret)
1106                 return ret;
1107
1108         vgxy61_write_reg(sensor, VGXY61_REG_FORMAT_CTRL,
1109                          get_bpp_by_code(sensor->fmt.code), &ret);
1110         vgxy61_write_reg(sensor, VGXY61_REG_OIF_ROI0_CTRL,
1111                          get_data_type_by_code(sensor->fmt.code), &ret);
1112
1113         vgxy61_write_reg(sensor, VGXY61_REG_READOUT_CTRL,
1114                          sensor->current_mode->bin_mode, &ret);
1115         vgxy61_write_reg(sensor, VGXY61_REG_ROI0_START_H, crop->left, &ret);
1116         vgxy61_write_reg(sensor, VGXY61_REG_ROI0_END_H,
1117                          crop->left + crop->width - 1, &ret);
1118         vgxy61_write_reg(sensor, VGXY61_REG_ROI0_START_V, crop->top, &ret);
1119         vgxy61_write_reg(sensor, VGXY61_REG_ROI0_END_V,
1120                          crop->top + crop->height - 1, &ret);
1121         if (ret)
1122                 goto err_rpm_put;
1123
1124         ret = vgxy61_apply_settings(sensor);
1125         if (ret)
1126                 goto err_rpm_put;
1127
1128         ret = vgxy61_write_reg(sensor, VGXY61_REG_STREAMING,
1129                                VGXY61_STREAMING_REQ_START, NULL);
1130         if (ret)
1131                 goto err_rpm_put;
1132
1133         ret = vgxy61_poll_reg(sensor, VGXY61_REG_STREAMING,
1134                               VGXY61_STREAMING_NO_REQ, VGXY61_TIMEOUT_MS);
1135         if (ret)
1136                 goto err_rpm_put;
1137
1138         ret = vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_STREAMING,
1139                                 VGXY61_TIMEOUT_MS);
1140         if (ret)
1141                 goto err_rpm_put;
1142
1143         /* vflip and hflip cannot change during streaming */
1144         __v4l2_ctrl_grab(sensor->vflip_ctrl, true);
1145         __v4l2_ctrl_grab(sensor->hflip_ctrl, true);
1146
1147         return 0;
1148
1149 err_rpm_put:
1150         pm_runtime_put(&client->dev);
1151         return ret;
1152 }
1153
1154 static int vgxy61_stream_disable(struct vgxy61_dev *sensor)
1155 {
1156         struct i2c_client *client = v4l2_get_subdevdata(&sensor->sd);
1157         int ret;
1158
1159         ret = vgxy61_write_reg(sensor, VGXY61_REG_STREAMING,
1160                                VGXY61_STREAMING_REQ_STOP, NULL);
1161         if (ret)
1162                 goto err_str_dis;
1163
1164         ret = vgxy61_poll_reg(sensor, VGXY61_REG_STREAMING,
1165                               VGXY61_STREAMING_NO_REQ, 2000);
1166         if (ret)
1167                 goto err_str_dis;
1168
1169         ret = vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_SW_STBY,
1170                                 VGXY61_TIMEOUT_MS);
1171         if (ret)
1172                 goto err_str_dis;
1173
1174         __v4l2_ctrl_grab(sensor->vflip_ctrl, false);
1175         __v4l2_ctrl_grab(sensor->hflip_ctrl, false);
1176
1177 err_str_dis:
1178         if (ret)
1179                 WARN(1, "Can't disable stream");
1180         pm_runtime_put(&client->dev);
1181
1182         return ret;
1183 }
1184
1185 static int vgxy61_s_stream(struct v4l2_subdev *sd, int enable)
1186 {
1187         struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1188         int ret = 0;
1189
1190         mutex_lock(&sensor->lock);
1191
1192         ret = enable ? vgxy61_stream_enable(sensor) :
1193               vgxy61_stream_disable(sensor);
1194         if (!ret)
1195                 sensor->streaming = enable;
1196
1197         mutex_unlock(&sensor->lock);
1198
1199         return ret;
1200 }
1201
1202 static int vgxy61_set_fmt(struct v4l2_subdev *sd,
1203                           struct v4l2_subdev_state *sd_state,
1204                           struct v4l2_subdev_format *format)
1205 {
1206         struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1207         const struct vgxy61_mode_info *new_mode;
1208         struct v4l2_mbus_framefmt *fmt;
1209         int ret;
1210
1211         mutex_lock(&sensor->lock);
1212
1213         if (sensor->streaming) {
1214                 ret = -EBUSY;
1215                 goto out;
1216         }
1217
1218         ret = vgxy61_try_fmt_internal(sd, &format->format, &new_mode);
1219         if (ret)
1220                 goto out;
1221
1222         if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
1223                 fmt = v4l2_subdev_get_try_format(sd, sd_state, 0);
1224                 *fmt = format->format;
1225         } else if (sensor->current_mode != new_mode ||
1226                    sensor->fmt.code != format->format.code) {
1227                 fmt = &sensor->fmt;
1228                 *fmt = format->format;
1229
1230                 sensor->current_mode = new_mode;
1231
1232                 /* Reset vblank and framelength to default */
1233                 ret = vgxy61_update_vblank(sensor,
1234                                            VGXY61_FRAME_LENGTH_DEF -
1235                                            new_mode->crop.height,
1236                                            sensor->hdr);
1237
1238                 /* Update controls to reflect new mode */
1239                 __v4l2_ctrl_s_ctrl_int64(sensor->pixel_rate_ctrl,
1240                                          get_pixel_rate(sensor));
1241                 __v4l2_ctrl_modify_range(sensor->vblank_ctrl,
1242                                          sensor->vblank_min,
1243                                          0xffff - new_mode->crop.height,
1244                                          1, sensor->vblank);
1245                 __v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, sensor->vblank);
1246                 __v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min,
1247                                          sensor->expo_max, 1,
1248                                          sensor->expo_long);
1249         }
1250
1251 out:
1252         mutex_unlock(&sensor->lock);
1253
1254         return ret;
1255 }
1256
1257 static int vgxy61_init_cfg(struct v4l2_subdev *sd,
1258                            struct v4l2_subdev_state *sd_state)
1259 {
1260         struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1261         struct v4l2_subdev_format fmt = { 0 };
1262
1263         vgxy61_fill_framefmt(sensor, sensor->current_mode, &fmt.format,
1264                              VGXY61_MEDIA_BUS_FMT_DEF);
1265
1266         return vgxy61_set_fmt(sd, sd_state, &fmt);
1267 }
1268
1269 static int vgxy61_s_ctrl(struct v4l2_ctrl *ctrl)
1270 {
1271         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
1272         struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1273         const struct vgxy61_mode_info *cur_mode = sensor->current_mode;
1274         int ret;
1275
1276         switch (ctrl->id) {
1277         case V4L2_CID_EXPOSURE:
1278                 ret = vgxy61_update_exposure(sensor, ctrl->val, sensor->hdr);
1279                 ctrl->val = sensor->expo_long;
1280                 break;
1281         case V4L2_CID_ANALOGUE_GAIN:
1282                 ret = vgxy61_update_analog_gain(sensor, ctrl->val);
1283                 break;
1284         case V4L2_CID_DIGITAL_GAIN:
1285                 ret = vgxy61_update_digital_gain(sensor, ctrl->val);
1286                 break;
1287         case V4L2_CID_VFLIP:
1288         case V4L2_CID_HFLIP:
1289                 if (sensor->streaming) {
1290                         ret = -EBUSY;
1291                         break;
1292                 }
1293                 if (ctrl->id == V4L2_CID_VFLIP)
1294                         sensor->vflip = ctrl->val;
1295                 if (ctrl->id == V4L2_CID_HFLIP)
1296                         sensor->hflip = ctrl->val;
1297                 ret = 0;
1298                 break;
1299         case V4L2_CID_TEST_PATTERN:
1300                 ret = vgxy61_update_patgen(sensor, ctrl->val);
1301                 break;
1302         case V4L2_CID_HDR_SENSOR_MODE:
1303                 ret = vgxy61_update_hdr(sensor, ctrl->val);
1304                 /* Update vblank and exposure controls to match new hdr */
1305                 __v4l2_ctrl_modify_range(sensor->vblank_ctrl,
1306                                          sensor->vblank_min,
1307                                          0xffff - cur_mode->crop.height,
1308                                          1, sensor->vblank);
1309                 __v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min,
1310                                          sensor->expo_max, 1,
1311                                          sensor->expo_long);
1312                 break;
1313         case V4L2_CID_VBLANK:
1314                 ret = vgxy61_update_vblank(sensor, ctrl->val, sensor->hdr);
1315                 /* Update exposure control to match new vblank */
1316                 __v4l2_ctrl_modify_range(sensor->expo_ctrl, sensor->expo_min,
1317                                          sensor->expo_max, 1,
1318                                          sensor->expo_long);
1319                 break;
1320         default:
1321                 ret = -EINVAL;
1322                 break;
1323         }
1324
1325         return ret;
1326 }
1327
1328 static const struct v4l2_ctrl_ops vgxy61_ctrl_ops = {
1329         .s_ctrl = vgxy61_s_ctrl,
1330 };
1331
1332 static int vgxy61_init_controls(struct vgxy61_dev *sensor)
1333 {
1334         const struct v4l2_ctrl_ops *ops = &vgxy61_ctrl_ops;
1335         struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
1336         const struct vgxy61_mode_info *cur_mode = sensor->current_mode;
1337         struct v4l2_ctrl *ctrl;
1338         int ret;
1339
1340         v4l2_ctrl_handler_init(hdl, 16);
1341         /* We can use our own mutex for the ctrl lock */
1342         hdl->lock = &sensor->lock;
1343         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN, 0, 0x1c, 1,
1344                           sensor->analog_gain);
1345         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN, 0, 0xfff, 1,
1346                           sensor->digital_gain);
1347         v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
1348                                      ARRAY_SIZE(vgxy61_test_pattern_menu) - 1,
1349                                      0, 0, vgxy61_test_pattern_menu);
1350         ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 0,
1351                                  sensor->line_length, 1,
1352                                  sensor->line_length - cur_mode->width);
1353         if (ctrl)
1354                 ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1355         ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
1356                                       ARRAY_SIZE(link_freq) - 1, 0, link_freq);
1357         if (ctrl)
1358                 ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1359         v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_HDR_SENSOR_MODE,
1360                                      ARRAY_SIZE(vgxy61_hdr_mode_menu) - 1, 0,
1361                                      VGXY61_NO_HDR, vgxy61_hdr_mode_menu);
1362
1363         /*
1364          * Keep a pointer to these controls as we need to update them when
1365          * setting the format
1366          */
1367         sensor->pixel_rate_ctrl = v4l2_ctrl_new_std(hdl, ops,
1368                                                     V4L2_CID_PIXEL_RATE, 1,
1369                                                     INT_MAX, 1,
1370                                                     get_pixel_rate(sensor));
1371         if (sensor->pixel_rate_ctrl)
1372                 sensor->pixel_rate_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1373         sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
1374                                               sensor->expo_min,
1375                                               sensor->expo_max, 1,
1376                                               sensor->expo_long);
1377         sensor->vblank_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK,
1378                                                 sensor->vblank_min,
1379                                                 0xffff - cur_mode->crop.height,
1380                                                 1, sensor->vblank);
1381         sensor->vflip_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
1382                                                0, 1, 1, sensor->vflip);
1383         sensor->hflip_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
1384                                                0, 1, 1, sensor->hflip);
1385
1386         if (hdl->error) {
1387                 ret = hdl->error;
1388                 goto free_ctrls;
1389         }
1390
1391         sensor->sd.ctrl_handler = hdl;
1392         return 0;
1393
1394 free_ctrls:
1395         v4l2_ctrl_handler_free(hdl);
1396         return ret;
1397 }
1398
1399 static const struct v4l2_subdev_video_ops vgxy61_video_ops = {
1400         .s_stream = vgxy61_s_stream,
1401 };
1402
1403 static const struct v4l2_subdev_pad_ops vgxy61_pad_ops = {
1404         .init_cfg = vgxy61_init_cfg,
1405         .enum_mbus_code = vgxy61_enum_mbus_code,
1406         .get_fmt = vgxy61_get_fmt,
1407         .set_fmt = vgxy61_set_fmt,
1408         .get_selection = vgxy61_get_selection,
1409         .enum_frame_size = vgxy61_enum_frame_size,
1410 };
1411
1412 static const struct v4l2_subdev_ops vgxy61_subdev_ops = {
1413         .video = &vgxy61_video_ops,
1414         .pad = &vgxy61_pad_ops,
1415 };
1416
1417 static const struct media_entity_operations vgxy61_subdev_entity_ops = {
1418         .link_validate = v4l2_subdev_link_validate,
1419 };
1420
1421 static int vgxy61_tx_from_ep(struct vgxy61_dev *sensor,
1422                              struct fwnode_handle *handle)
1423 {
1424         struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
1425         struct i2c_client *client = sensor->i2c_client;
1426         u32 log2phy[VGXY61_NB_POLARITIES] = {~0, ~0, ~0, ~0, ~0};
1427         u32 phy2log[VGXY61_NB_POLARITIES] = {~0, ~0, ~0, ~0, ~0};
1428         int polarities[VGXY61_NB_POLARITIES] = {0, 0, 0, 0, 0};
1429         int l_nb;
1430         unsigned int p, l, i;
1431         int ret;
1432
1433         ret = v4l2_fwnode_endpoint_alloc_parse(handle, &ep);
1434         if (ret)
1435                 return -EINVAL;
1436
1437         l_nb = ep.bus.mipi_csi2.num_data_lanes;
1438         if (l_nb != 1 && l_nb != 2 && l_nb != 4) {
1439                 dev_err(&client->dev, "invalid data lane number %d\n", l_nb);
1440                 goto error_ep;
1441         }
1442
1443         /* Build log2phy, phy2log and polarities from ep info */
1444         log2phy[0] = ep.bus.mipi_csi2.clock_lane;
1445         phy2log[log2phy[0]] = 0;
1446         for (l = 1; l < l_nb + 1; l++) {
1447                 log2phy[l] = ep.bus.mipi_csi2.data_lanes[l - 1];
1448                 phy2log[log2phy[l]] = l;
1449         }
1450         /*
1451          * Then fill remaining slots for every physical slot to have something
1452          * valid for hardware stuff.
1453          */
1454         for (p = 0; p < VGXY61_NB_POLARITIES; p++) {
1455                 if (phy2log[p] != ~0)
1456                         continue;
1457                 phy2log[p] = l;
1458                 log2phy[l] = p;
1459                 l++;
1460         }
1461         for (l = 0; l < l_nb + 1; l++)
1462                 polarities[l] = ep.bus.mipi_csi2.lane_polarities[l];
1463
1464         if (log2phy[0] != 0) {
1465                 dev_err(&client->dev, "clk lane must be map to physical lane 0\n");
1466                 goto error_ep;
1467         }
1468         sensor->oif_ctrl = (polarities[4] << 15) + ((phy2log[4] - 1) << 13) +
1469                            (polarities[3] << 12) + ((phy2log[3] - 1) << 10) +
1470                            (polarities[2] <<  9) + ((phy2log[2] - 1) <<  7) +
1471                            (polarities[1] <<  6) + ((phy2log[1] - 1) <<  4) +
1472                            (polarities[0] <<  3) +
1473                            l_nb;
1474         sensor->nb_of_lane = l_nb;
1475
1476         dev_dbg(&client->dev, "tx uses %d lanes", l_nb);
1477         for (i = 0; i < VGXY61_NB_POLARITIES; i++) {
1478                 dev_dbg(&client->dev, "log2phy[%d] = %d\n", i, log2phy[i]);
1479                 dev_dbg(&client->dev, "phy2log[%d] = %d\n", i, phy2log[i]);
1480                 dev_dbg(&client->dev, "polarity[%d] = %d\n", i, polarities[i]);
1481         }
1482         dev_dbg(&client->dev, "oif_ctrl = 0x%04x\n", sensor->oif_ctrl);
1483
1484         v4l2_fwnode_endpoint_free(&ep);
1485
1486         return 0;
1487
1488 error_ep:
1489         v4l2_fwnode_endpoint_free(&ep);
1490
1491         return -EINVAL;
1492 }
1493
1494 static int vgxy61_configure(struct vgxy61_dev *sensor)
1495 {
1496         u32 sensor_freq;
1497         u8 prediv, mult;
1498         int line_length;
1499         int ret = 0;
1500
1501         compute_pll_parameters_by_freq(sensor->clk_freq, &prediv, &mult);
1502         sensor_freq = (mult * sensor->clk_freq) / prediv;
1503         /* Frequency to data rate is 1:1 ratio for MIPI */
1504         sensor->data_rate_in_mbps = sensor_freq;
1505         /* Video timing ISP path (pixel clock)  requires 804/5 mhz = 160 mhz */
1506         sensor->pclk = sensor_freq / 5;
1507
1508         line_length = vgxy61_read_reg(sensor, VGXY61_REG_LINE_LENGTH);
1509         if (line_length < 0)
1510                 return line_length;
1511         sensor->line_length = line_length;
1512         vgxy61_write_reg(sensor, VGXY61_REG_EXT_CLOCK, sensor->clk_freq, &ret);
1513         vgxy61_write_reg(sensor, VGXY61_REG_CLK_PLL_PREDIV, prediv, &ret);
1514         vgxy61_write_reg(sensor, VGXY61_REG_CLK_SYS_PLL_MULT, mult, &ret);
1515         vgxy61_write_reg(sensor, VGXY61_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
1516         vgxy61_write_reg(sensor, VGXY61_REG_FRAME_CONTENT_CTRL, 0, &ret);
1517         vgxy61_write_reg(sensor, VGXY61_REG_BYPASS_CTRL, 4, &ret);
1518         if (ret)
1519                 return ret;
1520         vgxy61_update_gpios_strobe_polarity(sensor, sensor->gpios_polarity);
1521         /* Set pattern generator solid to middle value */
1522         vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_LONG_DATA_GR, 0x800, &ret);
1523         vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_LONG_DATA_R, 0x800, &ret);
1524         vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_LONG_DATA_B, 0x800, &ret);
1525         vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_LONG_DATA_GB, 0x800, &ret);
1526         vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_SHORT_DATA_GR, 0x800, &ret);
1527         vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_SHORT_DATA_R, 0x800, &ret);
1528         vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_SHORT_DATA_B, 0x800, &ret);
1529         vgxy61_write_reg(sensor, VGXY61_REG_PATGEN_SHORT_DATA_GB, 0x800, &ret);
1530         if (ret)
1531                 return ret;
1532
1533         return 0;
1534 }
1535
1536 static int vgxy61_patch(struct vgxy61_dev *sensor)
1537 {
1538         struct i2c_client *client = sensor->i2c_client;
1539         int patch, ret;
1540
1541         /*(DEBLOBBED)*/
1542
1543         ret = vgxy61_write_reg(sensor, VGXY61_REG_STBY, 0x10, NULL);
1544         if (ret)
1545                 return ret;
1546
1547         ret = vgxy61_poll_reg(sensor, VGXY61_REG_STBY, 0, VGXY61_TIMEOUT_MS);
1548         if (ret)
1549                 return ret;
1550
1551         patch = vgxy61_read_reg(sensor, VGXY61_REG_FWPATCH_REVISION);
1552         if (patch < 0)
1553                 return patch;
1554
1555         if (patch != (VGXY61_FWPATCH_REVISION_MAJOR << 12) +
1556                      (VGXY61_FWPATCH_REVISION_MINOR << 8) +
1557                      VGXY61_FWPATCH_REVISION_MICRO) {
1558                 dev_err(&client->dev, "bad patch version expected %d.%d.%d got %d.%d.%d\n",
1559                         VGXY61_FWPATCH_REVISION_MAJOR,
1560                         VGXY61_FWPATCH_REVISION_MINOR,
1561                         VGXY61_FWPATCH_REVISION_MICRO,
1562                         patch >> 12, (patch >> 8) & 0x0f, patch & 0xff);
1563                 return -ENODEV;
1564         }
1565         dev_dbg(&client->dev, "patch %d.%d.%d applied\n",
1566                 patch >> 12, (patch >> 8) & 0x0f, patch & 0xff);
1567
1568         return 0;
1569 }
1570
1571 static int vgxy61_detect_cut_version(struct vgxy61_dev *sensor)
1572 {
1573         struct i2c_client *client = sensor->i2c_client;
1574         int device_rev;
1575
1576         device_rev = vgxy61_read_reg(sensor, VGXY61_REG_REVISION);
1577         if (device_rev < 0)
1578                 return device_rev;
1579
1580         switch (device_rev >> 8) {
1581         case 0xA:
1582                 dev_dbg(&client->dev, "Cut1 detected\n");
1583                 dev_err(&client->dev, "Cut1 not supported by this driver\n");
1584                 return -ENODEV;
1585         case 0xB:
1586                 dev_dbg(&client->dev, "Cut2 detected\n");
1587                 return 0;
1588         case 0xC:
1589                 dev_dbg(&client->dev, "Cut3 detected\n");
1590                 return 0;
1591         default:
1592                 dev_err(&client->dev, "Unable to detect cut version\n");
1593                 return -ENODEV;
1594         }
1595 }
1596
1597 static int vgxy61_detect(struct vgxy61_dev *sensor)
1598 {
1599         struct i2c_client *client = sensor->i2c_client;
1600         int id = 0;
1601         int ret, st;
1602
1603         id = vgxy61_read_reg(sensor, VGXY61_REG_MODEL_ID);
1604         if (id < 0)
1605                 return id;
1606         if (id != VG5661_MODEL_ID && id != VG5761_MODEL_ID) {
1607                 dev_warn(&client->dev, "Unsupported sensor id %x\n", id);
1608                 return -ENODEV;
1609         }
1610         dev_dbg(&client->dev, "detected sensor id = 0x%04x\n", id);
1611         sensor->id = id;
1612
1613         ret = vgxy61_wait_state(sensor, VGXY61_SYSTEM_FSM_SW_STBY,
1614                                 VGXY61_TIMEOUT_MS);
1615         if (ret)
1616                 return ret;
1617
1618         st = vgxy61_read_reg(sensor, VGXY61_REG_NVM);
1619         if (st < 0)
1620                 return st;
1621         if (st != VGXY61_NVM_OK)
1622                 dev_warn(&client->dev, "Bad nvm state got %d\n", st);
1623
1624         ret = vgxy61_detect_cut_version(sensor);
1625         if (ret)
1626                 return ret;
1627
1628         return 0;
1629 }
1630
1631 /* Power/clock management functions */
1632 static int vgxy61_power_on(struct device *dev)
1633 {
1634         struct i2c_client *client = to_i2c_client(dev);
1635         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1636         struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1637         int ret;
1638
1639         ret = regulator_bulk_enable(ARRAY_SIZE(vgxy61_supply_name),
1640                                     sensor->supplies);
1641         if (ret) {
1642                 dev_err(&client->dev, "failed to enable regulators %d\n", ret);
1643                 return ret;
1644         }
1645
1646         ret = clk_prepare_enable(sensor->xclk);
1647         if (ret) {
1648                 dev_err(&client->dev, "failed to enable clock %d\n", ret);
1649                 goto disable_bulk;
1650         }
1651
1652         if (sensor->reset_gpio) {
1653                 ret = vgxy61_apply_reset(sensor);
1654                 if (ret) {
1655                         dev_err(&client->dev, "sensor reset failed %d\n", ret);
1656                         goto disable_clock;
1657                 }
1658         }
1659
1660         ret = vgxy61_detect(sensor);
1661         if (ret) {
1662                 dev_err(&client->dev, "sensor detect failed %d\n", ret);
1663                 goto disable_clock;
1664         }
1665
1666         ret = vgxy61_patch(sensor);
1667         if (ret) {
1668                 dev_err(&client->dev, "sensor patch failed %d\n", ret);
1669                 goto disable_clock;
1670         }
1671
1672         ret = vgxy61_configure(sensor);
1673         if (ret) {
1674                 dev_err(&client->dev, "sensor configuration failed %d\n", ret);
1675                 goto disable_clock;
1676         }
1677
1678         return 0;
1679
1680 disable_clock:
1681         clk_disable_unprepare(sensor->xclk);
1682 disable_bulk:
1683         regulator_bulk_disable(ARRAY_SIZE(vgxy61_supply_name),
1684                                sensor->supplies);
1685
1686         return ret;
1687 }
1688
1689 static int vgxy61_power_off(struct device *dev)
1690 {
1691         struct i2c_client *client = to_i2c_client(dev);
1692         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1693         struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1694
1695         clk_disable_unprepare(sensor->xclk);
1696         regulator_bulk_disable(ARRAY_SIZE(vgxy61_supply_name),
1697                                sensor->supplies);
1698         return 0;
1699 }
1700
1701 static void vgxy61_fill_sensor_param(struct vgxy61_dev *sensor)
1702 {
1703         if (sensor->id == VG5761_MODEL_ID) {
1704                 sensor->sensor_width = VGX761_WIDTH;
1705                 sensor->sensor_height = VGX761_HEIGHT;
1706                 sensor->sensor_modes = vgx761_mode_data;
1707                 sensor->sensor_modes_nb = ARRAY_SIZE(vgx761_mode_data);
1708                 sensor->default_mode = &vgx761_mode_data[VGX761_DEFAULT_MODE];
1709                 sensor->rot_term = VGX761_SHORT_ROT_TERM;
1710         } else if (sensor->id == VG5661_MODEL_ID) {
1711                 sensor->sensor_width = VGX661_WIDTH;
1712                 sensor->sensor_height = VGX661_HEIGHT;
1713                 sensor->sensor_modes = vgx661_mode_data;
1714                 sensor->sensor_modes_nb = ARRAY_SIZE(vgx661_mode_data);
1715                 sensor->default_mode = &vgx661_mode_data[VGX661_DEFAULT_MODE];
1716                 sensor->rot_term = VGX661_SHORT_ROT_TERM;
1717         } else {
1718                 /* Should never happen */
1719                 WARN_ON(true);
1720         }
1721         sensor->current_mode = sensor->default_mode;
1722 }
1723
1724 static int vgxy61_probe(struct i2c_client *client)
1725 {
1726         struct device *dev = &client->dev;
1727         struct fwnode_handle *handle;
1728         struct vgxy61_dev *sensor;
1729         int ret;
1730
1731         sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
1732         if (!sensor)
1733                 return -ENOMEM;
1734
1735         sensor->i2c_client = client;
1736         sensor->streaming = false;
1737         sensor->hdr = VGXY61_NO_HDR;
1738         sensor->expo_long = 200;
1739         sensor->expo_short = 0;
1740         sensor->hflip = false;
1741         sensor->vflip = false;
1742         sensor->analog_gain = 0;
1743         sensor->digital_gain = 256;
1744
1745         handle = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0, 0);
1746         if (!handle) {
1747                 dev_err(dev, "handle node not found\n");
1748                 return -EINVAL;
1749         }
1750
1751         ret = vgxy61_tx_from_ep(sensor, handle);
1752         fwnode_handle_put(handle);
1753         if (ret) {
1754                 dev_err(dev, "Failed to parse handle %d\n", ret);
1755                 return ret;
1756         }
1757
1758         sensor->xclk = devm_clk_get(dev, NULL);
1759         if (IS_ERR(sensor->xclk)) {
1760                 dev_err(dev, "failed to get xclk\n");
1761                 return PTR_ERR(sensor->xclk);
1762         }
1763         sensor->clk_freq = clk_get_rate(sensor->xclk);
1764         if (sensor->clk_freq < 6 * HZ_PER_MHZ ||
1765             sensor->clk_freq > 27 * HZ_PER_MHZ) {
1766                 dev_err(dev, "Only 6Mhz-27Mhz clock range supported. provide %lu MHz\n",
1767                         sensor->clk_freq / HZ_PER_MHZ);
1768                 return -EINVAL;
1769         }
1770         sensor->gpios_polarity =
1771                 device_property_read_bool(dev, "st,strobe-gpios-polarity");
1772
1773         v4l2_i2c_subdev_init(&sensor->sd, client, &vgxy61_subdev_ops);
1774         sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1775         sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
1776         sensor->sd.entity.ops = &vgxy61_subdev_entity_ops;
1777         sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1778
1779         sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
1780                                                      GPIOD_OUT_HIGH);
1781
1782         ret = vgxy61_get_regulators(sensor);
1783         if (ret) {
1784                 dev_err(&client->dev, "failed to get regulators %d\n", ret);
1785                 return ret;
1786         }
1787
1788         ret = vgxy61_power_on(dev);
1789         if (ret)
1790                 return ret;
1791
1792         vgxy61_fill_sensor_param(sensor);
1793         vgxy61_fill_framefmt(sensor, sensor->current_mode, &sensor->fmt,
1794                              VGXY61_MEDIA_BUS_FMT_DEF);
1795
1796         mutex_init(&sensor->lock);
1797
1798         ret = vgxy61_update_hdr(sensor, sensor->hdr);
1799         if (ret)
1800                 goto error_power_off;
1801
1802         ret = vgxy61_init_controls(sensor);
1803         if (ret) {
1804                 dev_err(&client->dev, "controls initialization failed %d\n",
1805                         ret);
1806                 goto error_power_off;
1807         }
1808
1809         ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
1810         if (ret) {
1811                 dev_err(&client->dev, "pads init failed %d\n", ret);
1812                 goto error_handler_free;
1813         }
1814
1815         /* Enable runtime PM and turn off the device */
1816         pm_runtime_set_active(dev);
1817         pm_runtime_enable(dev);
1818         pm_runtime_idle(dev);
1819
1820         ret = v4l2_async_register_subdev(&sensor->sd);
1821         if (ret) {
1822                 dev_err(&client->dev, "async subdev register failed %d\n", ret);
1823                 goto error_pm_runtime;
1824         }
1825
1826         pm_runtime_set_autosuspend_delay(&client->dev, 1000);
1827         pm_runtime_use_autosuspend(&client->dev);
1828
1829         dev_dbg(&client->dev, "vgxy61 probe successfully\n");
1830
1831         return 0;
1832
1833 error_pm_runtime:
1834         pm_runtime_disable(&client->dev);
1835         pm_runtime_set_suspended(&client->dev);
1836         media_entity_cleanup(&sensor->sd.entity);
1837 error_handler_free:
1838         v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
1839 error_power_off:
1840         mutex_destroy(&sensor->lock);
1841         vgxy61_power_off(dev);
1842
1843         return ret;
1844 }
1845
1846 static void vgxy61_remove(struct i2c_client *client)
1847 {
1848         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1849         struct vgxy61_dev *sensor = to_vgxy61_dev(sd);
1850
1851         v4l2_async_unregister_subdev(&sensor->sd);
1852         mutex_destroy(&sensor->lock);
1853         media_entity_cleanup(&sensor->sd.entity);
1854
1855         pm_runtime_disable(&client->dev);
1856         if (!pm_runtime_status_suspended(&client->dev))
1857                 vgxy61_power_off(&client->dev);
1858         pm_runtime_set_suspended(&client->dev);
1859 }
1860
1861 static const struct of_device_id vgxy61_dt_ids[] = {
1862         { .compatible = "st,st-vgxy61" },
1863         { /* sentinel */ }
1864 };
1865 MODULE_DEVICE_TABLE(of, vgxy61_dt_ids);
1866
1867 static const struct dev_pm_ops vgxy61_pm_ops = {
1868         SET_RUNTIME_PM_OPS(vgxy61_power_off, vgxy61_power_on, NULL)
1869 };
1870
1871 static struct i2c_driver vgxy61_i2c_driver = {
1872         .driver = {
1873                 .name  = "st-vgxy61",
1874                 .of_match_table = vgxy61_dt_ids,
1875                 .pm = &vgxy61_pm_ops,
1876         },
1877         .probe = vgxy61_probe,
1878         .remove = vgxy61_remove,
1879 };
1880
1881 module_i2c_driver(vgxy61_i2c_driver);
1882
1883 MODULE_AUTHOR("Benjamin Mugnier <benjamin.mugnier@foss.st.com>");
1884 MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>");
1885 MODULE_AUTHOR("Sylvain Petinot <sylvain.petinot@foss.st.com>");
1886 MODULE_DESCRIPTION("VGXY61 camera subdev driver");
1887 MODULE_LICENSE("GPL");