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