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